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);
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 throw std::invalid_argument(
"The dimension should be strictly positive.");
419 throw std::invalid_argument(
"The time step should be strictly positive.");
421 piecewise_curve_t piecewise_res;
423 file.open(filename.c_str());
424 point_t last_pos = point_t::Zero(dim),
425 last_vel = point_t::Zero(dim),
426 last_acc = point_t::Zero(dim),
427 new_pos = point_t::Zero(dim),
428 new_vel = point_t::Zero(dim),
429 new_acc = point_t::Zero(dim);
430 bool use_vel, use_acc;
433 std::getline(file, line);
434 std::istringstream iss_length(line);
435 const size_t length = std::distance(std::istream_iterator<std::string>(iss_length), std::istream_iterator<std::string>());
439 }
else if(length == dim * 2){
442 }
else if(length == dim * 3){
446 std::stringstream error;
447 error<<
"The first line of the file shold contains either " << dim <<
", " << dim * 2 <<
" or " << dim * 3 <<
448 "values, got : " << length;
449 throw std::invalid_argument(error.str());
453 std::istringstream iss(line);
454 for(
size_t i = 0 ; i <
dim ; ++i){
459 for(
size_t i = 0 ; i <
dim ; ++i){
465 for(
size_t i = 0 ; i <
dim ; ++i){
471 size_t current_length;
474 while (std::getline(file, line))
477 std::istringstream iss_length(line);
478 current_length = std::distance(std::istream_iterator<std::string>(iss_length), std::istream_iterator<std::string>());
479 if(current_length != length){
480 std::stringstream error;
481 error<<
"Cannot parse line " << line_id <<
" got " << current_length <<
" values instead of " << length;
482 throw std::invalid_argument(error.str());
484 std::istringstream iss(line);
486 for(
size_t i = 0 ; i <
dim ; ++i){
491 for(
size_t i = 0 ; i <
dim ; ++i){
497 for(
size_t i = 0 ; i <
dim ; ++i){
504 piecewise_res.
add_curve(Polynomial(last_pos, last_vel, last_acc,
505 new_pos, new_vel, new_acc,
506 dt * static_cast<time_t>(line_id - 1), dt * static_cast<time_t>(line_id)));
508 piecewise_res.
add_curve(Polynomial(last_pos, last_vel,
510 dt * static_cast<time_t>(line_id - 1), dt * static_cast<time_t>(line_id)));
512 piecewise_res.
add_curve(Polynomial(last_pos, new_pos,
513 dt * static_cast<time_t>(line_id - 1), dt * static_cast<time_t>(line_id)));
521 return piecewise_res;
529 std::size_t find_interval(
const Numeric t)
const {
539 std::size_t left_id = 0;
540 std::size_t right_id =
size_ - 1;
541 while (left_id <= right_id) {
542 const std::size_t middle_id = left_id + (right_id - left_id) / 2;
544 left_id = middle_id + 1;
546 right_id = middle_id - 1;
554 void check_if_not_empty()
const {
556 throw std::runtime_error(
"Error in piecewise curve : No curve added");
564 std::size_t
virtual dim()
const {
return dim_; };
574 throw std::runtime_error(
"degree() method is not implemented for this type of curve.");
589 friend class boost::serialization::access;
591 template <
class Archive>
592 void serialize(Archive& ar,
const unsigned int version) {
596 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(base_curve_t);
597 ar& boost::serialization::make_nvp(
"dim", dim_);
598 ar& boost::serialization::make_nvp(
"curves", curves_);
599 ar& boost::serialization::make_nvp(
"time_curves", time_curves_);
600 ar& boost::serialization::make_nvp(
"size", size_);
601 ar& boost::serialization::make_nvp(
"T_min", T_min_);
602 ar& boost::serialization::make_nvp(
"T_max", T_max_);
606 template <
typename Time,
typename Numeric,
bool Safe,
typename Po
int,
typename Po
int_derivate,
typename CurveType>
611 DEFINE_CLASS_TEMPLATE_VERSION(SINGLE_ARG(
typename Time,
typename Numeric,
bool Safe,
typename Point,
612 typename Point_derivate,
typename CurveType),
615 #endif // _CLASS_PIECEWISE_CURVE 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
static const double MARGIN
Definition: piecewise_curve.h:585
Time T_max_
Definition: piecewise_curve.h:584
std::vector< point_derivate_t, Eigen::aligned_allocator< point_derivate_t > > t_point_derivate_t
Definition: piecewise_curve.h:35
curve_abc< Time, Numeric, Safe, point_t, point_derivate_t > base_curve_t
Definition: piecewise_curve.h:38
Point point_t
Definition: piecewise_curve.h:32
piecewise_curve(const t_curve_ptr_t &curves_list)
Definition: piecewise_curve.h:59
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
t_curve_ptr_t curves_
Definition: piecewise_curve.h:581
double Numeric
Definition: effector_spline.h:26
Time T_min_
Definition: piecewise_curve.h:584
Point_derivate point_derivate_t
Definition: piecewise_curve.h:33
virtual Time min() const
Get the minimum time for which the curve is defined.
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
piecewise_curve(const curve_ptr_t &cf)
Constructor. Initialize a piecewise curve by giving the first curve.
Definition: piecewise_curve.h:57
virtual std::size_t degree() const
Get the degree of the curve.
Definition: piecewise_curve.h:573
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
virtual bool operator!=(const piecewise_curve_t &other) const
Definition: piecewise_curve.h:112
std::size_t dim_
Definition: piecewise_curve.h:580
interface for a Curve of arbitrary dimension.
void add_curve(const Curve &curve)
Definition: piecewise_curve.h:142
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
boost::shared_ptr< typename piecewise_curve_derivate_t::curve_t > curve_derivate_ptr_t
Definition: piecewise_curve.h:46
Definition: bernstein.h:20
CurveType curve_t
Definition: piecewise_curve.h:39
std::vector< point_t, Eigen::aligned_allocator< point_t > > t_point_t
Definition: piecewise_curve.h:34
boost::shared_ptr< curve_t > curve_ptr_t
Definition: piecewise_curve.h:40
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
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_t convert_piecewise_curve_to_polynomial()
Convert all curves in piecewise curve into polynomial curves.
Definition: piecewise_curve.h:281
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
virtual std::size_t dim() const
Get dimension of curve.
Definition: piecewise_curve.h:564
piecewise_curve< Time, Numeric, Safe, Point, Point_derivate, CurveType > piecewise_curve_t
Definition: piecewise_curve.h:43
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
bool is_continuous(const std::size_t order)
Check if the curve is continuous of order given.
Definition: piecewise_curve.h:186
std::size_t num_curves() const
Get number of curves in piecewise curve.
Definition: piecewise_curve.h:220
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
t_time_t time_curves_
Definition: piecewise_curve.h:582
piecewise_curve_t convert_piecewise_curve_to_bezier()
Convert all curves in piecewise curve into bezier curves.
Definition: piecewise_curve.h:244
piecewise_curve(const piecewise_curve &other)
Definition: piecewise_curve.h:65
std::size_t getNumberCurves()
Definition: piecewise_curve.h:576
piecewise_curve()
Empty constructor. Add at least one curve to call other class functions.
Definition: piecewise_curve.h:51
void serialize(Archive &ar, const unsigned int version)
Definition: piecewise_curve.h:592
virtual point_t operator()(const Time t) const
Evaluation of the cubic spline at time t.
Definition: piecewise_curve.h:75
piecewise_curve< Time, Numeric, Safe, Point_derivate, Point_derivate, typename CurveType::curve_derivate_t > piecewise_curve_derivate_t
Definition: piecewise_curve.h:45
virtual bool operator==(const piecewise_curve_t &other) const
Definition: piecewise_curve.h:110
Numeric num_t
Definition: piecewise_curve.h:37
std::size_t size_
Definition: piecewise_curve.h:583
std::vector< Time > t_time_t
Definition: piecewise_curve.h:42
double Time
Definition: effector_spline.h:27
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
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 Time max() const
Get the maximum time for which the curve is defined.
Definition: piecewise_curve.h:570
Eigen::Matrix< Numeric, Eigen::Dynamic, 1 > Point
Definition: effector_spline.h:28
Time time_t
Definition: piecewise_curve.h:36
std::vector< curve_ptr_t > t_curve_ptr_t
Definition: piecewise_curve.h:41
Represents a curve of dimension Dim. If value of parameter Safe is false, no verification is made on ...
Definition: curve_abc.h:34
virtual ~piecewise_curve()
Definition: piecewise_curve.h:73