diff --git a/geometry/BUILD.bazel b/geometry/BUILD.bazel index ffd6ec4e0a20..ba27b77e73c7 100644 --- a/geometry/BUILD.bazel +++ b/geometry/BUILD.bazel @@ -37,6 +37,8 @@ drake_cc_package_library( ":internal_frame", ":internal_geometry", ":meshcat", + ":meshcat_visualizer", + ":meshcat_visualizer_params", ":proximity_engine", ":proximity_properties", ":rgba", @@ -387,9 +389,18 @@ drake_cc_binary( name = "meshcat_manual_test", testonly = True, srcs = ["test/meshcat_manual_test.cc"], - data = ["//systems/sensors:test_models"], + data = [ + "//manipulation/models/iiwa_description:models", + "//systems/sensors:test_models", + ], visibility = ["//visibility:private"], - deps = [":meshcat"], + deps = [ + ":meshcat", + ":meshcat_visualizer", + "//multibody/parsing", + "//multibody/plant", + "//systems/analysis:simulator", + ], ) drake_py_binary( @@ -414,6 +425,48 @@ drake_cc_googletest( ], ) +drake_cc_library( + name = "meshcat_visualizer_params", + hdrs = ["meshcat_visualizer_params.h"], + deps = [ + ":geometry_roles", + ":rgba", + ], +) + +drake_cc_library( + name = "meshcat_visualizer", + srcs = ["meshcat_visualizer.cc"], + hdrs = ["meshcat_visualizer.h"], + deps = [ + ":geometry_roles", + ":meshcat", + ":meshcat_visualizer_params", + ":rgba", + ":scene_graph", + "//common:essential", + "//math:geometric_transform", + "//systems/framework:context", + "//systems/framework:diagram_builder", + "//systems/framework:leaf_system", + ], +) + +drake_cc_googletest( + name = "meshcat_visualizer_test", + data = [ + "//manipulation/models/iiwa_description:models", + ], + deps = [ + ":meshcat_visualizer", + "//common/test_utilities:expect_throws_message", + "//multibody/parsing", + "//multibody/plant", + "//systems/analysis:simulator", + "//systems/primitives:constant_vector_source", + ], +) + # ----------------------------------------------------- filegroup( diff --git a/geometry/meshcat.cc b/geometry/meshcat.cc index 4fa1a0a776ad..2644aa861308 100644 --- a/geometry/meshcat.cc +++ b/geometry/meshcat.cc @@ -1,9 +1,11 @@ #include "drake/geometry/meshcat.h" +#include #include #include #include #include +#include #include #include #include @@ -67,6 +69,12 @@ struct PerSocketData { using WebSocket = uWS::WebSocket; using MsgPackMap = std::map; +// The maximum "backpressure" allowed each websocket, in bytes. This +// effectively sets a maximum size for messages, which may include mesh files +// and texture maps. 50mb is a guess at a compromise that is safely larger than +// reasonable meshfiles. +constexpr static double kMaxBackPressure{50 * 1024 * 1024}; + class SceneTreeElement { public: // Member access methods (object_, transform_, and properties_ should be @@ -186,7 +194,8 @@ class MeshcatShapeReifier : public ShapeReifier { public: DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(MeshcatShapeReifier); - explicit MeshcatShapeReifier(std::string uuid) : uuid_(std::move(uuid)) {} + explicit MeshcatShapeReifier(std::string uuid) + : uuid_(std::move(uuid)) {} ~MeshcatShapeReifier() = default; @@ -224,6 +233,8 @@ class MeshcatShapeReifier : public ShapeReifier { } void ImplementGeometry(const HalfSpace&, void*) override { + // TODO(russt): Use PlaneGeometry with fields width, height, + // widthSegments, heightSegments drake::log()->warn("Meshcat does not display HalfSpace geometry (yet)."); } @@ -291,6 +302,14 @@ class MeshcatShapeReifier : public ShapeReifier { // We simply dump the binary contents of the file into the data field of the // message. The javascript meshcat takes care of the rest. int size = input.tellg(); + if (size > kMaxBackPressure) { + throw std::runtime_error(fmt::format( + "The meshfile at {} is too large for the current websocket setup. " + "Size {} is greater than the max backpressure {}. You will either " + "need to reduce the size of your mesh, or modify the code to " + "increase the allowance.", + mesh.filename(), size, kMaxBackPressure)); + } input.seekg(0, std::ios::beg); geometry.data.resize(size); input.read(geometry.data.data(), size); @@ -336,8 +355,12 @@ class Meshcat::WebSocketPublisher { ~WebSocketPublisher() { DRAKE_DEMAND(std::this_thread::get_id() == main_thread_id_); - loop_->defer( - [socket = listen_socket_]() { us_listen_socket_close(0, socket); }); + loop_->defer([this]() { + us_listen_socket_close(0, listen_socket_); + for (auto* ws : websockets_) { + ws->close(); + } + }); websocket_thread_.join(); } @@ -551,6 +574,15 @@ class Meshcat::WebSocketPublisher { ws->subscribe("all"); // Update this new connection with previously published data. SendTree(ws); + websockets_.emplace(ws); + }; + // TODO(russt): I could increase this more if necessary (when it was too + // low, some SetObject messages were dropped). But at some point the real + // fix is to actually throttle the sending (e.g. by slowing down the main + // thread). + behavior.maxBackpressure = kMaxBackPressure; + behavior.close = [this](WebSocket* ws, int, std::string_view) { + websockets_.erase(ws); }; uWS::App app = @@ -620,6 +652,7 @@ class Meshcat::WebSocketPublisher { // they are pointing to should be only used in the websocket thread. uWS::App* app_{nullptr}; us_listen_socket_t* listen_socket_{nullptr}; + std::set websockets_{}; // This pointer should only be accessed in the main thread, but the Loop // object itself should be only used in the websocket thread, with one diff --git a/geometry/meshcat.h b/geometry/meshcat.h index 0e09cd9b5c57..86d31ab86c52 100644 --- a/geometry/meshcat.h +++ b/geometry/meshcat.h @@ -72,8 +72,6 @@ in the visualizer. - All user objects can easily be cleared by a single, parameter-free call to Delete(). You are welcome to use absolute paths to organize your data, but the burden on tracking and cleaning them up lie on you. - - */ class Meshcat { public: @@ -97,7 +95,7 @@ class Meshcat { @ref meshcat_path. Any objects previously set at this `path` will be replaced. @param path a "/"-delimited string indicating the path in the scene tree. - See @ref meshcat_path for the semantics. + See @ref meshcat_path "Meshcat paths" for the semantics. @param shape a Shape that specifies the geometry of the object. @param rgba an Rgba that specifies the (solid) color of the object. */ @@ -111,7 +109,7 @@ class Meshcat { the transform of "/foo" will move the objects at "/foo/box1" and "/foo/robots/HAL9000". @param path a "/"-delimited string indicating the path in the scene tree. - See @ref meshcat_path for the semantics. + See @ref meshcat_path "Meshcat paths" for the semantics. @param X_ParentPath the relative transform from the path to its immediate parent. */ @@ -126,8 +124,8 @@ class Meshcat { @verbatim meshcat.SetProperty("/Background", "visible", false); @endverbatim - will turn off the background. See @ref meshcat_path for more details about - these properties and how to address them. + will turn off the background. See @ref meshcat_path "Meshcat paths" for more + details about these properties and how to address them. @param path a "/"-delimited string indicating the path in the scene tree. See @ref meshcat_path for the semantics. @@ -141,8 +139,8 @@ class Meshcat { meshcat.SetProperty("/Cameras/default/rotated/", "zoom", 2.0); meshcat.SetProperty("/Lights/DirectionalLight/", "intensity", 1.0); @endverbatim - See @ref meshcat_path for more details about these properties and how to - address them. + See @ref meshcat_path "Meshcat paths" for more details about these properties + and how to address them. @param path a "/"-delimited string indicating the path in the scene tree. See @ref meshcat_path for the semantics. diff --git a/geometry/meshcat_visualizer.cc b/geometry/meshcat_visualizer.cc new file mode 100644 index 000000000000..0815b6d380a8 --- /dev/null +++ b/geometry/meshcat_visualizer.cc @@ -0,0 +1,170 @@ +#include "drake/geometry/meshcat_visualizer.h" + +#include +#include +#include + +#include + +#include "drake/geometry/utilities.h" + +namespace drake { +namespace geometry { + +template +MeshcatVisualizer::MeshcatVisualizer(std::shared_ptr meshcat, + MeshcatVisualizerParams params) + : systems::LeafSystem(systems::SystemTypeTag{}), + meshcat_(std::move(meshcat)), + params_(std::move(params)) { + DRAKE_DEMAND(meshcat_ != nullptr); + DRAKE_DEMAND(params_.publish_period >= 0.0); + if (params_.role == Role::kUnassigned) { + throw std::runtime_error( + "MeshcatVisualizer cannot be used for geometries with the " + "Role::kUnassigned value. Please choose kProximity, kPerception, or " + "kIllustration"); + } + + this->DeclarePeriodicPublishEvent(params_.publish_period, 0.0, + &MeshcatVisualizer::UpdateMeshcat); + this->DeclareForcedPublishEvent(&MeshcatVisualizer::UpdateMeshcat); + + if (params_.delete_prefix_on_initialization_event) { + this->DeclareInitializationPublishEvent( + &MeshcatVisualizer::OnInitialization); + } + + query_object_input_port_ = + this->DeclareAbstractInputPort("query_object", Value>()) + .get_index(); +} + +template +template +MeshcatVisualizer::MeshcatVisualizer(const MeshcatVisualizer& other) + : MeshcatVisualizer(other.meshcat_, other.params_) {} + +template +void MeshcatVisualizer::Delete() const { + meshcat_->Delete(params_.prefix); + version_ = GeometryVersion(); +} + +template +const MeshcatVisualizer& MeshcatVisualizer::AddToBuilder( + systems::DiagramBuilder* builder, const SceneGraph& scene_graph, + std::shared_ptr meshcat, MeshcatVisualizerParams params) { + return AddToBuilder(builder, scene_graph.get_query_output_port(), + std::move(meshcat), std::move(params)); +} + +template +const MeshcatVisualizer& MeshcatVisualizer::AddToBuilder( + systems::DiagramBuilder* builder, + const systems::OutputPort& query_object_port, + std::shared_ptr meshcat, MeshcatVisualizerParams params) { + auto& visualizer = *builder->template AddSystem>( + std::move(meshcat), std::move(params)); + builder->Connect(query_object_port, visualizer.query_object_input_port()); + return visualizer; +} + +template +systems::EventStatus MeshcatVisualizer::UpdateMeshcat( + const systems::Context& context) const { + const auto& query_object = + query_object_input_port().template Eval>(context); + const GeometryVersion& current_version = + query_object.inspector().geometry_version(); + + if (!version_.IsSameAs(current_version, params_.role)) { + SetObjects(query_object.inspector()); + version_ = current_version; + } + SetTransforms(query_object); + + return systems::EventStatus::Succeeded(); +} + +template +void MeshcatVisualizer::SetObjects( + const SceneGraphInspector& inspector) const { + // Frames registered previously that are not set again here should be deleted. + std::map frames_to_delete{}; + dynamic_frames_.swap(frames_to_delete); + + // Geometries registered previously that are not set again here should be + // deleted. + std::map geometries_to_delete{}; + geometries_.swap(geometries_to_delete); + + // TODO(SeanCurtis-TRI): Mimic the full tree structure in SceneGraph. + // SceneGraph supports arbitrary hierarchies of frames just like Meshcat. + // This code is arbitrarily flattening it because the current SceneGraph API + // is insufficient to support walking the tree. + for (FrameId frame_id : inspector.GetAllFrameIds()) { + std::string frame_path = + frame_id == inspector.world_frame_id() + ? params_.prefix + : fmt::format("{}/{}", params_.prefix, inspector.GetName(frame_id)); + // MultibodyPlant declares frames with SceneGraph using "::". We replace + // those with `/` here to expose the full tree to Meshcat. + size_t pos = 0; + while ((pos = frame_path.find("::", pos)) != std::string::npos) { + frame_path.replace(pos++, 2, "/"); + } + if (frame_id != inspector.world_frame_id() && + inspector.NumGeometriesForFrameWithRole(frame_id, params_.role) > 0) { + dynamic_frames_[frame_id] = frame_path; + frames_to_delete.erase(frame_id); // Don't delete this one. + } + + for (GeometryId geom_id : inspector.GetGeometries(frame_id, params_.role)) { + // Note: We use the frame_path/id instead of instance.GetName(geom_id), + // which is a garbled mess of :: and _ and a memory address by default + // when coming from MultibodyPlant. + // TODO(russt): Use the geometry names if/when they are cleaned up. + const std::string path = + fmt::format("{}/{}", frame_path, geom_id.get_value()); + const Rgba rgba = inspector.GetProperties(geom_id, params_.role) + ->GetPropertyOrDefault("phong", "diffuse", params_.default_color); + + meshcat_->SetObject(path, inspector.GetShape(geom_id), rgba); + meshcat_->SetTransform(path, inspector.GetPoseInFrame(geom_id)); + geometries_[geom_id] = path; + geometries_to_delete.erase(geom_id); // Don't delete this one. + } + } + + for (const auto& [geom_id, path] : geometries_to_delete) { + unused(geom_id); + meshcat_->Delete(path); + } + for (const auto& [frame_id, path] : frames_to_delete) { + unused(frame_id); + meshcat_->Delete(path); + } +} + +template +void MeshcatVisualizer::SetTransforms( + const QueryObject& query_object) const { + for (const auto& [frame_id, path] : dynamic_frames_) { + meshcat_->SetTransform(path, internal::convert_to_double( + query_object.GetPoseInWorld(frame_id))); + } +} + +template +systems::EventStatus MeshcatVisualizer::OnInitialization( + const systems::Context&) const { + Delete(); + return systems::EventStatus::Succeeded(); +} + +} // namespace geometry +} // namespace drake + +DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS( + class ::drake::geometry::MeshcatVisualizer) diff --git a/geometry/meshcat_visualizer.h b/geometry/meshcat_visualizer.h new file mode 100644 index 000000000000..d2d25d65b10d --- /dev/null +++ b/geometry/meshcat_visualizer.h @@ -0,0 +1,169 @@ +#pragma once + +#include +#include +#include + +#include "drake/geometry/geometry_roles.h" +#include "drake/geometry/meshcat.h" +#include "drake/geometry/meshcat_visualizer_params.h" +#include "drake/geometry/rgba.h" +#include "drake/geometry/scene_graph.h" +#include "drake/systems/framework/diagram_builder.h" +#include "drake/systems/framework/leaf_system.h" + +namespace drake { +namespace geometry { + +/** A system wrapper for Meshcat that publishes the current state of a +SceneGraph instance (whose QueryObject-valued output port is connected to this +system's input port). While this system will add geometry to Meshcat, the +Meshcat instance is also available for users to add their own visualization +alongside the MeshcatVisualizer visualizations. This can be enormously valuable +for impromptu visualizations. + + @system + name: MeshcatVisualizer + input_ports: + - query_object + @endsystem + +The system uses the versioning mechanism provided by SceneGraph to detect +changes to the geometry so that a change in SceneGraph's data will propagate +to Meshcat. + +By default, %MeshcatVisualizer visualizes geometries with the illustration role +(see @ref geometry_roles for more details). It can be configured to visualize +geometries with other roles. Only one role can be specified. See +DrakeVisualizer which uses the same mechanisms for more details. + +Instances of %MeshcatVisualizer created by scalar-conversion will publish to the +same Meshcat instance. +@tparam_nonsymbolic_scalar +*/ +template +class MeshcatVisualizer final : public systems::LeafSystem { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(MeshcatVisualizer) + + /** Creates an instance of %MeshcatVisualizer. + + @param meshcat A Meshcat instance. This class will assume shared ownership + for the lifetime of the object. + @param params The set of parameters to control this system's behavior. + @throws std::exception if `params.publish_period <= 0`. + @throws std::exception if `params.role == Role::kUnassigned`. */ + explicit MeshcatVisualizer(std::shared_ptr meshcat, + MeshcatVisualizerParams params = {}); + + /** Scalar-converting copy constructor. See @ref system_scalar_conversion. + It should only be used to convert _from_ double _to_ other scalar types. + */ + template + explicit MeshcatVisualizer(const MeshcatVisualizer& other); + + /** Calls Meschat::Delete(std::string path), with the path set to + MeshcatVisualizerParams::prefix. Since this visualizer will only ever add + geometry under this prefix, this will remove all geometry/transforms added + by the visualizer, or by a previous instance of this visualizer using the + same prefix. Use MeshcatVisualizer::delete_prefix_on_initialization_event + to determine whether this should be called on initialization. */ + void Delete() const; + + /** Returns the QueryObject-valued input port. It should be connected to + SceneGraph's QueryObject-valued output port. Failure to do so will cause a + runtime error when attempting to broadcast messages. */ + const systems::InputPort& query_object_input_port() const { + return this->get_input_port(query_object_input_port_); + } + + /** Adds a MescatVisualizer and connects it to the given SceneGraph's + QueryObject-valued output port. See + MeshcatVisualizer::MeshcatVisualizer(MeshcatVisualizer*, + MeshcatVisualizerParams) for details. */ + static const MeshcatVisualizer& AddToBuilder( + systems::DiagramBuilder* builder, const SceneGraph& scene_graph, + std::shared_ptr meshcat, + MeshcatVisualizerParams params = {}); + + /** Adds a MescatVisualizer and connects it to the given QueryObject-valued + output port. See MeshcatVisualizer::MeshcatVisualizer(MeshcatVisualizer*, + MeshcatVisualizerParams) for details. */ + static const MeshcatVisualizer& AddToBuilder( + systems::DiagramBuilder* builder, + const systems::OutputPort& query_object_port, + std::shared_ptr meshcat, + MeshcatVisualizerParams params = {}); + //@} + + private: + /* MeshcatVisualizer of different scalar types can all access each other's + data. */ + template + friend class MeshcatVisualizer; + + /* The periodic event handler. It tests to see if the last scene description + is valid (if not, sends the objects) and then sends the transforms. */ + systems::EventStatus UpdateMeshcat(const systems::Context& context) const; + + /* Makes calls to Meshcat::SetObject to register geometry in SceneGraph. */ + void SetObjects(const SceneGraphInspector& inspector) const; + + /* Makes calls to Meshcat::SetTransform to update the poses from SceneGraph. + */ + void SetTransforms(const QueryObject& query_object) const; + + /* Handles the initialization event. */ + systems::EventStatus OnInitialization(const systems::Context&) const; + + /* The index of this System's QueryObject-valued input port. */ + int query_object_input_port_{}; + + /* Meshcat is mutable because we must send messages (a non-const operation) + from a const System (e.g. during simulation). We use shared_ptr instead of + unique_ptr to facilitate sharing ownership through scalar conversion; + creating a new Meshcat object during the conversion is not a viable option. + */ + mutable std::shared_ptr meshcat_{}; + + /* The version of the geometry that was last set in Meshcat by this + instance. Because the underlying Meshcat is shared, this visualizer has no + guarantees that the Meshcat state correlates with this value. If the version + found on the input port differs from this value, SetObjects is called again + before SetTransforms. This is intended to track the information in meshcat_, + and is therefore also a mutable member variable (instead of declared state). + */ + mutable GeometryVersion version_; + + /* A store of the dynamic frames and their path. It is coupled with the + version_. This is only for efficiency; it does not represent undeclared + state. */ + mutable std::map dynamic_frames_{}; + + /* A store of the geometries sent to Meshcat, so that they can be removed if a + new geometry version appears that does not contain them. */ + mutable std::map geometries_{}; + + /* The parameters for the visualizer. */ + MeshcatVisualizerParams params_; +}; + +/** A convenient alias for the MeshcatVisualizer class when using the `double` +scalar type. */ +using MeshcatVisualizerd = MeshcatVisualizer; + +} // namespace geometry + +// Define the conversion trait to *only* allow double -> AutoDiffXd conversion. +// Symbolic is not supported yet, and AutoDiff -> double doesn't "make sense". +namespace systems { +namespace scalar_conversion { +template <> +struct Traits : public NonSymbolicTraits {}; +} // namespace scalar_conversion +} // namespace systems + +} // namespace drake + +DRAKE_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS( + class ::drake::geometry::MeshcatVisualizer) diff --git a/geometry/meshcat_visualizer_params.h b/geometry/meshcat_visualizer_params.h new file mode 100644 index 000000000000..a34bbd7ca2d5 --- /dev/null +++ b/geometry/meshcat_visualizer_params.h @@ -0,0 +1,38 @@ +#pragma once + +#include + +#include "drake/geometry/geometry_roles.h" +#include "drake/geometry/rgba.h" + +namespace drake { +namespace geometry { + +/** The set of parameters for configuring MeshcatVisualizer. */ +struct MeshcatVisualizerParams { + /** The duration (in simulation seconds) between attempts to update poses in + the visualizer. */ + double publish_period{1 / 60.0}; + + /** The role of the geometries to be sent to the visualizer. */ + Role role{Role::kIllustration}; + + /** The color to apply to any geometry that hasn't defined one. */ + Rgba default_color{0.9, 0.9, 0.9, 1.0}; + + /** A prefix to add to the path for all objects and transforms curated by the + MeshcatVisualizer. It can be an absolute path or relative path. If relative, + this `prefix` will be appended to the Meshcat `prefix` based on the standard + path semantics in Meshcat. See @ref meshcat_path "Meshcat paths" for + details. */ + std::string prefix{"visualizer"}; + + /** Determines whether to send a Meschat::Delete("/prefix") message on an + initialization event to remove any visualizations e.g. from a previous + simulation. See @ref declare_initialization_events "Declare initialization + events" for more information. */ + bool delete_prefix_on_initialization_event{true}; +}; + +} // namespace geometry +} // namespace drake diff --git a/geometry/test/meshcat_manual_test.cc b/geometry/test/meshcat_manual_test.cc index b59065bbfd71..4ec57663f2a4 100644 --- a/geometry/test/meshcat_manual_test.cc +++ b/geometry/test/meshcat_manual_test.cc @@ -1,8 +1,12 @@ #include "drake/common/find_resource.h" #include "drake/geometry/meshcat.h" +#include "drake/geometry/meshcat_visualizer.h" #include "drake/geometry/rgba.h" #include "drake/geometry/shape_specification.h" #include "drake/math/rigid_transform.h" +#include "drake/multibody/parsing/parser.h" +#include "drake/multibody/plant/multibody_plant.h" +#include "drake/systems/analysis/simulator.h" /** To test, you must manually run `bazel run //geometry:meshcat_manual_test`, then follow the instructions on your console. */ @@ -14,28 +18,28 @@ using Eigen::Vector3d; using math::RigidTransformd; int do_main() { - Meshcat meshcat; + auto meshcat = std::make_shared(); // Turn off the background (it will appear white). - meshcat.SetProperty("/Background", "visible", false); + meshcat->SetProperty("/Background", "visible", false); - meshcat.SetObject("sphere", Sphere(.25), Rgba(1.0, 0, 0, 1)); - meshcat.SetTransform("sphere", RigidTransformd(Vector3d{-2.0, 0, 0})); + meshcat->SetObject("sphere", Sphere(.25), Rgba(1.0, 0, 0, 1)); + meshcat->SetTransform("sphere", RigidTransformd(Vector3d{-2.0, 0, 0})); - meshcat.SetObject("cylinder", Cylinder(.25, .5), Rgba(0.0, 1.0, 0, 1)); - meshcat.SetTransform("cylinder", RigidTransformd(Vector3d{-1.0, 0, 0})); + meshcat->SetObject("cylinder", Cylinder(.25, .5), Rgba(0.0, 1.0, 0, 1)); + meshcat->SetTransform("cylinder", RigidTransformd(Vector3d{-1.0, 0, 0})); - meshcat.SetObject("ellipsoid", Ellipsoid(.25, .25, .5), Rgba(1., 0, 1, .5)); - meshcat.SetTransform("ellipsoid", RigidTransformd(Vector3d{0.0, 0, 0})); + meshcat->SetObject("ellipsoid", Ellipsoid(.25, .25, .5), Rgba(1., 0, 1, .5)); + meshcat->SetTransform("ellipsoid", RigidTransformd(Vector3d{0.0, 0, 0})); - meshcat.SetObject("box", Box(.25, .25, .5), Rgba(0, 0, 1, 1)); - meshcat.SetTransform("box", RigidTransformd(Vector3d{1.0, 0, 0})); + meshcat->SetObject("box", Box(.25, .25, .5), Rgba(0, 0, 1, 1)); + meshcat->SetTransform("box", RigidTransformd(Vector3d{1.0, 0, 0})); - meshcat.SetObject( + meshcat->SetObject( "obj", Mesh(FindResourceOrThrow( "drake/systems/sensors/test/models/meshes/box.obj"), .25)); - meshcat.SetTransform("obj", RigidTransformd(Vector3d{2.0, 0, 0})); + meshcat->SetTransform("obj", RigidTransformd(Vector3d{2.0, 0, 0})); std::cout << R"""( Open up your browser to the URL above. @@ -51,8 +55,8 @@ Open up your browser to the URL above. std::cout << "[Press RETURN to continue]." << std::endl; std::cin.ignore(std::numeric_limits::max(), '\n'); - meshcat.Delete("box"); - meshcat.SetProperty("/Lights/AmbientLight/", "intensity", 0.1); + meshcat->Delete("box"); + meshcat->SetProperty("/Lights/AmbientLight/", "intensity", 0.1); std::cout << "- The blue box should have disappeared and the lights should " "have dimmed." @@ -60,13 +64,49 @@ Open up your browser to the URL above. std::cout << "[Press RETURN to continue]." << std::endl; std::cin.ignore(std::numeric_limits::max(), '\n'); - meshcat.Delete(); + meshcat->Delete(); std::cout << "- Everything else should have disappeared." << std::endl; std::cout << "[Press RETURN to continue]." << std::endl; std::cin.ignore(std::numeric_limits::max(), '\n'); - std::cout << "All done." << std::endl; + meshcat->SetProperty("/Lights/AmbientLight/", "intensity", 0.6); + systems::DiagramBuilder builder; + auto [plant, scene_graph] = + multibody::AddMultibodyPlantSceneGraph(&builder, 0.001); + multibody::Parser(&plant).AddModelFromFile( + FindResourceOrThrow("drake/manipulation/models/iiwa_description/urdf/" + "iiwa14_no_collision.urdf")); + plant.WeldFrames(plant.world_frame(), + plant.GetBodyByName("base").body_frame()); + plant.Finalize(); + + builder.ExportInput(plant.get_actuation_input_port(), "actuation_input"); + MeshcatVisualizerParams params; + params.delete_prefix_on_initialization_event = false; + MeshcatVisualizerd::AddToBuilder(&builder, scene_graph, meshcat, params); + + auto diagram = builder.Build(); + auto context = diagram->CreateDefaultContext(); + diagram->get_input_port().FixValue(context.get(), Eigen::VectorXd::Zero(7)); + + diagram->Publish(*context); + std::cout + << "- Now you should see a kuka model (from MultibodyPlant/SceneGraph)" + << std::endl; + + std::cout << "[Press RETURN to continue]." << std::endl; + std::cin.ignore(std::numeric_limits::max(), '\n'); + + std::cout << "Finally we'll run the simulation (you should see the robot " + "fall down) and exit." + << std::endl; + + systems::Simulator simulator(*diagram, std::move(context)); + simulator.set_target_realtime_rate(1.0); + simulator.AdvanceTo(4.0); + + std::cout << "Exiting..." << std::endl; return 0; } diff --git a/geometry/test/meshcat_visualizer_test.cc b/geometry/test/meshcat_visualizer_test.cc new file mode 100644 index 000000000000..d2d1d82c9d45 --- /dev/null +++ b/geometry/test/meshcat_visualizer_test.cc @@ -0,0 +1,249 @@ +#include "drake/geometry/meshcat_visualizer.h" + +#include + +#include "drake/common/find_resource.h" +#include "drake/common/test_utilities/expect_throws_message.h" +#include "drake/multibody/parsing/parser.h" +#include "drake/multibody/plant/multibody_plant.h" +#include "drake/systems/analysis/simulator.h" +#include "drake/systems/framework/diagram_builder.h" +#include "drake/systems/primitives/constant_vector_source.h" + +namespace drake { +namespace geometry { +namespace { + +// The tests in this file require a dependency on MultibodyPlant. One could +// implement the tests without that dependency, but not without duplicating (or +// sharing) a significant amount of setup code like we see in +// DrakeVisualizerTest. +// +// Relative to DrakeVisualizerTest, this file does not have to provide test +// coverage for the message passing, nor for the variety of geometry types. +// Those are covered by the tests for Meshcat. Here we only aim to demonstrate +// that we call Meshcat correctly from MeshcatVisualizer. +// +// We're intentionally not building MeshcatVisualizer directly. +// Parsing with AutoDiffXd is not supported and populating the MBP is more work +// than it's worth. The scalar-converted instance is tested and that provides +// sufficient evidence for the validity of the type. + +class MeshcatVisualizerWithIiwaTest : public ::testing::Test { + protected: + MeshcatVisualizerWithIiwaTest() : meshcat_(std::make_shared()) {} + + void SetUpDiagram(MeshcatVisualizerParams params = {}) { + systems::DiagramBuilder builder; + auto [plant, scene_graph] = + multibody::AddMultibodyPlantSceneGraph(&builder, 0.001); + plant_ = &plant; + scene_graph_ = &scene_graph; + multibody::Parser(plant_).AddModelFromFile( + FindResourceOrThrow("drake/manipulation/models/iiwa_description/urdf/" + "iiwa14_spheres_collision.urdf")); + plant.WeldFrames(plant.world_frame(), + plant.GetBodyByName("base").body_frame()); + plant.Finalize(); + + auto zero_torque = + builder.template AddSystem>( + Eigen::VectorXd::Zero(7)); + builder.Connect(zero_torque->get_output_port(), + plant.get_actuation_input_port()); + + visualizer_ = &MeshcatVisualizer::AddToBuilder( + &builder, scene_graph, meshcat_, std::move(params)); + + diagram_ = builder.Build(); + context_ = diagram_->CreateDefaultContext(); + } + + std::shared_ptr meshcat_; + multibody::MultibodyPlant* plant_{}; + SceneGraph* scene_graph_{}; + const MeshcatVisualizer* visualizer_{}; + std::unique_ptr> diagram_{}; + std::unique_ptr> context_{}; +}; + +TEST_F(MeshcatVisualizerWithIiwaTest, BasicTest) { + SetUpDiagram(); + + EXPECT_FALSE(meshcat_->HasPath("/drake/visualizer/iiwa14")); + diagram_->Publish(*context_); + EXPECT_TRUE(meshcat_->HasPath("/drake/visualizer/iiwa14")); + for (int link = 0; link < 8; link++) { + EXPECT_NE(meshcat_->GetPackedTransform( + fmt::format("/drake/visualizer/iiwa14/iiwa_link_{}", link)), + ""); + } + + // Confirm that the transforms change after running a simulation. + const std::string packed_X_W7 = + meshcat_->GetPackedTransform("visualizer/iiwa14/iiwa_link_7"); + systems::Simulator simulator(*diagram_); + simulator.AdvanceTo(0.1); + EXPECT_NE(meshcat_->GetPackedTransform("visualizer/iiwa14/iiwa_link_7"), + packed_X_W7); +} + +TEST_F(MeshcatVisualizerWithIiwaTest, PublishPeriod) { + MeshcatVisualizerParams params; + params.publish_period = 0.123; + SetUpDiagram(params); + auto periodic_events = visualizer_->GetPeriodicEvents(); + for (const auto& data_and_vector : periodic_events) { + EXPECT_EQ(data_and_vector.second.size(), 1); // only one periodic event + EXPECT_EQ(data_and_vector.first.period_sec(), params.publish_period); + EXPECT_EQ(data_and_vector.first.offset_sec(), 0.0); + } +} + +// Confirms that all geometry registered to iiwa_link_7 in the urdf (in all +// three allowed roles) gets properly added. +TEST_F(MeshcatVisualizerWithIiwaTest, Roles) { + MeshcatVisualizerParams params; + for (Role role : {Role::kProximity, Role::kIllustration, Role::kPerception}) { + params.role = role; + SetUpDiagram(params); + EXPECT_FALSE(meshcat_->HasPath("visualizer/iiwa14/iiwa_link_7")); + diagram_->Publish(*context_); + EXPECT_TRUE(meshcat_->HasPath("visualizer/iiwa14/iiwa_link_7")); + auto& inspector = scene_graph_->model_inspector(); + FrameId iiwa_link_7 = plant_->GetBodyFrameIdOrThrow( + plant_->GetBodyByName("iiwa_link_7").index()); + for (GeometryId geom_id : inspector.GetGeometries(iiwa_link_7, role)) { + EXPECT_TRUE(meshcat_->HasPath( + fmt::format("visualizer/iiwa14/iiwa_link_7/{}", geom_id))); + } + meshcat_->Delete(); + } + + params.role = Role::kUnassigned; + DRAKE_EXPECT_THROWS_MESSAGE(SetUpDiagram(params), + ".*Role::kUnassigned.*"); +} + +TEST_F(MeshcatVisualizerWithIiwaTest, Prefix) { + MeshcatVisualizerParams params; + + // Absolute path. + params.prefix = "/foo"; + SetUpDiagram(params); + EXPECT_FALSE(meshcat_->HasPath("/foo/iiwa14")); + diagram_->Publish(*context_); + EXPECT_TRUE(meshcat_->HasPath("/foo/iiwa14")); + EXPECT_FALSE(meshcat_->HasPath("/drake/visualizer")); + + // Relative path. + params.prefix = "foo"; + EXPECT_FALSE(meshcat_->HasPath("/drake/foo/iiwa14")); + SetUpDiagram(params); + diagram_->Publish(*context_); + EXPECT_TRUE(meshcat_->HasPath("/drake/foo/iiwa14")); + EXPECT_FALSE(meshcat_->HasPath("/drake/visualizer")); +} + +TEST_F(MeshcatVisualizerWithIiwaTest, DeletePrefixOnInitialization) { + MeshcatVisualizerParams params; + params.delete_prefix_on_initialization_event = true; + SetUpDiagram(params); + // Scribble a transform onto the scene tree beneath the visualizer prefix. + meshcat_->SetTransform("/drake/visualizer/my_random_path", + math::RigidTransformd()); + EXPECT_TRUE(meshcat_->HasPath("/drake/visualizer/my_random_path")); + + { // Send an initialization event. + auto events = diagram_->AllocateCompositeEventCollection(); + diagram_->GetInitializationEvents(*context_, events.get()); + diagram_->Publish(*context_, events->get_publish_events()); + } + // Confirm that my scribble was deleted. + EXPECT_FALSE(meshcat_->HasPath("/drake/visualizer/my_random_path")); + + // Repeat, but this time with delete prefix disabled. + params.delete_prefix_on_initialization_event = false; + SetUpDiagram(params); + meshcat_->SetTransform("/drake/visualizer/my_random_path", + math::RigidTransformd()); + { // Send an initialization event. + auto events = diagram_->AllocateCompositeEventCollection(); + diagram_->GetInitializationEvents(*context_, events.get()); + diagram_->Publish(*context_, events->get_publish_events()); + } + // Confirm that my scribble remains. + EXPECT_TRUE(meshcat_->HasPath("/drake/visualizer/my_random_path")); +} + +TEST_F(MeshcatVisualizerWithIiwaTest, Delete) { + SetUpDiagram(); + diagram_->Publish(*context_); + EXPECT_TRUE(meshcat_->HasPath("/drake/visualizer")); + visualizer_->Delete(); + EXPECT_FALSE(meshcat_->HasPath("/drake/visualizer")); +} + +TEST_F(MeshcatVisualizerWithIiwaTest, ScalarConversion) { + SetUpDiagram(); + + auto ad_diagram = diagram_->ToAutoDiffXd(); + auto ad_context = ad_diagram->CreateDefaultContext(); + + // Call publish to provide code coverage for the AutoDiffXd version of + // UpdateMeshcat / SetObjects SetTransforms. We simply confirm that the code + // doesn't blow up. + ad_diagram->Publish(*ad_context); +} + +GTEST_TEST(MeshcatVisualizerTest, MultipleModels) { + auto meshcat = std::make_shared(); + + systems::DiagramBuilder builder; + auto [plant, scene_graph] = + multibody::AddMultibodyPlantSceneGraph(&builder, 0.001); + std::string urdf = FindResourceOrThrow( + "drake/manipulation/models/iiwa_description/urdf/" + "iiwa14_no_collision.urdf"); + auto iiwa0 = multibody::Parser(&plant).AddModelFromFile(urdf); + plant.WeldFrames(plant.world_frame(), + plant.GetBodyByName("base", iiwa0).body_frame()); + auto iiwa1 = multibody::Parser(&plant).AddModelFromFile(urdf, "second_iiwa"); + plant.WeldFrames(plant.world_frame(), + plant.GetBodyByName("base", iiwa1).body_frame()); + plant.Finalize(); + + auto zero_torque = builder.AddSystem( + Eigen::VectorXd::Zero(7)); + builder.Connect(zero_torque->get_output_port(), + plant.get_actuation_input_port(iiwa0)); + builder.Connect(zero_torque->get_output_port(), + plant.get_actuation_input_port(iiwa1)); + + // Use the query_output_port version of AddToBuilder. + MeshcatVisualizer::AddToBuilder( + &builder, scene_graph.get_query_output_port(), meshcat); + + auto diagram = builder.Build(); + auto context = diagram->CreateDefaultContext(); + + EXPECT_FALSE(meshcat->HasPath("/drake/visualizer/iiwa14")); + EXPECT_FALSE(meshcat->HasPath("/drake/visualizer/second_iiwa")); + + diagram->Publish(*context); + + EXPECT_TRUE(meshcat->HasPath("/drake/visualizer/iiwa14")); + EXPECT_TRUE(meshcat->HasPath("/drake/visualizer/second_iiwa")); + for (int link = 0; link < 8; link++) { + EXPECT_NE(meshcat->GetPackedTransform( + fmt::format("/drake/visualizer/iiwa14/iiwa_link_{}", link)), + ""); + EXPECT_NE(meshcat->GetPackedTransform(fmt::format( + "/drake/visualizer/second_iiwa/iiwa_link_{}", link)), + ""); + } +} + +} // namespace +} // namespace geometry +} // namespace drake