Use AddMultibodyPlantConstraints in InverseKinematics

bindings/pydrake/multibody/test/inverse_kinematics_test.py

@@ -87,7 +87,7 @@ def test_AddMultibodyPlantConstraints(self):
             q=self.q,
             prog=self.prog,
             plant_context=self.ik_two_bodies.context())
-        self.assertEqual(len(bindings), 4)
+        self.assertEqual(len(bindings), 3)
 
     def test_AddPositionConstraint1(self):
         p_BQ = np.array([0.2, 0.3, 0.5])

multibody/inverse_kinematics/BUILD.bazel

@@ -135,6 +135,7 @@ drake_cc_library(
     ],
     visibility = ["//visibility:private"],
     deps = [
+        ":add_multibody_plant_constraints",
         ":kinematic_evaluators",
         "//multibody/plant",
         "//solvers:mathematical_program",
@@ -167,6 +168,7 @@ drake_cc_googletest(
     srcs = ["test/add_multibody_plant_constraints_test.cc"],
     deps = [
         ":add_multibody_plant_constraints",
+        "//common/test_utilities:eigen_matrix_compare",
         "//solvers:snopt_solver",
         "//solvers:solve",
     ],

multibody/inverse_kinematics/add_multibody_plant_constraints.cc

@@ -19,6 +19,9 @@ std::vector<Binding<Constraint>> AddMultibodyPlantConstraints(
     solvers::MathematicalProgram* prog,
     systems::Context<double>* plant_context) {
   DRAKE_THROW_UNLESS(prog != nullptr);
+  if (plant_context) {
+    plant.ValidateContext(*plant_context);
+  }
   // AddMultibodyPlantConstraints only supports coupler, ball, distance, and
   // weld.
   int num_supported_constraints =
@@ -36,49 +39,133 @@ std::vector<Binding<Constraint>> AddMultibodyPlantConstraints(
         "not supported here yet (but likely should be)",
         plant.num_constraints(), num_supported_constraints));
   }
-  std::vector<Binding<Constraint>> bindings =
-      AddUnitQuaternionConstraintOnPlant(plant, q, prog);
+  std::vector<Binding<Constraint>> bindings;
+
+  // Joint limits.
+  const int nq = plant.num_positions();
+  Eigen::VectorXd lb = plant.GetPositionLowerLimits();
+  Eigen::VectorXd ub = plant.GetPositionUpperLimits();
+
+  // Joint locking.
+  VectorX<bool> is_locked = VectorX<bool>::Constant(nq, false);
+  Eigen::VectorXd current_positions(nq);
+  if (plant_context) {
+    current_positions = plant.GetPositions(*plant_context);
+    for (JointIndex i : plant.GetJointIndices()) {
+      const Joint<double>& joint = plant.get_joint(i);
+      if (joint.is_locked(*plant_context)) {
+        const int start = joint.position_start();
+        const int size = joint.num_positions();
+        lb.segment(start, size) = current_positions.segment(start, size);
+        ub.segment(start, size) = current_positions.segment(start, size);
+        is_locked.segment(start, size).array() = true;
+      }
+    }
+  }
+
+  // Add the unit quaternion constraints.
+  for (BodyIndex i{0}; i < plant.num_bodies(); ++i) {
+    const RigidBody<double>& body = plant.get_body(i);
+    if (body.has_quaternion_dofs()) {
+      const int start = body.floating_positions_start();
+      constexpr int kSize = 4;
+      if (plant_context && is_locked.segment<kSize>(start).any()) {
+        // Sanity check the MultibodyTree invariant.
+        DRAKE_DEMAND(is_locked.segment<kSize>(start).all());
+        // Lock to the normalized value, in lieu of a unit norm constraint.
+        const Eigen::Vector4d quat =
+            current_positions.segment<kSize>(start).normalized();
+        lb.segment<kSize>(start) = quat;
+        ub.segment<kSize>(start) = quat;
+        prog->SetInitialGuess(q.segment<kSize>(start), quat);
+      } else {
+        // TODO(russt): Probably the joint limits should be [-1, 1] coming
+        // right out of the MultibodyPlant.
+        lb.segment<kSize>(start) = -Eigen::Vector4d::Ones();
+        ub.segment<kSize>(start) = Eigen::Vector4d::Ones();
+        bindings
+            .emplace_back(
+                prog->AddConstraint(solvers::Binding<solvers::Constraint>(
+                    std::make_shared<UnitQuaternionConstraint>(),
+                    q.segment<kSize>(start))))
+            .evaluator()
+            ->set_description(fmt::format(
+                "Unit quaternion constraint for body {}", body.name()));
+        prog->SetInitialGuess(q.segment<kSize>(start),
+                              Eigen::Vector4d{1, 0, 0, 0});
+      }
+    }
+  }
+  bindings.emplace_back(prog->AddBoundingBoxConstraint(lb, ub, q))
+      .evaluator()
+      ->set_description("Joint limits");
+
   for (const auto& [id, spec] : plant.get_coupler_constraint_specs()) {
     const int q0_index = plant.get_joint(spec.joint0_index).position_start();
     const int q1_index = plant.get_joint(spec.joint1_index).position_start();
-    bindings.emplace_back(prog->AddLinearEqualityConstraint(
-        q[q0_index] == spec.gear_ratio * q[q1_index] + spec.offset));
+    bindings
+        .emplace_back(prog->AddLinearEqualityConstraint(
+            q[q0_index] == spec.gear_ratio * q[q1_index] + spec.offset))
+        .evaluator()
+        ->set_description(
+            fmt::format("Coupler constraint for joint {} and joint {}",
+                        spec.joint0_index, spec.joint1_index));
   }
   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));
+    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))
+        .evaluator()
+        ->set_description(
+            fmt::format("Distance constraint between body {} and body {}",
+                        spec.body_A, spec.body_B));
   }
   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));
+    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))
+        .evaluator()
+        ->set_description(
+            fmt::format("Ball constraint between body {} and body {}",
+                        spec.body_A, spec.body_B));
   }
   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));
+    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))
+        .evaluator()
+        ->set_description(
+            fmt::format("Weld position constraint between body {} and body {}",
+                        spec.body_A, spec.body_B));
+    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))
+        .evaluator()
+        ->set_description(fmt::format(
+            "Weld orientation constraint between body {} and body {}",
+            spec.body_A, spec.body_B));
   }
   return bindings;
 }

multibody/inverse_kinematics/add_multibody_plant_constraints.h

@@ -8,9 +8,13 @@
 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.
+/** For all kinematic constraints associated with `plant` adds a corresponding
+solver::Constraint to `prog`, using decision variables `q` to represent the
+generalized positions of the plant.
+
+Adds joint limits constraints, unit quaternion constraints, and constraints for
+any locked joints (via Joint::Lock()). Note that you must pass a valid
+`plant_context` to use joint locking.
 
 Adds constraints for coupler, distance, ball, and weld constraints. The
 distance constraint is implemented here as a hard kinematic constraint (i.e.,

multibody/inverse_kinematics/inverse_kinematics.cc

@@ -3,6 +3,7 @@
 #include <limits>
 #include <utility>
 
+#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/distance_constraint.h"
@@ -21,8 +22,6 @@
 namespace drake {
 namespace multibody {
 
-constexpr double kInf = std::numeric_limits<double>::infinity();
-
 InverseKinematics::InverseKinematics(const MultibodyPlant<double>& plant,
                                      bool with_joint_limits)
     : InverseKinematics(plant, plant.CreateDefaultContext(), nullptr,
@@ -51,64 +50,21 @@ InverseKinematics::InverseKinematics(
   }
   DRAKE_DEMAND(context_ != nullptr);  // Sanity check.
 
-  // We're about to add constraints for position limits (if requested), locked
-  // joints, and quaternions (unit norm). When a quaternion is locked, we'll use
-  // the lock constraint instead of the unit norm constraint.
-
-  // Obey the joint limits (if requested).
-  const int nq = plant.num_positions();
-  Eigen::VectorXd lb;
-  Eigen::VectorXd ub;
-  if (with_joint_limits) {
-    lb = plant.GetPositionLowerLimits();
-    ub = plant.GetPositionUpperLimits();
-  } else {
-    lb = Eigen::VectorXd::Constant(nq, -kInf);
-    ub = Eigen::VectorXd::Constant(nq, +kInf);
-  }
-
-  // Obey joint locking. Joint locking trumps joint limits.
-  const Eigen::VectorXd current_positions = plant.GetPositions(context());
-  VectorX<bool> is_locked = VectorX<bool>::Constant(nq, false);
-  for (JointIndex i : plant.GetJointIndices()) {
-    const Joint<double>& joint = plant.get_joint(i);
-    if (joint.is_locked(context())) {
-      const int start = joint.position_start();
-      const int size = joint.num_positions();
-      lb.segment(start, size) = current_positions.segment(start, size);
-      ub.segment(start, size) = current_positions.segment(start, size);
-      is_locked.segment(start, size).array() = true;
-    }
-  }
-
-  // Add the unit quaternion constraints.
-  for (BodyIndex i{0}; i < plant.num_bodies(); ++i) {
-    const RigidBody<double>& body = plant.get_body(i);
-    if (body.has_quaternion_dofs()) {
-      const int start = body.floating_positions_start();
-      constexpr int size = 4;
-      if (is_locked.segment<size>(start).any()) {
-        // Sanity check the MultibodyTree invariant.
-        DRAKE_DEMAND(is_locked.segment<size>(start).all());
-        // Lock to the normalized value, in lieu of a unit norm constraint.
-        const Eigen::Vector4d quat =
-            current_positions.segment<size>(start).normalized();
-        lb.segment<size>(start) = quat;
-        ub.segment<size>(start) = quat;
-        prog_->SetInitialGuess(q_.segment<size>(start), quat);
-      } else {
-        prog_->AddConstraint(solvers::Binding<solvers::Constraint>(
-            std::make_shared<UnitQuaternionConstraint>(),
-            q_.segment<size>(start)));
-        // Set a non-zero initial guess to help avoid singularities.
-        prog_->SetInitialGuess(q_.segment<size>(start),
-                              Eigen::Vector4d{1, 0, 0, 0});
+  AddMultibodyPlantConstraints(plant, q_, prog_.get(), context_);
+
+  if (!with_joint_limits) {
+    // Remove only the joint limit constraint.
+    const auto& bbox_bindings = prog_->bounding_box_constraints();
+    bool removed_joint_limits = false;
+    for (const auto& binding : bbox_bindings) {
+      if (binding.evaluator()->get_description() == "Joint limits") {
+        prog_->RemoveConstraint(binding);
+        removed_joint_limits = true;
+        break;
       }
     }
+    DRAKE_DEMAND(removed_joint_limits);
   }
-
-  // Now we can finally add the bbox constraint for joint limits and locking.
-  prog_->AddBoundingBoxConstraint(lb, ub, q_);
 }
 
 solvers::Binding<solvers::Constraint> InverseKinematics::AddPositionConstraint(

multibody/inverse_kinematics/inverse_kinematics.h

@@ -33,8 +33,8 @@ class InverseKinematics {
    * @param plant The robot on which the inverse kinematics problem will be
    * solved.
    * @param with_joint_limits If set to true, then the constructor
-   * imposes the joint limit (obtained from plant.GetPositionLowerLimits()
-   * and plant.GetPositionUpperLimits(). If set to false, then the constructor
+   * imposes the joint limits (obtained from plant.GetPositionLowerLimits()
+   * and plant.GetPositionUpperLimits()). If set to false, then the constructor
    * does not impose the joint limit constraints in the constructor.
    * @note The inverse kinematics problem constructed in this way doesn't permit
    * collision related constraint (such as calling
@@ -77,10 +77,11 @@ class InverseKinematics {
    * result.GetSolution(ik.q()). The user could then use this context to perform
    * kinematic computation (like computing the position of the end-effector
    * etc.).
-   * @param with_joint_limits If set to true, then the constructor
-   * imposes the joint limit (obtained from plant.GetPositionLowerLimits()
-   * and plant.GetPositionUpperLimits(). If set to false, then the constructor
-   * does not impose the joint limit constraints in the constructor.  */
+   * @param with_joint_limits If set to true, then the constructor imposes the
+   * joint limits (obtained from plant.GetPositionLowerLimits() and
+   * plant.GetPositionUpperLimits(), and from any body/joint locks set in
+   * `plant_context`). If set to false, then the constructor does not impose
+   * the joint limit constraints in the constructor.  */
   InverseKinematics(const MultibodyPlant<double>& plant,
                     systems::Context<double>* plant_context,
                     bool with_joint_limits = true);