sinusoidal.h
Go to the documentation of this file.
1 
9 #ifndef _CLASS_SINUSOIDALCURVE
10 #define _CLASS_SINUSOIDALCURVE
11 
12 #include <cmath>
13 
14 #include "curve_abc.h"
15 
16 namespace ndcurves {
21 template <typename Time = double, typename Numeric = Time, bool Safe = false,
22  typename Point = Eigen::Matrix<Numeric, Eigen::Dynamic, 1> >
23 struct sinusoidal : public curve_abc<Time, Numeric, Safe, Point> {
24  typedef Point point_t;
26  typedef Time time_t;
27  typedef Numeric num_t;
30 
31  /* Constructors - destructors */
32  public:
36  sinusoidal() : T_min_(0), T_max_(0), dim_(0) {}
37 
46  sinusoidal(const Point& p0, const Point& amplitude, const time_t T,
47  const time_t phi, const time_t T_min = 0.,
48  const time_t T_max = std::numeric_limits<time_t>::max())
49  : p0_(p0),
50  amplitude_(amplitude),
51  T_(T),
52  phi_(std::fmod(phi, 2. * M_PI)),
53  T_min_(T_min),
54  T_max_(T_max),
55  dim_(p0_.size()) {
56  if (Safe && T_min_ > T_max_) {
57  throw std::invalid_argument(
58  "can't create constant curve: min bound is higher than max bound");
59  }
60  if (T_ <= 0.)
61  throw std::invalid_argument("The period must be strictly positive");
62  if (static_cast<size_t>(amplitude_.size()) != dim_)
63  throw std::invalid_argument(
64  "The offset and the amplitude must have the same dimension");
65  }
66 
74  sinusoidal(const time_t traj_time, const Point& p_init, const Point& p_final,
75  const time_t T_min = 0.,
76  const time_t T_max = std::numeric_limits<time_t>::max())
77  : T_(2. * traj_time),
78  phi_(M_PI / 2.),
79  T_min_(T_min),
80  T_max_(T_max),
81  dim_(p_init.size()) {
82  if (Safe && T_min_ > T_max_) {
83  throw std::invalid_argument(
84  "can't create constant curve: min bound is higher than max bound");
85  }
86  if (T_ <= 0)
87  throw std::invalid_argument("The period must be strictly positive");
88  if (p_init.size() != p_final.size())
89  throw std::invalid_argument(
90  "The two stationary points must have the same dimension");
91  p0_ = (p_init + p_final) / 2.;
92  amplitude_ = (p_init - p_final) / 2.;
93  }
94 
97  sinusoidal(const sinusoidal_t& other)
98  : p0_(other.p0_),
99  amplitude_(other.amplitude_),
100  T_(other.T_),
101  phi_(other.phi_),
102  T_min_(other.T_min_),
103  T_max_(other.T_max_),
104  dim_(other.dim_) {}
105 
107  virtual ~sinusoidal() {}
108  /* Constructors - destructors */
109 
110  /*Operations*/
114  virtual point_t operator()(const time_t t) const {
115  if (Safe && (t < T_min_ || t > T_max_)) {
116  throw std::invalid_argument(
117  "error in sinusoidal curve : time t to evaluate should be in range "
118  "[Tmin, Tmax] of the curve");
119  }
120  return p0_ + amplitude_ * sin(two_pi_f(t) + phi_);
121  }
122 
128  virtual point_derivate_t derivate(const time_t t,
129  const std::size_t order) const {
130  if (Safe && (t < T_min_ || t > T_max_)) {
131  throw std::invalid_argument(
132  "error in constant curve : time t to derivate should be in range "
133  "[Tmin, Tmax] of the curve");
134  }
135  if (order <= 0)
136  throw std::invalid_argument("Order must be strictly positive");
137  return amplitude_ * pow(2. * M_PI / T_, static_cast<num_t>(order)) *
138  sin(two_pi_f(t) + phi_ + (M_PI * static_cast<num_t>(order) / 2.));
139  }
140 
145  sinusoidal_t compute_derivate(const std::size_t order) const {
146  if (order <= 0)
147  throw std::invalid_argument("Order must be strictly positive");
148  const point_t amplitude =
149  amplitude_ * pow(2. * M_PI / T_, static_cast<num_t>(order));
150  const time_t phi = phi_ + (M_PI * static_cast<num_t>(order) / 2.);
151  return sinusoidal_t(point_t::Zero(dim_), amplitude, T_, phi, T_min_,
152  T_max_);
153  }
154 
159  virtual sinusoidal_t* compute_derivate_ptr(const std::size_t order) const {
160  return new sinusoidal_t(compute_derivate(order));
161  }
162 
172  virtual bool isApprox(
173  const sinusoidal_t& other,
174  const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
175  return ndcurves::isApprox<time_t>(T_min_, other.min()) &&
176  ndcurves::isApprox<time_t>(T_max_, other.max()) &&
177  dim_ == other.dim() && p0_.isApprox(other.p0_, prec) &&
178  amplitude_.isApprox(other.amplitude_, prec) &&
179  ndcurves::isApprox<time_t>(T_, other.T_) &&
180  ndcurves::isApprox<time_t>(phi_, other.phi_);
181  }
182 
183  virtual bool isApprox(
184  const curve_abc_t* other,
185  const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
186  const sinusoidal_t* other_cast = dynamic_cast<const sinusoidal_t*>(other);
187  if (other_cast)
188  return isApprox(*other_cast, prec);
189  else
190  return false;
191  }
192 
193  virtual bool operator==(const sinusoidal_t& other) const {
194  return isApprox(other);
195  }
196 
197  virtual bool operator!=(const sinusoidal_t& other) const {
198  return !(*this == other);
199  }
200 
201  /*Helpers*/
204  std::size_t virtual dim() const { return dim_; }
207  num_t virtual min() const { return T_min_; }
210  num_t virtual max() const { return T_max_; }
213  virtual std::size_t degree() const { return 1; }
214  /*Helpers*/
215 
216  /*Attributes*/
217  Point p0_; // offset
219  time_t T_; // period
220  time_t phi_; // phase
221  time_t T_min_, T_max_; // const
222  std::size_t dim_; // const
223  /*Attributes*/
224 
225  // Serialization of the class
227 
228  template <class Archive>
229  void serialize(Archive& ar, const unsigned int version) {
230  if (version) {
231  // Do something depending on version ?
232  }
233  ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t);
234  ar& boost::serialization::make_nvp("p0", p0_);
235  ar& boost::serialization::make_nvp("amplitude_", amplitude_);
236  ar& boost::serialization::make_nvp("T_", T_);
237  ar& boost::serialization::make_nvp("phi_", phi_);
238  ar& boost::serialization::make_nvp("T_min", T_min_);
239  ar& boost::serialization::make_nvp("T_max", T_max_);
240  ar& boost::serialization::make_nvp("dim", dim_);
241  }
242 
243  private:
244  inline const num_t two_pi_f(const time_t& t) const {
245  return (2 * M_PI / T_) * t;
246  }
247 
248 }; // struct sinusoidal
249 } // namespace ndcurves
250 
252  SINGLE_ARG(typename Time, typename Numeric, bool Safe, typename Point),
254 
255 #endif // _CLASS_SINUSOIDALCURVE
#define SINGLE_ARG(...)
Definition: archive.hpp:23
Definition: bernstein.h:20
virtual bool isApprox(const curve_abc_t *other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
Definition: sinusoidal.h:183
Point amplitude_
Definition: sinusoidal.h:218
virtual num_t max() const
Get the maximum time for which the curve is defined.
Definition: sinusoidal.h:210
friend class boost::serialization::access
Definition: sinusoidal.h:226
Point point_derivate_t
Definition: sinusoidal.h:25
Point p0_
Definition: sinusoidal.h:217
time_t phi_
Definition: sinusoidal.h:220
virtual std::size_t dim() const
Get dimension of curve.
Definition: sinusoidal.h:204
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:128
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:145
virtual sinusoidal_t * compute_derivate_ptr(const std::size_t order) const
Compute the derived curve at orderN.
Definition: sinusoidal.h:159
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:172
virtual std::size_t degree() const
Get the degree of the curve.
Definition: sinusoidal.h:213
time_t T_min_
Definition: sinusoidal.h:221
interface for a Curve of arbitrary dimension.
time_t T_
Definition: sinusoidal.h:219
Time time_t
Definition: sinusoidal.h:26
sinusoidal(const sinusoidal_t &other)
Copy constructor.
Definition: sinusoidal.h:97
time_t T_max_
Definition: sinusoidal.h:221
sinusoidal()
Empty constructor. Curve obtained this way can not perform other class functions. ...
Definition: sinusoidal.h:36
double Time
Definition: effector_spline.h:27
std::size_t dim_
Definition: sinusoidal.h:222
sinusoidal< Time, Numeric, Safe, Point > sinusoidal_t
Definition: sinusoidal.h:28
virtual num_t min() const
Get the minimum time for which the curve is defined.
Definition: sinusoidal.h:207
curve_abc< Time, Numeric, Safe, Point > curve_abc_t
Definition: sinusoidal.h:29
Numeric num_t
Definition: sinusoidal.h:27
#define DEFINE_CLASS_TEMPLATE_VERSION(Template, Type)
Definition: archive.hpp:27
virtual point_t operator()(const time_t t) const
Evaluation of the cubic spline at time t.
Definition: sinusoidal.h:114
Eigen::Matrix< Numeric, Eigen::Dynamic, 1 > Point
Definition: effector_spline.h:28
Represents a sinusoidal curve, evaluating the following equation: p0 + amplitude * (sin(2pi/T + phi) ...
Definition: fwd.h:48
Point point_t
Definition: sinusoidal.h:24
double Numeric
Definition: effector_spline.h:26
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:46
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:74
virtual bool operator!=(const sinusoidal_t &other) const
Definition: sinusoidal.h:197
void serialize(Archive &ar, const unsigned int version)
Definition: sinusoidal.h:229
virtual bool operator==(const sinusoidal_t &other) const
Definition: sinusoidal.h:193
virtual ~sinusoidal()
Destructor.
Definition: sinusoidal.h:107