Analytical Time Derivative of Site Jacobian

[dhruvthanki]

I'm trying to write an optimization-based controller for a humanoid robot in MuJoCo with equality constraints of the QP being the dynamics and contact constraint defined at the site as similar to #53. Obtaining the site jacobian was straightforward, but the time derivative of the jacobian is a little bit complicated. Until now I have been using finite difference but now I want to calculate the time derivative of the jacobian analytically.

From Equation 28-33 in the document, it is easy to see that computing the product, dJdq using Equation 31 is more efficient than computing the time derivative of the jacobian alone. Computing the acceleration drift (dJdq) as shown in Eqn 31 requires spatial velocities, v_i of all the bodies. One way I can think of is to compute jacobian using mj_jac() with the point as 0 vector and the bodyID being the ID of the parent Body of the site and map the system velocities, qdot using the jacobian to obtain the body spatial velocities. I am not sure if this is the right way to do this. Any help is appreciated.

[yuvaltassa]

see #411