Joint to World Matrix

Is there currently a function that provides the local to world transformation matrix for joints (for me in 2D). If not how would you suggest to do it.
My setting is a multibody with one freejoint and multiple revolute joints connecting the different parts. For each of these links i would need a local to worldspace matrix. One way would be if i could somehow access the world coordinates of the two ends of a bpdypart. (then i can easily genereate the translation and rotation parts from these). Could you help me out on this one or point me in the right direction.
Thanks a lot for your help


You can retrieve the world transformation of a body part (which is in your case a MultibodyLink) by calling its position method.

If what you need is the position of the point of application of the joint itself, you should be able to compute it by using the multibody link’s position and body_shift methods. Something like:

let joint_transform = link.position() * Translation2::from(-link.body_shift())

The body_shift is the position of the link Joint wrt. the MultibodyLink itself. This is expressed in the local-space of the joint.