[multibody] Add surface velocity to MultibodyPlant

bindings/generated_docstrings/multibody_plant.h

@@ -1703,6 +1703,7 @@ concepts/notation.
     - <em style="color:gray">model_instance_name[i]</em>_actuation
     - <em style="color:gray">model_instance_name[i]</em>_desired_state
     - <span style="color:green">geometry_query</span>
+    - surface_speeds
     output_ports:
     - state
     - body_poses
@@ -1718,6 +1719,7 @@ concepts/notation.
     - <em style="color:gray">model_instance_name[i]</em>_net_actuation
     - <span style="color:green">geometry_pose</span>
     - <span style="color:green">deformable_body_configuration</span>
+    - surface_displacements
 
 The ports whose names begin with <em style="color:gray">
 model_instance_name[i]</em> represent groups of ports, one for each of
@@ -4767,6 +4769,11 @@ Changing the order will cause exceptions to be thrown.
     RuntimeError if ``this`` MultibodyPlant was not registered with a
     SceneGraph.)""";
         } CollectRegisteredGeometries;
+        // Symbol: drake::multibody::MultibodyPlant::ComputeSurfaceVelocity
+        struct /* ComputeSurfaceVelocity */ {
+          // Source: drake/multibody/plant/multibody_plant.h
+          const char* doc = R"""()""";
+        } ComputeSurfaceVelocity;
         // Symbol: drake::multibody::MultibodyPlant::EvalBlockSystemJacobian
         struct /* EvalBlockSystemJacobian */ {
           // Source: drake/multibody/plant/multibody_plant.h
@@ -5778,6 +5785,14 @@ The returned names are guaranteed to be unique.
     RuntimeError if the plant is not finalized or if the
     ``model_instance`` is invalid.)""";
         } GetStateNames;
+        // Symbol: drake::multibody::MultibodyPlant::GetSurfaceVelocityAxis
+        struct /* GetSurfaceVelocityAxis */ {
+          // Source: drake/multibody/plant/multibody_plant.h
+          const char* doc =
+R"""(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().)""";
+        } GetSurfaceVelocityAxis;
         // Symbol: drake::multibody::MultibodyPlant::GetTopologyGraphvizString
         struct /* GetTopologyGraphvizString */ {
           // Source: drake/multibody/plant/multibody_plant.h
@@ -7170,15 +7185,43 @@ MultibodyPlant∷num_positions() or ``q_instance`` is not of size
         struct /* SetRandomState */ {
           // Source: drake/multibody/plant/multibody_plant.h
           const char* doc =
-R"""(Assigns random values to all elements of the state, by drawing samples
-independently for each joint/free body (coming soon: and then solving
-a mathematical program to "project" these samples onto the registered
-system constraints). If a random distribution is not specified for a
-joint/free body, the default state is used.
+R"""(Assigns random values to all elements of the state that support random
+values, by drawing samples independently for each joint/free body
+(coming soon: and then solving a mathematical program to "project"
+these samples onto the registered system constraints). If a random
+distribution is not specified for a joint/free body, the default state
+is used.
 
 See also:
     stochastic_systems)""";
         } SetRandomState;
+        // Symbol: drake::multibody::MultibodyPlant::SetSurfaceVelocityAxis
+        struct /* SetSurfaceVelocityAxis */ {
+          // Source: drake/multibody/plant/multibody_plant.h
+          const char* doc =
+R"""(Sets the surface-velocity axis for ``body`` to ``axis_B``, expressed
+in the body frame B. If ``axis_B`` is ``std∷nullopt``, any existing
+registration for ``body`` is cleared. May be called any number of
+times before Finalize(); a subsequent call overwrites any prior
+registration. A nonzero ``axis_B`` is normalized before storage.
+
+Parameter ``body``:
+    The rigid body.
+
+Parameter ``axis_B``:
+    A nonzero vector giving the rotation-axis direction in the body
+    frame B, or ``std∷nullopt`` to clear.
+
+Raises:
+    RuntimeError if called after Finalize().
+
+Raises:
+    RuntimeError if ``axis_B`` has a value and ``body`` is the world
+    body.
+
+Raises:
+    RuntimeError if ``axis_B`` has a value and can't be normalized.)""";
+        } SetSurfaceVelocityAxis;
         // Symbol: drake::multibody::MultibodyPlant::SetUseSampledOutputPorts
         struct /* SetUseSampledOutputPorts */ {
           // Source: drake/multibody/plant/multibody_plant.h
@@ -7939,6 +7982,34 @@ output port of size num_multibody_states(ModelInstanceIndex) const
 Raises:
     RuntimeError if the model instance does not exist.)""";
         } get_state_output_port;
+        // Symbol: drake::multibody::MultibodyPlant::get_surface_displacement_output_port
+        struct /* get_surface_displacement_output_port */ {
+          // Source: drake/multibody/plant/multibody_plant.h
+          const char* doc =
+R"""(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 signal's name is the fully-qualified
+body name. The displacement is initialized to zero and integrated from
+the ``"surface_speeds"`` input port.
+
+Precondition:
+    Finalize() was already called on ``this`` plant.)""";
+        } get_surface_displacement_output_port;
+        // Symbol: drake::multibody::MultibodyPlant::get_surface_speeds_input_port
+        struct /* get_surface_speeds_input_port */ {
+          // Source: drake/multibody/plant/multibody_plant.h
+          const char* doc =
+R"""(Returns a constant reference to the ``"surface_speeds"`` input port,
+which 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. If the port is not connected, or a body's
+signal is absent, that body's speed is treated as zero.
+
+Precondition:
+    Finalize() was already called on ``this`` plant.)""";
+        } get_surface_speeds_input_port;
         // Symbol: drake::multibody::MultibodyPlant::get_tendon_constraint_specs
         struct /* get_tendon_constraint_specs */ {
           // Source: drake/multibody/plant/multibody_plant.h
@@ -8159,6 +8230,16 @@ R"""(Returns the number of joints in the model.
 See also:
     AddJoint().)""";
         } num_joints;
+        // Symbol: drake::multibody::MultibodyPlant::num_misc_continuous_states
+        struct /* num_misc_continuous_states */ {
+          // Source: drake/multibody/plant/multibody_plant.h
+          const char* doc =
+R"""(Returns the size of the continuous miscellaneous state vector z for
+this model.
+
+Raises:
+    RuntimeError if called pre-finalize.)""";
+        } num_misc_continuous_states;
         // Symbol: drake::multibody::MultibodyPlant::num_model_instances
         struct /* num_model_instances */ {
           // Source: drake/multibody/plant/multibody_plant.h

bindings/pydrake/multibody/plant_py.cc

@@ -250,6 +250,10 @@ void DoScalarDependentDefinitions(py::module_ m, T) {
             overload_cast_explicit<int, ModelInstanceIndex>(
                 &Class::num_actuated_dofs),
             py::arg("model_instance"), cls_doc.num_actuated_dofs.doc_1args);
+    // Plant-owned state variables - miscellaneous continuous state.
+    cls  // BR
+        .def("num_misc_continuous_states", &Class::num_misc_continuous_states,
+            cls_doc.num_misc_continuous_states.doc);
     // Construction.
     cls  // BR
         .def("SetUseSampledOutputPorts", &Class::SetUseSampledOutputPorts,
@@ -1179,7 +1183,22 @@ void DoScalarDependentDefinitions(py::module_ m, T) {
                 ModelInstanceIndex>(
                 &Class::get_generalized_contact_forces_output_port),
             py_rvp::reference_internal, py::arg("model_instance"),
-            cls_doc.get_generalized_contact_forces_output_port.doc);
+            cls_doc.get_generalized_contact_forces_output_port.doc)
+        .def("get_surface_speeds_input_port",
+            overload_cast_explicit<const systems::InputPort<T>&>(
+                &Class::get_surface_speeds_input_port),
+            py_rvp::reference_internal,
+            cls_doc.get_surface_speeds_input_port.doc)
+        .def("get_surface_displacement_output_port",
+            overload_cast_explicit<const systems::OutputPort<T>&>(
+                &Class::get_surface_displacement_output_port),
+            py_rvp::reference_internal,
+            cls_doc.get_surface_displacement_output_port.doc)
+        .def("SetSurfaceVelocityAxis", &Class::SetSurfaceVelocityAxis,
+            py::arg("body"), py::arg("axis_B"),
+            cls_doc.SetSurfaceVelocityAxis.doc)
+        .def("GetSurfaceVelocityAxis", &Class::GetSurfaceVelocityAxis,
+            py::arg("body"), cls_doc.GetSurfaceVelocityAxis.doc);
     // Property accessors.
     cls  // BR
         .def("world_body", &Class::world_body, py_rvp::reference_internal,

bindings/pydrake/multibody/test/plant_test.py

@@ -474,6 +474,10 @@ def check_repr(element, expected):
             plant.num_actuated_dofs(model_instance=model_instance),
             benchmark.num_actuated_dofs(),
         )
+        self.assertEqual(
+            plant.num_misc_continuous_states(),
+            benchmark.num_misc_continuous_states(),
+        )
         self.assertTrue(plant.is_finalized())
         self.assertTrue(plant.HasBodyNamed(name="Link1"))
         self.assertTrue(
@@ -1846,6 +1850,24 @@ def extract_list_value(port):
         # TODO(eric.cousineau): Merge `check_applied_force_input_ports` into
         # this test.
 
+    @numpy_compare.check_all_types
+    def test_surface_velocity(self, T):
+        plant = MultibodyPlant_[T](0.0)
+        body = plant.AddRigidBody("body")
+
+        # confirm configuration of surface velocity axis.
+        axis_B = np.array([1.0, 0.0, 0.0])
+        plant.SetSurfaceVelocityAxis(body=body, axis_B=axis_B)
+        numpy_compare.assert_float_equal(
+            plant.GetSurfaceVelocityAxis(body=body), axis_B
+        )
+
+        plant.Finalize()
+
+        # Make sure ports are available.
+        self.assertIsNotNone(plant.get_surface_speeds_input_port())
+        self.assertIsNotNone(plant.get_surface_displacement_output_port())
+
     @numpy_compare.check_all_types
     def test_externally_applied_spatial_force(self, T):
         ExternallyAppliedSpatialForce = ExternallyAppliedSpatialForce_[T]

multibody/parsing/detail_sdf_parser.cc

@@ -1389,8 +1389,14 @@ std::optional<LinkInfo> AddRigidLinkFromSpecification(
   std::optional<LinkInfo> link_info;
 
   const std::set<std::string> supported_link_elements{
-      "drake:visual", "collision", "gravity", "inertial",
-      "kinematic",    "pose",      "visual"};
+      "drake:surface_velocity_axis",
+      "drake:visual",
+      "collision",
+      "gravity",
+      "inertial",
+      "kinematic",
+      "pose",
+      "visual"};
 
   sdf::ElementPtr link_element = link.Element();
 
@@ -1493,6 +1499,14 @@ std::optional<LinkInfo> AddRigidLinkFromSpecification(
       }
     }
   }
+
+  // Parse link-level surface velocity axis (if present) and register.
+  if (link_element->HasElement("drake:surface_velocity_axis")) {
+    const Vector3d axis_B = ToVector3(
+        link_element->Get<gz::math::Vector3d>("drake:surface_velocity_axis"));
+    plant->SetSurfaceVelocityAxis(body, axis_B);
+  }
+
   return link_info;
 }
 

multibody/parsing/detail_urdf_parser.cc

@@ -257,6 +257,29 @@ void UrdfParser::ParseBody(XMLElement* node, MaterialMap* materials) {
           std::move(*geometry_instance->mutable_proximity_properties()));
     }
   }
+
+  // Parse link-level surface velocity axis (if present) and register.
+  const XMLElement* sv_node =
+      node->FirstChildElement("drake:surface_velocity_axis");
+  if (sv_node != nullptr) {
+    if (body_pointer == &w_.plant->world_body()) {
+      Warning(*node,
+              "A URDF file declared the \"world\" link and then"
+              " attempted to assign surface velocity (via the "
+              "<drake:surface_velocity_axis> tag). Only geometries, "
+              "<collision> and <visual>, can be assigned to the world link. "
+              "The <drake:surface_velocity_axis> tag is being ignored.");
+    } else {
+      Eigen::Vector3d axis_B;
+      if (!ParseVectorAttribute(sv_node, "axis", &axis_B)) {
+        Error(*sv_node,
+              "Failed to parse 'axis' attribute of "
+              "<drake:surface_velocity_axis>; ignoring.");
+      } else {
+        w_.plant->SetSurfaceVelocityAxis(*body_pointer, axis_B);
+      }
+    }
+  }
 }
 
 void UrdfParser::ParseCollisionFilterGroup(XMLElement* node) {

multibody/parsing/parsing_doxygen.h

@@ -315,6 +315,7 @@ Here is the full list of custom elements:
 - @ref tag_drake_rotor_inertia
 - @ref tag_drake_screw_thread_pitch
 - @ref tag_drake_stiffness_damping
+- @ref tag_drake_surface_velocity_axis
 - @ref tag_drake_tendon_constraint
 - @ref tag_drake_tendon_constraint_joint
 - @ref tag_drake_tendon_constraint_offset
@@ -1539,6 +1540,17 @@ corresponding to a right-handed thread.
 - URDF path: n/a
 - Syntax: Non-negative floating point value.
 
+@subsection tag_drake_surface_velocity_axis drake:surface_velocity_axis
+
+- SDFormat path: `//model/link/drake:surface_velocity_axis`
+- URDF path: `//robot/link/drake:surface_velocity_axis`
+- Syntax: Three space-separated floating point values.
+
+@subsubsection tag_drake_surface_velocity_axis_semantics Semantics
+
+If present, this element defines the surface velocity axis for the associated
+body. See @ref mbp_surface_velocity "Surface Velocity" for details.
+
 @subsubsection tag_drake_stiffness_damping_semantics Semantics
 
 If present, this element provides a value (with unit `s`) for the stiffness

multibody/parsing/test/detail_sdf_parser_test.cc

@@ -198,6 +198,29 @@ const Frame<double>& GetModelFrameByName(const MultibodyPlant<double>& plant,
   return plant.GetFrameByName("__model__", model_instance);
 }
 
+TEST_F(SdfParserTest, SurfaceVelocityAxis) {
+  AddModelsFromSdfString(R"""(
+    <sdf version='1.12' xmlns:drake='https://drake.mit.edu'>
+      <model name='velocity_test'>
+        <link name='has_velocity'>
+          <drake:surface_velocity_axis>
+            0 2 0
+          </drake:surface_velocity_axis>
+        </link>
+        <link name='no_velocity'/>
+      </model>
+    </sdf>)""");
+
+  const auto& has_velocity = plant_.GetBodyByName("has_velocity");
+  const std::optional<Vector3d> axis_B =
+      plant_.GetSurfaceVelocityAxis(has_velocity);
+  ASSERT_TRUE(axis_B.has_value());
+  EXPECT_TRUE(CompareMatrices(*axis_B, Vector3d(0, 1, 0)));
+
+  const auto& no_velocity = plant_.GetBodyByName("no_velocity");
+  EXPECT_EQ(plant_.GetSurfaceVelocityAxis(no_velocity), std::nullopt);
+}
+
 // Verifies that the SDF loader can leverage a specified package map.
 TEST_F(SdfParserTest, PackageMapSpecified) {
   // We start with the world and default model instances (model_instance.h

multibody/parsing/test/detail_urdf_parser_test.cc

@@ -157,6 +157,34 @@ TEST_F(UrdfParserTest, ModelRenameWithColons) {
   EXPECT_EQ(plant_.GetModelInstanceName(*index), "left::robot");
 }
 
+TEST_F(UrdfParserTest, SurfaceVelocityAxis) {
+  EXPECT_NE(AddModelFromUrdfString(R"""(
+    <robot name='belt_model' xmlns:drake='https://drake.mit.edu'>
+      <link name='world'>
+        <!-- This should dispatch a warning. -->
+        <drake:surface_velocity_axis axis='0 0 3'/>
+      </link>
+      <link name='has_velocity'>
+        <drake:surface_velocity_axis axis='0 0 3'/>
+      </link>
+      <link name='no_velocity'/>
+    </robot>)""",
+                                   ""),
+            std::nullopt);
+  EXPECT_THAT(
+      TakeWarning(),
+      MatchesRegex(".*<drake:surface_velocity_axis> tag is being ignored.*"));
+
+  const auto& has_velocity = plant_.GetBodyByName("has_velocity");
+  const std::optional<Vector3d> axis_B =
+      plant_.GetSurfaceVelocityAxis(has_velocity);
+  ASSERT_TRUE(axis_B.has_value());
+  EXPECT_TRUE(CompareMatrices(*axis_B, Vector3d(0, 0, 1)));
+
+  const auto& no_velocity = plant_.GetBodyByName("no_velocity");
+  EXPECT_EQ(plant_.GetSurfaceVelocityAxis(no_velocity), std::nullopt);
+}
+
 TEST_F(UrdfParserTest, ObsoleteLoopJoint) {
   EXPECT_NE(AddModelFromUrdfString("<robot name='a'><loop_joint/></robot>", ""),
             std::nullopt);

multibody/plant/BUILD.bazel

@@ -1306,4 +1306,15 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "surface_velocity_test",
+    deps = [
+        ":plant",
+        "//common:value",
+        "//common/test_utilities:eigen_matrix_compare",
+        "//systems/analysis:simulator",
+        "//systems/framework:bus_value",
+    ],
+)
+
 add_lint_tests()