WIP: IRIS in configuration space

geometry/optimization/BUILD.bazel

@@ -49,6 +49,11 @@ drake_cc_library(
     deps = [
         ":convex_set",
         "//geometry:scene_graph",
+        "//multibody/parsing:parser",
+        "//multibody/plant",
+        "//solvers:ibex_solver",
+        "//solvers:snopt_solver",
+        "//systems/framework:diagram_builder",
     ],
 )
 

geometry/optimization/iris.cc

@@ -1,20 +1,27 @@
 #include "drake/geometry/optimization/iris.h"
 
 #include <algorithm>
+#include <limits>
 #include <optional>
+#include <unordered_map>
 #include <unordered_set>
 #include <utility>
 #include <vector>
 
 #include "drake/geometry/optimization/convex_set.h"
+#include "drake/multibody/plant/multibody_plant.h"
+#include "drake/solvers/ibex_solver.h"
+#include "drake/solvers/snopt_solver.h"
 
 namespace drake {
 namespace geometry {
 namespace optimization {
 
 using Eigen::MatrixXd;
 using Eigen::Ref;
+using Eigen::Vector3d;
 using Eigen::VectorXd;
+using symbolic::Expression;
 
 HPolyhedron Iris(const ConvexSets& obstacles, const Ref<const VectorXd>& sample,
                  const HPolyhedron& domain, const IrisOptions& options) {
@@ -106,43 +113,47 @@ class IrisConvexSetMaker final : public ShapeReifier {
                      std::optional<FrameId> reference_frame)
       : query_{query}, reference_frame_{reference_frame} {};
 
+  void set_reference_frame(const FrameId& reference_frame) {
+    DRAKE_DEMAND(reference_frame.is_valid());
+    *reference_frame_ = reference_frame;
+  }
+
+  void set_geometry_id(const GeometryId& geom_id) { geom_id_ = geom_id; }
+
   using ShapeReifier::ImplementGeometry;
 
   void ImplementGeometry(const Sphere&, void* data) {
-    GeometryId& geom_id = *static_cast<GeometryId*>(data);
-    sets_.emplace_back(
-        std::make_unique<Hyperellipsoid>(query_, geom_id, reference_frame_));
+    DRAKE_DEMAND(geom_id_.is_valid());
+    auto& set = *static_cast<copyable_unique_ptr<ConvexSet>*>(data);
+    set = std::make_unique<Hyperellipsoid>(query_, geom_id_, reference_frame_);
   }
 
   void ImplementGeometry(const HalfSpace&, void* data) {
-    GeometryId& geom_id = *static_cast<GeometryId*>(data);
-    sets_.emplace_back(
-        std::make_unique<HPolyhedron>(query_, geom_id, reference_frame_));
+    DRAKE_DEMAND(geom_id_.is_valid());
+    auto& set = *static_cast<copyable_unique_ptr<ConvexSet>*>(data);
+    set = std::make_unique<HPolyhedron>(query_, geom_id_, reference_frame_);
   }
 
   void ImplementGeometry(const Box&, void* data) {
-    GeometryId& geom_id = *static_cast<GeometryId*>(data);
+    DRAKE_DEMAND(geom_id_.is_valid());
+    auto& set = *static_cast<copyable_unique_ptr<ConvexSet>*>(data);
     // Note: We choose HPolyhedron over VPolytope here, but the IRIS paper
     // discusses a significant performance improvement using a "least-distance
     // programming" instance from CVXGEN that exploited the VPolytope
     // representation.  So we may wish to revisit this.
-    sets_.emplace_back(
-        std::make_unique<HPolyhedron>(query_, geom_id, reference_frame_));
+    set = std::make_unique<HPolyhedron>(query_, geom_id_, reference_frame_);
   }
 
   void ImplementGeometry(const Ellipsoid&, void* data) {
-    GeometryId& geom_id = *static_cast<GeometryId*>(data);
-    sets_.emplace_back(
-        std::make_unique<Hyperellipsoid>(query_, geom_id, reference_frame_));
+    DRAKE_DEMAND(geom_id_.is_valid());
+    auto& set = *static_cast<copyable_unique_ptr<ConvexSet>*>(data);
+    set = std::make_unique<Hyperellipsoid>(query_, geom_id_, reference_frame_);
   }
 
-  // This must be called last; it transfers ownership.
-  ConvexSets claim_sets() { return std::move(sets_); }
-
  private:
   const QueryObject<double>& query_{};
   std::optional<FrameId> reference_frame_{};
-  ConvexSets sets_{};
+  GeometryId geom_id_{};
 };
 
 }  // namespace
@@ -153,12 +164,216 @@ ConvexSets MakeIrisObstacles(const QueryObject<double>& query_object,
   const GeometrySet all_ids(inspector.GetAllGeometryIds());
   const std::unordered_set<GeometryId> geom_ids =
       inspector.GetGeometryIds(all_ids, Role::kProximity);
+  ConvexSets sets(geom_ids.size());
 
   IrisConvexSetMaker maker(query_object, reference_frame);
+  int count = 0;
   for (GeometryId geom_id : geom_ids) {
-    inspector.GetShape(geom_id).Reify(&maker, &geom_id);
+    maker.set_geometry_id(geom_id);
+    inspector.GetShape(geom_id).Reify(&maker, &sets[count++]);
+  }
+  return sets;
+}
+
+namespace {
+
+// Solves the optimization
+// min_q (q-d)*CᵀC(q-d)
+// s.t. setA on bodyA and setB on bodyB are in collision in q.
+//      Aq ≤ b.
+// where C, d are the center and matrix from the hyperellipsoid E.
+// Returns true iff a collision is found.
+// Sets `closest` to an optimizing solution q*, if a solution is found.
+bool FindClosestCollision(const multibody::MultibodyPlant<Expression>& plant,
+                          const multibody::Body<Expression>& bodyA,
+                          const multibody::Body<Expression>& bodyB,
+                          const ConvexSet& setA, const ConvexSet& setB,
+                          const Hyperellipsoid& E,
+                          const Eigen::Ref<const Eigen::MatrixXd>& A,
+                          const Eigen::Ref<const Eigen::VectorXd>& b,
+                          const solvers::SolverBase& solver,
+                          systems::Context<Expression>* context,
+                          VectorXd* closest) {
+  solvers::MathematicalProgram prog;
+  auto q = prog.NewContinuousVariables(plant.num_positions(), "q");
+
+  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);
+
+  auto p_AA = prog.NewContinuousVariables<3>("p_AA");
+  auto p_BB = prog.NewContinuousVariables<3>("p_BB");
+  setA.AddPointInSetConstraints(&prog, p_AA);
+  setB.AddPointInSetConstraints(&prog, p_BB);
+
+  plant.SetPositions(context, q);
+  const math::RigidTransform<Expression> X_WA =
+      plant.EvalBodyPoseInWorld(*context, bodyA);
+  const math::RigidTransform<Expression> X_WB =
+      plant.EvalBodyPoseInWorld(*context, bodyB);
+  prog.AddConstraint(X_WA * p_AA.cast<Expression>() ==
+                     X_WB * p_BB.cast<Expression>());
+
+  // Help nonlinear optimizers (e.g. SNOPT) avoid trivial local minima at the
+  // origin.
+  prog.SetSolverOption(solvers::SnoptSolver::id(), "Print file",
+                       "/tmp/iris_snopt.out");
+  prog.SetInitialGuess(q, VectorXd::Constant(plant.num_positions(), .01));
+  prog.SetInitialGuess(p_AA, Vector3d::Constant(.01));
+  prog.SetInitialGuess(p_BB, Vector3d::Constant(.01));
+  for (const auto binding : prog.lorentz_cone_constraints()) {
+    const auto c = binding.evaluator();
+    prog.AddConstraint(
+        std::make_shared<solvers::LorentzConeConstraint>(
+            c->A_dense(), c->b(),
+            solvers::LorentzConeConstraint::EvalType::kNonconvex),
+        binding.variables());
+    prog.RemoveConstraint(binding);
+  }
+
+  solvers::MathematicalProgramResult result = solver.Solve(prog);
+  if (result.is_success()) {
+    *closest = result.GetSolution(q);
+    return true;
+  }
+  std::cout << prog << std::endl;
+  return false;
+}
+
+}  // namespace
+
+HPolyhedron IrisInConfigurationSpace(
+    const multibody::MultibodyPlant<Expression>& plant,
+    const QueryObject<double>& query_object,
+    const Eigen::Ref<const Eigen::VectorXd>& sample,
+    const IrisOptions& options) {
+  const int nq = plant.num_positions();
+  DRAKE_DEMAND(sample.size() == nq);
+
+  // Note: We require finite joint limits to define the bounding box for the
+  // IRIS algorithm.
+  DRAKE_DEMAND(plant.GetPositionLowerLimits().array().isFinite().all());
+  DRAKE_DEMAND(plant.GetPositionUpperLimits().array().isFinite().all());
+  HPolyhedron P = HPolyhedron::MakeBox(plant.GetPositionLowerLimits(),
+                                       plant.GetPositionUpperLimits());
+  DRAKE_DEMAND(P.A().rows() == 2 * nq);
+
+  const double kEpsilonEllipsoid = 1e-2;
+  Hyperellipsoid E = Hyperellipsoid::MakeHypersphere(kEpsilonEllipsoid, sample);
+
+  const SceneGraphInspector<double>& inspector = query_object.inspector();
+
+  // Make all of the convex sets.  TODO(russt): Consider factoring this out so
+  // that I don't have to redo the work for every new region.
+  IrisConvexSetMaker maker(query_object, inspector.world_frame_id());
+  std::unordered_map<GeometryId, copyable_unique_ptr<ConvexSet>> sets{};
+  std::unordered_map<GeometryId, const multibody::Body<Expression>&> bodies{};
+  const std::unordered_set<GeometryId> geom_ids = inspector.GetGeometryIds(
+      GeometrySet(inspector.GetAllGeometryIds()), Role::kProximity);
+  copyable_unique_ptr<ConvexSet> temp_set;
+  for (GeometryId geom_id : geom_ids) {
+    // Make all sets in the local geometry frame.
+    FrameId frame_id = inspector.GetFrameId(geom_id);
+    maker.set_reference_frame(frame_id);
+    maker.set_geometry_id(geom_id);
+    inspector.GetShape(geom_id).Reify(&maker, &temp_set);
+    sets.emplace(geom_id, std::move(temp_set));
+    bodies.emplace(geom_id, *plant.GetBodyFromFrameId(frame_id));
+  }
+
+  // TODO(russt): As a surrogate for the true objective, we could use convex
+  // optimization to compute the (squared?) distance between each collision pair
+  // from the sample point configuration, then sort the pairs by that distance.
+  // This could improve computation times in Ibex here and produce regions with
+  // less faces.
+  auto pairs = inspector.GetCollisionCandidates();
+  const int N = static_cast<int>(pairs.size());
+
+  // On each iteration, we will build the collision-free polytope represented as
+  // {x | A * x <= b}.  Here we pre-allocate matrices with a generous maximum
+  // size.
+  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> A(
+      P.A().rows() + 2*N, nq);
+  VectorXd b(P.A().rows() + 2*N);
+  A.topRows(P.A().rows()) = P.A();
+  b.head(P.A().rows()) = P.b();
+
+  double best_volume = E.Volume();
+  int iteration = 0;
+  MatrixXd tangent_matrix;
+
+  //solvers::IbexSolver solver;
+  solvers::SnoptSolver solver;
+  DRAKE_DEMAND(solver.is_available() && solver.is_enabled());
+  auto context = plant.CreateDefaultContext();
+  VectorXd closest(nq);
+
+  while (true) {
+    tangent_matrix = 2.0 * E.A().transpose() * E.A();
+    int num_constraints = 2 * nq;  // Start with just the joint limits.
+    // Find separating hyperplanes
+    for (const auto [geomA, geomB] : pairs) {
+      while (true) {
+        const bool collision = FindClosestCollision(
+            plant, bodies.at(geomA), bodies.at(geomB), *sets.at(geomA),
+            *sets.at(geomB), E, A.topRows(num_constraints),
+            b.head(num_constraints), solver, context.get(), &closest);
+        std::cout << "geomA=" << inspector.GetName(geomA)
+                  << ", geomB=" << inspector.GetName(geomB);
+        if (collision) {
+          std::cout << ", closest=" << closest.transpose() << std::endl;
+          // Add the tangent to the (scaled) ellipsoid at this point as a
+          // constraint.
+          if (num_constraints >= A.rows()) {
+            // Increase pre-allocated polytope size.
+            A.conservativeResize(A.rows() + N, nq);
+            b.conservativeResize(b.rows() + N);
+          }
+
+          A.row(num_constraints) =
+              (tangent_matrix * (closest - E.center())).normalized();
+          b[num_constraints] = A.row(num_constraints) * closest -
+                               options.configuration_space_margin;
+          std::cout << "  "
+                    << A.row(num_constraints) *
+                           symbolic::MakeVectorVariable(nq, "q")
+                    << " ≤ " << b[num_constraints] << std::endl;
+          num_constraints++;
+        } else {
+          std::cout << ".  No collisions." << std::endl;
+          break;
+        }
+      }
+    }
+
+    if (options.require_sample_point_is_contained &&
+        ((A.topRows(num_constraints) * sample).array() >=
+         b.head(num_constraints).array())
+            .any()) {
+      break;
+    }
+    P = HPolyhedron(A.topRows(num_constraints), b.head(num_constraints));
+
+    iteration++;
+    if (iteration >= options.iteration_limit) {
+      break;
+    }
+
+    E = P.MaximumVolumeInscribedEllipsoid();
+    const double volume = E.Volume();
+    if (volume - best_volume <= options.termination_threshold) {
+      break;
+    }
+    best_volume = volume;
   }
-  return maker.claim_sets();
+  return P;
 }
 
 }  // namespace optimization

geometry/optimization/iris.h

@@ -4,8 +4,10 @@
 #include <optional>
 #include <vector>
 
+#include "drake/common/symbolic.h"
 #include "drake/geometry/optimization/convex_set.h"
 #include "drake/geometry/optimization/hpolyhedron.h"
+#include "drake/multibody/plant/multibody_plant.h"
 
 namespace drake {
 namespace geometry {
@@ -32,6 +34,12 @@ struct IrisOptions {
   /** IRIS will terminate if the change in the *volume* of the hyperellipsoid
   between iterations is less that this threshold. */
   double termination_threshold{2e-2};  // from rdeits/iris-distro.
+
+  /** For IRIS in configuration space, we retreat by this margin from each
+  C-space obstacle in order to avoid the possibility of requiring an infinite
+  number of faces to approximate a curved boundary.  TODO: clarify the units.
+  */
+  double configuration_space_margin{1e-2};
 };
 
 /** The IRIS (Iterative Region Inflation by Semidefinite programming) algorithm,
@@ -85,6 +93,14 @@ ConvexSets MakeIrisObstacles(
     const QueryObject<double>& query_object,
     std::optional<FrameId> reference_frame = std::nullopt);
 
+// TODO(russt): Pass a context of the plant if/when we support setting kinematic
+// parameters.
+HPolyhedron IrisInConfigurationSpace(
+    const multibody::MultibodyPlant<symbolic::Expression>& plant,
+    const QueryObject<double>& query_object,
+    const Eigen::Ref<const Eigen::VectorXd>& sample,
+    const IrisOptions& options = IrisOptions());
+
 }  // namespace optimization
 }  // namespace geometry
 }  // namespace drake