9 #ifndef _CLASS_SINUSOIDALCURVE 10 #define _CLASS_SINUSOIDALCURVE 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> {
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())
49 phi_(std::fmod(phi, 2. * M_PI)),
54 throw std::invalid_argument(
"can't create constant curve: min bound is higher than max bound");
56 if (
T_ <= 0.)
throw std::invalid_argument(
"The period must be strictly positive");
58 throw std::invalid_argument(
"The offset and the amplitude must have the same dimension");
69 const time_t T_max = std::numeric_limits<time_t>::max())
72 throw std::invalid_argument(
"can't create constant curve: min bound is higher than max bound");
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.;
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");
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");
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.));
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.);
149 const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision())
const {
150 return ndcurves::isApprox<time_t>(
T_min_, other.
min()) && ndcurves::isApprox<time_t>(
T_max_, other.
max()) &&
152 ndcurves::isApprox<time_t>(
T_, other.
T_) && ndcurves::isApprox<time_t>(
phi_, other.
phi_);
156 const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision())
const {
157 const sinusoidal_t* other_cast =
dynamic_cast<const sinusoidal_t*
>(other);
166 virtual bool operator!=(
const sinusoidal_t& other)
const {
return !(*
this == other); }
171 std::size_t
virtual dim()
const {
return dim_; }
180 virtual std::size_t
degree()
const {
return 1; }
195 template <
class Archive>
196 void serialize(Archive& ar,
const unsigned int version) {
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_);
211 inline const num_t two_pi_f(
const time_t& t)
const {
return (2 * M_PI / T_) * t; }
216 DEFINE_CLASS_TEMPLATE_VERSION(SINGLE_ARG(
typename Time,
typename Numeric,
bool Safe,
typename Point),
219 #endif // _CLASS_SINUSOIDALCURVE Definition: bernstein.h:20
virtual bool isApprox(const curve_abc_t *other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
Definition: sinusoidal.h:155
Point amplitude_
Definition: sinusoidal.h:185
virtual num_t max() const
Get the maximum time for which the curve is defined.
Definition: sinusoidal.h:177
friend class boost::serialization::access
Definition: sinusoidal.h:193
Point point_derivate_t
Definition: sinusoidal.h:24
Point p0_
Definition: sinusoidal.h:184
time_t phi_
Definition: sinusoidal.h:187
virtual std::size_t dim() const
Get dimension of curve.
Definition: sinusoidal.h:171
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
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
virtual sinusoidal_t * compute_derivate_ptr(const std::size_t order) const
Compute the derived curve at orderN.
Definition: sinusoidal.h:136
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
virtual std::size_t degree() const
Get the degree of the curve.
Definition: sinusoidal.h:180
time_t T_min_
Definition: sinusoidal.h:188
interface for a Curve of arbitrary dimension.
time_t T_
Definition: sinusoidal.h:186
Time time_t
Definition: sinusoidal.h:25
sinusoidal(const sinusoidal_t &other)
Copy constructor.
Definition: sinusoidal.h:83
time_t T_max_
Definition: sinusoidal.h:188
sinusoidal()
Empty constructor. Curve obtained this way can not perform other class functions. ...
Definition: sinusoidal.h:34
double Time
Definition: effector_spline.h:27
std::size_t dim_
Definition: sinusoidal.h:189
sinusoidal< Time, Numeric, Safe, Point > sinusoidal_t
Definition: sinusoidal.h:27
virtual num_t min() const
Get the minimum time for which the curve is defined.
Definition: sinusoidal.h:174
curve_abc< Time, Numeric, Safe, Point > curve_abc_t
Definition: sinusoidal.h:28
Numeric num_t
Definition: sinusoidal.h:26
virtual point_t operator()(const time_t t) const
Evaluation of the cubic spline at time t.
Definition: sinusoidal.h:100
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:43
Point point_t
Definition: sinusoidal.h:23
double Numeric
Definition: effector_spline.h:26
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:44
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:166
void serialize(Archive &ar, const unsigned int version)
Definition: sinusoidal.h:196
virtual bool operator==(const sinusoidal_t &other) const
Definition: sinusoidal.h:164
virtual ~sinusoidal()
Destructor.
Definition: sinusoidal.h:93