Computing Jacobian of the Center on Mass of the Robot

[dhruvthanki]

Hi,

I'm trying to calculate the jacobian of the COM of the robot. Is this the right way to do it?

def jacpCOM(model, data):
    total_mass = mujoco.mj_getTotalmass(model)
    jacp_i = np.zeros((3,model.nv))
    jacp_COM = np.zeros((3,model.nv))
    for i in range(model.nbody):
        mi = model.body_mass[i]/total_mass
        mujoco.mj_jacBodyCom(model, data, jacp_i, None, i)
        jacp_COM += mi*jacp_i
    return jacp_COM

[yuvaltassa]

The short answer is yes 🙂

The slightly longer answer is: You are looping over all bodies, this might not be what you want. Specifically you are including static bodies with no DoFs, which you probably don't want. Better to loop over a subtree passed as input parameter (bodies with model.body_rootid==n).

Also note the the subtree mass is given in model.subtree_mass, so it is always true that mj_getTotalmass(model) == model.subtree_mass[0]

[yuvaltassa]

This function is now implemented natively.