From 42c2249af42f17ffb52332c807e7912b7c2104e4 Mon Sep 17 00:00:00 2001 From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com> Date: Tue, 19 Nov 2024 10:45:23 +0100 Subject: [PATCH 1/5] Included turning radius calculations for vertical changes and removed the requirement for the same altitude in the 2D turning radius logic. --- src/lib/motion_planning/PositionSmoothing.cpp | 25 +++++++++++++++++-- .../motion_planning/TrajectoryConstraints.hpp | 6 +---- 2 files changed, 24 insertions(+), 7 deletions(-) diff --git a/src/lib/motion_planning/PositionSmoothing.cpp b/src/lib/motion_planning/PositionSmoothing.cpp index 28c70c690d60..ea93708bedc9 100644 --- a/src/lib/motion_planning/PositionSmoothing.cpp +++ b/src/lib/motion_planning/PositionSmoothing.cpp @@ -106,15 +106,36 @@ float PositionSmoothing::_getMaxXYSpeed(const Vector3f(&waypoints)[3]) const float PositionSmoothing::_getMaxZSpeed(const Vector3f(&waypoints)[3]) const { - + const auto &start_position = waypoints[0]; const auto &target = waypoints[1]; + const auto &next_target = waypoints[2]; Vector3f pos_traj(_trajectory[0].getCurrentPosition(), _trajectory[1].getCurrentPosition(), _trajectory[2].getCurrentPosition()); const float distance_start_target = fabs(target(2) - pos_traj(2)); - const float arrival_z_speed = 0.f; + float arrival_z_speed = 0.0f; + + const float distance_target_next = fabsf(target(2) - next_target(2)); + + const bool target_next_different = distance_target_next > 0.001f; + + Vector2f target_xy_z = {target.xy().norm(), target(2)}; + Vector2f next_target_xy_z = {next_target.xy().norm(), next_target(2)}; + Vector2f start_position_xy_z = {start_position.xy().norm(), start_position(2)}; + + if (target_next_different + ) { + const float alpha = acosf(Vector2f((target_xy_z - start_position_xy_z)).unit_or_zero().dot( + Vector2f((target_xy_z - next_target_xy_z)).unit_or_zero())); + + const float safe_alpha = math::constrain(alpha, 0.f, M_PI_F - FLT_EPSILON); + float accel_tmp = _trajectory[2].getMaxAccel(); + float max_speed_in_turn = math::trajectory::computeMaxSpeedInWaypoint(safe_alpha, accel_tmp, + _vertical_acceptance_radius); + arrival_z_speed = math::min(max_speed_in_turn, _trajectory[2].getMaxVel()); + } float max_speed = math::min(_trajectory[2].getMaxVel(), math::trajectory::computeMaxSpeedFromDistance( _trajectory[2].getMaxJerk(), _trajectory[2].getMaxAccel(), diff --git a/src/lib/motion_planning/TrajectoryConstraints.hpp b/src/lib/motion_planning/TrajectoryConstraints.hpp index 6d52e1861e6e..ffe68840cb12 100644 --- a/src/lib/motion_planning/TrajectoryConstraints.hpp +++ b/src/lib/motion_planning/TrajectoryConstraints.hpp @@ -74,15 +74,11 @@ inline float computeStartXYSpeedFromWaypoints(const Vector3f &start_position, co const bool target_next_different = distance_target_next > 0.001f; const bool waypoint_overlap = distance_target_next < config.xy_accept_rad; - const bool has_reached_altitude = fabsf(target(2) - start_position(2)) < config.z_accept_rad; - const bool altitude_stays_same = fabsf(next_target(2) - target(2)) < config.z_accept_rad; float speed_at_target = 0.0f; if (target_next_different && - !waypoint_overlap && - has_reached_altitude && - altitude_stays_same + !waypoint_overlap ) { const float alpha = acosf(Vector2f((target - start_position).xy()).unit_or_zero().dot( Vector2f((target - next_target).xy()).unit_or_zero())); From 9e5e2ccbcd1503401689d2ce09cf8faf27cd175a Mon Sep 17 00:00:00 2001 From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com> Date: Tue, 19 Nov 2024 13:58:43 +0100 Subject: [PATCH 2/5] MPC: PositonSmoothing, change test to reflect that we have to come inwithing the acceptance radius, and not exact position. --- src/lib/motion_planning/PositionSmoothingTest.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/src/lib/motion_planning/PositionSmoothingTest.cpp b/src/lib/motion_planning/PositionSmoothingTest.cpp index ee06f8c2b1be..5cf45fd5fec8 100644 --- a/src/lib/motion_planning/PositionSmoothingTest.cpp +++ b/src/lib/motion_planning/PositionSmoothingTest.cpp @@ -151,14 +151,17 @@ TEST_F(PositionSmoothingTest, reachesTargetVelocityIntegration) TEST_F(PositionSmoothingTest, reachesTargetInitialVelocity) { - const int N_ITER = 2000; + const int N_ITER = 20000; const float DELTA_T = 0.02f; const Vector3f INITIAL_POSITION{0.f, 0.f, 0.f}; const Vector3f TARGET{12.f, 17.f, 8.f}; const Vector3f NEXT_TARGET{8.f, 12.f, 80.f}; + const float XY_ACC_RAD = 10.f; + const float Z_ACC_RAD = 0.8f; + - Vector3f waypoints[3] = {INITIAL_POSITION, TARGET, NEXT_TARGET}; + Vector3f waypoints[3] = {INITIAL_POSITION, TARGET, TARGET}; Vector3f ff_velocity{1.f, 0.1f, 0.3f}; Vector3f position{0.f, 0.f, 0.f}; @@ -180,12 +183,13 @@ TEST_F(PositionSmoothingTest, reachesTargetInitialVelocity) ff_velocity = {0.f, 0.f, 0.f}; expectDynamicsLimitsRespected(out); - if (position == TARGET) { + if (Vector2f(position.xy() - TARGET.xy()).norm() < XY_ACC_RAD && fabsf(position(2) - TARGET(2)) < Z_ACC_RAD) { printf("Converged in %d iterations\n", iteration); break; } } - EXPECT_EQ(TARGET, position); + EXPECT_LT(Vector2f(position.xy() - TARGET.xy()).norm(), XY_ACC_RAD); + EXPECT_LT(fabsf(position(2) - TARGET(2)), Z_ACC_RAD); EXPECT_LT(iteration, N_ITER) << "Took too long to converge\n"; } From 6b595899ec9473e93d95d2a22afffc30d1569215 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 28 Nov 2024 16:19:09 +0100 Subject: [PATCH 3/5] TrajectoryConstraints: clarify waypoint indexing --- src/lib/motion_planning/TrajectoryConstraints.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/lib/motion_planning/TrajectoryConstraints.hpp b/src/lib/motion_planning/TrajectoryConstraints.hpp index ffe68840cb12..cc52a984de30 100644 --- a/src/lib/motion_planning/TrajectoryConstraints.hpp +++ b/src/lib/motion_planning/TrajectoryConstraints.hpp @@ -104,15 +104,15 @@ inline float computeStartXYSpeedFromWaypoints(const Vector3f &start_position, co * * @return the maximum speed at waypoint[0] which allows it to follow the trajectory while respecting the dynamic limits */ -template +template float computeXYSpeedFromWaypoints(const Vector3f waypoints[N], const VehicleDynamicLimits &config) { static_assert(N >= 2, "Need at least 2 points to compute speed"); float max_speed = 0.f; - for (size_t j = 0; j < N - 1; j++) { - size_t i = N - 2 - j; + // go backwards through the waypoints + for (int i = (N - 2); i >= 0; i--) { max_speed = computeStartXYSpeedFromWaypoints(waypoints[i], waypoints[i + 1], waypoints[min(i + 2, N - 1)], From 047e14ee3cd1a705024db86a6259f50e5f303e29 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 28 Nov 2024 17:34:35 +0100 Subject: [PATCH 4/5] PositionSmoothing: refactor _getMaxZSpeed() --- src/lib/motion_planning/PositionSmoothing.cpp | 24 ++++++++----------- 1 file changed, 10 insertions(+), 14 deletions(-) diff --git a/src/lib/motion_planning/PositionSmoothing.cpp b/src/lib/motion_planning/PositionSmoothing.cpp index ea93708bedc9..0d9f3acb8d66 100644 --- a/src/lib/motion_planning/PositionSmoothing.cpp +++ b/src/lib/motion_planning/PositionSmoothing.cpp @@ -106,27 +106,22 @@ float PositionSmoothing::_getMaxXYSpeed(const Vector3f(&waypoints)[3]) const float PositionSmoothing::_getMaxZSpeed(const Vector3f(&waypoints)[3]) const { - const auto &start_position = waypoints[0]; - const auto &target = waypoints[1]; - const auto &next_target = waypoints[2]; + const Vector3f &start_position = waypoints[0]; + const Vector3f &target = waypoints[1]; + const Vector3f &next_target = waypoints[2]; + + const Vector2f start_position_xy_z = {start_position.xy().norm(), start_position(2)}; + const Vector2f target_xy_z = {target.xy().norm(), target(2)}; + const Vector2f next_target_xy_z = {next_target.xy().norm(), next_target(2)}; Vector3f pos_traj(_trajectory[0].getCurrentPosition(), _trajectory[1].getCurrentPosition(), _trajectory[2].getCurrentPosition()); - const float distance_start_target = fabs(target(2) - pos_traj(2)); float arrival_z_speed = 0.0f; + const bool target_next_different = fabsf(target(2) - next_target(2)) > 0.001f; - const float distance_target_next = fabsf(target(2) - next_target(2)); - - const bool target_next_different = distance_target_next > 0.001f; - - Vector2f target_xy_z = {target.xy().norm(), target(2)}; - Vector2f next_target_xy_z = {next_target.xy().norm(), next_target(2)}; - Vector2f start_position_xy_z = {start_position.xy().norm(), start_position(2)}; - - if (target_next_different - ) { + if (target_next_different) { const float alpha = acosf(Vector2f((target_xy_z - start_position_xy_z)).unit_or_zero().dot( Vector2f((target_xy_z - next_target_xy_z)).unit_or_zero())); @@ -137,6 +132,7 @@ float PositionSmoothing::_getMaxZSpeed(const Vector3f(&waypoints)[3]) const arrival_z_speed = math::min(max_speed_in_turn, _trajectory[2].getMaxVel()); } + const float distance_start_target = fabs(target(2) - pos_traj(2)); float max_speed = math::min(_trajectory[2].getMaxVel(), math::trajectory::computeMaxSpeedFromDistance( _trajectory[2].getMaxJerk(), _trajectory[2].getMaxAccel(), distance_start_target, arrival_z_speed)); From 8db0cf020f38e290c1c4ff2418671c925681eae5 Mon Sep 17 00:00:00 2001 From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com> Date: Fri, 29 Nov 2024 10:51:13 +0100 Subject: [PATCH 5/5] Added possibility to modify the start position from external sources, as its done in the _getMaxXYSpeed --- src/lib/motion_planning/PositionSmoothing.cpp | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/src/lib/motion_planning/PositionSmoothing.cpp b/src/lib/motion_planning/PositionSmoothing.cpp index 0d9f3acb8d66..85c274ab5d5c 100644 --- a/src/lib/motion_planning/PositionSmoothing.cpp +++ b/src/lib/motion_planning/PositionSmoothing.cpp @@ -106,7 +106,10 @@ float PositionSmoothing::_getMaxXYSpeed(const Vector3f(&waypoints)[3]) const float PositionSmoothing::_getMaxZSpeed(const Vector3f(&waypoints)[3]) const { - const Vector3f &start_position = waypoints[0]; + const Vector3f &start_position = {_trajectory[0].getCurrentPosition(), + _trajectory[1].getCurrentPosition(), + _trajectory[2].getCurrentPosition() + }; const Vector3f &target = waypoints[1]; const Vector3f &next_target = waypoints[2]; @@ -114,10 +117,6 @@ float PositionSmoothing::_getMaxZSpeed(const Vector3f(&waypoints)[3]) const const Vector2f target_xy_z = {target.xy().norm(), target(2)}; const Vector2f next_target_xy_z = {next_target.xy().norm(), next_target(2)}; - Vector3f pos_traj(_trajectory[0].getCurrentPosition(), - _trajectory[1].getCurrentPosition(), - _trajectory[2].getCurrentPosition()); - float arrival_z_speed = 0.0f; const bool target_next_different = fabsf(target(2) - next_target(2)) > 0.001f; @@ -132,7 +131,7 @@ float PositionSmoothing::_getMaxZSpeed(const Vector3f(&waypoints)[3]) const arrival_z_speed = math::min(max_speed_in_turn, _trajectory[2].getMaxVel()); } - const float distance_start_target = fabs(target(2) - pos_traj(2)); + const float distance_start_target = fabs(target(2) - start_position(2)); float max_speed = math::min(_trajectory[2].getMaxVel(), math::trajectory::computeMaxSpeedFromDistance( _trajectory[2].getMaxJerk(), _trajectory[2].getMaxAccel(), distance_start_target, arrival_z_speed));