1
1
#include " drake/geometry/optimization/affine_subspace.h"
2
2
3
3
#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"
4
8
#include " drake/geometry/optimization/point.h"
9
+ #include " drake/geometry/optimization/vpolytope.h"
5
10
#include " drake/solvers/solve.h"
6
11
7
12
namespace drake {
@@ -34,8 +39,11 @@ AffineSubspace::AffineSubspace(const Eigen::Ref<const MatrixXd>& basis,
34
39
}
35
40
}
36
41
37
- AffineSubspace::AffineSubspace (const ConvexSet& set, double tol)
42
+ AffineSubspace::AffineSubspace (const ConvexSet& set, std::optional< double > tol)
38
43
: ConvexSet(0 , true ) {
44
+ if (tol) {
45
+ DRAKE_THROW_UNLESS (tol >= 0 );
46
+ }
39
47
// If the set is clearly a singleton, we can easily compute its affine hull.
40
48
const auto singleton_maybe = set.MaybeGetPoint ();
41
49
if (singleton_maybe.has_value ()) {
@@ -45,9 +53,26 @@ AffineSubspace::AffineSubspace(const ConvexSet& set, double tol)
45
53
return ;
46
54
}
47
55
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.
51
76
const auto translation_maybe = set.MaybeGetFeasiblePoint ();
52
77
if (!translation_maybe.has_value ()) {
53
78
throw std::runtime_error (
@@ -138,7 +163,7 @@ AffineSubspace::AffineSubspace(const ConvexSet& set, double tol)
138
163
result.get_solution_result ()));
139
164
}
140
165
141
- if (result.get_optimal_cost () < -tol) {
166
+ if (result.get_optimal_cost () < -tol. value () ) {
142
167
// x is in the affine hull, and is added to the basis.
143
168
VectorXd new_basis_vector = result.GetSolution (x);
144
169
basis.col (affine_dimension++) =
@@ -284,6 +309,11 @@ AffineSubspace::DoToShapeWithPose() const {
284
309
" ToShapeWithPose is not supported by AffineSubspace." );
285
310
}
286
311
312
+ std::unique_ptr<ConvexSet> AffineSubspace::DoAffineHullShortcut (
313
+ std::optional<double >) const {
314
+ return std::make_unique<AffineSubspace>(*this );
315
+ }
316
+
287
317
double AffineSubspace::DoCalcVolume () const {
288
318
if (AffineDimension () < ambient_dimension ()) {
289
319
// An AffineSubspace has zero volume if it has a lower affine dimension than
0 commit comments