diff --git a/geometry/optimization/hpolyhedron.cc b/geometry/optimization/hpolyhedron.cc index 9137e8d21563..556a474b27b8 100644 --- 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& lb, const Eigen::Ref& ub) { DRAKE_DEMAND(lb.size() == ub.size()); diff --git a/geometry/optimization/hpolyhedron.h b/geometry/optimization/hpolyhedron.h index 10263aacce1d..49d5c9b45f9f 100644 --- 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& lb, const Eigen::Ref& ub); diff --git a/geometry/optimization/test/hpolyhedron_test.cc b/geometry/optimization/test/hpolyhedron_test.cc index b428706ecedd..c7fb9f2dd95d 100644 --- 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