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.master
Thomas Debrunner
3 years ago
committed by
Daniel Agar
10 changed files with 970 additions and 345 deletions
@ -0,0 +1,323 @@
@@ -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 @@
@@ -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 @@
@@ -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"; |
||||
} |
||||
Loading…
Reference in new issue