Construct MinimumDistanceConstraint with a CollisionChecker. (#19760)

We can now compute the distance through CollisionChecker, and then
impose bounds on the minimum distance.

multibody/inverse_kinematics/BUILD.bazel

@@ -104,6 +104,7 @@ drake_cc_library(
         "//math:geometric_transform",
         "//math:gradient",
         "//multibody/plant",
+        "//planning:collision_checker",
         "//solvers:constraint",
         "//solvers:mathematical_program",
         "//solvers:minimum_value_constraint",
@@ -206,6 +207,7 @@ drake_cc_library(
         "//multibody/parsing",
         "//multibody/plant",
         "//multibody/tree",
+        "//planning:robot_diagram_builder",
         "@gtest//:without_main",
     ],
 )
@@ -290,6 +292,7 @@ drake_cc_googletest(
         ":inverse_kinematics_test_utilities",
         ":kinematic_evaluators",
         "//common/test_utilities:expect_throws_message",
+        "//planning:scene_graph_collision_checker",
         "//solvers/test_utilities",
     ],
 )

multibody/inverse_kinematics/minimum_distance_constraint.cc

@@ -67,13 +67,32 @@ VectorX<S> Distances(const MultibodyPlant<T>& plant,
   return distances;
 }
 
-template <typename T>
-void MinimumDistanceConstraint::Initialize(
-    const MultibodyPlant<T>& plant, systems::Context<T>* plant_context,
+Eigen::VectorXd Distances(
+    const planning::CollisionChecker& collision_checker,
+    planning::CollisionCheckerContext* collision_checker_context,
+    const Eigen::Ref<const Eigen::VectorXd>& x, double influence_distance_val) {
+  return collision_checker
+      .CalcContextRobotClearance(collision_checker_context, x,
+                                 influence_distance_val)
+      .distances();
+}
+
+AutoDiffVecXd Distances(
+    const planning::CollisionChecker& collision_checker,
+    planning::CollisionCheckerContext* collision_checker_context,
+    const Eigen::Ref<const AutoDiffVecXd>& x, double influence_distance_val) {
+  const planning::RobotClearance robot_clearance =
+      collision_checker.CalcContextRobotClearance(collision_checker_context,
+                                                  math::ExtractValue(x),
+                                                  influence_distance_val);
+  return math::InitializeAutoDiff(
+      robot_clearance.distances(),
+      robot_clearance.jacobians() * math::ExtractGradient(x));
+}
+
+void MinimumDistanceConstraint::CheckMinimumDistanceBounds(
     double minimum_distance_lower, double minimum_distance_upper,
-    double influence_distance,
-    MinimumDistancePenaltyFunction penalty_function) {
-  CheckPlantIsConnectedToSceneGraph(plant, *plant_context);
+    double influence_distance) const {
   if (!std::isfinite(influence_distance)) {
     throw std::invalid_argument(
         "MinimumDistanceConstraint: influence_distance must be finite.");
@@ -94,6 +113,17 @@ void MinimumDistanceConstraint::Initialize(
         "minimum_distance_upper={}.",
         influence_distance, minimum_distance_upper));
   }
+}
+
+template <typename T>
+void MinimumDistanceConstraint::Initialize(
+    const MultibodyPlant<T>& plant, systems::Context<T>* plant_context,
+    double minimum_distance_lower, double minimum_distance_upper,
+    double influence_distance,
+    MinimumDistancePenaltyFunction penalty_function) {
+  CheckPlantIsConnectedToSceneGraph(plant, *plant_context);
+  CheckMinimumDistanceBounds(minimum_distance_lower, minimum_distance_upper,
+                             influence_distance);
   const auto& query_port = plant.get_geometry_query_input_port();
   // Maximum number of SignedDistancePairs returned by calls to
   // ComputeSignedDistancePairwiseClosestPoints().
@@ -120,6 +150,37 @@ void MinimumDistanceConstraint::Initialize(
   }
 }
 
+void MinimumDistanceConstraint::Initialize(
+    const planning::CollisionChecker& collision_checker,
+    planning::CollisionCheckerContext* collision_checker_context,
+    double minimum_distance_lower, double minimum_distance_upper,
+    double influence_distance,
+    MinimumDistancePenaltyFunction penalty_function) {
+  CheckMinimumDistanceBounds(minimum_distance_lower, minimum_distance_upper,
+                             influence_distance);
+  minimum_value_constraint_ = std::make_unique<solvers::MinimumValueConstraint>(
+      collision_checker.plant().num_positions(), minimum_distance_lower,
+      minimum_distance_upper, influence_distance,
+      collision_checker.MaxContextNumDistances(*collision_checker_context),
+      [this](const Eigen::Ref<const AutoDiffVecXd>& x,
+             double influence_distance_val) {
+        return Distances(*(this->collision_checker_),
+                         this->collision_checker_context_, x,
+                         influence_distance_val);
+      },
+      [this](const Eigen::Ref<const Eigen::VectorXd>& x,
+             double influence_distance_val) {
+        return Distances(*(this->collision_checker_),
+                         this->collision_checker_context_, x,
+                         influence_distance_val);
+      });
+  this->set_bounds(minimum_value_constraint_->lower_bound(),
+                   minimum_value_constraint_->upper_bound());
+  if (penalty_function) {
+    minimum_value_constraint_->set_penalty_function(penalty_function);
+  }
+}
+
 MinimumDistanceConstraint::MinimumDistanceConstraint(
     const multibody::MultibodyPlant<double>* const plant,
     double minimum_distance, systems::Context<double>* plant_context,
@@ -148,7 +209,8 @@ MinimumDistanceConstraint::MinimumDistanceConstraint(
       plant_double_{plant},
       plant_context_double_{plant_context},
       plant_autodiff_{nullptr},
-      plant_context_autodiff_{nullptr} {
+      plant_context_autodiff_{nullptr},
+      collision_checker_{nullptr} {
   Initialize(*plant_double_, plant_context_double_, minimum_distance_lower,
              minimum_distance_upper, influence_distance_offset,
              penalty_function);
@@ -179,11 +241,51 @@ MinimumDistanceConstraint::MinimumDistanceConstraint(
       plant_double_{nullptr},
       plant_context_double_{nullptr},
       plant_autodiff_{plant},
-      plant_context_autodiff_{plant_context} {
+      plant_context_autodiff_{plant_context},
+      collision_checker_{nullptr} {
   Initialize(*plant_autodiff_, plant_context_autodiff_, minimum_distance_lower,
              minimum_distance_upper, influence_distance, penalty_function);
 }
 
+MinimumDistanceConstraint::MinimumDistanceConstraint(
+    const planning::CollisionChecker* collision_checker,
+    double minimum_distance_lower,
+    planning::CollisionCheckerContext* collision_checker_context,
+    MinimumDistancePenaltyFunction penalty_function,
+    double influence_distance_offset)
+    : MinimumDistanceConstraint(
+          collision_checker, minimum_distance_lower,
+          kInf /* minimum_distance_upper */, collision_checker_context,
+          penalty_function,
+          minimum_distance_lower + influence_distance_offset) {}
+
+MinimumDistanceConstraint::MinimumDistanceConstraint(
+    const planning::CollisionChecker* collision_checker,
+    double minimum_distance_lower, double minimum_distance_upper,
+    planning::CollisionCheckerContext* collision_checker_context,
+    MinimumDistancePenaltyFunction penalty_function, double influence_distance)
+    : solvers::Constraint(
+          NumConstraints(minimum_distance_lower, minimum_distance_upper),
+          internal::PtrOrThrow(
+              collision_checker,
+              "MinimumDistanceConstraint: collision_checker is nullptr")
+              ->plant()
+              .num_positions(),
+          Eigen::VectorXd::Zero(
+              NumConstraints(minimum_distance_lower, minimum_distance_upper)),
+          Eigen::VectorXd::Zero(
+              NumConstraints(minimum_distance_lower, minimum_distance_upper))),
+      plant_double_{nullptr},
+      plant_context_double_{nullptr},
+      plant_autodiff_{nullptr},
+      plant_context_autodiff_{nullptr},
+      collision_checker_{collision_checker},
+      collision_checker_context_{collision_checker_context} {
+  Initialize(*collision_checker_, collision_checker_context_,
+             minimum_distance_lower, minimum_distance_upper, influence_distance,
+             penalty_function);
+}
+
 template <typename T>
 void MinimumDistanceConstraint::DoEvalGeneric(
     const Eigen::Ref<const VectorX<T>>& x, VectorX<T>* y) const {

multibody/inverse_kinematics/minimum_distance_constraint.h

@@ -4,6 +4,7 @@
 #include <vector>
 
 #include "drake/multibody/plant/multibody_plant.h"
+#include "drake/planning/collision_checker.h"
 #include "drake/solvers/constraint.h"
 #include "drake/solvers/minimum_value_constraint.h"
 #include "drake/systems/framework/context.h"
@@ -56,8 +57,8 @@ configuration vector, q, of the associated MultibodyPlant.
 
 The formulation of the constraint is
 
-SmoothOverMax( φ((dᵢ(q) - d_influence)/(d_influence - lb)) / φ(-1) ) ≤ 1
-SmoothUnderMax( φ((dᵢ(q) - d_influence)/(d_influence - ub)) / φ(-1) ) ≥ 1
+    SmoothOverMax( φ((dᵢ(q) - d_influence)/(d_influence - lb)) / φ(-1) ) ≤ 1
+    SmoothUnderMax( φ((dᵢ(q) - d_influence)/(d_influence - ub)) / φ(-1) ) ≥ 1
 
 where dᵢ(q) is the signed distance of the i-th pair, lb is the minimum
 allowable distance, d_influence is the "influence distance" (the distance below
@@ -87,7 +88,14 @@ class MinimumDistanceConstraint final : public solvers::Constraint {
   influence_distance_offset. This value must be finite and strictly positive, as
   it is used to scale the signed distances between pairs of geometries. Smaller
   values may improve performance, as fewer pairs of geometries need to be
-  considered in each constraint evaluation. @default 1 meter
+  considered in each constraint evaluation. @default 1 meter.
+  The chosen influence_distance_offset can significantly affect the runtime and
+  optimization performance of using this constraint. Larger values result in
+  more expensive collision checks (since more potential collision candidates
+  must be considered) and may result in worse optimization performance (the
+  optimizer may not be able to find a configuration that satisfies the
+  constraint). In work at TRI, we have used much lower values (e.g. 1e-6) for
+  influence_distance_offset with good results.
   @throws std::exception if `plant` has not registered its geometry with
   a SceneGraph object.
   @throws std::exception if influence_distance_offset = ∞.
@@ -136,6 +144,7 @@ class MinimumDistanceConstraint final : public solvers::Constraint {
   @param minimum_distance_upper The upper bound of the minimum distance.
   min(distance) <= upper. If minimum_distance_upper is finite, then it must be
   smaller than influence_distance.
+  @param collision_checker_context The context for the collision checker.
   @pydrake_mkdoc_identifier{autodiff_with_upper_bound}
   */
   MinimumDistanceConstraint(
@@ -145,6 +154,34 @@ class MinimumDistanceConstraint final : public solvers::Constraint {
       MinimumDistancePenaltyFunction penalty_function,
       double influence_distance);
 
+  /** Overloaded constructor.
+  Constructs the constraint with CollisionChecker instead of MultibodyPlant.
+  @param collision_checker collision_checker must outlive this constraint.
+  @param collision_checker_context The context for the collision checker. See
+  CollisionChecker class for more details.
+  @pydrake_mkdoc_identifier{collision_checker_no_upper_bound}
+  */
+  MinimumDistanceConstraint(
+      const planning::CollisionChecker* collision_checker,
+      double minimum_distance,
+      planning::CollisionCheckerContext* collision_checker_context,
+      MinimumDistancePenaltyFunction penalty_function = {},
+      double influence_distance_offset = 1);
+
+  /** Overloaded constructor.
+  Constructs the constraint with CollisionChecker instead of MultibodyPlant.
+  @param collision_checker collision_checker must outlive this constraint.
+  @param collision_checker_context The context for the collision checker. See
+  CollisionChecker class for more details.
+  @pydrake_mkdoc_identifier{collision_checker_with_upper_bound}
+  */
+  MinimumDistanceConstraint(
+      const planning::CollisionChecker* collision_checker,
+      double minimum_distance_lower, double minimum_distance_upper,
+      planning::CollisionCheckerContext* collision_checker_context,
+      MinimumDistancePenaltyFunction penalty_function,
+      double influence_distance);
+
   ~MinimumDistanceConstraint() override {}
 
   /** Getter for the minimum distance. */
@@ -190,14 +227,28 @@ class MinimumDistanceConstraint final : public solvers::Constraint {
   void Initialize(const MultibodyPlant<T>& plant,
                   systems::Context<T>* plant_context,
                   double minimum_distance_lower, double minimum_distance_upper,
-                  double influence_distance_offset,
+                  double influence_distance,
+                  MinimumDistancePenaltyFunction penalty_function);
+
+  // Overload Initialize with CollisionChecker instead of MultibodyPlant.
+  void Initialize(const planning::CollisionChecker& collision_checker,
+                  planning::CollisionCheckerContext* collision_checker_context,
+                  double minimum_distance_lower, double minimum_distance_upper,
+                  double influence_distance,
                   MinimumDistancePenaltyFunction penalty_function);
 
+  void CheckMinimumDistanceBounds(double minimum_distance_lower,
+                                  double minimum_distance_upper,
+                                  double influence_distance) const;
+
   const multibody::MultibodyPlant<double>* const plant_double_;
   systems::Context<double>* const plant_context_double_;
   std::unique_ptr<solvers::MinimumValueConstraint> minimum_value_constraint_;
   const multibody::MultibodyPlant<AutoDiffXd>* const plant_autodiff_;
   systems::Context<AutoDiffXd>* const plant_context_autodiff_;
+
+  const planning::CollisionChecker* collision_checker_;
+  planning::CollisionCheckerContext* collision_checker_context_;
 };
 }  // namespace multibody
 }  // namespace drake

multibody/inverse_kinematics/test/inverse_kinematics_test_utilities.cc

@@ -3,8 +3,12 @@
 #include <algorithm>
 #include <utility>
 
+#include <fmt/format.h>
+
 #include "drake/common/default_scalars.h"
 #include "drake/common/find_resource.h"
+#include "drake/geometry/geometry_roles.h"
+#include "drake/geometry/proximity_properties.h"
 #include "drake/multibody/parsing/parser.h"
 #include "drake/multibody/plant/coulomb_friction.h"
 #include "drake/systems/framework/diagram_builder.h"
@@ -140,6 +144,48 @@ TwoFreeSpheresTest::TwoFreeSpheresTest() {
       *plant_autodiff_, diagram_context_autodiff_.get()));
 }
 
+SpheresAndWallsTest::SpheresAndWallsTest() : builder_{} {
+  builder_.plant().set_name("spheres_and_walls");
+  geometry::ProximityProperties proximity_properties{};
+  // Kinematics doesn't care about robot dynamics. Use arbitrary material
+  // properties.
+  AddContactMaterial(0.1, 250, multibody::CoulombFriction<double>{0.9, 0.5},
+                     &proximity_properties);
+  left_wall_ = builder_.plant().RegisterCollisionGeometry(
+      builder_.plant().world_body(),
+      math::RigidTransformd(Eigen::Vector3d(-(wall_length_ / 2 + 0.05), 0, 0)),
+      geometry::Box(0.1, wall_length_, 1), "left_wall", proximity_properties);
+  right_wall_ = builder_.plant().RegisterCollisionGeometry(
+      builder_.plant().world_body(),
+      math::RigidTransformd(Eigen::Vector3d(wall_length_ / 2 + 0.05, 0, 0)),
+      geometry::Box(0.1, wall_length_, 1), "right_wall", proximity_properties);
+  top_wall_ = builder_.plant().RegisterCollisionGeometry(
+      builder_.plant().world_body(),
+      math::RigidTransformd(Eigen::Vector3d(0, wall_length_ / 2 + 0.05, 0)),
+      geometry::Box(wall_length_, 0.1, 1), "top_wall", proximity_properties);
+  bottom_wall_ = builder_.plant().RegisterCollisionGeometry(
+      builder_.plant().world_body(),
+      math::RigidTransformd(Eigen::Vector3d(0, -(wall_length_ / 2 + 0.05), 0)),
+      geometry::Box(wall_length_, 0.1, 1), "bottom_wall", proximity_properties);
+  // Use arbitrary inertia.
+  const multibody::SpatialInertia<double> spatial_inertia(
+      1, Eigen::Vector3d::Zero(),
+      multibody::UnitInertia<double>(0.001, 0.001, 0.001));
+
+  for (int i = 0; i < static_cast<int>(body_indices_.size()); ++i) {
+    body_indices_[i] =
+        builder_.plant()
+            .AddRigidBody(fmt::format("body{}", i), spatial_inertia)
+            .index();
+    spheres_[i] = builder_.plant().RegisterCollisionGeometry(
+        builder_.plant().get_body(body_indices_[i]),
+        math::RigidTransformd(Eigen::Vector3d::Zero()),
+        geometry::Sphere(radius_), fmt::format("sphere{}", i),
+        proximity_properties);
+  }
+  builder_.plant().Finalize();
+}
+
 template <typename T>
 std::unique_ptr<systems::Diagram<T>> ConstructBoxSphereDiagram(
     const Eigen::Vector3d& box_size, double radius,
@@ -158,9 +204,9 @@ std::unique_ptr<systems::Diagram<T>> ConstructBoxSphereDiagram(
   const auto& sphere = (*plant)->AddRigidBody(
       "sphere", SpatialInertia<double>(1, Eigen::Vector3d::Zero(),
                                        UnitInertia<double>(1, 1, 1)));
-  (*plant)->RegisterCollisionGeometry(
-      sphere, RigidTransformd::Identity(), geometry::Sphere(radius), "sphere",
-      CoulombFriction<double>(0.9, 0.8));
+  (*plant)->RegisterCollisionGeometry(sphere, RigidTransformd::Identity(),
+                                      geometry::Sphere(radius), "sphere",
+                                      CoulombFriction<double>(0.9, 0.8));
 
   *box_frame_index = box.body_frame().index();
   *sphere_frame_index = sphere.body_frame().index();
@@ -213,8 +259,8 @@ T BoxSphereSignedDistance(const Eigen::Ref<const Eigen::Vector3d>& box_size,
     return -(half_size - p_BS.array().abs()).minCoeff() - radius;
   } else {
     T signed_distance = 0;
-    using std::pow;
     using std::abs;
+    using std::pow;
     for (int i = 0; i < 3; ++i) {
       // Compute the distance from the sphere center box face along the i'th
       // dimension.
@@ -227,10 +273,8 @@ T BoxSphereSignedDistance(const Eigen::Ref<const Eigen::Vector3d>& box_size,
   }
 }
 
-DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS((
-    &ConstructTwoFreeBodiesPlant<T>,
-    &BoxSphereSignedDistance<T>
-))
+DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS(
+    (&ConstructTwoFreeBodiesPlant<T>, &BoxSphereSignedDistance<T>))
 
 }  // namespace multibody
 }  // namespace drake