Adds zero-dimensional actuation ports to MultibodyPlant. (RobotLocomo…

…tion#10738)

examples/pendulum/print_symbolic_dynamics.cc

@@ -55,7 +55,8 @@ VectorX<Expression> MultibodyPlantDynamics() {
   // Obtain the symbolic dynamics.
   auto context = symbolic_plant.CreateDefaultContext();
   context->FixInputPort(
-      0, Vector1<Expression>::Constant(Variable("tau")));
+      symbolic_plant.get_actuation_input_port().get_index(),
+      Vector1<Expression>::Constant(Variable("tau")));
   symbolic_plant.SetPositionsAndVelocities(
       context.get(), Vector2<Expression>(
           Variable("theta"), Variable("thetadot")));

multibody/benchmarks/free_body/BUILD.bazel

@@ -8,8 +8,6 @@ load(
 )
 load("//tools/lint:lint.bzl", "add_lint_tests")
 
-# Because there are no meaninfully distinct components in this package, the
-# only public target we will offer is our drake_cc_package_library.
 package(default_visibility = ["//visibility:private"])
 
 filegroup(
@@ -21,6 +19,7 @@ filegroup(
         "**/*.urdf",
         "**/*.xml",
     ]),
+    visibility = ["//visibility:public"],
 )
 
 drake_cc_package_library(

multibody/plant/BUILD.bazel

@@ -193,6 +193,7 @@ drake_cc_googletest(
         "//manipulation/models/iiwa_description:models",
         "//manipulation/models/wsg_50_description:models",
         "//multibody/benchmarks/acrobot:models",
+        "//multibody/benchmarks/free_body:models",
         "//multibody/parsing:test_models",
     ],
     deps = [

multibody/plant/multibody_plant.cc

@@ -1243,13 +1243,19 @@ VectorX<T> MultibodyPlant<T>::AssembleActuationInput(
   int u_offset = 0;
   for (ModelInstanceIndex model_instance_index(0);
        model_instance_index < num_model_instances(); ++model_instance_index) {
+    // Ignore the port if the model instance has no actuated DoFs.
     const int instance_num_dofs =
         internal_tree().num_actuated_dofs(model_instance_index);
-    if (instance_num_dofs == 0) {
-      continue;
-    }
+    if (instance_num_dofs == 0) continue;
+
     const auto& input_port = this->get_input_port(
         instance_actuation_ports_[model_instance_index]);
+    if (!input_port.HasValue(context)) {
+        throw std::logic_error(fmt::format("Actuation input port for model "
+            "instance {} must be connected.",
+            GetModelInstanceName(model_instance_index)));
+    }
+
     const auto& u_instance = input_port.Eval(context);
     actuation_input.segment(u_offset, instance_num_dofs) = u_instance;
     u_offset += instance_num_dofs;
@@ -1605,11 +1611,10 @@ void MultibodyPlant<T>::DeclareStateCacheAndPorts() {
        model_instance_index < num_model_instances(); ++model_instance_index) {
     const int instance_num_dofs =
         internal_tree().num_actuated_dofs(model_instance_index);
-    if (instance_num_dofs == 0) {
-      continue;
+    if (instance_num_dofs > 0) {
+      ++num_actuated_instances;
+      last_actuated_instance = model_instance_index;
     }
-    ++num_actuated_instances;
-    last_actuated_instance = model_instance_index;
     instance_actuation_ports_[model_instance_index] =
         this->DeclareVectorInputPort(
                 internal_tree().GetModelInstanceName(model_instance_index) +
@@ -1618,9 +1623,7 @@ void MultibodyPlant<T>::DeclareStateCacheAndPorts() {
             .get_index();
   }
 
-  if (num_actuated_instances == 1) {
-    actuated_instance_ = last_actuated_instance;
-  }
+  if (num_actuated_instances == 1) actuated_instance_ = last_actuated_instance;
 
   // Declare the generalized force input port.
   applied_generalized_force_input_port_ = this->DeclareVectorInputPort(
@@ -1766,7 +1769,6 @@ MultibodyPlant<T>::get_actuation_input_port(
   DRAKE_MBP_THROW_IF_NOT_FINALIZED();
   DRAKE_THROW_UNLESS(model_instance.is_valid());
   DRAKE_THROW_UNLESS(model_instance < num_model_instances());
-  DRAKE_THROW_UNLESS(num_actuated_dofs(model_instance) > 0);
   return systems::System<T>::get_input_port(
       instance_actuation_ports_.at(model_instance));
 }

multibody/plant/multibody_plant.h

@@ -2578,8 +2578,7 @@ class MultibodyPlant : public MultibodyTreeSystem<T> {
   /// a specific model instance.  This input port is a vector valued port, which
   /// can be set with JointActuator::set_actuation_vector().
   /// @pre Finalize() was already called on `this` plant.
-  /// @throws std::exception if called before Finalize() or if the model
-  /// instance does not contain any actuators.
+  /// @throws std::exception if called before Finalize().
   /// @throws std::exception if the model instance does not exist.
   const systems::InputPort<T>& get_actuation_input_port(
       ModelInstanceIndex model_instance) const;
@@ -3381,8 +3380,7 @@ class MultibodyPlant : public MultibodyTreeSystem<T> {
 
   // Input/Output port indexes:
   // A vector containing actuation ports for each model instance indexed by
-  // ModelInstanceIndex.  An invalid value indicates that the model instance has
-  // no actuated dofs.
+  // ModelInstanceIndex.
   std::vector<systems::InputPortIndex> instance_actuation_ports_;
 
   // If only one model instance has actuated dofs, remember it here.  If

multibody/plant/test/joint_limits_test.cc

@@ -85,7 +85,9 @@ GTEST_TEST(JointLimitsTest, PrismaticJointConvergenceTest) {
 
     Simulator<double> simulator(plant);
     Context<double>& context = simulator.get_mutable_context();
-    context.FixInputPort(0, Vector1<double>::Constant(-10.0));
+    context.FixInputPort(
+      plant.get_actuation_input_port().get_index(),
+      Vector1<double>::Constant(-10.0));
     simulator.Initialize();
     simulator.StepTo(simulation_time);
 
@@ -101,7 +103,9 @@ GTEST_TEST(JointLimitsTest, PrismaticJointConvergenceTest) {
     EXPECT_NEAR(slider.get_translation_rate(context), 0.0, kVelocityTolerance);
 
     // Set the force to be positive and re-start the simulation.
-    context.FixInputPort(0, Vector1<double>::Constant(10.0));
+    context.FixInputPort(
+      plant.get_actuation_input_port().get_index(),
+      Vector1<double>::Constant(10.0));
     context.set_time(0.0);
     simulator.StepTo(simulation_time);
 
@@ -157,7 +161,9 @@ GTEST_TEST(JointLimitsTest, RevoluteJoint) {
 
     Simulator<double> simulator(plant);
     Context<double>& context = simulator.get_mutable_context();
-    context.FixInputPort(0, Vector1<double>::Constant(1.5));
+    context.FixInputPort(
+      plant.get_actuation_input_port().get_index(),
+      Vector1<double>::Constant(1.5));
     simulator.Initialize();
     simulator.StepTo(simulation_time);
 
@@ -173,7 +179,9 @@ GTEST_TEST(JointLimitsTest, RevoluteJoint) {
     EXPECT_NEAR(pin.get_angular_rate(context), 0.0, kVelocityTolerance);
 
     // Set the torque to be negative and re-start the simulation.
-    context.FixInputPort(0, Vector1<double>::Constant(-1.5));
+    context.FixInputPort(
+      plant.get_actuation_input_port().get_index(),
+      Vector1<double>::Constant(-1.5));
     context.set_time(0.0);
     simulator.StepTo(simulation_time);
 
@@ -254,7 +262,9 @@ GTEST_TEST(JointLimitsTest, KukaArm) {
   Context<double>& context = simulator.get_mutable_context();
 
   // Drive all the joints to their upper limit by applying a positive torque.
-  context.FixInputPort(0, VectorX<double>::Constant(nq, 0.4));
+  context.FixInputPort(
+    plant.get_actuation_input_port().get_index(),
+    VectorX<double>::Constant(nq, 0.4));
   simulator.Initialize();
   simulator.StepTo(simulation_time);
 
@@ -269,7 +279,9 @@ GTEST_TEST(JointLimitsTest, KukaArm) {
   }
 
   // Drive all the joints to their lower limit by applying a negative torque.
-  context.FixInputPort(0, VectorX<double>::Constant(nq, -0.4));
+  context.FixInputPort(
+    plant.get_actuation_input_port().get_index(),
+    VectorX<double>::Constant(nq, -0.4));
   plant.SetDefaultContext(&context);
   context.set_time(0.0);
   simulator.StepTo(simulation_time);

multibody/plant/test/multibody_plant_test.cc

@@ -306,6 +306,52 @@ GTEST_TEST(MultibodyPlantTest, AddMultibodyPlantSceneGraph) {
   // AddMultibodyPlantSceneGraphResult<double> extra{*plant, *scene_graph};
 }
 
+GTEST_TEST(ActuationPortsTest, CheckActuation) {
+  // Create a MultibodyPlant consisting of two model instances, one actuated
+  // and the other unactuated.
+  MultibodyPlant<double> plant;
+  const std::string acrobot_path = FindResourceOrThrow(
+      "drake/multibody/benchmarks/acrobot/acrobot.sdf");
+  const std::string cylinder_path = FindResourceOrThrow(
+      "drake/multibody/benchmarks/free_body/uniform_solid_cylinder.urdf");
+  auto acrobot_instance = Parser(&plant).AddModelFromFile(acrobot_path);
+  auto cylinder_instance = Parser(&plant).AddModelFromFile(cylinder_path);
+  plant.Finalize();
+
+  // Verify the number of actuators.
+  EXPECT_EQ(plant.num_actuated_dofs(acrobot_instance), 1);
+  EXPECT_EQ(plant.num_actuated_dofs(cylinder_instance), 0);
+
+  // Verify that we can get the actuation input ports.
+  EXPECT_NO_THROW(plant.get_actuation_input_port());
+  EXPECT_NO_THROW(plant.get_actuation_input_port(acrobot_instance));
+  EXPECT_NO_THROW(plant.get_actuation_input_port(cylinder_instance));
+
+  // Try to compute the derivatives without connecting the acrobot_instance
+  // port.
+  std::unique_ptr<Context<double>> context = plant.CreateDefaultContext();
+  std::unique_ptr<ContinuousState<double>> continuous_state = plant.
+      AllocateTimeDerivatives();
+  DRAKE_EXPECT_THROWS_MESSAGE(
+      plant.CalcTimeDerivatives(*context, continuous_state.get()),
+      std::logic_error, "Actuation input port for model instance .* must "
+          "be connected.");
+
+  // Verify that derivatives can be computed after fixing the acrobot actuation
+  // input port.
+  context->FixInputPort(
+      plant.get_actuation_input_port(acrobot_instance).get_index(),
+      Vector1d(0.0));
+  EXPECT_NO_THROW(plant.CalcTimeDerivatives(*context, continuous_state.get()));
+
+  // Verify that derivatives can be computed after fixing the cylinder actuation
+  // input port with an empty vector.
+  context->FixInputPort(
+      plant.get_actuation_input_port(cylinder_instance).get_index(),
+      VectorXd(0));
+  EXPECT_NO_THROW(plant.CalcTimeDerivatives(*context, continuous_state.get()));
+}
+
 // Fixture to perform a number of computational tests on an acrobot model.
 class AcrobotPlantTests : public ::testing::Test {
  public:

systems/controllers/linear_quadratic_regulator.cc

@@ -128,7 +128,7 @@ std::unique_ptr<systems::AffineSystem<double>> LinearQuadraticRegulator(
           ? context.get_continuous_state_vector().CopyToVector()
           : context.get_discrete_state(0).CopyToVector();
 
-  const auto& u0 = system.get_input_port(0).Eval(context);
+  const auto& u0 = system.get_input_port(input_port_index).Eval(context);
 
   // Return the affine controller: u = u0 - K(x-x0).
   return std::make_unique<systems::AffineSystem<double>>(

systems/framework/system.h

@@ -2150,7 +2150,8 @@ class System : public SystemBase {
   std::function<void(const AbstractValue&)> MakeFixInputPortTypeChecker(
       InputPortIndex port_index) const final {
     const InputPort<T>& port = this->get_input_port(port_index);
-    const std::string pathname = this->GetSystemPathname();
+    const std::string& port_name = port.get_name();
+    const std::string path_name = this->GetSystemPathname();
 
     // Note that our lambdas below will capture all necessary items by-value,
     // so that they do not rely on this System still being alive.  (We do not
@@ -2166,11 +2167,11 @@ class System : public SystemBase {
         // fine to let them also handle detailed error reporting on their own.
         const std::type_info& expected_type =
             this->AllocateInputAbstract(port)->static_type_info();
-        return [&expected_type, port_index, pathname](
+        return [&expected_type, port_index, path_name, port_name](
             const AbstractValue& actual) {
           if (actual.static_type_info() != expected_type) {
             SystemBase::ThrowInputPortHasWrongType(
-                "FixInputPortTypeCheck", pathname, port_index,
+                "FixInputPortTypeCheck", path_name, port_index, port_name,
                 NiceTypeName::Get(expected_type),
                 NiceTypeName::Get(actual.type_info()));
           }
@@ -2182,20 +2183,20 @@ class System : public SystemBase {
         const std::unique_ptr<BasicVector<T>> model_vector =
             this->AllocateInputVector(port);
         const int expected_size = model_vector->size();
-        return [expected_size, port_index, pathname](
+        return [expected_size, port_index, path_name, port_name](
             const AbstractValue& actual) {
           const BasicVector<T>* const actual_vector =
               actual.MaybeGetValue<BasicVector<T>>();
           if (actual_vector == nullptr) {
             SystemBase::ThrowInputPortHasWrongType(
-                "FixInputPortTypeCheck", pathname, port_index,
+                "FixInputPortTypeCheck", path_name, port_index, port_name,
                 NiceTypeName::Get<Value<BasicVector<T>>>(),
                 NiceTypeName::Get(actual));
           }
           // Check that vector sizes match.
           if (actual_vector->size() != expected_size) {
             SystemBase::ThrowInputPortHasWrongType(
-                "FixInputPortTypeCheck", pathname, port_index,
+                "FixInputPortTypeCheck", path_name, port_index, port_name,
                 fmt::format("{} with size={}",
                             NiceTypeName::Get<BasicVector<T>>(),
                             expected_size),

systems/framework/system_base.cc

@@ -197,49 +197,55 @@ void SystemBase::ThrowInputPortIndexOutOfRange(const char* func,
   throw std::out_of_range(fmt::format(
       "{}: there is no input port with index {} because there "
       "are only {} input ports in system {}.",
-      FmtFunc(func), port, get_num_input_ports(), GetSystemPathname()));
+      FmtFunc(func),  port, get_num_input_ports(), GetSystemPathname()));
 }
 
 void SystemBase::ThrowOutputPortIndexOutOfRange(const char* func,
                                                 OutputPortIndex port) const {
   throw std::out_of_range(fmt::format(
       "{}: there is no output port with index {} because there "
       "are only {} output ports in system {}.",
-      FmtFunc(func), port, get_num_output_ports(), GetSystemPathname()));
+      FmtFunc(func), port,
+      get_num_output_ports(), GetSystemPathname()));
 }
 
 void SystemBase::ThrowNotAVectorInputPort(const char* func,
                                           InputPortIndex port) const {
   throw std::logic_error(fmt::format(
-      "{}: vector port required, but input port[{}] was declared abstract. "
-          "Even if the actual value is a vector, use EvalInputValue<V> "
-          "instead for an abstract port containing a vector of type V. "
-          "(System {})",
-      FmtFunc(func), port, GetSystemPathname()));
+      "{}: vector port required, but input port '{}' (index {}) was declared "
+          "abstract. Even if the actual value is a vector, use "
+          "EvalInputValue<V> instead for an abstract port containing a vector "
+          "of type V. (System {})",
+      FmtFunc(func),  get_input_port_base(port).get_name(), port,
+      GetSystemPathname()));
 }
 
 void SystemBase::ThrowInputPortHasWrongType(
     const char* func, InputPortIndex port, const std::string& expected_type,
     const std::string& actual_type) const {
   ThrowInputPortHasWrongType(
-      func, GetSystemPathname(), port, expected_type, actual_type);
+      func, GetSystemPathname(), port, get_input_port_base(port).get_name(),
+      expected_type, actual_type);
 }
 
 void SystemBase::ThrowInputPortHasWrongType(
     const char* func, const std::string& system_pathname, InputPortIndex port,
-    const std::string& expected_type, const std::string& actual_type) {
+    const std::string& port_name, const std::string& expected_type,
+    const std::string& actual_type) {
   throw std::logic_error(fmt::format(
-      "{}: expected value of type {} for input port[{}] "
+      "{}: expected value of type {} for input port '{}' (index {}) "
           "but the actual type was {}. (System {})",
-      FmtFunc(func), expected_type, port, actual_type, system_pathname));
+      FmtFunc(func), expected_type, port_name, port, actual_type,
+      system_pathname));
 }
 
 void SystemBase::ThrowCantEvaluateInputPort(const char* func,
                                             InputPortIndex port) const {
   throw std::logic_error(
-      fmt::format("{}: input port[{}] is neither connected nor fixed so "
-                      "cannot be evaluated. (System {})",
-                  FmtFunc(func), port, GetSystemPathname()));
+      fmt::format("{}: input port '{}' (index {}) is neither connected nor "
+                      "fixed so cannot be evaluated. (System {})",
+                  FmtFunc(func), get_input_port_base(port).get_name(), port,
+                  GetSystemPathname()));
 }
 
 }  // namespace systems

systems/framework/system_base.h

@@ -962,7 +962,8 @@ class SystemBase : public internal::SystemMessageInterface {
   the input port had some value type that was wrong. */
   [[noreturn]] static void ThrowInputPortHasWrongType(
       const char* func, const std::string& system_pathname, InputPortIndex,
-      const std::string& expected_type, const std::string& actual_type);
+      const std::string& port_name, const std::string& expected_type,
+      const std::string& actual_type);
 
   /** Throws std::logic_error because someone called API method `func`, that
   requires this input port to be evaluatable, but the port was neither

systems/framework/test/leaf_system_test.cc

@@ -1052,15 +1052,15 @@ GTEST_TEST(ModelLeafSystemTest, ModelInputGovernsFixedInput) {
       std::exception,
       "System::FixInputPortTypeCheck\\(\\): expected value of type "
       "drake::systems::BasicVector<double> with size=1 "
-      "for input port\\[0\\] but the actual type was "
+      "for input port 'input' \\(index 0\\) but the actual type was "
       "drake::systems::BasicVector<double> with size=2. "
       "\\(System ::dut\\)");
   DRAKE_EXPECT_THROWS_MESSAGE(
       context->FixInputPort(0, Value<std::string>{}),
       std::exception,
       "System::FixInputPortTypeCheck\\(\\): expected value of type "
       "drake::Value<drake::systems::BasicVector<double>> "
-      "for input port\\[0\\] but the actual type was "
+      "for input port 'input' \\(index 0\\) but the actual type was "
       "drake::Value<std::string>. "
       "\\(System ::dut\\)");
 
@@ -1071,7 +1071,7 @@ GTEST_TEST(ModelLeafSystemTest, ModelInputGovernsFixedInput) {
       std::exception,
       "System::FixInputPortTypeCheck\\(\\): expected value of type "
       "int "
-      "for input port\\[2\\] but the actual type was "
+      "for input port 'abstract_input' \\(index 2\\) but the actual type was "
       "std::string. "
       "\\(System ::dut\\)");
 }