How to initialize a multibody force object for CalcForceElementsContribution()?

I want to calculate the manipulator equation for my plant context. I have a linearspringdamper, so I tried to use CalcForceElementsContribution() to get tau_app.

CalcForceElementsContribution() requires a MultibodyForces force object as an input, but it seems MultibodyForces can only be applied to a MultibodyTreeSystem? I have been using MultibodyPlant, so I am not sure where to go from here. Here is the code so far:

plant.SetPositions(plant_context, q_pos)
plant.SetVelocities(plant_context, q_vel)

M = plant.CalcMassMatrixViaInverseDynamics(plant_context)
Cv = plant.CalcBiasTerm(plant_context)
tauG = plant.CalcGravityGeneralizedForces(plant_context)

leg_frame = plant.GetBodyByName("stance_leg").body_frame()
ground_frame = plant.GetBodyByName("ground").body_frame()

# Jacobian of the stance foot
J = plant.CalcJacobianTranslationalVelocity(
    plant_context,
    JacobianWrtVariable(0),
    leg_frame,
    foot_in_leg["stance_leg"],
    ground_frame,
    ground_frame,
)

# discard y components since we are in 2D
J = J[[0, 2]]

tau_app = ???   #multibody forces
plant.CalcForceElementsContribution(plant_context, tau_app)

m_eq = M.dot(qdd) + Cv - tauG - J.T.dot(f) - tau_app

This problem is related to #13476, but I am not sure if the issue was resolved.

Solution

I think this is what you're looking for?

def ManipulatorDynamics(plant, q, v=None, context=None):
    if context is None:
        context = plant.CreateDefaultContext()
    plant.SetPositions(context, q)
    if v is not None:
        plant.SetVelocities(context, v)
    M = plant.CalcMassMatrixViaInverseDynamics(context)
    Cv = plant.CalcBiasTerm(context)
    tauG = plant.CalcGravityGeneralizedForces(context)
    B = plant.MakeActuationMatrix()
    forces = MultibodyForces_(plant)
    plant.CalcForceElementsContribution(context, forces)
    # TODO(russt): add in contact forces to tauExt.
    tauExt = plant.CalcGeneralizedForces(context, forces)

    return (M, Cv, tauG, B, tauExt)