Description

I am attempting to get the Pilz Industrial Motion Planner working with subframes. I am an intern and I would appreciate any help guiding me in the right direction to solve this issue, after which I can try and fix it myself and create a pull request with the help of @aa-tom. I would appreciate the help of @cambel and @felixvd for this!

My Environment

• ROS Distro: Noetic
• OS Version: Ubuntu 20.04
• Using Moveit! with the most up to date debian binaries, with the exception of moveit-core and moveit-ros which I compile locally to have all the up to date PRs relating to subframes, as well as Pilz which is compiled locally in order to implement a bugfix relevant to our project. Pilz is up to date with master.

Steps to Reproduce

My assumption is that any attempt to plan a path using Pilz with a subframe as the end effector link will create this error.

Action Taken So Far

• Tested with an identical stack except with OMPL planner in the pipeline. OMPL works as intended, but Pilz does not. Proof:

• Identified that the error message comes from the following code in trajectory_generator.cpp:

if (!robot_model_->getJointModelGroup(group_name)->canSetStateFromIK(pos_constraint.link_name))
 {
   std::ostringstream os;
   os << "CONFIRM: No IK solver available for link: \"" << pos_constraint.link_name << "\"";
   throw NoIKSolverAvailable(os.str());
 }

My assumption is that I don't need to worry too much about the specifics of what pos_constraint and canSetStateFromIK do, and that this is just the first step that was trying to validate that the use of the link will work, and that it fails to recognise subframe syntax at this stage.

• Confirmed that Pilz works as intended when subframes aren't used

• Tested commenting out the lines of code within Pilz that are throwing the issue, with the hopes that it was simply trying to validate the link but did not ultimately need it. The following code was commented out: trajectory_generator.cpp L168 -> L173 and trajectory_functions.cpp . L57 -> 61

  And the following error message persisted:

[ INFO] [1632298952.271913298] [/fake/move_group]: Generating PTP trajectory...
[ERROR] [1632298952.271976357] [/fake/move_group]: Link 'ee1/centre_blade' not found in model 'yaskawa'
[ERROR] [1632298952.272000187] [/fake/move_group]: Pose frame 'ee1/centre_blade' does not exist.
[ERROR] [1632298952.272256838] [/fake/move_group]: Inverse kinematics for pose 
 0.5
 0.4
0.35 has no solution.
[ERROR] [1632298952.272329973] [/fake/move_group]: No IK solution for goal pose

It is worth noting that this error is identical to the error I receive when I try and plan using ompl and I have not actually created that subframe within the relevant object.

Expected Behaviour

• Same as with OMPL, successful planning using subframe syntax for the end effector link.

Actual Behaviour

• Pilz seems to be attempting to validate the link (which is in subframe syntax eg. "tool/tip_centre") it will use when planning, at which point it throws the following error:

[ INFO] [1632302469.711223079] [/fake/move_group]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1632302469.711497997] [/fake/move_group]: Using planning pipeline 'pilz_industrial_motion_planner'
[ INFO] [1632302469.711654579] [/fake/move_group]: Initialized Point-to-Point Trajectory Generator.
[ INFO] [1632302469.711811425] [/fake/move_group]: Generating PTP trajectory...
[ERROR] [1632302469.711940790] [/fake/move_group]: CONFIRM: No IK solver available for link: "ee0/central_camera"