Skip to content

Commit

Permalink
[geometry] Add python bindings for Meshcat/MeshcatVisualizerCpp
Browse files Browse the repository at this point in the history
Follow-up to RobotLocomotion#13038.
  • Loading branch information
RussTedrake committed Aug 30, 2021
1 parent 705d6da commit 33d5879
Show file tree
Hide file tree
Showing 3 changed files with 141 additions and 0 deletions.
91 changes: 91 additions & 0 deletions bindings/pydrake/geometry_py.cc
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,8 @@
#include "drake/geometry/geometry_instance.h"
#include "drake/geometry/geometry_properties.h"
#include "drake/geometry/geometry_roles.h"
#include "drake/geometry/meshcat.h"
#include "drake/geometry/meshcat_visualizer.h"
#include "drake/geometry/optimization/hpolyhedron.h"
#include "drake/geometry/optimization/hyperellipsoid.h"
#include "drake/geometry/optimization/iris.h"
Expand Down Expand Up @@ -952,6 +954,49 @@ void DoScalarDependentDefinitions(py::module m, T) {
py::arg("lcm"), py::arg("params") = DrakeVisualizerParams{},
cls_doc.DispatchLoadMessage.doc);
}

// MeshcatVisualizer
{
using Class = MeshcatVisualizer<T>;
constexpr auto& cls_doc = doc.MeshcatVisualizer;
// Note that we are temporarily re-mapping MeshcatVisualizer =>
// MeshcatVisualizerCpp to avoid collisions with the python
// MeshcatVisualizer. See #13038.
auto cls = DefineTemplateClassWithDefault<Class, LeafSystem<T>>(
m, "MeshcatVisualizerCpp", param, cls_doc.doc);
cls // BR
.def(py::init<std::shared_ptr<Meshcat>, MeshcatVisualizerParams>(),
py::arg("meshcat"), py::arg("params") = MeshcatVisualizerParams{},
// Keep alive, reference: `self` keeps `meshcat` alive.
py::keep_alive<1, 2>(), // BR
cls_doc.ctor.doc)
.def("Delete", &Class::Delete, cls_doc.Delete.doc)
.def("query_object_input_port", &Class::query_object_input_port,
py_rvp::reference_internal, cls_doc.query_object_input_port.doc)
.def_static("AddToBuilder",
py::overload_cast<systems::DiagramBuilder<T>*, const SceneGraph<T>&,
std::shared_ptr<Meshcat>, MeshcatVisualizerParams>(
&MeshcatVisualizer<T>::AddToBuilder),
py::arg("builder"), py::arg("scene_graph"), py::arg("meshcat"),
py::arg("params") = MeshcatVisualizerParams{},
// Keep alive, ownership: `return` keeps `builder` alive.
py::keep_alive<0, 1>(),
// Keep alive, reference: `builder` keeps `meshcat` alive.
py::keep_alive<1, 3>(), py_rvp::reference,
cls_doc.AddToBuilder.doc_4args_builder_scene_graph_meshcat_params)
.def_static("AddToBuilder",
py::overload_cast<systems::DiagramBuilder<T>*,
const systems::OutputPort<T>&, std::shared_ptr<Meshcat>,
MeshcatVisualizerParams>(&MeshcatVisualizer<T>::AddToBuilder),
py::arg("builder"), py::arg("query_object_port"),
py::arg("meshcat"), py::arg("params") = MeshcatVisualizerParams{},
// Keep alive, ownership: `return` keeps `builder` alive.
py::keep_alive<0, 1>(),
// Keep alive, reference: `builder` keeps `meshcat` alive.
py::keep_alive<1, 3>(), py_rvp::reference,
cls_doc.AddToBuilder
.doc_4args_builder_query_object_port_meshcat_params);
}
} // NOLINT(readability/fn_size)

void DoScalarIndependentDefinitions(py::module m) {
Expand Down Expand Up @@ -1359,6 +1404,52 @@ void DoScalarIndependentDefinitions(py::module m) {
py::arg("filename"), py::arg("scale") = 1.0,
// N.B. We have not bound the optional "on_warning" argument.
doc.ReadObjToSurfaceMesh.doc_3args_filename_scale_on_warning);

// Meshcat
{
using Class = Meshcat;
constexpr auto& cls_doc = doc.Meshcat;
py::class_<Class, std::shared_ptr<Class>> cls(m, "Meshcat", cls_doc.doc);
cls // BR
.def(py::init<>(), cls_doc.ctor.doc)
.def("web_url", &Class::web_url, cls_doc.web_url.doc)
.def("ws_url", &Class::ws_url, cls_doc.ws_url.doc)
.def("SetObject", &Class::SetObject, py::arg("path"), py::arg("shape"),
py::arg("rgba") = Rgba(.9, .9, .9, 1.), cls_doc.SetObject.doc)
.def("SetTransform", &Class::SetTransform, py::arg("path"),
py::arg("X_ParentPath"), cls_doc.SetTransform.doc)
.def("Delete", &Class::Delete, py::arg("path") = "", cls_doc.Delete.doc)
.def("SetProperty",
py::overload_cast<std::string_view, std::string, double>(
&Class::SetProperty),
py::arg("path"), py::arg("property"), py::arg("value"),
cls_doc.SetProperty.doc_bool)
.def("SetProperty",
py::overload_cast<std::string_view, std::string, double>(
&Class::SetProperty),
py::arg("path"), py::arg("property"), py::arg("value"),
cls_doc.SetProperty.doc_double);
}

// MeshcatVisualizerParams
{
using Class = MeshcatVisualizerParams;
constexpr auto& cls_doc = doc.MeshcatVisualizerParams;
py::class_<Class>(
m, "MeshcatVisualizerParams", py::dynamic_attr(), cls_doc.doc)
.def(ParamInit<Class>())
.def_readwrite("publish_period",
&MeshcatVisualizerParams::publish_period,
cls_doc.publish_period.doc)
.def_readwrite("role", &MeshcatVisualizerParams::role, cls_doc.role.doc)
.def_readwrite("default_color", &MeshcatVisualizerParams::default_color,
cls_doc.default_color.doc)
.def_readwrite(
"prefix", &MeshcatVisualizerParams::prefix, cls_doc.prefix.doc)
.def_readwrite("delete_prefix_on_initialization_event",
&MeshcatVisualizerParams::delete_prefix_on_initialization_event,
cls_doc.delete_prefix_on_initialization_event.doc);
}
}

void def_geometry(py::module m) {
Expand Down
46 changes: 46 additions & 0 deletions bindings/pydrake/test/geometry_test.py
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@
import numpy as np

from drake import lcmt_viewer_load_robot, lcmt_viewer_draw
from pydrake.autodiffutils import AutoDiffXd
from pydrake.common import FindResourceOrThrow
from pydrake.common.test_utilities import numpy_compare
from pydrake.common.test_utilities.deprecation import catch_drake_warnings
Expand Down Expand Up @@ -364,6 +365,51 @@ def auto_connect_to_port(builder, scene_graph, params):
load_subscriber.clear()
draw_subscriber.clear()

def test_meshcat(self):
meshcat = mut.Meshcat()
self.assertIn("http", meshcat.web_url())
self.assertIn("ws", meshcat.ws_url())
meshcat.SetObject(path="/test/box",
shape=mut.Box(1, 1, 1),
rgba=mut.Rgba(.5, .5, .5))
meshcat.SetTransform(path="/test/box", X_ParentPath=RigidTransform())
meshcat.SetProperty(path="/Background",
property="visible",
value=True)
meshcat.SetProperty(path="/Lights/DirectionalLight/<object>",
property="intensity", value=1.0)

# Confirm that I can scalar-convert the visualizer.
vis = mut.MeshcatVisualizerCpp(meshcat)
vis_autodiff = vis.ToAutoDiffXd()
self.assertIsInstance(vis_autodiff,
mut.MeshcatVisualizerCpp_[AutoDiffXd])

@numpy_compare.check_nonsymbolic_types
def test_meshcat_visualizer(self, T):
meshcat = mut.Meshcat()
params = mut.MeshcatVisualizerParams()
params.publish_period = 1.0/10.0
params.role = mut.Role.kIllustration
params.default_color = mut.Rgba(0.5, 0.5, 0.5)
params.prefix = "py_visualizer"
params.delete_prefix_on_initialization_event = False
vis = mut.MeshcatVisualizerCpp_[T](meshcat=meshcat, params=params)
vis.Delete()
self.assertIsInstance(vis.query_object_input_port(), InputPort_[T])

builder = DiagramBuilder_[T]()
scene_graph = builder.AddSystem(mut.SceneGraph_[T]())
mut.MeshcatVisualizerCpp_[T].AddToBuilder(builder=builder,
scene_graph=scene_graph,
meshcat=meshcat,
params=params)
mut.MeshcatVisualizerCpp_[T].AddToBuilder(
builder=builder,
query_object_port=scene_graph.get_query_output_port(),
meshcat=meshcat,
params=params)

@numpy_compare.check_nonsymbolic_types
def test_frame_pose_vector_api(self, T):
FramePoseVector = mut.FramePoseVector_[T]
Expand Down
4 changes: 4 additions & 0 deletions geometry/meshcat.h
Original file line number Diff line number Diff line change
Expand Up @@ -131,6 +131,8 @@ class Meshcat {
See @ref meshcat_path for the semantics.
@param property the string name of the property to set
@param value the new value.
@pydrake_mkdoc_identifier{bool}
*/
void SetProperty(std::string_view path, std::string property, bool value);

Expand All @@ -146,6 +148,8 @@ class Meshcat {
See @ref meshcat_path for the semantics.
@param property the string name of the property to set
@param value the new value.
@pydrake_mkdoc_identifier{double}
*/
void SetProperty(std::string_view path, std::string property, double value);

Expand Down

0 comments on commit 33d5879

Please sign in to comment.