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

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

  1. 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=, tau_world =
    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 =
    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!

    Last edited: Jan 28, 2020