drake - 向线性化(或 LQR)MultiBodyPlant 添加广义力的最佳方法？

Question

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

思路一:线性化后改变LQR中的B矩阵:使用LinearQuadraticRegulator(system, context, Q, R, plant_actuation_input_pot())后手动编辑B矩阵

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

Answer 1

实际上这两个想法是我自己会做的。对于您的情况，似乎想法 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，并在世界坐标系中表示。