PyDrake搭建移动IIWA机械臂时Simulator报错求助

团队用PyDrake搭建带平面移动基座的IIWA机械臂，通过两个移动关节（Prismatic）和一个旋转关节（Revolute）手动定义基座。基础箱体测试正常，但添加IIWA后仿真报错：

Traceback (most recent call last):
  File "planar_joint.py", line 107, in <module>
    simulator.AdvanceTo(12.0)
RuntimeError: Error control wants to select step smaller than minimum allowed (1e-14)

已知可能和奇异位姿或物理引擎无穷值有关，但未强制机器人进入奇异位姿，求修复方案。相关代码与场景文件如下：

主代码 #

meshcat = StartMeshcat()

meshcat.Flush()
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0005)

parser = Parser(plant)

scene_models = parser.AddModels("cargobot-models/scene_without_robot.dmd.yaml")

iiwa = plant.GetModelInstanceByName("iiwa")

false_body = plant.AddRigidBody(
        "false_body", iiwa,
        SpatialInertia(0, [0, 0, 0], UnitInertia(0, 0, 0)))
false_body2 = plant.AddRigidBody(
        "false_body2", iiwa,
        SpatialInertia(0, [0, 0, 0], UnitInertia(0, 0, 0)))

mobile_base_y = plant.AddJoint(PrismaticJoint(
        "mobile_base_y", plant.GetFrameByName("world"), plant.GetFrameByName("false_body"), 
        [0, 1, 0], -3, 3))
plant.AddJointActuator("mobile_base_y_actuator", mobile_base_y)

mobile_base_x = plant.AddJoint(PrismaticJoint(
        "mobile_base_x", plant.GetFrameByName("false_body"), plant.GetFrameByName("false_body2"), 
        [1, 0, 0], -3, 3))
plant.AddJointActuator("mobile_base_x_actuator", mobile_base_x)

mobile_base_theta = plant.AddJoint(RevoluteJoint(
        "mobile_base_theta", plant.GetFrameByName("false_body2"), plant.GetFrameByName("iiwa_link_0"), 
        [0, 0, 1],  -np.pi, np.pi))
plant.AddJointActuator("mobile_base_theta_actuator", mobile_base_theta)

plant.Finalize()

visualizer = MeshcatVisualizer.AddToBuilder(
    builder, scene_graph, meshcat)

plant_context = plant.CreateDefaultContext()

kp=[100] * plant.num_positions()
ki=[1] * plant.num_positions()
kd=[20] * plant.num_positions()

planar_controller = builder.AddSystem(
    InverseDynamicsController(plant, kp, ki, kd, False))
planar_controller.set_name("planar_controller")
builder.Connect(plant.get_state_output_port(),
                planar_controller.get_input_port_estimated_state())
builder.Connect(planar_controller.get_output_port_control(),
                plant.get_actuation_input_port())

diagram = builder.Build()
context = diagram.CreateDefaultContext()

states = [1, 0, 0, -1.57, 0.1, 0, -1.2, 0, 1.6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ]
planar_controller.GetInputPort('desired_state').FixValue(
    planar_controller.GetMyMutableContextFromRoot(context), states)

simulator = Simulator(diagram, context)
visualizer.StartRecording()
simulator.AdvanceTo(12.0)
visualizer.StopRecording()
visualizer.PublishRecording()

场景YAML文件 #

directives:          
    - add_model:
        name: cargo-space
        file: file:///usr/cargobot/cargobot-project/trajopt/cargobot-models/cargo-space.sdf

    - add_weld:
        parent: world
        child: cargo-space::cargo-space
        X_PC:
            rotation: !Rpy { deg: [0, 90, 0 ]}
            translation: [-1.5, 0, 0.4]

    - add_frame: 
        name: table_top_center
        X_PF:
            base_frame: world
            rotation: !Rpy { deg: [0, 0, 0]}
            translation: [0, 0, -0.5]

    - add_model:
        name: table_top
        file: file:///usr/cargobot/cargobot-project/trajopt/cargobot-models/table_top.sdf

    - add_weld: 
        parent: table_top_center
        child: table_top_link

    - add_model:
        name: iiwa
        file: package://drake/manipulation/models/iiwa_description/iiwa7/iiwa7_no_collision.sdf
        default_joint_positions:
            iiwa_joint_1: [-1.57]
            iiwa_joint_2: [0.1]
            iiwa_joint_3: [0]
            iiwa_joint_4: [-1.2]
            iiwa_joint_5: [0]
            iiwa_joint_6: [ 1.6]
            iiwa_joint_7: [0]

    - add_model:
        name: wsg
        file: package://drake/manipulation/models/wsg_50_description/sdf/schunk_wsg_50_with_tip.sdf
    - add_weld:
        parent: iiwa::iiwa_link_7
        child: wsg::body
        X_PC:
            translation: [0, 0, 0.09]
            rotation: !Rpy { deg: [90, 0, 90]}

目标：让机器人左移1单位。

1. 替换零惯性刚体 #

代码中添加的false_body和false_body2是零惯性刚体，PyDrake处理这类刚体时极易出现数值奇异，导致仿真步长崩溃。将其改为带微小惯性的刚体：

# 替换原零惯性刚体定义
small_mass = 1e-3
false_body = plant.AddRigidBody(
        "false_body", iiwa,
        SpatialInertia(small_mass, [0, 0, 0], UnitInertia(1e-3, 1e-3, 1e-3)))
false_body2 = plant.AddRigidBody(
        "false_body2", iiwa,
        SpatialInertia(small_mass, [0, 0, 0], UnitInertia(1e-3, 1e-3, 1e-3)))

2. 修正状态数组维度 #

当前states数组有24个元素，但系统实际状态量应为：3个移动基座关节（位姿+速度）+7个IIWA关节（位姿+速度）=20个元素。冗余的状态输入会导致控制器逻辑混乱，引发动力学异常。修正后的状态数组：

# 格式：[x, y, theta, j1, j2, j3, j4, j5, j6, j7, vx, vy, vtheta, vj1, vj2, vj3, vj4, vj5, vj6, vj7]
states = [1, 0, 0, -1.57, 0.1, 0, -1.2, 0, 1.6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]

3. 优化控制器增益与时间步长 #

• 时间步长0.0005过小，搭配高增益控制器容易触发数值震荡，建议调整为0.001或0.002
• 移动基座与机械臂关节惯性差异大，统一增益会导致基座控制不稳定，设置差异化增益：

  # 前3个是移动基座关节，后7个是IIWA关节
  kp = [20, 20, 20] + [100]*7
  ki = [0.1, 0.1, 0.1] + [1]*7
  kd = [5, 5, 5] + [20]*7
  

4. 规避腕部奇异位姿 #

IIWA初始关节角度j4=-1.2、j6=1.6接近腕部奇异点，即使未强制进入，也可能在控制过程中触发数值问题。微调初始角度，比如将j4改为-1.0，j6改为1.5，降低奇异风险。

内容的提问来源于stack exchange，提问作者Berke Uçar