python-3.x - 如何获得将外力映射到广义力的矩阵？

Question

给定一个多体植物，我需要找到将外力 lambda 转换为广义力的矩阵。即以下等式中的 Phi（取自 Scott Kuindersma、Frank Permenter 和 Russ Tedrake 的 An Efficiently Solved Quadratic Program for Stabilizing Dynamic Locomotion）。

我的猜测是，我需要先将相应的力添加到多体植物中的相关物体上

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

但是，我不太确定如何继续获取矩阵 Phi。我怀疑我需要使用CalcForceElementsContribution()但不知道如何利用结果MultibodyForce来获取矩阵 Phi。

标签： python-3.xdrake

Answer 1

如果 λ 是附着在物体 B 上的点 Q 处的接触力，用世界坐标系表示，那么矩阵 Φ 就是雅可比矩阵。您可以使用CalcJacobianTranslationalVelocity计算雅可比矩阵

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

其中p_BQ是点 Q 的位置，测量并表示在脚框架中。

如果 λ 是摩擦锥在点的边缘的权重Q（即接触力为 f = ∑ᵢ λᵢeᵢ 其中 eᵢ 是摩擦锥在 处的第 i 个边缘，Q在世界坐标系中测量并表示），则矩阵 Φ是 Φᵀ = Jᵀ*E 其中 E = [e₁, ..., eₘ] （即假设在点 处有 m 个线性化摩擦锥的边Q，我们将这些摩擦锥边水平连接得到矩阵E）。

在这里，我展示了如何计算单个点的 Φ。如果您有多个接触点，那么您可以分别计算每个点的 Φ，然后将它们连接在一起。