轨迹优化视角下Drake中kQdot与kv的雅可比矩阵差异及kQdot应用示例

一、kQdot与kv的核心差异 #

CalcJacobianSpatialVelocity中的两个选项，本质对应不同广义速度定义下的雅可比矩阵：

• kv选项：对应Drake系统的标准广义速度v（即MultibodyPlant的generalized_velocity）。对于浮动基刚体，v包含刚体的线速度和角速度分量。此时计算的雅可比Jv满足：V = Jv * v（V为刚体的空间速度）。你当前将接触力映射为广义力的场景，正是用Jv的转置与接触力相乘，得到对应v的广义力，这是最常用的场景。
• kQdot选项：对应广义坐标的时间导数q̇（即Qdot）。此时的雅可比Jqdot满足：V = Jqdot * q̇。关键区别在于，q̇和v并非总是等价——比如浮动基刚体的广义坐标q可能包含位置+四元数，q̇是位置导数+四元数导数，而v是线速度+角速度，两者通过变换矩阵N(q)关联：v = N(q) * q̇，因此Jqdot = Jv * N(q)。

二、kQdot的适用场景与示例 #

kQdot主要用于直接基于广义坐标导数q̇建模的轨迹优化问题，这类场景不需要显式区分v和q̇，或需要直接对q̇做约束/优化：

• 全坐标轨迹优化：当轨迹优化以q和q̇作为状态变量（而非q和v）时，若要约束末端执行器速度，可通过Jqdot建立V_desired = Jqdot * q̇的约束关系，避免额外的N(q)变换。比如机械臂关节轨迹优化中，直接用关节角度导数q̇作为优化变量，约束末端速度就会用到它。
• 外部模型接口适配：若你的轨迹优化框架原生基于q和q̇设计，而非Drake默认的q和v，使用Jqdot可直接将Drake的刚体速度计算转换为框架兼容的形式，省去中间变换步骤。
• 特殊接触动力学建模：部分接触模型需要直接用q̇描述接触点相对速度，此时Jqdot可直接给出接触点速度与q̇的线性关系，简化动力学方程推导。

以下是简化的代码示例：

# 初始化plant与context后
q = plant.GetPositions(context)
q_dot = np.array([0.1, 0.2, 0.0, 0.0, 0.0, 0.0])  # 示例广义坐标导数

# 计算kQdot对应的雅可比
Jqdot = plant.CalcJacobianSpatialVelocity(
    context=context,
    with_respect_to=JacobianWrtVariable.kQdot,
    body=contact_body,
    p_BoBp_B=contact_point_body_frame,
    frame_B=contact_body.body_frame(),
    frame_A=plant.world_frame()
)

# 计算接触点空间速度
contact_velocity = Jqdot @ q_dot

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