PyDrake仿真：驱动机器人与操作对象的正确配置问询

问题描述 #

需搭建搭载InverseDynamicsController的驱动机器人仿真场景，包含可操作对象，但对MultibodyPlant、SceneGraph、Context、Simulator的组合使用存在困惑。尝试代码如下：

builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=1e-4)
parser = Parser(plant, scene_graph)
# 添加机器人 
robot = parser.AddModelFromFile(robot_urdf)
robot_base = plant.GetFrameByName('robot_base')
plant.WeldFrames(plant.world_frame(), robot_base)
# 添加操作对象
parser.AddModelFromFile(FindResourceOrThrow("drake/my_object.urdf"))
plant.finalize()

# 添加控制器
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())

# 获取图、仿真器及上下文
diagram = builder.Build()
simulator = Simulator(diagram)
context = simulator.get_mutable_context()
plant_context = plant.GetMyContextFromRoot(context)

遇到两个问题：

1. 添加操作对象后报错：

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

2. 调用SetPositions时需同时设置机器人关节和对象位姿，希望仅设置机器人关节位姿即可。

咨询：同时包含驱动机器人与可操作对象的Simulator实例的正确创建方式，是否需要多个MultibodyPlant或Context，各组件如何共享资源？

问题解答 #

1. 控制器参数维度不匹配的解决 #

报错原因是InverseDynamicsController的增益参数（Kp/Ki/Kd）维度，必须与机器人模型的关节自由度数量完全一致，而非固定的6。添加操作对象后，MultibodyPlant的总自由度包含了对象的自由度，但控制器仅需针对机器人模型配置。

修正步骤：

• 在plant.finalize()后，获取机器人模型的关节位置数：

  robot_num_positions = plant.num_positions(robot)
  robot_num_velocities = plant.num_velocities(robot)
  

• 基于该数量生成对应维度的增益参数：

  Kp = np.full(robot_num_positions, 100)
  Ki = 2 * np.sqrt(Kp)
  Kd = np.full(robot_num_velocities, 1)
  

2. 单独设置机器人关节位姿的方法 #

无需创建多个MultibodyPlant或Context，单个MultibodyPlant即可容纳机器人和操作对象，共享同一SceneGraph与仿真上下文。调用SetPositions时，指定目标模型实例即可仅修改机器人关节位姿：

# 仅设置机器人关节位姿
robot_q0 = np.array([0.0, 0.5, -1.0, ...])  # 维度匹配机器人关节数
plant.SetPositions(plant_context, robot, robot_q0)

完整修正示例代码 #

import numpy as np
from pydrake.systems.framework import DiagramBuilder
from pydrake.multibody.plant import AddMultibodyPlantSceneGraph
from pydrake.multibody.parsing import Parser
from pydrake.common import FindResourceOrThrow
from pydrake.systems.controllers import InverseDynamicsController
from pydrake.systems.analysis import Simulator

builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=1e-4)
parser = Parser(plant, scene_graph)

# 添加机器人
robot_urdf = "path/to/your/robot.urdf"  # 替换为实际URDF路径
robot = parser.AddModelFromFile(robot_urdf)
robot_base = plant.GetFrameByName('robot_base')
plant.WeldFrames(plant.world_frame(), robot_base)

# 添加操作对象
parser.AddModelFromFile(FindResourceOrThrow("drake/my_object.urdf"))

plant.finalize()

# 获取机器人自由度数量
robot_num_positions = plant.num_positions(robot)
robot_num_velocities = plant.num_velocities(robot)

# 初始化控制器（匹配机器人自由度）
Kp = np.full(robot_num_positions, 100)
Ki = 2 * np.sqrt(Kp)
Kd = np.full(robot_num_velocities, 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())

# 构建仿真系统
diagram = builder.Build()
simulator = Simulator(diagram)
context = simulator.get_mutable_context()
plant_context = plant.GetMyContextFromRoot(context)

# 仅设置机器人初始关节位姿
robot_q0 = np.zeros(robot_num_positions)  # 替换为实际初始位姿
plant.SetPositions(plant_context, robot, robot_q0)

# 启动仿真
simulator.AdvanceTo(5.0)

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