如何在Drake中结合自由落体物体与线性振动台仿真？

• 核心思路：将振动台的运动驱动系统与自由落体物体整合到同一个Diagram中，关键是保证台面的周期性运动约束生效，同时保留物体的自由落体特性，确保碰撞检测正常工作——不要直接拼接两个独立代码块，而是基于统一的DiagramBuilder搭建完整系统。

• 系统整合步骤

  1. 搭建基础振动台系统
     先在MultibodyPlant中创建台面刚体，添加线性移动关节（如PrismaticJoint），并通过轨迹驱动（如TrajectorySource生成正弦/周期性轨迹）实现台面的往复运动。示例代码：

     from drake.multibody.plant import MultibodyPlant, DiscreteContactSolverType
     from drake.systems.framework import DiagramBuilder
     from drake.trajectories import PiecewisePolynomial
     import numpy as np
     
     builder = DiagramBuilder()
     plant = builder.AddSystem(MultibodyPlant(time_step=1e-3))
     plant.set_discrete_contact_solver(DiscreteContactSolverType.kSap)
     plant.set_contact_model(ContactModel.kHydroelastic)
     
     # 添加台面刚体与移动关节
     table_body = plant.AddRigidBody("table", mass=10.0)
     table_frame = table_body.body_frame()
     joint = plant.AddPrismaticJoint(
         "table_joint",
         frame_on_parent=plant.world_frame(),
         frame_on_child=table_frame,
         axis=np.array([0.0, 0.0, 1.0])  # 沿Z轴振动，可按需修改方向
     )
     
     # 添加周期性轨迹驱动
     # 示例：Z轴方向正弦运动，振幅0.5m，周期2π秒
     time_points = [0, 2*np.pi]
     pos_points = np.array([[0.0], [0.5], [0.0]])  # 对应时间点的位置
     traj = PiecewisePolynomial.FirstOrderHold(time_points, pos_points)
     traj_source = builder.AddSystem(TrajectorySource(traj))
     builder.Connect(traj_source.get_output_port(), plant.get_actuation_input_port())
     

  2. 添加自由落体物体
     在同一个MultibodyPlant中逐个添加落体刚体，设置不同的初始高度位置，无需额外约束（默认受重力作用自由下落），同时必须为台面和落体添加碰撞几何体与接触材料：

     from drake.geometry import Box, CoulombFriction
     from drake.math import RigidTransform
     
     # 添加3个不同高度的落体
     for idx in range(3):
         box_body = plant.AddRigidBody(f"box_{idx}", mass=1.0)
         # 设置初始位置：X方向偏移，Z方向不同高度
         init_pose = RigidTransform(p=[idx*0.3, 0.0, 4.0 + idx*1.5])
         plant.SetDefaultFreeBodyPose(box_body, init_pose)
     
         # 为落体添加碰撞与视觉几何
         plant.AddCollisionGeometry(
             box_body, RigidTransform(), Box(0.2, 0.2, 0.2), 
             f"box_collision_{idx}", CoulombFriction(0.6, 0.6)
         )
         plant.AddVisualGeometry(
             box_body, RigidTransform(), Box(0.2, 0.2, 0.2), 
             f"box_visual_{idx}", np.array([1.0, 0.0, 0.0])
         )
     
     # 为台面添加碰撞与视觉几何
     plant.AddCollisionGeometry(
         table_body, RigidTransform(), Box(1.0, 1.0, 0.1), 
         "table_collision", CoulombFriction(0.6, 0.6)
     )
     plant.AddVisualGeometry(
         table_body, RigidTransform(), Box(1.0, 1.0, 0.1), 
         "table_visual", np.array([0.0, 1.0, 0.0])
     )
     
     plant.Finalize()
     

  3. 配置仿真与可视化
     添加MeshcatVisualizer并构建完整Diagram，启动仿真器运行：

     from drake.geometry import MeshcatVisualizer
     from drake.systems.analysis import Simulator
     
     # 添加可视化
     visualizer = MeshcatVisualizer.AddToBuilder(builder, plant.get_geometry_output_port())
     
     # 构建Diagram并初始化仿真器
     diagram = builder.Build()
     simulator = Simulator(diagram)
     context = simulator.get_mutable_context()
     
     # 运行仿真
     simulator.AdvanceTo(10.0)
     

• 常见问题排查

  • 碰撞无反应：检查是否为所有参与碰撞的刚体添加了碰撞几何体，确认MultibodyPlant启用了接触模型（如kHydroelastic或kPoint）。
  • 振动台不动：验证轨迹驱动的输出端口是否正确连接到plant.get_actuation_input_port()，检查关节是否被其他约束锁定。
  • 物体异常运动：检查初始位置是否与台面重叠，调整仿真步长（如time_step=1e-3）确保碰撞检测精度。

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