IrisNp2 Support Additional Containment Points

bindings/pydrake/planning/test/iris_test.py

@@ -181,9 +181,6 @@ def test_iris_np2(self):
         options = mut.IrisNp2Options()
         SetSampledIrisOptions(options)
 
-        # Feature still TODO.
-        options.sampled_iris_options.containment_points = None
-
         # For speed reasons -- IPOPT seems to be faster than SNOPT here.
         options.solver = IpoptSolver()
 

geometry/optimization/iris.cc

@@ -36,6 +36,8 @@ using Eigen::MatrixXd;
 using Eigen::Ref;
 using Eigen::Vector3d;
 using Eigen::VectorXd;
+using internal::CounterExampleConstraint;
+using internal::CounterExampleProgram;
 using internal::IrisConvexSetMaker;
 using math::RigidTransform;
 using multibody::Frame;
@@ -174,150 +176,6 @@ ConvexSets MakeIrisObstacles(const QueryObject<double>& query_object,
 
 namespace {
 
-// Takes a constraint bound to another mathematical program and defines a new
-// constraint that is the negation of one index and one (lower/upper) bound.
-class CounterExampleConstraint : public Constraint {
- public:
-  DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(CounterExampleConstraint);
-
-  explicit CounterExampleConstraint(const MathematicalProgram* prog)
-      : Constraint(1, prog->num_vars(),
-                   Vector1d(-std::numeric_limits<double>::infinity()),
-                   Vector1d::Constant(-kSolverConstraintTolerance - 1e-14)),
-        prog_{prog} {
-    DRAKE_DEMAND(prog != nullptr);
-  }
-
-  ~CounterExampleConstraint() = default;
-
-  // Sets the actual constraint to be falsified, overwriting any previously set
-  // constraints. The Binding<Constraint> must remain valid for the lifetime of
-  // this object (or until a new Binding<Constraint> is set).
-  void set(const Binding<Constraint>* binding_with_constraint_to_be_falsified,
-           int index, bool falsify_lower_bound) {
-    DRAKE_DEMAND(binding_with_constraint_to_be_falsified != nullptr);
-    const int N =
-        binding_with_constraint_to_be_falsified->evaluator()->num_constraints();
-    DRAKE_DEMAND(index >= 0 && index < N);
-    binding_ = binding_with_constraint_to_be_falsified;
-    index_ = index;
-    falsify_lower_bound_ = falsify_lower_bound;
-  }
-
- private:
-  void DoEval(const Eigen::Ref<const Eigen::VectorXd>& x,
-              Eigen::VectorXd* y) const override {
-    DRAKE_DEMAND(binding_ != nullptr);
-    const double val = prog_->EvalBinding(*binding_, x)[index_];
-    if (falsify_lower_bound_) {
-      // val - lb <= -kSolverConstraintTolerance < 0.
-      (*y)[0] = val - binding_->evaluator()->lower_bound()[index_];
-    } else {
-      // ub - val <= -kSolverConstraintTolerance < 0.
-      (*y)[0] = binding_->evaluator()->upper_bound()[index_] - val;
-    }
-  }
-
-  void DoEval(const Eigen::Ref<const AutoDiffVecXd>& x,
-              AutoDiffVecXd* y) const override {
-    DRAKE_DEMAND(binding_ != nullptr);
-    const AutoDiffXd val = prog_->EvalBinding(*binding_, x)[index_];
-    if (falsify_lower_bound_) {
-      // val - lb <= -kSolverConstraintTolerance < 0.
-      (*y)[0] = val - binding_->evaluator()->lower_bound()[index_];
-    } else {
-      // ub - val <= -kSolverConstraintTolerance < 0.
-      (*y)[0] = binding_->evaluator()->upper_bound()[index_] - val;
-    }
-  }
-
-  void DoEval(const Ref<const VectorX<symbolic::Variable>>&,
-              VectorX<symbolic::Expression>*) const override {
-    // MathematicalProgram::EvalBinding doesn't support symbolic, and we
-    // shouldn't get here.
-    throw std::logic_error(
-        "CounterExampleConstraint doesn't support DoEval for symbolic.");
-  }
-
-  const MathematicalProgram* prog_{};
-  const Binding<Constraint>* binding_{};
-  int index_{0};
-  bool falsify_lower_bound_{true};
-
-  // To find a counter-example for a constraints,
-  //  g(x) ≤ ub,
-  // we need to ask the solver to find
-  //  g(x) + kSolverConstraintTolerance > ub,
-  // which we implement as
-  //  g(x) + kSolverConstraintTolerance ≥ ub + eps.
-  // The variable is static so that it is initialized by the time it is accessed
-  // in the initializer list of the constructor.
-  // TODO(russt): We need a more robust way to get this from the solver. This
-  // value works for SNOPT and is reasonable for most solvers.
-  static constexpr double kSolverConstraintTolerance{1e-6};
-};
-
-// Defines a MathematicalProgram to solve the problem
-// min_q (q-d)*CᵀC(q-d)
-// s.t. counter-example-constraint
-//      Aq ≤ b.
-// where C, d are the matrix and center from the hyperellipsoid E.
-//
-// The class design supports repeated solutions of the (nearly) identical
-// problem from different initial guesses.
-class CounterExampleProgram {
- public:
-  CounterExampleProgram(
-      std::shared_ptr<CounterExampleConstraint> counter_example_constraint,
-      const Hyperellipsoid& E, const Eigen::Ref<const Eigen::MatrixXd>& A,
-      const Eigen::Ref<const Eigen::VectorXd>& b) {
-    q_ = prog_.NewContinuousVariables(A.cols(), "q");
-
-    P_constraint_ = prog_.AddLinearConstraint(
-        A,
-        VectorXd::Constant(b.size(), -std::numeric_limits<double>::infinity()),
-        b, q_);
-    // Scale the objective so the eigenvalues are close to 1, using
-    // scale*lambda_min = 1/scale*lambda_max.
-    const MatrixXd Asq = E.A().transpose() * E.A();
-    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(Asq);
-    const double scale = 1.0 / std::sqrt(es.eigenvalues().maxCoeff() *
-                                         es.eigenvalues().minCoeff());
-    prog_.AddQuadraticErrorCost(scale * Asq, E.center(), q_);
-
-    prog_.AddConstraint(counter_example_constraint, q_);
-  }
-
-  void UpdatePolytope(const Eigen::Ref<const Eigen::MatrixXd>& A,
-                      const Eigen::Ref<const Eigen::VectorXd>& b) {
-    P_constraint_->evaluator()->UpdateCoefficients(
-        A,
-        VectorXd::Constant(b.size(), -std::numeric_limits<double>::infinity()),
-        b);
-  }
-
-  // Returns true iff a counter-example is found.
-  // Sets `closest` to an optimizing solution q*, if a solution is found.
-  bool Solve(const solvers::SolverInterface& solver,
-             const Eigen::Ref<const Eigen::VectorXd>& q_guess,
-             const std::optional<solvers::SolverOptions>& solver_options,
-             VectorXd* closest) {
-    prog_.SetInitialGuess(q_, q_guess);
-    solvers::MathematicalProgramResult result;
-    solver.Solve(prog_, std::nullopt, solver_options, &result);
-    if (result.is_success()) {
-      *closest = result.GetSolution(q_);
-      return true;
-    }
-    return false;
-  }
-
- private:
-  MathematicalProgram prog_;
-  solvers::VectorXDecisionVariable q_;
-  std::optional<Binding<solvers::LinearConstraint>> P_constraint_{};
-};
-
 // Add the tangent to the (scaled) ellipsoid at @p point as a
 // constraint.
 void AddTangentToPolytope(

geometry/optimization/iris_internal.cc

@@ -9,6 +9,8 @@ namespace internal {
 
 using Eigen::MatrixXd;
 using Eigen::VectorXd;
+using solvers::Binding;
+using solvers::Constraint;
 
 SamePointConstraint::SamePointConstraint(
     const multibody::MultibodyPlant<double>* plant,
@@ -207,6 +209,87 @@ bool ClosestCollisionProgram::Solve(
   return false;
 }
 
+void CounterExampleConstraint::set(
+    const Binding<Constraint>* binding_with_constraint_to_be_falsified,
+    int index, bool falsify_lower_bound) {
+  DRAKE_DEMAND(binding_with_constraint_to_be_falsified != nullptr);
+  const int N =
+      binding_with_constraint_to_be_falsified->evaluator()->num_constraints();
+  DRAKE_DEMAND(index >= 0 && index < N);
+  binding_ = binding_with_constraint_to_be_falsified;
+  index_ = index;
+  falsify_lower_bound_ = falsify_lower_bound;
+}
+
+void CounterExampleConstraint::DoEval(const Eigen::Ref<const VectorXd>& x,
+                                      VectorXd* y) const {
+  DRAKE_DEMAND(binding_ != nullptr);
+  const double val = prog_->EvalBinding(*binding_, x)[index_];
+  if (falsify_lower_bound_) {
+    // val - lb <= -kSolverConstraintTolerance < 0.
+    (*y)[0] = val - binding_->evaluator()->lower_bound()[index_];
+  } else {
+    // ub - val <= -kSolverConstraintTolerance < 0.
+    (*y)[0] = binding_->evaluator()->upper_bound()[index_] - val;
+  }
+}
+
+void CounterExampleConstraint::DoEval(const Eigen::Ref<const AutoDiffVecXd>& x,
+                                      AutoDiffVecXd* y) const {
+  DRAKE_DEMAND(binding_ != nullptr);
+  const AutoDiffXd val = prog_->EvalBinding(*binding_, x)[index_];
+  if (falsify_lower_bound_) {
+    // val - lb <= -kSolverConstraintTolerance < 0.
+    (*y)[0] = val - binding_->evaluator()->lower_bound()[index_];
+  } else {
+    // ub - val <= -kSolverConstraintTolerance < 0.
+    (*y)[0] = binding_->evaluator()->upper_bound()[index_] - val;
+  }
+}
+
+CounterExampleProgram::CounterExampleProgram(
+    std::shared_ptr<CounterExampleConstraint> counter_example_constraint,
+    const Hyperellipsoid& E, const Eigen::Ref<const Eigen::MatrixXd>& A,
+    const Eigen::Ref<const Eigen::VectorXd>& b) {
+  q_ = prog_.NewContinuousVariables(A.cols(), "q");
+
+  P_constraint_ = prog_.AddLinearConstraint(
+      A, VectorXd::Constant(b.size(), -std::numeric_limits<double>::infinity()),
+      b, q_);
+  // Scale the objective so the eigenvalues are close to 1, using
+  // scale*lambda_min = 1/scale*lambda_max.
+  const MatrixXd Asq = E.A().transpose() * E.A();
+  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(Asq);
+  const double scale = 1.0 / std::sqrt(es.eigenvalues().maxCoeff() *
+                                       es.eigenvalues().minCoeff());
+  prog_.AddQuadraticErrorCost(scale * Asq, E.center(), q_);
+
+  prog_.AddConstraint(counter_example_constraint, q_);
+}
+
+void CounterExampleProgram::UpdatePolytope(
+    const Eigen::Ref<const Eigen::MatrixXd>& A,
+    const Eigen::Ref<const Eigen::VectorXd>& b) {
+  P_constraint_->evaluator()->UpdateCoefficients(
+      A, VectorXd::Constant(b.size(), -std::numeric_limits<double>::infinity()),
+      b);
+}
+
+bool CounterExampleProgram::Solve(
+    const solvers::SolverInterface& solver,
+    const Eigen::Ref<const Eigen::VectorXd>& q_guess,
+    const std::optional<solvers::SolverOptions>& solver_options,
+    VectorXd* closest) {
+  prog_.SetInitialGuess(q_, q_guess);
+  solvers::MathematicalProgramResult result;
+  solver.Solve(prog_, std::nullopt, solver_options, &result);
+  if (result.is_success()) {
+    *closest = result.GetSolution(q_);
+    return true;
+  }
+  return false;
+}
+
 void IrisConvexSetMaker::ImplementGeometry(const Box&, void* data) {
   DRAKE_DEMAND(geom_id_.is_valid());
   auto& set = *static_cast<copyable_unique_ptr<ConvexSet>*>(data);

geometry/optimization/iris_internal.h

@@ -4,6 +4,7 @@
  call these functions, but we expose them for unit tests.
  */
 
+#include <limits>
 #include <memory>
 #include <optional>
 #include <variant>
@@ -139,6 +140,94 @@ class ClosestCollisionProgram {
   std::optional<solvers::Binding<solvers::LinearConstraint>> P_constraint_{};
 };
 
+// Takes a constraint bound to another mathematical program and defines a new
+// constraint that is the negation of one index and one (lower/upper) bound.
+class CounterExampleConstraint : public solvers::Constraint {
+ public:
+  DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(CounterExampleConstraint);
+
+  explicit CounterExampleConstraint(const solvers::MathematicalProgram* prog)
+      : solvers::Constraint(
+            1, prog->num_vars(),
+            Vector1d(-std::numeric_limits<double>::infinity()),
+            Vector1d::Constant(-kSolverConstraintTolerance - 1e-14)),
+        prog_{prog} {
+    DRAKE_DEMAND(prog != nullptr);
+  }
+
+  ~CounterExampleConstraint() = default;
+
+  // Sets the actual constraint to be falsified, overwriting any previously set
+  // constraints. The Binding<Constraint> must remain valid for the lifetime of
+  // this object (or until a new Binding<Constraint> is set).
+  void set(const solvers::Binding<solvers::Constraint>*
+               binding_with_constraint_to_be_falsified,
+           int index, bool falsify_lower_bound);
+
+ private:
+  void DoEval(const Eigen::Ref<const Eigen::VectorXd>& x,
+              Eigen::VectorXd* y) const override;
+
+  void DoEval(const Eigen::Ref<const AutoDiffVecXd>& x,
+              AutoDiffVecXd* y) const override;
+
+  void DoEval(const Eigen::Ref<const Eigen::VectorX<symbolic::Variable>>&,
+              Eigen::VectorX<symbolic::Expression>*) const override {
+    // MathematicalProgram::EvalBinding doesn't support symbolic, and we
+    // shouldn't get here.
+    throw std::logic_error(
+        "CounterExampleConstraint doesn't support DoEval for symbolic.");
+  }
+
+  const solvers::MathematicalProgram* prog_{};
+  const solvers::Binding<solvers::Constraint>* binding_{};
+  int index_{0};
+  bool falsify_lower_bound_{true};
+
+  // To find a counter-example for a constraints,
+  //  g(x) ≤ ub,
+  // we need to ask the solver to find
+  //  g(x) + kSolverConstraintTolerance > ub,
+  // which we implement as
+  //  g(x) + kSolverConstraintTolerance ≥ ub + eps.
+  // The variable is static so that it is initialized by the time it is accessed
+  // in the initializer list of the constructor.
+  // TODO(russt): We need a more robust way to get this from the solver. This
+  // value works for SNOPT and is reasonable for most solvers.
+  static constexpr double kSolverConstraintTolerance{1e-6};
+};
+
+// Defines a MathematicalProgram to solve the problem
+// min_q (q-d)*CᵀC(q-d)
+// s.t. counter-example-constraint
+//      Aq ≤ b.
+// where C, d are the matrix and center from the hyperellipsoid E.
+//
+// The class design supports repeated solutions of the (nearly) identical
+// problem from different initial guesses.
+class CounterExampleProgram {
+ public:
+  CounterExampleProgram(
+      std::shared_ptr<CounterExampleConstraint> counter_example_constraint,
+      const Hyperellipsoid& E, const Eigen::Ref<const Eigen::MatrixXd>& A,
+      const Eigen::Ref<const Eigen::VectorXd>& b);
+
+  void UpdatePolytope(const Eigen::Ref<const Eigen::MatrixXd>& A,
+                      const Eigen::Ref<const Eigen::VectorXd>& b);
+
+  // Returns true iff a counter-example is found.
+  // Sets `closest` to an optimizing solution q*, if a solution is found.
+  bool Solve(const solvers::SolverInterface& solver,
+             const Eigen::Ref<const Eigen::VectorXd>& q_guess,
+             const std::optional<solvers::SolverOptions>& solver_options,
+             Eigen::VectorXd* closest);
+
+ private:
+  solvers::MathematicalProgram prog_;
+  solvers::VectorXDecisionVariable q_;
+  std::optional<solvers::Binding<solvers::LinearConstraint>> P_constraint_{};
+};
+
 // Constructs a ConvexSet for each supported Shape and adds it to the set.
 class IrisConvexSetMaker final : public ShapeReifier {
  public:

planning/iris/BUILD.bazel

@@ -49,7 +49,9 @@ drake_cc_library(
         "//geometry:meshcat",
         "//geometry/optimization:convex_set",
         "//multibody/rational:rational_forward_kinematics",
+        "//planning:collision_checker",
         "//solvers:mathematical_program",
+        "//solvers:solve",
     ],
     implementation_deps = [
         "@common_robotics_utilities_internal//:common_robotics_utilities",