Transforming contact forces from the contact frame to the local frame of a geom involved in said contact/collision?

[kurtenkera]

Hi,

I am using MuJoCo via Python, and I am currently using the code below to extract information about contact forces in a given simulation:

forcetorque = np.zeros(6)

for i, con in enumerate(data.contact):
  mujoco.mj_contactForce(model, data, i, forcetorque)
  con_force_i = forcetorque[0:3]                    # ith contact force expressed in contact frame
  con_frame_i = con.frame.reshape(3,3)      #ith contact frame expressed in global coordinates as a 3x3 matrix
  geom1_id = con.geom1                             # id of geom1
  geom2_id = con.geom2                             # id of geom2

This is my objective: For any ith contact pair, I need to express the associated con_force_i in the frame of the geom whose id is given by geom1_id (i.e. geom1), and also in the frame of the geom whose id is given by geom2_id (so I can analyse the forces acting on individual geoms in a simulation). How do I do this?

I have attempted to generate formulae which will do this, and the formulae seem to work on a couple of cases that I have tested them on - but I am far from confident that these formulae will hold in the general case. I am hoping someone can confirm whether my formulae are correct in the general case or not?

This is my current hypothesis/formulae: Let con_force_geom1frame be the contact force expressed in the frame of the geom with id geom1_id, and let con_force_geom2frame be defined analogously. I am hypothesising (but not convinced in the general case) that the following holds:

con_force_geom1frame = (data.geom_xmat[geom1_id ].reshape(3,3).T) @ con_frame_i @ con_force_i
con_force_geom2frame = (data.geom_xmat[geom2_id ].reshape(3,3).T) @ con_frame_i @ -con_force_i

My rationale behind the formulae above:

(1) con_force_i is negated in the expression for con_force_geom2frame due to Newton's 3rd law. (2) In both cases, con_frame_i is a rotation matrix rotating the contact force from the contact to global frame (I think)? (3) data.geom_xmat[geom1_id].reshape(3,3).T rotates from the global frame to the local frame of the geom1, and data.geom_xmat[geom2_id].reshape(3,3).T rotates from the global frame to the local frame of geom2?

Please let me know if these formulae are correct in the general case, and if not, how I can correct them?

[yuvaltassa]

You're in the right direction but missing an important bit. A frame transform is both a translation and rotation, you're missing the translation. What you want is to go via the world frame like (contact_frame → world_frame → geom_frame).

If y is in the geom frame and x is in the world frame then x = geom_xmat.T @ y + geom_xpos, and y = geom_xmat @ (x - geom_xpos).

I think you should be able to get it from here, just note the caveat: mjContact.frame, uniquely in MuJoCo, is column major rather than row major (i.e. transposed). Let me know if you couldn't figure it out 🙂

[kurtenkera]

Ah so if z is in the contact frame and x is in the geom frame then x = geom_xmat.T @ mjcontact.frame @ (z - contact_reference_pos), where contact_reference_pos is some reference position in the contact frame needed to capture the translation you mentioned?

[yuvaltassa]

You're once again forgetting a translation.

Let's transpose and re-order as explained in the link above and get a regular xmat: contact_xmat = contact.frame.reshape(3,3).T. Transpose to put the axes on the columns. So if f is the force we got from mj_contactForce then

x = contact_xpos + contact_xmat.T @ f
y = geom_xmat @ (x - geom_xpos) 

note I flipped x and y above since x in the world frame is more MuJoCo-esque.

[kurtenkera]

Thanks for the help @yuvaltassa, this makes sense.

For my purposes, I only need the correct direction and magnitude of f in the geom frame (it won't matter if f is a vector whose base is located at the correct contact position or not). So while I can see that your equations above for x and y would give f in the geom frame at the correct contact position, is it true that something like f_geom = geom_xmat @ contact_xmat.T @ f would give a contact force with the correct direction and magnitude (but obviously not the correct contact position in the geom frame)?

[yuvaltassa]

I don't think so, but I might be wrong.

[kurtenkera]

Hi @yuvaltassa. I believe the following rule you quoted above is wrong:

If y is in the geom frame and x is in the world frame then x = geom_xmat.T @ y + geom_xpos, and y = geom_xmat @ (x - geom_xpos).

It should instead be:

If y is in the geom frame and x is in the world frame then x = geom_xmat @ y + geom_xpos, and y = geom_xmat.T @ (x - geom_xpos).

I have played around with this extensively in MuJoCo and this has given me correct results, whereas the rule you quoted was giving me incorrect results. This also agrees with the rules for coordinate transforms explained here and Emo Todorov's comments in this old thread (i.e., "geom_xmat is a matrix that rotates from local to global coordinates").

Also, for a force f measured in the contact frame, we do not want to apply translations as doing so won't preserve the magnitude of f. Letting contact_xmat = contact.frame.reshape(3,3).T and applying the updated rule above, to obtain f_geom we would need to do f_geom = geom_xmat.T @ contact_xmat @ f. Since geom_xmat and contact_xmat are orthogonal matrices, it's easy to confirm the magnitude of f_geom equals the magnitude of f (but this isn't true if we include translations). Of course, we would need to apply the translations if we were transforming a point (not a force vector) in the contact frame.

I just wanted to include these comments (1) in case other users need to perform similar calculations and (2) in the off-chance I'm mistaken!

[yuvaltassa]

Yes of course. Thanks for correcting!

The moral of the story is: believe the code that works, not the code written from memory 😛

If you find a good place to add this to the docs, feel free to send a PR!

Maybe here?

https://mujoco.readthedocs.io/en/latest/programming/simulation.html#coordinate-frames-and-transformations