drake - 将广义力添加到线性化（或 LQR）MultiBodyPlant 的最佳方法？

Question

我正在尝试在从 MultiBodyPlant 线性化的自由浮动系统上添加广义力。对于固定底座，广义力基本上对应于关节扭矩，但对于自由浮动情况，它也包括底座扭矩/力。但是，这些不包括在 MultiBodyPlant 系统的驱动端口中，因此在为线性化（或 LQR）提供驱动端口的同时，基础扳手不包括在线性化后的驱动矩阵 B 中。对于我的工作，我想在线性化中也包括基本扭矩/力。为此，我可以在 drake 中想到 2 种方法，但我不确定在架构上哪个是更好/正确的决定。我计划稍后使用它来制作 TVLQR 控制器以稳定轨迹。

想法1：线性化后更改LQR中的B矩阵：使用LinearQuadraticRegulator（system，context，Q，R，plant_actuation_input_pot（））后手动编辑B矩阵

想法2：创建一个LeafSystem类（类似于this），输出端口连接到get_applied_generalized_force_input_port（）和输入端口暴露在图表之外（使用ExportOutput（）），然后将该系统作为一个整体线性化（包含连接到MultiBodyPlant的CustomLeafSystem）。我假设这样做会将基本驱动添加到线性化系统的 B 矩阵中。

标签： drake

Answer 1

实际上这两个想法是我自己会做的。对于您的情况，Idea 1 似乎更易于实现？矩阵 B 将驱动指数映射到广义速度指数。因此，您必须确保将必要的“1”放置在 B 的正确位置。您可以找到自由浮动体索引，使用Body::floating_velocities_start()它返回完整状态 x 内的起始索引（因此您需要减去 MultibodyPlant： :num_positions() 用于在 B) 中进行索引。对于浮体，广义速度依次是角速度 w_WB 和平移速度 v_WB。因此，广义力对应于在 B 处关于其原点 Bo 施加的扭矩 t_Bo_W 和 f_Bo_W，并在世界坐标系中表示。