Contact calculation

Discussion in 'Simulation' started by foolyc, Mar 20, 2018.

  1. i have a geom "foot"(condim=3), slide friction=0.7, i calculate the wrench from the contact force
    Code:
    F=sigma(Fi)
    M=sigma(cross(Fi, com - pi))
    where com is the data.xipos of the body, pi is the contact point in data.contact.pos.
    for each contact point Fi is calculated from data.efc_force by multiply E

    1) But the force is the same with data.cfrc_ext , while the torque is not.what is the problem?
    2) i want to calculate the constraint jacobian according to the wrench of the body("foot"), while eq below
    Code:
    Jac(foot) .T* wrench(foot) = (data.efc_J).T * data.efc_force 
    does not match, in which wrench(foot) reffer to the data.cfrc_ext of the foot body.



    Code:
    data.efc_force :  [11.85146135 13.42744924 13.01199359 12.266917  ]    contact point :  [-0.12220128  0.02669812  0.00031927]
    data.efc_force :  [36.21655201 37.8791954  37.596309   36.49943841]    contact point :  [0.13779852 0.02664688 0.00016388]
    data.efc_force :  [15.67559638 17.2787631  16.96416511 15.99019437]    contact point :  [-0.12217669  0.15158412  0.00031007]
    data.efc_force :  [42.36771452 44.06469799 43.93559766 42.49681485]    contact point :  [0.13782311 0.15153288 0.00015468]
    force from contact:  [ -2.97829051  -4.57714703 437.52285994] force from data.cfrc_ext:  [ -2.97829051  -4.57714703 437.52285994]
    torque from contact:  [  2.42543742 -35.73485602  -0.08754942] torque from data.cfrc_ext:  [37.54898519 -2.28141429  0.50151569]