[multibody] Add MultibodyPlant::RemoveJoint() (#21397)

bindings/pydrake/examples/gym/named_view_helpers.py

@@ -23,7 +23,7 @@ def MakeNamedViewPositions(plant,
                            view_name,
                            add_suffix_if_single_position=False):
     names = [None] * plant.num_positions()
-    for ind in range(plant.num_joints()):
+    for ind in plant.GetJointIndices():
         joint = plant.get_joint(JointIndex(ind))
         if joint.num_positions() == 1 and not add_suffix_if_single_position:
             names[joint.position_start()] = joint.name()
@@ -44,7 +44,7 @@ def MakeNamedViewVelocities(plant,
                             view_name,
                             add_suffix_if_single_velocity=False):
     names = [None] * plant.num_velocities()
-    for ind in range(plant.num_joints()):
+    for ind in plant.GetJointIndices():
         joint = plant.get_joint(JointIndex(ind))
         if joint.num_velocities() == 1 and not add_suffix_if_single_velocity:
             names[joint.velocity_start()] = joint.name()

bindings/pydrake/multibody/jupyter_widgets.py

@@ -74,7 +74,7 @@ def _reshape(x, num):
         self._default_position = robot.GetPositions(context)
 
         k = 0
-        for i in range(0, robot.num_joints()):
+        for i in robot.GetJointIndices():
             joint = robot.get_joint(JointIndex(i))
             low = joint.position_lower_limits()
             upp = joint.position_upper_limits()
@@ -214,7 +214,7 @@ def _slider_callback(change, index):
 
     slider_widgets = []
     slider_num = 0
-    for i in range(plant.num_joints()):
+    for i in plant.GetJointIndices():
         joint = plant.get_joint(JointIndex(i))
         low = joint.position_lower_limits()
         upp = joint.position_upper_limits()

bindings/pydrake/multibody/plant_py.cc

@@ -220,6 +220,8 @@ void DoScalarDependentDefinitions(py::module m, T) {
             },
             py::arg("joint"), py_rvp::reference_internal,
             cls_doc.AddJoint.doc_1args)
+        .def("RemoveJoint", &Class::RemoveJoint, py::arg("joint"),
+            cls_doc.RemoveJoint.doc)
         .def("AddJointActuator", &Class::AddJointActuator,
             py_rvp::reference_internal, py::arg("name"), py::arg("joint"),
             py::arg("effort_limit") = std::numeric_limits<double>::infinity(),
@@ -726,6 +728,8 @@ void DoScalarDependentDefinitions(py::module m, T) {
         .def("num_frames", &Class::num_frames, cls_doc.num_frames.doc)
         .def("get_body", &Class::get_body, py::arg("body_index"),
             py_rvp::reference_internal, cls_doc.get_body.doc)
+        .def("has_joint", &Class::has_joint, py::arg("joint_index"),
+            cls_doc.has_joint.doc)
         .def("get_joint", &Class::get_joint, py::arg("joint_index"),
             py_rvp::reference_internal, cls_doc.get_joint.doc)
         .def("get_mutable_joint", &Class::get_mutable_joint,
@@ -750,8 +754,14 @@ void DoScalarDependentDefinitions(py::module m, T) {
             cls_doc.set_gravity_enabled.doc)
         .def("is_gravity_enabled", &Class::is_gravity_enabled,
             py::arg("model_instance"), cls_doc.is_gravity_enabled.doc)
-        .def("GetJointIndices", &Class::GetJointIndices,
-            py::arg("model_instance"), cls_doc.GetJointIndices.doc)
+        .def("GetJointIndices",
+            overload_cast_explicit<const std::vector<JointIndex>&>(
+                &Class::GetJointIndices),
+            cls_doc.GetJointIndices.doc_0args)
+        .def("GetJointIndices",
+            overload_cast_explicit<std::vector<JointIndex>, ModelInstanceIndex>(
+                &Class::GetJointIndices),
+            py::arg("model_instance"), cls_doc.GetJointIndices.doc_1args)
         .def("GetJointActuatorIndices",
             overload_cast_explicit<const std::vector<JointActuatorIndex>&>(
                 &Class::GetJointActuatorIndices),

bindings/pydrake/multibody/test/plant_test.py

@@ -288,7 +288,36 @@ def test_joint_actuator_remodeling(self, T):
             plant.has_joint_actuator(actuator_index=actuator_index))
         plant.Finalize()
         self.assertEqual(
-            plant.GetJointActuatorIndices(model_instance=instance), [])
+            plant.GetJointActuatorIndices(), [])
+
+    @numpy_compare.check_all_types
+    def test_joint_remodeling(self, T):
+        """
+        Tests joint  APIs related to remodeling: `RemoveJoint`,
+        `has_joint` and the 0 argument `GetJointIndices()`.
+        """
+        plant = MultibodyPlant_[T](0)
+        instance = plant.AddModelInstance("instance")
+        body = plant.AddRigidBody(
+            name="body", model_instance=instance)
+        joint = plant.AddJoint(
+            PrismaticJoint_[T](
+                "joint",
+                plant.world_frame(),
+                body.body_frame(),
+                [0, 0, 1]))
+        joint_index = joint.index()
+        self.assertEqual(
+            plant.GetJointIndices(), [joint_index])
+        self.assertTrue(
+            plant.has_joint(joint_index=joint_index))
+        plant.RemoveJoint(joint=joint)
+        self.assertFalse(
+            plant.has_joint(joint_index=joint_index))
+        plant.Finalize()
+        # 6 dof joint will be added for the now free body.
+        self.assertEqual(
+            plant.GetJointIndices(), [JointIndex(1)])
 
     def test_multibody_plant_config(self):
         MultibodyPlantConfig()

manipulation/util/robot_plan_interpolator.cc

@@ -47,7 +47,7 @@ RobotPlanInterpolator::RobotPlanInterpolator(const std::string& model_path,
   // Search for any bodies with no parent.  We'll weld those to the world.
   std::set<BodyIndex> parent_bodies;
   std::set<BodyIndex> child_bodies;
-  for (JointIndex i(0); i < plant_.num_joints(); ++i) {
+  for (JointIndex i : plant_.GetJointIndices()) {
     const multibody::Joint<double>& joint = plant_.get_joint(i);
     if (joint.parent_body().index() == plant_.world_body().index()) {
       // Nothing to weld, we're connected to the world.

manipulation/util/robot_plan_utils.cc

@@ -16,9 +16,8 @@ std::vector<std::string> GetJointNames(
     const multibody::MultibodyPlant<T>& plant) {
   std::map<int, std::string> position_names;
   const int num_positions = plant.num_positions();
-  for (int i = 0; i < plant.num_joints(); ++i) {
-    const multibody::Joint<T>& joint =
-        plant.get_joint(multibody::JointIndex(i));
+  for (multibody::JointIndex i : plant.GetJointIndices()) {
+    const multibody::Joint<T>& joint = plant.get_joint(i);
     if (joint.num_positions() == 0) {
       continue;
     }

manipulation/util/test/robot_plan_interpolator_test.cc

@@ -87,9 +87,8 @@ void DoTrajectoryTest(InterpolatorType interp_type) {
   plan.plan.resize(num_time_steps, default_robot_state);
 
   std::vector<std::string> joint_names;
-  for (int i = 0; i < dut.plant().num_joints(); ++i) {
-    const multibody::Joint<double>& joint =
-        dut.plant().get_joint(multibody::JointIndex(i));
+  for (multibody::JointIndex i : dut.plant().GetJointIndices()) {
+    const multibody::Joint<double>& joint = dut.plant().get_joint(i);
     if (joint.num_positions() != 1) {
       continue;
     }

multibody/inverse_kinematics/global_inverse_kinematics.cc

@@ -30,8 +30,7 @@ std::map<BodyIndex, JointIndex> GetBodyToJointMap(
   // First loop through each joint, stores the map from the child body to the
   // joint.
   std::map<BodyIndex, JointIndex> body_to_joint_map;
-  for (JointIndex joint_index{0}; joint_index < plant.num_joints();
-       ++joint_index) {
+  for (JointIndex joint_index : plant.GetJointIndices()) {
     body_to_joint_map.emplace(plant.get_joint(joint_index).child_body().index(),
                               joint_index);
   }
@@ -626,8 +625,7 @@ void GlobalInverseKinematics::AddJointLimitConstraint(
         "the bounds on R_WB and p_WB directly.");
   }
   const Joint<double>* joint{nullptr};
-  for (JointIndex joint_index{0}; joint_index < plant_.num_joints();
-       ++joint_index) {
+  for (JointIndex joint_index : plant_.GetJointIndices()) {
     if (plant_.get_joint(joint_index).child_body().index() == body_index) {
       joint = &(plant_.get_joint(joint_index));
       break;

multibody/inverse_kinematics/inverse_kinematics.cc

@@ -70,7 +70,7 @@ InverseKinematics::InverseKinematics(
   // Obey joint locking. Joint locking trumps joint limits.
   const Eigen::VectorXd current_positions = plant.GetPositions(context());
   VectorX<bool> is_locked = VectorX<bool>::Constant(nq, false);
-  for (JointIndex i{0}; i < plant.num_joints(); ++i) {
+  for (JointIndex i : plant.GetJointIndices()) {
     const Joint<double>& joint = plant.get_joint(i);
     if (joint.is_locked(context())) {
       const int start = joint.position_start();

multibody/meshcat/joint_sliders.cc

@@ -69,7 +69,7 @@ std::map<int, std::string> GetPositionNames(
 
   // Map all joints into the positions-to-name result.
   std::map<int, std::string> result;
-  for (JointIndex i{0}; i < plant->num_joints(); ++i) {
+  for (JointIndex i : plant->GetJointIndices()) {
     const Joint<T>& joint = plant->get_joint(i);
     for (int j = 0; j < joint.num_positions(); ++j) {
       const int position_index = joint.position_start() + j;

multibody/plant/multibody_plant.cc

@@ -24,10 +24,12 @@
 #include "drake/multibody/plant/hydroelastic_traction_calculator.h"
 #include "drake/multibody/plant/make_discrete_update_manager.h"
 #include "drake/multibody/plant/slicing_and_indexing.h"
+#include "drake/multibody/tree/door_hinge.h"
 #include "drake/multibody/tree/prismatic_joint.h"
+#include "drake/multibody/tree/prismatic_spring.h"
 #include "drake/multibody/tree/quaternion_floating_joint.h"
 #include "drake/multibody/tree/revolute_joint.h"
-
+#include "drake/multibody/tree/revolute_spring.h"
 namespace drake {
 namespace multibody {
 
@@ -680,7 +682,7 @@ std::string MultibodyPlant<T>::GetTopologyGraphvizString() const {
     graphviz += "}\n";
   }
   // Add the graph edges (via the joints).
-  for (JointIndex joint_index(0); joint_index < num_joints(); ++joint_index) {
+  for (JointIndex joint_index : GetJointIndices()) {
     const Joint<T>& joint = get_joint(joint_index);
     graphviz += fmt::format(
         "body{} -> body{} [label=\"{} [{}]\"];\n", joint.child_body().index(),
@@ -777,6 +779,51 @@ void MultibodyPlant<T>::SetFreeBodyRandomRotationDistributionToUniform(
   SetFreeBodyRandomRotationDistribution(body, q_FM);
 }
 
+template <typename T>
+void MultibodyPlant<T>::RemoveJoint(const Joint<T>& joint) {
+  // Check for non-zero number of user-added force elements. (There is always 1
+  // force element, the gravity field added when the tree is constructed.)
+  if (num_force_elements() > 1) {
+    throw std::logic_error(fmt::format(
+        "{}: This plant has {} user-added force elements. This plant must have "
+        "0 user-added force elements in order to remove joint with index {}.",
+        __func__, num_force_elements() - 1, joint.index()));
+  }
+
+  // Check for non-zero number of user-added constraints.
+  if (num_constraints() > 0) {
+    throw std::logic_error(fmt::format(
+        "{}: This plant has {} user-added constraints. This plant must have "
+        "0 user-added constraints in order to remove joint with index {}.",
+        __func__, num_constraints(), joint.index()));
+  }
+
+  // Check for dependent JointActuators in the plant. Throw if any actuator
+  // depends on the joint to be removed.
+  // TODO(#21415): Find a way to make checking an element for dependency on a
+  // Joint more robust and uniform across all MultibodyElements and constraints.
+  std::vector<std::string> dependent_elements;
+
+  // Check JointActuators.
+  for (JointActuatorIndex index : GetJointActuatorIndices()) {
+    const JointActuator<T>& actuator = get_joint_actuator(index);
+    if (actuator.joint().index() == joint.index()) {
+      dependent_elements.push_back(
+          fmt::format("JointActuator(name: {}, index: {})", actuator.name(),
+                      actuator.index()));
+    }
+  }
+
+  if (dependent_elements.size() > 0) {
+    throw std::logic_error(fmt::format(
+        "{}: joint with index {} has the following dependent model elements "
+        "which must be removed prior to joint removal: [{}].",
+        __func__, joint.index(), fmt::join(dependent_elements, ", ")));
+  }
+
+  this->mutable_tree().RemoveJoint(joint);
+}
+
 template <typename T>
 const WeldJoint<T>& MultibodyPlant<T>::WeldFrames(
     const Frame<T>& frame_on_parent_F, const Frame<T>& frame_on_child_M,
@@ -971,9 +1018,11 @@ std::vector<BodyIndex> MultibodyPlant<T>::GetBodiesKinematicallyAffectedBy(
     const std::vector<JointIndex>& joint_indexes) const {
   DRAKE_MBP_THROW_IF_NOT_FINALIZED();
   for (const JointIndex& joint : joint_indexes) {
-    if (!joint.is_valid() || joint >= num_joints()) {
-      throw std::logic_error(fmt::format(
-          "{}: No joint with index {} has been registered.", __func__, joint));
+    if (!has_joint(joint)) {
+      throw std::logic_error(
+          fmt::format("{}: No joint with index {} has been registered or it "
+                      "has been removed.",
+                      __func__, joint));
     }
     if (get_joint(joint).num_velocities() == 0) {
       throw std::logic_error(
@@ -1184,7 +1233,7 @@ void MultibodyPlant<T>::Finalize() {
 
 template <typename T>
 void MultibodyPlant<T>::SetUpJointLimitsParameters() {
-  for (JointIndex joint_index(0); joint_index < num_joints(); ++joint_index) {
+  for (JointIndex joint_index : GetJointIndices()) {
     // Currently MultibodyPlant applies these "compliant" joint limit forces
     // using an explicit Euler strategy. Stability analysis of the explicit
     // Euler applied to the harmonic oscillator (the model used for these
@@ -1401,7 +1450,7 @@ void MultibodyPlant<T>::ApplyDefaultCollisionFilters() {
     // Disallow collisions between adjacent bodies. Adjacency is implied by the
     // existence of a joint between bodies, except in the case of 6-dof joints
     // or joints in which the parent body is `world`.
-    for (JointIndex j{0}; j < num_joints(); ++j) {
+    for (JointIndex j : GetJointIndices()) {
       const Joint<T>& joint = get_joint(j);
       const RigidBody<T>& child = joint.child_body();
       const RigidBody<T>& parent = joint.parent_body();
@@ -1533,7 +1582,7 @@ VectorX<T> MultibodyPlant<T>::GetDefaultPositions() const {
   DRAKE_MBP_THROW_IF_NOT_FINALIZED();
   VectorX<T> q;
   q.setConstant(num_positions(), std::numeric_limits<double>::quiet_NaN());
-  for (JointIndex i{0}; i < num_joints(); ++i) {
+  for (JointIndex i : GetJointIndices()) {
     const Joint<T>& joint = get_joint(i);
     q.segment(joint.position_start(), joint.num_positions()) =
         joint.default_positions();
@@ -1556,7 +1605,7 @@ void MultibodyPlant<T>::SetDefaultPositions(
     const Eigen::Ref<const Eigen::VectorXd>& q) {
   DRAKE_MBP_THROW_IF_NOT_FINALIZED();
   DRAKE_THROW_UNLESS(q.size() == num_positions());
-  for (JointIndex i{0}; i < num_joints(); ++i) {
+  for (JointIndex i : GetJointIndices()) {
     Joint<T>& joint = get_mutable_joint(i);
     joint.set_default_positions(
         q.segment(joint.position_start(), joint.num_positions()));
@@ -1586,8 +1635,8 @@ std::vector<std::string> MultibodyPlant<T>::GetPositionNames(
   DRAKE_MBP_THROW_IF_NOT_FINALIZED();
   std::vector<std::string> names(num_positions());
 
-  for (int joint_index = 0; joint_index < num_joints(); ++joint_index) {
-    const Joint<T>& joint = get_joint(JointIndex(joint_index));
+  for (JointIndex joint_index : GetJointIndices()) {
+    const Joint<T>& joint = get_joint(joint_index);
     const std::string prefix =
         add_model_instance_prefix
             ? fmt::format("{}_", GetModelInstanceName(joint.model_instance()))
@@ -1649,8 +1698,8 @@ std::vector<std::string> MultibodyPlant<T>::GetVelocityNames(
   DRAKE_MBP_THROW_IF_NOT_FINALIZED();
   std::vector<std::string> names(num_velocities());
 
-  for (int joint_index = 0; joint_index < num_joints(); ++joint_index) {
-    const Joint<T>& joint = get_joint(JointIndex(joint_index));
+  for (JointIndex joint_index : GetJointIndices()) {
+    const Joint<T>& joint = get_joint(joint_index);
     const std::string prefix =
         add_model_instance_prefix
             ? fmt::format("{}_", GetModelInstanceName(joint.model_instance()))
@@ -2593,7 +2642,7 @@ void MultibodyPlant<T>::CalcJointLockingCache(
 
   int unlocked_cursor = 0;
   int locked_cursor = 0;
-  for (JointIndex joint_index(0); joint_index < num_joints(); ++joint_index) {
+  for (JointIndex joint_index : GetJointIndices()) {
     const Joint<T>& joint = get_joint(joint_index);
     if (joint.is_locked(context)) {
       for (int k = 0; k < joint.num_velocities(); ++k) {
@@ -3528,7 +3577,7 @@ void MultibodyPlant<T>::CalcReactionForces(
 
   // Map mobilizer reaction forces to joint reaction forces and perform the
   // necessary frame conversions.
-  for (JointIndex joint_index(0); joint_index < num_joints(); ++joint_index) {
+  for (JointIndex joint_index : GetJointIndices()) {
     const Joint<T>& joint = get_joint(joint_index);
     const internal::MobilizerIndex mobilizer_index =
         internal_tree().get_joint_mobilizer(joint_index);
@@ -3584,7 +3633,7 @@ void MultibodyPlant<T>::CalcReactionForces(
     // Re-express in the joint's child frame Jc.
     const RotationMatrix<T> R_WJc = frame_Jc.CalcRotationMatrixInWorld(context);
     const RotationMatrix<T> R_JcW = R_WJc.inverse();
-    F_CJc_Jc_array->at(joint_index) = R_JcW * F_CJc_W;
+    F_CJc_Jc_array->at(joint.ordinal()) = R_JcW * F_CJc_W;
   }
 }