Skip to content

Commit 38e1049

Browse files
cohntRussTedrake
authored andcommitted
More efficient AffineHull computation for certain subclasses of ConvexSet. (RobotLocomotion#21828)
Includes shortcuts for AffineBall, AffineSubspace, CartesianProduct, Hyperellipsoid, Hyperrectangle, and VPolytope. (Note that Point, and sets which are obviously singletons, already had special shortcut logic.) The user-specified tolerance will now be forwarded to these shortcut methods, potentially taking a different role, depending on the type of the set. The function documentation includes details on exactly how each subclass uses that tolerance.
1 parent ccac33e commit 38e1049

15 files changed

+230
-14
lines changed

geometry/optimization/affine_ball.cc

+11
Original file line numberDiff line numberDiff line change
@@ -188,6 +188,17 @@ AffineBall::DoToShapeWithPose() const {
188188
"ToShapeWithPose is not yet supported by AffineBall.");
189189
}
190190

191+
std::unique_ptr<ConvexSet> AffineBall::DoAffineHullShortcut(
192+
std::optional<double> tol) const {
193+
Eigen::FullPivHouseholderQR<MatrixXd> qr(B_);
194+
if (tol) {
195+
qr.setThreshold(tol.value());
196+
}
197+
MatrixXd basis =
198+
qr.matrixQ() * MatrixXd::Identity(ambient_dimension(), qr.rank());
199+
return std::make_unique<AffineSubspace>(std::move(basis), center_);
200+
}
201+
191202
std::pair<VectorX<Variable>, std::vector<Binding<Constraint>>>
192203
AffineBall::DoAddPointInSetConstraints(
193204
MathematicalProgram* prog,

geometry/optimization/affine_ball.h

+3
Original file line numberDiff line numberDiff line change
@@ -163,6 +163,9 @@ class AffineBall final : public ConvexSet {
163163
std::pair<std::unique_ptr<Shape>, math::RigidTransformd> DoToShapeWithPose()
164164
const final;
165165

166+
std::unique_ptr<ConvexSet> DoAffineHullShortcut(
167+
std::optional<double> tol) const final;
168+
166169
void CheckInvariants() const;
167170

168171
double DoCalcVolume() const final;

geometry/optimization/affine_subspace.cc

+35-5
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,12 @@
11
#include "drake/geometry/optimization/affine_subspace.h"
22

33
#include "drake/common/is_approx_equal_abstol.h"
4+
#include "drake/geometry/optimization/affine_ball.h"
5+
#include "drake/geometry/optimization/cartesian_product.h"
6+
#include "drake/geometry/optimization/hyperellipsoid.h"
7+
#include "drake/geometry/optimization/hyperrectangle.h"
48
#include "drake/geometry/optimization/point.h"
9+
#include "drake/geometry/optimization/vpolytope.h"
510
#include "drake/solvers/solve.h"
611

712
namespace drake {
@@ -34,8 +39,11 @@ AffineSubspace::AffineSubspace(const Eigen::Ref<const MatrixXd>& basis,
3439
}
3540
}
3641

37-
AffineSubspace::AffineSubspace(const ConvexSet& set, double tol)
42+
AffineSubspace::AffineSubspace(const ConvexSet& set, std::optional<double> tol)
3843
: ConvexSet(0, true) {
44+
if (tol) {
45+
DRAKE_THROW_UNLESS(tol >= 0);
46+
}
3947
// If the set is clearly a singleton, we can easily compute its affine hull.
4048
const auto singleton_maybe = set.MaybeGetPoint();
4149
if (singleton_maybe.has_value()) {
@@ -45,9 +53,26 @@ AffineSubspace::AffineSubspace(const ConvexSet& set, double tol)
4553
return;
4654
}
4755

48-
// If the set is not clearly a singleton, we find a feasible point and
49-
// iteratively compute a basis of the affine hull. If no feasible point
50-
// exists, the set is empty, so we throw an error.
56+
// If the set is of a ConvexSet subclass that has an efficient algorithm for
57+
// computing the affine hull, we use it. Otherwise, we use the generic
58+
// iterative approach.
59+
std::unique_ptr<ConvexSet> shortcut = ConvexSet::AffineHullShortcut(set, tol);
60+
if (shortcut != nullptr) {
61+
// This downcast is per the post-condition of the AffineHullShortcut API.
62+
AffineSubspace* downcast = dynamic_cast<AffineSubspace*>(shortcut.get());
63+
DRAKE_THROW_UNLESS(downcast != nullptr);
64+
*this = std::move(*downcast);
65+
return;
66+
}
67+
68+
// If the set is not clearly a singleton and there's no obviously more
69+
// efficient algorithm based on the type of the set, we find a feasible point
70+
// and iteratively compute a basis of the affine hull. If the numerical
71+
// tolerance was not specified, we use a reasonable default.
72+
if (!tol) {
73+
tol = 1e-12;
74+
}
75+
// If no feasible point exists, the set is empty, so we throw an error.
5176
const auto translation_maybe = set.MaybeGetFeasiblePoint();
5277
if (!translation_maybe.has_value()) {
5378
throw std::runtime_error(
@@ -138,7 +163,7 @@ AffineSubspace::AffineSubspace(const ConvexSet& set, double tol)
138163
result.get_solution_result()));
139164
}
140165

141-
if (result.get_optimal_cost() < -tol) {
166+
if (result.get_optimal_cost() < -tol.value()) {
142167
// x is in the affine hull, and is added to the basis.
143168
VectorXd new_basis_vector = result.GetSolution(x);
144169
basis.col(affine_dimension++) =
@@ -284,6 +309,11 @@ AffineSubspace::DoToShapeWithPose() const {
284309
"ToShapeWithPose is not supported by AffineSubspace.");
285310
}
286311

312+
std::unique_ptr<ConvexSet> AffineSubspace::DoAffineHullShortcut(
313+
std::optional<double>) const {
314+
return std::make_unique<AffineSubspace>(*this);
315+
}
316+
287317
double AffineSubspace::DoCalcVolume() const {
288318
if (AffineDimension() < ambient_dimension()) {
289319
// An AffineSubspace has zero volume if it has a lower affine dimension than

geometry/optimization/affine_subspace.h

+33-8
Original file line numberDiff line numberDiff line change
@@ -43,19 +43,41 @@ class AffineSubspace final : public ConvexSet {
4343
const Eigen::Ref<const Eigen::VectorXd>& translation);
4444

4545
/** Constructs an affine subspace as the affine hull of another convex set.
46-
This is done by finding a feasible point in the set, and then iteratively
47-
computing feasible vectors until we have a basis that spans the set. If you
48-
pass in a convex set whose points are matrix-valued (e.g. a Spectrahedron),
49-
then the affine subspace will work over a flattened representation of those
50-
coordinates. (So a Spectrahedron with n-by-n matrices will output an
51-
AffineSubspace with ambient dimension (n * (n+1)) / 2.)
46+
The generic approach is to find a feasible point in the set, and then
47+
iteratively compute feasible vectors until we have a basis that spans the set.
48+
If you pass in a convex set whose points are matrix-valued (e.g. a
49+
Spectrahedron), then the affine subspace will work over a flattened
50+
representation of those coordinates. (So a Spectrahedron with n-by-n matrices
51+
will output an AffineSubspace with ambient dimension (n * (n+1)) / 2.)
5252
5353
`tol` sets the numerical precision of the computation. For each dimension, a
5454
pair of feasible points are constructed, so as to maximize the displacement in
5555
that dimension. If their displacement along that dimension is larger than tol,
5656
then the vector connecting the points is added as a basis vector.
57-
@pre !set.IsEmpty() */
58-
explicit AffineSubspace(const ConvexSet& set, double tol = 1e-12);
57+
58+
@throws std::exception if `set` is empty.
59+
@throws std::exception if `tol < 0`.
60+
61+
For several subclasses of ConvexSet, there is a closed-form computation (or
62+
more efficient numerical computation) that is preferred.
63+
- AffineBall: Can be computed via a rank-revealing decomposition; `tol` is
64+
used as the numerical tolerance for the rank of the matrix. Pass
65+
`std::nullopt` for `tol` to use Eigen's automatic tolerance computation.
66+
- AffineSubspace: Equivalent to the copy-constructor; `tol` is ignored.
67+
- CartesianProduct: Can compute the affine hull of each factor individually;
68+
`tol` is propagated to the constituent calls. (This is not done if the
69+
Cartesian product has an associated affine transformation.)
70+
- Hyperellipsoid: Always equal to the whole ambient space; `tol` is ignored.
71+
- Hyperrectangle: Can be computed in closed-form; `tol` has the same meaning
72+
as in the generic affine hull computation.
73+
- Point: Can be computed in closed-form; `tol` is ignored. This also
74+
encompasses sets which are obviously a singleton point, as determined via
75+
MaybeGetPoint.
76+
- VPolytope: Can be computed via a singular value decomposition; `tol` is
77+
used as the numerical tolerance for the rank of the matrix. Pass
78+
`std::nullopt` for `tol` to use Eigen's automatic tolerance computation. */
79+
explicit AffineSubspace(const ConvexSet& set,
80+
std::optional<double> tol = std::nullopt);
5981

6082
~AffineSubspace() final;
6183

@@ -160,6 +182,9 @@ class AffineSubspace final : public ConvexSet {
160182
std::pair<std::unique_ptr<Shape>, math::RigidTransformd> DoToShapeWithPose()
161183
const final;
162184

185+
std::unique_ptr<ConvexSet> DoAffineHullShortcut(
186+
std::optional<double>) const final;
187+
163188
double DoCalcVolume() const final;
164189

165190
std::vector<std::optional<double>> DoProjectionShortcut(

geometry/optimization/cartesian_product.cc

+33
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,7 @@
44

55
#include <fmt/format.h>
66

7+
#include "drake/geometry/optimization/affine_subspace.h"
78
#include "drake/geometry/optimization/hpolyhedron.h"
89
#include "drake/geometry/optimization/hyperellipsoid.h"
910
#include "drake/solvers/solve.h"
@@ -362,6 +363,38 @@ CartesianProduct::DoToShapeWithPose() const {
362363
"ToShapeWithPose is not implemented yet for CartesianProduct.");
363364
}
364365

366+
std::unique_ptr<ConvexSet> CartesianProduct::DoAffineHullShortcut(
367+
std::optional<double> tol) const {
368+
// TODO(cohnt): Support affine transformations of Cartesian products. For now,
369+
// we just return std::nullopt and use the generic affine hull computation.
370+
if (A_ != std::nullopt || b_ != std::nullopt) {
371+
return nullptr;
372+
}
373+
374+
// The basis will be a block diagonal matrix, whose blocks correspond to the
375+
// bases of the affine subspace of each factor. Not all blocks will be square,
376+
// and some of the columns on the right will be skipped, since the affine hull
377+
// may be a proper subspace.
378+
MatrixXd basis = MatrixXd::Zero(ambient_dimension(), ambient_dimension());
379+
// The translation will be a vector, concatenating all of the translations of
380+
// each factor. Zero-initialization is not needed, since all entries will be
381+
// overwritten in the following loop.
382+
VectorXd translation(ambient_dimension());
383+
int current_dimension = 0;
384+
int num_basis_vectors = 0;
385+
for (int i = 0; i < num_factors(); ++i) {
386+
AffineSubspace a(factor(i), tol);
387+
basis.block(current_dimension, num_basis_vectors, a.ambient_dimension(),
388+
a.AffineDimension()) = a.basis();
389+
translation.segment(current_dimension, a.ambient_dimension()) =
390+
a.translation();
391+
current_dimension += a.ambient_dimension();
392+
num_basis_vectors += a.AffineDimension();
393+
}
394+
return std::make_unique<AffineSubspace>(basis.leftCols(num_basis_vectors),
395+
std::move(translation));
396+
}
397+
365398
double CartesianProduct::DoCalcVolume() const {
366399
DRAKE_DEMAND(sets_.size() > 0);
367400
double volume = 1.0;

geometry/optimization/cartesian_product.h

+3
Original file line numberDiff line numberDiff line change
@@ -135,6 +135,9 @@ class CartesianProduct final : public ConvexSet {
135135
std::pair<std::unique_ptr<Shape>, math::RigidTransformd> DoToShapeWithPose()
136136
const final;
137137

138+
std::unique_ptr<ConvexSet> DoAffineHullShortcut(
139+
std::optional<double> tol) const final;
140+
138141
// The member variables are not const in order to support move semantics.
139142
ConvexSets sets_{};
140143

geometry/optimization/convex_set.cc

+10
Original file line numberDiff line numberDiff line change
@@ -444,6 +444,16 @@ double ConvexSet::DoCalcVolume() const {
444444
NiceTypeName::Get(*this)));
445445
}
446446

447+
std::unique_ptr<ConvexSet> ConvexSet::AffineHullShortcut(
448+
const ConvexSet& self, std::optional<double> tol) {
449+
return self.DoAffineHullShortcut(tol);
450+
}
451+
452+
std::unique_ptr<ConvexSet> ConvexSet::DoAffineHullShortcut(
453+
std::optional<double>) const {
454+
return nullptr;
455+
}
456+
447457
} // namespace optimization
448458
} // namespace geometry
449459
} // namespace drake

geometry/optimization/convex_set.h

+16
Original file line numberDiff line numberDiff line change
@@ -453,6 +453,22 @@ class ConvexSet {
453453
solvers::MathematicalProgram* prog, const ConvexSet& set,
454454
std::vector<solvers::Binding<solvers::Constraint>>* constraints) const;
455455

456+
/** When there is a more efficient strategy to compute the affine hull of this
457+
set, returns affine hull as an AffineSubspace. When no efficient conversion
458+
exists, returns null. The default base class implementation returns null. This
459+
method is used by the AffineSubspace constructor to short-circuit the generic
460+
iterative approach. (This function is static to allow calling it from the
461+
AffineSubspace constructor, but is conceptially a normal member function.)
462+
The return type is ConvexSet to avoid a forward declaration; any non-null
463+
result must always have the AffineSubspace as its runtime type. */
464+
static std::unique_ptr<ConvexSet> AffineHullShortcut(
465+
const ConvexSet& self, std::optional<double> tol);
466+
467+
/** NVI implementation of DoAffineHullShortcut, which trivially returns null.
468+
Derived classes that have efficient algorithms should override this method. */
469+
virtual std::unique_ptr<ConvexSet> DoAffineHullShortcut(
470+
std::optional<double> tol) const;
471+
456472
private:
457473
/** Generic implementation for IsBounded() -- applicable for all convex sets.
458474
@pre ambient_dimension() >= 0 */

geometry/optimization/hyperellipsoid.cc

+11
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@
66
#include <Eigen/Eigenvalues>
77
#include <fmt/format.h>
88

9+
#include "drake/geometry/optimization/affine_subspace.h"
910
#include "drake/math/matrix_util.h"
1011
#include "drake/math/rotation_matrix.h"
1112
#include "drake/solvers/choose_best_solver.h"
@@ -396,6 +397,16 @@ void Hyperellipsoid::ImplementGeometry(const Sphere& sphere, void* data) {
396397
*A = Eigen::Matrix3d::Identity() / sphere.radius();
397398
}
398399

400+
std::unique_ptr<ConvexSet> Hyperellipsoid::DoAffineHullShortcut(
401+
std::optional<double> tol) const {
402+
// Hyperellipsoids are always positive volume, so we can trivially construct
403+
// their affine hull as the whole vector space.
404+
unused(tol);
405+
const int n = ambient_dimension();
406+
return std::make_unique<AffineSubspace>(Eigen::MatrixXd::Identity(n, n),
407+
Eigen::VectorXd::Zero(n));
408+
}
409+
399410
} // namespace optimization
400411
} // namespace geometry
401412
} // namespace drake

geometry/optimization/hyperellipsoid.h

+3
Original file line numberDiff line numberDiff line change
@@ -177,6 +177,9 @@ class Hyperellipsoid final : public ConvexSet, private ShapeReifier {
177177
std::pair<std::unique_ptr<Shape>, math::RigidTransformd> DoToShapeWithPose()
178178
const final;
179179

180+
std::unique_ptr<ConvexSet> DoAffineHullShortcut(
181+
std::optional<double> tol) const final;
182+
180183
double DoCalcVolume() const final;
181184

182185
void CheckInvariants() const;

geometry/optimization/hyperrectangle.cc

+19
Original file line numberDiff line numberDiff line change
@@ -13,6 +13,7 @@
1313
#include <Eigen/Eigenvalues>
1414
#include <fmt/format.h>
1515

16+
#include "drake/geometry/optimization/affine_subspace.h"
1617
#include "drake/geometry/optimization/convex_set.h"
1718
#include "drake/solvers/solve.h"
1819

@@ -208,6 +209,24 @@ Hyperrectangle::DoToShapeWithPose() const {
208209
math::RigidTransformd(Center()));
209210
}
210211

212+
std::unique_ptr<ConvexSet> Hyperrectangle::DoAffineHullShortcut(
213+
std::optional<double> tol) const {
214+
MatrixXd basis = MatrixXd::Zero(ambient_dimension(), ambient_dimension());
215+
int current_dimension = 0;
216+
int num_basis_vectors = 0;
217+
for (int i = 0; i < ambient_dimension(); ++i) {
218+
// If the numerical tolerance was not specified, we use a reasonable
219+
// default.
220+
if (ub_[i] - lb_[i] > (tol ? tol.value() : 1e-12)) {
221+
basis(current_dimension, num_basis_vectors) = 1;
222+
++num_basis_vectors;
223+
}
224+
++current_dimension;
225+
}
226+
return std::make_unique<AffineSubspace>(basis.leftCols(num_basis_vectors),
227+
lb_);
228+
}
229+
211230
double Hyperrectangle::DoCalcVolume() const {
212231
return (ub_ - lb_).prod();
213232
}

geometry/optimization/hyperrectangle.h

+3
Original file line numberDiff line numberDiff line change
@@ -111,6 +111,9 @@ class Hyperrectangle final : public ConvexSet {
111111
std::pair<std::unique_ptr<Shape>, math::RigidTransformd> DoToShapeWithPose()
112112
const final;
113113

114+
std::unique_ptr<ConvexSet> DoAffineHullShortcut(
115+
std::optional<double> tol) const final;
116+
114117
// TODO(Alexandre.Amice) Implement DoProjectionShortcut.
115118

116119
double DoCalcVolume() const final;

0 commit comments

Comments
 (0)