double igl::angular_distance(
const Eigen::Quaterniond & A,
const Eigen::Quaterniond & B)
{
using namespace igl;
assert(fabs(A.norm()-1)<FLOAT_EPS && "A should be unit norm");
assert(fabs(B.norm()-1)<FLOAT_EPS && "B should be unit norm");
//// acos is always in [0,2*pi)
//return acos(fabs(A.dot(B)));
return fmod(2.*acos(A.dot(B)),2.*PI);
}
bool kinematic_constraints::OrientationConstraint::configure(const moveit_msgs::OrientationConstraint &oc)
{
//clearing out any old data
clear();
link_model_ = kmodel_->getLinkModel(oc.link_name);
if(!link_model_)
{
logWarn("Could not find link model for link name %s", oc.link_name.c_str());
return false;
}
Eigen::Quaterniond q;
tf::quaternionMsgToEigen(oc.orientation, q);
if (fabs(q.norm() - 1.0) > 1e-3)
{
logWarn("Orientation constraint for link '%s' is probably incorrect: %f, %f, %f, %f. Assuming identity instead.", oc.link_name.c_str(),
oc.orientation.x, oc.orientation.y, oc.orientation.z, oc.orientation.w);
q = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0);
}
if (oc.header.frame_id.empty())
logWarn("No frame specified for position constraint on link '%s'!", oc.link_name.c_str());
if (tf_->isFixedFrame(oc.header.frame_id))
{
tf_->transformQuaternion(oc.header.frame_id, q, q);
desired_rotation_frame_id_ = tf_->getTargetFrame();
desired_rotation_matrix_ = Eigen::Matrix3d(q);
desired_rotation_matrix_inv_ = desired_rotation_matrix_.inverse();
mobile_frame_ = false;
}
else
{
desired_rotation_frame_id_ = oc.header.frame_id;
desired_rotation_matrix_ = Eigen::Matrix3d(q);
mobile_frame_ = true;
}
std::stringstream matrix_str;
matrix_str << desired_rotation_matrix_;
logDebug("The desired rotation matrix for link '%s' in frame %s is:\n%s", oc.link_name.c_str(), desired_rotation_frame_id_.c_str(), matrix_str.str().c_str());
if (oc.weight <= std::numeric_limits<double>::epsilon())
{
logWarn("The weight on position constraint for link '%s' is near zero. Setting to 1.0.", oc.link_name.c_str());
constraint_weight_ = 1.0;
}
else
constraint_weight_ = oc.weight;
absolute_x_axis_tolerance_ = fabs(oc.absolute_x_axis_tolerance);
if (absolute_x_axis_tolerance_ < std::numeric_limits<double>::epsilon())
logWarn("Near-zero value for absolute_x_axis_tolerance");
absolute_y_axis_tolerance_ = fabs(oc.absolute_y_axis_tolerance);
if (absolute_y_axis_tolerance_ < std::numeric_limits<double>::epsilon())
logWarn("Near-zero value for absolute_y_axis_tolerance");
absolute_z_axis_tolerance_ = fabs(oc.absolute_z_axis_tolerance);
if (absolute_z_axis_tolerance_ < std::numeric_limits<double>::epsilon())
logWarn("Near-zero value for absolute_z_axis_tolerance");
return link_model_ != NULL;
}
TEST(TFEigenConversions, tf_eigen_quaternion)
{
tf::Quaternion t;
t[0] = gen_rand(-1.0,1.0);
t[1] = gen_rand(-1.0,1.0);
t[2] = gen_rand(-1.0,1.0);
t[3] = gen_rand(-1.0,1.0);
t.normalize();
Eigen::Quaterniond k;
quaternionTFToEigen(t,k);
ASSERT_NEAR(t[0],k.coeffs()(0),1e-6);
ASSERT_NEAR(t[1],k.coeffs()(1),1e-6);
ASSERT_NEAR(t[2],k.coeffs()(2),1e-6);
ASSERT_NEAR(t[3],k.coeffs()(3),1e-6);
ASSERT_NEAR(k.norm(),1.0,1e-10);
}