sinusoidal.h
Go to the documentation of this file.
1 
9 #ifndef _CLASS_SINUSOIDALCURVE
10 #define _CLASS_SINUSOIDALCURVE
11 
12 #include "curve_abc.h"
13 #include <cmath>
14 
15 namespace curves {
20 template <typename Time = double, typename Numeric = Time, bool Safe = false,
21  typename Point = Eigen::Matrix<Numeric, Eigen::Dynamic, 1> >
22 struct sinusoidal : public curve_abc<Time, Numeric, Safe, Point> {
23  typedef Point point_t;
25  typedef Time time_t;
26  typedef Numeric num_t;
29 
30  /* Constructors - destructors */
31  public:
34  sinusoidal() : T_min_(0), T_max_(0), dim_(0) {}
35 
44  sinusoidal(const Point& p0, const Point& amplitude, const time_t T, const time_t phi, const time_t T_min = 0.,
45  const time_t T_max = std::numeric_limits<time_t>::max())
46  : p0_(p0),
47  amplitude_(amplitude),
48  T_(T),
49  phi_(std::fmod(phi, 2. * M_PI)),
50  T_min_(T_min),
51  T_max_(T_max),
52  dim_(p0_.size()) {
53  if (Safe && T_min_ > T_max_) {
54  throw std::invalid_argument("can't create constant curve: min bound is higher than max bound");
55  }
56  if (T_ <= 0.) throw std::invalid_argument("The period must be strictly positive");
57  if (static_cast<size_t>(amplitude_.size()) != dim_)
58  throw std::invalid_argument("The offset and the amplitude must have the same dimension");
59  }
60 
68  sinusoidal(const time_t traj_time, const Point& p_init, const Point& p_final, const time_t T_min = 0.,
69  const time_t T_max = std::numeric_limits<time_t>::max())
70  : T_(2. * traj_time), phi_(M_PI / 2.), T_min_(T_min), T_max_(T_max), dim_(p_init.size()) {
71  if (Safe && T_min_ > T_max_) {
72  throw std::invalid_argument("can't create constant curve: min bound is higher than max bound");
73  }
74  if (T_ <= 0) throw std::invalid_argument("The period must be strictly positive");
75  if (p_init.size() != p_final.size())
76  throw std::invalid_argument("The two stationary points must have the same dimension");
77  p0_ = (p_init + p_final) / 2.;
78  amplitude_ = (p_init - p_final) / 2.;
79  }
80 
83  sinusoidal(const sinusoidal_t& other)
84  : p0_(other.p0_),
85  amplitude_(other.amplitude_),
86  T_(other.T_),
87  phi_(other.phi_),
88  T_min_(other.T_min_),
89  T_max_(other.T_max_),
90  dim_(other.dim_) {}
91 
93  virtual ~sinusoidal() {}
94  /* Constructors - destructors */
95 
96  /*Operations*/
100  virtual point_t operator()(const time_t t) const {
101  if (Safe && (t < T_min_ || t > T_max_)) {
102  throw std::invalid_argument(
103  "error in sinusoidal curve : time t to evaluate should be in range [Tmin, Tmax] of the curve");
104  }
105  return p0_ + amplitude_ * sin(two_pi_f(t) + phi_);
106  }
107 
112  virtual point_derivate_t derivate(const time_t t, const std::size_t order) const {
113  if (Safe && (t < T_min_ || t > T_max_)) {
114  throw std::invalid_argument(
115  "error in constant curve : time t to derivate should be in range [Tmin, Tmax] of the curve");
116  }
117  if (order <= 0) throw std::invalid_argument("Order must be strictly positive");
118  return amplitude_ * pow(2. * M_PI / T_, static_cast<num_t>(order)) *
119  sin(two_pi_f(t) + phi_ + (M_PI * static_cast<num_t>(order) / 2.));
120  }
121 
126  sinusoidal_t compute_derivate(const std::size_t order) const {
127  if (order <= 0) throw std::invalid_argument("Order must be strictly positive");
128  const point_t amplitude = amplitude_ * pow(2. * M_PI / T_, static_cast<num_t>(order));
129  const time_t phi = phi_ + (M_PI * static_cast<num_t>(order) / 2.);
130  return sinusoidal_t(point_t::Zero(dim_), amplitude, T_, phi, T_min_, T_max_);
131  }
132 
136  virtual sinusoidal_t* compute_derivate_ptr(const std::size_t order) const {
137  return new sinusoidal_t(compute_derivate(order));
138  }
139 
148  virtual bool isApprox(const sinusoidal_t& other,
149  const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
150  return curves::isApprox<time_t>(T_min_, other.min()) && curves::isApprox<time_t>(T_max_, other.max()) &&
151  dim_ == other.dim() && p0_.isApprox(other.p0_, prec) && amplitude_.isApprox(other.amplitude_, prec) &&
152  curves::isApprox<time_t>(T_, other.T_) && curves::isApprox<time_t>(phi_, other.phi_);
153  }
154 
155  virtual bool isApprox(const curve_abc_t* other,
156  const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
157  const sinusoidal_t* other_cast = dynamic_cast<const sinusoidal_t*>(other);
158  if (other_cast)
159  return isApprox(*other_cast, prec);
160  else
161  return false;
162  }
163 
164  virtual bool operator==(const sinusoidal_t& other) const { return isApprox(other); }
165 
166  virtual bool operator!=(const sinusoidal_t& other) const { return !(*this == other); }
167 
168  /*Helpers*/
171  std::size_t virtual dim() const { return dim_; }
174  num_t virtual min() const { return T_min_; }
177  num_t virtual max() const { return T_max_; }
180  virtual std::size_t degree() const { return 1; }
181  /*Helpers*/
182 
183  /*Attributes*/
184  Point p0_; // offset
186  time_t T_; // period
187  time_t phi_; // phase
188  time_t T_min_, T_max_; // const
189  std::size_t dim_; // const
190  /*Attributes*/
191 
192  // Serialization of the class
194 
195  template <class Archive>
196  void serialize(Archive& ar, const unsigned int version) {
197  if (version) {
198  // Do something depending on version ?
199  }
200  ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t);
201  ar& boost::serialization::make_nvp("p0", p0_);
202  ar& boost::serialization::make_nvp("amplitude_", amplitude_);
203  ar& boost::serialization::make_nvp("T_", T_);
204  ar& boost::serialization::make_nvp("phi_", phi_);
205  ar& boost::serialization::make_nvp("T_min", T_min_);
206  ar& boost::serialization::make_nvp("T_max", T_max_);
207  ar& boost::serialization::make_nvp("dim", dim_);
208  }
209 
210  private:
211  inline const num_t two_pi_f(const time_t& t) const { return (2 * M_PI / T_) * t; }
212 
213 }; // struct sinusoidal
214 } // namespace curves
215 
216 DEFINE_CLASS_TEMPLATE_VERSION(SINGLE_ARG(typename Time, typename Numeric, bool Safe, typename Point),
218 
219 #endif // _CLASS_SINUSOIDALCURVE
std::size_t dim_
Definition: sinusoidal.h:189
Point amplitude_
Definition: sinusoidal.h:185
time_t T_min_
Definition: sinusoidal.h:188
Point point_derivate_t
Definition: sinusoidal.h:24
sinusoidal_t compute_derivate(const std::size_t order) const
Compute the derived curve at order N. Computes the derivative order N, of bezier curve of parametric...
Definition: sinusoidal.h:126
double Numeric
Definition: effector_spline.h:26
sinusoidal(const time_t traj_time, const Point &p_init, const Point &p_final, const time_t T_min=0., const time_t T_max=std::numeric_limits< time_t >::max())
Constructor from stationary points.
Definition: sinusoidal.h:68
virtual bool operator==(const sinusoidal_t &other) const
Definition: sinusoidal.h:164
sinusoidal< Time, Numeric, Safe, Point > sinusoidal_t
Definition: sinusoidal.h:27
virtual point_t operator()(const time_t t) const
Evaluation of the cubic spline at time t.
Definition: sinusoidal.h:100
virtual num_t max() const
Get the maximum time for which the curve is defined.
Definition: sinusoidal.h:177
interface for a Curve of arbitrary dimension.
Definition: bernstein.h:20
virtual std::size_t dim() const
Get dimension of curve.
Definition: sinusoidal.h:171
Represents a sinusoidal curve, evaluating the following equation: p0 + amplitude * (sin(2pi/T + phi) ...
Definition: fwd.h:43
virtual bool operator!=(const sinusoidal_t &other) const
Definition: sinusoidal.h:166
void serialize(Archive &ar, const unsigned int version)
Definition: sinusoidal.h:196
curve_abc< Time, Numeric, Safe, Point > curve_abc_t
Definition: sinusoidal.h:28
virtual std::size_t degree() const
Get the degree of the curve.
Definition: sinusoidal.h:180
Point point_t
Definition: sinusoidal.h:23
virtual bool isApprox(const sinusoidal_t &other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
isApprox check if other and *this are approximately equals given a precision treshold Only two curves...
Definition: sinusoidal.h:148
sinusoidal(const Point &p0, const Point &amplitude, const time_t T, const time_t phi, const time_t T_min=0., const time_t T_max=std::numeric_limits< time_t >::max())
Constructor.
Definition: sinusoidal.h:44
friend class boost::serialization::access
Definition: sinusoidal.h:193
virtual ~sinusoidal()
Destructor.
Definition: sinusoidal.h:93
time_t phi_
Definition: sinusoidal.h:187
sinusoidal(const sinusoidal_t &other)
Copy constructor.
Definition: sinusoidal.h:83
sinusoidal()
Empty constructor. Curve obtained this way can not perform other class functions. ...
Definition: sinusoidal.h:34
virtual bool isApprox(const curve_abc_t *other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
Definition: sinusoidal.h:155
Time time_t
Definition: sinusoidal.h:25
time_t T_
Definition: sinusoidal.h:186
Point p0_
Definition: sinusoidal.h:184
virtual sinusoidal_t * compute_derivate_ptr(const std::size_t order) const
Compute the derived curve at orderN.
Definition: sinusoidal.h:136
virtual num_t min() const
Get the minimum time for which the curve is defined.
Definition: sinusoidal.h:174
double Time
Definition: effector_spline.h:27
time_t T_max_
Definition: sinusoidal.h:188
virtual point_derivate_t derivate(const time_t t, const std::size_t order) const
Evaluate the derivative of order N of curve at time t.
Definition: sinusoidal.h:112
Eigen::Matrix< Numeric, Eigen::Dynamic, 1 > Point
Definition: effector_spline.h:28
Numeric num_t
Definition: sinusoidal.h:26