fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-21 23:40:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

VelocitySmoothing: fix division by zero cases

Browse Source 
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.
This commit is contained in:
Matthias Grob
2023-11-21 10:17:41 +01:00
parent 6f295d91d1
commit 11cca72ef1
3 changed files with 92 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/motion_planning/VelocitySmoothing.cpp
+24 -19
View File
@@ -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
+8 -10
View File
@@ -107,8 +107,6 @@ public:
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 @@ private:
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 @@ private:
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
+60 -1
View File
@@ -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);