pydrake调用get_body_poses_output_port()获取末端位姿方案咨询

• Ubuntu 20.04
• Python 3.7
• drake 0.38.0
• PyCharm
• pydrake脚本开发

开发需求为建立机器人末端执行器笛卡尔空间当前位姿与控制器输入端口的连接，原计划调用get_body_poses_output_port()接口，经三步处理得到控制器所需输入：

1. 仅提取末端执行器（最后一个刚体）对应的RigidTransform数据
2. 将该RigidTransform拆解为旋转分量与平移分量
3. 将两类分量拼接为自定义控制器要求的输入格式

最初尝试通过Demultiplexer实现第一步，预期解复用器接收n_dof+1个刚体各自对应的9维分量，拆分为n_dof+1路输出，编写代码如下：

demux = builder.AddSystem(Demultiplexer(np.full(n_dof+1,9)))
builder.Connect(plant.get_body_poses_output_port(), demux.get_input_port())

运行时触发报错：

RuntimeError: DiagramBuilder::Connect: Cannot mix vector-valued and abstract-valued ports while connecting output port body_poses of System plant to input port u0 of System drake/systems/Demultiplexer@00000000040505c0

排查发现get_body_poses_output_port()为抽象值端口，无法直接连接仅支持向量值输入的Demultiplexer，需要可行的末端位姿获取方案。

get_body_poses_output_port()输出的是抽象类型数据，内容为所有刚体的RigidTransform集合，并非之前假设的按刚体拆分的固定维度数值向量，因此不能直接接入Demultiplexer做信号拆分。

不需要先拉取所有刚体位姿再做拆分，直接调用Drake提供的单刚体位姿输出端口，搭配自定义轻量转换系统即可完成格式适配，步骤如下：

1. 直接获取末端执行器的单独位姿端口
   先定位到末端执行器对应的刚体对象，直接调用单体位姿接口，该接口会单独输出目标刚体的RigidTransform抽象值，无需额外做全量刚体数据拆分：

   # 若已知末端刚体名称可直接按名称获取，也可遍历刚体列表取最后一个运动刚体
   ee_body = plant.GetBodyByName("your_end_effector_link_name")
   ee_pose_port = plant.get_body_pose_output_port(ee_body.index())
   

2. 编写位姿格式转换LeafSystem，完成RigidTransform到控制器输入向量的转换
   由于不同控制器对输入的旋转表示形式、分量顺序要求不同，自定义转换系统灵活性最高，示例代码如下（以3维平移+9维展平旋转矩阵共12维输出为例）：

   import numpy as np
   from pydrake.systems.framework import LeafSystem, BasicVector
   from pydrake.math import RigidTransform
   
   class Pose2CtrlInput(LeafSystem):
       def __init__(self):
           super().__init__()
           # 声明抽象输入端口，接收RigidTransform类型数据
           self.DeclareAbstractInputPort("ee_pose", RigidTransform())
           # 声明向量输出端口，维度和控制器输入要求保持一致
           self.DeclareVectorOutputPort("ctrl_input", BasicVector(12), self._calc_output)
       
       def _calc_output(self, context, output):
           ee_pose = self.get_input_port(0).Eval(context)
           # 提取平移分量
           trans = ee_pose.translation()
           # 提取旋转分量，按需替换为四元数/欧拉角等其他表示
           rot_mat = ee_pose.rotation().matrix().flatten()
           # 按控制器要求的分量顺序拼接
           ctrl_input_vec = np.concatenate([trans, rot_mat])
           output.SetFromVector(ctrl_input_vec)
   

3. 在DiagramBuilder中完成全链路连接

   # 实例化转换模块
   pose_converter = builder.AddSystem(Pose2CtrlInput())
   # 连接末端位姿端口到转换器输入
   builder.Connect(ee_pose_port, pose_converter.get_input_port())
   # 连接转换器输出到控制器输入端口
   builder.Connect(pose_converter.get_output_port(), controller.get_input_port())
   

提示：如果控制器要求旋转分量为四元数、RPY欧拉角等格式，仅需修改转换逻辑中旋转分量的提取代码即可，比如四元数可调用ee_pose.rotation().ToQuaternion().wxyz()获取，注意和控制器要求的分量顺序保持一致。

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