Implements AddMultibodyPlantConstraints()

This method creates a mapping from the constraints used in
MultibodyPlant/SAP to the constraints used in MathematicalProgram.

Towards RobotLocomotion#18917.

bindings/pydrake/multibody/inverse_kinematics_py.cc

@@ -5,6 +5,7 @@
 #include "drake/bindings/pydrake/common/sorted_pair_pybind.h"
 #include "drake/bindings/pydrake/documentation_pybind.h"
 #include "drake/bindings/pydrake/pydrake_pybind.h"
+#include "drake/multibody/inverse_kinematics/add_multibody_plant_constraints.h"
 #include "drake/multibody/inverse_kinematics/angle_between_vectors_constraint.h"
 #include "drake/multibody/inverse_kinematics/angle_between_vectors_cost.h"
 #include "drake/multibody/inverse_kinematics/com_in_polyhedron_constraint.h"
@@ -165,6 +166,11 @@ PYBIND11_MODULE(inverse_kinematics, m) {
         .def("get_mutable_context", &Class::get_mutable_context,
             py_rvp::reference_internal, cls_doc.get_mutable_context.doc);
   }
+
+  m.def("AddMultibodyPlantConstraints", &AddMultibodyPlantConstraints,
+      py::arg("plant"), py::arg("q"), py::arg("prog"), py::arg("plant_context"),
+      doc.AddMultibodyPlantConstraints.doc);
+
   {
     using Class = AngleBetweenVectorsConstraint;
     constexpr auto& cls_doc = doc.AngleBetweenVectorsConstraint;

bindings/pydrake/multibody/test/inverse_kinematics_test.py

@@ -81,6 +81,14 @@ def _body2_xyz(self, q):
         # TODO(eric.cousineau): Replace with state indexing.
         return q[11:14]
 
+    def test_AddMultibodyPlantConstraints(self):
+        bindings = ik.AddMultibodyPlantConstraints(
+            plant=self.plant,
+            q=self.q,
+            prog=self.prog,
+            plant_context=self.ik_two_bodies.context())
+        self.assertEqual(len(bindings), 2)
+
     def test_AddPositionConstraint1(self):
         p_BQ = np.array([0.2, 0.3, 0.5])
         p_AQ_lower = np.array([-0.1, -0.2, -0.3])

multibody/inverse_kinematics/BUILD.bazel

@@ -15,6 +15,7 @@ drake_cc_package_library(
     name = "inverse_kinematics",
     visibility = ["//visibility:public"],
     deps = [
+        ":add_multibody_plant_constraints",
         ":constraint_relaxing_ik",
         ":differential_inverse_kinematics",
         ":differential_inverse_kinematics_integrator",
@@ -24,6 +25,17 @@ drake_cc_package_library(
     ],
 )
 
+drake_cc_library(
+    name = "add_multibody_plant_constraints",
+    srcs = ["add_multibody_plant_constraints.cc"],
+    hdrs = ["add_multibody_plant_constraints.h"],
+    deps = [
+        ":kinematic_evaluators",
+        "//multibody/plant",
+        "//solvers:mathematical_program",
+    ],
+)
+
 drake_cc_library(
     name = "constraint_relaxing_ik",
     srcs = ["constraint_relaxing_ik.cc"],
@@ -150,6 +162,15 @@ drake_cc_library(
 
 #============ Test ============================
 
+drake_cc_googletest(
+    name = "add_multibody_plant_constraints_test",
+    srcs = ["test/add_multibody_plant_constraints_test.cc"],
+    deps = [
+        ":add_multibody_plant_constraints",
+        "//solvers:solve",
+    ],
+)
+
 drake_cc_googletest(
     name = "constraint_relaxing_ik_test",
     srcs = ["test/constraint_relaxing_ik_test.cc"],

multibody/inverse_kinematics/add_multibody_plant_constraints.cc

@@ -0,0 +1,86 @@
+#include "drake/multibody/inverse_kinematics/add_multibody_plant_constraints.h"
+
+#include <memory>
+
+#include "drake/multibody/inverse_kinematics/orientation_constraint.h"
+#include "drake/multibody/inverse_kinematics/point_to_point_distance_constraint.h"
+#include "drake/multibody/inverse_kinematics/position_constraint.h"
+#include "drake/multibody/inverse_kinematics/unit_quaternion_constraint.h"
+
+namespace drake {
+namespace multibody {
+
+using solvers::Binding;
+using solvers::Constraint;
+
+std::vector<Binding<Constraint>> AddMultibodyPlantConstraints(
+    const MultibodyPlant<double>& plant,
+    const solvers::VectorXDecisionVariable& q,
+    solvers::MathematicalProgram* prog,
+    systems::Context<double>* plant_context) {
+  DRAKE_THROW_UNLESS(prog != nullptr);
+  std::vector<Binding<Constraint>> bindings =
+      AddUnitQuaternionConstraintOnPlant(plant, q, prog);
+  int num_multibody_constraints = 0;
+  for (const auto& [id, spec] : plant.get_coupler_constraint_specs()) {
+    bindings.emplace_back(prog->AddLinearEqualityConstraint(
+        q[spec.joint0_index] ==
+        spec.gear_ratio * q[spec.joint1_index] + spec.offset));
+    ++num_multibody_constraints;
+  }
+  for (const auto& [id, spec] : plant.get_distance_constraint_specs()) {
+    DRAKE_THROW_UNLESS(plant_context != nullptr);
+    // d(q) == d₀.
+    bindings.emplace_back(prog->AddConstraint(
+        std::make_shared<PointToPointDistanceConstraint>(
+            &plant, plant.get_body(spec.body_A).body_frame(), spec.p_AP,
+            plant.get_body(spec.body_B).body_frame(), spec.p_BQ, spec.distance,
+            spec.distance, plant_context),
+        q));
+    ++num_multibody_constraints;
+  }
+  for (const auto& [id, spec] : plant.get_ball_constraint_specs()) {
+    DRAKE_THROW_UNLESS(plant_context != nullptr);
+    bindings.emplace_back(prog->AddConstraint(
+        std::make_shared<PositionConstraint>(
+            &plant, plant.get_body(spec.body_A).body_frame(), spec.p_AP,
+            spec.p_AP, plant.get_body(spec.body_B).body_frame(), *spec.p_BQ,
+            plant_context),
+        q));
+    ++num_multibody_constraints;
+  }
+  for (const auto& [id, spec] : plant.get_weld_constraint_specs()) {
+    DRAKE_THROW_UNLESS(plant_context != nullptr);
+    // TODO(russt): Consider implementing a WeldConstraint.
+    bindings.emplace_back(prog->AddConstraint(
+        std::make_shared<PositionConstraint>(
+            &plant, plant.get_body(spec.body_A).body_frame(),
+            spec.X_AP.translation(), spec.X_AP.translation(),
+            plant.get_body(spec.body_B).body_frame(), spec.X_BQ.translation(),
+            plant_context),
+        q));
+    bindings.emplace_back(prog->AddConstraint(
+        std::make_shared<OrientationConstraint>(
+            &plant, plant.get_body(spec.body_A).body_frame(),
+            spec.X_AP.rotation(), plant.get_body(spec.body_B).body_frame(),
+            spec.X_BQ.rotation(), 0.0, plant_context),
+        q));
+    ++num_multibody_constraints;
+  }
+  if (num_multibody_constraints != plant.num_constraints()) {
+    // TODO(russt): It would be better to say something specific about the
+    // unidentified constraint here, but the constraint API for MultibodyPlant
+    // does not have that surface area (yet).
+    log()->warn(
+        "plant.num_constraints() = {}, but only AddMultibodyPlantConstraints() "
+        "only captured {} of them. This method currently supports Coupler, "
+        "Distance, Ball, and Weld multibody constraints. If you're seeing "
+        "this, MultibodyPlant may have gained a new constraint type that is "
+        "not supported here yet (but perhaps should be)",
+        plant.num_constraints(), num_multibody_constraints);
+  }
+  return bindings;
+}
+
+}  // namespace multibody
+}  // namespace drake

multibody/inverse_kinematics/add_multibody_plant_constraints.h

@@ -0,0 +1,34 @@
+#pragma once
+
+#include <vector>
+
+#include "drake/multibody/plant/multibody_plant.h"
+#include "drake/solvers/mathematical_program.h"
+
+namespace drake {
+namespace multibody {
+
+/** For unit quaternion and (holonomic) constraints registered with `plant` adds
+a corresponding solver::Constraint to `prog`, using decision variables `q` to
+represent the generalized positions of the plant.
+
+Adds constraints for coupler, distance, ball, and weld constraints. The
+distance constraint is implemented here as a hard kinematic constraint (i.e.,
+d(q) == d₀), instead of a soft "spring" force.
+
+@see AddUnitQuaternionConstraintOnPlant() for the unit quaternion constraints.
+
+@pre plant.is_finalized() == true.
+@throws std::exception if `prog` is nullptr.
+@throws std::exception if `plant_context` is nullptr and one of the
+MultibodyPlant constraints requires it. (unit quaternion constraints and coupler
+constraints do not).
+*/
+std::vector<solvers::Binding<solvers::Constraint>> AddMultibodyPlantConstraints(
+    const MultibodyPlant<double>& plant,
+    const solvers::VectorXDecisionVariable& q,
+    solvers::MathematicalProgram* prog,
+    systems::Context<double>* plant_context = nullptr);
+
+}  // namespace multibody
+}  // namespace drake

multibody/inverse_kinematics/test/add_multibody_plant_constraints_test.cc

@@ -0,0 +1,114 @@
+#include "drake/multibody/inverse_kinematics/add_multibody_plant_constraints.h"
+
+#include <gtest/gtest.h>
+
+#include "drake/multibody/tree/revolute_joint.h"
+#include "drake/solvers/solve.h"
+
+namespace drake {
+namespace multibody {
+namespace {
+
+using Eigen::MatrixXd;
+using Eigen::Vector3d;
+using Eigen::VectorXd;
+using math::RigidTransformd;
+
+class AddMultibodyPlantConstraintsTest : public ::testing::Test {
+ public:
+  void SetUp() override {
+    plant_.set_discrete_contact_approximation(
+        DiscreteContactApproximation::kSap);
+    plant_.SetUseSampledOutputPorts(false);
+
+    body_A_ = &plant_.AddRigidBody(
+        "body_A", SpatialInertia<double>::SolidBoxWithMass(1, 1, 1, 1));
+    body_B_ = &plant_.AddRigidBody(
+        "body_B", SpatialInertia<double>::SolidBoxWithMass(1, 1, 1, 1));
+    world_A_ = &plant_.AddJoint<RevoluteJoint>(
+        "world_A", plant_.world_body(), RigidTransformd(Vector3d(-1, 0, 0)),
+        *body_A_, RigidTransformd(), Vector3d::UnitZ());
+    A_B_ = &plant_.AddJoint<RevoluteJoint>(
+        "A_B", *body_A_, RigidTransformd(Vector3d(1, 0, 0)), *body_B_,
+        RigidTransformd(), Vector3d::UnitZ());
+  }
+
+  void CheckConstraints(int expected_num_constraints = 1, double tol = 1e-6) {
+    if (!plant_.is_finalized()) {
+      plant_.Finalize();
+    }
+    plant_context_ = plant_.CreateDefaultContext();
+
+    // First demonstrate that these constraints work with MathematicalProgram,
+    // and use them to find a valid initial condition.
+    solvers::MathematicalProgram prog;
+    auto q = prog.NewContinuousVariables(plant_.num_positions());
+    VectorXd q0 = VectorXd::LinSpaced(plant_.num_positions(), 0, 1);
+    prog.AddQuadraticErrorCost(
+        MatrixXd::Identity(plant_.num_positions(), plant_.num_positions()), q0,
+        q);
+
+    auto constraints =
+        AddMultibodyPlantConstraints(plant_, q, &prog, plant_context_.get());
+    EXPECT_EQ(constraints.size(), expected_num_constraints);
+    auto result = solvers::Solve(prog);
+    EXPECT_TRUE(result.is_success());
+
+    // Now step the dynamics forward and verify that the constraint stays
+    // satisfied.
+    plant_.SetPositions(plant_context_.get(), result.GetSolution(q));
+    VectorXd qn = plant_.EvalUniquePeriodicDiscreteUpdate(*plant_context_)
+                      .value()
+                      .head(plant_.num_positions());
+
+    prog.SetInitialGuessForAllVariables(qn);
+    EXPECT_TRUE(prog.CheckSatisfiedAtInitialGuess(constraints, tol));
+
+    result.set_x_val(qn);
+    log()->info(
+        "Infeasible constraints:\n{}",
+        fmt::join(result.GetInfeasibleConstraintNames(prog, tol), "\n"));
+  }
+
+ protected:
+  MultibodyPlant<double> plant_{0.1};
+  std::unique_ptr<systems::Context<double>> plant_context_{};
+  const RigidBody<double>* body_A_{nullptr};
+  const RigidBody<double>* body_B_{nullptr};
+  const RevoluteJoint<double>* world_A_{nullptr};
+  const RevoluteJoint<double>* A_B_{nullptr};
+};
+
+TEST_F(AddMultibodyPlantConstraintsTest, CouplerConstraint) {
+  plant_.AddCouplerConstraint(*world_A_, *A_B_, 2.3);
+  CheckConstraints();
+
+  // Confirm that I also could have called the method with plant_context ==
+  // nullptr.
+  solvers::MathematicalProgram prog;
+  auto q = prog.NewContinuousVariables(plant_.num_positions());
+  EXPECT_NO_THROW(AddMultibodyPlantConstraints(plant_, q, &prog));
+}
+
+TEST_F(AddMultibodyPlantConstraintsTest, DistanceConstraint) {
+  plant_.AddDistanceConstraint(*body_A_, Vector3d(0.0, 1.0, 0.0), *body_B_,
+                               Vector3d(0.0, 1.0, 0.0), 1.5);
+  CheckConstraints();
+}
+
+TEST_F(AddMultibodyPlantConstraintsTest, BallConstraint) {
+  plant_.AddBallConstraint(*body_A_, Vector3d(0.0, 2.0, 0.0), *body_B_);
+  CheckConstraints();
+}
+
+TEST_F(AddMultibodyPlantConstraintsTest, WeldConstraint) {
+  const RigidBody<double>& body_C = plant_.AddRigidBody(
+      "body_C", SpatialInertia<double>::SolidBoxWithMass(1, 1, 1, 1));
+  plant_.AddWeldConstraint(*body_A_, RigidTransformd(Vector3d(1, 2, 3)), body_C,
+                           RigidTransformd(Vector3d(4, 5, 6)));
+  CheckConstraints(/* expected_num_constraints = */ 3, /* tol = */ 1e-2);
+}
+
+}  // namespace
+}  // namespace multibody
+}  // namespace drake

multibody/inverse_kinematics/test/unit_quaternion_constraint_test.cc

@@ -50,7 +50,8 @@ TEST_F(TwoFreeBodiesConstraintTest, AddUnitQuaternionConstraintOnPlant) {
   prog.SetInitialGuess(
       q.segment<4>(plant_->GetBodyByName("body1").floating_positions_start()),
       body1_guess);
-  AddUnitQuaternionConstraintOnPlant(*plant_, q, &prog);
+  auto bindings = AddUnitQuaternionConstraintOnPlant(*plant_, q, &prog);
+  EXPECT_EQ(bindings.size(), 2);
   EXPECT_EQ(prog.generic_constraints().size(), 2);
   // Confirm that body 1's non-default initial guess was not overwritten.
   EXPECT_TRUE(CompareMatrices(

multibody/inverse_kinematics/unit_quaternion_constraint.cc

@@ -24,24 +24,28 @@ void UnitQuaternionConstraint::DoEval(
 }
 
 template <typename T>
-void AddUnitQuaternionConstraintOnPlant(
+std::vector<solvers::Binding<solvers::Constraint>>
+AddUnitQuaternionConstraintOnPlant(
     const MultibodyPlant<T>& plant,
     const Eigen::Ref<const VectorX<symbolic::Variable>>& q_vars,
     solvers::MathematicalProgram* prog) {
   DRAKE_DEMAND(q_vars.rows() == plant.num_positions());
+  std::vector<solvers::Binding<solvers::Constraint>> bindings;
   // Loop through each body
   for (BodyIndex body_index{0}; body_index < plant.num_bodies(); ++body_index) {
     const auto& body = plant.get_body(body_index);
     if (body.has_quaternion_dofs()) {
       Vector4<symbolic::Variable> quat_vars =
           q_vars.segment<4>(body.floating_positions_start());
-      prog->AddConstraint(solvers::Binding<solvers::Constraint>(
-          std::make_shared<UnitQuaternionConstraint>(), quat_vars));
+      bindings.emplace_back(
+          prog->AddConstraint(solvers::Binding<solvers::Constraint>(
+              std::make_shared<UnitQuaternionConstraint>(), quat_vars)));
       if (prog->GetInitialGuess(quat_vars).array().isNaN().all()) {
         prog->SetInitialGuess(quat_vars, Eigen::Vector4d(1, 0, 0, 0));
       }
     }
   }
+  return bindings;
 }
 
 DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS((

multibody/inverse_kinematics/unit_quaternion_constraint.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <memory>
+#include <vector>
 
 #include "drake/multibody/plant/multibody_plant.h"
 #include "drake/solvers/constraint.h"
@@ -63,7 +64,8 @@ class UnitQuaternionConstraint : public solvers::Constraint {
  @tparam_default_scalar
  */
 template <typename T>
-void AddUnitQuaternionConstraintOnPlant(
+std::vector<solvers::Binding<solvers::Constraint>>
+AddUnitQuaternionConstraintOnPlant(
     const MultibodyPlant<T>& plant,
     const Eigen::Ref<const VectorX<symbolic::Variable>>& q_vars,
     solvers::MathematicalProgram* prog);

multibody/plant/constraint_specs.h

@@ -37,8 +37,10 @@ struct CouplerConstraintSpec {
 
 // Struct to store the specification for a distance constraint. A distance
 // constraint is modeled as a holonomic constraint. Distance constraints can
-// be "soft" which imposes the the condition:
-//   (d(q)-d₀) + c/k⋅ḋ(q) + 1/k⋅f = 0
+// be "soft", and are implemented as a spring force, f:
+//   f = -k⋅(d(q) - d₀) - c⋅ḋ(q),
+// which is implemented as the condition:
+//   (d(q)-d₀) + c/k⋅ḋ(q) + 1/k⋅f = 0,
 // where d₀ is a fixed length, k a stiffness parameter in N/m and c a damping
 // parameter in N⋅s/m. We use d(q) to denote the Euclidean distance between two
 // points P and Q, rigidly affixed to bodies A and B respectively, as a function

multibody/plant/test/multibody_plant_test.cc

@@ -4862,7 +4862,7 @@ GTEST_TEST(MultibodyPlantTest, AutoDiffAcrobotParameters) {
 }
 
 GTEST_TEST(MultibodyPlantTests, GetConstraintIds) {
-  // Set up a plant with 3 constraints with arbitrary parameters.
+  // Set up a plant with each constraint type with arbitrary parameters.
   MultibodyPlant<double> plant(0.01);
 
   EXPECT_EQ(plant.GetConstraintIds().size(), 0);