在 Drake 中模拟 MultiBodyPlant 时可以绕过 SpatialInertia 吗？

如何解决在 Drake 中模拟 MultiBodyPlant 时可以绕过 SpatialInertia 吗？

我正在尝试使用 Drake 来模拟水下操纵器。在水下的一个重要因素是，除了机械臂方程中的刚体质量项之外，还有一个附加的质量项。从 CalcInverseDynamics() 函数文档中，运动树具有逆动力学方程：

tau = M(q)v' + C(q,v)v - tau_app - sum(应用空间力)

要包括增加的质量，这应该变成：

tau = (M(q)+AM(q))v' + C(q,v)v - tau_app - sum(应用空间力)

其中 AM(q) 是依赖于配置的附加质量。我之前使用 Newton-Euler 递归方法通过在每个链接的空间惯性矩阵中包含“附加质量”来解决逆动力学问题。但是，如上一个问题 (Can a SpatialInertia object be constructed from a 6x6 matrix?) 中所述，增加的质量会产生空间惯性，其中质量子矩阵具有三个不同质量而不是一个均匀质量（[m1 m2 m3] 的对角线而不是 [mmm]），因此不能由 Drake 中的 SpatialInertia 对象表示。

我现在的后续问题是，是否有可能在 Drake 的框架内在没有 SpatialInertia 对象的情况下对运动树执行反向和正向动力学，或者以某种方式使用 ArticulatedBodyInertias 代替？在上一篇文章中建议 ArticulatedBodyInertia 可以表示增加质量的空间惯性，但我不清楚它如何用于正向/反向动力学和模拟。

从阅读源代码（我对 C++ 的理解非常有限）来看，SpatialInertia 似乎是 MultiBody Plant 的正向和反向动态函数以及模拟的基本构建块。

我应该学习 C++，但现在我正在使用 Drake 的 Python 绑定，并且不喜欢大量修改 Drake C++ 代码。如果答案是“不，在不对源代码进行重大修改的情况下不可能绕过 SpatialInertia 对象”，这是可以理解的，但我想我会在认输之前询问那些更熟悉该程序的人。

预先感谢您对此事的任何想法。

解决方法

不幸的是，简短的回答是否定的，你不能那样做。但是，如果您告诉我们更多有关此问题的信息，我们可以为您提供帮助。据我所知，我们对 SpatialInertia 对象的最大假设是，在幕后它们会导致 SPD 6x6 矩阵。如果包括虚拟质量项在内的新惯性具有此属性，我不明白为什么我们不能为这种建模打开大门。从 Can a SpatialInertia object be constructed from a 6x6 matrix? 我看到你只需要在每个方向都有不同的质量值。我相信很容易证明，只要我们“添加”到原始惯性，那么它应该保持 SPD。

长话短说，你能告诉我们更多关于这个新的 6x6 对象的数学特性吗？

黑客想法

您可以将虚拟质量建模为植物上的外力。但是，由于此外力取决于加速度，因此您将不可避免地创建 Drake 无法解析的代数环。对此的快速而肮脏的解决方案是在虚拟质量系统块和工厂之间添加一个短暂的延迟。如果延迟明显小于您的问题的时间尺度，那么您会没事的，而且 Drake 积分器将轻而易举地工作。

实现此目的的一种方法是在 MBP 计算的加速度（您可以使用 trackByIdentity(menuItem) { return menuItem.title; // here,you can track by any field. } 访问它们）和计算水动力的系统块之间添加 FirstOrderLowPassFilter。由于 get_body_spatial_accelerations_output_port() 具有状态，如果您将空间加速度相关的水动力直接连接到 MBP 的输入中，则会打破直接馈通。您需要选择过滤器的时间常数 tau。您可以通过考虑哪些频率高于 1/tau 而不关心来选择时间常数（此处的小波德分析将引导您走向正确的方向）。

总而言之，这是我对图表的 ASCII 草图的错误尝试：

                      ┌───────────────────┐
                      │                   │
applied_spatial_force │                   │ body_spatial_accelerations
               ┌─────►│   MultibodyPlant  ├────────────────────────┐
               │      │                   │                        │
               │      │                   │                        │
               │      └───────────────────┘        ┌───────────────┴─────────────────┐
               │                                   │                                 │
               │                                   │ SpatialAccelerationToBasicVector│
               │                                   │                                 │
               │                                   └───────────────┬─────────────────┘
               │                                                   │
               │    ┌─────────────┐       ┌──────────────┐         │
               │    │             │       │              │         │
               └────┤ HydroForces │◄──────┤ LowPassFilter├─────────┘
                    │             │       │              │
                    └─────────────┘       └──────────────┘

您需要自己编写 SpatialAccelerationBasicVector 以将 MBP 输出的内容 (SpatialAccleration) 转换为 LowPassFilter 消耗的内容 (BasicVector)。