From 6f295d91d1256f64644b41e8d81beaa7bfca712f Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 20 Nov 2023 13:20:54 +0100 Subject: [PATCH] Revert "motion_planning: sanitize inputs to position and velocity smoothing libs" This reverts commit 2951c846ee07e52c2e3d97ea4629185016f3a011. --- src/lib/motion_planning/PositionSmoothing.hpp | 10 +--- src/lib/motion_planning/VelocitySmoothing.cpp | 17 ------- src/lib/motion_planning/VelocitySmoothing.hpp | 47 +++---------------- 3 files changed, 8 insertions(+), 66 deletions(-) diff --git a/src/lib/motion_planning/PositionSmoothing.hpp b/src/lib/motion_planning/PositionSmoothing.hpp index a73ba17be5..61070d9dab 100644 --- a/src/lib/motion_planning/PositionSmoothing.hpp +++ b/src/lib/motion_planning/PositionSmoothing.hpp @@ -328,7 +328,7 @@ public: */ inline void setMaxAllowedHorizontalError(float error) { - _max_allowed_horizontal_error = (PX4_ISFINITE(error)) ? math::max(error, kMinHorizontalError) : kDefaultHorizontalError; + _max_allowed_horizontal_error = error; } /** @@ -410,14 +410,8 @@ public: private: - // [m] minimum max horizontal vehicle position error from trajectory allowing time integration along trajectory - static constexpr float kMinHorizontalError{0.1f}; - - // [m] default max horizontal vehicle position error from trajectory allowing time integration along trajectory - static constexpr float kDefaultHorizontalError{2.f}; - /* params, only modified from external */ - float _max_allowed_horizontal_error{kDefaultHorizontalError}; + float _max_allowed_horizontal_error{0.f}; float _vertical_acceptance_radius{0.f}; float _cruise_speed{0.f}; float _horizontal_trajectory_gain{0.f}; diff --git a/src/lib/motion_planning/VelocitySmoothing.cpp b/src/lib/motion_planning/VelocitySmoothing.cpp index d7fc1c2c8b..6471bfaf4c 100644 --- a/src/lib/motion_planning/VelocitySmoothing.cpp +++ b/src/lib/motion_planning/VelocitySmoothing.cpp @@ -57,10 +57,6 @@ void VelocitySmoothing::reset(float accel, float vel, float pos) float VelocitySmoothing::saturateT1ForAccel(float a0, float j_max, float T1, float a_max) const { - if (math::abs_t(j_max) < FLT_EPSILON) { - return 0.f; - } - /* Check maximum acceleration, saturate and recompute T1 if needed */ float accel_T1 = a0 + j_max * T1; float T1_new = T1; @@ -77,10 +73,6 @@ float VelocitySmoothing::saturateT1ForAccel(float a0, float j_max, float T1, flo float VelocitySmoothing::computeT1(float a0, float v3, float j_max, float a_max) const { - if (math::abs_t(j_max) < FLT_EPSILON) { - return 0.f; - } - float delta = 2.f * a0 * a0 + 4.f * j_max * v3; if (delta < 0.f) { @@ -89,7 +81,6 @@ 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; @@ -112,10 +103,6 @@ float VelocitySmoothing::computeT1(float a0, float v3, float j_max, float a_max) float VelocitySmoothing::computeT1(float T123, float a0, float v3, float j_max, float a_max) const { - if (math::abs_t(j_max) < FLT_EPSILON) { - return 0.f; - } - 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; @@ -169,10 +156,6 @@ float VelocitySmoothing::computeT2(float T123, float T1, float T3) const float VelocitySmoothing::computeT3(float T1, float a0, float j_max) const { - if (math::abs_t(j_max) < FLT_EPSILON) { - return 0.f; - } - float T3 = a0 / j_max + T1; return math::max(T3, 0.f); } diff --git a/src/lib/motion_planning/VelocitySmoothing.hpp b/src/lib/motion_planning/VelocitySmoothing.hpp index 74e1690ed7..25799d1ce9 100644 --- a/src/lib/motion_planning/VelocitySmoothing.hpp +++ b/src/lib/motion_planning/VelocitySmoothing.hpp @@ -33,9 +33,6 @@ #pragma once -#include -#include - struct Trajectory { float j; //< jerk float a; //< acceleration @@ -94,46 +91,20 @@ public: * Getters and setters */ float getMaxJerk() const { return _max_jerk; } - void setMaxJerk(float max_jerk) { _max_jerk = PX4_ISFINITE(max_jerk) ? math::max(max_jerk, kMinJerk) : kMinJerk; } + void setMaxJerk(float max_jerk) { _max_jerk = max_jerk; } float getMaxAccel() const { return _max_accel; } - void setMaxAccel(float max_accel) { _max_accel = PX4_ISFINITE(max_accel) ? math::max(max_accel, kMinAccel) : kMinAccel; } + void setMaxAccel(float max_accel) { _max_accel = max_accel; } float getMaxVel() const { return _max_vel; } - void setMaxVel(float max_vel) { _max_vel = PX4_ISFINITE(max_vel) ? math::max(max_vel, 0.f) : 0.f; } + void setMaxVel(float max_vel) { _max_vel = max_vel; } float getCurrentJerk() const { return _state.j; } - void setCurrentAcceleration(const float accel) - { - if (PX4_ISFINITE(accel)) { - _state.a = accel; - _state_init.a = accel; - - } else { - _state.a = 0.; - _state_init.a = 0.; - } - } + void setCurrentAcceleration(const float accel) { _state.a = _state_init.a = accel; } float getCurrentAcceleration() const { return _state.a; } - void setCurrentVelocity(const float vel) - { - if (PX4_ISFINITE(vel)) { - _state.v = vel; - _state_init.v = vel; - - } else { - _state.v = 0.f; - _state_init.v = 0.f; - } - } + void setCurrentVelocity(const float vel) { _state.v = _state_init.v = vel; } float getCurrentVelocity() const { return _state.v; } - void setCurrentPosition(const float pos) - { - if (PX4_ISFINITE(pos)) { - _state.x = pos; - _state_init.x = pos; - } - } + void setCurrentPosition(const float pos) { _state.x = _state_init.x = pos; } float getCurrentPosition() const { return _state.x; } float getVelSp() const { return _vel_sp; } @@ -152,12 +123,6 @@ public: */ static void timeSynchronization(VelocitySmoothing *traj, int n_traj); - // [m/s^2] minimum value of the smoother's maximum acceleration - static constexpr float kMinAccel{0.1f}; - - // [m/s^3] minimum value of the smoother's maximum jerk - static constexpr float kMinJerk{0.1f}; - private: /**