Drake中Meshcat无法正确可视化初始状态的问题排查

问题详情 #

模拟初始位置为0、初始速度非零的滑块地面滑动场景时，Meshcat可视化显示的滑块初始位置并非原点。通过日志验证：

• 滑块状态输出、SceneGraph接收的位姿均显示初始位置为0
• Meshcat中初始位置与初始速度成正比，仅当初始速度为0时显示正确

推测可能是仿真先执行若干积分步后Meshcat才开始录制，但仅为猜测。需明确两个问题：

1. 如何修改代码让Meshcat正确显示t=0时的滑块位置？
2. 这是否是Meshcat的问题？

最小可复现代码 #

import numpy as np

from pydrake.all import (
    DiagramBuilder,
    BasicVector,
    LeafSystem,
    LogVectorOutput,
    MeshcatVisualizer,
    RigidTransform,
    RotationMatrix,
    Box,
    GeometryFrame,
    FramePoseVector,
    GeometryInstance,
    IllustrationProperties,
    AbstractValue,
    SceneGraph,
    Simulator,
    StartMeshcat,
)

# simple sliding block with friction
class Block(LeafSystem):
    def __init__(self):
        LeafSystem.__init__(self)
        state_index = self.DeclareContinuousState(1,1,0)
        self.DeclareStateOutputPort("x", state_index)
        
    def DoCalcTimeDerivatives(self, context, derivatives):
        x, x_dot = context.get_continuous_state_vector().CopyToVector()

        # debug: check x position at t=0
        if context.get_time() == 0:
            print(f'DEBUG (Block): x={x}, x_dot={x_dot}')

        derivatives.get_mutable_vector().SetFromVector(
            np.array([x_dot, -2*x_dot])
        )

# tell the scene_graph where to put the block
class BlockVisualizer(LeafSystem):
    def __init__(self,frame_id):
        LeafSystem.__init__(self)
        self.frame_id = frame_id
        self.x_port = self.DeclareVectorInputPort("x", BasicVector(2))
        self.DeclareAbstractOutputPort("my_pose", lambda: AbstractValue.Make(FramePoseVector()), self.CalcFramePoseOutput)

    def CalcFramePoseOutput(self, context, output):
        x, x_dot = self.x_port.Eval(context)

        # debug: check x position at t=0
        if context.get_time() == 0:
            print(f'DEBUG (BlockVisualizer): x={x}, x_dot={x_dot}')

        output.get_mutable_value().set_value(self.frame_id, RigidTransform(
            RotationMatrix().Identity(),
            np.array([x,0,0])
        ))

def main():
    builder = DiagramBuilder()
    plant = builder.AddSystem(Block())
    scene_graph = builder.AddSystem(SceneGraph())

    # Register the geometry with the scene graph.
    source_id = scene_graph.RegisterSource("my_block_source")
    frame_id = scene_graph.RegisterFrame(source_id, GeometryFrame("my_frame", 0))
    geometry_id = scene_graph.RegisterGeometry(source_id, frame_id, 
        GeometryInstance(RigidTransform(), Box(0.1, 0.1, 0.1), "my_geometry_instance"))
    scene_graph.AssignRole(source_id, geometry_id, IllustrationProperties())
    
    meshcat = StartMeshcat()
    MeshcatVisualizer.AddToBuilder(builder, scene_graph, meshcat)
    viz = builder.AddSystem(BlockVisualizer(frame_id))

    builder.Connect(plant.get_output_port(), viz.get_input_port())
    builder.Connect(viz.get_output_port(), scene_graph.get_source_pose_port(source_id))

    logger = LogVectorOutput(plant.get_output_port(), builder)
    
    diagram = builder.Build()
    simulator = Simulator(diagram)
    context = simulator.get_mutable_context()

    # set the block initial conditions. x = 0.0, x_dot = 5.0
    plant_context = diagram.GetMutableSubsystemContext(plant, context)
    plant_context.get_mutable_continuous_state_vector().SetFromVector([0.0, 5.0])

    # Run the simulation.
    meshcat.StartRecording()
    simulator.Initialize()
    simulator.AdvanceTo(3.0)
    meshcat.PublishRecording()
    print('Simulation complete!')

    # debug: check x position at t=0
    log = logger.FindLog(context)
    data = log.data()
    t = log.sample_times()
    print(f'DEBUG (main): Initial Condition at t={t[0]}: x={data[0,0]}, x_dot={data[1,0]}')
    
    while True:
        pass

if __name__ == "__main__":
    main()

代码输出 #

INFO:drake:Meshcat listening for connections at http://localhost:7000
DEBUG (BlockVisualizer): x=0.0, x_dot=5.0
DEBUG (Block): x=0.0, x_dot=5.0
Simulation complete!
DEBUG (main): Initial Condition at t=0.0: x=0.0, x_dot=5.0

临时解决方案 #

设置meshcat.StartRecording(frames_per_second=100)提高可视化更新频率（默认更新频率为32帧/秒），可缓解该问题。

补充说明 #

附初始速度x_dot=5.0时Meshcat中滑块的异常显示截图，以及初始速度x_dot=0.0时的正确显示截图。

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