From 48fa0c34f413ac06fe0247592887cf915b7314b1 Mon Sep 17 00:00:00 2001 From: Vince Kurtz Date: Thu, 25 Jun 2026 15:26:20 -0500 Subject: [PATCH] Add a LeafSystem for dragging objects from meshcat Author: Vince Kurtz Assisted-By: Claude Opus 4.8 --- bindings/generated_docstrings/geometry.h | 57 ++++ .../generated_docstrings/multibody_meshcat.h | 116 ++++++++ geometry/meshcat.cc | 13 + geometry/meshcat.h | 41 +++ multibody/meshcat/BUILD.bazel | 26 ++ multibody/meshcat/meshcat_mouse_spring.cc | 214 ++++++++++++++ multibody/meshcat/meshcat_mouse_spring.h | 172 +++++++++++ .../meshcat/test/meshcat_mouse_spring_test.cc | 277 ++++++++++++++++++ 8 files changed, 916 insertions(+) create mode 100644 multibody/meshcat/meshcat_mouse_spring.cc create mode 100644 multibody/meshcat/meshcat_mouse_spring.h create mode 100644 multibody/meshcat/test/meshcat_mouse_spring_test.cc diff --git a/bindings/generated_docstrings/geometry.h b/bindings/generated_docstrings/geometry.h index 30aac049b8ce..9fc5ec1db1f4 100644 --- a/bindings/generated_docstrings/geometry.h +++ b/bindings/generated_docstrings/geometry.h @@ -3825,6 +3825,20 @@ std∷nullopt is returned if: connected. - The meshcat session transmitting has an orthographic camera.)"""; } GetTrackedCameraPose; + // Symbol: drake::geometry::Meshcat::GetVirtualSpringKinematics + struct /* GetVirtualSpringKinematics */ { + // Source: drake/geometry/meshcat.h + const char* doc = +R"""(Returns the current mouse-drag state if a user is presently dragging +an object in a connected Meshcat browser, or std∷nullopt otherwise. + +A downstream system (see multibody∷meshcat∷MeshcatMouseSpring) can +read this state and convert it into a force applied to a simulated +body, letting users drag objects with the cursor. + +If multiple browsers report drags concurrently, the returned value +reflects the most recently received message.)"""; + } GetVirtualSpringKinematics; // Symbol: drake::geometry::Meshcat::HasPath struct /* HasPath */ { // Source: drake/geometry/meshcat.h @@ -4668,6 +4682,49 @@ You can also use your browser to download this file, by typing R"""(Sets a flag to pause/stop recording. When stopped, publish events will not add frames to the animation.)"""; } StopRecording; + // Symbol: drake::geometry::Meshcat::VirtualSpringKinematics + struct /* VirtualSpringKinematics */ { + // Source: drake/geometry/meshcat.h + const char* doc = +R"""(The geometry of a virtual, interactive spring. This spring connects +the point F (a point fixed on the body B -- named by ``path``) with an +immovable target point T. Both points are measured and expressed in +the world frame. This definition is for kinematics only: the spring's +stiffness, damping, and resultant forces are not defined here.)"""; + // Symbol: drake::geometry::Meshcat::VirtualSpringKinematics::body_point_in_world + struct /* body_point_in_world */ { + // Source: drake/geometry/meshcat.h + const char* doc = +R"""(The current position of F affixed to body B. Note: in an interactive +scenario with active dynamics, this position will change as the body B +moves. + +Note: This is expressed in Drake's z-up world frame (not Meshcat's +y-up frame).)"""; + } body_point_in_world; + // Symbol: drake::geometry::Meshcat::VirtualSpringKinematics::path + struct /* path */ { + // Source: drake/geometry/meshcat.h + const char* doc = +R"""(The "/"-delimited Meshcat path of the object being dragged (e.g., +"/drake/visualizer/my_model/my_body/my_geometry"). + +This could be any valid Meshcat path, including for either a body or a +geometry. Whether it has any effect depends on the caller's treatment.)"""; + } path; + // Symbol: drake::geometry::Meshcat::VirtualSpringKinematics::target_point_in_world + struct /* target_point_in_world */ { + // Source: drake/geometry/meshcat.h + const char* doc = +R"""(The current position of the fixed target point T. The target point is +solely determined by the interactive Meshcat application. From Drake's +perspective it is a point with a prescribed position in the world +frame. + +Note: This is expressed in Drake's z-up world frame (not Meshcat's +y-up frame).)"""; + } target_point_in_world; + } VirtualSpringKinematics; // Symbol: drake::geometry::Meshcat::get_mutable_recording struct /* get_mutable_recording */ { // Source: drake/geometry/meshcat.h diff --git a/bindings/generated_docstrings/multibody_meshcat.h b/bindings/generated_docstrings/multibody_meshcat.h index f5e066a9f932..7452114620b9 100644 --- a/bindings/generated_docstrings/multibody_meshcat.h +++ b/bindings/generated_docstrings/multibody_meshcat.h @@ -16,6 +16,7 @@ // #include "drake/multibody/meshcat/contact_visualizer_params.h" // #include "drake/multibody/meshcat/hydroelastic_contact_visualizer.h" // #include "drake/multibody/meshcat/joint_sliders.h" +// #include "drake/multibody/meshcat/meshcat_mouse_spring.h" // #include "drake/multibody/meshcat/point_contact_visualizer.h" // Symbol: pydrake_doc_multibody_meshcat @@ -378,6 +379,121 @@ Parameter ``q``: MultibodyPlant∷num_positions().)"""; } SetPositions; } JointSliders; + // Symbol: drake::multibody::meshcat::MeshcatMouseSpring + struct /* MeshcatMouseSpring */ { + // Source: drake/multibody/meshcat/meshcat_mouse_spring.h + const char* doc = +R"""(MeshcatMouseSpring lets a user drag the bodies of a MultibodyPlant +with the mouse in a Meshcat browser, applying a virtual spring force +that pulls the grabbed point toward the cursor. + +This system reads the drag state from Meshcat (see +geometry∷Meshcat∷GetVirtualSpringKinematics()) and outputs a +corresponding ExternallyAppliedSpatialForce on the dragged body. +Connecting that output to +MultibodyPlant∷get_applied_spatial_force_input_port() applies the +force; AddToBuilder() performs that connection along with the input +connections. + +.. pydrake_system:: + + name: MeshcatMouseSpring + input_ports: + - body_poses + - body_spatial_velocities + output_ports: + - applied_spatial_force + +The ``body_poses`` and ``body_spatial_velocities`` inputs come from +the same-named MultibodyPlant output ports. + +With ``m`` the dragged body's mass, the applied force (in the world +frame) is ``m * stiffness * (target - anchor) - m * sqrt(stiffness) * +v_anchor``, where ``anchor`` is the grabbed point on the body, +``target`` is the cursor position, and ``v_anchor`` is the world +velocity of the grabbed point. Scaling by ``m`` makes the +translational response frequency ``sqrt(stiffness)`` and damping ratio +independent of the body's mass. + +When no drag is in progress the output is empty. Any body with +geometry published to Meshcat by a geometry∷MeshcatVisualizer can be +dragged; the world body cannot.)"""; + // Symbol: drake::multibody::meshcat::MeshcatMouseSpring::AddToBuilder + struct /* AddToBuilder */ { + // Source: drake/multibody/meshcat/meshcat_mouse_spring.h + const char* doc = +R"""(Adds a MeshcatMouseSpring to ``builder`` and connects it to `plant`'s +body-pose and body-spatial-velocity output ports and its +applied-spatial-force input port. Returns a reference to the +newly-added system. + +Precondition: + plant is part of builder and is finalized. + +Precondition: + plant is registered as a geometry source with scene_graph. + +Precondition: + `plant`'s applied-spatial-force input port is not already + connected.)"""; + } AddToBuilder; + // Symbol: drake::multibody::meshcat::MeshcatMouseSpring::MeshcatMouseSpring + struct /* ctor */ { + // Source: drake/multibody/meshcat/meshcat_mouse_spring.h + const char* doc = +R"""(Constructs a MeshcatMouseSpring for the given ``plant``. + +Parameter ``meshcat``: + The Meshcat instance the user will interact with. The pointer is + aliased and must outlive this system. + +Parameter ``plant``: + The MultibodyPlant whose bodies can be dragged. The pointer is + aliased and must outlive this system; the plant must already be + finalized and registered as a geometry source with + ``scene_graph``. + +Parameter ``scene_graph``: + The SceneGraph that ``plant`` is registered with. It is used at + construction to read the frame names identifying draggable bodies + in the Meshcat scene tree; the reference is used only during + construction and is not retained. + +Parameter ``stiffness``: + The mass-normalized spring stiffness, in 1/s²; see the class + overview for the force it produces. + +Precondition: + plant->is_finalized() is true. + +Precondition: + plant is registered as a geometry source with scene_graph. + +Precondition: + finite stiffness >= 0.)"""; + } ctor; + // Symbol: drake::multibody::meshcat::MeshcatMouseSpring::get_applied_spatial_force_output_port + struct /* get_applied_spatial_force_output_port */ { + // Source: drake/multibody/meshcat/meshcat_mouse_spring.h + const char* doc = +R"""(Returns the output port for the applied spatial forces (a +``std∷vector>``).)"""; + } get_applied_spatial_force_output_port; + // Symbol: drake::multibody::meshcat::MeshcatMouseSpring::get_body_poses_input_port + struct /* get_body_poses_input_port */ { + // Source: drake/multibody/meshcat/meshcat_mouse_spring.h + const char* doc = +R"""(Returns the input port for the bodies' poses in the world frame (a +``std∷vector>``).)"""; + } get_body_poses_input_port; + // Symbol: drake::multibody::meshcat::MeshcatMouseSpring::get_body_spatial_velocities_input_port + struct /* get_body_spatial_velocities_input_port */ { + // Source: drake/multibody/meshcat/meshcat_mouse_spring.h + const char* doc = +R"""(Returns the input port for the bodies' spatial velocities in the world +frame (a ``std∷vector>``).)"""; + } get_body_spatial_velocities_input_port; + } MeshcatMouseSpring; } meshcat; } multibody; // Symbol: drake::systems diff --git a/geometry/meshcat.cc b/geometry/meshcat.cc index ee50aef7dfcf..c0538e15a02a 100644 --- a/geometry/meshcat.cc +++ b/geometry/meshcat.cc @@ -1901,6 +1901,14 @@ class Meshcat::Impl { return gamepad_; } + std::optional GetVirtualSpringKinematics() + const { + DRAKE_DEMAND(IsThread(main_thread_id_)); + + // TODO(vincekurtz): read virtual spring kinematics from mouse drags. + return std::nullopt; + } + // This function is for use by the websocket thread. The Meshcat::StaticHtml() // and Meshcat::StaticZip() outer functions call into here using appropriate // deferred handling. @@ -2968,6 +2976,11 @@ Meshcat::Gamepad Meshcat::GetGamepad() const { return impl().GetGamepad(); } +std::optional +Meshcat::GetVirtualSpringKinematics() const { + return impl().GetVirtualSpringKinematics(); +} + std::string Meshcat::StaticHtml() const { return impl().StaticHtml(); } diff --git a/geometry/meshcat.h b/geometry/meshcat.h index d2191334b753..acef9a6feea8 100644 --- a/geometry/meshcat.h +++ b/geometry/meshcat.h @@ -903,6 +903,47 @@ class Meshcat { gamepad is working), see https://beej.us/blog/data/javascript-gamepad/. */ Gamepad GetGamepad() const; + /** The geometry of a virtual, interactive spring. This spring connects the + point F (a point fixed on the body B -- named by `path`) with an immovable + target point T. Both points are measured and expressed in the world frame. + This definition is for kinematics only: the spring's stiffness, damping, and + resultant forces are not defined here. */ + struct VirtualSpringKinematics { + /** The "/"-delimited Meshcat path of the object being dragged (e.g., + "/drake/visualizer/my_model/my_body/my_geometry"). + + This could be any valid Meshcat path, including for either a body or a + geometry. Whether it has any effect depends on the caller's treatment. */ + std::string path; + + /** The current position of F affixed to body B. Note: in an + interactive scenario with active dynamics, this position will change as the + body B moves. + + Note: This is expressed in Drake's z-up world frame (not Meshcat's y-up + frame). */ + Eigen::Vector3d body_point_in_world; + + /** The current position of the fixed target point T. The target point is + solely determined by the interactive Meshcat application. From Drake's + perspective it is a point with a prescribed position in the world frame. + + Note: This is expressed in Drake's z-up world frame (not Meshcat's y-up + frame). */ + Eigen::Vector3d target_point_in_world; + }; + + /** Returns the current mouse-drag state if a user is presently dragging an + object in a connected Meshcat browser, or std::nullopt otherwise. + + A downstream system (see multibody::meshcat::MeshcatMouseSpring) can read this + state and convert it into a force applied to a simulated body, letting users + drag objects with the cursor. + + If multiple browsers report drags concurrently, the returned value reflects + the most recently received message. */ + std::optional GetVirtualSpringKinematics() const; + //@} /** Returns an HTML string that can be saved to a file for a snapshot of the diff --git a/multibody/meshcat/BUILD.bazel b/multibody/meshcat/BUILD.bazel index 1155b8e391a7..64a74d6c0812 100644 --- a/multibody/meshcat/BUILD.bazel +++ b/multibody/meshcat/BUILD.bazel @@ -23,6 +23,7 @@ drake_cc_package_library( ":contact_visualizer_params", ":hydroelastic_contact_visualizer", ":joint_sliders", + ":meshcat_mouse_spring", ":point_contact_visualizer", ], ) @@ -111,6 +112,31 @@ drake_cc_googletest( ], ) +drake_cc_library( + name = "meshcat_mouse_spring", + srcs = ["meshcat_mouse_spring.cc"], + hdrs = ["meshcat_mouse_spring.h"], + deps = [ + "//common:essential", + "//geometry:meshcat", + "//geometry:scene_graph", + "//multibody/plant", + "//systems/framework:leaf_system", + ], +) + +drake_cc_googletest( + name = "meshcat_mouse_spring_test", + deps = [ + ":meshcat_mouse_spring", + "//common/test_utilities:eigen_matrix_compare", + "//common/test_utilities:expect_throws_message", + "//geometry:scene_graph", + "//geometry/test_utilities:meshcat_environment", + "//multibody/plant", + ], +) + drake_cc_library( name = "point_contact_visualizer", srcs = ["point_contact_visualizer.cc"], diff --git a/multibody/meshcat/meshcat_mouse_spring.cc b/multibody/meshcat/meshcat_mouse_spring.cc new file mode 100644 index 000000000000..4aa6169b8516 --- /dev/null +++ b/multibody/meshcat/meshcat_mouse_spring.cc @@ -0,0 +1,214 @@ +#include "drake/multibody/meshcat/meshcat_mouse_spring.h" + +#include +#include +#include +#include + +#include "drake/common/drake_assert.h" +#include "drake/geometry/scene_graph_inspector.h" +#include "drake/multibody/math/spatial_algebra.h" +#include "drake/multibody/plant/externally_applied_spatial_force.h" + +namespace drake { +namespace multibody { +namespace meshcat { + +using Eigen::Vector3d; +using geometry::FrameId; +using geometry::Meshcat; +using geometry::SceneGraph; +using geometry::SceneGraphInspector; +using math::RigidTransform; +using systems::Context; +using systems::DiagramBuilder; + +MeshcatMouseSpring::MeshcatMouseSpring(std::shared_ptr meshcat, + const MultibodyPlant* plant, + const SceneGraph& scene_graph, + double stiffness) + : systems::LeafSystem(), + meshcat_(std::move(meshcat)), + plant_(DRAKE_DEREF(plant)), + stiffness_(stiffness) { + DRAKE_THROW_UNLESS(meshcat_ != nullptr); + // N.B. Unlike geometry::meshcat::MeshcatVisualizer, we don't need to track + // geometry changes because bodies cannot be changed after plant finalization. + DRAKE_THROW_UNLESS(plant_.is_finalized()); + DRAKE_THROW_UNLESS(plant_.geometry_source_is_registered()); + DRAKE_THROW_UNLESS(stiffness_ >= 0.0); + + BuildPathToBodyMap(scene_graph); + + body_poses_input_port_ = + this->DeclareAbstractInputPort( + "body_poses", Value>>()) + .get_index(); + body_spatial_velocities_input_port_ = + this->DeclareAbstractInputPort( + "body_spatial_velocities", + Value>>()) + .get_index(); + applied_spatial_force_output_port_ = + this->DeclareAbstractOutputPort( + "applied_spatial_force", + std::vector>{}, + &MeshcatMouseSpring::CalcAppliedSpatialForce) + .get_index(); +} + +MeshcatMouseSpring::~MeshcatMouseSpring() = default; + +void MeshcatMouseSpring::BuildPathToBodyMap( + const SceneGraph& scene_graph) { + const SceneGraphInspector& inspector = scene_graph.model_inspector(); + for (const FrameId frame_id : inspector.GetAllFrameIds()) { + if (frame_id == inspector.world_frame_id()) continue; + const RigidBody* body = plant_.GetBodyFromFrameId(frame_id); + if (body == nullptr) continue; // Not a body frame of this plant. + // MeshcatVisualizer publishes each frame's geometry under a node named by + // the frame's name with "::" model-instance separators replaced by "/", + // e.g. "/drake//my_model/my_body/". We read that frame + // name straight from SceneGraph (rather than reconstructing it from the + // plant) and key on it ("my_model/my_body"), matching it within the dragged + // path below. + std::string path = inspector.GetName(frame_id); + for (size_t pos = 0; (pos = path.find("::", pos)) != std::string::npos;) { + path.replace(pos, 2, "/"); + pos += 1; // Start searching after the newly inserted `/`. + } + path_to_body_[std::move(path)] = body->index(); + } +} + +std::vector> +MeshcatMouseSpring::CalcAppliedSpatialForceFromDrag( + const std::optional& drag, + const std::vector>& X_WB_all, + const std::vector>& V_WB_all) const { + if (!drag.has_value()) { + return {}; + } + + const std::optional maybe_body_index = + internal::FindBodyForPath(path_to_body_, drag->path); + if (!maybe_body_index.has_value()) { + // The dragged object doesn't belong to a movable body of this plant. + return {}; + } + const BodyIndex body_index = *maybe_body_index; + + const RigidTransform& X_WB = X_WB_all[body_index]; + const SpatialVelocity& V_WB = V_WB_all[body_index]; + + // The attachment point Bq (a point fixed to body B) and the cursor target T, + // in world. + const Vector3d& p_WBq = drag->body_point_in_world; + const Vector3d& p_WT = drag->target_point_in_world; + + // The attachment point expressed in the body frame, where the force is + // applied. + const Vector3d p_BoBq_B = X_WB.inverse() * p_WBq; + + // The world velocity of the attachment point, for damping. + const Vector3d p_BoBq_W = p_WBq - X_WB.translation(); + const Vector3d v_WBq = V_WB.Shift(p_BoBq_W).translational(); + + // Mass-scaled spring + damper force: scaling by the body's mass makes the + // translational response frequency (sqrt(stiffness)) and damping ratio + // independent of mass. Note that we're using default_mass() because getting + // access to the current parameterized mass is more trouble than it's worth. + // TODO(vincekurtz): consider using composite mass instead of body mass. + const double mass = plant_.get_body(body_index).default_mass(); + const Vector3d f_W = + mass * stiffness_ * (p_WT - p_WBq) - mass * std::sqrt(stiffness_) * v_WBq; + + ExternallyAppliedSpatialForce force; + force.body_index = body_index; + force.p_BoBq_B = p_BoBq_B; + force.F_Bq_W = SpatialForce(Vector3d::Zero(), f_W); + return {force}; +} + +void MeshcatMouseSpring::CalcAppliedSpatialForce( + const Context& context, + std::vector>* forces) const { + *forces = CalcAppliedSpatialForceFromDrag( + meshcat_->GetVirtualSpringKinematics(), + get_body_poses_input_port().Eval>>( + context), + get_body_spatial_velocities_input_port() + .Eval>>(context)); +} + +MeshcatMouseSpring& MeshcatMouseSpring::AddToBuilder( + DiagramBuilder* builder, const MultibodyPlant* plant, + const SceneGraph& scene_graph, std::shared_ptr meshcat, + double stiffness) { + DRAKE_THROW_UNLESS(builder != nullptr); + DRAKE_THROW_UNLESS(plant != nullptr); + auto& spring = *builder->AddSystem( + std::move(meshcat), plant, scene_graph, stiffness); + spring.set_name("meshcat_mouse_spring"); + builder->Connect(plant->get_body_poses_output_port(), + spring.get_body_poses_input_port()); + builder->Connect(plant->get_body_spatial_velocities_output_port(), + spring.get_body_spatial_velocities_input_port()); + builder->Connect(spring.get_applied_spatial_force_output_port(), + plant->get_applied_spatial_force_input_port()); + return spring; +} + +namespace internal { + +std::optional FindBodyForPath( + const std::map& path_to_body, + const std::string& path) { + // Identify the body associated with `path`. + // + // 1) Search for the stored body path segment in `path`. It must appear + // prefixed by `/` (so "foo" doesn't match "some_foo"), and either be + // suffixed by '/' or be at the end of the path string. + // - We do a left search to find the portion of the path that is closest + // to the root of meshcat's scene hierarchy. + // 2) Because not all path segments consist of "model/body" (i.e., those + // bodies in the default model and world model instances), we need to make + // sure "body" doesn't match "m1/body", "m2/body", etc. As both "body" + // and "m1/body" would match "drake/foo/m1/body", we prefer the *longest* + // matching path segment (the pre- and post `/` delimiters prevent us from + // catching false matches based on common substrings). + // + // Note: we don't have to worry about multiple matches (e.g., matching body + // path segment "m/b" to the path "drake/foo/m/b/and/m/b") because we are + // only interested in Meshcat paths that correspond to MultibodyPlant links. + // Those are generally defined in SceneGraph as a flat list of bodies and, + // therefore, appear in Meshcat without nesting. Anything nested in that + // manner came from an alternative, and therefore ignorable, source. + // + // This search allows us to identify the body whether the user drags on the + // illustration or proximity geometry -- as long as the geometry is ultimately + // a child of an identifiable body. + std::optional body_index; + size_t best_len = 0; + for (const auto& [segment, index] : path_to_body) { + // A shorter-or-equal segment can't beat the current best match. + if (segment.size() <= best_len) continue; + const std::string needle = "/" + segment; + for (size_t pos = path.find(needle); pos != std::string::npos; + pos = path.find(needle, pos + 1)) { + const size_t after = pos + needle.size(); + if (after == path.size() || path[after] == '/') { + best_len = segment.size(); + body_index = index; + break; + } + } + } + return body_index; +} + +} // namespace internal + +} // namespace meshcat +} // namespace multibody +} // namespace drake diff --git a/multibody/meshcat/meshcat_mouse_spring.h b/multibody/meshcat/meshcat_mouse_spring.h new file mode 100644 index 000000000000..7f05ff87a304 --- /dev/null +++ b/multibody/meshcat/meshcat_mouse_spring.h @@ -0,0 +1,172 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include "drake/geometry/meshcat.h" +#include "drake/geometry/scene_graph.h" +#include "drake/multibody/plant/externally_applied_spatial_force.h" +#include "drake/multibody/plant/multibody_plant.h" +#include "drake/systems/framework/leaf_system.h" + +namespace drake { +namespace multibody { +namespace meshcat { + +/** %MeshcatMouseSpring lets a user drag the bodies of a MultibodyPlant with the +mouse in a Meshcat browser, applying a virtual spring force that pulls the +grabbed point toward the cursor. + +This system reads the drag state from Meshcat (see +geometry::Meshcat::GetVirtualSpringKinematics()) and outputs a corresponding +ExternallyAppliedSpatialForce on the dragged body. Connecting that +output to MultibodyPlant::get_applied_spatial_force_input_port() applies the +force; AddToBuilder() performs that connection along with the input connections. + +@system +name: MeshcatMouseSpring +input_ports: +- body_poses +- body_spatial_velocities +output_ports: +- applied_spatial_force +@endsystem + +The `body_poses` and `body_spatial_velocities` inputs come from the same-named +MultibodyPlant output ports. + +With `m` the dragged body's mass, the applied force (in the world frame) is +`m * stiffness * (target - anchor) - m * sqrt(stiffness) * v_anchor`, where +`anchor` is the grabbed point on the body, `target` is the cursor position, and +`v_anchor` is the world velocity of the grabbed point. Scaling by `m` makes the +translational response frequency `sqrt(stiffness)` and damping ratio independent +of the body's mass. + +When no drag is in progress the output is empty. Any body with geometry +published to Meshcat by a geometry::MeshcatVisualizer can be dragged; the world +body cannot. + +@ingroup visualization */ +class MeshcatMouseSpring final : public systems::LeafSystem { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(MeshcatMouseSpring); + + /** The default mass-normalized spring stiffness, in 1/s². */ + static constexpr double kDefaultStiffness = 1000.0; + + /** Constructs a %MeshcatMouseSpring for the given `plant`. + + @param meshcat The Meshcat instance the user will interact with. The pointer + is aliased and must outlive this system. + + @param plant The MultibodyPlant whose bodies can be dragged. The pointer is + aliased and must outlive this system; the plant must already be finalized and + registered as a geometry source with `scene_graph`. + + @param scene_graph The SceneGraph that `plant` is registered with. It is used + at construction to read the frame names identifying draggable bodies in the + Meshcat scene tree; the reference is used only during construction and is not + retained. + + @param stiffness The mass-normalized spring stiffness, in 1/s²; see the class + overview for the force it produces. + + @pre plant->is_finalized() is true. + @pre plant is registered as a geometry source with scene_graph. + @pre finite stiffness >= 0. */ + MeshcatMouseSpring(std::shared_ptr meshcat, + const MultibodyPlant* plant, + const geometry::SceneGraph& scene_graph, + double stiffness = kDefaultStiffness); + + ~MeshcatMouseSpring() final; + + /** Returns the input port for the bodies' poses in the world frame (a + `std::vector>`). */ + const systems::InputPort& get_body_poses_input_port() const { + return this->get_input_port(body_poses_input_port_); + } + + /** Returns the input port for the bodies' spatial velocities in the world + frame (a `std::vector>`). */ + const systems::InputPort& get_body_spatial_velocities_input_port() + const { + return this->get_input_port(body_spatial_velocities_input_port_); + } + + /** Returns the output port for the applied spatial forces (a + `std::vector>`). */ + const systems::OutputPort& get_applied_spatial_force_output_port() + const { + return this->get_output_port(applied_spatial_force_output_port_); + } + + /** Adds a %MeshcatMouseSpring to `builder` and connects it to `plant`'s + body-pose and body-spatial-velocity output ports and its applied-spatial-force + input port. Returns a reference to the newly-added system. + + @pre plant is part of builder and is finalized. + @pre plant is registered as a geometry source with scene_graph. + @pre `plant`'s applied-spatial-force input port is not already connected. */ + static MeshcatMouseSpring& AddToBuilder( + systems::DiagramBuilder* builder, + const MultibodyPlant* plant, + const geometry::SceneGraph& scene_graph, + std::shared_ptr meshcat, + double stiffness = kDefaultStiffness); + + private: + friend class MeshcatMouseSpringTester; + + // Builds the map from each body's scoped Meshcat-path segment to its index. + void BuildPathToBodyMap(const geometry::SceneGraph& scene_graph); + + // Returns the spring force produced by the given drag state, or an empty + // vector if there is no drag or the dragged path doesn't name a (non-world) + // body of plant_. The poses X_WB_all and spatial velocities V_WB_all of all + // the plant's bodies are indexed by BodyIndex. + std::vector> + CalcAppliedSpatialForceFromDrag( + const std::optional& drag, + const std::vector>& X_WB_all, + const std::vector>& V_WB_all) const; + + void CalcAppliedSpatialForce( + const systems::Context& context, + std::vector>* forces) const; + + std::shared_ptr meshcat_; + const MultibodyPlant& plant_; + const double stiffness_; + + // Maps each (non-world) body's scoped frame name as it appears in the Meshcat + // scene tree (e.g. "my_model/my_body", or just "my_body" for the default + // model instance) to that body's index. The leading "/drake//" and + // any trailing geometry path are matched separately, so this is independent + // of the visualizer prefix. + std::map path_to_body_; + + systems::InputPortIndex body_poses_input_port_; + systems::InputPortIndex body_spatial_velocities_input_port_; + systems::OutputPortIndex applied_spatial_force_output_port_; +}; + +namespace internal { + +/* Returns the index of the MultibodyPlant body whose stored Meshcat scene-tree +path segment matches `path`, or std::nullopt if none matches. `path_to_body` +maps each body's scoped path segment (e.g. "model/body", or just "body" for the +default and world model instances) to its index. Exposed for testing; see the +.cc for the exact matching rules. */ +std::optional FindBodyForPath( + const std::map& path_to_body, + const std::string& path); + +} // namespace internal + +} // namespace meshcat +} // namespace multibody +} // namespace drake diff --git a/multibody/meshcat/test/meshcat_mouse_spring_test.cc b/multibody/meshcat/test/meshcat_mouse_spring_test.cc new file mode 100644 index 000000000000..314ece4c6199 --- /dev/null +++ b/multibody/meshcat/test/meshcat_mouse_spring_test.cc @@ -0,0 +1,277 @@ +#include "drake/multibody/meshcat/meshcat_mouse_spring.h" + +#include +#include +#include +#include +#include +#include + +#include + +#include "drake/common/test_utilities/eigen_matrix_compare.h" +#include "drake/common/test_utilities/expect_throws_message.h" +#include "drake/geometry/scene_graph.h" +#include "drake/geometry/test_utilities/meshcat_environment.h" +#include "drake/math/rigid_transform.h" +#include "drake/math/roll_pitch_yaw.h" +#include "drake/multibody/plant/multibody_plant.h" +#include "drake/multibody/tree/spatial_inertia.h" + +namespace drake { +namespace multibody { +namespace meshcat { + +// Grants the tests below access to the force computation for dummy drag +// scenarios. +class MeshcatMouseSpringTester { + public: + static std::vector> + CalcAppliedSpatialForce( + const MeshcatMouseSpring& spring, + const std::optional& drag, + const std::vector>& X_WB_all, + const std::vector>& V_WB_all) { + return spring.CalcAppliedSpatialForceFromDrag(drag, X_WB_all, V_WB_all); + } +}; + +namespace { + +using Eigen::Vector3d; +using geometry::Meshcat; +using geometry::SceneGraph; +using math::RigidTransformd; +using math::RollPitchYawd; + +class MeshcatMouseSpringTest : public ::testing::Test { + protected: + // Build a plant with a free "ball" body in the default model instance plus a + // free "link" and a second "ball" inside a named "robot" model instance. The + // bodies have distinct (non-unit) masses so that the mass-scaling of the + // spring force is exercised, and the two "ball" bodies (scoped "ball" and + // "robot/ball") let us verify longest-match body identification. + MeshcatMouseSpringTest() + : meshcat_(geometry::GetTestEnvironmentMeshcat()), + plant_(0.0), + robot_(plant_.AddModelInstance("robot")), + ball_(&plant_.AddRigidBody( + "ball", default_model_instance(), + SpatialInertia::SolidSphereWithMass(kBallMass, 0.1))), + link_(&plant_.AddRigidBody( + "link", robot_, + SpatialInertia::SolidSphereWithMass(kLinkMass, 0.1))), + robot_ball_(&plant_.AddRigidBody( + "ball", robot_, + SpatialInertia::SolidSphereWithMass(kRobotBallMass, 0.1))) { + // Registering the plant as a geometry source gives every body a SceneGraph + // frame named by its scoped name; MeshcatMouseSpring reads those names to + // map dragged Meshcat paths back to bodies. + plant_.RegisterAsSourceForSceneGraph(&scene_graph_); + plant_.Finalize(); + } + + // Returns the forces computed by a freshly built spring, given the drag state + // and the provided body poses/velocities. + std::vector> CalcForces( + const std::optional& drag, + const std::vector& X_WBs, + const std::vector>& V_WBs) { + MeshcatMouseSpring spring(meshcat_, &plant_, scene_graph_, kStiffness); + return MeshcatMouseSpringTester::CalcAppliedSpatialForce(spring, drag, + X_WBs, V_WBs); + } + + std::vector DefaultPoses() const { + return std::vector(plant_.num_bodies(), + RigidTransformd::Identity()); + } + std::vector> ZeroVelocities() const { + return std::vector>( + plant_.num_bodies(), SpatialVelocity::Zero()); + } + + static constexpr double kStiffness = 100.0; // 1/s². + static constexpr double kBallMass = 2.0; // kg. + static constexpr double kLinkMass = 3.0; // kg. + static constexpr double kRobotBallMass = 5.0; // kg. + + std::shared_ptr meshcat_; + SceneGraph scene_graph_; + MultibodyPlant plant_; + ModelInstanceIndex robot_; + const RigidBody* const ball_; + const RigidBody* const link_; + const RigidBody* const robot_ball_; +}; + +// With no active drag, the output force vector is empty. +TEST_F(MeshcatMouseSpringTest, NoDrag) { + EXPECT_TRUE( + CalcForces(std::nullopt, DefaultPoses(), ZeroVelocities()).empty()); +} + +// A drag on the default-instance "ball" body produces a spring force at the +// expected body point, with the expected magnitude. +TEST_F(MeshcatMouseSpringTest, DragBall) { + // Place the ball at a non-identity pose so the inverse transform matters. + const RigidTransformd X_WB(RollPitchYawd(0.1, 0.2, 0.3), Vector3d(1, 2, 3)); + auto poses = DefaultPoses(); + poses[ball_->index()] = X_WB; + + const Vector3d anchor(1.2, 2.0, 3.1); // A point near the ball, in world. + const Vector3d target(1.5, 2.0, 3.1); // The cursor target, in world. + const Meshcat::VirtualSpringKinematics drag{.path = "/drake/visualizer/ball", + .body_point_in_world = anchor, + .target_point_in_world = target}; + + const auto forces = CalcForces(drag, poses, ZeroVelocities()); + ASSERT_EQ(forces.size(), 1); + EXPECT_EQ(forces[0].body_index, ball_->index()); + // The application point, in the body frame. + EXPECT_TRUE( + CompareMatrices(forces[0].p_BoBq_B, X_WB.inverse() * anchor, 1e-12)); + // Zero velocity => pure mass-scaled spring force mass*k*(target - anchor), + // no torque. + EXPECT_TRUE(CompareMatrices(forces[0].F_Bq_W.translational(), + kBallMass * kStiffness * (target - anchor), + 1e-12)); + EXPECT_TRUE( + CompareMatrices(forces[0].F_Bq_W.rotational(), Vector3d::Zero(), 1e-12)); +} + +// Body identification: a dragged Meshcat path resolves to the plant body whose +// scoped scene-tree name it contains, delimited by '/' (or the end of the path) +// on both sides. This exercises the full pipeline (SceneGraph frame names -> +// path map -> matching) via CalcForces, covering prefix independence, the +// longest-match preference, a trailing slash, and a variety of non-matches. +TEST_F(MeshcatMouseSpringTest, DragScopedBodyAnyPrefix) { + struct Case { + std::string path; + // The body the drag should resolve to, or nullopt for "no match". + std::optional expected; + }; + const std::vector cases = { + // "robot/link" matches regardless of the visualization-layer prefix and + // any trailing geometry path -- mirroring the real paths the browser + // sends for the different MeshcatVisualizer layers: illustration... + {"/drake/illustration/robot/link/some_geometry", link_->index()}, + // ...proximity (whose geometry name repeats the model scope)... + {"/drake/proximity/robot/link/robot/Mesh", link_->index()}, + // ...and inertia (which inserts an extra "InertiaVisualizer" node). + {"/drake/inertia/InertiaVisualizer/robot/link/$inertia", link_->index()}, + // The prefix is irrelevant, even one that no Drake code would produce. + {"/some/external/viewer/robot/link", link_->index()}, + + // A trailing '/' still delimits the segment (not a path we expect to + // receive, but the match should succeed). + {"/drake/foo/ball/", ball_->index()}, + + // Longest-match preference: "/robot/ball" contains both "ball" and the + // more-specific "robot/ball"; the longer segment wins. + {"/drake/foo/robot/ball", robot_ball_->index()}, + // A different model instance ("robot2") doesn't match "robot/ball", so + // only the default-instance "ball" matches. + {"/drake/foo/robot2/ball", ball_->index()}, + + // No substring matching: the stored segment must be bounded by '/' (or + // the end of the path) on both sides. + {"/drake/foo/robot/link2", std::nullopt}, // "link2" != "link". + {"/drake/foo/some_robot/link", std::nullopt}, // "some_robot" != "robot". + {"/drake/foo/not_a_body", std::nullopt}, + }; + + for (const Case& c : cases) { + SCOPED_TRACE(c.path); + const Meshcat::VirtualSpringKinematics drag{ + .path = c.path, + .body_point_in_world = Vector3d::Zero(), + .target_point_in_world = Vector3d::UnitZ()}; + const auto forces = CalcForces(drag, DefaultPoses(), ZeroVelocities()); + if (c.expected.has_value()) { + ASSERT_EQ(forces.size(), 1); + EXPECT_EQ(forces[0].body_index, *c.expected); + } else { + EXPECT_TRUE(forces.empty()); + } + } +} + +// Damping opposes the velocity of the attachment point, scaled by mass and +// sqrt(stiffness). +TEST_F(MeshcatMouseSpringTest, Damping) { + // With the ball at identity and the anchor at Bo, the anchor's velocity + // equals the body's translational velocity. + const Meshcat::VirtualSpringKinematics drag{ + .path = "/drake/visualizer/ball", + .body_point_in_world = Vector3d::Zero(), + .target_point_in_world = Vector3d::Zero()}; + auto vels = ZeroVelocities(); + const Vector3d v_WB(0.5, 0.0, 0.0); + vels[ball_->index()] = SpatialVelocity(Vector3d::Zero(), v_WB); + + const auto forces = CalcForces(drag, DefaultPoses(), vels); + ASSERT_EQ(forces.size(), 1); + const double expected_damping = kBallMass * std::sqrt(kStiffness); + EXPECT_TRUE(CompareMatrices(forces[0].F_Bq_W.translational(), + -expected_damping * v_WB, 1e-12)); +} + +// The output port reports no force when Meshcat reports no drag. (The rest of +// the port's calculation is the force computation tested above.) +TEST_F(MeshcatMouseSpringTest, OutputPort) { + ASSERT_FALSE(meshcat_->GetVirtualSpringKinematics().has_value()); + MeshcatMouseSpring spring(meshcat_, &plant_, scene_graph_, kStiffness); + auto context = spring.CreateDefaultContext(); + spring.get_body_poses_input_port().FixValue(context.get(), DefaultPoses()); + spring.get_body_spatial_velocities_input_port().FixValue(context.get(), + ZeroVelocities()); + EXPECT_TRUE( + spring.get_applied_spatial_force_output_port() + .Eval>>(*context) + .empty()); +} + +// Construction precondition checks. +TEST_F(MeshcatMouseSpringTest, ConstructorErrors) { + DRAKE_EXPECT_THROWS_MESSAGE( + MeshcatMouseSpring(meshcat_, &plant_, scene_graph_, -1.0 /* stiffness */), + ".*stiffness.*"); + MultibodyPlant unfinalized(0.0); + DRAKE_EXPECT_THROWS_MESSAGE( + MeshcatMouseSpring(meshcat_, &unfinalized, scene_graph_), + ".*is_finalized.*"); + // A finalized plant that was never registered with a SceneGraph has no body + // frames to read. + MultibodyPlant no_scene_graph(0.0); + no_scene_graph.Finalize(); + DRAKE_EXPECT_THROWS_MESSAGE( + MeshcatMouseSpring(meshcat_, &no_scene_graph, scene_graph_), + ".*geometry_source_is_registered.*"); +} + +// When a shorter segment ("body") and a longer, more-specific one ("m1/body") +// both match a path, the longest wins. +GTEST_TEST(FindBodyForPathTest, PrefersLongestSegment) { + const BodyIndex unscoped(1); + const BodyIndex scoped(2); + const std::map path_to_body = { + {"body", unscoped}, + {"m1/body", scoped}, + }; + + // Both "body" and "m1/body" match here; the longer one is chosen. + EXPECT_EQ(internal::FindBodyForPath(path_to_body, "/drake/foo/m1/body"), + scoped); + // Only the shorter segment is present here. + EXPECT_EQ(internal::FindBodyForPath(path_to_body, "/drake/foo/body"), + unscoped); + // Neither segment matches here. + EXPECT_EQ(internal::FindBodyForPath(path_to_body, "/drake/foo/m1"), + std::nullopt); +} + +} // namespace +} // namespace meshcat +} // namespace multibody +} // namespace drake