[multibody] Add surface velocity to MultibodyPlant by SeanCurtis-TRI · Pull Request #24725 · RobotLocomotion/drake

SeanCurtis-TRI · 2026-07-14T15:47:57Z

This includes:

The APIs on MultibodyPlant to declare and introspect surface velocities.
The requisite ports.
The parsing infrastructure to declare surface velocity in the model files.

It does not actually make use of the surface velocity in contact yet.

Relates #19599.

This change is

SeanCurtis-TRI · 2026-07-14T15:48:36Z

+(release notes: none) +a:@jwnimmer-tri for feature review please

SeanCurtis-TRI

Note: I've tagged this as none. I'm open to changing it to feature although it won't be meaningful until the next two PRs merge (implementing the contact responses).

@jwnimmer-tri I particularly undid something you suggested in MbP::SetDefaultState() in the previous PR. Look there and in SetRandomState(). Generally, there are some system level weirdnesses due to MbP's overriding of LeafSystem implementations.

@SeanCurtis-TRI made 1 comment.
Reviewable status: LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers (waiting on jwnimmer-tri).

jwnimmer-tri

First pass complete. This seems like a reasonable slice of the feature branch.

I particularly undid something you suggested in MbP::SetDefaultState() in the previous PR. Look there and in SetRandomState(). Generally, there are some system level weirdnesses due to MbP's overriding of LeafSystem implementations.

See discussions inline. I agree that we don't need to support a random distribution of initial displacement conditions, so the only question is the most maintainable way to implement what we need.

@jwnimmer-tri reviewed 13 files and all commit messages, and made 50 comments.
Reviewable status: 49 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers (waiting on SeanCurtis-TRI).

multibody/plant/multibody_plant.h line 289 at r1 (raw file):

surface_displacements

In various places this port name is plural (displacements) vs not (displacement). Everywhere should use the same spelling.

(Given that the input is uniformly plural, I imagine the goal is to use plural everywhere for output, too.)

multibody/parsing/detail_sdf_parser.cc line 1392 at r1 (raw file):

  const std::set<std::string> supported_link_elements{
      "drake:surface_velocity_axis",

The custom SDFormat and URDF elements need to be documented in parsing_doxygen.h.

multibody/parsing/detail_sdf_parser.cc line 1505 at r1 (raw file):

  // Parse link-level surface velocity axis (if present) and register.
  if (link_element->HasElement("drake:surface_velocity_axis")) {
    const Vector3d a = ToVector3(

nit It's not really defective since I was able to understand it basically immediately, but I do think we can afford to spell it out for absolute clarity.

Suggestion:

axis

multibody/parsing/detail_urdf_parser.cc line 262 at r1 (raw file):

  // Parse link-level surface velocity axis (if present) and register.
  // World body is excluded since it cannot have surface velocity.

If the user sets a surface axis on the world in the XML file, silently doing nothing is not a great outcome. IMO letting the plant throw is superior to that, but of course we could also use the Error() reporting for it, too.

multibody/parsing/detail_urdf_parser.cc line 267 at r1 (raw file):

        node->FirstChildElement("drake:surface_velocity_axis");
    if (sv_node != nullptr) {
      Eigen::Vector3d a;

nit Ditto

Suggestion:

axis

multibody/plant/BUILD.bazel line 1311 at r1 (raw file):

drake_cc_googletest(
    name = "surface_velocity_test",
    srcs = ["test/surface_velocity_test.cc"],

nit Don't list redundant filename -- it's already the default. (I thought our bzl code failed-fast on this mistake? Maybe that was only for Python tests.)

multibody/plant/BUILD.bazel line 1313 at r1 (raw file):

    srcs = ["test/surface_velocity_test.cc"],
    deps = [
        ":multibody_plant_config_functions",

Many of the deps here are not actually used by the test.

multibody/plant/multibody_plant.h line 1202 at r1 (raw file):

  /// carries a systems::BusValue whose signals set the surface speed for each
  /// body registered via SetSurfaceVelocityAxis(). Each signal's name is the
  /// fully qualified body name and its value is a finite `double` speed in m/s.

nit It's not totally obvious (even to me) exactly what this means. Is there a specific function that returns the exact necessary value which we could cite? Is it the body.scoped_name().get_full()? (Reading the implementation now, it seems like yes.)

Ditto on the output port.

Code quote:

fully qualified body name

multibody/plant/multibody_plant.h line 1210 at r1 (raw file):

  /// Returns the `"surface_displacements"` output port, which carries a
  /// systems::BusValue whose signals report the cumulative surface displacement
  /// (in metres) for each body registered via SetSurfaceVelocityAxis(). Each

nit freedom spelling

Suggestion:

meters

multibody/plant/multibody_plant.h line 2330 at r1 (raw file):

  ///    surface velocity registrations, but they can be connected by weld
  ///    joints to function as a single rigid body.
  ///  - **No random distribution for surface displacement.** The integrated

BTW This final bullet is not super important. If there is a way to un-bold it and/or trim down the verbiage, that would be helpful.

multibody/plant/multibody_plant.h line 2356 at r1 (raw file):

  /// Returns the surface-velocity axis for `body` expressed in the body frame
  /// B, or `std::nullopt` if `body` has not been registered. Works both
  /// before and after Finalize().

BTW It might be worth a reminder that because Set normalizes its argument, the return value might be different that what was passed to Set?

multibody/plant/multibody_plant.h line 3252 at r1 (raw file):

    SetDefaultMiscState(state);
    deformable_model().SetDefaultState(context, state);
  }

I believe a much simpler implementation is to just delegate all of this to the base class? Having to write out what it would have done longhand seems like a maintenance nightmare.

With this change (and the same idea in SetRandomState), I think SetDefaultMiscState can be removed.

Suggestion:

  void SetDefaultState(const systems::Context<T>& context,
                       systems::State<T>* state) const override {
    DRAKE_MBP_THROW_IF_NOT_FINALIZED();
    this->ValidateContext(context);
    this->ValidateCreatedForThisSystem(state);
    internal::MultibodyTreeSystem<T>::SetDefaultState(context, state);
    deformable_model().SetDefaultState(context, state);
  }

multibody/plant/multibody_plant.h line 3272 at r1 (raw file):

    // value.
    SetDefaultMiscState(state);
  }

BTW Ditto per SetDefaultState.

The difference here is that MbTS doesn't override the method, so we need to still call the MbT manually. (Or, I suppose, fix MbTS to also override this one.)

Suggestion:

  void SetRandomState(const systems::Context<T>& context,
                      systems::State<T>* state,
                      RandomGenerator* generator) const override {
    DRAKE_MBP_THROW_IF_NOT_FINALIZED();
    this->ValidateContext(context);
    this->ValidateCreatedForThisSystem(state);
    internal::MultibodyTreeSystem<T>::SetRandomState(context, state, generator);
    // TODO(...) This should be handled by MultibodyTreeSystem.
    internal_tree().SetRandomState(context, state, generator);
  }

multibody/plant/multibody_plant.h line 6036 at r1 (raw file):

  }

  // Computes the surface velocity for the body identified by `body_index`.

This new method is unusual. It is in the public section, but it doesn't have Doxygen and isn't bound in pydrake. (I think it was it supposed to be private, given the test fixture access pattern.)

multibody/plant/multibody_plant.h line 6115 at r1 (raw file):

    std::vector<Instance> instance;
    systems::OutputPortIndex geometry_pose;  // Declared in ctor, not Finalize.
    systems::OutputPortIndex surface_displacements;

We must document this as // Declared in ctor, not Finalize. (unless the code changes to not do that anymore).

multibody/plant/multibody_plant.h line 6917 at r1 (raw file):

  struct SurfaceVelocityEntry {
    std::string scoped_name;

Model instances can be renamed (pre-Finalize). We can't cache the scoped_name here unless we also invalidate/refresh the cache when a model instance gets renamed.

IMO trying to cache it is a premature optimization, and if we do need the scoped names to be cached, then the MbT should do that for us (post-Finalize), not the surface velocity axis table.

multibody/plant/multibody_plant.h line 6923 at r1 (raw file):

  // iteration order (ascending BodyIndex) defines displacement state indices.
  // Frozen at Finalize().
  std::map<BodyIndex, SurfaceVelocityEntry> surface_velocity_bodies_;

It's probably worth documenting our invariant that the axes stored here are always unit-length vectors.

multibody/plant/multibody_plant.h line 6928 at r1 (raw file):

  // integration (continuous mode only). Only valid when
  // surface_velocity_bodies_ is non-empty.
  int surface_displacement_continuous_state_start_{};

I'm not convinced that storing this as a runtime member field has utility that matches the implementation complexity. There are several places in the MbP code that already hard-code the assumption that surface displacement is the only z state. A halfway implementation of allowing other species of z state seems more confusing than helpful?

Instead, we could amend the comment in DeclareMiscContinuousStates() to advise future developers that they will need to start tracking z indices if/when they add more kinds of z state.

multibody/plant/multibody_plant.cc line 341 at r1 (raw file):

          systems::SystemTypeTag<MultibodyPlant>{},
          other.internal_tree().template CloneToScalar<T>(),
          other.is_discrete(), other.CalcNumRequiredMiscContinuousStates()) {

Re-deriving this number from first principles is overly brittle. We can just ask other what the right answer is.

Suggestion:

other.internal_tree().num_misc_continuous_states()

multibody/plant/multibody_plant.cc line 403 at r1 (raw file):

      surface_velocity_bodies_.emplace(
          body_index, SurfaceVelocityEntry{entry.scoped_name, entry.axis});
    }

The looping here seems gratuitous.

The only reason this loop can't be a simple assignment like all of the other modeling-parameter fields is because the SurfaceVelocityEntry is a nested class within the T-dependent plant.

If SurfaceVelocityEntry disappears entirely (because caching the scoped_name is unsound), then that will resolve it anyway.

If an entry struct sticks around, then move its declaration to the (internal) namespace instead of being plant-scoped, and then the simple assignment here will work.

Suggestion:

    surface_velocity_bodies_ = other.surface_velocity_bodies_;

multibody/plant/multibody_plant.cc line 846 at r1 (raw file):

template <typename T>
void MultibodyPlant<T>::SetSurfaceVelocityAxis(

BTW See #17734.

It would be plausible to check that the body is ours before computing with it. This is even more important if we're going to cache its scoped_name.

multibody/plant/multibody_plant.cc line 908 at r1 (raw file):

int MultibodyPlant<T>::num_misc_continuous_states() const {
  DRAKE_MBP_THROW_IF_NOT_FINALIZED();
  return CalcNumRequiredMiscContinuousStates();

Why isn't this simply calling the MbTS::num_misc_continuous_states()?

multibody/plant/multibody_plant.cc line 981 at r1 (raw file):

  return result;
}

In general for several of the new plant methods, there is a moderate level of code duplication for reading from the surface speed input port.

Consider a refactoring to add a CacheEntry for surface_speeds_vector which contains a vector of size ssize(surface_velocity_bodies_) with the speeds transcribed from the input port into state-order (or zero when absent).

Then all of the internal places that need the speed input would eval the cache entry and immediately be able to index exactly what they need.

multibody/plant/multibody_plant.cc line 989 at r1 (raw file):

  if (it == surface_velocity_bodies_.end()) {
    return Vector3<T>::Zero();
  }

BTW For readability, consider placing a named const-ref-alias here for it->second with the data type spelled out.

multibody/plant/multibody_plant.cc line 993 at r1 (raw file):

  // Read speed from the bus port. Default to zero if unconnected or absent.
  double speed = 0.0;
  const auto& port = get_surface_speeds_input_port();

nit This is the only call site that uses the public API sugar for this port (vs calling this->get_input_port(input_port_indices_.surface_speeds)). Be consistent.

Code quote:

get_surface_speeds_input_port()

multibody/plant/multibody_plant.cc line 1516 at r1 (raw file):

template <typename T>
int MultibodyPlant<T>::CalcNumRequiredMiscContinuousStates() const {

I anticipate that once all other defects are fixed, this function has no more callers and should be removed.

multibody/plant/multibody_plant.cc line 2495 at r1 (raw file):

    const T fn_AC = k * x * (1.0 + d * vn);

    if (fn_AC > 0) {

The changes to this function (including the TODO above) do not seem topical for this pull request.

multibody/plant/multibody_plant.cc line 3642 at r1 (raw file):

  }

  // Input "surface_speeds": one BusValue signal per registered body.

This comment is not accurate. (At least, if I take "registered body" to mean a body added with "AddBody".) Even if it means SetSurfaceVelocityAxis-non-null bodies, it's still imprecise because we allow bodies to be omitted, while this implies that all bodies must always be present.

Anyway, the whole thing seems spurious. The pattern in this function is to just name the port. The data type in the declaration speaks for itself, and for anything beyond that we have the Doxygen details.

Suggestion:

// Input "surface_speeds".

multibody/plant/multibody_plant.cc line 3650 at r1 (raw file):

template <typename T>
void MultibodyPlant<T>::DoCalcMiscDerivatives(

Unless I'm missing something, none of the state-update dynamics (zdot for continuous mode, abstract event handling for discrete mode) are unit tested at all.

multibody/plant/multibody_plant.cc line 3652 at r1 (raw file):

void MultibodyPlant<T>::DoCalcMiscDerivatives(
    const systems::Context<T>& context, systems::VectorBase<T>* zdot) const {
  if (CalcNumRequiredMiscContinuousStates() == 0) return;

This check is spurious now, I think? The MbTS promises not to call us when |z| = 0.

multibody/plant/multibody_plant.cc line 3654 at r1 (raw file):

  if (CalcNumRequiredMiscContinuousStates() == 0) return;

  zdot->SetZero();

BTW Plausibly the vector-wide "set zero" could be part of the early-return branch when the port is disconnected, instead of doing it unconditionally. When the port is connected, the loop already overwrites every value.

multibody/plant/multibody_plant.cc line 3662 at r1 (raw file):

  for (const auto& [_, entry] : surface_velocity_bodies_) {
    const AbstractValue* v = bus.Find(entry.scoped_name);
    zdot->SetAtIndex(i++, v != nullptr ? v->get_value<double>() : T(0));

I'm surprised that this actually compiled, since the operands to the ternary conditional have different types. Anyway, I think it would be helpful to unpack the nesting to help guide the reader with some names.

Suggestion:

const double speed = v != nullptr ? v->get_value<double>() : 0.0;
zdot->SetAtIndex(i++, speed);

multibody/plant/multibody_plant.cc line 3672 at r1 (raw file):

      state->template get_mutable_abstract_state<std::vector<double>>(
          surface_displacement_abstract_state_index_);
  // Initialize from the current context (not accumulated from next_state).

typo (I think this is trying to refer to the output argument, which is named state.)

Suggestion:

state

multibody/plant/multibody_plant.cc line 3717 at r1 (raw file):

      this->DeclarePeriodicUnrestrictedUpdateEvent(
          time_step_, 0.0, &MultibodyPlant<T>::CalcSurfaceDisplacementUpdate);
    }

The rationale for not declaring a Forced update event (to align with how q,v updates happen) needs to be explained in a comment.

multibody/plant/multibody_plant.cc line 4256 at r1 (raw file):

  // Output "surface_displacements": cumulative surface displacement per body.
  {

Hmm. If this is going to be part of DeclareSceneGraphPorts, should we color it green in the Doxgen overview?

I'm not sure, just thinking about what would be clearest for both users and maintainers.

(When I first looked at the rendered Doxygen, I wondered if it should be green, but then saw that it was a Finalize-time port so I figured maybe it's fine to keep it black. But if we're declaring it with the SG ports, maybe green is correct?)

multibody/plant/test/multibody_plant_test.cc line 2286 at r1 (raw file):

      {"geometry_pose", false},
      {"deformable_body_configuration", false},
      // Surface velocity group.

nit I buy the desire to put a comment here to separate it from the green group, but I'm not sure that calling the surface velocity a "group" is accurate.

The best I can come up with as an alternative is "Miscellaneous". Or something like "Continuing with the uncolored ports".

Suggestion:

// Miscellaneous.

multibody/plant/test/surface_velocity_test.cc line 2 at r1 (raw file):

#include <memory>
#include <optional>

nit Unused optional.

multibody/plant/test/surface_velocity_test.cc line 43 at r1 (raw file):

      auto& z = context->get_mutable_continuous_state()
                    .get_mutable_misc_continuous_state();
      for (int i = 0; i < z.size(); ++i) {

BTW I believe z.SetFromVector(VectorXd::Constant(z.size(), value)) would let us avoid writing out the loop by hand.

multibody/plant/test/surface_velocity_test.cc line 50 at r1 (raw file):

};

namespace {

Meta: In general, the test case organization here is not terribly smooth.

The tests that check ComputeSurfaceVelocity are reasonably well organized, but could perhaps be better grouped to help indicate "these are the tests of ComputeSurfaceVelocity". (The method name under test isn't part of either the test suite name or the test case name.)

The tests that check Get/Set of the axis are a lot more scattered and not organized in a way that tells a story about what/why/how we're checking.

multibody/plant/test/surface_velocity_test.cc line 76 at r1 (raw file):

}

// Fixture: finalized standalone plant, standalone context

It's important to document that this fixture provides a continuous-time plant. (A bunch of the plant logic branches on continuous vs discrete, so anything tested with this fixture only tests the continuous branches.)

Plausibly the test fixture name could reflect that distinction, as well?

multibody/plant/test/surface_velocity_test.cc line 78 at r1 (raw file):

// Fixture: finalized standalone plant, standalone context
//
// "belt" is registered with non-unit axis (2,0,0) to exercise normalization.

nit Clarity

Suggestion:

The "belt" body

multibody/plant/test/surface_velocity_test.cc line 80 at r1 (raw file):

// "belt" is registered with non-unit axis (2,0,0) to exercise normalization.
// "other" is intentionally left without surface velocity. Tests that need a
// speed wired to the surface_speeds port call FixValue().

typo

Suggestion:

FixBeltSpeed

multibody/plant/test/surface_velocity_test.cc line 118 at r1 (raw file):

// SetSurfaceVelocityAxis is pre-Finalize only.
TEST_F(SurfaceVelocityTest, SetAxisThrowsAfterFinalize) {

This test isn't 100% redundant with SetSurfaceVelocityAxisErrors, but it's close.

This tests setting an axis that was already set, and confirms it's a still error post-Finalize versus the other test case which is on a body without an existing axis.

It would be most clear if these were part of the same test case (to clarify the point), but if not then at least we need some docs explaining the difference.

multibody/plant/test/surface_velocity_test.cc line 125 at r1 (raw file):

// SetSurfaceVelocityAxis normalizes its input, can overwrite an existing
// registration, and the value survives Finalize().
GTEST_TEST(MultibodyPlantTest, SetSurfaceVelocityAxisNormalizesAndPersists) {

I find it weird that the test name is MultibodyPlantTest in a file not named multibody_plant_test.cc. We have two one-off instances of this pattern, but it's not typical.

Ditto throughout.

Code quote:

MultibodyPlantTest

multibody/plant/test/surface_velocity_test.cc line 188 at r1 (raw file):

}

TEST_F(SurfaceVelocityTest, SurfaceSpeedsPortName) {

We should also check the output port name.

multibody/plant/test/surface_velocity_test.cc line 256 at r1 (raw file):

  const Vector3d n_C_B(0, 0, 1);
  double speed = 0.75;
  const Vector3d v_ss_B_expected(0, -speed, 0);

nit Why is this value established so far away from its first use?

bindings/pydrake/multibody/plant_py.cc line 254 at r1 (raw file):

            py::arg("model_instance"), cls_doc.num_actuated_dofs.doc_1args);
    // Plant-owned state variables - miscellaneous continuous state.
    cls  // BR

This method should be bound in the same order as it is declared in the header file. So, this shouldn't have a new section break (unless we also re-jigger the header file, but that is probably not a good idea).

~~Yes, the "Forwarded methods from MultibodyTree" introduction is not accurate for this method, but it was already an fuzzy approximation when we added bindings for num_constraints() and such.~~ (Edited to add: This MbP method actually should be forwarded from MbTS as its implementation inside MbP, so in fact the section title will be accurate.)

bindings/pydrake/multibody/plant_py.cc line 1197 at r1 (raw file):

            py_rvp::reference_internal,
            cls_doc.get_surface_displacement_output_port.doc)
        .def("SetSurfaceVelocityAxis", &Class::SetSurfaceVelocityAxis,

For better or worse, putting the GetAxis/SetAxis into the "ports" section is inconsistent with existing practice. Only ports should live here.

I'd say "// Property accessors" is a probably good home for the Get/Set, or maybe there's somewhere even better.

bindings/pydrake/multibody/test/plant_test.py line 1858 at r1 (raw file):

        body = plant.AddRigidBody("body")

        # confirm configuration of surface velocity axis.

nit Capitalize (GSG).

SeanCurtis-TRI · 2026-07-15T14:57:17Z

multibody/plant/multibody_plant.h line 1202 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

nit It's not totally obvious (even to me) exactly what this means. Is there a specific function that returns the exact necessary value which we could cite? Is it the body.scoped_name().get_full()? (Reading the implementation now, it seems like yes.)

Ditto on the output port.

This is problematic on two levels:

MultibodyPlant doesn't register geometry::Frames with the scoped name of the corresponding body. Instead, it uses the bespoke GetScopedName() in multibody_plant.cc. This is an approximation of ScopedName -- it elides model instance names for the world and default model instances. That one-off function introduces the term "fully qualified" (which, in turn, is appears in the documentation of multiple multibody element types).
MeshcatVisualizer ultimately needs to map geometry::Frame back to RigidBody so that it can look up the surface velocity axis. I've just noted in my implementation, there's a defect there that isn't covered by test. MeshcatVisualizer is using RigidBody::scoped_name() and is assuming that the geometry::Frame::name() will match (which it does in many cases...but not all).

This is related to the name mangling referenced in #9128. I don't think any of us has actually figured out what the obstacle is to resolving that issue.

So, I have two action items:

Augment the meschat visualizer test to confirm the bug that I now perceive is real or not.
Possibly poke into resolving geometry: More SceneGraph introspection - get shapes and poses for geometries #9128 prior to this PR so that the geometry frame's name is simply the body's scoped name.

Thoughts?

jwnimmer-tri

@jwnimmer-tri made 1 comment.
Reviewable status: 49 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers (waiting on SeanCurtis-TRI).

multibody/plant/multibody_plant.h line 1202 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

This is problematic on two levels:

MultibodyPlant doesn't register geometry::Frames with the scoped name of the corresponding body. Instead, it uses the bespoke GetScopedName() in multibody_plant.cc. This is an approximation of ScopedName -- it elides model instance names for the world and default model instances. That one-off function introduces the term "fully qualified" (which, in turn, is appears in the documentation of multiple multibody element types).

MeshcatVisualizer ultimately needs to map geometry::Frame back to RigidBody so that it can look up the surface velocity axis. I've just noted in my implementation, there's a defect there that isn't covered by test. MeshcatVisualizer is using RigidBody::scoped_name() and is assuming that the geometry::Frame::name() will match (which it does in many cases...but not all).

This is related to the name mangling referenced in #9128. I don't think any of us has actually figured out what the obstacle is to resolving that issue.

So, I have two action items:

Augment the meschat visualizer test to confirm the bug that I now perceive is real or not.

Possibly poke into resolving #9128 prior to this PR so that the geometry frame's name is simply the body's scoped name.

Thoughts?

I don't think I have much to add. The explorations seem good.

One way to move forward would be to document and treat the Body's scoped_name() as authoritative (like we already do here) and leave the visualization problem for another day.

If the only gap is really down to the world and default model instances, another solution could be to forbid setting surface velocities on bodies in those instances (throwing in the plant). We could relax that restriction later once the naming is sorted out. Possibly this makes unit testing too annoying, though.

jwnimmer-tri

@jwnimmer-tri made 1 comment.
Reviewable status: 49 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers (waiting on SeanCurtis-TRI).

multibody/plant/multibody_plant.h line 1202 at r1 (raw file):

MeshcatVisualizer is using RigidBody::scoped_name() and is assuming that the geometry::Frame::name() will match (which it does in many cases...but not all).

Actually, can't this solve by just fixing the (draft) code in MeshcatVisualizer?

Right now, it just copies the axes dict with the body names (making the assumption that body name matches frame name, as you note). However, it that loop I believe it has access to all the data it needs to convert from body name to frame name while transcribing the dictionary?

jwnimmer-tri

@jwnimmer-tri made 1 comment.
Reviewable status: 49 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers (waiting on SeanCurtis-TRI).

multibody/plant/multibody_plant.h line 1202 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

MeshcatVisualizer is using RigidBody::scoped_name() and is assuming that the geometry::Frame::name() will match (which it does in many cases...but not all).

Actually, can't this solve by just fixing the (draft) code in MeshcatVisualizer?

Right now, it just copies the axes dict with the body names (making the assumption that body name matches frame name, as you note). However, it that loop I believe it has access to all the data it needs to convert from body name to frame name while transcribing the dictionary?

(It could even key its dict on geometry::FrameId instead of name, for even faster lookups.)

jwnimmer-tri

@jwnimmer-tri made 1 comment.
Reviewable status: 50 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on SeanCurtis-TRI).

multibody/plant/multibody_plant.h line 2274 at r2 (raw file):

  /// @anchor mbp_surface_velocity
  /// @name               Surface velocity
  ///

Somewhere in the MbP doxygen, we need a @warning that the contact dynamics are not actually implemented yet. (And we'll yank out that comment when we add the dynamics.)

SeanCurtis-TRI

Phew....

First, let me apologize for letting so much of that through.

Second, there are a couple of details that still bear scrutiny -- the scalar-converting constructor is a thorn in my side. See my notes below.

@SeanCurtis-TRI made 37 comments and resolved 18 discussions.
Reviewable status: 32 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers (waiting on jwnimmer-tri).

multibody/parsing/detail_sdf_parser.cc line 1392 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

The custom SDFormat and URDF elements need to be documented in parsing_doxygen.h.

Done.

multibody/parsing/detail_urdf_parser.cc line 262 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

If the user sets a surface axis on the world in the XML file, silently doing nothing is not a great outcome. IMO letting the plant throw is superior to that, but of course we could also use the Error() reporting for it, too.

Higher in this file, where a user might attempt to assign mass properties to the world body, we dispatch a warning about ignoring. I'm going to ape the same behavior here.

multibody/plant/BUILD.bazel line 1313 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Many of the deps here are not actually used by the test.

Done.

multibody/plant/multibody_plant.h line 289 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

surface_displacements

In various places this port name is plural (displacements) vs not (displacement). Everywhere should use the same spelling.

(Given that the input is uniformly plural, I imagine the goal is to use plural everywhere for output, too.)

I don't find any instances of the string surface_displacement, but 11 instances of the plural. Are you sure?

multibody/plant/multibody_plant.h line 1202 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

(It could even key its dict on geometry::FrameId instead of name, for even faster lookups.)

For now, I think I'll take your last suggestion.

Ultimately, the challenge is one of timing. The coordination between what it sees from QueryObject when evaluating its publish event can only be implicitly coordinated with what it sees from MultibodyPlant at construction.

We have three things we need to correlate:

Bus signal names. The input speeds should align with the output displacements (for reasons of sanity). And they should simply be scoped names (e.g., body.scoped_name().to_string()).
MeshcatVisualizer needs to stash the axes from MbP at construction.
- We can/should create a map from FrameId to (axis value, body bus signal name). The latter protects us from the weird logic in MbP that picks geometry::Frame names that aren't necessarily body.scoped_name().
- This relies on the fact that the finalized MbP knows the FrameId for its bodies and they can't change, because the bodies can't change.
MeshcatVisualizer needs to pull displacements from its input port at event evaluation.
- Based on the FrameId, it looks up the bus signal name and pulls the current displacement. Two look-ups, but not the end of the world.

multibody/plant/multibody_plant.h line 1210 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

nit freedom spelling

lol

multibody/plant/multibody_plant.h line 3252 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

I believe a much simpler implementation is to just delegate all of this to the base class? Having to write out what it would have done longhand seems like a maintenance nightmare.

With this change (and the same idea in SetRandomState), I think SetDefaultMiscState can be removed.

Oh, that's nice.

multibody/plant/multibody_plant.h line 6036 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

This new method is unusual. It is in the public section, but it doesn't have Doxygen and isn't bound in pydrake. (I think it was it supposed to be private, given the test fixture access pattern.)

Done.

multibody/plant/multibody_plant.h line 6115 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

We must document this as // Declared in ctor, not Finalize. (unless the code changes to not do that anymore).

Done.

multibody/plant/multibody_plant.h line 6917 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Model instances can be renamed (pre-Finalize). We can't cache the scoped_name here unless we also invalidate/refresh the cache when a model instance gets renamed.

IMO trying to cache it is a premature optimization, and if we do need the scoped names to be cached, then the MbT should do that for us (post-Finalize), not the surface velocity axis table.

I've deferred caching the scoped name until finalize.

multibody/plant/multibody_plant.h line 6923 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

It's probably worth documenting our invariant that the axes stored here are always unit-length vectors.

Probably not. SurfaceVelocityEntry is defined just before this and literally five lines above here it says "unit-length". If the value of the map were simply a Vector3d, I'd absolutely agree. But as a reader has to go find out what an entry is anyways, that being documented seems sufficient.

multibody/plant/multibody_plant.h line 6928 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

I'm not convinced that storing this as a runtime member field has utility that matches the implementation complexity. There are several places in the MbP code that already hard-code the assumption that surface displacement is the only z state. A halfway implementation of allowing other species of z state seems more confusing than helpful?

Instead, we could amend the comment in DeclareMiscContinuousStates() to advise future developers that they will need to start tracking z indices if/when they add more kinds of z state.

I hadn't realized it was halfway. I'll go ahead and strip it out.

multibody/plant/multibody_plant.h line 2274 at r2 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Somewhere in the MbP doxygen, we need a @warning that the contact dynamics are not actually implemented yet. (And we'll yank out that comment when we add the dynamics.)

Done.

multibody/plant/multibody_plant.cc line 341 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Re-deriving this number from first principles is overly brittle. We can just ask other what the right answer is.

I've stripped it out. What do you think of the alternative?

multibody/plant/multibody_plant.cc line 403 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

The looping here seems gratuitous.

The only reason this loop can't be a simple assignment like all of the other modeling-parameter fields is because the SurfaceVelocityEntry is a nested class within the T-dependent plant.

If SurfaceVelocityEntry disappears entirely (because caching the scoped_name is unsound), then that will resolve it anyway.

If an entry struct sticks around, then move its declaration to the (internal) namespace instead of being plant-scoped, and then the simple assignment here will work.

Entry moved to internal. It's necessary to keep around because, post finalization, it is very convenient to have the name cached (instead of rebuilding the string over and over).

multibody/plant/multibody_plant.cc line 846 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

BTW See #17734.

It would be plausible to check that the body is ours before computing with it. This is even more important if we're going to cache its scoped_name.

Despite my participation in that conversation, I had long since lost the details.

multibody/plant/multibody_plant.cc line 908 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Why isn't this simply calling the MbTS::num_misc_continuous_states()?

Because of the role of CalcNumRequiredMiscContinuousStates() - it seemed to make sense to have MbP have a consistent view of the question.

However, I've attempted solving the fundamental problem that CalcNumRequiredMiscContinuousStates() was solving so I no longer need it.

multibody/plant/multibody_plant.cc line 981 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

In general for several of the new plant methods, there is a moderate level of code duplication for reading from the surface speed input port.

Consider a refactoring to add a CacheEntry for surface_speeds_vector which contains a vector of size ssize(surface_velocity_bodies_) with the speeds transcribed from the input port into state-order (or zero when absent).

Then all of the internal places that need the speed input would eval the cache entry and immediately be able to index exactly what they need.

Right now I'm disinclined.

It doesn't seem particularly worth while. In this PR we have two methods that would benefit from what you propose: they iterate through the connected speeds in order.

DoCalcMiscDerivatives()
CalcSurfaceDisplacementUpdate()

However, the former only applies to continuous plants and the latter applies only to discrete plants. So, there wouldn't particularly be a runtime cache benefit. Only a code-consolidation-y kind of benefit.

In future PRs where we use the speeds for contact, our access pattern is much different -- we don't want them in state-order, we want random access based on identity of the body. So, the cache entry wouldn't help them.

When we get to all the accesses we can revisit this topic, but for now, I'm going to defer this.

multibody/plant/multibody_plant.cc line 993 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

nit This is the only call site that uses the public API sugar for this port (vs calling this->get_input_port(input_port_indices_.surface_speeds)). Be consistent.

Done.

multibody/plant/multibody_plant.cc line 1516 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

I anticipate that once all other defects are fixed, this function has no more callers and should be removed.

This is an admittedly weird thing. It was born of the following:

MbTS can only report misc states after finalization.
We have a unit test for scalar converting a non-finalized plant.
Historically, the error dispatched in this case clearly communicated something about scalar converting a non-finalized plant.
A throw--on-unfinalized query into num misc states would interfere with the behavior of the scalar coverter. Specifically, instead of an error message in (3), we'd get one complaining about accessing the number of misc states. Far less meaningful. This non-throwing query satisfied that.

I've now removed this method and solved that problem using an alternate solution in the scalar-converting copy constructor.

multibody/plant/multibody_plant.cc line 2495 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

The changes to this function (including the TODO above) do not seem topical for this pull request.

Done.

multibody/plant/multibody_plant.cc line 3642 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

This comment is not accurate. (At least, if I take "registered body" to mean a body added with "AddBody".) Even if it means SetSurfaceVelocityAxis-non-null bodies, it's still imprecise because we allow bodies to be omitted, while this implies that all bodies must always be present.

Anyway, the whole thing seems spurious. The pattern in this function is to just name the port. The data type in the declaration speaks for itself, and for anything beyond that we have the Doxygen details.

Done.

multibody/plant/multibody_plant.cc line 3650 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Unless I'm missing something, none of the state-update dynamics (zdot for continuous mode, abstract event handling for discrete mode) are unit tested at all.

I believe they are now.

multibody/plant/multibody_plant.cc line 3652 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

This check is spurious now, I think? The MbTS promises not to call us when |z| = 0.

Done.

multibody/plant/multibody_plant.cc line 3662 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

I'm surprised that this actually compiled, since the operands to the ternary conditional have different types. Anyway, I think it would be helpful to unpack the nesting to help guide the reader with some names.

Done.

multibody/plant/multibody_plant.cc line 3717 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

The rationale for not declaring a Forced update event (to align with how q,v updates happen) needs to be explained in a comment.

Done

multibody/plant/multibody_plant.cc line 4256 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Hmm. If this is going to be part of DeclareSceneGraphPorts, should we color it green in the Doxgen overview?

I'm not sure, just thinking about what would be clearest for both users and maintainers.

(When I first looked at the rendered Doxygen, I wondered if it should be green, but then saw that it was a Finalize-time port so I figured maybe it's fine to keep it black. But if we're declaring it with the SG ports, maybe green is correct?)

Reading up on what "green" means, I see it was an error to put this port declaration here. It doesn't "communicate with SceneGraph". So, I'm simply moving it.

multibody/plant/test/multibody_plant_test.cc line 2286 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

nit I buy the desire to put a comment here to separate it from the green group, but I'm not sure that calling the surface velocity a "group" is accurate.

The best I can come up with as an alternative is "Miscellaneous". Or something like "Continuing with the uncolored ports".

The group has certainly gotten smaller since I first wrote that. :)

multibody/plant/test/surface_velocity_test.cc line 50 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Meta: In general, the test case organization here is not terribly smooth.

The tests that check ComputeSurfaceVelocity are reasonably well organized, but could perhaps be better grouped to help indicate "these are the tests of ComputeSurfaceVelocity". (The method name under test isn't part of either the test suite name or the test case name.)

The tests that check Get/Set of the axis are a lot more scattered and not organized in a way that tells a story about what/why/how we're checking.

Reorged.

multibody/plant/test/surface_velocity_test.cc line 76 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

It's important to document that this fixture provides a continuous-time plant. (A bunch of the plant logic branches on continuous vs discrete, so anything tested with this fixture only tests the continuous branches.)

Plausibly the test fixture name could reflect that distinction, as well?

I dabbled with injecting "continuous" into the test fixture. At the end of the day, it simply felt that it was elevating the choice of plant mode to a level that far exceeded its actual importance. But I've clarified things in other ways.

multibody/plant/test/surface_velocity_test.cc line 118 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

This test isn't 100% redundant with SetSurfaceVelocityAxisErrors, but it's close.

This tests setting an axis that was already set, and confirms it's a still error post-Finalize versus the other test case which is on a body without an existing axis.

It would be most clear if these were part of the same test case (to clarify the point), but if not then at least we need some docs explaining the difference.

Should be resolved now.

multibody/plant/test/surface_velocity_test.cc line 125 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

I find it weird that the test name is MultibodyPlantTest in a file not named multibody_plant_test.cc. We have two one-off instances of this pattern, but it's not typical.

Ditto throughout.

Done.

multibody/plant/test/surface_velocity_test.cc line 188 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

We should also check the output port name.

Done.

multibody/plant/test/surface_velocity_test.cc line 256 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

nit Why is this value established so far away from its first use?

Changed in the refactor.

bindings/pydrake/multibody/plant_py.cc line 254 at r1 (raw file):

This MbP method actually should be forwarded from MbTS...

That's not literally true. MbP provides a "must be finalized guard" that MbTS can't have (at least in the current version -- re: the discussion on the scalar conversion constructor).

And while the principle of "bound in the same order" is one I generally subscribe to, this file doesn't have that property in the first place. For example, just prior to this group is num_actuated_dofs(). In the plant header, those methods follow num_actuators(). But that is not the case for the bindings. In fact, the num_actuators() has jumped up to the top of this section.

To say nothing that the fact that the bindings have introduced an arbitrary grouping strategy that is independent of the header layout is probably defective in itself.

So, I lob this back to you:

If you can convince me to push the finalize guard into MbTS, then I'll make this simply forward and put it in relative order in that group.

bindings/pydrake/multibody/plant_py.cc line 1197 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

For better or worse, putting the GetAxis/SetAxis into the "ports" section is inconsistent with existing practice. Only ports should live here.

I'd say "// Property accessors" is a probably good home for the Get/Set, or maybe there's somewhere even better.

Done.

jwnimmer-tri

Checkpoint on all except the test changes.

@jwnimmer-tri reviewed 13 files and all commit messages, made 12 comments, and resolved 22 discussions.
Reviewable status: 14 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers (waiting on SeanCurtis-TRI).

multibody/plant/multibody_plant.h line 289 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

I don't find any instances of the string surface_displacement, but 11 instances of the plural. Are you sure?

That's because you fixed most them already in r2->r3 (e.g., the output port getter method name).

The only remaining question (and I could go either way on this) is about the new abstract state (AbstractStateIndex surface_displacement_abstract_state_index_) and its dynamics method (CalcSurfaceDisplacementOutput). They are internally consistent now (both singular) but there would be a reasonable case for making them plural to match the output port they are feeding into.

multibody/plant/multibody_plant.h line 1202 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

For now, I think I'll take your last suggestion.

Ultimately, the challenge is one of timing. The coordination between what it sees from QueryObject when evaluating its publish event can only be implicitly coordinated with what it sees from MultibodyPlant at construction.

We have three things we need to correlate:

Bus signal names. The input speeds should align with the output displacements (for reasons of sanity). And they should simply be scoped names (e.g., body.scoped_name().to_string()).

MeshcatVisualizer needs to stash the axes from MbP at construction.

We can/should create a map from FrameId to (axis value, body bus signal name). The latter protects us from the weird logic in MbP that picks geometry::Frame names that aren't necessarily body.scoped_name().

This relies on the fact that the finalized MbP knows the FrameId for its bodies and they can't change, because the bodies can't change.

MeshcatVisualizer needs to pull displacements from its input port at event evaluation.

Based on the FrameId, it looks up the bus signal name and pulls the current displacement. Two look-ups, but not the end of the world.

This is good overall, but the API text here (and below) now says "scoped body name". That's still a bit imprecise and might not register as a term of art for new users. I would like to see at least scoped_name() to indicate we're talking about a specific function, or perhaps even a full RigidBody<T>::scoped_name() hyperlink.

multibody/plant/multibody_plant.h line 6115 at r1 (raw file):

unless the code changes to not do that anymore

The code changed to not do that anymore. It's declared during Finalize now, so the new comment is inaccurate.

multibody/plant/multibody_plant.cc line 341 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

I've stripped it out. What do you think of the alternative?

SGTM

multibody/plant/multibody_plant.cc line 981 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

Right now I'm disinclined.

It doesn't seem particularly worth while. In this PR we have two methods that would benefit from what you propose: they iterate through the connected speeds in order.

DoCalcMiscDerivatives()

CalcSurfaceDisplacementUpdate()

However, the former only applies to continuous plants and the latter applies only to discrete plants. So, there wouldn't particularly be a runtime cache benefit. Only a code-consolidation-y kind of benefit.

In future PRs where we use the speeds for contact, our access pattern is much different -- we don't want them in state-order, we want random access based on identity of the body. So, the cache entry wouldn't help them.

When we get to all the accesses we can revisit this topic, but for now, I'm going to defer this.

Indeed, I wasn't concerned about the performance aspect of it, rather only the "don't repeat yourself" part to make maintenance easier.

Yes those two methods could benefit, but so could ComputeSurfaceVelocity if it had a way to look up a body_index and know its offset within the speeds array, and that offset integer could be part of its SurfaceVelocityEntry struct, set during Finalize just like the names cache. This moots your objection in terms of body-indexing (we need to look up the body in the Entry map no matter what, to get the axis).

In any case, it's not defective yet (only rule of two) but in the past we've found that consolidating input port access to a single, preprocessed cache entry that applied the default values was easier for maintenance.

multibody/plant/multibody_plant.cc line 3717 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

Done

I don't understand the justification. I'm not entirely sure that I fully comprehend what it's try to say, but all of the ways I can imagine reading it seem like they would apply equally well to any other kind of declared state (DiscreteStepMemory or the numeric discrete state xd) so it doesn't tell me why the displacement in particular should be treated differently.

The use case for forced events is that the user is e.g. optimizing a discrete-time trajectory by manually creating the "state" at different time points by using forced update events. The surface displacement should update alongside all of the other kinematics, with the same species of events, absent some specific reason why it's characteristically different that other kinematic state.

bindings/pydrake/multibody/plant_py.cc line 254 at r1 (raw file):

That's not literally true. MbP provides a "must be finalized guard" ...

Many MbP methods that forward to MbT(S) add extra sanity checks before passing along to the MbT(S). The underlying implementation logic to compute the right answer still lives with the MbTS. See, e.g., AddJoint, but admittedly that is not a simple "num_foo" accessor like this one. In any case, my second paragraph wasn't strictly necessary to understand the problem here.

The defects, from my first paragraph, were "should be bound in the same order as it is declared in the header file" and "shouldn't have a new section break".

You're right that bits of existing code break the rules like num_actuated_dofs, but that doesn't chance the fact that the correct position for this new additional method within the bindings file is not in doubt -- it should appear between num_velocities and num_multibody_states, just like in the header. It definitely must not have a wholly-new subsection title opener "Plant-owned state variables", since that is just made up from whole cloth with no relationship to the MbP.h header file grouping or method ordering.

multibody/plant/multibody_plant.cc line 342 at r3 (raw file):

          other.internal_tree().template CloneToScalar<T>(),
          other.is_discrete(),
          static_cast<const internal::MultibodyTreeSystem<U>&>(other)

BTW I find it slightly nicer to see an implicit cast, rather than a static_cast I need to reason out to be implicit:

diff --git a/multibody/plant/BUILD.bazel b/multibody/plant/BUILD.bazel
index 4b9164019a..1f01e01a86 100644
--- a/multibody/plant/BUILD.bazel
+++ b/multibody/plant/BUILD.bazel
@@ -166,6 +166,7 @@ drake_cc_library(
         ":contact_properties",
         ":desired_state_input",
         ":hydroelastic_traction_calculator",
+        "@abseil_cpp_internal//absl/base",
     ],
 )
 
diff --git a/multibody/plant/multibody_plant.cc b/multibody/plant/multibody_plant.cc
index 202c29803d..6c60b4bfd5 100644
--- a/multibody/plant/multibody_plant.cc
+++ b/multibody/plant/multibody_plant.cc
@@ -10,6 +10,7 @@
 #include <typeinfo>
 #include <vector>
 
+#include "absl/base/casts.h"
 #include <fmt/ranges.h>
 
 #include "drake/common/drake_assert.h"
@@ -339,7 +340,7 @@ MultibodyPlant<T>::MultibodyPlant(const MultibodyPlant<U>& other)
           systems::SystemTypeTag<MultibodyPlant>{},
           other.internal_tree().template CloneToScalar<T>(),
           other.is_discrete(),
-          static_cast<const internal::MultibodyTreeSystem<U>&>(other)
+          absl::implicit_cast<const internal::MultibodyTreeSystem<U>&>(other)
               .num_misc_continuous_states()) {
   DRAKE_THROW_UNLESS(other.is_finalized());

multibody/plant/multibody_plant.cc line 3639 at r3 (raw file):

    const double speed = v != nullptr ? v->get_value<double>() : 0.0;
    zdot->SetAtIndex(++i, speed);
  }

nit I found the post-increment here actually more readable. (You could make it its own statement instead of in the argument, if you prefer. Really, the problem is that we don't get the Pythonic enumerate until the next version of C++ ranges.)

I also think the comment isn't super clear. It's not obvious that the "if" is talking about a future hypothetical code change, rather than some runtime condition in the existing code that we're about to check for.

Ditto for the other similar instance, below.

Suggestion:

  const auto& bus = port.template Eval<systems::BusValue>(context);
  int i = 0;
  for (const auto& [_, entry] : surface_velocity_bodies_) {
    const AbstractValue* v = bus.Find(entry.scoped_name);
    const double speed = v != nullptr ? v->get_value<double>() : 0.0;
    zdot->SetAtIndex(i++, speed);
  }

multibody/plant/multibody_plant.cc line 3833 at r3 (raw file):

    DependencyTicket prereq = this->nothing_ticket();
    if (is_discrete()) {
      if (!surface_velocity_bodies_.empty()) {

BTW I could imagine swapping the empty check to be the outmost if-guard, so that z_ticket wasn't used when there are no surface velocity bodies. Having a dependency edge isn't free (invalidating the whole xc, which happens frequently, would need to invalidate the output port cache entry).

Code quote:

if (!surface_velocity_bodies_.empty()) {

multibody/tree/multibody_tree_system.h line 96 at r3 (raw file):

  bool is_discrete() const { return is_discrete_; }

  /** Returns the size of the continuous miscellaneous state vector z for this

nit This method, even though public, is still on a namespace internal class so should still use the non-Doxygen /* opener.

SeanCurtis-TRI

Cleaned up

@SeanCurtis-TRI made 6 comments and resolved 4 discussions.
Reviewable status: 10 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers (waiting on jwnimmer-tri).

multibody/plant/multibody_plant.h line 289 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

That's because you fixed most them already in r2->r3 (e.g., the output port getter method name).

The only remaining question (and I could go either way on this) is about the new abstract state (AbstractStateIndex surface_displacement_abstract_state_index_) and its dynamics method (CalcSurfaceDisplacementOutput). They are internally consistent now (both singular) but there would be a reasonable case for making them plural to match the output port they are feeding into.

I've fixed it in some of the downstream code; might as well fix it in the state as well.

multibody/plant/multibody_plant.h line 1202 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

This is good overall, but the API text here (and below) now says "scoped body name". That's still a bit imprecise and might not register as a term of art for new users. I would like to see at least scoped_name() to indicate we're talking about a specific function, or perhaps even a full RigidBody<T>::scoped_name() hyperlink.

Done.

multibody/plant/multibody_plant.h line 6115 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

unless the code changes to not do that anymore

The code changed to not do that anymore. It's declared during Finalize now, so the new comment is inaccurate.

Done.

multibody/plant/multibody_plant.cc line 3717 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

I don't understand the justification. I'm not entirely sure that I fully comprehend what it's try to say, but all of the ways I can imagine reading it seem like they would apply equally well to any other kind of declared state (DiscreteStepMemory or the numeric discrete state xd) so it doesn't tell me why the displacement in particular should be treated differently.

The use case for forced events is that the user is e.g. optimizing a discrete-time trajectory by manually creating the "state" at different time points by using forced update events. The surface displacement should update alongside all of the other kinematics, with the same species of events, absent some specific reason why it's characteristically different that other kinematic state.

I hadn't considered your outlined workflow.

In my mind, the discrete update state is essentially just a counter. It doesn't measure kinematic quantities so much as report how many times the update has been invoked (with some awkward weighting on the counter). Other state updates may have interesting dependencies such that if you redundantly invoked the event without changing the inputs the result wouldn't change. This state does change because you invoked it and for literally no other reason. It's particularly obnoxious if the context's time is anything other than the last time stamp + MbP::time_step_. Then you get a change with the illusion of meaning.

However, if a user is responsible (setting state and setting time to t + delta_t) and only force updates once, they can indeed use forced updates to also realize an appropriate advance in surface displacement.

bindings/pydrake/multibody/plant_py.cc line 254 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

That's not literally true. MbP provides a "must be finalized guard" ...

Many MbP methods that forward to MbT(S) add extra sanity checks before passing along to the MbT(S). The underlying implementation logic to compute the right answer still lives with the MbTS. See, e.g., AddJoint, but admittedly that is not a simple "num_foo" accessor like this one. In any case, my second paragraph wasn't strictly necessary to understand the problem here.

The defects, from my first paragraph, were "should be bound in the same order as it is declared in the header file" and "shouldn't have a new section break".

You're right that bits of existing code break the rules like num_actuated_dofs, but that doesn't chance the fact that the correct position for this new additional method within the bindings file is not in doubt -- it should appear between num_velocities and num_multibody_states, just like in the header. It definitely must not have a wholly-new subsection title opener "Plant-owned state variables", since that is just made up from whole cloth with no relationship to the MbP.h header file grouping or method ordering.

Ah....so your interpretation of "Forwarded methods from MultibodyTree." is not as literal as mine. In my first pass, I'd sampled the block and it seemed clear to me that the documentation was a perfect characterization of the sampled methods and not this API. I've now done a complete survey and note the following exceptions:

ctor
time_step() - time step is owned by the plant.
num_constraints() - the plant owns all of the constraints.

I'm happy to defer to your defect over my defect. I avoiding injecting an API that would contradict the documentation. As it is clear to me that the contradiction already exists, I no longer feel a pang in introducing that inconsistency.

jwnimmer-tri

@jwnimmer-tri reviewed 5 files and all commit messages, and resolved 5 discussions.
Reviewable status: 5 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers (waiting on SeanCurtis-TRI).

jwnimmer-tri

feature.

Consider tagging Sherm for platform, to get some extra SME on the plant changes, and because Grant already got assigned a lot of reviews today.

@jwnimmer-tri reviewed 4 files and all commit messages, made 10 comments, and resolved 3 discussions.
Reviewable status: 9 unresolved discussions, needs at least two assigned reviewers (waiting on SeanCurtis-TRI).

multibody/plant/test/surface_velocity_test.cc line 118 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

Should be resolved now.

It's still the case that MultibodyPlantTest.SetSurfaceVelocityAxisErrors and SurfaceVelocityTest.GetSetSurfaceVelocityAxis both check the "can't set post-finalize" behavior. Perhaps the former should be fully folded into the latter? I think that just means moving the world-body case into this test.

multibody/plant/test/surface_velocity_test.cc line 125 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

Done.

We still have GTEST_TEST(MultibodyPlantTest, SetSurfaceVelocityAxisErrors) (above).

multibody/plant/multibody_plant.cc line 3629 at r5 (raw file):

  if (!port.HasValue(context)) {
    zdot->SetZero();
    return;

If I remove this line, no tests fail.

(This is an example where unifying input port handling default values with a cache entry would have helped.)

multibody/plant/test/surface_velocity_test.cc line 21 at r5 (raw file):

namespace multibody {

// Exposes ComputeSurfaceVelocity for testing.

nit This class overview is a bit too specific(/limiting) now.

multibody/plant/test/surface_velocity_test.cc line 137 at r5 (raw file):

//  G1. Can be called pre-Finalize().
//  G2. Return nullopt for unregistered body.
//  G3. Value still present post finalize.

nit Consistency. Ditto on S5.

Actually in the test body there is also some inconsistency. The main point is to be consistent, but I guess we also shouldn't say finalize() lowercase like a method when that isn't the method's name. So either {pre|post}-finalize (no parens) or else {pre|post}-Finalize() are the valid choices to become consistent with.

Suggestion:

post-Finalize()

multibody/plant/test/surface_velocity_test.cc line 196 at r5 (raw file):

      plant_.CreateDefaultContext();

  // Fixes a constant speed for the given body on the surface_speeds port.

This lambda seems somewhat redundant with the FixSurfaceSpeed function. (If we want to curry the plant and context args, that's fine, but at least the lambda should call the other helper function.)

multibody/plant/test/surface_velocity_test.cc line 276 at r5 (raw file):

    const AbstractValue* value = output.Find(belt_->scoped_name().to_string());
    DRAKE_DEMAND(value != nullptr);
    return value->get_value<double>();

This should probably just be re-using the ReadSurfaceDisplacement helper function, with hard-coded args?

Code quote:

    const auto& output =
        plant_->get_surface_displacements_output_port().Eval<systems::BusValue>(
            *context_);
    const AbstractValue* value = output.Find(belt_->scoped_name().to_string());
    DRAKE_DEMAND(value != nullptr);
    return value->get_value<double>();

multibody/plant/test/surface_velocity_test.cc line 344 at r5 (raw file):

  MultibodyPlantTester::CallSurfaceDisplacementUpdate(
      *plant_, *context_, &scratch->get_mutable_state());
  EXPECT_NEAR(ReadSurfaceDisplacement(*plant_, *scratch, *belt_),

Here and throughout, we have a ReadBeltDisplacement helper function available; why not use it?

(It might also be worth adding ReadRollerDisplacement helper to match.)

((Or plausibly a single helper could return both displacements, since we always care about validating both of them in our testing.))

Code quote:

ReadSurfaceDisplacement(*plant_, *scratch, *belt_)

multibody/plant/test/surface_velocity_test.cc line 352 at r5 (raw file):

    SCOPED_TRACE(fmt::format("after {} update(s)", k));
    MultibodyPlantTester::CallSurfaceDisplacementUpdate(
        *plant_, *context_, &context_->get_mutable_state());

BTW Passing in the mutable discrete_state output argument as an alias inside the const context input is a violation of system framework invariants.

Event handlers are allowed to write into the output argument anywhere within their update method, and the state they read back from the context is still supposed to remain intact as the pre-event state.

As it happens, the Calc function under test doesn't step on this hazard today, but it's still probably a worthwhile idea to have the event write into scratch here, and then this loop (acting like a simulator) copies the output state back into the context. That would avoid false-positive test failures in case the Calc function changes.

jwnimmer-tri

@jwnimmer-tri made 1 comment.
Reviewable status: 10 unresolved discussions, needs at least two assigned reviewers (waiting on SeanCurtis-TRI).

multibody/plant/multibody_plant.cc line 3654 at r5 (raw file):

  const auto& port = get_surface_speeds_input_port();
  if (!port.HasValue(context)) return systems::EventStatus::Succeeded();

If I remove this line, no tests fail.

(This is an example where unifying input port handling default values with a cache entry would have helped.)

SeanCurtis-TRI

+a:@sherm1 for out-of-cycle platform review, please.

@SeanCurtis-TRI made 8 comments and resolved 3 discussions.
Reviewable status: 7 unresolved discussions, LGTM missing from assignee sherm1(platform) (waiting on jwnimmer-tri and sherm1).

multibody/plant/multibody_plant.cc line 3629 at r5 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

If I remove this line, no tests fail.

(This is an example where unifying input port handling default values with a cache entry would have helped.)

I now have a test for an unconnected input port. For the record, removing the zdot->SetZero(); line will not cause any test to fail. I'm not sure how to get the Simulator to pass in a non-zero zdot to confirm it gets zeroed out (I could use the friend class to invoke it directly....probably not worth it). I've tested the zero-out indirectly.

multibody/plant/multibody_plant.cc line 3654 at r5 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

If I remove this line, no tests fail.

(This is an example where unifying input port handling default values with a cache entry would have helped.)

I don't know the cache entry would've helped. If the test never evaluates either of these methods without an connected input port, then it doesn't matter where the code lives.

multibody/plant/test/surface_velocity_test.cc line 118 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

It's still the case that MultibodyPlantTest.SetSurfaceVelocityAxisErrors and SurfaceVelocityTest.GetSetSurfaceVelocityAxis both check the "can't set post-finalize" behavior. Perhaps the former should be fully folded into the latter? I think that just means moving the world-body case into this test.

I swear I deleted that method twice. But upon review, I realize there was one test that I hadn't moved over which probably muddied my mental state.

multibody/plant/test/surface_velocity_test.cc line 125 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

We still have GTEST_TEST(MultibodyPlantTest, SetSurfaceVelocityAxisErrors) (above).

Done.

multibody/plant/test/surface_velocity_test.cc line 196 at r5 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

This lambda seems somewhat redundant with the FixSurfaceSpeed function. (If we want to curry the plant and context args, that's fine, but at least the lambda should call the other helper function.)

Done.

multibody/plant/test/surface_velocity_test.cc line 276 at r5 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

This should probably just be re-using the ReadSurfaceDisplacement helper function, with hard-coded args?

Done.

multibody/plant/test/surface_velocity_test.cc line 344 at r5 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Here and throughout, we have a ReadBeltDisplacement helper function available; why not use it?

(It might also be worth adding ReadRollerDisplacement helper to match.)

((Or plausibly a single helper could return both displacements, since we always care about validating both of them in our testing.))

Done.

jwnimmer-tri

@jwnimmer-tri reviewed 2 files and all commit messages, made 5 comments, and resolved 7 discussions.
Reviewable status: 3 unresolved discussions, LGTM missing from assignee sherm1(platform) (waiting on SeanCurtis-TRI and sherm1).

multibody/plant/multibody_plant.cc line 3629 at r5 (raw file):

For the record, removing the zdot->SetZero(); line will not cause any test to fail.

I am OK with the current tests. We have an end-to-end test that the integrator works, so in case the simulator stops setting a default, at least that test will trip.

multibody/plant/multibody_plant.cc line 3654 at r5 (raw file):

I don't know the cache entry would've helped. If the test never evaluates either of these methods without an connected input port, then it doesn't matter where the code lives.

The difference is that at least one test case (for the CalcSurfaceVelocity method) still would have evaluated the cache entry with a disconnected input port and therefore probed whether the entry's Calc checked for it correctly.

Yes, we wouldn't have a direct test of this specific call sequence failure being caught unless/until we added the new test cases you just pushed, but checking that all Calc functions use the cache entry would be a more obvious defect to spot in eyeball reviews, even without those tests added. Finding the lack of disconnected-port checks by inspection at every call site is harder to spot.

multibody/plant/multibody_plant.cc line 3628 at r6 (raw file):

  const auto& port = get_surface_speeds_input_port();
  if (!port.HasValue(context)) {
    // zdot->SetZero();

The commented out code for testing made it into the push.

multibody/plant/multibody_plant.cc line 3654 at r6 (raw file):

  const auto& port = get_surface_speeds_input_port();
  if (!port.HasValue(context)) return systems::EventStatus::Succeeded();

BTW @sherm1 would this be better as DidNothing?

multibody/plant/test/surface_velocity_test.cc line 328 at r6 (raw file):

    // Check the output - confirm both signals unchanged from baseline.
    const auto& output_bus =

nit The loop here seems a bit silly:

    // Check the output - confirm both signals unchanged from baseline.
    EXPECT_EQ(ReadBodySurfaceDisplacement(*belt_, &simulator.get_context()),
              baseline);
    EXPECT_EQ(ReadBodySurfaceDisplacement(*roller_, &simulator.get_context()),
              baseline);

jwnimmer-tri

@jwnimmer-tri reviewed 2 files and all commit messages, and resolved 2 discussions.
Reviewable status: 1 unresolved discussion, LGTM missing from assignee sherm1(platform) (waiting on SeanCurtis-TRI and sherm1).

sherm1

Platform review checkpoint

@sherm1 reviewed 8 files and all commit messages, and made 22 comments.
Reviewable status: 20 unresolved discussions, LGTM missing from assignee sherm1(platform) (waiting on jwnimmer-tri and SeanCurtis-TRI).

multibody/plant/multibody_plant.h line 2343 at r7 (raw file):
BTW consider saying this in a simpler way

Surface displacement always starts at zero. This is just a visualization tool; there is no value in setting it to a non-zero value.

multibody/plant/multibody_plant.cc line 3654 at r6 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

BTW @sherm1 would this be better as DidNothing?

Yes. That saves us from having to apply the state update to the context, preventing unnecessary cache invalidations and subsequent recalculations.

multibody/parsing/detail_sdf_parser.cc line 1504 at r7 (raw file):

  // Parse link-level surface velocity axis (if present) and register.
  if (link_element->HasElement("drake:surface_velocity_axis")) {

BTW the urdf code goes to some trouble to exclude World -- I presume that's already been done here but wanted to mention it in case that's missing.

multibody/parsing/detail_sdf_parser.cc line 1507 at r7 (raw file):

    const Vector3d axis_B = ToVector3(
        link_element->Get<gz::math::Vector3d>("drake:surface_velocity_axis"));
    plant->SetSurfaceVelocityAxis(body, axis_B);

BTW consider axis_L rather than B for the link frame (in the new "fused link" world the link/body distinction matters more, internally).

multibody/parsing/detail_urdf_parser.cc line 271 at r7 (raw file):

              " attempted to assign surface velocity (via the "
              "<drake:surface_velocity_axis> tag). Only geometries, "
              "<collision> and <visual>, can be assigned to the world link. "

BTW the relevance of the "Only geometries" sentence in this message is unclear to me. I'm not sure why they are mentioned here -- can the message be more clear? And is there no comparable situation in the sdf parser?

multibody/parsing/parsing_doxygen.h line 1701 at r7 (raw file):

If present, this element defines the surface velocity axis for the associated
body. See @ref mbp_surface_velocity "Surface Velocity" for details.

BTW consider "link" rather than "body"

multibody/parsing/test/detail_sdf_parser_test.cc line 218 at r7 (raw file):

      plant_.GetSurfaceVelocityAxis(has_velocity);
  ASSERT_TRUE(axis_B.has_value());
  EXPECT_TRUE(CompareMatrices(*axis_B, Vector3d(0, 1, 0)));

BTW I see this is expected to be normalized. Is there a test elsewhere that deals with fools who input "0 0 0"?

Also I'm wondering whether there ought to be a test here to catch an attempt to add a surface_velocity_axis to World (or does sdf syntax prevent that somehow)?

multibody/plant/multibody_plant.h line 135 at r7 (raw file):

struct SurfaceVelocityEntry {
  std::string scoped_name;  // Empty until Finalize() is called.
  Eigen::Vector3d axis;     // unit-length, expressed in body frame B.

nit: "body frame B" -> "link frame L" (ideally we do this consistently in internal code to avoid confusion when links are fused onto [mobilized] bodies).

multibody/plant/multibody_plant.h line 1207 at r7 (raw file):

  /// carries a systems::BusValue whose signals set the surface speed for each
  /// body registered via SetSurfaceVelocityAxis(). Each signal's name is the
  /// body's scoped name (i.e., RigidBody::scoped_name()) and its value is a

FYI Unfortunately, I can't advocate a switch to "link" in the public API, except on occasion a parenthetical body (link) when it seems relevant. Although there is a Link alias for RigidBody, there are so many APIs with "Body" in their names it is too daunting to change terms. However, the distinction doesn't matter much in public since "fusing" is mostly transparent there, in contrast to its very visible presence in internal code.

multibody/plant/multibody_plant.h line 1213 at r7 (raw file):

  const systems::InputPort<T>& get_surface_speeds_input_port() const;

  /// Returns the `"surface_displacements"` output port, which carries a

nit: the other port methods say "Returns a constant reference ..."

multibody/plant/multibody_plant.h line 2287 at r7 (raw file):

  /// #### Mathematical model
  ///
  /// Each registered body defines a velocity field over its contact surface.

minor: I don't understand the implied relationship between a "body" and a "contact surface". Isn't a surface a property of geometry? A body (Drake RigidBody/Link object) does not inherently have a surface. Can you clarify what's meant here? Is it using "body" to mean "all geometry attached to a body"?

multibody/plant/multibody_plant.h line 2294 at r7 (raw file):

  ///  - `speed` — the *signed* scalar measure of the surface velocity from the
  ///    `"surface_speeds"` input port (see get_surface_speeds_input_port()),
  ///  - n̂_C_B — the contact normal at C oriented to point *out* of the the

typo: the the -> the

multibody/plant/multibody_plant.h line 2295 at r7 (raw file):

  ///    `"surface_speeds"` input port (see get_surface_speeds_input_port()),
  ///  - n̂_C_B — the contact normal at C oriented to point *out* of the the
  ///    geometry B (likewise expressed in body frame B),

This is the first mention of geometry in this section. Previously "B" was the body frame and now it appears to be the name of a geometry. Maybe use "G" for geometry and keep "B" exclusive to the body and its body frame? Also I'd be inclined to mention geometry earlier since it seems highly relevant!

I'm inferring from what I've read so far that there could be multiple geometries attached to a body and all of them could have a surface velocity but they would all have to run at the same speed and about the same axis. If that's right it might be good to start with; if wrong it would definitely be good to start with a disabuse!

multibody/plant/multibody_plant.h line 2310 at r7 (raw file):

  /// with the axis, the smaller the resultant surface velocity magnitude
  /// should be; the lack of normalization serves this purpose and is
  /// intentional.

minor: I understand the point you're making but it is a little confusing because both â_ss_B and n̂_C_B are unit vectors. So "the equation does not include normalization" doesn't seem like quite the right phrasing. Maybe something a little more, like "although y and z are unit vectors, their cross product in general will have a magnitude less than one. We will use that length to scale speed ..."

multibody/plant/multibody_plant.h line 2335 at r7 (raw file):

  ///    contact. The only effect it has is in computing contact forces. It
  ///    plays *no* role in any other MultibodyPlant calculations.
  ///  - The body's surface velocity is applied to *all* geometries affixed to

BTW ah, this would be a good thing to start with!

BTW the bullet points above start with a bold lead sentence but this one and the next one do not. Would be nice to continue that consistently.

multibody/plant/multibody_plant.h line 2346 at r7 (raw file):

  ///
  /// @warning Surface velocity has not yet been applied to contact and will
  ///          have no effect.

minor: I think this is saying "despite the beautiful documentation you just read, none of this actually works yet in Drake"! Rather than leaving that to the end, I'd suggest starting the documentation block with "(Internal use only)" or "(Experimental)" or "(Coming soon)" to avoid breaking the reader's heart. Then remove that when it works. (See the SetFuseWeldedLinks() stuff which is in the same condition.)

multibody/plant/multibody_plant.h line 2355 at r7 (raw file):

  /// nonzero `axis_B` is normalized before storage.
  ///
  /// @param[in] body    The rigid body.

BTW consider: "The rigid body (link)" as a tie to the sdf/urdf terminology

multibody/plant/multibody_plant.h line 6659 at r7 (raw file):

    requires scalar_predicate<T>::is_bool;

  // Computes the surface velocity for the body identified by `body_index`.

BTW consider something like "Computes the surface velocity for any point on a surface of body B whose surface normal is given."

multibody/plant/multibody_plant.cc line 854 at r7 (raw file):

  // Eigen won't necessarily normalize a near-zero vector. So, we'll normalize
  // first and then test the result.
  Eigen::Vector3d axis = axis_in_B->normalized();

BTW could be const

multibody/plant/multibody_plant.cc line 2675 at r7 (raw file):

    }
  }
  if (speed == 0.0) return Vector3<T>::Zero();

BTW consider removing this special case. The code below will produce the same result anyway, and it doesn't seem like this would be a particularly likely occurrence worth optimizing.

multibody/plant/multibody_plant.cc line 4049 at r7 (raw file):

    for (const auto& [_, entry] : surface_velocity_bodies_) {
      output->Set(entry.scoped_name, Value<double>(values[++i]));
    }

BTW seems more conventional to start with 0

Suggestion:

    int i = 0;
    for (const auto& [_, entry] : surface_velocity_bodies_) {
      output->Set(entry.scoped_name, Value<double>(values[i++]));
    }

sherm1

Platform pass complete. Looks great -- quite a virtuoso use of the System Framework! I have one technical concern -- the use of an abstract state for the discrete plant precludes scalar conversion of that state (because the numerical values are hidden). For numerical values, it is better to use a discrete state because we know those are numerical so can scalar convert automatically. Otherwise just a few minor comments.

@sherm1 reviewed 7 files and made 5 comments.
Reviewable status: 24 unresolved discussions, LGTM missing from assignee sherm1(platform) (waiting on jwnimmer-tri and SeanCurtis-TRI).

multibody/plant/test/multibody_plant_scalar_conversion_test.cc line 180 at r7 (raw file):

  EXPECT_EQ(
      context_u->get_continuous_state().get_misc_continuous_state().size(), 1);
}

BTW is a similar test needed for a discrete plant to verify it maintains the abstract state properly? It occurs to me that an abstract state won't scalar-convert IIRC. If a discrete state were used instead of an abstract state it would convert.

multibody/plant/test/surface_velocity_test.cc line 201 at r7 (raw file):

                      Vector3d::Zero()));

  // (2) Connected port but missing signal --> zero velocity.

BTW not clear what "signal" means here. Is it the lack of a surface velocity axis that makes this zero?

multibody/plant/test/surface_velocity_test.cc line 237 at r7 (raw file):

  EXPECT_TRUE(CompareMatrices(MultibodyPlantTester::ComputeSurfaceVelocity(
                                  plant_, belt_->index(), *context, n_W),
                              v_B_expected, 1e-16));

BTW I'd loosen this a little to avoid future trouble. 1e-15 or 1e-14 would be equally convincing!

multibody/plant/test/surface_velocity_test.cc line 385 at r7 (raw file):

  // constant speed to speed*t_final.
  EXPECT_NEAR(ReadBodySurfaceDisplacement(*belt_, &simulator.get_context()),
              baseline + speed * t_final, 1e-12);

BTW is 1e-12 necessary here? Seems a little surprising since everything else is 1e-14. (You could insulate from roundoff by picking t_final as a power of 2, e.g. 2^-4=.0625)

jwnimmer-tri

@jwnimmer-tri made 1 comment.
Reviewable status: 25 unresolved discussions, LGTM missing from assignee sherm1(platform) (waiting on SeanCurtis-TRI and sherm1).

a discussion (no related file):
@sherm1

I have one technical concern -- the use of an abstract state for the discrete plant precludes scalar conversion of that state (because the numerical values are hidden). For numerical values, it is better to use a discrete state because we know those are numerical so can scalar convert automatically. Otherwise just a few minor comments.

I thought about this as well during feature review, and I think this way is okay. Maybe we need to capture the reasoning a comment near the state declaration for posterity?

There are two possible hazards: (1) defeating solvers other than the simulator (e.g., trajectory optimization) by introducing non-numeric state and (2) inability to transmogrify during "SetTimeStateAndParametersFrom" ala #5454.

(1) Introducing non-numeric state

The discrete-time MbP already has abstract state by default, because the default for SetUseSampledOutputPorts is true, so we store a lot of abstract state in service of output sampling.

The new abstract state here is only declared if bodies have a surface velocity axis assigned, so by default there is no additional abstract state for most users.

The only time this would matter is if a user is doing e.g. trajectory optimization and wants to have surface velocities assigned in their plant. In that case yes, the abstract state due to the positions would be a problem, but I deem it obscure enough that we can defer it to a follow-up if/when someone needs to run that use case. For now, I think the simpler code change wins out.

(2) #5454

This is not relevant, because the state is not T-dependent. The abstract value only ever uses T=double -- the prescribed input surface speeds are not autodiff-able, nor is the output port for displacement visualization.

sherm1

@sherm1 made 1 comment.
Reviewable status: 25 unresolved discussions, LGTM missing from assignee sherm1(platform) (waiting on jwnimmer-tri and SeanCurtis-TRI).

a discussion (no related file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

@sherm1

I have one technical concern -- the use of an abstract state for the discrete plant precludes scalar conversion of that state (because the numerical values are hidden). For numerical values, it is better to use a discrete state because we know those are numerical so can scalar convert automatically. Otherwise just a few minor comments.

I thought about this as well during feature review, and I think this way is okay. Maybe we need to capture the reasoning a comment near the state declaration for posterity?

There are two possible hazards: (1) defeating solvers other than the simulator (e.g., trajectory optimization) by introducing non-numeric state and (2) inability to transmogrify during "SetTimeStateAndParametersFrom" ala #5454.

(1) Introducing non-numeric state

The discrete-time MbP already has abstract state by default, because the default for SetUseSampledOutputPorts is true, so we store a lot of abstract state in service of output sampling.

The new abstract state here is only declared if bodies have a surface velocity axis assigned, so by default there is no additional abstract state for most users.

The only time this would matter is if a user is doing e.g. trajectory optimization and wants to have surface velocities assigned in their plant. In that case yes, the abstract state due to the positions would be a problem, but I deem it obscure enough that we can defer it to a follow-up if/when someone needs to run that use case. For now, I think the simpler code change wins out.

(2) #5454

This is not relevant, because the state is not T-dependent. The abstract value only ever uses T=double -- the prescribed input surface speeds are not autodiff-able, nor is the output port for displacement visualization.

I understand your point but what is the downside of using Discrete state rather than Abstract for purely numerical values like these? Discrete state is intended for that purpose.

jwnimmer-tri

@jwnimmer-tri made 1 comment.
Reviewable status: 25 unresolved discussions, LGTM missing from assignee sherm1(platform) (waiting on SeanCurtis-TRI and sherm1).

a discussion (no related file):

Previously, sherm1 (Michael Sherman) wrote…

I understand your point but what is the downside of using Discrete state rather than Abstract for purely numerical values like these? Discrete state is intended for that purpose.

Discrete state is indicated when we want an array of T's. In this case, we want an array of doubles, even in an autodiff or symbolic plant. Using discrete state would mean adding casts to/from double ala ExtractDoubleOrThrow.

SeanCurtis-TRI

Comments addressed.

Conversation on scalar conversion, see below.

@SeanCurtis-TRI made 14 comments and resolved 22 discussions.
Reviewable status: 3 unresolved discussions, LGTM missing from assignee sherm1(platform) (waiting on jwnimmer-tri and sherm1).

a discussion (no related file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Discrete state is indicated when we want an array of T's. In this case, we want an array of doubles, even in an autodiff or symbolic plant. Using discrete state would mean adding casts to/from double ala ExtractDoubleOrThrow.

And the newly modified test shows that conversion from MbP<double> to MbP<*> maintains the state for both continuous and discrete happily.

My take on scalar converting abstract is the idea if that if you ever do:

template <typename T>
void MultibodyPlant<T>::Thing() {
...
abstract_value.Get<Foo<T>>();  // <== the problem.
...
}

you're in trouble because the contents of the AbstractValue cannot intelligently transmogrify and what you probably have stored is the original Foo<double>.

But if you're always doing:

template <typename T>
void MultibodyPlant<T>::Thing() {
...
abstract_value.Get<Foo<double>>();  // <== no problem.
...
}

then you have no problem.

multibody/parsing/detail_sdf_parser.cc line 1504 at r7 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW the urdf code goes to some trouble to exclude World -- I presume that's already been done here but wanted to mention it in case that's missing.

I don't think it's necessary.

In URDF "world" is just the name applied to another link. In SDF <world> is its own tag and not a <link>. Therefore, attempting to assign the new element under <world> in sdformat will produce a parsing error in AddModelsFromSdf because the only acceptable world elements are "frame", "include", "joint", and "model".

multibody/parsing/detail_sdf_parser.cc line 1507 at r7 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW consider axis_L rather than B for the link frame (in the new "fused link" world the link/body distinction matters more, internally).

I was going to argue against that as part of this PR, because there are still many, many instances of B. In contrast, there are far fewer references to L.

But what changed my mind is that most of those L references are in this method. :) So, locally consistent wins. (It is a shame that this method still alternates between uses of L and B -- for example, link L has inertia M_BBo_B).

multibody/parsing/detail_urdf_parser.cc line 271 at r7 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW the relevance of the "Only geometries" sentence in this message is unclear to me. I'm not sure why they are mentioned here -- can the message be more clear? And is there no comparable situation in the sdf parser?

If you look up at line 210, this warning is basically a copy of the previous warning. In that case, if an <inertial> tag is specified as child of <world> we get this same warning (modulo erroneous tag name). I inferred that the explicit enumeration of what was allowed was an intentional design decision.

multibody/parsing/test/detail_sdf_parser_test.cc line 218 at r7 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW I see this is expected to be normalized. Is there a test elsewhere that deals with fools who input "0 0 0"?

Also I'm wondering whether there ought to be a test here to catch an attempt to add a surface_velocity_axis to World (or does sdf syntax prevent that somehow)?

The test is in surface_velocity_test.cc(line 148).

As for "world", your suspicion about SDF syntax is correct; see previous note.

multibody/plant/multibody_plant.h line 2287 at r7 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

minor: I don't understand the implied relationship between a "body" and a "contact surface". Isn't a surface a property of geometry? A body (Drake RigidBody/Link object) does not inherently have a surface. Can you clarify what's meant here? Is it using "body" to mean "all geometry attached to a body"?

Excellent point. I've qualified the phrase. PTAL.

multibody/plant/multibody_plant.h line 2295 at r7 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

This is the first mention of geometry in this section. Previously "B" was the body frame and now it appears to be the name of a geometry. Maybe use "G" for geometry and keep "B" exclusive to the body and its body frame? Also I'd be inclined to mention geometry earlier since it seems highly relevant!

I'm inferring from what I've read so far that there could be multiple geometries attached to a body and all of them could have a surface velocity but they would all have to run at the same speed and about the same axis. If that's right it might be good to start with; if wrong it would definitely be good to start with a disabuse!

Hopefully, the solution to the previous comment and this one combined help clarify things.

multibody/plant/multibody_plant.h line 2343 at r7 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW consider saying this in a simpler way

Surface displacement always starts at zero. This is just a visualization tool; there is no value in setting it to a non-zero value.

I still wanted to say something explicit about randomization. But otherwise took your suggestion as is.

multibody/plant/multibody_plant.h line 2346 at r7 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

minor: I think this is saying "despite the beautiful documentation you just read, none of this actually works yet in Drake"! Rather than leaving that to the end, I'd suggest starting the documentation block with "(Internal use only)" or "(Experimental)" or "(Coming soon)" to avoid breaking the reader's heart. Then remove that when it works. (See the SetFuseWeldedLinks() stuff which is in the same condition.)

I really wanted to put in a "Pardon our dust" warning. :) I went for something stronger than (Experimental). PTAL.

multibody/plant/multibody_plant.cc line 3654 at r6 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

Yes. That saves us from having to apply the state update to the context, preventing unnecessary cache invalidations and subsequent recalculations.

Done.

multibody/plant/multibody_plant.cc line 2675 at r7 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW consider removing this special case. The code below will produce the same result anyway, and it doesn't seem like this would be a particularly likely occurrence worth optimizing.

While it's true, that the mathematical end result is the same, I can easily imagine future readers wondering, "Why are you doing all that work if the speed is zero?" Arguably, "all that work" isn't that much. But, ultimately, this will be called for every body in contact. So, it's best to get to zero as cheaply as possible.

multibody/plant/test/multibody_plant_scalar_conversion_test.cc line 180 at r7 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW is a similar test needed for a discrete plant to verify it maintains the abstract state properly? It occurs to me that an abstract state won't scalar-convert IIRC. If a discrete state were used instead of an abstract state it would convert.

I've updated the test for both continuous and discrete (reformulating how the test works).

multibody/plant/test/surface_velocity_test.cc line 201 at r7 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW not clear what "signal" means here. Is it the lack of a surface velocity axis that makes this zero?

I hoped the reference to (2) would help. In the test overview documentation it says:

"2. A connected bus missing the signal reports zero velocity"

And then the next line emphasizes that we're setting the "other" signal and not the "belt".

I'd be glad to accept recommendations on changes to that language.

jwnimmer-tri

@jwnimmer-tri partially reviewed 6 files, made 1 comment, and resolved 1 discussion.
Reviewable status: 2 unresolved discussions, LGTM missing from assignee sherm1(platform) (waiting on sherm1).

a discussion (no related file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

And the newly modified test shows that conversion from MbP<double> to MbP<*> maintains the state for both continuous and discrete happily.

My take on scalar converting abstract is the idea if that if you ever do:
template <typename T>
void MultibodyPlant<T>::Thing() {
...
abstract_value.Get<Foo<T>>();  // <== the problem.
...
}
you're in trouble because the contents of the AbstractValue cannot intelligently transmogrify and what you probably have stored is the original Foo<double>.

But if you're always doing:
template <typename T>
void MultibodyPlant<T>::Thing() {
...
abstract_value.Get<Foo<double>>();  // <== no problem.
...
}
then you have no problem.

Perhaps another way of justifying the use of abstract state for the displacements is that the output port is a BusValue (abstract, not numeric). For efficiency we're storing the displacements in the Context as a std::vector<> where the BusValue's names are stored separately from the indices, but we could just as well stored the BusValue as our state and then the output port calc function would simply copy the state to the output. To make updates more efficient, though, we've stored a dis-aggregated form of the BusValue in a std::vector<> but that doesn't change the nature of the state from abstract to numeric.

(I still think some resolution of this discussion should appear in the code for posterity.)

This includes: - The APIs on MultibodyPlant to declare and introspect surface velocities. - The requisite ports. - The parsing infrastructure to declare surface velocity in the model files. It does *not* actually make use of the surface velocity in contact yet.

SeanCurtis-TRI

@SeanCurtis-TRI made 1 comment.
Reviewable status: 2 unresolved discussions, LGTM missing from assignee sherm1(platform) (waiting on jwnimmer-tri and sherm1).

a discussion (no related file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Perhaps another way of justifying the use of abstract state for the displacements is that the output port is a BusValue (abstract, not numeric). For efficiency we're storing the displacements in the Context as a std::vector<> where the BusValue's names are stored separately from the indices, but we could just as well stored the BusValue as our state and then the output port calc function would simply copy the state to the output. To make updates more efficient, though, we've stored a dis-aggregated form of the BusValue in a std::vector<> but that doesn't change the nature of the state from abstract to numeric.

(I still think some resolution of this discussion should appear in the code for posterity.)

The latest revision provides my take on the discussion at the point of state declaration.

sherm1

Platform

@sherm1 reviewed 7 files and all commit messages, made 4 comments, and resolved 1 discussion.
Reviewable status: 2 unresolved discussions (waiting on SeanCurtis-TRI).

multibody/parsing/detail_sdf_parser.cc line 1507 at r7 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

I was going to argue against that as part of this PR, because there are still many, many instances of B. In contrast, there are far fewer references to L.

But what changed my mind is that most of those L references are in this method. :) So, locally consistent wins. (It is a shame that this method still alternates between uses of L and B -- for example, link L has inertia M_BBo_B).

Yeah, we're grinding our way through the internals where M_BBo_B will be consistently the mass properties of a mobilized body (possibly a fused composite) and M_LLo_L is the mass properties of a link.

multibody/plant/test/surface_velocity_test.cc line 201 at r7 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

I hoped the reference to (2) would help. In the test overview documentation it says:

"2. A connected bus missing the signal reports zero velocity"

And then the next line emphasizes that we're setting the "other" signal and not the "belt".

I'd be glad to accept recommendations on changes to that language.

I didn't know the term "signal" was defined by BusValue. Likely the next reader won't know that either. Possible fix: say "BusValue signal" rather than just "signal"?

multibody/plant/multibody_plant.h line 2346 at r10 (raw file):

  ///    surface velocity registrations, but they can be connected by weld
  ///    joints to function as a single rigid body.
  ///  - Surface displacement always initializes to zero. This state is just

BTW the other bullets begin with a bold phrase. Embolden this one also?

jwnimmer-tri

@jwnimmer-tri reviewed 3 files and all commit messages.
Reviewable status: 2 unresolved discussions (waiting on SeanCurtis-TRI).

SeanCurtis-TRI added the release notes: none This pull request should not be mentioned in the release notes label Jul 14, 2026

SeanCurtis-TRI assigned jwnimmer-tri Jul 14, 2026

SeanCurtis-TRI commented Jul 14, 2026

View reviewed changes

jwnimmer-tri reviewed Jul 15, 2026

View reviewed changes

jwnimmer-tri reviewed Jul 16, 2026

View reviewed changes

SeanCurtis-TRI force-pushed the PR_mbp_surface_velocity branch from 16be17d to 0b9fb6d Compare July 20, 2026 16:05

SeanCurtis-TRI commented Jul 20, 2026

View reviewed changes

SeanCurtis-TRI force-pushed the PR_mbp_surface_velocity branch from 0b9fb6d to e86fb5e Compare July 20, 2026 16:10

jwnimmer-tri reviewed Jul 20, 2026

View reviewed changes

SeanCurtis-TRI force-pushed the PR_mbp_surface_velocity branch from e86fb5e to 84f19f2 Compare July 20, 2026 22:56

SeanCurtis-TRI commented Jul 20, 2026

View reviewed changes

jwnimmer-tri reviewed Jul 20, 2026

View reviewed changes

SeanCurtis-TRI force-pushed the PR_mbp_surface_velocity branch from 84f19f2 to 42047e4 Compare July 21, 2026 14:11

jwnimmer-tri reviewed Jul 21, 2026

View reviewed changes

SeanCurtis-TRI force-pushed the PR_mbp_surface_velocity branch from 42047e4 to 088a70a Compare July 21, 2026 16:31

SeanCurtis-TRI assigned sherm1 Jul 21, 2026

SeanCurtis-TRI commented Jul 21, 2026

View reviewed changes

jwnimmer-tri reviewed Jul 21, 2026

View reviewed changes

SeanCurtis-TRI force-pushed the PR_mbp_surface_velocity branch from 088a70a to fc06c5a Compare July 21, 2026 17:28

jwnimmer-tri reviewed Jul 21, 2026

View reviewed changes

sherm1 reviewed Jul 22, 2026

View reviewed changes

jwnimmer-tri reviewed Jul 22, 2026

View reviewed changes

sherm1 reviewed Jul 22, 2026

View reviewed changes

jwnimmer-tri reviewed Jul 22, 2026

View reviewed changes

SeanCurtis-TRI force-pushed the PR_mbp_surface_velocity branch from fc06c5a to d49bfe6 Compare July 22, 2026 15:53

SeanCurtis-TRI commented Jul 22, 2026

View reviewed changes

SeanCurtis-TRI force-pushed the PR_mbp_surface_velocity branch from d49bfe6 to 2e04204 Compare July 22, 2026 15:58

jwnimmer-tri reviewed Jul 22, 2026

View reviewed changes

SeanCurtis-TRI force-pushed the PR_mbp_surface_velocity branch from 2e04204 to ad8d726 Compare July 22, 2026 17:23

SeanCurtis-TRI commented Jul 22, 2026

View reviewed changes

sherm1 reviewed Jul 22, 2026

View reviewed changes

jwnimmer-tri reviewed Jul 22, 2026

View reviewed changes

Uh oh!

Conversation

SeanCurtis-TRI commented Jul 14, 2026 • edited by sherm1 Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

SeanCurtis-TRI commented Jul 14, 2026

Uh oh!

SeanCurtis-TRI left a comment

Choose a reason for hiding this comment

Uh oh!

jwnimmer-tri left a comment

Choose a reason for hiding this comment

Uh oh!

SeanCurtis-TRI commented Jul 15, 2026

Uh oh!

jwnimmer-tri left a comment

Choose a reason for hiding this comment

Uh oh!

jwnimmer-tri left a comment

Choose a reason for hiding this comment

Uh oh!

jwnimmer-tri left a comment

Choose a reason for hiding this comment

Uh oh!

jwnimmer-tri left a comment

Choose a reason for hiding this comment

Uh oh!

SeanCurtis-TRI left a comment

Choose a reason for hiding this comment

Uh oh!

jwnimmer-tri left a comment

Choose a reason for hiding this comment

Uh oh!

SeanCurtis-TRI left a comment

Choose a reason for hiding this comment

Uh oh!

jwnimmer-tri left a comment

Choose a reason for hiding this comment

Uh oh!

jwnimmer-tri left a comment

Choose a reason for hiding this comment

Uh oh!

jwnimmer-tri left a comment

Choose a reason for hiding this comment

Uh oh!

SeanCurtis-TRI left a comment

Choose a reason for hiding this comment

Uh oh!

jwnimmer-tri left a comment

Choose a reason for hiding this comment

Uh oh!

jwnimmer-tri left a comment

Choose a reason for hiding this comment

Uh oh!

sherm1 left a comment

Choose a reason for hiding this comment

Uh oh!

sherm1 left a comment

Choose a reason for hiding this comment

Uh oh!

jwnimmer-tri left a comment

Choose a reason for hiding this comment

Uh oh!

sherm1 left a comment

Choose a reason for hiding this comment

Uh oh!

jwnimmer-tri left a comment

Choose a reason for hiding this comment

Uh oh!

SeanCurtis-TRI left a comment

Choose a reason for hiding this comment

Uh oh!

jwnimmer-tri left a comment

Choose a reason for hiding this comment

Uh oh!

SeanCurtis-TRI left a comment

Choose a reason for hiding this comment

Uh oh!

sherm1 left a comment

Choose a reason for hiding this comment

SeanCurtis-TRI commented Jul 14, 2026 •

edited by sherm1

Loading