Add trajectory constraints helpers

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)

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 <px4_defines.h>
+
+#include <matrix/math.hpp>
+#include <mathlib/mathlib.h>
+
+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 <size_t N>
+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;
+}
+
+}
+}

src/lib/flight_tasks/tasks/AutoLineSmoothVel/TrajectoryConstraintsTest.cpp

@@ -0,0 +1,363 @@
+#include <gtest/gtest.h>
+#include <flight_tasks/tasks/AutoLineSmoothVel/TrajectoryConstraints.hpp>
+
+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));
+}