From 62383a4f9eaafc05605e8a62c3ed712c5d2ce738 Mon Sep 17 00:00:00 2001 From: Eric Cousineau Date: Mon, 24 Oct 2022 17:53:36 -0400 Subject: [PATCH 1/9] Add ExternallyAppliedSpatialForceMultiplexer py plant: - Remove unneeded qualifier for ExternallyAppliedSpatialForce - test_propeller: Use body name rather than magic number adder_test: - Add check for discrete number of states - Use std::make_unique instead of .reset() --- bindings/pydrake/multibody/plant_py.cc | 13 +- bindings/pydrake/multibody/test/plant_test.py | 11 +- multibody/plant/BUILD.bazel | 20 ++++ ...nally_applied_spatial_force_multiplexer.cc | 50 ++++++++ ...rnally_applied_spatial_force_multiplexer.h | 56 +++++++++ ..._applied_spatial_force_multiplexer_test.cc | 112 ++++++++++++++++++ systems/primitives/test/adder_test.cc | 5 +- 7 files changed, 261 insertions(+), 6 deletions(-) create mode 100644 multibody/plant/externally_applied_spatial_force_multiplexer.cc create mode 100644 multibody/plant/externally_applied_spatial_force_multiplexer.h create mode 100644 multibody/plant/test/externally_applied_spatial_force_multiplexer_test.cc diff --git a/bindings/pydrake/multibody/plant_py.cc b/bindings/pydrake/multibody/plant_py.cc index 057407a232f6..8afa8bcfe266 100644 --- a/bindings/pydrake/multibody/plant_py.cc +++ b/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_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; + using Class = ExternallyAppliedSpatialForce; constexpr auto& cls_doc = doc.ExternallyAppliedSpatialForce; auto cls = DefineTemplateClassWithDefault( m, "ExternallyAppliedSpatialForce", param, cls_doc.doc); @@ -1169,6 +1170,16 @@ void DoScalarDependentDefinitions(py::module m, T) { AddValueInstantiation>(m); } + // ExternallyAppliedSpatialForceMultiplexer + { + using Class = ExternallyAppliedSpatialForceMultiplexer; + constexpr auto& cls_doc = doc.ExternallyAppliedSpatialForceMultiplexer; + auto cls = DefineTemplateClassWithDefault>( + m, "ExternallyAppliedSpatialForceMultiplexer", param, cls_doc.doc); + cls // BR + .def(py::init(), py::arg("num_inputs"), cls_doc.ctor.doc); + } + // Propeller { using Class = Propeller; diff --git a/bindings/pydrake/multibody/test/plant_test.py b/bindings/pydrake/multibody/test/plant_test.py index b67fb1a71163..e6f1b3beb9c0 100644 --- a/bindings/pydrake/multibody/test/plant_test.py +++ b/bindings/pydrake/multibody/test/plant_test.py @@ -66,6 +66,7 @@ ContactResultsToLcmSystem, CoulombFriction_, ExternallyAppliedSpatialForce_, + ExternallyAppliedSpatialForceMultiplexer_, MultibodyPlant, MultibodyPlant_, MultibodyPlantConfig, @@ -2359,14 +2360,20 @@ 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) @@ -2374,7 +2381,7 @@ def test_propeller(self): 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) diff --git a/multibody/plant/BUILD.bazel b/multibody/plant/BUILD.bazel index c5f80decfcfe..1867332f2b8a 100644 --- a/multibody/plant/BUILD.bazel +++ b/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_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_multiplexer", + srcs = ["externally_applied_spatial_force_multiplexer.cc"], + hdrs = ["externally_applied_spatial_force_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_multiplexer_test", + deps = [ + ":externally_applied_spatial_force_multiplexer", + "//systems/framework/test_utilities", + ], +) + drake_cc_googletest( name = "multibody_plant_test", data = [ diff --git a/multibody/plant/externally_applied_spatial_force_multiplexer.cc b/multibody/plant/externally_applied_spatial_force_multiplexer.cc new file mode 100644 index 000000000000..b25e20629f92 --- /dev/null +++ b/multibody/plant/externally_applied_spatial_force_multiplexer.cc @@ -0,0 +1,50 @@ +#include "drake/multibody/plant/externally_applied_spatial_force_multiplexer.h" + +#include + +#include "drake/common/default_scalars.h" + +namespace drake { +namespace multibody { + +template +ExternallyAppliedSpatialForceMultiplexer:: +ExternallyAppliedSpatialForceMultiplexer(int num_inputs) + : systems::LeafSystem( + systems::SystemTypeTag{}) { + DRAKE_DEMAND(num_inputs >= 0); + for (int i = 0; i < num_inputs; ++i) { + this->DeclareAbstractInputPort( + systems::kUseDefaultName, + Value()); + } + this->DeclareAbstractOutputPort( + "combined", + &ExternallyAppliedSpatialForceMultiplexer::CombineInputsToOutput); + } + +template +template +ExternallyAppliedSpatialForceMultiplexer:: +ExternallyAppliedSpatialForceMultiplexer( + const ExternallyAppliedSpatialForceMultiplexer& other) + : ExternallyAppliedSpatialForceMultiplexer(other.num_input_ports()) { } + +template +void ExternallyAppliedSpatialForceMultiplexer::CombineInputsToOutput( + const systems::Context& context, + ListType* output) const { + output->clear(); + // auto iter = output->end(); + for (int i = 0; i < this->num_input_ports(); ++i) { + const ListType& values_i = + this->get_input_port(i).template Eval(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::ExternallyAppliedSpatialForceMultiplexer) diff --git a/multibody/plant/externally_applied_spatial_force_multiplexer.h b/multibody/plant/externally_applied_spatial_force_multiplexer.h new file mode 100644 index 000000000000..058cddd086ec --- /dev/null +++ b/multibody/plant/externally_applied_spatial_force_multiplexer.h @@ -0,0 +1,56 @@ +#pragma once + +#include + +#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: ExternallyAppliedSpatialForceMultiplexer +input_ports: +- u0 +- ... +- u(N-1) +output_ports: +- combined +@endsystem + +@tparam default_scalar +*/ +template +class ExternallyAppliedSpatialForceMultiplexer final + : public systems::LeafSystem { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(ExternallyAppliedSpatialForceMultiplexer) + + using ValueType = ExternallyAppliedSpatialForce; + using ListType = std::vector; + + /** + Constructor. + @param num_inputs Number of input ports to be added. + */ + + explicit ExternallyAppliedSpatialForceMultiplexer(int num_inputs); + /** + Scalar-converting copy constructor. See @ref system_scalar_conversion. + */ + template + explicit ExternallyAppliedSpatialForceMultiplexer( + const ExternallyAppliedSpatialForceMultiplexer& other); + + private: + // This is the calculator for the output port. + void CombineInputsToOutput( + const systems::Context& context, ListType* output) const; +}; + +} // namespace multibody +} // namespace drake diff --git a/multibody/plant/test/externally_applied_spatial_force_multiplexer_test.cc b/multibody/plant/test/externally_applied_spatial_force_multiplexer_test.cc new file mode 100644 index 000000000000..6d3e4a5138c6 --- /dev/null +++ b/multibody/plant/test/externally_applied_spatial_force_multiplexer_test.cc @@ -0,0 +1,112 @@ +#include "drake/multibody/plant/externally_applied_spatial_force_multiplexer.h" + +#include +#include +#include + +#include + +#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 MakeDummyForce(BodyIndex body_index) { + return { + .body_index = body_index, + .p_BoBq_B = Eigen::Vector3d::Zero(), + .F_Bq_W = SpatialForce::Zero()}; +} + +class ExternallyAppliedSpatialForceMultiplexerTest : public ::testing::Test { + protected: + void SetUp() override { + system_ = + std::make_unique>(2); + context_ = system_->CreateDefaultContext(); + + input0_ = {MakeDummyForce(BodyIndex{0}), MakeDummyForce(BodyIndex{1})}; + input1_ = {MakeDummyForce(BodyIndex{1}), MakeDummyForce(BodyIndex{2})}; + } + + std::unique_ptr> system_; + std::unique_ptr> 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& 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& 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& lhs, + const ExternallyAppliedSpatialForce& 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(*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 diff --git a/systems/primitives/test/adder_test.cc b/systems/primitives/test/adder_test.cc index 4731dd727215..11fa5f0c1b06 100644 --- a/systems/primitives/test/adder_test.cc +++ b/systems/primitives/test/adder_test.cc @@ -10,8 +10,6 @@ #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 { @@ -19,7 +17,7 @@ namespace { class AdderTest : public ::testing::Test { protected: void SetUp() override { - adder_.reset(new Adder(2 /* inputs */, 3 /* size */)); + adder_ = std::make_unique>(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. From 45639a89bc35d9f4a3fd358ec94692c0980f2671 Mon Sep 17 00:00:00 2001 From: Eric Cousineau Date: Wed, 26 Oct 2022 08:22:25 -0400 Subject: [PATCH 2/9] address review --- bindings/pydrake/multibody/plant_py.cc | 8 +++---- ...nally_applied_spatial_force_multiplexer.cc | 23 ++++++++++--------- ...rnally_applied_spatial_force_multiplexer.h | 12 +++++----- ..._applied_spatial_force_multiplexer_test.cc | 5 ++-- 4 files changed, 25 insertions(+), 23 deletions(-) diff --git a/bindings/pydrake/multibody/plant_py.cc b/bindings/pydrake/multibody/plant_py.cc index 8afa8bcfe266..905cbd071cdf 100644 --- a/bindings/pydrake/multibody/plant_py.cc +++ b/bindings/pydrake/multibody/plant_py.cc @@ -1170,12 +1170,12 @@ void DoScalarDependentDefinitions(py::module m, T) { AddValueInstantiation>(m); } - // ExternallyAppliedSpatialForceMultiplexer + // ExternallyAppliedSpatialForceListMultiplexer { - using Class = ExternallyAppliedSpatialForceMultiplexer; - constexpr auto& cls_doc = doc.ExternallyAppliedSpatialForceMultiplexer; + using Class = ExternallyAppliedSpatialForceListMultiplexer; + constexpr auto& cls_doc = doc.ExternallyAppliedSpatialForceListMultiplexer; auto cls = DefineTemplateClassWithDefault>( - m, "ExternallyAppliedSpatialForceMultiplexer", param, cls_doc.doc); + m, "ExternallyAppliedSpatialForceListMultiplexer", param, cls_doc.doc); cls // BR .def(py::init(), py::arg("num_inputs"), cls_doc.ctor.doc); } diff --git a/multibody/plant/externally_applied_spatial_force_multiplexer.cc b/multibody/plant/externally_applied_spatial_force_multiplexer.cc index b25e20629f92..b7932f9e6338 100644 --- a/multibody/plant/externally_applied_spatial_force_multiplexer.cc +++ b/multibody/plant/externally_applied_spatial_force_multiplexer.cc @@ -8,10 +8,11 @@ namespace drake { namespace multibody { template -ExternallyAppliedSpatialForceMultiplexer:: -ExternallyAppliedSpatialForceMultiplexer(int num_inputs) +ExternallyAppliedSpatialForceListMultiplexer:: +ExternallyAppliedSpatialForceListMultiplexer(int num_inputs) : systems::LeafSystem( - systems::SystemTypeTag{}) { + systems::SystemTypeTag< + ExternallyAppliedSpatialForceListMultiplexer>{}) { DRAKE_DEMAND(num_inputs >= 0); for (int i = 0; i < num_inputs; ++i) { this->DeclareAbstractInputPort( @@ -20,22 +21,22 @@ ExternallyAppliedSpatialForceMultiplexer(int num_inputs) } this->DeclareAbstractOutputPort( "combined", - &ExternallyAppliedSpatialForceMultiplexer::CombineInputsToOutput); + &ExternallyAppliedSpatialForceListMultiplexer + ::CombineInputsToOutput); } template template -ExternallyAppliedSpatialForceMultiplexer:: -ExternallyAppliedSpatialForceMultiplexer( - const ExternallyAppliedSpatialForceMultiplexer& other) - : ExternallyAppliedSpatialForceMultiplexer(other.num_input_ports()) { } +ExternallyAppliedSpatialForceListMultiplexer:: +ExternallyAppliedSpatialForceListMultiplexer( + const ExternallyAppliedSpatialForceListMultiplexer& other) + : ExternallyAppliedSpatialForceListMultiplexer(other.num_input_ports()) { } template -void ExternallyAppliedSpatialForceMultiplexer::CombineInputsToOutput( +void ExternallyAppliedSpatialForceListMultiplexer::CombineInputsToOutput( const systems::Context& context, ListType* output) const { output->clear(); - // auto iter = output->end(); for (int i = 0; i < this->num_input_ports(); ++i) { const ListType& values_i = this->get_input_port(i).template Eval(context); @@ -47,4 +48,4 @@ void ExternallyAppliedSpatialForceMultiplexer::CombineInputsToOutput( } // namespace drake DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( - class ::drake::multibody::ExternallyAppliedSpatialForceMultiplexer) + class ::drake::multibody::ExternallyAppliedSpatialForceListMultiplexer) diff --git a/multibody/plant/externally_applied_spatial_force_multiplexer.h b/multibody/plant/externally_applied_spatial_force_multiplexer.h index 058cddd086ec..125677b132cf 100644 --- a/multibody/plant/externally_applied_spatial_force_multiplexer.h +++ b/multibody/plant/externally_applied_spatial_force_multiplexer.h @@ -13,7 +13,7 @@ namespace multibody { Combines multiple lists of externally applied spatial forces. @system -name: ExternallyAppliedSpatialForceMultiplexer +name: ExternallyAppliedSpatialForceListMultiplexer input_ports: - u0 - ... @@ -25,10 +25,10 @@ name: ExternallyAppliedSpatialForceMultiplexer @tparam default_scalar */ template -class ExternallyAppliedSpatialForceMultiplexer final +class ExternallyAppliedSpatialForceListMultiplexer final : public systems::LeafSystem { public: - DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(ExternallyAppliedSpatialForceMultiplexer) + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(ExternallyAppliedSpatialForceListMultiplexer) using ValueType = ExternallyAppliedSpatialForce; using ListType = std::vector; @@ -37,14 +37,14 @@ class ExternallyAppliedSpatialForceMultiplexer final Constructor. @param num_inputs Number of input ports to be added. */ + explicit ExternallyAppliedSpatialForceListMultiplexer(int num_inputs); - explicit ExternallyAppliedSpatialForceMultiplexer(int num_inputs); /** Scalar-converting copy constructor. See @ref system_scalar_conversion. */ template - explicit ExternallyAppliedSpatialForceMultiplexer( - const ExternallyAppliedSpatialForceMultiplexer& other); + explicit ExternallyAppliedSpatialForceListMultiplexer( + const ExternallyAppliedSpatialForceListMultiplexer& other); private: // This is the calculator for the output port. diff --git a/multibody/plant/test/externally_applied_spatial_force_multiplexer_test.cc b/multibody/plant/test/externally_applied_spatial_force_multiplexer_test.cc index 6d3e4a5138c6..8b704d47d174 100644 --- a/multibody/plant/test/externally_applied_spatial_force_multiplexer_test.cc +++ b/multibody/plant/test/externally_applied_spatial_force_multiplexer_test.cc @@ -26,14 +26,15 @@ class ExternallyAppliedSpatialForceMultiplexerTest : public ::testing::Test { protected: void SetUp() override { system_ = - std::make_unique>(2); + std::make_unique< + ExternallyAppliedSpatialForceListMultiplexer>(2); context_ = system_->CreateDefaultContext(); input0_ = {MakeDummyForce(BodyIndex{0}), MakeDummyForce(BodyIndex{1})}; input1_ = {MakeDummyForce(BodyIndex{1}), MakeDummyForce(BodyIndex{2})}; } - std::unique_ptr> system_; + std::unique_ptr> system_; std::unique_ptr> context_; ListType input0_; ListType input1_; From 81623e4ff93e47b3bf65afe1cff15eba2f834fb5 Mon Sep 17 00:00:00 2001 From: Eric Cousineau Date: Wed, 26 Oct 2022 08:25:25 -0400 Subject: [PATCH 3/9] rename files --- bindings/pydrake/multibody/plant_py.cc | 2 +- multibody/plant/BUILD.bazel | 12 ++++++------ ...rnally_applied_spatial_force_list_multiplexer.cc} | 2 +- ...ernally_applied_spatial_force_list_multiplexer.h} | 0 ...y_applied_spatial_force_list_multiplexer_test.cc} | 2 +- 5 files changed, 9 insertions(+), 9 deletions(-) rename multibody/plant/{externally_applied_spatial_force_multiplexer.cc => externally_applied_spatial_force_list_multiplexer.cc} (98%) rename multibody/plant/{externally_applied_spatial_force_multiplexer.h => externally_applied_spatial_force_list_multiplexer.h} (100%) rename multibody/plant/test/{externally_applied_spatial_force_multiplexer_test.cc => externally_applied_spatial_force_list_multiplexer_test.cc} (99%) diff --git a/bindings/pydrake/multibody/plant_py.cc b/bindings/pydrake/multibody/plant_py.cc index 905cbd071cdf..8a31d83cc9f5 100644 --- a/bindings/pydrake/multibody/plant_py.cc +++ b/bindings/pydrake/multibody/plant_py.cc @@ -19,7 +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_multiplexer.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" diff --git a/multibody/plant/BUILD.bazel b/multibody/plant/BUILD.bazel index 1867332f2b8a..126691cdcf74 100644 --- a/multibody/plant/BUILD.bazel +++ b/multibody/plant/BUILD.bazel @@ -26,7 +26,7 @@ drake_cc_package_library( ":coulomb_friction", ":discrete_contact_pair", ":externally_applied_spatial_force", - ":externally_applied_spatial_force_multiplexer", + ":externally_applied_spatial_force_list_multiplexer", ":hydroelastic_contact_info", ":hydroelastic_quadrature_point_data", ":hydroelastic_traction", @@ -331,9 +331,9 @@ drake_cc_library( ) drake_cc_library( - name = "externally_applied_spatial_force_multiplexer", - srcs = ["externally_applied_spatial_force_multiplexer.cc"], - hdrs = ["externally_applied_spatial_force_multiplexer.h"], + 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", @@ -528,9 +528,9 @@ drake_cc_googletest( ) drake_cc_googletest( - name = "externally_applied_spatial_force_multiplexer_test", + name = "externally_applied_spatial_force_list_multiplexer_test", deps = [ - ":externally_applied_spatial_force_multiplexer", + ":externally_applied_spatial_force_list_multiplexer", "//systems/framework/test_utilities", ], ) diff --git a/multibody/plant/externally_applied_spatial_force_multiplexer.cc b/multibody/plant/externally_applied_spatial_force_list_multiplexer.cc similarity index 98% rename from multibody/plant/externally_applied_spatial_force_multiplexer.cc rename to multibody/plant/externally_applied_spatial_force_list_multiplexer.cc index b7932f9e6338..6e979394675c 100644 --- a/multibody/plant/externally_applied_spatial_force_multiplexer.cc +++ b/multibody/plant/externally_applied_spatial_force_list_multiplexer.cc @@ -1,4 +1,4 @@ -#include "drake/multibody/plant/externally_applied_spatial_force_multiplexer.h" +#include "drake/multibody/plant/externally_applied_spatial_force_list_multiplexer.h" #include diff --git a/multibody/plant/externally_applied_spatial_force_multiplexer.h b/multibody/plant/externally_applied_spatial_force_list_multiplexer.h similarity index 100% rename from multibody/plant/externally_applied_spatial_force_multiplexer.h rename to multibody/plant/externally_applied_spatial_force_list_multiplexer.h diff --git a/multibody/plant/test/externally_applied_spatial_force_multiplexer_test.cc b/multibody/plant/test/externally_applied_spatial_force_list_multiplexer_test.cc similarity index 99% rename from multibody/plant/test/externally_applied_spatial_force_multiplexer_test.cc rename to multibody/plant/test/externally_applied_spatial_force_list_multiplexer_test.cc index 8b704d47d174..369c0926427f 100644 --- a/multibody/plant/test/externally_applied_spatial_force_multiplexer_test.cc +++ b/multibody/plant/test/externally_applied_spatial_force_list_multiplexer_test.cc @@ -1,4 +1,4 @@ -#include "drake/multibody/plant/externally_applied_spatial_force_multiplexer.h" +#include "drake/multibody/plant/externally_applied_spatial_force_list_multiplexer.h" #include #include From 7be80e78d07bea6b889c4df766bee89b3e0fb9c4 Mon Sep 17 00:00:00 2001 From: Eric Cousineau Date: Wed, 26 Oct 2022 08:35:10 -0400 Subject: [PATCH 4/9] better docs --- .../plant/externally_applied_spatial_force_list_multiplexer.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/multibody/plant/externally_applied_spatial_force_list_multiplexer.h b/multibody/plant/externally_applied_spatial_force_list_multiplexer.h index 125677b132cf..d9809892c27c 100644 --- a/multibody/plant/externally_applied_spatial_force_list_multiplexer.h +++ b/multibody/plant/externally_applied_spatial_force_list_multiplexer.h @@ -10,7 +10,7 @@ namespace drake { namespace multibody { /** -Combines multiple lists of externally applied spatial forces. +Combines multiple std::vector<>'s of ExternallyAppliedSpatialForce. @system name: ExternallyAppliedSpatialForceListMultiplexer From 6427f330d70f1697e33e1b54985e734650cf8ecd Mon Sep 17 00:00:00 2001 From: Eric Cousineau Date: Wed, 26 Oct 2022 08:35:24 -0400 Subject: [PATCH 5/9] Revert "rename files" This reverts commit 81623e4ff93e47b3bf65afe1cff15eba2f834fb5. --- bindings/pydrake/multibody/plant_py.cc | 2 +- multibody/plant/BUILD.bazel | 12 ++++++------ ... externally_applied_spatial_force_multiplexer.cc} | 2 +- ...> externally_applied_spatial_force_multiplexer.h} | 0 ...rnally_applied_spatial_force_multiplexer_test.cc} | 2 +- 5 files changed, 9 insertions(+), 9 deletions(-) rename multibody/plant/{externally_applied_spatial_force_list_multiplexer.cc => externally_applied_spatial_force_multiplexer.cc} (98%) rename multibody/plant/{externally_applied_spatial_force_list_multiplexer.h => externally_applied_spatial_force_multiplexer.h} (100%) rename multibody/plant/test/{externally_applied_spatial_force_list_multiplexer_test.cc => externally_applied_spatial_force_multiplexer_test.cc} (99%) diff --git a/bindings/pydrake/multibody/plant_py.cc b/bindings/pydrake/multibody/plant_py.cc index 8a31d83cc9f5..905cbd071cdf 100644 --- a/bindings/pydrake/multibody/plant_py.cc +++ b/bindings/pydrake/multibody/plant_py.cc @@ -19,7 +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/externally_applied_spatial_force_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" diff --git a/multibody/plant/BUILD.bazel b/multibody/plant/BUILD.bazel index 126691cdcf74..1867332f2b8a 100644 --- a/multibody/plant/BUILD.bazel +++ b/multibody/plant/BUILD.bazel @@ -26,7 +26,7 @@ drake_cc_package_library( ":coulomb_friction", ":discrete_contact_pair", ":externally_applied_spatial_force", - ":externally_applied_spatial_force_list_multiplexer", + ":externally_applied_spatial_force_multiplexer", ":hydroelastic_contact_info", ":hydroelastic_quadrature_point_data", ":hydroelastic_traction", @@ -331,9 +331,9 @@ 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"], + name = "externally_applied_spatial_force_multiplexer", + srcs = ["externally_applied_spatial_force_multiplexer.cc"], + hdrs = ["externally_applied_spatial_force_multiplexer.h"], deps = [ ":externally_applied_spatial_force", "//common:default_scalars", @@ -528,9 +528,9 @@ drake_cc_googletest( ) drake_cc_googletest( - name = "externally_applied_spatial_force_list_multiplexer_test", + name = "externally_applied_spatial_force_multiplexer_test", deps = [ - ":externally_applied_spatial_force_list_multiplexer", + ":externally_applied_spatial_force_multiplexer", "//systems/framework/test_utilities", ], ) diff --git a/multibody/plant/externally_applied_spatial_force_list_multiplexer.cc b/multibody/plant/externally_applied_spatial_force_multiplexer.cc similarity index 98% rename from multibody/plant/externally_applied_spatial_force_list_multiplexer.cc rename to multibody/plant/externally_applied_spatial_force_multiplexer.cc index 6e979394675c..b7932f9e6338 100644 --- a/multibody/plant/externally_applied_spatial_force_list_multiplexer.cc +++ b/multibody/plant/externally_applied_spatial_force_multiplexer.cc @@ -1,4 +1,4 @@ -#include "drake/multibody/plant/externally_applied_spatial_force_list_multiplexer.h" +#include "drake/multibody/plant/externally_applied_spatial_force_multiplexer.h" #include diff --git a/multibody/plant/externally_applied_spatial_force_list_multiplexer.h b/multibody/plant/externally_applied_spatial_force_multiplexer.h similarity index 100% rename from multibody/plant/externally_applied_spatial_force_list_multiplexer.h rename to multibody/plant/externally_applied_spatial_force_multiplexer.h diff --git a/multibody/plant/test/externally_applied_spatial_force_list_multiplexer_test.cc b/multibody/plant/test/externally_applied_spatial_force_multiplexer_test.cc similarity index 99% rename from multibody/plant/test/externally_applied_spatial_force_list_multiplexer_test.cc rename to multibody/plant/test/externally_applied_spatial_force_multiplexer_test.cc index 369c0926427f..8b704d47d174 100644 --- a/multibody/plant/test/externally_applied_spatial_force_list_multiplexer_test.cc +++ b/multibody/plant/test/externally_applied_spatial_force_multiplexer_test.cc @@ -1,4 +1,4 @@ -#include "drake/multibody/plant/externally_applied_spatial_force_list_multiplexer.h" +#include "drake/multibody/plant/externally_applied_spatial_force_multiplexer.h" #include #include From 334891c3a3bfc494ac5167fa8feaf536e9308cb3 Mon Sep 17 00:00:00 2001 From: Eric Cousineau Date: Wed, 26 Oct 2022 08:35:25 -0400 Subject: [PATCH 6/9] Revert "address review" This reverts commit 45639a89bc35d9f4a3fd358ec94692c0980f2671. --- bindings/pydrake/multibody/plant_py.cc | 8 +++---- ...nally_applied_spatial_force_multiplexer.cc | 23 +++++++++---------- ...rnally_applied_spatial_force_multiplexer.h | 12 +++++----- ..._applied_spatial_force_multiplexer_test.cc | 5 ++-- 4 files changed, 23 insertions(+), 25 deletions(-) diff --git a/bindings/pydrake/multibody/plant_py.cc b/bindings/pydrake/multibody/plant_py.cc index 905cbd071cdf..8afa8bcfe266 100644 --- a/bindings/pydrake/multibody/plant_py.cc +++ b/bindings/pydrake/multibody/plant_py.cc @@ -1170,12 +1170,12 @@ void DoScalarDependentDefinitions(py::module m, T) { AddValueInstantiation>(m); } - // ExternallyAppliedSpatialForceListMultiplexer + // ExternallyAppliedSpatialForceMultiplexer { - using Class = ExternallyAppliedSpatialForceListMultiplexer; - constexpr auto& cls_doc = doc.ExternallyAppliedSpatialForceListMultiplexer; + using Class = ExternallyAppliedSpatialForceMultiplexer; + constexpr auto& cls_doc = doc.ExternallyAppliedSpatialForceMultiplexer; auto cls = DefineTemplateClassWithDefault>( - m, "ExternallyAppliedSpatialForceListMultiplexer", param, cls_doc.doc); + m, "ExternallyAppliedSpatialForceMultiplexer", param, cls_doc.doc); cls // BR .def(py::init(), py::arg("num_inputs"), cls_doc.ctor.doc); } diff --git a/multibody/plant/externally_applied_spatial_force_multiplexer.cc b/multibody/plant/externally_applied_spatial_force_multiplexer.cc index b7932f9e6338..b25e20629f92 100644 --- a/multibody/plant/externally_applied_spatial_force_multiplexer.cc +++ b/multibody/plant/externally_applied_spatial_force_multiplexer.cc @@ -8,11 +8,10 @@ namespace drake { namespace multibody { template -ExternallyAppliedSpatialForceListMultiplexer:: -ExternallyAppliedSpatialForceListMultiplexer(int num_inputs) +ExternallyAppliedSpatialForceMultiplexer:: +ExternallyAppliedSpatialForceMultiplexer(int num_inputs) : systems::LeafSystem( - systems::SystemTypeTag< - ExternallyAppliedSpatialForceListMultiplexer>{}) { + systems::SystemTypeTag{}) { DRAKE_DEMAND(num_inputs >= 0); for (int i = 0; i < num_inputs; ++i) { this->DeclareAbstractInputPort( @@ -21,22 +20,22 @@ ExternallyAppliedSpatialForceListMultiplexer(int num_inputs) } this->DeclareAbstractOutputPort( "combined", - &ExternallyAppliedSpatialForceListMultiplexer - ::CombineInputsToOutput); + &ExternallyAppliedSpatialForceMultiplexer::CombineInputsToOutput); } template template -ExternallyAppliedSpatialForceListMultiplexer:: -ExternallyAppliedSpatialForceListMultiplexer( - const ExternallyAppliedSpatialForceListMultiplexer& other) - : ExternallyAppliedSpatialForceListMultiplexer(other.num_input_ports()) { } +ExternallyAppliedSpatialForceMultiplexer:: +ExternallyAppliedSpatialForceMultiplexer( + const ExternallyAppliedSpatialForceMultiplexer& other) + : ExternallyAppliedSpatialForceMultiplexer(other.num_input_ports()) { } template -void ExternallyAppliedSpatialForceListMultiplexer::CombineInputsToOutput( +void ExternallyAppliedSpatialForceMultiplexer::CombineInputsToOutput( const systems::Context& context, ListType* output) const { output->clear(); + // auto iter = output->end(); for (int i = 0; i < this->num_input_ports(); ++i) { const ListType& values_i = this->get_input_port(i).template Eval(context); @@ -48,4 +47,4 @@ void ExternallyAppliedSpatialForceListMultiplexer::CombineInputsToOutput( } // namespace drake DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( - class ::drake::multibody::ExternallyAppliedSpatialForceListMultiplexer) + class ::drake::multibody::ExternallyAppliedSpatialForceMultiplexer) diff --git a/multibody/plant/externally_applied_spatial_force_multiplexer.h b/multibody/plant/externally_applied_spatial_force_multiplexer.h index d9809892c27c..ad14962190a4 100644 --- a/multibody/plant/externally_applied_spatial_force_multiplexer.h +++ b/multibody/plant/externally_applied_spatial_force_multiplexer.h @@ -13,7 +13,7 @@ namespace multibody { Combines multiple std::vector<>'s of ExternallyAppliedSpatialForce. @system -name: ExternallyAppliedSpatialForceListMultiplexer +name: ExternallyAppliedSpatialForceMultiplexer input_ports: - u0 - ... @@ -25,10 +25,10 @@ name: ExternallyAppliedSpatialForceListMultiplexer @tparam default_scalar */ template -class ExternallyAppliedSpatialForceListMultiplexer final +class ExternallyAppliedSpatialForceMultiplexer final : public systems::LeafSystem { public: - DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(ExternallyAppliedSpatialForceListMultiplexer) + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(ExternallyAppliedSpatialForceMultiplexer) using ValueType = ExternallyAppliedSpatialForce; using ListType = std::vector; @@ -37,14 +37,14 @@ class ExternallyAppliedSpatialForceListMultiplexer final Constructor. @param num_inputs Number of input ports to be added. */ - explicit ExternallyAppliedSpatialForceListMultiplexer(int num_inputs); + explicit ExternallyAppliedSpatialForceMultiplexer(int num_inputs); /** Scalar-converting copy constructor. See @ref system_scalar_conversion. */ template - explicit ExternallyAppliedSpatialForceListMultiplexer( - const ExternallyAppliedSpatialForceListMultiplexer& other); + explicit ExternallyAppliedSpatialForceMultiplexer( + const ExternallyAppliedSpatialForceMultiplexer& other); private: // This is the calculator for the output port. diff --git a/multibody/plant/test/externally_applied_spatial_force_multiplexer_test.cc b/multibody/plant/test/externally_applied_spatial_force_multiplexer_test.cc index 8b704d47d174..6d3e4a5138c6 100644 --- a/multibody/plant/test/externally_applied_spatial_force_multiplexer_test.cc +++ b/multibody/plant/test/externally_applied_spatial_force_multiplexer_test.cc @@ -26,15 +26,14 @@ class ExternallyAppliedSpatialForceMultiplexerTest : public ::testing::Test { protected: void SetUp() override { system_ = - std::make_unique< - ExternallyAppliedSpatialForceListMultiplexer>(2); + std::make_unique>(2); context_ = system_->CreateDefaultContext(); input0_ = {MakeDummyForce(BodyIndex{0}), MakeDummyForce(BodyIndex{1})}; input1_ = {MakeDummyForce(BodyIndex{1}), MakeDummyForce(BodyIndex{2})}; } - std::unique_ptr> system_; + std::unique_ptr> system_; std::unique_ptr> context_; ListType input0_; ListType input1_; From a6885463c9959dbc91e55747c9ac101f04afc478 Mon Sep 17 00:00:00 2001 From: Eric Cousineau Date: Wed, 26 Oct 2022 08:36:23 -0400 Subject: [PATCH 7/9] playback subset of review --- multibody/plant/externally_applied_spatial_force_multiplexer.cc | 1 - multibody/plant/externally_applied_spatial_force_multiplexer.h | 2 +- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/multibody/plant/externally_applied_spatial_force_multiplexer.cc b/multibody/plant/externally_applied_spatial_force_multiplexer.cc index b25e20629f92..f99c15fee797 100644 --- a/multibody/plant/externally_applied_spatial_force_multiplexer.cc +++ b/multibody/plant/externally_applied_spatial_force_multiplexer.cc @@ -35,7 +35,6 @@ void ExternallyAppliedSpatialForceMultiplexer::CombineInputsToOutput( const systems::Context& context, ListType* output) const { output->clear(); - // auto iter = output->end(); for (int i = 0; i < this->num_input_ports(); ++i) { const ListType& values_i = this->get_input_port(i).template Eval(context); diff --git a/multibody/plant/externally_applied_spatial_force_multiplexer.h b/multibody/plant/externally_applied_spatial_force_multiplexer.h index ad14962190a4..ded230363790 100644 --- a/multibody/plant/externally_applied_spatial_force_multiplexer.h +++ b/multibody/plant/externally_applied_spatial_force_multiplexer.h @@ -37,8 +37,8 @@ class ExternallyAppliedSpatialForceMultiplexer final Constructor. @param num_inputs Number of input ports to be added. */ - explicit ExternallyAppliedSpatialForceMultiplexer(int num_inputs); + /** Scalar-converting copy constructor. See @ref system_scalar_conversion. */ From ec621cd478807e9fd99c53d747b2078ec4245445 Mon Sep 17 00:00:00 2001 From: Eric Cousineau Date: Wed, 26 Oct 2022 16:12:08 -0400 Subject: [PATCH 8/9] review --- .../externally_applied_spatial_force_multiplexer.cc | 2 +- .../externally_applied_spatial_force_multiplexer.h | 10 +++++----- ...xternally_applied_spatial_force_multiplexer_test.cc | 4 ++-- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/multibody/plant/externally_applied_spatial_force_multiplexer.cc b/multibody/plant/externally_applied_spatial_force_multiplexer.cc index f99c15fee797..8b5eb71cee00 100644 --- a/multibody/plant/externally_applied_spatial_force_multiplexer.cc +++ b/multibody/plant/externally_applied_spatial_force_multiplexer.cc @@ -19,7 +19,7 @@ ExternallyAppliedSpatialForceMultiplexer(int num_inputs) Value()); } this->DeclareAbstractOutputPort( - "combined", + systems::kUseDefaultName, &ExternallyAppliedSpatialForceMultiplexer::CombineInputsToOutput); } diff --git a/multibody/plant/externally_applied_spatial_force_multiplexer.h b/multibody/plant/externally_applied_spatial_force_multiplexer.h index ded230363790..165c310de93e 100644 --- a/multibody/plant/externally_applied_spatial_force_multiplexer.h +++ b/multibody/plant/externally_applied_spatial_force_multiplexer.h @@ -10,7 +10,7 @@ namespace drake { namespace multibody { /** -Combines multiple std::vector<>'s of ExternallyAppliedSpatialForce. +Concatenates multiple std::vector<>'s of ExternallyAppliedSpatialForce. @system name: ExternallyAppliedSpatialForceMultiplexer @@ -19,7 +19,7 @@ name: ExternallyAppliedSpatialForceMultiplexer - ... - u(N-1) output_ports: -- combined +- y0 @endsystem @tparam default_scalar @@ -30,9 +30,6 @@ class ExternallyAppliedSpatialForceMultiplexer final public: DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(ExternallyAppliedSpatialForceMultiplexer) - using ValueType = ExternallyAppliedSpatialForce; - using ListType = std::vector; - /** Constructor. @param num_inputs Number of input ports to be added. @@ -47,6 +44,9 @@ class ExternallyAppliedSpatialForceMultiplexer final const ExternallyAppliedSpatialForceMultiplexer& other); private: + using ValueType = ExternallyAppliedSpatialForce; + using ListType = std::vector; + // This is the calculator for the output port. void CombineInputsToOutput( const systems::Context& context, ListType* output) const; diff --git a/multibody/plant/test/externally_applied_spatial_force_multiplexer_test.cc b/multibody/plant/test/externally_applied_spatial_force_multiplexer_test.cc index 6d3e4a5138c6..bd3e1bcf3d97 100644 --- a/multibody/plant/test/externally_applied_spatial_force_multiplexer_test.cc +++ b/multibody/plant/test/externally_applied_spatial_force_multiplexer_test.cc @@ -72,7 +72,7 @@ void AssertEqual(const ListType& lhs, const ListType& rhs) { } // Tests that the system computes the correct sum. -TEST_F(ExternallyAppliedSpatialForceMultiplexerTest, AddTwoVectors) { +TEST_F(ExternallyAppliedSpatialForceMultiplexerTest, ConcatenateTwoVectors) { system_->get_input_port(0).FixValue(context_.get(), input0_); system_->get_input_port(1).FixValue(context_.get(), input1_); const ListType expected = { @@ -91,7 +91,7 @@ TEST_F(ExternallyAppliedSpatialForceMultiplexerTest, Stateless) { } // Asserts that adders have direct-feedthrough from all inputs to the output. -TEST_F(ExternallyAppliedSpatialForceMultiplexerTest, AdderIsDirectFeedthrough) { +TEST_F(ExternallyAppliedSpatialForceMultiplexerTest, IsDirectFeedthrough) { EXPECT_TRUE(system_->HasAnyDirectFeedthrough()); const int output_index = 0; for (int i = 0; i < system_->num_input_ports(); ++i) { From 0f129a1ca31af1df22bb8e07ff5e55b6bc6f042e Mon Sep 17 00:00:00 2001 From: Eric Cousineau Date: Wed, 26 Oct 2022 16:39:16 -0400 Subject: [PATCH 9/9] oops --- .../test/externally_applied_spatial_force_multiplexer_test.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/multibody/plant/test/externally_applied_spatial_force_multiplexer_test.cc b/multibody/plant/test/externally_applied_spatial_force_multiplexer_test.cc index bd3e1bcf3d97..28b3ddef428e 100644 --- a/multibody/plant/test/externally_applied_spatial_force_multiplexer_test.cc +++ b/multibody/plant/test/externally_applied_spatial_force_multiplexer_test.cc @@ -71,7 +71,7 @@ void AssertEqual(const ListType& lhs, const ListType& rhs) { } } -// Tests that the system computes the correct sum. +// Tests that the system computes the correct output. TEST_F(ExternallyAppliedSpatialForceMultiplexerTest, ConcatenateTwoVectors) { system_->get_input_port(0).FixValue(context_.get(), input0_); system_->get_input_port(1).FixValue(context_.get(), input1_); @@ -90,7 +90,7 @@ TEST_F(ExternallyAppliedSpatialForceMultiplexerTest, Stateless) { EXPECT_EQ(0, context_->num_discrete_state_groups()); } -// Asserts that adders have direct-feedthrough from all inputs to the output. +// Asserts that system has direct-feedthrough from all inputs to the output. TEST_F(ExternallyAppliedSpatialForceMultiplexerTest, IsDirectFeedthrough) { EXPECT_TRUE(system_->HasAnyDirectFeedthrough()); const int output_index = 0;