Skip to content

Commit

Permalink
Add meshcat animation (and binder tutorial)
Browse files Browse the repository at this point in the history
Relates to RobotLocomotion#12645.  This is our current best solution given the discussion with @rdeits.
  • Loading branch information
RussTedrake committed Jun 7, 2020
1 parent 9acf636 commit 961b902
Show file tree
Hide file tree
Showing 3 changed files with 98 additions and 1 deletion.
38 changes: 38 additions & 0 deletions bindings/pydrake/systems/meshcat_visualizer.py
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,7 @@
import meshcat
import meshcat.geometry as g # noqa
import meshcat.transformations as tf # noqa
from meshcat.animation import Animation

_DEFAULT_PUBLISH_PERIOD = 1 / 30.

Expand Down Expand Up @@ -249,6 +250,8 @@ def __init__(self,
self.set_name('meshcat_visualizer')
self.DeclarePeriodicPublish(draw_period, 0.0)
self.draw_period = draw_period
self.reset_recording()
self._is_recording = False

# Pose bundle (from SceneGraph) input port.
# TODO(tehbelinda): Rename the `lcm_visualization` port to match
Expand Down Expand Up @@ -381,6 +384,41 @@ def DoPublish(self, context, event):
pose_matrix = pose_bundle.get_transform(frame_i)
self.vis[self.prefix][source_name][str(model_id)][frame_name]\
.set_transform(pose_matrix.GetAsMatrix4())
if self._is_recording:
with self._animation.at_frame(
self.vis[self.prefix][source_name][str(model_id)]
[frame_name], self._recording_frame_num) as frame:
frame.set_transform(pose_matrix.GetAsMatrix4())
self._recording_frame_num += 1

def start_recording(self):
"""
Sets a flag to record future publish events as animation frames.
Note: This syntax was chosen to match PyPlotVisualizer.
"""
self._is_recording = True

def stop_recording(self):
"""
Sets a flag to record future publish events as animation frames.
Note: This syntax was chosen to match PyPlotVisualizer.
"""
self._is_recording = False

def publish_recording(self):
"""
Publish any recorded animation to Meshcat. Use the controls dialog
in the browser to review it.
"""
self.vis.set_animation(self._animation)

def reset_recording(self):
"""
Resets all recorded data.
Note: This syntax was chosen to match PyPlotVisualizer.
"""
self._animation = Animation(default_framerate=1./self.draw_period)
self._recording_frame_num = 0


class MeshcatContactVisualizer(LeafSystem):
Expand Down
7 changes: 7 additions & 0 deletions bindings/pydrake/systems/test/meshcat_visualizer_test.py
Original file line number Diff line number Diff line change
Expand Up @@ -83,7 +83,14 @@ def test_cart_pole(self):

simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
visualizer.start_recording()
simulator.AdvanceTo(.1)
visualizer.stop_recording()
# Should have animation "clips" for both bodies.
self.assertEqual(len(visualizer._animation.clips), 2)
visualizer.publish_recording()
visualizer.reset_recording()
self.assertEqual(len(visualizer._animation.clips), 0)

def test_kuka(self):
"""Kuka IIWA with mesh geometry."""
Expand Down
54 changes: 53 additions & 1 deletion tutorials/rendering_multibody_plant.ipynb
Original file line number Diff line number Diff line change
Expand Up @@ -333,7 +333,8 @@
"metadata": {},
"outputs": [],
"source": [
"Simulator(diagram).Initialize()"
"simulator = Simulator(diagram)\n",
"simulator.Initialize()"
]
},
{
Expand Down Expand Up @@ -400,6 +401,57 @@
"\n",
"plt.imshow(colorize_labels(label_by_model))"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## For remote workflows: Render meshcat's current state\n",
"\n",
"Although we do not yet support interactive meshcat visualization, you *can* produce a window that gives an interactive meshcat interface (as in, you can still access all the controls, move the camera, and replay or record the animation), but any future calls from the MeshcatVisualizer will not affect what's displayed there. This *should* work on Binder. Please see [#12645](https://github.com/RobotLocomotion/drake/issues/12645) for more details."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"meshcat_vis.vis.render_static()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"This is particularly useful if you would like to play back recorded simulation. Open the controls in the meshcat window to play the animation."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# Set the context to make the simulation valid and slightly interesting.\n",
"plant_context = plant.GetMyContextFromRoot(simulator.get_mutable_context())\n",
"plant.get_actuation_input_port(iiwa_1).FixValue(plant_context, np.zeros((7,1)))\n",
"plant.get_actuation_input_port(iiwa_2).FixValue(plant_context, np.zeros((7,1)))\n",
"plant.SetPositions(plant_context, iiwa_1, [0.2, 0.4, 0, 0, 0, 0, 0])\n",
"\n",
"# Reset the recording (in case you are running this cell more than once).\n",
"meshcat_vis.reset_recording()\n",
"\n",
"# Start recording and simulate.\n",
"meshcat_vis.start_recording()\n",
"simulator.AdvanceTo(simulator.get_context().get_time() + 1.0)\n",
"\n",
"# Publish the recording to meshcat.\n",
"meshcat_vis.publish_recording()\n",
"\n",
"# Render meshcat's current state.\n",
"meshcat_vis.vis.render_static()"
]
}
],
"metadata": {
Expand Down

0 comments on commit 961b902

Please sign in to comment.