如何计算接触雅可比矩阵时间导数Jc_dot？Drake实现疑问

现有代码的核心问题 #

1. 状态污染：循环中直接修改输入context的速度值，却未保存和恢复原始状态，导致后续迭代计算依赖被篡改后的速度，引入累计误差。
2. 物理意义混淆：CalcBiasSpatialAcceleration返回的是包含重力、科氏/离心项的偏置空间加速度，对应公式$^W\dot{V}_B = J\dot{v} + \dot{J}v + \text{bias terms}$中的$\dot{J}v + \text{bias terms}$部分，并非纯$\dot{J}v$。直接用它提取$\dot{J}$会混入额外非雅可比导数项，导致$\dot{J}_c$与$J_c$秩不一致。
3. 雅可比定义模糊：若你需要的是接触位置约束的雅可比导数（如点接触约束$p(q)=0$的$\dot{J}_c = \frac{d}{dt}\frac{\partial p}{\partial q}$），当前代码计算的是空间速度雅可比的导数，二者物理意义完全不同，会导致结果偏差。

最优计算方法：利用Drake自动微分（AutoDiff） #

Drake的AutoDiff功能可以直接计算雅可比的解析时间导数，是最准确且高效的方案，无需数值扰动或迭代。以下分两种场景给出实现：

场景1：计算空间速度雅可比的时间导数$\dot{J}_v$ #

空间速度雅可比$J_v(q)$满足$^W V_B = J_v(q) \cdot v$，其时间导数$\dot{J}_v = \frac{\partial J_v}{\partial q} \cdot v$（因$J_v$仅依赖位置$q$，$\dot{q}=v$）。

import numpy as np
from pydrake.autodiffutils import AutoDiffXd, InitializeAutoDiff, ExtractGradient

def compute_Jv_dot(plant, context, points, p_BoBP_B=np.zeros(3)):
    # 创建AutoDiff版本的上下文，复制原始状态
    autodiff_context = plant.CreateDefaultContext()
    autodiff_context.SetTimeStateAndParametersFrom(context)
    
    num_pos = plant.num_positions()
    num_vel = plant.num_velocities()
    q = context.GetPositions()
    v = context.GetVelocities()

    # 将位置变量包装为AutoDiff，其对时间的导数为v（dq/dt = v）
    q_autodiff = InitializeAutoDiff(q, np.hstack([v, np.zeros(num_vel)]))
    autodiff_context.SetPositions(q_autodiff)

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

    for idx, point_name in enumerate(points):
        frame_B = plant.GetFrameByName(point_name)
        # 计算AutoDiff类型的空间速度雅可比Jv
        Jv_autodiff = plant.CalcJacobianSpatialVelocity(
            autodiff_context,
            JacobianWrtVariable.kQDot,
            frame_B,
            p_BoBP_B,
            frame_world,
            frame_world
        )
        # 提取Jv的梯度，计算dJv/dt = dJv/dq · v
        Jv_gradient = ExtractGradient(Jv_autodiff)
        Jv_dot_block = Jv_gradient[:, :, :num_pos] @ v
        Jv_dot[6*idx : 6*(idx+1), :] = Jv_dot_block

    return Jv_dot

场景2：计算接触位置约束的雅可比导数$\dot{J}_c$ #

若接触约束为位置约束$p(q)=0$，其雅可比$J_c = \frac{\partial p}{\partial q}$，时间导数$\dot{J}_c = \frac{\partial^2 p}{\partial q^2} \cdot v$，实现如下：

def compute_contact_Jc_dot(plant, context, points, p_BoBP_B=np.zeros(3)):
    autodiff_context = plant.CreateDefaultContext()
    autodiff_context.SetTimeStateAndParametersFrom(context)
    
    num_pos = plant.num_positions()
    num_vel = plant.num_velocities()
    q = context.GetPositions()
    v = context.GetVelocities()

    q_autodiff = InitializeAutoDiff(q, np.hstack([v, np.zeros(num_vel)]))
    autodiff_context.SetPositions(q_autodiff)

    frame_world = plant.world_frame()
    Jc_dot = np.zeros((3 * len(points), num_vel))  # 点接触位置约束为3维

    for idx, point_name in enumerate(points):
        frame_B = plant.GetFrameByName(point_name)
        # 计算AutoDiff类型的接触点世界位置p_WP
        p_WP_autodiff = plant.CalcPointsPositions(
            autodiff_context,
            frame_B,
            p_BoBP_B,
            frame_world
        )
        # 提取一阶雅可比Jc = ∂p_WP/∂q
        Jc_autodiff = ExtractGradient(p_WP_autodiff)
        # 计算二阶导数并乘以v得到dJc/dt
        Jc_gradient = ExtractGradient(Jc_autodiff)
        Jc_dot_block = Jc_gradient[:, :, :num_pos] @ v
        Jc_dot[3*idx : 3*(idx+1), :] = Jc_dot_block

    return Jc_dot

若坚持使用偏置加速度方法（不推荐） #

需先消除重力、科氏项的影响，并恢复原始上下文状态：

1. 保存原始速度，每次循环后恢复。
2. 临时关闭重力计算偏置加速度，再恢复重力。
3. 偏置加速度减去重力项后，除以速度值（因单速度分量设为1）得到$\dot{J}$的对应列。但此方法数值精度差，仅作 fallback 方案。

内容的提问来源于stack exchange，提问作者Noah Wechtler