9#ifndef _CLASS_SINUSOIDALCURVE
10#define _CLASS_SINUSOIDALCURVE
21template <
typename Time = double,
typename Numeric =
Time,
bool Safe =
false,
22 typename Point = Eigen::Matrix<Numeric, Eigen::Dynamic, 1> >
48 const time_t T_max = std::numeric_limits<time_t>::max())
52 phi_(std::fmod(phi, 2. * M_PI)),
57 throw std::invalid_argument(
58 "can't create constant curve: min bound is higher than max bound");
61 throw std::invalid_argument(
"The period must be strictly positive");
63 throw std::invalid_argument(
64 "The offset and the amplitude must have the same dimension");
76 const time_t T_max = std::numeric_limits<time_t>::max())
83 throw std::invalid_argument(
84 "can't create constant curve: min bound is higher than max bound");
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.;
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");
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");
136 throw std::invalid_argument(
"Order must be strictly positive");
138 sin(two_pi_f(t) +
phi_ + (M_PI *
static_cast<num_t>(order) / 2.));
147 throw std::invalid_argument(
"Order must be strictly positive");
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()) &&
179 ndcurves::isApprox<time_t>(
T_, other.
T_) &&
180 ndcurves::isApprox<time_t>(
phi_, other.
phi_);
185 const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision())
const {
198 return !(*
this == other);
204 std::size_t
virtual dim()
const {
return dim_; }
213 virtual std::size_t
degree()
const {
return 1; }
228 template <
class Archive>
229 void serialize(Archive& ar,
const unsigned int version) {
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_);
244 inline const num_t two_pi_f(
const time_t& t)
const {
245 return (2 * M_PI /
T_) * t;
252 SINGLE_ARG(
typename Time,
typename Numeric,
bool Safe,
typename Point),
#define DEFINE_CLASS_TEMPLATE_VERSION(Template, Type)
Definition: archive.hpp:27
#define SINGLE_ARG(...)
Definition: archive.hpp:23
interface for a Curve of arbitrary dimension.
Eigen::Matrix< Numeric, Eigen::Dynamic, 1 > Point
Definition: effector_spline.h:28
double Numeric
Definition: effector_spline.h:26
double Time
Definition: effector_spline.h:27
Definition: bernstein.h:20
Represents a curve of dimension Dim. If value of parameter Safe is false, no verification is made on ...
Definition: curve_abc.h:37
Represents a sinusoidal curve, evaluating the following equation: p0 + amplitude * (sin(2pi/T + phi)
Definition: sinusoidal.h:23
void serialize(Archive &ar, const unsigned int version)
Definition: sinusoidal.h:229
virtual num_t min() const
Get the minimum time for which the curve is defined.
Definition: sinusoidal.h:207
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 isApprox(const curve_abc_t *other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
Definition: sinusoidal.h:183
Point p0_
Definition: sinusoidal.h:217
time_t phi_
Definition: sinusoidal.h:220
Point amplitude_
Definition: sinusoidal.h:218
Point point_t
Definition: sinusoidal.h:24
Numeric num_t
Definition: sinusoidal.h:27
virtual std::size_t degree() const
Get the degree of the curve.
Definition: sinusoidal.h:213
sinusoidal(const sinusoidal_t &other)
Copy constructor.
Definition: sinusoidal.h:97
virtual bool operator==(const sinusoidal_t &other) const
Definition: sinusoidal.h:193
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
sinusoidal< Time, Numeric, Safe, Point > sinusoidal_t
Definition: sinusoidal.h:28
Point point_derivate_t
Definition: sinusoidal.h:25
sinusoidal(const Point &p0, const Point &litude, 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
virtual point_t operator()(const time_t t) const
Evaluation of the cubic spline at time t.
Definition: sinusoidal.h:114
time_t T_min_
Definition: sinusoidal.h:221
time_t T_
Definition: sinusoidal.h:219
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
virtual std::size_t dim() const
Get dimension of curve.
Definition: sinusoidal.h:204
std::size_t dim_
Definition: sinusoidal.h:222
virtual bool operator!=(const sinusoidal_t &other) const
Definition: sinusoidal.h:197
friend class boost::serialization::access
Definition: sinusoidal.h:226
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
virtual ~sinusoidal()
Destructor.
Definition: sinusoidal.h:107
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
Time time_t
Definition: sinusoidal.h:26
virtual num_t max() const
Get the maximum time for which the curve is defined.
Definition: sinusoidal.h:210
curve_abc< Time, Numeric, Safe, Point > curve_abc_t
Definition: sinusoidal.h:29
virtual sinusoidal_t * compute_derivate_ptr(const std::size_t order) const
Compute the derived curve at orderN.
Definition: sinusoidal.h:159