31 if (vel >
sqrt(std::fabs(Se) * accl / 2))
33 vel =
sqrt(std::fabs(Se) * accl / 2);
63 std::vector<double> array;
65 double direction = se/std::fabs(se);
73 array.push_back(direction * (accl*(0.25*
pow(i*t_ipo, 2) +
pow(pt.
tb, 2)/(8*
pow(M_PI, 2)) *(
cos(2*M_PI/pt.
tb * (i*t_ipo))-1))));
78 array.push_back(direction * (pt.
vel*(i*t_ipo-0.5*pt.
tb)));
83 array.push_back(direction * (0.5 * accl *(pt.
te*(i*t_ipo + pt.
tb) - 0.5*(
pow(i*t_ipo, 2)+
pow(pt.
te, 2)+2*
pow(pt.
tb, 2)) + (
pow(pt.
tb, 2)/(4*
pow(M_PI, 2))) * (1-
cos(((2*M_PI)/pt.
tb) * (i*t_ipo-pt.
tv))))));
const cob_cartesian_controller::CartesianActionStruct & params_
#define MIN_VELOCITY_THRESHOLD
double roundUpToMultiplier(const double numberToRound, const double multiplier)
virtual bool getProfileTimings(double Se, double te, bool calcMaxTe, cob_cartesian_controller::ProfileTimings &pt)
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
INLINE Rall1d< T, V, S > pow(const Rall1d< T, V, S > &arg, double m)
virtual std::vector< double > getTrajectory(double se, cob_cartesian_controller::ProfileTimings pt)
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)