43 int main(
int argc,
char *argv[])
47 float a0[3] = {0.f, 0.f, 0.f};
48 float v0[3] = {0.f, 0.f, 0.f};
49 float x0[3] = {0.f, 0.f, 0.f};
55 for (
int i = 0; i < 3; i++) {
63 const float dt = 0.01f;
65 float velocity_setpoint[3] = {1.f, 0.f, -1.f};
67 for (
int i = 0; i < 3; i++) {
72 int nb_steps =
ceil(t123 / dt);
73 printf(
"Nb steps = %d\n", nb_steps);
75 for (
int i = 0; i < nb_steps; i++) {
76 for (
int i = 0; i < 3; i++) {
80 for (
int i = 0; i < 3; i++) {
86 for (
int i = 0; i < 1; i++) {
87 printf(
"Traj[%d]\n", i);
88 printf(
"jerk = %.3f\taccel = %.3f\tvel = %.3f\tpos = %.3f\n",
89 trajectory[i].getCurrentJerk(),
90 trajectory[i].getCurrentAcceleration(),
91 trajectory[i].getCurrentVelocity(),
92 trajectory[i].getCurrentPosition());
93 printf(
"T1 = %.3f\tT2 = %.3f\tT3 = %.3f\n", trajectory[i].getT1(), trajectory[i].getT2(), trajectory[i].getT3());
int main(int argc, char *argv[])
Test code for the Velocity Smoothing library Build and run using: make && .
Dual< Scalar, N > ceil(const Dual< Scalar, N > &a)
void setMaxJerk(float max_jerk)
void setCurrentVelocity(const float vel)
TODO: document the algorithm |T1| T2 |T3| __| |____ __ Jerk |_| / \ Acceleration ___/ ___ ;" / / V...
void updateTraj(float dt, float time_stretch=1.f)
Generate the trajectory (acceleration, velocity and position) by integrating the current jerk...
float getTotalTime() const
void updateDurations(float vel_setpoint)
Compute T1, T2, T3 depending on the current state and velocity setpoint.
static void timeSynchronization(VelocitySmoothing *traj, int n_traj)
Synchronize several trajectories to have the same total time.
void setMaxVel(float max_vel)
void setMaxAccel(float max_accel)
void setCurrentAcceleration(const float accel)