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.

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

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 &&

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

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;
 
+	_direction = computeDirection();
+
+	if (_direction != 0) {
+		updateDurationsMinimizeTotalTime();
+
+	} else {
+		_T1 = _T2 = _T3 = 0.f;
+	}
+}
+
+int VelocitySmoothing::computeDirection()
+{
 	// 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;
-	}
+	float vel_zero_acc = computeVelAtZeroAcc();
 
 	/* Depending of the direction, start accelerating positively or negatively */
-	_direction = math::sign(_vel_sp - vel_zero_acc);
-	if (_direction == 0) {
+	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(_vel_sp - _state.v);
+		direction = -math::sign(_state.a);
 	}
 
-	if (_direction != 0) {
-		updateDurations();
+	return direction;
+}
 
-	} else {
-		_T1 = _T2 = _T3 = 0.f;
+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::updateDurations(float T123)
+void VelocitySmoothing::updateDurationsMinimizeTotalTime()
 {
-	float T1, T2, T3;
-
 	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;
+		t1 = _T1;
+		t2 = _T2;
+		t3 = _T3;
+		t4 = t - _T1 - _T2 - _T3;
+	}
+
+	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);
-		_state = evaluatePoly(0.f, 0.f, _state.v, _state.x, t4, 0.f);
 	}
 
-	accel_setpoint_smooth = _state.a;
-	vel_setpoint_smooth = _state.v;
-	pos_setpoint_smooth = _state.x;
+	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);
+}

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 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
 	 */
-	void updateDurations(float T123 = -1.f);
+	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
 };

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;
 	}
 

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");
 		}