Add mixing_steps to IrisOptions

Resolves RobotLocomotion#20036.
RussTedrake · Feb 21, 2024 · bff3679 · bff3679
1 parent 9b7d019
commit bff3679
Show file tree

Hide file tree

Showing 5 changed files with 24 additions and 4 deletions.
diff --git a/bindings/pydrake/geometry/geometry_py_optimization.cc b/bindings/pydrake/geometry/geometry_py_optimization.cc
@@ -507,6 +507,8 @@ void DefineGeometryOptimization(py::module m) {
             cls_doc.num_additional_constraint_infeasible_samples.doc)
         .def_readwrite(
             "random_seed", &IrisOptions::random_seed, cls_doc.random_seed.doc)
+        .def_readwrite("mixing_steps", &IrisOptions::mixing_steps,
+            cls_doc.mixing_steps.doc)
         .def("__repr__", [](const IrisOptions& self) {
           return py::str(
               "IrisOptions("
@@ -519,7 +521,8 @@ void DefineGeometryOptimization(py::module m) {
               "configuration_obstacles {}, "
               "prog_with_additional_constraints {}, "
               "num_additional_constraint_infeasible_samples={}, "
-              "random_seed={}"
+              "random_seed={}, "
+              "mixing_steps={}"
               ")")
               .format(self.require_sample_point_is_contained,
                   self.iteration_limit, self.termination_threshold,
@@ -530,7 +533,7 @@ void DefineGeometryOptimization(py::module m) {
                   self.prog_with_additional_constraints ? "is set"
                                                         : "is not set",
                   self.num_additional_constraint_infeasible_samples,
-                  self.random_seed);
+                  self.random_seed, self.mixing_steps);
         });
 
     DefReadWriteKeepAlive(&iris_options, "prog_with_additional_constraints",

diff --git a/bindings/pydrake/geometry/test/optimization_test.py b/bindings/pydrake/geometry/test/optimization_test.py
@@ -622,6 +622,7 @@ def test_make_from_scene_graph_and_iris(self):
         options.termination_threshold = 0.1
         options.relative_termination_threshold = 0.01
         options.random_seed = 1314
+        options.mixing_steps = 20
         options.starting_ellipse = mut.Hyperellipsoid.MakeUnitBall(3)
         options.bounding_region = mut.HPolyhedron.MakeBox(
             lb=[-6, -6, -6], ub=[6, 6, 6])

diff --git a/geometry/optimization/iris.cc b/geometry/optimization/iris.cc
@@ -746,7 +746,8 @@ HPolyhedron IrisInConfigurationSpace(const MultibodyPlant<double>& plant,
           guess = prev_counter_examples[counter_example_searches_for_this_pair];
         } else {
           MakeGuessFeasible(P_candidate, &guess);
-          guess = P_candidate.UniformSample(&generator, guess);
+          guess = P_candidate.UniformSample(&generator, guess,
+                                            options.mixing_steps);
         }
         ++counter_example_searches_for_this_pair;
         if (options.meshcat && nq <= 3) {
@@ -882,7 +883,8 @@ HPolyhedron IrisInConfigurationSpace(const MultibodyPlant<double>& plant,
               } else {
                 ++consecutive_failures;
               }
-              guess = P_candidate.UniformSample(&generator, guess);
+              guess = P_candidate.UniformSample(&generator, guess,
+                                                options.mixing_steps);
             }
           }
         }

diff --git a/geometry/optimization/iris.h b/geometry/optimization/iris.h
@@ -34,6 +34,7 @@ struct IrisOptions {
     a->Visit(DRAKE_NVP(num_collision_infeasible_samples));
     a->Visit(DRAKE_NVP(num_additional_constraint_infeasible_samples));
     a->Visit(DRAKE_NVP(random_seed));
+    a->Visit(DRAKE_NVP(mixing_steps));
   }
 
   /** The initial polytope is guaranteed to contain the point if that point is
@@ -152,6 +153,11 @@ struct IrisOptions {
   require_sample_point_is_contained is enforced.
   */
   std::function<bool(const HPolyhedron&)> termination_func{};
+
+  /* The `mixing_steps` parameters is passed to HPolyhedron::UniformSample to
+  control the total number of hit-and-run steps taken for each new random
+  sample. */
+  int mixing_steps{10};
 };
 
 /** The IRIS (Iterative Region Inflation by Semidefinite programming) algorithm,

diff --git a/geometry/optimization/test/iris_in_configuration_space_test.cc b/geometry/optimization/test/iris_in_configuration_space_test.cc
@@ -914,6 +914,14 @@ GTEST_TEST(IrisInConfigurationSpaceTest, ConvexConfigurationSpace) {
 
     MaybePauseForUser();
   }
+
+  // Confirm that mixing_steps has a tangible effect (this example is
+  // particularly sensitive to the sampling), and obtains a smaller volume due
+  // to non-uniform sampling with less mixing_steps.
+  options.mixing_steps = 1;  // Smaller than the default.
+  HPolyhedron region2 = IrisFromUrdf(convex_urdf, sample, options);
+  EXPECT_GE(region.MaximumVolumeInscribedEllipsoid().Volume(),
+            region2.MaximumVolumeInscribedEllipsoid().Volume());
 }
 
 // Three boxes.  Two on the outside are fixed.  One in the middle on a prismatic