Description

We are seeing a problem where sometimes a PILZ PTP plan to a pose target ends at the wrong pose. When the pose is "wrong", it is off by ~0.1mm (compared to an ~1m robot), and when it is "right" it is correct to ~1e-15m.

Debugging the problem, the issue seems to be the calls to pose_parent_to_frame = getFrameTransform(pose_frame) and tip_parent_to_tip = getFrameTransform(solver_tip_frame) in RobotState::setFromIK.

• The two calls to getFrameTransform both call updateLinkTransforms.
• This means the RobotState can update between the calls.
• This means that pose_parent_to_frame.inverse() * tip_parent_to_tip is not the rigid transform from pose_frame to solver_tip_frame, because it includes the change from updating the robot state.

My proposed solution is to add an overload to getFrameTransform which takes a source_frame_id and destination_frame_id, rather than just one frame_id, and returns the transform from source_frame_id to destination_frame_id. Importantly, this overload would only call updateLinkTransforms once, before fetching both pose.

Does this sound like a plausible diagnosis of this error? Does my proposed solution seem sensible?

I'll raise a PR when I've done a bit more testing.

ROS Distro

Humble

OS and version

Ubuntu 22.04

Source or binary build?

Source

If binary, which release version?

No response

If source, which branch?

cc3a82d (humble plus a few minor and unrelated tweaks)

Which RMW are you using?

None

Steps to Reproduce

• Set up a 6DOF robot arm
• Attach a collision object to the tip of the arm
• Perform a PILZ PTP plan, to a pose target specified using a subframe of the collision object.
• Perform FK on the final joint state of the resulting trajectory, and check for self-consistency with the target pose.
• Run this many times on a slow computer.

Expected behavior

A PTP trajectory to a pose target should end at that pose (up to numeric error).

Actual behavior

A PTP trajetory to a pose target sporadically ends at a slightly different pose to the target.

Backtrace or Console output

No response