python - 用于 pydrake 模拟的 MultibodyPlant、SceneGraph、Context 和 Simulator 的结构

Question

尝试在 PyDrake 中正确设置模拟时，我感到非常困惑。我想要的是有一个驱动的机器人(例如上面有一个 InverseDynamicsController)以及场景中机器人将操纵的对象。然而，我正在努力弄清楚如何正确创建和使用 MultibodyPlant、SceneGraph、Context、Simulator 组合。

这大致是我尝试做的事情:

builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=1e-4)
parser = Parser(plant, scene_graph)
# Add my robot 
robot = parser.AddModelFromFile(robot_urdf)
robot_base = plant.GetFrameByName('robot_base')
plant.WeldFrames(plant.world_frame(), robot_base)
# Add my object
parser.AddModelFromFile(FindResourceOrThrow("drake/my_object.urdf"))
plant.finalize()

# Add my controller
Kp = np.full(6, 100)
Ki = 2 * np.sqrt(Kp)
Kd = np.full(6, 1)
controller = builder.AddSystem(InverseDynamicsController(plant, Kp, Ki, Kd, False))
controller.set_name("sim_controller");
builder.Connect(plant.get_state_output_port(robot),
                controller.get_input_port_estimated_state())
builder.Connect(controller.get_output_port_control(),
                plant.get_actuation_input_port())

# Get the diagram, simulator, and contexts
diagram = builder.Build()
simulator = Simulator(diagram)
context = simulator.get_mutable_context()
plant_context = plant.GetMyContextFromRoot(context)

但是，这有一些不良的品质。首先，只要我添加了对象，就会收到此错误:

Failure at systems/controllers/inverse_dynamics_controller.cc:32 in SetUp(): condition 'num_positions == dim' failed.

其次，添加对象后，对象姿势成为我的 InverseKinematics 问题的一部分，当我使用 plant_context 执行 SetPositions 时，我必须设置我的两个 ARM 关节以及物体的姿势，当我觉得我应该只使用 SetPositions 设置机器人的关节位置时。

我意识到我在这个设置中做错了，我只是想知道拥有一个 Simulator 实例的正确方法是什么，我可以用它来运行模拟，该实例具有已启动的机器人和可操纵的物体？我应该创造多种植物吗？多个上下文？谁与谁分享什么？

我真的很感谢对此的一些建议，或者指向示例的指针。 Drake 很棒，但我很难找到满足我要求的最小示例。

Answer 1

1. 是的，您可以添加单独的 MultibodyPlant 进行控制。请参阅https://github.com/RobotLocomotion/drake/blob/master/examples/planar_gripper/planar_gripper_simulation.cc举个例子。尽管它是用 C++ 编写的，但该设置与您的类似。您可以尝试模仿图表的连接方式。
2. 当您确实有两个工厂时，您需要在模拟工厂(而不是控制工厂)上调用 SetPositions。您只能使用 ModelInstanceIndex 设置机器人位置。

# Add my robot 
robot = parser.AddModelFromFile(robot_urdf)
...
plant.SetPositions(plant_context, robot, robot_positions)