无plant模型时如何构造angular_momentum_constraint实现跳跃优化？

问题1解答 #

littledog教程的优化问题中，决策变量包含关节位置q与关节速度v，约束的自动微分需要传递约束对所有决策变量的梯度信息才能让求解器高效收敛。足端位置p_WF是关节位置q的函数，而足端平动雅可比Jq_WF的物理意义就是足端位置对q的偏导（和足端线速度对q_dot的偏导等价），所以教程中会把Jq_WF作为p_WF的梯度矩阵传入AutoDiff结构，保证约束梯度计算正确。

问题2解答 #

你的优化方案已经移除了q、v变量，且明确p_WF是固定接触点常量，它不随任何决策变量（质心com、角动量变化率Hdot、接触力f）变化，因此p_WF对所有决策变量的偏导全为0。
你当前代码迭代不收敛的核心原因就是直接照搬了教程的AutoDiff逻辑：你传入的Jq_WF是p_WF对不存在的决策变量q的梯度，求解器拿到完全错误的梯度信息后无法正常迭代到可行解。
正确修改方式如下：

1. 直接移除plant.CalcJacobianTranslationalVelocity相关逻辑，在AutoDiff分支中，直接构造梯度全为0的ad_p_WF即可，示例代码如下：

def angular_momentum_constraint(vars, context_index):
    com, Hdot, contact_force = np.split(vars, [3, 6])
    contact_force = contact_force.reshape(3, 4, order='F')
    # 提前把4个固定接触点的坐标存为常量列表，不需要每次调用plant计算
    fixed_p_WF_list = [p_WF_1, p_WF_2, p_WF_3, p_WF_4] # 替换为你实际的固定接触点坐标
    torque = np.zeros(3)
    for i in range(4):
        p_WF = fixed_p_WF_list[i]
        if isinstance(vars[0], AutoDiffXd):
            # p_WF是常量，梯度全为0，梯度矩阵维度为3x决策变量总数
            ad_p_WF = initializeAutoDiffGivenGradientMatrix(p_WF, np.zeros((3, len(vars))))
            torque += np.cross(ad_p_WF.reshape(3) - com, contact_force[:,i])
        else:
            torque += np.cross(p_WF.reshape(3) - com, contact_force[:,i])
    return Hdot - torque

2. 更推荐的方案是直接手写该约束的计算逻辑，完全不需要调用Drake的plant相关API，因为你的约束所有项都只和你定义的决策变量相关，没有依赖机器人运动学的动态变量，计算效率和梯度准确性都会更高。

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