Open dave992 opened 5 months ago
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.
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.
@Levi-Armstrong Thanks for the answer. Is this the CheckInputTask
you are referring to?
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)
Thanks, I will have a look at the function!
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: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:
Inspecting the
checkJointPositionFormat
function, I see that thejoint_names
are compared to the names defined in the waypoint: tesseract_command_language/src/utils.cpp#L280-L296:Here the
joint_names
parameter seems to be retrieved from the kinematic info. Since it is using the==
operator from thestd::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 theJointWaypoint
, as long as they correspond to the correct values.Additional questions
joint_names
defined? Is this just the order as they appear in the URDF/SRDF?joint_names
order? I currently use the following: