39 #include <gtest/gtest.h> 49 void setConstraints(
float j_max,
float a_max,
float v_max);
51 void updateTrajectories(
float dt,
Vector3f velocity_setpoints);
58 for (
int i = 0; i < 3; i++) {
59 _trajectories[i].setMaxJerk(j_max);
60 _trajectories[i].setMaxAccel(a_max);
61 _trajectories[i].setMaxVel(v_max);
67 for (
int i = 0; i < 3; i++) {
68 _trajectories[i].setCurrentAcceleration(a0(i));
69 _trajectories[i].setCurrentVelocity(v0(i));
70 _trajectories[i].setCurrentPosition(x0(i));
76 for (
int i = 0; i < 3; i++) {
77 _trajectories[i].updateTraj(dt);
78 EXPECT_LE(fabsf(_trajectories[i].getCurrentJerk()), _trajectories[i].getMaxJerk());
79 EXPECT_LE(fabsf(_trajectories[i].getCurrentAcceleration()), _trajectories[i].getMaxAccel());
80 EXPECT_LE(fabsf(_trajectories[i].getCurrentVelocity()), _trajectories[i].getMaxVel());
83 for (
int i = 0; i < 3; i++) {
84 _trajectories[i].updateDurations(velocity_setpoints(i));
93 const float j_max = 55.2f;
94 const float a_max = 6.f;
95 const float v_max = 6.f;
97 setConstraints(j_max, a_max, v_max);
104 setInitialConditions(a0, v0, x0);
108 updateTrajectories(0.
f, velocity_setpoints);
112 EXPECT_LE(fabsf(_trajectories[0].getTotalTime() - _trajectories[1].getTotalTime()), 0.0001);
118 const float j_max = 55.2f;
119 const float a_max = 6.f;
120 const float v_max = 6.f;
122 setConstraints(j_max, a_max, v_max);
129 setInitialConditions(a0, v0, x0);
136 const float dt = 0.01;
137 updateTrajectories(0.
f, velocity_setpoints);
138 float t123 = _trajectories[0].getTotalTime();
139 int nb_steps =
ceil(t123 / dt);
141 for (
int i = 0; i < nb_steps; i++) {
142 updateTrajectories(dt, velocity_setpoints);
147 for (
int i = 0; i < 3; i++) {
148 EXPECT_LE(fabsf(_trajectories[i].getCurrentVelocity() - velocity_setpoints(i)), 0.01
f);
149 EXPECT_LE(fabsf(_trajectories[i].getCurrentAcceleration()), 0.0001
f);
160 setInitialConditions(a0, v0, x0);
165 const float dt = 0.01f;
168 for (
int i = 0; i < 60; i++) {
169 updateTrajectories(dt, velocity_setpoints);
174 for (
int i = 0; i < 3; i++) {
175 EXPECT_EQ(_trajectories[i].getCurrentJerk(), 0.
f);
176 EXPECT_EQ(_trajectories[i].getCurrentAcceleration(), 0.
f);
177 EXPECT_EQ(_trajectories[i].getCurrentVelocity(), 0.
f);
178 EXPECT_EQ(_trajectories[i].getCurrentPosition(), 0.
f);
Dual< Scalar, N > ceil(const Dual< Scalar, N > &a)
TODO: document the algorithm |T1| T2 |T3| __| |____ __ Jerk |_| / \ Acceleration ___/ ___ ;" / / V...
TEST_F(VelocitySmoothingTest, testTimeSynchronization)
void updateTrajectories(float dt, Vector3f velocity_setpoints)
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
static void timeSynchronization(VelocitySmoothing *traj, int n_traj)
Synchronize several trajectories to have the same total time.
void setInitialConditions(Vector3f acc, Vector3f vel, Vector3f pos)
void setConstraints(float j_max, float a_max, float v_max)