如何在含多刚体的MultibodyPlant中使用JointStiffnessController

概念层面问题 #

1. 为什么JointStiffnessController需要传入MultibodyPlant？ #

JointStiffnessController核心是计算关节刚度/阻尼补偿力，这个过程依赖机器人的动力学参数（如质量矩阵、关节运动学关系）和实时构型信息，这些数据都存储在MultibodyPlant中。和仅依赖输入输出端口的PID控制器不同，它需要动态计算随构型变化的动力学项（比如质量矩阵M(q)），直接传入plant能让控制器高效调用内置计算逻辑，避免手动传递大量冗余信息。

2. MultibodyPlant是否提供重力力输出端口？ #

是的，MultibodyPlant有get_generalized_gravity_output_port()端口，可输出整个系统的广义重力项。但JointStiffnessController不止需要重力，还需要质量矩阵、关节雅克比等数据，直接依赖plant是更高效的实现方式。

3. 自定义系统能否直接引用MultibodyPlant？ #

可以，但要满足两个前提：

• 必须传入已完成Finalize()的plant实例的const引用，不能在自定义系统中修改plant结构；
• 调用plant的API时需符合Drake的线程安全要求（Drake系统默认线程安全，只读调用无问题）。

实践层面：解决维度不匹配问题 #

针对你的场景（主plant包含机器人、抓手、麦片盒等模型），有两种可行方案：

方案一：指定关节索引（推荐，更简洁） #

JointStiffnessController的构造函数支持传入joint_indices参数，直接限定控制器仅处理机器人的7个关节，无需创建新plant：

1. 从主plant中提取机器人所有可驱动关节的索引：

   std::vector<int> robot_joint_indices;
   for (const auto& joint : plant.GetJointIndices(robot_model_instance)) {
       if (joint.has_actuator()) {
           robot_joint_indices.push_back(joint.index());
       }
   }
   

2. 实例化控制器时传入主plant和关节索引：

   auto controller = std::make_unique<JointStiffnessController<double>>(
       plant, robot_joint_indices, "robot_stiffness_controller");
   

3. 端口连接：
   • 用JointSelector从主plant的状态输出中提取机器人关节的位置/速度，连接到控制器的get_state_input_port()和get_desired_state_input_port()；
   • 控制器的输出（机器人关节力）直接连接到主plant对应关节的驱动输入端口（可通过plant.GetJointActuationInputPort(joint_index)获取）。

方案二：创建仅含机器人的子Plant并关联到同一场景图 #

如果需要完全隔离机器人的动力学计算，可以创建独立的子plant：

1. 创建共享的场景图，分别初始化主plant（含所有模型）和机器人子plant（仅加载机器人模型）：

   auto scene_graph = std::make_unique<SceneGraph<double>>();
   // 主plant
   auto main_plant = std::make_unique<MultibodyPlant<double>>(time_step);
   main_plant->RegisterAsSourceForSceneGraph(scene_graph.get());
   // 加载机器人、抓手、麦片盒等模型
   // ...
   main_plant->Finalize();
   
   // 机器人子plant
   auto robot_plant = std::make_unique<MultibodyPlant<double>>(time_step);
   robot_plant->RegisterAsSourceForSceneGraph(scene_graph.get());
   // 加载和主plant中完全相同的机器人模型
   auto robot_model_instance = robot_plant->AddModelFromFile(robot_urdf_path);
   robot_plant->Finalize();
   

2. 实例化JointStiffnessController时传入机器人子plant；
3. 端口连接：
   • 将主plant的机器人关节状态端口连接到控制器的输入；
   • 控制器输出的关节力映射到主plant的机器人关节驱动输入端口。

注意：必须保证子plant和主plant中的机器人模型完全一致（URDF/SDF文件、关节配置相同），否则会出现动力学不匹配。

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