Current Contact Jacobian and dJacobian

[dhruvthanki]

I want to access the value of the contact jacobian (J(q)) and its derivative (dJ(q,dq)) at the current state [q, dq] at a particular point on the robot and the value of contact force between these 2 contacting geoms. e.g the foot making contact with the ground. I want to add the following constraints to a qp:

$$ M(q)\ddot{q} + H(q,\dot{q}) = Bu + J^T\lambda $$

$$ J(q)\ddot{q} + \dot{J}(q,\dot{q}) = 0 $$

How do I go about doing this?

[yuvaltassa]

Hi,

1. Regarding the contact Jacobian itself, this can be accessed as follows. Each contact in the data->contact array contains an efc_address integer, which tells you what is the first row in the Jacobian that corresponds to this contact. The actual number of rows depends on the dimensionality of the contact (condim) and whether you are using elliptic or pyramidal contacts. If you are using sparse Jacobians you'll need to read the sparse format, if dense you can just read from efc_J. I recommend making a small test model which will default to dense (under 60 DoFs) and experimenting with elliptic and pyramidal. This is explained here.
2. Regarding the derivative of the Jacobian, derivative with respect to what? It looks like, from your equations, that you want derivative with respect to time? For this you would need to finite difference, there is currently no way to compute dJ/dt analytically. I have not tried this but the following should work. At the end of mj_forward() (not mj_step(), since you don't want the state to change), save the Jacobian. Then call mj_integratePos, passing in data->qpos and data->qvel with some tiny epsilon in the last argument to nudge the positions. Then call mj_makeConstraint which computes the nudged Jacobian and fin-diff. Note that this will give you a possibly bad approximation of Jdot, since you are not recomputing the contact frames. You can do the nudging in the beginning of forward(), getting a better approximation (contacts will be recomputed), but then you run the risk of contacts appearing or disappearing, requiring some more logic.

UPDATE: thinking again about my suggestion above, as written this will definitely not work, since you need to at least call mj_forward() to update the world-frame cartesian positions. I am now thinking that best thing would be to run the full first half of the pipeline (i.e. mj_step1()) after nudging the positions, which will recompute everything including contact frames. This will mean that you might need to add logic to deal with changing indices if contacts appear or disappear, but this should be very rare if your epsilon is small enough, perhaps rare enough that you can ignore it.

[dhruvthanki]

Hi!
Thanks for the above explanation. I will look into the time derivative of the jacobian part.
The corresponding contact force can be assessed using mj_contactForce(m, d, i, result) right?

[yuvaltassa]

That is correct. Note that for elliptic cones this basically reads out the values from efc_force as the representation is the "natural" one. For pyramidal cones the normal and tangents need to be disambiguated.