-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
bspline_trajectory.h
122 lines (98 loc) · 4.94 KB
/
bspline_trajectory.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
#pragma once
#include <memory>
#include <vector>
#include "drake/common/drake_bool.h"
#include "drake/common/drake_copyable.h"
#include "drake/common/eigen_types.h"
#include "drake/common/trajectories/trajectory.h"
#include "drake/math/bspline_basis.h"
namespace drake {
namespace trajectories {
/** Represents a B-spline curve using a given `basis` with ordered
`control_points` such that each control point is a matrix in ℝʳᵒʷˢ ˣ ᶜᵒˡˢ.
@see math::BsplineBasis */
template <typename T>
class BsplineTrajectory final : public trajectories::Trajectory<T> {
public:
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(BsplineTrajectory);
/** Constructs a B-spline trajectory with the given `basis` and
`control_points`.
@pre control_points.size() == basis.num_basis_functions() */
BsplineTrajectory(math::BsplineBasis<T> basis,
std::vector<MatrixX<T>> control_points);
#ifdef DRAKE_DOXYGEN_CXX
/** Constructs a T-valued B-spline trajectory from a double-valued `basis` and
T-valued `control_points`.
@pre control_points.size() == basis.num_basis_functions() */
BsplineTrajectory(math::BsplineBasis<double> basis,
std::vector<MatrixX<T>> control_points);
#else
template <typename U = T>
BsplineTrajectory(math::BsplineBasis<double> basis,
std::vector<MatrixX<T>> control_points,
typename std::enable_if_t<!std::is_same_v<U, double>>* = {})
: BsplineTrajectory(math::BsplineBasis<T>(basis), control_points) {}
#endif
virtual ~BsplineTrajectory() = default;
// Required methods for trajectories::Trajectory interface.
std::unique_ptr<trajectories::Trajectory<T>> Clone() const override;
/** Evaluates the BsplineTrajectory at the given time t.
@param t The time at which to evaluate the %BsplineTrajectory.
@return The matrix of evaluated values.
@pre If T == symbolic::Expression, `t.is_constant()` must be true.
@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 t. For example, `value(-1)` will return
`value(0)` for a trajectory defined over [0, 1]. */
MatrixX<T> value(const T& time) const override;
Eigen::Index rows() const override { return control_points()[0].rows(); }
Eigen::Index cols() const override { return control_points()[0].cols(); }
T start_time() const override { return basis_.initial_parameter_value(); }
T end_time() const override { return basis_.final_parameter_value(); }
// Other methods
/** Returns the number of control points in this curve. */
int num_control_points() const { return basis_.num_basis_functions(); }
/** Returns the control points of this curve. */
const std::vector<MatrixX<T>>& control_points() const {
return control_points_;
}
/** Returns this->value(this->start_time()) */
MatrixX<T> InitialValue() const;
/** Returns this->value(this->end_time()) */
MatrixX<T> FinalValue() const;
/** Returns the basis of this curve. */
const math::BsplineBasis<T>& basis() const { return basis_; }
/** Adds new knots at the specified `additional_knots` without changing the
behavior of the trajectory. The basis and control points of the trajectory are
adjusted such that it produces the same value for any valid time before and
after this method is called. The resulting trajectory is guaranteed to have
the same level of continuity as the original, even if knot values are
duplicated. Note that `additional_knots` need not be sorted.
@pre start_time() <= t <= end_time() for all t in `additional_knots` */
void InsertKnots(const std::vector<T>& additional_knots);
/** Returns a new BsplineTrajectory that uses the same basis as `this`, and
whose control points are the result of calling `select(point)` on each `point`
in `this->control_points()`.*/
BsplineTrajectory<T> CopyWithSelector(
const std::function<MatrixX<T>(const MatrixX<T>&)>& select) const;
/** Returns a new BsplineTrajectory that uses the same basis as `this`, and
whose control points are the result of calling
`point.block(start_row, start_col, block_rows, block_cols)` on each `point`
in `this->control_points()`.*/
BsplineTrajectory<T> CopyBlock(int start_row, int start_col,
int block_rows, int block_cols) const;
/** Returns a new BsplineTrajectory that uses the same basis as `this`, and
whose control points are the result of calling `point.head(n)` on each `point`
in `this->control_points()`.
@pre this->cols() == 1
@pre control_points()[0].head(n) must be a valid operation. */
BsplineTrajectory<T> CopyHead(int n) const;
boolean<T> operator==(const BsplineTrajectory<T>& other) const;
private:
std::unique_ptr<trajectories::Trajectory<T>> DoMakeDerivative(
int derivative_order) const override;
math::BsplineBasis<T> basis_;
std::vector<MatrixX<T>> control_points_;
};
} // namespace trajectories
} // namespace drake