Example #1
0
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);
}