Add geometry::optimization::MakeConvexSets (#19207)

This is a simple method to help initialize lists of ConvexSets.

Co-Authored-By: Jeremy Nimmer <[email protected]>

geometry/optimization/BUILD.bazel

@@ -160,6 +160,7 @@ drake_cc_googletest(
     deps = [
         ":convex_set",
         "//common/test_utilities:eigen_matrix_compare",
+        "//common/test_utilities:limit_malloc",
         "//solvers:clp_solver",
     ],
 )

geometry/optimization/convex_set.h

@@ -244,6 +244,22 @@ std::unique_ptr<ConvexSet> ConvexSetCloner(const ConvexSet& other) {
 instances. */
 typedef std::vector<copyable_unique_ptr<ConvexSet>> ConvexSets;
 
+/** Helper function that allows the ConvexSets to be initialized from arguments
+containing ConvexSet references, or unique_ptr<ConvexSet> instances, or any
+object that can be assigned to ConvexSets::value_type. */
+template <typename... Args>
+ConvexSets MakeConvexSets(Args&&... args) {
+  ConvexSets sets;
+  constexpr size_t N = sizeof...(args);
+  sets.resize(N);
+  // This is a "constexpr for" loop for 0 <= I < N.
+  auto args_tuple = std::forward_as_tuple(std::forward<Args>(args)...);
+  [&]<size_t... I>(std::integer_sequence<size_t, I...> &&) {
+    ((sets[I] = std::get<I>(std::move(args_tuple))), ...);
+  }(std::make_index_sequence<N>{});
+  return sets;
+}
+
 }  // namespace optimization
 }  // namespace geometry
 }  // namespace drake

geometry/optimization/test/convex_set_test.cc

@@ -4,6 +4,7 @@
 
 #include "drake/common/is_approx_equal_abstol.h"
 #include "drake/common/test_utilities/eigen_matrix_compare.h"
+#include "drake/common/test_utilities/limit_malloc.h"
 #include "drake/geometry/optimization/hpolyhedron.h"
 #include "drake/geometry/optimization/point.h"
 
@@ -14,6 +15,7 @@ namespace {
 
 using Eigen::Vector2d;
 using Eigen::Vector3d;
+using test::LimitMalloc;
 
 GTEST_TEST(ConvexSetsTest, BasicTest) {
   ConvexSets sets;
@@ -81,6 +83,57 @@ arrangement of boxes:
   EXPECT_FALSE(set_A.IntersectsWith(set_C));
   EXPECT_FALSE(set_C.IntersectsWith(set_A));
 }
+
+GTEST_TEST(MakeConvexSetsTest, Basic) {
+  HPolyhedron box = HPolyhedron::MakeUnitBox(2);
+  ConvexSets sets =
+      MakeConvexSets(box, box.Clone(), Point(Vector3d(1.0, 2.0, 3.0)));
+
+  EXPECT_EQ(sets.size(), 3);
+  EXPECT_EQ(sets[0]->ambient_dimension(), 2);
+  EXPECT_EQ(sets[1]->ambient_dimension(), 2);
+  EXPECT_EQ(sets[2]->ambient_dimension(), 3);
+}
+
+// A mutable lvalue reference is copied, not moved.
+GTEST_TEST(MakeConvexSetsTest, MutableLvalueReference) {
+  const HPolyhedron box = HPolyhedron::MakeUnitBox(2);
+  std::unique_ptr<ConvexSet> box_clone = box.Clone();
+  ConvexSets sets = MakeConvexSets(box_clone);
+  EXPECT_EQ(sets.size(), 1);
+  EXPECT_NE(box_clone.get(), nullptr);
+}
+
+// The amount of copying is as small as possible.
+GTEST_TEST(MakeConvexSetsTest, NoExtraCopying) {
+  const HPolyhedron box = HPolyhedron::MakeUnitBox(2);
+
+  // A `unique_ptr<ConvexSet>` is moved into place, no copies.
+  // The only allocation is the std::vector storage itself.
+  {
+    std::unique_ptr<ConvexSet> box1{box.Clone()};
+    std::unique_ptr<ConvexSet> box2{box.Clone()};
+    LimitMalloc guard({.max_num_allocations = 1});
+    MakeConvexSets(std::move(box1), std::move(box2));
+  }
+
+  // A `copyable_unique_ptr<ConvexSet>` is moved into place, no copies.
+  {
+    copyable_unique_ptr<ConvexSet> box1{box.Clone()};
+    copyable_unique_ptr<ConvexSet> box2{box.Clone()};
+    LimitMalloc guard({.max_num_allocations = 1});
+    MakeConvexSets(std::move(box1), std::move(box2));
+  }
+
+  // A `const ConvexSet&` is copied just once.
+  {
+    const int box_clone_num_allocs = 3;  // HPolyhedron, A_ , b_.
+    const int num = 1 + box_clone_num_allocs;
+    LimitMalloc guard({.max_num_allocations = num, .min_num_allocations = num});
+    MakeConvexSets(box);
+  }
+};
+
 }  // namespace
 
 }  // namespace optimization