Add ExternallyAppliedSpatialForceMultiplexer

bindings/pydrake/multibody/plant_py.cc

@@ -19,6 +19,7 @@
 #include "drake/multibody/plant/contact_results.h"
 #include "drake/multibody/plant/contact_results_to_lcm.h"
 #include "drake/multibody/plant/externally_applied_spatial_force.h"
+#include "drake/multibody/plant/externally_applied_spatial_force_list_multiplexer.h"
 #include "drake/multibody/plant/multibody_plant.h"
 #include "drake/multibody/plant/multibody_plant_config.h"
 #include "drake/multibody/plant/multibody_plant_config_functions.h"
@@ -1153,7 +1154,7 @@ void DoScalarDependentDefinitions(py::module m, T) {
 
   // ExternallyAppliedSpatialForce
   {
-    using Class = multibody::ExternallyAppliedSpatialForce<T>;
+    using Class = ExternallyAppliedSpatialForce<T>;
     constexpr auto& cls_doc = doc.ExternallyAppliedSpatialForce;
     auto cls = DefineTemplateClassWithDefault<Class>(
         m, "ExternallyAppliedSpatialForce", param, cls_doc.doc);
@@ -1169,6 +1170,16 @@ void DoScalarDependentDefinitions(py::module m, T) {
     AddValueInstantiation<std::vector<Class>>(m);
   }
 
+  // ExternallyAppliedSpatialForceListMultiplexer
+  {
+    using Class = ExternallyAppliedSpatialForceListMultiplexer<T>;
+    constexpr auto& cls_doc = doc.ExternallyAppliedSpatialForceListMultiplexer;
+    auto cls = DefineTemplateClassWithDefault<Class, systems::LeafSystem<T>>(
+        m, "ExternallyAppliedSpatialForceListMultiplexer", param, cls_doc.doc);
+    cls  // BR
+        .def(py::init<int>(), py::arg("num_inputs"), cls_doc.ctor.doc);
+  }
+
   // Propeller
   {
     using Class = Propeller<T>;

bindings/pydrake/multibody/test/plant_test.py

@@ -66,6 +66,7 @@
     ContactResultsToLcmSystem,
     CoulombFriction_,
     ExternallyAppliedSpatialForce_,
+    ExternallyAppliedSpatialForceMultiplexer_,
     MultibodyPlant,
     MultibodyPlant_,
     MultibodyPlantConfig,
@@ -2359,22 +2360,28 @@ def get_body_from_frame_id(frame_id):
             body=plant.GetBodyByName("body1"))
         self.assertIsInstance(id_, GeometryId)
 
+    @numpy_compare.check_all_types
+    def test_externally_applied_spatial_force_multiplexer(self, T):
+        system = ExternallyAppliedSpatialForceMultiplexer_[T](num_inputs=2)
+        self.assertIsInstance(system, LeafSystem_[T])
+
     def test_propeller(self):
         plant = MultibodyPlant_[float](time_step=0.0)
         file_name = FindResourceOrThrow(
             "drake/multibody/benchmarks/acrobot/acrobot.sdf")
         Parser(plant).AddModelFromFile(file_name)
         plant.Finalize()
+        body = plant.GetBodyByName("Link1")
 
-        info = PropellerInfo(body_index=BodyIndex(1),
+        info = PropellerInfo(body_index=body.index(),
                              X_BP=RigidTransform_[float](),
                              thrust_ratio=1.0,
                              moment_ratio=0.1)
         self.assertEqual(info.thrust_ratio, 1.0)
         self.assertEqual(info.moment_ratio, 0.1)
         copy.copy(info)
 
-        prop = Propeller_[float](body_index=BodyIndex(1),
+        prop = Propeller_[float](body_index=body.index(),
                                  X_BP=RigidTransform_[float](),
                                  thrust_ratio=1.0,
                                  moment_ratio=0.1)

multibody/plant/BUILD.bazel

@@ -26,6 +26,7 @@ drake_cc_package_library(
         ":coulomb_friction",
         ":discrete_contact_pair",
         ":externally_applied_spatial_force",
+        ":externally_applied_spatial_force_list_multiplexer",
         ":hydroelastic_contact_info",
         ":hydroelastic_quadrature_point_data",
         ":hydroelastic_traction",
@@ -329,6 +330,17 @@ drake_cc_library(
     ],
 )
 
+drake_cc_library(
+    name = "externally_applied_spatial_force_list_multiplexer",
+    srcs = ["externally_applied_spatial_force_list_multiplexer.cc"],
+    hdrs = ["externally_applied_spatial_force_list_multiplexer.h"],
+    deps = [
+        ":externally_applied_spatial_force",
+        "//common:default_scalars",
+        "//systems/framework:leaf_system",
+    ],
+)
+
 drake_cc_library(
     name = "calc_distance_and_time_derivative",
     srcs = ["calc_distance_and_time_derivative.cc"],
@@ -515,6 +527,14 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "externally_applied_spatial_force_list_multiplexer_test",
+    deps = [
+        ":externally_applied_spatial_force_list_multiplexer",
+        "//systems/framework/test_utilities",
+    ],
+)
+
 drake_cc_googletest(
     name = "multibody_plant_test",
     data = [

multibody/plant/externally_applied_spatial_force_list_multiplexer.cc

@@ -0,0 +1,51 @@
+#include "drake/multibody/plant/externally_applied_spatial_force_list_multiplexer.h"
+
+#include <algorithm>
+
+#include "drake/common/default_scalars.h"
+
+namespace drake {
+namespace multibody {
+
+template <typename T>
+ExternallyAppliedSpatialForceListMultiplexer<T>::
+ExternallyAppliedSpatialForceListMultiplexer(int num_inputs)
+      : systems::LeafSystem<T>(
+          systems::SystemTypeTag<
+              ExternallyAppliedSpatialForceListMultiplexer>{}) {
+    DRAKE_DEMAND(num_inputs >= 0);
+    for (int i = 0; i < num_inputs; ++i) {
+      this->DeclareAbstractInputPort(
+          systems::kUseDefaultName,
+          Value<ListType>());
+    }
+    this->DeclareAbstractOutputPort(
+        "combined",
+        &ExternallyAppliedSpatialForceListMultiplexer<T>
+            ::CombineInputsToOutput);
+  }
+
+template <typename T>
+template <typename U>
+ExternallyAppliedSpatialForceListMultiplexer<T>::
+ExternallyAppliedSpatialForceListMultiplexer(
+    const ExternallyAppliedSpatialForceListMultiplexer<U>& other)
+    : ExternallyAppliedSpatialForceListMultiplexer(other.num_input_ports()) { }
+
+template <typename T>
+void ExternallyAppliedSpatialForceListMultiplexer<T>::CombineInputsToOutput(
+    const systems::Context<T>& context,
+    ListType* output) const {
+  output->clear();
+  for (int i = 0; i < this->num_input_ports(); ++i) {
+    const ListType& values_i =
+        this->get_input_port(i).template Eval<ListType>(context);
+    std::copy(values_i.begin(), values_i.end(), std::back_inserter(*output));
+  }
+}
+
+}  // namespace multibody
+}  // namespace drake
+
+DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS(
+    class ::drake::multibody::ExternallyAppliedSpatialForceListMultiplexer)

multibody/plant/externally_applied_spatial_force_list_multiplexer.h

@@ -0,0 +1,56 @@
+#pragma once
+
+#include <vector>
+
+#include "drake/common/drake_copyable.h"
+#include "drake/multibody/plant/externally_applied_spatial_force.h"
+#include "drake/systems/framework/leaf_system.h"
+
+namespace drake {
+namespace multibody {
+
+/**
+Combines multiple lists of externally applied spatial forces.
+
+@system
+name: ExternallyAppliedSpatialForceListMultiplexer
+input_ports:
+- u0
+- ...
+- u(N-1)
+output_ports:
+- combined
+@endsystem
+
+@tparam default_scalar
+*/
+template <typename T>
+class ExternallyAppliedSpatialForceListMultiplexer final
+    : public systems::LeafSystem<T> {
+ public:
+  DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(ExternallyAppliedSpatialForceListMultiplexer)
+
+  using ValueType = ExternallyAppliedSpatialForce<T>;
+  using ListType = std::vector<ValueType>;
+
+  /**
+  Constructor.
+  @param num_inputs Number of input ports to be added.
+  */
+  explicit ExternallyAppliedSpatialForceListMultiplexer(int num_inputs);
+
+  /**
+  Scalar-converting copy constructor.  See @ref system_scalar_conversion.
+  */
+  template <typename U>
+  explicit ExternallyAppliedSpatialForceListMultiplexer(
+      const ExternallyAppliedSpatialForceListMultiplexer<U>& other);
+
+ private:
+  // This is the calculator for the output port.
+  void CombineInputsToOutput(
+      const systems::Context<T>& context, ListType* output) const;
+};
+
+}  // namespace multibody
+}  // namespace drake

multibody/plant/test/externally_applied_spatial_force_list_multiplexer_test.cc

@@ -0,0 +1,113 @@
+#include "drake/multibody/plant/externally_applied_spatial_force_list_multiplexer.h"
+
+#include <memory>
+#include <stdexcept>
+#include <string>
+
+#include <gtest/gtest.h>
+
+#include "drake/systems/framework/fixed_input_port_value.h"
+#include "drake/systems/framework/test_utilities/scalar_conversion.h"
+
+namespace drake {
+namespace multibody {
+namespace {
+
+using ListType = std::vector<ExternallyAppliedSpatialForce<double>>;
+
+ExternallyAppliedSpatialForce<double> MakeDummyForce(BodyIndex body_index) {
+  return {
+      .body_index = body_index,
+      .p_BoBq_B = Eigen::Vector3d::Zero(),
+      .F_Bq_W = SpatialForce<double>::Zero()};
+}
+
+class ExternallyAppliedSpatialForceMultiplexerTest : public ::testing::Test {
+ protected:
+  void SetUp() override {
+    system_ =
+        std::make_unique<
+            ExternallyAppliedSpatialForceListMultiplexer<double>>(2);
+    context_ = system_->CreateDefaultContext();
+
+    input0_ = {MakeDummyForce(BodyIndex{0}), MakeDummyForce(BodyIndex{1})};
+    input1_ = {MakeDummyForce(BodyIndex{1}), MakeDummyForce(BodyIndex{2})};
+  }
+
+  std::unique_ptr<ExternallyAppliedSpatialForceListMultiplexer<double>> system_;
+  std::unique_ptr<systems::Context<double>> context_;
+  ListType input0_;
+  ListType input1_;
+};
+
+// Tests that the system exports the correct topology.
+TEST_F(ExternallyAppliedSpatialForceMultiplexerTest, Topology) {
+  const int num_inputs = 2;
+  ASSERT_EQ(num_inputs, system_->num_input_ports());
+  for (int i = 0; i < num_inputs; ++i) {
+    const systems::InputPort<double>& input_port = system_->get_input_port(i);
+    EXPECT_EQ(systems::kAbstractValued, input_port.get_data_type());
+  }
+
+  ASSERT_EQ(1, system_->num_output_ports());
+  const systems::OutputPort<double>& output_port = system_->get_output_port(0);
+  EXPECT_EQ(&output_port, &system_->get_output_port());
+  EXPECT_EQ(systems::kAbstractValued, output_port.get_data_type());
+}
+
+void AssertEqual(
+    const ExternallyAppliedSpatialForce<double>& lhs,
+    const ExternallyAppliedSpatialForce<double>& rhs) {
+  ASSERT_EQ(lhs.body_index, rhs.body_index);
+  ASSERT_EQ(lhs.p_BoBq_B, rhs.p_BoBq_B);
+  ASSERT_EQ(lhs.F_Bq_W.get_coeffs(), rhs.F_Bq_W.get_coeffs());
+}
+
+void AssertEqual(const ListType& lhs, const ListType& rhs) {
+  size_t count = lhs.size();
+  ASSERT_EQ(count, rhs.size());
+  for (size_t i = 0; i < count; ++i) {
+    SCOPED_TRACE(fmt::format("Element {}", i));
+    AssertEqual(lhs[i], rhs[i]);
+  }
+}
+
+// Tests that the system computes the correct sum.
+TEST_F(ExternallyAppliedSpatialForceMultiplexerTest, AddTwoVectors) {
+  system_->get_input_port(0).FixValue(context_.get(), input0_);
+  system_->get_input_port(1).FixValue(context_.get(), input1_);
+  const ListType expected = {
+      MakeDummyForce(BodyIndex{0}),
+      MakeDummyForce(BodyIndex{1}),
+      MakeDummyForce(BodyIndex{1}),
+      MakeDummyForce(BodyIndex{2})};
+  const ListType actual = system_->get_output_port().Eval<ListType>(*context_);
+  AssertEqual(actual, expected);
+}
+
+// Tests that system allocates no state variables in the context_.
+TEST_F(ExternallyAppliedSpatialForceMultiplexerTest, Stateless) {
+  EXPECT_EQ(0, context_->num_continuous_states());
+  EXPECT_EQ(0, context_->num_discrete_state_groups());
+}
+
+// Asserts that adders have direct-feedthrough from all inputs to the output.
+TEST_F(ExternallyAppliedSpatialForceMultiplexerTest, AdderIsDirectFeedthrough) {
+  EXPECT_TRUE(system_->HasAnyDirectFeedthrough());
+  const int output_index = 0;
+  for (int i = 0; i < system_->num_input_ports(); ++i) {
+    EXPECT_TRUE(system_->HasDirectFeedthrough(i, output_index));
+  }
+}
+
+TEST_F(ExternallyAppliedSpatialForceMultiplexerTest, ToAutoDiff) {
+  EXPECT_TRUE(systems::is_autodiffxd_convertible(*system_));
+}
+
+TEST_F(ExternallyAppliedSpatialForceMultiplexerTest, ToSymbolic) {
+  EXPECT_TRUE(systems::is_symbolic_convertible(*system_));
+}
+
+}  // namespace
+}  // namespace multibody
+}  // namespace drake

systems/primitives/test/adder_test.cc

@@ -10,16 +10,14 @@
 #include "drake/systems/framework/fixed_input_port_value.h"
 #include "drake/systems/framework/test_utilities/scalar_conversion.h"
 
-using std::make_unique;
-
 namespace drake {
 namespace systems {
 namespace {
 
 class AdderTest : public ::testing::Test {
  protected:
   void SetUp() override {
-    adder_.reset(new Adder<double>(2 /* inputs */, 3 /* size */));
+    adder_ = std::make_unique<Adder<double>>(2 /* inputs */, 3 /* size */);
     context_ = adder_->CreateDefaultContext();
     input0_ = Eigen::VectorXd(3 /* size */);
     input1_ = Eigen::VectorXd(3 /* size */);
@@ -64,6 +62,7 @@ TEST_F(AdderTest, AddTwoVectors) {
 // Tests that Adder allocates no state variables in the context_.
 TEST_F(AdderTest, AdderIsStateless) {
   EXPECT_EQ(0, context_->num_continuous_states());
+  EXPECT_EQ(0, context_->num_discrete_state_groups());
 }
 
 // Asserts that adders have direct-feedthrough from all inputs to the output.