向线性化（或LQR）MultiBodyPlant添加广义力的最优方法

针对你遇到的问题——要把自由漂浮系统的基座力矩/力纳入线性化模型（用于TVLQR控制器），我来帮你分析两种方案的优劣，以及更推荐的实现方式：

方案1：线性化后手动修改LQR的B矩阵 #

这个方法确实简单直接，调用LinearQuadraticRegulator()之后手动编辑B矩阵，把基座wrench对应的输入通道补上。但它的问题也很明显：

• 维护性差：如果后续你的模型（比如关节数量、基座构型）或者平衡点发生变化，手动修改B矩阵的逻辑必须同步调整，很容易出错。
• 语义脱节：线性化后的系统本应是原系统动力学的直接推导，手动修改相当于脱离了原系统的物理语义，排查问题时很难追溯根源。
• TVLQR适配困难：TVLQR需要随轨迹变化的时变线性化模型，每个时刻都要手动修改B矩阵的话，工作量大且容易引入一致性问题。

方案2：用LeafSystem封装输入并连接到广义力端口 #

这是Drake架构下更推荐的正确做法，完全契合Drake的模块化设计思路：
MultiBodyPlant的get_applied_generalized_force_input_port()本来就是用来接收外部施加的广义力的——对于自由漂浮系统，广义力向量包含两部分：前6维是基座的空间wrench（力矩在前、力在后，符合Drake的空间向量定义），后面的维度是关节力矩。

具体实现步骤大概是：

1. 创建一个自定义LeafSystem：
   • 定义输入端口：比如一个6维端口对应基座的wrench，如果你还需要关节力矩输入，可以再加对应维度的端口。
   • 定义输出端口：维度等于MultiBodyPlant的广义力数量（即plant.num_velocities()，因为自由漂浮系统的广义力维度等于广义速度维度）。
   • 在CalcOutput方法中，把输入的基座wrench填充到广义力向量的前6位，关节力矩部分可以设为0（如果不需要）或者从输入端口读取。
2. 将这个LeafSystem的输出端口连接到MultiBodyPlant的get_applied_generalized_force_input_port()。
3. 对这个复合系统进行线性化，或者直接调用LinearQuadraticRegulator()——此时得到的B矩阵会自动包含基座wrench的输入通道。

方案2的优势 #

• 正确性有保障：线性化是基于完整的复合系统动力学推导的，完全符合Drake的广义力定义，不会出现手动修改B矩阵时的顺序错误等问题。
• 扩展性强：后续如果要添加其他输入（比如额外的外力），只需要修改LeafSystem的逻辑即可，不需要改动原MultiBodyPlant的结构。
• 适配TVLQR更顺畅：TVLQR需要的时变线性化模型会自动从复合系统生成，不需要额外的手动调整，保证了每个时刻线性化模型的一致性。

总结 #

如果你的目标是构建稳定的TVLQR控制器，方案2是架构上更优、更可靠的选择——它既符合Drake的设计哲学，又能避免手动修改带来的潜在错误，长期维护和扩展也更方便。

内容的提问来源于stack exchange，提问作者Shubham Vyas