[inverse_kinematics] Locked joints are immobile (#19442)

Locked joints are held fixed during IK (using the their current value
from the context).

Locked joints trump position limits; it's OK to lock outside of the
limit, and the program will still solve.

When a quaternion is locked, we lock its decision variables to the
normalized value and do not add a norm constraint.

multibody/inverse_kinematics/inverse_kinematics.cc

@@ -1,6 +1,7 @@
 #include "drake/multibody/inverse_kinematics/inverse_kinematics.h"
 
 #include <limits>
+#include <utility>
 
 #include "drake/multibody/inverse_kinematics/angle_between_vectors_constraint.h"
 #include "drake/multibody/inverse_kinematics/angle_between_vectors_cost.h"
@@ -18,34 +19,91 @@
 
 namespace drake {
 namespace multibody {
+
+constexpr double kInf = std::numeric_limits<double>::infinity();
+
 InverseKinematics::InverseKinematics(const MultibodyPlant<double>& plant,
                                      bool with_joint_limits)
-    : prog_{new solvers::MathematicalProgram()},
-      plant_(plant),
-      owned_context_(plant_.CreateDefaultContext()),
-      context_(owned_context_.get()),
-      q_(prog_->NewContinuousVariables(plant_.num_positions(), "q")) {
-  if (with_joint_limits) {
-    prog_->AddBoundingBoxConstraint(plant.GetPositionLowerLimits(),
-                                    plant.GetPositionUpperLimits(), q_);
-  }
-  AddUnitQuaternionConstraintOnPlant(plant, q_, prog_.get());
-}
+    : InverseKinematics(plant, plant.CreateDefaultContext(), nullptr,
+                        with_joint_limits) {}
 
 InverseKinematics::InverseKinematics(const MultibodyPlant<double>& plant,
                                      systems::Context<double>* plant_context,
                                      bool with_joint_limits)
-    : prog_{new solvers::MathematicalProgram()},
+    : InverseKinematics(plant, nullptr, plant_context, with_joint_limits) {}
+
+InverseKinematics::InverseKinematics(
+    const MultibodyPlant<double>& plant,
+    std::unique_ptr<systems::Context<double>> owned_context,
+    systems::Context<double>* plant_context, bool with_joint_limits)
+    : prog_(std::make_unique<solvers::MathematicalProgram>()),
       plant_(plant),
-      owned_context_(nullptr),
-      context_(plant_context),
+      owned_context_(std::move(owned_context)),
+      context_(owned_context_ ? owned_context_.get() : plant_context),
       q_(prog_->NewContinuousVariables(plant.num_positions(), "q")) {
-  DRAKE_DEMAND(plant_context != nullptr);
+  if (owned_context_ != nullptr) {
+    // The public constructor must not pass both pointers when calling us.
+    DRAKE_DEMAND(plant_context == nullptr);
+  } else {
+    // The user calling the `plant_context` overload should not pass nullptr.
+    DRAKE_THROW_UNLESS(plant_context != nullptr);
+  }
+  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) {
-    prog_->AddBoundingBoxConstraint(plant.GetPositionLowerLimits(),
-                                    plant.GetPositionUpperLimits(), q_);
+    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{0}; i < plant.num_joints(); ++i) {
+    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;
+    }
   }
-  AddUnitQuaternionConstraintOnPlant(plant, q_, prog_.get());
+
+  // Add the unit quaternion constraints.
+  for (BodyIndex i{0}; i < plant.num_bodies(); ++i) {
+    const Body<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;
+      } else {
+        prog_->AddConstraint(solvers::Binding<solvers::Constraint>(
+            std::make_shared<UnitQuaternionConstraint>(),
+            q_.segment<size>(start)));
+      }
+    }
+  }
+
+  // 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

@@ -15,6 +15,10 @@ namespace multibody {
  * postures of the robot satisfying certain constraints.
  * The decision variables include the generalized position of the robot.
  *
+ * To perform IK on a subset of the plant, use the constructor overload that
+ * takes a `plant_context` and use `Joint::Lock` on the joints in that Context
+ * that should be fixed during IK.
+ *
  * @ingroup planning_kinematics
  */
 class InverseKinematics {
@@ -48,10 +52,12 @@ class InverseKinematics {
    * @param plant The robot on which the inverse kinematics problem will be
    * solved. This plant should have been connected to a SceneGraph within a
    * Diagram
-   * @param context The context for the plant. This context should be a part of
-   * the Diagram context.
-   * To construct a plant connected to a SceneGraph, with the corresponding
-   * plant_context, the steps are
+   * @param plant_context The context for the plant. This context should be a
+   * part of the Diagram context. Any locked joints in the `plant_context` will
+   * remain fixed at their locked value. (This provides a convenient way to
+   * perform IK on a subset of the plant.) To construct a plant connected to a
+   * SceneGraph, with the corresponding plant_context, the steps are:
+   * ```
    * // 1. Add a diagram containing the MultibodyPlant and SceneGraph
    * systems::DiagramBuilder<double> builder;
    * auto items = AddMultibodyPlantSceneGraph(&builder, 0.0);
@@ -64,6 +70,7 @@ class InverseKinematics {
    * // 5. Get the context for the plant.
    * auto plant_context = &(diagram->GetMutableSubsystemContext(items.plant,
    * diagram_context.get()));
+   * ```
    * This context will be modified during calling ik.prog.Solve(...). When
    * Solve() returns `result`, context will store the optimized posture, namely
    * plant.GetPositions(*context) will be the same as in
@@ -390,6 +397,13 @@ class InverseKinematics {
   systems::Context<double>* get_mutable_context() { return context_; }
 
  private:
+  /* Both public constructors delegate to here. Exactly one of owned_context or
+  plant_context must be non-null. */
+  InverseKinematics(const MultibodyPlant<double>& plant,
+                    std::unique_ptr<systems::Context<double>> owned_context,
+                    systems::Context<double>* plant_context,
+                    bool with_joint_limits);
+
   std::unique_ptr<solvers::MathematicalProgram> prog_;
   const MultibodyPlant<double>& plant_;
   std::unique_ptr<systems::Context<double>> const owned_context_;

multibody/inverse_kinematics/test/inverse_kinematics_test.cc

@@ -1,12 +1,15 @@
 #include "drake/multibody/inverse_kinematics/inverse_kinematics.h"
 
+#include <gmock/gmock.h>
 #include <gtest/gtest.h>
 
 #include "drake/common/find_resource.h"
 #include "drake/common/test_utilities/eigen_matrix_compare.h"
 #include "drake/math/rotation_matrix.h"
 #include "drake/math/wrap_to.h"
 #include "drake/multibody/inverse_kinematics/test/inverse_kinematics_test_utilities.h"
+#include "drake/multibody/tree/quaternion_floating_joint.h"
+#include "drake/multibody/tree/revolute_joint.h"
 #include "drake/solvers/create_constraint.h"
 #include "drake/solvers/solve.h"
 
@@ -15,6 +18,11 @@
 
 namespace drake {
 namespace multibody {
+
+namespace {
+constexpr double kInf = std::numeric_limits<double>::infinity();
+}  // namespace
+
 Eigen::Quaterniond Vector4ToQuaternion(
     const Eigen::Ref<const Eigen::Vector4d>& q) {
   return Eigen::Quaterniond(q(0), q(1), q(2), q(3));
@@ -140,6 +148,88 @@ TEST_F(TwoFreeBodiesTest, ConstructorAddsUnitQuaterionConstraints) {
   EXPECT_NEAR(q.segment<4>(7).squaredNorm(), 1.0, 1e-6);
 }
 
+GTEST_TEST(InverseKinematicsTest, ConstructorLockedJoints) {
+  MultibodyPlant<double> plant(0);
+
+  // Create a plant with four bodies.
+  const auto& world = plant.world_body();
+  const auto M = SpatialInertia<double>::SolidCubeWithMass(1.0, 0.1);
+  const auto& body1 = plant.AddRigidBody("body1", M);
+  const auto& body2 = plant.AddRigidBody("body2", M);
+  const auto& body3 = plant.AddRigidBody("body3", M);
+  const auto& body4 = plant.AddRigidBody("body4", M);
+
+  // Attach a specific joint to each body:
+  // (1) A quaternion floating joint that will not be locked.
+  // (2) A quaternion floating joint that we'll lock to its initial position.
+  // (3) A revolute joint that will not be locked.
+  // (4) A revolute joint that we'll lock to its initial position.
+  math::RigidTransform<double> I;
+  Eigen::Vector3d X = Eigen::Vector3d::UnitX();
+  const auto& joint1 =
+      plant.AddJoint<QuaternionFloatingJoint>("joint1", world, I, body1, I);
+  const auto& joint2 =
+      plant.AddJoint<QuaternionFloatingJoint>("joint2", world, I, body2, I);
+  const auto& joint3 =
+      plant.AddJoint<RevoluteJoint>("joint3", world, I, body3, I, X);
+  const auto& joint4 =
+      plant.AddJoint<RevoluteJoint>("joint4", world, I, body4, I, X);
+  plant.Finalize();
+  auto context = plant.CreateDefaultContext();
+
+  // Leave joint1 unlocked.
+
+  // Lock body2's floating joint to an un-normalized initial value.
+  joint2.set_quaternion(&*context, Eigen::Quaternion<double>(3.0, 0, 0, 0));
+  joint2.Lock(&*context);
+
+  // Set limits on joint3, but do not lock it.
+  dynamic_cast<RevoluteJoint<double>&>(plant.get_mutable_joint(joint3.index()))
+      .set_position_limits(Vector1d{-0.5}, Vector1d{0.5});
+
+  // Lock body4's revolute joint beyond its limit.
+  dynamic_cast<RevoluteJoint<double>&>(plant.get_mutable_joint(joint4.index()))
+      .set_position_limits(Vector1d{-1}, Vector1d{1});
+  joint4.set_angle(&*context, 1.1);
+  joint4.Lock(&*context);
+
+  // Initialize IK.
+  const InverseKinematics ik(plant, &*context);
+  const int nq = ik.q().size();
+  const solvers::MathematicalProgram& prog = ik.prog();
+
+  // The unit quaternion constraint is only added to joint1.
+  ASSERT_EQ(prog.generic_constraints().size(), 1);
+  const solvers::Binding<solvers::Constraint>& unit_quat =
+      prog.generic_constraints().front();
+  ASSERT_EQ(unit_quat.variables().size(), 4);
+  EXPECT_EQ(symbolic::Variables(unit_quat.variables()),
+            symbolic::Variables(ik.q().segment(joint1.position_start(), 4)));
+
+  // Check the default bbox constraint.
+  // Prepare our expected values:
+  Eigen::VectorXd lower = Eigen::VectorXd::Constant(nq, -kInf);
+  Eigen::VectorXd upper = Eigen::VectorXd::Constant(nq, +kInf);
+  // - Locked quaternion floating joints obey a single, normalized position.
+  const int j2_start = joint2.position_start();
+  lower.segment(j2_start, 7) = upper.segment(j2_start, 7) =
+      (Vector<double, 7>() << 1, 0, 0, 0, 0, 0, 0).finished();
+  // - Unlocked revolute joints still obey their position limits.
+  const int j3_start = joint3.position_start();
+  lower[j3_start] = -0.5;
+  upper[j3_start] = +0.5;
+  // - Locked revolute joints obey their initial position, ignoring limits.
+  const int j4_start = joint4.position_start();
+  lower[j4_start] = upper[j4_start] = 1.1;
+  // Now check the expected value against `prog`.
+  ASSERT_EQ(prog.bounding_box_constraints().size(), 1);
+  const solvers::Binding<solvers::BoundingBoxConstraint>& limits =
+      prog.bounding_box_constraints().front();
+  ASSERT_EQ(limits.variables().size(), nq);
+  EXPECT_TRUE(CompareMatrices(limits.evaluator()->lower_bound(), lower));
+  EXPECT_TRUE(CompareMatrices(limits.evaluator()->upper_bound(), upper));
+}
+
 TEST_F(TwoFreeBodiesTest, PositionConstraint) {
   const Eigen::Vector3d p_BQ(0.2, 0.3, 0.5);
   const Eigen::Vector3d p_AQ_lower(-0.1, -0.2, -0.3);