minimum-jerk.hpp
Go to the documentation of this file.
1
9#ifndef _parameteric_curves_minimum_jerk_hpp
10#define _parameteric_curves_minimum_jerk_hpp
11
13
14namespace parametriccurves {
15
19template <typename Numeric = double, Eigen::Index Dim = 1,
20 typename Point = Eigen::Matrix<Numeric, Dim, 1> >
21struct MinimumJerk : public AbstractCurve<Numeric, Point> {
22 typedef Point point_t;
23 typedef Numeric time_t;
24 typedef Numeric num_t;
26
27 public:
29
30 MinimumJerk(const time_t& traj_time_,
31 const point_t& x_init_ = Eigen::Matrix<Numeric, Dim, 1>::Zero(),
32 const point_t& x_final_ = Eigen::Matrix<Numeric, Dim, 1>::Zero())
33 : curve_abc_t(0, traj_time_), x_init(x_init_), x_final(x_final_) {}
34
37
38 public:
39 virtual const point_t operator()(const time_t& t) const {
40 time_t td = t / this->t_max;
41 time_t td2 = td * td;
42 time_t td3 = td2 * td;
43 time_t td4 = td3 * td;
44 time_t td5 = td4 * td;
45 time_t p = 10 * td3 - 15 * td4 + 6 * td5;
46 return x_init + (x_final - x_init) * p;
47 }
48
49 virtual const point_t derivate(const time_t& t,
50 const std::size_t& order) const {
51 time_t td = t / this->t_max;
52 time_t td2 = td * td;
53 time_t td3 = td2 * td;
54 time_t td4 = td3 * td;
55 time_t td5 = td4 * td;
56 if (order == 1) {
57 time_t dp = (30 * td2 - 60 * td3 + 30 * td4) / this->t_max;
58 return (x_final - x_init) * dp;
59 } else if (order == 2) {
60 time_t ddp =
61 (60 * td - 180 * td2 + 120 * td3) / (this->t_max * this->t_max);
62 return (x_final - x_init) * ddp;
63 } else {
64 std::cerr << "Higher order derivatives not supported" << std::endl;
65 return point_t::Zero(Dim);
66 }
67 }
68
69 public:
70 /*Setters*/
71 virtual bool setInitialPoint(const point_t& x_init_) {
72 if (x_init_.size() != x_init.size()) return false;
73 x_init = x_init_;
74 }
75
76 virtual bool setInitialPoint(const num_t& x_init_) {
77 if (Dim != 1) return false;
78 x_init[0] = x_init_;
79 return true;
80 }
81
82 virtual bool setFinalPoint(const point_t& x_final_) {
83 if (x_final.size() != x_final_.size()) return false;
84 x_final = x_final_;
85 return true;
86 }
87
88 virtual bool setFinalPoint(const double& x_final_) {
89 if (Dim != 1) return false;
90 x_final[0] = x_final_;
91 return true;
92 }
93
94 protected:
95 /*Attributes*/
98
99 private:
100};
101} // namespace parametriccurves
102#endif //_CLASS_EXACTCUBIC
Definition: abstract-curve.hpp:16
Eigen::Matrix< Numeric, 3, 1 > Point
Definition: effector_spline.h:28
double Numeric
Definition: effector_spline.h:26
Represents a curve of dimension Dim is Safe is false, no verification is made on the evaluation of th...
Definition: abstract-curve.hpp:21
Creates MinimumJerk curve.
Definition: minimum-jerk.hpp:21
MinimumJerk(const time_t &traj_time_, const point_t &x_init_=Eigen::Matrix< Numeric, Dim, 1 >::Zero(), const point_t &x_final_=Eigen::Matrix< Numeric, Dim, 1 >::Zero())
Constructor.
Definition: minimum-jerk.hpp:30
Numeric num_t
Definition: minimum-jerk.hpp:24
point_t x_init
Definition: minimum-jerk.hpp:96
Point point_t
Definition: minimum-jerk.hpp:22
virtual const point_t operator()(const time_t &t) const
Definition: minimum-jerk.hpp:39
virtual bool setInitialPoint(const point_t &x_init_)
Definition: minimum-jerk.hpp:71
~MinimumJerk()
Destructor.
Definition: minimum-jerk.hpp:36
virtual bool setInitialPoint(const num_t &x_init_)
Definition: minimum-jerk.hpp:76
virtual const point_t derivate(const time_t &t, const std::size_t &order) const
Definition: minimum-jerk.hpp:49
point_t x_final
Definition: minimum-jerk.hpp:97
virtual bool setFinalPoint(const point_t &x_final_)
Definition: minimum-jerk.hpp:82
Numeric time_t
Definition: minimum-jerk.hpp:23
AbstractCurve< Numeric, Point > curve_abc_t
Definition: minimum-jerk.hpp:25
virtual bool setFinalPoint(const double &x_final_)
Definition: minimum-jerk.hpp:88