# Finding all contact forces on an object to apply their opposite.

Discussion in 'Simulation' started by Eugene Valassakis, Jan 27, 2020.

1. ### Eugene Valassakis

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?)
(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!

Last edited: Jan 28, 2020