Drake仿真运行时动态添加物品的可行性及实现方法咨询

核心结论 #

完全可行，Drake支持运行时动态向仿真场景中添加待抓取物品这类Actor，核心是利用其动态系统扩展能力和状态迁移机制，以下是具体实现思路：

实现方案 #

1. 基于延迟初始化的MultibodyPlant动态扩展

• 初始化MultibodyPlant时，不立即调用Finalize()，预留动态添加物体的空间。
• 当外部应用告知需要添加新物品时：
  • 使用MultibodyPlant::AddRigidBody()或AddModelFromFile()导入物品模型；
  • 调用MultibodyPlant::Finalize()完成初始化；
  • 基于更新后的MultibodyPlant重新构建Diagram，并创建新的Context；
  • 将Simulator的Diagram和Context替换为新的实例，同时迁移原有机器人、场景的状态（如机器人关节角度、已存在物体的位姿）。

2. 使用MutableDiagram实现热更新

• 预先构建包含MultibodyPlant和其他系统的MutableDiagram（而非不可变的Diagram）；
• 在运行时，通过MutableDiagram::AddSystem()添加新的物品模型（可将物品作为独立LeafSystem或直接扩展MultibodyPlant）；
• 使用MutableDiagram::Connect()完成新系统与原有仿真环境的信号连接；
• 重新生成Context并同步原有状态，之后继续仿真循环。

3. 关键注意事项

• 状态迁移是核心：替换Diagram或更新MultibodyPlant后，需将原有仿真环境的状态（如机器人关节位置、已存在物体的位姿）复制到新的Context中，避免仿真断层；
• 控制性能开销：频繁动态添加物体会带来初始化开销，若物品数量较多，建议批量处理；
• 碰撞检测验证：添加新物品后，需确保MultibodyPlant的碰撞检测器（如Bullet或Hydroelastic）正确加载新的碰撞模型，避免碰撞检测失效。

示例代码片段 #

// 假设已初始化未Finalize的MultibodyPlant
multibody::MultibodyPlant<double>* plant = ...;

// 从外部应用获取物品模型路径
std::string object_model_path = GetObjectPathFromExternalApp();

// 将物品添加到Plant
auto [object_instance, _] = plant->AddModelFromFile(object_model_path);

// 完成Plant初始化
plant->Finalize();

// 重新构建Diagram
systems::DiagramBuilder<double> builder;
builder.AddSystem(plant);
// 连接机器人控制器、可视化器等其他系统...
auto diagram = builder.Build();

// 创建新Context并迁移原有状态
std::unique_ptr<systems::Context<double>> new_context = diagram->CreateDefaultContext();
CopyContextState(old_context, new_context.get()); // 自定义状态迁移函数

// 更新Simulator继续仿真
simulator->Reset(std::move(diagram), std::move(new_context));

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