compute maximum distance via sampling

bindings/pydrake/geometry/geometry_py_optimization.cc

@@ -141,6 +141,10 @@ void DefineGeometryOptimization(py::module m) {
  .def("CalcVolumeViaSampling", &ConvexSet::CalcVolumeViaSampling,
  py::arg("generator"), py::arg("desired_rel_accuracy") = 1e-2,
  py::arg("max_num_samples") = 1e4, cls_doc.CalcVolumeViaSampling.doc)
+ .def("CalcMaximumDistanceViaSampling",
+ &ConvexSet::CalcMaximumDistanceViaSampling, py::arg("generator"),
+ py::arg("num_samples") = 1000,
+ cls_doc.CalcMaximumDistanceViaSampling.doc)
  .def("Projection", &ConvexSet::Projection, py::arg("points"),
  cls_doc.Projection.doc);
  }

bindings/pydrake/geometry/test/optimization_test.py

@@ -55,6 +55,8 @@ def test_point_convex_set(self):
  np.testing.assert_array_equal(point.x(), 2*p)
  point.set_x(x=p)
  assert_pickle(self, point, lambda S: S.x())
+ self.assertAlmostEqual(point.CalcMaximumDistanceViaSampling(
+ generator=RandomGenerator(), num_samples=1000), 0.0, delta=1e-6)
 
  # TODO(SeanCurtis-TRI): This doesn't test the constructor that
  # builds from shape.
@@ -84,6 +86,8 @@ def test_affine_ball(self):
  self.assertTrue(E.IsBounded())
  self.assertTrue(E.PointInSet(E.MaybeGetFeasiblePoint()))
  self.assertTrue(E.IntersectsWith(E))
+ self.assertAlmostEqual(E.CalcMaximumDistanceViaSampling(
+ generator=RandomGenerator(), num_samples=1000), 2.0, delta=1e-6)
 
  mut.AffineBall.MakeAxisAligned(
  radius=np.ones(3), center=np.zeros(3))
@@ -197,6 +201,9 @@ def test_h_polyhedron(self):
  h_unit_box = mut.HPolyhedron.MakeUnitBox(dim=3)
  np.testing.assert_array_equal(h_box.A(), h_unit_box.A())
  np.testing.assert_array_equal(h_box.b(), h_unit_box.b())
+ self.assertAlmostEqual(h_unit_box.CalcMaximumDistanceViaSampling(
+ generator=RandomGenerator(), num_samples=1000), 2 * 3 ** 0.5,
+ delta=1e-6)
  A_l1 = np.array([[1, 1, 1],
  [-1, 1, 1],
  [1, -1, 1],

geometry/optimization/convex_set.cc

@@ -437,6 +437,55 @@ SampledVolume ConvexSet::CalcVolumeViaSampling(
  .num_samples = num_samples};
 }
 
+double ConvexSet::CalcMaximumDistanceViaSampling(RandomGenerator* generator,
+ const int num_samples) const {
+ if (ambient_dimension() == 0) {
+ throw std::runtime_error(fmt::format(
+ "Attempting to calculate the distance of a zero-dimensional "
+ "set {}. This is not well-defined.",
+ NiceTypeName::Get(*this)));
+ }
+ if (!IsBounded()) {
+ // return infinity, nan, 0 samples.
+ return std::numeric_limits<double>::infinity();
+ }
+ DRAKE_THROW_UNLESS(num_samples > 0);
+
+ Eigen::VectorXd cost_vector(ambient_dimension());
+ std::uniform_real_distribution<double> distribution(-1., 1.);
+ for (int i = 0; i < ambient_dimension(); ++i) {
+ cost_vector(i) = distribution(*generator);
+ }
+
+ // Create a mathematical program with a point in set constraint and a
+ // linear cost.
+ solvers::MathematicalProgram prog{};
+ const auto& x = prog.NewContinuousVariables(this->ambient_dimension(), "x");
+ auto cost = prog.AddLinearCost(cost_vector.transpose(), 0.0, x);
+ this->AddPointInSetConstraints(&prog, x);
+ solvers::MathematicalProgramResult result = solvers::Solve(prog);
+
+ std::vector<Eigen::VectorXd> points(num_samples);
+ points[0] = result.GetSolution(x);
+ double max_distance = 0.0;
+
+ for (int i = 1; i < num_samples; ++i) {
+ // Sample a new cost vector.
+ for (int j = 0; j < ambient_dimension(); ++j) {
+ cost_vector(j) = distribution(*generator);
+ }
+ cost.evaluator()->UpdateCoefficients(cost_vector);
+ result = solvers::Solve(prog);
+ points[i] = result.GetSolution(x);
+ // Compute the maximum distance from the previous points.
+ for (int j = 0; j < i; ++j) {
+ max_distance = std::max(max_distance, (points[i] - points[j]).norm());
+ }
+ }
+
+ return max_distance;
+}
+
 double ConvexSet::DoCalcVolume() const {
  throw std::runtime_error(
  fmt::format("The class {} has a defect -- has_exact_volume() is "

geometry/optimization/convex_set.h

@@ -276,6 +276,16 @@ class ConvexSet {
  const double desired_rel_accuracy = 1e-2,
  const int max_num_samples = 1e4) const;
 
+ /** Calculates an estimate of the largest distance in the convex set using
+ sampling.
+ @param generator a random number generator.
+ @param num_samples the number of samples to use.
+ @return a pair the estimated volume of the set and an upper bound for the
+ relative accuracy
+ @throws if ambient_dimension() == 0. */
+ double CalcMaximumDistanceViaSampling(RandomGenerator* generator,
+ const int num_samples = 1e3) const;
+
  /** Computes in the L₂ norm the distance and the nearest point in this convex
  set to every column of @p points. If this set is empty, we return nullopt.
  @pre points.rows() == ambient_dimension().

geometry/optimization/test/convex_set_test.cc

@@ -235,6 +235,37 @@ GTEST_TEST(ConvexSetTest, CalcVolumeViaSampling) {
  EXPECT_LT(good_result.num_samples, max_num_samples_high);
 };
 
+
+GTEST_TEST(ConvexSetTest, CalcMaximumDistanceViaSampling) {
+ RandomGenerator generator(1234);
+ const int max_num_samples = 1e3;
+
+ // Test for a set with ambient dimension 0.
+ const HPolyhedron zero_order_set = HPolyhedron::MakeUnitBox(0);
+ DRAKE_EXPECT_THROWS_MESSAGE(zero_order_set.CalcMaximumDistanceViaSampling(&generator, max_num_samples),
+ ".*zero-dimensional.*");
+
+ // Test with a unit circle.
+ const Hyperellipsoid unit_circle = Hyperellipsoid::MakeUnitBall(2);
+ double maximum_distance = unit_circle.CalcMaximumDistanceViaSampling(&generator, max_num_samples);
+ EXPECT_NEAR(maximum_distance, 2.0, 1e-6);
+
+ // Test with a unit sphere.
+ const Hyperellipsoid unit_sphere = Hyperellipsoid::MakeUnitBall(3);
+ maximum_distance = unit_sphere.CalcMaximumDistanceViaSampling(&generator, max_num_samples);
+ EXPECT_NEAR(maximum_distance, 2.0, 1e-6);
+
+ // Test with a unit square.
+ const HPolyhedron unit_square = HPolyhedron::MakeUnitBox(2);
+ maximum_distance = unit_square.CalcMaximumDistanceViaSampling(&generator, max_num_samples);
+ EXPECT_NEAR(maximum_distance, 2 * std::sqrt(2.0), 1e-6);
+
+ // Test with a unit square.
+ const HPolyhedron unit_box = HPolyhedron::MakeUnitBox(3);
+ maximum_distance = unit_box.CalcMaximumDistanceViaSampling(&generator, max_num_samples);
+ EXPECT_NEAR(maximum_distance, 2 * std::sqrt(3.0), 1e-6);
+};
+
 // Compute the projection of a point onto a set that has no point in set
 // shortcut and no projection shortcut.
 GTEST_TEST(ConvexSetTest, GenericProjection) {