Adds MultibodyPlant::MakeActuationMatrixPseudoinverse (RobotLocomotio…

…n#20975)

bindings/pydrake/multibody/plant_py.cc

@@ -655,6 +655,9 @@ void DoScalarDependentDefinitions(py::module m, T) {
             cls_doc.CalcGeneralizedForces.doc)
         .def("MakeActuationMatrix", &Class::MakeActuationMatrix,
             cls_doc.MakeActuationMatrix.doc)
+        .def("MakeActuationMatrixPseudoinverse",
+            &Class::MakeActuationMatrixPseudoinverse,
+            cls_doc.MakeActuationMatrixPseudoinverse.doc)
         .def(
             "MakeActuatorSelectorMatrix",
             [](const Class* self, const std::vector<JointActuatorIndex>&

bindings/pydrake/multibody/test/plant_test.py

@@ -2205,6 +2205,24 @@ def loop_body(make_joint, time_step):
                 loop_body(make_joint, 0.0)
                 loop_body(make_joint, 0.001)
 
+    def test_actuation_matrix(self):
+        iiwa_sdf_path = FindResourceOrThrow(
+            "drake/manipulation/models/"
+            "iiwa_description/sdf/iiwa14_no_collision.sdf")
+
+        plant = MultibodyPlant_[float](0.0)
+        parser = Parser(plant)
+        iiwa_model, = parser.AddModels(file_name=iiwa_sdf_path)
+        plant.WeldFrames(
+            frame_on_parent_F=plant.world_frame(),
+            frame_on_child_M=plant.GetFrameByName("iiwa_link_0", iiwa_model))
+        plant.Finalize()
+
+        B = plant.MakeActuationMatrix()
+        np.testing.assert_array_equal(B, np.eye(7))
+        B_inv = plant.MakeActuationMatrixPseudoinverse()
+        np.testing.assert_array_equal(B_inv.todense(), np.eye(7))
+
     def test_deprecated_weld_joint_api(self):
         plant = MultibodyPlant_[float](0.01)
         body1 = plant.AddRigidBody(

multibody/plant/multibody_plant.cc

@@ -1296,6 +1296,27 @@ MatrixX<T> MultibodyPlant<T>::MakeActuationMatrix() const {
   return B;
 }
 
+template <typename T>
+Eigen::SparseMatrix<double>
+MultibodyPlant<T>::MakeActuationMatrixPseudoinverse() const {
+  // We leverage here the assumption that B (the actuation matrix) is a
+  // permutation matrix, so Bᵀ is the pseudoinverse.
+  std::vector<Eigen::Triplet<double>> triplets;
+  for (JointActuatorIndex actuator_index(0); actuator_index < num_actuators();
+       ++actuator_index) {
+    const JointActuator<T>& actuator = get_joint_actuator(actuator_index);
+    // This method assumes actuators on single dof joints. Assert this
+    // condition.
+    DRAKE_DEMAND(actuator.joint().num_velocities() == 1);
+    triplets.push_back(Eigen::Triplet<double>(
+        int{actuator.index()}, actuator.joint().velocity_start(), 1.0));
+  }
+
+  Eigen::SparseMatrix<double> Bplus(num_actuated_dofs(), num_velocities());
+  Bplus.setFromTriplets(triplets.begin(), triplets.end());
+  return Bplus;
+}
+
 namespace {
 
 void ThrowForDisconnectedGeometryPort(std::string_view explanation) {

multibody/plant/multibody_plant.h

@@ -4399,6 +4399,13 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
   /// for very large systems.
   MatrixX<T> MakeActuationMatrix() const;
 
+  /// Creates the pseudoinverse of the actuation matrix B directly (without
+  /// requiring an explicit inverse calculation). See MakeActuationMatrix().
+  ///
+  /// Notably, when B is full row rank (the system is fully actuated), then the
+  /// pseudoinverse is a true inverse.
+  Eigen::SparseMatrix<double> MakeActuationMatrixPseudoinverse() const;
+
   /// 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

multibody/plant/test/multibody_plant_test.cc

@@ -2774,6 +2774,31 @@ GTEST_TEST(KukaModel, JointIndexes) {
   EXPECT_EQ(xc, xc_expected);
 }
 
+GTEST_TEST(KukaModel, ActuationMatrix) {
+  const char kSdfPath[] =
+      "drake/manipulation/models/iiwa_description/sdf/"
+      "iiwa14_no_collision.sdf";
+
+  MultibodyPlant<double> plant(0.0);
+  Parser(&plant).AddModels(FindResourceOrThrow(kSdfPath));
+  plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("iiwa_link_0"));
+  plant.Finalize();
+
+  EXPECT_EQ(plant.num_positions(), 7);
+  EXPECT_EQ(plant.num_velocities(), 7);
+  EXPECT_EQ(plant.num_actuated_dofs(), 7);
+
+  const Eigen::MatrixXd B = plant.MakeActuationMatrix();
+  EXPECT_EQ(B.rows(), plant.num_velocities());
+  EXPECT_EQ(B.cols(), plant.num_actuated_dofs());
+  const Eigen::SparseMatrix<double> B_inv =
+      plant.MakeActuationMatrixPseudoinverse();
+  EXPECT_EQ(B_inv.rows(), plant.num_actuated_dofs());
+  EXPECT_EQ(B_inv.cols(), plant.num_velocities());
+  EXPECT_TRUE((B * B_inv).isIdentity());
+  EXPECT_TRUE((B_inv * B).isIdentity());
+}
+
 // 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.
@@ -3258,6 +3283,14 @@ GTEST_TEST(StateSelection, KukaWithSimpleGripper) {
   const MatrixX<double> B = plant.MakeActuationMatrix();
   ASSERT_EQ(B.rows(), nv);
   ASSERT_EQ(B.cols(), nu);
+  // B is not invertible.
+  const Eigen::SparseMatrix<double> Bplus =
+      plant.MakeActuationMatrixPseudoinverse();
+  EXPECT_EQ(Bplus.rows(), nu);
+  EXPECT_EQ(Bplus.cols(), nv);
+  // since nv >= nu, it's a true left inverse.
+  EXPECT_TRUE((Bplus * B).isIdentity());
+
   MatrixX<double> B_expected = MatrixX<double>::Zero(nv, nu);
 
   // Fill in the block for the IIWA's actuators.