PILZ Industrial motion planner is ignoring the goal frame_id passed

[Danilrivero]

Description

Seems to be directly related to the following issues merged in MoveIt1:

moveit/moveit#3035
moveit/moveit#3522
moveit/moveit#3519

I am using PILZ to plan trajectories with LIN. Planning with OMPL, you can set the goal constraint frame_id as any frame present in the planning scene and the planner correctly plans the motion correctly. This does not seem to be the case with PILZ in MoveIt2.

Your environment

• ROS Distro: Humble
• OS Version: Ubuntu 22.04
• Source or Binary build? Binary
• If binary, which release version? 2.5.5-1
• If source, which branch? -
• Which RMW? Cyclone DDS

Steps to reproduce

• Create a MotionPlanRequest with a frame_id inside the TF frame or collision object sub-frame present in the planning scene.
• The planning will fail with PILZ.

Expected behaviour

By planning a linear movement with LIN setting a TargetPose in a certain frame it shall plan properly by knowing the RobotState.

Actual behaviour

The following ERROR happens:

[move_group-8] [ERROR] [1716888985.952557659] [moveit.pilz_industrial_motion_planner.trajectory_functions]: Given frame (ur_base_link) is unequal to model frame(world)

Backtrace or Console output

• Using LIN reports the following:

MoveGroup action client/server ready
[move_group-8] [INFO] [1716888985.952069236] [moveit_move_group_default_capabilities.move_action_capability]: Received request
[Node-10] [INFO] [1716888985.952241475] [move_group_interface]: Planning request accepted
[move_group-8] [INFO] [1716888985.952255668] [moveit_move_group_default_capabilities.move_action_capability]: executing..
[move_group-8] [INFO] [1716888985.952350877] [moveit_move_group_default_capabilities.move_action_capability]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[move_group-8] [INFO] [1716888985.952366258] [moveit_move_group_capabilities_base.move_group_capability]: Using planning pipeline 'pilz_industrial_motion_planner'
[move_group-8] [INFO] [1716888985.952446866] [moveit.pilz_industrial_motion_planner.trajectory_generator]: Generating LIN trajectory...
[move_group-8] [ERROR] [1716888985.952557659] [moveit.pilz_industrial_motion_planner.trajectory_functions]: Given frame (ur_base_link) is unequal to model frame(world)
[move_group-8] [ERROR] [1716888985.952625855] [moveit.pilz_industrial_motion_planner.trajectory_generator]: Failed to compute inverse kinematics for link: gripper_tool0 of goal pose
[move_group-8] [INFO] [1716888985.952667417] [moveit_move_group_default_capabilities.move_action_capability]: Unknown event
[Node-10] [INFO] [1716888985.953047990] [move_group_interface]: Planning request aborted
[Node-10] [ERROR] [1716888985.953390414] [move_group_interface]: MoveGroupInterface::plan() failed or timeout reached
[Node-10] [ERROR] [1716888985.953423159] [pick_place_executor]: Planning failed when planning with LIN pilz_industrial.

• I assume the same issue arises with PTP and CIRC.

---

I do not have the bandwidth to backport these changes but could if not solved yet in the near future, opening this issue to let it be known in case. Having a backport to humble and updating the binaries would be appreciated as well.

[sjahr]

Thanks for reporting this and pointing out the moveit solution. Either us or somebody from the community can port it over if you don't have the bandwidth.

[liver121888]

Hi, I tried to use adapter "default_planner_request_adapters/ResolveConstraintFrames" to solve this but in vain.
Any update on this? I faced the same issue and wanted to know if there was a solution.

[Danilrivero]

Hi, I tried to use adapter "default_planner_request_adapters/ResolveConstraintFrames" to solve this but in vain. Any update on this? I faced the same issue and wanted to know if there was a solution.

Hi, I still have this issue in my to-do list to make a PR with the fix as soon as possible, as a temporary fix we resolved to transform the target_pose or waypoint frame_id to the root frame of your tf_tree (which is the one PILZ asks for as of now due to not being able to solve the RobotState)

You can do something like this:

target_pose = TransformPose(target_pose, move_group->getRobotModel()->getModelFrame(), tf_buffer_);

Where we assign to reference_frame the value of move_group->getRobotModel()->getModelFrame().

Cheers

[TSNoble]

I've tested the humble backport of #3055 from a build-from-source copy of my fork, and I'm still having issues with LIN plans and subframes. The error I'm getting is something like frame <subframe> does not exist for <robot>, which then propagates to an IK error (just left work so I don't have the exact message to hand 😅 )

It seems like something in the pilz code isn't taking collision objects and / or their subframes into account when determining valid planning frames.

@sjahr It might be worth re-opening this issue and I'll look into the code in more depth tomorrow?

[TSNoble]

I think I might have found the cause:

ROS1 Moveit explicitly sets the frame id to the model frame, and appears to transform the start and goal poses to account for a different frame id (e.g. a subframe goal):

https://github.com/moveit/moveit/blob/f1a607129a6b5183c9296b29e0d126d9b48e7ea8/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp#L125-L140

ROS2 Moveit appears to only set the frame id to the model frame if no frame is specified (which for non-model-frame ids will immediately throw an error in computePoseIK), and does not apply these tranformations:

moveit2/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp

Lines 103 to 131 in 1a77d7d

	// goal given in Cartesian space
	else
	{
	info.link_name = req.goal_constraints.front().position_constraints.front().link_name;
	if (req.goal_constraints.front().position_constraints.front().header.frame_id.empty() ||
	req.goal_constraints.front().orientation_constraints.front().header.frame_id.empty())
	{
	RCLCPP_WARN(getLogger(), "Frame id is not set in position/orientation constraints of "
	"goal. Use model frame as default");
	frame_id = robot_model_->getModelFrame();
	}
	else
	{
	frame_id = req.goal_constraints.front().position_constraints.front().header.frame_id;
	}
	info.goal_pose = getConstraintPose(req.goal_constraints.front());
	}
	computeLinkFK(robot_state, info.link_name, info.start_joint_position, info.start_pose);
	// check goal pose ik before Cartesian motion plan starts
	std::map<std::string, double> ik_solution;
	if (!computePoseIK(scene, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position,
	ik_solution))
	{
	std::ostringstream os;
	os << "Failed to compute inverse kinematics for link: " << info.link_name << " of goal pose";
	throw LinInverseForGoalIncalculable(os.str());
	}

Edit: I just noticed these are exactly the changes addressed in moveit/moveit#3522 😅

I'll have a go at porting it tomorrow

[TSNoble]

I believe this specific issue should be fixed once #3075 is merged. @Danilrivero @liver121888 would you be able to test humble from source when it's ready to check that this is the case?

The issue I was still running into (The following Pose Frame does not exist: <subframe>) is slightly different (it's thrown when calling RobotState::setFromIK with a subframe as the tip), but related as pilz calls this method with a subframe in some situations. I've raised it as a separate issue #3072 and fix #3077