fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 00:43:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

flight_mode_manager: Extracted position trajectory motion planning into a library

Browse Source 
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.
This commit is contained in:
Thomas Debrunner
2021-10-13 21:25:43 -04:00
committed by Daniel Agar
parent 07303af8f8
commit fed234de81
10 changed files with 970 additions and 345 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/CMakeLists.txt
-3
View File
@@ -37,6 +37,3 @@ px4_add_library(FlightTaskAutoLineSmoothVel
target_link_libraries(FlightTaskAutoLineSmoothVel PUBLIC FlightTaskAutoMapper FlightTaskUtility)
target_include_directories(FlightTaskAutoLineSmoothVel PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
px4_add_unit_gtest(SRC TrajectoryConstraintsTest.cpp)
src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
+55 -326
View File
@@ -37,10 +37,9 @@
#include "FlightTaskAutoLineSmoothVel.hpp"
#include "TrajectoryConstraints.hpp"
using namespace matrix;
bool FlightTaskAutoLineSmoothVel::activate(const vehicle_local_position_setpoint_s &last_setpoint)
{
bool ret = FlightTaskAutoMapper::activate(last_setpoint);
@@ -60,9 +59,7 @@ bool FlightTaskAutoLineSmoothVel::activate(const vehicle_local_position_setpoint
if (!PX4_ISFINITE(accel_prev(i))) { accel_prev(i) = 0.f; }
}
for (int i = 0; i < 3; ++i) {
_trajectory[i].reset(accel_prev(i), vel_prev(i), pos_prev(i));
}
_position_smoothing.reset(accel_prev, vel_prev, pos_prev);
_yaw_sp_prev = PX4_ISFINITE(last_setpoint.yaw) ? last_setpoint.yaw : _yaw;
_updateTrajConstraints();
@@ -76,11 +73,7 @@ void FlightTaskAutoLineSmoothVel::reActivate()
FlightTaskAutoMapper::reActivate();
// On ground, reset acceleration and velocity to zero
for (int i = 0; i < 2; ++i) {
_trajectory[i].reset(0.f, 0.f, _position(i));
}
_trajectory[2].reset(0.f, 0.7f, _position(2));
_position_smoothing.reset({0.f, 0.f, 0.f}, {0.f, 0.f, 0.7f}, _position);
}
/**
@@ -90,24 +83,22 @@ void FlightTaskAutoLineSmoothVel::reActivate()
*/
void FlightTaskAutoLineSmoothVel::_ekfResetHandlerPositionXY(const matrix::Vector2f &delta_xy)
{
_trajectory[0].setCurrentPosition(_position(0));
_trajectory[1].setCurrentPosition(_position(1));
_position_smoothing.forceSetPosition({_position(0), _position(1), NAN});
}
void FlightTaskAutoLineSmoothVel::_ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vxy)
{
_trajectory[0].setCurrentVelocity(_velocity(0));
_trajectory[1].setCurrentVelocity(_velocity(1));
_position_smoothing.forceSetVelocity({_velocity(0), _velocity(1), NAN});
}
void FlightTaskAutoLineSmoothVel::_ekfResetHandlerPositionZ(float delta_z)
{
_trajectory[2].setCurrentPosition(_position(2));
_position_smoothing.forceSetPosition({NAN, NAN, _position(2)});
}
void FlightTaskAutoLineSmoothVel::_ekfResetHandlerVelocityZ(float delta_vz)
{
_trajectory[2].setCurrentVelocity(_velocity(2));
_position_smoothing.forceSetVelocity({NAN, NAN, _velocity(2)});
}
void FlightTaskAutoLineSmoothVel::_ekfResetHandlerHeading(float delta_psi)
@@ -118,9 +109,32 @@ void FlightTaskAutoLineSmoothVel::_ekfResetHandlerHeading(float delta_psi)
void FlightTaskAutoLineSmoothVel::_generateSetpoints()
{
_checkEmergencyBraking();
_updateTurningCheck();
_prepareSetpoints();
_generateTrajectory();
Vector3f waypoints[] = {_prev_wp, _position_setpoint, _next_wp};
if (isTargetModified()) {
// In case object avoidance has injected a new setpoint, we take this as the next waypoints
waypoints[2] = _position_setpoint;
}
const bool should_wait_for_yaw_align = _param_mpc_yaw_mode.get() == 4 && !_yaw_sp_aligned;
_updateTrajConstraints();
PositionSmoothing::PositionSmoothingSetpoints smoothed_setpoints;
_position_smoothing.generateSetpoints(
_position,
waypoints,
_velocity_setpoint,
_deltatime,
should_wait_for_yaw_align,
smoothed_setpoints
);
_jerk_setpoint = smoothed_setpoints.jerk;
_acceleration_setpoint = smoothed_setpoints.acceleration;
_velocity_setpoint = smoothed_setpoints.velocity;
_position_setpoint = smoothed_setpoints.position;
_unsmoothed_velocity_setpoint = smoothed_setpoints.unsmoothed_velocity;
_want_takeoff = smoothed_setpoints.unsmoothed_velocity(2) < -0.3f;
if (!PX4_ISFINITE(_yaw_setpoint) && !PX4_ISFINITE(_yawspeed_setpoint)) {
// no valid heading -> generate heading in this flight task
@@ -131,36 +145,17 @@ void FlightTaskAutoLineSmoothVel::_generateSetpoints()
void FlightTaskAutoLineSmoothVel::_checkEmergencyBraking()
{
if (!_is_emergency_braking_active) {
if (_trajectory[2].getCurrentVelocity() > (2.f * _param_mpc_z_vel_max_dn.get())) {
if (_position_smoothing.getCurrentVelocityZ() > (2.f * _param_mpc_z_vel_max_dn.get())) {
_is_emergency_braking_active = true;
}
} else {
if (fabsf(_trajectory[2].getCurrentVelocity()) < 0.01f) {
if (fabsf(_position_smoothing.getCurrentVelocityZ()) < 0.01f) {
_is_emergency_braking_active = false;
}
}
}
void FlightTaskAutoLineSmoothVel::_updateTurningCheck()
{
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.
_is_turning = vel_traj.longerThan(0.2f)
&& cos_align < 0.98f
&& pos_to_target.longerThan(_target_acceptance_radius);
}
void FlightTaskAutoLineSmoothVel::_generateHeading()
{
@@ -187,98 +182,6 @@ bool FlightTaskAutoLineSmoothVel::_generateHeadingAlongTraj()
return res;
}
/* 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
*/
float FlightTaskAutoLineSmoothVel::_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);
}
float FlightTaskAutoLineSmoothVel::_constrainAbs(float val, float max)
{
return sign(val) * math::min(fabsf(val), fabsf(max));
}
float FlightTaskAutoLineSmoothVel::_getMaxXYSpeed() const
{
Vector3f pos_traj(_trajectory[0].getCurrentPosition(),
_trajectory[1].getCurrentPosition(),
_trajectory[2].getCurrentPosition());
math::trajectory::VehicleDynamicLimits config;
config.z_accept_rad = _param_nav_mc_alt_rad.get();
config.xy_accept_rad = _target_acceptance_radius;
config.max_acc_xy = _trajectory[0].getMaxAccel();
config.max_jerk = _trajectory[0].getMaxJerk();
config.max_speed_xy = _mc_cruise_speed;
config.max_acc_xy_radius_scale = _param_mpc_xy_traj_p.get();
// constrain velocity to go to the position setpoint first if the position setpoint has been modified by an external source
// (eg. Obstacle Avoidance)
Vector3f waypoints[3] = {pos_traj, _target, _next_wp};
if (isTargetModified()) {
waypoints[2] = waypoints[1] = _position_setpoint;
}
float max_xy_speed = math::trajectory::computeXYSpeedFromWaypoints<3>(waypoints, config);
return max_xy_speed;
}
float FlightTaskAutoLineSmoothVel::_getMaxZSpeed() const
{
Vector3f pos_traj(_trajectory[0].getCurrentPosition(),
_trajectory[1].getCurrentPosition(),
_trajectory[2].getCurrentPosition());
float z_setpoint = _target(2);
// constrain velocity to go to the position setpoint first if the position setpoint has been modified by an external source
// (eg. Obstacle Avoidance)
bool z_valid = PX4_ISFINITE(_position_setpoint(2));
bool z_modified = z_valid && fabsf((_target - _position_setpoint)(2)) > FLT_EPSILON;
if (z_modified) {
z_setpoint = _position_setpoint(2);
}
const float distance_start_target = fabs(z_setpoint - 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;
}
Vector3f FlightTaskAutoLineSmoothVel::getCrossingPoint() const
{
Vector3f pos_crossing_point{};
if (isTargetModified()) {
// Strictly follow the modified setpoint
pos_crossing_point = _position_setpoint;
} else {
if (_is_turning) {
// Get the crossing point using L1-style guidance
pos_crossing_point.xy() = getL1Point();
pos_crossing_point(2) = _target(2);
} else {
pos_crossing_point = _target;
}
}
return pos_crossing_point;
}
bool FlightTaskAutoLineSmoothVel::isTargetModified() const
{
@@ -289,158 +192,34 @@ bool FlightTaskAutoLineSmoothVel::isTargetModified() const
return xy_modified || z_modified;
}
Vector2f FlightTaskAutoLineSmoothVel::getL1Point() const
{
const Vector2f pos_traj(_trajectory[0].getCurrentPosition(),
_trajectory[1].getCurrentPosition());
const Vector2f target_xy(_target);
const Vector2f u_prev_to_target = Vector2f(target_xy - Vector2f(_prev_wp)).unit_or_zero();
const Vector2f prev_to_pos(pos_traj - Vector2f(_prev_wp));
const Vector2f prev_to_closest(u_prev_to_target * (prev_to_pos * u_prev_to_target));
const Vector2f closest_pt = Vector2f(_prev_wp) + 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;
}
void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
{
// 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.
_want_takeoff = false;
const bool should_wait_for_yaw_align = _param_mpc_yaw_mode.get() == 4 && !_yaw_sp_aligned;
if (should_wait_for_yaw_align || _is_emergency_braking_active) {
_velocity_setpoint.setAll(0.f);
return;
}
const bool xy_pos_setpoint_valid = PX4_ISFINITE(_position_setpoint(0)) && PX4_ISFINITE(_position_setpoint(1));
const bool z_pos_setpoint_valid = PX4_ISFINITE(_position_setpoint(2));
if (xy_pos_setpoint_valid && z_pos_setpoint_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() - pos_traj).unit_or_zero());
float xy_speed = _getMaxXYSpeed();
const float z_speed = _getMaxZSpeed();
if (_is_turning) {
// Lock speed during turn
xy_speed = math::min(_max_speed_prev, xy_speed);
} else {
_max_speed_prev = 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_pos_setpoint_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()) - pos_traj;
Vector2f u_pos_traj_to_dest_xy(pos_traj_to_dest_xy.unit_or_zero());
float xy_speed = _getMaxXYSpeed();
if (_is_turning) {
// Lock speed during turn
xy_speed = math::min(_max_speed_prev, xy_speed);
} else {
_max_speed_prev = 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_pos_setpoint_valid) {
// Use Z position setpoint to generate a Z velocity setpoint
const float z_dir = sign(_position_setpoint(2) - _trajectory[2].getCurrentPosition());
const float vel_sp_z = z_dir * _getMaxZSpeed();
// 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;
}
}
_want_takeoff = _velocity_setpoint(2) < -0.3f;
}
void FlightTaskAutoLineSmoothVel::_updateTrajConstraints()
{
// update params of the position smoothing
_position_smoothing.setMaxAllowedHorizontalError(_param_mpc_xy_err_max.get());
_position_smoothing.setVerticalAcceptanceRadius(_param_nav_mc_alt_rad.get());
_position_smoothing.setCruiseSpeed(_mc_cruise_speed);
_position_smoothing.setHorizontalTrajectoryGain(_param_mpc_xy_traj_p.get());
_position_smoothing.setTargetAcceptanceRadius(_target_acceptance_radius);
// Update the constraints of the trajectories
_trajectory[0].setMaxAccel(_param_mpc_acc_hor.get()); // TODO : Should be computed using heading
_trajectory[1].setMaxAccel(_param_mpc_acc_hor.get());
_trajectory[0].setMaxVel(_param_mpc_xy_vel_max.get());
_trajectory[1].setMaxVel(_param_mpc_xy_vel_max.get());
_trajectory[0].setMaxJerk(_param_mpc_jerk_auto.get()); // TODO : Should be computed using heading
_trajectory[1].setMaxJerk(_param_mpc_jerk_auto.get());
_trajectory[2].setMaxJerk(_param_mpc_jerk_auto.get());
_position_smoothing.setMaxAccelerationXY(_param_mpc_acc_hor.get()); // TODO : Should be computed using heading
_position_smoothing.setMaxVelocityXY(_param_mpc_xy_vel_max.get());
float max_jerk = _param_mpc_jerk_auto.get();
_position_smoothing.setMaxJerk({max_jerk, max_jerk, max_jerk}); // TODO : Should be computed using heading
if (_is_emergency_braking_active) {
// When initializing with large downward velocity, allow 1g of vertical
// acceleration for fast braking
_trajectory[2].setMaxAccel(9.81f);
_trajectory[2].setMaxJerk(9.81f);
_position_smoothing.setMaxAccelerationZ(9.81f);
_position_smoothing.setMaxJerkZ(9.81f);
// If the current velocity is beyond the usual constraints, tell
// the controller to exceptionally increase its saturations to avoid
// cutting out the feedforward
_constraints.speed_down = math::max(fabsf(_trajectory[2].getCurrentVelocity()), _param_mpc_z_vel_max_dn.get());
_constraints.speed_down = math::max(fabsf(_position_smoothing.getCurrentVelocityZ()), _param_mpc_z_vel_max_dn.get());
} else if (_velocity_setpoint(2) < 0.f) { // up
} else if (_unsmoothed_velocity_setpoint(2) < 0.f) { // up
float z_accel_constraint = _param_mpc_acc_up_max.get();
float z_vel_constraint = _param_mpc_z_vel_max_up.get();
@@ -455,64 +234,14 @@ void FlightTaskAutoLineSmoothVel::_updateTrajConstraints()
// Keep the altitude setpoint at the current altitude
// to avoid having it going down into the ground during
// the initial ramp as the velocity does not start at 0
_trajectory[2].setCurrentPosition(_position(2));
_position_smoothing.forceSetPosition({NAN, NAN, _position(2)});
}
_trajectory[2].setMaxVel(z_vel_constraint);
_trajectory[2].setMaxAccel(z_accel_constraint);
_position_smoothing.setMaxVelocityZ(z_vel_constraint);
_position_smoothing.setMaxAccelerationZ(z_accel_constraint);
} else { // down
_trajectory[2].setMaxAccel(_param_mpc_acc_down_max.get());
_trajectory[2].setMaxVel(_param_mpc_z_vel_max_dn.get());
_position_smoothing.setMaxAccelerationZ(_param_mpc_acc_down_max.get());
_position_smoothing.setMaxVelocityZ(_param_mpc_z_vel_max_dn.get());
}
}
void FlightTaskAutoLineSmoothVel::_generateTrajectory()
{
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 / _param_mpc_xy_err_max.get(), 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;
}
Vector3f jerk_sp_smooth;
Vector3f accel_sp_smooth;
Vector3f vel_sp_smooth;
Vector3f pos_sp_smooth;
for (int i = 0; i < 3; ++i) {
_trajectory[i].updateTraj(_deltatime, time_stretch);
jerk_sp_smooth(i) = _trajectory[i].getCurrentJerk();
accel_sp_smooth(i) = _trajectory[i].getCurrentAcceleration();
vel_sp_smooth(i) = _trajectory[i].getCurrentVelocity();
pos_sp_smooth(i) = _trajectory[i].getCurrentPosition();
}
_updateTrajConstraints();
for (int i = 0; i < 3; ++i) {
_trajectory[i].updateDurations(_velocity_setpoint(i));
}
VelocitySmoothing::timeSynchronization(_trajectory, 3);
_jerk_setpoint = jerk_sp_smooth;
_acceleration_setpoint = accel_sp_smooth;
_velocity_setpoint = vel_sp_smooth;
_position_setpoint = pos_sp_smooth;
}
src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp
+5 -16
View File
@@ -41,7 +41,7 @@
#pragma once
#include "FlightTaskAutoMapper.hpp"
#include <motion_planning/VelocitySmoothing.hpp>
#include <motion_planning/PositionSmoothing.hpp>
class FlightTaskAutoLineSmoothVel : public FlightTaskAutoMapper
{
@@ -52,6 +52,10 @@ public:
bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
void reActivate() override;
private:
PositionSmoothing _position_smoothing;
Vector3f _unsmoothed_velocity_setpoint;
protected:
/** Reset position or velocity setpoints in case of EKF reset event */
@@ -64,35 +68,20 @@ protected:
void _generateSetpoints() override; /**< Generate setpoints along line. */
void _generateHeading();
void _checkEmergencyBraking();
void _updateTurningCheck();
bool _generateHeadingAlongTraj(); /**< Generates heading along trajectory. */
static float _constrainOneSide(float val, float constraint); /**< Constrain val between INF and constraint */
static float _constrainAbs(float val, float max); /** Constrain the value -max <= val <= max */
float _getMaxXYSpeed() const;
float _getMaxZSpeed() const;
matrix::Vector3f getCrossingPoint() const;
bool isTargetModified() const;
matrix::Vector2f getL1Point() const;
float _max_speed_prev{};
bool _is_turning{false};
bool _is_emergency_braking_active{false};
void _prepareSetpoints(); /**< Generate velocity target points for the trajectory generator. */
void _updateTrajConstraints();
void _generateTrajectory();
/** determines when to trigger a takeoff (ignored in flight) */
bool _checkTakeoff() override { return _want_takeoff; };
bool _want_takeoff{false};
VelocitySmoothing _trajectory[3]; ///< Trajectories in x, y and z directions
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAutoMapper,
(ParamFloat<px4::params::MIS_YAW_ERR>) _param_mis_yaw_err, // yaw-error threshold
(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor, // acceleration in flight
src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/TrajectoryConstraints.hpp
-180
View File
@@ -1,180 +0,0 @@
/****************************************************************************
*
* 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/modules/flight_mode_manager/tasks/AutoLineSmoothVel/TrajectoryConstraintsTest.cpp
-364
View File
@@ -1,364 +0,0 @@
#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));
}