linear-chirp.hpp
Go to the documentation of this file.
1
9#ifndef _parameteric_curves_linear_chirp_hpp
10#define _parameteric_curves_linear_chirp_hpp
11
13
14namespace parametriccurves {
15
23template <typename Numeric = double, Eigen::Index Dim = 1,
24 typename Point = Eigen::Matrix<Numeric, Dim, 1> >
25struct LinearChirp : public AbstractCurve<Numeric, Point> {
26 typedef Point point_t;
27 typedef Point freq_t;
28 typedef Point phase_t;
29 typedef Numeric time_t;
30 typedef Numeric num_t;
32
33 public:
35
36 LinearChirp(const time_t& traj_time_,
37 const point_t& x_init_ = Eigen::Matrix<Numeric, Dim, 1>::Zero(),
38 const point_t& x_final_ = Eigen::Matrix<Numeric, Dim, 1>::Zero())
39 : curve_abc_t(0, traj_time_), x_init(x_init_), x_final(x_final_) {
40 f0.setZero(Dim);
41 f1.setZero(Dim);
42 }
43
44 LinearChirp(const time_t& traj_time_, const freq_t& f0_, const freq_t& f1_,
45 const point_t& x_init_ = Eigen::Matrix<Numeric, Dim, 1>::Zero(),
46 const point_t& x_final_ = Eigen::Matrix<Numeric, Dim, 1>::Zero())
47 : curve_abc_t(0, traj_time_),
48 f0(f0_),
49 f1(f1_),
50 x_init(x_init_),
51 x_final(x_final_) {}
52
55
56 public:
57 virtual const point_t operator()(const time_t& t) const {
58 const point_t& m_p = 0.5 * (1.0 - phase(t).array().cos());
59 return x_init.array() + (x_final.array() - x_init.array()) * m_p.array();
60 }
61
62 virtual const point_t derivate(const time_t& t,
63 const std::size_t& order) const {
64 if (order == 1) {
65 const point_t& m_dp = M_PI * freq(t).array() * phase(t).array().sin();
66 return (x_final - x_init).array() * m_dp.array();
67 } else if (order == 2) {
68 const point_t& m_ddp = 2.0 * M_PI * M_PI * freq(t).array() *
69 freq(t).array() * phase(t).array().cos();
70 return (x_final - x_init).array() * m_ddp.array();
71 } else {
72 std::cerr << "Higher order derivatives not supported" << std::endl;
73 return point_t::Zero(Dim);
74 }
75 }
76
77 public:
78 /*Setters*/
79 virtual bool setInitialFrequency(const Eigen::VectorXd& f0_) {
80 if (f0.size() != f0_.size()) return false;
81 f0 = f0_;
82 return true;
83 }
84
85 virtual bool setInitialFrequency(const double& f0_) {
86 if (Dim != 1) return false;
87 f0[0] = f0_;
88 return true;
89 }
90
91 virtual bool setFinalFrequency(const Eigen::VectorXd& f1_) {
92 if (f1.size() != f1_.size()) return false;
93 f1 = f1_;
94 return true;
95 }
96
97 virtual bool setFinalFrequency(const double& f1_) {
98 if (Dim != 1) return false;
99 f1[0] = f1_;
100 return true;
101 }
102
103 virtual bool setInitialPoint(const point_t& x_init_) {
104 if (x_init_.size() != x_init.size()) return false;
105 x_init = x_init_;
106 }
107
108 virtual bool setInitialPoint(const double& x_init_) {
109 if (Dim != 1) return false;
110 x_init[0] = x_init_;
111 return true;
112 }
113
114 virtual bool setFinalPoint(const Eigen::VectorXd& x_final_) {
115 if (x_final.size() != x_final_.size()) return false;
116 x_final = x_final_;
117 return true;
118 }
119
120 virtual bool setFinalPoint(const double& x_final_) {
121 if (Dim != 1) return false;
122 x_final[0] = x_final_;
123 return true;
124 }
125
126 protected:
127 /*Attributes*/
132
133 private:
134 inline const freq_t freq(const time_t& t) const {
135 const freq_t& m_k = 2.0 * (f1 - f0) / this->t_max;
136 if (t < 0.5 * this->t_max)
137 return f0 + m_k * t;
138 else
139 return f1 + m_k * (0.5 * this->t_max - t);
140 }
141
142 inline const phase_t phase(const time_t& t) const {
143 const freq_t& m_k = 2.0 * (f1 - f0) / this->t_max;
144 const phase_t& m_phi_0 = M_PI * this->t_max * (f0 - f1);
145 if (t < 0.5 * this->t_max)
146 return 2 * M_PI * t * (f0 + 0.5 * m_k * t);
147 else
148 return m_phi_0 + 2 * M_PI * t * (f1 + 0.5 * m_k * (this->t_max - t));
149 }
150};
151} // namespace parametriccurves
152#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 LinearChirp curve Linear chirp trajectory generator. A linear chirp is a sinusoid whose frequ...
Definition: linear-chirp.hpp:25
freq_t f0
Definition: linear-chirp.hpp:128
virtual bool setInitialPoint(const point_t &x_init_)
Definition: linear-chirp.hpp:103
point_t x_final
Definition: linear-chirp.hpp:131
virtual bool setFinalFrequency(const double &f1_)
Definition: linear-chirp.hpp:97
virtual bool setFinalFrequency(const Eigen::VectorXd &f1_)
Definition: linear-chirp.hpp:91
freq_t f1
initial frequency
Definition: linear-chirp.hpp:129
Numeric time_t
Definition: linear-chirp.hpp:29
Point point_t
Definition: linear-chirp.hpp:26
virtual bool setFinalPoint(const double &x_final_)
Definition: linear-chirp.hpp:120
virtual const point_t derivate(const time_t &t, const std::size_t &order) const
Definition: linear-chirp.hpp:62
LinearChirp(const time_t &traj_time_, const freq_t &f0_, const freq_t &f1_, const point_t &x_init_=Eigen::Matrix< Numeric, Dim, 1 >::Zero(), const point_t &x_final_=Eigen::Matrix< Numeric, Dim, 1 >::Zero())
Definition: linear-chirp.hpp:44
virtual bool setFinalPoint(const Eigen::VectorXd &x_final_)
Definition: linear-chirp.hpp:114
AbstractCurve< Numeric, Point > curve_abc_t
Definition: linear-chirp.hpp:31
Numeric num_t
Definition: linear-chirp.hpp:30
virtual bool setInitialPoint(const double &x_init_)
Definition: linear-chirp.hpp:108
virtual bool setInitialFrequency(const Eigen::VectorXd &f0_)
Definition: linear-chirp.hpp:79
point_t x_init
final frequency
Definition: linear-chirp.hpp:130
Point phase_t
Definition: linear-chirp.hpp:28
Point freq_t
Definition: linear-chirp.hpp:27
virtual bool setInitialFrequency(const double &f0_)
Definition: linear-chirp.hpp:85
LinearChirp(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: linear-chirp.hpp:36
virtual const point_t operator()(const time_t &t) const
Definition: linear-chirp.hpp:57
~LinearChirp()
Destructor.
Definition: linear-chirp.hpp:54