如何在没有模型植物的情况下建造angular_momentum_constraint？

Question

关注liitledog tutorial，我想优化一个跳跃运动，质心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) 添加此约束。我的问题是：

在 liitledog 教程中，为什么 ad_p_WF 在计算接触扳手时需要雅可比 w.r.t QDot 作为 GradientMatrix？
数学程序中没有q和v，如何在不使用 plant.CalcJacobianTranslationalVelocity的情况下填充GradientMatrix？

这里是源码testjump.py，谢谢指教！

Answer 1

由于 q 和 v 不再是您的决策变量，因此您无需计算 p_WF 的雅可比矩阵。我会重写代码块

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 是世界坐标系中的不动点。（所以不要在该函数 angular_momentum_constraint 中调用 p_WF = plant.CalcPointsPositions(plant_context, foot_frame[i], [0,0,0], plant.world_frame())，因为 p_WF 的值不依赖于该函数的输入。

如何在没有模型植物的情况下建造angular_momentum_constraint？

How to construct angular_momentum_constraint without a model plant?

drake