Description

RobotState::setFromIK() returns false when the tip frame passed is a subframe.

This is due to the method only checking if the frame is the name of an attached body.

This is in contrast to the ROS1 code, which calls getRigidlyConnectedParentLinkModel(), which is subframe aware.

The code is a bit different in ROS2, however, RobotState already has a method for looking up frames with subframe awareness.

As such, I think the call to hasAttachedBody() should be replaced with getFrameTransform().

ROS Distro

Humble

OS and version

Ubuntu 22.04

Source or binary build?

Binary

If binary, which release version?

2.5.5

If source, which branch?

N/A

Which RMW are you using?

None

Steps to Reproduce

1. Call RobotState::setFromIK() where the tip parameter is a subframe
2. The code will error and return false

Expected behavior

The call should recognise that the tip is a subframe, return true, and set the robot state accordingly. This is consistent with ROS1 behaviour.

Actual behavior

The code prints an error "The following Pose Frame does not exist: ", and returns false.

Backtrace or Console output

No response