8 #ifndef _CLASS_PIECEWISE_CURVE 9 #define _CLASS_PIECEWISE_CURVE 11 #include <boost/serialization/vector.hpp> 12 #include <boost/smart_ptr/shared_ptr.hpp> 31 template <
typename Time = double,
typename Numeric =
Time,
bool Safe =
false,
32 typename Point = Eigen::Matrix<Numeric, Eigen::Dynamic, 1>,
33 typename Point_derivate =
Point,
35 curve_abc<Time, Numeric, Safe, Point, Point_derivate> >
36 struct piecewise_curve
37 :
public curve_abc<Time, Numeric, Safe, Point, Point_derivate> {
40 typedef std::vector<point_t, Eigen::aligned_allocator<point_t> >
t_point_t;
42 Eigen::aligned_allocator<point_derivate_t> >
55 typename CurveType::curve_derivate_t>
57 typedef boost::shared_ptr<typename piecewise_curve_derivate_t::curve_t>
77 for (
typename t_curve_ptr_t::const_iterator it = curves_list.begin();
78 it != curves_list.end(); ++it) {
98 throw std::out_of_range(
"can't evaluate piecewise curve, out of range");
100 return (*
curves_.at(find_interval(t)))(t);
114 const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision())
const {
125 const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision())
const {
139 return !(*
this == other);
149 const std::size_t order)
const {
150 check_if_not_empty();
152 throw std::invalid_argument(
153 "can't evaluate piecewise curve, out of range");
165 const std::size_t order)
const {
167 for (
typename t_curve_ptr_t::const_iterator itc =
curves_.begin();
175 template <
typename Curve>
177 curve_ptr_t curve_ptr = boost::make_shared<Curve>(curve);
193 if (
size_ != 0 && !(fabs(cf->min() -
T_max_) < MARGIN)) {
194 std::stringstream ss;
195 ss <<
"Can not add new Polynom to PiecewiseCurve : time discontinuity " 196 "between T_max_ and pol.min(). Current " 198 <<
T_max_ <<
" new curve min is " << cf->min();
199 throw std::invalid_argument(ss.str().c_str());
201 if (cf->dim() !=
dim_) {
202 std::stringstream ss;
203 ss <<
"All the curves in a piecewiseCurve should have the same " 204 "dimension. Current dim is " 205 <<
dim_ <<
" dim of the new curve is " << cf->dim();
206 throw std::invalid_argument(ss.str().c_str());
224 check_if_not_empty();
225 bool isContinuous =
true;
228 point_t value_end, value_start;
229 while (isContinuous && i < (
size_ - 1)) {
232 value_end = (*current)(current->max());
233 value_start = (*next)(next->min());
234 if (!value_end.isApprox(value_start, MARGIN)) {
235 isContinuous =
false;
241 while (isContinuous && i < (
size_ - 1)) {
244 value_end = current->derivate(current->max(), order);
245 value_start = next->derivate(next->min(), order);
246 if (!value_end.isApprox(value_start, MARGIN)) {
247 isContinuous =
false;
266 return curves_[find_interval(t)];
274 throw std::length_error(
275 "curve_at_index: requested index greater than number of curves in " 276 "piecewise_curve instance");
284 template <
typename Bezier>
286 check_if_not_empty();
289 boost::is_same<typename Bezier::point_t, point_t>::value);
290 BOOST_STATIC_ASSERT(boost::is_same<
typename Bezier::point_derivate_t,
295 for (std::size_t i = 0; i <
size_; i++) {
305 template <
typename Hermite>
307 check_if_not_empty();
310 boost::is_same<typename Hermite::point_t, point_t>::value);
311 BOOST_STATIC_ASSERT(boost::is_same<
typename Hermite::point_derivate_t,
316 for (std::size_t i = 0; i <
size_; i++) {
325 template <
typename Polynomial>
327 check_if_not_empty();
330 boost::is_same<typename Polynomial::point_t, point_t>::value);
331 BOOST_STATIC_ASSERT(boost::is_same<
typename Polynomial::point_derivate_t,
336 for (std::size_t i = 0; i <
size_; i++) {
347 template <
typename Polynomial>
350 if (Safe & !(points.size() > 1)) {
353 throw std::invalid_argument(
354 "piecewise_curve::convert_discrete_points_to_polynomial: Error, less " 355 "than 2 discrete points");
357 if (points.size() != time_points.size()) {
358 throw std::invalid_argument(
359 "piecewise_curve::convert_discrete_points_to_polynomial: Error, " 360 "points and time_points must have the same " 365 boost::is_same<typename Polynomial::point_t, point_t>::value);
366 BOOST_STATIC_ASSERT(boost::is_same<
typename Polynomial::point_derivate_t,
370 for (
size_t i = 1; i < points.size(); ++i) {
371 piecewise_res.
add_curve(Polynomial(points[i - 1], points[i],
372 time_points[i - 1], time_points[i]));
374 return piecewise_res;
383 template <
typename Polynomial>
387 if (Safe & !(points.size() > 1)) {
390 throw std::invalid_argument(
391 "piecewise_curve::convert_discrete_points_to_polynomial: Error, less " 392 "than 2 discrete points");
394 if (points.size() != time_points.size()) {
395 throw std::invalid_argument(
396 "piecewise_curve::convert_discrete_points_to_polynomial: Error, " 397 "points and time_points must have the same " 400 if (points.size() != points_derivative.size()) {
401 throw std::invalid_argument(
402 "piecewise_curve::convert_discrete_points_to_polynomial: Error, " 403 "points and points_derivative must have the " 408 boost::is_same<typename Polynomial::point_t, point_t>::value);
409 BOOST_STATIC_ASSERT(boost::is_same<
typename Polynomial::point_derivate_t,
413 for (
size_t i = 1; i < points.size(); ++i) {
415 Polynomial(points[i - 1], points_derivative[i - 1], points[i],
416 points_derivative[i], time_points[i - 1], time_points[i]));
418 return piecewise_res;
429 template <
typename Polynomial>
433 if (Safe & !(points.size() > 1)) {
436 throw std::invalid_argument(
437 "piecewise_curve::convert_discrete_points_to_polynomial: Error, less " 438 "than 2 discrete points");
440 if (points.size() != time_points.size()) {
441 throw std::invalid_argument(
442 "piecewise_curve::convert_discrete_points_to_polynomial: Error, " 443 "points and time_points must have the same " 446 if (points.size() != points_derivative.size()) {
447 throw std::invalid_argument(
448 "piecewise_curve::convert_discrete_points_to_polynomial: Error, " 449 "points and points_derivative must have the " 452 if (points.size() != points_second_derivative.size()) {
453 throw std::invalid_argument(
454 "piecewise_curve::convert_discrete_points_to_polynomial: Error, " 455 "points and points_second_derivative must " 456 "have the same size.");
460 boost::is_same<typename Polynomial::point_t, point_t>::value);
461 BOOST_STATIC_ASSERT(boost::is_same<
typename Polynomial::point_derivate_t,
465 for (
size_t i = 1; i < points.size(); ++i) {
467 points[i - 1], points_derivative[i - 1],
468 points_second_derivative[i - 1], points[i], points_derivative[i],
469 points_second_derivative[i], time_points[i - 1], time_points[i]));
471 return piecewise_res;
485 template <
typename Polynomial>
487 const std::string& filename,
const time_t dt,
const size_t dim) {
489 throw std::invalid_argument(
"The dimension should be strictly positive.");
491 throw std::invalid_argument(
"The time step should be strictly positive.");
495 file.open(filename.c_str());
496 point_t last_pos = point_t::Zero(
dim), last_vel = point_t::Zero(
dim),
497 last_acc = point_t::Zero(
dim), new_pos = point_t::Zero(
dim),
498 new_vel = point_t::Zero(
dim), new_acc = point_t::Zero(
dim);
499 bool use_vel, use_acc;
502 std::getline(file, line);
503 std::istringstream iss_length(line);
504 const size_t length =
505 std::distance(std::istream_iterator<std::string>(iss_length),
506 std::istream_iterator<std::string>());
510 }
else if (length ==
dim * 2) {
513 }
else if (length ==
dim * 3) {
517 std::stringstream error;
518 error <<
"The first line of the file shold contains either " <<
dim 519 <<
", " <<
dim * 2 <<
" or " <<
dim * 3
520 <<
"values, got : " << length;
521 throw std::invalid_argument(error.str());
525 std::istringstream iss(line);
526 for (
size_t i = 0; i <
dim; ++i) {
531 for (
size_t i = 0; i <
dim; ++i) {
537 for (
size_t i = 0; i <
dim; ++i) {
543 size_t current_length;
546 while (std::getline(file, line)) {
548 std::istringstream iss_length(line);
550 std::distance(std::istream_iterator<std::string>(iss_length),
551 std::istream_iterator<std::string>());
552 if (current_length != length) {
553 std::stringstream error;
554 error <<
"Cannot parse line " << line_id <<
" got " << current_length
555 <<
" values instead of " << length;
556 throw std::invalid_argument(error.str());
558 std::istringstream iss(line);
560 for (
size_t i = 0; i <
dim; ++i) {
565 for (
size_t i = 0; i <
dim; ++i) {
571 for (
size_t i = 0; i <
dim; ++i) {
579 Polynomial(last_pos, last_vel, last_acc, new_pos, new_vel, new_acc,
580 dt * static_cast<time_t>(line_id - 1),
581 dt * static_cast<time_t>(line_id)));
582 }
else if (use_vel) {
584 Polynomial(last_pos, last_vel, new_pos, new_vel,
585 dt * static_cast<time_t>(line_id - 1),
586 dt * static_cast<time_t>(line_id)));
589 Polynomial(last_pos, new_pos, dt * static_cast<time_t>(line_id - 1),
590 dt * static_cast<time_t>(line_id)));
598 return piecewise_res;
606 std::size_t find_interval(
const Numeric t)
const {
616 std::size_t left_id = 0;
617 std::size_t right_id =
size_ - 1;
618 while (left_id <= right_id) {
619 const std::size_t middle_id = left_id + (right_id - left_id) / 2;
621 left_id = middle_id + 1;
623 right_id = middle_id - 1;
631 void check_if_not_empty()
const {
633 throw std::runtime_error(
"Error in piecewise curve : No curve added");
641 std::size_t
virtual dim()
const {
return dim_; };
651 throw std::runtime_error(
652 "degree() method is not implemented for this type of curve.");
666 friend class boost::serialization::access;
668 template <
class Archive>
669 void serialize(Archive& ar,
const unsigned int version) {
674 ar& boost::serialization::make_nvp(
"dim",
dim_);
675 ar& boost::serialization::make_nvp(
"curves",
curves_);
676 ar& boost::serialization::make_nvp(
"time_curves",
time_curves_);
677 ar& boost::serialization::make_nvp(
"size",
size_);
678 ar& boost::serialization::make_nvp(
"T_min",
T_min_);
679 ar& boost::serialization::make_nvp(
"T_max",
T_max_);
684 DEFINE_CLASS_TEMPLATE_VERSION(
686 typename Point_derivate,
typename CurveType),
688 Point_derivate, CurveType>))
690 #endif // _CLASS_PIECEWISE_CURVE Definition: bernstein.h:20
Point point_t
Definition: curve_abc.h:39
piecewise_curve(const t_curve_ptr_t &curves_list)
Definition: piecewise_curve.h:75
piecewise_curve_derivate_t * compute_derivate_ptr(const std::size_t order) const
compute_derivate return a piecewise_curve which is the derivative of this at given order ...
Definition: piecewise_curve.h:164
boost::shared_ptr< curve_t > curve_ptr_t
Definition: piecewise_curve.h:49
std::vector< point_t, Eigen::aligned_allocator< point_t > > t_point_t
Definition: piecewise_curve.h:40
piecewise_curve(const curve_ptr_t &cf)
Constructor. Initialize a piecewise curve by giving the first curve.
Definition: piecewise_curve.h:70
void serialize(Archive &ar, const unsigned int version)
Definition: piecewise_curve.h:669
virtual point_derivate_t derivate(const Time t, const std::size_t order) const
Evaluate the derivative of order N of curve at time t.
Definition: piecewise_curve.h:148
Point_derivate point_derivate_t
Definition: piecewise_curve.h:39
piecewise_curve< Time, Numeric, Safe, Point_derivate, Point_derivate, typename CurveType::curve_derivate_t > piecewise_curve_derivate_t
Definition: piecewise_curve.h:56
Time T_max_
Definition: piecewise_curve.h:662
virtual ~piecewise_curve()
Definition: piecewise_curve.h:91
virtual std::size_t dim() const
Get dimension of curve.
Definition: piecewise_curve.h:641
std::size_t getNumberCurves()
Definition: piecewise_curve.h:654
std::size_t num_curves() const
Get number of curves in piecewise curve.
Definition: piecewise_curve.h:257
interface for a Curve of arbitrary dimension.
curve_ptr_t curve_at_index(const std::size_t idx) const
Get curve at specified index in piecewise curve.
Definition: piecewise_curve.h:272
piecewise_curve(const piecewise_curve &other)
Definition: piecewise_curve.h:83
void add_curve_ptr(const curve_ptr_t &cf)
Add a new curve to piecewise curve, which should be defined in where is equal to of the actual pie...
Definition: piecewise_curve.h:187
virtual bool isApprox(const base_curve_t *other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
isApprox check if other and *this are approximately equal given a precision treshold Only two curves ...
Definition: piecewise_curve.h:123
boost::shared_ptr< typename piecewise_curve_derivate_t::curve_t > curve_derivate_ptr_t
Definition: piecewise_curve.h:58
Numeric num_t
Definition: curve_abc.h:42
t_curve_ptr_t curves_
Definition: piecewise_curve.h:659
boost::shared_ptr< curve_t > curve_ptr_t
Definition: curve_abc.h:47
bool is_continuous(const std::size_t order)
Check if the curve is continuous of order given.
Definition: piecewise_curve.h:223
virtual bool operator==(const piecewise_curve_t &other) const
Definition: piecewise_curve.h:134
Point point_t
Definition: piecewise_curve.h:38
piecewise_curve_t convert_piecewise_curve_to_bezier()
Convert all curves in piecewise curve into bezier curves.
Definition: piecewise_curve.h:285
virtual bool operator!=(const piecewise_curve_t &other) const
Definition: piecewise_curve.h:138
double Time
Definition: effector_spline.h:27
Numeric num_t
Definition: piecewise_curve.h:45
virtual Time max() const
Get the maximum time for which the curve is defined.
Definition: piecewise_curve.h:647
std::size_t size_
Definition: piecewise_curve.h:661
virtual std::size_t degree() const
Get the degree of the curve.
Definition: piecewise_curve.h:650
curve_ptr_t curve_at_time(const time_t t) const
Get curve corresponding to time t in piecewise curve. Example : A piecewise curve PC made of two curv...
Definition: piecewise_curve.h:265
std::vector< Time > t_time_t
Definition: piecewise_curve.h:51
Eigen::Matrix< Numeric, Eigen::Dynamic, 1 > Point
Definition: effector_spline.h:28
Time T_min_
Definition: piecewise_curve.h:662
t_time_t time_curves_
Definition: piecewise_curve.h:660
std::size_t dim_
Definition: piecewise_curve.h:658
static piecewise_curve_t convert_discrete_points_to_polynomial(t_point_t points, t_point_derivate_t points_derivative, t_time_t time_points)
Convert discrete points into piecewise polynomial curve with C1 continuity.
Definition: piecewise_curve.h:384
Point point_derivate_t
Definition: curve_abc.h:40
bool isApprox(const piecewise_curve_t &other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
isApprox check if other and *this are approximately equals. Only two curves of the same class can be ...
Definition: piecewise_curve.h:112
static piecewise_curve_t convert_discrete_points_to_polynomial(t_point_t points, t_point_derivate_t points_derivative, t_point_derivate_t points_second_derivative, t_time_t time_points)
Convert discrete points into piecewise polynomial curve with C2 continuity.
Definition: piecewise_curve.h:430
double Numeric
Definition: effector_spline.h:26
virtual Time min() const
Get the minimum time for which the curve is defined.
Definition: piecewise_curve.h:644
std::vector< point_derivate_t, Eigen::aligned_allocator< point_derivate_t > > t_point_derivate_t
Definition: piecewise_curve.h:43
void add_curve(const Curve &curve)
Definition: piecewise_curve.h:176
piecewise_curve()
Empty constructor. Add at least one curve to call other class functions.
Definition: piecewise_curve.h:64
Time time_t
Definition: piecewise_curve.h:44
curve_abc< Time, Numeric, Safe, point_t, point_derivate_t > base_curve_t
Definition: piecewise_curve.h:47
std::vector< curve_ptr_t > t_curve_ptr_t
Definition: piecewise_curve.h:50
piecewise_curve< Time, Numeric, Safe, Point, Point_derivate, CurveType > piecewise_curve_t
Definition: piecewise_curve.h:53
static piecewise_curve_t convert_discrete_points_to_polynomial(t_point_t points, t_time_t time_points)
Convert discrete points into piecewise polynomial curve with C0 continuity.
Definition: piecewise_curve.h:348
Time time_t
Definition: curve_abc.h:41
piecewise_curve_t convert_piecewise_curve_to_polynomial()
Convert all curves in piecewise curve into polynomial curves.
Definition: piecewise_curve.h:326
CurveType curve_t
Definition: piecewise_curve.h:48
piecewise_curve_t convert_piecewise_curve_to_cubic_hermite()
Convert all curves in piecewise curve into cubic hermite curves. Curves need to be of degree inferior...
Definition: piecewise_curve.h:306
Represents a curve of dimension Dim. If value of parameter Safe is false, no verification is made on ...
Definition: curve_abc.h:37
static piecewise_curve_t load_piecewise_from_text_file(const std::string &filename, const time_t dt, const size_t dim)
load_piecewise_from_text_file build a piecewise polynomial from a list of discrete points read from a...
Definition: piecewise_curve.h:486
virtual point_t operator()(const Time t) const
Evaluation of the cubic spline at time t.
Definition: piecewise_curve.h:93