给定一个多体植物，我需要找到将外力 lambda 转换为广义力的矩阵。 IE。以下方程中的 Phi（取自 Scott Kuindersma、Frank Permenter 和 Russ Tedrake 的《稳定动态运动的有效可解二次规划》）。
我的猜测是，我需要首先将相应的力添加到多体工厂中的相关物体上
left_foot_contact_force = plant.AddForceElement(plant.GetModelInstanceByName("l_foot"))
right_foot_contact_force = plant.AddForceElement(plant.GetModelInstanceByName("r_foot"))
但是，我不太确定如何继续获取矩阵 Phi。我怀疑我需要使用CalcForceElementsContribution() https://drake.mit.edu/doxygen_cxx/classdrake_1_1multibody_1_1_multibody_plant.html#ae1b54c8562f107e4d6e11b4b0f8d53c5但不知道如何利用结果MultibodyForce得到矩阵 Phi。
如果 λ 是施加在连接到物体 B 的 Q 点上的接触力（以世界坐标系表示），则该矩阵 Φ 就是雅可比矩阵。您可以使用计算雅可比矩阵计算雅可比平移速度 https://drake.mit.edu/doxygen_cxx/classdrake_1_1multibody_1_1_multibody_plant.html#ae2516569f5a2cc27e76ee0cc8b728746 as
J = plant.CalcJacobianTranslationalVelocity(context, JacobianWrtVariable.kV, plant.GetBodyByName("l_foot"), p_BQ, plant.world_frame(), plant.world_frame())
where p_BQ是在脚坐标系中测量和表达的点 Q 的位置。
设 λ 为摩擦锥在该点的边重Q（即接触力为 f = Σᵢ λᵢeᵢ，其中 eᵢ 是摩擦锥的第 i 个边Q，在世界坐标系中测量和表达），则矩阵 Φ 为 Φᵀ = Jᵀ*E，其中 E = [e₁, ..., eₘ] （即假设点处线性化摩擦锥有 m 个边）Q，我们水平连接这些摩擦锥边缘以获得矩阵E).
在这里，我展示了如何计算单个点的 Φ。如果有多个接触点，则可以分别计算每个点的 Φ，然后将它们连接在一起。
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
