diff --git a/src/lib/motion_planning/VelocitySmoothing.cpp b/src/lib/motion_planning/VelocitySmoothing.cpp index 6471bfaf4c28..d4a741f051fc 100644 --- a/src/lib/motion_planning/VelocitySmoothing.cpp +++ b/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; + } } diff --git a/src/lib/motion_planning/VelocitySmoothing.hpp b/src/lib/motion_planning/VelocitySmoothing.hpp index 25799d1ce945..2ea584d156c0 100644 --- a/src/lib/motion_planning/VelocitySmoothing.hpp +++ b/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 }; diff --git a/src/lib/motion_planning/VelocitySmoothingTest.cpp b/src/lib/motion_planning/VelocitySmoothingTest.cpp index 0106235769f4..5f7e00d720d1 100644 --- a/src/lib/motion_planning/VelocitySmoothingTest.cpp +++ b/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);