向线性化（或 LQR）MultiBodyPlant 添加广义力的最佳方法？
Best method to add generalized forces to Linearized (or LQR) MultiBodyPlant?
我正在尝试在已从 MultiBodyPlant 线性化的自由浮动系统上添加广义力。对于固定底座，广义力基本上对应于关节力矩，但对于自由浮动的情况，它也包括底座 torques/forces。然而，这些不包括在 MultiBodyPlant 系统的驱动端口中，因此在为线性化（或 LQR）提供驱动端口时，基本扳手不包括在驱动矩阵 B post-线性化中。对于我的工作，我想在线性化中也包括基数 torques/forces。为此，我可以想到 drake 中的两种方法，但我不确定哪种方法在架构上是 better/correct 决定。我计划稍后使用它来制作 TVLQR 控制器来稳定轨迹。
思路1: Change B matrix in LQR after linearization: 使用LinearQuadraticRegulator(system, context, Q, R, plant_actuation_input_pot()) 后手动编辑B矩阵
想法 2：创建一个 LeafeSystem class（类似于 this），输出端口连接到 get_applied_generalized_force_input_port() 和输入端口暴露外部图表（使用 ExportOutput()），然后将该系统作为一个整体线性化（包含连接到 MultiBodyPlant 的 CustomLeafSystem）。我假设这样做会将基本驱动添加到线性化系统的 B 矩阵中。
实际上这两个想法是我自己会做的。对于您的情况，似乎想法 1 更易于实施？
矩阵 B 将驱动指数映射到广义速度指数。因此，您必须确保将必要的“1”放在 B 的正确位置。您可以使用 Body::floating_velocities_start() 找到自由浮体索引，其中 returns 是完整状态 x (因此您需要减去 MultibodyPlant::num_positions() 以便在 B) 中建立索引。对于浮体，广义速度依次为 angular 速度 w_WB 和平移速度 v_WB。因此，广义力对应于在 B 处围绕其原点 Bo 施加的力矩 t_Bo_W 和 f_Bo_W，并在世界坐标系中表示。
我正在尝试在已从 MultiBodyPlant 线性化的自由浮动系统上添加广义力。对于固定底座，广义力基本上对应于关节力矩，但对于自由浮动的情况，它也包括底座 torques/forces。然而，这些不包括在 MultiBodyPlant 系统的驱动端口中，因此在为线性化（或 LQR）提供驱动端口时，基本扳手不包括在驱动矩阵 B post-线性化中。对于我的工作，我想在线性化中也包括基数 torques/forces。为此，我可以想到 drake 中的两种方法，但我不确定哪种方法在架构上是 better/correct 决定。我计划稍后使用它来制作 TVLQR 控制器来稳定轨迹。
思路1: Change B matrix in LQR after linearization: 使用LinearQuadraticRegulator(system, context, Q, R, plant_actuation_input_pot()) 后手动编辑B矩阵
想法 2：创建一个 LeafeSystem class（类似于 this），输出端口连接到 get_applied_generalized_force_input_port() 和输入端口暴露外部图表（使用 ExportOutput()），然后将该系统作为一个整体线性化（包含连接到 MultiBodyPlant 的 CustomLeafSystem）。我假设这样做会将基本驱动添加到线性化系统的 B 矩阵中。
实际上这两个想法是我自己会做的。对于您的情况，似乎想法 1 更易于实施？
矩阵 B 将驱动指数映射到广义速度指数。因此，您必须确保将必要的“1”放在 B 的正确位置。您可以使用 Body::floating_velocities_start() 找到自由浮体索引，其中 returns 是完整状态 x (因此您需要减去 MultibodyPlant::num_positions() 以便在 B) 中建立索引。对于浮体，广义速度依次为 angular 速度 w_WB 和平移速度 v_WB。因此，广义力对应于在 B 处围绕其原点 Bo 施加的力矩 t_Bo_W 和 f_Bo_W，并在世界坐标系中表示。