[C-IRIS] include the HPolyhedron {s | C*s<=d, s_lower <= s <= s_upper…

…} in the search result. (RobotLocomotion#18667)
mqnc · Jan 27, 2023 · 51c042c · 51c042c
1 parent e34208f
commit 51c042c
Show file tree

Hide file tree

Showing 3 changed files with 13 additions and 3 deletions.
diff --git a/geometry/optimization/dev/cspace_free_polytope.cc b/geometry/optimization/dev/cspace_free_polytope.cc
@@ -1552,6 +1552,7 @@ CspaceFreePolytope::SearchWithBilinearAlternation(
       ret.back().C = C;
       ret.back().d = d;
       ret.back().num_iter = iter;
+      ret.back().certified_polytope = GetPolyhedronWithJointLimits(C, d);
       ret.back().SetSeparatingPlanes(certificates_result);
     }
     // Now fix the Lagrangian and search for C-space polytope and separating
@@ -1566,13 +1567,14 @@ CspaceFreePolytope::SearchWithBilinearAlternation(
       d = polytope_result->d;
       ret.back().C = polytope_result->C;
       ret.back().d = polytope_result->d;
+      ret.back().certified_polytope =
+          GetPolyhedronWithJointLimits(polytope_result->C, polytope_result->d);
       ret.back().a = std::move(polytope_result->a);
       ret.back().b = std::move(polytope_result->b);
       ret.back().num_iter = iter;
       // Now find the inscribed ellipsoid.
-      cspace_polytope =
-          this->GetPolyhedronWithJointLimits(ret.back().C, ret.back().d);
-      ellipsoid = cspace_polytope.MaximumVolumeInscribedEllipsoid();
+      ellipsoid =
+          ret.back().certified_polytope.MaximumVolumeInscribedEllipsoid();
       ellipsoid_Q = options.ellipsoid_scaling * (ellipsoid.A().inverse());
       const double cost = ellipsoid_Q.determinant();
       drake::log()->info("Iteration {}: det(Q)={}", iter, cost);
@@ -1676,6 +1678,7 @@ CspaceFreePolytope::BinarySearch(
     } else {
       ret.C = C;
       ret.d = d;
+      ret.certified_polytope = this->GetPolyhedronWithJointLimits(C, d);
       ret.UpdateSeparatingPlanes(certificates_result);
       return true;
     }

diff --git a/geometry/optimization/dev/cspace_free_polytope.h b/geometry/optimization/dev/cspace_free_polytope.h
@@ -311,6 +311,9 @@ class CspaceFreePolytope {
   struct SearchResult {
     Eigen::MatrixXd C;
     Eigen::VectorXd d;
+    // This is the certified C-space polytope {s | C * s <= d, s_lower <= s <=
+    // s_upper}.
+    HPolyhedron certified_polytope;
     // a[i].dot(x) + b[i]=0 is the separation plane for separating_planes()[i].
     std::unordered_map<int, Vector3<symbolic::Polynomial>> a;
     std::unordered_map<int, symbolic::Polynomial> b;

diff --git a/geometry/optimization/dev/test/cspace_free_polytope_with_mosek_test.cc b/geometry/optimization/dev/test/cspace_free_polytope_with_mosek_test.cc
@@ -1024,6 +1024,9 @@ TEST_F(CIrisToyRobotTest, SearchWithBilinearAlternation) {
   ASSERT_FALSE(bilinear_alternation_results.empty());
   ASSERT_EQ(bilinear_alternation_results.size(),
             bilinear_alternation_results.back().num_iter + 1);
+  EXPECT_EQ(bilinear_alternation_results.back()
+                .certified_polytope.ambient_dimension(),
+            C.cols());
   // Sample many s_values, project to {s | C*s <= d}. And then make sure that
   // the corresponding configurations are collision free.
   Eigen::Matrix<double, 10, 3> s_samples;
@@ -1094,6 +1097,7 @@ TEST_F(CIrisToyRobotTest, BinarySearch) {
   auto result = tester.cspace_free_polytope().BinarySearch(
       ignored_collision_pairs, C, d, s_center, options);
   ASSERT_TRUE(result.has_value());
+  EXPECT_EQ(result->certified_polytope.ambient_dimension(), C.cols());
   EXPECT_GT(result->num_iter, 0);
   Eigen::Matrix<double, 10, 3> s_samples;
   // clang-format off