-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
bezier_curve.h
144 lines (115 loc) · 5.44 KB
/
bezier_curve.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
141
142
143
144
#pragma once
#include <memory>
#include <optional>
#include "drake/common/eigen_types.h"
#include "drake/common/trajectories/trajectory.h"
namespace drake {
namespace trajectories {
/** A Bézier curve is defined by a set of control points p₀ through pₙ, where n
is called the order of the curve (n = 1 for linear, 2 for quadratic, 3 for
cubic, etc.). The first and last control points are always the endpoints of
the curve; however, the intermediate control points (if any) generally do not
lie on the curve, but the curve is guaranteed to stay within the convex hull
of the control points.
See also BsplineTrajectory. A B-spline can be thought of as a composition of
overlapping Bézier curves (where each evaluation only depends on a local
subset of the control points). In contrast, evaluating a Bézier curve will use
all of the control points.
@tparam_default_scalar
*/
template <typename T>
class BezierCurve final : public trajectories::Trajectory<T> {
public:
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(BezierCurve);
/** Default initializer. Constructs an empty Bézier curve over the interval
t ∈ [0, 1].*/
BezierCurve() : BezierCurve<T>(0, 1, MatrixX<T>()) {}
/** Constructs a Bézier curve over the interval t ∈ [`start_time`,
`end_time`] with control points defined in the columns of `control_points`.
@pre end_time >= start_time.
*/
BezierCurve(double start_time, double end_time,
const Eigen::Ref<const MatrixX<T>>& control_points);
// TODO(russt): Add support for MatrixX control points, but only if we have a
// use case for it.
virtual ~BezierCurve() = default;
/** Returns the order of the curve (1 for linear, 2 for quadratic, etc.). */
int order() const { return control_points_.cols() - 1; }
/** Returns the value of the ith basis function of `order` (1 for linear, 2
for quadratic, etc) evaluated at `time`. The default value for the optional
argument `order` is the `order()` of `this`. */
T BernsteinBasis(int i, const T& time,
std::optional<int> order = std::nullopt) const;
/** Returns a const reference to the control points which define the curve. */
const MatrixX<T>& control_points() const { return control_points_; }
/** Supports writing optimizations using the control points as decision
variables. This method returns the matrix, `M`, defining the control points
of the `order` derivative in the form:
<pre>
derivative.control_points() = this.control_points() * M
</pre>
For instance, since we have
<pre>
derivative.control_points().col(k) = this.control_points() * M.col(k),
</pre>
constraining the kth control point of the `n`th derivative to be in [ub,
lb], could be done with:
@code
auto M = curve.AsLinearInControlPoints(n);
for (int i=0; i<curve.rows(); ++i) {
auto c = std::make_shared<solvers::LinearConstraint>(
M.col(k).transpose(), Vector1d(lb(i)), Vector1d(ub(i)));
prog.AddConstraint(c, curve.row(i).transpose());
}
@endcode
Iterating over the rows of the control points is the natural sparsity pattern
here (since `M` is the same for all rows). For instance, we also have
<pre>
derivative.control_points().row(k).T = M.T * this.control_points().row(k).T,
</pre> or
<pre>
vec(derivative.control_points().T) = blockMT * vec(this.control_points().T),
blockMT = [ M.T, 0, .... 0 ]
[ 0, M.T, 0, ... ]
[ ... ]
[ ... , 0, M.T ].
</pre>
@pre derivative_order >= 0. */
Eigen::SparseMatrix<double> AsLinearInControlPoints(
int derivative_order = 1) const;
// Required methods for trajectories::Trajectory interface.
std::unique_ptr<trajectories::Trajectory<T>> Clone() const override;
/** Evaluates the curve at the given time.
@warning If t does not lie in the range [start_time(), end_time()], the
trajectory will silently be evaluated at the closest valid value of
time to `time`. For example, `value(-1)` will return `value(0)` for
a trajectory defined over [0, 1]. */
MatrixX<T> value(const T& time) const override;
/** Extracts the expanded underlying polynomial expression of this curve in
terms of variable `time`. */
VectorX<symbolic::Expression> GetExpression(
symbolic::Variable time = symbolic::Variable("t")) const;
/** Increases the order of the curve by 1. A Bézier curve of order n can be
converted into a Bézier curve of order n + 1 with the same shape. The
control points of `this` are modified to obtain the equivalent curve. */
void ElevateOrder();
Eigen::Index rows() const override { return control_points_.rows(); }
Eigen::Index cols() const override { return 1; }
T start_time() const override { return start_time_; }
T end_time() const override { return end_time_; }
private:
/* Calculates the control points of the derivative curve. */
MatrixX<T> CalcDerivativePoints(int derivative_order) const;
bool do_has_derivative() const override { return true; }
MatrixX<T> DoEvalDerivative(const T& t, int derivative_order) const override;
std::unique_ptr<trajectories::Trajectory<T>> DoMakeDerivative(
int derivative_order) const override;
VectorX<T> EvaluateT(const T& time) const;
double start_time_{};
double end_time_{};
MatrixX<T> control_points_;
};
} // namespace trajectories
} // namespace drake
DRAKE_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS(
class drake::trajectories::BezierCurve)