Time derivative of contact jacobian

Question

I was implementing some ideas from [1] and [2], and I came across the need to calculate Jc_dot. For example in [1] they project the linearized system into the nullspace of the contact constraints (top right of the second page). From what I understand, the approach in [2] is similar, but extended to the time-varying case.

From reading another stackoverflow question on this topic, [Derivative of jacobian in drake], it appears that drake does not have a way to directly calculate this matrix.

I was wondering what the best approach is to calculate Jc_dot? The way I currently obtain Jc_dot is by iteratively setting one of the velocity elements to 1 and calling the CalcBiasSpatialAcceleration function in drake. However, when I check the rank of Jc and Jc_dot, they are different and I cannot find a reason why that should be the case. So, I suspect there might be some issue with my method.

Here is the implementation for reference:

def makeJc_dot(plant, context, points, p_BoBP_B = np.zeros(3)):

    frame_world = plant.world_frame()
    v = np.zeros(num_vel)
    Jc_dot = np.zeros((6*len(points), num_vel))

    for index, point in enumerate(points):
        
        frame_B = plant.GetFrameByName(point)

        for col in range(num_vel):
            v[col] = 1.
            plant.SetVelocities(context, v)
            
            Sp_acc = plant.CalcBiasSpatialAcceleration(context,
                            JacobianWrtVariable.kV,
                            frame_B,
                            p_BoBP_B,
                            frame_world,
                            frame_world)

            Jc_dot[0 + 6*index:3 + 6*index, col] = Sp_acc.rotational()
            Jc_dot[3 + 6*index:6 + 6*index, col] = Sp_acc.translational()

            v[col] = 0

    return Jc_dot

Answer 1

I agree that you cannot get the desired result with the method you've proposed. The term Jdot*v does not only depend on velocities in the v term that is visible here. Jdot itself also depends on velocities, because \dot{J} = \sum_i dJdqi(q) \dot{qi}.

One approach that should work well and be relatively clean is to use autodiff on CalcJacobianSpatialVelocity to get dJdqi, then compute that sum yourself. Or if your autodiff powers are strong enough, then you could even initialize the autodiff q variables to contain dqdt in their derivatives; so you can avoid the loop.

This should be a reasonable tutorial for that autodiff workflow.