Drake仿真中自定义箱体悬浮不倒、旋转疑似锁定问题排查

我使用自定义obj和SDF文件搭建了一个简单的箱体仿真，已启用Hydroelastic接触（点接触时也存在相同问题）。箱体带有质量，惯性矩为球型且完全位于中心。我通过load_scenario和MakeHardwareStation将SDF添加到场景中，未锁定任何位置或旋转。但运行仿真时，箱体却处于“悬浮”状态，不会翻倒下落；即使以一定角度添加箱体，其旋转状态也保持不变，疑似旋转被锁定。核心代码如下：

builder = DiagramBuilder()

### Add Boxes
relative_path_to_gnd = 'Ground.sdf'
absolute_path_to_gnd = os.path.abspath(relative_path_to_gnd)

box_directives = f"""
directives:
- add_model:
    name: Ground
    file: file://{absolute_path_to_gnd}

- add_weld:
    parent: world
    child: Ground::Ground
"""
for i in range(NUM_BOXES):
    relative_path_to_box = 'Box_0_5_0_5_0_5.sdf'
    absolute_path_to_box = os.path.abspath(relative_path_to_box)

    box_pos_x = np.random.uniform(-1.5, 1.5, 1)
    box_pos_y = np.random.uniform(-1, 1, 1)
    box_pos_z = np.random.uniform(0, 5, 1)

    box_directives += f"""
- add_model: 
    name: Box_{i}
    file: file://{absolute_path_to_box}
    default_free_body_pose:
        Box_0_5_0_5_0_5:
            translation: [{box_pos_x[0]}, {box_pos_y[0]}, {box_pos_z[0]}]
            rotation: !Rpy {{ deg: [0, 0, 0] }}
"""

print(f"box_directives: {box_directives}")
    
scenario = load_scenario(data=box_directives)


### Hardware station setup
station = builder.AddSystem(MakeHardwareStation(
    scenario=scenario,
    meshcat=meshcat,
))
scene_graph = station.GetSubsystemByName("scene_graph")
plant = station.GetSubsystemByName("plant")

diagram = builder.Build()
simulator = Simulator(diagram)

####################################
### Running Simulation & Meshcat ###
####################################
simulator.set_target_realtime_rate(1)
simulator.set_publish_every_time_step(True)
plt.show()

meshcat.StartRecording()
simulator.AdvanceTo(box_randomization_runtime)
meshcat.PublishRecording()

核心原因 #

问题出在MakeHardwareStation的默认配置：

• 该函数默认启用重力补偿，抵消了重力对箱体的作用，导致箱体不会下落；
• 默认将所有模型的自由度设置为受控模式，相当于锁定了刚体的平移和旋转，所以箱体保持初始姿态不变。

另外需检查Box的SDF文件：如果没有显式设置<free>true</free>，结合MakeHardwareStation的默认行为，刚体可能被误识别为固定/受控状态，进一步加剧问题。

修复步骤 #

1. 修改MakeHardwareStation配置，关闭重力补偿并将箱体设置为自由运动刚体：

   station = builder.AddSystem(MakeHardwareStation(
       scenario=scenario,
       meshcat=meshcat,
       gravity_compensation=False,
       # 为每个箱体模型配置无驱动模式，使其自由运动
       model_configs={f"Box_{i}": ModelConfig(actuation=ActuationMode.kNone) for i in range(NUM_BOXES)}
   ))
   

2. 检查并修正Box的SDF文件，确保刚体标记为自由状态：

   <model name="Box_0_5_0_5_0_5">
     <link name="Box_0_5_0_5_0_5">
       <inertial>
         <mass>1.0</mass>
         <inertia>
           <ixx>0.041667</ixx>
           <ixy>0.0</ixy>
           <ixz>0.0</ixz>
           <iyy>0.041667</iyy>
           <iyz>0.0</iyz>
           <izz>0.041667</izz>
         </inertia>
       </inertial>
       <!-- 碰撞、视觉几何定义 -->
       <free>true</free>
     </link>
   </model>
   

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