如何在PyDrake仿真中利用传感器捕获模型动态运动数据？

问题描述 #

我在PyDrake中运行仿真，尝试捕获能反映模型随时间运动的传感器输出。已使用ForcedPublish和Simulator搭建仿真，并在单独线程中采集传感器数据，但传感器输出仅捕获模型初始位置，无法随仿真进程更新。

获取点云的代码 #

def get_point_cloud(diagram: Diagram, normals=True, down_sample=True):
    # Obtain the root context from the diagram
    root_context = diagram.CreateDefaultContext()
    
    # Obtain the context for the specific subsystem (e.g., 'plant')
    plant = diagram.GetSubsystemByName("plant")
    plant_context = plant.GetMyContextFromRoot(root_context)
    
    # Create a context for the diagram to ensure the output ports are evaluated correctly
    diagram_context = diagram.GetMyContextFromRoot(root_context)
    
        
    pcd = []
    number_of_camera = list_camera_ports(diagram)
    min_val = 5
    max_val = 5
    for i in range(number_of_camera):
        # cloud = diagram.GetOutputPort(f"camera{i}_point_cloud").Eval(plant_context)
        # pcd.append(cloud.Crop(lower_xyz=[-min_val, -min_val, -min_val], upper_xyz=[max_val, max_val, max_val]))

        # Access the output port from the diagram context
        output_port = diagram.GetOutputPort(f"camera{i}_point_cloud")
        
        # Evaluate the output port using the diagram context
        cloud = output_port.Eval(diagram_context)
        # Crop the point cloud
        pcd.append(cloud.Crop(lower_xyz=[-min_val, -min_val, -min_val], upper_xyz=[max_val, max_val, max_val]))
    
        if normals:
            pcd[i].EstimateNormals(radius=1, num_closest=50)
            camera = plant.GetModelInstanceByName(f"camera{i}")
            body = plant.GetBodyByName("base", camera)
            X_C = plant.EvalBodyPoseInWorld(plant_context, body)
            pcd[i].FlipNormalsTowardPoint(X_C.translation())

    # Merge point clouds
    merged_pcd = Concatenate(pcd)
    down_sampled_pcd = merged_pcd if not down_sample else merged_pcd.VoxelizedDownSample(voxel_size=0.005)

    # Retrieve color data if available
    colors = np.array(down_sampled_pcd.rgbs()) if down_sampled_pcd.has_rgbs() else np.ones((len(down_sampled_pcd.xyzs()), 3)) * [0.5, 0.5, 0.5]
    if not down_sampled_pcd.has_rgbs():
        print("Color data is not available in PointCloud.")

    return down_sampled_pcd, colors

相机设置代码 #

builder = DiagramBuilder()
plant: MultibodyPlant
plant, scene_graph = AddMultibodyPlantSceneGraph(
    builder, time_step=0.001)
parser = Parser(plant)
ConfigureParser(parser)
parser.AddModels("parcli/sdf/parcli/parcli.sdf") # parcli robot definition defined here
parser.AddModels("parcli/sensor_model/camera.dmd.yaml")  # sensor/camera definition defined here
ladder = Ladder(n_rungs = 20,spacing=0.25,z_offset=0.25)
ladder.add_to_world(plant)
plant.Finalize()

# creating Rgbd sensors 
AddRgbdSensors(builder, plant, scene_graph)
sensor = plant.GetModelInstanceByName("camera0")

解决方案 #

• 核心问题：使用了错误的上下文
  你的get_point_cloud函数每次调用时都会通过diagram.CreateDefaultContext()创建全新的默认上下文，这个上下文始终保持模型初始状态，和仿真运行的实际上下文完全无关。必须传入Simulator正在使用的根上下文，而非新建默认上下文。

• 修改后的get_point_cloud函数
  调整函数参数，接收仿真运行中的上下文：

  def get_point_cloud(diagram: Diagram, root_context, normals=True, down_sample=True):
      # 直接使用传入的仿真运行上下文，不再新建默认上下文
      plant = diagram.GetSubsystemByName("plant")
      plant_context = plant.GetMyContextFromRoot(root_context)
      diagram_context = diagram.GetMyContextFromRoot(root_context)
      
      pcd = []
      number_of_camera = list_camera_ports(diagram)
      min_val = 5
      max_val = 5
      for i in range(number_of_camera):
          output_port = diagram.GetOutputPort(f"camera{i}_point_cloud")
          cloud = output_port.Eval(diagram_context)
          pcd.append(cloud.Crop(lower_xyz=[-min_val, -min_val, -min_val], upper_xyz=[max_val, max_val, max_val]))
      
          if normals:
              pcd[i].EstimateNormals(radius=1, num_closest=50)
              camera = plant.GetModelInstanceByName(f"camera{i}")
              body = plant.GetBodyByName("base", camera)
              X_C = plant.EvalBodyPoseInWorld(plant_context, body)
              pcd[i].FlipNormalsTowardPoint(X_C.translation())
  
      merged_pcd = Concatenate(pcd)
      down_sampled_pcd = merged_pcd if not down_sample else merged_pcd.VoxelizedDownSample(voxel_size=0.005)
  
      colors = np.array(down_sampled_pcd.rgbs()) if down_sampled_pcd.has_rgbs() else np.ones((len(down_sampled_pcd.xyzs()), 3)) * [0.5, 0.5, 0.5]
      if not down_sampled_pcd.has_rgbs():
          print("Color data is not available in PointCloud.")
  
      return down_sampled_pcd, colors
  

• 调用方式调整
  在仿真循环中，传递Simulator的实际上下文给函数：

  # 假设已创建simulator实例
  simulator = Simulator(diagram)
  simulation_running = True
  
  while simulation_running:
      # 仿真步进
      simulator.AdvanceTo(simulator.get_context().get_time() + 0.01)
      # 获取实时点云
      pcd, colors = get_point_cloud(diagram, simulator.get_context())
      # 后续点云处理逻辑
  

• 额外注意事项

  • 若使用单独线程采集数据，需确保上下文访问线程安全：可调用simulator.get_context().Clone()创建上下文副本，避免并发访问冲突。
  • 确认ForcedPublish配置正确，确保传感器在仿真步进时更新输出。可在Diagram构建时添加ForcedPublish端口，或在仿真循环中手动调用diagram.ForcedPublish(root_context)。

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