Error while planning with orientation constraints in Moveit

While planning with orientation constraints in moveit I get the following error

[ERROR] [/move_group] [1449487062.868776897]: Link names vector must have size: 5

for my 5DOF chain.

The reason can be found in the TRAC_IKKinematicsPlugin::getPositionFK method (line 348 -352)

  if(link_names.size() != num_joints_)
  {
    ROS_ERROR_NAMED("trac_ik","Link names vector must have size: %d",num_joints_);
    return false;
  }

The original kdl moveit plugin is not that specific on the link_names size.

bool KDLKinematicsPlugin::getPositionFK(const std::vector<std::string> &link_names,
                                        const std::vector<double> &joint_angles,
                                        std::vector<geometry_msgs::Pose> &poses) const
{
  ros::WallTime n1 = ros::WallTime::now();
  if(!active_)
  {
    ROS_ERROR_NAMED("kdl","kinematics not active");
    return false;
  }
  poses.resize(link_names.size());
  if(joint_angles.size() != dimension_)
  {
    ROS_ERROR_NAMED("kdl","Joint angles vector must have size: %d",dimension_);
    return false;
  }

  KDL::Frame p_out;
  geometry_msgs::PoseStamped pose;
  tf::Stamped<tf::Pose> tf_pose;

This suggests that some where in the moveit movegroup there is a call of FK with less link names than the number of DOF.

Comments (3)