MultibodyPlant单对象端口PID控制及状态约定技术咨询

1. 获取单个物体的独立状态端口 #

• 无需使用返回全状态的get_state_output_port，直接调用MultibodyPlant的针对性接口即可：
  • GetBodyPoseOutputPort(body_index)：获取指定刚体的位姿输出端口（包含位置与旋转信息）
  • GetBodySpatialVelocityOutputPort(body_index)：获取指定刚体的空间速度输出端口
• 若需要从全状态向量中提取单个刚体的状态切片，可通过刚体索引定位其在全局状态中的起始位置：
  • 用body->get_position_start()拿到该刚体位置参数在全局q向量中的起始索引，结合body->num_positions()截取对应子向量（自由刚体为7维：3位置+4四元数）
  • 用body->get_velocity_start()拿到该刚体速度参数在全局v向量中的起始索引，结合body->num_velocities()截取对应子向量（自由刚体为6维：3角速度+3线速度）
  • 示例代码片段：

    const RigidBody<double>& body = plant.GetBodyByName("target_body");
    int q_start = body.get_position_start();
    Eigen::VectorXd body_q = plant.GetPositions(context).segment(q_start, body.num_positions());
    

2. RigidBody的状态q顺序约定 #

对于6自由度的自由刚体，其在全局状态q中的对应部分遵循固定顺序：

• 前3个元素：笛卡尔位置坐标(x, y, z)
• 后4个元素：单位四元数(w, x, y, z)（标量分量在前，用于表示旋转姿态）
  速度v的对应部分则是前3个为角速度(ωx, ωy, ωz)，后3个为线速度(vx, vy, vz)。

3. 从文档中确认状态约定的方法 #

• 查阅MultibodyPlant::get_state_output_port的文档说明，其中会明确状态向量[q, v]的整体结构，并链接到刚体状态的详细定义。
• 查看RigidBody类文档，或MultibodyPlant::GetPositions()、MultibodyPlant::GetVelocities()的接口说明，这些文档会直接标注单个刚体的位置、速度参数在全局向量中的排列逻辑。
• 快速验证：编写最小测试代码，创建一个已知位姿的自由刚体，获取其状态向量并打印，对比输出结果即可直观确认顺序。

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