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

Question

给定一个多体工厂，我需要找到将外力 lambda 转换为广义力的矩阵。 IE。以下等式中的 Phi(摘自 Scott Kuindersma、Frank Permenter 和 Russ Tedrake 的 An Efficiently Solvable 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。

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

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