How to construct angular_momentum_constraint without a model plant?

Question

Follow the liitledog tutorial, I want to optimize a jump motion with the center of mass com, comdot, comddot, spatial momentum H, Hdot, contact force f, as the decision variables（remove the position and velocity q and v）.

I want to add a constrain that Hdot = sum_i cross(p_WF_i-com, f_i), p_WF is the fix contact point (four points). I tried follow the littledog with

---

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

---

But the prog will always reach the iterations limit (1e6) adding this constrain. My question is:

1. In the liitledog tutorial, why does the ad_p_WF need a Jacobian w.r.t QDot as GradientMatrix when computing the contact wrench?
2. How to fill the GradientMatrix without using the plant.CalcJacobianTranslationalVelocity as there is no q and v in the mathematicalprogram?

Here is the source code testjump.py, thanks for any advices!

Answer 1

Since q and v are not your decision variables any more, you don't need to compute the Jacobian of p_WF. I would rewrite the code block

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])

as

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

where p_WF is fixed point in the world frame. (So don't call p_WF = plant.CalcPointsPositions(plant_context, foot_frame[i], [0,0,0], plant.world_frame()) within that function angular_momentum_constraint, as the value of p_WF is doesn't depend on the input to that function.