drake - 接触雅可比的时间导数

Question

我正在实现一些来自 [1]和 [2] ，我发现需要计算 Jc_dot。例如，在 [1] 中，他们将线性化系统投影到接触约束的零空间(第二页右上角)。据我了解，[2] 中的方法类似，但扩展到时变情况。

从阅读另一个关于这个主题的 stackoverflow 问题，[Derivative of jacobian in drake] ，看来德雷克没有办法直接计算这个矩阵。

我想知道计算 Jc_dot 的最佳方法是什么？我目前获得 Jc_dot 的方法是通过迭代将速度元素之一设置为 1 并调用 drake 中的 CalcBiasSpatialAcceleration 函数。但是，当我检查 Jc 和 Jc_dot 的排名时，它们是不同的，我找不到为什么会这样的原因。因此，我怀疑我的方法可能存在一些问题。

引用实现如下:

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

我同意您无法使用您提出的方法获得预期的结果。 Jdot*v 项不仅取决于此处可见的 v 项中的速度。 Jdot 本身也依赖于速度，因为\dot{J} =\sum_i dJdqi(q)\dot{qi}.

一种应该运作良好且相对干净的方法是在 CalcJacobianSpatialVelocity 上使用 autodiff 来获取 dJdqi，然后自己计算总和。或者，如果您的 autodiff 功能足够强大，那么您甚至可以初始化 autodiff q 变量以在其导数中包含 dqdt；这样你就可以避免循环。

This should be a reasonable tutorial对于那个 autodiff 工作流程。