diff --git a/CMakeLists.txt b/CMakeLists.txt index 27153211..bb137b63 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -64,8 +64,10 @@ add_library(${PROJECT_NAME} src/${PROJECT_NAME}_local_planner.cpp src/controller.cpp src/calculations.cpp - src/visualization.cpp + src/details/derivative.cpp + src/details/integral.cpp src/details/second_order_lowpass.cpp + src/visualization.cpp ) add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) @@ -108,8 +110,12 @@ install( if(CATKIN_ENABLE_TESTING) add_rostest(test/test_path_tracking_pid.test ARGS rviz:=false reconfigure:=false) catkin_add_gtest(unittests - test/unittests/test_main.cpp + test/unittests/test_calculations.cpp + test/unittests/test_derivative.cpp test/unittests/test_fifo_array.cpp - test/unittests/test_calculations.cpp) + test/unittests/test_integral.cpp + test/unittests/test_main.cpp + test/unittests/test_second_order_lowpass.cpp + ) target_link_libraries(unittests ${catkin_LIBRARIES} ${PROJECT_NAME}) endif() diff --git a/cfg/Pid.cfg b/cfg/Pid.cfg index 69c031cb..46607c36 100755 --- a/cfg/Pid.cfg +++ b/cfg/Pid.cfg @@ -2,6 +2,7 @@ PACKAGE = "path_tracking_pid" from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t, double_t, int_t +from math import sqrt gen = ParameterGenerator() @@ -33,6 +34,9 @@ gen.add("Kp_ang", double_t, 0, "Kp Angular", 1, 0, 10) gen.add("Ki_ang", double_t, 0, "Ki Angular", 0, 0, 2) gen.add("Kd_ang", double_t, 0, "Kd Angular", 0.3, 0, 10) +gen.add("lowpass_cutoff", double_t, 0, "Lowpass cutoff (Hz), 0 disables the filter", 0, 0, 1000) +gen.add("lowpass_damping", double_t, 0, "Lowpass damping", sqrt(2), 0, 10) + gen.add("feedback_lat", bool_t, 0, "Enable lateral feedback?", True) gen.add("feedback_ang", bool_t, 0, "Enable angular feedback?", False) diff --git a/doc/integral_tustin.ipynb b/doc/integral_tustin.ipynb new file mode 100644 index 00000000..27538c95 --- /dev/null +++ b/doc/integral_tustin.ipynb @@ -0,0 +1,130 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# How to discretize a integral filter with Tustin's method\n", + "First a continous time filter is constructed. This filter will be discretized with Tustin's method and converted into C++ code." + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": {}, + "outputs": [], + "source": [ + "from sympy import *" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [], + "source": [ + "s, T, z = symbols('s,T,z')" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "First our continous time system" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [], + "source": [ + "sys = 1 / s" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Translate to discrete" + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": {}, + "outputs": [ + { + "data": { + "text/latex": [ + "$\\displaystyle \\frac{T \\left(z + 1\\right)}{2 \\left(z - 1\\right)}$" + ], + "text/plain": [ + "T*(z + 1)/(2*(z - 1))" + ] + }, + "execution_count": 4, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "sys = sys.subs(s, 2 / T * (z - 1) / (z + 1))\n", + "sys" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Translate that to C++" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "```\n", + "H = y/u = T / 2 * (z + 1)/(z - 1)\n", + "\n", + "u * T / 2 * (z + 1) = y * (z - 1)\n", + "u * T / 2 * (1 + z^-1) = y * (1 - z^-1)\n", + "T / 2 * (u[0] + u[1]) = y[0] - y[1]\n", + "y[0] = T / 2 * (u[0] + u[1]) + y[1]\n", + "```\n", + "\n", + "```c++\n", + "y[0] = T / 2 * (u[0] + u[1]) + y[1]\n", + "```" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.8.10" + } + }, + "nbformat": 4, + "nbformat_minor": 4 +} diff --git a/doc/second_order_lowpass_tustin.ipynb b/doc/second_order_lowpass_tustin.ipynb new file mode 100644 index 00000000..9ef5eca0 --- /dev/null +++ b/doc/second_order_lowpass_tustin.ipynb @@ -0,0 +1,275 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# How to discretize a second order lowpass with Tustin's method\n", + "First a continous time filter is constructed. This filter will be discretized with Tustin's method and converted into C++ code." + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": {}, + "outputs": [], + "source": [ + "from sympy import *" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [], + "source": [ + "a, s, d, T, z = symbols('a,s,d,T,z')" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "First our continous time system" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [], + "source": [ + "# a = 2*pi*c\n", + "sys = 1 / (1/a**2 * s**2 + 2*d/a * s + 1)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Translate to discrete" + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": {}, + "outputs": [ + { + "data": { + "text/latex": [ + "$\\displaystyle \\frac{1}{1 + \\frac{4 d \\left(z - 1\\right)}{T a \\left(z + 1\\right)} + \\frac{4 \\left(z - 1\\right)^{2}}{T^{2} a^{2} \\left(z + 1\\right)^{2}}}$" + ], + "text/plain": [ + "1/(1 + 4*d*(z - 1)/(T*a*(z + 1)) + 4*(z - 1)**2/(T**2*a**2*(z + 1)**2))" + ] + }, + "execution_count": 4, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "sys = sys.subs(s, 2 / T * (z - 1) / (z + 1))\n", + "sys" + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "(T**2*a**2*z**2 + 2*T**2*a**2*z + T**2*a**2)/(T**2*a**2*z**2 + 2*T**2*a**2*z + T**2*a**2 + 4*T*a*d*z**2 - 4*T*a*d + 4*z**2 - 8*z + 4)\n" + ] + }, + { + "data": { + "text/latex": [ + "$\\displaystyle \\frac{T^{2} a^{2} z^{2} + 2 T^{2} a^{2} z + T^{2} a^{2}}{T^{2} a^{2} z^{2} + 2 T^{2} a^{2} z + T^{2} a^{2} + 4 T a d z^{2} - 4 T a d + 4 z^{2} - 8 z + 4}$" + ], + "text/plain": [ + "(T**2*a**2*z**2 + 2*T**2*a**2*z + T**2*a**2)/(T**2*a**2*z**2 + 2*T**2*a**2*z + T**2*a**2 + 4*T*a*d*z**2 - 4*T*a*d + 4*z**2 - 8*z + 4)" + ] + }, + "execution_count": 5, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "print(cancel(sys))\n", + "cancel(sys)" + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "[T**2*a**2, 2*T**2*a**2, T**2*a**2]" + ] + }, + "execution_count": 6, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "num = Poly(T**2*a**2*z**2 + 2*T**2*a**2*z + T**2*a**2, z)\n", + "num.all_coeffs()" + ] + }, + { + "cell_type": "code", + "execution_count": 7, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "[T**2*a**2 + 4*T*a*d + 4, 2*T**2*a**2 - 8, T**2*a**2 - 4*T*a*d + 4]" + ] + }, + "execution_count": 7, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "den = Poly(T**2*a**2*z**2 + 2*T**2*a**2*z + T**2*a**2 + 4*T*a*d*z**2 - 4*T*a*d + 4*z**2 - 8*z + 4, z)\n", + "den.all_coeffs()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Try to simplify" + ] + }, + { + "cell_type": "code", + "execution_count": 8, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "(b**2*z**2 + 2*b**2*z + b**2)/(b**2*z**2 + 2*b**2*z + b**2 + 4*b*d*z**2 - 4*b*d + 4*z**2 - 8*z + 4)\n" + ] + }, + { + "data": { + "text/latex": [ + "$\\displaystyle \\frac{b^{2} z^{2} + 2 b^{2} z + b^{2}}{b^{2} z^{2} + 2 b^{2} z + b^{2} + 4 b d z^{2} - 4 b d + 4 z^{2} - 8 z + 4}$" + ], + "text/plain": [ + "(b**2*z**2 + 2*b**2*z + b**2)/(b**2*z**2 + 2*b**2*z + b**2 + 4*b*d*z**2 - 4*b*d + 4*z**2 - 8*z + 4)" + ] + }, + "execution_count": 8, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "b = symbols('b')\n", + "# a*T = b -> T = b/a\n", + "sys = sys.subs(T, b/a)\n", + "print(cancel(sys))\n", + "cancel(sys)" + ] + }, + { + "cell_type": "code", + "execution_count": 9, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "[b**2, 2*b**2, b**2]" + ] + }, + "execution_count": 9, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "num = Poly(b**2*z**2 + 2*b**2*z + b**2, z)\n", + "num.all_coeffs()" + ] + }, + { + "cell_type": "code", + "execution_count": 10, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "[b**2 + 4*b*d + 4, 2*b**2 - 8, b**2 - 4*b*d + 4]" + ] + }, + "execution_count": 10, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "den = Poly(b**2*z**2 + 2*b**2*z + b**2 + 4*b*d*z**2 - 4*b*d + 4*z**2 - 8*z + 4, z)\n", + "den.all_coeffs()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Translate that to C++" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "```c++\n", + "auto a = 2 * M_PI * c;\n", + "auto b = T * a;\n", + "y_[0] = ((pow(b, 2)) * u_[0] + (2 * pow(b, 2)) * u_[1] + (pow(b, 2)) * u_[2] -\n", + " (2 * pow(b, 2) - 8) * y_[1] - (pow(b, 2) - 4 * T * a * d + 4) * y_[2]) /\n", + " (pow(b, 2) + 4 * T * a * d + 4);\n", + "```" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.8.10" + } + }, + "nbformat": 4, + "nbformat_minor": 4 +} diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index b36312cd..9cdb9e4d 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -9,7 +9,9 @@ #include #include +#include #include +#include #include #include @@ -34,15 +36,15 @@ struct ControllerState double previous_steering_yaw_vel = 0.0; bool end_phase_enabled = false; bool end_reached = false; - double error_integral_lat = 0.0; - double error_integral_ang = 0.0; double tracking_error_lat = 0.0; double tracking_error_ang = 0.0; // Errors with little history details::SecondOrderLowpass error_lat; - details::SecondOrderLowpass error_deriv_lat; details::SecondOrderLowpass error_ang; - details::SecondOrderLowpass error_deriv_ang; + details::Integral error_integral_lat; + details::Integral error_integral_ang; + details::Derivative error_deriv_lat; + details::Derivative error_deriv_ang; }; class Controller : private boost::noncopyable diff --git a/include/path_tracking_pid/details/derivative.hpp b/include/path_tracking_pid/details/derivative.hpp new file mode 100644 index 00000000..a4df7e07 --- /dev/null +++ b/include/path_tracking_pid/details/derivative.hpp @@ -0,0 +1,30 @@ +#pragma once + +#include + +namespace path_tracking_pid::details +{ +/** + * @brief Discrete time derivative filter + */ +class Derivative +{ +public: + /** + * @brief Filter one sample of a signal + * @param u Signal to be filtered + * @param step_size Time step from previous sample + * @return Derivative of the signal + */ + double filter(double u, double step_size); + + /** + * @brief Reset the signal buffers + */ + void reset(); + +private: + FifoArray u_ = {}; +}; + +} // namespace path_tracking_pid::details diff --git a/include/path_tracking_pid/details/fifo_array.hpp b/include/path_tracking_pid/details/fifo_array.hpp index 9ded1e1f..965499e0 100644 --- a/include/path_tracking_pid/details/fifo_array.hpp +++ b/include/path_tracking_pid/details/fifo_array.hpp @@ -24,6 +24,9 @@ class FifoArray // Read-only access to the element at the given index. constexpr const value_type & operator[](std::size_t index) const { return data_[index]; } + // Read-write access to the element at the given index. + value_type & operator[](std::size_t index) { return data_[index]; } + // Read-only access to the element at the given index (with compile-time range check). template constexpr const value_type & at() const diff --git a/include/path_tracking_pid/details/integral.hpp b/include/path_tracking_pid/details/integral.hpp new file mode 100644 index 00000000..cc0e6bf4 --- /dev/null +++ b/include/path_tracking_pid/details/integral.hpp @@ -0,0 +1,48 @@ +#pragma once + +#include + +namespace path_tracking_pid::details +{ +/** + * @brief Discrete time integral filter + */ +class Integral +{ + constexpr static auto NaN = std::numeric_limits::quiet_NaN(); + +public: + Integral() = default; + + /** + * @brief Construct an integral filter + * @param windup_limit Integral windup limit + */ + explicit Integral(double windup_limit); + + /** + * @brief Change the parameters of the filter + * @param windup_limit Integral windup limit + */ + void configure(double windup_limit); + + /** + * @brief Filter one sample of a signal + * @param u Signal to be filtered + * @param step_size Time step from previous sample + * @return Integral of the signal + */ + double filter(double u, double step_size); + + /** + * @brief Reset the signal buffers + */ + void reset(); + +private: + FifoArray u_ = {}; + FifoArray y_ = {}; + double windup_limit_ = NaN; +}; + +} // namespace path_tracking_pid::details diff --git a/include/path_tracking_pid/details/second_order_lowpass.hpp b/include/path_tracking_pid/details/second_order_lowpass.hpp index 857dadcb..bbf873f5 100644 --- a/include/path_tracking_pid/details/second_order_lowpass.hpp +++ b/include/path_tracking_pid/details/second_order_lowpass.hpp @@ -1,29 +1,55 @@ #pragma once +#include #include namespace path_tracking_pid::details { -// Error tracker for the last 3 error and filtered error values. +/** + * @brief Discrete time second order lowpass filter + */ class SecondOrderLowpass { + constexpr static auto NaN = std::numeric_limits::quiet_NaN(); + public: - // Pushes the given value to the errors FIFO buffer. A corresponding filtered error value is calculated and pushed - // to the filtered errors FIFO buffer. - void push(double value); + /** + * @brief Construct a SecondOrderLowpass instance with NaNs + */ + SecondOrderLowpass() = default; - // Resets both errors and filtered errors FIFO buffers. - void reset(); + /** + * @brief Construct a SecondOrderLowpass instance + * @param cutoff frequency in Hz, 0 disables the filter + * @param damping frequency in Hz + */ + SecondOrderLowpass(double cutoff, double damping); - // Read-only access to the errors FIFO buffer. - const FifoArray & errors() const; + /** + * @brief Change the parameters of the filter + * @param cutoff frequency in Hz, 0 disables the filter + * @param damping frequency in Hz + */ + void configure(double cutoff, double damping); - // Read-only access to the filtered errors FIFO buffer. - const FifoArray & filtered_errors() const; + /** + * @brief Filter one sample of a signal + * @param u Signal to be filtered + * @param step_size Time step from previous sample + * @return Lowpass-filtered signal + */ + double filter(double u, double step_size); + + /** + * @brief Reset the signal buffers + */ + void reset(); private: - FifoArray errors_; - FifoArray filtered_errors_; + FifoArray u_ = {}; + FifoArray y_ = {}; + double cutoff_ = NaN; + double damping_ = NaN; }; } // namespace path_tracking_pid::details diff --git a/param/controllers.yaml b/param/controllers.yaml index 5d563b7c..3fe1be86 100644 --- a/param/controllers.yaml +++ b/param/controllers.yaml @@ -15,4 +15,5 @@ PathTrackingPID: abs_minimum_x_vel: 0.0 anti_collision: true use_mpc: false + feedforward_ang: true controller_debug_enabled: true diff --git a/src/controller.cpp b/src/controller.cpp index f94396a5..7912d888 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -399,8 +399,9 @@ Controller::UpdateResult Controller::update( ROS_WARN("All three gains (Kp, Ki, Kd) should have the same sign for stability."); } - controller_state_.error_lat.push(error.getOrigin().y()); - controller_state_.error_ang.push(angles::normalize_angle(tf2::getYaw(error.getRotation()))); + auto error_lat_filtered = controller_state_.error_lat.filter(error.getOrigin().y(), dt.toSec()); + auto error_ang_filtered = controller_state_.error_ang.filter( + angles::normalize_angle(tf2::getYaw(error.getRotation())), dt.toSec()); // tracking error for diagnostic purposes // Transform current pose into local-path-frame to get tracked-frame-error @@ -422,38 +423,27 @@ Controller::UpdateResult Controller::update( // trackin_error here represents the error between tracked link and position on plan controller_state_.tracking_error_lat = current_tracking_err.y(); - controller_state_.tracking_error_ang = controller_state_.error_ang.errors().at<0>(); + controller_state_.tracking_error_ang = angles::normalize_angle(tf2::getYaw(error.getRotation())), + dt.toSec(); // integrate the error - controller_state_.error_integral_lat += controller_state_.error_lat.errors().at<0>() * dt.toSec(); - controller_state_.error_integral_ang += controller_state_.error_ang.errors().at<0>() * dt.toSec(); - - // Apply windup limit to limit the size of the integral term - controller_state_.error_integral_lat = - std::clamp(controller_state_.error_integral_lat, -windup_limit, windup_limit); - controller_state_.error_integral_ang = - std::clamp(controller_state_.error_integral_ang, -windup_limit, windup_limit); + auto error_integral_lat = + controller_state_.error_integral_lat.filter(error_lat_filtered, dt.toSec()); + auto error_integral_ang = + controller_state_.error_integral_ang.filter(error_lat_filtered, dt.toSec()); // Take derivative of error, first the raw unfiltered data: - controller_state_.error_deriv_lat.push( - (controller_state_.error_lat.errors().at<0>() - controller_state_.error_lat.errors().at<1>()) / - dt.toSec()); - controller_state_.error_deriv_ang.push( - (controller_state_.error_ang.errors().at<0>() - controller_state_.error_ang.errors().at<1>()) / - dt.toSec()); + auto error_deriv_lat = controller_state_.error_deriv_lat.filter(error_lat_filtered, dt.toSec()); + auto error_deriv_ang = controller_state_.error_deriv_ang.filter(error_ang_filtered, dt.toSec()); // calculate the control effort - const auto proportional_lat = - config_.Kp_lat * controller_state_.error_lat.filtered_errors().at<0>(); - const auto integral_lat = config_.Ki_lat * controller_state_.error_integral_lat; - const auto derivative_lat = - config_.Kd_lat * controller_state_.error_deriv_lat.filtered_errors().at<0>(); - - const auto proportional_ang = - config_.Kp_ang * controller_state_.error_ang.filtered_errors().at<0>(); - const auto integral_ang = config_.Ki_ang * controller_state_.error_integral_ang; - const auto derivative_ang = - config_.Kd_ang * controller_state_.error_deriv_ang.filtered_errors().at<0>(); + const auto proportional_lat = config_.Kp_lat * error_lat_filtered; + const auto integral_lat = config_.Ki_lat * error_integral_lat; + const auto derivative_lat = config_.Kd_lat * error_deriv_lat; + + const auto proportional_ang = config_.Kp_ang * error_ang_filtered; + const auto integral_ang = config_.Ki_ang * error_integral_ang; + const auto derivative_ang = config_.Kd_ang * error_deriv_ang; /***** Compute forward velocity *****/ // Apply acceleration limits and end velocity @@ -615,8 +605,8 @@ Controller::UpdateResult Controller::update( // Populate debug output // Error topic containing the 'control' error on which the PID acts result.pid_debug.control_error.linear.x = 0.0; - result.pid_debug.control_error.linear.y = controller_state_.error_lat.errors().at<0>(); - result.pid_debug.control_error.angular.z = controller_state_.error_ang.errors().at<0>(); + result.pid_debug.control_error.linear.y = error_lat_filtered; + result.pid_debug.control_error.angular.z = error_ang_filtered; // Error topic containing the 'tracking' error, i.e. the real error between path and tracked link result.pid_debug.tracking_error.linear.x = 0.0; result.pid_debug.tracking_error.linear.y = controller_state_.tracking_error_lat; @@ -734,8 +724,8 @@ Controller::UpdateResult Controller::update( // Publish control effort if controller enabled if (!enabled_) // Do nothing reset integral action and all filters { - controller_state_.error_integral_lat = 0.0; - controller_state_.error_integral_ang = 0.0; + controller_state_.error_integral_lat.reset(); + controller_state_.error_integral_ang.reset(); } controller_state_.current_x_vel = new_x_vel; @@ -820,7 +810,7 @@ double Controller::mpc_based_max_vel( // max_target_x_vel we can increase it if ( mpc_fwd_iter == config_.mpc_max_fwd_iterations && - fabs(controller_state_.error_lat.errors().at<0>()) <= config_.mpc_max_error_lat && + fabs(controller_state_.tracking_error_lat) <= config_.mpc_max_error_lat && fabs(new_nominal_x_vel) < abs(target_x_vel)) { mpc_vel_optimization_iter += 1; @@ -843,7 +833,7 @@ double Controller::mpc_based_max_vel( mpc_fwd_iter = 0; } // If the robot gets out of bounds earlier we decrease the velocity - else if (abs(controller_state_.error_lat.errors().at<0>()) >= config_.mpc_max_error_lat) { + else if (abs(controller_state_.tracking_error_lat) >= config_.mpc_max_error_lat) { mpc_vel_optimization_iter += 1; // Lower speed @@ -927,6 +917,11 @@ void Controller::printParameters() const void Controller::configure(path_tracking_pid::PidConfig & config) { + controller_state_.error_lat.configure(config.lowpass_cutoff, config.lowpass_damping); + controller_state_.error_ang.configure(config.lowpass_cutoff, config.lowpass_damping); + controller_state_.error_integral_lat.configure(windup_limit); + controller_state_.error_integral_ang.configure(windup_limit); + // Erase all queues when config changes controller_state_.error_lat.reset(); controller_state_.error_deriv_lat.reset(); @@ -978,11 +973,11 @@ void Controller::reset() controller_state_.previous_steering_angle = 0.0; controller_state_.previous_steering_yaw_vel = 0.0; controller_state_.previous_steering_x_vel = 0.0; - controller_state_.error_integral_lat = 0.0; - controller_state_.error_integral_ang = 0.0; controller_state_.error_lat.reset(); - controller_state_.error_deriv_lat.reset(); controller_state_.error_ang.reset(); + controller_state_.error_integral_lat.reset(); + controller_state_.error_integral_ang.reset(); + controller_state_.error_deriv_lat.reset(); controller_state_.error_deriv_ang.reset(); } diff --git a/src/details/derivative.cpp b/src/details/derivative.cpp new file mode 100644 index 00000000..25dc4f63 --- /dev/null +++ b/src/details/derivative.cpp @@ -0,0 +1,13 @@ +#include + +namespace path_tracking_pid::details +{ +double Derivative::filter(double u, double step_size) +{ + // save history + u_.push(u); + return (u_[0] - u_[1]) / step_size; +} + +void Derivative::reset() { u_ = {}; } +} // namespace path_tracking_pid::details diff --git a/src/details/integral.cpp b/src/details/integral.cpp new file mode 100644 index 00000000..3b8997cb --- /dev/null +++ b/src/details/integral.cpp @@ -0,0 +1,28 @@ +#include + +namespace path_tracking_pid::details +{ +Integral::Integral(double windup_limit) : windup_limit_(windup_limit) {} + +void Integral::configure(double windup_limit) { windup_limit_ = windup_limit; } + +double Integral::filter(double u, double step_size) +{ + // save history + u_.push(u); + y_.push(u); // increase index so the math below looks correct + + // A continous time integrator was discretized with Tustin's method. For a mathematical + // explanation, see doc/integral_tustin.ipynb + auto T = step_size; + y_[0] = T / 2 * (u_[0] + u_[1]) + y_[1]; + y_[0] = std::clamp(y_[0], -windup_limit_, windup_limit_); + return y_[0]; +} + +void Integral::reset() +{ + u_ = {}; + y_ = {}; +} +} // namespace path_tracking_pid::details diff --git a/src/details/second_order_lowpass.cpp b/src/details/second_order_lowpass.cpp index 8614c5f0..3da54727 100644 --- a/src/details/second_order_lowpass.cpp +++ b/src/details/second_order_lowpass.cpp @@ -3,34 +3,43 @@ namespace path_tracking_pid::details { -namespace +SecondOrderLowpass::SecondOrderLowpass(double cutoff, double damping) +: cutoff_(cutoff), damping_(damping) { -// Used in filter calculations. Default 1.0 corresponds to a cutoff frequency at 1/4 of the sample rate. -constexpr double cutoff = 1.; - -} // namespace +} -void SecondOrderLowpass::push(double value) +void SecondOrderLowpass::configure(double cutoff, double damping) { - errors_.push(value); - - filtered_errors_.push( - (1 / (1 + cutoff * cutoff + M_SQRT2 * cutoff)) * - (errors_.at<2>() + 2 * errors_.at<1>() + errors_.at<0>() - - (cutoff * cutoff - M_SQRT2 * cutoff + 1) * filtered_errors_.at<1>() - - (-2 * cutoff * cutoff + 2) * filtered_errors_.at<0>())); + cutoff_ = cutoff; + damping_ = damping; } -void SecondOrderLowpass::reset() +double SecondOrderLowpass::filter(double u, double step_size) { - errors_.reset(); - filtered_errors_.reset(); + // save history + u_.push(u); + y_.push(u); // increase index so the math below looks correct + + if (cutoff_ == 0) { + return u; + } + + // A continous time second order lowpass was discretized with Tustin's method. For a mathematical + // explanation, see doc/second_order_lowpass_tustin.ipynb + auto c = cutoff_; + auto d = damping_; + auto T = step_size; + auto a = 2 * M_PI * c; + auto b = T * a; + y_[0] = ((pow(b, 2)) * u_[0] + (2 * pow(b, 2)) * u_[1] + (pow(b, 2)) * u_[2] - + (2 * pow(b, 2) - 8) * y_[1] - (pow(b, 2) - 4 * T * a * d + 4) * y_[2]) / + (pow(b, 2) + 4 * T * a * d + 4); + return y_[0]; } -const FifoArray & SecondOrderLowpass::errors() const { return errors_; } -const FifoArray & SecondOrderLowpass::filtered_errors() const +void SecondOrderLowpass::reset() { - return filtered_errors_; + u_ = {}; + y_ = {}; } - } // namespace path_tracking_pid::details diff --git a/test/test_path_tracking_pid.py b/test/test_path_tracking_pid.py index aa4efb8e..74ec6f68 100755 --- a/test/test_path_tracking_pid.py +++ b/test/test_path_tracking_pid.py @@ -104,7 +104,7 @@ def test_exepath_action(self): rospy.sleep(10.0) self.assertTrue(checker.slowed_down) - finished_in_time = client.wait_for_result(timeout=rospy.Duration(60)) + finished_in_time = client.wait_for_result(timeout=rospy.Duration(120)) self.assertTrue(finished_in_time, msg="Action call didn't return in time") self.assertEqual(client.get_state(), outcome_exp, msg="Wrong action outcome") diff --git a/test/unittests/test_derivative.cpp b/test/unittests/test_derivative.cpp new file mode 100644 index 00000000..23f033e5 --- /dev/null +++ b/test/unittests/test_derivative.cpp @@ -0,0 +1,34 @@ +#include + +#include +#include + +using path_tracking_pid::details::Derivative; + +constexpr double eps = 1e-6; + +TEST(Derivative, StepResponse) +{ + double dt = 0.1; + + Derivative filter; + + std::vector expected_response = {10, 0, 0}; + for (int i = 0; i < static_cast(expected_response.size()); ++i) { + SCOPED_TRACE(i); + + auto result = filter.filter(1, dt); + EXPECT_NEAR(result, expected_response[i], eps); + } +} + +TEST(Derivative, Reset) +{ + double dt = 0.1; + + Derivative filter; + + EXPECT_NEAR(filter.filter(1, dt), 10, eps); + filter.reset(); + EXPECT_NEAR(filter.filter(0, dt), 0, eps); +} diff --git a/test/unittests/test_fifo_array.cpp b/test/unittests/test_fifo_array.cpp index 42a1562b..bbf8cfdb 100644 --- a/test/unittests/test_fifo_array.cpp +++ b/test/unittests/test_fifo_array.cpp @@ -4,7 +4,6 @@ namespace { - using path_tracking_pid::details::FifoArray; TEST(PathTrackingPidDetailsFifoArray, Initialize) @@ -114,4 +113,14 @@ TEST(PathTrackingPidDetailsFifoArray, OtherSize) EXPECT_EQ(fifo[4], 2); } +TEST(PathTrackingPidDetailsFifoArray, Assign) +{ + FifoArray fifo; + fifo[0] = 1; + + EXPECT_EQ(fifo[0], 1); + EXPECT_EQ(fifo[1], 0); + EXPECT_EQ(fifo[2], 0); +} + } // namespace diff --git a/test/unittests/test_integral.cpp b/test/unittests/test_integral.cpp new file mode 100644 index 00000000..1032cfc9 --- /dev/null +++ b/test/unittests/test_integral.cpp @@ -0,0 +1,53 @@ +#include + +#include +#include + +using path_tracking_pid::details::Integral; + +constexpr double eps = 1e-6; + +TEST(Integral, StepResponse) +{ + double dt = 0.1; + double windup_limit = 0.5; + + Integral filter{windup_limit}; + + std::vector expected_response = {0.05, 0.15, 0.25, 0.35, 0.45, 0.5, 0.5}; + for (int i = 0; i < static_cast(expected_response.size()); ++i) { + SCOPED_TRACE(i); + + auto result = filter.filter(1, dt); + EXPECT_NEAR(result, expected_response[i], eps); + } +} + +TEST(Integral, Reset) +{ + double dt = 0.1; + double windup_limit = 0.5; + + Integral filter{windup_limit}; + + EXPECT_NEAR(filter.filter(1, dt), 0.05, eps); + EXPECT_NEAR(filter.filter(1, dt), 0.15, eps); + filter.reset(); + EXPECT_NEAR(filter.filter(1, dt), 0.05, eps); + EXPECT_NEAR(filter.filter(1, dt), 0.15, eps); +} + +TEST(Integral, Configure) +{ + double dt = 0.1; + double windup_limit = 0.2; + + Integral filter{windup_limit}; + + EXPECT_NEAR(filter.filter(1, dt), 0.05, eps); + EXPECT_NEAR(filter.filter(1, dt), 0.15, eps); + EXPECT_NEAR(filter.filter(1, dt), 0.20, eps); + filter.configure(0.35); + EXPECT_NEAR(filter.filter(1, dt), 0.30, eps); + EXPECT_NEAR(filter.filter(1, dt), 0.35, eps); +} diff --git a/test/unittests/test_second_order_lowpass.cpp b/test/unittests/test_second_order_lowpass.cpp new file mode 100644 index 00000000..93d23ea8 --- /dev/null +++ b/test/unittests/test_second_order_lowpass.cpp @@ -0,0 +1,133 @@ +#include + +#include +#include +#include + +using path_tracking_pid::details::SecondOrderLowpass; + +constexpr double eps = 1e-6; + +TEST(SecondOrderLowpass, StepResponse) +{ + double dt = 0.1; + double cutoff = 1 / dt / 4; + double damping = sqrt(2); + + SecondOrderLowpass filter(cutoff, damping); + + std::vector expected_response = {0.16071, 0.514214, 0.770813, 0.877725, 0.939488, + 0.968659, 0.984211, 0.991911, 0.995898, 0.997907}; + for (int i = 0; i < static_cast(expected_response.size()); ++i) { + SCOPED_TRACE(i); + + auto result = filter.filter(1, dt); + EXPECT_NEAR(result, expected_response[i], eps); + } +} + +TEST(SecondOrderLowpass, Disable) +{ + double dt = 0.1; + double cutoff = 0; + double damping = sqrt(2); + + SecondOrderLowpass filter(cutoff, damping); + EXPECT_NEAR(filter.filter(0, dt), 0, eps); + EXPECT_NEAR(filter.filter(1, dt), 1, eps); + EXPECT_NEAR(filter.filter(5, dt), 5, eps); +} + +TEST(SecondOrderLowpass, StepResponseCutoff) +{ + double dt = 0.1; + double cutoff = 1 / dt / 8; // lower cutoff so slower response + double damping = sqrt(2); + + SecondOrderLowpass filter(cutoff, damping); + + std::vector expected_response = { + 0.068087, 0.255112, 0.461572, 0.612177, 0.720691, + 0.798844, 0.855129, 0.895665, 0.924859, 0.945884, + }; + for (int i = 0; i < static_cast(expected_response.size()); ++i) { + SCOPED_TRACE(i); + + auto result = filter.filter(1, dt); + EXPECT_NEAR(result, expected_response[i], eps); + } +} + +TEST(SecondOrderLowpass, StepResponseDamping) +{ + double dt = 0.1; + double cutoff = 1 / dt / 4; + double damping = sqrt(2) * 2; // more damping + + SecondOrderLowpass filter(cutoff, damping); + + std::vector expected_response = { + 0.101795, 0.318258, 0.494899, 0.618187, 0.716157, + 0.786043, 0.84057, 0.880057, 0.91048, 0.932743, + }; + for (int i = 0; i < static_cast(expected_response.size()); ++i) { + SCOPED_TRACE(i); + + auto result = filter.filter(1, dt); + EXPECT_NEAR(result, expected_response[i], eps); + } +} + +TEST(SecondOrderLowpass, Reset) +{ + double dt = 0.1; + double cutoff = 1 / dt / 4; + double damping = sqrt(2); + + SecondOrderLowpass filter(cutoff, damping); + + std::vector expected_response = {0.16071, 0.514214, 0.770813, 0.877725, 0.939488}; + for (int i = 0; i < static_cast(expected_response.size()); ++i) { + SCOPED_TRACE(i); + + auto result = filter.filter(1, dt); + EXPECT_NEAR(result, expected_response[i], eps); + } + + filter.reset(); + + for (int i = 0; i < static_cast(expected_response.size()); ++i) { + SCOPED_TRACE(i); + + auto result = filter.filter(1, dt); + EXPECT_NEAR(result, expected_response[i], eps); + } +} + +TEST(SecondOrderLowpass, Configure) +{ + double dt = 0.1; + double cutoff = 1 / dt / 4; + double damping = sqrt(2); + + SecondOrderLowpass filter(cutoff, damping); + + std::vector expected_response = {0.16071, 0.514214, 0.770813, 0.877725, 0.939488}; + for (int i = 0; i < static_cast(expected_response.size()); ++i) { + SCOPED_TRACE(i); + + auto result = filter.filter(1, dt); + EXPECT_NEAR(result, expected_response[i], eps); + } + + filter.configure(cutoff / 2, damping); + + // no configure step response is {0.968659, 0.984211, 0.991911, 0.995898, 0.997907} + expected_response = {0.957154, 0.969162, 0.977792, 0.984006, 0.988481}; + for (int i = 0; i < static_cast(expected_response.size()); ++i) { + SCOPED_TRACE(i); + + auto result = filter.filter(1, dt); + EXPECT_NEAR(result, expected_response[i], eps); + } +}