flight_mode_manager: Extracted position trajectory motion planning into a library

Extract the functionality to plan smooth position motion trajectories into a
motion planning library, such that it can be used in other parts of the code as well.

src/lib/motion_planning/CMakeLists.txt

@@ -33,8 +33,10 @@
 
 px4_add_library(motion_planning
 	VelocitySmoothing.cpp
+	PositionSmoothing.cpp
 )
 
 target_include_directories(motion_planning PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 
 px4_add_unit_gtest(SRC VelocitySmoothingTest.cpp LINKLIBS motion_planning)
+px4_add_unit_gtest(SRC PositionSmoothingTest.cpp LINKLIBS motion_planning)

src/lib/motion_planning/PositionSmoothing.cpp

@@ -0,0 +1,323 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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.
+ *
+ ****************************************************************************/
+
+#include "PositionSmoothing.hpp"
+#include "TrajectoryConstraints.hpp"
+#include <mathlib/mathlib.h>
+#include <matrix/matrix/math.hpp>
+#include <matrix/matrix/helper_functions.hpp>
+
+
+void PositionSmoothing::generateSetpoints(
+	const Vector3f &position,
+	const Vector3f(&waypoints)[3],
+	const Vector3f &feedforward_velocity,
+	float delta_time,
+	bool force_zero_velocity_setpoint,
+	PositionSmoothingSetpoints &out_setpoints)
+{
+	Vector3f velocity_setpoint{0.f, 0.f, 0.f};
+
+	if (!force_zero_velocity_setpoint) {
+		velocity_setpoint = _generateVelocitySetpoint(position, waypoints, feedforward_velocity);
+	}
+
+	out_setpoints.unsmoothed_velocity = velocity_setpoint;
+
+	_generateTrajectory(
+		position,
+		velocity_setpoint,
+		delta_time,
+		out_setpoints
+	);
+}
+
+
+bool PositionSmoothing::_isTurning(const Vector3f &target) const
+{
+	const Vector2f vel_traj(_trajectory[0].getCurrentVelocity(),
+				_trajectory[1].getCurrentVelocity());
+	const Vector2f pos_traj(_trajectory[0].getCurrentPosition(),
+				_trajectory[1].getCurrentPosition());
+	const Vector2f target_xy(target);
+	const Vector2f u_vel_traj = vel_traj.unit_or_zero();
+	const Vector2f pos_to_target = Vector2f(target_xy - pos_traj);
+	const float cos_align = u_vel_traj * pos_to_target.unit_or_zero();
+
+	// The vehicle is turning if the angle between the velocity vector
+	// and the direction to the target is greater than 10 degrees, the
+	// velocity is large enough and the drone isn't in the acceptance
+	// radius of the last WP.
+	return (vel_traj.longerThan(0.2f)
+		&& cos_align < 0.98f
+		&& pos_to_target.longerThan(_target_acceptance_radius));
+}
+
+
+
+/* Constrain some value vith a constrain depending on the sign of the constraint
+ * Example: 	- if the constrain is -5, the value will be constrained between -5 and 0
+ * 		- if the constrain is 5, the value will be constrained between 0 and 5
+ */
+inline float _constrainOneSide(float val, float constraint)
+{
+	const float min = (constraint < FLT_EPSILON) ? constraint : 0.f;
+	const float max = (constraint > FLT_EPSILON) ? constraint : 0.f;
+
+	return math::constrain(val, min, max);
+}
+
+inline float _constrainAbs(float val, float max)
+{
+	return matrix::sign(val) * math::min(fabsf(val), fabsf(max));
+}
+
+
+float PositionSmoothing::_getMaxXYSpeed(const Vector3f(&waypoints)[3]) const
+{
+	Vector3f pos_traj(_trajectory[0].getCurrentPosition(),
+			  _trajectory[1].getCurrentPosition(),
+			  _trajectory[2].getCurrentPosition());
+
+	math::trajectory::VehicleDynamicLimits config;
+	config.z_accept_rad = _vertical_acceptance_radius;
+	config.xy_accept_rad = _target_acceptance_radius;
+	config.max_acc_xy = _trajectory[0].getMaxAccel();
+	config.max_jerk = _trajectory[0].getMaxJerk();
+	config.max_speed_xy = _cruise_speed;
+	config.max_acc_xy_radius_scale = _horizontal_trajectory_gain;
+
+	// constrain velocity to go to the position setpoint first if the position setpoint has been modified by an external source
+	// (eg. Obstacle Avoidance)
+
+	Vector3f pos_to_waypoints[3] = {pos_traj, waypoints[1], waypoints[2]};
+
+	return math::trajectory::computeXYSpeedFromWaypoints<3>(pos_to_waypoints, config);
+}
+
+float PositionSmoothing::_getMaxZSpeed(const Vector3f(&waypoints)[3]) const
+{
+
+	const auto &target = waypoints[1];
+
+	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 max_speed = math::min(_trajectory[2].getMaxVel(), math::trajectory::computeMaxSpeedFromDistance(
+					    _trajectory[2].getMaxJerk(), _trajectory[2].getMaxAccel(),
+					    distance_start_target, arrival_z_speed));
+
+	return max_speed;
+}
+
+const Vector3f PositionSmoothing::_getCrossingPoint(const Vector3f &position, const Vector3f(&waypoints)[3]) const
+{
+	const auto &target = waypoints[1];
+
+	if (!_isTurning(target)) {
+		return target;
+	}
+
+	// Get the crossing point using L1-style guidance
+	auto l1_point = _getL1Point(position, waypoints);
+	return {l1_point(0), l1_point(1), target(2)};
+}
+
+const Vector2f PositionSmoothing::_getL1Point(const Vector3f &position, const Vector3f(&waypoints)[3]) const
+{
+	const Vector2f pos_traj(_trajectory[0].getCurrentPosition(),
+				_trajectory[1].getCurrentPosition());
+	const Vector2f u_prev_to_target = Vector2f(waypoints[1] - waypoints[0]).unit_or_zero();
+	const Vector2f prev_to_pos(pos_traj - Vector2f(waypoints[0]));
+	const Vector2f prev_to_closest(u_prev_to_target * (prev_to_pos * u_prev_to_target));
+	const Vector2f closest_pt = Vector2f(waypoints[0]) + prev_to_closest;
+
+	// Compute along-track error using L1 distance and cross-track error
+	const float crosstrack_error = Vector2f(closest_pt - pos_traj).length();
+
+	const float l1 = math::max(_target_acceptance_radius, 5.f);
+	float alongtrack_error = 0.f;
+
+	// Protect against sqrt of a negative number
+	if (l1 > crosstrack_error) {
+		alongtrack_error = sqrtf(l1 * l1 - crosstrack_error * crosstrack_error);
+	}
+
+	// Position of the point on the line where L1 intersect the line between the two waypoints
+	const Vector2f l1_point = closest_pt + alongtrack_error * u_prev_to_target;
+
+	return l1_point;
+}
+
+const Vector3f PositionSmoothing::_generateVelocitySetpoint(const Vector3f &position, const Vector3f(&waypoints)[3],
+		const Vector3f &feedforward_velocity_setpoint)
+{
+	// Interface: A valid position setpoint generates a velocity target using conservative motion constraints.
+	// If a velocity is specified, that is used as a feedforward to track the position setpoint
+	// (ie. it assumes the position setpoint is moving at the specified velocity)
+	// If the position setpoints are set to NAN, the values in the velocity setpoints are used as velocity targets: nothing to do here.
+	auto &target = waypoints[1];
+	const bool xy_target_valid = PX4_ISFINITE(target(0)) && PX4_ISFINITE(target(1));
+	const bool z_target_valid = PX4_ISFINITE(target(2));
+
+	Vector3f velocity_setpoint = feedforward_velocity_setpoint;
+
+	if (xy_target_valid && z_target_valid) {
+		// Use 3D position setpoint to generate a 3D velocity setpoint
+		Vector3f pos_traj(_trajectory[0].getCurrentPosition(),
+				  _trajectory[1].getCurrentPosition(),
+				  _trajectory[2].getCurrentPosition());
+
+		const Vector3f u_pos_traj_to_dest((_getCrossingPoint(position, waypoints) - pos_traj).unit_or_zero());
+
+		float xy_speed = _getMaxXYSpeed(waypoints);
+		const float z_speed = _getMaxZSpeed(waypoints);
+
+		if (_isTurning(target)) {
+			// Limit speed during a turn
+			xy_speed = math::min(_max_speed_previous, xy_speed);
+
+		} else {
+			_max_speed_previous = xy_speed;
+		}
+
+		Vector3f vel_sp_constrained = u_pos_traj_to_dest * sqrtf(xy_speed * xy_speed + z_speed * z_speed);
+		math::trajectory::clampToXYNorm(vel_sp_constrained, xy_speed, 0.5f);
+		math::trajectory::clampToZNorm(vel_sp_constrained, z_speed, 0.5f);
+
+		for (int i = 0; i < 3; i++) {
+			// If available, use the existing velocity as a feedforward, otherwise replace it
+			if (PX4_ISFINITE(velocity_setpoint(i))) {
+				velocity_setpoint(i) += vel_sp_constrained(i);
+
+			} else {
+				velocity_setpoint(i) = vel_sp_constrained(i);
+			}
+		}
+	}
+
+	else if (xy_target_valid) {
+		// Use 2D position setpoint to generate a 2D velocity setpoint
+
+		// Get various path specific vectors
+		Vector2f pos_traj(_trajectory[0].getCurrentPosition(), _trajectory[1].getCurrentPosition());
+		Vector2f pos_traj_to_dest_xy = Vector2f(_getCrossingPoint(position, waypoints)) - pos_traj;
+		Vector2f u_pos_traj_to_dest_xy(pos_traj_to_dest_xy.unit_or_zero());
+
+		float xy_speed = _getMaxXYSpeed(waypoints);
+
+		if (_isTurning(target)) {
+			// Lock speed during turn
+			xy_speed = math::min(_max_speed_previous, xy_speed);
+
+		} else {
+			_max_speed_previous = xy_speed;
+		}
+
+		Vector2f vel_sp_constrained_xy = u_pos_traj_to_dest_xy * xy_speed;
+
+		for (int i = 0; i < 2; i++) {
+			// If available, use the existing velocity as a feedforward, otherwise replace it
+			if (PX4_ISFINITE(velocity_setpoint(i))) {
+				velocity_setpoint(i) += vel_sp_constrained_xy(i);
+
+			} else {
+				velocity_setpoint(i) = vel_sp_constrained_xy(i);
+			}
+		}
+	}
+
+	else if (z_target_valid) {
+		// Use Z position setpoint to generate a Z velocity setpoint
+
+		const float z_dir = matrix::sign(target(2) - _trajectory[2].getCurrentPosition());
+		const float vel_sp_z = z_dir * _getMaxZSpeed(waypoints);
+
+		// If available, use the existing velocity as a feedforward, otherwise replace it
+		if (PX4_ISFINITE(velocity_setpoint(2))) {
+			velocity_setpoint(2) += vel_sp_z;
+
+		} else {
+			velocity_setpoint(2) = vel_sp_z;
+		}
+	}
+
+	return velocity_setpoint;
+}
+
+
+void PositionSmoothing::_generateTrajectory(
+	const Vector3f &position,
+	const Vector3f &velocity_setpoint,
+	float delta_time,
+	PositionSmoothingSetpoints &out_setpoints)
+{
+	if (!PX4_ISFINITE(velocity_setpoint(0)) || !PX4_ISFINITE(velocity_setpoint(1))
+	    || !PX4_ISFINITE(velocity_setpoint(2))) {
+		return;
+	}
+
+	/* Slow down the trajectory by decreasing the integration time based on the position error.
+	 * This is only performed when the drone is behind the trajectory
+	 */
+	Vector2f position_trajectory_xy(_trajectory[0].getCurrentPosition(), _trajectory[1].getCurrentPosition());
+	Vector2f position_xy(position);
+	Vector2f vel_traj_xy(_trajectory[0].getCurrentVelocity(), _trajectory[1].getCurrentVelocity());
+	Vector2f drone_to_trajectory_xy(position_trajectory_xy - position_xy);
+	float position_error = drone_to_trajectory_xy.length();
+
+	float time_stretch = 1.f - math::constrain(position_error / _max_allowed_horizontal_error, 0.f, 1.f);
+
+	// Don't stretch time if the drone is ahead of the position setpoint
+	if (drone_to_trajectory_xy.dot(vel_traj_xy) < 0.f) {
+		time_stretch = 1.f;
+	}
+
+	for (int i = 0; i < 3; ++i) {
+		_trajectory[i].updateTraj(delta_time, time_stretch);
+		out_setpoints.jerk(i) = _trajectory[i].getCurrentJerk();
+		out_setpoints.acceleration(i) = _trajectory[i].getCurrentAcceleration();
+		out_setpoints.velocity(i) = _trajectory[i].getCurrentVelocity();
+		out_setpoints.position(i) = _trajectory[i].getCurrentPosition();
+	}
+
+	for (int i = 0; i < 3; ++i) {
+		_trajectory[i].updateDurations(velocity_setpoint(i));
+	}
+
+	VelocitySmoothing::timeSynchronization(_trajectory, 3);
+}

src/lib/motion_planning/PositionSmoothing.hpp

@@ -0,0 +1,393 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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 <cmath>
+#include <motion_planning/VelocitySmoothing.hpp>
+
+#include <matrix/matrix/math.hpp>
+#include <px4_defines.h>
+
+using matrix::Vector2f;
+using matrix::Vector3f;
+
+/**
+ * @brief Class that generates setpoints for a smooth trajectory
+ * to position waypoints.
+ *
+ * This is achieved by first generating an unsmoothed velocity setpoint
+ * which then gets smoothed using the VelocitySmoothing library.
+ */
+class PositionSmoothing
+{
+
+public:
+	/**
+	 * @brief Result type of the position smoothing algorithm.
+	 * The included values shall then be used as setpoints for
+	 * the controllers.
+	 */
+	struct PositionSmoothingSetpoints {
+		Vector3f jerk;
+		Vector3f acceleration;
+		Vector3f velocity;
+		Vector3f position;
+		Vector3f unsmoothed_velocity;
+	};
+
+	/**
+	 * @brief Generates new setpoints for jerk, acceleration, velocity and position
+	 * to reach the given waypoint smoothly from current position.
+	 *
+	 * @param position Current position of the vehicle
+	 * @param waypoints 0: Past waypoint, 1: target, 2: Target after next target
+	 * @param feedforward_velocity FF velocity
+	 * @param delta_time Time since last invocation of the function
+	 * @param force_zero_velocity_setpoint Force vehicle to stop. Generate trajectory that ends with still vehicle.
+	 * @param out_setpoints Output of the generated setpoints
+	 */
+	void generateSetpoints(
+		const Vector3f &position,
+		const Vector3f(&waypoints)[3],
+		const Vector3f &feedforward_velocity,
+		float delta_time,
+		bool force_zero_velocity_setpoint,
+		PositionSmoothingSetpoints &out_setpoints
+	);
+
+	/**
+	 * @brief Reset internal state to the given values
+	 *
+	 * @param acceleration current acceleration
+	 * @param velocity current velocity
+	 * @param position current position
+	 */
+	void reset(const Vector3f &acceleration, const Vector3f &velocity, const Vector3f &position)
+	{
+		for (size_t i = 0; i < 3; i++) {
+			_trajectory[i].reset(acceleration(i), velocity(i), position(i));
+		}
+	}
+
+	/**
+	 * @return float Current trajectory acceleration in X
+	 */
+	inline float getCurrentAccelerationX() const
+	{
+		return _trajectory[0].getCurrentAcceleration();
+	}
+
+	/**
+	 * @return float Current trajectory acceleration in Y
+	 */
+	inline float getCurrentAccelerationY() const
+	{
+		return _trajectory[1].getCurrentAcceleration();
+	}
+
+	/**
+	 * @return float Current trajectory acceleration in Z
+	 */
+	inline float getCurrentAccelerationZ() const
+	{
+		return _trajectory[2].getCurrentAcceleration();
+	}
+
+	/**
+	 * @return float Current trajectory acceleration
+	 */
+	inline Vector3f getCurrentAcceleration() const
+	{
+		return {getCurrentAccelerationX(), getCurrentAccelerationY(), getCurrentAccelerationZ()};
+	}
+
+	/**
+	 * @return float Current trajectory velocity in X
+	 */
+	inline float getCurrentVelocityX() const
+	{
+		return _trajectory[0].getCurrentVelocity();
+	}
+
+	/**
+	 * @return float Current trajectory velocity in Y
+	 */
+	inline float getCurrentVelocityY() const
+	{
+		return _trajectory[1].getCurrentVelocity();
+	}
+
+	/**
+	 * @return float Current trajectory velocity in Z
+	 */
+	inline float getCurrentVelocityZ() const
+	{
+		return _trajectory[2].getCurrentVelocity();
+	}
+
+	/**
+	 * @return float Current trajectory velocity
+	 */
+	inline Vector3f getCurrentVelocity() const
+	{
+		return {getCurrentVelocityX(), getCurrentVelocityY(), getCurrentVelocityZ()};
+	}
+
+	/**
+	 * @return float Current trajectory position in X
+	 */
+	inline float getCurrentPositionX() const
+	{
+		return _trajectory[0].getCurrentPosition();
+	}
+
+	/**
+	 * @return float Current trajectory position in Y
+	 */
+	inline float getCurrentPositionY() const
+	{
+		return _trajectory[1].getCurrentPosition();
+	}
+
+	/**
+	 * @return float Current trajectory position in Z
+	 */
+	inline float getCurrentPositionZ() const
+	{
+		return _trajectory[2].getCurrentPosition();
+	}
+
+	/**
+	 * @return float Current trajectory position
+	 */
+	inline Vector3f getCurrentPosition() const
+	{
+		return {getCurrentPositionX(), getCurrentPositionY(), getCurrentPositionZ()};
+	}
+
+	/**
+	 * @param jerk maximum jerk for generated trajectory
+	 */
+	inline void setMaxJerkXY(float jerk)
+	{
+		_trajectory[0].setMaxJerk(jerk);
+		_trajectory[1].setMaxJerk(jerk);
+	}
+
+	/**
+	 * @param jerk maximum jerk for generated trajectory
+	 */
+	inline void setMaxJerkZ(float jerk)
+	{
+		_trajectory[2].setMaxJerk(jerk);
+	}
+
+	/**
+	 * @param jerk maximum jerk for generated trajectory
+	 */
+	inline void setMaxJerk(const Vector3f &jerk)
+	{
+		_trajectory[0].setMaxJerk(jerk(0));
+		_trajectory[1].setMaxJerk(jerk(1));
+		_trajectory[2].setMaxJerk(jerk(2));
+	}
+
+	/**
+	 * @param accel maximum acceleration for generated trajectory
+	 */
+	inline void setMaxAccelerationXY(float accel)
+	{
+		_trajectory[0].setMaxAccel(accel);
+		_trajectory[1].setMaxAccel(accel);
+	}
+
+	/**
+	 * @param accel maximum acceleration for generated trajectory
+	 */
+	inline void setMaxAccelerationZ(float accel)
+	{
+		_trajectory[2].setMaxAccel(accel);
+	}
+
+	/**
+	 * @param accel maximum acceleration for generated trajectory
+	 */
+	inline void setMaxAcceleration(const Vector3f &accel)
+	{
+		_trajectory[0].setMaxAccel(accel(0));
+		_trajectory[1].setMaxAccel(accel(1));
+		_trajectory[2].setMaxAccel(accel(2));
+	}
+
+	/**
+	 * @param vel maximum velocity for generated trajectory
+	 */
+	inline void setMaxVelocityXY(float vel)
+	{
+		_trajectory[0].setMaxVel(vel);
+		_trajectory[1].setMaxVel(vel);
+	}
+
+	/**
+	 * @param vel maximum velocity for generated trajectory
+	 */
+	inline void setMaxVelocityZ(float vel)
+	{
+		_trajectory[2].setMaxVel(vel);
+	}
+
+	/**
+	 * @param vel maximum velocity for generated trajectory
+	 */
+	inline void setMaxVelocity(const Vector3f &vel)
+	{
+		_trajectory[0].setMaxVel(vel(0));
+		_trajectory[1].setMaxVel(vel(1));
+		_trajectory[2].setMaxVel(vel(2));
+	}
+
+	/**
+	 * @param error Maximum horizontal error allowed by the trajectory generator. Often set to param MPC_XY_ERR_MAX
+	 */
+	inline void setMaxAllowedHorizontalError(float error)
+	{
+		_max_allowed_horizontal_error = error;
+	}
+
+	/**
+	 * @param radius  Altitude Acceptance Radius. Often set to NAV_MC_ALT_RAD
+	 */
+	inline void setVerticalAcceptanceRadius(float radius)
+	{
+		_vertical_acceptance_radius = radius;
+	}
+
+	/**
+	 * @param speed vehicle cruise speed
+	 */
+	inline void setCruiseSpeed(float speed)
+	{
+		_cruise_speed = speed;
+	}
+
+	/**
+	 * @param gain Proportional gain for horizontal trajectory position error. Set to MPC_XY_TRAJ_P
+	 */
+	inline void setHorizontalTrajectoryGain(float gain)
+	{
+		_horizontal_trajectory_gain = gain;
+	}
+
+	/**
+	 * @param radius target acceptance radius
+	 */
+	inline void setTargetAcceptanceRadius(float radius)
+	{
+		_target_acceptance_radius = radius;
+	}
+
+	/**
+	 * @brief Set the current position in the trajectory to the given value.
+	 * Any coordinate with NAN will not be set
+	 *
+	 * @param position
+	 */
+	void forceSetPosition(const Vector3f &position)
+	{
+		for (size_t i = 0; i < 3; i++) {
+			if (PX4_ISFINITE(position(i))) {
+				_trajectory[i].setCurrentPosition(position(i));
+			}
+		}
+	}
+
+	/**
+	 * @brief Set the current velocity in the trajectory to the given value.
+	 * Any coordinate with NAN will not be set
+	 *
+	 * @param velocity
+	 */
+	void forceSetVelocity(const Vector3f &velocity)
+	{
+		for (size_t i = 0; i < 3; i++) {
+			if (PX4_ISFINITE(velocity(i))) {
+				_trajectory[i].setCurrentVelocity(velocity(i));
+			}
+		}
+	}
+
+	/**
+	 * @brief Set the current acceleration in the trajectory to the given value.
+	 * Any coordinate with NAN will not be set
+	 *
+	 * @param acceleration
+	 */
+	void forceSetAcceleration(const Vector3f &acceleration)
+	{
+		for (size_t i = 0; i < 3; i++) {
+			if (PX4_ISFINITE(acceleration(i))) {
+				_trajectory[i].setCurrentAcceleration(acceleration(i));
+			}
+		}
+	}
+
+
+private:
+	/* params, only modified from external */
+	float _max_allowed_horizontal_error{0.f};
+	float _vertical_acceptance_radius{0.f};
+	float _cruise_speed{0.f};
+	float _horizontal_trajectory_gain{0.f};
+	float _target_acceptance_radius{0.f};
+
+
+	/* Internal state */
+	VelocitySmoothing _trajectory[3]; ///< Trajectories in x, y and z directions
+	float _max_speed_previous{0.f};
+
+	/* Internal functions */
+	bool _isTurning(const Vector3f &target) const;
+	const Vector3f _generateVelocitySetpoint(const Vector3f &position, const Vector3f(&waypoints)[3],
+			const Vector3f &feedforward_velocity_setpoint);
+	const Vector2f _getL1Point(const Vector3f &position, const Vector3f(&waypoints)[3]) const;
+	const Vector3f _getCrossingPoint(const Vector3f &position, const Vector3f(&waypoints)[3]) const;
+	float _getMaxXYSpeed(const Vector3f(&waypoints)[3]) const;
+	float _getMaxZSpeed(const Vector3f(&waypoints)[3]) const;
+
+	void _generateTrajectory(
+		const Vector3f &position,
+		const Vector3f &velocity_setpoint,
+		float delta_time,
+		PositionSmoothingSetpoints &out_setpoints);
+};

src/lib/motion_planning/PositionSmoothingTest.cpp

@@ -0,0 +1,191 @@
+#include <gtest/gtest.h>
+
+#include <motion_planning/PositionSmoothing.hpp>
+
+
+static constexpr float MAX_JERK = 4.f;
+static constexpr float MAX_ACCELERATION = 3.f;
+
+static constexpr float MAX_ALLOWED_HOR_ERR = 2.f;
+static constexpr float VERTICAL_ACCEPTANCE_RADIUS = 0.8f;
+static constexpr float CRUISE_SPEED = 5.f;
+static constexpr float MAX_VELOCITY = CRUISE_SPEED;
+
+static constexpr float HORIZONTAL_TRAJECTORY_GAIN = 0.5f;
+static constexpr float TARGET_ACCEPTANCE_RADIUS = 0.5f;
+
+
+class PositionSmoothingTest : public ::testing::Test
+{
+
+public:
+	PositionSmoothing _position_smoothing;
+
+	PositionSmoothingTest()
+	{
+		_position_smoothing.setMaxJerk({MAX_JERK, MAX_JERK, MAX_JERK});
+		_position_smoothing.setMaxAcceleration({MAX_ACCELERATION, MAX_ACCELERATION, MAX_ACCELERATION});
+		_position_smoothing.setMaxVelocity({MAX_VELOCITY, MAX_VELOCITY, MAX_VELOCITY});
+		_position_smoothing.setMaxAllowedHorizontalError(MAX_ALLOWED_HOR_ERR);
+		_position_smoothing.setVerticalAcceptanceRadius(VERTICAL_ACCEPTANCE_RADIUS);
+		_position_smoothing.setCruiseSpeed(CRUISE_SPEED);
+		_position_smoothing.setHorizontalTrajectoryGain(HORIZONTAL_TRAJECTORY_GAIN);
+		_position_smoothing.setTargetAcceptanceRadius(TARGET_ACCEPTANCE_RADIUS);
+
+		_position_smoothing.reset({0.f, 0.f, 0.f}, {0.f, 0.f, 0.f}, {0.f, 0.f, 0.f});
+	}
+
+	static bool _vectorNear(const Vector3f &a, const Vector3f &b, float EPS = 1e-4f)
+	{
+		return (fabsf(a(0) - b(0)) < EPS) && (fabsf(a(1) - b(1)) < EPS) && (fabsf(a(2) - b(2)) < EPS);
+	}
+
+	static void expectVectorEqual(const Vector3f &expected, const Vector3f &value, const char *name, float EPS)
+	{
+		EXPECT_TRUE(_vectorNear(expected, value, EPS)) <<
+				"Vector \"" << name << "\" expected [" <<
+				expected(0) << ", " <<
+				expected(1) << ", " <<
+				expected(2) << "] has value [" <<
+				value(0) << ", " <<
+				value(1) << ", " <<
+				value(2) << "]\n";
+	}
+
+	static void expectDynamicsLimitsRespected(const PositionSmoothing::PositionSmoothingSetpoints &setpoints)
+	{
+		EXPECT_LE(fabsf(setpoints.velocity(0)), MAX_VELOCITY) << "Velocity in x too high\n";
+		EXPECT_LE(fabsf(setpoints.velocity(1)), MAX_VELOCITY) << "Velocity in y too high\n";
+		EXPECT_LE(fabsf(setpoints.velocity(2)), MAX_VELOCITY) << "Velocity in z too high\n";
+		EXPECT_LE(fabsf(setpoints.acceleration(0)), MAX_ACCELERATION) << "Acceleration in x too high\n";
+		EXPECT_LE(fabsf(setpoints.acceleration(1)), MAX_ACCELERATION) << "Acceleration in y too high\n";
+		EXPECT_LE(fabsf(setpoints.acceleration(2)), MAX_ACCELERATION) << "Acceleration in z too high\n";
+		EXPECT_LE(fabsf(setpoints.jerk(0)), MAX_JERK) << "Jerk in x too high\n";
+		EXPECT_LE(fabsf(setpoints.jerk(1)), MAX_JERK) << "Jerk in y too high\n";
+		EXPECT_LE(fabsf(setpoints.jerk(2)), MAX_JERK) << "Jerk in z too high\n";
+	}
+};
+
+
+TEST_F(PositionSmoothingTest, reachesTargetPositionSetpoint)
+{
+	const float EPS = 1e-4;
+	const int N_ITER = 2000;
+	const float DELTA_T = 0.02f;
+	const Vector3f INITIAL_POSITION{0.f, 0.f, 0.f};
+	const Vector3f FF_VELOCITY{0.f, 0.f, 0.f};
+	const Vector3f TARGET{12.f, 17.f, 8.f};
+
+	Vector3f waypoints[3] = {INITIAL_POSITION, TARGET, TARGET};
+
+	Vector3f position{0.f, 0.f, 0.f};
+
+	PositionSmoothing::PositionSmoothingSetpoints out;
+
+	int iteration = 0;
+
+	for (; iteration < N_ITER; iteration++) {
+		_position_smoothing.generateSetpoints(
+			position,
+			waypoints,
+			FF_VELOCITY,
+			DELTA_T,
+			false,
+			out
+		);
+		position = out.position;
+		expectDynamicsLimitsRespected(out);
+
+		if ((position - TARGET).length() < EPS) {
+			printf("Converged in %d iterations\n", iteration);
+			break;
+		}
+	}
+
+	expectVectorEqual(TARGET, position, "position", EPS);
+	EXPECT_LT(iteration, N_ITER) << "Took too long to converge\n";
+}
+
+
+TEST_F(PositionSmoothingTest, reachesTargetVelocityIntegration)
+{
+	const float EPS = 1e-4;
+	const int N_ITER = 2000;
+	const float DELTA_T = 0.02f;
+	const Vector3f INITIAL_POSITION{0.f, 0.f, 0.f};
+	const Vector3f FF_VELOCITY{0.f, 0.f, 0.f};
+	const Vector3f TARGET{12.f, 17.f, 8.f};
+
+	Vector3f waypoints[3] = {INITIAL_POSITION, TARGET, TARGET};
+
+	Vector3f position{0.f, 0.f, 0.f};
+
+	PositionSmoothing::PositionSmoothingSetpoints out;
+
+	int iteration = 0;
+
+	for (; iteration < N_ITER; iteration++) {
+		_position_smoothing.generateSetpoints(
+			position,
+			waypoints,
+			FF_VELOCITY,
+			DELTA_T,
+			false,
+			out
+		);
+		position += out.velocity * DELTA_T;
+		expectDynamicsLimitsRespected(out);
+
+
+		if ((position - TARGET).length() < EPS) {
+			printf("Converged in %d iterations\n", iteration);
+			break;
+		}
+	}
+
+	expectVectorEqual(TARGET, position, "position", EPS);
+	EXPECT_LT(iteration, N_ITER) << "Took too long to converge\n";
+}
+
+
+TEST_F(PositionSmoothingTest, reachesTargetInitialVelocity)
+{
+	const float EPS = 1e-4;
+	const int N_ITER = 2000;
+	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};
+
+
+	Vector3f waypoints[3] = {INITIAL_POSITION, TARGET, NEXT_TARGET};
+	Vector3f ff_velocity{1.f, 0.1f, 0.3f};
+
+	Vector3f position{0.f, 0.f, 0.f};
+
+	PositionSmoothing::PositionSmoothingSetpoints out;
+
+	int iteration = 0;
+
+	for (; iteration < N_ITER; iteration++) {
+		_position_smoothing.generateSetpoints(
+			position,
+			waypoints,
+			ff_velocity,
+			DELTA_T,
+			false,
+			out
+		);
+		position = out.position;
+		ff_velocity = {0.f, 0.f, 0.f};
+		expectDynamicsLimitsRespected(out);
+
+		if ((position - TARGET).length() < EPS) {
+			printf("Converged in %d iterations\n", iteration);
+			break;
+		}
+	}
+
+	expectVectorEqual(TARGET, position, "position", EPS);
+	EXPECT_LT(iteration, N_ITER) << "Took too long to converge\n";
+}

src/lib/motion_planning/TrajectoryConstraints.hpp

@@ -0,0 +1,180 @@
+/****************************************************************************
+ *
+ *   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;
+}
+
+/*
+ * Constrain the 3D vector given a maximum XY norm
+ * If the XY norm of the 3D vector is larger than the maximum norm, the whole vector
+ * is scaled down to respect the constraint.
+ * If the maximum norm is small (defined by the "accuracy" parameter),
+ * only the XY components are scaled down to avoid affecting
+ * Z in case of numerical issues
+ */
+inline void clampToXYNorm(Vector3f &target, float max_xy_norm, float accuracy = FLT_EPSILON)
+{
+	const float xynorm = target.xy().norm();
+	const float scale_factor = (xynorm > FLT_EPSILON)
+				   ? max_xy_norm / xynorm
+				   : 1.f;
+
+	if (scale_factor < 1.f) {
+		if (max_xy_norm < accuracy && xynorm < accuracy) {
+			target.xy() = Vector2f(target) * scale_factor;
+
+		} else {
+			target *= scale_factor;
+		}
+	}
+}
+
+/*
+ * Constrain the 3D vector given a maximum Z norm
+ * If the Z component of the 3D vector is larger than the maximum norm, the whole vector
+ * is scaled down to respect the constraint.
+ * If the maximum norm is small (defined by the "accuracy parameter),
+ * only the Z component is scaled down to avoid affecting
+ * XY in case of numerical issues
+ */
+inline void clampToZNorm(Vector3f &target, float max_z_norm, float accuracy = FLT_EPSILON)
+{
+	const float znorm = fabs(target(2));
+	const float scale_factor = (znorm > FLT_EPSILON)
+				   ? max_z_norm / znorm
+				   : 1.f;
+
+	if (scale_factor < 1.f) {
+		if (max_z_norm < accuracy && znorm < accuracy) {
+			target(2) *= scale_factor;
+
+		} else {
+			target *= scale_factor;
+		}
+	}
+}
+
+}
+}

src/lib/motion_planning/TrajectoryConstraintsTest.cpp

@@ -0,0 +1,364 @@
+#include <gtest/gtest.h>
+
+#include "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));
+}