diff --git a/.clang-tidy b/.clang-tidy index 649cd6d8ac..ed0758e9dc 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -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, diff --git a/src/lib/motion_planning/CMakeLists.txt b/src/lib/motion_planning/CMakeLists.txt index b090aa0337..0d1fa8546c 100644 --- a/src/lib/motion_planning/CMakeLists.txt +++ b/src/lib/motion_planning/CMakeLists.txt @@ -33,8 +33,10 @@ px4_add_library(motion_planning VelocitySmoothing.cpp + PositionSmoothing.cpp ) target_include_directories(motion_planning PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) px4_add_unit_gtest(SRC VelocitySmoothingTest.cpp LINKLIBS motion_planning) +px4_add_unit_gtest(SRC PositionSmoothingTest.cpp LINKLIBS motion_planning) diff --git a/src/lib/motion_planning/PositionSmoothing.cpp b/src/lib/motion_planning/PositionSmoothing.cpp new file mode 100644 index 0000000000..20311cb37c --- /dev/null +++ b/src/lib/motion_planning/PositionSmoothing.cpp @@ -0,0 +1,323 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "PositionSmoothing.hpp" +#include "TrajectoryConstraints.hpp" +#include +#include +#include + + +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); +} diff --git a/src/lib/motion_planning/PositionSmoothing.hpp b/src/lib/motion_planning/PositionSmoothing.hpp new file mode 100644 index 0000000000..420366f282 --- /dev/null +++ b/src/lib/motion_planning/PositionSmoothing.hpp @@ -0,0 +1,393 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include + +#include +#include + +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); +}; diff --git a/src/lib/motion_planning/PositionSmoothingTest.cpp b/src/lib/motion_planning/PositionSmoothingTest.cpp new file mode 100644 index 0000000000..f550a5ee77 --- /dev/null +++ b/src/lib/motion_planning/PositionSmoothingTest.cpp @@ -0,0 +1,191 @@ +#include + +#include + + +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"; +} diff --git a/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/TrajectoryConstraints.hpp b/src/lib/motion_planning/TrajectoryConstraints.hpp similarity index 100% rename from src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/TrajectoryConstraints.hpp rename to src/lib/motion_planning/TrajectoryConstraints.hpp diff --git a/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/TrajectoryConstraintsTest.cpp b/src/lib/motion_planning/TrajectoryConstraintsTest.cpp similarity index 100% rename from src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/TrajectoryConstraintsTest.cpp rename to src/lib/motion_planning/TrajectoryConstraintsTest.cpp diff --git a/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/CMakeLists.txt index 4e22acde80..11aa634814 100644 --- a/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/CMakeLists.txt +++ b/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/CMakeLists.txt @@ -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) diff --git a/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp b/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp index f9142f4893..0306fb31b8 100644 --- a/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp +++ b/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.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; -} diff --git a/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp b/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp index f7d4ec3367..ece4e8bbf1 100644 --- a/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp +++ b/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp @@ -41,7 +41,7 @@ #pragma once #include "FlightTaskAutoMapper.hpp" -#include +#include 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) _param_mis_yaw_err, // yaw-error threshold (ParamFloat) _param_mpc_acc_hor, // acceleration in flight