在MultibodyPlant中如何为CalcForceElementsContribution()初始化MultibodyForces对象？

问题描述 #

我需要在MultibodyPlant上下文里计算机械臂动力学方程，因为使用了LinearSpringDamper，想要调用CalcForceElementsContribution()获取对应的广义力tau_app。但这个函数需要MultibodyForces对象作为输入，我之前误以为它只适用于MultibodyTreeSystem，不清楚在MultibodyPlant里该怎么操作。现有代码如下：

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

该问题与github issue#13476相关，但不确定问题是否已解决。

解决方案 #

核心说明 #

MultibodyPlant是MultibodyTreeSystem的子类，因此CalcForceElementsContribution()完全兼容MultibodyPlant，无需特殊适配。你只需要正确创建并使用MultibodyForces对象即可。

修正后的代码 #

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

# 1. 基于当前plant创建MultibodyForces对象
tau_app_forces = MultibodyForces(plant)
# 2. 调用函数填充力数据
plant.CalcForceElementsContribution(plant_context, tau_app_forces)
# 3. 将MultibodyForces转换为可直接使用的广义力向量
tau_app = tau_app_forces.CalcGeneralizedForces()

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

关于issue#13476 #

该问题已被标记为解决，后续版本中修复了相关API的兼容性问题，现在MultibodyPlant下使用MultibodyForces和CalcForceElementsContribution()没有障碍。

内容的提问来源于stack exchange，提问作者Jasmine Terrones