9 #ifndef _parameteric_curves_infinite_constant_acceleration_hpp 10 #define _parameteric_curves_infinite_constant_acceleration_hpp 21 template <
typename Numeric = double, Eigen::Index Dim = 1,
22 typename Point = Eigen::Matrix<Numeric, Dim, 1> >
34 const time_t& traj_time_,
35 const point_t& x_init_ = Eigen::Matrix<Numeric, Dim, 1>::Zero(),
36 const point_t& x_final_ = Eigen::Matrix<Numeric, Dim, 1>::Zero())
37 : curve_abc_t(-1, -1),
46 virtual const point_t
operator()(
const time_t& t_)
const {
47 const time_t t = std::fmod(t_, 2 *
traj_time);
49 if (t <= 0.5 * traj_time)
50 return x_init + 0.5 * m_ddx0 * t * t;
51 else if (t > 0.5 * traj_time && t <= 1.5 * traj_time)
53 0.5 * m_ddx0 * traj_time * (t - 0.5 *
traj_time) -
54 0.5 * m_ddx0 * (t - 0.5 * traj_time) * (t - 0.5 *
traj_time);
57 0.5 * m_ddx0 * traj_time * (t - 1.5 *
traj_time) +
58 0.5 * m_ddx0 * (t - 1.5 * traj_time) * (t - 1.5 *
traj_time);
61 virtual const point_t
derivate(
const time_t& t_,
62 const std::size_t& order)
const {
63 const time_t t = std::fmod(t_, 2 *
traj_time);
66 if (t <= 0.5 * traj_time)
68 else if (t > 0.5 * traj_time && t <= 1.5 * traj_time)
69 return 0.5 * m_ddx0 * traj_time - m_ddx0 * (t - 0.5 *
traj_time);
71 return -0.5 * m_ddx0 * traj_time + m_ddx0 * (t - 1.5 *
traj_time);
72 }
else if (order == 2) {
73 if (t <= 0.5 * traj_time)
75 else if (t > 0.5 * traj_time && t <= 1.5 * traj_time)
80 std::cerr <<
"Higher order derivatives not supported" << std::endl;
81 return point_t::Zero(Dim);
87 if (x_init_.size() !=
x_init.size())
return false;
92 if (Dim != 1)
return false;
98 if (
x_final.size() != x_final_.size())
return false;
104 if (Dim != 1)
return false;
110 if (traj_time_ <= 0.0)
return false;
122 #endif //_CLASS_EXACTCUBIC virtual bool setFinalPoint(const Eigen::VectorXd &x_final_)
Definition: infinite-const-acc.hpp:97
Numeric num_t
Definition: infinite-const-acc.hpp:26
time_t traj_time
Definition: infinite-const-acc.hpp:119
virtual const point_t operator()(const time_t &t_) const
Evaluation of the cubic spline at time t.
Definition: infinite-const-acc.hpp:46
virtual const point_t derivate(const time_t &t_, const std::size_t &order) const
Evaluation of the derivative spline at time t.
Definition: infinite-const-acc.hpp:61
virtual bool setTrajectoryTime(double traj_time_)
Definition: infinite-const-acc.hpp:109
InfiniteConstAcc(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: infinite-const-acc.hpp:33
double Numeric
Definition: effector_spline.h:26
Point point_t
Definition: infinite-const-acc.hpp:24
Creates InfiniteConstAcc curve s = s_0 + u_0*t+0.5*a_0*t^2.
Definition: infinite-const-acc.hpp:23
AbstractCurve< Numeric, Point > curve_abc_t
Definition: infinite-const-acc.hpp:28
point_t x_init
Definition: infinite-const-acc.hpp:117
virtual bool setInitialPoint(const point_t &x_init_)
Definition: infinite-const-acc.hpp:86
Numeric time_t
Definition: infinite-const-acc.hpp:25
Represents a curve of dimension Dim is Safe is false, no verification is made on the evaluation of th...
Definition: abstract-curve.hpp:21
Definition: abstract-curve.hpp:16
point_t x_final
Definition: infinite-const-acc.hpp:118
virtual bool setFinalPoint(const double &x_final_)
Definition: infinite-const-acc.hpp:103
~InfiniteConstAcc()
Destructor.
Definition: infinite-const-acc.hpp:43
Eigen::Matrix< Numeric, 3, 1 > Point
Definition: effector_spline.h:28
virtual bool setInitialPoint(const double &x_init_)
Definition: infinite-const-acc.hpp:91