Description

Hi, I'm using pilz_industrial_motion_planner to plan some trajectories interfacing directly with it through some calls to /move_group action server.

I noticed that if I plan with OMPL pipeline, I can set the goal constraint frame_id as any frame present in the planning scene (TF, collision objects sub-frames) and the planner correctly plan to the right goal as desired.

Instead planning with pilz_industrial_motion_planner pipeline:

• PTP simply ignores the goal constraint frame_id and instead it assumes that goal position and orientation constraints are respect to the robot model frame.
• LIN always fails reporting:

[ INFO] [1643103242.859113763]: Using planning pipeline 'pilz_industrial_motion_planner'
[ INFO] [1643103242.859239956]: Generating LIN trajectory...
[ERROR] [1643103242.859357051]: Given frame (goal_link) is unequal to model frame(world)
[ERROR] [1643103242.859459598]: Failed to compute inverse kinematics for link: tool0 of goal pose

• CIRC I've not tested yet but I suppose there is some issue there too.

Your environment

• ROS Distro: [Noetic]
• OS Version: Ubuntu 20.04
• Binary build
• 1.1.7

Steps to reproduce

• Create a MotionPlanRequest with a goal constraint frame_id equal to a TF frame or collision object sub-frame present in the planning scene;
• Send a goal with that motion request to /move_group action server.

Expected behaviour

Plan taking account of the goal constraint frame_id in the planning request as does OMPL.

Actual behaviour

• PTP ignores the goal constraint frame_id.
• LIN always fails.
• CIRC not yet tested.