infinite-const-acc.hpp
Go to the documentation of this file.
1
9#ifndef _parameteric_curves_infinite_constant_acceleration_hpp
10#define _parameteric_curves_infinite_constant_acceleration_hpp
11
12#include <cmath>
14
15namespace parametriccurves {
16
20
21template <typename Numeric = double, Eigen::Index Dim = 1,
22 typename Point = Eigen::Matrix<Numeric, Dim, 1> >
23struct InfiniteConstAcc : public AbstractCurve<Numeric, Point> {
24 typedef Point point_t;
25 typedef Numeric time_t;
26 typedef Numeric num_t;
27
29
30 public:
32
34 const time_t& traj_time_,
35 const point_t& x_init_ = Eigen::Matrix<Numeric, Dim, 1>::Zero(),
36 const point_t& x_final_ = Eigen::Matrix<Numeric, Dim, 1>::Zero())
37 : curve_abc_t(-1, -1),
38 traj_time(traj_time_),
39 x_init(x_init_),
40 x_final(x_final_) {}
41
44
45 public:
46 virtual const point_t operator()(const time_t& t_) const {
47 const time_t t = std::fmod(t_, 2 * traj_time);
48 const point_t& m_ddx0 = 4.0 * (x_final - x_init) / (traj_time * traj_time);
49 if (t <= 0.5 * traj_time)
50 return x_init + 0.5 * m_ddx0 * t * t;
51 else if (t > 0.5 * traj_time && t <= 1.5 * traj_time)
52 return (x_init + x_final) / 2 +
53 0.5 * m_ddx0 * traj_time * (t - 0.5 * traj_time) -
54 0.5 * m_ddx0 * (t - 0.5 * traj_time) * (t - 0.5 * traj_time);
55 else //(t>1.5*traj_time && t<=2*traj_time)
56 return (x_init + x_final) / 2 -
57 0.5 * m_ddx0 * traj_time * (t - 1.5 * traj_time) +
58 0.5 * m_ddx0 * (t - 1.5 * traj_time) * (t - 1.5 * traj_time);
59 }
60
61 virtual const point_t derivate(const time_t& t_,
62 const std::size_t& order) const {
63 const time_t t = std::fmod(t_, 2 * traj_time);
64 const point_t& m_ddx0 = 4.0 * (x_final - x_init) / (traj_time * traj_time);
65 if (order == 1) {
66 if (t <= 0.5 * traj_time)
67 return m_ddx0 * t;
68 else if (t > 0.5 * traj_time && t <= 1.5 * traj_time)
69 return 0.5 * m_ddx0 * traj_time - m_ddx0 * (t - 0.5 * traj_time);
70 else //(t>1.5*traj_time && t<=2*traj_time)
71 return -0.5 * m_ddx0 * traj_time + m_ddx0 * (t - 1.5 * traj_time);
72 } else if (order == 2) {
73 if (t <= 0.5 * traj_time)
74 return m_ddx0;
75 else if (t > 0.5 * traj_time && t <= 1.5 * traj_time)
76 return -m_ddx0;
77 else //(t>1.5*traj_time && t<=2*traj_time)
78 return m_ddx0;
79 } else {
80 std::cerr << "Higher order derivatives not supported" << std::endl;
81 return point_t::Zero(Dim);
82 }
83 }
84
85 public:
86 virtual bool setInitialPoint(const point_t& x_init_) {
87 if (x_init_.size() != x_init.size()) return false;
88 x_init = x_init_;
89 }
90
91 virtual bool setInitialPoint(const double& x_init_) {
92 if (Dim != 1) return false;
93 x_init[0] = x_init_;
94 return true;
95 }
96
97 virtual bool setFinalPoint(const Eigen::VectorXd& x_final_) {
98 if (x_final.size() != x_final_.size()) return false;
99 x_final = x_final_;
100 return true;
101 }
102
103 virtual bool setFinalPoint(const double& x_final_) {
104 if (Dim != 1) return false;
105 x_final[0] = x_final_;
106 return true;
107 }
108
109 virtual bool setTrajectoryTime(double traj_time_) {
110 if (traj_time_ <= 0.0) return false;
111 traj_time = traj_time_;
112 return true;
113 }
114
115 protected:
116 /*Attributes*/
120};
121} // namespace parametriccurves
122#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 InfiniteConstAcc curve s = s_0 + u_0*t+0.5*a_0*t^2.
Definition: infinite-const-acc.hpp:23
Point point_t
Definition: infinite-const-acc.hpp:24
point_t x_final
Definition: infinite-const-acc.hpp:118
virtual const point_t operator()(const time_t &t_) const
Definition: infinite-const-acc.hpp:46
Numeric num_t
Definition: infinite-const-acc.hpp:26
virtual const point_t derivate(const time_t &t_, const std::size_t &order) const
Definition: infinite-const-acc.hpp:61
virtual bool setInitialPoint(const point_t &x_init_)
Definition: infinite-const-acc.hpp:86
virtual bool setFinalPoint(const Eigen::VectorXd &x_final_)
Definition: infinite-const-acc.hpp:97
virtual bool setTrajectoryTime(double traj_time_)
Definition: infinite-const-acc.hpp:109
time_t traj_time
Definition: infinite-const-acc.hpp:119
Numeric time_t
Definition: infinite-const-acc.hpp:25
virtual bool setFinalPoint(const double &x_final_)
Definition: infinite-const-acc.hpp:103
~InfiniteConstAcc()
Destructor.
Definition: infinite-const-acc.hpp:43
point_t x_init
Definition: infinite-const-acc.hpp:117
AbstractCurve< Numeric, Point > curve_abc_t
Definition: infinite-const-acc.hpp:28
virtual bool setInitialPoint(const double &x_init_)
Definition: infinite-const-acc.hpp:91
InfiniteConstAcc(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: infinite-const-acc.hpp:33