Extract transforms between two frames from robot model

[domire8]

It would be nice (for the robot state broadcaster https://github.com/aica-technology/modulo-controllers/issues/67) if there was a method that can directly give transforms between two frames of the robot model.

Additionally, it'd also be nice to have a helper that can tell us which joints in the model are fixed, and which are movable

[domire8]

@JasperTan97 this can be a nice side task for you if you have spare time in between other tasks.

Right now, we have forward_kinematics that give us the transform from the base frame of the robot to a desired frame passed as argument of the function. It would be great if we could extend that and not just provide the target frame but also the base frame. Like that, function calls like forward_kinematics(joint_positions, "link6", "link3") would give us the transform from link3 to link4.

It would great if you could look into that and see how that could be implemented as efficiently as possible. Maybe pinocchio already provides something like that so we could just wrap it (or a more recent version of pinocchio, I don't know)

[JasperTan97]

Like that, function calls like forward_kinematics(joint_positions, "link6", "link3") would give us the transform from link3 to link4.

Is this a typo, or what exactly would the inputs of this function do. Currently there is

   * @brief Compute the forward kinematics, i.e. the pose of certain frames from the joint positions for a single frame
   * @param joint_positions the joint state of the robot
   * @param frame_id id of the frames at which to extract the pose
   * @return the desired pose

[domire8]

The last work in that sentence should be link6 but the rest is not a typo.

The inputs of the function would decide from where to where the transform is. Right now, it's always from the base frame to the frame_id provided as argument. But we might be interested in the transform from link3 to link6, which we cannot get easily at the moment.

[JasperTan97]

So something like:

• @brief Compute the forward kinematics, i.e. the pose of certain frames from the joint positions for a single frame
• @param joint_positions the joint state of the robot
• @param frame_id id of the frames at which to extract the pose
• @param reference frame_id for ^, with default frame "base" or "world"
• @return the desired pose

[domire8]

yes, reference frame would default to the first element in the list of frames (frame_id defaults to the last element in the list of frames)

[domire8]

I took some time to look into our robot model class and pinocchio and compared it to what kind of functionality I was looking for here and I came to the conclusion that we should move this back to the deep deep backlog. I don't think that we can add nice functionality here without a proper effort to improve the robot model class. This would require a major time effort and someone that would try to understand pinocchio properly and that's not something we should do at this point as there is no real need (simplifying a few lines and removing some very lightweight dependencies in the modulo controllers does not count as real need unfortunately 😄).

In that sense I think #145 should be closed without merging. Sorry for the inconvenience.

@eeberhard @JasperTan97

[eeberhard]

Happy to follow your judgement on that. If a proper solution is too much effort and the underlying issue is small, I agree it doesn't make sense to spend time on it.