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); }