8 #ifndef _CLASS_PIECEWISE_CURVE 9 #define _CLASS_PIECEWISE_CURVE 13 #include <boost/smart_ptr/shared_ptr.hpp> 14 #include <boost/serialization/vector.hpp> 28 template <
typename Time = double,
typename Numeric =
Time,
bool Safe =
false,
29 typename Point = Eigen::Matrix<Numeric, Eigen::Dynamic, 1>,
typename Point_derivate =
Point,
30 typename CurveType = curve_abc<Time, Numeric, Safe, Point, Point_derivate> >
31 struct piecewise_curve :
public curve_abc<Time, Numeric, Safe, Point, Point_derivate> {
34 typedef std::vector<point_t, Eigen::aligned_allocator<point_t> >
t_point_t;
35 typedef std::vector<point_derivate_t, Eigen::aligned_allocator<point_derivate_t> >
t_point_derivate_t;
60 for (
typename t_curve_ptr_t::const_iterator it = curves_list.begin(); it != curves_list.end(); ++it) {
79 throw std::out_of_range(
"can't evaluate piecewise curve, out of range");
81 return (*
curves_.at(find_interval(t)))(t);
93 const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision())
const {
102 const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision())
const {
103 const piecewise_curve_t* other_cast =
dynamic_cast<const piecewise_curve_t*
>(other);
112 virtual bool operator!=(
const piecewise_curve_t& other)
const {
return !(*
this == other); }
119 virtual point_derivate_t
derivate(
const Time t,
const std::size_t order)
const {
120 check_if_not_empty();
122 throw std::invalid_argument(
"can't evaluate piecewise curve, out of range");
134 for (
typename t_curve_ptr_t::const_iterator itc =
curves_.begin(); itc <
curves_.end(); ++itc) {
135 curve_derivate_ptr_t ptr((*itc)->compute_derivate_ptr(order));
141 template <
typename Curve>
143 curve_ptr_t curve_ptr = boost::make_shared<Curve>(curve);
158 if (
size_ != 0 && !(fabs(cf->min() -
T_max_) < MARGIN)) {
159 std::stringstream ss;
160 ss <<
"Can not add new Polynom to PiecewiseCurve : time discontinuity between T_max_ and pol.min(). Current " 162 <<
T_max_ <<
" new curve min is " << cf->min();
163 throw std::invalid_argument(ss.str().c_str());
165 if (cf->dim() !=
dim_) {
166 std::stringstream ss;
167 ss <<
"All the curves in a piecewiseCurve should have the same dimension. Current dim is " <<
dim_ 168 <<
" dim of the new curve is " << cf->dim();
169 throw std::invalid_argument(ss.str().c_str());
187 check_if_not_empty();
188 bool isContinuous =
true;
191 point_t value_end, value_start;
192 while (isContinuous && i < (
size_ - 1)) {
193 curve_ptr_t current =
curves_.at(i);
194 curve_ptr_t next =
curves_.at(i + 1);
195 value_end = (*current)(current->max());
196 value_start = (*next)(next->min());
197 if (!value_end.isApprox(value_start, MARGIN)) {
198 isContinuous =
false;
203 point_derivate_t value_end, value_start;
204 while (isContinuous && i < (
size_ - 1)) {
205 curve_ptr_t current =
curves_.at(i);
206 curve_ptr_t next =
curves_.at(i + 1);
207 value_end = current->derivate(current->max(), order);
208 value_start = next->derivate(next->min(), order);
209 if (!value_end.isApprox(value_start, MARGIN)) {
210 isContinuous =
false;
234 throw std::length_error(
235 "curve_at_index: requested index greater than number of curves in piecewise_curve instance");
243 template <
typename Bezier>
245 check_if_not_empty();
247 BOOST_STATIC_ASSERT(boost::is_same<typename Bezier::point_t, point_t>::value);
248 BOOST_STATIC_ASSERT(boost::is_same<typename Bezier::point_derivate_t, point_derivate_t>::value);
250 piecewise_curve_t pc_res;
252 for (std::size_t i = 0; i <
size_; i++) {
262 template <
typename Hermite>
264 check_if_not_empty();
266 BOOST_STATIC_ASSERT(boost::is_same<typename Hermite::point_t, point_t>::value);
267 BOOST_STATIC_ASSERT(boost::is_same<typename Hermite::point_derivate_t, point_derivate_t>::value);
269 piecewise_curve_t pc_res;
271 for (std::size_t i = 0; i <
size_; i++) {
280 template <
typename Polynomial>
282 check_if_not_empty();
284 BOOST_STATIC_ASSERT(boost::is_same<typename Polynomial::point_t, point_t>::value);
285 BOOST_STATIC_ASSERT(boost::is_same<typename Polynomial::point_derivate_t, point_derivate_t>::value);
287 piecewise_curve_t pc_res;
289 for (std::size_t i = 0; i <
size_; i++) {
300 template <
typename Polynomial>
302 if (Safe & !(points.size() > 1)) {
304 throw std::invalid_argument(
305 "piecewise_curve::convert_discrete_points_to_polynomial: Error, less than 2 discrete points");
307 if (points.size() != time_points.size()) {
308 throw std::invalid_argument(
309 "piecewise_curve::convert_discrete_points_to_polynomial: Error, points and time_points must have the same " 313 BOOST_STATIC_ASSERT(boost::is_same<typename Polynomial::point_t, point_t>::value);
314 BOOST_STATIC_ASSERT(boost::is_same<typename Polynomial::point_derivate_t, point_derivate_t>::value);
315 piecewise_curve_t piecewise_res;
317 for (
size_t i = 1; i < points.size(); ++i) {
318 piecewise_res.
add_curve(Polynomial(points[i - 1], points[i], time_points[i - 1], time_points[i]));
320 return piecewise_res;
329 template <
typename Polynomial>
331 t_point_derivate_t points_derivative,
332 t_time_t time_points) {
333 if (Safe & !(points.size() > 1)) {
335 throw std::invalid_argument(
336 "piecewise_curve::convert_discrete_points_to_polynomial: Error, less than 2 discrete points");
338 if (points.size() != time_points.size()) {
339 throw std::invalid_argument(
340 "piecewise_curve::convert_discrete_points_to_polynomial: Error, points and time_points must have the same " 343 if (points.size() != points_derivative.size()) {
344 throw std::invalid_argument(
345 "piecewise_curve::convert_discrete_points_to_polynomial: Error, points and points_derivative must have the " 349 BOOST_STATIC_ASSERT(boost::is_same<typename Polynomial::point_t, point_t>::value);
350 BOOST_STATIC_ASSERT(boost::is_same<typename Polynomial::point_derivate_t, point_derivate_t>::value);
351 piecewise_curve_t piecewise_res;
353 for (
size_t i = 1; i < points.size(); ++i) {
354 piecewise_res.
add_curve(Polynomial(points[i - 1], points_derivative[i - 1], points[i], points_derivative[i],
355 time_points[i - 1], time_points[i]));
357 return piecewise_res;
367 template <
typename Polynomial>
369 t_point_derivate_t points_derivative,
370 t_point_derivate_t points_second_derivative,
371 t_time_t time_points) {
372 if (Safe & !(points.size() > 1)) {
374 throw std::invalid_argument(
375 "piecewise_curve::convert_discrete_points_to_polynomial: Error, less than 2 discrete points");
377 if (points.size() != time_points.size()) {
378 throw std::invalid_argument(
379 "piecewise_curve::convert_discrete_points_to_polynomial: Error, points and time_points must have the same " 382 if (points.size() != points_derivative.size()) {
383 throw std::invalid_argument(
384 "piecewise_curve::convert_discrete_points_to_polynomial: Error, points and points_derivative must have the " 387 if (points.size() != points_second_derivative.size()) {
388 throw std::invalid_argument(
389 "piecewise_curve::convert_discrete_points_to_polynomial: Error, points and points_second_derivative must " 390 "have the same size.");
393 BOOST_STATIC_ASSERT(boost::is_same<typename Polynomial::point_t, point_t>::value);
394 BOOST_STATIC_ASSERT(boost::is_same<typename Polynomial::point_derivate_t, point_derivate_t>::value);
395 piecewise_curve_t piecewise_res;
397 for (
size_t i = 1; i < points.size(); ++i) {
398 piecewise_res.
add_curve(Polynomial(points[i - 1], points_derivative[i - 1], points_second_derivative[i - 1],
399 points[i], points_derivative[i], points_second_derivative[i],
400 time_points[i - 1], time_points[i]));
402 return piecewise_res;
414 template <
typename Polynomial>
417 if (dim <= 0)
throw std::invalid_argument(
"The dimension should be strictly positive.");
418 if (dt <= 0.)
throw std::invalid_argument(
"The time step should be strictly positive.");
420 piecewise_curve_t piecewise_res;
422 file.open(filename.c_str());
423 point_t last_pos = point_t::Zero(dim), last_vel = point_t::Zero(dim), last_acc = point_t::Zero(dim),
424 new_pos = point_t::Zero(dim), new_vel = point_t::Zero(dim), new_acc = point_t::Zero(dim);
425 bool use_vel, use_acc;
428 std::getline(file, line);
429 std::istringstream iss_length(line);
430 const size_t length =
431 std::distance(std::istream_iterator<std::string>(iss_length), std::istream_iterator<std::string>());
435 }
else if (length == dim * 2) {
438 }
else if (length == dim * 3) {
442 std::stringstream error;
443 error <<
"The first line of the file shold contains either " << dim <<
", " << dim * 2 <<
" or " << dim * 3
444 <<
"values, got : " << length;
445 throw std::invalid_argument(error.str());
449 std::istringstream iss(line);
450 for (
size_t i = 0; i <
dim; ++i) {
455 for (
size_t i = 0; i <
dim; ++i) {
461 for (
size_t i = 0; i <
dim; ++i) {
467 size_t current_length;
470 while (std::getline(file, line)) {
472 std::istringstream iss_length(line);
474 std::distance(std::istream_iterator<std::string>(iss_length), std::istream_iterator<std::string>());
475 if (current_length != length) {
476 std::stringstream error;
477 error <<
"Cannot parse line " << line_id <<
" got " << current_length <<
" values instead of " << length;
478 throw std::invalid_argument(error.str());
480 std::istringstream iss(line);
482 for (
size_t i = 0; i <
dim; ++i) {
487 for (
size_t i = 0; i <
dim; ++i) {
493 for (
size_t i = 0; i <
dim; ++i) {
500 piecewise_res.
add_curve(Polynomial(last_pos, last_vel, last_acc, new_pos, new_vel, new_acc,
501 dt * static_cast<time_t>(line_id - 1), dt * static_cast<time_t>(line_id)));
502 }
else if (use_vel) {
503 piecewise_res.
add_curve(Polynomial(last_pos, last_vel, new_pos, new_vel, dt * static_cast<time_t>(line_id - 1),
504 dt * static_cast<time_t>(line_id)));
507 Polynomial(last_pos, new_pos, dt * static_cast<time_t>(line_id - 1), dt * static_cast<time_t>(line_id)));
515 return piecewise_res;
523 std::size_t find_interval(
const Numeric t)
const {
533 std::size_t left_id = 0;
534 std::size_t right_id =
size_ - 1;
535 while (left_id <= right_id) {
536 const std::size_t middle_id = left_id + (right_id - left_id) / 2;
538 left_id = middle_id + 1;
540 right_id = middle_id - 1;
548 void check_if_not_empty()
const {
550 throw std::runtime_error(
"Error in piecewise curve : No curve added");
558 std::size_t
virtual dim()
const {
return dim_; };
568 throw std::runtime_error(
"degree() method is not implemented for this type of curve.");
582 friend class boost::serialization::access;
584 template <
class Archive>
585 void serialize(Archive& ar,
const unsigned int version) {
589 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(base_curve_t);
590 ar& boost::serialization::make_nvp(
"dim", dim_);
591 ar& boost::serialization::make_nvp(
"curves", curves_);
592 ar& boost::serialization::make_nvp(
"time_curves", time_curves_);
593 ar& boost::serialization::make_nvp(
"size", size_);
594 ar& boost::serialization::make_nvp(
"T_min", T_min_);
595 ar& boost::serialization::make_nvp(
"T_max", T_max_);
600 DEFINE_CLASS_TEMPLATE_VERSION(
601 SINGLE_ARG(
typename Time,
typename Numeric,
bool Safe,
typename Point,
typename Point_derivate,
605 #endif // _CLASS_PIECEWISE_CURVE Definition: bernstein.h:20
piecewise_curve(const t_curve_ptr_t &curves_list)
Definition: piecewise_curve.h:59
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:132
boost::shared_ptr< curve_t > curve_ptr_t
Definition: piecewise_curve.h:40
std::vector< point_t, Eigen::aligned_allocator< point_t > > t_point_t
Definition: piecewise_curve.h:34
piecewise_curve(const curve_ptr_t &cf)
Constructor. Initialize a piecewise curve by giving the first curve.
Definition: piecewise_curve.h:57
void serialize(Archive &ar, const unsigned int version)
Definition: piecewise_curve.h:585
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:119
Point_derivate point_derivate_t
Definition: piecewise_curve.h:33
piecewise_curve< Time, Numeric, Safe, Point_derivate, Point_derivate, typename CurveType::curve_derivate_t > piecewise_curve_derivate_t
Definition: piecewise_curve.h:45
Time T_max_
Definition: piecewise_curve.h:578
virtual ~piecewise_curve()
Definition: piecewise_curve.h:73
virtual std::size_t dim() const
Get dimension of curve.
Definition: piecewise_curve.h:558
std::size_t getNumberCurves()
Definition: piecewise_curve.h:570
std::size_t num_curves() const
Get number of curves in piecewise curve.
Definition: piecewise_curve.h:220
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:232
piecewise_curve(const piecewise_curve &other)
Definition: piecewise_curve.h:65
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:153
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:101
boost::shared_ptr< typename piecewise_curve_derivate_t::curve_t > curve_derivate_ptr_t
Definition: piecewise_curve.h:46
t_curve_ptr_t curves_
Definition: piecewise_curve.h:575
bool is_continuous(const std::size_t order)
Check if the curve is continuous of order given.
Definition: piecewise_curve.h:186
virtual bool operator==(const piecewise_curve_t &other) const
Definition: piecewise_curve.h:110
Point point_t
Definition: piecewise_curve.h:32
piecewise_curve_t convert_piecewise_curve_to_bezier()
Convert all curves in piecewise curve into bezier curves.
Definition: piecewise_curve.h:244
virtual bool operator!=(const piecewise_curve_t &other) const
Definition: piecewise_curve.h:112
double Time
Definition: effector_spline.h:27
Numeric num_t
Definition: piecewise_curve.h:37
virtual Time max() const
Get the maximum time for which the curve is defined.
Definition: piecewise_curve.h:564
std::size_t size_
Definition: piecewise_curve.h:577
virtual std::size_t degree() const
Get the degree of the curve.
Definition: piecewise_curve.h:567
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:227
std::vector< Time > t_time_t
Definition: piecewise_curve.h:42
Eigen::Matrix< Numeric, Eigen::Dynamic, 1 > Point
Definition: effector_spline.h:28
Time T_min_
Definition: piecewise_curve.h:578
t_time_t time_curves_
Definition: piecewise_curve.h:576
std::size_t dim_
Definition: piecewise_curve.h:574
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:330
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:92
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:368
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:561
std::vector< point_derivate_t, Eigen::aligned_allocator< point_derivate_t > > t_point_derivate_t
Definition: piecewise_curve.h:35
void add_curve(const Curve &curve)
Definition: piecewise_curve.h:142
piecewise_curve()
Empty constructor. Add at least one curve to call other class functions.
Definition: piecewise_curve.h:51
Time time_t
Definition: piecewise_curve.h:36
curve_abc< Time, Numeric, Safe, point_t, point_derivate_t > base_curve_t
Definition: piecewise_curve.h:38
std::vector< curve_ptr_t > t_curve_ptr_t
Definition: piecewise_curve.h:41
piecewise_curve< Time, Numeric, Safe, Point, Point_derivate, CurveType > piecewise_curve_t
Definition: piecewise_curve.h:43
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:301
piecewise_curve_t convert_piecewise_curve_to_polynomial()
Convert all curves in piecewise curve into polynomial curves.
Definition: piecewise_curve.h:281
CurveType curve_t
Definition: piecewise_curve.h:39
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:263
Represents a curve of dimension Dim. If value of parameter Safe is false, no verification is made on ...
Definition: curve_abc.h:34
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:415
virtual point_t operator()(const Time t) const
Evaluation of the cubic spline at time t.
Definition: piecewise_curve.h:75