1 #ifndef ORG_EEROS_CONTROL_CONSTANTACCTRAJECTORYGENERATOR_HPP_ 2 #define ORG_EEROS_CONTROL_CONSTANTACCTRAJECTORYGENERATOR_HPP_ 18 using E =
typename T::value_type;
23 std::lock_guard<std::mutex> lck(
mtx);
27 virtual std::array<T, 3>
get(
double dt) {
28 std::lock_guard<std::mutex> lck(
mtx);
29 std::array<T, 3> y = this->
last;
32 for(
unsigned int i = 0; i <
a1p.size(); i++) {
34 if(
t >= 0 &&
t <
dT1) {
35 y[0][i] =
a1p[i] * pow(
t, 2) +
c1p[i];
51 y[0][i] = this->
last[0][i];
56 throw Fault(
"get() failed, t < 0");
60 this->
last[0][i] = y[0][i];
61 this->
last[1][i] = y[1][i];
62 this->
last[2][i] = y[2][i];
68 virtual bool push(std::array<T, 3> start, std::array<T, 3> end) {
70 T calcVelNorm, calcAccNorm, calcDecNorm;
71 E velNorm, accNorm, decNorm, squareNormVel;
72 T distance = end[0] - start[0];
80 for(
unsigned int i = 0; i < calcVelNorm.size(); i++) {
81 calcVelNorm[i] = fabs(
velMax[i] / distance[i]);
82 calcAccNorm[i] = fabs(
accMax[i] / distance[i]);
83 calcDecNorm[i] = fabs(
decMax[i] / distance[i]);
87 velNorm = calcVelNorm[0]; accNorm = calcAccNorm[0]; decNorm = calcDecNorm[0];
88 for(
unsigned int i = 0; i < calcVelNorm.size(); i++) {
89 if(calcVelNorm[i] < velNorm) velNorm = calcVelNorm[i];
90 if(calcAccNorm[i] < accNorm) accNorm = calcAccNorm[i];
91 if(calcDecNorm[i] < decNorm) decNorm = calcDecNorm[i];
95 squareNormVel = sqrt(2 * (accNorm * decNorm) / (accNorm + decNorm));
96 if(velNorm > squareNormVel) velNorm = squareNormVel;
99 dT1 = velNorm / accNorm;
100 dT3 = velNorm / decNorm;
111 a1p = 0.5 * velNorm /
dT1 * distance *
dt;
113 b1v = velNorm /
dT1 * dt * distance *
dt;
114 c1a = velNorm * dt /
dT1 * distance * pow(dt, 2);
116 b2p = velNorm * dt * distance;
117 c2p = start[0] - 0.5 * velNorm *
dT1 * dt * distance;
118 c2v = velNorm * distance * dt *
dt;
121 a3p = (-1) * velNorm * 0.5 * dt /
dT3 * distance;
123 c3p = start[0] + (1 - velNorm * 0.5 * dt /
dT3 * pow(
dT1 +
dT2 +
dT3, 2)) * distance;
124 b3v = (-1) *velNorm * dt * distance /
dT3 * dt;
126 c3a = (-1) * velNorm * dt /
dT3 * distance * dt * dt;
128 std::lock_guard<std::mutex> lck(
mtx);
136 std::lock_guard<std::mutex> lck(
mtx);
158 T
a1p,
c1p,
b1v,
c1a,
b2p,
c2p,
c2v,
c2a,
a3p,
b3p,
c3p,
b3v,
c3v,
c3a;
T velMax
Definition: ConstantAccTrajectoryGenerator.hpp:155
virtual void reset(std::array< T, 3 > last)
Definition: ConstantAccTrajectoryGenerator.hpp:135
T a1p
Definition: ConstantAccTrajectoryGenerator.hpp:158
T b1v
Definition: ConstantAccTrajectoryGenerator.hpp:158
T c3a
Definition: ConstantAccTrajectoryGenerator.hpp:158
typename T::value_type E
Definition: ConstantAccTrajectoryGenerator.hpp:18
virtual void setMaxDec(T dec)
Definition: ConstantAccTrajectoryGenerator.hpp:148
virtual void setMaxAcc(T acc)
Definition: ConstantAccTrajectoryGenerator.hpp:145
virtual bool finished()
Definition: ConstantAccTrajectoryGenerator.hpp:22
T c2p
Definition: ConstantAccTrajectoryGenerator.hpp:158
T b3p
Definition: ConstantAccTrajectoryGenerator.hpp:158
Definition: TrajectoryGenerator.hpp:10
T c1p
Definition: ConstantAccTrajectoryGenerator.hpp:158
Definition: Config.hpp:14
virtual bool push(std::array< T, 3 > start, std::array< T, 3 > end)
Definition: ConstantAccTrajectoryGenerator.hpp:68
Definition: ConstantAccTrajectoryGenerator.hpp:14
T b2p
Definition: ConstantAccTrajectoryGenerator.hpp:158
std::mutex mtx
Definition: ConstantAccTrajectoryGenerator.hpp:153
std::array< T, N > last
Definition: TrajectoryGenerator.hpp:46
T c3v
Definition: ConstantAccTrajectoryGenerator.hpp:158
virtual void setMaxSpeed(T speed)
Definition: ConstantAccTrajectoryGenerator.hpp:141
T a3p
Definition: ConstantAccTrajectoryGenerator.hpp:158
double t
Definition: ConstantAccTrajectoryGenerator.hpp:160
T c1a
Definition: ConstantAccTrajectoryGenerator.hpp:158
T c2a
Definition: ConstantAccTrajectoryGenerator.hpp:158
double dT1
Definition: ConstantAccTrajectoryGenerator.hpp:157
double dT2
Definition: ConstantAccTrajectoryGenerator.hpp:157
bool finish
Definition: ConstantAccTrajectoryGenerator.hpp:161
T c3p
Definition: ConstantAccTrajectoryGenerator.hpp:158
T accMax
Definition: ConstantAccTrajectoryGenerator.hpp:155
T b3v
Definition: ConstantAccTrajectoryGenerator.hpp:158
ConstantAccTrajectoryGenerator(T velMax, T accMax, T decMax, double dt)
Definition: ConstantAccTrajectoryGenerator.hpp:20
T decMax
Definition: ConstantAccTrajectoryGenerator.hpp:155
T c2v
Definition: ConstantAccTrajectoryGenerator.hpp:158
double dT3
Definition: ConstantAccTrajectoryGenerator.hpp:157
double dt
Definition: ConstantAccTrajectoryGenerator.hpp:154