-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
hyperellipsoid.h
140 lines (109 loc) · 5.54 KB
/
hyperellipsoid.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
#pragma once
#include <memory>
#include <optional>
#include <utility>
#include <vector>
#include "drake/geometry/optimization/convex_set.h"
namespace drake {
namespace geometry {
namespace optimization {
/** Implements an ellipsoidal convex set represented by the quadratic form `{x
| (x-center)ᵀAᵀA(x-center) ≤ 1}`. Note that `A` need not be square; we require
only that the matrix AᵀA is positive semi-definite.
Compare this with an alternative (very useful) parameterization of the
ellipsoid: `{Bu + center | |u|₂ ≤ 1}`, which is an affine scaling of the unit
ball. This is related to the quadratic form by `B = A⁻¹`, when `A` is
invertible, but the quadratic form can also represent unbounded sets.
Note: the name Hyperellipsoid was taken here to avoid conflicting with
geometry::Ellipsoid and to distinguish that this class supports N dimensions.
@ingroup geometry_optimization */
class Hyperellipsoid final : public ConvexSet {
public:
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Hyperellipsoid)
/** Constructs a default (zero-dimensional) set. */
Hyperellipsoid();
/** Constructs the ellipsoid.
@pre A.cols() == center.size(). */
Hyperellipsoid(const Eigen::Ref<const Eigen::MatrixXd>& A,
const Eigen::Ref<const Eigen::VectorXd>& center);
/** Constructs a Hyperellipsoid from a SceneGraph geometry and pose in the
`reference_frame` frame, obtained via the QueryObject. If `reference_frame`
frame is std::nullopt, then it will be expressed in the world frame.
@throws std::exception if geometry_id does not represent a shape that can be
described as an Hyperellipsoid. */
Hyperellipsoid(const QueryObject<double>& query_object,
GeometryId geometry_id,
std::optional<FrameId> reference_frame = std::nullopt);
~Hyperellipsoid() final;
/** Returns the quadratic form matrix A. */
const Eigen::MatrixXd& A() const { return A_; }
/** Returns the center of the ellipsoid. */
const Eigen::VectorXd& center() const { return center_; }
/** Returns the volume of the hyperellipsoid (in Euclidean space). */
double Volume() const;
/** Computes the smallest uniform scaling of this ellipsoid for which it still
intersects @p other. √ minₓ (x-center)ᵀAᵀA(x-center) s.t. x ∈ other. Note
that if center ∈ other, then we expect scaling = 0 and x = center (up to
precision).
@pre `other` must have the same ambient_dimension as this.
@returns the minimal scaling and the witness point, x, on other.
@throws std::exception if ambient_dimension() == 0 */
std::pair<double, Eigen::VectorXd> MinimumUniformScalingToTouch(
const ConvexSet& other) const;
/** Constructs a Ellipsoid shape description of this set. Note that the
choice of ellipsoid is not unique. This method chooses to order the Ellipsoid
parameters a ≥ b ≥ c.
@throws std::exception if ambient_dimension() != 3
@throws std::exception if A is not invertible (has any eigenvalue less than
sqrt(1e-12)). This tolerance is not carefully tuned (yet). We use Eigen's
SelfAdjointEigenSolver to take the eigenvalues of AᵀA; this solver is listed
as having accuracy "Good":
https://eigen.tuxfamily.org/dox/group__TopicLinearAlgebraDecompositions.html
but how does that translate into a numerical precision? */
using ConvexSet::ToShapeWithPose;
/** Constructs the an axis-aligned Hyperellipsoid with the implicit form
(x₀-c₀)²/r₀² + (x₁-c₁)²/r₁² + ... + (x_N - c_N)²/r_N² ≤ 1, where c is
shorthand for `center` and r is shorthand for `radius`. */
static Hyperellipsoid MakeAxisAligned(
const Eigen::Ref<const Eigen::VectorXd>& radius,
const Eigen::Ref<const Eigen::VectorXd>& center);
/** Constructs a hypersphere with `radius` and `center`. */
static Hyperellipsoid MakeHypersphere(
double radius, const Eigen::Ref<const Eigen::VectorXd>& center);
/** Constructs the L₂-norm unit ball in `dim` dimensions, {x | |x|₂ <= 1 }. */
static Hyperellipsoid MakeUnitBall(int dim);
private:
std::unique_ptr<ConvexSet> DoClone() const final;
bool DoIsBounded() const final;
// N.B. No need to override DoMaybeGetPoint here.
bool DoPointInSet(const Eigen::Ref<const Eigen::VectorXd>& x,
double tol) const final;
void DoAddPointInSetConstraints(
solvers::MathematicalProgram* prog,
const Eigen::Ref<const solvers::VectorXDecisionVariable>& vars)
const final;
std::vector<solvers::Binding<solvers::Constraint>>
DoAddPointInNonnegativeScalingConstraints(
solvers::MathematicalProgram* prog,
const Eigen::Ref<const solvers::VectorXDecisionVariable>& x,
const symbolic::Variable& t) const final;
std::vector<solvers::Binding<solvers::Constraint>>
DoAddPointInNonnegativeScalingConstraints(
solvers::MathematicalProgram* prog,
const Eigen::Ref<const Eigen::MatrixXd>& A_x,
const Eigen::Ref<const Eigen::VectorXd>& b,
const Eigen::Ref<const Eigen::VectorXd>& c, double d,
const Eigen::Ref<const solvers::VectorXDecisionVariable>& x,
const Eigen::Ref<const solvers::VectorXDecisionVariable>& t) const final;
std::pair<std::unique_ptr<Shape>, math::RigidTransformd> DoToShapeWithPose()
const final;
// Implement support shapes for the ShapeReifier interface.
using ShapeReifier::ImplementGeometry;
void ImplementGeometry(const Ellipsoid& ellipsoid, void* data) final;
void ImplementGeometry(const Sphere& sphere, void* data) final;
Eigen::MatrixXd A_{};
Eigen::VectorXd center_{};
};
} // namespace optimization
} // namespace geometry
} // namespace drake