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.
let joint_transform = link.position() * Translation2::from(-link.body_shift())
body_shift is the position of the link
Joint wrt. the
MultibodyLink itself. This is expressed in the local-space of the joint.