如何在PyDrake中让Meshcat完整记录接触力与外力扰动的动态可视化？

要解决Meshcat录制仅显示最后时刻箭头的问题，核心是确保扰动箭头的更新和ContactVisualizer的接触力更新都在每个仿真时间步触发发布事件，让MeshcatRecorder能捕获每一步的可视化状态。以下是具体修改方案：

一、自定义扰动LeafSystem的发布配置 #

你的自定义系统需要在每个仿真步重新绘制扰动箭头，并触发发布事件：

1. 在系统构造函数中声明与仿真步长匹配的周期发布，确保每步都触发更新；
2. 在DoPublish方法中实时更新Meshcat箭头的状态，而不是仅初始化时设置一次。

示例代码：

from pydrake.systems.framework import LeafSystem

class ExternalDisturbance(LeafSystem):
    def __init__(self, plant, frame_id, meshcat, step_size=0.01):
        super().__init__()
        self.plant = plant
        self.frame_id = frame_id
        self.meshcat = meshcat
        # 声明周期发布，周期与仿真步长一致
        self.DeclarePeriodicPublish(period=step_size, offset=0.0)
        # 声明输入端口（比如扰动空间力）
        self.force_input = self.DeclareVectorInputPort("disturbance_force", size=6)

    def DoPublish(self, context, event):
        # 获取当前时刻的扰动空间力
        force_vec = self.EvalVectorInput(context, self.force_input.get_index()).get_value()
        # 获取目标帧的位姿
        frame_pose = self.plant.GetFramePoseInWorld(context, self.plant.get_frame(self.frame_id))
        # 计算箭头的起始/终点位置（根据空间力的方向和大小调整）
        start_pos = frame_pose.translation()
        end_pos = start_pos + force_vec[:3] * 0.1  # 缩放力的大小便于可视化
        # 更新Meshcat中的箭头
        self.meshcat.SetArrow(
            "/disturbance_arrow",
            start=start_pos,
            end=end_pos,
            radius=0.01,
            color=[1, 0, 0]  # 红色箭头标记扰动
        )

二、ContactVisualizer的发布配置 #

默认ContactVisualizer不会在每个仿真步都发布，需要显式开启实时更新：

from pydrake.geometry import ContactVisualizer

# 将ContactVisualizer添加到仿真构建器时，配置发布行为
contact_vis = ContactVisualizer.AddToBuilder(
    builder, plant, scene_graph, meshcat
)
# 让ContactVisualizer在每个仿真时间步都发布接触力可视化
contact_vis.set_publish_every_time_step(True)
# 或者设置与仿真步长一致的发布周期
# contact_vis.set_publish_period(0.01)

三、Simulator与MeshcatRecorder的关键设置 #

确保Simulator触发所有系统的发布事件，且Recorder捕获完整的更新序列：

from pydrake.systems.analysis import Simulator
from pydrake.systems.meshcat_visualizer import MeshcatRecorder

# 创建仿真器并配置强制每步发布
simulator = Simulator(plant)
simulator.set_publish_every_time_step(True)

# 启动Meshcat录制
meshcat.StartRecording()

# 运行仿真
simulator.AdvanceTo(5.0)  # 示例仿真时长5秒

# 停止录制并播放
meshcat.StopRecording()
meshcat.PublishRecording()

关键注意事项 #

• 必须在DoPublish中每次发布都重新绘制箭头，MeshcatRecorder仅捕获发布时刻的可视化状态，初始化时的单次设置只会被记录一次；
• 发布周期必须与仿真步长匹配，避免跳帧或漏帧；
• simulator.set_publish_every_time_step(True)是核心，它确保Simulator在每个时间步都触发所有系统的发布事件，即使系统声明了周期发布也需要这个设置兜底。

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