添加碰撞约束后轨迹优化出现SNOPT RuntimeError问题咨询

错误核心解释 #

这个错误的本质是：你的MultibodyPlant（多体系统）的几何查询输入端口未与SceneGraph连接。当你在URDF中添加碰撞标签后，Drake需要通过SceneGraph获取几何查询对象来处理碰撞检测、距离计算这类几何相关的约束或残差计算，但你的代码里没完成这个端口连接，导致系统在尝试计算这些依赖几何信息的项时失败。

关于你提到的“时间导数或其残差”：这里指的是轨迹优化过程中，与碰撞约束（比如小球和障碍物的最小距离约束）相关的残差项，或者这些约束对时间/状态变量的导数计算——这类计算必须依赖SceneGraph提供的几何数据，没有连接端口的话，系统无法获取这些数据，因此抛出这个错误。

为什么之前无障碍物时正常 #

未添加碰撞标签时，你的多体系统不需要处理任何几何交互逻辑，因此不需要依赖SceneGraph的几何查询能力，端口未连接也不会触发错误。添加碰撞后，系统自动启用了几何约束相关的计算，这时端口未连接的问题就暴露出来了。

解决步骤 #

1. 创建并关联SceneGraph与MultibodyPlant
   在构建MultibodyPlant时，必须同时创建SceneGraph并完成注册：

   DiagramBuilder<double> builder;
   // 同时添加Plant和SceneGraph（推荐方式）
   auto [plant, scene_graph] = AddMultibodyPlantSceneGraph(&builder, time_step);
   // 加载你的URDF模型
   parser.AddModelsFromFile("your_model.urdf");
   

2. 连接几何查询端口
   确保在构建Diagram前，将SceneGraph的查询输出端口连接到Plant的几何查询输入端口：

   builder.Connect(
       scene_graph.get_query_output_port(),
       plant.get_geometry_query_input_port());
   // 完成Diagram构建
   auto diagram = builder.Build();
   

3. 同步上下文初始化
   在轨迹优化的Context设置中，要使用Diagram的Context，而不是单独的Plant Context，这样能保证Plant和SceneGraph的状态同步：

   auto diagram_context = diagram->CreateDefaultContext();
   auto plant_context = &diagram->GetMutableSubsystemContext(*plant, diagram_context);
   

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