如何在没有模型工厂的情况下构建angular_momentum_constraint？

Question

跟着狗狗教程，我想优化一个跳跃运动，以质心com，comdot，comddot，空间动量H，Hdot，接触力f作为决策变量(去掉位置和速度q和v)。

我想添加一个约束，即Hdot = sum_i cross(p_WF_i-com, f_i)，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')
    if isinstance(vars[0], AutoDiffXd):
        torque = np.zeros(3)
        for i in range(4):
            p_WF = plant.CalcPointsPositions(plant_context, foot_frame[i], [0,0,0], plant.world_frame())
            Jq_WF = plant.CalcJacobianTranslationalVelocity(
                plant_context, JacobianWrtVariable.kQDot,
                foot_frame[i], [0, 0, 0], plant.world_frame(), plant.world_frame())
            ad_p_WF = initializeAutoDiffGivenGradientMatrix(p_WF, np.hstack((Jq_WF, np.zeros((3, 18)))))
            torque = torque     + np.cross(ad_p_WF.reshape(3) - com, contact_force[:,i])
    else:
        torque = np.zeros(3)
        for i in range(4):
            p_WF = plant.CalcPointsPositions(plant_context, foot_frame[i], [0,0,0], plant.world_frame())
            torque += np.cross(p_WF.reshape(3) - com, contact_force[:,i])
    return Hdot - torque

但是，程序始终会达到迭代限制(1e6)，添加此约束。我的问题是：

1. 在Litledog教程中，为什么ad_p_WF在计算接触扳手时需要一个Jacobian w.r.t QDot作为GradientMatrix？
2. 如何在不使用GradientMatrix的情况下填充plant.CalcJacobianTranslationalVelocity，因为在数学家程序中没有q和v？

这是源代码testjump.py，谢谢您的建议！

Answer 1

因为q和v不再是您的决策变量，所以不需要计算p_WF的Jacobian。我会重写代码块

Jq_WF = plant.CalcJacobianTranslationalVelocity(
                plant_context, JacobianWrtVariable.kQDot,
                foot_frame[i], [0, 0, 0], plant.world_frame(), plant.world_frame())
ad_p_WF = initializeAutoDiffGivenGradientMatrix(p_WF, np.hstack((Jq_WF, np.zeros((3, 18)))))
torque = torque     + np.cross(ad_p_WF.reshape(3) - com, contact_force[:,i])

作为

torque = torque + np.cross(p_WF.reshape(3) - com, contact_force[:, i])

其中p_WF是世界帧中的不动点。(所以不要在函数p_WF = plant.CalcPointsPositions(plant_context, foot_frame[i], [0,0,0], plant.world_frame())中调用angular_momentum_constraint，因为p_WF的值并不取决于该函数的输入。