forked from Archive/PX4-Autopilot
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.
This commit is contained in:
Thomas Debrunner
Daniel Agar
parent
07303af8f8
commit
fed234de81
|
|
@ -82,6 +82,7 @@ Checks: '*,
|
|||
-modernize-use-equals-delete,
|
||||
-modernize-use-override,
|
||||
-modernize-use-using,
|
||||
-modernize-use-trailing-return-type,
|
||||
-performance-inefficient-string-concatenation,
|
||||
-readability-avoid-const-params-in-decls,
|
||||
-readability-container-size-empty,
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
|
@ -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);
|
||||
};
|
||||
|
|
@ -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";
|
||||
}
|
||||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Reference in New Issue