Meshcat Not Visualizing Initial Conditions Correctly [drake]

Question

I am trying to simulate a simple block sliding across the ground with zero initial position and non-zero initial velocity. When I visualize this system in Meshcat, the block's initial position is non-zero.

I verified that the block's position is initially zero by connecting a logger to the state output of my block. I also verified the pose received by the SceneGraph has a position offset of zero.

Despite this, when visualizing the system in Meshcat, the block is not at the origin at time zero. Further, the block's initial position in Meshcat is proportional to the initial velocity, and when the initial velocity is zero, the block is in the proper position. I think that my system is being integrated a few time steps before Meshcat actually starts recording, but this is just a hunch.

How can I modify this code so that Meshcat correctly visualizes the block's position at $t=0$? Or, is this an issue with Meshcat?

Below is a minimal working example.

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()

The output of this code is:

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

Below is a screenshot of the block with initial velocity of x_dot=5.0:

Below is a screenshot of the expected behavior (I setx_dot=0.0 to produce this image):

Edit: as a workaround, I increased how often Meshcat visualization updates by setting the frames_per_second parameter. Setting this value large enough will negate the undesired effect.

meshcat.StartRecording(frames_per_second=100)

By default, the Meshcat visualization updates every 32 frames. Source.

Answer 1

Thanks for the reproduction.

Long story short, your first integration step is happening much faster than your first recording frame, so it is getting rounded down to time zero in the animation. If you change your start recording line to

    meshcat.StartRecording(frames_per_second=100.0)

then you should see that the block starts at zero.

Here is some debugging I did to figure that out:

I took a look, and was able to confirm that at least by the time it gets to meshcat's javascript animation clip (which I inspected by going into the javascript console on the browser), it has a non-zero position in the animation at time (exactly) zero.

Adding

    log()->info("t = {}, x = {}", ExtractDoubleOrThrow(context.get_time()),
                X_WF.translation()[0]);

to MeshcatVisualizer::SetTransforms results in

INFO:drake:Meshcat listening for connections at http://localhost:7001
DEBUG (BlockVisualizer): x=0.0, x_dot=5.0
INFO:drake:t = 0, x = 0
DEBUG (Block): x=0.0, x_dot=5.0
INFO:drake:t = 0.015625, x = 0.07691695410783563
INFO:drake:t = 0.03125, x = 0.15146747771115646

but by the time it gets packed in Meshcat::SetAnimation, I see

INFO:drake:time: 0, value: 0.07691695410783563
INFO:drake:time: 1, value: 0.2237243198911553
INFO:drake:time: 2, value: 0.361637065787313

This is definitely surprising behavior in this situation... I'll have to think about if we could make it better. (Suggestions welcome!)