Hello, I am simulating a 7 DoF robotic arm, and I would like it not to feel any "pushback" when interacting with objects in the world. To do so, I am trying to find the total contact force on its end-effector, and apply its opposite. Here are the steps I am following: For each of the contacts with the end effector geom, do : 1. Find the contact force/torque in the contact frame f_contact , tau_contact using mj_contactForce 2. Find the contact position p_contact using mjContact->pos (Question 1 : which frame is this expressed in ? world frame or contact frame?) 3. Find the contact frame R_world_contact using mjContact -> frame (Question 2: My understanding is that this gives the 3x3 rotation matrix of the contact frame in the world frame ; is that right?) 4. Find the body_id of the body that is parent to the end effector contact geom. 5. Find the contact force, torque into the world frame using f_world= R_world_contact.dot(f_contact), tau_world = R_world_contact.dot(tau_contact). 6. Find the contact position in the body frame by: a) Finding the body_xpos of the body centre in the world frame b) finding the body_xmat of the body frame in the world frame c) Getting the contact point in the body frame through p_body = body_xmat.transpose.dot(p_contact-body_xpos) 7. Find the generalised force on all the joints from this contact force using mj_applyFT(model,data, [f_world, tau_world], p_body, body_id, placeholder_array) (Question 3: when mj_applyFT does the point need to be in the local body coordinates of the body the force will be applied to ? Is the force/torque in Cartesian world coordinates?) 8. Applying the opposite torque to each joint : qfrc_applied = qfrc_applied - placeholder_array Note: I have also tried skipping step 6. i.e. using p_contact instead of p_body in mj_applyFT, but this still does not seem to work! Question 4: Is that the correct way to find all the contact forces that will be exerted on my end-effector and nullify it on the fly? I suspect the issue lies somewhere in the questions I asked, but please if someone can spot anything else wrong with this procedure let me know! Thanks!