Joint names for waypoints need to be ordered

[dave992]

I ran into some assertion errors when creating a plan using waypoints created from information from the SRDF. To be more specific, using the tesseract_environment::Environment I can get a group joint state from the SRDF:

std::string group_name("manipulator");
std::string state_name("home");

auto kinematic_info = env->getKinematicsInformation();
std::unordered_map<std::string, double> group_joint_state = kinematics_info.group_states[group_name][state_name];

This information is used to create a tesseract_planning::JointWaypoint. I would expect it does not matter in what order I define the joint names and their values, as long as I define them all.

Using this approach I sometimes run into assertion errors when creating a plan. More specifically here: tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp#L280:

assert(checkJointPositionFormat(joint_names, move_instruction.getWaypoint()));

Inspecting the checkJointPositionFormat function, I see that the joint_names are compared to the names defined in the waypoint: tesseract_command_language/src/utils.cpp#L280-L296:

bool checkJointPositionFormat(const std::vector<std::string>& joint_names, const WaypointPoly& waypoint)
{
  if (waypoint.isJointWaypoint())
    return (joint_names == waypoint.as<JointWaypointPoly>().getNames());

  if (waypoint.isStateWaypoint())
    return (joint_names == waypoint.as<StateWaypointPoly>().getNames());

  if (waypoint.isCartesianWaypoint())
  {
    const auto& cwp = waypoint.as<CartesianWaypointPoly>();
    if (cwp.hasSeed())
      return (joint_names == waypoint.as<CartesianWaypointPoly>().getSeed().joint_names);
  }

  throw std::runtime_error("Unsupported waypoint type.");
}

Here the joint_names parameter seems to be retrieved from the kinematic info. Since it is using the == operator from the std::vector, it check if the vectors are equal, not if they contain the same information.

Expected behavior

The order of the joint_names does not matter, at least for the JointWaypoint, as long as they correspond to the correct values.

Additional questions

• How is the correct order of the joint_names defined? Is this just the order as they appear in the URDF/SRDF?
• How do you recommend to retrieve the correct joint_names order? I currently use the following:

  std::vector<std::string> joint_names = env->getKinematicsInformation().joint_groups[group_name_];

[rjoomen]

I noticed this issue as well. In many places there is no fixed coupling between the order of joint names and values. I think it would be good to somehow make this more consistent and robust through Tesseract.

[Levi-Armstrong]

This is intended behavior for performance reason. Forward and Inverse kinematics is called thousands of time during a motion planning and supporting unordered data at the low level would be a significant performance hit. Instead there is a task which you can added to your pipeline which will format the program to correctly work with motion planning. This way you just pay the penalty once.

[dave992]

@Levi-Armstrong Thanks for the answer. Is this the CheckInputTask you are referring to?

[Levi-Armstrong]

Well I was wrong about the task existing but the following utility function exists in tesseract_motion_planners utils.cpp to format the input. I will work on creating a task which leverages this.

bool formatProgram(CompositeInstruction& composite_instructions, const tesseract_environment::Environment& env)

[dave992]

Thanks, I will have a look at the function!