diff --git a/src/lib/matrix/matrix/helper_functions.hpp b/src/lib/matrix/matrix/helper_functions.hpp index 6a9e871ef0cd..5a0a77c5f8a8 100644 --- a/src/lib/matrix/matrix/helper_functions.hpp +++ b/src/lib/matrix/matrix/helper_functions.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include namespace matrix { @@ -41,7 +42,7 @@ Floating wrap_floating(Floating x, Floating low, Floating high) return x - range * num_wraps; } -} // namespace detail +} // namespace detail /** * Wrap single precision floating point value to stay in range [low, high) diff --git a/src/lib/motion_planning/HeadingSmoother.hpp b/src/lib/motion_planning/HeadingSmoother.hpp index 662e6f373c39..9179e0a8b0fd 100644 --- a/src/lib/motion_planning/HeadingSmoother.hpp +++ b/src/lib/motion_planning/HeadingSmoother.hpp @@ -31,16 +31,11 @@ * ****************************************************************************/ -/** - * @file HeadingSmoother.hpp - * - */ - #pragma once -#include -#include #include +#include +#include "VelocitySmoothing.hpp" /** * @brief Wrapper class for smoothing heading via maximum angular acceleration limited trajectories. @@ -68,8 +63,8 @@ class HeadingSmoother HeadingSmoother() { reset(0.f, 0.f); - setMaxHeadingRate(VelocitySmoothing::kMinAccel); - setMaxHeadingAccel(VelocitySmoothing::kMinJerk); + setMaxHeadingRate(0.f); + setMaxHeadingAccel(0.f); _heading_smoother.setMaxVel(6.f * M_PI_F); // arbitrary large angle the wrapped value should never reach } @@ -144,10 +139,10 @@ class HeadingSmoother } // [rad/s] minimum value of the smoother's maximum heading rate - static constexpr float kMinHeadingRate = VelocitySmoothing::kMinAccel; + static constexpr float kMinHeadingRate = 0.f; // [rad/s^2] minimum value of the smoother's maximum heading acceleration - static constexpr float kMinHeadingAccel = VelocitySmoothing::kMinJerk; + static constexpr float kMinHeadingAccel = 0.f; private: diff --git a/src/lib/motion_planning/VelocitySmoothing.cpp b/src/lib/motion_planning/VelocitySmoothing.cpp index 6471bfaf4c28..9abd6b882b73 100644 --- a/src/lib/motion_planning/VelocitySmoothing.cpp +++ b/src/lib/motion_planning/VelocitySmoothing.cpp @@ -68,6 +68,8 @@ float VelocitySmoothing::saturateT1ForAccel(float a0, float j_max, float T1, flo T1_new = (-a_max - a0) / j_max; } + // printf("T1_new: %.3f\n", (double)T1_new); + return T1_new; } @@ -97,6 +99,12 @@ float VelocitySmoothing::computeT1(float a0, float v3, float j_max, float a_max) } T1 = saturateT1ForAccel(a0, j_max, T1, a_max); + // printf("j_max: %.3f\n", (double)j_max); + // printf("T1_plus: %.3f\n", (double)T1_plus); + // printf("T1_minus: %.3f\n", (double)T1_minus); + // printf("T3_plus: %.3f\n", (double)T3_plus); + // printf("T3_minus: %.3f\n", (double)T3_minus); + // printf("T1: %.3f\n", (double)T1); return math::max(T1, 0.f); } @@ -167,13 +175,9 @@ void VelocitySmoothing::updateDurations(float vel_setpoint) _state_init = _state; _direction = computeDirection(); + printf("_direction %.3f\n", (double) _direction); - if (_direction != 0) { - updateDurationsMinimizeTotalTime(); - - } else { - _T1 = _T2 = _T3 = 0.f; - } + updateDurationsMinimizeTotalTime(); } int VelocitySmoothing::computeDirection() const @@ -187,7 +191,7 @@ int VelocitySmoothing::computeDirection() const if (direction == 0) { // If by braking immediately the velocity is exactly - // the require one with zero acceleration, then brake + // the required one with zero acceleration, then brake direction = sign(_state.a); } @@ -212,14 +216,19 @@ void VelocitySmoothing::updateDurationsMinimizeTotalTime() float jerk_max_T1 = _direction * _max_jerk; float delta_v = _vel_sp - _state.v; - // compute increasing acceleration time - _T1 = computeT1(_state.a, delta_v, jerk_max_T1, _max_accel); + if (jerk_max_T1 > FLT_EPSILON) { // zero direction or jerk would lead to division by zero + // compute increasing acceleration time + _T1 = computeT1(_state.a, delta_v, jerk_max_T1, _max_accel); + + // compute decreasing acceleration time + _T3 = computeT3(_T1, _state.a, jerk_max_T1); - // compute decreasing acceleration time - _T3 = computeT3(_T1, _state.a, jerk_max_T1); + // compute constant acceleration time + _T2 = computeT2(_T1, _T3, _state.a, delta_v, jerk_max_T1); - // compute constant acceleration time - _T2 = computeT2(_T1, _T3, _state.a, delta_v, jerk_max_T1); + } else { + _T1 = _T2 = _T3 = 0.f; + } } Trajectory VelocitySmoothing::evaluatePoly(float j, float a0, float v0, float x0, float t, int d) const @@ -292,12 +301,17 @@ void VelocitySmoothing::updateDurationsGivenTotalTime(float T123) float jerk_max_T1 = _direction * _max_jerk; float delta_v = _vel_sp - _state.v; - // compute increasing acceleration time - _T1 = computeT1(T123, _state.a, delta_v, jerk_max_T1, _max_accel); + if (jerk_max_T1 > FLT_EPSILON) { // zero direction or jerk would lead to division by zero + // compute increasing acceleration time + _T1 = computeT1(T123, _state.a, delta_v, jerk_max_T1, _max_accel); + + // compute decreasing acceleration time + _T3 = computeT3(_T1, _state.a, jerk_max_T1); - // compute decreasing acceleration time - _T3 = computeT3(_T1, _state.a, jerk_max_T1); + // compute constant acceleration time + _T2 = computeT2(T123, _T1, _T3); - // compute constant acceleration time - _T2 = computeT2(T123, _T1, _T3); + } else { + _T1 = _T2 = _T3 = 0.f; + } } diff --git a/src/lib/motion_planning/VelocitySmoothing.hpp b/src/lib/motion_planning/VelocitySmoothing.hpp index 25799d1ce945..2ea584d156c0 100644 --- a/src/lib/motion_planning/VelocitySmoothing.hpp +++ b/src/lib/motion_planning/VelocitySmoothing.hpp @@ -107,8 +107,6 @@ class VelocitySmoothing void setCurrentPosition(const float pos) { _state.x = _state_init.x = pos; } float getCurrentPosition() const { return _state.x; } - float getVelSp() const { return _vel_sp; } - float getT1() const { return _T1; } float getT2() const { return _T2; } float getT3() const { return _T3; } @@ -192,12 +190,12 @@ class VelocitySmoothing inline Trajectory evaluatePoly(float j, float a0, float v0, float x0, float t, int d) const; /* Input */ - float _vel_sp{0.0f}; + float _vel_sp{0.f}; /* Constraints */ - float _max_jerk = 22.f; - float _max_accel = 8.f; - float _max_vel = 6.f; + float _max_jerk{0.f}; + float _max_accel{0.f}; + float _max_vel{0.f}; /* State (previous setpoints) */ Trajectory _state{}; @@ -207,9 +205,9 @@ class VelocitySmoothing Trajectory _state_init{}; /* Duration of each phase */ - float _T1 = 0.f; ///< Increasing acceleration [s] - float _T2 = 0.f; ///< Constant acceleration [s] - float _T3 = 0.f; ///< Decreasing acceleration [s] + float _T1{0.f}; ///< Increasing acceleration [s] + float _T2{0.f}; ///< Constant acceleration [s] + float _T3{0.f}; ///< Decreasing acceleration [s] - float _local_time = 0.f; ///< Current local time + float _local_time{0.f}; ///< Current local time }; diff --git a/src/lib/motion_planning/VelocitySmoothingTest.cpp b/src/lib/motion_planning/VelocitySmoothingTest.cpp index 0106235769f4..cf4e0a8f9766 100644 --- a/src/lib/motion_planning/VelocitySmoothingTest.cpp +++ b/src/lib/motion_planning/VelocitySmoothingTest.cpp @@ -43,6 +43,65 @@ using namespace matrix; +TEST(VelocitySmoothingBasicTest, AllZeroCase) +{ + // GIVEN: An unconfigured VelocitySmoothing instance + VelocitySmoothing trajectory; + + // WHEN: We update it + trajectory.updateDurations(0.f); + trajectory.updateTraj(0.f); + + // THEN: All the trajectories should still be zero + EXPECT_FLOAT_EQ(trajectory.getTotalTime(), 0.f); + EXPECT_FLOAT_EQ(trajectory.getCurrentJerk(), 0.f); + EXPECT_FLOAT_EQ(trajectory.getCurrentAcceleration(), 0.f); + EXPECT_FLOAT_EQ(trajectory.getCurrentVelocity(), 0.f); + EXPECT_FLOAT_EQ(trajectory.getCurrentPosition(), 0.f); +} + +TEST(VelocitySmoothingBasicTest, DivisionByZeroComputeT1) +{ + // GIVEN: A VelocitySmoothing instance with zero maximum jerk and acceleration + VelocitySmoothing trajectory; + trajectory.setMaxJerk(0.f); + trajectory.setMaxAccel(0.f); + trajectory.setMaxVel(1.f); + + // WHEN: We update it + trajectory.updateDurations(1.f); + trajectory.updateTraj(1.f); + + // THEN: All the trajectories should still be zero + EXPECT_FLOAT_EQ(trajectory.getTotalTime(), 0.f); + EXPECT_FLOAT_EQ(trajectory.getCurrentJerk(), 0.f); + EXPECT_FLOAT_EQ(trajectory.getCurrentAcceleration(), 0.f); + EXPECT_FLOAT_EQ(trajectory.getCurrentVelocity(), 0.f); + EXPECT_FLOAT_EQ(trajectory.getCurrentPosition(), 0.f); +} + +TEST(VelocitySmoothingBasicTest, DivisionByZeroSaturateT1ForAccel) +{ + // GIVEN: A VelocitySmoothing instance with zero maximum jerk and acceleration + VelocitySmoothing trajectory(1.f, 0.f, 0.f); + trajectory.setMaxJerk(0.f); + trajectory.setMaxAccel(0.f); + trajectory.setMaxVel(1.f); + + // WHEN: We update it + trajectory.updateDurations(1.f); + trajectory.updateTraj(1.f); + + // THEN: All the trajectories should still be zero + EXPECT_FLOAT_EQ(trajectory.getTotalTime(), 0.f); + EXPECT_FLOAT_EQ(trajectory.getCurrentJerk(), 0.f); + EXPECT_FLOAT_EQ(trajectory.getCurrentAcceleration(), 0.f); + EXPECT_FLOAT_EQ(trajectory.getCurrentVelocity(), 0.f); + EXPECT_FLOAT_EQ(trajectory.getCurrentPosition(), 0.f); +} + +//TEST(Test, Blocker) { EXPECT_TRUE(false); } + class VelocitySmoothingTest : public ::testing::Test { public: @@ -174,7 +233,7 @@ TEST_F(VelocitySmoothingTest, testConstantSetpoint) updateTrajectories(dt, velocity_setpoints); } - // THEN: All the trajectories should have reach their + // THEN: All the trajectories should have reached their // final state: desired velocity target and zero acceleration for (int i = 0; i < 3; i++) { EXPECT_LE(fabsf(_trajectories[i].getCurrentVelocity() - velocity_setpoints(i)), 0.01f); diff --git a/src/lib/motion_planning/test_velocity_smoothing.cpp b/src/lib/motion_planning/test_velocity_smoothing.cpp deleted file mode 100644 index 65ef639d5c5e..000000000000 --- a/src/lib/motion_planning/test_velocity_smoothing.cpp +++ /dev/null @@ -1,99 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2018 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. - * - ****************************************************************************/ - -/** - * Test code for the Velocity Smoothing library - * Build and run using: make && ./test_velocity_smoothing - */ - -#include "VelocitySmoothing.hpp" -#include -#include - -int main(int argc, char *argv[]) -{ - VelocitySmoothing trajectory[3]; - - float a0[3] = {0.f, 0.f, 0.f}; - float v0[3] = {0.f, 0.f, 0.f}; - float x0[3] = {0.f, 0.f, 0.f}; - - float j_max = 55.2f; - float a_max = 6.f; - float v_max = 6.f; - - for (int i = 0; i < 3; i++) { - trajectory[i].setMaxJerk(j_max); - trajectory[i].setMaxAccel(a_max); - trajectory[i].setMaxVel(v_max); - trajectory[i].setCurrentAcceleration(a0[i]); - trajectory[i].setCurrentVelocity(v0[i]); - } - - const float dt = 0.01f; - - float velocity_setpoint[3] = {1.f, 0.f, -1.f}; - - for (int i = 0; i < 3; i++) { - trajectory[i].updateDurations(velocity_setpoint[i]); - } - - float t123 = trajectory[0].getTotalTime(); - int nb_steps = ceil(t123 / dt); - printf("Nb steps = %d\n", nb_steps); - - for (int i = 0; i < nb_steps; i++) { - for (int i = 0; i < 3; i++) { - trajectory[i].updateTraj(dt); - } - - for (int i = 0; i < 3; i++) { - trajectory[i].updateDurations(velocity_setpoint[i]); - } - - VelocitySmoothing::timeSynchronization(trajectory, 2); - - for (int i = 0; i < 1; i++) { - printf("Traj[%d]\n", i); - printf("jerk = %.3f\taccel = %.3f\tvel = %.3f\tpos = %.3f\n", - trajectory[i].getCurrentJerk(), - trajectory[i].getCurrentAcceleration(), - trajectory[i].getCurrentVelocity(), - trajectory[i].getCurrentPosition()); - printf("T1 = %.3f\tT2 = %.3f\tT3 = %.3f\n", trajectory[i].getT1(), trajectory[i].getT2(), trajectory[i].getT3()); - printf("\n"); - } - } - - return 0; -} diff --git a/src/modules/flight_mode_manager/tasks/Utility/Makefile b/src/modules/flight_mode_manager/tasks/Utility/Makefile deleted file mode 100644 index df018d73ab68..000000000000 --- a/src/modules/flight_mode_manager/tasks/Utility/Makefile +++ /dev/null @@ -1,12 +0,0 @@ - -.PHONY: all tests clean -all: test_velocity_smoothing - -test_velocity_smoothing: test_velocity_smoothing.cpp VelocitySmoothing.cpp - @g++ $^ -std=c++11 -I ../../../ -o $@ - -tests: test_velocity_smoothing - @echo "Test velocity smoothing" - -clean: - @rm test_velocity_smoothing diff --git a/src/modules/mc_pos_control/GotoControl/GotoControl.cpp b/src/modules/mc_pos_control/GotoControl/GotoControl.cpp index dd05e635a3b5..559b93e73a97 100644 --- a/src/modules/mc_pos_control/GotoControl/GotoControl.cpp +++ b/src/modules/mc_pos_control/GotoControl/GotoControl.cpp @@ -147,7 +147,7 @@ void GotoControl::setPositionSmootherLimits(const goto_setpoint_s &goto_setpoint if (_position_smoother.getCurrentVelocityXY().norm() <= max_horizontal_speed) { const float speed_scale = max_horizontal_speed / _vehicle_constraints.max_horizontal_speed; max_horizontal_accel = math::constrain(_vehicle_constraints.max_horizontal_accel * speed_scale, - VelocitySmoothing::kMinAccel, + 0.f, _vehicle_constraints.max_horizontal_accel); } } @@ -175,7 +175,7 @@ void GotoControl::setPositionSmootherLimits(const goto_setpoint_s &goto_setpoint // only limit acceleration once within velocity constraints if (fabsf(_position_smoother.getCurrentVelocityZ()) <= max_vertical_speed) { const float speed_scale = max_vertical_speed / vehicle_max_vertical_speed; - max_vertical_accel = math::constrain(vehicle_max_vertical_accel * speed_scale, VelocitySmoothing::kMinAccel, + max_vertical_accel = math::constrain(vehicle_max_vertical_accel * speed_scale, 0.f, vehicle_max_vertical_accel); } } diff --git a/src/modules/mc_pos_control/GotoControl/GotoControl.hpp b/src/modules/mc_pos_control/GotoControl/GotoControl.hpp index ee9d4d2a9ca6..d771404d79bd 100644 --- a/src/modules/mc_pos_control/GotoControl/GotoControl.hpp +++ b/src/modules/mc_pos_control/GotoControl/GotoControl.hpp @@ -43,9 +43,10 @@ #pragma once -#include -#include #include +#include +#include +#include #include #include @@ -63,10 +64,10 @@ class GotoControl float max_horizontal_speed = kMinSpeed; // [m/s] float max_down_speed = kMinSpeed; // [m/s] float max_up_speed = kMinSpeed; // [m/s] - float max_horizontal_accel = VelocitySmoothing::kMinAccel; // [m/s^2] - float max_down_accel = VelocitySmoothing::kMinAccel; // [m/s^2] - float max_up_accel = VelocitySmoothing::kMinAccel; // [m/s^2] - float max_jerk = VelocitySmoothing::kMinJerk; // [m/s^3] + float max_horizontal_accel = 0.f; // [m/s^2] + float max_down_accel = 0.f; // [m/s^2] + float max_up_accel = 0.f; // [m/s^2] + float max_jerk = 0.f; // [m/s^3] float max_heading_rate = HeadingSmoother::kMinHeadingRate; // [rad/s] float max_heading_accel = HeadingSmoother::kMinHeadingAccel; // [rad/s^2] }; @@ -109,11 +110,11 @@ class GotoControl _vehicle_constraints.max_horizontal_speed = math::max(kMinSpeed, vehicle_constraints.max_horizontal_speed); _vehicle_constraints.max_down_speed = math::max(kMinSpeed, vehicle_constraints.max_down_speed); _vehicle_constraints.max_up_speed = math::max(kMinSpeed, vehicle_constraints.max_up_speed); - _vehicle_constraints.max_horizontal_accel = math::max(VelocitySmoothing::kMinAccel, + _vehicle_constraints.max_horizontal_accel = math::max(0.f, vehicle_constraints.max_horizontal_accel); - _vehicle_constraints.max_down_accel = math::max(VelocitySmoothing::kMinAccel, vehicle_constraints.max_down_accel); - _vehicle_constraints.max_up_accel = math::max(VelocitySmoothing::kMinAccel, vehicle_constraints.max_up_accel); - _vehicle_constraints.max_jerk = math::max(VelocitySmoothing::kMinJerk, vehicle_constraints.max_jerk); + _vehicle_constraints.max_down_accel = math::max(0.f, vehicle_constraints.max_down_accel); + _vehicle_constraints.max_up_accel = math::max(0.f, vehicle_constraints.max_up_accel); + _vehicle_constraints.max_jerk = math::max(0.f, vehicle_constraints.max_jerk); _vehicle_constraints.max_heading_rate = math::max(HeadingSmoother::kMinHeadingRate, vehicle_constraints.max_heading_rate); _vehicle_constraints.max_heading_accel = math::max(HeadingSmoother::kMinHeadingAccel,