geometry/opt: Add HPolyhedron::CartesianPower() (RobotLocomotion#15371)

rpoyner-tri · Jul 19, 2021 · 1f2b1ba · 1f2b1ba
1 parent 7174ad6
commit 1f2b1ba
Show file tree

Hide file tree

Showing 3 changed files with 57 additions and 1 deletion.
diff --git a/geometry/optimization/hpolyhedron.cc b/geometry/optimization/hpolyhedron.cc
@@ -117,6 +117,15 @@ VectorXd HPolyhedron::ChebyshevCenter() const {
   return result.GetSolution(x);
 }
 
+HPolyhedron HPolyhedron::CartesianPower(int n) const {
+  MatrixXd A_power = MatrixXd::Zero(n * A_.rows(), n * A_.cols());
+  for (int i{0}; i < n; ++i) {
+    A_power.block(i * A_.rows(), i * A_.cols(), A_.rows(), A_.cols()) = A_;
+  }
+  VectorXd b_power = b_.replicate(n, 1);
+  return {A_power, b_power};
+}
+
 HPolyhedron HPolyhedron::MakeBox(const Eigen::Ref<const VectorXd>& lb,
                                  const Eigen::Ref<const VectorXd>& ub) {
   DRAKE_DEMAND(lb.size() == ub.size());

diff --git a/geometry/optimization/hpolyhedron.h b/geometry/optimization/hpolyhedron.h
@@ -92,8 +92,13 @@ class HPolyhedron final : public ConvexSet {
   */
   Eigen::VectorXd ChebyshevCenter() const;
 
+  /** Returns the `n`-ary Cartesian power of `this`.
+  The n-ary Cartesian power of a set H is the set H ⨉ H ⨉ ... ⨉ H, where H is
+  repeated n times. */
+  HPolyhedron CartesianPower(int n) const;
+
   /** Constructs a polyhedron as an axis-aligned box from the lower and upper
-   * corners. */
+  corners. */
   static HPolyhedron MakeBox(const Eigen::Ref<const Eigen::VectorXd>& lb,
                              const Eigen::Ref<const Eigen::VectorXd>& ub);
 

diff --git a/geometry/optimization/test/hpolyhedron_test.cc b/geometry/optimization/test/hpolyhedron_test.cc
@@ -318,6 +318,48 @@ GTEST_TEST(HPolyhedronTest, IsBounded4) {
   EXPECT_FALSE(H.IsBounded());
 }
 
+GTEST_TEST(HPolyhedronTest, CartesianPowerTest) {
+  // First test the concept. If x ∈ H, then [x; x]  ∈ H x H and
+  // [x; x; x]  ∈ H x H x H.
+  MatrixXd A{4, 2};
+  A << MatrixXd::Identity(2, 2), -MatrixXd::Identity(2, 2);
+  VectorXd b = VectorXd::Ones(4);
+  HPolyhedron H(A, b);
+  VectorXd x = VectorXd::Zero(2);
+  EXPECT_TRUE(H.PointInSet(x));
+  EXPECT_TRUE(H.CartesianPower(2).PointInSet((VectorXd(4) << x, x).finished()));
+  EXPECT_TRUE(
+      H.CartesianPower(3).PointInSet((VectorXd(6) << x, x, x).finished()));
+
+  // Now test the HPolyhedron-specific behavior.
+  MatrixXd A_1{2, 3};
+  MatrixXd A_2{4, 6};
+  MatrixXd A_3{6, 9};
+  VectorXd b_1{2};
+  VectorXd b_2{4};
+  VectorXd b_3{6};
+  MatrixXd zero = MatrixXd::Zero(2, 3);
+  // clang-format off
+  A_1 << 1, 2, 3,
+         4, 5, 6;
+  b_1 << 1, 2;
+  A_2 <<  A_1, zero,
+         zero,  A_1;
+  b_2 << b_1, b_1;
+  A_3 <<  A_1, zero, zero,
+         zero,  A_1, zero,
+         zero, zero,  A_1;
+  b_3 << b_1, b_1, b_1;
+  // clang-format on
+  HPolyhedron H_1(A_1, b_1);
+  HPolyhedron H_2 = H_1.CartesianPower(2);
+  HPolyhedron H_3 = H_1.CartesianPower(3);
+  EXPECT_TRUE(CompareMatrices(H_2.A(), A_2));
+  EXPECT_TRUE(CompareMatrices(H_2.b(), b_2));
+  EXPECT_TRUE(CompareMatrices(H_3.A(), A_3));
+  EXPECT_TRUE(CompareMatrices(H_3.b(), b_3));
+}
+
 
 }  // namespace optimization
 }  // namespace geometry