[BUG] Scene getObjectGraspPose() implementation incorrectly incorporates Robot's pose

[scchow]

The docstring of the getObjectGraspPose() function defined in scene.cpp says the function should return a grasp pose in the global frame. However, looking at the source code, it appears to be:

• Start with the transform from the world to the robot
• Multiply by the transform from the world to the object
• Multiply by the offset transform

This corresponds to T(world->robot) * T(world->object) * T(offset), which does not seem to be a valid transform.

Also, I am not sure why the current robot's pose is used in computing the object's grasp pose, since we are returning the grasp pose in the global frame.

I have a branch where I have removed the robot's pose from the computation and can submit a PR, but I wanted to ask whether I am missing something.

Expected Behavior

I would expect the function to return the object's current position in the global frame offset by the provided offset.

Current Behavior

The function returns the result of multiplying: the transform from the world to the robot transform from the world to the object offset transform, which does not seem to correspond to the object's location in the global frame (unless the robot is located at the origin of the world).

Proposed Solution

Remove the robot pose from the calculation. e.g., just T(world->object) * T(offset)

Steps to Reproduce

1. Create a scene with a robot in a position not at the origin and an object to be grasped.
2. Call getObjectGraspPose(object_name, offset) and observe that the resulting pose does not match the object's location.

Context (Environment)

I've been running a series of IK queries on a robot with a mobile base to grab an object. When I specify the IKQuery using position and orientation, everything seemed to work. However, when running IKQuery using an object in the scene, the solution did not appear to match the object's location,

It turns out IKQuery uses getObjectGraspPose() to find a solution end effector pose when one isn't directly provided, and since my robot does not start at the origin, the resulting end effector pose was incorrect.

[zkingston]

Thanks for reporting. Taking a closer look at the function, I believe you are correct and the transform with respect to the robot pose should be removed, as the object's pose is already in the global frame. Please submit the PR.