Add bindings for ConvexSet::CalcVolumeViaSampling (RobotLocomotion#20843

)
mcx · Feb 8, 2024 · f078ff4 · f078ff4
1 parent 2e8e856
commit f078ff4
Show file tree

Hide file tree

Showing 2 changed files with 34 additions and 2 deletions.
diff --git a/bindings/pydrake/geometry/geometry_py_optimization.cc b/bindings/pydrake/geometry/geometry_py_optimization.cc
@@ -83,6 +83,17 @@ void DefineGeometryOptimization(py::module m) {
 
   py::module::import("pydrake.solvers");
 
+  // SampledVolume. This struct must be declared before ConvexSet as methods in
+  // ConvexSet depend on this struct.
+  {
+    py::class_<SampledVolume>(m, "SampledVolume", doc.SampledVolume.doc)
+        .def_readwrite(
+            "volume", &SampledVolume::volume, doc.SampledVolume.volume.doc)
+        .def_readwrite("rel_accuracy", &SampledVolume::rel_accuracy,
+            doc.SampledVolume.rel_accuracy.doc)
+        .def_readwrite("num_samples", &SampledVolume::num_samples,
+            doc.SampledVolume.num_samples.doc);
+  }
   // ConvexSet
   {
     const auto& cls_doc = doc.ConvexSet;
@@ -127,7 +138,11 @@ void DefineGeometryOptimization(py::module m) {
             cls_doc.AddPointInNonnegativeScalingConstraints.doc_7args)
         .def("ToShapeWithPose", &ConvexSet::ToShapeWithPose,
             cls_doc.ToShapeWithPose.doc)
-        .def("CalcVolume", &ConvexSet::CalcVolume, cls_doc.CalcVolume.doc);
+        .def("CalcVolume", &ConvexSet::CalcVolume, cls_doc.CalcVolume.doc)
+        .def("CalcVolumeViaSampling", &ConvexSet::CalcVolumeViaSampling,
+            py::arg("generator"), py::arg("desired_rel_accuracy") = 1e-2,
+            py::arg("max_num_samples") = 1e4,
+            cls_doc.CalcVolumeViaSampling.doc);
   }
   // There is a dependency cycle between Hyperellipsoid and AffineBall, so we
   // need to "forward declare" the Hyperellipsoid class here.

diff --git a/bindings/pydrake/geometry/test/optimization_test.py b/bindings/pydrake/geometry/test/optimization_test.py
@@ -340,6 +340,9 @@ def test_hyperrectangle(self):
         mut.Hyperrectangle()
         rect = mut.Hyperrectangle(lb=-self.b, ub=self.b)
 
+        # Used for testing methods that require randomness.
+        generator = RandomGenerator()
+
         # Methods inherited from ConvexSet
         self.assertEqual(rect.ambient_dimension(), self.b.shape[0])
         self.assertTrue(rect.IntersectsWith(rect))
@@ -371,7 +374,6 @@ def test_hyperrectangle(self):
         np.testing.assert_array_equal(rect.lb(), -self.b)
         np.testing.assert_array_equal(rect.ub(), self.b)
         np.testing.assert_array_equal(rect.Center(), np.zeros_like(self.b))
-        generator = RandomGenerator()
         sample = rect.UniformSample(generator=generator)
         self.assertEqual(sample.shape, (self.b.shape[0],))
         hpoly = rect.MakeHPolyhedron()
@@ -384,6 +386,21 @@ def test_hyperrectangle(self):
         bbox = mut.Hyperrectangle.MaybeCalcAxisAlignedBoundingBox(set=other)
         self.assertIsInstance(bbox, mut.Hyperrectangle)
 
+    def test_calc_volume_via_sampling(self):
+        rect = mut.Hyperrectangle(lb=-self.b, ub=self.b)
+        generator = RandomGenerator()
+        desired_rel_accuracy = 1e-2
+        max_num_samples = 100
+        sampled_volume = rect.CalcVolumeViaSampling(
+            generator=generator,
+            desired_rel_accuracy=desired_rel_accuracy,
+            max_num_samples=max_num_samples
+        )
+        self.assertAlmostEqual(rect.CalcVolume(), sampled_volume.volume)
+        self.assertGreaterEqual(sampled_volume.rel_accuracy,
+                                desired_rel_accuracy)
+        self.assertEqual(sampled_volume.num_samples, max_num_samples)
+
     def test_minkowski_sum(self):
         mut.MinkowskiSum()
         point = mut.Point(np.array([11.1, 12.2, 13.3]))