VelocitySmoothing: fix division by zero cases

Problem: Zero maximum jerk and/or zero
direction would lead to divisions by zero in
various cases depending on the exact configuration and code path.

Solution: There was already one check for the
direction being zero in one path and I
summarized to check in both
updateDurations...() functions the product
of direction * max_jerk to not be zero
because that's exactly the value calculations
devide by.

src/lib/motion_planning/VelocitySmoothing.cpp

@@ -168,12 +168,7 @@ void VelocitySmoothing::updateDurations(float vel_setpoint)
 
 	_direction = computeDirection();
 
-	if (_direction != 0) {
-		updateDurationsMinimizeTotalTime();
-
-	} else {
-		_T1 = _T2 = _T3 = 0.f;
-	}
+	updateDurationsMinimizeTotalTime();
 }
 
 int VelocitySmoothing::computeDirection() const
@@ -187,7 +182,7 @@ int VelocitySmoothing::computeDirection() const
 
 	if (direction == 0) {
 		// If by braking immediately the velocity is exactly
-		// the require one with zero acceleration, then brake
+		// the required one with zero acceleration, then brake
 		direction = sign(_state.a);
 	}
 
@@ -212,14 +207,19 @@ void VelocitySmoothing::updateDurationsMinimizeTotalTime()
 	float jerk_max_T1 = _direction * _max_jerk;
 	float delta_v = _vel_sp - _state.v;
 
-	// compute increasing acceleration time
-	_T1 = computeT1(_state.a, delta_v, jerk_max_T1, _max_accel);
+	if (fabsf(jerk_max_T1) > FLT_EPSILON) { // zero direction or jerk would lead to division by zero
+		// compute increasing acceleration time
+		_T1 = computeT1(_state.a, delta_v, jerk_max_T1, _max_accel);
+
+		// compute decreasing acceleration time
+		_T3 = computeT3(_T1, _state.a, jerk_max_T1);
 
-	// compute decreasing acceleration time
-	_T3 = computeT3(_T1, _state.a, jerk_max_T1);
+		// compute constant acceleration time
+		_T2 = computeT2(_T1, _T3, _state.a, delta_v, jerk_max_T1);
 
-	// compute constant acceleration time
-	_T2 = computeT2(_T1, _T3, _state.a, delta_v, jerk_max_T1);
+	} else {
+		_T1 = _T2 = _T3 = 0.f;
+	}
 }
 
 Trajectory VelocitySmoothing::evaluatePoly(float j, float a0, float v0, float x0, float t, int d) const
@@ -292,12 +292,17 @@ 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);
+	if (fabsf(jerk_max_T1) > FLT_EPSILON) { // zero direction or jerk would lead to division by zero
+		// 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 decreasing acceleration time
-	_T3 = computeT3(_T1, _state.a, jerk_max_T1);
+		// compute constant acceleration time
+		_T2 = computeT2(T123, _T1, _T3);
 
-	// compute constant acceleration time
-	_T2 = computeT2(T123, _T1, _T3);
+	} else {
+		_T1 = _T2 = _T3 = 0.f;
+	}
 }

src/lib/motion_planning/VelocitySmoothing.hpp

@@ -107,8 +107,6 @@ class VelocitySmoothing
 	void setCurrentPosition(const float pos) { _state.x = _state_init.x = pos; }
 	float getCurrentPosition() const { return _state.x; }
 
-	float getVelSp() const { return _vel_sp; }
-
 	float getT1() const { return _T1; }
 	float getT2() const { return _T2; }
 	float getT3() const { return _T3; }
@@ -192,12 +190,12 @@ class VelocitySmoothing
 	inline Trajectory evaluatePoly(float j, float a0, float v0, float x0, float t, int d) const;
 
 	/* Input */
-	float _vel_sp{0.0f};
+	float _vel_sp{0.f};
 
 	/* Constraints */
-	float _max_jerk = 22.f;
-	float _max_accel = 8.f;
-	float _max_vel = 6.f;
+	float _max_jerk{0.f};
+	float _max_accel{0.f};
+	float _max_vel{0.f};
 
 	/* State (previous setpoints) */
 	Trajectory _state{};
@@ -207,9 +205,9 @@ class VelocitySmoothing
 	Trajectory _state_init{};
 
 	/* Duration of each phase */
-	float _T1 = 0.f; ///< Increasing acceleration [s]
-	float _T2 = 0.f; ///< Constant acceleration [s]
-	float _T3 = 0.f; ///< Decreasing acceleration [s]
+	float _T1{0.f}; ///< Increasing acceleration [s]
+	float _T2{0.f}; ///< Constant acceleration [s]
+	float _T3{0.f}; ///< Decreasing acceleration [s]
 
-	float _local_time = 0.f; ///< Current local time
+	float _local_time{0.f}; ///< Current local time
 };

src/lib/motion_planning/VelocitySmoothingTest.cpp

@@ -43,6 +43,65 @@
 
 using namespace matrix;
 
+TEST(VelocitySmoothingBasicTest, AllZeroCase)
+{
+	// GIVEN: An unconfigured VelocitySmoothing instance
+	VelocitySmoothing trajectory;
+
+	// WHEN: We update it
+	trajectory.updateDurations(0.f);
+	trajectory.updateTraj(0.f);
+
+	// THEN: All the trajectories should still be zero
+	EXPECT_FLOAT_EQ(trajectory.getTotalTime(), 0.f);
+	EXPECT_FLOAT_EQ(trajectory.getCurrentJerk(), 0.f);
+	EXPECT_FLOAT_EQ(trajectory.getCurrentAcceleration(), 0.f);
+	EXPECT_FLOAT_EQ(trajectory.getCurrentVelocity(), 0.f);
+	EXPECT_FLOAT_EQ(trajectory.getCurrentPosition(), 0.f);
+}
+
+TEST(VelocitySmoothingBasicTest, DivisionByZeroComputeT1)
+{
+	// GIVEN: A try to trigger division by zero in computeT1()
+	// zero jerk but enough input to go that code path
+	VelocitySmoothing trajectory;
+	trajectory.setMaxJerk(0.f);
+	trajectory.setMaxAccel(0.f);
+	trajectory.setMaxVel(1.f);
+
+	// WHEN: We update it
+	trajectory.updateDurations(1.f);
+	trajectory.updateTraj(1.f);
+
+	// THEN: All the trajectories should still be zero
+	EXPECT_FLOAT_EQ(trajectory.getTotalTime(), 0.f);
+	EXPECT_FLOAT_EQ(trajectory.getCurrentJerk(), 0.f);
+	EXPECT_FLOAT_EQ(trajectory.getCurrentAcceleration(), 0.f);
+	EXPECT_FLOAT_EQ(trajectory.getCurrentVelocity(), 0.f);
+	EXPECT_FLOAT_EQ(trajectory.getCurrentPosition(), 0.f);
+}
+
+TEST(VelocitySmoothingBasicTest, DivisionByZeroSaturateT1ForAccel)
+{
+	// GIVEN: We trigger division by zero in saturateT1ForAccel()
+	// zero jerk but enough input to go that code path
+	VelocitySmoothing trajectory(1.f, 0.f, 0.f);
+	trajectory.setMaxJerk(0.f);
+	trajectory.setMaxAccel(0.f);
+	trajectory.setMaxVel(1.f);
+
+	// WHEN: We update it
+	trajectory.updateDurations(1.f);
+	trajectory.updateTraj(1.f);
+
+	// THEN: All the trajectories should still be zero
+	EXPECT_FLOAT_EQ(trajectory.getTotalTime(), 0.f);
+	EXPECT_FLOAT_EQ(trajectory.getCurrentJerk(), 0.f);
+	EXPECT_FLOAT_EQ(trajectory.getCurrentAcceleration(), 0.f);
+	EXPECT_FLOAT_EQ(trajectory.getCurrentVelocity(), 0.f);
+	EXPECT_FLOAT_EQ(trajectory.getCurrentPosition(), 0.f);
+}
+
 class VelocitySmoothingTest : public ::testing::Test
 {
 public:
@@ -174,7 +233,7 @@ TEST_F(VelocitySmoothingTest, testConstantSetpoint)
 		updateTrajectories(dt, velocity_setpoints);
 	}
 
-	// THEN: All the trajectories should have reach their
+	// THEN: All the trajectories should have reached their
 	// final state: desired velocity target and zero acceleration
 	for (int i = 0; i < 3; i++) {
 		EXPECT_LE(fabsf(_trajectories[i].getCurrentVelocity() - velocity_setpoints(i)), 0.01f);