From 8cc2a7018e17f62fbc682dee6cc41dea4bfcf75b Mon Sep 17 00:00:00 2001 From: bresch Date: Sun, 8 Sep 2019 15:42:18 +0200 Subject: [PATCH] VelocitySmoothing - Re-enable time stretch, integrate dt to get local time. Also split a few functions into smaller ones for readability, fix formatting, use geters to get the current state of the trajectory instead of return arguments. --- .../FlightTaskAutoLineSmoothVel.cpp | 15 +- .../FlightTaskManualAltitudeSmoothVel.cpp | 11 +- .../FlightTaskManualPositionSmoothVel.cpp | 8 +- .../tasks/Utility/VelocitySmoothing.cpp | 164 ++++++++++-------- .../tasks/Utility/VelocitySmoothing.hpp | 37 ++-- .../tasks/Utility/VelocitySmoothingTest.cpp | 18 +- .../tasks/Utility/test_velocity_smoothing.cpp | 19 +- 7 files changed, 160 insertions(+), 112 deletions(-) diff --git a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp index 878ac17a55..af7f18cc00 100644 --- a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp +++ b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp @@ -349,13 +349,13 @@ void FlightTaskAutoLineSmoothVel::_generateTrajectory() Vector2f position_xy(_position); Vector2f vel_traj_xy(_trajectory[0].getCurrentVelocity(), _trajectory[1].getCurrentVelocity()); Vector2f drone_to_trajectory_xy(position_trajectory_xy - position_xy); - //float position_error = drone_to_trajectory_xy.length(); + float position_error = drone_to_trajectory_xy.length(); - //float time_stretch = 1.f - math::constrain(position_error * 0.5f, 0.f, 1.f); + float time_stretch = 1.f - math::constrain(position_error * 0.5f, 0.f, 1.f); // Don't stretch time if the drone is ahead of the position setpoint if (drone_to_trajectory_xy.dot(vel_traj_xy) < 0.f) { - //time_stretch = 1.f; + time_stretch = 1.f; } Vector3f jerk_sp_smooth; @@ -364,16 +364,17 @@ void FlightTaskAutoLineSmoothVel::_generateTrajectory() Vector3f pos_sp_smooth; for (int i = 0; i < 3; ++i) { - // TODO: fix time stretch - //_trajectory[i].updateTraj(_time_stamp_current, time_stretch, accel_sp_smooth(i), vel_sp_smooth(i), pos_sp_smooth(i)); - _trajectory[i].updateTraj(_time, accel_sp_smooth(i), vel_sp_smooth(i), pos_sp_smooth(i)); + _trajectory[i].updateTraj(_deltatime, time_stretch); jerk_sp_smooth(i) = _trajectory[i].getCurrentJerk(); + accel_sp_smooth(i) = _trajectory[i].getCurrentAcceleration(); + vel_sp_smooth(i) = _trajectory[i].getCurrentVelocity(); + pos_sp_smooth(i) = _trajectory[i].getCurrentPosition(); } _updateTrajConstraints(); for (int i = 0; i < 3; ++i) { - _trajectory[i].updateDurations(_time, _velocity_setpoint(i)); + _trajectory[i].updateDurations(_velocity_setpoint(i)); } VelocitySmoothing::timeSynchronization(_trajectory, 2); // Synchronize x and y only diff --git a/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp b/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp index 28afa170b9..3c1b507596 100644 --- a/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp +++ b/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp @@ -147,7 +147,7 @@ void FlightTaskManualAltitudeSmoothVel::_updateSetpoints() jerk = _position_lock_z_active ? 1.f : _param_mpc_jerk_max.get(); _smoothing.setMaxJerk(jerk); - _smoothing.updateDurations(_deltatime, _velocity_setpoint(2)); + _smoothing.updateDurations(_velocity_setpoint(2)); if (!_position_lock_z_active) { _smoothing.setCurrentPosition(_position(2)); @@ -156,9 +156,14 @@ void FlightTaskManualAltitudeSmoothVel::_updateSetpoints() float pos_sp_smooth; // TODO: move before updateDurations - _smoothing.updateTraj(_time_stamp_current, _acceleration_setpoint(2), _vel_sp_smooth, pos_sp_smooth); - _velocity_setpoint(2) = _vel_sp_smooth; // Feedforward + _smoothing.updateTraj(_deltatime); + _jerk_setpoint(2) = _smoothing.getCurrentJerk(); + _acceleration_setpoint(2) = _smoothing.getCurrentAcceleration(); + _vel_sp_smooth = _smoothing.getCurrentVelocity(); + pos_sp_smooth = _smoothing.getCurrentPosition(); + + _velocity_setpoint(2) = _vel_sp_smooth; // Feedforward if (fabsf(_vel_sp_smooth) < 0.1f && fabsf(_acceleration_setpoint(2)) < .2f && diff --git a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp index 3d42e8f959..b8f4eee53b 100644 --- a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp +++ b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp @@ -144,8 +144,12 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints() Vector3f pos_sp_smooth; for (int i = 0; i < 3; ++i) { - _smoothing[i].updateTraj(_time_stamp_current, _acceleration_setpoint(i), _vel_sp_smooth(i), pos_sp_smooth(i)); + _smoothing[i].updateTraj(_deltatime); + _jerk_setpoint(i) = _smoothing[i].getCurrentJerk(); + _acceleration_setpoint(i) = _smoothing[i].getCurrentAcceleration(); + _vel_sp_smooth(i) = _smoothing[i].getCurrentVelocity(); + pos_sp_smooth(i) = _smoothing[i].getCurrentPosition(); } /* Get yaw setpont, un-smoothed position setpoints.*/ @@ -203,7 +207,7 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints() for (int i = 0; i < 3; ++i) { _smoothing[i].setMaxJerk(jerk[i]); - _smoothing[i].updateDurations(_time_stamp_current, _velocity_setpoint(i)); + _smoothing[i].updateDurations(_velocity_setpoint(i)); } VelocitySmoothing::timeSynchronization(_smoothing, 2); // Synchronize x and y only diff --git a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp index 5be4af3f9f..b4fa42ad0c 100644 --- a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp +++ b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp @@ -78,7 +78,7 @@ float VelocitySmoothing::computeT1(float a0, float v3, float j_max, float a_max) float sqrt_delta = sqrtf(delta); float T1_plus = (-a0 + 0.5f * sqrt_delta) / j_max; - float T1_minus = (-a0 - 0.5f * sqrt_delta) / j_max; + float T1_minus = (-a0 - 0.5f * sqrt_delta) / j_max; float T3_plus = a0 / j_max + T1_plus; float T3_minus = a0 / j_max + T1_minus; @@ -101,7 +101,7 @@ float VelocitySmoothing::computeT1(float T123, float a0, float v3, float j_max, { float a = -j_max; float b = j_max * T123 - a0; - float delta = T123 * T123 * j_max * j_max + 2.f * T123 *a0 * j_max - a0 * a0 - 4.f * j_max * v3; + float delta = T123 * T123 * j_max * j_max + 2.f * T123 * a0 * j_max - a0 * a0 - 4.f * j_max * v3; if (delta < 0.f) { // Solution is not real @@ -134,14 +134,14 @@ float VelocitySmoothing::computeT1(float T123, float a0, float v3, float j_max, } -float VelocitySmoothing::computeT2(float T1, float T3, float a0 , float v3, float j_max) +float VelocitySmoothing::computeT2(float T1, float T3, float a0, float v3, float j_max) { float T2 = 0.f; float den = a0 + j_max * T1; if (math::abs_t(den) > FLT_EPSILON) { - T2 = (-0.5f *T1 * T1 * j_max - T1 * T3*j_max - T1 * a0 + 0.5f * T3 * T3 * j_max - T3 * a0 + v3) / den; + T2 = (-0.5f * T1 * T1 * j_max - T1 * T3 * j_max - T1 * a0 + 0.5f * T3 * T3 * j_max - T3 * a0 + v3) / den; } return math::max(T2, 0.f); @@ -156,70 +156,69 @@ float VelocitySmoothing::computeT2(float T123, float T1, float T3) float VelocitySmoothing::computeT3(float T1, float a0, float j_max) { float T3 = a0 / j_max + T1; - return math::max(T3, 0.f); } -void VelocitySmoothing::updateDurations(float t_now, float vel_setpoint) +void VelocitySmoothing::updateDurations(float vel_setpoint) { _vel_sp = math::constrain(vel_setpoint, -_max_vel, _max_vel); - _t0 = t_now; + _local_time = 0.f; _state_init = _state; - // Compute the velocity at which the trajectory will be - // when the acceleration will be zero - float vel_zero_acc = _state.v; - if (fabsf(_state.a) > FLT_EPSILON) { - float j_zero_acc = -math::sign(_state.a) * _max_jerk; // Required jerk to reduce the acceleration - float t_zero_acc = -_state.a / j_zero_acc; // Required time to cancel the current acceleration - vel_zero_acc = _state.v + _state.a * t_zero_acc + 0.5f * j_zero_acc * t_zero_acc * t_zero_acc; - } - - /* Depending of the direction, start accelerating positively or negatively */ - _direction = math::sign(_vel_sp - vel_zero_acc); - if (_direction == 0) { - // If by braking immediately the velocity is exactly - // the require one with zero acceleration, then brake - _direction = -math::sign(_vel_sp - _state.v); - } + _direction = computeDirection(); if (_direction != 0) { - updateDurations(); + updateDurationsMinimizeTotalTime(); } else { _T1 = _T2 = _T3 = 0.f; } } -void VelocitySmoothing::updateDurations(float T123) +int VelocitySmoothing::computeDirection() { - float T1, T2, T3; + // Compute the velocity at which the trajectory will be + // when the acceleration will be zero + float vel_zero_acc = computeVelAtZeroAcc(); + /* Depending of the direction, start accelerating positively or negatively */ + int direction = math::sign(_vel_sp - vel_zero_acc); + + if (direction == 0) { + // If by braking immediately the velocity is exactly + // the require one with zero acceleration, then brake + direction = -math::sign(_state.a); + } + + return direction; +} + +float VelocitySmoothing::computeVelAtZeroAcc() +{ + float vel_zero_acc = _state.v; + + if (fabsf(_state.a) > FLT_EPSILON) { + float j_zero_acc = -math::sign(_state.a) * _max_jerk; // Required jerk to reduce the acceleration + float t_zero_acc = -_state.a / j_zero_acc; // Required time to cancel the current acceleration + vel_zero_acc = _state.v + _state.a * t_zero_acc + 0.5f * j_zero_acc * t_zero_acc * t_zero_acc; + } + + return vel_zero_acc; +} + +void VelocitySmoothing::updateDurationsMinimizeTotalTime() +{ float jerk_max_T1 = _direction * _max_jerk; float delta_v = _vel_sp - _state.v; // compute increasing acceleration time - if (T123 < 0.f) { - T1 = computeT1(_state.a, delta_v, jerk_max_T1, _max_accel); - - } else { - T1 = computeT1(T123, _state.a, delta_v, jerk_max_T1, _max_accel); - } + _T1 = computeT1(_state.a, delta_v, jerk_max_T1, _max_accel); // compute decreasing acceleration time - T3 = computeT3(T1, _state.a, jerk_max_T1); + _T3 = computeT3(_T1, _state.a, jerk_max_T1); // compute constant acceleration time - if (T123 < 0.f) { - T2 = computeT2(T1, T3, _state.a, delta_v, jerk_max_T1); - - } else { - T2 = computeT2(T123, T1, T3); - } - - _T1 = T1; - _T2 = T2; - _T3 = T3; + _T2 = computeT2(_T1, _T3, _state.a, delta_v, jerk_max_T1); } Trajectory VelocitySmoothing::evaluatePoly(float j, float a0, float v0, float x0, float t, int d) @@ -237,42 +236,54 @@ Trajectory VelocitySmoothing::evaluatePoly(float j, float a0, float v0, float x0 return traj; } -void VelocitySmoothing::updateTraj(float t_now, float &accel_setpoint_smooth, float &vel_setpoint_smooth, float &pos_setpoint_smooth) +void VelocitySmoothing::updateTraj(float dt, float time_stretch) { - const float t = t_now - _t0; + updateTraj(dt * time_stretch); +} + +void VelocitySmoothing::updateTraj(float dt) +{ + _local_time += dt; + const float t = _local_time; + float t1 = 0.f; + float t2 = 0.f; + float t3 = 0.f; + float t4 = 0.f; if (t <= _T1) { - float t1 = t; - _state = evaluatePoly(_max_jerk, _state_init.a, _state_init.v, _state_init.x, t1, _direction); + t1 = t; } else if (t <= _T1 + _T2) { - float t1 = _T1; - float t2 = t - _T1; - _state = evaluatePoly(_max_jerk, _state_init.a, _state_init.v, _state_init.x, t1, _direction); - _state = evaluatePoly(0.f, _state.a, _state.v, _state.x, t2, 0.f); + t1 = _T1; + t2 = t - _T1; } else if (t <= _T1 + _T2 + _T3) { - float t1 = _T1; - float t2 = _T2; - float t3 = t - _T1 - _T2; - _state = evaluatePoly(_max_jerk, _state_init.a, _state_init.v, _state_init.x, t1, _direction); - _state = evaluatePoly(0.f, _state.a, _state.v, _state.x, t2, 0.f); - _state = evaluatePoly(_max_jerk, _state.a, _state.v, _state.x, t3, -_direction); + t1 = _T1; + t2 = _T2; + t3 = t - _T1 - _T2; } else { - float t1 = _T1; - float t2 = _T2; - float t3 = _T3; - float t4 = t - _T1 - _T2 - _T3; - _state = evaluatePoly(_max_jerk, _state_init.a, _state_init.v, _state_init.x, t1, _direction); - _state = evaluatePoly(0.f, _state.a, _state.v, _state.x, t2, 0.f); - _state = evaluatePoly(_max_jerk, _state.a, _state.v, _state.x, t3, -_direction); - _state = evaluatePoly(0.f, 0.f, _state.v, _state.x, t4, 0.f); + t1 = _T1; + t2 = _T2; + t3 = _T3; + t4 = t - _T1 - _T2 - _T3; } - accel_setpoint_smooth = _state.a; - vel_setpoint_smooth = _state.v; - pos_setpoint_smooth = _state.x; + if (t > 0.f) { + _state = evaluatePoly(_max_jerk, _state_init.a, _state_init.v, _state_init.x, t1, _direction); + } + + if (t >= _T1) { + _state = evaluatePoly(0.f, _state.a, _state.v, _state.x, t2, 0.f); + } + + if (t >= _T1 + _T2) { + _state = evaluatePoly(_max_jerk, _state.a, _state.v, _state.x, t3, -_direction); + } + + if (t >= _T1 + _T2 + _T3) { + _state = evaluatePoly(0.f, 0.f, _state.v, _state.x, t4, 0.f); + } } void VelocitySmoothing::timeSynchronization(VelocitySmoothing *traj, int n_traj) @@ -292,8 +303,23 @@ void VelocitySmoothing::timeSynchronization(VelocitySmoothing *traj, int n_traj) if (desired_time > FLT_EPSILON) { for (int i = 0; i < n_traj; i++) { if (i != longest_traj_index) { - traj[i].updateDurations(desired_time); + traj[i].updateDurationsGivenTotalTime(desired_time); } } } } + +void VelocitySmoothing::updateDurationsGivenTotalTime(float T123) +{ + float jerk_max_T1 = _direction * _max_jerk; + float delta_v = _vel_sp - _state.v; + + // compute increasing acceleration time + _T1 = computeT1(T123, _state.a, delta_v, jerk_max_T1, _max_accel); + + // compute decreasing acceleration time + _T3 = computeT3(_T1, _state.a, jerk_max_T1); + + // compute constant acceleration time + _T2 = computeT2(T123, _T1, _T3); +} diff --git a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp index abcafa7465..37871523df 100644 --- a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp +++ b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp @@ -74,21 +74,18 @@ public: /** * Compute T1, T2, T3 depending on the current state and velocity setpoint. This should be called on every cycle - * and before evaluateTraj(). + * and before updateTraj(). * @param vel_setpoint velocity setpoint input - * @param t current time */ - void updateDurations(float t_now, float vel_setpoint); + void updateDurations(float vel_setpoint); /** * Generate the trajectory (acceleration, velocity and position) by integrating the current jerk * @param dt optional integration period. If not given, the integration period provided during updateDuration call is used. * A dt different from the one given during the computation of T1-T3 can be used to fast-forward or slow-down the trajectory. - * @param acc_setpoint_smooth returned smoothed acceleration setpoint - * @param vel_setpoint_smooth returned smoothed velocity setpoint - * @param pos_setpoint_smooth returned smoothed position setpoint */ - void updateTraj(float t_now, float &accel_setpoint_smooth, float &vel_setpoint_smooth, float &pos_setpoint_smooth); + void updateTraj(float dt, float time_stretch); + void updateTraj(float dt); /* Get / Set constraints (constraints can be updated at any time) */ float getMaxJerk() const { return _max_jerk; } @@ -129,10 +126,28 @@ private: /** * Compute T1, T2, T3 depending on the current state and velocity setpoint. - * @param T123 optional parameter. If set, the total trajectory time will be T123, if not, - * the algorithm optimizes for time. + * Minimize the total time of the trajectory */ - void updateDurations(float T123 = -1.f); + void updateDurationsMinimizeTotalTime(); + + /** + * Compute T1, T2, T3 depending on the current state and velocity setpoint. + * @param T123 desired total time of the trajectory + */ + void updateDurationsGivenTotalTime(float T123); + + /** + * Compute the direction of the jerk to be applied in order to drive the current state + * to the desired one + */ + int computeDirection(); + + /** + * Compute the velocity at which the trajectory will be if the maximum jerk is applied + * during the time required to cancel the current acceleration + */ + float computeVelAtZeroAcc(); + /** * Compute increasing acceleration time */ @@ -187,5 +202,5 @@ private: float _T2 = 0.f; ///< Constant acceleration [s] float _T3 = 0.f; ///< Decreasing acceleration [s] - float _t0 = 0.f; ///< Starting time + float _local_time = 0.f; ///< Current local time }; diff --git a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothingTest.cpp b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothingTest.cpp index 2ae98cc9fd..6cfce4ba4a 100644 --- a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothingTest.cpp +++ b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothingTest.cpp @@ -48,7 +48,7 @@ class VelocitySmoothingTest : public ::testing::Test public: void setConstraints(float j_max, float a_max, float v_max); void setInitialConditions(Vector3f acc, Vector3f vel, Vector3f pos); - void updateTrajectories(float t, Vector3f velocity_setpoints); + void updateTrajectories(float dt, Vector3f velocity_setpoints); VelocitySmoothing _trajectories[3]; }; @@ -71,16 +71,14 @@ void VelocitySmoothingTest::setInitialConditions(Vector3f a0, Vector3f v0, Vecto } } -void VelocitySmoothingTest::updateTrajectories(float t, Vector3f velocity_setpoints) +void VelocitySmoothingTest::updateTrajectories(float dt, Vector3f velocity_setpoints) { - float dummy; // We don't care about the immediate result - for (int i = 0; i < 3; i++) { - _trajectories[i].updateTraj(t, dummy, dummy, dummy); + _trajectories[i].updateTraj(dt); } for (int i = 0; i < 3; i++) { - _trajectories[i].updateDurations(t, velocity_setpoints(i)); + _trajectories[i].updateDurations(velocity_setpoints(i)); } VelocitySmoothing::timeSynchronization(_trajectories, 2); @@ -132,15 +130,13 @@ TEST_F(VelocitySmoothingTest, testConstantSetpoint) // Compute the number of steps required to reach desired value // The updateTrajectories is fist called once to compute the total time - float t = 0.f; const float dt = 0.01; - updateTrajectories(t, velocity_setpoints); + updateTrajectories(0.f, velocity_setpoints); float t123 = _trajectories[0].getTotalTime(); int nb_steps = ceil(t123 / dt); for (int i = 0; i < nb_steps; i++) { - t += dt; - updateTrajectories(t, velocity_setpoints); + updateTrajectories(dt, velocity_setpoints); } // THEN: All the trajectories should have reach their @@ -167,7 +163,7 @@ TEST_F(VelocitySmoothingTest, testZeroSetpoint) // WHEN: We run a few times the algorithm for (int i = 0; i < 60; i++) { - updateTrajectories(t, velocity_setpoints); + updateTrajectories(dt, velocity_setpoints); t += dt; } diff --git a/src/lib/FlightTasks/tasks/Utility/test_velocity_smoothing.cpp b/src/lib/FlightTasks/tasks/Utility/test_velocity_smoothing.cpp index 8608035401..65ef639d5c 100644 --- a/src/lib/FlightTasks/tasks/Utility/test_velocity_smoothing.cpp +++ b/src/lib/FlightTasks/tasks/Utility/test_velocity_smoothing.cpp @@ -60,35 +60,36 @@ int main(int argc, char *argv[]) trajectory[i].setCurrentVelocity(v0[i]); } - float t = 0.f; const float dt = 0.01f; float velocity_setpoint[3] = {1.f, 0.f, -1.f}; for (int i = 0; i < 3; i++) { - trajectory[i].updateDurations(t, velocity_setpoint[i]); + trajectory[i].updateDurations(velocity_setpoint[i]); } - float t123 = trajectory[2].getTotalTime(); + float t123 = trajectory[0].getTotalTime(); int nb_steps = ceil(t123 / dt); printf("Nb steps = %d\n", nb_steps); for (int i = 0; i < nb_steps; i++) { - t += dt; for (int i = 0; i < 3; i++) { - trajectory[i].updateTraj(t, a0[i], v0[i], x0[i]); + trajectory[i].updateTraj(dt); } for (int i = 0; i < 3; i++) { - trajectory[i].updateDurations(t, velocity_setpoint[i]); + trajectory[i].updateDurations(velocity_setpoint[i]); } VelocitySmoothing::timeSynchronization(trajectory, 2); - for (int i = 0; i < 3; i++) { + for (int i = 0; i < 1; i++) { printf("Traj[%d]\n", i); - printf("jerk = %.3f\taccel = %.3f\tvel = %.3f\tpos = %.3f\n", trajectory[i].getCurrentJerk(), - a0[i], v0[i], x0[i]); + printf("jerk = %.3f\taccel = %.3f\tvel = %.3f\tpos = %.3f\n", + trajectory[i].getCurrentJerk(), + trajectory[i].getCurrentAcceleration(), + trajectory[i].getCurrentVelocity(), + trajectory[i].getCurrentPosition()); printf("T1 = %.3f\tT2 = %.3f\tT3 = %.3f\n", trajectory[i].getT1(), trajectory[i].getT2(), trajectory[i].getT3()); printf("\n"); }