diff --git a/bindings/pydrake/examples/gym/named_view_helpers.py b/bindings/pydrake/examples/gym/named_view_helpers.py index 30927801062a..ca08ae388251 100644 --- a/bindings/pydrake/examples/gym/named_view_helpers.py +++ b/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) diff --git a/bindings/pydrake/multibody/plant_py.cc b/bindings/pydrake/multibody/plant_py.cc index 5c50d405c9cc..db4814fba425 100644 --- a/bindings/pydrake/multibody/plant_py.cc +++ b/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::infinity(), cls_doc.AddJointActuator.doc) + .def("RemoveJointActuator", &Class::RemoveJointActuator, + py::arg("actuator"), cls_doc.RemoveJointActuator.doc) .def( "AddFrame", [](Class * self, std::unique_ptr> 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&>( + &Class::GetJointActuatorIndices), + cls_doc.GetJointActuatorIndices.doc_0args) + .def("GetJointActuatorIndices", + overload_cast_explicit, + 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", diff --git a/bindings/pydrake/multibody/test/plant_test.py b/bindings/pydrake/multibody/test/plant_test.py index a4919ea63574..96cd53411189 100644 --- a/bindings/pydrake/multibody/test/plant_test.py +++ b/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) diff --git a/examples/allegro_hand/joint_control/allegro_single_object_simulation.cc b/examples/allegro_hand/joint_control/allegro_single_object_simulation.cc index 89a28a0544a0..e3fb24b61f76 100644 --- a/examples/allegro_hand/joint_control/allegro_single_object_simulation.cc +++ b/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& actuator = plant.get_mutable_joint_actuator(actuator_index); actuator.set_default_rotor_inertia(rotor_inertia); diff --git a/manipulation/models/franka_description/urdf/test/franka_arm_test.cc b/manipulation/models/franka_description/urdf/test/franka_arm_test.cc index 462d0f223dac..9b9cb8aa7127 100644 --- a/manipulation/models/franka_description/urdf/test/franka_arm_test.cc +++ b/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& canonical_joint_actuator = - canonical_plant.get_joint_actuator( - drake::multibody::JointActuatorIndex(i)); + canonical_plant.get_joint_actuator(index); const multibody::JointActuator& joint_actuator = - plant.get_joint_actuator(drake::multibody::JointActuatorIndex(i)); + plant.get_joint_actuator(index); CompareActuatorLimits(canonical_joint_actuator, joint_actuator); } diff --git a/manipulation/models/iiwa_description/test/iiwa_variants_parsing_test.cc b/manipulation/models/iiwa_description/test/iiwa_variants_parsing_test.cc index 0e41985d2522..d2b762956c76 100644 --- a/manipulation/models/iiwa_description/test/iiwa_variants_parsing_test.cc +++ b/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& canonical_joint_actuator = - canonical_plant.get_joint_actuator( - drake::multibody::JointActuatorIndex(i)); + canonical_plant.get_joint_actuator(index); const multibody::JointActuator& 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& canonical_joint_actuator = - canonical_plant.get_joint_actuator( - drake::multibody::JointActuatorIndex(i)); + canonical_plant.get_joint_actuator(index); const multibody::JointActuator& 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& 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); } diff --git a/multibody/plant/BUILD.bazel b/multibody/plant/BUILD.bazel index fcf9135d5c3d..f553695ab18d 100644 --- a/multibody/plant/BUILD.bazel +++ b/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", diff --git a/multibody/plant/discrete_update_manager.cc b/multibody/plant/discrete_update_manager.cc index f24c6c948a27..99b268a59342 100644 --- a/multibody/plant/discrete_update_manager.cc +++ b/multibody/plant/discrete_update_manager.cc @@ -440,8 +440,8 @@ void DiscreteUpdateManager::CalcJointActuationForces( actuation_wo_pd->setZero(); if (plant().num_actuators() > 0) { const VectorX u = AssembleActuationInput(context); - for (JointActuatorIndex actuator_index(0); - actuator_index < plant().num_actuators(); ++actuator_index) { + for (JointActuatorIndex actuator_index : + plant().GetJointActuatorIndices()) { const JointActuator& actuator = plant().get_joint_actuator(actuator_index); const Joint& joint = actuator.joint(); @@ -450,7 +450,7 @@ void DiscreteUpdateManager::CalcJointActuationForces( const int v_index = joint.velocity_start(); VectorX& actuation = actuator.has_controller() ? *actuation_w_pd : *actuation_wo_pd; - actuation[v_index] = u[actuator_index]; + actuation[v_index] = u[actuator.input_start()]; } } } diff --git a/multibody/plant/discrete_update_manager.h b/multibody/plant/discrete_update_manager.h index af5451f76807..76f66a71d3b9 100644 --- a/multibody/plant/discrete_update_manager.h +++ b/multibody/plant/discrete_update_manager.h @@ -195,7 +195,8 @@ class DiscreteUpdateManager : public ScalarConvertibleComponent { /* 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& EvalActuation(const systems::Context& context) const; /* Evaluates discrete contact pairs from all types of contact (point contact, diff --git a/multibody/plant/multibody_plant.cc b/multibody/plant/multibody_plant.cc index 8765dba1953c..11b0cd378d33 100644 --- a/multibody/plant/multibody_plant.cc +++ b/multibody/plant/multibody_plant.cc @@ -263,7 +263,7 @@ std::string GetScopedName(const MultibodyPlant& plant, // controlled. template bool AnyActuatorHasPdControl(const MultibodyPlant& 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 int NumOfPdControlledActuators(const MultibodyPlant& plant, ModelInstanceIndex model_instance) { int num_actuators = 0; - for (JointActuatorIndex a(0); a < plant.num_actuators(); ++a) { + for (JointActuatorIndex a : plant.GetJointActuatorIndices()) { const JointActuator& actuator = plant.get_joint_actuator(a); if (actuator.model_instance() == model_instance && actuator.has_controller()) { @@ -802,6 +802,11 @@ const JointActuator& MultibodyPlant::AddJointActuator( return this->mutable_tree().AddJointActuator(name, joint, effort_limit); } +template +void MultibodyPlant::RemoveJointActuator(const JointActuator& actuator) { + this->mutable_tree().RemoveJointActuator(actuator); +} + template geometry::SourceId MultibodyPlant::RegisterAsSourceForSceneGraph( SceneGraph* scene_graph) { @@ -1279,13 +1284,12 @@ void MultibodyPlant::FinalizePlantOnly() { template MatrixX MultibodyPlant::MakeActuationMatrix() const { MatrixX B = MatrixX::Zero(num_velocities(), num_actuated_dofs()); - for (JointActuatorIndex actuator_index(0); actuator_index < num_actuators(); - ++actuator_index) { + for (JointActuatorIndex actuator_index : GetJointActuatorIndices()) { const JointActuator& 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::MakeActuationMatrixPseudoinverse() const { // We leverage here the assumption that B (the actuation matrix) is a // permutation matrix, so Bᵀ is the pseudoinverse. std::vector> triplets; - for (JointActuatorIndex actuator_index(0); actuator_index < num_actuators(); - ++actuator_index) { + for (JointActuatorIndex actuator_index : GetJointActuatorIndices()) { const JointActuator& 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( - int{actuator.index()}, actuator.joint().velocity_start(), 1.0)); + int{actuator.input_start()}, actuator.joint().velocity_start(), 1.0)); } Eigen::SparseMatrix Bplus(num_actuated_dofs(), num_velocities()); @@ -1733,10 +1736,8 @@ std::vector MultibodyPlant::GetActuatorNames( DRAKE_MBP_THROW_IF_NOT_FINALIZED(); std::vector names(num_actuators()); - for (int actuator_index = 0; actuator_index < num_actuators(); - ++actuator_index) { - const JointActuator& actuator = - get_joint_actuator(JointActuatorIndex(actuator_index)); + for (JointActuatorIndex actuator_index : GetJointActuatorIndices()) { + const JointActuator& actuator = get_joint_actuator(actuator_index); const std::string prefix = add_model_instance_prefix ? fmt::format("{}_", @@ -2341,13 +2342,12 @@ void MultibodyPlant::AddJointActuationForces( DRAKE_DEMAND(forces->size() == num_velocities()); if (num_actuators() > 0) { const VectorX u = AssembleActuationInput(context); - for (JointActuatorIndex actuator_index(0); actuator_index < num_actuators(); - ++actuator_index) { + for (JointActuatorIndex actuator_index : GetJointActuatorIndices()) { const JointActuator& actuator = get_joint_actuator(actuator_index); const Joint& 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()]; } } } diff --git a/multibody/plant/multibody_plant.h b/multibody/plant/multibody_plant.h index 3763ce0afc77..e77dd6e0b208 100644 --- a/multibody/plant/multibody_plant.h +++ b/multibody/plant/multibody_plant.h @@ -402,11 +402,13 @@ the per model-instance ports ( @ref get_actuation_input_port(ModelInstanceIndex)const "get_actuation_input_port(ModelInstanceIndex)") are summed up. -@note The vector data supplied to %MultibodyPlant's actuation input ports should -be ordered by @ref JointActuatorIndex. For the get_actuation_input_port() that -covers all actuators, the iᵗʰ vector element corresponds to -`JointActuatorIndex(i)`. For the -@ref get_actuation_input_port(ModelInstanceIndex)const +@note A JointActuator's index into the vector data supplied to %MultibodyPlant's +actuation input port for all actuators (get_actuation_input_port()) is given by +JointActuator::input_start(), NOT by its JointActuatorIndex. That is, the vector +element data for a JointActuator at index JointActuatorIndex(i) in the full +input port vector is found at index: + MultibodyPlant::get_joint_actuator(JointActuatorIndex(i)).input_start(). +For the @ref get_actuation_input_port(ModelInstanceIndex)const "get_actuation_input_port(ModelInstanceIndex)" specific to a model index, the vector data is ordered by monotonically increasing @ref JointActuatorIndex for the actuators within that model instance: the 0ᵗʰ vector element @@ -849,7 +851,7 @@ class MultibodyPlant : public internal::MultibodyTreeSystem { /// Actuation values can be provided through a single input port which /// describes the entire plant, or through multiple input ports which each /// provide the actuation values for a specific model instance. See - /// AddJointActuator() and num_actuators(). + /// AddJointActuator() and num_actuated_dofs(). /// /// Output ports provide information about the entire %MultibodyPlant /// or its individual model instances. @@ -905,10 +907,10 @@ class MultibodyPlant : public internal::MultibodyTreeSystem { const; /// Returns a constant reference to the input port for external actuation for - /// all actuated dofs. This input port is a vector valued port indexed by - /// @ref JointActuatorIndex, see JointActuator::index(), and can be set with - /// JointActuator::set_actuation_vector(). - /// Refer to @ref mbp_actuation "Actuation" for further details. + /// all actuated dofs. This input port is a vector valued port and can be set + /// with JointActuator::set_actuation_vector(). JointActuators index into this + /// vector with an index given by JointActuator::index_start(). Refer to @ref + /// mbp_actuation "Actuation" for further details. /// @pre Finalize() was already called on `this` plant. /// @throws std::exception if called before Finalize(). const systems::InputPort& get_actuation_input_port() const; @@ -932,10 +934,10 @@ class MultibodyPlant : public internal::MultibodyTreeSystem { /// Returns a constant reference to the output port that reports actuation /// values applied through joint actuators. This output port is a vector - /// valued port indexed by @ref JointActuatorIndex, see - /// JointActuator::index(). Models that include PD controllers will include - /// their contribution in this port, refer to @ref mbp_actuation "Actuation" - /// for further details. + /// valued port. JointActuators index into this vector with an index given by + /// JointActuator::index_start(). Models that include PD controllers will + /// include their contribution in this port, refer to @ref mbp_actuation + /// "Actuation" for further details. /// @note PD controllers are not considered for actuators on locked joints, /// see Joint::Lock(). Therefore they do not contribute to this port. /// @pre Finalize() was already called on `this` plant. @@ -973,8 +975,13 @@ class MultibodyPlant : public internal::MultibodyTreeSystem { /// sized. This port must provide one desired position and one desired /// velocity per joint actuator. Desired state is assumed to be packed as xd = /// [qd, vd] that is, configurations first followed by velocities. - /// Configurations in qd are ordered by JointActuatorIndex, see - /// JointActuator::set_actuation_vector(). Similarly for velocities in vd. + /// Actuators index into both qd and vd using JointActuator::input_start(). + /// For example: + /// ``` + /// const double qd_actuator = xd[actuator.input_start()]; + /// const double vd_actuator = + /// xd[actuator.input_start() + plant.num_actuated_dofs()]; + /// ``` /// /// @warning If a user specifies a PD controller for an actuator from a given /// model instance, then all actuators of that model instance are required to @@ -1408,10 +1415,19 @@ class MultibodyPlant : public internal::MultibodyTreeSystem { /// will remain valid for the lifetime of `this` plant. /// @throws std::exception if `joint.num_velocities() > 1` since for now we /// only support actuators for single dof joints. + /// @see RemoveJointActuator() const JointActuator& AddJointActuator( const std::string& name, const Joint& joint, double effort_limit = std::numeric_limits::infinity()); + /// Removes and deletes `actuator` from this %MultibodyPlant. Any existing + /// references to `actuator` will become invalid, and future calls to + /// `get_joint_actuator(actuator_index)` will throw an exception. + /// + /// @throws std::exception if the plant is already finalized. + /// @see AddJointActuator() + void RemoveJointActuator(const JointActuator& actuator); + /// Creates a new model instance. Returns the index for the model /// instance. /// @@ -2911,7 +2927,7 @@ class MultibodyPlant : public internal::MultibodyTreeSystem { /// `u` of actuation values for the entire plant model. Refer to @ref /// mbp_actuation "Actuation" for further details. /// @param[in] u Actuation values for the entire model, indexed by - /// @ref JointActuatorIndex. + /// JointActuator::input_start(). /// @returns Actuation values for `model_instance`, ordered by monotonically /// increasing @ref JointActuatorIndex. /// @throws std::exception if `u` is not of size @@ -2930,8 +2946,8 @@ class MultibodyPlant : public internal::MultibodyTreeSystem { /// ordered by monotonically increasing @ref JointActuatorIndex within the /// model instance. /// @param[in,out] u Actuation values for the entire plant model, indexed by - /// @ref JointActuatorIndex. Only values corresponding to `model_instance` - /// are changed. + /// JointActuator::input_start(). Only values corresponding to + /// `model_instance` are changed. /// @throws std::exception if the size of `u_instance` is not equal to the /// number of actuation inputs for the joints of `model_instance`. void SetActuationInArray(ModelInstanceIndex model_instance, @@ -4377,7 +4393,8 @@ class MultibodyPlant : public internal::MultibodyTreeSystem { /// `user_to_actuator_index_map`. The actuation values in uₛ are a /// concatenation of the values for each actuator in the order they appear in /// `user_to_actuator_index_map`. - /// The full vector of actuation values u is ordered by JointActuatorIndex. + /// The full vector of actuation values u is indexed by + /// JointActuator::input_start(). MatrixX MakeActuatorSelectorMatrix( const std::vector& user_to_actuator_index_map) const { return internal_tree().MakeActuatorSelectorMatrix( @@ -4386,12 +4403,12 @@ class MultibodyPlant : public internal::MultibodyTreeSystem { /// This method creates an actuation matrix B mapping a vector of actuation /// values u into generalized forces `tau_u = B * u`, where B is a matrix of - /// size `nv x nu` with `nu` equal to num_actuators() and `nv` equal to + /// size `nv x nu` with `nu` equal to num_actuated_dofs() and `nv` equal to /// num_velocities(). - /// The vector u of actuation values is of size num_actuators(). For a given - /// JointActuator, `u[JointActuator::index()]` stores the value for the - /// external actuation corresponding to that actuator. `tau_u` on the other - /// hand is indexed by generalized velocity indexes according to + /// The vector u of actuation values is of size num_actuated_dofs(). For a + /// given JointActuator, `u[JointActuator::input_start()]` stores the value + /// for the external actuation corresponding to that actuator. `tau_u` on the + /// other hand is indexed by generalized velocity indexes according to /// `Joint::velocity_start()`. /// @warning B is a permutation matrix. While making a permutation has /// `O(n)` complexity, making a full B matrix has `O(n²)` complexity. For most @@ -4408,9 +4425,9 @@ class MultibodyPlant : public internal::MultibodyTreeSystem { /// Alternative signature to build an actuation selector matrix `Su` such /// that `u = Su⋅uₛ`, where u is the vector of actuation values for the full - /// model (ordered by JointActuatorIndex) and uₛ is a vector of actuation - /// values for the actuators acting on the joints listed by - /// `user_to_joint_index_map`. It is assumed that all joints referenced by + /// model (ordered by monotonically increasing JointActuatorIndex) and uₛ is a + /// vector of actuation values for the actuators acting on the joints listed + /// by `user_to_joint_index_map`. It is assumed that all joints referenced by /// `user_to_joint_index_map` are actuated. /// See MakeActuatorSelectorMatrix(const std::vector&) for /// details. @@ -4643,6 +4660,12 @@ class MultibodyPlant : public internal::MultibodyTreeSystem { return internal_tree().GetJointIndices(model_instance); } + /// Returns a list of all joint actuator indices. The vector is ordered by + /// monotonically increasing @ref JointActuatorIndex. + const std::vector& GetJointActuatorIndices() const { + return internal_tree().GetJointActuatorIndices(); + } + /// Returns a list of joint actuator indices associated with `model_instance`. /// The vector is ordered by monotonically increasing @ref JointActuatorIndex. /// @throws std::exception if called pre-finalize. @@ -4768,6 +4791,13 @@ class MultibodyPlant : public internal::MultibodyTreeSystem { return internal_tree().num_actuated_dofs(model_instance); } + /// Returns true if plant has a joint actuator with unique index + /// `actuator_index`. The value could be false if the actuator was removed + /// using RemoveJointActuator(). + bool has_joint_actuator(JointActuatorIndex actuator_index) const { + return internal_tree().has_joint_actuator(actuator_index); + } + /// Returns a constant reference to the joint actuator with unique index /// `actuator_index`. /// @throws std::exception if `actuator_index` does not correspond to a joint @@ -5251,14 +5281,17 @@ class MultibodyPlant : public internal::MultibodyTreeSystem { void EstimatePointContactParameters(double penetration_allowance); // Helper method to assemble actuation input vector from the appropriate - // ports. The return value is indexed by JointActuatorIndex. + // ports. The return value is indexed by JointActuator::input_start() (see + // MultibodyPlant::get_actuation_input_port()). VectorX AssembleActuationInput(const systems::Context& context) const; // Computes the net applied actuation through actuators. For continuous // models (thus far) this only inludes values coming from the // actuation_input_port. For discrete models, it includes actuator // controllers, see @ref mbp_actuation. Similarly to AssembleActuationInput(), - // this function assembles actuation values indexed by JointActuatorIndex. + // this function assembles actuation values indexed by + // JointActuator::input_start() (see + // MultibodyPlant::get_actuation_input_port()). void CalcActuationOutput(const systems::Context& context, systems::BasicVector* actuation) const; @@ -5266,7 +5299,8 @@ class MultibodyPlant : public internal::MultibodyTreeSystem { // assemble desired states for the full model from the input ports for // individual model instances. // The return stacks desired state as xd = [qd, vd], with both qd and vd - // indexed by JointActuatorIndex (it is assumed that qd.size() == vd.size()). + // indexed by JointActuator::input_start() (see + // MultibodyPlant::get_actuation_input_port()). VectorX AssembleDesiredStateInput( const systems::Context& context) const; diff --git a/multibody/plant/sap_driver.cc b/multibody/plant/sap_driver.cc index 7d20638949fc..dc904b58f0e7 100644 --- a/multibody/plant/sap_driver.cc +++ b/multibody/plant/sap_driver.cc @@ -719,15 +719,15 @@ void SapDriver::AddPdControllerConstraints( if (plant().num_actuators() == 0) return; // Desired positions & velocities. - const int num_actuators = plant().num_actuators(); + const int num_actuated_dofs = plant().num_actuated_dofs(); + // TODO(amcastro-tri): makes these EvalFoo() instead to avoid heap // allocations. const VectorX desired_state = manager_->AssembleDesiredStateInput(context); const VectorX feed_forward_actuation = manager_->AssembleActuationInput(context); - for (JointActuatorIndex actuator_index(0); - actuator_index < plant().num_actuators(); ++actuator_index) { + for (JointActuatorIndex actuator_index : plant().GetJointActuatorIndices()) { const JointActuator& actuator = plant().get_joint_actuator(actuator_index); if (actuator.has_controller()) { @@ -737,9 +737,9 @@ void SapDriver::AddPdControllerConstraints( // controllers on locked joints is considered to be zero. if (!joint.is_locked(context)) { const double effort_limit = actuator.effort_limit(); - const T& qd = desired_state[actuator.index()]; - const T& vd = desired_state[num_actuators + actuator.index()]; - const T& u0 = feed_forward_actuation[actuator.index()]; + const T& qd = desired_state[actuator.input_start()]; + const T& vd = desired_state[num_actuated_dofs + actuator.input_start()]; + const T& u0 = feed_forward_actuation[actuator.input_start()]; const T& q0 = joint.GetOnePosition(context); const int dof = joint.velocity_start(); @@ -1102,8 +1102,7 @@ void SapDriver::CalcActuation(const systems::Context& context, // Map generalized forces to actuation indexing. int constraint_index = start; - for (JointActuatorIndex actuator_index(0); - actuator_index < plant().num_actuators(); ++actuator_index) { + for (JointActuatorIndex actuator_index : plant().GetJointActuatorIndices()) { const JointActuator& actuator = plant().get_joint_actuator(actuator_index); const Joint& joint = actuator.joint(); @@ -1119,7 +1118,7 @@ void SapDriver::CalcActuation(const systems::Context& context, DRAKE_DEMAND(c.num_constraint_equations() == 1); // Each actuator defines a single PD controller. - actuation->coeffRef(actuator_index) = tau_pd(dof); + actuation->coeffRef(actuator.input_start()) = tau_pd(dof); } } // Sanity check consistency with the code that added the constraints, diff --git a/multibody/plant/test/actuated_models_test.cc b/multibody/plant/test/actuated_models_test.cc index 6c93411d3319..4c7517f8984f 100644 --- a/multibody/plant/test/actuated_models_test.cc +++ b/multibody/plant/test/actuated_models_test.cc @@ -44,7 +44,7 @@ namespace { // This fixture loads a MultibodyPlant model of a KUKA Iiiwa arm with a Schunk // gripper. -class ActuatedIiiwaArmTest : public ::testing::Test { +class ActuatedIiwaArmTest : public ::testing::Test { public: // Enum to control the PD model for ther kuka arm and its gripper. // This does not affect the acrobot model, which has no PD controllers. @@ -106,8 +106,8 @@ class ActuatedIiiwaArmTest : public ::testing::Test { // Arm actuators. std::vector arm_actuators; - for (JointActuatorIndex actuator_index(0); - actuator_index < plant_->num_actuators(); ++actuator_index) { + for (JointActuatorIndex actuator_index : + plant_->GetJointActuatorIndices()) { if (plant_->get_joint_actuator(actuator_index).model_instance() == arm_model_) { arm_actuators.push_back(actuator_index); @@ -130,8 +130,8 @@ class ActuatedIiiwaArmTest : public ::testing::Test { auto& actuator3 = plant_->get_mutable_joint_actuator(arm_actuators[3]); actuator3.set_controller_gains({kProportionalGain_, kDerivativeGain_}); } else if (model_config == ModelConfiguration::kModelWithZeroGains) { - for (JointActuatorIndex actuator_index(0); - actuator_index < plant_->num_actuators(); ++actuator_index) { + for (JointActuatorIndex actuator_index : + plant_->GetJointActuatorIndices()) { JointActuator& actuator = plant_->get_mutable_joint_actuator(actuator_index); // We do not add PD controllers to the acrobot. @@ -146,20 +146,26 @@ class ActuatedIiiwaArmTest : public ::testing::Test { // We make the acrobot fully actuated. const Joint& acrobot_shoulder = plant_->GetJointByName("ShoulderJoint", acrobot_model_); - plant_->AddJointActuator("ShoulderActuator", acrobot_shoulder); + plant_->AddJointActuator("ShoulderJoint", acrobot_shoulder); // N.B. Notice that this actuator is added at a later state long after other // model instances were added to the plant. This will allow testing that // actuation input is assembled as documented by monotonically // increasing JointActuatorIndex, regardless of model instance index. + if (test_remove_joint_actuators_) { + plant_->RemoveJointActuator( + plant_->GetJointActuatorByName("iiwa_joint_3")); + plant_->RemoveJointActuator(plant_->GetJointActuatorByName("ElbowJoint")); + } + plant_->Finalize(); context_ = plant_->CreateDefaultContext(); } void SetGripperModel() { - for (JointActuatorIndex actuator_index(0); - actuator_index < plant_->num_actuators(); ++actuator_index) { + for (JointActuatorIndex actuator_index : + plant_->GetJointActuatorIndices()) { JointActuator& actuator = plant_->get_mutable_joint_actuator(actuator_index); if (actuator.model_instance() == gripper_model_) { @@ -191,8 +197,8 @@ class ActuatedIiiwaArmTest : public ::testing::Test { // Given the actuation for each model instance separately, this function // assembles the actuation vector for the full MultibodyPlant model. This is // the actuation vector consumed by - // MultibodyPlant::get_actuation_input_port(), ordered by JointActuatorIndex, - // regardless of model instance. + // MultibodyPlant::get_actuation_input_port(), ordered by JointActuatorIndex + // (with possible gaps), regardless of model instance. VectorXd AssembleFullModelActuation(const VectorXd& arm_u, const VectorXd& acrobot_u, const VectorXd& gripper_u) { @@ -279,9 +285,10 @@ class ActuatedIiiwaArmTest : public ::testing::Test { ModelInstanceIndex gripper_model_; ModelInstanceIndex box_model_; std::unique_ptr> context_; + bool test_remove_joint_actuators_{false}; }; -TEST_F(ActuatedIiiwaArmTest, JointActuatorApis) { +TEST_F(ActuatedIiwaArmTest, JointActuatorApis) { SetUpModel(); for (JointActuatorIndex actuator_index : plant_->GetJointActuatorIndices(gripper_model_)) { @@ -293,7 +300,7 @@ TEST_F(ActuatedIiiwaArmTest, JointActuatorApis) { } } -TEST_F(ActuatedIiiwaArmTest, GetActuationInputPort) { +TEST_F(ActuatedIiwaArmTest, GetActuationInputPort) { SetUpModel(); EXPECT_NO_THROW(plant_->get_actuation_input_port(arm_model_)); @@ -311,7 +318,7 @@ TEST_F(ActuatedIiiwaArmTest, GetActuationInputPort) { "num_model_instances\\(\\)' failed."); } -TEST_F(ActuatedIiiwaArmTest, GetDesiredStatePort) { +TEST_F(ActuatedIiwaArmTest, GetDesiredStatePort) { SetUpModel(ModelConfiguration::kArmIsNotControlled); EXPECT_NO_THROW(plant_->get_desired_state_input_port(arm_model_)); @@ -336,7 +343,7 @@ TEST_F(ActuatedIiiwaArmTest, GetDesiredStatePort) { // Verify the assembly of actuation input ports. In particular, we verify this // assembly is performed in the order of JointActuatorIndex and that // disconnected ports default to zero values. -TEST_F(ActuatedIiiwaArmTest, AssembleActuationInput) { +TEST_F(ActuatedIiwaArmTest, AssembleActuationInput) { // We setup a model with one PD controlled model instance (the gripper) and a // model instance without PD control (the arm). SetUpModel(ModelConfiguration::kArmIsNotControlled); @@ -370,7 +377,7 @@ TEST_F(ActuatedIiiwaArmTest, AssembleActuationInput) { // Verify that MultibodyPlant::AssembleDesiredStateInput() throws an exception // when not all actuators in a model instance are PD controlled. Once a PD // controller is defined in a model instance, all actuators must use PD control. -TEST_F(ActuatedIiiwaArmTest, +TEST_F(ActuatedIiwaArmTest, AssembleDesiredStateInput_ThrowsIfPartiallyPDControlled) { SetUpModel(ModelConfiguration::kArmIsPartiallyControlled); @@ -385,7 +392,7 @@ TEST_F(ActuatedIiiwaArmTest, "Model iiwa7 is partially PD controlled. .*"); } -TEST_F(ActuatedIiiwaArmTest, +TEST_F(ActuatedIiwaArmTest, AssembleDesiredStateInput_ThrowsIfDesiredStateNotConnected) { SetUpModel(); @@ -399,7 +406,7 @@ TEST_F(ActuatedIiiwaArmTest, // Verify the assembly of desired states for a plant with a single PD controlled // model instance. -TEST_F(ActuatedIiiwaArmTest, +TEST_F(ActuatedIiwaArmTest, AssembleDesiredStateInput_VerifyAssemblyWithOneModel) { SetUpModel(); @@ -435,7 +442,7 @@ TEST_F(ActuatedIiiwaArmTest, // Verify the assembly of desired states for a plant with two PD controlled // model instances. -TEST_F(ActuatedIiiwaArmTest, +TEST_F(ActuatedIiwaArmTest, AssembleDesiredStateInput_VerifyAssemblyWithTwoModels) { SetUpModel(ModelConfiguration::kArmIsControlled); @@ -472,7 +479,7 @@ TEST_F(ActuatedIiiwaArmTest, EXPECT_EQ(full_xd, expected_xd); } -TEST_F(ActuatedIiiwaArmTest, +TEST_F(ActuatedIiwaArmTest, PdControlledActuatorsOnlySupportedForDiscreteModels) { DRAKE_EXPECT_THROWS_MESSAGE( SetUpModel(ModelConfiguration::kArmIsControlled, @@ -485,7 +492,7 @@ TEST_F(ActuatedIiiwaArmTest, // This unit test verifies that, when within effort limits, forces applied // through the generalized forces input port has the same effect as applying the // same forces using the actuation input port. -TEST_F(ActuatedIiiwaArmTest, +TEST_F(ActuatedIiwaArmTest, WithinEffortLimitsActuationMatchesAppliedGeneralizedForces) { // We add PD controllers but set their gains to zero since for this test we // are only interested on verifying that the effect of input actuation in the @@ -553,7 +560,7 @@ TEST_F(ActuatedIiiwaArmTest, } // We verify that the PD controlled actuators exert effort limits. -TEST_F(ActuatedIiiwaArmTest, +TEST_F(ActuatedIiwaArmTest, OutsideEffortLimitsActuationMatchesAppliedGeneralizedForces) { // We add PD controllers but set their gains to zero since for this test we // are only interested on verifying that the effect of input actuation in the @@ -640,7 +647,7 @@ TEST_F(ActuatedIiiwaArmTest, // This test verifies that for continuous models the actuation output port // simply feeds through the actuation inputs. -TEST_F(ActuatedIiiwaArmTest, +TEST_F(ActuatedIiwaArmTest, ActuationOutputForContinuousModelsFeedsThroughActuationInput) { SetUpModel(ModelConfiguration::kNoPdControl, MultibodyPlantConfig{.time_step = 0.0}); @@ -649,7 +656,7 @@ TEST_F(ActuatedIiiwaArmTest, // This test verifies that discrete models using a solver other than SAP, simply // feed through the actuation inputs. -TEST_F(ActuatedIiiwaArmTest, +TEST_F(ActuatedIiwaArmTest, ActuationOutputForDiscreteNonSapModelsFeedsThroughActuationInput) { SetUpModel(ModelConfiguration::kNoPdControl, MultibodyPlantConfig{.time_step = 0.01, @@ -659,7 +666,7 @@ TEST_F(ActuatedIiiwaArmTest, // This test verifies that SAP models without PD controllers also feed through // the actuation input to the actuation output. -TEST_F(ActuatedIiiwaArmTest, +TEST_F(ActuatedIiwaArmTest, ActuationOutputForDiscreteSapModelsFeedsThroughActuationInput) { SetUpModel(ModelConfiguration::kNoPdControl, MultibodyPlantConfig{.time_step = 0.01, @@ -667,6 +674,71 @@ TEST_F(ActuatedIiiwaArmTest, VerifyActuationOutputFeedsThroughActuationInputs(); } +// Call the same methods as the RemoveJointActuator test to confirm the values +// _without_ the actuators removed. +TEST_F(ActuatedIiwaArmTest, DontRemoveJointActuator) { + SetUpModel(); + + EXPECT_EQ(plant_->num_actuators(), 7 + 2 + 2); + EXPECT_EQ(plant_->num_actuators(arm_model_), 7); + EXPECT_EQ(plant_->num_actuators(acrobot_model_), 2); + EXPECT_EQ(plant_->num_actuators(gripper_model_), 2); + + EXPECT_EQ(plant_->num_actuated_dofs(), 7 + 2 + 2); + EXPECT_EQ(plant_->num_actuated_dofs(arm_model_), 7); + EXPECT_EQ(plant_->num_actuated_dofs(acrobot_model_), 2); + EXPECT_EQ(plant_->num_actuated_dofs(gripper_model_), 2); + + EXPECT_TRUE(plant_->HasJointActuatorNamed("iiwa_joint_3")); + EXPECT_TRUE(plant_->HasJointActuatorNamed("iiwa_joint_3", arm_model_)); + EXPECT_TRUE(plant_->HasJointActuatorNamed("ElbowJoint")); + EXPECT_TRUE(plant_->HasJointActuatorNamed("ElbowJoint", acrobot_model_)); + + const JointActuator& iiwa_joint_3_actuator = + plant_->GetJointActuatorByName("iiwa_joint_3"); + EXPECT_EQ(plant_->GetJointActuatorByName("iiwa_joint_3", arm_model_).index(), + iiwa_joint_3_actuator.index()); + EXPECT_TRUE(plant_->has_joint_actuator(iiwa_joint_3_actuator.index())); + + // GetJointActuatorIndices. + EXPECT_EQ(plant_->GetJointActuatorIndices().size(), 7 + 2 + 2); + const JointActuator& shoulder_joint_actuator = + plant_->GetJointActuatorByName("ShoulderJoint"); + const JointActuator& elbow_joint_actuator = + plant_->GetJointActuatorByName("ElbowJoint"); + EXPECT_THAT(plant_->GetJointActuatorIndices(acrobot_model_), + testing::ElementsAre(elbow_joint_actuator.index(), + shoulder_joint_actuator.index())); +} + +TEST_F(ActuatedIiwaArmTest, RemoveJointActuator) { + // Remove iiwa_joint_3 and ElbowJoint from the model. + test_remove_joint_actuators_ = true; + SetUpModel(); + + EXPECT_EQ(plant_->num_actuators(), 6 + 1 + 2); + EXPECT_EQ(plant_->num_actuators(arm_model_), 6); + EXPECT_EQ(plant_->num_actuators(acrobot_model_), 1); + EXPECT_EQ(plant_->num_actuators(gripper_model_), 2); + + EXPECT_EQ(plant_->num_actuated_dofs(), 6 + 1 + 2); + EXPECT_EQ(plant_->num_actuated_dofs(arm_model_), 6); + EXPECT_EQ(plant_->num_actuated_dofs(acrobot_model_), 1); + EXPECT_EQ(plant_->num_actuated_dofs(gripper_model_), 2); + + EXPECT_FALSE(plant_->HasJointActuatorNamed("iiwa_joint_3")); + EXPECT_FALSE(plant_->HasJointActuatorNamed("iiwa_joint_3", arm_model_)); + EXPECT_FALSE(plant_->HasJointActuatorNamed("ElbowJoint")); + EXPECT_FALSE(plant_->HasJointActuatorNamed("ElbowJoint", acrobot_model_)); + + // GetJointActuatorIndices. + EXPECT_EQ(plant_->GetJointActuatorIndices().size(), 6 + 1 + 2); + const JointActuator& shoulder_joint_actuator = + plant_->GetJointActuatorByName("ShoulderJoint"); + EXPECT_THAT(plant_->GetJointActuatorIndices(acrobot_model_), + testing::ElementsAre(shoulder_joint_actuator.index())); +} + } // namespace } // namespace multibody } // namespace drake diff --git a/multibody/plant/test/discrete_update_manager_test.cc b/multibody/plant/test/discrete_update_manager_test.cc index 6a7336d561b0..dcfb692166bb 100644 --- a/multibody/plant/test/discrete_update_manager_test.cc +++ b/multibody/plant/test/discrete_update_manager_test.cc @@ -7,15 +7,31 @@ #include "drake/multibody/plant/multibody_plant.h" #include "drake/multibody/plant/multibody_plant_config_functions.h" #include "drake/multibody/plant/test/dummy_model.h" +#include "drake/multibody/plant/test_utilities/multibody_plant_remodeling.h" +#include "drake/multibody/tree/revolute_joint.h" #include "drake/systems/analysis/simulator.h" #include "drake/systems/framework/abstract_value_cloner.h" #include "drake/systems/primitives/pass_through.h" #include "drake/systems/primitives/zero_order_hold.h" namespace drake { namespace multibody { + +class MultibodyPlantTester { + public: + MultibodyPlantTester() = delete; + + template + static internal::DiscreteUpdateManager* discrete_update_manager( + const MultibodyPlant& plant) { + return plant.discrete_update_manager_.get(); + } +}; + namespace internal { namespace test { using contact_solvers::internal::ContactSolverResults; +using Eigen::Vector2d; +using Eigen::Vector3d; using Eigen::VectorXd; using systems::BasicVector; using systems::Context; @@ -23,6 +39,7 @@ using systems::ContextBase; using systems::DiscreteStateIndex; using systems::DiscreteValues; using systems::OutputPortIndex; + // Dummy state data. constexpr int kNumRigidDofs = 6; constexpr int kNumAdditionalDofs = 9; @@ -476,6 +493,42 @@ GTEST_TEST(DiscreteUpdateManagerCacheEntry, ContactSolverResults) { EXPECT_TRUE(CompareMatrices(v, Vector1(nonzero_actuation * dt))); } +/* Tests that actuation forces are accumulated using the correct indexing from + JointActuaton::input_start() */ +TEST_F(MultibodyPlantRemodeling, RemoveJointActuator) { + // Set gravity vector to zero so there is no force element contribution. + plant_->mutable_gravity_field().set_gravity_vector(Vector3d::Zero()); + + FinalizeAndBuild(); + + const systems::InputPort& u_input = + plant_->get_actuation_input_port(); + u_input.FixValue(plant_context_, Vector2d(1.0, 3.0)); + + DiscreteUpdateManager* manager = + MultibodyPlantTester::discrete_update_manager(*plant_); + + // CalcNonContactForces includes: + // - Force elements + // - Externally applied general/spatial forces + // - Feed forward actuation + // - PD controlled actuation + // - Joint limits penalty forces + // By construction of the model above, all of these are zero except for the + // feed forward actuation. Thus + // DiscreteUpdateManager::CalcJointActuationForces() is the only function that + // contributes to the accumulated forces. This tests that the indexing in + // CalcJointActuationForces() correctly uses JointActuaton::input_start(). + MultibodyForces forces(*plant_); + manager->CalcNonContactForces( + *plant_context_, false /* no joint limit penalty forces */, + false /* no pd controlled actuator forces */, &forces); + + const Vector3d expected_actuation_wo_pd(1.0, 0.0, 3.0); + EXPECT_TRUE( + CompareMatrices(forces.generalized_forces(), expected_actuation_wo_pd)); +} + } // namespace } // namespace test } // namespace internal diff --git a/multibody/plant/test/joint_limits_test.cc b/multibody/plant/test/joint_limits_test.cc index 59bab7b47df2..c182601cbb21 100644 --- a/multibody/plant/test/joint_limits_test.cc +++ b/multibody/plant/test/joint_limits_test.cc @@ -196,7 +196,7 @@ VectorX KukaPositionUpperLimits() { void SetReflectedInertiaToZero(MultibodyPlant* plant) { DRAKE_DEMAND(plant != nullptr); - for (JointActuatorIndex index(0); index < plant->num_actuators(); ++index) { + for (JointActuatorIndex index : plant->GetJointActuatorIndices()) { JointActuator& joint_actuator = plant->get_mutable_joint_actuator(index); joint_actuator.set_default_rotor_inertia(0.0); diff --git a/multibody/plant/test/multibody_plant_reflected_inertia_test.cc b/multibody/plant/test/multibody_plant_reflected_inertia_test.cc index 51af99118a56..2e3955bb97c6 100644 --- a/multibody/plant/test/multibody_plant_reflected_inertia_test.cc +++ b/multibody/plant/test/multibody_plant_reflected_inertia_test.cc @@ -46,7 +46,10 @@ class MultibodyPlantReflectedInertiaTests : public ::testing::Test { // @pre It is expected that: // rotor_inertias.size() == gear_ratios.size() == plant_ri_.num_actuators() // and that the index in the vector corresponds with the joint actuator - // index. + // index. For consistency with the rest of Drake we still call + // GetJointActuatorIndices() below to iterate over the actuators, but the + // returned indices will always start from 0 and count up by 1 each with no + // gaps because no actuators have been removed. // // See the section 'Reflected Inertia' in documentation for // drake::multibody::JointActuator for more details. @@ -137,7 +140,7 @@ class MultibodyPlantReflectedInertiaTests : public ::testing::Test { const VectorX& rotor_inertias, const VectorX& gear_ratios) { DRAKE_DEMAND(plant != nullptr); - for (JointActuatorIndex index(0); index < plant->num_actuators(); ++index) { + for (JointActuatorIndex index : plant->GetJointActuatorIndices()) { JointActuator& joint_actuator = plant->get_mutable_joint_actuator(index); joint_actuator.set_default_rotor_inertia(rotor_inertias(int{index})); @@ -147,7 +150,7 @@ class MultibodyPlantReflectedInertiaTests : public ::testing::Test { void SetReflectedInertiaToZero(MultibodyPlant* plant) { DRAKE_DEMAND(plant != nullptr); - for (JointActuatorIndex index(0); index < plant->num_actuators(); ++index) { + for (JointActuatorIndex index : plant->GetJointActuatorIndices()) { JointActuator& joint_actuator = plant->get_mutable_joint_actuator(index); joint_actuator.set_default_rotor_inertia(0.0); @@ -609,11 +612,10 @@ TEST_F(MultibodyPlantReflectedInertiaTests, DefaultParameters) { // Load the models. LoadBothModelsSetStateAndFinalize(rotor_inertias, gear_ratios); - for (JointActuatorIndex index(0); index < plant_ri_.num_actuators(); - ++index) { + for (JointActuatorIndex index : plant_ri_.GetJointActuatorIndices()) { JointActuator& joint_actuator = dynamic_cast&>( - plant_ri_.get_mutable_joint_actuator(JointActuatorIndex(index))); + plant_ri_.get_mutable_joint_actuator(index)); // Default gear ratio and rotor inertia end up in the context. EXPECT_EQ(joint_actuator.default_gear_ratio(), @@ -622,11 +624,10 @@ TEST_F(MultibodyPlantReflectedInertiaTests, DefaultParameters) { joint_actuator.rotor_inertia(*context_ri_)); } - for (JointActuatorIndex index(0); index < plant_ri_.num_actuators(); - ++index) { + for (JointActuatorIndex index : plant_ri_.GetJointActuatorIndices()) { JointActuator& joint_actuator = dynamic_cast&>( - plant_ri_.get_mutable_joint_actuator(JointActuatorIndex(index))); + plant_ri_.get_mutable_joint_actuator(index)); // Set the model parameters to something different. joint_actuator.set_default_gear_ratio(99); joint_actuator.set_default_rotor_inertia(100); @@ -635,11 +636,10 @@ TEST_F(MultibodyPlantReflectedInertiaTests, DefaultParameters) { // Create a new default context. auto context = plant_ri_.CreateDefaultContext(); - for (JointActuatorIndex index(0); index < plant_ri_.num_actuators(); - ++index) { + for (JointActuatorIndex index : plant_ri_.GetJointActuatorIndices()) { JointActuator& joint_actuator = dynamic_cast&>( - plant_ri_.get_mutable_joint_actuator(JointActuatorIndex(index))); + plant_ri_.get_mutable_joint_actuator(index)); // New default values should propagate to a new default context. EXPECT_EQ(joint_actuator.default_gear_ratio(), joint_actuator.gear_ratio(*context)); diff --git a/multibody/plant/test/multibody_plant_test.cc b/multibody/plant/test/multibody_plant_test.cc index d96b0a22a721..4a3b9ea2e780 100644 --- a/multibody/plant/test/multibody_plant_test.cc +++ b/multibody/plant/test/multibody_plant_test.cc @@ -34,6 +34,7 @@ #include "drake/multibody/parsing/parser.h" #include "drake/multibody/plant/discrete_contact_pair.h" #include "drake/multibody/plant/externally_applied_spatial_force.h" +#include "drake/multibody/plant/test_utilities/multibody_plant_remodeling.h" #include "drake/multibody/test_utilities/add_fixed_objects_to_plant.h" #include "drake/multibody/tree/planar_joint.h" #include "drake/multibody/tree/prismatic_joint.h" @@ -102,6 +103,13 @@ class MultibodyPlantTester { GeometryId id) { return plant.FindBodyByGeometryId(id); } + + template + static void AddJointActuationForces(const MultibodyPlant& plant, + const systems::Context& context, + VectorX* forces) { + plant.AddJointActuationForces(context, forces); + } }; namespace { @@ -2799,6 +2807,65 @@ GTEST_TEST(KukaModel, ActuationMatrix) { EXPECT_TRUE((B_inv * B).isIdentity()); } +TEST_F(MultibodyPlantRemodeling, MakeActuationMatrix) { + FinalizeAndBuild(); + + // Actuator with index 1 has been removed. + // clang-format off + const Eigen::MatrixXd B_expected = + (Eigen::MatrixXd(3, 2) << 1, 0, + 0, 0, + 0, 1).finished(); + // clang-format on + + // Test that MakeActuationMatrix uses the correct indices into 'u' + // using JointActuator::input_start(). + const Eigen::MatrixXd B = plant_->MakeActuationMatrix(); + EXPECT_TRUE(CompareMatrices(B, B_expected)); +} + +TEST_F(MultibodyPlantRemodeling, MakeActuatorSelectorMatrix) { + FinalizeAndBuild(); + + // Actuator with index 1 ("actuator1") has been removed. + // Flip the order of the actuators in the user list compared to the input + // ordering. + const std::vector user_to_actuator_index_map{ + plant_->GetJointActuatorByName("actuator2").index(), + plant_->GetJointActuatorByName("actuator0").index()}; + + // clang-format off + const Eigen::MatrixXd Su_expected = + (Eigen::MatrixXd(2, 2) << 0, 1, + 1, 0).finished(); + // clang-format on + + // Test that MakeActuationMatrix uses the correct indices into 'u' + // using JointActuator::input_start(). + const Eigen::MatrixXd Su = + plant_->MakeActuatorSelectorMatrix(user_to_actuator_index_map); + EXPECT_TRUE(CompareMatrices(Su, Su_expected)); +} + +TEST_F(MultibodyPlantRemodeling, AddJointActuationForces) { + FinalizeAndBuild(); + + // Actuator with index 1 has been removed. + const systems::InputPort& u_input = + plant_->get_actuation_input_port(); + u_input.FixValue(plant_context_, Vector2d(1.0, 3.0)); + + const VectorXd forces_expected = (VectorXd(3) << 1.0, 0.0, 3.0).finished(); + + // Test that AddJointActuationForces uses the correct indices into 'u' + // using JointActuator::input_start(). + VectorXd forces(3); + forces.setZero(); + MultibodyPlantTester::AddJointActuationForces(*plant_, *plant_context_, + &forces); + EXPECT_TRUE(CompareMatrices(forces, forces_expected)); +} + // Unit test fixture for a model of Kuka Iiwa arm parametrized on the periodic // update period of the plant. This allows us to test some of the plant's // functionality for both continuous and discrete models. diff --git a/multibody/plant/test/sap_driver_joint_limits_test.cc b/multibody/plant/test/sap_driver_joint_limits_test.cc index b913f45d341f..e4f1f61cb088 100644 --- a/multibody/plant/test/sap_driver_joint_limits_test.cc +++ b/multibody/plant/test/sap_driver_joint_limits_test.cc @@ -277,7 +277,7 @@ class KukaIiwaArmTests : public ::testing::Test { const VectorX& gear_ratios) const { DRAKE_DEMAND(plant != nullptr); int local_joint_index = 0; - for (JointActuatorIndex index(0); index < plant->num_actuators(); ++index) { + for (JointActuatorIndex index : plant->GetJointActuatorIndices()) { JointActuator& joint_actuator = plant->get_mutable_joint_actuator(index); if (std::count(models.begin(), models.end(), diff --git a/multibody/plant/test/sap_driver_pd_controller_constraints_test.cc b/multibody/plant/test/sap_driver_pd_controller_constraints_test.cc index edf1c0ca3c1c..667e6a0029ef 100644 --- a/multibody/plant/test/sap_driver_pd_controller_constraints_test.cc +++ b/multibody/plant/test/sap_driver_pd_controller_constraints_test.cc @@ -113,8 +113,8 @@ class ActuatedIiiwaArmTest : public ::testing::Test { } void SetGripperModel() { - for (JointActuatorIndex actuator_index(0); - actuator_index < plant_->num_actuators(); ++actuator_index) { + for (JointActuatorIndex actuator_index : + plant_->GetJointActuatorIndices()) { JointActuator& actuator = plant_->get_mutable_joint_actuator(actuator_index); if (actuator.model_instance() == gripper_model_) { @@ -148,7 +148,7 @@ TEST_F(ActuatedIiiwaArmTest, VerifyConstraints) { // Sanity check we only defined PD controllers for the grippers DOFs. int num_controlled_actuators = 0; - for (JointActuatorIndex a(0); a < plant_->num_actuators(); ++a) { + for (JointActuatorIndex a : plant_->GetJointActuatorIndices()) { const JointActuator& actuator = plant_->get_joint_actuator(a); if (actuator.has_controller()) ++num_controlled_actuators; } diff --git a/multibody/plant/test/sap_driver_test.cc b/multibody/plant/test/sap_driver_test.cc index c676c4685a1a..50d569775ad1 100644 --- a/multibody/plant/test/sap_driver_test.cc +++ b/multibody/plant/test/sap_driver_test.cc @@ -9,12 +9,15 @@ #include "drake/multibody/contact_solvers/contact_solver_utils.h" #include "drake/multibody/contact_solvers/sap/sap_contact_problem.h" #include "drake/multibody/contact_solvers/sap/sap_friction_cone_constraint.h" +#include "drake/multibody/contact_solvers/sap/sap_pd_controller_constraint.h" #include "drake/multibody/contact_solvers/sap/sap_solver.h" #include "drake/multibody/contact_solvers/sap/sap_solver_results.h" #include "drake/multibody/plant/compliant_contact_manager.h" #include "drake/multibody/plant/multibody_plant.h" #include "drake/multibody/plant/test/compliant_contact_manager_tester.h" #include "drake/multibody/plant/test/spheres_stack.h" +#include "drake/multibody/plant/test_utilities/multibody_plant_remodeling.h" +#include "drake/multibody/tree/multibody_tree_indexes.h" #include "drake/multibody/tree/revolute_joint.h" #include "drake/multibody/tree/rigid_body.h" @@ -25,19 +28,35 @@ using drake::multibody::contact_solvers::internal::ContactSolverResults; using drake::multibody::contact_solvers::internal::MergeNormalAndTangent; using drake::multibody::contact_solvers::internal::SapContactProblem; using drake::multibody::contact_solvers::internal::SapFrictionConeConstraint; +using drake::multibody::contact_solvers::internal::SapPdControllerConstraint; using drake::multibody::contact_solvers::internal::SapSolver; using drake::multibody::contact_solvers::internal::SapSolverParameters; using drake::multibody::contact_solvers::internal::SapSolverResults; +using drake::multibody::internal::CompliantContactManager; using drake::multibody::internal::DiscreteContactPair; using drake::systems::Context; using Eigen::MatrixXd; +using Eigen::Vector2d; using Eigen::Vector3d; +using Eigen::Vector4d; using Eigen::VectorXd; // TODO(amcastro-tri): Implement AutoDiffXd testing. namespace drake { namespace multibody { + +class MultibodyPlantTester { + public: + static CompliantContactManager& manager( + const MultibodyPlant& plant) { + auto* manager = dynamic_cast*>( + plant.discrete_update_manager_.get()); + DRAKE_DEMAND(manager != nullptr); + return *manager; + } +}; + namespace internal { constexpr double kEps = std::numeric_limits::epsilon(); @@ -490,6 +509,57 @@ GTEST_TEST(SapDriverTest, ConstraintActiveStatus) { } } +TEST_F(MultibodyPlantRemodeling, AddPdControllerConstraints) { + // Set up actuators with pd controllers. + for (JointActuatorIndex actuator_index : plant_->GetJointActuatorIndices()) { + JointActuator& actuator = + plant_->get_mutable_joint_actuator(actuator_index); + actuator.set_controller_gains({100, 10}); + } + + FinalizeAndBuild(); + + // Actuator with index 1 has been removed. + const Vector2d u_feed_forward(1.0, 2.0); + const Vector4d x_desired(3.0, 4.0, 5.0, 6.0); + + // This is hardcoded based on internal knowledge of the model. + const std::vector dof_to_actuator_input_start{0, -1, 1}; + + const systems::InputPort& u_input = + plant_->get_actuation_input_port(); + u_input.FixValue(plant_context_, u_feed_forward); + + const systems::InputPort& x_desired_input = + plant_->get_desired_state_input_port(default_model_instance()); + x_desired_input.FixValue(plant_context_, x_desired); + + // Test that AddPdControllerConstraints uses the correct indices into 'u' + // using JointActuator::input_start(). + const SapDriver& sap_driver = + CompliantContactManagerTester::sap_driver( + MultibodyPlantTester::manager(*plant_)); + const ContactProblemCache& problem_cache = + SapDriverTest::EvalContactProblemCache(sap_driver, *plant_context_); + const SapContactProblem& problem = *problem_cache.sap_problem; + + EXPECT_EQ(problem_cache.num_pd_controller_constraints, 2); + + for (int i = 0; i < problem_cache.num_pd_controller_constraints; ++i) { + const int constraint_id = problem_cache.pd_controller_constraints_start + i; + const SapPdControllerConstraint& pd_constraint = + dynamic_cast&>( + problem.get_constraint(constraint_id)); + const auto configuration = pd_constraint.configuration(); + const int actuator_input_start = + dof_to_actuator_input_start[configuration.clique_dof]; + EXPECT_EQ(configuration.qd, x_desired(actuator_input_start)); + EXPECT_EQ(configuration.vd, + x_desired(actuator_input_start + plant_->num_actuated_dofs())); + EXPECT_EQ(configuration.u0, u_feed_forward(actuator_input_start)); + } +} + } // namespace internal } // namespace multibody } // namespace drake diff --git a/multibody/plant/test_utilities/BUILD.bazel b/multibody/plant/test_utilities/BUILD.bazel index 69b94613d93b..8cb9dcb30047 100644 --- a/multibody/plant/test_utilities/BUILD.bazel +++ b/multibody/plant/test_utilities/BUILD.bazel @@ -14,6 +14,7 @@ drake_cc_package_library( testonly = 1, visibility = ["//visibility:public"], deps = [ + ":multibody_plant_remodeling", ":rigid_body_on_compliant_ground", ], ) @@ -32,4 +33,17 @@ drake_cc_library( ], ) +drake_cc_library( + name = "multibody_plant_remodeling", + testonly = 1, + srcs = ["multibody_plant_remodeling.cc"], + hdrs = ["multibody_plant_remodeling.h"], + deps = [ + "//multibody/plant", + "//systems/analysis:simulator", + "//systems/framework:diagram_builder", + "@gtest//:without_main", + ], +) + add_lint_tests(enable_clang_format_lint = False) diff --git a/multibody/plant/test_utilities/multibody_plant_remodeling.cc b/multibody/plant/test_utilities/multibody_plant_remodeling.cc new file mode 100644 index 000000000000..fb5ed0d64410 --- /dev/null +++ b/multibody/plant/test_utilities/multibody_plant_remodeling.cc @@ -0,0 +1,53 @@ +#include "drake/multibody/plant/test_utilities/multibody_plant_remodeling.h" + +#include "drake/multibody/plant/multibody_plant_config_functions.h" +#include "drake/multibody/tree/revolute_joint.h" + +namespace drake { +namespace multibody { + +using Eigen::Vector3d; +using systems::DiagramBuilder; +using systems::Simulator; + +void MultibodyPlantRemodeling::SetUp() { + builder_ = std::make_unique>(); + MultibodyPlantConfig config = {.time_step = kTimeStep, + .discrete_contact_approximation = "sap"}; + std::tie(plant_, scene_graph_) = AddMultibodyPlant(config, builder_.get()); + + plant_->AddRigidBody("body0", SpatialInertia::MakeUnitary()); + plant_->AddRigidBody("body1", SpatialInertia::MakeUnitary()); + plant_->AddRigidBody("body2", SpatialInertia::MakeUnitary()); + plant_->AddJoint("joint0", plant_->world_body(), {}, + plant_->GetBodyByName("body0"), {}, + Vector3d::UnitZ()); + plant_->AddJoint("joint1", plant_->GetBodyByName("body0"), {}, + plant_->GetBodyByName("body1"), {}, + Vector3d::UnitZ()); + plant_->AddJoint("joint2", plant_->GetBodyByName("body1"), {}, + plant_->GetBodyByName("body2"), {}, + Vector3d::UnitZ()); + plant_->AddJointActuator("actuator0", + plant_->GetJointByName("joint0"), 1); + plant_->AddJointActuator("actuator1", + plant_->GetJointByName("joint1"), 1); + plant_->AddJointActuator("actuator2", + plant_->GetJointByName("joint2"), 1); + + // Remove an actuator in the middle of the array. + plant_->RemoveJointActuator(plant_->GetJointActuatorByName("actuator1")); +} + +void MultibodyPlantRemodeling::FinalizeAndBuild() { + plant_->Finalize(); + + diagram_ = builder_->Build(); + builder_->reset(); + + simulator_ = std::make_unique>(*diagram_); + plant_context_ = + &plant_->GetMyMutableContextFromRoot(&simulator_->get_mutable_context()); +} +} // namespace multibody +} // namespace drake diff --git a/multibody/plant/test_utilities/multibody_plant_remodeling.h b/multibody/plant/test_utilities/multibody_plant_remodeling.h new file mode 100644 index 000000000000..9c29c6c8d324 --- /dev/null +++ b/multibody/plant/test_utilities/multibody_plant_remodeling.h @@ -0,0 +1,49 @@ +#pragma once + +#include + +#include + +#include "drake/multibody/plant/multibody_plant.h" +#include "drake/systems/analysis/simulator.h" +#include "drake/systems/framework/diagram.h" +#include "drake/systems/framework/diagram_builder.h" + +namespace drake { +namespace multibody { + +// The purpose of this fixture is to unit test various MultibodyPlant remodeling +// features. It sets up a generic multibody plant with arbitrary bodies, joints, +// actuators etc. and then exercises remodeling functionality (removal, etc.) +// Users can add/remove additional elements to `plant_` and then call +// FinalizeAndBuild() to observe post Finalize() behavior. +// TODO(joemasterjohn) Update this class and documentation for removal of other +// multibody elements when support is added. +// TODO(joemasterjohn) Consider adding a test param or user functor to specify +// the model/remodeling to be done. +class MultibodyPlantRemodeling : public ::testing::Test { + public: + // This fixture sets up a plant with a serial chain of 3 bodies connected by + // revolute joints. An actuator is added to each joint. Then the actuator + // controlling `joint1` is removed. + void SetUp() override; + + // Finalize the plant, build the diagram and initialize `simulator_` + void FinalizeAndBuild(); + + protected: + // These members are only valid after SetUp(). + // `builder_` is invalid after calling FinalizeAndBuild(). + std::unique_ptr> builder_; + MultibodyPlant* plant_{nullptr}; + geometry::SceneGraph* scene_graph_{nullptr}; + + // These members are only valid after FinalizeAndBuild(). + std::unique_ptr> diagram_; + std::unique_ptr> simulator_; + systems::Context* plant_context_{nullptr}; + + const double kTimeStep{0.1}; // Discrete time step of plant_ +}; +} // namespace multibody +} // namespace drake diff --git a/multibody/tree/joint_actuator.h b/multibody/tree/joint_actuator.h index 4d49f3dbe9d5..94c7f8af0ff8 100644 --- a/multibody/tree/joint_actuator.h +++ b/multibody/tree/joint_actuator.h @@ -122,8 +122,8 @@ class JointActuator final : public MultibodyElement { /// sub-class documentation. /// @param[in,out] u /// Actuation values for the entire plant model to which `this` actuator - /// belongs to, indexed by JointActuatorIndex. Only values corresponding to - /// this actuator are changed. + /// belongs to, indexed by JointActuator::input_start(). Only values + /// corresponding to this actuator are changed. /// @throws std::exception if /// `u_actuator.size() != this->num_inputs()`. /// @throws std::exception if u is nullptr. diff --git a/multibody/tree/model_instance.h b/multibody/tree/model_instance.h index a1abb74cf4e3..08885038c0fe 100644 --- a/multibody/tree/model_instance.h +++ b/multibody/tree/model_instance.h @@ -109,8 +109,8 @@ class ModelInstance final : public MultibodyElement { std::vector GetActuatedJointIndices() const; // Returns the actuation for `this` model instance extracted from `u`. - // @param[in] u Actuation for the full plant model, indexed by - // JointActuatorIndex. + // @param[in] u Actuation for the full plant model (ordered by monotonically + // increasing JointActuatorIndex). // @returns the per model instance actuation, order by monotonically // increasing JointActuatorIndex within this model instance. // @throws std::exception if `u` is not of size @@ -126,10 +126,10 @@ class ModelInstance final : public MultibodyElement { // joints in this model instance. It is ordered by monotonically increasing // JointActuatorIndex within this model instance. // @param[in,out] u Actuation values for the entire plant model to which - // `this` actuator belongs, indexed by JointActuatorIndex. It must be of - // size equal to the number of degrees of freedom of all of the actuated - // joints in the entire MultibodyTree model. Only values corresponding to - // this model instance are updated. + // `this` actuator belongs (ordered by monotonically increasing + // JointActuatorIndex). It must be of size equal to the number of degrees of + // freedom of all of the actuated joints in the entire MultibodyTree model. + // Only values corresponding to this model instance are updated. // @throws std::exception if `u_instance` is not of size equal to the number // of degrees of freedom of all of the actuated joints in this model or `u` // is not of size equal to the number of degrees of freedom of all of the diff --git a/multibody/tree/multibody_tree.cc b/multibody/tree/multibody_tree.cc index 432aca5f8b65..afd6f90cf2d9 100644 --- a/multibody/tree/multibody_tree.cc +++ b/multibody/tree/multibody_tree.cc @@ -120,6 +120,22 @@ const RigidBody& MultibodyTree::AddRigidBody( return AddRigidBody(name, default_model_instance(), M_BBo_B); } +template +void MultibodyTree::RemoveJointActuator(const JointActuator& actuator) { + DRAKE_MBT_THROW_IF_FINALIZED(); + JointActuatorIndex actuator_index = actuator.index(); + + if (!actuators_.has_element(actuator_index)) { + throw std::logic_error( + fmt::format("RemoveJointActuator(): Actuator with index {} has " + "already been removed.", + actuator_index)); + } + + actuators_.Remove(actuator_index); + topology_.RemoveJointActuator(actuator_index); +} + template const std::string& MultibodyTree::GetModelInstanceName( ModelInstanceIndex model_instance) const { @@ -3602,7 +3618,7 @@ MatrixX MultibodyTree::MakeActuatorSelectorMatrix( MatrixX::Zero(num_actuated_dofs(), num_selected_actuators); int user_index = 0; for (JointActuatorIndex actuator_index : user_to_actuator_index_map) { - Su(int{actuator_index}, user_index) = 1.0; + Su(get_joint_actuator(actuator_index).input_start(), user_index) = 1.0; ++user_index; } @@ -3615,8 +3631,7 @@ MatrixX MultibodyTree::MakeActuatorSelectorMatrix( DRAKE_MBT_THROW_IF_NOT_FINALIZED(); std::vector joint_to_actuator_index(num_joints()); - for (JointActuatorIndex actuator_index(0); - actuator_index < num_actuators(); ++actuator_index) { + for (JointActuatorIndex actuator_index : GetJointActuatorIndices()) { const auto& actuator = get_joint_actuator(actuator_index); joint_to_actuator_index[actuator.joint().index()] = actuator_index; } @@ -3723,7 +3738,7 @@ VectorX MultibodyTree::GetEffortLowerLimits() const { DRAKE_MBT_THROW_IF_NOT_FINALIZED(); Eigen::VectorXd lower = Eigen::VectorXd::Constant( num_actuated_dofs(), -std::numeric_limits::infinity()); - for (JointActuatorIndex i{0}; i < num_actuators(); ++i) { + for (JointActuatorIndex i : GetJointActuatorIndices()) { const auto& actuator = get_joint_actuator(i); for (int j = actuator.input_start(); j < actuator.input_start() + actuator.num_inputs(); ++j) { @@ -3739,7 +3754,7 @@ VectorX MultibodyTree::GetEffortUpperLimits() const { DRAKE_MBT_THROW_IF_NOT_FINALIZED(); Eigen::VectorXd upper = Eigen::VectorXd::Constant( num_actuated_dofs(), std::numeric_limits::infinity()); - for (JointActuatorIndex i{0}; i < num_actuators(); ++i) { + for (JointActuatorIndex i : GetJointActuatorIndices()) { const auto& actuator = get_joint_actuator(i); for (int j = actuator.input_start(); j < actuator.input_start() + actuator.num_inputs(); ++j) { diff --git a/multibody/tree/multibody_tree.h b/multibody/tree/multibody_tree.h index 6659e79f286e..61e6a1e8ab11 100644 --- a/multibody/tree/multibody_tree.h +++ b/multibody/tree/multibody_tree.h @@ -462,6 +462,9 @@ class MultibodyTree { const std::string& name, const Joint& joint, double effort_limit = std::numeric_limits::infinity()); + // See MultibodyPlant documentation. + void RemoveJointActuator(const JointActuator& actuator); + // Creates a new model instance. Returns the index for a new model // instance (as there is no concrete object beyond the index). // @@ -608,6 +611,11 @@ class MultibodyTree { return joints_.get_mutable_element(joint_index); } + // See MultibodyPlant method. + bool has_joint_actuator(JointActuatorIndex actuator_index) const { + return actuators_.has_element(actuator_index); + } + // See MultibodyPlant method. const JointActuator& get_joint_actuator( JointActuatorIndex actuator_index) const { @@ -756,6 +764,11 @@ class MultibodyTree { std::vector GetJointIndices(ModelInstanceIndex model_instance) const; + // See MultibodyPlant method. + const std::vector& GetJointActuatorIndices() const { + return actuators_.indices(); + } + // See MultibodyPlant method. std::vector GetJointActuatorIndices( ModelInstanceIndex model_instance) const; @@ -2227,6 +2240,11 @@ class MultibodyTree { tree_clone->CloneJointAndAdd(*joint); } + // Fill the `actuators_` collection with nulls. This is to preserve the + // removed index structure of the ElementCollection when adding the cloned + // actuators. + tree_clone->actuators_.ResizeToMatch(actuators_); + for (const JointActuator* actuator : actuators_.elements()) { tree_clone->CloneActuatorAndAdd(*actuator); } diff --git a/multibody/tree/multibody_tree_system.cc b/multibody/tree/multibody_tree_system.cc index 658316753f40..1cee719a4cba 100644 --- a/multibody/tree/multibody_tree_system.cc +++ b/multibody/tree/multibody_tree_system.cc @@ -102,8 +102,8 @@ void MultibodyTreeSystem::SetDefaultParameters( .SetDefaultParameters(parameters); } // JointActuators. - for (JointActuatorIndex joint_actuator_index(0); - joint_actuator_index < tree_->num_actuators(); ++joint_actuator_index) { + for (JointActuatorIndex joint_actuator_index : + tree_->GetJointActuatorIndices()) { internal_tree() .get_joint_actuator(joint_actuator_index) .SetDefaultParameters(parameters); @@ -156,8 +156,8 @@ void MultibodyTreeSystem::DeclareMultibodyElementParameters() { mutable_tree().get_mutable_joint(joint_index).DeclareParameters(this); } // JointActuators. - for (JointActuatorIndex joint_actuator_index(0); - joint_actuator_index < tree_->num_actuators(); ++joint_actuator_index) { + for (JointActuatorIndex joint_actuator_index : + tree_->GetJointActuatorIndices()) { mutable_tree() .get_mutable_joint_actuator(joint_actuator_index) .DeclareParameters(this); diff --git a/multibody/tree/multibody_tree_topology.h b/multibody/tree/multibody_tree_topology.h index 792457d0f85a..a444138b880a 100644 --- a/multibody/tree/multibody_tree_topology.h +++ b/multibody/tree/multibody_tree_topology.h @@ -20,6 +20,7 @@ /// key into the Context for that element's state. #include +#include #include #include #include @@ -251,19 +252,6 @@ struct MobilizerTopology { // Data structure to store the topological information associated with a // JointActuator. struct JointActuatorTopology { - DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(JointActuatorTopology); - - // Default construction to an invalid configuration. - JointActuatorTopology() {} - - // Constructs a joint actuator topology with index `joint_actuator_index`. - JointActuatorTopology( - JointActuatorIndex joint_actuator_index, - int start_index, int ndofs) : - index(joint_actuator_index), - actuator_index_start(start_index), - num_dofs(ndofs) {} - // Returns `true` if all members of `this` topology are exactly equal to the // members of `other`. bool operator==(const JointActuatorTopology& other) const { @@ -450,11 +438,6 @@ class MultibodyTreeTopology { return ssize(body_nodes_); } - // Returns the number of joint actuators in the topology. - int num_joint_actuators() const { - return ssize(joint_actuators_); - } - // Returns the number of levels in the forest topology. int forest_height() const { return forest_height_; @@ -491,8 +474,9 @@ class MultibodyTreeTopology { // given a JointActuatorIndex. const JointActuatorTopology& get_joint_actuator( JointActuatorIndex index) const { - DRAKE_ASSERT(index < num_joint_actuators()); - return joint_actuators_[index]; + DRAKE_ASSERT(index < ssize(joint_actuators_)); + DRAKE_DEMAND(joint_actuators_[index].has_value()); + return *joint_actuators_[index]; } // Returns a constant reference to the corresponding BodyNodeTopology given @@ -697,13 +681,43 @@ class MultibodyTreeTopology { "See documentation for Finalize() for details."); } const int actuator_index_start = num_actuated_dofs(); - const JointActuatorIndex actuator_index(num_joint_actuators()); - joint_actuators_.emplace_back( - actuator_index, actuator_index_start, num_dofs); + const JointActuatorIndex actuator_index(ssize(joint_actuators_)); + joint_actuators_.push_back( + JointActuatorTopology{.index = actuator_index, + .actuator_index_start = actuator_index_start, + .num_dofs = num_dofs}); num_actuated_dofs_ += num_dofs; return actuator_index; } + // Removes `actuator_index` from the list of joint actuators. The + // `actuator_index_start` will be modified if necessary for other actuators. + // @throws std::exception if called post-Finalize. + // @throws std::exception if the actuator with the index `actuator_index` has + // already been removed. + void RemoveJointActuator(JointActuatorIndex actuator_index) { + DRAKE_DEMAND(actuator_index < ssize(joint_actuators_)); + DRAKE_THROW_UNLESS(!is_valid()); + DRAKE_THROW_UNLESS(joint_actuators_[actuator_index].has_value()); + + // Reduce the total number of actuated dofs. + const int num_dofs = (*joint_actuators_[actuator_index]).num_dofs; + DRAKE_ASSERT(num_actuated_dofs_ >= num_dofs); + num_actuated_dofs_ -= num_dofs; + + // Mark the actuator as "removed". + joint_actuators_[actuator_index] = std::nullopt; + + // Update the actuator_index_start for all joint actuators that come after + // the one we just removed. + for (JointActuatorIndex i(actuator_index); i < ssize(joint_actuators_); + ++i) { + if (joint_actuators_[i].has_value()) { + (*joint_actuators_[i]).actuator_index_start -= num_dofs; + } + } + } + // This method must be called by MultibodyTree::Finalize() after all // topological elements in the plant (rigid bodies, joints, constraints) were // added and before any computations are performed. @@ -1175,7 +1189,7 @@ class MultibodyTreeTopology { std::vector frames_; std::vector rigid_bodies_; std::vector mobilizers_; - std::vector joint_actuators_; + std::vector> joint_actuators_; std::vector body_nodes_; // Total number of generalized positions and velocities in the MultibodyTree diff --git a/multibody/tree/test/joint_actuator_test.cc b/multibody/tree/test/joint_actuator_test.cc index 45ec2ef22f0d..7ee0764b93e6 100644 --- a/multibody/tree/test/joint_actuator_test.cc +++ b/multibody/tree/test/joint_actuator_test.cc @@ -2,11 +2,13 @@ #include +#include #include #include "drake/common/eigen_types.h" #include "drake/common/test_utilities/expect_throws_message.h" #include "drake/multibody/tree/multibody_tree-inl.h" +#include "drake/multibody/tree/multibody_tree_indexes.h" #include "drake/multibody/tree/planar_joint.h" #include "drake/multibody/tree/prismatic_joint.h" #include "drake/multibody/tree/rigid_body.h" @@ -95,6 +97,150 @@ GTEST_TEST(JointActuatorTest, JointActuatorLimitTest) { EXPECT_EQ(actuator1.num_inputs(), 1); EXPECT_EQ(actuator4.input_start(), 1); EXPECT_EQ(actuator4.num_inputs(), 3); + + EXPECT_THAT(tree.GetJointActuatorIndices(), + testing::ElementsAre(actuator1.index(), actuator4.index())); +} + +GTEST_TEST(JointActuatorTest, RemoveJointActuatorTest) { + auto tree_pointer = std::make_unique>(); + internal::MultibodyTree& tree = *tree_pointer; + + // Spatial inertia for adding body. The actual value is not important for + // these tests and therefore we do not initialize it. + const SpatialInertia M_B; // Default construction is ok for this. + + // Add model instances. + const auto model_instance1 = tree.AddModelInstance("instance1"); + const auto model_instance2 = tree.AddModelInstance("instance2"); + + // Add bodies so we can add joints to them. + const auto body1 = &tree.AddRigidBody("body1", model_instance1, M_B); + const auto body2 = &tree.AddRigidBody("body2", model_instance1, M_B); + const auto body3 = &tree.AddRigidBody("body3", model_instance1, M_B); + const auto body4 = &tree.AddRigidBody("body4", model_instance2, M_B); + + // Add a prismatic joint between the world and body1: + const Joint& body1_world = + tree.AddJoint(std::make_unique>( + "prism1", tree.world_body().body_frame(), body1->body_frame(), + Eigen::Vector3d(0, 0, 1))); + const JointActuator& actuator1 = + tree.AddJointActuator("act1", body1_world, kPositiveEffortLimit); + + const Joint& body2_body1 = + tree.AddJoint(std::make_unique>( + "prism2", body1->body_frame(), body2->body_frame(), + Eigen::Vector3d(0, 0, 1))); + const JointActuator& actuator2 = + tree.AddJointActuator("act2", body2_body1, kPositiveEffortLimit); + + const Joint& body3_body2 = + tree.AddJoint(std::make_unique>( + "prism3", body2->body_frame(), body3->body_frame(), + Eigen::Vector3d(0, 0, 1))); + const JointActuator& actuator3 = + tree.AddJointActuator("act3", body3_body2, kPositiveEffortLimit); + + const Joint& body4_world = + tree.AddJoint(std::make_unique>( + "prism4", tree.world_body().body_frame(), body4->body_frame(), + Eigen::Vector3d(0, 0, 1))); + const JointActuator& actuator4 = + tree.AddJointActuator("act4", body4_world, kPositiveEffortLimit); + + JointActuatorIndex actuator1_index = actuator1.index(); + JointActuatorIndex actuator2_index = actuator2.index(); + JointActuatorIndex actuator3_index = actuator3.index(); + JointActuatorIndex actuator4_index = actuator4.index(); + + EXPECT_EQ(tree.num_actuators(), 4); + EXPECT_EQ(tree.num_actuated_dofs(), 4); + EXPECT_THAT(tree.GetJointActuatorIndices(), + testing::ElementsAre(actuator1_index, actuator2_index, + actuator3_index, actuator4_index)); + + // Remove the first actuator. + tree.RemoveJointActuator(actuator1); + EXPECT_EQ(tree.num_actuators(), 3); + EXPECT_EQ(tree.num_actuated_dofs(), 3); + EXPECT_FALSE(tree.has_joint_actuator(actuator1_index)); + EXPECT_THAT( + tree.GetJointActuatorIndices(), + testing::ElementsAre(actuator2_index, actuator3_index, actuator4_index)); + + // Replace the third actuator. + tree.RemoveJointActuator(actuator3); + EXPECT_EQ(tree.num_actuators(), 2); + EXPECT_EQ(tree.num_actuated_dofs(), 2); + EXPECT_FALSE(tree.has_joint_actuator(actuator3_index)); + EXPECT_THAT(tree.GetJointActuatorIndices(), + testing::ElementsAre(actuator2_index, actuator4_index)); + EXPECT_FALSE(tree.HasJointActuatorNamed("act3")); + EXPECT_FALSE(tree.HasJointActuatorNamed("act3", model_instance1)); + + // Add a new actuator with the same name as the third. + const JointActuator& new_actuator3 = + tree.AddJointActuator("act3", body3_body2, 2 * kPositiveEffortLimit); + JointActuatorIndex new_actuator3_index = new_actuator3.index(); + EXPECT_EQ(tree.num_actuators(), 3); + EXPECT_EQ(tree.num_actuated_dofs(), 3); + EXPECT_TRUE(tree.has_joint_actuator(new_actuator3_index)); + EXPECT_THAT(tree.GetJointActuatorIndices(), + testing::ElementsAre(actuator2_index, actuator4_index, + new_actuator3_index)); + EXPECT_TRUE(tree.HasJointActuatorNamed("act3")); + EXPECT_TRUE(tree.HasJointActuatorNamed("act3", model_instance1)); + EXPECT_EQ(tree.GetJointActuatorByName("act3").index(), new_actuator3.index()); + EXPECT_EQ(tree.GetJointActuatorByName("act3", model_instance1).index(), + new_actuator3.index()); + + tree.Finalize(); + + EXPECT_EQ(actuator2.input_start(), 0); + EXPECT_EQ(actuator4.input_start(), 1); + EXPECT_EQ(new_actuator3.input_start(), 2); + EXPECT_EQ(actuator2.num_inputs(), 1); + EXPECT_EQ(actuator4.num_inputs(), 1); + EXPECT_EQ(new_actuator3.num_inputs(), 1); + EXPECT_EQ(tree.num_actuators(model_instance1), 2); + EXPECT_EQ(tree.num_actuated_dofs(model_instance1), 2); + EXPECT_EQ(tree.num_actuators(model_instance2), 1); + EXPECT_EQ(tree.num_actuated_dofs(model_instance2), 1); + EXPECT_THAT(tree.GetJointActuatorIndices(model_instance1), + testing::ElementsAre(actuator2_index, new_actuator3_index)); + + DRAKE_EXPECT_THROWS_MESSAGE( + tree.RemoveJointActuator(actuator2), + "Post-finalize calls to 'RemoveJointActuator.*' are not allowed.*"); + + // Confirm that removed joint logic is preserved after cloning. + std::unique_ptr> clone = + tree.CloneToScalar(); + EXPECT_EQ(clone->num_actuators(), 3); + EXPECT_EQ(clone->num_actuated_dofs(), 3); + EXPECT_TRUE(clone->has_joint_actuator(new_actuator3_index)); + EXPECT_THAT(clone->GetJointActuatorIndices(), + testing::ElementsAre(actuator2_index, actuator4_index, + new_actuator3_index)); + EXPECT_TRUE(clone->HasJointActuatorNamed("act3")); + EXPECT_TRUE(clone->HasJointActuatorNamed("act3", model_instance1)); + EXPECT_EQ(clone->GetJointActuatorByName("act3").index(), + new_actuator3.index()); + EXPECT_EQ(clone->GetJointActuatorByName("act3", model_instance1).index(), + new_actuator3.index()); + EXPECT_EQ(actuator2.input_start(), 0); + EXPECT_EQ(actuator4.input_start(), 1); + EXPECT_EQ(new_actuator3.input_start(), 2); + EXPECT_EQ(actuator2.num_inputs(), 1); + EXPECT_EQ(actuator4.num_inputs(), 1); + EXPECT_EQ(new_actuator3.num_inputs(), 1); + EXPECT_EQ(clone->num_actuators(model_instance1), 2); + EXPECT_EQ(clone->num_actuated_dofs(model_instance1), 2); + EXPECT_EQ(clone->num_actuators(model_instance2), 1); + EXPECT_EQ(clone->num_actuated_dofs(model_instance2), 1); + EXPECT_THAT(clone->GetJointActuatorIndices(model_instance1), + testing::ElementsAre(actuator2_index, new_actuator3_index)); } } // namespace