From 31d026c68e401471502bc760286eebcce056d930 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Sun, 4 May 2025 16:08:18 -0800 Subject: [PATCH] test for computeMaxSpeedFromDistance and experimental chatgpt fix --- src/lib/mathlib/CMakeLists.txt | 1 + .../mathlib/math/MaxSpeedFromDistanceTest.cpp | 28 ++++++++++++ src/lib/mathlib/math/TrajMath.hpp | 45 +++++++++++++++---- 3 files changed, 66 insertions(+), 8 deletions(-) create mode 100644 src/lib/mathlib/math/MaxSpeedFromDistanceTest.cpp diff --git a/src/lib/mathlib/CMakeLists.txt b/src/lib/mathlib/CMakeLists.txt index 7f3a3d534e..87e54c1a30 100644 --- a/src/lib/mathlib/CMakeLists.txt +++ b/src/lib/mathlib/CMakeLists.txt @@ -50,3 +50,4 @@ px4_add_unit_gtest(SRC math/test/UtilitiesTest.cpp) px4_add_unit_gtest(SRC math/WelfordMeanTest.cpp) px4_add_unit_gtest(SRC math/WelfordMeanVectorTest.cpp) px4_add_unit_gtest(SRC math/MaxDistanceToCircleTest.cpp) +px4_add_unit_gtest(SRC math/MaxSpeedFromDistanceTest.cpp) diff --git a/src/lib/mathlib/math/MaxSpeedFromDistanceTest.cpp b/src/lib/mathlib/math/MaxSpeedFromDistanceTest.cpp new file mode 100644 index 0000000000..d73da36df8 --- /dev/null +++ b/src/lib/mathlib/math/MaxSpeedFromDistanceTest.cpp @@ -0,0 +1,28 @@ +#include +#include +#include "Functions.hpp" + +#include "TrajMath.hpp" + +using namespace math; +using matrix::Vector2f; + +TEST(MaxSpeedFromDistance, MaxSpeedFromDistance) +{ + // When max acceleration is higher we should be able to fly faster + const float acc_hor_small = 1.f; + const float acc_hor_large = 10.f; + + const float jerk_max = 8.f; + const float stop_distance = 10.f; + const float final_speed = 0.f; + + const float max_vel_smaller = trajectory::computeMaxSpeedFromDistance(jerk_max, acc_hor_small, stop_distance, final_speed); + const float max_vel_larger = trajectory::computeMaxSpeedFromDistance(jerk_max, acc_hor_large, stop_distance, final_speed); + + printf("max_vel_larger: %f\n", (double)max_vel_larger); + printf("max_vel_smaller: %f\n", (double)max_vel_smaller); + + + EXPECT_TRUE(max_vel_larger < max_vel_smaller); +} diff --git a/src/lib/mathlib/math/TrajMath.hpp b/src/lib/mathlib/math/TrajMath.hpp index f26a3406eb..033d5e700a 100644 --- a/src/lib/mathlib/math/TrajMath.hpp +++ b/src/lib/mathlib/math/TrajMath.hpp @@ -58,17 +58,46 @@ namespace trajectory * * @return maximum speed */ -inline float computeMaxSpeedFromDistance(const float jerk, const float accel, const float braking_distance, - const float final_speed) +inline float computeMaxSpeedFromDistance(float jerk, + float accel, + float x, + float v_f) { - auto sqr = [](float f) {return f * f;}; - float b = 4.0f * sqr(accel) / jerk; - float c = - 2.0f * accel * braking_distance - sqr(final_speed); - float max_speed = 0.5f * (-b + sqrtf(sqr(b) - 4.0f * c)); + auto sqr = [](float w){ return w*w; }; - // don't slow down more than the end speed, even if the conservative accel ramp time requests it - return fmaxf(max_speed, final_speed); + // 1) linear term b = 2 * a^2 / j + float b = 2.0f * accel*accel / jerk; + + // 2) constant term c = -2*a*x - v_f^2 - (a^4)/(3*j^2) + float jerk2 = jerk*jerk; + float a4 = accel*accel*accel*accel; + float c = -2.0f * accel * x + - sqr(v_f) + - (a4 / (3.0f * jerk2)); + + // 3) solve quadratic + float disc = b*b - 4.0f * c; + if (disc <= 0.0f) { + // no real solution → can’t brake from anything > v_f + return v_f; + } + + float v_i = 0.5f * ( -b + sqrtf(disc) ); + + // 4) never suggest below your target end‐speed + return fmaxf(v_i, v_f); } +// inline float computeMaxSpeedFromDistance(const float jerk, const float accel, const float braking_distance, +// const float final_speed) +// { +// auto sqr = [](float f) {return f * f;}; +// float b = 4.0f * sqr(accel) / jerk; +// float c = - 2.0f * accel * braking_distance - sqr(final_speed); +// float max_speed = 0.5f * (-b + sqrtf(sqr(b) - 4.0f * c)); + +// // don't slow down more than the end speed, even if the conservative accel ramp time requests it +// return fmaxf(max_speed, final_speed); +// } /* Compute the maximum tangential speed in a circle defined by two line segments of length "d" * forming a V shape, opened by an angle "alpha". The circle is tangent to the end of the