PILZ LIN planner calculates first IK using start state containing wrong joint values for joints outside the current joint model group.

[rr-mark]

Description

Context:

we are planning a PILZ LIN path, where one robot (in one joint model group) moves past another robot (in another joint model group).

The request has a start state where the second robot has moved out of the way of the first, but in the actual scene (and so in the PlanningSceneMonitor) the robot has not yet moved.

When we try to plan this move, collisions are reported between the two robots, even though the second robot should already be out of the way.

Technical details:

In TrajectoryGenerator::generate:

• scene is the current planning scene, taken from the PlanningSceneMonitor (second robot has not moved).
• req.start_state contains the start state request (second robot has moved).
• MotionPlanInfo plan_info is constructed; plan_info.start_joint_position only contains the joints from req.start_state which are in the current JointModelGroup.
• extractMotionPlanInfo is called (in this case, calling the TrajectoryGeneratorLIN variant).

In TrajectoryGeneratorLIN::extractMotionPlanInfo:

• The goal constraint is cartesian, so computePoseIK is called, taking info.start_joint_position as its seed.

In computePoseIK:

• RobotState rstate is constructed from scene, with joints updated from seed. This leaves a state where the second robot has not moved.
• This seed is then used for IK, where a collision between the robots is detected.

This is a problem in Humble, but not in Noetic. I assume a PR in ROS1 hasn't been ported to ROS2, but I can't find any relevant differences in the relevant code.

Proposed solution:

In TrajectoryGeneratorLIN::extractMotionPlanInfo, the computePoseIK call should not use info.start_joint_position as its seed, but rather a map containing all joints (including joints not in the current joint model group).

If this solution is sensible, I will write it. But if there's a better fix (ideally just porting an existing fix from ROS1) I will do that instead.

ROS Distro

Humble

OS and version

22.04

Source or binary build?

Source

If binary, which release version?

No response

If source, which branch?

Humble

Which RMW are you using?

None

Steps to Reproduce

• Start with a scene containing two robots.
• Plan a LIN motion with cartesian goal, where the first robot collides with where the second robot currently is, but where the second robot is moved in `request.start_state).

Expected behavior

The plan should succeed.

Actual behavior

The plan reports a collision and fails.

Backtrace or Console output

No response

[rr-mark]

For context, this is the same problem we saw in moveit1 in moveit/moveit#3519 (comment) . As far as I can tell, that PR has been entirely ported to Humble, so I'm confused why we're seeing it again.

[rr-mark]

Hmm, actually, reading through #3519 it looks like the problem is that collisions are being checked at all by computePoseIK - this collision check is redundant with the collision check performed after pilz has created the trajectory.

I'll try to work out why this collision check is happening in Humble when it isn't in Noetic.

[rr-mark]

Having said that, the comments in #3519 don't seem to match what Noetic is doing any more - computePoseIK defaults check_self_collision to true in both Noetic and Humble, and this is not overridden when it is called in extractMotionPlanInfo in either distribution.

[MarcoMagriDev]

Could PR #1856 have introduced the issue you are facing?

---

Maybe the problem is not present in ROS1 since this change was not backported to MoveIt1.
At the same time, if this is the cause, it seems like you were relying on the previous behavior where computeLinkFK implicitly set default states.

[rr-mark]

Could PR #1856 have introduced the issue you are facing?

Maybe the problem is not present in ROS1 since this change was not backported to MoveIt1. At the same time, if this is the cause, it seems like you were relying on the previous behavior where computeLinkFK implicitly set default states.

I don't think so? As far as I can tell, moveit/moveit#3519 largely ported #1856 and unified the two distributions?

Also, the full state is present in both the PlanningSceneMonitor scene, and in the req.start_state we pass in, so I don't think this is a default state problem.