Add a LeafSystem for dragging objects from meshcat

[vincekurtz]

PR 1/2 toward landing #24642.

Adds a MeshcatMouseSpring system that produces spring forces on dragged bodies, and adds a small shim to Meshcat to facilitate testing. Full integration with the browser is deferred to a second PR.

---

This change is 

[vincekurtz]

+a:@SeanCurtis-TRI for feature review please!

[SeanCurtis-TRI]

First pass done. Sorry for the delay -- vacation followed by vacation catch up.

@SeanCurtis-TRI reviewed 9 files and all commit messages, and made 22 comments.
Reviewable status: 21 unresolved discussions, LGTM missing from assignee SeanCurtis-TRI(platform), needs at least two assigned reviewers, missing label for release notes (waiting on vincekurtz).

---

multibody/meshcat/test/meshcat_mouse_spring_test.cc line 75 at r1 (raw file):

  // meshcat drag state and the provided body poses/velocities.
  std::vector<ExternallyAppliedSpatialForce<double>> CalcForces(
      const std::vector<RigidTransformd>& X_WB,

BTW IT would be helpful to pluralize this parameters: X_WBs and V_WBs.

---

multibody/meshcat/test/meshcat_mouse_spring_test.cc line 100 at r1 (raw file):

  std::shared_ptr<Meshcat> meshcat_;
  MultibodyPlant<double> plant_{0.0};

BTW This would typically be initialized in the constructor's initializer list and not inline. That practice is limited to plain old data.

---

multibody/meshcat/test/meshcat_mouse_spring_test.cc line 149 at r1 (raw file):

// path. This mirrors the real paths the browser sends for the different
// MeshcatVisualizer layers (illustration / proximity / inertia).
TEST_F(MeshcatMouseSpringTest, DragScopedBodyAnyPrefix) {

nit: There are some untested elements of body identification. Particularly, you've tested a bunch of positive matches, we want to also get a hint for non-matches.

• best_len logic: Two body path segments match a drag path, but the longer is selected.
  • You'll have to add another body, say "robot/ball" and then you can show that "/foo/robot/ball" never matches ball, just "robot/ball".
  • Similarly, "/foo/robot2/ball" only matches against "ball" and not "robot/ball".
• prefix agnostic: /drake/foo/robot/link matches. You can change one of the strings in this test to use a prefix that is obviously not a part of any Drake code.
• No substring matching
  • /drake/foo/robot/link2 doesn't match.
  • `/drake/foo/some_robot/link" doesn't match
• /drake/foo/ball/ matches (even though it's not a path we expect to get).

---

multibody/meshcat/meshcat_mouse_spring.h line 18 at r1 (raw file):

/** %MeshcatMouseSpring lets a user drag the bodies of a MultibodyPlant with the
mouse in a Meshcat browser: holding Ctrl and dragging a body with the left mouse

nit: Again, better to document the controls in one place and this can just cite it.

---

multibody/meshcat/meshcat_mouse_spring.h line 51 at r1 (raw file):

body cannot.

This system is `double`-only, because Meshcat reports drag state as plain

BTW This is not actually an obstacle. The force can be derived from double values and simply inject AutoDiffXd-valued forces that have no derivatives.

I'd argue the true reason to only allow double-valued is because an AutoDiffXd-valued system would be too slow to meaningfully interact.

That said, we may consider implementing this on T and nominally supporting AutoDiffXd just so that this system doesn't preclude type conversion of the underlying system.

---

multibody/meshcat/meshcat_mouse_spring.h line 82 at r1 (raw file):

  /** Returns the input port for the bodies' poses (a
  `std::vector<math::RigidTransform<double>>`). */

nit: Let's be clear about the poses w.r.t. what?

---

multibody/meshcat/meshcat_mouse_spring.h line 87 at r1 (raw file):

  }

  /** Returns the input port for the bodies' spatial velocities (a

nit: Let's be clear about forces expressed in what? Also, as a spatial force, do we want to make a claim about the rotational componenent?

---

multibody/meshcat/meshcat_mouse_spring.h line 114 at r1 (raw file):

 private:
  // Builds the map from each body's scoped frame name to its index.
  void BuildPathToBodyMap(const MultibodyPlant<double>& plant);

nit: Is there a reason to pass the plant? You have the plant as a member.

---

multibody/meshcat/meshcat_mouse_spring.h line 121 at r1 (raw file):

  std::shared_ptr<geometry::Meshcat> meshcat_;
  const MultibodyPlant<double>* const plant_;

nit As you're not copyable, you might as well store this as a const reference. You can confirm it's not null in the constructor and the rest of the code can simply invoke on a reference instead of a pointer.

---

geometry/meshcat.h line 908 at r1 (raw file):

  /** The state of an in-progress mouse drag of a scene object, as reported by a
  Meshcat browser. See GetObjectDrag(). */
  struct ObjectDrag {

This struct has been causing me angst -- I feel the language and concepts are a bit muddy. Here's a random shopping list of points of mental friction for me:

1. The word "drag" overlaps with UI concepts (mouse drag) that don't properly apply here. Mouse drags are usually in screen space and the starting point is fixed.
2. The struct essentially defines a position vector p_AB_W but there's no hint of that. (A would be the "anchor" point and B the "target" point).
3. It's not clear to me if this type should more directly reflect what it is or how it is used. (The current term "drag" here seems to favor the latter.)
4. While its type is a position vector, its semantics are that of the endpoints of a spring. One end fixed to B the other fixed to the named body at point A.
   • Interpreted as the ends points of a spring, the term "anchor" feels misleading. It is, in fact, point B that is "anchored", in that the spring force will always attempt to move point A towards an immovable B ("immovable" in the sense that nothing in MbP's dynamics can move B; its position is proscribed).

So, here's an off-the-cuff proposal to see what it gets you to think of. The underlying goal of the proposal was:

1. Abstract out as much of meshcat as possible (i.e., focus on what the quantities are and elide where they come from).
2. Give it its physical semantics so its purpose and intent is obvious.

  /** The definition of a virtual, interactive spring's geometry. The 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 has no spring coefficient and can't produce forces by
  itself. The spring coefficient is presumably defined elsewhere. */
  struct VirtualSpring {
    /** The "/"-delimited Meshcat path of the object connected to the spring
    (e.g., "/drake/visualizer/my_model/my_body/my_geometry"). */
    std::string path;

    /** The current position of F affixed to body B. Note: in an
    interactive scenario with active dynamics, p_WF 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 proscribed 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_in_world;
  };

Feel free to reject this if you have an alternative. Ultimately, I hope all of this together successfully communicates my concern with the current language.

Also, if there is any credible idea for why we'd want a position vector with these properties that wouldn't be interpreted as a spring's geometry, we might consider some more generic name. But my imagination was insufficient to that purpose.

---

geometry/meshcat.h line 910 at r1 (raw file):

  struct ObjectDrag {
    /** The "/"-delimited Meshcat path of the object being dragged (e.g.,
    "/drake/visualizer/my_model/my_body/my_geometry"). */

BTW We want to be clear here about whether what we're getting is the name of the body, the geometry, or either.

Right now, the implication is "either". Let's call that out explicitly and not leave it implicit in the fake path provided.

Specifically, it is a path to anything. However, whether or not it has any effect depends on the caller's treatment of the path.

---

geometry/meshcat.h line 930 at r1 (raw file):

  object in a connected Meshcat browser, or std::nullopt otherwise.

  A drag is initiated in the browser by holding the <kbd>Ctrl</kbd> key and

nit: Instructions on how to drag do not belong in this file. They belong in the html itself (with discoverability enabled there). The reference to MeshcatMouseSpring, however, is appropriate.

---

geometry/meshcat.h line 939 at r1 (raw file):

  If multiple browsers report drags concurrently, the returned value reflects
  the most recently received message. */
  std::optional<ObjectDrag> GetObjectDrag() const;

BTW Something to consider. Instead of an optional ObjectDrag, you could define the semantics of an empty name meaning that there is no active drag. Then you could just maintain local copy of ObjectDrag and always return a reference to it. It also reduces the cost of clearing it (simply zero out the path and leave the point values as garbage).

The primary reason to favor the sentinel value is so that you don't have to copy the string on every update an extra time. Currently, we copy it once in meshcat.cc when we convert message to state and then we copy it again when we call GetObjectDrag().

I'll defer to you.

---

geometry/meshcat.cc line 2458 at r1 (raw file):

    }
    if (data.type == "mouse_drag") {
      if (data.drag_anchor.size() == 3 && data.drag_target.size() == 3) {

BTW You're testing both vector sizes. Why both? And if both, why not also check for a non-empty path?

You could argue that just testing path would be sufficient (and state the invariant that a non-empty path must also contain two vectors.

Or you could validate everything.

But this is neither the one thing, nor the other.

---

geometry/meshcat.cc line 2459 at r1 (raw file):

    if (data.type == "mouse_drag") {
      if (data.drag_anchor.size() == 3 && data.drag_target.size() == 3) {
        Meshcat::ObjectDrag drag;

nit This construction can be greatly simplified:

        mouse_drag_ = Meshcat::ObjectDrag{
            .path = std::move(data.name),
            .anchor_in_world = Eigen::Vector3d(data.drag_anchor.data()),
            .target_in_world = Eigen::Vector3d(data.drag_target.data())};

(If you buy my argument about ditching optional for a sentinel value, it becomes even simpler:

mouse_drag_.path = std::move(data.name);
if (!mouse_drag_.path.empty()) {
  mouse_drag_.anchor_in_world = Eigen::Vector3d(data.drag_anchor.data());
  mouse_drag_.target_in_world = Eigen::Vector3d(data.drag_target.data());
}

---

multibody/meshcat/meshcat_mouse_spring.cc at r1 (raw file):
Several big ticket items:

1. MeshcatVisualizer is sensitive to changes to the geometry (it has the version_ member. We might consider adding it here. (Although there are arguments for why it is not necessary -- ultimately that being the inability to change the bodies after finalization).
   • If we chose to go with this argument, we should be clear about it.
2. Rather than assuming the process by which geometry::Frames get named, you should take a SceneGraph as construction parameter and simply iterate through the frames and get their names directly. It removes one level of brittleness. (And then you use the plant to map geometry::Frame to body.)

---

multibody/meshcat/meshcat_mouse_spring.cc line 24 at r1 (raw file):

namespace {

// Replicates MultibodyPlant's body-frame naming (see GetScopedName in

nit: This is fundamentally brittle. So, let's be a bit more candid about the brittleness and the conditions under which this works:

// Reproduces the portion of the meshcat path that corresponds to each body's
// name. It's done by:
//
//   1. Construct a scoped name based on MultibodyPlant's logic (see
//      GetScopedName() in multibody_plant.cc). This is *not* the same logic as
//      contained in the ScopedName() class.
//      - We assume that the geometry Frame gets *this* name (as
//        MeshcatVisualizer uses the geometry Frame and not body to create
//        paths).
//   2. Transform the scoped name to a meshcat path using MeshcatVisualizer's
//      conversion (MeshcatVisualizer::SetObjects()).
//
// By design, the path segment omits prefixed path elements (e.g.,
// "/drake/visualizer").
//
// This must be kept in sync with that logic.

(1) would get replaced if you go with my suggestion about operating on SceneGraph's Frames instead of MultibodyPlant's bodies.

---

multibody/meshcat/meshcat_mouse_spring.cc line 87 at r1 (raw file):

    if (index == plant.world_body().index()) continue;
    const RigidBody<double>& body = plant.get_body(index);
    // MeshcatVisualizer publishes each body's geometry under a node named by

nit: This documentation is redundant of the documentation of BodyFramePathSegment.

---

multibody/meshcat/meshcat_mouse_spring.cc line 110 at r1 (raw file):

  // this way is independent of which visualization layer (illustration,
  // proximity, inertia, ...) was clicked. Among matches we keep the longest
  // (most specific) scoped name.

nit The "longest scoped name" rubric was really not obvious to me. I wracked my brains to figure out what value it served. I was also wrestling with the possibility of a body path segment appearing multiple times. So, I propose some alternate wordsmithing. This is a subtle a tricky enough approach that it's worth more fully characterizing it:

  // Identify the body associated with drag->path.
  //
  //  1) Search for the stored body path segment in drag->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.

All of this documentation is enough that it's probably worth elevating this particular logic into its own method that can be explicitly unit tested.

---

multibody/meshcat/meshcat_mouse_spring.cc line 146 at r1 (raw file):

  // The anchor expressed in the body frame, where the force is applied.
  const Vector3d p_BoBq_B = X_WB.inverse() * p_WA;

BTW The change of symbol A to Bq in the same expression where we remeasure/express it may be a bit too much. Perhaps it would be better to define it as p_WBq when translating it from drag->anchor_in_world?

---

multibody/meshcat/meshcat_mouse_spring.cc line 156 at r1 (raw file):

  // independent of mass.
  // TODO(vincekurtz): consider using composite mass instead of body mass.
  const double mass = plant_->get_body(body_index).default_mass();

nit: Let's put a note here that we're ignoring the body's parameterized mass. Specifically:

• We're using default_mass() because we expect the likelihood of a user tweaking the parameterized mass beyond the initial model declaration for an interactive visualization session where they want to drag things around to be vanishingly small. Getting access to the current parameterized mass is more trouble than it's worth.