新版Drake中MultiBodyPlant与刚体树广义坐标向量映射实现方法

新版Drake的映射实现方法 #

如果你的业务场景没有必须兼容旧RigidBodyTree的存量代码，优先直接将全部逻辑迁移到MultibodyPlant原生接口，不需要额外做坐标映射，是成本最低的方案。如果必须要兼容旧逻辑，可以采用如下两种实现方式：

• 关节索引遍历构造映射矩阵（最稳定）
  提前整理旧RigidBodyTree中广义坐标对应的关节名称顺序，通过MultibodyPlant的关节查询接口获取每个关节在MBP广义坐标中的索引位置，直接构造映射矩阵，示例代码如下：

  import numpy as np
  from pydrake.multibody.plant import MultibodyPlant
  
  # 入参：已加载对应URDF模型的MultibodyPlant实例
  plant: MultibodyPlant = ...
  # 旧RigidBodyTree中广义坐标对应的关节名称顺序，按原RBT的q排序规则填写
  rbt_joint_order = ["joint_1", "joint_2", "joint_3", ...] 
  num_q = plant.num_positions()
  # 初始化映射矩阵 q_rbt = X @ q_mbp
  X = np.zeros((num_q, num_q))
  
  for rbt_q_idx, joint_name in enumerate(rbt_joint_order):
      joint = plant.GetJointByName(joint_name)
      # 获取该关节在MBP q向量中的起始索引
      mbp_q_start = joint.position_start()
      # 以下为1自由度关节的映射逻辑，多自由度关节可按自由度数量扩展赋值逻辑
      X[rbt_q_idx, mbp_q_start] = 1
  

• 驱动矩阵适配方案
  你之前用到的通过驱动矩阵构造映射的逻辑依然可用，仅需将原RigidBodyTree的B矩阵替换为按照旧RBT关节顺序手动构造的驱动矩阵即可，不需要依赖已弃用的RigidBodyTree接口。

两类对象广义坐标排序不统一的原因 #

• 历史迭代差异：RigidBodyTree是Drake早期推出的运动学专用组件，排序逻辑优先匹配URDF文件的节点写入顺序，浮动基6维坐标默认放在q向量最前端；而MultibodyPlant是后续重构的新一代多体动力学核心组件，设计排序逻辑时需要适配闭环约束、多浮动基、耦合关节等更复杂的场景，两者属于不同迭代周期的产物，设计初期就没有做排序对齐的规划。
• 设计目标差异：RigidBodyTree仅支持树结构的运动学计算，逻辑简单不需要考虑复杂的系统适配；MultibodyPlant需要同时支持离散/连续动力学求解、接触计算、执行器绑定、外部系统交互等能力，排序逻辑需要兼顾内部计算效率，因此没有沿用旧RBT的排序规则。
• 长期兼容性考量：MultibodyPlant设计时为了保证自身API的长期稳定性，没有为了兼容已被弃用的RigidBodyTree做排序适配，避免后续自身功能迭代受旧组件逻辑的限制。

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