dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Revert "motion_planning: sanitize inputs to position and velocity smoothing libs" 

This reverts commit 2951c846ee07e52c2e3d97ea4629185016f3a011.
This commit is contained in:
Matthias Grob 2023-11-20 13:20:54 +01:00
parent 14785eeb47
commit 6f295d91d1
3 changed files with 8 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/lib/motion_planning/PositionSmoothing.hpp
View File

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

17
src/lib/motion_planning/VelocitySmoothing.cpp
View File

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

47
src/lib/motion_planning/VelocitySmoothing.hpp
View File

@ -33,9 +33,6 @@
#pragma once
#include <px4_defines.h>
#include <mathlib/math/Limits.hpp>
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:
/**