[multibody] Implements MultibodyPlant::RemoveJointActuator

In order to keep existing actuator indices valid, MultibodyTree
replaces the actuator in the list with nullptr.  As a result, we must
also introduce new supporting api: for checking if an actuator index
is valid, and for iterating over all actuator indices:
- has_joint_actuator(actuator_index)
- GetJointActuatorIndices()

It is no longer sound to iterate with a for loop of JointActuatorIndex
from 0 to num_actuators() -- both because some elements will be
omitted AND because num_actuators() is not necessarily the highest
actuator index.

Co-authored-by: Joe Masterjohn <joemasterjohn@gmail.com>

bindings/pydrake/examples/gym/named_view_helpers.py

@@ -4,15 +4,14 @@
 """
 from pydrake.common.containers import namedview
 from pydrake.multibody.tree import (
-    JointActuatorIndex,
     JointIndex,
     )
 
 
 def MakeNamedViewActuation(plant, view_name):
     names = [None] * plant.get_actuation_input_port().size()
-    for ind in range(plant.num_actuators()):
-        actuator = plant.get_joint_actuator(JointActuatorIndex(ind))
+    for ind in plant.GetJointActuatorIndices():
+        actuator = plant.get_joint_actuator(ind)
         assert actuator.num_inputs() == 1
         names[actuator.input_start()] = actuator.name()
     return namedview(view_name, names)

bindings/pydrake/multibody/plant_py.cc

@@ -225,6 +225,8 @@ void DoScalarDependentDefinitions(py::module m, T) {
             py_rvp::reference_internal, py::arg("name"), py::arg("joint"),
             py::arg("effort_limit") = std::numeric_limits<double>::infinity(),
             cls_doc.AddJointActuator.doc)
+        .def("RemoveJointActuator", &Class::RemoveJointActuator,
+            py::arg("actuator"), cls_doc.RemoveJointActuator.doc)
         .def(
             "AddFrame",
             [](Class * self, std::unique_ptr<Frame<T>> frame) -> auto& {
@@ -720,6 +722,8 @@ void DoScalarDependentDefinitions(py::module m, T) {
         .def("get_mutable_joint", &Class::get_mutable_joint,
             py::arg("joint_index"), py_rvp::reference_internal,
             cls_doc.get_mutable_joint.doc)
+        .def("has_joint_actuator", &Class::has_joint_actuator,
+            py::arg("actuator_index"), cls_doc.has_joint_actuator.doc)
         .def("get_joint_actuator", &Class::get_joint_actuator,
             py::arg("actuator_index"), py_rvp::reference_internal,
             cls_doc.get_joint_actuator.doc)
@@ -739,8 +743,15 @@ void DoScalarDependentDefinitions(py::module m, T) {
             py::arg("model_instance"), cls_doc.is_gravity_enabled.doc)
         .def("GetJointIndices", &Class::GetJointIndices,
             py::arg("model_instance"), cls_doc.GetJointIndices.doc)
-        .def("GetJointActuatorIndices", &Class::GetJointActuatorIndices,
-            py::arg("model_instance"), cls_doc.GetJointActuatorIndices.doc)
+        .def("GetJointActuatorIndices",
+            overload_cast_explicit<const std::vector<JointActuatorIndex>&>(
+                &Class::GetJointActuatorIndices),
+            cls_doc.GetJointActuatorIndices.doc_0args)
+        .def("GetJointActuatorIndices",
+            overload_cast_explicit<std::vector<JointActuatorIndex>,
+                ModelInstanceIndex>(&Class::GetJointActuatorIndices),
+            py::arg("model_instance"),
+            cls_doc.GetJointActuatorIndices.doc_1args)
         .def("GetActuatedJointIndices", &Class::GetActuatedJointIndices,
             py::arg("model_instance"), cls_doc.GetActuatedJointIndices.doc)
         .def("GetModelInstanceName",

bindings/pydrake/multibody/test/plant_test.py

@@ -266,6 +266,36 @@ def test_multibody_plant_construction_api(self, T):
         with self.assertRaises(RuntimeError):
             plant.EvalBodyPoseInWorld(context, body)
 
+    @numpy_compare.check_all_types
+    def test_joint_actuator_remodeling(self, T):
+        """
+        Tests joint actuator APIs related to remodeling: `RemoveJointActuator`,
+        `has_joint_actuator` and the 0 argument `GetJointActuatorIndices()`.
+        """
+        plant = MultibodyPlant_[T](0)
+        instance = plant.AddModelInstance("instance")
+        body = plant.AddRigidBody(
+            name="body", model_instance=instance,
+            M_BBo_B=SpatialInertia_[float]())
+        joint = plant.AddJoint(
+            PrismaticJoint_[T](
+                "joint",
+                plant.world_frame(),
+                body.body_frame(),
+                [0, 0, 1]))
+        actuator = plant.AddJointActuator("actuator", joint, 1)
+        actuator_index = actuator.index()
+        self.assertEqual(
+            plant.GetJointActuatorIndices(), [actuator.index()])
+        self.assertTrue(
+            plant.has_joint_actuator(actuator_index=actuator_index))
+        plant.RemoveJointActuator(actuator=actuator)
+        self.assertFalse(
+            plant.has_joint_actuator(actuator_index=actuator_index))
+        plant.Finalize()
+        self.assertEqual(
+            plant.GetJointActuatorIndices(model_instance=instance), [])
+
     def test_multibody_plant_config(self):
         MultibodyPlantConfig()
         config = MultibodyPlantConfig(time_step=0.01)

examples/allegro_hand/joint_control/allegro_single_object_simulation.cc

@@ -165,8 +165,7 @@ void DoMain() {
   DRAKE_DEMAND(plant.num_actuators() == 16);
   // N.B. This change MUST be performed before Finalize() in order to take
   // effect.
-  for (JointActuatorIndex actuator_index(0);
-       actuator_index < plant.num_actuators(); ++actuator_index) {
+  for (JointActuatorIndex actuator_index : plant.GetJointActuatorIndices()) {
     JointActuator<double>& actuator =
         plant.get_mutable_joint_actuator(actuator_index);
     actuator.set_default_rotor_inertia(rotor_inertia);

manipulation/models/franka_description/urdf/test/franka_arm_test.cc

@@ -101,12 +101,12 @@ GTEST_TEST(FrankaArmTest, TestReflectedInertia) {
   parser.AddModels(kPath);
   plant.Finalize();
 
-  for (int i = 0; i < canonical_plant.num_actuators(); ++i) {
+  for (multibody::JointActuatorIndex index :
+       canonical_plant.GetJointActuatorIndices()) {
     const multibody::JointActuator<double>& canonical_joint_actuator =
-        canonical_plant.get_joint_actuator(
-            drake::multibody::JointActuatorIndex(i));
+        canonical_plant.get_joint_actuator(index);
     const multibody::JointActuator<double>& joint_actuator =
-        plant.get_joint_actuator(drake::multibody::JointActuatorIndex(i));
+        plant.get_joint_actuator(index);
 
     CompareActuatorLimits(canonical_joint_actuator, joint_actuator);
   }

manipulation/models/iiwa_description/test/iiwa_variants_parsing_test.cc

@@ -127,12 +127,12 @@ GTEST_TEST(JointLimitsIiwa7, TestEffortVelocityPositionValues) {
     parser.AddModels(FindResourceOrThrow(model_file));
     plant.Finalize();
 
-    for (int i = 0; i < canonical_plant.num_actuators(); ++i) {
+    for (multibody::JointActuatorIndex index :
+         canonical_plant.GetJointActuatorIndices()) {
       const multibody::JointActuator<double>& canonical_joint_actuator =
-          canonical_plant.get_joint_actuator(
-              drake::multibody::JointActuatorIndex(i));
+          canonical_plant.get_joint_actuator(index);
       const multibody::JointActuator<double>& joint_actuator =
-          plant.get_joint_actuator(drake::multibody::JointActuatorIndex(i));
+          plant.get_joint_actuator(index);
 
       CompareActuatorLimits(canonical_joint_actuator, joint_actuator);
     }
@@ -201,12 +201,12 @@ GTEST_TEST(JointLimitsIiwa14, TestEffortVelocityPositionValues) {
     parser.AddModels(FindResourceOrThrow(model_file));
     plant.Finalize();
 
-    for (int i = 0; i < canonical_plant.num_actuators(); ++i) {
+    for (multibody::JointActuatorIndex index :
+         canonical_plant.GetJointActuatorIndices()) {
       const multibody::JointActuator<double>& canonical_joint_actuator =
-          canonical_plant.get_joint_actuator(
-              drake::multibody::JointActuatorIndex(i));
+          canonical_plant.get_joint_actuator(index);
       const multibody::JointActuator<double>& joint_actuator =
-          plant.get_joint_actuator(drake::multibody::JointActuatorIndex(i));
+          plant.get_joint_actuator(index);
 
       CompareActuatorLimits(canonical_joint_actuator, joint_actuator);
 
@@ -215,8 +215,7 @@ GTEST_TEST(JointLimitsIiwa14, TestEffortVelocityPositionValues) {
       std::filesystem::path model_path(model_file);
       if (model_path.filename() == "dual_iiwa14_polytope_collision.urdf") {
         const multibody::JointActuator<double>& second_instance_joint_actuator =
-            plant.get_joint_actuator(
-                drake::multibody::JointActuatorIndex(i + 7));
+            plant.get_joint_actuator(multibody::JointActuatorIndex(index + 7));
         CompareActuatorLimits(canonical_joint_actuator,
                               second_instance_joint_actuator);
       }

multibody/plant/BUILD.bazel

@@ -519,6 +519,7 @@ drake_cc_googletest(
         ":spheres_stack",
         "//common/test_utilities:eigen_matrix_compare",
         "//common/test_utilities:expect_throws_message",
+        "//multibody/plant/test_utilities:multibody_plant_remodeling",
     ],
 )
 
@@ -698,6 +699,7 @@ drake_cc_googletest(
         "//multibody/benchmarks/acrobot",
         "//multibody/benchmarks/pendulum",
         "//multibody/parsing",
+        "//multibody/plant/test_utilities:multibody_plant_remodeling",
         "//multibody/test_utilities:add_fixed_objects_to_plant",
         "//systems/primitives:constant_vector_source",
         "//systems/primitives:linear_system",
@@ -1230,6 +1232,7 @@ drake_cc_googletest(
         "//common/test_utilities:eigen_matrix_compare",
         "//common/test_utilities:expect_throws_message",
         "//multibody/parsing",
+        "//multibody/plant/test_utilities:multibody_plant_remodeling",
         "//systems/analysis:simulator",
         "//systems/framework:abstract_value_cloner",
         "//systems/primitives:pass_through",

multibody/plant/discrete_update_manager.cc

@@ -440,8 +440,8 @@ void DiscreteUpdateManager<T>::CalcJointActuationForces(
   actuation_wo_pd->setZero();
   if (plant().num_actuators() > 0) {
     const VectorX<T> u = AssembleActuationInput(context);
-    for (JointActuatorIndex actuator_index(0);
-         actuator_index < plant().num_actuators(); ++actuator_index) {
+    for (JointActuatorIndex actuator_index :
+         plant().GetJointActuatorIndices()) {
       const JointActuator<T>& actuator =
           plant().get_joint_actuator(actuator_index);
       const Joint<T>& joint = actuator.joint();
@@ -450,7 +450,7 @@ void DiscreteUpdateManager<T>::CalcJointActuationForces(
       const int v_index = joint.velocity_start();
       VectorX<T>& actuation =
           actuator.has_controller() ? *actuation_w_pd : *actuation_wo_pd;
-      actuation[v_index] = u[actuator_index];
+      actuation[v_index] = u[actuator.input_start()];
     }
   }
 }

multibody/plant/discrete_update_manager.h

@@ -195,7 +195,8 @@ class DiscreteUpdateManager : public ScalarConvertibleComponent<T> {
 
   /* Evaluate the actuation applied through actuators during the discrete
    update. This will include actuation input as well as controller models.
-   The returned vector is indexed by JointActuatorIndex. */
+   Actuators index into the returned vector with JointActuator::input_start().
+   */
   const VectorX<T>& EvalActuation(const systems::Context<T>& context) const;
 
   /* Evaluates discrete contact pairs from all types of contact (point contact,

multibody/plant/multibody_plant.cc

@@ -263,7 +263,7 @@ std::string GetScopedName(const MultibodyPlant<T>& plant,
 // controlled.
 template <typename T>
 bool AnyActuatorHasPdControl(const MultibodyPlant<T>& plant) {
-  for (JointActuatorIndex a(0); a < plant.num_actuators(); ++a) {
+  for (JointActuatorIndex a : plant.GetJointActuatorIndices()) {
     if (plant.get_joint_actuator(a).has_controller()) return true;
   }
   return false;
@@ -275,7 +275,7 @@ template <typename T>
 int NumOfPdControlledActuators(const MultibodyPlant<T>& plant,
                                ModelInstanceIndex model_instance) {
   int num_actuators = 0;
-  for (JointActuatorIndex a(0); a < plant.num_actuators(); ++a) {
+  for (JointActuatorIndex a : plant.GetJointActuatorIndices()) {
     const JointActuator<T>& actuator = plant.get_joint_actuator(a);
     if (actuator.model_instance() == model_instance &&
         actuator.has_controller()) {
@@ -802,6 +802,11 @@ const JointActuator<T>& MultibodyPlant<T>::AddJointActuator(
   return this->mutable_tree().AddJointActuator(name, joint, effort_limit);
 }
 
+template <typename T>
+void MultibodyPlant<T>::RemoveJointActuator(const JointActuator<T>& actuator) {
+  this->mutable_tree().RemoveJointActuator(actuator);
+}
+
 template <typename T>
 geometry::SourceId MultibodyPlant<T>::RegisterAsSourceForSceneGraph(
     SceneGraph<T>* scene_graph) {
@@ -1279,13 +1284,12 @@ void MultibodyPlant<T>::FinalizePlantOnly() {
 template <typename T>
 MatrixX<T> MultibodyPlant<T>::MakeActuationMatrix() const {
   MatrixX<T> B = MatrixX<T>::Zero(num_velocities(), num_actuated_dofs());
-  for (JointActuatorIndex actuator_index(0); actuator_index < num_actuators();
-       ++actuator_index) {
+  for (JointActuatorIndex actuator_index : GetJointActuatorIndices()) {
     const JointActuator<T>& actuator = get_joint_actuator(actuator_index);
     // This method assumes actuators on single dof joints. Assert this
     // condition.
     DRAKE_DEMAND(actuator.joint().num_velocities() == 1);
-    B(actuator.joint().velocity_start(), int{actuator.index()}) = 1;
+    B(actuator.joint().velocity_start(), actuator.input_start()) = 1;
   }
   return B;
 }
@@ -1296,14 +1300,13 @@ MultibodyPlant<T>::MakeActuationMatrixPseudoinverse() const {
   // We leverage here the assumption that B (the actuation matrix) is a
   // permutation matrix, so Bᵀ is the pseudoinverse.
   std::vector<Eigen::Triplet<double>> triplets;
-  for (JointActuatorIndex actuator_index(0); actuator_index < num_actuators();
-       ++actuator_index) {
+  for (JointActuatorIndex actuator_index : GetJointActuatorIndices()) {
     const JointActuator<T>& actuator = get_joint_actuator(actuator_index);
     // This method assumes actuators on single dof joints. Assert this
     // condition.
     DRAKE_DEMAND(actuator.joint().num_velocities() == 1);
     triplets.push_back(Eigen::Triplet<double>(
-        int{actuator.index()}, actuator.joint().velocity_start(), 1.0));
+        int{actuator.input_start()}, actuator.joint().velocity_start(), 1.0));
   }
 
   Eigen::SparseMatrix<double> Bplus(num_actuated_dofs(), num_velocities());
@@ -1733,10 +1736,8 @@ std::vector<std::string> MultibodyPlant<T>::GetActuatorNames(
   DRAKE_MBP_THROW_IF_NOT_FINALIZED();
   std::vector<std::string> names(num_actuators());
 
-  for (int actuator_index = 0; actuator_index < num_actuators();
-       ++actuator_index) {
-    const JointActuator<T>& actuator =
-        get_joint_actuator(JointActuatorIndex(actuator_index));
+  for (JointActuatorIndex actuator_index : GetJointActuatorIndices()) {
+    const JointActuator<T>& actuator = get_joint_actuator(actuator_index);
     const std::string prefix =
         add_model_instance_prefix
             ? fmt::format("{}_",
@@ -2341,13 +2342,12 @@ void MultibodyPlant<T>::AddJointActuationForces(
   DRAKE_DEMAND(forces->size() == num_velocities());
   if (num_actuators() > 0) {
     const VectorX<T> u = AssembleActuationInput(context);
-    for (JointActuatorIndex actuator_index(0); actuator_index < num_actuators();
-         ++actuator_index) {
+    for (JointActuatorIndex actuator_index : GetJointActuatorIndices()) {
       const JointActuator<T>& actuator = get_joint_actuator(actuator_index);
       const Joint<T>& joint = actuator.joint();
       // We only support actuators on single dof joints for now.
       DRAKE_DEMAND(joint.num_velocities() == 1);
-      (*forces)[joint.velocity_start()] += u[actuator_index];
+      (*forces)[joint.velocity_start()] += u[actuator.input_start()];
     }
   }
 }