Analytical Time Derivative of Site Jacobian

[dhruvthanki]

Discussed in https://github.com/deepmind/mujoco/discussions/410

^{Originally posted by **dhruvthanki** July 31, 2022} 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](https://ieeexplore.ieee.org/document/5663690), 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]

What is wrong with finite differences? It is very efficient and much faster than anything you could do analytically, even in principle. You shouldn't use the value from the previous timestep of course, but rather a very tiny $dt$ value. This will be extremely fast, just one more call to the Jacobian function which is already very fast.

If you were to try to do this analytically you would need to do something like the following. Let's call the global position of your site as a function of joint angles $f(q)$ . You want

$$ \dot J = \frac{\partial}{\partial t} J = \frac{\partial}{\partial t} \frac{\partial f}{\partial q} = \frac{\partial}{\partial q} \frac{\partial f}{\partial t} = \frac{\partial}{\partial q} \frac{\partial f}{\partial q} \cdot \dot{q} = \frac{\partial^2}{\partial q^2}f \cdot \dot{q} $$

So the analytical solution involves computing a 3 x nv x nv tensor (the Hessian of the vector function $f$) and reducing it with the velocities $\dot q$. There is no way this will be faster and only negligibly more accurate.

Let us know if you need help getting precise $\dot J$ with finite differences, in case you are having some issue with that.

[yuvaltassa]

Browsing the tutorial you linked to, it seems like perhaps what you actually want is the object acceleration? This can be computed with mj_ObjectAcceleration.

[dhruvthanki]

Hello @yuvaltassa, I am not sure if it's the mj_ObjectAcceleration() function. Isn't a body-centered frame the same as a body COM frame? I will confirm if this is what I was looking for.

Could you also explain how to obtain a better estimate of dJac by using the value from $\delta t$ rather than from the last time-step?

Thank you for the time!

[Balint-H]

You shouldn't use the value from the previous timestep of course, but rather a very tiny dt value.

@yuvaltassa Could you please elaborate a bit what you meant by this? Is this referring to using the value from a couple timesteps ago to "denoise" the finite differences (i.e. dt is a multiple of the global time step)?

[yuvaltassa]

Ah thanks @Balint-H for reminding me to follow up.

Problem statement: we want to compute the time derivative of some Jacobian that we can compute with mj_jac and friends.

Proceed as follows.

1. Given some qpos we need to first update the internal datastructures. I'm assuming all this happens within a rollout, so qpos has just been updated by mj_step but derived quantities have not. mj_forward will do this for us but it's enough to call mj_kinematics and mj_comPos.
2. Call the relevant jac function and save the Jacobian in J.
3. Choose a very small positive number h. Anything in the range $10^{-12}-10^{-6}$ should give identical results.
4. Call mj_integratePos(m, d->qpos, d->qvel, h). This will integrate qpos in-place with the timestep h.
5. Do step 1 again to update mjData kinematic quantities.
6. Get the new Jacobian as in step 2, call it Jh.
7. The quantity we want is Jdot = (Jh-J)/h.
8. Reset d->qpos to the original value, continue with the simulation. Kinematic quantities will be overwritten, no need to call kinematics and comPos again.

Let me know if that is the answer you were looking for.

[Balint-H]

This makes a lot of sense, I was not considering this option of "forecasting" the kinematics. Thanks!

[dhruvthanki]

Hi @yuvaltassa, @Balint-H

Thank you for your time. I appreciate all the help! The FDs are better using the steps above.

One last question! As in my case, it's not a problem to get the whole product $\dot{J}\dot{q}$ instead of getting $\dot{J}$ alone. I could so as follows:

$$Acc{site} = J{site}(q)\ddot{q} + \dot{J}_{site}(q,\dot{q})\dot{q}$$

$$\dot{J}{site}(q,\dot{q})\dot{q} = Acc{site} - J_{site}(q)\ddot{q}$$

Setting the current $[q$ $\dot{q}]$ and after doing mj_forward(), we could obtain the global site acceleration using mj_ObjectAcceleration() and we also have the site jacobian and the current $\ddot{q}$.

It seems like this should work, but I am not getting the correct results! Snippet below:

import mujoco
import numpy as np

model = mujoco.MjModel.from_xml_path(xml_path)
data = mujoco.MjData(model)
mujoco.mj_forward(model,data)

site_ID = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "site_name")

site_Jac = np.zeros((3,model.nv))
mujoco.mj_jacSite(model, data, site_Jac, None, site_ID)

site_acc = np.zeros(6)
mujoco.mj_objectAcceleration(model, data, mujoco.mjtObj.mjOBJ_SITE, site_ID, site_acc, 0)   # Not sure why this only has linear acceleration and not angular?

site_dJac_dq = site_acc[3:6] - site_Jac@data.qacc    # Bottom 3 rows of spatial acceleration for linear
print(site_dJac_dq)

Any insight on why this would not work?

[yuvaltassa]

Re # Not sure why this only has linear acceleration and not angular? see here, for whatever reason rot acc is only available in the global frame.

Regarding the actual question, I'm not sure, but I have a debugging proposal. You can either set velocities to 0 (before calling forward) or set accelerations (d->cacc) to 0 (after calling forward).

Let me know what you discover!

[dhruvthanki]

Hi @yuvaltassa

So I tried it but I am not sure if what I am coding is right. The acceleration remains zero throughout the simulation. The blue curve in the image below is for acceleration and the red curve is for velocity, both along the z-axis. The velocity curve is what is expected but the acceleration curve should be constant at -9.81, not 0.

import mujoco
import numpy as np

model = mujoco.MjModel.from_xml_path("ball.xml")
data = mujoco.MjData(model)
data.qvel = np.zeros(model.nv)
mujoco.mj_forward(model,data)

site_ID = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ball")
site_acc = np.zeros(6)
mujoco.mj_objectAcceleration(model, data, mujoco.mjtObj.mjOBJ_SITE, site_ID, site_acc, 0) 
# Output: site_acc = [0 0 0 0 0 0]
# Expected Output = [0 0 0 0 0 -9.81] # order: [rot, lin]

site_acc = np.zeros(6)
site_vel = np.zeros(6)
while(True):
    mujoco.mj_objectAcceleration(model, data, mujoco.mjtObj.mjOBJ_SITE, site_ID, site_acc, 0)
    mujoco.mj_objectVelocity(model, data, mujoco.mjtObj.mjOBJ_SITE, site_ID, site_vel, 0)
    plt.scatter(data.time, site_vel[5], color="red")
    plt.scatter(data.time, site_acc[5], color="blue")
    mujoco.mj_step(model,data)
    if data.time > 0.6:
        break

plt.show()

<mujoco model="ball">
  <worldbody>
    <light pos='0 0 3' dir='0 0 -1' diffuse='0.2 0.2 0.2'/>
    <geom type="plane" size="1 1 .01" rgba="1 1 1 1"/>
    <body name="mug" pos='0 0 1'>
      <freejoint/>
      <geom name="base" type="sphere" rgba='1 0 0 1' size=".05" mass="1" conaffinity='1' condim='3' contype='1'/>
      <site name="ball" size='0.01'/>
    </body>
  </worldbody>
</mujoco>

[quagla]

Could you please plot site_acc[2]?

[dhruvthanki]

Hello @quagla

Acceleration and velocity both are zero throughout for site_acc[2]. We can also see that the velocity profile is correct in the previous graph so it must be the velocity along z-axis. Hence linear acceleration along z-axis is at index 5 and not 2.

[quagla]

So the issue is that cacc is not computed by default. It is computed mj_rnePostConstraint so you need to either define a sensor or call that function explicitly:

mujoco.mj_rnePostConstraint(model, data)
data.cacc[1] = data.cacc[0] 
mujoco.mj_objectAcceleration(model, data, mujoco.mjtObj.mjOBJ_SITE, site_ID, site_acc, 0)

[dhruvthanki]

I see. Thank you! I know this is really dragging on. I really appreciate your time @quagla and @yuvaltassa

Just 2 more stupid questions!

• Why data.cacc[1] = data.cacc[0]? How would it work if the site was at the feet of a humanoid robot? Do I have to add the acceleration data.cacc[0] to all the bodies?
• As the +Z-axis is pointing up, I should expect -9.81 acceleration along +z but I am getting +9.81 along +z.

data.qvel = np.zeros(model.nv)
mujoco.mj_forward(model,data)
mujoco.mj_rnePostConstraint(model, data)
data.cacc[1] = data.cacc[0]  # Why?
mujoco.mj_objectAcceleration(model, data, mujoco.mjtObj.mjOBJ_SITE, site_ID, site_acc, 0)
print(site_acc)  # Output: [0.   0.   0.   0.   0.   9.81]

[quagla]

data.cacc[0] is the acceleration of the world body, gravity is implemented there in MuJoCo rather than in the body. So you will see acceleration of zero in body 1 and positive gravity in the world (i.e. body 0). If you use a sensor in body 1, you should get the correct value and also mj_rnePostConstraint will be called by the sensor update function.

[Krishnendu1984]

How to access the acceleration data of a body in mujoco