Get Body Pose Relative to another Body

[ahundt]

How hard would it be to implement a function like the following?

GetTransformRelativeTo(multibodyGraph g, multiBodyConfig c, string objectname, string baseObjectName)

In V-REP I often use a function just like the above to get the transform from the frame of any object to any other. I'd love to have an equivalent of RBDyn, do you think this might be easy to implement?

Here is the equivalent V-REP function I use grl.getTransformBetweenHandles:

--- Get the pose of one object relative to another
---
--- @param objectpositionHandle The Object you want to get the position of
--- @param relativeToBaseFrameHandle The object or frame the position should be defined in, nil for world
--- @return position,orientation a 3 vector and quaternion value pair
grl.getTransformBetweenHandles=function(objectPositionHandle, relativeToBaseFrameHandle)
    local p=simGetObjectPosition(objectPositionHandle,relativeToBaseFrameHandle)
    local o=simGetObjectQuaternion(objectPositionHandle,relativeToBaseFrameHandle)
    return p,o
end

[gergondet]

The implementation is very easy:

using namespace rbd;
sva::PTransformd relativeTransform(const MultiBody & mb, const MultiBodyConfig & mbc,
                                   const std::string & object, const std::string & relativeTo)
{
  // X_rel_o = X_0_o * X_rel_0
  return mbc.bodyPosW[mb.bodyIndexByName(object)] *
         mbc.bodyPosW[mb.bodyIndexByName(relativeTo)].inv();
}

I'm not convinced this is a complex enough operation that we'd want it in the library.

Also, that V-REP function works regardless of the kinematic tree where the object here whereas that simple RBDyn implementation expects both objects to exist in the same tree. This can be easily improved though but that's left as an exercice ;)

[haudren]

I agree with @gergondet , the general problem solved by ROS' tf or other modules is how to aggregate a coherent transform tree from various sources. If, as in RBDyn, you have a well-defined reference (the world frame), it is very easy.

[ahundt]

I'm not convinced this is a complex enough operation that we'd want it in the library.

Why would an operation being simple preclude it from being included in a library? For example, std::copy or std::back_inserter are each pretty simple while simultaneously being worthwhile. I expect I'll be using relativeTransform quite a bit myself.

Do you mind if I submit a PR?

Also, that V-REP function works regardless of the kinematic tree where the object here whereas that simple RBDyn implementation expects both objects to exist in the same tree. This can be easily improved though but that's left as an exercice ;)

I assume you mean adding breadth first search?

I agree with @gergondet , the general problem solved by ROS' tf or other modules is how to aggregate a coherent transform tree from various sources.

It is best if I don't add ROS as a dependency at the moment, it is a pretty substantial dependency that is difficult to install considering I use more up to date libraries, and it has limitations on some platforms like windows.

If, as in RBDyn, you have a well-defined reference (the world frame), it is very easy.

Just to confirm every rbdyn configuration has a world frame?

Thanks for getting back to me on my questions!