运行仿真时System context mismatch error的调试方法咨询

我参照MakeManipulationStation脚本构建操作场景，创建包含物理引擎与控制器用两个multibody plant的AssemblyStation子系统，将controller_plant用于添加iiwa_impedance_controller，并作为子系统嵌入外层peg-in-hole图中。运行代码时出现如下错误：

Traceback (most recent call last):
  File "peginsert.py", line 990, in <module>
    main()
  File "peginsert.py", line 837, in main
    simulator.AdvanceTo(t)
  File "peginsert.py", line 407, in TauCalcOutput
    V_WS_W = self._S.CalcSpatialVelocity(
RuntimeError: A function call on a drake::multibody::MultibodyPlant<double> system named '::control_plant' was passed the Context of a system named '::peg-in-hole::AssemblyStation::iiwa_impedance_controller' instead of the appropriate subsystem Context.
For more information about Context-System mismatches, see https://drake.mit.edu/troubleshooting.html#framework-context-system-mismatch

调试步骤 #

• 定位Context来源：检查TauCalcOutput函数中调用CalcSpatialVelocity时传入的Context对象，确认它是controller_plant对应的子Context，而非控制器子系统的Context。必须从AssemblyStation的Context里提取controller_plant的子Context，不能直接使用当前控制器系统的Context。
• 规范子系统Context获取方式：在Drake中获取子系统Context，必须通过GetSubsystemContext方法，比如从AssemblyStation的Context中调用GetSubsystemContext(controller_plant, station_context)，禁止直接传递上层或其他子系统的Context。
• 校验系统层级关系：确认controller_plant在AssemblyStation中的层级路径，错误信息显示controller_plant路径为::control_plant，控制器路径为::peg-in-hole::AssemblyStation::iiwa_impedance_controller，说明你混淆了不同层级的Context，要确保调用CalcSpatialVelocity时使用的是controller_plant对应的Context。
• 添加调试打印：在出错代码前添加打印语句，比如print(context.get_system().get_name())，确认传入的Context所属系统是否正确，快速定位Context传递错误。
• 参考原脚本实现逻辑：对照MakeManipulationStation中双Plant的Context管理方式，原脚本中物理Plant和控制器Plant的Context均从Station的Context中提取，确保你的AssemblyStation遵循相同逻辑传递Context。

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