hpp-spline
4.10.0
template based classes for creating and manipulating spline and bezier curves. Comes with extra options specific to end-effector trajectories in robotics.
|
Go to the documentation of this file.
9 #ifndef _CLASS_BEZIERCURVE
10 #define _CLASS_BEZIERCURVE
29 template <
typename Time = double,
typename Numeric =
Time, std::size_t Dim = 3,
bool Safe =
false,
30 typename Point = Eigen::Matrix<Numeric, Dim, 1> >
36 typedef std::vector<point_t, Eigen::aligned_allocator<point_t> >
t_point_t;
45 template <
typename In>
49 size_(std::distance(PointsBegin, PointsEnd)),
54 if (Safe && (
size_ < 1 ||
T_ <= 0.))
55 throw std::out_of_range(
"can't create bezier min bound is higher than max bound");
56 for (; it != PointsEnd; ++it) pts_.push_back(*it);
62 template <
typename In>
66 size_(std::distance(PointsBegin, PointsEnd)),
71 if (Safe && (
size_ < 1 ||
T_ <= 0.))
72 throw std::out_of_range(
"can't create bezier min bound is higher than max bound");
73 for (; it != PointsEnd; ++it) pts_.push_back(*it);
79 template <
typename In>
83 size_(std::distance(PointsBegin, PointsEnd)),
88 if (Safe && (
size_ < 1 ||
T_ <= 0.))
89 throw std::out_of_range(
"can't create bezier min bound is higher than max bound");
90 for (; it != PointsEnd; ++it) pts_.push_back(*it);
99 template <
typename In>
103 size_(std::distance(PointsBegin, PointsEnd) + 4),
106 if (Safe && (
size_ < 1 ||
T_ <= 0.))
107 throw std::out_of_range(
"can't create bezier min bound is higher than max bound");
108 t_point_t updatedList = add_constraints<In>(PointsBegin, PointsEnd, constraints);
109 for (
cit_point_t cit = updatedList.begin(); cit != updatedList.end(); ++cit) pts_.push_back(*cit);
128 if (Safe & !(0 <= t && t <=
T_))
throw std::out_of_range(
"can't evaluate bezier curve, out of range");
140 if (order == 0)
return *
this;
142 for (
typename t_point_t::const_iterator pit = pts_.begin(); pit != pts_.end() - 1; ++pit)
143 derived_wp.push_back((
num_t)
degree_ * (*(pit + 1) - (*pit)));
144 if (derived_wp.empty()) derived_wp.push_back(point_t::Zero(Dim));
153 if (order == 0)
return *
this;
156 point_t current_sum = point_t::Zero(Dim);
159 n_wp.push_back(current_sum);
160 for (
typename t_point_t::const_iterator pit = pts_.begin(); pit != pts_.end(); ++pit) {
162 n_wp.push_back(current_sum / new_degree);
186 point_t res = point_t::Zero(Dim);
187 typename t_point_t::const_iterator pts_it = pts_.begin();
190 res += cit->operator()(u) * (*pts_it);
199 typename t_point_t::const_iterator pts_it = pts_.begin();
204 point_t tmp = (*pts_it) * u_op;
206 for (
unsigned int i = 1; i <
degree_; i++, ++pts_it) {
209 tmp = (tmp + tn * bc * (*pts_it)) * u_op;
211 return (tmp + tn * u * (*pts_it)) *
mult_T_;
225 while (pts.size() > 1) {
240 if (u < 0 || u > 1)
throw std::out_of_range(
"In deCasteljau reduction : u is not in [0;1]");
241 if (pts.size() == 1)
return pts;
244 for (
cit_point_t cit = pts.begin(); cit != (pts.end() - 1); ++cit) {
245 new_pts.push_back((1 - u) * (*cit) + u * (*(cit + 1)));
256 if (t ==
T_)
throw std::runtime_error(
"can't split curve, interval range is equal to original curve");
258 const double u = t /
T_;
259 wps_first[0] = pts_.front();
260 wps_second[
degree_] = pts_.back();
263 while (casteljau_pts.size() > 1) {
265 wps_first[id] = casteljau_pts.front();
266 wps_second[
degree_ - id] = casteljau_pts.back();
272 return std::make_pair(c_first, c_second);
276 if (t1 < 0. || t1 >
T_ || t2 < 0. || t2 >
T_)
throw std::out_of_range(
"In Extract curve : times out of bounds");
278 if (t1 == 0.)
return split(t2).first;
279 if (t2 ==
T_)
return split(t1).second;
281 std::pair<bezier_curve_t, bezier_curve_t> c_split = this->
split(t1);
282 return c_split.second.split(t2 - t1).first;
286 template <
typename In>
289 point_t P0, P1, P2, P_n_2, P_n_1, PN;
291 PN = *(PointsEnd - 1);
301 for (In it = PointsBegin + 1; it != PointsEnd - 1; ++it) res.push_back(*it);
303 res.push_back(P_n_2);
304 res.push_back(P_n_1);
329 std::vector<point_t> ts;
330 ts.push_back(point_t::Zero(Dim));
335 #endif //_CLASS_BEZIERCURVE
bezier_curve(In PointsBegin, In PointsEnd)
Constructor.
Definition: bezier_curve.h:46
Numeric num_t
Definition: bezier_curve.h:34
virtual time_t max() const
Definition: bezier_curve.h:314
Time time_t
Definition: bezier_curve.h:33
static bezier_curve_t zero(const time_t T=1.)
Definition: bezier_curve.h:328
bezier_curve(In PointsBegin, In PointsEnd, const curve_constraints_t &constraints, const time_t T=1.)
Constructor This constructor will add 4 points (2 after the first one, 2 before the last one) to ensu...
Definition: bezier_curve.h:100
bezier_curve< Time, Numeric, Dim, Safe, Point > bezier_curve_t
Definition: bezier_curve.h:38
std::size_t size_
Definition: bezier_curve.h:320
virtual point_t derivate(const time_t t, const std::size_t order) const
Evaluates the derivative at order N of the curve. If the derivative is to be evaluated several times,...
Definition: bezier_curve.h:174
std::vector< point_t, Eigen::aligned_allocator< point_t > > t_point_t
Definition: bezier_curve.h:36
bezier_curve_t extract(const Numeric t1, const Numeric t2)
Definition: bezier_curve.h:275
bezier_curve_t compute_primitive(const std::size_t order) const
Computes the primitive of the curve at order N.
Definition: bezier_curve.h:152
t_point_t deCasteljauReduction(const t_point_t &pts, const Numeric u) const
deCasteljauReduction compute the de Casteljau's reduction of the given list of points at time t
Definition: bezier_curve.h:239
Point point_t
Definition: bezier_curve.h:32
point_t evalHorner(const Numeric t) const
Evaluates all Bernstein polynomes for a certain degree using horner's scheme.
Definition: bezier_curve.h:197
time_t mult_T_
Definition: bezier_curve.h:319
t_point_t::const_iterator cit_point_t
Definition: bezier_curve.h:37
std::vector< Bern< Numeric > > makeBernstein(const unsigned int n)
Computes all Bernstein polynomes for a certain degree.
Definition: bernstein.h:58
~bezier_curve()
Destructor.
Definition: bezier_curve.h:113
std::pair< bezier_curve_t, bezier_curve_t > split(const Numeric t)
split split the curve in 2 at time t
Definition: bezier_curve.h:255
point_t evalDeCasteljau(const Numeric t) const
evalDeCasteljau evaluate the curve value at time t using deCasteljau algorithm
Definition: bezier_curve.h:221
std::size_t degree_
Definition: bezier_curve.h:321
double Numeric
Definition: effector_spline.h:26
double Time
Definition: effector_spline.h:27
bezier_curve(In PointsBegin, In PointsEnd, const time_t T, const time_t mult_T)
Constructor.
Definition: bezier_curve.h:80
Definition: bernstein.h:39
interface for a Curve of arbitrary dimension.
Eigen::Matrix< Numeric, 3, 1 > Point
Definition: effector_spline.h:28
point_t evalBernstein(const Numeric t) const
Evaluates all Bernstein polynomes for a certain degree Warning: the horner scheme is about 100 times ...
Definition: bezier_curve.h:184
Definition: bezier_curve.h:31
curve_constraints< point_t > curve_constraints_t
Definition: bezier_curve.h:35
bezier_curve(In PointsBegin, In PointsEnd, const time_t T)
Constructor.
Definition: bezier_curve.h:63
Represents a curve of dimension Dim is Safe is false, no verification is made on the evaluation of th...
Definition: curve_abc.h:24
virtual time_t min() const
Definition: bezier_curve.h:313
struct to define constraints on start / end velocities and acceleration on a curve
std::vector< Bern< Numeric > > bernstein_
Definition: bezier_curve.h:322
time_t T_
Definition: bezier_curve.h:318
Definition: bernstein.h:20
const t_point_t & waypoints() const
Definition: bezier_curve.h:214
t_point_t deCasteljauReduction(const Numeric t) const
Definition: bezier_curve.h:231
bezier_curve_t compute_derivate(const std::size_t order) const
Computes the derivative curve at order N.
Definition: bezier_curve.h:139
virtual point_t operator()(const time_t t) const
Evaluation of the cubic spline at time t.
Definition: bezier_curve.h:127
Definition: curve_constraint.h:21