3

Given a multibody plant, I need to find the the matrix that transforms the external forces lambda into generalized forces. I.e. Phi in the following equation (taken from An Efficiently Solvable Quadratic Program for Stabilizing Dynamic Locomotion by Scott Kuindersma, Frank Permenter, and Russ Tedrake).

enter image description here

My guess is that I need to first add the corresponding forces to the relevant bodies in the multibody plant with

left_foot_contact_force = plant.AddForceElement(plant.GetModelInstanceByName("l_foot"))
right_foot_contact_force = plant.AddForceElement(plant.GetModelInstanceByName("r_foot"))

However, I'm not quite sure how to proceed to get the matrix Phi. I suspected that I would need to use CalcForceElementsContribution() but don't know how to make use of the resultant MultibodyForce to get the matrix Phi.

Rufus
  • 5,111
  • 4
  • 28
  • 45

1 Answers1

4

If λ is the contact force applied at point Q attached to a body B, expressed in the world frame, then that matrix Φ is the Jacobian matrix. You can compute the Jacobian matrix using CalcJacobianTranslationalVelocity as

J = plant.CalcJacobianTranslationalVelocity(context, JacobianWrtVariable.kV, plant.GetBodyByName("l_foot"), p_BQ, plant.world_frame(), plant.world_frame())

where p_BQ is the position of the point Q measured and expressed in the foot frame.

If λ is the weight of edge of friction cone at point Q (namely the contact force is f = ∑ᵢ λᵢeᵢ where eᵢ is the i'th edge of the friction cone at Q, measured and expressed in the world frame), then the matrix Φ is Φᵀ = Jᵀ*E where E = [e₁, ..., eₘ] (Namely assuming there are m edges of the linearized friction cone at point Q, and we horizontally concatenate these friction cone edges to get the matrix E).

Here I showed how to compute Φ for a single point. If you have multiple contact points, then you can compute Φ for each of the point separately, and then concatenate them together.

Hongkai Dai
  • 2,546
  • 11
  • 12