diff --git a/src/lib/flight_tasks/tasks/AutoLineSmoothVel/CMakeLists.txt b/src/lib/flight_tasks/tasks/AutoLineSmoothVel/CMakeLists.txt index 6224784dde..03bca0c201 100644 --- a/src/lib/flight_tasks/tasks/AutoLineSmoothVel/CMakeLists.txt +++ b/src/lib/flight_tasks/tasks/AutoLineSmoothVel/CMakeLists.txt @@ -37,3 +37,6 @@ px4_add_library(FlightTaskAutoLineSmoothVel target_link_libraries(FlightTaskAutoLineSmoothVel PUBLIC FlightTaskAutoMapper2 FlightTaskUtility) target_include_directories(FlightTaskAutoLineSmoothVel PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + + +px4_add_unit_gtest(SRC TrajectoryConstraintsTest.cpp) diff --git a/src/lib/flight_tasks/tasks/AutoLineSmoothVel/TrajectoryConstraints.hpp b/src/lib/flight_tasks/tasks/AutoLineSmoothVel/TrajectoryConstraints.hpp new file mode 100644 index 0000000000..b41e032531 --- /dev/null +++ b/src/lib/flight_tasks/tasks/AutoLineSmoothVel/TrajectoryConstraints.hpp @@ -0,0 +1,156 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include + +#include +#include + +namespace math +{ +namespace trajectory +{ +using matrix::Vector3f; +using matrix::Vector2f; + +struct VehicleDynamicLimits { + float z_accept_rad; + float xy_accept_rad; + + float max_acc_xy; + float max_jerk; + + float max_speed_xy; + + // TODO: remove this + float max_acc_xy_radius_scale; +}; + +/* + * Compute the maximum allowed speed at the waypoint assuming that we want to + * connect the two lines (current-target and target-next) + * with a tangent circle with constant speed and desired centripetal acceleration: a_centripetal = speed^2 / radius + * The circle should in theory start and end at the intersection of the lines and the waypoint's acceptance radius. + * This is not exactly true in reality since Navigator switches the waypoint so we have to take in account that + * the real acceptance radius is smaller. + * + */ +inline float computeStartXYSpeedFromWaypoints(const Vector3f &start_position, const Vector3f &target, + const Vector3f &next_target, float exit_speed, const VehicleDynamicLimits &config) +{ + const float distance_target_next = (target - next_target).xy().norm(); + + 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 + ) { + const float alpha = acosf(Vector2f((target - start_position).xy()).unit_or_zero().dot( + Vector2f((target - next_target).xy()).unit_or_zero())); + const float safe_alpha = constrain(alpha, 0.f, M_PI_F - FLT_EPSILON); + float accel_tmp = config.max_acc_xy_radius_scale * config.max_acc_xy; + float max_speed_in_turn = computeMaxSpeedInWaypoint(safe_alpha, accel_tmp, config.xy_accept_rad); + speed_at_target = min(min(max_speed_in_turn, exit_speed), config.max_speed_xy); + } + + float start_to_target = (start_position - target).xy().norm(); + float max_speed = computeMaxSpeedFromDistance(config.max_jerk, config.max_acc_xy, start_to_target, speed_at_target); + + return min(config.max_speed_xy, max_speed); +} + +/* + * This function computes the maximum speed XY that can be travelled, given a set of waypoints and vehicle dynamics + * + * The first waypoint should be the starting location, and the later waypoints the desired points to be followed. + * + * @param waypoints the list of waypoints to be followed, the first of which should be the starting location + * @param config the vehicle dynamic limits + * + * @return the maximum speed at waypoint[0] which allows it to follow the trajectory while respecting the dynamic limits + */ +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; + max_speed = computeStartXYSpeedFromWaypoints(waypoints[i], + waypoints[i + 1], + waypoints[min(i + 2, N - 1)], + max_speed, config); + } + + return max_speed; +} + +inline bool clampToXYNorm(Vector3f &target, float max_xy_norm) +{ + const float xynorm = target.xy().norm(); + const float scale_factor = max_xy_norm / xynorm; + + if (scale_factor < 1 && PX4_ISFINITE(scale_factor) && xynorm > FLT_EPSILON) { + target *= scale_factor; + return true; + } + + return false; +} + +inline bool clampToZNorm(Vector3f &target, float max_z_norm) +{ + float znorm = fabs(target(2)); + const float scale_factor = max_z_norm / znorm; + + if (scale_factor < 1 && PX4_ISFINITE(scale_factor) && znorm > FLT_EPSILON) { + target *= scale_factor; + return true; + } + + return false; +} + +} +} diff --git a/src/lib/flight_tasks/tasks/AutoLineSmoothVel/TrajectoryConstraintsTest.cpp b/src/lib/flight_tasks/tasks/AutoLineSmoothVel/TrajectoryConstraintsTest.cpp new file mode 100644 index 0000000000..ad0e7c0969 --- /dev/null +++ b/src/lib/flight_tasks/tasks/AutoLineSmoothVel/TrajectoryConstraintsTest.cpp @@ -0,0 +1,363 @@ +#include +#include + +using namespace matrix; +using namespace math::trajectory; + +class TrajectoryConstraintsTest : public ::testing::Test +{ +public: + VehicleDynamicLimits config; + + Vector3f vehicle_location; + Vector3f target; + Vector3f next_target; + + float final_speed = 0; + + void SetUp() override + { + config.z_accept_rad = 1.f; + config.xy_accept_rad = 0.99f; + + config.max_acc_xy = 3.f; + config.max_jerk = 10.f; + + config.max_speed_xy = 10.f; + + config.max_acc_xy_radius_scale = 0.8f; + + /* + * (20,20) + * Next target + * + * ^ + * | + * + * (10,10) (20,10) + * Vehicle -> Target + * + */ + vehicle_location = Vector3f(10, 10, 5); + target = Vector3f(20, 10, 5); + next_target = Vector3f(20, 20, 5); + } +}; + +TEST_F(TrajectoryConstraintsTest, testStraight) +{ + // GIVEN: 3 waypoints in straight line + next_target = target + 2.f * (target - vehicle_location); + target = vehicle_location + 0.5f * (next_target - vehicle_location); + + // WHEN: we get the speed for straight line travel + Vector3f waypoints[3] = {vehicle_location, target, next_target}; + float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config); + + // THEN: it should be the same as speed directly to the end point + Vector3f direct_points[2] = {vehicle_location, next_target}; + float direct_speed = computeXYSpeedFromWaypoints<2>(direct_points, config); + + EXPECT_FLOAT_EQ(through_speed, direct_speed); +} + +TEST_F(TrajectoryConstraintsTest, testStraightNaN) +{ + // GIVEN: 3 waypoints in straight line + next_target = target + 2.f * (target - vehicle_location); + target = vehicle_location + 0.5f * (next_target - vehicle_location); + next_target(0) = NAN; + next_target(1) = NAN; + + // WHEN: we get the speed for points which are NaN afterwards + Vector3f waypoints[3] = {vehicle_location, target, next_target}; + float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config); + + // THEN: it should be the same as speed to the closer point + Vector3f direct_points[2] = {vehicle_location, target}; + float direct_speed = computeXYSpeedFromWaypoints<2>(direct_points, config); + + EXPECT_FLOAT_EQ(through_speed, direct_speed); +} + +TEST_F(TrajectoryConstraintsTest, testStraightLowJerkClose) +{ + // GIVEN: 3 waypoints in straight line + next_target = target + 2.f * (target - vehicle_location); + target = vehicle_location + 0.05f * (next_target - vehicle_location); + config.max_jerk = 8.f; + + // WHEN: we get the speed for straight line travel + Vector3f waypoints[3] = {vehicle_location, target, next_target}; + float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config); + + // THEN: it should be the same as speed directly to the end point + Vector3f direct_points[2] = {vehicle_location, next_target}; + float direct_speed = computeXYSpeedFromWaypoints<2>(direct_points, config); + + EXPECT_FLOAT_EQ(through_speed, direct_speed); +} + +TEST_F(TrajectoryConstraintsTest, testStraightMidClose) +{ + // GIVEN: 3 waypoints in straight line + next_target = target + 2.f * (target - vehicle_location); + target = vehicle_location + 0.05f * (next_target - vehicle_location); + + // WHEN: we get the speed for straight line travel + Vector3f waypoints[3] = {vehicle_location, target, next_target}; + float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config); + + // THEN: it should be the same as speed directly to the end point + Vector3f direct_points[2] = {vehicle_location, next_target}; + float direct_speed = computeXYSpeedFromWaypoints<2>(direct_points, config); + + EXPECT_FLOAT_EQ(through_speed, direct_speed); +} + +TEST_F(TrajectoryConstraintsTest, testStraightMidFar) +{ + // GIVEN: 3 waypoints in straight line + next_target = target + 2.f * (target - vehicle_location); + target = vehicle_location + 0.95f * (next_target - vehicle_location); + + // WHEN: we get the speed for straight line travel + Vector3f waypoints[3] = {vehicle_location, target, next_target}; + float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config); + + // THEN: it should be the same as speed directly to the end point + Vector3f direct_points[2] = {vehicle_location, next_target}; + float direct_speed = computeXYSpeedFromWaypoints<2>(direct_points, config); + + EXPECT_FLOAT_EQ(through_speed, direct_speed); +} + + +TEST_F(TrajectoryConstraintsTest, test90Angle) +{ + // GIVEN: 3 waypoints in 90 degree angle + EXPECT_FLOAT_EQ(0.f, (vehicle_location - target).dot(target - next_target)); + + // WHEN: we get the speed for travel around the path + Vector3f waypoints[3] = {vehicle_location, target, next_target}; + float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config); + + // THEN: it should be slightly faster than stopping at the intermediate point + Vector3f stop_points[2] = {vehicle_location, target}; + float stop_speed = computeXYSpeedFromWaypoints<2>(stop_points, config); + + EXPECT_GT(through_speed, stop_speed); //faster + EXPECT_LT(through_speed, stop_speed * 1.03f); // but less than 3% faster +} + +TEST_F(TrajectoryConstraintsTest, test45Angle) +{ + // GIVEN: 3 waypoints in 45 degree angle + next_target = Vector3f(25, 15, 5); + + // WHEN: we get the speed for travel around the path + Vector3f waypoints[3] = {vehicle_location, target, next_target}; + float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config); + + // THEN: it should be slightly faster than stopping at the intermediate point + Vector3f stop_points[2] = {vehicle_location, target}; + float stop_speed = computeXYSpeedFromWaypoints<2>(stop_points, config); + + EXPECT_GT(through_speed, stop_speed * 1.03f); // more than 3% faster + EXPECT_LT(through_speed, stop_speed * 1.06f); // but less than 6% faster +} + +TEST_F(TrajectoryConstraintsTest, test10Angle) +{ + // GIVEN: 3 waypoints in 10 degree angle + next_target = Vector3f(30, 11.7, 5); + + // WHEN: we get the speed for travel around the path + Vector3f waypoints[3] = {vehicle_location, target, next_target}; + float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config); + + // THEN: it should be slightly faster than stopping at the intermediate point + Vector3f stop_points[2] = {vehicle_location, target}; + float stop_speed = computeXYSpeedFromWaypoints<2>(stop_points, config); + + EXPECT_GT(through_speed, stop_speed * 1.25f); // more than 25% faster + EXPECT_LT(through_speed, stop_speed * 1.3f); // but less than 30% faster +} + +TEST_F(TrajectoryConstraintsTest, test10AngleFarNext) +{ + // GIVEN: 3 waypoints in 10 degree angle, but next waypoint is far + next_target = 2.f * (Vector3f(30, 11.7, 5) - target) + target; + + // WHEN: we get the speed for travel around the path + Vector3f far_waypoints[3] = {vehicle_location, target, next_target}; + float far_speed = computeXYSpeedFromWaypoints<3>(far_waypoints, config); + + // THEN: it should be the same speed as a closer next waypoint at the same angle, since the bottleneck is the turn + next_target = Vector3f(30, 11.7, 5); + Vector3f close_waypoints[3] = {vehicle_location, target, next_target}; + float close_speed = computeXYSpeedFromWaypoints<3>(close_waypoints, config); + + EXPECT_FLOAT_EQ(far_speed, close_speed); +} + +TEST_F(TrajectoryConstraintsTest, test10AngleCloseNext) +{ + // GIVEN: 3 waypoints in right angle, but next waypoint is far + next_target = .2f * (Vector3f(30, 11.7, 5) - target) + target; + + // WHEN: we get the speed for travel around the path + Vector3f close_waypoints[3] = {vehicle_location, target, next_target}; + float close_speed = computeXYSpeedFromWaypoints<3>(close_waypoints, config); + + // THEN: it should be slower than a further next waypoint at the same angle, since the bottleneck is the distance + next_target = Vector3f(30, 11.7, 5); + Vector3f normal_waypoints[3] = {vehicle_location, target, next_target}; + float normal_speed = computeXYSpeedFromWaypoints<3>(normal_waypoints, config); + + EXPECT_LT(close_speed, normal_speed); +} + +TEST(TrajectoryConstraintsClamp, clampToXYNormNoEffectLarge) +{ + // GIVEN: a short vector + Vector3f vec(1, 2, 3); + + // WHEN: we clamp it on XY with a long cutoff + clampToXYNorm(vec, 1000.f); + + // THEN: it shouldn't change + EXPECT_EQ(vec, Vector3f(1, 2, 3)); +} + +TEST(TrajectoryConstraintsClamp, clampToZNormNoEffect) +{ + // GIVEN: a short vector + Vector3f vec(1, 2, 3); + + // WHEN: we clamp it on XY with a long cutoff + clampToZNorm(vec, 1000.f); + + // THEN: it shouldn't change + EXPECT_EQ(vec, Vector3f(1, 2, 3)); +} + +TEST(TrajectoryConstraintsClamp, clampToXYNormNoEffectExact) +{ + // GIVEN: a vector + Vector3f vec(3, 4, 1); + + // WHEN: we clamp it on XY with exact cutoff + clampToXYNorm(vec, 5.f); + + // THEN: it shouldn't change + EXPECT_EQ(vec, Vector3f(3, 4, 1)); +} + +TEST(TrajectoryConstraintsClamp, clampToZNormNoEffectExact) +{ + // GIVEN: a vector + Vector3f vec(3, 4, -1); + + // WHEN: we clamp it on Z with exact cutoff + clampToZNorm(vec, 1.f); + + // THEN: it shouldn't change + EXPECT_EQ(vec, Vector3f(3, 4, -1)); +} + +TEST(TrajectoryConstraintsClamp, clampToXYNormHalf) +{ + // GIVEN: a vector + Vector3f vec(3, 4, 1); + + // WHEN: we clamp it on XY with half hypot length + clampToXYNorm(vec, 2.5f); + + // THEN: it should be half length + EXPECT_TRUE(vec == Vector3f(1.5f, 2.f, 0.5f)); +} + +TEST(TrajectoryConstraintsClamp, clampToZNormHalf) +{ + // GIVEN: a vector + Vector3f vec(3, 4, 10); + + // WHEN: we clamp it on Z with half length + clampToZNorm(vec, 5.f); + + // THEN: it should be half length + EXPECT_TRUE(vec == Vector3f(1.5f, 2.f, 5.f)); +} + +TEST(TrajectoryConstraintsClamp, clampToXYNormZero) +{ + // GIVEN: a vector + Vector3f vec(3, 4, 1); + + // WHEN: we clamp it on XY with half hypot length + clampToXYNorm(vec, 0.f); + + // THEN: it should be 0 + EXPECT_TRUE(vec == Vector3f(0.f, 0.f, 0.f)); +} + +TEST(TrajectoryConstraintsClamp, clampToZNormZero) +{ + // GIVEN: a vector + Vector3f vec(3, 4, 1); + + // WHEN: we clamp it on Z with half hypot length + clampToZNorm(vec, 0.f); + + // THEN: it should be 0 + EXPECT_TRUE(vec == Vector3f(0.f, 0.f, 0.f)); +} + +TEST(TrajectoryConstraintsClamp, clampToXYNormVecZero) +{ + // GIVEN: a vector + Vector3f vec(0, 0, 0); + + // WHEN: we clamp it on XY + clampToXYNorm(vec, 1.f); + + // THEN: it should be 0 still + EXPECT_TRUE(vec == Vector3f(0.f, 0.f, 0.f)); +} + +TEST(TrajectoryConstraintsClamp, clampToZNormVecZero) +{ + // GIVEN: a vector + Vector3f vec(0, 0, 0); + + // WHEN: we clamp it on Z + clampToZNorm(vec, 1.f); + + // THEN: it should be 0 still + EXPECT_TRUE(vec == Vector3f(0.f, 0.f, 0.f)); +} + +TEST(TrajectoryConstraintsClamp, clampToXYNormVecZeroToZero) +{ + // GIVEN: a vector + Vector3f vec(0, 0, 0); + + // WHEN: we clamp it on XY + clampToXYNorm(vec, 0.f); + + // THEN: it should be 0 still + EXPECT_TRUE(vec == Vector3f(0.f, 0.f, 0.f)); +} + +TEST(TrajectoryConstraintsClamp, clampToZNormVecZeroToZero) +{ + // GIVEN: a vector + Vector3f vec(0, 0, 0); + + // WHEN: we clamp it on XY + clampToZNorm(vec, 0.f); + + // THEN: it should be 0 still + EXPECT_TRUE(vec == Vector3f(0.f, 0.f, 0.f)); +}