From ab3bd3a7a2daf786e2e13d8ad71b405545d46fd4 Mon Sep 17 00:00:00 2001 From: Henrik Date: Wed, 24 Sep 2025 13:41:00 +0200 Subject: [PATCH 001/128] added Node, publisher, launch file, config file --- control/velocity_controller/CMakeLists.txt | 44 ++++++++++++++ .../config/parameters.yaml | 6 ++ .../launch/velocity_controller.launch.py | 29 +++++++++ control/velocity_controller/package.xml | 22 +++++++ .../src/velocity_controller.cpp | 59 +++++++++++++++++++ 5 files changed, 160 insertions(+) create mode 100644 control/velocity_controller/CMakeLists.txt create mode 100644 control/velocity_controller/config/parameters.yaml create mode 100644 control/velocity_controller/launch/velocity_controller.launch.py create mode 100644 control/velocity_controller/package.xml create mode 100644 control/velocity_controller/src/velocity_controller.cpp diff --git a/control/velocity_controller/CMakeLists.txt b/control/velocity_controller/CMakeLists.txt new file mode 100644 index 000000000..a905a4d09 --- /dev/null +++ b/control/velocity_controller/CMakeLists.txt @@ -0,0 +1,44 @@ +cmake_minimum_required(VERSION 3.8) +project(velocity_controller) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(vortex_msgs REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() + +add_executable(velocity_controller_node src/velocity_controller.cpp) + +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME}/ +) +install(TARGETS velocity_controller_node + DESTINATION lib/${PROJECT_NAME} +) +install(DIRECTORY config + DESTINATION share/${PROJECT_NAME}/ +) + +ament_target_dependencies(velocity_controller_node + rclcpp + std_msgs + vortex_msgs +) diff --git a/control/velocity_controller/config/parameters.yaml b/control/velocity_controller/config/parameters.yaml new file mode 100644 index 000000000..e6c322a0b --- /dev/null +++ b/control/velocity_controller/config/parameters.yaml @@ -0,0 +1,6 @@ +velocity_controller_node: # Name of the node usually goes here. We use wildcard so that we can change the name from the launch file. + ros__parameters: + topic_info_out: "velocity_out_topic" # parameter name: parameter value + topic_w_in: "reference" #reference speed and direction + topic_v_in: "current_velocity" #current speed + #publish_value: "Hello from config!" # parameter name: parameter value diff --git a/control/velocity_controller/launch/velocity_controller.launch.py b/control/velocity_controller/launch/velocity_controller.launch.py new file mode 100644 index 000000000..148a697a0 --- /dev/null +++ b/control/velocity_controller/launch/velocity_controller.launch.py @@ -0,0 +1,29 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +import os +#from launch.actions import IncludeLaunchDescription +from launch.actions import DeclareLaunchArgument +#from launch.launch_description_sources import PythonLaunchDescriptionSource +from ament_index_python.packages import get_package_share_directory +from launch.substitutions import LaunchConfiguration + +def generate_launch_description(): + pkg_share = get_package_share_directory('velocity_controller') + config_path = os.path.join(pkg_share, 'config', 'parameters.yaml') + + node_name_arg = DeclareLaunchArgument( + 'node_name', default_value='velocity_controller_node', + description='Name of the velocity controller node' + ) + + velocity_controller_name = LaunchConfiguration('node_name') + + + return LaunchDescription([ + node_name_arg, + Node(package='velocity_controller', + executable='velocity_controller_node', + name=velocity_controller_name, + output='screen', + parameters=[config_path]) + ]) \ No newline at end of file diff --git a/control/velocity_controller/package.xml b/control/velocity_controller/package.xml new file mode 100644 index 000000000..713d2d8ff --- /dev/null +++ b/control/velocity_controller/package.xml @@ -0,0 +1,22 @@ + + + + velocity_controller + 0.0.0 + TODO: Package description + henrik + TODO: License declaration + + ament_cmake + + rclcpp + std_msgs + vortex_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp new file mode 100644 index 000000000..1fc740a82 --- /dev/null +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -0,0 +1,59 @@ +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" +//#include "vortex-msgs/msg" kan legge til nye meldinger nå + +//Lager en klasse velocity node +class Velocity_node : public rclcpp::Node +{ +public: +//Konstruktør + Velocity_node() : Node("velocity_controller_node") + { + //Dytter info til log + RCLCPP_INFO(this->get_logger(), "Velocity control node has been started."); + + //Parameter from config. !!Needs to create launch file to prevent writing where to get the parameters from + this->declare_parameter("topic_info_out"); + this->declare_parameter("topic_w_in"); + info_out_topic = this->get_parameter("topic_info_out").as_string(); + reference_topic=this->get_parameter("topic_w_in").as_string(); + + // Lager en publisher som publisher på topic, velocity topic, 10 i "sikkerhet" + publisher_ = create_publisher(info_out_topic, 10); + //Lager en timer som kaller funksjonen timer_callback hvert 500ms + timer_ = this->create_wall_timer( + std::chrono::milliseconds(500), + std::bind(&Velocity_node::send_velocity, this)); + } + + +//Alle funksjonens variabler +//Publisher og timer instansene + rclcpp::Publisher::SharedPtr publisher_; + rclcpp::TimerBase::SharedPtr timer_; + + std::string info_out_topic; + std::string reference_topic; + + +//Utdata på topic + void send_velocity() + { + auto message = std_msgs::msg::String(); + message.data = "Velocity command"; + //RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str()); + publisher_->publish(message); + } + + + +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} + From 23fea72229b0a4cdec8cea9e76390c43432b3d7b Mon Sep 17 00:00:00 2001 From: Henrik Date: Wed, 24 Sep 2025 14:33:11 +0200 Subject: [PATCH 002/128] added subscriber with topic in config --- .../velocity_controller/config/parameters.yaml | 2 +- .../src/velocity_controller.cpp | 15 ++++++++++++--- 2 files changed, 13 insertions(+), 4 deletions(-) diff --git a/control/velocity_controller/config/parameters.yaml b/control/velocity_controller/config/parameters.yaml index e6c322a0b..7fbf4f035 100644 --- a/control/velocity_controller/config/parameters.yaml +++ b/control/velocity_controller/config/parameters.yaml @@ -1,6 +1,6 @@ velocity_controller_node: # Name of the node usually goes here. We use wildcard so that we can change the name from the launch file. ros__parameters: topic_info_out: "velocity_out_topic" # parameter name: parameter value - topic_w_in: "reference" #reference speed and direction + topic_ref_in: "reference" #reference speed and direction topic_v_in: "current_velocity" #current speed #publish_value: "Hello from config!" # parameter name: parameter value diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index 1fc740a82..efd6cd384 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -14,9 +14,9 @@ class Velocity_node : public rclcpp::Node //Parameter from config. !!Needs to create launch file to prevent writing where to get the parameters from this->declare_parameter("topic_info_out"); - this->declare_parameter("topic_w_in"); + this->declare_parameter("topic_ref_in"); info_out_topic = this->get_parameter("topic_info_out").as_string(); - reference_topic=this->get_parameter("topic_w_in").as_string(); + reference_topic=this->get_parameter("topic_ref_in").as_string(); // Lager en publisher som publisher på topic, velocity topic, 10 i "sikkerhet" publisher_ = create_publisher(info_out_topic, 10); @@ -24,6 +24,10 @@ class Velocity_node : public rclcpp::Node timer_ = this->create_wall_timer( std::chrono::milliseconds(500), std::bind(&Velocity_node::send_velocity, this)); + + subscriber_ = this->create_subscription( + reference_topic,10, + std::bind(&Velocity_node::recieve_new_reference,this, std::placeholders::_1)); } @@ -31,6 +35,7 @@ class Velocity_node : public rclcpp::Node //Publisher og timer instansene rclcpp::Publisher::SharedPtr publisher_; rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Subscription::SharedPtr subscriber_; std::string info_out_topic; std::string reference_topic; @@ -45,7 +50,11 @@ class Velocity_node : public rclcpp::Node publisher_->publish(message); } - +//Ny referanse funksjon: + void recieve_new_reference(const std_msgs::msg::String::SharedPtr msg_ptr){ + RCLCPP_INFO(this->get_logger(), "Received reference: '%s'", msg_ptr->data.c_str()); + + } }; From 1d1cb530bb5db09bbbbb9b6c67b4b5d4ea3b193a Mon Sep 17 00:00:00 2001 From: Henrik Date: Wed, 24 Sep 2025 15:00:46 +0200 Subject: [PATCH 003/128] Added hpp file --- .../include/velocity_controller/velocity_controller.hpp | 0 control/velocity_controller/src/velocity_controller.cpp | 3 ++- 2 files changed, 2 insertions(+), 1 deletion(-) create mode 100644 control/velocity_controller/include/velocity_controller/velocity_controller.hpp diff --git a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp new file mode 100644 index 000000000..e69de29bb diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index efd6cd384..90a52254e 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -1,5 +1,6 @@ #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/string.hpp" +#include "velocity_controller/velocity_controller.hpp" //#include "vortex-msgs/msg" kan legge til nye meldinger nå //Lager en klasse velocity node @@ -12,7 +13,7 @@ class Velocity_node : public rclcpp::Node //Dytter info til log RCLCPP_INFO(this->get_logger(), "Velocity control node has been started."); - //Parameter from config. !!Needs to create launch file to prevent writing where to get the parameters from + //Parameter from config. this->declare_parameter("topic_info_out"); this->declare_parameter("topic_ref_in"); info_out_topic = this->get_parameter("topic_info_out").as_string(); From 60dbb728fe9f5533f80e2ef33cde605d7f4ec775 Mon Sep 17 00:00:00 2001 From: Henrik Date: Wed, 24 Sep 2025 16:41:11 +0200 Subject: [PATCH 004/128] added subscriber to current velocity and pitch maybe --- control/velocity_controller/CMakeLists.txt | 6 + .../config/parameters.yaml | 4 +- .../velocity_controller.hpp | 33 +++++ control/velocity_controller/package.xml | 1 + .../src/velocity_controller.cpp | 118 +++++++++++------- 5 files changed, 115 insertions(+), 47 deletions(-) diff --git a/control/velocity_controller/CMakeLists.txt b/control/velocity_controller/CMakeLists.txt index a905a4d09..cf6d27f61 100644 --- a/control/velocity_controller/CMakeLists.txt +++ b/control/velocity_controller/CMakeLists.txt @@ -27,6 +27,11 @@ ament_package() add_executable(velocity_controller_node src/velocity_controller.cpp) +target_include_directories(velocity_controller_node PUBLIC + $ + $ +) + install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}/ ) @@ -41,4 +46,5 @@ ament_target_dependencies(velocity_controller_node rclcpp std_msgs vortex_msgs + geometry_msgs ) diff --git a/control/velocity_controller/config/parameters.yaml b/control/velocity_controller/config/parameters.yaml index 7fbf4f035..feba48758 100644 --- a/control/velocity_controller/config/parameters.yaml +++ b/control/velocity_controller/config/parameters.yaml @@ -1,6 +1,6 @@ -velocity_controller_node: # Name of the node usually goes here. We use wildcard so that we can change the name from the launch file. +/**: # Name of the node usually goes here. We use wildcard so that we can change the name from the launch file. ros__parameters: topic_info_out: "velocity_out_topic" # parameter name: parameter value topic_ref_in: "reference" #reference speed and direction - topic_v_in: "current_velocity" #current speed + topic_velocity_and_orientation_in: "current_velocity" #current speed #publish_value: "Hello from config!" # parameter name: parameter value diff --git a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp index e69de29bb..1e8822496 100644 --- a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp +++ b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp @@ -0,0 +1,33 @@ +#pragma once + +#include +#include +#include +//#include "vortex-msgs/msg" kan legge til nye meldinger nå + +class Velocity_node : public rclcpp::Node{ + public: + Velocity_node(); + //publiserer fart + void send_velocity(); + //New reference + void recieve_new_reference(const geometry_msgs::msg::WrenchStamped::SharedPtr msg_ptr); + //New current velocity and orientation + void recieve_new_velocity_and_orientation(const geometry_msgs::msg::WrenchStamped::SharedPtr msg_ptr); + //Alle funksjonens variabler +//Publisher og timer instansene + rclcpp::Publisher::SharedPtr publisher_; + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Subscription::SharedPtr subscriber_; + rclcpp::Subscription::SharedPtr sub_velocity_and_orientation_; + //Variabler + + std::string info_out_topic; + std::string reference_topic; + std::string velocity_and_orientation_topic; + //lagrer referanse og nåværende fart + + geometry_msgs::msg::WrenchStamped reference; + geometry_msgs::msg::WrenchStamped current_velocity_and_orientation; +}; + diff --git a/control/velocity_controller/package.xml b/control/velocity_controller/package.xml index 713d2d8ff..87c5c268b 100644 --- a/control/velocity_controller/package.xml +++ b/control/velocity_controller/package.xml @@ -12,6 +12,7 @@ rclcpp std_msgs vortex_msgs + geometry_msgs ament_lint_auto ament_lint_common diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index 90a52254e..209df774b 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -1,63 +1,67 @@ #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/string.hpp" #include "velocity_controller/velocity_controller.hpp" +#include "geometry_msgs/msg/wrench_stamped.hpp" //#include "vortex-msgs/msg" kan legge til nye meldinger nå //Lager en klasse velocity node -class Velocity_node : public rclcpp::Node -{ -public: -//Konstruktør - Velocity_node() : Node("velocity_controller_node") - { - //Dytter info til log - RCLCPP_INFO(this->get_logger(), "Velocity control node has been started."); - //Parameter from config. - this->declare_parameter("topic_info_out"); - this->declare_parameter("topic_ref_in"); - info_out_topic = this->get_parameter("topic_info_out").as_string(); - reference_topic=this->get_parameter("topic_ref_in").as_string(); - - // Lager en publisher som publisher på topic, velocity topic, 10 i "sikkerhet" - publisher_ = create_publisher(info_out_topic, 10); - //Lager en timer som kaller funksjonen timer_callback hvert 500ms - timer_ = this->create_wall_timer( - std::chrono::milliseconds(500), - std::bind(&Velocity_node::send_velocity, this)); +//Konstruktør +Velocity_node::Velocity_node() : Node("velocity_controller_node") +{ + //Dytter info til log + RCLCPP_INFO(this->get_logger(), "Velocity control node has been started."); - subscriber_ = this->create_subscription( - reference_topic,10, - std::bind(&Velocity_node::recieve_new_reference,this, std::placeholders::_1)); - } + //Parameter from config. + this->declare_parameter("topic_info_out"); + this->declare_parameter("topic_ref_in"); + this->declare_parameter("topic_velocity_and_orientation_in"); + info_out_topic = this->get_parameter("topic_info_out").as_string(); + reference_topic=this->get_parameter("topic_ref_in").as_string(); + velocity_and_orientation_topic = this->get_parameter("topic_velocity_and_orientation_in").as_string(); + // Lager en publisher som publisher på topic, velocity topic, 10 i "sikkerhet" + publisher_ = create_publisher(info_out_topic, 10); + sub_velocity_and_orientation_ = this->create_subscription( + velocity_and_orientation_topic, 10, + std::bind(&Velocity_node::recieve_new_velocity_and_orientation,this, std::placeholders::_1)); + //Lager en timer som kaller funksjonen timer_callback hvert 500ms + timer_ = this->create_wall_timer( + std::chrono::milliseconds(500), + std::bind(&Velocity_node::send_velocity, this)); -//Alle funksjonens variabler -//Publisher og timer instansene - rclcpp::Publisher::SharedPtr publisher_; - rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Subscription::SharedPtr subscriber_; + subscriber_ = this->create_subscription( + reference_topic,10, + std::bind(&Velocity_node::recieve_new_reference,this, std::placeholders::_1)); +} - std::string info_out_topic; - std::string reference_topic; //Utdata på topic - void send_velocity() - { - auto message = std_msgs::msg::String(); - message.data = "Velocity command"; - //RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str()); - publisher_->publish(message); - } - -//Ny referanse funksjon: - void recieve_new_reference(const std_msgs::msg::String::SharedPtr msg_ptr){ - RCLCPP_INFO(this->get_logger(), "Received reference: '%s'", msg_ptr->data.c_str()); - - } +void Velocity_node::send_velocity() +{ + auto message = geometry_msgs::msg::WrenchStamped(); + message.wrench.force.x = 1.0; + message.wrench.force.y = 0.0; + message.wrench.force.z = 0.0; + message.wrench.torque.x = 0.0; + message.wrench.torque.y = 0.0; + message.wrench.torque.z = 0.0; + publisher_->publish(message); +} -}; +//Ny referanse funksjon: +void Velocity_node::recieve_new_reference(const geometry_msgs::msg::WrenchStamped::SharedPtr msg_ptr){ + RCLCPP_INFO(this->get_logger(), "Received reference: '%f'", msg_ptr->wrench.force.x); + reference = *msg_ptr; + return; +} +//Ny velocity og orientation funksjon: +void Velocity_node::recieve_new_velocity_and_orientation(const geometry_msgs::msg::WrenchStamped::SharedPtr msg_ptr){ + RCLCPP_INFO(this->get_logger(), "Received velocity and orientation: '%f'", msg_ptr->wrench.force.x); + current_velocity_and_orientation = *msg_ptr; + return; +} int main(int argc, char * argv[]) { @@ -67,3 +71,27 @@ int main(int argc, char * argv[]) return 0; } +//---------------------------------------------------------------------------------------------------------------- +//Operator overloading for geometry_msgs::msg::WrenchStamped +geometry_msgs::msg::WrenchStamped operator-(const geometry_msgs::msg::WrenchStamped & a, const geometry_msgs::msg::WrenchStamped & b) +{ + geometry_msgs::msg::WrenchStamped result; + result.wrench.force.x = a.wrench.force.x - b.wrench.force.x; + result.wrench.force.y = a.wrench.force.y - b.wrench.force.y; + result.wrench.force.z = a.wrench.force.z - b.wrench.force.z; + result.wrench.torque.x = a.wrench.torque.x - b.wrench.torque.x; + result.wrench.torque.y = a.wrench.torque.y - b.wrench.torque.y; + result.wrench.torque.z = a.wrench.torque.z - b.wrench.torque.z; + return result; +} +geometry_msgs::msg::WrenchStamped operator+(const geometry_msgs::msg::WrenchStamped & a, const geometry_msgs::msg::WrenchStamped & b) +{ + geometry_msgs::msg::WrenchStamped result; + result.wrench.force.x = a.wrench.force.x + b.wrench.force.x; + result.wrench.force.y = a.wrench.force.y + b.wrench.force.y; + result.wrench.force.z = a.wrench.force.z + b.wrench.force.z; + result.wrench.torque.x = a.wrench.torque.x + b.wrench.torque.x; + result.wrench.torque.y = a.wrench.torque.y + b.wrench.torque.y; + result.wrench.torque.z = a.wrench.torque.z + b.wrench.torque.z; + return result; +} \ No newline at end of file From 252ec54a121ef6b930b43bdc298554a42885e941 Mon Sep 17 00:00:00 2001 From: Henrik Date: Wed, 1 Oct 2025 13:20:43 +0200 Subject: [PATCH 005/128] Changed parameter file, topic and subscriber name --- control/velocity_controller/CMakeLists.txt | 13 +++- .../config/parameters.yaml | 36 ++++++++-- .../velocity_controller.hpp | 39 ++++++----- .../launch/VCnTest.launch.py | 40 +++++++++++ control/velocity_controller/src/test_VC.cpp | 43 ++++++++++++ .../src/velocity_controller.cpp | 66 ++++++++++++------- 6 files changed, 190 insertions(+), 47 deletions(-) create mode 100644 control/velocity_controller/launch/VCnTest.launch.py create mode 100644 control/velocity_controller/src/test_VC.cpp diff --git a/control/velocity_controller/CMakeLists.txt b/control/velocity_controller/CMakeLists.txt index cf6d27f61..a0e30f505 100644 --- a/control/velocity_controller/CMakeLists.txt +++ b/control/velocity_controller/CMakeLists.txt @@ -26,7 +26,7 @@ endif() ament_package() add_executable(velocity_controller_node src/velocity_controller.cpp) - +add_executable(test_VC_node src/test_VC.cpp) target_include_directories(velocity_controller_node PUBLIC $ $ @@ -38,7 +38,10 @@ install(DIRECTORY launch install(TARGETS velocity_controller_node DESTINATION lib/${PROJECT_NAME} ) -install(DIRECTORY config +install(TARGETS test_VC_node + DESTINATION lib/${PROJECT_NAME} +) +install(DIRECTORY config DESTINATION share/${PROJECT_NAME}/ ) @@ -48,3 +51,9 @@ ament_target_dependencies(velocity_controller_node vortex_msgs geometry_msgs ) +ament_target_dependencies(test_VC_node + rclcpp + std_msgs + vortex_msgs + geometry_msgs +) diff --git a/control/velocity_controller/config/parameters.yaml b/control/velocity_controller/config/parameters.yaml index feba48758..c0dc252b8 100644 --- a/control/velocity_controller/config/parameters.yaml +++ b/control/velocity_controller/config/parameters.yaml @@ -1,6 +1,34 @@ -/**: # Name of the node usually goes here. We use wildcard so that we can change the name from the launch file. +/**: ros__parameters: - topic_info_out: "velocity_out_topic" # parameter name: parameter value - topic_ref_in: "reference" #reference speed and direction - topic_velocity_and_orientation_in: "current_velocity" #current speed + + topics: + odom_topic: /orca/odom #Odoemtry + twist_topic: /dvl/twist #Twist + pose_topic: /dvl/pose #Pose + guidance_topic: /guidance/los #Guidance + thrust_topic: /thrust/wrench_input #Thrust + softwareoperation_topic: /softwareOperationMode #Software Operation + killswitch_topic: /softwareKillSwitch #Kill Switch + + LQR_params: + q_surge: 75 + q_pitch: 175 + q_yaw: 175 + + r_surge: 0.3 + r_pitch: 0.4 + r_yaw: 0.4 + + i_surge: 0.3 + i_pitch: 0.4 + i_yaw: 0.3 + + i_weight: 0.5 + + dt: 0.1 + + inertia_matrix: [30.0, 0.6, 0.0, 0.6, 1.629, 0.0, 0.0, 0.0, 1.729] + + #Clamp parameter + max_force: 99.5 #publish_value: "Hello from config!" # parameter name: parameter value diff --git a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp index 1e8822496..60c55a93c 100644 --- a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp +++ b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp @@ -3,30 +3,37 @@ #include #include #include + //#include "vortex-msgs/msg" kan legge til nye meldinger nå class Velocity_node : public rclcpp::Node{ public: Velocity_node(); - //publiserer fart - void send_velocity(); - //New reference - void recieve_new_reference(const geometry_msgs::msg::WrenchStamped::SharedPtr msg_ptr); - //New current velocity and orientation - void recieve_new_velocity_and_orientation(const geometry_msgs::msg::WrenchStamped::SharedPtr msg_ptr); - //Alle funksjonens variabler -//Publisher og timer instansene - rclcpp::Publisher::SharedPtr publisher_; + //Timer functions + void publish_thrust(); + //Callback functions + void guidance_callback(const geometry_msgs::msg::WrenchStamped::SharedPtr msg_ptr); + void killswitch_callback(const geometry_msgs::msg::WrenchStamped::SharedPtr msg_ptr); + void twist_callback(const geometry_msgs::msg::WrenchStamped::SharedPtr msg_ptr); + + //Publisher instance + rclcpp::Publisher::SharedPtr publisher_thrust; + + //Timer instance rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Subscription::SharedPtr subscriber_; - rclcpp::Subscription::SharedPtr sub_velocity_and_orientation_; - //Variabler + //Subscriber instance + rclcpp::Subscription::SharedPtr subscriber_twist; + rclcpp::Subscription::SharedPtr subscriber_guidance; + rclcpp::Subscription::SharedPtr subscriber_killswitch; + + //Variables for topics - std::string info_out_topic; - std::string reference_topic; - std::string velocity_and_orientation_topic; - //lagrer referanse og nåværende fart + std::string topic_thrust; + std::string topic_guidance; + std::string topic_killswitch; + std::string topic_twist; + //Stored values geometry_msgs::msg::WrenchStamped reference; geometry_msgs::msg::WrenchStamped current_velocity_and_orientation; }; diff --git a/control/velocity_controller/launch/VCnTest.launch.py b/control/velocity_controller/launch/VCnTest.launch.py new file mode 100644 index 000000000..ab2b64fd8 --- /dev/null +++ b/control/velocity_controller/launch/VCnTest.launch.py @@ -0,0 +1,40 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +import os +#from launch.actions import IncludeLaunchDescription +from launch.actions import DeclareLaunchArgument +#from launch.launch_description_sources import PythonLaunchDescriptionSource +from ament_index_python.packages import get_package_share_directory +from launch.substitutions import LaunchConfiguration + +def generate_launch_description(): + pkg_share = get_package_share_directory('velocity_controller') + config_path = os.path.join(pkg_share, 'config', 'parameters.yaml') + + node_name_arg = DeclareLaunchArgument( + 'node_name', default_value='velocity_controller_node', + description='Name of the velocity controller node' + ) + + node_name_arg2 = DeclareLaunchArgument( + 'node_name', default_value='test_VC_node', + description='Name of the test VC node' + ) + + velocity_controller_name = LaunchConfiguration('node_name') + test_VC_name = LaunchConfiguration('node_name') + + return LaunchDescription([ + node_name_arg, + node_name_arg2, + Node(package='velocity_controller', + executable='velocity_controller_node', + name=velocity_controller_name, + output='screen', + parameters=[config_path]), + Node(package='velocity_controller', + executable='test_VC_node', + name=test_VC_name, + output='screen', + parameters=[config_path]) + ]) \ No newline at end of file diff --git a/control/velocity_controller/src/test_VC.cpp b/control/velocity_controller/src/test_VC.cpp new file mode 100644 index 000000000..bad2dfcba --- /dev/null +++ b/control/velocity_controller/src/test_VC.cpp @@ -0,0 +1,43 @@ +#include +#include +#include +#include +//Denne noden er kun for å teste velocity_controller noden + +class test_VC : public rclcpp::Node{ + public: + test_VC() : Node("test_VC_node") + { + this->declare_parameter("topics.guidance_topic"); + topic_guidance=this->get_parameter("topics.guidance_topic").as_string(); + publisher_ = this->create_publisher(topic_guidance,10); + timer_ = this->create_wall_timer( + std::chrono::milliseconds(500), + std::bind(&test_VC::send_velocity, this)); + clock_ = this->get_clock(); + RCLCPP_INFO(this->get_logger(), "Test_VC node has been started"); + } + + rclcpp::Publisher::SharedPtr publisher_; + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Clock::SharedPtr clock_; + std::string topic_guidance; + void send_velocity() + { + auto message = geometry_msgs::msg::WrenchStamped(); + message.wrench.force.x = std::sin(clock_->now().seconds()); + message.wrench.force.y = 0.0; + message.wrench.force.z = 0.0; + message.wrench.torque.x = 0.0; + message.wrench.torque.y = 0.0; + message.wrench.torque.z = 0.0; + publisher_->publish(message); + } +}; +int main(int argc, char const *argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index 209df774b..dcfe1efae 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -13,32 +13,39 @@ Velocity_node::Velocity_node() : Node("velocity_controller_node") RCLCPP_INFO(this->get_logger(), "Velocity control node has been started."); //Parameter from config. - this->declare_parameter("topic_info_out"); - this->declare_parameter("topic_ref_in"); - this->declare_parameter("topic_velocity_and_orientation_in"); - info_out_topic = this->get_parameter("topic_info_out").as_string(); - reference_topic=this->get_parameter("topic_ref_in").as_string(); - velocity_and_orientation_topic = this->get_parameter("topic_velocity_and_orientation_in").as_string(); + this->declare_parameter("topics.thrust_topic"); + this->declare_parameter("topics.guidance_topic"); + this->declare_parameter("topics.twist_topic"); + this->declare_parameter("topics.killswitch_topic"); + this->topic_thrust = this->get_parameter("topics.thrust_topic").as_string(); + this->topic_guidance = this->get_parameter("topics.guidance_topic").as_string(); + this->topic_twist = this->get_parameter("topics.twist_topic").as_string(); + this->topic_killswitch = this->get_parameter("topics.killswitch_topic").as_string(); - // Lager en publisher som publisher på topic, velocity topic, 10 i "sikkerhet" - publisher_ = create_publisher(info_out_topic, 10); - sub_velocity_and_orientation_ = this->create_subscription( - velocity_and_orientation_topic, 10, - std::bind(&Velocity_node::recieve_new_velocity_and_orientation,this, std::placeholders::_1)); - //Lager en timer som kaller funksjonen timer_callback hvert 500ms - timer_ = this->create_wall_timer( - std::chrono::milliseconds(500), - std::bind(&Velocity_node::send_velocity, this)); + // Publishers + publisher_thrust = create_publisher(topic_thrust, 10); + + //Subscribers + subscriber_twist = this->create_subscription( + topic_twist, 10, + std::bind(&Velocity_node::twist_callback,this, std::placeholders::_1)); + subscriber_guidance = this->create_subscription( + topic_guidance,10, + std::bind(&Velocity_node::guidance_callback,this, std::placeholders::_1)); + subscriber_killswitch = this->create_subscription( + topic_killswitch,10, + std::bind(&Velocity_node::killswitch_callback,this, std::placeholders::_1)); - subscriber_ = this->create_subscription( - reference_topic,10, - std::bind(&Velocity_node::recieve_new_reference,this, std::placeholders::_1)); + //Timer + timer_ = this->create_wall_timer(std::chrono::milliseconds(500),std::bind(&Velocity_node::publish_thrust, this)); + + } -//Utdata på topic -void Velocity_node::send_velocity() +//Publish functions +void Velocity_node::publish_thrust() { auto message = geometry_msgs::msg::WrenchStamped(); message.wrench.force.x = 1.0; @@ -47,22 +54,31 @@ void Velocity_node::send_velocity() message.wrench.torque.x = 0.0; message.wrench.torque.y = 0.0; message.wrench.torque.z = 0.0; - publisher_->publish(message); + publisher_thrust->publish(message); } -//Ny referanse funksjon: -void Velocity_node::recieve_new_reference(const geometry_msgs::msg::WrenchStamped::SharedPtr msg_ptr){ +//Callback functions +void Velocity_node::guidance_callback(const geometry_msgs::msg::WrenchStamped::SharedPtr msg_ptr){ RCLCPP_INFO(this->get_logger(), "Received reference: '%f'", msg_ptr->wrench.force.x); reference = *msg_ptr; return; } -//Ny velocity og orientation funksjon: -void Velocity_node::recieve_new_velocity_and_orientation(const geometry_msgs::msg::WrenchStamped::SharedPtr msg_ptr){ +void Velocity_node::twist_callback(const geometry_msgs::msg::WrenchStamped::SharedPtr msg_ptr){ RCLCPP_INFO(this->get_logger(), "Received velocity and orientation: '%f'", msg_ptr->wrench.force.x); current_velocity_and_orientation = *msg_ptr; return; } +//**Needs to update to shutdown the node +void Velocity_node::killswitch_callback(const geometry_msgs::msg::WrenchStamped::SharedPtr msg_ptr){ + RCLCPP_INFO(this->get_logger(), "Received killswitch: '%f'", msg_ptr->wrench.force.x); + if(msg_ptr->wrench.force.x == 1.0){ + reference = geometry_msgs::msg::WrenchStamped(); + current_velocity_and_orientation = geometry_msgs::msg::WrenchStamped(); + RCLCPP_INFO(this->get_logger(), "Killswitch activated, reference and current velocity set to zero"); + } + return; +} int main(int argc, char * argv[]) { rclcpp::init(argc, argv); From 58a87f9769738ad33becf5ec5c3b68f860d1ca2f Mon Sep 17 00:00:00 2001 From: Henrik Date: Wed, 1 Oct 2025 14:49:36 +0200 Subject: [PATCH 006/128] Made PI regulator, able to plot the thrust --- .../config/parameters.yaml | 2 ++ .../velocity_controller.hpp | 21 +++++++++-- control/velocity_controller/src/test_VC.cpp | 26 +++++++++++++- .../src/velocity_controller.cpp | 36 +++++++++++-------- 4 files changed, 68 insertions(+), 17 deletions(-) diff --git a/control/velocity_controller/config/parameters.yaml b/control/velocity_controller/config/parameters.yaml index c0dc252b8..dee5d9f25 100644 --- a/control/velocity_controller/config/parameters.yaml +++ b/control/velocity_controller/config/parameters.yaml @@ -29,6 +29,8 @@ inertia_matrix: [30.0, 0.6, 0.0, 0.6, 1.629, 0.0, 0.0, 0.0, 1.729] + calculation_rate: 10 #ms integer + publish_rate: 10 #ms #Clamp parameter max_force: 99.5 #publish_value: "Hello from config!" # parameter name: parameter value diff --git a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp index 60c55a93c..5707b9d95 100644 --- a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp +++ b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp @@ -9,8 +9,11 @@ class Velocity_node : public rclcpp::Node{ public: Velocity_node(); + //Timer functions void publish_thrust(); + void calc_thrust(); + //Callback functions void guidance_callback(const geometry_msgs::msg::WrenchStamped::SharedPtr msg_ptr); void killswitch_callback(const geometry_msgs::msg::WrenchStamped::SharedPtr msg_ptr); @@ -20,7 +23,9 @@ class Velocity_node : public rclcpp::Node{ rclcpp::Publisher::SharedPtr publisher_thrust; //Timer instance - rclcpp::TimerBase::SharedPtr timer_; + rclcpp::TimerBase::SharedPtr timer_PID; + rclcpp::TimerBase::SharedPtr timer_publish; + //Subscriber instance rclcpp::Subscription::SharedPtr subscriber_twist; rclcpp::Subscription::SharedPtr subscriber_guidance; @@ -33,8 +38,20 @@ class Velocity_node : public rclcpp::Node{ std::string topic_killswitch; std::string topic_twist; - //Stored values + //Variables for timers + int calculation_rate; + int publish_rate; + + //Stored wrenches values geometry_msgs::msg::WrenchStamped reference; geometry_msgs::msg::WrenchStamped current_velocity_and_orientation; + geometry_msgs::msg::WrenchStamped thrust; + + //PID parameters temporary + double k_p = 5.0; + double k_i = 0.5; + double k_d = 0.0; + double integral = 0.0; + double previous_error = 0.0; //improved Riemanns sums }; diff --git a/control/velocity_controller/src/test_VC.cpp b/control/velocity_controller/src/test_VC.cpp index bad2dfcba..7cda54c97 100644 --- a/control/velocity_controller/src/test_VC.cpp +++ b/control/velocity_controller/src/test_VC.cpp @@ -2,6 +2,7 @@ #include #include #include +#include //Denne noden er kun for å teste velocity_controller noden class test_VC : public rclcpp::Node{ @@ -11,21 +12,33 @@ class test_VC : public rclcpp::Node{ this->declare_parameter("topics.guidance_topic"); topic_guidance=this->get_parameter("topics.guidance_topic").as_string(); publisher_ = this->create_publisher(topic_guidance,10); + this->declare_parameter("topics.thrust_topic"); + topic_subscription = this->get_parameter("topics.thrust_topic").as_string(); + subscription_ = this->create_subscription( + topic_subscription, 10, + std::bind(&test_VC::listen, this, std::placeholders::_1)); timer_ = this->create_wall_timer( std::chrono::milliseconds(500), std::bind(&test_VC::send_velocity, this)); clock_ = this->get_clock(); + thrust_pub = this->create_publisher("thrust_value", 10); + RCLCPP_INFO(this->get_logger(), "Test_VC node has been started"); } rclcpp::Publisher::SharedPtr publisher_; + rclcpp::Subscription::SharedPtr subscription_; rclcpp::TimerBase::SharedPtr timer_; rclcpp::Clock::SharedPtr clock_; std::string topic_guidance; + std::string topic_subscription; + std::vector thrust_vector; + rclcpp::Publisher::SharedPtr thrust_pub; + void send_velocity() { auto message = geometry_msgs::msg::WrenchStamped(); - message.wrench.force.x = std::sin(clock_->now().seconds()); + message.wrench.force.x = std::sin(clock_->now().seconds()*2*3.14159/10); //sinuskurve med periode 10 sekunder og amplitude 1 message.wrench.force.y = 0.0; message.wrench.force.z = 0.0; message.wrench.torque.x = 0.0; @@ -33,6 +46,17 @@ class test_VC : public rclcpp::Node{ message.wrench.torque.z = 0.0; publisher_->publish(message); } + + void listen(geometry_msgs::msg::WrenchStamped::SharedPtr msg) + { + thrust_vector.push_back(msg->wrench.force.x); + std_msgs::msg::Float64 pub_info; + pub_info.data = thrust_vector.back(); + thrust_pub->publish(pub_info); + //RCLCPP_INFO(this->get_logger(), "Received thrust: '%f'", msg->wrench.force.x); + return; + } + }; int main(int argc, char const *argv[]) { diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index dcfe1efae..d29397672 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -21,7 +21,7 @@ Velocity_node::Velocity_node() : Node("velocity_controller_node") this->topic_guidance = this->get_parameter("topics.guidance_topic").as_string(); this->topic_twist = this->get_parameter("topics.twist_topic").as_string(); this->topic_killswitch = this->get_parameter("topics.killswitch_topic").as_string(); - + this-> // Publishers publisher_thrust = create_publisher(topic_thrust, 10); @@ -37,34 +37,42 @@ Velocity_node::Velocity_node() : Node("velocity_controller_node") std::bind(&Velocity_node::killswitch_callback,this, std::placeholders::_1)); //Timer - timer_ = this->create_wall_timer(std::chrono::milliseconds(500),std::bind(&Velocity_node::publish_thrust, this)); + this->declare_parameter("calculation_rate"); + this->declare_parameter("publish_rate"); + this->calculation_rate = this->get_parameter("calculation_rate").as_int(); + this->publish_rate = this->get_parameter("publish_rate").as_int(); + timer_PID = this->create_wall_timer(std::chrono::milliseconds(calculation_rate), std::bind(&Velocity_node::publish_thrust, this)); + timer_publish = this->create_wall_timer(std::chrono::milliseconds(publish_rate), std::bind(&Velocity_node::calc_thrust, this)); - } -//Publish functions +//Publish/timer functions void Velocity_node::publish_thrust() { - auto message = geometry_msgs::msg::WrenchStamped(); - message.wrench.force.x = 1.0; - message.wrench.force.y = 0.0; - message.wrench.force.z = 0.0; - message.wrench.torque.x = 0.0; - message.wrench.torque.y = 0.0; - message.wrench.torque.z = 0.0; - publisher_thrust->publish(message); + publisher_thrust->publish(thrust); +} + +void Velocity_node::calc_thrust() +{ + auto error_x = reference.wrench.force.x - current_velocity_and_orientation.wrench.force.x; + //PID controller here + integral += error_x * (calculation_rate / 1000.0); //integral term + thrust.wrench.force.x = k_p*error_x + k_i*integral; //Placeholder + return; } + + //Callback functions void Velocity_node::guidance_callback(const geometry_msgs::msg::WrenchStamped::SharedPtr msg_ptr){ - RCLCPP_INFO(this->get_logger(), "Received reference: '%f'", msg_ptr->wrench.force.x); + //RCLCPP_INFO(this->get_logger(), "Received reference: '%f'", msg_ptr->wrench.force.x); reference = *msg_ptr; return; } void Velocity_node::twist_callback(const geometry_msgs::msg::WrenchStamped::SharedPtr msg_ptr){ - RCLCPP_INFO(this->get_logger(), "Received velocity and orientation: '%f'", msg_ptr->wrench.force.x); + //RCLCPP_INFO(this->get_logger(), "Received velocity and orientation: '%f'", msg_ptr->wrench.force.x); current_velocity_and_orientation = *msg_ptr; return; } From 2760cbeedcf917754112a4adde047b8a8e38ef15 Mon Sep 17 00:00:00 2001 From: Henrik Date: Tue, 7 Oct 2025 22:18:42 +0200 Subject: [PATCH 007/128] Implemented the LQR lib from python --- control/velocity_controller/CMakeLists.txt | 7 +- .../include/velocity_controller/LQR_setup.hpp | 70 +++++++ .../velocity_controller.hpp | 7 +- control/velocity_controller/src/LQR_setup.cpp | 191 ++++++++++++++++++ control/velocity_controller/src/test_VC.cpp | 12 +- .../src/velocity_controller.cpp | 30 ++- 6 files changed, 300 insertions(+), 17 deletions(-) create mode 100644 control/velocity_controller/include/velocity_controller/LQR_setup.hpp create mode 100644 control/velocity_controller/src/LQR_setup.cpp diff --git a/control/velocity_controller/CMakeLists.txt b/control/velocity_controller/CMakeLists.txt index a0e30f505..f062f2f86 100644 --- a/control/velocity_controller/CMakeLists.txt +++ b/control/velocity_controller/CMakeLists.txt @@ -10,6 +10,9 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(vortex_msgs REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(drake REQUIRED) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) @@ -31,7 +34,9 @@ target_include_directories(velocity_controller_node PUBLIC $ $ ) - +include_directories(${EIGEN3_INCLUDE_DIR}) +include_directories(${drake_INCLUDE_DIRS}) +target_link_libraries(velocity_controller_node ${drake_LIBRARIES}) install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}/ ) diff --git a/control/velocity_controller/include/velocity_controller/LQR_setup.hpp b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp new file mode 100644 index 000000000..c13e54442 --- /dev/null +++ b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp @@ -0,0 +1,70 @@ +#pragma once + +#include +#include +#include + + + +class State{ + //Dataclass to store state values for LQR controller + public: + double surge=0.0; double pitch=0.0; double yaw=0.0; + double integral_surge=0.0; double integral_pitch=0.0; double integral_yaw=0.0; +}; + +class Guidance_values{ + //Dataclass to store guidance values for LQR controller + public: + double surge=0.0; double pitch=0.0; double yaw=0.0; + double integral_surge=0.0; double integral_pitch=0.0; double integral_yaw=0.0; +}; + +class LQRparameters{ + //Dataclass to store LQR parameters + public: + double q_surge=0.0; double q_pitch=0.0; double q_yaw=0.0; + double r_surge=0.0; double r_pitch=0.0; double r_yaw=0.0; + double i_surge=0.0; double i_pitch=0.0; double i_yaw=0.0; + double i_weight=0.0; double max_force=0.0; +}; + +class angle{ + public: + double phit=0.0; + double thetat=0.0; + double psit=0.0; +}; +class LQRController{ + + public: + LQRController(LQRparameters params,std::vector inertia_matrix); + angle quaternion_to_euler_angle(double w, double x, double y, double z); + double ssa(double angle); + std::tuple saturate (double value, bool windup, double limit); + double anti_windup(double ki, double error, double integral_sum, bool windup); + std::vector> calculate_coriolis_matrix(double pitchrate, double yaw_rate, double sway_vel, double heave_vel); + void set_params(LQRparameters params); + void set_matrices(std::vector inertia_matrix); + void update_augmented_matrices(std::vector > coriolis_matrix); + std::vector update_error(Guidance_values guidance_values, State states); + std::vector saturate_input(std::vector u); + std::vector calculate_lqr_u(std::vector> coriolis_matrix, State states, Guidance_values guidance_values); + void reset_controller(); + + // Variables + const double pi=3.14159265358979323846; + double integral_error_surge; double integral_error_pitch; double integral_error_yaw; + bool surge_windup; bool pitch_windup; bool yaw_windup; + double q_surge; double q_pitch; double q_yaw; + double r_surge; double r_pitch; double r_yaw; + double i_surge; double i_pitch; double i_yaw; + double i_weight; double max_force; + + std::vector> inertia_matrix_inv; + std::vector> state_weight_matrix; + std::vector> input_weight_matrix; + std::vector> augmented_system_matrix; + std::vector> augmented_input_matrix; +}; + diff --git a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp index 5707b9d95..6cf0fcd18 100644 --- a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp +++ b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp @@ -41,17 +41,20 @@ class Velocity_node : public rclcpp::Node{ //Variables for timers int calculation_rate; int publish_rate; + double max_force; //Stored wrenches values geometry_msgs::msg::WrenchStamped reference; - geometry_msgs::msg::WrenchStamped current_velocity_and_orientation; + geometry_msgs::msg::WrenchStamped current_twist; geometry_msgs::msg::WrenchStamped thrust; //PID parameters temporary double k_p = 5.0; - double k_i = 0.5; + double k_i = 2.0; double k_d = 0.0; double integral = 0.0; double previous_error = 0.0; //improved Riemanns sums }; + + diff --git a/control/velocity_controller/src/LQR_setup.cpp b/control/velocity_controller/src/LQR_setup.cpp new file mode 100644 index 000000000..b901b7b66 --- /dev/null +++ b/control/velocity_controller/src/LQR_setup.cpp @@ -0,0 +1,191 @@ +#include "velocity_controller/LQR_setup.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +LQRController::LQRController(LQRparameters params,std::vector inertia_matrix){ + set_params(params); + set_matrices(inertia_matrix); +}; + +angle LQRController::quaternion_to_euler_angle(double w, double x, double y, double z){ + double ysqr = y * y; + + double t0 = +2.0 * (w * x + y * z); + double t1 = +1.0 - 2.0 * (x * x + ysqr); + double phi = std::atan2(t0, t1); + + double t2 = +2.0 * (w * y - z * x); + t2 = t2 > 1.0 ? 1.0 : t2; + t2 = t2 < -1.0 ? -1.0 : t2; + double theta = std::asin(t2); + + double t3 = +2.0 * (w * z + x * y); + double t4 = +1.0 - 2.0 * (ysqr + z * z); + double psi = std::atan2(t3, t4); + + return {phi, theta, psi}; +}; + +double LQRController::ssa(double angle){ + return std::fmod(angle+pi, 2*pi)-pi; +}; + +//Can be optimized +std::tuple LQRController::saturate (double value, bool windup, double limit){ + if (abs(value) > limit){ + windup=true; + value = limit * (value/abs(value)); + } + else { + windup=false; + } + return {windup,value}; +} + + + +double LQRController::anti_windup(double ki, double error, double integral_sum, bool windup){ + if (!windup){ + integral_sum += error * ki; + } + return integral_sum; +} + +std::vector> LQRController::calculate_coriolis_matrix(double pitchrate, double yaw_rate, double sway_vel, double heave_vel){ + //Inertia matrix values?? + return {{0.2,-30*sway_vel*0.01,-30*heave_vel*0.01}, + {30 * sway_vel*0.01,0,1.629 * pitchrate}, + {30 * heave_vel*0.01,1.769 * yaw_rate,0}}; +} + + +void LQRController::set_params(LQRparameters params){ + //set LQR parameters + integral_error_surge= 0.0; integral_error_pitch= 0.0; integral_error_yaw= 0.0; + surge_windup= false; pitch_windup= false; yaw_windup= false; + q_surge= params.q_surge; q_pitch= params.q_pitch; q_yaw= params.q_yaw; + r_surge= params.r_surge; r_pitch= params.r_pitch; r_yaw= params.r_yaw; + i_surge= params.i_surge; i_pitch= params.i_pitch; i_yaw= params.i_yaw; + i_weight= params.i_weight; max_force= params.max_force; + return; + +} +void LQRController::set_matrices(std::vector inertia_matrix){ + Eigen::Matrix3d mat= vector_to_matrix3d(inertia_matrix); + inertia_matrix_inv = matrix3d_to_vector2d(mat.inverse()); + state_weight_matrix = {{q_surge,0,0,0,0,0}, + {0,q_pitch,0,0,0,0}, + {0,0,q_yaw,0,0,0}, + {0,0,0,i_weight,0,0}, + {0,0,0,0,i_weight,0}, + {0,0,0,0,0,i_weight}}; + input_weight_matrix = {{r_surge,0,0}, + {0,r_pitch,0}, + {0,0,r_yaw}}; + + return; +} + + +void LQRController::update_augmented_matrices(std::vector > coriolis_matrix){ + std::vector> system_matrix = matrix3d_to_vector2d(vector2d_to_matrix3d(inertia_matrix_inv) * vector2d_to_matrix3d(coriolis_matrix)); + //input_matrix = inertia_matrix_inv; + augmented_system_matrix = {{system_matrix[0][0],system_matrix[0][1],system_matrix[0][2],0,0,0}, + {system_matrix[1][0],system_matrix[1][1],system_matrix[1][2],0,0,0}, + {system_matrix[2][0],system_matrix[2][1],system_matrix[2][2],0,0,0}, + {-1,0,0,0,0,0}, + {0,-1,0,0,0,0}, + {0,0,-1,0,0,0}}; //Skal det være -1 her? + augmented_input_matrix = {{inertia_matrix_inv[0][0],inertia_matrix_inv[0][1],inertia_matrix_inv[0][2],0,0,0}, + {inertia_matrix_inv[1][0],inertia_matrix_inv[1][1],inertia_matrix_inv[1][2],0,0,0}, + {inertia_matrix_inv[2][0],inertia_matrix_inv[2][1],inertia_matrix_inv[2][2],0,0,0}}; + +}; +std::vector LQRController::update_error(Guidance_values guidance_values, State states){ + double surge_error = guidance_values.surge - states.surge; + double pitch_error = ssa(guidance_values.pitch - states.pitch); + double yaw_error = ssa(guidance_values.yaw - states.yaw); + + integral_error_surge = anti_windup(i_surge, surge_error, integral_error_surge, surge_windup); + integral_error_pitch = anti_windup(i_pitch, pitch_error, integral_error_pitch, pitch_windup); + integral_error_yaw = anti_windup(i_yaw, yaw_error, integral_error_yaw, yaw_windup); + + std::vector state_error= {-surge_error, -pitch_error, -yaw_error, integral_error_surge, integral_error_pitch, integral_error_yaw}; + return state_error; +} +std::vector LQRController::saturate_input(std::vector u){ + double force_x, torque_y, torque_z; + std::tie(surge_windup, force_x) = saturate(u[0], surge_windup, max_force); + std::tie(pitch_windup, torque_y) = saturate(u[1], pitch_windup, max_force); + std::tie(yaw_windup, torque_z) = saturate(u[2], yaw_windup, max_force); + return {force_x, torque_y, torque_z}; +} +std::vector LQRController::calculate_lqr_u(std::vector> coriolis_matrix, State states, Guidance_values guidance_values){ + update_augmented_matrices(coriolis_matrix); + auto result = drake::systems::controllers::LinearQuadraticRegulator( + vector2d_to_matrix3d(augmented_system_matrix), + vector2d_to_matrix3d(augmented_input_matrix), + vector2d_to_matrix3d(state_weight_matrix), + vector2d_to_matrix3d(input_weight_matrix)); + std::vector state_error = update_error(guidance_values, states); + std::vector u= saturate_input(matrix3d_to_vector(-result * vector_to_matrix3d(state_error))); + return u; +} +void LQRController::reset_controller(){ + integral_error_surge=0.0; + integral_error_pitch=0.0; + integral_error_yaw=0.0; + + surge_windup=false; + pitch_windup=false; + yaw_windup=false; + return; +} + + + + + +int main(){ + return 0; +}; + +//Hjelpefunksjoner for å konvertere mellom std::vector og Eigen::Matrix3d +Eigen::Matrix3d vector_to_matrix3d(const std::vector &other_matrix){ + Eigen::Matrix3d mat; + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 3; ++j) + mat(i, j) = other_matrix[i * 3 + j]; + return mat; +} +std::vector matrix3d_to_vector(const Eigen::Matrix3d &mat){ + std::vector other_matrix(9); + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 3; ++j) + other_matrix[i * 3 + j] = mat(i, j); + return other_matrix; +} + +std::vector> matrix3d_to_vector2d(const Eigen::Matrix3d &mat){ + std::vector> other_matrix(3, std::vector(3)); + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 3; ++j) + other_matrix[i][j] = mat(i, j); + return other_matrix; +} + +Eigen::Matrix3d vector2d_to_matrix3d(const std::vector> &other_matrix){ + Eigen::Matrix3d mat; + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 3; ++j) + mat(i, j) = other_matrix[i][j]; + return mat; +} \ No newline at end of file diff --git a/control/velocity_controller/src/test_VC.cpp b/control/velocity_controller/src/test_VC.cpp index 7cda54c97..3c3e153e8 100644 --- a/control/velocity_controller/src/test_VC.cpp +++ b/control/velocity_controller/src/test_VC.cpp @@ -3,6 +3,7 @@ #include #include #include +//#include "LQR_setup.hpp" //Denne noden er kun for å teste velocity_controller noden class test_VC : public rclcpp::Node{ @@ -24,6 +25,8 @@ class test_VC : public rclcpp::Node{ thrust_pub = this->create_publisher("thrust_value", 10); RCLCPP_INFO(this->get_logger(), "Test_VC node has been started"); + + message.wrench.force.x=1; } rclcpp::Publisher::SharedPtr publisher_; @@ -34,16 +37,11 @@ class test_VC : public rclcpp::Node{ std::string topic_subscription; std::vector thrust_vector; rclcpp::Publisher::SharedPtr thrust_pub; + geometry_msgs::msg::WrenchStamped message; void send_velocity() { - auto message = geometry_msgs::msg::WrenchStamped(); message.wrench.force.x = std::sin(clock_->now().seconds()*2*3.14159/10); //sinuskurve med periode 10 sekunder og amplitude 1 - message.wrench.force.y = 0.0; - message.wrench.force.z = 0.0; - message.wrench.torque.x = 0.0; - message.wrench.torque.y = 0.0; - message.wrench.torque.z = 0.0; publisher_->publish(message); } @@ -53,6 +51,7 @@ class test_VC : public rclcpp::Node{ std_msgs::msg::Float64 pub_info; pub_info.data = thrust_vector.back(); thrust_pub->publish(pub_info); + message.wrench.force.x+=0.01*msg->wrench.force.x; //RCLCPP_INFO(this->get_logger(), "Received thrust: '%f'", msg->wrench.force.x); return; } @@ -60,6 +59,7 @@ class test_VC : public rclcpp::Node{ }; int main(int argc, char const *argv[]) { + rclcpp::init(argc, argv); rclcpp::spin(std::make_shared()); rclcpp::shutdown(); diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index d29397672..354f1fdda 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -17,11 +17,13 @@ Velocity_node::Velocity_node() : Node("velocity_controller_node") this->declare_parameter("topics.guidance_topic"); this->declare_parameter("topics.twist_topic"); this->declare_parameter("topics.killswitch_topic"); + this->declare_parameter("max_force"); + this->max_force = this->get_parameter("max_force").as_double(); this->topic_thrust = this->get_parameter("topics.thrust_topic").as_string(); this->topic_guidance = this->get_parameter("topics.guidance_topic").as_string(); this->topic_twist = this->get_parameter("topics.twist_topic").as_string(); this->topic_killswitch = this->get_parameter("topics.killswitch_topic").as_string(); - this-> + // Publishers publisher_thrust = create_publisher(topic_thrust, 10); @@ -54,12 +56,24 @@ void Velocity_node::publish_thrust() publisher_thrust->publish(thrust); } +//** må forbedre integrasjon og derivasjons beregningene void Velocity_node::calc_thrust() { - auto error_x = reference.wrench.force.x - current_velocity_and_orientation.wrench.force.x; - //PID controller here - integral += error_x * (calculation_rate / 1000.0); //integral term - thrust.wrench.force.x = k_p*error_x + k_i*integral; //Placeholder + auto error_x = reference.wrench.force.x - current_twist.wrench.force.x; + /*if (!(thrust.wrench.force.x == max_force || thrust.wrench.force.x == -max_force)){ + integral += error_x * (calculation_rate / 1000.0); //anti windup + }*/ + integral += error_x * (calculation_rate / 1000.0); + double derivative = (error_x - previous_error) / (calculation_rate / 1000.0); + previous_error = error_x; + + thrust.wrench.force.x = k_p*error_x + k_i*integral+k_d*derivative; + if (thrust.wrench.force.x > max_force){ + thrust.wrench.force.x = max_force; + } + else if (thrust.wrench.force.x < -max_force){ + thrust.wrench.force.x = -max_force; + } return; } @@ -73,7 +87,7 @@ void Velocity_node::guidance_callback(const geometry_msgs::msg::WrenchStamped::S } void Velocity_node::twist_callback(const geometry_msgs::msg::WrenchStamped::SharedPtr msg_ptr){ //RCLCPP_INFO(this->get_logger(), "Received velocity and orientation: '%f'", msg_ptr->wrench.force.x); - current_velocity_and_orientation = *msg_ptr; + current_twist = *msg_ptr; return; } @@ -82,8 +96,8 @@ void Velocity_node::killswitch_callback(const geometry_msgs::msg::WrenchStamped: RCLCPP_INFO(this->get_logger(), "Received killswitch: '%f'", msg_ptr->wrench.force.x); if(msg_ptr->wrench.force.x == 1.0){ reference = geometry_msgs::msg::WrenchStamped(); - current_velocity_and_orientation = geometry_msgs::msg::WrenchStamped(); - RCLCPP_INFO(this->get_logger(), "Killswitch activated, reference and current velocity set to zero"); + current_twist = geometry_msgs::msg::WrenchStamped(); + RCLCPP_INFO(this->get_logger(), "Killswitch activated, reference and current twist set to zero"); } return; } From fc5d99bc5914bf76ccd3312dbd466a3f7a008132 Mon Sep 17 00:00:00 2001 From: Henrik Date: Wed, 8 Oct 2025 12:49:24 +0200 Subject: [PATCH 008/128] added a PID_controller class --- .../include/velocity_controller/PID_setup.hpp | 20 ++++++++++ control/velocity_controller/src/PID_setup.cpp | 39 +++++++++++++++++++ 2 files changed, 59 insertions(+) create mode 100644 control/velocity_controller/include/velocity_controller/PID_setup.hpp create mode 100644 control/velocity_controller/src/PID_setup.cpp diff --git a/control/velocity_controller/include/velocity_controller/PID_setup.hpp b/control/velocity_controller/include/velocity_controller/PID_setup.hpp new file mode 100644 index 000000000..59de2484c --- /dev/null +++ b/control/velocity_controller/include/velocity_controller/PID_setup.hpp @@ -0,0 +1,20 @@ +#pragma once + +#include +#include +#include + +class PID_controller { + PID_controller( double k_p, double k_i, double k_d, double max_output, double min_output); + double calculate_thrust(double reference, double current_position, double dt); + void reset_controller(); + private: + double k_p; + double k_i; + double k_d; + double max_output; + double min_output; + double integral; + double previous_error; + double previous_position; +}; \ No newline at end of file diff --git a/control/velocity_controller/src/PID_setup.cpp b/control/velocity_controller/src/PID_setup.cpp new file mode 100644 index 000000000..1901b9926 --- /dev/null +++ b/control/velocity_controller/src/PID_setup.cpp @@ -0,0 +1,39 @@ +#include "velocity_controller/PID_setup.hpp" + +PID_controller::PID_controller( double k_p, double k_i, double k_d, double max_output, double min_output):k_p(k_p), k_i(k_i), k_d(k_d), max_output(max_output), min_output(min_output) { + integral = 0.0; + previous_error = 0.0; + previous_position = 0.0; +}; +double PID_controller::calculate_thrust(double reference, double current_position, double dt){ + //Error calculation + double error = reference - current_position; + //P calculation + double output=k_p*error; + //D calculation + output += k_d * (current_position - previous_position) / dt; + previous_position = current_position; + //I calculation with anti-windup + integral += error * dt; + if (integral > max_output) { + integral -= error * dt; //anti windup + } else if (integral < min_output) { + integral -= error * dt; //anti windup + } + output += k_i * integral; + previous_error = error; + //Output calculation with saturation + + if (output > max_output){ + output = max_output; + } + else if (output < min_output){ + output = min_output; + } + return output; +}; +void PID_controller::reset_controller(){ + integral = 0.0; + previous_error = 0.0; + previous_position = 0.0; +} \ No newline at end of file From b962ad5c6476d36a86480cfc2ab6f262c7f51c4f Mon Sep 17 00:00:00 2001 From: Henrik Date: Wed, 8 Oct 2025 15:53:04 +0200 Subject: [PATCH 009/128] changed datatypes, information flow --- control/velocity_controller/CMakeLists.txt | 3 +- .../config/parameters.yaml | 6 +- .../include/velocity_controller/PID_setup.hpp | 20 +++- .../velocity_controller.hpp | 57 ++++++--- control/velocity_controller/src/LQR_setup.cpp | 2 +- control/velocity_controller/src/PID_setup.cpp | 49 ++++++-- .../src/velocity_controller.cpp | 113 +++++++++++++----- 7 files changed, 181 insertions(+), 69 deletions(-) diff --git a/control/velocity_controller/CMakeLists.txt b/control/velocity_controller/CMakeLists.txt index f062f2f86..aafca1823 100644 --- a/control/velocity_controller/CMakeLists.txt +++ b/control/velocity_controller/CMakeLists.txt @@ -28,8 +28,9 @@ endif() ament_package() -add_executable(velocity_controller_node src/velocity_controller.cpp) +add_executable(velocity_controller_node src/velocity_controller.cpp src/PID_setup.cpp) add_executable(test_VC_node src/test_VC.cpp) + target_include_directories(velocity_controller_node PUBLIC $ $ diff --git a/control/velocity_controller/config/parameters.yaml b/control/velocity_controller/config/parameters.yaml index dee5d9f25..5be2714b8 100644 --- a/control/velocity_controller/config/parameters.yaml +++ b/control/velocity_controller/config/parameters.yaml @@ -2,7 +2,7 @@ ros__parameters: topics: - odom_topic: /orca/odom #Odoemtry + odom_topic: /orca/odom #Odometry twist_topic: /dvl/twist #Twist pose_topic: /dvl/pose #Pose guidance_topic: /guidance/los #Guidance @@ -29,8 +29,8 @@ inertia_matrix: [30.0, 0.6, 0.0, 0.6, 1.629, 0.0, 0.0, 0.0, 1.729] - calculation_rate: 10 #ms integer - publish_rate: 10 #ms + calculation_rate: 200 #ms integer + publish_rate: 200 #ms #Clamp parameter max_force: 99.5 #publish_value: "Hello from config!" # parameter name: parameter value diff --git a/control/velocity_controller/include/velocity_controller/PID_setup.hpp b/control/velocity_controller/include/velocity_controller/PID_setup.hpp index 59de2484c..10f42401a 100644 --- a/control/velocity_controller/include/velocity_controller/PID_setup.hpp +++ b/control/velocity_controller/include/velocity_controller/PID_setup.hpp @@ -5,16 +5,28 @@ #include class PID_controller { + public: PID_controller( double k_p, double k_i, double k_d, double max_output, double min_output); - double calculate_thrust(double reference, double current_position, double dt); + PID_controller(double k_p, double k_i, double k_d) : PID_controller(k_p, k_i, k_d, 100.0, -100.0) {}; + void calculate_thrust(double reference, double current_position, double dt); void reset_controller(); + double output(); + void set_output_limits(double min_output, double max_output); private: double k_p; double k_i; double k_d; - double max_output; - double min_output; double integral; double previous_error; double previous_position; -}; \ No newline at end of file + double last_output; + double max_output; + double min_output; +}; +class angle{ + public: + double phit=0.0; + double thetat=0.0; + double psit=0.0; +}; +angle quaternion_to_euler_angle(double w, double x, double y, double z); diff --git a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp index 6cf0fcd18..1d14a0299 100644 --- a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp +++ b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp @@ -2,10 +2,26 @@ #include #include +#include +#include #include +#include +#include +#include "velocity_controller/PID_setup.hpp" + //#include "vortex-msgs/msg" kan legge til nye meldinger nå +class guidance_data{ + public: + double surge; double pitch; double yaw; + guidance_data(std_msgs::msg::Float64MultiArray msg); + guidance_data():surge(0.0), pitch(0.0), yaw(0.0) {}; + + guidance_data operator-(const guidance_data& other) const; + guidance_data& operator=(const std_msgs::msg::Float64MultiArray& msg); +}; + class Velocity_node : public rclcpp::Node{ public: Velocity_node(); @@ -15,21 +31,23 @@ class Velocity_node : public rclcpp::Node{ void calc_thrust(); //Callback functions - void guidance_callback(const geometry_msgs::msg::WrenchStamped::SharedPtr msg_ptr); - void killswitch_callback(const geometry_msgs::msg::WrenchStamped::SharedPtr msg_ptr); - void twist_callback(const geometry_msgs::msg::WrenchStamped::SharedPtr msg_ptr); - + void guidance_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg_ptr); + void killswitch_callback(const std_msgs::msg::Bool::SharedPtr msg_ptr); + void twist_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg_ptr); + void pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg_ptr); + //Publisher instance rclcpp::Publisher::SharedPtr publisher_thrust; //Timer instance - rclcpp::TimerBase::SharedPtr timer_PID; + rclcpp::TimerBase::SharedPtr timer_calculation; rclcpp::TimerBase::SharedPtr timer_publish; //Subscriber instance - rclcpp::Subscription::SharedPtr subscriber_twist; - rclcpp::Subscription::SharedPtr subscriber_guidance; - rclcpp::Subscription::SharedPtr subscriber_killswitch; + rclcpp::Subscription::SharedPtr subscriber_twist; + rclcpp::Subscription::SharedPtr subscriber_pose; + rclcpp::Subscription::SharedPtr subscriber_guidance; + rclcpp::Subscription::SharedPtr subscriber_killswitch; //Variables for topics @@ -37,6 +55,7 @@ class Velocity_node : public rclcpp::Node{ std::string topic_guidance; std::string topic_killswitch; std::string topic_twist; + std::string topic_pose; //Variables for timers int calculation_rate; @@ -44,17 +63,19 @@ class Velocity_node : public rclcpp::Node{ double max_force; //Stored wrenches values - geometry_msgs::msg::WrenchStamped reference; - geometry_msgs::msg::WrenchStamped current_twist; - geometry_msgs::msg::WrenchStamped thrust; - - //PID parameters temporary - double k_p = 5.0; - double k_i = 2.0; - double k_d = 0.0; - double integral = 0.0; - double previous_error = 0.0; //improved Riemanns sums + std_msgs::msg::Float64MultiArray reference_in; + guidance_data reference; + guidance_data current_state; + geometry_msgs::msg::WrenchStamped thrust_out; + + //PID controllers + PID_controller PID_surge; + PID_controller PID_yaw; + PID_controller PID_pitch; + + }; + diff --git a/control/velocity_controller/src/LQR_setup.cpp b/control/velocity_controller/src/LQR_setup.cpp index b901b7b66..84d94c600 100644 --- a/control/velocity_controller/src/LQR_setup.cpp +++ b/control/velocity_controller/src/LQR_setup.cpp @@ -136,7 +136,7 @@ std::vector LQRController::calculate_lqr_u(std::vector state_error = update_error(guidance_values, states); - std::vector u= saturate_input(matrix3d_to_vector(-result * vector_to_matrix3d(state_error))); + std::vector u= saturate_input(matrix3d_to_vector(- result * vector_to_matrix3d(state_error))); return u; } void LQRController::reset_controller(){ diff --git a/control/velocity_controller/src/PID_setup.cpp b/control/velocity_controller/src/PID_setup.cpp index 1901b9926..74c4fe335 100644 --- a/control/velocity_controller/src/PID_setup.cpp +++ b/control/velocity_controller/src/PID_setup.cpp @@ -5,13 +5,13 @@ PID_controller::PID_controller( double k_p, double k_i, double k_d, double max_o previous_error = 0.0; previous_position = 0.0; }; -double PID_controller::calculate_thrust(double reference, double current_position, double dt){ +void PID_controller::calculate_thrust(double reference, double current_position, double dt){ //Error calculation double error = reference - current_position; //P calculation - double output=k_p*error; + last_output=k_p*error; //D calculation - output += k_d * (current_position - previous_position) / dt; + last_output += k_d * (current_position - previous_position) / dt; previous_position = current_position; //I calculation with anti-windup integral += error * dt; @@ -20,20 +20,49 @@ double PID_controller::calculate_thrust(double reference, double current_positio } else if (integral < min_output) { integral -= error * dt; //anti windup } - output += k_i * integral; + last_output += k_i * integral; previous_error = error; //Output calculation with saturation - if (output > max_output){ - output = max_output; + if (last_output > max_output){ + last_output = max_output; } - else if (output < min_output){ - output = min_output; + else if (last_output < min_output){ + last_output = min_output; } - return output; + return; }; void PID_controller::reset_controller(){ integral = 0.0; previous_error = 0.0; previous_position = 0.0; -} \ No newline at end of file +} + +double PID_controller::output(){ + return last_output; +}; + +void PID_controller::set_output_limits(double min_output, double max_output){ + this->min_output = min_output; + this->max_output = max_output; + return; +}; + +angle quaternion_to_euler_angle(double w, double x, double y, double z){ + double ysqr = y * y; + + double t0 = +2.0 * (w * x + y * z); + double t1 = +1.0 - 2.0 * (x * x + ysqr); + double phi = std::atan2(t0, t1); + + double t2 = +2.0 * (w * y - z * x); + t2 = t2 > 1.0 ? 1.0 : t2; + t2 = t2 < -1.0 ? -1.0 : t2; + double theta = std::asin(t2); + + double t3 = +2.0 * (w * z + x * y); + double t4 = +1.0 - 2.0 * (ysqr + z * z); + double psi = std::atan2(t3, t4); + + return {phi, theta, psi}; +}; \ No newline at end of file diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index 354f1fdda..42886eb87 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -2,12 +2,18 @@ #include "std_msgs/msg/string.hpp" #include "velocity_controller/velocity_controller.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" +#include "std_msgs/msg/float64_multi_array.hpp" +#include +#include +#include "std_msgs/msg/bool.hpp" +#include "velocity_controller/PID_setup.hpp" +#include //#include "vortex-msgs/msg" kan legge til nye meldinger nå //Lager en klasse velocity node //Konstruktør -Velocity_node::Velocity_node() : Node("velocity_controller_node") +Velocity_node::Velocity_node() : Node("velocity_controller_node"), PID_surge(1,1,1), PID_yaw(1,1,1), PID_pitch(1,1,1) { //Dytter info til log RCLCPP_INFO(this->get_logger(), "Velocity control node has been started."); @@ -16,6 +22,7 @@ Velocity_node::Velocity_node() : Node("velocity_controller_node") this->declare_parameter("topics.thrust_topic"); this->declare_parameter("topics.guidance_topic"); this->declare_parameter("topics.twist_topic"); + this->declare_parameter("topics.pose_topic"); this->declare_parameter("topics.killswitch_topic"); this->declare_parameter("max_force"); this->max_force = this->get_parameter("max_force").as_double(); @@ -23,18 +30,24 @@ Velocity_node::Velocity_node() : Node("velocity_controller_node") this->topic_guidance = this->get_parameter("topics.guidance_topic").as_string(); this->topic_twist = this->get_parameter("topics.twist_topic").as_string(); this->topic_killswitch = this->get_parameter("topics.killswitch_topic").as_string(); - + this->topic_pose = this->get_parameter("topics.pose_topic").as_string(); + // Publishers publisher_thrust = create_publisher(topic_thrust, 10); //Subscribers - subscriber_twist = this->create_subscription( + subscriber_twist = this->create_subscription( topic_twist, 10, std::bind(&Velocity_node::twist_callback,this, std::placeholders::_1)); - subscriber_guidance = this->create_subscription( + + subscriber_pose = this->create_subscription( + topic_pose, 10, + std::bind(&Velocity_node::pose_callback,this, std::placeholders::_1)); + + subscriber_guidance = this->create_subscription( topic_guidance,10, std::bind(&Velocity_node::guidance_callback,this, std::placeholders::_1)); - subscriber_killswitch = this->create_subscription( + subscriber_killswitch = this->create_subscription( topic_killswitch,10, std::bind(&Velocity_node::killswitch_callback,this, std::placeholders::_1)); @@ -43,9 +56,12 @@ Velocity_node::Velocity_node() : Node("velocity_controller_node") this->declare_parameter("publish_rate"); this->calculation_rate = this->get_parameter("calculation_rate").as_int(); this->publish_rate = this->get_parameter("publish_rate").as_int(); - timer_PID = this->create_wall_timer(std::chrono::milliseconds(calculation_rate), std::bind(&Velocity_node::publish_thrust, this)); + timer_calculation = this->create_wall_timer(std::chrono::milliseconds(calculation_rate), std::bind(&Velocity_node::publish_thrust, this)); timer_publish = this->create_wall_timer(std::chrono::milliseconds(publish_rate), std::bind(&Velocity_node::calc_thrust, this)); + PID_surge.set_output_limits(-max_force, max_force); + PID_pitch.set_output_limits(-max_force, max_force); + PID_yaw.set_output_limits(-max_force, max_force); } @@ -53,51 +69,49 @@ Velocity_node::Velocity_node() : Node("velocity_controller_node") //Publish/timer functions void Velocity_node::publish_thrust() { - publisher_thrust->publish(thrust); + publisher_thrust->publish(thrust_out); } //** må forbedre integrasjon og derivasjons beregningene void Velocity_node::calc_thrust() { - auto error_x = reference.wrench.force.x - current_twist.wrench.force.x; - /*if (!(thrust.wrench.force.x == max_force || thrust.wrench.force.x == -max_force)){ - integral += error_x * (calculation_rate / 1000.0); //anti windup - }*/ - integral += error_x * (calculation_rate / 1000.0); - double derivative = (error_x - previous_error) / (calculation_rate / 1000.0); - previous_error = error_x; - - thrust.wrench.force.x = k_p*error_x + k_i*integral+k_d*derivative; - if (thrust.wrench.force.x > max_force){ - thrust.wrench.force.x = max_force; - } - else if (thrust.wrench.force.x < -max_force){ - thrust.wrench.force.x = -max_force; - } + PID_surge.calculate_thrust(reference.surge, current_state.surge,calculation_rate/1000.0); + PID_pitch.calculate_thrust(reference.pitch, current_state.pitch,calculation_rate/1000.0); + PID_yaw.calculate_thrust(reference.yaw, current_state.yaw,calculation_rate/1000.0); + thrust_out.wrench.force.x = PID_surge.output(); + thrust_out.wrench.torque.y = PID_pitch.output(); + thrust_out.wrench.torque.z = PID_yaw.output(); + return; } //Callback functions -void Velocity_node::guidance_callback(const geometry_msgs::msg::WrenchStamped::SharedPtr msg_ptr){ +void Velocity_node::guidance_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg_ptr){ //RCLCPP_INFO(this->get_logger(), "Received reference: '%f'", msg_ptr->wrench.force.x); reference = *msg_ptr; return; } -void Velocity_node::twist_callback(const geometry_msgs::msg::WrenchStamped::SharedPtr msg_ptr){ +void Velocity_node::twist_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg_ptr){ //RCLCPP_INFO(this->get_logger(), "Received velocity and orientation: '%f'", msg_ptr->wrench.force.x); - current_twist = *msg_ptr; + current_state.surge = msg_ptr->twist.twist.linear.x; + return; +} +void Velocity_node::pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg_ptr){ + angle temp=quaternion_to_euler_angle(msg_ptr->pose.pose.orientation.w, msg_ptr->pose.pose.orientation.x, msg_ptr->pose.pose.orientation.y, msg_ptr->pose.pose.orientation.z); + current_state.pitch = temp.thetat; + current_state.yaw = temp.psit; return; } //**Needs to update to shutdown the node -void Velocity_node::killswitch_callback(const geometry_msgs::msg::WrenchStamped::SharedPtr msg_ptr){ - RCLCPP_INFO(this->get_logger(), "Received killswitch: '%f'", msg_ptr->wrench.force.x); - if(msg_ptr->wrench.force.x == 1.0){ - reference = geometry_msgs::msg::WrenchStamped(); - current_twist = geometry_msgs::msg::WrenchStamped(); - RCLCPP_INFO(this->get_logger(), "Killswitch activated, reference and current twist set to zero"); +void Velocity_node::killswitch_callback(const std_msgs::msg::Bool::SharedPtr msg_ptr){ + RCLCPP_INFO(this->get_logger(), "Received killswitch: '%d'", msg_ptr->data); + if(msg_ptr->data == true){ + reference = guidance_data(); + current_state = guidance_data(); + RCLCPP_INFO(this->get_logger(), "Killswitch activated, reference and current state set to zero"); } return; } @@ -132,4 +146,39 @@ geometry_msgs::msg::WrenchStamped operator+(const geometry_msgs::msg::WrenchStam result.wrench.torque.y = a.wrench.torque.y + b.wrench.torque.y; result.wrench.torque.z = a.wrench.torque.z + b.wrench.torque.z; return result; -} \ No newline at end of file +} +//operator overloading for guidance_data +guidance_data guidance_data::operator-(const guidance_data & b) const +{ + guidance_data result; + result.surge = this->surge - b.surge; + result.pitch = this->pitch - b.pitch; + result.yaw = this->yaw - b.yaw; + return result; +} + +guidance_data& guidance_data::operator=(const std_msgs::msg::Float64MultiArray& msg) +{ + if (msg.data.size()>=3){ + surge=msg.data[0]; + pitch=msg.data[1]; + yaw=msg.data[2]; + } + else{ + //throw std::runtime_error("Guidance message too short, needs at least 3 values"); + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Guidance message too short, needs at least 3 values"); + } + return *this; +} + +guidance_data::guidance_data(std_msgs::msg::Float64MultiArray msg){ + if (msg.data.size()>=3){ + surge=msg.data[0]; + pitch=msg.data[1]; + yaw=msg.data[2]; + } + else{ + //throw std::runtime_error("Guidance message too short, needs at least 3 values"); + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Guidance message too short, needs at least 3 values"); + } + }; \ No newline at end of file From 5e8e77abd077b7a9a472ca088a2ad51ea57ede2d Mon Sep 17 00:00:00 2001 From: Henrik Date: Sun, 12 Oct 2025 11:20:11 +0200 Subject: [PATCH 010/128] New get_paramters function, changed topics in test_node --- control/velocity_controller/CMakeLists.txt | 134 +++++++++++++----- .../include/velocity_controller/PID_setup.hpp | 12 ++ .../include/velocity_controller/test_VC.hpp | 47 ++++++ .../velocity_controller.hpp | 12 +- control/velocity_controller/src/test_VC.cpp | 128 +++++++++++------ .../src/velocity_controller.cpp | 42 +++--- 6 files changed, 270 insertions(+), 105 deletions(-) create mode 100644 control/velocity_controller/include/velocity_controller/test_VC.hpp diff --git a/control/velocity_controller/CMakeLists.txt b/control/velocity_controller/CMakeLists.txt index aafca1823..424df4baf 100644 --- a/control/velocity_controller/CMakeLists.txt +++ b/control/velocity_controller/CMakeLists.txt @@ -1,10 +1,15 @@ cmake_minimum_required(VERSION 3.8) project(velocity_controller) +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 20) +endif() + if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() + # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) @@ -13,42 +18,29 @@ find_package(vortex_msgs REQUIRED) find_package(Eigen3 REQUIRED) find_package(drake REQUIRED) +include_directories( + ${EIGEN3_INCLUDE_DIR} + ${drake_INCLUDE_DIRS} + include +) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -ament_package() - -add_executable(velocity_controller_node src/velocity_controller.cpp src/PID_setup.cpp) -add_executable(test_VC_node src/test_VC.cpp) +#set(LIB_NAME "${PROJECT_NAME}_component") -target_include_directories(velocity_controller_node PUBLIC - $ - $ -) -include_directories(${EIGEN3_INCLUDE_DIR}) -include_directories(${drake_INCLUDE_DIRS}) -target_link_libraries(velocity_controller_node ${drake_LIBRARIES}) -install(DIRECTORY launch - DESTINATION share/${PROJECT_NAME}/ -) -install(TARGETS velocity_controller_node - DESTINATION lib/${PROJECT_NAME} -) -install(TARGETS test_VC_node - DESTINATION lib/${PROJECT_NAME} +#add_library(${LIB_NAME} SHARED +# src/LQR_setup.cpp +# src/PID_setup.cpp +# src/test_VC.cpp +# src/velocity_controller.cpp +#) +add_executable(velocity_controller_node + src/velocity_controller.cpp + src/PID_setup.cpp ) -install(DIRECTORY config - DESTINATION share/${PROJECT_NAME}/ + +add_executable(test_VC_node + src/test_VC.cpp + src/PID_setup.cpp +# src/velocity_controller.cpp ) ament_target_dependencies(velocity_controller_node @@ -56,10 +48,86 @@ ament_target_dependencies(velocity_controller_node std_msgs vortex_msgs geometry_msgs + Eigen3 ) + ament_target_dependencies(test_VC_node rclcpp std_msgs vortex_msgs geometry_msgs + Eigen3 +) + +install(TARGETS + velocity_controller_node + test_VC_node + DESTINATION lib/${PROJECT_NAME} +) + + +#rclcpp_components_register_node( +# ${LIB_NAME} +# PLUGIN "velocity_controller_node" +# EXECUTABLE ${PROJECT_NAME}_node +#) +#ament_export_targets(export_${LIB_NAME}) +#install(TARGETS ${LIB_NAME} +# EXPORT export_${LIB_NAME} +# ARCHIVE DESTINATION lib +# LIBRARY DESTINATION lib +# RUNTIME DESTINATION bin +#) +install(TARGETS + velocity_controller_node + test_VC_node + DESTINATION lib/${PROJECT_NAME} +) +install( + DIRECTORY include/ + DESTINATION include ) + +install(DIRECTORY + launch + config + DESTINATION share/${PROJECT_NAME}/ +) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() + #add_subdirectory(test) +endif() + +ament_package() + +#add_executable(velocity_controller_node src/velocity_controller.cpp src/PID_setup.cpp) +#add_executable(test_VC_node src/test_VC.cpp src/PID_setup.cpp src/velocity_controller.cpp) + +#target_include_directories(velocity_controller_node PUBLIC +# $ +# $ +#) + +#target_link_libraries(velocity_controller_node ${drake_LIBRARIES}) +#install(DIRECTORY launch +# DESTINATION share/${PROJECT_NAME}/ +#) +#install(TARGETS velocity_controller_node +# DESTINATION lib/${PROJECT_NAME} +#) +#install(TARGETS test_VC_node +# DESTINATION lib/${PROJECT_NAME} +#) +#install(DIRECTORY config +# DESTINATION share/${PROJECT_NAME}/ +#) + + diff --git a/control/velocity_controller/include/velocity_controller/PID_setup.hpp b/control/velocity_controller/include/velocity_controller/PID_setup.hpp index 10f42401a..c91076466 100644 --- a/control/velocity_controller/include/velocity_controller/PID_setup.hpp +++ b/control/velocity_controller/include/velocity_controller/PID_setup.hpp @@ -2,6 +2,7 @@ #include #include +#include #include class PID_controller { @@ -30,3 +31,14 @@ class angle{ double psit=0.0; }; angle quaternion_to_euler_angle(double w, double x, double y, double z); + +class guidance_data{ + public: + double surge; double pitch; double yaw; + guidance_data(std_msgs::msg::Float64MultiArray msg); + guidance_data(double surge, double pitch, double yaw):surge(surge), pitch(pitch), yaw(yaw) {}; + guidance_data():surge(0), pitch(0), yaw(0) {}; + + guidance_data operator-(const guidance_data& other) const; + guidance_data& operator=(const std_msgs::msg::Float64MultiArray& msg); +}; diff --git a/control/velocity_controller/include/velocity_controller/test_VC.hpp b/control/velocity_controller/include/velocity_controller/test_VC.hpp new file mode 100644 index 000000000..474f636a3 --- /dev/null +++ b/control/velocity_controller/include/velocity_controller/test_VC.hpp @@ -0,0 +1,47 @@ +#pragma once + +#include +#include +#include +#include +#include +#include "velocity_controller/PID_setup.hpp" +#include "velocity_controller/velocity_controller.hpp" +#include +#include + +class test_VC : public rclcpp::Node{ + public: + test_VC(); + //Callback functions + void read_thrust(geometry_msgs::msg::WrenchStamped::SharedPtr msg); + void send_guidance(); + void send_state(); + + //Variables + //guidance_data reference; + guidance_data current_state; + //Subscribers and publishers + rclcpp::Publisher::SharedPtr publisher_guidance; + rclcpp::Publisher::SharedPtr publisher_twist; + rclcpp::Publisher::SharedPtr publisher_pose; + rclcpp::Subscription::SharedPtr subscription_thrust; + //Timers + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Clock::SharedPtr clock_; + //Messages + std::vector thrust_vector; + std_msgs::msg::Float64MultiArray reference_msg; + + //Topics + std::string topic_twist; + std::string topic_pose; + std::string topic_thrust; + std::string topic_guidance; + + //MSGS + geometry_msgs::msg::TwistWithCovarianceStamped twist_msg; + geometry_msgs::msg::PoseWithCovarianceStamped pose_msg; +}; + +geometry_msgs::msg::Quaternion euler_angle_to_quaternion(double roll, double pitch, double yaw); \ No newline at end of file diff --git a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp index 1d14a0299..044225728 100644 --- a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp +++ b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp @@ -12,19 +12,13 @@ //#include "vortex-msgs/msg" kan legge til nye meldinger nå -class guidance_data{ - public: - double surge; double pitch; double yaw; - guidance_data(std_msgs::msg::Float64MultiArray msg); - guidance_data():surge(0.0), pitch(0.0), yaw(0.0) {}; - - guidance_data operator-(const guidance_data& other) const; - guidance_data& operator=(const std_msgs::msg::Float64MultiArray& msg); -}; + class Velocity_node : public rclcpp::Node{ public: Velocity_node(); + //Different initializatin functions + void get_new_parameters(); //Timer functions void publish_thrust(); diff --git a/control/velocity_controller/src/test_VC.cpp b/control/velocity_controller/src/test_VC.cpp index 3c3e153e8..c6ccfdec1 100644 --- a/control/velocity_controller/src/test_VC.cpp +++ b/control/velocity_controller/src/test_VC.cpp @@ -2,61 +2,80 @@ #include #include #include -#include +#include +#include "velocity_controller/PID_setup.hpp" +#include "velocity_controller/test_VC.hpp" +#include +#include +//#include "velocity_controller/velocity_controller.hpp" //#include "LQR_setup.hpp" //Denne noden er kun for å teste velocity_controller noden -class test_VC : public rclcpp::Node{ - public: - test_VC() : Node("test_VC_node") - { - this->declare_parameter("topics.guidance_topic"); - topic_guidance=this->get_parameter("topics.guidance_topic").as_string(); - publisher_ = this->create_publisher(topic_guidance,10); - this->declare_parameter("topics.thrust_topic"); - topic_subscription = this->get_parameter("topics.thrust_topic").as_string(); - subscription_ = this->create_subscription( - topic_subscription, 10, - std::bind(&test_VC::listen, this, std::placeholders::_1)); - timer_ = this->create_wall_timer( - std::chrono::milliseconds(500), - std::bind(&test_VC::send_velocity, this)); - clock_ = this->get_clock(); - thrust_pub = this->create_publisher("thrust_value", 10); +test_VC::test_VC() : Node("test_VC_node"), current_state(0,2,2) +{ + this->declare_parameter("topics.guidance_topic"); + this->declare_parameter("topics.thrust_topic"); + this->declare_parameter("topics.twist_topic"); + this->declare_parameter("topics.pose_topic"); + topic_thrust = this->get_parameter("topics.thrust_topic").as_string(); + topic_twist = this->get_parameter("topics.twist_topic").as_string(); + topic_pose = this->get_parameter("topics.pose_topic").as_string(); + + topic_guidance = this->get_parameter("topics.guidance_topic").as_string(); + publisher_guidance = this->create_publisher(topic_guidance, 10); + publisher_twist = this->create_publisher(topic_twist,10); + publisher_pose = this->create_publisher(topic_pose,10); + + subscription_thrust = this->create_subscription( + topic_thrust, 10, + std::bind(&test_VC::read_thrust, this, std::placeholders::_1)); + timer_ = this->create_wall_timer( + std::chrono::milliseconds(200), + std::bind(&test_VC::send_guidance, this)); + clock_ = this->get_clock(); + RCLCPP_INFO(this->get_logger(), "Test_VC node has been started"); + reference_msg.data={2.0, 0.0, 0.0}; //Surge, pitch, yaw + +} + +void test_VC::send_guidance() +{ + publisher_guidance->publish(reference_msg); + send_state(); +} + +void test_VC::read_thrust(geometry_msgs::msg::WrenchStamped::SharedPtr msg) +{ + current_state.surge += 0.01 * msg->wrench.force.x; + current_state.pitch += 0.01 * msg->wrench.torque.x; + current_state.yaw += 0.01 * msg->wrench.torque.y; + RCLCPP_INFO(this->get_logger(),"info: '%f'", current_state.surge); + RCLCPP_INFO(this->get_logger(),"info: '%f'", current_state.pitch); + RCLCPP_INFO(this->get_logger(),"info: '%f'", current_state.yaw); + return; +} - RCLCPP_INFO(this->get_logger(), "Test_VC node has been started"); +void test_VC::send_state() +{ + + twist_msg.header.stamp = clock_->now(); + twist_msg.header.frame_id = "base_link"; + twist_msg.twist.twist.linear.x = current_state.surge; - message.wrench.force.x=1; - } + pose_msg.header.stamp = clock_->now(); + pose_msg.header.frame_id = "base_link"; + pose_msg.pose.pose.orientation = euler_angle_to_quaternion(0.0, current_state.pitch, current_state.yaw); - rclcpp::Publisher::SharedPtr publisher_; - rclcpp::Subscription::SharedPtr subscription_; - rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Clock::SharedPtr clock_; - std::string topic_guidance; - std::string topic_subscription; - std::vector thrust_vector; - rclcpp::Publisher::SharedPtr thrust_pub; - geometry_msgs::msg::WrenchStamped message; + publisher_twist->publish(twist_msg); + publisher_pose->publish(pose_msg); - void send_velocity() - { - message.wrench.force.x = std::sin(clock_->now().seconds()*2*3.14159/10); //sinuskurve med periode 10 sekunder og amplitude 1 - publisher_->publish(message); - } + //RCLCPP_INFO(this->get_logger(), "Published state: '%f'", current_state.surge); + return; + //RCLCPP_INFO(this->get_logger(), "Published state: '%f'", current_state.pitch); + //RCLCPP_INFO(this->get_logger(), "Published state: '%f'", current_state.yaw); +} - void listen(geometry_msgs::msg::WrenchStamped::SharedPtr msg) - { - thrust_vector.push_back(msg->wrench.force.x); - std_msgs::msg::Float64 pub_info; - pub_info.data = thrust_vector.back(); - thrust_pub->publish(pub_info); - message.wrench.force.x+=0.01*msg->wrench.force.x; - //RCLCPP_INFO(this->get_logger(), "Received thrust: '%f'", msg->wrench.force.x); - return; - } -}; int main(int argc, char const *argv[]) { @@ -65,3 +84,20 @@ int main(int argc, char const *argv[]) rclcpp::shutdown(); return 0; } + +geometry_msgs::msg::Quaternion euler_angle_to_quaternion(double roll, double pitch, double yaw){ + double cy = cos(yaw * 0.5); + double sy = sin(yaw * 0.5); + double cp = cos(pitch * 0.5); + double sp = sin(pitch * 0.5); + double cr = cos(roll * 0.5); + double sr = sin(roll * 0.5); + + geometry_msgs::msg::Quaternion q; + q.w = cr * cp * cy + sr * sp * sy; + q.x = sr * cp * cy - cr * sp * sy; + q.y = cr * sp * cy + sr * cp * sy; + q.z = cr * cp * sy - sr * sp * cy; + + return q; +} \ No newline at end of file diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index 42886eb87..0f38c89d4 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -19,19 +19,7 @@ Velocity_node::Velocity_node() : Node("velocity_controller_node"), PID_surge(1,1 RCLCPP_INFO(this->get_logger(), "Velocity control node has been started."); //Parameter from config. - this->declare_parameter("topics.thrust_topic"); - this->declare_parameter("topics.guidance_topic"); - this->declare_parameter("topics.twist_topic"); - this->declare_parameter("topics.pose_topic"); - this->declare_parameter("topics.killswitch_topic"); - this->declare_parameter("max_force"); - this->max_force = this->get_parameter("max_force").as_double(); - this->topic_thrust = this->get_parameter("topics.thrust_topic").as_string(); - this->topic_guidance = this->get_parameter("topics.guidance_topic").as_string(); - this->topic_twist = this->get_parameter("topics.twist_topic").as_string(); - this->topic_killswitch = this->get_parameter("topics.killswitch_topic").as_string(); - this->topic_pose = this->get_parameter("topics.pose_topic").as_string(); - + get_new_parameters(); // Publishers publisher_thrust = create_publisher(topic_thrust, 10); @@ -52,16 +40,15 @@ Velocity_node::Velocity_node() : Node("velocity_controller_node"), PID_surge(1,1 std::bind(&Velocity_node::killswitch_callback,this, std::placeholders::_1)); //Timer - this->declare_parameter("calculation_rate"); - this->declare_parameter("publish_rate"); - this->calculation_rate = this->get_parameter("calculation_rate").as_int(); - this->publish_rate = this->get_parameter("publish_rate").as_int(); + timer_calculation = this->create_wall_timer(std::chrono::milliseconds(calculation_rate), std::bind(&Velocity_node::publish_thrust, this)); timer_publish = this->create_wall_timer(std::chrono::milliseconds(publish_rate), std::bind(&Velocity_node::calc_thrust, this)); PID_surge.set_output_limits(-max_force, max_force); PID_pitch.set_output_limits(-max_force, max_force); PID_yaw.set_output_limits(-max_force, max_force); + this->calculation_rate = this->get_parameter("calculation_rate").as_int(); + this->publish_rate = this->get_parameter("publish_rate").as_int(); } @@ -115,6 +102,27 @@ void Velocity_node::killswitch_callback(const std_msgs::msg::Bool::SharedPtr msg } return; } + + +void Velocity_node::get_new_parameters(){ + this->declare_parameter("topics.thrust_topic"); + this->topic_thrust = this->get_parameter("topics.thrust_topic").as_string(); + this->declare_parameter("topics.guidance_topic"); + this->topic_guidance = this->get_parameter("topics.guidance_topic").as_string(); + this->declare_parameter("topics.twist_topic"); + this->topic_twist = this->get_parameter("topics.twist_topic").as_string(); + this->declare_parameter("topics.pose_topic"); + this->topic_pose = this->get_parameter("topics.pose_topic").as_string(); + this->declare_parameter("topics.killswitch_topic"); + this->topic_killswitch = this->get_parameter("topics.killswitch_topic").as_string(); + this->declare_parameter("max_force"); + this->max_force = this->get_parameter("max_force").as_double(); + this->declare_parameter("calculation_rate"); + this->calculation_rate = this->get_parameter("calculation_rate").as_int(); + this->declare_parameter("publish_rate"); + this->publish_rate = this->get_parameter("publish_rate").as_int(); +} + int main(int argc, char * argv[]) { rclcpp::init(argc, argv); From 2fb9d916e804f2275905e41cbd49010993198868 Mon Sep 17 00:00:00 2001 From: Anbit Date: Sun, 19 Oct 2025 15:56:52 +0200 Subject: [PATCH 011/128] nye versjonen av guidance --- .../CMakeLists.txt | 61 ++++++ .../action/LOSGuidance.action | 10 + .../config/guidanceParamsa.yaml | 10 + .../guidance_velocity_controller/guidance.hpp | 80 +++++++ .../guidance_ros.hpp | 73 +++++++ .../launch/launch_Guidance.py | 32 +++ .../guidance_velocity_controller/package.xml | 34 +++ .../src/guidance.cpp | 89 ++++++++ .../src/guidance_node.cpp | 14 ++ .../src/guidance_ros.cpp | 200 ++++++++++++++++++ 10 files changed, 603 insertions(+) create mode 100644 guidance/guidance_velocity_controller/CMakeLists.txt create mode 100644 guidance/guidance_velocity_controller/action/LOSGuidance.action create mode 100755 guidance/guidance_velocity_controller/config/guidanceParamsa.yaml create mode 100755 guidance/guidance_velocity_controller/include/guidance_velocity_controller/guidance.hpp create mode 100755 guidance/guidance_velocity_controller/include/guidance_velocity_controller/guidance_ros.hpp create mode 100755 guidance/guidance_velocity_controller/launch/launch_Guidance.py create mode 100644 guidance/guidance_velocity_controller/package.xml create mode 100755 guidance/guidance_velocity_controller/src/guidance.cpp create mode 100755 guidance/guidance_velocity_controller/src/guidance_node.cpp create mode 100755 guidance/guidance_velocity_controller/src/guidance_ros.cpp diff --git a/guidance/guidance_velocity_controller/CMakeLists.txt b/guidance/guidance_velocity_controller/CMakeLists.txt new file mode 100644 index 000000000..7945b90be --- /dev/null +++ b/guidance/guidance_velocity_controller/CMakeLists.txt @@ -0,0 +1,61 @@ +cmake_minimum_required(VERSION 3.8) +project(guidance_velocity_controller) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 20) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(vortex_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(spdlog REQUIRED) +find_package(fmt REQUIRED) + +include_directories(include) + +add_executable(guidance_node + src/guidance_node.cpp + src/guidance_ros.cpp + src/guidance.cpp +) + +ament_target_dependencies(guidance_node + rclcpp + rclcpp_action + geometry_msgs + vortex_msgs + Eigen3 + tf2 + tf2_geometry_msgs + spdlog + fmt +) + +target_link_libraries(guidance_node fmt::fmt) + +install(TARGETS + guidance_node + DESTINATION lib/${PROJECT_NAME} +) + +install( + DIRECTORY include/ + DESTINATION include +) + +install(DIRECTORY + launch + config + DESTINATION share/${PROJECT_NAME}/ +) + +ament_package() \ No newline at end of file diff --git a/guidance/guidance_velocity_controller/action/LOSGuidance.action b/guidance/guidance_velocity_controller/action/LOSGuidance.action new file mode 100644 index 000000000..49c487775 --- /dev/null +++ b/guidance/guidance_velocity_controller/action/LOSGuidance.action @@ -0,0 +1,10 @@ +# Goal: +geometry_msgs/PointStamped goal + +--- +# Result: +bool success + +--- +# Feedback: +std_msgs/Float64MultiArray feedback diff --git a/guidance/guidance_velocity_controller/config/guidanceParamsa.yaml b/guidance/guidance_velocity_controller/config/guidanceParamsa.yaml new file mode 100755 index 000000000..5a333a316 --- /dev/null +++ b/guidance/guidance_velocity_controller/config/guidanceParamsa.yaml @@ -0,0 +1,10 @@ +/**: + ros__parameters: + lookahead_distance_h = 1.5; + lookahead_distance_v = 0.6; + gamma_h = 0.05; + gamma_v = 0.1; + time_step = 0.01; + u_desired = 0.3; + u_min = 0.1; + d_scale = 1.09; \ No newline at end of file diff --git a/guidance/guidance_velocity_controller/include/guidance_velocity_controller/guidance.hpp b/guidance/guidance_velocity_controller/include/guidance_velocity_controller/guidance.hpp new file mode 100755 index 000000000..26ca12642 --- /dev/null +++ b/guidance/guidance_velocity_controller/include/guidance_velocity_controller/guidance.hpp @@ -0,0 +1,80 @@ +#pragma once +#include +#include +#include +#include + +extern std::vector g_waypoints; + +namespace LOS { + +struct Point { + double x; + double y; + double z; + + Point operator-(const Point& other) const { + return Point{x - other.x, y - other.y, z - other.z}; + } + + Eigen::Vector3d as_vector() const { return Eigen::Vector3d(x, y, z); } +}; + +struct CrossTrackError { + double x_e; + double y_e; + double z_e; + + + inline static CrossTrackError from_vector(const Eigen::Vector3d& v) { + return CrossTrackError{v.x(), v.y(), v.z()}; + } +}; + +} + +class LOSGuidance { +public: + LOSGuidance( + double lookahead_distance_h_, + double lookahead_distance_v_, + double gamma_h_, + double gamma_v_, + double time_step_, + double u_desired_, + double u_min_, + double d_scale_); + ~LOSGuidance() = default; + + void update_angles(const LOS::Point& new_point, const LOS::Point& prev_point); + void cross_track_error(const LOS::Point& current_position, const LOS::Point& prev_point); + double desired_surge_speed(const LOS::Point& destination , const LOS::Point& current_position) ; + double desired_heading(); + double desired_pitch(); + void update_adaptive_estimates(); + + Eigen::Vector3d get_outputs() const; + LOS::CrossTrackError& cross_track(); + +private: + + double lookahead_distance_h; + double lookahead_distance_v; + double gamma_h; + double gamma_v; + double time_step; + double u_desired; + double u_min; + double d_scale; + + Eigen::Matrix3d rotation_y_; + Eigen::Matrix3d rotation_z_; + + LOS::CrossTrackError cte; + + double pi_h_; + double pi_v_; + + double beta_c_hat_ = 0.0; + double alpha_c_hat_ = 0.0; +}; diff --git a/guidance/guidance_velocity_controller/include/guidance_velocity_controller/guidance_ros.hpp b/guidance/guidance_velocity_controller/include/guidance_velocity_controller/guidance_ros.hpp new file mode 100755 index 000000000..0c5a66b97 --- /dev/null +++ b/guidance/guidance_velocity_controller/include/guidance_velocity_controller/guidance_ros.hpp @@ -0,0 +1,73 @@ +#pragma once +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include + +#include + +#include +#include +#include + +class GuidanceNode : public rclcpp::Node { +public: + explicit GuidanceNode(); + +private: + // init + void establish_communications(); + void setup_parameters(); + + // io + void waypoint_callback(const geometry_msgs::msg::PointStamped::SharedPtr msg); + void pose_callback(const geometry_msgs::msg::PoseStamped::SharedPtr msg); + void publish_setpoints(); + vortex_msgs::msg::LOSGuidance fill_los_reference(); + void fill_los_waypoints(const geometry_msgs::msg::PointStamped & g_waypoints); + + // actions + using GoalHandle = rclcpp_action::ServerGoalHandle; + rclcpp_action::GoalResponse handle_goal( + const rclcpp_action::GoalUUID &, + std::shared_ptr goal); + rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr goal_handle); + void handle_accepted(const std::shared_ptr goal_handle); + void execute_guidance(const std::shared_ptr goal_handle); + + // pubs/subs + rclcpp::Publisher::SharedPtr refrence_out; + rclcpp::Subscription::SharedPtr waypoint_in; + rclcpp::Subscription::SharedPtr pose_in; + rclcpp_action::Server::SharedPtr guidance_action_server; + + // guidance + std::unique_ptr adaptive_los_guidance_; + + // state + LOS::Point current_position_{}; + LOS::Point last_waypoint_{}; + LOS::Point target_waypoint_{}; + + std::mutex mutex_; + rclcpp_action::GoalUUID preempted_goal_id_{}; + std::shared_ptr goal_handle_{}; + + // timing + std::chrono::milliseconds time_step{0}; + + // outputs + double yaw_d_; + double pitch_d_; + double u_d_ ; +}; diff --git a/guidance/guidance_velocity_controller/launch/launch_Guidance.py b/guidance/guidance_velocity_controller/launch/launch_Guidance.py new file mode 100755 index 000000000..b17fd0344 --- /dev/null +++ b/guidance/guidance_velocity_controller/launch/launch_Guidance.py @@ -0,0 +1,32 @@ +from os import path + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + +adapt_params = path.join( + get_package_share_directory("guidance_velocity_controller"), + "config", + "guidanceParamsa.yaml", +) +auv_params = path.join( + get_package_share_directory("auv_setup"), + "config", + "robots", + "orca.yaml", +) + + +def generate_launch_description(): + guidance_node = Node( + package="guidance_velocity_controller", + executable="guidance_node", + name="guidance_node", + namespace="orca", + parameters=[ + auv_params, + adapt_params, + ], + output="screen", + ) + return LaunchDescription([guidance_node]) \ No newline at end of file diff --git a/guidance/guidance_velocity_controller/package.xml b/guidance/guidance_velocity_controller/package.xml new file mode 100644 index 000000000..a8b1876a0 --- /dev/null +++ b/guidance/guidance_velocity_controller/package.xml @@ -0,0 +1,34 @@ + + + + guidance_velocity_controller + 0.0.1 + LOS guidance + action server for the velocity controller. + anbitadhi + Apache-2.0 + + ament_cmake + + rclcpp + rclcpp_action + geometry_msgs + nav_msgs + tf2 + tf2_geometry_msgs + vortex_msgs + + rosidl_default_generators + rosidl_default_runtime + builtin_interfaces + + ament_lint_auto + ament_lint_common + + rosidl_interface_packages + + + ament_cmake + rosidl_interface_packages + + + diff --git a/guidance/guidance_velocity_controller/src/guidance.cpp b/guidance/guidance_velocity_controller/src/guidance.cpp new file mode 100755 index 000000000..c0a94dd66 --- /dev/null +++ b/guidance/guidance_velocity_controller/src/guidance.cpp @@ -0,0 +1,89 @@ +#include + +LOSGuidance::LOSGuidance( + double lookahead_distance_h_, + double lookahead_distance_v_, + double gamma_h_, + double gamma_v_, + double time_step_, + double u_desired_, + double u_min_, + double d_scale_) + : lookahead_distance_h(lookahead_distance_h_), + lookahead_distance_v(lookahead_distance_v_), + gamma_h(gamma_h_), + gamma_v(gamma_v_), + time_step(time_step_), + u_desired(u_desired_), + u_min(u_min_), + d_scale(d_scale_) { + rotation_y_ = Eigen::Matrix3d::Identity(); + rotation_z_ = Eigen::Matrix3d::Identity(); +}; + +void LOSGuidance::update_angles(const LOS::Point& new_point, const LOS::Point& prev_point) { + const LOS::Point difference = new_point - prev_point; + + pi_h_ = atan2(difference.y, difference.x); + pi_v_ = atan2(-difference.z, sqrt(difference.x * difference.x + difference.y * difference.y)); + + rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()).toRotationMatrix(); + rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()).toRotationMatrix(); + +}; + +void LOSGuidance::cross_track_error( + const LOS::Point& current_position, const LOS::Point& prev_point) { + const LOS::Point difference = current_position - prev_point; + const Eigen::Vector3d difference_vector = difference.as_vector(); + + const Eigen::Vector3d cross_track_error = rotation_y_.transpose() * rotation_z_.transpose() * difference_vector; + + //save the cross track error + cte = LOS::CrossTrackError::from_vector(cross_track_error); +} + +LOS::CrossTrackError& LOSGuidance::cross_track() { + return cte; +}; + +void LOSGuidance::update_adaptive_estimates() { + double beta_c_hat_dot = + gamma_h * + (lookahead_distance_h / + sqrt(lookahead_distance_h* lookahead_distance_h + + cte.y_e * cte.y_e)) * + cte.y_e; + double alpha_c_hat_dot = + gamma_v * + (lookahead_distance_v / + sqrt(lookahead_distance_v* lookahead_distance_v + + cte.z_e * cte.z_e))* + cte.z_e; + + beta_c_hat_ += beta_c_hat_dot * time_step; + alpha_c_hat_ += alpha_c_hat_dot * time_step; +} + +double LOSGuidance::desired_surge_speed(const LOS::Point& destination , const LOS::Point& current_position) { + double dx = destination.x - current_position.x; + double dy = destination.y - current_position.y; + double dz = destination.z - current_position.z; + double distance = std::sqrt(dx*dx + dy*dy + dz*dz); + + double d = u_min + (u_desired - u_min) * std::tanh(distance/d_scale); + + return d; +} + +double LOSGuidance::desired_heading() { + return pi_h_ - beta_c_hat_ - atan(cte.y_e / lookahead_distance_h); +} + +double LOSGuidance::desired_pitch() { + return pi_v_ + alpha_c_hat_ + atan(cte.z_e / lookahead_distance_v); +} + +Eigen::Vector3d LOSGuidance::get_outputs() const { + return Eigen::Vector3d::Zero(); +} diff --git a/guidance/guidance_velocity_controller/src/guidance_node.cpp b/guidance/guidance_velocity_controller/src/guidance_node.cpp new file mode 100755 index 000000000..399eafb92 --- /dev/null +++ b/guidance/guidance_velocity_controller/src/guidance_node.cpp @@ -0,0 +1,14 @@ +#include +#include + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::executors::MultiThreadedExecutor exec; + exec.add_node(node); + exec.spin(); + rclcpp::shutdown(); + return 0; +} + diff --git a/guidance/guidance_velocity_controller/src/guidance_ros.cpp b/guidance/guidance_velocity_controller/src/guidance_ros.cpp new file mode 100755 index 000000000..287040439 --- /dev/null +++ b/guidance/guidance_velocity_controller/src/guidance_ros.cpp @@ -0,0 +1,200 @@ +#include + +GuidanceNode::GuidanceNode() : Node("guidance_node") { + establish_communications(); + setup_parameters(); + spdlog::info("LOS guidance node initialized"); +} + +void GuidanceNode::establish_communications() { + this->declare_parameter("topics.pose"); + this->declare_parameter("topics.guidance.los"); + this->declare_parameter("topics.waypoint"); + this->declare_parameter("action_servers.los"); + + const std::string pose_topic = this->get_parameter("topics.pose").as_string(); + const std::string guidance_topic = this->get_parameter("topics.guidance.los").as_string(); + const std::string waypoint_topic = this->get_parameter("topics.waypoint").as_string(); + const std::string actions_server = this->get_parameter("action_servers.los").as_string(); + + refrence_out = this->create_publisher(guidance_topic, 10); + + waypoint_in = this->create_subscription( + waypoint_topic, 10, + std::bind(&GuidanceNode::waypoint_callback, this, std::placeholders::_1)); + + pose_in = this->create_subscription( + pose_topic, 10, + std::bind(&GuidanceNode::pose_callback, this, std::placeholders::_1)); + + guidance_action_server = rclcpp_action::create_server( + this, + actions_server, + std::bind(&GuidanceNode::handle_goal, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&GuidanceNode::handle_cancel, this, std::placeholders::_1), + std::bind(&GuidanceNode::handle_accepted, this, std::placeholders::_1)); +} + +void GuidanceNode::setup_parameters() { + this->declare_parameter("lookahead_distance_h"); + this->declare_parameter("lookahead_distance_v"); + this->declare_parameter("gamma_h"); + this->declare_parameter("gamma_v"); + this->declare_parameter("time_step"); // seconds + this->declare_parameter("u_desired"); // m/s + this->declare_parameter("u_min"); + this->declare_parameter("d_scale"); + + const double lookahead_distance_h_ = this->get_parameter("lookahead_distance_h").as_double(); + const double lookahead_distance_v_ = this->get_parameter("lookahead_distance_v").as_double(); + const double gamma_h_ = this->get_parameter("gamma_h").as_double(); + const double gamma_v_ = this->get_parameter("gamma_v").as_double(); + const double time_step_s = this->get_parameter("time_step").as_double(); + const double u_desired_param = this->get_parameter("u_desired").as_double(); + const double u_min_ = this->get_parameter("u_min").as_double(); + const double d_scale_ = this->get_parameter("d_scale").as_double(); + + // Construct guidance object + adaptive_los_guidance_ = std::make_unique( + lookahead_distance_h_, + lookahead_distance_v_, + gamma_h_, + gamma_v_, + time_step_s, + u_desired_param, + u_min_, + d_scale_); + + // Convert seconds -> milliseconds for loop timing + time_step = std::chrono::milliseconds(static_cast(std::round(time_step_s * 1000.0))); +} + + +void GuidanceNode::fill_los_waypoints(const geometry_msgs::msg::PointStamped & msg) { + last_waypoint_.x = current_position_.x; + last_waypoint_.y = current_position_.y; + last_waypoint_.z = current_position_.z; + + target_waypoint_.x = msg.point.x; + target_waypoint_.y = msg.point.y; + target_waypoint_.z = msg.point.z; +} + +void GuidanceNode::waypoint_callback(const geometry_msgs::msg::PointStamped::SharedPtr g_waypoints) { + fill_los_waypoints(*g_waypoints); + // NOTE: update_angles(prev, next) + adaptive_los_guidance_->update_angles(last_waypoint_, target_waypoint_); + spdlog::info("Waypoint received"); +} + +void GuidanceNode::pose_callback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) { + current_position_.x = msg->pose.position.x; + current_position_.y = msg->pose.position.y; + current_position_.z = msg->pose.position.z; +} + +rclcpp_action::GoalResponse GuidanceNode::handle_goal( + const rclcpp_action::GoalUUID &, + std::shared_ptr goal) { + (void)goal; + { + std::lock_guard lock(mutex_); + if (goal_handle_ && goal_handle_->is_active()) { + spdlog::info("Aborting current goal and accepting new goal"); + preempted_goal_id_ = goal_handle_->get_goal_id(); + } + } + spdlog::info("Accepted goal request"); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse GuidanceNode::handle_cancel(const std::shared_ptr goal_handle) { + spdlog::info("Received request to cancel goal"); + (void)goal_handle; + return rclcpp_action::CancelResponse::ACCEPT; +} + +void GuidanceNode::handle_accepted( + const std::shared_ptr< + rclcpp_action::ServerGoalHandle> + goal_handle) { + execute_guidance(goal_handle); +} + +vortex_msgs::msg::LOSGuidance GuidanceNode::fill_los_reference() { + vortex_msgs::msg::LOSGuidance reference_msg; + reference_msg.pitch = pitch_d_; + reference_msg.yaw = yaw_d_; + reference_msg.surge = u_d_; + return reference_msg; +} + +void GuidanceNode::execute_guidance( + const std::shared_ptr< + rclcpp_action::ServerGoalHandle> + goal_handle) { + { + std::lock_guard lock(mutex_); + goal_handle_ = goal_handle; + } + + const geometry_msgs::msg::PointStamped goal_to_go = goal_handle->get_goal()->goal; + fill_los_waypoints(goal_to_go); + // correct member + argument order: (prev, next) + adaptive_los_guidance_->update_angles(last_waypoint_, target_waypoint_); + + const double loop_hz = + (time_step.count() > 0) ? (1000.0 / static_cast(time_step.count())) : 10.0; + rclcpp::Rate loop_rate(loop_hz); + + auto feedback = std::make_shared(); + auto result = std::make_shared(); + + while (rclcpp::ok()) { + // Preemption / cancel checks + { + std::lock_guard lock(mutex_); + if (goal_handle_ != goal_handle) { + spdlog::info("New goal accepted, preempting current goal"); + result->success = false; + goal_handle->canceled(result); + return; + } + if (goal_handle->is_canceling()) { + spdlog::info("Goal canceled"); + result->success = false; + goal_handle->canceled(result); + return; + } + } + + // Guidance update + adaptive_los_guidance_->cross_track_error(last_waypoint_, current_position_); + yaw_d_ = adaptive_los_guidance_->desired_heading(); + pitch_d_ = adaptive_los_guidance_->desired_pitch(); + u_d_ = adaptive_los_guidance_->desired_surge_speed(target_waypoint_, current_position_); + adaptive_los_guidance_->update_adaptive_estimates(); + + // Publish feedback + reference + const vortex_msgs::msg::LOSGuidance reference_msg = fill_los_reference(); + feedback->feedback = reference_msg; + goal_handle->publish_feedback(feedback); + refrence_out->publish(reference_msg); + + // Goal check using explicit distance calc + const double dx = current_position_.x - target_waypoint_.x; + const double dy = current_position_.y - target_waypoint_.y; + const double dz = current_position_.z - target_waypoint_.z; + const double dist = std::sqrt(dx * dx + dy * dy + dz * dz); + if (dist <= 0.5) { + result->success = true; + goal_handle->succeed(result); + refrence_out->publish(fill_los_reference()); + spdlog::info("Goal reached"); + return; + } + + loop_rate.sleep(); + } +} + From 40eb88fa0d90d3d40e22d8838e56bbf6dcf4e8cf Mon Sep 17 00:00:00 2001 From: Henrik Date: Sun, 19 Oct 2025 17:07:37 +0200 Subject: [PATCH 012/128] fixed code so that it likely could compile if I had Drake. Made a Utility file --- control/velocity_controller/CMakeLists.txt | 7 ++ .../config/parameters.yaml | 7 +- .../include/velocity_controller/LQR_setup.hpp | 51 +++++++----- .../include/velocity_controller/PID_setup.hpp | 18 +---- .../include/velocity_controller/test_VC.hpp | 2 +- .../include/velocity_controller/utilities.hpp | 31 +++++++ .../velocity_controller.hpp | 13 ++- control/velocity_controller/src/LQR_setup.cpp | 21 +++-- control/velocity_controller/src/PID_setup.cpp | 20 +---- control/velocity_controller/src/utilities.cpp | 21 +++++ .../src/velocity_controller.cpp | 81 +++++++++++++++---- 11 files changed, 182 insertions(+), 90 deletions(-) create mode 100644 control/velocity_controller/include/velocity_controller/utilities.hpp create mode 100644 control/velocity_controller/src/utilities.cpp diff --git a/control/velocity_controller/CMakeLists.txt b/control/velocity_controller/CMakeLists.txt index 424df4baf..2f2ea04c0 100644 --- a/control/velocity_controller/CMakeLists.txt +++ b/control/velocity_controller/CMakeLists.txt @@ -24,6 +24,8 @@ include_directories( include ) +link_directories("/opt/drake/lib/") + #set(LIB_NAME "${PROJECT_NAME}_component") #add_library(${LIB_NAME} SHARED @@ -35,11 +37,15 @@ include_directories( add_executable(velocity_controller_node src/velocity_controller.cpp src/PID_setup.cpp + src/LQR_setup.cpp + src/utilities.cpp ) add_executable(test_VC_node src/test_VC.cpp src/PID_setup.cpp + src/LQR_setup.cpp + src/utilities.cpp # src/velocity_controller.cpp ) @@ -49,6 +55,7 @@ ament_target_dependencies(velocity_controller_node vortex_msgs geometry_msgs Eigen3 + drake ) ament_target_dependencies(test_VC_node diff --git a/control/velocity_controller/config/parameters.yaml b/control/velocity_controller/config/parameters.yaml index 5be2714b8..bcdd2a151 100644 --- a/control/velocity_controller/config/parameters.yaml +++ b/control/velocity_controller/config/parameters.yaml @@ -11,9 +11,9 @@ killswitch_topic: /softwareKillSwitch #Kill Switch LQR_params: - q_surge: 75 - q_pitch: 175 - q_yaw: 175 + q_surge: 75.0 + q_pitch: 175.0 + q_yaw: 175.0 r_surge: 0.3 r_pitch: 0.4 @@ -33,4 +33,5 @@ publish_rate: 200 #ms #Clamp parameter max_force: 99.5 + controller_type: 1 #publish_value: "Hello from config!" # parameter name: parameter value diff --git a/control/velocity_controller/include/velocity_controller/LQR_setup.hpp b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp index c13e54442..4fa1fc023 100644 --- a/control/velocity_controller/include/velocity_controller/LQR_setup.hpp +++ b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp @@ -3,23 +3,19 @@ #include #include #include +#include "PID_setup.hpp" +#include +#include "velocity_controller/utilities.hpp" -class State{ - //Dataclass to store state values for LQR controller - public: - double surge=0.0; double pitch=0.0; double yaw=0.0; - double integral_surge=0.0; double integral_pitch=0.0; double integral_yaw=0.0; -}; - -class Guidance_values{ +/*class Guidance_values{ //Dataclass to store guidance values for LQR controller public: double surge=0.0; double pitch=0.0; double yaw=0.0; double integral_surge=0.0; double integral_pitch=0.0; double integral_yaw=0.0; }; - +*/ class LQRparameters{ //Dataclass to store LQR parameters public: @@ -29,30 +25,38 @@ class LQRparameters{ double i_weight=0.0; double max_force=0.0; }; -class angle{ +/*class angle{ public: double phit=0.0; double thetat=0.0; double psit=0.0; -}; + +};*/ class LQRController{ public: - LQRController(LQRparameters params,std::vector inertia_matrix); - angle quaternion_to_euler_angle(double w, double x, double y, double z); - double ssa(double angle); - std::tuple saturate (double value, bool windup, double limit); - double anti_windup(double ki, double error, double integral_sum, bool windup); - std::vector> calculate_coriolis_matrix(double pitchrate, double yaw_rate, double sway_vel, double heave_vel); + LQRController(LQRparameters params={0,0,0,0,0,0,0,0,0,0,0},std::vector inertia_matrix={0,0,0,0,0,0,0,0,0}); + + void set_params(LQRparameters params); + std::vector> calculate_coriolis_matrix(double pitchrate, double yaw_rate, double sway_vel, double heave_vel); void set_matrices(std::vector inertia_matrix); void update_augmented_matrices(std::vector > coriolis_matrix); - std::vector update_error(Guidance_values guidance_values, State states); + + //angle quaternion_to_euler_angle(double w, double x, double y, double z); + double ssa(double angle); + + std::tuple saturate (double value, bool windup, double limit); + double anti_windup(double ki, double error, double integral_sum, bool windup); std::vector saturate_input(std::vector u); - std::vector calculate_lqr_u(std::vector> coriolis_matrix, State states, Guidance_values guidance_values); + + std::vector update_error(Guidance_data guidance_values, State states); + std::vector calculate_lqr_u(std::vector> coriolis_matrix, State states, Guidance_data guidance_values); + + //Resets controller void reset_controller(); - // Variables + // VariablesEigen::Matrix3d vector_to_matrix3d(const std::vector &other_matrix) const double pi=3.14159265358979323846; double integral_error_surge; double integral_error_pitch; double integral_error_yaw; bool surge_windup; bool pitch_windup; bool yaw_windup; @@ -66,5 +70,12 @@ class LQRController{ std::vector> input_weight_matrix; std::vector> augmented_system_matrix; std::vector> augmented_input_matrix; + + }; +//Extra operations +Eigen::Matrix3d vector_to_matrix3d(const std::vector &other_matrix); +std::vector matrix3d_to_vector(const Eigen::Matrix3d &mat); +std::vector> matrix3d_to_vector2d(const Eigen::Matrix3d &mat); +Eigen::Matrix3d vector2d_to_matrix3d(const std::vector> &other_matrix); diff --git a/control/velocity_controller/include/velocity_controller/PID_setup.hpp b/control/velocity_controller/include/velocity_controller/PID_setup.hpp index c91076466..91303d5dc 100644 --- a/control/velocity_controller/include/velocity_controller/PID_setup.hpp +++ b/control/velocity_controller/include/velocity_controller/PID_setup.hpp @@ -4,6 +4,7 @@ #include #include #include +#include "utilities.hpp" class PID_controller { public: @@ -24,21 +25,4 @@ class PID_controller { double max_output; double min_output; }; -class angle{ - public: - double phit=0.0; - double thetat=0.0; - double psit=0.0; -}; -angle quaternion_to_euler_angle(double w, double x, double y, double z); -class guidance_data{ - public: - double surge; double pitch; double yaw; - guidance_data(std_msgs::msg::Float64MultiArray msg); - guidance_data(double surge, double pitch, double yaw):surge(surge), pitch(pitch), yaw(yaw) {}; - guidance_data():surge(0), pitch(0), yaw(0) {}; - - guidance_data operator-(const guidance_data& other) const; - guidance_data& operator=(const std_msgs::msg::Float64MultiArray& msg); -}; diff --git a/control/velocity_controller/include/velocity_controller/test_VC.hpp b/control/velocity_controller/include/velocity_controller/test_VC.hpp index 474f636a3..5e6c8c3da 100644 --- a/control/velocity_controller/include/velocity_controller/test_VC.hpp +++ b/control/velocity_controller/include/velocity_controller/test_VC.hpp @@ -20,7 +20,7 @@ class test_VC : public rclcpp::Node{ //Variables //guidance_data reference; - guidance_data current_state; + Guidance_data current_state; //Subscribers and publishers rclcpp::Publisher::SharedPtr publisher_guidance; rclcpp::Publisher::SharedPtr publisher_twist; diff --git a/control/velocity_controller/include/velocity_controller/utilities.hpp b/control/velocity_controller/include/velocity_controller/utilities.hpp new file mode 100644 index 000000000..82806dadf --- /dev/null +++ b/control/velocity_controller/include/velocity_controller/utilities.hpp @@ -0,0 +1,31 @@ +#pragma once +#include +#include +#include "std_msgs/msg/float64_multi_array.hpp" + + +class angle{ + public: + double phit=0.0; + double thetat=0.0; + double psit=0.0; +}; +angle quaternion_to_euler_angle(double w, double x, double y, double z); + +class State{ + //Dataclass to store state values for LQR controller + public: + double surge=0.0; double pitch=0.0; double yaw=0.0; + double integral_surge=0.0; double integral_pitch=0.0; double integral_yaw=0.0; +}; + +class Guidance_data:public State{ + public: + //double surge; double pitch; double yaw; + Guidance_data(std_msgs::msg::Float64MultiArray msg); + Guidance_data(double surge, double pitch, double yaw):State{surge, pitch, yaw} {}; + Guidance_data():State{0, 0, 0} {}; + + Guidance_data operator-(const Guidance_data& other) const; + Guidance_data& operator=(const std_msgs::msg::Float64MultiArray& msg); +}; diff --git a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp index 044225728..d9849df7c 100644 --- a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp +++ b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp @@ -8,6 +8,7 @@ #include #include #include "velocity_controller/PID_setup.hpp" +#include "LQR_setup.hpp" //#include "vortex-msgs/msg" kan legge til nye meldinger nå @@ -58,15 +59,23 @@ class Velocity_node : public rclcpp::Node{ //Stored wrenches values std_msgs::msg::Float64MultiArray reference_in; - guidance_data reference; - guidance_data current_state; + Guidance_data guidance_values; + Guidance_data current_state; geometry_msgs::msg::WrenchStamped thrust_out; + + int controller_type; //1 PID, 2 LQR + //PID controllers PID_controller PID_surge; PID_controller PID_yaw; PID_controller PID_pitch; + //LQR Controller + LQRController lqr_controller; + LQRparameters lqr_parameters; + std::vector inertia_matrix; + }; diff --git a/control/velocity_controller/src/LQR_setup.cpp b/control/velocity_controller/src/LQR_setup.cpp index 84d94c600..3025d6b5d 100644 --- a/control/velocity_controller/src/LQR_setup.cpp +++ b/control/velocity_controller/src/LQR_setup.cpp @@ -8,6 +8,8 @@ #include #include #include +#include "velocity_controller/PID_setup.hpp" +#include "velocity_controller/utilities.hpp" LQRController::LQRController(LQRparameters params,std::vector inertia_matrix){ @@ -15,7 +17,8 @@ LQRController::LQRController(LQRparameters params,std::vector inertia_ma set_matrices(inertia_matrix); }; -angle LQRController::quaternion_to_euler_angle(double w, double x, double y, double z){ + +/*angle LQRController::quaternion_to_euler_angle(double w, double x, double y, double z){ double ysqr = y * y; double t0 = +2.0 * (w * x + y * z); @@ -32,7 +35,7 @@ angle LQRController::quaternion_to_euler_angle(double w, double x, double y, dou double psi = std::atan2(t3, t4); return {phi, theta, psi}; -}; +};*/ double LQRController::ssa(double angle){ return std::fmod(angle+pi, 2*pi)-pi; @@ -109,10 +112,10 @@ void LQRController::update_augmented_matrices(std::vector > {inertia_matrix_inv[2][0],inertia_matrix_inv[2][1],inertia_matrix_inv[2][2],0,0,0}}; }; -std::vector LQRController::update_error(Guidance_values guidance_values, State states){ +std::vector LQRController::update_error(Guidance_data guidance_values, State states){ double surge_error = guidance_values.surge - states.surge; double pitch_error = ssa(guidance_values.pitch - states.pitch); - double yaw_error = ssa(guidance_values.yaw - states.yaw); + double yaw_error = ssa(guidance_values.yaw - states.yaw); integral_error_surge = anti_windup(i_surge, surge_error, integral_error_surge, surge_windup); integral_error_pitch = anti_windup(i_pitch, pitch_error, integral_error_pitch, pitch_windup); @@ -128,7 +131,7 @@ std::vector LQRController::saturate_input(std::vector u){ std::tie(yaw_windup, torque_z) = saturate(u[2], yaw_windup, max_force); return {force_x, torque_y, torque_z}; } -std::vector LQRController::calculate_lqr_u(std::vector> coriolis_matrix, State states, Guidance_values guidance_values){ +std::vector LQRController::calculate_lqr_u(std::vector> coriolis_matrix, State states, Guidance_data guidance_values){ update_augmented_matrices(coriolis_matrix); auto result = drake::systems::controllers::LinearQuadraticRegulator( vector2d_to_matrix3d(augmented_system_matrix), @@ -136,7 +139,7 @@ std::vector LQRController::calculate_lqr_u(std::vector state_error = update_error(guidance_values, states); - std::vector u= saturate_input(matrix3d_to_vector(- result * vector_to_matrix3d(state_error))); + std::vector u= saturate_input(matrix3d_to_vector(- (result.K * vector_to_matrix3d(state_error)))); return u; } void LQRController::reset_controller(){ @@ -152,12 +155,6 @@ void LQRController::reset_controller(){ - - -int main(){ - return 0; -}; - //Hjelpefunksjoner for å konvertere mellom std::vector og Eigen::Matrix3d Eigen::Matrix3d vector_to_matrix3d(const std::vector &other_matrix){ Eigen::Matrix3d mat; diff --git a/control/velocity_controller/src/PID_setup.cpp b/control/velocity_controller/src/PID_setup.cpp index 74c4fe335..cf0641152 100644 --- a/control/velocity_controller/src/PID_setup.cpp +++ b/control/velocity_controller/src/PID_setup.cpp @@ -1,4 +1,6 @@ #include "velocity_controller/PID_setup.hpp" +#include "velocity_controller/LQR_setup.hpp" +#include "velocity_controller/utilities.hpp" PID_controller::PID_controller( double k_p, double k_i, double k_d, double max_output, double min_output):k_p(k_p), k_i(k_i), k_d(k_d), max_output(max_output), min_output(min_output) { integral = 0.0; @@ -48,21 +50,3 @@ void PID_controller::set_output_limits(double min_output, double max_output){ return; }; -angle quaternion_to_euler_angle(double w, double x, double y, double z){ - double ysqr = y * y; - - double t0 = +2.0 * (w * x + y * z); - double t1 = +1.0 - 2.0 * (x * x + ysqr); - double phi = std::atan2(t0, t1); - - double t2 = +2.0 * (w * y - z * x); - t2 = t2 > 1.0 ? 1.0 : t2; - t2 = t2 < -1.0 ? -1.0 : t2; - double theta = std::asin(t2); - - double t3 = +2.0 * (w * z + x * y); - double t4 = +1.0 - 2.0 * (ysqr + z * z); - double psi = std::atan2(t3, t4); - - return {phi, theta, psi}; -}; \ No newline at end of file diff --git a/control/velocity_controller/src/utilities.cpp b/control/velocity_controller/src/utilities.cpp new file mode 100644 index 000000000..3ad236870 --- /dev/null +++ b/control/velocity_controller/src/utilities.cpp @@ -0,0 +1,21 @@ +#include "velocity_controller/utilities.hpp" +#include "Eigen/Dense" + +angle quaternion_to_euler_angle(double w, double x, double y, double z){ + double ysqr = y * y; + + double t0 = +2.0 * (w * x + y * z); + double t1 = +1.0 - 2.0 * (x * x + ysqr); + double phi = std::atan2(t0, t1); + + double t2 = +2.0 * (w * y - z * x); + t2 = t2 > 1.0 ? 1.0 : t2; + t2 = t2 < -1.0 ? -1.0 : t2; + double theta = std::asin(t2); + + double t3 = +2.0 * (w * z + x * y); + double t4 = +1.0 - 2.0 * (ysqr + z * z); + double psi = std::atan2(t3, t4); + + return {phi, theta, psi}; +}; \ No newline at end of file diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index 0f38c89d4..ae4893c99 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -8,18 +8,21 @@ #include "std_msgs/msg/bool.hpp" #include "velocity_controller/PID_setup.hpp" #include +#include //#include "vortex-msgs/msg" kan legge til nye meldinger nå //Lager en klasse velocity node //Konstruktør -Velocity_node::Velocity_node() : Node("velocity_controller_node"), PID_surge(1,1,1), PID_yaw(1,1,1), PID_pitch(1,1,1) +Velocity_node::Velocity_node() : Node("velocity_controller_node"), PID_surge(1,1,1), PID_yaw(1,1,1), PID_pitch(1,1,1), lqr_controller() { //Dytter info til log RCLCPP_INFO(this->get_logger(), "Velocity control node has been started."); //Parameter from config. get_new_parameters(); + + // Publishers publisher_thrust = create_publisher(topic_thrust, 10); @@ -43,12 +46,14 @@ Velocity_node::Velocity_node() : Node("velocity_controller_node"), PID_surge(1,1 timer_calculation = this->create_wall_timer(std::chrono::milliseconds(calculation_rate), std::bind(&Velocity_node::publish_thrust, this)); timer_publish = this->create_wall_timer(std::chrono::milliseconds(publish_rate), std::bind(&Velocity_node::calc_thrust, this)); - + //Controllers PID_surge.set_output_limits(-max_force, max_force); PID_pitch.set_output_limits(-max_force, max_force); PID_yaw.set_output_limits(-max_force, max_force); - this->calculation_rate = this->get_parameter("calculation_rate").as_int(); - this->publish_rate = this->get_parameter("publish_rate").as_int(); + lqr_controller.set_params(lqr_parameters); + lqr_controller.set_matrices(inertia_matrix); + + } @@ -62,12 +67,22 @@ void Velocity_node::publish_thrust() //** må forbedre integrasjon og derivasjons beregningene void Velocity_node::calc_thrust() { - PID_surge.calculate_thrust(reference.surge, current_state.surge,calculation_rate/1000.0); - PID_pitch.calculate_thrust(reference.pitch, current_state.pitch,calculation_rate/1000.0); - PID_yaw.calculate_thrust(reference.yaw, current_state.yaw,calculation_rate/1000.0); - thrust_out.wrench.force.x = PID_surge.output(); - thrust_out.wrench.torque.y = PID_pitch.output(); - thrust_out.wrench.torque.z = PID_yaw.output(); + switch (controller_type) + { + case 1: + PID_surge.calculate_thrust(guidance_values.surge, current_state.surge,calculation_rate/1000.0); + PID_pitch.calculate_thrust(guidance_values.pitch, current_state.pitch,calculation_rate/1000.0); + PID_yaw.calculate_thrust(guidance_values.yaw, current_state.yaw,calculation_rate/1000.0); + thrust_out.wrench.force.x = PID_surge.output(); + thrust_out.wrench.torque.y = PID_pitch.output(); + thrust_out.wrench.torque.z = PID_yaw.output(); + break; + case 2: + lqr_controller.update_error(guidance_values,current_state); + default: + break; + } + return; } @@ -77,7 +92,7 @@ void Velocity_node::calc_thrust() //Callback functions void Velocity_node::guidance_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg_ptr){ //RCLCPP_INFO(this->get_logger(), "Received reference: '%f'", msg_ptr->wrench.force.x); - reference = *msg_ptr; + guidance_values = *msg_ptr; return; } void Velocity_node::twist_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg_ptr){ @@ -96,8 +111,8 @@ void Velocity_node::pose_callback(const geometry_msgs::msg::PoseWithCovarianceSt void Velocity_node::killswitch_callback(const std_msgs::msg::Bool::SharedPtr msg_ptr){ RCLCPP_INFO(this->get_logger(), "Received killswitch: '%d'", msg_ptr->data); if(msg_ptr->data == true){ - reference = guidance_data(); - current_state = guidance_data(); + guidance_values = Guidance_data(); + current_state = Guidance_data(); RCLCPP_INFO(this->get_logger(), "Killswitch activated, reference and current state set to zero"); } return; @@ -121,6 +136,38 @@ void Velocity_node::get_new_parameters(){ this->calculation_rate = this->get_parameter("calculation_rate").as_int(); this->declare_parameter("publish_rate"); this->publish_rate = this->get_parameter("publish_rate").as_int(); + this->declare_parameter("controller_type"); + this->controller_type=this->get_parameter("controller_type").as_int(); + + + //LQR Parameters + this->declare_parameter("LQR_params.q_surge"); + this->declare_parameter("LQR_params.q_pitch"); + this->declare_parameter("LQR_params.q_yaw"); + this->declare_parameter("LQR_params.r_surge"); + this->declare_parameter("LQR_params.r_pitch"); + this->declare_parameter("LQR_params.r_yaw"); + this->declare_parameter("LQR_params.i_surge"); + this->declare_parameter("LQR_params.i_pitch"); + this->declare_parameter("LQR_params.i_yaw"); + this->declare_parameter("LQR_params.i_weight"); + this->declare_parameter("LQR_params.dt"); + this->declare_parameter>("inertia_matrix"); + + this->lqr_parameters.q_surge=this->get_parameter("LQR_params.q_surge").as_double(); + this->lqr_parameters.q_pitch=this->get_parameter("LQR_params.q_pitch").as_double(); + this->lqr_parameters.q_yaw=this->get_parameter("LQR_params.q_yaw").as_double(); + this->lqr_parameters.r_surge=this->get_parameter("LQR_params.r_surge").as_double(); + this->lqr_parameters.r_pitch=this->get_parameter("LQR_params.r_pitch").as_double(); + this->lqr_parameters.r_yaw=this->get_parameter("LQR_params.r_yaw").as_double(); + this->lqr_parameters.i_surge=this->get_parameter("LQR_params.i_surge").as_double(); + this->lqr_parameters.i_pitch=this->get_parameter("LQR_params.i_pitch").as_double(); + this->lqr_parameters.i_yaw=this->get_parameter("LQR_params.i_yaw").as_double(); + this->lqr_parameters.i_weight=this->get_parameter("LQR_params.i_weight").as_double(); + this->lqr_parameters.max_force=max_force; + this->inertia_matrix=this->get_parameter("inertia_matrix").as_double_array(); + + } int main(int argc, char * argv[]) @@ -156,16 +203,16 @@ geometry_msgs::msg::WrenchStamped operator+(const geometry_msgs::msg::WrenchStam return result; } //operator overloading for guidance_data -guidance_data guidance_data::operator-(const guidance_data & b) const +Guidance_data Guidance_data::operator-(const Guidance_data & b) const { - guidance_data result; + Guidance_data result; result.surge = this->surge - b.surge; result.pitch = this->pitch - b.pitch; result.yaw = this->yaw - b.yaw; return result; } -guidance_data& guidance_data::operator=(const std_msgs::msg::Float64MultiArray& msg) +Guidance_data& Guidance_data::operator=(const std_msgs::msg::Float64MultiArray& msg) { if (msg.data.size()>=3){ surge=msg.data[0]; @@ -179,7 +226,7 @@ guidance_data& guidance_data::operator=(const std_msgs::msg::Float64MultiArray& return *this; } -guidance_data::guidance_data(std_msgs::msg::Float64MultiArray msg){ +Guidance_data::Guidance_data(std_msgs::msg::Float64MultiArray msg){ if (msg.data.size()>=3){ surge=msg.data[0]; pitch=msg.data[1]; From 9141eca39ffbd505c01b408bdfb08c2822b939bb Mon Sep 17 00:00:00 2001 From: Henrik Date: Wed, 22 Oct 2025 11:59:53 +0200 Subject: [PATCH 013/128] Removed Drake, Changed from std::vector to Eigen in LQR --- control/velocity_controller/CMakeLists.txt | 5 +- .../include/velocity_controller/LQR_setup.hpp | 31 +++++--- control/velocity_controller/src/LQR_setup.cpp | 79 +++++++++---------- .../src/velocity_controller.cpp | 2 +- 4 files changed, 60 insertions(+), 57 deletions(-) diff --git a/control/velocity_controller/CMakeLists.txt b/control/velocity_controller/CMakeLists.txt index 2f2ea04c0..69195bdd1 100644 --- a/control/velocity_controller/CMakeLists.txt +++ b/control/velocity_controller/CMakeLists.txt @@ -9,6 +9,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +set(DRAKE_DIR /ros2_ws_v/src/drake-build/install/lib/cmake/drake) # find dependencies find_package(ament_cmake REQUIRED) @@ -16,7 +17,7 @@ find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(vortex_msgs REQUIRED) find_package(Eigen3 REQUIRED) -find_package(drake REQUIRED) +#find_package(drake CONFIG REQUIRED) include_directories( ${EIGEN3_INCLUDE_DIR} @@ -55,7 +56,7 @@ ament_target_dependencies(velocity_controller_node vortex_msgs geometry_msgs Eigen3 - drake + #drake ) ament_target_dependencies(test_VC_node diff --git a/control/velocity_controller/include/velocity_controller/LQR_setup.hpp b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp index 4fa1fc023..e3131d56e 100644 --- a/control/velocity_controller/include/velocity_controller/LQR_setup.hpp +++ b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp @@ -32,26 +32,33 @@ class LQRparameters{ double psit=0.0; };*/ +template +struct LQRsolveResult{ + Eigen::Matrix K; + Eigen::Matrix P; +}; class LQRController{ public: - LQRController(LQRparameters params={0,0,0,0,0,0,0,0,0,0,0},std::vector inertia_matrix={0,0,0,0,0,0,0,0,0}); + LQRController(LQRparameters params={0,0,0,0,0,0,0,0,0,0,0},Eigen::Matrix3d inertia_matrix=Eigen::Matrix3d::Identity()); void set_params(LQRparameters params); - std::vector> calculate_coriolis_matrix(double pitchrate, double yaw_rate, double sway_vel, double heave_vel); - void set_matrices(std::vector inertia_matrix); - void update_augmented_matrices(std::vector > coriolis_matrix); + Eigen::Matrix3d calculate_coriolis_matrix(double pitchrate, double yaw_rate, double sway_vel, double heave_vel); + void set_matrices(Eigen::Matrix3d inertia_matrix); + void update_augmented_matrices(Eigen::Matrix3d coriolis_matrix); //angle quaternion_to_euler_angle(double w, double x, double y, double z); double ssa(double angle); std::tuple saturate (double value, bool windup, double limit); double anti_windup(double ki, double error, double integral_sum, bool windup); - std::vector saturate_input(std::vector u); + Eigen::Vector saturate_input(Eigen::Vector u); - std::vector update_error(Guidance_data guidance_values, State states); - std::vector calculate_lqr_u(std::vector> coriolis_matrix, State states, Guidance_data guidance_values); + Eigen::Vector update_error(Guidance_data guidance_values, State states); + Eigen::Vector calculate_lqr_u(Eigen::Matrix3d coriolis_matrix, State states, Guidance_data guidance_values); + template + LQRsolveResult<6,6> solve_k_p(Eigen::Matrix A,Eigen::Matrix B,Eigen::Matrix R, Eigen::Matrix Q); //Resets controller void reset_controller(); @@ -65,11 +72,11 @@ class LQRController{ double i_surge; double i_pitch; double i_yaw; double i_weight; double max_force; - std::vector> inertia_matrix_inv; - std::vector> state_weight_matrix; - std::vector> input_weight_matrix; - std::vector> augmented_system_matrix; - std::vector> augmented_input_matrix; + Eigen::Matrix3d inertia_matrix_inv; + Eigen::Matrix state_weight_matrix; + Eigen::Matrix3d input_weight_matrix; + Eigen::Matrix augmented_system_matrix; + Eigen::Matrix augmented_input_matrix; }; diff --git a/control/velocity_controller/src/LQR_setup.cpp b/control/velocity_controller/src/LQR_setup.cpp index 3025d6b5d..d7f7615d9 100644 --- a/control/velocity_controller/src/LQR_setup.cpp +++ b/control/velocity_controller/src/LQR_setup.cpp @@ -4,15 +4,15 @@ #include #include #include -#include -#include -#include -#include +//#include +//#include +//#include +//#include #include "velocity_controller/PID_setup.hpp" #include "velocity_controller/utilities.hpp" -LQRController::LQRController(LQRparameters params,std::vector inertia_matrix){ +LQRController::LQRController(LQRparameters params,Eigen::Matrix3d inertia_matrix){ set_params(params); set_matrices(inertia_matrix); }; @@ -62,11 +62,13 @@ double LQRController::anti_windup(double ki, double error, double integral_sum, return integral_sum; } -std::vector> LQRController::calculate_coriolis_matrix(double pitchrate, double yaw_rate, double sway_vel, double heave_vel){ +Eigen::Matrix3d LQRController::calculate_coriolis_matrix(double pitchrate, double yaw_rate, double sway_vel, double heave_vel){ //Inertia matrix values?? - return {{0.2,-30*sway_vel*0.01,-30*heave_vel*0.01}, - {30 * sway_vel*0.01,0,1.629 * pitchrate}, - {30 * heave_vel*0.01,1.769 * yaw_rate,0}}; + Eigen::Matrix3d result; + result<<0.2,-30*sway_vel*0.01,-30*heave_vel*0.01, + 30 * sway_vel*0.01,0,1.629 * pitchrate, + 30 * heave_vel*0.01,1.769 * yaw_rate,0; + return result; } @@ -81,38 +83,29 @@ void LQRController::set_params(LQRparameters params){ return; } -void LQRController::set_matrices(std::vector inertia_matrix){ - Eigen::Matrix3d mat= vector_to_matrix3d(inertia_matrix); - inertia_matrix_inv = matrix3d_to_vector2d(mat.inverse()); - state_weight_matrix = {{q_surge,0,0,0,0,0}, - {0,q_pitch,0,0,0,0}, - {0,0,q_yaw,0,0,0}, - {0,0,0,i_weight,0,0}, - {0,0,0,0,i_weight,0}, - {0,0,0,0,0,i_weight}}; - input_weight_matrix = {{r_surge,0,0}, - {0,r_pitch,0}, - {0,0,r_yaw}}; - +void LQRController::set_matrices(Eigen::Matrix3d inertia_matrix){ + inertia_matrix_inv = inertia_matrix.inverse(); + state_weight_matrix.diagonal() <> coriolis_matrix){ - std::vector> system_matrix = matrix3d_to_vector2d(vector2d_to_matrix3d(inertia_matrix_inv) * vector2d_to_matrix3d(coriolis_matrix)); +void LQRController::update_augmented_matrices(Eigen::Matrix3d coriolis_matrix){ + Eigen::Matrix3d system_matrix = inertia_matrix_inv*coriolis_matrix; //input_matrix = inertia_matrix_inv; - augmented_system_matrix = {{system_matrix[0][0],system_matrix[0][1],system_matrix[0][2],0,0,0}, - {system_matrix[1][0],system_matrix[1][1],system_matrix[1][2],0,0,0}, - {system_matrix[2][0],system_matrix[2][1],system_matrix[2][2],0,0,0}, - {-1,0,0,0,0,0}, - {0,-1,0,0,0,0}, - {0,0,-1,0,0,0}}; //Skal det være -1 her? - augmented_input_matrix = {{inertia_matrix_inv[0][0],inertia_matrix_inv[0][1],inertia_matrix_inv[0][2],0,0,0}, - {inertia_matrix_inv[1][0],inertia_matrix_inv[1][1],inertia_matrix_inv[1][2],0,0,0}, - {inertia_matrix_inv[2][0],inertia_matrix_inv[2][1],inertia_matrix_inv[2][2],0,0,0}}; - + augmented_system_matrix < LQRController::update_error(Guidance_data guidance_values, State states){ +Eigen::Vector LQRController::update_error(Guidance_data guidance_values, State states){ double surge_error = guidance_values.surge - states.surge; double pitch_error = ssa(guidance_values.pitch - states.pitch); double yaw_error = ssa(guidance_values.yaw - states.yaw); @@ -121,25 +114,27 @@ std::vector LQRController::update_error(Guidance_data guidance_values, S integral_error_pitch = anti_windup(i_pitch, pitch_error, integral_error_pitch, pitch_windup); integral_error_yaw = anti_windup(i_yaw, yaw_error, integral_error_yaw, yaw_windup); - std::vector state_error= {-surge_error, -pitch_error, -yaw_error, integral_error_surge, integral_error_pitch, integral_error_yaw}; + Eigen::Vector state_error= {-surge_error, -pitch_error, -yaw_error, integral_error_surge, integral_error_pitch, integral_error_yaw}; return state_error; } -std::vector LQRController::saturate_input(std::vector u){ +Eigen::Vector LQRController::saturate_input(Eigen::Vector u){ double force_x, torque_y, torque_z; std::tie(surge_windup, force_x) = saturate(u[0], surge_windup, max_force); std::tie(pitch_windup, torque_y) = saturate(u[1], pitch_windup, max_force); std::tie(yaw_windup, torque_z) = saturate(u[2], yaw_windup, max_force); return {force_x, torque_y, torque_z}; } -std::vector LQRController::calculate_lqr_u(std::vector> coriolis_matrix, State states, Guidance_data guidance_values){ +Eigen::Vector LQRController::calculate_lqr_u(Eigen::Matrix3d coriolis_matrix, State states, Guidance_data guidance_values){ update_augmented_matrices(coriolis_matrix); - auto result = drake::systems::controllers::LinearQuadraticRegulator( + + Eigen::Matrix result=Eigen::Matrix::Identity(); + /*auto result = drake::systems::controllers::LinearQuadraticRegulator( vector2d_to_matrix3d(augmented_system_matrix), vector2d_to_matrix3d(augmented_input_matrix), vector2d_to_matrix3d(state_weight_matrix), - vector2d_to_matrix3d(input_weight_matrix)); - std::vector state_error = update_error(guidance_values, states); - std::vector u= saturate_input(matrix3d_to_vector(- (result.K * vector_to_matrix3d(state_error)))); + vector2d_to_matrix3d(input_weight_matrix));*/ + Eigen::Vector state_error = update_error(guidance_values, states); + Eigen::Vector u= saturate_input(- (result*state_error)); return u; } void LQRController::reset_controller(){ diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index ae4893c99..dcb2c7103 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -51,7 +51,7 @@ Velocity_node::Velocity_node() : Node("velocity_controller_node"), PID_surge(1,1 PID_pitch.set_output_limits(-max_force, max_force); PID_yaw.set_output_limits(-max_force, max_force); lqr_controller.set_params(lqr_parameters); - lqr_controller.set_matrices(inertia_matrix); + lqr_controller.set_matrices(vector_to_matrix3d(inertia_matrix)); } From 064adef0ab5fbfdcbc4af38df6f85c3d71b607ca Mon Sep 17 00:00:00 2001 From: Anbit Date: Sun, 26 Oct 2025 13:52:33 +0100 Subject: [PATCH 014/128] los test --- .github/workflows/simulator-test.yml | 2 +- tests/simulator_tests/los_test/simulator_test.sh | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/simulator-test.yml b/.github/workflows/simulator-test.yml index d226aa02e..6ff6d50bf 100644 --- a/.github/workflows/simulator-test.yml +++ b/.github/workflows/simulator-test.yml @@ -14,7 +14,7 @@ jobs: matrix: test_script: - "tests/simulator_tests/waypoint_navigation/simulator_test.sh" - #- "tests/simulator_tests/los_test/simulator_test.sh" + - "tests/simulator_tests/los_test/simulator_test.sh" uses: vortexntnu/vortex-ci/.github/workflows/reusable-ros2-simulator-test.yml@main with: vcs_repos_file: "tests/dependencies.repos" diff --git a/tests/simulator_tests/los_test/simulator_test.sh b/tests/simulator_tests/los_test/simulator_test.sh index 97da70538..0c75818db 100755 --- a/tests/simulator_tests/los_test/simulator_test.sh +++ b/tests/simulator_tests/los_test/simulator_test.sh @@ -60,7 +60,7 @@ echo "Waiting for pose data..." timeout 10s ros2 topic echo /orca/pose --once echo "Got pose data" -setsid ros2 launch los_guidance los_guidance.launch.py & +setsid ros2 launch guidance_velocity_controller launch_Guidance.py & LOS_PID=$! setsid ros2 launch velocity_controller_lqr velocity_controller_lqr.launch.py & From 70197875803822c3d6217a952aec1af9099255f7 Mon Sep 17 00:00:00 2001 From: Henrik Date: Sun, 26 Oct 2025 17:18:28 +0100 Subject: [PATCH 015/128] Implemented LQR solver, SLICOT call and some dimension mistakes --- control/velocity_controller/CMakeLists.txt | 53 ++++++++------- .../include/velocity_controller/LQR_setup.hpp | 15 +++-- control/velocity_controller/src/LQR_setup.cpp | 64 +++++++++++++++---- 3 files changed, 89 insertions(+), 43 deletions(-) diff --git a/control/velocity_controller/CMakeLists.txt b/control/velocity_controller/CMakeLists.txt index 69195bdd1..f24299582 100644 --- a/control/velocity_controller/CMakeLists.txt +++ b/control/velocity_controller/CMakeLists.txt @@ -9,7 +9,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(DRAKE_DIR /ros2_ws_v/src/drake-build/install/lib/cmake/drake) # find dependencies find_package(ament_cmake REQUIRED) @@ -17,15 +16,18 @@ find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(vortex_msgs REQUIRED) find_package(Eigen3 REQUIRED) -#find_package(drake CONFIG REQUIRED) +find_package(CasADi REQUIRED) +find_package(LAPACK REQUIRED) +find_package(BLAS REQUIRED) +find_package(geometry_msgs REQUIRED) + include_directories( ${EIGEN3_INCLUDE_DIR} - ${drake_INCLUDE_DIRS} + include ) -link_directories("/opt/drake/lib/") #set(LIB_NAME "${PROJECT_NAME}_component") @@ -47,7 +49,6 @@ add_executable(test_VC_node src/PID_setup.cpp src/LQR_setup.cpp src/utilities.cpp -# src/velocity_controller.cpp ) ament_target_dependencies(velocity_controller_node @@ -56,7 +57,8 @@ ament_target_dependencies(velocity_controller_node vortex_msgs geometry_msgs Eigen3 - #drake + CasADi + ) ament_target_dependencies(test_VC_node @@ -67,30 +69,22 @@ ament_target_dependencies(test_VC_node Eigen3 ) -install(TARGETS - velocity_controller_node - test_VC_node - DESTINATION lib/${PROJECT_NAME} -) +link_directories(/usr/lib/gcc/x86_64-linux-gnu/11) +set(SLICOT_LIB /usr/lib/x86_64-linux-gnu/libslicot.so) +set(GFORTRAN_LIB /usr/lib/gcc/x86_64-linux-gnu/11/libgfortran.so) + +target_link_libraries(velocity_controller_node ${LAPACK_LIBRARIES} ${BLAS_LIBRARIES} ${SLICOT_LIB} ${GFORTRAN_LIB}) +target_link_libraries(test_VC_node ${LAPACK_LIBRARIES} ${BLAS_LIBRARIES} ${SLICOT_LIB} ${GFORTRAN_LIB}) -#rclcpp_components_register_node( -# ${LIB_NAME} -# PLUGIN "velocity_controller_node" -# EXECUTABLE ${PROJECT_NAME}_node -#) -#ament_export_targets(export_${LIB_NAME}) -#install(TARGETS ${LIB_NAME} -# EXPORT export_${LIB_NAME} -# ARCHIVE DESTINATION lib -# LIBRARY DESTINATION lib -# RUNTIME DESTINATION bin -#) install(TARGETS velocity_controller_node test_VC_node DESTINATION lib/${PROJECT_NAME} ) + + + install( DIRECTORY include/ DESTINATION include @@ -138,4 +132,15 @@ ament_package() # DESTINATION share/${PROJECT_NAME}/ #) - +#rclcpp_components_register_node( +# ${LIB_NAME} +# PLUGIN "velocity_controller_node" +# EXECUTABLE ${PROJECT_NAME}_node +#) +#ament_export_targets(export_${LIB_NAME}) +#install(TARGETS ${LIB_NAME} +# EXPORT export_${LIB_NAME} +# ARCHIVE DESTINATION lib +# LIBRARY DESTINATION lib +# RUNTIME DESTINATION bin +#) diff --git a/control/velocity_controller/include/velocity_controller/LQR_setup.hpp b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp index e3131d56e..5a4bb2a9d 100644 --- a/control/velocity_controller/include/velocity_controller/LQR_setup.hpp +++ b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp @@ -32,10 +32,11 @@ class LQRparameters{ double psit=0.0; };*/ -template + struct LQRsolveResult{ - Eigen::Matrix K; - Eigen::Matrix P; + Eigen::Matrix K; + Eigen::Matrix P; + LQRsolveResult(Eigen::Matrix K,Eigen::Matrix P):K(K),P(P){}; }; class LQRController{ @@ -53,12 +54,12 @@ class LQRController{ std::tuple saturate (double value, bool windup, double limit); double anti_windup(double ki, double error, double integral_sum, bool windup); - Eigen::Vector saturate_input(Eigen::Vector u); + Eigen::Vector saturate_input(Eigen::Vector u); Eigen::Vector update_error(Guidance_data guidance_values, State states); Eigen::Vector calculate_lqr_u(Eigen::Matrix3d coriolis_matrix, State states, Guidance_data guidance_values); - template - LQRsolveResult<6,6> solve_k_p(Eigen::Matrix A,Eigen::Matrix B,Eigen::Matrix R, Eigen::Matrix Q); + + LQRsolveResult solve_k_p(Eigen::Matrix A,Eigen::Matrix B,Eigen::Matrix Q, Eigen::Matrix R); //Resets controller void reset_controller(); @@ -76,7 +77,7 @@ class LQRController{ Eigen::Matrix state_weight_matrix; Eigen::Matrix3d input_weight_matrix; Eigen::Matrix augmented_system_matrix; - Eigen::Matrix augmented_input_matrix; + Eigen::Matrix augmented_input_matrix; }; diff --git a/control/velocity_controller/src/LQR_setup.cpp b/control/velocity_controller/src/LQR_setup.cpp index d7f7615d9..dcfb8e518 100644 --- a/control/velocity_controller/src/LQR_setup.cpp +++ b/control/velocity_controller/src/LQR_setup.cpp @@ -1,5 +1,5 @@ #include "velocity_controller/LQR_setup.hpp" -#include +#include "rclcpp/rclcpp.hpp" #include #include #include @@ -10,7 +10,8 @@ //#include #include "velocity_controller/PID_setup.hpp" #include "velocity_controller/utilities.hpp" - +#include +#include LQRController::LQRController(LQRparameters params,Eigen::Matrix3d inertia_matrix){ set_params(params); @@ -117,7 +118,7 @@ Eigen::Vector LQRController::update_error(Guidance_data guidance_value Eigen::Vector state_error= {-surge_error, -pitch_error, -yaw_error, integral_error_surge, integral_error_pitch, integral_error_yaw}; return state_error; } -Eigen::Vector LQRController::saturate_input(Eigen::Vector u){ +Eigen::Vector LQRController::saturate_input(Eigen::Vector u){ double force_x, torque_y, torque_z; std::tie(surge_windup, force_x) = saturate(u[0], surge_windup, max_force); std::tie(pitch_windup, torque_y) = saturate(u[1], pitch_windup, max_force); @@ -126,15 +127,9 @@ Eigen::Vector LQRController::saturate_input(Eigen::Vector u) } Eigen::Vector LQRController::calculate_lqr_u(Eigen::Matrix3d coriolis_matrix, State states, Guidance_data guidance_values){ update_augmented_matrices(coriolis_matrix); - - Eigen::Matrix result=Eigen::Matrix::Identity(); - /*auto result = drake::systems::controllers::LinearQuadraticRegulator( - vector2d_to_matrix3d(augmented_system_matrix), - vector2d_to_matrix3d(augmented_input_matrix), - vector2d_to_matrix3d(state_weight_matrix), - vector2d_to_matrix3d(input_weight_matrix));*/ - Eigen::Vector state_error = update_error(guidance_values, states); - Eigen::Vector u= saturate_input(- (result*state_error)); + LQRsolveResult result = solve_k_p(augmented_system_matrix,augmented_input_matrix,state_weight_matrix,input_weight_matrix); + Eigen::Matrix state_error = update_error(guidance_values, states); + Eigen::Vector u= saturate_input(- (result.K*state_error)); return u; } void LQRController::reset_controller(){ @@ -149,6 +144,51 @@ void LQRController::reset_controller(){ } +extern "C" { + // Fortran subroutine for solving symplectic Schur decomposition(double precision version) +void sb02mt_( + const char* JOBG, const char* JOBL, const char* FACT, const char* UPLO, + const int* N, const int* M, double* A, const int* LDA, double* B, const int* LDB, + double* Q, const int* LDQ, double* R, const int* LDR, double* L, const int* LDL, + int* IPIV, const int* OUFACT, double* G, const int* LDG, + int* IWORK, double* DWORK, const int* LDWORK, int* INFO +); +} + + +LQRsolveResult LQRController::solve_k_p(Eigen::Matrix A,Eigen::Matrix B, Eigen::Matrix Q,Eigen::Matrix R){ + //First calculate G with sb02mt_ + char JOBG='G'; //calculate G + char JOBL='Z'; //L is zero + char FACT='N'; //unfactored R + char UPLO='U'; //Upper triangle i think + const int N=6; //Order of matrices A, Q, G and X(P) + const int M=3; //Order of matrix R and nuber of columns in B and L(is zero) + int LDA=N, LDB=M, LDQ=N,LDR=M,LDL=N,LDG=N; + + std::vector IWORK(N); + int LDWORK=10*N*N; //Upper bounds + std::vector DWORK(LDWORK); + std::vector IPIV(N); + int OUFACT=0; //Output but initialized JIC + Eigen::Matrix L=Eigen::Matrix::Zero(); + Eigen::Matrix G; + int INFO; + sb02mt_(&JOBG,&JOBL,&FACT,&UPLO,&N,&M,A.data(),&LDA,B.data(),&LDB,Q.data(),&LDQ,R.data(),&LDR,L.data(),&LDL,IPIV.data(),&OUFACT,G.data(),&LDG,IWORK.data(),DWORK.data(),&LDWORK,&INFO); + + if (INFO!=0){ + //Some Error handling here. Also check that BRB in invertible + } + Eigen::Matrix K; + Eigen::Matrix BRB = R+B.transpose()*G*B; + K=BRB.inverse()*B.transpose()*G*A; + + return LQRsolveResult(K,G); + +} + + + //Hjelpefunksjoner for å konvertere mellom std::vector og Eigen::Matrix3d Eigen::Matrix3d vector_to_matrix3d(const std::vector &other_matrix){ From e6ae1452c6f0592062ef2e018be1e2d9bdff6f9d Mon Sep 17 00:00:00 2001 From: Anbit Date: Tue, 28 Oct 2025 20:23:33 +0100 Subject: [PATCH 016/128] Resolve stash conflicts: keep my YAML; keep main's joystick node --- .../config/guidanceParamsa.yaml | 16 ++++++++-------- .../los_guidance/config/guidance_params.yaml | 14 ++++++-------- 2 files changed, 14 insertions(+), 16 deletions(-) diff --git a/guidance/guidance_velocity_controller/config/guidanceParamsa.yaml b/guidance/guidance_velocity_controller/config/guidanceParamsa.yaml index 5a333a316..0225d102a 100755 --- a/guidance/guidance_velocity_controller/config/guidanceParamsa.yaml +++ b/guidance/guidance_velocity_controller/config/guidanceParamsa.yaml @@ -1,10 +1,10 @@ /**: ros__parameters: - lookahead_distance_h = 1.5; - lookahead_distance_v = 0.6; - gamma_h = 0.05; - gamma_v = 0.1; - time_step = 0.01; - u_desired = 0.3; - u_min = 0.1; - d_scale = 1.09; \ No newline at end of file + lookahead_distance_h: 1.5 + lookahead_distance_v: 0.6 + gamma_h: 0.05 + gamma_v: 0.1 + time_step: 0.01 + u_desired: 0.3 + u_min: 0.1 + d_scale: 1.09 \ No newline at end of file diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index a580cf57c..6ab3a2298 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -1,10 +1,8 @@ /**: ros__parameters: - los: - lookahead_distance_h: 1.5 - lookahead_distance_v: 0.6 - gamma_h: 0.05 - gamma_v: 0.1 - time_step: 0.01 - u_desired: 0.3 - goal_reached_tol: 1.0 + lookahead_distance_h: 1.5 + lookahead_distance_v: 0.6 + gamma_h: 0.05 + gamma_v: 0.1 + time_step: 0.01 + u_desired: 0.3 From 4d222674cd61e03b00e879d2fdc4544b902a010c Mon Sep 17 00:00:00 2001 From: Anbit Date: Tue, 28 Oct 2025 20:31:44 +0100 Subject: [PATCH 017/128] fixing my error --- guidance/los_guidance/config/guidance_params.yaml | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 6ab3a2298..ebee79903 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -1,8 +1,10 @@ /**: ros__parameters: - lookahead_distance_h: 1.5 - lookahead_distance_v: 0.6 - gamma_h: 0.05 - gamma_v: 0.1 - time_step: 0.01 - u_desired: 0.3 + los: + lookahead_distance_h: 1.5 + lookahead_distance_v: 0.6 + gamma_h: 0.05 + gamma_v: 0.1 + time_step: 0.01 + u_desired: 0.3 + goal_reached_tol: 1.0 \ No newline at end of file From 3f3e6718695a11501b25a1ad74bf32b09a1633a0 Mon Sep 17 00:00:00 2001 From: Anbit Date: Wed, 29 Oct 2025 13:51:14 +0100 Subject: [PATCH 018/128] maths behind proportional LOS --- .../include/los_guidance/los_guidance.hpp | 130 +++++++++++------- guidance/los_guidance/src/los_guidance.cpp | 39 ++++++ 2 files changed, 120 insertions(+), 49 deletions(-) diff --git a/guidance/los_guidance/include/los_guidance/los_guidance.hpp b/guidance/los_guidance/include/los_guidance/los_guidance.hpp index 5f04ca3cc..c642a2072 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance.hpp @@ -2,76 +2,108 @@ #define LOS_GUIDANCE_HPP #include +#include +#include namespace vortex::guidance { -namespace LOS { + namespace LOS { -struct Point { - double x{}; - double y{}; - double z{}; + struct Point { + double x{}; + double y{}; + double z{}; - Point operator-(const Point& other) const { - return Point{x - other.x, y - other.y, z - other.z}; - } + Point operator-(const Point& other) const { + return Point{x - other.x, y - other.y, z - other.z}; + } - Eigen::Vector3d as_vector() const { return Eigen::Vector3d(x, y, z); } -}; + Eigen::Vector3d as_vector() const { return Eigen::Vector3d(x, y, z); } + }; -struct CrossTrackError { - double x_e{}; - double y_e{}; - double z_e{}; + struct CrossTrackError { + double x_e{}; + double y_e{}; + double z_e{}; - inline static CrossTrackError from_vector(const Eigen::Vector3d& vector) { - return CrossTrackError{vector.x(), vector.y(), vector.z()}; - } -}; + inline static CrossTrackError from_vector(const Eigen::Vector3d& vector) { + return CrossTrackError{vector.x(), vector.y(), vector.z()}; + } + }; -struct Params { - double lookahead_distance_h{}; - double lookahead_distance_v{}; - double gamma_h{}; - double gamma_v{}; - double time_step{}; -}; + struct Params { + double lookahead_distance_h{}; + double lookahead_distance_v{}; + double gamma_h{}; + double gamma_v{}; + double time_step{}; + }; -} // namespace LOS + } // namespace LOS /** * @brief Adaptive Line-of-Sight (LOS) guidance algorithm based on slide 113 * in "Fossen 2024 Lecture on 2D and 3D path-following control". */ -class AdaptiveLOSGuidance { - public: - AdaptiveLOSGuidance(const LOS::Params& params); - ~AdaptiveLOSGuidance() = default; - void update_angles(const LOS::Point& prev_point, - const LOS::Point& next_point); + class AdaptiveLOSGuidance { + public: + AdaptiveLOSGuidance(const LOS::Params& params); + ~AdaptiveLOSGuidance() = default; - LOS::CrossTrackError calculate_crosstrack_error( - const LOS::Point& prev_point, - const LOS::Point& current_position) const; + void update_angles(const LOS::Point& prev_point, + const LOS::Point& next_point); - double calculate_psi_d(const double& y_e) const; + LOS::CrossTrackError calculate_crosstrack_error( + const LOS::Point& prev_point, + const LOS::Point& current_position) const; - double calculate_theta_d(const double& z_e) const; + double calculate_psi_d(const double& y_e) const; - void update_adaptive_estimates( - const LOS::CrossTrackError& crosstrack_error); + double calculate_theta_d(const double& z_e) const; - private: - LOS::Params params_; - Eigen::Matrix3d rotation_y_ = Eigen::Matrix3d::Zero(); - Eigen::Matrix3d rotation_z_ = Eigen::Matrix3d::Zero(); - double pi_h_{}; - double pi_v_{}; - double beta_c_hat_{}; - double alpha_c_hat_{}; -}; + void update_adaptive_estimates( + const LOS::CrossTrackError& crosstrack_error); + + private: + LOS::Params params_; + Eigen::Matrix3d rotation_y_ = Eigen::Matrix3d::Zero(); + Eigen::Matrix3d rotation_z_ = Eigen::Matrix3d::Zero(); + double pi_h_{}; + double pi_v_{}; + double beta_c_hat_{}; + double alpha_c_hat_{}; + }; + +// ---------------- Proportional LOS Guidance Implementation ---------------- + +/* + * Proportional Line-of-Sight (LOS) guidance algorithm based on page 356 + * form "The handbook of marine craft hydrodynamic and motion controll". +*/ + class ProportionalLOSGuidance { + public: + ProportionalLOSGuidance(const LOS::Params& params); + ~ProportionalLOSGuidance() = default; + + void update_angles(const LOS::Point& prev_point, + const LOS::Point& next_point); + + LOS::CrossTrackError calculate_crosstrack_error( + const LOS::Point& prev_point, + const LOS::Point& current_position) const; + + double calculate_psi_d(const double& y_e) const; + double calculate_theta_d(const double& z_e) const; + + private: + LOS::Params params_; + Eigen::Matrix3d rotation_y_{Eigen::Matrix3d::Zero()}; + Eigen::Matrix3d rotation_z_{Eigen::Matrix3d::Zero()}; + double pi_h_{}; + double pi_v_{}; + }; } // namespace vortex::guidance -#endif // LOS_GUIDANCE_HPP +#endif // LOS_GUIDANCE_HPP \ No newline at end of file diff --git a/guidance/los_guidance/src/los_guidance.cpp b/guidance/los_guidance/src/los_guidance.cpp index 3171d5442..07ee1d35f 100644 --- a/guidance/los_guidance/src/los_guidance.cpp +++ b/guidance/los_guidance/src/los_guidance.cpp @@ -56,4 +56,43 @@ void AdaptiveLOSGuidance::update_adaptive_estimates( alpha_c_hat_ += alpha_c_hat_dot * params_.time_step; } +// ---------------- Proportional LOS Guidance Implementation ---------------- + +ProportionalLOSGuidance::ProportionalLOSGuidance(const LOS::Params& params) + : params_(params) {} + +void ProportionalLOSGuidance::update_angles(const LOS::Point& prev_point, + const LOS::Point& next_point) { + const LOS::Point difference = next_point - prev_point; + + pi_h_ = std::atan2(difference.y, difference.x); + pi_v_ = std::atan2(-difference.z, + std::sqrt(difference.x * difference.x + + difference.y * difference.y)); + + rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); + rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); +} + +LOS::CrossTrackError ProportionalLOSGuidance::calculate_crosstrack_error( + const LOS::Point& prev_point, + const LOS::Point& current_position) const { + const Eigen::Vector3d diff_vec = (current_position - prev_point).as_vector(); + + const Eigen::Vector3d e_perp = + rotation_y_.transpose() * rotation_z_.transpose() * diff_vec; + + return LOS::CrossTrackError::from_vector(e_perp); +} + +double ProportionalLOSGuidance::calculate_psi_d(const double& y_e) const { + const double k_p_h = 1.0 / std::max(params_.lookahead_distance_h, 1e-9); + return pi_h_ - std::atan(k_p_h * y_e); +} + +double ProportionalLOSGuidance::calculate_theta_d(const double& z_e) const { + const double k_p_v = 1.0 / std::max(params_.lookahead_distance_v, 1e-9); + return pi_v_ + std::atan(k_p_v * z_e); +} + } // namespace vortex::guidance From 2688e27ed1bcab7c0e0499dbc904a4cdf826fca7 Mon Sep 17 00:00:00 2001 From: Anbit Date: Sun, 2 Nov 2025 11:55:05 +0100 Subject: [PATCH 019/128] refactor: change los structure --- guidance/los_guidance/CMakeLists.txt | 2 +- .../include/los_guidance/los_guidance.hpp | 109 ------------------ .../include/los_guidance/los_guidance_ros.hpp | 3 +- guidance/los_guidance/src/los_guidance.cpp | 98 ---------------- .../los_guidance/test/test_los_guidance.cpp | 2 +- 5 files changed, 3 insertions(+), 211 deletions(-) delete mode 100644 guidance/los_guidance/include/los_guidance/los_guidance.hpp delete mode 100644 guidance/los_guidance/src/los_guidance.cpp diff --git a/guidance/los_guidance/CMakeLists.txt b/guidance/los_guidance/CMakeLists.txt index b7bd8d746..9bde13d8f 100644 --- a/guidance/los_guidance/CMakeLists.txt +++ b/guidance/los_guidance/CMakeLists.txt @@ -23,7 +23,7 @@ include_directories(include) set(LIB_NAME los_guidance_lib) add_library(${LIB_NAME} SHARED - src/los_guidance.cpp + src/lib/adaptive_los.cpp src/los_guidance_ros.cpp ) diff --git a/guidance/los_guidance/include/los_guidance/los_guidance.hpp b/guidance/los_guidance/include/los_guidance/los_guidance.hpp deleted file mode 100644 index c642a2072..000000000 --- a/guidance/los_guidance/include/los_guidance/los_guidance.hpp +++ /dev/null @@ -1,109 +0,0 @@ -#ifndef LOS_GUIDANCE_HPP -#define LOS_GUIDANCE_HPP - -#include -#include -#include - -namespace vortex::guidance { - - namespace LOS { - - struct Point { - double x{}; - double y{}; - double z{}; - - Point operator-(const Point& other) const { - return Point{x - other.x, y - other.y, z - other.z}; - } - - Eigen::Vector3d as_vector() const { return Eigen::Vector3d(x, y, z); } - }; - - struct CrossTrackError { - double x_e{}; - double y_e{}; - double z_e{}; - - inline static CrossTrackError from_vector(const Eigen::Vector3d& vector) { - return CrossTrackError{vector.x(), vector.y(), vector.z()}; - } - }; - - struct Params { - double lookahead_distance_h{}; - double lookahead_distance_v{}; - double gamma_h{}; - double gamma_v{}; - double time_step{}; - }; - - } // namespace LOS - -/** - * @brief Adaptive Line-of-Sight (LOS) guidance algorithm based on slide 113 - * in "Fossen 2024 Lecture on 2D and 3D path-following control". - */ - - class AdaptiveLOSGuidance { - public: - AdaptiveLOSGuidance(const LOS::Params& params); - ~AdaptiveLOSGuidance() = default; - - void update_angles(const LOS::Point& prev_point, - const LOS::Point& next_point); - - LOS::CrossTrackError calculate_crosstrack_error( - const LOS::Point& prev_point, - const LOS::Point& current_position) const; - - double calculate_psi_d(const double& y_e) const; - - double calculate_theta_d(const double& z_e) const; - - void update_adaptive_estimates( - const LOS::CrossTrackError& crosstrack_error); - - private: - LOS::Params params_; - Eigen::Matrix3d rotation_y_ = Eigen::Matrix3d::Zero(); - Eigen::Matrix3d rotation_z_ = Eigen::Matrix3d::Zero(); - double pi_h_{}; - double pi_v_{}; - double beta_c_hat_{}; - double alpha_c_hat_{}; - }; - -// ---------------- Proportional LOS Guidance Implementation ---------------- - -/* - * Proportional Line-of-Sight (LOS) guidance algorithm based on page 356 - * form "The handbook of marine craft hydrodynamic and motion controll". -*/ - class ProportionalLOSGuidance { - public: - ProportionalLOSGuidance(const LOS::Params& params); - ~ProportionalLOSGuidance() = default; - - void update_angles(const LOS::Point& prev_point, - const LOS::Point& next_point); - - LOS::CrossTrackError calculate_crosstrack_error( - const LOS::Point& prev_point, - const LOS::Point& current_position) const; - - double calculate_psi_d(const double& y_e) const; - double calculate_theta_d(const double& z_e) const; - - private: - LOS::Params params_; - Eigen::Matrix3d rotation_y_{Eigen::Matrix3d::Zero()}; - Eigen::Matrix3d rotation_z_{Eigen::Matrix3d::Zero()}; - double pi_h_{}; - double pi_v_{}; - }; - -} // namespace vortex::guidance - -#endif // LOS_GUIDANCE_HPP \ No newline at end of file diff --git a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp index 94e3073ee..c12838ba4 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -4,13 +4,12 @@ #include #include #include -#include +#include "los_guidance/lib/adaptive_los.hpp" #include #include #include #include #include -#include "los_guidance.hpp" namespace vortex::guidance { diff --git a/guidance/los_guidance/src/los_guidance.cpp b/guidance/los_guidance/src/los_guidance.cpp deleted file mode 100644 index 07ee1d35f..000000000 --- a/guidance/los_guidance/src/los_guidance.cpp +++ /dev/null @@ -1,98 +0,0 @@ -#include "los_guidance/los_guidance.hpp" - -namespace vortex::guidance { - -AdaptiveLOSGuidance::AdaptiveLOSGuidance(const LOS::Params& params) - : params_(params) {} - -void AdaptiveLOSGuidance::update_angles(const LOS::Point& prev_point, - const LOS::Point& next_point) { - const LOS::Point difference = next_point - prev_point; - - pi_h_ = atan2(difference.y, difference.x); - pi_v_ = atan2(-difference.z, sqrt(difference.x * difference.x + - difference.y * difference.y)); - - rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); - rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); -} - -LOS::CrossTrackError AdaptiveLOSGuidance::calculate_crosstrack_error( - const LOS::Point& prev_point, - const LOS::Point& current_position) const { - const LOS::Point difference = current_position - prev_point; - const Eigen::Vector3d difference_vector = difference.as_vector(); - - const Eigen::Vector3d cross_track_error = - rotation_y_.transpose() * rotation_z_.transpose() * difference_vector; - - return LOS::CrossTrackError::from_vector(cross_track_error); -} - -double AdaptiveLOSGuidance::calculate_psi_d(const double& y_e) const { - return pi_h_ - beta_c_hat_ - atan(y_e / params_.lookahead_distance_h); -} - -double AdaptiveLOSGuidance::calculate_theta_d(const double& z_e) const { - return pi_v_ + alpha_c_hat_ + atan(z_e / params_.lookahead_distance_v); -} - -void AdaptiveLOSGuidance::update_adaptive_estimates( - const LOS::CrossTrackError& crosstrack_error) { - double beta_c_hat_dot = - params_.gamma_h * - (params_.lookahead_distance_h / - sqrt(params_.lookahead_distance_h * params_.lookahead_distance_h + - crosstrack_error.y_e * crosstrack_error.y_e)) * - crosstrack_error.y_e; - double alpha_c_hat_dot = - params_.gamma_v * - (params_.lookahead_distance_v / - sqrt(params_.lookahead_distance_v * params_.lookahead_distance_v + - crosstrack_error.z_e * crosstrack_error.z_e)) * - crosstrack_error.z_e; - - beta_c_hat_ += beta_c_hat_dot * params_.time_step; - alpha_c_hat_ += alpha_c_hat_dot * params_.time_step; -} - -// ---------------- Proportional LOS Guidance Implementation ---------------- - -ProportionalLOSGuidance::ProportionalLOSGuidance(const LOS::Params& params) - : params_(params) {} - -void ProportionalLOSGuidance::update_angles(const LOS::Point& prev_point, - const LOS::Point& next_point) { - const LOS::Point difference = next_point - prev_point; - - pi_h_ = std::atan2(difference.y, difference.x); - pi_v_ = std::atan2(-difference.z, - std::sqrt(difference.x * difference.x + - difference.y * difference.y)); - - rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); - rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); -} - -LOS::CrossTrackError ProportionalLOSGuidance::calculate_crosstrack_error( - const LOS::Point& prev_point, - const LOS::Point& current_position) const { - const Eigen::Vector3d diff_vec = (current_position - prev_point).as_vector(); - - const Eigen::Vector3d e_perp = - rotation_y_.transpose() * rotation_z_.transpose() * diff_vec; - - return LOS::CrossTrackError::from_vector(e_perp); -} - -double ProportionalLOSGuidance::calculate_psi_d(const double& y_e) const { - const double k_p_h = 1.0 / std::max(params_.lookahead_distance_h, 1e-9); - return pi_h_ - std::atan(k_p_h * y_e); -} - -double ProportionalLOSGuidance::calculate_theta_d(const double& z_e) const { - const double k_p_v = 1.0 / std::max(params_.lookahead_distance_v, 1e-9); - return pi_v_ + std::atan(k_p_v * z_e); -} - -} // namespace vortex::guidance diff --git a/guidance/los_guidance/test/test_los_guidance.cpp b/guidance/los_guidance/test/test_los_guidance.cpp index 78b3d4fe1..84ebe7bff 100644 --- a/guidance/los_guidance/test/test_los_guidance.cpp +++ b/guidance/los_guidance/test/test_los_guidance.cpp @@ -1,6 +1,6 @@ #include -#include "los_guidance/los_guidance.hpp" +#include "los_guidance/lib/adaptive_los.hpp" namespace vortex::guidance { From 798542a4b338ad029b62023cd4fc2a53981fff5c Mon Sep 17 00:00:00 2001 From: Anbit Date: Sun, 2 Nov 2025 11:57:45 +0100 Subject: [PATCH 020/128] refactor: remove unused los package --- .../CMakeLists.txt | 61 ------ .../action/LOSGuidance.action | 10 - .../config/guidanceParamsa.yaml | 10 - .../guidance_velocity_controller/guidance.hpp | 80 ------- .../guidance_ros.hpp | 73 ------- .../launch/launch_Guidance.py | 32 --- .../guidance_velocity_controller/package.xml | 34 --- .../src/guidance.cpp | 89 -------- .../src/guidance_node.cpp | 14 -- .../src/guidance_ros.cpp | 200 ------------------ 10 files changed, 603 deletions(-) delete mode 100644 guidance/guidance_velocity_controller/CMakeLists.txt delete mode 100644 guidance/guidance_velocity_controller/action/LOSGuidance.action delete mode 100755 guidance/guidance_velocity_controller/config/guidanceParamsa.yaml delete mode 100755 guidance/guidance_velocity_controller/include/guidance_velocity_controller/guidance.hpp delete mode 100755 guidance/guidance_velocity_controller/include/guidance_velocity_controller/guidance_ros.hpp delete mode 100755 guidance/guidance_velocity_controller/launch/launch_Guidance.py delete mode 100644 guidance/guidance_velocity_controller/package.xml delete mode 100755 guidance/guidance_velocity_controller/src/guidance.cpp delete mode 100755 guidance/guidance_velocity_controller/src/guidance_node.cpp delete mode 100755 guidance/guidance_velocity_controller/src/guidance_ros.cpp diff --git a/guidance/guidance_velocity_controller/CMakeLists.txt b/guidance/guidance_velocity_controller/CMakeLists.txt deleted file mode 100644 index 7945b90be..000000000 --- a/guidance/guidance_velocity_controller/CMakeLists.txt +++ /dev/null @@ -1,61 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(guidance_velocity_controller) - -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 20) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_action REQUIRED) -find_package(vortex_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(Eigen3 REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_geometry_msgs REQUIRED) -find_package(spdlog REQUIRED) -find_package(fmt REQUIRED) - -include_directories(include) - -add_executable(guidance_node - src/guidance_node.cpp - src/guidance_ros.cpp - src/guidance.cpp -) - -ament_target_dependencies(guidance_node - rclcpp - rclcpp_action - geometry_msgs - vortex_msgs - Eigen3 - tf2 - tf2_geometry_msgs - spdlog - fmt -) - -target_link_libraries(guidance_node fmt::fmt) - -install(TARGETS - guidance_node - DESTINATION lib/${PROJECT_NAME} -) - -install( - DIRECTORY include/ - DESTINATION include -) - -install(DIRECTORY - launch - config - DESTINATION share/${PROJECT_NAME}/ -) - -ament_package() \ No newline at end of file diff --git a/guidance/guidance_velocity_controller/action/LOSGuidance.action b/guidance/guidance_velocity_controller/action/LOSGuidance.action deleted file mode 100644 index 49c487775..000000000 --- a/guidance/guidance_velocity_controller/action/LOSGuidance.action +++ /dev/null @@ -1,10 +0,0 @@ -# Goal: -geometry_msgs/PointStamped goal - ---- -# Result: -bool success - ---- -# Feedback: -std_msgs/Float64MultiArray feedback diff --git a/guidance/guidance_velocity_controller/config/guidanceParamsa.yaml b/guidance/guidance_velocity_controller/config/guidanceParamsa.yaml deleted file mode 100755 index 0225d102a..000000000 --- a/guidance/guidance_velocity_controller/config/guidanceParamsa.yaml +++ /dev/null @@ -1,10 +0,0 @@ -/**: - ros__parameters: - lookahead_distance_h: 1.5 - lookahead_distance_v: 0.6 - gamma_h: 0.05 - gamma_v: 0.1 - time_step: 0.01 - u_desired: 0.3 - u_min: 0.1 - d_scale: 1.09 \ No newline at end of file diff --git a/guidance/guidance_velocity_controller/include/guidance_velocity_controller/guidance.hpp b/guidance/guidance_velocity_controller/include/guidance_velocity_controller/guidance.hpp deleted file mode 100755 index 26ca12642..000000000 --- a/guidance/guidance_velocity_controller/include/guidance_velocity_controller/guidance.hpp +++ /dev/null @@ -1,80 +0,0 @@ -#pragma once -#include -#include -#include -#include - -extern std::vector g_waypoints; - -namespace LOS { - -struct Point { - double x; - double y; - double z; - - Point operator-(const Point& other) const { - return Point{x - other.x, y - other.y, z - other.z}; - } - - Eigen::Vector3d as_vector() const { return Eigen::Vector3d(x, y, z); } -}; - -struct CrossTrackError { - double x_e; - double y_e; - double z_e; - - - inline static CrossTrackError from_vector(const Eigen::Vector3d& v) { - return CrossTrackError{v.x(), v.y(), v.z()}; - } -}; - -} - -class LOSGuidance { -public: - LOSGuidance( - double lookahead_distance_h_, - double lookahead_distance_v_, - double gamma_h_, - double gamma_v_, - double time_step_, - double u_desired_, - double u_min_, - double d_scale_); - ~LOSGuidance() = default; - - void update_angles(const LOS::Point& new_point, const LOS::Point& prev_point); - void cross_track_error(const LOS::Point& current_position, const LOS::Point& prev_point); - double desired_surge_speed(const LOS::Point& destination , const LOS::Point& current_position) ; - double desired_heading(); - double desired_pitch(); - void update_adaptive_estimates(); - - Eigen::Vector3d get_outputs() const; - LOS::CrossTrackError& cross_track(); - -private: - - double lookahead_distance_h; - double lookahead_distance_v; - double gamma_h; - double gamma_v; - double time_step; - double u_desired; - double u_min; - double d_scale; - - Eigen::Matrix3d rotation_y_; - Eigen::Matrix3d rotation_z_; - - LOS::CrossTrackError cte; - - double pi_h_; - double pi_v_; - - double beta_c_hat_ = 0.0; - double alpha_c_hat_ = 0.0; -}; diff --git a/guidance/guidance_velocity_controller/include/guidance_velocity_controller/guidance_ros.hpp b/guidance/guidance_velocity_controller/include/guidance_velocity_controller/guidance_ros.hpp deleted file mode 100755 index 0c5a66b97..000000000 --- a/guidance/guidance_velocity_controller/include/guidance_velocity_controller/guidance_ros.hpp +++ /dev/null @@ -1,73 +0,0 @@ -#pragma once -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include - -#include - -#include - -#include -#include -#include - -class GuidanceNode : public rclcpp::Node { -public: - explicit GuidanceNode(); - -private: - // init - void establish_communications(); - void setup_parameters(); - - // io - void waypoint_callback(const geometry_msgs::msg::PointStamped::SharedPtr msg); - void pose_callback(const geometry_msgs::msg::PoseStamped::SharedPtr msg); - void publish_setpoints(); - vortex_msgs::msg::LOSGuidance fill_los_reference(); - void fill_los_waypoints(const geometry_msgs::msg::PointStamped & g_waypoints); - - // actions - using GoalHandle = rclcpp_action::ServerGoalHandle; - rclcpp_action::GoalResponse handle_goal( - const rclcpp_action::GoalUUID &, - std::shared_ptr goal); - rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr goal_handle); - void handle_accepted(const std::shared_ptr goal_handle); - void execute_guidance(const std::shared_ptr goal_handle); - - // pubs/subs - rclcpp::Publisher::SharedPtr refrence_out; - rclcpp::Subscription::SharedPtr waypoint_in; - rclcpp::Subscription::SharedPtr pose_in; - rclcpp_action::Server::SharedPtr guidance_action_server; - - // guidance - std::unique_ptr adaptive_los_guidance_; - - // state - LOS::Point current_position_{}; - LOS::Point last_waypoint_{}; - LOS::Point target_waypoint_{}; - - std::mutex mutex_; - rclcpp_action::GoalUUID preempted_goal_id_{}; - std::shared_ptr goal_handle_{}; - - // timing - std::chrono::milliseconds time_step{0}; - - // outputs - double yaw_d_; - double pitch_d_; - double u_d_ ; -}; diff --git a/guidance/guidance_velocity_controller/launch/launch_Guidance.py b/guidance/guidance_velocity_controller/launch/launch_Guidance.py deleted file mode 100755 index b17fd0344..000000000 --- a/guidance/guidance_velocity_controller/launch/launch_Guidance.py +++ /dev/null @@ -1,32 +0,0 @@ -from os import path - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch_ros.actions import Node - -adapt_params = path.join( - get_package_share_directory("guidance_velocity_controller"), - "config", - "guidanceParamsa.yaml", -) -auv_params = path.join( - get_package_share_directory("auv_setup"), - "config", - "robots", - "orca.yaml", -) - - -def generate_launch_description(): - guidance_node = Node( - package="guidance_velocity_controller", - executable="guidance_node", - name="guidance_node", - namespace="orca", - parameters=[ - auv_params, - adapt_params, - ], - output="screen", - ) - return LaunchDescription([guidance_node]) \ No newline at end of file diff --git a/guidance/guidance_velocity_controller/package.xml b/guidance/guidance_velocity_controller/package.xml deleted file mode 100644 index a8b1876a0..000000000 --- a/guidance/guidance_velocity_controller/package.xml +++ /dev/null @@ -1,34 +0,0 @@ - - - - guidance_velocity_controller - 0.0.1 - LOS guidance + action server for the velocity controller. - anbitadhi - Apache-2.0 - - ament_cmake - - rclcpp - rclcpp_action - geometry_msgs - nav_msgs - tf2 - tf2_geometry_msgs - vortex_msgs - - rosidl_default_generators - rosidl_default_runtime - builtin_interfaces - - ament_lint_auto - ament_lint_common - - rosidl_interface_packages - - - ament_cmake - rosidl_interface_packages - - - diff --git a/guidance/guidance_velocity_controller/src/guidance.cpp b/guidance/guidance_velocity_controller/src/guidance.cpp deleted file mode 100755 index c0a94dd66..000000000 --- a/guidance/guidance_velocity_controller/src/guidance.cpp +++ /dev/null @@ -1,89 +0,0 @@ -#include - -LOSGuidance::LOSGuidance( - double lookahead_distance_h_, - double lookahead_distance_v_, - double gamma_h_, - double gamma_v_, - double time_step_, - double u_desired_, - double u_min_, - double d_scale_) - : lookahead_distance_h(lookahead_distance_h_), - lookahead_distance_v(lookahead_distance_v_), - gamma_h(gamma_h_), - gamma_v(gamma_v_), - time_step(time_step_), - u_desired(u_desired_), - u_min(u_min_), - d_scale(d_scale_) { - rotation_y_ = Eigen::Matrix3d::Identity(); - rotation_z_ = Eigen::Matrix3d::Identity(); -}; - -void LOSGuidance::update_angles(const LOS::Point& new_point, const LOS::Point& prev_point) { - const LOS::Point difference = new_point - prev_point; - - pi_h_ = atan2(difference.y, difference.x); - pi_v_ = atan2(-difference.z, sqrt(difference.x * difference.x + difference.y * difference.y)); - - rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()).toRotationMatrix(); - rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()).toRotationMatrix(); - -}; - -void LOSGuidance::cross_track_error( - const LOS::Point& current_position, const LOS::Point& prev_point) { - const LOS::Point difference = current_position - prev_point; - const Eigen::Vector3d difference_vector = difference.as_vector(); - - const Eigen::Vector3d cross_track_error = rotation_y_.transpose() * rotation_z_.transpose() * difference_vector; - - //save the cross track error - cte = LOS::CrossTrackError::from_vector(cross_track_error); -} - -LOS::CrossTrackError& LOSGuidance::cross_track() { - return cte; -}; - -void LOSGuidance::update_adaptive_estimates() { - double beta_c_hat_dot = - gamma_h * - (lookahead_distance_h / - sqrt(lookahead_distance_h* lookahead_distance_h + - cte.y_e * cte.y_e)) * - cte.y_e; - double alpha_c_hat_dot = - gamma_v * - (lookahead_distance_v / - sqrt(lookahead_distance_v* lookahead_distance_v + - cte.z_e * cte.z_e))* - cte.z_e; - - beta_c_hat_ += beta_c_hat_dot * time_step; - alpha_c_hat_ += alpha_c_hat_dot * time_step; -} - -double LOSGuidance::desired_surge_speed(const LOS::Point& destination , const LOS::Point& current_position) { - double dx = destination.x - current_position.x; - double dy = destination.y - current_position.y; - double dz = destination.z - current_position.z; - double distance = std::sqrt(dx*dx + dy*dy + dz*dz); - - double d = u_min + (u_desired - u_min) * std::tanh(distance/d_scale); - - return d; -} - -double LOSGuidance::desired_heading() { - return pi_h_ - beta_c_hat_ - atan(cte.y_e / lookahead_distance_h); -} - -double LOSGuidance::desired_pitch() { - return pi_v_ + alpha_c_hat_ + atan(cte.z_e / lookahead_distance_v); -} - -Eigen::Vector3d LOSGuidance::get_outputs() const { - return Eigen::Vector3d::Zero(); -} diff --git a/guidance/guidance_velocity_controller/src/guidance_node.cpp b/guidance/guidance_velocity_controller/src/guidance_node.cpp deleted file mode 100755 index 399eafb92..000000000 --- a/guidance/guidance_velocity_controller/src/guidance_node.cpp +++ /dev/null @@ -1,14 +0,0 @@ -#include -#include - -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared(); - rclcpp::executors::MultiThreadedExecutor exec; - exec.add_node(node); - exec.spin(); - rclcpp::shutdown(); - return 0; -} - diff --git a/guidance/guidance_velocity_controller/src/guidance_ros.cpp b/guidance/guidance_velocity_controller/src/guidance_ros.cpp deleted file mode 100755 index 287040439..000000000 --- a/guidance/guidance_velocity_controller/src/guidance_ros.cpp +++ /dev/null @@ -1,200 +0,0 @@ -#include - -GuidanceNode::GuidanceNode() : Node("guidance_node") { - establish_communications(); - setup_parameters(); - spdlog::info("LOS guidance node initialized"); -} - -void GuidanceNode::establish_communications() { - this->declare_parameter("topics.pose"); - this->declare_parameter("topics.guidance.los"); - this->declare_parameter("topics.waypoint"); - this->declare_parameter("action_servers.los"); - - const std::string pose_topic = this->get_parameter("topics.pose").as_string(); - const std::string guidance_topic = this->get_parameter("topics.guidance.los").as_string(); - const std::string waypoint_topic = this->get_parameter("topics.waypoint").as_string(); - const std::string actions_server = this->get_parameter("action_servers.los").as_string(); - - refrence_out = this->create_publisher(guidance_topic, 10); - - waypoint_in = this->create_subscription( - waypoint_topic, 10, - std::bind(&GuidanceNode::waypoint_callback, this, std::placeholders::_1)); - - pose_in = this->create_subscription( - pose_topic, 10, - std::bind(&GuidanceNode::pose_callback, this, std::placeholders::_1)); - - guidance_action_server = rclcpp_action::create_server( - this, - actions_server, - std::bind(&GuidanceNode::handle_goal, this, std::placeholders::_1, std::placeholders::_2), - std::bind(&GuidanceNode::handle_cancel, this, std::placeholders::_1), - std::bind(&GuidanceNode::handle_accepted, this, std::placeholders::_1)); -} - -void GuidanceNode::setup_parameters() { - this->declare_parameter("lookahead_distance_h"); - this->declare_parameter("lookahead_distance_v"); - this->declare_parameter("gamma_h"); - this->declare_parameter("gamma_v"); - this->declare_parameter("time_step"); // seconds - this->declare_parameter("u_desired"); // m/s - this->declare_parameter("u_min"); - this->declare_parameter("d_scale"); - - const double lookahead_distance_h_ = this->get_parameter("lookahead_distance_h").as_double(); - const double lookahead_distance_v_ = this->get_parameter("lookahead_distance_v").as_double(); - const double gamma_h_ = this->get_parameter("gamma_h").as_double(); - const double gamma_v_ = this->get_parameter("gamma_v").as_double(); - const double time_step_s = this->get_parameter("time_step").as_double(); - const double u_desired_param = this->get_parameter("u_desired").as_double(); - const double u_min_ = this->get_parameter("u_min").as_double(); - const double d_scale_ = this->get_parameter("d_scale").as_double(); - - // Construct guidance object - adaptive_los_guidance_ = std::make_unique( - lookahead_distance_h_, - lookahead_distance_v_, - gamma_h_, - gamma_v_, - time_step_s, - u_desired_param, - u_min_, - d_scale_); - - // Convert seconds -> milliseconds for loop timing - time_step = std::chrono::milliseconds(static_cast(std::round(time_step_s * 1000.0))); -} - - -void GuidanceNode::fill_los_waypoints(const geometry_msgs::msg::PointStamped & msg) { - last_waypoint_.x = current_position_.x; - last_waypoint_.y = current_position_.y; - last_waypoint_.z = current_position_.z; - - target_waypoint_.x = msg.point.x; - target_waypoint_.y = msg.point.y; - target_waypoint_.z = msg.point.z; -} - -void GuidanceNode::waypoint_callback(const geometry_msgs::msg::PointStamped::SharedPtr g_waypoints) { - fill_los_waypoints(*g_waypoints); - // NOTE: update_angles(prev, next) - adaptive_los_guidance_->update_angles(last_waypoint_, target_waypoint_); - spdlog::info("Waypoint received"); -} - -void GuidanceNode::pose_callback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) { - current_position_.x = msg->pose.position.x; - current_position_.y = msg->pose.position.y; - current_position_.z = msg->pose.position.z; -} - -rclcpp_action::GoalResponse GuidanceNode::handle_goal( - const rclcpp_action::GoalUUID &, - std::shared_ptr goal) { - (void)goal; - { - std::lock_guard lock(mutex_); - if (goal_handle_ && goal_handle_->is_active()) { - spdlog::info("Aborting current goal and accepting new goal"); - preempted_goal_id_ = goal_handle_->get_goal_id(); - } - } - spdlog::info("Accepted goal request"); - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; -} - -rclcpp_action::CancelResponse GuidanceNode::handle_cancel(const std::shared_ptr goal_handle) { - spdlog::info("Received request to cancel goal"); - (void)goal_handle; - return rclcpp_action::CancelResponse::ACCEPT; -} - -void GuidanceNode::handle_accepted( - const std::shared_ptr< - rclcpp_action::ServerGoalHandle> - goal_handle) { - execute_guidance(goal_handle); -} - -vortex_msgs::msg::LOSGuidance GuidanceNode::fill_los_reference() { - vortex_msgs::msg::LOSGuidance reference_msg; - reference_msg.pitch = pitch_d_; - reference_msg.yaw = yaw_d_; - reference_msg.surge = u_d_; - return reference_msg; -} - -void GuidanceNode::execute_guidance( - const std::shared_ptr< - rclcpp_action::ServerGoalHandle> - goal_handle) { - { - std::lock_guard lock(mutex_); - goal_handle_ = goal_handle; - } - - const geometry_msgs::msg::PointStamped goal_to_go = goal_handle->get_goal()->goal; - fill_los_waypoints(goal_to_go); - // correct member + argument order: (prev, next) - adaptive_los_guidance_->update_angles(last_waypoint_, target_waypoint_); - - const double loop_hz = - (time_step.count() > 0) ? (1000.0 / static_cast(time_step.count())) : 10.0; - rclcpp::Rate loop_rate(loop_hz); - - auto feedback = std::make_shared(); - auto result = std::make_shared(); - - while (rclcpp::ok()) { - // Preemption / cancel checks - { - std::lock_guard lock(mutex_); - if (goal_handle_ != goal_handle) { - spdlog::info("New goal accepted, preempting current goal"); - result->success = false; - goal_handle->canceled(result); - return; - } - if (goal_handle->is_canceling()) { - spdlog::info("Goal canceled"); - result->success = false; - goal_handle->canceled(result); - return; - } - } - - // Guidance update - adaptive_los_guidance_->cross_track_error(last_waypoint_, current_position_); - yaw_d_ = adaptive_los_guidance_->desired_heading(); - pitch_d_ = adaptive_los_guidance_->desired_pitch(); - u_d_ = adaptive_los_guidance_->desired_surge_speed(target_waypoint_, current_position_); - adaptive_los_guidance_->update_adaptive_estimates(); - - // Publish feedback + reference - const vortex_msgs::msg::LOSGuidance reference_msg = fill_los_reference(); - feedback->feedback = reference_msg; - goal_handle->publish_feedback(feedback); - refrence_out->publish(reference_msg); - - // Goal check using explicit distance calc - const double dx = current_position_.x - target_waypoint_.x; - const double dy = current_position_.y - target_waypoint_.y; - const double dz = current_position_.z - target_waypoint_.z; - const double dist = std::sqrt(dx * dx + dy * dy + dz * dz); - if (dist <= 0.5) { - result->success = true; - goal_handle->succeed(result); - refrence_out->publish(fill_los_reference()); - spdlog::info("Goal reached"); - return; - } - - loop_rate.sleep(); - } -} - From 3bb892627bd89d847961fe2da87048f7a998d616 Mon Sep 17 00:00:00 2001 From: Anbit Date: Sun, 2 Nov 2025 12:36:03 +0100 Subject: [PATCH 021/128] fix: fix ignore lib --- .gitignore | 1 - .../include/los_guidance/lib/adaptive_los.hpp | 135 ++++++++++++++++++ .../include/los_guidance/lib/types.hpp | 56 ++++++++ .../los_guidance/src/lib/adaptive_los.cpp | 98 +++++++++++++ 4 files changed, 289 insertions(+), 1 deletion(-) create mode 100644 guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp create mode 100755 guidance/los_guidance/include/los_guidance/lib/types.hpp create mode 100644 guidance/los_guidance/src/lib/adaptive_los.cpp diff --git a/.gitignore b/.gitignore index f86dbd841..2a085ac8d 100644 --- a/.gitignore +++ b/.gitignore @@ -9,7 +9,6 @@ install/ log/ build/ bin/ -lib/ msg_gen/ srv_gen/ msg/*Action.msg diff --git a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp new file mode 100644 index 000000000..d7a50fec4 --- /dev/null +++ b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp @@ -0,0 +1,135 @@ +#ifndef LOS_GUIDANCE_HPP +#define LOS_GUIDANCE_HPP + +#include +#include +#include + +namespace vortex::guidance { + + namespace LOS { + + struct Point { + double x{}; + double y{}; + double z{}; + + Point operator-(const Point& other) const { + return Point{x - other.x, y - other.y, z - other.z}; + } + + Eigen::Vector3d as_vector() const { return Eigen::Vector3d(x, y, z); } + }; + + struct CrossTrackError { + double x_e{}; + double y_e{}; + double z_e{}; + + inline static CrossTrackError from_vector(const Eigen::Vector3d& vector) { + return CrossTrackError{vector.x(), vector.y(), vector.z()}; + } + }; + + struct Params { + double lookahead_distance_h{}; + double lookahead_distance_v{}; + double gamma_h{}; + double gamma_v{}; + double time_step{}; + }; + + } // namespace LOS + +/** + * @brief Adaptive Line-of-Sight (LOS) guidance algorithm based on slide 113 + * in "Fossen 2024 Lecture on 2D and 3D path-following control". + */ + + class AdaptiveLOSGuidance { + public: + AdaptiveLOSGuidance(const LOS::Params& params); + ~AdaptiveLOSGuidance() = default; + + void update_angles(const LOS::Point& prev_point, + const LOS::Point& next_point); + + LOS::CrossTrackError calculate_crosstrack_error( + const LOS::Point& prev_point, + const LOS::Point& current_position) const; + + double calculate_psi_d(const double& y_e) const; + + double calculate_theta_d(const double& z_e) const; + + void update_adaptive_estimates( + const LOS::CrossTrackError& crosstrack_error); + + private: + LOS::Params params_; + Eigen::Matrix3d rotation_y_ = Eigen::Matrix3d::Zero(); + Eigen::Matrix3d rotation_z_ = Eigen::Matrix3d::Zero(); + double pi_h_{}; + double pi_v_{}; + double beta_c_hat_{}; + double alpha_c_hat_{}; + }; + +// ---------------- Proportional LOS Guidance Implementation ---------------- + +/* + * Proportional Line-of-Sight (LOS) guidance algorithm based on page 356 + * form "The handbook of marine craft hydrodynamic and motion controll". +*/ + class ProportionalLOSGuidance { + public: + ProportionalLOSGuidance(const LOS::Params& params); + ~ProportionalLOSGuidance() = default; + + void update_angles(const LOS::Point& prev_point, + const LOS::Point& next_point); + + LOS::CrossTrackError calculate_crosstrack_error( + const LOS::Point& prev_point, + const LOS::Point& current_position) const; + + double calculate_psi_d(const double& y_e) const; + double calculate_theta_d(const double& z_e) const; + + private: + LOS::Params params_; + Eigen::Matrix3d rotation_y_{Eigen::Matrix3d::Zero()}; + Eigen::Matrix3d rotation_z_{Eigen::Matrix3d::Zero()}; + double pi_h_{}; + double pi_v_{}; + }; + +// ---------------- Integer LOS Guidance Implementation ---------------- + +/* + * Integer Line-of-Sight (LOS) guidance algorithm based on page 356 + * form "The handbook of marine craft hydrodynamic and motion controll". +*/ + + class IntergalLOSGuidance { + public: + IntergalLOSGuidance(const LOS::Params& params); + ~IntergalLOSGuidance() = default; + void update_angles(const LOS::Point& prev_point, + const LOS::Point& next_point); + LOS::CrossTrackError calculate_crosstrack_error( + const LOS::Point& prev_point, + const LOS::Point& current_position) const; + double calculate_psi_d(const double& y_e) const; + double calculate_theta_d(const double& z_e) const; + private: + LOS::Params params_; + Eigen::Matrix3d rotation_y_{Eigen::Matrix3d::Zero()}; + Eigen::Matrix3d rotation_z_{Eigen::Matrix3d::Zero()}; + double pi_h_{}; + double pi_v_{}; + double beta_c_hat_{}; + double alpha_c_hat_{}; + }; +} // namespace vortex::guidance +#endif // LOS_GUIDANCE_HPP \ No newline at end of file diff --git a/guidance/los_guidance/include/los_guidance/lib/types.hpp b/guidance/los_guidance/include/los_guidance/lib/types.hpp new file mode 100755 index 000000000..6c7643a6c --- /dev/null +++ b/guidance/los_guidance/include/los_guidance/lib/types.hpp @@ -0,0 +1,56 @@ +#ifndef types_hpp +#define types_hpp + +#include +#include +#include + +namespace vortex::guidance::lod::types{ + + namespace LOS { + + struct Point { + double x{}; + double y{}; + double z{}; + + Point operator-(const Point& other) const { + return Point{x - other.x, y - other.y, z - other.z}; + } + + Eigen::Vector3d as_vector() const { return Eigen::Vector3d(x, y, z); } + }; + + struct CrossTrackError { + double x_e{}; + double y_e{}; + double z_e{}; + + inline static CrossTrackError from_vector(const Eigen::Vector3d& vector) { + return CrossTrackError{vector.x(), vector.y(), vector.z()}; + } + }; + + struct Output { + double psi_d{}; + double theta_d{}; + }; + + struct Inputs{ + Point prev_point{}; + Point next_point{}; + Point current_position{}; + }; + + enum class Active_LOSMethod { + PROPORTIONAL, + INTEGRAL, + ADAPTIVE + }; + + + } // namespace LOS + +} // namespace vortex::guidance::los::types + +#endif \ No newline at end of file diff --git a/guidance/los_guidance/src/lib/adaptive_los.cpp b/guidance/los_guidance/src/lib/adaptive_los.cpp new file mode 100644 index 000000000..7e6b6a650 --- /dev/null +++ b/guidance/los_guidance/src/lib/adaptive_los.cpp @@ -0,0 +1,98 @@ +#include "los_guidance/lib/adaptive_los.hpp" + +namespace vortex::guidance { + +AdaptiveLOSGuidance::AdaptiveLOSGuidance(const LOS::Params& params) + : params_(params) {} + +void AdaptiveLOSGuidance::update_angles(const LOS::Point& prev_point, + const LOS::Point& next_point) { + const LOS::Point difference = next_point - prev_point; + + pi_h_ = atan2(difference.y, difference.x); + pi_v_ = atan2(-difference.z, sqrt(difference.x * difference.x + + difference.y * difference.y)); + + rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); + rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); +} + +LOS::CrossTrackError AdaptiveLOSGuidance::calculate_crosstrack_error( + const LOS::Point& prev_point, + const LOS::Point& current_position) const { + const LOS::Point difference = current_position - prev_point; + const Eigen::Vector3d difference_vector = difference.as_vector(); + + const Eigen::Vector3d cross_track_error = + rotation_y_.transpose() * rotation_z_.transpose() * difference_vector; + + return LOS::CrossTrackError::from_vector(cross_track_error); +} + +double AdaptiveLOSGuidance::calculate_psi_d(const double& y_e) const { + return pi_h_ - beta_c_hat_ - atan(y_e / params_.lookahead_distance_h); +} + +double AdaptiveLOSGuidance::calculate_theta_d(const double& z_e) const { + return pi_v_ + alpha_c_hat_ + atan(z_e / params_.lookahead_distance_v); +} + +void AdaptiveLOSGuidance::update_adaptive_estimates( + const LOS::CrossTrackError& crosstrack_error) { + double beta_c_hat_dot = + params_.gamma_h * + (params_.lookahead_distance_h / + sqrt(params_.lookahead_distance_h * params_.lookahead_distance_h + + crosstrack_error.y_e * crosstrack_error.y_e)) * + crosstrack_error.y_e; + double alpha_c_hat_dot = + params_.gamma_v * + (params_.lookahead_distance_v / + sqrt(params_.lookahead_distance_v * params_.lookahead_distance_v + + crosstrack_error.z_e * crosstrack_error.z_e)) * + crosstrack_error.z_e; + + beta_c_hat_ += beta_c_hat_dot * params_.time_step; + alpha_c_hat_ += alpha_c_hat_dot * params_.time_step; +} + +// ---------------- Proportional LOS Guidance Implementation ---------------- + +ProportionalLOSGuidance::ProportionalLOSGuidance(const LOS::Params& params) + : params_(params) {} + +void ProportionalLOSGuidance::update_angles(const LOS::Point& prev_point, + const LOS::Point& next_point) { + const LOS::Point difference = next_point - prev_point; + + pi_h_ = std::atan2(difference.y, difference.x); + pi_v_ = std::atan2(-difference.z, + std::sqrt(difference.x * difference.x + + difference.y * difference.y)); + + rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); + rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); +} + +LOS::CrossTrackError ProportionalLOSGuidance::calculate_crosstrack_error( + const LOS::Point& prev_point, + const LOS::Point& current_position) const { + const Eigen::Vector3d diff_vec = (current_position - prev_point).as_vector(); + + const Eigen::Vector3d e_perp = + rotation_y_.transpose() * rotation_z_.transpose() * diff_vec; + + return LOS::CrossTrackError::from_vector(e_perp); +} + +double ProportionalLOSGuidance::calculate_psi_d(const double& y_e) const { + const double k_p_h = 1.0 / std::max(params_.lookahead_distance_h, 1e-9); + return pi_h_ - std::atan(k_p_h * y_e); +} + +double ProportionalLOSGuidance::calculate_theta_d(const double& z_e) const { + const double k_p_v = 1.0 / std::max(params_.lookahead_distance_v, 1e-9); + return pi_v_ + std::atan(k_p_v * z_e); +} + +} // namespace vortex::guidance From 3afca9745957f5e964361e645e7714e54fc46c44 Mon Sep 17 00:00:00 2001 From: Henrik Date: Sun, 2 Nov 2025 16:18:36 +0100 Subject: [PATCH 022/128] Fixed LQR_solve_k_p --- control/velocity_controller/CMakeLists.txt | 69 +++++----------- .../include/velocity_controller/LQR_setup.hpp | 7 +- .../include/velocity_controller/test_VC.hpp | 9 +-- .../include/velocity_controller/utilities.hpp | 5 +- .../velocity_controller.hpp | 26 +++--- control/velocity_controller/package.xml | 1 + control/velocity_controller/src/LQR_setup.cpp | 78 ++++++++++++------ control/velocity_controller/src/LQR_test.cpp | 45 +++++++++++ control/velocity_controller/src/test_VC.cpp | 42 +++++----- .../src/velocity_controller.cpp | 79 ++++++++++--------- 10 files changed, 205 insertions(+), 156 deletions(-) create mode 100644 control/velocity_controller/src/LQR_test.cpp diff --git a/control/velocity_controller/CMakeLists.txt b/control/velocity_controller/CMakeLists.txt index f24299582..228f188b0 100644 --- a/control/velocity_controller/CMakeLists.txt +++ b/control/velocity_controller/CMakeLists.txt @@ -1,9 +1,7 @@ cmake_minimum_required(VERSION 3.8) project(velocity_controller) -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 20) -endif() +set(CMAKE_CXX_STANDARD 20) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) @@ -20,23 +18,14 @@ find_package(CasADi REQUIRED) find_package(LAPACK REQUIRED) find_package(BLAS REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) include_directories( ${EIGEN3_INCLUDE_DIR} - include ) - -#set(LIB_NAME "${PROJECT_NAME}_component") - -#add_library(${LIB_NAME} SHARED -# src/LQR_setup.cpp -# src/PID_setup.cpp -# src/test_VC.cpp -# src/velocity_controller.cpp -#) add_executable(velocity_controller_node src/velocity_controller.cpp src/PID_setup.cpp @@ -50,7 +39,11 @@ add_executable(test_VC_node src/LQR_setup.cpp src/utilities.cpp ) - +add_executable(test_LQR_node + src/LQR_test.cpp + src/LQR_setup.cpp + src/utilities.cpp +) ament_target_dependencies(velocity_controller_node rclcpp std_msgs @@ -58,6 +51,7 @@ ament_target_dependencies(velocity_controller_node geometry_msgs Eigen3 CasADi + nav_msgs ) @@ -67,6 +61,16 @@ ament_target_dependencies(test_VC_node vortex_msgs geometry_msgs Eigen3 + nav_msgs +) + +ament_target_dependencies(test_LQR_node + rclcpp + Eigen3 + vortex_msgs + geometry_msgs + std_msgs + nav_msgs ) link_directories(/usr/lib/gcc/x86_64-linux-gnu/11) @@ -75,11 +79,13 @@ set(GFORTRAN_LIB /usr/lib/gcc/x86_64-linux-gnu/11/libgfortran.so) target_link_libraries(velocity_controller_node ${LAPACK_LIBRARIES} ${BLAS_LIBRARIES} ${SLICOT_LIB} ${GFORTRAN_LIB}) target_link_libraries(test_VC_node ${LAPACK_LIBRARIES} ${BLAS_LIBRARIES} ${SLICOT_LIB} ${GFORTRAN_LIB}) +target_link_libraries(test_LQR_node ${LAPACK_LIBRARIES} ${BLAS_LIBRARIES} ${SLICOT_LIB} ${GFORTRAN_LIB}) install(TARGETS velocity_controller_node test_VC_node + test_LQR_node DESTINATION lib/${PROJECT_NAME} ) @@ -109,38 +115,3 @@ if(BUILD_TESTING) endif() ament_package() - -#add_executable(velocity_controller_node src/velocity_controller.cpp src/PID_setup.cpp) -#add_executable(test_VC_node src/test_VC.cpp src/PID_setup.cpp src/velocity_controller.cpp) - -#target_include_directories(velocity_controller_node PUBLIC -# $ -# $ -#) - -#target_link_libraries(velocity_controller_node ${drake_LIBRARIES}) -#install(DIRECTORY launch -# DESTINATION share/${PROJECT_NAME}/ -#) -#install(TARGETS velocity_controller_node -# DESTINATION lib/${PROJECT_NAME} -#) -#install(TARGETS test_VC_node -# DESTINATION lib/${PROJECT_NAME} -#) -#install(DIRECTORY config -# DESTINATION share/${PROJECT_NAME}/ -#) - -#rclcpp_components_register_node( -# ${LIB_NAME} -# PLUGIN "velocity_controller_node" -# EXECUTABLE ${PROJECT_NAME}_node -#) -#ament_export_targets(export_${LIB_NAME}) -#install(TARGETS ${LIB_NAME} -# EXPORT export_${LIB_NAME} -# ARCHIVE DESTINATION lib -# LIBRARY DESTINATION lib -# RUNTIME DESTINATION bin -#) diff --git a/control/velocity_controller/include/velocity_controller/LQR_setup.hpp b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp index 5a4bb2a9d..9b39f4746 100644 --- a/control/velocity_controller/include/velocity_controller/LQR_setup.hpp +++ b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp @@ -36,7 +36,8 @@ class LQRparameters{ struct LQRsolveResult{ Eigen::Matrix K; Eigen::Matrix P; - LQRsolveResult(Eigen::Matrix K,Eigen::Matrix P):K(K),P(P){}; + int INFO=0; + LQRsolveResult(Eigen::Matrix K,Eigen::Matrix P, int INFO=0):K(K),P(P),INFO(INFO) {}; }; class LQRController{ @@ -57,9 +58,9 @@ class LQRController{ Eigen::Vector saturate_input(Eigen::Vector u); Eigen::Vector update_error(Guidance_data guidance_values, State states); - Eigen::Vector calculate_lqr_u(Eigen::Matrix3d coriolis_matrix, State states, Guidance_data guidance_values); + Eigen::Vector calculate_lqr_u(State states, Guidance_data guidance_values); - LQRsolveResult solve_k_p(Eigen::Matrix A,Eigen::Matrix B,Eigen::Matrix Q, Eigen::Matrix R); + LQRsolveResult solve_k_p(const Eigen::Matrix &A,const Eigen::Matrix &B,const Eigen::Matrix &Q, const Eigen::Matrix &R); //Resets controller void reset_controller(); diff --git a/control/velocity_controller/include/velocity_controller/test_VC.hpp b/control/velocity_controller/include/velocity_controller/test_VC.hpp index 5e6c8c3da..c8974d097 100644 --- a/control/velocity_controller/include/velocity_controller/test_VC.hpp +++ b/control/velocity_controller/include/velocity_controller/test_VC.hpp @@ -23,8 +23,7 @@ class test_VC : public rclcpp::Node{ Guidance_data current_state; //Subscribers and publishers rclcpp::Publisher::SharedPtr publisher_guidance; - rclcpp::Publisher::SharedPtr publisher_twist; - rclcpp::Publisher::SharedPtr publisher_pose; + rclcpp::Publisher::SharedPtr publisher_odom; rclcpp::Subscription::SharedPtr subscription_thrust; //Timers rclcpp::TimerBase::SharedPtr timer_; @@ -34,14 +33,12 @@ class test_VC : public rclcpp::Node{ std_msgs::msg::Float64MultiArray reference_msg; //Topics - std::string topic_twist; - std::string topic_pose; + std::string topic_odom; std::string topic_thrust; std::string topic_guidance; //MSGS - geometry_msgs::msg::TwistWithCovarianceStamped twist_msg; - geometry_msgs::msg::PoseWithCovarianceStamped pose_msg; + nav_msgs::msg::Odometry odom_msg; }; geometry_msgs::msg::Quaternion euler_angle_to_quaternion(double roll, double pitch, double yaw); \ No newline at end of file diff --git a/control/velocity_controller/include/velocity_controller/utilities.hpp b/control/velocity_controller/include/velocity_controller/utilities.hpp index 82806dadf..c6627fa17 100644 --- a/control/velocity_controller/include/velocity_controller/utilities.hpp +++ b/control/velocity_controller/include/velocity_controller/utilities.hpp @@ -2,6 +2,7 @@ #include #include #include "std_msgs/msg/float64_multi_array.hpp" +#include "vortex_msgs/msg/los_guidance.hpp" class angle{ @@ -15,14 +16,14 @@ angle quaternion_to_euler_angle(double w, double x, double y, double z); class State{ //Dataclass to store state values for LQR controller public: - double surge=0.0; double pitch=0.0; double yaw=0.0; + double surge=0.0, pitch=0.0, yaw=0.0, pitch_rate=0.0, yaw_rate=0.0, heave_vel=0, sway_vel=0; double integral_surge=0.0; double integral_pitch=0.0; double integral_yaw=0.0; }; class Guidance_data:public State{ public: //double surge; double pitch; double yaw; - Guidance_data(std_msgs::msg::Float64MultiArray msg); + Guidance_data(vortex_msgs::msg::LOSGuidance msg):State{msg.surge,msg.pitch,msg.yaw}{}; Guidance_data(double surge, double pitch, double yaw):State{surge, pitch, yaw} {}; Guidance_data():State{0, 0, 0} {}; diff --git a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp index d9849df7c..e559297ef 100644 --- a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp +++ b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp @@ -9,9 +9,8 @@ #include #include "velocity_controller/PID_setup.hpp" #include "LQR_setup.hpp" - - -//#include "vortex-msgs/msg" kan legge til nye meldinger nå +#include "nav_msgs/msg/odometry.hpp" +#include "vortex_msgs/msg/los_guidance.hpp" @@ -26,10 +25,11 @@ class Velocity_node : public rclcpp::Node{ void calc_thrust(); //Callback functions - void guidance_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg_ptr); + void guidance_callback(const vortex_msgs::msg::LOSGuidance::SharedPtr msg_ptr); void killswitch_callback(const std_msgs::msg::Bool::SharedPtr msg_ptr); - void twist_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg_ptr); - void pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg_ptr); + //void twist_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg_ptr); + //void pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg_ptr); + void odometry_callback(const nav_msgs::msg::Odometry::SharedPtr msg_ptr); //Publisher instance rclcpp::Publisher::SharedPtr publisher_thrust; @@ -39,9 +39,10 @@ class Velocity_node : public rclcpp::Node{ rclcpp::TimerBase::SharedPtr timer_publish; //Subscriber instance - rclcpp::Subscription::SharedPtr subscriber_twist; - rclcpp::Subscription::SharedPtr subscriber_pose; - rclcpp::Subscription::SharedPtr subscriber_guidance; + //rclcpp::Subscription::SharedPtr subscriber_twist; + //rclcpp::Subscription::SharedPtr subscriber_pose; + rclcpp::Subscription::SharedPtr subscriber_Odometry; + rclcpp::Subscription::SharedPtr subscriber_guidance; rclcpp::Subscription::SharedPtr subscriber_killswitch; //Variables for topics @@ -49,8 +50,9 @@ class Velocity_node : public rclcpp::Node{ std::string topic_thrust; std::string topic_guidance; std::string topic_killswitch; - std::string topic_twist; - std::string topic_pose; + //std::string topic_twist; + //std::string topic_pose; + std::string topic_odometry; //Variables for timers int calculation_rate; @@ -58,7 +60,7 @@ class Velocity_node : public rclcpp::Node{ double max_force; //Stored wrenches values - std_msgs::msg::Float64MultiArray reference_in; + vortex_msgs::msg::LOSGuidance reference_in; Guidance_data guidance_values; Guidance_data current_state; geometry_msgs::msg::WrenchStamped thrust_out; diff --git a/control/velocity_controller/package.xml b/control/velocity_controller/package.xml index 87c5c268b..3f116b992 100644 --- a/control/velocity_controller/package.xml +++ b/control/velocity_controller/package.xml @@ -13,6 +13,7 @@ std_msgs vortex_msgs geometry_msgs + nav_msgs ament_lint_auto ament_lint_common diff --git a/control/velocity_controller/src/LQR_setup.cpp b/control/velocity_controller/src/LQR_setup.cpp index dcfb8e518..441f17813 100644 --- a/control/velocity_controller/src/LQR_setup.cpp +++ b/control/velocity_controller/src/LQR_setup.cpp @@ -125,10 +125,13 @@ Eigen::Vector LQRController::saturate_input(Eigen::Vector u) std::tie(yaw_windup, torque_z) = saturate(u[2], yaw_windup, max_force); return {force_x, torque_y, torque_z}; } -Eigen::Vector LQRController::calculate_lqr_u(Eigen::Matrix3d coriolis_matrix, State states, Guidance_data guidance_values){ - update_augmented_matrices(coriolis_matrix); +Eigen::Vector LQRController::calculate_lqr_u(State state, Guidance_data guidance_values){ + update_augmented_matrices(calculate_coriolis_matrix(state.pitch_rate,state.yaw_rate,state.sway_vel,state.heave_vel)); LQRsolveResult result = solve_k_p(augmented_system_matrix,augmented_input_matrix,state_weight_matrix,input_weight_matrix); - Eigen::Matrix state_error = update_error(guidance_values, states); + if(result.INFO!=0){ + return {9999,9999,9999}; //Need to fix + } + Eigen::Matrix state_error = update_error(guidance_values, state); Eigen::Vector u= saturate_input(- (result.K*state_error)); return u; } @@ -153,37 +156,60 @@ void sb02mt_( int* IPIV, const int* OUFACT, double* G, const int* LDG, int* IWORK, double* DWORK, const int* LDWORK, int* INFO ); +void sb02md_( const char* DICO, const char* HINV, const char* UPLO, const char* SCAL, const char* SORT, const int* N, double* A, const int* LDA, double* G, + const int* LDG, double* Q, const int* LDQ, const double* RCOND, double* WR, double* WI, double* S, const int* LDS, double* U, const int* LDU, + int* IWORK, double* DWORK, const int* LDWORK, int* BWORK, const int* INFO + ); } -LQRsolveResult LQRController::solve_k_p(Eigen::Matrix A,Eigen::Matrix B, Eigen::Matrix Q,Eigen::Matrix R){ +LQRsolveResult LQRController::solve_k_p(const Eigen::Matrix &A,const Eigen::Matrix &B, const Eigen::Matrix &Q, const Eigen::Matrix &R){ + Eigen::Matrix A_copy=A, Q_copy=Q; + Eigen::Matrix B_copy=B; Eigen::Matrix R_copy=R; + //First calculate G with sb02mt_ - char JOBG='G'; //calculate G - char JOBL='Z'; //L is zero - char FACT='N'; //unfactored R - char UPLO='U'; //Upper triangle i think - const int N=6; //Order of matrices A, Q, G and X(P) - const int M=3; //Order of matrix R and nuber of columns in B and L(is zero) - int LDA=N, LDB=M, LDQ=N,LDR=M,LDL=N,LDG=N; - - std::vector IWORK(N); - int LDWORK=10*N*N; //Upper bounds + //calculate G, L is zero, unfactored R, Upper triangle i think + char JOBG='G',JOBL='Z',FACT='N',UPLO='U'; + //Order of matrices A, Q, G and X(P), Order of matrix R and nuber of columns in B and L(is zero) + const int N=6, M=3; + //Dimensions of matrices + int LDA=N, LDB=N, LDQ=N,LDR=M,LDL=N,LDG=N; + std::vector IWORK(8*N),IPIV(N); + //Upper bounds Output but initialized JIC output placeholder + int LDWORK=20*N*N,OUFACT=0,INFO=0; std::vector DWORK(LDWORK); - std::vector IPIV(N); - int OUFACT=0; //Output but initialized JIC - Eigen::Matrix L=Eigen::Matrix::Zero(); - Eigen::Matrix G; - int INFO; - sb02mt_(&JOBG,&JOBL,&FACT,&UPLO,&N,&M,A.data(),&LDA,B.data(),&LDB,Q.data(),&LDQ,R.data(),&LDR,L.data(),&LDL,IPIV.data(),&OUFACT,G.data(),&LDG,IWORK.data(),DWORK.data(),&LDWORK,&INFO); - + Eigen::Matrix L=Eigen::Matrix::Zero(), G=L; + sb02mt_(&JOBG,&JOBL,&FACT,&UPLO,&N,&M,A_copy.data(),&LDA,B_copy.data(),&LDB,Q_copy.data(),&LDQ,R_copy.data(),&LDR,L.data(),&LDL,IPIV.data(),&OUFACT,G.data(),&LDG,IWORK.data(),DWORK.data(),&LDWORK,&INFO); + Eigen::Matrix K; if (INFO!=0){ //Some Error handling here. Also check that BRB in invertible + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "sb02mt_ returned INFO=%d", INFO); + // Consider throwing or returning a default result. We'll return zeroed K and G for now. + Eigen::Matrix K_zero = Eigen::Matrix::Zero(); + return LQRsolveResult(K_zero, G,INFO); + + } + char DICO='D',HINV='D',SCAL='N',SORT='U'; + std::vector WR(2*N,0),WI(2*N,0),RCOND(2*N,0); + int BWORK[8*N]; + Eigen::Matrix S=Eigen::Matrix::Zero(); + Eigen::MatrixU=Eigen::Matrix::Zero(); + int LDS=2*N,LDU=2*N,INFO1=0; + A_copy=A;Q_copy=Q; R_copy=R; + sb02md_(&DICO,&HINV,&UPLO,&SCAL,&SORT,&N,A_copy.data(),&LDA,G.data(),&LDG,Q_copy.data(),&LDQ,RCOND.data(),WR.data(),WI.data(),S.data(),&LDS,U.data(),&LDU,IWORK.data(),DWORK.data(),&LDWORK,BWORK,&INFO1); + if (INFO1!=0){ + //Some Error handling here. Also check that BRB in invertible + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "sb02md_ returned INFO=%d", INFO1); + // Consider throwing or returning a default result. We'll return zeroed K and G for now. + Eigen::Matrix K_zero = Eigen::Matrix::Zero(); + return LQRsolveResult(K_zero, G,INFO1); } - Eigen::Matrix K; - Eigen::Matrix BRB = R+B.transpose()*G*B; - K=BRB.inverse()*B.transpose()*G*A; - - return LQRsolveResult(K,G); + Eigen::MatrixU11=U.topRows(6); + Eigen::MatrixU21=U.bottomRows(6); + Eigen::MatrixXd X=U21*U11.inverse(); + K=R.inverse()*B.transpose()*X; + + return LQRsolveResult(K,G,INFO1); } diff --git a/control/velocity_controller/src/LQR_test.cpp b/control/velocity_controller/src/LQR_test.cpp new file mode 100644 index 000000000..108c2354c --- /dev/null +++ b/control/velocity_controller/src/LQR_test.cpp @@ -0,0 +1,45 @@ +#include "rclcpp/rclcpp.hpp" +#include +#include "velocity_controller/LQR_setup.hpp" + +class test_LQR_node : public rclcpp::Node{ + public: + double q_surge =75.0, q_pitch= 175.0, q_yaw= 175.0, r_surge= 0.3, r_pitch= 0.4, r_yaw= 0.4, i_surge= 0.3, + i_pitch= 0.4, i_yaw= 0.3, i_weight= 0.5, dt= 0.1; + LQRparameters param={q_surge, q_pitch, q_yaw, r_surge, r_pitch, r_yaw, i_surge, i_pitch, i_yaw, i_weight}; + LQRController controller; + test_LQR_node():Node("test_LQR_node"), controller(){ + RCLCPP_INFO(this->get_logger(),"LQR test node started"); + Eigen::Matrix Q=(Eigen::Matrix()<<75,0,0,0,0,0,0,175,0,0,0,0,0,0,175,0,0,0,0,0,0,0.3,0,0,0,0,0,0,0.4,0,0,0,0,0,0,0.3).finished(); + Eigen::Matrix R=(Eigen::Matrix3d()<<0.3,0,0,0,0.4,0,0,0,0.4).finished(); + Eigen::Matrix A=(Eigen::Matrix()<<5,7,23,0,0,0,0,45,21,4,3,4,0,23,1,7,6,5,5,7,6,3,5,7,2,2,3,2,1,0,0,0,8,7,6,5).finished(); + Eigen::Matrix B=(Eigen::Matrix()<<2,0,0,3,0,2,0,3,0,1,2,0,3,4,0,3,5,0).finished(); + /*Eigen::Matrix3d inertia_matrix=(Eigen::Matrix3d()<<30.0, 0.6, 0.0, 0.6, 1.629, 0.0, 0.0, 0.0, 1.729).finished(); + Eigen::Matrix3d inertia_matrix_inv=inertia_matrix.inverse(); + Eigen::Matrix3d coriolis_matrix=(Eigen::Matrix3d()<<0.2,-30*2*0.01,-30*2*0.0,30 * 2*0.01,0,1.629 * 2,30 * 2*0.01,1.769 * 2,0).finished(); + Eigen::Matrix3d system_matrix=inertia_matrix.inverse()*coriolis_matrix; + Eigen::Matrix augmented_system_matrix =(Eigen::Matrix()< augmented_input_matrix=(Eigen::Matrix()<< inertia_matrix_inv(0,0),inertia_matrix_inv(0,1),inertia_matrix_inv(0,2),0,0,0, + inertia_matrix_inv(1,0),inertia_matrix_inv(1,1),inertia_matrix_inv(1,2),0,0,0, + inertia_matrix_inv(2,0),inertia_matrix_inv(2,1),inertia_matrix_inv(2,2),0,0,0).finished();*/ + LQRsolveResult result=controller.solve_k_p(A,B,Q,R); + RCLCPP_INFO(this->get_logger(),"LQR Gain K matrix:"); + RCLCPP_INFO(this->get_logger(),"\n%f %f %f %f %f %f\n%f %f %f %f %f %f\n%f %f %f %f %f %f", + result.K(0,0),result.K(0,1),result.K(0,2),result.K(0,3),result.K(0,4),result.K(0,5), + result.K(1,0),result.K(1,1),result.K(1,2),result.K(1,3),result.K(1,4),result.K(1,5), + result.K(2,0),result.K(2,1),result.K(2,2),result.K(2,3),result.K(2,4),result.K(2,5)); + } +}; + +int main(int argc, char const *argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/control/velocity_controller/src/test_VC.cpp b/control/velocity_controller/src/test_VC.cpp index c6ccfdec1..567174702 100644 --- a/control/velocity_controller/src/test_VC.cpp +++ b/control/velocity_controller/src/test_VC.cpp @@ -15,16 +15,12 @@ test_VC::test_VC() : Node("test_VC_node"), current_state(0,2,2) { this->declare_parameter("topics.guidance_topic"); this->declare_parameter("topics.thrust_topic"); - this->declare_parameter("topics.twist_topic"); - this->declare_parameter("topics.pose_topic"); + this->declare_parameter("topics.odom_topic"); topic_thrust = this->get_parameter("topics.thrust_topic").as_string(); - topic_twist = this->get_parameter("topics.twist_topic").as_string(); - topic_pose = this->get_parameter("topics.pose_topic").as_string(); - + topic_odom = this->get_parameter("topics.odom_topic").as_string(); topic_guidance = this->get_parameter("topics.guidance_topic").as_string(); publisher_guidance = this->create_publisher(topic_guidance, 10); - publisher_twist = this->create_publisher(topic_twist,10); - publisher_pose = this->create_publisher(topic_pose,10); + publisher_odom = this->create_publisher(topic_odom,10); subscription_thrust = this->create_subscription( topic_thrust, 10, @@ -46,28 +42,34 @@ void test_VC::send_guidance() void test_VC::read_thrust(geometry_msgs::msg::WrenchStamped::SharedPtr msg) { - current_state.surge += 0.01 * msg->wrench.force.x; + /*current_state.surge += 0.01 * msg->wrench.force.x; current_state.pitch += 0.01 * msg->wrench.torque.x; - current_state.yaw += 0.01 * msg->wrench.torque.y; - RCLCPP_INFO(this->get_logger(),"info: '%f'", current_state.surge); - RCLCPP_INFO(this->get_logger(),"info: '%f'", current_state.pitch); - RCLCPP_INFO(this->get_logger(),"info: '%f'", current_state.yaw); + current_state.yaw += 0.01 * msg->wrench.torque.y;*/ + //RCLCPP_INFO(this->get_logger(),"info: '%f'", current_state.surge); + //RCLCPP_INFO(this->get_logger(),"info: '%f'", current_state.pitch); + //RCLCPP_INFO(this->get_logger(),"info: '%f'", current_state.yaw); return; } void test_VC::send_state() { - twist_msg.header.stamp = clock_->now(); - twist_msg.header.frame_id = "base_link"; - twist_msg.twist.twist.linear.x = current_state.surge; + odom_msg.header.stamp = clock_->now(); + odom_msg.header.frame_id = "base_link"; + odom_msg.twist.twist.linear.x = current_state.surge; + odom_msg.pose.pose.orientation = euler_angle_to_quaternion(0.0, current_state.pitch, current_state.yaw); + odom_msg.twist.twist.linear.y=1; + odom_msg.twist.twist.linear.z=1; + odom_msg.twist.twist.angular.x=1; + odom_msg.twist.twist.angular.y=1; + odom_msg.twist.twist.angular.z=1; + odom_msg.twist.twist.linear.y=1; + odom_msg.twist.twist.linear.z=1; + + - pose_msg.header.stamp = clock_->now(); - pose_msg.header.frame_id = "base_link"; - pose_msg.pose.pose.orientation = euler_angle_to_quaternion(0.0, current_state.pitch, current_state.yaw); - publisher_twist->publish(twist_msg); - publisher_pose->publish(pose_msg); + publisher_odom->publish(odom_msg); //RCLCPP_INFO(this->get_logger(), "Published state: '%f'", current_state.surge); return; diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index dcb2c7103..804731916 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -9,6 +9,7 @@ #include "velocity_controller/PID_setup.hpp" #include #include +#include "vortex_msgs/msg/los_guidance.hpp" //#include "vortex-msgs/msg" kan legge til nye meldinger nå //Lager en klasse velocity node @@ -26,16 +27,11 @@ Velocity_node::Velocity_node() : Node("velocity_controller_node"), PID_surge(1,1 // Publishers publisher_thrust = create_publisher(topic_thrust, 10); - //Subscribers - subscriber_twist = this->create_subscription( - topic_twist, 10, - std::bind(&Velocity_node::twist_callback,this, std::placeholders::_1)); - - subscriber_pose = this->create_subscription( - topic_pose, 10, - std::bind(&Velocity_node::pose_callback,this, std::placeholders::_1)); - - subscriber_guidance = this->create_subscription( + //Subscribers + subscriber_Odometry = this->create_subscription( + topic_odometry,10, + std::bind(&Velocity_node::odometry_callback,this,std::placeholders::_1)); + subscriber_guidance = this->create_subscription( topic_guidance,10, std::bind(&Velocity_node::guidance_callback,this, std::placeholders::_1)); subscriber_killswitch = this->create_subscription( @@ -69,7 +65,8 @@ void Velocity_node::calc_thrust() { switch (controller_type) { - case 1: + case 1:{ + RCLCPP_INFO(this->get_logger(),"PID controller"); PID_surge.calculate_thrust(guidance_values.surge, current_state.surge,calculation_rate/1000.0); PID_pitch.calculate_thrust(guidance_values.pitch, current_state.pitch,calculation_rate/1000.0); PID_yaw.calculate_thrust(guidance_values.yaw, current_state.yaw,calculation_rate/1000.0); @@ -77,33 +74,46 @@ void Velocity_node::calc_thrust() thrust_out.wrench.torque.y = PID_pitch.output(); thrust_out.wrench.torque.z = PID_yaw.output(); break; - case 2: - lqr_controller.update_error(guidance_values,current_state); - default: + } + case 2:{ + //RCLCPP_INFO(this->get_logger(),"LQR controller"); + Eigen::Vector3d u=lqr_controller.calculate_lqr_u(current_state,guidance_values); + if (u==Eigen::Vector3d{9999,9999,9999}){ + controller_type=1; + } + else{ + thrust_out.wrench.force.x=u[0]; + thrust_out.wrench.torque.y=u[1]; + thrust_out.wrench.torque.z=u[2]; + } break; } + default:{ + break; + } + } - + publisher_thrust->publish(thrust_out); return; } //Callback functions -void Velocity_node::guidance_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg_ptr){ - //RCLCPP_INFO(this->get_logger(), "Received reference: '%f'", msg_ptr->wrench.force.x); +void Velocity_node::guidance_callback(const vortex_msgs::msg::LOSGuidance::SharedPtr msg_ptr){ guidance_values = *msg_ptr; return; } -void Velocity_node::twist_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg_ptr){ - //RCLCPP_INFO(this->get_logger(), "Received velocity and orientation: '%f'", msg_ptr->wrench.force.x); - current_state.surge = msg_ptr->twist.twist.linear.x; - return; -} -void Velocity_node::pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg_ptr){ + +void Velocity_node::odometry_callback(const nav_msgs::msg::Odometry::SharedPtr msg_ptr){ + //RCLCPP_INFO(this->get_logger(),"Recieved odometry"); angle temp=quaternion_to_euler_angle(msg_ptr->pose.pose.orientation.w, msg_ptr->pose.pose.orientation.x, msg_ptr->pose.pose.orientation.y, msg_ptr->pose.pose.orientation.z); current_state.pitch = temp.thetat; current_state.yaw = temp.psit; + current_state.surge = msg_ptr->twist.twist.linear.x; + current_state.pitch_rate=msg_ptr->twist.twist.angular.y; + current_state.yaw_rate=msg_ptr->twist.twist.angular.z; + //Need to update angular speed NB!!. return; } @@ -124,10 +134,12 @@ void Velocity_node::get_new_parameters(){ this->topic_thrust = this->get_parameter("topics.thrust_topic").as_string(); this->declare_parameter("topics.guidance_topic"); this->topic_guidance = this->get_parameter("topics.guidance_topic").as_string(); - this->declare_parameter("topics.twist_topic"); - this->topic_twist = this->get_parameter("topics.twist_topic").as_string(); - this->declare_parameter("topics.pose_topic"); - this->topic_pose = this->get_parameter("topics.pose_topic").as_string(); + //this->declare_parameter("topics.twist_topic"); + //this->topic_twist = this->get_parameter("topics.twist_topic").as_string(); + //this->declare_parameter("topics.pose_topic"); + //this->topic_pose = this->get_parameter("topics.pose_topic").as_string(); + this->declare_parameter("topics.odom_topic"); + this->topic_odometry = this->get_parameter("topics.odom_topic").as_string(); this->declare_parameter("topics.killswitch_topic"); this->topic_killswitch = this->get_parameter("topics.killswitch_topic").as_string(); this->declare_parameter("max_force"); @@ -180,6 +192,7 @@ int main(int argc, char * argv[]) //---------------------------------------------------------------------------------------------------------------- //Operator overloading for geometry_msgs::msg::WrenchStamped +/* geometry_msgs::msg::WrenchStamped operator-(const geometry_msgs::msg::WrenchStamped & a, const geometry_msgs::msg::WrenchStamped & b) { geometry_msgs::msg::WrenchStamped result; @@ -225,15 +238,5 @@ Guidance_data& Guidance_data::operator=(const std_msgs::msg::Float64MultiArray& } return *this; } +*/ -Guidance_data::Guidance_data(std_msgs::msg::Float64MultiArray msg){ - if (msg.data.size()>=3){ - surge=msg.data[0]; - pitch=msg.data[1]; - yaw=msg.data[2]; - } - else{ - //throw std::runtime_error("Guidance message too short, needs at least 3 values"); - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Guidance message too short, needs at least 3 values"); - } - }; \ No newline at end of file From 2f6c253dbc9b4268e414ef17ad89443e1ca800e9 Mon Sep 17 00:00:00 2001 From: Henrik Date: Wed, 5 Nov 2025 11:11:04 +0100 Subject: [PATCH 023/128] Changed LQR test file, it works --- control/velocity_controller/src/LQR_test.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/control/velocity_controller/src/LQR_test.cpp b/control/velocity_controller/src/LQR_test.cpp index 108c2354c..4358f61e6 100644 --- a/control/velocity_controller/src/LQR_test.cpp +++ b/control/velocity_controller/src/LQR_test.cpp @@ -33,6 +33,7 @@ class test_LQR_node : public rclcpp::Node{ result.K(0,0),result.K(0,1),result.K(0,2),result.K(0,3),result.K(0,4),result.K(0,5), result.K(1,0),result.K(1,1),result.K(1,2),result.K(1,3),result.K(1,4),result.K(1,5), result.K(2,0),result.K(2,1),result.K(2,2),result.K(2,3),result.K(2,4),result.K(2,5)); + } }; From 9ec7fd48b8123f4a9db539e0204ff1741084fcb1 Mon Sep 17 00:00:00 2001 From: Henrik Date: Wed, 5 Nov 2025 11:11:50 +0100 Subject: [PATCH 024/128] Changed launch file, fixed QoS problem and changed ouput topic --- .../config/parameters.yaml | 8 ++--- .../launch/VCnTest.launch.py | 33 ++++++++++++++++--- control/velocity_controller/src/test_VC.cpp | 8 +++-- .../src/velocity_controller.cpp | 9 +++-- 4 files changed, 44 insertions(+), 14 deletions(-) diff --git a/control/velocity_controller/config/parameters.yaml b/control/velocity_controller/config/parameters.yaml index bcdd2a151..7f08156e6 100644 --- a/control/velocity_controller/config/parameters.yaml +++ b/control/velocity_controller/config/parameters.yaml @@ -6,7 +6,7 @@ twist_topic: /dvl/twist #Twist pose_topic: /dvl/pose #Pose guidance_topic: /guidance/los #Guidance - thrust_topic: /thrust/wrench_input #Thrust + thrust_topic: /orca/wrench_input #Thrust softwareoperation_topic: /softwareOperationMode #Software Operation killswitch_topic: /softwareKillSwitch #Kill Switch @@ -30,8 +30,8 @@ inertia_matrix: [30.0, 0.6, 0.0, 0.6, 1.629, 0.0, 0.0, 0.0, 1.729] calculation_rate: 200 #ms integer - publish_rate: 200 #ms + publish_rate: 1000 #ms #Clamp parameter max_force: 99.5 - controller_type: 1 - #publish_value: "Hello from config!" # parameter name: parameter value + controller_type: 1 #1 PID 2 LQR + diff --git a/control/velocity_controller/launch/VCnTest.launch.py b/control/velocity_controller/launch/VCnTest.launch.py index ab2b64fd8..bc53f7daa 100644 --- a/control/velocity_controller/launch/VCnTest.launch.py +++ b/control/velocity_controller/launch/VCnTest.launch.py @@ -1,9 +1,10 @@ from launch import LaunchDescription from launch_ros.actions import Node import os -#from launch.actions import IncludeLaunchDescription +from launch.actions import IncludeLaunchDescription, TimerAction +from launch.actions import IncludeLaunchDescription from launch.actions import DeclareLaunchArgument -#from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.launch_description_sources import PythonLaunchDescriptionSource from ament_index_python.packages import get_package_share_directory from launch.substitutions import LaunchConfiguration @@ -11,20 +12,42 @@ def generate_launch_description(): pkg_share = get_package_share_directory('velocity_controller') config_path = os.path.join(pkg_share, 'config', 'parameters.yaml') + stonefish_dir = get_package_share_directory('stonefish_sim') + + stonefish_sim = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(stonefish_dir, 'launch', 'simulation.launch.py') + ), + launch_arguments={'scenario': 'tacc','rendering_quality': 'low','rendering':'true'}.items(), + ) + orca_sim = TimerAction( + period=12.0, + actions=[ + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(stonefish_dir, 'launch', 'orca_sim.launch.py') + ) + ) + ] + ) + + node_name_arg = DeclareLaunchArgument( 'node_name', default_value='velocity_controller_node', description='Name of the velocity controller node' ) node_name_arg2 = DeclareLaunchArgument( - 'node_name', default_value='test_VC_node', + 'node_name_1', default_value='test_VC_node', description='Name of the test VC node' ) velocity_controller_name = LaunchConfiguration('node_name') - test_VC_name = LaunchConfiguration('node_name') + test_VC_name = LaunchConfiguration('node_name_1') return LaunchDescription([ + stonefish_sim, + orca_sim, node_name_arg, node_name_arg2, Node(package='velocity_controller', @@ -36,5 +59,5 @@ def generate_launch_description(): executable='test_VC_node', name=test_VC_name, output='screen', - parameters=[config_path]) + parameters=[config_path]) ]) \ No newline at end of file diff --git a/control/velocity_controller/src/test_VC.cpp b/control/velocity_controller/src/test_VC.cpp index 567174702..617cbd34a 100644 --- a/control/velocity_controller/src/test_VC.cpp +++ b/control/velocity_controller/src/test_VC.cpp @@ -22,8 +22,11 @@ test_VC::test_VC() : Node("test_VC_node"), current_state(0,2,2) publisher_guidance = this->create_publisher(topic_guidance, 10); publisher_odom = this->create_publisher(topic_odom,10); + rclcpp::QoS orca_QoS(2); + orca_QoS.keep_last(2).reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT); + subscription_thrust = this->create_subscription( - topic_thrust, 10, + topic_thrust, orca_QoS, std::bind(&test_VC::read_thrust, this, std::placeholders::_1)); timer_ = this->create_wall_timer( std::chrono::milliseconds(200), @@ -37,7 +40,7 @@ test_VC::test_VC() : Node("test_VC_node"), current_state(0,2,2) void test_VC::send_guidance() { publisher_guidance->publish(reference_msg); - send_state(); + //send_state(); } void test_VC::read_thrust(geometry_msgs::msg::WrenchStamped::SharedPtr msg) @@ -48,6 +51,7 @@ void test_VC::read_thrust(geometry_msgs::msg::WrenchStamped::SharedPtr msg) //RCLCPP_INFO(this->get_logger(),"info: '%f'", current_state.surge); //RCLCPP_INFO(this->get_logger(),"info: '%f'", current_state.pitch); //RCLCPP_INFO(this->get_logger(),"info: '%f'", current_state.yaw); + (void) msg; return; } diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index 804731916..9974a77b2 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -15,7 +15,7 @@ //Lager en klasse velocity node //Konstruktør -Velocity_node::Velocity_node() : Node("velocity_controller_node"), PID_surge(1,1,1), PID_yaw(1,1,1), PID_pitch(1,1,1), lqr_controller() +Velocity_node::Velocity_node() : Node("velocity_controller_node"), PID_surge(10,1,1), PID_yaw(10,1,1), PID_pitch(10,1,1), lqr_controller() { //Dytter info til log RCLCPP_INFO(this->get_logger(), "Velocity control node has been started."); @@ -25,7 +25,10 @@ Velocity_node::Velocity_node() : Node("velocity_controller_node"), PID_surge(1,1 // Publishers - publisher_thrust = create_publisher(topic_thrust, 10); + rclcpp::QoS orca_QoS(2); + orca_QoS.keep_last(2).reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT); + + publisher_thrust = create_publisher(topic_thrust, orca_QoS); //Subscribers subscriber_Odometry = this->create_subscription( @@ -66,7 +69,7 @@ void Velocity_node::calc_thrust() switch (controller_type) { case 1:{ - RCLCPP_INFO(this->get_logger(),"PID controller"); + //RCLCPP_INFO(this->get_logger(),"PID controller"); PID_surge.calculate_thrust(guidance_values.surge, current_state.surge,calculation_rate/1000.0); PID_pitch.calculate_thrust(guidance_values.pitch, current_state.pitch,calculation_rate/1000.0); PID_yaw.calculate_thrust(guidance_values.yaw, current_state.yaw,calculation_rate/1000.0); From b7f63a90cced3d38fdd21097af5dc97bca7f97a2 Mon Sep 17 00:00:00 2001 From: Anbit Adhikari Date: Wed, 5 Nov 2025 13:13:27 +0100 Subject: [PATCH 025/128] Test: add test files for ALos and Plos --- guidance/los_guidance/CMakeLists.txt | 2 + .../include/los_guidance/lib/adaptive_los.hpp | 148 +++----------- .../include/los_guidance/lib/integral_los.hpp | 32 +++ .../los_guidance/lib/proportional_los.hpp | 38 ++++ .../include/los_guidance/lib/types.hpp | 88 ++++----- .../los_guidance/src/lib/adaptive_los.cpp | 122 ++++-------- .../los_guidance/src/lib/integral_los.cpp | 1 + .../los_guidance/src/lib/proportional_los.cpp | 38 ++++ guidance/los_guidance/test/CMakeLists.txt | 5 +- .../los_guidance/test/adaptive_los_test.cpp | 179 +++++++++++++++++ .../los_guidance/test/integral_los_test.cpp | 0 .../test/proportional_los_test.cpp | 113 +++++++++++ .../los_guidance/test/test_los_guidance.cpp | 182 ------------------ 13 files changed, 519 insertions(+), 429 deletions(-) create mode 100755 guidance/los_guidance/include/los_guidance/lib/integral_los.hpp create mode 100755 guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp create mode 100755 guidance/los_guidance/src/lib/integral_los.cpp create mode 100755 guidance/los_guidance/src/lib/proportional_los.cpp create mode 100644 guidance/los_guidance/test/adaptive_los_test.cpp create mode 100755 guidance/los_guidance/test/integral_los_test.cpp create mode 100755 guidance/los_guidance/test/proportional_los_test.cpp delete mode 100644 guidance/los_guidance/test/test_los_guidance.cpp diff --git a/guidance/los_guidance/CMakeLists.txt b/guidance/los_guidance/CMakeLists.txt index 9bde13d8f..34996951e 100644 --- a/guidance/los_guidance/CMakeLists.txt +++ b/guidance/los_guidance/CMakeLists.txt @@ -23,6 +23,8 @@ include_directories(include) set(LIB_NAME los_guidance_lib) add_library(${LIB_NAME} SHARED + src/lib/proportional_los.cpp + src/lib/integral_los.cpp src/lib/adaptive_los.cpp src/los_guidance_ros.cpp ) diff --git a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp index d7a50fec4..ee655ccb8 100644 --- a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp @@ -1,135 +1,49 @@ -#ifndef LOS_GUIDANCE_HPP -#define LOS_GUIDANCE_HPP +#ifndef ADAPTIVE_LOS_GUIDANCE_HPP +#define ADAPTIVE_LOS_GUIDANCE_HPP #include #include +#include "los_guidance/lib/types.hpp" #include -namespace vortex::guidance { - - namespace LOS { - - struct Point { - double x{}; - double y{}; - double z{}; - - Point operator-(const Point& other) const { - return Point{x - other.x, y - other.y, z - other.z}; - } - - Eigen::Vector3d as_vector() const { return Eigen::Vector3d(x, y, z); } - }; - - struct CrossTrackError { - double x_e{}; - double y_e{}; - double z_e{}; - - inline static CrossTrackError from_vector(const Eigen::Vector3d& vector) { - return CrossTrackError{vector.x(), vector.y(), vector.z()}; - } - }; - - struct Params { - double lookahead_distance_h{}; - double lookahead_distance_v{}; - double gamma_h{}; - double gamma_v{}; - double time_step{}; - }; - - } // namespace LOS - /** * @brief Adaptive Line-of-Sight (LOS) guidance algorithm based on slide 113 * in "Fossen 2024 Lecture on 2D and 3D path-following control". */ - class AdaptiveLOSGuidance { - public: - AdaptiveLOSGuidance(const LOS::Params& params); - ~AdaptiveLOSGuidance() = default; - - void update_angles(const LOS::Point& prev_point, - const LOS::Point& next_point); - - LOS::CrossTrackError calculate_crosstrack_error( - const LOS::Point& prev_point, - const LOS::Point& current_position) const; - - double calculate_psi_d(const double& y_e) const; - - double calculate_theta_d(const double& z_e) const; +namespace vortex::guidance::los { - void update_adaptive_estimates( - const LOS::CrossTrackError& crosstrack_error); + struct AdaptiveLosParams { + double lookahead_distance_h{}; + double lookahead_distance_v{}; + double gamma_h{}; + double gamma_v{}; + double time_step{}; + }; - private: - LOS::Params params_; - Eigen::Matrix3d rotation_y_ = Eigen::Matrix3d::Zero(); - Eigen::Matrix3d rotation_z_ = Eigen::Matrix3d::Zero(); - double pi_h_{}; - double pi_v_{}; - double beta_c_hat_{}; - double alpha_c_hat_{}; - }; - -// ---------------- Proportional LOS Guidance Implementation ---------------- - -/* - * Proportional Line-of-Sight (LOS) guidance algorithm based on page 356 - * form "The handbook of marine craft hydrodynamic and motion controll". -*/ - class ProportionalLOSGuidance { - public: - ProportionalLOSGuidance(const LOS::Params& params); - ~ProportionalLOSGuidance() = default; - - void update_angles(const LOS::Point& prev_point, - const LOS::Point& next_point); + class AdaptiveLOSGuidance { + public: + AdaptiveLOSGuidance(const AdaptiveLosParams& params); + ~AdaptiveLOSGuidance() = default; - LOS::CrossTrackError calculate_crosstrack_error( - const LOS::Point& prev_point, - const LOS::Point& current_position) const; + types::Output calculate_outputs(const types::Inputs& inputs); - double calculate_psi_d(const double& y_e) const; - double calculate_theta_d(const double& z_e) const; + private: + void update_angles(const types::Inputs& inputs); + const types::CrossTrackError calculate_crosstrack_error(const types::Inputs& inputs); + void update_adaptive_estimates(const types::CrossTrackError& cross_track_error); - private: - LOS::Params params_; - Eigen::Matrix3d rotation_y_{Eigen::Matrix3d::Zero()}; - Eigen::Matrix3d rotation_z_{Eigen::Matrix3d::Zero()}; - double pi_h_{}; - double pi_v_{}; - }; + AdaptiveLosParams m_params{}; + //usikker om disse skal vaere med i den nye strukturen? + Eigen::Matrix3d rotation_y_ = Eigen::Matrix3d::Zero(); + Eigen::Matrix3d rotation_z_ = Eigen::Matrix3d::Zero(); + double pi_h_{}; + double pi_v_{}; + double beta_c_hat_{}; + double alpha_c_hat_{}; -// ---------------- Integer LOS Guidance Implementation ---------------- -/* - * Integer Line-of-Sight (LOS) guidance algorithm based on page 356 - * form "The handbook of marine craft hydrodynamic and motion controll". -*/ + }; // namespace vortex::guidance::los - class IntergalLOSGuidance { - public: - IntergalLOSGuidance(const LOS::Params& params); - ~IntergalLOSGuidance() = default; - void update_angles(const LOS::Point& prev_point, - const LOS::Point& next_point); - LOS::CrossTrackError calculate_crosstrack_error( - const LOS::Point& prev_point, - const LOS::Point& current_position) const; - double calculate_psi_d(const double& y_e) const; - double calculate_theta_d(const double& z_e) const; - private: - LOS::Params params_; - Eigen::Matrix3d rotation_y_{Eigen::Matrix3d::Zero()}; - Eigen::Matrix3d rotation_z_{Eigen::Matrix3d::Zero()}; - double pi_h_{}; - double pi_v_{}; - double beta_c_hat_{}; - double alpha_c_hat_{}; - }; -} // namespace vortex::guidance -#endif // LOS_GUIDANCE_HPP \ No newline at end of file +} +#endif // LOS_GUIDANCE_HPP \ No newline at end of file diff --git a/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp b/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp new file mode 100755 index 000000000..29a750e98 --- /dev/null +++ b/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp @@ -0,0 +1,32 @@ +#ifndef INTEGRAL_LOS_GUIDANCE_HPP +#define INTEGRAL_LOS_GUIDANCE_HPP + +#include +#include +#include "los_guidance/lib/types.hpp" +#include + +namespace vortex::guidance::los { + + struct IntergalLosParams { + double lookahead_distance_h{}; + double lookahead_distance_v{}; + double gamma_h{}; + double gamma_v{}; + double time_step{}; + }; + + class IntergalLOSGuidance { + public: + IntergalLOSGuidance(const IntergalLosParams& params); + ~IntergalLOSGuidance() = default; + + private: + void update_angles(const types::Inputs& inputs); + types::CrossTrackError calculate_crosstrack_error(const tyoes::Inputs& inputs); + + IntergalLosParams m_params{}; + }; +} + +#endif // INTEGRAL_LOS_GUIDANCE_HPP \ No newline at end of file diff --git a/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp b/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp new file mode 100755 index 000000000..730646758 --- /dev/null +++ b/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp @@ -0,0 +1,38 @@ +#ifndef PROPORTIONAL_LOS_GUIDANCE_HPP +#define PROPORTIONAL_LOS_GUIDANCE_HPP + +#include +#include +#include "los_guidance/lib/types.hpp" +#include + +namespace vortex::guidance::los { + struct ProportionalLosParams { + double lookahead_distance_h{}; + double lookahead_distance_v{}; + double k_p_h{}; + double k_p_v{}; + }; + + class ProportionalLOSGuidance { + public: + ProportionalLOSGuidance(const ProportionalLosParams& params); + ~ProportionalLOSGuidance() = default; + + tyes::Output calculate_outputs(const types::Inputs& inputs); + + private: + void update_angles(const types::Inputs& inputs); + types::CrossTrackError calculate_crosstrack_error(const types::Inputs& inputs) const; + + ProportionalLosParams m_params{}; + //again i dont know if i should have them here or just in the functions + double pi_h_{0.0}; + double pi_v_{0.0}; + Eigen::AngleAxisd rotation_y_{0.0, Eigen::Vector3d::UnitY()}; + Eigen::AngleAxisd rotation_z_{0.0, Eigen::Vector3d::UnitZ()}; + }; + +} + +#endif // PROPORTIONAL_LOS_GUIDANCE_HPP diff --git a/guidance/los_guidance/include/los_guidance/lib/types.hpp b/guidance/los_guidance/include/los_guidance/lib/types.hpp index 6c7643a6c..c0b53fcf0 100755 --- a/guidance/los_guidance/include/los_guidance/lib/types.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/types.hpp @@ -1,55 +1,49 @@ -#ifndef types_hpp -#define types_hpp +#ifndef TYPES_HPP +#define TYPES_HPP #include #include #include -namespace vortex::guidance::lod::types{ - - namespace LOS { - - struct Point { - double x{}; - double y{}; - double z{}; - - Point operator-(const Point& other) const { - return Point{x - other.x, y - other.y, z - other.z}; - } - - Eigen::Vector3d as_vector() const { return Eigen::Vector3d(x, y, z); } - }; - - struct CrossTrackError { - double x_e{}; - double y_e{}; - double z_e{}; - - inline static CrossTrackError from_vector(const Eigen::Vector3d& vector) { - return CrossTrackError{vector.x(), vector.y(), vector.z()}; - } - }; - - struct Output { - double psi_d{}; - double theta_d{}; - }; - - struct Inputs{ - Point prev_point{}; - Point next_point{}; - Point current_position{}; - }; - - enum class Active_LOSMethod { - PROPORTIONAL, - INTEGRAL, - ADAPTIVE - }; - - - } // namespace LOS +namespace vortex::guidance::los::types{ + struct Point { + double x{}; + double y{}; + double z{}; + + Point operator-(const Point& other) const { + return Point{x - other.x, y - other.y, z - other.z}; + } + + Eigen::Vector3d as_vector() const { return Eigen::Vector3d(x, y, z); } + }; + + struct CrossTrackError { + double x_e{}; + double y_e{}; + double z_e{}; + + inline static CrossTrackError from_vector(const Eigen::Vector3d& vector) { + return CrossTrackError{vector.x(), vector.y(), vector.z()}; + } + }; + + struct Output { + double psi_d{}; + double theta_d{}; + }; + + struct Inputs{ + Point prev_point{}; + Point next_point{}; + Point current_position{}; + }; + + enum class Active_LOSMethod { + PROPORTIONAL, + INTEGRAL, + ADAPTIVE + }; } // namespace vortex::guidance::los::types diff --git a/guidance/los_guidance/src/lib/adaptive_los.cpp b/guidance/los_guidance/src/lib/adaptive_los.cpp index 7e6b6a650..86c9763f6 100644 --- a/guidance/los_guidance/src/lib/adaptive_los.cpp +++ b/guidance/los_guidance/src/lib/adaptive_los.cpp @@ -1,98 +1,56 @@ #include "los_guidance/lib/adaptive_los.hpp" -namespace vortex::guidance { +namespace vortex::guidance::los { + + AdaptiveLOSGuidance::AdaptiveLOSGuidance(const AdaptiveLosParams& params): m_params{params} {} -AdaptiveLOSGuidance::AdaptiveLOSGuidance(const LOS::Params& params) - : params_(params) {} + void AdaptiveLOSGuidance::update_angles(const types::Inputs& inputs) { + const double dx = inputs.next_point.x - inputs.prev_point.x; + const double dy = inputs.next_point.y - inputs.prev_point.y; + const double dz = inputs.next_point.z - inputs.prev_point.z; -void AdaptiveLOSGuidance::update_angles(const LOS::Point& prev_point, - const LOS::Point& next_point) { - const LOS::Point difference = next_point - prev_point; + pi_h_ = std::atan2(dy, dx); + pi_v_ = std::atan2(-dz, std::sqrt(dx*dx + dy*dy)); - pi_h_ = atan2(difference.y, difference.x); - pi_v_ = atan2(-difference.z, sqrt(difference.x * difference.x + - difference.y * difference.y)); + rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); + rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); + } - rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); - rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); -} + types::CrossTrackError AdaptiveLOSGuidance::calculate_crosstrack_error(const types::Inputs& inputs) const { -LOS::CrossTrackError AdaptiveLOSGuidance::calculate_crosstrack_error( - const LOS::Point& prev_point, - const LOS::Point& current_position) const { - const LOS::Point difference = current_position - prev_point; - const Eigen::Vector3d difference_vector = difference.as_vector(); + const types::Point difference = inputs.current_position - inputs.prev_point; + const Eigen::Vector3d difference_vector = difference.as_vector(); - const Eigen::Vector3d cross_track_error = - rotation_y_.transpose() * rotation_z_.transpose() * difference_vector; + const Eigen::Vector3d cross_track_error = + rotation_y_.transpose() * rotation_z_.transpose() * difference_vector; - return LOS::CrossTrackError::from_vector(cross_track_error); -} + return types::CrossTrackError::from_vector(cross_track_error); + } -double AdaptiveLOSGuidance::calculate_psi_d(const double& y_e) const { - return pi_h_ - beta_c_hat_ - atan(y_e / params_.lookahead_distance_h); -} + void AdaptiveLOSGuidance::update_adaptive_estimates(const types::CrossTrackError& e) { + const double denom_h = std::sqrt( + m_params.lookahead_distance_h * m_params.lookahead_distance_h + e.y_e * e.y_e); + const double denom_v = std::sqrt( + m_params.lookahead_distance_v * m_params.lookahead_distance_v + e.z_e * e.z_e); -double AdaptiveLOSGuidance::calculate_theta_d(const double& z_e) const { - return pi_v_ + alpha_c_hat_ + atan(z_e / params_.lookahead_distance_v); -} + const double beta_dot = m_params.gamma_h * (m_params.lookahead_distance_h / denom_h) * e.y_e; + const double alpha_dot = m_params.gamma_v * (m_params.lookahead_distance_v / denom_v) * e.z_e; -void AdaptiveLOSGuidance::update_adaptive_estimates( - const LOS::CrossTrackError& crosstrack_error) { - double beta_c_hat_dot = - params_.gamma_h * - (params_.lookahead_distance_h / - sqrt(params_.lookahead_distance_h * params_.lookahead_distance_h + - crosstrack_error.y_e * crosstrack_error.y_e)) * - crosstrack_error.y_e; - double alpha_c_hat_dot = - params_.gamma_v * - (params_.lookahead_distance_v / - sqrt(params_.lookahead_distance_v * params_.lookahead_distance_v + - crosstrack_error.z_e * crosstrack_error.z_e)) * - crosstrack_error.z_e; + beta_c_hat_ += beta_dot * m_params.time_step; + alpha_c_hat_ += alpha_dot * m_params.time_step; + } + + types::Output AdaptiveLOSGuidance::calculate_outputs(const types::Inputs& inputs) { - beta_c_hat_ += beta_c_hat_dot * params_.time_step; - alpha_c_hat_ += alpha_c_hat_dot * params_.time_step; -} + update_angles(inputs) + const types::CrossTrackError e = calculate_crosstrack_error(inputs); + update_adaptive_estimates(e); -// ---------------- Proportional LOS Guidance Implementation ---------------- - -ProportionalLOSGuidance::ProportionalLOSGuidance(const LOS::Params& params) - : params_(params) {} - -void ProportionalLOSGuidance::update_angles(const LOS::Point& prev_point, - const LOS::Point& next_point) { - const LOS::Point difference = next_point - prev_point; - - pi_h_ = std::atan2(difference.y, difference.x); - pi_v_ = std::atan2(-difference.z, - std::sqrt(difference.x * difference.x + - difference.y * difference.y)); - - rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); - rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); -} - -LOS::CrossTrackError ProportionalLOSGuidance::calculate_crosstrack_error( - const LOS::Point& prev_point, - const LOS::Point& current_position) const { - const Eigen::Vector3d diff_vec = (current_position - prev_point).as_vector(); - - const Eigen::Vector3d e_perp = - rotation_y_.transpose() * rotation_z_.transpose() * diff_vec; - - return LOS::CrossTrackError::from_vector(e_perp); -} - -double ProportionalLOSGuidance::calculate_psi_d(const double& y_e) const { - const double k_p_h = 1.0 / std::max(params_.lookahead_distance_h, 1e-9); - return pi_h_ - std::atan(k_p_h * y_e); -} - -double ProportionalLOSGuidance::calculate_theta_d(const double& z_e) const { - const double k_p_v = 1.0 / std::max(params_.lookahead_distance_v, 1e-9); - return pi_v_ + std::atan(k_p_v * z_e); -} + const double psi_d = pi_h_ - beta_c_hat_ - std::atan(e.y_e / params_.lookahead_distance_h); + const double theta_d = pi_v_ + alpha_c_hat_ + std::atan(e.z_e / params_.lookahead_distance_v); + + return types::Output{psi_d, theta_d}; + } + } // namespace vortex::guidance diff --git a/guidance/los_guidance/src/lib/integral_los.cpp b/guidance/los_guidance/src/lib/integral_los.cpp new file mode 100755 index 000000000..847f0d408 --- /dev/null +++ b/guidance/los_guidance/src/lib/integral_los.cpp @@ -0,0 +1 @@ + #include "los_guidance/lib/integral_los.hpp" \ No newline at end of file diff --git a/guidance/los_guidance/src/lib/proportional_los.cpp b/guidance/los_guidance/src/lib/proportional_los.cpp new file mode 100755 index 000000000..e94cf57e0 --- /dev/null +++ b/guidance/los_guidance/src/lib/proportional_los.cpp @@ -0,0 +1,38 @@ + #include "los_guidance/lib/proportional_los.hpp" + +namespace vortex::guidance::los { + + ProportionalLOSGuidance::ProportionalLOSGuidance(const ProportionalLosParams& params) : m_params{params} {} + + void ProportionalLOSGuidance::update_angles(const types::Inputs& inputs) { + const types::Point difference = inputs.next_point - inputs.prev_point; + + pi_h_ = std::atan2(difference.y, difference.x); + pi_v_ = std::atan2(-difference.z, std::sqrt(difference.x * difference.x + difference.y * difference.y)); + + rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); + rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); + } + + types::CrossTrackError ProportionalLOSGuidance::calculate_crosstrack_error(const types::Inputs& inputs) const { + + const Eigen::Vector3d diff_vec = (inputs.current_position - inputs.prev_point).as_vector(); + const Eigen::Vector3d e_perp = rotation_y_.transpose() * rotation_z_.transpose() * diff_vec; + + return types::CrossTrackError::from_vector(e_perp); + } + + types::Output ProportionalLOSGuidance::calculate_outputs(const types::Inputs& inputs) { + update_angles(inputs); + const types::CrossTrackError e = calculate_crosstrack_error(inputs); + + const double k_p_h = 1.0 / std::max(params_.lookahead_distance_h, 1e-9); + const double k_p_v = 1.0 / std::max(params_.lookahead_distance_v, 1e-9); + + const double psi_d = pi_h_ - std::atan(k_p_h * e.y_e); + const double theta_d = pi_v_ + std::atan(k_p_v * e.z_e); + + return types::Output{psi_d, theta_d}; + } + +} // namespace vortex::guidance::los \ No newline at end of file diff --git a/guidance/los_guidance/test/CMakeLists.txt b/guidance/los_guidance/test/CMakeLists.txt index 69c0dfa52..f2b81bdc2 100644 --- a/guidance/los_guidance/test/CMakeLists.txt +++ b/guidance/los_guidance/test/CMakeLists.txt @@ -6,7 +6,10 @@ include(GoogleTest) set(TEST_BINARY_NAME ${PROJECT_NAME}_test) add_executable( ${TEST_BINARY_NAME} - test_los_guidance.cpp + adaptive_los_test.cpp + proportional_los_test.cpp + integral_los_test.cpp + ) target_link_libraries( diff --git a/guidance/los_guidance/test/adaptive_los_test.cpp b/guidance/los_guidance/test/adaptive_los_test.cpp new file mode 100644 index 000000000..a0195a79a --- /dev/null +++ b/guidance/los_guidance/test/adaptive_los_test.cpp @@ -0,0 +1,179 @@ +#include +#include "los_guidance/lib/adaptive_los.hpp" + +namespace vortex::guidance::los{ //new namespace added los + + class AdaptiveLosTest : public ::testing::Test { + protected: + AdaptiveLosTest() : los_{get_params()} {} + + AdaptiveLosParams get_params() { + AdaptiveLosParams p; + p.lookahead_distance_h = 1.0; + p.lookahead_distance_v = 1.0; + p.gamma_h = 1.0; + p.gamma_v = 1.0; + p.time_step = 0.01; + return p; + } + + AdaptiveLOSGuidance los_; + const double tol = 1e-9; + }; + /* + TEST_F(AdaptiveLosTest, T01_test_cross_track_error_on_track){ + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, 0.0}; + + const types::Output O = los_.calculate_outputs(inputs); + + EXPECT_NEAR(e.x_e, 0.0, tol); + EXPECT_NEAR(e.y_e, 0.0, tol); + EXPECT_NEAR(e.z_e, 0.0, tol); + } + + TEST_F(AdaptiveLosTests, T02_test_cross_track_error_right_off_track) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.5, 0.0}; + + const types::Output O = los_.calculate_outputs(inputs); + + EXPECT_NEAR(e.x_e, 0.0, tol); + EXPECT_NEAR(e.y_e, 0.5, tol); + EXPECT_NEAR(e.z_e, 0.0, tol); + } + + TEST_F(AdaptiveLosTests, T03_test_cross_track_error_left_off_track) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, -0.5, 0.0}; + + const types::Output O = los_.calculate_outputs(inputs); + + EXPECT_NEAR(e.x_e, 0.0, tol); + EXPECT_NEAR(e.y_e, -0.5, tol); + EXPECT_NEAR(e.z_e, 0.0, tol); + } + + TEST_F(AdaptiveLosTests, T04_test_cross_track_error_under_track) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, 0.5}; + + const types::Output O = los_.calculate_outputs(inputs); + + EXPECT_NEAR(e.x_e, 0.0, tol); + EXPECT_NEAR(e.y_e, 0.0, tol); + EXPECT_NEAR(e.z_e, 0.5, tol); + } + + TEST_F(AdaptiveLosTests, T05_test_cross_track_error_over_track) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, -0.5}; + + const types::Output O = los_.calculate_outputs(inputs); + + EXPECT_NEAR(e.x_e, 0.0, tol); + EXPECT_NEAR(e.y_e, 0.0, tol); + EXPECT_NEAR(e.z_e, -0.5, tol); + } + */ + + // Test commanded angles when drone is to the right of the track + TEST_F(AdaptiveLosTests, T06_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.5, 0.0}; + + const types::Output O = los_.calculate_outputs(inputs); + + // Heading cmd should be between -pi/2 and 0 + EXPECT_LT(O.psi_d 0.0); + EXPECT_GT(O.psi_d, -1.57); + + // Pitch cmd should be zero + EXPECT_NEAR(O.theta_d, 0.0, tol); + } + + // Test commanded angles when drone is to the left of the track + TEST_F(AdaptiveLosTests, T07_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, -0.5, 0.0}; + + const types::Output O = los_.calculate_outputs(inputs); + + // Heading cmd should be between 0 and pi/2 + EXPECT_GT(O.psi_d, 0.0); + EXPECT_LT(O.psi_d, 1.57); + // Pitch cmd should be zero + EXPECT_NEAR(O.theta_d, 0.0, tol); + } + + // Test commanded angles when drone is under the track + TEST_F(AdaptiveLosTests, T08_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, 0.5}; + + const types::Output O = los_.calculate_outputs(inputs); + + // Heading cmd should be 0 + EXPECT_NEAR(O.psi_d, 0.0, tol); + // Pitch cmd should be between 0 and pi/2 + EXPECT_GT(O.theta_d, 0.0); + EXPECT_LT(O.theta_d, 1.57); + } + + // Test commanded angles when drone is over the track + TEST_F(AdaptiveLosTests, T09_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, -0.5}; + + const types::Output O = los_.calculate_outputs(inputs); + + // Heading cmd should be 0 + EXPECT_NEAR(O.psi_d, 0.0, tol); + // Pitch cmd should be between -pi/2 and 0 + EXPECT_LT(O.theta_d, 0.0); + EXPECT_GT(O.theta_d, -1.57); + } + + // Test commanded angles when drone is over and to the right of the track + + TEST_F(AdaptiveLosTests, T10_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.5, -0.5}; + + const types::Output O = los_.calculate_outputs(inputs); + + // Heading cmd should be between -pi/2 and 0 + EXPECT_LT(O.psi_d, 0.0); + EXPECT_GT(O.psi_d, -1.57); + // Pitch cmd should be between -pi/2 and 0 + EXPECT_LT(O.theta_d, 0.0); + EXPECT_GT(O.theta_d, -1.57); + } + +} // namespace vortex::guidance + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} diff --git a/guidance/los_guidance/test/integral_los_test.cpp b/guidance/los_guidance/test/integral_los_test.cpp new file mode 100755 index 000000000..e69de29bb diff --git a/guidance/los_guidance/test/proportional_los_test.cpp b/guidance/los_guidance/test/proportional_los_test.cpp new file mode 100755 index 000000000..de040614c --- /dev/null +++ b/guidance/los_guidance/test/proportional_los_test.cpp @@ -0,0 +1,113 @@ +#include +#include "los_guidance/lib/adaptive_los.hpp" + +namespace vortex::guidance::los{ + + class ProportionalLosTest : public ::testing::Test { + protected: + ProportionalLosTest() : Plos_{get_params()} {} + + ProportionalLosTest get_params() { + ProportionalLosParams params; + params.lookahead_distance_h = 10.0; + params.lookahead_distance_v = 10.0; + params.k_p_h = 0.667; // needs tuning + params.k_p_v = 0.582; // needs tuning + params.time_step = 0.01; + return params; + } + + ProportionalLOSGuidance Plos_; + const double tol = 1e-9; + }; + + // Test commanded angles when drone is to the right of the track + TEST_F(ProportionalLosTest, T06_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.5, 0.0}; + + const types::Output O = Plos_.calculate_outputs(inputs); + + // Heading cmd should be between -pi/2 and 0 + EXPECT_LT(O.psi_d 0.0); + EXPECT_GT(O.psi_d, -1.57); + + // Pitch cmd should be zero + EXPECT_NEAR(O.theta_d, 0.0, tol); + } + + // Test commanded angles when drone is to the left of the track + TEST_F(ProportionalLosTest, T07_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, -0.5, 0.0}; + + const types::Output O = Plos_.calculate_outputs(inputs); + + // Heading cmd should be between 0 and pi/2 + EXPECT_GT(O.psi_d, 0.0); + EXPECT_LT(O.psi_d, 1.57); + // Pitch cmd should be zero + EXPECT_NEAR(O.theta_d, 0.0, tol); + } + + // Test commanded angles when drone is under the track + TEST_F(ProportionalLosTest, T08_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, 0.5}; + + const types::Output O = Plos_.calculate_outputs(inputs); + + // Heading cmd should be 0 + EXPECT_NEAR(O.psi_d, 0.0, tol); + // Pitch cmd should be between 0 and pi/2 + EXPECT_GT(O.theta_d, 0.0); + EXPECT_LT(O.theta_d, 1.57); + } + + // Test commanded angles when drone is over the track + TEST_F(ProportionalLosTest, T09_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, -0.5}; + + const types::Output O = Plos_.calculate_outputs(inputs); + + // Heading cmd should be 0 + EXPECT_NEAR(O.psi_d, 0.0, tol); + // Pitch cmd should be between -pi/2 and 0 + EXPECT_LT(O.theta_d, 0.0); + EXPECT_GT(O.theta_d, -1.57); + } + + // Test commanded angles when drone is over and to the right of the track + + TEST_F(ProportionalLosTest, T10_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.5, -0.5}; + + const types::Output O = Plos_.calculate_outputs(inputs); + + // Heading cmd should be between -pi/2 and 0 + EXPECT_LT(O.psi_d, 0.0); + EXPECT_GT(O.psi_d, -1.57); + // Pitch cmd should be between -pi/2 and 0 + EXPECT_LT(O.theta_d, 0.0); + EXPECT_GT(O.theta_d, -1.57); + } + +} // namespace vortex::guidance + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} diff --git a/guidance/los_guidance/test/test_los_guidance.cpp b/guidance/los_guidance/test/test_los_guidance.cpp deleted file mode 100644 index 84ebe7bff..000000000 --- a/guidance/los_guidance/test/test_los_guidance.cpp +++ /dev/null @@ -1,182 +0,0 @@ -#include - -#include "los_guidance/lib/adaptive_los.hpp" - -namespace vortex::guidance { - -class LOSTests : public ::testing::Test { - protected: - LOSTests() : los_guidance_{get_los_params()} {} - - LOS::Params get_los_params() { - LOS::Params params; - params.lookahead_distance_h = 1.0; - params.lookahead_distance_v = 1.0; - params.gamma_h = 1.0; - params.gamma_v = 1.0; - params.time_step = 0.01; - return params; - } - - AdaptiveLOSGuidance los_guidance_; - const double tol = 1e-9; -}; - -TEST_F(LOSTests, T01_test_cross_track_error_on_track) { - LOS::Point prev{0.0, 0.0, 0.0}; - LOS::Point next{1.0, 0.0, 0.0}; - los_guidance_.update_angles(prev, next); - LOS::Point current{0.0, 0.0, 0.0}; - LOS::CrossTrackError e = - los_guidance_.calculate_crosstrack_error(prev, current); - - EXPECT_NEAR(e.x_e, 0.0, tol); - EXPECT_NEAR(e.y_e, 0.0, tol); - EXPECT_NEAR(e.z_e, 0.0, tol); -} - -TEST_F(LOSTests, T02_test_cross_track_error_right_off_track) { - LOS::Point prev{0.0, 0.0, 0.0}; - LOS::Point next{1.0, 0.0, 0.0}; - los_guidance_.update_angles(prev, next); - LOS::Point current{0.0, 0.5, 0.0}; - LOS::CrossTrackError e = - los_guidance_.calculate_crosstrack_error(prev, current); - - EXPECT_NEAR(e.x_e, 0.0, tol); - EXPECT_NEAR(e.y_e, 0.5, tol); - EXPECT_NEAR(e.z_e, 0.0, tol); -} - -TEST_F(LOSTests, T03_test_cross_track_error_left_off_track) { - LOS::Point prev{0.0, 0.0, 0.0}; - LOS::Point next{1.0, 0.0, 0.0}; - los_guidance_.update_angles(prev, next); - LOS::Point current{0.0, -0.5, 0.0}; - LOS::CrossTrackError e = - los_guidance_.calculate_crosstrack_error(prev, current); - - EXPECT_NEAR(e.x_e, 0.0, tol); - EXPECT_NEAR(e.y_e, -0.5, tol); - EXPECT_NEAR(e.z_e, 0.0, tol); -} - -TEST_F(LOSTests, T04_test_cross_track_error_under_track) { - LOS::Point prev{0.0, 0.0, 0.0}; - LOS::Point next{1.0, 0.0, 0.0}; - los_guidance_.update_angles(prev, next); - LOS::Point current{0.0, 0.0, 0.5}; - LOS::CrossTrackError e = - los_guidance_.calculate_crosstrack_error(prev, current); - - EXPECT_NEAR(e.x_e, 0.0, tol); - EXPECT_NEAR(e.y_e, 0.0, tol); - EXPECT_NEAR(e.z_e, 0.5, tol); -} - -TEST_F(LOSTests, T05_test_cross_track_error_over_track) { - LOS::Point prev{0.0, 0.0, 0.0}; - LOS::Point next{1.0, 0.0, 0.0}; - los_guidance_.update_angles(prev, next); - LOS::Point current{0.0, 0.0, -0.5}; - LOS::CrossTrackError e = - los_guidance_.calculate_crosstrack_error(prev, current); - - EXPECT_NEAR(e.x_e, 0.0, tol); - EXPECT_NEAR(e.y_e, 0.0, tol); - EXPECT_NEAR(e.z_e, -0.5, tol); -} - -// Test commanded angles when drone is to the right of the track -TEST_F(LOSTests, T06_test_commanded_angles) { - LOS::Point prev{0.0, 0.0, 0.0}; - LOS::Point next{1.0, 0.0, 0.0}; - los_guidance_.update_angles(prev, next); - LOS::Point current{0.0, 0.5, 0.0}; - LOS::CrossTrackError e = - los_guidance_.calculate_crosstrack_error(prev, current); - double psi_d{los_guidance_.calculate_psi_d(e.y_e)}; - double theta_d{los_guidance_.calculate_theta_d(e.z_e)}; - // Heading cmd should be between -pi/2 and 0 - EXPECT_LT(psi_d, 0.0); - EXPECT_GT(psi_d, -1.57); - // Pitch cmd should be zero - EXPECT_NEAR(theta_d, 0.0, tol); -} - -// Test commanded angles when drone is to the left of the track -TEST_F(LOSTests, T07_test_commanded_angles) { - LOS::Point prev{0.0, 0.0, 0.0}; - LOS::Point next{1.0, 0.0, 0.0}; - los_guidance_.update_angles(prev, next); - LOS::Point current{0.0, -0.5, 0.0}; - LOS::CrossTrackError e = - los_guidance_.calculate_crosstrack_error(prev, current); - double psi_d{los_guidance_.calculate_psi_d(e.y_e)}; - double theta_d{los_guidance_.calculate_theta_d(e.z_e)}; - // Heading cmd should be between 0 and pi/2 - EXPECT_GT(psi_d, 0.0); - EXPECT_LT(psi_d, 1.57); - // Pitch cmd should be zero - EXPECT_NEAR(theta_d, 0.0, tol); -} - -// Test commanded angles when drone is under the track -TEST_F(LOSTests, T08_test_commanded_angles) { - LOS::Point prev{0.0, 0.0, 0.0}; - LOS::Point next{1.0, 0.0, 0.0}; - los_guidance_.update_angles(prev, next); - LOS::Point current{0.0, 0.0, 0.5}; - LOS::CrossTrackError e = - los_guidance_.calculate_crosstrack_error(prev, current); - double psi_d{los_guidance_.calculate_psi_d(e.y_e)}; - double theta_d{los_guidance_.calculate_theta_d(e.z_e)}; - // Heading cmd should be 0 - EXPECT_NEAR(psi_d, 0.0, tol); - // Pitch cmd should be between 0 and pi/2 - EXPECT_GT(theta_d, 0.0); - EXPECT_LT(theta_d, 1.57); -} - -// Test commanded angles when drone is over the track -TEST_F(LOSTests, T09_test_commanded_angles) { - LOS::Point prev{0.0, 0.0, 0.0}; - LOS::Point next{1.0, 0.0, 0.0}; - los_guidance_.update_angles(prev, next); - LOS::Point current{0.0, 0.0, -0.5}; - LOS::CrossTrackError e = - los_guidance_.calculate_crosstrack_error(prev, current); - double psi_d{los_guidance_.calculate_psi_d(e.y_e)}; - double theta_d{los_guidance_.calculate_theta_d(e.z_e)}; - // Heading cmd should be 0 - EXPECT_NEAR(psi_d, 0.0, tol); - // Pitch cmd should be between -pi/2 and 0 - EXPECT_LT(theta_d, 0.0); - EXPECT_GT(theta_d, -1.57); -} - -// Test commanded angles when drone is over and to the right of the track -TEST_F(LOSTests, T10_test_commanded_angles) { - LOS::Point prev{0.0, 0.0, 0.0}; - LOS::Point next{1.0, 0.0, 0.0}; - los_guidance_.update_angles(prev, next); - LOS::Point current{0.0, 0.5, -0.5}; - LOS::CrossTrackError e = - los_guidance_.calculate_crosstrack_error(prev, current); - double psi_d{los_guidance_.calculate_psi_d(e.y_e)}; - double theta_d{los_guidance_.calculate_theta_d(e.z_e)}; - // Heading cmd should be between -pi/2 and 0 - EXPECT_LT(psi_d, 0.0); - EXPECT_GT(psi_d, -1.57); - // Pitch cmd should be between -pi/2 and 0 - EXPECT_LT(theta_d, 0.0); - EXPECT_GT(theta_d, -1.57); -} - -} // namespace vortex::guidance - -int main(int argc, char** argv) { - testing::InitGoogleTest(&argc, argv); - - return RUN_ALL_TESTS(); -} From ee303e7e5f7b7ea86c01ca2a3a3a3b462afde39f Mon Sep 17 00:00:00 2001 From: Anbit Date: Sun, 9 Nov 2025 17:55:05 +0100 Subject: [PATCH 026/128] complete the restructuring of ros node --- guidance/los_guidance/CMakeLists.txt | 12 +- .../los_guidance/config/guidance_params.yaml | 35 +- .../include/los_guidance/lib/adaptive_los.hpp | 4 +- .../include/los_guidance/lib/integral_los.hpp | 32 +- .../los_guidance/lib/proportional_los.hpp | 5 +- .../include/los_guidance/lib/types.hpp | 14 +- .../include/los_guidance/los_guidance_ros.hpp | 168 +++---- .../launch/los_guidance.launch.py | 5 +- .../los_guidance/src/lib/adaptive_los.cpp | 17 +- .../los_guidance/src/lib/integral_los.cpp | 42 +- .../los_guidance/src/lib/proportional_los.cpp | 14 +- .../los_guidance/src/los_guidance_node.cpp | 2 +- .../los_guidance/src/los_guidance_ros.cpp | 434 ++++++++++-------- guidance/los_guidance/test/CMakeLists.txt | 10 +- .../los_guidance/test/adaptive_los_test.cpp | 30 +- .../los_guidance/test/integral_los_test.cpp | 110 +++++ .../test/proportional_los_test.cpp | 23 +- guidance/los_guidance/test/test_main.cpp | 8 + 18 files changed, 609 insertions(+), 356 deletions(-) create mode 100644 guidance/los_guidance/test/test_main.cpp diff --git a/guidance/los_guidance/CMakeLists.txt b/guidance/los_guidance/CMakeLists.txt index 34996951e..b9f48e915 100644 --- a/guidance/los_guidance/CMakeLists.txt +++ b/guidance/los_guidance/CMakeLists.txt @@ -18,12 +18,14 @@ find_package(geometry_msgs REQUIRED) find_package(Eigen3 REQUIRED) find_package(spdlog REQUIRED) find_package(fmt REQUIRED) +find_package(yaml-cpp REQUIRED) + include_directories(include) set(LIB_NAME los_guidance_lib) add_library(${LIB_NAME} SHARED - src/lib/proportional_los.cpp + src/lib/proportional_los.cpp src/lib/integral_los.cpp src/lib/adaptive_los.cpp src/los_guidance_ros.cpp @@ -44,7 +46,10 @@ add_executable(los_guidance_node src/los_guidance_node.cpp ) -target_link_libraries(los_guidance_node ${LIB_NAME}) +target_link_libraries(los_guidance_node + ${LIB_NAME} + yaml-cpp +) install(TARGETS ${LIB_NAME} @@ -61,7 +66,7 @@ install(TARGETS install( DIRECTORY include/ - DESTINATION include + DESTINATION include/ ) install(DIRECTORY @@ -71,6 +76,7 @@ install(DIRECTORY ) if(BUILD_TESTING) + include(CTest) add_subdirectory(test) endif() diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index ebee79903..c65637117 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -1,10 +1,25 @@ -/**: - ros__parameters: - los: - lookahead_distance_h: 1.5 - lookahead_distance_v: 0.6 - gamma_h: 0.05 - gamma_v: 0.1 - time_step: 0.01 - u_desired: 0.3 - goal_reached_tol: 1.0 \ No newline at end of file +adaptive_los: + lookahead_distance_h: 1.5 + lookahead_distance_v: 0.6 + gamma_h: 0.05 + gamma_v: 0.1 + +prop_los: + lookahead_distance_h: 1.5 + lookahead_distance_v: 0.6 + k_p_h: 0.5 + k_p_v: 0.5 + +integer_los: + lookahead_distance_h: 1.5 + lookahead_distance_v: 0.6 + k_p_h: 0.5 + k_p_v: 0.5 + K_i_h: 0.1 + K_i_v: 0.1 + +common: + los_method: "ADAPTIVE" + u_desired: 0.3 + goal_reached_tol: 1.0 + \ No newline at end of file diff --git a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp index ee655ccb8..9282fc8b2 100644 --- a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp @@ -5,7 +5,7 @@ #include #include "los_guidance/lib/types.hpp" #include - + /** * @brief Adaptive Line-of-Sight (LOS) guidance algorithm based on slide 113 * in "Fossen 2024 Lecture on 2D and 3D path-following control". @@ -26,7 +26,7 @@ namespace vortex::guidance::los { AdaptiveLOSGuidance(const AdaptiveLosParams& params); ~AdaptiveLOSGuidance() = default; - types::Output calculate_outputs(const types::Inputs& inputs); + types::Outputs calculate_outputs(const types::Inputs& inputs); private: void update_angles(const types::Inputs& inputs); diff --git a/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp b/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp index 29a750e98..3fbf122df 100755 --- a/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp @@ -4,28 +4,40 @@ #include #include #include "los_guidance/lib/types.hpp" -#include - +#include + namespace vortex::guidance::los { - struct IntergalLosParams { + struct IntegralLosParams { double lookahead_distance_h{}; double lookahead_distance_v{}; - double gamma_h{}; - double gamma_v{}; + double k_p_h{}; + double k_p_v{}; + double k_i_h{}; + double k_i_v{}; double time_step{}; }; - class IntergalLOSGuidance { + class IntegralLOSGuidance { public: - IntergalLOSGuidance(const IntergalLosParams& params); - ~IntergalLOSGuidance() = default; + IntegralLOSGuidance(const IntegralLosParams& params); + ~IntegralLOSGuidance() = default; + + types::Outputs calculate_outputs(const types::Inputs& inputs); private: void update_angles(const types::Inputs& inputs); - types::CrossTrackError calculate_crosstrack_error(const tyoes::Inputs& inputs); + types::CrossTrackError calculate_crosstrack_error(const types::Inputs& inputs); + + IntegralLosParams m_params{}; - IntergalLosParams m_params{}; + double int_h{}; + double int_v{}; + //again i dont know if i should have them here or just in the functions + double pi_h_{}; + double pi_v_{}; + Eigen::AngleAxisd rotation_y_{0.0, Eigen::Vector3d::UnitY()}; + Eigen::AngleAxisd rotation_z_{0.0, Eigen::Vector3d::UnitZ()}; }; } diff --git a/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp b/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp index 730646758..fad30eecc 100755 --- a/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp @@ -5,13 +5,14 @@ #include #include "los_guidance/lib/types.hpp" #include - + namespace vortex::guidance::los { struct ProportionalLosParams { double lookahead_distance_h{}; double lookahead_distance_v{}; double k_p_h{}; double k_p_v{}; + double time_step{}; }; class ProportionalLOSGuidance { @@ -19,7 +20,7 @@ namespace vortex::guidance::los { ProportionalLOSGuidance(const ProportionalLosParams& params); ~ProportionalLOSGuidance() = default; - tyes::Output calculate_outputs(const types::Inputs& inputs); + types::Outputs calculate_outputs(const types::Inputs& inputs); private: void update_angles(const types::Inputs& inputs); diff --git a/guidance/los_guidance/include/los_guidance/lib/types.hpp b/guidance/los_guidance/include/los_guidance/lib/types.hpp index c0b53fcf0..fd2340bf2 100755 --- a/guidance/los_guidance/include/los_guidance/lib/types.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/types.hpp @@ -3,9 +3,12 @@ #include #include +#include +#include #include namespace vortex::guidance::los::types{ + struct Point { double x{}; double y{}; @@ -16,6 +19,13 @@ namespace vortex::guidance::los::types{ } Eigen::Vector3d as_vector() const { return Eigen::Vector3d(x, y, z); } + + static Point point_from_ros( + const geometry_msgs::msg::Point& msg) { + return Point{msg.x, msg.y, msg.z}; + } + + }; struct CrossTrackError { @@ -28,7 +38,7 @@ namespace vortex::guidance::los::types{ } }; - struct Output { + struct Outputs { double psi_d{}; double theta_d{}; }; @@ -39,7 +49,7 @@ namespace vortex::guidance::los::types{ Point current_position{}; }; - enum class Active_LOSMethod { + enum class ActiveLosMethod { PROPORTIONAL, INTEGRAL, ADAPTIVE diff --git a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp index c12838ba4..4c9ca3f7b 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -5,113 +5,123 @@ #include #include #include "los_guidance/lib/adaptive_los.hpp" +#include "los_guidance/lib/integral_los.hpp" +#include "los_guidance/lib/proportional_los.hpp" +#include "los_guidance/lib/types.hpp" #include #include #include +#include #include #include +#include -namespace vortex::guidance { - -class LOSGuidanceNode : public rclcpp::Node { - public: - explicit LOSGuidanceNode(); - - private: - // @brief Set the subscribers and publishers - void set_subscribers_and_publisher(); - - // @brief Set the action server - void set_action_server(); - - // @brief Set the adaptive LOS guidance parameters - void set_adaptive_los_guidance(); - - // @brief Callback for the waypoint topic - // @param msg The reference message - void waypoint_callback( - const geometry_msgs::msg::PointStamped::SharedPtr msg); - - // @brief Callback for the pose topic - // @param msg The pose message - void pose_callback( - const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); - - // @brief Handle the goal request - // @param uuid The goal UUID - // @param goal The goal message - // @return The goal response - rclcpp_action::GoalResponse handle_goal( - const rclcpp_action::GoalUUID& uuid, - std::shared_ptr goal); - - // @brief Handle the cancel request - // @param goal_handle The goal handle - // @return The cancel response - rclcpp_action::CancelResponse handle_cancel( - const std::shared_ptr< - rclcpp_action::ServerGoalHandle> - goal_handle); +namespace vortex::guidance::los { + + class LosGuidanceNode : public rclcpp::Node { + public: + explicit LosGuidanceNode(); + + private: + // @brief Set the subscribers and publishers + void set_subscribers_and_publisher(); + + // @brief Set the action server + void set_action_server(); + + // @brief Determine the LOS mode service + void set_los_mode_service(); + + // @brief Set the adaptive LOS guidance parameters + void set_adaptive_los_guidance(YAML::Node config); - // @brief Handle the accepted request - // @param goal_handle The goal handle - void handle_accepted(const std::shared_ptr> goal_handle); + // @brief Set the proportional LOS guidance parameters + void set_proportional_los_guidance(YAML::Node config); - // @brief Execute the goal - // @param goal_handle The goal handle - void execute(const std::shared_ptr> goal_handle); + // @brief Set the integral LOS guidance parameters + void set_integral_los_guidance(YAML::Node config); - // @brief Fill the lost waypoints - // @param goal The goal message - void fill_los_waypoints( - const geometry_msgs::msg::PointStamped& los_waypoint); + // @brief Callback for the waypoint topic + // @param msg The reference message + void waypoint_callback( + const geometry_msgs::msg::PointStamped::SharedPtr msg); - vortex_msgs::msg::LOSGuidance fill_los_reference(); + // @brief Callback for the pose topic + // @param msg The pose message + void pose_callback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); - rclcpp_action::Server::SharedPtr - action_server_; + // @brief Handle the goal request + // @param uuid The goal UUID + // @param goal The goal message + // @return The goal response + rclcpp_action::GoalResponse handle_goal( + const rclcpp_action::GoalUUID& uuid, + std::shared_ptr goal); - rclcpp::Publisher::SharedPtr reference_pub_; + // @brief Handle the cancel request + // @param goal_handle The goal handle + // @return The cancel response + rclcpp_action::CancelResponse handle_cancel( + const std::shared_ptr< + rclcpp_action::ServerGoalHandle> + goal_handle); - rclcpp::Subscription::SharedPtr - waypoint_sub_; + // @brief Handle the accepted request + // @param goal_handle The goal handle + void handle_accepted(const std::shared_ptr> goal_handle); - rclcpp::Subscription< - geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_; + // @brief Execute the goal + // @param goal_handle The goal handle + void execute(const std::shared_ptr> goal_handle); - rclcpp::TimerBase::SharedPtr reference_pub_timer_; + vortex_msgs::msg::LOSGuidance fill_los_reference(types::Outputs output); - std::chrono::milliseconds time_step_; + YAML::Node get_los_config(std::string yaml_file_path); - std::mutex mutex_; + void parse_common_config(YAML::Node common_config); - rclcpp_action::GoalUUID preempted_goal_id_; + rclcpp_action::Server::SharedPtr + action_server_; - std::shared_ptr< - rclcpp_action::ServerGoalHandle> - goal_handle_; + rclcpp::Service::SharedPtr los_mode_service_; - rclcpp::CallbackGroup::SharedPtr cb_group_; + rclcpp::Publisher::SharedPtr reference_pub_; - LOS::Point eta_; + rclcpp::Subscription::SharedPtr + waypoint_sub_; - LOS::Point last_point_; + rclcpp::Subscription< + geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_; - LOS::Point next_point_; + rclcpp::TimerBase::SharedPtr reference_pub_timer_; + + std::chrono::milliseconds time_step_; + + std::mutex mutex_; + + rclcpp_action::GoalUUID preempted_goal_id_; + + std::shared_ptr< + rclcpp_action::ServerGoalHandle> + goal_handle_; - std::unique_ptr adaptive_los_guidance_; + rclcpp::CallbackGroup::SharedPtr cb_group_; - double yaw_d_{}; + types::Inputs path_inputs_{}; - double pitch_d_{}; + double u_desired_{}; - double u_desired_{}; + double goal_reached_tol_{}; - double goal_reached_tol_{}; -}; + std::unique_ptr m_adaptive_los{}; + std::unique_ptr m_integral_los{}; + std::unique_ptr m_proportional_los{}; + types::ActiveLosMethod m_method{}; + }; -} // namespace vortex::guidance +} // namespace vortex::guidance::los #endif // LOS_GUIDANCE_ROS_HPP diff --git a/guidance/los_guidance/launch/los_guidance.launch.py b/guidance/los_guidance/launch/los_guidance.launch.py index 03366e897..f726983f0 100644 --- a/guidance/los_guidance/launch/los_guidance.launch.py +++ b/guidance/los_guidance/launch/los_guidance.launch.py @@ -4,7 +4,7 @@ from launch import LaunchDescription from launch_ros.actions import Node -adapt_params = path.join( +los_params = path.join( get_package_share_directory("los_guidance"), "config", "guidance_params.yaml", @@ -25,7 +25,8 @@ def generate_launch_description(): namespace="orca", parameters=[ orca_params, - adapt_params, + {"los_config_file": los_params}, + {"time_step": 0.01}, ], output="screen", ) diff --git a/guidance/los_guidance/src/lib/adaptive_los.cpp b/guidance/los_guidance/src/lib/adaptive_los.cpp index 86c9763f6..67f282fd4 100644 --- a/guidance/los_guidance/src/lib/adaptive_los.cpp +++ b/guidance/los_guidance/src/lib/adaptive_los.cpp @@ -1,4 +1,5 @@ -#include "los_guidance/lib/adaptive_los.hpp" +#include "los_guidance/lib/types.hpp" +#include namespace vortex::guidance::los { @@ -7,7 +8,7 @@ namespace vortex::guidance::los { void AdaptiveLOSGuidance::update_angles(const types::Inputs& inputs) { const double dx = inputs.next_point.x - inputs.prev_point.x; const double dy = inputs.next_point.y - inputs.prev_point.y; - const double dz = inputs.next_point.z - inputs.prev_point.z; + const double dz = inputs.next_point.z - inputs.prev_point.z; pi_h_ = std::atan2(dy, dx); pi_v_ = std::atan2(-dz, std::sqrt(dx*dx + dy*dy)); @@ -16,7 +17,7 @@ namespace vortex::guidance::los { rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); } - types::CrossTrackError AdaptiveLOSGuidance::calculate_crosstrack_error(const types::Inputs& inputs) const { + const types::CrossTrackError AdaptiveLOSGuidance::calculate_crosstrack_error(const types::Inputs& inputs) { const types::Point difference = inputs.current_position - inputs.prev_point; const Eigen::Vector3d difference_vector = difference.as_vector(); @@ -40,16 +41,16 @@ namespace vortex::guidance::los { alpha_c_hat_ += alpha_dot * m_params.time_step; } - types::Output AdaptiveLOSGuidance::calculate_outputs(const types::Inputs& inputs) { + types::Outputs AdaptiveLOSGuidance::calculate_outputs(const types::Inputs& inputs) { - update_angles(inputs) + update_angles(inputs); const types::CrossTrackError e = calculate_crosstrack_error(inputs); update_adaptive_estimates(e); - const double psi_d = pi_h_ - beta_c_hat_ - std::atan(e.y_e / params_.lookahead_distance_h); - const double theta_d = pi_v_ + alpha_c_hat_ + std::atan(e.z_e / params_.lookahead_distance_v); + const double psi_d = pi_h_ - beta_c_hat_ - std::atan(e.y_e / m_params.lookahead_distance_h); + const double theta_d = pi_v_ + alpha_c_hat_ + std::atan(e.z_e / m_params.lookahead_distance_v); - return types::Output{psi_d, theta_d}; + return types::Outputs{psi_d, theta_d}; } diff --git a/guidance/los_guidance/src/lib/integral_los.cpp b/guidance/los_guidance/src/lib/integral_los.cpp index 847f0d408..2a088d6b0 100755 --- a/guidance/los_guidance/src/lib/integral_los.cpp +++ b/guidance/los_guidance/src/lib/integral_los.cpp @@ -1 +1,41 @@ - #include "los_guidance/lib/integral_los.hpp" \ No newline at end of file + #include + +namespace vortex::guidance::los { + + IntegralLOSGuidance::IntegralLOSGuidance(const IntegralLosParams& params): m_params{params} {} + + void IntegralLOSGuidance::update_angles(const types::Inputs& inputs) { + const types::Point difference = inputs.next_point - inputs.prev_point; + + pi_h_ = std::atan2(difference.y, difference.x); + pi_v_ = std::atan2(-difference.z, std::sqrt(difference.x * difference.x + difference.y * difference.y)); + + rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); + rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); + } + + types::CrossTrackError IntegralLOSGuidance::calculate_crosstrack_error(const types::Inputs& inputs) { + + const Eigen::Vector3d diff_vec = (inputs.current_position - inputs.prev_point).as_vector(); + const Eigen::Vector3d e_perp = rotation_y_.toRotationMatrix().transpose() * rotation_z_.toRotationMatrix().transpose() * diff_vec; + + return types::CrossTrackError::from_vector(e_perp); + } + + types::Outputs IntegralLOSGuidance::calculate_outputs(const types::Inputs& inputs) { + update_angles(inputs); + const types::CrossTrackError e = calculate_crosstrack_error(inputs); + + int_h += e.y_e * m_params.time_step; + int_v += e.z_e * m_params.time_step; + + const double u_h = m_params.k_p_h * e.y_e + m_params.k_i_h * int_h; + const double u_v = m_params.k_p_v * e.z_e + m_params.k_i_v * int_v; + + const double psi_d = pi_h_ - std::atan(u_h); + const double theta_d = pi_v_ + std::atan(u_v); + + return types::Outputs{psi_d, theta_d}; + } + +} // namespace vortex::guidance::los \ No newline at end of file diff --git a/guidance/los_guidance/src/lib/proportional_los.cpp b/guidance/los_guidance/src/lib/proportional_los.cpp index e94cf57e0..f50dc81bf 100755 --- a/guidance/los_guidance/src/lib/proportional_los.cpp +++ b/guidance/los_guidance/src/lib/proportional_los.cpp @@ -1,4 +1,4 @@ - #include "los_guidance/lib/proportional_los.hpp" + #include namespace vortex::guidance::los { @@ -12,27 +12,27 @@ namespace vortex::guidance::los { rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); - } + } types::CrossTrackError ProportionalLOSGuidance::calculate_crosstrack_error(const types::Inputs& inputs) const { const Eigen::Vector3d diff_vec = (inputs.current_position - inputs.prev_point).as_vector(); - const Eigen::Vector3d e_perp = rotation_y_.transpose() * rotation_z_.transpose() * diff_vec; + const Eigen::Vector3d e_perp = rotation_y_.toRotationMatrix().transpose() * rotation_z_.toRotationMatrix().transpose() * diff_vec; return types::CrossTrackError::from_vector(e_perp); } - types::Output ProportionalLOSGuidance::calculate_outputs(const types::Inputs& inputs) { + types::Outputs ProportionalLOSGuidance::calculate_outputs(const types::Inputs& inputs) { update_angles(inputs); const types::CrossTrackError e = calculate_crosstrack_error(inputs); - const double k_p_h = 1.0 / std::max(params_.lookahead_distance_h, 1e-9); - const double k_p_v = 1.0 / std::max(params_.lookahead_distance_v, 1e-9); + const double k_p_h = 1.0 / std::max(m_params.lookahead_distance_h, 1e-9); + const double k_p_v = 1.0 / std::max(m_params.lookahead_distance_v, 1e-9); const double psi_d = pi_h_ - std::atan(k_p_h * e.y_e); const double theta_d = pi_v_ + std::atan(k_p_v * e.z_e); - return types::Output{psi_d, theta_d}; + return types::Outputs{psi_d, theta_d}; } } // namespace vortex::guidance::los \ No newline at end of file diff --git a/guidance/los_guidance/src/los_guidance_node.cpp b/guidance/los_guidance/src/los_guidance_node.cpp index 931a57ded..439730a0c 100644 --- a/guidance/los_guidance/src/los_guidance_node.cpp +++ b/guidance/los_guidance/src/los_guidance_node.cpp @@ -4,7 +4,7 @@ int main(int argc, char** argv) { rclcpp::init(argc, argv); - auto node = std::make_shared(); + auto node = std::make_shared(); rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(node); diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 18884bb22..5d0e484bb 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -1,6 +1,9 @@ #include "los_guidance/los_guidance_ros.hpp" +#include "los_guidance/lib/types.hpp" #include +#include #include +#include const auto start_message = R"( _ ___ ____ ____ _ _ @@ -11,227 +14,268 @@ const auto start_message = R"( )"; -namespace vortex::guidance { - -LOSGuidanceNode::LOSGuidanceNode() : Node("los_guidance_node") { - time_step_ = std::chrono::milliseconds(10); - - set_subscribers_and_publisher(); - - set_action_server(); - - set_adaptive_los_guidance(); - - spdlog::info(start_message); -} - -void LOSGuidanceNode::set_subscribers_and_publisher() { - this->declare_parameter("topics.pose"); - this->declare_parameter("topics.guidance.los"); - this->declare_parameter("topics.waypoint"); - - std::string pose_topic = this->get_parameter("topics.pose").as_string(); - std::string guidance_topic = - this->get_parameter("topics.guidance.los").as_string(); - std::string waypoint_topic = - this->get_parameter("topics.waypoint").as_string(); - - auto qos_sensor_data = vortex::utils::qos_profiles::sensor_data_profile(1); - - reference_pub_ = this->create_publisher( - guidance_topic, qos_sensor_data); - - waypoint_sub_ = this->create_subscription( - waypoint_topic, qos_sensor_data, - std::bind(&LOSGuidanceNode::waypoint_callback, this, - std::placeholders::_1)); - - pose_sub_ = this->create_subscription< - geometry_msgs::msg::PoseWithCovarianceStamped>( - pose_topic, qos_sensor_data, - std::bind(&LOSGuidanceNode::pose_callback, this, - std::placeholders::_1)); -} - -void LOSGuidanceNode::set_action_server() { - this->declare_parameter("action_servers.los"); - std::string action_server_name = - this->get_parameter("action_servers.los").as_string(); - cb_group_ = - this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); - - action_server_ = - rclcpp_action::create_server( - this, action_server_name, - std::bind(&LOSGuidanceNode::handle_goal, this, - std::placeholders::_1, std::placeholders::_2), - std::bind(&LOSGuidanceNode::handle_cancel, this, - std::placeholders::_1), - std::bind(&LOSGuidanceNode::handle_accepted, this, - std::placeholders::_1), - rcl_action_server_get_default_options(), cb_group_); -} - -void LOSGuidanceNode::set_adaptive_los_guidance() { - this->declare_parameter("los.lookahead_distance_h"); - this->declare_parameter("los.lookahead_distance_v"); - this->declare_parameter("los.gamma_h"); - this->declare_parameter("los.gamma_v"); - this->declare_parameter("los.time_step"); - this->declare_parameter("los.u_desired"); - this->declare_parameter("los.goal_reached_tol"); - - LOS::Params params; - params.lookahead_distance_h = - this->get_parameter("los.lookahead_distance_h").as_double(); - params.lookahead_distance_v = - this->get_parameter("los.lookahead_distance_v").as_double(); - params.gamma_h = this->get_parameter("los.gamma_h").as_double(); - params.gamma_v = this->get_parameter("los.gamma_v").as_double(); - params.time_step = this->get_parameter("los.time_step").as_double(); - - adaptive_los_guidance_ = std::make_unique(params); -} - -void LOSGuidanceNode::waypoint_callback( - const geometry_msgs::msg::PointStamped::SharedPtr los_waypoint) { - fill_los_waypoints(*los_waypoint); - adaptive_los_guidance_->update_angles(last_point_, next_point_); - spdlog::info("Received waypoint"); -} - -void LOSGuidanceNode::pose_callback( - const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr - current_pose) { - eta_.x = current_pose->pose.pose.position.x; - eta_.y = current_pose->pose.pose.position.y; - eta_.z = current_pose->pose.pose.position.z; -} - -rclcpp_action::GoalResponse LOSGuidanceNode::handle_goal( - const rclcpp_action::GoalUUID&, - std::shared_ptr goal) { - (void)goal; - { - std::lock_guard lock(mutex_); - if (goal_handle_) { - if (goal_handle_->is_active()) { - spdlog::info("Aborting current goal and accepting new goal"); - preempted_goal_id_ = goal_handle_->get_goal_id(); - } - } +namespace vortex::guidance::los{ + + LosGuidanceNode::LosGuidanceNode() : Node("los_guidance_node"){ + double time_step_s = this->declare_parameter("time_step"); + time_step_ = std::chrono::milliseconds(static_cast(time_step_s * 1000)); + //auto config = this->declare_parameter("los_config_file"); + + + YAML::Node config = get_los_config("config/guidance_params.yaml"); + + parse_common_config(config["common"]); + set_subscribers_and_publisher(); + set_action_server(); + //set_los_mode_service(); + set_adaptive_los_guidance(config); + set_proportional_los_guidance(config); + set_integral_los_guidance(config); + + spdlog::info(start_message); + } + + void LosGuidanceNode::set_subscribers_and_publisher() { + + this->declare_parameter("topics.pose"); + this->declare_parameter("topics.guidance.los"); + this->declare_parameter("topics.waypoint"); + + std::string pose_topic = this->get_parameter("topics.pose").as_string(); + std::string guidance_topic = + this->get_parameter("topics.guidance.los").as_string(); + std::string waypoint_topic = + this->get_parameter("topics.waypoint").as_string(); + + auto qos_sensor_data = vortex::utils::qos_profiles::sensor_data_profile(1); + + reference_pub_ = this->create_publisher( + guidance_topic, qos_sensor_data); + + waypoint_sub_ = this->create_subscription( + waypoint_topic, qos_sensor_data, + std::bind(&LosGuidanceNode::waypoint_callback, this, + std::placeholders::_1)); + + pose_sub_ = this->create_subscription< + geometry_msgs::msg::PoseWithCovarianceStamped>( + pose_topic, qos_sensor_data, + std::bind(&LosGuidanceNode::pose_callback, this, + std::placeholders::_1)); } - spdlog::info("Accepted goal request"); - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; -} - -rclcpp_action::CancelResponse LOSGuidanceNode::handle_cancel( - const std::shared_ptr< - rclcpp_action::ServerGoalHandle> - goal_handle) { - spdlog::info("Received request to cancel goal"); - (void)goal_handle; - return rclcpp_action::CancelResponse::ACCEPT; -} - -void LOSGuidanceNode::handle_accepted( - const std::shared_ptr< - rclcpp_action::ServerGoalHandle> - goal_handle) { - execute(goal_handle); -} - -void LOSGuidanceNode::fill_los_waypoints( - const geometry_msgs::msg::PointStamped& los_waypoint) { - last_point_.x = eta_.x; - last_point_.y = eta_.y; - last_point_.z = eta_.z; - - next_point_.x = los_waypoint.point.x; - next_point_.y = los_waypoint.point.y; - next_point_.z = los_waypoint.point.z; -} - -vortex_msgs::msg::LOSGuidance LOSGuidanceNode::fill_los_reference() { - vortex_msgs::msg::LOSGuidance reference_msg; - reference_msg.pitch = pitch_d_; - reference_msg.yaw = yaw_d_; - reference_msg.surge = u_desired_; - - return reference_msg; -} - -void LOSGuidanceNode::execute( - const std::shared_ptr< - rclcpp_action::ServerGoalHandle> - goal_handle) { - { - std::lock_guard lock(mutex_); - this->goal_handle_ = goal_handle; + + void LosGuidanceNode::set_action_server() { + this->declare_parameter("action_servers.los"); + std::string action_server_name = + this->get_parameter("action_servers.los").as_string(); + cb_group_ = + this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + + action_server_ = + rclcpp_action::create_server( + this, action_server_name, + std::bind(&LosGuidanceNode::handle_goal, this, + std::placeholders::_1, std::placeholders::_2), + std::bind(&LosGuidanceNode::handle_cancel, this, + std::placeholders::_1), + std::bind(&LosGuidanceNode::handle_accepted, this, + std::placeholders::_1), + rcl_action_server_get_default_options(), cb_group_); } - u_desired_ = this->get_parameter("los.u_desired").as_double(); - goal_reached_tol_ = this->get_parameter("los.goal_reached_tol").as_double(); + + + void LosGuidanceNode::set_adaptive_los_guidance(YAML::Node config) { + auto adaptive_los_config = config["adaptive_los"]; + auto params = AdaptiveLosParams{}; + params.lookahead_distance_h = + adaptive_los_config["lookahead_distance_h"].as(); + params.lookahead_distance_v = + adaptive_los_config["lookahead_distance_v"].as(); + params.gamma_h = + adaptive_los_config["gama_h"].as(); + params.gamma_v = + adaptive_los_config["gama_v"].as(); + params.time_step = + adaptive_los_config["time_step"].as(); + + m_adaptive_los = std::make_unique(params); + } - spdlog::info("Executing goal"); + void LosGuidanceNode::set_proportional_los_guidance(YAML::Node config) { + auto proportional_los_config = config["proportional_los"]; + auto params = ProportionalLosParams{}; + params.lookahead_distance_h = + proportional_los_config["lookahead_distance_h"].as(); + params.lookahead_distance_v = + proportional_los_config["lookahead_distance_v"].as(); + params.k_p_h = proportional_los_config["k_p_h"].as(); + params.k_p_v = proportional_los_config["k_p_v"].as(); + params.time_step = + proportional_los_config["time_step"].as(); + + m_proportional_los = std::make_unique(params); + } - const geometry_msgs::msg::PointStamped los_waypoint = - goal_handle->get_goal()->goal; + void LosGuidanceNode::set_integral_los_guidance(YAML::Node config) { + auto integral_los_config = config["integral_los"]; + auto params = IntegralLosParams{}; + params.lookahead_distance_h = + integral_los_config["lookahead_distance_h"].as(); + params.lookahead_distance_v = + integral_los_config["lookahead_distance_v"].as(); + params.k_p_h = integral_los_config["k_p_h"].as(); + params.k_p_v = integral_los_config["k_p_v"].as(); + params.k_i_h = integral_los_config["k_i_h"].as(); + params.k_i_v = integral_los_config["k_i_v"].as(); + params.time_step = + integral_los_config["time_step"].as(); + + m_integral_los = std::make_unique(params); + } - fill_los_waypoints(los_waypoint); + void LosGuidanceNode::waypoint_callback( + const geometry_msgs::msg::PointStamped::SharedPtr los_waypoint) { - adaptive_los_guidance_->update_angles(last_point_, next_point_); + path_inputs_.prev_point = path_inputs_.current_position; + path_inputs_.next_point = types::Point::point_from_ros(los_waypoint->point); + spdlog::info("Received waypoint"); // remember to print waypoint that you get + } - auto feedback = - std::make_shared(); - auto result = std::make_shared(); + void LosGuidanceNode::pose_callback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr + current_pose) { - rclcpp::Rate loop_rate(1000.0 / time_step_.count()); + path_inputs_.current_position = types::Point::point_from_ros(current_pose->pose.pose.position); + + } - while (rclcpp::ok()) { + rclcpp_action::GoalResponse LosGuidanceNode::handle_goal( + const rclcpp_action::GoalUUID&, + std::shared_ptr goal) { + (void)goal; { std::lock_guard lock(mutex_); - if (goal_handle->get_goal_id() == preempted_goal_id_) { - result->success = false; - goal_handle->abort(result); - return; + if (goal_handle_) { + if (goal_handle_->is_active()) { + spdlog::info("Aborting current goal and accepting new goal"); + preempted_goal_id_ = goal_handle_->get_goal_id(); + } } } - if (goal_handle->is_canceling()) { - result->success = false; - goal_handle->canceled(result); - spdlog::info("Goal canceled"); - return; + spdlog::info("Accepted goal request"); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } + + rclcpp_action::CancelResponse LosGuidanceNode::handle_cancel( + const std::shared_ptr< + rclcpp_action::ServerGoalHandle> + goal_handle) { + spdlog::info("Received request to cancel goal"); + (void)goal_handle; + return rclcpp_action::CancelResponse::ACCEPT; + } + + void LosGuidanceNode::handle_accepted( + const std::shared_ptr< + rclcpp_action::ServerGoalHandle> + goal_handle) { + execute(goal_handle); + } + + vortex_msgs::msg::LOSGuidance LosGuidanceNode::fill_los_reference(types::Outputs outputs) { + vortex_msgs::msg::LOSGuidance reference_msg; + reference_msg.pitch = outputs.theta_d; + reference_msg.yaw = outputs.psi_d; + reference_msg.surge = u_desired_; + + return reference_msg; + } + + YAML::Node LosGuidanceNode::get_los_config(std::string yaml_file_path) { + YAML::Node config = YAML::LoadFile(yaml_file_path); + return config; + } + + void LosGuidanceNode::parse_common_config(YAML::Node common_config) { + u_desired_ = common_config["u_desired"].as(); + goal_reached_tol_ = common_config["goal_reached_tol"].as(); + } + + void LosGuidanceNode::execute( + + + const std::shared_ptr< + rclcpp_action::ServerGoalHandle> + goal_handle) { + { + std::lock_guard lock(mutex_); + this->goal_handle_ = goal_handle; } - LOS::CrossTrackError errors = - adaptive_los_guidance_->calculate_crosstrack_error(last_point_, - eta_); + spdlog::info("Executing goal"); + + const geometry_msgs::msg::PointStamped los_waypoint = goal_handle->get_goal()->goal; - yaw_d_ = adaptive_los_guidance_->calculate_psi_d(errors.y_e); - pitch_d_ = adaptive_los_guidance_->calculate_theta_d(errors.z_e); + path_inputs_.prev_point = path_inputs_.current_position; + path_inputs_.next_point = types::Point::point_from_ros(los_waypoint.point); - adaptive_los_guidance_->update_adaptive_estimates(errors); + auto feedback = std::make_shared(); + auto result = std::make_shared(); - vortex_msgs::msg::LOSGuidance reference_msg = fill_los_reference(); + rclcpp::Rate loop_rate(1000.0 / time_step_.count()); - feedback->feedback = reference_msg; + while (rclcpp::ok()) { + { + std::lock_guard lock(mutex_); + if (goal_handle->get_goal_id() == preempted_goal_id_) { + result->success = false; + goal_handle->abort(result); + return; + } + } + + if (goal_handle->is_canceling()) { + result->success = false; + goal_handle->canceled(result); + spdlog::info("Goal canceled"); + return; + } + + types::Outputs outputs; + + switch(m_method) { + case types::ActiveLosMethod::ADAPTIVE: + outputs = m_adaptive_los->calculate_outputs(path_inputs_); + break; + case types::ActiveLosMethod::PROPORTIONAL: + outputs = m_proportional_los->calculate_outputs(path_inputs_); + break; + case types::ActiveLosMethod::INTEGRAL: + outputs = m_integral_los->calculate_outputs(path_inputs_); + break; + default: + spdlog::error("Invalid LOS method selected"); + result->success = false; + goal_handle->abort(result); + return; + } + + vortex_msgs::msg::LOSGuidance reference_msg = fill_los_reference(outputs); + feedback->feedback = reference_msg; - goal_handle->publish_feedback(feedback); - reference_pub_->publish(reference_msg); + goal_handle->publish_feedback(feedback); + reference_pub_->publish(reference_msg); - if ((eta_ - next_point_).as_vector().norm() < goal_reached_tol_) { + if ((path_inputs_.current_position - path_inputs_.next_point).as_vector().norm() < goal_reached_tol_) { result->success = true; goal_handle->succeed(result); - u_desired_ = 0.0; - vortex_msgs::msg::LOSGuidance reference_msg = fill_los_reference(); - reference_pub_->publish(reference_msg); spdlog::info("Goal reached"); return; } - loop_rate.sleep(); - } -} + loop_rate.sleep(); + } + } -} // namespace vortex::guidance +} // namespace vortex::guidance diff --git a/guidance/los_guidance/test/CMakeLists.txt b/guidance/los_guidance/test/CMakeLists.txt index f2b81bdc2..e1552c071 100644 --- a/guidance/los_guidance/test/CMakeLists.txt +++ b/guidance/los_guidance/test/CMakeLists.txt @@ -6,17 +6,21 @@ include(GoogleTest) set(TEST_BINARY_NAME ${PROJECT_NAME}_test) add_executable( ${TEST_BINARY_NAME} + test_main.cpp adaptive_los_test.cpp proportional_los_test.cpp integral_los_test.cpp ) -target_link_libraries( - ${TEST_BINARY_NAME} +target_link_libraries(${TEST_BINARY_NAME} PRIVATE ${LIB_NAME} - GTest::GTest + yaml-cpp + Eigen3::Eigen + spdlog::spdlog + fmt::fmt + GTest::GTest ) ament_target_dependencies(${TEST_BINARY_NAME} PUBLIC Eigen3) diff --git a/guidance/los_guidance/test/adaptive_los_test.cpp b/guidance/los_guidance/test/adaptive_los_test.cpp index a0195a79a..42b5800d8 100644 --- a/guidance/los_guidance/test/adaptive_los_test.cpp +++ b/guidance/los_guidance/test/adaptive_los_test.cpp @@ -17,7 +17,7 @@ namespace vortex::guidance::los{ //new namespace added los return p; } - AdaptiveLOSGuidance los_; + AdaptiveLOSGuidance los_; const double tol = 1e-9; }; /* @@ -88,16 +88,16 @@ namespace vortex::guidance::los{ //new namespace added los */ // Test commanded angles when drone is to the right of the track - TEST_F(AdaptiveLosTests, T06_test_commanded_angles) { + TEST_F(AdaptiveLosTest, T06_test_commanded_angles) { types::Inputs inputs; inputs.prev_point = types::Point{0.0, 0.0, 0.0}; inputs.next_point = types::Point{1.0, 0.0, 0.0}; inputs.current_position = types::Point{0.0, 0.5, 0.0}; - const types::Output O = los_.calculate_outputs(inputs); + const types::Outputs O = los_.calculate_outputs(inputs); // Heading cmd should be between -pi/2 and 0 - EXPECT_LT(O.psi_d 0.0); + EXPECT_LT(O.psi_d, 0.0); EXPECT_GT(O.psi_d, -1.57); // Pitch cmd should be zero @@ -105,13 +105,13 @@ namespace vortex::guidance::los{ //new namespace added los } // Test commanded angles when drone is to the left of the track - TEST_F(AdaptiveLosTests, T07_test_commanded_angles) { + TEST_F(AdaptiveLosTest, T07_test_commanded_angles) { types::Inputs inputs; inputs.prev_point = types::Point{0.0, 0.0, 0.0}; inputs.next_point = types::Point{1.0, 0.0, 0.0}; inputs.current_position = types::Point{0.0, -0.5, 0.0}; - const types::Output O = los_.calculate_outputs(inputs); + const types::Outputs O = los_.calculate_outputs(inputs); // Heading cmd should be between 0 and pi/2 EXPECT_GT(O.psi_d, 0.0); @@ -121,13 +121,13 @@ namespace vortex::guidance::los{ //new namespace added los } // Test commanded angles when drone is under the track - TEST_F(AdaptiveLosTests, T08_test_commanded_angles) { + TEST_F(AdaptiveLosTest, T08_test_commanded_angles) { types::Inputs inputs; inputs.prev_point = types::Point{0.0, 0.0, 0.0}; inputs.next_point = types::Point{1.0, 0.0, 0.0}; inputs.current_position = types::Point{0.0, 0.0, 0.5}; - const types::Output O = los_.calculate_outputs(inputs); + const types::Outputs O = los_.calculate_outputs(inputs); // Heading cmd should be 0 EXPECT_NEAR(O.psi_d, 0.0, tol); @@ -137,13 +137,13 @@ namespace vortex::guidance::los{ //new namespace added los } // Test commanded angles when drone is over the track - TEST_F(AdaptiveLosTests, T09_test_commanded_angles) { + TEST_F(AdaptiveLosTest, T09_test_commanded_angles) { types::Inputs inputs; inputs.prev_point = types::Point{0.0, 0.0, 0.0}; inputs.next_point = types::Point{1.0, 0.0, 0.0}; inputs.current_position = types::Point{0.0, 0.0, -0.5}; - const types::Output O = los_.calculate_outputs(inputs); + const types::Outputs O = los_.calculate_outputs(inputs); // Heading cmd should be 0 EXPECT_NEAR(O.psi_d, 0.0, tol); @@ -154,13 +154,13 @@ namespace vortex::guidance::los{ //new namespace added los // Test commanded angles when drone is over and to the right of the track - TEST_F(AdaptiveLosTests, T10_test_commanded_angles) { + TEST_F(AdaptiveLosTest, T10_test_commanded_angles) { types::Inputs inputs; inputs.prev_point = types::Point{0.0, 0.0, 0.0}; inputs.next_point = types::Point{1.0, 0.0, 0.0}; inputs.current_position = types::Point{0.0, 0.5, -0.5}; - const types::Output O = los_.calculate_outputs(inputs); + const types::Outputs O = los_.calculate_outputs(inputs); // Heading cmd should be between -pi/2 and 0 EXPECT_LT(O.psi_d, 0.0); @@ -170,10 +170,6 @@ namespace vortex::guidance::los{ //new namespace added los EXPECT_GT(O.theta_d, -1.57); } -} // namespace vortex::guidance +} // namespace vortex::guidance::los -int main(int argc, char** argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/guidance/los_guidance/test/integral_los_test.cpp b/guidance/los_guidance/test/integral_los_test.cpp index e69de29bb..218e818c1 100755 --- a/guidance/los_guidance/test/integral_los_test.cpp +++ b/guidance/los_guidance/test/integral_los_test.cpp @@ -0,0 +1,110 @@ +#include +#include "los_guidance/lib/adaptive_los.hpp" +#include "los_guidance/lib/integral_los.hpp" + +namespace vortex::guidance::los{ + + class IntegralLosTest : public ::testing::Test { + protected: + IntegralLosTest() : Ilos_{get_params()} {} + + IntegralLosParams get_params() { + IntegralLosParams params; + params.lookahead_distance_h = 1.0; + params.lookahead_distance_v = 1.0; + params.k_i_h = 0.1; // needs tuning + params.k_i_v = 0.1; // needs tuning + params.k_p_h = 0.667; // needs tuning + params.k_p_v = 0.582; // needs tuning + params.time_step = 0.01; + return params; + } + + IntegralLOSGuidance Ilos_; + const double tol = 1e-9; + }; + + // Test commanded angles when drone is to the right of the track + TEST_F(IntegralLosTest, T06_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.5, 0.0}; + + const types::Outputs O = Ilos_.calculate_outputs(inputs); + + // Heading cmd should be between -pi/2 and 0 + EXPECT_LT(O.psi_d, 0.0); + EXPECT_GT(O.psi_d, -1.57); + + // Pitch cmd should be zero + EXPECT_NEAR(O.theta_d, 0.0, tol); + } + + // Test commanded angles when drone is to the left of the track + TEST_F(IntegralLosTest, T07_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, -0.5, 0.0}; + + const types::Outputs O = Ilos_.calculate_outputs(inputs); + + // Heading cmd should be between 0 and pi/2 + EXPECT_GT(O.psi_d, 0.0); + EXPECT_LT(O.psi_d, 1.57); + // Pitch cmd should be zero + EXPECT_NEAR(O.theta_d, 0.0, tol); + } + + // Test commanded angles when drone is under the track + TEST_F(IntegralLosTest, T08_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, 0.5}; + + const types::Outputs O = Ilos_.calculate_outputs(inputs); + + // Heading cmd should be 0 + EXPECT_NEAR(O.psi_d, 0.0, tol); + // Pitch cmd should be between 0 and pi/2 + EXPECT_GT(O.theta_d, 0.0); + EXPECT_LT(O.theta_d, 1.57); + } + + // Test commanded angles when drone is over the track + TEST_F(IntegralLosTest, T09_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, -0.5}; + + const types::Outputs O = Ilos_.calculate_outputs(inputs); + + // Heading cmd should be 0 + EXPECT_NEAR(O.psi_d, 0.0, tol); + // Pitch cmd should be between -pi/2 and 0 + EXPECT_LT(O.theta_d, 0.0); + EXPECT_GT(O.theta_d, -1.57); + } + + // Test commanded angles when drone is over and to the right of the track + + TEST_F(IntegralLosTest, T10_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.5, -0.5}; + + const types::Outputs O = Ilos_.calculate_outputs(inputs); + + // Heading cmd should be between -pi/2 and 0 + EXPECT_LT(O.psi_d, 0.0); + EXPECT_GT(O.psi_d, -1.57); + // Pitch cmd should be between -pi/2 and 0 + EXPECT_LT(O.theta_d, 0.0); + EXPECT_GT(O.theta_d, -1.57); + } + +} // namespace vortex::guidance diff --git a/guidance/los_guidance/test/proportional_los_test.cpp b/guidance/los_guidance/test/proportional_los_test.cpp index de040614c..3c890cbcd 100755 --- a/guidance/los_guidance/test/proportional_los_test.cpp +++ b/guidance/los_guidance/test/proportional_los_test.cpp @@ -1,5 +1,5 @@ #include -#include "los_guidance/lib/adaptive_los.hpp" +#include "los_guidance/lib/proportional_los.hpp" namespace vortex::guidance::los{ @@ -7,7 +7,7 @@ namespace vortex::guidance::los{ protected: ProportionalLosTest() : Plos_{get_params()} {} - ProportionalLosTest get_params() { + ProportionalLosParams get_params() { ProportionalLosParams params; params.lookahead_distance_h = 10.0; params.lookahead_distance_v = 10.0; @@ -19,7 +19,7 @@ namespace vortex::guidance::los{ ProportionalLOSGuidance Plos_; const double tol = 1e-9; - }; + }; // Test commanded angles when drone is to the right of the track TEST_F(ProportionalLosTest, T06_test_commanded_angles) { @@ -28,10 +28,10 @@ namespace vortex::guidance::los{ inputs.next_point = types::Point{1.0, 0.0, 0.0}; inputs.current_position = types::Point{0.0, 0.5, 0.0}; - const types::Output O = Plos_.calculate_outputs(inputs); + const types::Outputs O = Plos_.calculate_outputs(inputs); // Heading cmd should be between -pi/2 and 0 - EXPECT_LT(O.psi_d 0.0); + EXPECT_LT(O.psi_d, 0.0); EXPECT_GT(O.psi_d, -1.57); // Pitch cmd should be zero @@ -45,7 +45,7 @@ namespace vortex::guidance::los{ inputs.next_point = types::Point{1.0, 0.0, 0.0}; inputs.current_position = types::Point{0.0, -0.5, 0.0}; - const types::Output O = Plos_.calculate_outputs(inputs); + const types::Outputs O = Plos_.calculate_outputs(inputs); // Heading cmd should be between 0 and pi/2 EXPECT_GT(O.psi_d, 0.0); @@ -61,7 +61,7 @@ namespace vortex::guidance::los{ inputs.next_point = types::Point{1.0, 0.0, 0.0}; inputs.current_position = types::Point{0.0, 0.0, 0.5}; - const types::Output O = Plos_.calculate_outputs(inputs); + const types::Outputs O = Plos_.calculate_outputs(inputs); // Heading cmd should be 0 EXPECT_NEAR(O.psi_d, 0.0, tol); @@ -77,7 +77,7 @@ namespace vortex::guidance::los{ inputs.next_point = types::Point{1.0, 0.0, 0.0}; inputs.current_position = types::Point{0.0, 0.0, -0.5}; - const types::Output O = Plos_.calculate_outputs(inputs); + const types::Outputs O = Plos_.calculate_outputs(inputs); // Heading cmd should be 0 EXPECT_NEAR(O.psi_d, 0.0, tol); @@ -94,7 +94,7 @@ namespace vortex::guidance::los{ inputs.next_point = types::Point{1.0, 0.0, 0.0}; inputs.current_position = types::Point{0.0, 0.5, -0.5}; - const types::Output O = Plos_.calculate_outputs(inputs); + const types::Outputs O = Plos_.calculate_outputs(inputs); // Heading cmd should be between -pi/2 and 0 EXPECT_LT(O.psi_d, 0.0); @@ -106,8 +106,3 @@ namespace vortex::guidance::los{ } // namespace vortex::guidance -int main(int argc, char** argv) { - testing::InitGoogleTest(&argc, argv); - - return RUN_ALL_TESTS(); -} diff --git a/guidance/los_guidance/test/test_main.cpp b/guidance/los_guidance/test/test_main.cpp new file mode 100644 index 000000000..6ace5b598 --- /dev/null +++ b/guidance/los_guidance/test/test_main.cpp @@ -0,0 +1,8 @@ +// test/test_main.cpp +#include + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} \ No newline at end of file From c687343baea52c6b11f2de4bb47cb90d1d175ca1 Mon Sep 17 00:00:00 2001 From: Anbit Date: Wed, 12 Nov 2025 14:45:44 +0100 Subject: [PATCH 027/128] Complete los switchin --- auv_setup/config/robots/orca.yaml | 3 + .../los_guidance/config/guidance_params.yaml | 6 +- .../include/los_guidance/lib/adaptive_los.hpp | 1 - .../include/los_guidance/lib/types.hpp | 6 +- .../include/los_guidance/los_guidance_ros.hpp | 8 +- .../launch/los_guidance.launch.py | 3 + .../los_guidance/src/los_guidance_ros.cpp | 75 ++++++++++++------- 7 files changed, 64 insertions(+), 38 deletions(-) diff --git a/auv_setup/config/robots/orca.yaml b/auv_setup/config/robots/orca.yaml index 47039a4b9..1fef5f08b 100644 --- a/auv_setup/config/robots/orca.yaml +++ b/auv_setup/config/robots/orca.yaml @@ -136,6 +136,9 @@ reference_filter: "reference_filter" los: "los_guidance" + services: # Maybe not the right place for this? + los_mode: "set_los_mode" + fsm: docking: docking_station_offset: -1.0 diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index c65637117..0708d38a4 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -15,11 +15,11 @@ integer_los: lookahead_distance_v: 0.6 k_p_h: 0.5 k_p_v: 0.5 - K_i_h: 0.1 - K_i_v: 0.1 + k_i_h: 0.1 + k_i_v: 0.1 common: - los_method: "ADAPTIVE" + active_los_method: 2 # 0: Proportional, 1: Integral, 2: Adaptive u_desired: 0.3 goal_reached_tol: 1.0 \ No newline at end of file diff --git a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp index 9282fc8b2..3b18afb47 100644 --- a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp @@ -34,7 +34,6 @@ namespace vortex::guidance::los { void update_adaptive_estimates(const types::CrossTrackError& cross_track_error); AdaptiveLosParams m_params{}; - //usikker om disse skal vaere med i den nye strukturen? Eigen::Matrix3d rotation_y_ = Eigen::Matrix3d::Zero(); Eigen::Matrix3d rotation_z_ = Eigen::Matrix3d::Zero(); double pi_h_{}; diff --git a/guidance/los_guidance/include/los_guidance/lib/types.hpp b/guidance/los_guidance/include/los_guidance/lib/types.hpp index fd2340bf2..d20f873f6 100755 --- a/guidance/los_guidance/include/los_guidance/lib/types.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/types.hpp @@ -50,9 +50,9 @@ namespace vortex::guidance::los::types{ }; enum class ActiveLosMethod { - PROPORTIONAL, - INTEGRAL, - ADAPTIVE + PROPORTIONAL, // 0 + INTEGRAL, // 1 + ADAPTIVE // 2 }; } // namespace vortex::guidance::los::types diff --git a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp index 4c9ca3f7b..f3bdea8bd 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -12,7 +12,7 @@ #include #include #include -#include +#include #include #include @@ -30,7 +30,7 @@ namespace vortex::guidance::los { void set_action_server(); // @brief Determine the LOS mode service - void set_los_mode_service(); + void set_service_server(); // @brief Set the adaptive LOS guidance parameters void set_adaptive_los_guidance(YAML::Node config); @@ -77,6 +77,10 @@ namespace vortex::guidance::los { void execute(const std::shared_ptr> goal_handle); + void set_los_mode( + const std::shared_ptr request, + std::shared_ptr response); + vortex_msgs::msg::LOSGuidance fill_los_reference(types::Outputs output); YAML::Node get_los_config(std::string yaml_file_path); diff --git a/guidance/los_guidance/launch/los_guidance.launch.py b/guidance/los_guidance/launch/los_guidance.launch.py index f726983f0..a9f0b628b 100644 --- a/guidance/los_guidance/launch/los_guidance.launch.py +++ b/guidance/los_guidance/launch/los_guidance.launch.py @@ -31,3 +31,6 @@ def generate_launch_description(): output="screen", ) return LaunchDescription([los_guidance_node]) + +# remeber to make them able to swich in the middle of a mission and if you swirch methid the parameters should'nt be reloaded +# unless its a new section \ No newline at end of file diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 5d0e484bb..639325328 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -21,13 +21,15 @@ namespace vortex::guidance::los{ time_step_ = std::chrono::milliseconds(static_cast(time_step_s * 1000)); //auto config = this->declare_parameter("los_config_file"); + const std::string yaml_path =this->declare_parameter("los_config_file"); - YAML::Node config = get_los_config("config/guidance_params.yaml"); + + YAML::Node config = get_los_config(yaml_path); parse_common_config(config["common"]); set_subscribers_and_publisher(); set_action_server(); - //set_los_mode_service(); + set_service_server(); set_adaptive_los_guidance(config); set_proportional_los_guidance(config); set_integral_los_guidance(config); @@ -83,53 +85,60 @@ namespace vortex::guidance::los{ rcl_action_server_get_default_options(), cb_group_); } - + void LosGuidanceNode::set_service_server(){ + this->declare_parameter("services.los_mode", "set_los_mode"); + std::string service_name = + this->get_parameter("services.los_mode").as_string(); + + los_mode_service_ = this->create_service( + service_name, + std::bind(&LosGuidanceNode::set_los_mode, this, + std::placeholders::_1, std::placeholders::_2)); + + } void LosGuidanceNode::set_adaptive_los_guidance(YAML::Node config) { - auto adaptive_los_config = config["adaptive_los"]; + auto adaptive_los_config = config["adaptive_los"]; spdlog::info("A0"); auto params = AdaptiveLosParams{}; params.lookahead_distance_h = - adaptive_los_config["lookahead_distance_h"].as(); + adaptive_los_config["lookahead_distance_h"].as(); spdlog::info("A1"); params.lookahead_distance_v = - adaptive_los_config["lookahead_distance_v"].as(); + adaptive_los_config["lookahead_distance_v"].as();spdlog::info("A2"); params.gamma_h = - adaptive_los_config["gama_h"].as(); + adaptive_los_config["gamma_h"].as();spdlog::info("A3"); params.gamma_v = - adaptive_los_config["gama_v"].as(); - params.time_step = - adaptive_los_config["time_step"].as(); + adaptive_los_config["gamma_v"].as();spdlog::info("A4"); + params.time_step = static_cast(time_step_.count()) / 1000.0;spdlog::info("A5"); m_adaptive_los = std::make_unique(params); } void LosGuidanceNode::set_proportional_los_guidance(YAML::Node config) { - auto proportional_los_config = config["proportional_los"]; + auto proportional_los_config = config["prop_los"];spdlog::info("P0"); auto params = ProportionalLosParams{}; params.lookahead_distance_h = - proportional_los_config["lookahead_distance_h"].as(); + proportional_los_config["lookahead_distance_h"].as();spdlog::info("P1"); params.lookahead_distance_v = - proportional_los_config["lookahead_distance_v"].as(); - params.k_p_h = proportional_los_config["k_p_h"].as(); - params.k_p_v = proportional_los_config["k_p_v"].as(); - params.time_step = - proportional_los_config["time_step"].as(); + proportional_los_config["lookahead_distance_v"].as();spdlog::info("P2"); + params.k_p_h = proportional_los_config["k_p_h"].as();spdlog::info("P3"); + params.k_p_v = proportional_los_config["k_p_v"].as();spdlog::info("P4"); + params.time_step = static_cast(time_step_.count()) / 1000.0; spdlog::info("P5"); m_proportional_los = std::make_unique(params); } void LosGuidanceNode::set_integral_los_guidance(YAML::Node config) { - auto integral_los_config = config["integral_los"]; + auto integral_los_config = config["integer_los"];spdlog::info("I0"); auto params = IntegralLosParams{}; params.lookahead_distance_h = - integral_los_config["lookahead_distance_h"].as(); + integral_los_config["lookahead_distance_h"].as();spdlog::info("I1"); params.lookahead_distance_v = - integral_los_config["lookahead_distance_v"].as(); - params.k_p_h = integral_los_config["k_p_h"].as(); - params.k_p_v = integral_los_config["k_p_v"].as(); - params.k_i_h = integral_los_config["k_i_h"].as(); - params.k_i_v = integral_los_config["k_i_v"].as(); - params.time_step = - integral_los_config["time_step"].as(); + integral_los_config["lookahead_distance_v"].as();spdlog::info("I2"); + params.k_p_h = integral_los_config["k_p_h"].as();spdlog::info("I3"); + params.k_p_v = integral_los_config["k_p_v"].as();spdlog::info("I4"); + params.k_i_h = integral_los_config["k_i_h"].as();spdlog::info("I5"); + params.k_i_v = integral_los_config["k_i_v"].as();spdlog::info("I6"); + params.time_step = static_cast(time_step_.count()) / 1000.0;spdlog::info("I7"); m_integral_los = std::make_unique(params); } @@ -182,7 +191,14 @@ namespace vortex::guidance::los{ goal_handle) { execute(goal_handle); } - + void LosGuidanceNode::set_los_mode( + const std::shared_ptr request, + std::shared_ptr response) { + + m_method = static_cast(request->mode); + spdlog::info("LOS mode set to {}", static_cast(m_method)); + response->success = true; + } vortex_msgs::msg::LOSGuidance LosGuidanceNode::fill_los_reference(types::Outputs outputs) { vortex_msgs::msg::LOSGuidance reference_msg; reference_msg.pitch = outputs.theta_d; @@ -198,8 +214,9 @@ namespace vortex::guidance::los{ } void LosGuidanceNode::parse_common_config(YAML::Node common_config) { - u_desired_ = common_config["u_desired"].as(); - goal_reached_tol_ = common_config["goal_reached_tol"].as(); + u_desired_ = common_config["u_desired"].as(); spdlog::info("C1"); + goal_reached_tol_ = common_config["goal_reached_tol"].as(); spdlog::info("C2"); + m_method = static_cast(common_config["active_los_method"].as()); spdlog::info("C3"); } void LosGuidanceNode::execute( From 314a60b9f7759db9ee576263dfcbf265bd1e79c8 Mon Sep 17 00:00:00 2001 From: Henrik Date: Wed, 12 Nov 2025 14:55:35 +0100 Subject: [PATCH 028/128] All changes until christmas --- .../config/parameters.yaml | 6 ++-- .../include/velocity_controller/test_VC.hpp | 5 +-- .../velocity_controller.hpp | 4 +++ .../launch/VCnTest.launch.py | 6 ++-- control/velocity_controller/src/LQR_setup.cpp | 34 +++++++++++++++---- control/velocity_controller/src/PID_setup.cpp | 5 +-- control/velocity_controller/src/test_VC.cpp | 5 +-- .../src/velocity_controller.cpp | 25 +++++++++++--- 8 files changed, 68 insertions(+), 22 deletions(-) diff --git a/control/velocity_controller/config/parameters.yaml b/control/velocity_controller/config/parameters.yaml index 7f08156e6..9f396c83d 100644 --- a/control/velocity_controller/config/parameters.yaml +++ b/control/velocity_controller/config/parameters.yaml @@ -30,8 +30,8 @@ inertia_matrix: [30.0, 0.6, 0.0, 0.6, 1.629, 0.0, 0.0, 0.0, 1.729] calculation_rate: 200 #ms integer - publish_rate: 1000 #ms + publish_rate: 100 #ms #Clamp parameter - max_force: 99.5 - controller_type: 1 #1 PID 2 LQR + max_force: 1000.0 #should maybe be 99.5 + controller_type: 2 #1 PID 2 LQR diff --git a/control/velocity_controller/include/velocity_controller/test_VC.hpp b/control/velocity_controller/include/velocity_controller/test_VC.hpp index c8974d097..c8b4fb85b 100644 --- a/control/velocity_controller/include/velocity_controller/test_VC.hpp +++ b/control/velocity_controller/include/velocity_controller/test_VC.hpp @@ -9,6 +9,7 @@ #include "velocity_controller/velocity_controller.hpp" #include #include +#include "vortex_msgs/msg/los_guidance.hpp" class test_VC : public rclcpp::Node{ public: @@ -22,7 +23,7 @@ class test_VC : public rclcpp::Node{ //guidance_data reference; Guidance_data current_state; //Subscribers and publishers - rclcpp::Publisher::SharedPtr publisher_guidance; + rclcpp::Publisher::SharedPtr publisher_guidance; rclcpp::Publisher::SharedPtr publisher_odom; rclcpp::Subscription::SharedPtr subscription_thrust; //Timers @@ -30,7 +31,7 @@ class test_VC : public rclcpp::Node{ rclcpp::Clock::SharedPtr clock_; //Messages std::vector thrust_vector; - std_msgs::msg::Float64MultiArray reference_msg; + vortex_msgs::msg::LOSGuidance reference_msg; //Topics std::string topic_odom; diff --git a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp index e559297ef..4b3762254 100644 --- a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp +++ b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp @@ -78,6 +78,10 @@ class Velocity_node : public rclcpp::Node{ LQRparameters lqr_parameters; std::vector inertia_matrix; + + //Test + rclcpp::Publisher::SharedPtr publisher_reference; + }; diff --git a/control/velocity_controller/launch/VCnTest.launch.py b/control/velocity_controller/launch/VCnTest.launch.py index bc53f7daa..6e2e20efd 100644 --- a/control/velocity_controller/launch/VCnTest.launch.py +++ b/control/velocity_controller/launch/VCnTest.launch.py @@ -18,7 +18,7 @@ def generate_launch_description(): PythonLaunchDescriptionSource( os.path.join(stonefish_dir, 'launch', 'simulation.launch.py') ), - launch_arguments={'scenario': 'tacc','rendering_quality': 'low','rendering':'true'}.items(), + launch_arguments={'rendering_quality': 'low','rendering':'false'}.items(), ) orca_sim = TimerAction( period=12.0, @@ -54,7 +54,9 @@ def generate_launch_description(): executable='velocity_controller_node', name=velocity_controller_name, output='screen', - parameters=[config_path]), + parameters=[config_path] + #arguments=['--ros-args','--log-level','debug'] + ), Node(package='velocity_controller', executable='test_VC_node', name=test_VC_name, diff --git a/control/velocity_controller/src/LQR_setup.cpp b/control/velocity_controller/src/LQR_setup.cpp index 441f17813..8d77b51ba 100644 --- a/control/velocity_controller/src/LQR_setup.cpp +++ b/control/velocity_controller/src/LQR_setup.cpp @@ -4,6 +4,7 @@ #include #include #include +#include //#include //#include //#include @@ -12,7 +13,7 @@ #include "velocity_controller/utilities.hpp" #include #include - +Eigen::IOFormat fmt(Eigen::StreamPrecision, 0, ", ", "\n", "[", "]"); LQRController::LQRController(LQRparameters params,Eigen::Matrix3d inertia_matrix){ set_params(params); set_matrices(inertia_matrix); @@ -86,24 +87,30 @@ void LQRController::set_params(LQRparameters params){ } void LQRController::set_matrices(Eigen::Matrix3d inertia_matrix){ inertia_matrix_inv = inertia_matrix.inverse(); - state_weight_matrix.diagonal() < LQRController::update_error(Guidance_data guidance_values, State states){ @@ -179,10 +186,17 @@ LQRsolveResult LQRController::solve_k_p(const Eigen::Matrix &A,const int LDWORK=20*N*N,OUFACT=0,INFO=0; std::vector DWORK(LDWORK); Eigen::Matrix L=Eigen::Matrix::Zero(), G=L; + { + double detA = A_copy.determinant(); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Calling sb02md_: det(A)=%.6g, A(0,0)=%.6g, G(0,0)=%.6g", detA, A_copy(0,0), G(0,0)); + Eigen::EigenSolver> es(A_copy); + for (int i=0;i<6;++i){ + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "eigA[%d]=% .6g%+.6gi", i, es.eigenvalues()[i].real(), es.eigenvalues()[i].imag()); + } + } sb02mt_(&JOBG,&JOBL,&FACT,&UPLO,&N,&M,A_copy.data(),&LDA,B_copy.data(),&LDB,Q_copy.data(),&LDQ,R_copy.data(),&LDR,L.data(),&LDL,IPIV.data(),&OUFACT,G.data(),&LDG,IWORK.data(),DWORK.data(),&LDWORK,&INFO); Eigen::Matrix K; if (INFO!=0){ - //Some Error handling here. Also check that BRB in invertible RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "sb02mt_ returned INFO=%d", INFO); // Consider throwing or returning a default result. We'll return zeroed K and G for now. Eigen::Matrix K_zero = Eigen::Matrix::Zero(); @@ -195,7 +209,15 @@ LQRsolveResult LQRController::solve_k_p(const Eigen::Matrix &A,const Eigen::Matrix S=Eigen::Matrix::Zero(); Eigen::MatrixU=Eigen::Matrix::Zero(); int LDS=2*N,LDU=2*N,INFO1=0; - A_copy=A;Q_copy=Q; R_copy=R; + //A_copy=A;Q_copy=Q; R_copy=R; + { + double detA = A_copy.determinant(); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Calling sb02md_: det(A)=%.6g, A(0,0)=%.6g, G(0,0)=%.6g", detA, A_copy(0,0), G(0,0)); + Eigen::EigenSolver> es(A_copy); + for (int i=0;i<6;++i){ + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "eigA[%d]=% .6g%+.6gi", i, es.eigenvalues()[i].real(), es.eigenvalues()[i].imag()); + } + } sb02md_(&DICO,&HINV,&UPLO,&SCAL,&SORT,&N,A_copy.data(),&LDA,G.data(),&LDG,Q_copy.data(),&LDQ,RCOND.data(),WR.data(),WI.data(),S.data(),&LDS,U.data(),&LDU,IWORK.data(),DWORK.data(),&LDWORK,BWORK,&INFO1); if (INFO1!=0){ //Some Error handling here. Also check that BRB in invertible diff --git a/control/velocity_controller/src/PID_setup.cpp b/control/velocity_controller/src/PID_setup.cpp index cf0641152..451d26935 100644 --- a/control/velocity_controller/src/PID_setup.cpp +++ b/control/velocity_controller/src/PID_setup.cpp @@ -13,7 +13,7 @@ void PID_controller::calculate_thrust(double reference, double current_position, //P calculation last_output=k_p*error; //D calculation - last_output += k_d * (current_position - previous_position) / dt; + last_output += k_d * (error - previous_error) / dt; previous_position = current_position; //I calculation with anti-windup integral += error * dt; @@ -32,8 +32,9 @@ void PID_controller::calculate_thrust(double reference, double current_position, else if (last_output < min_output){ last_output = min_output; } + return; -}; +}; void PID_controller::reset_controller(){ integral = 0.0; previous_error = 0.0; diff --git a/control/velocity_controller/src/test_VC.cpp b/control/velocity_controller/src/test_VC.cpp index 617cbd34a..5145c4f59 100644 --- a/control/velocity_controller/src/test_VC.cpp +++ b/control/velocity_controller/src/test_VC.cpp @@ -7,6 +7,7 @@ #include "velocity_controller/test_VC.hpp" #include #include +#include "vortex_msgs/msg/los_guidance.hpp" //#include "velocity_controller/velocity_controller.hpp" //#include "LQR_setup.hpp" //Denne noden er kun for å teste velocity_controller noden @@ -19,7 +20,7 @@ test_VC::test_VC() : Node("test_VC_node"), current_state(0,2,2) topic_thrust = this->get_parameter("topics.thrust_topic").as_string(); topic_odom = this->get_parameter("topics.odom_topic").as_string(); topic_guidance = this->get_parameter("topics.guidance_topic").as_string(); - publisher_guidance = this->create_publisher(topic_guidance, 10); + publisher_guidance = this->create_publisher(topic_guidance, 10); publisher_odom = this->create_publisher(topic_odom,10); rclcpp::QoS orca_QoS(2); @@ -33,7 +34,7 @@ test_VC::test_VC() : Node("test_VC_node"), current_state(0,2,2) std::bind(&test_VC::send_guidance, this)); clock_ = this->get_clock(); RCLCPP_INFO(this->get_logger(), "Test_VC node has been started"); - reference_msg.data={2.0, 0.0, 0.0}; //Surge, pitch, yaw + reference_msg.surge=0.2;reference_msg.pitch=0.3;reference_msg.yaw=0.0; //Surge, pitch, yaw } diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index 9974a77b2..94c4fdfe8 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -15,7 +15,7 @@ //Lager en klasse velocity node //Konstruktør -Velocity_node::Velocity_node() : Node("velocity_controller_node"), PID_surge(10,1,1), PID_yaw(10,1,1), PID_pitch(10,1,1), lqr_controller() +Velocity_node::Velocity_node() : Node("velocity_controller_node"), PID_surge(20,1,2), PID_yaw(4,0.5,1), PID_pitch(4,0.5,1), lqr_controller() { //Dytter info til log RCLCPP_INFO(this->get_logger(), "Velocity control node has been started."); @@ -29,6 +29,7 @@ Velocity_node::Velocity_node() : Node("velocity_controller_node"), PID_surge(10, orca_QoS.keep_last(2).reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT); publisher_thrust = create_publisher(topic_thrust, orca_QoS); + publisher_reference = create_publisher("/reference",2); //Subscribers subscriber_Odometry = this->create_subscription( @@ -41,10 +42,12 @@ Velocity_node::Velocity_node() : Node("velocity_controller_node"), PID_surge(10, topic_killswitch,10, std::bind(&Velocity_node::killswitch_callback,this, std::placeholders::_1)); + + //Timer - timer_calculation = this->create_wall_timer(std::chrono::milliseconds(calculation_rate), std::bind(&Velocity_node::publish_thrust, this)); - timer_publish = this->create_wall_timer(std::chrono::milliseconds(publish_rate), std::bind(&Velocity_node::calc_thrust, this)); + timer_calculation = this->create_wall_timer(std::chrono::milliseconds(calculation_rate), std::bind(&Velocity_node::calc_thrust, this)); + timer_publish = this->create_wall_timer(std::chrono::milliseconds(publish_rate), std::bind(&Velocity_node::publish_thrust, this)); //Controllers PID_surge.set_output_limits(-max_force, max_force); PID_pitch.set_output_limits(-max_force, max_force); @@ -60,7 +63,14 @@ Velocity_node::Velocity_node() : Node("velocity_controller_node"), PID_surge(10, //Publish/timer functions void Velocity_node::publish_thrust() { + /*thrust_out.wrench.force.x=100; + thrust_out.wrench.force.y=0; + thrust_out.wrench.force.z=0; + thrust_out.wrench.torque.x=0; + thrust_out.wrench.torque.y=0; + thrust_out.wrench.torque.z=0;*/ publisher_thrust->publish(thrust_out); + //RCLCPP_DEBUG(this->get_logger(),"Publishing thrust: %.3f",thrust_out.wrench.force.x); } //** må forbedre integrasjon og derivasjons beregningene @@ -76,6 +86,7 @@ void Velocity_node::calc_thrust() thrust_out.wrench.force.x = PID_surge.output(); thrust_out.wrench.torque.y = PID_pitch.output(); thrust_out.wrench.torque.z = PID_yaw.output(); + break; } case 2:{ @@ -83,6 +94,7 @@ void Velocity_node::calc_thrust() Eigen::Vector3d u=lqr_controller.calculate_lqr_u(current_state,guidance_values); if (u==Eigen::Vector3d{9999,9999,9999}){ controller_type=1; + RCLCPP_ERROR(this->get_logger(),"Switching to PID"); } else{ thrust_out.wrench.force.x=u[0]; @@ -95,8 +107,9 @@ void Velocity_node::calc_thrust() break; } } - - publisher_thrust->publish(thrust_out); + std_msgs::msg::Float64MultiArray msg; + msg.data={guidance_values.surge,guidance_values.pitch,guidance_values.yaw}; + publisher_reference->publish(msg); return; } @@ -105,6 +118,8 @@ void Velocity_node::calc_thrust() //Callback functions void Velocity_node::guidance_callback(const vortex_msgs::msg::LOSGuidance::SharedPtr msg_ptr){ guidance_values = *msg_ptr; + //RCLCPP_DEBUG(this->get_logger(), "Guidance received: surge=%.3f pitch=%.3f yaw=%.3f", + // guidance_values.surge, guidance_values.pitch, guidance_values.yaw); return; } From 03560cf0204a231ee3d61d6fe09d8c8be033915e Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 12 Nov 2025 15:08:04 +0000 Subject: [PATCH 029/128] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- guidance/los_guidance/CMakeLists.txt | 6 +- .../los_guidance/config/guidance_params.yaml | 2 +- .../include/los_guidance/lib/adaptive_los.hpp | 71 +-- .../include/los_guidance/lib/integral_los.hpp | 73 +-- .../los_guidance/lib/proportional_los.hpp | 63 +- .../include/los_guidance/lib/types.hpp | 79 ++- .../include/los_guidance/los_guidance_ros.hpp | 166 +++--- .../launch/los_guidance.launch.py | 5 +- .../los_guidance/src/lib/adaptive_los.cpp | 89 +-- .../los_guidance/src/lib/integral_los.cpp | 60 +- .../los_guidance/src/lib/proportional_los.cpp | 57 +- .../los_guidance/src/los_guidance_ros.cpp | 537 +++++++++--------- guidance/los_guidance/test/CMakeLists.txt | 4 +- .../los_guidance/test/adaptive_los_test.cpp | 332 ++++++----- .../los_guidance/test/integral_los_test.cpp | 210 +++---- .../test/proportional_los_test.cpp | 203 ++++--- guidance/los_guidance/test/test_main.cpp | 2 +- 17 files changed, 1000 insertions(+), 959 deletions(-) mode change 100755 => 100644 guidance/los_guidance/include/los_guidance/lib/integral_los.hpp mode change 100755 => 100644 guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp mode change 100755 => 100644 guidance/los_guidance/include/los_guidance/lib/types.hpp mode change 100755 => 100644 guidance/los_guidance/src/lib/integral_los.cpp mode change 100755 => 100644 guidance/los_guidance/src/lib/proportional_los.cpp mode change 100755 => 100644 guidance/los_guidance/test/integral_los_test.cpp mode change 100755 => 100644 guidance/los_guidance/test/proportional_los_test.cpp diff --git a/guidance/los_guidance/CMakeLists.txt b/guidance/los_guidance/CMakeLists.txt index b9f48e915..443e7cb51 100644 --- a/guidance/los_guidance/CMakeLists.txt +++ b/guidance/los_guidance/CMakeLists.txt @@ -25,7 +25,7 @@ include_directories(include) set(LIB_NAME los_guidance_lib) add_library(${LIB_NAME} SHARED - src/lib/proportional_los.cpp + src/lib/proportional_los.cpp src/lib/integral_los.cpp src/lib/adaptive_los.cpp src/los_guidance_ros.cpp @@ -48,7 +48,7 @@ add_executable(los_guidance_node target_link_libraries(los_guidance_node ${LIB_NAME} - yaml-cpp + yaml-cpp ) install(TARGETS @@ -76,7 +76,7 @@ install(DIRECTORY ) if(BUILD_TESTING) - include(CTest) + include(CTest) add_subdirectory(test) endif() diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 0708d38a4..64f922a27 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -22,4 +22,4 @@ common: active_los_method: 2 # 0: Proportional, 1: Integral, 2: Adaptive u_desired: 0.3 goal_reached_tol: 1.0 - \ No newline at end of file + diff --git a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp index 3b18afb47..9b87a68cd 100644 --- a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp @@ -1,11 +1,11 @@ #ifndef ADAPTIVE_LOS_GUIDANCE_HPP #define ADAPTIVE_LOS_GUIDANCE_HPP +#include #include #include #include "los_guidance/lib/types.hpp" -#include - + /** * @brief Adaptive Line-of-Sight (LOS) guidance algorithm based on slide 113 * in "Fossen 2024 Lecture on 2D and 3D path-following control". @@ -13,36 +13,37 @@ namespace vortex::guidance::los { - struct AdaptiveLosParams { - double lookahead_distance_h{}; - double lookahead_distance_v{}; - double gamma_h{}; - double gamma_v{}; - double time_step{}; - }; - - class AdaptiveLOSGuidance { - public: - AdaptiveLOSGuidance(const AdaptiveLosParams& params); - ~AdaptiveLOSGuidance() = default; - - types::Outputs calculate_outputs(const types::Inputs& inputs); - - private: - void update_angles(const types::Inputs& inputs); - const types::CrossTrackError calculate_crosstrack_error(const types::Inputs& inputs); - void update_adaptive_estimates(const types::CrossTrackError& cross_track_error); - - AdaptiveLosParams m_params{}; - Eigen::Matrix3d rotation_y_ = Eigen::Matrix3d::Zero(); - Eigen::Matrix3d rotation_z_ = Eigen::Matrix3d::Zero(); - double pi_h_{}; - double pi_v_{}; - double beta_c_hat_{}; - double alpha_c_hat_{}; - - - }; // namespace vortex::guidance::los - -} -#endif // LOS_GUIDANCE_HPP \ No newline at end of file +struct AdaptiveLosParams { + double lookahead_distance_h{}; + double lookahead_distance_v{}; + double gamma_h{}; + double gamma_v{}; + double time_step{}; +}; + +class AdaptiveLOSGuidance { + public: + AdaptiveLOSGuidance(const AdaptiveLosParams& params); + ~AdaptiveLOSGuidance() = default; + + types::Outputs calculate_outputs(const types::Inputs& inputs); + + private: + void update_angles(const types::Inputs& inputs); + const types::CrossTrackError calculate_crosstrack_error( + const types::Inputs& inputs); + void update_adaptive_estimates( + const types::CrossTrackError& cross_track_error); + + AdaptiveLosParams m_params{}; + Eigen::Matrix3d rotation_y_ = Eigen::Matrix3d::Zero(); + Eigen::Matrix3d rotation_z_ = Eigen::Matrix3d::Zero(); + double pi_h_{}; + double pi_v_{}; + double beta_c_hat_{}; + double alpha_c_hat_{}; + +}; // namespace vortex::guidance::los + +} // namespace vortex::guidance::los +#endif // LOS_GUIDANCE_HPP diff --git a/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp b/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp old mode 100755 new mode 100644 index 3fbf122df..dbb437241 --- a/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp @@ -1,44 +1,45 @@ #ifndef INTEGRAL_LOS_GUIDANCE_HPP #define INTEGRAL_LOS_GUIDANCE_HPP +#include #include #include #include "los_guidance/lib/types.hpp" -#include - + namespace vortex::guidance::los { - struct IntegralLosParams { - double lookahead_distance_h{}; - double lookahead_distance_v{}; - double k_p_h{}; - double k_p_v{}; - double k_i_h{}; - double k_i_v{}; - double time_step{}; - }; - - class IntegralLOSGuidance { - public: - IntegralLOSGuidance(const IntegralLosParams& params); - ~IntegralLOSGuidance() = default; - - types::Outputs calculate_outputs(const types::Inputs& inputs); - - private: - void update_angles(const types::Inputs& inputs); - types::CrossTrackError calculate_crosstrack_error(const types::Inputs& inputs); - - IntegralLosParams m_params{}; - - double int_h{}; - double int_v{}; - //again i dont know if i should have them here or just in the functions - double pi_h_{}; - double pi_v_{}; - Eigen::AngleAxisd rotation_y_{0.0, Eigen::Vector3d::UnitY()}; - Eigen::AngleAxisd rotation_z_{0.0, Eigen::Vector3d::UnitZ()}; - }; -} - -#endif // INTEGRAL_LOS_GUIDANCE_HPP \ No newline at end of file +struct IntegralLosParams { + double lookahead_distance_h{}; + double lookahead_distance_v{}; + double k_p_h{}; + double k_p_v{}; + double k_i_h{}; + double k_i_v{}; + double time_step{}; +}; + +class IntegralLOSGuidance { + public: + IntegralLOSGuidance(const IntegralLosParams& params); + ~IntegralLOSGuidance() = default; + + types::Outputs calculate_outputs(const types::Inputs& inputs); + + private: + void update_angles(const types::Inputs& inputs); + types::CrossTrackError calculate_crosstrack_error( + const types::Inputs& inputs); + + IntegralLosParams m_params{}; + + double int_h{}; + double int_v{}; + // again i dont know if i should have them here or just in the functions + double pi_h_{}; + double pi_v_{}; + Eigen::AngleAxisd rotation_y_{0.0, Eigen::Vector3d::UnitY()}; + Eigen::AngleAxisd rotation_z_{0.0, Eigen::Vector3d::UnitZ()}; +}; +} // namespace vortex::guidance::los + +#endif // INTEGRAL_LOS_GUIDANCE_HPP diff --git a/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp b/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp old mode 100755 new mode 100644 index fad30eecc..f5ff3d6d9 --- a/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp @@ -1,39 +1,40 @@ #ifndef PROPORTIONAL_LOS_GUIDANCE_HPP #define PROPORTIONAL_LOS_GUIDANCE_HPP +#include #include #include -#include "los_guidance/lib/types.hpp" -#include - +#include "los_guidance/lib/types.hpp" + namespace vortex::guidance::los { - struct ProportionalLosParams { - double lookahead_distance_h{}; - double lookahead_distance_v{}; - double k_p_h{}; - double k_p_v{}; - double time_step{}; - }; - - class ProportionalLOSGuidance { - public: - ProportionalLOSGuidance(const ProportionalLosParams& params); - ~ProportionalLOSGuidance() = default; - - types::Outputs calculate_outputs(const types::Inputs& inputs); - - private: - void update_angles(const types::Inputs& inputs); - types::CrossTrackError calculate_crosstrack_error(const types::Inputs& inputs) const; - - ProportionalLosParams m_params{}; - //again i dont know if i should have them here or just in the functions - double pi_h_{0.0}; - double pi_v_{0.0}; - Eigen::AngleAxisd rotation_y_{0.0, Eigen::Vector3d::UnitY()}; - Eigen::AngleAxisd rotation_z_{0.0, Eigen::Vector3d::UnitZ()}; - }; - -} +struct ProportionalLosParams { + double lookahead_distance_h{}; + double lookahead_distance_v{}; + double k_p_h{}; + double k_p_v{}; + double time_step{}; +}; + +class ProportionalLOSGuidance { + public: + ProportionalLOSGuidance(const ProportionalLosParams& params); + ~ProportionalLOSGuidance() = default; + + types::Outputs calculate_outputs(const types::Inputs& inputs); + + private: + void update_angles(const types::Inputs& inputs); + types::CrossTrackError calculate_crosstrack_error( + const types::Inputs& inputs) const; + + ProportionalLosParams m_params{}; + // again i dont know if i should have them here or just in the functions + double pi_h_{0.0}; + double pi_v_{0.0}; + Eigen::AngleAxisd rotation_y_{0.0, Eigen::Vector3d::UnitY()}; + Eigen::AngleAxisd rotation_z_{0.0, Eigen::Vector3d::UnitZ()}; +}; + +} // namespace vortex::guidance::los #endif // PROPORTIONAL_LOS_GUIDANCE_HPP diff --git a/guidance/los_guidance/include/los_guidance/lib/types.hpp b/guidance/los_guidance/include/los_guidance/lib/types.hpp old mode 100755 new mode 100644 index d20f873f6..5207855cc --- a/guidance/los_guidance/include/los_guidance/lib/types.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/types.hpp @@ -1,60 +1,57 @@ #ifndef TYPES_HPP #define TYPES_HPP +#include #include #include #include #include -#include - -namespace vortex::guidance::los::types{ - - struct Point { - double x{}; - double y{}; - double z{}; - Point operator-(const Point& other) const { - return Point{x - other.x, y - other.y, z - other.z}; - } +namespace vortex::guidance::los::types { - Eigen::Vector3d as_vector() const { return Eigen::Vector3d(x, y, z); } +struct Point { + double x{}; + double y{}; + double z{}; - static Point point_from_ros( - const geometry_msgs::msg::Point& msg) { - return Point{msg.x, msg.y, msg.z}; - } + Point operator-(const Point& other) const { + return Point{x - other.x, y - other.y, z - other.z}; + } + Eigen::Vector3d as_vector() const { return Eigen::Vector3d(x, y, z); } - }; + static Point point_from_ros(const geometry_msgs::msg::Point& msg) { + return Point{msg.x, msg.y, msg.z}; + } +}; - struct CrossTrackError { - double x_e{}; - double y_e{}; - double z_e{}; +struct CrossTrackError { + double x_e{}; + double y_e{}; + double z_e{}; - inline static CrossTrackError from_vector(const Eigen::Vector3d& vector) { - return CrossTrackError{vector.x(), vector.y(), vector.z()}; - } - }; + inline static CrossTrackError from_vector(const Eigen::Vector3d& vector) { + return CrossTrackError{vector.x(), vector.y(), vector.z()}; + } +}; - struct Outputs { - double psi_d{}; - double theta_d{}; - }; +struct Outputs { + double psi_d{}; + double theta_d{}; +}; - struct Inputs{ - Point prev_point{}; - Point next_point{}; - Point current_position{}; - }; +struct Inputs { + Point prev_point{}; + Point next_point{}; + Point current_position{}; +}; - enum class ActiveLosMethod { - PROPORTIONAL, // 0 - INTEGRAL, // 1 - ADAPTIVE // 2 - }; +enum class ActiveLosMethod { + PROPORTIONAL, // 0 + INTEGRAL, // 1 + ADAPTIVE // 2 +}; -} // namespace vortex::guidance::los::types +} // namespace vortex::guidance::los::types -#endif \ No newline at end of file +#endif diff --git a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp index f3bdea8bd..eb53d461c 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -1,130 +1,130 @@ #ifndef LOS_GUIDANCE_ROS_HPP #define LOS_GUIDANCE_ROS_HPP +#include #include #include #include -#include "los_guidance/lib/adaptive_los.hpp" -#include "los_guidance/lib/integral_los.hpp" -#include "los_guidance/lib/proportional_los.hpp" -#include "los_guidance/lib/types.hpp" #include #include #include -#include -#include +#include #include -#include +#include +#include "los_guidance/lib/adaptive_los.hpp" +#include "los_guidance/lib/integral_los.hpp" +#include "los_guidance/lib/proportional_los.hpp" +#include "los_guidance/lib/types.hpp" namespace vortex::guidance::los { - class LosGuidanceNode : public rclcpp::Node { - public: - explicit LosGuidanceNode(); +class LosGuidanceNode : public rclcpp::Node { + public: + explicit LosGuidanceNode(); - private: - // @brief Set the subscribers and publishers - void set_subscribers_and_publisher(); + private: + // @brief Set the subscribers and publishers + void set_subscribers_and_publisher(); - // @brief Set the action server - void set_action_server(); + // @brief Set the action server + void set_action_server(); - // @brief Determine the LOS mode service - void set_service_server(); + // @brief Determine the LOS mode service + void set_service_server(); - // @brief Set the adaptive LOS guidance parameters - void set_adaptive_los_guidance(YAML::Node config); + // @brief Set the adaptive LOS guidance parameters + void set_adaptive_los_guidance(YAML::Node config); - // @brief Set the proportional LOS guidance parameters - void set_proportional_los_guidance(YAML::Node config); + // @brief Set the proportional LOS guidance parameters + void set_proportional_los_guidance(YAML::Node config); - // @brief Set the integral LOS guidance parameters - void set_integral_los_guidance(YAML::Node config); + // @brief Set the integral LOS guidance parameters + void set_integral_los_guidance(YAML::Node config); - // @brief Callback for the waypoint topic - // @param msg The reference message - void waypoint_callback( - const geometry_msgs::msg::PointStamped::SharedPtr msg); + // @brief Callback for the waypoint topic + // @param msg The reference message + void waypoint_callback( + const geometry_msgs::msg::PointStamped::SharedPtr msg); - // @brief Callback for the pose topic - // @param msg The pose message - void pose_callback( - const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); + // @brief Callback for the pose topic + // @param msg The pose message + void pose_callback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); - // @brief Handle the goal request - // @param uuid The goal UUID - // @param goal The goal message - // @return The goal response - rclcpp_action::GoalResponse handle_goal( - const rclcpp_action::GoalUUID& uuid, - std::shared_ptr goal); + // @brief Handle the goal request + // @param uuid The goal UUID + // @param goal The goal message + // @return The goal response + rclcpp_action::GoalResponse handle_goal( + const rclcpp_action::GoalUUID& uuid, + std::shared_ptr goal); - // @brief Handle the cancel request - // @param goal_handle The goal handle - // @return The cancel response - rclcpp_action::CancelResponse handle_cancel( - const std::shared_ptr< - rclcpp_action::ServerGoalHandle> - goal_handle); + // @brief Handle the cancel request + // @param goal_handle The goal handle + // @return The cancel response + rclcpp_action::CancelResponse handle_cancel( + const std::shared_ptr< + rclcpp_action::ServerGoalHandle> + goal_handle); - // @brief Handle the accepted request - // @param goal_handle The goal handle - void handle_accepted(const std::shared_ptr> goal_handle); + // @brief Handle the accepted request + // @param goal_handle The goal handle + void handle_accepted(const std::shared_ptr> goal_handle); - // @brief Execute the goal - // @param goal_handle The goal handle - void execute(const std::shared_ptr> goal_handle); + // @brief Execute the goal + // @param goal_handle The goal handle + void execute(const std::shared_ptr> goal_handle); - void set_los_mode( - const std::shared_ptr request, - std::shared_ptr response); + void set_los_mode( + const std::shared_ptr request, + std::shared_ptr response); - vortex_msgs::msg::LOSGuidance fill_los_reference(types::Outputs output); + vortex_msgs::msg::LOSGuidance fill_los_reference(types::Outputs output); - YAML::Node get_los_config(std::string yaml_file_path); + YAML::Node get_los_config(std::string yaml_file_path); - void parse_common_config(YAML::Node common_config); + void parse_common_config(YAML::Node common_config); - rclcpp_action::Server::SharedPtr - action_server_; + rclcpp_action::Server::SharedPtr + action_server_; - rclcpp::Service::SharedPtr los_mode_service_; + rclcpp::Service::SharedPtr los_mode_service_; - rclcpp::Publisher::SharedPtr reference_pub_; + rclcpp::Publisher::SharedPtr reference_pub_; - rclcpp::Subscription::SharedPtr - waypoint_sub_; + rclcpp::Subscription::SharedPtr + waypoint_sub_; - rclcpp::Subscription< - geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_; + rclcpp::Subscription< + geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_; - rclcpp::TimerBase::SharedPtr reference_pub_timer_; + rclcpp::TimerBase::SharedPtr reference_pub_timer_; - std::chrono::milliseconds time_step_; + std::chrono::milliseconds time_step_; - std::mutex mutex_; + std::mutex mutex_; - rclcpp_action::GoalUUID preempted_goal_id_; + rclcpp_action::GoalUUID preempted_goal_id_; - std::shared_ptr< - rclcpp_action::ServerGoalHandle> - goal_handle_; + std::shared_ptr< + rclcpp_action::ServerGoalHandle> + goal_handle_; - rclcpp::CallbackGroup::SharedPtr cb_group_; + rclcpp::CallbackGroup::SharedPtr cb_group_; - types::Inputs path_inputs_{}; + types::Inputs path_inputs_{}; - double u_desired_{}; + double u_desired_{}; - double goal_reached_tol_{}; + double goal_reached_tol_{}; - std::unique_ptr m_adaptive_los{}; - std::unique_ptr m_integral_los{}; - std::unique_ptr m_proportional_los{}; - types::ActiveLosMethod m_method{}; - }; + std::unique_ptr m_adaptive_los{}; + std::unique_ptr m_integral_los{}; + std::unique_ptr m_proportional_los{}; + types::ActiveLosMethod m_method{}; +}; } // namespace vortex::guidance::los diff --git a/guidance/los_guidance/launch/los_guidance.launch.py b/guidance/los_guidance/launch/los_guidance.launch.py index a9f0b628b..859c0a255 100644 --- a/guidance/los_guidance/launch/los_guidance.launch.py +++ b/guidance/los_guidance/launch/los_guidance.launch.py @@ -32,5 +32,6 @@ def generate_launch_description(): ) return LaunchDescription([los_guidance_node]) -# remeber to make them able to swich in the middle of a mission and if you swirch methid the parameters should'nt be reloaded -# unless its a new section \ No newline at end of file + +# remember to make them able to swich in the middle of a mission and if you swirch method the parameters shouldn't be reloaded +# unless its a new section diff --git a/guidance/los_guidance/src/lib/adaptive_los.cpp b/guidance/los_guidance/src/lib/adaptive_los.cpp index 67f282fd4..319ce376c 100644 --- a/guidance/los_guidance/src/lib/adaptive_los.cpp +++ b/guidance/los_guidance/src/lib/adaptive_los.cpp @@ -1,57 +1,64 @@ -#include "los_guidance/lib/types.hpp" #include +#include "los_guidance/lib/types.hpp" namespace vortex::guidance::los { - - AdaptiveLOSGuidance::AdaptiveLOSGuidance(const AdaptiveLosParams& params): m_params{params} {} - void AdaptiveLOSGuidance::update_angles(const types::Inputs& inputs) { - const double dx = inputs.next_point.x - inputs.prev_point.x; - const double dy = inputs.next_point.y - inputs.prev_point.y; - const double dz = inputs.next_point.z - inputs.prev_point.z; +AdaptiveLOSGuidance::AdaptiveLOSGuidance(const AdaptiveLosParams& params) + : m_params{params} {} + +void AdaptiveLOSGuidance::update_angles(const types::Inputs& inputs) { + const double dx = inputs.next_point.x - inputs.prev_point.x; + const double dy = inputs.next_point.y - inputs.prev_point.y; + const double dz = inputs.next_point.z - inputs.prev_point.z; - pi_h_ = std::atan2(dy, dx); - pi_v_ = std::atan2(-dz, std::sqrt(dx*dx + dy*dy)); + pi_h_ = std::atan2(dy, dx); + pi_v_ = std::atan2(-dz, std::sqrt(dx * dx + dy * dy)); - rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); - rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); - } + rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); + rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); +} - const types::CrossTrackError AdaptiveLOSGuidance::calculate_crosstrack_error(const types::Inputs& inputs) { +const types::CrossTrackError AdaptiveLOSGuidance::calculate_crosstrack_error( + const types::Inputs& inputs) { + const types::Point difference = inputs.current_position - inputs.prev_point; + const Eigen::Vector3d difference_vector = difference.as_vector(); - const types::Point difference = inputs.current_position - inputs.prev_point; - const Eigen::Vector3d difference_vector = difference.as_vector(); + const Eigen::Vector3d cross_track_error = + rotation_y_.transpose() * rotation_z_.transpose() * difference_vector; - const Eigen::Vector3d cross_track_error = - rotation_y_.transpose() * rotation_z_.transpose() * difference_vector; + return types::CrossTrackError::from_vector(cross_track_error); +} - return types::CrossTrackError::from_vector(cross_track_error); - } +void AdaptiveLOSGuidance::update_adaptive_estimates( + const types::CrossTrackError& e) { + const double denom_h = std::sqrt(m_params.lookahead_distance_h * + m_params.lookahead_distance_h + + e.y_e * e.y_e); + const double denom_v = std::sqrt(m_params.lookahead_distance_v * + m_params.lookahead_distance_v + + e.z_e * e.z_e); - void AdaptiveLOSGuidance::update_adaptive_estimates(const types::CrossTrackError& e) { - const double denom_h = std::sqrt( - m_params.lookahead_distance_h * m_params.lookahead_distance_h + e.y_e * e.y_e); - const double denom_v = std::sqrt( - m_params.lookahead_distance_v * m_params.lookahead_distance_v + e.z_e * e.z_e); + const double beta_dot = + m_params.gamma_h * (m_params.lookahead_distance_h / denom_h) * e.y_e; + const double alpha_dot = + m_params.gamma_v * (m_params.lookahead_distance_v / denom_v) * e.z_e; - const double beta_dot = m_params.gamma_h * (m_params.lookahead_distance_h / denom_h) * e.y_e; - const double alpha_dot = m_params.gamma_v * (m_params.lookahead_distance_v / denom_v) * e.z_e; + beta_c_hat_ += beta_dot * m_params.time_step; + alpha_c_hat_ += alpha_dot * m_params.time_step; +} - beta_c_hat_ += beta_dot * m_params.time_step; - alpha_c_hat_ += alpha_dot * m_params.time_step; - } - - types::Outputs AdaptiveLOSGuidance::calculate_outputs(const types::Inputs& inputs) { +types::Outputs AdaptiveLOSGuidance::calculate_outputs( + const types::Inputs& inputs) { + update_angles(inputs); + const types::CrossTrackError e = calculate_crosstrack_error(inputs); + update_adaptive_estimates(e); - update_angles(inputs); - const types::CrossTrackError e = calculate_crosstrack_error(inputs); - update_adaptive_estimates(e); + const double psi_d = + pi_h_ - beta_c_hat_ - std::atan(e.y_e / m_params.lookahead_distance_h); + const double theta_d = + pi_v_ + alpha_c_hat_ + std::atan(e.z_e / m_params.lookahead_distance_v); - const double psi_d = pi_h_ - beta_c_hat_ - std::atan(e.y_e / m_params.lookahead_distance_h); - const double theta_d = pi_v_ + alpha_c_hat_ + std::atan(e.z_e / m_params.lookahead_distance_v); - - return types::Outputs{psi_d, theta_d}; - } + return types::Outputs{psi_d, theta_d}; +} - -} // namespace vortex::guidance +} // namespace vortex::guidance::los diff --git a/guidance/los_guidance/src/lib/integral_los.cpp b/guidance/los_guidance/src/lib/integral_los.cpp old mode 100755 new mode 100644 index 2a088d6b0..0ca1a3e2b --- a/guidance/los_guidance/src/lib/integral_los.cpp +++ b/guidance/los_guidance/src/lib/integral_los.cpp @@ -1,41 +1,47 @@ - #include +#include namespace vortex::guidance::los { - IntegralLOSGuidance::IntegralLOSGuidance(const IntegralLosParams& params): m_params{params} {} +IntegralLOSGuidance::IntegralLOSGuidance(const IntegralLosParams& params) + : m_params{params} {} - void IntegralLOSGuidance::update_angles(const types::Inputs& inputs) { - const types::Point difference = inputs.next_point - inputs.prev_point; +void IntegralLOSGuidance::update_angles(const types::Inputs& inputs) { + const types::Point difference = inputs.next_point - inputs.prev_point; - pi_h_ = std::atan2(difference.y, difference.x); - pi_v_ = std::atan2(-difference.z, std::sqrt(difference.x * difference.x + difference.y * difference.y)); + pi_h_ = std::atan2(difference.y, difference.x); + pi_v_ = std::atan2(-difference.z, std::sqrt(difference.x * difference.x + + difference.y * difference.y)); - rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); - rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); - } + rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); + rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); +} - types::CrossTrackError IntegralLOSGuidance::calculate_crosstrack_error(const types::Inputs& inputs) { +types::CrossTrackError IntegralLOSGuidance::calculate_crosstrack_error( + const types::Inputs& inputs) { + const Eigen::Vector3d diff_vec = + (inputs.current_position - inputs.prev_point).as_vector(); + const Eigen::Vector3d e_perp = rotation_y_.toRotationMatrix().transpose() * + rotation_z_.toRotationMatrix().transpose() * + diff_vec; - const Eigen::Vector3d diff_vec = (inputs.current_position - inputs.prev_point).as_vector(); - const Eigen::Vector3d e_perp = rotation_y_.toRotationMatrix().transpose() * rotation_z_.toRotationMatrix().transpose() * diff_vec; + return types::CrossTrackError::from_vector(e_perp); +} - return types::CrossTrackError::from_vector(e_perp); - } +types::Outputs IntegralLOSGuidance::calculate_outputs( + const types::Inputs& inputs) { + update_angles(inputs); + const types::CrossTrackError e = calculate_crosstrack_error(inputs); - types::Outputs IntegralLOSGuidance::calculate_outputs(const types::Inputs& inputs) { - update_angles(inputs); - const types::CrossTrackError e = calculate_crosstrack_error(inputs); + int_h += e.y_e * m_params.time_step; + int_v += e.z_e * m_params.time_step; - int_h += e.y_e * m_params.time_step; - int_v += e.z_e * m_params.time_step; + const double u_h = m_params.k_p_h * e.y_e + m_params.k_i_h * int_h; + const double u_v = m_params.k_p_v * e.z_e + m_params.k_i_v * int_v; - const double u_h = m_params.k_p_h * e.y_e + m_params.k_i_h * int_h; - const double u_v = m_params.k_p_v * e.z_e + m_params.k_i_v * int_v; + const double psi_d = pi_h_ - std::atan(u_h); + const double theta_d = pi_v_ + std::atan(u_v); - const double psi_d = pi_h_ - std::atan(u_h); - const double theta_d = pi_v_ + std::atan(u_v); + return types::Outputs{psi_d, theta_d}; +} - return types::Outputs{psi_d, theta_d}; - } - -} // namespace vortex::guidance::los \ No newline at end of file +} // namespace vortex::guidance::los diff --git a/guidance/los_guidance/src/lib/proportional_los.cpp b/guidance/los_guidance/src/lib/proportional_los.cpp old mode 100755 new mode 100644 index f50dc81bf..40a739e04 --- a/guidance/los_guidance/src/lib/proportional_los.cpp +++ b/guidance/los_guidance/src/lib/proportional_los.cpp @@ -1,38 +1,45 @@ - #include +#include namespace vortex::guidance::los { - ProportionalLOSGuidance::ProportionalLOSGuidance(const ProportionalLosParams& params) : m_params{params} {} +ProportionalLOSGuidance::ProportionalLOSGuidance( + const ProportionalLosParams& params) + : m_params{params} {} - void ProportionalLOSGuidance::update_angles(const types::Inputs& inputs) { - const types::Point difference = inputs.next_point - inputs.prev_point; +void ProportionalLOSGuidance::update_angles(const types::Inputs& inputs) { + const types::Point difference = inputs.next_point - inputs.prev_point; - pi_h_ = std::atan2(difference.y, difference.x); - pi_v_ = std::atan2(-difference.z, std::sqrt(difference.x * difference.x + difference.y * difference.y)); + pi_h_ = std::atan2(difference.y, difference.x); + pi_v_ = std::atan2(-difference.z, std::sqrt(difference.x * difference.x + + difference.y * difference.y)); - rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); - rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); - } + rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); + rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); +} - types::CrossTrackError ProportionalLOSGuidance::calculate_crosstrack_error(const types::Inputs& inputs) const { +types::CrossTrackError ProportionalLOSGuidance::calculate_crosstrack_error( + const types::Inputs& inputs) const { + const Eigen::Vector3d diff_vec = + (inputs.current_position - inputs.prev_point).as_vector(); + const Eigen::Vector3d e_perp = rotation_y_.toRotationMatrix().transpose() * + rotation_z_.toRotationMatrix().transpose() * + diff_vec; - const Eigen::Vector3d diff_vec = (inputs.current_position - inputs.prev_point).as_vector(); - const Eigen::Vector3d e_perp = rotation_y_.toRotationMatrix().transpose() * rotation_z_.toRotationMatrix().transpose() * diff_vec; + return types::CrossTrackError::from_vector(e_perp); +} - return types::CrossTrackError::from_vector(e_perp); - } +types::Outputs ProportionalLOSGuidance::calculate_outputs( + const types::Inputs& inputs) { + update_angles(inputs); + const types::CrossTrackError e = calculate_crosstrack_error(inputs); - types::Outputs ProportionalLOSGuidance::calculate_outputs(const types::Inputs& inputs) { - update_angles(inputs); - const types::CrossTrackError e = calculate_crosstrack_error(inputs); + const double k_p_h = 1.0 / std::max(m_params.lookahead_distance_h, 1e-9); + const double k_p_v = 1.0 / std::max(m_params.lookahead_distance_v, 1e-9); - const double k_p_h = 1.0 / std::max(m_params.lookahead_distance_h, 1e-9); - const double k_p_v = 1.0 / std::max(m_params.lookahead_distance_v, 1e-9); + const double psi_d = pi_h_ - std::atan(k_p_h * e.y_e); + const double theta_d = pi_v_ + std::atan(k_p_v * e.z_e); - const double psi_d = pi_h_ - std::atan(k_p_h * e.y_e); - const double theta_d = pi_v_ + std::atan(k_p_v * e.z_e); + return types::Outputs{psi_d, theta_d}; +} - return types::Outputs{psi_d, theta_d}; - } - -} // namespace vortex::guidance::los \ No newline at end of file +} // namespace vortex::guidance::los diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 639325328..d365510a3 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -1,9 +1,9 @@ #include "los_guidance/los_guidance_ros.hpp" -#include "los_guidance/lib/types.hpp" #include +#include #include #include -#include +#include "los_guidance/lib/types.hpp" const auto start_message = R"( _ ___ ____ ____ _ _ @@ -14,285 +14,308 @@ const auto start_message = R"( )"; -namespace vortex::guidance::los{ - - LosGuidanceNode::LosGuidanceNode() : Node("los_guidance_node"){ - double time_step_s = this->declare_parameter("time_step"); - time_step_ = std::chrono::milliseconds(static_cast(time_step_s * 1000)); - //auto config = this->declare_parameter("los_config_file"); - - const std::string yaml_path =this->declare_parameter("los_config_file"); - - - YAML::Node config = get_los_config(yaml_path); - - parse_common_config(config["common"]); - set_subscribers_and_publisher(); - set_action_server(); - set_service_server(); - set_adaptive_los_guidance(config); - set_proportional_los_guidance(config); - set_integral_los_guidance(config); - - spdlog::info(start_message); - } - - void LosGuidanceNode::set_subscribers_and_publisher() { - - this->declare_parameter("topics.pose"); - this->declare_parameter("topics.guidance.los"); - this->declare_parameter("topics.waypoint"); - - std::string pose_topic = this->get_parameter("topics.pose").as_string(); - std::string guidance_topic = - this->get_parameter("topics.guidance.los").as_string(); - std::string waypoint_topic = - this->get_parameter("topics.waypoint").as_string(); - - auto qos_sensor_data = vortex::utils::qos_profiles::sensor_data_profile(1); - - reference_pub_ = this->create_publisher( - guidance_topic, qos_sensor_data); - - waypoint_sub_ = this->create_subscription( - waypoint_topic, qos_sensor_data, - std::bind(&LosGuidanceNode::waypoint_callback, this, - std::placeholders::_1)); - - pose_sub_ = this->create_subscription< - geometry_msgs::msg::PoseWithCovarianceStamped>( - pose_topic, qos_sensor_data, - std::bind(&LosGuidanceNode::pose_callback, this, - std::placeholders::_1)); - } - - void LosGuidanceNode::set_action_server() { - this->declare_parameter("action_servers.los"); - std::string action_server_name = - this->get_parameter("action_servers.los").as_string(); - cb_group_ = - this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); - - action_server_ = - rclcpp_action::create_server( - this, action_server_name, - std::bind(&LosGuidanceNode::handle_goal, this, - std::placeholders::_1, std::placeholders::_2), - std::bind(&LosGuidanceNode::handle_cancel, this, - std::placeholders::_1), - std::bind(&LosGuidanceNode::handle_accepted, this, - std::placeholders::_1), - rcl_action_server_get_default_options(), cb_group_); - } - - void LosGuidanceNode::set_service_server(){ - this->declare_parameter("services.los_mode", "set_los_mode"); - std::string service_name = - this->get_parameter("services.los_mode").as_string(); - - los_mode_service_ = this->create_service( - service_name, - std::bind(&LosGuidanceNode::set_los_mode, this, - std::placeholders::_1, std::placeholders::_2)); - - } - - void LosGuidanceNode::set_adaptive_los_guidance(YAML::Node config) { - auto adaptive_los_config = config["adaptive_los"]; spdlog::info("A0"); - auto params = AdaptiveLosParams{}; - params.lookahead_distance_h = - adaptive_los_config["lookahead_distance_h"].as(); spdlog::info("A1"); - params.lookahead_distance_v = - adaptive_los_config["lookahead_distance_v"].as();spdlog::info("A2"); - params.gamma_h = - adaptive_los_config["gamma_h"].as();spdlog::info("A3"); - params.gamma_v = - adaptive_los_config["gamma_v"].as();spdlog::info("A4"); - params.time_step = static_cast(time_step_.count()) / 1000.0;spdlog::info("A5"); - - m_adaptive_los = std::make_unique(params); - } - - void LosGuidanceNode::set_proportional_los_guidance(YAML::Node config) { - auto proportional_los_config = config["prop_los"];spdlog::info("P0"); - auto params = ProportionalLosParams{}; - params.lookahead_distance_h = - proportional_los_config["lookahead_distance_h"].as();spdlog::info("P1"); - params.lookahead_distance_v = - proportional_los_config["lookahead_distance_v"].as();spdlog::info("P2"); - params.k_p_h = proportional_los_config["k_p_h"].as();spdlog::info("P3"); - params.k_p_v = proportional_los_config["k_p_v"].as();spdlog::info("P4"); - params.time_step = static_cast(time_step_.count()) / 1000.0; spdlog::info("P5"); - - m_proportional_los = std::make_unique(params); - } - - void LosGuidanceNode::set_integral_los_guidance(YAML::Node config) { - auto integral_los_config = config["integer_los"];spdlog::info("I0"); - auto params = IntegralLosParams{}; - params.lookahead_distance_h = - integral_los_config["lookahead_distance_h"].as();spdlog::info("I1"); - params.lookahead_distance_v = - integral_los_config["lookahead_distance_v"].as();spdlog::info("I2"); - params.k_p_h = integral_los_config["k_p_h"].as();spdlog::info("I3"); - params.k_p_v = integral_los_config["k_p_v"].as();spdlog::info("I4"); - params.k_i_h = integral_los_config["k_i_h"].as();spdlog::info("I5"); - params.k_i_v = integral_los_config["k_i_v"].as();spdlog::info("I6"); - params.time_step = static_cast(time_step_.count()) / 1000.0;spdlog::info("I7"); - - m_integral_los = std::make_unique(params); - } - - void LosGuidanceNode::waypoint_callback( - const geometry_msgs::msg::PointStamped::SharedPtr los_waypoint) { - - path_inputs_.prev_point = path_inputs_.current_position; - path_inputs_.next_point = types::Point::point_from_ros(los_waypoint->point); - spdlog::info("Received waypoint"); // remember to print waypoint that you get - } - - void LosGuidanceNode::pose_callback( - const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr - current_pose) { - - path_inputs_.current_position = types::Point::point_from_ros(current_pose->pose.pose.position); - - } - - rclcpp_action::GoalResponse LosGuidanceNode::handle_goal( - const rclcpp_action::GoalUUID&, - std::shared_ptr goal) { - (void)goal; - { - std::lock_guard lock(mutex_); - if (goal_handle_) { - if (goal_handle_->is_active()) { - spdlog::info("Aborting current goal and accepting new goal"); - preempted_goal_id_ = goal_handle_->get_goal_id(); - } +namespace vortex::guidance::los { + +LosGuidanceNode::LosGuidanceNode() : Node("los_guidance_node") { + double time_step_s = this->declare_parameter("time_step"); + time_step_ = + std::chrono::milliseconds(static_cast(time_step_s * 1000)); + // auto config = this->declare_parameter("los_config_file"); + + const std::string yaml_path = + this->declare_parameter("los_config_file"); + + YAML::Node config = get_los_config(yaml_path); + + parse_common_config(config["common"]); + set_subscribers_and_publisher(); + set_action_server(); + set_service_server(); + set_adaptive_los_guidance(config); + set_proportional_los_guidance(config); + set_integral_los_guidance(config); + + spdlog::info(start_message); +} + +void LosGuidanceNode::set_subscribers_and_publisher() { + this->declare_parameter("topics.pose"); + this->declare_parameter("topics.guidance.los"); + this->declare_parameter("topics.waypoint"); + + std::string pose_topic = this->get_parameter("topics.pose").as_string(); + std::string guidance_topic = + this->get_parameter("topics.guidance.los").as_string(); + std::string waypoint_topic = + this->get_parameter("topics.waypoint").as_string(); + + auto qos_sensor_data = vortex::utils::qos_profiles::sensor_data_profile(1); + + reference_pub_ = this->create_publisher( + guidance_topic, qos_sensor_data); + + waypoint_sub_ = this->create_subscription( + waypoint_topic, qos_sensor_data, + std::bind(&LosGuidanceNode::waypoint_callback, this, + std::placeholders::_1)); + + pose_sub_ = this->create_subscription< + geometry_msgs::msg::PoseWithCovarianceStamped>( + pose_topic, qos_sensor_data, + std::bind(&LosGuidanceNode::pose_callback, this, + std::placeholders::_1)); +} + +void LosGuidanceNode::set_action_server() { + this->declare_parameter("action_servers.los"); + std::string action_server_name = + this->get_parameter("action_servers.los").as_string(); + cb_group_ = + this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + + action_server_ = + rclcpp_action::create_server( + this, action_server_name, + std::bind(&LosGuidanceNode::handle_goal, this, + std::placeholders::_1, std::placeholders::_2), + std::bind(&LosGuidanceNode::handle_cancel, this, + std::placeholders::_1), + std::bind(&LosGuidanceNode::handle_accepted, this, + std::placeholders::_1), + rcl_action_server_get_default_options(), cb_group_); +} + +void LosGuidanceNode::set_service_server() { + this->declare_parameter("services.los_mode", "set_los_mode"); + std::string service_name = + this->get_parameter("services.los_mode").as_string(); + + los_mode_service_ = this->create_service( + service_name, std::bind(&LosGuidanceNode::set_los_mode, this, + std::placeholders::_1, std::placeholders::_2)); +} + +void LosGuidanceNode::set_adaptive_los_guidance(YAML::Node config) { + auto adaptive_los_config = config["adaptive_los"]; + spdlog::info("A0"); + auto params = AdaptiveLosParams{}; + params.lookahead_distance_h = + adaptive_los_config["lookahead_distance_h"].as(); + spdlog::info("A1"); + params.lookahead_distance_v = + adaptive_los_config["lookahead_distance_v"].as(); + spdlog::info("A2"); + params.gamma_h = adaptive_los_config["gamma_h"].as(); + spdlog::info("A3"); + params.gamma_v = adaptive_los_config["gamma_v"].as(); + spdlog::info("A4"); + params.time_step = static_cast(time_step_.count()) / 1000.0; + spdlog::info("A5"); + + m_adaptive_los = std::make_unique(params); +} + +void LosGuidanceNode::set_proportional_los_guidance(YAML::Node config) { + auto proportional_los_config = config["prop_los"]; + spdlog::info("P0"); + auto params = ProportionalLosParams{}; + params.lookahead_distance_h = + proportional_los_config["lookahead_distance_h"].as(); + spdlog::info("P1"); + params.lookahead_distance_v = + proportional_los_config["lookahead_distance_v"].as(); + spdlog::info("P2"); + params.k_p_h = proportional_los_config["k_p_h"].as(); + spdlog::info("P3"); + params.k_p_v = proportional_los_config["k_p_v"].as(); + spdlog::info("P4"); + params.time_step = static_cast(time_step_.count()) / 1000.0; + spdlog::info("P5"); + + m_proportional_los = std::make_unique(params); +} + +void LosGuidanceNode::set_integral_los_guidance(YAML::Node config) { + auto integral_los_config = config["integer_los"]; + spdlog::info("I0"); + auto params = IntegralLosParams{}; + params.lookahead_distance_h = + integral_los_config["lookahead_distance_h"].as(); + spdlog::info("I1"); + params.lookahead_distance_v = + integral_los_config["lookahead_distance_v"].as(); + spdlog::info("I2"); + params.k_p_h = integral_los_config["k_p_h"].as(); + spdlog::info("I3"); + params.k_p_v = integral_los_config["k_p_v"].as(); + spdlog::info("I4"); + params.k_i_h = integral_los_config["k_i_h"].as(); + spdlog::info("I5"); + params.k_i_v = integral_los_config["k_i_v"].as(); + spdlog::info("I6"); + params.time_step = static_cast(time_step_.count()) / 1000.0; + spdlog::info("I7"); + + m_integral_los = std::make_unique(params); +} + +void LosGuidanceNode::waypoint_callback( + const geometry_msgs::msg::PointStamped::SharedPtr los_waypoint) { + path_inputs_.prev_point = path_inputs_.current_position; + path_inputs_.next_point = types::Point::point_from_ros(los_waypoint->point); + spdlog::info( + "Received waypoint"); // remember to print waypoint that you get +} + +void LosGuidanceNode::pose_callback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr + current_pose) { + path_inputs_.current_position = + types::Point::point_from_ros(current_pose->pose.pose.position); +} + +rclcpp_action::GoalResponse LosGuidanceNode::handle_goal( + const rclcpp_action::GoalUUID&, + std::shared_ptr goal) { + (void)goal; + { + std::lock_guard lock(mutex_); + if (goal_handle_) { + if (goal_handle_->is_active()) { + spdlog::info("Aborting current goal and accepting new goal"); + preempted_goal_id_ = goal_handle_->get_goal_id(); } } - spdlog::info("Accepted goal request"); - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } - - rclcpp_action::CancelResponse LosGuidanceNode::handle_cancel( - const std::shared_ptr< - rclcpp_action::ServerGoalHandle> - goal_handle) { - spdlog::info("Received request to cancel goal"); - (void)goal_handle; - return rclcpp_action::CancelResponse::ACCEPT; + spdlog::info("Accepted goal request"); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse LosGuidanceNode::handle_cancel( + const std::shared_ptr< + rclcpp_action::ServerGoalHandle> + goal_handle) { + spdlog::info("Received request to cancel goal"); + (void)goal_handle; + return rclcpp_action::CancelResponse::ACCEPT; +} + +void LosGuidanceNode::handle_accepted( + const std::shared_ptr< + rclcpp_action::ServerGoalHandle> + goal_handle) { + execute(goal_handle); +} +void LosGuidanceNode::set_los_mode( + const std::shared_ptr request, + std::shared_ptr response) { + m_method = static_cast(request->mode); + spdlog::info("LOS mode set to {}", static_cast(m_method)); + response->success = true; +} +vortex_msgs::msg::LOSGuidance LosGuidanceNode::fill_los_reference( + types::Outputs outputs) { + vortex_msgs::msg::LOSGuidance reference_msg; + reference_msg.pitch = outputs.theta_d; + reference_msg.yaw = outputs.psi_d; + reference_msg.surge = u_desired_; + + return reference_msg; +} + +YAML::Node LosGuidanceNode::get_los_config(std::string yaml_file_path) { + YAML::Node config = YAML::LoadFile(yaml_file_path); + return config; +} + +void LosGuidanceNode::parse_common_config(YAML::Node common_config) { + u_desired_ = common_config["u_desired"].as(); + spdlog::info("C1"); + goal_reached_tol_ = common_config["goal_reached_tol"].as(); + spdlog::info("C2"); + m_method = static_cast( + common_config["active_los_method"].as()); + spdlog::info("C3"); +} + +void LosGuidanceNode::execute( + + const std::shared_ptr< + rclcpp_action::ServerGoalHandle> + goal_handle) { + { + std::lock_guard lock(mutex_); + this->goal_handle_ = goal_handle; } - void LosGuidanceNode::handle_accepted( - const std::shared_ptr< - rclcpp_action::ServerGoalHandle> - goal_handle) { - execute(goal_handle); - } - void LosGuidanceNode::set_los_mode( - const std::shared_ptr request, - std::shared_ptr response) { - - m_method = static_cast(request->mode); - spdlog::info("LOS mode set to {}", static_cast(m_method)); - response->success = true; - } - vortex_msgs::msg::LOSGuidance LosGuidanceNode::fill_los_reference(types::Outputs outputs) { - vortex_msgs::msg::LOSGuidance reference_msg; - reference_msg.pitch = outputs.theta_d; - reference_msg.yaw = outputs.psi_d; - reference_msg.surge = u_desired_; + spdlog::info("Executing goal"); - return reference_msg; - } - - YAML::Node LosGuidanceNode::get_los_config(std::string yaml_file_path) { - YAML::Node config = YAML::LoadFile(yaml_file_path); - return config; - } + const geometry_msgs::msg::PointStamped los_waypoint = + goal_handle->get_goal()->goal; - void LosGuidanceNode::parse_common_config(YAML::Node common_config) { - u_desired_ = common_config["u_desired"].as(); spdlog::info("C1"); - goal_reached_tol_ = common_config["goal_reached_tol"].as(); spdlog::info("C2"); - m_method = static_cast(common_config["active_los_method"].as()); spdlog::info("C3"); - } + path_inputs_.prev_point = path_inputs_.current_position; + path_inputs_.next_point = types::Point::point_from_ros(los_waypoint.point); - void LosGuidanceNode::execute( + auto feedback = + std::make_shared(); + auto result = std::make_shared(); + rclcpp::Rate loop_rate(1000.0 / time_step_.count()); - const std::shared_ptr< - rclcpp_action::ServerGoalHandle> - goal_handle) { + while (rclcpp::ok()) { { std::lock_guard lock(mutex_); - this->goal_handle_ = goal_handle; + if (goal_handle->get_goal_id() == preempted_goal_id_) { + result->success = false; + goal_handle->abort(result); + return; + } } - spdlog::info("Executing goal"); - - const geometry_msgs::msg::PointStamped los_waypoint = goal_handle->get_goal()->goal; - - path_inputs_.prev_point = path_inputs_.current_position; - path_inputs_.next_point = types::Point::point_from_ros(los_waypoint.point); - - auto feedback = std::make_shared(); - auto result = std::make_shared(); - - rclcpp::Rate loop_rate(1000.0 / time_step_.count()); - - while (rclcpp::ok()) { - { - std::lock_guard lock(mutex_); - if (goal_handle->get_goal_id() == preempted_goal_id_) { - result->success = false; - goal_handle->abort(result); - return; - } - } + if (goal_handle->is_canceling()) { + result->success = false; + goal_handle->canceled(result); + spdlog::info("Goal canceled"); + return; + } - if (goal_handle->is_canceling()) { + types::Outputs outputs; + + switch (m_method) { + case types::ActiveLosMethod::ADAPTIVE: + outputs = m_adaptive_los->calculate_outputs(path_inputs_); + break; + case types::ActiveLosMethod::PROPORTIONAL: + outputs = m_proportional_los->calculate_outputs(path_inputs_); + break; + case types::ActiveLosMethod::INTEGRAL: + outputs = m_integral_los->calculate_outputs(path_inputs_); + break; + default: + spdlog::error("Invalid LOS method selected"); result->success = false; - goal_handle->canceled(result); - spdlog::info("Goal canceled"); + goal_handle->abort(result); return; - } - - types::Outputs outputs; - - switch(m_method) { - case types::ActiveLosMethod::ADAPTIVE: - outputs = m_adaptive_los->calculate_outputs(path_inputs_); - break; - case types::ActiveLosMethod::PROPORTIONAL: - outputs = m_proportional_los->calculate_outputs(path_inputs_); - break; - case types::ActiveLosMethod::INTEGRAL: - outputs = m_integral_los->calculate_outputs(path_inputs_); - break; - default: - spdlog::error("Invalid LOS method selected"); - result->success = false; - goal_handle->abort(result); - return; - } + } - vortex_msgs::msg::LOSGuidance reference_msg = fill_los_reference(outputs); - feedback->feedback = reference_msg; + vortex_msgs::msg::LOSGuidance reference_msg = + fill_los_reference(outputs); + feedback->feedback = reference_msg; - goal_handle->publish_feedback(feedback); - reference_pub_->publish(reference_msg); + goal_handle->publish_feedback(feedback); + reference_pub_->publish(reference_msg); - if ((path_inputs_.current_position - path_inputs_.next_point).as_vector().norm() < goal_reached_tol_) { + if ((path_inputs_.current_position - path_inputs_.next_point) + .as_vector() + .norm() < goal_reached_tol_) { result->success = true; goal_handle->succeed(result); spdlog::info("Goal reached"); return; } - loop_rate.sleep(); - } - } + loop_rate.sleep(); + } +} -} // namespace vortex::guidance +} // namespace vortex::guidance::los diff --git a/guidance/los_guidance/test/CMakeLists.txt b/guidance/los_guidance/test/CMakeLists.txt index e1552c071..8f5842651 100644 --- a/guidance/los_guidance/test/CMakeLists.txt +++ b/guidance/los_guidance/test/CMakeLists.txt @@ -10,7 +10,7 @@ add_executable( adaptive_los_test.cpp proportional_los_test.cpp integral_los_test.cpp - + ) target_link_libraries(${TEST_BINARY_NAME} @@ -20,7 +20,7 @@ target_link_libraries(${TEST_BINARY_NAME} Eigen3::Eigen spdlog::spdlog fmt::fmt - GTest::GTest + GTest::GTest ) ament_target_dependencies(${TEST_BINARY_NAME} PUBLIC Eigen3) diff --git a/guidance/los_guidance/test/adaptive_los_test.cpp b/guidance/los_guidance/test/adaptive_los_test.cpp index 42b5800d8..5fd71027a 100644 --- a/guidance/los_guidance/test/adaptive_los_test.cpp +++ b/guidance/los_guidance/test/adaptive_los_test.cpp @@ -1,175 +1,173 @@ -#include #include "los_guidance/lib/adaptive_los.hpp" +#include -namespace vortex::guidance::los{ //new namespace added los - - class AdaptiveLosTest : public ::testing::Test { - protected: - AdaptiveLosTest() : los_{get_params()} {} - - AdaptiveLosParams get_params() { - AdaptiveLosParams p; - p.lookahead_distance_h = 1.0; - p.lookahead_distance_v = 1.0; - p.gamma_h = 1.0; - p.gamma_v = 1.0; - p.time_step = 0.01; - return p; - } - - AdaptiveLOSGuidance los_; - const double tol = 1e-9; - }; - /* - TEST_F(AdaptiveLosTest, T01_test_cross_track_error_on_track){ - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.0, 0.0}; - - const types::Output O = los_.calculate_outputs(inputs); - - EXPECT_NEAR(e.x_e, 0.0, tol); - EXPECT_NEAR(e.y_e, 0.0, tol); - EXPECT_NEAR(e.z_e, 0.0, tol); - } - - TEST_F(AdaptiveLosTests, T02_test_cross_track_error_right_off_track) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.5, 0.0}; - - const types::Output O = los_.calculate_outputs(inputs); - - EXPECT_NEAR(e.x_e, 0.0, tol); - EXPECT_NEAR(e.y_e, 0.5, tol); - EXPECT_NEAR(e.z_e, 0.0, tol); - } - - TEST_F(AdaptiveLosTests, T03_test_cross_track_error_left_off_track) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, -0.5, 0.0}; - - const types::Output O = los_.calculate_outputs(inputs); - - EXPECT_NEAR(e.x_e, 0.0, tol); - EXPECT_NEAR(e.y_e, -0.5, tol); - EXPECT_NEAR(e.z_e, 0.0, tol); - } - - TEST_F(AdaptiveLosTests, T04_test_cross_track_error_under_track) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.0, 0.5}; - - const types::Output O = los_.calculate_outputs(inputs); - - EXPECT_NEAR(e.x_e, 0.0, tol); - EXPECT_NEAR(e.y_e, 0.0, tol); - EXPECT_NEAR(e.z_e, 0.5, tol); - } - - TEST_F(AdaptiveLosTests, T05_test_cross_track_error_over_track) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.0, -0.5}; - - const types::Output O = los_.calculate_outputs(inputs); - - EXPECT_NEAR(e.x_e, 0.0, tol); - EXPECT_NEAR(e.y_e, 0.0, tol); - EXPECT_NEAR(e.z_e, -0.5, tol); - } - */ - - // Test commanded angles when drone is to the right of the track - TEST_F(AdaptiveLosTest, T06_test_commanded_angles) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.5, 0.0}; - - const types::Outputs O = los_.calculate_outputs(inputs); - - // Heading cmd should be between -pi/2 and 0 - EXPECT_LT(O.psi_d, 0.0); - EXPECT_GT(O.psi_d, -1.57); - - // Pitch cmd should be zero - EXPECT_NEAR(O.theta_d, 0.0, tol); - } - - // Test commanded angles when drone is to the left of the track - TEST_F(AdaptiveLosTest, T07_test_commanded_angles) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, -0.5, 0.0}; - - const types::Outputs O = los_.calculate_outputs(inputs); - - // Heading cmd should be between 0 and pi/2 - EXPECT_GT(O.psi_d, 0.0); - EXPECT_LT(O.psi_d, 1.57); - // Pitch cmd should be zero - EXPECT_NEAR(O.theta_d, 0.0, tol); - } - - // Test commanded angles when drone is under the track - TEST_F(AdaptiveLosTest, T08_test_commanded_angles) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.0, 0.5}; - - const types::Outputs O = los_.calculate_outputs(inputs); - - // Heading cmd should be 0 - EXPECT_NEAR(O.psi_d, 0.0, tol); - // Pitch cmd should be between 0 and pi/2 - EXPECT_GT(O.theta_d, 0.0); - EXPECT_LT(O.theta_d, 1.57); - } - - // Test commanded angles when drone is over the track - TEST_F(AdaptiveLosTest, T09_test_commanded_angles) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.0, -0.5}; +namespace vortex::guidance::los { // new namespace added los - const types::Outputs O = los_.calculate_outputs(inputs); +class AdaptiveLosTest : public ::testing::Test { + protected: + AdaptiveLosTest() : los_{get_params()} {} - // Heading cmd should be 0 - EXPECT_NEAR(O.psi_d, 0.0, tol); - // Pitch cmd should be between -pi/2 and 0 - EXPECT_LT(O.theta_d, 0.0); - EXPECT_GT(O.theta_d, -1.57); + AdaptiveLosParams get_params() { + AdaptiveLosParams p; + p.lookahead_distance_h = 1.0; + p.lookahead_distance_v = 1.0; + p.gamma_h = 1.0; + p.gamma_v = 1.0; + p.time_step = 0.01; + return p; } - // Test commanded angles when drone is over and to the right of the track - - TEST_F(AdaptiveLosTest, T10_test_commanded_angles) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.5, -0.5}; - - const types::Outputs O = los_.calculate_outputs(inputs); - - // Heading cmd should be between -pi/2 and 0 - EXPECT_LT(O.psi_d, 0.0); - EXPECT_GT(O.psi_d, -1.57); - // Pitch cmd should be between -pi/2 and 0 - EXPECT_LT(O.theta_d, 0.0); - EXPECT_GT(O.theta_d, -1.57); - } + AdaptiveLOSGuidance los_; + const double tol = 1e-9; +}; +/* +TEST_F(AdaptiveLosTest, T01_test_cross_track_error_on_track){ + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, 0.0}; + + const types::Output O = los_.calculate_outputs(inputs); + + EXPECT_NEAR(e.x_e, 0.0, tol); + EXPECT_NEAR(e.y_e, 0.0, tol); + EXPECT_NEAR(e.z_e, 0.0, tol); +} + +TEST_F(AdaptiveLosTests, T02_test_cross_track_error_right_off_track) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.5, 0.0}; + + const types::Output O = los_.calculate_outputs(inputs); + + EXPECT_NEAR(e.x_e, 0.0, tol); + EXPECT_NEAR(e.y_e, 0.5, tol); + EXPECT_NEAR(e.z_e, 0.0, tol); +} + +TEST_F(AdaptiveLosTests, T03_test_cross_track_error_left_off_track) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, -0.5, 0.0}; + + const types::Output O = los_.calculate_outputs(inputs); + + EXPECT_NEAR(e.x_e, 0.0, tol); + EXPECT_NEAR(e.y_e, -0.5, tol); + EXPECT_NEAR(e.z_e, 0.0, tol); +} + +TEST_F(AdaptiveLosTests, T04_test_cross_track_error_under_track) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, 0.5}; + + const types::Output O = los_.calculate_outputs(inputs); + + EXPECT_NEAR(e.x_e, 0.0, tol); + EXPECT_NEAR(e.y_e, 0.0, tol); + EXPECT_NEAR(e.z_e, 0.5, tol); +} + +TEST_F(AdaptiveLosTests, T05_test_cross_track_error_over_track) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, -0.5}; + + const types::Output O = los_.calculate_outputs(inputs); + + EXPECT_NEAR(e.x_e, 0.0, tol); + EXPECT_NEAR(e.y_e, 0.0, tol); + EXPECT_NEAR(e.z_e, -0.5, tol); +} +*/ + +// Test commanded angles when drone is to the right of the track +TEST_F(AdaptiveLosTest, T06_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.5, 0.0}; + + const types::Outputs O = los_.calculate_outputs(inputs); + + // Heading cmd should be between -pi/2 and 0 + EXPECT_LT(O.psi_d, 0.0); + EXPECT_GT(O.psi_d, -1.57); + + // Pitch cmd should be zero + EXPECT_NEAR(O.theta_d, 0.0, tol); +} + +// Test commanded angles when drone is to the left of the track +TEST_F(AdaptiveLosTest, T07_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, -0.5, 0.0}; + + const types::Outputs O = los_.calculate_outputs(inputs); + + // Heading cmd should be between 0 and pi/2 + EXPECT_GT(O.psi_d, 0.0); + EXPECT_LT(O.psi_d, 1.57); + // Pitch cmd should be zero + EXPECT_NEAR(O.theta_d, 0.0, tol); +} + +// Test commanded angles when drone is under the track +TEST_F(AdaptiveLosTest, T08_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, 0.5}; + + const types::Outputs O = los_.calculate_outputs(inputs); + + // Heading cmd should be 0 + EXPECT_NEAR(O.psi_d, 0.0, tol); + // Pitch cmd should be between 0 and pi/2 + EXPECT_GT(O.theta_d, 0.0); + EXPECT_LT(O.theta_d, 1.57); +} + +// Test commanded angles when drone is over the track +TEST_F(AdaptiveLosTest, T09_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, -0.5}; + + const types::Outputs O = los_.calculate_outputs(inputs); + + // Heading cmd should be 0 + EXPECT_NEAR(O.psi_d, 0.0, tol); + // Pitch cmd should be between -pi/2 and 0 + EXPECT_LT(O.theta_d, 0.0); + EXPECT_GT(O.theta_d, -1.57); +} + +// Test commanded angles when drone is over and to the right of the track + +TEST_F(AdaptiveLosTest, T10_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.5, -0.5}; + + const types::Outputs O = los_.calculate_outputs(inputs); + + // Heading cmd should be between -pi/2 and 0 + EXPECT_LT(O.psi_d, 0.0); + EXPECT_GT(O.psi_d, -1.57); + // Pitch cmd should be between -pi/2 and 0 + EXPECT_LT(O.theta_d, 0.0); + EXPECT_GT(O.theta_d, -1.57); +} } // namespace vortex::guidance::los - - diff --git a/guidance/los_guidance/test/integral_los_test.cpp b/guidance/los_guidance/test/integral_los_test.cpp old mode 100755 new mode 100644 index 218e818c1..f94e93df1 --- a/guidance/los_guidance/test/integral_los_test.cpp +++ b/guidance/los_guidance/test/integral_los_test.cpp @@ -1,110 +1,110 @@ +#include "los_guidance/lib/integral_los.hpp" #include #include "los_guidance/lib/adaptive_los.hpp" -#include "los_guidance/lib/integral_los.hpp" - -namespace vortex::guidance::los{ - - class IntegralLosTest : public ::testing::Test { - protected: - IntegralLosTest() : Ilos_{get_params()} {} - - IntegralLosParams get_params() { - IntegralLosParams params; - params.lookahead_distance_h = 1.0; - params.lookahead_distance_v = 1.0; - params.k_i_h = 0.1; // needs tuning - params.k_i_v = 0.1; // needs tuning - params.k_p_h = 0.667; // needs tuning - params.k_p_v = 0.582; // needs tuning - params.time_step = 0.01; - return params; - } - - IntegralLOSGuidance Ilos_; - const double tol = 1e-9; - }; - - // Test commanded angles when drone is to the right of the track - TEST_F(IntegralLosTest, T06_test_commanded_angles) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.5, 0.0}; - - const types::Outputs O = Ilos_.calculate_outputs(inputs); - - // Heading cmd should be between -pi/2 and 0 - EXPECT_LT(O.psi_d, 0.0); - EXPECT_GT(O.psi_d, -1.57); - - // Pitch cmd should be zero - EXPECT_NEAR(O.theta_d, 0.0, tol); - } - - // Test commanded angles when drone is to the left of the track - TEST_F(IntegralLosTest, T07_test_commanded_angles) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, -0.5, 0.0}; - - const types::Outputs O = Ilos_.calculate_outputs(inputs); - - // Heading cmd should be between 0 and pi/2 - EXPECT_GT(O.psi_d, 0.0); - EXPECT_LT(O.psi_d, 1.57); - // Pitch cmd should be zero - EXPECT_NEAR(O.theta_d, 0.0, tol); - } - - // Test commanded angles when drone is under the track - TEST_F(IntegralLosTest, T08_test_commanded_angles) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.0, 0.5}; - - const types::Outputs O = Ilos_.calculate_outputs(inputs); - - // Heading cmd should be 0 - EXPECT_NEAR(O.psi_d, 0.0, tol); - // Pitch cmd should be between 0 and pi/2 - EXPECT_GT(O.theta_d, 0.0); - EXPECT_LT(O.theta_d, 1.57); - } - - // Test commanded angles when drone is over the track - TEST_F(IntegralLosTest, T09_test_commanded_angles) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.0, -0.5}; - - const types::Outputs O = Ilos_.calculate_outputs(inputs); - - // Heading cmd should be 0 - EXPECT_NEAR(O.psi_d, 0.0, tol); - // Pitch cmd should be between -pi/2 and 0 - EXPECT_LT(O.theta_d, 0.0); - EXPECT_GT(O.theta_d, -1.57); - } - - // Test commanded angles when drone is over and to the right of the track - - TEST_F(IntegralLosTest, T10_test_commanded_angles) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.5, -0.5}; - - const types::Outputs O = Ilos_.calculate_outputs(inputs); - // Heading cmd should be between -pi/2 and 0 - EXPECT_LT(O.psi_d, 0.0); - EXPECT_GT(O.psi_d, -1.57); - // Pitch cmd should be between -pi/2 and 0 - EXPECT_LT(O.theta_d, 0.0); - EXPECT_GT(O.theta_d, -1.57); +namespace vortex::guidance::los { + +class IntegralLosTest : public ::testing::Test { + protected: + IntegralLosTest() : Ilos_{get_params()} {} + + IntegralLosParams get_params() { + IntegralLosParams params; + params.lookahead_distance_h = 1.0; + params.lookahead_distance_v = 1.0; + params.k_i_h = 0.1; // needs tuning + params.k_i_v = 0.1; // needs tuning + params.k_p_h = 0.667; // needs tuning + params.k_p_v = 0.582; // needs tuning + params.time_step = 0.01; + return params; } -} // namespace vortex::guidance + IntegralLOSGuidance Ilos_; + const double tol = 1e-9; +}; + +// Test commanded angles when drone is to the right of the track +TEST_F(IntegralLosTest, T06_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.5, 0.0}; + + const types::Outputs O = Ilos_.calculate_outputs(inputs); + + // Heading cmd should be between -pi/2 and 0 + EXPECT_LT(O.psi_d, 0.0); + EXPECT_GT(O.psi_d, -1.57); + + // Pitch cmd should be zero + EXPECT_NEAR(O.theta_d, 0.0, tol); +} + +// Test commanded angles when drone is to the left of the track +TEST_F(IntegralLosTest, T07_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, -0.5, 0.0}; + + const types::Outputs O = Ilos_.calculate_outputs(inputs); + + // Heading cmd should be between 0 and pi/2 + EXPECT_GT(O.psi_d, 0.0); + EXPECT_LT(O.psi_d, 1.57); + // Pitch cmd should be zero + EXPECT_NEAR(O.theta_d, 0.0, tol); +} + +// Test commanded angles when drone is under the track +TEST_F(IntegralLosTest, T08_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, 0.5}; + + const types::Outputs O = Ilos_.calculate_outputs(inputs); + + // Heading cmd should be 0 + EXPECT_NEAR(O.psi_d, 0.0, tol); + // Pitch cmd should be between 0 and pi/2 + EXPECT_GT(O.theta_d, 0.0); + EXPECT_LT(O.theta_d, 1.57); +} + +// Test commanded angles when drone is over the track +TEST_F(IntegralLosTest, T09_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, -0.5}; + + const types::Outputs O = Ilos_.calculate_outputs(inputs); + + // Heading cmd should be 0 + EXPECT_NEAR(O.psi_d, 0.0, tol); + // Pitch cmd should be between -pi/2 and 0 + EXPECT_LT(O.theta_d, 0.0); + EXPECT_GT(O.theta_d, -1.57); +} + +// Test commanded angles when drone is over and to the right of the track + +TEST_F(IntegralLosTest, T10_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.5, -0.5}; + + const types::Outputs O = Ilos_.calculate_outputs(inputs); + + // Heading cmd should be between -pi/2 and 0 + EXPECT_LT(O.psi_d, 0.0); + EXPECT_GT(O.psi_d, -1.57); + // Pitch cmd should be between -pi/2 and 0 + EXPECT_LT(O.theta_d, 0.0); + EXPECT_GT(O.theta_d, -1.57); +} + +} // namespace vortex::guidance::los diff --git a/guidance/los_guidance/test/proportional_los_test.cpp b/guidance/los_guidance/test/proportional_los_test.cpp old mode 100755 new mode 100644 index 3c890cbcd..c704dec68 --- a/guidance/los_guidance/test/proportional_los_test.cpp +++ b/guidance/los_guidance/test/proportional_los_test.cpp @@ -1,108 +1,107 @@ -#include #include "los_guidance/lib/proportional_los.hpp" +#include -namespace vortex::guidance::los{ - - class ProportionalLosTest : public ::testing::Test { - protected: - ProportionalLosTest() : Plos_{get_params()} {} - - ProportionalLosParams get_params() { - ProportionalLosParams params; - params.lookahead_distance_h = 10.0; - params.lookahead_distance_v = 10.0; - params.k_p_h = 0.667; // needs tuning - params.k_p_v = 0.582; // needs tuning - params.time_step = 0.01; - return params; - } - - ProportionalLOSGuidance Plos_; - const double tol = 1e-9; - }; - - // Test commanded angles when drone is to the right of the track - TEST_F(ProportionalLosTest, T06_test_commanded_angles) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.5, 0.0}; - - const types::Outputs O = Plos_.calculate_outputs(inputs); - - // Heading cmd should be between -pi/2 and 0 - EXPECT_LT(O.psi_d, 0.0); - EXPECT_GT(O.psi_d, -1.57); - - // Pitch cmd should be zero - EXPECT_NEAR(O.theta_d, 0.0, tol); - } - - // Test commanded angles when drone is to the left of the track - TEST_F(ProportionalLosTest, T07_test_commanded_angles) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, -0.5, 0.0}; - - const types::Outputs O = Plos_.calculate_outputs(inputs); - - // Heading cmd should be between 0 and pi/2 - EXPECT_GT(O.psi_d, 0.0); - EXPECT_LT(O.psi_d, 1.57); - // Pitch cmd should be zero - EXPECT_NEAR(O.theta_d, 0.0, tol); - } - - // Test commanded angles when drone is under the track - TEST_F(ProportionalLosTest, T08_test_commanded_angles) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.0, 0.5}; - - const types::Outputs O = Plos_.calculate_outputs(inputs); - - // Heading cmd should be 0 - EXPECT_NEAR(O.psi_d, 0.0, tol); - // Pitch cmd should be between 0 and pi/2 - EXPECT_GT(O.theta_d, 0.0); - EXPECT_LT(O.theta_d, 1.57); - } - - // Test commanded angles when drone is over the track - TEST_F(ProportionalLosTest, T09_test_commanded_angles) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.0, -0.5}; - - const types::Outputs O = Plos_.calculate_outputs(inputs); - - // Heading cmd should be 0 - EXPECT_NEAR(O.psi_d, 0.0, tol); - // Pitch cmd should be between -pi/2 and 0 - EXPECT_LT(O.theta_d, 0.0); - EXPECT_GT(O.theta_d, -1.57); - } - - // Test commanded angles when drone is over and to the right of the track - - TEST_F(ProportionalLosTest, T10_test_commanded_angles) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.5, -0.5}; +namespace vortex::guidance::los { - const types::Outputs O = Plos_.calculate_outputs(inputs); +class ProportionalLosTest : public ::testing::Test { + protected: + ProportionalLosTest() : Plos_{get_params()} {} - // Heading cmd should be between -pi/2 and 0 - EXPECT_LT(O.psi_d, 0.0); - EXPECT_GT(O.psi_d, -1.57); - // Pitch cmd should be between -pi/2 and 0 - EXPECT_LT(O.theta_d, 0.0); - EXPECT_GT(O.theta_d, -1.57); + ProportionalLosParams get_params() { + ProportionalLosParams params; + params.lookahead_distance_h = 10.0; + params.lookahead_distance_v = 10.0; + params.k_p_h = 0.667; // needs tuning + params.k_p_v = 0.582; // needs tuning + params.time_step = 0.01; + return params; } -} // namespace vortex::guidance - + ProportionalLOSGuidance Plos_; + const double tol = 1e-9; +}; + +// Test commanded angles when drone is to the right of the track +TEST_F(ProportionalLosTest, T06_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.5, 0.0}; + + const types::Outputs O = Plos_.calculate_outputs(inputs); + + // Heading cmd should be between -pi/2 and 0 + EXPECT_LT(O.psi_d, 0.0); + EXPECT_GT(O.psi_d, -1.57); + + // Pitch cmd should be zero + EXPECT_NEAR(O.theta_d, 0.0, tol); +} + +// Test commanded angles when drone is to the left of the track +TEST_F(ProportionalLosTest, T07_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, -0.5, 0.0}; + + const types::Outputs O = Plos_.calculate_outputs(inputs); + + // Heading cmd should be between 0 and pi/2 + EXPECT_GT(O.psi_d, 0.0); + EXPECT_LT(O.psi_d, 1.57); + // Pitch cmd should be zero + EXPECT_NEAR(O.theta_d, 0.0, tol); +} + +// Test commanded angles when drone is under the track +TEST_F(ProportionalLosTest, T08_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, 0.5}; + + const types::Outputs O = Plos_.calculate_outputs(inputs); + + // Heading cmd should be 0 + EXPECT_NEAR(O.psi_d, 0.0, tol); + // Pitch cmd should be between 0 and pi/2 + EXPECT_GT(O.theta_d, 0.0); + EXPECT_LT(O.theta_d, 1.57); +} + +// Test commanded angles when drone is over the track +TEST_F(ProportionalLosTest, T09_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, -0.5}; + + const types::Outputs O = Plos_.calculate_outputs(inputs); + + // Heading cmd should be 0 + EXPECT_NEAR(O.psi_d, 0.0, tol); + // Pitch cmd should be between -pi/2 and 0 + EXPECT_LT(O.theta_d, 0.0); + EXPECT_GT(O.theta_d, -1.57); +} + +// Test commanded angles when drone is over and to the right of the track + +TEST_F(ProportionalLosTest, T10_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.5, -0.5}; + + const types::Outputs O = Plos_.calculate_outputs(inputs); + + // Heading cmd should be between -pi/2 and 0 + EXPECT_LT(O.psi_d, 0.0); + EXPECT_GT(O.psi_d, -1.57); + // Pitch cmd should be between -pi/2 and 0 + EXPECT_LT(O.theta_d, 0.0); + EXPECT_GT(O.theta_d, -1.57); +} + +} // namespace vortex::guidance::los diff --git a/guidance/los_guidance/test/test_main.cpp b/guidance/los_guidance/test/test_main.cpp index 6ace5b598..af89a87e6 100644 --- a/guidance/los_guidance/test/test_main.cpp +++ b/guidance/los_guidance/test/test_main.cpp @@ -5,4 +5,4 @@ int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); -} \ No newline at end of file +} From 66b252eed09cffff0e1e02cecc36982b9a29ccf6 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Thu, 13 Nov 2025 00:13:53 +0100 Subject: [PATCH 030/128] ci: trigger Industrial CI on PRs and main branch pushes --- .github/workflows/industrial-ci.yml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/.github/workflows/industrial-ci.yml b/.github/workflows/industrial-ci.yml index 03c872f26..d2c0e782f 100644 --- a/.github/workflows/industrial-ci.yml +++ b/.github/workflows/industrial-ci.yml @@ -2,6 +2,10 @@ name: Industrial CI on: push: + branches: + - main + pull_request: + types: [ opened, synchronize, reopened, ready_for_review ] workflow_dispatch: schedule: - cron: '0 1 * * *' # Runs daily to check for dependency issues or flaking tests From a36297aa9ac82cb4a2a6f32441169d0a5cdce894 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Thu, 13 Nov 2025 00:43:36 +0100 Subject: [PATCH 031/128] Add C++ and CMake pre-commit hooks (#568) * ci(pre-commit): add CI workflow for checking pre-commit hooks * ci(pre-commit): add hooks for cpp and CMake * refactor(pre-commit.yml): now uses the workflow on main * docs(README.md): update status badge for pre-commit * refactor(ci): move local hooks into a different config file * docs(README.md): update status badge for pre-commit * refactor(ci): pre-commit.yml now only runs on pull_request and workflow_dispatch * ci(pre-commit-local): ignore build/include_order in ament_cpplint since conflict with clang-format * refactor: fix ament_cpplint and cppcheck warnings * ci(pre-commit-config-local): increase line length to 120 for ament_cpplint due to some lines being longer * refactor: fix ament_cpplint and cppcheck warnings * chore(pre-commit): exclude test dirs from ament_cppcheck to avoid false positives * ci(pre-commit): update workflow to run on push to main and pull_request --- .github/workflows/pre-commit.yml | 15 ++++++ .pre-commit-config-local.yaml | 47 +++++++++++++++++++ .../dp_adapt_backs_controller.hpp | 6 +-- .../dp_adapt_backs_controller_ros.hpp | 8 ++-- .../dp_adapt_backs_controller_utils.hpp | 6 +-- .../dp_adapt_backs_controller/typedefs.hpp | 6 +-- .../src/dp_adapt_backs_controller_ros.cpp | 2 +- .../pid_controller_dp/pid_controller.hpp | 8 ++-- .../pid_controller_conversions.hpp | 6 +-- .../pid_controller_dp/pid_controller_ros.hpp | 9 ++-- .../pid_controller_utils.hpp | 6 +-- .../include/pid_controller_dp/typedefs.hpp | 6 +-- .../pid_controller.hpp | 8 ++-- .../pid_controller_ros.hpp | 8 ++-- .../pid_controller_utils.hpp | 6 +-- .../pid_controller_dp_euler/typedefs.hpp | 6 +-- .../ekf_pose_filtering_ros.hpp | 10 ++-- .../pose_action_server_ros.hpp | 10 ++-- .../include/los_guidance/los_guidance_ros.hpp | 10 ++-- .../reference_filter_dp/eigen_typedefs.hpp | 6 +-- .../reference_filter_dp/reference_filter.hpp | 6 +-- .../reference_filter_ros.hpp | 7 +-- .../FSM/docking/include/docking/docking.hpp | 9 ++-- mission/FSM/docking/src/docking.cpp | 21 ++++----- .../eigen_vector6d_typedef.hpp | 6 +-- .../pseudoinverse_allocator.hpp | 6 +-- .../thrust_allocator_ros.hpp | 6 +-- .../thrust_allocator_utils.hpp | 7 +-- .../thruster_interface_auv_driver.hpp | 8 ++-- .../thruster_interface_auv_ros.hpp | 9 ++-- .../src/thruster_interface_auv_driver.cpp | 3 +- 31 files changed, 175 insertions(+), 102 deletions(-) create mode 100644 .github/workflows/pre-commit.yml create mode 100644 .pre-commit-config-local.yaml diff --git a/.github/workflows/pre-commit.yml b/.github/workflows/pre-commit.yml new file mode 100644 index 000000000..7b3fccffe --- /dev/null +++ b/.github/workflows/pre-commit.yml @@ -0,0 +1,15 @@ +name: pre-commit + +on: + push: + branches: + - main + pull_request: + types: [ opened, synchronize, reopened, ready_for_review ] + workflow_dispatch: +jobs: + call_reusable_workflow: + uses: vortexntnu/vortex-ci/.github/workflows/reusable-pre-commit.yml@main + with: + ros_distro: 'humble' + config_path: '.pre-commit-config-local.yaml' diff --git a/.pre-commit-config-local.yaml b/.pre-commit-config-local.yaml new file mode 100644 index 000000000..cb7994b70 --- /dev/null +++ b/.pre-commit-config-local.yaml @@ -0,0 +1,47 @@ +# To use: +# +# pre-commit run --all-files -c .pre-commit-config-local.yaml +# +# Or to install it for automatic checks on commit: +# +# pre-commit install -c .pre-commit-config-local.yaml +# +# To update this file: +# +# pre-commit autoupdate -c .pre-commit-config-local.yaml +# +# See https://pre-commit.com/ for documentation +# +# NOTE: This configuration uses local hooks specific to ROS2 (ament_* linters) + +repos: + - repo: local + hooks: + - id: ament_cppcheck + name: ament_cppcheck + description: Static code analysis of C/C++ files. + entry: env AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS=1 ament_cppcheck + language: system + files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ + exclude: (^|/)(test|tests)/.* # exclude test dirs since ament_cppcheck misparses GTest macros and throws false syntax errors + - repo: local + hooks: + - id: ament_cpplint + name: ament_cpplint + description: Static code analysis of C/C++ files. + entry: ament_cpplint + language: system + files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ + args: [ + "--linelength=120", + "--filter=-whitespace/newline,-legal/copyright,-build/include_order" # ignore build/include_order since it conflicts with .clang-format + ] + # CMake hooks + - repo: local + hooks: + - id: ament_lint_cmake + name: ament_lint_cmake + description: Check format of CMakeLists.txt files. + entry: ament_lint_cmake + language: system + files: CMakeLists\.txt$ diff --git a/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller.hpp b/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller.hpp index 49cbe759f..6a44644e8 100644 --- a/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller.hpp +++ b/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller.hpp @@ -1,5 +1,5 @@ -#ifndef DP_ADAPT_BACKS_CONTROLLER_HPP -#define DP_ADAPT_BACKS_CONTROLLER_HPP +#ifndef DP_ADAPT_BACKS_CONTROLLER__DP_ADAPT_BACKS_CONTROLLER_HPP_ +#define DP_ADAPT_BACKS_CONTROLLER__DP_ADAPT_BACKS_CONTROLLER_HPP_ #include #include @@ -59,4 +59,4 @@ class DPAdaptBacksController { } // namespace vortex::control -#endif // DP_ADAPT_BACKS_CONTROLLER_HPP +#endif // DP_ADAPT_BACKS_CONTROLLER__DP_ADAPT_BACKS_CONTROLLER_HPP_ diff --git a/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_ros.hpp b/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_ros.hpp index 456bb7937..85b1c7f7a 100644 --- a/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_ros.hpp +++ b/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_ros.hpp @@ -1,16 +1,18 @@ -#ifndef DP_ADAPT_BACKS_CONTROLLER_ROS_HPP -#define DP_ADAPT_BACKS_CONTROLLER_ROS_HPP +#ifndef DP_ADAPT_BACKS_CONTROLLER__DP_ADAPT_BACKS_CONTROLLER_ROS_HPP_ +#define DP_ADAPT_BACKS_CONTROLLER__DP_ADAPT_BACKS_CONTROLLER_ROS_HPP_ #include #include #include #include #include +#include #include #include #include #include #include +#include #include #include #include "dp_adapt_backs_controller/dp_adapt_backs_controller.hpp" @@ -93,4 +95,4 @@ class DPAdaptBacksControllerNode : public rclcpp::Node { } // namespace vortex::control -#endif +#endif // DP_ADAPT_BACKS_CONTROLLER__DP_ADAPT_BACKS_CONTROLLER_ROS_HPP_ diff --git a/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_utils.hpp b/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_utils.hpp index 33ceeb295..b48a21867 100644 --- a/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_utils.hpp +++ b/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_utils.hpp @@ -1,5 +1,5 @@ -#ifndef DP_ADAPT_BACKS_CONTROLLER_UTILS_HPP -#define DP_ADAPT_BACKS_CONTROLLER_UTILS_HPP +#ifndef DP_ADAPT_BACKS_CONTROLLER__DP_ADAPT_BACKS_CONTROLLER_UTILS_HPP_ +#define DP_ADAPT_BACKS_CONTROLLER__DP_ADAPT_BACKS_CONTROLLER_UTILS_HPP_ #include #include "dp_adapt_backs_controller/typedefs.hpp" @@ -50,4 +50,4 @@ Eigen::Matrix6x12d calculate_Y_v(const vortex::utils::types::Nu& nu); } // namespace vortex::control -#endif +#endif // DP_ADAPT_BACKS_CONTROLLER__DP_ADAPT_BACKS_CONTROLLER_UTILS_HPP_ diff --git a/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/typedefs.hpp b/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/typedefs.hpp index 4e43dbf63..49f908072 100644 --- a/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/typedefs.hpp +++ b/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/typedefs.hpp @@ -3,8 +3,8 @@ * @brief Contains the Eigen typedefs for the controller. */ -#ifndef VORTEX_DP_ADAPT_BACKSTEPPING_CONTROLLER_TYPEDEFS_H -#define VORTEX_DP_ADAPT_BACKSTEPPING_CONTROLLER_TYPEDEFS_H +#ifndef DP_ADAPT_BACKS_CONTROLLER__TYPEDEFS_HPP_ +#define DP_ADAPT_BACKS_CONTROLLER__TYPEDEFS_HPP_ #include @@ -19,4 +19,4 @@ typedef Eigen::Matrix Matrix12d; } // namespace Eigen -#endif +#endif // DP_ADAPT_BACKS_CONTROLLER__TYPEDEFS_HPP_ diff --git a/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_ros.cpp b/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_ros.cpp index 4bb6d5cee..da70eaf79 100644 --- a/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_ros.cpp +++ b/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_ros.cpp @@ -8,7 +8,7 @@ #include "dp_adapt_backs_controller/dp_adapt_backs_controller_utils.hpp" #include "dp_adapt_backs_controller/typedefs.hpp" -const std::string start_message = R"( +constexpr std::string_view start_message = R"( ____ ____ ____ _ _ _ | _ \| _ \ / ___|___ _ __ | |_ _ __ ___ | | | ___ _ __ | | | | |_) | | | / _ \| '_ \| __| '__/ _ \| | |/ _ \ '__| diff --git a/control/pid_controller_dp/include/pid_controller_dp/pid_controller.hpp b/control/pid_controller_dp/include/pid_controller_dp/pid_controller.hpp index 4ead4b716..1017b5bd6 100644 --- a/control/pid_controller_dp/include/pid_controller_dp/pid_controller.hpp +++ b/control/pid_controller_dp/include/pid_controller_dp/pid_controller.hpp @@ -1,11 +1,11 @@ -#ifndef PID_CONTROLLER_HPP -#define PID_CONTROLLER_HPP +#ifndef PID_CONTROLLER_DP__PID_CONTROLLER_HPP_ +#define PID_CONTROLLER_DP__PID_CONTROLLER_HPP_ #include "pid_controller_dp/typedefs.hpp" class PIDController { public: - explicit PIDController(); + PIDController(); // @brief Calculate the control input tau // @param eta: struct containing the vehicle pose [position, orientation] @@ -44,4 +44,4 @@ class PIDController { double dt_; }; -#endif +#endif // PID_CONTROLLER_DP__PID_CONTROLLER_HPP_ diff --git a/control/pid_controller_dp/include/pid_controller_dp/pid_controller_conversions.hpp b/control/pid_controller_dp/include/pid_controller_dp/pid_controller_conversions.hpp index 16045e085..a28097e9f 100644 --- a/control/pid_controller_dp/include/pid_controller_dp/pid_controller_conversions.hpp +++ b/control/pid_controller_dp/include/pid_controller_dp/pid_controller_conversions.hpp @@ -1,5 +1,5 @@ -#ifndef PID_CONTROLLER_CONVERSIONS_HPP -#define PID_CONTROLLER_CONVERSIONS_HPP +#ifndef PID_CONTROLLER_DP__PID_CONTROLLER_CONVERSIONS_HPP_ +#define PID_CONTROLLER_DP__PID_CONTROLLER_CONVERSIONS_HPP_ #include #include @@ -15,4 +15,4 @@ types::Eta eta_convert_from_ros_to_eigen( types::Nu nu_convert_from_ros_to_eigen( const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); -#endif +#endif // PID_CONTROLLER_DP__PID_CONTROLLER_CONVERSIONS_HPP_ diff --git a/control/pid_controller_dp/include/pid_controller_dp/pid_controller_ros.hpp b/control/pid_controller_dp/include/pid_controller_dp/pid_controller_ros.hpp index 06b57c111..128c8650c 100644 --- a/control/pid_controller_dp/include/pid_controller_dp/pid_controller_ros.hpp +++ b/control/pid_controller_dp/include/pid_controller_dp/pid_controller_ros.hpp @@ -1,5 +1,5 @@ -#ifndef PID_CONTROLLER_ROS_HPP -#define PID_CONTROLLER_ROS_HPP +#ifndef PID_CONTROLLER_DP__PID_CONTROLLER_ROS_HPP_ +#define PID_CONTROLLER_DP__PID_CONTROLLER_ROS_HPP_ #include #include @@ -11,6 +11,7 @@ #include #include #include +#include #include #include #include "pid_controller_dp/pid_controller.hpp" @@ -19,7 +20,7 @@ // @brief Class for the PID controller node class PIDControllerNode : public rclcpp::Node { public: - explicit PIDControllerNode(); + PIDControllerNode(); private: // @brief Callback function for the killswitch topic @@ -95,4 +96,4 @@ class PIDControllerNode : public rclcpp::Node { std::string software_mode_; }; -#endif +#endif // PID_CONTROLLER_DP__PID_CONTROLLER_ROS_HPP_ diff --git a/control/pid_controller_dp/include/pid_controller_dp/pid_controller_utils.hpp b/control/pid_controller_dp/include/pid_controller_dp/pid_controller_utils.hpp index 36253ae38..1bdce214b 100644 --- a/control/pid_controller_dp/include/pid_controller_dp/pid_controller_utils.hpp +++ b/control/pid_controller_dp/include/pid_controller_dp/pid_controller_utils.hpp @@ -1,5 +1,5 @@ -#ifndef PID_UTILS_HPP -#define PID_UTILS_HPP +#ifndef PID_CONTROLLER_DP__PID_CONTROLLER_UTILS_HPP_ +#define PID_CONTROLLER_DP__PID_CONTROLLER_UTILS_HPP_ #include #include @@ -71,4 +71,4 @@ types::Vector7d anti_windup(const double dt, const types::Eta& error, const types::Vector7d& integral); -#endif +#endif // PID_CONTROLLER_DP__PID_CONTROLLER_UTILS_HPP_ diff --git a/control/pid_controller_dp/include/pid_controller_dp/typedefs.hpp b/control/pid_controller_dp/include/pid_controller_dp/typedefs.hpp index 474abb25c..beeea3174 100644 --- a/control/pid_controller_dp/include/pid_controller_dp/typedefs.hpp +++ b/control/pid_controller_dp/include/pid_controller_dp/typedefs.hpp @@ -3,8 +3,8 @@ * @brief Contains the typedef for a 6x1 Eigen vector and a 6x6 Eigen matrix. */ -#ifndef VORTEX_EIGEN_TYPEDEFS_H -#define VORTEX_EIGEN_TYPEDEFS_H +#ifndef PID_CONTROLLER_DP__TYPEDEFS_HPP_ +#define PID_CONTROLLER_DP__TYPEDEFS_HPP_ #include @@ -56,4 +56,4 @@ struct J_transformation { }; } // namespace types -#endif +#endif // PID_CONTROLLER_DP__TYPEDEFS_HPP_ diff --git a/control/pid_controller_dp_euler/include/pid_controller_dp_euler/pid_controller.hpp b/control/pid_controller_dp_euler/include/pid_controller_dp_euler/pid_controller.hpp index a44c2ef4d..6872213a3 100644 --- a/control/pid_controller_dp_euler/include/pid_controller_dp_euler/pid_controller.hpp +++ b/control/pid_controller_dp_euler/include/pid_controller_dp_euler/pid_controller.hpp @@ -1,11 +1,11 @@ -#ifndef PID_CONTROLLER_HPP -#define PID_CONTROLLER_HPP +#ifndef PID_CONTROLLER_DP_EULER__PID_CONTROLLER_HPP_ +#define PID_CONTROLLER_DP_EULER__PID_CONTROLLER_HPP_ #include class PIDController { public: - explicit PIDController(); + PIDController(); Vector6d calculate_tau(const Eta& eta, const Eta& eta_d, @@ -28,4 +28,4 @@ class PIDController { double dt_; }; -#endif +#endif // PID_CONTROLLER_DP_EULER__PID_CONTROLLER_HPP_ diff --git a/control/pid_controller_dp_euler/include/pid_controller_dp_euler/pid_controller_ros.hpp b/control/pid_controller_dp_euler/include/pid_controller_dp_euler/pid_controller_ros.hpp index 04f1ef097..22e3df334 100644 --- a/control/pid_controller_dp_euler/include/pid_controller_dp_euler/pid_controller_ros.hpp +++ b/control/pid_controller_dp_euler/include/pid_controller_dp_euler/pid_controller_ros.hpp @@ -1,5 +1,5 @@ -#ifndef PID_CONTROLLER_ROS_HPP -#define PID_CONTROLLER_ROS_HPP +#ifndef PID_CONTROLLER_DP_EULER__PID_CONTROLLER_ROS_HPP_ +#define PID_CONTROLLER_DP_EULER__PID_CONTROLLER_ROS_HPP_ #include #include @@ -17,7 +17,7 @@ class PIDControllerNode : public rclcpp::Node { public: - explicit PIDControllerNode(); + PIDControllerNode(); private: void killswitch_callback(const std_msgs::msg::Bool::SharedPtr msg); @@ -73,4 +73,4 @@ class PIDControllerNode : public rclcpp::Node { std::string software_mode_; }; -#endif +#endif // PID_CONTROLLER_DP_EULER__PID_CONTROLLER_ROS_HPP_ diff --git a/control/pid_controller_dp_euler/include/pid_controller_dp_euler/pid_controller_utils.hpp b/control/pid_controller_dp_euler/include/pid_controller_dp_euler/pid_controller_utils.hpp index a9e622fe7..f1db15a2d 100644 --- a/control/pid_controller_dp_euler/include/pid_controller_dp_euler/pid_controller_utils.hpp +++ b/control/pid_controller_dp_euler/include/pid_controller_dp_euler/pid_controller_utils.hpp @@ -1,5 +1,5 @@ -#ifndef PID_UTILS_HPP -#define PID_UTILS_HPP +#ifndef PID_CONTROLLER_DP_EULER__PID_CONTROLLER_UTILS_HPP_ +#define PID_CONTROLLER_DP_EULER__PID_CONTROLLER_UTILS_HPP_ #include #include @@ -22,4 +22,4 @@ Vector6d clamp_values(const Vector6d& values, double min_val, double max_val); Vector6d limit_input(const Vector6d& input); -#endif +#endif // PID_CONTROLLER_DP_EULER__PID_CONTROLLER_UTILS_HPP_ diff --git a/control/pid_controller_dp_euler/include/pid_controller_dp_euler/typedefs.hpp b/control/pid_controller_dp_euler/include/pid_controller_dp_euler/typedefs.hpp index 0c2f1db08..5d659fa93 100644 --- a/control/pid_controller_dp_euler/include/pid_controller_dp_euler/typedefs.hpp +++ b/control/pid_controller_dp_euler/include/pid_controller_dp_euler/typedefs.hpp @@ -3,8 +3,8 @@ * @brief Contains the typedef for a 6x1 Eigen vector and a 6x6 Eigen matrix. */ -#ifndef VORTEX_EIGEN_TYPEDEFS_H -#define VORTEX_EIGEN_TYPEDEFS_H +#ifndef PID_CONTROLLER_DP_EULER__TYPEDEFS_HPP_ +#define PID_CONTROLLER_DP_EULER__TYPEDEFS_HPP_ #include #include @@ -102,4 +102,4 @@ struct Nu { } }; -#endif +#endif // PID_CONTROLLER_DP_EULER__TYPEDEFS_HPP_ diff --git a/filtering/ekf_pose_filtering/include/ekf_pose_filtering/ekf_pose_filtering_ros.hpp b/filtering/ekf_pose_filtering/include/ekf_pose_filtering/ekf_pose_filtering_ros.hpp index 94f3267b2..815b239ac 100644 --- a/filtering/ekf_pose_filtering/include/ekf_pose_filtering/ekf_pose_filtering_ros.hpp +++ b/filtering/ekf_pose_filtering/include/ekf_pose_filtering/ekf_pose_filtering_ros.hpp @@ -1,5 +1,5 @@ -#ifndef EKF_POSE_FILTERING_ROS_HPP -#define EKF_POSE_FILTERING_ROS_HPP +#ifndef EKF_POSE_FILTERING__EKF_POSE_FILTERING_ROS_HPP_ +#define EKF_POSE_FILTERING__EKF_POSE_FILTERING_ROS_HPP_ #include #include @@ -8,8 +8,10 @@ #include #include #include +#include #include #include +#include #include #include #include @@ -18,7 +20,7 @@ class EKFPoseFilteringNode : public rclcpp::Node { public: EKFPoseFilteringNode(); - ~EKFPoseFilteringNode() {}; + ~EKFPoseFilteringNode() {} private: void reset_EFK_state( @@ -65,4 +67,4 @@ class EKFPoseFilteringNode : public rclcpp::Node { bool enu_orientation_; }; -#endif // EKF_POSE_FILTERING_ROS_HPP +#endif // EKF_POSE_FILTERING__EKF_POSE_FILTERING_ROS_HPP_ diff --git a/filtering/pose_action_server/include/pose_action_server/pose_action_server_ros.hpp b/filtering/pose_action_server/include/pose_action_server/pose_action_server_ros.hpp index 4ac479570..244d7d5f4 100644 --- a/filtering/pose_action_server/include/pose_action_server/pose_action_server_ros.hpp +++ b/filtering/pose_action_server/include/pose_action_server/pose_action_server_ros.hpp @@ -1,13 +1,15 @@ -#ifndef POSE_ACTION_SERVER_ROS_HPP -#define POSE_ACTION_SERVER_ROS_HPP +#ifndef POSE_ACTION_SERVER__POSE_ACTION_SERVER_ROS_HPP_ +#define POSE_ACTION_SERVER__POSE_ACTION_SERVER_ROS_HPP_ #include #include #include #include #include +#include #include #include +#include #include class PoseActionServerNode : public rclcpp::Node { @@ -17,7 +19,7 @@ class PoseActionServerNode : public rclcpp::Node { public: PoseActionServerNode(); - ~PoseActionServerNode() {}; + ~PoseActionServerNode() {} private: rclcpp_action::Server::SharedPtr @@ -53,4 +55,4 @@ class PoseActionServerNode : public rclcpp::Node { const std::vector& quaternions); }; -#endif // POSE_ACTION_SERVER_ROS_HPP +#endif // POSE_ACTION_SERVER__POSE_ACTION_SERVER_ROS_HPP_ diff --git a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp index eb53d461c..1a7bb74a2 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -1,10 +1,12 @@ -#ifndef LOS_GUIDANCE_ROS_HPP -#define LOS_GUIDANCE_ROS_HPP +#ifndef LOS_GUIDANCE__LOS_GUIDANCE_ROS_HPP_ +#define LOS_GUIDANCE__LOS_GUIDANCE_ROS_HPP_ #include #include #include #include +#include +#include #include #include #include @@ -20,7 +22,7 @@ namespace vortex::guidance::los { class LosGuidanceNode : public rclcpp::Node { public: - explicit LosGuidanceNode(); + LosGuidanceNode(); private: // @brief Set the subscribers and publishers @@ -128,4 +130,4 @@ class LosGuidanceNode : public rclcpp::Node { } // namespace vortex::guidance::los -#endif // LOS_GUIDANCE_ROS_HPP +#endif // LOS_GUIDANCE__LOS_GUIDANCE_ROS_HPP_ diff --git a/guidance/reference_filter_dp/include/reference_filter_dp/eigen_typedefs.hpp b/guidance/reference_filter_dp/include/reference_filter_dp/eigen_typedefs.hpp index 9828efb30..f9b29feae 100644 --- a/guidance/reference_filter_dp/include/reference_filter_dp/eigen_typedefs.hpp +++ b/guidance/reference_filter_dp/include/reference_filter_dp/eigen_typedefs.hpp @@ -3,8 +3,8 @@ * @brief Contains Eigen typedefs used in this package. */ -#ifndef REFERENCE_FILTER_DP_EIGEN_TYPEDEFS_HPP -#define REFERENCE_FILTER_DP_EIGEN_TYPEDEFS_HPP +#ifndef REFERENCE_FILTER_DP__EIGEN_TYPEDEFS_HPP_ +#define REFERENCE_FILTER_DP__EIGEN_TYPEDEFS_HPP_ #include @@ -18,4 +18,4 @@ typedef Eigen::Matrix Vector18d; } // namespace Eigen -#endif // REFERENCE_FILTER_DP_EIGEN_TYPEDEFS_HPP +#endif // REFERENCE_FILTER_DP__EIGEN_TYPEDEFS_HPP_ diff --git a/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter.hpp b/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter.hpp index eff869bb4..dd9c10d98 100644 --- a/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter.hpp +++ b/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter.hpp @@ -1,5 +1,5 @@ -#ifndef REFERENCE_FILTER_HPP -#define REFERENCE_FILTER_HPP +#ifndef REFERENCE_FILTER_DP__REFERENCE_FILTER_HPP_ +#define REFERENCE_FILTER_DP__REFERENCE_FILTER_HPP_ #include "reference_filter_dp/eigen_typedefs.hpp" @@ -41,4 +41,4 @@ class ReferenceFilter { } // namespace vortex::guidance -#endif // REFERENCE_FILTER_HPP +#endif // REFERENCE_FILTER_DP__REFERENCE_FILTER_HPP_ diff --git a/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter_ros.hpp b/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter_ros.hpp index c9553e761..6bc1584f1 100644 --- a/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter_ros.hpp +++ b/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter_ros.hpp @@ -1,9 +1,10 @@ -#ifndef REFERENCE_FILTER_ROS_HPP -#define REFERENCE_FILTER_ROS_HPP +#ifndef REFERENCE_FILTER_DP__REFERENCE_FILTER_ROS_HPP_ +#define REFERENCE_FILTER_DP__REFERENCE_FILTER_ROS_HPP_ #include #include #include +#include #include #include #include @@ -124,4 +125,4 @@ class ReferenceFilterNode : public rclcpp::Node { } // namespace vortex::guidance -#endif // REFERENCE_FILTER_ROS_HPP +#endif // REFERENCE_FILTER_DP__REFERENCE_FILTER_ROS_HPP_ diff --git a/mission/FSM/docking/include/docking/docking.hpp b/mission/FSM/docking/include/docking/docking.hpp index 82dd88349..906696228 100644 --- a/mission/FSM/docking/include/docking/docking.hpp +++ b/mission/FSM/docking/include/docking/docking.hpp @@ -1,5 +1,5 @@ -#ifndef VORTEX_DOCKING_HPP -#define VORTEX_DOCKING_HPP +#ifndef DOCKING__DOCKING_HPP_ +#define DOCKING__DOCKING_HPP_ #include #include @@ -98,7 +98,8 @@ std::string DockedState( class ReturnHomeState : public yasmin_ros::ActionState { public: - ReturnHomeState(std::shared_ptr blackboard); + explicit ReturnHomeState( + std::shared_ptr blackboard); docking_fsm::ReturnHomeAction::Goal create_goal_handler( std::shared_ptr blackboard); @@ -145,4 +146,4 @@ void add_states(std::shared_ptr sm, auto initialize_blackboard(); -#endif +#endif // DOCKING__DOCKING_HPP_ diff --git a/mission/FSM/docking/src/docking.cpp b/mission/FSM/docking/src/docking.cpp index 459bfb236..4476d90a1 100644 --- a/mission/FSM/docking/src/docking.cpp +++ b/mission/FSM/docking/src/docking.cpp @@ -9,7 +9,7 @@ FindDockingStationState::FindDockingStationState( blackboard->get("pose_action"), std::bind(&FindDockingStationState::create_goal_handler, this, _1), std::bind(&FindDockingStationState::response_handler, this, _1, _2), - std::bind(&FindDockingStationState::print_feedback, this, _1, _2)) {}; + std::bind(&FindDockingStationState::print_feedback, this, _1, _2)) {} docking_fsm::FindDockingAction::Goal FindDockingStationState::create_goal_handler( @@ -70,7 +70,7 @@ ApproachDockingStationState::ApproachDockingStationState( std::bind(&ApproachDockingStationState::print_feedback, this, _1, - _2)) {}; + _2)) {} docking_fsm::ApproachDockingAction::Goal ApproachDockingStationState::create_goal_handler( @@ -136,7 +136,7 @@ GoAboveDockingStationState::GoAboveDockingStationState( std::bind(&GoAboveDockingStationState::print_feedback, this, _1, - _2)) {}; + _2)) {} docking_fsm::GoAboveDockingAction::Goal GoAboveDockingStationState::create_goal_handler( @@ -204,7 +204,7 @@ ConvergeDockingStationState::ConvergeDockingStationState( std::bind(&ConvergeDockingStationState::print_feedback, this, _1, - _2)) {}; + _2)) {} docking_fsm::ConvergeDockingAction::Goal ConvergeDockingStationState::create_goal_handler( @@ -267,7 +267,7 @@ std::string DockedState( return yasmin_ros::basic_outcomes::ABORT; } } -}; +} ReturnHomeState::ReturnHomeState( std::shared_ptr blackboard) @@ -275,7 +275,7 @@ ReturnHomeState::ReturnHomeState( blackboard->get("reference_filter_action"), std::bind(&ReturnHomeState::create_goal_handler, this, _1), std::bind(&ReturnHomeState::response_handler, this, _1, _2), - std::bind(&ReturnHomeState::print_feedback, this, _1, _2)) {}; + std::bind(&ReturnHomeState::print_feedback, this, _1, _2)) {} docking_fsm::ReturnHomeAction::Goal ReturnHomeState::create_goal_handler( std::shared_ptr blackboard) { @@ -330,13 +330,13 @@ std::string AbortState( std::shared_ptr blackboard) { blackboard->set("is_abort", true); return yasmin_ros::basic_outcomes::ABORT; -}; +} std::string ErrorState( std::shared_ptr blackboard) { blackboard->set("is_error", true); return yasmin_ros::basic_outcomes::SUCCEED; -}; +} std::shared_ptr create_state_machines() { std::set outcomes = { @@ -375,7 +375,6 @@ void add_states(std::shared_ptr sm, "UPDATE_DOCKING_STATION_POSITION"}, {yasmin_ros::basic_outcomes::ABORT, "ABORT"}, {yasmin_ros::basic_outcomes::CANCEL, "APPROACH_DOCKING_STATION"}, - }); sm->add_state( @@ -385,7 +384,6 @@ void add_states(std::shared_ptr sm, {yasmin_ros::basic_outcomes::SUCCEED, "CONVERGE_DOCKING_STATION"}, {yasmin_ros::basic_outcomes::ABORT, "ABORT"}, {yasmin_ros::basic_outcomes::CANCEL, "GO_ABOVE_DOCKING_STATION"}, - }); sm->add_state( @@ -395,7 +393,6 @@ void add_states(std::shared_ptr sm, {yasmin_ros::basic_outcomes::SUCCEED, "DOCKED"}, {yasmin_ros::basic_outcomes::ABORT, "ABORT"}, {yasmin_ros::basic_outcomes::CANCEL, "GO_ABOVE_DOCKING_STATION"}, - }); sm->add_state("DOCKED", @@ -408,7 +405,6 @@ void add_states(std::shared_ptr sm, {yasmin_ros::basic_outcomes::SUCCEED, yasmin_ros::basic_outcomes::SUCCEED}, {yasmin_ros::basic_outcomes::ABORT, "ABORT"}, - }); sm->add_state( "RETURN_HOME", std::make_shared(blackboard), @@ -416,7 +412,6 @@ void add_states(std::shared_ptr sm, {yasmin_ros::basic_outcomes::SUCCEED, "FIND_DOCKING_STATION"}, {yasmin_ros::basic_outcomes::CANCEL, "error"}, {yasmin_ros::basic_outcomes::ABORT, "ABORT"}, - }); sm->add_state("ABORT", std::make_shared( diff --git a/motion/thrust_allocator_auv/include/thrust_allocator_auv/eigen_vector6d_typedef.hpp b/motion/thrust_allocator_auv/include/thrust_allocator_auv/eigen_vector6d_typedef.hpp index e06511362..23ecff92f 100644 --- a/motion/thrust_allocator_auv/include/thrust_allocator_auv/eigen_vector6d_typedef.hpp +++ b/motion/thrust_allocator_auv/include/thrust_allocator_auv/eigen_vector6d_typedef.hpp @@ -3,8 +3,8 @@ * @brief Contains the typedef for a 6x1 Eigen vector. */ -#ifndef VORTEX_EIGEN_TYPEDEFS_H -#define VORTEX_EIGEN_TYPEDEFS_H +#ifndef THRUST_ALLOCATOR_AUV__EIGEN_VECTOR6D_TYPEDEF_HPP_ +#define THRUST_ALLOCATOR_AUV__EIGEN_VECTOR6D_TYPEDEF_HPP_ #include @@ -12,4 +12,4 @@ namespace Eigen { typedef Eigen::Matrix Vector6d; } -#endif // VORTEX_EIGEN_TYPEDEFS_H +#endif // THRUST_ALLOCATOR_AUV__EIGEN_VECTOR6D_TYPEDEF_HPP_ diff --git a/motion/thrust_allocator_auv/include/thrust_allocator_auv/pseudoinverse_allocator.hpp b/motion/thrust_allocator_auv/include/thrust_allocator_auv/pseudoinverse_allocator.hpp index e1d96ce9f..6b99c6e4b 100644 --- a/motion/thrust_allocator_auv/include/thrust_allocator_auv/pseudoinverse_allocator.hpp +++ b/motion/thrust_allocator_auv/include/thrust_allocator_auv/pseudoinverse_allocator.hpp @@ -5,8 +5,8 @@ * Handbook of Marine Craft Hydrodynamics and Motion Control (chapter 12.3.2). */ -#ifndef VORTEX_ALLOCATOR_PSEUDOINVERSE_ALLOCATOR_HPP -#define VORTEX_ALLOCATOR_PSEUDOINVERSE_ALLOCATOR_HPP +#ifndef THRUST_ALLOCATOR_AUV__PSEUDOINVERSE_ALLOCATOR_HPP_ +#define THRUST_ALLOCATOR_AUV__PSEUDOINVERSE_ALLOCATOR_HPP_ #include @@ -36,4 +36,4 @@ class PseudoinverseAllocator { Eigen::MatrixXd T_pinv; }; -#endif // VORTEX_ALLOCATOR_PSEUDOINVERSE_ALLOCATOR_HPP +#endif // THRUST_ALLOCATOR_AUV__PSEUDOINVERSE_ALLOCATOR_HPP_ diff --git a/motion/thrust_allocator_auv/include/thrust_allocator_auv/thrust_allocator_ros.hpp b/motion/thrust_allocator_auv/include/thrust_allocator_auv/thrust_allocator_ros.hpp index 4d000010e..9f0b5e307 100644 --- a/motion/thrust_allocator_auv/include/thrust_allocator_auv/thrust_allocator_ros.hpp +++ b/motion/thrust_allocator_auv/include/thrust_allocator_auv/thrust_allocator_ros.hpp @@ -3,8 +3,8 @@ * @brief Contains the ROS logic for the thruster allocator node. */ -#ifndef VORTEX_ALLOCATOR_ROS_HPP -#define VORTEX_ALLOCATOR_ROS_HPP +#ifndef THRUST_ALLOCATOR_AUV__THRUST_ALLOCATOR_ROS_HPP_ +#define THRUST_ALLOCATOR_AUV__THRUST_ALLOCATOR_ROS_HPP_ #include #include @@ -94,4 +94,4 @@ class ThrustAllocator : public rclcpp::Node { rclcpp::TimerBase::SharedPtr watchdog_timer_; }; -#endif // VORTEX_ALLOCATOR_ROS_HPP +#endif // THRUST_ALLOCATOR_AUV__THRUST_ALLOCATOR_ROS_HPP_ diff --git a/motion/thrust_allocator_auv/include/thrust_allocator_auv/thrust_allocator_utils.hpp b/motion/thrust_allocator_auv/include/thrust_allocator_auv/thrust_allocator_utils.hpp index 8e13a0fe4..7768c3090 100644 --- a/motion/thrust_allocator_auv/include/thrust_allocator_auv/thrust_allocator_utils.hpp +++ b/motion/thrust_allocator_auv/include/thrust_allocator_auv/thrust_allocator_utils.hpp @@ -4,9 +4,10 @@ * module. */ -#ifndef VORTEX_ALLOCATOR_UTILS_HPP -#define VORTEX_ALLOCATOR_UTILS_HPP +#ifndef THRUST_ALLOCATOR_AUV__THRUST_ALLOCATOR_UTILS_HPP_ +#define THRUST_ALLOCATOR_AUV__THRUST_ALLOCATOR_UTILS_HPP_ +#include #include #include #include @@ -150,4 +151,4 @@ inline Eigen::Vector6d wrench_to_vector( return msg_vector; } -#endif // VORTEX_ALLOCATOR_UTILS_HPP +#endif // THRUST_ALLOCATOR_AUV__THRUST_ALLOCATOR_UTILS_HPP_ diff --git a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp index 35c60d5e2..6e2f0ebd4 100644 --- a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp +++ b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp @@ -1,5 +1,5 @@ -#ifndef THRUSTER_INTERFACE_AUV_DRIVER_HPP -#define THRUSTER_INTERFACE_AUV_DRIVER_HPP +#ifndef THRUSTER_INTERFACE_AUV__THRUSTER_INTERFACE_AUV_DRIVER_HPP_ +#define THRUSTER_INTERFACE_AUV__THRUSTER_INTERFACE_AUV_DRIVER_HPP_ #include #include @@ -61,7 +61,7 @@ class ThrusterInterfaceAUVDriver { * polynomial coefficients */ ThrusterInterfaceAUVDriver( - short i2c_bus, + std::int16_t i2c_bus, int pico_i2c_address, const std::vector& thruster_parameters, const std::vector>& poly_coeffs); @@ -152,4 +152,4 @@ class ThrusterInterfaceAUVDriver { } }; -#endif // THRUSTER_INTERFACE_AUV_DRIVER_HPP +#endif // THRUSTER_INTERFACE_AUV__THRUSTER_INTERFACE_AUV_DRIVER_HPP_ diff --git a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp index 4171ba968..58ca254fb 100644 --- a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp +++ b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp @@ -1,10 +1,13 @@ -#ifndef THRUSTER_INTERFACE_AUV_NODE_HPP -#define THRUSTER_INTERFACE_AUV_NODE_HPP +#ifndef THRUSTER_INTERFACE_AUV__THRUSTER_INTERFACE_AUV_ROS_HPP_ +#define THRUSTER_INTERFACE_AUV__THRUSTER_INTERFACE_AUV_ROS_HPP_ +#include #include #include #include #include +#include +#include #include #include "thruster_interface_auv/thruster_interface_auv_driver.hpp" @@ -94,4 +97,4 @@ class ThrusterInterfaceAUVNode : public rclcpp::Node { void update_debug_flag(const rclcpp::Parameter& p); }; -#endif // THRUSTER_INTERFACE_AUV_NODE_HPP +#endif // THRUSTER_INTERFACE_AUV__THRUSTER_INTERFACE_AUV_ROS_HPP_ diff --git a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp index a932a66d4..86834a8e7 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp @@ -1,10 +1,11 @@ #include "thruster_interface_auv/thruster_interface_auv_driver.hpp" #include +#include #include #include ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver( - short i2c_bus, + std::int16_t i2c_bus, int pico_i2c_address, const std::vector& thruster_parameters, const std::vector>& poly_coeffs) From a10e6392bfc4408be2d5e928f9ec0c60557356b5 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Thu, 13 Nov 2025 00:45:17 +0100 Subject: [PATCH 032/128] docs: add pre-commit CI badge to README --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index 0fc3e31d0..23f4c5eb5 100644 --- a/README.md +++ b/README.md @@ -3,6 +3,7 @@ [![Simulator Test](https://github.com/vortexntnu/vortex-auv/actions/workflows/simulator-test.yml/badge.svg)](https://github.com/vortexntnu/vortex-auv/actions/workflows/simulator-test.yml) [![ROS Node Test](https://github.com/vortexntnu/vortex-auv/actions/workflows/ros-node-tests.yml/badge.svg)](https://github.com/vortexntnu/vortex-auv/actions/workflows/ros-node-tests.yml) [![pre-commit.ci status](https://results.pre-commit.ci/badge/github/vortexntnu/vortex-auv/main.svg)](https://results.pre-commit.ci/latest/github/vortexntnu/vortex-auv/main) +[![pre-commit](https://github.com/vortexntnu/vortex-auv/actions/workflows/pre-commit.yml/badge.svg)](https://github.com/vortexntnu/vortex-auv/actions/workflows/pre-commit.yml) [![codecov](https://codecov.io/github/vortexntnu/vortex-auv/graph/badge.svg?token=UXIGc2qG7N)](https://codecov.io/github/vortexntnu/vortex-auv) ![Banner](docs/banner_image.png) From b2e632b5b80da952b83ef20e9704c9a8eff02e0c Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Sun, 16 Nov 2025 21:53:02 +0100 Subject: [PATCH 033/128] refactor: merge the two pre-commit files to replace the pre-commit-ci bot --- .github/workflows/pre-commit.yml | 2 +- .pre-commit-config-local.yaml | 47 -------------------------------- .pre-commit-config.yaml | 30 ++++++++++++++++++++ README.md | 1 - 4 files changed, 31 insertions(+), 49 deletions(-) delete mode 100644 .pre-commit-config-local.yaml diff --git a/.github/workflows/pre-commit.yml b/.github/workflows/pre-commit.yml index 7b3fccffe..76e7310bc 100644 --- a/.github/workflows/pre-commit.yml +++ b/.github/workflows/pre-commit.yml @@ -12,4 +12,4 @@ jobs: uses: vortexntnu/vortex-ci/.github/workflows/reusable-pre-commit.yml@main with: ros_distro: 'humble' - config_path: '.pre-commit-config-local.yaml' + config_path: '.pre-commit-config.yaml' diff --git a/.pre-commit-config-local.yaml b/.pre-commit-config-local.yaml deleted file mode 100644 index cb7994b70..000000000 --- a/.pre-commit-config-local.yaml +++ /dev/null @@ -1,47 +0,0 @@ -# To use: -# -# pre-commit run --all-files -c .pre-commit-config-local.yaml -# -# Or to install it for automatic checks on commit: -# -# pre-commit install -c .pre-commit-config-local.yaml -# -# To update this file: -# -# pre-commit autoupdate -c .pre-commit-config-local.yaml -# -# See https://pre-commit.com/ for documentation -# -# NOTE: This configuration uses local hooks specific to ROS2 (ament_* linters) - -repos: - - repo: local - hooks: - - id: ament_cppcheck - name: ament_cppcheck - description: Static code analysis of C/C++ files. - entry: env AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS=1 ament_cppcheck - language: system - files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ - exclude: (^|/)(test|tests)/.* # exclude test dirs since ament_cppcheck misparses GTest macros and throws false syntax errors - - repo: local - hooks: - - id: ament_cpplint - name: ament_cpplint - description: Static code analysis of C/C++ files. - entry: ament_cpplint - language: system - files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ - args: [ - "--linelength=120", - "--filter=-whitespace/newline,-legal/copyright,-build/include_order" # ignore build/include_order since it conflicts with .clang-format - ] - # CMake hooks - - repo: local - hooks: - - id: ament_lint_cmake - name: ament_lint_cmake - description: Check format of CMakeLists.txt files. - entry: ament_lint_cmake - language: system - files: CMakeLists\.txt$ diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index c7452a826..6609d6cbc 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -74,6 +74,36 @@ repos: hooks: - id: clang-format args: [--style=file] + - repo: local + hooks: + - id: ament_cppcheck + name: ament_cppcheck + description: Static code analysis of C/C++ files. + entry: env AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS=1 ament_cppcheck + language: system + files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ + exclude: (^|/)(test|tests)/.* # exclude test dirs since ament_cppcheck misparses GTest macros and throws false syntax errors + - repo: local + hooks: + - id: ament_cpplint + name: ament_cpplint + description: Static code analysis of C/C++ files. + entry: ament_cpplint + language: system + files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ + args: [ + "--linelength=120", + "--filter=-whitespace/newline,-legal/copyright,-build/include_order" # ignore build/include_order since it conflicts with .clang-format + ] + # CMake hooks + - repo: local + hooks: + - id: ament_lint_cmake + name: ament_lint_cmake + description: Check format of CMakeLists.txt files. + entry: ament_lint_cmake + language: system + files: CMakeLists\.txt$ # Spellcheck in comments and docs - repo: https://github.com/codespell-project/codespell rev: v2.4.1 diff --git a/README.md b/README.md index 23f4c5eb5..8d819ed16 100644 --- a/README.md +++ b/README.md @@ -2,7 +2,6 @@ [![Industrial CI](https://github.com/vortexntnu/vortex-auv/actions/workflows/industrial-ci.yml/badge.svg)](https://github.com/vortexntnu/vortex-auv/actions/workflows/industrial-ci.yml) [![Simulator Test](https://github.com/vortexntnu/vortex-auv/actions/workflows/simulator-test.yml/badge.svg)](https://github.com/vortexntnu/vortex-auv/actions/workflows/simulator-test.yml) [![ROS Node Test](https://github.com/vortexntnu/vortex-auv/actions/workflows/ros-node-tests.yml/badge.svg)](https://github.com/vortexntnu/vortex-auv/actions/workflows/ros-node-tests.yml) -[![pre-commit.ci status](https://results.pre-commit.ci/badge/github/vortexntnu/vortex-auv/main.svg)](https://results.pre-commit.ci/latest/github/vortexntnu/vortex-auv/main) [![pre-commit](https://github.com/vortexntnu/vortex-auv/actions/workflows/pre-commit.yml/badge.svg)](https://github.com/vortexntnu/vortex-auv/actions/workflows/pre-commit.yml) [![codecov](https://codecov.io/github/vortexntnu/vortex-auv/graph/badge.svg?token=UXIGc2qG7N)](https://codecov.io/github/vortexntnu/vortex-auv) From ef96e1eed2fc7e1312cc9dc43e834e5e3f2481fa Mon Sep 17 00:00:00 2001 From: jorgenfj <144696109+jorgenfj@users.noreply.github.com> Date: Fri, 19 Dec 2025 11:48:51 +0100 Subject: [PATCH 034/128] Feat/waypoint manager (#645) * waypoint mode handling * update tests with new message * initial package setup * ros interface function declaration * node setup * working prototype * reentrant cb, multithreaded * single threaded impl * conv threshold action goal * default thresholdref conv value * removed switching logic * removed timer execution * sim test utils * waypoint_manager_test setup * no rendering test arg * waypoint tests, timeout error * test refactor * format * rename utils package * test suite and description * first waypoint test * removed unused function * renamed service field to priority. Added simple tests * waypoint manager readme * uniform attitude naming convention * fix pr requests * update tests with new service fields * four corner test * update util func name * update with new action def * removed failing build type * test dependencies * ignore failing yasmin package * remove __init__ files * quat_to_euler in make_pose helper * added __init__ file * removed sim deps for test packages * added action shutdown handling * removed waypoint manager set setup * added waypoint manager node tests * waypoint manager 4 corner sim test * added missing launch testing test dependency * add sleep for topic discovery * fix action member field name * removed unnecessary if here --- .github/workflows/simulator-test.yml | 3 +- .gitignore | 3 - .../reference_filter_ros.hpp | 5 +- .../src/reference_filter_ros.cpp | 78 +++- mission/FSM/docking/COLCON_IGNORE | 2 + mission/FSM/docking/src/docking.cpp | 72 ++-- mission/waypoint_manager/CMakeLists.txt | 69 ++++ mission/waypoint_manager/README.md | 84 ++++ .../waypoint_manager/waypoint_manager_ros.hpp | 120 ++++++ .../launch/waypoint_manager.launch.py | 14 + mission/waypoint_manager/package.xml | 28 ++ .../src/waypoint_manager_ros.cpp | 378 ++++++++++++++++++ mission/waypoint_manager/test/CMakeLists.txt | 16 + mission/waypoint_manager/test/test_service.py | 153 +++++++ .../test/test_single_waypoint.py | 147 +++++++ .../reference_filter_node_test.sh | 5 +- .../waypoint_manager_test/check_goal.py | 103 +++++ .../waypoint_manager_test/send_goal.py | 91 +++++ .../waypoint_manager_test/simulator_test.sh | 115 ++++++ .../waypoint_navigation/send_goal.py | 15 +- 20 files changed, 1445 insertions(+), 56 deletions(-) create mode 100644 mission/FSM/docking/COLCON_IGNORE create mode 100644 mission/waypoint_manager/CMakeLists.txt create mode 100644 mission/waypoint_manager/README.md create mode 100644 mission/waypoint_manager/include/waypoint_manager/waypoint_manager_ros.hpp create mode 100644 mission/waypoint_manager/launch/waypoint_manager.launch.py create mode 100644 mission/waypoint_manager/package.xml create mode 100644 mission/waypoint_manager/src/waypoint_manager_ros.cpp create mode 100644 mission/waypoint_manager/test/CMakeLists.txt create mode 100644 mission/waypoint_manager/test/test_service.py create mode 100644 mission/waypoint_manager/test/test_single_waypoint.py mode change 100644 => 100755 tests/ros_node_tests/reference_filter_node_test.sh create mode 100644 tests/simulator_tests/waypoint_manager_test/check_goal.py create mode 100644 tests/simulator_tests/waypoint_manager_test/send_goal.py create mode 100755 tests/simulator_tests/waypoint_manager_test/simulator_test.sh diff --git a/.github/workflows/simulator-test.yml b/.github/workflows/simulator-test.yml index e73d03ac5..052af48de 100644 --- a/.github/workflows/simulator-test.yml +++ b/.github/workflows/simulator-test.yml @@ -16,5 +16,6 @@ jobs: setup_script: "tests/setup.sh" test_scripts: '[ "tests/simulator_tests/waypoint_navigation/simulator_test.sh", - "tests/simulator_tests/los_test/simulator_test.sh" + "tests/simulator_tests/los_test/simulator_test.sh", + "tests/simulator_tests/waypoint_manager_test/simulator_test.sh" ]' diff --git a/.gitignore b/.gitignore index 2a085ac8d..52ae0a3c0 100644 --- a/.gitignore +++ b/.gitignore @@ -56,7 +56,4 @@ qtcreator-* # Emacs .#* -# Catkin custom files -COLCON_IGNORE - # End of https://www.gitignore.io/api/ros diff --git a/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter_ros.hpp b/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter_ros.hpp index 6bc1584f1..2fb46649e 100644 --- a/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter_ros.hpp +++ b/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter_ros.hpp @@ -74,8 +74,9 @@ class ReferenceFilterNode : public rclcpp::Node { Eigen::Vector18d fill_reference_state(); - Eigen::Vector6d fill_reference_goal( - const geometry_msgs::msg::PoseStamped& goal); + Eigen::Vector6d fill_reference_goal(const geometry_msgs::msg::Pose& goal); + + Eigen::Vector6d apply_mode_logic(const Eigen::Vector6d& r_in, uint8_t mode); vortex_msgs::msg::ReferenceFilter fill_reference_msg(); diff --git a/guidance/reference_filter_dp/src/reference_filter_ros.cpp b/guidance/reference_filter_dp/src/reference_filter_ros.cpp index 02d463c0f..614e10e93 100644 --- a/guidance/reference_filter_dp/src/reference_filter_ros.cpp +++ b/guidance/reference_filter_dp/src/reference_filter_ros.cpp @@ -195,12 +195,12 @@ Eigen::Vector18d ReferenceFilterNode::fill_reference_state() { } Eigen::Vector6d ReferenceFilterNode::fill_reference_goal( - const geometry_msgs::msg::PoseStamped& goal) { - double x{goal.pose.position.x}; - double y{goal.pose.position.y}; - double z{goal.pose.position.z}; + const geometry_msgs::msg::Pose& goal) { + double x{goal.position.x}; + double y{goal.position.y}; + double z{goal.position.z}; - const auto& o = goal.pose.orientation; + const auto& o = goal.orientation; Eigen::Quaterniond q(o.w, o.x, o.y, o.z); Eigen::Vector3d euler_angles = vortex::utils::math::quat_to_euler(q); double roll{euler_angles(0)}; @@ -231,6 +231,43 @@ vortex_msgs::msg::ReferenceFilter ReferenceFilterNode::fill_reference_msg() { return feedback_msg; } +Eigen::Vector6d ReferenceFilterNode::apply_mode_logic( + const Eigen::Vector6d& r_in, + uint8_t mode) { + Eigen::Vector6d r_out = r_in; + + switch (mode) { + case vortex_msgs::msg::Waypoint::FULL_POSE: + break; + + case vortex_msgs::msg::Waypoint::ONLY_POSITION: + r_out(3) = x_(3); + r_out(4) = x_(4); + r_out(5) = x_(5); + break; + + case vortex_msgs::msg::Waypoint::FORWARD_HEADING: { + double dx = r_in(0) - x_(0); + double dy = r_in(1) - x_(1); + + double forward_heading = std::atan2(dy, dx); + + r_out(3) = 0.0; + r_out(4) = 0.0; + r_out(5) = vortex::utils::math::ssa(forward_heading); + break; + } + + case vortex_msgs::msg::Waypoint::ONLY_ORIENTATION: + r_out(0) = x_(0); + r_out(1) = x_(1); + r_out(2) = x_(2); + break; + } + + return r_out; +} + void ReferenceFilterNode::execute( const std::shared_ptr> goal_handle) { @@ -243,9 +280,21 @@ void ReferenceFilterNode::execute( x_ = fill_reference_state(); - const geometry_msgs::msg::PoseStamped goal = goal_handle->get_goal()->goal; + const geometry_msgs::msg::Pose goal = + goal_handle->get_goal()->waypoint.pose; + uint8_t mode = goal_handle->get_goal()->waypoint.mode; + double convergence_threshold = + goal_handle->get_goal()->convergence_threshold; + + if (convergence_threshold <= 0.0) { + convergence_threshold = 0.1; + spdlog::warn( + "ReferenceFilter: Invalid convergence_threshold received (<= 0). " + "Using default 0.1"); + } - r_ = fill_reference_goal(goal); + Eigen::Vector6d r_temp = fill_reference_goal(goal); + r_ = apply_mode_logic(r_temp, mode); auto feedback = std::make_shared< vortex_msgs::action::ReferenceFilterWaypoint::Feedback>(); @@ -274,12 +323,12 @@ void ReferenceFilterNode::execute( vortex_msgs::msg::ReferenceFilter feedback_msg = fill_reference_msg(); - feedback->feedback = feedback_msg; + feedback->reference = feedback_msg; goal_handle->publish_feedback(feedback); reference_pub_->publish(feedback_msg); - if ((x_.head(6) - r_.head(6)).norm() < 0.1) { + if ((x_.head(6) - r_.head(6)).norm() < convergence_threshold) { result->success = true; goal_handle->succeed(result); x_.head(6) = r_.head(6); @@ -292,6 +341,17 @@ void ReferenceFilterNode::execute( loop_rate.sleep(); } + if (!rclcpp::ok() && goal_handle->is_active()) { + auto result = std::make_shared< + vortex_msgs::action::ReferenceFilterWaypoint::Result>(); + result->success = false; + + try { + goal_handle->abort(result); + } catch (...) { + // Ignore exceptions during shutdown + } + } } RCLCPP_COMPONENTS_REGISTER_NODE(ReferenceFilterNode) diff --git a/mission/FSM/docking/COLCON_IGNORE b/mission/FSM/docking/COLCON_IGNORE new file mode 100644 index 000000000..f4db3a26d --- /dev/null +++ b/mission/FSM/docking/COLCON_IGNORE @@ -0,0 +1,2 @@ +# Ignored temporarily to skip building while debugging yasmin upgrade +# Remove this file to include the package in colcon builds again diff --git a/mission/FSM/docking/src/docking.cpp b/mission/FSM/docking/src/docking.cpp index 4476d90a1..c251dda75 100644 --- a/mission/FSM/docking/src/docking.cpp +++ b/mission/FSM/docking/src/docking.cpp @@ -79,16 +79,18 @@ ApproachDockingStationState::create_goal_handler( blackboard->set("is_home", false); - docking_fsm::PoseStamped docking_offset_goal = - blackboard->get("docking_offset_goal"); + docking_fsm::Pose docking_offset_goal = + blackboard->get("docking_offset_goal").pose; + + vortex_msgs::msg::Waypoint waypoint; + waypoint.pose = docking_offset_goal; - goal.goal = docking_offset_goal; + goal.waypoint = waypoint; spdlog::info("Goal sent to action server:"); spdlog::info(" Position: x = {}, y = {}, z = {}", - docking_offset_goal.pose.position.x, - docking_offset_goal.pose.position.y, - docking_offset_goal.pose.position.z); + docking_offset_goal.position.x, docking_offset_goal.position.y, + docking_offset_goal.position.z); return goal; } @@ -113,15 +115,15 @@ void ApproachDockingStationState::print_feedback( std::shared_ptr feedback) { docking_fsm::Pose current_pose = docking_fsm::Pose(); - current_pose.position.x = feedback->feedback.x; - current_pose.position.y = feedback->feedback.y; - current_pose.position.z = feedback->feedback.z; + current_pose.position.x = feedback->reference.x; + current_pose.position.y = feedback->reference.y; + current_pose.position.z = feedback->reference.z; blackboard->set("current_pose", current_pose); spdlog::debug("Current position: x = {}, y = {}, z = {}", - feedback->feedback.x, feedback->feedback.y, - feedback->feedback.z); + feedback->reference.x, feedback->reference.y, + feedback->reference.z); } GoAboveDockingStationState::GoAboveDockingStationState( @@ -145,7 +147,9 @@ GoAboveDockingStationState::create_goal_handler( auto docking_offset_goal = blackboard->get("docking_offset_goal"); - goal.goal = docking_offset_goal; + vortex_msgs::msg::Waypoint waypoint; + waypoint.pose = docking_offset_goal.pose; + goal.waypoint = waypoint; spdlog::info("Goal sent to action server:"); spdlog::info(" Position: x = {}, y = {}, z = {}", @@ -179,15 +183,15 @@ void GoAboveDockingStationState::print_feedback( std::shared_ptr feedback) { docking_fsm::Pose current_pose = docking_fsm::Pose(); - current_pose.position.x = feedback->feedback.x; - current_pose.position.y = feedback->feedback.y; - current_pose.position.z = feedback->feedback.z; + current_pose.position.x = feedback->reference.x; + current_pose.position.y = feedback->reference.y; + current_pose.position.z = feedback->reference.z; blackboard->set("current_pose", current_pose); spdlog::debug("Current position: x = {}, y = {}, z = {}", - feedback->feedback.x, feedback->feedback.y, - feedback->feedback.z); + feedback->reference.x, feedback->reference.y, + feedback->reference.z); } ConvergeDockingStationState::ConvergeDockingStationState( @@ -213,7 +217,9 @@ ConvergeDockingStationState::create_goal_handler( docking_fsm::PoseStamped dock_pose = blackboard->get("dock_pose"); - goal.goal = dock_pose; + vortex_msgs::msg::Waypoint waypoint; + waypoint.pose = dock_pose.pose; + goal.waypoint = waypoint; spdlog::info("Goal sent to action server:"); spdlog::info(" Position: x = {}, y = {}, z = {}", @@ -242,14 +248,14 @@ void ConvergeDockingStationState::print_feedback( std::shared_ptr feedback) { docking_fsm::Pose current_pose = docking_fsm::Pose(); - current_pose.position.x = feedback->feedback.x; - current_pose.position.y = feedback->feedback.y; - current_pose.position.z = feedback->feedback.z; + current_pose.position.x = feedback->reference.x; + current_pose.position.y = feedback->reference.y; + current_pose.position.z = feedback->reference.z; blackboard->set("current_pose", current_pose); spdlog::debug("Current position: x = {}, y = {}, z = {}", - feedback->feedback.x, feedback->feedback.y, - feedback->feedback.z); + feedback->reference.x, feedback->reference.y, + feedback->reference.z); } std::string DockedState( @@ -286,7 +292,9 @@ docking_fsm::ReturnHomeAction::Goal ReturnHomeState::create_goal_handler( docking_fsm::PoseStamped start_pose = blackboard->get("start_pose"); - goal.goal = start_pose; + vortex_msgs::msg::Waypoint waypoint; + waypoint.pose = start_pose.pose; + goal.waypoint = waypoint; spdlog::info("Goal sent to action server:"); spdlog::info(" Position: x = {}, y = {}, z = {}", start_pose.pose.position.x, start_pose.pose.position.y, @@ -313,17 +321,17 @@ void ReturnHomeState::print_feedback( std::shared_ptr blackboard, std::shared_ptr feedback) { docking_fsm::Pose current_pose = docking_fsm::Pose(); - current_pose.position.x = feedback->feedback.x; - current_pose.position.y = feedback->feedback.y; - current_pose.position.z = feedback->feedback.z; - current_pose.orientation.x = feedback->feedback.roll; - current_pose.orientation.y = feedback->feedback.pitch; - current_pose.orientation.z = feedback->feedback.yaw; + current_pose.position.x = feedback->reference.x; + current_pose.position.y = feedback->reference.y; + current_pose.position.z = feedback->reference.z; + current_pose.orientation.x = feedback->reference.roll; + current_pose.orientation.y = feedback->reference.pitch; + current_pose.orientation.z = feedback->reference.yaw; blackboard->set("current_pose", current_pose); spdlog::debug("Current position: x = {}, y = {}, z = {}", - feedback->feedback.x, feedback->feedback.y, - feedback->feedback.z); + feedback->reference.x, feedback->reference.y, + feedback->reference.z); } std::string AbortState( diff --git a/mission/waypoint_manager/CMakeLists.txt b/mission/waypoint_manager/CMakeLists.txt new file mode 100644 index 000000000..0a76ae510 --- /dev/null +++ b/mission/waypoint_manager/CMakeLists.txt @@ -0,0 +1,69 @@ +cmake_minimum_required(VERSION 3.8) +project(waypoint_manager) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 20) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(vortex_msgs REQUIRED) +find_package(vortex_utils REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(spdlog REQUIRED) +find_package(fmt REQUIRED) + +include_directories(include) + +set(LIB_NAME "${PROJECT_NAME}_component") + +add_library(${LIB_NAME} SHARED + src/waypoint_manager_ros.cpp) + +ament_target_dependencies(${LIB_NAME} PUBLIC + rclcpp + rclcpp_components + rclcpp_action + vortex_msgs + vortex_utils + tf2_geometry_msgs + spdlog + fmt +) + +rclcpp_components_register_node( + ${LIB_NAME} + PLUGIN "WaypointManagerNode" + EXECUTABLE ${PROJECT_NAME}_node +) + +ament_export_targets(export_${LIB_NAME}) + +install(TARGETS ${LIB_NAME} + EXPORT export_${LIB_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install( + DIRECTORY include/ + DESTINATION include +) + +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}/ +) + +if(BUILD_TESTING) + add_subdirectory(test) +endif() + +ament_package() diff --git a/mission/waypoint_manager/README.md b/mission/waypoint_manager/README.md new file mode 100644 index 000000000..3e6c6411a --- /dev/null +++ b/mission/waypoint_manager/README.md @@ -0,0 +1,84 @@ + + +# Waypoint Manager — ROS 2 Node + + +The **Waypoint Manager** node coordinates mission-level navigation by managing waypoint queues, forwarding them to a **Reference Filter** for trajectory generation, and exposing both an **action interface** (for mission planners) and a **service interface** (for perception-driven waypoint updates). + + + +# Interfaces + +* **[WaypointManager](https://github.com/vortexntnu/vortex-msgs/blob/main/action/WaypointManager.action) (action server)** +* **[WaypointAddition](https://github.com/vortexntnu/vortex-msgs/blob/main/srv/WaypointAddition.srv) (service server)** +* **[ReferenceFilterWaypoint](https://github.com/vortexntnu/vortex-msgs/blob/main/action/ReferenceFilterWaypoint.action) (action client)** + + +# Behavior Summary + +### **Mission Start** + +* On receiving a new action goal, any existing mission is aborted. +* Waypoints are stored, state is reset, and the first waypoint is sent to the reference filter. + +### **During Mission** + +* Each waypoint is executed sequentially. +* Pose feedback from the reference filter is forwarded to the mission planner. +* `convergence_threshold` determines when a waypoint is reached. +* If `persistent = true`, the action does not end even when waypoints run out. + +### **Mission End** + +* If `persistent = false`, the action completes when the last waypoint is reached. +* If cancelled externally, all state is cleared and reference filter goals are cancelled. + +### **Dynamic Waypoint Addition** + +Allowed only during **persistent** missions: + +* `overwrite = true` → clears old queue and restarts from new waypoints +* `overwrite = false` → appends waypoints to queue +* `priority = true` → turn on priority mode. In this mode additional service requests with `priority = false` are ignored. Priority mode is set to false again when there are no more waypoints in queue. + +--- + +### Check available interfaces: + +```bash +ros2 action list +ros2 service list +``` +beware of namespace! + + +## Example Action Goal (CLI) + +```bash +ros2 action send_goal /orca/waypoint_manager vortex_msgs/action/WaypointManager \ +'{ + waypoints:[ + { + pose:{position:{x:5.0,y:0.0,z:0.0}, + orientation:{x:0,y:0,z:0,w:1}}, + mode:1 + } + ], + convergence_threshold:0.1, + persistent:false +}' +``` + + +## Example Waypoint Addition Service Call + +```bash +ros2 service call /orca/waypoint_addition vortex_msgs/srv/WaypointAddition \ +'{ + waypoints:[ + {pose:{position:{x:2,y:3,z:0},orientation:{x:0,y:0,z:0,w:1}},mode:1} + ], + overwrite:false, + priority:true +}' +``` diff --git a/mission/waypoint_manager/include/waypoint_manager/waypoint_manager_ros.hpp b/mission/waypoint_manager/include/waypoint_manager/waypoint_manager_ros.hpp new file mode 100644 index 000000000..86ba42d3e --- /dev/null +++ b/mission/waypoint_manager/include/waypoint_manager/waypoint_manager_ros.hpp @@ -0,0 +1,120 @@ +#ifndef WAYPOINT_MANAGER__WAYPOINT_MANAGER_ROS_HPP_ +#define WAYPOINT_MANAGER__WAYPOINT_MANAGER_ROS_HPP_ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace vortex::mission { + +using WaypointManager = vortex_msgs::action::WaypointManager; +using WaypointManagerGoalHandle = + rclcpp_action::ServerGoalHandle; + +using ReferenceFilterAction = vortex_msgs::action::ReferenceFilterWaypoint; +using ReferenceFilterGoalHandle = + rclcpp_action::ClientGoalHandle; + +class WaypointManagerNode : public rclcpp::Node { + public: + explicit WaypointManagerNode( + const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); + + ~WaypointManagerNode() override; + + private: + // @brief Create the action server for WaypointManager. + void set_waypoint_action_server(); + + // @brief Create the action client for ReferenceFilterWaypoint. + void set_reference_action_client(); + + // @brief Create the service servers for SendWaypoints. + void set_waypoint_service_server(); + + // @brief Construct the result message for the WaypointManager action + // @param success Whether the action was successful + // @return The constructed result message + std::shared_ptr + construct_result(bool success) const; + + // @brief Clean up the mission state after completion or cancellation of a + // waypoint action. Cancel active goals and reset internal variables. Make + // system ready for next action. + void cleanup_mission_state(); + + // @brief Send the next goal to the ReferenceFilter action server based on + // the current waypoint index or finish the waypoint action if all waypoints + // have been processed. + void send_next_reference_filter_goal(); + + // @brief Handle incoming action goal requests + // @param uuid The goal UUID + // @param goal The goal message + // @return The goal response + rclcpp_action::GoalResponse handle_waypoint_goal( + const rclcpp_action::GoalUUID& uuid, + std::shared_ptr + goal_msg); + + // @brief Handle requests to cancel the waypoint action + // @param goal_handle The goal handle + // @return The cancel response + rclcpp_action::CancelResponse handle_waypoint_cancel( + const std::shared_ptr> goal_handle); + + // @brief Handle the accepted goal request + // @param goal_handle The goal handle + void handle_waypoint_accepted( + const std::shared_ptr> goal_handle); + + // @brief Handle incoming send waypoints service requests + // Only accepted if waypoint action is running. + // @param request Incoming service request containing waypoint information. + // @param response Service response that should be populated and sent back + // to the caller. + void handle_send_waypoints_service_request( + const std::shared_ptr request, + std::shared_ptr response); + + // @brief Send a goal to the reference filter + // @param goal_msg The action goal + void send_reference_filter_goal( + const vortex_msgs::action::ReferenceFilterWaypoint::Goal& goal_msg); + + rclcpp_action::Client:: + SharedPtr reference_filter_client_; + rclcpp_action::Server::SharedPtr + waypoint_action_server_; + rclcpp::Service::SharedPtr + waypoint_service_server_; + + std::vector waypoints_{}; + std::size_t current_index_{0}; + double convergence_threshold_{0.1}; + + bool persistent_action_mode_active_{false}; + bool priority_mode_active_{false}; + + ReferenceFilterAction::Feedback latest_ref_feedback_; + bool has_reference_pose_{false}; + bool is_cancel_in_progress_{false}; + + std::uint64_t mission_id_ = 0; + + std::shared_ptr active_reference_filter_goal_; + std::shared_ptr active_action_goal_; +}; + +} // namespace vortex::mission + +#endif // WAYPOINT_MANAGER__WAYPOINT_MANAGER_ROS_HPP_ diff --git a/mission/waypoint_manager/launch/waypoint_manager.launch.py b/mission/waypoint_manager/launch/waypoint_manager.launch.py new file mode 100644 index 000000000..65286ceb0 --- /dev/null +++ b/mission/waypoint_manager/launch/waypoint_manager.launch.py @@ -0,0 +1,14 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + waypoint_manager_node = Node( + package='waypoint_manager', + executable='waypoint_manager_node', + name='waypoint_manager_node', + namespace='orca', + parameters=[], + output='screen', + ) + return LaunchDescription([waypoint_manager_node]) diff --git a/mission/waypoint_manager/package.xml b/mission/waypoint_manager/package.xml new file mode 100644 index 000000000..db3feabde --- /dev/null +++ b/mission/waypoint_manager/package.xml @@ -0,0 +1,28 @@ + + + + waypoint_manager + 0.0.0 + Waypoint manager for waypoint navigation with dp reference filter + Jorgen Fjermedal + MIT + + ament_cmake + + rclcpp + rclcpp_action + geometry_msgs + vortex_msgs + vortex_utils + tf2_geometry_msgs + + launch_testing + launch_testing_ros + launch_testing_ament_cmake + reference_filter_dp + auv_setup + + + ament_cmake + + diff --git a/mission/waypoint_manager/src/waypoint_manager_ros.cpp b/mission/waypoint_manager/src/waypoint_manager_ros.cpp new file mode 100644 index 000000000..1d4928470 --- /dev/null +++ b/mission/waypoint_manager/src/waypoint_manager_ros.cpp @@ -0,0 +1,378 @@ +#include "waypoint_manager/waypoint_manager_ros.hpp" +#include +#include +#include +#include + +namespace vortex::mission { + +WaypointManagerNode::WaypointManagerNode(const rclcpp::NodeOptions& options) + : Node("waypoint_manager_node", options) { + set_reference_action_client(); + set_waypoint_action_server(); + set_waypoint_service_server(); + + spdlog::info("WaypointManagerNode started"); +} + +WaypointManagerNode::~WaypointManagerNode() { + if (active_action_goal_ && (active_action_goal_->is_active() || + active_action_goal_->is_canceling())) { + try { + auto res = construct_result(false); + active_action_goal_->abort(res); + } catch (...) { + } + } + + if (active_reference_filter_goal_) { + try { + reference_filter_client_->async_cancel_goal( + active_reference_filter_goal_); + } catch (...) { + } + active_reference_filter_goal_.reset(); + } +} + +// --------------------------------------------------------- +// SETUP INTERFACES +// --------------------------------------------------------- + +void WaypointManagerNode::set_reference_action_client() { + reference_filter_client_ = + rclcpp_action::create_client(this, + "reference_filter"); + + if (!reference_filter_client_->wait_for_action_server( + std::chrono::seconds(3))) { + spdlog::warn("ReferenceFilter server not ready"); + } +} + +void WaypointManagerNode::set_waypoint_action_server() { + waypoint_action_server_ = rclcpp_action::create_server( + this, "waypoint_manager", + + [this](auto goal_id, auto goal) { + return handle_waypoint_goal(goal_id, goal); + }, + + [this](auto goal_id) { return handle_waypoint_cancel(goal_id); }, + + [this](auto goal_handle) { + return handle_waypoint_accepted(goal_handle); + }); +} + +void WaypointManagerNode::set_waypoint_service_server() { + waypoint_service_server_ = + this->create_service( + "waypoint_addition", + std::bind( + &WaypointManagerNode::handle_send_waypoints_service_request, + this, std::placeholders::_1, std::placeholders::_2)); +} + +// --------------------------------------------------------- +// HELPERS +// --------------------------------------------------------- + +std::shared_ptr +WaypointManagerNode::construct_result(bool success) const { + auto result = + std::make_shared(); + result->success = success; + result->pose_valid = has_reference_pose_; + if (has_reference_pose_) { + result->final_pose = + vortex::utils::ros_conversions::pose_like_to_pose_msg( + latest_ref_feedback_.reference); + } + return result; +} + +void WaypointManagerNode::cleanup_mission_state() { + waypoints_.clear(); + current_index_ = 0; + persistent_action_mode_active_ = false; + priority_mode_active_ = false; + has_reference_pose_ = false; + + if (active_reference_filter_goal_) { + reference_filter_client_->async_cancel_goal( + active_reference_filter_goal_); + active_reference_filter_goal_.reset(); + } + + active_action_goal_.reset(); +} + +void WaypointManagerNode::send_next_reference_filter_goal() { + if (current_index_ >= waypoints_.size()) { + if (!persistent_action_mode_active_ && active_action_goal_ && + active_action_goal_->is_active()) { + auto wm_res = construct_result(true); + active_action_goal_->succeed(wm_res); + cleanup_mission_state(); + } + return; + } + + ReferenceFilterAction::Goal rf_goal; + rf_goal.waypoint = waypoints_[current_index_]; + rf_goal.convergence_threshold = convergence_threshold_; + + send_reference_filter_goal(rf_goal); +} + +// --------------------------------------------------------- +// WAYPOINT MANAGER ACTION SERVER +// --------------------------------------------------------- + +rclcpp_action::GoalResponse WaypointManagerNode::handle_waypoint_goal( + const rclcpp_action::GoalUUID& /*goal_uuid*/, + std::shared_ptr goal) { + if (active_action_goal_ && active_action_goal_->is_active()) { + auto wp_res = construct_result(false); + active_action_goal_->abort(wp_res); + } + + if (active_reference_filter_goal_) { + reference_filter_client_->async_cancel_goal( + active_reference_filter_goal_); + active_reference_filter_goal_.reset(); + } + + ++mission_id_; + + waypoints_ = goal->waypoints; + current_index_ = 0; + persistent_action_mode_active_ = goal->persistent; + priority_mode_active_ = false; + has_reference_pose_ = false; + convergence_threshold_ = goal->convergence_threshold; + + if (waypoints_.empty() && !persistent_action_mode_active_) { + spdlog::warn( + "WaypointManager: received empty waypoint list and non-persistent " + "mode"); + return rclcpp_action::GoalResponse::REJECT; + } + + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +void WaypointManagerNode::handle_waypoint_accepted( + const std::shared_ptr goal_handle) { + spdlog::info("WaypointManager: action goal accepted"); + active_action_goal_ = goal_handle; + + send_next_reference_filter_goal(); +} + +rclcpp_action::CancelResponse WaypointManagerNode::handle_waypoint_cancel( + const std::shared_ptr /*goal_handle*/) { + spdlog::info("WaypointManagerAction: cancel requested"); + + if (active_reference_filter_goal_) { + reference_filter_client_->async_cancel_goal( + active_reference_filter_goal_); + active_reference_filter_goal_.reset(); + } + + return rclcpp_action::CancelResponse::ACCEPT; +} + +// --------------------------------------------------------- +// WAYPOINT MANAGER SERVICE SERVER +// --------------------------------------------------------- + +void WaypointManagerNode::handle_send_waypoints_service_request( + const std::shared_ptr request, + std::shared_ptr response) { + if (!persistent_action_mode_active_ || !active_action_goal_ || + !active_action_goal_->is_active()) { + response->success = false; + return; + } + + if (priority_mode_active_ && !request->take_priority && + current_index_ < waypoints_.size()) { + response->success = false; + return; + } + + priority_mode_active_ = request->take_priority; + + if (request->overwrite_prior_waypoints) { + waypoints_ = request->waypoints; + current_index_ = 0; + has_reference_pose_ = false; + + if (active_reference_filter_goal_) { + reference_filter_client_->async_cancel_goal( + active_reference_filter_goal_); + active_reference_filter_goal_.reset(); + } + + if (!waypoints_.empty()) { + send_next_reference_filter_goal(); + } + + response->success = true; + return; + } + + waypoints_.insert(waypoints_.end(), request->waypoints.begin(), + request->waypoints.end()); + + if (!active_reference_filter_goal_ && current_index_ < waypoints_.size()) { + send_next_reference_filter_goal(); + } + + response->success = true; +} + +// --------------------------------------------------------- +// REFERENCE FILTER ACTION CLIENT +// --------------------------------------------------------- + +void WaypointManagerNode::send_reference_filter_goal( + const ReferenceFilterAction::Goal& goal_msg) { + if (active_reference_filter_goal_) { + reference_filter_client_->async_cancel_goal( + active_reference_filter_goal_); + active_reference_filter_goal_.reset(); + } + + const std::uint64_t this_mission = mission_id_; + + rclcpp_action::Client::SendGoalOptions options; + + options.goal_response_callback = + [this, this_mission](ReferenceFilterGoalHandle::SharedPtr gh) { + if (!gh) { + spdlog::warn("ReferenceFilter goal rejected"); + return; + } + + if (this_mission == mission_id_) { + active_reference_filter_goal_ = gh; + } else { + spdlog::info( + "RF goal response for old mission, ignoring handle"); + } + }; + + options.feedback_callback = + [this, this_mission]( + ReferenceFilterGoalHandle::SharedPtr, + const std::shared_ptr fb) { + if (this_mission != mission_id_) { + return; + } + + latest_ref_feedback_ = *fb; + has_reference_pose_ = true; + + if (!active_action_goal_ || !active_action_goal_->is_active()) + return; + + geometry_msgs::msg::Pose robot_pose = + vortex::utils::ros_conversions::pose_like_to_pose_msg( + fb->reference); + + if (current_index_ < waypoints_.size()) { + auto wm_fb = std::make_shared(); + wm_fb->current_pose = robot_pose; + wm_fb->current_waypoint = waypoints_[current_index_]; + active_action_goal_->publish_feedback(wm_fb); + } + }; + + options + .result_callback = [this, this_mission]( + const ReferenceFilterGoalHandle::WrappedResult& + res) { + if (this_mission != mission_id_) { + spdlog::info( + "ReferenceFilter result received for old mission, ignoring."); + return; + } + + active_reference_filter_goal_.reset(); + + if (!active_action_goal_) { + spdlog::info( + "ReferenceFilter result received but no active WM goal"); + return; + } + + const bool wm_canceling = active_action_goal_->is_canceling(); + const bool wm_active = active_action_goal_->is_active(); + + switch (res.code) { + case rclcpp_action::ResultCode::SUCCEEDED: { + spdlog::info("ReferenceFilter goal reached waypoint"); + + if (wm_canceling) { + bool action_success = false; + if (persistent_action_mode_active_ && + current_index_ >= waypoints_.size()) { + action_success = true; + } + + auto wp_res = construct_result(action_success); + + active_action_goal_->canceled(wp_res); + + cleanup_mission_state(); + } else { + current_index_++; + send_next_reference_filter_goal(); + } + break; + } + + case rclcpp_action::ResultCode::CANCELED: { + spdlog::info("ReferenceFilter goal cancelled"); + + if (wm_canceling && wm_active) { + bool action_success = false; + if (persistent_action_mode_active_ && + current_index_ >= waypoints_.size()) { + action_success = true; + } + + auto wp_res = construct_result(action_success); + + active_action_goal_->canceled(wp_res); + cleanup_mission_state(); + } + break; + } + + case rclcpp_action::ResultCode::ABORTED: { + spdlog::warn("ReferenceFilter goal aborted unexpectedly"); + if (wm_active) { + auto wp_res = construct_result(false); + active_action_goal_->abort(wp_res); + cleanup_mission_state(); + } + break; + } + + default: + spdlog::error( + "ReferenceFilter goal returned unknown result code"); + break; + } + }; + + reference_filter_client_->async_send_goal(goal_msg, options); +} + +RCLCPP_COMPONENTS_REGISTER_NODE(WaypointManagerNode) + +} // namespace vortex::mission diff --git a/mission/waypoint_manager/test/CMakeLists.txt b/mission/waypoint_manager/test/CMakeLists.txt new file mode 100644 index 000000000..d63481816 --- /dev/null +++ b/mission/waypoint_manager/test/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.8) +project(waypoint_manager_test) + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) + +find_package(ament_cmake_ros REQUIRED) +find_package(launch_testing_ament_cmake REQUIRED) + +function(add_ros_isolated_launch_test path) + set(RUNNER "${ament_cmake_ros_DIR}/run_test_isolated.py") + add_launch_test("${path}" RUNNER "${RUNNER}" ${ARGN}) +endfunction() + +add_ros_isolated_launch_test(test_single_waypoint.py TIMEOUT 300) +add_ros_isolated_launch_test(test_service.py TIMEOUT 300) diff --git a/mission/waypoint_manager/test/test_service.py b/mission/waypoint_manager/test/test_service.py new file mode 100644 index 000000000..6bc827b97 --- /dev/null +++ b/mission/waypoint_manager/test/test_service.py @@ -0,0 +1,153 @@ +import os +import unittest +import uuid + +import launch +import launch_ros.actions +import launch_testing +import launch_testing.actions +import rclpy +from ament_index_python.packages import get_package_share_directory +from rclpy.action import ActionClient +from vortex_msgs.action import WaypointManager +from vortex_msgs.msg import Waypoint +from vortex_msgs.srv import SendWaypoints + + +def generate_test_description(): + rf_pkg_share = get_package_share_directory('reference_filter_dp') + rf_config = os.path.join(rf_pkg_share, 'config', 'reference_filter_params.yaml') + + auv_setup_share = get_package_share_directory('auv_setup') + orca_config = os.path.join(auv_setup_share, 'config', 'robots', 'orca.yaml') + + wm_node = launch_ros.actions.Node( + package='waypoint_manager', + executable='waypoint_manager_node', + name='waypoint_manager_node', + namespace='orca', + output='screen', + ) + + rf_node = launch_ros.actions.Node( + package='reference_filter_dp', + executable='reference_filter_dp_node', + name='reference_filter_node', + namespace='orca', + parameters=[rf_config, orca_config], + output='screen', + ) + + return launch.LaunchDescription( + [ + wm_node, + rf_node, + launch_testing.actions.ReadyToTest(), + ] + ) + + +class TestWaypointManagerService(unittest.TestCase): + @classmethod + def setUpClass(cls): + rclpy.init() + + @classmethod + def tearDownClass(cls): + rclpy.shutdown() + + def setUp(self): + self.node = rclpy.create_node( + f'test_waypoint_manager_client_{uuid.uuid4().hex[:8]}' + ) + + def tearDown(self): + self.node.destroy_node() + + def _call_send_waypoints( + self, + waypoints, + switching_threshold=0.3, + overwrite=False, + take_priority=False, + timeout=5.0, + ): + client = self.node.create_client(SendWaypoints, '/orca/waypoint_addition') + assert client.wait_for_service(timeout_sec=5.0), ( + 'SendWaypoints service not available' + ) + + req = SendWaypoints.Request() + req.waypoints = list(waypoints) + req.switching_threshold = float(switching_threshold) + req.overwrite_prior_waypoints = bool(overwrite) + req.take_priority = bool(take_priority) + + fut = client.call_async(req) + rclpy.spin_until_future_complete(self.node, fut, timeout_sec=timeout) + assert fut.done(), 'Timed out waiting for SendWaypoints response' + return fut.result() + + def test_service_returns_false_when_no_persistent_action(self): + # When no persistent action is active, service should return false + wp = Waypoint() + wp.pose.position.x = 1.0 + wp.pose.position.y = 2.0 + wp.pose.position.z = 1.0 + + resp = self._call_send_waypoints([wp], overwrite=False, take_priority=False) + assert not resp.success, ( + 'Service should return false when no persistent action is active' + ) + + def test_priority_blocks_non_priority_calls(self): + # Send a persistent action goal so the waypoint manager is in persistent mode + action_client = ActionClient( + self.node, WaypointManager, '/orca/waypoint_manager' + ) + assert action_client.wait_for_server(timeout_sec=10.0), ( + 'WaypointManager action server not available' + ) + + goal_msg = WaypointManager.Goal() + wp1 = Waypoint() + wp1.pose.position.x = 0.0 + wp1.pose.position.y = 0.0 + wp1.pose.position.z = 1.0 + wp2 = Waypoint() + wp2.pose.position.x = 1.0 + wp2.pose.position.y = 1.0 + wp2.pose.position.z = 1.0 + + goal_msg.waypoints = [wp1, wp2] + goal_msg.persistent = True + goal_msg.convergence_threshold = 0.3 + + send_fut = action_client.send_goal_async(goal_msg) + rclpy.spin_until_future_complete(self.node, send_fut, timeout_sec=20.0) + assert send_fut.done(), 'Timed out waiting for goal response' + goal_handle = send_fut.result() + assert goal_handle.accepted, 'Goal was rejected' + + # Call service to take priority + resp1 = self._call_send_waypoints([], overwrite=False, take_priority=True) + assert resp1.success, 'Priority service call should succeed' + + # Now a non-priority call while priority is active should be rejected + wp_extra = Waypoint() + wp_extra.pose.position.x = 2.0 + wp_extra.pose.position.y = 2.0 + wp_extra.pose.position.z = 1.0 + + resp2 = self._call_send_waypoints( + [wp_extra], overwrite=False, take_priority=False + ) + assert not resp2.success, ( + 'Non-priority call should be rejected while priority is active' + ) + + +@launch_testing.post_shutdown_test() +class TestAfterShutdown(unittest.TestCase): + def test_exit_codes(self, proc_info): + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/mission/waypoint_manager/test/test_single_waypoint.py b/mission/waypoint_manager/test/test_single_waypoint.py new file mode 100644 index 000000000..35e02b79e --- /dev/null +++ b/mission/waypoint_manager/test/test_single_waypoint.py @@ -0,0 +1,147 @@ +import os +import time +import unittest +import uuid + +import launch +import launch_ros.actions +import launch_testing +import launch_testing.actions +import rclpy +from ament_index_python.packages import get_package_share_directory +from nav_msgs.msg import Odometry +from rclpy.action import ActionClient +from rclpy.qos import qos_profile_sensor_data +from vortex_msgs.action import WaypointManager +from vortex_msgs.msg import Waypoint + + +def generate_test_description(): + rf_pkg_share = get_package_share_directory('reference_filter_dp') + rf_config = os.path.join(rf_pkg_share, 'config', 'reference_filter_params.yaml') + + auv_setup_share = get_package_share_directory('auv_setup') + orca_config = os.path.join(auv_setup_share, 'config', 'robots', 'orca.yaml') + + wm_node = launch_ros.actions.Node( + package='waypoint_manager', + executable='waypoint_manager_node', + name='waypoint_manager_node', + namespace='orca', + output='screen', + ) + + rf_node = launch_ros.actions.Node( + package='reference_filter_dp', + executable='reference_filter_dp_node', + name='reference_filter_node', + namespace='orca', + parameters=[rf_config, orca_config], + output='screen', + ) + + return launch.LaunchDescription( + [ + wm_node, + rf_node, + launch_testing.actions.ReadyToTest(), + ] + ) + + +class TestWaypointManagerAcceptsGoal(unittest.TestCase): + @classmethod + def setUpClass(cls): + rclpy.init() + + @classmethod + def tearDownClass(cls): + rclpy.shutdown() + + def setUp(self): + self.node = rclpy.create_node( + f'test_waypoint_manager_client_{uuid.uuid4().hex[:8]}' + ) + + def tearDown(self): + self.node.destroy_node() + + def _publish_fake_odom(self, x, y, z, duration_sec=5.0, rate_hz=10.0): + pub = self.node.create_publisher( + Odometry, + '/orca/odom', + qos_profile_sensor_data, + ) + + msg = Odometry() + msg.header.frame_id = 'odom' + msg.child_frame_id = 'base_link' + msg.pose.pose.position.x = x + msg.pose.pose.position.y = y + msg.pose.pose.position.z = z + + end_time = time.time() + duration_sec + period = 1.0 / rate_hz + + while time.time() < end_time: + msg.header.stamp = self.node.get_clock().now().to_msg() + pub.publish(msg) + rclpy.spin_once(self.node, timeout_sec=0.1) + time.sleep(period) + + self.node.destroy_publisher(pub) + + def test_accepts_and_executes_goal(self): + client = ActionClient(self.node, WaypointManager, '/orca/waypoint_manager') + + assert client.wait_for_server(timeout_sec=10.0), ( + 'WaypointManager action server not available' + ) + + goal_msg = WaypointManager.Goal() + wp = Waypoint() + wp.pose.position.x = 0.0 + wp.pose.position.y = 0.0 + wp.pose.position.z = 1.0 + + goal_msg.waypoints = [wp] + goal_msg.persistent = False + goal_msg.convergence_threshold = 0.3 + + send_fut = client.send_goal_async(goal_msg) + rclpy.spin_until_future_complete( + self.node, + send_fut, + timeout_sec=20.0, + ) + + assert send_fut.done(), 'Timed out waiting for goal response' + + goal_handle = send_fut.result() + assert goal_handle.accepted, 'Goal was rejected' + + # Publish fake odometry at the goal position + self._publish_fake_odom( + x=wp.pose.position.x, + y=wp.pose.position.y, + z=wp.pose.position.z, + duration_sec=5.0, + ) + + result_fut = goal_handle.get_result_async() + rclpy.spin_until_future_complete( + self.node, + result_fut, + timeout_sec=20.0, + ) + + assert result_fut.done(), 'Timed out waiting for result' + + result = result_fut.result().result + assert result.success, 'Waypoint execution failed' + + +@launch_testing.post_shutdown_test() +class TestAfterShutdown(unittest.TestCase): + def test_exit_codes(self, proc_info): + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/tests/ros_node_tests/reference_filter_node_test.sh b/tests/ros_node_tests/reference_filter_node_test.sh old mode 100644 new mode 100755 index f9278d31c..8561bf190 --- a/tests/ros_node_tests/reference_filter_node_test.sh +++ b/tests/ros_node_tests/reference_filter_node_test.sh @@ -28,7 +28,10 @@ fi # Send action goal echo "Sending goal..." -ros2 action send_goal /orca/reference_filter vortex_msgs/action/ReferenceFilterWaypoint "{goal: {pose: {position: {x: 1.0}}}}" & +ros2 action send_goal /orca/reference_filter vortex_msgs/action/ReferenceFilterWaypoint \ +"{waypoint: {pose: {position: {x: 1.0,y: 0.0,z: 0.0}, orientation:{x: 0,y: 0,z: 0,w: 1}}, mode: 0}}" & + +sleep 2 # Check if controller correctly publishes guidance echo "Waiting for guidance data..." diff --git a/tests/simulator_tests/waypoint_manager_test/check_goal.py b/tests/simulator_tests/waypoint_manager_test/check_goal.py new file mode 100644 index 000000000..89f4a023e --- /dev/null +++ b/tests/simulator_tests/waypoint_manager_test/check_goal.py @@ -0,0 +1,103 @@ +import math +import os +import time + +import rclpy +import yaml +from geometry_msgs.msg import PoseWithCovarianceStamped +from rclpy.node import Node +from rclpy.qos import QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy +from vortex_utils.python_utils import quat_to_euler + +best_effort_qos = QoSProfile( + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + reliability=QoSReliabilityPolicy.BEST_EFFORT, +) + +# Read goal from temp file +file_path = 'goal_pose.yaml' +with open(file_path) as f: + data = yaml.safe_load(f) + +# Remove temp file +os.remove(file_path) +print(f"Temp file {file_path} deleted") +goal_pos = data['pos'] +goal_ori = data['ori'] + +pos_tol = 0.6 # meters (match python test tolerance) +ori_tol = 0.5 # rad (loose) + + +class CheckGoalNode(Node): + def __init__(self): + super().__init__('check_goal_node') + self.pose_sub_ = self.create_subscription( + PoseWithCovarianceStamped, '/orca/pose', self.pose_callback, best_effort_qos + ) + + self.current_pose_: PoseWithCovarianceStamped = None + self.received_pose_: bool = False + + def pose_callback(self, msg: PoseWithCovarianceStamped): + self.current_pose_ = msg + self.received_pose_ = True + + +def main(args=None): + rclpy.init(args=args) + node = CheckGoalNode() + + print(f"Waiting for vehicle to reach goal: {goal_pos} with orientation {goal_ori}") + + start_time = time.time() + timeout = 240 # seconds; waypoint manager may take longer + + x = y = z = None + current_ori = (0.0, 0.0, 0.0) + + while rclpy.ok() and time.time() - start_time < timeout: + rclpy.spin_once(node) + if node.received_pose_: + x = node.current_pose_.pose.pose.position.x + y = node.current_pose_.pose.pose.position.y + z = node.current_pose_.pose.pose.position.z + q_w = node.current_pose_.pose.pose.orientation.w + q_x = node.current_pose_.pose.pose.orientation.x + q_y = node.current_pose_.pose.pose.orientation.y + q_z = node.current_pose_.pose.pose.orientation.z + current_ori = quat_to_euler(x=q_x, y=q_y, z=q_z, w=q_w) + dist = math.sqrt( + (goal_pos[0] - x) ** 2 + (goal_pos[1] - y) ** 2 + (goal_pos[2] - z) ** 2 + ) + + dist_ori = math.sqrt( + (goal_ori[0] - current_ori[0]) ** 2 + + (goal_ori[1] - current_ori[1]) ** 2 + + (goal_ori[2] - current_ori[2]) ** 2 + ) + + if dist < pos_tol and dist_ori < ori_tol: + print( + f"Vehicle reached final waypoint: {goal_pos} and orientation: {goal_ori}" + ) + print(f"Final vehicle pose: ({x, y, z}), {current_ori}") + print(f"Euclidean error: {dist}, {dist_ori}") + rclpy.shutdown() + exit(0) + + print( + f"Vehicle did not reach final waypoint: {goal_pos} and orientation: {goal_ori}" + ) + if x is not None: + print(f"Current_vehicle pose: ({x, y, z}), {current_ori}") + print( + f"Euclidean error: {dist if 'dist' in locals() else 'N/A'}, {dist_ori if 'dist_ori' in locals() else 'N/A'}" + ) + rclpy.shutdown() + exit(1) + + +if __name__ == '__main__': + main() diff --git a/tests/simulator_tests/waypoint_manager_test/send_goal.py b/tests/simulator_tests/waypoint_manager_test/send_goal.py new file mode 100644 index 000000000..427bcb853 --- /dev/null +++ b/tests/simulator_tests/waypoint_manager_test/send_goal.py @@ -0,0 +1,91 @@ +import random + +import rclpy +import yaml +from rclpy.action import ActionClient +from rclpy.node import Node +from vortex_msgs.action import ReferenceFilterWaypoint +from vortex_utils.python_utils import PoseData, euler_to_quat + + +def randomize_pose() -> PoseData: + pose: PoseData = PoseData() + pose.x = random.uniform(-10.0, 10.0) + pose.y = random.uniform(-10.0, 10.0) + pose.z = random.uniform(0.5, 3.0) + pose.roll = 0.0 + pose.pitch = random.uniform(-1.0, 1.0) + pose.yaw = random.uniform(-1.57, 1.57) + + return pose + + +class ReferenceFilterWaypointClient(Node): + def __init__(self): + super().__init__('reference_filter_waypoint_client') + + self._action_client = ActionClient( + self, ReferenceFilterWaypoint, '/orca/reference_filter' + ) + self.send_goal() + + def send_goal(self): + goal_pose = randomize_pose() + goal_msg = ReferenceFilterWaypoint.Goal() + goal_msg.waypoint.pose.position.x = goal_pose.x + goal_msg.waypoint.pose.position.y = goal_pose.y + goal_msg.waypoint.pose.position.z = goal_pose.z + roll = goal_pose.roll + pitch = goal_pose.pitch + yaw = goal_pose.yaw + + quat = euler_to_quat(roll=roll, pitch=pitch, yaw=yaw) + + goal_msg.waypoint.pose.orientation.x = quat[0] + goal_msg.waypoint.pose.orientation.y = quat[1] + goal_msg.waypoint.pose.orientation.z = quat[2] + goal_msg.waypoint.pose.orientation.w = quat[3] + + # Write goal pose to temp file + file_path = "goal_pose.yaml" + + data = { + "pos": [goal_pose.x, goal_pose.y, goal_pose.z], + "ori": [goal_pose.roll, goal_pose.pitch, goal_pose.yaw], + } + + with open(file_path, "w") as f: + yaml.safe_dump(data, f) + + # Send the goal asynchronously + self._action_client.wait_for_server(timeout_sec=10.0) + self.get_logger().info(f'Sending goal {goal_pose}...') + self._send_goal_future = self._action_client.send_goal_async(goal_msg) + self._send_goal_future.add_done_callback(self.goal_response_callback) + + def goal_response_callback(self, future): + goal_handle = future.result() + if not goal_handle.accepted: + self.get_logger().info('Goal rejected :(') + return + + self.get_logger().info('Goal accepted :)') + self._get_result_future = goal_handle.get_result_async() + self._get_result_future.add_done_callback(self.get_result_callback) + + def get_result_callback(self, future): + result = future.result().result.success + self.get_logger().info(f'Goal result: {result}') + self.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + +def main(args=None): + rclpy.init(args=args) + action_client = ReferenceFilterWaypointClient() + rclpy.spin(action_client) + + +if __name__ == '__main__': + main() diff --git a/tests/simulator_tests/waypoint_manager_test/simulator_test.sh b/tests/simulator_tests/waypoint_manager_test/simulator_test.sh new file mode 100755 index 000000000..1d726305f --- /dev/null +++ b/tests/simulator_tests/waypoint_manager_test/simulator_test.sh @@ -0,0 +1,115 @@ +#!/bin/bash +set -e +set -o pipefail + +echo "Setting up ROS 2 environment..." +. /opt/ros/humble/setup.sh +. "${WORKSPACE:-$HOME/ros2_ws}/install/setup.bash" +export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/lib + +# Get the directory of this script dynamically +SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" + +# Function to terminate processes safely on error +cleanup() { + echo "Error detected. Cleaning up..." + # Safely kill any started PIDs (ignore empty values) + for _pid in "$SIM_PID" "$ORCA_PID" "$WM_PID" "$CONTROLLER_PID" "$FILTER_PID" "$BAG_PID"; do + if [ -n "$_pid" ]; then + kill -TERM "$_pid" 2>/dev/null || true + fi + done + exit 1 +} +trap cleanup ERR + +setsid ros2 bag record -o ${WORKSPACE}/bags/recording -s mcap -a & +BAG_PID=$! +echo "Started bagging with PID: $BAG_PID" + +# Launch Stonefish Simulator +setsid ros2 launch stonefish_sim simulation.launch.py rendering:=false scenario:=orca_no_gpu & +SIM_PID=$! +echo "Launched simulator with PID: $SIM_PID" + +echo "Waiting for simulator to start..." +timeout 30s bash -c ' + while ! ros2 topic list | grep -q "/orca/odom"; do + sleep 1 + done || true' +echo "Simulator started" + +# Check for ROS errors in logs +if journalctl -u ros2 | grep -i "error"; then + echo "Error detected in ROS logs. Exiting..." + exit 1 +fi + +# Wait for odometry data +echo "Waiting for odom data..." +timeout 10s ros2 topic echo /orca/odom --once +echo "Got odom data" + +# Launch ORCA Simulation +setsid ros2 launch stonefish_sim orca_sim.launch.py & +ORCA_PID=$! +echo "Launched orca with PID: $ORCA_PID" + +setsid ros2 launch waypoint_manager waypoint_manager.launch.py & +WM_PID=$! +echo "Launched waypoint manager with PID: $WM_PID" + +echo "Waiting for sim interface to start..." +timeout 30s bash -c 'until ros2 topic list | grep -q "/orca/pose"; do sleep 1; done' +echo "Simulator started" + +# Check for ROS errors again +if journalctl -u ros2 | grep -i "error"; then + echo "Error detected in ROS logs. Exiting..." + exit 1 +fi + +# Wait for pose data +echo "Waiting for pose data..." +timeout 10s ros2 topic echo /orca/pose --once +echo "Got pose data" + +# Launch controller and reference filter +setsid ros2 launch auv_setup dp.launch.py & +CONTROLLER_PID=$! +echo "Launched controller and reference filter with PID: $CONTROLLER_PID" + +# Check for ROS errors before continuing +if journalctl -u ros2 | grep -i "error"; then + echo "Error detected in ROS logs. Exiting..." + exit 1 +fi + +# Set operation mode +echo "Turning off killswitch and setting operation mode to autonomous mode" +ros2 topic pub /orca/killswitch std_msgs/msg/Bool "{data: false}" -t 5 # Ensure the message arrives +ros2 topic pub /orca/operation_mode std_msgs/msg/String "{data: 'autonomous mode'}" -t 5 # Ensure the message arrives + +# Send waypoint goal +echo "Sending goal" +python3 "$SCRIPT_DIR/send_goal.py" + +# Check if goal reached +echo "Checking if goal reached" +python3 "$SCRIPT_DIR/check_goal.py" + +if [ $? -ne 0 ]; then + echo "Test failed: Vehicle did not reach final waypoint." + exit 1 +else + echo "Test passed: Vehicle reached final waypoint." +fi + +# Terminate processes (safely) +for _pid in "$SIM_PID" "$ORCA_PID" "$WM_PID" "$CONTROLLER_PID" "$BAG_PID"; do + if [ -n "$_pid" ]; then + kill -TERM "$_pid" 2>/dev/null || true + fi +done + +echo "Test completed successfully." diff --git a/tests/simulator_tests/waypoint_navigation/send_goal.py b/tests/simulator_tests/waypoint_navigation/send_goal.py index bb04a7686..25cb3275d 100644 --- a/tests/simulator_tests/waypoint_navigation/send_goal.py +++ b/tests/simulator_tests/waypoint_navigation/send_goal.py @@ -33,19 +33,18 @@ def send_goal(self): goal_pose = randomize_pose() goal_msg = ReferenceFilterWaypoint.Goal() - goal_msg.goal.pose.position.x = goal_pose.x - goal_msg.goal.pose.position.y = goal_pose.y - goal_msg.goal.pose.position.z = goal_pose.z + goal_msg.waypoint.pose.position.x = goal_pose.x + goal_msg.waypoint.pose.position.y = goal_pose.y + goal_msg.waypoint.pose.position.z = goal_pose.z roll = goal_pose.roll pitch = goal_pose.pitch yaw = goal_pose.yaw quat = euler_to_quat(roll=roll, pitch=pitch, yaw=yaw) - - goal_msg.goal.pose.orientation.x = quat[0] - goal_msg.goal.pose.orientation.y = quat[1] - goal_msg.goal.pose.orientation.z = quat[2] - goal_msg.goal.pose.orientation.w = quat[3] + goal_msg.waypoint.pose.orientation.x = quat[0] + goal_msg.waypoint.pose.orientation.y = quat[1] + goal_msg.waypoint.pose.orientation.z = quat[2] + goal_msg.waypoint.pose.orientation.w = quat[3] # Write goal pose to temp file file_path = "goal_pose.yaml" From 8f1555276224d3d4333911f25cf2a07013ec7fda Mon Sep 17 00:00:00 2001 From: jorgenfj <144696109+jorgenfj@users.noreply.github.com> Date: Mon, 22 Dec 2025 10:55:07 +0100 Subject: [PATCH 035/128] Refactor/type rename (#647) * waypoint mode handling * update tests with new message * initial package setup * ros interface function declaration * node setup * working prototype * reentrant cb, multithreaded * single threaded impl * conv threshold action goal * default thresholdref conv value * removed switching logic * removed timer execution * sim test utils * waypoint_manager_test setup * no rendering test arg * waypoint tests, timeout error * test refactor * format * rename utils package * test suite and description * first waypoint test * removed unused function * renamed service field to priority. Added simple tests * waypoint manager readme * uniform attitude naming convention * fix pr requests * update tests with new service fields * four corner test * update util func name * update with new action def * removed failing build type * test dependencies * ignore failing yasmin package * remove __init__ files * quat_to_euler in make_pose helper * added __init__ file * removed sim deps for test packages * added action shutdown handling * removed waypoint manager set setup * added waypoint manager node tests * waypoint manager 4 corner sim test * added missing launch testing test dependency * add sleep for topic discovery * fix action member field name * updated to new utils type names * renamed variables to match types * update function arg to reflect vortex type * update variable name in tests * renamed function arg names --- .../dp_adapt_backs_controller.hpp | 13 +- .../dp_adapt_backs_controller_ros.hpp | 6 +- .../dp_adapt_backs_controller_utils.hpp | 40 ++-- .../src/dp_adapt_backs_controller.cpp | 28 +-- .../src/dp_adapt_backs_controller_ros.cpp | 40 ++-- .../src/dp_adapt_backs_controller_utils.cpp | 72 +++---- .../test/dp_adapt_backs_controller_tests.cpp | 192 +++++++++--------- .../src/reference_filter_ros.cpp | 42 ++-- .../src/waypoint_manager_ros.cpp | 8 +- 9 files changed, 222 insertions(+), 219 deletions(-) diff --git a/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller.hpp b/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller.hpp index 6a44644e8..a26969cee 100644 --- a/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller.hpp +++ b/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller.hpp @@ -23,15 +23,16 @@ class DPAdaptBacksController { explicit DPAdaptBacksController(const DPAdaptParams& dp_adapt_params); // @brief Calculate thecontrol input tau - // @param eta: 6D vector containing the vehicle pose [x, y, z, roll, pitch, + // @param pose: 6D vector containing the vehicle pose [x, y, z, roll, pitch, // yaw] - // @param eta_d: 6D vector containing the desired vehicle pose [x, y, z, + // @param pose_d: 6D vector containing the desired vehicle pose [x, y, z, // roll, pitch, yaw] - // @param nu: 6D vector containing the vehicle velocity [u, v, w, p, q, r] + // @param twist: 6D vector containing the vehicle velocity [u, v, w, p, q, + // r] // @return 6D vector containing the control input tau [X, Y, Z, K, M, N] - Eigen::Vector6d calculate_tau(const vortex::utils::types::Eta& eta, - const vortex::utils::types::Eta& eta_d, - const vortex::utils::types::Nu& nu); + Eigen::Vector6d calculate_tau(const vortex::utils::types::PoseEuler& pose, + const vortex::utils::types::PoseEuler& pose_d, + const vortex::utils::types::Twist& twist); // @brief Reset the adaptive parameters void reset_adap_param(); diff --git a/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_ros.hpp b/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_ros.hpp index 85b1c7f7a..478bdce2f 100644 --- a/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_ros.hpp +++ b/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_ros.hpp @@ -80,11 +80,11 @@ class DPAdaptBacksControllerNode : public rclcpp::Node { std::chrono::milliseconds time_step_{}; - vortex::utils::types::Eta eta_; + vortex::utils::types::PoseEuler pose_; - vortex::utils::types::Eta eta_d_; + vortex::utils::types::PoseEuler pose_d_; - vortex::utils::types::Nu nu_; + vortex::utils::types::Twist twist_; std::unique_ptr dp_adapt_backs_controller_{}; diff --git a/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_utils.hpp b/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_utils.hpp index b48a21867..7a0e214a9 100644 --- a/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_utils.hpp +++ b/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_utils.hpp @@ -8,45 +8,49 @@ namespace vortex::control { // @brief Calculate the derivative of the rotation matrix -// @param eta: 6D vector containing the vehicle pose [x, y, z, roll, pitch, yaw] -// @param nu: 6D vector containing the vehicle velocity [u, v, w, p, q, r] +// @param pose: 6D vector containing the vehicle pose [x, y, z, roll, pitch, +// yaw] +// @param twist: 6D vector containing the vehicle velocity [u, v, w, p, q, r] // @return 3x3 derivative of the rotation matrix -Eigen::Matrix3d calculate_R_dot(const vortex::utils::types::Eta& eta, - const vortex::utils::types::Nu& nu); +Eigen::Matrix3d calculate_R_dot(const vortex::utils::types::PoseEuler& pose, + const vortex::utils::types::Twist& twist); // @brief Calculate the derivative of the transformation matrix -// @param eta: 6D vector containing the vehicle pose [x, y, z, roll, pitch, yaw] -// @param nu: 6D vector containing the vehicle velocity [u, v, w, p, q, r] +// @param pose: 6D vector containing the vehicle pose [x, y, z, roll, pitch, +// yaw] +// @param twist: 6D vector containing the vehicle velocity [u, v, w, p, q, r] // @return 3x3 derivative of the transformation matrix -Eigen::Matrix3d calculate_T_dot(const vortex::utils::types::Eta& eta, - const vortex::utils::types::Nu& nu); +Eigen::Matrix3d calculate_T_dot(const vortex::utils::types::PoseEuler& pose, + const vortex::utils::types::Twist& twist); // @brief Calculate the pseudo-inverse of the Jacobian matrix -// @param eta: 6D vector containing the vehicle pose [x, y, z, roll, pitch, yaw] +// @param pose: 6D vector containing the vehicle pose [x, y, z, roll, pitch, +// yaw] // @return 6x6 pseudo-inverse Jacobian matrix -Eigen::Matrix6d calculate_J_inv(const vortex::utils::types::Eta& eta); +Eigen::Matrix6d calculate_J_inv(const vortex::utils::types::PoseEuler& pose); // @brief calculate the derivative of the Jacobian matrix -// @param eta: 6D vector containing the vehicle pose [x, y, z, roll, pitch, yaw] -// @param nu: 6D vector containing the vehicle velocity [u, v, w, p, q, r] -Eigen::Matrix6d calculate_J_dot(const vortex::utils::types::Eta& eta, - const vortex::utils::types::Nu& nu); +// @param pose: 6D vector containing the vehicle pose [x, y, z, roll, pitch, +// yaw] +// @param twist: 6D vector containing the vehicle velocity [u, v, w, p, q, r] +Eigen::Matrix6d calculate_J_dot(const vortex::utils::types::PoseEuler& pose, + const vortex::utils::types::Twist& twist); // @brief Calculate the coriolis matrix // @param m: mass of the vehicle // @param r_b_bg: 3D vector of the body frame to the center of gravity -// @param nu_2: 3D vector containing angular velocity of the vehicle +// @param twist: 6D vector containing linear and angular velocity of the vehicle // @param I_b : 3D matrix containing the inertia matrix // @return 6x6 coriolis matrix Eigen::Matrix6d calculate_coriolis(const double mass, const Eigen::Vector3d& r_b_bg, - const vortex::utils::types::Nu& nu, + const vortex::utils::types::Twist& twist, const Eigen::Matrix3d& I_b); // @brief Calculate the damping matrix for the adaptive backstepping controller -// @param nu: 6D vector containing the vehicle velocity [u, v, w, p, q, r] +// @param twist: 6D vector containing the vehicle velocity [u, v, w, p, q, r] // @return 6x6 damping matrix -Eigen::Matrix6x12d calculate_Y_v(const vortex::utils::types::Nu& nu); +Eigen::Matrix6x12d calculate_Y_v(const vortex::utils::types::Twist& twist); } // namespace vortex::control diff --git a/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller.cpp b/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller.cpp index e95af7ba5..78102c0bf 100644 --- a/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller.cpp +++ b/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller.cpp @@ -7,8 +7,8 @@ namespace vortex::control { -using vortex::utils::types::Eta; -using vortex::utils::types::Nu; +using vortex::utils::types::PoseEuler; +using vortex::utils::types::Twist; DPAdaptBacksController::DPAdaptBacksController( const DPAdaptParams& dp_adapt_params) @@ -24,29 +24,29 @@ DPAdaptBacksController::DPAdaptBacksController( m_(dp_adapt_params.mass), dt_(0.01) {} -Eigen::Vector6d DPAdaptBacksController::calculate_tau(const Eta& eta, - const Eta& eta_d, - const Nu& nu) { - Eta error = eta - eta_d; +Eigen::Vector6d DPAdaptBacksController::calculate_tau(const PoseEuler& pose, + const PoseEuler& pose_d, + const Twist& twist) { + PoseEuler error = pose - pose_d; error.roll = vortex::utils::math::ssa(error.roll); error.pitch = vortex::utils::math::ssa(error.pitch); error.yaw = vortex::utils::math::ssa(error.yaw); - Eigen::Matrix6d C = calculate_coriolis(m_, r_b_bg_, nu, I_b_); - Eigen::Matrix6d J_inv = calculate_J_inv(eta); - Eigen::Matrix6d J_dot = calculate_J_dot(eta, nu); + Eigen::Matrix6d C = calculate_coriolis(m_, r_b_bg_, twist, I_b_); + Eigen::Matrix6d J_inv = calculate_J_inv(pose); + Eigen::Matrix6d J_dot = calculate_J_dot(pose, twist); Eigen::Vector6d alpha = -J_inv * K1_ * error.to_vector(); Eigen::Vector6d z_1 = error.to_vector(); - Eigen::Vector6d z_2 = nu.to_vector() - alpha; + Eigen::Vector6d z_2 = twist.to_vector() - alpha; Eigen::Vector6d alpha_dot = ((J_inv * J_dot * J_inv) * K1_ * z_1) - - (J_inv * K1_ * eta.as_j_matrix() * nu.to_vector()); - Eigen::Matrix6x12d Y_v = calculate_Y_v(nu); + (J_inv * K1_ * pose.as_j_matrix() * twist.to_vector()); + Eigen::Matrix6x12d Y_v = calculate_Y_v(twist); Eigen::Vector12d adapt_param_dot = adapt_gain_ * Y_v.transpose() * z_2; Eigen::Vector6d d_est_dot = d_gain_ * z_2; Eigen::Vector6d F_est = Y_v * adapt_param_; - Eigen::Vector6d tau = (mass_matrix_ * alpha_dot) + (C * nu.to_vector()) - - (eta.as_j_matrix().transpose() * z_1) - (K2_ * z_2) - + Eigen::Vector6d tau = (mass_matrix_ * alpha_dot) + (C * twist.to_vector()) - + (pose.as_j_matrix().transpose() * z_1) - (K2_ * z_2) - F_est - d_est_; tau = tau.cwiseMax(-80.0).cwiseMin(80.0); diff --git a/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_ros.cpp b/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_ros.cpp index da70eaf79..e2cb298e5 100644 --- a/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_ros.cpp +++ b/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_ros.cpp @@ -100,31 +100,31 @@ void DPAdaptBacksControllerNode::software_mode_callback( spdlog::info("Software mode: {}", software_mode_); if (software_mode_ == "autonomous mode") { - eta_d_ = eta_; + pose_d_ = pose_; } } void DPAdaptBacksControllerNode::pose_callback( const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { - eta_.x = msg->pose.pose.position.x; - eta_.y = msg->pose.pose.position.y; - eta_.z = msg->pose.pose.position.z; + pose_.x = msg->pose.pose.position.x; + pose_.y = msg->pose.pose.position.y; + pose_.z = msg->pose.pose.position.z; const auto& o = msg->pose.pose.orientation; Eigen::Quaterniond q(o.w, o.x, o.y, o.z); Eigen::Vector3d euler_angles = vortex::utils::math::quat_to_euler(q); - eta_.roll = euler_angles(0); - eta_.pitch = euler_angles(1); - eta_.yaw = euler_angles(2); + pose_.roll = euler_angles(0); + pose_.pitch = euler_angles(1); + pose_.yaw = euler_angles(2); } void DPAdaptBacksControllerNode::twist_callback( const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { - nu_.u = msg->twist.twist.linear.x; - nu_.v = msg->twist.twist.linear.y; - nu_.w = msg->twist.twist.linear.z; - nu_.p = msg->twist.twist.angular.x; - nu_.q = msg->twist.twist.angular.y; - nu_.r = msg->twist.twist.angular.z; + twist_.u = msg->twist.twist.linear.x; + twist_.v = msg->twist.twist.linear.y; + twist_.w = msg->twist.twist.linear.z; + twist_.p = msg->twist.twist.angular.x; + twist_.q = msg->twist.twist.angular.y; + twist_.r = msg->twist.twist.angular.z; } void DPAdaptBacksControllerNode::set_adap_params() { @@ -185,7 +185,7 @@ void DPAdaptBacksControllerNode::publish_tau() { } Eigen::Vector6d tau = - dp_adapt_backs_controller_->calculate_tau(eta_, eta_d_, nu_); + dp_adapt_backs_controller_->calculate_tau(pose_, pose_d_, twist_); geometry_msgs::msg::WrenchStamped tau_msg; tau_msg.header.stamp = this->now(); @@ -203,12 +203,12 @@ void DPAdaptBacksControllerNode::publish_tau() { void DPAdaptBacksControllerNode::guidance_callback( const vortex_msgs::msg::ReferenceFilter::SharedPtr msg) { - eta_d_.x = msg->x; - eta_d_.y = msg->y; - eta_d_.z = msg->z; - eta_d_.roll = msg->roll; - eta_d_.pitch = msg->pitch; - eta_d_.yaw = msg->yaw; + pose_d_.x = msg->x; + pose_d_.y = msg->y; + pose_d_.z = msg->z; + pose_d_.roll = msg->roll; + pose_d_.pitch = msg->pitch; + pose_d_.yaw = msg->yaw; } RCLCPP_COMPONENTS_REGISTER_NODE(DPAdaptBacksControllerNode) diff --git a/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_utils.cpp b/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_utils.cpp index f62216ab1..116326a6b 100644 --- a/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_utils.cpp +++ b/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_utils.cpp @@ -6,13 +6,13 @@ namespace vortex::control { -Eigen::Matrix6d calculate_J_inv(const vortex::utils::types::Eta& eta) { - Eigen::Matrix6d J = eta.as_j_matrix(); +Eigen::Matrix6d calculate_J_inv(const vortex::utils::types::PoseEuler& pose) { + Eigen::Matrix6d J = pose.as_j_matrix(); constexpr double tolerance = 1e-8; if (std::abs(J.determinant()) < tolerance) { - spdlog::error("J(eta) is singular"); + spdlog::error("J is singular"); // Moore-Penrose pseudoinverse in case of near singular matrix, better // result for smaller singular values @@ -22,26 +22,26 @@ Eigen::Matrix6d calculate_J_inv(const vortex::utils::types::Eta& eta) { return J.inverse(); } -Eigen::Matrix3d calculate_R_dot(const vortex::utils::types::Eta& eta, - const vortex::utils::types::Nu& nu) { - return eta.as_rotation_matrix() * +Eigen::Matrix3d calculate_R_dot(const vortex::utils::types::PoseEuler& pose, + const vortex::utils::types::Twist& twist) { + return pose.as_rotation_matrix() * vortex::utils::math::get_skew_symmetric_matrix( - nu.to_vector().tail(3)); + twist.to_vector().tail(3)); } -Eigen::Matrix3d calculate_T_dot(const vortex::utils::types::Eta& eta, - const vortex::utils::types::Nu& nu) { - double cos_phi{std::cos(eta.roll)}; - double sin_phi{std::sin(eta.roll)}; - double cos_theta{std::cos(eta.pitch)}; - double sin_theta{std::sin(eta.pitch)}; +Eigen::Matrix3d calculate_T_dot(const vortex::utils::types::PoseEuler& pose, + const vortex::utils::types::Twist& twist) { + double cos_phi{std::cos(pose.roll)}; + double sin_phi{std::sin(pose.roll)}; + double cos_theta{std::cos(pose.pitch)}; + double sin_theta{std::sin(pose.pitch)}; double tan_theta{sin_theta / cos_theta}; double inv_cos2{1.0 / (cos_theta * cos_theta)}; - Eigen::Vector6d eta_dot = eta.as_j_matrix() * nu.to_vector(); + Eigen::Vector6d pose_dot = pose.as_j_matrix() * twist.to_vector(); - double phi_dot{eta_dot(3)}; - double theta_dot{eta_dot(4)}; + double phi_dot{pose_dot(3)}; + double theta_dot{pose_dot(4)}; Eigen::Matrix3d dt_dphi; dt_dphi << 0.0, cos_phi * tan_theta * phi_dot, @@ -58,10 +58,10 @@ Eigen::Matrix3d calculate_T_dot(const vortex::utils::types::Eta& eta, return dt_dphi + dt_dtheta; } -Eigen::Matrix6d calculate_J_dot(const vortex::utils::types::Eta& eta, - const vortex::utils::types::Nu& nu) { - Eigen::Matrix3d R_dot = calculate_R_dot(eta, nu); - Eigen::Matrix3d T_dot = calculate_T_dot(eta, nu); +Eigen::Matrix6d calculate_J_dot(const vortex::utils::types::PoseEuler& pose, + const vortex::utils::types::Twist& twist) { + Eigen::Matrix3d R_dot = calculate_R_dot(pose, twist); + Eigen::Matrix3d T_dot = calculate_T_dot(pose, twist); Eigen::Matrix6d J_dot = Eigen::Matrix6d::Zero(); J_dot.topLeftCorner<3, 3>() = R_dot; @@ -72,11 +72,11 @@ Eigen::Matrix6d calculate_J_dot(const vortex::utils::types::Eta& eta, Eigen::Matrix6d calculate_coriolis(const double mass, const Eigen::Vector3d& r_b_bg, - const vortex::utils::types::Nu& nu, + const vortex::utils::types::Twist& twist, const Eigen::Matrix3d& I_b) { using vortex::utils::math::get_skew_symmetric_matrix; - Eigen::Vector3d linear_speed = nu.to_vector().head(3); - Eigen::Vector3d angular_speed = nu.to_vector().tail(3); + Eigen::Vector3d linear_speed = twist.to_vector().head(3); + Eigen::Vector3d angular_speed = twist.to_vector().tail(3); Eigen::Matrix6d C; C.topLeftCorner<3, 3>() = mass * vortex::utils::math::get_skew_symmetric_matrix(linear_speed); @@ -93,27 +93,27 @@ Eigen::Matrix6d calculate_coriolis(const double mass, return C; } -Eigen::Matrix6x12d calculate_Y_v(const vortex::utils::types::Nu& nu) { +Eigen::Matrix6x12d calculate_Y_v(const vortex::utils::types::Twist& twist) { Eigen::Matrix6x12d Y_v; Y_v.setZero(); - Y_v(0, 0) = nu.u; - Y_v(0, 1) = nu.u * std::abs(nu.u); + Y_v(0, 0) = twist.u; + Y_v(0, 1) = twist.u * std::abs(twist.u); - Y_v(1, 2) = nu.v; - Y_v(1, 3) = nu.v * std::abs(nu.v); + Y_v(1, 2) = twist.v; + Y_v(1, 3) = twist.v * std::abs(twist.v); - Y_v(2, 4) = nu.w; - Y_v(2, 5) = nu.w * std::abs(nu.w); + Y_v(2, 4) = twist.w; + Y_v(2, 5) = twist.w * std::abs(twist.w); - Y_v(3, 6) = nu.p; - Y_v(3, 7) = nu.p * std::abs(nu.p); + Y_v(3, 6) = twist.p; + Y_v(3, 7) = twist.p * std::abs(twist.p); - Y_v(4, 8) = nu.q; - Y_v(4, 9) = nu.q * std::abs(nu.q); + Y_v(4, 8) = twist.q; + Y_v(4, 9) = twist.q * std::abs(twist.q); - Y_v(5, 10) = nu.r; - Y_v(5, 11) = nu.r * std::abs(nu.r); + Y_v(5, 10) = twist.r; + Y_v(5, 11) = twist.r * std::abs(twist.r); return Y_v; } diff --git a/control/dp_adapt_backs_controller/test/dp_adapt_backs_controller_tests.cpp b/control/dp_adapt_backs_controller/test/dp_adapt_backs_controller_tests.cpp index 5e179a411..b3adf9d17 100644 --- a/control/dp_adapt_backs_controller/test/dp_adapt_backs_controller_tests.cpp +++ b/control/dp_adapt_backs_controller/test/dp_adapt_backs_controller_tests.cpp @@ -8,8 +8,8 @@ namespace vortex::control { -using vortex::utils::types::Eta; -using vortex::utils::types::Nu; +using vortex::utils::types::PoseEuler; +using vortex::utils::types::Twist; class DPAdaptBacksControllerTests : public ::testing::Test { protected: @@ -29,32 +29,32 @@ class DPAdaptBacksControllerTests : public ::testing::Test { return params; } - Eta generate_current_pose(const double north_pos, - const double east_pos, - const double down_pos, - const double roll_angle, - const double pitch_angle, - const double yaw_angle) { + PoseEuler generate_current_pose(const double north_pos, + const double east_pos, + const double down_pos, + const double roll_angle, + const double pitch_angle, + const double yaw_angle) { return {north_pos, east_pos, down_pos, roll_angle, pitch_angle, yaw_angle}; } - Eta generate_reference_pose(const double north_pos, - const double east_pos, - const double down_pos, - const double roll_angle, - const double pitch_angle, - const double yaw_angle) { + PoseEuler generate_reference_pose(const double north_pos, + const double east_pos, + const double down_pos, + const double roll_angle, + const double pitch_angle, + const double yaw_angle) { return {north_pos, east_pos, down_pos, roll_angle, pitch_angle, yaw_angle}; } - Nu generate_current_velocity(const double surge_vel, - const double sway_vel, - const double heave_vel, - const double roll_rate, - const double pitch_rate, - const double yaw_rate) { + Twist generate_current_velocity(const double surge_vel, + const double sway_vel, + const double heave_vel, + const double roll_rate, + const double pitch_rate, + const double yaw_rate) { return {surge_vel, sway_vel, heave_vel, roll_rate, pitch_rate, yaw_rate}; } @@ -68,11 +68,11 @@ Test that negative north error only (in body) gives positive surge command only. TEST_F(DPAdaptBacksControllerTests, T01_neg_north_error_with_zero_heading_gives_surge_only_command) { - Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; - Eta eta_d{generate_reference_pose(10.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; - Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose_d{generate_reference_pose(10.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; Eigen::Vector6d tau{ - dp_adapt_backs_controller_.calculate_tau(eta, eta_d, nu)}; + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; EXPECT_GT(tau[0], 0.0); EXPECT_NEAR(tau[1], 0.0, 0.01); EXPECT_NEAR(tau[2], 0.0, 0.01); @@ -89,11 +89,11 @@ command and negative sway command. TEST_F( DPAdaptBacksControllerTests, T02_neg_north_error_with_positive_heading_gives_pos_surge_and_neg_sway_command) { - Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 1.5)}; - Eta eta_d{generate_reference_pose(10.0, 0.0, 0.0, 0.0, 0.0, 1.5)}; - Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 1.5)}; + PoseEuler pose_d{generate_reference_pose(10.0, 0.0, 0.0, 0.0, 0.0, 1.5)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; Eigen::Vector6d tau{ - dp_adapt_backs_controller_.calculate_tau(eta, eta_d, nu)}; + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; EXPECT_GT(tau[0], 0.0); EXPECT_LT(tau[1], 0.0); EXPECT_NEAR(tau[2], 0.0, 0.01); @@ -110,11 +110,11 @@ command and positive sway command. TEST_F( DPAdaptBacksControllerTests, T03_neg_north_error_with_negative_heading_gives_pos_surge_and_pos_sway_command) { - Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, -1.5)}; - Eta eta_d{generate_reference_pose(10.0, 0.0, 0.0, 0.0, 0.0, -1.5)}; - Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, -1.5)}; + PoseEuler pose_d{generate_reference_pose(10.0, 0.0, 0.0, 0.0, 0.0, -1.5)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; Eigen::Vector6d tau{ - dp_adapt_backs_controller_.calculate_tau(eta, eta_d, nu)}; + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; EXPECT_GT(tau[0], 0.0); EXPECT_GT(tau[1], 0.0); EXPECT_NEAR(tau[2], 0.0, 0.01); @@ -131,11 +131,11 @@ command. TEST_F( DPAdaptBacksControllerTests, T04_neg_down_error_with_zero_roll_and_pitch_gives_positive_heave_command) { - Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; - Eta eta_d{generate_reference_pose(0.0, 0.0, 2.0, 0.0, 0.0, 0.0)}; - Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose_d{generate_reference_pose(0.0, 0.0, 2.0, 0.0, 0.0, 0.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; Eigen::Vector6d tau{ - dp_adapt_backs_controller_.calculate_tau(eta, eta_d, nu)}; + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; EXPECT_NEAR(tau[0], 0.0, 0.01); EXPECT_NEAR(tau[1], 0.0, 0.01); EXPECT_GT(tau[2], 0.0); @@ -152,11 +152,11 @@ heave and positive surge command. TEST_F( DPAdaptBacksControllerTests, T05_neg_down_error_with_zero_roll_and_neg_pitch_gives_positive_heave_and_positive_surge_command) { - Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, -0.5, 0.0)}; - Eta eta_d{generate_reference_pose(0.0, 0.0, 2.0, 0.0, -0.5, 0.0)}; - Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, -0.5, 0.0)}; + PoseEuler pose_d{generate_reference_pose(0.0, 0.0, 2.0, 0.0, -0.5, 0.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; Eigen::Vector6d tau{ - dp_adapt_backs_controller_.calculate_tau(eta, eta_d, nu)}; + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; EXPECT_GT(tau[0], 0.0); EXPECT_NEAR(tau[1], 0.0, 0.01); EXPECT_GT(tau[2], 0.0); @@ -173,11 +173,11 @@ heave and negative surge command. TEST_F( DPAdaptBacksControllerTests, T06_neg_down_error_with_zero_roll_and_pos_pitch_gives_positive_heave_and_negative_surge_command) { - Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.5, 0.0)}; - Eta eta_d{generate_reference_pose(0.0, 0.0, 2.0, 0.0, 0.5, 0.0)}; - Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.5, 0.0)}; + PoseEuler pose_d{generate_reference_pose(0.0, 0.0, 2.0, 0.0, 0.5, 0.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; Eigen::Vector6d tau{ - dp_adapt_backs_controller_.calculate_tau(eta, eta_d, nu)}; + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; EXPECT_LT(tau[0], 0.0); EXPECT_NEAR(tau[1], 0.0, 0.01); EXPECT_GT(tau[2], 0.0); @@ -192,11 +192,11 @@ Test that negative east error with zero heading gives a positive sway command. TEST_F(DPAdaptBacksControllerTests, T07_neg_east_error_with_zero_heading_gives_positive_sway_command) { - Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; - Eta eta_d{generate_reference_pose(0.0, 10.0, 0.0, 0.0, 0.0, 0.0)}; - Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose_d{generate_reference_pose(0.0, 10.0, 0.0, 0.0, 0.0, 0.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; Eigen::Vector6d tau{ - dp_adapt_backs_controller_.calculate_tau(eta, eta_d, nu)}; + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; EXPECT_NEAR(tau[0], 0.0, 0.01); EXPECT_GT(tau[1], 0.0); EXPECT_NEAR(tau[2], 0.0, 0.01); @@ -211,11 +211,11 @@ Test that positive east error with zero heading gives a negative sway command. TEST_F(DPAdaptBacksControllerTests, T08_pos_east_error_with_zero_heading_gives_pos_sway_command) { - Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; - Eta eta_d{generate_reference_pose(0.0, -10.0, 0.0, 0.0, 0.0, 0.0)}; - Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose_d{generate_reference_pose(0.0, -10.0, 0.0, 0.0, 0.0, 0.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; Eigen::Vector6d tau{ - dp_adapt_backs_controller_.calculate_tau(eta, eta_d, nu)}; + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; EXPECT_NEAR(tau[0], 0.0, 0.01); EXPECT_LT(tau[1], 0.0); EXPECT_NEAR(tau[2], 0.0, 0.01); @@ -232,11 +232,11 @@ sway command. TEST_F( DPAdaptBacksControllerTests, T09_neg_east_error_with_positive_heading_gives_pos_sway_and_pos_surge_command) { - Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 1.5)}; - Eta eta_d{generate_reference_pose(0.0, 10.0, 0.0, 0.0, 0.0, 1.5)}; - Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 1.5)}; + PoseEuler pose_d{generate_reference_pose(0.0, 10.0, 0.0, 0.0, 0.0, 1.5)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; Eigen::Vector6d tau{ - dp_adapt_backs_controller_.calculate_tau(eta, eta_d, nu)}; + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; EXPECT_GT(tau[0], 0.0); EXPECT_GT(tau[1], 0.0); EXPECT_NEAR(tau[2], 0.0, 0.01); @@ -253,11 +253,11 @@ positive sway command. TEST_F( DPAdaptBacksControllerTests, T10_neg_east_error_with_negative_heading_gives_pos_sway_and_neg_surge_command) { - Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, -1.5)}; - Eta eta_d{generate_reference_pose(0.0, 10.0, 0.0, 0.0, 0.0, -1.5)}; - Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, -1.5)}; + PoseEuler pose_d{generate_reference_pose(0.0, 10.0, 0.0, 0.0, 0.0, -1.5)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; Eigen::Vector6d tau{ - dp_adapt_backs_controller_.calculate_tau(eta, eta_d, nu)}; + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; EXPECT_LT(tau[0], 0.0); EXPECT_GT(tau[1], 0.0); EXPECT_NEAR(tau[2], 0.0, 0.01); @@ -272,11 +272,11 @@ Test that negative roll error gives positive roll command. TEST_F(DPAdaptBacksControllerTests, T11_neg_roll_error_gives_positive_roll_command) { - Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; - Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 1.0, 0.0, 0.0)}; - Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose_d{generate_reference_pose(0.0, 0.0, 0.0, 1.0, 0.0, 0.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; Eigen::Vector6d tau{ - dp_adapt_backs_controller_.calculate_tau(eta, eta_d, nu)}; + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; EXPECT_NEAR(tau[0], 0.0, 0.01); EXPECT_NEAR(tau[1], 0.0, 0.01); EXPECT_NEAR(tau[2], 0.0, 0.01); @@ -290,11 +290,11 @@ Test that positive roll error gives negative roll command. */ TEST_F(DPAdaptBacksControllerTests, T12_pos_roll_error_gives_neg_roll_command) { - Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; - Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, -1.0, 0.0, 0.0)}; - Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose_d{generate_reference_pose(0.0, 0.0, 0.0, -1.0, 0.0, 0.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; Eigen::Vector6d tau{ - dp_adapt_backs_controller_.calculate_tau(eta, eta_d, nu)}; + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; EXPECT_NEAR(tau[0], 0.0, 0.01); EXPECT_NEAR(tau[1], 0.0, 0.01); EXPECT_NEAR(tau[2], 0.0, 0.01); @@ -309,11 +309,11 @@ Test that negative pitch error gives positive pitch command. TEST_F(DPAdaptBacksControllerTests, T13_neg_pitch_error_gives_pos_pitch_command) { - Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; - Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 1.0, 0.0)}; - Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 1.0, 0.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; Eigen::Vector6d tau{ - dp_adapt_backs_controller_.calculate_tau(eta, eta_d, nu)}; + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; EXPECT_NEAR(tau[0], 0.0, 0.01); EXPECT_NEAR(tau[1], 0.0, 0.01); EXPECT_NEAR(tau[2], 0.0, 0.01); @@ -328,11 +328,11 @@ Test that positive pitch error gives negative pitch command. TEST_F(DPAdaptBacksControllerTests, T14_pos_pitch_error_gives_neg_pitch_command) { - Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; - Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, -1.0, 0.0)}; - Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, -1.0, 0.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; Eigen::Vector6d tau{ - dp_adapt_backs_controller_.calculate_tau(eta, eta_d, nu)}; + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; EXPECT_NEAR(tau[0], 0.0, 0.01); EXPECT_NEAR(tau[1], 0.0, 0.01); EXPECT_NEAR(tau[2], 0.0, 0.01); @@ -346,11 +346,11 @@ Test that negative yaw error gives positive yaw command. */ TEST_F(DPAdaptBacksControllerTests, T15_neg_yaw_error_gives_pos_yaw_command) { - Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; - Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, 1.0)}; - Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, 1.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; Eigen::Vector6d tau{ - dp_adapt_backs_controller_.calculate_tau(eta, eta_d, nu)}; + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; EXPECT_NEAR(tau[0], 0.0, 0.01); EXPECT_NEAR(tau[1], 0.0, 0.01); EXPECT_NEAR(tau[2], 0.0, 0.01); @@ -364,11 +364,11 @@ Test that positive yaw error gives negative yaw command. */ TEST_F(DPAdaptBacksControllerTests, T16_pos_yaw_error_gives_neg_yaw_command) { - Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; - Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, -1.0)}; - Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, -1.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; Eigen::Vector6d tau{ - dp_adapt_backs_controller_.calculate_tau(eta, eta_d, nu)}; + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; EXPECT_NEAR(tau[0], 0.0, 0.01); EXPECT_NEAR(tau[1], 0.0, 0.01); EXPECT_NEAR(tau[2], 0.0, 0.01); @@ -384,11 +384,11 @@ Test that positive surge velocity only results in negative surge command TEST_F(DPAdaptBacksControllerTests, T17_pos_surge_vel_gives_negative_surge_command) { - Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; - Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; - Nu nu{generate_current_velocity(1.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Twist twist{generate_current_velocity(1.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; Eigen::Vector6d tau{ - dp_adapt_backs_controller_.calculate_tau(eta, eta_d, nu)}; + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; EXPECT_LT(tau[0], 0.0); EXPECT_NEAR(tau[1], 0.0, 0.01); EXPECT_NEAR(tau[2], 0.0, 0.01); @@ -404,11 +404,11 @@ effect). TEST_F(DPAdaptBacksControllerTests, T18_pos_sway_vel_gives_negative_sway_command) { - Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; - Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; - Nu nu{generate_current_velocity(0.0, 1.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Twist twist{generate_current_velocity(0.0, 1.0, 0.0, 0.0, 0.0, 0.0)}; Eigen::Vector6d tau{ - dp_adapt_backs_controller_.calculate_tau(eta, eta_d, nu)}; + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; EXPECT_NEAR(tau[0], 0.0, 0.01); EXPECT_LT(tau[1], 0.0); EXPECT_NEAR(tau[2], 0.0, 0.01); @@ -424,11 +424,11 @@ Test that positive heave velocity only results in negative heave command TEST_F(DPAdaptBacksControllerTests, T19_pos_heave_vel_gives_negative_heave_command) { - Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; - Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; - Nu nu{generate_current_velocity(0.0, 0.0, 1.0, 0.0, 0.0, 0.0)}; + PoseEuler pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 1.0, 0.0, 0.0, 0.0)}; Eigen::Vector6d tau{ - dp_adapt_backs_controller_.calculate_tau(eta, eta_d, nu)}; + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; EXPECT_NEAR(tau[0], 0.0, 0.01); EXPECT_NEAR(tau[1], 0.0, 0.01); EXPECT_LT(tau[2], 0.0); diff --git a/guidance/reference_filter_dp/src/reference_filter_ros.cpp b/guidance/reference_filter_dp/src/reference_filter_ros.cpp index 614e10e93..a08f415c7 100644 --- a/guidance/reference_filter_dp/src/reference_filter_ros.cpp +++ b/guidance/reference_filter_dp/src/reference_filter_ros.cpp @@ -169,27 +169,27 @@ Eigen::Vector18d ReferenceFilterNode::fill_reference_state() { x(4) = vortex::utils::math::ssa(pitch); x(5) = vortex::utils::math::ssa(yaw); - vortex::utils::types::Eta eta{current_pose_.pose.pose.position.x, - current_pose_.pose.pose.position.y, - current_pose_.pose.pose.position.z, - roll, - pitch, - yaw}; - Eigen::Matrix6d J = eta.as_j_matrix(); - vortex::utils::types::Nu nu{current_twist_.twist.twist.linear.x, - current_twist_.twist.twist.linear.y, - current_twist_.twist.twist.linear.z, - current_twist_.twist.twist.angular.x, - current_twist_.twist.twist.angular.y, - current_twist_.twist.twist.angular.z}; - Eigen::Vector6d eta_dot = J * nu.to_vector(); - - x(6) = eta_dot(0); - x(7) = eta_dot(1); - x(8) = eta_dot(2); - x(9) = eta_dot(3); - x(10) = eta_dot(4); - x(11) = eta_dot(5); + vortex::utils::types::PoseEuler pose{current_pose_.pose.pose.position.x, + current_pose_.pose.pose.position.y, + current_pose_.pose.pose.position.z, + roll, + pitch, + yaw}; + Eigen::Matrix6d J = pose.as_j_matrix(); + vortex::utils::types::Twist twist{current_twist_.twist.twist.linear.x, + current_twist_.twist.twist.linear.y, + current_twist_.twist.twist.linear.z, + current_twist_.twist.twist.angular.x, + current_twist_.twist.twist.angular.y, + current_twist_.twist.twist.angular.z}; + Eigen::Vector6d pose_dot = J * twist.to_vector(); + + x(6) = pose_dot(0); + x(7) = pose_dot(1); + x(8) = pose_dot(2); + x(9) = pose_dot(3); + x(10) = pose_dot(4); + x(11) = pose_dot(5); return x; } diff --git a/mission/waypoint_manager/src/waypoint_manager_ros.cpp b/mission/waypoint_manager/src/waypoint_manager_ros.cpp index 1d4928470..472cf9378 100644 --- a/mission/waypoint_manager/src/waypoint_manager_ros.cpp +++ b/mission/waypoint_manager/src/waypoint_manager_ros.cpp @@ -85,9 +85,8 @@ WaypointManagerNode::construct_result(bool success) const { result->success = success; result->pose_valid = has_reference_pose_; if (has_reference_pose_) { - result->final_pose = - vortex::utils::ros_conversions::pose_like_to_pose_msg( - latest_ref_feedback_.reference); + result->final_pose = vortex::utils::ros_conversions::to_pose_msg( + latest_ref_feedback_.reference); } return result; } @@ -280,8 +279,7 @@ void WaypointManagerNode::send_reference_filter_goal( return; geometry_msgs::msg::Pose robot_pose = - vortex::utils::ros_conversions::pose_like_to_pose_msg( - fb->reference); + vortex::utils::ros_conversions::to_pose_msg(fb->reference); if (current_index_ < waypoints_.size()) { auto wm_fb = std::make_shared(); From 84ae150e78f8c2eaab3665c58d9efe437016644e Mon Sep 17 00:00:00 2001 From: jorgenfj <144696109+jorgenfj@users.noreply.github.com> Date: Thu, 25 Dec 2025 10:24:27 +0100 Subject: [PATCH 036/128] refactor to new utils ros packages (#648) * refactor to new utils ros packages * added utils dep for velocity controller * fix typo --- control/dp_adapt_backs_controller/CMakeLists.txt | 2 ++ control/dp_adapt_backs_controller/package.xml | 1 + .../src/dp_adapt_backs_controller_ros.cpp | 2 +- control/velocity_controller_lqr/CMakeLists.txt | 2 ++ control/velocity_controller_lqr/package.xml | 1 + .../scripts/velocity_controller_lqr_node.py | 4 ++-- guidance/los_guidance/CMakeLists.txt | 4 ++-- guidance/los_guidance/package.xml | 2 +- guidance/los_guidance/src/los_guidance_ros.cpp | 2 +- guidance/reference_filter_dp/CMakeLists.txt | 2 ++ guidance/reference_filter_dp/package.xml | 1 + guidance/reference_filter_dp/src/reference_filter_ros.cpp | 2 +- mission/waypoint_manager/CMakeLists.txt | 4 ++-- mission/waypoint_manager/package.xml | 2 +- mission/waypoint_manager/src/waypoint_manager_ros.cpp | 2 +- motion/thrust_allocator_auv/CMakeLists.txt | 4 ++-- motion/thrust_allocator_auv/package.xml | 2 +- motion/thrust_allocator_auv/src/thrust_allocator_ros.cpp | 2 +- motion/thruster_interface_auv/CMakeLists.txt | 4 ++-- motion/thruster_interface_auv/package.xml | 2 +- .../thruster_interface_auv/src/thruster_interface_auv_ros.cpp | 2 +- 21 files changed, 29 insertions(+), 20 deletions(-) diff --git a/control/dp_adapt_backs_controller/CMakeLists.txt b/control/dp_adapt_backs_controller/CMakeLists.txt index 09bf9709c..4aae02409 100644 --- a/control/dp_adapt_backs_controller/CMakeLists.txt +++ b/control/dp_adapt_backs_controller/CMakeLists.txt @@ -11,6 +11,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(vortex_utils REQUIRED) +find_package(vortex_utils_ros REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(nav_msgs REQUIRED) @@ -41,6 +42,7 @@ ament_target_dependencies(${LIB_NAME} PUBLIC spdlog vortex_msgs vortex_utils + vortex_utils_ros ) rclcpp_components_register_node( diff --git a/control/dp_adapt_backs_controller/package.xml b/control/dp_adapt_backs_controller/package.xml index 2d1b6f547..0b708c8b7 100644 --- a/control/dp_adapt_backs_controller/package.xml +++ b/control/dp_adapt_backs_controller/package.xml @@ -16,6 +16,7 @@ tf2 vortex_msgs vortex_utils + vortex_utils_ros ament_cmake diff --git a/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_ros.cpp b/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_ros.cpp index e2cb298e5..18be10a52 100644 --- a/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_ros.cpp +++ b/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_ros.cpp @@ -3,7 +3,7 @@ #include #include #include -#include +#include #include #include "dp_adapt_backs_controller/dp_adapt_backs_controller_utils.hpp" #include "dp_adapt_backs_controller/typedefs.hpp" diff --git a/control/velocity_controller_lqr/CMakeLists.txt b/control/velocity_controller_lqr/CMakeLists.txt index 05f937270..791fdefcf 100644 --- a/control/velocity_controller_lqr/CMakeLists.txt +++ b/control/velocity_controller_lqr/CMakeLists.txt @@ -6,6 +6,8 @@ find_package(rclpy REQUIRED) find_package(nav_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(vortex_msgs REQUIRED) +find_package(vortex_utils REQUIRED) +find_package(vortex_utils_ros REQUIRED) install(DIRECTORY launch diff --git a/control/velocity_controller_lqr/package.xml b/control/velocity_controller_lqr/package.xml index 8f2935a41..9e389e70f 100644 --- a/control/velocity_controller_lqr/package.xml +++ b/control/velocity_controller_lqr/package.xml @@ -14,6 +14,7 @@ nav_msgs vortex_msgs vortex_utils + vortex_utils_ros python-control-pip std_msgs diff --git a/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py b/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py index 1d9f9dba4..0ae504430 100755 --- a/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py +++ b/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py @@ -16,8 +16,8 @@ ) from vortex_msgs.msg import LOSGuidance from vortex_utils.python_utils import State -from vortex_utils.qos_profiles import reliable_profile, sensor_data_profile -from vortex_utils.ros_converter import pose_from_ros, twist_from_ros +from vortex_utils_ros.qos_profiles import reliable_profile, sensor_data_profile +from vortex_utils_ros.ros_converter import pose_from_ros, twist_from_ros class LinearQuadraticRegulator(Node): diff --git a/guidance/los_guidance/CMakeLists.txt b/guidance/los_guidance/CMakeLists.txt index 443e7cb51..6a51f5780 100644 --- a/guidance/los_guidance/CMakeLists.txt +++ b/guidance/los_guidance/CMakeLists.txt @@ -13,7 +13,7 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(vortex_msgs REQUIRED) -find_package(vortex_utils REQUIRED) +find_package(vortex_utils_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(Eigen3 REQUIRED) find_package(spdlog REQUIRED) @@ -36,7 +36,7 @@ ament_target_dependencies(${LIB_NAME} rclcpp_action geometry_msgs vortex_msgs - vortex_utils + vortex_utils_ros Eigen3 spdlog fmt diff --git a/guidance/los_guidance/package.xml b/guidance/los_guidance/package.xml index 9ee0850b1..5ff6218fe 100644 --- a/guidance/los_guidance/package.xml +++ b/guidance/los_guidance/package.xml @@ -13,7 +13,7 @@ rclcpp_action geometry_msgs vortex_msgs - vortex_utils + vortex_utils_ros eigen diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index d365510a3..6447ecc51 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -2,7 +2,7 @@ #include #include #include -#include +#include #include "los_guidance/lib/types.hpp" const auto start_message = R"( diff --git a/guidance/reference_filter_dp/CMakeLists.txt b/guidance/reference_filter_dp/CMakeLists.txt index e2c22f522..d7754462a 100644 --- a/guidance/reference_filter_dp/CMakeLists.txt +++ b/guidance/reference_filter_dp/CMakeLists.txt @@ -15,6 +15,7 @@ find_package(rclcpp_action REQUIRED) find_package(rclcpp_components REQUIRED) find_package(vortex_msgs REQUIRED) find_package(vortex_utils REQUIRED) +find_package(vortex_utils_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(Eigen3 REQUIRED) @@ -38,6 +39,7 @@ ament_target_dependencies(${LIB_NAME} PUBLIC Eigen3 vortex_msgs vortex_utils + vortex_utils_ros spdlog fmt ) diff --git a/guidance/reference_filter_dp/package.xml b/guidance/reference_filter_dp/package.xml index 659e742b2..40793be91 100644 --- a/guidance/reference_filter_dp/package.xml +++ b/guidance/reference_filter_dp/package.xml @@ -14,6 +14,7 @@ geometry_msgs vortex_msgs vortex_utils + vortex_utils_ros nav_msgs eigen diff --git a/guidance/reference_filter_dp/src/reference_filter_ros.cpp b/guidance/reference_filter_dp/src/reference_filter_ros.cpp index a08f415c7..e7b1c231d 100644 --- a/guidance/reference_filter_dp/src/reference_filter_ros.cpp +++ b/guidance/reference_filter_dp/src/reference_filter_ros.cpp @@ -3,7 +3,7 @@ #include #include #include -#include +#include #include const auto start_message = R"( diff --git a/mission/waypoint_manager/CMakeLists.txt b/mission/waypoint_manager/CMakeLists.txt index 0a76ae510..451de3132 100644 --- a/mission/waypoint_manager/CMakeLists.txt +++ b/mission/waypoint_manager/CMakeLists.txt @@ -14,7 +14,7 @@ find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(rclcpp_components REQUIRED) find_package(vortex_msgs REQUIRED) -find_package(vortex_utils REQUIRED) +find_package(vortex_utils_ros REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(spdlog REQUIRED) find_package(fmt REQUIRED) @@ -31,7 +31,7 @@ ament_target_dependencies(${LIB_NAME} PUBLIC rclcpp_components rclcpp_action vortex_msgs - vortex_utils + vortex_utils_ros tf2_geometry_msgs spdlog fmt diff --git a/mission/waypoint_manager/package.xml b/mission/waypoint_manager/package.xml index db3feabde..be6d9148b 100644 --- a/mission/waypoint_manager/package.xml +++ b/mission/waypoint_manager/package.xml @@ -13,7 +13,7 @@ rclcpp_action geometry_msgs vortex_msgs - vortex_utils + vortex_utils_ros tf2_geometry_msgs launch_testing diff --git a/mission/waypoint_manager/src/waypoint_manager_ros.cpp b/mission/waypoint_manager/src/waypoint_manager_ros.cpp index 472cf9378..93a9da5d2 100644 --- a/mission/waypoint_manager/src/waypoint_manager_ros.cpp +++ b/mission/waypoint_manager/src/waypoint_manager_ros.cpp @@ -2,7 +2,7 @@ #include #include #include -#include +#include namespace vortex::mission { diff --git a/motion/thrust_allocator_auv/CMakeLists.txt b/motion/thrust_allocator_auv/CMakeLists.txt index 1b8cd0b99..3063537a0 100644 --- a/motion/thrust_allocator_auv/CMakeLists.txt +++ b/motion/thrust_allocator_auv/CMakeLists.txt @@ -13,7 +13,7 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(vortex_msgs REQUIRED) -find_package(vortex_utils REQUIRED) +find_package(vortex_utils_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(Eigen3 REQUIRED) find_package(spdlog REQUIRED) @@ -33,7 +33,7 @@ add_library(${LIB_NAME} SHARED geometry_msgs Eigen3 vortex_msgs - vortex_utils + vortex_utils_ros fmt spdlog ) diff --git a/motion/thrust_allocator_auv/package.xml b/motion/thrust_allocator_auv/package.xml index feb767aaf..f5b492348 100644 --- a/motion/thrust_allocator_auv/package.xml +++ b/motion/thrust_allocator_auv/package.xml @@ -10,7 +10,7 @@ rclcpp geometry_msgs vortex_msgs - vortex_utils + vortex_utils_ros eigen ament_cmake diff --git a/motion/thrust_allocator_auv/src/thrust_allocator_ros.cpp b/motion/thrust_allocator_auv/src/thrust_allocator_ros.cpp index 3785b718f..eba02da21 100644 --- a/motion/thrust_allocator_auv/src/thrust_allocator_ros.cpp +++ b/motion/thrust_allocator_auv/src/thrust_allocator_ros.cpp @@ -4,7 +4,7 @@ #include #include #include -#include +#include #include "thrust_allocator_auv/pseudoinverse_allocator.hpp" #include "thrust_allocator_auv/thrust_allocator_utils.hpp" diff --git a/motion/thruster_interface_auv/CMakeLists.txt b/motion/thruster_interface_auv/CMakeLists.txt index 0ee76eb4a..9813202b5 100644 --- a/motion/thruster_interface_auv/CMakeLists.txt +++ b/motion/thruster_interface_auv/CMakeLists.txt @@ -10,7 +10,7 @@ find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(std_msgs REQUIRED) find_package(vortex_msgs REQUIRED) -find_package(vortex_utils REQUIRED) +find_package(vortex_utils_ros REQUIRED) find_package(spdlog REQUIRED) find_package(fmt REQUIRED) @@ -27,7 +27,7 @@ ament_target_dependencies(${LIB_NAME} PUBLIC rclcpp_components std_msgs vortex_msgs - vortex_utils + vortex_utils_ros spdlog fmt ) diff --git a/motion/thruster_interface_auv/package.xml b/motion/thruster_interface_auv/package.xml index 0b1860e4f..d4a05278a 100644 --- a/motion/thruster_interface_auv/package.xml +++ b/motion/thruster_interface_auv/package.xml @@ -12,7 +12,7 @@ rclcpp std_msgs vortex_msgs - vortex_utils + vortex_utils_ros ament_cmake diff --git a/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp index 8681fa025..603b2d46e 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp @@ -2,7 +2,7 @@ #include #include #include -#include +#include const auto start_message = R"( _____ _ _ ___ _ __ From 76da1d94748ea41bbbed264cb5bebe2f9ec7881e Mon Sep 17 00:00:00 2001 From: Henrik Date: Mon, 5 Jan 2026 12:20:14 +0100 Subject: [PATCH 037/128] Added support for vortex utils and changed to using their ssa --- control/velocity_controller/CMakeLists.txt | 20 ++++++++++++------- .../include/velocity_controller/LQR_setup.hpp | 2 +- control/velocity_controller/src/LQR_setup.cpp | 11 ++++++---- .../src/velocity_controller.cpp | 3 +++ 4 files changed, 24 insertions(+), 12 deletions(-) diff --git a/control/velocity_controller/CMakeLists.txt b/control/velocity_controller/CMakeLists.txt index 228f188b0..4f072393e 100644 --- a/control/velocity_controller/CMakeLists.txt +++ b/control/velocity_controller/CMakeLists.txt @@ -13,6 +13,7 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(vortex_msgs REQUIRED) +find_package(vortex_utils REQUIRED) find_package(Eigen3 REQUIRED) find_package(CasADi REQUIRED) find_package(LAPACK REQUIRED) @@ -20,7 +21,11 @@ find_package(BLAS REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) - +#target_include_directories(velocity_controller_node +# PUBLIC +# $ +# $ +#) include_directories( ${EIGEN3_INCLUDE_DIR} include @@ -49,9 +54,10 @@ ament_target_dependencies(velocity_controller_node std_msgs vortex_msgs geometry_msgs - Eigen3 + #Eigen3 CasADi nav_msgs + vortex_utils ) @@ -60,13 +66,13 @@ ament_target_dependencies(test_VC_node std_msgs vortex_msgs geometry_msgs - Eigen3 + #Eigen3 nav_msgs ) ament_target_dependencies(test_LQR_node rclcpp - Eigen3 + #Eigen3 vortex_msgs geometry_msgs std_msgs @@ -77,9 +83,9 @@ link_directories(/usr/lib/gcc/x86_64-linux-gnu/11) set(SLICOT_LIB /usr/lib/x86_64-linux-gnu/libslicot.so) set(GFORTRAN_LIB /usr/lib/gcc/x86_64-linux-gnu/11/libgfortran.so) -target_link_libraries(velocity_controller_node ${LAPACK_LIBRARIES} ${BLAS_LIBRARIES} ${SLICOT_LIB} ${GFORTRAN_LIB}) -target_link_libraries(test_VC_node ${LAPACK_LIBRARIES} ${BLAS_LIBRARIES} ${SLICOT_LIB} ${GFORTRAN_LIB}) -target_link_libraries(test_LQR_node ${LAPACK_LIBRARIES} ${BLAS_LIBRARIES} ${SLICOT_LIB} ${GFORTRAN_LIB}) +target_link_libraries(velocity_controller_node ${LAPACK_LIBRARIES} ${BLAS_LIBRARIES} ${SLICOT_LIB} ${GFORTRAN_LIB} Eigen3::Eigen) +target_link_libraries(test_VC_node ${LAPACK_LIBRARIES} ${BLAS_LIBRARIES} ${SLICOT_LIB} ${GFORTRAN_LIB} Eigen3::Eigen) +target_link_libraries(test_LQR_node ${LAPACK_LIBRARIES} ${BLAS_LIBRARIES} ${SLICOT_LIB} ${GFORTRAN_LIB} Eigen3::Eigen) install(TARGETS diff --git a/control/velocity_controller/include/velocity_controller/LQR_setup.hpp b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp index 9b39f4746..cfb4a4395 100644 --- a/control/velocity_controller/include/velocity_controller/LQR_setup.hpp +++ b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp @@ -51,7 +51,7 @@ class LQRController{ void update_augmented_matrices(Eigen::Matrix3d coriolis_matrix); //angle quaternion_to_euler_angle(double w, double x, double y, double z); - double ssa(double angle); + //double ssa(double angle); std::tuple saturate (double value, bool windup, double limit); double anti_windup(double ki, double error, double integral_sum, bool windup); diff --git a/control/velocity_controller/src/LQR_setup.cpp b/control/velocity_controller/src/LQR_setup.cpp index 8d77b51ba..8bbd0dbc4 100644 --- a/control/velocity_controller/src/LQR_setup.cpp +++ b/control/velocity_controller/src/LQR_setup.cpp @@ -13,6 +13,9 @@ #include "velocity_controller/utilities.hpp" #include #include +#include "vortex/utils/math.hpp" + + Eigen::IOFormat fmt(Eigen::StreamPrecision, 0, ", ", "\n", "[", "]"); LQRController::LQRController(LQRparameters params,Eigen::Matrix3d inertia_matrix){ set_params(params); @@ -39,9 +42,9 @@ LQRController::LQRController(LQRparameters params,Eigen::Matrix3d inertia_matrix return {phi, theta, psi}; };*/ -double LQRController::ssa(double angle){ +/*double LQRController::ssa(double angle){ return std::fmod(angle+pi, 2*pi)-pi; -}; +};*/ //Can be optimized std::tuple LQRController::saturate (double value, bool windup, double limit){ @@ -115,8 +118,8 @@ void LQRController::update_augmented_matrices(Eigen::Matrix3d coriolis_matrix){ }; Eigen::Vector LQRController::update_error(Guidance_data guidance_values, State states){ double surge_error = guidance_values.surge - states.surge; - double pitch_error = ssa(guidance_values.pitch - states.pitch); - double yaw_error = ssa(guidance_values.yaw - states.yaw); + double pitch_error = vortex::utils::math::ssa(guidance_values.pitch - states.pitch); + double yaw_error = vortex::utils::math::ssa(guidance_values.yaw - states.yaw); integral_error_surge = anti_windup(i_surge, surge_error, integral_error_surge, surge_windup); integral_error_pitch = anti_windup(i_pitch, pitch_error, integral_error_pitch, pitch_windup); diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index 94c4fdfe8..1875bcd80 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -10,6 +10,9 @@ #include #include #include "vortex_msgs/msg/los_guidance.hpp" +#include "vortex/utils/math.hpp" + + //#include "vortex-msgs/msg" kan legge til nye meldinger nå //Lager en klasse velocity node From cb4156b15a5b7a2bf537ab1d267ce66c81dde215 Mon Sep 17 00:00:00 2001 From: Anbit Date: Tue, 6 Jan 2026 12:32:51 +0100 Subject: [PATCH 038/128] Fix: Issues from feedback on PR draft --- guidance/los_guidance/CMakeLists.txt | 8 ++ .../los_guidance/config/guidance_params.yaml | 2 + .../include/los_guidance/lib/adaptive_los.hpp | 2 +- .../include/los_guidance/lib/integral_los.hpp | 2 +- .../los_guidance/lib/proportional_los.hpp | 2 +- .../include/los_guidance/los_guidance_ros.hpp | 13 ++- .../los_guidance/src/los_guidance_ros.cpp | 85 ++++++++----------- 7 files changed, 56 insertions(+), 58 deletions(-) diff --git a/guidance/los_guidance/CMakeLists.txt b/guidance/los_guidance/CMakeLists.txt index 6a51f5780..00182bc1c 100644 --- a/guidance/los_guidance/CMakeLists.txt +++ b/guidance/los_guidance/CMakeLists.txt @@ -46,6 +46,14 @@ add_executable(los_guidance_node src/los_guidance_node.cpp ) +ament_target_dependencies(los_guidance_node + rclcpp + rclcpp_action + geometry_msgs + vortex_msgs + vortex_utils_ros +) + target_link_libraries(los_guidance_node ${LIB_NAME} yaml-cpp diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 64f922a27..0639512ed 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -18,6 +18,8 @@ integer_los: k_i_h: 0.1 k_i_v: 0.1 + + common: active_los_method: 2 # 0: Proportional, 1: Integral, 2: Adaptive u_desired: 0.3 diff --git a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp index 9b87a68cd..4f8bc189e 100644 --- a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp @@ -41,7 +41,7 @@ class AdaptiveLOSGuidance { double pi_h_{}; double pi_v_{}; double beta_c_hat_{}; - double alpha_c_hat_{}; + double alpha_c_hat_{}; }; // namespace vortex::guidance::los diff --git a/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp b/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp index dbb437241..a8e501709 100644 --- a/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp @@ -16,7 +16,7 @@ struct IntegralLosParams { double k_i_h{}; double k_i_v{}; double time_step{}; -}; +}; class IntegralLOSGuidance { public: diff --git a/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp b/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp index f5ff3d6d9..2a5ea00a1 100644 --- a/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp @@ -12,7 +12,7 @@ struct ProportionalLosParams { double lookahead_distance_v{}; double k_p_h{}; double k_p_v{}; - double time_step{}; + double time_step{}; }; class ProportionalLOSGuidance { diff --git a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp index 1a7bb74a2..68ef71e49 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -5,7 +5,6 @@ #include #include #include -#include #include #include #include @@ -15,8 +14,8 @@ #include #include "los_guidance/lib/adaptive_los.hpp" #include "los_guidance/lib/integral_los.hpp" -#include "los_guidance/lib/proportional_los.hpp" -#include "los_guidance/lib/types.hpp" +#include "los_guidance/lib/proportional_los.hpp" +#include "los_guidance/lib/types.hpp" namespace vortex::guidance::los { @@ -122,10 +121,10 @@ class LosGuidanceNode : public rclcpp::Node { double goal_reached_tol_{}; - std::unique_ptr m_adaptive_los{}; - std::unique_ptr m_integral_los{}; - std::unique_ptr m_proportional_los{}; - types::ActiveLosMethod m_method{}; + std::unique_ptr adaptive_los_{}; + std::unique_ptr integral_los_{}; + std::unique_ptr proportional_los_{}; + types::ActiveLosMethod method_{}; }; } // namespace vortex::guidance::los diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 6447ecc51..04facf075 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -97,66 +97,57 @@ void LosGuidanceNode::set_service_server() { void LosGuidanceNode::set_adaptive_los_guidance(YAML::Node config) { auto adaptive_los_config = config["adaptive_los"]; - spdlog::info("A0"); auto params = AdaptiveLosParams{}; params.lookahead_distance_h = adaptive_los_config["lookahead_distance_h"].as(); - spdlog::info("A1"); params.lookahead_distance_v = adaptive_los_config["lookahead_distance_v"].as(); - spdlog::info("A2"); - params.gamma_h = adaptive_los_config["gamma_h"].as(); - spdlog::info("A3"); - params.gamma_v = adaptive_los_config["gamma_v"].as(); - spdlog::info("A4"); - params.time_step = static_cast(time_step_.count()) / 1000.0; - spdlog::info("A5"); - - m_adaptive_los = std::make_unique(params); + params.gamma_h = + adaptive_los_config["gamma_h"].as(); + params.gamma_v = + adaptive_los_config["gamma_v"].as(); + params.time_step = + static_cast(time_step_.count()) / 1000.0; + + adaptive_los_ = std::make_unique(params); } void LosGuidanceNode::set_proportional_los_guidance(YAML::Node config) { auto proportional_los_config = config["prop_los"]; - spdlog::info("P0"); auto params = ProportionalLosParams{}; params.lookahead_distance_h = proportional_los_config["lookahead_distance_h"].as(); - spdlog::info("P1"); + params.lookahead_distance_v = proportional_los_config["lookahead_distance_v"].as(); - spdlog::info("P2"); - params.k_p_h = proportional_los_config["k_p_h"].as(); - spdlog::info("P3"); - params.k_p_v = proportional_los_config["k_p_v"].as(); - spdlog::info("P4"); - params.time_step = static_cast(time_step_.count()) / 1000.0; - spdlog::info("P5"); - - m_proportional_los = std::make_unique(params); + params.k_p_h = + proportional_los_config["k_p_h"].as(); + params.k_p_v = + proportional_los_config["k_p_v"].as(); + params.time_step = + static_cast(time_step_.count()) / 1000.0; + + proportional_los_ = std::make_unique(params); } void LosGuidanceNode::set_integral_los_guidance(YAML::Node config) { auto integral_los_config = config["integer_los"]; - spdlog::info("I0"); auto params = IntegralLosParams{}; params.lookahead_distance_h = integral_los_config["lookahead_distance_h"].as(); - spdlog::info("I1"); params.lookahead_distance_v = integral_los_config["lookahead_distance_v"].as(); - spdlog::info("I2"); - params.k_p_h = integral_los_config["k_p_h"].as(); - spdlog::info("I3"); - params.k_p_v = integral_los_config["k_p_v"].as(); - spdlog::info("I4"); - params.k_i_h = integral_los_config["k_i_h"].as(); - spdlog::info("I5"); - params.k_i_v = integral_los_config["k_i_v"].as(); - spdlog::info("I6"); - params.time_step = static_cast(time_step_.count()) / 1000.0; - spdlog::info("I7"); - - m_integral_los = std::make_unique(params); + params.k_p_h = + integral_los_config["k_p_h"].as(); + params.k_p_v = + integral_los_config["k_p_v"].as(); + params.k_i_h = + integral_los_config["k_i_h"].as(); + params.k_i_v = + integral_los_config["k_i_v"].as(); + params.time_step = + static_cast(time_step_.count()) / 1000.0; + integral_los_ = std::make_unique(params); } void LosGuidanceNode::waypoint_callback( @@ -209,8 +200,8 @@ void LosGuidanceNode::handle_accepted( void LosGuidanceNode::set_los_mode( const std::shared_ptr request, std::shared_ptr response) { - m_method = static_cast(request->mode); - spdlog::info("LOS mode set to {}", static_cast(m_method)); + method_ = static_cast(request->mode); + spdlog::info("LOS mode set to {}", static_cast(method_)); response->success = true; } vortex_msgs::msg::LOSGuidance LosGuidanceNode::fill_los_reference( @@ -229,13 +220,11 @@ YAML::Node LosGuidanceNode::get_los_config(std::string yaml_file_path) { } void LosGuidanceNode::parse_common_config(YAML::Node common_config) { + std::lock_guard lock(mutex_); u_desired_ = common_config["u_desired"].as(); - spdlog::info("C1"); goal_reached_tol_ = common_config["goal_reached_tol"].as(); - spdlog::info("C2"); - m_method = static_cast( + method_ = static_cast( common_config["active_los_method"].as()); - spdlog::info("C3"); } void LosGuidanceNode::execute( @@ -281,15 +270,15 @@ void LosGuidanceNode::execute( types::Outputs outputs; - switch (m_method) { + switch (method_) { case types::ActiveLosMethod::ADAPTIVE: - outputs = m_adaptive_los->calculate_outputs(path_inputs_); + outputs = adaptive_los_->calculate_outputs(path_inputs_); break; case types::ActiveLosMethod::PROPORTIONAL: - outputs = m_proportional_los->calculate_outputs(path_inputs_); + outputs = proportional_los_->calculate_outputs(path_inputs_); break; case types::ActiveLosMethod::INTEGRAL: - outputs = m_integral_los->calculate_outputs(path_inputs_); + outputs = integral_los_->calculate_outputs(path_inputs_); break; default: spdlog::error("Invalid LOS method selected"); @@ -318,4 +307,4 @@ void LosGuidanceNode::execute( } } -} // namespace vortex::guidance::los +} // namespace vortex::guidance::los From 5ce96efe159b9e1c76d97c86d849f2a81cec3342 Mon Sep 17 00:00:00 2001 From: Henrik Date: Tue, 6 Jan 2026 12:32:55 +0100 Subject: [PATCH 039/128] New parameters for water resistance --- control/velocity_controller/config/parameters.yaml | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/control/velocity_controller/config/parameters.yaml b/control/velocity_controller/config/parameters.yaml index 9f396c83d..8aea0ed68 100644 --- a/control/velocity_controller/config/parameters.yaml +++ b/control/velocity_controller/config/parameters.yaml @@ -27,11 +27,15 @@ dt: 0.1 - inertia_matrix: [30.0, 0.6, 0.0, 0.6, 1.629, 0.0, 0.0, 0.0, 1.729] + inertia_matrix: [ 30.0, 0.0, 0.0, 0.0, 0.0, 0.6, 0.0, 30.0, 0.0, 0.0, -0.6, 0.3, 0.0, 0.0, 30.0, 0.6, 0.3, 0.0, 0.0, 0.0, 0.6, 0.68, 0.0, 0.0, 0.0, -0.6, 0.3, 0.0, 3.32, 0.0, 0.6, 0.3, 0.0, 0.0, 0.0, 3.34] + + + dampening_matrix_low: [1.0,0.0,0.0,0.0,0.0,0.0, 0.0,1.0,0.0,0.0,0.0,0.0, 0.0,0.0,1.0,0.0,0.0,0.0, 0.0,0.0,0.0,1.0,0.0,0.0, 0.0,0.0,0.0,0.0,1.0,0.0, 0.0,0.0,0.0,0.0,0.0,1.0] + dampening_matrix_high: [1.0,0.0,0.0,0.0,0.0,0.0, 0.0,1.0,0.0,0.0,0.0,0.0, 0.0,0.0,1.0,0.0,0.0,0.0, 0.0,0.0,0.0,1.0,0.0,0.0, 0.0,0.0,0.0,0.0,1.0,0.0, 0.0,0.0,0.0,0.0,0.0,1.0] calculation_rate: 200 #ms integer publish_rate: 100 #ms #Clamp parameter max_force: 1000.0 #should maybe be 99.5 - controller_type: 2 #1 PID 2 LQR + controller_type: 1 #1 PID 2 LQR From 1305ea7c3da88bb45bb97cb626193c675b789527 Mon Sep 17 00:00:00 2001 From: Henrik Date: Tue, 6 Jan 2026 12:34:20 +0100 Subject: [PATCH 040/128] removed unneccessary features, added euler angle publisher --- .../include/velocity_controller/test_VC.hpp | 23 ++++--- .../launch/VCnTest.launch.py | 2 +- control/velocity_controller/src/test_VC.cpp | 67 +++++-------------- 3 files changed, 31 insertions(+), 61 deletions(-) diff --git a/control/velocity_controller/include/velocity_controller/test_VC.hpp b/control/velocity_controller/include/velocity_controller/test_VC.hpp index c8b4fb85b..18a81f91f 100644 --- a/control/velocity_controller/include/velocity_controller/test_VC.hpp +++ b/control/velocity_controller/include/velocity_controller/test_VC.hpp @@ -15,31 +15,34 @@ class test_VC : public rclcpp::Node{ public: test_VC(); //Callback functions - void read_thrust(geometry_msgs::msg::WrenchStamped::SharedPtr msg); + //void read_thrust(geometry_msgs::msg::WrenchStamped::SharedPtr msg); void send_guidance(); - void send_state(); + void odometry_callback(const nav_msgs::msg::Odometry::SharedPtr msg_ptr); + //void send_state(); //Variables - //guidance_data reference; - Guidance_data current_state; + //Subscribers and publishers rclcpp::Publisher::SharedPtr publisher_guidance; - rclcpp::Publisher::SharedPtr publisher_odom; - rclcpp::Subscription::SharedPtr subscription_thrust; + rclcpp::Publisher::SharedPtr publisher_state; + rclcpp::Subscription::SharedPtr subscription_state; //Timers rclcpp::TimerBase::SharedPtr timer_; rclcpp::Clock::SharedPtr clock_; //Messages - std::vector thrust_vector; + //std::vector thrust_vector; vortex_msgs::msg::LOSGuidance reference_msg; //Topics - std::string topic_odom; - std::string topic_thrust; + //std::string topic_odom; + //std::string topic_thrust; std::string topic_guidance; + std::string topic_state; + std::string topic_odometry; + //MSGS - nav_msgs::msg::Odometry odom_msg; + //nav_msgs::msg::Odometry odom_msg; }; geometry_msgs::msg::Quaternion euler_angle_to_quaternion(double roll, double pitch, double yaw); \ No newline at end of file diff --git a/control/velocity_controller/launch/VCnTest.launch.py b/control/velocity_controller/launch/VCnTest.launch.py index 6e2e20efd..454a50d96 100644 --- a/control/velocity_controller/launch/VCnTest.launch.py +++ b/control/velocity_controller/launch/VCnTest.launch.py @@ -18,7 +18,7 @@ def generate_launch_description(): PythonLaunchDescriptionSource( os.path.join(stonefish_dir, 'launch', 'simulation.launch.py') ), - launch_arguments={'rendering_quality': 'low','rendering':'false'}.items(), + launch_arguments={'rendering_quality': 'low','rendering':'true'}.items(), ) orca_sim = TimerAction( period=12.0, diff --git a/control/velocity_controller/src/test_VC.cpp b/control/velocity_controller/src/test_VC.cpp index 5145c4f59..b9ff1ccd6 100644 --- a/control/velocity_controller/src/test_VC.cpp +++ b/control/velocity_controller/src/test_VC.cpp @@ -12,23 +12,21 @@ //#include "LQR_setup.hpp" //Denne noden er kun for å teste velocity_controller noden -test_VC::test_VC() : Node("test_VC_node"), current_state(0,2,2) +test_VC::test_VC() : Node("test_VC_node") { this->declare_parameter("topics.guidance_topic"); - this->declare_parameter("topics.thrust_topic"); - this->declare_parameter("topics.odom_topic"); - topic_thrust = this->get_parameter("topics.thrust_topic").as_string(); - topic_odom = this->get_parameter("topics.odom_topic").as_string(); - topic_guidance = this->get_parameter("topics.guidance_topic").as_string(); + this->declare_parameter("topics.odometry_topic"); + this->topic_guidance=this->get_parameter("topics.guidance_topic").as_string(); + this->topic_odometry=this->get_parameter("topics.odometry_topic").as_string(); + topic_state="state"; publisher_guidance = this->create_publisher(topic_guidance, 10); - publisher_odom = this->create_publisher(topic_odom,10); - + publisher_state = this->create_publisher(topic_state,10); + subscription_state = this->create_subscription( + topic_odometry,10, + std::bind(&test_VC::odometry_callback,this,std::placeholders::_1)); rclcpp::QoS orca_QoS(2); orca_QoS.keep_last(2).reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT); - subscription_thrust = this->create_subscription( - topic_thrust, orca_QoS, - std::bind(&test_VC::read_thrust, this, std::placeholders::_1)); timer_ = this->create_wall_timer( std::chrono::milliseconds(200), std::bind(&test_VC::send_guidance, this)); @@ -41,48 +39,16 @@ test_VC::test_VC() : Node("test_VC_node"), current_state(0,2,2) void test_VC::send_guidance() { publisher_guidance->publish(reference_msg); - //send_state(); -} - -void test_VC::read_thrust(geometry_msgs::msg::WrenchStamped::SharedPtr msg) -{ - /*current_state.surge += 0.01 * msg->wrench.force.x; - current_state.pitch += 0.01 * msg->wrench.torque.x; - current_state.yaw += 0.01 * msg->wrench.torque.y;*/ - //RCLCPP_INFO(this->get_logger(),"info: '%f'", current_state.surge); - //RCLCPP_INFO(this->get_logger(),"info: '%f'", current_state.pitch); - //RCLCPP_INFO(this->get_logger(),"info: '%f'", current_state.yaw); - (void) msg; - return; } -void test_VC::send_state() -{ - - odom_msg.header.stamp = clock_->now(); - odom_msg.header.frame_id = "base_link"; - odom_msg.twist.twist.linear.x = current_state.surge; - odom_msg.pose.pose.orientation = euler_angle_to_quaternion(0.0, current_state.pitch, current_state.yaw); - odom_msg.twist.twist.linear.y=1; - odom_msg.twist.twist.linear.z=1; - odom_msg.twist.twist.angular.x=1; - odom_msg.twist.twist.angular.y=1; - odom_msg.twist.twist.angular.z=1; - odom_msg.twist.twist.linear.y=1; - odom_msg.twist.twist.linear.z=1; - +void test_VC::odometry_callback(const nav_msgs::msg::Odometry::SharedPtr msg_ptr){ + vortex_msgs::msg::LOSGuidance msg; + angle temp=quaternion_to_euler_angle(msg_ptr->pose.pose.orientation.w, msg_ptr->pose.pose.orientation.x, msg_ptr->pose.pose.orientation.y, msg_ptr->pose.pose.orientation.z); + msg.set__pitch(temp.thetat); + msg.set__yaw(temp.phit); + publisher_state->publish(msg); - - - publisher_odom->publish(odom_msg); - - //RCLCPP_INFO(this->get_logger(), "Published state: '%f'", current_state.surge); - return; - //RCLCPP_INFO(this->get_logger(), "Published state: '%f'", current_state.pitch); - //RCLCPP_INFO(this->get_logger(), "Published state: '%f'", current_state.yaw); } - - int main(int argc, char const *argv[]) { @@ -107,4 +73,5 @@ geometry_msgs::msg::Quaternion euler_angle_to_quaternion(double roll, double pit q.z = cr * cp * sy - sr * sp * cy; return q; -} \ No newline at end of file +} + \ No newline at end of file From f5d698a0e332d4cba2cf9ebf6da34607c50a8adb Mon Sep 17 00:00:00 2001 From: Henrik Date: Tue, 6 Jan 2026 12:34:45 +0100 Subject: [PATCH 041/128] fixed dumb bugs --- control/velocity_controller/CMakeLists.txt | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/control/velocity_controller/CMakeLists.txt b/control/velocity_controller/CMakeLists.txt index 4f072393e..a6eaa4f11 100644 --- a/control/velocity_controller/CMakeLists.txt +++ b/control/velocity_controller/CMakeLists.txt @@ -20,6 +20,10 @@ find_package(LAPACK REQUIRED) find_package(BLAS REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) +#find_package(stonefish_ros2 REQUIRED) +#find_package(auv_setup REQUIRED) +#find_package(stonefish_sim REQUIRED) +#find_package(thrust_allocator_auv REQUIRED) #target_include_directories(velocity_controller_node # PUBLIC @@ -68,6 +72,7 @@ ament_target_dependencies(test_VC_node geometry_msgs #Eigen3 nav_msgs + vortex_utils ) ament_target_dependencies(test_LQR_node @@ -77,6 +82,7 @@ ament_target_dependencies(test_LQR_node geometry_msgs std_msgs nav_msgs + vortex_utils ) link_directories(/usr/lib/gcc/x86_64-linux-gnu/11) From b7805eaca1721c8464e7fb5cf85c54869535399b Mon Sep 17 00:00:00 2001 From: Henrik Date: Tue, 6 Jan 2026 13:40:42 +0100 Subject: [PATCH 042/128] Fixed bugs so that it works --- .../include/velocity_controller/test_VC.hpp | 2 +- control/velocity_controller/src/test_VC.cpp | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/control/velocity_controller/include/velocity_controller/test_VC.hpp b/control/velocity_controller/include/velocity_controller/test_VC.hpp index 18a81f91f..6d64e671d 100644 --- a/control/velocity_controller/include/velocity_controller/test_VC.hpp +++ b/control/velocity_controller/include/velocity_controller/test_VC.hpp @@ -37,7 +37,7 @@ class test_VC : public rclcpp::Node{ //std::string topic_odom; //std::string topic_thrust; std::string topic_guidance; - std::string topic_state; + std::string topic_state="/state"; std::string topic_odometry; diff --git a/control/velocity_controller/src/test_VC.cpp b/control/velocity_controller/src/test_VC.cpp index b9ff1ccd6..ef2af22a1 100644 --- a/control/velocity_controller/src/test_VC.cpp +++ b/control/velocity_controller/src/test_VC.cpp @@ -15,10 +15,9 @@ test_VC::test_VC() : Node("test_VC_node") { this->declare_parameter("topics.guidance_topic"); - this->declare_parameter("topics.odometry_topic"); + this->declare_parameter("topics.odom_topic"); this->topic_guidance=this->get_parameter("topics.guidance_topic").as_string(); - this->topic_odometry=this->get_parameter("topics.odometry_topic").as_string(); - topic_state="state"; + this->topic_odometry=this->get_parameter("topics.odom_topic").as_string(); publisher_guidance = this->create_publisher(topic_guidance, 10); publisher_state = this->create_publisher(topic_state,10); subscription_state = this->create_subscription( @@ -32,7 +31,7 @@ test_VC::test_VC() : Node("test_VC_node") std::bind(&test_VC::send_guidance, this)); clock_ = this->get_clock(); RCLCPP_INFO(this->get_logger(), "Test_VC node has been started"); - reference_msg.surge=0.2;reference_msg.pitch=0.3;reference_msg.yaw=0.0; //Surge, pitch, yaw + reference_msg.surge=0.2;reference_msg.pitch=0.3;reference_msg.yaw=0.3; //Surge, pitch, yaw } @@ -45,7 +44,8 @@ void test_VC::odometry_callback(const nav_msgs::msg::Odometry::SharedPtr msg_ptr vortex_msgs::msg::LOSGuidance msg; angle temp=quaternion_to_euler_angle(msg_ptr->pose.pose.orientation.w, msg_ptr->pose.pose.orientation.x, msg_ptr->pose.pose.orientation.y, msg_ptr->pose.pose.orientation.z); msg.set__pitch(temp.thetat); - msg.set__yaw(temp.phit); + msg.set__yaw(temp.psit); + msg.set__surge(msg_ptr->twist.twist.linear.x); publisher_state->publish(msg); } From 8c61007ad8fc3a78b77e654672ec523cabb31d98 Mon Sep 17 00:00:00 2001 From: Anbit Date: Wed, 7 Jan 2026 11:31:22 +0100 Subject: [PATCH 043/128] Start vector Feild LOS & Clean code --- .../los_guidance/config/guidance_params.yaml | 5 ++ .../include/los_guidance/lib/adaptive_los.hpp | 2 +- .../include/los_guidance/lib/integral_los.hpp | 2 +- .../los_guidance/lib/vector_field_los.hpp | 41 ++++++++++++++++ .../los_guidance/src/lib/adaptive_los.cpp | 32 ++++++------ .../los_guidance/src/lib/integral_los.cpp | 16 +++--- .../los_guidance/src/lib/proportional_los.cpp | 27 ++++++---- .../los_guidance/src/lib/vector_field_los.cpp | 49 +++++++++++++++++++ 8 files changed, 139 insertions(+), 35 deletions(-) create mode 100644 guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp create mode 100644 guidance/los_guidance/src/lib/vector_field_los.cpp diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 0639512ed..5e56f6908 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -18,6 +18,11 @@ integer_los: k_i_h: 0.1 k_i_v: 0.1 +vector_field_los: + max_approach_angle_h: 1.0 # rad + max_approach_angle_v: 0.6 # rad + k_p_h: 0.5 + k_p_v: 0.5 common: diff --git a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp index 4f8bc189e..d82684469 100644 --- a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp @@ -35,7 +35,7 @@ class AdaptiveLOSGuidance { void update_adaptive_estimates( const types::CrossTrackError& cross_track_error); - AdaptiveLosParams m_params{}; + AdaptiveLosParams params_{}; Eigen::Matrix3d rotation_y_ = Eigen::Matrix3d::Zero(); Eigen::Matrix3d rotation_z_ = Eigen::Matrix3d::Zero(); double pi_h_{}; diff --git a/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp b/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp index a8e501709..4af1b60ac 100644 --- a/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp @@ -8,7 +8,7 @@ namespace vortex::guidance::los { -struct IntegralLosParams { +struct IntegralLosParams { double lookahead_distance_h{}; double lookahead_distance_v{}; double k_p_h{}; diff --git a/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp b/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp new file mode 100644 index 000000000..a0a171089 --- /dev/null +++ b/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp @@ -0,0 +1,41 @@ +#ifndef VECTOR_FIELD_LOS_GUIDANCE_HPP +#define VECTOR_FIELD_LOS_GUIDANCE_HPP + +#include +#include +#include +#include "los_guidance/lib/types.hpp" + +namespace vortex::guidance::los { + +struct VectorFieldLosParams { + double max_approach_angle_h{}; + double max_approach_angle_v{}; + double k_p_h{}; + double k_p_v{}; + double time_step{}; +}; + +class VectorFieldLOSGuidance { + public: + VectorFieldLOSGuidance(const VectorFieldLosParams& params); + ~VectorFieldLOSGuidance() = default; + + types::Outputs calculate_outputs(const types::Inputs& inputs); + + private: + void update_angles(const types::Inputs& inputs); + types::CrossTrackError calculate_crosstrack_error( + const types::Inputs& inputs) const; + + VectorFieldLosParams m_params{}; + + double pi_h_{0.0}; + double pi_v_{0.0}; + Eigen::AngleAxisd rotation_y_{0.0, Eigen::Vector3d::UnitY()}; + Eigen::AngleAxisd rotation_z_{0.0, Eigen::Vector3d::UnitZ()}; +}; + +} // namespace vortex::guidance::los + +#endif // VECTOR_FIELD_LOS_GUIDANCE_HPP diff --git a/guidance/los_guidance/src/lib/adaptive_los.cpp b/guidance/los_guidance/src/lib/adaptive_los.cpp index 319ce376c..4a34ccb79 100644 --- a/guidance/los_guidance/src/lib/adaptive_los.cpp +++ b/guidance/los_guidance/src/lib/adaptive_los.cpp @@ -4,7 +4,7 @@ namespace vortex::guidance::los { AdaptiveLOSGuidance::AdaptiveLOSGuidance(const AdaptiveLosParams& params) - : m_params{params} {} + : params_{params} {} void AdaptiveLOSGuidance::update_angles(const types::Inputs& inputs) { const double dx = inputs.next_point.x - inputs.prev_point.x; @@ -30,33 +30,33 @@ const types::CrossTrackError AdaptiveLOSGuidance::calculate_crosstrack_error( } void AdaptiveLOSGuidance::update_adaptive_estimates( - const types::CrossTrackError& e) { - const double denom_h = std::sqrt(m_params.lookahead_distance_h * - m_params.lookahead_distance_h + - e.y_e * e.y_e); - const double denom_v = std::sqrt(m_params.lookahead_distance_v * - m_params.lookahead_distance_v + - e.z_e * e.z_e); + const types::CrossTrackError& cross_track_error) { + const double denom_h = std::sqrt(params_.lookahead_distance_h * + params_.lookahead_distance_h + + cross_track_error.y_e * cross_track_error.y_e); + const double denom_v = std::sqrt(params_.lookahead_distance_v * + params_.lookahead_distance_v + + cross_track_error.z_e * cross_track_error.z_e); const double beta_dot = - m_params.gamma_h * (m_params.lookahead_distance_h / denom_h) * e.y_e; + params_.gamma_h * (params_.lookahead_distance_h / denom_h) * cross_track_error.y_e; const double alpha_dot = - m_params.gamma_v * (m_params.lookahead_distance_v / denom_v) * e.z_e; + params_.gamma_v * (params_.lookahead_distance_v / denom_v) * cross_track_error.z_e; - beta_c_hat_ += beta_dot * m_params.time_step; - alpha_c_hat_ += alpha_dot * m_params.time_step; + beta_c_hat_ += beta_dot * params_.time_step; + alpha_c_hat_ += alpha_dot * params_.time_step; } types::Outputs AdaptiveLOSGuidance::calculate_outputs( const types::Inputs& inputs) { update_angles(inputs); - const types::CrossTrackError e = calculate_crosstrack_error(inputs); - update_adaptive_estimates(e); + const types::CrossTrackError cross_track_error = calculate_crosstrack_error(inputs); + update_adaptive_estimates(cross_track_error); const double psi_d = - pi_h_ - beta_c_hat_ - std::atan(e.y_e / m_params.lookahead_distance_h); + pi_h_ - beta_c_hat_ - std::atan(cross_track_error.y_e / params_.lookahead_distance_h); const double theta_d = - pi_v_ + alpha_c_hat_ + std::atan(e.z_e / m_params.lookahead_distance_v); + pi_v_ + alpha_c_hat_ + std::atan(cross_track_error.z_e / params_.lookahead_distance_v); return types::Outputs{psi_d, theta_d}; } diff --git a/guidance/los_guidance/src/lib/integral_los.cpp b/guidance/los_guidance/src/lib/integral_los.cpp index 0ca1a3e2b..b1a4e3962 100644 --- a/guidance/los_guidance/src/lib/integral_los.cpp +++ b/guidance/los_guidance/src/lib/integral_los.cpp @@ -13,30 +13,30 @@ void IntegralLOSGuidance::update_angles(const types::Inputs& inputs) { difference.y * difference.y)); rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); - rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); + rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); } types::CrossTrackError IntegralLOSGuidance::calculate_crosstrack_error( const types::Inputs& inputs) { const Eigen::Vector3d diff_vec = (inputs.current_position - inputs.prev_point).as_vector(); - const Eigen::Vector3d e_perp = rotation_y_.toRotationMatrix().transpose() * + const Eigen::Vector3d path_frame_error = rotation_y_.toRotationMatrix().transpose() * rotation_z_.toRotationMatrix().transpose() * diff_vec; - return types::CrossTrackError::from_vector(e_perp); + return types::CrossTrackError::from_vector(path_frame_error); } types::Outputs IntegralLOSGuidance::calculate_outputs( const types::Inputs& inputs) { update_angles(inputs); - const types::CrossTrackError e = calculate_crosstrack_error(inputs); + const types::CrossTrackError cross_track_error = calculate_crosstrack_error(inputs); - int_h += e.y_e * m_params.time_step; - int_v += e.z_e * m_params.time_step; + int_h += cross_track_error.y_e * m_params.time_step; + int_v += cross_track_error.z_e * m_params.time_step; - const double u_h = m_params.k_p_h * e.y_e + m_params.k_i_h * int_h; - const double u_v = m_params.k_p_v * e.z_e + m_params.k_i_v * int_v; + const double u_h = m_params.k_p_h * cross_track_error.y_e + m_params.k_i_h * int_h; + const double u_v = m_params.k_p_v * cross_track_error.z_e + m_params.k_i_v * int_v; const double psi_d = pi_h_ - std::atan(u_h); const double theta_d = pi_v_ + std::atan(u_v); diff --git a/guidance/los_guidance/src/lib/proportional_los.cpp b/guidance/los_guidance/src/lib/proportional_los.cpp index 40a739e04..0bb65ba01 100644 --- a/guidance/los_guidance/src/lib/proportional_los.cpp +++ b/guidance/los_guidance/src/lib/proportional_los.cpp @@ -3,8 +3,17 @@ namespace vortex::guidance::los { ProportionalLOSGuidance::ProportionalLOSGuidance( - const ProportionalLosParams& params) - : m_params{params} {} + const ProportionalLosParams& params) : m_params{params} { + + if (m_params.lookahead_distance_h <= 0.0) { + m_params.lookahead_distance_h = 1e-9; + } + + if (m_params.lookahead_distance_v <= 0.0) { + m_params.lookahead_distance_v = 1e-9; + } + + } void ProportionalLOSGuidance::update_angles(const types::Inputs& inputs) { const types::Point difference = inputs.next_point - inputs.prev_point; @@ -21,23 +30,23 @@ types::CrossTrackError ProportionalLOSGuidance::calculate_crosstrack_error( const types::Inputs& inputs) const { const Eigen::Vector3d diff_vec = (inputs.current_position - inputs.prev_point).as_vector(); - const Eigen::Vector3d e_perp = rotation_y_.toRotationMatrix().transpose() * + const Eigen::Vector3d path_frame_error = rotation_y_.toRotationMatrix().transpose() * rotation_z_.toRotationMatrix().transpose() * diff_vec; - return types::CrossTrackError::from_vector(e_perp); + return types::CrossTrackError::from_vector(path_frame_error); } types::Outputs ProportionalLOSGuidance::calculate_outputs( const types::Inputs& inputs) { update_angles(inputs); - const types::CrossTrackError e = calculate_crosstrack_error(inputs); + const types::CrossTrackError cross_track_error = calculate_crosstrack_error(inputs); - const double k_p_h = 1.0 / std::max(m_params.lookahead_distance_h, 1e-9); - const double k_p_v = 1.0 / std::max(m_params.lookahead_distance_v, 1e-9); + const double k_p_h = 1.0 / m_params.lookahead_distance_h; + const double k_p_v = 1.0 / m_params.lookahead_distance_v; - const double psi_d = pi_h_ - std::atan(k_p_h * e.y_e); - const double theta_d = pi_v_ + std::atan(k_p_v * e.z_e); + const double psi_d = pi_h_ - std::atan(k_p_h * cross_track_error.y_e); + const double theta_d = pi_v_ + std::atan(k_p_v * cross_track_error.z_e); return types::Outputs{psi_d, theta_d}; } diff --git a/guidance/los_guidance/src/lib/vector_field_los.cpp b/guidance/los_guidance/src/lib/vector_field_los.cpp new file mode 100644 index 000000000..e008bfe57 --- /dev/null +++ b/guidance/los_guidance/src/lib/vector_field_los.cpp @@ -0,0 +1,49 @@ +#include + +namespace vortex::guidance::los { + +VectorFieldLOSGuidance::VectorFieldLOSGuidance(const VectorFieldLosParams& params) + : m_params{params} {} + +void VectorFieldLOSGuidance::update_angles(const types::Inputs& inputs) { + const types::Point difference = inputs.next_point - inputs.prev_point; + + pi_h_ = std::atan2(difference.y, difference.x); + pi_v_ = std::atan2(-difference.z, std::sqrt(difference.x * difference.x + + difference.y * difference.y)); + + rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); + rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); +} + +types::CrossTrackError VectorFieldLOSGuidance::calculate_crosstrack_error( + const types::Inputs& inputs) const { + const Eigen::Vector3d diff_vec = + (inputs.current_position - inputs.prev_point).as_vector(); + + const Eigen::Vector3d path_frame_error = rotation_y_.toRotationMatrix().transpose() * + rotation_z_.toRotationMatrix().transpose() * + diff_vec; + + return types::CrossTrackError::from_vector(path_frame_error); +} + +types::Outputs VectorFieldLOSGuidance::calculate_outputs( + const types::Inputs& inputs) { + update_angles(inputs); + const types::CrossTrackError cross_track_error = calculate_crosstrack_error(inputs); + + const double approach_h = + m_params.max_approach_angle_h * (2.0 / M_PI) * std::atan(m_params.k_p_h * cross_track_error.y_e); + + const double approach_v = + m_params.max_approach_angle_v * (2.0 / M_PI) * std::atan(m_params.k_p_v * cross_track_error.z_e); + + const double psi_d = pi_h_ - approach_h; + + const double theta_d = pi_v_ + approach_v; + + return types::Outputs{psi_d, theta_d}; +} + +} // namespace vortex::guidance::los From 294ab778e135f9c7e441f6da48027e9bd24879d7 Mon Sep 17 00:00:00 2001 From: Anbit Date: Wed, 7 Jan 2026 12:37:40 +0100 Subject: [PATCH 044/128] Adda vector field LOS & Test --- guidance/los_guidance/CMakeLists.txt | 2 +- .../include/los_guidance/lib/adaptive_los.hpp | 2 +- .../include/los_guidance/lib/types.hpp | 3 +- .../los_guidance/lib/vector_field_los.hpp | 2 +- .../include/los_guidance/los_guidance.hpp | 77 ------------ .../include/los_guidance/los_guidance_ros.hpp | 5 + .../los_guidance/src/lib/vector_field_los.cpp | 2 +- .../los_guidance/src/los_guidance_ros.cpp | 21 ++++ guidance/los_guidance/test/CMakeLists.txt | 1 + .../test/vector_field_los_test.cpp | 111 ++++++++++++++++++ 10 files changed, 144 insertions(+), 82 deletions(-) delete mode 100644 guidance/los_guidance/include/los_guidance/los_guidance.hpp create mode 100644 guidance/los_guidance/test/vector_field_los_test.cpp diff --git a/guidance/los_guidance/CMakeLists.txt b/guidance/los_guidance/CMakeLists.txt index 00182bc1c..9d8fca42b 100644 --- a/guidance/los_guidance/CMakeLists.txt +++ b/guidance/los_guidance/CMakeLists.txt @@ -29,6 +29,7 @@ add_library(${LIB_NAME} SHARED src/lib/integral_los.cpp src/lib/adaptive_los.cpp src/los_guidance_ros.cpp + src/lib/vector_field_los.cpp ) ament_target_dependencies(${LIB_NAME} @@ -84,7 +85,6 @@ install(DIRECTORY ) if(BUILD_TESTING) - include(CTest) add_subdirectory(test) endif() diff --git a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp index d82684469..f874db35f 100644 --- a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp @@ -18,7 +18,7 @@ struct AdaptiveLosParams { double lookahead_distance_v{}; double gamma_h{}; double gamma_v{}; - double time_step{}; + double time_step{}; }; class AdaptiveLOSGuidance { diff --git a/guidance/los_guidance/include/los_guidance/lib/types.hpp b/guidance/los_guidance/include/los_guidance/lib/types.hpp index 5207855cc..ca2492d10 100644 --- a/guidance/los_guidance/include/los_guidance/lib/types.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/types.hpp @@ -49,7 +49,8 @@ struct Inputs { enum class ActiveLosMethod { PROPORTIONAL, // 0 INTEGRAL, // 1 - ADAPTIVE // 2 + ADAPTIVE, // 2 + VECTOR_FIELD // 3 }; } // namespace vortex::guidance::los::types diff --git a/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp b/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp index a0a171089..9d3210ffd 100644 --- a/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp @@ -3,7 +3,7 @@ #include #include -#include +#include #include "los_guidance/lib/types.hpp" namespace vortex::guidance::los { diff --git a/guidance/los_guidance/include/los_guidance/los_guidance.hpp b/guidance/los_guidance/include/los_guidance/los_guidance.hpp deleted file mode 100644 index 71ddfdb11..000000000 --- a/guidance/los_guidance/include/los_guidance/los_guidance.hpp +++ /dev/null @@ -1,77 +0,0 @@ -#ifndef LOS_GUIDANCE__LOS_GUIDANCE_HPP_ -#define LOS_GUIDANCE__LOS_GUIDANCE_HPP_ - -#include - -namespace vortex::guidance { - -namespace LOS { - -struct Point { - double x{}; - double y{}; - double z{}; - - Point operator-(const Point& other) const { - return Point{x - other.x, y - other.y, z - other.z}; - } - - Eigen::Vector3d as_vector() const { return Eigen::Vector3d(x, y, z); } -}; - -struct CrossTrackError { - double x_e{}; - double y_e{}; - double z_e{}; - - inline static CrossTrackError from_vector(const Eigen::Vector3d& vector) { - return CrossTrackError{vector.x(), vector.y(), vector.z()}; - } -}; - -struct Params { - double lookahead_distance_h{}; - double lookahead_distance_v{}; - double gamma_h{}; - double gamma_v{}; - double time_step{}; -}; - -} // namespace LOS - -/** - * @brief Adaptive Line-of-Sight (LOS) guidance algorithm based on slide 113 - * in "Fossen 2024 Lecture on 2D and 3D path-following control". - */ -class AdaptiveLOSGuidance { - public: - explicit AdaptiveLOSGuidance(const LOS::Params& params); - ~AdaptiveLOSGuidance() = default; - - void update_angles(const LOS::Point& prev_point, - const LOS::Point& next_point); - - LOS::CrossTrackError calculate_crosstrack_error( - const LOS::Point& prev_point, - const LOS::Point& current_position) const; - - double calculate_psi_d(const double& y_e) const; - - double calculate_theta_d(const double& z_e) const; - - void update_adaptive_estimates( - const LOS::CrossTrackError& crosstrack_error); - - private: - LOS::Params params_; - Eigen::Matrix3d rotation_y_ = Eigen::Matrix3d::Zero(); - Eigen::Matrix3d rotation_z_ = Eigen::Matrix3d::Zero(); - double pi_h_{}; - double pi_v_{}; - double beta_c_hat_{}; - double alpha_c_hat_{}; -}; - -} // namespace vortex::guidance - -#endif // LOS_GUIDANCE__LOS_GUIDANCE_HPP_ diff --git a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp index 68ef71e49..fdeb06a9d 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -15,6 +15,7 @@ #include "los_guidance/lib/adaptive_los.hpp" #include "los_guidance/lib/integral_los.hpp" #include "los_guidance/lib/proportional_los.hpp" +#include "los_guidance/lib/vector_field_los.hpp" #include "los_guidance/lib/types.hpp" namespace vortex::guidance::los { @@ -42,6 +43,9 @@ class LosGuidanceNode : public rclcpp::Node { // @brief Set the integral LOS guidance parameters void set_integral_los_guidance(YAML::Node config); + // @brief Set the vector field LOS guidance parameters + void set_vector_field_guidance(YAML::Node config); + // @brief Callback for the waypoint topic // @param msg The reference message void waypoint_callback( @@ -124,6 +128,7 @@ class LosGuidanceNode : public rclcpp::Node { std::unique_ptr adaptive_los_{}; std::unique_ptr integral_los_{}; std::unique_ptr proportional_los_{}; + std::unique_ptr vector_field_los_{}; types::ActiveLosMethod method_{}; }; diff --git a/guidance/los_guidance/src/lib/vector_field_los.cpp b/guidance/los_guidance/src/lib/vector_field_los.cpp index e008bfe57..52b4f1644 100644 --- a/guidance/los_guidance/src/lib/vector_field_los.cpp +++ b/guidance/los_guidance/src/lib/vector_field_los.cpp @@ -21,7 +21,7 @@ types::CrossTrackError VectorFieldLOSGuidance::calculate_crosstrack_error( const Eigen::Vector3d diff_vec = (inputs.current_position - inputs.prev_point).as_vector(); - const Eigen::Vector3d path_frame_error = rotation_y_.toRotationMatrix().transpose() * + const Eigen::Vector3d path_frame_error = rotation_y_.toRotationMatrix().transpose() * rotation_z_.toRotationMatrix().transpose() * diff_vec; diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 04facf075..220009fc2 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -34,6 +34,7 @@ LosGuidanceNode::LosGuidanceNode() : Node("los_guidance_node") { set_adaptive_los_guidance(config); set_proportional_los_guidance(config); set_integral_los_guidance(config); + set_vector_field_guidance(config); spdlog::info(start_message); } @@ -150,6 +151,23 @@ void LosGuidanceNode::set_integral_los_guidance(YAML::Node config) { integral_los_ = std::make_unique(params); } +void LosGuidanceNode::set_vector_field_guidance(YAML::Node config) { + auto vector_field_config = config["vector_field_los"]; + auto params = VectorFieldLosParams{}; + params.max_approach_angle_h = + vector_field_config["max_approach_angle_h"].as(); + params.max_approach_angle_v = + vector_field_config["max_approach_angle_v"].as(); + params.k_p_h = + vector_field_config["k_p_h"].as(); + params.k_p_v = + vector_field_config["k_p_v"].as(); + params.time_step = + static_cast(time_step_.count()) / 1000.0; + + vector_field_los_ = std::make_unique(params); +} + void LosGuidanceNode::waypoint_callback( const geometry_msgs::msg::PointStamped::SharedPtr los_waypoint) { path_inputs_.prev_point = path_inputs_.current_position; @@ -280,6 +298,9 @@ void LosGuidanceNode::execute( case types::ActiveLosMethod::INTEGRAL: outputs = integral_los_->calculate_outputs(path_inputs_); break; + case types::ActiveLosMethod::VECTOR_FIELD: + outputs = vector_field_los_->calculate_outputs(path_inputs_); + break; default: spdlog::error("Invalid LOS method selected"); result->success = false; diff --git a/guidance/los_guidance/test/CMakeLists.txt b/guidance/los_guidance/test/CMakeLists.txt index 8f5842651..c0cdc1ae7 100644 --- a/guidance/los_guidance/test/CMakeLists.txt +++ b/guidance/los_guidance/test/CMakeLists.txt @@ -10,6 +10,7 @@ add_executable( adaptive_los_test.cpp proportional_los_test.cpp integral_los_test.cpp + vector_field_los_test.cpp ) diff --git a/guidance/los_guidance/test/vector_field_los_test.cpp b/guidance/los_guidance/test/vector_field_los_test.cpp new file mode 100644 index 000000000..527b33694 --- /dev/null +++ b/guidance/los_guidance/test/vector_field_los_test.cpp @@ -0,0 +1,111 @@ +#include "los_guidance/lib/proportional_los.hpp" +#include "los_guidance/lib/vector_field_los.hpp" +#include + +namespace vortex::guidance::los { + +class VectorFieldLosTest : public ::testing::Test { + protected: + VectorFieldLosTest() : Vflos_{get_params()} {} + + VectorFieldLosParams get_params() { + VectorFieldLosParams params; + params.max_approach_angle_h = 30.0 * M_PI / 180.0; // 30 degrees in rad + params.max_approach_angle_v = 20.0 * M_PI / 180.0; // 20 degrees in rad + params.k_p_h = 0.1; // needs tuning + params.k_p_v = 0.1; // needs tuning + params.time_step = 0.01; + return params; + } + + VectorFieldLOSGuidance Vflos_; + const double tol = 1e-9; +}; + +// Test commanded angles when drone is to the right of the track +TEST_F(VectorFieldLosTest, T06_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.5, 0.0}; + + const types::Outputs O = Vflos_.calculate_outputs(inputs); + + // Heading cmd should be between -pi/2 and 0 + EXPECT_LT(O.psi_d, 0.0); + EXPECT_GT(O.psi_d, -1.57); + + // Pitch cmd should be zero + EXPECT_NEAR(O.theta_d, 0.0, tol); +} + +// Test commanded angles when drone is to the left of the track +TEST_F(VectorFieldLosTest, T07_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, -0.5, 0.0}; + + const types::Outputs O = Vflos_.calculate_outputs(inputs); + + // Heading cmd should be between 0 and pi/2 + EXPECT_GT(O.psi_d, 0.0); + EXPECT_LT(O.psi_d, 1.57); + + // Pitch cmd should be zero + EXPECT_NEAR(O.theta_d, 0.0, tol); +} + +// Test commanded angles when drone is under the track +TEST_F(VectorFieldLosTest, T08_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, 0.5}; + + const types::Outputs O = Vflos_.calculate_outputs(inputs); + + // Heading cmd should be 0 + EXPECT_NEAR(O.psi_d, 0.0, tol); + + // Pitch cmd should be between 0 and pi/2 + EXPECT_GT(O.theta_d, 0.0); + EXPECT_LT(O.theta_d, 1.57); +} + +// Test commanded angles when drone is over the track +TEST_F(VectorFieldLosTest, T09_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, -0.5}; + + const types::Outputs O = Vflos_.calculate_outputs(inputs); + + // Heading cmd should be 0 + EXPECT_NEAR(O.psi_d, 0.0, tol); + + // Pitch cmd should be between -pi/2 and 0 + EXPECT_LT(O.theta_d, 0.0); + EXPECT_GT(O.theta_d, -1.57); +} + +// Test commanded angles when drone is over and to the right of the track +TEST_F(VectorFieldLosTest, T10_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.5, -0.5}; + + const types::Outputs O = Vflos_.calculate_outputs(inputs); + + // Heading cmd should be between -pi/2 and 0 + EXPECT_LT(O.psi_d, 0.0); + EXPECT_GT(O.psi_d, -1.57); + + // Pitch cmd should be between -pi/2 and 0 + EXPECT_LT(O.theta_d, 0.0); + EXPECT_GT(O.theta_d, -1.57); +} + +} // namespace vortex::guidance::los From edc7185445a2eb55a346202851613caa192212e3 Mon Sep 17 00:00:00 2001 From: Anbit Date: Thu, 8 Jan 2026 14:07:12 +0100 Subject: [PATCH 045/128] Start quto-euler for graphing --- .../los_guidance/config/guidance_params.yaml | 4 +- .../include/los_guidance/los_guidance_ros.hpp | 8 +++ .../launch/guidance_test.launch.py | 50 +++++++++++++++++++ .../los_guidance/src/lib/adaptive_los.cpp | 2 + .../los_guidance/src/los_guidance_ros.cpp | 11 ++++ 5 files changed, 74 insertions(+), 1 deletion(-) create mode 100644 guidance/los_guidance/launch/guidance_test.launch.py diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 5e56f6908..ccdcd1aad 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -24,9 +24,11 @@ vector_field_los: k_p_h: 0.5 k_p_v: 0.5 - common: active_los_method: 2 # 0: Proportional, 1: Integral, 2: Adaptive u_desired: 0.3 goal_reached_tol: 1.0 +debug: + enable_debug: false + debug_topic_name: /los_guidance_debug \ No newline at end of file diff --git a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp index fdeb06a9d..2a4b528b4 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -17,6 +17,8 @@ #include "los_guidance/lib/proportional_los.hpp" #include "los_guidance/lib/vector_field_los.hpp" #include "los_guidance/lib/types.hpp" +#include +//#include namespace vortex::guidance::los { @@ -99,6 +101,12 @@ class LosGuidanceNode : public rclcpp::Node { rclcpp::Publisher::SharedPtr reference_pub_; + bool enable_debug_; + + std::string debug_topic_name_; + + rclcpp::Publisher::SharedPtr debug_pub_; + rclcpp::Subscription::SharedPtr waypoint_sub_; diff --git a/guidance/los_guidance/launch/guidance_test.launch.py b/guidance/los_guidance/launch/guidance_test.launch.py new file mode 100644 index 000000000..f982f669f --- /dev/null +++ b/guidance/los_guidance/launch/guidance_test.launch.py @@ -0,0 +1,50 @@ +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription, TimerAction +from launch.launch_description_sources import PythonLaunchDescriptionSource +from ament_index_python.packages import get_package_share_directory +import os + +def generate_launch_description(): + stonefish_dir = get_package_share_directory('stonefish_sim') + los_guidance_dir = get_package_share_directory('los_guidance') + velocity_controller_dir = get_package_share_directory('velocity_controller_lqr') + + stonefish_sim = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(stonefish_dir, 'launch', 'simulation.launch.py') + ), + launch_arguments={ + 'scenario': 'tacc', + 'rendering': 'false', + }.items(), + ) + + los_guidance_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(los_guidance_dir, 'launch', 'los_guidance.launch.py') + ) + ) + + velocity_controller_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(velocity_controller_dir, 'launch', 'velocity_controller_lqr.launch.py') + ) + ) + + orca_sim = TimerAction( + period=12.0, + actions=[ + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(stonefish_dir, 'launch', 'orca_sim.launch.py') + ) + ) + ] + ) + + return LaunchDescription([ + stonefish_sim, + los_guidance_launch, + velocity_controller_launch, + orca_sim, + ]) \ No newline at end of file diff --git a/guidance/los_guidance/src/lib/adaptive_los.cpp b/guidance/los_guidance/src/lib/adaptive_los.cpp index 4a34ccb79..f35d9a053 100644 --- a/guidance/los_guidance/src/lib/adaptive_los.cpp +++ b/guidance/los_guidance/src/lib/adaptive_los.cpp @@ -61,4 +61,6 @@ types::Outputs AdaptiveLOSGuidance::calculate_outputs( return types::Outputs{psi_d, theta_d}; } + + } // namespace vortex::guidance::los diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 220009fc2..34cb0b631 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -43,6 +43,8 @@ void LosGuidanceNode::set_subscribers_and_publisher() { this->declare_parameter("topics.pose"); this->declare_parameter("topics.guidance.los"); this->declare_parameter("topics.waypoint"); + this->declare_parameter("debug.debug_topic_name"); + this->declare_parameter("debug.enable_debug"); std::string pose_topic = this->get_parameter("topics.pose").as_string(); std::string guidance_topic = @@ -50,6 +52,9 @@ void LosGuidanceNode::set_subscribers_and_publisher() { std::string waypoint_topic = this->get_parameter("topics.waypoint").as_string(); + debug_topic_name_ = this->get_parameter("debug.debug_topic_name").as_string(); + enable_debug_ = this->get_parameter("debug.enable_debug").as_bool(); + auto qos_sensor_data = vortex::utils::qos_profiles::sensor_data_profile(1); reference_pub_ = this->create_publisher( @@ -65,6 +70,12 @@ void LosGuidanceNode::set_subscribers_and_publisher() { pose_topic, qos_sensor_data, std::bind(&LosGuidanceNode::pose_callback, this, std::placeholders::_1)); + + + if (enable_debug_) { + debug_pub_ = this->create_publisher( + debug_topic_name_, qos_sensor_data); + } } void LosGuidanceNode::set_action_server() { From 763f784042a3732c89309ad5b6ffc1cfd21573b8 Mon Sep 17 00:00:00 2001 From: Anbit Date: Thu, 15 Jan 2026 11:58:55 +0100 Subject: [PATCH 046/128] Fix emailaddress error for git --- guidance/los_guidance/CMakeLists.txt | 2 + .../los_guidance/config/guidance_params.yaml | 6 +- .../include/los_guidance/los_guidance_ros.hpp | 18 +++-- guidance/los_guidance/package.xml | 1 + .../los_guidance/src/los_guidance_ros.cpp | 68 ++++++++++++++++--- 5 files changed, 78 insertions(+), 17 deletions(-) diff --git a/guidance/los_guidance/CMakeLists.txt b/guidance/los_guidance/CMakeLists.txt index 9d8fca42b..1dbdbe0d9 100644 --- a/guidance/los_guidance/CMakeLists.txt +++ b/guidance/los_guidance/CMakeLists.txt @@ -15,6 +15,7 @@ find_package(rclcpp_action REQUIRED) find_package(vortex_msgs REQUIRED) find_package(vortex_utils_ros REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) find_package(Eigen3 REQUIRED) find_package(spdlog REQUIRED) find_package(fmt REQUIRED) @@ -36,6 +37,7 @@ ament_target_dependencies(${LIB_NAME} rclcpp rclcpp_action geometry_msgs + nav_msgs vortex_msgs vortex_utils_ros Eigen3 diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index ccdcd1aad..5d5d0b754 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -25,10 +25,10 @@ vector_field_los: k_p_v: 0.5 common: - active_los_method: 2 # 0: Proportional, 1: Integral, 2: Adaptive + active_los_method: 0 # 0: Proportional, 1: Integral, 2: Adaptive u_desired: 0.3 goal_reached_tol: 1.0 debug: - enable_debug: false - debug_topic_name: /los_guidance_debug \ No newline at end of file + enable_debug: true + debug_topic_name: "/los_guidance_debug" \ No newline at end of file diff --git a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp index 2a4b528b4..5f79d75b5 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -5,6 +5,7 @@ #include #include #include +#include #include #include #include @@ -18,7 +19,8 @@ #include "los_guidance/lib/vector_field_los.hpp" #include "los_guidance/lib/types.hpp" #include -//#include +#include +#include namespace vortex::guidance::los { @@ -28,7 +30,7 @@ class LosGuidanceNode : public rclcpp::Node { private: // @brief Set the subscribers and publishers - void set_subscribers_and_publisher(); + void set_subscribers_and_publisher(YAML::Node config); // @brief Set the action server void set_action_server(); @@ -58,6 +60,9 @@ class LosGuidanceNode : public rclcpp::Node { void pose_callback( const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); + void odom_callback( + const nav_msgs::msg::Odometry::SharedPtr msg); + // @brief Handle the goal request // @param uuid The goal UUID // @param goal The goal message @@ -88,6 +93,9 @@ class LosGuidanceNode : public rclcpp::Node { const std::shared_ptr request, std::shared_ptr response); + void publish_state_debug(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr + current_pose); + vortex_msgs::msg::LOSGuidance fill_los_reference(types::Outputs output); YAML::Node get_los_config(std::string yaml_file_path); @@ -105,11 +113,13 @@ class LosGuidanceNode : public rclcpp::Node { std::string debug_topic_name_; - rclcpp::Publisher::SharedPtr debug_pub_; + rclcpp::Publisher::SharedPtr debug_pub_; rclcpp::Subscription::SharedPtr waypoint_sub_; + rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Subscription< geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_; @@ -141,5 +151,5 @@ class LosGuidanceNode : public rclcpp::Node { }; } // namespace vortex::guidance::los - + #endif // LOS_GUIDANCE__LOS_GUIDANCE_ROS_HPP_ diff --git a/guidance/los_guidance/package.xml b/guidance/los_guidance/package.xml index 5ff6218fe..8fef0da18 100644 --- a/guidance/los_guidance/package.xml +++ b/guidance/los_guidance/package.xml @@ -13,6 +13,7 @@ rclcpp_action geometry_msgs vortex_msgs + nav_msgs vortex_utils_ros eigen diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 34cb0b631..320eeea7d 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -1,7 +1,7 @@ #include "los_guidance/los_guidance_ros.hpp" +#include #include #include -#include #include #include "los_guidance/lib/types.hpp" @@ -10,7 +10,7 @@ const auto start_message = R"( | | / _ \/ ___| / ___|_ _(_) __| | __ _ _ __ ___ ___ | | | | | \___ \ | | _| | | | |/ _` |/ _` | '_ \ / __/ _ \ | |__| |_| |___) | | |_| | |_| | | (_| | (_| | | | | (_| __/ - |_____\___/|____/ \____|\__,_|_|\__,_|\__,_|_| |_|\___\___| + |_____\___/|____/ \____|\__,_|_|\__,_|\__,_|_| |_|\___\___| )"; @@ -28,7 +28,7 @@ LosGuidanceNode::LosGuidanceNode() : Node("los_guidance_node") { YAML::Node config = get_los_config(yaml_path); parse_common_config(config["common"]); - set_subscribers_and_publisher(); + set_subscribers_and_publisher(config); set_action_server(); set_service_server(); set_adaptive_los_guidance(config); @@ -39,21 +39,20 @@ LosGuidanceNode::LosGuidanceNode() : Node("los_guidance_node") { spdlog::info(start_message); } -void LosGuidanceNode::set_subscribers_and_publisher() { +void LosGuidanceNode::set_subscribers_and_publisher(YAML::Node config) { this->declare_parameter("topics.pose"); this->declare_parameter("topics.guidance.los"); this->declare_parameter("topics.waypoint"); - this->declare_parameter("debug.debug_topic_name"); - this->declare_parameter("debug.enable_debug"); + this->declare_parameter("nav_topics.odometry"); std::string pose_topic = this->get_parameter("topics.pose").as_string(); std::string guidance_topic = this->get_parameter("topics.guidance.los").as_string(); std::string waypoint_topic = this->get_parameter("topics.waypoint").as_string(); - - debug_topic_name_ = this->get_parameter("debug.debug_topic_name").as_string(); - enable_debug_ = this->get_parameter("debug.enable_debug").as_bool(); + + std::string odom_topic = + this->get_parameter("nav_topics.odometry").as_string(); auto qos_sensor_data = vortex::utils::qos_profiles::sensor_data_profile(1); @@ -71,9 +70,17 @@ void LosGuidanceNode::set_subscribers_and_publisher() { std::bind(&LosGuidanceNode::pose_callback, this, std::placeholders::_1)); + odom_sub_ = this->create_subscription( + odom_topic, qos_sensor_data, + std::bind(&LosGuidanceNode::odom_callback, this, + std::placeholders::_1)); + + auto debug_config = config["debug"]; + enable_debug_ = debug_config["enable_debug"].as(); + debug_topic_name_ = debug_config["debug_topic_name"].as(); if (enable_debug_) { - debug_pub_ = this->create_publisher( + debug_pub_ = this->create_publisher( debug_topic_name_, qos_sensor_data); } } @@ -192,6 +199,8 @@ void LosGuidanceNode::pose_callback( current_pose) { path_inputs_.current_position = types::Point::point_from_ros(current_pose->pose.pose.position); + + publish_state_debug(current_pose); } rclcpp_action::GoalResponse LosGuidanceNode::handle_goal( @@ -235,6 +244,7 @@ void LosGuidanceNode::set_los_mode( } vortex_msgs::msg::LOSGuidance LosGuidanceNode::fill_los_reference( types::Outputs outputs) { + vortex_msgs::msg::LOSGuidance reference_msg; reference_msg.pitch = outputs.theta_d; reference_msg.yaw = outputs.psi_d; @@ -243,6 +253,39 @@ vortex_msgs::msg::LOSGuidance LosGuidanceNode::fill_los_reference( return reference_msg; } +void LosGuidanceNode::publish_state_debug(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr + current_pose) { + if (!enable_debug_) { + return; + } + + const auto &q_msg = current_pose->pose.pose.orientation; + + Eigen::Quaterniond quat( + q_msg.w, + q_msg.x, + q_msg.y, + q_msg.z + ); + + + Eigen::Vector3d euler = vortex::utils::math::quat_to_euler(quat); + + vortex_msgs::msg::PoseEulerStamped debug_msg; + debug_msg.header.stamp = this->now(); + debug_msg.header.frame_id = "los_guidance_debug"; + + debug_msg.x = current_pose->pose.pose.position.x; + debug_msg.y = current_pose->pose.pose.position.y; + debug_msg.z = current_pose->pose.pose.position.z; + debug_msg.roll = euler.x(); + debug_msg.pitch = euler.y(); + debug_msg.yaw = euler.z(); + + debug_pub_->publish(debug_msg); + +} + YAML::Node LosGuidanceNode::get_los_config(std::string yaml_file_path) { YAML::Node config = YAML::LoadFile(yaml_file_path); return config; @@ -256,6 +299,11 @@ void LosGuidanceNode::parse_common_config(YAML::Node common_config) { common_config["active_los_method"].as()); } +void LosGuidanceNode::odom_callback( + const nav_msgs::msg::Odometry::SharedPtr msg) { + +} + void LosGuidanceNode::execute( const std::shared_ptr< From f073936fe3b91531c8649b435d43b46a7b0aa7f2 Mon Sep 17 00:00:00 2001 From: Anbit Date: Fri, 16 Jan 2026 14:35:57 +0100 Subject: [PATCH 047/128] Fix building error --- guidance/los_guidance/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/guidance/los_guidance/package.xml b/guidance/los_guidance/package.xml index 8fef0da18..5f696a725 100644 --- a/guidance/los_guidance/package.xml +++ b/guidance/los_guidance/package.xml @@ -16,7 +16,7 @@ nav_msgs vortex_utils_ros eigen - + yaml-cpp ament_cmake From 70d0bb33c2dca0c2c9839fc2c82a3fab49924f29 Mon Sep 17 00:00:00 2001 From: Anbit Date: Fri, 23 Jan 2026 16:47:20 +0100 Subject: [PATCH 048/128] Clean the code --- auv_setup/config/robots/orca.yaml | 1 + .../los_guidance/config/guidance_params.yaml | 6 +- .../include/los_guidance/lib/integral_los.hpp | 2 - .../los_guidance/lib/proportional_los.hpp | 6 +- .../include/los_guidance/los_guidance_ros.hpp | 19 ++-- .../launch/guidance_test.launch.py | 21 +++- .../los_guidance/src/lib/adaptive_los.cpp | 2 +- .../los_guidance/src/lib/integral_los.cpp | 2 +- .../los_guidance/src/lib/proportional_los.cpp | 2 +- .../los_guidance/src/los_guidance_ros.cpp | 104 +++++++----------- 10 files changed, 75 insertions(+), 90 deletions(-) diff --git a/auv_setup/config/robots/orca.yaml b/auv_setup/config/robots/orca.yaml index 1fef5f08b..879dcdca5 100644 --- a/auv_setup/config/robots/orca.yaml +++ b/auv_setup/config/robots/orca.yaml @@ -121,6 +121,7 @@ joy: "joy" pose: "pose" twist: "twist" + odom: "odom" operation_mode: "operation_mode" killswitch: "killswitch" aruco_board_pose_camera: "aruco_board_pose_camera" diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 5d5d0b754..496980fa4 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -7,12 +7,8 @@ adaptive_los: prop_los: lookahead_distance_h: 1.5 lookahead_distance_v: 0.6 - k_p_h: 0.5 - k_p_v: 0.5 integer_los: - lookahead_distance_h: 1.5 - lookahead_distance_v: 0.6 k_p_h: 0.5 k_p_v: 0.5 k_i_h: 0.1 @@ -25,7 +21,7 @@ vector_field_los: k_p_v: 0.5 common: - active_los_method: 0 # 0: Proportional, 1: Integral, 2: Adaptive + active_los_method: 3 # 0: Proportional, 1: Integral, 2: Adaptive, 3: VFLos u_desired: 0.3 goal_reached_tol: 1.0 diff --git a/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp b/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp index 4af1b60ac..0509aaa28 100644 --- a/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp @@ -9,8 +9,6 @@ namespace vortex::guidance::los { struct IntegralLosParams { - double lookahead_distance_h{}; - double lookahead_distance_v{}; double k_p_h{}; double k_p_v{}; double k_i_h{}; diff --git a/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp b/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp index 2a5ea00a1..1dd96654c 100644 --- a/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp @@ -10,12 +10,9 @@ namespace vortex::guidance::los { struct ProportionalLosParams { double lookahead_distance_h{}; double lookahead_distance_v{}; - double k_p_h{}; - double k_p_v{}; - double time_step{}; }; -class ProportionalLOSGuidance { +class ProportionalLOSGuidance { public: ProportionalLOSGuidance(const ProportionalLosParams& params); ~ProportionalLOSGuidance() = default; @@ -28,7 +25,6 @@ class ProportionalLOSGuidance { const types::Inputs& inputs) const; ProportionalLosParams m_params{}; - // again i dont know if i should have them here or just in the functions double pi_h_{0.0}; double pi_v_{0.0}; Eigen::AngleAxisd rotation_y_{0.0, Eigen::Vector3d::UnitY()}; diff --git a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp index 5f79d75b5..202e38573 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -1,6 +1,7 @@ #ifndef LOS_GUIDANCE__LOS_GUIDANCE_ROS_HPP_ #define LOS_GUIDANCE__LOS_GUIDANCE_ROS_HPP_ +#include #include #include #include @@ -20,7 +21,7 @@ #include "los_guidance/lib/types.hpp" #include #include -#include + namespace vortex::guidance::los { @@ -30,7 +31,7 @@ class LosGuidanceNode : public rclcpp::Node { private: // @brief Set the subscribers and publishers - void set_subscribers_and_publisher(YAML::Node config); + void set_subscribers_and_publisher(); // @brief Set the action server void set_action_server(); @@ -109,20 +110,20 @@ class LosGuidanceNode : public rclcpp::Node { rclcpp::Publisher::SharedPtr reference_pub_; - bool enable_debug_; - - std::string debug_topic_name_; + rclcpp::Publisher::SharedPtr los_debug_pub_; - rclcpp::Publisher::SharedPtr debug_pub_; + rclcpp::Publisher::SharedPtr state_debug_pub_; rclcpp::Subscription::SharedPtr waypoint_sub_; - rclcpp::Subscription::SharedPtr odom_sub_; - rclcpp::Subscription< geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_; + rclcpp::Subscription< + nav_msgs::msg::Odometry>::SharedPtr odom_sub_; + + rclcpp::TimerBase::SharedPtr reference_pub_timer_; std::chrono::milliseconds time_step_; @@ -148,6 +149,8 @@ class LosGuidanceNode : public rclcpp::Node { std::unique_ptr proportional_los_{}; std::unique_ptr vector_field_los_{}; types::ActiveLosMethod method_{}; + + nav_msgs::msg::Odometry::SharedPtr debug_current_odom_{}; }; } // namespace vortex::guidance::los diff --git a/guidance/los_guidance/launch/guidance_test.launch.py b/guidance/los_guidance/launch/guidance_test.launch.py index f982f669f..56f3f5b47 100644 --- a/guidance/los_guidance/launch/guidance_test.launch.py +++ b/guidance/los_guidance/launch/guidance_test.launch.py @@ -1,5 +1,5 @@ from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription, TimerAction +from launch.actions import IncludeLaunchDescription, TimerAction, ExecuteProcess, SetEnvironmentVariable from launch.launch_description_sources import PythonLaunchDescriptionSource from ament_index_python.packages import get_package_share_directory import os @@ -42,9 +42,28 @@ def generate_launch_description(): ] ) + set_autonomy = TimerAction( + period=12.0, + actions=[ + ExecuteProcess( + cmd=[ + "bash", "-lc", + "for i in {1..5}; do " + " ros2 topic pub --once /orca/killswitch std_msgs/msg/Bool \"{data: false}\"; " + " ros2 topic pub --once /orca/operation_mode std_msgs/msg/String \"{data: 'autonomous mode'}\"; " + " sleep 1; " + "done" + ], + output="screen", + ), + ], + ) + + return LaunchDescription([ stonefish_sim, los_guidance_launch, velocity_controller_launch, orca_sim, + set_autonomy, ]) \ No newline at end of file diff --git a/guidance/los_guidance/src/lib/adaptive_los.cpp b/guidance/los_guidance/src/lib/adaptive_los.cpp index f35d9a053..7a1e0d265 100644 --- a/guidance/los_guidance/src/lib/adaptive_los.cpp +++ b/guidance/los_guidance/src/lib/adaptive_los.cpp @@ -4,7 +4,7 @@ namespace vortex::guidance::los { AdaptiveLOSGuidance::AdaptiveLOSGuidance(const AdaptiveLosParams& params) - : params_{params} {} + : params_{params} {} void AdaptiveLOSGuidance::update_angles(const types::Inputs& inputs) { const double dx = inputs.next_point.x - inputs.prev_point.x; diff --git a/guidance/los_guidance/src/lib/integral_los.cpp b/guidance/los_guidance/src/lib/integral_los.cpp index b1a4e3962..c0450a62f 100644 --- a/guidance/los_guidance/src/lib/integral_los.cpp +++ b/guidance/los_guidance/src/lib/integral_los.cpp @@ -14,7 +14,7 @@ void IntegralLOSGuidance::update_angles(const types::Inputs& inputs) { rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); -} +} types::CrossTrackError IntegralLOSGuidance::calculate_crosstrack_error( const types::Inputs& inputs) { diff --git a/guidance/los_guidance/src/lib/proportional_los.cpp b/guidance/los_guidance/src/lib/proportional_los.cpp index 0bb65ba01..522de51e9 100644 --- a/guidance/los_guidance/src/lib/proportional_los.cpp +++ b/guidance/los_guidance/src/lib/proportional_los.cpp @@ -49,6 +49,6 @@ types::Outputs ProportionalLOSGuidance::calculate_outputs( const double theta_d = pi_v_ + std::atan(k_p_v * cross_track_error.z_e); return types::Outputs{psi_d, theta_d}; -} +} } // namespace vortex::guidance::los diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 320eeea7d..0aa21dcb0 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -21,14 +21,14 @@ LosGuidanceNode::LosGuidanceNode() : Node("los_guidance_node") { time_step_ = std::chrono::milliseconds(static_cast(time_step_s * 1000)); // auto config = this->declare_parameter("los_config_file"); - + // Do you need yaml path here? Can't you just use ros params directly from the guidance_params.yaml file? const std::string yaml_path = this->declare_parameter("los_config_file"); YAML::Node config = get_los_config(yaml_path); parse_common_config(config["common"]); - set_subscribers_and_publisher(config); + set_subscribers_and_publisher(); set_action_server(); set_service_server(); set_adaptive_los_guidance(config); @@ -39,11 +39,11 @@ LosGuidanceNode::LosGuidanceNode() : Node("los_guidance_node") { spdlog::info(start_message); } -void LosGuidanceNode::set_subscribers_and_publisher(YAML::Node config) { +void LosGuidanceNode::set_subscribers_and_publisher() { this->declare_parameter("topics.pose"); this->declare_parameter("topics.guidance.los"); this->declare_parameter("topics.waypoint"); - this->declare_parameter("nav_topics.odometry"); + this->declare_parameter("topics.odom"); std::string pose_topic = this->get_parameter("topics.pose").as_string(); std::string guidance_topic = @@ -52,13 +52,19 @@ void LosGuidanceNode::set_subscribers_and_publisher(YAML::Node config) { this->get_parameter("topics.waypoint").as_string(); std::string odom_topic = - this->get_parameter("nav_topics.odometry").as_string(); + this->get_parameter("topics.odom").as_string(); auto qos_sensor_data = vortex::utils::qos_profiles::sensor_data_profile(1); reference_pub_ = this->create_publisher( guidance_topic, qos_sensor_data); + los_debug_pub_ = this->create_publisher( + "los_debug", qos_sensor_data); + + state_debug_pub_ = this->create_publisher( + "state_debug", qos_sensor_data); + waypoint_sub_ = this->create_subscription( waypoint_topic, qos_sensor_data, std::bind(&LosGuidanceNode::waypoint_callback, this, @@ -73,16 +79,7 @@ void LosGuidanceNode::set_subscribers_and_publisher(YAML::Node config) { odom_sub_ = this->create_subscription( odom_topic, qos_sensor_data, std::bind(&LosGuidanceNode::odom_callback, this, - std::placeholders::_1)); - - auto debug_config = config["debug"]; - enable_debug_ = debug_config["enable_debug"].as(); - debug_topic_name_ = debug_config["debug_topic_name"].as(); - - if (enable_debug_) { - debug_pub_ = this->create_publisher( - debug_topic_name_, qos_sensor_data); - } + std::placeholders::_1)); } void LosGuidanceNode::set_action_server() { @@ -136,15 +133,8 @@ void LosGuidanceNode::set_proportional_los_guidance(YAML::Node config) { auto params = ProportionalLosParams{}; params.lookahead_distance_h = proportional_los_config["lookahead_distance_h"].as(); - params.lookahead_distance_v = proportional_los_config["lookahead_distance_v"].as(); - params.k_p_h = - proportional_los_config["k_p_h"].as(); - params.k_p_v = - proportional_los_config["k_p_v"].as(); - params.time_step = - static_cast(time_step_.count()) / 1000.0; proportional_los_ = std::make_unique(params); } @@ -152,10 +142,6 @@ void LosGuidanceNode::set_proportional_los_guidance(YAML::Node config) { void LosGuidanceNode::set_integral_los_guidance(YAML::Node config) { auto integral_los_config = config["integer_los"]; auto params = IntegralLosParams{}; - params.lookahead_distance_h = - integral_los_config["lookahead_distance_h"].as(); - params.lookahead_distance_v = - integral_los_config["lookahead_distance_v"].as(); params.k_p_h = integral_los_config["k_p_h"].as(); params.k_p_v = @@ -199,10 +185,14 @@ void LosGuidanceNode::pose_callback( current_pose) { path_inputs_.current_position = types::Point::point_from_ros(current_pose->pose.pose.position); +} - publish_state_debug(current_pose); +void LosGuidanceNode::odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg) { + std::lock_guard lock(mutex_); + debug_current_odom_ = msg; } + rclcpp_action::GoalResponse LosGuidanceNode::handle_goal( const rclcpp_action::GoalUUID&, std::shared_ptr goal) { @@ -253,39 +243,6 @@ vortex_msgs::msg::LOSGuidance LosGuidanceNode::fill_los_reference( return reference_msg; } -void LosGuidanceNode::publish_state_debug(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr - current_pose) { - if (!enable_debug_) { - return; - } - - const auto &q_msg = current_pose->pose.pose.orientation; - - Eigen::Quaterniond quat( - q_msg.w, - q_msg.x, - q_msg.y, - q_msg.z - ); - - - Eigen::Vector3d euler = vortex::utils::math::quat_to_euler(quat); - - vortex_msgs::msg::PoseEulerStamped debug_msg; - debug_msg.header.stamp = this->now(); - debug_msg.header.frame_id = "los_guidance_debug"; - - debug_msg.x = current_pose->pose.pose.position.x; - debug_msg.y = current_pose->pose.pose.position.y; - debug_msg.z = current_pose->pose.pose.position.z; - debug_msg.roll = euler.x(); - debug_msg.pitch = euler.y(); - debug_msg.yaw = euler.z(); - - debug_pub_->publish(debug_msg); - -} - YAML::Node LosGuidanceNode::get_los_config(std::string yaml_file_path) { YAML::Node config = YAML::LoadFile(yaml_file_path); return config; @@ -299,16 +256,11 @@ void LosGuidanceNode::parse_common_config(YAML::Node common_config) { common_config["active_los_method"].as()); } -void LosGuidanceNode::odom_callback( - const nav_msgs::msg::Odometry::SharedPtr msg) { - -} - void LosGuidanceNode::execute( - const std::shared_ptr< rclcpp_action::ServerGoalHandle> goal_handle) { + { std::lock_guard lock(mutex_); this->goal_handle_ = goal_handle; @@ -371,9 +323,29 @@ void LosGuidanceNode::execute( fill_los_reference(outputs); feedback->feedback = reference_msg; + los_debug_pub_->publish(reference_msg); + + double surge = std::sqrt(debug_current_odom_->twist.twist.linear.x + + debug_current_odom_->twist.twist.linear.y + + debug_current_odom_->twist.twist.linear.z); + + vortex_msgs::msg::LOSGuidance state_debug_msg; + Eigen::Vector3d euler = vortex::utils::math::quat_to_euler( + Eigen::Quaterniond( + debug_current_odom_->pose.pose.orientation.w, + debug_current_odom_->pose.pose.orientation.x, + debug_current_odom_->pose.pose.orientation.y, + debug_current_odom_->pose.pose.orientation.z)); + + state_debug_msg.pitch = euler.y(); + state_debug_msg.yaw = euler.z(); + state_debug_msg.surge = surge; + state_debug_pub_->publish(state_debug_msg); + goal_handle->publish_feedback(feedback); reference_pub_->publish(reference_msg); + if ((path_inputs_.current_position - path_inputs_.next_point) .as_vector() .norm() < goal_reached_tol_) { From 65a37c2fa473b399ef19b23acb82508382b12157 Mon Sep 17 00:00:00 2001 From: Henrik Date: Sat, 24 Jan 2026 11:06:04 +0100 Subject: [PATCH 049/128] reworked the LQR --- control/velocity_controller/CMakeLists.txt | 24 ++- .../config/parameters.yaml | 34 ++-- .../include/velocity_controller/LQR_setup.hpp | 44 ++--- .../include/velocity_controller/test_VC.hpp | 4 +- .../include/velocity_controller/utilities.hpp | 10 +- .../velocity_controller.hpp | 10 +- control/velocity_controller/package.xml | 7 + control/velocity_controller/src/LQR_setup.cpp | 183 ++++++++++++------ control/velocity_controller/src/LQR_test.cpp | 29 ++- .../src/ct_instantiations.cpp | 16 ++ control/velocity_controller/src/test_VC.cpp | 4 +- control/velocity_controller/src/utilities.cpp | 42 +++- .../src/velocity_controller.cpp | 100 +++++----- 13 files changed, 328 insertions(+), 179 deletions(-) create mode 100644 control/velocity_controller/src/ct_instantiations.cpp diff --git a/control/velocity_controller/CMakeLists.txt b/control/velocity_controller/CMakeLists.txt index a6eaa4f11..47799ece9 100644 --- a/control/velocity_controller/CMakeLists.txt +++ b/control/velocity_controller/CMakeLists.txt @@ -20,6 +20,9 @@ find_package(LAPACK REQUIRED) find_package(BLAS REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) +find_package(ct_optcon REQUIRED) +find_package(ct_core REQUIRED) + #find_package(stonefish_ros2 REQUIRED) #find_package(auv_setup REQUIRED) #find_package(stonefish_sim REQUIRED) @@ -32,6 +35,7 @@ find_package(nav_msgs REQUIRED) #) include_directories( ${EIGEN3_INCLUDE_DIR} + include ) @@ -40,19 +44,23 @@ add_executable(velocity_controller_node src/PID_setup.cpp src/LQR_setup.cpp src/utilities.cpp + src/ct_instantiations.cpp ) add_executable(test_VC_node src/test_VC.cpp - src/PID_setup.cpp - src/LQR_setup.cpp + #src/PID_setup.cpp + #src/LQR_setup.cpp src/utilities.cpp + src/ct_instantiations.cpp + ) add_executable(test_LQR_node src/LQR_test.cpp src/LQR_setup.cpp src/utilities.cpp -) + src/ct_instantiations.cpp + ) ament_target_dependencies(velocity_controller_node rclcpp std_msgs @@ -62,7 +70,6 @@ ament_target_dependencies(velocity_controller_node CasADi nav_msgs vortex_utils - ) ament_target_dependencies(test_VC_node @@ -85,13 +92,14 @@ ament_target_dependencies(test_LQR_node vortex_utils ) + link_directories(/usr/lib/gcc/x86_64-linux-gnu/11) set(SLICOT_LIB /usr/lib/x86_64-linux-gnu/libslicot.so) set(GFORTRAN_LIB /usr/lib/gcc/x86_64-linux-gnu/11/libgfortran.so) - -target_link_libraries(velocity_controller_node ${LAPACK_LIBRARIES} ${BLAS_LIBRARIES} ${SLICOT_LIB} ${GFORTRAN_LIB} Eigen3::Eigen) -target_link_libraries(test_VC_node ${LAPACK_LIBRARIES} ${BLAS_LIBRARIES} ${SLICOT_LIB} ${GFORTRAN_LIB} Eigen3::Eigen) -target_link_libraries(test_LQR_node ${LAPACK_LIBRARIES} ${BLAS_LIBRARIES} ${SLICOT_LIB} ${GFORTRAN_LIB} Eigen3::Eigen) +#${ct_optcon_LIBRARIES} +target_link_libraries(velocity_controller_node Eigen3::Eigen ct_optcon ct_core ${LAPACK_LIBRARIES} ${BLAS_LIBRARIES} ${SLICOT_LIB} ${GFORTRAN_LIB}) +target_link_libraries(test_VC_node Eigen3::Eigen ct_optcon ct_core ${LAPACK_LIBRARIES} ${BLAS_LIBRARIES} ${SLICOT_LIB} ${GFORTRAN_LIB}) +target_link_libraries(test_LQR_node Eigen3::Eigen ct_optcon ct_core ${LAPACK_LIBRARIES} ${BLAS_LIBRARIES} ${SLICOT_LIB} ${GFORTRAN_LIB} ) install(TARGETS diff --git a/control/velocity_controller/config/parameters.yaml b/control/velocity_controller/config/parameters.yaml index 8aea0ed68..e78a75601 100644 --- a/control/velocity_controller/config/parameters.yaml +++ b/control/velocity_controller/config/parameters.yaml @@ -11,31 +11,33 @@ killswitch_topic: /softwareKillSwitch #Kill Switch LQR_params: - q_surge: 75.0 - q_pitch: 175.0 - q_yaw: 175.0 + Q: [400.0,32.84,32.84,0.33,0.33,400.0,32.84,32.84] + R: [0.0005,3.1,3.10] #0.1,0.1,0.1] + #q_surge: 75.0 + #q_pitch: 175.0 + #q_yaw: 175.0 - r_surge: 0.3 - r_pitch: 0.4 - r_yaw: 0.4 + #r_surge: 0.3 + #r_pitch: 0.4 + #r_yaw: 0.4 - i_surge: 0.3 - i_pitch: 0.4 - i_yaw: 0.3 + #i_surge: 0.3 + #i_pitch: 0.4 + #i_yaw: 0.3 - i_weight: 0.5 + #i_weight: 0.5 - dt: 0.1 + #dt: 0.1 inertia_matrix: [ 30.0, 0.0, 0.0, 0.0, 0.0, 0.6, 0.0, 30.0, 0.0, 0.0, -0.6, 0.3, 0.0, 0.0, 30.0, 0.6, 0.3, 0.0, 0.0, 0.0, 0.6, 0.68, 0.0, 0.0, 0.0, -0.6, 0.3, 0.0, 3.32, 0.0, 0.6, 0.3, 0.0, 0.0, 0.0, 3.34] - dampening_matrix_low: [1.0,0.0,0.0,0.0,0.0,0.0, 0.0,1.0,0.0,0.0,0.0,0.0, 0.0,0.0,1.0,0.0,0.0,0.0, 0.0,0.0,0.0,1.0,0.0,0.0, 0.0,0.0,0.0,0.0,1.0,0.0, 0.0,0.0,0.0,0.0,0.0,1.0] + dampening_matrix_low: [23.0,0.0,0.0,0.0,0.0,0.0, 0.0,46.0,0.0,0.0,0.0,0.0, 0.0,0.0,46.0,0.0,0.0,0.0, 0.0,0.0,0.0,46.0,0.0,0.0, 0.0,0.0,0.0,0.0,46.0,0.0, 0.0,0.0,0.0,0.0,0.0,46.0] dampening_matrix_high: [1.0,0.0,0.0,0.0,0.0,0.0, 0.0,1.0,0.0,0.0,0.0,0.0, 0.0,0.0,1.0,0.0,0.0,0.0, 0.0,0.0,0.0,1.0,0.0,0.0, 0.0,0.0,0.0,0.0,1.0,0.0, 0.0,0.0,0.0,0.0,0.0,1.0] - calculation_rate: 200 #ms integer - publish_rate: 100 #ms + #calculation_rate: 200 #ms integer + publish_rate: 10 #ms #Clamp parameter - max_force: 1000.0 #should maybe be 99.5 - controller_type: 1 #1 PID 2 LQR + max_force: 99.5 #should maybe be 99.5 + controller_type: 2 #1 PID 2 LQR diff --git a/control/velocity_controller/include/velocity_controller/LQR_setup.hpp b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp index cfb4a4395..9fa07b97c 100644 --- a/control/velocity_controller/include/velocity_controller/LQR_setup.hpp +++ b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp @@ -32,49 +32,49 @@ class LQRparameters{ double psit=0.0; };*/ - +/* struct LQRsolveResult{ - Eigen::Matrix K; - Eigen::Matrix P; + Eigen::MatrixXd K; + Eigen::MatrixXd P; int INFO=0; - LQRsolveResult(Eigen::Matrix K,Eigen::Matrix P, int INFO=0):K(K),P(P),INFO(INFO) {}; -}; + LQRsolveResult(Eigen::MatrixXd K=Eigen::MatrixXd::Zero(),Eigen::MatrixXd P=Eigen::MatrixXd::Zero(), int INFO=0):K(K),P(P),INFO(INFO) {}; +};*/ class LQRController{ public: - LQRController(LQRparameters params={0,0,0,0,0,0,0,0,0,0,0},Eigen::Matrix3d inertia_matrix=Eigen::Matrix3d::Identity()); - + LQRController(); + int set_matrices(std::vector Q_,std::vector R_,std::vector inertia_matrix, double max_force,std::vector water_r_low,std::vector water_r_high); + void reset_controller(); + Eigen::Vector calculate_thrust(State states, Guidance_data guidance_values); + int set_interval(double interval); - void set_params(LQRparameters params); - Eigen::Matrix3d calculate_coriolis_matrix(double pitchrate, double yaw_rate, double sway_vel, double heave_vel); - void set_matrices(Eigen::Matrix3d inertia_matrix); - void update_augmented_matrices(Eigen::Matrix3d coriolis_matrix); + private: + //void set_params(LQRparameters params); + //Eigen::Matrix3d calculate_coriolis_matrix(double pitchrate, double yaw_rate, double sway_vel, double heave_vel); + Eigen::Matrix linearize(State states); //angle quaternion_to_euler_angle(double w, double x, double y, double z); //double ssa(double angle); std::tuple saturate (double value, bool windup, double limit); - double anti_windup(double ki, double error, double integral_sum, bool windup); + double anti_windup(double error, double integral_sum, bool windup); Eigen::Vector saturate_input(Eigen::Vector u); - Eigen::Vector update_error(Guidance_data guidance_values, State states); - Eigen::Vector calculate_lqr_u(State states, Guidance_data guidance_values); + Eigen::Vector update_error(Guidance_data guidance_values, State states); - LQRsolveResult solve_k_p(const Eigen::Matrix &A,const Eigen::Matrix &B,const Eigen::Matrix &Q, const Eigen::Matrix &R); + //LQRsolveResult solve_lqr(const Eigen::MatrixXd &A,const Eigen::MatrixXd &B,const Eigen::MatrixXd &Q, const Eigen::MatrixXd &R); //Resets controller - void reset_controller(); // VariablesEigen::Matrix3d vector_to_matrix3d(const std::vector &other_matrix) - const double pi=3.14159265358979323846; + double interval_; double integral_error_surge; double integral_error_pitch; double integral_error_yaw; bool surge_windup; bool pitch_windup; bool yaw_windup; - double q_surge; double q_pitch; double q_yaw; - double r_surge; double r_pitch; double r_yaw; - double i_surge; double i_pitch; double i_yaw; - double i_weight; double max_force; + Eigen::Matrix Q; Eigen::Matrix R; Eigen::Matrix B; + Eigen::Matrix D_low; Eigen::Matrix D_high; + double max_force; double mass; - Eigen::Matrix3d inertia_matrix_inv; + Eigen::Matrix inertia_matrix_inv; Eigen::Matrix state_weight_matrix; Eigen::Matrix3d input_weight_matrix; Eigen::Matrix augmented_system_matrix; diff --git a/control/velocity_controller/include/velocity_controller/test_VC.hpp b/control/velocity_controller/include/velocity_controller/test_VC.hpp index 6d64e671d..55ff86549 100644 --- a/control/velocity_controller/include/velocity_controller/test_VC.hpp +++ b/control/velocity_controller/include/velocity_controller/test_VC.hpp @@ -22,7 +22,7 @@ class test_VC : public rclcpp::Node{ //Variables - //Subscribers and publishers + //Subscribers and publishers rclcpp::Publisher::SharedPtr publisher_guidance; rclcpp::Publisher::SharedPtr publisher_state; rclcpp::Subscription::SharedPtr subscription_state; @@ -37,7 +37,7 @@ class test_VC : public rclcpp::Node{ //std::string topic_odom; //std::string topic_thrust; std::string topic_guidance; - std::string topic_state="/state"; + std::string topic_state="state"; std::string topic_odometry; diff --git a/control/velocity_controller/include/velocity_controller/utilities.hpp b/control/velocity_controller/include/velocity_controller/utilities.hpp index c6627fa17..1c6e85574 100644 --- a/control/velocity_controller/include/velocity_controller/utilities.hpp +++ b/control/velocity_controller/include/velocity_controller/utilities.hpp @@ -3,6 +3,7 @@ #include #include "std_msgs/msg/float64_multi_array.hpp" #include "vortex_msgs/msg/los_guidance.hpp" +#include class angle{ @@ -16,8 +17,9 @@ angle quaternion_to_euler_angle(double w, double x, double y, double z); class State{ //Dataclass to store state values for LQR controller public: - double surge=0.0, pitch=0.0, yaw=0.0, pitch_rate=0.0, yaw_rate=0.0, heave_vel=0, sway_vel=0; - double integral_surge=0.0; double integral_pitch=0.0; double integral_yaw=0.0; + double surge=0.0, sway=0.0, heave=0.0, roll_rate=0.0, pitch_rate=0.0, yaw_rate=0.0; //roll_rate=0.0, pitch_rate=0.0, yaw_rate=0.0; + double roll=0.0, pitch=0.0, yaw=0.0; //phi, theta, psi + //double integral_surge=0.0; double integral_pitch=0.0; double integral_yaw=0.0; }; class Guidance_data:public State{ @@ -30,3 +32,7 @@ class Guidance_data:public State{ Guidance_data operator-(const Guidance_data& other) const; Guidance_data& operator=(const std_msgs::msg::Float64MultiArray& msg); }; + +angle NED_to_BODY(const angle &a,const State &s); +Eigen::Vector3d NED_to_BODY(const Eigen::Vector3d &a, const State &s); + diff --git a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp index 4b3762254..1254ab493 100644 --- a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp +++ b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp @@ -55,7 +55,7 @@ class Velocity_node : public rclcpp::Node{ std::string topic_odometry; //Variables for timers - int calculation_rate; + //int calculation_rate; int publish_rate; double max_force; @@ -75,8 +75,14 @@ class Velocity_node : public rclcpp::Node{ //LQR Controller LQRController lqr_controller; - LQRparameters lqr_parameters; + //LQRparameters lqr_parameters; + std::vector Q; + std::vector R; + std::vector Qi; + std::vector Ri; std::vector inertia_matrix; + std::vector dampening_matrix_low; + std::vector dampening_matrix_high; //Test diff --git a/control/velocity_controller/package.xml b/control/velocity_controller/package.xml index 3f116b992..a8fd24457 100644 --- a/control/velocity_controller/package.xml +++ b/control/velocity_controller/package.xml @@ -14,6 +14,13 @@ vortex_msgs geometry_msgs nav_msgs + ct_optcon + ct_core + vortex_utils + + + + ament_lint_auto ament_lint_common diff --git a/control/velocity_controller/src/LQR_setup.cpp b/control/velocity_controller/src/LQR_setup.cpp index 8bbd0dbc4..99302e86c 100644 --- a/control/velocity_controller/src/LQR_setup.cpp +++ b/control/velocity_controller/src/LQR_setup.cpp @@ -1,3 +1,4 @@ + #include "velocity_controller/LQR_setup.hpp" #include "rclcpp/rclcpp.hpp" #include @@ -12,15 +13,57 @@ #include "velocity_controller/PID_setup.hpp" #include "velocity_controller/utilities.hpp" #include -#include +//#include #include "vortex/utils/math.hpp" +#include "ct/optcon/lqr/LQR.hpp" + Eigen::IOFormat fmt(Eigen::StreamPrecision, 0, ", ", "\n", "[", "]"); -LQRController::LQRController(LQRparameters params,Eigen::Matrix3d inertia_matrix){ - set_params(params); - set_matrices(inertia_matrix); +LQRController::LQRController() +{ + }; +int LQRController::set_matrices(std::vector Q_,std::vector R_,std::vector inertia_matrix_,double max_force_, std::vector water_r_low,std::vector water_r_high){ + //Possible error handling here to check for size and allowed values. + if (Q_.size()!=8){ + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The Q matrix has the wrong amount of elements"); + return 0; + } + if(R_.size()!=3){ + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The R matrix has the wrong amount of elements"); + return 0; + } + if(inertia_matrix_.size()!=36){ + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The M matrix has the wrong amount of elements"); + return 0; + } + if(water_r_low.size()!=36||water_r_high.size()!=36){ + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The D matrix has the wrong amount of elements"); + return 0; + } + max_force=max_force_; + Q.diagonal()=Eigen::Map(Q_.data(),Q_.size()); + R.diagonal()=Eigen::Map(R_.data(),R_.size()); + Eigen::Matrix inertia_matrix = Eigen::Map>(inertia_matrix_.data(),6,6); + D_low=Eigen::Map>(water_r_low.data(),6,6); + D_high=Eigen::Map>(water_r_high.data(),6,6); + inertia_matrix_inv=inertia_matrix.inverse(); + + Eigen::MatrixB_t=inertia_matrix_inv*(Eigen::Matrix()<<1,0,0, 0,0,0, 0,0,0, 0,0,0, 0,1,0, 0,0,1).finished(); + B.setZero(); + Eigen::Matrix B_m; + B_m.setZero(); + B_m.block<6,3>(0,0)=B_t; + std::vector> swaplines{{1,7},{2,8},{3,4},{4,5}}; + for (long unsigned int i=0;i(0,0)=B_m.block<5,3>(0,0); + integral_error_surge= 0.0; integral_error_pitch= 0.0; integral_error_yaw= 0.0; + surge_windup= false; pitch_windup= false; yaw_windup= false; mass=inertia_matrix_[0]; + return 1; +} /*angle LQRController::quaternion_to_euler_angle(double w, double x, double y, double z){ @@ -60,72 +103,77 @@ std::tuple LQRController::saturate (double value, bool windup, do -double LQRController::anti_windup(double ki, double error, double integral_sum, bool windup){ +double LQRController::anti_windup(double error, double integral_sum, bool windup){ if (!windup){ - integral_sum += error * ki; + integral_sum += error*interval_; } return integral_sum; } -Eigen::Matrix3d LQRController::calculate_coriolis_matrix(double pitchrate, double yaw_rate, double sway_vel, double heave_vel){ +/*Eigen::Matrix3d LQRController::calculate_coriolis_matrix(double pitchrate, double yaw_rate, double sway_vel, double heave_vel){ //Inertia matrix values?? Eigen::Matrix3d result; result<<0.2,-30*sway_vel*0.01,-30*heave_vel*0.01, 30 * sway_vel*0.01,0,1.629 * pitchrate, 30 * heave_vel*0.01,1.769 * yaw_rate,0; return result; -} +}*/ -void LQRController::set_params(LQRparameters params){ - //set LQR parameters - integral_error_surge= 0.0; integral_error_pitch= 0.0; integral_error_yaw= 0.0; - surge_windup= false; pitch_windup= false; yaw_windup= false; - q_surge= params.q_surge; q_pitch= params.q_pitch; q_yaw= params.q_yaw; - r_surge= params.r_surge; r_pitch= params.r_pitch; r_yaw= params.r_yaw; - i_surge= params.i_surge; i_pitch= params.i_pitch; i_yaw= params.i_yaw; - i_weight= params.i_weight; max_force= params.max_force; - return; -} -void LQRController::set_matrices(Eigen::Matrix3d inertia_matrix){ - inertia_matrix_inv = inertia_matrix.inverse(); - state_weight_matrix.diagonal()< LQRController::linearize(State s){ + //Eigen::Matrix A; + Eigen::Matrix D; + if (s.surge<100){ //Threshold tbd + D=-inertia_matrix_inv*D_low; + } + else { + D=-inertia_matrix_inv*D_high; + } + Eigen::Matrix C; + C.setZero(); //Unødvendig kanskje + C(1,5)=-mass*s.surge; + C(2,4)=mass*s.surge; + D-=inertia_matrix_inv*C; //To avoid unneccessary allocation + /*Eigen::Matrix T(1.0,sin(s.psi)*tan(s.theta),cos(s.psi)*tan(s.theta),s.pitch*cos(s.psi)*tan(s.theta)-s.yaw*sin(s.psi)*tan(s.theta),(s.pitch*sin(s.psi)+s.yaw*cos(s.psi))/(cos(s.theta)*cos(s.theta)), + 0,cos(s.psi),-sin(s.psi),s.yaw*sin(s.psi)+s.pitch*cos(s.psi),0,0, + 0,sin(s.psi)*1/cos(s.theta),cos(s.psi)/cos(s.theta),s.sway*cos(s.psi)/cos(s.theta)-s.pitch*sin(s.psi)/cos(s.theta),(s.yaw*sin(s.psi)+s.pitch*cos(s.psi)*sin(s.theta)/(cos(s.theta)*cos(s.theta)))); +*/ + Eigen::Matrix T; + T<<1,sin(s.yaw)*tan(s.pitch),cos(s.yaw)*tan(s.pitch), + 0,cos(s.yaw),-sin(s.yaw), + 0,sin(s.yaw)/cos(s.pitch),cos(s.yaw)/cos(s.pitch); + Eigen::Matrix A; + A.block<6,6>(0,0)=D; + A.block<3,3>(0,6)=A.block<3,3>(6,0)=A.block<3,3>(6,6)=Eigen::Matrix3d::Zero(); + A.block<3,3>(6,3)=T; + std::vector> swaplines{{1,7},{2,8},{3,4},{4,5}}; + for (long unsigned int i=0;i ret; + ret.setZero(); + ret.block<5,5>(0,0)=A.block<5,5>(0,0); + //legge inn integral state #TODO + ret.block<3,3>(5,0)=-Eigen::Matrix3d::Identity(); + + return ret; }; -Eigen::Vector LQRController::update_error(Guidance_data guidance_values, State states){ +Eigen::Vector LQRController::update_error(Guidance_data guidance_values, State states){ double surge_error = guidance_values.surge - states.surge; double pitch_error = vortex::utils::math::ssa(guidance_values.pitch - states.pitch); double yaw_error = vortex::utils::math::ssa(guidance_values.yaw - states.yaw); - - integral_error_surge = anti_windup(i_surge, surge_error, integral_error_surge, surge_windup); - integral_error_pitch = anti_windup(i_pitch, pitch_error, integral_error_pitch, pitch_windup); - integral_error_yaw = anti_windup(i_yaw, yaw_error, integral_error_yaw, yaw_windup); - - Eigen::Vector state_error= {-surge_error, -pitch_error, -yaw_error, integral_error_surge, integral_error_pitch, integral_error_yaw}; + + integral_error_surge = anti_windup(surge_error, integral_error_surge, surge_windup); + integral_error_pitch = anti_windup(pitch_error, integral_error_pitch, pitch_windup); + integral_error_yaw = anti_windup(yaw_error, integral_error_yaw, yaw_windup); + + Eigen::Vector state_error= {-surge_error, -pitch_error, -yaw_error, -states.pitch_rate, -states.yaw_rate, integral_error_surge, integral_error_pitch, integral_error_yaw}; return state_error; } Eigen::Vector LQRController::saturate_input(Eigen::Vector u){ @@ -135,15 +183,21 @@ Eigen::Vector LQRController::saturate_input(Eigen::Vector u) std::tie(yaw_windup, torque_z) = saturate(u[2], yaw_windup, max_force); return {force_x, torque_y, torque_z}; } -Eigen::Vector LQRController::calculate_lqr_u(State state, Guidance_data guidance_values){ - update_augmented_matrices(calculate_coriolis_matrix(state.pitch_rate,state.yaw_rate,state.sway_vel,state.heave_vel)); - LQRsolveResult result = solve_k_p(augmented_system_matrix,augmented_input_matrix,state_weight_matrix,input_weight_matrix); - if(result.INFO!=0){ +Eigen::Vector LQRController::calculate_thrust(State state, Guidance_data guidance_values){ + ct::optcon::LQR<8,3> lqr; + Eigen::Matrix K_l; + bool INFO= lqr.compute(Q,R,linearize(state),B,K_l,true,false); + if(INFO==0){ return {9999,9999,9999}; //Need to fix } - Eigen::Matrix state_error = update_error(guidance_values, state); - Eigen::Vector u= saturate_input(- (result.K*state_error)); - return u; + /* + Eigen::Matrix K; + K.block<3,3>(0,0)=K_l.block<3,3>(0,0); + K.block<3,3>(0,3)=K_l.block<3,3>(0,5); + */ + + Eigen::Matrix state_error = update_error(guidance_values, state); + return saturate_input(- (K_l*state_error)); } void LQRController::reset_controller(){ integral_error_surge=0.0; @@ -155,7 +209,10 @@ void LQRController::reset_controller(){ yaw_windup=false; return; } - +int LQRController::set_interval(double interval){ + interval_=interval; + return 1; +} extern "C" { // Fortran subroutine for solving symplectic Schur decomposition(double precision version) @@ -172,8 +229,16 @@ void sb02md_( const char* DICO, const char* HINV, const char* UPLO, const char* ); } +/* +LQRsolveResult LQRController::solve_lqr(const Eigen::MatrixXd &A,const Eigen::MatrixXd &B, const Eigen::MatrixXd &Q, const Eigen::MatrixXd &R){ + + const int N=A.rows(); + const int M=B.cols(); + if (A.cols()!=N||B.rows()!=N||R.rows()!=M||R.cols()!=M||Q.rows()!=N||Q.cols()!=N){ + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The dimensions of the matrices for solve_lqr are wrong"); + return LQRsolveResult{}; + } -LQRsolveResult LQRController::solve_k_p(const Eigen::Matrix &A,const Eigen::Matrix &B, const Eigen::Matrix &Q, const Eigen::Matrix &R){ Eigen::Matrix A_copy=A, Q_copy=Q; Eigen::Matrix B_copy=B; Eigen::Matrix R_copy=R; @@ -181,7 +246,6 @@ LQRsolveResult LQRController::solve_k_p(const Eigen::Matrix &A,const //calculate G, L is zero, unfactored R, Upper triangle i think char JOBG='G',JOBL='Z',FACT='N',UPLO='U'; //Order of matrices A, Q, G and X(P), Order of matrix R and nuber of columns in B and L(is zero) - const int N=6, M=3; //Dimensions of matrices int LDA=N, LDB=N, LDQ=N,LDR=M,LDL=N,LDG=N; std::vector IWORK(8*N),IPIV(N); @@ -237,6 +301,7 @@ LQRsolveResult LQRController::solve_k_p(const Eigen::Matrix &A,const return LQRsolveResult(K,G,INFO1); } + */ diff --git a/control/velocity_controller/src/LQR_test.cpp b/control/velocity_controller/src/LQR_test.cpp index 4358f61e6..392103cff 100644 --- a/control/velocity_controller/src/LQR_test.cpp +++ b/control/velocity_controller/src/LQR_test.cpp @@ -1,6 +1,7 @@ #include "rclcpp/rclcpp.hpp" #include #include "velocity_controller/LQR_setup.hpp" +#include "ct/optcon/lqr/LQR.hpp" class test_LQR_node : public rclcpp::Node{ public: @@ -10,10 +11,16 @@ class test_LQR_node : public rclcpp::Node{ LQRController controller; test_LQR_node():Node("test_LQR_node"), controller(){ RCLCPP_INFO(this->get_logger(),"LQR test node started"); - Eigen::Matrix Q=(Eigen::Matrix()<<75,0,0,0,0,0,0,175,0,0,0,0,0,0,175,0,0,0,0,0,0,0.3,0,0,0,0,0,0,0.4,0,0,0,0,0,0,0.3).finished(); - Eigen::Matrix R=(Eigen::Matrix3d()<<0.3,0,0,0,0.4,0,0,0,0.4).finished(); - Eigen::Matrix A=(Eigen::Matrix()<<5,7,23,0,0,0,0,45,21,4,3,4,0,23,1,7,6,5,5,7,6,3,5,7,2,2,3,2,1,0,0,0,8,7,6,5).finished(); - Eigen::Matrix B=(Eigen::Matrix()<<2,0,0,3,0,2,0,3,0,1,2,0,3,4,0,3,5,0).finished(); + Eigen::Matrix Q; + Eigen::VectorXd qdiag(8); + qdiag << 75, 175, 175, 1, 0.3, 0.4, 0.3, 1; + Q.diagonal() = qdiag; + Eigen::Matrix R; + Eigen::VectorXd rdiag(3); + rdiag << 0.3, 0.4, 0.4; + R.diagonal() = rdiag; + Eigen::Matrix A=(Eigen::Matrix()<<5,7,23,0,0,0,1,1,0,45,21,4,3,4,3,2,0,23,1,7,6,5,5,7,5,7,6,3,5,7,0,9,2,2,3,2,1,0,3,7,0,0,8,7,6,5,8,5,4,7,1,5,6,2,4,7,6,2,4,5,2,12,5,6).finished(); + Eigen::Matrix B=(Eigen::Matrix()<<2,0,0,3,0,2,0,3,0,1,2,0,3,4,0,3,5,0,6,3,4,1,2,3).finished(); /*Eigen::Matrix3d inertia_matrix=(Eigen::Matrix3d()<<30.0, 0.6, 0.0, 0.6, 1.629, 0.0, 0.0, 0.0, 1.729).finished(); Eigen::Matrix3d inertia_matrix_inv=inertia_matrix.inverse(); Eigen::Matrix3d coriolis_matrix=(Eigen::Matrix3d()<<0.2,-30*2*0.01,-30*2*0.0,30 * 2*0.01,0,1.629 * 2,30 * 2*0.01,1.769 * 2,0).finished(); @@ -27,13 +34,15 @@ class test_LQR_node : public rclcpp::Node{ Eigen::Matrix augmented_input_matrix=(Eigen::Matrix()<< inertia_matrix_inv(0,0),inertia_matrix_inv(0,1),inertia_matrix_inv(0,2),0,0,0, inertia_matrix_inv(1,0),inertia_matrix_inv(1,1),inertia_matrix_inv(1,2),0,0,0, inertia_matrix_inv(2,0),inertia_matrix_inv(2,1),inertia_matrix_inv(2,2),0,0,0).finished();*/ - LQRsolveResult result=controller.solve_k_p(A,B,Q,R); + ct::optcon::LQR<8,3> lqr_solver; + Eigen::Matrix K; + lqr_solver.compute(Q,R,A,B,K,true,false); RCLCPP_INFO(this->get_logger(),"LQR Gain K matrix:"); - RCLCPP_INFO(this->get_logger(),"\n%f %f %f %f %f %f\n%f %f %f %f %f %f\n%f %f %f %f %f %f", - result.K(0,0),result.K(0,1),result.K(0,2),result.K(0,3),result.K(0,4),result.K(0,5), - result.K(1,0),result.K(1,1),result.K(1,2),result.K(1,3),result.K(1,4),result.K(1,5), - result.K(2,0),result.K(2,1),result.K(2,2),result.K(2,3),result.K(2,4),result.K(2,5)); - + RCLCPP_INFO(this->get_logger(),"\n%f %f %f %f %f %f %f %f\n%f %f %f %f %f %f %f %f\n%f %f %f %f %f %f %f %f", + K(0,0),K(0,1),K(0,2),K(0,3),K(0,4),K(0,5),K(0,6),K(0,7), + K(1,0),K(1,1),K(1,2),K(1,3),K(1,4),K(1,5),K(1,6),K(1,7), + K(2,0),K(2,1),K(2,2),K(2,3),K(2,4),K(2,5),K(2,6),K(2,7)); + } }; diff --git a/control/velocity_controller/src/ct_instantiations.cpp b/control/velocity_controller/src/ct_instantiations.cpp new file mode 100644 index 000000000..124a25e99 --- /dev/null +++ b/control/velocity_controller/src/ct_instantiations.cpp @@ -0,0 +1,16 @@ +// src/ct_instantiations.cpp +// This file exists ONLY to emit Control Toolbox symbols + +//#include +#include +//#define CT_OPTCON_ENABLE_LQR +//#define CT_CORE_ENABLE_CARE + +//#include +//#include + +//#include +//#include + +template class ct::optcon::LQR<8, 3>; +template class ct::optcon::CARE<8, 3>; \ No newline at end of file diff --git a/control/velocity_controller/src/test_VC.cpp b/control/velocity_controller/src/test_VC.cpp index ef2af22a1..fe5ac3f37 100644 --- a/control/velocity_controller/src/test_VC.cpp +++ b/control/velocity_controller/src/test_VC.cpp @@ -3,7 +3,7 @@ #include #include #include -#include "velocity_controller/PID_setup.hpp" +///#include "velocity_controller/PID_setup.hpp" #include "velocity_controller/test_VC.hpp" #include #include @@ -31,7 +31,7 @@ test_VC::test_VC() : Node("test_VC_node") std::bind(&test_VC::send_guidance, this)); clock_ = this->get_clock(); RCLCPP_INFO(this->get_logger(), "Test_VC node has been started"); - reference_msg.surge=0.2;reference_msg.pitch=0.3;reference_msg.yaw=0.3; //Surge, pitch, yaw + reference_msg.surge=0.2;reference_msg.pitch=-1.22;reference_msg.yaw=0.0; //Surge, pitch, yaw } diff --git a/control/velocity_controller/src/utilities.cpp b/control/velocity_controller/src/utilities.cpp index 3ad236870..29faf687a 100644 --- a/control/velocity_controller/src/utilities.cpp +++ b/control/velocity_controller/src/utilities.cpp @@ -18,4 +18,44 @@ angle quaternion_to_euler_angle(double w, double x, double y, double z){ double psi = std::atan2(t3, t4); return {phi, theta, psi}; -}; \ No newline at end of file +}; +angle NED_to_BODY(const angle &a,const State &s){ + //TODO tests for illegal angles + Eigen::Vector3d q; + q< navigation: v_nav = R_n_b * v_body + Eigen::Matrix3d R_n_b = Rz * Ry * Rx; + + // To get body from navigation (NED->BODY): apply transpose (inverse) + Eigen::Vector3d v_body = R_n_b.transpose() * a; + /* + Eigen::Matrix3d T; + T< -#include +//#include +//#include #include "std_msgs/msg/bool.hpp" #include "velocity_controller/PID_setup.hpp" #include #include #include "vortex_msgs/msg/los_guidance.hpp" #include "vortex/utils/math.hpp" +#include "velocity_controller/utilities.hpp" -//#include "vortex-msgs/msg" kan legge til nye meldinger nå - -//Lager en klasse velocity node - //Konstruktør -Velocity_node::Velocity_node() : Node("velocity_controller_node"), PID_surge(20,1,2), PID_yaw(4,0.5,1), PID_pitch(4,0.5,1), lqr_controller() +Velocity_node::Velocity_node() : Node("velocity_controller_node"), PID_surge(100,5,0), PID_yaw(1,0.1,0), PID_pitch(35,1,0), lqr_controller() { //Dytter info til log RCLCPP_INFO(this->get_logger(), "Velocity control node has been started."); @@ -49,43 +46,43 @@ Velocity_node::Velocity_node() : Node("velocity_controller_node"), PID_surge(20, //Timer - timer_calculation = this->create_wall_timer(std::chrono::milliseconds(calculation_rate), std::bind(&Velocity_node::calc_thrust, this)); + timer_calculation = this->create_wall_timer(std::chrono::milliseconds(publish_rate), std::bind(&Velocity_node::calc_thrust, this)); timer_publish = this->create_wall_timer(std::chrono::milliseconds(publish_rate), std::bind(&Velocity_node::publish_thrust, this)); //Controllers PID_surge.set_output_limits(-max_force, max_force); PID_pitch.set_output_limits(-max_force, max_force); PID_yaw.set_output_limits(-max_force, max_force); - lqr_controller.set_params(lqr_parameters); - lqr_controller.set_matrices(vector_to_matrix3d(inertia_matrix)); - + if(!lqr_controller.set_matrices(Q,R,inertia_matrix,max_force,dampening_matrix_low,dampening_matrix_high)||!lqr_controller.set_interval(publish_rate/1000)){ + controller_type=1; + RCLCPP_INFO(this->get_logger(),"Switching to PID"); + }; } + + //Publish/timer functions void Velocity_node::publish_thrust() { - /*thrust_out.wrench.force.x=100; - thrust_out.wrench.force.y=0; - thrust_out.wrench.force.z=0; - thrust_out.wrench.torque.x=0; - thrust_out.wrench.torque.y=0; - thrust_out.wrench.torque.z=0;*/ publisher_thrust->publish(thrust_out); - //RCLCPP_DEBUG(this->get_logger(),"Publishing thrust: %.3f",thrust_out.wrench.force.x); } //** må forbedre integrasjon og derivasjons beregningene void Velocity_node::calc_thrust() { + angle NED_error={guidance_values.roll-current_state.roll,guidance_values.pitch-current_state.pitch,guidance_values.yaw-current_state.yaw}; + angle error=NED_to_BODY(NED_error,current_state); + Guidance_data mod_g_values=guidance_values; + mod_g_values.surge=guidance_values.surge*cos(error.psit)*cos(error.thetat); switch (controller_type) { case 1:{ - //RCLCPP_INFO(this->get_logger(),"PID controller"); - PID_surge.calculate_thrust(guidance_values.surge, current_state.surge,calculation_rate/1000.0); - PID_pitch.calculate_thrust(guidance_values.pitch, current_state.pitch,calculation_rate/1000.0); - PID_yaw.calculate_thrust(guidance_values.yaw, current_state.yaw,calculation_rate/1000.0); + + PID_surge.calculate_thrust(mod_g_values.surge-current_state.surge,publish_rate/1000.0); + PID_pitch.calculate_thrust(error.thetat,publish_rate/1000.0); + PID_yaw.calculate_thrust(error.psit,publish_rate/1000.0); thrust_out.wrench.force.x = PID_surge.output(); thrust_out.wrench.torque.y = PID_pitch.output(); thrust_out.wrench.torque.z = PID_yaw.output(); @@ -93,8 +90,8 @@ void Velocity_node::calc_thrust() break; } case 2:{ - //RCLCPP_INFO(this->get_logger(),"LQR controller"); - Eigen::Vector3d u=lqr_controller.calculate_lqr_u(current_state,guidance_values); + + Eigen::Vector3d u=lqr_controller.calculate_thrust(current_state,mod_g_values); if (u==Eigen::Vector3d{9999,9999,9999}){ controller_type=1; RCLCPP_ERROR(this->get_logger(),"Switching to PID"); @@ -107,6 +104,8 @@ void Velocity_node::calc_thrust() break; } default:{ + //Some crash handling here + RCLCPP_ERROR(this->get_logger(),"Unknown controller set"); break; } } @@ -129,12 +128,18 @@ void Velocity_node::guidance_callback(const vortex_msgs::msg::LOSGuidance::Share void Velocity_node::odometry_callback(const nav_msgs::msg::Odometry::SharedPtr msg_ptr){ //RCLCPP_INFO(this->get_logger(),"Recieved odometry"); angle temp=quaternion_to_euler_angle(msg_ptr->pose.pose.orientation.w, msg_ptr->pose.pose.orientation.x, msg_ptr->pose.pose.orientation.y, msg_ptr->pose.pose.orientation.z); + //angles + current_state.roll = temp.phit; current_state.pitch = temp.thetat; current_state.yaw = temp.psit; - current_state.surge = msg_ptr->twist.twist.linear.x; + //angular velocity + current_state.roll_rate=msg_ptr->twist.twist.angular.x; current_state.pitch_rate=msg_ptr->twist.twist.angular.y; current_state.yaw_rate=msg_ptr->twist.twist.angular.z; - //Need to update angular speed NB!!. + //velocity + current_state.surge = msg_ptr->twist.twist.linear.x; + current_state.sway = msg_ptr->twist.twist.linear.y; + current_state.heave = msg_ptr->twist.twist.linear.z; return; } @@ -155,18 +160,12 @@ void Velocity_node::get_new_parameters(){ this->topic_thrust = this->get_parameter("topics.thrust_topic").as_string(); this->declare_parameter("topics.guidance_topic"); this->topic_guidance = this->get_parameter("topics.guidance_topic").as_string(); - //this->declare_parameter("topics.twist_topic"); - //this->topic_twist = this->get_parameter("topics.twist_topic").as_string(); - //this->declare_parameter("topics.pose_topic"); - //this->topic_pose = this->get_parameter("topics.pose_topic").as_string(); this->declare_parameter("topics.odom_topic"); this->topic_odometry = this->get_parameter("topics.odom_topic").as_string(); this->declare_parameter("topics.killswitch_topic"); this->topic_killswitch = this->get_parameter("topics.killswitch_topic").as_string(); this->declare_parameter("max_force"); this->max_force = this->get_parameter("max_force").as_double(); - this->declare_parameter("calculation_rate"); - this->calculation_rate = this->get_parameter("calculation_rate").as_int(); this->declare_parameter("publish_rate"); this->publish_rate = this->get_parameter("publish_rate").as_int(); this->declare_parameter("controller_type"); @@ -174,31 +173,22 @@ void Velocity_node::get_new_parameters(){ //LQR Parameters - this->declare_parameter("LQR_params.q_surge"); - this->declare_parameter("LQR_params.q_pitch"); - this->declare_parameter("LQR_params.q_yaw"); - this->declare_parameter("LQR_params.r_surge"); - this->declare_parameter("LQR_params.r_pitch"); - this->declare_parameter("LQR_params.r_yaw"); - this->declare_parameter("LQR_params.i_surge"); - this->declare_parameter("LQR_params.i_pitch"); - this->declare_parameter("LQR_params.i_yaw"); - this->declare_parameter("LQR_params.i_weight"); - this->declare_parameter("LQR_params.dt"); + + this->declare_parameter>("LQR_params.Q"); + Q=this->get_parameter("LQR_params.Q").as_double_array(); + this->declare_parameter>("LQR_params.R"); + R=this->get_parameter("LQR_params.R").as_double_array(); this->declare_parameter>("inertia_matrix"); + this->get_parameter("inertia_matrix", inertia_matrix); + + //D + this->declare_parameter>("dampening_matrix_low"); + this->declare_parameter>("dampening_matrix_high"); + this->dampening_matrix_low=this->get_parameter("dampening_matrix_low").as_double_array(); + this->dampening_matrix_high=this->get_parameter("dampening_matrix_high").as_double_array(); + + - this->lqr_parameters.q_surge=this->get_parameter("LQR_params.q_surge").as_double(); - this->lqr_parameters.q_pitch=this->get_parameter("LQR_params.q_pitch").as_double(); - this->lqr_parameters.q_yaw=this->get_parameter("LQR_params.q_yaw").as_double(); - this->lqr_parameters.r_surge=this->get_parameter("LQR_params.r_surge").as_double(); - this->lqr_parameters.r_pitch=this->get_parameter("LQR_params.r_pitch").as_double(); - this->lqr_parameters.r_yaw=this->get_parameter("LQR_params.r_yaw").as_double(); - this->lqr_parameters.i_surge=this->get_parameter("LQR_params.i_surge").as_double(); - this->lqr_parameters.i_pitch=this->get_parameter("LQR_params.i_pitch").as_double(); - this->lqr_parameters.i_yaw=this->get_parameter("LQR_params.i_yaw").as_double(); - this->lqr_parameters.i_weight=this->get_parameter("LQR_params.i_weight").as_double(); - this->lqr_parameters.max_force=max_force; - this->inertia_matrix=this->get_parameter("inertia_matrix").as_double_array(); } From 7273d4374d54cda811bee21dfcddda3f2be8cfc6 Mon Sep 17 00:00:00 2001 From: Henrik Date: Sat, 24 Jan 2026 11:06:21 +0100 Subject: [PATCH 050/128] minor changes to PID --- .../include/velocity_controller/PID_setup.hpp | 3 +-- control/velocity_controller/src/PID_setup.cpp | 7 +------ 2 files changed, 2 insertions(+), 8 deletions(-) diff --git a/control/velocity_controller/include/velocity_controller/PID_setup.hpp b/control/velocity_controller/include/velocity_controller/PID_setup.hpp index 91303d5dc..48b940cbc 100644 --- a/control/velocity_controller/include/velocity_controller/PID_setup.hpp +++ b/control/velocity_controller/include/velocity_controller/PID_setup.hpp @@ -10,7 +10,7 @@ class PID_controller { public: PID_controller( double k_p, double k_i, double k_d, double max_output, double min_output); PID_controller(double k_p, double k_i, double k_d) : PID_controller(k_p, k_i, k_d, 100.0, -100.0) {}; - void calculate_thrust(double reference, double current_position, double dt); + void calculate_thrust(double error, double dt); void reset_controller(); double output(); void set_output_limits(double min_output, double max_output); @@ -20,7 +20,6 @@ class PID_controller { double k_d; double integral; double previous_error; - double previous_position; double last_output; double max_output; double min_output; diff --git a/control/velocity_controller/src/PID_setup.cpp b/control/velocity_controller/src/PID_setup.cpp index 451d26935..bf9845e05 100644 --- a/control/velocity_controller/src/PID_setup.cpp +++ b/control/velocity_controller/src/PID_setup.cpp @@ -5,16 +5,12 @@ PID_controller::PID_controller( double k_p, double k_i, double k_d, double max_output, double min_output):k_p(k_p), k_i(k_i), k_d(k_d), max_output(max_output), min_output(min_output) { integral = 0.0; previous_error = 0.0; - previous_position = 0.0; }; -void PID_controller::calculate_thrust(double reference, double current_position, double dt){ - //Error calculation - double error = reference - current_position; +void PID_controller::calculate_thrust(double error, double dt){ //P calculation last_output=k_p*error; //D calculation last_output += k_d * (error - previous_error) / dt; - previous_position = current_position; //I calculation with anti-windup integral += error * dt; if (integral > max_output) { @@ -38,7 +34,6 @@ void PID_controller::calculate_thrust(double reference, double current_position, void PID_controller::reset_controller(){ integral = 0.0; previous_error = 0.0; - previous_position = 0.0; } double PID_controller::output(){ From ab5a0679e3a38d619cd26c896755700318028415 Mon Sep 17 00:00:00 2001 From: Henrik Date: Sat, 24 Jan 2026 17:29:24 +0100 Subject: [PATCH 051/128] added framework for tests --- control/velocity_controller/CMakeLists.txt | 156 ++++++++++-------- .../velocity_controller/test/test_Node.cpp | 28 ++++ 2 files changed, 114 insertions(+), 70 deletions(-) create mode 100644 control/velocity_controller/test/test_Node.cpp diff --git a/control/velocity_controller/CMakeLists.txt b/control/velocity_controller/CMakeLists.txt index 47799ece9..ac870003d 100644 --- a/control/velocity_controller/CMakeLists.txt +++ b/control/velocity_controller/CMakeLists.txt @@ -16,26 +16,13 @@ find_package(vortex_msgs REQUIRED) find_package(vortex_utils REQUIRED) find_package(Eigen3 REQUIRED) find_package(CasADi REQUIRED) -find_package(LAPACK REQUIRED) -find_package(BLAS REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(ct_optcon REQUIRED) find_package(ct_core REQUIRED) -#find_package(stonefish_ros2 REQUIRED) -#find_package(auv_setup REQUIRED) -#find_package(stonefish_sim REQUIRED) -#find_package(thrust_allocator_auv REQUIRED) - -#target_include_directories(velocity_controller_node -# PUBLIC -# $ -# $ -#) include_directories( ${EIGEN3_INCLUDE_DIR} - include ) @@ -47,70 +34,31 @@ add_executable(velocity_controller_node src/ct_instantiations.cpp ) -add_executable(test_VC_node - src/test_VC.cpp - #src/PID_setup.cpp - #src/LQR_setup.cpp - src/utilities.cpp - src/ct_instantiations.cpp -) -add_executable(test_LQR_node - src/LQR_test.cpp - src/LQR_setup.cpp - src/utilities.cpp - src/ct_instantiations.cpp - ) + +#add_executable(test_LQR_node +# src/LQR_test.cpp +# src/LQR_setup.cpp +# src/utilities.cpp +# src/ct_instantiations.cpp +# ) ament_target_dependencies(velocity_controller_node rclcpp std_msgs vortex_msgs geometry_msgs - #Eigen3 CasADi nav_msgs vortex_utils ) -ament_target_dependencies(test_VC_node - rclcpp - std_msgs - vortex_msgs - geometry_msgs - #Eigen3 - nav_msgs - vortex_utils -) - -ament_target_dependencies(test_LQR_node - rclcpp - #Eigen3 - vortex_msgs - geometry_msgs - std_msgs - nav_msgs - vortex_utils -) - - -link_directories(/usr/lib/gcc/x86_64-linux-gnu/11) -set(SLICOT_LIB /usr/lib/x86_64-linux-gnu/libslicot.so) -set(GFORTRAN_LIB /usr/lib/gcc/x86_64-linux-gnu/11/libgfortran.so) -#${ct_optcon_LIBRARIES} -target_link_libraries(velocity_controller_node Eigen3::Eigen ct_optcon ct_core ${LAPACK_LIBRARIES} ${BLAS_LIBRARIES} ${SLICOT_LIB} ${GFORTRAN_LIB}) -target_link_libraries(test_VC_node Eigen3::Eigen ct_optcon ct_core ${LAPACK_LIBRARIES} ${BLAS_LIBRARIES} ${SLICOT_LIB} ${GFORTRAN_LIB}) -target_link_libraries(test_LQR_node Eigen3::Eigen ct_optcon ct_core ${LAPACK_LIBRARIES} ${BLAS_LIBRARIES} ${SLICOT_LIB} ${GFORTRAN_LIB} ) - +target_link_libraries(velocity_controller_node Eigen3::Eigen ct_optcon ct_core) install(TARGETS velocity_controller_node - test_VC_node - test_LQR_node DESTINATION lib/${PROJECT_NAME} ) - - install( DIRECTORY include/ DESTINATION include @@ -122,16 +70,84 @@ install(DIRECTORY DESTINATION share/${PROJECT_NAME}/ ) if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() - #add_subdirectory(test) + + #find_package(GTest REQUIRED) + #find_package(yaml-cpp REQUIRED) + #find_package(tf2 REQUIRED) + include(GoogleTest) + find_package(ament_cmake_gtest REQUIRED) + + set(TEST_BINARY_NAME ${PROJECT_NAME}_test) + + ament_add_gtest(${TEST_BINARY_NAME} + test/test_Node.cpp + src/utilities.cpp + src/ct_instantiations.cpp + src/LQR_setup.cpp + src/PID_setup.cpp + ) + target_include_directories(${TEST_BINARY_NAME} PUBLIC + $ + $ + ${EIGEN3_INCLUDE_DIR} + ) + ament_target_dependencies(${TEST_BINARY_NAME} + rclcpp + std_msgs + vortex_msgs + geometry_msgs + nav_msgs + vortex_utils + ) + target_link_libraries(${TEST_BINARY_NAME} + Eigen3::Eigen + ct_optcon + ct_core + ) + + add_executable(test_VC_node + src/test_VC.cpp + src/utilities.cpp + src/ct_instantiations.cpp + ) + + target_include_directories(test_VC_node PUBLIC + $ + $ + ${EIGEN3_INCLUDE_DIR} + ) + + + target_link_libraries(test_VC_node Eigen3::Eigen ct_optcon ct_core) + + + target_link_libraries( + ${TEST_BINARY_NAME} + ${LIB_NAME} + #yaml-cpp + #GTest::GTest + #spdlog::spdlog + Eigen3::Eigen + ct_optcon + ct_core + ) + + ament_target_dependencies(test_VC_node + rclcpp + std_msgs + vortex_msgs + geometry_msgs + nav_msgs + vortex_utils + ) + + install(TARGETS test_VC_node + DESTINATION lib/${PROJECT_NAME} + ) + + #gtest_discover_tests(${TEST_BINARY_NAME}) + endif() + ament_package() diff --git a/control/velocity_controller/test/test_Node.cpp b/control/velocity_controller/test/test_Node.cpp new file mode 100644 index 000000000..e59cd1464 --- /dev/null +++ b/control/velocity_controller/test/test_Node.cpp @@ -0,0 +1,28 @@ +#include +#include +#include +#include +#include + + +TEST(PID,BASIC){ + PID_controller foo (1,1,0); + ASSERT_NO_THROW(foo.set_output_limits(-10,100)); + ASSERT_NO_THROW(foo.calculate_thrust(1,1)); + EXPECT_TRUE(foo.output()>0); + EXPECT_NEAR(foo.output(),1,0.01); + ASSERT_NO_THROW(foo.calculate_thrust(1,1)); + EXPECT_NEAR(foo.output(),2,0.01); + ASSERT_NO_THROW(foo.calculate_thrust(1000000,1)); + EXPECT_EQ(foo.output(),100); + ASSERT_NO_THROW(foo.calculate_thrust(-100000,1)); + EXPECT_EQ(foo.output(),-10); +} + + + + +int main(int argc,char** argv){ + testing::InitGoogleTest(&argc,argv); + return RUN_ALL_TESTS(); +} \ No newline at end of file From 98d4faab5b3cb40d7da6349dc0886c817af4fd2b Mon Sep 17 00:00:00 2001 From: Henrik Date: Sun, 25 Jan 2026 11:33:22 +0100 Subject: [PATCH 052/128] Finished tests for PID, and changed PID --- control/velocity_controller/CMakeLists.txt | 2 +- .../include/velocity_controller/PID_setup.hpp | 17 +-- control/velocity_controller/package.xml | 4 +- control/velocity_controller/src/PID_setup.cpp | 52 +++++---- .../src/velocity_controller.cpp | 6 +- .../velocity_controller/test/test_Node.cpp | 28 ----- control/velocity_controller/test/test_PID.cpp | 107 ++++++++++++++++++ 7 files changed, 150 insertions(+), 66 deletions(-) delete mode 100644 control/velocity_controller/test/test_Node.cpp create mode 100644 control/velocity_controller/test/test_PID.cpp diff --git a/control/velocity_controller/CMakeLists.txt b/control/velocity_controller/CMakeLists.txt index ac870003d..a35486e5a 100644 --- a/control/velocity_controller/CMakeLists.txt +++ b/control/velocity_controller/CMakeLists.txt @@ -80,7 +80,7 @@ if(BUILD_TESTING) set(TEST_BINARY_NAME ${PROJECT_NAME}_test) ament_add_gtest(${TEST_BINARY_NAME} - test/test_Node.cpp + test/test_PID.cpp src/utilities.cpp src/ct_instantiations.cpp src/LQR_setup.cpp diff --git a/control/velocity_controller/include/velocity_controller/PID_setup.hpp b/control/velocity_controller/include/velocity_controller/PID_setup.hpp index 48b940cbc..d3f5c2564 100644 --- a/control/velocity_controller/include/velocity_controller/PID_setup.hpp +++ b/control/velocity_controller/include/velocity_controller/PID_setup.hpp @@ -8,19 +8,20 @@ class PID_controller { public: - PID_controller( double k_p, double k_i, double k_d, double max_output, double min_output); - PID_controller(double k_p, double k_i, double k_d) : PID_controller(k_p, k_i, k_d, 100.0, -100.0) {}; - void calculate_thrust(double error, double dt); + PID_controller( double k_p=0, double k_i=0, double k_d=0, double max_output=100, double min_output=-100); + //PID_controller(double k_p, double k_i, double k_d) : PID_controller(k_p, k_i, k_d, 100.0, -100.0) {}; + double calculate_thrust(double error, double dt); void reset_controller(); - double output(); - void set_output_limits(double min_output, double max_output); + double get_output(); + bool set_output_limits(double min_output, double max_output); + void set_parameters(double k_p,double k_i, double k_d); private: double k_p; double k_i; double k_d; - double integral; - double previous_error; - double last_output; + double integral=0; + double previous_error=0; + double output=0; double max_output; double min_output; }; diff --git a/control/velocity_controller/package.xml b/control/velocity_controller/package.xml index a8fd24457..897f5f19f 100644 --- a/control/velocity_controller/package.xml +++ b/control/velocity_controller/package.xml @@ -17,8 +17,8 @@ ct_optcon ct_core vortex_utils - - + ament_cmake_gtest + diff --git a/control/velocity_controller/src/PID_setup.cpp b/control/velocity_controller/src/PID_setup.cpp index bf9845e05..0a6466721 100644 --- a/control/velocity_controller/src/PID_setup.cpp +++ b/control/velocity_controller/src/PID_setup.cpp @@ -6,43 +6,47 @@ PID_controller::PID_controller( double k_p, double k_i, double k_d, double max_o integral = 0.0; previous_error = 0.0; }; -void PID_controller::calculate_thrust(double error, double dt){ - //P calculation - last_output=k_p*error; - //D calculation - last_output += k_d * (error - previous_error) / dt; - //I calculation with anti-windup - integral += error * dt; - if (integral > max_output) { - integral -= error * dt; //anti windup - } else if (integral < min_output) { - integral -= error * dt; //anti windup +double PID_controller::calculate_thrust(double error, double dt){ + if (dt<=0){ + return 0; } - last_output += k_i * integral; - previous_error = error; - //Output calculation with saturation + //P + I + D + output=k_p*error+k_i*integral + k_d * (error - previous_error) / dt; - if (last_output > max_output){ - last_output = max_output; + //Saturation + if (output>max_output){ + output = max_output; } - else if (last_output < min_output){ - last_output = min_output; + else if (output < min_output){ + output = min_output; } + else{ + integral+=error*dt; //anti-wind up + } + previous_error = error; - return; + return output; }; void PID_controller::reset_controller(){ integral = 0.0; previous_error = 0.0; + output=0.0; } -double PID_controller::output(){ - return last_output; +double PID_controller::get_output(){ + return output; }; -void PID_controller::set_output_limits(double min_output, double max_output){ +bool PID_controller::set_output_limits(double min_output, double max_output){ + if (max_outputmin_output = min_output; this->max_output = max_output; - return; + return true; }; - +void PID_controller::set_parameters(double k_p,double k_i, double k_d){ + this->k_p=k_p; + this->k_i=k_i; + this->k_d=k_d; +} diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index a37aaf589..82cc38607 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -83,9 +83,9 @@ void Velocity_node::calc_thrust() PID_surge.calculate_thrust(mod_g_values.surge-current_state.surge,publish_rate/1000.0); PID_pitch.calculate_thrust(error.thetat,publish_rate/1000.0); PID_yaw.calculate_thrust(error.psit,publish_rate/1000.0); - thrust_out.wrench.force.x = PID_surge.output(); - thrust_out.wrench.torque.y = PID_pitch.output(); - thrust_out.wrench.torque.z = PID_yaw.output(); + thrust_out.wrench.force.x = PID_surge.get_output(); + thrust_out.wrench.torque.y = PID_pitch.get_output(); + thrust_out.wrench.torque.z = PID_yaw.get_output(); break; } diff --git a/control/velocity_controller/test/test_Node.cpp b/control/velocity_controller/test/test_Node.cpp deleted file mode 100644 index e59cd1464..000000000 --- a/control/velocity_controller/test/test_Node.cpp +++ /dev/null @@ -1,28 +0,0 @@ -#include -#include -#include -#include -#include - - -TEST(PID,BASIC){ - PID_controller foo (1,1,0); - ASSERT_NO_THROW(foo.set_output_limits(-10,100)); - ASSERT_NO_THROW(foo.calculate_thrust(1,1)); - EXPECT_TRUE(foo.output()>0); - EXPECT_NEAR(foo.output(),1,0.01); - ASSERT_NO_THROW(foo.calculate_thrust(1,1)); - EXPECT_NEAR(foo.output(),2,0.01); - ASSERT_NO_THROW(foo.calculate_thrust(1000000,1)); - EXPECT_EQ(foo.output(),100); - ASSERT_NO_THROW(foo.calculate_thrust(-100000,1)); - EXPECT_EQ(foo.output(),-10); -} - - - - -int main(int argc,char** argv){ - testing::InitGoogleTest(&argc,argv); - return RUN_ALL_TESTS(); -} \ No newline at end of file diff --git a/control/velocity_controller/test/test_PID.cpp b/control/velocity_controller/test/test_PID.cpp new file mode 100644 index 000000000..1fa4daa56 --- /dev/null +++ b/control/velocity_controller/test/test_PID.cpp @@ -0,0 +1,107 @@ +#include +#include +#include +#include +#include + +class PID_test : public ::testing::Test{ + protected: + double delta=0.0001; + PID_controller PID; + void SetUp() override{ + PID.set_parameters(0,0,0); + PID.reset_controller(); + } + void TearDown() override{ + + } + +}; +/* +class Node_test : public ::testing:Test{ + protected: + static void SetUpTestSuite(){ + int argc=0; + char ** argv==nullptr; + rclcpp::init(); + } + static void TearDownTestSuite(){ + rclcpp::shutdown(); + } + +}; + +class LQR_test : public ::testing:Test{ + +}; +*/ +TEST_F(PID_test,reset_controller){ + PID.set_parameters(0,1,0); + PID.calculate_thrust(100,100); + PID.calculate_thrust(0,1); + PID.reset_controller(); + SCOPED_TRACE("Scenario: reset"); + EXPECT_NEAR(PID.get_output(),0,delta); + PID.calculate_thrust(0,1); + SCOPED_TRACE("Scenario: reset2"); + EXPECT_NEAR(PID.get_output(),0,delta); +} +TEST_F(PID_test,P){ + PID.set_parameters(1,0,0); + EXPECT_NEAR(PID.calculate_thrust(1,1),1,delta); + EXPECT_NEAR(PID.calculate_thrust(2,1),2,delta); + PID.set_parameters(1.2,0,0); + EXPECT_NEAR(PID.calculate_thrust(-2.2,1),-2.64,delta); + EXPECT_NEAR(PID.calculate_thrust(-1.5,1),-1.8,delta); +} +TEST_F(PID_test,I){ + PID.set_parameters(0,1.1,0); + PID.calculate_thrust(1,1); + EXPECT_NEAR(PID.get_output(),0,delta); + PID.calculate_thrust(1,1); + EXPECT_NEAR(PID.get_output(),1.1,delta); + PID.calculate_thrust(-1,1); + EXPECT_NEAR(PID.get_output(),2.2,delta); + EXPECT_NEAR(PID.calculate_thrust(0,1),1.1,delta); + PID.set_output_limits(-101,101); + PID.reset_controller(); + PID.set_parameters(1,1,0); + EXPECT_NEAR(PID.calculate_thrust(1000,10),101,delta); + EXPECT_NEAR(PID.calculate_thrust(0,1),0,delta); + PID.reset_controller(); + EXPECT_NEAR(PID.calculate_thrust(-10000,1),-101,delta); + PID.calculate_thrust(-50,1); + EXPECT_NEAR(PID.calculate_thrust(1,1),-49,delta); +} +TEST_F(PID_test,D){ + +} +TEST_F(PID_test,illegal_inputs){ + double temp=PID.get_output(); + EXPECT_FALSE(PID.calculate_thrust(1,0)); + EXPECT_NEAR(PID.get_output(),temp,delta); + EXPECT_FALSE(PID.set_output_limits(1,-1)); + EXPECT_FALSE(PID.calculate_thrust(1,-1)); +} +/* +TEST(PID,BASIC){ + PID_controller foo (1,1,0); + ASSERT_NO_THROW(foo.set_output_limits(-10,100)); + ASSERT_NO_THROW(foo.calculate_thrust(1,1)); + EXPECT_TRUE(foo.get_output()>0); + EXPECT_NEAR(foo.get_output(),1,0.01); + ASSERT_NO_THROW(foo.calculate_thrust(1,1)); + EXPECT_NEAR(foo.get_output(),2,0.01); + ASSERT_NO_THROW(foo.calculate_thrust(1000000,1)); + EXPECT_EQ(foo.get_output(),100); + ASSERT_NO_THROW(foo.calculate_thrust(-100000,1)); + EXPECT_EQ(foo.get_output(),-10); +} +*/ + + + +int main(int argc,char** argv){ + testing::InitGoogleTest(&argc,argv); + return RUN_ALL_TESTS(); +} \ No newline at end of file From 47b5121a2c6526c4d61a62ed990958fd43b58a56 Mon Sep 17 00:00:00 2001 From: Anbit Date: Sun, 25 Jan 2026 15:33:34 +0100 Subject: [PATCH 053/128] Fix build error --- guidance/los_guidance/src/los_guidance_ros.cpp | 6 ++++-- guidance/los_guidance/test/integral_los_test.cpp | 2 -- guidance/los_guidance/test/proportional_los_test.cpp | 3 --- 3 files changed, 4 insertions(+), 7 deletions(-) diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 0aa21dcb0..1cac3aa3b 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -65,7 +65,8 @@ void LosGuidanceNode::set_subscribers_and_publisher() { state_debug_pub_ = this->create_publisher( "state_debug", qos_sensor_data); - waypoint_sub_ = this->create_subscription( + waypoint_sub_ = this->create_subscription< + geometry_msgs::msg::PointStamped>( waypoint_topic, qos_sensor_data, std::bind(&LosGuidanceNode::waypoint_callback, this, std::placeholders::_1)); @@ -76,7 +77,8 @@ void LosGuidanceNode::set_subscribers_and_publisher() { std::bind(&LosGuidanceNode::pose_callback, this, std::placeholders::_1)); - odom_sub_ = this->create_subscription( + odom_sub_ = this->create_subscription< + nav_msgs::msg::Odometry>( odom_topic, qos_sensor_data, std::bind(&LosGuidanceNode::odom_callback, this, std::placeholders::_1)); diff --git a/guidance/los_guidance/test/integral_los_test.cpp b/guidance/los_guidance/test/integral_los_test.cpp index f94e93df1..1999abac8 100644 --- a/guidance/los_guidance/test/integral_los_test.cpp +++ b/guidance/los_guidance/test/integral_los_test.cpp @@ -10,8 +10,6 @@ class IntegralLosTest : public ::testing::Test { IntegralLosParams get_params() { IntegralLosParams params; - params.lookahead_distance_h = 1.0; - params.lookahead_distance_v = 1.0; params.k_i_h = 0.1; // needs tuning params.k_i_v = 0.1; // needs tuning params.k_p_h = 0.667; // needs tuning diff --git a/guidance/los_guidance/test/proportional_los_test.cpp b/guidance/los_guidance/test/proportional_los_test.cpp index c704dec68..b35a3ba6f 100644 --- a/guidance/los_guidance/test/proportional_los_test.cpp +++ b/guidance/los_guidance/test/proportional_los_test.cpp @@ -11,9 +11,6 @@ class ProportionalLosTest : public ::testing::Test { ProportionalLosParams params; params.lookahead_distance_h = 10.0; params.lookahead_distance_v = 10.0; - params.k_p_h = 0.667; // needs tuning - params.k_p_v = 0.582; // needs tuning - params.time_step = 0.01; return params; } From a55e63617be23d5081b328b2b53356e0067f974c Mon Sep 17 00:00:00 2001 From: Henrik Date: Sun, 25 Jan 2026 15:55:22 +0100 Subject: [PATCH 054/128] added framework for LQR_tests, changed CMake into 2 files(test and nontest) --- control/velocity_controller/CMakeLists.txt | 79 +------------ .../include/velocity_controller/LQR_setup.hpp | 2 +- control/velocity_controller/src/LQR_setup.cpp | 14 ++- .../src/velocity_controller.cpp | 5 + .../velocity_controller/tests/CMakeLists.txt | 109 ++++++++++++++++++ .../velocity_controller/tests/test_LQR.cpp | 64 ++++++++++ .../{test => tests}/test_PID.cpp | 10 +- 7 files changed, 194 insertions(+), 89 deletions(-) create mode 100644 control/velocity_controller/tests/CMakeLists.txt create mode 100644 control/velocity_controller/tests/test_LQR.cpp rename control/velocity_controller/{test => tests}/test_PID.cpp (95%) diff --git a/control/velocity_controller/CMakeLists.txt b/control/velocity_controller/CMakeLists.txt index a35486e5a..fefad79fa 100644 --- a/control/velocity_controller/CMakeLists.txt +++ b/control/velocity_controller/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.8) project(velocity_controller) set(CMAKE_CXX_STANDARD 20) +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) @@ -70,83 +71,7 @@ install(DIRECTORY DESTINATION share/${PROJECT_NAME}/ ) if(BUILD_TESTING) - - #find_package(GTest REQUIRED) - #find_package(yaml-cpp REQUIRED) - #find_package(tf2 REQUIRED) - include(GoogleTest) - find_package(ament_cmake_gtest REQUIRED) - - set(TEST_BINARY_NAME ${PROJECT_NAME}_test) - - ament_add_gtest(${TEST_BINARY_NAME} - test/test_PID.cpp - src/utilities.cpp - src/ct_instantiations.cpp - src/LQR_setup.cpp - src/PID_setup.cpp - ) - target_include_directories(${TEST_BINARY_NAME} PUBLIC - $ - $ - ${EIGEN3_INCLUDE_DIR} - ) - ament_target_dependencies(${TEST_BINARY_NAME} - rclcpp - std_msgs - vortex_msgs - geometry_msgs - nav_msgs - vortex_utils - ) - target_link_libraries(${TEST_BINARY_NAME} - Eigen3::Eigen - ct_optcon - ct_core - ) - - add_executable(test_VC_node - src/test_VC.cpp - src/utilities.cpp - src/ct_instantiations.cpp - ) - - target_include_directories(test_VC_node PUBLIC - $ - $ - ${EIGEN3_INCLUDE_DIR} - ) - - - target_link_libraries(test_VC_node Eigen3::Eigen ct_optcon ct_core) - - - target_link_libraries( - ${TEST_BINARY_NAME} - ${LIB_NAME} - #yaml-cpp - #GTest::GTest - #spdlog::spdlog - Eigen3::Eigen - ct_optcon - ct_core - ) - - ament_target_dependencies(test_VC_node - rclcpp - std_msgs - vortex_msgs - geometry_msgs - nav_msgs - vortex_utils - ) - - install(TARGETS test_VC_node - DESTINATION lib/${PROJECT_NAME} - ) - - #gtest_discover_tests(${TEST_BINARY_NAME}) - + add_subdirectory(tests) endif() diff --git a/control/velocity_controller/include/velocity_controller/LQR_setup.hpp b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp index 9fa07b97c..fd756be43 100644 --- a/control/velocity_controller/include/velocity_controller/LQR_setup.hpp +++ b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp @@ -43,7 +43,7 @@ class LQRController{ public: LQRController(); - int set_matrices(std::vector Q_,std::vector R_,std::vector inertia_matrix, double max_force,std::vector water_r_low,std::vector water_r_high); + bool set_matrices(std::vector Q_,std::vector R_,std::vector inertia_matrix, double max_force,std::vector water_r_low,std::vector water_r_high); void reset_controller(); Eigen::Vector calculate_thrust(State states, Guidance_data guidance_values); int set_interval(double interval); diff --git a/control/velocity_controller/src/LQR_setup.cpp b/control/velocity_controller/src/LQR_setup.cpp index 99302e86c..a49ec1170 100644 --- a/control/velocity_controller/src/LQR_setup.cpp +++ b/control/velocity_controller/src/LQR_setup.cpp @@ -19,27 +19,31 @@ -Eigen::IOFormat fmt(Eigen::StreamPrecision, 0, ", ", "\n", "[", "]"); +//Eigen::IOFormat fmt(Eigen::StreamPrecision, 0, ", ", "\n", "[", "]"); LQRController::LQRController() { }; -int LQRController::set_matrices(std::vector Q_,std::vector R_,std::vector inertia_matrix_,double max_force_, std::vector water_r_low,std::vector water_r_high){ +bool LQRController::set_matrices(std::vector Q_,std::vector R_,std::vector inertia_matrix_,double max_force_, std::vector water_r_low,std::vector water_r_high){ //Possible error handling here to check for size and allowed values. if (Q_.size()!=8){ RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The Q matrix has the wrong amount of elements"); return 0; } if(R_.size()!=3){ - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The R matrix has the wrong amount of elements"); + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The R matrix has the wrong amount of elements"); return 0; } if(inertia_matrix_.size()!=36){ - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The M matrix has the wrong amount of elements"); + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The M matrix has the wrong amount of elements"); return 0; } if(water_r_low.size()!=36||water_r_high.size()!=36){ - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The D matrix has the wrong amount of elements"); + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The D matrix has the wrong amount of elements"); + return 0; + } + if (max_force_<0){ + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The max_force need to be >0"); return 0; } max_force=max_force_; diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index 82cc38607..d2e6ce1e5 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -75,7 +75,12 @@ void Velocity_node::calc_thrust() angle NED_error={guidance_values.roll-current_state.roll,guidance_values.pitch-current_state.pitch,guidance_values.yaw-current_state.yaw}; angle error=NED_to_BODY(NED_error,current_state); Guidance_data mod_g_values=guidance_values; + if (error.psit<3.14/2 && error.thetat<3.14/2){ //Need to fix to pi mod_g_values.surge=guidance_values.surge*cos(error.psit)*cos(error.thetat); + } + else{ + mod_g_values.surge=0; + } switch (controller_type) { case 1:{ diff --git a/control/velocity_controller/tests/CMakeLists.txt b/control/velocity_controller/tests/CMakeLists.txt new file mode 100644 index 000000000..97fb0db69 --- /dev/null +++ b/control/velocity_controller/tests/CMakeLists.txt @@ -0,0 +1,109 @@ +cmake_minimum_required(VERSION 3.8) +include(GoogleTest) +find_package(ament_cmake_gtest REQUIRED) +#find_package(GTest REQUIRED) +find_package(yaml-cpp REQUIRED) + #find_package(tf2 REQUIRED) + +#Unit test +set(TEST_BINARY_NAME PID_test) + +ament_add_gtest(${TEST_BINARY_NAME} +test_PID.cpp +../src/utilities.cpp +../src/ct_instantiations.cpp +../src/PID_setup.cpp +) + +ament_add_gtest(LQR_test +test_LQR.cpp +../src/utilities.cpp +../src/ct_instantiations.cpp +../src/LQR_setup.cpp +) + +#To DO need to clean up the include directories/dependencies and such +target_include_directories(${TEST_BINARY_NAME} PUBLIC +$ +$ +${EIGEN3_INCLUDE_DIR} +) +target_include_directories(LQR_test PUBLIC +$ +$ +${EIGEN3_INCLUDE_DIR} +) #Need to fix +ament_target_dependencies(${TEST_BINARY_NAME} +rclcpp +std_msgs +vortex_msgs +geometry_msgs +nav_msgs +vortex_utils +) +ament_target_dependencies(LQR_test + rclcpp + vortex_utils + std_msgs + geometry_msgs + vortex_msgs + nav_msgs +) +target_compile_definitions(LQR_test PRIVATE +YAML_PATH="${PROJECT_SOURCE_DIR}/config/parameters.yaml") + +target_link_libraries(${TEST_BINARY_NAME} +Eigen3::Eigen +ct_optcon +ct_core +) +target_link_libraries(LQR_test + Eigen3::Eigen + ct_optcon + ct_core + yaml-cpp +) + + +#System tests + +add_executable(test_VC_node +../src/test_VC.cpp +../src/utilities.cpp +../src/ct_instantiations.cpp +) + +target_include_directories(test_VC_node PUBLIC +$ +$ +${EIGEN3_INCLUDE_DIR} +) + + +target_link_libraries(test_VC_node Eigen3::Eigen ct_optcon ct_core) + + +target_link_libraries( + ${TEST_BINARY_NAME} + ${LIB_NAME} + #yaml-cpp + #GTest::GTest + #spdlog::spdlog + Eigen3::Eigen + ct_optcon + ct_core +) + +ament_target_dependencies(test_VC_node +rclcpp +std_msgs +vortex_msgs +geometry_msgs +nav_msgs +vortex_utils +) + +install(TARGETS test_VC_node +DESTINATION lib/${PROJECT_NAME} +) +#gtest_discover_tests(${TEST_BINARY_NAME}) \ No newline at end of file diff --git a/control/velocity_controller/tests/test_LQR.cpp b/control/velocity_controller/tests/test_LQR.cpp new file mode 100644 index 000000000..2e5485049 --- /dev/null +++ b/control/velocity_controller/tests/test_LQR.cpp @@ -0,0 +1,64 @@ +#include +#include +#include +#include +#include + +class LQR_test : public ::testing::Test{ + protected: + double delta=0.0001; + static inline YAML::Node cfg; + LQRController controller; + static void SetUpTestSuite() { + try { + cfg = YAML::LoadFile(YAML_PATH); + } + catch (const YAML::Exception& e) { + FAIL() << "Failed to load YAML from '" << YAML_PATH << "': " << e.what(); + } + + }; + void SetUp() override{ + controller.set_matrices(cfg["/**"]["ros__parameters"]["LQR_params"]["Q"].as>(),cfg["/**"]["ros__parameters"]["LQR_params"]["R"].as>(),cfg["/**"]["ros__parameters"]["inertia_matrix"].as>(),cfg["/**"]["ros__parameters"]["max_force"].as(),cfg["/**"]["ros__parameters"]["dampening_matrix_low"].as>(),cfg["/**"]["ros__parameters"]["dampening_matrix_high"].as>()); + controller.reset_controller(); + controller.set_interval(0.01); + } + void TearDown() override{ + + } +}; +/* +TEST(LQR,setup){ + LQRController controller; + controller.set_interval(1); + YAML::Node cfg; + ASSERT_NO_THROW(cfg=YAML::LoadFile(YAML_PATH)); + controller.set_matrices(cfg["LQR"]["Q"],cfg["LQR"]["Q"],cfg["LQR"]["Q"],cfg["LQR"]["Q"],100); +}; +*/ +TEST_F(LQR_test,wrong_setup){ + //LQRController controller; + std::vector eight={1,2,3,4,5,6,7,8}; + std::vector six={1,2,3,4,5,6}; + std::vector thirty_six={1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24,25,26,27,28,29,30,31,32,33,34,35,36}; + std::vector three={1,2,3}; + EXPECT_TRUE(controller.set_matrices(eight,three,thirty_six,100,thirty_six,thirty_six)); + EXPECT_FALSE(controller.set_matrices(eight,eight,thirty_six,100,thirty_six,thirty_six)); + EXPECT_FALSE(controller.set_matrices(three,three,thirty_six,100,thirty_six,thirty_six)); + EXPECT_FALSE(controller.set_matrices(eight,three,eight,100,thirty_six,thirty_six)); + EXPECT_FALSE(controller.set_matrices(eight,three,thirty_six,100,eight,thirty_six)); + EXPECT_FALSE(controller.set_matrices(eight,three,thirty_six,100,thirty_six,eight)); + EXPECT_FALSE(controller.set_matrices(eight,three,thirty_six,-100,thirty_six,thirty_six)); +}; + +TEST_F(LQR_test,solve){ + +}; +TEST_F(LQR_test,Direction){ + +} + +int main(int argc,char** argv){ + testing::InitGoogleTest(&argc,argv); + return RUN_ALL_TESTS(); +} diff --git a/control/velocity_controller/test/test_PID.cpp b/control/velocity_controller/tests/test_PID.cpp similarity index 95% rename from control/velocity_controller/test/test_PID.cpp rename to control/velocity_controller/tests/test_PID.cpp index 1fa4daa56..9fcd1a0df 100644 --- a/control/velocity_controller/test/test_PID.cpp +++ b/control/velocity_controller/tests/test_PID.cpp @@ -1,8 +1,8 @@ #include -#include -#include +//#include +//#include #include -#include +//#include class PID_test : public ::testing::Test{ protected: @@ -31,9 +31,7 @@ class Node_test : public ::testing:Test{ }; -class LQR_test : public ::testing:Test{ - -}; + */ TEST_F(PID_test,reset_controller){ PID.set_parameters(0,1,0); From c79cee820eb0f97f544343e6426ebf1d1592d62d Mon Sep 17 00:00:00 2001 From: Henrik Date: Sat, 31 Jan 2026 14:28:52 +0100 Subject: [PATCH 055/128] Fixed integrator, unstability and LQR tests, added repository, other minor changes --- .../config/parameters.yaml | 4 +- .../include/velocity_controller/utilities.hpp | 2 + control/velocity_controller/src/LQR_setup.cpp | 81 ++++++++++--------- .../src/velocity_controller.cpp | 2 +- .../velocity_controller/tests/test_LQR.cpp | 37 ++++++++- dependencies.repos | 3 + 6 files changed, 85 insertions(+), 44 deletions(-) diff --git a/control/velocity_controller/config/parameters.yaml b/control/velocity_controller/config/parameters.yaml index e78a75601..986845f4f 100644 --- a/control/velocity_controller/config/parameters.yaml +++ b/control/velocity_controller/config/parameters.yaml @@ -11,8 +11,8 @@ killswitch_topic: /softwareKillSwitch #Kill Switch LQR_params: - Q: [400.0,32.84,32.84,0.33,0.33,400.0,32.84,32.84] - R: [0.0005,3.1,3.10] #0.1,0.1,0.1] + Q: [300.0,32.84,32.84,32.84,32.84,300.0,32.84,32.84] + R: [0.002,3.1,3.10] #0.1,0.1,0.1] #q_surge: 75.0 #q_pitch: 175.0 #q_yaw: 175.0 diff --git a/control/velocity_controller/include/velocity_controller/utilities.hpp b/control/velocity_controller/include/velocity_controller/utilities.hpp index 1c6e85574..13574d271 100644 --- a/control/velocity_controller/include/velocity_controller/utilities.hpp +++ b/control/velocity_controller/include/velocity_controller/utilities.hpp @@ -20,6 +20,8 @@ class State{ double surge=0.0, sway=0.0, heave=0.0, roll_rate=0.0, pitch_rate=0.0, yaw_rate=0.0; //roll_rate=0.0, pitch_rate=0.0, yaw_rate=0.0; double roll=0.0, pitch=0.0, yaw=0.0; //phi, theta, psi //double integral_surge=0.0; double integral_pitch=0.0; double integral_yaw=0.0; + State(double surge=0,double pitch=0, double yaw=0):surge{surge}, pitch{pitch},yaw{yaw}{}; + State(){}; }; class Guidance_data:public State{ diff --git a/control/velocity_controller/src/LQR_setup.cpp b/control/velocity_controller/src/LQR_setup.cpp index a49ec1170..ee2acae89 100644 --- a/control/velocity_controller/src/LQR_setup.cpp +++ b/control/velocity_controller/src/LQR_setup.cpp @@ -1,6 +1,7 @@ #include "velocity_controller/LQR_setup.hpp" #include "rclcpp/rclcpp.hpp" +#include #include #include #include @@ -22,7 +23,21 @@ //Eigen::IOFormat fmt(Eigen::StreamPrecision, 0, ", ", "\n", "[", "]"); LQRController::LQRController() { - + interval_ = 0.0; + integral_error_surge = 0.0; + integral_error_pitch = 0.0; + integral_error_yaw = 0.0; + surge_windup = false; + pitch_windup = false; + yaw_windup = false; + max_force = 0.0; + mass = 0.0; + Q.setZero(); + R.setZero(); + B.setZero(); + D_low.setZero(); + D_high.setZero(); + inertia_matrix_inv.setZero(); }; bool LQRController::set_matrices(std::vector Q_,std::vector R_,std::vector inertia_matrix_,double max_force_, std::vector water_r_low,std::vector water_r_high){ //Possible error handling here to check for size and allowed values. @@ -47,8 +62,11 @@ bool LQRController::set_matrices(std::vector Q_,std::vector R_,s return 0; } max_force=max_force_; - Q.diagonal()=Eigen::Map(Q_.data(),Q_.size()); - R.diagonal()=Eigen::Map(R_.data(),R_.size()); + // Ensure full matrices are zeroed before assigning diagonals + Q.setZero(); + R.setZero(); + Q.diagonal() = Eigen::Map(Q_.data(), Q_.size()); + R.diagonal() = Eigen::Map(R_.data(), R_.size()); Eigen::Matrix inertia_matrix = Eigen::Map>(inertia_matrix_.data(),6,6); D_low=Eigen::Map>(water_r_low.data(),6,6); D_high=Eigen::Map>(water_r_high.data(),6,6); @@ -69,30 +87,6 @@ bool LQRController::set_matrices(std::vector Q_,std::vector R_,s return 1; } - -/*angle LQRController::quaternion_to_euler_angle(double w, double x, double y, double z){ - double ysqr = y * y; - - double t0 = +2.0 * (w * x + y * z); - double t1 = +1.0 - 2.0 * (x * x + ysqr); - double phi = std::atan2(t0, t1); - - double t2 = +2.0 * (w * y - z * x); - t2 = t2 > 1.0 ? 1.0 : t2; - t2 = t2 < -1.0 ? -1.0 : t2; - double theta = std::asin(t2); - - double t3 = +2.0 * (w * z + x * y); - double t4 = +1.0 - 2.0 * (ysqr + z * z); - double psi = std::atan2(t3, t4); - - return {phi, theta, psi}; -};*/ - -/*double LQRController::ssa(double angle){ - return std::fmod(angle+pi, 2*pi)-pi; -};*/ - //Can be optimized std::tuple LQRController::saturate (double value, bool windup, double limit){ if (abs(value) > limit){ @@ -114,20 +108,11 @@ double LQRController::anti_windup(double error, double integral_sum, bool windup return integral_sum; } -/*Eigen::Matrix3d LQRController::calculate_coriolis_matrix(double pitchrate, double yaw_rate, double sway_vel, double heave_vel){ - //Inertia matrix values?? - Eigen::Matrix3d result; - result<<0.2,-30*sway_vel*0.01,-30*heave_vel*0.01, - 30 * sway_vel*0.01,0,1.629 * pitchrate, - 30 * heave_vel*0.01,1.769 * yaw_rate,0; - return result; -}*/ - Eigen::Matrix LQRController::linearize(State s){ //Eigen::Matrix A; - Eigen::Matrix D; + Eigen::Matrix D=Eigen::Matrix::Zero(); if (s.surge<100){ //Threshold tbd D=-inertia_matrix_inv*D_low; @@ -176,7 +161,10 @@ Eigen::Vector LQRController::update_error(Guidance_data guidance_value integral_error_surge = anti_windup(surge_error, integral_error_surge, surge_windup); integral_error_pitch = anti_windup(pitch_error, integral_error_pitch, pitch_windup); integral_error_yaw = anti_windup(yaw_error, integral_error_yaw, yaw_windup); - + //RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"integral errors: %f, %f, %f",integral_error_surge,integral_error_pitch,integral_error_yaw); + //RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"windup status: %d, %d, %d",surge_windup,pitch_windup,yaw_windup); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"pitch value n state %f, %f",guidance_values.pitch,states.pitch); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"errors: %f, %f, %f",surge_error,pitch_error,yaw_error); Eigen::Vector state_error= {-surge_error, -pitch_error, -yaw_error, -states.pitch_rate, -states.yaw_rate, integral_error_surge, integral_error_pitch, integral_error_yaw}; return state_error; } @@ -190,6 +178,13 @@ Eigen::Vector LQRController::saturate_input(Eigen::Vector u) Eigen::Vector LQRController::calculate_thrust(State state, Guidance_data guidance_values){ ct::optcon::LQR<8,3> lqr; Eigen::Matrix K_l; + /*RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"A matrix: %f, %f, %f, %f, %f, %f, %f, %f; %f, %f, %f, %f, %f, %f, %f, %f; ...",linearize(state)(0,0),linearize(state)(0,1),linearize(state)(0,2),linearize(state)(0,3),linearize(state)(0,4),linearize(state)(0,5),linearize(state)(0,6),linearize(state)(0,7), + linearize(state)(1,0),linearize(state)(1,1),linearize(state)(1,2),linearize(state)(1,3),linearize(state)(1,4),linearize(state)(1,5),linearize(state)(1,6),linearize(state)(1,7)); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"B matrix: %f, %f, %f; %f, %f, %f; %f, %f, %f; %f, %f, %f; ...",B(0,0),B(0,1),B(0,2), + B(1,0),B(1,1),B(1,2), + B(2,0),B(2,1),B(2,2), + B(3,0),B(3,1),B(3,2)); + */ bool INFO= lqr.compute(Q,R,linearize(state),B,K_l,true,false); if(INFO==0){ return {9999,9999,9999}; //Need to fix @@ -201,6 +196,16 @@ Eigen::Vector LQRController::calculate_thrust(State state, Guidance_da */ Eigen::Matrix state_error = update_error(guidance_values, state); + /*RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"Guidance values: %f, %f, %f",guidance_values.surge,guidance_values.pitch,guidance_values.yaw); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"Current states: %f, %f, %f, %f, %f",state.surge,state.pitch,state.yaw,state.pitch_rate,state.yaw_rate); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"State error: %f, %f, %f, %f, %f, %f, %f, %f",state_error(0),state_error(1),state_error(2),state_error(3),state_error(4),state_error(5),state_error(6),state_error(7)); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"Control input: %f, %f, %f",- (K_l*state_error)(0),- (K_l*state_error)(1),- (K_l*state_error)(2)); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"saturated_input: %f, %f, %f",saturate_input(- (K_l*state_error))(0),saturate_input(- (K_l*state_error))(1),saturate_input(- (K_l*state_error))(2)); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"K matrix: %f, %f, %f, %f, %f, %f, %f, %f; %f, %f, %f, %f, %f, %f, %f, %f; %f, %f, %f, %f, %f, %f, %f, %f", + K_l(0,0),K_l(0,1),K_l(0,2),K_l(0,3),K_l(0,4),K_l(0,5),K_l(0,6),K_l(0,7), + K_l(1,0),K_l(1,1),K_l(1,2),K_l(1,3),K_l(1,4),K_l(1,5),K_l(1,6),K_l(1,7), + K_l(2,0),K_l(2,1),K_l(2,2),K_l(2,3),K_l(2,4),K_l(2,5),K_l(2,6),K_l(2,7)); + */ return saturate_input(- (K_l*state_error)); } void LQRController::reset_controller(){ diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index d2e6ce1e5..d461bd05c 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -52,7 +52,7 @@ Velocity_node::Velocity_node() : Node("velocity_controller_node"), PID_surge(100 PID_surge.set_output_limits(-max_force, max_force); PID_pitch.set_output_limits(-max_force, max_force); PID_yaw.set_output_limits(-max_force, max_force); - if(!lqr_controller.set_matrices(Q,R,inertia_matrix,max_force,dampening_matrix_low,dampening_matrix_high)||!lqr_controller.set_interval(publish_rate/1000)){ + if(!lqr_controller.set_matrices(Q,R,inertia_matrix,max_force,dampening_matrix_low,dampening_matrix_high)||!lqr_controller.set_interval(static_cast(publish_rate)/1000)){ controller_type=1; RCLCPP_INFO(this->get_logger(),"Switching to PID"); }; diff --git a/control/velocity_controller/tests/test_LQR.cpp b/control/velocity_controller/tests/test_LQR.cpp index 2e5485049..4e2a45f98 100644 --- a/control/velocity_controller/tests/test_LQR.cpp +++ b/control/velocity_controller/tests/test_LQR.cpp @@ -2,6 +2,7 @@ #include #include #include +#include "velocity_controller/utilities.hpp" #include class LQR_test : public ::testing::Test{ @@ -50,13 +51,43 @@ TEST_F(LQR_test,wrong_setup){ EXPECT_FALSE(controller.set_matrices(eight,three,thirty_six,100,thirty_six,eight)); EXPECT_FALSE(controller.set_matrices(eight,three,thirty_six,-100,thirty_six,thirty_six)); }; - +/* TEST_F(LQR_test,solve){ - -}; + State states{1,1,1,2,2,2,1,2,1}; + Guidance_data value{1,3,2}; + Eigen::Vector result=controller.calculate_thrust(states,value); + EXPECT_NEAR(result(0),0,delta); + EXPECT_NEAR(result(1),0,delta); + EXPECT_NEAR(result(2),0,delta); +};*/ TEST_F(LQR_test,Direction){ + Guidance_data value; + State state{}; + value.surge=0.2; + Eigen::Vector result=controller.calculate_thrust(state,value); + EXPECT_TRUE(result(0)>0); } +TEST_F(LQR_test,zero_input){ + State states{}; + states.surge=1; + states.yaw=0.2; + states.pitch=0.3; + Guidance_data value{1,0.3,0.2}; + value.pitch=0.3; + value.yaw=0.2; + Eigen::Vector result=controller.calculate_thrust(states,value); + EXPECT_NEAR(result(0),0,delta); + EXPECT_NEAR(result(1),0,delta); + EXPECT_NEAR(result(2),0,delta); + controller.reset_controller(); + states={0,0,0,0,0,0,0,0,0}; + value={0,0,0}; + result=controller.calculate_thrust(states, value); + EXPECT_NEAR(result(0),0,delta); + EXPECT_NEAR(result(1),0,delta); + EXPECT_NEAR(result(2),0,delta); +} int main(int argc,char** argv){ testing::InitGoogleTest(&argc,argv); diff --git a/dependencies.repos b/dependencies.repos index 6570d22ca..16eb5d116 100644 --- a/dependencies.repos +++ b/dependencies.repos @@ -5,3 +5,6 @@ repositories: vortex-vkf: type: git url: https://github.com/vortexntnu/vortex-vkf.git + control-toolbox: + type: git + url: https://github.com/ethz-adrl/control-toolbox.git From 5099e513a9aac60ac3f6d20018b71149e24e5d9d Mon Sep 17 00:00:00 2001 From: Henrik Date: Sat, 31 Jan 2026 14:43:00 +0100 Subject: [PATCH 056/128] fixed bug and crash in LQR test --- .../include/velocity_controller/utilities.hpp | 3 ++- control/velocity_controller/tests/test_LQR.cpp | 13 +++++++++---- 2 files changed, 11 insertions(+), 5 deletions(-) diff --git a/control/velocity_controller/include/velocity_controller/utilities.hpp b/control/velocity_controller/include/velocity_controller/utilities.hpp index 13574d271..8a59a6e71 100644 --- a/control/velocity_controller/include/velocity_controller/utilities.hpp +++ b/control/velocity_controller/include/velocity_controller/utilities.hpp @@ -21,7 +21,8 @@ class State{ double roll=0.0, pitch=0.0, yaw=0.0; //phi, theta, psi //double integral_surge=0.0; double integral_pitch=0.0; double integral_yaw=0.0; State(double surge=0,double pitch=0, double yaw=0):surge{surge}, pitch{pitch},yaw{yaw}{}; - State(){}; + //State(){}; + State operator=(int n){if (n){surge=0.0,sway=0.0,heave=0.0,roll_rate=0.0,pitch_rate=0.0,yaw_rate=0.0,roll=0.0,pitch=0.0,yaw=0.0;} return *this;}; }; class Guidance_data:public State{ diff --git a/control/velocity_controller/tests/test_LQR.cpp b/control/velocity_controller/tests/test_LQR.cpp index 4e2a45f98..f97f112bd 100644 --- a/control/velocity_controller/tests/test_LQR.cpp +++ b/control/velocity_controller/tests/test_LQR.cpp @@ -73,16 +73,21 @@ TEST_F(LQR_test,zero_input){ states.surge=1; states.yaw=0.2; states.pitch=0.3; - Guidance_data value{1,0.3,0.2}; - value.pitch=0.3; + Guidance_data value; + value.surge=1.0; value.yaw=0.2; + value.pitch=0.3; Eigen::Vector result=controller.calculate_thrust(states,value); EXPECT_NEAR(result(0),0,delta); EXPECT_NEAR(result(1),0,delta); EXPECT_NEAR(result(2),0,delta); controller.reset_controller(); - states={0,0,0,0,0,0,0,0,0}; - value={0,0,0}; + states.surge=0; + states.pitch=0; + states.yaw=0; + value.surge=0; + value.pitch=0; + value.yaw=0; result=controller.calculate_thrust(states, value); EXPECT_NEAR(result(0),0,delta); EXPECT_NEAR(result(1),0,delta); From 6a13ca136e74bb2c0aae3073b21c641674cb2aea Mon Sep 17 00:00:00 2001 From: Anbit Date: Wed, 4 Feb 2026 11:46:25 +0100 Subject: [PATCH 057/128] Fix surge error --- guidance/los_guidance/src/los_guidance_ros.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 1cac3aa3b..e69c9ed67 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -327,9 +327,8 @@ void LosGuidanceNode::execute( los_debug_pub_->publish(reference_msg); - double surge = std::sqrt(debug_current_odom_->twist.twist.linear.x + - debug_current_odom_->twist.twist.linear.y + - debug_current_odom_->twist.twist.linear.z); + const auto& v = debug_current_odom_->twist.twist.linear; + double surge = std::sqrt(v.x*v.x + v.y*v.y + v.z*v.z); vortex_msgs::msg::LOSGuidance state_debug_msg; Eigen::Vector3d euler = vortex::utils::math::quat_to_euler( From 5d314097f54a8750551a80b84308db19a2f3ab4f Mon Sep 17 00:00:00 2001 From: Henrik Date: Mon, 9 Feb 2026 18:42:06 +0100 Subject: [PATCH 058/128] Added NMPC, functioning. Sinus references --- control/velocity_controller/CMakeLists.txt | 12 +- .../config/parameters.yaml | 27 +- .../velocity_controller/NMPC_setup.hpp | 38 ++ .../include/velocity_controller/test_VC.hpp | 1 + .../include/velocity_controller/utilities.hpp | 3 + .../velocity_controller.hpp | 20 +- control/velocity_controller/src/LQR_setup.cpp | 98 +----- .../velocity_controller/src/NMPC_setup.cpp | 329 ++++++++++++++++++ control/velocity_controller/src/test_VC.cpp | 6 +- control/velocity_controller/src/utilities.cpp | 18 + .../src/velocity_controller.cpp | 27 +- 11 files changed, 444 insertions(+), 135 deletions(-) create mode 100644 control/velocity_controller/include/velocity_controller/NMPC_setup.hpp create mode 100644 control/velocity_controller/src/NMPC_setup.cpp diff --git a/control/velocity_controller/CMakeLists.txt b/control/velocity_controller/CMakeLists.txt index fefad79fa..9fa6b01dc 100644 --- a/control/velocity_controller/CMakeLists.txt +++ b/control/velocity_controller/CMakeLists.txt @@ -16,14 +16,14 @@ find_package(std_msgs REQUIRED) find_package(vortex_msgs REQUIRED) find_package(vortex_utils REQUIRED) find_package(Eigen3 REQUIRED) -find_package(CasADi REQUIRED) +#find_package(CasADi REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(ct_optcon REQUIRED) find_package(ct_core REQUIRED) +find_package(casadi REQUIRED) include_directories( - ${EIGEN3_INCLUDE_DIR} include ) @@ -33,6 +33,7 @@ add_executable(velocity_controller_node src/LQR_setup.cpp src/utilities.cpp src/ct_instantiations.cpp + src/NMPC_setup.cpp ) @@ -48,13 +49,12 @@ ament_target_dependencies(velocity_controller_node std_msgs vortex_msgs geometry_msgs - CasADi + # CasADi nav_msgs vortex_utils ) - -target_link_libraries(velocity_controller_node Eigen3::Eigen ct_optcon ct_core) - +#target_include_directories(velocity_controller_node PRIVATE casadi Eigen3) +target_link_libraries(velocity_controller_node Eigen3::Eigen casadi::casadi ct_optcon ct_core) install(TARGETS velocity_controller_node DESTINATION lib/${PROJECT_NAME} diff --git a/control/velocity_controller/config/parameters.yaml b/control/velocity_controller/config/parameters.yaml index 986845f4f..22dae8f43 100644 --- a/control/velocity_controller/config/parameters.yaml +++ b/control/velocity_controller/config/parameters.yaml @@ -11,27 +11,12 @@ killswitch_topic: /softwareKillSwitch #Kill Switch LQR_params: - Q: [300.0,32.84,32.84,32.84,32.84,300.0,32.84,32.84] - R: [0.002,3.1,3.10] #0.1,0.1,0.1] - #q_surge: 75.0 - #q_pitch: 175.0 - #q_yaw: 175.0 - - #r_surge: 0.3 - #r_pitch: 0.4 - #r_yaw: 0.4 - - #i_surge: 0.3 - #i_pitch: 0.4 - #i_yaw: 0.3 - - #i_weight: 0.5 - - #dt: 0.1 - + Q: [300.0,32.84,32.84,32.84,32.84,100.0,32.84,32.84] + R: [0.02,3.1,3.10] + NMPC_params: + Q: [300.0,0.01,0.01,0.01,32.84,32.84,32.84,32.84,32.84] # u,v,w,p,q,r,phi,theta,psi + R: [0.02,3.1,3.10] # u_surge, u_theta, u_psi inertia_matrix: [ 30.0, 0.0, 0.0, 0.0, 0.0, 0.6, 0.0, 30.0, 0.0, 0.0, -0.6, 0.3, 0.0, 0.0, 30.0, 0.6, 0.3, 0.0, 0.0, 0.0, 0.6, 0.68, 0.0, 0.0, 0.0, -0.6, 0.3, 0.0, 3.32, 0.0, 0.6, 0.3, 0.0, 0.0, 0.0, 3.34] - - dampening_matrix_low: [23.0,0.0,0.0,0.0,0.0,0.0, 0.0,46.0,0.0,0.0,0.0,0.0, 0.0,0.0,46.0,0.0,0.0,0.0, 0.0,0.0,0.0,46.0,0.0,0.0, 0.0,0.0,0.0,0.0,46.0,0.0, 0.0,0.0,0.0,0.0,0.0,46.0] dampening_matrix_high: [1.0,0.0,0.0,0.0,0.0,0.0, 0.0,1.0,0.0,0.0,0.0,0.0, 0.0,0.0,1.0,0.0,0.0,0.0, 0.0,0.0,0.0,1.0,0.0,0.0, 0.0,0.0,0.0,0.0,1.0,0.0, 0.0,0.0,0.0,0.0,0.0,1.0] @@ -39,5 +24,5 @@ publish_rate: 10 #ms #Clamp parameter max_force: 99.5 #should maybe be 99.5 - controller_type: 2 #1 PID 2 LQR + controller_type: 3 #1 PID 2 LQR 3 NMPC diff --git a/control/velocity_controller/include/velocity_controller/NMPC_setup.hpp b/control/velocity_controller/include/velocity_controller/NMPC_setup.hpp new file mode 100644 index 000000000..a45b633a8 --- /dev/null +++ b/control/velocity_controller/include/velocity_controller/NMPC_setup.hpp @@ -0,0 +1,38 @@ +#pragma once +//#include +#include +#include +#include "velocity_controller/utilities.hpp" + +class NMPC_controller{ + public: + Eigen::Matrix calculate_thrust(Guidance_data guidance_values, State state); + bool set_matrices(std::vector Q_,std::vector R_,std::vector inertia_matrix, double max_force,std::vector water_r_low,std::vector water_r_high); + void reset_controller(); + bool set_interval(double interval); + bool initialize_MPC(); + private: + Eigen::Matrix Q_; + Eigen::MatrixR_; + Eigen::MatrixM_inv; + Eigen::MatrixD_low; + Eigen::MatrixD_high; + //Eigen::MatrixB_; + double interval_; + double mass; + double Iz; + double Ix; + double Iy; + int N=20; + int n=9; + int m=3; + casadi::DM Z0_next; //For warm start + casadi::DM lbx; + casadi::DM ubx; + casadi::DM lbg; + casadi::DM ubg; + casadi::DM Pval; + casadi::Function solver; + + +}; \ No newline at end of file diff --git a/control/velocity_controller/include/velocity_controller/test_VC.hpp b/control/velocity_controller/include/velocity_controller/test_VC.hpp index 55ff86549..127aa4211 100644 --- a/control/velocity_controller/include/velocity_controller/test_VC.hpp +++ b/control/velocity_controller/include/velocity_controller/test_VC.hpp @@ -43,6 +43,7 @@ class test_VC : public rclcpp::Node{ //MSGS //nav_msgs::msg::Odometry odom_msg; + double time1=0; }; geometry_msgs::msg::Quaternion euler_angle_to_quaternion(double roll, double pitch, double yaw); \ No newline at end of file diff --git a/control/velocity_controller/include/velocity_controller/utilities.hpp b/control/velocity_controller/include/velocity_controller/utilities.hpp index 8a59a6e71..796701f1c 100644 --- a/control/velocity_controller/include/velocity_controller/utilities.hpp +++ b/control/velocity_controller/include/velocity_controller/utilities.hpp @@ -4,6 +4,7 @@ #include "std_msgs/msg/float64_multi_array.hpp" #include "vortex_msgs/msg/los_guidance.hpp" #include +#include class angle{ @@ -39,3 +40,5 @@ class Guidance_data:public State{ angle NED_to_BODY(const angle &a,const State &s); Eigen::Vector3d NED_to_BODY(const Eigen::Vector3d &a, const State &s); +//casadi::MX mtimes(const casadi::MX& A, const casadi::MX& B); + diff --git a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp index 1254ab493..31073d565 100644 --- a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp +++ b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp @@ -11,7 +11,7 @@ #include "LQR_setup.hpp" #include "nav_msgs/msg/odometry.hpp" #include "vortex_msgs/msg/los_guidance.hpp" - +#include "velocity_controller/NMPC_setup.hpp" class Velocity_node : public rclcpp::Node{ @@ -27,8 +27,6 @@ class Velocity_node : public rclcpp::Node{ //Callback functions void guidance_callback(const vortex_msgs::msg::LOSGuidance::SharedPtr msg_ptr); void killswitch_callback(const std_msgs::msg::Bool::SharedPtr msg_ptr); - //void twist_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg_ptr); - //void pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg_ptr); void odometry_callback(const nav_msgs::msg::Odometry::SharedPtr msg_ptr); //Publisher instance @@ -39,8 +37,6 @@ class Velocity_node : public rclcpp::Node{ rclcpp::TimerBase::SharedPtr timer_publish; //Subscriber instance - //rclcpp::Subscription::SharedPtr subscriber_twist; - //rclcpp::Subscription::SharedPtr subscriber_pose; rclcpp::Subscription::SharedPtr subscriber_Odometry; rclcpp::Subscription::SharedPtr subscriber_guidance; rclcpp::Subscription::SharedPtr subscriber_killswitch; @@ -50,12 +46,9 @@ class Velocity_node : public rclcpp::Node{ std::string topic_thrust; std::string topic_guidance; std::string topic_killswitch; - //std::string topic_twist; - //std::string topic_pose; std::string topic_odometry; //Variables for timers - //int calculation_rate; int publish_rate; double max_force; @@ -78,13 +71,16 @@ class Velocity_node : public rclcpp::Node{ //LQRparameters lqr_parameters; std::vector Q; std::vector R; - std::vector Qi; - std::vector Ri; + //std::vector Qi; + //std::vector Ri; std::vector inertia_matrix; std::vector dampening_matrix_low; std::vector dampening_matrix_high; - - + //NMPC controller + NMPC_controller NMPC; + //NMPC parameters + std::vector Q2; + std::vector R2; //Test rclcpp::Publisher::SharedPtr publisher_reference; diff --git a/control/velocity_controller/src/LQR_setup.cpp b/control/velocity_controller/src/LQR_setup.cpp index ee2acae89..180ed791c 100644 --- a/control/velocity_controller/src/LQR_setup.cpp +++ b/control/velocity_controller/src/LQR_setup.cpp @@ -16,7 +16,8 @@ #include //#include #include "vortex/utils/math.hpp" -#include "ct/optcon/lqr/LQR.hpp" +#include "ct/optcon/lqr/LQR.hpp" +#include "velocity_controller/NMPC_setup.hpp" @@ -163,8 +164,8 @@ Eigen::Vector LQRController::update_error(Guidance_data guidance_value integral_error_yaw = anti_windup(yaw_error, integral_error_yaw, yaw_windup); //RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"integral errors: %f, %f, %f",integral_error_surge,integral_error_pitch,integral_error_yaw); //RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"windup status: %d, %d, %d",surge_windup,pitch_windup,yaw_windup); - RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"pitch value n state %f, %f",guidance_values.pitch,states.pitch); - RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"errors: %f, %f, %f",surge_error,pitch_error,yaw_error); + //RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"pitch value n state %f, %f",guidance_values.pitch,states.pitch); + //RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"errors: %f, %f, %f",surge_error,pitch_error,yaw_error); Eigen::Vector state_error= {-surge_error, -pitch_error, -yaw_error, -states.pitch_rate, -states.yaw_rate, integral_error_surge, integral_error_pitch, integral_error_yaw}; return state_error; } @@ -223,97 +224,6 @@ int LQRController::set_interval(double interval){ return 1; } -extern "C" { - // Fortran subroutine for solving symplectic Schur decomposition(double precision version) -void sb02mt_( - const char* JOBG, const char* JOBL, const char* FACT, const char* UPLO, - const int* N, const int* M, double* A, const int* LDA, double* B, const int* LDB, - double* Q, const int* LDQ, double* R, const int* LDR, double* L, const int* LDL, - int* IPIV, const int* OUFACT, double* G, const int* LDG, - int* IWORK, double* DWORK, const int* LDWORK, int* INFO -); -void sb02md_( const char* DICO, const char* HINV, const char* UPLO, const char* SCAL, const char* SORT, const int* N, double* A, const int* LDA, double* G, - const int* LDG, double* Q, const int* LDQ, const double* RCOND, double* WR, double* WI, double* S, const int* LDS, double* U, const int* LDU, - int* IWORK, double* DWORK, const int* LDWORK, int* BWORK, const int* INFO - ); -} - -/* -LQRsolveResult LQRController::solve_lqr(const Eigen::MatrixXd &A,const Eigen::MatrixXd &B, const Eigen::MatrixXd &Q, const Eigen::MatrixXd &R){ - - const int N=A.rows(); - const int M=B.cols(); - if (A.cols()!=N||B.rows()!=N||R.rows()!=M||R.cols()!=M||Q.rows()!=N||Q.cols()!=N){ - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The dimensions of the matrices for solve_lqr are wrong"); - return LQRsolveResult{}; - } - - Eigen::Matrix A_copy=A, Q_copy=Q; - Eigen::Matrix B_copy=B; Eigen::Matrix R_copy=R; - - //First calculate G with sb02mt_ - //calculate G, L is zero, unfactored R, Upper triangle i think - char JOBG='G',JOBL='Z',FACT='N',UPLO='U'; - //Order of matrices A, Q, G and X(P), Order of matrix R and nuber of columns in B and L(is zero) - //Dimensions of matrices - int LDA=N, LDB=N, LDQ=N,LDR=M,LDL=N,LDG=N; - std::vector IWORK(8*N),IPIV(N); - //Upper bounds Output but initialized JIC output placeholder - int LDWORK=20*N*N,OUFACT=0,INFO=0; - std::vector DWORK(LDWORK); - Eigen::Matrix L=Eigen::Matrix::Zero(), G=L; - { - double detA = A_copy.determinant(); - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Calling sb02md_: det(A)=%.6g, A(0,0)=%.6g, G(0,0)=%.6g", detA, A_copy(0,0), G(0,0)); - Eigen::EigenSolver> es(A_copy); - for (int i=0;i<6;++i){ - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "eigA[%d]=% .6g%+.6gi", i, es.eigenvalues()[i].real(), es.eigenvalues()[i].imag()); - } - } - sb02mt_(&JOBG,&JOBL,&FACT,&UPLO,&N,&M,A_copy.data(),&LDA,B_copy.data(),&LDB,Q_copy.data(),&LDQ,R_copy.data(),&LDR,L.data(),&LDL,IPIV.data(),&OUFACT,G.data(),&LDG,IWORK.data(),DWORK.data(),&LDWORK,&INFO); - Eigen::Matrix K; - if (INFO!=0){ - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "sb02mt_ returned INFO=%d", INFO); - // Consider throwing or returning a default result. We'll return zeroed K and G for now. - Eigen::Matrix K_zero = Eigen::Matrix::Zero(); - return LQRsolveResult(K_zero, G,INFO); - - } - char DICO='D',HINV='D',SCAL='N',SORT='U'; - std::vector WR(2*N,0),WI(2*N,0),RCOND(2*N,0); - int BWORK[8*N]; - Eigen::Matrix S=Eigen::Matrix::Zero(); - Eigen::MatrixU=Eigen::Matrix::Zero(); - int LDS=2*N,LDU=2*N,INFO1=0; - //A_copy=A;Q_copy=Q; R_copy=R; - { - double detA = A_copy.determinant(); - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Calling sb02md_: det(A)=%.6g, A(0,0)=%.6g, G(0,0)=%.6g", detA, A_copy(0,0), G(0,0)); - Eigen::EigenSolver> es(A_copy); - for (int i=0;i<6;++i){ - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "eigA[%d]=% .6g%+.6gi", i, es.eigenvalues()[i].real(), es.eigenvalues()[i].imag()); - } - } - sb02md_(&DICO,&HINV,&UPLO,&SCAL,&SORT,&N,A_copy.data(),&LDA,G.data(),&LDG,Q_copy.data(),&LDQ,RCOND.data(),WR.data(),WI.data(),S.data(),&LDS,U.data(),&LDU,IWORK.data(),DWORK.data(),&LDWORK,BWORK,&INFO1); - if (INFO1!=0){ - //Some Error handling here. Also check that BRB in invertible - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "sb02md_ returned INFO=%d", INFO1); - // Consider throwing or returning a default result. We'll return zeroed K and G for now. - Eigen::Matrix K_zero = Eigen::Matrix::Zero(); - return LQRsolveResult(K_zero, G,INFO1); - } - Eigen::MatrixU11=U.topRows(6); - Eigen::MatrixU21=U.bottomRows(6); - Eigen::MatrixXd X=U21*U11.inverse(); - K=R.inverse()*B.transpose()*X; - - return LQRsolveResult(K,G,INFO1); - -} - */ - - - //Hjelpefunksjoner for å konvertere mellom std::vector og Eigen::Matrix3d Eigen::Matrix3d vector_to_matrix3d(const std::vector &other_matrix){ diff --git a/control/velocity_controller/src/NMPC_setup.cpp b/control/velocity_controller/src/NMPC_setup.cpp new file mode 100644 index 000000000..6bc41c704 --- /dev/null +++ b/control/velocity_controller/src/NMPC_setup.cpp @@ -0,0 +1,329 @@ +#include +#include +#include +#include +#include +#include +#include +#include "rclcpp/rclcpp.hpp" +#include "velocity_controller/utilities.hpp" +#include "velocity_controller/NMPC_setup.hpp" + + +bool NMPC_controller::set_matrices(std::vector Q,std::vector R,std::vector inertia_matrix, double max_force,std::vector water_r_low,std::vector water_r_high){ + if (Q.size()!=9){ + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The Q matrix has the wrong amount of elements"); + return 0; + } + if(R.size()!=3){ + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The R matrix has the wrong amount of elements"); + return 0; + } + if(inertia_matrix.size()!=36){ + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The M matrix has the wrong amount of elements"); + return 0; + } + if(water_r_low.size()!=36||water_r_high.size()!=36){ + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The D matrix has the wrong amount of elements"); + return 0; + } + if (max_force<0){ + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The max_force need to be >0"); + return 0; + } + if (inertia_matrix[0]<0){ + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"Negative mass?"); + return 0; + } + Q_.setZero(); + R_.setZero(); + Q_.diagonal() = Eigen::Map(Q.data(), Q.size()); + R_.diagonal() = Eigen::Map(R.data(), R.size()); + Eigen::Matrix inertia_matrix_ = Eigen::Map>(inertia_matrix.data(),6,6); + D_low=Eigen::Map>(water_r_low.data(),6,6); + D_high=Eigen::Map>(water_r_high.data(),6,6); + M_inv=inertia_matrix_.inverse(); + mass=inertia_matrix[0]; + Ix=inertia_matrix_(3,3); + Iy=inertia_matrix_(4,4); + Iz=inertia_matrix_(5,5); + + //B_=M_inv*(Eigen::Matrix() << 1,0,0,0,1,0,0,0,1,0,0,0,0,0,0,0,0,0).finished(); + + return true; +}; +void NMPC_controller::reset_controller(){ + return; +} +bool NMPC_controller::set_interval(double interval){ + interval_=interval; + return false; +} +bool NMPC_controller::initialize_MPC(){ + using SYM=casadi::MX; + SYM X=SYM::sym("X",n,1); //u,v,w,p,q,r,phi,theta,psi + SYM A=SYM::zeros(n,n); + casadi::DM M_i=casadi::DM::zeros(6,6); + SYM U=SYM::sym("U",3,1); + casadi::DM Q=casadi::DM::zeros(n,n); + casadi::DM R=casadi::DM::zeros(m,m); + /*U(0,0)=SYM::sym("u_surge"); + U(1,0)=SYM::sym("u_pitch"); + U(2,0)=SYM::sym("u_yaw");*/ + + //Creating M_i matrix + for (int i=0;i X_v(N+1), U_v(N); + for (int i=0;i<=N;i++) X_v[i] = SYM::sym("X_"+std::to_string(i),n); + for (int i=0;i z_parts; + z_parts.insert(z_parts.end(), X_v.begin(), X_v.end()); + z_parts.insert(z_parts.end(), U_v.begin(), U_v.end()); + SYM Z = vertcat(z_parts); + + //Initial state + SYM x0=SYM::sym("x0",n); + SYM xr=SYM::sym("xr",n); + SYM ur=SYM::sym("ur",m); + + SYM P=SYM::vertcat({x0,xr,ur}); + + + auto p_x0 = P(casadi::Slice(0, n)); // x0 + auto p_xr = P(casadi::Slice(n, 2*n)); // xr + auto p_ur = P(casadi::Slice(2*n, 2*n + m)); // ur + + //Dynamic constraints + + std::vector g_list; + g_list.push_back(X_v[0]-p_x0); + for (int i=0; i lbx_parts, ubx_parts; + // X0..XN + for (int k = 0; k <= N; ++k) { + lbx_parts.push_back(x_min); + ubx_parts.push_back(x_max); + } + // U0..U_{N-1} + for (int k = 0; k < N; ++k) { + lbx_parts.push_back(u_min); + ubx_parts.push_back(u_max); + } + lbx = vertcat(lbx_parts); + ubx = vertcat(ubx_parts); + + // Equality constraints: G == 0 + lbg = casadi::DM::zeros(n*(N+1)); + ubg = casadi::DM::zeros(n*(N+1)); + + + //building NLP + casadi::MXDict nlp; + nlp["x"]=Z; + nlp["f"]=J; + nlp["g"]=G; + nlp["p"]=P; + + solver=casadi::nlpsol("solver","ipopt",nlp); + + // -------------------------------------------------------- + // Prepare parameter vector Pval and initial guess Z0 + // -------------------------------------------------------- + // Example numeric values: + casadi::DM x0_val = casadi::DM::zeros(n,1); + std::vector Pval_parts; + Pval_parts.push_back(x0_val); + Pval_parts.push_back(casadi::DM::zeros(n,1)); + Pval_parts.push_back(casadi::DM::zeros(m,1)); + Pval = vertcat(Pval_parts); + + // Initial guess: Z0 (X guesses then U guesses) + std::vector Z0_parts; + for (int k = 0; k <= N; ++k) Z0_parts.push_back(x0_val); // start with x0 everywhere + for (int k = 0; k < N; ++k) Z0_parts.push_back(casadi::DM::zeros(m,1)); + Z0_next = vertcat(Z0_parts); + + + return true; +} + +Eigen::Matrix NMPC_controller::calculate_thrust(Guidance_data guidance_values, State state){ + + casadi::DM x0_val={state.surge,state.sway,state.heave,state.roll_rate,state.pitch_rate,state.yaw_rate,state.roll,state.pitch,state.yaw}; + casadi::DM xr_val={guidance_values.surge,guidance_values.sway,guidance_values.heave,guidance_values.roll_rate,guidance_values.pitch_rate,guidance_values.yaw_rate,guidance_values.roll,guidance_values.pitch,guidance_values.yaw}; + casadi::DM ur_val=casadi::DM::zeros(m); + Pval=casadi::DM::vertcat({x0_val,xr_val,ur_val}); + // Solve + + casadi::DMDict solver_in; + solver_in["x0"] = Z0_next; + solver_in["lbx"] = lbx; + solver_in["ubx"] = ubx; + solver_in["lbg"] = lbg; + solver_in["ubg"] = ubg; + solver_in["p"] = Pval; + auto sol = solver(solver_in); + if (sol.count("x") == 0) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "NLP solver failed"); + return {9999,9999,9999}; //TODO: check for 9999,9999,9999 + } + casadi::DM Zstar = sol.at("x"); // optimal stacked decision vector + //TODO: check to see NAN or INF values in solution + + // index of U0 start: + int offset_u0 = n*(N+1); + // Extract u0* (first control block after all states) + // Z = [X0; X1; ...; XN; U0; U1; ...; U_{N-1}] + casadi::DM u0_star = Zstar(casadi::Slice(offset_u0, offset_u0 + m)); + + std::cout << "u0* = " << u0_star << std::endl; + + + // Warm-start shift for next iteration + // Build new Z0_next = [X1*; X2*; ...; XN*; XN*; U1*; ...; U_{N-1}*; U_{N-1}*] + // (X0 will be re-anchored by the new measured x0 in constraints) + std::vector Xstar(N+1), Ustar(N); + // Unstack from Zstar: + for (int k = 0; k <= N; ++k) { + int i0 = k*n; + Xstar[k] = Zstar(casadi::Slice(i0, i0+n)); + } + for (int k = 0; k < N; ++k) { + int i0 = n*(N+1) + k*m; + Ustar[k] = Zstar(casadi::Slice(i0, i0+m)); + } + + std::vector Z0_next_parts; + // shifted states: X1..XN, repeat XN at end + for (int k = 1; k <= N; ++k) Z0_next_parts.push_back(Xstar[k]); + Z0_next_parts.push_back(Xstar[N]); + // shifted inputs: U1..U_{N-1}, repeat last + for (int k = 1; k < N; ++k) Z0_next_parts.push_back(Ustar[k]); + Z0_next_parts.push_back(Ustar[N-1]); + + Z0_next = vertcat(Z0_next_parts); + + Eigen::Matrix result; + result(0) = double(u0_star(0)); + result(1) = double(u0_star(1)); + result(2) = double(u0_star(2)); + return result; +} diff --git a/control/velocity_controller/src/test_VC.cpp b/control/velocity_controller/src/test_VC.cpp index fe5ac3f37..5b397b885 100644 --- a/control/velocity_controller/src/test_VC.cpp +++ b/control/velocity_controller/src/test_VC.cpp @@ -1,3 +1,4 @@ +#include #include #include #include @@ -32,11 +33,14 @@ test_VC::test_VC() : Node("test_VC_node") clock_ = this->get_clock(); RCLCPP_INFO(this->get_logger(), "Test_VC node has been started"); reference_msg.surge=0.2;reference_msg.pitch=-1.22;reference_msg.yaw=0.0; //Surge, pitch, yaw - + } void test_VC::send_guidance() { + time1+=0.2; + reference_msg.yaw=0.6*sin(time1*std::numbers::pi/9); + reference_msg.pitch=0.3*sin(time1*std::numbers::pi/9); publisher_guidance->publish(reference_msg); } diff --git a/control/velocity_controller/src/utilities.cpp b/control/velocity_controller/src/utilities.cpp index 29faf687a..b51cf687e 100644 --- a/control/velocity_controller/src/utilities.cpp +++ b/control/velocity_controller/src/utilities.cpp @@ -1,5 +1,7 @@ #include "velocity_controller/utilities.hpp" #include "Eigen/Dense" +#include +#include angle quaternion_to_euler_angle(double w, double x, double y, double z){ double ysqr = y * y; @@ -59,3 +61,19 @@ Eigen::Vector3d NED_to_BODY(const Eigen::Vector3d &a, const State &s){ return v_body; } +/* +casadi::MX mtimes(const casadi::MX& A, const casadi::MX& B){ + if (A.size2()!=B.size1()){ + throw std::invalid_argument("Wrong dimensions size. A has %f columns and B has %f rows"); + } + casadi::MX result=casadi::MX::zeros(A.size1(),B.size2()); + for (int i=0;iget_logger(), "Velocity control node has been started."); @@ -56,6 +56,10 @@ Velocity_node::Velocity_node() : Node("velocity_controller_node"), PID_surge(100 controller_type=1; RCLCPP_INFO(this->get_logger(),"Switching to PID"); }; + //NMPC controller + NMPC.set_matrices(Q2,R2, inertia_matrix, max_force, dampening_matrix_low, dampening_matrix_high); + NMPC.set_interval(publish_rate/1000.0); + NMPC.initialize_MPC(); } @@ -108,6 +112,21 @@ void Velocity_node::calc_thrust() } break; } + case 3:{ + Eigen::Matrix u; + u=NMPC.calculate_thrust(guidance_values, current_state); + if (u==Eigen::Matrix{9999,9999,9999}){ + controller_type=1; + RCLCPP_ERROR(this->get_logger(),"Switching to PID"); + } + else{ + thrust_out.wrench.force.x=u[0]; + thrust_out.wrench.torque.x=u[1]; + thrust_out.wrench.torque.x=u[2]; + } + + break; + } default:{ //Some crash handling here RCLCPP_ERROR(this->get_logger(),"Unknown controller set"); @@ -192,6 +211,12 @@ void Velocity_node::get_new_parameters(){ this->dampening_matrix_low=this->get_parameter("dampening_matrix_low").as_double_array(); this->dampening_matrix_high=this->get_parameter("dampening_matrix_high").as_double_array(); + //NMPC Parameters + this->declare_parameter>("NMPC_params.Q"); + this->declare_parameter>("NMPC_params.R"); + Q2=this->get_parameter("NMPC_params.Q").as_double_array(); + R2=this->get_parameter("NMPC_params.R").as_double_array(); + From 2bd2c4ea89115e623cc1e8212afe176f5fd01149 Mon Sep 17 00:00:00 2001 From: Anbit Date: Wed, 11 Feb 2026 15:24:54 +0100 Subject: [PATCH 059/128] Add stoping msg publishes when goal reached --- guidance/los_guidance/config/guidance_params.yaml | 4 ++-- guidance/los_guidance/launch/guidance_test.launch.py | 8 ++++++++ guidance/los_guidance/src/los_guidance_ros.cpp | 11 ++++++++--- 3 files changed, 18 insertions(+), 5 deletions(-) diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 496980fa4..8e17c67e4 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -21,9 +21,9 @@ vector_field_los: k_p_v: 0.5 common: - active_los_method: 3 # 0: Proportional, 1: Integral, 2: Adaptive, 3: VFLos + active_los_method: 2 # 0: Proportional, 1: Integral, 2: Adaptive, 3: VFLos u_desired: 0.3 - goal_reached_tol: 1.0 + goal_reached_tol: 0.5 debug: enable_debug: true diff --git a/guidance/los_guidance/launch/guidance_test.launch.py b/guidance/los_guidance/launch/guidance_test.launch.py index 56f3f5b47..d49971b00 100644 --- a/guidance/los_guidance/launch/guidance_test.launch.py +++ b/guidance/los_guidance/launch/guidance_test.launch.py @@ -6,6 +6,7 @@ def generate_launch_description(): stonefish_dir = get_package_share_directory('stonefish_sim') + vortex_sim_interface_dir = get_package_share_directory('vortex_sim_interface') los_guidance_dir = get_package_share_directory('los_guidance') velocity_controller_dir = get_package_share_directory('velocity_controller_lqr') @@ -19,6 +20,12 @@ def generate_launch_description(): }.items(), ) + vortex_sim_interface = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(vortex_sim_interface_dir, 'launch', 'vortex_sim_interface.launch.py') + ) + ) + los_guidance_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(los_guidance_dir, 'launch', 'los_guidance.launch.py') @@ -62,6 +69,7 @@ def generate_launch_description(): return LaunchDescription([ stonefish_sim, + vortex_sim_interface, los_guidance_launch, velocity_controller_launch, orca_sim, diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index e69c9ed67..ff5a1b5ce 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -347,9 +347,14 @@ void LosGuidanceNode::execute( reference_pub_->publish(reference_msg); - if ((path_inputs_.current_position - path_inputs_.next_point) - .as_vector() - .norm() < goal_reached_tol_) { + if ((path_inputs_.current_position - path_inputs_.next_point).as_vector().norm() < goal_reached_tol_) { + + auto stop_ref = reference_msg; + stop_ref.surge = 0.0; + stop_ref.pitch = 0.0; + stop_ref.yaw = reference_msg.yaw; + + reference_pub_->publish(stop_ref); result->success = true; goal_handle->succeed(result); spdlog::info("Goal reached"); From 52c919f68a58a4086f8e75f037f8f6a3abf103c7 Mon Sep 17 00:00:00 2001 From: Anbit Date: Mon, 16 Feb 2026 15:25:39 +0100 Subject: [PATCH 060/128] Add a square test script --- .../los_guidance/config/guidance_params.yaml | 6 +- .../launch/guidance_test.launch.py | 21 +++- guidance/los_guidance/scripts/square_test.py | 100 ++++++++++++++++++ .../los_guidance/src/lib/adaptive_los.cpp | 1 + 4 files changed, 120 insertions(+), 8 deletions(-) create mode 100644 guidance/los_guidance/scripts/square_test.py diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 8e17c67e4..1d11850d0 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -15,15 +15,15 @@ integer_los: k_i_v: 0.1 vector_field_los: - max_approach_angle_h: 1.0 # rad - max_approach_angle_v: 0.6 # rad + max_approach_angle_h: 1.0 + max_approach_angle_v: 0.6 k_p_h: 0.5 k_p_v: 0.5 common: active_los_method: 2 # 0: Proportional, 1: Integral, 2: Adaptive, 3: VFLos u_desired: 0.3 - goal_reached_tol: 0.5 + goal_reached_tol: 1.0 debug: enable_debug: true diff --git a/guidance/los_guidance/launch/guidance_test.launch.py b/guidance/los_guidance/launch/guidance_test.launch.py index d49971b00..5ee3886ad 100644 --- a/guidance/los_guidance/launch/guidance_test.launch.py +++ b/guidance/los_guidance/launch/guidance_test.launch.py @@ -1,3 +1,4 @@ +from yaml import Node from launch import LaunchDescription from launch.actions import IncludeLaunchDescription, TimerAction, ExecuteProcess, SetEnvironmentVariable from launch.launch_description_sources import PythonLaunchDescriptionSource @@ -55,17 +56,26 @@ def generate_launch_description(): ExecuteProcess( cmd=[ "bash", "-lc", - "for i in {1..5}; do " - " ros2 topic pub --once /orca/killswitch std_msgs/msg/Bool \"{data: false}\"; " - " ros2 topic pub --once /orca/operation_mode std_msgs/msg/String \"{data: 'autonomous mode'}\"; " - " sleep 1; " - "done" + "ros2 topic pub --once /orca/killswitch std_msgs/msg/Bool \"{data: false}\"; " + "ros2 topic pub --once /orca/operation_mode std_msgs/msg/String \"{data: 'autonomous mode'}\"; " ], output="screen", ), ], ) + square_test = TimerAction( + period=14.0, + actions=[ + ExecuteProcess( + cmd=[ + "bash", "-lc", + f"python3 {os.path.join(los_guidance_dir, 'scripts', 'square_test.py')}" + ], + output="screen", + ) + ], + ) return LaunchDescription([ stonefish_sim, @@ -74,4 +84,5 @@ def generate_launch_description(): velocity_controller_launch, orca_sim, set_autonomy, + square_test, ]) \ No newline at end of file diff --git a/guidance/los_guidance/scripts/square_test.py b/guidance/los_guidance/scripts/square_test.py new file mode 100644 index 000000000..c5bd078b0 --- /dev/null +++ b/guidance/los_guidance/scripts/square_test.py @@ -0,0 +1,100 @@ +import rclpy +from rclpy.node import Node +from rclpy.action import ActionClient + +from vortex_msgs.action import LOSGuidance +from geometry_msgs.msg import Point +from std_msgs.msg import Header + + +class SquareTest(Node): + + def __init__(self): + super().__init__('square_test_client') + + self._action_client = ActionClient( + self, + LOSGuidance, + '/orca/los_guidance' + ) + + self.depth = 2.5 + self.size = 10.0 + + self.waypoints = [ + (self.size, 0.0, self.depth), + (self.size, self.size, self.depth), + (0.0, self.size, self.depth), + (0.0, 0.0, self.depth), + ] + + self.current_index = 0 + + self.send_next_goal() + + def send_next_goal(self): + + if self.current_index >= len(self.waypoints): + self.get_logger().info("Square test completed!") + rclpy.shutdown() + return + + self._action_client.wait_for_server() + + goal_msg = LOSGuidance.Goal() + + header = Header() + header.frame_id = "world_ned" + + goal_msg.goal.header = header + + x, y, z = self.waypoints[self.current_index] + + goal_msg.goal.point.x = x + goal_msg.goal.point.y = y + goal_msg.goal.point.z = z + + self.get_logger().info( + f"Sending waypoint {self.current_index + 1}: " + f"x={x}, y={y}, z={z}" + ) + + self._send_goal_future = self._action_client.send_goal_async( + goal_msg, + feedback_callback=self.feedback_callback + ) + + self._send_goal_future.add_done_callback(self.goal_response_callback) + + def goal_response_callback(self, future): + + goal_handle = future.result() + + if not goal_handle.accepted: + self.get_logger().info('Goal rejected') + return + + self.get_logger().info('Goal accepted') + + self._get_result_future = goal_handle.get_result_async() + self._get_result_future.add_done_callback(self.result_callback) + + def result_callback(self, future): + + self.get_logger().info("Waypoint reached") + + self.current_index += 1 + self.send_next_goal() + + def feedback_callback(self, feedback_msg): + pass + + +def main(args=None): + rclpy.init(args=args) + node = SquareTest() + rclpy.spin(node) + + +if __name__ == '__main__': + main() diff --git a/guidance/los_guidance/src/lib/adaptive_los.cpp b/guidance/los_guidance/src/lib/adaptive_los.cpp index 7a1e0d265..3303589f8 100644 --- a/guidance/los_guidance/src/lib/adaptive_los.cpp +++ b/guidance/los_guidance/src/lib/adaptive_los.cpp @@ -58,6 +58,7 @@ types::Outputs AdaptiveLOSGuidance::calculate_outputs( const double theta_d = pi_v_ + alpha_c_hat_ + std::atan(cross_track_error.z_e / params_.lookahead_distance_v); + return types::Outputs{psi_d, theta_d}; } From 11e8d60dcb53b8999a8bc22f21eb89d06777ea0e Mon Sep 17 00:00:00 2001 From: Anbit Date: Mon, 16 Feb 2026 15:59:55 +0100 Subject: [PATCH 061/128] Fix Cmake problome for squaretest --- guidance/los_guidance/CMakeLists.txt | 6 ++++++ guidance/los_guidance/launch/guidance_test.launch.py | 2 +- guidance/los_guidance/scripts/square_test.py | 3 +++ 3 files changed, 10 insertions(+), 1 deletion(-) diff --git a/guidance/los_guidance/CMakeLists.txt b/guidance/los_guidance/CMakeLists.txt index 1dbdbe0d9..a943a4f24 100644 --- a/guidance/los_guidance/CMakeLists.txt +++ b/guidance/los_guidance/CMakeLists.txt @@ -86,8 +86,14 @@ install(DIRECTORY DESTINATION share/${PROJECT_NAME}/ ) +install(PROGRAMS + scripts/square_test.py + DESTINATION share/${PROJECT_NAME}/scripts +) + if(BUILD_TESTING) add_subdirectory(test) endif() ament_package() + diff --git a/guidance/los_guidance/launch/guidance_test.launch.py b/guidance/los_guidance/launch/guidance_test.launch.py index 5ee3886ad..862e39b50 100644 --- a/guidance/los_guidance/launch/guidance_test.launch.py +++ b/guidance/los_guidance/launch/guidance_test.launch.py @@ -65,7 +65,7 @@ def generate_launch_description(): ) square_test = TimerAction( - period=14.0, + period=20.0, actions=[ ExecuteProcess( cmd=[ diff --git a/guidance/los_guidance/scripts/square_test.py b/guidance/los_guidance/scripts/square_test.py index c5bd078b0..8fe518e9d 100644 --- a/guidance/los_guidance/scripts/square_test.py +++ b/guidance/los_guidance/scripts/square_test.py @@ -5,6 +5,7 @@ from vortex_msgs.action import LOSGuidance from geometry_msgs.msg import Point from std_msgs.msg import Header +from launch.actions import LogInfo class SquareTest(Node): @@ -12,6 +13,8 @@ class SquareTest(Node): def __init__(self): super().__init__('square_test_client') + self.get_logger().info("Square test started") + self._action_client = ActionClient( self, LOSGuidance, From 30568ab65cd8711f1f6d6328db4b99bd666cf926 Mon Sep 17 00:00:00 2001 From: Anbit Date: Mon, 16 Feb 2026 19:32:36 +0100 Subject: [PATCH 062/128] Fix adaptive param lag --- guidance/los_guidance/config/guidance_params.yaml | 2 +- .../include/los_guidance/lib/adaptive_los.hpp | 1 + guidance/los_guidance/src/lib/adaptive_los.cpp | 8 +++++++- 3 files changed, 9 insertions(+), 2 deletions(-) diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 1d11850d0..7a87ae308 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -23,7 +23,7 @@ vector_field_los: common: active_los_method: 2 # 0: Proportional, 1: Integral, 2: Adaptive, 3: VFLos u_desired: 0.3 - goal_reached_tol: 1.0 + goal_reached_tol: 0.5 debug: enable_debug: true diff --git a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp index f874db35f..5e4956061 100644 --- a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp @@ -34,6 +34,7 @@ class AdaptiveLOSGuidance { const types::Inputs& inputs); void update_adaptive_estimates( const types::CrossTrackError& cross_track_error); + void reset_adaptive_params(); AdaptiveLosParams params_{}; Eigen::Matrix3d rotation_y_ = Eigen::Matrix3d::Zero(); diff --git a/guidance/los_guidance/src/lib/adaptive_los.cpp b/guidance/los_guidance/src/lib/adaptive_los.cpp index 3303589f8..367ccb431 100644 --- a/guidance/los_guidance/src/lib/adaptive_los.cpp +++ b/guidance/los_guidance/src/lib/adaptive_los.cpp @@ -28,7 +28,7 @@ const types::CrossTrackError AdaptiveLOSGuidance::calculate_crosstrack_error( return types::CrossTrackError::from_vector(cross_track_error); } - + void AdaptiveLOSGuidance::update_adaptive_estimates( const types::CrossTrackError& cross_track_error) { const double denom_h = std::sqrt(params_.lookahead_distance_h * @@ -47,10 +47,16 @@ void AdaptiveLOSGuidance::update_adaptive_estimates( alpha_c_hat_ += alpha_dot * params_.time_step; } +void AdaptiveLOSGuidance::reset_adaptive_params(){ + beta_c_hat_= 0; + alpha_c_hat_ = 0; +} + types::Outputs AdaptiveLOSGuidance::calculate_outputs( const types::Inputs& inputs) { update_angles(inputs); const types::CrossTrackError cross_track_error = calculate_crosstrack_error(inputs); + void reset_adaptive_params(); update_adaptive_estimates(cross_track_error); const double psi_d = From 73b460c25e3ab6f5493a37c3003be0b555b117d0 Mon Sep 17 00:00:00 2001 From: Anbit Date: Sun, 22 Feb 2026 18:32:38 +0100 Subject: [PATCH 063/128] Add tuning parameters --- .../velocity_controller_lqr/__init__.py | 0 .../los_guidance/config/guidance_params.yaml | 32 ++++++++--------- .../include/los_guidance/lib/adaptive_los.hpp | 5 ++- .../include/los_guidance/los_guidance_ros.hpp | 2 ++ .../los_guidance/src/lib/adaptive_los.cpp | 6 ---- .../los_guidance/src/lib/proportional_los.cpp | 9 ----- .../los_guidance/src/los_guidance_ros.cpp | 34 +++++++++++++++---- 7 files changed, 47 insertions(+), 41 deletions(-) delete mode 100644 control/velocity_controller_lqr/velocity_controller_lqr/__init__.py diff --git a/control/velocity_controller_lqr/velocity_controller_lqr/__init__.py b/control/velocity_controller_lqr/velocity_controller_lqr/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 7a87ae308..66bd0c2e3 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -1,29 +1,29 @@ adaptive_los: - lookahead_distance_h: 1.5 - lookahead_distance_v: 0.6 - gamma_h: 0.05 - gamma_v: 0.1 + lookahead_distance_h: 0.9 #Done + lookahead_distance_v: 1.4 #Done + gamma_h: 0.03 #Done + gamma_v: 0.02 #Done prop_los: - lookahead_distance_h: 1.5 - lookahead_distance_v: 0.6 + lookahead_distance_h: 0.74 #Done + lookahead_distance_v: 0.8 #Done integer_los: - k_p_h: 0.5 - k_p_v: 0.5 - k_i_h: 0.1 - k_i_v: 0.1 + k_p_h: 0.5 #Done + k_p_v: 0.5 #Done + k_i_h: 0.1 #Done + k_i_v: 0.1 #Done vector_field_los: max_approach_angle_h: 1.0 - max_approach_angle_v: 0.6 - k_p_h: 0.5 - k_p_v: 0.5 + max_approach_angle_v: 0.6 + k_p_h: 1.5 + k_p_v: 0.3 common: - active_los_method: 2 # 0: Proportional, 1: Integral, 2: Adaptive, 3: VFLos - u_desired: 0.3 - goal_reached_tol: 0.5 + active_los_method: 3 # 0: Proportional, 1: Integral, 2: Adaptive, 3: VFLos + u_desired: 0.3 # Done + goal_reached_tol: 0.35 # Done debug: enable_debug: true diff --git a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp index 5e4956061..cca8b3553 100644 --- a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp @@ -34,15 +34,14 @@ class AdaptiveLOSGuidance { const types::Inputs& inputs); void update_adaptive_estimates( const types::CrossTrackError& cross_track_error); - void reset_adaptive_params(); AdaptiveLosParams params_{}; Eigen::Matrix3d rotation_y_ = Eigen::Matrix3d::Zero(); Eigen::Matrix3d rotation_z_ = Eigen::Matrix3d::Zero(); double pi_h_{}; double pi_v_{}; - double beta_c_hat_{}; - double alpha_c_hat_{}; + double beta_c_hat_ = 0.0; + double alpha_c_hat_ = 0.0; }; // namespace vortex::guidance::los diff --git a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp index 202e38573..a3ba2f764 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -87,6 +87,8 @@ class LosGuidanceNode : public rclcpp::Node { // @brief Execute the goal // @param goal_handle The goal handle + + bool has_active_segment_{false}; void execute(const std::shared_ptr> goal_handle); diff --git a/guidance/los_guidance/src/lib/adaptive_los.cpp b/guidance/los_guidance/src/lib/adaptive_los.cpp index 367ccb431..52607bb4e 100644 --- a/guidance/los_guidance/src/lib/adaptive_los.cpp +++ b/guidance/los_guidance/src/lib/adaptive_los.cpp @@ -47,16 +47,10 @@ void AdaptiveLOSGuidance::update_adaptive_estimates( alpha_c_hat_ += alpha_dot * params_.time_step; } -void AdaptiveLOSGuidance::reset_adaptive_params(){ - beta_c_hat_= 0; - alpha_c_hat_ = 0; -} - types::Outputs AdaptiveLOSGuidance::calculate_outputs( const types::Inputs& inputs) { update_angles(inputs); const types::CrossTrackError cross_track_error = calculate_crosstrack_error(inputs); - void reset_adaptive_params(); update_adaptive_estimates(cross_track_error); const double psi_d = diff --git a/guidance/los_guidance/src/lib/proportional_los.cpp b/guidance/los_guidance/src/lib/proportional_los.cpp index 522de51e9..ae7aeff4e 100644 --- a/guidance/los_guidance/src/lib/proportional_los.cpp +++ b/guidance/los_guidance/src/lib/proportional_los.cpp @@ -4,15 +4,6 @@ namespace vortex::guidance::los { ProportionalLOSGuidance::ProportionalLOSGuidance( const ProportionalLosParams& params) : m_params{params} { - - if (m_params.lookahead_distance_h <= 0.0) { - m_params.lookahead_distance_h = 1e-9; - } - - if (m_params.lookahead_distance_v <= 0.0) { - m_params.lookahead_distance_v = 1e-9; - } - } void ProportionalLOSGuidance::update_angles(const types::Inputs& inputs) { diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index ff5a1b5ce..45ecd9dde 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -175,13 +175,25 @@ void LosGuidanceNode::set_vector_field_guidance(YAML::Node config) { } void LosGuidanceNode::waypoint_callback( - const geometry_msgs::msg::PointStamped::SharedPtr los_waypoint) { - path_inputs_.prev_point = path_inputs_.current_position; - path_inputs_.next_point = types::Point::point_from_ros(los_waypoint->point); - spdlog::info( - "Received waypoint"); // remember to print waypoint that you get + const geometry_msgs::msg::PointStamped::SharedPtr wp_msg) +{ + const auto new_wp = types::Point::point_from_ros(wp_msg->point); + + if (!has_active_segment_) { + path_inputs_.prev_point = path_inputs_.current_position; + path_inputs_.next_point = new_wp; + has_active_segment_ = true; + } else { + path_inputs_.prev_point = path_inputs_.next_point; + path_inputs_.next_point = new_wp; + } + + spdlog::info("Received waypoint: ({}, {}, {})", + new_wp.x, new_wp.y, new_wp.z); } + + void LosGuidanceNode::pose_callback( const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr current_pose) { @@ -273,8 +285,16 @@ void LosGuidanceNode::execute( const geometry_msgs::msg::PointStamped los_waypoint = goal_handle->get_goal()->goal; - path_inputs_.prev_point = path_inputs_.current_position; - path_inputs_.next_point = types::Point::point_from_ros(los_waypoint.point); + const auto new_wp = types::Point::point_from_ros(los_waypoint.point); + + if (!has_active_segment_) { + path_inputs_.prev_point = path_inputs_.current_position; + path_inputs_.next_point = new_wp; + has_active_segment_ = true; + } else { + path_inputs_.prev_point = path_inputs_.next_point; + path_inputs_.next_point = new_wp; + } auto feedback = std::make_shared(); From 29a57097bb383bffa7adee26b305359a6171cb1c Mon Sep 17 00:00:00 2001 From: Anbit Date: Sun, 22 Feb 2026 18:39:16 +0100 Subject: [PATCH 064/128] merge main updates into this branch --- auv_setup/config/robots/orca.yaml | 2 -- 1 file changed, 2 deletions(-) diff --git a/auv_setup/config/robots/orca.yaml b/auv_setup/config/robots/orca.yaml index 49fd3f273..77c827647 100644 --- a/auv_setup/config/robots/orca.yaml +++ b/auv_setup/config/robots/orca.yaml @@ -142,8 +142,6 @@ set_killswitch: "set_killswitch" toggle_killswitch: "toggle_killswitch" get_operation_mode: "get_operation_mode" - - services: # Maybe not the right place for this? los_mode: "set_los_mode" fsm: From 76f803e2924495404cc29cacf669dedb23064ba8 Mon Sep 17 00:00:00 2001 From: Anbit Date: Tue, 24 Feb 2026 15:30:30 +0100 Subject: [PATCH 065/128] Add tuned parameters --- guidance/los_guidance/config/guidance_params.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 66bd0c2e3..62164a9a1 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -21,7 +21,7 @@ vector_field_los: k_p_v: 0.3 common: - active_los_method: 3 # 0: Proportional, 1: Integral, 2: Adaptive, 3: VFLos + active_los_method: 2 # 0: Proportional, 1: Integral, 2: Adaptive, 3: VFLos u_desired: 0.3 # Done goal_reached_tol: 0.35 # Done From 6ecdc9fa8b4e138ab98848049de43b87fae2c253 Mon Sep 17 00:00:00 2001 From: Anbit Date: Tue, 24 Feb 2026 17:13:52 +0100 Subject: [PATCH 066/128] Fix vector feild simulation issues --- .../los_guidance/config/guidance_params.yaml | 2 +- guidance/los_guidance/src/los_guidance_ros.cpp | 17 ++++++++++++----- 2 files changed, 13 insertions(+), 6 deletions(-) diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 62164a9a1..66bd0c2e3 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -21,7 +21,7 @@ vector_field_los: k_p_v: 0.3 common: - active_los_method: 2 # 0: Proportional, 1: Integral, 2: Adaptive, 3: VFLos + active_los_method: 3 # 0: Proportional, 1: Integral, 2: Adaptive, 3: VFLos u_desired: 0.3 # Done goal_reached_tol: 0.35 # Done diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 45ecd9dde..ba816d4d9 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -177,6 +177,8 @@ void LosGuidanceNode::set_vector_field_guidance(YAML::Node config) { void LosGuidanceNode::waypoint_callback( const geometry_msgs::msg::PointStamped::SharedPtr wp_msg) { + std::lock_guard lock(mutex_); + const auto new_wp = types::Point::point_from_ros(wp_msg->point); if (!has_active_segment_) { @@ -184,19 +186,18 @@ void LosGuidanceNode::waypoint_callback( path_inputs_.next_point = new_wp; has_active_segment_ = true; } else { - path_inputs_.prev_point = path_inputs_.next_point; + path_inputs_.prev_point = path_inputs_.next_point; path_inputs_.next_point = new_wp; } - spdlog::info("Received waypoint: ({}, {}, {})", - new_wp.x, new_wp.y, new_wp.z); + spdlog::info("Received waypoint: ({}, {}, {})", new_wp.x, new_wp.y, new_wp.z); } void LosGuidanceNode::pose_callback( - const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr - current_pose) { + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr current_pose) { + std::lock_guard lock(mutex_); path_inputs_.current_position = types::Point::point_from_ros(current_pose->pose.pose.position); } @@ -319,6 +320,12 @@ void LosGuidanceNode::execute( return; } + types::Inputs inputs_copy; + { + std::lock_guard lock(mutex_); + inputs_copy = path_inputs_; + } + types::Outputs outputs; switch (method_) { From 4c59e21a680ee44b99922534119350cda338aa66 Mon Sep 17 00:00:00 2001 From: Henrik Date: Wed, 25 Feb 2026 18:00:50 +0100 Subject: [PATCH 067/128] Added acados NMPC, ill conditioned needs fixing --- control/velocity_controller/CMakeLists.txt | 59 +- .../config/parameters.yaml | 11 +- .../velocity_controller/NMPC_acados.hpp | 89 + .../velocity_controller/NMPC_setup.hpp | 2 +- .../include/velocity_controller/PID_setup.hpp | 4 +- .../include/velocity_controller/test_VC.hpp | 2 +- .../velocity_controller.hpp | 3 + .../launch/VCnTest.launch.py | 26 +- .../velocity_controller/src/NMPC_acados.cpp | 197 + .../velocity_controller/src/NMPC_setup.cpp | 8 +- control/velocity_controller/src/PID_setup.cpp | 14 +- control/velocity_controller/src/auv_ocp.json | 1561 ++++++++ .../src/build_auv_solver/Makefile | 206 + .../acados_sim_solver_auv_model.c | 310 ++ .../acados_sim_solver_auv_model.h | 105 + .../src/build_auv_solver/acados_solver.pxd | 63 + .../acados_solver_auv_model.c | 1198 ++++++ .../acados_solver_auv_model.h | 174 + .../acados_solver_auv_model.o | Bin 0 -> 35480 bytes .../auv_model_model/auv_model_expl_ode_fun.c | 386 ++ .../auv_model_model/auv_model_expl_ode_fun.o | Bin 0 -> 8232 bytes .../auv_model_model/auv_model_expl_vde_adj.c | 524 +++ .../auv_model_model/auv_model_expl_vde_adj.o | Bin 0 -> 12672 bytes .../auv_model_model/auv_model_expl_vde_forw.c | 3385 +++++++++++++++++ .../auv_model_model/auv_model_expl_vde_forw.o | Bin 0 -> 49648 bytes .../auv_model_model/auv_model_model.h | 74 + .../libacados_ocp_solver_auv_model.so | Bin 0 -> 85424 bytes .../src/build_auv_solver/main_auv_model.c | 189 + .../src/build_auv_solver/main_sim_auv_model.c | 141 + control/velocity_controller/src/dynamics.py | 168 + control/velocity_controller/src/generator.py | 187 + control/velocity_controller/src/test_VC.cpp | 28 +- .../src/velocity_controller.cpp | 82 +- .../velocity_controller/tests/CMakeLists.txt | 26 - 34 files changed, 9135 insertions(+), 87 deletions(-) create mode 100644 control/velocity_controller/include/velocity_controller/NMPC_acados.hpp create mode 100644 control/velocity_controller/src/NMPC_acados.cpp create mode 100644 control/velocity_controller/src/auv_ocp.json create mode 100644 control/velocity_controller/src/build_auv_solver/Makefile create mode 100644 control/velocity_controller/src/build_auv_solver/acados_sim_solver_auv_model.c create mode 100644 control/velocity_controller/src/build_auv_solver/acados_sim_solver_auv_model.h create mode 100644 control/velocity_controller/src/build_auv_solver/acados_solver.pxd create mode 100644 control/velocity_controller/src/build_auv_solver/acados_solver_auv_model.c create mode 100644 control/velocity_controller/src/build_auv_solver/acados_solver_auv_model.h create mode 100644 control/velocity_controller/src/build_auv_solver/acados_solver_auv_model.o create mode 100644 control/velocity_controller/src/build_auv_solver/auv_model_model/auv_model_expl_ode_fun.c create mode 100644 control/velocity_controller/src/build_auv_solver/auv_model_model/auv_model_expl_ode_fun.o create mode 100644 control/velocity_controller/src/build_auv_solver/auv_model_model/auv_model_expl_vde_adj.c create mode 100644 control/velocity_controller/src/build_auv_solver/auv_model_model/auv_model_expl_vde_adj.o create mode 100644 control/velocity_controller/src/build_auv_solver/auv_model_model/auv_model_expl_vde_forw.c create mode 100644 control/velocity_controller/src/build_auv_solver/auv_model_model/auv_model_expl_vde_forw.o create mode 100644 control/velocity_controller/src/build_auv_solver/auv_model_model/auv_model_model.h create mode 100755 control/velocity_controller/src/build_auv_solver/libacados_ocp_solver_auv_model.so create mode 100644 control/velocity_controller/src/build_auv_solver/main_auv_model.c create mode 100644 control/velocity_controller/src/build_auv_solver/main_sim_auv_model.c create mode 100644 control/velocity_controller/src/dynamics.py create mode 100644 control/velocity_controller/src/generator.py diff --git a/control/velocity_controller/CMakeLists.txt b/control/velocity_controller/CMakeLists.txt index 9fa6b01dc..03fc60b89 100644 --- a/control/velocity_controller/CMakeLists.txt +++ b/control/velocity_controller/CMakeLists.txt @@ -23,10 +23,37 @@ find_package(ct_optcon REQUIRED) find_package(ct_core REQUIRED) find_package(casadi REQUIRED) +set(ACADOS_ROOT "$ENV{ACADOS_SOURCE_DIR}" CACHE PATH "acados root") include_directories( + ${ACADOS_ROOT}/include + ${ACADOS_ROOT}/include/acados + ${ACADOS_ROOT}/include/acados/ocp_nlp + ${ACADOS_ROOT}/include/acados_c + ${ACADOS_ROOT}/include/blasfeo/include + ${ACADOS_ROOT}/include/hpipm/include + ${ACADOS_ROOT}/include/qpOASES_e + ${CMAKE_CURRENT_SOURCE_DIR}/src/build_auv_solver + include + +) +link_directories(${ACADOS_ROOT}/lib) + + +# After setting ACADOS_ROOT etc. +file(GLOB_RECURSE GEN_SRCS + ${CMAKE_CURRENT_SOURCE_DIR}/src/build_auv_solver/*.c + ${CMAKE_CURRENT_SOURCE_DIR}/src/build_auv_solver/*/*.c ) + + +add_library(auv_nmpc STATIC + src/NMPC_acados.cpp + ${GEN_SRCS} +) + + add_executable(velocity_controller_node src/velocity_controller.cpp src/PID_setup.cpp @@ -34,6 +61,7 @@ add_executable(velocity_controller_node src/utilities.cpp src/ct_instantiations.cpp src/NMPC_setup.cpp + #src/NMPC_acados.cpp ) @@ -54,7 +82,8 @@ ament_target_dependencies(velocity_controller_node vortex_utils ) #target_include_directories(velocity_controller_node PRIVATE casadi Eigen3) -target_link_libraries(velocity_controller_node Eigen3::Eigen casadi::casadi ct_optcon ct_core) +target_link_libraries(auv_nmpc acados hpipm blasfeo qpOASES_e m) +target_link_libraries(velocity_controller_node Eigen3::Eigen casadi::casadi ct_optcon ct_core auv_nmpc) install(TARGETS velocity_controller_node DESTINATION lib/${PROJECT_NAME} @@ -74,5 +103,33 @@ if(BUILD_TESTING) add_subdirectory(tests) endif() +add_executable(test_VC_node +src/test_VC.cpp +src/utilities.cpp +src/ct_instantiations.cpp +) + +target_include_directories(test_VC_node PUBLIC +$ +$ +${EIGEN3_INCLUDE_DIR} +) + + +target_link_libraries(test_VC_node Eigen3::Eigen ct_optcon ct_core) + +ament_target_dependencies(test_VC_node +rclcpp +std_msgs +vortex_msgs +geometry_msgs +nav_msgs +vortex_utils +) + +install(TARGETS test_VC_node +DESTINATION lib/${PROJECT_NAME} +) + ament_package() diff --git a/control/velocity_controller/config/parameters.yaml b/control/velocity_controller/config/parameters.yaml index 22dae8f43..f3b3c6ad4 100644 --- a/control/velocity_controller/config/parameters.yaml +++ b/control/velocity_controller/config/parameters.yaml @@ -14,15 +14,18 @@ Q: [300.0,32.84,32.84,32.84,32.84,100.0,32.84,32.84] R: [0.02,3.1,3.10] NMPC_params: - Q: [300.0,0.01,0.01,0.01,32.84,32.84,32.84,32.84,32.84] # u,v,w,p,q,r,phi,theta,psi - R: [0.02,3.1,3.10] # u_surge, u_theta, u_psi + Q: [1.0,0.0,0.0,0.0,0.1,0.1,0.0,2.0,5.0] # u,v,w,p,q,r,phi,theta,psi + R: [0.1,1.0,1.0] # u_surge, u_theta, u_psi + N: 20 inertia_matrix: [ 30.0, 0.0, 0.0, 0.0, 0.0, 0.6, 0.0, 30.0, 0.0, 0.0, -0.6, 0.3, 0.0, 0.0, 30.0, 0.6, 0.3, 0.0, 0.0, 0.0, 0.6, 0.68, 0.0, 0.0, 0.0, -0.6, 0.3, 0.0, 3.32, 0.0, 0.6, 0.3, 0.0, 0.0, 0.0, 3.34] dampening_matrix_low: [23.0,0.0,0.0,0.0,0.0,0.0, 0.0,46.0,0.0,0.0,0.0,0.0, 0.0,0.0,46.0,0.0,0.0,0.0, 0.0,0.0,0.0,46.0,0.0,0.0, 0.0,0.0,0.0,0.0,46.0,0.0, 0.0,0.0,0.0,0.0,0.0,46.0] dampening_matrix_high: [1.0,0.0,0.0,0.0,0.0,0.0, 0.0,1.0,0.0,0.0,0.0,0.0, 0.0,0.0,1.0,0.0,0.0,0.0, 0.0,0.0,0.0,1.0,0.0,0.0, 0.0,0.0,0.0,0.0,1.0,0.0, 0.0,0.0,0.0,0.0,0.0,1.0] #calculation_rate: 200 #ms integer - publish_rate: 10 #ms + publish_rate: 100 #ms #Clamp parameter max_force: 99.5 #should maybe be 99.5 - controller_type: 3 #1 PID 2 LQR 3 NMPC + controller_type: 4 #1 PID 2 LQR 3 NMPC 4NMPC fast + #Q: [300.0,0.01,0.01,0.01,32.84,32.84,32.84,32.84,32.84] # u,v,w,p,q,r,phi,theta,psi + # R: [0.02,3.1,3.10] # u_surge, u_theta, u_psi diff --git a/control/velocity_controller/include/velocity_controller/NMPC_acados.hpp b/control/velocity_controller/include/velocity_controller/NMPC_acados.hpp new file mode 100644 index 000000000..9ec826c17 --- /dev/null +++ b/control/velocity_controller/include/velocity_controller/NMPC_acados.hpp @@ -0,0 +1,89 @@ + +#pragma once +#include +#include +#include +#include +#include +#include + +// acados C API (generated) +extern "C" { +#include "acados_c/ocp_nlp_interface.h" +#include "acados/ocp_nlp/ocp_nlp_common.h" +#include "acados/utils/print.h" + +// Generated solver headers +#include "acados_solver_auv_model.h" // <-- from build_auv_solver/ +} + +class AuvNMPC { +public: + // Adjust sizes if your model differs + static constexpr int NX = 9; // [u v w p q r phi theta psi] + static constexpr int NU = 3; // [Fx My Mz] + static constexpr int NY = NX + NU; + static constexpr int NY_E = NX; + + +// Pass N if your generated header does not provide _acados_get_N() + explicit AuvNMPC(int N_horizon_override = -1) + : N_override_(N_horizon_override) {} + + ~AuvNMPC(); + + + // Not copyable + AuvNMPC(const AuvNMPC&) = delete; + AuvNMPC& operator=(const AuvNMPC&) = delete; + + // Lifecycle + bool init(); + + + void set_weights(const std::vector& W_diag, const std::vector& W_e_diag); + void set_max_force(double max_force); // updates bound on con_h: Fx^2+My^2+Mz^2 <= max^2 + + // Inputs + void setState(const std::array& x); + void setReference(const std::array& x_ref, const std::array& u_ref); + void setWeights(const std::vector& W_diag, const std::vector& W_e_diag); // sizes: NY, NY_E + void setMaxForce(double max_force); // updates con_h bounds + + + // One-shot solve: provide current state, desired state & input refs, get u0 back. + // Returns solver status (0 == success). + int solve_once(); + + // Outputs + std::vector getU0(); + + private: + + // generated capsule type (from acados_solver_auv_model.h) + auv_model_solver_capsule* capsule_ = nullptr; + + + // acados solver + ocp_nlp_solver* solver_ = nullptr; + ocp_nlp_config* config_ = nullptr; + ocp_nlp_dims* dims_ = nullptr; + ocp_nlp_in* nlp_in_ = nullptr; + ocp_nlp_out* nlp_out_= nullptr; + + int N_=20; + int N_override_=-1; + + std::vector W_diag_{NY,0.0}; // length NY + std::vector W_e_diag_{NY_E,0.0}; // length NY_E + double max_force2_ = 100*100; // squared constraint, default 100^2 + + + static void set_diag(double* M, int n, const std::vector& diag); + //U out + std::vector u0_out={0,0,0}; + //Recorded states states + std::array x0; + std::array xr; + std::array ur={0,0,0}; +}; diff --git a/control/velocity_controller/include/velocity_controller/NMPC_setup.hpp b/control/velocity_controller/include/velocity_controller/NMPC_setup.hpp index a45b633a8..2100e96cc 100644 --- a/control/velocity_controller/include/velocity_controller/NMPC_setup.hpp +++ b/control/velocity_controller/include/velocity_controller/NMPC_setup.hpp @@ -23,7 +23,7 @@ class NMPC_controller{ double Iz; double Ix; double Iy; - int N=20; + int N=3; int n=9; int m=3; casadi::DM Z0_next; //For warm start diff --git a/control/velocity_controller/include/velocity_controller/PID_setup.hpp b/control/velocity_controller/include/velocity_controller/PID_setup.hpp index d3f5c2564..a690fce04 100644 --- a/control/velocity_controller/include/velocity_controller/PID_setup.hpp +++ b/control/velocity_controller/include/velocity_controller/PID_setup.hpp @@ -22,7 +22,7 @@ class PID_controller { double integral=0; double previous_error=0; double output=0; - double max_output; - double min_output; + double max_output_; + double min_output_; }; diff --git a/control/velocity_controller/include/velocity_controller/test_VC.hpp b/control/velocity_controller/include/velocity_controller/test_VC.hpp index 127aa4211..711c9f334 100644 --- a/control/velocity_controller/include/velocity_controller/test_VC.hpp +++ b/control/velocity_controller/include/velocity_controller/test_VC.hpp @@ -37,7 +37,7 @@ class test_VC : public rclcpp::Node{ //std::string topic_odom; //std::string topic_thrust; std::string topic_guidance; - std::string topic_state="state"; + std::string topic_state="/state"; std::string topic_odometry; diff --git a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp index 31073d565..5e1545a45 100644 --- a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp +++ b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp @@ -12,6 +12,7 @@ #include "nav_msgs/msg/odometry.hpp" #include "vortex_msgs/msg/los_guidance.hpp" #include "velocity_controller/NMPC_setup.hpp" +#include "velocity_controller/NMPC_acados.hpp" class Velocity_node : public rclcpp::Node{ @@ -78,6 +79,8 @@ class Velocity_node : public rclcpp::Node{ std::vector dampening_matrix_high; //NMPC controller NMPC_controller NMPC; + //NMPC acados + AuvNMPC NMPC_acados; //NMPC parameters std::vector Q2; std::vector R2; diff --git a/control/velocity_controller/launch/VCnTest.launch.py b/control/velocity_controller/launch/VCnTest.launch.py index 454a50d96..6e2defd8f 100644 --- a/control/velocity_controller/launch/VCnTest.launch.py +++ b/control/velocity_controller/launch/VCnTest.launch.py @@ -18,7 +18,7 @@ def generate_launch_description(): PythonLaunchDescriptionSource( os.path.join(stonefish_dir, 'launch', 'simulation.launch.py') ), - launch_arguments={'rendering_quality': 'low','rendering':'true'}.items(), + launch_arguments={'rendering_quality': 'low','rendering':'false'}.items(), ) orca_sim = TimerAction( period=12.0, @@ -32,31 +32,31 @@ def generate_launch_description(): ) - node_name_arg = DeclareLaunchArgument( - 'node_name', default_value='velocity_controller_node', - description='Name of the velocity controller node' - ) + #node_name_arg = DeclareLaunchArgument( + # 'node_name', default_value='velocity_controller_node', + # description='Name of the velocity controller node' + #) node_name_arg2 = DeclareLaunchArgument( 'node_name_1', default_value='test_VC_node', description='Name of the test VC node' ) - velocity_controller_name = LaunchConfiguration('node_name') + #velocity_controller_name = LaunchConfiguration('node_name') test_VC_name = LaunchConfiguration('node_name_1') return LaunchDescription([ stonefish_sim, orca_sim, - node_name_arg, + #node_name_arg, node_name_arg2, - Node(package='velocity_controller', - executable='velocity_controller_node', - name=velocity_controller_name, - output='screen', - parameters=[config_path] + #Node(package='velocity_controller', + # executable='velocity_controller_node', + # name=velocity_controller_name, + # output='screen', + # parameters=[config_path] #arguments=['--ros-args','--log-level','debug'] - ), + # ), Node(package='velocity_controller', executable='test_VC_node', name=test_VC_name, diff --git a/control/velocity_controller/src/NMPC_acados.cpp b/control/velocity_controller/src/NMPC_acados.cpp new file mode 100644 index 000000000..dbaca183c --- /dev/null +++ b/control/velocity_controller/src/NMPC_acados.cpp @@ -0,0 +1,197 @@ +//#include "rclcpp/rclcpp.hpp" +#include "velocity_controller/NMPC_acados.hpp" +#include // memcpy +#include +//#include "acados_solver_auv_model.h" +#include + +void AuvNMPC::set_diag(double* M, int n, const std::vector& diag) { + for (int i = 0; i < n; ++i) { + std::memset(M + i*n, 0, n * sizeof(double)); + if (i < (int)diag.size()) M[i*n + i] = diag[i]; + } +} + +AuvNMPC::~AuvNMPC() { + if (capsule_) { + auv_model_acados_free(capsule_); + auv_model_acados_free_capsule(capsule_); + capsule_ = nullptr; + } +} + +bool AuvNMPC::init() { + capsule_ = auv_model_acados_create_capsule(); + if (!capsule_) return false; + + int status = auv_model_acados_create(capsule_); + if (status) { + std::cerr << "[AuvNMPC] create failed, status: " << status << std::endl; + return false; + } + + solver_ = auv_model_acados_get_nlp_solver(capsule_); + config_ = auv_model_acados_get_nlp_config(capsule_); + dims_ = auv_model_acados_get_nlp_dims(capsule_); + nlp_in_ = auv_model_acados_get_nlp_in(capsule_); + nlp_out_= auv_model_acados_get_nlp_out(capsule_); + + // Get N from generated getter if available; else use override. + // If your header doesn’t have *_get_N, pass N in the constructor. + #ifdef auv_model_acados_get_N + N_ = auv_model_acados_get_N(capsule_); + #else + N_ = (N_override_ > 0) ? N_override_ : 20; // fallback + #endif + + // Provide some safe default weights, tune later or call set_weights() + if (W_diag_.size() == NY) { + // Example diag: [Q; R] + // states: 5,5,8, 1,1,1, 10,15,10 + // inputs: 1,0.5,0.5 + double Wd[NY] = {5,5,8, 1,1,1, 10,15,10, 1,0.5,0.5}; + W_diag_.assign(Wd, Wd + NY); + } + if (W_e_diag_.size() == NY_E) { + double Wed[NY_E] = {10,10,15, 2,2,2, 30,40,30}; + W_e_diag_.assign(Wed, Wed + NY_E); + } + + // Initialize per-stage yref & W (zeros ref by default) + std::vector W(NY * NY, 0.0); + set_diag(W.data(), NY, W_diag_); + for (int k = 0; k < N_; ++k) { + double yref[NY] = {0}; + ocp_nlp_cost_model_set(config_, dims_, nlp_in_, k, "yref", yref); + ocp_nlp_cost_model_set(config_, dims_, nlp_in_, k, "W", W.data()); + } + { + std::vector W_e(NY_E * NY_E, 0.0); + set_diag(W_e.data(), NY_E, W_e_diag_); + double yref_e[NY_E] = {0}; + ocp_nlp_cost_model_set(config_, dims_, nlp_in_, N_, "yref", yref_e); + ocp_nlp_cost_model_set(config_, dims_, nlp_in_, N_, "W", W_e.data()); + } + + // Initialize nonlinear constraint bounds at stage 0 (if you defined con_h_expr with nh=1) + double lh0[1] = { 0.0 }; + double uh0[1] = { max_force2_ }; + ocp_nlp_constraints_model_set(config_, dims_, nlp_in_, nlp_out_, 0, "lh", lh0); + ocp_nlp_constraints_model_set(config_, dims_, nlp_in_, nlp_out_, 0, "uh", uh0); + + return true; +} + +void AuvNMPC::set_weights(const std::vector& Wd, const std::vector& We) { + for (int i=0;i<(int)Wd.size();i++){ + std::cout<(x0.data())); + ocp_nlp_constraints_model_set(config_, dims_, nlp_in_, nlp_out_, 0, "ubx", const_cast(x0.data())); + + // Build W and W_e from current diagonals (could cache) + std::vector W(NY * NY, 0.0); + set_diag(W.data(), NY, W_diag_); + std::vector W_e(NY_E * NY_E, 0.0); + set_diag(W_e.data(), NY_E, W_e_diag_); + + // Update stages + for (int k = 0; k < N_; ++k) { + double yref[NY] = {0}; + std::memcpy(yref, xr.data(), NX*sizeof(double)); + std::memcpy(yref+NX, ur.data(), NU*sizeof(double)); + ocp_nlp_cost_model_set(config_, dims_, nlp_in_, k, "yref", yref); + ocp_nlp_cost_model_set(config_, dims_, nlp_in_, k, "W", W.data()); + + double lh[1] = { 0.0 }; + double uh[1] = { max_force2_ }; + ocp_nlp_constraints_model_set(config_, dims_, nlp_in_, nlp_out_, k, "lh", lh); + ocp_nlp_constraints_model_set(config_, dims_, nlp_in_, nlp_out_, k, "uh", uh); + } + + + { + double yref_e[NY_E] = {0}; + std::memcpy(yref_e, xr.data(), NX*sizeof(double)); + ocp_nlp_cost_model_set(config_, dims_, nlp_in_, N_, "yref", yref_e); + ocp_nlp_cost_model_set(config_, dims_, nlp_in_, N_, "W", W_e.data()); + } + + // Solve (blocking) + int status = auv_model_acados_solve(capsule_); + + // Read u0 + double u0[NU] = {0}; + ocp_nlp_out_get(config_, dims_, nlp_out_, 0, "u", u0); + for (int i=0;i AuvNMPC::getU0(){ + return u0_out; +} + +void AuvNMPC::setState(const std::array& x){ + x0=x; + for (int i=0;i& x_ref, const std::array& u_ref){ + xr=x_ref; + ur=u_ref; + std::cout<<"xr: "; + for (int i=0;imax_output){ - output = max_output; + if (output>max_output_){ + output = max_output_; } - else if (output < min_output){ - output = min_output; + else if (output < min_output_){ + output = min_output_; } else{ integral+=error*dt; //anti-wind up @@ -41,8 +41,8 @@ bool PID_controller::set_output_limits(double min_output, double max_output){ if (max_outputmin_output = min_output; - this->max_output = max_output; + min_output_ = min_output; + max_output_ = max_output; return true; }; void PID_controller::set_parameters(double k_p,double k_i, double k_d){ diff --git a/control/velocity_controller/src/auv_ocp.json b/control/velocity_controller/src/auv_ocp.json new file mode 100644 index 000000000..74563f32c --- /dev/null +++ b/control/velocity_controller/src/auv_ocp.json @@ -0,0 +1,1561 @@ +{ + "code_gen_opts": { + "acados_include_path": "/home/henrik/ros2_ws_v/src/acados/include", + "acados_lib_path": "/home/henrik/ros2_ws_v/src/acados/lib", + "acados_link_libs": { + "clarabel": "", + "daqp": "", + "hpmpc": "", + "ooqp": "", + "openmp": "-fopenmp", + "osqp": "-losqp", + "qpdunes": "", + "qpoases": "-lqpOASES_e" + }, + "acados_version": "508482dac", + "code_export_directory": "/home/henrik/ros2_ws_v/src/vortex-auv/control/velocity_controller/src/build_auv_solver", + "cython_include_dirs": [ + "/usr/lib/python3/dist-packages/numpy/core/include", + "/usr/include/python3.10" + ], + "json_file": "auv_ocp.json", + "os": "unix", + "shared_lib_ext": ".so" + }, + "constraints": { + "C": [], + "C_e": [], + "D": [], + "constr_type": "BGH", + "constr_type_0": "BGH", + "constr_type_e": "BGH", + "constr_types": [ + "BGH", + "BGP" + ], + "has_x0": true, + "idxbu": [ + 0, + 1, + 2 + ], + "idxbx": [], + "idxbx_0": [ + 0, + 1, + 2, + 3, + 4, + 5, + 6, + 7, + 8 + ], + "idxbx_e": [], + "idxbxe_0": [ + 0, + 1, + 2, + 3, + 4, + 5, + 6, + 7, + 8 + ], + "idxe": [], + "idxs_rev": [], + "idxs_rev_0": [], + "idxs_rev_e": [], + "idxsbu": [], + "idxsbx": [], + "idxsbx_e": [], + "idxsg": [], + "idxsg_e": [], + "idxsh": [], + "idxsh_0": [], + "idxsh_e": [], + "idxsphi": [], + "idxsphi_0": [], + "idxsphi_e": [], + "lbu": [ + -99.5, + -99.5, + -99.5 + ], + "lbx": [], + "lbx_0": [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "lbx_e": [], + "lg": [], + "lg_e": [], + "lh": [], + "lh_0": [], + "lh_e": [], + "lphi": [], + "lphi_0": [], + "lphi_e": [], + "ls": [], + "ls_0": [], + "ls_e": [], + "lsbu": [], + "lsbx": [], + "lsbx_e": [], + "lsg": [], + "lsg_e": [], + "lsh": [], + "lsh_0": [], + "lsh_e": [], + "lsphi": [], + "lsphi_0": [], + "lsphi_e": [], + "ubu": [ + 99.5, + 99.5, + 99.5 + ], + "ubx": [], + "ubx_0": [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "ubx_e": [], + "ug": [], + "ug_e": [], + "uh": [], + "uh_0": [], + "uh_e": [], + "uphi": [], + "uphi_0": [], + "uphi_e": [], + "us": [], + "us_0": [], + "us_e": [], + "usbu": [], + "usbx": [], + "usbx_e": [], + "usg": [], + "usg_e": [], + "ush": [], + "ush_0": [], + "ush_e": [], + "usphi": [], + "usphi_0": [], + "usphi_e": [] + }, + "cost": { + "Vu": [ + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 1.0, + 0.0, + 0.0 + ], + [ + 0.0, + 1.0, + 0.0 + ], + [ + 0.0, + 0.0, + 1.0 + ] + ], + "Vu_0": [ + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 1.0, + 0.0, + 0.0 + ], + [ + 0.0, + 1.0, + 0.0 + ], + [ + 0.0, + 0.0, + 1.0 + ] + ], + "Vx": [ + [ + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "Vx_0": [ + [ + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "Vx_e": [ + [ + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0 + ] + ], + "Vz": [], + "Vz_0": [], + "W": [ + [ + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.1, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.1, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 2.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 5.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.1, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0 + ] + ], + "W_0": [ + [ + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.1, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.1, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 2.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 5.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.1, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0 + ] + ], + "W_e": [ + [ + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.1, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.1, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 2.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 5.0 + ] + ], + "Zl": [], + "Zl_0": [], + "Zl_e": [], + "Zu": [], + "Zu_0": [], + "Zu_e": [], + "cost_ext_fun_type": "casadi", + "cost_ext_fun_type_0": "casadi", + "cost_ext_fun_type_e": "casadi", + "cost_ext_fun_types": [ + "casadi", + "generic" + ], + "cost_function_ext_cost": null, + "cost_function_ext_cost_0": null, + "cost_function_ext_cost_e": null, + "cost_source_ext_cost": null, + "cost_source_ext_cost_0": null, + "cost_source_ext_cost_e": null, + "cost_type": "LINEAR_LS", + "cost_type_0": "LINEAR_LS", + "cost_type_e": "LINEAR_LS", + "cost_types": [ + "LINEAR_LS", + "NONLINEAR_LS", + "EXTERNAL", + "CONVEX_OVER_NONLINEAR", + "AUTO" + ], + "yref": [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "yref_0": [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "yref_e": [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "zl": [], + "zl_0": [], + "zl_e": [], + "zu": [], + "zu_0": [], + "zu_e": [] + }, + "dims": { + "N": 20, + "n_global_data": 0, + "nbu": 3, + "nbx": 0, + "nbx_0": 9, + "nbx_e": 0, + "nbxe_0": 9, + "ng": 0, + "ng_e": 0, + "nh": 0, + "nh_0": 0, + "nh_e": 0, + "np": 0, + "np_global": 0, + "nphi": 0, + "nphi_0": 0, + "nphi_e": 0, + "nr": 0, + "nr_0": 0, + "nr_e": 0, + "ns": 0, + "ns_0": 0, + "ns_e": 0, + "nsbu": 0, + "nsbx": 0, + "nsbx_e": 0, + "nsg": 0, + "nsg_e": 0, + "nsh": 0, + "nsh_0": 0, + "nsh_e": 0, + "nsphi": 0, + "nsphi_0": 0, + "nsphi_e": 0, + "nu": 3, + "nx": 9, + "nx_next": 9, + "ny": 12, + "ny_0": 12, + "ny_e": 9, + "nz": 0 + }, + "external_function_files_model": [ + "auv_model_model/auv_model_expl_ode_fun.c", + "auv_model_model/auv_model_expl_vde_forw.c", + "auv_model_model/auv_model_expl_vde_adj.c" + ], + "external_function_files_ocp": [], + "hash": "dab2d97fc9b228f38aa6cd2b8839020c", + "model": { + "con_h_expr": [], + "con_h_expr_0": [], + "con_h_expr_e": [], + "con_phi_expr": [], + "con_phi_expr_0": [], + "con_phi_expr_e": [], + "con_r_expr": [], + "con_r_expr_0": [], + "con_r_expr_e": [], + "con_r_in_phi": [], + "con_r_in_phi_0": [], + "con_r_in_phi_e": [], + "cost_conl_custom_outer_hess": [], + "cost_conl_custom_outer_hess_0": [], + "cost_conl_custom_outer_hess_e": [], + "cost_expr_ext_cost": [], + "cost_expr_ext_cost_0": [], + "cost_expr_ext_cost_custom_hess": [], + "cost_expr_ext_cost_custom_hess_0": [], + "cost_expr_ext_cost_custom_hess_e": [], + "cost_expr_ext_cost_e": [], + "cost_psi_expr": [], + "cost_psi_expr_0": [], + "cost_psi_expr_e": [], + "cost_r_in_psi_expr": [], + "cost_r_in_psi_expr_0": [], + "cost_r_in_psi_expr_e": [], + "cost_y_expr": [], + "cost_y_expr_0": [], + "cost_y_expr_e": [], + "disc_dyn_custom_hess_ux_expr": [], + "disc_dyn_custom_jac_ux_expr": [], + "disc_dyn_expr": [], + "dyn_disc_fun": null, + "dyn_disc_fun_jac": null, + "dyn_disc_fun_jac_hess": null, + "dyn_ext_fun_type": "casadi", + "dyn_generic_source": null, + "dyn_impl_dae_fun": null, + "dyn_impl_dae_fun_jac": null, + "dyn_impl_dae_jac": null, + "expression_names": [ + "f_expl_expr", + "p", + "p_global", + "pi", + "u", + "x", + "xdot", + "z" + ], + "f_expl_expr": "SX(@1=30, @2=-30, @3=((Fx-(((@1*w)*q)+((@2*v)*r)))-((23*u)+(fabs(u)*u))), @4=46, @5=((((@2*w)*p)+((@1*u)*r))+((@4*v)+(fabs(v)*v))), @6=((((@1*v)*p)+((@2*u)*q))+((@4*w)+(fabs(w)*w))), @7=((((((@1*w)*v)+((@2*v)*w))+((-3.34*r)*q))+((3.32*q)*r))+((@4*p)+(fabs(p)*p))), @8=((My-(((((@2*w)*u)+((@1*u)*w))+((3.34*r)*p))+((-0.68*p)*r)))-((@4*q)+(fabs(q)*q))), @9=((Mz-(((((@1*v)*u)+((@2*u)*v))+((-3.32*q)*p))+((0.68*p)*q)))-((@4*r)+(fabs(r)*r))), @10=-0.000546005, [((((((0.0334536*@3)-(6.0369e-05*@5))-(-1.11163e-07*@6))-(9.80847e-08*@7))+(1.09201e-05*@8))+(-0.00601506*@9)), ((((((6.0369e-05*@3)-(0.0334847*@5))-(-6.16582e-05*@6))-(5.44043e-05*@7))+(0.00605702*@8))+(-0.00301845*@9)), ((((((-1.11163e-07*@3)-(-6.16582e-05*@5))-(0.0339635*@6))-(-0.0299678*@7))+(-0.00308013*@8))+(5.55814e-06*@9)), ((((((9.80847e-08*@3)-(5.44043e-05*@5))-(-0.0299678*@6))-(1.49703*@7))+(0.00271776*@8))+(-4.90424e-06*@9)), ((((((1.09201e-05*@3)-(0.00605702*@5))-(-0.00308013*@6))-(0.00271776*@7))+(0.302578*@8))+(@10*@9)), ((((((-0.00601506*@3)-(-0.00301845*@5))-(5.55814e-06*@6))-(-4.90424e-06*@7))+(@10*@8))+(0.300753*@9)), ((p+((sin(phi)*tan(theta))*q))+((cos(phi)*tan(theta))*r)), ((cos(phi)*q)-(sin(phi)*r)), (((sin(phi)/cos(theta))*q)+((cos(phi)/cos(theta))*r))])", + "f_impl_expr": [], + "gnsf_model": null, + "name": "auv_model", + "nu_original": null, + "p": "SX(0x1)", + "p_global": "SX(0x1)", + "pi": "SX([pi_0, pi_1, pi_2, pi_3, pi_4, pi_5, pi_6, pi_7, pi_8])", + "serialized_expressions": "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", + "t": [], + "t0": null, + "t_label": "t", + "u": "SX([Fx, My, Mz])", + "u_labels": [ + "u0", + "u1", + "u2" + ], + "x": "SX([u, v, w, p, q, r, phi, theta, psi])", + "x_labels": [ + "x0", + "x1", + "x2", + "x3", + "x4", + "x5", + "x6", + "x7", + "x8" + ], + "xdot": "SX([xdot_0, xdot_1, xdot_2, xdot_3, xdot_4, xdot_5, xdot_6, xdot_7, xdot_8])", + "z": "SX(0x1)" + }, + "name": "auv_model", + "p_global_values": [], + "parameter_values": [], + "problem_class": "OCP", + "ros_opts": null, + "simulink_opts": null, + "solver_options": { + "N_horizon": 20, + "Tsim": 0.1, + "adaptive_levenberg_marquardt_lam": 5.0, + "adaptive_levenberg_marquardt_mu0": 0.001, + "adaptive_levenberg_marquardt_mu_min": 1e-16, + "adaptive_levenberg_marquardt_obj_scalar": 2.0, + "allow_direction_mode_switch_to_nominal": true, + "anderson_activation_threshold": 10.0, + "as_rti_iter": 1, + "as_rti_level": 4, + "byrd_omojokon_slack_relaxation_factor": 1.00001, + "collocation_type": "GAUSS_LEGENDRE", + "cost_discretization": "EULER", + "cost_scaling": [ + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 1.0 + ], + "custom_templates": [], + "custom_update_copy": true, + "custom_update_filename": "", + "custom_update_header_filename": "", + "eval_residual_at_max_iter": false, + "exact_hess_constr": 1, + "exact_hess_cost": 1, + "exact_hess_dyn": 1, + "ext_cost_num_hess": 0, + "ext_fun_compile_flags": "-O2", + "ext_fun_expand_constr": false, + "ext_fun_expand_cost": false, + "ext_fun_expand_dyn": false, + "ext_fun_expand_precompute": false, + "fixed_hess": 0, + "globalization": "MERIT_BACKTRACKING", + "globalization_alpha_min": 0.05, + "globalization_alpha_reduction": 0.7, + "globalization_eps_sufficient_descent": 0.0001, + "globalization_fixed_step_length": 1.0, + "globalization_full_step_dual": 0, + "globalization_funnel_fraction_switching_condition": 0.001, + "globalization_funnel_init_increase_factor": 15.0, + "globalization_funnel_init_upper_bound": 1.0, + "globalization_funnel_initial_penalty_parameter": 1.0, + "globalization_funnel_kappa": 0.9, + "globalization_funnel_sufficient_decrease_factor": 0.9, + "globalization_funnel_use_merit_fun_only": false, + "globalization_line_search_use_sufficient_descent": 0, + "globalization_use_SOC": 0, + "hessian_approx": "GAUSS_NEWTON", + "hpipm_mode": "BALANCE", + "integrator_type": "ERK", + "levenberg_marquardt": 0.0001, + "log_dual_step_norm": false, + "log_primal_step_norm": false, + "model_external_shared_lib_dir": null, + "model_external_shared_lib_name": null, + "nlp_qp_tol_min_comp": 1e-11, + "nlp_qp_tol_min_eq": 1e-10, + "nlp_qp_tol_min_ineq": 1e-10, + "nlp_qp_tol_min_stat": 1e-09, + "nlp_qp_tol_reduction_factor": 0.1, + "nlp_qp_tol_safety_factor": 0.1, + "nlp_qp_tol_strategy": "FIXED_QP_TOL", + "nlp_solver_ext_qp_res": 0, + "nlp_solver_max_iter": 100, + "nlp_solver_tol_comp": 1e-06, + "nlp_solver_tol_eq": 1e-06, + "nlp_solver_tol_ineq": 1e-06, + "nlp_solver_tol_min_step_norm": 0.0, + "nlp_solver_tol_stat": 1e-06, + "nlp_solver_type": "SQP", + "nlp_solver_warm_start_first_qp": false, + "nlp_solver_warm_start_first_qp_from_nlp": false, + "print_level": 2, + "qp_solver": "FULL_CONDENSING_HPIPM", + "qp_solver_cond_N": 20, + "qp_solver_cond_block_size": null, + "qp_solver_cond_ric_alg": 1, + "qp_solver_iter_max": 50, + "qp_solver_mu0": 0.0, + "qp_solver_ric_alg": 1, + "qp_solver_t0_init": 2, + "qp_solver_tol_comp": null, + "qp_solver_tol_eq": null, + "qp_solver_tol_ineq": null, + "qp_solver_tol_stat": null, + "qp_solver_warm_start": 1, + "qpscaling_lb_norm_inf_grad_obj": 0.0001, + "qpscaling_scale_constraints": "NO_CONSTRAINT_SCALING", + "qpscaling_scale_objective": "NO_OBJECTIVE_SCALING", + "qpscaling_ub_max_abs_eig": 100000.0, + "reg_adaptive_eps": false, + "reg_epsilon": 0.0001, + "reg_max_cond_block": 10000000.0, + "reg_min_epsilon": 1e-08, + "regularize_method": "NO_REGULARIZE", + "rti_log_only_available_residuals": 0, + "rti_log_residuals": 0, + "search_direction_mode": "NOMINAL_QP", + "sens_forw_p": false, + "shooting_nodes": [ + 0.0, + 0.1, + 0.2, + 0.30000000000000004, + 0.4, + 0.5, + 0.6, + 0.7, + 0.7999999999999999, + 0.8999999999999999, + 0.9999999999999999, + 1.0999999999999999, + 1.2, + 1.3, + 1.4000000000000001, + 1.5000000000000002, + 1.6000000000000003, + 1.7000000000000004, + 1.8000000000000005, + 1.9000000000000006, + 2.0000000000000004 + ], + "sim_method_jac_reuse": [ + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0 + ], + "sim_method_newton_iter": 3, + "sim_method_newton_tol": 0.0, + "sim_method_num_stages": [ + 4, + 4, + 4, + 4, + 4, + 4, + 4, + 4, + 4, + 4, + 4, + 4, + 4, + 4, + 4, + 4, + 4, + 4, + 4, + 4 + ], + "sim_method_num_steps": [ + 2, + 2, + 2, + 2, + 2, + 2, + 2, + 2, + 2, + 2, + 2, + 2, + 2, + 2, + 2, + 2, + 2, + 2, + 2, + 2 + ], + "solution_sens_qp_t_lam_min": 1e-09, + "store_iterates": false, + "tau_min": 0.0, + "tf": 2.0, + "time_steps": [ + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1 + ], + "timeout_heuristic": "LAST", + "timeout_max_time": 0.0, + "use_constraint_hessian_in_feas_qp": false, + "with_adaptive_levenberg_marquardt": false, + "with_anderson_acceleration": false, + "with_batch_functionality": false, + "with_solution_sens_wrt_params": false, + "with_value_sens_wrt_params": false + }, + "zoro_description": null +} \ No newline at end of file diff --git a/control/velocity_controller/src/build_auv_solver/Makefile b/control/velocity_controller/src/build_auv_solver/Makefile new file mode 100644 index 000000000..febdbaf3b --- /dev/null +++ b/control/velocity_controller/src/build_auv_solver/Makefile @@ -0,0 +1,206 @@ +# +# Copyright (c) The acados authors. +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# 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. +# +# 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 HOLDER 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.; +# + + + + + +# define sources and use make's implicit rules to generate object files (*.o) + +# model +MODEL_SRC= + +MODEL_SRC+= auv_model_model/auv_model_expl_ode_fun.c +MODEL_SRC+= auv_model_model/auv_model_expl_vde_forw.c +MODEL_SRC+= auv_model_model/auv_model_expl_vde_adj.c + +MODEL_OBJ := $(MODEL_SRC:.c=.o) +# optimal control problem - mostly CasADi exports +OCP_SRC= + + + +OCP_SRC+= acados_solver_auv_model.c + +OCP_OBJ := $(OCP_SRC:.c=.o) +# for sim solver +SIM_SRC= acados_sim_solver_auv_model.c +SIM_OBJ := $(SIM_SRC:.c=.o) + +# for target example_sim +EX_SIM_SRC= main_sim_auv_model.c +EX_SIM_OBJ := $(EX_SIM_SRC:.c=.o) +EX_SIM_EXE := $(EX_SIM_SRC:.c=) +# for target example +EX_SRC= main_auv_model.c +EX_OBJ := $(EX_SRC:.c=.o) +EX_EXE := $(EX_SRC:.c=) + + +# combine model, (potentially) sim and ocp object files +OBJ= +OBJ+= $(MODEL_OBJ) +OBJ+= $(SIM_OBJ) +OBJ+= $(OCP_OBJ) + +EXTERNAL_DIR= +EXTERNAL_LIB= + +INCLUDE_PATH = /home/henrik/ros2_ws_v/src/acados/include +LIB_PATH = /home/henrik/ros2_ws_v/src/acados/lib + +# preprocessor flags for make's implicit rules +CPPFLAGS+= -I$(INCLUDE_PATH) +CPPFLAGS+= -I$(INCLUDE_PATH)/acados +CPPFLAGS+= -I$(INCLUDE_PATH)/blasfeo/include +CPPFLAGS+= -I$(INCLUDE_PATH)/hpipm/include + + +# define the c-compiler flags for make's implicit rules +CFLAGS = -fPIC -std=c99 -O2 + +# # Debugging +# CFLAGS += -g3 -fno-diagnostics-show-line-numbers -g + +# linker flags +LDFLAGS+= -L$(LIB_PATH) + + +# link to libraries +LDLIBS+= -lacados +LDLIBS+= -lhpipm +LDLIBS+= -lblasfeo +LDLIBS+= -lm +LDLIBS+= + +# libraries +LIBACADOS_SOLVER=libacados_solver_auv_model.so +LIBACADOS_OCP_SOLVER=libacados_ocp_solver_auv_model.so +LIBACADOS_SIM_SOLVER=lib$(SIM_SRC:.c=.so) + +# virtual targets +.PHONY : all clean + + all: clean example_sim example +shared_lib: bundled_shared_lib ocp_shared_lib sim_shared_lib + +# some linker targets +example: $(EX_OBJ) $(OBJ) + $(CC) $^ -o $(EX_EXE) $(LDFLAGS) $(LDLIBS) + +example_sim: $(EX_SIM_OBJ) $(MODEL_OBJ) $(SIM_OBJ) + $(CC) $^ -o $(EX_SIM_EXE) $(LDFLAGS) $(LDLIBS) + +sim_shared_lib: $(SIM_OBJ) $(MODEL_OBJ) + $(CC) -shared $^ -o $(LIBACADOS_SIM_SOLVER) $(LDFLAGS) $(LDLIBS) +bundled_shared_lib: $(OBJ) + $(CC) -shared $^ -o $(LIBACADOS_SOLVER) $(LDFLAGS) $(LDLIBS) + +ocp_shared_lib: $(OCP_OBJ) $(MODEL_OBJ) + $(CC) -shared $^ -o $(LIBACADOS_OCP_SOLVER) $(LDFLAGS) $(LDLIBS) \ + -L$(EXTERNAL_DIR) -l$(EXTERNAL_LIB) + + +# Cython targets +ocp_cython_c: ocp_shared_lib + cython \ + -o acados_ocp_solver_pyx.c \ + -I $(INCLUDE_PATH)/../interfaces/acados_template/acados_template \ + $(INCLUDE_PATH)/../interfaces/acados_template/acados_template/acados_ocp_solver_pyx.pyx \ + -I /home/henrik/ros2_ws_v/src/vortex-auv/control/velocity_controller/src/build_auv_solver \ + +ocp_cython_o: ocp_cython_c + $(CC) $(ACADOS_FLAGS) -c -O2 \ + -fPIC \ + -DNPY_NO_DEPRECATED_API=NPY_1_7_API_VERSION \ + -o acados_ocp_solver_pyx.o \ + -I $(INCLUDE_PATH)/blasfeo/include/ \ + -I $(INCLUDE_PATH)/hpipm/include/ \ + -I $(INCLUDE_PATH) \ + -I /usr/lib/python3/dist-packages/numpy/core/include \ + -I /usr/include/python3.10 \ + acados_ocp_solver_pyx.c \ + +ocp_cython: ocp_cython_o + $(CC) $(ACADOS_FLAGS) -shared \ + -o acados_ocp_solver_pyx.so \ + -Wl,-rpath=$(LIB_PATH) \ + acados_ocp_solver_pyx.o \ + $(abspath .)/libacados_ocp_solver_auv_model.so \ + $(LDFLAGS) $(LDLIBS) + + +# Sim Cython targets +sim_cython_c: sim_shared_lib + cython \ + -o acados_sim_solver_pyx.c \ + -I $(INCLUDE_PATH)/../interfaces/acados_template/acados_template \ + $(INCLUDE_PATH)/../interfaces/acados_template/acados_template/acados_sim_solver_pyx.pyx \ + -I /home/henrik/ros2_ws_v/src/vortex-auv/control/velocity_controller/src/build_auv_solver \ + +sim_cython_o: sim_cython_c + $(CC) $(ACADOS_FLAGS) -c -O2 \ + -fPIC \ + -DNPY_NO_DEPRECATED_API=NPY_1_7_API_VERSION \ + -o acados_sim_solver_pyx.o \ + -I $(INCLUDE_PATH)/blasfeo/include/ \ + -I $(INCLUDE_PATH)/hpipm/include/ \ + -I $(INCLUDE_PATH) \ + -I /usr/lib/python3/dist-packages/numpy/core/include \ + -I /usr/include/python3.10 \ + acados_sim_solver_pyx.c \ + +sim_cython: sim_cython_o + $(CC) $(ACADOS_FLAGS) -shared \ + -o acados_sim_solver_pyx.so \ + -Wl,-rpath=$(LIB_PATH) \ + acados_sim_solver_pyx.o \ + $(abspath .)/libacados_sim_solver_auv_model.so \ + $(LDFLAGS) $(LDLIBS) + +clean: + $(RM) $(OBJ) $(EX_OBJ) $(EX_SIM_OBJ) + $(RM) $(LIBACADOS_SOLVER) $(LIBACADOS_OCP_SOLVER) $(LIBACADOS_SIM_SOLVER) + $(RM) $(EX_EXE) $(EX_SIM_EXE) +clean_ocp_shared_lib: + $(RM) $(LIBACADOS_OCP_SOLVER) + $(RM) $(OCP_OBJ) + +clean_ocp_cython: + $(RM) libacados_ocp_solver_auv_model.so + $(RM) acados_solver_auv_model.o + $(RM) acados_ocp_solver_pyx.so + $(RM) acados_ocp_solver_pyx.o + +clean_sim_cython: + $(RM) libacados_sim_solver_auv_model.so + $(RM) acados_sim_solver_auv_model.o + $(RM) acados_sim_solver_pyx.so + $(RM) acados_sim_solver_pyx.o diff --git a/control/velocity_controller/src/build_auv_solver/acados_sim_solver_auv_model.c b/control/velocity_controller/src/build_auv_solver/acados_sim_solver_auv_model.c new file mode 100644 index 000000000..1fa9ee42a --- /dev/null +++ b/control/velocity_controller/src/build_auv_solver/acados_sim_solver_auv_model.c @@ -0,0 +1,310 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * 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. + * + * 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 HOLDER 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.; + */ +// standard +#include +#include + +// acados +#include "acados_c/external_function_interface.h" +#include "acados_c/sim_interface.h" +#include "acados_c/external_function_interface.h" + +#include "acados/sim/sim_common.h" +#include "acados/utils/external_function_generic.h" +#include "acados/utils/print.h" + + +// example specific +#include "auv_model_model/auv_model_model.h" +#include "acados_sim_solver_auv_model.h" + + +// ** solver data ** + +auv_model_sim_solver_capsule * auv_model_acados_sim_solver_create_capsule() +{ + void* capsule_mem = malloc(sizeof(auv_model_sim_solver_capsule)); + auv_model_sim_solver_capsule *capsule = (auv_model_sim_solver_capsule *) capsule_mem; + + return capsule; +} + + +int auv_model_acados_sim_solver_free_capsule(auv_model_sim_solver_capsule * capsule) +{ + free(capsule); + return 0; +} + + +int auv_model_acados_sim_create(auv_model_sim_solver_capsule * capsule) +{ + // initialize + const int nx = AUV_MODEL_NX; + const int nu = AUV_MODEL_NU; + const int nz = AUV_MODEL_NZ; + const int np = AUV_MODEL_NP; + bool tmp_bool; + + double Tsim = 0.1; + + capsule->acados_sim_mem = NULL; + + external_function_opts ext_fun_opts; + external_function_opts_set_to_default(&ext_fun_opts); + ext_fun_opts.external_workspace = false; + + + // explicit ode + capsule->sim_expl_vde_forw = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); + capsule->sim_vde_adj_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); + capsule->sim_expl_ode_fun_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); + + capsule->sim_expl_vde_forw_p = NULL; + + + capsule->sim_expl_vde_forw->casadi_fun = &auv_model_expl_vde_forw; + capsule->sim_expl_vde_forw->casadi_n_in = &auv_model_expl_vde_forw_n_in; + capsule->sim_expl_vde_forw->casadi_n_out = &auv_model_expl_vde_forw_n_out; + capsule->sim_expl_vde_forw->casadi_sparsity_in = &auv_model_expl_vde_forw_sparsity_in; + capsule->sim_expl_vde_forw->casadi_sparsity_out = &auv_model_expl_vde_forw_sparsity_out; + capsule->sim_expl_vde_forw->casadi_work = &auv_model_expl_vde_forw_work; + external_function_param_casadi_create(capsule->sim_expl_vde_forw, np, &ext_fun_opts); + + capsule->sim_vde_adj_casadi->casadi_fun = &auv_model_expl_vde_adj; + capsule->sim_vde_adj_casadi->casadi_n_in = &auv_model_expl_vde_adj_n_in; + capsule->sim_vde_adj_casadi->casadi_n_out = &auv_model_expl_vde_adj_n_out; + capsule->sim_vde_adj_casadi->casadi_sparsity_in = &auv_model_expl_vde_adj_sparsity_in; + capsule->sim_vde_adj_casadi->casadi_sparsity_out = &auv_model_expl_vde_adj_sparsity_out; + capsule->sim_vde_adj_casadi->casadi_work = &auv_model_expl_vde_adj_work; + external_function_param_casadi_create(capsule->sim_vde_adj_casadi, np, &ext_fun_opts); + + capsule->sim_expl_ode_fun_casadi->casadi_fun = &auv_model_expl_ode_fun; + capsule->sim_expl_ode_fun_casadi->casadi_n_in = &auv_model_expl_ode_fun_n_in; + capsule->sim_expl_ode_fun_casadi->casadi_n_out = &auv_model_expl_ode_fun_n_out; + capsule->sim_expl_ode_fun_casadi->casadi_sparsity_in = &auv_model_expl_ode_fun_sparsity_in; + capsule->sim_expl_ode_fun_casadi->casadi_sparsity_out = &auv_model_expl_ode_fun_sparsity_out; + capsule->sim_expl_ode_fun_casadi->casadi_work = &auv_model_expl_ode_fun_work; + external_function_param_casadi_create(capsule->sim_expl_ode_fun_casadi, np, &ext_fun_opts); + + + + + + // sim plan & config + sim_solver_plan_t plan; + plan.sim_solver = ERK; + + // create correct config based on plan + sim_config * auv_model_sim_config = sim_config_create(plan); + capsule->acados_sim_config = auv_model_sim_config; + + // sim dims + void *auv_model_sim_dims = sim_dims_create(auv_model_sim_config); + capsule->acados_sim_dims = auv_model_sim_dims; + sim_dims_set(auv_model_sim_config, auv_model_sim_dims, "nx", &nx); + sim_dims_set(auv_model_sim_config, auv_model_sim_dims, "nu", &nu); + sim_dims_set(auv_model_sim_config, auv_model_sim_dims, "nz", &nz); + sim_dims_set(auv_model_sim_config, auv_model_sim_dims, "np", &np); + + + // sim opts + sim_opts *auv_model_sim_opts = sim_opts_create(auv_model_sim_config, auv_model_sim_dims); + capsule->acados_sim_opts = auv_model_sim_opts; + int tmp_int = 3; + sim_opts_set(auv_model_sim_config, auv_model_sim_opts, "newton_iter", &tmp_int); + double tmp_double = 0; + sim_opts_set(auv_model_sim_config, auv_model_sim_opts, "newton_tol", &tmp_double); + sim_collocation_type collocation_type = GAUSS_LEGENDRE; + sim_opts_set(auv_model_sim_config, auv_model_sim_opts, "collocation_type", &collocation_type); + + + tmp_int = 4; + sim_opts_set(auv_model_sim_config, auv_model_sim_opts, "num_stages", &tmp_int); + tmp_int = 2; + sim_opts_set(auv_model_sim_config, auv_model_sim_opts, "num_steps", &tmp_int); + tmp_bool = 0; + sim_opts_set(auv_model_sim_config, auv_model_sim_opts, "jac_reuse", &tmp_bool); + + + // sim in / out + sim_in *auv_model_sim_in = sim_in_create(auv_model_sim_config, auv_model_sim_dims); + capsule->acados_sim_in = auv_model_sim_in; + sim_out *auv_model_sim_out = sim_out_create(auv_model_sim_config, auv_model_sim_dims); + capsule->acados_sim_out = auv_model_sim_out; + + sim_in_set(auv_model_sim_config, auv_model_sim_dims, + auv_model_sim_in, "T", &Tsim); + + // model functions + auv_model_sim_config->model_set(auv_model_sim_in->model, + "expl_vde_forw", capsule->sim_expl_vde_forw); + auv_model_sim_config->model_set(auv_model_sim_in->model, + "expl_vde_adj", capsule->sim_vde_adj_casadi); + auv_model_sim_config->model_set(auv_model_sim_in->model, + "expl_ode_fun", capsule->sim_expl_ode_fun_casadi); + + + // sim solver + sim_solver *auv_model_sim_solver = sim_solver_create(auv_model_sim_config, + auv_model_sim_dims, auv_model_sim_opts, auv_model_sim_in); + capsule->acados_sim_solver = auv_model_sim_solver; + + capsule->acados_sim_mem = auv_model_sim_solver->mem; + + + + /* initialize input */ + // x + double x0[9]; + for (int ii = 0; ii < 9; ii++) + x0[ii] = 0.0; + + sim_in_set(auv_model_sim_config, auv_model_sim_dims, + auv_model_sim_in, "x", x0); + + + // u + double u0[3]; + for (int ii = 0; ii < 3; ii++) + u0[ii] = 0.0; + + sim_in_set(auv_model_sim_config, auv_model_sim_dims, + auv_model_sim_in, "u", u0); + + // S_forw + double S_forw[108]; + for (int ii = 0; ii < 108; ii++) + S_forw[ii] = 0.0; + for (int ii = 0; ii < 9; ii++) + S_forw[ii + ii * 9 ] = 1.0; + + + sim_in_set(auv_model_sim_config, auv_model_sim_dims, + auv_model_sim_in, "S_forw", S_forw); + + int status = sim_precompute(auv_model_sim_solver, auv_model_sim_in, auv_model_sim_out); + + return status; +} + + +int auv_model_acados_sim_solve(auv_model_sim_solver_capsule *capsule) +{ + // integrate dynamics using acados sim_solver + int status = sim_solve(capsule->acados_sim_solver, + capsule->acados_sim_in, capsule->acados_sim_out); + if (status != 0) + printf("error in auv_model_acados_sim_solve()! Exiting.\n"); + + return status; +} + + + + +int auv_model_acados_sim_free(auv_model_sim_solver_capsule *capsule) +{ + // free memory + sim_solver_destroy(capsule->acados_sim_solver); + sim_in_destroy(capsule->acados_sim_in); + sim_out_destroy(capsule->acados_sim_out); + sim_opts_destroy(capsule->acados_sim_opts); + sim_dims_destroy(capsule->acados_sim_dims); + sim_config_destroy(capsule->acados_sim_config); + + // free external function + external_function_param_casadi_free(capsule->sim_expl_vde_forw); + external_function_param_casadi_free(capsule->sim_vde_adj_casadi); + external_function_param_casadi_free(capsule->sim_expl_ode_fun_casadi); + + free(capsule->sim_expl_vde_forw); + free(capsule->sim_vde_adj_casadi); + free(capsule->sim_expl_ode_fun_casadi); + + + return 0; +} + + +int auv_model_acados_sim_update_params(auv_model_sim_solver_capsule *capsule, double *p, int np) +{ + int status = 0; + int casadi_np = AUV_MODEL_NP; + + if (casadi_np != np) { + printf("auv_model_acados_sim_update_params: trying to set %i parameters for external functions." + " External function has %i parameters. Exiting.\n", np, casadi_np); + exit(1); + } + capsule->sim_expl_vde_forw[0].set_param(capsule->sim_expl_vde_forw, p); + capsule->sim_vde_adj_casadi[0].set_param(capsule->sim_vde_adj_casadi, p); + capsule->sim_expl_ode_fun_casadi[0].set_param(capsule->sim_expl_ode_fun_casadi, p); + + + return status; +} + +/* getters pointers to C objects*/ +sim_config * auv_model_acados_get_sim_config(auv_model_sim_solver_capsule *capsule) +{ + return capsule->acados_sim_config; +}; + +sim_in * auv_model_acados_get_sim_in(auv_model_sim_solver_capsule *capsule) +{ + return capsule->acados_sim_in; +}; + +sim_out * auv_model_acados_get_sim_out(auv_model_sim_solver_capsule *capsule) +{ + return capsule->acados_sim_out; +}; + +void * auv_model_acados_get_sim_dims(auv_model_sim_solver_capsule *capsule) +{ + return capsule->acados_sim_dims; +}; + +sim_opts * auv_model_acados_get_sim_opts(auv_model_sim_solver_capsule *capsule) +{ + return capsule->acados_sim_opts; +}; + +sim_solver * auv_model_acados_get_sim_solver(auv_model_sim_solver_capsule *capsule) +{ + return capsule->acados_sim_solver; +}; + +void * auv_model_acados_get_sim_mem(auv_model_sim_solver_capsule *capsule) +{ + return capsule->acados_sim_mem; +}; + diff --git a/control/velocity_controller/src/build_auv_solver/acados_sim_solver_auv_model.h b/control/velocity_controller/src/build_auv_solver/acados_sim_solver_auv_model.h new file mode 100644 index 000000000..beed43e59 --- /dev/null +++ b/control/velocity_controller/src/build_auv_solver/acados_sim_solver_auv_model.h @@ -0,0 +1,105 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * 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. + * + * 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 HOLDER 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.; + */ + +#ifndef ACADOS_SIM_auv_model_H_ +#define ACADOS_SIM_auv_model_H_ + +#include "acados_c/sim_interface.h" +#include "acados_c/external_function_interface.h" + +#define AUV_MODEL_NX 9 +#define AUV_MODEL_NZ 0 +#define AUV_MODEL_NU 3 +#define AUV_MODEL_NP 0 + +#ifdef __cplusplus +extern "C" { +#endif + + +// ** capsule for solver data ** +typedef struct auv_model_sim_solver_capsule +{ + // acados objects + sim_in *acados_sim_in; + sim_out *acados_sim_out; + sim_solver *acados_sim_solver; + sim_opts *acados_sim_opts; + sim_config *acados_sim_config; + void *acados_sim_dims; + void *acados_sim_mem; + + /* external functions */ + // ERK + external_function_param_casadi * sim_expl_vde_forw; + external_function_param_casadi * sim_vde_adj_casadi; + external_function_param_casadi * sim_expl_ode_fun_casadi; + external_function_param_casadi * sim_expl_ode_hess; + external_function_param_casadi * sim_expl_vde_forw_p; + + // IRK + external_function_param_casadi * sim_impl_dae_fun; + external_function_param_casadi * sim_impl_dae_fun_jac_x_xdot_z; + external_function_param_casadi * sim_impl_dae_jac_x_xdot_u_z; + external_function_param_casadi * sim_impl_dae_hess; + external_function_param_casadi * sim_impl_dae_jac_p; + + // GNSF + external_function_param_casadi * sim_gnsf_phi_fun; + external_function_param_casadi * sim_gnsf_phi_fun_jac_y; + external_function_param_casadi * sim_gnsf_phi_jac_y_uhat; + external_function_param_casadi * sim_gnsf_f_lo_jac_x1_x1dot_u_z; + external_function_param_casadi * sim_gnsf_get_matrices_fun; + +} auv_model_sim_solver_capsule; + + +ACADOS_SYMBOL_EXPORT int auv_model_acados_sim_create(auv_model_sim_solver_capsule *capsule); +ACADOS_SYMBOL_EXPORT int auv_model_acados_sim_solve(auv_model_sim_solver_capsule *capsule); + +ACADOS_SYMBOL_EXPORT int auv_model_acados_sim_free(auv_model_sim_solver_capsule *capsule); +ACADOS_SYMBOL_EXPORT int auv_model_acados_sim_update_params(auv_model_sim_solver_capsule *capsule, double *value, int np); + +ACADOS_SYMBOL_EXPORT sim_config * auv_model_acados_get_sim_config(auv_model_sim_solver_capsule *capsule); +ACADOS_SYMBOL_EXPORT sim_in * auv_model_acados_get_sim_in(auv_model_sim_solver_capsule *capsule); +ACADOS_SYMBOL_EXPORT sim_out * auv_model_acados_get_sim_out(auv_model_sim_solver_capsule *capsule); +ACADOS_SYMBOL_EXPORT void * auv_model_acados_get_sim_dims(auv_model_sim_solver_capsule *capsule); +ACADOS_SYMBOL_EXPORT sim_opts * auv_model_acados_get_sim_opts(auv_model_sim_solver_capsule *capsule); +ACADOS_SYMBOL_EXPORT sim_solver * auv_model_acados_get_sim_solver(auv_model_sim_solver_capsule *capsule); +ACADOS_SYMBOL_EXPORT void * auv_model_acados_get_sim_mem(auv_model_sim_solver_capsule *capsule); + +ACADOS_SYMBOL_EXPORT auv_model_sim_solver_capsule * auv_model_acados_sim_solver_create_capsule(void); +ACADOS_SYMBOL_EXPORT int auv_model_acados_sim_solver_free_capsule(auv_model_sim_solver_capsule *capsule); + +#ifdef __cplusplus +} +#endif + +#endif // ACADOS_SIM_auv_model_H_ diff --git a/control/velocity_controller/src/build_auv_solver/acados_solver.pxd b/control/velocity_controller/src/build_auv_solver/acados_solver.pxd new file mode 100644 index 000000000..a6a039341 --- /dev/null +++ b/control/velocity_controller/src/build_auv_solver/acados_solver.pxd @@ -0,0 +1,63 @@ +# +# Copyright (c) The acados authors. +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# 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. +# +# 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 HOLDER 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.; +# + +cimport acados_solver_common + +cdef extern from "acados_solver_auv_model.h": + ctypedef struct nlp_solver_capsule "auv_model_solver_capsule": + pass + + nlp_solver_capsule * acados_create_capsule "auv_model_acados_create_capsule"() + int acados_free_capsule "auv_model_acados_free_capsule"(nlp_solver_capsule *capsule) + + int acados_create "auv_model_acados_create"(nlp_solver_capsule * capsule) + + int acados_create_with_discretization "auv_model_acados_create_with_discretization"(nlp_solver_capsule * capsule, int n_time_steps, double* new_time_steps) + int acados_update_time_steps "auv_model_acados_update_time_steps"(nlp_solver_capsule * capsule, int N, double* new_time_steps) + int acados_update_qp_solver_cond_N "auv_model_acados_update_qp_solver_cond_N"(nlp_solver_capsule * capsule, int qp_solver_cond_N) + + int acados_update_params "auv_model_acados_update_params"(nlp_solver_capsule * capsule, int stage, double *value, int np_) + int acados_update_params_sparse "auv_model_acados_update_params_sparse"(nlp_solver_capsule * capsule, int stage, int *idx, double *p, int n_update) + int acados_set_p_global_and_precompute_dependencies "auv_model_acados_set_p_global_and_precompute_dependencies"(nlp_solver_capsule * capsule, double *value, int data_len) + int acados_solve "auv_model_acados_solve"(nlp_solver_capsule * capsule) + int acados_reset "auv_model_acados_reset"(nlp_solver_capsule * capsule, int reset_qp_solver_mem) + int acados_free "auv_model_acados_free"(nlp_solver_capsule * capsule) + void acados_print_stats "auv_model_acados_print_stats"(nlp_solver_capsule * capsule) + + int acados_custom_update "auv_model_acados_custom_update"(nlp_solver_capsule* capsule, double * data, int data_len) + + acados_solver_common.ocp_nlp_in *acados_get_nlp_in "auv_model_acados_get_nlp_in"(nlp_solver_capsule * capsule) + acados_solver_common.ocp_nlp_out *acados_get_nlp_out "auv_model_acados_get_nlp_out"(nlp_solver_capsule * capsule) + acados_solver_common.ocp_nlp_out *acados_get_sens_out "auv_model_acados_get_sens_out"(nlp_solver_capsule * capsule) + acados_solver_common.ocp_nlp_solver *acados_get_nlp_solver "auv_model_acados_get_nlp_solver"(nlp_solver_capsule * capsule) + acados_solver_common.ocp_nlp_config *acados_get_nlp_config "auv_model_acados_get_nlp_config"(nlp_solver_capsule * capsule) + void *acados_get_nlp_opts "auv_model_acados_get_nlp_opts"(nlp_solver_capsule * capsule) + acados_solver_common.ocp_nlp_dims *acados_get_nlp_dims "auv_model_acados_get_nlp_dims"(nlp_solver_capsule * capsule) + acados_solver_common.ocp_nlp_plan *acados_get_nlp_plan "auv_model_acados_get_nlp_plan"(nlp_solver_capsule * capsule) diff --git a/control/velocity_controller/src/build_auv_solver/acados_solver_auv_model.c b/control/velocity_controller/src/build_auv_solver/acados_solver_auv_model.c new file mode 100644 index 000000000..6cce3247e --- /dev/null +++ b/control/velocity_controller/src/build_auv_solver/acados_solver_auv_model.c @@ -0,0 +1,1198 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * 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. + * + * 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 HOLDER 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.; + */ + +// standard +#include +#include +#include +// acados +// #include "acados/utils/print.h" +#include "acados_c/ocp_nlp_interface.h" +#include "acados_c/external_function_interface.h" + +// example specific + +#include "auv_model_model/auv_model_model.h" + + + + + +#include "acados_solver_auv_model.h" + +#define NX AUV_MODEL_NX +#define NZ AUV_MODEL_NZ +#define NU AUV_MODEL_NU +#define NP AUV_MODEL_NP +#define NP_GLOBAL AUV_MODEL_NP_GLOBAL +#define NY0 AUV_MODEL_NY0 +#define NY AUV_MODEL_NY +#define NYN AUV_MODEL_NYN + +#define NBX AUV_MODEL_NBX +#define NBX0 AUV_MODEL_NBX0 +#define NBU AUV_MODEL_NBU +#define NG AUV_MODEL_NG +#define NBXN AUV_MODEL_NBXN +#define NGN AUV_MODEL_NGN + +#define NH AUV_MODEL_NH +#define NHN AUV_MODEL_NHN +#define NH0 AUV_MODEL_NH0 +#define NPHI AUV_MODEL_NPHI +#define NPHIN AUV_MODEL_NPHIN +#define NPHI0 AUV_MODEL_NPHI0 +#define NR AUV_MODEL_NR + +#define NS AUV_MODEL_NS +#define NS0 AUV_MODEL_NS0 +#define NSN AUV_MODEL_NSN + +#define NSBX AUV_MODEL_NSBX +#define NSBU AUV_MODEL_NSBU +#define NSH0 AUV_MODEL_NSH0 +#define NSH AUV_MODEL_NSH +#define NSHN AUV_MODEL_NSHN +#define NSG AUV_MODEL_NSG +#define NSPHI0 AUV_MODEL_NSPHI0 +#define NSPHI AUV_MODEL_NSPHI +#define NSPHIN AUV_MODEL_NSPHIN +#define NSGN AUV_MODEL_NSGN +#define NSBXN AUV_MODEL_NSBXN + + + +// ** solver data ** + +auv_model_solver_capsule * auv_model_acados_create_capsule(void) +{ + void* capsule_mem = malloc(sizeof(auv_model_solver_capsule)); + auv_model_solver_capsule *capsule = (auv_model_solver_capsule *) capsule_mem; + + return capsule; +} + + +int auv_model_acados_free_capsule(auv_model_solver_capsule *capsule) +{ + free(capsule); + return 0; +} + + +int auv_model_acados_create(auv_model_solver_capsule* capsule) +{ + int N_shooting_intervals = AUV_MODEL_N; + double* new_time_steps = NULL; // NULL -> don't alter the code generated time-steps + return auv_model_acados_create_with_discretization(capsule, N_shooting_intervals, new_time_steps); +} + + +int auv_model_acados_update_time_steps(auv_model_solver_capsule* capsule, int N, double* new_time_steps) +{ + + if (N != capsule->nlp_solver_plan->N) { + fprintf(stderr, "auv_model_acados_update_time_steps: given number of time steps (= %d) " \ + "differs from the currently allocated number of " \ + "time steps (= %d)!\n" \ + "Please recreate with new discretization and provide a new vector of time_stamps!\n", + N, capsule->nlp_solver_plan->N); + return 1; + } + + ocp_nlp_config * nlp_config = capsule->nlp_config; + ocp_nlp_dims * nlp_dims = capsule->nlp_dims; + ocp_nlp_in * nlp_in = capsule->nlp_in; + + for (int i = 0; i < N; i++) + { + ocp_nlp_in_set(nlp_config, nlp_dims, nlp_in, i, "Ts", &new_time_steps[i]); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "scaling", &new_time_steps[i]); + } + return 0; + +} + +/** + * Internal function for auv_model_acados_create: step 1 + */ +void auv_model_acados_create_set_plan(ocp_nlp_plan_t* nlp_solver_plan, const int N) +{ + assert(N == nlp_solver_plan->N); + + /************************************************ + * plan + ************************************************/ + + nlp_solver_plan->nlp_solver = SQP; + + nlp_solver_plan->ocp_qp_solver_plan.qp_solver = FULL_CONDENSING_HPIPM; + nlp_solver_plan->relaxed_ocp_qp_solver_plan.qp_solver = FULL_CONDENSING_HPIPM; + nlp_solver_plan->nlp_cost[0] = LINEAR_LS; + for (int i = 1; i < N; i++) + nlp_solver_plan->nlp_cost[i] = LINEAR_LS; + + nlp_solver_plan->nlp_cost[N] = LINEAR_LS; + + for (int i = 0; i < N; i++) + { + nlp_solver_plan->nlp_dynamics[i] = CONTINUOUS_MODEL; + nlp_solver_plan->sim_solver_plan[i].sim_solver = ERK; + } + + nlp_solver_plan->nlp_constraints[0] = BGH; + + for (int i = 1; i < N; i++) + { + nlp_solver_plan->nlp_constraints[i] = BGH; + } + nlp_solver_plan->nlp_constraints[N] = BGH; + + nlp_solver_plan->regularization = NO_REGULARIZE; + + nlp_solver_plan->globalization = MERIT_BACKTRACKING; +} + + +static ocp_nlp_dims* auv_model_acados_create_setup_dimensions(auv_model_solver_capsule* capsule) +{ + ocp_nlp_plan_t* nlp_solver_plan = capsule->nlp_solver_plan; + const int N = nlp_solver_plan->N; + ocp_nlp_config* nlp_config = capsule->nlp_config; + + /************************************************ + * dimensions + ************************************************/ + #define NINTNP1MEMS 18 + int* intNp1mem = (int*)malloc( (N+1)*sizeof(int)*NINTNP1MEMS ); + + int* nx = intNp1mem + (N+1)*0; + int* nu = intNp1mem + (N+1)*1; + int* nbx = intNp1mem + (N+1)*2; + int* nbu = intNp1mem + (N+1)*3; + int* nsbx = intNp1mem + (N+1)*4; + int* nsbu = intNp1mem + (N+1)*5; + int* nsg = intNp1mem + (N+1)*6; + int* nsh = intNp1mem + (N+1)*7; + int* nsphi = intNp1mem + (N+1)*8; + int* ns = intNp1mem + (N+1)*9; + int* ng = intNp1mem + (N+1)*10; + int* nh = intNp1mem + (N+1)*11; + int* nphi = intNp1mem + (N+1)*12; + int* nz = intNp1mem + (N+1)*13; + int* ny = intNp1mem + (N+1)*14; + int* nr = intNp1mem + (N+1)*15; + int* nbxe = intNp1mem + (N+1)*16; + int* np = intNp1mem + (N+1)*17; + + for (int i = 0; i < N+1; i++) + { + // common + nx[i] = NX; + nu[i] = NU; + nz[i] = NZ; + ns[i] = NS; + // cost + ny[i] = NY; + // constraints + nbx[i] = NBX; + nbu[i] = NBU; + nsbx[i] = NSBX; + nsbu[i] = NSBU; + nsg[i] = NSG; + nsh[i] = NSH; + nsphi[i] = NSPHI; + ng[i] = NG; + nh[i] = NH; + nphi[i] = NPHI; + nr[i] = NR; + nbxe[i] = 0; + np[i] = NP; + } + + // for initial state + nbx[0] = NBX0; + nsbx[0] = 0; + ns[0] = NS0; + + nbxe[0] = 9; + + ny[0] = NY0; + nh[0] = NH0; + nsh[0] = NSH0; + nsphi[0] = NSPHI0; + nphi[0] = NPHI0; + + + // terminal - common + nu[N] = 0; + nz[N] = 0; + ns[N] = NSN; + // cost + ny[N] = NYN; + // constraint + nbx[N] = NBXN; + nbu[N] = 0; + ng[N] = NGN; + nh[N] = NHN; + nphi[N] = NPHIN; + nr[N] = 0; + + nsbx[N] = NSBXN; + nsbu[N] = 0; + nsg[N] = NSGN; + nsh[N] = NSHN; + nsphi[N] = NSPHIN; + + /* create and set ocp_nlp_dims */ + ocp_nlp_dims * nlp_dims = ocp_nlp_dims_create(nlp_config); + + ocp_nlp_dims_set_opt_vars(nlp_config, nlp_dims, "nx", nx); + ocp_nlp_dims_set_opt_vars(nlp_config, nlp_dims, "nu", nu); + ocp_nlp_dims_set_opt_vars(nlp_config, nlp_dims, "nz", nz); + ocp_nlp_dims_set_opt_vars(nlp_config, nlp_dims, "ns", ns); + ocp_nlp_dims_set_opt_vars(nlp_config, nlp_dims, "np", np); + + ocp_nlp_dims_set_global(nlp_config, nlp_dims, "np_global", 0); + ocp_nlp_dims_set_global(nlp_config, nlp_dims, "n_global_data", 0); + + for (int i = 0; i <= N; i++) + { + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nbx", &nbx[i]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nbu", &nbu[i]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nsbx", &nsbx[i]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nsbu", &nsbu[i]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "ng", &ng[i]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nsg", &nsg[i]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nbxe", &nbxe[i]); + } + ocp_nlp_dims_set_cost(nlp_config, nlp_dims, 0, "ny", &ny[0]); + for (int i = 1; i < N; i++) + ocp_nlp_dims_set_cost(nlp_config, nlp_dims, i, "ny", &ny[i]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, 0, "nh", &nh[0]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, 0, "nsh", &nsh[0]); + + for (int i = 1; i < N; i++) + { + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nh", &nh[i]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nsh", &nsh[i]); + } + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, N, "nh", &nh[N]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, N, "nsh", &nsh[N]); + ocp_nlp_dims_set_cost(nlp_config, nlp_dims, N, "ny", &ny[N]); + free(intNp1mem); + + return nlp_dims; +} + + +/** + * Internal function for auv_model_acados_create: step 3 + */ +void auv_model_acados_create_setup_functions(auv_model_solver_capsule* capsule) +{ + const int N = capsule->nlp_solver_plan->N; + + /************************************************ + * external functions + ************************************************/ + +#define MAP_CASADI_FNC(__CAPSULE_FNC__, __MODEL_BASE_FNC__) do{ \ + capsule->__CAPSULE_FNC__.casadi_fun = & __MODEL_BASE_FNC__ ;\ + capsule->__CAPSULE_FNC__.casadi_n_in = & __MODEL_BASE_FNC__ ## _n_in; \ + capsule->__CAPSULE_FNC__.casadi_n_out = & __MODEL_BASE_FNC__ ## _n_out; \ + capsule->__CAPSULE_FNC__.casadi_sparsity_in = & __MODEL_BASE_FNC__ ## _sparsity_in; \ + capsule->__CAPSULE_FNC__.casadi_sparsity_out = & __MODEL_BASE_FNC__ ## _sparsity_out; \ + capsule->__CAPSULE_FNC__.casadi_work = & __MODEL_BASE_FNC__ ## _work; \ + external_function_external_param_casadi_create(&capsule->__CAPSULE_FNC__, &ext_fun_opts); \ + } while(false) + + external_function_opts ext_fun_opts; + external_function_opts_set_to_default(&ext_fun_opts); + + + ext_fun_opts.external_workspace = true; + if (N > 0) + { + + + + + // explicit ode + capsule->expl_vde_forw = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*N); + for (int i = 0; i < N; i++) { + MAP_CASADI_FNC(expl_vde_forw[i], auv_model_expl_vde_forw); + } + + + + capsule->expl_ode_fun = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*N); + for (int i = 0; i < N; i++) { + MAP_CASADI_FNC(expl_ode_fun[i], auv_model_expl_ode_fun); + } + + capsule->expl_vde_adj = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*N); + for (int i = 0; i < N; i++) { + MAP_CASADI_FNC(expl_vde_adj[i], auv_model_expl_vde_adj); + } + + + } // N > 0 + +#undef MAP_CASADI_FNC +} + + +/** + * Internal function for auv_model_acados_create: step 5 + */ +void auv_model_acados_create_set_default_parameters(auv_model_solver_capsule* capsule) +{ + + // no parameters defined + + + // no global parameters defined +} + + +/** + * Internal function for auv_model_acados_create: step 5 + */ +void auv_model_acados_setup_nlp_in(auv_model_solver_capsule* capsule, const int N, double* new_time_steps) +{ + assert(N == capsule->nlp_solver_plan->N); + ocp_nlp_config* nlp_config = capsule->nlp_config; + ocp_nlp_dims* nlp_dims = capsule->nlp_dims; + + int tmp_int = 0; + + /************************************************ + * nlp_in + ************************************************/ + ocp_nlp_in * nlp_in = capsule->nlp_in; + /************************************************ + * nlp_out + ************************************************/ + ocp_nlp_out * nlp_out = capsule->nlp_out; + + // set up time_steps and cost_scaling + + if (new_time_steps) + { + // NOTE: this sets scaling and time_steps + auv_model_acados_update_time_steps(capsule, N, new_time_steps); + } + else + { + // set time_steps + + double time_step = 0.1; + for (int i = 0; i < N; i++) + { + ocp_nlp_in_set(nlp_config, nlp_dims, nlp_in, i, "Ts", &time_step); + } + // set cost scaling + double* cost_scaling = malloc((N+1)*sizeof(double)); + cost_scaling[0] = 0.1; + cost_scaling[1] = 0.1; + cost_scaling[2] = 0.1; + cost_scaling[3] = 0.1; + cost_scaling[4] = 0.1; + cost_scaling[5] = 0.1; + cost_scaling[6] = 0.1; + cost_scaling[7] = 0.1; + cost_scaling[8] = 0.1; + cost_scaling[9] = 0.1; + cost_scaling[10] = 0.1; + cost_scaling[11] = 0.1; + cost_scaling[12] = 0.1; + cost_scaling[13] = 0.1; + cost_scaling[14] = 0.1; + cost_scaling[15] = 0.1; + cost_scaling[16] = 0.1; + cost_scaling[17] = 0.1; + cost_scaling[18] = 0.1; + cost_scaling[19] = 0.1; + cost_scaling[20] = 1; + for (int i = 0; i <= N; i++) + { + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "scaling", &cost_scaling[i]); + } + free(cost_scaling); + } + + + + /**** Dynamics ****/ + for (int i = 0; i < N; i++) + { + ocp_nlp_dynamics_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, i, "expl_vde_forw", &capsule->expl_vde_forw[i]); + + ocp_nlp_dynamics_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, i, "expl_ode_fun", &capsule->expl_ode_fun[i]); + ocp_nlp_dynamics_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, i, "expl_vde_adj", &capsule->expl_vde_adj[i]); + } + + /**** Cost ****/ + double* yref_0 = calloc(NY0, sizeof(double)); + // change only the non-zero elements: + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "yref", yref_0); + free(yref_0); + + double* W_0 = calloc(NY0*NY0, sizeof(double)); + // change only the non-zero elements: + W_0[0+(NY0) * 0] = 1; + W_0[4+(NY0) * 4] = 0.1; + W_0[5+(NY0) * 5] = 0.1; + W_0[7+(NY0) * 7] = 2; + W_0[8+(NY0) * 8] = 5; + W_0[9+(NY0) * 9] = 0.1; + W_0[10+(NY0) * 10] = 1; + W_0[11+(NY0) * 11] = 1; + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "W", W_0); + free(W_0); + double* Vx_0 = calloc(NY0*NX, sizeof(double)); + // change only the non-zero elements: + Vx_0[0+(NY0) * 0] = 1; + Vx_0[1+(NY0) * 1] = 1; + Vx_0[2+(NY0) * 2] = 1; + Vx_0[3+(NY0) * 3] = 1; + Vx_0[4+(NY0) * 4] = 1; + Vx_0[5+(NY0) * 5] = 1; + Vx_0[6+(NY0) * 6] = 1; + Vx_0[7+(NY0) * 7] = 1; + Vx_0[8+(NY0) * 8] = 1; + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "Vx", Vx_0); + free(Vx_0); + double* Vu_0 = calloc(NY0*NU, sizeof(double)); + // change only the non-zero elements: + Vu_0[9+(NY0) * 0] = 1; + Vu_0[10+(NY0) * 1] = 1; + Vu_0[11+(NY0) * 2] = 1; + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "Vu", Vu_0); + free(Vu_0); + double* yref = calloc(NY, sizeof(double)); + // change only the non-zero elements: + + for (int i = 1; i < N; i++) + { + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "yref", yref); + } + free(yref); + double* W = calloc(NY*NY, sizeof(double)); + // change only the non-zero elements: + W[0+(NY) * 0] = 1; + W[4+(NY) * 4] = 0.1; + W[5+(NY) * 5] = 0.1; + W[7+(NY) * 7] = 2; + W[8+(NY) * 8] = 5; + W[9+(NY) * 9] = 0.1; + W[10+(NY) * 10] = 1; + W[11+(NY) * 11] = 1; + + for (int i = 1; i < N; i++) + { + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "W", W); + } + free(W); + double* Vx = calloc(NY*NX, sizeof(double)); + // change only the non-zero elements: + Vx[0+(NY) * 0] = 1; + Vx[1+(NY) * 1] = 1; + Vx[2+(NY) * 2] = 1; + Vx[3+(NY) * 3] = 1; + Vx[4+(NY) * 4] = 1; + Vx[5+(NY) * 5] = 1; + Vx[6+(NY) * 6] = 1; + Vx[7+(NY) * 7] = 1; + Vx[8+(NY) * 8] = 1; + for (int i = 1; i < N; i++) + { + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "Vx", Vx); + } + free(Vx); + + + double* Vu = calloc(NY*NU, sizeof(double)); + // change only the non-zero elements: + Vu[9+(NY) * 0] = 1; + Vu[10+(NY) * 1] = 1; + Vu[11+(NY) * 2] = 1; + + for (int i = 1; i < N; i++) + { + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "Vu", Vu); + } + free(Vu); + double* yref_e = calloc(NYN, sizeof(double)); + // change only the non-zero elements: + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "yref", yref_e); + free(yref_e); + + double* W_e = calloc(NYN*NYN, sizeof(double)); + // change only the non-zero elements: + W_e[0+(NYN) * 0] = 1; + W_e[4+(NYN) * 4] = 0.1; + W_e[5+(NYN) * 5] = 0.1; + W_e[7+(NYN) * 7] = 2; + W_e[8+(NYN) * 8] = 5; + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "W", W_e); + free(W_e); + double* Vx_e = calloc(NYN*NX, sizeof(double)); + // change only the non-zero elements: + Vx_e[0+(NYN) * 0] = 1; + Vx_e[1+(NYN) * 1] = 1; + Vx_e[2+(NYN) * 2] = 1; + Vx_e[3+(NYN) * 3] = 1; + Vx_e[4+(NYN) * 4] = 1; + Vx_e[5+(NYN) * 5] = 1; + Vx_e[6+(NYN) * 6] = 1; + Vx_e[7+(NYN) * 7] = 1; + Vx_e[8+(NYN) * 8] = 1; + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "Vx", Vx_e); + free(Vx_e); + + + + + + + + /**** Constraints ****/ + + // bounds for initial stage + // x0 + int* idxbx0 = malloc(NBX0 * sizeof(int)); + idxbx0[0] = 0; + idxbx0[1] = 1; + idxbx0[2] = 2; + idxbx0[3] = 3; + idxbx0[4] = 4; + idxbx0[5] = 5; + idxbx0[6] = 6; + idxbx0[7] = 7; + idxbx0[8] = 8; + + double* lubx0 = calloc(2*NBX0, sizeof(double)); + double* lbx0 = lubx0; + double* ubx0 = lubx0 + NBX0; + // change only the non-zero elements: + + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "idxbx", idxbx0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "lbx", lbx0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "ubx", ubx0); + free(idxbx0); + free(lubx0); + // idxbxe_0 + int* idxbxe_0 = malloc(9 * sizeof(int)); + idxbxe_0[0] = 0; + idxbxe_0[1] = 1; + idxbxe_0[2] = 2; + idxbxe_0[3] = 3; + idxbxe_0[4] = 4; + idxbxe_0[5] = 5; + idxbxe_0[6] = 6; + idxbxe_0[7] = 7; + idxbxe_0[8] = 8; + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "idxbxe", idxbxe_0); + free(idxbxe_0); + + + + + + + + + + + + + /* constraints that are the same for initial and intermediate */ + // u + int* idxbu = malloc(NBU * sizeof(int)); + idxbu[0] = 0; + idxbu[1] = 1; + idxbu[2] = 2; + double* lubu = calloc(2*NBU, sizeof(double)); + double* lbu = lubu; + double* ubu = lubu + NBU; + lbu[0] = -99.5; + ubu[0] = 99.5; + lbu[1] = -99.5; + ubu[1] = 99.5; + lbu[2] = -99.5; + ubu[2] = 99.5; + + for (int i = 0; i < N; i++) + { + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxbu", idxbu); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lbu", lbu); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "ubu", ubu); + } + free(idxbu); + free(lubu); + + + + + + + /* Path constraints */ + + + + + + + + + + + + + + + /* terminal constraints */ + + + + + + + + + + + + + + + + + + + + +} + + +static void auv_model_acados_create_set_opts(auv_model_solver_capsule* capsule) +{ + const int N = capsule->nlp_solver_plan->N; + ocp_nlp_config* nlp_config = capsule->nlp_config; + void *nlp_opts = capsule->nlp_opts; + + /************************************************ + * opts + ************************************************/ + + + + int fixed_hess = 0; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "fixed_hess", &fixed_hess); + double globalization_alpha_min = 0.05; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "globalization_alpha_min", &globalization_alpha_min); + + double globalization_alpha_reduction = 0.7; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "globalization_alpha_reduction", &globalization_alpha_reduction); + + + + int globalization_line_search_use_sufficient_descent = 0; + ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "globalization_line_search_use_sufficient_descent", &globalization_line_search_use_sufficient_descent); + + int globalization_use_SOC = 0; + ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "globalization_use_SOC", &globalization_use_SOC); + + double globalization_eps_sufficient_descent = 0.0001; + ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "globalization_eps_sufficient_descent", &globalization_eps_sufficient_descent); + + int with_solution_sens_wrt_params = false; + ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "with_solution_sens_wrt_params", &with_solution_sens_wrt_params); + + int with_value_sens_wrt_params = false; + ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "with_value_sens_wrt_params", &with_value_sens_wrt_params); + + double solution_sens_qp_t_lam_min = 0.000000001; + ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "solution_sens_qp_t_lam_min", &solution_sens_qp_t_lam_min); + + int globalization_full_step_dual = 0; + ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "globalization_full_step_dual", &globalization_full_step_dual); + + // set collocation type (relevant for implicit integrators) + sim_collocation_type collocation_type = GAUSS_LEGENDRE; + for (int i = 0; i < N; i++) + ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_collocation_type", &collocation_type); + + // set up sim_method_num_steps + // all sim_method_num_steps are identical + int sim_method_num_steps = 2; + for (int i = 0; i < N; i++) + ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_num_steps", &sim_method_num_steps); + + // set up sim_method_num_stages + // all sim_method_num_stages are identical + int sim_method_num_stages = 4; + for (int i = 0; i < N; i++) + ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_num_stages", &sim_method_num_stages); + + int newton_iter_val = 3; + for (int i = 0; i < N; i++) + ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_newton_iter", &newton_iter_val); + + double newton_tol_val = 0; + for (int i = 0; i < N; i++) + ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_newton_tol", &newton_tol_val); + + // set up sim_method_jac_reuse + bool tmp_bool = (bool) 0; + for (int i = 0; i < N; i++) + ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_jac_reuse", &tmp_bool); + + double levenberg_marquardt = 0.0001; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "levenberg_marquardt", &levenberg_marquardt); + + /* options QP solver */ + + int nlp_solver_ext_qp_res = 0; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "ext_qp_res", &nlp_solver_ext_qp_res); + + bool store_iterates = false; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "store_iterates", &store_iterates); + int log_primal_step_norm = false; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "log_primal_step_norm", &log_primal_step_norm); + + int log_dual_step_norm = false; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "log_dual_step_norm", &log_dual_step_norm); + + double nlp_solver_tol_min_step_norm = 0; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "tol_min_step_norm", &nlp_solver_tol_min_step_norm); + // set HPIPM mode: should be done before setting other QP solver options + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_hpipm_mode", "BALANCE"); + + + + int qp_solver_t0_init = 2; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_t0_init", &qp_solver_t0_init); + + + + + // set SQP specific options + double nlp_solver_tol_stat = 0.000001; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "tol_stat", &nlp_solver_tol_stat); + + double nlp_solver_tol_eq = 0.000001; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "tol_eq", &nlp_solver_tol_eq); + + double nlp_solver_tol_ineq = 0.000001; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "tol_ineq", &nlp_solver_tol_ineq); + + double nlp_solver_tol_comp = 0.000001; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "tol_comp", &nlp_solver_tol_comp); + + int nlp_solver_max_iter = 100; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "max_iter", &nlp_solver_max_iter); + + // set options for adaptive Levenberg-Marquardt Update + bool with_adaptive_levenberg_marquardt = false; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "with_adaptive_levenberg_marquardt", &with_adaptive_levenberg_marquardt); + + double adaptive_levenberg_marquardt_lam = 5; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "adaptive_levenberg_marquardt_lam", &adaptive_levenberg_marquardt_lam); + + double adaptive_levenberg_marquardt_mu_min = 0.0000000000000001; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "adaptive_levenberg_marquardt_mu_min", &adaptive_levenberg_marquardt_mu_min); + + double adaptive_levenberg_marquardt_mu0 = 0.001; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "adaptive_levenberg_marquardt_mu0", &adaptive_levenberg_marquardt_mu0); + + double adaptive_levenberg_marquardt_obj_scalar = 2; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "adaptive_levenberg_marquardt_obj_scalar", &adaptive_levenberg_marquardt_obj_scalar); + + bool eval_residual_at_max_iter = false; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "eval_residual_at_max_iter", &eval_residual_at_max_iter); + + // QP scaling + double qpscaling_ub_max_abs_eig = 100000; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qpscaling_ub_max_abs_eig", &qpscaling_ub_max_abs_eig); + + double qpscaling_lb_norm_inf_grad_obj = 0.0001; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qpscaling_lb_norm_inf_grad_obj", &qpscaling_lb_norm_inf_grad_obj); + + qpscaling_scale_objective_type qpscaling_scale_objective = NO_OBJECTIVE_SCALING; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qpscaling_scale_objective", &qpscaling_scale_objective); + + ocp_nlp_qpscaling_constraint_type qpscaling_scale_constraints = NO_CONSTRAINT_SCALING; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qpscaling_scale_constraints", &qpscaling_scale_constraints); + + // NLP QP tol strategy + ocp_nlp_qp_tol_strategy_t nlp_qp_tol_strategy = FIXED_QP_TOL; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_strategy", &nlp_qp_tol_strategy); + + double nlp_qp_tol_reduction_factor = 0.1; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_reduction_factor", &nlp_qp_tol_reduction_factor); + + double nlp_qp_tol_safety_factor = 0.1; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_safety_factor", &nlp_qp_tol_safety_factor); + + double nlp_qp_tol_min_stat = 0.000000001; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_min_stat", &nlp_qp_tol_min_stat); + + double nlp_qp_tol_min_eq = 0.0000000001; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_min_eq", &nlp_qp_tol_min_eq); + + double nlp_qp_tol_min_ineq = 0.0000000001; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_min_ineq", &nlp_qp_tol_min_ineq); + + double nlp_qp_tol_min_comp = 0.00000000001; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_min_comp", &nlp_qp_tol_min_comp); + + bool with_anderson_acceleration = false; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "with_anderson_acceleration", &with_anderson_acceleration); + + double anderson_activation_threshold = 10; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "anderson_activation_threshold", &anderson_activation_threshold); + + int qp_solver_iter_max = 50; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_iter_max", &qp_solver_iter_max); + + + int qp_solver_warm_start = 1; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_warm_start", &qp_solver_warm_start); + + int print_level = 2; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "print_level", &print_level); + + int ext_cost_num_hess = 0; +} + + +/** + * Internal function for auv_model_acados_create: step 7 + */ +void auv_model_acados_set_nlp_out(auv_model_solver_capsule* capsule) +{ + const int N = capsule->nlp_solver_plan->N; + ocp_nlp_config* nlp_config = capsule->nlp_config; + ocp_nlp_dims* nlp_dims = capsule->nlp_dims; + ocp_nlp_out* nlp_out = capsule->nlp_out; + ocp_nlp_in* nlp_in = capsule->nlp_in; + + // initialize primal solution + double* xu0 = calloc(NX+NU, sizeof(double)); + double* x0 = xu0; + + // initialize with x0 + + + double* u0 = xu0 + NX; + + for (int i = 0; i < N; i++) + { + // x0 + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, i, "x", x0); + // u0 + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, i, "u", u0); + } + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, N, "x", x0); + free(xu0); +} + + +/** + * Internal function for auv_model_acados_create: step 9 + */ +int auv_model_acados_create_precompute(auv_model_solver_capsule* capsule) { + int status = ocp_nlp_precompute(capsule->nlp_solver, capsule->nlp_in, capsule->nlp_out); + + if (status != ACADOS_SUCCESS) { + printf("\nocp_nlp_precompute failed!\n\n"); + exit(1); + } + + return status; +} + + +int auv_model_acados_create_with_discretization(auv_model_solver_capsule* capsule, int N, double* new_time_steps) +{ + // If N does not match the number of shooting intervals used for code generation, new_time_steps must be given. + if (N != AUV_MODEL_N && !new_time_steps) { + fprintf(stderr, "auv_model_acados_create_with_discretization: new_time_steps is NULL " \ + "but the number of shooting intervals (= %d) differs from the number of " \ + "shooting intervals (= %d) during code generation! Please provide a new vector of time_stamps!\n", \ + N, AUV_MODEL_N); + return 1; + } + + // number of expected runtime parameters + capsule->nlp_np = NP; + + // 1) create and set nlp_solver_plan; create nlp_config + capsule->nlp_solver_plan = ocp_nlp_plan_create(N); + auv_model_acados_create_set_plan(capsule->nlp_solver_plan, N); + capsule->nlp_config = ocp_nlp_config_create(*capsule->nlp_solver_plan); + + // 2) create and set dimensions + capsule->nlp_dims = auv_model_acados_create_setup_dimensions(capsule); + + // 3) create and set nlp_opts + capsule->nlp_opts = ocp_nlp_solver_opts_create(capsule->nlp_config, capsule->nlp_dims); + auv_model_acados_create_set_opts(capsule); + + // 4) create and set nlp_out + // 4.1) nlp_out + capsule->nlp_out = ocp_nlp_out_create(capsule->nlp_config, capsule->nlp_dims); + // 4.2) sens_out + capsule->sens_out = ocp_nlp_out_create(capsule->nlp_config, capsule->nlp_dims); + auv_model_acados_set_nlp_out(capsule); + + // 5) create nlp_in + capsule->nlp_in = ocp_nlp_in_create(capsule->nlp_config, capsule->nlp_dims); + + // 6) setup functions, nlp_in and default parameters + auv_model_acados_create_setup_functions(capsule); + auv_model_acados_setup_nlp_in(capsule, N, new_time_steps); + auv_model_acados_create_set_default_parameters(capsule); + + // 7) create solver + capsule->nlp_solver = ocp_nlp_solver_create(capsule->nlp_config, capsule->nlp_dims, capsule->nlp_opts, capsule->nlp_in); + + + // 8) do precomputations + int status = auv_model_acados_create_precompute(capsule); + + return status; +} + +/** + * This function is for updating an already initialized solver with a different number of qp_cond_N. It is useful for code reuse after code export. + */ +int auv_model_acados_update_qp_solver_cond_N(auv_model_solver_capsule* capsule, int qp_solver_cond_N) +{ + printf("\nacados_update_qp_solver_cond_N() not implemented, since no partial condensing solver is used!\n\n"); + exit(1); +} + + +int auv_model_acados_reset(auv_model_solver_capsule* capsule, int reset_qp_solver_mem) +{ + + // set initialization to all zeros + + const int N = capsule->nlp_solver_plan->N; + ocp_nlp_config* nlp_config = capsule->nlp_config; + ocp_nlp_dims* nlp_dims = capsule->nlp_dims; + ocp_nlp_out* nlp_out = capsule->nlp_out; + ocp_nlp_in* nlp_in = capsule->nlp_in; + ocp_nlp_solver* nlp_solver = capsule->nlp_solver; + + double* buffer = calloc(NX+NU+NZ+2*NS+2*NSN+2*NS0+NBX+NBU+NG+NH+NPHI+NBX0+NBXN+NHN+NH0+NPHIN+NGN, sizeof(double)); + + for(int i=0; inlp_config, capsule->nlp_dims, capsule->nlp_in, stage, "parameter_values", p); + + return solver_status; +} + + +int auv_model_acados_update_params_sparse(auv_model_solver_capsule * capsule, int stage, int *idx, double *p, int n_update) +{ + ocp_nlp_in_set_params_sparse(capsule->nlp_config, capsule->nlp_dims, capsule->nlp_in, stage, idx, p, n_update); + + return 0; +} + + +int auv_model_acados_set_p_global_and_precompute_dependencies(auv_model_solver_capsule* capsule, double* data, int data_len) +{ + + // printf("No global_data, auv_model_acados_set_p_global_and_precompute_dependencies does nothing.\n"); + return 0; +} + + + + +int auv_model_acados_solve(auv_model_solver_capsule* capsule) +{ + // solve NLP + int solver_status = ocp_nlp_solve(capsule->nlp_solver, capsule->nlp_in, capsule->nlp_out); + + return solver_status; +} + + + +int auv_model_acados_setup_qp_matrices_and_factorize(auv_model_solver_capsule* capsule) +{ + int solver_status = ocp_nlp_setup_qp_matrices_and_factorize(capsule->nlp_solver, capsule->nlp_in, capsule->nlp_out); + + return solver_status; +} + + + + + + +int auv_model_acados_free(auv_model_solver_capsule* capsule) +{ + // before destroying, keep some info + const int N = capsule->nlp_solver_plan->N; + // free memory + ocp_nlp_solver_opts_destroy(capsule->nlp_opts); + ocp_nlp_in_destroy(capsule->nlp_in); + ocp_nlp_out_destroy(capsule->nlp_out); + ocp_nlp_out_destroy(capsule->sens_out); + ocp_nlp_solver_destroy(capsule->nlp_solver); + ocp_nlp_dims_destroy(capsule->nlp_dims); + ocp_nlp_config_destroy(capsule->nlp_config); + ocp_nlp_plan_destroy(capsule->nlp_solver_plan); + + /* free external function */ + // dynamics + for (int i = 0; i < N; i++) + { + external_function_external_param_casadi_free(&capsule->expl_vde_forw[i]); + + external_function_external_param_casadi_free(&capsule->expl_ode_fun[i]); + external_function_external_param_casadi_free(&capsule->expl_vde_adj[i]); + } + free(capsule->expl_vde_adj); + free(capsule->expl_vde_forw); + + free(capsule->expl_ode_fun); + + // cost + + // constraints + + + + return 0; +} + + +void auv_model_acados_print_stats(auv_model_solver_capsule* capsule) +{ + int nlp_iter, stat_m, stat_n, tmp_int; + ocp_nlp_get(capsule->nlp_solver, "nlp_iter", &nlp_iter); + ocp_nlp_get(capsule->nlp_solver, "stat_n", &stat_n); + ocp_nlp_get(capsule->nlp_solver, "stat_m", &stat_m); + + + int stat_n_max = 16; + if (stat_n > stat_n_max) + { + printf("stat_n_max = %d is too small, increase it in the template!\n", stat_n_max); + exit(1); + } + double stat[1616]; + ocp_nlp_get(capsule->nlp_solver, "statistics", stat); + + int nrow = nlp_iter+1 < stat_m ? nlp_iter+1 : stat_m; + + + printf("iter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter\talpha"); + if (stat_n > 8) + printf("\t\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp"); + printf("\n"); + for (int i = 0; i < nrow; i++) + { + for (int j = 0; j < stat_n + 1; j++) + { + if (j == 0 || j == 5 || j == 6) + { + tmp_int = (int) stat[i + j * nrow]; + printf("%d\t", tmp_int); + } + else + { + printf("%e\t", stat[i + j * nrow]); + } + } + printf("\n"); + } +} + +int auv_model_acados_custom_update(auv_model_solver_capsule* capsule, double* data, int data_len) +{ + (void)capsule; + (void)data; + (void)data_len; + printf("\ndummy function that can be called in between solver calls to update parameters or numerical data efficiently in C.\n"); + printf("nothing set yet..\n"); + return 1; + +} + + + +ocp_nlp_in *auv_model_acados_get_nlp_in(auv_model_solver_capsule* capsule) { return capsule->nlp_in; } +ocp_nlp_out *auv_model_acados_get_nlp_out(auv_model_solver_capsule* capsule) { return capsule->nlp_out; } +ocp_nlp_out *auv_model_acados_get_sens_out(auv_model_solver_capsule* capsule) { return capsule->sens_out; } +ocp_nlp_solver *auv_model_acados_get_nlp_solver(auv_model_solver_capsule* capsule) { return capsule->nlp_solver; } +ocp_nlp_config *auv_model_acados_get_nlp_config(auv_model_solver_capsule* capsule) { return capsule->nlp_config; } +void *auv_model_acados_get_nlp_opts(auv_model_solver_capsule* capsule) { return capsule->nlp_opts; } +ocp_nlp_dims *auv_model_acados_get_nlp_dims(auv_model_solver_capsule* capsule) { return capsule->nlp_dims; } +ocp_nlp_plan_t *auv_model_acados_get_nlp_plan(auv_model_solver_capsule* capsule) { return capsule->nlp_solver_plan; } diff --git a/control/velocity_controller/src/build_auv_solver/acados_solver_auv_model.h b/control/velocity_controller/src/build_auv_solver/acados_solver_auv_model.h new file mode 100644 index 000000000..73ddf4ca8 --- /dev/null +++ b/control/velocity_controller/src/build_auv_solver/acados_solver_auv_model.h @@ -0,0 +1,174 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * 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. + * + * 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 HOLDER 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.; + */ + +#ifndef ACADOS_SOLVER_auv_model_H_ +#define ACADOS_SOLVER_auv_model_H_ + +#include "acados/utils/types.h" + +#include "acados_c/ocp_nlp_interface.h" +#include "acados_c/external_function_interface.h" + +#define AUV_MODEL_NX 9 +#define AUV_MODEL_NZ 0 +#define AUV_MODEL_NU 3 +#define AUV_MODEL_NP 0 +#define AUV_MODEL_NP_GLOBAL 0 +#define AUV_MODEL_NBX 0 +#define AUV_MODEL_NBX0 9 +#define AUV_MODEL_NBU 3 +#define AUV_MODEL_NSBX 0 +#define AUV_MODEL_NSBU 0 +#define AUV_MODEL_NSH 0 +#define AUV_MODEL_NSH0 0 +#define AUV_MODEL_NSG 0 +#define AUV_MODEL_NSPHI 0 +#define AUV_MODEL_NSHN 0 +#define AUV_MODEL_NSGN 0 +#define AUV_MODEL_NSPHIN 0 +#define AUV_MODEL_NSPHI0 0 +#define AUV_MODEL_NSBXN 0 +#define AUV_MODEL_NS 0 +#define AUV_MODEL_NS0 0 +#define AUV_MODEL_NSN 0 +#define AUV_MODEL_NG 0 +#define AUV_MODEL_NBXN 0 +#define AUV_MODEL_NGN 0 +#define AUV_MODEL_NY0 12 +#define AUV_MODEL_NY 12 +#define AUV_MODEL_NYN 9 +#define AUV_MODEL_N 20 +#define AUV_MODEL_NH 0 +#define AUV_MODEL_NHN 0 +#define AUV_MODEL_NH0 0 +#define AUV_MODEL_NPHI0 0 +#define AUV_MODEL_NPHI 0 +#define AUV_MODEL_NPHIN 0 +#define AUV_MODEL_NR 0 + +#ifdef __cplusplus +extern "C" { +#endif + + +// ** capsule for solver data ** +typedef struct auv_model_solver_capsule +{ + // acados objects + ocp_nlp_in *nlp_in; + ocp_nlp_out *nlp_out; + ocp_nlp_out *sens_out; + ocp_nlp_solver *nlp_solver; + void *nlp_opts; + ocp_nlp_plan_t *nlp_solver_plan; + ocp_nlp_config *nlp_config; + ocp_nlp_dims *nlp_dims; + + // number of expected runtime parameters + unsigned int nlp_np; + + /* external functions */ + + // dynamics + + external_function_external_param_casadi *expl_vde_forw; + external_function_external_param_casadi *expl_vde_forw_p; + external_function_external_param_casadi *expl_ode_fun; + external_function_external_param_casadi *expl_vde_adj; + + + + + // cost + + + + + + + // constraints + + + + + + + +} auv_model_solver_capsule; + +ACADOS_SYMBOL_EXPORT auv_model_solver_capsule * auv_model_acados_create_capsule(void); +ACADOS_SYMBOL_EXPORT int auv_model_acados_free_capsule(auv_model_solver_capsule *capsule); + +ACADOS_SYMBOL_EXPORT int auv_model_acados_create(auv_model_solver_capsule * capsule); + +ACADOS_SYMBOL_EXPORT int auv_model_acados_reset(auv_model_solver_capsule* capsule, int reset_qp_solver_mem); + +/** + * Generic version of auv_model_acados_create which allows to use a different number of shooting intervals than + * the number used for code generation. If new_time_steps=NULL and n_time_steps matches the number used for code + * generation, the time-steps from code generation is used. + */ +ACADOS_SYMBOL_EXPORT int auv_model_acados_create_with_discretization(auv_model_solver_capsule * capsule, int n_time_steps, double* new_time_steps); +/** + * Update the time step vector. Number N must be identical to the currently set number of shooting nodes in the + * nlp_solver_plan. Returns 0 if no error occurred and a otherwise a value other than 0. + */ +ACADOS_SYMBOL_EXPORT int auv_model_acados_update_time_steps(auv_model_solver_capsule * capsule, int N, double* new_time_steps); +/** + * This function is used for updating an already initialized solver with a different number of qp_cond_N. + */ +ACADOS_SYMBOL_EXPORT int auv_model_acados_update_qp_solver_cond_N(auv_model_solver_capsule * capsule, int qp_solver_cond_N); +ACADOS_SYMBOL_EXPORT int auv_model_acados_update_params(auv_model_solver_capsule * capsule, int stage, double *value, int np); +ACADOS_SYMBOL_EXPORT int auv_model_acados_update_params_sparse(auv_model_solver_capsule * capsule, int stage, int *idx, double *p, int n_update); +ACADOS_SYMBOL_EXPORT int auv_model_acados_set_p_global_and_precompute_dependencies(auv_model_solver_capsule* capsule, double* data, int data_len); + +ACADOS_SYMBOL_EXPORT int auv_model_acados_solve(auv_model_solver_capsule * capsule); +ACADOS_SYMBOL_EXPORT int auv_model_acados_setup_qp_matrices_and_factorize(auv_model_solver_capsule* capsule); + + + +ACADOS_SYMBOL_EXPORT int auv_model_acados_free(auv_model_solver_capsule * capsule); +ACADOS_SYMBOL_EXPORT void auv_model_acados_print_stats(auv_model_solver_capsule * capsule); +ACADOS_SYMBOL_EXPORT int auv_model_acados_custom_update(auv_model_solver_capsule* capsule, double* data, int data_len); + +ACADOS_SYMBOL_EXPORT ocp_nlp_in *auv_model_acados_get_nlp_in(auv_model_solver_capsule * capsule); +ACADOS_SYMBOL_EXPORT ocp_nlp_out *auv_model_acados_get_nlp_out(auv_model_solver_capsule * capsule); +ACADOS_SYMBOL_EXPORT ocp_nlp_out *auv_model_acados_get_sens_out(auv_model_solver_capsule * capsule); +ACADOS_SYMBOL_EXPORT ocp_nlp_solver *auv_model_acados_get_nlp_solver(auv_model_solver_capsule * capsule); +ACADOS_SYMBOL_EXPORT ocp_nlp_config *auv_model_acados_get_nlp_config(auv_model_solver_capsule * capsule); +ACADOS_SYMBOL_EXPORT void *auv_model_acados_get_nlp_opts(auv_model_solver_capsule * capsule); +ACADOS_SYMBOL_EXPORT ocp_nlp_dims *auv_model_acados_get_nlp_dims(auv_model_solver_capsule * capsule); +ACADOS_SYMBOL_EXPORT ocp_nlp_plan_t *auv_model_acados_get_nlp_plan(auv_model_solver_capsule * capsule); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_SOLVER_auv_model_H_ diff --git a/control/velocity_controller/src/build_auv_solver/acados_solver_auv_model.o b/control/velocity_controller/src/build_auv_solver/acados_solver_auv_model.o new file mode 100644 index 0000000000000000000000000000000000000000..52e35367604918dfd1b0565a8f2915b91deb8024 GIT binary patch literal 35480 zcmcJ23w&Hvwf9Mzv}viCv<8e?VSqtXEg{oqQhCg@NqYj5HbM&lDwAn4X@Yq%k3Nu= zUv_uk)k zew>`M|NFnzUVH7e_t|HkJ>|-#$kGWVC5|~toHLz#%Ak%@eNFy8#J3@5hBMs>U*Kks zrF8iG@T%s_Kf)_lo#$Tlay@joqtQBdbim)~j+UNM1B#ouI#9zI%WB+gBv3Wh7^osy zH}k5Sd9HD^z5XC4k=Ly-ceG3)T!x!z40PuxgI<(DrZkVH{ZvXdRBIbiUNxyJ$e&q| zKQouFi#BRDXdT3c85;_8_zQISb2^N0pddex%P%NnYhVZ&vY4wGDJZF?KvN9_RBceT z*_3D+dBJzx+DW(xz7ok^5ttpx&L`Y=!qs$f8Z&=x$-EuOysn%V9^O;J`kNYFPXAvh zsha?jmaWUlhW@GCywbefy#Iz={lvUn{l6;bj$Iwt0y#R^)}L}RC*Jr^>bbWu;4_u< z<)ns4-BJBxMzCM&T}ngv!>vk-IBZd^>4T6{Px8vN-Y1 zLLw!L6L%C6DOsF&Um=l_iU`+7>?%Y$W%l%fY`A{9M;SW{%b;X&V;(IeQnEO4Um=l_ z#fke1iIh}CRBwiD{|pZ$2MU!?vUq8Pigu)AuC&&I(hv`=$jxW3w6j+hWVhMbZTW0F znhqmNSV76+=DuA>q-1d-J<5beN){)U7ZNF1oLE^%q-1eobs>?Giiie$e}PjFfbaKE z;xAM}$()ka1xnB^tLNnWfHK~;v}r918HPSMNNW4hD*gnDkLUH7Wp^hCe-)8W8@>m@g4uShq__eA)Jm% zyTdP+Gc&l;9o}8OO9(JOd;umKnK#3$c(MVqJG!ihABcq=!Z+`fN31??ZKi1qZmYi}9Zrp~k#08Pe^T9V+1K&k5s?ow@ zm{Xv{f0U$VWLKfI%>{+2+AkR^t>r>8ORA{GqO!rAXadxCtaLIlU>0B9;hm+ChJ)#Y zFMUPjyB&KYqwB|+otmNzcXaOJ5gt{)*o+-&cN<39YSQr~f$?>mmOeaETX%xk_`k$~SfYh|F) zH*01QZUP4=BY?d5j)1{wo?{EY-EBPFjPWN}{=XufLCK9t~_zO2qSeMQJO z{eo82ZVj)bA4U16aQT_nly#?43aFY59>YhrbSK^;nlmH&k)_8?f=4PbRsaqmm(v@NE@DcrFr_h2&Zby3c$G_JlS19NAELfT zS9&ngmF%Rewr*%pSvf=|5l%J~DLQb}qS=#sARfYqK<*gYp^ME;C?W+;rRFeBMNOJB zW22>k;kWUe_!UnLWAw+?aX2MLZ9w=}c=iW)3%H*OfFi{5AbMJO;v`f8LJy8O;B}F0190T_M+QWc6J97t2xo2dxDyXb_GB)l?Xp zRLeDD2rAx35er2E^iw&(%(k0MOS^Upnq-;sh+TXo4V#&-ZveCa%P0)i#iyP z8ba!5rJ|V904zh%`5u|;78oL_SA<&0YujNdT@UN4wqr=!F?2xpP%?mcRCFVT^Mc|Z z$~BAf+YBhf1BM@0xbk2<7V?(^MvM8Tm!ow3aKcQqDYz4chA)VWrUG@UFGbWO1!Edk zU35{eE1u5Myv%H4$++z1Y!mf=H&bdTYF1|Ye%@%5p>%5-m8T$UY&Uh$Wt-IxR04Go z1<qD3-O&_~y+Y44tgAhun{jM0b#SzWntGzi*iG<{n{}(Zd{sL!9(4I?7$tuh zhRO>0qv5v3(P@FEk;i=3eU3F@JtQ)U>42NT3Leda$hnz=Effgv97!RUA2VjGY6Db_ zd{Zla+$q1Z=@%j2Px+a5faX{j?w-pnfn9h>W5et~`X(y3(#0RueOx>MX%bk)YnZ-~ z9f%^D1frAW=ePX!Li0h?jOSF1qcia&=Ows0#9A-)7yPYj8hLs%R)(OwDf4RM=Hdv?VYP$z6E@Q2Uguy2HJ+-t4f`X>2A774-T2v!}rlDzsHO5Rl7;temBm| zPexYOh?m$t=a#yo)5UXprqL!P4ax=tr*(MLPa*PWJSC&pJC>^#GNfJ^dBB@FQq; zWw!BD$L)Bu^K}heW!@?RdQ8HS}*Hv}6PKyB+V9yy|v9#p(B>sJm(W zz{r@w`)kMHl7sFo1h!u7Jl z9eKsK!e9Kva4iZo?o6<`8gNw^XiL4?tTuxfE%BkuhuM^JOhcxl2~ zS%6xGX@$XUz$%q*q?MKoP&ZT%RizpYQf06RfT1aZrop;VBoGRZO;nYkqR8Zcx=vPT z2~}jQi7;o3|GL@5?~buyk<5)8NouhK?l|^cV&G;5-=K|3kf`EB1P*Txk4+%8{Fa5B z_v@V70z-`AJv%3y&(5wcgHGki#^YFItnmYUr4be`ZLHLaR->XAjyB};Q)(Elc1sTD zz|3>1CS#4$l@^E$a$j-bk-FRxDrO*fncK@b7<|+NR_AK^R@IgCCjwsV(DXkRqem#J z)OVCTH~84UhN0bDYBIDNsu&qnUK`oz+p-GIp%!h$Gq@@ELIh!5N0GdtIrFTWc`1_F zW5#7j)-}>x;#rKA`Vp}_TUw*5Lytes)mb3ra+lLEVCUxKD(#${9L1aesJYdixht}z zr`eg|;oW%J?orR6#)i&->&pfZ>k;ayIhk~!$&EgfDer@pX( zCv|LoBZ7z>TOJ-hf>*~m=$S28G=u{y)jMGG@b1!<%zs8k@gl(MdM%m5_;eP@=tibV zRrBy(Wl~eaYw71`R)t3<3RC4PTj(K1(*b;w)VBCBZ@>U*US6mUHI@Vnhc)0UTZmjq z2I)9Mfb-l616K_n<2nU!W{N}<`F$1!VGYa!H8{Vds>>921d-CPGrU4ysqfb$>x z>VYFinn%}ho+9c5a@EWU!H9eH_EEs(j7cm+V_h4n5CLY?Yi*nBnnx>|N6+_@tjUAI zYApBDL)n@;a64NuM70ZoAben%(;H(KcMpdZ-#DVCa~}hG6U)dD^#y z2FT#kW<8GPQsK;gJp3b>H&*3dEMCQWG7IJM3bNdce%Jf|n$1SNN=2wwP*_OI+@|ac zFKIy4HlIXQK$bG4NEO;H8s1exRA07&o}hZY3A3eY>oZ8BY7bI4@>u$#m^9ZnXP$6J zKY-67qgP@(0lxs62Z)|-W`4%)7kmt9)qd=_{FGC)Uhex)Wpn09QV-pvIo#Tud8{R~ z59R2m6DzOY#q-_Z&Rx0r3bntRpZjtQi2~I8_@CtCrNGVn<2?0FaHSuyqT*w8lUm7e zM>nbdi5tB%FuF-ivw`HuYBCB$J!|k{xY+I36@1#wR)r$jz&3oJDaH7;$#qN2kg=&v z+_&T2y=oV&Lvs1e4bPYsLUlEvD>AQXv zy`kxRzs`3vRRQK;#Hpe=qQ*H+hIf`U?DH+(7u*@n{5Gel+;{z7sI!{5%7;|b$eX_F z?m=F2$6ibi+$_Jd^PuErXYN4;KA0(Nl;5|PXXhP2!81p8Zv6O57r<|Ly-U?bGOw~u zetT&yd6rA2w>5fAn|Z});q0=%W7)ZK5*A?B|MCsr?0p#HSptTFW<;32=?Ph*;7&Zz z)LS1-7s{I(cKb#Kcmk{Zock=5j$pXH51!8t*SiWeu~~=yiV>8{{xMqEG~c+PlDVQo zaDT}Ut=f2I6)A)rfq|hs_kCFPx5-v|84-ra%_@IcrIoI>(sfqaZ>8(4bj_}=o(=KN zXm>oBbk_C`tcmsZTpmmH4D?51y@TDcXkSl%Q9KdvOm`5^%1tNZ(eqX;an|>wx}(W~ z-Zbk<#`}}e^@&t;FqVk*B~|XaSZ_KWwX+IkE*p%dqP?-cB6f78dwZkFRD3YnnU3{3 zog4dOeLWq?XvaWr??8vDLu%t-JfG8_?qh+Z4aL^RB^F-=NbM+n}lzhyTgC9xgSOg6TTvvcY5r;-G(RG`)t=*qUTC-m?}3nGCwdqXTO$ zjUx!wkqL!49RvNzR3g^XpGrFYy@TWkWm*CrSi4bTxk!q3#X8_9nU{=p#Zw!LWGJ_w zo$ur^Ydf6f{xi<-lW{t4us7C! z>Y2+CJA=K^b)E5O*Fa*uN}-{nUFrUOR;=?>`Zk|>73%65})Gq52U&gl>THq<=+@j)z(hMxL7!5 zA!unti6uI^qcjpF(_LLX9X)ZhN@qOTfh&!rbQH?P2hlu5xhhIJV}lgt+_ZK-SNdW{ zq0j)PSdMd4USFEWIpY2kUfsW>5B)PrK^RMj4%)jWN}~(Li>~O}M68o~G0HPhiJrjG znjS<$$D^s9KDZI1Y;uu*EnLv=$0&-C&OgxQr#wIB`D@PbpVB$Y-`UgEh5qF4N(}V* zQ{8cYM>>&!*LyemV`@~6cOFf_iBns9i9@#gTJdt9A4(@3kvB6}zH-74wMY^1#jrW7HBg%15Xu~2Z zRyfGtlk_iN6^Z!Qq*Gk^yr(d545TP_F+Ri-i2bCe$NnXuGzOZr{tk=@{!0~CXaFC33NXmP>f#-qDoBz7Wq?&jU<&C;7)x?52=jx(Ex!MLSy0?8c+122#kC@ z+ep^>n{3SA9ZUKPWQo*6{^lWqEl}eNjoE$*RkAQOfMW3w?wyThp=L(h^`Nf(+<2)t z>Wm1~5k!O36g&a=>B=|p%ck&^9x!}EgXt7Oq`?RLtbdiho7mEMFi5n1s>4>*1r z!u@eGG@^4t)snogbdwca#*Ggch12Ref*E3w@k93;(4zEIGs?brD;?iDtBa5SJnPgY zGhcds!H3Se>9-#}cuei|GqbP!X!~1xcbuvCRiU3|_J84?CvQ9b-EY1AWXqb?v$UQN zUoU+-r1af?)=wUI>UYmw@$?xtb#DCQmH+t08HyJO(b_hzZ!>V%$a9L8qG>Ii7KmwW zo!+{M%RBCHbq=)cqzkSgXW5b^i~KdK)};GWX@78TZLqfP)OtKqkqmC0GpDw0-YgBX z2;VQM0uyTv2z;q;N(3@Z@C`hbK&Ya`M!QdSKy-8gLd@R6}NQMS1fv z5GNaGV+S1->cV9YiF?Pbs63~l+*u8b*0bpNIsI8mWkcU}6Pu{)#f4=zR)i)bQ1X&- zB{v<@QBl8i9PC2n+oTuAYI1&FKi92@{+}1ES@;;I9^#Fp1IJ0!h|rZKTenUOSNN|f zg)fH7mQ@T+Xq;M610-y`(sT^_WJt^MN(;oVz?n@$tN`$yEqpw zNAt<1ing;W+9%w_-Z+~R@DDt5VX-hJpKQ_k&!aNolOF98%DY47ou=}tTPytMqnog; zJ9VBJE6D!eXw39&!VX$k4X}4L_6X^PueVM>bY3&DvBE!GI^ph$>LuKL9j6W1H8g18 zIF{Nc-!8@a%d(35{E>?LLO9za6^G*!Zz%EVJ!<^9TI;T-0#Nq>RE})EP2&^_bP%>f zW7IY1AZ&LLY=05#Y)betu40{C&I<3x3F&c&5*a~yAOaGfY{19$U{sfj@yM?DC~bJ7_uq#`S^O8Yo_8$#&stBnrANhOS#EA* zvftEt=IFVE!Qa*RnHD~Q%PVst7S0<|QQtNTuh9I9EnLk}CpoI$m{J|*L(DIC2J;Z# z`L&)7OV39&zRAK>+>~Rbvhbj-71S;_S^Rp9f8D~*)c6hyZ`Ak$7JjzIAF=ROjqkSb z^EIw^7v*hT&G;0aALUWUQSn{o{LIqRt@Z4)@P3V(Ii=C1=I~|Cev5y(=09iQLmK~+ zg}{A9)_IU&9pFF4M}HUAQe zAJllv!uiLp=)av7ewOBUTli9qUuxkk8t=34m5fhv=od5cP~%sbv)SUW(R!}6@E(m{ zXW@eyzsbVaGd{_&?cc2V+b#Zx#=mCaTQ&ag7XEKq|D6{8MUB^3_*XS<-ouP9lwZo6 zZ(IEDgI{S#;!5LMF`V9Ki{bA8pMl9ns9Y&AYfT?SdBymC;KXm$eDlsc1-nLz@u~k1 ze^B#(u5G0_E5;x6z^T29@xSbW-{FDZ1DxxtU*&i`3pV}Qga3OE{GiseU4N+HwJhkN zUxpO3bAboG+yhT|;MaTL-}S(s@W5a5z>mkPU@`mWd*E#zc*+Cc=7Imv1AoE;Kj?we z&#sHv@AtsZ@W9(W@T-AS{OqG$(m3EoK7OF_0~+VGNaQ@>q34JPJ_-G#nEe6ZT<&qI zJYH+WZP`7$Y5WwG?Htkg4vq7=De{iP_(FOr zPf|o)@5JqVjSp&^*E)e;4BV8ZB-Gyxn!jE15q^B!qwyUYH{#M|8PMJ@{)p@U#b>@xZrx;6L)ff8l}u(F3nQKc{-_JXuwU*LiXK36JOC zLd`#*8{DiDoUd{JN0lCQ2R;%SuhTffgOBSq9cZ=CJG>mYu`0b69pR%g$xlxynqIoy)RwS#~bV&SlxTEIXHF z=dtWOmYv74^Hd#Kb{@;lW7&BuJC9}OvFv=7ozJrKS$00l&S%;AEIXfN=dsiSBg{)yAYgow4g({c%3z=WfoORGCur5ZT5fpZ&_PPVFnIStGS*0Jhf9qSI(vGQOYYY)~jJy^$jf_1DY zSjU=zb*v;@rCpcHtPX%01MN<&rWcHBh%_7tdvF|G7g-5Zg z3)>>r#?dQr?f6Wzqx&=1*4NYPx#33d)xmy6+Bh_x)|H6IqlPqYh#0qfDQ6k&rxb>E z3?#AP3>#^%ZxV2HS!4w^Sw>eZU3y;A%IM1Q;z&~z+tIK~Fag`Uc!ysXSwG$)F)Z

RX(+CHG3hg zDropS*$E4lGhd!%;P~Y@^Mbv}3b446hV-Iz*zy?9uA9H$9Z*jP=%? zqZHOhZweC;ts_ot;N}flqAHrMVwm>Up+GZ2R6Y!RYS2O2b{1l3gR`uG4Ju;4Soxs;6J>L@i>4JZ+2mdj_pCR~ae}>xEDeLpPG-mi}|A8vETHt1X zhh6S^q345wul50quh$)d{~^KOE$qBr;0FYs_EZ@=)puy6U-GLd&~VUYCQiejr*X1h z^38Wb%KnJp_j>TJ*En5hkCxG|zAuiqXOG~YAo%Kg-gy3D!I$y#mcac&&kTGQp~G&M zfW}GeM4ZM>_5Ex-e~I9qB>3uk*m!^qX>cQVG^wUz5vHxxl z{(XW^OFxFcN8le5xcN?Q+xd*(*9gA(j&1V~3;ry@FC!uj^8d$i8hgxlW?K(^$D)Jy zQctbGKY=`>=aU+@^(+>AsmFYeCH=F79`n7_*7F$;JwpONRp`lT+}3lu;MWTNcRlzI z3jS$=|ELH5alx+>{1*fs6!_~Ndfpa%{DwE@=lAIc9{Fvqz(1sM5|iWqiGn{*@ael6 z9i(SIPScJ{JoKC`_zMI-D)5B@@Ac5LS@3Bs*Oa@(gHPY>=pZ{Aa2oy{0$(KXyFK*W zC-`z4`=!7?DfIl_L(j{CPkU#LJ@0t%kC_A#4zizqq+$5g8n@f) zW)FU=;5Q3?T;OL5JmH~-zQ58zcAkUN*!h1v_+J^ zP4F)i_>BU;U*lx|a)FNt{>=jali;rq_@4#;3j#l83KBTz*?22Xp#IN3=qH+BvQoI;-Fpmb~%IPLW} z{C^Ypc>=#h<5cdK1-?(od)!2^H=&us^R|P&(;9nE?Y=M7W;By6jhrkyJ z{2Kyq)i~LIgTSvB_`eJMHlgR60{^za?-ckw0{@o4?-%&D1-?_@-x2tjz`rZ--)r2q z|Ac8E;UHYvKU?GE|L@^6{$C{Uy96E<`1b|w3VesaR|))Xfwv3%9)Vw~aohgy3tZa& zpupw$Fz!@Y{v{9|?Si;GZw>2R-xLH5Y@`hdo5{Xv0C{UL!%{g(;-vc0YpeCf~I1upg9C2*<#5usnU*FM3Q z`u7W5>VHw-Qvb9W>>l=y^#8{Mz5o;xhl>PWC-9{jxBaVHb$HwZfqX`JGLrdp<6uM2#&z$eqo7!EtmPtiE(`7}sV+-+n6ac46l(fyV^?YmJjXrGK6k zxU~PEz@?ofRZxI~?3DA#N{!p?RW0~5=P>P6D{z`B8hnw!X=-EeVWD5ze}mw!5&W-v z@cEyX29NCT5d1L@{<8wVQShsdMFI!uk@JdA34A&7jQt%NC;!|m@XrZc`uX!hkJSG& zfv*&LrW}U^4zgzjPE+m)8mDq)|2<3KvR)T?;5`DrSLpwOz@?sV3VfU3-=%S~lioXw zoj(@%xdMM!;GF{BEA-3vjwv4i35Okri!@H<#&Mc*n*{%UfnOx}T>|g%;BOOrssAPq z{%(Q)MCgB7=vgc9=LP?#0)JWH4+#ADY9w%c9Otb#O?^+*IQgMl;HPVx{3hR92L%3* z;BON6mju2;;5!BWjKF1_Kl5Rba8S7q<1}_2)HvDMBk(r`{}F*t(l4;Yzf|DIXq@y* zzF+V^BlxFy@aGGDui!88;70_%Pw+qG!EYD*e!-6m{w{&11b;x_8$I|>2>zhp|6cHS z3;bolzf9n7d+?`z1SA~f{{&9cjx#h){+Id#f}a%pIuHJ0!A}YPau0sH;HL$@*Mq-F z@Ye}`)`S0j!Cx=#6PKV^4kWS#&4?yU;3?2@HYy6)`Neq2mTum zyzB%h#bLMCM?CQP0+;nVOW^XkutMN_QI4r^x4_2)zR?385%_(Ae}lju75JA0{uP1W zCGZ~#{9b|oK;SDyy6rjaFpVd<%T`*6&fdhN_(On{96V8^N=y^@(mCEPXvEd@E`Hu z|3cuhKRzLF`9AfM(7#pae@pPCAKnxEtl&@mC=xhqznx8s4jPy9_HyywD&zk<;(bKo zFP@Os^JCl^zg^+a;|~ga=Sg||Nr4{__-_R6S9BgHsV&Vxb6q+JqtpB@`)UGl7~K5+ z`ZfjUaPvFscP%_biy1h~zyD$MnBOm(ePsqWzi+*zM9CT4{JvH02cx`w$0^K^%9E5H^Y026dj>Ub{#_7*o8Lw5xAg4L z{Fg0!pT;==QIyehSmU!PFa>UYC%DDJ&F=xXTe$f>;14a_{2tKk+cD*u-vd5l@y+i6 z4_mnTJz!~Bp`GUUfYlameh)a?!p-jio!aEazEo@tuBn8&cAL9Iyf;>x!bW1Jmj4mz z+BNt`>uVDOlpuN#zxpdk;+Iz{*Ia8mlBpa>3Hp&0{=HTDr)`~D{7bj-TKr?3r}9sf zbb4)nx)%SS?jU}_vk_{@gm`zfi~gBfRpbBNUq8gDYfQ4#gmn5=JwBQ2N^_&Hv@AeP z64GgNx$`1*clcxK>cfS~(ZgEq-$f(!gzK=xojQJJm1}U~%40SXr1O6&AA~IFoP$#y zrhNXNn2QWkzPW~wS4{qvPbi@l9cM;`slSn5q2($53l1_2M_65QfBm^Tb#nnOq-%zS zyIOw8!cF<++KK#P^82)Woh5JD-&_+~-sTyHQv1lm)Z1L?9mCv9-fX);bf(RvXZvKF zw*T<^NIv}PlKX4wZGf-ihIHEUty;d`qy3CLz0($p|64w%82hwhihuglY|5`z*+t8y ztSt)_VZVOflfSRSY0J~&(2jkQY;U2z*aJm{io^EbbmSG2?;cS?-xIQQYs>#XVEgTJ literal 0 HcmV?d00001 diff --git a/control/velocity_controller/src/build_auv_solver/auv_model_model/auv_model_expl_ode_fun.c b/control/velocity_controller/src/build_auv_solver/auv_model_model/auv_model_expl_ode_fun.c new file mode 100644 index 000000000..bf4e3e79a --- /dev/null +++ b/control/velocity_controller/src/build_auv_solver/auv_model_model/auv_model_expl_ode_fun.c @@ -0,0 +1,386 @@ +/* This file was automatically generated by CasADi 3.7.2. + * It consists of: + * 1) content generated by CasADi runtime: not copyrighted + * 2) template code copied from CasADi source: permissively licensed (MIT-0) + * 3) user code: owned by the user + * + */ +#ifdef __cplusplus +extern "C" { +#endif + +/* How to prefix internal symbols */ +#ifdef CASADI_CODEGEN_PREFIX + #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID) + #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID + #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID) +#else + #define CASADI_PREFIX(ID) auv_model_expl_ode_fun_ ## ID +#endif + +#include + +#ifndef casadi_real +#define casadi_real double +#endif + +#ifndef casadi_int +#define casadi_int int +#endif + +/* Add prefix to internal symbols */ +#define casadi_f0 CASADI_PREFIX(f0) +#define casadi_fabs CASADI_PREFIX(fabs) +#define casadi_s0 CASADI_PREFIX(s0) +#define casadi_s1 CASADI_PREFIX(s1) +#define casadi_s2 CASADI_PREFIX(s2) + +/* Symbol visibility in DLLs */ +#ifndef CASADI_SYMBOL_EXPORT + #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) + #if defined(STATIC_LINKED) + #define CASADI_SYMBOL_EXPORT + #else + #define CASADI_SYMBOL_EXPORT __declspec(dllexport) + #endif + #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) + #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default"))) + #else + #define CASADI_SYMBOL_EXPORT + #endif +#endif + +casadi_real casadi_fabs(casadi_real x) { +/* Pre-c99 compatibility */ +#if __STDC_VERSION__ < 199901L + return x>0 ? x : -x; +#else + return fabs(x); +#endif +} + +static const casadi_int casadi_s0[3] = {9, 1, 1}; +static const casadi_int casadi_s1[3] = {3, 1, 1}; +static const casadi_int casadi_s2[3] = {0, 1, 1}; + +/* auv_model_expl_ode_fun:(i0[9],i1[3],i2[0])->(o0[9]) */ +static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { + casadi_real a00, a01, a02, a03, a04, a05, a06, a07, a08, a09, a10, a11; + casadi_real a12, a13, a14, a15, a16, a17, a18, a19; + a00=3.3453634479321918e-02; + a01=arg[1]? arg[1][0] : 0; + a02=30.; + a03=arg[0]? arg[0][2] : 0; + a04=(a02*a03); + a05=arg[0]? arg[0][4] : 0; + a06=(a04*a05); + a07=-30.; + a08=arg[0]? arg[0][1] : 0; + a09=(a07*a08); + a10=arg[0]? arg[0][5] : 0; + a11=(a09*a10); + a06=(a06+a11); + a01=(a01-a06); + a06=23.; + a11=arg[0]? arg[0][0] : 0; + a06=(a06*a11); + a12=casadi_fabs(a11); + a12=(a12*a11); + a06=(a06+a12); + a01=(a01-a06); + a00=(a00*a01); + a06=6.0368975005218254e-05; + a12=(a07*a03); + a13=arg[0]? arg[0][3] : 0; + a14=(a12*a13); + a15=(a02*a11); + a16=(a15*a10); + a14=(a14+a16); + a16=46.; + a17=(a16*a08); + a18=casadi_fabs(a08); + a18=(a18*a08); + a17=(a17+a18); + a14=(a14+a17); + a06=(a06*a14); + a00=(a00-a06); + a06=-1.1116270017027866e-07; + a02=(a02*a08); + a17=(a02*a13); + a07=(a07*a11); + a18=(a07*a05); + a17=(a17+a18); + a18=(a16*a03); + a19=casadi_fabs(a03); + a19=(a19*a03); + a18=(a18+a19); + a17=(a17+a18); + a06=(a06*a17); + a00=(a00-a06); + a06=9.8084735444363873e-08; + a04=(a04*a08); + a09=(a09*a03); + a04=(a04+a09); + a09=-3.3399999999999999e+00; + a09=(a09*a10); + a09=(a09*a05); + a04=(a04+a09); + a09=3.3199999999999998e+00; + a09=(a09*a05); + a09=(a09*a10); + a04=(a04+a09); + a09=(a16*a13); + a18=casadi_fabs(a13); + a18=(a18*a13); + a09=(a09+a18); + a04=(a04+a09); + a06=(a06*a04); + a00=(a00-a06); + a06=1.0920100546139184e-05; + a09=arg[1]? arg[1][1] : 0; + a12=(a12*a11); + a15=(a15*a03); + a12=(a12+a15); + a15=3.3399999999999999e+00; + a15=(a15*a10); + a15=(a15*a13); + a12=(a12+a15); + a15=-6.8000000000000005e-01; + a15=(a15*a13); + a15=(a15*a10); + a12=(a12+a15); + a09=(a09-a12); + a12=(a16*a05); + a15=casadi_fabs(a05); + a15=(a15*a05); + a12=(a12+a15); + a09=(a09-a12); + a06=(a06*a09); + a00=(a00+a06); + a06=-6.0150572994295574e-03; + a12=arg[1]? arg[1][2] : 0; + a02=(a02*a11); + a07=(a07*a08); + a02=(a02+a07); + a07=-3.3199999999999998e+00; + a07=(a07*a05); + a07=(a07*a13); + a02=(a02+a07); + a07=6.8000000000000005e-01; + a07=(a07*a13); + a07=(a07*a05); + a02=(a02+a07); + a12=(a12-a02); + a16=(a16*a10); + a02=casadi_fabs(a10); + a02=(a02*a10); + a16=(a16+a02); + a12=(a12-a16); + a06=(a06*a12); + a00=(a00+a06); + if (res[0]!=0) res[0][0]=a00; + a00=6.0368975005218423e-05; + a00=(a00*a01); + a06=3.3484658136227786e-02; + a06=(a06*a14); + a00=(a00-a06); + a06=-6.1658244361114702e-05; + a06=(a06*a17); + a00=(a00-a06); + a06=5.4404333259807184e-05; + a06=(a06*a04); + a00=(a00-a06); + a06=6.0570157695918683e-03; + a06=(a06*a09); + a00=(a00+a06); + a06=-3.0184487502609172e-03; + a06=(a06*a12); + a00=(a00+a06); + if (res[0]!=0) res[0][1]=a00; + a00=-1.1116270017027936e-07; + a00=(a00*a01); + a06=-6.1658244361114783e-05; + a06=(a06*a14); + a00=(a00-a06); + a06=3.3963490377380855e-02; + a06=(a06*a17); + a00=(a00-a06); + a06=-2.9967785627100754e-02; + a06=(a06*a04); + a00=(a00-a06); + a06=-3.0801331505514837e-03; + a06=(a06*a09); + a00=(a00+a06); + a06=5.5581350085139543e-06; + a06=(a06*a12); + a00=(a00+a06); + if (res[0]!=0) res[0][2]=a00; + a00=9.8084735444364535e-08; + a00=(a00*a01); + a06=5.4404333259807062e-05; + a06=(a06*a14); + a00=(a00-a06); + a06=-2.9967785627100469e-02; + a06=(a06*a17); + a00=(a00-a06); + a06=1.4970303990827358e+00; + a06=(a06*a04); + a00=(a00-a06); + a06=2.7177645446042520e-03; + a06=(a06*a09); + a00=(a00+a06); + a06=-4.9042367722181902e-06; + a06=(a06*a12); + a00=(a00+a06); + if (res[0]!=0) res[0][3]=a00; + a00=1.0920100546139210e-05; + a00=(a00*a01); + a06=6.0570157695918657e-03; + a06=(a06*a14); + a00=(a00-a06); + a06=-3.0801331505514781e-03; + a06=(a06*a17); + a00=(a00-a06); + a06=2.7177645446042498e-03; + a06=(a06*a04); + a00=(a00-a06); + a06=3.0257778596593993e-01; + a06=(a06*a09); + a00=(a00+a06); + a06=-5.4600502730695923e-04; + a16=(a06*a12); + a00=(a00+a16); + if (res[0]!=0) res[0][4]=a00; + a00=-6.0150572994295635e-03; + a00=(a00*a01); + a01=-3.0184487502609133e-03; + a01=(a01*a14); + a00=(a00-a01); + a01=5.5581350085139340e-06; + a01=(a01*a17); + a00=(a00-a01); + a01=-4.9042367722181935e-06; + a01=(a01*a04); + a00=(a00-a01); + a06=(a06*a09); + a00=(a00+a06); + a06=3.0075286497147785e-01; + a06=(a06*a12); + a00=(a00+a06); + if (res[0]!=0) res[0][5]=a00; + a00=arg[0]? arg[0][6] : 0; + a06=sin(a00); + a12=arg[0]? arg[0][7] : 0; + a09=tan(a12); + a01=(a06*a09); + a01=(a01*a05); + a13=(a13+a01); + a00=cos(a00); + a09=(a00*a09); + a09=(a09*a10); + a13=(a13+a09); + if (res[0]!=0) res[0][6]=a13; + a13=(a00*a05); + a09=(a06*a10); + a13=(a13-a09); + if (res[0]!=0) res[0][7]=a13; + a12=cos(a12); + a06=(a06/a12); + a06=(a06*a05); + a00=(a00/a12); + a00=(a00*a10); + a06=(a06+a00); + if (res[0]!=0) res[0][8]=a06; + return 0; +} + +CASADI_SYMBOL_EXPORT int auv_model_expl_ode_fun(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ + return casadi_f0(arg, res, iw, w, mem); +} + +CASADI_SYMBOL_EXPORT int auv_model_expl_ode_fun_alloc_mem(void) { + return 0; +} + +CASADI_SYMBOL_EXPORT int auv_model_expl_ode_fun_init_mem(int mem) { + return 0; +} + +CASADI_SYMBOL_EXPORT void auv_model_expl_ode_fun_free_mem(int mem) { +} + +CASADI_SYMBOL_EXPORT int auv_model_expl_ode_fun_checkout(void) { + return 0; +} + +CASADI_SYMBOL_EXPORT void auv_model_expl_ode_fun_release(int mem) { +} + +CASADI_SYMBOL_EXPORT void auv_model_expl_ode_fun_incref(void) { +} + +CASADI_SYMBOL_EXPORT void auv_model_expl_ode_fun_decref(void) { +} + +CASADI_SYMBOL_EXPORT casadi_int auv_model_expl_ode_fun_n_in(void) { return 3;} + +CASADI_SYMBOL_EXPORT casadi_int auv_model_expl_ode_fun_n_out(void) { return 1;} + +CASADI_SYMBOL_EXPORT casadi_real auv_model_expl_ode_fun_default_in(casadi_int i) { + switch (i) { + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const char* auv_model_expl_ode_fun_name_in(casadi_int i) { + switch (i) { + case 0: return "i0"; + case 1: return "i1"; + case 2: return "i2"; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const char* auv_model_expl_ode_fun_name_out(casadi_int i) { + switch (i) { + case 0: return "o0"; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const casadi_int* auv_model_expl_ode_fun_sparsity_in(casadi_int i) { + switch (i) { + case 0: return casadi_s0; + case 1: return casadi_s1; + case 2: return casadi_s2; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const casadi_int* auv_model_expl_ode_fun_sparsity_out(casadi_int i) { + switch (i) { + case 0: return casadi_s0; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT int auv_model_expl_ode_fun_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { + if (sz_arg) *sz_arg = 3; + if (sz_res) *sz_res = 1; + if (sz_iw) *sz_iw = 0; + if (sz_w) *sz_w = 0; + return 0; +} + +CASADI_SYMBOL_EXPORT int auv_model_expl_ode_fun_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { + if (sz_arg) *sz_arg = 3*sizeof(const casadi_real*); + if (sz_res) *sz_res = 1*sizeof(casadi_real*); + if (sz_iw) *sz_iw = 0*sizeof(casadi_int); + if (sz_w) *sz_w = 0*sizeof(casadi_real); + return 0; +} + + +#ifdef __cplusplus +} /* extern "C" */ +#endif diff --git a/control/velocity_controller/src/build_auv_solver/auv_model_model/auv_model_expl_ode_fun.o b/control/velocity_controller/src/build_auv_solver/auv_model_model/auv_model_expl_ode_fun.o new file mode 100644 index 0000000000000000000000000000000000000000..6cd530b508e0bcfd0ca6b946a18b25895a29592c GIT binary patch literal 8232 zcmb`M32>BW8G!!~jx?qFZz$Rn6t;FL3zqHfh9g)@c1Z%O6FX^}(A_bREXl6PkgM50 z5)f$^5(Wm5RH?&E6=6EoI@W^II)hlrmITUCAmyf14jU*4!h| z16GL2(m+&u2y%WKL|#Xao>5|lIQUhp1`v`8y9>xDlVlXw!7?Myo(6HX9n6B`Oi6H; zu&xBsDXhwbMtzSSc)v$4>AI>f6pfGQFCKxfQ?yE$WJ8>1_d<1fcH3F>hK>Co&z^~S z)a%5)JO~PbOkn{r=(_UVy+R#TwhH7BW#P{+KqmRi#rEde3(-+JND8s*3l9q&Yg{Ly zU+NZm0n{4+(lQ(;v$qxYCQ_`?B6+$a=5PI6Z*le)0|i3?4r@OI?FzdX*5f^Sy4A2} zVp1T6aCby^9Khn2uwR&2fP)z*u-Bu%?nu7wxQN)C12L`_GzOAty90)Jf~ZCh-i10o0&3V@*v8$cy#tu(&P(nB zAET@NSlS0}j)B4rQJ)_XOfLF>>dVX`G?Nxd07r3wy%Jg;`~-zJun!Mm<=|%@HhwQy z?=`yu@>_^bVRRh1ur5TyyI>eN1^tNY21H|Tnj-@SyRni3pd0uQE2)6N08jyguMgdU z_<|To$d#N&1+$4*6Y{@9RWOTK^EQk=#pnb?fi0jMIE|xo32iT!&I2g_4Bg&_(Z3`b ziYtDB-3#@74vqf;8vBgdQ|K00Ucu-pXrBRzn1F~Lyo~T9m~=<<64?ZH^nK+UO8HJi zs=~i~IQY6PBPid}Ga6x><8_H1W1aBPev_*x=>QJZ2E`h@zq*nwz@Z62n!Ee#-UklN zF?aj@Z3t`y4(&3}2vZj5gmzi=jC`BF^_bo=0**QuzFe1o#e4RpFkt=_@7kLo6*~$f zs|1CNiD45K;X!150S^ZQ=hv}CKrdUEauDHM8E_s!0{aORcfyCm4_n4sgy%|dJAB(< z62xQ%FJf*lM%!Qt3t)@VAqNK<b22j(vBguf#XgH_KPr5or@Hq4k$w zSwR(<(1rux@*sTG=r*tuzTM}4tB-5}V=NXVN(1;*6Cum7H;K#)cU& zxX>{e_p);@-R$j+M4VigKGG}h8+Qy=57m$Nky`GMZ2l9us;`zerb5H8$~;ZfzY#A? zsSKkPgS)U9sF3?th!wxMH^mqc&-XKiwDwYI6KeYP{4E`A*uK_Y6PRX#-`BsmkN=my z)?Q=a2ZOLz{?_Y|9f%$Bx9)>%Khd<-9gr2z0vy|WGY}E&^_%v1Es{OXb5i>%msaK0 zsyteqSOuDRZwTjLg^j>Gmmg5dy=HhBupFAXaH1pq-GjDuIon6x%*9o;wT(A6(;f=_gIFM7JF{hVvprncNj&gv5_En4B>DUYn)JL9jf9N7G4s(&oi zJBr$IGsmpIfb5&euABS}C;P07J(KEY?$0S~d-8a8x38Vk=+-?~mgelwF~@0&m(rM- zPNMR3@5T|W=bzh{9$UHPnxkZ8y0-(9EvMVH>AAVzusMonH`W@BHg}fO?R1UHZWKlL zLXXGk8b5}xa0$;3cpYjQuKelG7HhDMV#siQF$^^mEoJ`mT1t~OW%%GhX>Bm+X!8}4 ziSr)v1@O^Q(Lw{EFwJ6J*>9>P^T(M3hl9u}*m}4Ey@L?EaCW5V^yv*JtCU!97(e}LrdR+#!wYf~x(s zE4_(?sqtNXPdW^;DcV5M7o>wpG@dvW0q|VAKP(W%G)14B+3Kg=OUXuL|k3b z4-;oSP9G)CX9M$*z=ueKz-Q{y#I2+#&Q-`i51bkMz%-bI7boE}$$v9-fX~#0#M>0U zhWJ|IeBN#*zCqDHPCTaQ-yzjo&Y=sjCQAmxi0_t4kqQ z8fmO`mM0OEM%_szo}?0&<}AqdU^EdUQOv^Jc#I}sGzp_@j3#5`7CLTW<8})o4BW!P zElk|PnOhjSg}p~eJ*KAc;}M!3vjIZiBfNP`ec|6DJY<=zGNr;cD`|%!q1n+Sozh;@B^|Q@EX1B`%M{cvOUdiQF$d zH!)yhfsg(CUgEmMC(#cG6yrU``maemL*jps_z;Qjk@&+BKQ3{6`?CLw630JqFuyDD zFH1ZFCjxPe^>O#IXtdJ7lrKxjnBaoPXoKorK%KCkAXkejM<35eDY` zR%-L+KV{O!uj2`LE-TW!5N)bV}xdF&ZyB~rj?w7YdPdJ z!cB(etOyw)%{e<7)tn7=SU`2R6CNjKkyB@R)WByDDM1O|GUatOHQ`$DQd?()o$yzc zaZw{wK8MQRt8H}FH`LXK8;p5ih;@c5OCt?%yV6|zFNqC|vFo(m;;y`u%fdrl3cM~E z;TeT{+RK1a7apb%Dp3dl?*Oca-SO{a+ + +#ifndef casadi_real +#define casadi_real double +#endif + +#ifndef casadi_int +#define casadi_int int +#endif + +/* Add prefix to internal symbols */ +#define casadi_f0 CASADI_PREFIX(f0) +#define casadi_fabs CASADI_PREFIX(fabs) +#define casadi_s0 CASADI_PREFIX(s0) +#define casadi_s1 CASADI_PREFIX(s1) +#define casadi_s2 CASADI_PREFIX(s2) +#define casadi_s3 CASADI_PREFIX(s3) +#define casadi_sign CASADI_PREFIX(sign) +#define casadi_sq CASADI_PREFIX(sq) + +/* Symbol visibility in DLLs */ +#ifndef CASADI_SYMBOL_EXPORT + #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) + #if defined(STATIC_LINKED) + #define CASADI_SYMBOL_EXPORT + #else + #define CASADI_SYMBOL_EXPORT __declspec(dllexport) + #endif + #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) + #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default"))) + #else + #define CASADI_SYMBOL_EXPORT + #endif +#endif + +casadi_real casadi_fabs(casadi_real x) { +/* Pre-c99 compatibility */ +#if __STDC_VERSION__ < 199901L + return x>0 ? x : -x; +#else + return fabs(x); +#endif +} + +casadi_real casadi_sign(casadi_real x) { return x<0 ? -1 : x>0 ? 1 : x;} + +casadi_real casadi_sq(casadi_real x) { return x*x;} + +static const casadi_int casadi_s0[3] = {9, 1, 1}; +static const casadi_int casadi_s1[3] = {3, 1, 1}; +static const casadi_int casadi_s2[3] = {0, 1, 1}; +static const casadi_int casadi_s3[3] = {12, 1, 1}; + +/* auv_model_expl_vde_adj:(i0[9],i1[9],i2[3],i3[0])->(o0[12]) */ +static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { + casadi_real a00, a01, a02, a03, a04, a05, a06, a07, a08, a09, a10, a11; + casadi_real a12, a13, a14, a15, a16, a17, a18, a19, a20, a21, a22, a23; + casadi_real a24, a25, a26, a27, a28; + a00=-30.; + a01=arg[0]? arg[0][1] : 0; + a02=3.0075286497147785e-01; + a03=arg[1]? arg[1][5] : 0; + a02=(a02*a03); + a04=-5.4600502730695923e-04; + a05=arg[1]? arg[1][4] : 0; + a06=(a04*a05); + a02=(a02+a06); + a06=-4.9042367722181902e-06; + a07=arg[1]? arg[1][3] : 0; + a06=(a06*a07); + a02=(a02+a06); + a06=5.5581350085139543e-06; + a08=arg[1]? arg[1][2] : 0; + a06=(a06*a08); + a02=(a02+a06); + a06=-3.0184487502609172e-03; + a09=arg[1]? arg[1][1] : 0; + a06=(a06*a09); + a02=(a02+a06); + a06=-6.0150572994295574e-03; + a10=arg[1]? arg[1][0] : 0; + a06=(a06*a10); + a02=(a02+a06); + a06=(a01*a02); + a06=(a00*a06); + a11=30.; + a12=(a11*a01); + a13=(a12*a02); + a06=(a06+a13); + a13=arg[0]? arg[0][2] : 0; + a04=(a04*a03); + a14=3.0257778596593993e-01; + a14=(a14*a05); + a04=(a04+a14); + a14=2.7177645446042520e-03; + a14=(a14*a07); + a04=(a04+a14); + a14=-3.0801331505514837e-03; + a14=(a14*a08); + a04=(a04+a14); + a14=6.0570157695918683e-03; + a14=(a14*a09); + a04=(a04+a14); + a14=1.0920100546139184e-05; + a14=(a14*a10); + a04=(a04+a14); + a14=(a13*a04); + a14=(a11*a14); + a06=(a06+a14); + a14=(a00*a13); + a15=(a14*a04); + a06=(a06+a15); + a15=arg[0]? arg[0][4] : 0; + a16=5.5581350085139340e-06; + a16=(a16*a03); + a17=-3.0801331505514781e-03; + a17=(a17*a05); + a16=(a16+a17); + a17=-2.9967785627100469e-02; + a17=(a17*a07); + a16=(a16+a17); + a17=3.3963490377380855e-02; + a17=(a17*a08); + a16=(a16+a17); + a17=-6.1658244361114702e-05; + a17=(a17*a09); + a16=(a16+a17); + a17=-1.1116270017027866e-07; + a17=(a17*a10); + a16=(a16+a17); + a17=(a15*a16); + a17=(a00*a17); + a06=(a06+a17); + a17=arg[0]? arg[0][5] : 0; + a18=-3.0184487502609133e-03; + a18=(a18*a03); + a19=6.0570157695918657e-03; + a19=(a19*a05); + a18=(a18+a19); + a19=5.4404333259807062e-05; + a19=(a19*a07); + a18=(a18+a19); + a19=-6.1658244361114783e-05; + a19=(a19*a08); + a18=(a18+a19); + a19=3.3484658136227786e-02; + a19=(a19*a09); + a18=(a18+a19); + a19=6.0368975005218254e-05; + a19=(a19*a10); + a18=(a18+a19); + a19=(a17*a18); + a19=(a11*a19); + a06=(a06+a19); + a19=arg[0]? arg[0][0] : 0; + a20=casadi_fabs(a19); + a21=-6.0150572994295635e-03; + a21=(a21*a03); + a22=1.0920100546139210e-05; + a22=(a22*a05); + a21=(a21+a22); + a22=9.8084735444364535e-08; + a22=(a22*a07); + a21=(a21+a22); + a22=-1.1116270017027936e-07; + a22=(a22*a08); + a21=(a21+a22); + a22=6.0368975005218423e-05; + a22=(a22*a09); + a21=(a21+a22); + a22=3.3453634479321918e-02; + a22=(a22*a10); + a21=(a21+a22); + a20=(a20*a21); + a06=(a06+a20); + a20=casadi_sign(a19); + a22=(a19*a21); + a20=(a20*a22); + a06=(a06+a20); + a20=23.; + a20=(a20*a21); + a06=(a06+a20); + a06=(-a06); + if (res[0]!=0) res[0][0]=a06; + a06=(a00*a19); + a20=(a06*a02); + a22=(a19*a02); + a22=(a11*a22); + a20=(a20+a22); + a22=-4.9042367722181935e-06; + a22=(a22*a03); + a03=2.7177645446042498e-03; + a03=(a03*a05); + a22=(a22+a03); + a03=1.4970303990827358e+00; + a03=(a03*a07); + a22=(a22+a03); + a03=-2.9967785627100754e-02; + a03=(a03*a08); + a22=(a22+a03); + a03=5.4404333259807184e-05; + a03=(a03*a09); + a22=(a22+a03); + a03=9.8084735444363873e-08; + a03=(a03*a10); + a22=(a22+a03); + a03=(a13*a22); + a03=(a00*a03); + a20=(a20+a03); + a03=(a11*a13); + a10=(a03*a22); + a20=(a20+a10); + a10=arg[0]? arg[0][3] : 0; + a09=(a10*a16); + a09=(a11*a09); + a20=(a20+a09); + a09=casadi_fabs(a01); + a09=(a09*a18); + a20=(a20+a09); + a09=casadi_sign(a01); + a08=(a01*a18); + a09=(a09*a08); + a20=(a20+a09); + a09=46.; + a08=(a09*a18); + a20=(a20+a08); + a08=(a17*a21); + a08=(a00*a08); + a20=(a20+a08); + a20=(-a20); + if (res[0]!=0) res[0][1]=a20; + a20=(a11*a19); + a08=(a20*a04); + a19=(a19*a04); + a19=(a00*a19); + a08=(a08+a19); + a19=(a00*a01); + a07=(a19*a22); + a08=(a08+a07); + a01=(a01*a22); + a01=(a11*a01); + a08=(a08+a01); + a01=casadi_fabs(a13); + a01=(a01*a16); + a08=(a08+a01); + a01=casadi_sign(a13); + a13=(a13*a16); + a01=(a01*a13); + a08=(a08+a01); + a01=(a09*a16); + a08=(a08+a01); + a01=(a10*a18); + a00=(a00*a01); + a08=(a08+a00); + a00=(a15*a21); + a11=(a11*a00); + a08=(a08+a11); + a08=(-a08); + if (res[0]!=0) res[0][2]=a08; + a08=arg[1]? arg[1][6] : 0; + a11=6.8000000000000005e-01; + a00=(a15*a02); + a00=(a11*a00); + a00=(a08-a00); + a01=-3.3199999999999998e+00; + a13=(a01*a15); + a13=(a13*a02); + a00=(a00-a13); + a13=-6.8000000000000005e-01; + a07=(a17*a04); + a07=(a13*a07); + a00=(a00-a07); + a07=3.3399999999999999e+00; + a05=(a07*a17); + a05=(a05*a04); + a00=(a00-a05); + a05=casadi_fabs(a10); + a05=(a05*a22); + a00=(a00-a05); + a05=casadi_sign(a10); + a23=(a10*a22); + a05=(a05*a23); + a00=(a00-a05); + a05=(a09*a22); + a00=(a00-a05); + a12=(a12*a16); + a00=(a00-a12); + a14=(a14*a18); + a00=(a00-a14); + if (res[0]!=0) res[0][3]=a00; + a00=arg[0]? arg[0][6] : 0; + a14=sin(a00); + a12=arg[0]? arg[0][7] : 0; + a05=cos(a12); + a23=(a14/a05); + a24=arg[1]? arg[1][8] : 0; + a25=(a23*a24); + a00=cos(a00); + a26=arg[1]? arg[1][7] : 0; + a27=(a00*a26); + a25=(a25+a27); + a27=tan(a12); + a28=(a14*a27); + a28=(a28*a08); + a25=(a25+a28); + a11=(a11*a10); + a11=(a11*a02); + a25=(a25-a11); + a11=(a10*a02); + a01=(a01*a11); + a25=(a25-a01); + a01=casadi_fabs(a15); + a01=(a01*a04); + a25=(a25-a01); + a01=casadi_sign(a15); + a11=(a15*a04); + a01=(a01*a11); + a25=(a25-a01); + a01=(a09*a04); + a25=(a25-a01); + a01=3.3199999999999998e+00; + a11=(a17*a22); + a11=(a01*a11); + a25=(a25-a11); + a11=-3.3399999999999999e+00; + a28=(a11*a17); + a28=(a28*a22); + a25=(a25-a28); + a06=(a06*a16); + a25=(a25-a06); + a03=(a03*a21); + a25=(a25-a03); + if (res[0]!=0) res[0][4]=a25; + a25=(a00/a05); + a03=(a25*a24); + a06=(a14*a26); + a03=(a03-a06); + a06=(a00*a27); + a06=(a06*a08); + a03=(a03+a06); + a06=casadi_fabs(a17); + a06=(a06*a02); + a03=(a03-a06); + a06=casadi_sign(a17); + a16=(a17*a02); + a06=(a06*a16); + a03=(a03-a06); + a09=(a09*a02); + a03=(a03-a09); + a13=(a13*a10); + a13=(a13*a04); + a03=(a03-a13); + a10=(a10*a04); + a07=(a07*a10); + a03=(a03-a07); + a01=(a01*a15); + a01=(a01*a22); + a03=(a03-a01); + a22=(a15*a22); + a11=(a11*a22); + a03=(a03-a11); + a20=(a20*a18); + a03=(a03-a20); + a19=(a19*a21); + a03=(a03-a19); + if (res[0]!=0) res[0][5]=a03; + a03=(a15*a24); + a19=(a03/a05); + a19=(a00*a19); + a24=(a17*a24); + a20=(a24/a05); + a20=(a14*a20); + a19=(a19-a20); + a20=(a17*a26); + a20=(a00*a20); + a19=(a19-a20); + a26=(a15*a26); + a26=(a14*a26); + a19=(a19-a26); + a17=(a17*a08); + a26=(a27*a17); + a26=(a14*a26); + a19=(a19-a26); + a15=(a15*a08); + a27=(a27*a15); + a27=(a00*a27); + a19=(a19+a27); + if (res[0]!=0) res[0][6]=a19; + a12=sin(a12); + a25=(a25/a05); + a25=(a25*a24); + a25=(a12*a25); + a23=(a23/a05); + a23=(a23*a03); + a12=(a12*a23); + a25=(a25+a12); + a00=(a00*a17); + a05=casadi_sq(a05); + a00=(a00/a05); + a25=(a25+a00); + a14=(a14*a15); + a14=(a14/a05); + a25=(a25+a14); + if (res[0]!=0) res[0][7]=a25; + a25=0.; + if (res[0]!=0) res[0][8]=a25; + if (res[0]!=0) res[0][9]=a21; + if (res[0]!=0) res[0][10]=a04; + if (res[0]!=0) res[0][11]=a02; + return 0; +} + +CASADI_SYMBOL_EXPORT int auv_model_expl_vde_adj(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ + return casadi_f0(arg, res, iw, w, mem); +} + +CASADI_SYMBOL_EXPORT int auv_model_expl_vde_adj_alloc_mem(void) { + return 0; +} + +CASADI_SYMBOL_EXPORT int auv_model_expl_vde_adj_init_mem(int mem) { + return 0; +} + +CASADI_SYMBOL_EXPORT void auv_model_expl_vde_adj_free_mem(int mem) { +} + +CASADI_SYMBOL_EXPORT int auv_model_expl_vde_adj_checkout(void) { + return 0; +} + +CASADI_SYMBOL_EXPORT void auv_model_expl_vde_adj_release(int mem) { +} + +CASADI_SYMBOL_EXPORT void auv_model_expl_vde_adj_incref(void) { +} + +CASADI_SYMBOL_EXPORT void auv_model_expl_vde_adj_decref(void) { +} + +CASADI_SYMBOL_EXPORT casadi_int auv_model_expl_vde_adj_n_in(void) { return 4;} + +CASADI_SYMBOL_EXPORT casadi_int auv_model_expl_vde_adj_n_out(void) { return 1;} + +CASADI_SYMBOL_EXPORT casadi_real auv_model_expl_vde_adj_default_in(casadi_int i) { + switch (i) { + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const char* auv_model_expl_vde_adj_name_in(casadi_int i) { + switch (i) { + case 0: return "i0"; + case 1: return "i1"; + case 2: return "i2"; + case 3: return "i3"; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const char* auv_model_expl_vde_adj_name_out(casadi_int i) { + switch (i) { + case 0: return "o0"; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const casadi_int* auv_model_expl_vde_adj_sparsity_in(casadi_int i) { + switch (i) { + case 0: return casadi_s0; + case 1: return casadi_s0; + case 2: return casadi_s1; + case 3: return casadi_s2; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const casadi_int* auv_model_expl_vde_adj_sparsity_out(casadi_int i) { + switch (i) { + case 0: return casadi_s3; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT int auv_model_expl_vde_adj_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { + if (sz_arg) *sz_arg = 4; + if (sz_res) *sz_res = 1; + if (sz_iw) *sz_iw = 0; + if (sz_w) *sz_w = 0; + return 0; +} + +CASADI_SYMBOL_EXPORT int auv_model_expl_vde_adj_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { + if (sz_arg) *sz_arg = 4*sizeof(const casadi_real*); + if (sz_res) *sz_res = 1*sizeof(casadi_real*); + if (sz_iw) *sz_iw = 0*sizeof(casadi_int); + if (sz_w) *sz_w = 0*sizeof(casadi_real); + return 0; +} + + +#ifdef __cplusplus +} /* extern "C" */ +#endif diff --git a/control/velocity_controller/src/build_auv_solver/auv_model_model/auv_model_expl_vde_adj.o b/control/velocity_controller/src/build_auv_solver/auv_model_model/auv_model_expl_vde_adj.o new file mode 100644 index 0000000000000000000000000000000000000000..669b0a2701794ffd0473b5ba7e704e90de694d2e GIT binary patch literal 12672 zcmbuF4|LVVmB)V}C@IqX-fOjHi^XUCp$~+Z#N5SPTl0X1$pE$y;4o?4G~J(X@bE2~%@1SO(kR#0(eg|ll_ z#yqyCopavrH}jc$@66npxii1Oipr|1CuLhW>!VL~5+)IaUn=#^U-<37u`d9>_+Y;`gT?zNbZ4eJTx&nnBvO)Gn z+-TY?m)}@kvnaCqFDUC`6w{<|KYkkGQq}1F0MGM#|mV4?uo%$^hhcq5ik1 z|3{yy2O1nF@El6DJ0P(nM@vZa5PkmBXt%WE@Aksb;%=kWj|rb3%c`4Denue|k~DHH zi9XiuG|HHkapjM!^2b*B6RQj__$xfatWRQSJr@v2rSS zA@zC>3nL$5j`7$|gCen$R3ayxK!3vUEc%Hh8LgoCkS-~YcEKrdz7Mz?LxCE30&4y( zUK$NRzmz5NkgM?U^vApoJU4lpx!V7rH=r^#UOjk~NAhz#+9{;XW#1eRSqLQl-d#=$ zNhx6|e}Lcr0*skk>#b+SsB(zwZz8i%SsDIf7{Vs%S52<-R&l2GBbdPwS4zka!X)=| z{IWsKel11ARFd$S8^n^)jj-3$Si8Ma9!O+OHY`%(m}>Ngj3+AGU>aG~3Ct|=x@SH$ zjMUd?YHP?PSfJ`?{X6=8&o!vB{6d@=I)V>Fj^6D|3 z)KoOn3@J<}51@d3q2~~*UHb<9+s8|L3-<+UIskXh!bO$hBg@ zxzW97^A(n;M`<^HI)Fw2Bw>^FTvDy#F}dA*5+X zA(klxUWw44wJuH6QfM54d#l*JksR&^dJ0p`z$wO?c$HaWZ~oNSx2mx>ah$2*u`5lO zYvo0Ze!?jQXdg~ij?q0K*F+!n=sZA|;01Ur473*`rcQ1n7eJpJR6C4nbPy$S28~rF zc5JNT9P+OoDv2s5m4d2xu!Ak@kP`MqcEW||0*zFLmcPSfE@7o{^AmC5~5b5Q^0;4*JLd>fs(}!g~-Cl=!>H4ZM zE_8x+W<4zjY<>iCv9LiRf0R}c#)IkbuhZdvEr)wGu@QGU)+R}CSVwUDw30$_S~#Nb@k9y@+eDchw3VtjDMhM=m>T}E&mA%7$Q zUO@}cUqfLI4ONf0i(_(HhAn6L3}-=p%u`>=#?;`rcu-9sauoFOdQ|xiIa~_pc&9kR zJpw48W8_7mZ-TZ`FQA7h!)*XHhmk?Zpz*=G=qMry97YEKxd<2HpyVxA1;-pR;LwtZ z!XdF85dWPxhIT*28|7AgsSqE@9z}KZ0Dd^dD8R8pu>jc~scw+-8v!Lk$0;pCx`vP= zeKaQ8w{UUnIW$ZYw^kw~!Z>+x5^Wp<~d z5efx;ue-fJz=(MVa~;IEA2nyFDTxQi!q^455js9 ziwqGe!XtGT;}#Wd;wX3!p2lgp7~`x(NW%odSN@wIh%A{f#)UNY-*f2JD7m6w@9m)D= zbQIr{Xk#X7oyx-Zt5$E~U3uN=_o`OobB-R(P*5LrO`afc-(RParg zKe(bN;0O9J(KlKwUnPra$7huu=4^6qB44|=pT2(!)A+;?e@|2%G!*`ldRyKuZ=Q_7 zP?MM!vbQ;Fe3_hReve1)=J>I4bW-Yy6&FdVZ#0#^ay07+@{^#5Te)59tsKo3sw7lZ znA__c{gl?Y@f+=3vf>NlWfxKl==a}bAALV_=M?fnr0;!X&m;$6^B_Bk$J6&LvIMb7 zP46Xmz9LFLfNIgFwUbf5rNC(^bXtm>mXOn407**t(*ytAg&ScqzKnBeT>WRPvZy$t zDy`6EdH0MjAEkRT($O;YUJy<`z5Cr&WqZo1`{%q-dZ>Ksx$9rN@i+JF{nh#dy}eHL zs|$MMU3JlpLw8=$vUTRr?r8@fyMODG-+O)Fk;Vtg!dET-m*H8ytykXR``y%6|LIrR z;o3zrpJ?8CVdKRQE%-^-){uYqy!M;-lpQH}?uq_|iL$8J3I4cZ*H>QbD*O4b_ZK{N z`0#?6QvR?^>96_X?1#_1Yg*aWV*ljR^B+6xJy7=8j2ABb{zI^q}J zYHIFS@Ga@jkjNhxoVw}dzyqQ;FFakwc}V*8(rYixeC62jvfE^Q|0eBE7JJHH*M#rg z{^wxq-l^%$Gy2~C`sS(OK`L&4vrOJwr9Le6Y3G`S3oj1@7cE)VmRc4l4CNQ*7tAeP z#>K+rMMe1q#j^x+6SNp99??#_=K4hr)?u0$tpiGDtCy2?#|=(amoIDjv?)3LSUjY2 zp44%KL!OvkXY-|w7`6>3T1%9!#IKffj&mp%nX7Ys_e{DfH*alrd2ZmY$rZW5m1kU= z+dnCL@3g78!G*bj^4z?ua(!3j=2YU(Dcla-^(jqJFJ_NQ-ehTE97&L;{_GWI%Dpv# z+{Y6zpKKh{o+oUo$Wt4o6R;{_+7BdGFRaSYO$j^KV5@|Y?kJ4FXdv&O3g>g( zsQa1lQiD-;qwp}lPd)xqc<>bB+>^q!pLO*%;Smkwy;FF##a|Gv_oc4hBfMS%dA}yS z+2RL;cUb&Q;awJgTX>Jf-xa>f;>U&eTl}Q(^%l?OwV&fWZ1K+sAF#M9e51wB5x&{t z(}kxkeu3~Ii(f2!*y3}A@3VMF_(6-$7yg#T%Y+}bc%|?Y7XPYnduxAPcn-f$DSbn@ zj)S`9X5o33e53Gy#ao02E#5A?z~ak<>$s}h{f_XkC4Z;zh{eAvyxQV-3$L~K_k`D5 z{D;DuE&gNS9Txv7m(TcAB>Q*adOZ653*q`4*8Fkd`V7(hDdA?^q`E=y&l!`e&*}rh zePTeL*B=S57OvYl2NwgfQ!kv)ZRF>GYa3FXgx6cV zTX=_XeNJx>-ebvc5#Ddfzb<^h;vWd#Xz_C~QPj`0aD5jD2_Lq2weW+&wVykM9~E9D zPQ6cfSe&iz2EPzqFI?Xb_6i@c~D;L^wI#IG9E+-X?$?YgDyP4U*ccvnYj z-M5?Kbq!75%x}z4QI{;tSW}R(rYK`g$jPr-SWJ)0=utqAB6<|kql6x%^q9wtVIkMg zXTb7>Y^0E_6tYSo8!J>wtY66TMan*#DPl84tXsrdMQovnb&FWHh;@rtH^jOj)(x?4 zh;>7(6=Dk^)(x?4h;>7(Tg7*LkD4Rj5uA#NHy|Hd-d}$_umbRAE z>FN`m@%ZWL8=K>ex3(`!WwPHHZ;dx3ZK(eEuGue{5{WK=VL{4Mow)W0jGr5N9>XzJ|iYF05awKjcTsjmB zKbAijTw-wgmeX8+Ba%FwVVbLZyF=fl4xL?^uS1>^af+Rqzd;a*IDJ}b{(FK*#3{6D zK0*+QIE6&b^#>4f3W=JZ#pkS&|D3_kHu&ca9yU0AOKUp~2B&|;(tNqWzhLnD3{J6M z%Rgf9JcB=J@V_(o9)r`jo3`^?gHJd3F@v9H@N9An620hO@3o!J8+?YrFEsf11}`yq zz~Gezzrf&&4E|+<#|?g=!Iv4_Gx$RWpK0*j8Tfpho0O>9l)qm%j!&lyKkbJDhWtea z|AWCVHu%|O0Ez4e@zZu@8+?|*FE{vXgWq6q+H2a*EyC#)Hu!EsevZM98+@+8M-6WJ z!Ig^^^*`T`pKkC=4Su1)3k-gV!3zx@GI)`}lftQ6rvFzOJZ#9nX4si&@PPd3f%;E( zAMLj*4Q|S}3%A={VeqhF=a~$4rpblYZueq?o9!kIPJd(3{k+%U^iSNH|3tXm&;KyE z+0Um8JC_-D&Xs^p{hVj;YYlGt?U%x--Gzqys|K$y_;G_*8oXOB#&-YL8a!;se$4~QL z8vJU5$K->K+9eli`MV50?%Xsuxk}47(P4>1cBpHbKWT8&z9-)&ByXI2G27%eY@y(^9@e_`$_Zb4ZgtOw-}t3Fr8JD?yxw;I7;_eT-@vY+~T_30Bb4# z&=_>0HAaaT{Y2e~5Jb|vS{0p`yfv?v@&=1niyt>yyk7VYi+2due?QXpdxXDZ$@dG# zCo3l{KOo$^&rsb);rj12T0SlOO3Ti$aQ$}{Eq_pWlO=ytc$dY);?T7guNOXG@d4qR zES?rVWO2USkq7dVw=YdKEWta~sou>xhYyB^{8YRv<>WUtq#B(3CCQ|d-`P$D5--fh zXO}AKcYb3sMIV_Wgc82k8rzpHjkm!{TYD;=kAH2Mn@lw{-YVr=+Lq;abhdZIJ5#qq zk=l$m*Cje}@8=o%+a3X^)6nG@8Z#D_Tu8qAb%nS~7ah72QcMbu5tTW5fqD;1{HAb0 z?MTs~J1M1$@FOnQA3~gxR=b*$teI%vlFh$XpY9T5lkN^?((TLg8>`ao>vtG6l&5uU zrd85^id|;X7<+YyX-N;Z}w*9wW^aEN@-n!o!uS14x+WMPCf1lN$ zt$#0cCtAPh*{X$uqEG908h%<|uiuH + +#ifndef casadi_real +#define casadi_real double +#endif + +#ifndef casadi_int +#define casadi_int int +#endif + +/* Add prefix to internal symbols */ +#define casadi_f0 CASADI_PREFIX(f0) +#define casadi_fabs CASADI_PREFIX(fabs) +#define casadi_s0 CASADI_PREFIX(s0) +#define casadi_s1 CASADI_PREFIX(s1) +#define casadi_s2 CASADI_PREFIX(s2) +#define casadi_s3 CASADI_PREFIX(s3) +#define casadi_s4 CASADI_PREFIX(s4) +#define casadi_sign CASADI_PREFIX(sign) +#define casadi_sq CASADI_PREFIX(sq) + +/* Symbol visibility in DLLs */ +#ifndef CASADI_SYMBOL_EXPORT + #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) + #if defined(STATIC_LINKED) + #define CASADI_SYMBOL_EXPORT + #else + #define CASADI_SYMBOL_EXPORT __declspec(dllexport) + #endif + #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) + #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default"))) + #else + #define CASADI_SYMBOL_EXPORT + #endif +#endif + +casadi_real casadi_fabs(casadi_real x) { +/* Pre-c99 compatibility */ +#if __STDC_VERSION__ < 199901L + return x>0 ? x : -x; +#else + return fabs(x); +#endif +} + +casadi_real casadi_sign(casadi_real x) { return x<0 ? -1 : x>0 ? 1 : x;} + +casadi_real casadi_sq(casadi_real x) { return x*x;} + +static const casadi_int casadi_s0[3] = {9, 1, 1}; +static const casadi_int casadi_s1[3] = {9, 9, 1}; +static const casadi_int casadi_s2[3] = {9, 3, 1}; +static const casadi_int casadi_s3[3] = {3, 1, 1}; +static const casadi_int casadi_s4[3] = {0, 1, 1}; + +/* auv_model_expl_vde_forw:(i0[9],i1[9x9],i2[9x3],i3[3],i4[0])->(o0[9],o1[9x9],o2[9x3]) */ +static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { + casadi_real a000, a001, a002, a003, a004, a005, a006, a007, a008, a009, a010, a011; + casadi_real a012, a013, a014, a015, a016, a017, a018, a019, a020, a021, a022, a023; + casadi_real a024, a025, a026, a027, a028, a029, a030, a031, a032, a033, a034, a035; + casadi_real a036, a037, a038, a039, a040, a041, a042, a043, a044, a045, a046, a047; + casadi_real a048, a049, a050, a051, a052, a053, a054, a055, a056, a057, a058, a059; + casadi_real a060, a061, a062, a063, a064, a065, a066, a067, a068, a069, a070, a071; + casadi_real a072, a073, a074, a075, a076, a077, a078, a079, a080, a081, a082, a083; + casadi_real a084, a085, a086, a087, a088, a089, a090, a091, a092, a093, a094, a095; + casadi_real a096, a097, a098, a099, a100, a101, a102, a103, a104, a105; + a000=3.3453634479321918e-02; + a001=arg[3]? arg[3][0] : 0; + a002=30.; + a003=arg[0]? arg[0][2] : 0; + a004=(a002*a003); + a005=arg[0]? arg[0][4] : 0; + a006=(a004*a005); + a007=-30.; + a008=arg[0]? arg[0][1] : 0; + a009=(a007*a008); + a010=arg[0]? arg[0][5] : 0; + a011=(a009*a010); + a006=(a006+a011); + a001=(a001-a006); + a006=23.; + a011=arg[0]? arg[0][0] : 0; + a012=(a006*a011); + a013=casadi_fabs(a011); + a014=(a013*a011); + a012=(a012+a014); + a001=(a001-a012); + a012=(a000*a001); + a014=6.0368975005218254e-05; + a015=(a007*a003); + a016=arg[0]? arg[0][3] : 0; + a017=(a015*a016); + a018=(a002*a011); + a019=(a018*a010); + a017=(a017+a019); + a019=46.; + a020=(a019*a008); + a021=casadi_fabs(a008); + a022=(a021*a008); + a020=(a020+a022); + a017=(a017+a020); + a020=(a014*a017); + a012=(a012-a020); + a020=-1.1116270017027866e-07; + a022=(a002*a008); + a023=(a022*a016); + a024=(a007*a011); + a025=(a024*a005); + a023=(a023+a025); + a025=(a019*a003); + a026=casadi_fabs(a003); + a027=(a026*a003); + a025=(a025+a027); + a023=(a023+a025); + a025=(a020*a023); + a012=(a012-a025); + a025=9.8084735444363873e-08; + a027=(a004*a008); + a028=(a009*a003); + a027=(a027+a028); + a028=-3.3399999999999999e+00; + a029=(a028*a010); + a030=(a029*a005); + a027=(a027+a030); + a030=3.3199999999999998e+00; + a031=(a030*a005); + a032=(a031*a010); + a027=(a027+a032); + a032=(a019*a016); + a033=casadi_fabs(a016); + a034=(a033*a016); + a032=(a032+a034); + a027=(a027+a032); + a032=(a025*a027); + a012=(a012-a032); + a032=1.0920100546139184e-05; + a034=arg[3]? arg[3][1] : 0; + a035=(a015*a011); + a036=(a018*a003); + a035=(a035+a036); + a036=3.3399999999999999e+00; + a037=(a036*a010); + a038=(a037*a016); + a035=(a035+a038); + a038=-6.8000000000000005e-01; + a039=(a038*a016); + a040=(a039*a010); + a035=(a035+a040); + a034=(a034-a035); + a035=(a019*a005); + a040=casadi_fabs(a005); + a041=(a040*a005); + a035=(a035+a041); + a034=(a034-a035); + a035=(a032*a034); + a012=(a012+a035); + a035=-6.0150572994295574e-03; + a041=arg[3]? arg[3][2] : 0; + a042=(a022*a011); + a043=(a024*a008); + a042=(a042+a043); + a043=-3.3199999999999998e+00; + a044=(a043*a005); + a045=(a044*a016); + a042=(a042+a045); + a045=6.8000000000000005e-01; + a046=(a045*a016); + a047=(a046*a005); + a042=(a042+a047); + a041=(a041-a042); + a042=(a019*a010); + a047=casadi_fabs(a010); + a048=(a047*a010); + a042=(a042+a048); + a041=(a041-a042); + a042=(a035*a041); + a012=(a012+a042); + if (res[0]!=0) res[0][0]=a012; + a012=6.0368975005218423e-05; + a042=(a012*a001); + a048=3.3484658136227786e-02; + a049=(a048*a017); + a042=(a042-a049); + a049=-6.1658244361114702e-05; + a050=(a049*a023); + a042=(a042-a050); + a050=5.4404333259807184e-05; + a051=(a050*a027); + a042=(a042-a051); + a051=6.0570157695918683e-03; + a052=(a051*a034); + a042=(a042+a052); + a052=-3.0184487502609172e-03; + a053=(a052*a041); + a042=(a042+a053); + if (res[0]!=0) res[0][1]=a042; + a042=-1.1116270017027936e-07; + a053=(a042*a001); + a054=-6.1658244361114783e-05; + a055=(a054*a017); + a053=(a053-a055); + a055=3.3963490377380855e-02; + a056=(a055*a023); + a053=(a053-a056); + a056=-2.9967785627100754e-02; + a057=(a056*a027); + a053=(a053-a057); + a057=-3.0801331505514837e-03; + a058=(a057*a034); + a053=(a053+a058); + a058=5.5581350085139543e-06; + a059=(a058*a041); + a053=(a053+a059); + if (res[0]!=0) res[0][2]=a053; + a053=9.8084735444364535e-08; + a059=(a053*a001); + a060=5.4404333259807062e-05; + a061=(a060*a017); + a059=(a059-a061); + a061=-2.9967785627100469e-02; + a062=(a061*a023); + a059=(a059-a062); + a062=1.4970303990827358e+00; + a063=(a062*a027); + a059=(a059-a063); + a063=2.7177645446042520e-03; + a064=(a063*a034); + a059=(a059+a064); + a064=-4.9042367722181902e-06; + a065=(a064*a041); + a059=(a059+a065); + if (res[0]!=0) res[0][3]=a059; + a059=1.0920100546139210e-05; + a065=(a059*a001); + a066=6.0570157695918657e-03; + a067=(a066*a017); + a065=(a065-a067); + a067=-3.0801331505514781e-03; + a068=(a067*a023); + a065=(a065-a068); + a068=2.7177645446042498e-03; + a069=(a068*a027); + a065=(a065-a069); + a069=3.0257778596593993e-01; + a070=(a069*a034); + a065=(a065+a070); + a070=-5.4600502730695923e-04; + a071=(a070*a041); + a065=(a065+a071); + if (res[0]!=0) res[0][4]=a065; + a065=-6.0150572994295635e-03; + a001=(a065*a001); + a071=-3.0184487502609133e-03; + a017=(a071*a017); + a001=(a001-a017); + a017=5.5581350085139340e-06; + a023=(a017*a023); + a001=(a001-a023); + a023=-4.9042367722181935e-06; + a027=(a023*a027); + a001=(a001-a027); + a034=(a070*a034); + a001=(a001+a034); + a034=3.0075286497147785e-01; + a041=(a034*a041); + a001=(a001+a041); + if (res[0]!=0) res[0][5]=a001; + a001=arg[0]? arg[0][6] : 0; + a041=sin(a001); + a027=arg[0]? arg[0][7] : 0; + a072=tan(a027); + a073=(a041*a072); + a074=(a073*a005); + a074=(a016+a074); + a001=cos(a001); + a075=(a001*a072); + a076=(a075*a010); + a074=(a074+a076); + if (res[0]!=0) res[0][6]=a074; + a074=(a001*a005); + a076=(a041*a010); + a074=(a074-a076); + if (res[0]!=0) res[0][7]=a074; + a074=cos(a027); + a076=(a041/a074); + a077=(a076*a005); + a078=(a001/a074); + a079=(a078*a010); + a077=(a077+a079); + if (res[0]!=0) res[0][8]=a077; + a077=arg[1]? arg[1][2] : 0; + a079=(a002*a077); + a080=(a005*a079); + a081=arg[1]? arg[1][4] : 0; + a082=(a004*a081); + a080=(a080+a082); + a082=arg[1]? arg[1][1] : 0; + a083=(a007*a082); + a084=(a010*a083); + a085=arg[1]? arg[1][5] : 0; + a086=(a009*a085); + a084=(a084+a086); + a080=(a080+a084); + a084=arg[1]? arg[1][0] : 0; + a086=(a006*a084); + a087=casadi_sign(a011); + a088=(a087*a084); + a088=(a011*a088); + a089=(a013*a084); + a088=(a088+a089); + a086=(a086+a088); + a080=(a080+a086); + a086=(a000*a080); + a088=(a007*a077); + a089=(a016*a088); + a090=arg[1]? arg[1][3] : 0; + a091=(a015*a090); + a089=(a089+a091); + a091=(a002*a084); + a092=(a010*a091); + a093=(a018*a085); + a092=(a092+a093); + a089=(a089+a092); + a092=(a019*a082); + a093=casadi_sign(a008); + a094=(a093*a082); + a094=(a008*a094); + a095=(a021*a082); + a094=(a094+a095); + a092=(a092+a094); + a089=(a089+a092); + a092=(a014*a089); + a086=(a086+a092); + a092=(a002*a082); + a094=(a016*a092); + a095=(a022*a090); + a094=(a094+a095); + a095=(a007*a084); + a096=(a005*a095); + a097=(a024*a081); + a096=(a096+a097); + a094=(a094+a096); + a096=(a019*a077); + a097=casadi_sign(a003); + a098=(a097*a077); + a098=(a003*a098); + a099=(a026*a077); + a098=(a098+a099); + a096=(a096+a098); + a094=(a094+a096); + a096=(a020*a094); + a086=(a086+a096); + a079=(a008*a079); + a096=(a004*a082); + a079=(a079+a096); + a083=(a003*a083); + a096=(a009*a077); + a083=(a083+a096); + a079=(a079+a083); + a083=(a028*a085); + a083=(a005*a083); + a096=(a029*a081); + a083=(a083+a096); + a079=(a079+a083); + a083=(a030*a081); + a083=(a010*a083); + a096=(a031*a085); + a083=(a083+a096); + a079=(a079+a083); + a083=(a019*a090); + a096=casadi_sign(a016); + a098=(a096*a090); + a098=(a016*a098); + a099=(a033*a090); + a098=(a098+a099); + a083=(a083+a098); + a079=(a079+a083); + a083=(a025*a079); + a086=(a086+a083); + a088=(a011*a088); + a083=(a015*a084); + a088=(a088+a083); + a091=(a003*a091); + a077=(a018*a077); + a091=(a091+a077); + a088=(a088+a091); + a091=(a036*a085); + a091=(a016*a091); + a077=(a037*a090); + a091=(a091+a077); + a088=(a088+a091); + a091=(a038*a090); + a091=(a010*a091); + a077=(a039*a085); + a091=(a091+a077); + a088=(a088+a091); + a091=(a019*a081); + a077=casadi_sign(a005); + a083=(a077*a081); + a083=(a005*a083); + a098=(a040*a081); + a083=(a083+a098); + a091=(a091+a083); + a088=(a088+a091); + a091=(a032*a088); + a086=(a086+a091); + a092=(a011*a092); + a084=(a022*a084); + a092=(a092+a084); + a095=(a008*a095); + a082=(a024*a082); + a095=(a095+a082); + a092=(a092+a095); + a095=(a043*a081); + a095=(a016*a095); + a082=(a044*a090); + a095=(a095+a082); + a092=(a092+a095); + a095=(a045*a090); + a095=(a005*a095); + a082=(a046*a081); + a095=(a095+a082); + a092=(a092+a095); + a095=(a019*a085); + a082=casadi_sign(a010); + a084=(a082*a085); + a084=(a010*a084); + a091=(a047*a085); + a084=(a084+a091); + a095=(a095+a084); + a092=(a092+a095); + a095=(a035*a092); + a086=(a086+a095); + a086=(-a086); + if (res[1]!=0) res[1][0]=a086; + a086=(a012*a080); + a095=(a048*a089); + a086=(a086+a095); + a095=(a049*a094); + a086=(a086+a095); + a095=(a050*a079); + a086=(a086+a095); + a095=(a051*a088); + a086=(a086+a095); + a095=(a052*a092); + a086=(a086+a095); + a086=(-a086); + if (res[1]!=0) res[1][1]=a086; + a086=(a042*a080); + a095=(a054*a089); + a086=(a086+a095); + a095=(a055*a094); + a086=(a086+a095); + a095=(a056*a079); + a086=(a086+a095); + a095=(a057*a088); + a086=(a086+a095); + a095=(a058*a092); + a086=(a086+a095); + a086=(-a086); + if (res[1]!=0) res[1][2]=a086; + a086=(a053*a080); + a095=(a060*a089); + a086=(a086+a095); + a095=(a061*a094); + a086=(a086+a095); + a095=(a062*a079); + a086=(a086+a095); + a095=(a063*a088); + a086=(a086+a095); + a095=(a064*a092); + a086=(a086+a095); + a086=(-a086); + if (res[1]!=0) res[1][3]=a086; + a086=(a059*a080); + a095=(a066*a089); + a086=(a086+a095); + a095=(a067*a094); + a086=(a086+a095); + a095=(a068*a079); + a086=(a086+a095); + a095=(a069*a088); + a086=(a086+a095); + a095=(a070*a092); + a086=(a086+a095); + a086=(-a086); + if (res[1]!=0) res[1][4]=a086; + a080=(a065*a080); + a089=(a071*a089); + a080=(a080+a089); + a094=(a017*a094); + a080=(a080+a094); + a079=(a023*a079); + a080=(a080+a079); + a088=(a070*a088); + a080=(a080+a088); + a092=(a034*a092); + a080=(a080+a092); + a080=(-a080); + if (res[1]!=0) res[1][5]=a080; + a080=arg[1]? arg[1][6] : 0; + a092=(a001*a080); + a088=(a072*a092); + a079=arg[1]? arg[1][7] : 0; + a094=casadi_sq(a074); + a089=(a079/a094); + a086=(a041*a089); + a088=(a088+a086); + a088=(a005*a088); + a086=(a073*a081); + a088=(a088+a086); + a090=(a090+a088); + a089=(a001*a089); + a080=(a041*a080); + a088=(a072*a080); + a089=(a089-a088); + a089=(a010*a089); + a088=(a075*a085); + a089=(a089+a088); + a090=(a090+a089); + if (res[1]!=0) res[1][6]=a090; + a090=(a001*a081); + a089=(a005*a080); + a090=(a090-a089); + a089=(a010*a092); + a088=(a041*a085); + a089=(a089+a088); + a090=(a090-a089); + if (res[1]!=0) res[1][7]=a090; + a092=(a092/a074); + a090=(a076/a074); + a027=sin(a027); + a079=(a027*a079); + a089=(a090*a079); + a092=(a092+a089); + a092=(a005*a092); + a081=(a076*a081); + a092=(a092+a081); + a081=(a078/a074); + a079=(a081*a079); + a080=(a080/a074); + a079=(a079-a080); + a079=(a010*a079); + a085=(a078*a085); + a079=(a079+a085); + a092=(a092+a079); + if (res[1]!=0) res[1][8]=a092; + a092=arg[1]? arg[1][11] : 0; + a079=(a002*a092); + a085=(a005*a079); + a080=arg[1]? arg[1][13] : 0; + a089=(a004*a080); + a085=(a085+a089); + a089=arg[1]? arg[1][10] : 0; + a088=(a007*a089); + a086=(a010*a088); + a095=arg[1]? arg[1][14] : 0; + a084=(a009*a095); + a086=(a086+a084); + a085=(a085+a086); + a086=arg[1]? arg[1][9] : 0; + a084=(a006*a086); + a091=(a087*a086); + a091=(a011*a091); + a083=(a013*a086); + a091=(a091+a083); + a084=(a084+a091); + a085=(a085+a084); + a084=(a000*a085); + a091=(a007*a092); + a083=(a016*a091); + a098=arg[1]? arg[1][12] : 0; + a099=(a015*a098); + a083=(a083+a099); + a099=(a002*a086); + a100=(a010*a099); + a101=(a018*a095); + a100=(a100+a101); + a083=(a083+a100); + a100=(a019*a089); + a101=(a093*a089); + a101=(a008*a101); + a102=(a021*a089); + a101=(a101+a102); + a100=(a100+a101); + a083=(a083+a100); + a100=(a014*a083); + a084=(a084+a100); + a100=(a002*a089); + a101=(a016*a100); + a102=(a022*a098); + a101=(a101+a102); + a102=(a007*a086); + a103=(a005*a102); + a104=(a024*a080); + a103=(a103+a104); + a101=(a101+a103); + a103=(a019*a092); + a104=(a097*a092); + a104=(a003*a104); + a105=(a026*a092); + a104=(a104+a105); + a103=(a103+a104); + a101=(a101+a103); + a103=(a020*a101); + a084=(a084+a103); + a079=(a008*a079); + a103=(a004*a089); + a079=(a079+a103); + a088=(a003*a088); + a103=(a009*a092); + a088=(a088+a103); + a079=(a079+a088); + a088=(a028*a095); + a088=(a005*a088); + a103=(a029*a080); + a088=(a088+a103); + a079=(a079+a088); + a088=(a030*a080); + a088=(a010*a088); + a103=(a031*a095); + a088=(a088+a103); + a079=(a079+a088); + a088=(a019*a098); + a103=(a096*a098); + a103=(a016*a103); + a104=(a033*a098); + a103=(a103+a104); + a088=(a088+a103); + a079=(a079+a088); + a088=(a025*a079); + a084=(a084+a088); + a091=(a011*a091); + a088=(a015*a086); + a091=(a091+a088); + a099=(a003*a099); + a092=(a018*a092); + a099=(a099+a092); + a091=(a091+a099); + a099=(a036*a095); + a099=(a016*a099); + a092=(a037*a098); + a099=(a099+a092); + a091=(a091+a099); + a099=(a038*a098); + a099=(a010*a099); + a092=(a039*a095); + a099=(a099+a092); + a091=(a091+a099); + a099=(a019*a080); + a092=(a077*a080); + a092=(a005*a092); + a088=(a040*a080); + a092=(a092+a088); + a099=(a099+a092); + a091=(a091+a099); + a099=(a032*a091); + a084=(a084+a099); + a100=(a011*a100); + a086=(a022*a086); + a100=(a100+a086); + a102=(a008*a102); + a089=(a024*a089); + a102=(a102+a089); + a100=(a100+a102); + a102=(a043*a080); + a102=(a016*a102); + a089=(a044*a098); + a102=(a102+a089); + a100=(a100+a102); + a102=(a045*a098); + a102=(a005*a102); + a089=(a046*a080); + a102=(a102+a089); + a100=(a100+a102); + a102=(a019*a095); + a089=(a082*a095); + a089=(a010*a089); + a086=(a047*a095); + a089=(a089+a086); + a102=(a102+a089); + a100=(a100+a102); + a102=(a035*a100); + a084=(a084+a102); + a084=(-a084); + if (res[1]!=0) res[1][9]=a084; + a084=(a012*a085); + a102=(a048*a083); + a084=(a084+a102); + a102=(a049*a101); + a084=(a084+a102); + a102=(a050*a079); + a084=(a084+a102); + a102=(a051*a091); + a084=(a084+a102); + a102=(a052*a100); + a084=(a084+a102); + a084=(-a084); + if (res[1]!=0) res[1][10]=a084; + a084=(a042*a085); + a102=(a054*a083); + a084=(a084+a102); + a102=(a055*a101); + a084=(a084+a102); + a102=(a056*a079); + a084=(a084+a102); + a102=(a057*a091); + a084=(a084+a102); + a102=(a058*a100); + a084=(a084+a102); + a084=(-a084); + if (res[1]!=0) res[1][11]=a084; + a084=(a053*a085); + a102=(a060*a083); + a084=(a084+a102); + a102=(a061*a101); + a084=(a084+a102); + a102=(a062*a079); + a084=(a084+a102); + a102=(a063*a091); + a084=(a084+a102); + a102=(a064*a100); + a084=(a084+a102); + a084=(-a084); + if (res[1]!=0) res[1][12]=a084; + a084=(a059*a085); + a102=(a066*a083); + a084=(a084+a102); + a102=(a067*a101); + a084=(a084+a102); + a102=(a068*a079); + a084=(a084+a102); + a102=(a069*a091); + a084=(a084+a102); + a102=(a070*a100); + a084=(a084+a102); + a084=(-a084); + if (res[1]!=0) res[1][13]=a084; + a085=(a065*a085); + a083=(a071*a083); + a085=(a085+a083); + a101=(a017*a101); + a085=(a085+a101); + a079=(a023*a079); + a085=(a085+a079); + a091=(a070*a091); + a085=(a085+a091); + a100=(a034*a100); + a085=(a085+a100); + a085=(-a085); + if (res[1]!=0) res[1][14]=a085; + a085=arg[1]? arg[1][15] : 0; + a100=(a001*a085); + a091=(a072*a100); + a079=arg[1]? arg[1][16] : 0; + a101=(a079/a094); + a083=(a041*a101); + a091=(a091+a083); + a091=(a005*a091); + a083=(a073*a080); + a091=(a091+a083); + a098=(a098+a091); + a101=(a001*a101); + a085=(a041*a085); + a091=(a072*a085); + a101=(a101-a091); + a101=(a010*a101); + a091=(a075*a095); + a101=(a101+a091); + a098=(a098+a101); + if (res[1]!=0) res[1][15]=a098; + a098=(a001*a080); + a101=(a005*a085); + a098=(a098-a101); + a101=(a010*a100); + a091=(a041*a095); + a101=(a101+a091); + a098=(a098-a101); + if (res[1]!=0) res[1][16]=a098; + a100=(a100/a074); + a079=(a027*a079); + a098=(a090*a079); + a100=(a100+a098); + a100=(a005*a100); + a080=(a076*a080); + a100=(a100+a080); + a079=(a081*a079); + a085=(a085/a074); + a079=(a079-a085); + a079=(a010*a079); + a095=(a078*a095); + a079=(a079+a095); + a100=(a100+a079); + if (res[1]!=0) res[1][17]=a100; + a100=arg[1]? arg[1][20] : 0; + a079=(a002*a100); + a095=(a005*a079); + a085=arg[1]? arg[1][22] : 0; + a080=(a004*a085); + a095=(a095+a080); + a080=arg[1]? arg[1][19] : 0; + a098=(a007*a080); + a101=(a010*a098); + a091=arg[1]? arg[1][23] : 0; + a083=(a009*a091); + a101=(a101+a083); + a095=(a095+a101); + a101=arg[1]? arg[1][18] : 0; + a083=(a006*a101); + a084=(a087*a101); + a084=(a011*a084); + a102=(a013*a101); + a084=(a084+a102); + a083=(a083+a084); + a095=(a095+a083); + a083=(a000*a095); + a084=(a007*a100); + a102=(a016*a084); + a089=arg[1]? arg[1][21] : 0; + a086=(a015*a089); + a102=(a102+a086); + a086=(a002*a101); + a099=(a010*a086); + a092=(a018*a091); + a099=(a099+a092); + a102=(a102+a099); + a099=(a019*a080); + a092=(a093*a080); + a092=(a008*a092); + a088=(a021*a080); + a092=(a092+a088); + a099=(a099+a092); + a102=(a102+a099); + a099=(a014*a102); + a083=(a083+a099); + a099=(a002*a080); + a092=(a016*a099); + a088=(a022*a089); + a092=(a092+a088); + a088=(a007*a101); + a103=(a005*a088); + a104=(a024*a085); + a103=(a103+a104); + a092=(a092+a103); + a103=(a019*a100); + a104=(a097*a100); + a104=(a003*a104); + a105=(a026*a100); + a104=(a104+a105); + a103=(a103+a104); + a092=(a092+a103); + a103=(a020*a092); + a083=(a083+a103); + a079=(a008*a079); + a103=(a004*a080); + a079=(a079+a103); + a098=(a003*a098); + a103=(a009*a100); + a098=(a098+a103); + a079=(a079+a098); + a098=(a028*a091); + a098=(a005*a098); + a103=(a029*a085); + a098=(a098+a103); + a079=(a079+a098); + a098=(a030*a085); + a098=(a010*a098); + a103=(a031*a091); + a098=(a098+a103); + a079=(a079+a098); + a098=(a019*a089); + a103=(a096*a089); + a103=(a016*a103); + a104=(a033*a089); + a103=(a103+a104); + a098=(a098+a103); + a079=(a079+a098); + a098=(a025*a079); + a083=(a083+a098); + a084=(a011*a084); + a098=(a015*a101); + a084=(a084+a098); + a086=(a003*a086); + a100=(a018*a100); + a086=(a086+a100); + a084=(a084+a086); + a086=(a036*a091); + a086=(a016*a086); + a100=(a037*a089); + a086=(a086+a100); + a084=(a084+a086); + a086=(a038*a089); + a086=(a010*a086); + a100=(a039*a091); + a086=(a086+a100); + a084=(a084+a086); + a086=(a019*a085); + a100=(a077*a085); + a100=(a005*a100); + a098=(a040*a085); + a100=(a100+a098); + a086=(a086+a100); + a084=(a084+a086); + a086=(a032*a084); + a083=(a083+a086); + a099=(a011*a099); + a101=(a022*a101); + a099=(a099+a101); + a088=(a008*a088); + a080=(a024*a080); + a088=(a088+a080); + a099=(a099+a088); + a088=(a043*a085); + a088=(a016*a088); + a080=(a044*a089); + a088=(a088+a080); + a099=(a099+a088); + a088=(a045*a089); + a088=(a005*a088); + a080=(a046*a085); + a088=(a088+a080); + a099=(a099+a088); + a088=(a019*a091); + a080=(a082*a091); + a080=(a010*a080); + a101=(a047*a091); + a080=(a080+a101); + a088=(a088+a080); + a099=(a099+a088); + a088=(a035*a099); + a083=(a083+a088); + a083=(-a083); + if (res[1]!=0) res[1][18]=a083; + a083=(a012*a095); + a088=(a048*a102); + a083=(a083+a088); + a088=(a049*a092); + a083=(a083+a088); + a088=(a050*a079); + a083=(a083+a088); + a088=(a051*a084); + a083=(a083+a088); + a088=(a052*a099); + a083=(a083+a088); + a083=(-a083); + if (res[1]!=0) res[1][19]=a083; + a083=(a042*a095); + a088=(a054*a102); + a083=(a083+a088); + a088=(a055*a092); + a083=(a083+a088); + a088=(a056*a079); + a083=(a083+a088); + a088=(a057*a084); + a083=(a083+a088); + a088=(a058*a099); + a083=(a083+a088); + a083=(-a083); + if (res[1]!=0) res[1][20]=a083; + a083=(a053*a095); + a088=(a060*a102); + a083=(a083+a088); + a088=(a061*a092); + a083=(a083+a088); + a088=(a062*a079); + a083=(a083+a088); + a088=(a063*a084); + a083=(a083+a088); + a088=(a064*a099); + a083=(a083+a088); + a083=(-a083); + if (res[1]!=0) res[1][21]=a083; + a083=(a059*a095); + a088=(a066*a102); + a083=(a083+a088); + a088=(a067*a092); + a083=(a083+a088); + a088=(a068*a079); + a083=(a083+a088); + a088=(a069*a084); + a083=(a083+a088); + a088=(a070*a099); + a083=(a083+a088); + a083=(-a083); + if (res[1]!=0) res[1][22]=a083; + a095=(a065*a095); + a102=(a071*a102); + a095=(a095+a102); + a092=(a017*a092); + a095=(a095+a092); + a079=(a023*a079); + a095=(a095+a079); + a084=(a070*a084); + a095=(a095+a084); + a099=(a034*a099); + a095=(a095+a099); + a095=(-a095); + if (res[1]!=0) res[1][23]=a095; + a095=arg[1]? arg[1][24] : 0; + a099=(a001*a095); + a084=(a072*a099); + a079=arg[1]? arg[1][25] : 0; + a092=(a079/a094); + a102=(a041*a092); + a084=(a084+a102); + a084=(a005*a084); + a102=(a073*a085); + a084=(a084+a102); + a089=(a089+a084); + a092=(a001*a092); + a095=(a041*a095); + a084=(a072*a095); + a092=(a092-a084); + a092=(a010*a092); + a084=(a075*a091); + a092=(a092+a084); + a089=(a089+a092); + if (res[1]!=0) res[1][24]=a089; + a089=(a001*a085); + a092=(a005*a095); + a089=(a089-a092); + a092=(a010*a099); + a084=(a041*a091); + a092=(a092+a084); + a089=(a089-a092); + if (res[1]!=0) res[1][25]=a089; + a099=(a099/a074); + a079=(a027*a079); + a089=(a090*a079); + a099=(a099+a089); + a099=(a005*a099); + a085=(a076*a085); + a099=(a099+a085); + a079=(a081*a079); + a095=(a095/a074); + a079=(a079-a095); + a079=(a010*a079); + a091=(a078*a091); + a079=(a079+a091); + a099=(a099+a079); + if (res[1]!=0) res[1][26]=a099; + a099=arg[1]? arg[1][29] : 0; + a079=(a002*a099); + a091=(a005*a079); + a095=arg[1]? arg[1][31] : 0; + a085=(a004*a095); + a091=(a091+a085); + a085=arg[1]? arg[1][28] : 0; + a089=(a007*a085); + a092=(a010*a089); + a084=arg[1]? arg[1][32] : 0; + a102=(a009*a084); + a092=(a092+a102); + a091=(a091+a092); + a092=arg[1]? arg[1][27] : 0; + a102=(a006*a092); + a083=(a087*a092); + a083=(a011*a083); + a088=(a013*a092); + a083=(a083+a088); + a102=(a102+a083); + a091=(a091+a102); + a102=(a000*a091); + a083=(a007*a099); + a088=(a016*a083); + a080=arg[1]? arg[1][30] : 0; + a101=(a015*a080); + a088=(a088+a101); + a101=(a002*a092); + a086=(a010*a101); + a100=(a018*a084); + a086=(a086+a100); + a088=(a088+a086); + a086=(a019*a085); + a100=(a093*a085); + a100=(a008*a100); + a098=(a021*a085); + a100=(a100+a098); + a086=(a086+a100); + a088=(a088+a086); + a086=(a014*a088); + a102=(a102+a086); + a086=(a002*a085); + a100=(a016*a086); + a098=(a022*a080); + a100=(a100+a098); + a098=(a007*a092); + a103=(a005*a098); + a104=(a024*a095); + a103=(a103+a104); + a100=(a100+a103); + a103=(a019*a099); + a104=(a097*a099); + a104=(a003*a104); + a105=(a026*a099); + a104=(a104+a105); + a103=(a103+a104); + a100=(a100+a103); + a103=(a020*a100); + a102=(a102+a103); + a079=(a008*a079); + a103=(a004*a085); + a079=(a079+a103); + a089=(a003*a089); + a103=(a009*a099); + a089=(a089+a103); + a079=(a079+a089); + a089=(a028*a084); + a089=(a005*a089); + a103=(a029*a095); + a089=(a089+a103); + a079=(a079+a089); + a089=(a030*a095); + a089=(a010*a089); + a103=(a031*a084); + a089=(a089+a103); + a079=(a079+a089); + a089=(a019*a080); + a103=(a096*a080); + a103=(a016*a103); + a104=(a033*a080); + a103=(a103+a104); + a089=(a089+a103); + a079=(a079+a089); + a089=(a025*a079); + a102=(a102+a089); + a083=(a011*a083); + a089=(a015*a092); + a083=(a083+a089); + a101=(a003*a101); + a099=(a018*a099); + a101=(a101+a099); + a083=(a083+a101); + a101=(a036*a084); + a101=(a016*a101); + a099=(a037*a080); + a101=(a101+a099); + a083=(a083+a101); + a101=(a038*a080); + a101=(a010*a101); + a099=(a039*a084); + a101=(a101+a099); + a083=(a083+a101); + a101=(a019*a095); + a099=(a077*a095); + a099=(a005*a099); + a089=(a040*a095); + a099=(a099+a089); + a101=(a101+a099); + a083=(a083+a101); + a101=(a032*a083); + a102=(a102+a101); + a086=(a011*a086); + a092=(a022*a092); + a086=(a086+a092); + a098=(a008*a098); + a085=(a024*a085); + a098=(a098+a085); + a086=(a086+a098); + a098=(a043*a095); + a098=(a016*a098); + a085=(a044*a080); + a098=(a098+a085); + a086=(a086+a098); + a098=(a045*a080); + a098=(a005*a098); + a085=(a046*a095); + a098=(a098+a085); + a086=(a086+a098); + a098=(a019*a084); + a085=(a082*a084); + a085=(a010*a085); + a092=(a047*a084); + a085=(a085+a092); + a098=(a098+a085); + a086=(a086+a098); + a098=(a035*a086); + a102=(a102+a098); + a102=(-a102); + if (res[1]!=0) res[1][27]=a102; + a102=(a012*a091); + a098=(a048*a088); + a102=(a102+a098); + a098=(a049*a100); + a102=(a102+a098); + a098=(a050*a079); + a102=(a102+a098); + a098=(a051*a083); + a102=(a102+a098); + a098=(a052*a086); + a102=(a102+a098); + a102=(-a102); + if (res[1]!=0) res[1][28]=a102; + a102=(a042*a091); + a098=(a054*a088); + a102=(a102+a098); + a098=(a055*a100); + a102=(a102+a098); + a098=(a056*a079); + a102=(a102+a098); + a098=(a057*a083); + a102=(a102+a098); + a098=(a058*a086); + a102=(a102+a098); + a102=(-a102); + if (res[1]!=0) res[1][29]=a102; + a102=(a053*a091); + a098=(a060*a088); + a102=(a102+a098); + a098=(a061*a100); + a102=(a102+a098); + a098=(a062*a079); + a102=(a102+a098); + a098=(a063*a083); + a102=(a102+a098); + a098=(a064*a086); + a102=(a102+a098); + a102=(-a102); + if (res[1]!=0) res[1][30]=a102; + a102=(a059*a091); + a098=(a066*a088); + a102=(a102+a098); + a098=(a067*a100); + a102=(a102+a098); + a098=(a068*a079); + a102=(a102+a098); + a098=(a069*a083); + a102=(a102+a098); + a098=(a070*a086); + a102=(a102+a098); + a102=(-a102); + if (res[1]!=0) res[1][31]=a102; + a091=(a065*a091); + a088=(a071*a088); + a091=(a091+a088); + a100=(a017*a100); + a091=(a091+a100); + a079=(a023*a079); + a091=(a091+a079); + a083=(a070*a083); + a091=(a091+a083); + a086=(a034*a086); + a091=(a091+a086); + a091=(-a091); + if (res[1]!=0) res[1][32]=a091; + a091=arg[1]? arg[1][33] : 0; + a086=(a001*a091); + a083=(a072*a086); + a079=arg[1]? arg[1][34] : 0; + a100=(a079/a094); + a088=(a041*a100); + a083=(a083+a088); + a083=(a005*a083); + a088=(a073*a095); + a083=(a083+a088); + a080=(a080+a083); + a100=(a001*a100); + a091=(a041*a091); + a083=(a072*a091); + a100=(a100-a083); + a100=(a010*a100); + a083=(a075*a084); + a100=(a100+a083); + a080=(a080+a100); + if (res[1]!=0) res[1][33]=a080; + a080=(a001*a095); + a100=(a005*a091); + a080=(a080-a100); + a100=(a010*a086); + a083=(a041*a084); + a100=(a100+a083); + a080=(a080-a100); + if (res[1]!=0) res[1][34]=a080; + a086=(a086/a074); + a079=(a027*a079); + a080=(a090*a079); + a086=(a086+a080); + a086=(a005*a086); + a095=(a076*a095); + a086=(a086+a095); + a079=(a081*a079); + a091=(a091/a074); + a079=(a079-a091); + a079=(a010*a079); + a084=(a078*a084); + a079=(a079+a084); + a086=(a086+a079); + if (res[1]!=0) res[1][35]=a086; + a086=arg[1]? arg[1][38] : 0; + a079=(a002*a086); + a084=(a005*a079); + a091=arg[1]? arg[1][40] : 0; + a095=(a004*a091); + a084=(a084+a095); + a095=arg[1]? arg[1][37] : 0; + a080=(a007*a095); + a100=(a010*a080); + a083=arg[1]? arg[1][41] : 0; + a088=(a009*a083); + a100=(a100+a088); + a084=(a084+a100); + a100=arg[1]? arg[1][36] : 0; + a088=(a006*a100); + a102=(a087*a100); + a102=(a011*a102); + a098=(a013*a100); + a102=(a102+a098); + a088=(a088+a102); + a084=(a084+a088); + a088=(a000*a084); + a102=(a007*a086); + a098=(a016*a102); + a085=arg[1]? arg[1][39] : 0; + a092=(a015*a085); + a098=(a098+a092); + a092=(a002*a100); + a101=(a010*a092); + a099=(a018*a083); + a101=(a101+a099); + a098=(a098+a101); + a101=(a019*a095); + a099=(a093*a095); + a099=(a008*a099); + a089=(a021*a095); + a099=(a099+a089); + a101=(a101+a099); + a098=(a098+a101); + a101=(a014*a098); + a088=(a088+a101); + a101=(a002*a095); + a099=(a016*a101); + a089=(a022*a085); + a099=(a099+a089); + a089=(a007*a100); + a103=(a005*a089); + a104=(a024*a091); + a103=(a103+a104); + a099=(a099+a103); + a103=(a019*a086); + a104=(a097*a086); + a104=(a003*a104); + a105=(a026*a086); + a104=(a104+a105); + a103=(a103+a104); + a099=(a099+a103); + a103=(a020*a099); + a088=(a088+a103); + a079=(a008*a079); + a103=(a004*a095); + a079=(a079+a103); + a080=(a003*a080); + a103=(a009*a086); + a080=(a080+a103); + a079=(a079+a080); + a080=(a028*a083); + a080=(a005*a080); + a103=(a029*a091); + a080=(a080+a103); + a079=(a079+a080); + a080=(a030*a091); + a080=(a010*a080); + a103=(a031*a083); + a080=(a080+a103); + a079=(a079+a080); + a080=(a019*a085); + a103=(a096*a085); + a103=(a016*a103); + a104=(a033*a085); + a103=(a103+a104); + a080=(a080+a103); + a079=(a079+a080); + a080=(a025*a079); + a088=(a088+a080); + a102=(a011*a102); + a080=(a015*a100); + a102=(a102+a080); + a092=(a003*a092); + a086=(a018*a086); + a092=(a092+a086); + a102=(a102+a092); + a092=(a036*a083); + a092=(a016*a092); + a086=(a037*a085); + a092=(a092+a086); + a102=(a102+a092); + a092=(a038*a085); + a092=(a010*a092); + a086=(a039*a083); + a092=(a092+a086); + a102=(a102+a092); + a092=(a019*a091); + a086=(a077*a091); + a086=(a005*a086); + a080=(a040*a091); + a086=(a086+a080); + a092=(a092+a086); + a102=(a102+a092); + a092=(a032*a102); + a088=(a088+a092); + a101=(a011*a101); + a100=(a022*a100); + a101=(a101+a100); + a089=(a008*a089); + a095=(a024*a095); + a089=(a089+a095); + a101=(a101+a089); + a089=(a043*a091); + a089=(a016*a089); + a095=(a044*a085); + a089=(a089+a095); + a101=(a101+a089); + a089=(a045*a085); + a089=(a005*a089); + a095=(a046*a091); + a089=(a089+a095); + a101=(a101+a089); + a089=(a019*a083); + a095=(a082*a083); + a095=(a010*a095); + a100=(a047*a083); + a095=(a095+a100); + a089=(a089+a095); + a101=(a101+a089); + a089=(a035*a101); + a088=(a088+a089); + a088=(-a088); + if (res[1]!=0) res[1][36]=a088; + a088=(a012*a084); + a089=(a048*a098); + a088=(a088+a089); + a089=(a049*a099); + a088=(a088+a089); + a089=(a050*a079); + a088=(a088+a089); + a089=(a051*a102); + a088=(a088+a089); + a089=(a052*a101); + a088=(a088+a089); + a088=(-a088); + if (res[1]!=0) res[1][37]=a088; + a088=(a042*a084); + a089=(a054*a098); + a088=(a088+a089); + a089=(a055*a099); + a088=(a088+a089); + a089=(a056*a079); + a088=(a088+a089); + a089=(a057*a102); + a088=(a088+a089); + a089=(a058*a101); + a088=(a088+a089); + a088=(-a088); + if (res[1]!=0) res[1][38]=a088; + a088=(a053*a084); + a089=(a060*a098); + a088=(a088+a089); + a089=(a061*a099); + a088=(a088+a089); + a089=(a062*a079); + a088=(a088+a089); + a089=(a063*a102); + a088=(a088+a089); + a089=(a064*a101); + a088=(a088+a089); + a088=(-a088); + if (res[1]!=0) res[1][39]=a088; + a088=(a059*a084); + a089=(a066*a098); + a088=(a088+a089); + a089=(a067*a099); + a088=(a088+a089); + a089=(a068*a079); + a088=(a088+a089); + a089=(a069*a102); + a088=(a088+a089); + a089=(a070*a101); + a088=(a088+a089); + a088=(-a088); + if (res[1]!=0) res[1][40]=a088; + a084=(a065*a084); + a098=(a071*a098); + a084=(a084+a098); + a099=(a017*a099); + a084=(a084+a099); + a079=(a023*a079); + a084=(a084+a079); + a102=(a070*a102); + a084=(a084+a102); + a101=(a034*a101); + a084=(a084+a101); + a084=(-a084); + if (res[1]!=0) res[1][41]=a084; + a084=arg[1]? arg[1][42] : 0; + a101=(a001*a084); + a102=(a072*a101); + a079=arg[1]? arg[1][43] : 0; + a099=(a079/a094); + a098=(a041*a099); + a102=(a102+a098); + a102=(a005*a102); + a098=(a073*a091); + a102=(a102+a098); + a085=(a085+a102); + a099=(a001*a099); + a084=(a041*a084); + a102=(a072*a084); + a099=(a099-a102); + a099=(a010*a099); + a102=(a075*a083); + a099=(a099+a102); + a085=(a085+a099); + if (res[1]!=0) res[1][42]=a085; + a085=(a001*a091); + a099=(a005*a084); + a085=(a085-a099); + a099=(a010*a101); + a102=(a041*a083); + a099=(a099+a102); + a085=(a085-a099); + if (res[1]!=0) res[1][43]=a085; + a101=(a101/a074); + a079=(a027*a079); + a085=(a090*a079); + a101=(a101+a085); + a101=(a005*a101); + a091=(a076*a091); + a101=(a101+a091); + a079=(a081*a079); + a084=(a084/a074); + a079=(a079-a084); + a079=(a010*a079); + a083=(a078*a083); + a079=(a079+a083); + a101=(a101+a079); + if (res[1]!=0) res[1][44]=a101; + a101=arg[1]? arg[1][47] : 0; + a079=(a002*a101); + a083=(a005*a079); + a084=arg[1]? arg[1][49] : 0; + a091=(a004*a084); + a083=(a083+a091); + a091=arg[1]? arg[1][46] : 0; + a085=(a007*a091); + a099=(a010*a085); + a102=arg[1]? arg[1][50] : 0; + a098=(a009*a102); + a099=(a099+a098); + a083=(a083+a099); + a099=arg[1]? arg[1][45] : 0; + a098=(a006*a099); + a088=(a087*a099); + a088=(a011*a088); + a089=(a013*a099); + a088=(a088+a089); + a098=(a098+a088); + a083=(a083+a098); + a098=(a000*a083); + a088=(a007*a101); + a089=(a016*a088); + a095=arg[1]? arg[1][48] : 0; + a100=(a015*a095); + a089=(a089+a100); + a100=(a002*a099); + a092=(a010*a100); + a086=(a018*a102); + a092=(a092+a086); + a089=(a089+a092); + a092=(a019*a091); + a086=(a093*a091); + a086=(a008*a086); + a080=(a021*a091); + a086=(a086+a080); + a092=(a092+a086); + a089=(a089+a092); + a092=(a014*a089); + a098=(a098+a092); + a092=(a002*a091); + a086=(a016*a092); + a080=(a022*a095); + a086=(a086+a080); + a080=(a007*a099); + a103=(a005*a080); + a104=(a024*a084); + a103=(a103+a104); + a086=(a086+a103); + a103=(a019*a101); + a104=(a097*a101); + a104=(a003*a104); + a105=(a026*a101); + a104=(a104+a105); + a103=(a103+a104); + a086=(a086+a103); + a103=(a020*a086); + a098=(a098+a103); + a079=(a008*a079); + a103=(a004*a091); + a079=(a079+a103); + a085=(a003*a085); + a103=(a009*a101); + a085=(a085+a103); + a079=(a079+a085); + a085=(a028*a102); + a085=(a005*a085); + a103=(a029*a084); + a085=(a085+a103); + a079=(a079+a085); + a085=(a030*a084); + a085=(a010*a085); + a103=(a031*a102); + a085=(a085+a103); + a079=(a079+a085); + a085=(a019*a095); + a103=(a096*a095); + a103=(a016*a103); + a104=(a033*a095); + a103=(a103+a104); + a085=(a085+a103); + a079=(a079+a085); + a085=(a025*a079); + a098=(a098+a085); + a088=(a011*a088); + a085=(a015*a099); + a088=(a088+a085); + a100=(a003*a100); + a101=(a018*a101); + a100=(a100+a101); + a088=(a088+a100); + a100=(a036*a102); + a100=(a016*a100); + a101=(a037*a095); + a100=(a100+a101); + a088=(a088+a100); + a100=(a038*a095); + a100=(a010*a100); + a101=(a039*a102); + a100=(a100+a101); + a088=(a088+a100); + a100=(a019*a084); + a101=(a077*a084); + a101=(a005*a101); + a085=(a040*a084); + a101=(a101+a085); + a100=(a100+a101); + a088=(a088+a100); + a100=(a032*a088); + a098=(a098+a100); + a092=(a011*a092); + a099=(a022*a099); + a092=(a092+a099); + a080=(a008*a080); + a091=(a024*a091); + a080=(a080+a091); + a092=(a092+a080); + a080=(a043*a084); + a080=(a016*a080); + a091=(a044*a095); + a080=(a080+a091); + a092=(a092+a080); + a080=(a045*a095); + a080=(a005*a080); + a091=(a046*a084); + a080=(a080+a091); + a092=(a092+a080); + a080=(a019*a102); + a091=(a082*a102); + a091=(a010*a091); + a099=(a047*a102); + a091=(a091+a099); + a080=(a080+a091); + a092=(a092+a080); + a080=(a035*a092); + a098=(a098+a080); + a098=(-a098); + if (res[1]!=0) res[1][45]=a098; + a098=(a012*a083); + a080=(a048*a089); + a098=(a098+a080); + a080=(a049*a086); + a098=(a098+a080); + a080=(a050*a079); + a098=(a098+a080); + a080=(a051*a088); + a098=(a098+a080); + a080=(a052*a092); + a098=(a098+a080); + a098=(-a098); + if (res[1]!=0) res[1][46]=a098; + a098=(a042*a083); + a080=(a054*a089); + a098=(a098+a080); + a080=(a055*a086); + a098=(a098+a080); + a080=(a056*a079); + a098=(a098+a080); + a080=(a057*a088); + a098=(a098+a080); + a080=(a058*a092); + a098=(a098+a080); + a098=(-a098); + if (res[1]!=0) res[1][47]=a098; + a098=(a053*a083); + a080=(a060*a089); + a098=(a098+a080); + a080=(a061*a086); + a098=(a098+a080); + a080=(a062*a079); + a098=(a098+a080); + a080=(a063*a088); + a098=(a098+a080); + a080=(a064*a092); + a098=(a098+a080); + a098=(-a098); + if (res[1]!=0) res[1][48]=a098; + a098=(a059*a083); + a080=(a066*a089); + a098=(a098+a080); + a080=(a067*a086); + a098=(a098+a080); + a080=(a068*a079); + a098=(a098+a080); + a080=(a069*a088); + a098=(a098+a080); + a080=(a070*a092); + a098=(a098+a080); + a098=(-a098); + if (res[1]!=0) res[1][49]=a098; + a083=(a065*a083); + a089=(a071*a089); + a083=(a083+a089); + a086=(a017*a086); + a083=(a083+a086); + a079=(a023*a079); + a083=(a083+a079); + a088=(a070*a088); + a083=(a083+a088); + a092=(a034*a092); + a083=(a083+a092); + a083=(-a083); + if (res[1]!=0) res[1][50]=a083; + a083=arg[1]? arg[1][51] : 0; + a092=(a001*a083); + a088=(a072*a092); + a079=arg[1]? arg[1][52] : 0; + a086=(a079/a094); + a089=(a041*a086); + a088=(a088+a089); + a088=(a005*a088); + a089=(a073*a084); + a088=(a088+a089); + a095=(a095+a088); + a086=(a001*a086); + a083=(a041*a083); + a088=(a072*a083); + a086=(a086-a088); + a086=(a010*a086); + a088=(a075*a102); + a086=(a086+a088); + a095=(a095+a086); + if (res[1]!=0) res[1][51]=a095; + a095=(a001*a084); + a086=(a005*a083); + a095=(a095-a086); + a086=(a010*a092); + a088=(a041*a102); + a086=(a086+a088); + a095=(a095-a086); + if (res[1]!=0) res[1][52]=a095; + a092=(a092/a074); + a079=(a027*a079); + a095=(a090*a079); + a092=(a092+a095); + a092=(a005*a092); + a084=(a076*a084); + a092=(a092+a084); + a079=(a081*a079); + a083=(a083/a074); + a079=(a079-a083); + a079=(a010*a079); + a102=(a078*a102); + a079=(a079+a102); + a092=(a092+a079); + if (res[1]!=0) res[1][53]=a092; + a092=arg[1]? arg[1][56] : 0; + a079=(a002*a092); + a102=(a005*a079); + a083=arg[1]? arg[1][58] : 0; + a084=(a004*a083); + a102=(a102+a084); + a084=arg[1]? arg[1][55] : 0; + a095=(a007*a084); + a086=(a010*a095); + a088=arg[1]? arg[1][59] : 0; + a089=(a009*a088); + a086=(a086+a089); + a102=(a102+a086); + a086=arg[1]? arg[1][54] : 0; + a089=(a006*a086); + a098=(a087*a086); + a098=(a011*a098); + a080=(a013*a086); + a098=(a098+a080); + a089=(a089+a098); + a102=(a102+a089); + a089=(a000*a102); + a098=(a007*a092); + a080=(a016*a098); + a091=arg[1]? arg[1][57] : 0; + a099=(a015*a091); + a080=(a080+a099); + a099=(a002*a086); + a100=(a010*a099); + a101=(a018*a088); + a100=(a100+a101); + a080=(a080+a100); + a100=(a019*a084); + a101=(a093*a084); + a101=(a008*a101); + a085=(a021*a084); + a101=(a101+a085); + a100=(a100+a101); + a080=(a080+a100); + a100=(a014*a080); + a089=(a089+a100); + a100=(a002*a084); + a101=(a016*a100); + a085=(a022*a091); + a101=(a101+a085); + a085=(a007*a086); + a103=(a005*a085); + a104=(a024*a083); + a103=(a103+a104); + a101=(a101+a103); + a103=(a019*a092); + a104=(a097*a092); + a104=(a003*a104); + a105=(a026*a092); + a104=(a104+a105); + a103=(a103+a104); + a101=(a101+a103); + a103=(a020*a101); + a089=(a089+a103); + a079=(a008*a079); + a103=(a004*a084); + a079=(a079+a103); + a095=(a003*a095); + a103=(a009*a092); + a095=(a095+a103); + a079=(a079+a095); + a095=(a028*a088); + a095=(a005*a095); + a103=(a029*a083); + a095=(a095+a103); + a079=(a079+a095); + a095=(a030*a083); + a095=(a010*a095); + a103=(a031*a088); + a095=(a095+a103); + a079=(a079+a095); + a095=(a019*a091); + a103=(a096*a091); + a103=(a016*a103); + a104=(a033*a091); + a103=(a103+a104); + a095=(a095+a103); + a079=(a079+a095); + a095=(a025*a079); + a089=(a089+a095); + a098=(a011*a098); + a095=(a015*a086); + a098=(a098+a095); + a099=(a003*a099); + a092=(a018*a092); + a099=(a099+a092); + a098=(a098+a099); + a099=(a036*a088); + a099=(a016*a099); + a092=(a037*a091); + a099=(a099+a092); + a098=(a098+a099); + a099=(a038*a091); + a099=(a010*a099); + a092=(a039*a088); + a099=(a099+a092); + a098=(a098+a099); + a099=(a019*a083); + a092=(a077*a083); + a092=(a005*a092); + a095=(a040*a083); + a092=(a092+a095); + a099=(a099+a092); + a098=(a098+a099); + a099=(a032*a098); + a089=(a089+a099); + a100=(a011*a100); + a086=(a022*a086); + a100=(a100+a086); + a085=(a008*a085); + a084=(a024*a084); + a085=(a085+a084); + a100=(a100+a085); + a085=(a043*a083); + a085=(a016*a085); + a084=(a044*a091); + a085=(a085+a084); + a100=(a100+a085); + a085=(a045*a091); + a085=(a005*a085); + a084=(a046*a083); + a085=(a085+a084); + a100=(a100+a085); + a085=(a019*a088); + a084=(a082*a088); + a084=(a010*a084); + a086=(a047*a088); + a084=(a084+a086); + a085=(a085+a084); + a100=(a100+a085); + a085=(a035*a100); + a089=(a089+a085); + a089=(-a089); + if (res[1]!=0) res[1][54]=a089; + a089=(a012*a102); + a085=(a048*a080); + a089=(a089+a085); + a085=(a049*a101); + a089=(a089+a085); + a085=(a050*a079); + a089=(a089+a085); + a085=(a051*a098); + a089=(a089+a085); + a085=(a052*a100); + a089=(a089+a085); + a089=(-a089); + if (res[1]!=0) res[1][55]=a089; + a089=(a042*a102); + a085=(a054*a080); + a089=(a089+a085); + a085=(a055*a101); + a089=(a089+a085); + a085=(a056*a079); + a089=(a089+a085); + a085=(a057*a098); + a089=(a089+a085); + a085=(a058*a100); + a089=(a089+a085); + a089=(-a089); + if (res[1]!=0) res[1][56]=a089; + a089=(a053*a102); + a085=(a060*a080); + a089=(a089+a085); + a085=(a061*a101); + a089=(a089+a085); + a085=(a062*a079); + a089=(a089+a085); + a085=(a063*a098); + a089=(a089+a085); + a085=(a064*a100); + a089=(a089+a085); + a089=(-a089); + if (res[1]!=0) res[1][57]=a089; + a089=(a059*a102); + a085=(a066*a080); + a089=(a089+a085); + a085=(a067*a101); + a089=(a089+a085); + a085=(a068*a079); + a089=(a089+a085); + a085=(a069*a098); + a089=(a089+a085); + a085=(a070*a100); + a089=(a089+a085); + a089=(-a089); + if (res[1]!=0) res[1][58]=a089; + a102=(a065*a102); + a080=(a071*a080); + a102=(a102+a080); + a101=(a017*a101); + a102=(a102+a101); + a079=(a023*a079); + a102=(a102+a079); + a098=(a070*a098); + a102=(a102+a098); + a100=(a034*a100); + a102=(a102+a100); + a102=(-a102); + if (res[1]!=0) res[1][59]=a102; + a102=arg[1]? arg[1][60] : 0; + a100=(a001*a102); + a098=(a072*a100); + a079=arg[1]? arg[1][61] : 0; + a101=(a079/a094); + a080=(a041*a101); + a098=(a098+a080); + a098=(a005*a098); + a080=(a073*a083); + a098=(a098+a080); + a091=(a091+a098); + a101=(a001*a101); + a102=(a041*a102); + a098=(a072*a102); + a101=(a101-a098); + a101=(a010*a101); + a098=(a075*a088); + a101=(a101+a098); + a091=(a091+a101); + if (res[1]!=0) res[1][60]=a091; + a091=(a001*a083); + a101=(a005*a102); + a091=(a091-a101); + a101=(a010*a100); + a098=(a041*a088); + a101=(a101+a098); + a091=(a091-a101); + if (res[1]!=0) res[1][61]=a091; + a100=(a100/a074); + a079=(a027*a079); + a091=(a090*a079); + a100=(a100+a091); + a100=(a005*a100); + a083=(a076*a083); + a100=(a100+a083); + a079=(a081*a079); + a102=(a102/a074); + a079=(a079-a102); + a079=(a010*a079); + a088=(a078*a088); + a079=(a079+a088); + a100=(a100+a079); + if (res[1]!=0) res[1][62]=a100; + a100=arg[1]? arg[1][65] : 0; + a079=(a002*a100); + a088=(a005*a079); + a102=arg[1]? arg[1][67] : 0; + a083=(a004*a102); + a088=(a088+a083); + a083=arg[1]? arg[1][64] : 0; + a091=(a007*a083); + a101=(a010*a091); + a098=arg[1]? arg[1][68] : 0; + a080=(a009*a098); + a101=(a101+a080); + a088=(a088+a101); + a101=arg[1]? arg[1][63] : 0; + a080=(a006*a101); + a089=(a087*a101); + a089=(a011*a089); + a085=(a013*a101); + a089=(a089+a085); + a080=(a080+a089); + a088=(a088+a080); + a080=(a000*a088); + a089=(a007*a100); + a085=(a016*a089); + a084=arg[1]? arg[1][66] : 0; + a086=(a015*a084); + a085=(a085+a086); + a086=(a002*a101); + a099=(a010*a086); + a092=(a018*a098); + a099=(a099+a092); + a085=(a085+a099); + a099=(a019*a083); + a092=(a093*a083); + a092=(a008*a092); + a095=(a021*a083); + a092=(a092+a095); + a099=(a099+a092); + a085=(a085+a099); + a099=(a014*a085); + a080=(a080+a099); + a099=(a002*a083); + a092=(a016*a099); + a095=(a022*a084); + a092=(a092+a095); + a095=(a007*a101); + a103=(a005*a095); + a104=(a024*a102); + a103=(a103+a104); + a092=(a092+a103); + a103=(a019*a100); + a104=(a097*a100); + a104=(a003*a104); + a105=(a026*a100); + a104=(a104+a105); + a103=(a103+a104); + a092=(a092+a103); + a103=(a020*a092); + a080=(a080+a103); + a079=(a008*a079); + a103=(a004*a083); + a079=(a079+a103); + a091=(a003*a091); + a103=(a009*a100); + a091=(a091+a103); + a079=(a079+a091); + a091=(a028*a098); + a091=(a005*a091); + a103=(a029*a102); + a091=(a091+a103); + a079=(a079+a091); + a091=(a030*a102); + a091=(a010*a091); + a103=(a031*a098); + a091=(a091+a103); + a079=(a079+a091); + a091=(a019*a084); + a103=(a096*a084); + a103=(a016*a103); + a104=(a033*a084); + a103=(a103+a104); + a091=(a091+a103); + a079=(a079+a091); + a091=(a025*a079); + a080=(a080+a091); + a089=(a011*a089); + a091=(a015*a101); + a089=(a089+a091); + a086=(a003*a086); + a100=(a018*a100); + a086=(a086+a100); + a089=(a089+a086); + a086=(a036*a098); + a086=(a016*a086); + a100=(a037*a084); + a086=(a086+a100); + a089=(a089+a086); + a086=(a038*a084); + a086=(a010*a086); + a100=(a039*a098); + a086=(a086+a100); + a089=(a089+a086); + a086=(a019*a102); + a100=(a077*a102); + a100=(a005*a100); + a091=(a040*a102); + a100=(a100+a091); + a086=(a086+a100); + a089=(a089+a086); + a086=(a032*a089); + a080=(a080+a086); + a099=(a011*a099); + a101=(a022*a101); + a099=(a099+a101); + a095=(a008*a095); + a083=(a024*a083); + a095=(a095+a083); + a099=(a099+a095); + a095=(a043*a102); + a095=(a016*a095); + a083=(a044*a084); + a095=(a095+a083); + a099=(a099+a095); + a095=(a045*a084); + a095=(a005*a095); + a083=(a046*a102); + a095=(a095+a083); + a099=(a099+a095); + a095=(a019*a098); + a083=(a082*a098); + a083=(a010*a083); + a101=(a047*a098); + a083=(a083+a101); + a095=(a095+a083); + a099=(a099+a095); + a095=(a035*a099); + a080=(a080+a095); + a080=(-a080); + if (res[1]!=0) res[1][63]=a080; + a080=(a012*a088); + a095=(a048*a085); + a080=(a080+a095); + a095=(a049*a092); + a080=(a080+a095); + a095=(a050*a079); + a080=(a080+a095); + a095=(a051*a089); + a080=(a080+a095); + a095=(a052*a099); + a080=(a080+a095); + a080=(-a080); + if (res[1]!=0) res[1][64]=a080; + a080=(a042*a088); + a095=(a054*a085); + a080=(a080+a095); + a095=(a055*a092); + a080=(a080+a095); + a095=(a056*a079); + a080=(a080+a095); + a095=(a057*a089); + a080=(a080+a095); + a095=(a058*a099); + a080=(a080+a095); + a080=(-a080); + if (res[1]!=0) res[1][65]=a080; + a080=(a053*a088); + a095=(a060*a085); + a080=(a080+a095); + a095=(a061*a092); + a080=(a080+a095); + a095=(a062*a079); + a080=(a080+a095); + a095=(a063*a089); + a080=(a080+a095); + a095=(a064*a099); + a080=(a080+a095); + a080=(-a080); + if (res[1]!=0) res[1][66]=a080; + a080=(a059*a088); + a095=(a066*a085); + a080=(a080+a095); + a095=(a067*a092); + a080=(a080+a095); + a095=(a068*a079); + a080=(a080+a095); + a095=(a069*a089); + a080=(a080+a095); + a095=(a070*a099); + a080=(a080+a095); + a080=(-a080); + if (res[1]!=0) res[1][67]=a080; + a088=(a065*a088); + a085=(a071*a085); + a088=(a088+a085); + a092=(a017*a092); + a088=(a088+a092); + a079=(a023*a079); + a088=(a088+a079); + a089=(a070*a089); + a088=(a088+a089); + a099=(a034*a099); + a088=(a088+a099); + a088=(-a088); + if (res[1]!=0) res[1][68]=a088; + a088=arg[1]? arg[1][69] : 0; + a099=(a001*a088); + a089=(a072*a099); + a079=arg[1]? arg[1][70] : 0; + a092=(a079/a094); + a085=(a041*a092); + a089=(a089+a085); + a089=(a005*a089); + a085=(a073*a102); + a089=(a089+a085); + a084=(a084+a089); + a092=(a001*a092); + a088=(a041*a088); + a089=(a072*a088); + a092=(a092-a089); + a092=(a010*a092); + a089=(a075*a098); + a092=(a092+a089); + a084=(a084+a092); + if (res[1]!=0) res[1][69]=a084; + a084=(a001*a102); + a092=(a005*a088); + a084=(a084-a092); + a092=(a010*a099); + a089=(a041*a098); + a092=(a092+a089); + a084=(a084-a092); + if (res[1]!=0) res[1][70]=a084; + a099=(a099/a074); + a079=(a027*a079); + a084=(a090*a079); + a099=(a099+a084); + a099=(a005*a099); + a102=(a076*a102); + a099=(a099+a102); + a079=(a081*a079); + a088=(a088/a074); + a079=(a079-a088); + a079=(a010*a079); + a098=(a078*a098); + a079=(a079+a098); + a099=(a099+a079); + if (res[1]!=0) res[1][71]=a099; + a099=arg[1]? arg[1][74] : 0; + a079=(a002*a099); + a098=(a005*a079); + a088=arg[1]? arg[1][76] : 0; + a102=(a004*a088); + a098=(a098+a102); + a102=arg[1]? arg[1][73] : 0; + a084=(a007*a102); + a092=(a010*a084); + a089=arg[1]? arg[1][77] : 0; + a085=(a009*a089); + a092=(a092+a085); + a098=(a098+a092); + a092=arg[1]? arg[1][72] : 0; + a085=(a006*a092); + a080=(a087*a092); + a080=(a011*a080); + a095=(a013*a092); + a080=(a080+a095); + a085=(a085+a080); + a098=(a098+a085); + a085=(a000*a098); + a080=(a007*a099); + a095=(a016*a080); + a083=arg[1]? arg[1][75] : 0; + a101=(a015*a083); + a095=(a095+a101); + a101=(a002*a092); + a086=(a010*a101); + a100=(a018*a089); + a086=(a086+a100); + a095=(a095+a086); + a086=(a019*a102); + a100=(a093*a102); + a100=(a008*a100); + a091=(a021*a102); + a100=(a100+a091); + a086=(a086+a100); + a095=(a095+a086); + a086=(a014*a095); + a085=(a085+a086); + a086=(a002*a102); + a100=(a016*a086); + a091=(a022*a083); + a100=(a100+a091); + a091=(a007*a092); + a103=(a005*a091); + a104=(a024*a088); + a103=(a103+a104); + a100=(a100+a103); + a103=(a019*a099); + a104=(a097*a099); + a104=(a003*a104); + a105=(a026*a099); + a104=(a104+a105); + a103=(a103+a104); + a100=(a100+a103); + a103=(a020*a100); + a085=(a085+a103); + a079=(a008*a079); + a103=(a004*a102); + a079=(a079+a103); + a084=(a003*a084); + a103=(a009*a099); + a084=(a084+a103); + a079=(a079+a084); + a084=(a028*a089); + a084=(a005*a084); + a103=(a029*a088); + a084=(a084+a103); + a079=(a079+a084); + a084=(a030*a088); + a084=(a010*a084); + a103=(a031*a089); + a084=(a084+a103); + a079=(a079+a084); + a084=(a019*a083); + a103=(a096*a083); + a103=(a016*a103); + a104=(a033*a083); + a103=(a103+a104); + a084=(a084+a103); + a079=(a079+a084); + a084=(a025*a079); + a085=(a085+a084); + a080=(a011*a080); + a084=(a015*a092); + a080=(a080+a084); + a101=(a003*a101); + a099=(a018*a099); + a101=(a101+a099); + a080=(a080+a101); + a101=(a036*a089); + a101=(a016*a101); + a099=(a037*a083); + a101=(a101+a099); + a080=(a080+a101); + a101=(a038*a083); + a101=(a010*a101); + a099=(a039*a089); + a101=(a101+a099); + a080=(a080+a101); + a101=(a019*a088); + a099=(a077*a088); + a099=(a005*a099); + a084=(a040*a088); + a099=(a099+a084); + a101=(a101+a099); + a080=(a080+a101); + a101=(a032*a080); + a085=(a085+a101); + a086=(a011*a086); + a092=(a022*a092); + a086=(a086+a092); + a091=(a008*a091); + a102=(a024*a102); + a091=(a091+a102); + a086=(a086+a091); + a091=(a043*a088); + a091=(a016*a091); + a102=(a044*a083); + a091=(a091+a102); + a086=(a086+a091); + a091=(a045*a083); + a091=(a005*a091); + a102=(a046*a088); + a091=(a091+a102); + a086=(a086+a091); + a091=(a019*a089); + a102=(a082*a089); + a102=(a010*a102); + a092=(a047*a089); + a102=(a102+a092); + a091=(a091+a102); + a086=(a086+a091); + a091=(a035*a086); + a085=(a085+a091); + a085=(-a085); + if (res[1]!=0) res[1][72]=a085; + a085=(a012*a098); + a091=(a048*a095); + a085=(a085+a091); + a091=(a049*a100); + a085=(a085+a091); + a091=(a050*a079); + a085=(a085+a091); + a091=(a051*a080); + a085=(a085+a091); + a091=(a052*a086); + a085=(a085+a091); + a085=(-a085); + if (res[1]!=0) res[1][73]=a085; + a085=(a042*a098); + a091=(a054*a095); + a085=(a085+a091); + a091=(a055*a100); + a085=(a085+a091); + a091=(a056*a079); + a085=(a085+a091); + a091=(a057*a080); + a085=(a085+a091); + a091=(a058*a086); + a085=(a085+a091); + a085=(-a085); + if (res[1]!=0) res[1][74]=a085; + a085=(a053*a098); + a091=(a060*a095); + a085=(a085+a091); + a091=(a061*a100); + a085=(a085+a091); + a091=(a062*a079); + a085=(a085+a091); + a091=(a063*a080); + a085=(a085+a091); + a091=(a064*a086); + a085=(a085+a091); + a085=(-a085); + if (res[1]!=0) res[1][75]=a085; + a085=(a059*a098); + a091=(a066*a095); + a085=(a085+a091); + a091=(a067*a100); + a085=(a085+a091); + a091=(a068*a079); + a085=(a085+a091); + a091=(a069*a080); + a085=(a085+a091); + a091=(a070*a086); + a085=(a085+a091); + a085=(-a085); + if (res[1]!=0) res[1][76]=a085; + a098=(a065*a098); + a095=(a071*a095); + a098=(a098+a095); + a100=(a017*a100); + a098=(a098+a100); + a079=(a023*a079); + a098=(a098+a079); + a080=(a070*a080); + a098=(a098+a080); + a086=(a034*a086); + a098=(a098+a086); + a098=(-a098); + if (res[1]!=0) res[1][77]=a098; + a098=arg[1]? arg[1][78] : 0; + a086=(a001*a098); + a080=(a072*a086); + a079=arg[1]? arg[1][79] : 0; + a100=(a079/a094); + a095=(a041*a100); + a080=(a080+a095); + a080=(a005*a080); + a095=(a073*a088); + a080=(a080+a095); + a083=(a083+a080); + a100=(a001*a100); + a098=(a041*a098); + a080=(a072*a098); + a100=(a100-a080); + a100=(a010*a100); + a080=(a075*a089); + a100=(a100+a080); + a083=(a083+a100); + if (res[1]!=0) res[1][78]=a083; + a083=(a001*a088); + a100=(a005*a098); + a083=(a083-a100); + a100=(a010*a086); + a080=(a041*a089); + a100=(a100+a080); + a083=(a083-a100); + if (res[1]!=0) res[1][79]=a083; + a086=(a086/a074); + a079=(a027*a079); + a083=(a090*a079); + a086=(a086+a083); + a086=(a005*a086); + a088=(a076*a088); + a086=(a086+a088); + a079=(a081*a079); + a098=(a098/a074); + a079=(a079-a098); + a079=(a010*a079); + a089=(a078*a089); + a079=(a079+a089); + a086=(a086+a079); + if (res[1]!=0) res[1][80]=a086; + a086=arg[2]? arg[2][2] : 0; + a079=(a002*a086); + a089=(a005*a079); + a098=arg[2]? arg[2][4] : 0; + a088=(a004*a098); + a089=(a089+a088); + a088=arg[2]? arg[2][1] : 0; + a083=(a007*a088); + a100=(a010*a083); + a080=arg[2]? arg[2][5] : 0; + a095=(a009*a080); + a100=(a100+a095); + a089=(a089+a100); + a100=arg[2]? arg[2][0] : 0; + a095=(a006*a100); + a085=(a087*a100); + a085=(a011*a085); + a091=(a013*a100); + a085=(a085+a091); + a095=(a095+a085); + a089=(a089+a095); + a095=(a000*a089); + a085=(a007*a086); + a091=(a016*a085); + a102=arg[2]? arg[2][3] : 0; + a092=(a015*a102); + a091=(a091+a092); + a092=(a002*a100); + a101=(a010*a092); + a099=(a018*a080); + a101=(a101+a099); + a091=(a091+a101); + a101=(a019*a088); + a099=(a093*a088); + a099=(a008*a099); + a084=(a021*a088); + a099=(a099+a084); + a101=(a101+a099); + a091=(a091+a101); + a101=(a014*a091); + a095=(a095+a101); + a101=(a002*a088); + a099=(a016*a101); + a084=(a022*a102); + a099=(a099+a084); + a084=(a007*a100); + a103=(a005*a084); + a104=(a024*a098); + a103=(a103+a104); + a099=(a099+a103); + a103=(a019*a086); + a104=(a097*a086); + a104=(a003*a104); + a105=(a026*a086); + a104=(a104+a105); + a103=(a103+a104); + a099=(a099+a103); + a103=(a020*a099); + a095=(a095+a103); + a079=(a008*a079); + a103=(a004*a088); + a079=(a079+a103); + a083=(a003*a083); + a103=(a009*a086); + a083=(a083+a103); + a079=(a079+a083); + a083=(a028*a080); + a083=(a005*a083); + a103=(a029*a098); + a083=(a083+a103); + a079=(a079+a083); + a083=(a030*a098); + a083=(a010*a083); + a103=(a031*a080); + a083=(a083+a103); + a079=(a079+a083); + a083=(a019*a102); + a103=(a096*a102); + a103=(a016*a103); + a104=(a033*a102); + a103=(a103+a104); + a083=(a083+a103); + a079=(a079+a083); + a083=(a025*a079); + a095=(a095+a083); + a085=(a011*a085); + a083=(a015*a100); + a085=(a085+a083); + a092=(a003*a092); + a086=(a018*a086); + a092=(a092+a086); + a085=(a085+a092); + a092=(a036*a080); + a092=(a016*a092); + a086=(a037*a102); + a092=(a092+a086); + a085=(a085+a092); + a092=(a038*a102); + a092=(a010*a092); + a086=(a039*a080); + a092=(a092+a086); + a085=(a085+a092); + a092=(a019*a098); + a086=(a077*a098); + a086=(a005*a086); + a083=(a040*a098); + a086=(a086+a083); + a092=(a092+a086); + a085=(a085+a092); + a092=(a032*a085); + a095=(a095+a092); + a101=(a011*a101); + a100=(a022*a100); + a101=(a101+a100); + a084=(a008*a084); + a088=(a024*a088); + a084=(a084+a088); + a101=(a101+a084); + a084=(a043*a098); + a084=(a016*a084); + a088=(a044*a102); + a084=(a084+a088); + a101=(a101+a084); + a084=(a045*a102); + a084=(a005*a084); + a088=(a046*a098); + a084=(a084+a088); + a101=(a101+a084); + a084=(a019*a080); + a088=(a082*a080); + a088=(a010*a088); + a100=(a047*a080); + a088=(a088+a100); + a084=(a084+a088); + a101=(a101+a084); + a084=(a035*a101); + a095=(a095+a084); + a095=(a000-a095); + if (res[2]!=0) res[2][0]=a095; + a095=(a012*a089); + a084=(a048*a091); + a095=(a095+a084); + a084=(a049*a099); + a095=(a095+a084); + a084=(a050*a079); + a095=(a095+a084); + a084=(a051*a085); + a095=(a095+a084); + a084=(a052*a101); + a095=(a095+a084); + a095=(a012-a095); + if (res[2]!=0) res[2][1]=a095; + a095=(a042*a089); + a084=(a054*a091); + a095=(a095+a084); + a084=(a055*a099); + a095=(a095+a084); + a084=(a056*a079); + a095=(a095+a084); + a084=(a057*a085); + a095=(a095+a084); + a084=(a058*a101); + a095=(a095+a084); + a095=(a042-a095); + if (res[2]!=0) res[2][2]=a095; + a095=(a053*a089); + a084=(a060*a091); + a095=(a095+a084); + a084=(a061*a099); + a095=(a095+a084); + a084=(a062*a079); + a095=(a095+a084); + a084=(a063*a085); + a095=(a095+a084); + a084=(a064*a101); + a095=(a095+a084); + a095=(a053-a095); + if (res[2]!=0) res[2][3]=a095; + a095=(a059*a089); + a084=(a066*a091); + a095=(a095+a084); + a084=(a067*a099); + a095=(a095+a084); + a084=(a068*a079); + a095=(a095+a084); + a084=(a069*a085); + a095=(a095+a084); + a084=(a070*a101); + a095=(a095+a084); + a095=(a059-a095); + if (res[2]!=0) res[2][4]=a095; + a089=(a065*a089); + a091=(a071*a091); + a089=(a089+a091); + a099=(a017*a099); + a089=(a089+a099); + a079=(a023*a079); + a089=(a089+a079); + a085=(a070*a085); + a089=(a089+a085); + a101=(a034*a101); + a089=(a089+a101); + a089=(a065-a089); + if (res[2]!=0) res[2][5]=a089; + a089=arg[2]? arg[2][6] : 0; + a101=(a001*a089); + a085=(a072*a101); + a079=arg[2]? arg[2][7] : 0; + a099=(a079/a094); + a091=(a041*a099); + a085=(a085+a091); + a085=(a005*a085); + a091=(a073*a098); + a085=(a085+a091); + a102=(a102+a085); + a099=(a001*a099); + a089=(a041*a089); + a085=(a072*a089); + a099=(a099-a085); + a099=(a010*a099); + a085=(a075*a080); + a099=(a099+a085); + a102=(a102+a099); + if (res[2]!=0) res[2][6]=a102; + a102=(a001*a098); + a099=(a005*a089); + a102=(a102-a099); + a099=(a010*a101); + a085=(a041*a080); + a099=(a099+a085); + a102=(a102-a099); + if (res[2]!=0) res[2][7]=a102; + a101=(a101/a074); + a079=(a027*a079); + a102=(a090*a079); + a101=(a101+a102); + a101=(a005*a101); + a098=(a076*a098); + a101=(a101+a098); + a079=(a081*a079); + a089=(a089/a074); + a079=(a079-a089); + a079=(a010*a079); + a080=(a078*a080); + a079=(a079+a080); + a101=(a101+a079); + if (res[2]!=0) res[2][8]=a101; + a101=arg[2]? arg[2][11] : 0; + a079=(a002*a101); + a080=(a005*a079); + a089=arg[2]? arg[2][13] : 0; + a098=(a004*a089); + a080=(a080+a098); + a098=arg[2]? arg[2][10] : 0; + a102=(a007*a098); + a099=(a010*a102); + a085=arg[2]? arg[2][14] : 0; + a091=(a009*a085); + a099=(a099+a091); + a080=(a080+a099); + a099=arg[2]? arg[2][9] : 0; + a091=(a006*a099); + a095=(a087*a099); + a095=(a011*a095); + a084=(a013*a099); + a095=(a095+a084); + a091=(a091+a095); + a080=(a080+a091); + a091=(a000*a080); + a095=(a007*a101); + a084=(a016*a095); + a088=arg[2]? arg[2][12] : 0; + a100=(a015*a088); + a084=(a084+a100); + a100=(a002*a099); + a092=(a010*a100); + a086=(a018*a085); + a092=(a092+a086); + a084=(a084+a092); + a092=(a019*a098); + a086=(a093*a098); + a086=(a008*a086); + a083=(a021*a098); + a086=(a086+a083); + a092=(a092+a086); + a084=(a084+a092); + a092=(a014*a084); + a091=(a091+a092); + a092=(a002*a098); + a086=(a016*a092); + a083=(a022*a088); + a086=(a086+a083); + a083=(a007*a099); + a103=(a005*a083); + a104=(a024*a089); + a103=(a103+a104); + a086=(a086+a103); + a103=(a019*a101); + a104=(a097*a101); + a104=(a003*a104); + a105=(a026*a101); + a104=(a104+a105); + a103=(a103+a104); + a086=(a086+a103); + a103=(a020*a086); + a091=(a091+a103); + a079=(a008*a079); + a103=(a004*a098); + a079=(a079+a103); + a102=(a003*a102); + a103=(a009*a101); + a102=(a102+a103); + a079=(a079+a102); + a102=(a028*a085); + a102=(a005*a102); + a103=(a029*a089); + a102=(a102+a103); + a079=(a079+a102); + a102=(a030*a089); + a102=(a010*a102); + a103=(a031*a085); + a102=(a102+a103); + a079=(a079+a102); + a102=(a019*a088); + a103=(a096*a088); + a103=(a016*a103); + a104=(a033*a088); + a103=(a103+a104); + a102=(a102+a103); + a079=(a079+a102); + a102=(a025*a079); + a091=(a091+a102); + a095=(a011*a095); + a102=(a015*a099); + a095=(a095+a102); + a100=(a003*a100); + a101=(a018*a101); + a100=(a100+a101); + a095=(a095+a100); + a100=(a036*a085); + a100=(a016*a100); + a101=(a037*a088); + a100=(a100+a101); + a095=(a095+a100); + a100=(a038*a088); + a100=(a010*a100); + a101=(a039*a085); + a100=(a100+a101); + a095=(a095+a100); + a100=(a019*a089); + a101=(a077*a089); + a101=(a005*a101); + a102=(a040*a089); + a101=(a101+a102); + a100=(a100+a101); + a095=(a095+a100); + a100=(a032*a095); + a091=(a091+a100); + a092=(a011*a092); + a099=(a022*a099); + a092=(a092+a099); + a083=(a008*a083); + a098=(a024*a098); + a083=(a083+a098); + a092=(a092+a083); + a083=(a043*a089); + a083=(a016*a083); + a098=(a044*a088); + a083=(a083+a098); + a092=(a092+a083); + a083=(a045*a088); + a083=(a005*a083); + a098=(a046*a089); + a083=(a083+a098); + a092=(a092+a083); + a083=(a019*a085); + a098=(a082*a085); + a098=(a010*a098); + a099=(a047*a085); + a098=(a098+a099); + a083=(a083+a098); + a092=(a092+a083); + a083=(a035*a092); + a091=(a091+a083); + a091=(a032-a091); + if (res[2]!=0) res[2][9]=a091; + a091=(a012*a080); + a083=(a048*a084); + a091=(a091+a083); + a083=(a049*a086); + a091=(a091+a083); + a083=(a050*a079); + a091=(a091+a083); + a083=(a051*a095); + a091=(a091+a083); + a083=(a052*a092); + a091=(a091+a083); + a091=(a051-a091); + if (res[2]!=0) res[2][10]=a091; + a091=(a042*a080); + a083=(a054*a084); + a091=(a091+a083); + a083=(a055*a086); + a091=(a091+a083); + a083=(a056*a079); + a091=(a091+a083); + a083=(a057*a095); + a091=(a091+a083); + a083=(a058*a092); + a091=(a091+a083); + a091=(a057-a091); + if (res[2]!=0) res[2][11]=a091; + a091=(a053*a080); + a083=(a060*a084); + a091=(a091+a083); + a083=(a061*a086); + a091=(a091+a083); + a083=(a062*a079); + a091=(a091+a083); + a083=(a063*a095); + a091=(a091+a083); + a083=(a064*a092); + a091=(a091+a083); + a091=(a063-a091); + if (res[2]!=0) res[2][12]=a091; + a091=(a059*a080); + a083=(a066*a084); + a091=(a091+a083); + a083=(a067*a086); + a091=(a091+a083); + a083=(a068*a079); + a091=(a091+a083); + a083=(a069*a095); + a091=(a091+a083); + a083=(a070*a092); + a091=(a091+a083); + a091=(a069-a091); + if (res[2]!=0) res[2][13]=a091; + a080=(a065*a080); + a084=(a071*a084); + a080=(a080+a084); + a086=(a017*a086); + a080=(a080+a086); + a079=(a023*a079); + a080=(a080+a079); + a095=(a070*a095); + a080=(a080+a095); + a092=(a034*a092); + a080=(a080+a092); + a080=(a070-a080); + if (res[2]!=0) res[2][14]=a080; + a080=arg[2]? arg[2][15] : 0; + a092=(a001*a080); + a095=(a072*a092); + a079=arg[2]? arg[2][16] : 0; + a086=(a079/a094); + a084=(a041*a086); + a095=(a095+a084); + a095=(a005*a095); + a084=(a073*a089); + a095=(a095+a084); + a088=(a088+a095); + a086=(a001*a086); + a080=(a041*a080); + a095=(a072*a080); + a086=(a086-a095); + a086=(a010*a086); + a095=(a075*a085); + a086=(a086+a095); + a088=(a088+a086); + if (res[2]!=0) res[2][15]=a088; + a088=(a001*a089); + a086=(a005*a080); + a088=(a088-a086); + a086=(a010*a092); + a095=(a041*a085); + a086=(a086+a095); + a088=(a088-a086); + if (res[2]!=0) res[2][16]=a088; + a092=(a092/a074); + a079=(a027*a079); + a088=(a090*a079); + a092=(a092+a088); + a092=(a005*a092); + a089=(a076*a089); + a092=(a092+a089); + a079=(a081*a079); + a080=(a080/a074); + a079=(a079-a080); + a079=(a010*a079); + a085=(a078*a085); + a079=(a079+a085); + a092=(a092+a079); + if (res[2]!=0) res[2][17]=a092; + a092=arg[2]? arg[2][20] : 0; + a079=(a002*a092); + a085=(a005*a079); + a080=arg[2]? arg[2][22] : 0; + a089=(a004*a080); + a085=(a085+a089); + a089=arg[2]? arg[2][19] : 0; + a088=(a007*a089); + a086=(a010*a088); + a095=arg[2]? arg[2][23] : 0; + a084=(a009*a095); + a086=(a086+a084); + a085=(a085+a086); + a086=arg[2]? arg[2][18] : 0; + a006=(a006*a086); + a087=(a087*a086); + a087=(a011*a087); + a013=(a013*a086); + a087=(a087+a013); + a006=(a006+a087); + a085=(a085+a006); + a000=(a000*a085); + a006=(a007*a092); + a087=(a016*a006); + a013=arg[2]? arg[2][21] : 0; + a084=(a015*a013); + a087=(a087+a084); + a084=(a002*a086); + a091=(a010*a084); + a083=(a018*a095); + a091=(a091+a083); + a087=(a087+a091); + a091=(a019*a089); + a093=(a093*a089); + a093=(a008*a093); + a021=(a021*a089); + a093=(a093+a021); + a091=(a091+a093); + a087=(a087+a091); + a014=(a014*a087); + a000=(a000+a014); + a002=(a002*a089); + a014=(a016*a002); + a091=(a022*a013); + a014=(a014+a091); + a007=(a007*a086); + a091=(a005*a007); + a093=(a024*a080); + a091=(a091+a093); + a014=(a014+a091); + a091=(a019*a092); + a097=(a097*a092); + a097=(a003*a097); + a026=(a026*a092); + a097=(a097+a026); + a091=(a091+a097); + a014=(a014+a091); + a020=(a020*a014); + a000=(a000+a020); + a079=(a008*a079); + a004=(a004*a089); + a079=(a079+a004); + a088=(a003*a088); + a009=(a009*a092); + a088=(a088+a009); + a079=(a079+a088); + a028=(a028*a095); + a028=(a005*a028); + a029=(a029*a080); + a028=(a028+a029); + a079=(a079+a028); + a030=(a030*a080); + a030=(a010*a030); + a031=(a031*a095); + a030=(a030+a031); + a079=(a079+a030); + a030=(a019*a013); + a096=(a096*a013); + a096=(a016*a096); + a033=(a033*a013); + a096=(a096+a033); + a030=(a030+a096); + a079=(a079+a030); + a025=(a025*a079); + a000=(a000+a025); + a006=(a011*a006); + a015=(a015*a086); + a006=(a006+a015); + a003=(a003*a084); + a018=(a018*a092); + a003=(a003+a018); + a006=(a006+a003); + a036=(a036*a095); + a036=(a016*a036); + a037=(a037*a013); + a036=(a036+a037); + a006=(a006+a036); + a038=(a038*a013); + a038=(a010*a038); + a039=(a039*a095); + a038=(a038+a039); + a006=(a006+a038); + a038=(a019*a080); + a077=(a077*a080); + a077=(a005*a077); + a040=(a040*a080); + a077=(a077+a040); + a038=(a038+a077); + a006=(a006+a038); + a032=(a032*a006); + a000=(a000+a032); + a011=(a011*a002); + a022=(a022*a086); + a011=(a011+a022); + a008=(a008*a007); + a024=(a024*a089); + a008=(a008+a024); + a011=(a011+a008); + a043=(a043*a080); + a016=(a016*a043); + a044=(a044*a013); + a016=(a016+a044); + a011=(a011+a016); + a045=(a045*a013); + a045=(a005*a045); + a046=(a046*a080); + a045=(a045+a046); + a011=(a011+a045); + a019=(a019*a095); + a082=(a082*a095); + a082=(a010*a082); + a047=(a047*a095); + a082=(a082+a047); + a019=(a019+a082); + a011=(a011+a019); + a019=(a035*a011); + a000=(a000+a019); + a035=(a035-a000); + if (res[2]!=0) res[2][18]=a035; + a012=(a012*a085); + a048=(a048*a087); + a012=(a012+a048); + a049=(a049*a014); + a012=(a012+a049); + a050=(a050*a079); + a012=(a012+a050); + a051=(a051*a006); + a012=(a012+a051); + a051=(a052*a011); + a012=(a012+a051); + a052=(a052-a012); + if (res[2]!=0) res[2][19]=a052; + a042=(a042*a085); + a054=(a054*a087); + a042=(a042+a054); + a055=(a055*a014); + a042=(a042+a055); + a056=(a056*a079); + a042=(a042+a056); + a057=(a057*a006); + a042=(a042+a057); + a057=(a058*a011); + a042=(a042+a057); + a058=(a058-a042); + if (res[2]!=0) res[2][20]=a058; + a053=(a053*a085); + a060=(a060*a087); + a053=(a053+a060); + a061=(a061*a014); + a053=(a053+a061); + a062=(a062*a079); + a053=(a053+a062); + a063=(a063*a006); + a053=(a053+a063); + a063=(a064*a011); + a053=(a053+a063); + a064=(a064-a053); + if (res[2]!=0) res[2][21]=a064; + a059=(a059*a085); + a066=(a066*a087); + a059=(a059+a066); + a067=(a067*a014); + a059=(a059+a067); + a068=(a068*a079); + a059=(a059+a068); + a069=(a069*a006); + a059=(a059+a069); + a069=(a070*a011); + a059=(a059+a069); + a059=(a070-a059); + if (res[2]!=0) res[2][22]=a059; + a065=(a065*a085); + a071=(a071*a087); + a065=(a065+a071); + a017=(a017*a014); + a065=(a065+a017); + a023=(a023*a079); + a065=(a065+a023); + a070=(a070*a006); + a065=(a065+a070); + a011=(a034*a011); + a065=(a065+a011); + a034=(a034-a065); + if (res[2]!=0) res[2][23]=a034; + a034=arg[2]? arg[2][24] : 0; + a065=(a001*a034); + a011=(a072*a065); + a070=arg[2]? arg[2][25] : 0; + a094=(a070/a094); + a006=(a041*a094); + a011=(a011+a006); + a011=(a005*a011); + a073=(a073*a080); + a011=(a011+a073); + a013=(a013+a011); + a094=(a001*a094); + a034=(a041*a034); + a072=(a072*a034); + a094=(a094-a072); + a094=(a010*a094); + a075=(a075*a095); + a094=(a094+a075); + a013=(a013+a094); + if (res[2]!=0) res[2][24]=a013; + a001=(a001*a080); + a013=(a005*a034); + a001=(a001-a013); + a013=(a010*a065); + a041=(a041*a095); + a013=(a013+a041); + a001=(a001-a013); + if (res[2]!=0) res[2][25]=a001; + a065=(a065/a074); + a027=(a027*a070); + a090=(a090*a027); + a065=(a065+a090); + a005=(a005*a065); + a076=(a076*a080); + a005=(a005+a076); + a081=(a081*a027); + a034=(a034/a074); + a081=(a081-a034); + a010=(a010*a081); + a078=(a078*a095); + a010=(a010+a078); + a005=(a005+a010); + if (res[2]!=0) res[2][26]=a005; + return 0; +} + +CASADI_SYMBOL_EXPORT int auv_model_expl_vde_forw(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ + return casadi_f0(arg, res, iw, w, mem); +} + +CASADI_SYMBOL_EXPORT int auv_model_expl_vde_forw_alloc_mem(void) { + return 0; +} + +CASADI_SYMBOL_EXPORT int auv_model_expl_vde_forw_init_mem(int mem) { + return 0; +} + +CASADI_SYMBOL_EXPORT void auv_model_expl_vde_forw_free_mem(int mem) { +} + +CASADI_SYMBOL_EXPORT int auv_model_expl_vde_forw_checkout(void) { + return 0; +} + +CASADI_SYMBOL_EXPORT void auv_model_expl_vde_forw_release(int mem) { +} + +CASADI_SYMBOL_EXPORT void auv_model_expl_vde_forw_incref(void) { +} + +CASADI_SYMBOL_EXPORT void auv_model_expl_vde_forw_decref(void) { +} + +CASADI_SYMBOL_EXPORT casadi_int auv_model_expl_vde_forw_n_in(void) { return 5;} + +CASADI_SYMBOL_EXPORT casadi_int auv_model_expl_vde_forw_n_out(void) { return 3;} + +CASADI_SYMBOL_EXPORT casadi_real auv_model_expl_vde_forw_default_in(casadi_int i) { + switch (i) { + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const char* auv_model_expl_vde_forw_name_in(casadi_int i) { + switch (i) { + case 0: return "i0"; + case 1: return "i1"; + case 2: return "i2"; + case 3: return "i3"; + case 4: return "i4"; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const char* auv_model_expl_vde_forw_name_out(casadi_int i) { + switch (i) { + case 0: return "o0"; + case 1: return "o1"; + case 2: return "o2"; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const casadi_int* auv_model_expl_vde_forw_sparsity_in(casadi_int i) { + switch (i) { + case 0: return casadi_s0; + case 1: return casadi_s1; + case 2: return casadi_s2; + case 3: return casadi_s3; + case 4: return casadi_s4; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const casadi_int* auv_model_expl_vde_forw_sparsity_out(casadi_int i) { + switch (i) { + case 0: return casadi_s0; + case 1: return casadi_s1; + case 2: return casadi_s2; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT int auv_model_expl_vde_forw_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { + if (sz_arg) *sz_arg = 5; + if (sz_res) *sz_res = 3; + if (sz_iw) *sz_iw = 0; + if (sz_w) *sz_w = 0; + return 0; +} + +CASADI_SYMBOL_EXPORT int auv_model_expl_vde_forw_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { + if (sz_arg) *sz_arg = 5*sizeof(const casadi_real*); + if (sz_res) *sz_res = 3*sizeof(casadi_real*); + if (sz_iw) *sz_iw = 0*sizeof(casadi_int); + if (sz_w) *sz_w = 0*sizeof(casadi_real); + return 0; +} + + +#ifdef __cplusplus +} /* extern "C" */ +#endif diff --git a/control/velocity_controller/src/build_auv_solver/auv_model_model/auv_model_expl_vde_forw.o b/control/velocity_controller/src/build_auv_solver/auv_model_model/auv_model_expl_vde_forw.o new file mode 100644 index 0000000000000000000000000000000000000000..6559bfa668a750a75297b25f33717bd1f0432f33 GIT binary patch literal 49648 zcmeHw4S1Brx%Q%{@sHhDY*V$G;&NPU#RRb8X_X+M#uodf6=nAr`4JLB(o|)%;X5QI52M_uDab3EE{HIIkicsdxqoGjELz(Lmxhx5t5y}tk z4WE0UDm?TALMy`Ox%*v@R)u3tRpE;pZ#{QUc=_5cd%_Wkf~d=?!q=^>3SYkr(Mv); z05?g>pu9cdM9SU|ynf(ygNt_#9D4OV`>rdy=g_rfcfDS8Tqsm_?^Ok5Gg7rDLr7lL z)!{pyD7$xX2uQf@m(xNNeL6+ryE=k@HF@|~65A8b9~Uh_9<8{ljp9CWiAKwx6;E1j!RFZ`_sd4c{Sm zRpDW+_}d-sH6&Uj5=Y{HZ8VOA5H;7*s zokn8!NqT)0KPUTL751WC}k*s=3WytiZWJY&r=imL8Vb$ht1haQuh;{jL!%@23=Sv$jDW;4G~-i94X#Cmq##EM z*mnHAi+rAyLN&9t5WfM))6C+}jDlpZ@@6B@*ATA)G()6k5VnOJODC7sMaX(4#~;K* z=0%fE=(CUmc_`LYqGzl1WUD%cW6rf-+dz3eYG1E|lWhBhiNL|0M1&M?FEW z#34w=@K^Dc!QQd=L+%~Vcn2+pdkmv-n+7H9aDOPA-`8Y&k;|?PN!`$Dv#eMJ9N8v% zYP=3%D}f{X#3!5ocr9KB$Du-DKStsG68aPQ?yWDG+p730)gjf|?r z-q;f!z8`-rkg_z|hyYwHjK+{>*^K9E$0K7EPTI)$h*80nXbd_vSb`xw1}(;3;thK+ zI#psHSTj_I3yfgkYB)xiy=x=>(4i}~0^fl0u0)R4mW1;M6(L}jlpG^!URC&Gv!YX_ z9GKufJynz-6B}nM9Y$r2LviD(qM;nW6ARs}20f;vKImLik;md_jYLaNf6$Twt0c5h zZ#JP=;JnV9b!L}K6<>~ER8l6sS<$41x~T!GO$mUkm*4{R!PJ8xSY|;{>>>?c z9Gxi_s79eQlh$|_MkxqkL;2CZnp4ZdXoHU)bFGSKMII~3VEsK7$zUZO%gbP89xD*+ z>U(8c!c?LeL3%l2pb+7@H__ctVOLquzzzMgoIY6xgfCWpAzUYmd0IZ)&vOj|KI3j2 zSdpGuz?dxrr(r0>*Bn(h#zPFB5gB3dJwK{i!$?UE#&o*iC>TyW#sTj|GLK_tVjH9+ z4xx#fh=W=2Inokxp=}di0V(s;7GU;hq&-L)-vM}N6twI9W)()wY0;u^uf?Jcijtzw zazsnK1$iqm=ngx`k#`6GEo07WAnav?ZN>Lmmgh;WTpZYTS2QR}+8&{pW8=-pwp6Cf z=W#VZDh-I@U}(g20G%vV79;vrO2i1A7()X_SU5{<72uCDuH#{RHC4nx8Y4)2AHKIC z)eeHYS=Bbit&0jTP-@UW-iAo)7-1DF|2i^9w~fePitoCp>l`$lkeN)@710tT!mx$l zCZSUTwiuH?4cyJ48aterqhvvD7!?=?V|5I5k2BsJ*SQdjJsBbBWk4IG%K=Tv1*%20 ztL!;qf1z5AGtzk#-=IC{D#=)Cc=( zw1%}Ly%<=iIm)(&8eESIWF|(Eil?CN#3p1A$C!+S5zRC47GvU(N@Hpy=)$qOfU!^% zH*`}n^It(Z?4N44Kr*cx@E)?Hd2o@yG=|;}cz*|Y8SYHlaAN=AaaIQ7yeo029u@ES zGi-l2pC*7epE7m{@-TK4r$!hbD9hU{i}Y+I_ zO46wimJPIthdb#sAU+Z{hx11ka#E5h;J?O^l##L`{40%?54pFxKRAkM5c7l^o7Hr| zvzoLnh6EZJh~TCT&S7kLH``ROi-9-F1wXe?9QhB+BV0G>tCHtpu>lZy8muIkI~c5n zYb;lfK!SjraS)f*-O2o(zr|AQejMC%m-mfB;mmwSjRlEf@FHN5p@Ymvu zwa9DX{9CBVh0fd>zR>H+sECm_QvVV$CfQ+ZQKhn`(3w^XI%B^@4wz1cb8N%li6W|M zhd+)s@YY6R4?@wGDlm~^94-(ki$pf#aYkV$7uZ#KmMf%~L}_b?GX`>Dg8V@xLXnS; zm2QVxEfHl!;v70)3rvN$vTAS&&n4b$D=$Y*2Z_A`>>?x}y%tB4a)F5sDf!5XFK@Oc zUS&7HtV|FUVoY4=V1a(YDJdcbK15@Z1m8qI2x6=^DS~NglMXD4f*$vV{3_v28TvYj}R0}g$g&3|q5OsdE#bwOtZx}VE zwjatmBz%X=COy#*GTv@u9WaNXZp}^35kZ%*MXwNz=WmB7{q8|_%rFV5wStzz!07kuCR@29&S_ z!1fSp0v2Da?TutK-jL=H!V`^>kZD*x05Yk*OGP~g#kzSL?bik^ERIU=fKiH|rO^h~ zoQgwtIY1F}geQmy9}SW&(C{{fV@z_X+7zX80`X8V4A~G?#sWKGV$C=koF#n*te!x( z33$SM#_3bS;;5%~t#(TESF|8oZ47;jP+kqIPcI3IBSs!nLTpJ zH~Td3g5Y+m6ffk9hXU(^gP2}I={TMu7>vm9;esn9xJY8gZm@)WSj7lF2zkzuLjsK8 zczg#d5%M%b=8J0%AT0Ixd}ThgPe+rQsu3*+Ms7UPusfs^7HCK_fS0K-Oh%9(ok>9Zf3IDV(@m0@wf@($GUBBNzu-{b|hr)Y{Bs zdr^Hj%Hnvk8Fnz7|BY&y7pD(nQ$^yO24TXG88ee81TVy9LLY|EHM~#TI<--mFwxk< z^^u67ZX$bf?%!>z9X=I9DKMcED6bTDkgiN#{%AYPrgc!2Hz zI^SD7`Up~}ghB^^2eXl71&<(!o{?>+DV;`%*N|}q^1y_zM#=C)Y{6zI06eImsjtLM zLRvU~H0wVJ;@( z+ZuBloJQ5$n8`lMf@Um^dS^L(0E~{L8b~?pj)r=z%6pq0WNF%5IHJWtRfy>Tdlbe2 zW>hsPaFy6dF&TpacQ7!*kp(h{OH58`>@3dEz?Ik$YQk{p;N%BprL^GQz(D(W+rkCWW=0Dd8ENa||Bb0}>WRv-PE&)Ot<%&fq!x0T8qUDLm?ZWF zM&T@01_bQn0KJ+2wzB8ZC~>PUAsGq99_=(WXi8wpai+x4ni{iZ@bnWlMj*8%Fl2o= ztc}?X97BEiF&iF>!SN@~dNM!m_>xGOn^I}s?2xG(N(q`BbG(Zsr`dr-bgH00VmhX+ zkM%N4W2XL~njH@~6}(T&qseRbg=>PAM+g8DZO11^m#COr{&S<p{T22WoMj4>ZJ%R}ZdU;bmYJUHu#|Et__Di1P|c30fZi#14-K zoC@Bjy;xC!O`Bekk#!i1 zux4ci!Hw{8{)3lSYIS*KVS0ImyE)Js;T6udydrCb&fS3ubG1di8(AD5^m(u!J9h`Y zADFY3>LypN{y>49%PW7K&qGE|GeoB?59|Pcj};bf_Daap5H8GG3E27fUtz(rJXcs| z%RJ`Gf2@`Vd!NjY*M3edk8*a$%u-8Sd*@-6$I@f)dC2bVw9n%i^8ACm)bfB+!TYp4 zMtaS*4pciMV|h$j=0WGi zj`y{(0~)mE2keO~Mu)dYI#*k)VSyR;Z?v&vwhW$r=7zbOwmd#0t1U8*`SKsD<-u7` zERQeuQ_EwnOf+eqM}rRdhglx~%svmNg7;~8EbyAWxPQ>{NI9iC%cBXmv^=oO40_JP z|N142fi()kufSCE??j+o3V?6*js%40i#Cko-5ZIuyzAiOkYx|NIe`6(vTu^Rbdp@Z z0AKV4Dp`VPbJ^l4t{KHhZq|FXI$Go=j$IaMw~FjBZCJzr3{!k}58K2FQ9$v3=q^(( z67QF%CKmcN-pY2np}J!aFFD4!ixS?K{VK=rq{Y@rvJdO0yQl{4pj#;0GjKNyZ~Gyf zIcpfLS>pQzucxH+t2fx_p4nH5IBv5lv-Df=xVv;gBU5o@hy~RZ92RW8?gYhi-wp%} z+Y#6_nr%M^^@ef>w~jWs;^x{qDy@#1q_>VriKGamjBFhh-v*nnh^0k4b7Kd-=Be|F zy9~jg1Rv`Y+vKqwJ4WaE9i!NV8Dq#0*+dz~i#IAIf$es9&M)Akp58u+c%mxVV7i~l z-1bpqw^Uy}0(O9Do@N8P_cBEJz+^WmcZedAzB!AXp}-H{Pnyov?IA@ZYgweXl0wjX zY#(LIsehx56@#F|{2PvCIsF@upnE#Cqqr@!hdVWPGp{F^g><($x{DGIpnO|8xP25F zTc~gFpboNmVmDL1!G9Z}AcU5=mS|-Uky86Tq@d(RkZuC^JmK9G*+fb&2V21tn7d-B zX1Y2enN6YI>4AjYA8NLiG8cJwWHVEA+~$cOI5x0LmpRZYj)bz~75hEoW}heGVQ-q- z>?vIJ8$`K?kGQ&bF2b7my{7CMxX~S;G7$QVLK5QS7icte^F*@_8QhxMq*2T!RKNce z;~00DnysVKncjQrs17lI*R7+o7fG?x<8#Ks_>lNEw3*D}6p7dqP0YdfJvNa3iESnp z#+pyD;Eu;5o!3gAKGJZUTp zH@Y3Yg~8!YhNTW>hOD57b%7b=7_Ey!`t|&qD?)>C7VE-~*T-&M$mnz|R>aX75ZpEF z-_i*h5#CuT`%D9tM9{?PXk4&UwSE9BjFYqP80A2T*3y|A=0*|kQRHp>7kD>Cr(w)n zVl_O=SZgLDl^t3a$@(L4|!7^VVCO&Lw#aHxWa7JhND0|w^b`|qqP4(RY4DPT3^j=C>>zJt$Z;Lsz(8hL<3W6R zHzjCRAT-B*(RED1w^uOx9>JDKn+}H?6kIa#<_fd})5*PIq6P$Cxx)J?o2ew!A!C$; zRBgr<;qprdQv~t(K8rO$(iTKV^P{6Vfix<%0D0im0zP-p1o;5l7mj>>V9wYV9}*9S z%vS#S6`DS#{i2-P#J(7CKD$z6ZcL_?vJJIuFK3+y?B6GRFAFdoZfC81Lc1DADSC=sg&5)&t{c!0z~9c`$y{mCWe$V8B>7 z;tmZl4KSN`dNBA_EUb`D4@TB~p)}0#`lHPL9bP!WO!~K4Bas2p&(N4m(`zIj5)X#V zSHAqmYGH786ANP-=RtVqYh+S^<*P5kPSx4NSs3iDiPhq(y^vNaw3S{gnWk^V;*gEu z^Yj!ZgyPsiM&fxrDA^{?qcB?WJ|gb;=pe6|9-#dzn~#>`wNkw98r#os9LdqW9gPpy zm^}Gs-iXCZvza$y-$AM8`{O=3yb+5tK5*ojdm|R-cF9J`j&H>N1tpRM}4 z;(G^2M9sY~@M?4JjFA!KSYC*KLvVNx^g|bKshr6Xr?Z>Y<4K&*qT0aW#Bx4@j?;5C z+~FGt)k8R`Q~H$k5uToFZi{)6DPZiwVUVZoyEK}lQc?0(p(SzJ^C=b!0@CGNI%Li| zM9N|ivRl%-hdO#&#M}M^bCl65-o{HV`l>H{8FOF9oayk!fu$JlSKtbMdOnZES0j8F z8VnRlu`H7NB1bD4UnpD~HIkwXagopkddFMD~j0C|lYjvYR(GRHdqQ0uxq-fG#-?6xxoo&E_PAOf|}ha>7_savY93E#mtUHp2E ztX(1U8!Yr82pFdA1D7@$5o&K{@^Fl+L-49JvRH`O&J6HuhX{NQ||4&sab`s@~(fcxVuo@!&n-ksfi`x$TZWL()ymQ%);kfpA0wNym6x?851cQn4~J z1EpV&TM!xlbV&{BcWeh43mI1^fzUFRoNUMH3`eIL{2zz4EAcuYo}N?0EVx4ya%5Qq z*{)(j9*~CvQ4yJ*g1DT4!+0Ivi6yb)IL!~5ckT^3G}Jg}GL9sf<2OX5?qDiN*W)*I zATM|Nhj%2w(R>UJZqq|JZ~y_8lJXeNjuwvl(;?+BSu6u;2u5JIRiuZJ5lmB{A=nZq zi@I)jKzjBv2x~?F&*zzd^D?^c#2;pZ*nB0UUku>-i+|e{J(tD@V~H$u)C7)SFfsJ# zjZ6eg5FHZW04kJBm$9+T6VC5}Ta|^uXQ^fl&FGMZ5*}rtkVJ%b$V^Iy5N}0gDk8;n zCvMpKNO9e^(++sYVncNpeK=L#gd@zlrxpe#q~Y#-F+_G2hI{!{HiFW^5U&=G z>_C6yQ5YyfiVUZPfu@LYC6ecmu(`4H?ktR1SnhY++bRIX4sj(3qxEhiYR0(p2K zTMSze@Zu%T+*pk}F)DCoNd=}~FmSHK9}dT91Z2ZporE-~jg`-s7pNJz2t#M4WY+P_ z4S4P?UEC^3r%jQ|rf_YGvv4rLi7A3b1~g!H zn}THG2y{f7LOK9=^!K(Y$TybyC~XQy^!H;^AhWb3fw54m3k=M(m5oi|UObU?CKi#q zZ?fAIq&(BAO+kLEB=ogovMFfi;~YX~B;;wPYFMx&}XDbPT2AjkAUoFGEEruyV#?tD6Wi@C~_Q{g2j_DCf>m$&3$%iQ1FBc*cZIRj0l55&+!^`D%E~;=Xf2JO;Mp< zi(|7XR{TwE3LJxZ%r?bTDB);q3Rs|A4a{Owd}k)i6)`KqG}6$o-0BFf_+XB&WP?~V zk-4!dV)83}_IG2lQ>jO}*G8NOd|QvuCd$S@$7iVKga7WJx{0!J5Yz5 z2H-p=M8Yu{e%-FO6mtjH9^?E>TXq=tu;;`3g8;?&O980;laRd%Z%c81oF2>-&c{Ld z7zkJ$*+#%e$NV|E-V0UW`JHv)3Cb~=I_W;jZFg`d;U8=v2%!ofM6<5IZ(g7*n<-0O zHH`QIF*++yG#R-dZeBErgbbC)IN_b65(15S=y(_(Au)ll!uifGLN^5597#kI zlj9FuI6*8mIkZXPByjD|AH_jn#;t(|Sv=WGgQF(m)8JNOl#dbdj4S>JdA#Ofp@8JE z{25A_CJdI%9vD&F4#j2^gwbAy#_*7cXgsJA09yik83AC1C*V%D1?ryO`%`zg701IE zJY7TQY-~9p6kjhhkgbFIv@jcl?$NP7BYm06lU_t5mwM!h zls1(j2Xtc3C>o%Fad`%zv=eHBBbr_EQdN6}?4Z!?A4BM(LKD?cC^Hd42A4Wdska&=qIL~>s7EOike z-^`X!=0g23GIHSan|M5D`4?XwV{%cA8+LFYA|o?ZiWdlO)EvuMb==ApaCO$AjLQ+o zXEs#wQKVwifgU77o@x&dg>a1|DT_7@3V_iB!37#D-tqkia)Y7{Ul5XFUZj5dcw@ez z><le-R!0KZO?^Re~(#_3Ni7L?#55f`d$E+vNd<$>E*$9CaMs z>5dI$#WTr~5ZFh+Mz=? zzJoR22diT@{;&d3u`-;0H%TTF<@Nb&zDPrgf;u=faGGeNfcoOV8X4|B#kNJuE!3A> zaRomHvc|_Qj!=VYeXd7A8@BL$C(PiIRZB#%1I{*rs9gn5z4_o z9LJCsN3HUIz_OOzwXz5g+{LLIZX8%K@Wz3|S5);5pI3(vR%#cYcBv#>;%H=yDCMY^ zOiqb4t&3K)xAS-1-Yee9=Z`Ml=bk>gc)xw@=;D2Rr^bWg;K6N?IAs-6j_S z?kRogdcb-*3FoK>!XYbm7R^rC4P#uFhQYqgD$cO4QIVr$UxUOxjD5GS$1qc$oqwJB z!#auYy*c!^xiPo?I>$fS3uz7J82|b`(%FBHV)n?vzq6i6^KUA8F}*!vaB+VM`NtpM zfx`+TToSVm2EWHR(BRPh7fAFGBF{u*KD*7>@$Vg5r5MIA zf7O?~cJmT_GP`JGlX(2d)Q(3ju2;^BCIvB5d)Ynk^&l#r#^jt`KKg7S>TOA0JI2G5 zq-f5%{D60V8?zEmXZcw&235wp3lI*h?nUKYYHU9MPT0Z`5 z#Ety2BSf)diUho-gP@({GW~wBCt6?qA&vGV#PazorFw-ogg+E^#}iUhrv!P00Yzo2 zwB9(I$@KY7+CiR+1{bGr!@Y4Zwkj}yToazq#%u9c23n_e$UXd}G2fNZNUqZ^8*&eR zKN`8PZTJ(B_*F#=3$_Ia%Fh_CX%W&93QB8a-RVe?>g*RyV8Ki2 z!@;DD5FT=0XEJFDG>Y@N;wtSkOAlcX>KbYzGv$&L1{gd?&y@DzGc1^ye-oaBnmNDo$DsMk$um}etGzw#Jz&p;MSBQWGf&cm(pJMWv>E!6&1w?#H@wz@8 znrP&GP39im18%J(@?k&<-~8~}w-vABE&;ET>BM|=PFoS_=X-tn5{;v?p`XExhT(~g z*t%USq2tru@VN(yQ&{$M<5jl%6;+x3cQoO__`&-nC!luB61=CpCC7KX=UBVKjk^q= z-rn`r5oPy@Px_wrve8{1EbG#AMSC;-VrBPHSP0RzK7Z{!bdWBOlQF%$OYOPo>*H$A zn9=U;9%Pnf;layS)PEStS2m-)_QW+OQBh?x-o)jJR5(1jxWvrB%SUl3lp+^0tz9Mc zbLlw-y7U|VDbIZK;gT!w_(4;z$Rl09(cU_A=$77YX)Su}!TLc}1LnR|R~IUO=8BKaee(K0fB)%+ zA9DF$=<@C9%5h+T=0C)xPr7ukI5}s#^nLT!T{`i`4Fk&SKmB6=R|Y=fzbCI+w|nM* z4QYLN{ixpB9&z6T^!thu_np%3OG}oVJL9!4FF7Ukz2$$2RebN1k`??~zRjh(sw8yP zph5py7`yJKDHCd^6c+dGUEI6qqW)9lw)l=deR>!5>*Zh(*%kjI_#>&W`qFirQBuUn z34XfkSE$SFUkP=Y8tHQOX(#1z1ZJ9ZT$s!OxF%NQu*=*xF$_=gA(!~!eO#_Wp_7uh z_caOWWTfu$D5AIett7~bu*tbq7?N&7X#H+eBcRhZ@_*1$S4eAyf*sTz8uIv`M zvRmF4vCBj9Yrr=@C+U}9i`})w8XYF19Ijce=d;9X%M!1vBfm{skOlj+!%lHBm`6z# zY^cM$J{YgsVYisH(;RlO!TjuJn#G;o#x;X%5$NWfGm?@S+SP zSK9yb0%$1o35kDlC>YYi;lTtKI6M*v(7t?fs33qByZD6x{4)-Z1@O;0JXrpL4iEC@ zDu+teG{Di|t2JjIMuLUi5<-g|eMS=LYIy@rZnZR#4 zJe7guYP!P<1M%;1cvB$$_Z(gni2uJGUJ}56B=;wz=QA(vCl2@b{`+SR_v4Pof9dex z2=_aO`?1`||D(gPbFRbZ5r>NrZbDW$JZvytJ_h^@qkKQtwm3ZE;`{mb1We$JU+!>U zu0n^8bhymBxaJ86UWN;juK_vm&*s1f<-o53&hpeGRl1*($2vS2z`x`0dWZWt_ZJRt z2*iKf;fn(CUw3#afS(G(hUIAr;1@c)+2MW;zslil0ld=Tp}Ruz@p_)=@QB0xJiWl- zNr(G6{ZWT63dG;(@WQ)QzK?%KSJ%D{_jCEh4zCa3p;1%5K4SdDF_q&+R8FlKH{$DK zDo0dJoOEmN(YZoKOfJqHRFpfYU+$njxr6$KdJh@YkN^7fU*A6bUo2=}`Clx7eJ>&2 zr%2*_M*d$ah+b>j%ZT)oIK_HhEQrYLBNF;)ej-yMYJQ9+;UZ2Xi-2MkCSr;;szfW6 zc*PR2STgS;ar$VXG(*KD!#*Odk5)x1A=mvxPCqT07Fi_pleCwJ%u7VtCAl?UHR`6x zxfMM2<_Wn2zm_W~GWG0OZn~|uaniy!@XU{L=>imBBAE_`?RrS(9lwYYomh*vo&`;6(=CZt!A*ziDu~@VuN0 zok7WFD>3*r1|MkfUmE;MgRgRi72Efd2LG6Q5F*~g;6okGd@ncn^c?sPbKt+ofj^Q1 z-;e_z?+hrG)AIR#XNVDxnsV+kxb2UZI>Vjudz$!ToZ(E|^6k3@x4c?maNCbI8Qk`x zod&o4sNLW(Q=WV##)svx{pfsy+kSM3!Ko_GpD!5P_OI&=Zu`+_gWLXco53$K^6xdc z<^PWjZh5}M;I`kT4Bp$wZ#1~=M@XBby)U z+4hU?8@va?eEj$v2?>{QA;G~Q%&t`-7 zHTWwAw|x7H!7blTAnEa8zLsxi8r<@&r@<}XE;Bfr!k6<(gHwebA7gM{dVG?>ZNIzI z;Fh2F8Qk*fM+UdNU0`s_x8EDw@_CuTEf1eGc(Ez}^9Hy4eAD2hfUj>?=SZUdyBmD% z$8m!Xam&vWsT6#O_rRZ*)7{{fpS|g5#fR}N4@Vf>^6*xJTOJnDIKzi>EDr}6-16{$ z8Qk)4q`@r@YYc9Am^8TMVZFgE4}WHG%fm$mw>(^La5jmZfhE4>;l&2GJiOfCmWN+3xaHx^2Ddz%VsOjD(jFoCu>6*X zNrUs!>+QP+w>(@;^5H`{mWNvmZh5%d;FgE~NiyL>IhKbH8Qk*lQG;6^K5KBx!&eP% zd3eC!mWQV>F+MDh<>4m{Zh3g2!7UGm7~Jyk-wbYfc(cJR52qO1@^HGrEf4DrZh81) zgIgZ{+~Agne=xY^;Yx#B9&R4-aTOK}4rQ<_ATORH=cn^em9(JKp@L_z*!xAb5 zAL5pW*BadN@D_tx9(HHt@u3{c!%Gcrc{s@6mWM+P&Z_(Jk1@FAVXeU}59L|++}df!$StQJUoS@!H4>^Jp7o!Ef3E(I7NEB6&swF z9>2oimWMYO-0~1REab!TSRVFgqvAu{^03z6JrL&ez1!fHhY$9nl*4)WguyKjw;9~> z@VjoIh~=?7{Efja4<9nP<>5MmTOPh>aLdCt3~qV&uE8x2&vXkBET`q+xdzWinAcm0 z!7UHJWN^#F8w_rFIM(2nhqoHs^03a}mWSUrxaHxG4Q_e(3xktNzFtcVZh5%T;FgCk z8{G2ncb|n&e1bfD#^9ER+YD}bct4$APLAbUi^1(Uc@>=kF23bqTZzDjjfWo@n87U% z&oj8?VUfWt4@(Sgd01|6%fpcdw>-Sn;FgC;gIgZ{)Zmtfiwtgg_>jRZ4<9qQ<>3~C zTOMvVxaHwqgIgZ98{G2n^eZ#^w>&({;FgD%8r<@5u)!@4&n(Tz|0FIw&#y3e4}-Sl;FgE=2DdzHFnBMM?;?X+9;OU_ zvWfqU!7UG)4Q_dO+!r$SvOJ6!-16`f2Dd!?l))_zFEhC1;pYu*d3deCEe}T+-12a| z!7UGOH@M~D4-Ia4_}>P%Je)ElqaVw||1h}a;m-_i`8MjBj2z3i|6_2=+mpYPiEnxM z+b?Hu%fpQZw>;cxaLdCz2Ddys?kgGjy>RLK-5CbI(BM4`Zh3f_!7UH3GO4HfvXc{%U83~qUN(BPJbU9Zo?w>-Se;FgCI4Q_dO{=a49aBqp{$yW^C z&EUfgZh1J-;FgC;gIgZf8{G1+!QiKvd>0y=u1a6dKN-Bh;Oh+j5rgkAc*Nj)4Q_eZ z3Vew><1?aLdC12Dd!?qQNZ>hZ)@VyU_;!IO^c_Hqqc` z8~jd#pJnjAH{u2#>cNhmBMokOJ9M~2m-v>qHyYgX_6G*HylphN9=hqwD@_ekpEziGUaBC0z$l%r<_?5vwWy-nK;Fh-=3~qV*lEE!+ zcN*OC_JF}Hukyxa^lABgmci{j9y7R|$FDND<^S#p8Tsd$`qtHC@RJR`@@v`g(B$lR zNo{t#Zc27M_4Vxd?pw3trMG3ryWFmLS9}e^pYLDKIs7u?**gG@0ZI?@v8QkW3&RrS&cwGAW_BD7vgAX$JB?iCN z;GZ%0XoFvB@W}?JtIX&7ZG&HC@Yx3cSA+k=;GZ@4QiHQzoO}4$5Wt<88hSZ^kJ0F% zy#btKF+a!Ot>0{S$}iR1&_@C|b;r*I0o>=?H-LLN0|U4(k4jVfYjMmoHEAbzvMIfnD&<3H_ij@A5lJnwG3FVwFzevZSV0X&o= zzKa&}diC;sxyu4_%H4fM03Ye@=LhhbqykF?m%`iafj`~qbH6ZUpWDyCQPiY?2T9XE}C3BYVCJ^5hIXMazds4ML6{@6x7y)V}_z?eU6Ci`h{`Iq~G zxNT$9{Zq{5Q=ne{JK9ap^6_>tNiw zfSrWDp#G~}`a*=+k1w~!{sR}38B9OZrEd;o7)(D0X|uIo%Y&MOPtW$_^NCOI+b>(* zOj-SH{ttzkSvh>zUR?4Crl-vm%tv0}^ZwU$yA;Nef9V>O>P;yEzorG^@fKOZeQK`v^-m$q1WcaG@pHS=i3{o&}gL(b{BZqyp%R3ACsF%apA6fLHWyJ+KX&s&szOTiKJR=TF&)DZH*~fgf zuHWPP;gL7}BrUVUU)nQ=y!iKRy*S?!aQ7X2PBf73nd?co7MXj>Qi{(h_yqAe9iMW1 z?7uT{^L>0K+W=gr;4>AUFg}&|@GpYTbbPAtIUk>Dd}{FFUlgBOeCn9MUp+n-;?sc7 zOne&g;a?Lzt@vDw&!zZWjt~E?#OEq}uEysYeC)s3xVaXeIrzl!`4K(`{XMqh?RD9I zzVwhs?^rq5y=mDuPX1Qf!ov^0`OW;j>o@LoVJLUg1Lw}U>xM%wUizayU-iw?{@!+X z`Dv&1#@klBbIa_(EBF5DdAII5=Iwa<`Z4#tTQ=*5ueSee#C4CRZ`m(@=Euj)e5UdF zfcM!)TPp7wTlnZhYhOC@$$iG$`^VESFaGO~E}MPB@|#}EJ^c2bpB(V5@_Cy+Xujv3 z@>AY8bl}Ag)0zelsbew+l#2J0pKK=0#g-5UY&6nQ%(~tJOIB?U{^{;Jz z;^A{9eRNWP#a&C!{M#>JNHjDdhZe3O?); z!^!_Z3iyJ3hNlmwfZvs(+|tb9`Fxl{|6fW`-;5M|?n?pZnCizT`L8ks{ePr@9}5Ql zWF|=0r>O736zy^f@*Ft~pNmqIyH5%}t5TF(kb+MYCXnIU{nZqF#;4H#wiNX5rD(?= zqpri1dvJ<+{WOJ~TT|HE^(p9|PC>se1wGxK>|xqvN{agaY5(Et^_3L*e{Mh0ALyy5 z)k2^y`8)Rjg%??xMsf1@$X*IxWJX4Np1n-UJbdg>>{w!1O z+X_HZ{&t#jeFpwb13$#T{YD^V{|9=e8|PsZ+hv8$xEhA@vk<+g47NdtPMh{;${7*D;PJdegwtZF^`b7r*I@+6mM;QKn1Ddw& zHTCs}HJz&$Pm8S|V?Q?kdyPB=N%k_j&`bF#12&w99u* ze=M3YwA}Lyyu-k4d$``%`Mjk5wcgNsOuN|a_%j3VGkk2j?KS%8F!~v9>b2O`&zTB% z*}x|nc!$y3+lEml3Zfp0OnvQgBc|QAM3sRJA7R?DFR6c>XzXg?fYRG~yV11oqNMiy zvw`;*xZRFt*!nkmrk}^ZU8dZgBzX=s`k80!&9>W@&_1kJfsyAzW#ehL?ZoKcw$D#Y zeWx2fw)}rI?NyOvR|gsRmL&OqiE>zOe~}glGX0B0X3T7AjI_q8T4IriCsHimYN;)cgWs@tm~we^iv4fU5rJx$fkk;aDRNNZEW#nF~X zQ**2}(i)AWf=8-iEPn<{;I;a~NOj$Xk=m;I22WMn#gUm!HPMDhRdrQOQ){HUC0Z4W zMyjitTiYPg%&LZlrt00vYFnZSqTQIZHP?_ytbS$`1xA}&9m(pU374SNO|3CgKEbUq zXr={H);71)H^yoy=k8@25s@L%+)&l%nHinQ%#lb{Yikrr-ebi;+14DXZELKK)i*V^ zdZO)U1*p{lIogcGn$S?SRc#G1mkiPN=7z|{HPJ|IQ_Cf(;E~qms+QLJ*rk#B#*{RT zdqQ5))Z(uBkWv%eo7z%}7HNzSyL31adaXbNTT-eP71zQ~y)AaPCPjNJ)aqR_zZV3S|0a2N|5 zf>!%aG_#KJYQdP7I21;AFM*10j;MkoRS+&=9q0tj(Z-r+V|9IWn8HORjuthwn_{$& zp#C$fVlDO6(N-y=wyHYT)G}lWmH8kw-ENFI^9?QBOu#i!bfu0*TWhRoW<(8P_tr%o z88L>8{-$HwIF2T}2|#3Pw6RrYuVJwX$7lH5#H*rGfUmBLR$n+&zeD6~i8e&5hV;rI zXjGsjTANC^n&_Uf;xg@F-p4!@{WSC*Wal&;IsekwkfFhu`{|a!14J_<>7FyOAY1Ea z?A9;|`4!6Vgvh)q=#OYCyqVG+f&ev&W_*!Jb-<_&qfo@q21F;`h*OTKsUmiYg;Eb)zYE zPi;nuJv5nvPSIk-yBe&a{(K$B$Fw$ikeG?g#-?Gn+{P-NsRY)>?S<%+-YbsZ?wmH==liPR0?POj zf(Q0KSKA@=h=As;`! zxZuq$__Z#0y9<7p3*O;^pW=egalwyp!RNW)r@7z@T=4I^;0s-FdrybxJ6v%0d5A?W z_|+~xOI+|y7re&>Hz6v)u5rP?=c4a(!OwER*Sp}Sy5RjT__the%LOlR!MC{JC%fR= zT<{q#cus-Z3FC<0cESBFxKb!G&jm+l(fKQI!7q299?y6e93ejEugC>Q2-Nuty5M%~ zk;n=coHlL$gMMTj+w@u~s7QaKYXCr;A*0_x|q^7yL^OcN}(d!RvUgUyb5Z*$SFcfrTH;QcQ6`7XHS zf?wu>Z*jqocfq&0;P&2`3;o6I7aI6N17B$1bEARxyoY}roV77C*p)G15XX=^dtxI7 z`+~DpX08w!2gkh%@YccH*KqM3;URq;F-85|s(CuvUYTL;!TX*!x)2bMW$I+9xl z?sn32Ah!JE&O-FI-z(r1)4&v5@;A$WVVLO-Bs%chX$Yq5e*qju-0hr0H;>{!W?; zI@I4u)A2(6oirUT)Za-TA?bH_+V%HK`V}Wl2MYCf(sZ0qe@;AbWndMO$P_{chYoJP=6;)2L<(a(sWEve@;AFi?Ld zO~(T5|A}3HF5uAqPMVGc>hGlK5TO1}nvMYK@1z;BLH(WdI7#2&q`7cJ{hc%ydZ@pX z9xv%CCrw8Z^>@{;s6QI%zJfQGYv)-hJf`_U;mo^lzkhCZ#_}O23noej_RU zVp964r1T?6=~YST`;*f5B&F|4O5dK8{zX#yrlfRtQo1uKePvR*Eh#-SDIHBppO=)L zl9Zm5lrBw57bd00C8hI|(z!|L(Mjo1N$ISlbVgEo=f^|s;e({~J4xv`lF~0GrJqVl zKa!MQm6X0eDSb~;`mUt(?MdlhB&BakN_QuvJCo8^CZ*ew(le9N(WLZwN$Dv`=}AfH z(xh}@QhHocIzK6$o0J}Hr-R*vU&Vc}JL3>uCTxq1!c^b*kxupd4)%MG=)h>;Sz)fA ze|J0S$Gy$VQM-ciEx~1*P6{sDwpTE1b#UFT*hnxaGz>Ba`)g&~u=CR&=qS7kc|C2% zObN~^JQ9?iVEm2P0l}`q9|La1cMJ|%HJDsiXLKT+b{_INCUySv+~l*}-czj)%(;?1~fwyPEtp!LIaNP6@&I>_DC%PR$E;mj_1nmINeL z5{z#O#$PV!nm*n-5rq?1r!Z9IImqv&V)JRX}(JL?G^q=p#n)=ykK z{L14u%EOPkdaM=~KA`y96ff}x>-$>9xHk5k^6o1F$Ch`G>j1I$h}op|l*Hej6yH`Je^-@PJZn{& zbqDg7PT1J?8kiK&H#B(*kawEoEgCjovn$^T>4|(NAm4;NS!mr4@ZH>@(_;&Aos*>38&x+LJW3-uz`TFKQxcEjbHW9NfOtbA0O z==aEB%&&WHU>-8EwRp8I+ol$LqKzMQ1Bjto371>v8nn+rUkNlM@_DS246ew*HEY%) zF!oq}gD!Eh23L-RwOkP8fuIAP zsN91z3F}s(yE2}9B$01!U?DKbpDWB}OVYYb3IA&d;T=ktVF;PD&Qii34k28mgs-h+ zF|z|qT6szs9zwW830D|GCau8_RrTdg75)x1R9{|0biYAoJLo@P;DYM2J=RL%x-xb? z?5A%ts`@Q;R&NvlN(?;Aqcr-Uq*9GSGrll77Sjq zk~8|b=M`5LpI1DsI0BboHrNScnKDqmOaZvcknSF$Q9-X~-~o)@RzA$Mwswr2yH?^8 z57dFt!11V;rWx}tE8GL7vSqKhIpI~fx;;Zf^%VQxhjbpB3l2fN0P?Xm9N z3Y{X)5`u0>_(E_-!^_13#6v3gZ`S8D-CrQ+q?$96JHu3d1Sj;iOf8SUI`%{Bc5slp^7x>&0Eyz>y_tR1&a~J;-BVE7 zRd!*Qv9PX;uR+v_z3E(%aAz~?>cuz+f7WX3qW!J3g zL&d$>rxtj#&n)t0Ppz=d+k$qR@J`zr$dOy5C8%`yCnE*uxXYyY+zaKBKVL56K6o39 zj;f&~4V2`Bfc0yzf+QHgdpBe`{Ri@dPZ)tBn+t z#dl#K!XWewt`_{k&d0s8`(g53(5wX^wt9WzmaIgW$Lq^jFW2lCmMYaF1Epgh2l0<@ zp-snO7F)g9C4oG9{6atd{-mxmf+g{F=%16ikNu$z9d5B|P{8eC20akMmeYm2hJLnUJpa+@18Y2BY6Cyrv z0KP=TOlu*yja`d!P(WFH83);=SU|;B!OO5F0$Glcc7O2Hob!;=Y5*AQCI@mPr*O`c zESyNec-}&Ex=HZ?aQhv&5t50i3Re!0QA~YXi@n)5-LV%->Rb;Lz~3_9P9Me!fQx`7 zS!X-a))H0=qI7JsMQNZYuV}NB1TsYwt--hL8f=mhMWmuS+;_0=@ydyjSIWt&k*=dz zrR<F<>n1QRiEk*Ri!soS`H>pmN^!H@BScK$McW#bs>qgw-mi3VG{BTP4!C>5h;I@&5bm znDAg6xegp6*OZiU?eEAXE#Ck_+vusbqF{0(>%8cc^SgV8)F0g3^;cGCEpUM$Tff*~ zYk_+()-IA`2y*w)Gr&;$C~U#H85Fdr8Ykmf%(zO*c~|qHkeWb_Ej4?$_RoF63FR&< z(w?wzo?hAV_dg&g49f9N`ilVl1R$h3kg~pRO5OCfEv}S$Ccr2rJpjp2wD!o`m$i@6#(I*c40{VGof1t1-ZF2Qha{T7vFrJGRv1JW$F57>fmuLu)N z)H;!sJ~0fExa zwca29P&gqTQr-o3AQ(pkkA4u98@DFW5VjA-fR$+m6|=TO$Php|z@t{b`3pM;r#scF z^(^o|0Nzi0d^@0^nk`fyd09eW_kbomtz^Qnfwr4it}lqMOluly9mE7kCvc?1Fuk3N zU`2ES(a942?Z643q(6w7<@aOGDCs&3Gr5G|3Sesu>@WB(DD8ayDny3ByfnV4r0dv# zGrpN|_y_m6`gifH)oIoQu;w8A0*xvY!#UP7-R)_?>cQZ$xAw9=ep5Amfs{HDp~haL zac$HhKd$hAii+^5z-Kx>%`|@~8W)b?Y>)5j>?*ye6duT&yG=u6Qq1x=bY2?YrK6`f zB+QSQc{@92cgF3DljJU&)thNeQrWLuj}nkcw90CzSCeV&RN*mH4R{dcshyV?x?TlJ z9R=x;6TwNCz&jXzpd83RU>I8FxOaPk?-cO0?s*Fh*`4(V4AOtQjt$s7?vp?BQrHUX zx2}ByWjp(8#Z#qRMzqZeGl%O06VHZEHv=kZ=K@puMhRLo-MSSel*LzLL4%$O5lTA; zuL_~1?6|$yF9NRv93Res1-``cC0t-#hrG04DA-T{zs$jNC2CcJ<_F=D2`H=WPwWja zU7K48)+r;^uIC_^^=;O6*2=u{_(v`iy#bY!^|R#enhHPl+rh4@bF9$ogM*ztZTpzz z_5;ul6%++4EMG4JRzOw}?reV;{;ko@H5(Uz9_E3l+vGrK)~=DU)8Ov5m11!C`paAr z7cXIy9}DB-(CJ(Y3&|Q~? zVACA%hsuh|t9PE>eWLa(%6D4#NVvpUVW0R8FERxk>+3Gb_XMlgq*;@?4~yfbtowvG zhL*BrZ|xmi_DXsX+Q{sV-Q}5-_SmHE^nHWXkEVTC)_p*nqnWS!0MD}i5y9%UHY6v$ z{ip#~IxkrLSlVXvZxT%E4%{28-j%i~SPd2@-ixB{=JisGA>y_Fu{cGPB&V6Y$p*^dRXAyCoP5D0d@elun@bXr6LF@gn=v&d;5j*gjF&8N}7DDExE@W9RG@}3g{ujGN~ zRECGs2+~A*faaB%eSi@}7%zn7xbE{@_kNV)B6XNaXd)@H0JZ{VgdsQq8ynuv3Wf|& zH&hT+WsRCy83X|^G?k-i5H~6h6czXGrIle(6gi;ReH5C;iu9He=0Us^f5GmPKj{_0 z%H#9Jk~CNXb&UFi1h_d!ZqP<)NQ{&O7irUrd-W)nv)RlBKug%W~xi3ze4m45La^6vI(_0w2?1I4794#R0}&)|&K|WGfeBX}0|R z)ECskpzI$}PkJEmC01LjU#5#Npu-oi=K^cp4PdZqu68ARYoUagp9%RZ4*Br}{!qx> z@#Vt_z7XKCYaP-o!cK)CF4$g#eG@RWTaZnLc0(1*yHsnPJ>EG}pd7Yn1?J$=v9FiI ztP5z#?Pc+og7FRI@m2P?jGP4}^c7yhXo)Fe@gMLqr|$GT>v5?)Msmdh^$VatB-q8k zp21dTY=U^IL`K~Znzk|K#t_0U<2AktxzwOFf1nimJ>Bl+9!xdRPYh_y5 ztX1jO5g;g;P!kAxy90x0A@)@22Oamt^7wn$WPTTgjeT6TJ1K4ju}X)QGT9f}#i_H7 zArBalC^oZr)*zOS$D?OXLeLNj^ywO~Y}U&3N%1$!yRZ>H3%1V6B=<}97Cx$R|oRhTkSn3mt2Q3qK#iP0Uvo}`h*55EBeC@1j>>Lv`2Ly*S zpjTN)?4v-|5zGJ~^TfBp09QMY_L~XrCGo9VYqGaKd`T@E<@P%mI$OeKJ+O4RR#03& zX>pIliMq0mr!65dwUIq=v zx)7>R4q|K5^MbQ3Mlme1tn0wCu5gDlb_0h%FQd#dX;kYB!^=N#`L0AfckJVTr$aGN1T6g# zby(;Ub&p$$E&-=y6)LJuOBe2Ti{}mN-@&iLK*E^aC7E3zTuyo*2gP9MIKWzkqA>P# zKJT5w0dnl~c07*0RB?R0EI77Kf!nsv@7bL==-;vXL z2kjpE!K@W&B=vS5I7l^YwoZXk;VtDPZG)8Peg+wKu5J4|oaXUm@pZwjFJoJz>uMY) z$e07xoIs*rVBF34gVIoA*CO3Lt_EG)+UGgD^&WY4Nb5QKL%y>3<75xst-A1XufEE7m>)RT7c@|rog&dQF+#zxWU5k3@cyNj*)%_C82WYE@e~b4R2VIAI?!XQt6pxJ- zEGvTDlLI5Ijq6$0%O!Of^85;UtY=(6V8-1@;Bo+GT@g6U=e>RkS7;`aANb3F55`9Z ztXn{X5oaXEH0JvV!-W{nte&(9ecm(r#`YA)pSIRKqnt9m*T2WkO1oP88$)XS9@x^( z54}JB6L4kKtKl95yYr8fA~5R&yAN9h1Og-Z2NBjK>+LhTPv}R%hjsQ`dh~{=&>NO_ zu`%f9QoO8Q%FHtj2qrI1o z)&1jO{DZS;u)2yxm(Ju7`+X5<@YR-iuX;MzwHE;1*()<7N)lq|YI-J6jZfn)=*L%4 z9)b>)0epu?KXRv#5mFOC;3u5BLLgj^YG?xA4FO~e=ie=u@I#Wj4v>|SYr0T0Z}oMl_Hg78INzwzK{SLOx9j(5eSP7c+OfL(ye4&_6I?T(IUr+!!)}H|gc1_F_UbQ!=-r=DQ8fY#uwT&)n!^wgGk^Ln`KoIM7MK9E2aVA%LDZfgRf)|a-Hczp=w z@dD2tYIy{a!zY;>bshA;8!U04p7KC5`Mq9S>do6ku(m;d8MRgH%^TcW>J7gQ9X<>0 z<$>|&iQz}V?$`ZJK=^Eicn z3{;P4Re;|?&@23UiEznX!Hm0*G4u-R;n0Re*pF2+$nXdYc@ngthgnDslmP&?dr{I_ zf;WmLaaXy41f$1MM3zMO`zQ*n$nM3T zP(KujkOazm2f}{@VGD1#gEd99kn>%By$5P+ZRd@fp_nZM_9M&dwcbj}(x`}!lhWIh z(mQKS5u^`x9g0#uu|b!cQoY%2C_Qi7xtbomLa$puoAFz@*CG{x^NTqei`5pC!Bwyq_yVX)_d&2QLb?Abq+4H5^^DYRf-+s=e@HJ$3zJA zO~eXZk2nUq7Y91fyR9c5mvE?t8$hC9bhpVIhuA6tY5TZ7MPG=el{coXj)Y6$^dLTuZ1^wRUkBBwZJ4z!@-18$AY71_|*`C3)*x|UpUhYT44(h}dpK&=-h?+wQ-qPjjPj?O~pgUs*kOCj- zUw3CjUHr<1@H=-;{Ju9lKloaE_wFMR!_E(JyElVvh%w_MpT6Yt{>z zpVMMc^Rt~(=XZ{$8GnKP!{S>L{sZ*47IxNG>pgDGC5LbIE`Kl$W|ff*5|E1iMNhH{6lC zu8@Ht>pUn7*gA9?Uo*^Ioe_k6P{shP5}^y+jiX+Xnj2Vxii3)NOEw$@d}E;w3OJ|e z#qIdE2NLPz%XH%NR<6O#RUq<#7+H`#ob@j1E2#?jC|S4unzf%gfX}-;(^McFr{g;o z2qB|%cX}$E0=!4v8JLdtvVQR>y$n8-$dg8+7IAWzVm z3`Rl{gaBH)9(V;%oFq^+TVs?e>l6_^jPXLsvs5b(0ZBd8jBHtXHWe#9H*g22tc{O| zz?yH1n2~x$L1`iQ7d~ZRi-FQL`!+2k>p3V2(yK!_S5G+q)NrL`i9{YK$M&Uy9GnrQ z1Om@Rpny9ilaxf6P?81QodF(3MPdyw+O~58a*%aAxR-Su77)%k!WnbR`7|L6{?IC0 z`2*xKvk8ERAtFr~G2E}ysX|;b@v$&ASI{kafrtjUVcxv$Bz%d$*K|gaiG|Y$NM;AL z@|9b`6ayvwgI9qF!@~`)LvBnczHz_%6~z}v>`lPwemy5b^;2_T{{$dUvWQoKmYIqP z4E3Q6psR$Lfe2nz_*vSRlU-UDn8%qNAF%Ohq?q*d|G*DowIkQ-b~#G4(3JeLlZ^@| z4P7v$p%Ji5d{p1SyiCDhkEjG_VXRUU)*MNy6U`q&anBk)yJYj@?SadRfp;auqkKkM5el;E%wE1ov&qiMByb*g7@-DIL!clD?$@ z+5~w}+De#DXcv?+?v-2E9^oyRj~_$2(4^tbjKuJ#-qDLn11~0#F7JY)4}omhvq}T+ zNxdJFhJ>0PQ2b7B_BUDVgPP1g{+C)jSY;e6_gaMFD;?+Q5Et(6lDBfD79SpcpbPgW z#W{}L2*IfczY$yrBf2_rA8HytXXrt1b}JmR)BTgUY`MKoH03K5k1S8(Ad3TjT&Ux+ab&7<{jJYAr~&^Q)K-f7$Lv4B__ zSfdno>SI}*&&Os;-{>J2Mk(VXMg=1gR%o;m4Dnb$dGlUof<2fH7uEdjTtBObL~?3xW_8pb*u6sG$y1S(t%Yjn6guWF5pl#lC>o$>QdK$cOaJra@rO zNHuU)g%Qo6K4CG>5_koMLR{vnx-lMdQC;}k|D;wi7OK`TQql(Vbh?mbNBAKQcrTF; z$IkE@;1b@6Ch8*`mKB;WEg?7BHlY>Za&w^HE*_1v6@XHfgR$b7K`4= zN<^P)3@!W-_^rgCJ9P&~-naO*j3wiw4dy#td_PEe9@EN&0DIGjhK?+44=m=`P(O;r zP7yM`fVLV-2PQ=wzu{G00ybM~u)|s23q8x*9sQW$Lk(v{{`$7bBHrIR|Ns zAffkh#R3Y8HjMXNSckTdc6LDQ0#d{LLt8-fC<#`v@*6p(gMDtk@B^*s9CV!^giO|z zT)uPI0=iGkDGXnX*`Ea5%%Mu{4bN9&L1`EjNP{UN4|NZbZhoMHwICIHGBD_6fG)J5 zJ#rJmPzSXu@A=|?NuGHuTv{-pE+`g+^e^VigG0pANu+l`g=n$jh5!RWfWZyz2NsI1 z5PFkRu4LQ8GQw=*kv&ZHsPd!($pnfVS74zDY7BY_V?{KD`m{)%@d1Wh+(5fFRflS< zM!lbH6iaeb0Vt1s2=a*YbhGqD5y`I7A}R!v^Hy>k#-cje*wT3ut0V|_}1(ET!l^R6IEN|5MpQ3W5-pX}Qclc=(5W<*@j6N+h{1GV`ofviC z&tNS=1YH=b&LI^wv-}n0!~R+L6|iR}dN;6p@X9!tAmqU`Hu6K@LfZuFHQ;4P zY2GYCyy0yLn*9yUN{41&rP(DkR8PjwZbB|&KU24Pv*`j9j!-hVMd8usC=&$tfY?rI znHu4IK$h*4CF?yxA>taABXsWy9U?{!&1OUMsDg^UEUev!60*{@w2TAvnmbaj;|`N+ z2-*x`p~_AJ!$-z`Z}xOQr(u}_ZfT%V`ho@c!UKQhU3KGOqZoiNPb|{rRM!P-HA!Mf zppyXtdyd2$#)h{p?BQ%9!5!F*+5(0#?G9i^`-MHBG4i z!)X@C@hG%LH@^=b3=tB=@`h|tAi+Wv5pznV`snij;P_hWJ$SxU04lm!JP+K=m!+t- zK;C-r(g#k z7gCXT8h2~jowlxwiq=0MvG~TPsDB~MlL}yLQKhn`(3w^XI-~MX0;ZE`9NRE>LPS;V zGzQFUZzH@FSoEbzOr#ixbA-zx;Y~WuDD30{)*%;581)IoC33tW&KM|#8L}r9iHUrr zUb-D>wL}`;7vj(XU!WcAf_1PdtV?J;WM5H%l6DY=eF9(pA8#~G-62^q(IF?7thm^2 zYxr$;1I)?a*Tm~Ubr|5Z{&o}ITi{)sad@@hzzi_9*SiG;qWGW zBsAlaktW0u6D_iYZ3YEwVzuaiz>fy;BmCM77UB1BhlMHy}JjAd^fk`KSFv$t)JI3KYKllN1DfS(FlR zPJdIWG4=f*Yq|HTw@93Oh{^v3>wq~7b?av?Iv;?%M-kAA0A6pZ{1KV$B%Fcf67N*H zMqI-HPiPHn1c0ULRp9P6aX(~K_=L_Visig%O9K~)+GJLm7qE`+sSe>L6gYCQ#Cz1z zfX^u9IqG8SjQRYatEP|PJluRoUcwF3VD&4pWqiGCBRdAplv4TSpr@rV5xe2fN>8f z^b%X>BSjQZ0>HKs)(2Qp|(Ro9joxmfM&y37N`2dhfbr%(dEh}^|F;}$T7HDB{ zKzaw9QlORw7P9749J{#a~<1}>afuo)~-CcI*x zcYvSqZ}JBv z`Z57<#cTqT#xo>C@Zcyhu+hJrOL|VPQ=h=v9u4Cg^(D#xwPYMbX6StI=gc^?5;}uy z#omz;ugX}@>@dmIAPbK>TInT(FzQ93K>Y+!qmLts$|S#xb;JR@8lsL1xO4T2JTND3 zxbV;+dG0rP7$M5_IVc*B5gzvB8}%2EiI6m*+(pa;X`e)C5DTWYo>IJr>LXAV!pdgq z4sZ75buur`nu@I0apyD$7ltaB(?N^GjrdIH!(iIWwBzf<0y1Huv8U;hh*%ubrVJ8P zg_U67L5l9NI6UBi06&IX(FZDkIa+p4*$vrraBRKd#p2|EZnlJY0DTM4+iZ_Vmmq#M zD0G0t^#3aCfRT|mP*Vnt!rT|GL>ZXy)hU^_lRD{#0$>M~boJG6JHXAGUCsL67HB4G z$U_GNX!u%lq_Al(f;YE_!^ntDX>rulMG4uC#w^bHqSK#Ir>M8%Yb;o3G^*~#9QIKP zn(#R08P6%|g>xWIfIavEsAI}Rwj4xabwZ|`94ccT%SHn0M7?Z@`z$iS7mjMRvz;;9H`Sl8W9-R`i>+)~m zYCJg`)`H%-2Up|LYthGzt8o{|F*+q&jXMc&xf&+zxEdzyxEd!4z3pn;D1#>^?As{@ z`fkQ;#P8ADxPTOnx3N^|)!S%d0@A6su}o9O+o)DT^)~+E&J|{Y&MUkPQ$LgEOp`~v zjbAfiPu9lUxN#xJP{>~jq^OZLmwS?ueu~3!A7?$8AKyAdxGd;VZnoR8hDj(T+3lEb zhged&9mo{J$_lCw*U|Al9;bMpXSd^4qk@0Z^XP--M6)kWQWYjV56dXkcpmKhoNis7 zM?Wdt;f|gkO7r};Jde4|3#F^)ajkIr?4E}iVys~!9t+oNRfh3AI8xCYN1%f9HCI#1 z_~mvw*3h)@&NupV7}Fl17KRT-+_6OqLcGLYw`=5tCA`5BUZaEXC`&aC4r=@`lDox2 zP?FoZAjz=d5z?8!2c*nD=Ni?bE9P-JK+N6`Qn%;vU6Ov`d9by=2y)OAK6gQmFFX$} zG)Mh_=a=^Ec?@33nOTDz-vl}5b|=VjJOM7x!=xS0!=xS0WBKKzcRUXnJR!z06a!;S z#?j=rN6%vmIXIrj$x5%D#|K0?p2w*~iK_s983V*&kdipZiSAr4i%9BunEDyd<0+Fz zJdZqwoeb*|q`6V%G06WAC=;H?D(3stP{)0o^<;i5J%wGww;+?VKqBBF!7C^w+4J}} z4tU&ZRPaxF9t`e@X1_+C4k}D|9tG4fAjb13LK02o_B`^W6a_a z5u9Oz<#Y4rpY%M&YbpPGp2rLAqDO>4P%GVSSqIQ?p2sqhe&Kn1PCO49pl)x7slyJ< z*$ehN*lc2-1siU_+IuT#S?s8wpEx@*vJZo?-L>Tnm2YMqS zZNqJ^i1nIq2P(|n7L9HU6Zl}v!?yT;yX}?!R#?EEJ&(>d^l{^P+zWEf?T+WMgaDW4 zVbYH0VbYH05fpmI^N^7fL-gGg14DGiBI04LH~~HaAQ)Hj4$H-)0Gx-t!ue~IUOkV? znQ%OhM>J);2>hyd!t+?}&V?VNW)&pnVd`h{oNMxk=kW(7?3f3(<+(j{qs(KF|12OA zp2z7*`e`u__CA>(&pr{k;dxZBJ0?7jkoL~qJddUSK+HozId8!9VGF zOt&>VPz;U2t`eR{yHTp~Jmw&o)whe56&C;E9?0+Z2PI$G6B2o=57l9iNZ`JP$qFafZ^X=aI&Q<9VE|DdRUo&@nLMwUJmPtr;IMPWJY*h&{Lg?g;d!iMzE6*NaMlyg|B@Yn3P2ls$36I{KAdGO(J#nrSH0H1>i*%nZy<-h?L8SL6sP5oUu-o(` zC~EK)5L4eu92$LNh7@B?le*4xP;Zbs`RHh$5jWM*QE7G5#5p=D5{U>j896#C=Q-GX z1(X);%rm68+^oSXo-%|5g}JN`zX7$6d>$u8Z?;d2;uK~cF?b;)E+IU4qg?ppCpJfO z=q!kX%HyM;6IICp(``(e6V5rx9#Mbhqm zp42JToFN5~`^czsloT93<@hLDPU9O}STQi#Exuu_EM`r}x2X&Cc6YnzTCVo8y%>WxTEiu3zB2wFDNMVvs!RYWd z5@hCsXP{@4gRNj2Fi*u&&3+;iheGY32Qu<}sOxAcOOaHp;YKA?593XDt~e7WwqfdL@?355h==hw6KrW6u{E}#&Ey-^2BfGF zl?dlHDe0%hHrQt3Ve}ru#`7)cS8dqmIm`(3f6Bu!D)=Wo3=VOk+3qoFR|yZp3=8(I zO3nyrRkw#>MmLv-v3a_*Pbv?C_A0~Ds7FZ%3*fjXJ1F8^U_ILo=cid0UBaW6pq|C6Uv zP)!C>v1=k$%IQ-26*R($kx$>CGWI44?st1Yg?$+F)<>M<%+uKmId_7?TIOXR8`h&p z&~GvHa>!^MGd)djfsfMQAqyF5Ky3UV+slyGb=VFD3X3J4O-XhufK72;^idQ6+w^lz z*n=lkPp@ohXcBCZJN`8*3x zRm|6iZWA8FQ1@e~JApjPmM#dw1YCE}1=*8-aoZ2jWPA26$|}iC{fjF>&Kcaj(=g&j(>50&^!KxF}_i46odXnjCdGl>IlYnNCAy&1Y@q!tABA46Oc~Ni{79q z<12hq3Dv*2)}8ApCX9b!>SyvCVe*K7(ZU3pM*Iu=yr|4pSig<}Qq+h_gmb-0`f2_J z=Qf!IPx=~i!@tOrX~l_P6fg;;B%j*&ciuZOD)=Y;i}AK*-#JQEnD8$uj8aVmV>*&( zRkwc;B*l;j#`Pg-pH%(@+h|lPY0&r@!I)`M|3n0%PQ~~XAEQXC^FJ5CSRv0#{Ug2y zYduT^qeU2f)(A$3^+2yq@;g#TFutbGb^S{t7(v_BE92b$N-7) z`AZao9>ziB2M;541Y$AAIUdGwM2SlPei`eCLu1PxNx+?}S47f1 z5>r3pVcc)>h=<{G*h$P;BN#GYLH<93GT~v|#eAQ>N5a`nJd8Iu4ERza6Uwsg^ zU1tyDVX(V~S4*t+Xk8+qFSm=o3ZF!th{a1brroS>VS+C4+Ci$6@9Tk1o`yht-wmEe zM2brXdCc@YJbql<*AfyzO4 zzKLXxls64;Za~Zz!qPeCL$P@Op}e%m%V*K?2!=O%RtHtwRq7KpkNJig8J^9QO6hyI zEBq+7953&Iei-5{k#};080=>C_$E$z5Jy#r9+uNM8}vOJ=H(l}>PtAOQ^u5iqtijm zKo^h2JjNVw_AzN^YWpq?bg+C#{x-BEL3^%Zu_3S!HM8@faMr;W%mb5mCBIqJaX&|* z?ed~(8NEVVSU5L*H5R^%Tpne~40z*(r5Nv5;0|$mhIvD)fuD*7g9-Ie7AbuZ^$Hp~ zUl48!HPS;K5+Y#;^s0{tlXrCJ+d`0>D(_Y({K}b{IoHn{hHRE7JJ|wg<_-W(r9c+G z5dyCWLy5EPMXfL6HJsn&{>Pgv?oCoK=#O{kh+qumiCQSNRXfRhl?8+^|9*Xvi=H2d z<&Dcb!C1f^vNc5;uS1X z4t>7QcmlU=pkCP$U_OjTgA_wvBhycY+=GBc^pPdrS4JWv*O1b@U}K@t7vBRfF&^{! zj5`QtDd!4sn3vyCF7*j3EP z7v$lEs37j=m%IaqbT}j`F(Hb`oiaf1Ha}>7kyaQz8#SQaGaE+|HFD*Ia?FdVz+GRz zp#yn^$$;pHjOI0bL%6(z<3(`CuBMiU7aWA_w?oRyWU&pX7>vLOt4I%{MKDi}VsIo- zHg$8Cl~HEoXD>rJ{YdcrJdJP}M!tg&W`n%j)EKc?|2fDpMQ}P9eGBg~FfqJAs@z`~ z0A9bK6gLZXnBIpFnDiTV`b5E3a-tbM5BDg4y*K+vBy;0Lp`+?PBksb5FDV74s3f7H z!bXd%=e-DpETivJj1(R*m{-Yc)ZMN;Q{a zz?a!CVHg935gZT3iFa-|!Hfp=FfbuaGw&AzvGFj>!>@7>lpY3cRmXa~aSt2u5TS<_ zP7ech%i~TY{V+B+w%(10fmgx8A^;uo&J9EeVb*@N6*9-`UBt%a4Th^=JbWQr9*!W$ zyOb(dBPC7+d^Wxq9~QVk=%*oY>VvVt^gF-}YGdUS?geT_Tg0nB=15^f-?@RvT_v0f z^p+eHxdtD+DL?zPBGKk9KeQ>G*Y+zyU|ms%uigEM3UG1z73)rw_WW#q#hoCgCjUqL z3Jyi;S6EVGzKzIN7er0;7clW&7Bo-iIJui+ zv5hrCri>pUQ)My!;Xa!1_!96A6Ic7<7=A?B;5IWT7IZHkfcYTjhQmw5UwKH~WGh z()q_}nqs&_NVpmhW~5{AiBHM1{O`($KHW z?g;Lp7p*hB@%WLe%|{i(qTzVlv|==S6%FWSyMj75Q()s**C z@x5gh%FKBCdm@B9vAP}r6FI>Y4x5uDoovy}*RTX^?IF(3^kt{=40|@BKM+e{{G|ZY z{xR@gg{P%>K2BfE<;}*6^3%k8;VMB6E*-Py>lrbq0_%4|`3B`Y#!!6u=c-sdc872h zzTgXi2~_}{XwjAU%?rq~p1d?vL&6scW3U3E$;btE2x1{4u|kYDyh9k4GkQm%D&{#> zh>-wrT12G&d;mytEkh!QGT0(h`72Nmm!~&HDAvrI_mJk6=0#geEC_Gch15MLoFgJ% zbUc+yNH{XAaJGqya5;=HM+eBn<@hT%PC%tDhc+pK1g8D@qc}h&!Wvk}Vr4H44yh!j z!K1{GkAzs`O8h~V*XFdREX$uEmn+1A<*)}v6mvqcAA&IFMraI+M3C`Cl>o3MaF!7O z%2mz;7dri~IyvX#zk$WvfsIO=K|7bi170!^c7@#1cboHR(hDwM@?B!Mx@mpP7c zhpF|06vr5S(3Ht2BC<<8(CFZ@MY+feI&o$c4X}{3-f$uaiB~k6>?OLdQ7I=V^!P_P zLsV#@W-_HsFt!M3IoxCMgtHRj8kkTaZOb?M;P0RWkgunN%P7Ab%;4`u0Lq^l0M$4T zseoHU-7++YKE24)Z8;Gp&f!zx@aE}xMogAM{gD_YFj6TI&$;^RF5d7%%r2@i!wz1E zD0)Dy9^Oc?Qep*pA}Il(QHv)a1sRv~3u`u1axIj`rh|FV7&1xkMVm1+(m`JIX&?Yb z6QFYxEz$99K$$@?<37W+s8!Iin=VRjJ<(={#_5UCdj^W@l?j7E< z;*$M(rjQdqZxhaXbqH!UQz(q(NGPuGCxD1}Y=Zo`im!YSa{g!`wGXb_sD5}!c^S78 z4Y!2iFrXWHi>1B~uj5^Oume%K(wqH58kwxW&&dV@OnW_KG$f#bw~4M1pguUzE5qF} zY+JP4ow|F1JH#UrM6fq4VDR$m~kq!X7G+iDc#@q5S z70~4Ht3adyPlP-WvB+b)^9K?bUg(2BsMAR93!s?kY7>kppyT+Z&@e*{m`+4=#Oig5 z4`b5CM27@l;-rxdu!PF+dVln0AEF_s)B@y!c6uZ;ST+kwhCdKaXNhwZfEWT?e#nVz zc;%kA5?d(f)Hz6D$P1xX**~GI_$9#ttV0sNB)G!$i*eq~T1?nY} zQ@D3v5q>;{zv~9%9_d!f52k-Q-HA_rHhzD4=(Ev%#SeTy5&T1wzF_}=5HjVTj;EW5XS?wa8sI(^gF|>BEq|x2PQloCpcMda2c)nc zgW+x`{(kRgqdV;G&qjCU9gd#3slgu8ZnV{a(G?p*94oMy zV!J-Nbt8@zT?ZM8#5EBe$hZJ2Cd3{S(fcsK!E`WC18k|XFcrY|fOe%2e4Q$o_6WV1c#v};Gob(VL2TJ%nr$}c&^-OhjK4}C zZ%do6^D5rv4EFX6Bq#l_tDOhIw7=jUO%Gv~sF3QyQa5 zQ-9a~E|vZ`0@09F-pf0)c|2~$kE3fAjIH^lr}y)F%oK4!`ohu zv@)7|i`=N~JzQXPr{)XW<9<3#)tDX(4n?uEXmM)YaK?3O7}mF5*}1H5jY2-7^@(M3 z(&@p#ld!(a@C&%|`>(^;-{(hvbC3JH>Tms;)cX6JjemS-gR4GT)NsvOas~ur zC6VwyvMUi`+INQRnGy^VfG!%>36OO1)AHW3-22`y8itdpT<%wYopppPoZ^T6|p&uX*5Av58 z_)|iVS;DiZh&=zJ<0%ju2C_G&vcJz)|DAL6=d=HQ{3p?${^XJQwWeuv&F(6iwHYR4 zefJ#wu|_nh?&o4n<>$)@4)Po{P( zwYXn-bD%>YuBkm3N2~{=T#d&E zx~Oz)4-z~Z2WP7ijWWsGxP|w*>v5_ssdWa*{*Q4V{Cn*4L+pObiK^e5jg9~AyXm?s|OB$+F561b$p$AEeV9!5WK124eBYOJhXd< zBz;-eNdq9tg*5RxRP$HH3k?exo3hBXZ`$pfBKyYA8~G{nfh%S~X73RtVmGdJJA(eQ zzW3(IZ+`U-+_j0~f=(3vDR#0bSM7x6%0DpMvz@RRXbnT{g#CqoyW0s{Kb4*I0LSQ; z+KCtiVks^=;efG6I}uA0+c7@7j^xQ#9p&TbfNctqZu)QJl2P{ut0LaXetKoyUuP`uz&}FZ(YatkkuR~JNkDH zA$Cjgex8F3uVK15kw*7GSgQl&5YdAx|9Ee=74JhP1MfSW%v?JAvTl+mtRU%JEE0B4 zeoOWmn&w!$!i>Ab&l*g>Bogc%l^5`M;)B8J^uGnu`o1^V&$w8yJEQzaaI@t{dO}kA zb?C;8KOzagjCkn4kq12f4A`u}wAkUe|GnIgh#k~9h}7#}^?3d!&EqXyzG9wRzMDZf zjC^a!-z8si`qCtPJSknzbdr2+fZ6he<=&R>LZp)93w931_U_$>S`T)9h}*sQlH12| zJ3^|`xfZu#QnZCcYu8HsOn#1mCO-#CM1oV~cLrU3^aQu2 z%Xkl9Mo@v=yTgWNq9x1Qq-s&Lo^YF|tjCuYu~!mY*slj5 zK6{OGqSQ8iC={B7oG?kamuQc0#*V|;9#a2y*cz1Y1NaJ51=*+xM#ao;cYA%~g(3%> zX)TL|F*naoX0Ol71$HelTnl(mDTea8ln9Rzf4X-Jw0rp(t+(dTy9 z*>hre^k~1Zm0!yY7k*h&Cx8+9je4x`Am>QgJoAzV6AzEx1})+q(-P)M$f=F|Hg(Ha zdde@Ahey9EX}q9HDCOsoVKU*u#|<30wXRnL##_0lNU&=!z*sn%8r39odWJo?RUy!_ zxVI(qX*FVJYKuZ8WJtH1hmeeFrNz+3Z-Xd2s&O=A1gHmJe8G;kq_hSao#%k@=4tS`{qXj0DbXnraR>ft1JNK^L$Kr|u7E-FeW0wPf8jCvN2M2IYiO3Q#_jtcnR|HHsuu z4NhBgK7dHj0_0%TxQRg_mr(`CPX)=6M^=SlDuZ<$H(t5ebv4SNezUT)sw7@pxR1Xg28Z~R$<`qP$vGA5FJk0KgiUh(QrVm23oz zf~0~LRn51Iob?Gi=ekr>=qJrI%$ zX|B>hVR=w7ZCRysn)VgRhMv$NH(XL9i>^d)BO{CQL`wP&w$xGE4>*^Sy^7xP~o!`ltXN$we?ITjp-Q@qmY@IG)KPr5n&r ze1{E=P~z(a)fn#N7zrewwh7tg9EN<+_+VA+`f4%28biqr7C=69SL^ogv2_ncAsh-i zpZCuG8Dy3oY~5QVeOKhbK-V7Zomd2<2U%;xSC)}=8pk$SKA71O-57GSJ5ED`qSf=j zy_GQi!S6mG;6>pCnGfIn&@^VtX{aX;ilBke#k0?aT5$^#$UZj&45p3j)5qidh=uaa z`H^Y%x+s6)mpBPb$&`QqCB&aIJ0Pb$Rr&INGm_#%q3u}zvt#M~I`qY-uQ;ncOhlf|*RK?Kt`MiKh$$jqU?GlFIPn9>Un6rvOspKcBOu<0 z3crmH$MN^@;V3GO1x*O|eEi9qeOO>%+Ad**xBz1S_#nWCVXXtjARA++bgFxh97x;2 z$Qi)C+)NzvMRcb;zF2zYr1<;gaV+QfRfi0^sH+L&4E+3K<{>xf#F)5P3_C4`S!$8P z2LA}kDh6BDvW^hm_;|yp*(#uoGr0ohYF@GmTJdyc!G$xF ztPt8eTzjm>Hb4GIIqezxew?17B$13l01$G*DaL9<7PgMO#}vGa8!CuWG2jtSVOD z)EKF1Xs)Y@%&c!57TyxAX{#n(61J^18aeySiJnX9V|9_%riM1*)f#PVjau@C#8ChGShQs~V64ei(gju3k(MZG>}iNz z9Bn*5+AKp509%gBcRq0LiB1zXbnw#pT zna!Rk)J~!5rLd|PByQKbUew%LjplEh5otSLpjGF$MxymIz~4sj8jUoae?b&&e=$oQ zim7gDY>l;4)i=gkJ&g^`tiMXsf{M?$)D3eCC{kNh4gI)rtyQ(r*rmfDR2!(Ci#cJa zU=lobB4dL&`;qEuDPMAG5>pK zz+{>mA{W<0BehK}muL#@8>ww;OrWZ2F7RC160P-2^_%|A*8CM zx-P<@r?suNw!XSPiuS09wpQahe7Rhc8_th#IKco>8=28kRm1*^@@#uV9}qijYeq9iBeD9K zP$P!b*6;XdKn0C{44W9*{7tof;{1a1=N0;MYmV~Q)YsOcC;4kznr8ZAby0tHTT2VH z-f*eEN{8WS&7K&1b-#*+XjN;}55sJUqI^H~@5c}f)~x`=%yi^O=kzzXG+kU@6ZKaK z!NnManjE><7iTuNe$_S<>0{MZ&8=+>(fscX)7gwdchiH)5fM8WD#QO*-qi=kRbBU| z58J|49t$x=0h>IGO=W79^+$~WW34R7mTlQyjG$|9x{{@pv{+wgS4E~KY{tf;1_KHo zsgu%H43ydvCRQn(ip0bkV=@j9qZ&7u!hk|uA{%#RNu0){PPV^u?>TSZd)jA`>7ThX z>)(67d+)jDo_p?jci-MC-%QOq3}hve*7`f@>#Z$)Db@L$r4TZ@Q{=#i4DnvLf6~`t zzf+|?gqSv~8PULMi+6Fry4qsn{XTY??jCMLG7dtB-7T^8UCJ^(~@*)|uYQ3D*-juXlb*WH-?464fI>b=4w#ZdU!&En##Z$Lq z0fvQ!8Ge^QU%S-crQ+x_Jdizz;?zZm{pxY5F5>rIq>vsh>ViV)MPxvPF+x>EEt_D2 zi-@u7^EE2+l@h|&>bW$%rLVJdo851y)ReNCo4Twmm{iai^9YJ=iKp(1V@|T!DTDfD z>B;M~CX5NsiuWc^#-adj#U(Uiaza$Kw$&v6N678dPYW#l^27 z+E>}C(#Mt8SI_$7qor3=KJ}ApCuRgMS^mg>|I5&?emIOn!<1%*5B$}Mzkc!9)6359 z`}EklEs;vi8)1d_oC|AtFIRs5m3Q9#aQl0=KGm}Arw{(-A8ys<=7y)@f<8KBq`tbP zZ|jBl(5?si5|7vGs_ZnNO5@Q{#&9BJBo-QpMMk$BVhyTKA4(*{d4G}TKC9D%RMwHw za{b|}(_i{`>zT?USN-^gZ$5u<<$lKBymr(6kLNy9x$OSH&!)fgzz@PJzWVss{J_ED zzs#O$Xt;m*-(GRu-l@AvE5G~G_e1+X{&;!$-`Bi;>s$Sm*EIj}^UI&@KUfrw-ZAfc z?FaAW`b)XqN|y65*Z=+#?+xv1ex`EXk)x&K`~U9XeT>K6JvQ$rzxY+CVuA~Z}BVfGx1}wgZPQ~jrgVbsn|*8h4_j1jrgVb zsmueJ7wiY4>=)t>l?7K8-EtC>{%+VZdNsxw2CkR~UjWtt!@v{sv)L%HY*9A57g!EF zK>5Jq#4pZfmmpyDFUe-pz`aY64;%rG(LH`Bh#;5@;HRQt;1S@Pz`8Zr>@ozSC@=yX z0JZ=p)@HMZfpzuS>^QI=c%Jfs0R*GHz`4L9zzX1=b*Kj%*^teS0?z?Y081j->=|Gg z(7>z2a^Os01+W}g2V4e>0wch7U<Fz$HUQg!?ZAHEcHjVT5V!~U9PkKm1ULbF6Id2Se}NUiv%r2}2^MA}Knr*T zxCB_T3BN=D%Yd7K<=FZA9Iyg70;~hR35){Af$hMvzp z3OoYr09O15#uL~Md=hv9xEEN0cyRz&2OI+qPz(X?0iFk*00uCTDXz=~o;VG=(mk*r zcn-K5Sn^3WI|M8Pz62}>9syPW-v`zKGr%aY_zTbjSO)9|E&v_@hJh2n24MLa*c(^} z+zuQ74gx9RhaHs)?%!w>^al#AES**|h@deSEPlN=B3Hpy@MXgctTfBkTs-5xlC)8I z#m(0(o;!ysQvDiyO6H(%L~tU0GhKrpqxzicf%GM+|JrPJF?_~YVFn&6Ty2&;TC~En z9x1Lh%Xd$yGDEwjR+$w$rmZqFg+<4vO*bpRRhi`|v%)MxofT%uYM9iZHV@;|9?WJ< zASG)11U@a`=-Q|=18dC^rB4R89pK*}ekHXHd5;#=Qrj!sZP%FL!d|poJ*nl|8O>(J z%1Lw`L`Gn{_aT2X1>T%|)wewI-^kmusu@NFsy_`GnH#d%IkfnItVc-K-9=TVwW}Dm z*fC|5*;80E-7E)FC041Op=@#jbx$tLW@)WLX*)h=z>R}DjXCtWx}^(>W=}N-3hKNw$d; z)|%0^X5;N-e2n>B)6Ei?(&)hL1oF-zZ#(&1 zZd~$>`zmv+;6e0b@&K=ykuV1f3#mX+X?~|%gZ9WL>A6shoiH*d2s>yVfSU&})|mma zNgCWNaCh7Oav_^MU`E%NjfLM;)>xx%U>{g!Q$9B(o6y{yg-uPjQ=70!f^9;1=aJWl zJRJF?yt0UC-HBdCR8H*7l7*lr{)yyi3@|M7HGmrccL>*H>)nO$ z&Rs<{rnRHE@Sn`GYBhafBV^NaaRD}2U54>-j7z@xvdSE>>dlcbK8^KeCSLS-f#2-W zG2ng3&S3Le1?hplccXKp^I7m^*ziZLK#8~#Oq7}6XlhU*ZWg$JhYNu#^>8)dN<3T? zT(O7i0B3l(G`RE7(b?{k;Ld^L5N%M*+zak3xF$UB9d`2gZwaVjLt(`)iaHk_OoxTj3ceVQ|hd z8wU5Lr`%z1Fsr?l>48udFqaF5@U;MzUhUT`hooa1)@T%)Jl z7`V-zawow>J={5P4IZu(0~Z1396wrL*LyfxYuAB;2$g88Xx&`{j$#!h;%E(?2IriI zJ>YhOBim7;ay!9|c*^YocNm%@=zGzV z{!ePAHG`Wf5!K=o|^uH|F@m}!)u5T`(2nZ!q~{z z!rZIk_lf`Qa<^vojC6}fxm6!M_JXJ3+zGRfJ-dcOZs8;+; zL*kv3zXum2N8+*Yf1%2M(UABl{8PBdr~iKNBxi{ARPRE-jp0*(r=m4oYeITc`WPSL z4d!vWOQrMpH=q2=P+iHHq2);Yzk&IHhhNQn$ip{-&nN!@E^mDS7f2$1kI3=x|IB=* z)Ls54^8ru!Q_LG4z8C>HpFXp}Q-AwC1izSlPjDCe<}QP{LeKZ&&v`#@Q62lt$BIQU(0-Cljha?3|!vId?v1W zc`kp2`N1~L%R2jh<^wU!OS^j%uQ}(x-{JCU)>Eea>&!=ZeIw!NjE|gRyc3{)rE9fx z^^Odea}@7vQD0a3@SA-2gb%;NhkwzB|DobdBgFO>`%Ew&iD>=8tnv&D5cMm=gshw9 zgSWVh6Dm}BC!?>|vK*sT-{V%LdziO8<2B$T=UYDfcUVrqBmYM}<^R)%KhJW;PHFvQ z+`ohcDA{2wu6gmpn-s4(=f8Jx`SH&*3$ZF~V?N;VtEYVA?B()9EJxb?kx%(gefX>o ze>ntFzk2T0`pNj#F&~)Fyy%}$yyl$$?!%P6)JgDl+Nb;zEI-_;3(9(anE9d8niv0k z-AB#`KKyC$7VF@IsaVKUf9Y@IQ+h*#F<YZDxLl)nuIzv%;4R0!T&`z1 z#wpF*z!}}lhq+ym|0werk3YYlc#)dJLtH+>^IE-kME1MP53+oDU-OxdoG-(Gb8=$s ztDC`-9YUVC5mmfh%)WSt%ZJ-ECG+|pnIC3*iXC1T`5rr*7CF^gj{0o{mzUxTDCv{o zaTkAB%6yphl<&qC&Uo>&~Mvp%9dm+gm^vECbk@Jd_=XkDuw?HeWm=B!RO^O{# zu>d3aJ=|Y(Q>E*eAHGZ9-_HDM<^voz#BSZpM?CRlm*Tmy1OA4~8{96Ytx5-&A7(w( z`!VFa?IWiEh9&(YTwd%voB8p$mMi{zEAvCAG%xyZQM~4y|MqkF5SQP|nNRzaf6<5k zG0SQ6*!eW`W1jWWG!<7dF15Nr(SIrPjojaxSf7o|8y-LJVm`!vCFA>b=0~~R5|;D4 z;`zn_FZ=MvSx%V!v!3hxLgahm#uqQv<7IJq(I+Il#~*G}yh9PbZ0GXBEJxl8KI~I| zAD16v`ylM7beQ>!$NndoH#~8s7+e5VSNv?tMnrCV{Ct!U&ojq_V7Oke+k!h!W59y-@qo#qL@mvnIGi&eu(+S%#X94 z)6zO)t>U?m17cF1pUdj^Z{(+$AC2q6qW?2ep8Z+k?Q6{UY|!OD3u>DWfu-Gk#2u~=kdZ9~JR*vdQBS2wKQus#+GF3ih@9DBFbi2*(%07K+j zac0Dl3w?7I`Q}V&Mh+%SZbx$RG?dHol^07c@hXw4HA%l**5Z7%a6BTve#r1qPvzv* zPUYm)K;?!J_d4RHDa z4%X_(<+|9B9G_H)9Vph^9&g_2Bb1J$#FB|N9Fy^$93XvYgT38{8mXo(m-d+Q>VQux%(K~T6^PhG_KA* zz|Gn^ancE!)N%eUW^0kRzAI|<;$#%I$WIn8<4v}M5fbjv7R$oy`8z4zruN{sLcgYD zG7jD7M3QY^@|zf)lIu!MHhdndDfc9oaj5I+4IPh*!8vUMc$_JQqe!q(o3p>JF_~-* zSGH=K4i|JcyE>6aSMqLE(5;X1T^tGMY)WC*K29DW8t@so#QiysReIaKu>F_MY~`AB zO$mRoxe9i7lr5DHIqii*AFYWtI#!kH?cQc9fN=3SvvBWThCzM*LgLS;^S?(U>hOtyGBtYB-OtLJi(yz|ag0*6d-0MbA1;@oJSIjc_y z(m9c&ugVxAc1e8_$5GHdGM}#`2Y(tumoF+yevTd8ZC$9l%1w9@d+bRMX&PP5m5{(L zbiuIT+ij%^jBCeOJL<~w0B21xz$$Vgb~N`l(bJa>G`p>k)0KTj;+Z z5-8O`t22iAI8UFd1rxp-0U^K3>x&sLP;g7NBNB@?M8*9y}RY?KuAV`a$BzclU{( zf6^laG?|)oVA8WdlTz%g$}Oe15wx17C`RF#zQQl>(Nl`;aHm+!Q{5iRMQq zJlNLN7sTo79(;e1-|O(NEt!(_dSBR47PRSmM)U0%1N9xPIMfyDlTN>>8vfolr_E(~aN+iRX=(Q>Bbrk(&f1}_ZaRt#|$_cikKJA^9 zd)YrJ*vO5M{!~WvXWF<>OMU78*SP))7AX5a1!cb|)u(sN-ue%N zqb^RxN2QP($-kUQ^bl?rE}Zo(t}i%FgX2i@-d*q;$aB^=mT+N4DJLT2UhvzV`T?oO z=sbHzGNhj1v!430pHA@ESE*JmNj+iq;lioE>|YX;-#4VZ*M9rCzKp+?Mx{wA&N9jm zA?nUBEJlHU#RZj=`m(P|koGq@lehiXkm;;He2H#Xa8!QSVj~~68AsZmNJXT69c>Uq zs{69W5p|dQN9qe^kmamz?^CmMeS2V7gwzwHpRZi?c^{@RN7t8fqJj55iwxpKf7!=b z;!~fs5^+W`2;xFEqc5cXpLlqw??jJ_sTvjBewqzU`{l32NA#EYMZaH)?y4gHGs1R= edw#@{cBL$S{>oc_cC9YS+Zd&|>Z^T&i~j@R_x?Np literal 0 HcmV?d00001 diff --git a/control/velocity_controller/src/build_auv_solver/main_auv_model.c b/control/velocity_controller/src/build_auv_solver/main_auv_model.c new file mode 100644 index 000000000..800de47b4 --- /dev/null +++ b/control/velocity_controller/src/build_auv_solver/main_auv_model.c @@ -0,0 +1,189 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * 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. + * + * 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 HOLDER 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.; + */ + + +// standard +#include +#include +// acados +#include "acados/utils/print.h" +#include "acados/utils/math.h" +#include "acados_c/ocp_nlp_interface.h" +#include "acados_c/external_function_interface.h" +#include "acados_solver_auv_model.h" + +// blasfeo +#include "blasfeo_d_aux_ext_dep.h" + +#define NX AUV_MODEL_NX +#define NP AUV_MODEL_NP +#define NU AUV_MODEL_NU +#define NBX0 AUV_MODEL_NBX0 +#define NP_GLOBAL AUV_MODEL_NP_GLOBAL + + +int main() +{ + + auv_model_solver_capsule *acados_ocp_capsule = auv_model_acados_create_capsule(); + // there is an opportunity to change the number of shooting intervals in C without new code generation + int N = AUV_MODEL_N; + // allocate the array and fill it accordingly + double* new_time_steps = NULL; + int status = auv_model_acados_create_with_discretization(acados_ocp_capsule, N, new_time_steps); + + if (status) + { + printf("auv_model_acados_create() returned status %d. Exiting.\n", status); + exit(1); + } + + ocp_nlp_config *nlp_config = auv_model_acados_get_nlp_config(acados_ocp_capsule); + ocp_nlp_dims *nlp_dims = auv_model_acados_get_nlp_dims(acados_ocp_capsule); + ocp_nlp_in *nlp_in = auv_model_acados_get_nlp_in(acados_ocp_capsule); + ocp_nlp_out *nlp_out = auv_model_acados_get_nlp_out(acados_ocp_capsule); + ocp_nlp_solver *nlp_solver = auv_model_acados_get_nlp_solver(acados_ocp_capsule); + void *nlp_opts = auv_model_acados_get_nlp_opts(acados_ocp_capsule); + // initial condition + double lbx0[NBX0]; + double ubx0[NBX0]; + lbx0[0] = 0; + ubx0[0] = 0; + lbx0[1] = 0; + ubx0[1] = 0; + lbx0[2] = 0; + ubx0[2] = 0; + lbx0[3] = 0; + ubx0[3] = 0; + lbx0[4] = 0; + ubx0[4] = 0; + lbx0[5] = 0; + ubx0[5] = 0; + lbx0[6] = 0; + ubx0[6] = 0; + lbx0[7] = 0; + ubx0[7] = 0; + lbx0[8] = 0; + ubx0[8] = 0; + + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "lbx", lbx0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "ubx", ubx0); + + // initialization for state values + double x_init[NX]; + x_init[0] = 0.0; + x_init[1] = 0.0; + x_init[2] = 0.0; + x_init[3] = 0.0; + x_init[4] = 0.0; + x_init[5] = 0.0; + x_init[6] = 0.0; + x_init[7] = 0.0; + x_init[8] = 0.0; + + // initial value for control input + double u0[NU]; + u0[0] = 0.0; + u0[1] = 0.0; + u0[2] = 0.0; + + // prepare evaluation + int NTIMINGS = 1; + double min_time = 1e12; + double kkt_norm_inf; + double elapsed_time; + int sqp_iter; + + double xtraj[NX * (N+1)]; + double utraj[NU * N]; + + // solve ocp in loop + for (int ii = 0; ii < NTIMINGS; ii++) + { + // initialize solution + for (int i = 0; i < N; i++) + { + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, i, "x", x_init); + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, i, "u", u0); + } + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, N, "x", x_init); + status = auv_model_acados_solve(acados_ocp_capsule); + ocp_nlp_get(nlp_solver, "time_tot", &elapsed_time); + min_time = MIN(elapsed_time, min_time); + } + + /* print solution and statistics */ + for (int ii = 0; ii <= nlp_dims->N; ii++) + ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "x", &xtraj[ii*NX]); + for (int ii = 0; ii < nlp_dims->N; ii++) + ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "u", &utraj[ii*NU]); + + printf("\n--- xtraj ---\n"); + d_print_exp_tran_mat( NX, N+1, xtraj, NX); + printf("\n--- utraj ---\n"); + d_print_exp_tran_mat( NU, N, utraj, NU ); + // ocp_nlp_out_print(nlp_solver->dims, nlp_out); + + printf("\nsolved ocp %d times, solution printed above\n\n", NTIMINGS); + + if (status == ACADOS_SUCCESS) + { + printf("auv_model_acados_solve(): SUCCESS!\n"); + } + else + { + printf("auv_model_acados_solve() failed with status %d.\n", status); + } + + // get solution + ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "kkt_norm_inf", &kkt_norm_inf); + ocp_nlp_get(nlp_solver, "sqp_iter", &sqp_iter); + + auv_model_acados_print_stats(acados_ocp_capsule); + + printf("\nSolver info:\n"); + printf(" SQP iterations %2d\n minimum time for %d solve %f [ms]\n KKT %e\n", + sqp_iter, NTIMINGS, min_time*1000, kkt_norm_inf); + + + + // free solver + status = auv_model_acados_free(acados_ocp_capsule); + if (status) { + printf("auv_model_acados_free() returned status %d. \n", status); + } + // free solver capsule + status = auv_model_acados_free_capsule(acados_ocp_capsule); + if (status) { + printf("auv_model_acados_free_capsule() returned status %d. \n", status); + } + + return status; +} diff --git a/control/velocity_controller/src/build_auv_solver/main_sim_auv_model.c b/control/velocity_controller/src/build_auv_solver/main_sim_auv_model.c new file mode 100644 index 000000000..3b036370b --- /dev/null +++ b/control/velocity_controller/src/build_auv_solver/main_sim_auv_model.c @@ -0,0 +1,141 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * 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. + * + * 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 HOLDER 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.; + */ + + +// standard +#include +#include +// acados +#include "acados/utils/print.h" +#include "acados/utils/math.h" +#include "acados_c/sim_interface.h" +#include "acados_sim_solver_auv_model.h" + +#define NX AUV_MODEL_NX +#define NZ AUV_MODEL_NZ +#define NU AUV_MODEL_NU +#define NP AUV_MODEL_NP + + +int main() +{ + int status = 0; + auv_model_sim_solver_capsule *capsule = auv_model_acados_sim_solver_create_capsule(); + status = auv_model_acados_sim_create(capsule); + + if (status) + { + printf("acados_create() returned status %d. Exiting.\n", status); + exit(1); + } + + sim_config *acados_sim_config = auv_model_acados_get_sim_config(capsule); + sim_in *acados_sim_in = auv_model_acados_get_sim_in(capsule); + sim_out *acados_sim_out = auv_model_acados_get_sim_out(capsule); + void *acados_sim_dims = auv_model_acados_get_sim_dims(capsule); + + // initial condition + double x_current[NX]; + x_current[0] = 0.0; + x_current[1] = 0.0; + x_current[2] = 0.0; + x_current[3] = 0.0; + x_current[4] = 0.0; + x_current[5] = 0.0; + x_current[6] = 0.0; + x_current[7] = 0.0; + x_current[8] = 0.0; + + + x_current[0] = 0; + x_current[1] = 0; + x_current[2] = 0; + x_current[3] = 0; + x_current[4] = 0; + x_current[5] = 0; + x_current[6] = 0; + x_current[7] = 0; + x_current[8] = 0; + + + + + // initial value for control input + double u0[NU]; + u0[0] = 0.0; + u0[1] = 0.0; + u0[2] = 0.0; + + + + + int n_sim_steps = 3; + // solve ocp in loop + for (int ii = 0; ii < n_sim_steps; ii++) + { + // set inputs + sim_in_set(acados_sim_config, acados_sim_dims, + acados_sim_in, "x", x_current); + sim_in_set(acados_sim_config, acados_sim_dims, + acados_sim_in, "u", u0); + + // solve + status = auv_model_acados_sim_solve(capsule); + if (status != ACADOS_SUCCESS) + { + printf("acados_solve() failed with status %d.\n", status); + } + + // get outputs + sim_out_get(acados_sim_config, acados_sim_dims, + acados_sim_out, "x", x_current); + + + + // print solution + printf("\nx_current, %d\n", ii); + for (int jj = 0; jj < NX; jj++) + { + printf("%e\n", x_current[jj]); + } + } + + printf("\nPerformed %d simulation steps with acados integrator successfully.\n\n", n_sim_steps); + + // free solver + status = auv_model_acados_sim_free(capsule); + if (status) { + printf("auv_model_acados_sim_free() returned status %d. \n", status); + } + + auv_model_acados_sim_solver_free_capsule(capsule); + + return status; +} diff --git a/control/velocity_controller/src/dynamics.py b/control/velocity_controller/src/dynamics.py new file mode 100644 index 000000000..9dc4f19f1 --- /dev/null +++ b/control/velocity_controller/src/dynamics.py @@ -0,0 +1,168 @@ + +# auv_model.py +from casadi import SX, vertcat, diag, cos, sin, tan, fabs, inv +import numpy as np +import yaml + +def euler_kinematics_T(phi, theta): + """ + Euler ZYX rate mapping: [phi_dot, theta_dot, psi_dot] = T(phi,theta) * [p q r] + Avoid singularity at cos(theta)=0 with a tiny epsilon if needed (handled later in OCP). + """ + T = SX.zeros(3, 3) + T[0, 0] = 1.0 + T[0, 1] = sin(phi) * tan(theta) + T[0, 2] = cos(phi) * tan(theta) + T[1, 0] = 0.0 + T[1, 1] = cos(phi) + T[1, 2] = -sin(phi) + T[2, 0] = 0.0 + T[2, 1] = sin(phi) / cos(theta) + T[2, 2] = cos(phi) / cos(theta) + return T + +def coriolis_rb_diag(m, Ix, Iy, Iz, u, v, w, p, q, r): + """ + Rigid-body Coriolis/centripetal matrix C_RB for diagonal inertia matrices. + Fossen 2011-style, for 6DOF body velocities [u v w p q r]. + """ + C = SX.zeros(6, 6) + # Linear-Linear block (zero) + # Linear-Angular block + C[0, 4] = m * w + C[0, 5] = -m * v + C[1, 3] = -m * w + C[1, 5] = m * u + C[2, 3] = m * v + C[2, 4] = -m * u + # Angular-Linear block + C[3, 1] = m * w + C[3, 2] = -m * v + C[4, 0] = -m * w + C[4, 2] = m * u + C[5, 0] = m * v + C[5, 1] = -m * u + # Angular-Angular block (J*omega x omega) + C[3, 4] = -Iz * r + C[3, 5] = Iy * q + C[4, 3] = Iz * r + C[4, 5] = -Ix * p + C[5, 3] = -Iy * q + C[5, 4] = Ix * p + return C + +def dampening(linear_diag, quad_diag, u, v, w, p, q, r): + """ + Linear + quadratic diagonal damping D(v)*v = (Dl + Dq*|v|) v. + Inputs Dl, Dq are 6-vectors (or lists) for [u v w p q r]. + """ + Dl = SX(linear_diag) + Dq_vec = SX(6, 1) + vel = vertcat(u, v, w, p, q, r) + abs_vel = SX(6, 1) + for i in range(6): + abs_vel[i] = fabs(vel[i]) + Dq = SX(quad_diag) # elementwise times abs_vel when applied + # Effective damping operator when multiplying right by vel: + # (Dl + Dq*|vel|) * vel + return Dl, Dq, abs_vel + +def make_auv_model(mass_inertia_matrix,D_lin,D_quad): + """ + Build symbolic CasADi model for a 9x3 AUV suitable for acados codegen. + + params (dict) expected keys: + - m, Ix, Iy, Iz : scalars + - D_lin: length-6 list/tuple : linear damping diag for [u v w p q r] + - D_quad: length-6 list/tuple : quadratic damping diag + - g_vec: length-6 list/tuple : restoring forces/torques in body (optional; use zeros if unknown) + Returns: + dict with keys: x, u, f_expl_expr, name + """ + # Unpack parameters + m=SX(mass_inertia_matrix) + Ix = mass_inertia_matrix[3][3] + Iy = mass_inertia_matrix[4][4] + Iz = mass_inertia_matrix[5][5] + #D_lin = params.get('D_lin') + #D_quad = params.get('D_quad') + + # States: body velocities and Euler angles + u = SX.sym('u') # surge + v = SX.sym('v') # sway + w = SX.sym('w') # heave + p = SX.sym('p') # roll rate NED + q = SX.sym('q') # pitch rate NED + r = SX.sym('r') # yaw rate NED + phi = SX.sym('phi') #Body + theta = SX.sym('theta') #Body + psi = SX.sym('psi') #Body + + x = vertcat(u, v, w, p, q, r, phi, theta, psi) + + # Inputs: surge force, pitch & yaw moments + Fx = SX.sym('Fx') + My = SX.sym('My') + Mz = SX.sym('Mz') + u_in = vertcat(Fx, My, Mz) + + # Inertia (diagonal for now) + #M = diag(SX([m, m, m, Ix, Iy, Iz])) + + # Coriolis matrix for diagonal inertia + C = coriolis_rb_diag(m[0][0], Ix, Iy, Iz, u, v, w, p, q, r) + + # Damping (linear + quadratic diagonal) + Dl, Dq, abs_vel = dampening(D_lin, D_quad, u, v, w, p, q, r) + + # Generalized input vector tau: map [Fx, My, Mz] to 6x1 wrench + # tau = [Fx, 0, 0, 0, My, Mz]^T + tau = vertcat(Fx, 0.0, 0.0, 0.0, My, Mz) + + + # 6DOF body dynamics: nu_dot = M^{-1} (tau - C*nu - (Dl + Dq*|nu|) nu - g) + nu = vertcat(u, v, w, p, q, r) + D_eff_times_nu = (Dl @ nu) + (Dq @ (abs_vel * nu)) # elementwise: (Dq*|nu|) * nu + rhs_nu = SX(6, 1) + rhs_nu = inv(m) @ (tau - C @ nu - D_eff_times_nu) + + # Kinematics: eta_dot = T(eta) * omega + T = euler_kinematics_T(phi, theta) + eta_dot = T @ vertcat(p, q, r) + + xdot = vertcat(rhs_nu, eta_dot) + + model = { + "name": "auv_model", + "x": x, + #"xdot": xdot, + "u": u_in, + "f_expl_expr": xdot, # explicit ODE f(x,u) + # implicit form (optional in acados): f_impl = xdot - f(x,u) + #"f_impl_expr": xdot - xdot + } + return model + +if __name__ == "__main__": + # Quick smoke test + m=[[6,0,0,0,0,0,], + [0,5,0,0,0,0,], + [0,0,4,0,0,0,], + [0,0,0,3,0,0,], + [0,0,0,0,5,0,], + [0,0,0,0,0,9]] + D_lin=[[6,0,0,0,0,0,], + [0,5,0,0,0,0,], + [0,0,4,0,0,0,], + [0,0,0,3,0,0,], + [0,0,0,0,5,0,], + [0,0,0,0,0,9]] + D_quad=[[6,0,0,0,0,0,], + [0,5,0,0,0,0,], + [0,0,4,0,0,0,], + [0,0,0,3,0,0,], + [0,0,0,0,5,0,], + [0,0,0,0,0,9]] + mdl = make_auv_model(m,D_lin,D_quad) + print("Model:", mdl["name"]) + print("x dim:", mdl["x"].numel(), "u dim:", mdl["u"].numel()) diff --git a/control/velocity_controller/src/generator.py b/control/velocity_controller/src/generator.py new file mode 100644 index 000000000..8edaa1cb1 --- /dev/null +++ b/control/velocity_controller/src/generator.py @@ -0,0 +1,187 @@ + +#!/usr/bin/env python3 +""" +AUV NMPC OCP generator for acados +N = 20, Tf = 0.05 (2.5 ms steps) +Nonlinear thrust magnitude constraint. +Diagonal Q/R/Qe loaded from weights.yaml or defaults. + +Generates a solver in ./build_auv_solver/ +""" + +import numpy as np +import yaml +from pathlib import Path + +from acados_template import AcadosOcp, AcadosOcpSolver, AcadosModel +from casadi import SX, vertcat + +# Import the underwater vehicle model from Step 1 +from dynamics import make_auv_model + + +# ----------------------------- +# Load weighting matrices +# ----------------------------- +def load_matrices(path="../config/parameters.yaml"): + if Path(path).exists(): + with open(path, "r") as f: + data = yaml.safe_load(f) + node_key = next(iter(data.keys())) + print("Top-level keys:", list(data.keys())) + print("[INFO] Loading weights from", path) + Q = np.diag(data[node_key]["ros__parameters"]["NMPC_params"]["Q"]) + R = np.diag(data[node_key]["ros__parameters"]["NMPC_params"]["R"]) + Qe = np.diag(data[node_key]["ros__parameters"]["NMPC_params"]["Q"]) + inertia_M=np.reshape(data[node_key]["ros__parameters"]["inertia_matrix"],(6,6)) + D_lin = np.reshape(data[node_key]["ros__parameters"]["dampening_matrix_low"],(6,6)) + D_quad = np.reshape(data[node_key]["ros__parameters"]["dampening_matrix_high"],(6,6)) + N=data[node_key]["ros__parameters"]["NMPC_params"]["N"] + delta_t=data[node_key]["ros__parameters"]["publish_rate"] + max_force=data[node_key]["ros__parameters"]["max_force"] + else: + print("[INFO] Using default Q/R/Qe weights.") + # Default weights — to be removed + Q = np.diag([ 5, 5, 8, 1, 1, 1, 10, 15, 10 ]) + R = np.diag([ 1.0, 0.5, 0.5 ]) + Qe = np.diag([10,10,15, 2,2,2, 30,40,30 ]) + return Q, R, Qe, inertia_M, D_lin, D_quad,N,delta_t,max_force + + +# ----------------------------- +# Build the OCP +# ----------------------------- +def create_auv_ocp(): + # Load weights + Q, R, Qe, inertia_Matrix, D_lin, D_quad, N, delta_t, max_force = load_matrices() + + # Load dynamical model + mdl = make_auv_model(inertia_Matrix,D_lin,D_quad) + + # Wrap into acados model format + acados_model = AcadosModel() + acados_model.name = mdl["name"] + acados_model.x = mdl["x"] + acados_model.u = mdl["u"] + acados_model.f_expl_expr = mdl["f_expl_expr"] + #acados_model.f_impl_expr = mdl["f_impl_expr"] + + # Create OCP + ocp = AcadosOcp() + ocp.model = acados_model + + # Horizon settings + Tf = (delta_t*N)/1000 # total horizon [seconds] + ocp.dims.N = N + ocp.solver_options.tf = Tf + + ocp.solver_options.integrator_type="ERK" + ocp.solver_options.sim_method_num_stages=4 + ocp.solver_options.sim_method_num_steps=2 + + nx = acados_model.x.size()[0] + nu = acados_model.u.size()[0] + + # ---------------------------------- + # Cost: LINEAR_LS (yref-based) + # ---------------------------------- + ocp.cost.cost_type = "LINEAR_LS" + ocp.cost.cost_type_e = "LINEAR_LS" + + # Select which states and inputs enter the cost + + Vx = np.zeros((nx + nu, nx)) # 12×9 + Vu = np.zeros((nx + nu, nu)) # 12×3 + + # Top-left block (state tracking) + Vx[0:nx, 0:nx] = np.eye(nx) + + # Bottom-right block (input tracking) + Vu[nx:nx + nu, 0:nu] = np.eye(nu) + + ocp.cost.Vx = Vx + ocp.cost.Vu = Vu + + ocp.cost.W = np.block([ + [Q, np.zeros((nx, nu))], + [np.zeros((nu, nx)), R] + ]) + + ocp.cost.Vx_e = np.eye(nx) + ocp.cost.W_e = Qe + + # Default references (0 until updated at runtime) + ocp.cost.yref = np.zeros(nx + nu) + ocp.cost.yref_e = np.zeros(nx) + + # ---------------------------------- + # Nonlinear input constraint: + # Fx^2 + My^2 + Mz^2 <= 10000 + # ---------------------------------- + #Fx, My, Mz = acados_model.u[0], acados_model.u[1], acados_model.u[2] + #h_expr = Fx**2 + My**2 + Mz**2 + + #ocp.model.con_h_expr = vertcat(h_expr) # 1 constraint + #ocp.dims.nh=1 + #ocp.constraints.lh = np.array([0.0]) # lower bound: h >= 0 (redundant) + #ocp.constraints.uh = np.array([max_force**2]) # upper bound: magnitude <= 100 + + # No bounds on slack variables (we don't use slacks) + #ocp.constraints.idxsh = np.array([], dtype=int) + #ocp.constraints.lsh = np.array([]) + #ocp.constraints.ush = np.array([]) + + # No box-constraints on h (handled via lh/uh) + #ocp.constraints.idxbh = np.array([], dtype=int) + #ocp.constraints.lbh = np.array([]) + #ocp.constraints.ubh = np.array([]) + + u_max = max_force # from YAML + + ocp.constraints.lbu = -u_max * np.ones(nu) + ocp.constraints.ubu = u_max * np.ones(nu) + ocp.constraints.idxbu = np.arange(nu, dtype=int) + + # ---------------------------------- + # Initial state constraint (must be updated before solve) + # ---------------------------------- + ocp.constraints.x0 = np.zeros(nx) + + # ---------------------------------- + # Solver options + # ---------------------------------- + ocp.solver_options.qp_solver = "FULL_CONDENSING_HPIPM" + ocp.solver_options.qp_solver_warm_start=1 + ocp.solver_options.hessian_approx = "GAUSS_NEWTON" + #ocp.solver_options.integrator_type = "ERK" + #cp.solver_options.nlp_solver_type = "SQP" # fast real-time iteration + ocp.solver_options.nlp_solver_max_iter = 100 + + ocp.solver_options.globalization = 'MERIT_BACKTRACKING' + ocp.solver_options.levenberg_marquardt = 1e-4 + ocp.solver_options.print_level = 2 + ocp.constraints.idxe = np.array([], dtype=int) + + # ---------------------------------- + # Output directory + # ---------------------------------- + ocp.code_gen_opts.code_export_directory = "build_auv_solver" + + print("nh:", ocp.dims.nh) + print("lh:", ocp.constraints.lh) + print("uh:", ocp.constraints.uh) + + #print("idxbh:", ocp.constraints.idxbh) + print("idxsh:", ocp.constraints.idxsh) + + return ocp + + +# ----------------------------- +# Main entry: generate solver +# ----------------------------- +if __name__ == "__main__": + ocp = create_auv_ocp() + print("[INFO] Generating AUV NMPC solver...") + AcadosOcpSolver(ocp, json_file="auv_ocp.json") + print("[INFO] Done. Solver code is in ./build_auv_solver/") diff --git a/control/velocity_controller/src/test_VC.cpp b/control/velocity_controller/src/test_VC.cpp index 5b397b885..466b4026f 100644 --- a/control/velocity_controller/src/test_VC.cpp +++ b/control/velocity_controller/src/test_VC.cpp @@ -6,6 +6,7 @@ #include ///#include "velocity_controller/PID_setup.hpp" #include "velocity_controller/test_VC.hpp" +#include #include #include #include "vortex_msgs/msg/los_guidance.hpp" @@ -19,32 +20,39 @@ test_VC::test_VC() : Node("test_VC_node") this->declare_parameter("topics.odom_topic"); this->topic_guidance=this->get_parameter("topics.guidance_topic").as_string(); this->topic_odometry=this->get_parameter("topics.odom_topic").as_string(); - publisher_guidance = this->create_publisher(topic_guidance, 10); - publisher_state = this->create_publisher(topic_state,10); + rclcpp::QoS pub_QoS(10); + pub_QoS.keep_last(10).reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT).durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); + + publisher_guidance = this->create_publisher(topic_guidance, pub_QoS); + publisher_state = this->create_publisher("/state", pub_QoS); + + rclcpp::QoS sub_QoS(10); + sub_QoS.keep_last(10).reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT).durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); subscription_state = this->create_subscription( - topic_odometry,10, + topic_odometry,sub_QoS, std::bind(&test_VC::odometry_callback,this,std::placeholders::_1)); - rclcpp::QoS orca_QoS(2); - orca_QoS.keep_last(2).reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT); - timer_ = this->create_wall_timer( - std::chrono::milliseconds(200), + std::chrono::milliseconds(1000), std::bind(&test_VC::send_guidance, this)); clock_ = this->get_clock(); RCLCPP_INFO(this->get_logger(), "Test_VC node has been started"); - reference_msg.surge=0.2;reference_msg.pitch=-1.22;reference_msg.yaw=0.0; //Surge, pitch, yaw + } void test_VC::send_guidance() { - time1+=0.2; + /*time1+=0.2; reference_msg.yaw=0.6*sin(time1*std::numbers::pi/9); - reference_msg.pitch=0.3*sin(time1*std::numbers::pi/9); + reference_msg.pitch=0.3*sin(time1*std::numbers::pi/9);*/ + reference_msg.surge=1.0;reference_msg.pitch=0.3;reference_msg.yaw=-1.57; //Surge, pitch, yaw + RCLCPP_INFO(this->get_logger(), "guidance callback: %f, %f, %f",reference_msg.surge,reference_msg.pitch,reference_msg.yaw); + publisher_guidance->publish(reference_msg); } void test_VC::odometry_callback(const nav_msgs::msg::Odometry::SharedPtr msg_ptr){ + //RCLCPP_INFO(this->get_logger(), "odo callback"); vortex_msgs::msg::LOSGuidance msg; angle temp=quaternion_to_euler_angle(msg_ptr->pose.pose.orientation.w, msg_ptr->pose.pose.orientation.x, msg_ptr->pose.pose.orientation.y, msg_ptr->pose.pose.orientation.z); msg.set__pitch(temp.thetat); diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index a23136283..58bac9f6e 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -1,14 +1,18 @@ #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/string.hpp" #include "velocity_controller/velocity_controller.hpp" +#include #include "geometry_msgs/msg/wrench_stamped.hpp" #include "std_msgs/msg/float64_multi_array.hpp" //#include //#include #include "std_msgs/msg/bool.hpp" #include "velocity_controller/PID_setup.hpp" +#include #include #include +#include +#include #include "vortex_msgs/msg/los_guidance.hpp" #include "vortex/utils/math.hpp" #include "velocity_controller/utilities.hpp" @@ -24,25 +28,43 @@ Velocity_node::Velocity_node() : Node("velocity_controller_node"), PID_surge(100 get_new_parameters(); - // Publishers - rclcpp::QoS orca_QoS(2); - orca_QoS.keep_last(2).reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT); + // Publishers - use TRANSIENT_LOCAL for internal topics + rclcpp::QoS pub_QoS(10); + pub_QoS.keep_last(10).reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT).durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); - publisher_thrust = create_publisher(topic_thrust, orca_QoS); - publisher_reference = create_publisher("/reference",2); + publisher_thrust = create_publisher(topic_thrust, pub_QoS); + publisher_reference = create_publisher("/reference", pub_QoS); + + //Subscribers - use VOLATILE for external topics (simulator, sensors) + rclcpp::QoS sub_QoS(10); + sub_QoS.keep_last(10).reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT).durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); - //Subscribers subscriber_Odometry = this->create_subscription( - topic_odometry,10, + topic_odometry,sub_QoS, std::bind(&Velocity_node::odometry_callback,this,std::placeholders::_1)); subscriber_guidance = this->create_subscription( - topic_guidance,10, + topic_guidance,sub_QoS, std::bind(&Velocity_node::guidance_callback,this, std::placeholders::_1)); subscriber_killswitch = this->create_subscription( - topic_killswitch,10, + topic_killswitch,sub_QoS, std::bind(&Velocity_node::killswitch_callback,this, std::placeholders::_1)); + //NMPC controller + NMPC.set_matrices(Q2,R2, inertia_matrix, max_force, dampening_matrix_low, dampening_matrix_high); + NMPC.set_interval(publish_rate/1000.0); + NMPC.initialize_MPC(); + + //NMPC acados controller + NMPC_acados.init(); + NMPC_acados.set_max_force(max_force); + std::vector W=Q2; + W.insert(W.end(),R2.begin(),R2.end()); + std::vector We=Q2; + for (int i=0;i<(int)We.size();i++){ + We[i]+=1e-6; + } + NMPC_acados.set_weights(W, We); //Timer @@ -56,10 +78,7 @@ Velocity_node::Velocity_node() : Node("velocity_controller_node"), PID_surge(100 controller_type=1; RCLCPP_INFO(this->get_logger(),"Switching to PID"); }; - //NMPC controller - NMPC.set_matrices(Q2,R2, inertia_matrix, max_force, dampening_matrix_low, dampening_matrix_high); - NMPC.set_interval(publish_rate/1000.0); - NMPC.initialize_MPC(); + } @@ -68,8 +87,8 @@ Velocity_node::Velocity_node() : Node("velocity_controller_node"), PID_surge(100 //Publish/timer functions -void Velocity_node::publish_thrust() -{ +void Velocity_node::publish_thrust(){ + RCLCPP_INFO(this->get_logger(),"sending thrust"); publisher_thrust->publish(thrust_out); } @@ -113,6 +132,7 @@ void Velocity_node::calc_thrust() break; } case 3:{ + RCLCPP_INFO(this->get_logger(),"Guidance: %f, %f, %f",guidance_values.surge,guidance_values.pitch,guidance_values.yaw); Eigen::Matrix u; u=NMPC.calculate_thrust(guidance_values, current_state); if (u==Eigen::Matrix{9999,9999,9999}){ @@ -121,12 +141,30 @@ void Velocity_node::calc_thrust() } else{ thrust_out.wrench.force.x=u[0]; - thrust_out.wrench.torque.x=u[1]; - thrust_out.wrench.torque.x=u[2]; + thrust_out.wrench.torque.y=u[1]; + thrust_out.wrench.torque.z=u[2]; + RCLCPP_INFO(this->get_logger(),"NMPC: surge: %f, pitch %f, yaw %f",u(0),u(1),u(2)); } break; } + case 4:{ + std::vectoru; + std::array x_ref={guidance_values.surge,guidance_values.sway,guidance_values.heave,guidance_values.roll_rate,guidance_values.pitch_rate,guidance_values.yaw_rate,guidance_values.roll,guidance_values.pitch,guidance_values.yaw}; + std::array u_ref={0,0,0}; + + NMPC_acados.setReference(x_ref,u_ref); + std::array state_array={current_state.surge,current_state.sway,current_state.heave,current_state.roll_rate,current_state.pitch_rate,current_state.yaw_rate,current_state.roll,current_state.pitch,current_state.yaw}; + NMPC_acados.setState(state_array); + if(NMPC_acados.solve_once()){ + rclcpp::shutdown(); + }; + u=NMPC_acados.getU0(); + thrust_out.wrench.force.x=u[0]; + thrust_out.wrench.torque.y=u[1]; + thrust_out.wrench.torque.z=u[2]; + break; + } default:{ //Some crash handling here RCLCPP_ERROR(this->get_logger(),"Unknown controller set"); @@ -144,8 +182,9 @@ void Velocity_node::calc_thrust() //Callback functions void Velocity_node::guidance_callback(const vortex_msgs::msg::LOSGuidance::SharedPtr msg_ptr){ guidance_values = *msg_ptr; - //RCLCPP_DEBUG(this->get_logger(), "Guidance received: surge=%.3f pitch=%.3f yaw=%.3f", - // guidance_values.surge, guidance_values.pitch, guidance_values.yaw); + //RCLCPP_INFO(this->get_logger(), "Guidance received: surge=%.3f pitch=%.3f yaw=%.3f", + // guidance_values.surge, guidance_values.pitch, guidance_values.yaw); + //RCLCPP_INFO(this->get_logger(),"message: s: %f, p:%f, y:%f", msg_ptr->surge,msg_ptr->pitch,msg_ptr->yaw); return; } @@ -226,7 +265,10 @@ void Velocity_node::get_new_parameters(){ int main(int argc, char * argv[]) { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + auto node = std::make_shared(); + rclcpp::executors::MultiThreadedExecutor exec; + exec.add_node(node); + exec.spin(); rclcpp::shutdown(); return 0; } diff --git a/control/velocity_controller/tests/CMakeLists.txt b/control/velocity_controller/tests/CMakeLists.txt index 97fb0db69..b76128d1c 100644 --- a/control/velocity_controller/tests/CMakeLists.txt +++ b/control/velocity_controller/tests/CMakeLists.txt @@ -67,21 +67,6 @@ target_link_libraries(LQR_test #System tests -add_executable(test_VC_node -../src/test_VC.cpp -../src/utilities.cpp -../src/ct_instantiations.cpp -) - -target_include_directories(test_VC_node PUBLIC -$ -$ -${EIGEN3_INCLUDE_DIR} -) - - -target_link_libraries(test_VC_node Eigen3::Eigen ct_optcon ct_core) - target_link_libraries( ${TEST_BINARY_NAME} @@ -94,16 +79,5 @@ target_link_libraries( ct_core ) -ament_target_dependencies(test_VC_node -rclcpp -std_msgs -vortex_msgs -geometry_msgs -nav_msgs -vortex_utils -) -install(TARGETS test_VC_node -DESTINATION lib/${PROJECT_NAME} -) #gtest_discover_tests(${TEST_BINARY_NAME}) \ No newline at end of file From 3a1040263a9efc89ef52c59050c988d626758ab6 Mon Sep 17 00:00:00 2001 From: Henrik Date: Sun, 1 Mar 2026 13:41:33 +0100 Subject: [PATCH 068/128] no longer numericly unstable, just numericly wrong --- .../config/parameters.yaml | 4 +- .../velocity_controller/NMPC_acados.hpp | 10 +- .../velocity_controller/src/NMPC_acados.cpp | 120 ++-- control/velocity_controller/src/auv_ocp.json | 529 ++---------------- .../acados_sim_solver_auv_model.c | 2 +- .../acados_solver_auv_model.c | 201 ++----- .../acados_solver_auv_model.h | 8 +- .../acados_solver_auv_model.o | Bin 35480 -> 31144 bytes .../libacados_ocp_solver_auv_model.so | Bin 85424 -> 85368 bytes control/velocity_controller/src/generator.py | 91 ++- control/velocity_controller/src/test_VC.cpp | 2 +- .../src/velocity_controller.cpp | 13 +- 12 files changed, 240 insertions(+), 740 deletions(-) diff --git a/control/velocity_controller/config/parameters.yaml b/control/velocity_controller/config/parameters.yaml index f3b3c6ad4..fa4a9b8d9 100644 --- a/control/velocity_controller/config/parameters.yaml +++ b/control/velocity_controller/config/parameters.yaml @@ -14,8 +14,8 @@ Q: [300.0,32.84,32.84,32.84,32.84,100.0,32.84,32.84] R: [0.02,3.1,3.10] NMPC_params: - Q: [1.0,0.0,0.0,0.0,0.1,0.1,0.0,2.0,5.0] # u,v,w,p,q,r,phi,theta,psi - R: [0.1,1.0,1.0] # u_surge, u_theta, u_psi + Q: [10.0,1.0,1.0,10.0,10.0] # u,q,r,theta,psi + R: [0.01,0.001,1.0] # u_surge, u_theta, u_psi N: 20 inertia_matrix: [ 30.0, 0.0, 0.0, 0.0, 0.0, 0.6, 0.0, 30.0, 0.0, 0.0, -0.6, 0.3, 0.0, 0.0, 30.0, 0.6, 0.3, 0.0, 0.0, 0.0, 0.6, 0.68, 0.0, 0.0, 0.0, -0.6, 0.3, 0.0, 3.32, 0.0, 0.6, 0.3, 0.0, 0.0, 0.0, 3.34] dampening_matrix_low: [23.0,0.0,0.0,0.0,0.0,0.0, 0.0,46.0,0.0,0.0,0.0,0.0, 0.0,0.0,46.0,0.0,0.0,0.0, 0.0,0.0,0.0,46.0,0.0,0.0, 0.0,0.0,0.0,0.0,46.0,0.0, 0.0,0.0,0.0,0.0,0.0,46.0] diff --git a/control/velocity_controller/include/velocity_controller/NMPC_acados.hpp b/control/velocity_controller/include/velocity_controller/NMPC_acados.hpp index 9ec826c17..dcc65b37d 100644 --- a/control/velocity_controller/include/velocity_controller/NMPC_acados.hpp +++ b/control/velocity_controller/include/velocity_controller/NMPC_acados.hpp @@ -20,10 +20,10 @@ extern "C" { class AuvNMPC { public: // Adjust sizes if your model differs - static constexpr int NX = 9; // [u v w p q r phi theta psi] - static constexpr int NU = 3; // [Fx My Mz] - static constexpr int NY = NX + NU; - static constexpr int NY_E = NX; + static constexpr int NX = AUV_MODEL_NX; // [u v w p q r phi theta psi] + static constexpr int NU = AUV_MODEL_NU; // [Fx My Mz] + static constexpr int NY = AUV_MODEL_NY; + static constexpr int NY_E = AUV_MODEL_NYN; // Pass N if your generated header does not provide _acados_get_N() @@ -83,7 +83,7 @@ class AuvNMPC { //U out std::vector u0_out={0,0,0}; //Recorded states states - std::array x0; + std::array x0; std::array xr; std::array ur={0,0,0}; }; diff --git a/control/velocity_controller/src/NMPC_acados.cpp b/control/velocity_controller/src/NMPC_acados.cpp index dbaca183c..3db57ce8e 100644 --- a/control/velocity_controller/src/NMPC_acados.cpp +++ b/control/velocity_controller/src/NMPC_acados.cpp @@ -35,73 +35,24 @@ bool AuvNMPC::init() { dims_ = auv_model_acados_get_nlp_dims(capsule_); nlp_in_ = auv_model_acados_get_nlp_in(capsule_); nlp_out_= auv_model_acados_get_nlp_out(capsule_); - - // Get N from generated getter if available; else use override. - // If your header doesn’t have *_get_N, pass N in the constructor. - #ifdef auv_model_acados_get_N - N_ = auv_model_acados_get_N(capsule_); - #else - N_ = (N_override_ > 0) ? N_override_ : 20; // fallback - #endif - - // Provide some safe default weights, tune later or call set_weights() - if (W_diag_.size() == NY) { - // Example diag: [Q; R] - // states: 5,5,8, 1,1,1, 10,15,10 - // inputs: 1,0.5,0.5 - double Wd[NY] = {5,5,8, 1,1,1, 10,15,10, 1,0.5,0.5}; - W_diag_.assign(Wd, Wd + NY); - } - if (W_e_diag_.size() == NY_E) { - double Wed[NY_E] = {10,10,15, 2,2,2, 30,40,30}; - W_e_diag_.assign(Wed, Wed + NY_E); - } - - // Initialize per-stage yref & W (zeros ref by default) - std::vector W(NY * NY, 0.0); - set_diag(W.data(), NY, W_diag_); - for (int k = 0; k < N_; ++k) { - double yref[NY] = {0}; - ocp_nlp_cost_model_set(config_, dims_, nlp_in_, k, "yref", yref); - ocp_nlp_cost_model_set(config_, dims_, nlp_in_, k, "W", W.data()); - } - { - std::vector W_e(NY_E * NY_E, 0.0); - set_diag(W_e.data(), NY_E, W_e_diag_); - double yref_e[NY_E] = {0}; - ocp_nlp_cost_model_set(config_, dims_, nlp_in_, N_, "yref", yref_e); - ocp_nlp_cost_model_set(config_, dims_, nlp_in_, N_, "W", W_e.data()); - } - - // Initialize nonlinear constraint bounds at stage 0 (if you defined con_h_expr with nh=1) - double lh0[1] = { 0.0 }; - double uh0[1] = { max_force2_ }; - ocp_nlp_constraints_model_set(config_, dims_, nlp_in_, nlp_out_, 0, "lh", lh0); - ocp_nlp_constraints_model_set(config_, dims_, nlp_in_, nlp_out_, 0, "uh", uh0); + N_ = (N_override_ > 0) ? N_override_ : AUV_MODEL_N; // fallback return true; } void AuvNMPC::set_weights(const std::vector& Wd, const std::vector& We) { - for (int i=0;i<(int)Wd.size();i++){ - std::cout<(x0.data())); @@ -126,28 +76,44 @@ int AuvNMPC::solve_once() set_diag(W_e.data(), NY_E, W_e_diag_); // Update stages + // Stage yref: [u, q, r, theta, psi, tau1, tau2, tau3] + // Matches Vx selecting states [0,4,5,7,8] and Vu selecting all 3 inputs + double yref[NY] = { + xr[0], // u (surge velocity) + xr[4], // q (pitch rate) + xr[5], // r (yaw rate) + xr[7], // theta (pitch) + xr[8], // psi (yaw) + ur[0], // tau_surge + ur[1], // tau_pitch + ur[2] // tau_yaw + }; for (int k = 0; k < N_; ++k) { - double yref[NY] = {0}; - std::memcpy(yref, xr.data(), NX*sizeof(double)); - std::memcpy(yref+NX, ur.data(), NU*sizeof(double)); ocp_nlp_cost_model_set(config_, dims_, nlp_in_, k, "yref", yref); ocp_nlp_cost_model_set(config_, dims_, nlp_in_, k, "W", W.data()); - - double lh[1] = { 0.0 }; - double uh[1] = { max_force2_ }; - ocp_nlp_constraints_model_set(config_, dims_, nlp_in_, nlp_out_, k, "lh", lh); - ocp_nlp_constraints_model_set(config_, dims_, nlp_in_, nlp_out_, k, "uh", uh); } - + // Terminal yref_e: [u, q, r, theta, psi] + double yref_e[NY_E] = { + xr[0], // u + xr[4], // q + xr[5], // r + xr[7], // theta + xr[8] // psi + }; - { - double yref_e[NY_E] = {0}; - std::memcpy(yref_e, xr.data(), NX*sizeof(double)); - ocp_nlp_cost_model_set(config_, dims_, nlp_in_, N_, "yref", yref_e); - ocp_nlp_cost_model_set(config_, dims_, nlp_in_, N_, "W", W_e.data()); - } + ocp_nlp_cost_model_set(config_, dims_, nlp_in_, N_, "yref", yref_e); + ocp_nlp_cost_model_set(config_, dims_, nlp_in_, N_, "W", W_e.data()); // Solve (blocking) + /* + for (int k = 0; k <= N_; ++k) { + ocp_nlp_out_set(config_, dims_, nlp_out_, nlp_in_,k, "x", const_cast(x0.data())); + } + double u_init[NU] = {0.0, 0.0, 0.0}; + for (int k = 0; k < N_; ++k) { + ocp_nlp_out_set(config_, dims_, nlp_out_,nlp_in_, k, "u", u_init); + } + */ int status = auv_model_acados_solve(capsule_); // Read u0 @@ -162,9 +128,9 @@ std::vector AuvNMPC::getU0(){ return u0_out; } -void AuvNMPC::setState(const std::array& x){ +void AuvNMPC::setState(const std::array& x){ x0=x; - for (int i=0;inlp_solver = SQP; + nlp_solver_plan->nlp_solver = SQP_RTI; nlp_solver_plan->ocp_qp_solver_plan.qp_solver = FULL_CONDENSING_HPIPM; nlp_solver_plan->relaxed_ocp_qp_solver_plan.qp_solver = FULL_CONDENSING_HPIPM; @@ -177,7 +177,7 @@ void auv_model_acados_create_set_plan(ocp_nlp_plan_t* nlp_solver_plan, const int nlp_solver_plan->regularization = NO_REGULARIZE; - nlp_solver_plan->globalization = MERIT_BACKTRACKING; + nlp_solver_plan->globalization = FIXED_STEP; } @@ -468,14 +468,14 @@ void auv_model_acados_setup_nlp_in(auv_model_solver_capsule* capsule, const int double* W_0 = calloc(NY0*NY0, sizeof(double)); // change only the non-zero elements: - W_0[0+(NY0) * 0] = 1; - W_0[4+(NY0) * 4] = 0.1; - W_0[5+(NY0) * 5] = 0.1; - W_0[7+(NY0) * 7] = 2; - W_0[8+(NY0) * 8] = 5; - W_0[9+(NY0) * 9] = 0.1; - W_0[10+(NY0) * 10] = 1; - W_0[11+(NY0) * 11] = 1; + W_0[0+(NY0) * 0] = 10; + W_0[1+(NY0) * 1] = 1; + W_0[2+(NY0) * 2] = 1; + W_0[3+(NY0) * 3] = 10; + W_0[4+(NY0) * 4] = 10; + W_0[5+(NY0) * 5] = 0.01; + W_0[6+(NY0) * 6] = 0.001; + W_0[7+(NY0) * 7] = 1; ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "W", W_0); free(W_0); double* Vx_0 = calloc(NY0*NX, sizeof(double)); @@ -485,17 +485,13 @@ void auv_model_acados_setup_nlp_in(auv_model_solver_capsule* capsule, const int Vx_0[2+(NY0) * 2] = 1; Vx_0[3+(NY0) * 3] = 1; Vx_0[4+(NY0) * 4] = 1; - Vx_0[5+(NY0) * 5] = 1; - Vx_0[6+(NY0) * 6] = 1; - Vx_0[7+(NY0) * 7] = 1; - Vx_0[8+(NY0) * 8] = 1; ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "Vx", Vx_0); free(Vx_0); double* Vu_0 = calloc(NY0*NU, sizeof(double)); // change only the non-zero elements: - Vu_0[9+(NY0) * 0] = 1; - Vu_0[10+(NY0) * 1] = 1; - Vu_0[11+(NY0) * 2] = 1; + Vu_0[5+(NY0) * 0] = 1; + Vu_0[6+(NY0) * 1] = 1; + Vu_0[7+(NY0) * 2] = 1; ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "Vu", Vu_0); free(Vu_0); double* yref = calloc(NY, sizeof(double)); @@ -508,14 +504,14 @@ void auv_model_acados_setup_nlp_in(auv_model_solver_capsule* capsule, const int free(yref); double* W = calloc(NY*NY, sizeof(double)); // change only the non-zero elements: - W[0+(NY) * 0] = 1; - W[4+(NY) * 4] = 0.1; - W[5+(NY) * 5] = 0.1; - W[7+(NY) * 7] = 2; - W[8+(NY) * 8] = 5; - W[9+(NY) * 9] = 0.1; - W[10+(NY) * 10] = 1; - W[11+(NY) * 11] = 1; + W[0+(NY) * 0] = 10; + W[1+(NY) * 1] = 1; + W[2+(NY) * 2] = 1; + W[3+(NY) * 3] = 10; + W[4+(NY) * 4] = 10; + W[5+(NY) * 5] = 0.01; + W[6+(NY) * 6] = 0.001; + W[7+(NY) * 7] = 1; for (int i = 1; i < N; i++) { @@ -529,10 +525,6 @@ void auv_model_acados_setup_nlp_in(auv_model_solver_capsule* capsule, const int Vx[2+(NY) * 2] = 1; Vx[3+(NY) * 3] = 1; Vx[4+(NY) * 4] = 1; - Vx[5+(NY) * 5] = 1; - Vx[6+(NY) * 6] = 1; - Vx[7+(NY) * 7] = 1; - Vx[8+(NY) * 8] = 1; for (int i = 1; i < N; i++) { ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "Vx", Vx); @@ -542,9 +534,9 @@ void auv_model_acados_setup_nlp_in(auv_model_solver_capsule* capsule, const int double* Vu = calloc(NY*NU, sizeof(double)); // change only the non-zero elements: - Vu[9+(NY) * 0] = 1; - Vu[10+(NY) * 1] = 1; - Vu[11+(NY) * 2] = 1; + Vu[5+(NY) * 0] = 1; + Vu[6+(NY) * 1] = 1; + Vu[7+(NY) * 2] = 1; for (int i = 1; i < N; i++) { @@ -558,11 +550,11 @@ void auv_model_acados_setup_nlp_in(auv_model_solver_capsule* capsule, const int double* W_e = calloc(NYN*NYN, sizeof(double)); // change only the non-zero elements: - W_e[0+(NYN) * 0] = 1; - W_e[4+(NYN) * 4] = 0.1; - W_e[5+(NYN) * 5] = 0.1; - W_e[7+(NYN) * 7] = 2; - W_e[8+(NYN) * 8] = 5; + W_e[0+(NYN) * 0] = 10; + W_e[1+(NYN) * 1] = 1; + W_e[2+(NYN) * 2] = 1; + W_e[3+(NYN) * 3] = 10; + W_e[4+(NYN) * 4] = 10; ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "W", W_e); free(W_e); double* Vx_e = calloc(NYN*NX, sizeof(double)); @@ -572,10 +564,6 @@ void auv_model_acados_setup_nlp_in(auv_model_solver_capsule* capsule, const int Vx_e[2+(NYN) * 2] = 1; Vx_e[3+(NYN) * 3] = 1; Vx_e[4+(NYN) * 4] = 1; - Vx_e[5+(NYN) * 5] = 1; - Vx_e[6+(NYN) * 6] = 1; - Vx_e[7+(NYN) * 7] = 1; - Vx_e[8+(NYN) * 8] = 1; ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "Vx", Vx_e); free(Vx_e); @@ -667,6 +655,23 @@ void auv_model_acados_setup_nlp_in(auv_model_solver_capsule* capsule, const int /* Path constraints */ + // x + int* idxbx = malloc(NBX * sizeof(int)); + idxbx[0] = 7; + double* lubx = calloc(2*NBX, sizeof(double)); + double* lbx = lubx; + double* ubx = lubx + NBX; + lbx[0] = -1.4; + ubx[0] = 1.4; + + for (int i = 1; i < N; i++) + { + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxbx", idxbx); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lbx", lbx); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "ubx", ubx); + } + free(idxbx); + free(lubx); @@ -718,22 +723,12 @@ static void auv_model_acados_create_set_opts(auv_model_solver_capsule* capsule) int fixed_hess = 0; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "fixed_hess", &fixed_hess); - double globalization_alpha_min = 0.05; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "globalization_alpha_min", &globalization_alpha_min); - double globalization_alpha_reduction = 0.7; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "globalization_alpha_reduction", &globalization_alpha_reduction); + double globalization_fixed_step_length = 1; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "globalization_fixed_step_length", &globalization_fixed_step_length); - int globalization_line_search_use_sufficient_descent = 0; - ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "globalization_line_search_use_sufficient_descent", &globalization_line_search_use_sufficient_descent); - - int globalization_use_SOC = 0; - ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "globalization_use_SOC", &globalization_use_SOC); - - double globalization_eps_sufficient_descent = 0.0001; - ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "globalization_eps_sufficient_descent", &globalization_eps_sufficient_descent); int with_solution_sens_wrt_params = false; ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "with_solution_sens_wrt_params", &with_solution_sens_wrt_params); @@ -754,7 +749,7 @@ static void auv_model_acados_create_set_opts(auv_model_solver_capsule* capsule) // set up sim_method_num_steps // all sim_method_num_steps are identical - int sim_method_num_steps = 2; + int sim_method_num_steps = 4; for (int i = 0; i < N; i++) ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_num_steps", &sim_method_num_steps); @@ -787,14 +782,6 @@ static void auv_model_acados_create_set_opts(auv_model_solver_capsule* capsule) bool store_iterates = false; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "store_iterates", &store_iterates); - int log_primal_step_norm = false; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "log_primal_step_norm", &log_primal_step_norm); - - int log_dual_step_norm = false; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "log_dual_step_norm", &log_dual_step_norm); - - double nlp_solver_tol_min_step_norm = 0; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "tol_min_step_norm", &nlp_solver_tol_min_step_norm); // set HPIPM mode: should be done before setting other QP solver options ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_hpipm_mode", "BALANCE"); @@ -806,75 +793,17 @@ static void auv_model_acados_create_set_opts(auv_model_solver_capsule* capsule) - // set SQP specific options - double nlp_solver_tol_stat = 0.000001; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "tol_stat", &nlp_solver_tol_stat); - - double nlp_solver_tol_eq = 0.000001; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "tol_eq", &nlp_solver_tol_eq); - - double nlp_solver_tol_ineq = 0.000001; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "tol_ineq", &nlp_solver_tol_ineq); - - double nlp_solver_tol_comp = 0.000001; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "tol_comp", &nlp_solver_tol_comp); - - int nlp_solver_max_iter = 100; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "max_iter", &nlp_solver_max_iter); - - // set options for adaptive Levenberg-Marquardt Update - bool with_adaptive_levenberg_marquardt = false; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "with_adaptive_levenberg_marquardt", &with_adaptive_levenberg_marquardt); - - double adaptive_levenberg_marquardt_lam = 5; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "adaptive_levenberg_marquardt_lam", &adaptive_levenberg_marquardt_lam); - - double adaptive_levenberg_marquardt_mu_min = 0.0000000000000001; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "adaptive_levenberg_marquardt_mu_min", &adaptive_levenberg_marquardt_mu_min); - - double adaptive_levenberg_marquardt_mu0 = 0.001; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "adaptive_levenberg_marquardt_mu0", &adaptive_levenberg_marquardt_mu0); - - double adaptive_levenberg_marquardt_obj_scalar = 2; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "adaptive_levenberg_marquardt_obj_scalar", &adaptive_levenberg_marquardt_obj_scalar); - - bool eval_residual_at_max_iter = false; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "eval_residual_at_max_iter", &eval_residual_at_max_iter); - - // QP scaling - double qpscaling_ub_max_abs_eig = 100000; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qpscaling_ub_max_abs_eig", &qpscaling_ub_max_abs_eig); - - double qpscaling_lb_norm_inf_grad_obj = 0.0001; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qpscaling_lb_norm_inf_grad_obj", &qpscaling_lb_norm_inf_grad_obj); - - qpscaling_scale_objective_type qpscaling_scale_objective = NO_OBJECTIVE_SCALING; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qpscaling_scale_objective", &qpscaling_scale_objective); - - ocp_nlp_qpscaling_constraint_type qpscaling_scale_constraints = NO_CONSTRAINT_SCALING; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qpscaling_scale_constraints", &qpscaling_scale_constraints); - - // NLP QP tol strategy - ocp_nlp_qp_tol_strategy_t nlp_qp_tol_strategy = FIXED_QP_TOL; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_strategy", &nlp_qp_tol_strategy); - - double nlp_qp_tol_reduction_factor = 0.1; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_reduction_factor", &nlp_qp_tol_reduction_factor); - - double nlp_qp_tol_safety_factor = 0.1; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_safety_factor", &nlp_qp_tol_safety_factor); - - double nlp_qp_tol_min_stat = 0.000000001; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_min_stat", &nlp_qp_tol_min_stat); + int as_rti_iter = 1; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "as_rti_iter", &as_rti_iter); - double nlp_qp_tol_min_eq = 0.0000000001; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_min_eq", &nlp_qp_tol_min_eq); + int as_rti_level = 4; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "as_rti_level", &as_rti_level); - double nlp_qp_tol_min_ineq = 0.0000000001; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_min_ineq", &nlp_qp_tol_min_ineq); + int rti_log_residuals = 0; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "rti_log_residuals", &rti_log_residuals); - double nlp_qp_tol_min_comp = 0.00000000001; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_min_comp", &nlp_qp_tol_min_comp); + int rti_log_only_available_residuals = 0; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "rti_log_only_available_residuals", &rti_log_only_available_residuals); bool with_anderson_acceleration = false; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "with_anderson_acceleration", &with_anderson_acceleration); @@ -1153,23 +1082,13 @@ void auv_model_acados_print_stats(auv_model_solver_capsule* capsule) int nrow = nlp_iter+1 < stat_m ? nlp_iter+1 : stat_m; - printf("iter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter\talpha"); - if (stat_n > 8) - printf("\t\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp"); - printf("\n"); + printf("iter\tqp_stat\tqp_iter\n"); for (int i = 0; i < nrow; i++) { for (int j = 0; j < stat_n + 1; j++) { - if (j == 0 || j == 5 || j == 6) - { - tmp_int = (int) stat[i + j * nrow]; - printf("%d\t", tmp_int); - } - else - { - printf("%e\t", stat[i + j * nrow]); - } + tmp_int = (int) stat[i + j * nrow]; + printf("%d\t", tmp_int); } printf("\n"); } diff --git a/control/velocity_controller/src/build_auv_solver/acados_solver_auv_model.h b/control/velocity_controller/src/build_auv_solver/acados_solver_auv_model.h index 73ddf4ca8..03368e349 100644 --- a/control/velocity_controller/src/build_auv_solver/acados_solver_auv_model.h +++ b/control/velocity_controller/src/build_auv_solver/acados_solver_auv_model.h @@ -41,7 +41,7 @@ #define AUV_MODEL_NU 3 #define AUV_MODEL_NP 0 #define AUV_MODEL_NP_GLOBAL 0 -#define AUV_MODEL_NBX 0 +#define AUV_MODEL_NBX 1 #define AUV_MODEL_NBX0 9 #define AUV_MODEL_NBU 3 #define AUV_MODEL_NSBX 0 @@ -61,9 +61,9 @@ #define AUV_MODEL_NG 0 #define AUV_MODEL_NBXN 0 #define AUV_MODEL_NGN 0 -#define AUV_MODEL_NY0 12 -#define AUV_MODEL_NY 12 -#define AUV_MODEL_NYN 9 +#define AUV_MODEL_NY0 8 +#define AUV_MODEL_NY 8 +#define AUV_MODEL_NYN 5 #define AUV_MODEL_N 20 #define AUV_MODEL_NH 0 #define AUV_MODEL_NHN 0 diff --git a/control/velocity_controller/src/build_auv_solver/acados_solver_auv_model.o b/control/velocity_controller/src/build_auv_solver/acados_solver_auv_model.o index 52e35367604918dfd1b0565a8f2915b91deb8024..f60968d67ff0a8630a084d28ea31190d9e5ac548 100644 GIT binary patch literal 31144 zcmcJ13w%`7x$mA3LO_}cl+v_P9qg!~g_w~5i51Pj1a@=+9083=orGlau*oFN%=@+ zWoI_+x&7VsW6xguzy9C1zV)s5+BT&i$)J{1na;=SIa+T`wWe6F z`!9Is;Vu9t2<+HH&-UQS(&O|P8ny=ymLBNnsPbR9g8+-u!#9Ml4?AbVtHY~SuOzaa zSy~y%)YpdVPN&z|nZD|1rn5Ghxoqgnmg{Wi-y1?_JB)%x=gElkb~HPlv_+jiMx7UJ z=M~OPCmAd}IO4Mh50q*yQyS{tPd`XDg@$kbba+kp)8Sjfu><}K>n%IO8l6||?1xD8 zL*xWIQyUs)L6X@|&?vEp#pUPnVg? z*5La-|L&h7lD+ zF%+Dbd-m*EJNp4r_R~A@V`t}4+|Dkf8{16vLhVU66z9)qD|Wk`DZaRpm`)^EOD!jw zZ4b_mI)AX+x@uYOa3nb2H}vw>kJ}kg+U-v&0sqi1sa6IL+Nx-(-m{Tjt0DCA>;5fl z3+`P1xPOccv9moxmi@@@i~Z9%-9Md^{nJ&d^NN2~G#K#DS`{q!&zceB{B38kZ95|` zP4Hhx^<{VX`y#;rext$isP7dtS;VIW8@C@if*kx)mR9?xEU))ZxnTv1N9$frKZ^V> z=KP&kly!6H2`GEXKb;f((;4+oSKzF6vZRMBsmWQ=OqP5gSAVdl8M&`vd)kFPv-9@E zU{5FP=^JeiR}4Cv@Rol%gT^sl*hW;>k6!!EL{xZYDaNB% zb)$0@BN0ZSOR1p+?4if~J1#7>tPI4tEU)>?dC0KaRs~BNLXU&6`|GG9+gTN?c+Ed0 z60C$h=$F?uWtQ6!=Qrq&P1)I>vz=46bF{(vT_ajmZXiSZY4V+5#h^DwoL52DsY*g5 zx11w6nt`gu!Jqk}b+E%fvJ){jhbaMnaDvA7s$zJfd@JbPmBjwj`=Y zRwY>vNrqWnIZF~*jhxEJ*TViO4R(`rBB!juYIKI4fGBQ({HRF8p*;%ja=~xm8SH(y z6)?jtXvQe(PoaRQhoiXF+xXc=8PT2WpgS2-Ky^hy)ra!FK_N+JrPkd~y7n8P(cUUG zaD-Znav8i8<}v>&44&bKVT6UrEblE<6~_A!=dFh9Rv*n-PeOOtIjlt3@Pbv6uMJfX zjn3cZeYD?m+edS3xbCpBkIN&BvyBIGg@?Py{oZxbvFC5d&sBSH2GCE*M%5PK|9;OuwBgzj`@rZ$& zCq>+OC>*H|lmc#a4*RE+LZcq^vBGNz4Z~I8>!X?5gVk!Nh^omSMn#>sbyjaHo=(y{ zXSSNmKyxQIW*acm9<`lfLwVD~=$x>&QG4GxYHT-Ui8`=Q8$d-sNrZZ}(tLRGXkXCM z3We4qVR&RoDJ}FGof0+%{%7wrIwN-0uITVj8;09E{FRJS9)_Wk>I31M!Z$}UJ(yO8 zUiR;P63T2R8mz#?8%3>^M=_!V{1*T6cQm5Ywm~t2z z_U}A~I9tdKka|M}mO@~u^-PoVHZ4BP{wrtmVPE6mh#HR?>i)F(2_AGRZL*yTpcX`G zJm94A69>NTtyj_7xLGW|cVHBt zV}il=r=>oPC3d=5_aEog@P68fdhEtCN>FU=4q-^Pw;!b33hvXXKi6i~1Xp2nu7_ev zFL9k`8=WPU=s>E1GKdeJtbg6#hkjBV#NEaWfPEWcD+2y0t7wDM*NnckCRlC!SPvDL z)DAR4I+E$H4QCU+Xxn?&X0Ob5)oZfT9;9q}Ejhb98<~Tx@}s`vP1zX^B8}M^2UAqz zBY!BdA9=3Wc4~sz)IVEIzF#+GiznJ`zw(`F%wFkaCtHskE3(@jBafC6!d*3c<;z^8 zw`ZL|T4Q!HVC+imwqN^R!FWMHQ#SY$yX}47DZ7m{{RA2QkYp=pKU8nGo%S8KzeZqF zcG~vO94e!S;_cK@ZRhwS$BG>cO0!!^V_Q=^y$# zI#$6 z4JC|bl+*k>i%Mw?5)E#^a;qWqLKN+|ni_Crqw}ooycBf~sq;e2<`J5+pT#Jur;cUW z;!0hR;-E^@Dd6N1mr>_;r)n`w4DOVi9JRBGbEcMirY_GGU*=8>4<5v%eMn7Bk-B!Y z@$AKD<5B9Knq`Hl z@23xvkFdIl&|okb_5GSs!8UT9l-93tQfJnhgtz2zjKS)>g?pBk#B>F9jZUJrF|$QY zHI12Du!`Ijya%0IH*U;40oyr5GmpxAWCtgMVMQ`6>O6GR2@NSYmRDdqJNS`vZOp6- zQ2Jg<&vhWEB!I#w*1Z5mu?V2RES;%QNtm{oWRNQ%qx5}`C^A<9C@>3Fd|y3U4MQ*Z zcb|`8AIqm`_I}3oN_!6~vdKA}8(nfsqlUV7)Bgz_`=A4oMwA}4sM=@zCHebD%hTh3 zNuExNXz10Z)8k(+Z=(RGAwfYaphvBM+w(c?KfEJICzM8(;`z*XPzuJSbNDaUkWhiA zsG+g37{ZJcjGyy;}lFJKrr}(A+JS>4wIxB7oY7B>mD&B7lM<9l`S`n+TvFN#DTps4^3v8Rj?@^VhB*8JZ_j zw;@ET`YoxFJ|N)ooHpuDyMj-+Nd)A50LIhIQ1xnRi?F;mvGT#|mL#rCdgZGh@VY~7 z{J3K;E?3r#Vp5M+TUJ^&HDdvkW^=U;>XiudU7< zqnyI~Hx|mJjY_f|eTMh{HJOb%MOm1mtk>wZpvn8jTk24>Ef-M{V7xM=KoOd68a&`5 zsy|ysOMd+l1RLfG*K6a5djCrbhaO8`feCzX0=8h zdTPreyX`>eDLXr@KAH{g!TaoDoS$vBZJ${VZEhCv7R2rC2WULy{2S|@P9Fik#aLsT zNHa%n4h`cSzkC^}-w%rS_1jNc_sKblzVVs5I@nRasBzAUH zaGJJY6F<$6^^0MXLHzLBT4v%HbVj-Vi;ts7COx3jX)~?7ilJv3MolBg4P|iHS9ip} z>zeFrJ_*3S#m;^VTA>zO3ak~DWowQhI%8;f!zHwT!CQpc)3JF` zZE=_vx4u)XiO!H_Y<^bTp$S;|s#D3Px|8XDqCuGopF4m_*I+E(4x+T;Y1Ic0C7N z&$X`S$^)bvp*6iO*4x(}?}@dxwYK*qW9k0()>J%}>h6ullBsxq@`^xb_quo@kVyBg zjSmF+Is%jy;Iu&H!obDtvjXki9Ubw3WT0c9uQ!nDiU-=#0|W6ys%K-MwWp`A4I10e zrC{cy6+Q9RWIQksZySh%9a!I;>Ix*{>!CUcDAj#iYpT015ok@c2l@y4)^)eX1FcM0 z7jH}TFnuS+uEbO8tdrZ5O3$|j`hS7ovAKsAk`h~>FbR3 zC3-f-TGzF9_q49_ysY|g)JeTnwivdUS3L|-b<-P_+2??r=% zx6ckFyAy43z=8f&Yjie{Z0$wMObu)#sZ?Je z8BYZ+?k1J3y>a*&Z4r$kz5&q$vgk-B+Nk{`s{##fEYQ`O94$+vZmPGcT>fZQsaPV` z+qxk@O`a@F^{wC~xdUTQ9AzuhQnf*V09jxwVq>y|jFJ>A>8F=q!Tttqt2L|`pC z3v{4=KyYn5wLXr%rrkgZlutl)N?AG-4lO1=&<#!iJ+?Iv@95}m>sFl(susicRY@y} zbQHw+PdENtudixTJ)}U#h;QiciLGmo$Kc5I7C)ig#o$TxKuN9bw^|zq;vLov*7X~# z>(f?u`-Zg}tR7rxT>KR0TN+>*VA?{GHL0(yKSp`>qr><0_NUR~JJ7~dl_xe>iL{lt z%}OM#L_hw;RBN>oIufG-TM6WlfWkzQB1wvM;*M+WhPaj3XeGMv*k!54-4#zJE$+?m zL7Lh^EQv-JTR)JBsph0o(Mi)7`CLh(Wzfn~v7Xl6SZ{a2a`mKiPmk(I)J?4RjfvLY z?zUvCtxt`Ely7Qde>|UpYE%QQ8*1&0Cj}N?pMq>R8uS=2)hBYgwY3f16y3+_iDTTx znB9qHJMh_b>p*)7RhNpveDq^0iD5I&3eilH)}nAUylioUh19P8?tT;w)lE;S>R5N8 zJ7u*dV>BwT1v*YS^yJ2h{NTWT#!!uZ2%B5m+TuNWu(AZ2>R!hKSS-~A(XPIpcFKi)!%L9I(>{Q4~+&bi_KKM79~aJ?d4l{ zGKK1}D0Lz-pcJ~(^Cauy_KDUc3j2tU3xcc4y@#^DEYgb(T)!$um{Jg3uSIt>G zIs>3pJn5zv*&L2F59@n<2Zu8c>-*K#(#4Cf2vn|Kn@*(C zfzaHlP*wGuTJ%#qhPGUOc~y1IEDf^?(oahy%46xW)s*irVU#xo+W1lM-FBVj+feR1 zfAWOVJur}DW^jsSmFYWit3}+h%gV1UE49`Dvo-z;x-Tg!|6W@_298tr!Ar5wR8JqY1xsYq6Eq@ zw&s_dmnhpat`CH9ThA+4rPm-oYO9ax{3uR=?oVs%Vgj1V%E>3EHFk5Z9i7W3o6DN7 zDQhYE7xuiqL95KQ2-h7%VROVOhmu?p&x7B%jvYFXzh7{j^Y< zSX#C}5G~tZkE~8E{roT@I&q?wq8A3APHlq?pPx6e8hbi!2=qbsqZ0;>qqMX z6Ty94e#FnAOF3q|l7p=#51iYXH8Gd3k+@Ow0|wCdn8vBy$PniTnuSJ(TlujhH(!|4 zK8~1C(4dx;;;5v`2Rkplv-{-@7H>0jFjOaekie& zU-KykIXaR1M;^7TpKJbr1Q9=?@eMBgcN)Llg+HzFZ7%$Ijo;zIk83>Z!cS@ZOD_Bk zex8_HH<;viI8ti4TZ-y%+csPF2_iW2LqE_{-YfD-F27hbOMue>B9Rp{;msOr*Zeddb`Gp_?{Q~oW@IB z_)dNvZ@uNhcWeF>7k;P4FLU8{^K*Y_-d_KVtr7rkBrFknty={ zKd$jPF8q|n=eh7VG``q{zoYSGE}S<=@WV|myo8@itWFm`nV-j76)t?L=5KW2idSOY z>B3bxORW1`cu?zk$c2AGAHgk>)?^!flN|@4}TI zORN`M_$tl+g9}&w9dCWlg|}$_TQ0mq<37EPH0`8EUkzUhTp^ zukjieo@IQzRq4XNqVX;lez(>$--Z7p@Tp>5XZF000H?L7JJ5cE_$JM)(f^D+puwcf zi<)1p!C&b6MA%V?U*UmY=7BeO;873!Cg5CN?thlGM&CDi@IU8)@6mcxd$BCuQ=@GA zJ^0Uh;AcGWk7B)E$Uh4_@Kql8Mi2Zh5B#tP{)`7s2LgrcoZ*4jdf@nWvY?)g9{3&) z{QDmGqaOJ0J@8U&2?~{Kwg{9zCL8Lj`c z?zl&FzVCSOCt;&ab{_F7V!h_i0bZ!Q^&WVW2Y#K_U#^w$ISBID;=%v22mU<|`~eUA zcOLjD5BxkVzzdakrU!nd2foS!PkZ1yJ@C6d@O{9kzFPRLbKd4hHQulBi*-G|s`0%V z=kp$@9|K;<53@9X#YKu=t8SL9@jV(xGvLs!@x2;{dpT^^_&yi@4;mkK;SX#4hzmce z@nbIhJ&m7s;pb1*?eOEu4r9+bUi`&kE3Ru;wd%&$lGV!=uWDSrEEcN@S%uJQt17zq z^7;I$;6Y-YFJARYpG$` z8kVhL*&3Fer%KAQ^H_Es%g$rjc`Q4RW#_T%JeHluvh!JXKFiK$+4(FxpJnH>)O=+= zOU-Ag1uV6Ir53Q%0+w39QVUd>S#|--E@0UOEL+R6wJcl9vb8K*%d)jBTg$SwEL+R6 zwJcl5vUMz5$Fg-SRmU3YShkL3>sYpqr9#y#7OG~sP_+_dwV`U33{}^7et4n3*|6vZ zdtcjFTE{><9y6re`)tpzSS<6#xtremrgD#MeM!6;oa35a=+>8jrX$3SZgvF$6IE4!F?_%yhhA@raz&bW>pk#T60b^=-KmXuq3b1!j~vdCxV~>d zmV8vMdF_2^Pt%~DQpXlAhw$56)NHb~y*s9S>bBKG;4RR>&T$PJ7csY0lWOelYSm3Y8~Dufy9uL#_KSHdfOF z)wdJ;#f_*X{k0BSYyRE1>43RUB67Vh=Bj)8#DqS5LAN(T_(Uj~8fe8Q07=nX^X1NG z$e)!^X^bf1+~dcPb$`u&f|3&vPz;}_AXnAp�$+;d72;zv)0T~0C{jmv zryFhW?#0I<{0&@AnlbA67y|H#!$5LuivB>x=*~$9zl{~5u`h?}FN1iLapf_98xZLhtF1(?@UkU@JM+HZD=li|VP6dUP(7 zzqd7o&%okI&ZDE1zJ~0+EpIH$I45t6Vwzb;XFi}NpY}NVVBf}E8rW6hl2+-PBDOZWJp=MFB%n9`~5i0HeYd^hSW)Ii$O_&};xe-Ashjv&KB z&8V?e4?`x6hM_SAP$GX`=9!xR7_YE2r7K5yr&B-nP9@)ar;@kzcE+N=u%JN4F9g1e zzc8Sn-oL8vj<6-k;ddZTf%yHn4FB5%A{61mRwal36M+cC|12)U{{?{vpTL#GW$@n; zh(LPiQxJpyCxHm0Cxy%47wB_H;?q%%!B=aX`0E6Imj`~Y2mUh;yq+>dpc|c8&7cQ_ zsKDu{$k?+|;IuzAc(caI&h-M{BXFtI|RN> z;6nnxL*Snm_}v}4v#_1;g-zV^Gg1<%J^iJN`b4K9wO@YDR*Em)G6oD_IMvp-FR9uF?T;n9RN8l}j z|3QH#JosA$pU%XLe)T6zs=Z154+uTIxeICe1U&h;57oLZ$=FNT7lD;Z15J1lRu@MT>_VOt{1qpbC=NnQKA1|1z+kv zAaJSwae+(ye-!%Z+a_cG1bxuyw%;#sss93jOZ|-+CqK)2yg~4#{V9P<{euFR`oALd z%X+*|;Ih6R6S%Ce69Sj@{-(y=ewd;U90`|p1_Uncyiw!i2U(Auf-n8BUEosx9)U~! zKN0$6J&p*z)c*&8OZ{gAF7=;B10jOD9zUXSw|`~|T>N~V8J6gaIx41S@;$)D2B zg@RA7It8(rz2MU;2c!QB9(;OFK*8Pq@Akm&6}a@zA&pbJ zl79F%flI$VDewSfjGZThep#+p1upB0-rrLo`=vd8S~Me&Jv1~L{TFK7T`!jkK6M?# zUnp>D8U}9^IGJGZI|V*d;Pifig1f$cAov#{-ROD9ga2=W|8c>8)`RbxOu3G&--#ah z2L&$u`ALDx_B>bPqriIvf4k5l^*<=^DT4pDz{zBjufGfr2$ZiJ z$1W4NELX$>k9y$O3tZaYC2(2qpA&cjHW@p=Byh=pK;V-9D~*%?X^Cg_{2zhKecW>b zzeMm~)HwM^?&Dq(cvkRdkx~TWOFM7UIN5V4E@Mwz@b47(ZGvAZaL0qcPw=JwA9?U! z5cuB<{eKpEW(mBAHn#{=uDfs<`zH(hYXV=X@lODg=KyOpPJW;?qvx|ik30u>K;U-^ z{?7%T7WjJt|AxSWQ}BR5_RDtoQQCwc5PlCXW6vcTCp$kW@EXDYZvu}9{%nCadhl-% z{5gW(=D|-2ewE;F@Zf(z@GlemFM9C5Dfrcb|1H7)roitL{E)zZ?7^Q%i+cp}=jFJJ zpQmb^{QnOE|G41K75HTy{IKBH2!5jn|7O9TC-|Kn{EdP?U+{-K_;(5Z0>S^52mgM- zr*9aHe-3)^Ulja0!Jj~jV+8W&UR*|x?SZe=IMvG)g1=etWxebc{7(t~w>|j(;eo&I zfq#S+@d)nr5c0qq1-=(@ro1Z!o)CDK!2eO;n>0>-xKiMI1^>Sb{JVlrLyO7x0f8?R z_+uV=jtc%&g8zyKe>`ng5y;M~ahZHSq;c|_v~z~w*9-n-0uKxPY7afD1%Hv?$36HN z!Cx%+_X_^E1b$fXBLaWHgI_|Ma|H5311{r-pvK7$QvVXcUn2PJ9{f85f2rW#>%l)D z__pA`;K4sF_>F=;o;H~XRIY1qnQ~3lxVwHY75r-je}M9B!M{%MOXzR}!R_bsHBR-i5|^o$ zPkQhd3jQj=Z}Q-;5_}pOjs6aSuNL?^4?P*dzh3aa;lbZ0_%{gtum}G?1ixAEfA7J6 zQ{eXsekpC{5y;OHpC<6HAkFw;rog`~@Yw?2An=6(|DwPf1io9~Hwyea0)I;5RPS{H ze_QbP3cQ#$r3l2o0++G>B8^jd<$O9<@INK^i#_<)30&?wwh28q3O#oS{#^q9lEA+v z@c%9FFA3a7n`ZgWGW9P&G9uNrs5iWyI4d4O6jh`Q2nDPIG zsNn|%{$qj95%_+A&lmVl1l}z0p9;L)17Gif4+&h>*KQB|E)V>q#;JZ=ahdX#&%^@) z`R4`lYQ_-h4zu?K&i;I|3>6(0Q6g5NIqF%N!1@Z*9%B>1wu-6QxNg8yp|{xgE# zDflmY@GCC@34#36h0FNuGL4fzr9E{5m+k*5fy;6AW{np?Pq)zDDfm)38x%lSqP2O2bjZ=0RJ(l)wzYA~C_%0W2 z{;p)N3*W2x_q*_6jX&zbk7@i37rrl`>|3GzXY5>Yk;1!N_#Tbl?!wLAQ<(ZOdiH7l zy)M4_dy4%o{D|iN(uJG9r#R-qPiy`gF5LRKvfrvoZtP99uEjkypzd8JHW2S=txDmu zG^>jLW3sBX_^-yQ2KwlM=%Ffn^e!LqLA-i2_o}vJD$l_K{RIU6V>J3N^Q`rf}>MOJ8k(=bH&lfeZNHGx|wMd|4qLuUz4D_t9i6$BSt#^U*$uP zC7pD}D8mY!|1&Pd#(rZ*J%kF$&!I&xLZeIG$eBC2iQ+{>7GWD5b|Zw?^3x8D0*Sm0Izqz*~y^wr49bzNY^Kbsg)W5k8Ac^G7cx{*~v^>?148)~i?wdrM z6ZFsRT3%ulH*yBgATj~h82@ScfFbI;DYt>Yg$U_%%kMS$k2g$pcguehaG~};{8`1{ zuN71KrzzUVoAzHQZ<5@r9lA$XEcyF01?1^nu&apyL47Rdn^iYi-OaDte^Zc9NFIY6 MhkrJL`tFwhza}_AS^xk5 literal 35480 zcmcJ23w&Hvwf9Mzv}viCv<8e?VSqtXEg{oqQhCg@NqYj5HbM&lDwAn4X@Yq%k3Nu= zUv_uk)k zew>`M|NFnzUVH7e_t|HkJ>|-#$kGWVC5|~toHLz#%Ak%@eNFy8#J3@5hBMs>U*Kks zrF8iG@T%s_Kf)_lo#$Tlay@joqtQBdbim)~j+UNM1B#ouI#9zI%WB+gBv3Wh7^osy zH}k5Sd9HD^z5XC4k=Ly-ceG3)T!x!z40PuxgI<(DrZkVH{ZvXdRBIbiUNxyJ$e&q| zKQouFi#BRDXdT3c85;_8_zQISb2^N0pddex%P%NnYhVZ&vY4wGDJZF?KvN9_RBceT z*_3D+dBJzx+DW(xz7ok^5ttpx&L`Y=!qs$f8Z&=x$-EuOysn%V9^O;J`kNYFPXAvh zsha?jmaWUlhW@GCywbefy#Iz={lvUn{l6;bj$Iwt0y#R^)}L}RC*Jr^>bbWu;4_u< z<)ns4-BJBxMzCM&T}ngv!>vk-IBZd^>4T6{Px8vN-Y1 zLLw!L6L%C6DOsF&Um=l_iU`+7>?%Y$W%l%fY`A{9M;SW{%b;X&V;(IeQnEO4Um=l_ z#fke1iIh}CRBwiD{|pZ$2MU!?vUq8Pigu)AuC&&I(hv`=$jxW3w6j+hWVhMbZTW0F znhqmNSV76+=DuA>q-1d-J<5beN){)U7ZNF1oLE^%q-1eobs>?Giiie$e}PjFfbaKE z;xAM}$()ka1xnB^tLNnWfHK~;v}r918HPSMNNW4hD*gnDkLUH7Wp^hCe-)8W8@>m@g4uShq__eA)Jm% zyTdP+Gc&l;9o}8OO9(JOd;umKnK#3$c(MVqJG!ihABcq=!Z+`fN31??ZKi1qZmYi}9Zrp~k#08Pe^T9V+1K&k5s?ow@ zm{Xv{f0U$VWLKfI%>{+2+AkR^t>r>8ORA{GqO!rAXadxCtaLIlU>0B9;hm+ChJ)#Y zFMUPjyB&KYqwB|+otmNzcXaOJ5gt{)*o+-&cN<39YSQr~f$?>mmOeaETX%xk_`k$~SfYh|F) zH*01QZUP4=BY?d5j)1{wo?{EY-EBPFjPWN}{=XufLCK9t~_zO2qSeMQJO z{eo82ZVj)bA4U16aQT_nly#?43aFY59>YhrbSK^;nlmH&k)_8?f=4PbRsaqmm(v@NE@DcrFr_h2&Zby3c$G_JlS19NAELfT zS9&ngmF%Rewr*%pSvf=|5l%J~DLQb}qS=#sARfYqK<*gYp^ME;C?W+;rRFeBMNOJB zW22>k;kWUe_!UnLWAw+?aX2MLZ9w=}c=iW)3%H*OfFi{5AbMJO;v`f8LJy8O;B}F0190T_M+QWc6J97t2xo2dxDyXb_GB)l?Xp zRLeDD2rAx35er2E^iw&(%(k0MOS^Upnq-;sh+TXo4V#&-ZveCa%P0)i#iyP z8ba!5rJ|V904zh%`5u|;78oL_SA<&0YujNdT@UN4wqr=!F?2xpP%?mcRCFVT^Mc|Z z$~BAf+YBhf1BM@0xbk2<7V?(^MvM8Tm!ow3aKcQqDYz4chA)VWrUG@UFGbWO1!Edk zU35{eE1u5Myv%H4$++z1Y!mf=H&bdTYF1|Ye%@%5p>%5-m8T$UY&Uh$Wt-IxR04Go z1<qD3-O&_~y+Y44tgAhun{jM0b#SzWntGzi*iG<{n{}(Zd{sL!9(4I?7$tuh zhRO>0qv5v3(P@FEk;i=3eU3F@JtQ)U>42NT3Leda$hnz=Effgv97!RUA2VjGY6Db_ zd{Zla+$q1Z=@%j2Px+a5faX{j?w-pnfn9h>W5et~`X(y3(#0RueOx>MX%bk)YnZ-~ z9f%^D1frAW=ePX!Li0h?jOSF1qcia&=Ows0#9A-)7yPYj8hLs%R)(OwDf4RM=Hdv?VYP$z6E@Q2Uguy2HJ+-t4f`X>2A774-T2v!}rlDzsHO5Rl7;temBm| zPexYOh?m$t=a#yo)5UXprqL!P4ax=tr*(MLPa*PWJSC&pJC>^#GNfJ^dBB@FQq; zWw!BD$L)Bu^K}heW!@?RdQ8HS}*Hv}6PKyB+V9yy|v9#p(B>sJm(W zz{r@w`)kMHl7sFo1h!u7Jl z9eKsK!e9Kva4iZo?o6<`8gNw^XiL4?tTuxfE%BkuhuM^JOhcxl2~ zS%6xGX@$XUz$%q*q?MKoP&ZT%RizpYQf06RfT1aZrop;VBoGRZO;nYkqR8Zcx=vPT z2~}jQi7;o3|GL@5?~buyk<5)8NouhK?l|^cV&G;5-=K|3kf`EB1P*Txk4+%8{Fa5B z_v@V70z-`AJv%3y&(5wcgHGki#^YFItnmYUr4be`ZLHLaR->XAjyB};Q)(Elc1sTD zz|3>1CS#4$l@^E$a$j-bk-FRxDrO*fncK@b7<|+NR_AK^R@IgCCjwsV(DXkRqem#J z)OVCTH~84UhN0bDYBIDNsu&qnUK`oz+p-GIp%!h$Gq@@ELIh!5N0GdtIrFTWc`1_F zW5#7j)-}>x;#rKA`Vp}_TUw*5Lytes)mb3ra+lLEVCUxKD(#${9L1aesJYdixht}z zr`eg|;oW%J?orR6#)i&->&pfZ>k;ayIhk~!$&EgfDer@pX( zCv|LoBZ7z>TOJ-hf>*~m=$S28G=u{y)jMGG@b1!<%zs8k@gl(MdM%m5_;eP@=tibV zRrBy(Wl~eaYw71`R)t3<3RC4PTj(K1(*b;w)VBCBZ@>U*US6mUHI@Vnhc)0UTZmjq z2I)9Mfb-l616K_n<2nU!W{N}<`F$1!VGYa!H8{Vds>>921d-CPGrU4ysqfb$>x z>VYFinn%}ho+9c5a@EWU!H9eH_EEs(j7cm+V_h4n5CLY?Yi*nBnnx>|N6+_@tjUAI zYApBDL)n@;a64NuM70ZoAben%(;H(KcMpdZ-#DVCa~}hG6U)dD^#y z2FT#kW<8GPQsK;gJp3b>H&*3dEMCQWG7IJM3bNdce%Jf|n$1SNN=2wwP*_OI+@|ac zFKIy4HlIXQK$bG4NEO;H8s1exRA07&o}hZY3A3eY>oZ8BY7bI4@>u$#m^9ZnXP$6J zKY-67qgP@(0lxs62Z)|-W`4%)7kmt9)qd=_{FGC)Uhex)Wpn09QV-pvIo#Tud8{R~ z59R2m6DzOY#q-_Z&Rx0r3bntRpZjtQi2~I8_@CtCrNGVn<2?0FaHSuyqT*w8lUm7e zM>nbdi5tB%FuF-ivw`HuYBCB$J!|k{xY+I36@1#wR)r$jz&3oJDaH7;$#qN2kg=&v z+_&T2y=oV&Lvs1e4bPYsLUlEvD>AQXv zy`kxRzs`3vRRQK;#Hpe=qQ*H+hIf`U?DH+(7u*@n{5Gel+;{z7sI!{5%7;|b$eX_F z?m=F2$6ibi+$_Jd^PuErXYN4;KA0(Nl;5|PXXhP2!81p8Zv6O57r<|Ly-U?bGOw~u zetT&yd6rA2w>5fAn|Z});q0=%W7)ZK5*A?B|MCsr?0p#HSptTFW<;32=?Ph*;7&Zz z)LS1-7s{I(cKb#Kcmk{Zock=5j$pXH51!8t*SiWeu~~=yiV>8{{xMqEG~c+PlDVQo zaDT}Ut=f2I6)A)rfq|hs_kCFPx5-v|84-ra%_@IcrIoI>(sfqaZ>8(4bj_}=o(=KN zXm>oBbk_C`tcmsZTpmmH4D?51y@TDcXkSl%Q9KdvOm`5^%1tNZ(eqX;an|>wx}(W~ z-Zbk<#`}}e^@&t;FqVk*B~|XaSZ_KWwX+IkE*p%dqP?-cB6f78dwZkFRD3YnnU3{3 zog4dOeLWq?XvaWr??8vDLu%t-JfG8_?qh+Z4aL^RB^F-=NbM+n}lzhyTgC9xgSOg6TTvvcY5r;-G(RG`)t=*qUTC-m?}3nGCwdqXTO$ zjUx!wkqL!49RvNzR3g^XpGrFYy@TWkWm*CrSi4bTxk!q3#X8_9nU{=p#Zw!LWGJ_w zo$ur^Ydf6f{xi<-lW{t4us7C! z>Y2+CJA=K^b)E5O*Fa*uN}-{nUFrUOR;=?>`Zk|>73%65})Gq52U&gl>THq<=+@j)z(hMxL7!5 zA!unti6uI^qcjpF(_LLX9X)ZhN@qOTfh&!rbQH?P2hlu5xhhIJV}lgt+_ZK-SNdW{ zq0j)PSdMd4USFEWIpY2kUfsW>5B)PrK^RMj4%)jWN}~(Li>~O}M68o~G0HPhiJrjG znjS<$$D^s9KDZI1Y;uu*EnLv=$0&-C&OgxQr#wIB`D@PbpVB$Y-`UgEh5qF4N(}V* zQ{8cYM>>&!*LyemV`@~6cOFf_iBns9i9@#gTJdt9A4(@3kvB6}zH-74wMY^1#jrW7HBg%15Xu~2Z zRyfGtlk_iN6^Z!Qq*Gk^yr(d545TP_F+Ri-i2bCe$NnXuGzOZr{tk=@{!0~CXaFC33NXmP>f#-qDoBz7Wq?&jU<&C;7)x?52=jx(Ex!MLSy0?8c+122#kC@ z+ep^>n{3SA9ZUKPWQo*6{^lWqEl}eNjoE$*RkAQOfMW3w?wyThp=L(h^`Nf(+<2)t z>Wm1~5k!O36g&a=>B=|p%ck&^9x!}EgXt7Oq`?RLtbdiho7mEMFi5n1s>4>*1r z!u@eGG@^4t)snogbdwca#*Ggch12Ref*E3w@k93;(4zEIGs?brD;?iDtBa5SJnPgY zGhcds!H3Se>9-#}cuei|GqbP!X!~1xcbuvCRiU3|_J84?CvQ9b-EY1AWXqb?v$UQN zUoU+-r1af?)=wUI>UYmw@$?xtb#DCQmH+t08HyJO(b_hzZ!>V%$a9L8qG>Ii7KmwW zo!+{M%RBCHbq=)cqzkSgXW5b^i~KdK)};GWX@78TZLqfP)OtKqkqmC0GpDw0-YgBX z2;VQM0uyTv2z;q;N(3@Z@C`hbK&Ya`M!QdSKy-8gLd@R6}NQMS1fv z5GNaGV+S1->cV9YiF?Pbs63~l+*u8b*0bpNIsI8mWkcU}6Pu{)#f4=zR)i)bQ1X&- zB{v<@QBl8i9PC2n+oTuAYI1&FKi92@{+}1ES@;;I9^#Fp1IJ0!h|rZKTenUOSNN|f zg)fH7mQ@T+Xq;M610-y`(sT^_WJt^MN(;oVz?n@$tN`$yEqpw zNAt<1ing;W+9%w_-Z+~R@DDt5VX-hJpKQ_k&!aNolOF98%DY47ou=}tTPytMqnog; zJ9VBJE6D!eXw39&!VX$k4X}4L_6X^PueVM>bY3&DvBE!GI^ph$>LuKL9j6W1H8g18 zIF{Nc-!8@a%d(35{E>?LLO9za6^G*!Zz%EVJ!<^9TI;T-0#Nq>RE})EP2&^_bP%>f zW7IY1AZ&LLY=05#Y)betu40{C&I<3x3F&c&5*a~yAOaGfY{19$U{sfj@yM?DC~bJ7_uq#`S^O8Yo_8$#&stBnrANhOS#EA* zvftEt=IFVE!Qa*RnHD~Q%PVst7S0<|QQtNTuh9I9EnLk}CpoI$m{J|*L(DIC2J;Z# z`L&)7OV39&zRAK>+>~Rbvhbj-71S;_S^Rp9f8D~*)c6hyZ`Ak$7JjzIAF=ROjqkSb z^EIw^7v*hT&G;0aALUWUQSn{o{LIqRt@Z4)@P3V(Ii=C1=I~|Cev5y(=09iQLmK~+ zg}{A9)_IU&9pFF4M}HUAQe zAJllv!uiLp=)av7ewOBUTli9qUuxkk8t=34m5fhv=od5cP~%sbv)SUW(R!}6@E(m{ zXW@eyzsbVaGd{_&?cc2V+b#Zx#=mCaTQ&ag7XEKq|D6{8MUB^3_*XS<-ouP9lwZo6 zZ(IEDgI{S#;!5LMF`V9Ki{bA8pMl9ns9Y&AYfT?SdBymC;KXm$eDlsc1-nLz@u~k1 ze^B#(u5G0_E5;x6z^T29@xSbW-{FDZ1DxxtU*&i`3pV}Qga3OE{GiseU4N+HwJhkN zUxpO3bAboG+yhT|;MaTL-}S(s@W5a5z>mkPU@`mWd*E#zc*+Cc=7Imv1AoE;Kj?we z&#sHv@AtsZ@W9(W@T-AS{OqG$(m3EoK7OF_0~+VGNaQ@>q34JPJ_-G#nEe6ZT<&qI zJYH+WZP`7$Y5WwG?Htkg4vq7=De{iP_(FOr zPf|o)@5JqVjSp&^*E)e;4BV8ZB-Gyxn!jE15q^B!qwyUYH{#M|8PMJ@{)p@U#b>@xZrx;6L)ff8l}u(F3nQKc{-_JXuwU*LiXK36JOC zLd`#*8{DiDoUd{JN0lCQ2R;%SuhTffgOBSq9cZ=CJG>mYu`0b69pR%g$xlxynqIoy)RwS#~bV&SlxTEIXHF z=dtWOmYv74^Hd#Kb{@;lW7&BuJC9}OvFv=7ozJrKS$00l&S%;AEIXfN=dsiSBg{)yAYgow4g({c%3z=WfoORGCur5ZT5fpZ&_PPVFnIStGS*0Jhf9qSI(vGQOYYY)~jJy^$jf_1DY zSjU=zb*v;@rCpcHtPX%01MN<&rWcHBh%_7tdvF|G7g-5Zg z3)>>r#?dQr?f6Wzqx&=1*4NYPx#33d)xmy6+Bh_x)|H6IqlPqYh#0qfDQ6k&rxb>E z3?#AP3>#^%ZxV2HS!4w^Sw>eZU3y;A%IM1Q;z&~z+tIK~Fag`Uc!ysXSwG$)F)Z

RX(+CHG3hg zDropS*$E4lGhd!%;P~Y@^Mbv}3b446hV-Iz*zy?9uA9H$9Z*jP=%? zqZHOhZweC;ts_ot;N}flqAHrMVwm>Up+GZ2R6Y!RYS2O2b{1l3gR`uG4Ju;4Soxs;6J>L@i>4JZ+2mdj_pCR~ae}>xEDeLpPG-mi}|A8vETHt1X zhh6S^q345wul50quh$)d{~^KOE$qBr;0FYs_EZ@=)puy6U-GLd&~VUYCQiejr*X1h z^38Wb%KnJp_j>TJ*En5hkCxG|zAuiqXOG~YAo%Kg-gy3D!I$y#mcac&&kTGQp~G&M zfW}GeM4ZM>_5Ex-e~I9qB>3uk*m!^qX>cQVG^wUz5vHxxl z{(XW^OFxFcN8le5xcN?Q+xd*(*9gA(j&1V~3;ry@FC!uj^8d$i8hgxlW?K(^$D)Jy zQctbGKY=`>=aU+@^(+>AsmFYeCH=F79`n7_*7F$;JwpONRp`lT+}3lu;MWTNcRlzI z3jS$=|ELH5alx+>{1*fs6!_~Ndfpa%{DwE@=lAIc9{Fvqz(1sM5|iWqiGn{*@ael6 z9i(SIPScJ{JoKC`_zMI-D)5B@@Ac5LS@3Bs*Oa@(gHPY>=pZ{Aa2oy{0$(KXyFK*W zC-`z4`=!7?DfIl_L(j{CPkU#LJ@0t%kC_A#4zizqq+$5g8n@f) zW)FU=;5Q3?T;OL5JmH~-zQ58zcAkUN*!h1v_+J^ zP4F)i_>BU;U*lx|a)FNt{>=jali;rq_@4#;3j#l83KBTz*?22Xp#IN3=qH+BvQoI;-Fpmb~%IPLW} z{C^Ypc>=#h<5cdK1-?(od)!2^H=&us^R|P&(;9nE?Y=M7W;By6jhrkyJ z{2Kyq)i~LIgTSvB_`eJMHlgR60{^za?-ckw0{@o4?-%&D1-?_@-x2tjz`rZ--)r2q z|Ac8E;UHYvKU?GE|L@^6{$C{Uy96E<`1b|w3VesaR|))Xfwv3%9)Vw~aohgy3tZa& zpupw$Fz!@Y{v{9|?Si;GZw>2R-xLH5Y@`hdo5{Xv0C{UL!%{g(;-vc0YpeCf~I1upg9C2*<#5usnU*FM3Q z`u7W5>VHw-Qvb9W>>l=y^#8{Mz5o;xhl>PWC-9{jxBaVHb$HwZfqX`JGLrdp<6uM2#&z$eqo7!EtmPtiE(`7}sV+-+n6ac46l(fyV^?YmJjXrGK6k zxU~PEz@?ofRZxI~?3DA#N{!p?RW0~5=P>P6D{z`B8hnw!X=-EeVWD5ze}mw!5&W-v z@cEyX29NCT5d1L@{<8wVQShsdMFI!uk@JdA34A&7jQt%NC;!|m@XrZc`uX!hkJSG& zfv*&LrW}U^4zgzjPE+m)8mDq)|2<3KvR)T?;5`DrSLpwOz@?sV3VfU3-=%S~lioXw zoj(@%xdMM!;GF{BEA-3vjwv4i35Okri!@H<#&Mc*n*{%UfnOx}T>|g%;BOOrssAPq z{%(Q)MCgB7=vgc9=LP?#0)JWH4+#ADY9w%c9Otb#O?^+*IQgMl;HPVx{3hR92L%3* z;BON6mju2;;5!BWjKF1_Kl5Rba8S7q<1}_2)HvDMBk(r`{}F*t(l4;Yzf|DIXq@y* zzF+V^BlxFy@aGGDui!88;70_%Pw+qG!EYD*e!-6m{w{&11b;x_8$I|>2>zhp|6cHS z3;bolzf9n7d+?`z1SA~f{{&9cjx#h){+Id#f}a%pIuHJ0!A}YPau0sH;HL$@*Mq-F z@Ye}`)`S0j!Cx=#6PKV^4kWS#&4?yU;3?2@HYy6)`Neq2mTum zyzB%h#bLMCM?CQP0+;nVOW^XkutMN_QI4r^x4_2)zR?385%_(Ae}lju75JA0{uP1W zCGZ~#{9b|oK;SDyy6rjaFpVd<%T`*6&fdhN_(On{96V8^N=y^@(mCEPXvEd@E`Hu z|3cuhKRzLF`9AfM(7#pae@pPCAKnxEtl&@mC=xhqznx8s4jPy9_HyywD&zk<;(bKo zFP@Os^JCl^zg^+a;|~ga=Sg||Nr4{__-_R6S9BgHsV&Vxb6q+JqtpB@`)UGl7~K5+ z`ZfjUaPvFscP%_biy1h~zyD$MnBOm(ePsqWzi+*zM9CT4{JvH02cx`w$0^K^%9E5H^Y026dj>Ub{#_7*o8Lw5xAg4L z{Fg0!pT;==QIyehSmU!PFa>UYC%DDJ&F=xXTe$f>;14a_{2tKk+cD*u-vd5l@y+i6 z4_mnTJz!~Bp`GUUfYlameh)a?!p-jio!aEazEo@tuBn8&cAL9Iyf;>x!bW1Jmj4mz z+BNt`>uVDOlpuN#zxpdk;+Iz{*Ia8mlBpa>3Hp&0{=HTDr)`~D{7bj-TKr?3r}9sf zbb4)nx)%SS?jU}_vk_{@gm`zfi~gBfRpbBNUq8gDYfQ4#gmn5=JwBQ2N^_&Hv@AeP z64GgNx$`1*clcxK>cfS~(ZgEq-$f(!gzK=xojQJJm1}U~%40SXr1O6&AA~IFoP$#y zrhNXNn2QWkzPW~wS4{qvPbi@l9cM;`slSn5q2($53l1_2M_65QfBm^Tb#nnOq-%zS zyIOw8!cF<++KK#P^82)Woh5JD-&_+~-sTyHQv1lm)Z1L?9mCv9-fX);bf(RvXZvKF zw*T<^NIv}PlKX4wZGf-ihIHEUty;d`qy3CLz0($p|64w%82hwhihuglY|5`z*+t8y ztSt)_VZVOflfSRSY0J~&(2jkQY;U2z*aJm{io^EbbmSG2?;cS?-xIQQYs>#XVEgTJ diff --git a/control/velocity_controller/src/build_auv_solver/libacados_ocp_solver_auv_model.so b/control/velocity_controller/src/build_auv_solver/libacados_ocp_solver_auv_model.so index 376b79ab908f7e432a3af6a4b64d541dce609c16..d06c5638160d6207fba98e78bc82bc9985e10b0e 100755 GIT binary patch delta 22469 zcmaKU349bq_J4OL10fJ*0^|S^2snX32qYYu$d!QvCNSYpl!QaV9TXu#;)0+PgzRPv zqtS95R9wS~E?z4dSdK*rawMp$fT*Yl5kVW|J_CaD|Gui~$;w7I#;>t=db6Jm#1 zH|_m49oMX#_4|}1-6E@&KeYKB%d{VN?JloAtsT_VwrqzbPW_&J&?JtXx5TRxcqyq# zyn2d7H;q&Kv9V3!)L|Sg1^OBLpjn*y8-21m)RVaW5!B;JMlB&Nmf18_EoP54J*TE} z^Z}A$l@Y1xN8IX4Ztr|Vyz1cg-eR4a^-{O9vH1Mdzov^Cr+&@_CxhKfeSrOh&s8L9 z`5FC+W=^fOn!ucqqdhg#L`RzmqD*X|D6N#1%KGC1utd%XeQ?|k5Rvsu+${{d9y-hZt$Oayk4a|@|sF^5$sZ9i(C5NU`I%-6s zBRu8~NqM6Kf1`%cZYkPD{}sh(c!_KvNruSiS&fWPsf^GkqFv>Z9Lq7XVZ+XKnG>aQ zXbk*c5-(^RO5VQmiuAP7A0u(YP6N2SXEMf^ICnR>ig_H3W* zfid>qpoi4(7@54r*n6a-MJ7j=az{N_>C%NO#=O5F4Xu|7Mgvbshe}NjeI+}PY3jf) za^9y(c~trJgG|!Yfv8gc8=)H^8Kp9NjTP`^$WYJrNk>29W)$UoxWL!TC3ct`v)wY( zip&XP8b2%XOgRJ-5lKG(`pbE_UWU$Sc(TOLOWeqbXJxi4(orMShopRw92#RN{Swcd zAv{MQ`BfW%_Niy*CkQ}xP+1~-RykbYM+H$LC7wy4!!=rtO$1sZJE^kAM#EF&*cZs5 zG;mE0W#&ZQe?>7ox+s&&_KRr12pyASoMal~YKf;x+!)GH#t_I*+k`pg57}_4;gD=! zX)i;h$WR+0eL*@@bwXHpSQ_eSVnoY!q}kJ-Kr1Ao>STR_@*yrs1Yoob?54x@jrHKf63b6#`Z$! zom_A;5SUd-b=UIK5&9js*zcJS+G;*k6u0-3+xv^ldrptJ#pXB@n)(6ipWI?AogLdP zh7aBgE?-Rf9!2>WJy%>6>1j3k^&4!rGb8%V|3JTDc9P5cyZ1`%aGS=gL%L{PZ?LcLGcwI`vG zfn9l}g&pjJ#O}7#Zqx5=U@fwTMn7>a5Xd1PPbU2l2XvV)@K z&9bMvDuz2UiqfrRH!uTGiEvd+NzL&FTov=v-4$I)CdpNHz`CMigrZbHUGD-{FIsJu zZxCYF^l|ox?6dqBT0p?yz=LM*70<&8-;i%JS~V(C7MkJmqnD&$_%v zv%TNsAn0zNr7sf7?L~NV)swmpmLZ>)<-GuTaP8m<4Pws|?1CV>)otw26v7Ia%31!2 z>dt^8*0Lo5?kn8+815_EJ@m=_v+3s{pSBkbp@kgp4m7uKx1!{Dcf&!Qkugk$iHM?5 z&4i}D4KVtiWw&un60Wd(TW5HA(P@p&cIA3c!dN*smJNethoB0Qq+(Tt{CdcfWLc%a z3o)SpuQA{T)FHkeL~kMeIC$0Y!$p4dGy6gAw>8{9iyk6wM?o@O_$Z}@h#sO<-w&Th6_keHjqlnJOyLdOrs6!erB3qkDX{NN560{XT@H2q3C~Jhl9FKRm zbNL^t^_f9f@txb@g_7mHE&_Hx?2FN$0f-?@Z$q}vqh@FPQSu!eb$a&*6}|3?5sgf0 zrCZQrF(5g$BEZ?M9PjM|fc_)Wy(Ql?0CyO0$N(UVgAQ)p8R}pOdI|@f%WG9<#vb7y z?~vX-=p7%cl@OHO_j-0CVl)D#aX~LAlr$`}DuWHWZ)?!<|84N!myK}n0Za&KklM?1 z&<7tY4-Hmh1Lr;o?a>?XJG4i_X#^hX$aww!U^5nq`oBn=#x`^cF<2B+2(whb4v}eq11ldaDd+8B*JDiVL4uv*%XT4bE^oM9zX&hg-fQ~X@R5S@cTnN=p$h>I zAZc8BDF?uNl)N90DM-B;%%>t;ga`FC*jUgB^o84J-2ORzB-zDHi=r>PU__&d=r)G) z7E;$)J1j!uNE6)$EewIjVu9%#Z>`Jcil1d|Q-j$s%j&S4gE%z;5zB8V3sz>5bAoeX zR>fTGF=c10&wK}KF0b1jkGQ-y^CsKPv-Hm8>KO zY}mfyDdPPR2RhyK0o@)Bm-IR3jmS_OYL%w6{-XE#GFYR2H$z{SFSSRgNEVkb6A{Kl z!`9Hmx}pN2)X50mN#{o@>NxadnR@CR4{y`pt@QI68qY&NY@#2DydQF=xxAjE`nM36 zK=B~~-aQ!j{bRWh9Mw00pXJr_oy&Kt4`yW9<4Y>Z1I+S(J`w$KRp8Vx!_{4!A*^K= z;22J4E<~X5fOawsl9kAVEl5!8$v7iib^B7{J!psenCkN3qwK(weO+FhS+c#?5CFOI zUC1TJXk|c3)AFik!&p`Z!c=mECXNjrkU`%f)ipif+VN|c9(InWmt`cM54AG0MA~L1 zfu`aw(;Cf2jY{yuL5L=K6(UNe%4)3T^(b>!c(%FWR6Gtre0`ylOs@AjojQD$p&x~; zn9k*U)STtD5&C2}n4NLui9>vqak$7y@>Cp!Yh6NngK*whXLPSL zH%ai}(Px7v)2+dca}OQcqIw1SeE5c*W6j4TCtm$G48}no+2Xi8TOK{ba>vAZ(oY8h zWi=&D(`x9rzmxB#aOf*2;$~w4xMG7%LW%BSoa3DjIPZ6kyZ=7dveWV8J;p5^b7q_i zb22aFpt}N7jEpS*qfPN-9A``Nkee5Np}QLA<~B6;z8Gx|0u^#|^YCw!>_Rgn-*;op zb1mCN&27e?(wOu+`Rt_0l3p>%J`T%YCin^tGpPu;_ehR+7!FRgdhfHWq@)=1v~k=l81V}!5GN33FOkF36SU-5fNuKH!JFT9y+ z`WNce9Nz%1uZ6OsUUN;~OZ|@^gr~A^z!~2D^=>D@%ki}UjJp|jWvsnPybEE3b-)MJ`{DGFswX-bwZX z7rL97RzrJfA$1?!nQO-^m}_?#i*rRAyR$Z2#XSbLg)EB_#tY#{bGGf?mRYo4f*(c! ztqs?R>!M`?7>XPtLYNJFqns2{j%O|J>p#+AEgOq7KY3I@hm%55DI!-gY2iw8BLWV0 zh1>3O)`kgh$QU(l7qqEBasT|neWN>A0J^#JS*>r9fZMy;UP=@##4~MNZwU4q*Re?w zpY&|PxXzwqzmfxJ2T8SO6U7KSGwv$(=;h#GmWa6n?mOV>3%U48dpWEIIWG_=pNsz# z1fL=>OInx52UwX$a0=iwB*0mh2hI6nCKK39H$aZvN7xkzVmK)V2t<~Bk-I|l$Xc#p z7$`uCu)k)f9d~1FQ)y5gIo=~K?{9AJZoULT2{0DuBbdclJR)IslrP+&zl`Qw6`pXr z;DUAeUu5wb=RN`MC|_0z&RnDm?)QSLPv?4B_J|;7cZgoLM2aF$4FjzGNhho067dv&iNB(e1tD_6>AFBkh0~ z1WG9f-=D6#y)Eo~pWqRtyBc=|?!=$bJvarC>0Iw=xoh_n&ngsbBlS2#;0PwH(H5Jt zd{?->fyN}`Y{^mDG&0GKXwYZ*5?$p7;szIPQ86~SyR8l92eDpIB6pJv**Ar(2mV2r zT%d^^p*MvfoN?o3g`Xm_z2~q;h@%9Wf_RqqhVY1o5gm{ zMG3oH7R|9181jq4gxf=2>I`DUERqZe<8*UWUn22>MjUI=QN5ZsO3yD&o*+#uqOh0a zotK_d@novL0qmTL2XSs#X8u@mLbstO7vu)E6@YW?_W@y7X5=q}BouQj>%f0d(nq z63rY!t{5hD;INkqNzy^9IvxM?+z^B7NQO5%L>MH1F1;(H;KsY6s$`cPw?5Mzi(z(J z!0p?>aU7_+L8}s@m+L(rTvzy>TL<;*jGHAt!^V2pfF@KuT$k$5jexE=Q2s9iiW;|>>{(%_M^mKhq0mtU$`eKFD~N?9>Ayo*ZWuuNv=CE^atz7R01b?64_KgKrKiZg{>Vi`L7E%OIvgq$L3gPjpZ0~g; zL;zj-pBshtp)Q=t|0j2f0J`+4K_QR*6#(ehYX(A^w)6>L<9XqT=!njnEHDm%Kx5dINGMDB0d%gy~~w@G8c6?0Hz&a5_Dz zPZ2)qzj97;2hZv0_ipJzc zaWkzy{A`5^%^|vs>2b3CExIS=H<&)leeXAJdbEtY`1aER$3)x_>S1t@b}5(7l426f zH1;XJM?H-YkhPv8!)qPdBmsg53=b zbM-9ndCOom|iIC$x_m6Ev#dI?9$!h|wN6!d6YVyLSU-9i1^t5CVAv z<^7c3FQx58-Pdo|+n5@0KN_L>@i0>-_R$=N*mgrct&g$Vr;6oJ_EQ=$;BNAoH3M7q8XZGn}lKPy-c{pw++PfT*Q z5sW;SdQ^`P%zlMO|^a~^K06;u!Nw8U;JwQ1mm6E_eg+!wJnh5KJ4-e+1pVGhm9-B+qnI{WJcziw1z!-Ly<1Sy~F0uo4Jf6T= zw{Ja)31u~ly3n~9k1Tt{;@~UBq=p};)U&z|rs&~D!V@?W_lv?HNr4<3QT6%Y=4PBO zd7IXPby(ekccLYIu>@@H4XAMq5Vy=*ZPYtozezYuV0NUy1UIQ)`VKR!!(n_8)W#ua z&>YCf_Vg=YL+$E2jnJ|0*t-v|){5U{Bc{yknTsgmxdA;+o?*ws(){G19EryC<Z&`;K)lNV#JM8km)1eG<hlh}G!fRV0ypri zVB9d(ubb+(W?A!s@~(>ZuyxabeitTww?>zs4M)EpL_KlnTcSh9{g@3e9A|qJCXgj=`xt!X8Fk0&?482CTFL?TQ{iy!_UMGco+;Bi`r5%C6HxB#R^K&v|Cm1Zs*Oh#m)Vu6w`kYBx z-99$#p*OT{f3H6C&@-xM>T5D#tdqW^Jdt(8z0F=DHzo@i#9NFj)!h}3P@?^AVEIA@ z<1EHW)-krCK1c9xS)=<ZzLkLU?j`PbQyg{P8OSGS#_)G|| zwG^I}!}nsM0t zv0BNi?4|krwYpmN&HR*mU#jIZd4)X{yEC86nR=Olr5l({f7HM-4Ga%Y4a{X=@p?`z zyT53a)^&CDxuR56v%JDOEf}S~S6#GVxT=-BRJ~NCt*(%19Six@8~ z0e@hI=bfLGg@@}e5`?7zeQt9Vyu1buKP%xKsO8~Nv16c*5Nm!zqfCXA*%g%W3YkB+ z3@tO3I6|`V9R{HBlN+qj>169EA&2qLPy9w$$>RyMm^1Re;z5Qp*U6cvg+e61jvC@} zj|u}fsUaaV|3%jEv9@hj2!>^)pvf-?+HW-*`&g0o`_Af}k9AVD!kz4wKb_Nhy};bd z?y>&PFdPWMG4eR^3QJmCq78qZ`4|7Y&8N=`mX1C+b`w8kJDc@*kMNhlVCwRRK3vi- zOxx~bwyI8ZXFN+0BIg~6t9&RU(mw$B9zcwr0Bim?hJdW&8>QR$2Z@i|NbJhx+dzN( zSJvqJXz`C>z<5-j_pz#~Bu^g6WLxw1ATIg+-liG?gjLZIGnp!A|0`l8zAtKd`%rUO z#tK2GL2GTO)^Ms=^S{O4z*lb587OsV+9X#B7Mt#AZMzF)+Eorx1d~&J?9obl_1m5* zRo%))F6(dmGrpoBmI!WX1?#jph3#DSn>O!hw*IMJTK3cIr>A1Ieot4QVJp?@z{>WT zHm;1~IqLeK$r};RlrqtuU_5&e5E4&W55`m0gYn$PqW==xJQ-R%g7z{V!BZ<>B^bd0 zWYdV?IbsDP=oIbC2>whwbc#ptu28M-h$AB?eU>_3N*x}-&cYnq|CdxfH&bK2Std9k>G|0*9?yR-3yJ6VODUcn&y(bv zh-dXu(Ys(gKO`U|p0XZ{r>qC#IfTm_`N`;pky7*>z&yZj6Cc^O!EVDn;zh)mt*^3q ziUDJShC7{rkZ{X-Fx;{p40qi^k`IPEN$dhKX)wdXol5o)?spr* zeb*|R=PIco+`B1TQ(Qtwg!^}51;d?7EG(vCg*Zn%Bfp1)YMtPIh!sM1O6vR|b$GaY z3Uh4ts#x28FdBpV*+QP*wwI9K?gc!*;}?jCiY21e0-oP#uOPpV75!JNzb)dit}SZJ zZ}k^ut?E1)XNq9Fw?8a87mWAE1cby})`Rhu^^01wrDP?T zyrh? z%)|NgnzvMK{~UI$y0dNL91*ln$??SZ=df-YoNV3ef6^>-Sf{nawGD-=$HpG)#kFbL z%tCgC#Z}j@tqfDK?yT!Ps@qI*NF?p#>7q-)AWkA6B#5#e45F+DgLs<#v@W*!{m|ku z95mI#*OFmAl@s? zu{CeS+OB}n7{qhaco0v&g&@wK%KYzi)kaNaHSe6%78bB~-fgFiE?~9q_SZTWu<-Yi zwXgz;8mnJBvHI2b`h;oriJ0$gS?-pIQF|wmDW8tmzoOh2y=N!z=(#^c^sve!1+ACzxX<|_1w)>|-F7kZrO~1%$yKcEqrTeo(d^$K14mb1 z_~^qh%{hX-u_MLPZUnWS_cI+#J5U+^Q&t`Zken!iV}?Wmm(Hh{asgRCVbr(fkhMua z(<#*kS=>b)vPO4CHMtq7BAnaomDr#t-v&N7LSbV+iS?Y|TEd5uROUWFBC$VjPqcMZ za*mO1Oq$?uzrW`CH_ZXp`~5jecdPw5nb&*Zr9a!ZcVj=Kq0EgQkGMo+V0H$8;@ zTVDNeXSJ%0b+O6Yx@$dL?CEVIS<3EzG|wC^BRhOJJG#4%7C9WDO<<{e;5}@=jAgcT*X?gWGv<%aO860;ddMCjsan?aFx=ct`RMHuuxE z(VKw7#d>S>Lv-Fxd7r)bX=l$1QhMNTRFI84OJ&{&h43fA;jNWP&M6+kZ$mgssC-+q zi#eA(%}@M~(95gG*q)}roC^Tn#MV>P3Q>Vud59VW9G1%J+|Tx@>aVQBe;!I$J(!0o zA1RS{a+cfrhEg8gK|DuytIH-Eh7v-AzA{)IwSuJHH z`d1>bz(?!9!t}r?8SL2uZMAV3?Ck^XGF%Wt zFZiA3@93l>aTp=!?RN-Mo`8lDglT;Vc-}QKGB$(VI?%zBL5+a%H9qq8lX_H7Df>Cu zNG_$C)7s_+@eYzYzYT3mFtv&ssYir}ydAYt4Vriik{>ngHdZ**&=|GDAI=T5wE{$Q z{T3Fb{FD~x(pQOEaz^#uuhwZQt6uQU^Qu5%ql zHz01UPdE0?vAtTJqx!L*A5b-=EBoSj*Y-^vc%W5_FEemXMlRrC&}J0q3DZZ+c4DQL z+NJt$Cq}ATdprB>m(R5rJKOT>&z^O5e0LCVO%LDdQuhrC)brm%_$;4yRuugD2X{NT z@W^b7sXha0q5RiOd?)eKYQztS_^Lp`4))@yVa<}@JEEMdvlFLs?mmkelC?MfV!jvUffqqB6TYKeEb0sg^XZ3m z$$O~`&{D@{{?=XHz@Gap$@7W;^FIS8Zx_U-MKsQ^KTGE4jnEPaeIX!gG=6Xat$_~{ zj%TL?H}F=bO36I}BFQ9@94jQRV*n&0KLL7WRlTA%c1lXR_sane<)^NHD?jbs!c}QE*kyV_TuJv@Y z^JfNIUrpj!Kf_*tP7n06L1(+%zX%utzY_*0*^7*}AL5eqK+|y;vWe4p9naFZB!7U0 zp5za_SK`pdtU_P#C8fS`B-?U!rivfDopTJ_*pUzRz>SGC^~CS3pc(ZBa92e(g+FEu zm&s3*G+EMqYuJi&gInc~5MLnBIO9i1`b#6&*XPDz4ZidIFm)|^^n815Q38ABd|Io? zIiZq;`l~r?|M_e<5_ch6-OMImXs7zvq6;J4bKndbp2Yt}A>0nXSKBKU?r7ru29KT= zuePrOtao!Y8pf}!A41ht5e9#vdL=UJ#XNO4E4a2a8&AMIc zpl!@zIhU5V8Z!(gT*lWR5qjk?cH~l#dW4O>+(mtjJ$AXH`Ye0#^7K}BWQPjc^y%5G z`IWzF#c}NKR|aSgv}N^Ix@s|Ttl8Bg9r_I+N80loN_Ca}U|r#dyZE+c-Yz{Si&?Jy zLrZGI{&~$|?GnR>ey)8H=3xrv;k7T-F>GVQ1L_Xe;(DQ4$x5ytNbes_lUpn9@9BaC ztt)C}f#RN+?qcEbS>gckZv^B|{FVp8AowrlxzSPk)yAr3#dRClTG;6Bi8ZwD0pzj{ z{X{GzLZ?YRuV5>-(+i|)~kL!Ty5EB)|^E%XG~dGJhN!Z z+?n%c7Z)lEisww3JAd|+`Sa#3o-*~ZsdMH|oi=yolm#;v&Y4j%b?!oC)5)f4FV$b( zOfB(`4pRe>!baiDg$p-6peCw*95*}NId$O_vNWf7<^n~Q=gxd==G>tDph5pRO)c<` z3Rg4JNV?ghMet{8F;S{VD#V>wg{a@Ur zw(xg|RPP*2Ka;1wmtUf53yv6cotM;}xunfMhyV2WiorMEx^N(O+L*zA{iciFyl3>F zl%WZ~9lNWYzcy0Mu+8Gv)$NiF{#SFgPEB5fuP+Pnw+esj@K=t%aC~uk8PpEi7k_Ow zooS&SRXy}7g$Zc5&>jdhpzLx40!d+tk_Va&s`m&4#(*|}&H#;12?UlB4tf~W)hiHi zge%ITH1I*oK^K7@>K_Q$EckisfIy%Kv;lM_s0*|ACTJe0tqFdNQjUThfzU?=0xLjW z?m(albP?z_&~ngypzA=7gVupIfUe4gUQc*Q~eYo z+X8jabkIehZqRbj0?<{b=${Z!SO=gSbTjB`&^pldpoc(rf$E@#KpQ|$gSzl@XnHk8 zA!r-Wb)XK=I#7C>K*VW;-2Yaz+Epv;5pu!-Z7?B4J)y-8W%{s| zsm_*;AuW@Jw2U9p(l(@J#IWY`T2_Y5Z@Etk^E9V^$GjM$Mx@X(l9kmMb_#7@V41*@ zslD8mHge1bY?8lUj5@K?Ix1qryN}p#XO47-4(PkUtMY#kqsBSwQQQDAOk;kL<;%d< z1LFgPI$;r5h_;1b4qyqu>io@O)vl2)6z5_eeqN;x_SU+p|P)*$0>|vb_)3L137e{37f)uze=10odmztOZh_&V(fa+hxM~0;?GwxaYP;TmZM3 z1SbL83=Fx?FE>7E)!pEq9H)--)S!3}>t$%X4g%X}!gOGt0|Vg~^@$eiI+Gk0Ze^EA zj@IxR6P6BaTW-K}n?X0g%@D*26l|mb*aj0;3~W8H&;XYMdly)*5Z%?l$icV}Y&|gQ zu#m$dPd(WM@VrUz5U`uT`bcFO_|w1~qZ%<{k$yPBHwL&7WWwTsxq;yZmtTbS0JaF2 z;W+=thD?CP{*<=rP)|9E1HQO@G*$ykH(~36^#v9>8oPj{n&b`v>tT{R4a{M}6r`{H z3)5({0hk2A(9v)JOE6&rfW-qdIz=5F1FQ|Oheh9ckY)fY0v0+#i-4^R$|2OGyb_>p z608Ci{$*q2X*4zfOZBg5r?z%}j$$6zp#xA4Y#gxA0k{mT6qw=b0GNqDWvTy4JGH;Z zg<=o0kZ=OjsFT@NhY~fz!FSy?*fZAVKuBB_zaZ8E{TbWI7`OC-t>{s)_()_v<0Z+4PlD$LNpOM+?D~?6svOint=#f;UAl<#z{O#4B~nBSMxVBk{MSPx@lu&<#$r)ST5bO#xi+a~*jz_kbszsc!tGy`lIE9Vb%hGGTYCKVXt!L zyhEgAp_eIjrW9}679k*S%b zFOd0a?2Iuu=cC>BB|arU|?@iC?>On?BnZGK0W)4vQ}v>h+m<~L?7;cWFePCV~QsePuH z?3H+?X+iu&;{2s;h?B8Y#o!K}521A4I3%V?ylA=zsxcP10>4wKlD#qpV~$iXzb&pz ztVXLinOQBR*2}5ymP2;H&^E247bRY4S}Lqo;dZ6$KGr9Gr4mk4Dsi}!NRl}yPIij& zkRdU(TuPYV>h`+C3rr#0De)o`{932!ft*u-J=q( zN*Db>_xbgl#4}|ajQ)Ng@gx(z-@hyktK4zPRvr*$Q03Q6iI?Vy`X>@^g@@JTmiZN+ zclv*Vxv7IByGmw>@ovN^5>J&mVR%<+n3G9ltSf6JZhk%94&F@Se#4yf&M?=!l^op2 z&QyY*1;_nw_f|jJRNhC`TWy*@Or090wXX3W$WePn8V9}0*Zh$q)L~JZv5EvQwya5O zf6)ll7Hymp4SLxB+z7Q-^eQ@Q<1%RZru`$-!CF+I3}FIz$^OJ#wR^KFd1TD;kIhx@ N){bZSpUzdg{6AcEV&?z= delta 24304 zcmb7s3s_Xu`u^TqP`tnlDuNdjZLqOK!An7kG735*7?vp{ipWJ&R8;7g*%?YH=fEho z@{E<0T`cW%(deROg?BAAH7hJjEYppdqG@@n`Muv-YY)Sm{^xoA`+1meulKvIZ+&a6 zz4r|J>N9~GpAD?GYO$kM-ZrPr%Fh*kto(fCOa0oEbQ7(TZkxXCr}o?%qVl#sJ*5e; zQ>>5nc1O#XyI*>KOW(J{t1da8UOQ*etjgR!YtCs$H8qcI3k+B9XWN3pSz};?%2;Tt za2C}vLLJ084mPf3xa#0&InV$)Q!Tu`7wwrOA2@ZZ_JzMuzud;FYT+Z!u;&T6Hn}DofC-HfhBm-m6 z-%w`LIv{9Un|mA?g`%Ua1yLroQzK#6yec$5sNmBe!~LKJ9;#M?-`Bt+mzGSD=M z2gnX6GEiaPp(rC@kql)_5|SOI!dPi2RvI!gbX*3cGzh`|QhqlwOLpp|gI48&@qCcL zr)GR(om9AFdRH012X#s^rh^;A-@is z4icHGl|zXpKRU{lu_Oge2mXwRo|2BuH-w;3bf?gyAu!97elFNbA1D%DEtlBE(!;Kn zQeM`CF^yM9JVOq_Bn%DtSBA)WnKWHkHXKiuc)7%lnph#Lz1nY@)k%3p4vjICZiyd~ zc92MZ>Or|oQM_clYozEN0i2NdNQswI>TsNumV)4k>?Fw^8;*12*f$mkc>~wv zP#&7X`>!ZQL_=k9t!)qrM(UUx{3RXN8UacFN>`bp zYFTtf_5Oe{qMpUdfM!WU3yoTlLtqr?P1)nA(vH!Ax8;y!_|;ag_L9-)*Mk?}h#c1^ zv6tHCv6mvk*^3dq*rHB-Y9c#KZoys;YvZm?Qv0);or7xjciOGid>-*t4~K1{-4!-* zkD}Nn*zQkt{%IS3|9$pH&&8t~_R5@Cd*$4ye0ya;v=!#<&ZQPBXN-W-sg-3>WGaGWTw!^iD+(M&P(t;uefZr3|0tBf=a-B6`{2XwH-{9(Ftd<43L7Mi zI2P(pLdIcmqlA%Hg^?)kWWJ9Ri{QT^_!iE;S2%gt>!iw34hHWZ5>iK7P?coZqncCIZAvK2Vu4vii4n-@IMseee~i{+lF2| z^uqp!o?__P_bjz6la~`hc`MqA0{vfhQD>9?zNfX_gS1 zp`mZ*j5Leg3y&1m?Nk=}%fzk>JbFOrasO48wP3+OG`F^tH}yqA@;_3N0x3GdC`=N@ylX#VZf9`KeOqHcp_}B<3iiVo{vn1%K3_RXUa<7nlTS`(>pD#>) z)J*V@5G2QBlA3y|5PYVYV4V?|!_|R}Vsv;-#kER|vX%1Tp?sh2#t=Nlm?lknHOtS!GdZ z>v4r;NkMAr-(Kb^7}-oPNC+lNL2BxogkYCuf@T%{hshWzX(q{KLh{;Y6uFVr2w^fp z3Q|+gAwgudgCQ0c3^uB;gQAZVMz%IH5+#gi(g-yjdKYOW+iOJyA7hg}fHHC8H{oQe zmwkRaXHSK;q92#+d@uWZ!Oqu+{V}mC#jc%izg=wOWp?|cJ86TTG}&g$woSH8vE}SA zHWxVN`v)lB4nXoLExixWG9e_8~%VF0>PSpA!+`*r)0b3yt~ zm)S@0Q_PpPVNrLUO>>^MJ1^*8U1og}y0zK|P=D+SOH1g{CE>iHU^8=tC1H+#Lfx`g z#>ds_TQ0GRgoM!dF$dEsr$^bHH=I}Yj5aImi-g|VnTza?goG&*)0{uX{jRUO=<)Eg zG^a;jjfQPcONw;KpK57)*ZrPU*FOW|ez$M?QPUTJi}m3aSRJ`1pdU%uog+Kim+TBe zK8$q4)fQq0V$1Kkx64iJW*@|)?O zVTz!UMD4EP3dO$d=Kxcu;PPLvr*#TD#!2LM5M;yDC(RON>NDPAHudRd(feLx?+i&y zpWKMfB>Y;o7xto)ge10ev|>h}1v+>UHxoUJlbEMC8GPj|T=dB&i^XJdkVSu~kqsR> zGX21XCX0!65QA@FG5ASR>Xf*Eli+jQ#9~VnoFty%jdm8WpOUy)Z{NuF4;>kr2#cu{ zV?)$mD1+xOvZ%y?jzJcmYef`}y>hfQp(Nf^LC*`&2%<~MROer~s*F#oyp=A)qU;q% zOiSxqDM}^c)nWFM$vk*7DgiXGj?>UmTm$BHw4}rxf$;L@`H&%LmrS9_7OPRYV_c4a zd($h&+LN7!Q48s=zK@AXk>dO&6@!rG3Y_pUmEw3yP`cWnCnC)d&rEh+gE+XOz!e(A z?k?C1$ywZXX=ML#lHd z++Es=Yg6Ygtp55)8gWdHwlFF*tD&hE14b6cTOzn7PLtt}RA^a(;p(-RE$Pk%Sla+= z4uXO(@j;R9zyd5Jmu#lcRqI=jtBN|X0N)|u3UdI5lfWghiiK$Fo2g@nrTn}ROEJZA zQj81&;IW7<+-Y>-Joku;3DBe>e3a9ENR-wYq7*4n8D4pb2(z4GBAkNIsjA3#px3vB zd>8(m?@vGCqlI)pJqcGkCoVZ)XQTAXSLpMwL-yYP)#wY2h#u~>m+IV88DLp*RW&Vt z2@8geuRb>CE=>NVkfbZgi|MEjSKw&46Qf51whD%mosB832i25>D`m&9?AV;U9k!2Y z_0sKLcYGK(Kv;|&KC>+maEg$r*Q;e#R#R3mG9PS1GB6S^hyi;-4%jjYKOqL}31h&X zpaC=WC>Mb?q&s+|uYBkm>92?-*{Rb_f-PaUh*U)R@;M_Ze1HSIQ8vZ*|2dQnpRp|L z*ObBrk;0H>f!+WA46Lm;Fh2Ul5Hw1k5|?DhN3(IJrJ9DyAsubl(kufo=bHgx3nKr8L^+v4gW%q*uY(D?G)Rf7MWnWgX_cjxSh1R= zxq`2Lz_W}STRn~Yx+vtp`Uxy4=j?0IWBk~A1v@6yl|l=F-5Frf+i)3=z8yhQ*dKz5 zup4zkF0wHryEs;S!s8A82Ppi^g(2KZ5}|+*H@CF`U4{*8nfeqm!&i}~VCGZ0%HS(D z?PYjSu^oeUJ8bP4K~owrYx)`}V}Jq&0Y)zDE_+0wsbeix0A$P>*c~wWB9SgQ7rdmX z99w2`Wjjks#Xi&W$G9onT&7iGZ?-$}OvKbf;hA*m=~QypJf-0B-G$btYlt!vP(+GT zN4H*mpC$xtb0UfQG4LM%-w{Ou&TD`nwv>xO<^+GH@E(M+rX(lywUn(Q$7VY|g7hiq zw;elQJT53Z^5;UPij625TtV3}tC}vN8U{hq)FTOHWR9uFR*_uU4SP6$rV)$MDz-#8 zIEhWE`0k&$(T4RD=lSHyz83GICRg7F-t(E!wk5k&Jps09={-(06of@QCC=kosM_;9 z_HAcc=+{q+q|fATJL2ih9+`A8u1Qg#u_iWjR8PmfkfBOni!7($0%O@GneHG8CC!P< zra1o+t0KSNC0KSh98fE*19 z?u!enn^vSB!4sDOe434fDQ*RO|KBfw4S{&1hAz-@w$5v~;eOcF-#dfhas|JI#rMM} zXqa4sZ`Ke=T!1RqAOATB&iMk$sz)biFP~wbkM1A$1Bvj<4E+fhpjtu_@X)CNMTAn> z=H=U#>{0c+GwgPIbmTIi`kmx($xdsU^ADd|Sot%XZ4Y;3p7MAqYRg*1)zSs`j!zK~ zMN|MqT=6dgmqU6I&ge$MHqkZ#7mT+1<#Wl^kjH@Fxxjc!=90fUmfnr)$Fda6y51+~ zF2c5%9!TO|qCaZc=U!s%PYCMKokK}~GdXPvZHITWugtcj+4hV|0$af(dIXtt{*dY% zWkvPqj?=6-HEzfn1O^DJ){fN5QP#?&cvq2%8{B2kL6?%!^8Os-8Y+sAoZju~i0ej0 zaSi>I?N9AH48`gyeN#zSzesllw6^CRRDVx(wR6&(HoMv>+v+uY-aZ3~a9-=v;M2CU zp?71>+L3{HdtRfe+gGQ$25m}rb$oQzt`KSreUzqAlJmm0GXc)4eJ(%*GlCdfQeAz) zEVg7+7F+BUznYfOlW2QhGCe68Zz-}nvn^BYNI|^a)wd!5JgWQ>tkkGlJUA%EN)64V z!eC=#r$r!$ow_?Zg4RRZ9{R!-D_?*{l$d7Lb4-WM5kQDRh}GvjilvVkGA7nkk%6mb zat!Z?gWQ#nC)_%5*3dJg9#jsNCe0q7quX4ATT7!PWeHS(2cZ8(bX=w#o6zI zJJyvPg9`*w26u_z>d#0$vzIf+M{lv@MtQmO-fXfuMRFr>MI+7L>7$$k_C2nE{$Abg zf@xc_Q%zm6D?sl7f#igIi{0e1crZv*wUi+g@%L%Y-|Viyi|`hAM2tncQ@7)O40#Ho z(p$(ka{YF)QMuS5avNk@;=wJ#0Mv21^Bmt1#HD`flAQtR&eLg?xYpxqXu7izkLS~z za-eA+O~t0Hm5u5y&T$K zxtY2j9AcB*K>yZ@_pW1}uV(Emw zhx^AhjsEBNw64yHPp$kX*4Fd-0n%_f{=ixrrN4;LNUeO3GxbuElnro$^gIBlNSvGX zC(v_)slR?)GFs{8i zcF_2tX#7Cd8&Ga+x!pyg&8 zA6R{?G#1tH@L%G(iZ;l&BcITY*N_A*wZ)E#Yi#VQTQM=PtL{Qq#jaZU9UC}dNZSY~ zU@>T?@A{rSG{Nd<^*T+y^J&gA`bLkH>+yyJ2E&~^>Z$F$K~eQRXnz%e|#9H>PcW4$wP>vjAXu7jHHTE%0` z%87Vnb*H`35z+9*F*ZLl+V&od+bbVY^eS+SL>KRl%EzbNiz^=(&mhGoaLj<7J1SJXL>92-x-K#Yv{V>KIGN zx=-8x8e5*#%W`u?R1eSHDrz6P?U#wT z!AYs`l=eema{v6QEmi$N9}C-b(V3ipo-FQfbGSy|z(W&rOFYK;$AL?xzK5`Wh+(fK zInwD09WMXKVe~23h%>Gh>zu0p`86dFaiu2gG*!&?P;04WJ)XZeJu^?kLa-S~(?YNV zmbxN44(Ut(0A?DlU~Y2YLo+%Ek%v&^n1s)O$#H!u&h$!T*{9l!XXcQ<26@9`0R`q7 ze0g0HbT1&D3V!`emtw5%q$=(5f7pkU3i=&*gKlNaR@@8ZTX2t+6}^Kap;(8-{Q*9U zEm7daYNc>`lc)69i*8&pWqt#_#g?K=$)>F1 z7^JLgI5P*=yw38bEL8_HeM+9Tay?7WiEvC`PxroAjgZQ&$GzIALR0o1g(hp_9KpEm z*S_Z0{6N!vc7?5*2K2Bnal;f^1|QbJlSDgmhx7?qN8oF`A$$sfY}^`T!EaXB ze|&iNi1Y2h5kpq+3qHJ&;(Qx$#FG`ncw5AEr#Q#6MG7n{`Ux*DbEhekfe4ou$2Zi1o>RR=c{cW_NC=8o&A#e4pV_OS@w;S;5b>()JaokHCk~!tEW( z&6Kiz2Vc1=JOXe9PG_dPi1-2@<wX9MJ#6qO zwx=Mvla(uEVPiybi7%D)a|f7KnAGXpm&g`aRIgRyd*go}V3~#4TKoa_e&IT8!;3YO zr!Q9>C!d!EW6J)5>O{7Y_m2CF+W20`Am3tKshw6in+olQfgKYv7-um~vW>A7?FPaB z(-b<8YUeZ2jByNjLHLKQz)t*J_z(iw-f`~Y!~29QXYUmLvm!%$c<%_Fx8^j?6EVEX zd6|cx$7}BQ=M~m4BTgH!k9C{bQ+r`I8$NT2HhfRb3o|>Z>Tve{tZ~|vHLP{<5G{Tu z8&w>0Z|j|WCNH(bV&UhLIZ3~=Lt^m;X4X#{n0PLTNgt#iFfh9z7ol(2!FCm|)?RzI zCTDi6s#QG0R?Zo##?;i$8LetpR@d|?xn0%j{>>)Lv#KG?F>jEg^S_ZVECGLEhG#Yw zCY!8*oe09xfIfHp9lXq|95zXK8(NuoZevXhCd8C=wMnL}l-cQ(X)R>_;xe?%1iDv2 zD+SNm08N~H)f5^}wlpD!@z08!AgpBa0=h=xrpzySk`c@`3MTkBLJ{L$a>V5>ayd+5 zugry2Y{mS}odbmBz;=R0cNBDRH@0nli8iaYX5fOJNXn;dLg@wVZN|1fa+m30hT(wu zA1UI|D;Aw5|V&>sFNvz2JA{taLjrn2X0p5*x z23vf!_Hd_J-++K&Jf6>l6S(Kx{w#&)JIXY?YIcp|YSE)5~on1BU=u*n(*oPiR zoV*&Wk#8;){qg4W4FY`fDcjzB%C6sFvAmk8`(p(y=yV!p8XPX%%Yw% zI~qyNoA41rNR;dqVtErjnpl|LqGTJ0XO!$vU#$~74v}!#DXH_7)Zq!Y3Uj@|!&FA) z)2w2;jQVqkHVwh9higt&9Z=O#?Cs?VZ8|OzJ&Np5b7}c7HSG98-p=yL`Qe2;=i3${ z=dZQU>KE{ISCk=UneHtMMBlvW-b{c`x@Fs&ZrS#x+s@^^>5dZHK-eb0Jlz|Kk95D$ zlF~MSVXxkJtB$Zq)a*lK;x}#1fwC(Irjw ztrjxJnqK#`olT*M@*X}*bk3XaVFdW(TeiLVmThmo56)zbYr@*x2{oSIJDBOYo{pO{ zVZ~d$W67qmT&U8cSa9zXEt#4tq-IpF-B;@u;>dK$n5E81sl(GbNSI@9KiA3p4H!-7 zJUo-9^ZIieRBhZ0Hhp6+^WYgGX`fNxk^N_|r#ISI#M*yo6-8|2+R@seBDQ8zU)J)) zIPK@@Y;tvYO~#AWE%0wvCtv9`c0)b|B$D|3G@PSCyh%JxfKL)-+nYq$_9n5Ija%2L z?N`v^IXpUz{rjmX$1`NbTa)LgWhC(hYItk%qG-v4Fd-;v^0cp(lNvIKGG?jckUBhx zN2r0C41KkexeVt`Nt`>4Co%g~B=N#j)@uE&+QF$Tc722PTMp~Gp^NrK4!d*15bdQL zR=OctTarUrV;L__stJC5U<>V)2Qc3|vya~k8k?F;p@{T7@_^`!H+_o<@JXL+d($V| z-t@I&o8IiyrVLs`(Okr8hRR#D5y-f(Jov$s;TWF`g*L6N=3eYN^fLuOBg zEOmNF9iF}U)Ij#;z17JahV!QEnI7QT+xix=w<42D(6k_v$DAcfa5h`Laf`NNB2}L{ zpFO*2igwFHZwhC>gDa>J_Zlg5oVkzU5h?6?ujr9Cg|GjM1k(#$eZxcfD!irelB=;wlcb7U$JY$|)e6{xQIOK&@Go6p6 z4o|XKu-KRHcQV(2(Ujyj$MGb0|KLqk?ZftO?u+?)ZS$ozo$nDniH>4dwhYpK8q03~ zaFF)#*qYoA-)W(3OJ!l7#5kTwMO>!LpJ?NMp3dc@J`N!IISKq|NUY(~S(H;QAlpA1 z?V+5X{S#fKR^uG^igl*YUT8);iG_4-wa{-FC>4o6q$+INC!HJxTua0-gU)#jkcj+9 zaJa7=$@!6V!#4To+|SP`s;tBfJ{su)M|4>+FZ$p=+u_3`qD)9ty6`X&YT#f9nL?+T zQ5JRRe_&)y$)`1{c6c;9u=#fFjnVA-<}vKKZLhVh9VJt{XB4|<`#|kqqmbKfZ0+_4 zEq@ft{V=-b#P%0js1ulDcQ>2OM(#4tQAShf9Nj~{GZB|HCRqY!4PYbz=pk*(nHYI% zw2d9#-8uA5;E1u_6grbG_+$FBmV0_RI!fupS#%B6&MhK5ID>7Z^W!58Y=V&y~fr)53)l=GZG!RxM(gp!*pt5sp*L|cYMA~Q#-Lw4@9d9 z`x(z2-c4YO4o>X%QGzg3i^|UwPpUE`w3niI84Y+p1Jj8o;+f}QXYFV_>+(gHguU^+ zC;Z;?26}Fd+)W6Qa|~(96wnTWFt2~3y#GN)4#%_EUvzhD0fVklzrsi6VNwr%OUmvi z8_^0mwwXF!C*Dy~=QpBHlMrsG@i+KaI;XI?`U+}66AwbNf`4qrT1qWABR~A{T%!3& zKs4F^#Hy4PQ-v-aiqzJ{*CZTTuc>2demwG|s@Vs!13$#nynHlN)q)1_^Pj#7R@MD1 zuI_2}uWw$}s`}Rqs2c&venRpXxb12-eT%09-`!sG(|6-kZP{(aVtL;$*M>}BzkOe) zU9xq3@-K{mJPfSp?^4@IN$y>WVaVs8a;RnbO3J(4_Xfsae8B-!FKUt-1 zjIIg!ag3^cc?+BL(|&E$Ev(O}pBz>TzTNQH^8!A!tA~bp>gg9Kcu1DkOHsVPaz;DB zuYKy4=KLOh{B6XKtA&YViUGfIb_Lb|Gakvk&d;?{hj=D;s^@<_lUw2m_(u-DDP9A~ zzX@<3Iq-hB?h5p8Kjg=E`n6|JJFeE+EW$E?Mub`X+<0g>iw!vaZ|zhRJ=)W3Q7ke) zT-)4>wLCMb^;*P^EJqiy`_H7_IR}YAu@1mr*aPA`aXu(!!ZF&$f@gCui+<@6ol55b z1MTdqGq>Y^>Qz0;(N%z1pM#UR6JpbXnr2u_$=u%r-GF~8#e)Jeh2j?t&`SJ;a6COF z_$v2uQcCU?5J^Un$el8sBj)qumc@I|9&>J@qD;j2MH@dSM4SCx0b~g3w za8qOybr%ovr=rt|;q0xmz3;!;gC~9m3}#zO4BuygLgogB5*8T93A~KED*GbJ&VstcioUggJ3=Buzc>6ANetM+3K4rjVDgo?Ip? zQqpKi2lr%`e;M9>-zf1F0*y0%R-m^Y%~H;d#~S?1xlwqJ!pU=8wX0p(U+3c5A4u_) zEYk6C*!F7*0(kNr8vajOXhnWy@aTE*8vg%L z`qRlxhVe_lZ_%_@wt#f-+ufMse5RVh>dp_)u6JhbE=<=NMzXRCy|k1O?CA^LHSC33 zE-YzZI|3%`#up($x-ycDZ7flzuumI%t3BB1#vZDPwY-?up4of_&H9%%_VC43+T~8H z{iUJW*AXn^(yiL6PVAveKXhL=i~{M3K#>@G#doHqZmehczGco%eM=H6`|UMtZ3ou< zveon|zSD(Rv1K9VVGQQs=cLMT<9!oqefZo9_{N6*GZf54ZaO=nIg^k-CoeZrH0ohF>w~K{D z=sZq3>V%rPf2e^I3TG@V$j>P%Sg=5uUOabNZt;voxurAa&dJFwE-A{*nLT69-{SKN z^2_o_&W~HRpdjbIdq#e6IzWw76N=|f&ncNdV|H$F&Vtf{lAJkn=g(FM%r9#OmCh|D zPfbE@t)<$_LGXgo+){;33g&S#V~#w@n>)KiDR>Ze<`*oO!42k?=FHArm@}iaV7@Z1 zWI-M}GiQ2E*))#kPFs*uFk`w8g9Hk4=1!YgfSx>9@LwE9-rPA0O6TX!m{YnynNwVn zGp{5^gfu_5v|#!}zL+;3Ifc1-rE}-|@)qP47L-2pHw@8b^v}mQVdxJ&QmR-S|4SsNI2R9D#PPRs4rwcAb{Q{168&4jCb?K??|;i- zymM&ebA4;1cp9%wq`5F>`uyDdoVnVxnKgfTuB#s;X{uFqP))T)q49d;Ck69bb4nw_ zPiQz=W5}ptiEGRAJd1KFi$yXrUHr_@>}u z=MHRtr*`gPt<>8b&pb`XhT(<$I6XXOWVc^_yrawTRbThk+x80`=KA%$wSVm1I85ZF zW71Y;-jGhY6qo%hp6j=AE|;y4Dfi?gIYni5+C#^;R8J$`hT{CgFK#c z(DiqK54suj5Vey$o{T_6S%&3&Bj{ex{h;={J)WU0@o&PJILSaqmVxGj>SH{fouKwK zkEb4VA?QuwgPL2xAgBd&FK9gI>U8LVZobFkq1Urq1+4=O!ml6CfkuETK?oeQBWOIR z71RzoG{}LT<0Jz>5okW>LePbv%Rs9@uTJoIj)MkG^mrOUBR~UM!ysq`Xgp|NP&;T6 zXeQ`*&?3+x(1oCjL6?D6gRTaB1#~ayW(Q7moa_gU$V87p<3TTjE(8r~gPws#f$jwz z1R9iu-3~MYG#k{4U$49Z8V|Y|)DF5IG!wKQvI{A9JC1ZDbPC5^`Jqht*xMT&_kfhsNO(VgWd$K12wlp$3QKhb!U-T zYJ)BWy$V_d8uW{2nrAIeBJeK-?}A!E_kzZQ(#wVHppBrJpaH=M1T+G4A!uLFy`V{; zI_P*%>p7$x)DF5BbQx&1du51vtCrE-{b7jO+Y!_Yu^F=llz!udr+bP5{1EEdIMQ%J zYv;n=9?w9Gx-uffyh2M2iCErZL`ak?ASJ|F6=(~IeY~YDBz{S&(IJgmizBVtgv0~0 zg;*goA|wJjBSL~wkrIV`?o^ciN>M+LCj<>T$ax+9@`2I0Vh=Ho2?^o>HKJME&pphf zKF~5lRg~lX+|QfTKI-T0?j6(!cfCmsXt@S>V}JKqlX^$@I-H-1^LQ|~_^}wD=YZ7% zJKNel%&hhv6xlLlnQ9NIR$IiXI2#dCuI7QJ1Gxt@88lncsh~FSXS+9<)v!?XfwEML z)gohv$8$n)pAA(l?(1fCz@YsA4*^Ufzv#gOZ9;-<=yNfeI(V1C<8NdcMJYsDmbXZ8 zPYqQ&wNM>x+~p>NM^`L=sx!*cvIcE?bUEw1lFZeh+%c$2U+e5tOOVz z2DAxF0=C!(vjQ6rY?*sYd-c{fwA}T@MR}M;+3o3W+d&OgpL9odP>!5aU zT*hgH9a9h5bdcpBj8aEnnC1K+tQ#=19~KL&ogbDAEXWVb1Qy_j6$4Z3p1YbkDF=9S zbaTf~0lNwe6NVpD`s;yRb_aJ<$2hLywBy+3*=mOc+3bf!0c!^g;{2dK4FVSACuav1 z;3t<2O!31?fZa^@IGPz;3h*jcd@N`DASbJVUG~G?0M-c1H^H^Q&H?N1qk9w>1sLvw z>Ab)3C4O>d*e~{zvj8jd!{UME zAM_iIG=Nhf=sOxyfo1z)rNAydjnXbJG8U{5dKt{k7|Q$3rA1> zsVJvG30EZEUE(+0<9ey#jxb1bwYEa7on-fyxY-X+m3XWlo(DWYx#*GVUh;^Pj|#>K z4#WOxfjg8~KSqsIXl&P9;h@CLe)0_xSNw4L@1DO4C=xhzcp;Y`(-hI2lCjLs&^U<~ z`Qfu9o@wCj)Lv?j&J`SPuM~F^@q07D^PS4T#+x%0<3*tc`&HnYVp5jgFc3lC;O0c(Dgo(Nsy{;wTvixr7{H6{sp%rs0%uA*DyO-#W&GW&Pt#}T|412gp^!n7AN#n>Pu%`ZDRZbn@QgL1 zGj>@D&i@^Y3G8BEM66``zhv_viTl4Ta21roJFl1Awbz6zW8vOMT=gRV^w4KqI9TM| z{|%CMiTl6Aa|W~F4YnKHFZMy+%Bc~@q^SQJ6E{lS|LvEbaT7oA&$I5h+b|zmVl|{F z{_7ow>~O1a$1uswC=|Uh%AR&z((-?6Ub$gSRtF{$KWL{VdH+|jZjrd(%PEx~1%8`S zt~YlahzQ6~lnlri?ORw+ykmC|xKZ?u-PEyCPH7PO#uh(U;z`o3p}$<>{%_-ZgY3p_ zm)s0lhsFzFk4d~*S~D&#uJLGkp!dBfn#j9wMp4K5)o~`_B;lx#68>+3d`;pTWq*x+ zd}dhm>&IEc&PdU3J)5nVu!#Fw!zt z;O+nGox+d1x5c14&q_T-`oTQmhw^&L(e2c*K>ucF%Z1U%RR@y@ z+y5oXu?FrpRO8%gEOJ;R*&Ag6it9zZeTcJjH%PYsn@@L2-2XMdrzEcU&Cmc`2lA1Z zQ5aF*;@*ZR)H?)86{LWZf-mxlXz-i zsEE#-B`kr#k4TC8zc+fI#Ovjdw<;I%W8Ag@YItsr!L|w5c!hAe#A^!#Zp@k&4S888 z#-g%I;wAS8`4fUDKXNynFG;rlOJn=G=OKzVBk{ErrM&Ts`^f?7$L`RPY6Pq85D+l8 zq;$atV+X1Y?LUYgrA}?3y}ZNyOsd+ijd2OM^oILLsya&B8{+PAx7sh%xMVcwDRxOW9yMInoZ*RR>UL_W|JEyC6wuwv;n!&33fzfLB F{{v~?^w$6Y diff --git a/control/velocity_controller/src/generator.py b/control/velocity_controller/src/generator.py index 8edaa1cb1..4820ec544 100644 --- a/control/velocity_controller/src/generator.py +++ b/control/velocity_controller/src/generator.py @@ -12,6 +12,7 @@ import numpy as np import yaml from pathlib import Path +import scipy.linalg from acados_template import AcadosOcp, AcadosOcpSolver, AcadosModel from casadi import SX, vertcat @@ -40,11 +41,8 @@ def load_matrices(path="../config/parameters.yaml"): delta_t=data[node_key]["ros__parameters"]["publish_rate"] max_force=data[node_key]["ros__parameters"]["max_force"] else: - print("[INFO] Using default Q/R/Qe weights.") - # Default weights — to be removed - Q = np.diag([ 5, 5, 8, 1, 1, 1, 10, 15, 10 ]) - R = np.diag([ 1.0, 0.5, 0.5 ]) - Qe = np.diag([10,10,15, 2,2,2, 30,40,30 ]) + print("[ERROR], yaml file not found") + return Q, R, Qe, inertia_M, D_lin, D_quad,N,delta_t,max_force @@ -77,7 +75,7 @@ def create_auv_ocp(): ocp.solver_options.integrator_type="ERK" ocp.solver_options.sim_method_num_stages=4 - ocp.solver_options.sim_method_num_steps=2 + ocp.solver_options.sim_method_num_steps=4 nx = acados_model.x.size()[0] nu = acados_model.u.size()[0] @@ -85,34 +83,56 @@ def create_auv_ocp(): # ---------------------------------- # Cost: LINEAR_LS (yref-based) # ---------------------------------- - ocp.cost.cost_type = "LINEAR_LS" + # ---------------------------------- + ocp.cost.cost_type = "LINEAR_LS" ocp.cost.cost_type_e = "LINEAR_LS" - # Select which states and inputs enter the cost + # States you care about: u=0, q=4, r=5 + idx_states = [0, 1, 2,3,4] + idx_controls = [0, 1, 2] + + n_y = len(idx_states) + len(idx_controls) # 8 + n_ye = len(idx_states) # 5 + + # Vx: (8, nx) — selects only u, q, r from state vector + Vx = np.zeros((n_y, nx)) + for i, idx in enumerate(idx_states): + Vx[i, idx] = 1.0 + + # Vu: (8, nu) — selects all 3 controls, placed in lower block + Vu = np.zeros((n_y, nu)) + for i, idx in enumerate(idx_controls): + Vu[len(idx_states) + i, idx] = 1.0 - Vx = np.zeros((nx + nu, nx)) # 12×9 - Vu = np.zeros((nx + nu, nu)) # 12×3 + # W: (8, 8) — only track what you care about, well conditioned + Q_tracked = np.diag([ + Q[0, 0], # u + Q[1, 1], # q + Q[2, 2], # r + Q[3, 3], # theta + Q[4, 4], # psi +]) + R_tracked = np.diag([R[0,0], R[1,1], R[2,2]]) # weights for controls - # Top-left block (state tracking) - Vx[0:nx, 0:nx] = np.eye(nx) + W = scipy.linalg.block_diag(Q_tracked, R_tracked) - # Bottom-right block (input tracking) - Vu[nx:nx + nu, 0:nu] = np.eye(nu) + ocp.cost.Vx = Vx # (8, nx) + ocp.cost.Vu = Vu # (8, nu) + ocp.cost.W = W # (8, 8) - ocp.cost.Vx = Vx - ocp.cost.Vu = Vu + # Terminal cost — same state selection, no controls + Vx_e = np.zeros((n_ye, nx)) + for i, idx in enumerate(idx_states): + Vx_e[i, idx] = 1.0 - ocp.cost.W = np.block([ - [Q, np.zeros((nx, nu))], - [np.zeros((nu, nx)), R] - ]) + Q_e_tracked = np.diag([Qe[0,0], Qe[1,1], Qe[2,2], Qe[3,3],Qe[4,4]]) - ocp.cost.Vx_e = np.eye(nx) - ocp.cost.W_e = Qe + ocp.cost.Vx_e = Vx_e # (5, nx) + ocp.cost.W_e = Q_e_tracked # (5, 5) - # Default references (0 until updated at runtime) - ocp.cost.yref = np.zeros(nx + nu) - ocp.cost.yref_e = np.zeros(nx) + # References must match ny=6 and ny_e=3 + ocp.cost.yref = np.zeros(n_y) # [u, q, r, theta, psi, u1, u2, u3] + ocp.cost.yref_e = np.zeros(n_ye) # [u, q, r, theta, psi] # ---------------------------------- # Nonlinear input constraint: @@ -141,6 +161,10 @@ def create_auv_ocp(): ocp.constraints.lbu = -u_max * np.ones(nu) ocp.constraints.ubu = u_max * np.ones(nu) ocp.constraints.idxbu = np.arange(nu, dtype=int) + ocp.constraints.idxbx = np.array([7]) + ocp.constraints.lbx = np.array([-1.4]) + ocp.constraints.ubx = np.array([1.4]) + ocp.dims.nbx = 1 # ---------------------------------- # Initial state constraint (must be updated before solve) @@ -150,17 +174,26 @@ def create_auv_ocp(): # ---------------------------------- # Solver options # ---------------------------------- + print("W shape:", ocp.cost.W.shape) + print("W diagonal:", np.diag(ocp.cost.W)) + print("W_e shape:", ocp.cost.W_e.shape) + print("W_e diagonal:", np.diag(ocp.cost.W_e)) + print("Vx shape:", ocp.cost.Vx.shape) + print("Vx_e shape:", ocp.cost.Vx_e.shape) + print("yref:", ocp.cost.yref) + print("yref_e:", ocp.cost.yref_e) + print("ny:", ocp.dims.ny) + print("ny_e:", ocp.dims.ny_e) ocp.solver_options.qp_solver = "FULL_CONDENSING_HPIPM" ocp.solver_options.qp_solver_warm_start=1 ocp.solver_options.hessian_approx = "GAUSS_NEWTON" #ocp.solver_options.integrator_type = "ERK" - #cp.solver_options.nlp_solver_type = "SQP" # fast real-time iteration - ocp.solver_options.nlp_solver_max_iter = 100 + ocp.solver_options.nlp_solver_type = "SQP_RTI" # fast real-time iteration + #ocp.solver_options.nlp_solver_max_iter = 100 - ocp.solver_options.globalization = 'MERIT_BACKTRACKING' + #ocp.solver_options.globalization = 'MERIT_BACKTRACKING' ocp.solver_options.levenberg_marquardt = 1e-4 ocp.solver_options.print_level = 2 - ocp.constraints.idxe = np.array([], dtype=int) # ---------------------------------- # Output directory diff --git a/control/velocity_controller/src/test_VC.cpp b/control/velocity_controller/src/test_VC.cpp index 466b4026f..cb413c72e 100644 --- a/control/velocity_controller/src/test_VC.cpp +++ b/control/velocity_controller/src/test_VC.cpp @@ -46,7 +46,7 @@ void test_VC::send_guidance() reference_msg.yaw=0.6*sin(time1*std::numbers::pi/9); reference_msg.pitch=0.3*sin(time1*std::numbers::pi/9);*/ reference_msg.surge=1.0;reference_msg.pitch=0.3;reference_msg.yaw=-1.57; //Surge, pitch, yaw - RCLCPP_INFO(this->get_logger(), "guidance callback: %f, %f, %f",reference_msg.surge,reference_msg.pitch,reference_msg.yaw); + //RCLCPP_INFO(this->get_logger(), "guidance callback: %f, %f, %f",reference_msg.surge,reference_msg.pitch,reference_msg.yaw); publisher_guidance->publish(reference_msg); } diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index 58bac9f6e..b54e6cae9 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -50,11 +50,12 @@ Velocity_node::Velocity_node() : Node("velocity_controller_node"), PID_surge(100 std::bind(&Velocity_node::killswitch_callback,this, std::placeholders::_1)); - //NMPC controller - NMPC.set_matrices(Q2,R2, inertia_matrix, max_force, dampening_matrix_low, dampening_matrix_high); + //NMPC controller + /* + NMPC.set_matrices(Q2,R2, inertia_matrix, max_force, dampening_matrix_low, dampening_matrix_high); NMPC.set_interval(publish_rate/1000.0); NMPC.initialize_MPC(); - + */ //NMPC acados controller NMPC_acados.init(); NMPC_acados.set_max_force(max_force); @@ -78,8 +79,6 @@ Velocity_node::Velocity_node() : Node("velocity_controller_node"), PID_surge(100 controller_type=1; RCLCPP_INFO(this->get_logger(),"Switching to PID"); }; - - } @@ -156,7 +155,9 @@ void Velocity_node::calc_thrust() NMPC_acados.setReference(x_ref,u_ref); std::array state_array={current_state.surge,current_state.sway,current_state.heave,current_state.roll_rate,current_state.pitch_rate,current_state.yaw_rate,current_state.roll,current_state.pitch,current_state.yaw}; NMPC_acados.setState(state_array); - if(NMPC_acados.solve_once()){ + int status=NMPC_acados.solve_once(); + if(status){ + RCLCPP_ERROR(this->get_logger(),"Error status %i",status); rclcpp::shutdown(); }; u=NMPC_acados.getU0(); From 17d89c98ccfca092d9071c756aee3e5fd08ea7ad Mon Sep 17 00:00:00 2001 From: Anbit Date: Sun, 8 Mar 2026 15:30:10 +0100 Subject: [PATCH 069/128] Clean code and readMe draft --- guidance/los_guidance/README.md | 260 ++++++++++++++++-- .../los_guidance/config/guidance_params.yaml | 38 +-- .../include/los_guidance/lib/adaptive_los.hpp | 28 +- .../include/los_guidance/lib/integral_los.hpp | 21 +- .../los_guidance/lib/proportional_los.hpp | 18 +- .../include/los_guidance/lib/types.hpp | 21 +- .../los_guidance/lib/vector_field_los.hpp | 24 +- .../include/los_guidance/los_guidance_ros.hpp | 120 ++++---- .../launch/guidance_test.launch.py | 2 +- .../launch/los_guidance.launch.py | 2 - .../los_guidance/src/lib/adaptive_los.cpp | 45 +-- .../los_guidance/src/lib/integral_los.cpp | 27 +- .../los_guidance/src/lib/proportional_los.cpp | 23 +- .../los_guidance/src/lib/vector_field_los.cpp | 28 +- .../los_guidance/src/los_guidance_node.cpp | 4 +- .../los_guidance/src/los_guidance_ros.cpp | 109 ++++---- 16 files changed, 537 insertions(+), 233 deletions(-) diff --git a/guidance/los_guidance/README.md b/guidance/los_guidance/README.md index 33c616c59..2afe923c1 100644 --- a/guidance/los_guidance/README.md +++ b/guidance/los_guidance/README.md @@ -1,22 +1,60 @@ -## ALSO Guidance Law for 3D Path Following +# 3D LOS Guidance Library (Read me draft) -The guidance law gives calculates the desired heading angle $\psi_d$ and desired pitch angle $\theta_d$. The crab angles $\beta_c$ and $\alpha_c$ are estimated adaptively. The guidance law looks like +This package implements several **Line-of-Sight (LOS) guidance algorithms** for **3D path following**. +The guidance system computes the **desired yaw** $\psi_d$ and **desired pitch** $\theta_d$ so a vehicle can follow a path between waypoints. + +The vehicle surge speed is kept **constant** during path following and is defined in the configuration file using the parameter + +``` +u_desired +``` + +This value represents the desired forward velocity of the vehicle. + +--- + +# Implemented LOS Methods + +The library supports four LOS guidance algorithms. + +| Mode | Method | +|-----|------| +| 0 | Proportional LOS | +| 1 | Integral LOS | +| 2 | Adaptive LOS | +| 3 | Vector Field LOS | + +The guidance method can be **changed during runtime** using a ROS service. + +--- + +# Adaptive LOS (ALOS) + +Adaptive LOS estimates **crab angles caused by disturbances** such as ocean currents or wind. ```math \psi_d = \pi_h - \hat{\beta}_c - \tan^{-1}\left(\frac{y_e^p}{\Delta_h}\right) - ``` ```math -\dot{\hat{\beta}}_c = \gamma_h \frac{\Delta_h}{\sqrt{\Delta_h^2 + (y_e^p)^2}} y_e^p +\dot{\hat{\beta}}_c = +\gamma_h +\frac{\Delta_h}{\sqrt{\Delta_h^2 + (y_e^p)^2}} +y_e^p ``` ```math -\theta_d = \pi_v + \hat{\alpha}_c + \tan^{-1}\left(\frac{z_e^p}{\Delta_v}\right) +\theta_d = +\pi_v + +\hat{\alpha}_c + +\tan^{-1}\left(\frac{z_e^p}{\Delta_v}\right) ``` ```math -\dot{\hat{\alpha}}_c = \gamma_v \frac{\Delta_v}{\sqrt{\Delta_v^2 + (z_e^p)^2}} z_e^p +\dot{\hat{\alpha}}_c = +\gamma_v +\frac{\Delta_v}{\sqrt{\Delta_v^2 + (z_e^p)^2}} +z_e^p ``` where @@ -27,27 +65,217 @@ where - $y_e^p$ is the cross-track error - $z_e^p$ is the vertical-track error -The azimuth angle $\pi_v$ and the elevation angle $\pi_h$ can be found by +Adaptive LOS is generally the **most robust method** and works well for: + +- curved trajectories +- long paths +- environments with disturbances. + +--- + +# Proportional LOS (PLOS) + +Proportional LOS is the simplest LOS guidance law. + +```math +\psi_d = +\pi_h - +\tan^{-1}\left(\frac{y_e^p}{\Delta_h}\right) +``` + +```math +\theta_d = +\pi_v + +\tan^{-1}\left(\frac{z_e^p}{\Delta_v}\right) +``` + +It is best suited for + +- simple waypoint following +- calm environments with little disturbance. + +However, steady-state tracking error may occur if disturbances are present. + +--- + +# Integral LOS (ILOS) + +Integral LOS adds integral action to remove steady-state error. ```math -\pi_h = \text{atan2}(y_{i+1}^n - y_i^n, x_{i+1}^n, - x_i^n) +u_h = +k_{p,h}y_e^p + +k_{i,h}\int y_e^p dt ``` + ```math -\pi_v = \text{atan2}(-(z_{i+1}^n - z_i^n), \sqrt{(x_{i+1}^n - x_i^n)^2 + (y_{i+1}^n - y_i^n)^2}) +u_v = +k_{p,v}z_e^p + +k_{i,v}\int z_e^p dt ``` -where $P_i^n = (x_i^n, y_i^n, z_i^n)$ is the previous waypoint in the north-east-down frame and $P_{i+1}^n = (x_{i+1}^n, y_{i+1}^n, z_{i+1}^n)$ is the next waypoint in north-east-down frame. +```math +\psi_d = +\pi_h - +\tan^{-1}(u_h) +``` + +```math +\theta_d = +\pi_v + +\tan^{-1}(u_v) +``` + +Integral LOS performs well when there are **constant disturbances**, such as steady ocean currents or wind. + +--- + +# Vector Field LOS (VF-LOS) + +Vector Field LOS generates a **bounded approach angle** toward the path. + +```math +\psi_d = \pi_h - \chi_h +``` + +```math +\chi_h = +\psi_{max} +\frac{2}{\pi} +\tan^{-1}(k_p y_e^p) +``` + +This method is best suited for + +- long straight path following +- corridor tracking. + +However it performs worse when the path contains **sharp turns**. + +--- + +# Path Geometry + +The path between two waypoints defines the reference angles used by the guidance law. + +```math +\pi_h = +\text{atan2}(y_{i+1}^n - y_i^n, x_{i+1}^n - x_i^n) +``` + +```math +\pi_v = +\text{atan2} +\left( +-(z_{i+1}^n - z_i^n), +\sqrt{(x_{i+1}^n - x_i^n)^2 + +(y_{i+1}^n - y_i^n)^2} +\right) +``` + +where + +- $P_i^n = (x_i^n, y_i^n, z_i^n)$ is the previous waypoint in the north-east-down frame +- $P_{i+1}^n = (x_{i+1}^n, y_{i+1}^n, z_{i+1}^n)$ is the next waypoint. + +--- + +# Path Frame Errors The along-, cross- and vertical-track errors in the path-tangential frame are found by ```math \begin{bmatrix} -x_e^p \\ y_e^p \\ z_e^p -\end{bmatrix} = \mathbf{R}_{y, \pi_v}^\top \mathbf{R}_{z, \pi_h}^\top \left( \begin{bmatrix} -x^n \\ y^n \\ z^n -\end{bmatrix} - \begin{bmatrix} -x_i^n \\ y_i^n \\ z_i^n +x_e^p \\ +y_e^p \\ +z_e^p +\end{bmatrix} += +\mathbf{R}_{y,\pi_v}^\top +\mathbf{R}_{z,\pi_h}^\top +\left( +\begin{bmatrix} +x^n \\ +y^n \\ +z^n +\end{bmatrix} +- +\begin{bmatrix} +x_i^n \\ +y_i^n \\ +z_i^n \end{bmatrix} \right) ``` -where $P^n = (x^n, y^n, z^n)$ is the current position of the drone. + +where + +- $x_e^p$ is the along-track error +- $y_e^p$ is the cross-track error +- $z_e^p$ is the vertical-track error + +and + +- $P^n = (x^n, y^n, z^n)$ is the current position of the vehicle. + +--- + +# Sending a LOS Goal + +A waypoint can be sent to the guidance node using the action interface: + +``` +ros2 action send_goal /orca/los_guidance \ +vortex_msgs/action/LOSGuidance \ +"{goal: {header: {frame_id: world_ned}, point: {x: 10.0, y: 5.0, z: -2.0}}}" +``` + +This command instructs the guidance node to start following a path toward the waypoint. + +--- + +# Switching LOS Method + +The active LOS guidance method can be changed during runtime. + +``` +ros2 service call /orca/set_los_mode \ +vortex_msgs/srv/SetLosMode "{mode: X}" +``` + +Where + +| X | Method | +|---|---| +| 0 | Proportional LOS | +| 1 | Integral LOS | +| 2 | Adaptive LOS | +| 3 | Vector Field LOS | + +Example: + +``` +ros2 service call /orca/set_los_mode vortex_msgs/srv/SetLosMode "{mode: 2}" +``` + +This switches the guidance system to **Adaptive LOS**. + +--- + +# ROS Topics + +### Subscribed Topics + +| Topic | Message | +|------|------| +| `/orca/pose` | `geometry_msgs/PoseWithCovarianceStamped` | +| `/orca/odom` | `nav_msgs/Odometry` | +| `/orca/waypoint` | `geometry_msgs/PointStamped` | + +### Published Topics + +| Topic | Message | +|------|------| +| `/orca/guidance/los` | `vortex_msgs/LOSGuidance` | +| `/los_debug` | `vortex_msgs/LOSGuidance` | +| `/state_debug` | `vortex_msgs/LOSGuidance` | \ No newline at end of file diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 66bd0c2e3..8ba560c1a 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -1,30 +1,36 @@ +# Adaptive LOS Parameters adaptive_los: - lookahead_distance_h: 0.9 #Done - lookahead_distance_v: 1.4 #Done - gamma_h: 0.03 #Done - gamma_v: 0.02 #Done + lookahead_distance_h: 0.9 + lookahead_distance_v: 1.4 + gamma_h: 0.03 + gamma_v: 0.02 +# Proportional LOS Parameters prop_los: - lookahead_distance_h: 0.74 #Done - lookahead_distance_v: 0.8 #Done + lookahead_distance_h: 0.74 + lookahead_distance_v: 0.8 +# Integral LOS Parameters integer_los: - k_p_h: 0.5 #Done - k_p_v: 0.5 #Done - k_i_h: 0.1 #Done - k_i_v: 0.1 #Done + k_p_h: 0.5 + k_p_v: 0.5 + k_i_h: 0.1 + k_i_v: 0.1 +# Vector Field LOS Parameters vector_field_los: - max_approach_angle_h: 1.0 - max_approach_angle_v: 0.6 + max_approach_angle_h: 1.0 + max_approach_angle_v: 1.0 k_p_h: 1.5 - k_p_v: 0.3 + k_p_v: 0.9 +# Common Guidance Parameters common: - active_los_method: 3 # 0: Proportional, 1: Integral, 2: Adaptive, 3: VFLos - u_desired: 0.3 # Done - goal_reached_tol: 0.35 # Done + active_los_method: 2 + u_desired: 0.3 + goal_reached_tol: 0.35 +# Debug Settings debug: enable_debug: true debug_topic_name: "/los_guidance_debug" \ No newline at end of file diff --git a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp index cca8b3553..f7174a89b 100644 --- a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp @@ -4,46 +4,56 @@ #include #include #include -#include "los_guidance/lib/types.hpp" -/** - * @brief Adaptive Line-of-Sight (LOS) guidance algorithm based on slide 113 - * in "Fossen 2024 Lecture on 2D and 3D path-following control". - */ +#include "los_guidance/lib/types.hpp" namespace vortex::guidance::los { +// Parameter Structure struct AdaptiveLosParams { double lookahead_distance_h{}; double lookahead_distance_v{}; double gamma_h{}; double gamma_v{}; - double time_step{}; + double time_step{}; }; +// Adaptive LOS Guidance Class class AdaptiveLOSGuidance { public: + + // Constructor / Destructor AdaptiveLOSGuidance(const AdaptiveLosParams& params); ~AdaptiveLOSGuidance() = default; + // Main Output Calculation types::Outputs calculate_outputs(const types::Inputs& inputs); private: + + // Internal Update Functions void update_angles(const types::Inputs& inputs); const types::CrossTrackError calculate_crosstrack_error( const types::Inputs& inputs); void update_adaptive_estimates( const types::CrossTrackError& cross_track_error); + // Parameters AdaptiveLosParams params_{}; + + // Rotation Matrices Eigen::Matrix3d rotation_y_ = Eigen::Matrix3d::Zero(); Eigen::Matrix3d rotation_z_ = Eigen::Matrix3d::Zero(); + + // Path Angles double pi_h_{}; double pi_v_{}; + + // Adaptive Estimates double beta_c_hat_ = 0.0; double alpha_c_hat_ = 0.0; - -}; // namespace vortex::guidance::los +}; } // namespace vortex::guidance::los -#endif // LOS_GUIDANCE_HPP + +#endif // ADAPTIVE_LOS_GUIDANCE_HPP \ No newline at end of file diff --git a/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp b/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp index 0509aaa28..83a528461 100644 --- a/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp @@ -4,40 +4,53 @@ #include #include #include + #include "los_guidance/lib/types.hpp" namespace vortex::guidance::los { -struct IntegralLosParams { +// Parameter Structure +struct IntegralLosParams { double k_p_h{}; double k_p_v{}; double k_i_h{}; double k_i_v{}; double time_step{}; -}; +}; +// Integral LOS Guidance Class class IntegralLOSGuidance { public: + + // Constructor / Destructor IntegralLOSGuidance(const IntegralLosParams& params); ~IntegralLOSGuidance() = default; + // Main Output Calculation types::Outputs calculate_outputs(const types::Inputs& inputs); private: + // Internal Update Functions void update_angles(const types::Inputs& inputs); types::CrossTrackError calculate_crosstrack_error( const types::Inputs& inputs); + // Parameters IntegralLosParams m_params{}; + // Integral States double int_h{}; double int_v{}; - // again i dont know if i should have them here or just in the functions + + // Path Angles double pi_h_{}; double pi_v_{}; + + // Rotation Representation Eigen::AngleAxisd rotation_y_{0.0, Eigen::Vector3d::UnitY()}; Eigen::AngleAxisd rotation_z_{0.0, Eigen::Vector3d::UnitZ()}; }; + } // namespace vortex::guidance::los -#endif // INTEGRAL_LOS_GUIDANCE_HPP +#endif // INTEGRAL_LOS_GUIDANCE_HPP \ No newline at end of file diff --git a/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp b/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp index 1dd96654c..a56a5efae 100644 --- a/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp @@ -4,33 +4,47 @@ #include #include #include + #include "los_guidance/lib/types.hpp" namespace vortex::guidance::los { + +// Parameter Structure struct ProportionalLosParams { double lookahead_distance_h{}; double lookahead_distance_v{}; }; -class ProportionalLOSGuidance { +// Proportional LOS Guidance Class +class ProportionalLOSGuidance { public: + + // Constructor / Destructor ProportionalLOSGuidance(const ProportionalLosParams& params); ~ProportionalLOSGuidance() = default; + // Main Output Calculation types::Outputs calculate_outputs(const types::Inputs& inputs); private: + + // Internal Update Functions void update_angles(const types::Inputs& inputs); types::CrossTrackError calculate_crosstrack_error( const types::Inputs& inputs) const; + // Parameters ProportionalLosParams m_params{}; + + // Path Angles double pi_h_{0.0}; double pi_v_{0.0}; + + // Rotation Representation Eigen::AngleAxisd rotation_y_{0.0, Eigen::Vector3d::UnitY()}; Eigen::AngleAxisd rotation_z_{0.0, Eigen::Vector3d::UnitZ()}; }; } // namespace vortex::guidance::los -#endif // PROPORTIONAL_LOS_GUIDANCE_HPP +#endif // PROPORTIONAL_LOS_GUIDANCE_HPP \ No newline at end of file diff --git a/guidance/los_guidance/include/los_guidance/lib/types.hpp b/guidance/los_guidance/include/los_guidance/lib/types.hpp index ca2492d10..a12aa5078 100644 --- a/guidance/los_guidance/include/los_guidance/lib/types.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/types.hpp @@ -9,22 +9,28 @@ namespace vortex::guidance::los::types { +// Point Representation struct Point { double x{}; double y{}; double z{}; + // Point Operations Point operator-(const Point& other) const { return Point{x - other.x, y - other.y, z - other.z}; } - Eigen::Vector3d as_vector() const { return Eigen::Vector3d(x, y, z); } + // Conversion Functions + Eigen::Vector3d as_vector() const { + return Eigen::Vector3d(x, y, z); + } static Point point_from_ros(const geometry_msgs::msg::Point& msg) { return Point{msg.x, msg.y, msg.z}; } }; +// Cross Track Error struct CrossTrackError { double x_e{}; double y_e{}; @@ -35,24 +41,27 @@ struct CrossTrackError { } }; +// Guidance Outputs struct Outputs { double psi_d{}; double theta_d{}; }; +// Guidance Inputs struct Inputs { Point prev_point{}; Point next_point{}; Point current_position{}; }; +// Active LOS Method enum class ActiveLosMethod { - PROPORTIONAL, // 0 - INTEGRAL, // 1 - ADAPTIVE, // 2 - VECTOR_FIELD // 3 + PROPORTIONAL, + INTEGRAL, + ADAPTIVE, + VECTOR_FIELD }; } // namespace vortex::guidance::los::types -#endif +#endif // TYPES_HPP \ No newline at end of file diff --git a/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp b/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp index 9d3210ffd..3348b0f74 100644 --- a/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp @@ -3,39 +3,51 @@ #include #include -#include +#include + #include "los_guidance/lib/types.hpp" namespace vortex::guidance::los { +// Parameter Structure struct VectorFieldLosParams { - double max_approach_angle_h{}; - double max_approach_angle_v{}; - double k_p_h{}; - double k_p_v{}; + double max_approach_angle_h{}; + double max_approach_angle_v{}; + double k_p_h{}; + double k_p_v{}; double time_step{}; }; +// Vector Field LOS Guidance Class class VectorFieldLOSGuidance { public: + + // Constructor / Destructor VectorFieldLOSGuidance(const VectorFieldLosParams& params); ~VectorFieldLOSGuidance() = default; + // Main Output Calculation types::Outputs calculate_outputs(const types::Inputs& inputs); private: + + // Internal Update Functions void update_angles(const types::Inputs& inputs); types::CrossTrackError calculate_crosstrack_error( const types::Inputs& inputs) const; + // Parameters VectorFieldLosParams m_params{}; + // Path Angles double pi_h_{0.0}; double pi_v_{0.0}; + + // Rotation Representation Eigen::AngleAxisd rotation_y_{0.0, Eigen::Vector3d::UnitY()}; Eigen::AngleAxisd rotation_z_{0.0, Eigen::Vector3d::UnitZ()}; }; } // namespace vortex::guidance::los -#endif // VECTOR_FIELD_LOS_GUIDANCE_HPP +#endif // VECTOR_FIELD_LOS_GUIDANCE_HPP \ No newline at end of file diff --git a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp index a3ba2f764..e7bbc1482 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -1,160 +1,134 @@ #ifndef LOS_GUIDANCE__LOS_GUIDANCE_ROS_HPP_ #define LOS_GUIDANCE__LOS_GUIDANCE_ROS_HPP_ -#include -#include #include #include #include #include -#include #include #include +#include #include +#include #include #include #include +#include + +#include +#include + #include "los_guidance/lib/adaptive_los.hpp" #include "los_guidance/lib/integral_los.hpp" -#include "los_guidance/lib/proportional_los.hpp" +#include "los_guidance/lib/proportional_los.hpp" +#include "los_guidance/lib/types.hpp" #include "los_guidance/lib/vector_field_los.hpp" -#include "los_guidance/lib/types.hpp" -#include -#include - namespace vortex::guidance::los { +// LOS Guidance ROS Node class LosGuidanceNode : public rclcpp::Node { public: + + // Constructor LosGuidanceNode(); private: - // @brief Set the subscribers and publishers - void set_subscribers_and_publisher(); - // @brief Set the action server + // Setup Functions + void set_subscribers_and_publisher(); void set_action_server(); - - // @brief Determine the LOS mode service void set_service_server(); - // @brief Set the adaptive LOS guidance parameters + // Configuration Functions void set_adaptive_los_guidance(YAML::Node config); - - // @brief Set the proportional LOS guidance parameters void set_proportional_los_guidance(YAML::Node config); - - // @brief Set the integral LOS guidance parameters void set_integral_los_guidance(YAML::Node config); - - // @brief Set the vector field LOS guidance parameters void set_vector_field_guidance(YAML::Node config); + YAML::Node get_los_config(std::string yaml_file_path); + void parse_common_config(YAML::Node common_config); - // @brief Callback for the waypoint topic - // @param msg The reference message + // Callback Functions void waypoint_callback( const geometry_msgs::msg::PointStamped::SharedPtr msg); - - // @brief Callback for the pose topic - // @param msg The pose message void pose_callback( const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); - void odom_callback( const nav_msgs::msg::Odometry::SharedPtr msg); - // @brief Handle the goal request - // @param uuid The goal UUID - // @param goal The goal message - // @return The goal response + // Action Server Functions rclcpp_action::GoalResponse handle_goal( const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal); - - // @brief Handle the cancel request - // @param goal_handle The goal handle - // @return The cancel response rclcpp_action::CancelResponse handle_cancel( const std::shared_ptr< rclcpp_action::ServerGoalHandle> goal_handle); + void handle_accepted( + const std::shared_ptr< + rclcpp_action::ServerGoalHandle> + goal_handle); + void execute( + const std::shared_ptr< + rclcpp_action::ServerGoalHandle> + goal_handle); - // @brief Handle the accepted request - // @param goal_handle The goal handle - void handle_accepted(const std::shared_ptr> goal_handle); - - // @brief Execute the goal - // @param goal_handle The goal handle - - bool has_active_segment_{false}; - void execute(const std::shared_ptr> goal_handle); - + // Service Functions void set_los_mode( const std::shared_ptr request, std::shared_ptr response); - void publish_state_debug(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr - current_pose); - + // Publish Functions + void publish_state_debug( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr + current_pose); vortex_msgs::msg::LOSGuidance fill_los_reference(types::Outputs output); - YAML::Node get_los_config(std::string yaml_file_path); - - void parse_common_config(YAML::Node common_config); + // State Flags + bool has_active_segment_{false}; + // ROS Interfaces rclcpp_action::Server::SharedPtr action_server_; - rclcpp::Service::SharedPtr los_mode_service_; - rclcpp::Publisher::SharedPtr reference_pub_; - rclcpp::Publisher::SharedPtr los_debug_pub_; - - rclcpp::Publisher::SharedPtr state_debug_pub_; - + rclcpp::Publisher::SharedPtr + state_debug_pub_; rclcpp::Subscription::SharedPtr waypoint_sub_; - rclcpp::Subscription< geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_; - - rclcpp::Subscription< - nav_msgs::msg::Odometry>::SharedPtr odom_sub_; - - + rclcpp::Subscription::SharedPtr odom_sub_; rclcpp::TimerBase::SharedPtr reference_pub_timer_; + rclcpp::CallbackGroup::SharedPtr cb_group_; + // Timing and Synchronization std::chrono::milliseconds time_step_; - std::mutex mutex_; + // Action State rclcpp_action::GoalUUID preempted_goal_id_; - std::shared_ptr< rclcpp_action::ServerGoalHandle> goal_handle_; - rclcpp::CallbackGroup::SharedPtr cb_group_; - + // Guidance State types::Inputs path_inputs_{}; - double u_desired_{}; - double goal_reached_tol_{}; + types::ActiveLosMethod method_{}; + // Guidance Modules std::unique_ptr adaptive_los_{}; std::unique_ptr integral_los_{}; std::unique_ptr proportional_los_{}; std::unique_ptr vector_field_los_{}; - types::ActiveLosMethod method_{}; + // Debug Data nav_msgs::msg::Odometry::SharedPtr debug_current_odom_{}; }; } // namespace vortex::guidance::los - -#endif // LOS_GUIDANCE__LOS_GUIDANCE_ROS_HPP_ + +#endif // LOS_GUIDANCE_ROS_HPP \ No newline at end of file diff --git a/guidance/los_guidance/launch/guidance_test.launch.py b/guidance/los_guidance/launch/guidance_test.launch.py index 862e39b50..2bc63f2f9 100644 --- a/guidance/los_guidance/launch/guidance_test.launch.py +++ b/guidance/los_guidance/launch/guidance_test.launch.py @@ -17,7 +17,7 @@ def generate_launch_description(): ), launch_arguments={ 'scenario': 'tacc', - 'rendering': 'false', + 'rendering': 'true', }.items(), ) diff --git a/guidance/los_guidance/launch/los_guidance.launch.py b/guidance/los_guidance/launch/los_guidance.launch.py index 859c0a255..2a677d5c8 100644 --- a/guidance/los_guidance/launch/los_guidance.launch.py +++ b/guidance/los_guidance/launch/los_guidance.launch.py @@ -33,5 +33,3 @@ def generate_launch_description(): return LaunchDescription([los_guidance_node]) -# remember to make them able to swich in the middle of a mission and if you swirch method the parameters shouldn't be reloaded -# unless its a new section diff --git a/guidance/los_guidance/src/lib/adaptive_los.cpp b/guidance/los_guidance/src/lib/adaptive_los.cpp index 52607bb4e..a4acee79f 100644 --- a/guidance/los_guidance/src/lib/adaptive_los.cpp +++ b/guidance/los_guidance/src/lib/adaptive_los.cpp @@ -1,11 +1,14 @@ #include + #include "los_guidance/lib/types.hpp" namespace vortex::guidance::los { +// Constructor AdaptiveLOSGuidance::AdaptiveLOSGuidance(const AdaptiveLosParams& params) - : params_{params} {} + : params_{params} {} +// Angle Update void AdaptiveLOSGuidance::update_angles(const types::Inputs& inputs) { const double dx = inputs.next_point.x - inputs.prev_point.x; const double dy = inputs.next_point.y - inputs.prev_point.y; @@ -18,6 +21,7 @@ void AdaptiveLOSGuidance::update_angles(const types::Inputs& inputs) { rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); } +// Cross-Track Error Calculation const types::CrossTrackError AdaptiveLOSGuidance::calculate_crosstrack_error( const types::Inputs& inputs) { const types::Point difference = inputs.current_position - inputs.prev_point; @@ -28,40 +32,47 @@ const types::CrossTrackError AdaptiveLOSGuidance::calculate_crosstrack_error( return types::CrossTrackError::from_vector(cross_track_error); } - + +// Adaptive Estimate Update void AdaptiveLOSGuidance::update_adaptive_estimates( const types::CrossTrackError& cross_track_error) { - const double denom_h = std::sqrt(params_.lookahead_distance_h * - params_.lookahead_distance_h + - cross_track_error.y_e * cross_track_error.y_e); - const double denom_v = std::sqrt(params_.lookahead_distance_v * - params_.lookahead_distance_v + - cross_track_error.z_e * cross_track_error.z_e); + const double denom_h = + std::sqrt(params_.lookahead_distance_h * params_.lookahead_distance_h + + cross_track_error.y_e * cross_track_error.y_e); + const double denom_v = + std::sqrt(params_.lookahead_distance_v * params_.lookahead_distance_v + + cross_track_error.z_e * cross_track_error.z_e); const double beta_dot = - params_.gamma_h * (params_.lookahead_distance_h / denom_h) * cross_track_error.y_e; + params_.gamma_h * (params_.lookahead_distance_h / denom_h) * + cross_track_error.y_e; const double alpha_dot = - params_.gamma_v * (params_.lookahead_distance_v / denom_v) * cross_track_error.z_e; + params_.gamma_v * (params_.lookahead_distance_v / denom_v) * + cross_track_error.z_e; beta_c_hat_ += beta_dot * params_.time_step; alpha_c_hat_ += alpha_dot * params_.time_step; } +// Output Calculation types::Outputs AdaptiveLOSGuidance::calculate_outputs( const types::Inputs& inputs) { update_angles(inputs); - const types::CrossTrackError cross_track_error = calculate_crosstrack_error(inputs); + + const types::CrossTrackError cross_track_error = + calculate_crosstrack_error(inputs); + update_adaptive_estimates(cross_track_error); const double psi_d = - pi_h_ - beta_c_hat_ - std::atan(cross_track_error.y_e / params_.lookahead_distance_h); + pi_h_ - beta_c_hat_ - + std::atan(cross_track_error.y_e / params_.lookahead_distance_h); + const double theta_d = - pi_v_ + alpha_c_hat_ + std::atan(cross_track_error.z_e / params_.lookahead_distance_v); + pi_v_ + alpha_c_hat_ + + std::atan(cross_track_error.z_e / params_.lookahead_distance_v); - return types::Outputs{psi_d, theta_d}; } - - -} // namespace vortex::guidance::los +} // namespace vortex::guidance::los \ No newline at end of file diff --git a/guidance/los_guidance/src/lib/integral_los.cpp b/guidance/los_guidance/src/lib/integral_los.cpp index c0450a62f..3da6deb5a 100644 --- a/guidance/los_guidance/src/lib/integral_los.cpp +++ b/guidance/los_guidance/src/lib/integral_los.cpp @@ -2,9 +2,11 @@ namespace vortex::guidance::los { +// Constructor IntegralLOSGuidance::IntegralLOSGuidance(const IntegralLosParams& params) : m_params{params} {} +// Angle Update void IntegralLOSGuidance::update_angles(const types::Inputs& inputs) { const types::Point difference = inputs.next_point - inputs.prev_point; @@ -13,30 +15,37 @@ void IntegralLOSGuidance::update_angles(const types::Inputs& inputs) { difference.y * difference.y)); rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); - rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); -} + rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); +} +// Cross-Track Error Calculation types::CrossTrackError IntegralLOSGuidance::calculate_crosstrack_error( const types::Inputs& inputs) { const Eigen::Vector3d diff_vec = (inputs.current_position - inputs.prev_point).as_vector(); - const Eigen::Vector3d path_frame_error = rotation_y_.toRotationMatrix().transpose() * - rotation_z_.toRotationMatrix().transpose() * - diff_vec; + + const Eigen::Vector3d path_frame_error = + rotation_y_.toRotationMatrix().transpose() * + rotation_z_.toRotationMatrix().transpose() * diff_vec; return types::CrossTrackError::from_vector(path_frame_error); } +// Output Calculation types::Outputs IntegralLOSGuidance::calculate_outputs( const types::Inputs& inputs) { update_angles(inputs); - const types::CrossTrackError cross_track_error = calculate_crosstrack_error(inputs); + + const types::CrossTrackError cross_track_error = + calculate_crosstrack_error(inputs); int_h += cross_track_error.y_e * m_params.time_step; int_v += cross_track_error.z_e * m_params.time_step; - const double u_h = m_params.k_p_h * cross_track_error.y_e + m_params.k_i_h * int_h; - const double u_v = m_params.k_p_v * cross_track_error.z_e + m_params.k_i_v * int_v; + const double u_h = + m_params.k_p_h * cross_track_error.y_e + m_params.k_i_h * int_h; + const double u_v = + m_params.k_p_v * cross_track_error.z_e + m_params.k_i_v * int_v; const double psi_d = pi_h_ - std::atan(u_h); const double theta_d = pi_v_ + std::atan(u_v); @@ -44,4 +53,4 @@ types::Outputs IntegralLOSGuidance::calculate_outputs( return types::Outputs{psi_d, theta_d}; } -} // namespace vortex::guidance::los +} // namespace vortex::guidance::los \ No newline at end of file diff --git a/guidance/los_guidance/src/lib/proportional_los.cpp b/guidance/los_guidance/src/lib/proportional_los.cpp index ae7aeff4e..33aeaa5d6 100644 --- a/guidance/los_guidance/src/lib/proportional_los.cpp +++ b/guidance/los_guidance/src/lib/proportional_los.cpp @@ -2,10 +2,12 @@ namespace vortex::guidance::los { +// Constructor ProportionalLOSGuidance::ProportionalLOSGuidance( - const ProportionalLosParams& params) : m_params{params} { - } + const ProportionalLosParams& params) + : m_params{params} {} +// Angle Update void ProportionalLOSGuidance::update_angles(const types::Inputs& inputs) { const types::Point difference = inputs.next_point - inputs.prev_point; @@ -17,21 +19,26 @@ void ProportionalLOSGuidance::update_angles(const types::Inputs& inputs) { rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); } +// Cross-Track Error Calculation types::CrossTrackError ProportionalLOSGuidance::calculate_crosstrack_error( const types::Inputs& inputs) const { const Eigen::Vector3d diff_vec = (inputs.current_position - inputs.prev_point).as_vector(); - const Eigen::Vector3d path_frame_error = rotation_y_.toRotationMatrix().transpose() * - rotation_z_.toRotationMatrix().transpose() * - diff_vec; + + const Eigen::Vector3d path_frame_error = + rotation_y_.toRotationMatrix().transpose() * + rotation_z_.toRotationMatrix().transpose() * diff_vec; return types::CrossTrackError::from_vector(path_frame_error); } +// Output Calculation types::Outputs ProportionalLOSGuidance::calculate_outputs( const types::Inputs& inputs) { update_angles(inputs); - const types::CrossTrackError cross_track_error = calculate_crosstrack_error(inputs); + + const types::CrossTrackError cross_track_error = + calculate_crosstrack_error(inputs); const double k_p_h = 1.0 / m_params.lookahead_distance_h; const double k_p_v = 1.0 / m_params.lookahead_distance_v; @@ -40,6 +47,6 @@ types::Outputs ProportionalLOSGuidance::calculate_outputs( const double theta_d = pi_v_ + std::atan(k_p_v * cross_track_error.z_e); return types::Outputs{psi_d, theta_d}; -} +} -} // namespace vortex::guidance::los +} // namespace vortex::guidance::los \ No newline at end of file diff --git a/guidance/los_guidance/src/lib/vector_field_los.cpp b/guidance/los_guidance/src/lib/vector_field_los.cpp index 52b4f1644..52eb51ead 100644 --- a/guidance/los_guidance/src/lib/vector_field_los.cpp +++ b/guidance/los_guidance/src/lib/vector_field_los.cpp @@ -2,9 +2,12 @@ namespace vortex::guidance::los { -VectorFieldLOSGuidance::VectorFieldLOSGuidance(const VectorFieldLosParams& params) +// Constructor +VectorFieldLOSGuidance::VectorFieldLOSGuidance( + const VectorFieldLosParams& params) : m_params{params} {} +// Angle Update void VectorFieldLOSGuidance::update_angles(const types::Inputs& inputs) { const types::Point difference = inputs.next_point - inputs.prev_point; @@ -16,34 +19,39 @@ void VectorFieldLOSGuidance::update_angles(const types::Inputs& inputs) { rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); } +// Cross-Track Error Calculation types::CrossTrackError VectorFieldLOSGuidance::calculate_crosstrack_error( const types::Inputs& inputs) const { const Eigen::Vector3d diff_vec = (inputs.current_position - inputs.prev_point).as_vector(); - const Eigen::Vector3d path_frame_error = rotation_y_.toRotationMatrix().transpose() * - rotation_z_.toRotationMatrix().transpose() * - diff_vec; + const Eigen::Vector3d path_frame_error = + rotation_y_.toRotationMatrix().transpose() * + rotation_z_.toRotationMatrix().transpose() * diff_vec; return types::CrossTrackError::from_vector(path_frame_error); } +// Output Calculation types::Outputs VectorFieldLOSGuidance::calculate_outputs( const types::Inputs& inputs) { update_angles(inputs); - const types::CrossTrackError cross_track_error = calculate_crosstrack_error(inputs); + + const types::CrossTrackError cross_track_error = + calculate_crosstrack_error(inputs); const double approach_h = - m_params.max_approach_angle_h * (2.0 / M_PI) * std::atan(m_params.k_p_h * cross_track_error.y_e); + m_params.max_approach_angle_h * (2.0 / M_PI) * + std::atan(m_params.k_p_h * cross_track_error.y_e); const double approach_v = - m_params.max_approach_angle_v * (2.0 / M_PI) * std::atan(m_params.k_p_v * cross_track_error.z_e); + m_params.max_approach_angle_v * (2.0 / M_PI) * + std::atan(m_params.k_p_v * cross_track_error.z_e); const double psi_d = pi_h_ - approach_h; - - const double theta_d = pi_v_ + approach_v; + const double theta_d = pi_v_ - approach_v; return types::Outputs{psi_d, theta_d}; } -} // namespace vortex::guidance::los +} // namespace vortex::guidance::los \ No newline at end of file diff --git a/guidance/los_guidance/src/los_guidance_node.cpp b/guidance/los_guidance/src/los_guidance_node.cpp index 439730a0c..29a19da13 100644 --- a/guidance/los_guidance/src/los_guidance_node.cpp +++ b/guidance/los_guidance/src/los_guidance_node.cpp @@ -1,6 +1,8 @@ #include + #include "los_guidance/los_guidance_ros.hpp" +// Main Entry Point int main(int argc, char** argv) { rclcpp::init(argc, argv); @@ -11,4 +13,4 @@ int main(int argc, char** argv) { executor.spin(); return 0; -} +} \ No newline at end of file diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index ba816d4d9..9bc88875e 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -1,8 +1,10 @@ #include "los_guidance/los_guidance_ros.hpp" + #include #include -#include #include +#include + #include "los_guidance/lib/types.hpp" const auto start_message = R"( @@ -10,18 +12,18 @@ const auto start_message = R"( | | / _ \/ ___| / ___|_ _(_) __| | __ _ _ __ ___ ___ | | | | | \___ \ | | _| | | | |/ _` |/ _` | '_ \ / __/ _ \ | |__| |_| |___) | | |_| | |_| | | (_| | (_| | | | | (_| __/ - |_____\___/|____/ \____|\__,_|_|\__,_|\__,_|_| |_|\___\___| + |_____\___/|____/ \____|\__,_|_|\__,_|\__,_|_| |_|\___\___| )"; namespace vortex::guidance::los { +// Constructor LosGuidanceNode::LosGuidanceNode() : Node("los_guidance_node") { double time_step_s = this->declare_parameter("time_step"); time_step_ = std::chrono::milliseconds(static_cast(time_step_s * 1000)); - // auto config = this->declare_parameter("los_config_file"); - // Do you need yaml path here? Can't you just use ros params directly from the guidance_params.yaml file? + const std::string yaml_path = this->declare_parameter("los_config_file"); @@ -34,11 +36,12 @@ LosGuidanceNode::LosGuidanceNode() : Node("los_guidance_node") { set_adaptive_los_guidance(config); set_proportional_los_guidance(config); set_integral_los_guidance(config); - set_vector_field_guidance(config); + set_vector_field_guidance(config); spdlog::info(start_message); } +// ROS Setup void LosGuidanceNode::set_subscribers_and_publisher() { this->declare_parameter("topics.pose"); this->declare_parameter("topics.guidance.los"); @@ -50,7 +53,6 @@ void LosGuidanceNode::set_subscribers_and_publisher() { this->get_parameter("topics.guidance.los").as_string(); std::string waypoint_topic = this->get_parameter("topics.waypoint").as_string(); - std::string odom_topic = this->get_parameter("topics.odom").as_string(); @@ -65,11 +67,11 @@ void LosGuidanceNode::set_subscribers_and_publisher() { state_debug_pub_ = this->create_publisher( "state_debug", qos_sensor_data); - waypoint_sub_ = this->create_subscription< - geometry_msgs::msg::PointStamped>( - waypoint_topic, qos_sensor_data, - std::bind(&LosGuidanceNode::waypoint_callback, this, - std::placeholders::_1)); + waypoint_sub_ = + this->create_subscription( + waypoint_topic, qos_sensor_data, + std::bind(&LosGuidanceNode::waypoint_callback, this, + std::placeholders::_1)); pose_sub_ = this->create_subscription< geometry_msgs::msg::PoseWithCovarianceStamped>( @@ -77,17 +79,17 @@ void LosGuidanceNode::set_subscribers_and_publisher() { std::bind(&LosGuidanceNode::pose_callback, this, std::placeholders::_1)); - odom_sub_ = this->create_subscription< - nav_msgs::msg::Odometry>( + odom_sub_ = this->create_subscription( odom_topic, qos_sensor_data, std::bind(&LosGuidanceNode::odom_callback, this, - std::placeholders::_1)); + std::placeholders::_1)); } void LosGuidanceNode::set_action_server() { this->declare_parameter("action_servers.los"); std::string action_server_name = this->get_parameter("action_servers.los").as_string(); + cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); @@ -113,19 +115,18 @@ void LosGuidanceNode::set_service_server() { std::placeholders::_1, std::placeholders::_2)); } +// Guidance Configuration void LosGuidanceNode::set_adaptive_los_guidance(YAML::Node config) { auto adaptive_los_config = config["adaptive_los"]; auto params = AdaptiveLosParams{}; + params.lookahead_distance_h = adaptive_los_config["lookahead_distance_h"].as(); params.lookahead_distance_v = adaptive_los_config["lookahead_distance_v"].as(); - params.gamma_h = - adaptive_los_config["gamma_h"].as(); - params.gamma_v = - adaptive_los_config["gamma_v"].as(); - params.time_step = - static_cast(time_step_.count()) / 1000.0; + params.gamma_h = adaptive_los_config["gamma_h"].as(); + params.gamma_v = adaptive_los_config["gamma_v"].as(); + params.time_step = static_cast(time_step_.count()) / 1000.0; adaptive_los_ = std::make_unique(params); } @@ -133,6 +134,7 @@ void LosGuidanceNode::set_adaptive_los_guidance(YAML::Node config) { void LosGuidanceNode::set_proportional_los_guidance(YAML::Node config) { auto proportional_los_config = config["prop_los"]; auto params = ProportionalLosParams{}; + params.lookahead_distance_h = proportional_los_config["lookahead_distance_h"].as(); params.lookahead_distance_v = @@ -144,39 +146,34 @@ void LosGuidanceNode::set_proportional_los_guidance(YAML::Node config) { void LosGuidanceNode::set_integral_los_guidance(YAML::Node config) { auto integral_los_config = config["integer_los"]; auto params = IntegralLosParams{}; - params.k_p_h = - integral_los_config["k_p_h"].as(); - params.k_p_v = - integral_los_config["k_p_v"].as(); - params.k_i_h = - integral_los_config["k_i_h"].as(); - params.k_i_v = - integral_los_config["k_i_v"].as(); - params.time_step = - static_cast(time_step_.count()) / 1000.0; + + params.k_p_h = integral_los_config["k_p_h"].as(); + params.k_p_v = integral_los_config["k_p_v"].as(); + params.k_i_h = integral_los_config["k_i_h"].as(); + params.k_i_v = integral_los_config["k_i_v"].as(); + params.time_step = static_cast(time_step_.count()) / 1000.0; + integral_los_ = std::make_unique(params); } void LosGuidanceNode::set_vector_field_guidance(YAML::Node config) { auto vector_field_config = config["vector_field_los"]; auto params = VectorFieldLosParams{}; + params.max_approach_angle_h = vector_field_config["max_approach_angle_h"].as(); params.max_approach_angle_v = vector_field_config["max_approach_angle_v"].as(); - params.k_p_h = - vector_field_config["k_p_h"].as(); - params.k_p_v = - vector_field_config["k_p_v"].as(); - params.time_step = - static_cast(time_step_.count()) / 1000.0; + params.k_p_h = vector_field_config["k_p_h"].as(); + params.k_p_v = vector_field_config["k_p_v"].as(); + params.time_step = static_cast(time_step_.count()) / 1000.0; vector_field_los_ = std::make_unique(params); } +// Topic Callbacks void LosGuidanceNode::waypoint_callback( - const geometry_msgs::msg::PointStamped::SharedPtr wp_msg) -{ + const geometry_msgs::msg::PointStamped::SharedPtr wp_msg) { std::lock_guard lock(mutex_); const auto new_wp = types::Point::point_from_ros(wp_msg->point); @@ -190,28 +187,30 @@ void LosGuidanceNode::waypoint_callback( path_inputs_.next_point = new_wp; } - spdlog::info("Received waypoint: ({}, {}, {})", new_wp.x, new_wp.y, new_wp.z); + spdlog::info("Received waypoint: ({}, {}, {})", new_wp.x, new_wp.y, + new_wp.z); } - - void LosGuidanceNode::pose_callback( - const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr current_pose) { + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr + current_pose) { std::lock_guard lock(mutex_); path_inputs_.current_position = types::Point::point_from_ros(current_pose->pose.pose.position); } -void LosGuidanceNode::odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg) { +void LosGuidanceNode::odom_callback( + const nav_msgs::msg::Odometry::SharedPtr msg) { std::lock_guard lock(mutex_); debug_current_odom_ = msg; } - +// Action Server Callbacks rclcpp_action::GoalResponse LosGuidanceNode::handle_goal( const rclcpp_action::GoalUUID&, std::shared_ptr goal) { (void)goal; + { std::lock_guard lock(mutex_); if (goal_handle_) { @@ -221,6 +220,7 @@ rclcpp_action::GoalResponse LosGuidanceNode::handle_goal( } } } + spdlog::info("Accepted goal request"); return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } @@ -240,6 +240,8 @@ void LosGuidanceNode::handle_accepted( goal_handle) { execute(goal_handle); } + +// Service Callback void LosGuidanceNode::set_los_mode( const std::shared_ptr request, std::shared_ptr response) { @@ -247,9 +249,10 @@ void LosGuidanceNode::set_los_mode( spdlog::info("LOS mode set to {}", static_cast(method_)); response->success = true; } + +// Message Helpers vortex_msgs::msg::LOSGuidance LosGuidanceNode::fill_los_reference( types::Outputs outputs) { - vortex_msgs::msg::LOSGuidance reference_msg; reference_msg.pitch = outputs.theta_d; reference_msg.yaw = outputs.psi_d; @@ -271,11 +274,11 @@ void LosGuidanceNode::parse_common_config(YAML::Node common_config) { common_config["active_los_method"].as()); } +// Goal Execution void LosGuidanceNode::execute( const std::shared_ptr< rclcpp_action::ServerGoalHandle> goal_handle) { - { std::lock_guard lock(mutex_); this->goal_handle_ = goal_handle; @@ -355,7 +358,7 @@ void LosGuidanceNode::execute( los_debug_pub_->publish(reference_msg); const auto& v = debug_current_odom_->twist.twist.linear; - double surge = std::sqrt(v.x*v.x + v.y*v.y + v.z*v.z); + double surge = std::sqrt(v.x * v.x + v.y * v.y + v.z * v.z); vortex_msgs::msg::LOSGuidance state_debug_msg; Eigen::Vector3d euler = vortex::utils::math::quat_to_euler( @@ -368,15 +371,15 @@ void LosGuidanceNode::execute( state_debug_msg.pitch = euler.y(); state_debug_msg.yaw = euler.z(); state_debug_msg.surge = surge; - state_debug_pub_->publish(state_debug_msg); + state_debug_pub_->publish(state_debug_msg); goal_handle->publish_feedback(feedback); reference_pub_->publish(reference_msg); - - if ((path_inputs_.current_position - path_inputs_.next_point).as_vector().norm() < goal_reached_tol_) { - - auto stop_ref = reference_msg; + if ((path_inputs_.current_position - path_inputs_.next_point) + .as_vector() + .norm() < goal_reached_tol_) { + auto stop_ref = reference_msg; stop_ref.surge = 0.0; stop_ref.pitch = 0.0; stop_ref.yaw = reference_msg.yaw; @@ -392,4 +395,4 @@ void LosGuidanceNode::execute( } } -} // namespace vortex::guidance::los +} // namespace vortex::guidance::los \ No newline at end of file From 374230a34a204ab81ef0167c46e52b99ab802b8b Mon Sep 17 00:00:00 2001 From: Anbit Date: Sun, 8 Mar 2026 15:33:29 +0100 Subject: [PATCH 070/128] ReadMe draft --- guidance/los_guidance/README.md | 92 ++++++++++++++++----------------- 1 file changed, 45 insertions(+), 47 deletions(-) diff --git a/guidance/los_guidance/README.md b/guidance/los_guidance/README.md index 2afe923c1..216bd6df9 100644 --- a/guidance/los_guidance/README.md +++ b/guidance/los_guidance/README.md @@ -3,11 +3,7 @@ This package implements several **Line-of-Sight (LOS) guidance algorithms** for **3D path following**. The guidance system computes the **desired yaw** $\psi_d$ and **desired pitch** $\theta_d$ so a vehicle can follow a path between waypoints. -The vehicle surge speed is kept **constant** during path following and is defined in the configuration file using the parameter - -``` -u_desired -``` +The vehicle surge speed is kept **constant** during path following and is defined in the configuration file using the parameter **u_desired** This value represents the desired forward velocity of the vehicle. @@ -28,6 +24,50 @@ The guidance method can be **changed during runtime** using a ROS service. --- + +# Sending a LOS Goal + +A waypoint can be sent to the guidance node using the action interface: + +``` +ros2 action send_goal /orca/los_guidance \ +vortex_msgs/action/LOSGuidance \ +"{goal: {header: {frame_id: world_ned}, point: {x: 10.0, y: 5.0, z: -2.0}}}" +``` + +This command instructs the guidance node to start following a path toward the waypoint. + +--- + +# Switching LOS Method + +The active LOS guidance method can be changed during runtime. + +``` +ros2 service call /orca/set_los_mode \ +vortex_msgs/srv/SetLosMode "{mode: X}" +``` + +Where + +| X | Method | +|---|---| +| 0 | Proportional LOS | +| 1 | Integral LOS | +| 2 | Adaptive LOS | +| 3 | Vector Field LOS | + +Example: + +``` +ros2 service call /orca/set_los_mode vortex_msgs/srv/SetLosMode "{mode: 2}" +``` + +This switches the guidance system to **Adaptive LOS**. + +--- + + # Adaptive LOS (ALOS) Adaptive LOS estimates **crab angles caused by disturbances** such as ocean currents or wind. @@ -220,48 +260,6 @@ and --- -# Sending a LOS Goal - -A waypoint can be sent to the guidance node using the action interface: - -``` -ros2 action send_goal /orca/los_guidance \ -vortex_msgs/action/LOSGuidance \ -"{goal: {header: {frame_id: world_ned}, point: {x: 10.0, y: 5.0, z: -2.0}}}" -``` - -This command instructs the guidance node to start following a path toward the waypoint. - ---- - -# Switching LOS Method - -The active LOS guidance method can be changed during runtime. - -``` -ros2 service call /orca/set_los_mode \ -vortex_msgs/srv/SetLosMode "{mode: X}" -``` - -Where - -| X | Method | -|---|---| -| 0 | Proportional LOS | -| 1 | Integral LOS | -| 2 | Adaptive LOS | -| 3 | Vector Field LOS | - -Example: - -``` -ros2 service call /orca/set_los_mode vortex_msgs/srv/SetLosMode "{mode: 2}" -``` - -This switches the guidance system to **Adaptive LOS**. - ---- - # ROS Topics ### Subscribed Topics From 853741d77fdc49e11101edcaa87dee766a3863b1 Mon Sep 17 00:00:00 2001 From: Anbit Date: Sun, 8 Mar 2026 16:36:42 +0100 Subject: [PATCH 071/128] add changes to readMe file --- guidance/los_guidance/README.md | 237 +++++++++++--------------------- 1 file changed, 79 insertions(+), 158 deletions(-) diff --git a/guidance/los_guidance/README.md b/guidance/los_guidance/README.md index 216bd6df9..8bce11d2d 100644 --- a/guidance/los_guidance/README.md +++ b/guidance/los_guidance/README.md @@ -1,74 +1,37 @@ -# 3D LOS Guidance Library (Read me draft) +## Path Geometry -This package implements several **Line-of-Sight (LOS) guidance algorithms** for **3D path following**. -The guidance system computes the **desired yaw** $\psi_d$ and **desired pitch** $\theta_d$ so a vehicle can follow a path between waypoints. +The path between two waypoints defines the **path direction angles** used by the guidance law. -The vehicle surge speed is kept **constant** during path following and is defined in the configuration file using the parameter **u_desired** - -This value represents the desired forward velocity of the vehicle. - ---- - -# Implemented LOS Methods - -The library supports four LOS guidance algorithms. - -| Mode | Method | -|-----|------| -| 0 | Proportional LOS | -| 1 | Integral LOS | -| 2 | Adaptive LOS | -| 3 | Vector Field LOS | - -The guidance method can be **changed during runtime** using a ROS service. - ---- - - -# Sending a LOS Goal - -A waypoint can be sent to the guidance node using the action interface: - -``` -ros2 action send_goal /orca/los_guidance \ -vortex_msgs/action/LOSGuidance \ -"{goal: {header: {frame_id: world_ned}, point: {x: 10.0, y: 5.0, z: -2.0}}}" +```math +\pi_h = +\text{atan2}(y_{i+1}^n - y_i^n, x_{i+1}^n - x_i^n) ``` -This command instructs the guidance node to start following a path toward the waypoint. - ---- - -# Switching LOS Method - -The active LOS guidance method can be changed during runtime. - -``` -ros2 service call /orca/set_los_mode \ -vortex_msgs/srv/SetLosMode "{mode: X}" +```math +\pi_v = +\text{atan2} +\left( +-(z_{i+1}^n - z_i^n), +\sqrt{(x_{i+1}^n - x_i^n)^2 + +(y_{i+1}^n - y_i^n)^2} +\right) ``` -Where +where -| X | Method | -|---|---| -| 0 | Proportional LOS | -| 1 | Integral LOS | -| 2 | Adaptive LOS | -| 3 | Vector Field LOS | +- $\pi_h$ is the **horizontal path angle (azimuth angle)** +- $\pi_v$ is the **vertical path angle (elevation angle)** -Example: +These angles define the **direction of the current path segment** between two waypoints. -``` -ros2 service call /orca/set_los_mode vortex_msgs/srv/SetLosMode "{mode: 2}" -``` +Additionally -This switches the guidance system to **Adaptive LOS**. +- $P_i^n = (x_i^n, y_i^n, z_i^n)$ is the previous waypoint in the north-east-down (NED) frame +- $P_{i+1}^n = (x_{i+1}^n, y_{i+1}^n, z_{i+1}^n)$ is the next waypoint. --- - -# Adaptive LOS (ALOS) +## Adaptive LOS (ALOS) Adaptive LOS estimates **crab angles caused by disturbances** such as ocean currents or wind. @@ -99,13 +62,21 @@ z_e^p where -- $\Delta_h$ is the horizontal lookahead distance -- $\Delta_v$ is the vertical lookahead distance -- $\gamma_h$ and $\gamma_v$ are the adaptive gains -- $y_e^p$ is the cross-track error -- $z_e^p$ is the vertical-track error +- $\Delta_h$ is the horizontal lookahead distance +- $\Delta_v$ is the vertical lookahead distance +- $\gamma_h$ and $\gamma_v$ are the adaptive gains +- $y_e^p$ is the cross-track error +- $z_e^p$ is the vertical-track error + +The terms -Adaptive LOS is generally the **most robust method** and works well for: +- $\hat{\beta}_c$ is the **estimated horizontal crab angle** +- $\hat{\alpha}_c$ is the **estimated vertical crab angle** + +These angles represent the **estimated disturbance-induced deviation** between the vehicle heading and the actual direction of motion. +They allow the guidance system to **compensate for disturbances such as currents or wind**. + +Adaptive LOS is generally the **most robust method** and works well for - curved trajectories - long paths @@ -113,7 +84,7 @@ Adaptive LOS is generally the **most robust method** and works well for: --- -# Proportional LOS (PLOS) +## Proportional LOS (PLOS) Proportional LOS is the simplest LOS guidance law. @@ -129,16 +100,28 @@ Proportional LOS is the simplest LOS guidance law. \tan^{-1}\left(\frac{z_e^p}{\Delta_v}\right) ``` -It is best suited for +### Parameters + +- $\Delta_h$ — horizontal lookahead distance +- $\Delta_v$ — vertical lookahead distance -- simple waypoint following -- calm environments with little disturbance. +The lookahead distances determine **how aggressively the vehicle corrects path errors**. -However, steady-state tracking error may occur if disturbances are present. +- small values → aggressive corrections +- large values → smoother but slower convergence + +### Use case + +PLOS works best for + +- simple waypoint following +- environments with minimal disturbances. + +However, it may suffer from **steady-state tracking error** when disturbances are present. --- -# Integral LOS (ILOS) +## Integral LOS (ILOS) Integral LOS adds integral action to remove steady-state error. @@ -166,11 +149,25 @@ k_{i,v}\int z_e^p dt \tan^{-1}(u_v) ``` -Integral LOS performs well when there are **constant disturbances**, such as steady ocean currents or wind. +### Parameters + +- $k_{p,h}$ — horizontal proportional gain +- $k_{p,v}$ — vertical proportional gain +- $k_{i,h}$ — horizontal integral gain +- $k_{i,v}$ — vertical integral gain + +The integral term allows the controller to **eliminate steady-state cross-track errors** caused by constant disturbances. + +### Use case + +ILOS works well when there are **persistent disturbances**, such as + +- steady ocean currents +- constant wind disturbances. --- -# Vector Field LOS (VF-LOS) +## Vector Field LOS (VF-LOS) Vector Field LOS generates a **bounded approach angle** toward the path. @@ -185,95 +182,19 @@ Vector Field LOS generates a **bounded approach angle** toward the path. \tan^{-1}(k_p y_e^p) ``` -This method is best suited for - -- long straight path following -- corridor tracking. - -However it performs worse when the path contains **sharp turns**. - ---- - -# Path Geometry - -The path between two waypoints defines the reference angles used by the guidance law. - -```math -\pi_h = -\text{atan2}(y_{i+1}^n - y_i^n, x_{i+1}^n - x_i^n) -``` +### Parameters -```math -\pi_v = -\text{atan2} -\left( --(z_{i+1}^n - z_i^n), -\sqrt{(x_{i+1}^n - x_i^n)^2 + -(y_{i+1}^n - y_i^n)^2} -\right) -``` - -where - -- $P_i^n = (x_i^n, y_i^n, z_i^n)$ is the previous waypoint in the north-east-down frame -- $P_{i+1}^n = (x_{i+1}^n, y_{i+1}^n, z_{i+1}^n)$ is the next waypoint. - ---- - -# Path Frame Errors - -The along-, cross- and vertical-track errors in the path-tangential frame are found by - -```math -\begin{bmatrix} -x_e^p \\ -y_e^p \\ -z_e^p -\end{bmatrix} -= -\mathbf{R}_{y,\pi_v}^\top -\mathbf{R}_{z,\pi_h}^\top -\left( -\begin{bmatrix} -x^n \\ -y^n \\ -z^n -\end{bmatrix} -- -\begin{bmatrix} -x_i^n \\ -y_i^n \\ -z_i^n -\end{bmatrix} -\right) -``` - -where - -- $x_e^p$ is the along-track error -- $y_e^p$ is the cross-track error -- $z_e^p$ is the vertical-track error - -and - -- $P^n = (x^n, y^n, z^n)$ is the current position of the vehicle. - ---- +- $\psi_{max}$ — maximum allowed approach angle +- $k_p$ — proportional gain controlling path convergence -# ROS Topics +The bounded approach angle prevents excessively aggressive heading changes. -### Subscribed Topics +### Use case -| Topic | Message | -|------|------| -| `/orca/pose` | `geometry_msgs/PoseWithCovarianceStamped` | -| `/orca/odom` | `nav_msgs/Odometry` | -| `/orca/waypoint` | `geometry_msgs/PointStamped` | +VF-LOS works best for -### Published Topics +- long straight path following +- corridor tracking +- inspection missions along pipelines or cables. -| Topic | Message | -|------|------| -| `/orca/guidance/los` | `vortex_msgs/LOSGuidance` | -| `/los_debug` | `vortex_msgs/LOSGuidance` | -| `/state_debug` | `vortex_msgs/LOSGuidance` | \ No newline at end of file +However it performs worse when the path contains **sharp turns or rapidly changing path segments**. \ No newline at end of file From 2476aceec03520ff2792836cdd1e58336bac9747 Mon Sep 17 00:00:00 2001 From: Anbit Date: Sun, 8 Mar 2026 17:02:36 +0100 Subject: [PATCH 072/128] Add more stuff in readme-file --- guidance/los_guidance/README.md | 180 +++++++++++++++++++++++++++++++- 1 file changed, 179 insertions(+), 1 deletion(-) diff --git a/guidance/los_guidance/README.md b/guidance/los_guidance/README.md index 8bce11d2d..10f796b69 100644 --- a/guidance/los_guidance/README.md +++ b/guidance/los_guidance/README.md @@ -1,3 +1,81 @@ +# 3D LOS Guidance Library + +This package implements several **Line-of-Sight (LOS) guidance algorithms** for **3D path following**. + +The guidance system computes the **desired yaw** $\psi_d$ and **desired pitch** $\theta_d$ that allow a vehicle to follow a path between waypoints. + +The node receives + +- vehicle pose +- waypoint information + +and computes guidance references based on the selected LOS algorithm. The resulting guidance commands consist of + +- desired yaw +- desired pitch +- desired surge velocity. + +The vehicle surge speed is kept **constant during path following** and is defined in the configuration file using the parameter `u_desired`. + +--- + +# Implemented LOS Methods + +The library supports four LOS guidance algorithms. + +| Mode | Method | +|-----|------| +| 0 | Proportional LOS | +| 1 | Integral LOS | +| 2 | Adaptive LOS | +| 3 | Vector Field LOS | + +The guidance method can be **changed during runtime** using a ROS service. + +--- + + +# Sending a LOS Goal + +A waypoint can be sent to the guidance node using the action interface: + +``` +ros2 action send_goal /orca/los_guidance \ +vortex_msgs/action/LOSGuidance \ +"{goal: {header: {frame_id: world_ned}, point: {x: 0.0, y: 0.0, z: 0.0}}}" +``` + +This command instructs the guidance node to start following a path toward the waypoint. + +--- + +# Switching LOS Method + +The active LOS guidance method can be changed during runtime. + +``` +ros2 service call /orca/set_los_mode \ +vortex_msgs/srv/SetLosMode "{mode: X}" +``` + +Where + +| X | Method | +|---|---| +| 0 | Proportional LOS | +| 1 | Integral LOS | +| 2 | Adaptive LOS | +| 3 | Vector Field LOS | + +Example: + +``` +ros2 service call /orca/set_los_mode vortex_msgs/srv/SetLosMode "{mode: 2}" +``` + +This switches the guidance system to **Adaptive LOS**. + +--- ## Path Geometry The path between two waypoints defines the **path direction angles** used by the guidance law. @@ -31,6 +109,46 @@ Additionally --- +## Path Frame Errors + +The along-, cross- and vertical-track errors in the path-tangential frame are found by + +```math +\begin{bmatrix} +x_e^p \\ +y_e^p \\ +z_e^p +\end{bmatrix} += +\mathbf{R}_{y,\pi_v}^\top +\mathbf{R}_{z,\pi_h}^\top +\left( +\begin{bmatrix} +x^n \\ +y^n \\ +z^n +\end{bmatrix} +- +\begin{bmatrix} +x_i^n \\ +y_i^n \\ +z_i^n +\end{bmatrix} +\right) +``` + +where + +- $x_e^p$ is the along-track error +- $y_e^p$ is the cross-track error +- $z_e^p$ is the vertical-track error + +and + +- $P^n = (x^n, y^n, z^n)$ is the current position of the vehicle. + +--- + ## Adaptive LOS (ALOS) Adaptive LOS estimates **crab angles caused by disturbances** such as ocean currents or wind. @@ -197,4 +315,64 @@ VF-LOS works best for - corridor tracking - inspection missions along pipelines or cables. -However it performs worse when the path contains **sharp turns or rapidly changing path segments**. \ No newline at end of file +However it performs worse when the path contains **sharp turns or rapidly changing path segments**. + +--- + +## ROS Interfaces + +| Interface | Name | Type | Message-Type | +|----------|------|------|---------| +| Action Server | `/orca/los_guidance` | Goal input | `vortex_msgs/action/LOSGuidance` | +| Subscriber | `/orca/waypoint` | Waypoint input | `geometry_msgs/PointStamped` | +| Subscriber | `/orca/pose` | Vehicle pose | `geometry_msgs/PoseWithCovarianceStamped` | +| Subscriber | `/orca/odom` | Vehicle velocity | `nav_msgs/Odometry` | +| Publisher | `/orca/guidance/los` | Guidance reference (yaw, pitch, surge) | `vortex_msgs/LOSGuidance` | +| Publisher | `/los_debug` | LOS debug output | `vortex_msgs/LOSGuidance` | +| Publisher | `/state_debug` | Vehicle state debug | `vortex_msgs/LOSGuidance` | + +--- + +## Guidance Node Architecture +The LOS guidance node computes reference commands for the vehicle based on +the current vehicle state and the active waypoint. + +The process works as follows + +1. The node receives the **vehicle pose**. +2. The current **path segment** is defined between two waypoints. +3. The **path geometry** is computed to determine the path direction. +4. The **cross-track and vertical-track errors** are calculated in the path frame. +5. The selected **LOS guidance algorithm** computes desired yaw and pitch. +6. The node publishes the resulting **guidance reference**. + +### Data Flow + +``` +Vehicle Pose + Waypoint + │ + ▼ + Path Geometry + (π_h , π_v angles) + │ + ▼ + Path Frame Errors + (x_e^p , y_e^p , z_e^p) + │ + ▼ + LOS Algorithm + (PLOS / ILOS / ALOS / VF-LOS) + │ + ▼ + Guidance Output + (ψ_d , θ_d , u_desired) +``` + +The resulting guidance command is published as a `vortex_msgs/LOSGuidance` +message containing + +- desired yaw +- desired pitch +- desired surge velocity. + +--- \ No newline at end of file From 0f4094af8d0dcf9a2f601343f4d70645ffe8119b Mon Sep 17 00:00:00 2001 From: Anbit Date: Sun, 8 Mar 2026 17:08:22 +0100 Subject: [PATCH 073/128] fix pre-commit issues --- guidance/los_guidance/CMakeLists.txt | 1 - guidance/los_guidance/README.md | 60 +++++++++---------- .../los_guidance/config/guidance_params.yaml | 2 +- .../include/los_guidance/lib/adaptive_los.hpp | 4 +- .../include/los_guidance/lib/integral_los.hpp | 3 +- .../los_guidance/lib/proportional_los.hpp | 4 +- .../include/los_guidance/lib/types.hpp | 13 +--- .../los_guidance/lib/vector_field_los.hpp | 4 +- .../include/los_guidance/los_guidance_ros.hpp | 21 +++---- .../launch/guidance_test.launch.py | 55 ++++++++++------- .../launch/los_guidance.launch.py | 2 - guidance/los_guidance/scripts/square_test.py | 25 ++------ .../los_guidance/src/lib/adaptive_los.cpp | 14 ++--- .../los_guidance/src/lib/integral_los.cpp | 2 +- .../los_guidance/src/lib/proportional_los.cpp | 2 +- .../los_guidance/src/lib/vector_field_los.cpp | 12 ++-- .../los_guidance/src/los_guidance_node.cpp | 2 +- .../los_guidance/src/los_guidance_ros.cpp | 25 ++++---- .../test/vector_field_los_test.cpp | 6 +- 19 files changed, 114 insertions(+), 143 deletions(-) diff --git a/guidance/los_guidance/CMakeLists.txt b/guidance/los_guidance/CMakeLists.txt index a943a4f24..d6bdb26bf 100644 --- a/guidance/los_guidance/CMakeLists.txt +++ b/guidance/los_guidance/CMakeLists.txt @@ -96,4 +96,3 @@ if(BUILD_TESTING) endif() ament_package() - diff --git a/guidance/los_guidance/README.md b/guidance/los_guidance/README.md index 10f796b69..af54fdb90 100644 --- a/guidance/los_guidance/README.md +++ b/guidance/los_guidance/README.md @@ -97,14 +97,14 @@ The path between two waypoints defines the **path direction angles** used by the where -- $\pi_h$ is the **horizontal path angle (azimuth angle)** -- $\pi_v$ is the **vertical path angle (elevation angle)** +- $\pi_h$ is the **horizontal path angle (azimuth angle)** +- $\pi_v$ is the **vertical path angle (elevation angle)** These angles define the **direction of the current path segment** between two waypoints. Additionally -- $P_i^n = (x_i^n, y_i^n, z_i^n)$ is the previous waypoint in the north-east-down (NED) frame +- $P_i^n = (x_i^n, y_i^n, z_i^n)$ is the previous waypoint in the north-east-down (NED) frame - $P_{i+1}^n = (x_{i+1}^n, y_{i+1}^n, z_{i+1}^n)$ is the next waypoint. --- @@ -139,9 +139,9 @@ z_i^n where -- $x_e^p$ is the along-track error -- $y_e^p$ is the cross-track error -- $z_e^p$ is the vertical-track error +- $x_e^p$ is the along-track error +- $y_e^p$ is the cross-track error +- $z_e^p$ is the vertical-track error and @@ -149,7 +149,7 @@ and --- -## Adaptive LOS (ALOS) +## Adaptive LOS (ALSO) Adaptive LOS estimates **crab angles caused by disturbances** such as ocean currents or wind. @@ -180,24 +180,24 @@ z_e^p where -- $\Delta_h$ is the horizontal lookahead distance -- $\Delta_v$ is the vertical lookahead distance -- $\gamma_h$ and $\gamma_v$ are the adaptive gains -- $y_e^p$ is the cross-track error -- $z_e^p$ is the vertical-track error +- $\Delta_h$ is the horizontal lookahead distance +- $\Delta_v$ is the vertical lookahead distance +- $\gamma_h$ and $\gamma_v$ are the adaptive gains +- $y_e^p$ is the cross-track error +- $z_e^p$ is the vertical-track error The terms - $\hat{\beta}_c$ is the **estimated horizontal crab angle** - $\hat{\alpha}_c$ is the **estimated vertical crab angle** -These angles represent the **estimated disturbance-induced deviation** between the vehicle heading and the actual direction of motion. +These angles represent the **estimated disturbance-induced deviation** between the vehicle heading and the actual direction of motion. They allow the guidance system to **compensate for disturbances such as currents or wind**. Adaptive LOS is generally the **most robust method** and works well for -- curved trajectories -- long paths +- curved trajectories +- long paths - environments with disturbances. --- @@ -220,19 +220,19 @@ Proportional LOS is the simplest LOS guidance law. ### Parameters -- $\Delta_h$ — horizontal lookahead distance -- $\Delta_v$ — vertical lookahead distance +- $\Delta_h$ — horizontal lookahead distance +- $\Delta_v$ — vertical lookahead distance The lookahead distances determine **how aggressively the vehicle corrects path errors**. -- small values → aggressive corrections +- small values → aggressive corrections - large values → smoother but slower convergence ### Use case PLOS works best for -- simple waypoint following +- simple waypoint following - environments with minimal disturbances. However, it may suffer from **steady-state tracking error** when disturbances are present. @@ -269,10 +269,10 @@ k_{i,v}\int z_e^p dt ### Parameters -- $k_{p,h}$ — horizontal proportional gain -- $k_{p,v}$ — vertical proportional gain -- $k_{i,h}$ — horizontal integral gain -- $k_{i,v}$ — vertical integral gain +- $k_{p,h}$ — horizontal proportional gain +- $k_{p,v}$ — vertical proportional gain +- $k_{i,h}$ — horizontal integral gain +- $k_{i,v}$ — vertical integral gain The integral term allows the controller to **eliminate steady-state cross-track errors** caused by constant disturbances. @@ -280,7 +280,7 @@ The integral term allows the controller to **eliminate steady-state cross-track ILOS works well when there are **persistent disturbances**, such as -- steady ocean currents +- steady ocean currents - constant wind disturbances. --- @@ -302,7 +302,7 @@ Vector Field LOS generates a **bounded approach angle** toward the path. ### Parameters -- $\psi_{max}$ — maximum allowed approach angle +- $\psi_{max}$ — maximum allowed approach angle - $k_p$ — proportional gain controlling path convergence The bounded approach angle prevents excessively aggressive heading changes. @@ -311,8 +311,8 @@ The bounded approach angle prevents excessively aggressive heading changes. VF-LOS works best for -- long straight path following -- corridor tracking +- long straight path following +- corridor tracking - inspection missions along pipelines or cables. However it performs worse when the path contains **sharp turns or rapidly changing path segments**. @@ -339,7 +339,7 @@ the current vehicle state and the active waypoint. The process works as follows -1. The node receives the **vehicle pose**. +1. The node receives the **vehicle pose**. 2. The current **path segment** is defined between two waypoints. 3. The **path geometry** is computed to determine the path direction. 4. The **cross-track and vertical-track errors** are calculated in the path frame. @@ -361,7 +361,7 @@ Vehicle Pose + Waypoint │ ▼ LOS Algorithm - (PLOS / ILOS / ALOS / VF-LOS) + (PLOS / ILOS / ALSO / VF-LOS) │ ▼ Guidance Output @@ -375,4 +375,4 @@ message containing - desired pitch - desired surge velocity. ---- \ No newline at end of file +--- diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 8ba560c1a..35ac01df6 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -33,4 +33,4 @@ common: # Debug Settings debug: enable_debug: true - debug_topic_name: "/los_guidance_debug" \ No newline at end of file + debug_topic_name: "/los_guidance_debug" diff --git a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp index f7174a89b..f63bdfe57 100644 --- a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp @@ -21,7 +21,6 @@ struct AdaptiveLosParams { // Adaptive LOS Guidance Class class AdaptiveLOSGuidance { public: - // Constructor / Destructor AdaptiveLOSGuidance(const AdaptiveLosParams& params); ~AdaptiveLOSGuidance() = default; @@ -30,7 +29,6 @@ class AdaptiveLOSGuidance { types::Outputs calculate_outputs(const types::Inputs& inputs); private: - // Internal Update Functions void update_angles(const types::Inputs& inputs); const types::CrossTrackError calculate_crosstrack_error( @@ -56,4 +54,4 @@ class AdaptiveLOSGuidance { } // namespace vortex::guidance::los -#endif // ADAPTIVE_LOS_GUIDANCE_HPP \ No newline at end of file +#endif // ADAPTIVE_LOS_GUIDANCE_HPP diff --git a/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp b/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp index 83a528461..a8983d8a3 100644 --- a/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp @@ -21,7 +21,6 @@ struct IntegralLosParams { // Integral LOS Guidance Class class IntegralLOSGuidance { public: - // Constructor / Destructor IntegralLOSGuidance(const IntegralLosParams& params); ~IntegralLOSGuidance() = default; @@ -53,4 +52,4 @@ class IntegralLOSGuidance { } // namespace vortex::guidance::los -#endif // INTEGRAL_LOS_GUIDANCE_HPP \ No newline at end of file +#endif // INTEGRAL_LOS_GUIDANCE_HPP diff --git a/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp b/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp index a56a5efae..7290839e6 100644 --- a/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp @@ -18,7 +18,6 @@ struct ProportionalLosParams { // Proportional LOS Guidance Class class ProportionalLOSGuidance { public: - // Constructor / Destructor ProportionalLOSGuidance(const ProportionalLosParams& params); ~ProportionalLOSGuidance() = default; @@ -27,7 +26,6 @@ class ProportionalLOSGuidance { types::Outputs calculate_outputs(const types::Inputs& inputs); private: - // Internal Update Functions void update_angles(const types::Inputs& inputs); types::CrossTrackError calculate_crosstrack_error( @@ -47,4 +45,4 @@ class ProportionalLOSGuidance { } // namespace vortex::guidance::los -#endif // PROPORTIONAL_LOS_GUIDANCE_HPP \ No newline at end of file +#endif // PROPORTIONAL_LOS_GUIDANCE_HPP diff --git a/guidance/los_guidance/include/los_guidance/lib/types.hpp b/guidance/los_guidance/include/los_guidance/lib/types.hpp index a12aa5078..6de4e0360 100644 --- a/guidance/los_guidance/include/los_guidance/lib/types.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/types.hpp @@ -21,9 +21,7 @@ struct Point { } // Conversion Functions - Eigen::Vector3d as_vector() const { - return Eigen::Vector3d(x, y, z); - } + Eigen::Vector3d as_vector() const { return Eigen::Vector3d(x, y, z); } static Point point_from_ros(const geometry_msgs::msg::Point& msg) { return Point{msg.x, msg.y, msg.z}; @@ -55,13 +53,8 @@ struct Inputs { }; // Active LOS Method -enum class ActiveLosMethod { - PROPORTIONAL, - INTEGRAL, - ADAPTIVE, - VECTOR_FIELD -}; +enum class ActiveLosMethod { PROPORTIONAL, INTEGRAL, ADAPTIVE, VECTOR_FIELD }; } // namespace vortex::guidance::los::types -#endif // TYPES_HPP \ No newline at end of file +#endif // TYPES_HPP diff --git a/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp b/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp index 3348b0f74..545013203 100644 --- a/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp @@ -21,7 +21,6 @@ struct VectorFieldLosParams { // Vector Field LOS Guidance Class class VectorFieldLOSGuidance { public: - // Constructor / Destructor VectorFieldLOSGuidance(const VectorFieldLosParams& params); ~VectorFieldLOSGuidance() = default; @@ -30,7 +29,6 @@ class VectorFieldLOSGuidance { types::Outputs calculate_outputs(const types::Inputs& inputs); private: - // Internal Update Functions void update_angles(const types::Inputs& inputs); types::CrossTrackError calculate_crosstrack_error( @@ -50,4 +48,4 @@ class VectorFieldLOSGuidance { } // namespace vortex::guidance::los -#endif // VECTOR_FIELD_LOS_GUIDANCE_HPP \ No newline at end of file +#endif // VECTOR_FIELD_LOS_GUIDANCE_HPP diff --git a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp index e7bbc1482..73ebc7b99 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -1,6 +1,7 @@ #ifndef LOS_GUIDANCE__LOS_GUIDANCE_ROS_HPP_ #define LOS_GUIDANCE__LOS_GUIDANCE_ROS_HPP_ +#include #include #include #include @@ -13,7 +14,6 @@ #include #include #include -#include #include #include @@ -29,12 +29,10 @@ namespace vortex::guidance::los { // LOS Guidance ROS Node class LosGuidanceNode : public rclcpp::Node { public: - // Constructor LosGuidanceNode(); private: - // Setup Functions void set_subscribers_and_publisher(); void set_action_server(); @@ -53,8 +51,7 @@ class LosGuidanceNode : public rclcpp::Node { const geometry_msgs::msg::PointStamped::SharedPtr msg); void pose_callback( const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); - void odom_callback( - const nav_msgs::msg::Odometry::SharedPtr msg); + void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg); // Action Server Functions rclcpp_action::GoalResponse handle_goal( @@ -64,14 +61,10 @@ class LosGuidanceNode : public rclcpp::Node { const std::shared_ptr< rclcpp_action::ServerGoalHandle> goal_handle); - void handle_accepted( - const std::shared_ptr< - rclcpp_action::ServerGoalHandle> - goal_handle); - void execute( - const std::shared_ptr< - rclcpp_action::ServerGoalHandle> - goal_handle); + void handle_accepted(const std::shared_ptr> goal_handle); + void execute(const std::shared_ptr> goal_handle); // Service Functions void set_los_mode( @@ -131,4 +124,4 @@ class LosGuidanceNode : public rclcpp::Node { } // namespace vortex::guidance::los -#endif // LOS_GUIDANCE_ROS_HPP \ No newline at end of file +#endif // LOS_GUIDANCE_ROS_HPP diff --git a/guidance/los_guidance/launch/guidance_test.launch.py b/guidance/los_guidance/launch/guidance_test.launch.py index 2bc63f2f9..8dd953671 100644 --- a/guidance/los_guidance/launch/guidance_test.launch.py +++ b/guidance/los_guidance/launch/guidance_test.launch.py @@ -1,9 +1,14 @@ -from yaml import Node +import os + +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription, TimerAction, ExecuteProcess, SetEnvironmentVariable +from launch.actions import ( + ExecuteProcess, + IncludeLaunchDescription, + TimerAction, +) from launch.launch_description_sources import PythonLaunchDescriptionSource -from ament_index_python.packages import get_package_share_directory -import os + def generate_launch_description(): stonefish_dir = get_package_share_directory('stonefish_sim') @@ -23,7 +28,9 @@ def generate_launch_description(): vortex_sim_interface = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(vortex_sim_interface_dir, 'launch', 'vortex_sim_interface.launch.py') + os.path.join( + vortex_sim_interface_dir, 'launch', 'vortex_sim_interface.launch.py' + ) ) ) @@ -35,7 +42,9 @@ def generate_launch_description(): velocity_controller_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(velocity_controller_dir, 'launch', 'velocity_controller_lqr.launch.py') + os.path.join( + velocity_controller_dir, 'launch', 'velocity_controller_lqr.launch.py' + ) ) ) @@ -47,17 +56,18 @@ def generate_launch_description(): os.path.join(stonefish_dir, 'launch', 'orca_sim.launch.py') ) ) - ] + ], ) set_autonomy = TimerAction( - period=12.0, + period=12.0, actions=[ ExecuteProcess( cmd=[ - "bash", "-lc", + "bash", + "-lc", "ros2 topic pub --once /orca/killswitch std_msgs/msg/Bool \"{data: false}\"; " - "ros2 topic pub --once /orca/operation_mode std_msgs/msg/String \"{data: 'autonomous mode'}\"; " + "ros2 topic pub --once /orca/operation_mode std_msgs/msg/String \"{data: 'autonomous mode'}\"; ", ], output="screen", ), @@ -69,20 +79,23 @@ def generate_launch_description(): actions=[ ExecuteProcess( cmd=[ - "bash", "-lc", - f"python3 {os.path.join(los_guidance_dir, 'scripts', 'square_test.py')}" + "bash", + "-lc", + f"python3 {os.path.join(los_guidance_dir, 'scripts', 'square_test.py')}", ], output="screen", ) ], ) - return LaunchDescription([ - stonefish_sim, - vortex_sim_interface, - los_guidance_launch, - velocity_controller_launch, - orca_sim, - set_autonomy, - square_test, - ]) \ No newline at end of file + return LaunchDescription( + [ + stonefish_sim, + vortex_sim_interface, + los_guidance_launch, + velocity_controller_launch, + orca_sim, + set_autonomy, + square_test, + ] + ) diff --git a/guidance/los_guidance/launch/los_guidance.launch.py b/guidance/los_guidance/launch/los_guidance.launch.py index 2a677d5c8..f726983f0 100644 --- a/guidance/los_guidance/launch/los_guidance.launch.py +++ b/guidance/los_guidance/launch/los_guidance.launch.py @@ -31,5 +31,3 @@ def generate_launch_description(): output="screen", ) return LaunchDescription([los_guidance_node]) - - diff --git a/guidance/los_guidance/scripts/square_test.py b/guidance/los_guidance/scripts/square_test.py index 8fe518e9d..954530849 100644 --- a/guidance/los_guidance/scripts/square_test.py +++ b/guidance/los_guidance/scripts/square_test.py @@ -1,27 +1,19 @@ import rclpy -from rclpy.node import Node from rclpy.action import ActionClient - -from vortex_msgs.action import LOSGuidance -from geometry_msgs.msg import Point +from rclpy.node import Node from std_msgs.msg import Header -from launch.actions import LogInfo +from vortex_msgs.action import LOSGuidance class SquareTest(Node): - def __init__(self): super().__init__('square_test_client') self.get_logger().info("Square test started") - self._action_client = ActionClient( - self, - LOSGuidance, - '/orca/los_guidance' - ) + self._action_client = ActionClient(self, LOSGuidance, '/orca/los_guidance') - self.depth = 2.5 + self.depth = 2.5 self.size = 10.0 self.waypoints = [ @@ -36,7 +28,6 @@ def __init__(self): self.send_next_goal() def send_next_goal(self): - if self.current_index >= len(self.waypoints): self.get_logger().info("Square test completed!") rclpy.shutdown() @@ -58,19 +49,16 @@ def send_next_goal(self): goal_msg.goal.point.z = z self.get_logger().info( - f"Sending waypoint {self.current_index + 1}: " - f"x={x}, y={y}, z={z}" + f"Sending waypoint {self.current_index + 1}: x={x}, y={y}, z={z}" ) self._send_goal_future = self._action_client.send_goal_async( - goal_msg, - feedback_callback=self.feedback_callback + goal_msg, feedback_callback=self.feedback_callback ) self._send_goal_future.add_done_callback(self.goal_response_callback) def goal_response_callback(self, future): - goal_handle = future.result() if not goal_handle.accepted: @@ -83,7 +71,6 @@ def goal_response_callback(self, future): self._get_result_future.add_done_callback(self.result_callback) def result_callback(self, future): - self.get_logger().info("Waypoint reached") self.current_index += 1 diff --git a/guidance/los_guidance/src/lib/adaptive_los.cpp b/guidance/los_guidance/src/lib/adaptive_los.cpp index a4acee79f..db6177b1c 100644 --- a/guidance/los_guidance/src/lib/adaptive_los.cpp +++ b/guidance/los_guidance/src/lib/adaptive_los.cpp @@ -43,12 +43,12 @@ void AdaptiveLOSGuidance::update_adaptive_estimates( std::sqrt(params_.lookahead_distance_v * params_.lookahead_distance_v + cross_track_error.z_e * cross_track_error.z_e); - const double beta_dot = - params_.gamma_h * (params_.lookahead_distance_h / denom_h) * - cross_track_error.y_e; - const double alpha_dot = - params_.gamma_v * (params_.lookahead_distance_v / denom_v) * - cross_track_error.z_e; + const double beta_dot = params_.gamma_h * + (params_.lookahead_distance_h / denom_h) * + cross_track_error.y_e; + const double alpha_dot = params_.gamma_v * + (params_.lookahead_distance_v / denom_v) * + cross_track_error.z_e; beta_c_hat_ += beta_dot * params_.time_step; alpha_c_hat_ += alpha_dot * params_.time_step; @@ -75,4 +75,4 @@ types::Outputs AdaptiveLOSGuidance::calculate_outputs( return types::Outputs{psi_d, theta_d}; } -} // namespace vortex::guidance::los \ No newline at end of file +} // namespace vortex::guidance::los diff --git a/guidance/los_guidance/src/lib/integral_los.cpp b/guidance/los_guidance/src/lib/integral_los.cpp index 3da6deb5a..c8f0b4d59 100644 --- a/guidance/los_guidance/src/lib/integral_los.cpp +++ b/guidance/los_guidance/src/lib/integral_los.cpp @@ -53,4 +53,4 @@ types::Outputs IntegralLOSGuidance::calculate_outputs( return types::Outputs{psi_d, theta_d}; } -} // namespace vortex::guidance::los \ No newline at end of file +} // namespace vortex::guidance::los diff --git a/guidance/los_guidance/src/lib/proportional_los.cpp b/guidance/los_guidance/src/lib/proportional_los.cpp index 33aeaa5d6..f76df385c 100644 --- a/guidance/los_guidance/src/lib/proportional_los.cpp +++ b/guidance/los_guidance/src/lib/proportional_los.cpp @@ -49,4 +49,4 @@ types::Outputs ProportionalLOSGuidance::calculate_outputs( return types::Outputs{psi_d, theta_d}; } -} // namespace vortex::guidance::los \ No newline at end of file +} // namespace vortex::guidance::los diff --git a/guidance/los_guidance/src/lib/vector_field_los.cpp b/guidance/los_guidance/src/lib/vector_field_los.cpp index 52eb51ead..edf352a6e 100644 --- a/guidance/los_guidance/src/lib/vector_field_los.cpp +++ b/guidance/los_guidance/src/lib/vector_field_los.cpp @@ -40,13 +40,11 @@ types::Outputs VectorFieldLOSGuidance::calculate_outputs( const types::CrossTrackError cross_track_error = calculate_crosstrack_error(inputs); - const double approach_h = - m_params.max_approach_angle_h * (2.0 / M_PI) * - std::atan(m_params.k_p_h * cross_track_error.y_e); + const double approach_h = m_params.max_approach_angle_h * (2.0 / M_PI) * + std::atan(m_params.k_p_h * cross_track_error.y_e); - const double approach_v = - m_params.max_approach_angle_v * (2.0 / M_PI) * - std::atan(m_params.k_p_v * cross_track_error.z_e); + const double approach_v = m_params.max_approach_angle_v * (2.0 / M_PI) * + std::atan(m_params.k_p_v * cross_track_error.z_e); const double psi_d = pi_h_ - approach_h; const double theta_d = pi_v_ - approach_v; @@ -54,4 +52,4 @@ types::Outputs VectorFieldLOSGuidance::calculate_outputs( return types::Outputs{psi_d, theta_d}; } -} // namespace vortex::guidance::los \ No newline at end of file +} // namespace vortex::guidance::los diff --git a/guidance/los_guidance/src/los_guidance_node.cpp b/guidance/los_guidance/src/los_guidance_node.cpp index 29a19da13..31ad2eb74 100644 --- a/guidance/los_guidance/src/los_guidance_node.cpp +++ b/guidance/los_guidance/src/los_guidance_node.cpp @@ -13,4 +13,4 @@ int main(int argc, char** argv) { executor.spin(); return 0; -} \ No newline at end of file +} diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 9bc88875e..40210dae6 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -2,8 +2,8 @@ #include #include -#include #include +#include #include "los_guidance/lib/types.hpp" @@ -53,8 +53,7 @@ void LosGuidanceNode::set_subscribers_and_publisher() { this->get_parameter("topics.guidance.los").as_string(); std::string waypoint_topic = this->get_parameter("topics.waypoint").as_string(); - std::string odom_topic = - this->get_parameter("topics.odom").as_string(); + std::string odom_topic = this->get_parameter("topics.odom").as_string(); auto qos_sensor_data = vortex::utils::qos_profiles::sensor_data_profile(1); @@ -67,11 +66,10 @@ void LosGuidanceNode::set_subscribers_and_publisher() { state_debug_pub_ = this->create_publisher( "state_debug", qos_sensor_data); - waypoint_sub_ = - this->create_subscription( - waypoint_topic, qos_sensor_data, - std::bind(&LosGuidanceNode::waypoint_callback, this, - std::placeholders::_1)); + waypoint_sub_ = this->create_subscription( + waypoint_topic, qos_sensor_data, + std::bind(&LosGuidanceNode::waypoint_callback, this, + std::placeholders::_1)); pose_sub_ = this->create_subscription< geometry_msgs::msg::PoseWithCovarianceStamped>( @@ -362,11 +360,10 @@ void LosGuidanceNode::execute( vortex_msgs::msg::LOSGuidance state_debug_msg; Eigen::Vector3d euler = vortex::utils::math::quat_to_euler( - Eigen::Quaterniond( - debug_current_odom_->pose.pose.orientation.w, - debug_current_odom_->pose.pose.orientation.x, - debug_current_odom_->pose.pose.orientation.y, - debug_current_odom_->pose.pose.orientation.z)); + Eigen::Quaterniond(debug_current_odom_->pose.pose.orientation.w, + debug_current_odom_->pose.pose.orientation.x, + debug_current_odom_->pose.pose.orientation.y, + debug_current_odom_->pose.pose.orientation.z)); state_debug_msg.pitch = euler.y(); state_debug_msg.yaw = euler.z(); @@ -395,4 +392,4 @@ void LosGuidanceNode::execute( } } -} // namespace vortex::guidance::los \ No newline at end of file +} // namespace vortex::guidance::los diff --git a/guidance/los_guidance/test/vector_field_los_test.cpp b/guidance/los_guidance/test/vector_field_los_test.cpp index 527b33694..956e9b5f0 100644 --- a/guidance/los_guidance/test/vector_field_los_test.cpp +++ b/guidance/los_guidance/test/vector_field_los_test.cpp @@ -1,6 +1,6 @@ -#include "los_guidance/lib/proportional_los.hpp" #include "los_guidance/lib/vector_field_los.hpp" #include +#include "los_guidance/lib/proportional_los.hpp" namespace vortex::guidance::los { @@ -12,8 +12,8 @@ class VectorFieldLosTest : public ::testing::Test { VectorFieldLosParams params; params.max_approach_angle_h = 30.0 * M_PI / 180.0; // 30 degrees in rad params.max_approach_angle_v = 20.0 * M_PI / 180.0; // 20 degrees in rad - params.k_p_h = 0.1; // needs tuning - params.k_p_v = 0.1; // needs tuning + params.k_p_h = 0.1; // needs tuning + params.k_p_v = 0.1; // needs tuning params.time_step = 0.01; return params; } From a8bf5b4c1f6c15122c4157926ee6526468b4ece2 Mon Sep 17 00:00:00 2001 From: Anbit Date: Sun, 8 Mar 2026 17:28:32 +0100 Subject: [PATCH 074/128] Fix pre-commit header issues --- .../include/los_guidance/lib/adaptive_los.hpp | 8 ++++---- .../include/los_guidance/lib/integral_los.hpp | 8 ++++---- .../include/los_guidance/lib/proportional_los.hpp | 8 ++++---- guidance/los_guidance/include/los_guidance/lib/types.hpp | 6 +++--- .../include/los_guidance/lib/vector_field_los.hpp | 8 ++++---- .../include/los_guidance/los_guidance_ros.hpp | 2 +- 6 files changed, 20 insertions(+), 20 deletions(-) diff --git a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp index f63bdfe57..1f9784178 100644 --- a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp @@ -1,5 +1,5 @@ -#ifndef ADAPTIVE_LOS_GUIDANCE_HPP -#define ADAPTIVE_LOS_GUIDANCE_HPP +#ifndef LOS_GUIDANCE__LIB__ADAPTIVE_LOS_HPP_ +#define LOS_GUIDANCE__LIB__ADAPTIVE_LOS_HPP_ #include #include @@ -22,7 +22,7 @@ struct AdaptiveLosParams { class AdaptiveLOSGuidance { public: // Constructor / Destructor - AdaptiveLOSGuidance(const AdaptiveLosParams& params); + explicit AdaptiveLOSGuidance(const AdaptiveLosParams& params); ~AdaptiveLOSGuidance() = default; // Main Output Calculation @@ -54,4 +54,4 @@ class AdaptiveLOSGuidance { } // namespace vortex::guidance::los -#endif // ADAPTIVE_LOS_GUIDANCE_HPP +#endif // LOS_GUIDANCE__LIB__ADAPTIVE_LOS_HPP_ diff --git a/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp b/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp index a8983d8a3..bde7d0d5f 100644 --- a/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp @@ -1,5 +1,5 @@ -#ifndef INTEGRAL_LOS_GUIDANCE_HPP -#define INTEGRAL_LOS_GUIDANCE_HPP +#ifndef LOS_GUIDANCE__LIB__INTEGRAL_LOS_HPP_ +#define LOS_GUIDANCE__LIB__INTEGRAL_LOS_HPP_ #include #include @@ -22,7 +22,7 @@ struct IntegralLosParams { class IntegralLOSGuidance { public: // Constructor / Destructor - IntegralLOSGuidance(const IntegralLosParams& params); + explicit IntegralLOSGuidance(const IntegralLosParams& params); ~IntegralLOSGuidance() = default; // Main Output Calculation @@ -52,4 +52,4 @@ class IntegralLOSGuidance { } // namespace vortex::guidance::los -#endif // INTEGRAL_LOS_GUIDANCE_HPP +#endif // LOS_GUIDANCE__LIB__INTEGRAL_LOS_HPP_ diff --git a/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp b/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp index 7290839e6..a2e9663e1 100644 --- a/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp @@ -1,5 +1,5 @@ -#ifndef PROPORTIONAL_LOS_GUIDANCE_HPP -#define PROPORTIONAL_LOS_GUIDANCE_HPP +#ifndef LOS_GUIDANCE__LIB__PROPORTIONAL_LOS_HPP_ +#define LOS_GUIDANCE__LIB__PROPORTIONAL_LOS_HPP_ #include #include @@ -19,7 +19,7 @@ struct ProportionalLosParams { class ProportionalLOSGuidance { public: // Constructor / Destructor - ProportionalLOSGuidance(const ProportionalLosParams& params); + explicit ProportionalLOSGuidance(const ProportionalLosParams& params); ~ProportionalLOSGuidance() = default; // Main Output Calculation @@ -45,4 +45,4 @@ class ProportionalLOSGuidance { } // namespace vortex::guidance::los -#endif // PROPORTIONAL_LOS_GUIDANCE_HPP +#endif // LOS_GUIDANCE__LIB__PROPORTIONAL_LOS_HPP_ diff --git a/guidance/los_guidance/include/los_guidance/lib/types.hpp b/guidance/los_guidance/include/los_guidance/lib/types.hpp index 6de4e0360..52aab3d48 100644 --- a/guidance/los_guidance/include/los_guidance/lib/types.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/types.hpp @@ -1,5 +1,5 @@ -#ifndef TYPES_HPP -#define TYPES_HPP +#ifndef LOS_GUIDANCE__LIB__TYPES_HPP_ +#define LOS_GUIDANCE__LIB__TYPES_HPP_ #include #include @@ -57,4 +57,4 @@ enum class ActiveLosMethod { PROPORTIONAL, INTEGRAL, ADAPTIVE, VECTOR_FIELD }; } // namespace vortex::guidance::los::types -#endif // TYPES_HPP +#endif // LOS_GUIDANCE__LIB__TYPES_HPP_ diff --git a/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp b/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp index 545013203..60d332929 100644 --- a/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp @@ -1,5 +1,5 @@ -#ifndef VECTOR_FIELD_LOS_GUIDANCE_HPP -#define VECTOR_FIELD_LOS_GUIDANCE_HPP +#ifndef LOS_GUIDANCE__LIB__VECTOR_FIELD_LOS_HPP_ +#define LOS_GUIDANCE__LIB__VECTOR_FIELD_LOS_HPP_ #include #include @@ -22,7 +22,7 @@ struct VectorFieldLosParams { class VectorFieldLOSGuidance { public: // Constructor / Destructor - VectorFieldLOSGuidance(const VectorFieldLosParams& params); + explicit VectorFieldLOSGuidance(const VectorFieldLosParams& params); ~VectorFieldLOSGuidance() = default; // Main Output Calculation @@ -48,4 +48,4 @@ class VectorFieldLOSGuidance { } // namespace vortex::guidance::los -#endif // VECTOR_FIELD_LOS_GUIDANCE_HPP +#endif // LOS_GUIDANCE__LIB__VECTOR_FIELD_LOS_HPP_ diff --git a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp index 73ebc7b99..54ff2e24c 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -124,4 +124,4 @@ class LosGuidanceNode : public rclcpp::Node { } // namespace vortex::guidance::los -#endif // LOS_GUIDANCE_ROS_HPP +#endif // LOS_GUIDANCE__LOS_GUIDANCE_ROS_HPP_ From bdb00db5e17a3a17ced383dc86561d06fadcb212 Mon Sep 17 00:00:00 2001 From: Anbit Date: Sun, 8 Mar 2026 17:48:41 +0100 Subject: [PATCH 075/128] fix build error --- auv_setup/config/robots/orca.yaml | 1 - 1 file changed, 1 deletion(-) diff --git a/auv_setup/config/robots/orca.yaml b/auv_setup/config/robots/orca.yaml index 302e414e4..938cb1bfc 100644 --- a/auv_setup/config/robots/orca.yaml +++ b/auv_setup/config/robots/orca.yaml @@ -126,7 +126,6 @@ killswitch: "killswitch" reference_pose: "reference_pose" landmarks: "landmarks" - odom: "odom" waypoint: "waypoint" waypoint_list: "waypoint_list" guidance: From 1d8df3a22955fc9df076a70546a7c45dcb105dbf Mon Sep 17 00:00:00 2001 From: Henrik Date: Mon, 9 Mar 2026 11:07:08 +0100 Subject: [PATCH 076/128] Fixed LQR controller, signs, refactored into scripts folder, tried to implement implicit integrator in NMPC --- control/velocity_controller/CMakeLists.txt | 6 +- .../config/parameters.yaml | 12 +- .../velocity_controller/NMPC_acados.hpp | 9 +- .../velocity_controller/NMPC_setup.hpp | 4 +- .../velocity_controller.hpp | 5 +- .../velocity_controller/scripts/auv_ocp.json | 1145 ++++++ .../build_auv_solver/Makefile | 12 +- .../acados_sim_solver_auv_model.c | 86 +- .../acados_sim_solver_auv_model.h | 0 .../build_auv_solver/acados_solver.pxd | 0 .../acados_solver_auv_model.c | 202 +- .../acados_solver_auv_model.h | 8 +- .../acados_solver_auv_model.o | Bin 0 -> 35504 bytes .../auv_model_model/auv_model_impl_dae_fun.c | 394 ++ .../auv_model_model/auv_model_impl_dae_fun.o | Bin 0 -> 11584 bytes .../auv_model_impl_dae_fun_jac_x_xdot_u.c | 847 +++++ .../auv_model_impl_dae_fun_jac_x_xdot_u.o | Bin 0 -> 21328 bytes .../auv_model_impl_dae_fun_jac_x_xdot_u_z.c | 851 +++++ .../auv_model_impl_dae_fun_jac_x_xdot_u_z.o | Bin 0 -> 21944 bytes .../auv_model_impl_dae_fun_jac_x_xdot_z.c | 809 ++++ .../auv_model_impl_dae_fun_jac_x_xdot_z.o | Bin 0 -> 20472 bytes .../auv_model_impl_dae_jac_x_xdot_u_z.c | 708 ++++ .../auv_model_impl_dae_jac_x_xdot_u_z.o | Bin 0 -> 17736 bytes .../auv_model_model/auv_model_model.h | 81 + .../libacados_ocp_solver_auv_model.so | Bin 0 -> 86256 bytes .../build_auv_solver/main_auv_model.c | 0 .../build_auv_solver/main_sim_auv_model.c | 0 .../{src => scripts}/dynamics.py | 59 +- .../{src => scripts}/generator.py | 21 +- control/velocity_controller/src/LQR_setup.cpp | 6 +- .../velocity_controller/src/NMPC_acados.cpp | 86 +- .../velocity_controller/src/NMPC_setup.cpp | 136 +- control/velocity_controller/src/auv_ocp.json | 1142 ------ .../acados_solver_auv_model.o | Bin 31144 -> 0 bytes .../auv_model_model/auv_model_expl_ode_fun.c | 386 -- .../auv_model_model/auv_model_expl_ode_fun.o | Bin 8232 -> 0 bytes .../auv_model_model/auv_model_expl_vde_adj.c | 524 --- .../auv_model_model/auv_model_expl_vde_adj.o | Bin 12672 -> 0 bytes .../auv_model_model/auv_model_expl_vde_forw.c | 3385 ----------------- .../auv_model_model/auv_model_expl_vde_forw.o | Bin 49648 -> 0 bytes .../auv_model_model/auv_model_model.h | 74 - .../libacados_ocp_solver_auv_model.so | Bin 85368 -> 0 bytes control/velocity_controller/src/test_VC.cpp | 2 +- .../src/velocity_controller.cpp | 44 +- 44 files changed, 5243 insertions(+), 5801 deletions(-) create mode 100644 control/velocity_controller/scripts/auv_ocp.json rename control/velocity_controller/{src => scripts}/build_auv_solver/Makefile (93%) rename control/velocity_controller/{src => scripts}/build_auv_solver/acados_sim_solver_auv_model.c (70%) rename control/velocity_controller/{src => scripts}/build_auv_solver/acados_sim_solver_auv_model.h (100%) rename control/velocity_controller/{src => scripts}/build_auv_solver/acados_solver.pxd (100%) rename control/velocity_controller/{src => scripts}/build_auv_solver/acados_solver_auv_model.c (82%) rename control/velocity_controller/{src => scripts}/build_auv_solver/acados_solver_auv_model.h (96%) create mode 100644 control/velocity_controller/scripts/build_auv_solver/acados_solver_auv_model.o create mode 100644 control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun.c create mode 100644 control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun.o create mode 100644 control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u.c create mode 100644 control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u.o create mode 100644 control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u_z.c create mode 100644 control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u_z.o create mode 100644 control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_z.c create mode 100644 control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_z.o create mode 100644 control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_jac_x_xdot_u_z.c create mode 100644 control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_jac_x_xdot_u_z.o create mode 100644 control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_model.h create mode 100755 control/velocity_controller/scripts/build_auv_solver/libacados_ocp_solver_auv_model.so rename control/velocity_controller/{src => scripts}/build_auv_solver/main_auv_model.c (100%) rename control/velocity_controller/{src => scripts}/build_auv_solver/main_sim_auv_model.c (100%) rename control/velocity_controller/{src => scripts}/dynamics.py (83%) rename control/velocity_controller/{src => scripts}/generator.py (93%) delete mode 100644 control/velocity_controller/src/auv_ocp.json delete mode 100644 control/velocity_controller/src/build_auv_solver/acados_solver_auv_model.o delete mode 100644 control/velocity_controller/src/build_auv_solver/auv_model_model/auv_model_expl_ode_fun.c delete mode 100644 control/velocity_controller/src/build_auv_solver/auv_model_model/auv_model_expl_ode_fun.o delete mode 100644 control/velocity_controller/src/build_auv_solver/auv_model_model/auv_model_expl_vde_adj.c delete mode 100644 control/velocity_controller/src/build_auv_solver/auv_model_model/auv_model_expl_vde_adj.o delete mode 100644 control/velocity_controller/src/build_auv_solver/auv_model_model/auv_model_expl_vde_forw.c delete mode 100644 control/velocity_controller/src/build_auv_solver/auv_model_model/auv_model_expl_vde_forw.o delete mode 100644 control/velocity_controller/src/build_auv_solver/auv_model_model/auv_model_model.h delete mode 100755 control/velocity_controller/src/build_auv_solver/libacados_ocp_solver_auv_model.so diff --git a/control/velocity_controller/CMakeLists.txt b/control/velocity_controller/CMakeLists.txt index 03fc60b89..8247d1976 100644 --- a/control/velocity_controller/CMakeLists.txt +++ b/control/velocity_controller/CMakeLists.txt @@ -32,7 +32,7 @@ include_directories( ${ACADOS_ROOT}/include/blasfeo/include ${ACADOS_ROOT}/include/hpipm/include ${ACADOS_ROOT}/include/qpOASES_e - ${CMAKE_CURRENT_SOURCE_DIR}/src/build_auv_solver + ${CMAKE_CURRENT_SOURCE_DIR}/scripts/build_auv_solver include @@ -42,8 +42,8 @@ link_directories(${ACADOS_ROOT}/lib) # After setting ACADOS_ROOT etc. file(GLOB_RECURSE GEN_SRCS - ${CMAKE_CURRENT_SOURCE_DIR}/src/build_auv_solver/*.c - ${CMAKE_CURRENT_SOURCE_DIR}/src/build_auv_solver/*/*.c + ${CMAKE_CURRENT_SOURCE_DIR}/scripts/build_auv_solver/*.c + ${CMAKE_CURRENT_SOURCE_DIR}/scripts/build_auv_solver/*/*.c ) diff --git a/control/velocity_controller/config/parameters.yaml b/control/velocity_controller/config/parameters.yaml index fa4a9b8d9..88ad626bc 100644 --- a/control/velocity_controller/config/parameters.yaml +++ b/control/velocity_controller/config/parameters.yaml @@ -13,11 +13,15 @@ LQR_params: Q: [300.0,32.84,32.84,32.84,32.84,100.0,32.84,32.84] R: [0.02,3.1,3.10] + NMPCA_params: + Q: [1.0,1.0,1.0,5.0,5.0] # u,q,r,theta,psi + R: [0.1,0.1,0.1] # u_surge, u_theta, u_psi + N: 20 NMPC_params: - Q: [10.0,1.0,1.0,10.0,10.0] # u,q,r,theta,psi - R: [0.01,0.001,1.0] # u_surge, u_theta, u_psi + Q: [100.0,0.0,0.0,0.0,1.0,1.0,0.0,5.0,50.0] # u,q,r,theta,psi + R: [0.1,0.1,0.1] # u_surge, u_theta, u_psi N: 20 - inertia_matrix: [ 30.0, 0.0, 0.0, 0.0, 0.0, 0.6, 0.0, 30.0, 0.0, 0.0, -0.6, 0.3, 0.0, 0.0, 30.0, 0.6, 0.3, 0.0, 0.0, 0.0, 0.6, 0.68, 0.0, 0.0, 0.0, -0.6, 0.3, 0.0, 3.32, 0.0, 0.6, 0.3, 0.0, 0.0, 0.0, 3.34] + inertia_matrix: [ 30.0, 0.0, 0.0, 0.0, 0.0, 0.60, 0.0, 30.0, 0.0, 0.0, -0.60, 0.30, 0.0, 0.0, 30.0, 0.60, 0.30, 0.0, 0.0, 0.0, 0.60, 0.68, 0.0, 0.0, 0.0, -0.60, 0.30, 0.0, 3.32, 0.0, 0.60, 0.30, 0.0, 0.0, 0.0, 3.34] dampening_matrix_low: [23.0,0.0,0.0,0.0,0.0,0.0, 0.0,46.0,0.0,0.0,0.0,0.0, 0.0,0.0,46.0,0.0,0.0,0.0, 0.0,0.0,0.0,46.0,0.0,0.0, 0.0,0.0,0.0,0.0,46.0,0.0, 0.0,0.0,0.0,0.0,0.0,46.0] dampening_matrix_high: [1.0,0.0,0.0,0.0,0.0,0.0, 0.0,1.0,0.0,0.0,0.0,0.0, 0.0,0.0,1.0,0.0,0.0,0.0, 0.0,0.0,0.0,1.0,0.0,0.0, 0.0,0.0,0.0,0.0,1.0,0.0, 0.0,0.0,0.0,0.0,0.0,1.0] @@ -25,7 +29,7 @@ publish_rate: 100 #ms #Clamp parameter max_force: 99.5 #should maybe be 99.5 - controller_type: 4 #1 PID 2 LQR 3 NMPC 4NMPC fast + controller_type: 2 #1 PID 2 LQR 3 NMPC 4NMPC fast #Q: [300.0,0.01,0.01,0.01,32.84,32.84,32.84,32.84,32.84] # u,v,w,p,q,r,phi,theta,psi # R: [0.02,3.1,3.10] # u_surge, u_theta, u_psi diff --git a/control/velocity_controller/include/velocity_controller/NMPC_acados.hpp b/control/velocity_controller/include/velocity_controller/NMPC_acados.hpp index dcc65b37d..a54c495a6 100644 --- a/control/velocity_controller/include/velocity_controller/NMPC_acados.hpp +++ b/control/velocity_controller/include/velocity_controller/NMPC_acados.hpp @@ -46,7 +46,7 @@ class AuvNMPC { // Inputs void setState(const std::array& x); - void setReference(const std::array& x_ref, const std::array& u_ref); + void setReference(const std::array& x_ref); void setWeights(const std::vector& W_diag, const std::vector& W_e_diag); // sizes: NY, NY_E void setMaxForce(double max_force); // updates con_h bounds @@ -54,6 +54,7 @@ class AuvNMPC { // One-shot solve: provide current state, desired state & input refs, get u0 back. // Returns solver status (0 == success). int solve_once(); + bool initialize_guess(std::array x,std::array u_init); // Outputs std::vector getU0(); @@ -74,8 +75,8 @@ class AuvNMPC { int N_=20; int N_override_=-1; - std::vector W_diag_{NY,0.0}; // length NY - std::vector W_e_diag_{NY_E,0.0}; // length NY_E + std::array W_; // length NY + std::vector W_e_{NY_E*NY_E,0.0}; // length NY_E double max_force2_ = 100*100; // squared constraint, default 100^2 @@ -84,6 +85,6 @@ class AuvNMPC { std::vector u0_out={0,0,0}; //Recorded states states std::array x0; - std::array xr; + std::array xr; std::array ur={0,0,0}; }; diff --git a/control/velocity_controller/include/velocity_controller/NMPC_setup.hpp b/control/velocity_controller/include/velocity_controller/NMPC_setup.hpp index 2100e96cc..19208d44c 100644 --- a/control/velocity_controller/include/velocity_controller/NMPC_setup.hpp +++ b/control/velocity_controller/include/velocity_controller/NMPC_setup.hpp @@ -6,7 +6,8 @@ class NMPC_controller{ public: - Eigen::Matrix calculate_thrust(Guidance_data guidance_values, State state); + Eigen::Matrix get_thrust(); + bool calculate_thrust(Guidance_data guidance_values, State state); bool set_matrices(std::vector Q_,std::vector R_,std::vector inertia_matrix, double max_force,std::vector water_r_low,std::vector water_r_high); void reset_controller(); bool set_interval(double interval); @@ -33,6 +34,7 @@ class NMPC_controller{ casadi::DM ubg; casadi::DM Pval; casadi::Function solver; + Eigen::Matrixthrust; }; \ No newline at end of file diff --git a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp index 5e1545a45..87741af89 100644 --- a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp +++ b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp @@ -81,9 +81,12 @@ class Velocity_node : public rclcpp::Node{ NMPC_controller NMPC; //NMPC acados AuvNMPC NMPC_acados; - //NMPC parameters std::vector Q2; std::vector R2; + //NMPC parameters + std::vector Q3; + std::vector R3; + //Test rclcpp::Publisher::SharedPtr publisher_reference; diff --git a/control/velocity_controller/scripts/auv_ocp.json b/control/velocity_controller/scripts/auv_ocp.json new file mode 100644 index 000000000..1becb5b1f --- /dev/null +++ b/control/velocity_controller/scripts/auv_ocp.json @@ -0,0 +1,1145 @@ +{ + "code_gen_opts": { + "acados_include_path": "/home/henrik/ros2_ws_v/src/acados/include", + "acados_lib_path": "/home/henrik/ros2_ws_v/src/acados/lib", + "acados_link_libs": { + "clarabel": "", + "daqp": "", + "hpmpc": "", + "ooqp": "", + "openmp": "-fopenmp", + "osqp": "-losqp", + "qpdunes": "", + "qpoases": "-lqpOASES_e" + }, + "acados_version": "508482dac", + "code_export_directory": "/home/henrik/ros2_ws_v/src/vortex-auv/control/velocity_controller/scripts/build_auv_solver", + "cython_include_dirs": [ + "/usr/lib/python3/dist-packages/numpy/core/include", + "/usr/include/python3.10" + ], + "json_file": "auv_ocp.json", + "os": "unix", + "shared_lib_ext": ".so" + }, + "constraints": { + "C": [], + "C_e": [], + "D": [], + "constr_type": "BGH", + "constr_type_0": "BGH", + "constr_type_e": "BGH", + "constr_types": [ + "BGH", + "BGP" + ], + "has_x0": true, + "idxbu": [ + 0, + 1, + 2 + ], + "idxbx": [ + 7 + ], + "idxbx_0": [ + 0, + 1, + 2, + 3, + 4, + 5, + 6, + 7, + 8 + ], + "idxbx_e": [], + "idxbxe_0": [ + 0, + 1, + 2, + 3, + 4, + 5, + 6, + 7, + 8 + ], + "idxs_rev": [], + "idxs_rev_0": [], + "idxs_rev_e": [], + "idxsbu": [], + "idxsbx": [], + "idxsbx_e": [], + "idxsg": [], + "idxsg_e": [], + "idxsh": [], + "idxsh_0": [], + "idxsh_e": [], + "idxsphi": [], + "idxsphi_0": [], + "idxsphi_e": [], + "lbu": [ + -99.5, + -99.5, + -99.5 + ], + "lbx": [ + -1.4 + ], + "lbx_0": [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "lbx_e": [], + "lg": [], + "lg_e": [], + "lh": [], + "lh_0": [], + "lh_e": [], + "lphi": [], + "lphi_0": [], + "lphi_e": [], + "ls": [], + "ls_0": [], + "ls_e": [], + "lsbu": [], + "lsbx": [], + "lsbx_e": [], + "lsg": [], + "lsg_e": [], + "lsh": [], + "lsh_0": [], + "lsh_e": [], + "lsphi": [], + "lsphi_0": [], + "lsphi_e": [], + "ubu": [ + 99.5, + 99.5, + 99.5 + ], + "ubx": [ + 1.4 + ], + "ubx_0": [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "ubx_e": [], + "ug": [], + "ug_e": [], + "uh": [], + "uh_0": [], + "uh_e": [], + "uphi": [], + "uphi_0": [], + "uphi_e": [], + "us": [], + "us_0": [], + "us_e": [], + "usbu": [], + "usbx": [], + "usbx_e": [], + "usg": [], + "usg_e": [], + "ush": [], + "ush_0": [], + "ush_e": [], + "usphi": [], + "usphi_0": [], + "usphi_e": [] + }, + "cost": { + "Vu": [ + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 1.0, + 0.0, + 0.0 + ], + [ + 0.0, + 1.0, + 0.0 + ], + [ + 0.0, + 0.0, + 1.0 + ] + ], + "Vu_0": [ + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 1.0, + 0.0, + 0.0 + ], + [ + 0.0, + 1.0, + 0.0 + ], + [ + 0.0, + 0.0, + 1.0 + ] + ], + "Vx": [ + [ + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "Vx_0": [ + [ + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "Vx_e": [ + [ + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0 + ] + ], + "Vz": [], + "Vz_0": [], + "W": [ + [ + 100.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.1, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.1, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.1 + ] + ], + "W_0": [ + [ + 100.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.1, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.1, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.1 + ] + ], + "W_e": [ + [ + 100.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 1.0 + ] + ], + "Zl": [], + "Zl_0": [], + "Zl_e": [], + "Zu": [], + "Zu_0": [], + "Zu_e": [], + "cost_ext_fun_type": "casadi", + "cost_ext_fun_type_0": "casadi", + "cost_ext_fun_type_e": "casadi", + "cost_ext_fun_types": [ + "casadi", + "generic" + ], + "cost_function_ext_cost": null, + "cost_function_ext_cost_0": null, + "cost_function_ext_cost_e": null, + "cost_source_ext_cost": null, + "cost_source_ext_cost_0": null, + "cost_source_ext_cost_e": null, + "cost_type": "LINEAR_LS", + "cost_type_0": "LINEAR_LS", + "cost_type_e": "LINEAR_LS", + "cost_types": [ + "LINEAR_LS", + "NONLINEAR_LS", + "EXTERNAL", + "CONVEX_OVER_NONLINEAR", + "AUTO" + ], + "yref": [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "yref_0": [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "yref_e": [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "zl": [], + "zl_0": [], + "zl_e": [], + "zu": [], + "zu_0": [], + "zu_e": [] + }, + "dims": { + "N": 20, + "n_global_data": 0, + "nbu": 3, + "nbx": 1, + "nbx_0": 9, + "nbx_e": 0, + "nbxe_0": 9, + "ng": 0, + "ng_e": 0, + "nh": 0, + "nh_0": 0, + "nh_e": 0, + "np": 0, + "np_global": 0, + "nphi": 0, + "nphi_0": 0, + "nphi_e": 0, + "nr": 0, + "nr_0": 0, + "nr_e": 0, + "ns": 0, + "ns_0": 0, + "ns_e": 0, + "nsbu": 0, + "nsbx": 0, + "nsbx_e": 0, + "nsg": 0, + "nsg_e": 0, + "nsh": 0, + "nsh_0": 0, + "nsh_e": 0, + "nsphi": 0, + "nsphi_0": 0, + "nsphi_e": 0, + "nu": 3, + "nx": 9, + "nx_next": 9, + "ny": 8, + "ny_0": 8, + "ny_e": 5, + "nz": 0 + }, + "external_function_files_model": [ + "auv_model_model/auv_model_impl_dae_fun.c", + "auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_z.c", + "auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u_z.c", + "auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u.c", + "auv_model_model/auv_model_impl_dae_jac_x_xdot_u_z.c" + ], + "external_function_files_ocp": [], + "hash": "78dcc9b0fd627d3908081bfa461a62e0", + "model": { + "con_h_expr": [], + "con_h_expr_0": [], + "con_h_expr_e": [], + "con_phi_expr": [], + "con_phi_expr_0": [], + "con_phi_expr_e": [], + "con_r_expr": [], + "con_r_expr_0": [], + "con_r_expr_e": [], + "con_r_in_phi": [], + "con_r_in_phi_0": [], + "con_r_in_phi_e": [], + "cost_conl_custom_outer_hess": [], + "cost_conl_custom_outer_hess_0": [], + "cost_conl_custom_outer_hess_e": [], + "cost_expr_ext_cost": [], + "cost_expr_ext_cost_0": [], + "cost_expr_ext_cost_custom_hess": [], + "cost_expr_ext_cost_custom_hess_0": [], + "cost_expr_ext_cost_custom_hess_e": [], + "cost_expr_ext_cost_e": [], + "cost_psi_expr": [], + "cost_psi_expr_0": [], + "cost_psi_expr_e": [], + "cost_r_in_psi_expr": [], + "cost_r_in_psi_expr_0": [], + "cost_r_in_psi_expr_e": [], + "cost_y_expr": [], + "cost_y_expr_0": [], + "cost_y_expr_e": [], + "disc_dyn_custom_hess_ux_expr": [], + "disc_dyn_custom_jac_ux_expr": [], + "disc_dyn_expr": [], + "dyn_disc_fun": null, + "dyn_disc_fun_jac": null, + "dyn_disc_fun_jac_hess": null, + "dyn_ext_fun_type": "casadi", + "dyn_generic_source": null, + "dyn_impl_dae_fun": null, + "dyn_impl_dae_fun_jac": null, + "dyn_impl_dae_jac": null, + "expression_names": [ + "f_expl_expr", + "f_impl_expr", + "p", + "p_global", + "pi", + "u", + "x", + "xdot", + "z" + ], + "f_expl_expr": "SX(@1=-30, @2=30, @3=((Fx-(((@1*r)*v)+((@2*q)*w)))-((23+fabs(u))*u)), @4=46, @5=((((@2*r)*u)+((@1*p)*w))+((@4+fabs(v))*v)), @6=((((@1*q)*u)+((@2*p)*v))+((@4+fabs(w))*w)), @7=((((3.34*r)*q)+((-3.32*q)*r))+((@4+fabs(p))*p)), @8=((My-(((-3.34*r)*p)+((0.68*p)*r)))-((@4+fabs(q))*q)), @9=((Mz-(((3.32*q)*p)+((-0.68*p)*q)))-((@4+fabs(r))*r)), @10=-0.000546005, [((((((0.0334536*@3)-(6.0369e-05*@5))-(-1.11163e-07*@6))-(9.80847e-08*@7))+(1.09201e-05*@8))+(-0.00601506*@9)), ((((((6.0369e-05*@3)-(0.0334847*@5))-(-6.16582e-05*@6))-(5.44043e-05*@7))+(0.00605702*@8))+(-0.00301845*@9)), ((((((-1.11163e-07*@3)-(-6.16582e-05*@5))-(0.0339635*@6))-(-0.0299678*@7))+(-0.00308013*@8))+(5.55814e-06*@9)), ((((((9.80847e-08*@3)-(5.44043e-05*@5))-(-0.0299678*@6))-(1.49703*@7))+(0.00271776*@8))+(-4.90424e-06*@9)), ((((((1.09201e-05*@3)-(0.00605702*@5))-(-0.00308013*@6))-(0.00271776*@7))+(0.302578*@8))+(@10*@9)), ((((((-0.00601506*@3)-(-0.00301845*@5))-(5.55814e-06*@6))-(-4.90424e-06*@7))+(@10*@8))+(0.300753*@9)), ((p+((sin(phi)*tan(theta))*q))+((cos(phi)*tan(theta))*r)), ((cos(phi)*q)-(sin(phi)*r)), (((sin(phi)/cos(theta))*q)+((cos(phi)/cos(theta))*r))])", + "f_impl_expr": "SX(@1=-30, @2=30, @3=((Fx-(((@1*r)*v)+((@2*q)*w)))-((23+fabs(u))*u)), @4=46, @5=((((@2*r)*u)+((@1*p)*w))+((@4+fabs(v))*v)), @6=((((@1*q)*u)+((@2*p)*v))+((@4+fabs(w))*w)), @7=((((3.34*r)*q)+((-3.32*q)*r))+((@4+fabs(p))*p)), @8=((My-(((-3.34*r)*p)+((0.68*p)*r)))-((@4+fabs(q))*q)), @9=((Mz-(((3.32*q)*p)+((-0.68*p)*q)))-((@4+fabs(r))*r)), @10=-0.000546005, [(xdot_0-((((((0.0334536*@3)-(6.0369e-05*@5))-(-1.11163e-07*@6))-(9.80847e-08*@7))+(1.09201e-05*@8))+(-0.00601506*@9))), (xdot_1-((((((6.0369e-05*@3)-(0.0334847*@5))-(-6.16582e-05*@6))-(5.44043e-05*@7))+(0.00605702*@8))+(-0.00301845*@9))), (xdot_2-((((((-1.11163e-07*@3)-(-6.16582e-05*@5))-(0.0339635*@6))-(-0.0299678*@7))+(-0.00308013*@8))+(5.55814e-06*@9))), (xdot_3-((((((9.80847e-08*@3)-(5.44043e-05*@5))-(-0.0299678*@6))-(1.49703*@7))+(0.00271776*@8))+(-4.90424e-06*@9))), (xdot_4-((((((1.09201e-05*@3)-(0.00605702*@5))-(-0.00308013*@6))-(0.00271776*@7))+(0.302578*@8))+(@10*@9))), (xdot_5-((((((-0.00601506*@3)-(-0.00301845*@5))-(5.55814e-06*@6))-(-4.90424e-06*@7))+(@10*@8))+(0.300753*@9))), (xdot_6-((p+((sin(phi)*tan(theta))*q))+((cos(phi)*tan(theta))*r))), (xdot_7-((cos(phi)*q)-(sin(phi)*r))), (xdot_8-(((sin(phi)/cos(theta))*q)+((cos(phi)/cos(theta))*r)))])", + "gnsf_model": null, + "name": "auv_model", + "nu_original": null, + "p": "SX(0x1)", + "p_global": "SX(0x1)", + "pi": "SX([pi_0, pi_1, pi_2, pi_3, pi_4, pi_5, pi_6, pi_7, pi_8])", + "serialized_expressions": "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", + "t": [], + "t0": null, + "t_label": "t", + "u": "SX([Fx, My, Mz])", + "u_labels": [ + "u0", + "u1", + "u2" + ], + "x": "SX([u, v, w, p, q, r, phi, theta, psi])", + "x_labels": [ + "x0", + "x1", + "x2", + "x3", + "x4", + "x5", + "x6", + "x7", + "x8" + ], + "xdot": "SX([xdot_0, xdot_1, xdot_2, xdot_3, xdot_4, xdot_5, xdot_6, xdot_7, xdot_8])", + "z": "SX(0x1)" + }, + "name": "auv_model", + "p_global_values": [], + "parameter_values": [], + "problem_class": "OCP", + "ros_opts": null, + "simulink_opts": null, + "solver_options": { + "N_horizon": 20, + "Tsim": 0.1, + "adaptive_levenberg_marquardt_lam": 5.0, + "adaptive_levenberg_marquardt_mu0": 0.001, + "adaptive_levenberg_marquardt_mu_min": 1e-16, + "adaptive_levenberg_marquardt_obj_scalar": 2.0, + "allow_direction_mode_switch_to_nominal": true, + "anderson_activation_threshold": 10.0, + "as_rti_iter": 1, + "as_rti_level": 4, + "byrd_omojokon_slack_relaxation_factor": 1.00001, + "collocation_type": "GAUSS_LEGENDRE", + "cost_discretization": "EULER", + "cost_scaling": [ + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 1.0 + ], + "custom_templates": [], + "custom_update_copy": true, + "custom_update_filename": "", + "custom_update_header_filename": "", + "eval_residual_at_max_iter": false, + "exact_hess_constr": 1, + "exact_hess_cost": 1, + "exact_hess_dyn": 1, + "ext_cost_num_hess": 0, + "ext_fun_compile_flags": "-O2", + "ext_fun_expand_constr": false, + "ext_fun_expand_cost": false, + "ext_fun_expand_dyn": false, + "ext_fun_expand_precompute": false, + "fixed_hess": 0, + "globalization": "FIXED_STEP", + "globalization_alpha_min": 0.05, + "globalization_alpha_reduction": 0.7, + "globalization_eps_sufficient_descent": 0.0001, + "globalization_fixed_step_length": 1.0, + "globalization_full_step_dual": 0, + "globalization_funnel_fraction_switching_condition": 0.001, + "globalization_funnel_init_increase_factor": 15.0, + "globalization_funnel_init_upper_bound": 1.0, + "globalization_funnel_initial_penalty_parameter": 1.0, + "globalization_funnel_kappa": 0.9, + "globalization_funnel_sufficient_decrease_factor": 0.9, + "globalization_funnel_use_merit_fun_only": false, + "globalization_line_search_use_sufficient_descent": 0, + "globalization_use_SOC": 0, + "hessian_approx": "GAUSS_NEWTON", + "hpipm_mode": "BALANCE", + "integrator_type": "IRK", + "levenberg_marquardt": 0.01, + "log_dual_step_norm": false, + "log_primal_step_norm": false, + "model_external_shared_lib_dir": null, + "model_external_shared_lib_name": null, + "nlp_qp_tol_min_comp": 1e-11, + "nlp_qp_tol_min_eq": 1e-10, + "nlp_qp_tol_min_ineq": 1e-10, + "nlp_qp_tol_min_stat": 1e-09, + "nlp_qp_tol_reduction_factor": 0.1, + "nlp_qp_tol_safety_factor": 0.1, + "nlp_qp_tol_strategy": "FIXED_QP_TOL", + "nlp_solver_ext_qp_res": 0, + "nlp_solver_max_iter": 100, + "nlp_solver_tol_comp": 1e-06, + "nlp_solver_tol_eq": 1e-06, + "nlp_solver_tol_ineq": 1e-06, + "nlp_solver_tol_min_step_norm": 0.0, + "nlp_solver_tol_stat": 1e-06, + "nlp_solver_type": "SQP", + "nlp_solver_warm_start_first_qp": false, + "nlp_solver_warm_start_first_qp_from_nlp": false, + "print_level": 2, + "qp_solver": "FULL_CONDENSING_HPIPM", + "qp_solver_cond_N": 20, + "qp_solver_cond_block_size": null, + "qp_solver_cond_ric_alg": 1, + "qp_solver_iter_max": 50, + "qp_solver_mu0": 0.0, + "qp_solver_ric_alg": 1, + "qp_solver_t0_init": 2, + "qp_solver_tol_comp": null, + "qp_solver_tol_eq": null, + "qp_solver_tol_ineq": null, + "qp_solver_tol_stat": null, + "qp_solver_warm_start": 1, + "qpscaling_lb_norm_inf_grad_obj": 0.0001, + "qpscaling_scale_constraints": "NO_CONSTRAINT_SCALING", + "qpscaling_scale_objective": "NO_OBJECTIVE_SCALING", + "qpscaling_ub_max_abs_eig": 100000.0, + "reg_adaptive_eps": false, + "reg_epsilon": 0.0001, + "reg_max_cond_block": 10000000.0, + "reg_min_epsilon": 1e-08, + "regularize_method": "NO_REGULARIZE", + "rti_log_only_available_residuals": 0, + "rti_log_residuals": 0, + "search_direction_mode": "NOMINAL_QP", + "sens_forw_p": false, + "shooting_nodes": [ + 0.0, + 0.1, + 0.2, + 0.30000000000000004, + 0.4, + 0.5, + 0.6, + 0.7, + 0.7999999999999999, + 0.8999999999999999, + 0.9999999999999999, + 1.0999999999999999, + 1.2, + 1.3, + 1.4000000000000001, + 1.5000000000000002, + 1.6000000000000003, + 1.7000000000000004, + 1.8000000000000005, + 1.9000000000000006, + 2.0000000000000004 + ], + "sim_method_jac_reuse": [ + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0 + ], + "sim_method_newton_iter": 3, + "sim_method_newton_tol": 0.0, + "sim_method_num_stages": [ + 2, + 2, + 2, + 2, + 2, + 2, + 2, + 2, + 2, + 2, + 2, + 2, + 2, + 2, + 2, + 2, + 2, + 2, + 2, + 2 + ], + "sim_method_num_steps": [ + 4, + 4, + 4, + 4, + 4, + 4, + 4, + 4, + 4, + 4, + 4, + 4, + 4, + 4, + 4, + 4, + 4, + 4, + 4, + 4 + ], + "solution_sens_qp_t_lam_min": 1e-09, + "store_iterates": false, + "tau_min": 0.0, + "tf": 2.0, + "time_steps": [ + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1 + ], + "timeout_heuristic": "LAST", + "timeout_max_time": 0.0, + "use_constraint_hessian_in_feas_qp": false, + "with_adaptive_levenberg_marquardt": false, + "with_anderson_acceleration": false, + "with_batch_functionality": false, + "with_solution_sens_wrt_params": false, + "with_value_sens_wrt_params": false + }, + "zoro_description": null +} \ No newline at end of file diff --git a/control/velocity_controller/src/build_auv_solver/Makefile b/control/velocity_controller/scripts/build_auv_solver/Makefile similarity index 93% rename from control/velocity_controller/src/build_auv_solver/Makefile rename to control/velocity_controller/scripts/build_auv_solver/Makefile index febdbaf3b..7e20d181d 100644 --- a/control/velocity_controller/src/build_auv_solver/Makefile +++ b/control/velocity_controller/scripts/build_auv_solver/Makefile @@ -37,9 +37,11 @@ # model MODEL_SRC= -MODEL_SRC+= auv_model_model/auv_model_expl_ode_fun.c -MODEL_SRC+= auv_model_model/auv_model_expl_vde_forw.c -MODEL_SRC+= auv_model_model/auv_model_expl_vde_adj.c +MODEL_SRC+= auv_model_model/auv_model_impl_dae_fun.c +MODEL_SRC+= auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_z.c +MODEL_SRC+= auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u_z.c +MODEL_SRC+= auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u.c +MODEL_SRC+= auv_model_model/auv_model_impl_dae_jac_x_xdot_u_z.c MODEL_OBJ := $(MODEL_SRC:.c=.o) # optimal control problem - mostly CasADi exports @@ -134,7 +136,7 @@ ocp_cython_c: ocp_shared_lib -o acados_ocp_solver_pyx.c \ -I $(INCLUDE_PATH)/../interfaces/acados_template/acados_template \ $(INCLUDE_PATH)/../interfaces/acados_template/acados_template/acados_ocp_solver_pyx.pyx \ - -I /home/henrik/ros2_ws_v/src/vortex-auv/control/velocity_controller/src/build_auv_solver \ + -I /home/henrik/ros2_ws_v/src/vortex-auv/control/velocity_controller/scripts/build_auv_solver \ ocp_cython_o: ocp_cython_c $(CC) $(ACADOS_FLAGS) -c -O2 \ @@ -163,7 +165,7 @@ sim_cython_c: sim_shared_lib -o acados_sim_solver_pyx.c \ -I $(INCLUDE_PATH)/../interfaces/acados_template/acados_template \ $(INCLUDE_PATH)/../interfaces/acados_template/acados_template/acados_sim_solver_pyx.pyx \ - -I /home/henrik/ros2_ws_v/src/vortex-auv/control/velocity_controller/src/build_auv_solver \ + -I /home/henrik/ros2_ws_v/src/vortex-auv/control/velocity_controller/scripts/build_auv_solver \ sim_cython_o: sim_cython_c $(CC) $(ACADOS_FLAGS) -c -O2 \ diff --git a/control/velocity_controller/src/build_auv_solver/acados_sim_solver_auv_model.c b/control/velocity_controller/scripts/build_auv_solver/acados_sim_solver_auv_model.c similarity index 70% rename from control/velocity_controller/src/build_auv_solver/acados_sim_solver_auv_model.c rename to control/velocity_controller/scripts/build_auv_solver/acados_sim_solver_auv_model.c index ec4d3a806..3da4f7b13 100644 --- a/control/velocity_controller/src/build_auv_solver/acados_sim_solver_auv_model.c +++ b/control/velocity_controller/scripts/build_auv_solver/acados_sim_solver_auv_model.c @@ -82,37 +82,37 @@ int auv_model_acados_sim_create(auv_model_sim_solver_capsule * capsule) ext_fun_opts.external_workspace = false; - // explicit ode - capsule->sim_expl_vde_forw = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); - capsule->sim_vde_adj_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); - capsule->sim_expl_ode_fun_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); + capsule->sim_impl_dae_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); + capsule->sim_impl_dae_fun_jac_x_xdot_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); + capsule->sim_impl_dae_jac_x_xdot_u_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); + - capsule->sim_expl_vde_forw_p = NULL; + capsule->sim_impl_dae_jac_p = NULL; - - capsule->sim_expl_vde_forw->casadi_fun = &auv_model_expl_vde_forw; - capsule->sim_expl_vde_forw->casadi_n_in = &auv_model_expl_vde_forw_n_in; - capsule->sim_expl_vde_forw->casadi_n_out = &auv_model_expl_vde_forw_n_out; - capsule->sim_expl_vde_forw->casadi_sparsity_in = &auv_model_expl_vde_forw_sparsity_in; - capsule->sim_expl_vde_forw->casadi_sparsity_out = &auv_model_expl_vde_forw_sparsity_out; - capsule->sim_expl_vde_forw->casadi_work = &auv_model_expl_vde_forw_work; - external_function_param_casadi_create(capsule->sim_expl_vde_forw, np, &ext_fun_opts); - - capsule->sim_vde_adj_casadi->casadi_fun = &auv_model_expl_vde_adj; - capsule->sim_vde_adj_casadi->casadi_n_in = &auv_model_expl_vde_adj_n_in; - capsule->sim_vde_adj_casadi->casadi_n_out = &auv_model_expl_vde_adj_n_out; - capsule->sim_vde_adj_casadi->casadi_sparsity_in = &auv_model_expl_vde_adj_sparsity_in; - capsule->sim_vde_adj_casadi->casadi_sparsity_out = &auv_model_expl_vde_adj_sparsity_out; - capsule->sim_vde_adj_casadi->casadi_work = &auv_model_expl_vde_adj_work; - external_function_param_casadi_create(capsule->sim_vde_adj_casadi, np, &ext_fun_opts); - - capsule->sim_expl_ode_fun_casadi->casadi_fun = &auv_model_expl_ode_fun; - capsule->sim_expl_ode_fun_casadi->casadi_n_in = &auv_model_expl_ode_fun_n_in; - capsule->sim_expl_ode_fun_casadi->casadi_n_out = &auv_model_expl_ode_fun_n_out; - capsule->sim_expl_ode_fun_casadi->casadi_sparsity_in = &auv_model_expl_ode_fun_sparsity_in; - capsule->sim_expl_ode_fun_casadi->casadi_sparsity_out = &auv_model_expl_ode_fun_sparsity_out; - capsule->sim_expl_ode_fun_casadi->casadi_work = &auv_model_expl_ode_fun_work; - external_function_param_casadi_create(capsule->sim_expl_ode_fun_casadi, np, &ext_fun_opts); + // external functions (implicit model) + capsule->sim_impl_dae_fun->casadi_fun = &auv_model_impl_dae_fun; + capsule->sim_impl_dae_fun->casadi_work = &auv_model_impl_dae_fun_work; + capsule->sim_impl_dae_fun->casadi_sparsity_in = &auv_model_impl_dae_fun_sparsity_in; + capsule->sim_impl_dae_fun->casadi_sparsity_out = &auv_model_impl_dae_fun_sparsity_out; + capsule->sim_impl_dae_fun->casadi_n_in = &auv_model_impl_dae_fun_n_in; + capsule->sim_impl_dae_fun->casadi_n_out = &auv_model_impl_dae_fun_n_out; + external_function_param_casadi_create(capsule->sim_impl_dae_fun, np, &ext_fun_opts); + + capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_fun = &auv_model_impl_dae_fun_jac_x_xdot_z; + capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_work = &auv_model_impl_dae_fun_jac_x_xdot_z_work; + capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_sparsity_in = &auv_model_impl_dae_fun_jac_x_xdot_z_sparsity_in; + capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_sparsity_out = &auv_model_impl_dae_fun_jac_x_xdot_z_sparsity_out; + capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_n_in = &auv_model_impl_dae_fun_jac_x_xdot_z_n_in; + capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_n_out = &auv_model_impl_dae_fun_jac_x_xdot_z_n_out; + external_function_param_casadi_create(capsule->sim_impl_dae_fun_jac_x_xdot_z, np, &ext_fun_opts); + + capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_fun = &auv_model_impl_dae_jac_x_xdot_u_z; + capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_work = &auv_model_impl_dae_jac_x_xdot_u_z_work; + capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_sparsity_in = &auv_model_impl_dae_jac_x_xdot_u_z_sparsity_in; + capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_sparsity_out = &auv_model_impl_dae_jac_x_xdot_u_z_sparsity_out; + capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_n_in = &auv_model_impl_dae_jac_x_xdot_u_z_n_in; + capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_n_out = &auv_model_impl_dae_jac_x_xdot_u_z_n_out; + external_function_param_casadi_create(capsule->sim_impl_dae_jac_x_xdot_u_z, np, &ext_fun_opts); @@ -120,7 +120,7 @@ int auv_model_acados_sim_create(auv_model_sim_solver_capsule * capsule) // sim plan & config sim_solver_plan_t plan; - plan.sim_solver = ERK; + plan.sim_solver = IRK; // create correct config based on plan sim_config * auv_model_sim_config = sim_config_create(plan); @@ -146,7 +146,7 @@ int auv_model_acados_sim_create(auv_model_sim_solver_capsule * capsule) sim_opts_set(auv_model_sim_config, auv_model_sim_opts, "collocation_type", &collocation_type); - tmp_int = 4; + tmp_int = 2; sim_opts_set(auv_model_sim_config, auv_model_sim_opts, "num_stages", &tmp_int); tmp_int = 4; sim_opts_set(auv_model_sim_config, auv_model_sim_opts, "num_steps", &tmp_int); @@ -165,11 +165,11 @@ int auv_model_acados_sim_create(auv_model_sim_solver_capsule * capsule) // model functions auv_model_sim_config->model_set(auv_model_sim_in->model, - "expl_vde_forw", capsule->sim_expl_vde_forw); + "impl_ode_fun", capsule->sim_impl_dae_fun); auv_model_sim_config->model_set(auv_model_sim_in->model, - "expl_vde_adj", capsule->sim_vde_adj_casadi); + "impl_ode_fun_jac_x_xdot", capsule->sim_impl_dae_fun_jac_x_xdot_z); auv_model_sim_config->model_set(auv_model_sim_in->model, - "expl_ode_fun", capsule->sim_expl_ode_fun_casadi); + "impl_ode_jac_x_xdot_u", capsule->sim_impl_dae_jac_x_xdot_u_z); // sim solver @@ -241,13 +241,13 @@ int auv_model_acados_sim_free(auv_model_sim_solver_capsule *capsule) sim_config_destroy(capsule->acados_sim_config); // free external function - external_function_param_casadi_free(capsule->sim_expl_vde_forw); - external_function_param_casadi_free(capsule->sim_vde_adj_casadi); - external_function_param_casadi_free(capsule->sim_expl_ode_fun_casadi); + external_function_param_casadi_free(capsule->sim_impl_dae_fun); + external_function_param_casadi_free(capsule->sim_impl_dae_fun_jac_x_xdot_z); + external_function_param_casadi_free(capsule->sim_impl_dae_jac_x_xdot_u_z); - free(capsule->sim_expl_vde_forw); - free(capsule->sim_vde_adj_casadi); - free(capsule->sim_expl_ode_fun_casadi); + free(capsule->sim_impl_dae_fun); + free(capsule->sim_impl_dae_fun_jac_x_xdot_z); + free(capsule->sim_impl_dae_jac_x_xdot_u_z); return 0; @@ -264,9 +264,9 @@ int auv_model_acados_sim_update_params(auv_model_sim_solver_capsule *capsule, do " External function has %i parameters. Exiting.\n", np, casadi_np); exit(1); } - capsule->sim_expl_vde_forw[0].set_param(capsule->sim_expl_vde_forw, p); - capsule->sim_vde_adj_casadi[0].set_param(capsule->sim_vde_adj_casadi, p); - capsule->sim_expl_ode_fun_casadi[0].set_param(capsule->sim_expl_ode_fun_casadi, p); + capsule->sim_impl_dae_fun[0].set_param(capsule->sim_impl_dae_fun, p); + capsule->sim_impl_dae_fun_jac_x_xdot_z[0].set_param(capsule->sim_impl_dae_fun_jac_x_xdot_z, p); + capsule->sim_impl_dae_jac_x_xdot_u_z[0].set_param(capsule->sim_impl_dae_jac_x_xdot_u_z, p); return status; diff --git a/control/velocity_controller/src/build_auv_solver/acados_sim_solver_auv_model.h b/control/velocity_controller/scripts/build_auv_solver/acados_sim_solver_auv_model.h similarity index 100% rename from control/velocity_controller/src/build_auv_solver/acados_sim_solver_auv_model.h rename to control/velocity_controller/scripts/build_auv_solver/acados_sim_solver_auv_model.h diff --git a/control/velocity_controller/src/build_auv_solver/acados_solver.pxd b/control/velocity_controller/scripts/build_auv_solver/acados_solver.pxd similarity index 100% rename from control/velocity_controller/src/build_auv_solver/acados_solver.pxd rename to control/velocity_controller/scripts/build_auv_solver/acados_solver.pxd diff --git a/control/velocity_controller/src/build_auv_solver/acados_solver_auv_model.c b/control/velocity_controller/scripts/build_auv_solver/acados_solver_auv_model.c similarity index 82% rename from control/velocity_controller/src/build_auv_solver/acados_solver_auv_model.c rename to control/velocity_controller/scripts/build_auv_solver/acados_solver_auv_model.c index fd036f114..6fbe2da06 100644 --- a/control/velocity_controller/src/build_auv_solver/acados_solver_auv_model.c +++ b/control/velocity_controller/scripts/build_auv_solver/acados_solver_auv_model.c @@ -151,7 +151,7 @@ void auv_model_acados_create_set_plan(ocp_nlp_plan_t* nlp_solver_plan, const int * plan ************************************************/ - nlp_solver_plan->nlp_solver = SQP_RTI; + nlp_solver_plan->nlp_solver = SQP; nlp_solver_plan->ocp_qp_solver_plan.qp_solver = FULL_CONDENSING_HPIPM; nlp_solver_plan->relaxed_ocp_qp_solver_plan.qp_solver = FULL_CONDENSING_HPIPM; @@ -164,7 +164,7 @@ void auv_model_acados_create_set_plan(ocp_nlp_plan_t* nlp_solver_plan, const int for (int i = 0; i < N; i++) { nlp_solver_plan->nlp_dynamics[i] = CONTINUOUS_MODEL; - nlp_solver_plan->sim_solver_plan[i].sim_solver = ERK; + nlp_solver_plan->sim_solver_plan[i].sim_solver = IRK; } nlp_solver_plan->nlp_constraints[0] = BGH; @@ -345,24 +345,23 @@ void auv_model_acados_create_setup_functions(auv_model_solver_capsule* capsule) - // explicit ode - capsule->expl_vde_forw = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*N); + // implicit dae + capsule->impl_dae_fun = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*N); for (int i = 0; i < N; i++) { - MAP_CASADI_FNC(expl_vde_forw[i], auv_model_expl_vde_forw); + MAP_CASADI_FNC(impl_dae_fun[i], auv_model_impl_dae_fun); } - - - capsule->expl_ode_fun = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*N); + capsule->impl_dae_fun_jac_x_xdot_z = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*N); for (int i = 0; i < N; i++) { - MAP_CASADI_FNC(expl_ode_fun[i], auv_model_expl_ode_fun); + MAP_CASADI_FNC(impl_dae_fun_jac_x_xdot_z[i], auv_model_impl_dae_fun_jac_x_xdot_z); } - capsule->expl_vde_adj = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*N); + capsule->impl_dae_jac_x_xdot_u_z = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*N); for (int i = 0; i < N; i++) { - MAP_CASADI_FNC(expl_vde_adj[i], auv_model_expl_vde_adj); + MAP_CASADI_FNC(impl_dae_jac_x_xdot_u_z[i], auv_model_impl_dae_jac_x_xdot_u_z); } + } // N > 0 @@ -454,10 +453,12 @@ void auv_model_acados_setup_nlp_in(auv_model_solver_capsule* capsule, const int /**** Dynamics ****/ for (int i = 0; i < N; i++) { - ocp_nlp_dynamics_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, i, "expl_vde_forw", &capsule->expl_vde_forw[i]); + ocp_nlp_dynamics_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, i, "impl_dae_fun", &capsule->impl_dae_fun[i]); + ocp_nlp_dynamics_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, i, + "impl_dae_fun_jac_x_xdot_z", &capsule->impl_dae_fun_jac_x_xdot_z[i]); + ocp_nlp_dynamics_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, i, + "impl_dae_jac_x_xdot_u", &capsule->impl_dae_jac_x_xdot_u_z[i]); - ocp_nlp_dynamics_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, i, "expl_ode_fun", &capsule->expl_ode_fun[i]); - ocp_nlp_dynamics_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, i, "expl_vde_adj", &capsule->expl_vde_adj[i]); } /**** Cost ****/ @@ -468,23 +469,20 @@ void auv_model_acados_setup_nlp_in(auv_model_solver_capsule* capsule, const int double* W_0 = calloc(NY0*NY0, sizeof(double)); // change only the non-zero elements: - W_0[0+(NY0) * 0] = 10; - W_0[1+(NY0) * 1] = 1; - W_0[2+(NY0) * 2] = 1; - W_0[3+(NY0) * 3] = 10; - W_0[4+(NY0) * 4] = 10; - W_0[5+(NY0) * 5] = 0.01; - W_0[6+(NY0) * 6] = 0.001; - W_0[7+(NY0) * 7] = 1; + W_0[0+(NY0) * 0] = 100; + W_0[4+(NY0) * 4] = 1; + W_0[5+(NY0) * 5] = 0.1; + W_0[6+(NY0) * 6] = 0.1; + W_0[7+(NY0) * 7] = 0.1; ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "W", W_0); free(W_0); double* Vx_0 = calloc(NY0*NX, sizeof(double)); // change only the non-zero elements: Vx_0[0+(NY0) * 0] = 1; - Vx_0[1+(NY0) * 1] = 1; - Vx_0[2+(NY0) * 2] = 1; - Vx_0[3+(NY0) * 3] = 1; - Vx_0[4+(NY0) * 4] = 1; + Vx_0[1+(NY0) * 4] = 1; + Vx_0[2+(NY0) * 5] = 1; + Vx_0[3+(NY0) * 7] = 1; + Vx_0[4+(NY0) * 8] = 1; ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "Vx", Vx_0); free(Vx_0); double* Vu_0 = calloc(NY0*NU, sizeof(double)); @@ -504,14 +502,11 @@ void auv_model_acados_setup_nlp_in(auv_model_solver_capsule* capsule, const int free(yref); double* W = calloc(NY*NY, sizeof(double)); // change only the non-zero elements: - W[0+(NY) * 0] = 10; - W[1+(NY) * 1] = 1; - W[2+(NY) * 2] = 1; - W[3+(NY) * 3] = 10; - W[4+(NY) * 4] = 10; - W[5+(NY) * 5] = 0.01; - W[6+(NY) * 6] = 0.001; - W[7+(NY) * 7] = 1; + W[0+(NY) * 0] = 100; + W[4+(NY) * 4] = 1; + W[5+(NY) * 5] = 0.1; + W[6+(NY) * 6] = 0.1; + W[7+(NY) * 7] = 0.1; for (int i = 1; i < N; i++) { @@ -521,10 +516,10 @@ void auv_model_acados_setup_nlp_in(auv_model_solver_capsule* capsule, const int double* Vx = calloc(NY*NX, sizeof(double)); // change only the non-zero elements: Vx[0+(NY) * 0] = 1; - Vx[1+(NY) * 1] = 1; - Vx[2+(NY) * 2] = 1; - Vx[3+(NY) * 3] = 1; - Vx[4+(NY) * 4] = 1; + Vx[1+(NY) * 4] = 1; + Vx[2+(NY) * 5] = 1; + Vx[3+(NY) * 7] = 1; + Vx[4+(NY) * 8] = 1; for (int i = 1; i < N; i++) { ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "Vx", Vx); @@ -550,20 +545,17 @@ void auv_model_acados_setup_nlp_in(auv_model_solver_capsule* capsule, const int double* W_e = calloc(NYN*NYN, sizeof(double)); // change only the non-zero elements: - W_e[0+(NYN) * 0] = 10; - W_e[1+(NYN) * 1] = 1; - W_e[2+(NYN) * 2] = 1; - W_e[3+(NYN) * 3] = 10; - W_e[4+(NYN) * 4] = 10; + W_e[0+(NYN) * 0] = 100; + W_e[4+(NYN) * 4] = 1; ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "W", W_e); free(W_e); double* Vx_e = calloc(NYN*NX, sizeof(double)); // change only the non-zero elements: Vx_e[0+(NYN) * 0] = 1; - Vx_e[1+(NYN) * 1] = 1; - Vx_e[2+(NYN) * 2] = 1; - Vx_e[3+(NYN) * 3] = 1; - Vx_e[4+(NYN) * 4] = 1; + Vx_e[1+(NYN) * 4] = 1; + Vx_e[2+(NYN) * 5] = 1; + Vx_e[3+(NYN) * 7] = 1; + Vx_e[4+(NYN) * 8] = 1; ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "Vx", Vx_e); free(Vx_e); @@ -755,7 +747,7 @@ static void auv_model_acados_create_set_opts(auv_model_solver_capsule* capsule) // set up sim_method_num_stages // all sim_method_num_stages are identical - int sim_method_num_stages = 4; + int sim_method_num_stages = 2; for (int i = 0; i < N; i++) ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_num_stages", &sim_method_num_stages); @@ -772,7 +764,7 @@ static void auv_model_acados_create_set_opts(auv_model_solver_capsule* capsule) for (int i = 0; i < N; i++) ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_jac_reuse", &tmp_bool); - double levenberg_marquardt = 0.0001; + double levenberg_marquardt = 0.01; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "levenberg_marquardt", &levenberg_marquardt); /* options QP solver */ @@ -782,6 +774,14 @@ static void auv_model_acados_create_set_opts(auv_model_solver_capsule* capsule) bool store_iterates = false; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "store_iterates", &store_iterates); + int log_primal_step_norm = false; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "log_primal_step_norm", &log_primal_step_norm); + + int log_dual_step_norm = false; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "log_dual_step_norm", &log_dual_step_norm); + + double nlp_solver_tol_min_step_norm = 0; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "tol_min_step_norm", &nlp_solver_tol_min_step_norm); // set HPIPM mode: should be done before setting other QP solver options ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_hpipm_mode", "BALANCE"); @@ -793,17 +793,75 @@ static void auv_model_acados_create_set_opts(auv_model_solver_capsule* capsule) - int as_rti_iter = 1; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "as_rti_iter", &as_rti_iter); + // set SQP specific options + double nlp_solver_tol_stat = 0.000001; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "tol_stat", &nlp_solver_tol_stat); + + double nlp_solver_tol_eq = 0.000001; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "tol_eq", &nlp_solver_tol_eq); + + double nlp_solver_tol_ineq = 0.000001; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "tol_ineq", &nlp_solver_tol_ineq); + + double nlp_solver_tol_comp = 0.000001; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "tol_comp", &nlp_solver_tol_comp); + + int nlp_solver_max_iter = 100; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "max_iter", &nlp_solver_max_iter); + + // set options for adaptive Levenberg-Marquardt Update + bool with_adaptive_levenberg_marquardt = false; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "with_adaptive_levenberg_marquardt", &with_adaptive_levenberg_marquardt); + + double adaptive_levenberg_marquardt_lam = 5; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "adaptive_levenberg_marquardt_lam", &adaptive_levenberg_marquardt_lam); + + double adaptive_levenberg_marquardt_mu_min = 0.0000000000000001; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "adaptive_levenberg_marquardt_mu_min", &adaptive_levenberg_marquardt_mu_min); + + double adaptive_levenberg_marquardt_mu0 = 0.001; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "adaptive_levenberg_marquardt_mu0", &adaptive_levenberg_marquardt_mu0); + + double adaptive_levenberg_marquardt_obj_scalar = 2; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "adaptive_levenberg_marquardt_obj_scalar", &adaptive_levenberg_marquardt_obj_scalar); + + bool eval_residual_at_max_iter = false; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "eval_residual_at_max_iter", &eval_residual_at_max_iter); + + // QP scaling + double qpscaling_ub_max_abs_eig = 100000; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qpscaling_ub_max_abs_eig", &qpscaling_ub_max_abs_eig); + + double qpscaling_lb_norm_inf_grad_obj = 0.0001; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qpscaling_lb_norm_inf_grad_obj", &qpscaling_lb_norm_inf_grad_obj); + + qpscaling_scale_objective_type qpscaling_scale_objective = NO_OBJECTIVE_SCALING; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qpscaling_scale_objective", &qpscaling_scale_objective); + + ocp_nlp_qpscaling_constraint_type qpscaling_scale_constraints = NO_CONSTRAINT_SCALING; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qpscaling_scale_constraints", &qpscaling_scale_constraints); + + // NLP QP tol strategy + ocp_nlp_qp_tol_strategy_t nlp_qp_tol_strategy = FIXED_QP_TOL; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_strategy", &nlp_qp_tol_strategy); + + double nlp_qp_tol_reduction_factor = 0.1; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_reduction_factor", &nlp_qp_tol_reduction_factor); + + double nlp_qp_tol_safety_factor = 0.1; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_safety_factor", &nlp_qp_tol_safety_factor); + + double nlp_qp_tol_min_stat = 0.000000001; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_min_stat", &nlp_qp_tol_min_stat); - int as_rti_level = 4; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "as_rti_level", &as_rti_level); + double nlp_qp_tol_min_eq = 0.0000000001; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_min_eq", &nlp_qp_tol_min_eq); - int rti_log_residuals = 0; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "rti_log_residuals", &rti_log_residuals); + double nlp_qp_tol_min_ineq = 0.0000000001; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_min_ineq", &nlp_qp_tol_min_ineq); - int rti_log_only_available_residuals = 0; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "rti_log_only_available_residuals", &rti_log_only_available_residuals); + double nlp_qp_tol_min_comp = 0.00000000001; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_min_comp", &nlp_qp_tol_min_comp); bool with_anderson_acceleration = false; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "with_anderson_acceleration", &with_anderson_acceleration); @@ -958,6 +1016,8 @@ int auv_model_acados_reset(auv_model_solver_capsule* capsule, int reset_qp_solve if (iexpl_vde_forw[i]); + external_function_external_param_casadi_free(&capsule->impl_dae_fun[i]); + external_function_external_param_casadi_free(&capsule->impl_dae_fun_jac_x_xdot_z[i]); + external_function_external_param_casadi_free(&capsule->impl_dae_jac_x_xdot_u_z[i]); - external_function_external_param_casadi_free(&capsule->expl_ode_fun[i]); - external_function_external_param_casadi_free(&capsule->expl_vde_adj[i]); } - free(capsule->expl_vde_adj); - free(capsule->expl_vde_forw); + free(capsule->impl_dae_fun); + free(capsule->impl_dae_fun_jac_x_xdot_z); + free(capsule->impl_dae_jac_x_xdot_u_z); - free(capsule->expl_ode_fun); // cost @@ -1082,13 +1142,23 @@ void auv_model_acados_print_stats(auv_model_solver_capsule* capsule) int nrow = nlp_iter+1 < stat_m ? nlp_iter+1 : stat_m; - printf("iter\tqp_stat\tqp_iter\n"); + printf("iter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter\talpha"); + if (stat_n > 8) + printf("\t\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp"); + printf("\n"); for (int i = 0; i < nrow; i++) { for (int j = 0; j < stat_n + 1; j++) { - tmp_int = (int) stat[i + j * nrow]; - printf("%d\t", tmp_int); + if (j == 0 || j == 5 || j == 6) + { + tmp_int = (int) stat[i + j * nrow]; + printf("%d\t", tmp_int); + } + else + { + printf("%e\t", stat[i + j * nrow]); + } } printf("\n"); } diff --git a/control/velocity_controller/src/build_auv_solver/acados_solver_auv_model.h b/control/velocity_controller/scripts/build_auv_solver/acados_solver_auv_model.h similarity index 96% rename from control/velocity_controller/src/build_auv_solver/acados_solver_auv_model.h rename to control/velocity_controller/scripts/build_auv_solver/acados_solver_auv_model.h index 03368e349..8fd75b39a 100644 --- a/control/velocity_controller/src/build_auv_solver/acados_solver_auv_model.h +++ b/control/velocity_controller/scripts/build_auv_solver/acados_solver_auv_model.h @@ -98,10 +98,10 @@ typedef struct auv_model_solver_capsule // dynamics - external_function_external_param_casadi *expl_vde_forw; - external_function_external_param_casadi *expl_vde_forw_p; - external_function_external_param_casadi *expl_ode_fun; - external_function_external_param_casadi *expl_vde_adj; + external_function_external_param_casadi *impl_dae_fun; + external_function_external_param_casadi *impl_dae_fun_jac_x_xdot_z; + external_function_external_param_casadi *impl_dae_jac_x_xdot_u_z; + external_function_external_param_casadi *impl_dae_jac_p; diff --git a/control/velocity_controller/scripts/build_auv_solver/acados_solver_auv_model.o b/control/velocity_controller/scripts/build_auv_solver/acados_solver_auv_model.o new file mode 100644 index 0000000000000000000000000000000000000000..17dcd34d54974794d4f68bd95dce9a5b47a9c28e GIT binary patch literal 35504 zcmcJ23w&Hvwf9Mzv}u7%3bo{7l>-i#LW!9^6TTvuHfc{_(ne{aKy^AzCJ#&|Au~f; zC{RpO8Ha)TKnp73^8*#d2iGDXDQyX;2%;j0Lcs@A9xZU?ExrG>_TDpVcIH&>cYojc zF*)b__kXRu_S$Pd&OZB;%UdE#CzO>r<|uPM>=f<{>NxdV3)fA2+2kDQOmo8Lc)9n` zefaF~s@BZE!Yfyu*XTB!^fI~hm)+A zdC|)}*_>V5@GRdVZMDL@Y=uHN4=>XkT+Ni$Y}zFi)sWK$Kv^|$i_)hTrBBbNYrRIz zTCIcF$YX7hj+!DJHF+J#a3^mx12O6K{qeh^l#;Y6AE&{|AAcX#DZMyyOq=S^>{G22l7r{ z@X&VN+Z>#13O6|~vzas;(l7a1j|2yG9H=0p${A$;QRUArN`q~)Npn8AvM3q0R2I_e zi_#!kUnDxuPM%kkyg?^JZUZIg=+QYJ!4QXZaY7{6VC!w5YE*JM(zV=G!Ocj?hvgQ& zEls?#m`Jy!iQ9^abX%IZy_iV1iU_AKvWu{&QzlO-N`}*?_~db8aUOJA+L)V)iF8|< zcxy3{Zc7t)6cg!I5mCIUw*6Cml#CWDq1)2A-B(Pc+k9?KMY$oInvhyZZnl%rp@?M9 zPWB4Pb~HVNBoR$?tE{kzj}{Z@wlr~XF_CUd6A2Yr(`{+ufnp-vmL|SlOr%>yLt@Q}z5mCl7nshR`SrlFSZ*ip0=7sJw-OG_G$5jO=Gg zWgw;_UgoWE=D)_4RRhM%V{&+`g|3|8cgg}+UyF;}*#}#5mzIZK^>#izL6xLqcX+IM zglUoNC+fZ2Y`*cbXV!YzvtWJRA7kZYl`~d8fejuWE%zGtr1zOj!ozQurKX2-D}#0( z$lw&tz#Ch_^AswgDjfpC@jC9!n_#k!pk`82jq2y_x~m)t_H`dTc+ku7AoR1(1Mt*r z#LJyV>bX230EE9XZy``TF1TJHfaOSBT&hI08yj<`R} z1l_=_mBH%3teHVo`I#l?`o^A{On%8;;&lZE&|2^p^T9~jeuN;drw3cF*hAf1VA`_! zz_b-jfobQovv{QOh4g*M{{+rIvtL=q&IhWdZZkMBFoV&+3C9aDN87S1yynbr zBAKV#aVk5Xq|#ksQfFRqOCQWsyc0 z6BxM!&GF}zFz#;p!x@C`hi)is&Fs`Uv}X3;A%hbmy7yEvO-2T#?+ezlB$4H)jU=>= zw#>el!+~kH^q>vqwYE5n-3p8^Ue_1by0|EjohPz`PMuwml`q2c)i4&PZS*ilXBz(K z{HQ*=81G7GuQ{Z4cE}#6X|Gl4+XqSK4kJ|Tm`VdTXBP!qGxvsv52E`%5fK!=3s&Ia zOO1!knSE`jOEW%^+l{B2Gtai<`pa4x_op9=WL^tr#=<E~emr@@bA- z*_u(FzKy(3=xxU3w~@=yZV)l3As;&qgop1f3pb9D9s9=_Au9zv3bKb^T>Ng-{=ZTC z|3>Zq8_EBHV>iPM|C@Yeb)l`Pd^b?b+dwt5gKB2myDyh;YXv0vmMX7wTfn4(8O62! zA9+E{IdmH`jRDPI4X+0j{A{Y_E2BoPSRM85$_}GWDM7=eO3pl~>#I4lD2Ni*;J5uI z$k6#weXge($NgHIi!8o7Xtb+XIuf_&iFuU6nS9`kHMx~IA$|kXDg)D+bT5u6SW9RW z1q`1Z$!-dwv!tMns6ic;Me@#hL-BNy=4EE9(Frs=xi!~JuD`|$@nwi8LR04aaH8bYr zyqd1S^wGdH_^y^w%EK^JQj;4#FMNJ;c6P93GnW# z;g-y^Sdf8x52i36DzA!uA)j276hREIbx}yw5Jw3@O@4KU)3LGdv)@hgHAa+I$OmH%x-fWRjB!)u;YGnR@>i}2zHJUxqKBCj*g`#A z1j8+tPj!3Sipm#0r>F?5ay86n4;b^w5u)_xkyDflfEEggnrC_Rn&;9g*2f#M-j?1L z!GLYbV^0U)LX)8BAj(~srmN?Y`~wJTl3PT!oCfkpE=94#vo18M2#@oTVExGGWmxRN zv;h6h8|cs5s0Ovw^jx7VYg`meKgn|iXrl-~D>|6lQ08?U^maZokx3?U%#?yNhilQQ zbeyReZgO$Pyr>EDqNaBI*5Wr9n6`N{oQ~5roM7RVH{y)k*x2fk(ZH2|1VDM$aOkyU z-f7G1r_j{#*OqyMCJ)wlME!s3@R*wQv^4(n(nmRH$VljUjmyI3v)TJ9UORYD>w7mb zWughi+}Dn29X(%SmdFZK&|B; zc{`sf_ox@orEYQB${uXXl~3_HeqHuzYwqw2ck{W!ot=9pcpdi}q$cymtbO9X*6Vn% zY(IJdBDCd#w|gD$l)dP6K*gflk<|~WYjG))2Cw5l*|Xku0^4%aulU5CD!N&81u>d3 z`*%K5o_T%Neo)Y*Py)nfBe5^op6v^I7=5mM6Q%MxnuCbimBAh_vnIIKgVP)6p`H4B z>J|nuxMNUgCqcX-=?gaH>&K(VhcH$V@T}zEiNC_31$zKAIMA7NV0Ea%sZ{q9siu)k zG!m``2bYY|iLe?iPS>^K+AXYBC<#&mWi&kFL?aPpY0eCzL1IY*3+925c3Npb*-$_f zl}a>7g~6f$7ONuY1F>!t2{whtCaS`aI?@+Z=gA5!qk`x)m@!I0WG{YujEV(keV!v} zjI)M2-t#uWxHyt8P)B9Bna(#6IDBn*Yy!;^_$dr2N9mL=2RAc{=kmO8Avw3E0yO<61xB0VW>BcstompB1W>x zYc#5@f^(=w+cBqV3H>#Ku&$>_-q4zP!pl4t$?P%xG9>GpX>5K1y`>%@mgmZAb#c&> zt5lr?Qa*J#bpv*4Uas0s$;(l^xra=x@l9QkEB}z47#`k@(RhyLIBlGOuBPbD#alQwI;WW-sD2MbtM))y`q2dI&3GB1+A}3H5cW zQG^IEqh4#ftiCl{)tWuqC0UaO)DDSYIsd1iV6Y5^<0A5U@BQw zFgjnWg2CcSRW?y2?7KwI18II1diduwpAWm)^zlVMvmiT zc(knXp}_KoLZji#BY91gfoooYbMkRj2&tBlR|8l72x+Yy_hNeCsTBb3)V$oxJxIWs zHm?eJSzahuo}0H98P6OU-FVV-=fH1xJWSO^GB2`De)?%GdxCSOr#*Vio4M&j;oP#f zusT~kY54WB^$)xpnEepO2$q1MpcxUSU-}zeQcb{!sMZ;1x>4EMxH~X1z!PBQ=lpB1 zbOhb~4tT!M-R~;a#AfaLCt5M9{uyf6RNuI+mbs!qnD$&kV+n{IK`#>V3ULd0QS}Ge z$=3ZcB5W?CFRQlhYpnZv>)y5Q8?5`A=wNIp)~{19iuI-AFyg?7whZ2IL11=(tUl=WGX%w z?M%n|oX(AjSbuLvGTJfF*Ei6iIH`?;@j^->-OmC^8;W(uB^FdqfFf|}@ zx**mO9g3%uai=eSQ9Q9OKGYrUj}2XzjtzCDocM-R6y^^>buu+D6laC86mI(lx}$?b zz5Ow5e_~*$pFt{!DiTA;jLI#L#oZ84h-jqWk$y7{T z#4qIg-h{sB80a5#`ePeZ(cPq}Ff|#A4}-xZ3*aI-KS1 zX{WhloK73;izQC^@N%cOf3Ppw8H-1|(uty*D8<}{=!VXLRP^FPia^uO#-VtZbB=TN z2IuUw)7!aW-3F%*zchY)7w1zNU>acBL6S3dpkpvfh71nH$vf#(-0h0>_QgAon>y7= zY;Y23CvmZpKy41O<%Acq7LCX#fKq^oY6@msec?j%s_J-F?0l6_7R z*~j{wi=DwNHoU2by4a`&^L5NyN6<()MAmJi9kd>M`n5u z^%sw(di&8Iqeo4i>UP5g2^T#kdNp^T%cV4z)7;wA+!H!yxt+aTU1&RQ*U&(}o9cPXL`NL3ix!#cjrG9}^q2|2XkXl) zktZ+&azA1GS`<}k$UoIh4Q(W;)Brck6M9Kyte+Y=VhELq@iIP?Ko;opcsxkfxh*#4 z_QaBIkt~sV$=~^At~!SV_0KMaDp{BsK(-ig`(~qBsG1RXy(nvfD=!sCnGu0Hf~cdJ zg5I7-Qoe{^IEBwNnD7PlkyFTl=!fXcBC)=~o)~;Eg^;|wzJ{Q_VkP>TN1~~n>HhwW z`EtXgSjz2)CERsrRInNC2!^hUr`E^OCbiosfy(8o#w)!EXCkuVL%ra*)HA#BuCCsW zUe!3EYDvLYRAuAZ0ea4~8V)e)Dv_?+320Fou}DYZIB@#4kDU1IVRh3!e983hFMI0Z zEvM&Rxb4Q*?!EQHiZi|G&dj4<{L#Z-Ui9|OFFoA0uKjc^+r;M!UT->O$;{`TUU1Cm z*I!4+?Wf;y*B>5xa?|6dUE8_wsmuTMtp6RETtnp+pDt3RayLnUrQm(7_| zS3hr-hFJyar};hQv26J&%6F77UTnshxBB5+cJW7@vJKT`?>&5SWM9s_u}Hbo2!~8 z3?b(wC35blYFIiBZh*?yNiU8Qs4}2GNBXat7_M@+mc#zxisq{N38Pc1YJr4JNtzFH zNOm5{;5eQtv6Sqxs=*1(mMpwLJ{Z)pyz+s2+9A0`V=fI6g|f1r$CY(+*~Fz4RacgI zRoluY)|TOFanTp&2nT)J1l|v0wZqoXjNI&ZS zys1@{VFU}s$#$K#sS-B@pOmV9#9rCuD8~4TKI5?7s%;Y{5MkosrGxVfVsT%}c z(?RE%8asi2wyNs2IMr+HGdgarfp*g-e0?(E`|OT0*d3Si^|__u75d39pVxVug=_Ll zul5V2-Kx_*q|$2ItK75EWZ2$c=`_=iP#N}W%(QdD{>#E@fmKqcfa4ze1AkvN0a3qo zVsn){Tt4B4RW(bvDLc+;B+t{__fQ=b>ZVk@F00z%Myhr+;kPzYbs#?Rvt=dw2O z54LID4Y;SW51?>l^GzD3aG-;*J2gh{@#rAzp%U0%OJJ{;zz)}qQtERpjQlvy!YFUQ zgTkcTdFf!O^og|R-#Xrl`w0T)z8Bac{K_ywxH!{soH{ul#hGw(yx&~lJW=2V?%@kl zLW4Sv8Ysu(Db05aRKB{{2j8r5iYa+)%r9ZG2f6K~2iFd{N<0yvXBv zzNv6#Nf6gx<@-v0TP>5m$u|{FvjlN{v&N$q{(X&avhW{k{0kO-yT+BiLgL*TzpDW8 z)gFyMY~jDu_}?x3H+-+!hD`c{=D#69T>n|)M_3{E7me3g_&$w)(8B3s2AWMQvhaUu ze1(O-rSX`Bt4}^FoL&o`%=Z<}dJ9)}S91TzD)pbEH2;$p|35UIweVv#{#gs>c>v1w zB?~`U^S^1~A-Ow(RZ{cEEdEB#x66Au)C7R zQT12hylLS){-VBS=y`%s^+o1YI_(C~=dWn}Ct3V&Xnd)K->mVdh5tb7r*Tjow`qRT z;@_e15ewg`^{kajaIO4U;aq3&AJBSkvGCt&Jw2A5$29*wi~pp?|8C)b)A-94{+!11 z4-7h=mo$F3o=ceQU(tq1D3Q ztNCYJ_;DH^wD1pVe5-|@!gwVXB#V!^nxD1!jT+Bc_~{zoX5lT2PvJR?%;8MU|D47D zh{mt8@N+c&6$}5E#=mCa9U8yM!h1FTeG4DZ_>U|+&G?jY{n90jqrF@8eudWapO&6a zX?%x;=QRE^3;&$fU(M%&=T!Tta2~bzU)6dZxA1Rk{0R$J?XALj%EEu5`G2+WJ2d`` zh2Nv`mn{5#jUTk|hcrG>&r?mw{-E*67XF0B54Z5YX?&W6KdJ;$F*?vJXPU*z`|9(fO(x&-WgiY2QB{lG=7qW2Q_|*g`cePkcH3D_*@Hb)c67m zZ_;?!!k1~h)xwuEKFR6fv#Bk|p^a-aUpmj?uhDpig?DKD0t>%D;}=@^g^W*fYAt+& z=5Mm_%Qb$Pg{%Iq!uh0ytNsh+vT*f`igwNV3g!Khwr9J=|GLh1tA*bTyt*KszuM`8 zKkb9R0{lok32oDlLS~KWDCAd4|1rRcf1~D`=i}?}rAR4$gXWKFzM7ZgCA*J)s=rct zuJFOX=!4(jgMSY=m$y>6OwC^%=K&x7?|ks5wVu%_N)NAT;c_Y-)k@i4?}IP%!7uQ^ zxBB2;_rdS=!T;ieABirel>G~Q@Gc+x8XtUz55CU_KMHYE%AWZ?_!=L4*a!cv5B``B zUWvG&_&J~-q~S(B7HZr*mulw*1LmZc~v&IL1A8_(Rlh)7cjmV?bhyO7jJn4gf$_M|N4}QB3{)i9$ybnGZ zYssbjaEuQ=+Xr6^ocuF-ta7~3d7j1}()bCwUWPU9xC&6uX^wNfkN$6K{*A{eKCgM= za*xq-yux|C6PM5V=&3{;lb!XNkFe)svBsM;Zul0{x4~~+QM(wxNG5u z*dR0R)ued!I53FKMbT*cM_X2|JU6;@)$%1PTURWPM(aW)(R!yYvZP@?{ax6|*9-Y? z1O08}RQ?m9`#JPyF8!HDe^_b(T`3v53Mn?zL(C7cK!_zmED~aw5DSGkqY!63hh^ue ztXOsq%g$lhIV?MeW#_Q$9G0EKvU6E>F3ZkUcCzeTmYvJ8b6IvS%g$xlxhy-6W#_T% zJeHlO3dyqbSau%E&STkmEIW^7=d+{zB$2WX?jCTF6ohnYmCgS+;?5X<(@amTF+B29|1M zypi!n#v7FlY+)noY-F8{th14IHgamHo>hnHSx2azm4xb9OQ@dJgz8yCsGe1X>RCsq zo|S~^l@=2+N{bE}9W^>^blm8`(UGG=N5xJ^1y4vtPiT&^Ma54@g-%GtQAmYQNX1Y{ z1yM-FPe=t&Xs#-Q3Z9UPo{$QwkczjE3Z@XE$!rqS+eWAXV*h5!507Gx7B)z9$I%{f z?)XHsqvsRY%-7pDe#?Pw*Lv3ub}7;pqVcq@p?ExMNaH$(ar>C^meDRsVQ9xd65GeH zr3Skt0Y{fbR$yyobj8x8XSJ-1t_&}Zv_!Fu1bYLAV0#zu>+2%x$6F*iB;E_rAMa_%3mcaO0d${`}uDfzJ)1P6!j~LMSGWa{X3UiT7BH3 zH>!!YrDI=hAm>eE!ULMFS`+Q}LoJ%VD!(s0-`t~C=+Pcq+D(Q2#t4b+%c7eoxV(@d zZ{Qde)c zjdu3-W5+yizsyTB247O&gZ;Nd$?++A`>)ZRmk@p%FGQVg9+g9gl|OmVg3x_ih?du= zkF&=+Pmd7TePwKZh`h<8q)%Fng_| z*aa(gXJarJjMG+3Z0k#o&zwt_&rRKxQjgIGzUYsouvrir9w`sC3%~c`g0V2;ki2n< zX_PL`d`JysopF2>Ft9P71~=Fc)xM4Cy0T#oI~&w-Zo@P-=k?}005kI1U4kBsxE$k3 zr*`?%&d);2=q?o?9cg^{(H~WDGQN%=!@Wn*kfVDRlSVzuI0Gn=zo_v|E$n$NElugl zQU2-tcvUiQk|y8#%c@7CORCKg!9Lu6kPT=b_PJBEn=J~w>r!{Az zC#i8;&sM$1M_}>w?4X0IgiuNFA{lVNlapZ+ld!|&6$&A(FcY3b1L&AtqqKPvdNv}gE# z^3nf>;MWNLbTSZ!tzW$lR`$~u#YTU`hre3mbUH@xFAzBG{W5w|LXWK9t%5&O@YQ?J z@%8vE!GE9N|G-E8PQj3j}!Ru0zc75PlMopK=509_z}Sm3VuxBwB%&$N%-hV3%=CzX@S#{lhN}3tg=RF5a&XUZGY zxXnLV@J|-}CV|fuc$<%&m4bhY;P(o=PT&{$=($wzX)W5=|5<_83;e4-dcH0AA;G^x z;P`eoum3(DJ%1GZxq@%@x>Bj;h8dZr8h0>Pgp@Pz_j z;G@SAd|E3u`L6QecME=_;HP}}^nQ^JD%Yv_8U3I0;a@NK|3~nDAaL4)YxLadqi2`k zOFfSYoIVmUdj964=Xt@G<9OL52;-n~(MKyrPnE{)`g)JxpDy?(3A{<*3w`u_Sn#Eu zvjiR%de-{r=@$IOg1<@NO9Y!T`Zwvev0{_1Pe^}r*3H&jE-z@OG z0{@P{pAqNeKK1!M{h~^vrAYJSg=4 zMBsY`|04o_(T9If@aY>BqyGpp0teYI_16phHl!K;VvSR|&JuW=;LHAJmB3}c*C}wR zf1}X9Qt00<@Kpl8Q|OWT?iRSr_qPI<`93T3pDpwsQ-vEGCee;8MRU@XtWb_-BsBsa|M`VenH0zFOezLXWgFA^7JC{zk$7Pl0Cy|2%8(TD#p!H)`l)ls;?Ve79Kxb$0t558F7(x2xET>5RD#;G3H;%EHub%Dz`zeVVg z^?Rq_%Q%0)hyS$T%Q%0*hhI4z#^E6Ud=5WjXS2r1emO5XTi|C1{-=bVYX$y2flI&r zMCg(FcM6<_dSg%HdvJq;>^T!ZgRjsy+5ZKBe?s80Tw8qbt9d&O-y!hZefWC?U+Vvx4}UtnoWMcl`Wb%44<~D!{L?A$If8$Oz!wSp zPJt&heiAVGJbtOr9~bhku^n4+#D`AAU;k z2L*q#5C5}*f1%*t;KRRF@P`EdP9OdQf}a%pKl|_x3VurPkE54uILM#7@iTrN^ueo@3wukykBeee;1%ko|$a5*k~Rp5JIm+{XI zf!{0e`+e}=3;d^o|Fpoz1pb1+zasE=1pZ@zPdXMiILL25!q3=wjKJ>`_!5m%J<51k zC-~Ao3Bi~3o)dgo?>7j(toQHx@b?P-dST}?f`5m=UlRD80&jA0gM<7a$I}a3LREjV z4rk+^v`eVs%kgx?WtjUPIi7xABVpgTQYV_@e^@{ zz#kL*-2(rkz#kC!;{ty~;C~YM>jM9?z{@{?8yr+#i68BQpCE8qUv)nCLLdApjZZ+n zSKw#-e5>F;DezIj|D?e8`0#%(_``z#M<4z_1%Hd+zvII{A_x)=vVR0WV}DTNj;navE!=zu`V$Lxj#vDLEZltWY5pA!V^5Rj zzi#o@YTW!g8HT@ExKsChUg)D%~*7c56V=kIoEY0Q+beX=-V;;yR!68_&Rm?7k%S(_(w@k z;SaF%eRm>VhkrPD5Z@p--V^Pje~wp`?0@&yg*Y{tNtT)>eLw5O;spKLArHP6 zS5zfUc|=`oI8U7$YSlS_6Xm0uuUQV^Y9d^Rr6S{Rua&P!Ag(-S>zgf$heR+m&%in;6Qt^N5CzXWycZ=}9D8((U zdmH&0m0U7!O1f`>B0TyYbtZr3@U!LV-HjdlBynSe{yNSdC{uCR{+ouhQu3Qel+Z0g JmM(4i{{~5-Bo6=p literal 0 HcmV?d00001 diff --git a/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun.c b/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun.c new file mode 100644 index 000000000..acd0b61e9 --- /dev/null +++ b/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun.c @@ -0,0 +1,394 @@ +/* This file was automatically generated by CasADi 3.7.2. + * It consists of: + * 1) content generated by CasADi runtime: not copyrighted + * 2) template code copied from CasADi source: permissively licensed (MIT-0) + * 3) user code: owned by the user + * + */ +#ifdef __cplusplus +extern "C" { +#endif + +/* How to prefix internal symbols */ +#ifdef CASADI_CODEGEN_PREFIX + #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID) + #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID + #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID) +#else + #define CASADI_PREFIX(ID) auv_model_impl_dae_fun_ ## ID +#endif + +#include + +#ifndef casadi_real +#define casadi_real double +#endif + +#ifndef casadi_int +#define casadi_int int +#endif + +/* Add prefix to internal symbols */ +#define casadi_f0 CASADI_PREFIX(f0) +#define casadi_fabs CASADI_PREFIX(fabs) +#define casadi_s0 CASADI_PREFIX(s0) +#define casadi_s1 CASADI_PREFIX(s1) +#define casadi_s2 CASADI_PREFIX(s2) +#define casadi_s3 CASADI_PREFIX(s3) + +/* Symbol visibility in DLLs */ +#ifndef CASADI_SYMBOL_EXPORT + #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) + #if defined(STATIC_LINKED) + #define CASADI_SYMBOL_EXPORT + #else + #define CASADI_SYMBOL_EXPORT __declspec(dllexport) + #endif + #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) + #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default"))) + #else + #define CASADI_SYMBOL_EXPORT + #endif +#endif + +casadi_real casadi_fabs(casadi_real x) { +/* Pre-c99 compatibility */ +#if __STDC_VERSION__ < 199901L + return x>0 ? x : -x; +#else + return fabs(x); +#endif +} + +static const casadi_int casadi_s0[3] = {9, 1, 1}; +static const casadi_int casadi_s1[3] = {3, 1, 1}; +static const casadi_int casadi_s2[3] = {0, 1, 1}; +static const casadi_int casadi_s3[3] = {0, 0, 1}; + +/* auv_model_impl_dae_fun:(i0[9],i1[9],i2[3],i3[0],i4[],i5[0])->(o0[9]) */ +static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { + casadi_real a00, a01, a02, a03, a04, a05, a06, a07, a08, a09, a10, a11; + casadi_real a12, a13, a14; + a00=arg[1]? arg[1][0] : 0; + a01=3.3453634479321918e-02; + a02=arg[2]? arg[2][0] : 0; + a03=-30.; + a04=arg[0]? arg[0][5] : 0; + a05=(a03*a04); + a06=arg[0]? arg[0][1] : 0; + a05=(a05*a06); + a07=30.; + a08=arg[0]? arg[0][4] : 0; + a09=(a07*a08); + a10=arg[0]? arg[0][2] : 0; + a09=(a09*a10); + a05=(a05+a09); + a02=(a02-a05); + a05=23.; + a09=arg[0]? arg[0][0] : 0; + a11=casadi_fabs(a09); + a05=(a05+a11); + a05=(a05*a09); + a02=(a02-a05); + a01=(a01*a02); + a05=6.0368975005218254e-05; + a11=(a07*a04); + a11=(a11*a09); + a12=arg[0]? arg[0][3] : 0; + a13=(a03*a12); + a13=(a13*a10); + a11=(a11+a13); + a13=46.; + a14=casadi_fabs(a06); + a14=(a13+a14); + a14=(a14*a06); + a11=(a11+a14); + a05=(a05*a11); + a01=(a01-a05); + a05=-1.1116270017027866e-07; + a03=(a03*a08); + a03=(a03*a09); + a07=(a07*a12); + a07=(a07*a06); + a03=(a03+a07); + a07=casadi_fabs(a10); + a07=(a13+a07); + a07=(a07*a10); + a03=(a03+a07); + a05=(a05*a03); + a01=(a01-a05); + a05=9.8084735444363873e-08; + a07=3.3399999999999999e+00; + a07=(a07*a04); + a07=(a07*a08); + a10=-3.3199999999999998e+00; + a10=(a10*a08); + a10=(a10*a04); + a07=(a07+a10); + a10=casadi_fabs(a12); + a10=(a13+a10); + a10=(a10*a12); + a07=(a07+a10); + a05=(a05*a07); + a01=(a01-a05); + a05=1.0920100546139184e-05; + a10=arg[2]? arg[2][1] : 0; + a06=-3.3399999999999999e+00; + a06=(a06*a04); + a06=(a06*a12); + a09=6.8000000000000005e-01; + a09=(a09*a12); + a09=(a09*a04); + a06=(a06+a09); + a10=(a10-a06); + a06=casadi_fabs(a08); + a06=(a13+a06); + a06=(a06*a08); + a10=(a10-a06); + a05=(a05*a10); + a01=(a01+a05); + a05=-6.0150572994295574e-03; + a06=arg[2]? arg[2][2] : 0; + a09=3.3199999999999998e+00; + a09=(a09*a08); + a09=(a09*a12); + a14=-6.8000000000000005e-01; + a14=(a14*a12); + a14=(a14*a08); + a09=(a09+a14); + a06=(a06-a09); + a09=casadi_fabs(a04); + a13=(a13+a09); + a13=(a13*a04); + a06=(a06-a13); + a05=(a05*a06); + a01=(a01+a05); + a00=(a00-a01); + if (res[0]!=0) res[0][0]=a00; + a00=arg[1]? arg[1][1] : 0; + a01=6.0368975005218423e-05; + a01=(a01*a02); + a05=3.3484658136227786e-02; + a05=(a05*a11); + a01=(a01-a05); + a05=-6.1658244361114702e-05; + a05=(a05*a03); + a01=(a01-a05); + a05=5.4404333259807184e-05; + a05=(a05*a07); + a01=(a01-a05); + a05=6.0570157695918683e-03; + a05=(a05*a10); + a01=(a01+a05); + a05=-3.0184487502609172e-03; + a05=(a05*a06); + a01=(a01+a05); + a00=(a00-a01); + if (res[0]!=0) res[0][1]=a00; + a00=arg[1]? arg[1][2] : 0; + a01=-1.1116270017027936e-07; + a01=(a01*a02); + a05=-6.1658244361114783e-05; + a05=(a05*a11); + a01=(a01-a05); + a05=3.3963490377380855e-02; + a05=(a05*a03); + a01=(a01-a05); + a05=-2.9967785627100754e-02; + a05=(a05*a07); + a01=(a01-a05); + a05=-3.0801331505514837e-03; + a05=(a05*a10); + a01=(a01+a05); + a05=5.5581350085139543e-06; + a05=(a05*a06); + a01=(a01+a05); + a00=(a00-a01); + if (res[0]!=0) res[0][2]=a00; + a00=arg[1]? arg[1][3] : 0; + a01=9.8084735444364535e-08; + a01=(a01*a02); + a05=5.4404333259807062e-05; + a05=(a05*a11); + a01=(a01-a05); + a05=-2.9967785627100469e-02; + a05=(a05*a03); + a01=(a01-a05); + a05=1.4970303990827358e+00; + a05=(a05*a07); + a01=(a01-a05); + a05=2.7177645446042520e-03; + a05=(a05*a10); + a01=(a01+a05); + a05=-4.9042367722181902e-06; + a05=(a05*a06); + a01=(a01+a05); + a00=(a00-a01); + if (res[0]!=0) res[0][3]=a00; + a00=arg[1]? arg[1][4] : 0; + a01=1.0920100546139210e-05; + a01=(a01*a02); + a05=6.0570157695918657e-03; + a05=(a05*a11); + a01=(a01-a05); + a05=-3.0801331505514781e-03; + a05=(a05*a03); + a01=(a01-a05); + a05=2.7177645446042498e-03; + a05=(a05*a07); + a01=(a01-a05); + a05=3.0257778596593993e-01; + a05=(a05*a10); + a01=(a01+a05); + a05=-5.4600502730695923e-04; + a13=(a05*a06); + a01=(a01+a13); + a00=(a00-a01); + if (res[0]!=0) res[0][4]=a00; + a00=arg[1]? arg[1][5] : 0; + a01=-6.0150572994295635e-03; + a01=(a01*a02); + a02=-3.0184487502609133e-03; + a02=(a02*a11); + a01=(a01-a02); + a02=5.5581350085139340e-06; + a02=(a02*a03); + a01=(a01-a02); + a02=-4.9042367722181935e-06; + a02=(a02*a07); + a01=(a01-a02); + a05=(a05*a10); + a01=(a01+a05); + a05=3.0075286497147785e-01; + a05=(a05*a06); + a01=(a01+a05); + a00=(a00-a01); + if (res[0]!=0) res[0][5]=a00; + a00=arg[1]? arg[1][6] : 0; + a01=arg[0]? arg[0][6] : 0; + a05=sin(a01); + a06=arg[0]? arg[0][7] : 0; + a10=tan(a06); + a02=(a05*a10); + a02=(a02*a08); + a12=(a12+a02); + a01=cos(a01); + a10=(a01*a10); + a10=(a10*a04); + a12=(a12+a10); + a00=(a00-a12); + if (res[0]!=0) res[0][6]=a00; + a00=arg[1]? arg[1][7] : 0; + a12=(a01*a08); + a10=(a05*a04); + a12=(a12-a10); + a00=(a00-a12); + if (res[0]!=0) res[0][7]=a00; + a00=arg[1]? arg[1][8] : 0; + a06=cos(a06); + a05=(a05/a06); + a05=(a05*a08); + a01=(a01/a06); + a01=(a01*a04); + a05=(a05+a01); + a00=(a00-a05); + if (res[0]!=0) res[0][8]=a00; + return 0; +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ + return casadi_f0(arg, res, iw, w, mem); +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_alloc_mem(void) { + return 0; +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_init_mem(int mem) { + return 0; +} + +CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_free_mem(int mem) { +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_checkout(void) { + return 0; +} + +CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_release(int mem) { +} + +CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_incref(void) { +} + +CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_decref(void) { +} + +CASADI_SYMBOL_EXPORT casadi_int auv_model_impl_dae_fun_n_in(void) { return 6;} + +CASADI_SYMBOL_EXPORT casadi_int auv_model_impl_dae_fun_n_out(void) { return 1;} + +CASADI_SYMBOL_EXPORT casadi_real auv_model_impl_dae_fun_default_in(casadi_int i) { + switch (i) { + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const char* auv_model_impl_dae_fun_name_in(casadi_int i) { + switch (i) { + case 0: return "i0"; + case 1: return "i1"; + case 2: return "i2"; + case 3: return "i3"; + case 4: return "i4"; + case 5: return "i5"; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const char* auv_model_impl_dae_fun_name_out(casadi_int i) { + switch (i) { + case 0: return "o0"; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const casadi_int* auv_model_impl_dae_fun_sparsity_in(casadi_int i) { + switch (i) { + case 0: return casadi_s0; + case 1: return casadi_s0; + case 2: return casadi_s1; + case 3: return casadi_s2; + case 4: return casadi_s3; + case 5: return casadi_s2; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const casadi_int* auv_model_impl_dae_fun_sparsity_out(casadi_int i) { + switch (i) { + case 0: return casadi_s0; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { + if (sz_arg) *sz_arg = 6; + if (sz_res) *sz_res = 1; + if (sz_iw) *sz_iw = 0; + if (sz_w) *sz_w = 0; + return 0; +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { + if (sz_arg) *sz_arg = 6*sizeof(const casadi_real*); + if (sz_res) *sz_res = 1*sizeof(casadi_real*); + if (sz_iw) *sz_iw = 0*sizeof(casadi_int); + if (sz_w) *sz_w = 0*sizeof(casadi_real); + return 0; +} + + +#ifdef __cplusplus +} /* extern "C" */ +#endif diff --git a/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun.o b/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun.o new file mode 100644 index 0000000000000000000000000000000000000000..0dc1612cdaca9a4f10a4ca3b8fc76bd5eb4d0921 GIT binary patch literal 11584 zcmb`N4Rq7h6~O<60&QhU>Nf3wL8d_?LQK*YDk4dsg-olb)1t|Yv>{DwN55=RDS}v} zbvQ)pWFB-*@p$yG=@B+Pj>A+kJ_J920v({DFs;~57*M7%26p$p|9#2rqv@RT&dLA1 z_j~u=_jTXL|KHwQR6Ju)TAC(9nl@ES<|Ng$xHXw}i=<&hkER_Ga$`C%uc0tON<~vY8p))_5$l$7nJ|erjB_*SoC9ROIOF z13Lg}K#VPmGBgcD>#gwkAyxtL_b4tm^%(todyLsJO9skhM2yzWkgc#-!E$XUWIqz6 zW-Nt=e}~AT+WYYPYmd>lsmB;Nbkev;G(TdzwH1a1LxWJSkI@t>?O(ZxH#-+>K`hl_K&S4t% zpmsY*iN5*7&?4&SDNGLZLJJOmOtdm!!GU}iiz~n|MAep@9*@y?qIg+2BT@uYfR6(p zbP~pP7bq56>LIris@(`VF`)hqAQd>MzPAN)qX`i9oe&BzM*epo^<#Yu{n*uo7U+%t zV5HD!g{<#gKx0L*$LNRILT#wZ3r+HGf&tf6!RlK`I>5evJ3c<`DKh#tV#7-zeLU4d{>p~wtkHhIFpOc-0rF?Fk026i6 z%J#!l`gNb_qY!u#I83Xbwl8ih01LtoQ|zZLtxp+jPkx;4vx&OE`L+N&0kS_aErhNFa2pChUD>x4 z=y`XdZN*r7HkgE3V^gr%vZ7u`Ae};xEBT9L(6XO;>~zm zIpVk7?g@Bq_ssT`LHouO3im$-JKV82I{!O3s<^^Wfo5`|jOYy63&1ejANqI&dIX^Eq#lc5^ErJjWC3X8p-SZu=M4r$NTb~R{-B$KEo||C(PAm zIPN;Z0eszf-oR|aSq@w9c6`L)k$t@xpVO`A^sN`1?S->ZzdC?lbinTj{J@CyKz9%d zVRBnw!Zi(@t@T+T-KP+@ftFag-mQg}aumRXRVGX_VU~#unXqF*xMpSdmalqyjboaT zh+`6(F6t>i1NF>-N5m%U=k@te0qe`bgs@fL{kpKFLH9op#0-6alLqc6FehT=;9?CG zNY^hFOA<D!uy|^ z-xNvq=bMzP@@jtFQ0P9bKeV?FFQ_b!ZHBvNGf;8=T+%yaUY&Qjn70Rdy`l8=-n90q zy@!lr+_2jTcLj01{pTwWV<7eiY+q~ddTa`r_8N;e_8l*5pX)oePu8cl|C`kBZS9Q> z-8u|M)!X_dB!}XFdRuox61Hkgz6D9~8h~?~oXtoZ+9!SVaMDMwPpMyJ*Qy*^l~b$A z)vEHes(h_ZEDo&c{|@}!1k*AP$Ad`Ae5R6@74_=jUYV!3xo`fsOV3Y@Q|#8`Nfgg? zuN&R^`Ons6xgXy6g*EVSR(umi8$Wd&+?4go-^|ar){NeM)nm^aT~qEheU`QFo;T7L z^mdn)-aYlDzaOx#Ied8PrJ>88pZaXmhHWHwcFWak4qKjaxu-4qb^8^j4Zj;PZuQXn zhr6s#7ruS%&L-Djw{OD`d`;Dc zl?Pi|w32;OCsMu3$lh3z+x_0|v3pN`YEhW50BzY&Li$t_>IKv3a=zy zqVW4fK0}Nl6^ZDVDfolLxh$q15g=2;9h8U1L?-hb`@bbFt^`bB6>&3Tn0!v;GZX$V zi(epam4M>4#0wPuDsivEbzjcZ+zQ`F@^ai{@iyYpk0jqgyj%i`JBU{*{C(n03O_`= zUg3v{H!J)I@udpyA>O9&Q^Z#){A=Q?6`n49KT}(!@GRo`cqa=@#MdQ}NL@rcuJBRB z^>IxWUP63-5{Z*JcN_ZP(V_9l5f@f^j@UBu;nDK!=nH!Je@5!c6CN^9Cu;z^>X zmJ2>4alXm&M~Tb4j2|a%lz`%=iK{!}bHwG|FV$WkF84>t|495o2`K(E@C(_b$TM~i zaTCdja~9J704^Cfbs_~nnu32p_K#BsH-%<@}5%-bP%W&({+-;Y9?3Jfr6lw-fj2 z#o9yCjw1gj;%kY^^LhvI;|kY;jdzvR)K!G5%c^SXtIH~a;j&0$tu2(Ipe$-nS>jAt zlAE%`q1lS3JMl3QA9j4?;Uga(lkic14~HnZS|o*zBUhw_szYcvgo;DhaR^O^E-wr? zg#o8tpU`p&3r?Zs6c(I9*C}+Jx|XnzE3|TjghayTQwInfkDtwpP9Lv>Lt7OYJn zQ5LMOt_zjbglkd>RMl3+1}cvuZX57vbLSMdN9MC*eM(W=<|0Zb%B1~4Jfp~14U zIrC%TXyQH!+m?n~5Pba+VJ__}$Z?C1JjytpC6aGt9QQWKzaWn9bol8~@)5AlV^HnO za}+tZGlk?;J2#}@`aJ>PG5Xf4pV#Md{d_!^?_ll3Gf=kw0OJP6XJf-5V1#G3l)r~@ zJSQc8oN?aY_ZiP-^77(<#du~*JCk5P$AJ9%@RNKdpIcHE3}e=A{}`&&8V+~4Li zK9<@472_7hpJ1H(+aDOeoXM|eocr6`jB|h6%Q*M9&l%@_b%t^7hv^y62naaN+^YQnW(sXyeH^kIOXmlOzQstCsq=Sb?y@w)*Sir_mB567u~Gl!6t`-_Zk1J<9u{=Xucrhjiv z8jVX literal 0 HcmV?d00001 diff --git a/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u.c b/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u.c new file mode 100644 index 000000000..0321b6e18 --- /dev/null +++ b/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u.c @@ -0,0 +1,847 @@ +/* This file was automatically generated by CasADi 3.7.2. + * It consists of: + * 1) content generated by CasADi runtime: not copyrighted + * 2) template code copied from CasADi source: permissively licensed (MIT-0) + * 3) user code: owned by the user + * + */ +#ifdef __cplusplus +extern "C" { +#endif + +/* How to prefix internal symbols */ +#ifdef CASADI_CODEGEN_PREFIX + #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID) + #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID + #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID) +#else + #define CASADI_PREFIX(ID) auv_model_impl_dae_fun_jac_x_xdot_u_ ## ID +#endif + +#include + +#ifndef casadi_real +#define casadi_real double +#endif + +#ifndef casadi_int +#define casadi_int int +#endif + +/* Add prefix to internal symbols */ +#define casadi_f0 CASADI_PREFIX(f0) +#define casadi_fabs CASADI_PREFIX(fabs) +#define casadi_s0 CASADI_PREFIX(s0) +#define casadi_s1 CASADI_PREFIX(s1) +#define casadi_s2 CASADI_PREFIX(s2) +#define casadi_s3 CASADI_PREFIX(s3) +#define casadi_s4 CASADI_PREFIX(s4) +#define casadi_s5 CASADI_PREFIX(s5) +#define casadi_s6 CASADI_PREFIX(s6) +#define casadi_sign CASADI_PREFIX(sign) +#define casadi_sq CASADI_PREFIX(sq) + +/* Symbol visibility in DLLs */ +#ifndef CASADI_SYMBOL_EXPORT + #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) + #if defined(STATIC_LINKED) + #define CASADI_SYMBOL_EXPORT + #else + #define CASADI_SYMBOL_EXPORT __declspec(dllexport) + #endif + #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) + #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default"))) + #else + #define CASADI_SYMBOL_EXPORT + #endif +#endif + +casadi_real casadi_fabs(casadi_real x) { +/* Pre-c99 compatibility */ +#if __STDC_VERSION__ < 199901L + return x>0 ? x : -x; +#else + return fabs(x); +#endif +} + +casadi_real casadi_sign(casadi_real x) { return x<0 ? -1 : x>0 ? 1 : x;} + +casadi_real casadi_sq(casadi_real x) { return x*x;} + +static const casadi_int casadi_s0[3] = {9, 1, 1}; +static const casadi_int casadi_s1[3] = {3, 1, 1}; +static const casadi_int casadi_s2[3] = {0, 1, 1}; +static const casadi_int casadi_s3[3] = {0, 0, 1}; +static const casadi_int casadi_s4[60] = + {9, 9, 0, 6, 12, 18, 25, 34, + 43, 46, 48, 48, 0, 1, 2, 3, + 4, 5, 0, 1, 2, 3, 4, 5, + 0, 1, 2, 3, 4, 5, 0, 1, + 2, 3, 4, 5, 6, 0, 1, 2, + 3, 4, 5, 6, 7, 8, 0, 1, + 2, 3, 4, 5, 6, 7, 8, 6, + 7, 8, 6, 8}; +static const casadi_int casadi_s5[21] = + {9, 9, 0, 1, 2, 3, 4, 5, + 6, 7, 8, 9, 0, 1, 2, 3, + 4, 5, 6, 7, 8}; +static const casadi_int casadi_s6[24] = + {9, 3, 0, 6, 12, 18, 0, 1, + 2, 3, 4, 5, 0, 1, 2, 3, + 4, 5, 0, 1, 2, 3, 4, 5}; + +/* auv_model_impl_dae_fun_jac_x_xdot_u:(i0[9],i1[9],i2[3],i3[0],i4[],i5[0])->(o0[9],o1[9x9,48nz],o2[9x9,9nz],o3[9x3,18nz]) */ +static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { + casadi_real a00, a01, a02, a03, a04, a05, a06, a07, a08, a09, a10, a11; + casadi_real a12, a13, a14, a15, a16, a17, a18, a19, a20, a21, a22, a23; + casadi_real a24, a25, a26, a27, a28, a29, a30, a31, a32, a33, a34, a35; + casadi_real a36, a37, a38, a39, a40, a41, a42, a43, a44, a45, a46, a47; + casadi_real a48, a49, a50, a51, a52, a53, a54, a55, a56, a57, a58, a59; + casadi_real a60, a61, a62, a63, a64, a65, a66, a67, a68, a69, a70, a71; + casadi_real a72, a73, a74, a75, a76, a77, a78; + a00=arg[1]? arg[1][0] : 0; + a01=3.3453634479321918e-02; + a02=arg[2]? arg[2][0] : 0; + a03=-30.; + a04=arg[0]? arg[0][5] : 0; + a05=(a03*a04); + a06=arg[0]? arg[0][1] : 0; + a07=(a05*a06); + a08=30.; + a09=arg[0]? arg[0][4] : 0; + a10=(a08*a09); + a11=arg[0]? arg[0][2] : 0; + a12=(a10*a11); + a07=(a07+a12); + a02=(a02-a07); + a07=23.; + a12=arg[0]? arg[0][0] : 0; + a13=casadi_fabs(a12); + a07=(a07+a13); + a13=(a07*a12); + a02=(a02-a13); + a13=(a01*a02); + a14=6.0368975005218254e-05; + a15=(a08*a04); + a16=(a15*a12); + a17=arg[0]? arg[0][3] : 0; + a18=(a03*a17); + a19=(a18*a11); + a16=(a16+a19); + a19=46.; + a20=casadi_fabs(a06); + a20=(a19+a20); + a21=(a20*a06); + a16=(a16+a21); + a21=(a14*a16); + a13=(a13-a21); + a21=-1.1116270017027866e-07; + a22=(a03*a09); + a23=(a22*a12); + a24=(a08*a17); + a25=(a24*a06); + a23=(a23+a25); + a25=casadi_fabs(a11); + a25=(a19+a25); + a26=(a25*a11); + a23=(a23+a26); + a26=(a21*a23); + a13=(a13-a26); + a26=9.8084735444363873e-08; + a27=3.3399999999999999e+00; + a28=(a27*a04); + a29=(a28*a09); + a30=-3.3199999999999998e+00; + a31=(a30*a09); + a32=(a31*a04); + a29=(a29+a32); + a32=casadi_fabs(a17); + a32=(a19+a32); + a33=(a32*a17); + a29=(a29+a33); + a33=(a26*a29); + a13=(a13-a33); + a33=1.0920100546139184e-05; + a34=arg[2]? arg[2][1] : 0; + a35=-3.3399999999999999e+00; + a36=(a35*a04); + a37=(a36*a17); + a38=6.8000000000000005e-01; + a39=(a38*a17); + a40=(a39*a04); + a37=(a37+a40); + a34=(a34-a37); + a37=casadi_fabs(a09); + a37=(a19+a37); + a40=(a37*a09); + a34=(a34-a40); + a40=(a33*a34); + a13=(a13+a40); + a40=-6.0150572994295574e-03; + a41=arg[2]? arg[2][2] : 0; + a42=3.3199999999999998e+00; + a43=(a42*a09); + a44=(a43*a17); + a45=-6.8000000000000005e-01; + a46=(a45*a17); + a47=(a46*a09); + a44=(a44+a47); + a41=(a41-a44); + a44=casadi_fabs(a04); + a19=(a19+a44); + a44=(a19*a04); + a41=(a41-a44); + a44=(a40*a41); + a13=(a13+a44); + a00=(a00-a13); + if (res[0]!=0) res[0][0]=a00; + a00=arg[1]? arg[1][1] : 0; + a13=6.0368975005218423e-05; + a44=(a13*a02); + a47=3.3484658136227786e-02; + a48=(a47*a16); + a44=(a44-a48); + a48=-6.1658244361114702e-05; + a49=(a48*a23); + a44=(a44-a49); + a49=5.4404333259807184e-05; + a50=(a49*a29); + a44=(a44-a50); + a50=6.0570157695918683e-03; + a51=(a50*a34); + a44=(a44+a51); + a51=-3.0184487502609172e-03; + a52=(a51*a41); + a44=(a44+a52); + a00=(a00-a44); + if (res[0]!=0) res[0][1]=a00; + a00=arg[1]? arg[1][2] : 0; + a44=-1.1116270017027936e-07; + a52=(a44*a02); + a53=-6.1658244361114783e-05; + a54=(a53*a16); + a52=(a52-a54); + a54=3.3963490377380855e-02; + a55=(a54*a23); + a52=(a52-a55); + a55=-2.9967785627100754e-02; + a56=(a55*a29); + a52=(a52-a56); + a56=-3.0801331505514837e-03; + a57=(a56*a34); + a52=(a52+a57); + a57=5.5581350085139543e-06; + a58=(a57*a41); + a52=(a52+a58); + a00=(a00-a52); + if (res[0]!=0) res[0][2]=a00; + a00=arg[1]? arg[1][3] : 0; + a52=9.8084735444364535e-08; + a58=(a52*a02); + a59=5.4404333259807062e-05; + a60=(a59*a16); + a58=(a58-a60); + a60=-2.9967785627100469e-02; + a61=(a60*a23); + a58=(a58-a61); + a61=1.4970303990827358e+00; + a62=(a61*a29); + a58=(a58-a62); + a62=2.7177645446042520e-03; + a63=(a62*a34); + a58=(a58+a63); + a63=-4.9042367722181902e-06; + a64=(a63*a41); + a58=(a58+a64); + a00=(a00-a58); + if (res[0]!=0) res[0][3]=a00; + a00=arg[1]? arg[1][4] : 0; + a58=1.0920100546139210e-05; + a64=(a58*a02); + a65=6.0570157695918657e-03; + a66=(a65*a16); + a64=(a64-a66); + a66=-3.0801331505514781e-03; + a67=(a66*a23); + a64=(a64-a67); + a67=2.7177645446042498e-03; + a68=(a67*a29); + a64=(a64-a68); + a68=3.0257778596593993e-01; + a69=(a68*a34); + a64=(a64+a69); + a69=-5.4600502730695923e-04; + a70=(a69*a41); + a64=(a64+a70); + a00=(a00-a64); + if (res[0]!=0) res[0][4]=a00; + a00=arg[1]? arg[1][5] : 0; + a64=-6.0150572994295635e-03; + a02=(a64*a02); + a70=-3.0184487502609133e-03; + a16=(a70*a16); + a02=(a02-a16); + a16=5.5581350085139340e-06; + a23=(a16*a23); + a02=(a02-a23); + a23=-4.9042367722181935e-06; + a29=(a23*a29); + a02=(a02-a29); + a34=(a69*a34); + a02=(a02+a34); + a34=3.0075286497147785e-01; + a41=(a34*a41); + a02=(a02+a41); + a00=(a00-a02); + if (res[0]!=0) res[0][5]=a00; + a00=arg[1]? arg[1][6] : 0; + a02=arg[0]? arg[0][6] : 0; + a41=sin(a02); + a29=arg[0]? arg[0][7] : 0; + a71=tan(a29); + a72=(a41*a71); + a73=(a72*a09); + a73=(a17+a73); + a02=cos(a02); + a74=(a02*a71); + a75=(a74*a04); + a73=(a73+a75); + a00=(a00-a73); + if (res[0]!=0) res[0][6]=a00; + a00=arg[1]? arg[1][7] : 0; + a73=(a02*a09); + a75=(a41*a04); + a73=(a73-a75); + a00=(a00-a73); + if (res[0]!=0) res[0][7]=a00; + a00=arg[1]? arg[1][8] : 0; + a73=cos(a29); + a75=(a41/a73); + a76=(a75*a09); + a77=(a02/a73); + a78=(a77*a04); + a76=(a76+a78); + a00=(a00-a76); + if (res[0]!=0) res[0][8]=a00; + a00=casadi_sign(a12); + a00=(a12*a00); + a00=(a00+a07); + a07=(a01*a00); + a76=(a14*a15); + a07=(a07+a76); + a76=(a21*a22); + a07=(a07+a76); + if (res[1]!=0) res[1][0]=a07; + a07=(a13*a00); + a76=(a47*a15); + a07=(a07+a76); + a76=(a48*a22); + a07=(a07+a76); + if (res[1]!=0) res[1][1]=a07; + a07=(a44*a00); + a76=(a53*a15); + a07=(a07+a76); + a76=(a54*a22); + a07=(a07+a76); + if (res[1]!=0) res[1][2]=a07; + a07=(a52*a00); + a76=(a59*a15); + a07=(a07+a76); + a76=(a60*a22); + a07=(a07+a76); + if (res[1]!=0) res[1][3]=a07; + a07=(a58*a00); + a76=(a65*a15); + a07=(a07+a76); + a76=(a66*a22); + a07=(a07+a76); + if (res[1]!=0) res[1][4]=a07; + a00=(a64*a00); + a15=(a70*a15); + a00=(a00+a15); + a22=(a16*a22); + a00=(a00+a22); + if (res[1]!=0) res[1][5]=a00; + a00=(a01*a05); + a22=casadi_sign(a06); + a22=(a06*a22); + a22=(a22+a20); + a20=(a14*a22); + a00=(a00+a20); + a20=(a21*a24); + a00=(a00+a20); + if (res[1]!=0) res[1][6]=a00; + a00=(a13*a05); + a20=(a47*a22); + a00=(a00+a20); + a20=(a48*a24); + a00=(a00+a20); + if (res[1]!=0) res[1][7]=a00; + a00=(a44*a05); + a20=(a53*a22); + a00=(a00+a20); + a20=(a54*a24); + a00=(a00+a20); + if (res[1]!=0) res[1][8]=a00; + a00=(a52*a05); + a20=(a59*a22); + a00=(a00+a20); + a20=(a60*a24); + a00=(a00+a20); + if (res[1]!=0) res[1][9]=a00; + a00=(a58*a05); + a20=(a65*a22); + a00=(a00+a20); + a20=(a66*a24); + a00=(a00+a20); + if (res[1]!=0) res[1][10]=a00; + a05=(a64*a05); + a22=(a70*a22); + a05=(a05+a22); + a24=(a16*a24); + a05=(a05+a24); + if (res[1]!=0) res[1][11]=a05; + a05=(a01*a10); + a24=(a14*a18); + a05=(a05+a24); + a24=casadi_sign(a11); + a24=(a11*a24); + a24=(a24+a25); + a25=(a21*a24); + a05=(a05+a25); + if (res[1]!=0) res[1][12]=a05; + a05=(a13*a10); + a25=(a47*a18); + a05=(a05+a25); + a25=(a48*a24); + a05=(a05+a25); + if (res[1]!=0) res[1][13]=a05; + a05=(a44*a10); + a25=(a53*a18); + a05=(a05+a25); + a25=(a54*a24); + a05=(a05+a25); + if (res[1]!=0) res[1][14]=a05; + a05=(a52*a10); + a25=(a59*a18); + a05=(a05+a25); + a25=(a60*a24); + a05=(a05+a25); + if (res[1]!=0) res[1][15]=a05; + a05=(a58*a10); + a25=(a65*a18); + a05=(a05+a25); + a25=(a66*a24); + a05=(a05+a25); + if (res[1]!=0) res[1][16]=a05; + a10=(a64*a10); + a18=(a70*a18); + a10=(a10+a18); + a24=(a16*a24); + a10=(a10+a24); + if (res[1]!=0) res[1][17]=a10; + a10=(a03*a11); + a24=(a14*a10); + a18=(a08*a06); + a05=(a21*a18); + a24=(a24+a05); + a05=casadi_sign(a17); + a05=(a17*a05); + a05=(a05+a32); + a32=(a26*a05); + a24=(a24+a32); + a38=(a38*a04); + a36=(a36+a38); + a38=(a33*a36); + a24=(a24+a38); + a45=(a45*a09); + a43=(a43+a45); + a45=(a40*a43); + a24=(a24+a45); + if (res[1]!=0) res[1][18]=a24; + a24=(a47*a10); + a45=(a48*a18); + a24=(a24+a45); + a45=(a49*a05); + a24=(a24+a45); + a45=(a50*a36); + a24=(a24+a45); + a45=(a51*a43); + a24=(a24+a45); + if (res[1]!=0) res[1][19]=a24; + a24=(a53*a10); + a45=(a54*a18); + a24=(a24+a45); + a45=(a55*a05); + a24=(a24+a45); + a45=(a56*a36); + a24=(a24+a45); + a45=(a57*a43); + a24=(a24+a45); + if (res[1]!=0) res[1][20]=a24; + a24=(a59*a10); + a45=(a60*a18); + a24=(a24+a45); + a45=(a61*a05); + a24=(a24+a45); + a45=(a62*a36); + a24=(a24+a45); + a45=(a63*a43); + a24=(a24+a45); + if (res[1]!=0) res[1][21]=a24; + a24=(a65*a10); + a45=(a66*a18); + a24=(a24+a45); + a45=(a67*a05); + a24=(a24+a45); + a45=(a68*a36); + a24=(a24+a45); + a45=(a69*a43); + a24=(a24+a45); + if (res[1]!=0) res[1][22]=a24; + a10=(a70*a10); + a18=(a16*a18); + a10=(a10+a18); + a05=(a23*a05); + a10=(a10+a05); + a36=(a69*a36); + a10=(a10+a36); + a43=(a34*a43); + a10=(a10+a43); + if (res[1]!=0) res[1][23]=a10; + a10=-1.; + if (res[1]!=0) res[1][24]=a10; + a11=(a08*a11); + a10=(a01*a11); + a43=(a03*a12); + a21=(a21*a43); + a10=(a10+a21); + a30=(a30*a04); + a28=(a28+a30); + a30=(a26*a28); + a10=(a10+a30); + a30=casadi_sign(a09); + a30=(a09*a30); + a30=(a30+a37); + a37=(a33*a30); + a10=(a10+a37); + a42=(a42*a17); + a42=(a42+a46); + a46=(a40*a42); + a10=(a10+a46); + if (res[1]!=0) res[1][25]=a10; + a10=(a13*a11); + a48=(a48*a43); + a10=(a10+a48); + a48=(a49*a28); + a10=(a10+a48); + a48=(a50*a30); + a10=(a10+a48); + a48=(a51*a42); + a10=(a10+a48); + if (res[1]!=0) res[1][26]=a10; + a10=(a44*a11); + a54=(a54*a43); + a10=(a10+a54); + a54=(a55*a28); + a10=(a10+a54); + a54=(a56*a30); + a10=(a10+a54); + a54=(a57*a42); + a10=(a10+a54); + if (res[1]!=0) res[1][27]=a10; + a10=(a52*a11); + a60=(a60*a43); + a10=(a10+a60); + a60=(a61*a28); + a10=(a10+a60); + a60=(a62*a30); + a10=(a10+a60); + a60=(a63*a42); + a10=(a10+a60); + if (res[1]!=0) res[1][28]=a10; + a10=(a58*a11); + a66=(a66*a43); + a10=(a10+a66); + a66=(a67*a28); + a10=(a10+a66); + a66=(a68*a30); + a10=(a10+a66); + a66=(a69*a42); + a10=(a10+a66); + if (res[1]!=0) res[1][29]=a10; + a11=(a64*a11); + a16=(a16*a43); + a11=(a11+a16); + a28=(a23*a28); + a11=(a11+a28); + a30=(a69*a30); + a11=(a11+a30); + a42=(a34*a42); + a11=(a11+a42); + if (res[1]!=0) res[1][30]=a11; + a72=(-a72); + if (res[1]!=0) res[1][31]=a72; + a72=(-a02); + if (res[1]!=0) res[1][32]=a72; + a72=(-a75); + if (res[1]!=0) res[1][33]=a72; + a03=(a03*a06); + a01=(a01*a03); + a08=(a08*a12); + a14=(a14*a08); + a01=(a01+a14); + a27=(a27*a09); + a27=(a27+a31); + a26=(a26*a27); + a01=(a01+a26); + a35=(a35*a17); + a35=(a35+a39); + a33=(a33*a35); + a01=(a01+a33); + a33=casadi_sign(a04); + a33=(a04*a33); + a33=(a33+a19); + a40=(a40*a33); + a01=(a01+a40); + if (res[1]!=0) res[1][34]=a01; + a13=(a13*a03); + a47=(a47*a08); + a13=(a13+a47); + a49=(a49*a27); + a13=(a13+a49); + a50=(a50*a35); + a13=(a13+a50); + a51=(a51*a33); + a13=(a13+a51); + if (res[1]!=0) res[1][35]=a13; + a44=(a44*a03); + a53=(a53*a08); + a44=(a44+a53); + a55=(a55*a27); + a44=(a44+a55); + a56=(a56*a35); + a44=(a44+a56); + a57=(a57*a33); + a44=(a44+a57); + if (res[1]!=0) res[1][36]=a44; + a52=(a52*a03); + a59=(a59*a08); + a52=(a52+a59); + a61=(a61*a27); + a52=(a52+a61); + a62=(a62*a35); + a52=(a52+a62); + a63=(a63*a33); + a52=(a52+a63); + if (res[1]!=0) res[1][37]=a52; + a58=(a58*a03); + a65=(a65*a08); + a58=(a58+a65); + a67=(a67*a27); + a58=(a58+a67); + a68=(a68*a35); + a58=(a58+a68); + a68=(a69*a33); + a58=(a58+a68); + if (res[1]!=0) res[1][38]=a58; + a64=(a64*a03); + a70=(a70*a08); + a64=(a64+a70); + a23=(a23*a27); + a64=(a64+a23); + a69=(a69*a35); + a64=(a64+a69); + a34=(a34*a33); + a64=(a64+a34); + if (res[1]!=0) res[1][39]=a64; + a74=(-a74); + if (res[1]!=0) res[1][40]=a74; + if (res[1]!=0) res[1][41]=a41; + a74=(-a77); + if (res[1]!=0) res[1][42]=a74; + a74=(a71*a02); + a74=(a09*a74); + a71=(a71*a41); + a71=(a04*a71); + a74=(a74-a71); + a74=(-a74); + if (res[1]!=0) res[1][43]=a74; + a74=(a09*a41); + a71=(a04*a02); + a74=(a74+a71); + if (res[1]!=0) res[1][44]=a74; + a74=(a09*a77); + a71=(a04*a75); + a74=(a74-a71); + a74=(-a74); + if (res[1]!=0) res[1][45]=a74; + a74=casadi_sq(a73); + a41=(a41/a74); + a41=(a09*a41); + a02=(a02/a74); + a02=(a04*a02); + a41=(a41+a02); + a41=(-a41); + if (res[1]!=0) res[1][46]=a41; + a75=(a75/a73); + a29=sin(a29); + a75=(a75*a29); + a09=(a09*a75); + a77=(a77/a73); + a77=(a77*a29); + a04=(a04*a77); + a09=(a09+a04); + a09=(-a09); + if (res[1]!=0) res[1][47]=a09; + a09=1.; + if (res[2]!=0) res[2][0]=a09; + if (res[2]!=0) res[2][1]=a09; + if (res[2]!=0) res[2][2]=a09; + if (res[2]!=0) res[2][3]=a09; + if (res[2]!=0) res[2][4]=a09; + if (res[2]!=0) res[2][5]=a09; + if (res[2]!=0) res[2][6]=a09; + if (res[2]!=0) res[2][7]=a09; + if (res[2]!=0) res[2][8]=a09; + a09=-3.3453634479321918e-02; + if (res[3]!=0) res[3][0]=a09; + a09=-6.0368975005218423e-05; + if (res[3]!=0) res[3][1]=a09; + a09=1.1116270017027936e-07; + if (res[3]!=0) res[3][2]=a09; + a09=-9.8084735444364535e-08; + if (res[3]!=0) res[3][3]=a09; + a09=-1.0920100546139210e-05; + if (res[3]!=0) res[3][4]=a09; + a09=6.0150572994295635e-03; + if (res[3]!=0) res[3][5]=a09; + a09=-1.0920100546139184e-05; + if (res[3]!=0) res[3][6]=a09; + a09=-6.0570157695918683e-03; + if (res[3]!=0) res[3][7]=a09; + a09=3.0801331505514837e-03; + if (res[3]!=0) res[3][8]=a09; + a09=-2.7177645446042520e-03; + if (res[3]!=0) res[3][9]=a09; + a09=-3.0257778596593993e-01; + if (res[3]!=0) res[3][10]=a09; + a09=5.4600502730695923e-04; + if (res[3]!=0) res[3][11]=a09; + a04=6.0150572994295574e-03; + if (res[3]!=0) res[3][12]=a04; + a04=3.0184487502609172e-03; + if (res[3]!=0) res[3][13]=a04; + a04=-5.5581350085139543e-06; + if (res[3]!=0) res[3][14]=a04; + a04=4.9042367722181902e-06; + if (res[3]!=0) res[3][15]=a04; + if (res[3]!=0) res[3][16]=a09; + a09=-3.0075286497147785e-01; + if (res[3]!=0) res[3][17]=a09; + return 0; +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_u(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ + return casadi_f0(arg, res, iw, w, mem); +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_u_alloc_mem(void) { + return 0; +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_u_init_mem(int mem) { + return 0; +} + +CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_jac_x_xdot_u_free_mem(int mem) { +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_u_checkout(void) { + return 0; +} + +CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_jac_x_xdot_u_release(int mem) { +} + +CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_jac_x_xdot_u_incref(void) { +} + +CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_jac_x_xdot_u_decref(void) { +} + +CASADI_SYMBOL_EXPORT casadi_int auv_model_impl_dae_fun_jac_x_xdot_u_n_in(void) { return 6;} + +CASADI_SYMBOL_EXPORT casadi_int auv_model_impl_dae_fun_jac_x_xdot_u_n_out(void) { return 4;} + +CASADI_SYMBOL_EXPORT casadi_real auv_model_impl_dae_fun_jac_x_xdot_u_default_in(casadi_int i) { + switch (i) { + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const char* auv_model_impl_dae_fun_jac_x_xdot_u_name_in(casadi_int i) { + switch (i) { + case 0: return "i0"; + case 1: return "i1"; + case 2: return "i2"; + case 3: return "i3"; + case 4: return "i4"; + case 5: return "i5"; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const char* auv_model_impl_dae_fun_jac_x_xdot_u_name_out(casadi_int i) { + switch (i) { + case 0: return "o0"; + case 1: return "o1"; + case 2: return "o2"; + case 3: return "o3"; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const casadi_int* auv_model_impl_dae_fun_jac_x_xdot_u_sparsity_in(casadi_int i) { + switch (i) { + case 0: return casadi_s0; + case 1: return casadi_s0; + case 2: return casadi_s1; + case 3: return casadi_s2; + case 4: return casadi_s3; + case 5: return casadi_s2; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const casadi_int* auv_model_impl_dae_fun_jac_x_xdot_u_sparsity_out(casadi_int i) { + switch (i) { + case 0: return casadi_s0; + case 1: return casadi_s4; + case 2: return casadi_s5; + case 3: return casadi_s6; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_u_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { + if (sz_arg) *sz_arg = 6; + if (sz_res) *sz_res = 4; + if (sz_iw) *sz_iw = 0; + if (sz_w) *sz_w = 0; + return 0; +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_u_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { + if (sz_arg) *sz_arg = 6*sizeof(const casadi_real*); + if (sz_res) *sz_res = 4*sizeof(casadi_real*); + if (sz_iw) *sz_iw = 0*sizeof(casadi_int); + if (sz_w) *sz_w = 0*sizeof(casadi_real); + return 0; +} + + +#ifdef __cplusplus +} /* extern "C" */ +#endif diff --git a/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u.o b/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u.o new file mode 100644 index 0000000000000000000000000000000000000000..4f9555c24d94168607aa270aeeee8e89f9bc61ee GIT binary patch literal 21328 zcmcJX3w+eomB;_c!${fuCl)PjT^)76U?ol-KxmarAdzW}HfoTW8caeS5``p^L1U#B zOK4TJ#_FeKw{D9IwY1AtyLG#)bxGtIK`{cpz#>|TP+tLjRnhG4oO^EOLFk`DUr-^<0{aM>rbsPWH0AqyFh1Hux38 z32pGtkoUKrXz;Up8vOISZ#?~ozkchGBYuEIAo_TNKXYq?f92y4k9al#FN;YB={w@b zyYzeUtl}$6bKfhTSzIwQdi%#$L~lPDz4eVYm>FF*BO|)BD{&Ik1RK8U-*Oq6lper)Rzzw$8T zpKkDHJ&9{ygFpI^KYmSUAM61?M4G8q?;_!Cyd1+tLLcCIg#^PW!yo@xs23Uh@vA~X zyps$JDG6mmYCq8~q#nctb7R~JbKg)FuFNT_McAkXz7Cd~`=kP1*cLQflOpqvf<+fP`> zu$CUtG91!T%2L8zqAV2QJY~zyK2qWbCPKt)+3<^YJbVn{eJrwKFe-c1plLd z{So=^V?V=(ce1ZQ5}_b!_&n*Uc2|X*hb303pdmvv`|)=r)!P{nT*;^G}3k zr%w=_7mC^p6(Nj@007MF!L}HyC7X#RAoB`vXG8+q!rz8|R z;$M9b*FG+!8uz+T6dKoXW<);5^(^q3P&ur|RD#J8>lRf3DX;Nlb_dFVT3!@+MhX~X zU>&l?QI{CaPJSHoTx>nO){U$i$#Z42#9_LDRq?0EojeL=lK04&@J(em1WLHJn@kRJ zSXl_BQjS(iZ3;vopUw4-E}Kp(zu&%rEsXPWm(AS|7CdH`&{L4=L6=$Ts9LBe zt~l1)(Or1<;Nt$%fb|&Z_wi~8$Tq;qp4|!ffOFUc&_5r1@*$_EB>OXA8i?^ zj;GL*#}I6$X?f%=<(Hb!lAjcc?Vzy@huvA%4_J|C5sr%~`8E23kYghre0p^1p-56~{$q)@m6)PAdd>7KkpBaYnnS z_~Gd79bH_i>k+$Fx!%Tx4c3&{oWk4-3Jz-4N>6 zz|}^9?V%p1r2Q6kpal1AO#9fxy+PTZq8k~vC^Q!;4&pM?L5V+lJ#vl4gcFxZW%Qc> z@h%KttSE8zGjdy(RVG4h7+M>k!OFjfyfhIh7XQ8TzE|E~8c-Oyc`$%F;p=I-7)BiZH6h-3UHpx zleE6fY7Z?2MA`=?4eK2$Fbg}8ti^JIG{=jIPfZ^@W&9jd2DzS3jAr^l0|)6ma6t%~ zvgwR!PBvoktUpuYJRB?V;NDF6w-{Lk;{v9>InI$}vb`0hkQMjaypWg7t_Y)R!a$p19uKSt6|lZ%egD^_kALD|nkyayTEc^il+h!xY(3*ws~ z35gPa)+;cx2jW{nMn8#)R-1}q*C{j6b=aL@x4g>qdb=$}*NtFHiW08w=DjScPHP=L zssvDK1#JeeuAr?e2v%B}?pPt$r5cHKlt@z?O2<6O{hXHNAT+&%i;Oo5GhE5K`7n#K zmgzjGAD#Be$CzzqVS!)T0^6}e3DB2=anWVc?xrI}Z;z4(yZkJS;YfgXYPiA>A4B3z zu!8#((v#6hK*fV99#(OvwOJ$ODqgMPEy*}=VWbaDbkG{`p3Wh9?_Vg3+VnV< z)juu#r83cL`pY!44%gF87rK7O@o-6BUNDw0m*EtIspUnqMRqsd9tXzg!L18Z5ij3( z$gvV1D(UMJjg+OU5BFiUG#{(*u@f!ND^_$1Rt%a@qg%|$%fw4U5sKKOAVx|Z%_X?U z4vjg|*Vo{m(M|&Ro@%~1qMRezg$-C+Q*ZbNLs2LRrKajq)!v?-n+u3nW z=}Y>C-`E_zi`U`X`=YhO9*qv^ncBCHzNbg;;!Y27)BeZn9X{!|PjoKnJ0bCvZ;z9= zrG1HShM>bF$Y@oP(r?%4X zH8nErmLK$t4-{P_&t+dYclC(xjw-s|q_3NFbKjwgif>N+-MxEO{G#@OqOslE zN9}v)Cq-j_TC(-Rr`w7?EQ-w>`{>-F|9R!5@IwauoCl|_Z0jr=w{iciUui5_ z{BnD{SN{A|(cfwHJNY}so^@hRr&FHQ*J<_H`dE3J-_~!K*!?e3pI?c6*<$zEV&~8Z z+utemooefQqp4@-ju&@~eeUS{MR8M~&O_mCk9JINC|WM{`rPuTuYC2MUF)R2ol^gP ze^`6>OTI6Pe16vU$<4EO7L|9Lx3_SAai@K@ZrXnMwxXT3y*i~nu6^so-|K97QonUl zzfQ5YNbK#D@@zRpQvV{Uzpc+Ysn1lgr%3GSG##v0o>RNb`_zbePL}74$lBAFzJA%- zk&(OCznfio_sFgd^jQDLB6)TSw{l(HjOo+A9L%0MXJJ!fVK6s8CpRZNp>QF;=H8N* zmlG}+FPMv>`7MBp=C~P`&ZK#-iX{*8`2gS^2JdnW^`a+QJ&KzjZYSc z6SH&86iFjiB5^y-JBJr%hQB+aB(reI ziC@cnc<9jgP9B+AI6X65oS8i>GYHvfnSp7U8PU%mJC(6o^wFG3f!JdlGxMxvJ;Lmq zPICJPk)zKV6tvtZnQSx*4#EnAjWjCQ7TVKM(5zCJ^$XcThc*f~n+NA@7e;=gKyu54 zSw9k6Gl*Q5FuR~qd+u@=P|84}!@R?Ng9!}f8TQjDLp{41B-748bO?BmHxg#OrPND> z^MZ>wt-}-+;v)H8vDNbXg$Eox8-%aGlR|}gcz?o^^jK4Sg=ax8@rb-v%d?Qn%kT>2 z-99c99=1T9(}V{dJ|hJ$6JGAfU!H=~*@RZWh$DY>3SKAN*3)LYHU&5R4f;9VN_*Z7 zR=>j+2`_i}cT(`%gjYNA-%r8s6K?ypwdkLP^O?~?JXZ*}!CyUz`->*t!&aqwpR_erpQeBfP_re=Y@oN%&%ZCNsU7f*-Vc9Qn6W@OOk8 zzb5s6n1Y*eo8jG-6y?|m9-kRro5M#5Z*lm^!ZRE@#|ZZve!6hezmm#A!k0Vp=L%o# zaQf+r#@EA16&#zCf?ptfg(E*L1&<03IOSfNf>#JP{UTY&H&XCw;rkr@4Jr6M;k}Oh zf)u<>_}Zi?9J@vML5H^s-{MP3myIe^9--r;h%_n#NoquBbng^9lwnf9(H(u`AJ^X(Q~TEcRBK>iF~;u zf0oFb`6-#0E!^%OY)X#sAlJiE&npma=7}W#vhau_9}!;e@QZ|3JG@MIi^DG$zS!Y0 z;ckDJ&3t(OzTH|{E8M=@XQOc60(rIwcXx0%2zPt#BH?y_X4Neb?)J|+gxhZ|R{kF0 z{PE91JeLdqqGA;NDfr21QLtx_ZsFEH_8f9pxa}vl9%FFWAwAWipU)$RUjS}(ND@D$ zr{F~?_>2_%VsI+Aht59;_S|xl@O=({PJ#=s({Q==t{ygD9htCoo zcKCOMM}*t+%fAUPclfiytA$%X4?~9{ds>9s^G#TIuW)Wms z&gv0v&pWRP57UP+1k1geg*Q~qZ?3DKSJgPbWnNWXO?_3v!ltTgYig_7s@m$B6IBay zYSWRbY7M6+mY1GbetKfL>4{BDPplw4u}NM|+4MZRO{N>aPNY~a-zVmB+~6?Bjhx|y zoQLnZQ#j7|JeHiy_dJ%&<0A6%NhYs=@2o%1l+QXR8qV4#8a;fU$ddU+C+o>)os(Hx zzA>Kj<{QHCi6+i@Cz)JaRz8=N&!yxW{Y(m2|3sE4VEvO=LxIt6WSC6myaim!WY$o? zr4(@9Nvx;9)W}GhJ4+TCZLGhLwG|pYtf7$atiVW+qc~r#flPB*M=s~iHFae&iQ|)u z6aY01JR3+fxv60NIGTePVvP5(Ak zrCX`G`i7c?^Ac$*u9;u|SIMSpM{7&Xg4V{wP3bD{mru8V8=DtgmsWd_VAY(P67{WE zAMN>rww?5C?wI^=Lcb?bAWq+0EiWO6K>X{tEZ;;BfjI5CEPs_C0`bdmS#Hk(#PPS( z{rnfQ-^Vt`TcG%_g_Hc{ihrH}f-8TfaC%&U%a%Jah5Us|p1vho`BKHTo+}iespQT1 z`P23+R`OcUD#fo-2_jH(7A{-g>lLT7g5|d={#nKE zQJnVjR{jCSX{%@XuN9{)n&n#+r{5VYe@SsV6IlL+;`9fAmLF66bj3$upGtx1dj>8m zf12V!#Y+@FTXEVqQy@KO;<9?KQGBf8H!4ow#I5}I6d$knj})gdXyt#cIQ`CR`9{Sj zD85Vau;Tj_&r|#z#q$-XeLMxKZvie_Zb0!FijPx#vf@(|pQw1T;?oqrMDb$9w)e z|GIFuzSk;wUEg-ab$$O`ac$3kE3WJNgyOosFDf2VcD|yx_QRWsPgn9I@9z}PRpo9`{L6}urcDEa zTi=Po$v=7>xlnPv9$l>XmB?f3yHN3%;@?wTuSY*p{BuhF=Zfp~=)V-#>&rI9^*Xgj zalP)muDD)zK2kiW?D>p*SR?<8Rs0Oa^?G!^;^!#&BE`om{&mH_qT(3Jz z6xZv>y^8B~WToPI9r>N&dOqKv_+(}0HpTTkyhCxlFMU&SJ^%k%@r#uH;qqaT{IBQ# zGZmkqUTP4QyIOBBCNxH~S^D|x-2Jf?VD>B*!`GXmN94P16yoUQm(il3)=rQ%Z+ zKUML|6{k;ZR{tEuzo7Wdihot{A1JQpjh`rk~H_yvmpR`GKcU$6L7#kVVdp5nU| zk0}0@;$@1TLYrU&@-sDs^}`jy-FCcF$y3zIuTWgiZ@*Do&nJ&5K0)c}R$QM`{-F5R zl>9r2U!wRBT5J%gUV2|WTJhOPv-W2zuJ>;<6pt$TS&Cn*c&*|&iZ?5+=k1#n*Ym&+ z6xZ|ZLyGHuyIyhKZ}%v!=g-#_*Yn#)itB!ee>lN`{HgofIK}n+lcTuqx04i)K$rD@ zRB=6DolKhp1nP$gTvonT@tYK1Bi!xJI~Av;$?DmYf)Ay|8-es(jmyesDqf{H{cjQ! zNWL1El^>^gjpBKV&ry7;;+$;&#r65_mx@nQ<^EQ2y-(Pzc!`qVp|~D*uPUy`5&e%k6v)qd-9LdQBLw1l zzj2x3dc0IBPFoD?|5b|9Qe*kUieIDn9>w*#^N!->N`53Q-UwvpWw@+<`}-zwt-n>t zYyFF8bBsXpLvdL>_bNVD@s)}tTCKhpX^!Sefr%V&Uo-YuN^HU%sHl5klb`?>v{*ZR-O zZxnekvtPbi0PTe+Sou2PL8eK*TzJUgi-gnOl7iLKA)MA|3YNF0&~K#7w=XSk>q&ba z&1plLu=BK^+tpzx5x!@>(4 z9uaQ-o;B&Wa^cmEe6{d|!&`*gzvHv^^a{V7)M2Glu1%9>x39d1CY}G^{JF?WI%%(?gDs!V3_95I?Xwh#6sP{9!#=V9 zJSQQjg%FS68q9y$7aPH#6h!``Hgf%U2^fjU_8T!10x~gnIOW^+v(Kv_K3M)0Qt_Bo zC{J6yea@BqVMp!*$#3URn=IAgGKtYzYMGUxXNW$64H8`0TOo5IE~lDaw&V{wI;`Cm z{}Upl)6E~1{0E%^-Te0 + +#ifndef casadi_real +#define casadi_real double +#endif + +#ifndef casadi_int +#define casadi_int int +#endif + +/* Add prefix to internal symbols */ +#define casadi_f0 CASADI_PREFIX(f0) +#define casadi_fabs CASADI_PREFIX(fabs) +#define casadi_s0 CASADI_PREFIX(s0) +#define casadi_s1 CASADI_PREFIX(s1) +#define casadi_s2 CASADI_PREFIX(s2) +#define casadi_s3 CASADI_PREFIX(s3) +#define casadi_s4 CASADI_PREFIX(s4) +#define casadi_s5 CASADI_PREFIX(s5) +#define casadi_s6 CASADI_PREFIX(s6) +#define casadi_s7 CASADI_PREFIX(s7) +#define casadi_sign CASADI_PREFIX(sign) +#define casadi_sq CASADI_PREFIX(sq) + +/* Symbol visibility in DLLs */ +#ifndef CASADI_SYMBOL_EXPORT + #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) + #if defined(STATIC_LINKED) + #define CASADI_SYMBOL_EXPORT + #else + #define CASADI_SYMBOL_EXPORT __declspec(dllexport) + #endif + #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) + #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default"))) + #else + #define CASADI_SYMBOL_EXPORT + #endif +#endif + +casadi_real casadi_fabs(casadi_real x) { +/* Pre-c99 compatibility */ +#if __STDC_VERSION__ < 199901L + return x>0 ? x : -x; +#else + return fabs(x); +#endif +} + +casadi_real casadi_sign(casadi_real x) { return x<0 ? -1 : x>0 ? 1 : x;} + +casadi_real casadi_sq(casadi_real x) { return x*x;} + +static const casadi_int casadi_s0[3] = {9, 1, 1}; +static const casadi_int casadi_s1[3] = {3, 1, 1}; +static const casadi_int casadi_s2[3] = {0, 1, 1}; +static const casadi_int casadi_s3[3] = {0, 0, 1}; +static const casadi_int casadi_s4[60] = + {9, 9, 0, 6, 12, 18, 25, 34, + 43, 46, 48, 48, 0, 1, 2, 3, + 4, 5, 0, 1, 2, 3, 4, 5, + 0, 1, 2, 3, 4, 5, 0, 1, + 2, 3, 4, 5, 6, 0, 1, 2, + 3, 4, 5, 6, 7, 8, 0, 1, + 2, 3, 4, 5, 6, 7, 8, 6, + 7, 8, 6, 8}; +static const casadi_int casadi_s5[21] = + {9, 9, 0, 1, 2, 3, 4, 5, + 6, 7, 8, 9, 0, 1, 2, 3, + 4, 5, 6, 7, 8}; +static const casadi_int casadi_s6[24] = + {9, 3, 0, 6, 12, 18, 0, 1, + 2, 3, 4, 5, 0, 1, 2, 3, + 4, 5, 0, 1, 2, 3, 4, 5}; +static const casadi_int casadi_s7[3] = {9, 0, 1}; + +/* auv_model_impl_dae_fun_jac_x_xdot_u_z:(i0[9],i1[9],i2[3],i3[0],i4[],i5[0])->(o0[9],o1[9x9,48nz],o2[9x9,9nz],o3[9x3,18nz],o4[9x0]) */ +static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { + casadi_real a00, a01, a02, a03, a04, a05, a06, a07, a08, a09, a10, a11; + casadi_real a12, a13, a14, a15, a16, a17, a18, a19, a20, a21, a22, a23; + casadi_real a24, a25, a26, a27, a28, a29, a30, a31, a32, a33, a34, a35; + casadi_real a36, a37, a38, a39, a40, a41, a42, a43, a44, a45, a46, a47; + casadi_real a48, a49, a50, a51, a52, a53, a54, a55, a56, a57, a58, a59; + casadi_real a60, a61, a62, a63, a64, a65, a66, a67, a68, a69, a70, a71; + casadi_real a72, a73, a74, a75, a76, a77, a78; + a00=arg[1]? arg[1][0] : 0; + a01=3.3453634479321918e-02; + a02=arg[2]? arg[2][0] : 0; + a03=-30.; + a04=arg[0]? arg[0][5] : 0; + a05=(a03*a04); + a06=arg[0]? arg[0][1] : 0; + a07=(a05*a06); + a08=30.; + a09=arg[0]? arg[0][4] : 0; + a10=(a08*a09); + a11=arg[0]? arg[0][2] : 0; + a12=(a10*a11); + a07=(a07+a12); + a02=(a02-a07); + a07=23.; + a12=arg[0]? arg[0][0] : 0; + a13=casadi_fabs(a12); + a07=(a07+a13); + a13=(a07*a12); + a02=(a02-a13); + a13=(a01*a02); + a14=6.0368975005218254e-05; + a15=(a08*a04); + a16=(a15*a12); + a17=arg[0]? arg[0][3] : 0; + a18=(a03*a17); + a19=(a18*a11); + a16=(a16+a19); + a19=46.; + a20=casadi_fabs(a06); + a20=(a19+a20); + a21=(a20*a06); + a16=(a16+a21); + a21=(a14*a16); + a13=(a13-a21); + a21=-1.1116270017027866e-07; + a22=(a03*a09); + a23=(a22*a12); + a24=(a08*a17); + a25=(a24*a06); + a23=(a23+a25); + a25=casadi_fabs(a11); + a25=(a19+a25); + a26=(a25*a11); + a23=(a23+a26); + a26=(a21*a23); + a13=(a13-a26); + a26=9.8084735444363873e-08; + a27=3.3399999999999999e+00; + a28=(a27*a04); + a29=(a28*a09); + a30=-3.3199999999999998e+00; + a31=(a30*a09); + a32=(a31*a04); + a29=(a29+a32); + a32=casadi_fabs(a17); + a32=(a19+a32); + a33=(a32*a17); + a29=(a29+a33); + a33=(a26*a29); + a13=(a13-a33); + a33=1.0920100546139184e-05; + a34=arg[2]? arg[2][1] : 0; + a35=-3.3399999999999999e+00; + a36=(a35*a04); + a37=(a36*a17); + a38=6.8000000000000005e-01; + a39=(a38*a17); + a40=(a39*a04); + a37=(a37+a40); + a34=(a34-a37); + a37=casadi_fabs(a09); + a37=(a19+a37); + a40=(a37*a09); + a34=(a34-a40); + a40=(a33*a34); + a13=(a13+a40); + a40=-6.0150572994295574e-03; + a41=arg[2]? arg[2][2] : 0; + a42=3.3199999999999998e+00; + a43=(a42*a09); + a44=(a43*a17); + a45=-6.8000000000000005e-01; + a46=(a45*a17); + a47=(a46*a09); + a44=(a44+a47); + a41=(a41-a44); + a44=casadi_fabs(a04); + a19=(a19+a44); + a44=(a19*a04); + a41=(a41-a44); + a44=(a40*a41); + a13=(a13+a44); + a00=(a00-a13); + if (res[0]!=0) res[0][0]=a00; + a00=arg[1]? arg[1][1] : 0; + a13=6.0368975005218423e-05; + a44=(a13*a02); + a47=3.3484658136227786e-02; + a48=(a47*a16); + a44=(a44-a48); + a48=-6.1658244361114702e-05; + a49=(a48*a23); + a44=(a44-a49); + a49=5.4404333259807184e-05; + a50=(a49*a29); + a44=(a44-a50); + a50=6.0570157695918683e-03; + a51=(a50*a34); + a44=(a44+a51); + a51=-3.0184487502609172e-03; + a52=(a51*a41); + a44=(a44+a52); + a00=(a00-a44); + if (res[0]!=0) res[0][1]=a00; + a00=arg[1]? arg[1][2] : 0; + a44=-1.1116270017027936e-07; + a52=(a44*a02); + a53=-6.1658244361114783e-05; + a54=(a53*a16); + a52=(a52-a54); + a54=3.3963490377380855e-02; + a55=(a54*a23); + a52=(a52-a55); + a55=-2.9967785627100754e-02; + a56=(a55*a29); + a52=(a52-a56); + a56=-3.0801331505514837e-03; + a57=(a56*a34); + a52=(a52+a57); + a57=5.5581350085139543e-06; + a58=(a57*a41); + a52=(a52+a58); + a00=(a00-a52); + if (res[0]!=0) res[0][2]=a00; + a00=arg[1]? arg[1][3] : 0; + a52=9.8084735444364535e-08; + a58=(a52*a02); + a59=5.4404333259807062e-05; + a60=(a59*a16); + a58=(a58-a60); + a60=-2.9967785627100469e-02; + a61=(a60*a23); + a58=(a58-a61); + a61=1.4970303990827358e+00; + a62=(a61*a29); + a58=(a58-a62); + a62=2.7177645446042520e-03; + a63=(a62*a34); + a58=(a58+a63); + a63=-4.9042367722181902e-06; + a64=(a63*a41); + a58=(a58+a64); + a00=(a00-a58); + if (res[0]!=0) res[0][3]=a00; + a00=arg[1]? arg[1][4] : 0; + a58=1.0920100546139210e-05; + a64=(a58*a02); + a65=6.0570157695918657e-03; + a66=(a65*a16); + a64=(a64-a66); + a66=-3.0801331505514781e-03; + a67=(a66*a23); + a64=(a64-a67); + a67=2.7177645446042498e-03; + a68=(a67*a29); + a64=(a64-a68); + a68=3.0257778596593993e-01; + a69=(a68*a34); + a64=(a64+a69); + a69=-5.4600502730695923e-04; + a70=(a69*a41); + a64=(a64+a70); + a00=(a00-a64); + if (res[0]!=0) res[0][4]=a00; + a00=arg[1]? arg[1][5] : 0; + a64=-6.0150572994295635e-03; + a02=(a64*a02); + a70=-3.0184487502609133e-03; + a16=(a70*a16); + a02=(a02-a16); + a16=5.5581350085139340e-06; + a23=(a16*a23); + a02=(a02-a23); + a23=-4.9042367722181935e-06; + a29=(a23*a29); + a02=(a02-a29); + a34=(a69*a34); + a02=(a02+a34); + a34=3.0075286497147785e-01; + a41=(a34*a41); + a02=(a02+a41); + a00=(a00-a02); + if (res[0]!=0) res[0][5]=a00; + a00=arg[1]? arg[1][6] : 0; + a02=arg[0]? arg[0][6] : 0; + a41=sin(a02); + a29=arg[0]? arg[0][7] : 0; + a71=tan(a29); + a72=(a41*a71); + a73=(a72*a09); + a73=(a17+a73); + a02=cos(a02); + a74=(a02*a71); + a75=(a74*a04); + a73=(a73+a75); + a00=(a00-a73); + if (res[0]!=0) res[0][6]=a00; + a00=arg[1]? arg[1][7] : 0; + a73=(a02*a09); + a75=(a41*a04); + a73=(a73-a75); + a00=(a00-a73); + if (res[0]!=0) res[0][7]=a00; + a00=arg[1]? arg[1][8] : 0; + a73=cos(a29); + a75=(a41/a73); + a76=(a75*a09); + a77=(a02/a73); + a78=(a77*a04); + a76=(a76+a78); + a00=(a00-a76); + if (res[0]!=0) res[0][8]=a00; + a00=casadi_sign(a12); + a00=(a12*a00); + a00=(a00+a07); + a07=(a01*a00); + a76=(a14*a15); + a07=(a07+a76); + a76=(a21*a22); + a07=(a07+a76); + if (res[1]!=0) res[1][0]=a07; + a07=(a13*a00); + a76=(a47*a15); + a07=(a07+a76); + a76=(a48*a22); + a07=(a07+a76); + if (res[1]!=0) res[1][1]=a07; + a07=(a44*a00); + a76=(a53*a15); + a07=(a07+a76); + a76=(a54*a22); + a07=(a07+a76); + if (res[1]!=0) res[1][2]=a07; + a07=(a52*a00); + a76=(a59*a15); + a07=(a07+a76); + a76=(a60*a22); + a07=(a07+a76); + if (res[1]!=0) res[1][3]=a07; + a07=(a58*a00); + a76=(a65*a15); + a07=(a07+a76); + a76=(a66*a22); + a07=(a07+a76); + if (res[1]!=0) res[1][4]=a07; + a00=(a64*a00); + a15=(a70*a15); + a00=(a00+a15); + a22=(a16*a22); + a00=(a00+a22); + if (res[1]!=0) res[1][5]=a00; + a00=(a01*a05); + a22=casadi_sign(a06); + a22=(a06*a22); + a22=(a22+a20); + a20=(a14*a22); + a00=(a00+a20); + a20=(a21*a24); + a00=(a00+a20); + if (res[1]!=0) res[1][6]=a00; + a00=(a13*a05); + a20=(a47*a22); + a00=(a00+a20); + a20=(a48*a24); + a00=(a00+a20); + if (res[1]!=0) res[1][7]=a00; + a00=(a44*a05); + a20=(a53*a22); + a00=(a00+a20); + a20=(a54*a24); + a00=(a00+a20); + if (res[1]!=0) res[1][8]=a00; + a00=(a52*a05); + a20=(a59*a22); + a00=(a00+a20); + a20=(a60*a24); + a00=(a00+a20); + if (res[1]!=0) res[1][9]=a00; + a00=(a58*a05); + a20=(a65*a22); + a00=(a00+a20); + a20=(a66*a24); + a00=(a00+a20); + if (res[1]!=0) res[1][10]=a00; + a05=(a64*a05); + a22=(a70*a22); + a05=(a05+a22); + a24=(a16*a24); + a05=(a05+a24); + if (res[1]!=0) res[1][11]=a05; + a05=(a01*a10); + a24=(a14*a18); + a05=(a05+a24); + a24=casadi_sign(a11); + a24=(a11*a24); + a24=(a24+a25); + a25=(a21*a24); + a05=(a05+a25); + if (res[1]!=0) res[1][12]=a05; + a05=(a13*a10); + a25=(a47*a18); + a05=(a05+a25); + a25=(a48*a24); + a05=(a05+a25); + if (res[1]!=0) res[1][13]=a05; + a05=(a44*a10); + a25=(a53*a18); + a05=(a05+a25); + a25=(a54*a24); + a05=(a05+a25); + if (res[1]!=0) res[1][14]=a05; + a05=(a52*a10); + a25=(a59*a18); + a05=(a05+a25); + a25=(a60*a24); + a05=(a05+a25); + if (res[1]!=0) res[1][15]=a05; + a05=(a58*a10); + a25=(a65*a18); + a05=(a05+a25); + a25=(a66*a24); + a05=(a05+a25); + if (res[1]!=0) res[1][16]=a05; + a10=(a64*a10); + a18=(a70*a18); + a10=(a10+a18); + a24=(a16*a24); + a10=(a10+a24); + if (res[1]!=0) res[1][17]=a10; + a10=(a03*a11); + a24=(a14*a10); + a18=(a08*a06); + a05=(a21*a18); + a24=(a24+a05); + a05=casadi_sign(a17); + a05=(a17*a05); + a05=(a05+a32); + a32=(a26*a05); + a24=(a24+a32); + a38=(a38*a04); + a36=(a36+a38); + a38=(a33*a36); + a24=(a24+a38); + a45=(a45*a09); + a43=(a43+a45); + a45=(a40*a43); + a24=(a24+a45); + if (res[1]!=0) res[1][18]=a24; + a24=(a47*a10); + a45=(a48*a18); + a24=(a24+a45); + a45=(a49*a05); + a24=(a24+a45); + a45=(a50*a36); + a24=(a24+a45); + a45=(a51*a43); + a24=(a24+a45); + if (res[1]!=0) res[1][19]=a24; + a24=(a53*a10); + a45=(a54*a18); + a24=(a24+a45); + a45=(a55*a05); + a24=(a24+a45); + a45=(a56*a36); + a24=(a24+a45); + a45=(a57*a43); + a24=(a24+a45); + if (res[1]!=0) res[1][20]=a24; + a24=(a59*a10); + a45=(a60*a18); + a24=(a24+a45); + a45=(a61*a05); + a24=(a24+a45); + a45=(a62*a36); + a24=(a24+a45); + a45=(a63*a43); + a24=(a24+a45); + if (res[1]!=0) res[1][21]=a24; + a24=(a65*a10); + a45=(a66*a18); + a24=(a24+a45); + a45=(a67*a05); + a24=(a24+a45); + a45=(a68*a36); + a24=(a24+a45); + a45=(a69*a43); + a24=(a24+a45); + if (res[1]!=0) res[1][22]=a24; + a10=(a70*a10); + a18=(a16*a18); + a10=(a10+a18); + a05=(a23*a05); + a10=(a10+a05); + a36=(a69*a36); + a10=(a10+a36); + a43=(a34*a43); + a10=(a10+a43); + if (res[1]!=0) res[1][23]=a10; + a10=-1.; + if (res[1]!=0) res[1][24]=a10; + a11=(a08*a11); + a10=(a01*a11); + a43=(a03*a12); + a21=(a21*a43); + a10=(a10+a21); + a30=(a30*a04); + a28=(a28+a30); + a30=(a26*a28); + a10=(a10+a30); + a30=casadi_sign(a09); + a30=(a09*a30); + a30=(a30+a37); + a37=(a33*a30); + a10=(a10+a37); + a42=(a42*a17); + a42=(a42+a46); + a46=(a40*a42); + a10=(a10+a46); + if (res[1]!=0) res[1][25]=a10; + a10=(a13*a11); + a48=(a48*a43); + a10=(a10+a48); + a48=(a49*a28); + a10=(a10+a48); + a48=(a50*a30); + a10=(a10+a48); + a48=(a51*a42); + a10=(a10+a48); + if (res[1]!=0) res[1][26]=a10; + a10=(a44*a11); + a54=(a54*a43); + a10=(a10+a54); + a54=(a55*a28); + a10=(a10+a54); + a54=(a56*a30); + a10=(a10+a54); + a54=(a57*a42); + a10=(a10+a54); + if (res[1]!=0) res[1][27]=a10; + a10=(a52*a11); + a60=(a60*a43); + a10=(a10+a60); + a60=(a61*a28); + a10=(a10+a60); + a60=(a62*a30); + a10=(a10+a60); + a60=(a63*a42); + a10=(a10+a60); + if (res[1]!=0) res[1][28]=a10; + a10=(a58*a11); + a66=(a66*a43); + a10=(a10+a66); + a66=(a67*a28); + a10=(a10+a66); + a66=(a68*a30); + a10=(a10+a66); + a66=(a69*a42); + a10=(a10+a66); + if (res[1]!=0) res[1][29]=a10; + a11=(a64*a11); + a16=(a16*a43); + a11=(a11+a16); + a28=(a23*a28); + a11=(a11+a28); + a30=(a69*a30); + a11=(a11+a30); + a42=(a34*a42); + a11=(a11+a42); + if (res[1]!=0) res[1][30]=a11; + a72=(-a72); + if (res[1]!=0) res[1][31]=a72; + a72=(-a02); + if (res[1]!=0) res[1][32]=a72; + a72=(-a75); + if (res[1]!=0) res[1][33]=a72; + a03=(a03*a06); + a01=(a01*a03); + a08=(a08*a12); + a14=(a14*a08); + a01=(a01+a14); + a27=(a27*a09); + a27=(a27+a31); + a26=(a26*a27); + a01=(a01+a26); + a35=(a35*a17); + a35=(a35+a39); + a33=(a33*a35); + a01=(a01+a33); + a33=casadi_sign(a04); + a33=(a04*a33); + a33=(a33+a19); + a40=(a40*a33); + a01=(a01+a40); + if (res[1]!=0) res[1][34]=a01; + a13=(a13*a03); + a47=(a47*a08); + a13=(a13+a47); + a49=(a49*a27); + a13=(a13+a49); + a50=(a50*a35); + a13=(a13+a50); + a51=(a51*a33); + a13=(a13+a51); + if (res[1]!=0) res[1][35]=a13; + a44=(a44*a03); + a53=(a53*a08); + a44=(a44+a53); + a55=(a55*a27); + a44=(a44+a55); + a56=(a56*a35); + a44=(a44+a56); + a57=(a57*a33); + a44=(a44+a57); + if (res[1]!=0) res[1][36]=a44; + a52=(a52*a03); + a59=(a59*a08); + a52=(a52+a59); + a61=(a61*a27); + a52=(a52+a61); + a62=(a62*a35); + a52=(a52+a62); + a63=(a63*a33); + a52=(a52+a63); + if (res[1]!=0) res[1][37]=a52; + a58=(a58*a03); + a65=(a65*a08); + a58=(a58+a65); + a67=(a67*a27); + a58=(a58+a67); + a68=(a68*a35); + a58=(a58+a68); + a68=(a69*a33); + a58=(a58+a68); + if (res[1]!=0) res[1][38]=a58; + a64=(a64*a03); + a70=(a70*a08); + a64=(a64+a70); + a23=(a23*a27); + a64=(a64+a23); + a69=(a69*a35); + a64=(a64+a69); + a34=(a34*a33); + a64=(a64+a34); + if (res[1]!=0) res[1][39]=a64; + a74=(-a74); + if (res[1]!=0) res[1][40]=a74; + if (res[1]!=0) res[1][41]=a41; + a74=(-a77); + if (res[1]!=0) res[1][42]=a74; + a74=(a71*a02); + a74=(a09*a74); + a71=(a71*a41); + a71=(a04*a71); + a74=(a74-a71); + a74=(-a74); + if (res[1]!=0) res[1][43]=a74; + a74=(a09*a41); + a71=(a04*a02); + a74=(a74+a71); + if (res[1]!=0) res[1][44]=a74; + a74=(a09*a77); + a71=(a04*a75); + a74=(a74-a71); + a74=(-a74); + if (res[1]!=0) res[1][45]=a74; + a74=casadi_sq(a73); + a41=(a41/a74); + a41=(a09*a41); + a02=(a02/a74); + a02=(a04*a02); + a41=(a41+a02); + a41=(-a41); + if (res[1]!=0) res[1][46]=a41; + a75=(a75/a73); + a29=sin(a29); + a75=(a75*a29); + a09=(a09*a75); + a77=(a77/a73); + a77=(a77*a29); + a04=(a04*a77); + a09=(a09+a04); + a09=(-a09); + if (res[1]!=0) res[1][47]=a09; + a09=1.; + if (res[2]!=0) res[2][0]=a09; + if (res[2]!=0) res[2][1]=a09; + if (res[2]!=0) res[2][2]=a09; + if (res[2]!=0) res[2][3]=a09; + if (res[2]!=0) res[2][4]=a09; + if (res[2]!=0) res[2][5]=a09; + if (res[2]!=0) res[2][6]=a09; + if (res[2]!=0) res[2][7]=a09; + if (res[2]!=0) res[2][8]=a09; + a09=-3.3453634479321918e-02; + if (res[3]!=0) res[3][0]=a09; + a09=-6.0368975005218423e-05; + if (res[3]!=0) res[3][1]=a09; + a09=1.1116270017027936e-07; + if (res[3]!=0) res[3][2]=a09; + a09=-9.8084735444364535e-08; + if (res[3]!=0) res[3][3]=a09; + a09=-1.0920100546139210e-05; + if (res[3]!=0) res[3][4]=a09; + a09=6.0150572994295635e-03; + if (res[3]!=0) res[3][5]=a09; + a09=-1.0920100546139184e-05; + if (res[3]!=0) res[3][6]=a09; + a09=-6.0570157695918683e-03; + if (res[3]!=0) res[3][7]=a09; + a09=3.0801331505514837e-03; + if (res[3]!=0) res[3][8]=a09; + a09=-2.7177645446042520e-03; + if (res[3]!=0) res[3][9]=a09; + a09=-3.0257778596593993e-01; + if (res[3]!=0) res[3][10]=a09; + a09=5.4600502730695923e-04; + if (res[3]!=0) res[3][11]=a09; + a04=6.0150572994295574e-03; + if (res[3]!=0) res[3][12]=a04; + a04=3.0184487502609172e-03; + if (res[3]!=0) res[3][13]=a04; + a04=-5.5581350085139543e-06; + if (res[3]!=0) res[3][14]=a04; + a04=4.9042367722181902e-06; + if (res[3]!=0) res[3][15]=a04; + if (res[3]!=0) res[3][16]=a09; + a09=-3.0075286497147785e-01; + if (res[3]!=0) res[3][17]=a09; + return 0; +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_u_z(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ + return casadi_f0(arg, res, iw, w, mem); +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_u_z_alloc_mem(void) { + return 0; +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_u_z_init_mem(int mem) { + return 0; +} + +CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_jac_x_xdot_u_z_free_mem(int mem) { +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_u_z_checkout(void) { + return 0; +} + +CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_jac_x_xdot_u_z_release(int mem) { +} + +CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_jac_x_xdot_u_z_incref(void) { +} + +CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_jac_x_xdot_u_z_decref(void) { +} + +CASADI_SYMBOL_EXPORT casadi_int auv_model_impl_dae_fun_jac_x_xdot_u_z_n_in(void) { return 6;} + +CASADI_SYMBOL_EXPORT casadi_int auv_model_impl_dae_fun_jac_x_xdot_u_z_n_out(void) { return 5;} + +CASADI_SYMBOL_EXPORT casadi_real auv_model_impl_dae_fun_jac_x_xdot_u_z_default_in(casadi_int i) { + switch (i) { + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const char* auv_model_impl_dae_fun_jac_x_xdot_u_z_name_in(casadi_int i) { + switch (i) { + case 0: return "i0"; + case 1: return "i1"; + case 2: return "i2"; + case 3: return "i3"; + case 4: return "i4"; + case 5: return "i5"; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const char* auv_model_impl_dae_fun_jac_x_xdot_u_z_name_out(casadi_int i) { + switch (i) { + case 0: return "o0"; + case 1: return "o1"; + case 2: return "o2"; + case 3: return "o3"; + case 4: return "o4"; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const casadi_int* auv_model_impl_dae_fun_jac_x_xdot_u_z_sparsity_in(casadi_int i) { + switch (i) { + case 0: return casadi_s0; + case 1: return casadi_s0; + case 2: return casadi_s1; + case 3: return casadi_s2; + case 4: return casadi_s3; + case 5: return casadi_s2; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const casadi_int* auv_model_impl_dae_fun_jac_x_xdot_u_z_sparsity_out(casadi_int i) { + switch (i) { + case 0: return casadi_s0; + case 1: return casadi_s4; + case 2: return casadi_s5; + case 3: return casadi_s6; + case 4: return casadi_s7; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_u_z_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { + if (sz_arg) *sz_arg = 6; + if (sz_res) *sz_res = 5; + if (sz_iw) *sz_iw = 0; + if (sz_w) *sz_w = 0; + return 0; +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_u_z_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { + if (sz_arg) *sz_arg = 6*sizeof(const casadi_real*); + if (sz_res) *sz_res = 5*sizeof(casadi_real*); + if (sz_iw) *sz_iw = 0*sizeof(casadi_int); + if (sz_w) *sz_w = 0*sizeof(casadi_real); + return 0; +} + + +#ifdef __cplusplus +} /* extern "C" */ +#endif diff --git a/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u_z.o b/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u_z.o new file mode 100644 index 0000000000000000000000000000000000000000..dc3e918bb13381d14374a20d16c91be469187efa GIT binary patch literal 21944 zcmchf3wTwt>2LXIlvAO@XW=*m(a(crp z?R?+fd*(N5)~s1Gdmj4)zExCw`62L&?=N@qsy`ShB|Jx3#VyxjsbBa3GwMHY3%PlK93-Ism0?~g1l z@)u>iGYGx`=z;j!-c9wW#+EeP!c?H)eZT_2m)+C zR%vhy?j6A-)Q|j1Kbras7YQI4KUmPGqvh{WE?``7%md}2-mA57(yMs39Ey>DBzpW z5ey(rGBBhtmo8lO1+QOh8HZKg8K+`6eRTvB* z_9izfVk#g5U`*;>s=OUas2V@=xZ*KI8GVE5L2xV$nWd+ZWGdH!Otz zF}~hF`G?rg@Zr7eE0B0FfEK<$ddj)cj|53Ka+U{|8NGgLf9w?!DTU#96bD<17te)@ z%3p)74Y*#d^PRO1&z&e(L&GxmJg&FVnjIjI@UUPdzUZGJ_8eewunB|;bckzX`;mx~ zW1~AD^zxAcU+hmvP$kb%Vh7zYRz)c}naGRC|D>+Km$n;O9~0RF@;pVh6UN9ci({N4 zjhC$aMe+t-hVsK~MHn$-B)OY}P-pp8@9bemZ$z*r*DAfC}PpT{QrL8?)&1N{9 z)K+qh7{{M&W;wqRC$X~FznJckW@1DhuTtQC=i0X&{TbuAX>gJ9jo|3pn7qVVUHJxZ z^mFVoQ|X|*P2hw3ywDeWiXqtGo~ma`Dv5Lkx! zCrq=`UlE-XjMxIDLA(`y0GQc>Ya^tw7-ACKg%)DE{gejmQ#9wc0@z|)0PjB)HWr{v zVK92ccf$c(d%2QI+-rgnXk5;P5&0C?AAy$#OJFso5=@p@x2Orod6Orz@1P#25R)}d$&ZHdzC`y$x>0lkd9Ik2I7~ONDz=T>$#20-@*X)8zA5j9Kq0qw zqbWfSD-Oa`DiQ4_!6KBmU4ba%)41J{#nWl!_qDBO3uC<8rSb5C1*`26dKPj$=rT(k zH4E*;6~lU)+J$EiE*?L1SdWqa5U-YiTvDLKV?DlDbtHubQxbt1nUas zds?88A$CQv_W=wz(3cVpaMQ*NZrT&JX=6}qpe4%~qpN9$fgFB+@$BH$1?|G-E4JIT zkG2fd#?dhAm6w*#l0P66-ANfx5JrnixJ72I z@9(U~jp+{sWglVt%j{9CS?GW463~F{+;g-8KqXjjY_CPPSW&QDs8pw4g!3zrFS>0< zQrUhFXnB;kJsRDDvP~!pS?XD8OlzyLxrUZ(jLtEzj(3L`iZt-=m5m_YE(l=Q4|JlH z$GaKeu^Kvs-=-XB#Gvfo>9C^y84=63KNaf_#w;(jz8Kc37_STE<+Mzj7U8uhrL{at zJJ?A^V+`+m^j)N-hS*Uj|Ibc7G^Z?UA8ZY6AO3Eg5B~#nE;%VWQ6s|jyXY=l z*c+v^oBqfCFAbVX=}3a{MWd`ExWp9a-7HQd{SyaN@&N@S)r`3x(JPAG25rTBu@OPo z_9BG(HE^?0VOy{VDrvt(11Qd88`C~Eac@)cXX!@9Eey_viUYXJbWrF^U58Ssm~dh; zsieLG5bMGVj1?uuenx5QvhsMa1+Uh6Xt44hpe#*9ipBoqq+gcwOM@!I``Jxq`_k5# z39tWE#%nDWVwr+|ZYOkTjs-Txua3i?0ykzGdD4yjw$bfNd6<{u0$&tOj6!!5dZJIk z6zT;yPv%KlU&`8ot$@h;$mC(YLj%gN6G>^66Qns_lzwLV;3?x5m@>%qd}1`y4>~wN z=YdOu(3D1JRCBTsi_88(nR9Tgz=KCKmEUA!fp}h|Jn?=Rwiq(`px1igwnNC}!wD_J z=Dj#l#9Qtdoy}P%M4~us$V`KRH1Ny$a4`-oB^c)bf5adI0K-^9gOHde_5ntW_AF-Q)$+ zGvFJ5qJvQvBVWqha*nli;nWcw@PjY*3DSRsm&Dz%_h2{e1hy&$FGjaQ6|MBvY>XfF zj2%bd3wf`D3e1}W#n0lxLYGQMopRc)MV~>egExTWKD-raJ7H)Uj(t(uq0;vSYz1fs zN2zBqtjwMRyD%CUVJO=G7-mb#pF{a=RQ?!AZzC5Sqa;>tdV|tlfOroIw(&L)QxGer zqZhWmw=BHNkf5Q2g}eV0>h;^t%~Ik$WQK!7g74-r=yHc51jn z5FbP04X}d86!Hh6VZVw8R6L~OP-}~ZOH{m4#hV7=ybX!mv4OsNWprJ4eL;D_^#wN+ zRM_uiXw%DeMZQB_G|Zo-j_Sn@0qcMNzG4@;FeT1k8+ho#J$Q}up@|M!gVIAeM8Etu z>Y_e9iFNf)3xBIljGF#B&8)-iw9|!d-$^`N(w7&!OPI@W3c}Q~8-0=1jnrCTygj&e zVJhO~8xJ{F;zK2UeWEvI(XzvRSS`)RDtzoj&-02E*@P8?Ce+9#bMi9r!eE#p_9%!q zrH1Ab+@lA_9O>(;^PSg50{EV4zB!_vF0C^_nJ6$K$X_Gr`&};xJ@;~sv4koxlNk0K z=8NV)22C_vXNff&sX~5l&6`C!dkFsq%uV9`l%(a(DO`VM~hS2nefnuoU* z6?-reYn}a`h!jtnlx|mcv06Gr9Q+Q{;=42X5yDs`%FPsjnFPBX8uO*WT-aC2b8RV& z|0pEN>9Du<;8J+Kt}fkrtFE2CKWB7dYUuahXVdFdiQ~z;`TtjX)hM)#s)Qd%-zpln zDouT7-*d{pvv1@rjgk9!9lob8Qa$2{$grNgzPj1w_bB=_q8QyY z*Xz+^#;24Iu_TvI<7k5N^&zi5!>iBq>a)E1Y_C4YYYcgf8D3+i*O=usW>Yo)^l+Ng z$c_9PwUgX%$tQ=Mj_V9bQ(5vC<&VI{BOlKb;iO;N;_9P1PsNpV0&)=T4_d#r{|k^f zAJ@gW(s5B|(naA!>_1U?vLR6!-PWPVXnBg0k8IHWN^zT}zS3#(k52cQ@GYyydA3U0 zKAr5|{1xXud&&2I?#uslOMCuX>`N27FAzJ2huQv4X>Xow?=7aCo!fVB zANTyx5A$QDJ)H+bTb^j2UYGx{wCnQ^Z@c!j2X?NJ_I679`~6|<-6!SF7Wu4{ty3DW z-;rO^KJn$;eFdHN*}QS<;k)y9*#7F2_PF+~5r40-^-25INc%d)-h8pQQ|hzzSyGBfRR|MQ>fTYIOMib?>K@-#@x*Jw4XF zkuT3q;a0B8n=yU*mjY=sXU%Vj&ktl|rDvpvCgslOWXA28ndzbI34*yQn&14mXpWol zwV5>UbrG|3BYwZ-FzmLgyVP~IyYEnC1EGl!38Vux3Ky(;*B`t&qwBAy<8W-`f!>Rqm>C-j^E8l@Q z@e+qWD?BW1>!m^nAK)T+>nE${JjfB>OF{^x=HXF45pR`rr95wDy-8lKr0wGy!b29w zbAj-H!&?*ZMZ!xQ`NawN1H!|O{0|cF9}BmBvBj1q;J>tb_-Um*Z$$$BTj3>+{F4dz zTH%$ByxE;5c`koek*^oN(hAD6F#(@1e2F8! zAOXKaxZkPwt_1u(;XRK0g9-Q#h3|Fbf0}^*On9#&|LX+&G2yG6_WoY@0f+xV_+E#v z7vAmgr-gSpe4FrHj-7uL{*lA?2yb%us|omBj@9;B)k8$`Hga;gcp71n>j}snp_{G9=9iG7)^R~mMhBR^B*OC0&@M4k`$Dz8eopZjMZ<3`~DhtCym_b0YQ zvv9kgvHVuycHd(8w}e+Z`WFdra`&;Jzevp}A| z6z=ZqRtR@T?r(+L{gzd?S~!1Xv=Gnr!tFO0o3&Z^xfaOtIpOX&-zEHPNB$M?Gt^>X z&n%-y7;gP$&n-E^?f9|nyFqxR=;!kb;%(qohpu{Y0)AHlet!b~AUN6IL+2#~d(K%e ze6Pcg2=5hc&n#mRVLyC+u$~g(e&JUBcHses|5|v+;k$%~h1>Jaa10!(x5VL>2(J`w z{ah)$Nw_@^eOGv|aC;{DgViGgB5IoF9TMIp+{&L12a-J@`e24&&qr4XZ*sU-HUAqG za~o@F=Ty|sZJJY2Q&n40H@~6c>s8eiEfp;_jq!^46}P2V4^6J3IW$zk%%KWq4OK8> zsDjx;6`V3u!JMHAPWIA^r)Sb_3f(xFL$M6L=VWl);1I`+oZ-1#hVL0uInMV?mYl-( zOqR^#Dl)T3CNrDwtUuG#&pLAqXKgt~58rcGGRx>>Jz1=C3Tw+U#&g*$LpYve;#_vJ zDaCbVaa~zlOP0~kB%Af;uuL}VpUfJvjeaA;WD1wf=31t(hHS1So6AmSJ=vy3M$+6_ zGS_Hh{kg0y*XUslxqN2@MuHs0r@+7l3Ns2H-p8!Cq0yqem&s`+!` zC!oA)ZtV$558ayPrmA_(_3>MWUO|6}p;vNC=Ev55^UL zOwJ)Be~sc_Ab@~vnWyE=cNkMIeS@_6a}vm3CY&BKmAv_W@>xCCDEVuZd`klTtx8_o z)1i2o((^iQ*}WZ&W;_ z_=}2172mJ;1&Y6~_!PyH;uG_m?@o~z|R~6TOct`Oj-L4YFwI3EJPTzX0 zpBF2x?f;SDy1h#k*Y>YaT(|d0#WPjCn-u?d#Zy0r1cKY%9O2|2y^dU_xL%LGs<>Vs z=PMqAF6)Q86xZv~4-_Ax
KUXOmSxL#kjD6ZG3J&Nmf=Pktps@_i&*Xz#bPw}AQmnfd5c)sEnDSoBm6BVyeT(1|+itBafPQ~>)@}S~+9a*ZlUPqo(T+iq0 z70*?6Zc#j}_;$thzVsc%_5A-A#b+ozBjp1o`CrffV-=4m`ALfF^{YT}y&l~y+Ku{o!8;A&mM>8J zD~f+laXoMRMDYnq{#S}$rugp^zgY2gisvi7Rq;uRzo>YD;_oVch2m$?;RJ#FOd__u z*9dp}@m?jb_s>fd*Yn$N6xZ{~YQ@u4z1@n>RQz?tzpD6qihoV_Y~Lj?Fz;9xLv2X9=CfG*YoFF zitG996UFs7#6KwEK>pO@ZM@=o{z+F{kK4(L>+u;;T+df$&|pHKarg~fw!f+sU!eGM z;qG|ep*T&AR?nUUd^l}N5J*o2E-Rm`c%|a>zidz-`6^sie!Swd6wg$=TJb!^YZU*A z;wUsgitBZDyW;x2drfiu9?}0aM1lOQ*ZosylZ`+;jLZ7-D#i8tQm!~H-B!Lsahj4X zU#a+P#rG(#*PZtize>rErb!cl?7SM6)o*`CC9d^1D|xMdA#DN>NPajjtLH(*>lI(B z_}3Nxjp8>czFzS;itkW-uHvsN-k|s~#mP?TKJQH0+#`_vjkv7+7bxDOc$VTfD?VNE zd5T}7c(dYj6eoM=ou_c8!^_Dqga;jdsqm!^pD6q{4lfq|jKjmM-Sb{|_@%>4#!-h` zdxq0ui9l+}{!&UHoaykF4E8Q?_(I|KL3-#~BK<_`69vm35}ssmKkpV!?-T_q|B`T7 ze*3xoz1sG_mER!p(%gRefPiz1`hK1&JmBz%aGLWeSpDU~Z9iKc7EW_G1DZS@|yE8ytE2cO3Tb0G)Zhg$(P@ zt8R{G;Fn~PL<&DVS2xa`TiXCt4UO^Ibo`TwNzL)9>YF5eW5fLPrg@D`we#Y)iq6{E z6?OCQyPkC5-}wljJh^6+qbDrsr83@awjIwlO=k|81H%@`vxGKF2qm%})82sUNg{|K z-NN63h|}Dx!|P7HHcz_ICOw*uNaz2zJnbb(XC^KkZ2e<3MBLVIpR_llIE^120`$=h zVUCj#U^;LMBYHglEs=@J{(T|&kNU{ye1Hz0 z2vPIszuWrlb2f6Qe5hZ-L|UXg^^XoDOTj*AZ(`$|A-4`GuNlQ%**m~a#pN{9TPEcL z$kV~vZP^bIA)RjdZYh7jsn9L|5Xv5}|5kp@)Ua2|Q~%Mv*_OBC|9Ev%$(3X2?s)+! ugh2fj$K{r%&-L!u2l9H->F#;AlXFyXnuGs2_4wu6OH84qsw_p_^8X71$7nqO literal 0 HcmV?d00001 diff --git a/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_z.c b/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_z.c new file mode 100644 index 000000000..7cbf6b868 --- /dev/null +++ b/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_z.c @@ -0,0 +1,809 @@ +/* This file was automatically generated by CasADi 3.7.2. + * It consists of: + * 1) content generated by CasADi runtime: not copyrighted + * 2) template code copied from CasADi source: permissively licensed (MIT-0) + * 3) user code: owned by the user + * + */ +#ifdef __cplusplus +extern "C" { +#endif + +/* How to prefix internal symbols */ +#ifdef CASADI_CODEGEN_PREFIX + #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID) + #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID + #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID) +#else + #define CASADI_PREFIX(ID) auv_model_impl_dae_fun_jac_x_xdot_z_ ## ID +#endif + +#include + +#ifndef casadi_real +#define casadi_real double +#endif + +#ifndef casadi_int +#define casadi_int int +#endif + +/* Add prefix to internal symbols */ +#define casadi_f0 CASADI_PREFIX(f0) +#define casadi_fabs CASADI_PREFIX(fabs) +#define casadi_s0 CASADI_PREFIX(s0) +#define casadi_s1 CASADI_PREFIX(s1) +#define casadi_s2 CASADI_PREFIX(s2) +#define casadi_s3 CASADI_PREFIX(s3) +#define casadi_s4 CASADI_PREFIX(s4) +#define casadi_s5 CASADI_PREFIX(s5) +#define casadi_s6 CASADI_PREFIX(s6) +#define casadi_sign CASADI_PREFIX(sign) +#define casadi_sq CASADI_PREFIX(sq) + +/* Symbol visibility in DLLs */ +#ifndef CASADI_SYMBOL_EXPORT + #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) + #if defined(STATIC_LINKED) + #define CASADI_SYMBOL_EXPORT + #else + #define CASADI_SYMBOL_EXPORT __declspec(dllexport) + #endif + #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) + #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default"))) + #else + #define CASADI_SYMBOL_EXPORT + #endif +#endif + +casadi_real casadi_fabs(casadi_real x) { +/* Pre-c99 compatibility */ +#if __STDC_VERSION__ < 199901L + return x>0 ? x : -x; +#else + return fabs(x); +#endif +} + +casadi_real casadi_sign(casadi_real x) { return x<0 ? -1 : x>0 ? 1 : x;} + +casadi_real casadi_sq(casadi_real x) { return x*x;} + +static const casadi_int casadi_s0[3] = {9, 1, 1}; +static const casadi_int casadi_s1[3] = {3, 1, 1}; +static const casadi_int casadi_s2[3] = {0, 1, 1}; +static const casadi_int casadi_s3[3] = {0, 0, 1}; +static const casadi_int casadi_s4[60] = + {9, 9, 0, 6, 12, 18, 25, 34, + 43, 46, 48, 48, 0, 1, 2, 3, + 4, 5, 0, 1, 2, 3, 4, 5, + 0, 1, 2, 3, 4, 5, 0, 1, + 2, 3, 4, 5, 6, 0, 1, 2, + 3, 4, 5, 6, 7, 8, 0, 1, + 2, 3, 4, 5, 6, 7, 8, 6, + 7, 8, 6, 8}; +static const casadi_int casadi_s5[21] = + {9, 9, 0, 1, 2, 3, 4, 5, + 6, 7, 8, 9, 0, 1, 2, 3, + 4, 5, 6, 7, 8}; +static const casadi_int casadi_s6[3] = {9, 0, 1}; + +/* auv_model_impl_dae_fun_jac_x_xdot_z:(i0[9],i1[9],i2[3],i3[0],i4[],i5[0])->(o0[9],o1[9x9,48nz],o2[9x9,9nz],o3[9x0]) */ +static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { + casadi_real a00, a01, a02, a03, a04, a05, a06, a07, a08, a09, a10, a11; + casadi_real a12, a13, a14, a15, a16, a17, a18, a19, a20, a21, a22, a23; + casadi_real a24, a25, a26, a27, a28, a29, a30, a31, a32, a33, a34, a35; + casadi_real a36, a37, a38, a39, a40, a41, a42, a43, a44, a45, a46, a47; + casadi_real a48, a49, a50, a51, a52, a53, a54, a55, a56, a57, a58, a59; + casadi_real a60, a61, a62, a63, a64, a65, a66, a67, a68, a69, a70, a71; + casadi_real a72, a73, a74, a75, a76, a77, a78; + a00=arg[1]? arg[1][0] : 0; + a01=3.3453634479321918e-02; + a02=arg[2]? arg[2][0] : 0; + a03=-30.; + a04=arg[0]? arg[0][5] : 0; + a05=(a03*a04); + a06=arg[0]? arg[0][1] : 0; + a07=(a05*a06); + a08=30.; + a09=arg[0]? arg[0][4] : 0; + a10=(a08*a09); + a11=arg[0]? arg[0][2] : 0; + a12=(a10*a11); + a07=(a07+a12); + a02=(a02-a07); + a07=23.; + a12=arg[0]? arg[0][0] : 0; + a13=casadi_fabs(a12); + a07=(a07+a13); + a13=(a07*a12); + a02=(a02-a13); + a13=(a01*a02); + a14=6.0368975005218254e-05; + a15=(a08*a04); + a16=(a15*a12); + a17=arg[0]? arg[0][3] : 0; + a18=(a03*a17); + a19=(a18*a11); + a16=(a16+a19); + a19=46.; + a20=casadi_fabs(a06); + a20=(a19+a20); + a21=(a20*a06); + a16=(a16+a21); + a21=(a14*a16); + a13=(a13-a21); + a21=-1.1116270017027866e-07; + a22=(a03*a09); + a23=(a22*a12); + a24=(a08*a17); + a25=(a24*a06); + a23=(a23+a25); + a25=casadi_fabs(a11); + a25=(a19+a25); + a26=(a25*a11); + a23=(a23+a26); + a26=(a21*a23); + a13=(a13-a26); + a26=9.8084735444363873e-08; + a27=3.3399999999999999e+00; + a28=(a27*a04); + a29=(a28*a09); + a30=-3.3199999999999998e+00; + a31=(a30*a09); + a32=(a31*a04); + a29=(a29+a32); + a32=casadi_fabs(a17); + a32=(a19+a32); + a33=(a32*a17); + a29=(a29+a33); + a33=(a26*a29); + a13=(a13-a33); + a33=1.0920100546139184e-05; + a34=arg[2]? arg[2][1] : 0; + a35=-3.3399999999999999e+00; + a36=(a35*a04); + a37=(a36*a17); + a38=6.8000000000000005e-01; + a39=(a38*a17); + a40=(a39*a04); + a37=(a37+a40); + a34=(a34-a37); + a37=casadi_fabs(a09); + a37=(a19+a37); + a40=(a37*a09); + a34=(a34-a40); + a40=(a33*a34); + a13=(a13+a40); + a40=-6.0150572994295574e-03; + a41=arg[2]? arg[2][2] : 0; + a42=3.3199999999999998e+00; + a43=(a42*a09); + a44=(a43*a17); + a45=-6.8000000000000005e-01; + a46=(a45*a17); + a47=(a46*a09); + a44=(a44+a47); + a41=(a41-a44); + a44=casadi_fabs(a04); + a19=(a19+a44); + a44=(a19*a04); + a41=(a41-a44); + a44=(a40*a41); + a13=(a13+a44); + a00=(a00-a13); + if (res[0]!=0) res[0][0]=a00; + a00=arg[1]? arg[1][1] : 0; + a13=6.0368975005218423e-05; + a44=(a13*a02); + a47=3.3484658136227786e-02; + a48=(a47*a16); + a44=(a44-a48); + a48=-6.1658244361114702e-05; + a49=(a48*a23); + a44=(a44-a49); + a49=5.4404333259807184e-05; + a50=(a49*a29); + a44=(a44-a50); + a50=6.0570157695918683e-03; + a51=(a50*a34); + a44=(a44+a51); + a51=-3.0184487502609172e-03; + a52=(a51*a41); + a44=(a44+a52); + a00=(a00-a44); + if (res[0]!=0) res[0][1]=a00; + a00=arg[1]? arg[1][2] : 0; + a44=-1.1116270017027936e-07; + a52=(a44*a02); + a53=-6.1658244361114783e-05; + a54=(a53*a16); + a52=(a52-a54); + a54=3.3963490377380855e-02; + a55=(a54*a23); + a52=(a52-a55); + a55=-2.9967785627100754e-02; + a56=(a55*a29); + a52=(a52-a56); + a56=-3.0801331505514837e-03; + a57=(a56*a34); + a52=(a52+a57); + a57=5.5581350085139543e-06; + a58=(a57*a41); + a52=(a52+a58); + a00=(a00-a52); + if (res[0]!=0) res[0][2]=a00; + a00=arg[1]? arg[1][3] : 0; + a52=9.8084735444364535e-08; + a58=(a52*a02); + a59=5.4404333259807062e-05; + a60=(a59*a16); + a58=(a58-a60); + a60=-2.9967785627100469e-02; + a61=(a60*a23); + a58=(a58-a61); + a61=1.4970303990827358e+00; + a62=(a61*a29); + a58=(a58-a62); + a62=2.7177645446042520e-03; + a63=(a62*a34); + a58=(a58+a63); + a63=-4.9042367722181902e-06; + a64=(a63*a41); + a58=(a58+a64); + a00=(a00-a58); + if (res[0]!=0) res[0][3]=a00; + a00=arg[1]? arg[1][4] : 0; + a58=1.0920100546139210e-05; + a64=(a58*a02); + a65=6.0570157695918657e-03; + a66=(a65*a16); + a64=(a64-a66); + a66=-3.0801331505514781e-03; + a67=(a66*a23); + a64=(a64-a67); + a67=2.7177645446042498e-03; + a68=(a67*a29); + a64=(a64-a68); + a68=3.0257778596593993e-01; + a69=(a68*a34); + a64=(a64+a69); + a69=-5.4600502730695923e-04; + a70=(a69*a41); + a64=(a64+a70); + a00=(a00-a64); + if (res[0]!=0) res[0][4]=a00; + a00=arg[1]? arg[1][5] : 0; + a64=-6.0150572994295635e-03; + a02=(a64*a02); + a70=-3.0184487502609133e-03; + a16=(a70*a16); + a02=(a02-a16); + a16=5.5581350085139340e-06; + a23=(a16*a23); + a02=(a02-a23); + a23=-4.9042367722181935e-06; + a29=(a23*a29); + a02=(a02-a29); + a34=(a69*a34); + a02=(a02+a34); + a34=3.0075286497147785e-01; + a41=(a34*a41); + a02=(a02+a41); + a00=(a00-a02); + if (res[0]!=0) res[0][5]=a00; + a00=arg[1]? arg[1][6] : 0; + a02=arg[0]? arg[0][6] : 0; + a41=sin(a02); + a29=arg[0]? arg[0][7] : 0; + a71=tan(a29); + a72=(a41*a71); + a73=(a72*a09); + a73=(a17+a73); + a02=cos(a02); + a74=(a02*a71); + a75=(a74*a04); + a73=(a73+a75); + a00=(a00-a73); + if (res[0]!=0) res[0][6]=a00; + a00=arg[1]? arg[1][7] : 0; + a73=(a02*a09); + a75=(a41*a04); + a73=(a73-a75); + a00=(a00-a73); + if (res[0]!=0) res[0][7]=a00; + a00=arg[1]? arg[1][8] : 0; + a73=cos(a29); + a75=(a41/a73); + a76=(a75*a09); + a77=(a02/a73); + a78=(a77*a04); + a76=(a76+a78); + a00=(a00-a76); + if (res[0]!=0) res[0][8]=a00; + a00=casadi_sign(a12); + a00=(a12*a00); + a00=(a00+a07); + a07=(a01*a00); + a76=(a14*a15); + a07=(a07+a76); + a76=(a21*a22); + a07=(a07+a76); + if (res[1]!=0) res[1][0]=a07; + a07=(a13*a00); + a76=(a47*a15); + a07=(a07+a76); + a76=(a48*a22); + a07=(a07+a76); + if (res[1]!=0) res[1][1]=a07; + a07=(a44*a00); + a76=(a53*a15); + a07=(a07+a76); + a76=(a54*a22); + a07=(a07+a76); + if (res[1]!=0) res[1][2]=a07; + a07=(a52*a00); + a76=(a59*a15); + a07=(a07+a76); + a76=(a60*a22); + a07=(a07+a76); + if (res[1]!=0) res[1][3]=a07; + a07=(a58*a00); + a76=(a65*a15); + a07=(a07+a76); + a76=(a66*a22); + a07=(a07+a76); + if (res[1]!=0) res[1][4]=a07; + a00=(a64*a00); + a15=(a70*a15); + a00=(a00+a15); + a22=(a16*a22); + a00=(a00+a22); + if (res[1]!=0) res[1][5]=a00; + a00=(a01*a05); + a22=casadi_sign(a06); + a22=(a06*a22); + a22=(a22+a20); + a20=(a14*a22); + a00=(a00+a20); + a20=(a21*a24); + a00=(a00+a20); + if (res[1]!=0) res[1][6]=a00; + a00=(a13*a05); + a20=(a47*a22); + a00=(a00+a20); + a20=(a48*a24); + a00=(a00+a20); + if (res[1]!=0) res[1][7]=a00; + a00=(a44*a05); + a20=(a53*a22); + a00=(a00+a20); + a20=(a54*a24); + a00=(a00+a20); + if (res[1]!=0) res[1][8]=a00; + a00=(a52*a05); + a20=(a59*a22); + a00=(a00+a20); + a20=(a60*a24); + a00=(a00+a20); + if (res[1]!=0) res[1][9]=a00; + a00=(a58*a05); + a20=(a65*a22); + a00=(a00+a20); + a20=(a66*a24); + a00=(a00+a20); + if (res[1]!=0) res[1][10]=a00; + a05=(a64*a05); + a22=(a70*a22); + a05=(a05+a22); + a24=(a16*a24); + a05=(a05+a24); + if (res[1]!=0) res[1][11]=a05; + a05=(a01*a10); + a24=(a14*a18); + a05=(a05+a24); + a24=casadi_sign(a11); + a24=(a11*a24); + a24=(a24+a25); + a25=(a21*a24); + a05=(a05+a25); + if (res[1]!=0) res[1][12]=a05; + a05=(a13*a10); + a25=(a47*a18); + a05=(a05+a25); + a25=(a48*a24); + a05=(a05+a25); + if (res[1]!=0) res[1][13]=a05; + a05=(a44*a10); + a25=(a53*a18); + a05=(a05+a25); + a25=(a54*a24); + a05=(a05+a25); + if (res[1]!=0) res[1][14]=a05; + a05=(a52*a10); + a25=(a59*a18); + a05=(a05+a25); + a25=(a60*a24); + a05=(a05+a25); + if (res[1]!=0) res[1][15]=a05; + a05=(a58*a10); + a25=(a65*a18); + a05=(a05+a25); + a25=(a66*a24); + a05=(a05+a25); + if (res[1]!=0) res[1][16]=a05; + a10=(a64*a10); + a18=(a70*a18); + a10=(a10+a18); + a24=(a16*a24); + a10=(a10+a24); + if (res[1]!=0) res[1][17]=a10; + a10=(a03*a11); + a24=(a14*a10); + a18=(a08*a06); + a05=(a21*a18); + a24=(a24+a05); + a05=casadi_sign(a17); + a05=(a17*a05); + a05=(a05+a32); + a32=(a26*a05); + a24=(a24+a32); + a38=(a38*a04); + a36=(a36+a38); + a38=(a33*a36); + a24=(a24+a38); + a45=(a45*a09); + a43=(a43+a45); + a45=(a40*a43); + a24=(a24+a45); + if (res[1]!=0) res[1][18]=a24; + a24=(a47*a10); + a45=(a48*a18); + a24=(a24+a45); + a45=(a49*a05); + a24=(a24+a45); + a45=(a50*a36); + a24=(a24+a45); + a45=(a51*a43); + a24=(a24+a45); + if (res[1]!=0) res[1][19]=a24; + a24=(a53*a10); + a45=(a54*a18); + a24=(a24+a45); + a45=(a55*a05); + a24=(a24+a45); + a45=(a56*a36); + a24=(a24+a45); + a45=(a57*a43); + a24=(a24+a45); + if (res[1]!=0) res[1][20]=a24; + a24=(a59*a10); + a45=(a60*a18); + a24=(a24+a45); + a45=(a61*a05); + a24=(a24+a45); + a45=(a62*a36); + a24=(a24+a45); + a45=(a63*a43); + a24=(a24+a45); + if (res[1]!=0) res[1][21]=a24; + a24=(a65*a10); + a45=(a66*a18); + a24=(a24+a45); + a45=(a67*a05); + a24=(a24+a45); + a45=(a68*a36); + a24=(a24+a45); + a45=(a69*a43); + a24=(a24+a45); + if (res[1]!=0) res[1][22]=a24; + a10=(a70*a10); + a18=(a16*a18); + a10=(a10+a18); + a05=(a23*a05); + a10=(a10+a05); + a36=(a69*a36); + a10=(a10+a36); + a43=(a34*a43); + a10=(a10+a43); + if (res[1]!=0) res[1][23]=a10; + a10=-1.; + if (res[1]!=0) res[1][24]=a10; + a11=(a08*a11); + a10=(a01*a11); + a43=(a03*a12); + a21=(a21*a43); + a10=(a10+a21); + a30=(a30*a04); + a28=(a28+a30); + a30=(a26*a28); + a10=(a10+a30); + a30=casadi_sign(a09); + a30=(a09*a30); + a30=(a30+a37); + a37=(a33*a30); + a10=(a10+a37); + a42=(a42*a17); + a42=(a42+a46); + a46=(a40*a42); + a10=(a10+a46); + if (res[1]!=0) res[1][25]=a10; + a10=(a13*a11); + a48=(a48*a43); + a10=(a10+a48); + a48=(a49*a28); + a10=(a10+a48); + a48=(a50*a30); + a10=(a10+a48); + a48=(a51*a42); + a10=(a10+a48); + if (res[1]!=0) res[1][26]=a10; + a10=(a44*a11); + a54=(a54*a43); + a10=(a10+a54); + a54=(a55*a28); + a10=(a10+a54); + a54=(a56*a30); + a10=(a10+a54); + a54=(a57*a42); + a10=(a10+a54); + if (res[1]!=0) res[1][27]=a10; + a10=(a52*a11); + a60=(a60*a43); + a10=(a10+a60); + a60=(a61*a28); + a10=(a10+a60); + a60=(a62*a30); + a10=(a10+a60); + a60=(a63*a42); + a10=(a10+a60); + if (res[1]!=0) res[1][28]=a10; + a10=(a58*a11); + a66=(a66*a43); + a10=(a10+a66); + a66=(a67*a28); + a10=(a10+a66); + a66=(a68*a30); + a10=(a10+a66); + a66=(a69*a42); + a10=(a10+a66); + if (res[1]!=0) res[1][29]=a10; + a11=(a64*a11); + a16=(a16*a43); + a11=(a11+a16); + a28=(a23*a28); + a11=(a11+a28); + a30=(a69*a30); + a11=(a11+a30); + a42=(a34*a42); + a11=(a11+a42); + if (res[1]!=0) res[1][30]=a11; + a72=(-a72); + if (res[1]!=0) res[1][31]=a72; + a72=(-a02); + if (res[1]!=0) res[1][32]=a72; + a72=(-a75); + if (res[1]!=0) res[1][33]=a72; + a03=(a03*a06); + a01=(a01*a03); + a08=(a08*a12); + a14=(a14*a08); + a01=(a01+a14); + a27=(a27*a09); + a27=(a27+a31); + a26=(a26*a27); + a01=(a01+a26); + a35=(a35*a17); + a35=(a35+a39); + a33=(a33*a35); + a01=(a01+a33); + a33=casadi_sign(a04); + a33=(a04*a33); + a33=(a33+a19); + a40=(a40*a33); + a01=(a01+a40); + if (res[1]!=0) res[1][34]=a01; + a13=(a13*a03); + a47=(a47*a08); + a13=(a13+a47); + a49=(a49*a27); + a13=(a13+a49); + a50=(a50*a35); + a13=(a13+a50); + a51=(a51*a33); + a13=(a13+a51); + if (res[1]!=0) res[1][35]=a13; + a44=(a44*a03); + a53=(a53*a08); + a44=(a44+a53); + a55=(a55*a27); + a44=(a44+a55); + a56=(a56*a35); + a44=(a44+a56); + a57=(a57*a33); + a44=(a44+a57); + if (res[1]!=0) res[1][36]=a44; + a52=(a52*a03); + a59=(a59*a08); + a52=(a52+a59); + a61=(a61*a27); + a52=(a52+a61); + a62=(a62*a35); + a52=(a52+a62); + a63=(a63*a33); + a52=(a52+a63); + if (res[1]!=0) res[1][37]=a52; + a58=(a58*a03); + a65=(a65*a08); + a58=(a58+a65); + a67=(a67*a27); + a58=(a58+a67); + a68=(a68*a35); + a58=(a58+a68); + a68=(a69*a33); + a58=(a58+a68); + if (res[1]!=0) res[1][38]=a58; + a64=(a64*a03); + a70=(a70*a08); + a64=(a64+a70); + a23=(a23*a27); + a64=(a64+a23); + a69=(a69*a35); + a64=(a64+a69); + a34=(a34*a33); + a64=(a64+a34); + if (res[1]!=0) res[1][39]=a64; + a74=(-a74); + if (res[1]!=0) res[1][40]=a74; + if (res[1]!=0) res[1][41]=a41; + a74=(-a77); + if (res[1]!=0) res[1][42]=a74; + a74=(a71*a02); + a74=(a09*a74); + a71=(a71*a41); + a71=(a04*a71); + a74=(a74-a71); + a74=(-a74); + if (res[1]!=0) res[1][43]=a74; + a74=(a09*a41); + a71=(a04*a02); + a74=(a74+a71); + if (res[1]!=0) res[1][44]=a74; + a74=(a09*a77); + a71=(a04*a75); + a74=(a74-a71); + a74=(-a74); + if (res[1]!=0) res[1][45]=a74; + a74=casadi_sq(a73); + a41=(a41/a74); + a41=(a09*a41); + a02=(a02/a74); + a02=(a04*a02); + a41=(a41+a02); + a41=(-a41); + if (res[1]!=0) res[1][46]=a41; + a75=(a75/a73); + a29=sin(a29); + a75=(a75*a29); + a09=(a09*a75); + a77=(a77/a73); + a77=(a77*a29); + a04=(a04*a77); + a09=(a09+a04); + a09=(-a09); + if (res[1]!=0) res[1][47]=a09; + a09=1.; + if (res[2]!=0) res[2][0]=a09; + if (res[2]!=0) res[2][1]=a09; + if (res[2]!=0) res[2][2]=a09; + if (res[2]!=0) res[2][3]=a09; + if (res[2]!=0) res[2][4]=a09; + if (res[2]!=0) res[2][5]=a09; + if (res[2]!=0) res[2][6]=a09; + if (res[2]!=0) res[2][7]=a09; + if (res[2]!=0) res[2][8]=a09; + return 0; +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_z(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ + return casadi_f0(arg, res, iw, w, mem); +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_z_alloc_mem(void) { + return 0; +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_z_init_mem(int mem) { + return 0; +} + +CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_jac_x_xdot_z_free_mem(int mem) { +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_z_checkout(void) { + return 0; +} + +CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_jac_x_xdot_z_release(int mem) { +} + +CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_jac_x_xdot_z_incref(void) { +} + +CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_jac_x_xdot_z_decref(void) { +} + +CASADI_SYMBOL_EXPORT casadi_int auv_model_impl_dae_fun_jac_x_xdot_z_n_in(void) { return 6;} + +CASADI_SYMBOL_EXPORT casadi_int auv_model_impl_dae_fun_jac_x_xdot_z_n_out(void) { return 4;} + +CASADI_SYMBOL_EXPORT casadi_real auv_model_impl_dae_fun_jac_x_xdot_z_default_in(casadi_int i) { + switch (i) { + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const char* auv_model_impl_dae_fun_jac_x_xdot_z_name_in(casadi_int i) { + switch (i) { + case 0: return "i0"; + case 1: return "i1"; + case 2: return "i2"; + case 3: return "i3"; + case 4: return "i4"; + case 5: return "i5"; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const char* auv_model_impl_dae_fun_jac_x_xdot_z_name_out(casadi_int i) { + switch (i) { + case 0: return "o0"; + case 1: return "o1"; + case 2: return "o2"; + case 3: return "o3"; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const casadi_int* auv_model_impl_dae_fun_jac_x_xdot_z_sparsity_in(casadi_int i) { + switch (i) { + case 0: return casadi_s0; + case 1: return casadi_s0; + case 2: return casadi_s1; + case 3: return casadi_s2; + case 4: return casadi_s3; + case 5: return casadi_s2; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const casadi_int* auv_model_impl_dae_fun_jac_x_xdot_z_sparsity_out(casadi_int i) { + switch (i) { + case 0: return casadi_s0; + case 1: return casadi_s4; + case 2: return casadi_s5; + case 3: return casadi_s6; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_z_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { + if (sz_arg) *sz_arg = 6; + if (sz_res) *sz_res = 4; + if (sz_iw) *sz_iw = 0; + if (sz_w) *sz_w = 0; + return 0; +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_z_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { + if (sz_arg) *sz_arg = 6*sizeof(const casadi_real*); + if (sz_res) *sz_res = 4*sizeof(casadi_real*); + if (sz_iw) *sz_iw = 0*sizeof(casadi_int); + if (sz_w) *sz_w = 0*sizeof(casadi_real); + return 0; +} + + +#ifdef __cplusplus +} /* extern "C" */ +#endif diff --git a/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_z.o b/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_z.o new file mode 100644 index 0000000000000000000000000000000000000000..1ffd6fd57b05f22d4aee0d6afbc8d7620d5dfd28 GIT binary patch literal 20472 zcmcJV4|LSkmB%N75GkAAMA5QUaasoqD(U1OBvr`-5}4L#qXwDLND`8aL?MY}DABSQ zYiPA-jayE&d)SIw+NGrvg$dZ`~Px7=>FrOdyaI#%+Si&nV}W&*jZ5Hi+<5rekioE!dsE| z`yuchKo5x1)iTaQHBO|O@b1XLPtb>-B15v5`7=@bDUbzT{8S;8J<<>KD>Ue3B&z-6 z@!ROHLTxSYAiB_x@{zsBK8n;ve;8&V@{B)|Baxl~C%k>Ysd*C;yU0Wo@}dZIi6)xOO<=M^0oeMt^pN~ugDu5QXki~&Sv{0&vt>a2~{?d6$hQJ@14!#Y8m{G;qm**m5ZoV ztJv3w#QZ*Z^8w>6=&9k+wwF|14%57={ZtD?-Xv5FoAIjjcfcdRi#q2X#4vgvxqBem zNaHZ_8d5JH^`w6tyDD-7t$Bqt(Ga0>095AJfR+9>5GuV^TpKxrM2s98eg#5r43s&M z-y-oAmp}r#L?8K=vJW8Z&(Sg`XFsw|68R0tYZTc{_!ve_%paiiN3iz*Ibts&#wzkJ zWucN9su4$^MM+`ZTmi4MukaeH)R9tku6TnIuTDgdZtoNEgi2AXQ`_Wqw z2tBybzW@_I^x!l8I=rz=Jm9CjlUE8faZxdTj0_@cSqMWOEy6Mzq!sE9h&KA`Y=LS_ z1e)1(FtZoWR!F}OkuLu}cnM4H5gM?+qII_uz!u{IUZ_N2V*$#P`@;jyyc2l(xW167 z3K}*CCZ7si`et< zSPv9#C9hRt{+oI2M6hYrY~~rTpFBs7gkNfUAW-gyRgrC`1Uajcn~oCU9ullTi)=Sg zZYo9UQ=9G<&)Z{$4%?ZUw;vfs3iI+)scF1>krBLK` zS9-sbT@|p^B`s$^NxP}Mw1l?)q)-?qpzvlW2%<$*+*!V%Eqcb7zREA>h-81okz>um zfGDS3a@=n2x%Heyn@)rdL2*>F!-|6KbEOZ!`E|$_-L@mCY@(-K&xgJ=#Lc0*%h`u&%~$T1Nx_0XgLHxf>$iW0|^ z4-g-RnT9dT`>hkfK4s2EHGKXtEyA>@roB9j9X6rqJ*2UqBz8=!nnUujk_^d*=InLd zL#;vmL(2SJIv@Ri(7EQc=*(Ux^T*W-=G+-tDf27>H$;ww?(dFst!|IleQJm|X^*0H z@FR>Mcshd1fbFXxa>vQ*G^$n49X34^rWLgh&mPEhoBqckFbA5dY0zPOVHjqw_OCI; z`813BNMf2nC0|c4Qq7qA9!*i4H0V6$L`WV-Oer1ndnwDz6r8&LKxa*!<%nQdVzHY` z!=cOHi|Wv-cerKPJj?047-1J;N*)0|1x=Bck-$|DbF>0K7@{$M2a<1N>dE30;X$9A zisL@UaWDL*q!|@Xq{oGzco?*krJU>yJ;t^_!w9SqTtAMZi3(+<3)98Pu7Gc&Y~%!4 zY}d48F3jme@e_FdI+HucUSlJlK*st2%hKE~eRt@qG?uO9GD-inFM}2jrAG&kss%kDbV$9q> zQ6_ELc=3Ry^4p9o5O+A`iErlOx-F+M_@y6?I*MHE@c7wNBS{(aM>b7>`Y=b#Jl*ugx)$xS`vqR8ahx<3!bsj34fBP z!3qBwap80sFk=bt9Z<`s>hOC|CnFeWJJq`dhGV-bbH??M7fhdmZv_euMR7(>_Ibjo6MI}T_#EU)FEo;a0a=mv2jMGf??tWEbRL{n zO($0nY__!Gu|jSOoHMVHGHI1V?HG_e&S_rwpy_oyWIS)Fbnax`e4oWt%M2d0k1qV= z<1Lu9*yC5U!FHS|z4Y;5QfQ@&F*A@t_lL-XaVHxyJLsju8lFJto~<1yl^4JHma>|% zTgv8@)!Oe>Q2quMz@u>*Z!eO^`r&u1`iUFQKKL>_#)oP%d(1^yUrw=^=*tXUa)zXb zUTk>Ipw`rNk#kRDU5WYmx9UXqCF(TOpWA8Yt!~e0yyW4N2n}j1D!7qgj_*fb|9#VLff#Qn~1puSs0{lRHirK5=Q=<`tD%=(Lm$nl$48QnHiQiRxNt_3KQ>26Z;Za8h2l3sP{D<~om98;!5L1~>92o1> z{$h-UYQEc5)3^^qqJ}P9n-5woKJD5TDMr%aKsd(Pneed815i}AeE6DAo_LTmE?XmW` z+e`jB-S&pi)=}F=Q?G?qe2nNQ>afs?-H776M9~)!#pt9pTKBdYUs686l3d=$(G=yI z1D@tQPjkMfxxmw0=xHwUv<5t_d7jpMPip}Rp4M=dXk+6Mr?h6&z7cq+#Wd2$38V33 z;z?HyqKTmOldiFN@Dbbd&v-tE=Q2FGc*t4wP$1jfcAPFwHl)&}+nSDymZv!R$Oh|Q zN>kjX$+tR9{?X|{6Yd5cE_4dCZC+Ql{`_}-=#>8Dg_5Ta`5!O+-Ig)Wyy|*VW~z+B;qJZ?gJbF<^OV?^ zBX(abc8&2P4jv)wbJrH@KGKmF*g z8{c|(&vPz+lpYkh^CiC^d*`&)TV5%x>c0Gq;)7+I?7RJioyYDgeWlcNX#Xbjo*tKX z(7dO~dvwMoq9V)p;K&(S zIS-s!mKFH=sPe4hdq;mMYyF53%g-K@RXj5*P?nW5Bg+Tb8Cl*LS(%|vp*YpCPV~_l zMuFHyj+u4MvR+}lyCP0<2Ziafbo-{D<;Ke7*335yD-<@ysNgz6!>}4*)-PlW-6AM_ z)x5ZDmoV}h1(JJInDryECx?-X3$q&x_2(WJ14PV(zre2efP-W00M%kw_otY6ao^3Fzi;sHrp z{d5+k6~0)weO)Eo%3D4|xX%K4&rZQBg;%-cuTR0l!h?6wr#jJHfr zkn^py$MY4d-^G^+uX6FPrQr7ouXD+ND+OOA+>UE&(Z31jyOV`@uMuwNtIhhMmFKsm zc8+9vj9-)dCn8UOiPK?&@J3%!+szbD-I zDJlPl6#PTsc7E9sCsS}fz+t}JX9eXwiVk+*9WFjrc$5r!13x%6;k<`Cf_=Y6nScjE&@#R*(i+2g{aq({okGpuc@O>`(za#vFi?0^m@8bU< zywkvZ#VFbUezqs+G<#NmK)CgfJ+J>%xb>%P$LqrD zL_eR~DUQp6)gf8@8l8fVNWnjqf;-@3f7N+LyFH^{CA`kXZxh}o+@94R5#H&N-zvP@ zCEqW6jf=c~RTK+Q#~(+USy&+S}?I zYCCE>8e3zvcjY#uBURfTNKY(3J+XrH#PZS;D@sqSFg>xUp4`fr`Sdf5emGe~u{{1Q z%Hz1f0gf9v!;85L|K^o&oPYCKavJ~Uvt&M3kzYVE`Gx$;`twcwth2~))>dTn@NW@I z78sqZp@4NxWBCGOypb`v%!^E%`BauE;JOO9t^%&5!02aE$oh*|rjYedWetT!zmZ`w zjms8tErqPVkV{Qv4TYvHM#lVdEz|h7*l6S5V%Ap7dZw|4V%ElL*a9xjyi?-CJ>ED*xbQ{&!6s=#fFqXFB z`bACuC)srEXm6`u+}<3!GhO8g`E)B-+Pe7mwEBYtYvge_U}onpys1#p!Qj zmTy;_zN=gQy5e-UxBQ6W^uIrrpHh5+;$v}sq(JSx2#=M&Kyjbq<%(ad_*}(5tN5*o zPgH!V;`9yJ*82^`CoBGl;xq@X{EroIU_(8?<6{qtx1@cb;9;<&O z&czgn7vizptN1L%Cn-Kn@e;+06faY}Oz~?J4=Nr}obGJao@T{oD!xSVD->U$c!lB* zD^CBiXZ8O;ar)zx<>TZ+M#FBA;uVUwD85YbR>j+7AtpVyDZWxT`Je8>w%*l>)BV%( z&5D0s@z)fetN2@re@^jZiceL1j9k>no=J*dta!2F#fq0JezkCSd!tHTx3^Pq-QFK5 zuI>4$;<~*rD6ZT4s^WfS=Wi9)emJc73?+YtTzttty1f?)cenQnC9m66skrvTQpM?S zS+?K5p}4mH5yf?TA5&c0|AgYYy+2nxPu06c@h>Rur9%UPyS;hB$v=7@DN$VSN3#{z z`{NyohoQ^*;al!ZuJ@6L6xaL6V~Xp2Um6Yy}tcKalM}WLh&g|&o;&NJ>@OMzog`kD}IgQ_M*X!+5itF*YS#do+4=AqJ z&-WGA>)T1i^?1voPDLQU>2WntalQUbQCyGDBE|K1D^pyrSJ`wpK%jAWJ09C#^A&GX z{0ZS?ryl3K6{oGq>N${tUq+iZ0_oBBpy`V1ed=1p^}aSwaqa&W#r6KRTyedBJ)*dt zFaM>u9&gVo9zYvxd$%i|ulNDQ3lx7}alNm7thnAUK1GK%1oE4nM-vp+<9w3hL6os} z<}0rEo#~3}{q$PJ^?r1d;(C1Ernnw&Ur}7|kKa^W@5}oY*W>D-;9std_&mjLR$PzgcEzide3#Ag4T?7^zEtrh#aAdERs2!KZ&my$#TO|4lH%kC=~mC%F1|4X znFz;Se6{cqv{)lpe?B1m92d9$<9CUR)0u!mk&D}ULoUABhs<1E!ecJpFZ@0i zx8ual8*8Wi`}ca6y#4#P`MWj@s2Gv?*%r7zZKQz z#+o`}p4`UzSiL8AetWwocX2BvNIWkWf9zu+!xDK7?Xf)k@lYg@!k-KqS{E&9YJsYj z)>u<6{sntVd#t|Uc1hpbvLv@{acf)C;@F*{vuQzXbTR%?BOUk;zI-T8E*s_26O{DZ zcH&9ej%O=QXJ=Xyf)>bo105a^#>@Ue$8f60t`k=6B;mAn>TofAj2I?J_a<^^ev;0= zw|pMTl1?!fF%xY4bhg&P)^Fc*HlR3-A06t%{+FzvWcsua;z2ya`LCA_zX(1lPyVAm za{KQZFcOhtDqJ<=y`$=`Ozt zWryp({*bAmPYkC1qw|w3Z^!>|byLYWeJn#5Ki0gdznby5%bx}29{W^cA6*U*4!9<^ Zm}4^WOV_I5%J + +#ifndef casadi_real +#define casadi_real double +#endif + +#ifndef casadi_int +#define casadi_int int +#endif + +/* Add prefix to internal symbols */ +#define casadi_f0 CASADI_PREFIX(f0) +#define casadi_fabs CASADI_PREFIX(fabs) +#define casadi_s0 CASADI_PREFIX(s0) +#define casadi_s1 CASADI_PREFIX(s1) +#define casadi_s2 CASADI_PREFIX(s2) +#define casadi_s3 CASADI_PREFIX(s3) +#define casadi_s4 CASADI_PREFIX(s4) +#define casadi_s5 CASADI_PREFIX(s5) +#define casadi_s6 CASADI_PREFIX(s6) +#define casadi_s7 CASADI_PREFIX(s7) +#define casadi_sign CASADI_PREFIX(sign) +#define casadi_sq CASADI_PREFIX(sq) + +/* Symbol visibility in DLLs */ +#ifndef CASADI_SYMBOL_EXPORT + #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) + #if defined(STATIC_LINKED) + #define CASADI_SYMBOL_EXPORT + #else + #define CASADI_SYMBOL_EXPORT __declspec(dllexport) + #endif + #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) + #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default"))) + #else + #define CASADI_SYMBOL_EXPORT + #endif +#endif + +casadi_real casadi_sign(casadi_real x) { return x<0 ? -1 : x>0 ? 1 : x;} + +casadi_real casadi_fabs(casadi_real x) { +/* Pre-c99 compatibility */ +#if __STDC_VERSION__ < 199901L + return x>0 ? x : -x; +#else + return fabs(x); +#endif +} + +casadi_real casadi_sq(casadi_real x) { return x*x;} + +static const casadi_int casadi_s0[3] = {9, 1, 1}; +static const casadi_int casadi_s1[3] = {3, 1, 1}; +static const casadi_int casadi_s2[3] = {0, 1, 1}; +static const casadi_int casadi_s3[3] = {0, 0, 1}; +static const casadi_int casadi_s4[60] = + {9, 9, 0, 6, 12, 18, 25, 34, + 43, 46, 48, 48, 0, 1, 2, 3, + 4, 5, 0, 1, 2, 3, 4, 5, + 0, 1, 2, 3, 4, 5, 0, 1, + 2, 3, 4, 5, 6, 0, 1, 2, + 3, 4, 5, 6, 7, 8, 0, 1, + 2, 3, 4, 5, 6, 7, 8, 6, + 7, 8, 6, 8}; +static const casadi_int casadi_s5[21] = + {9, 9, 0, 1, 2, 3, 4, 5, + 6, 7, 8, 9, 0, 1, 2, 3, + 4, 5, 6, 7, 8}; +static const casadi_int casadi_s6[24] = + {9, 3, 0, 6, 12, 18, 0, 1, + 2, 3, 4, 5, 0, 1, 2, 3, + 4, 5, 0, 1, 2, 3, 4, 5}; +static const casadi_int casadi_s7[3] = {9, 0, 1}; + +/* auv_model_impl_dae_jac_x_xdot_u_z:(i0[9],i1[9],i2[3],i3[0],i4[],i5[0])->(o0[9x9,48nz],o1[9x9,9nz],o2[9x3,18nz],o3[9x0]) */ +static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { + casadi_real a00, a01, a02, a03, a04, a05, a06, a07, a08, a09, a10, a11; + casadi_real a12, a13, a14, a15, a16, a17, a18, a19, a20, a21, a22, a23; + casadi_real a24, a25, a26, a27, a28, a29, a30, a31, a32, a33, a34, a35; + casadi_real a36, a37, a38, a39, a40, a41, a42, a43, a44, a45, a46, a47; + casadi_real a48, a49, a50, a51, a52, a53; + a00=3.3453634479321918e-02; + a01=arg[0]? arg[0][0] : 0; + a02=casadi_sign(a01); + a02=(a01*a02); + a03=23.; + a04=casadi_fabs(a01); + a03=(a03+a04); + a02=(a02+a03); + a03=(a00*a02); + a04=6.0368975005218254e-05; + a05=30.; + a06=arg[0]? arg[0][5] : 0; + a07=(a05*a06); + a08=(a04*a07); + a03=(a03+a08); + a08=-1.1116270017027866e-07; + a09=-30.; + a10=arg[0]? arg[0][4] : 0; + a11=(a09*a10); + a12=(a08*a11); + a03=(a03+a12); + if (res[0]!=0) res[0][0]=a03; + a03=6.0368975005218423e-05; + a12=(a03*a02); + a13=3.3484658136227786e-02; + a14=(a13*a07); + a12=(a12+a14); + a14=-6.1658244361114702e-05; + a15=(a14*a11); + a12=(a12+a15); + if (res[0]!=0) res[0][1]=a12; + a12=-1.1116270017027936e-07; + a15=(a12*a02); + a16=-6.1658244361114783e-05; + a17=(a16*a07); + a15=(a15+a17); + a17=3.3963490377380855e-02; + a18=(a17*a11); + a15=(a15+a18); + if (res[0]!=0) res[0][2]=a15; + a15=9.8084735444364535e-08; + a18=(a15*a02); + a19=5.4404333259807062e-05; + a20=(a19*a07); + a18=(a18+a20); + a20=-2.9967785627100469e-02; + a21=(a20*a11); + a18=(a18+a21); + if (res[0]!=0) res[0][3]=a18; + a18=1.0920100546139210e-05; + a21=(a18*a02); + a22=6.0570157695918657e-03; + a23=(a22*a07); + a21=(a21+a23); + a23=-3.0801331505514781e-03; + a24=(a23*a11); + a21=(a21+a24); + if (res[0]!=0) res[0][4]=a21; + a21=-6.0150572994295635e-03; + a02=(a21*a02); + a24=-3.0184487502609133e-03; + a07=(a24*a07); + a02=(a02+a07); + a07=5.5581350085139340e-06; + a11=(a07*a11); + a02=(a02+a11); + if (res[0]!=0) res[0][5]=a02; + a02=(a09*a06); + a11=(a00*a02); + a25=arg[0]? arg[0][1] : 0; + a26=casadi_sign(a25); + a26=(a25*a26); + a27=46.; + a28=casadi_fabs(a25); + a28=(a27+a28); + a26=(a26+a28); + a28=(a04*a26); + a11=(a11+a28); + a28=arg[0]? arg[0][3] : 0; + a29=(a05*a28); + a30=(a08*a29); + a11=(a11+a30); + if (res[0]!=0) res[0][6]=a11; + a11=(a03*a02); + a30=(a13*a26); + a11=(a11+a30); + a30=(a14*a29); + a11=(a11+a30); + if (res[0]!=0) res[0][7]=a11; + a11=(a12*a02); + a30=(a16*a26); + a11=(a11+a30); + a30=(a17*a29); + a11=(a11+a30); + if (res[0]!=0) res[0][8]=a11; + a11=(a15*a02); + a30=(a19*a26); + a11=(a11+a30); + a30=(a20*a29); + a11=(a11+a30); + if (res[0]!=0) res[0][9]=a11; + a11=(a18*a02); + a30=(a22*a26); + a11=(a11+a30); + a30=(a23*a29); + a11=(a11+a30); + if (res[0]!=0) res[0][10]=a11; + a02=(a21*a02); + a26=(a24*a26); + a02=(a02+a26); + a29=(a07*a29); + a02=(a02+a29); + if (res[0]!=0) res[0][11]=a02; + a02=(a05*a10); + a29=(a00*a02); + a26=(a09*a28); + a11=(a04*a26); + a29=(a29+a11); + a11=arg[0]? arg[0][2] : 0; + a30=casadi_sign(a11); + a30=(a11*a30); + a31=casadi_fabs(a11); + a31=(a27+a31); + a30=(a30+a31); + a31=(a08*a30); + a29=(a29+a31); + if (res[0]!=0) res[0][12]=a29; + a29=(a03*a02); + a31=(a13*a26); + a29=(a29+a31); + a31=(a14*a30); + a29=(a29+a31); + if (res[0]!=0) res[0][13]=a29; + a29=(a12*a02); + a31=(a16*a26); + a29=(a29+a31); + a31=(a17*a30); + a29=(a29+a31); + if (res[0]!=0) res[0][14]=a29; + a29=(a15*a02); + a31=(a19*a26); + a29=(a29+a31); + a31=(a20*a30); + a29=(a29+a31); + if (res[0]!=0) res[0][15]=a29; + a29=(a18*a02); + a31=(a22*a26); + a29=(a29+a31); + a31=(a23*a30); + a29=(a29+a31); + if (res[0]!=0) res[0][16]=a29; + a02=(a21*a02); + a26=(a24*a26); + a02=(a02+a26); + a30=(a07*a30); + a02=(a02+a30); + if (res[0]!=0) res[0][17]=a02; + a02=(a09*a11); + a30=(a04*a02); + a26=(a05*a25); + a29=(a08*a26); + a30=(a30+a29); + a29=9.8084735444363873e-08; + a31=casadi_sign(a28); + a31=(a28*a31); + a32=casadi_fabs(a28); + a32=(a27+a32); + a31=(a31+a32); + a32=(a29*a31); + a30=(a30+a32); + a32=1.0920100546139184e-05; + a33=-3.3399999999999999e+00; + a34=(a33*a06); + a35=6.8000000000000005e-01; + a36=(a35*a06); + a34=(a34+a36); + a36=(a32*a34); + a30=(a30+a36); + a36=-6.0150572994295574e-03; + a37=3.3199999999999998e+00; + a38=(a37*a10); + a39=-6.8000000000000005e-01; + a40=(a39*a10); + a38=(a38+a40); + a40=(a36*a38); + a30=(a30+a40); + if (res[0]!=0) res[0][18]=a30; + a30=(a13*a02); + a40=(a14*a26); + a30=(a30+a40); + a40=5.4404333259807184e-05; + a41=(a40*a31); + a30=(a30+a41); + a41=6.0570157695918683e-03; + a42=(a41*a34); + a30=(a30+a42); + a42=-3.0184487502609172e-03; + a43=(a42*a38); + a30=(a30+a43); + if (res[0]!=0) res[0][19]=a30; + a30=(a16*a02); + a43=(a17*a26); + a30=(a30+a43); + a43=-2.9967785627100754e-02; + a44=(a43*a31); + a30=(a30+a44); + a44=-3.0801331505514837e-03; + a45=(a44*a34); + a30=(a30+a45); + a45=5.5581350085139543e-06; + a46=(a45*a38); + a30=(a30+a46); + if (res[0]!=0) res[0][20]=a30; + a30=(a19*a02); + a46=(a20*a26); + a30=(a30+a46); + a46=1.4970303990827358e+00; + a47=(a46*a31); + a30=(a30+a47); + a47=2.7177645446042520e-03; + a48=(a47*a34); + a30=(a30+a48); + a48=-4.9042367722181902e-06; + a49=(a48*a38); + a30=(a30+a49); + if (res[0]!=0) res[0][21]=a30; + a30=(a22*a02); + a49=(a23*a26); + a30=(a30+a49); + a49=2.7177645446042498e-03; + a50=(a49*a31); + a30=(a30+a50); + a50=3.0257778596593993e-01; + a51=(a50*a34); + a30=(a30+a51); + a51=-5.4600502730695923e-04; + a52=(a51*a38); + a30=(a30+a52); + if (res[0]!=0) res[0][22]=a30; + a02=(a24*a02); + a26=(a07*a26); + a02=(a02+a26); + a26=-4.9042367722181935e-06; + a31=(a26*a31); + a02=(a02+a31); + a34=(a51*a34); + a02=(a02+a34); + a34=3.0075286497147785e-01; + a38=(a34*a38); + a02=(a02+a38); + if (res[0]!=0) res[0][23]=a02; + a02=-1.; + if (res[0]!=0) res[0][24]=a02; + a11=(a05*a11); + a02=(a00*a11); + a38=(a09*a01); + a08=(a08*a38); + a02=(a02+a08); + a08=3.3399999999999999e+00; + a31=(a08*a06); + a30=-3.3199999999999998e+00; + a52=(a30*a06); + a31=(a31+a52); + a52=(a29*a31); + a02=(a02+a52); + a52=casadi_sign(a10); + a52=(a10*a52); + a53=casadi_fabs(a10); + a53=(a27+a53); + a52=(a52+a53); + a53=(a32*a52); + a02=(a02+a53); + a37=(a37*a28); + a39=(a39*a28); + a37=(a37+a39); + a39=(a36*a37); + a02=(a02+a39); + if (res[0]!=0) res[0][25]=a02; + a02=(a03*a11); + a14=(a14*a38); + a02=(a02+a14); + a14=(a40*a31); + a02=(a02+a14); + a14=(a41*a52); + a02=(a02+a14); + a14=(a42*a37); + a02=(a02+a14); + if (res[0]!=0) res[0][26]=a02; + a02=(a12*a11); + a17=(a17*a38); + a02=(a02+a17); + a17=(a43*a31); + a02=(a02+a17); + a17=(a44*a52); + a02=(a02+a17); + a17=(a45*a37); + a02=(a02+a17); + if (res[0]!=0) res[0][27]=a02; + a02=(a15*a11); + a20=(a20*a38); + a02=(a02+a20); + a20=(a46*a31); + a02=(a02+a20); + a20=(a47*a52); + a02=(a02+a20); + a20=(a48*a37); + a02=(a02+a20); + if (res[0]!=0) res[0][28]=a02; + a02=(a18*a11); + a23=(a23*a38); + a02=(a02+a23); + a23=(a49*a31); + a02=(a02+a23); + a23=(a50*a52); + a02=(a02+a23); + a23=(a51*a37); + a02=(a02+a23); + if (res[0]!=0) res[0][29]=a02; + a11=(a21*a11); + a07=(a07*a38); + a11=(a11+a07); + a31=(a26*a31); + a11=(a11+a31); + a52=(a51*a52); + a11=(a11+a52); + a37=(a34*a37); + a11=(a11+a37); + if (res[0]!=0) res[0][30]=a11; + a11=arg[0]? arg[0][6] : 0; + a37=sin(a11); + a52=arg[0]? arg[0][7] : 0; + a31=tan(a52); + a07=(a37*a31); + a07=(-a07); + if (res[0]!=0) res[0][31]=a07; + a11=cos(a11); + a07=(-a11); + if (res[0]!=0) res[0][32]=a07; + a07=cos(a52); + a38=(a37/a07); + a02=(-a38); + if (res[0]!=0) res[0][33]=a02; + a09=(a09*a25); + a00=(a00*a09); + a05=(a05*a01); + a04=(a04*a05); + a00=(a00+a04); + a08=(a08*a10); + a30=(a30*a10); + a08=(a08+a30); + a29=(a29*a08); + a00=(a00+a29); + a33=(a33*a28); + a35=(a35*a28); + a33=(a33+a35); + a32=(a32*a33); + a00=(a00+a32); + a32=casadi_sign(a06); + a32=(a06*a32); + a35=casadi_fabs(a06); + a27=(a27+a35); + a32=(a32+a27); + a36=(a36*a32); + a00=(a00+a36); + if (res[0]!=0) res[0][34]=a00; + a03=(a03*a09); + a13=(a13*a05); + a03=(a03+a13); + a40=(a40*a08); + a03=(a03+a40); + a41=(a41*a33); + a03=(a03+a41); + a42=(a42*a32); + a03=(a03+a42); + if (res[0]!=0) res[0][35]=a03; + a12=(a12*a09); + a16=(a16*a05); + a12=(a12+a16); + a43=(a43*a08); + a12=(a12+a43); + a44=(a44*a33); + a12=(a12+a44); + a45=(a45*a32); + a12=(a12+a45); + if (res[0]!=0) res[0][36]=a12; + a15=(a15*a09); + a19=(a19*a05); + a15=(a15+a19); + a46=(a46*a08); + a15=(a15+a46); + a47=(a47*a33); + a15=(a15+a47); + a48=(a48*a32); + a15=(a15+a48); + if (res[0]!=0) res[0][37]=a15; + a18=(a18*a09); + a22=(a22*a05); + a18=(a18+a22); + a49=(a49*a08); + a18=(a18+a49); + a50=(a50*a33); + a18=(a18+a50); + a50=(a51*a32); + a18=(a18+a50); + if (res[0]!=0) res[0][38]=a18; + a21=(a21*a09); + a24=(a24*a05); + a21=(a21+a24); + a26=(a26*a08); + a21=(a21+a26); + a51=(a51*a33); + a21=(a21+a51); + a34=(a34*a32); + a21=(a21+a34); + if (res[0]!=0) res[0][39]=a21; + a21=(a11*a31); + a21=(-a21); + if (res[0]!=0) res[0][40]=a21; + if (res[0]!=0) res[0][41]=a37; + a21=(a11/a07); + a34=(-a21); + if (res[0]!=0) res[0][42]=a34; + a34=(a31*a11); + a34=(a10*a34); + a31=(a31*a37); + a31=(a06*a31); + a34=(a34-a31); + a34=(-a34); + if (res[0]!=0) res[0][43]=a34; + a34=(a10*a37); + a31=(a06*a11); + a34=(a34+a31); + if (res[0]!=0) res[0][44]=a34; + a34=(a10*a21); + a31=(a06*a38); + a34=(a34-a31); + a34=(-a34); + if (res[0]!=0) res[0][45]=a34; + a34=casadi_sq(a07); + a37=(a37/a34); + a37=(a10*a37); + a11=(a11/a34); + a11=(a06*a11); + a37=(a37+a11); + a37=(-a37); + if (res[0]!=0) res[0][46]=a37; + a38=(a38/a07); + a52=sin(a52); + a38=(a38*a52); + a10=(a10*a38); + a21=(a21/a07); + a21=(a21*a52); + a06=(a06*a21); + a10=(a10+a06); + a10=(-a10); + if (res[0]!=0) res[0][47]=a10; + a10=1.; + if (res[1]!=0) res[1][0]=a10; + if (res[1]!=0) res[1][1]=a10; + if (res[1]!=0) res[1][2]=a10; + if (res[1]!=0) res[1][3]=a10; + if (res[1]!=0) res[1][4]=a10; + if (res[1]!=0) res[1][5]=a10; + if (res[1]!=0) res[1][6]=a10; + if (res[1]!=0) res[1][7]=a10; + if (res[1]!=0) res[1][8]=a10; + a10=-3.3453634479321918e-02; + if (res[2]!=0) res[2][0]=a10; + a10=-6.0368975005218423e-05; + if (res[2]!=0) res[2][1]=a10; + a10=1.1116270017027936e-07; + if (res[2]!=0) res[2][2]=a10; + a10=-9.8084735444364535e-08; + if (res[2]!=0) res[2][3]=a10; + a10=-1.0920100546139210e-05; + if (res[2]!=0) res[2][4]=a10; + a10=6.0150572994295635e-03; + if (res[2]!=0) res[2][5]=a10; + a10=-1.0920100546139184e-05; + if (res[2]!=0) res[2][6]=a10; + a10=-6.0570157695918683e-03; + if (res[2]!=0) res[2][7]=a10; + a10=3.0801331505514837e-03; + if (res[2]!=0) res[2][8]=a10; + a10=-2.7177645446042520e-03; + if (res[2]!=0) res[2][9]=a10; + a10=-3.0257778596593993e-01; + if (res[2]!=0) res[2][10]=a10; + a10=5.4600502730695923e-04; + if (res[2]!=0) res[2][11]=a10; + a06=6.0150572994295574e-03; + if (res[2]!=0) res[2][12]=a06; + a06=3.0184487502609172e-03; + if (res[2]!=0) res[2][13]=a06; + a06=-5.5581350085139543e-06; + if (res[2]!=0) res[2][14]=a06; + a06=4.9042367722181902e-06; + if (res[2]!=0) res[2][15]=a06; + if (res[2]!=0) res[2][16]=a10; + a10=-3.0075286497147785e-01; + if (res[2]!=0) res[2][17]=a10; + return 0; +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_jac_x_xdot_u_z(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ + return casadi_f0(arg, res, iw, w, mem); +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_jac_x_xdot_u_z_alloc_mem(void) { + return 0; +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_jac_x_xdot_u_z_init_mem(int mem) { + return 0; +} + +CASADI_SYMBOL_EXPORT void auv_model_impl_dae_jac_x_xdot_u_z_free_mem(int mem) { +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_jac_x_xdot_u_z_checkout(void) { + return 0; +} + +CASADI_SYMBOL_EXPORT void auv_model_impl_dae_jac_x_xdot_u_z_release(int mem) { +} + +CASADI_SYMBOL_EXPORT void auv_model_impl_dae_jac_x_xdot_u_z_incref(void) { +} + +CASADI_SYMBOL_EXPORT void auv_model_impl_dae_jac_x_xdot_u_z_decref(void) { +} + +CASADI_SYMBOL_EXPORT casadi_int auv_model_impl_dae_jac_x_xdot_u_z_n_in(void) { return 6;} + +CASADI_SYMBOL_EXPORT casadi_int auv_model_impl_dae_jac_x_xdot_u_z_n_out(void) { return 4;} + +CASADI_SYMBOL_EXPORT casadi_real auv_model_impl_dae_jac_x_xdot_u_z_default_in(casadi_int i) { + switch (i) { + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const char* auv_model_impl_dae_jac_x_xdot_u_z_name_in(casadi_int i) { + switch (i) { + case 0: return "i0"; + case 1: return "i1"; + case 2: return "i2"; + case 3: return "i3"; + case 4: return "i4"; + case 5: return "i5"; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const char* auv_model_impl_dae_jac_x_xdot_u_z_name_out(casadi_int i) { + switch (i) { + case 0: return "o0"; + case 1: return "o1"; + case 2: return "o2"; + case 3: return "o3"; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const casadi_int* auv_model_impl_dae_jac_x_xdot_u_z_sparsity_in(casadi_int i) { + switch (i) { + case 0: return casadi_s0; + case 1: return casadi_s0; + case 2: return casadi_s1; + case 3: return casadi_s2; + case 4: return casadi_s3; + case 5: return casadi_s2; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const casadi_int* auv_model_impl_dae_jac_x_xdot_u_z_sparsity_out(casadi_int i) { + switch (i) { + case 0: return casadi_s4; + case 1: return casadi_s5; + case 2: return casadi_s6; + case 3: return casadi_s7; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_jac_x_xdot_u_z_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { + if (sz_arg) *sz_arg = 6; + if (sz_res) *sz_res = 4; + if (sz_iw) *sz_iw = 0; + if (sz_w) *sz_w = 0; + return 0; +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_jac_x_xdot_u_z_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { + if (sz_arg) *sz_arg = 6*sizeof(const casadi_real*); + if (sz_res) *sz_res = 4*sizeof(casadi_real*); + if (sz_iw) *sz_iw = 0*sizeof(casadi_int); + if (sz_w) *sz_w = 0*sizeof(casadi_real); + return 0; +} + + +#ifdef __cplusplus +} /* extern "C" */ +#endif diff --git a/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_jac_x_xdot_u_z.o b/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_jac_x_xdot_u_z.o new file mode 100644 index 0000000000000000000000000000000000000000..1fde7b8e4e1dfd14abd342bbdf246919a4508b66 GIT binary patch literal 17736 zcmcJV4Rlr2mB()cG*FuRQe#V7icfvmU@^Xge6=EZfgn#Oc2Wg-Z;T`%$t$TLiR3XN zwHO!B1qH|SV;t?+8LUog*Eq^j*DR`x315=>fe0vQ)zX3#sul|MWBh35zxUnyWphfF z&$e^cy6>LzJNxW&&ffc+d+tqco>_b8X*oHbrW|jYH&jyUc>}Sbe2|TKpYrm(6aIwv zQvT6jr2N92DgUB@8zvn0H$Rzk+z*YS1}T5`rj$SD$&`QfrY-*2F;6Q{=plii{cIKn zCHs=T=``J*Rs1ocQVwuZpie5x?b)4QLu)b4Gl7O(aHY;w$^pW1ze6xS!u1 zi6MuI!&HOpO_3OecNl7q;=UaslOs}>;Es>`iIEWqUc#!@%*3z@_d)zuQ0uR_^OLOQ zLeUmSAE7cS)PI%Q(P^#K5c$s9PsP901!At(6yCS_%7hiK` zeB~~G#VEKBJ)9*Ygy``aKQxhx$OJtjBLc~tR2`TB8O#;UYy9M|>E}iId5(TI(a$gN zllaY$a&DWz=uT3W*mV-{ld3#{FeuzjI{G0YmQQXuUhOBgVkY|)`uXQ#!dq|!gGD_H zy#%2xe*W~33PNHtu3#pR+C-`lh6S4;6{g?_(R3&{K7lp~cqhL;Ld{Wwd~Ku)zmG(E zAcV|KkpzC*BaIq3Es+pLd>CE4f%{gj9LJS$1PNRxb4b)g#z86Cqbp>f0>7#^gl^D$ zC)6M`67Tsd8pg7V8mZCjNF9DP9`>M*mdH4Ww?x8dfGN-YO{&H(+=HKjT{Zrs*)-UnZugU~p*A@{KNx=U7&3{cF{+8s%x8da}BQdUfIHwOGii~%eZFme+F$g*m}p-T7>!cU>@ zbzDI}`ib8|$pB{jJUWV*j!5|)$Ztm7Cb%6d?daR&@lg=!BLBUB>j_#VJ(x|(|MYD3 zeL9jLV`hAfGTCuE35$zIKm%dF5OTZ~Jk1&k168PH1JO!+79-t(k??HL;PsRQZ^}RG zP1Ip_m=y${!7!hu%pqj@aUFzYpawQaOUWnqr%?9uyA zhuA`tBmYBOR}7Cnz6O?qD3+VXLlEG>>fk<-1rZMQI)BANY*=IwM4T?D(Bue_KS50` zFD)p8tp-t+pb8Yy%HZk8*GQZsvAZT(u-_tgQQvXYjEM5e>;p05I>w@1So7 z?2{(jvKLqb^6*NsjHHl%(-@AVg(r`}JGT6mQ%1NC)%*FYXoIF1Jqw+9Rwno5WgWIb-|O{^ifC=Mso zi_hUMJi!{-EuoBqx3PnB)xi+1U2`5*eyCm?1EUtcf5pm!*n5-jqw!6VE`P=Ir*k)x z;biP^6vMPriSIIA=D40)OAoxjvhg(w$?7lS!dgh4IPTZ~j?DHv8sp&#B!ORmMU}wb zFY$%0ccV=|;9lrrA4B+maj92#!M5+1x(}W5yG|LWn#4QR^mG^y-=TE@{Hn2$z|%Jq z5AeTu#6O86PBQD$${z>kHHThar5RFJcSZJ5SFja&g0xPSR(1j#2kn1}Cn30&k1z3+ zZ~80l8p(#98I9k%F>)W8$8UWkvJQ86;$B|*>bZrLFxR~-gD&7!JZNI*df8Ex*vJi0 zc?^{`{0!qDgz?jKrh;p+SJ3(>+>wg=%P{Ez1oYein!t8{(%MK5I#z9@owgrhtS@xj zpSK?fTL!wuwmP?lo^coxtqz{nyCjR;ibyV>VlagsLVjpuaAKqJQbbA-I}Pv4ua1Ba zVlvn^o~QMc7TrO62|2_GLJ6CCC00)1@2Sa66jpQJ0OioaToKq9^j@@&vPbc-XoUaUdGiQ%9s~-kzMWLQ z0*@roKDm>u4}}F=(K5y@>z^Y{^q^(ekn@rskQhDR@$@fz9^%xFV!Qq+Kfkw*+F=Xa zPEy#d>3q=BsKW@hxcHhcq3HmxB??1Kdp5s6VDgy9b12u+gKXZr=$Ov0czsCH%K{xz zB!Fva1D%J*3no^}3uR>&df`=}s{tI1ccSqCRPqCM9v?#a1j}7GYV%hvfs*)|8M$>0r!sBy{B9`-6jcFN6@kQ_Tb8nsGz*i__jgyg%_GomoulmAiv~ z4%DtLPbr)6yiZ+QNs_DEP?o6v$Q36dEw92$lys$HEX_v0a1cjSl12M6g1#lf;W z2$o?s-X7Gd>sx8#y6p#pTDaI1M8`;yzVfc7~>}fpzmA^n1yB}>S*88bm9#7Cd%8j^vn6tyQZ;N_u#a>&9 z*EYp#EA`sSyw0fCS?qO|c%4(w@Nb$%i#8rS@>Is*gcD5|Z~pjh79^6P`BFVufFlq0 zkHbm7?bFpqeV&0U_hZOGG?T1;JO0l?;#^!`!c~NeoIw|*k2C&{D^Ft>u8o~*HZoS8 z@?;}T>OU$|Ud!YwTPFM1GKxsYs)Bn(?E0ml-;UY)t;a8D{^C!j-Ls@G;Im>ICaiq@ zYa7P&-?sU!!us3B#J13F^J`Ud?-SlHaGQQaKW#8L5Dby$9omT$M!6UP0FP(O2z^m4L?t=SH|Mu9b$G-o| zbw6vqyK3UVjN zM2!3IlkxZI_-~ML?|O09#1~GyQ~mXzz?v)g3a<&)o6s)xY`fp2wZ}^c@tr z&rAK3{2djY*Y2*W>z({+<-zJcy?1Zh@rPAayEFDxd#n9*K59?3x7y#eui8)TsrFX; zsJ%pgpY-SY^KtQ)#$%P(?{TqTpV*^H?9nIl(|J{ieXGR2YOlw|Uejd$RWkoRZ`O<% zUkMk^nZLLry*OMvrKq?ldST^aE*38J__7wv_!zB-5Y$9`hEhZZ1D zDQD@GUe1zG&gaLTk=u&}NNvV*jpuz??!8o_sv?A`DBXwfB5*hG%s}9 zY18w@{nLo)dEr|}PR}b`JE}S_`t8$e@+y~~ae3acoDtvq6^ig=dnT<*^Ls(-62fvNbLSo$n^`;2QT?^kHdh5O(Z(f`@`U=1V->q z{O2_zJbh{m)lEb;2Ruvcc6{5v9Fr-E{)J%RMQ{C44SNB%D1haLW7 z;rktapYSN(hbkW!hCd>F!H_8D9u?l`@P6S-9R7sxE{8uYyw~BohT;2!uXE&I9flti z9{6pj(eH%oKA|-sAA)!s{HqT6nL+Zxi0=@P8J* z&f)*cJlBh#G%L@$TjbX}@;?>6)8YRmyx-vu3qR)Yp9|mb@IK-CJknupW1jnG``c3@ zug@cue@?hQ@09Nq9>l3q&wE99jPFDIfbcqp9}(W@@T0=J9R8N@9*4g#yw~A5d;lEn zt#kMp!rgT=mieg6xlw2EDdAey`#9lr{Ih9-aQ6fe5w2$?ZF7NeeNHQn3U}9axp4g^ zpz>3N^G7O$xK9`UdBZ3h2mh2e6g+wc?i3yqc|8Z;F1${-&f_uRjl%gnjQlI$u8m3L z;4pmuF#PZ^{0(p#e@HsfbMrVH{)vYjK0|m^xSoMKgvT8DJB8Od@>_&AI{XdcT@F7B z9npAtgzK4ls_I#`T8;8(fq&xwZY9hB)ms>LU;iJlg6{(k)I(v zDuJcv?QaP0a=6#D_#Yb4wD(ON*L6TBe~p znvGCNHbPUf5h~6`s5~2?(rkpvvJtBAifU&>>4$UWlq;j3FVoMI68bIX`cnQaE@r~K zl$#V!VM+ciF6CNQP|CWBS*9##RmqZpOrW17OIY(1mMme(DJ)sSI?DnXZdJnhDXh69 z_~lkpSX)V;KhPHR$Q{xTR}}|McirQB(0Fa>T^%93TQwv6S=SWj6n zykLUdwv6S=SiYR)%UQmh^_R0uIkzfj{pGBuoHdkls|s#a!SWT{wu0p=xNQZus^C@? z+=_8!(1;aO280zuB!m z_R>uql>O)i4NdLsoy`pkTNh>_+t$&R&QdhBsI@gq(dGrM&DV7QIMXl|vP2H_o zNWu(@T2omFwzOs`*wK)cJv$n*n@US-s%de1I!mQZ3tK;iWOgIy?rK`p-Il&FJH;94 z?3CZox#+qqtWRPM^KVSIc6*+lbLiMtjOzpW0A4E3KjL)OQm*HA;w6Uv2zg3)tb6lu zsXTqBp+vkHm-0;nkw_0cJC*Msh(w$o-O67lh(w$ot;*jeh(w&ebt#`9M>XPf##3Ht z_-72)?{y?kXH=E{p^^Wr;XgC{bA~@;IDIcwJ&ze4HhjSF^9_H_@QC5>7*0M?{lWWs zV225Yf0ibMM7MKssr*F4=`Z@qgZJY=p5ASh>-z{bn`C(K{u;>B->OvpMx%eS;VTWN zziX-d3&P#_IbpbspFyLi+~}cmIwh*M{=L(1>&Lqd52KBa=K;g5fB)NXYo9*Dt>5WC zrzp{QF2beyPZ%CGd=wb~iR7&x^9{H5|AOJ?qK)b=HaueZbi-}DebsR5$N7d^e=RlK z`ggN%y3IHJzF>H>;e$qxwR7+9rgz@_6k$M8zS zCmBxft}0(=xV7^l!@pwWml|&MEH~WR?K_6s=g32bTYElj_{FB*eTIM9@PmfavrOl8 z%mQ8BZkHGyk{4p)^hZ?H6E^${hEFy8i-uorc$MMT7#=gc&G2f&ml{6Z@KuJ- zF#Jx#YYcx;xEohT47YJrl#9klWQUo!)DGo_+c>%0@Jo&S^@h(f{1(G)+}>_@+{pjf z@XHK;$Z+f5&4ye5?lRo^_m76t-==iF`cEHZ2kXbN@_~VP%*c;3-1;$M_;e#5HQf4f zs^Qj;^9=txqvsys$y z@R%9ry@uO3S#P+FlZ}SkI2kb9#>oN0&oljwq|FzJ>^Tva+HIq7@_UQn=g5al;%$bv z3nxAHeIY#zzsvBi8a+QX{O=8a*zh{T`wah@;ZGTUrQv%Gzsm3fhST4G)ec7upJVv@ zhF@)Xgcc8yYlo?ZQ&ZJH%W%pnUuC#`@A$sq_Pypl!{-`3>kX%@>fdI#eQ$c%aQl8V zo(6$Lez*I{G{fzF(r&ojH-0Rf>|@8d!EigyJ%-!3+9=;|NzVveI+m;n0{PtzA1iu(>u}mzDZS70G7j0sGJL#nX_n#o-Cyme`bUa9y(3UkzE1e~0B88U!ov>V zCp_Ztjlv5ZzEil`L;Kx2jDBhEl{$K2QjR;kPRjKTZ!`ANTa4;J!d}`0(0Plx|&;_KI4X(oJ5` z{O)eAXi+B>NW8cR-^^JkV41q+?leB@iy#X0M}+3ig$r9dpsJ%Y-CBfy{kyO`-PC-Y zl)v7wxTtGUXIJZ@^o^pkbwNXF5k7&6WB-VB7L(jPVkfN-w`h^SQ_`Lg z%a9F13gBlAKfQ(J)bX}_D-7)IQ?p!xKWE9bwfJ;()Pt@OFBz%*`)nXun>9e zU+?rTMtO=io5J)F0co;R5oS8{6GQe?_6yUY7%40rk^RU=uKlh6BN5kJb#!<@qGzy8 z+FtMU%so~6_0oR5Gk*2I-WN#wsO<##Mscw2pIR-`;Xa97k4u@#(0#22h=s_zvbTWI zE3-39uUFcKQD>9RTiKn+kWRP#dTD>y>CkQe1GGJr|6=s=g0x@SlmF=4sqJ-or|O&9 z-9aDSkhB!}?D75m|G&SZ zo!3>bURAw%_0H;UZq3fUqEA$mqW$_RBb6|uC{CFX$O&VeO1g49a?Vr|nKoK0+(TM$ znLui(ViPcNv(T-R{@~~2zk2%ULju-PV>y=BCh1LS-me8-OO56FAr>fug|OjTKl z4y`g$&ps$aoXXWwUT^JSwwE+7+md*Y&y2ltq5|_Y zlP-Ow0(2iIVagx%`>c$=9lWjjn}0+%4S0C>Yje(e_6;KS;Y!0ryawPp`?#~CZ3`@x ze&-h`3HE-`cE#o#V2`^*$-Sfh0K5C%sJ|*oOw;XsLbpf9oqx_raVwRq`N>x-UGe2f zw!Y3kMcew>PduSk={q*s1{ITB1-@M8Hm7s&`ASmE>T{G~Co0Y;#TngHdt$Uhv8USm z*dY_T)}DBu(sYM%e;?B2Qe5e{sH=>?H4@ioT-mtjcZEn47t$Pg{~M&^aOFyPJkl$1 zU4`pvToZ6j#5D;Q{oJ@bxUMI_@Vf!`UISc!d$LiX0jJ&iaWfTH39eFHH{mM7MZa>9 zC>2O6aRnq?g>)LO=@OoSbQZ2#aLvJWE3Vsc{SH?huKBpM-|e{hJ+3=&g>c=4s{t4N z?#6WwuKNkV??GID#Pw$a@LP!M5nPWFfFGJsc?{PgTuDYwt40 zH4V?5*7k<`e=?(Mp5AU5_jr8U?`Mr(cjv4})r-CJF3Wp-^{rj^z1@0=_ocXoS+{?9 z;H+gGBUWc74)@;HC;Qc4?8SR04_o%}xR(n5ym`ZiC3`Ra$M0Ww=(|hb`*2_9@GBl` zIzR4<@4x%Q##z>F?bnRD{IvTnTGKEoD(%pDYkV&_Mj_uh?%eG|9nnv8S?ep)rZhk5H^sDc=W}G@acGsqN18-h(!QTe!8f8@FTS~O->9~{yJx*%eQ(30@hg5c`jWN&?OT66 zW@_t{)f*O7T{Pt%$2b1r!2GT`{kqB?tiRBio3P-5nPs1E|J_YL-*`pxE9yNrXTAK` zhmW1|T=uf|q@Yo?Gxnr+={8h)q#-tC> z{5}2(qZH-D9&)Hp^@P9exSsKe7$ke5Z^Yo!6Mk4Pa%cAGnZAE7{J}=C_n`mXmY(@M z(+hq{FZ^%pMbAfj;X~s?PxW2i3;u$bp6U1WV&_YH(evY8_}tZt{U7dy&!2nYliG_u zSM~2%f0}pfxXi!KUie?w3!foiU^g=X{B1AxZ|bGJu0WY{df;a_!tU{C%~)(fANz2JY>3qI6K`_jD8liZVgsh7PMd*0DYy-w|gzN{De>w2M2fj)zJ zXqV4>p)VWQbG@GJMef5`ES;#Fpfs-)LTG(Jzs|k_Z{H%|i$$WWKUv@lHVFjO;9t%u zl76#*b0z*)@dCf3MIbajv~VN2Hd+MWhi2lR3mt>;PYGDVhfWaqwg&`$xJZTPlRmsa0G-BX2K&hgit`#S zuABpT^t+D7ffJNkS`^`Tp-7Y@Y0o^VzsBbg_Wu)<08J?PY4*G-R>-ZD_0sSorT$HF zvG%zrrYzuo3VmqNg&#ovZHJSS{(&_D*7!`Z3Vie5Bk@;CJr${^){gPgPue6OjsH~1 zzl|19_!YuG==bjNf`60A{vXPEwaR+QY^CfZNpI4#Qrf50vc5KH2aQh>2-qK_ z{WX7hPV#A%{Sn=nfBDirc2hsimi;AF+D-F^fQFa+HMtK+{hLhndP}x%?L;BZ4m$ds zF2@1oDFJJGPU3M4#w!!}l_F8zlydV-^}0~nxzW_`MnEyLgI%_x?8Zu8*)N*@E*NY2 z+ofOGO@5Uk$A{FX1-)D9^Qjyk+N9kye6_5XUDiuhSGiBx-zn*}esPlQ7quooyjSYk zYSQy0Y3Ev#zpa=4pqTt2UGsk_P;2*Asi#xwsp<2I>=y+leJ+;zEHUZxH|cN9vRyPk zc}ez{Jd>Wk)%?VSUnKQ+%KoeI3CM9NAm^`KY5(0)pAM;y=09i2d8$d;L95r3+W0K_ zKaTSBJ0$JUEbVZUKr1gvJ(rmLd6V?3W=RP3__smYx#@lZYyJ>_yzs*|$w$+(S?b?r z(toA&hh}Mi&3{gj?b7&+;6Gd>idXt)qx4Tr|Jl<1snX8qR{T4p*|SB!7fbxrQcp$t zr>5t7l23u;qqWO4*C>^~1JVD8m^-7ZX zx1|1dX-~~RN6YcScC{c(l6JTWh9ds81p@CBi85REqej_}K*qm=(w=RyVKskSEBQA| z`+O=JX8%CJzd-UqasK@Ue@U)Q)(gWX|At9Bx0VSw9)3)}ak70=WqWCS9+CW=CjQht zdAybUH9a>;yE&!Z(B1gwJW+M%j*<9Xh2xjZzjyzY0iy zPL=+w(GQdO4wFA*O8*RO7W%oRo^MM20h8bEmi|*K^+)sZ??o7v?eL(0HMxJ3^;%-8 z*I(rLlloL7{UkXac1XX{`g zORIzastM!AlvP&vC-|n6`9)sOIo`qGY{y?>^St%@GP_(eJIw<86x;hBUR6)z) zKvii)u$c7hE?cUI0^UHGuRNbhmj9kF;Lb7yu~#Yg~8IwifY9_1FZnF z>L}fs-e4sfs@PXk7BuKkS{^9#7Ww={-wWDX9q?6Emj-8gOM4}$I85I3$|_^6jS|DG zD{Few%3I;3%yfca_gWg$3Llai8_qrQ-LfNAv zJe*Y`IO^6vjEN!&2z!N{SU0$Vs;1}aM^f}!853D=RWXqrrCvuexru8p=85e7)VHMX zd)Zt3=q1$#2ax2VH_=dpR@6qNqL~%G^3uZUaL4wVT#x!rp%_`TLWSsz!73kmy`fx> z0(s=4w$+N_o(Gk13r1EZP~|VIEDzKK!vm)Q<@FlI8vl$^W4DUrGrcre;w>tz#`qH~ zo#mtPL)U=@BCRWFdZ9|irBlt=qSA70l+{Z!j%<#R+&c|}NB5kmWtCHWW!>=xyY-Y= z9X6A3vJgY4UVtoULSdjbVPtuU?Zw#LTTIRcH|w(%G%oU9*`2V;4-4oj(S0`=k{5eq z58fD&ExJXx$;Hegycx444blGY5~vXZUSVvn4~m981sx*buPE|Y6qfpX5YFZat4}j? zkL)q~z!v4cU{z_MznWzf`wD}VRS^?A&r-e9b+Zi3$C1M2vY^P1ZdEx`&x0AvC~AE( zPByj~&5mkDNe33Vwv-?iOudltfb7L;syBktic+lBde7%Y;bHO%OZ@}du^~t!kejyd$Udi=@A<`g8E3U9zlF07lg68$l_C6OP4 zV-oo>xF(Swy>klH4>weq>~ho&N}P{!Hzi1q&fkbu+X@`j_`=K&KbV@T$UWugwb6Gl zN3D)t;^>9+wnyv1bC0n<@@#zMp}*$n`(pJ`^~9seEK;efT-a$*mah`Bh02;!W_h(JOh550q-{8p`iXNFyLhfkMl|l_=!3- zp8Gc7Pcz_W81VF5w)U$v;1e|@(mM?JMgzXlfT!o=wci2*-mW2$HW~1>2K-_JKEr@t zV!+GL8|N-H;Ij<$%?A8s2K+h$-eq zvFg7(10Eq+{pU8|wU{tv78vl<2ee;_0k6fbDKB8aYki6EGYt4Y6GHxK4R|fKOj&mr z@Yia@NE;1!gqro=0s|f)bp6+4z-uvN%3N%~Bjl_9mKg9)=vYNrYQS3!_+|rMi@j6! zIs+b|Z2i}2z#HEo*<`>Ybg%!~40wdN^`C0MYtI5uW`_Zf5V!u@Z@?qeAO4*k_FFmh za$bx9pAe>2YzF+P27H16|3?GfZor>zz$Y881StI{2d1TCIkL? z1HR3GPch(C1O5U7zQcgO(171>z)vyY6*=$I7?x_l#~APv40xLXe~|&7V8B0Qz}pS@ zu?Bpy0e`UppK8F4<`7YqEq+XCNO&phC&+Zp4kx8&fR64(6ZV4tp5 zSKaEERjhbd+Lu6gb|rm<#Cn!O^d*$j+5SaWS63s$v=r-Xe^-ZT!_e8jLWgM~-`W10 z4$}g@v;9#WriFWF`@K3$3--?T+jN*Fxz6^Rb(j|Do$WpyrVVRn`&Bwj3-ZqPOdX~L zcW3)h9j1kLXZzVYObhJJ_CY#K3+vAIC>^GSb7%YBLt1@l!HoLrFfEi(e;uZUFzT-{yIzxT-0BOX<>`{>o6^7QGXq#jW_DA!?b`!{dJfY zuBg8b)5aV1*I`<~qW(I37Q=fFYW24>{DltF0v7exVOpS~{yIzxQ`BFFX@QCQ>o6@K zQGXq#g(K>(!?a*T{dJfYim1O1(?SsS*I`-!qW(He3p~_chiPGl`s*+)&`^IJriB^m zufwz;L;L@%)t?qzXn!51jXdhF!?eIc{dJfYN~pgM(*g;% z*5P!9eL74J0i*spOa~FDzYbr{@K7Bd!SLBSOa~ySzYf#E3hJ-JqZr=%lU9Ex!(Zrd zCd2RQ@Mwlt=w6Qt%LH#T!JnGo4@~e|CU}hre$52G zWP+bI!H=8ZhfVNl)cz)TmkHixflz= zo^OKZnBW=>b~R+o#J#J*Qboz|{lR!FW&ZG?SjzP8YO|hIi_u3}B~#GnFn!u+N(LWt zg*sd-c8_wc*x%O`wbu3SpJTFa+^qpn9;T|Sj>pN>qaPv&G-L;UnVC`#s}%!xUngP9X1 zUgNrLX9AW~u6l2(tG?1+I>4< zWe2*tD9h;Ln7)e&;JEqqmgV1z+)e1h#E2q)HDN+1i@pk;6ZB_MU6*31kyr8$)3N}dUr0? zeAz>HTQe$A??BP3Wz5)k3ttDkaI|{Dk zf=;EUa;*@mm2=PGUaY12TJfq-5F4&`< z$y&L!S&rj0VaK6N8kebs20fPR_)*pmt#igB)atp8fb@fg_~TKS8YiGG*e`r!h@RIM zdC-jyywsU8J4Vmm^MIm2ml#F8Ugo9hd0!$AMfnU$9VxSi>Dik^_At^}JxgZKkl83V zgEHVft3aoA{XmMvuE)L3U^QiI=ZsuOx{m%Q&?ru?u_|f{W!GC$9}bth%TX&BKg`VL zF{nNw2#<>(yh9Mqk%R=QHG=T7g+`8zg77*?NT8Z42w#dITp$Q9mxKhW=Mo_rWQ1OY zOOTB!j3}b)Xqnw#&mMr|Xc)b}qN@A2dY%XwO+v;%P|>(Rpt@WTrbiGi7KF!3LITx4 z2*Oh$2$u*#l1m&3RLce7zCRo7uv8HCmxKhWSwx8H4bb(!;(peDfTAV~Ca**=X%-xBD82l7Tk3AeL@QY)jUDCmq%6_fn5!EIW`HxFC-y>>aPT0Z3JPP zAbeL65~v>fjt%g)2trj5E|Y`=svii#xeQuqR9>GMRnW!OtmSjSpI#M#p)tSshHrgfkB9d{I%+AxZi$!)G z3M;B2vnS};6Gip}?&V*6OU4mXV3F(BVot2dbltX^X3OiY&&ga4 zDG&8XdFa-MU>;lBH)5z&7sEfTXU&5OVn4FH1UK!^Lz`kg-=OmmwHj?Ww3!b^iifAGA8fPzRZa{dGle)*yOBwXL{!Fj+$wh!c%kWr>5uDUpRl?+^b!o_p*m}VNoIE z<%Bk8g?`L!uuQubA9D+RlN;LZ3hhxRLYmCdaG7;2QLegGF`yj}TJ=O|ojv?e&1Rc0`$_go-GJ?m_F5a7BhsPq)tB!zC@} zFMxtrj<`ZUWrn_M8EXR??sHsbi-^zE9g4Evjd?_AnADZiFeiHGKG%vb`>3aPq9%o_ zGh4FeQvtBJDb>|5gaKFmxMWxTHPBwSM@uwmrL;u%VTJ3Oqg}(-)U<0tGV6Yh3dUtN zOmG-wyu7!o3j$~x50zTR#Bqu$irUfo%T>5mS3whx+N1^tXsma<_F6Q`wNJ(343p4f z7;j$12n`=k+jb8n8T*l_*P%WJ2Qb(x^b;J#Wmv6YdUDl|#VVo5aY=6IYgb`Ox|$2) z!}TtSn!oGTb6xcy(Oua(e*@}_mT~E7J0Ui!oR$FJTIa^Ddz0E{ZhZO%P7@WRu7@){_&z0R&gF4$gJht%D~xY}UbNIMh#IDpzO>hQ5{^$B|v? zZ*dh{E74o<7wf^?s6B8&+{Za`Zd*gcoORIHRO_JMI<12y=c&6oy1H_Q@2Xh`J(7}z z1fgBZokF{`t67Vr>o`f-`35PIHiw|WMWjg?X)?^AUM)1iSkST~R&RgklLzVftWS|a zpCMs=ywGPV^r_5KuQ%wE(F$3pv9wknRv@F06^LC($>7-zI~31&nSp^0>&Y4U5^qyi zf)y--g|kT?H}nazKC=z_M2Gd64t;KgK4bILxAwr>$v*9NoSvyyMmmc;KV4<^9~wWj z!NV0A=IHK2!vpAu8*t7Ehr(GcHU^;|F|c4z`4z3{?5_D6t@Ao?PFg?A;R?-m#Hf#< zI-OQq+BLKYsn2!90La*fk)YE$DCkJWU)07`80Wxfgu-{Dcf)uS90}t?2XaF1Xyenk zhUlxu*ZYb=ih?v_<9VKTTXw}GI-6<_23PrIiVGh z@G@BBgw|mCRlBG>M$Ze8h+N%$+Eg+18mY#3sM{lCH-MmB9AQ|5$(rC%cJ6ZR#|PhZ6T`o7*G7zHj=xajm4C;9^sAZN?%QWiz4}^^MO6$&F-Mdq( zk%i1Ug-FnN#=h`(Fd_5eFd+1!+5uIl)tBl8Po;u`nfH%cIqD3P8p#1|`FtKA?CfV& zqFEkprgD!;cBMKynsC)EK+_V2yQE-(jw!T2dz}S7S)maQ)Hngxg2gC9=_Z+`Qi}sF zK&pX0SrLPH$tYEyQJ$j)iV#?*;Jj%bJa zD3ko4dY~ei`6{GB>FM7XcuO65yrPyN-TR3;#O}VE8XX!!fdP;}RX|Oc{b**tM?C;$ z%LkGz?6?!d8U@HuX|&u^;E>sJMGUQwb3*;pT$Dm1SY8A^C)DCV)1P*JMz#&9IFNyP zyMmYy8)7H_UNfEA02}Dp$QrF@<-!Nl3S@gxDuHMkVv~^zHu~v~dL{C4a+B$%fhE(E zHGGJprh&xAV1lt>kWqKGlWDuutw@*CdQBaGJ7E>FL5qSRjB2iq1~01fs}1^oZhGABdHqTBV_J&Oi#Lw&YDLu?*$gdv3E zsP`cMS~7|-0k9~40aIbV`dqC@=h#`24_iQJ;hQtaiHv zhb@vH`P2erP#ZKN6+ZQk?M9y>Z~6|5QO))|EbC~0iB+On0D9qhJk-An5;O;zAI3n1 z0yNYIXsBO8Lw$QX_L?MaFp9Mg=JTG|M9eC$-cIe@kalYwB^g`bFY5Q(uzIwCA@46H zvlPQ3F@g3K@_ZO;z^O4=^)qOL$>T&88ai4XBNTSkHDkbn%V*E;n(Ib;M)YGGthU35 z7$zHHr;~X02=!8wB|nEBOA6#)tLL-FoCKO{(L*7t=6UKha3zgf7WOqbIVSHv8>Q5* zQDZoAGHP#dJr=u`(#ea2_2G4=-YNK06oSn970TRxa$tZ1%HF(aZNTx%gRLd^@eOge-L&Qe0!G|M6wc6|6<oRJsNp5ECevQ zV^YYYf(Vh6Ih=a?xM=&3iBp9Bpv8{gg7-1#CYmM(N{I(fhb&5rfcsKQBb&&munjc0 zWgI1hOOVCsKG|?@51&?u62t_|`iX>QA`+>kj7EpVkp>RTt@HEfqycq91yNP1QGhCg zLjxRENhMbdR&O%2mW3eyS3x~5C@-5&gDxY;S7iq~l zg;N*SUodHwO)|c9P&T`{l zCiI&?4_&Y0o*>*u;B3GxB-|?C&~7ejGPE12m|HKbMzh*P7>8Ok4{NIIp<8p|)~V#l zGjc+oxk5W~Lu<5g86{J*Xm0)tqb1Iv#)J8l4bjQ!)nAZ)7|BI0vMxauh&Xo`a<8Oe zK+ioxge&I?YZ?uI5VK*usFz?2DLLVklK3cAoFhJJ|Cfc;VzJJuB#Kw)>amD~n(NY+YhgsD&hibEUF z3tfoVMhdBap#o4cIXkpF3k6-pjl2K>_pDH-s5P-yr-6`iAh%7=p-nBbaU&OtnwnLl zuATJ5=`vdc;Y8_~R1q>A+VOz;D^?U;1_0#~XXvR~CB7hxPaync7LPGpZTO6)ql)yL z`njnnsa{VdLO)=Jva46Yk#g#%ah@7Ygc3k*jQZ`T8l_FW6h_uE)zh?$erhEO(OBn3 z6>`B=9r+0tQu{#+iOi`#A*X(loyw{kK`cZk$VoXoAGgm(L2hUG&=t3YeTX`qQ}ZI> zK(mMsnFpk2nn=^NMivdchg1azy@B`6lB3KDZdCPcCV-XOZ#g8^fwcY@y76^dP_#EA zmte7Ap(Rm6nge|ei>p3BjN$ta{}Vv%;Mp2fSgbW`@8$m~;1 z+3!VWAAl8%%wLYo{+LWiyT|QB)DYV?gVg#4=e8_+8L~Y$3`hOu{+3piIw!6JJ|v~p z;eHVEyn%HN#?XfNNyt+hVBHApIKoZ`)_rZ=swm=XZD>10Ds-t$L|nHzAtHEUIGd$) zKD8(5F@Jr{d1!(3oY1?j`cn{AslOE)Ez5V%o+hBj!AdSyLvL_H4_yzqA?+><5$dH| zwAMekIimIT_!71Z8;L!56X(ploY4C5p;pKN^K=wq<_W8Ky*#uzydGu!!fWUDArNJ# zf>>W0@^?lm!1)MUxk3l85qpYT?eH}<9{%d8pQETd*`RY`)N{cY7@K;UxJgiN7BuOq zDln<)J75l)Wc3nY=6^hQw5xE{(2rdWan9TZM`O;4tbmL% zIutxr)7$-0%qM88^y!LXIC`JwSd6G843EGLwk0kEo#NCHtide|oq;m3D5L($fJ9DO z*EAqc1hNiKv)HWnOhnH_V1o#Ls~IM2ybsDGAc3-MVNffyQoodjZ|}JGp4dAH3hl6h{+kg z+B(0oiy&cVJq%Wpi{XALEFT{3SAo+Q48>BE`t^s{L4S{4%!pUf!qj|^qN`)5eFK?{ zR~vDsz6lc>I)v7rL4US^ZIG3;$=UC@{ z=&H}gx!%?YShslZ}6te(PNldTZt8dfx0DoRJnI8FrnwEMu4p#Yes1vSXcJnT66P+QTWpg_!( zamng(4dG-&g8B@(0W8E+E;7;p+Y=Aa@4JL0?!0v9%eh zj|oqQT~rt5=Xp0{A4rx$$FXtU+_AgivV?lSGYf`R0U>WTo!a3TiL}Ii3w7H-*+pQC zqztvqw7U1^*85|Mv(W_f=MxZp3){YnB6#Kt&^r*j5)d2I^FA;|WK{PXjB^qT5WDws z0ocg%Chiiau?8NhDznq8D_=|c>%t)4Z+;3K-*M0YK$%M29w z9CsNzG2ue5L&(bigiQA}IOMDJ=PMZC)8b5PG6nKHI|=nAp-?R4X;DlCyocCG_<#mK zF7kiW;D-hLGb+mT4+{7vzz0bxqIiPQT_WQU+WglfC@5fEkc4$XWg=e}R3u#{x*urBLE0YjE2QRt;rGC`BMN|3NjAZ;oC zBM@m@L<-p+%UZq{Nr^e;!AQFc)aCJI9#!(agmSI!oeuqgIVY`b9gJu&7AQyjm zFcD)c4>a^|+W?K@e*DE|7L`CR@_YuJ^Lea9uXm%m4@x?vxLd$5Gm;0RJ%{Zy#RIq31I0XQUdT5ni~s}jH7F_kg8_ym{hY?*WSpc>!^+^#C4TiWukl zntH)IFve<3cU$2A?sl4Q)&aAMhTym8Pc!}u8fT}!W0J|~=O2HNJvR3Cn`xR)ejnFP zTws#?3BR|cpVyMJ&3_E!?fdbkxDoY?Us{Fr93I#!Kri8&WbKAldW^AaH+1;Pm{hu9 z*DPsY^FuhkL(?QwXE_BwL(UZZiHM)YyQDm~#w$VR6=;8rd7&12K4$KA)B!UM)+lHb z?m5(13)9svH^^R!PJx*bVOJ_aPEXD+Twb`paQyZ@sBj~wnn53%A7D#u0(UHmlGXN$ zpr%~knfH5U*ti9v_3e@J&Gcq?DV-M678B=a#E)0O2AFg!T-CipB9e4+`q5R5wN`pxfpm0PaC(oqR#ux76$ zWy9-r(r*;>qud_!JKMy6FUXAgzDuws5Bh&$%qRPV<$s8*?($vOTlH%>j;!jMzYn+l z=x`n}DcoYHrrY(n%=#Tt&>*322S)So5h8(06;B-a(Q?&Szm{H-ha&CO>cPr zVaK{@?sl>N*hNcb+LSPIo4)_3h@(DiOIlmODLpJ>b5egwTgDiyt79M?W2mi!2H`wF zh_L~+78Sz68nY(4B_`H9TBU8nEl-i2kC2UVZO}4@8*1U^MShb81<^OTKYjsT4t~nf25X>9JFX4D5q=Nu_ge=qa&&NB+>b=( zgh1=yWe$4I3HfbckM#nmw-RMDDAlR!Xy|ZTJ}*bJVlM{$#1s86lXYH{Fv<+7m{^t1 zM)j=8Ezl;5bdohomjb|A#q~7_x3Ta=jz)-K!3nex`k_V0n+|y_Yq6GXS3l-FTT&tC zxh>aliroWDZOfP_Aby-jyduC{(H5|I2`h;AOC{?F`H_7odkeCDqLm9)R_+6Y`52fF z>1HFL_rh!NkO;xwL*FfA2yF23g;r!=%7T!5k_aZKpp#IvQ2drSuojrw^0B|SrQO6G zlX>wLQo@^%{~6`~B=W!0;L9-(p<+Ln;7(}E^fI58sNa%qKng|!G(mWH=L~9%#2A6m zMJ+Ii+9Y6Il!SFr65czW*6Clxl3wm_QB~XZyw$>bNJvgFNM@I?CB7uwRu?T{y_S!g zL`zr~En%)DON1{AVY0t{4=k3}2WCSlD2)`dAu!X&H4;1kv?J(qhE%H{2cJ^e@xTcBvSlrWwL|5<~ z!eOCFE7wlkU~NgQU`Km&;EuK*?ze?O?39z83~nn3fsvH<1cV%)%KNRhSdu08Tl6@o z3;Qj4xDcbzT@D2|>f+VBQsXVxG6z3$N?neK;PR*uhFK>~1@x?L5#DC}XoXzbIl&nS zsM(5(mb2gR;I@f}g}9yMuxn}Dtp!4DUCG6`0Cr@=s z7&l+zLeIpUMbD>-@nz5=t^yBccyvesOB@W~FOA5ydHnVfG($1kySjOwg+)9a!C3Rx zf#GVh8Y0x7rhZMQG%zLifX!q%GGsc@f&kHM5YW~aC4fYU0#Sm`ZEV>nuQda1L7%K{ zK%HInW9Zb;I&T$g=;7%uIX3e~w(lZs=8Sg&0oBA*2i<-~9dXgwRa`UDR$NJ!b164g zB*_o(Yzbui$zf#t`F!tb#dDL$vW#Ppres-$23jg}qAX(_CE_Dr+Jq(z|$>ftj)iIvZjA|xy)1aGpDc^fOK^+(#E8xv|wmX%?{a(PaUXhZWEOmTO^lniTn);Cu z`x}vg6_9%c7&uX*Jg$*lJjPPfU>e> zsWP>-5Rba>DJi@&0ovu`n2}u3hWoqZA`b&88j-ea>X2s9h@fG5D$Bl_Dh^H3#z7h~ zQ|_NCD+@-pB!3Y@J8&O|o(7-g`PS;8L6z4=`Mmz|#Gi|rqgza%Q&=}5$>2O_aigeUO02*^z9@50z&cL}>pUf_^Ncs}Tp)R7nt1Lt z@szO6Q^GpW;Rc>flIMfPnw^B6kDGW(Sm!BW$#c-+#ez@THs)hXDHmzR)8ON(kAGdT zgBd)Cj5aniwUAV;OY&GM`CeG_CV_`U9tXdN2SJ(#n*mS!R0G{Op*c>`2Q4PArj|QJ z;MzsYxM@Uf!}UIB_`M3y@EI32_i#G+mmPl>I+{drUd>`ZqAq<+_c`94!1IL9Jwja+ z=UTKLvwDagOH5fB=XR78`;#9uSat2DwWj3*A2h>EiDfU=apVs39IX(CT5aQ+Ve-tP z^`a;3uTUOcawpI~gOdkm?r{gWk7lEz!bS1)5x}p|#WHRuAy!Yy+agUmS(M!e!R{Yf zjd_%VGx>F4)NO#-{s1bq^529aTjG&EE#?6d# ze}%H1{lwVv7lGm2Lb``KZ79AUm*qY~<|J{C#<5vu6%b3$AzW#!4-ZR|=Q(Pj3&xE7 zNI-|ljRj{~Cu8sE-i~IcSsZVLs7x@ZR=gbo!WgtFp4#LVPEf!3r(qiS8%7PX47UZzRVb2Y@t0*G00(FmDM^><0KyrXzO)V`@zm}+GM&XHZn%79yXd&xO z$YwiSCSaVWy3zK@X%|sJEKX4oEGXP>6OQ?_72R)x6;1ZU>QX_PG)knFv4Ry?dlJv( zdp8B`%4qrHCO<|FCJu5Sii<50%PyGjQwVt{$cK;O| zc11oJCXP1zWMiD+O~xEQKLFja?6T3}0Up7L?WC>%NBRb-E94>zHskdcENk$XwRr6S z;RHH;iU=pj^C~d^Ophn5CySvQDz(?lZn^_ zy)N?g9j1i!jg5pcl|!mg)|?1gWZblC1&TK1ENhEN*0m;C64qr&m=+|lcQdU?=FkY4 zXg*uo2?Av~QY8O`@Xha1E_Mg_6S>u1R8!!q*W$F!OEWE?Q$?GyJAn>=bS(0xcw zjX?!F67=!SS~Ot=&~RgpCSPFmPGkjp47my%C)7{L|cc*R|w$2r*fclV(d^c2E;} zw1eKAP9+8%z);%8IkxygC`LzkG>T9v`mimjKkz8|fIMon#VZ1Vy#ml;Izh!)mpmq< znWnvs-dDo=K;<#C8=IemmsZ=m@QjN%hlg%_{+JxU1>J;dYl9jrR}P3tw|RtqjjSJb zinNe#r&sB44lK`nF-qr)K8UVND-QVj1m4?;2L&e3WakD&K5e*Gw4W9E0<$=eHpdst zu<-=@zY>p#Mh%Z59?=Ga^l;w4Y;xrJ{*xwnu?fD%1pm$i*AT452dKV?5x60`uB%V* z@>|aXifOhsP9xz9F@M$pxCP{)~Ijycn39D@9~Nw$2;tYE#FhTqi7Nv z<#6$iCt=PW;~nnnxNI$i{+Ht&yXDb71_Q)7Xv(D>V^YgR8bL&KgCiaouP{<@+#~6E z&Jo}Xk8@b@dp$&BVH_+{IJd7|1 z;vKl!Ftv6Mb>LhKn<$QT;IJl{h_D`y`+|TL*uyH zvSR{Vqg%{l&JklCIMl+)m_5tuGPB30Pku zN?2dENtl+1k@1exG_jbXMZ6=yBxFDSlrg$+kL5VN7kXYJ?)9bcPux94-VY*AUkdLrl%w$tb&XgGe@!?n zgZ~rp4mY>sVdEW3|ISP1Iq9h9A}!wWlos!RW|%8D-hur*1~ZrX^TrgxS zF1+ELjME4Vf^<@X;g%nTM7(2}qZSCY?MW`i1!!*zaoWxj%U(COhbpc%7E-`cv4c() z!4ljY#PtEtPigTE%numi6*0trZH#&Fq$pytddvew=`lAM^T0UQd&~pp1v2I_k*wD} z=COv>#m1NiO7tG{aR1G4BD4yYfF?5LG5#u^pO*2a0riW|1p>VEmQ_SN`Sw@d}&;*S`k=S^V5gGHaOC)Wh(RP?P)j3ZX zszxh4t{Uy`Cd;~2yD6h{~EH3HUS z9`_44_9l_yc*A44hwpocc}x-+I5z1P^Z5JYh8n%dHNu1_*AcN#|5mErj2zS<@h%{^ z&URE}Be&cV&gvfX7$h{?1fd)Q`GR?cV;;8&IDWH8(c+$@hxtB&9_2ljZ4%#m%tI{i zM9hO%hiEShs^ORi6Tpda{=7(38Eq|J_c}#{hWTyP(N?BQ2m?Yjjjv-9nJ;XeC zUdYGWybu|GA!?2;Wr%r9WRBsOhlFv?B2MciEYJOIX`c$o7_!4Lk5dJXmJhTlPU&yr zDPf(bgms>08+a}d4V^OH#PgtlF)oQ{frNFQ64rTU8+bNJo{x^#>?HJj&csu~I!_5p zp5d6sUgl#o75oMGk2UJ?4@1SKa3#V;+B|5eR3!TFe8T3{RF|UXmO#I&q8x zfikXv^%e!q5Dh_PFzjPnvXhFxMSn0EyJtH}z(U1kY0hQVaVzQA6G2u_;_HO5#_a;a z(~KL;D4w(sYKYPvOR8{Nj)U;?(20)-dYmfpK!h`fN((%flTs6L%`!QiRu>cxJt;<10Fd7)&m}yRL~UgVE$~yh=9ixf;8z$ky<7( zD?Fo8fXZ;#V+dm??D1a@c)SDGo%}O3M)!aRo?7^K10LTzOz!(140x12%FA3nNdC71 z9<5{8$&M88h(hnh5NP=kU81*uNBtPQgwY(Vph2b-?v8*6-B$3ela7QeRpQ=G_tV6E zD&5Z#_fEQ>EAI2?{&(WOfbMS>_W`=UlkaP5F}D0`ah93Xwdw7yaGd2I0)=T_Y^aI_ ztnc_Hu?fNt4N6$w@kyArmyvOn_y}3FtV!$4*1D{aRb`UZDPVmUEn!`jgn3vvkM%Oi zTrUL3`qAbx?FoUh{6m-RN!h~pLWh0gUf*0kV#s@l^$EvW?h`P_i0&5m5u3|9h5r9; zoMo~2Xl7DfmKJBp2;lZeaTc)^#B36dvm{UqK|7zON2)hD$?rMdk}**7w8Nd?6&!Ct zpo7luI)I_HjdN^Ck2(on#_<+VF4E#FNeg5KZEjd{9&Da;C$NCJM_t5GX?sy*yybCx z3$y(PJ>H^6Sm5$fA39G2l;lCTCA}J@Uj2ojMZAeyu+n;RI0$i7_T{>sc<;j%Ff?oyk;cIXz`i@!%?wc;x)(oq6!vXhmY56 zhKGth8D3%O7O#0zYbs=!Zt)tw$cT*B z1SHZBuSqmk?NzO69IshTP!LFxx%{xLHSlMHFlkY{mrbXZ(v3tB`m4WVW zLi31tO|`)NyYZT>^}5d;W4vbcg=EfuC0=8L>Nxy1#A{99-(HrA6O@e`l*Nh{~kHM1G>e?z?HwE5(||G{|8E5GMu?!Om$vibeCat&9_WhcxZ0V=V_N1K8_hNH-g}B!@ zm%Eu>H+(&FKw=nMCelJFg*SrTO74OAlG~@O^5wA)975Tl0*W^o{ zcDNI~;&Abrz30(UWe@QhRc1uSYqkLUUy0W|Ihgba$7?)7(Er8pnrO7mQN(N7&>MP) zmn?Mfar!)JVSb;)1m9tTD^2he6FiaNhGAtto6n|h-oy95Qn9vx0c*+dWDgKoqR(YqukjZC$9TdYBVI~vR(~9gMowZw z4=W77NSllvU&c=gBSWpcm1k=D`U)QD!kj~Ieq*p4gny9j1OfJ@eN#Cd@Ngy%etYRE&dD) ziqMjnxp7Qli$7OXuoXy4B~c@T-T{1%Zkj=1k@D%=JoGupxHdu#fq3hnb5IJOKlLOg zkv7O@B7Te4I%qUas$LgUEP{vy&6}=%%FDl2a1eIM6xs!7ok1}qw4nEyM#rfG* zJj@yZ=0!&>JP0?l`AuZJm91VfM=tSf&{&0tok%eQqa_p6naC6oE4O97ol4>p2{g^4 zJVfD!U+lJNd=Xusg%eoS%`QN(XM^W(O3$Tay& z+AqCN8{3Up^ztZlc55F%pm(6RldIsHccS?Za$Rgmm!m_0ecClhrP*z<+r`=-{&mWv zW^Z6Bd{r5(yN>=qee9FaL>o{zX(VQ9AjlWE@je0&dlghAKjbej!zi|-vp{AYbag83 z<8RV9;?cviM5W$?!gy^yc^mRmc91VnGl3OZ)^ZLPaC7D*;+~Er>8Y_DB<>=C!&94h zgWnqW0R%Zo(1}72F_*8i$E`rKqo?iJM=fpvQGnWyKAMJ?3KlwQVVYv=$^7y;-pF>7 zF$>h++`?@QnCytHdGcEI55T$V`+@_Zt;wJROY6K1%pqB@LT=K7Sg27?#Pj_860#+D zCYyT)He!jbJOIY;LLO>|?-GEJzM8RtZnop585iDf@+95Plx`deCMTW7BY~$4&s^eF zd;Yu)zOMwT*d=gtVEJM2XhFq?g)w~3cs!v0bj2n=k2cjUuFxILXfd)qc;FwcK}|EZfE*ZnBrlG>;eh9cWh=xk z0SDZpRXk~{#Xa5=_rxydd-27G4JZv<2ik$Z03l&K5*-1<-32@Z^!Q*xBh;W8;gt%0 zeF0&1PpqAIp(}K7HC8fb_dBFHsbJX~k^O?mrjJ)FK6pBTk5AR`#2lg)%J-m>KY=80_awe8#s&0v`oukrf9^Lx zco%;+lE1jIFq#c5J_k;b82UV26H>H<*rKTYr*X^i=jj&lYpPHj&5CArqF!_NsAH>{ zVi^}iW(P8}=-FMHW%jAW$`hnD6`B@f1@({Qw~JlkwOx9*m;S|tiWckOyBxI;tPTz` zH{wpOgc6SV+3000plBY&_YzPri(e@gYI3#q6D{uMX&>5JB#YxQ3H8yMutx&y1c;G_ z$K}uZq8#QkF>HK5{fS>e#cKey#1aF`#|g~Z16RVJo{oMeK7l0P0RT2}9gi&d;#UH+ zo9p=3z_v03ghx*yZ~px>9Txikk(6>B|VlhmLba4>byNE zFY>*>&k%UX@FYHEzy|~#vM~N@cqtP{Em+T>xjktfWI-~0mV)Ywmd8Ii0V~i{MnjDy zLFhoAJ;KODuZQ1IM#BQV-{7W~Cz3xxd9gdkSW45M9WP>->(uMv!VGNC*tB`Hbev8&PU{|F%KGpD~teU7_Di2 zP+LGqE7Y*5jaB49zedq@+?QyOM%D!0KcMoc$Gd=YX;n|3R`k&NmCjmdvp`?PlMbBA z?;>J)LB;sZU-6L{?iFgi=v%;}FfDr4v2;}b2Iv5oR#CFhp`sR!bRR-xxHFw0Z zewJFKy&nYfhi&-5?IrMMc@X##--?64SH!&?`dD5Q_ZWtgEi1%5>1kOb?(^t=y|^!+ z`?th>fbQSr`&#N?I0#&dB;o@(;yWX#Y+CC{$l68AaW{ydI@ZTv5NpTCoD)KG+br?% zR5dI=T>6%dL0kLQpZNJ#5Fryr<2QFen9zO+9RO3A<*!IG41DZ_F;HFyF2_}f4&%V z;?bN}1>nFvoLB7}#77bH$X;|_wb=x3GQrDD@bf14ae}q;AbI|C2h1Nn{|SM@bpBHW zg6`)(6M*f0{-d2&EyY0HLw%malEZjjHPZxp2-ftY=ZB5?Qs4Pt9oF|!7|x{cY7lhS zcO7~IE zZ*^r^O|Z1G!dvaHsP;~;3VH*+Dqp$Go#rd6@psFLEOT?f8}yd>%Dv^K70T4I$|=6G z(pf&{SzJ?A=B*C;1Ky$K1gyBk?a9n?OOkYKjzp5IteT9Yo zGC#C~B8mZv+DQ*8fvrj^%ZkX#8lX5aIQ0A*h8F{5rS{^9#7Ww?%;+l#Gz)P++!#ks> zGU%NZ&M{zXl$lljVr8;2X@)YXMky_tF=d8QhN}h_Lw-(bfUE(sMj23981Rx3fhs>) zrY7jO7yC-f{6%LE7@$60iux-_{{BiuWv~PuYp?bP?KAzs3oaNyW14B8;8BT3iZXx2)L@AioP0&T068%? zoVoLgk@0XOG%9K1=V9r}Yk2S?+9S}V9u+_IJ}vPtLF>6%Do*l@=GH zx7drTD$DJ`62HB$rmD(c5iFZ&_lcp_UvxMIXAj6L^ZTm(cKBJ9AM)+g2kjO9>0n(A zR8S7-c62~{psI3OX_4RVV}fZIXexERsK1p5s?X*|hOxLG75W0zHD&&kkv()MY0z$V z5PEodEQr+MG7@VrkiE3pe&xj6T>F%oAXh$YDGUphL2^Fyet#8Qx4Ku09aW(k3?ozR zg&5K8Q~eckP&(T#jdv_|84%gr7_H<$gYmh@du8%qJNk{CMjL-Q8q!}h#9m!mQRoNS zj?Ne?^_4*njFA;UqjT{HMwY-_$D@O0*9fl?bNglXVAV`26|Cf5nN&(FedW}@;X`Ol zOpN}j3W&fc$1_0n1@>$+&R*iHwnvmDqEfQA=;Ne<7#?Uuwv(%phQUgR#mrhZ1kFOt z48JQyT`Rcpf_~H)9w|RKu>9ntoWjWTc>pb1n;Q`s4E(7moP($-NuLPstl4 z%Q`u|albQV%o#hryyUb|5B-ULuZ;TVYaea?eD=o~4;Ib*;?{$YX9#+S6Y(U4F7fj? zwL40qaP`53Ab>(gKNeg>dmJwQwYi&R>MZ8pzU49*+g_4%p+FMeQubkwtF zzIEXrp0JJ@G0XP#fRE<99(_w!`-BOzMn3b!C#lbF-#+r(!e2cx^067q*GamAYlc6& z-SOxs=jgfjwGOr|fB3ZX7x%k;;Hc!kWWAB`&Wurgo$iTAua+#oS@OF=(*5)PkDHz> ze00?Gb#D)Bd-jp#p7U03z3uYS(p^00?Mo}Ke{WP?yFN$}0ii=G%|^7@1N# z($CngMqF=x8G|xMOUDA_A1b4jBOk|;Bnpme6@=*&9H%RI9znsAJ_YyCpw zr`1QJ*UD@4)B3;GueEVN>)+aVp!IW2o>pI3|B+h%)%v^EkF|cU^=GaBoBOr2$0)5o zYyH&RueJWI^%+kzia)M;GX)>vEq&6 zLr?sSeaxt{d3-R^kq+9=$ls{{FXNA}5ZQ<9uK9u1KeYa$^-s;;wei5zU$yyHn^(2@ zQkw@~*UHQO`nv3|+B~bxpUY+cSuXp{DA}L2d3%)XkFU%AI8vjN{cE|_?po^Hi1`s$ z0j>b9T3mU!RCy)5hxrlrF*4Q4X(`#~7ylR5=&65F&avRB57MuvGUh&fBpM?<-TxQn zUuDUcU0qGMT5+}E>cG{E%l(zO?m$|CYlcjXV-(f%7yl9a8?r@D?Rl*D|Bv`jE!9g= zI^=j{-v4O(ppnhm`bJyNY5N^*{S$S1-^(4?B-Vjj?O5#JaNjx(K8LgoX)UFB*!v+3 zxG}CFtws7X(k7&F*jxrYU0p$>P1kmHEkW9vk2L_&Hl#eMPepkcwhifir1lb&!)CN8 zfHeiuB}f+_ZA1DL(%LGlL(tjUkPbm=!y`j3q;{nFNS#OnNDGk8Lz;>Q?3N(S!-IVT z(IFaLYO-H%}X)e-cqy2hxQ|mFeJv)P{5&QajR52!9*&zyX^L zsS~Lk=>(*yNK249kg!Uj9sSOLLB}g4en~|m?ZAF@k zv<+zi(hj6Sq{>66KT;dgg-Gp4mmp0=x*F*cq??c`e@1&EEkLTo0*^EfX$R6|q^S#G z7o<+46Oh&-EkW9dbT(4u1=Js@4JrNe!FHtIAx%XZa{|gCJp*YT(p01cNVAXzkkaS( zYmt^AZA4m&vv^Nz11 z8&qu80!&7Ln_j7z5eXb;aEf3^bFw5`HnKTv5d?8(PnKj$Shfz`DcFL*wRnI`1SrHz zndFa(32jVqK#AN@;6?(_D&&I<3PT4Nx;7sNd&(pJ(;%d%N~JlmD7` zMtXKX`@GNlywCe%-#zWUYFdKpkjo)c&mu06X~=HKnO9MN$P(lXWEt`xWCijNWEJul zr1O2$zX|n#Tnd?hY=KNcUIUqi%s^%!2O!IkyC4&<&CN|h79ppJLp}}J^aJ!W$P8o! zatiW&$Z5z%yaF==xePMtiLUuuxAvZ&&eu92R9C8n23Gxw0N_b67rMiiYPTj7ix(gPc-Z+H` zcR46tYH?cIQFqJndroU}Tkk%7ja#m7NS=O% z+X~j^wm@f<+l)M`+{R7}4Qe*$AbyIQ=H}2PRHCvE;pYx8RlhE`=~}l@`BMeo1O5tW zuBNhK?}3I+DtlG1><%|wKZcUqLrU&spuCgFbF^n}?m~J<3Cp|2&DD2Uc@YJQ!7TC~{^Hymre~G* z;`d>&LtqKaNk&~&oNMY@tZP`k(7ms&%blugXsN@|Dz{j73*>r`F~|t;8(y&Y~g z>-M&}nQIm44J)A;Cf|Vl3hbxJKHC1~Gu*~Dbe0jER&i}XALcLISB#j1wLkh^-Cd|g zZ9}hHH0(~**ONlS;_%M74&_n1WbqKNba;Mj5G#N+fn9`SXT95$g}4vw60omxzj+YW zWlTiIr2a`2j&&*=cd6r>!ul)hQ(TUrjGxdGr(Qu_ib!Y{94orellAEJ_cV04iMton|E1g9 zu7)z^30$6p?yl`~bLXOsjJ65uuWQ@`iEj5m8b8@?w>sGHP+jO8LCvn_C|ciKvjg`B zoKxLLQ8}uIg8?%DPTheLFK~}_MD|R0(QV+MX;wWwik@nb~J5KqB8b@?YG!-VEe$# zb}EBC26ns3sq-EIqqQGijaG^5(Ry_m81*|!#1^9hLzZ3wtNbEcu;&a*??vrF3Z5bRCfA8eCj zVE0*!9)9Kk<*`5nfV98)&a~b$iaI-yAU`N2r_Ur&_ zdT`!vDz6u8F<3v|N{`c?AD*N=_rcyJ_!m5ALY1h_&w;gAtPHjs%xupiU*`5vt%t0`-J(q%2!NhNB&la#_V85>R+y#5| znY!7YlVFV&n+96|X13?kV2-6X3r3&Qn|g=Aj$5n>_P)gyEJl675FV8%R?T4adB4dn z0XqsNaUs1l*by+ZJ$t}1@UL5&Q?VZcn*=l4^A4~>mfjTD(!UF~=VM@LgOOR)|G|gA z&9*Cp?FBR2_z2h(nE2g|{Kvs2!TyY%|K{(X)>?z_pVhjCht3S0Z~gmc7YaP~{a|}; z20Lo89M};svz_*URV=+nzz$n_`@s%btOWLk#ohp;XH>K8j)J{vv5&!K!6Yse(z|zZty$>eilgcQ7CGZR^4CpuMHxW<~$gS0ZNU@~H zAxe+(;!VzPHWrk&z zWsYT$t{uq?4Gv#hYJ zvUFB)`79GGQ!LXgGc2<#b1aK2r&vz2oMBmFS!P*bS!L<8bNMV2EK@AgEHf;#EORW2 zET>pbvz%dBVp(QcVOeGAba44B6D(6K(=6$KyHLuqY`w5fov&$czdF%!!mjBjza^JX&d85VGGC%%b zLH&N_&6fUGz{AS@HtQGP4cd8`dDi0ZG0#}MxnBF9v-lU7J3rI*d$e%2GS4!vGQXdB z^;poKr@_PgKP>it9@Jmhp#7=*Pmm{>mo0uH^Qy&*V&^?=NBn%8`Edr)UavDRvcA;! z7t9mPv)Y_PpJj&CYaKZC2j|0}|F<%)d=TXHcT>_YTm0{tS1tYy^Wyv3j`(>d77%2o z!d&XRl6iu8R%dg*1Rm!9U96uz9`utwJ0$xFi~l3@CX2tzJZ157upS{hS&onR)5<*Y zzq(xU{|}j$SYPV<$ILTUoYBog>-8O_f40-Th%|yJEDp~=-*L`zrrCeVF$-SrEOBHV zDEu{LAFHZF&^~=;Scg@b%zL6=WiIo$@Lwu^u4AtYPQwDN-oYv`Et*==XN_lLeoH;1 z&t)BVHS>&GWzN3?+snNCL{R@;=A{=k@72Qj9`oX`2I}t$IDC!yv}NZL(YMN7h6Ux> zY||8%DegMeotn1t#SP4J>`yoI+av6JgY`@754xX9&oi&=)#vhz|1;)Ohcy2!Eu8al zK=CX-se!Ef+lAktx%yie4*Qr-ar~v+N#==%wLVT&dPX=uN651)y+;rhpO0AIVgDr# zpFvj$)9;Aj^gaUBYntmT&*gVU=+ozBVfJ5&;BT@0Vpi8Pt%dUmbNb)rl$L7YEL()3 z!pElgEI9dD+OO$$zIe0ZrlvmL!TO0e^|{1{-hUuF)h9HUc6ovMl+{1qW}bOL>!Ta1 zbk<^Rr)=4$&#TCOncGGB$#%s#vmxj+s<3!I5W)Ac{jBBBzeMQ2%lZ|ozRTbk`8oAn z?aX%e={n|Fs~^()7^Gj}a;4us!Mx1lLip>78x`flpGWZ1-0(Og6-T+;F_B%R8=23% zq|ar%+!JAk-uoc`r`e9^Kg&FQhqj~sK7zv^ihZu{LVej;xJ27Y@pzH`kW!qL4AI5< zmE9VOoo&K7@Y4SuiLmq2=J5DD6`}up1pn6v{vV3FPI|xgQ|5stELp<(=Q8kwQBOH+ zRr&~?6(4%9h2o#)@%{>DxR?11_fLuEvx=a`pAG*^EU#^FZhnPJT@(!#l2aXvA`q}bv17611$pW=By`rBK~XDt0+ zfF}&Qa(FI+PjM@9JTYym^m*nvtG>g`QxjTG#^cu%=gfw9l=ZXLxcJ)${kK`aWcBBU zbM*IS4AbiH)8J~{as0)fYZSM&9cP&JtCszHnP+Bo9`(684j*Tp-lX}3cz&StPs}S% z>T~HQA22UIq`CBy1O_0gR+BDM9Q~H#DdS>8g#LEccUWKSf0cPDtBX+Yhv4uT=2bR) z6?^dS%o996aP^#DGA}Z}jP-x-T<+)0p{CN+%%{1&(tr9DXC*`IV*T8k8us$FKWCoi z_)9y!!aTwKQ|kL+g#G28(tc*y9~m##Gq1j&3zRs2g?X8|`2QW|P1Zc~y5dHK`0yy} zCtlK^oB4TInp6KQvp-U9C-ah3?oQ?@o+l*^f66?;T>N}R@mha=62U)aJ4G&6;&XAc z_A_mbqt(o(SbrfGHO4%5g9gGMWuDoq`4z1HGV>JM$1+%@cNFInLwpp$m!7BnoMAh1 zzfLi4;&CDI=~mn@%!mD~?{GZD|0(7@5ZkocC0x3mEvsF5HGU+4BHWZs?00gPb5C)-~&nWvuUF? zqTUb1;Y#Myp62SENE}`-`qq4PhvHh-{5{F~GkkxM{`O*o{*PEcXZ0W2HJJQN->$7o zKTk5xTJdrE#|rtvICe(tcd*gS;5O`9g56CDJG}mp(Vf@<$nyq9{e4uh!9^b1!|ZZs zkAM;ElQ57>g==_QvB{6uH#XL{+rxMK$96kgX=ep&fP`%Yki|Gt+W`esqqn1}*%rX~ z+W%+{TlsiiW@Beh&rROy8#c7}ti5i7=OtH!(v-2sfVrCu0x@5TdBMv^W%2TtN9MUA z^2&;+E5=?q6Vcw7S|@NVs21sGP%YBwpjxaNc=^x8si>D<8Bgu^W2lXL@d5|Ec%f5@ zae}852M*oAgcyZ3hi*l@PibCTx zMLsla0rw-iXiz62WVTXh*R+*F!(}Ul2Fq58QMrgf*_J}*23zS$OyH|Gwyp2CTM%orhZD{GKZZsHX!>S&FCOytCfxyi@4g8PQ4ndRXqM zK7ekFb)edja4SZ_cHhPo$(6JvrP_Im_7~K9FZx8)!|U(M_YDktdj02DRXN9OKJwC6 z)s}9?CW1x~SVi@!e!Wc_yxX>eb_AuJ6EW+|+w-esMU0xKrhCutSr}#^EHH5=gb8$0 ztK)DrXey#%OyLMCwZ`wTDZ#I$?%AsvV)s*1To~1hrwY@5il?iNn zFM_&P3wPCwC+$ec-)8UDDlZG`X z`n{;8YDOgd@@f0NsJIyVCy(4jl$AhpL{Gc{HRDDtp8B&O?=>3XQEgWW+#{kRE#+Wu z!>z;HaEHKVwWGWJ#>EsNV@5Oy*=q4`r!pgpHOwH6x3*_JOv;A>SVYlB0zG|OuyhKk zuNAVeDTOOvv$toMxtjh$(;W|R@sf7>^D7#b_y{#^7yl9T< zU~SRXJS6qjmiRzqa4;|mBPwtTi^K`<v2k~*6 z+S1=;JEFJL%)!C>g=~Bq<%JQkngxGwEb67OVJGZz?Mj)o)G#(OM+XsGv+wlhY!wul zp+kaUxUbgvviJ@>?)hce+Hsx={4Ou8>hDBPo3mp(b}SqHPOX>O=sU4$dJ2=&xLzZN zSIh3JUgS(;1;C%y{JP0=O8;m+-b3XnE^K+NAG;ng73p#ClpaFiN7(Z;&z}+wb2)h8 zsWlMTkap!!AX8!7rlfzA$p!P z6RpnUuEuND1l%+{P~_u@h)Nkg({1F$3qng-#%R8PW&c=UyqvtWV8ui9t-?+;o-Eby z0bVnWah#&oTb5BZW<%VxTGaSRCyugzABv-V;+fv^m%D}ZDD~`96N2dO8#!dDzW%K* zaFYu5J6av6b?;rN2*F^KR3dGQlOvHUIV9ft0*GcAg?$^IV4B;hX#YmZ9B)4_&*6b zeAu8(n;blxowmW@`L?OvVf(Ylcc}XLwi*HkCf!#5dUR_dE)R?3&}4~uN#43f_-|61fDv5!cx9`KUm}tXAu8IPvj8t(|3>MT)uxS zGRuXL|D+@SGj?vn0oj)P@_lEK@_lD1U-C=+|A_Oi5`!!GfTeujn)1^-FLwT~fl(Fl zH6NYQK}r3~OyY;IdvIXpPo(t~k!p`UQHGTFp+tTi%*>zb(8?l3PfW0F))sjmUU4&%^dDhAzyYby7^EcMgY7+&tj_m0 zbz+>0owvZ2;79Vy_inQ>^ZzIEQs0&Q@;$X7&R@}P`1=`1xzdggBQL2){;uEA8M>Be zc>veUB>6>Fana20f8VZ2=WmsUVIz`H4^N= zs#wF^13)Y_kJORm|E$F&zbUOMsvJ2k{}H_ZObM?!I)8v4@n5<%y{9O?l5&O7yPRS9y)LcE-9Xe;UHuar{2yICD8>K) literal 0 HcmV?d00001 diff --git a/control/velocity_controller/src/build_auv_solver/main_auv_model.c b/control/velocity_controller/scripts/build_auv_solver/main_auv_model.c similarity index 100% rename from control/velocity_controller/src/build_auv_solver/main_auv_model.c rename to control/velocity_controller/scripts/build_auv_solver/main_auv_model.c diff --git a/control/velocity_controller/src/build_auv_solver/main_sim_auv_model.c b/control/velocity_controller/scripts/build_auv_solver/main_sim_auv_model.c similarity index 100% rename from control/velocity_controller/src/build_auv_solver/main_sim_auv_model.c rename to control/velocity_controller/scripts/build_auv_solver/main_sim_auv_model.c diff --git a/control/velocity_controller/src/dynamics.py b/control/velocity_controller/scripts/dynamics.py similarity index 83% rename from control/velocity_controller/src/dynamics.py rename to control/velocity_controller/scripts/dynamics.py index 9dc4f19f1..cfd1e220b 100644 --- a/control/velocity_controller/src/dynamics.py +++ b/control/velocity_controller/scripts/dynamics.py @@ -27,28 +27,22 @@ def coriolis_rb_diag(m, Ix, Iy, Iz, u, v, w, p, q, r): Fossen 2011-style, for 6DOF body velocities [u v w p q r]. """ C = SX.zeros(6, 6) - # Linear-Linear block (zero) - # Linear-Angular block - C[0, 4] = m * w - C[0, 5] = -m * v - C[1, 3] = -m * w - C[1, 5] = m * u - C[2, 3] = m * v - C[2, 4] = -m * u - # Angular-Linear block - C[3, 1] = m * w - C[3, 2] = -m * v - C[4, 0] = -m * w - C[4, 2] = m * u - C[5, 0] = m * v - C[5, 1] = -m * u + + # Angular velocity block + C[0, 1] = -m * r + C[0, 2] = m * q + C[1, 0] = m * r + C[1, 2] = -m * p + C[2, 0] = -m * q + C[2, 1] = m * p + # Angular-Angular block (J*omega x omega) - C[3, 4] = -Iz * r - C[3, 5] = Iy * q - C[4, 3] = Iz * r - C[4, 5] = -Ix * p - C[5, 3] = -Iy * q - C[5, 4] = Ix * p + C[3, 4] = Iz * r + C[3, 5] = -Iy * q + C[4, 3] = -Iz * r + C[4, 5] = Ix * p + C[5, 3] = Iy * q + C[5, 4] = -Ix * p return C def dampening(linear_diag, quad_diag, u, v, w, p, q, r): @@ -61,7 +55,7 @@ def dampening(linear_diag, quad_diag, u, v, w, p, q, r): vel = vertcat(u, v, w, p, q, r) abs_vel = SX(6, 1) for i in range(6): - abs_vel[i] = fabs(vel[i]) + abs_vel[i] = fabs(vel[i]) Dq = SX(quad_diag) # elementwise times abs_vel when applied # Effective damping operator when multiplying right by vel: # (Dl + Dq*|vel|) * vel @@ -80,10 +74,11 @@ def make_auv_model(mass_inertia_matrix,D_lin,D_quad): dict with keys: x, u, f_expl_expr, name """ # Unpack parameters - m=SX(mass_inertia_matrix) + M=SX(mass_inertia_matrix) Ix = mass_inertia_matrix[3][3] Iy = mass_inertia_matrix[4][4] Iz = mass_inertia_matrix[5][5] + m=M[0][0] #D_lin = params.get('D_lin') #D_quad = params.get('D_quad') @@ -110,11 +105,11 @@ def make_auv_model(mass_inertia_matrix,D_lin,D_quad): #M = diag(SX([m, m, m, Ix, Iy, Iz])) # Coriolis matrix for diagonal inertia - C = coriolis_rb_diag(m[0][0], Ix, Iy, Iz, u, v, w, p, q, r) + C = coriolis_rb_diag(m, Ix, Iy, Iz, u, v, w, p, q, r) - # Damping (linear + quadratic diagonal) + # Damping (linear + quadratic diagonal) Dl, Dq, abs_vel = dampening(D_lin, D_quad, u, v, w, p, q, r) - + #TODO: blend the dampening functions # Generalized input vector tau: map [Fx, My, Mz] to 6x1 wrench # tau = [Fx, 0, 0, 0, My, Mz]^T tau = vertcat(Fx, 0.0, 0.0, 0.0, My, Mz) @@ -122,24 +117,24 @@ def make_auv_model(mass_inertia_matrix,D_lin,D_quad): # 6DOF body dynamics: nu_dot = M^{-1} (tau - C*nu - (Dl + Dq*|nu|) nu - g) nu = vertcat(u, v, w, p, q, r) - D_eff_times_nu = (Dl @ nu) + (Dq @ (abs_vel * nu)) # elementwise: (Dq*|nu|) * nu + D_eff_times_nu = (Dl + diag(Dq @ abs_vel)) @ nu # elementwise: (Dq*|nu|) * nu rhs_nu = SX(6, 1) - rhs_nu = inv(m) @ (tau - C @ nu - D_eff_times_nu) - + rhs_nu = inv(M) @ (tau - C @ nu - D_eff_times_nu) # + print(D_eff_times_nu) # Kinematics: eta_dot = T(eta) * omega T = euler_kinematics_T(phi, theta) eta_dot = T @ vertcat(p, q, r) xdot = vertcat(rhs_nu, eta_dot) - + xdot_sym = SX.sym("xdot", x.size()[0]) model = { "name": "auv_model", "x": x, - #"xdot": xdot, + "xdot": xdot_sym, "u": u_in, "f_expl_expr": xdot, # explicit ODE f(x,u) # implicit form (optional in acados): f_impl = xdot - f(x,u) - #"f_impl_expr": xdot - xdot + "f_impl_expr": xdot_sym - xdot } return model diff --git a/control/velocity_controller/src/generator.py b/control/velocity_controller/scripts/generator.py similarity index 93% rename from control/velocity_controller/src/generator.py rename to control/velocity_controller/scripts/generator.py index 4820ec544..cbd2ee512 100644 --- a/control/velocity_controller/src/generator.py +++ b/control/velocity_controller/scripts/generator.py @@ -54,15 +54,16 @@ def create_auv_ocp(): Q, R, Qe, inertia_Matrix, D_lin, D_quad, N, delta_t, max_force = load_matrices() # Load dynamical model - mdl = make_auv_model(inertia_Matrix,D_lin,D_quad) + mdl = make_auv_model(inertia_Matrix, D_lin, D_quad) # Wrap into acados model format acados_model = AcadosModel() acados_model.name = mdl["name"] acados_model.x = mdl["x"] + acados_model.xdot=mdl["xdot"] acados_model.u = mdl["u"] acados_model.f_expl_expr = mdl["f_expl_expr"] - #acados_model.f_impl_expr = mdl["f_impl_expr"] + acados_model.f_impl_expr = mdl["f_impl_expr"] # Create OCP ocp = AcadosOcp() @@ -73,8 +74,8 @@ def create_auv_ocp(): ocp.dims.N = N ocp.solver_options.tf = Tf - ocp.solver_options.integrator_type="ERK" - ocp.solver_options.sim_method_num_stages=4 + ocp.solver_options.integrator_type="IRK" + ocp.solver_options.sim_method_num_stages=2 ocp.solver_options.sim_method_num_steps=4 nx = acados_model.x.size()[0] @@ -88,7 +89,7 @@ def create_auv_ocp(): ocp.cost.cost_type_e = "LINEAR_LS" # States you care about: u=0, q=4, r=5 - idx_states = [0, 1, 2,3,4] + idx_states = [0, 4, 5, 7, 8] idx_controls = [0, 1, 2] n_y = len(idx_states) + len(idx_controls) # 8 @@ -162,8 +163,8 @@ def create_auv_ocp(): ocp.constraints.ubu = u_max * np.ones(nu) ocp.constraints.idxbu = np.arange(nu, dtype=int) ocp.constraints.idxbx = np.array([7]) - ocp.constraints.lbx = np.array([-1.4]) - ocp.constraints.ubx = np.array([1.4]) + ocp.constraints.lbx = -1.4 + ocp.constraints.ubx = 1.4 ocp.dims.nbx = 1 # ---------------------------------- @@ -184,15 +185,17 @@ def create_auv_ocp(): print("yref_e:", ocp.cost.yref_e) print("ny:", ocp.dims.ny) print("ny_e:", ocp.dims.ny_e) + print("VX",ocp.cost.Vx) + print("VU", ocp.cost.Vu) ocp.solver_options.qp_solver = "FULL_CONDENSING_HPIPM" ocp.solver_options.qp_solver_warm_start=1 ocp.solver_options.hessian_approx = "GAUSS_NEWTON" #ocp.solver_options.integrator_type = "ERK" - ocp.solver_options.nlp_solver_type = "SQP_RTI" # fast real-time iteration + ocp.solver_options.nlp_solver_type = "SQP" # fast real-time iteration #ocp.solver_options.nlp_solver_max_iter = 100 #ocp.solver_options.globalization = 'MERIT_BACKTRACKING' - ocp.solver_options.levenberg_marquardt = 1e-4 + ocp.solver_options.levenberg_marquardt = 1e-2 ocp.solver_options.print_level = 2 # ---------------------------------- diff --git a/control/velocity_controller/src/LQR_setup.cpp b/control/velocity_controller/src/LQR_setup.cpp index 180ed791c..00065adc7 100644 --- a/control/velocity_controller/src/LQR_setup.cpp +++ b/control/velocity_controller/src/LQR_setup.cpp @@ -150,7 +150,7 @@ Eigen::Matrix LQRController::linearize(State s){ ret.setZero(); ret.block<5,5>(0,0)=A.block<5,5>(0,0); //legge inn integral state #TODO - ret.block<3,3>(5,0)=-Eigen::Matrix3d::Identity(); + ret.block<3,3>(5,0)=Eigen::Matrix3d::Identity(); return ret; }; @@ -166,7 +166,7 @@ Eigen::Vector LQRController::update_error(Guidance_data guidance_value //RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"windup status: %d, %d, %d",surge_windup,pitch_windup,yaw_windup); //RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"pitch value n state %f, %f",guidance_values.pitch,states.pitch); //RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"errors: %f, %f, %f",surge_error,pitch_error,yaw_error); - Eigen::Vector state_error= {-surge_error, -pitch_error, -yaw_error, -states.pitch_rate, -states.yaw_rate, integral_error_surge, integral_error_pitch, integral_error_yaw}; + Eigen::Vector state_error= {surge_error, pitch_error, yaw_error, states.pitch_rate, states.yaw_rate, integral_error_surge, integral_error_pitch, integral_error_yaw}; return state_error; } Eigen::Vector LQRController::saturate_input(Eigen::Vector u){ @@ -207,7 +207,7 @@ Eigen::Vector LQRController::calculate_thrust(State state, Guidance_da K_l(1,0),K_l(1,1),K_l(1,2),K_l(1,3),K_l(1,4),K_l(1,5),K_l(1,6),K_l(1,7), K_l(2,0),K_l(2,1),K_l(2,2),K_l(2,3),K_l(2,4),K_l(2,5),K_l(2,6),K_l(2,7)); */ - return saturate_input(- (K_l*state_error)); + return saturate_input( (K_l*state_error)); } void LQRController::reset_controller(){ integral_error_surge=0.0; diff --git a/control/velocity_controller/src/NMPC_acados.cpp b/control/velocity_controller/src/NMPC_acados.cpp index 3db57ce8e..9f7e99a11 100644 --- a/control/velocity_controller/src/NMPC_acados.cpp +++ b/control/velocity_controller/src/NMPC_acados.cpp @@ -17,6 +17,7 @@ AuvNMPC::~AuvNMPC() { auv_model_acados_free(capsule_); auv_model_acados_free_capsule(capsule_); capsule_ = nullptr; + std::cout << "Destroying AuvNMPC..." << std::endl; } } @@ -24,6 +25,7 @@ bool AuvNMPC::init() { capsule_ = auv_model_acados_create_capsule(); if (!capsule_) return false; + int status = auv_model_acados_create(capsule_); if (status) { std::cerr << "[AuvNMPC] create failed, status: " << status << std::endl; @@ -36,11 +38,21 @@ bool AuvNMPC::init() { nlp_in_ = auv_model_acados_get_nlp_in(capsule_); nlp_out_= auv_model_acados_get_nlp_out(capsule_); N_ = (N_override_ > 0) ? N_override_ : AUV_MODEL_N; // fallback - return true; } +bool AuvNMPC::initialize_guess(std::array x,std::array u_init){ + for (int i=0;i& Wd, const std::vector& We) { + std::vector W_diag_(NY, 0.0); + std::vector W_e_diag_(NY_E, 0.0); if ((int)Wd.size() == NY) { W_diag_ = Wd; } else { @@ -53,6 +65,11 @@ void AuvNMPC::set_weights(const std::vector& Wd, const std::vector(x0.data())); ocp_nlp_constraints_model_set(config_, dims_, nlp_in_, nlp_out_, 0, "ubx", const_cast(x0.data())); - // Build W and W_e from current diagonals (could cache) - std::vector W(NY * NY, 0.0); - set_diag(W.data(), NY, W_diag_); - std::vector W_e(NY_E * NY_E, 0.0); - set_diag(W_e.data(), NY_E, W_e_diag_); + // Update stages // Stage yref: [u, q, r, theta, psi, tau1, tau2, tau3] // Matches Vx selecting states [0,4,5,7,8] and Vu selecting all 3 inputs double yref[NY] = { xr[0], // u (surge velocity) - xr[4], // q (pitch rate) - xr[5], // r (yaw rate) - xr[7], // theta (pitch) - xr[8], // psi (yaw) + xr[1], // q (pitch rate) + xr[2], // r (yaw rate) + xr[3], // theta (pitch) + xr[4], // psi (yaw) ur[0], // tau_surge ur[1], // tau_pitch ur[2] // tau_yaw }; for (int k = 0; k < N_; ++k) { ocp_nlp_cost_model_set(config_, dims_, nlp_in_, k, "yref", yref); - ocp_nlp_cost_model_set(config_, dims_, nlp_in_, k, "W", W.data()); + ocp_nlp_cost_model_set(config_, dims_, nlp_in_, k, "W", W_.data()); } // Terminal yref_e: [u, q, r, theta, psi] double yref_e[NY_E] = { xr[0], // u - xr[4], // q - xr[5], // r - xr[7], // theta - xr[8] // psi + xr[1], // q + xr[2], // r + xr[3], // theta + xr[4] // psi }; - ocp_nlp_cost_model_set(config_, dims_, nlp_in_, N_, "yref", yref_e); - ocp_nlp_cost_model_set(config_, dims_, nlp_in_, N_, "W", W_e.data()); + ocp_nlp_cost_model_set(config_, dims_, nlp_in_, N_, "yref_e", yref_e); + ocp_nlp_cost_model_set(config_, dims_, nlp_in_, N_, "W_e", W_e_.data()); // Solve (blocking) /* @@ -112,9 +125,23 @@ int AuvNMPC::solve_once() double u_init[NU] = {0.0, 0.0, 0.0}; for (int k = 0; k < N_; ++k) { ocp_nlp_out_set(config_, dims_, nlp_out_,nlp_in_, k, "u", u_init); - } - */ + }*/ + int status = auv_model_acados_solve(capsule_); + + if (status == 0) { + std::cout << "--- Predicted Trajectory ---" << std::endl; + for (int k = 0; k <= N_; ++k) { + double x_k[NX]; + ocp_nlp_out_get(config_, dims_, nlp_out_, k, "x", x_k); + std::cout << "Step " << k << ": " <& x){ }; -void AuvNMPC::setReference(const std::array& x_ref, const std::array& u_ref){ - xr=x_ref; - ur=u_ref; +void AuvNMPC::setReference(const std::array& x_ref){ //surge, pitch_rate, yaw_rate, pitch, yaw + xr=x_ref; std::cout<<"xr: "; - for (int i=0;i #include #include +#include #include #include #include @@ -44,7 +45,8 @@ bool NMPC_controller::set_matrices(std::vector Q,std::vector R,s D_high=Eigen::Map>(water_r_high.data(),6,6); M_inv=inertia_matrix_.inverse(); mass=inertia_matrix[0]; - Ix=inertia_matrix_(3,3); + Ix=inertia_matrix_(3,3);std::vector Q2; + std::vector R2; Iy=inertia_matrix_(4,4); Iz=inertia_matrix_(5,5); @@ -62,7 +64,7 @@ bool NMPC_controller::set_interval(double interval){ bool NMPC_controller::initialize_MPC(){ using SYM=casadi::MX; SYM X=SYM::sym("X",n,1); //u,v,w,p,q,r,phi,theta,psi - SYM A=SYM::zeros(n,n); + //SYM A=SYM::zeros(n,n); casadi::DM M_i=casadi::DM::zeros(6,6); SYM U=SYM::sym("U",3,1); casadi::DM Q=casadi::DM::zeros(n,n); @@ -71,7 +73,8 @@ bool NMPC_controller::initialize_MPC(){ U(1,0)=SYM::sym("u_pitch"); U(2,0)=SYM::sym("u_yaw");*/ - //Creating M_i matrix + //Creating M_i matrixstd::vector Q2; + std::vector R2; for (int i=0;i g_list; g_list.push_back(X_v[0]-p_x0); for (int i=0; i lbx_parts, ubx_parts; + x_min(7)=-1.4; + x_max(7)=1.4; // X0..XN for (int k = 0; k <= N; ++k) { lbx_parts.push_back(x_min); @@ -242,9 +251,14 @@ bool NMPC_controller::initialize_MPC(){ nlp["g"]=G; nlp["p"]=P; casadi::Dict opts1; - opts1["ipopt.print_level"]=0; + opts1["ipopt.print_level"]=2; opts1["print_time"]=false; opts1["ipopt.sb"]="yes"; + opts1["expand"]=true; + opts1["jit"]=false; + opts1["ipopt.tol"]=1e-4; + opts1["ipopt.max_iter"]=100; + opts1["ipopt.linear_solver"]="mumps"; //robust default solver=casadi::nlpsol("solver","ipopt",nlp,opts1); @@ -269,11 +283,11 @@ bool NMPC_controller::initialize_MPC(){ return true; } -Eigen::Matrix NMPC_controller::calculate_thrust(Guidance_data guidance_values, State state){ +bool NMPC_controller::calculate_thrust(Guidance_data guidance_values, State state){ casadi::DM x0_val={state.surge,state.sway,state.heave,state.roll_rate,state.pitch_rate,state.yaw_rate,state.roll,state.pitch,state.yaw}; casadi::DM xr_val={guidance_values.surge,guidance_values.sway,guidance_values.heave,guidance_values.roll_rate,guidance_values.pitch_rate,guidance_values.yaw_rate,guidance_values.roll,guidance_values.pitch,guidance_values.yaw}; - casadi::DM ur_val=casadi::DM::zeros(m); + casadi::DM ur_val={0,0,0}; Pval=casadi::DM::vertcat({x0_val,xr_val,ur_val}); // Solve @@ -287,8 +301,8 @@ Eigen::Matrix NMPC_controller::calculate_thrust(Guidance_data guid auto sol = solver(solver_in); if (sol.count("x") == 0) { RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "NLP solver failed"); - return {9999,9999,9999}; //TODO: check for 9999,9999,9999 - } + return 1; + } casadi::DM Zstar = sol.at("x"); // optimal stacked decision vector //TODO: check to see NAN or INF values in solution @@ -325,9 +339,13 @@ Eigen::Matrix NMPC_controller::calculate_thrust(Guidance_data guid Z0_next = vertcat(Z0_next_parts); - Eigen::Matrix result; - result(0) = double(u0_star(0)); - result(1) = double(u0_star(1)); - result(2) = double(u0_star(2)); - return result; + + thrust(0) = double(u0_star(0)); + thrust(1) = double(u0_star(1)); + thrust(2) = double(u0_star(2)); + return 0; } + +Eigen::Matrix NMPC_controller::get_thrust(){ + return thrust; +} \ No newline at end of file diff --git a/control/velocity_controller/src/auv_ocp.json b/control/velocity_controller/src/auv_ocp.json deleted file mode 100644 index a7d46cc48..000000000 --- a/control/velocity_controller/src/auv_ocp.json +++ /dev/null @@ -1,1142 +0,0 @@ -{ - "code_gen_opts": { - "acados_include_path": "/home/henrik/ros2_ws_v/src/acados/include", - "acados_lib_path": "/home/henrik/ros2_ws_v/src/acados/lib", - "acados_link_libs": { - "clarabel": "", - "daqp": "", - "hpmpc": "", - "ooqp": "", - "openmp": "-fopenmp", - "osqp": "-losqp", - "qpdunes": "", - "qpoases": "-lqpOASES_e" - }, - "acados_version": "508482dac", - "code_export_directory": "/home/henrik/ros2_ws_v/src/vortex-auv/control/velocity_controller/src/build_auv_solver", - "cython_include_dirs": [ - "/usr/lib/python3/dist-packages/numpy/core/include", - "/usr/include/python3.10" - ], - "json_file": "auv_ocp.json", - "os": "unix", - "shared_lib_ext": ".so" - }, - "constraints": { - "C": [], - "C_e": [], - "D": [], - "constr_type": "BGH", - "constr_type_0": "BGH", - "constr_type_e": "BGH", - "constr_types": [ - "BGH", - "BGP" - ], - "has_x0": true, - "idxbu": [ - 0, - 1, - 2 - ], - "idxbx": [ - 7 - ], - "idxbx_0": [ - 0, - 1, - 2, - 3, - 4, - 5, - 6, - 7, - 8 - ], - "idxbx_e": [], - "idxbxe_0": [ - 0, - 1, - 2, - 3, - 4, - 5, - 6, - 7, - 8 - ], - "idxs_rev": [], - "idxs_rev_0": [], - "idxs_rev_e": [], - "idxsbu": [], - "idxsbx": [], - "idxsbx_e": [], - "idxsg": [], - "idxsg_e": [], - "idxsh": [], - "idxsh_0": [], - "idxsh_e": [], - "idxsphi": [], - "idxsphi_0": [], - "idxsphi_e": [], - "lbu": [ - -99.5, - -99.5, - -99.5 - ], - "lbx": [ - -1.4 - ], - "lbx_0": [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - "lbx_e": [], - "lg": [], - "lg_e": [], - "lh": [], - "lh_0": [], - "lh_e": [], - "lphi": [], - "lphi_0": [], - "lphi_e": [], - "ls": [], - "ls_0": [], - "ls_e": [], - "lsbu": [], - "lsbx": [], - "lsbx_e": [], - "lsg": [], - "lsg_e": [], - "lsh": [], - "lsh_0": [], - "lsh_e": [], - "lsphi": [], - "lsphi_0": [], - "lsphi_e": [], - "ubu": [ - 99.5, - 99.5, - 99.5 - ], - "ubx": [ - 1.4 - ], - "ubx_0": [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - "ubx_e": [], - "ug": [], - "ug_e": [], - "uh": [], - "uh_0": [], - "uh_e": [], - "uphi": [], - "uphi_0": [], - "uphi_e": [], - "us": [], - "us_0": [], - "us_e": [], - "usbu": [], - "usbx": [], - "usbx_e": [], - "usg": [], - "usg_e": [], - "ush": [], - "ush_0": [], - "ush_e": [], - "usphi": [], - "usphi_0": [], - "usphi_e": [] - }, - "cost": { - "Vu": [ - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 1.0, - 0.0, - 0.0 - ], - [ - 0.0, - 1.0, - 0.0 - ], - [ - 0.0, - 0.0, - 1.0 - ] - ], - "Vu_0": [ - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 1.0, - 0.0, - 0.0 - ], - [ - 0.0, - 1.0, - 0.0 - ], - [ - 0.0, - 0.0, - 1.0 - ] - ], - "Vx": [ - [ - 1.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 1.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 1.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 1.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 1.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ] - ], - "Vx_0": [ - [ - 1.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 1.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 1.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 1.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 1.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ] - ], - "Vx_e": [ - [ - 1.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 1.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 1.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 1.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 1.0, - 0.0, - 0.0, - 0.0, - 0.0 - ] - ], - "Vz": [], - "Vz_0": [], - "W": [ - [ - 10.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 1.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 1.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 10.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 10.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.01, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.001, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 1.0 - ] - ], - "W_0": [ - [ - 10.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 1.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 1.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 10.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 10.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.01, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.001, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 1.0 - ] - ], - "W_e": [ - [ - 10.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 1.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 1.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 10.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 10.0 - ] - ], - "Zl": [], - "Zl_0": [], - "Zl_e": [], - "Zu": [], - "Zu_0": [], - "Zu_e": [], - "cost_ext_fun_type": "casadi", - "cost_ext_fun_type_0": "casadi", - "cost_ext_fun_type_e": "casadi", - "cost_ext_fun_types": [ - "casadi", - "generic" - ], - "cost_function_ext_cost": null, - "cost_function_ext_cost_0": null, - "cost_function_ext_cost_e": null, - "cost_source_ext_cost": null, - "cost_source_ext_cost_0": null, - "cost_source_ext_cost_e": null, - "cost_type": "LINEAR_LS", - "cost_type_0": "LINEAR_LS", - "cost_type_e": "LINEAR_LS", - "cost_types": [ - "LINEAR_LS", - "NONLINEAR_LS", - "EXTERNAL", - "CONVEX_OVER_NONLINEAR", - "AUTO" - ], - "yref": [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - "yref_0": [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - "yref_e": [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - "zl": [], - "zl_0": [], - "zl_e": [], - "zu": [], - "zu_0": [], - "zu_e": [] - }, - "dims": { - "N": 20, - "n_global_data": 0, - "nbu": 3, - "nbx": 1, - "nbx_0": 9, - "nbx_e": 0, - "nbxe_0": 9, - "ng": 0, - "ng_e": 0, - "nh": 0, - "nh_0": 0, - "nh_e": 0, - "np": 0, - "np_global": 0, - "nphi": 0, - "nphi_0": 0, - "nphi_e": 0, - "nr": 0, - "nr_0": 0, - "nr_e": 0, - "ns": 0, - "ns_0": 0, - "ns_e": 0, - "nsbu": 0, - "nsbx": 0, - "nsbx_e": 0, - "nsg": 0, - "nsg_e": 0, - "nsh": 0, - "nsh_0": 0, - "nsh_e": 0, - "nsphi": 0, - "nsphi_0": 0, - "nsphi_e": 0, - "nu": 3, - "nx": 9, - "nx_next": 9, - "ny": 8, - "ny_0": 8, - "ny_e": 5, - "nz": 0 - }, - "external_function_files_model": [ - "auv_model_model/auv_model_expl_ode_fun.c", - "auv_model_model/auv_model_expl_vde_forw.c", - "auv_model_model/auv_model_expl_vde_adj.c" - ], - "external_function_files_ocp": [], - "hash": "4a78e726b7f1dc57012a3685a64c9a29", - "model": { - "con_h_expr": [], - "con_h_expr_0": [], - "con_h_expr_e": [], - "con_phi_expr": [], - "con_phi_expr_0": [], - "con_phi_expr_e": [], - "con_r_expr": [], - "con_r_expr_0": [], - "con_r_expr_e": [], - "con_r_in_phi": [], - "con_r_in_phi_0": [], - "con_r_in_phi_e": [], - "cost_conl_custom_outer_hess": [], - "cost_conl_custom_outer_hess_0": [], - "cost_conl_custom_outer_hess_e": [], - "cost_expr_ext_cost": [], - "cost_expr_ext_cost_0": [], - "cost_expr_ext_cost_custom_hess": [], - "cost_expr_ext_cost_custom_hess_0": [], - "cost_expr_ext_cost_custom_hess_e": [], - "cost_expr_ext_cost_e": [], - "cost_psi_expr": [], - "cost_psi_expr_0": [], - "cost_psi_expr_e": [], - "cost_r_in_psi_expr": [], - "cost_r_in_psi_expr_0": [], - "cost_r_in_psi_expr_e": [], - "cost_y_expr": [], - "cost_y_expr_0": [], - "cost_y_expr_e": [], - "disc_dyn_custom_hess_ux_expr": [], - "disc_dyn_custom_jac_ux_expr": [], - "disc_dyn_expr": [], - "dyn_disc_fun": null, - "dyn_disc_fun_jac": null, - "dyn_disc_fun_jac_hess": null, - "dyn_ext_fun_type": "casadi", - "dyn_generic_source": null, - "dyn_impl_dae_fun": null, - "dyn_impl_dae_fun_jac": null, - "dyn_impl_dae_jac": null, - "expression_names": [ - "f_expl_expr", - "p", - "p_global", - "pi", - "u", - "x", - "xdot", - "z" - ], - "f_expl_expr": "SX(@1=30, @2=-30, @3=((Fx-(((@1*w)*q)+((@2*v)*r)))-((23*u)+(fabs(u)*u))), @4=46, @5=((((@2*w)*p)+((@1*u)*r))+((@4*v)+(fabs(v)*v))), @6=((((@1*v)*p)+((@2*u)*q))+((@4*w)+(fabs(w)*w))), @7=((((((@1*w)*v)+((@2*v)*w))+((-3.34*r)*q))+((3.32*q)*r))+((@4*p)+(fabs(p)*p))), @8=((My-(((((@2*w)*u)+((@1*u)*w))+((3.34*r)*p))+((-0.68*p)*r)))-((@4*q)+(fabs(q)*q))), @9=((Mz-(((((@1*v)*u)+((@2*u)*v))+((-3.32*q)*p))+((0.68*p)*q)))-((@4*r)+(fabs(r)*r))), @10=-0.000546005, [((((((0.0334536*@3)-(6.0369e-05*@5))-(-1.11163e-07*@6))-(9.80847e-08*@7))+(1.09201e-05*@8))+(-0.00601506*@9)), ((((((6.0369e-05*@3)-(0.0334847*@5))-(-6.16582e-05*@6))-(5.44043e-05*@7))+(0.00605702*@8))+(-0.00301845*@9)), ((((((-1.11163e-07*@3)-(-6.16582e-05*@5))-(0.0339635*@6))-(-0.0299678*@7))+(-0.00308013*@8))+(5.55814e-06*@9)), ((((((9.80847e-08*@3)-(5.44043e-05*@5))-(-0.0299678*@6))-(1.49703*@7))+(0.00271776*@8))+(-4.90424e-06*@9)), ((((((1.09201e-05*@3)-(0.00605702*@5))-(-0.00308013*@6))-(0.00271776*@7))+(0.302578*@8))+(@10*@9)), ((((((-0.00601506*@3)-(-0.00301845*@5))-(5.55814e-06*@6))-(-4.90424e-06*@7))+(@10*@8))+(0.300753*@9)), ((p+((sin(phi)*tan(theta))*q))+((cos(phi)*tan(theta))*r)), ((cos(phi)*q)-(sin(phi)*r)), (((sin(phi)/cos(theta))*q)+((cos(phi)/cos(theta))*r))])", - "f_impl_expr": [], - "gnsf_model": null, - "name": "auv_model", - "nu_original": null, - "p": "SX(0x1)", - "p_global": "SX(0x1)", - "pi": "SX([pi_0, pi_1, pi_2, pi_3, pi_4, pi_5, pi_6, pi_7, pi_8])", - "serialized_expressions": "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", - "t": [], - "t0": null, - "t_label": "t", - "u": "SX([Fx, My, Mz])", - "u_labels": [ - "u0", - "u1", - "u2" - ], - "x": "SX([u, v, w, p, q, r, phi, theta, psi])", - "x_labels": [ - "x0", - "x1", - "x2", - "x3", - "x4", - "x5", - "x6", - "x7", - "x8" - ], - "xdot": "SX([xdot_0, xdot_1, xdot_2, xdot_3, xdot_4, xdot_5, xdot_6, xdot_7, xdot_8])", - "z": "SX(0x1)" - }, - "name": "auv_model", - "p_global_values": [], - "parameter_values": [], - "problem_class": "OCP", - "ros_opts": null, - "simulink_opts": null, - "solver_options": { - "N_horizon": 20, - "Tsim": 0.1, - "adaptive_levenberg_marquardt_lam": 5.0, - "adaptive_levenberg_marquardt_mu0": 0.001, - "adaptive_levenberg_marquardt_mu_min": 1e-16, - "adaptive_levenberg_marquardt_obj_scalar": 2.0, - "allow_direction_mode_switch_to_nominal": true, - "anderson_activation_threshold": 10.0, - "as_rti_iter": 1, - "as_rti_level": 4, - "byrd_omojokon_slack_relaxation_factor": 1.00001, - "collocation_type": "GAUSS_LEGENDRE", - "cost_discretization": "EULER", - "cost_scaling": [ - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 1.0 - ], - "custom_templates": [], - "custom_update_copy": true, - "custom_update_filename": "", - "custom_update_header_filename": "", - "eval_residual_at_max_iter": false, - "exact_hess_constr": 1, - "exact_hess_cost": 1, - "exact_hess_dyn": 1, - "ext_cost_num_hess": 0, - "ext_fun_compile_flags": "-O2", - "ext_fun_expand_constr": false, - "ext_fun_expand_cost": false, - "ext_fun_expand_dyn": false, - "ext_fun_expand_precompute": false, - "fixed_hess": 0, - "globalization": "FIXED_STEP", - "globalization_alpha_min": 0.05, - "globalization_alpha_reduction": 0.7, - "globalization_eps_sufficient_descent": 0.0001, - "globalization_fixed_step_length": 1.0, - "globalization_full_step_dual": 0, - "globalization_funnel_fraction_switching_condition": 0.001, - "globalization_funnel_init_increase_factor": 15.0, - "globalization_funnel_init_upper_bound": 1.0, - "globalization_funnel_initial_penalty_parameter": 1.0, - "globalization_funnel_kappa": 0.9, - "globalization_funnel_sufficient_decrease_factor": 0.9, - "globalization_funnel_use_merit_fun_only": false, - "globalization_line_search_use_sufficient_descent": 0, - "globalization_use_SOC": 0, - "hessian_approx": "GAUSS_NEWTON", - "hpipm_mode": "BALANCE", - "integrator_type": "ERK", - "levenberg_marquardt": 0.0001, - "log_dual_step_norm": false, - "log_primal_step_norm": false, - "model_external_shared_lib_dir": null, - "model_external_shared_lib_name": null, - "nlp_qp_tol_min_comp": 1e-11, - "nlp_qp_tol_min_eq": 1e-10, - "nlp_qp_tol_min_ineq": 1e-10, - "nlp_qp_tol_min_stat": 1e-09, - "nlp_qp_tol_reduction_factor": 0.1, - "nlp_qp_tol_safety_factor": 0.1, - "nlp_qp_tol_strategy": "FIXED_QP_TOL", - "nlp_solver_ext_qp_res": 0, - "nlp_solver_max_iter": 100, - "nlp_solver_tol_comp": 1e-06, - "nlp_solver_tol_eq": 1e-06, - "nlp_solver_tol_ineq": 1e-06, - "nlp_solver_tol_min_step_norm": 0.0, - "nlp_solver_tol_stat": 1e-06, - "nlp_solver_type": "SQP_RTI", - "nlp_solver_warm_start_first_qp": false, - "nlp_solver_warm_start_first_qp_from_nlp": false, - "print_level": 2, - "qp_solver": "FULL_CONDENSING_HPIPM", - "qp_solver_cond_N": 20, - "qp_solver_cond_block_size": null, - "qp_solver_cond_ric_alg": 1, - "qp_solver_iter_max": 50, - "qp_solver_mu0": 0.0, - "qp_solver_ric_alg": 1, - "qp_solver_t0_init": 2, - "qp_solver_tol_comp": null, - "qp_solver_tol_eq": null, - "qp_solver_tol_ineq": null, - "qp_solver_tol_stat": null, - "qp_solver_warm_start": 1, - "qpscaling_lb_norm_inf_grad_obj": 0.0001, - "qpscaling_scale_constraints": "NO_CONSTRAINT_SCALING", - "qpscaling_scale_objective": "NO_OBJECTIVE_SCALING", - "qpscaling_ub_max_abs_eig": 100000.0, - "reg_adaptive_eps": false, - "reg_epsilon": 0.0001, - "reg_max_cond_block": 10000000.0, - "reg_min_epsilon": 1e-08, - "regularize_method": "NO_REGULARIZE", - "rti_log_only_available_residuals": 0, - "rti_log_residuals": 0, - "search_direction_mode": "NOMINAL_QP", - "sens_forw_p": false, - "shooting_nodes": [ - 0.0, - 0.1, - 0.2, - 0.30000000000000004, - 0.4, - 0.5, - 0.6, - 0.7, - 0.7999999999999999, - 0.8999999999999999, - 0.9999999999999999, - 1.0999999999999999, - 1.2, - 1.3, - 1.4000000000000001, - 1.5000000000000002, - 1.6000000000000003, - 1.7000000000000004, - 1.8000000000000005, - 1.9000000000000006, - 2.0000000000000004 - ], - "sim_method_jac_reuse": [ - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0 - ], - "sim_method_newton_iter": 3, - "sim_method_newton_tol": 0.0, - "sim_method_num_stages": [ - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4 - ], - "sim_method_num_steps": [ - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4 - ], - "solution_sens_qp_t_lam_min": 1e-09, - "store_iterates": false, - "tau_min": 0.0, - "tf": 2.0, - "time_steps": [ - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1 - ], - "timeout_heuristic": "LAST", - "timeout_max_time": 0.0, - "use_constraint_hessian_in_feas_qp": false, - "with_adaptive_levenberg_marquardt": false, - "with_anderson_acceleration": false, - "with_batch_functionality": false, - "with_solution_sens_wrt_params": false, - "with_value_sens_wrt_params": false - }, - "zoro_description": null -} \ No newline at end of file diff --git a/control/velocity_controller/src/build_auv_solver/acados_solver_auv_model.o b/control/velocity_controller/src/build_auv_solver/acados_solver_auv_model.o deleted file mode 100644 index f60968d67ff0a8630a084d28ea31190d9e5ac548..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 31144 zcmcJ13w%`7x$mA3LO_}cl+v_P9qg!~g_w~5i51Pj1a@=+9083=orGlau*oFN%=@+ zWoI_+x&7VsW6xguzy9C1zV)s5+BT&i$)J{1na;=SIa+T`wWe6F z`!9Is;Vu9t2<+HH&-UQS(&O|P8ny=ymLBNnsPbR9g8+-u!#9Ml4?AbVtHY~SuOzaa zSy~y%)YpdVPN&z|nZD|1rn5Ghxoqgnmg{Wi-y1?_JB)%x=gElkb~HPlv_+jiMx7UJ z=M~OPCmAd}IO4Mh50q*yQyS{tPd`XDg@$kbba+kp)8Sjfu><}K>n%IO8l6||?1xD8 zL*xWIQyUs)L6X@|&?vEp#pUPnVg? z*5La-|L&h7lD+ zF%+Dbd-m*EJNp4r_R~A@V`t}4+|Dkf8{16vLhVU66z9)qD|Wk`DZaRpm`)^EOD!jw zZ4b_mI)AX+x@uYOa3nb2H}vw>kJ}kg+U-v&0sqi1sa6IL+Nx-(-m{Tjt0DCA>;5fl z3+`P1xPOccv9moxmi@@@i~Z9%-9Md^{nJ&d^NN2~G#K#DS`{q!&zceB{B38kZ95|` zP4Hhx^<{VX`y#;rext$isP7dtS;VIW8@C@if*kx)mR9?xEU))ZxnTv1N9$frKZ^V> z=KP&kly!6H2`GEXKb;f((;4+oSKzF6vZRMBsmWQ=OqP5gSAVdl8M&`vd)kFPv-9@E zU{5FP=^JeiR}4Cv@Rol%gT^sl*hW;>k6!!EL{xZYDaNB% zb)$0@BN0ZSOR1p+?4if~J1#7>tPI4tEU)>?dC0KaRs~BNLXU&6`|GG9+gTN?c+Ed0 z60C$h=$F?uWtQ6!=Qrq&P1)I>vz=46bF{(vT_ajmZXiSZY4V+5#h^DwoL52DsY*g5 zx11w6nt`gu!Jqk}b+E%fvJ){jhbaMnaDvA7s$zJfd@JbPmBjwj`=Y zRwY>vNrqWnIZF~*jhxEJ*TViO4R(`rBB!juYIKI4fGBQ({HRF8p*;%ja=~xm8SH(y z6)?jtXvQe(PoaRQhoiXF+xXc=8PT2WpgS2-Ky^hy)ra!FK_N+JrPkd~y7n8P(cUUG zaD-Znav8i8<}v>&44&bKVT6UrEblE<6~_A!=dFh9Rv*n-PeOOtIjlt3@Pbv6uMJfX zjn3cZeYD?m+edS3xbCpBkIN&BvyBIGg@?Py{oZxbvFC5d&sBSH2GCE*M%5PK|9;OuwBgzj`@rZ$& zCq>+OC>*H|lmc#a4*RE+LZcq^vBGNz4Z~I8>!X?5gVk!Nh^omSMn#>sbyjaHo=(y{ zXSSNmKyxQIW*acm9<`lfLwVD~=$x>&QG4GxYHT-Ui8`=Q8$d-sNrZZ}(tLRGXkXCM z3We4qVR&RoDJ}FGof0+%{%7wrIwN-0uITVj8;09E{FRJS9)_Wk>I31M!Z$}UJ(yO8 zUiR;P63T2R8mz#?8%3>^M=_!V{1*T6cQm5Ywm~t2z z_U}A~I9tdKka|M}mO@~u^-PoVHZ4BP{wrtmVPE6mh#HR?>i)F(2_AGRZL*yTpcX`G zJm94A69>NTtyj_7xLGW|cVHBt zV}il=r=>oPC3d=5_aEog@P68fdhEtCN>FU=4q-^Pw;!b33hvXXKi6i~1Xp2nu7_ev zFL9k`8=WPU=s>E1GKdeJtbg6#hkjBV#NEaWfPEWcD+2y0t7wDM*NnckCRlC!SPvDL z)DAR4I+E$H4QCU+Xxn?&X0Ob5)oZfT9;9q}Ejhb98<~Tx@}s`vP1zX^B8}M^2UAqz zBY!BdA9=3Wc4~sz)IVEIzF#+GiznJ`zw(`F%wFkaCtHskE3(@jBafC6!d*3c<;z^8 zw`ZL|T4Q!HVC+imwqN^R!FWMHQ#SY$yX}47DZ7m{{RA2QkYp=pKU8nGo%S8KzeZqF zcG~vO94e!S;_cK@ZRhwS$BG>cO0!!^V_Q=^y$# zI#$6 z4JC|bl+*k>i%Mw?5)E#^a;qWqLKN+|ni_Crqw}ooycBf~sq;e2<`J5+pT#Jur;cUW z;!0hR;-E^@Dd6N1mr>_;r)n`w4DOVi9JRBGbEcMirY_GGU*=8>4<5v%eMn7Bk-B!Y z@$AKD<5B9Knq`Hl z@23xvkFdIl&|okb_5GSs!8UT9l-93tQfJnhgtz2zjKS)>g?pBk#B>F9jZUJrF|$QY zHI12Du!`Ijya%0IH*U;40oyr5GmpxAWCtgMVMQ`6>O6GR2@NSYmRDdqJNS`vZOp6- zQ2Jg<&vhWEB!I#w*1Z5mu?V2RES;%QNtm{oWRNQ%qx5}`C^A<9C@>3Fd|y3U4MQ*Z zcb|`8AIqm`_I}3oN_!6~vdKA}8(nfsqlUV7)Bgz_`=A4oMwA}4sM=@zCHebD%hTh3 zNuExNXz10Z)8k(+Z=(RGAwfYaphvBM+w(c?KfEJICzM8(;`z*XPzuJSbNDaUkWhiA zsG+g37{ZJcjGyy;}lFJKrr}(A+JS>4wIxB7oY7B>mD&B7lM<9l`S`n+TvFN#DTps4^3v8Rj?@^VhB*8JZ_j zw;@ET`YoxFJ|N)ooHpuDyMj-+Nd)A50LIhIQ1xnRi?F;mvGT#|mL#rCdgZGh@VY~7 z{J3K;E?3r#Vp5M+TUJ^&HDdvkW^=U;>XiudU7< zqnyI~Hx|mJjY_f|eTMh{HJOb%MOm1mtk>wZpvn8jTk24>Ef-M{V7xM=KoOd68a&`5 zsy|ysOMd+l1RLfG*K6a5djCrbhaO8`feCzX0=8h zdTPreyX`>eDLXr@KAH{g!TaoDoS$vBZJ${VZEhCv7R2rC2WULy{2S|@P9Fik#aLsT zNHa%n4h`cSzkC^}-w%rS_1jNc_sKblzVVs5I@nRasBzAUH zaGJJY6F<$6^^0MXLHzLBT4v%HbVj-Vi;ts7COx3jX)~?7ilJv3MolBg4P|iHS9ip} z>zeFrJ_*3S#m;^VTA>zO3ak~DWowQhI%8;f!zHwT!CQpc)3JF` zZE=_vx4u)XiO!H_Y<^bTp$S;|s#D3Px|8XDqCuGopF4m_*I+E(4x+T;Y1Ic0C7N z&$X`S$^)bvp*6iO*4x(}?}@dxwYK*qW9k0()>J%}>h6ullBsxq@`^xb_quo@kVyBg zjSmF+Is%jy;Iu&H!obDtvjXki9Ubw3WT0c9uQ!nDiU-=#0|W6ys%K-MwWp`A4I10e zrC{cy6+Q9RWIQksZySh%9a!I;>Ix*{>!CUcDAj#iYpT015ok@c2l@y4)^)eX1FcM0 z7jH}TFnuS+uEbO8tdrZ5O3$|j`hS7ovAKsAk`h~>FbR3 zC3-f-TGzF9_q49_ysY|g)JeTnwivdUS3L|-b<-P_+2??r=% zx6ckFyAy43z=8f&Yjie{Z0$wMObu)#sZ?Je z8BYZ+?k1J3y>a*&Z4r$kz5&q$vgk-B+Nk{`s{##fEYQ`O94$+vZmPGcT>fZQsaPV` z+qxk@O`a@F^{wC~xdUTQ9AzuhQnf*V09jxwVq>y|jFJ>A>8F=q!Tttqt2L|`pC z3v{4=KyYn5wLXr%rrkgZlutl)N?AG-4lO1=&<#!iJ+?Iv@95}m>sFl(susicRY@y} zbQHw+PdENtudixTJ)}U#h;QiciLGmo$Kc5I7C)ig#o$TxKuN9bw^|zq;vLov*7X~# z>(f?u`-Zg}tR7rxT>KR0TN+>*VA?{GHL0(yKSp`>qr><0_NUR~JJ7~dl_xe>iL{lt z%}OM#L_hw;RBN>oIufG-TM6WlfWkzQB1wvM;*M+WhPaj3XeGMv*k!54-4#zJE$+?m zL7Lh^EQv-JTR)JBsph0o(Mi)7`CLh(Wzfn~v7Xl6SZ{a2a`mKiPmk(I)J?4RjfvLY z?zUvCtxt`Ely7Qde>|UpYE%QQ8*1&0Cj}N?pMq>R8uS=2)hBYgwY3f16y3+_iDTTx znB9qHJMh_b>p*)7RhNpveDq^0iD5I&3eilH)}nAUylioUh19P8?tT;w)lE;S>R5N8 zJ7u*dV>BwT1v*YS^yJ2h{NTWT#!!uZ2%B5m+TuNWu(AZ2>R!hKSS-~A(XPIpcFKi)!%L9I(>{Q4~+&bi_KKM79~aJ?d4l{ zGKK1}D0Lz-pcJ~(^Cauy_KDUc3j2tU3xcc4y@#^DEYgb(T)!$um{Jg3uSIt>G zIs>3pJn5zv*&L2F59@n<2Zu8c>-*K#(#4Cf2vn|Kn@*(C zfzaHlP*wGuTJ%#qhPGUOc~y1IEDf^?(oahy%46xW)s*irVU#xo+W1lM-FBVj+feR1 zfAWOVJur}DW^jsSmFYWit3}+h%gV1UE49`Dvo-z;x-Tg!|6W@_298tr!Ar5wR8JqY1xsYq6Eq@ zw&s_dmnhpat`CH9ThA+4rPm-oYO9ax{3uR=?oVs%Vgj1V%E>3EHFk5Z9i7W3o6DN7 zDQhYE7xuiqL95KQ2-h7%VROVOhmu?p&x7B%jvYFXzh7{j^Y< zSX#C}5G~tZkE~8E{roT@I&q?wq8A3APHlq?pPx6e8hbi!2=qbsqZ0;>qqMX z6Ty94e#FnAOF3q|l7p=#51iYXH8Gd3k+@Ow0|wCdn8vBy$PniTnuSJ(TlujhH(!|4 zK8~1C(4dx;;;5v`2Rkplv-{-@7H>0jFjOaekie& zU-KykIXaR1M;^7TpKJbr1Q9=?@eMBgcN)Llg+HzFZ7%$Ijo;zIk83>Z!cS@ZOD_Bk zex8_HH<;viI8ti4TZ-y%+csPF2_iW2LqE_{-YfD-F27hbOMue>B9Rp{;msOr*Zeddb`Gp_?{Q~oW@IB z_)dNvZ@uNhcWeF>7k;P4FLU8{^K*Y_-d_KVtr7rkBrFknty={ zKd$jPF8q|n=eh7VG``q{zoYSGE}S<=@WV|myo8@itWFm`nV-j76)t?L=5KW2idSOY z>B3bxORW1`cu?zk$c2AGAHgk>)?^!flN|@4}TI zORN`M_$tl+g9}&w9dCWlg|}$_TQ0mq<37EPH0`8EUkzUhTp^ zukjieo@IQzRq4XNqVX;lez(>$--Z7p@Tp>5XZF000H?L7JJ5cE_$JM)(f^D+puwcf zi<)1p!C&b6MA%V?U*UmY=7BeO;873!Cg5CN?thlGM&CDi@IU8)@6mcxd$BCuQ=@GA zJ^0Uh;AcGWk7B)E$Uh4_@Kql8Mi2Zh5B#tP{)`7s2LgrcoZ*4jdf@nWvY?)g9{3&) z{QDmGqaOJ0J@8U&2?~{Kwg{9zCL8Lj`c z?zl&FzVCSOCt;&ab{_F7V!h_i0bZ!Q^&WVW2Y#K_U#^w$ISBID;=%v22mU<|`~eUA zcOLjD5BxkVzzdakrU!nd2foS!PkZ1yJ@C6d@O{9kzFPRLbKd4hHQulBi*-G|s`0%V z=kp$@9|K;<53@9X#YKu=t8SL9@jV(xGvLs!@x2;{dpT^^_&yi@4;mkK;SX#4hzmce z@nbIhJ&m7s;pb1*?eOEu4r9+bUi`&kE3Ru;wd%&$lGV!=uWDSrEEcN@S%uJQt17zq z^7;I$;6Y-YFJARYpG$` z8kVhL*&3Fer%KAQ^H_Es%g$rjc`Q4RW#_T%JeHluvh!JXKFiK$+4(FxpJnH>)O=+= zOU-Ag1uV6Ir53Q%0+w39QVUd>S#|--E@0UOEL+R6wJcl9vb8K*%d)jBTg$SwEL+R6 zwJcl5vUMz5$Fg-SRmU3YShkL3>sYpqr9#y#7OG~sP_+_dwV`U33{}^7et4n3*|6vZ zdtcjFTE{><9y6re`)tpzSS<6#xtremrgD#MeM!6;oa35a=+>8jrX$3SZgvF$6IE4!F?_%yhhA@raz&bW>pk#T60b^=-KmXuq3b1!j~vdCxV~>d zmV8vMdF_2^Pt%~DQpXlAhw$56)NHb~y*s9S>bBKG;4RR>&T$PJ7csY0lWOelYSm3Y8~Dufy9uL#_KSHdfOF z)wdJ;#f_*X{k0BSYyRE1>43RUB67Vh=Bj)8#DqS5LAN(T_(Uj~8fe8Q07=nX^X1NG z$e)!^X^bf1+~dcPb$`u&f|3&vPz;}_AXnAp�$+;d72;zv)0T~0C{jmv zryFhW?#0I<{0&@AnlbA67y|H#!$5LuivB>x=*~$9zl{~5u`h?}FN1iLapf_98xZLhtF1(?@UkU@JM+HZD=li|VP6dUP(7 zzqd7o&%okI&ZDE1zJ~0+EpIH$I45t6Vwzb;XFi}NpY}NVVBf}E8rW6hl2+-PBDOZWJp=MFB%n9`~5i0HeYd^hSW)Ii$O_&};xe-Ashjv&KB z&8V?e4?`x6hM_SAP$GX`=9!xR7_YE2r7K5yr&B-nP9@)ar;@kzcE+N=u%JN4F9g1e zzc8Sn-oL8vj<6-k;ddZTf%yHn4FB5%A{61mRwal36M+cC|12)U{{?{vpTL#GW$@n; zh(LPiQxJpyCxHm0Cxy%47wB_H;?q%%!B=aX`0E6Imj`~Y2mUh;yq+>dpc|c8&7cQ_ zsKDu{$k?+|;IuzAc(caI&h-M{BXFtI|RN> z;6nnxL*Snm_}v}4v#_1;g-zV^Gg1<%J^iJN`b4K9wO@YDR*Em)G6oD_IMvp-FR9uF?T;n9RN8l}j z|3QH#JosA$pU%XLe)T6zs=Z154+uTIxeICe1U&h;57oLZ$=FNT7lD;Z15J1lRu@MT>_VOt{1qpbC=NnQKA1|1z+kv zAaJSwae+(ye-!%Z+a_cG1bxuyw%;#sss93jOZ|-+CqK)2yg~4#{V9P<{euFR`oALd z%X+*|;Ih6R6S%Ce69Sj@{-(y=ewd;U90`|p1_Uncyiw!i2U(Auf-n8BUEosx9)U~! zKN0$6J&p*z)c*&8OZ{gAF7=;B10jOD9zUXSw|`~|T>N~V8J6gaIx41S@;$)D2B zg@RA7It8(rz2MU;2c!QB9(;OFK*8Pq@Akm&6}a@zA&pbJ zl79F%flI$VDewSfjGZThep#+p1upB0-rrLo`=vd8S~Me&Jv1~L{TFK7T`!jkK6M?# zUnp>D8U}9^IGJGZI|V*d;Pifig1f$cAov#{-ROD9ga2=W|8c>8)`RbxOu3G&--#ah z2L&$u`ALDx_B>bPqriIvf4k5l^*<=^DT4pDz{zBjufGfr2$ZiJ z$1W4NELX$>k9y$O3tZaYC2(2qpA&cjHW@p=Byh=pK;V-9D~*%?X^Cg_{2zhKecW>b zzeMm~)HwM^?&Dq(cvkRdkx~TWOFM7UIN5V4E@Mwz@b47(ZGvAZaL0qcPw=JwA9?U! z5cuB<{eKpEW(mBAHn#{=uDfs<`zH(hYXV=X@lODg=KyOpPJW;?qvx|ik30u>K;U-^ z{?7%T7WjJt|AxSWQ}BR5_RDtoQQCwc5PlCXW6vcTCp$kW@EXDYZvu}9{%nCadhl-% z{5gW(=D|-2ewE;F@Zf(z@GlemFM9C5Dfrcb|1H7)roitL{E)zZ?7^Q%i+cp}=jFJJ zpQmb^{QnOE|G41K75HTy{IKBH2!5jn|7O9TC-|Kn{EdP?U+{-K_;(5Z0>S^52mgM- zr*9aHe-3)^Ulja0!Jj~jV+8W&UR*|x?SZe=IMvG)g1=etWxebc{7(t~w>|j(;eo&I zfq#S+@d)nr5c0qq1-=(@ro1Z!o)CDK!2eO;n>0>-xKiMI1^>Sb{JVlrLyO7x0f8?R z_+uV=jtc%&g8zyKe>`ng5y;M~ahZHSq;c|_v~z~w*9-n-0uKxPY7afD1%Hv?$36HN z!Cx%+_X_^E1b$fXBLaWHgI_|Ma|H5311{r-pvK7$QvVXcUn2PJ9{f85f2rW#>%l)D z__pA`;K4sF_>F=;o;H~XRIY1qnQ~3lxVwHY75r-je}M9B!M{%MOXzR}!R_bsHBR-i5|^o$ zPkQhd3jQj=Z}Q-;5_}pOjs6aSuNL?^4?P*dzh3aa;lbZ0_%{gtum}G?1ixAEfA7J6 zQ{eXsekpC{5y;OHpC<6HAkFw;rog`~@Yw?2An=6(|DwPf1io9~Hwyea0)I;5RPS{H ze_QbP3cQ#$r3l2o0++G>B8^jd<$O9<@INK^i#_<)30&?wwh28q3O#oS{#^q9lEA+v z@c%9FFA3a7n`ZgWGW9P&G9uNrs5iWyI4d4O6jh`Q2nDPIG zsNn|%{$qj95%_+A&lmVl1l}z0p9;L)17Gif4+&h>*KQB|E)V>q#;JZ=ahdX#&%^@) z`R4`lYQ_-h4zu?K&i;I|3>6(0Q6g5NIqF%N!1@Z*9%B>1wu-6QxNg8yp|{xgE# zDflmY@GCC@34#36h0FNuGL4fzr9E{5m+k*5fy;6AW{np?Pq)zDDfm)38x%lSqP2O2bjZ=0RJ(l)wzYA~C_%0W2 z{;p)N3*W2x_q*_6jX&zbk7@i37rrl`>|3GzXY5>Yk;1!N_#Tbl?!wLAQ<(ZOdiH7l zy)M4_dy4%o{D|iN(uJG9r#R-qPiy`gF5LRKvfrvoZtP99uEjkypzd8JHW2S=txDmu zG^>jLW3sBX_^-yQ2KwlM=%Ffn^e!LqLA-i2_o}vJD$l_K{RIU6V>J3N^Q`rf}>MOJ8k(=bH&lfeZNHGx|wMd|4qLuUz4D_t9i6$BSt#^U*$uP zC7pD}D8mY!|1&Pd#(rZ*J%kF$&!I&xLZeIG$eBC2iQ+{>7GWD5b|Zw?^3x8D0*Sm0Izqz*~y^wr49bzNY^Kbsg)W5k8Ac^G7cx{*~v^>?148)~i?wdrM z6ZFsRT3%ulH*yBgATj~h82@ScfFbI;DYt>Yg$U_%%kMS$k2g$pcguehaG~};{8`1{ zuN71KrzzUVoAzHQZ<5@r9lA$XEcyF01?1^nu&apyL47Rdn^iYi-OaDte^Zc9NFIY6 MhkrJL`tFwhza}_AS^xk5 diff --git a/control/velocity_controller/src/build_auv_solver/auv_model_model/auv_model_expl_ode_fun.c b/control/velocity_controller/src/build_auv_solver/auv_model_model/auv_model_expl_ode_fun.c deleted file mode 100644 index bf4e3e79a..000000000 --- a/control/velocity_controller/src/build_auv_solver/auv_model_model/auv_model_expl_ode_fun.c +++ /dev/null @@ -1,386 +0,0 @@ -/* This file was automatically generated by CasADi 3.7.2. - * It consists of: - * 1) content generated by CasADi runtime: not copyrighted - * 2) template code copied from CasADi source: permissively licensed (MIT-0) - * 3) user code: owned by the user - * - */ -#ifdef __cplusplus -extern "C" { -#endif - -/* How to prefix internal symbols */ -#ifdef CASADI_CODEGEN_PREFIX - #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID) - #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID - #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID) -#else - #define CASADI_PREFIX(ID) auv_model_expl_ode_fun_ ## ID -#endif - -#include - -#ifndef casadi_real -#define casadi_real double -#endif - -#ifndef casadi_int -#define casadi_int int -#endif - -/* Add prefix to internal symbols */ -#define casadi_f0 CASADI_PREFIX(f0) -#define casadi_fabs CASADI_PREFIX(fabs) -#define casadi_s0 CASADI_PREFIX(s0) -#define casadi_s1 CASADI_PREFIX(s1) -#define casadi_s2 CASADI_PREFIX(s2) - -/* Symbol visibility in DLLs */ -#ifndef CASADI_SYMBOL_EXPORT - #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) - #if defined(STATIC_LINKED) - #define CASADI_SYMBOL_EXPORT - #else - #define CASADI_SYMBOL_EXPORT __declspec(dllexport) - #endif - #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) - #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default"))) - #else - #define CASADI_SYMBOL_EXPORT - #endif -#endif - -casadi_real casadi_fabs(casadi_real x) { -/* Pre-c99 compatibility */ -#if __STDC_VERSION__ < 199901L - return x>0 ? x : -x; -#else - return fabs(x); -#endif -} - -static const casadi_int casadi_s0[3] = {9, 1, 1}; -static const casadi_int casadi_s1[3] = {3, 1, 1}; -static const casadi_int casadi_s2[3] = {0, 1, 1}; - -/* auv_model_expl_ode_fun:(i0[9],i1[3],i2[0])->(o0[9]) */ -static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { - casadi_real a00, a01, a02, a03, a04, a05, a06, a07, a08, a09, a10, a11; - casadi_real a12, a13, a14, a15, a16, a17, a18, a19; - a00=3.3453634479321918e-02; - a01=arg[1]? arg[1][0] : 0; - a02=30.; - a03=arg[0]? arg[0][2] : 0; - a04=(a02*a03); - a05=arg[0]? arg[0][4] : 0; - a06=(a04*a05); - a07=-30.; - a08=arg[0]? arg[0][1] : 0; - a09=(a07*a08); - a10=arg[0]? arg[0][5] : 0; - a11=(a09*a10); - a06=(a06+a11); - a01=(a01-a06); - a06=23.; - a11=arg[0]? arg[0][0] : 0; - a06=(a06*a11); - a12=casadi_fabs(a11); - a12=(a12*a11); - a06=(a06+a12); - a01=(a01-a06); - a00=(a00*a01); - a06=6.0368975005218254e-05; - a12=(a07*a03); - a13=arg[0]? arg[0][3] : 0; - a14=(a12*a13); - a15=(a02*a11); - a16=(a15*a10); - a14=(a14+a16); - a16=46.; - a17=(a16*a08); - a18=casadi_fabs(a08); - a18=(a18*a08); - a17=(a17+a18); - a14=(a14+a17); - a06=(a06*a14); - a00=(a00-a06); - a06=-1.1116270017027866e-07; - a02=(a02*a08); - a17=(a02*a13); - a07=(a07*a11); - a18=(a07*a05); - a17=(a17+a18); - a18=(a16*a03); - a19=casadi_fabs(a03); - a19=(a19*a03); - a18=(a18+a19); - a17=(a17+a18); - a06=(a06*a17); - a00=(a00-a06); - a06=9.8084735444363873e-08; - a04=(a04*a08); - a09=(a09*a03); - a04=(a04+a09); - a09=-3.3399999999999999e+00; - a09=(a09*a10); - a09=(a09*a05); - a04=(a04+a09); - a09=3.3199999999999998e+00; - a09=(a09*a05); - a09=(a09*a10); - a04=(a04+a09); - a09=(a16*a13); - a18=casadi_fabs(a13); - a18=(a18*a13); - a09=(a09+a18); - a04=(a04+a09); - a06=(a06*a04); - a00=(a00-a06); - a06=1.0920100546139184e-05; - a09=arg[1]? arg[1][1] : 0; - a12=(a12*a11); - a15=(a15*a03); - a12=(a12+a15); - a15=3.3399999999999999e+00; - a15=(a15*a10); - a15=(a15*a13); - a12=(a12+a15); - a15=-6.8000000000000005e-01; - a15=(a15*a13); - a15=(a15*a10); - a12=(a12+a15); - a09=(a09-a12); - a12=(a16*a05); - a15=casadi_fabs(a05); - a15=(a15*a05); - a12=(a12+a15); - a09=(a09-a12); - a06=(a06*a09); - a00=(a00+a06); - a06=-6.0150572994295574e-03; - a12=arg[1]? arg[1][2] : 0; - a02=(a02*a11); - a07=(a07*a08); - a02=(a02+a07); - a07=-3.3199999999999998e+00; - a07=(a07*a05); - a07=(a07*a13); - a02=(a02+a07); - a07=6.8000000000000005e-01; - a07=(a07*a13); - a07=(a07*a05); - a02=(a02+a07); - a12=(a12-a02); - a16=(a16*a10); - a02=casadi_fabs(a10); - a02=(a02*a10); - a16=(a16+a02); - a12=(a12-a16); - a06=(a06*a12); - a00=(a00+a06); - if (res[0]!=0) res[0][0]=a00; - a00=6.0368975005218423e-05; - a00=(a00*a01); - a06=3.3484658136227786e-02; - a06=(a06*a14); - a00=(a00-a06); - a06=-6.1658244361114702e-05; - a06=(a06*a17); - a00=(a00-a06); - a06=5.4404333259807184e-05; - a06=(a06*a04); - a00=(a00-a06); - a06=6.0570157695918683e-03; - a06=(a06*a09); - a00=(a00+a06); - a06=-3.0184487502609172e-03; - a06=(a06*a12); - a00=(a00+a06); - if (res[0]!=0) res[0][1]=a00; - a00=-1.1116270017027936e-07; - a00=(a00*a01); - a06=-6.1658244361114783e-05; - a06=(a06*a14); - a00=(a00-a06); - a06=3.3963490377380855e-02; - a06=(a06*a17); - a00=(a00-a06); - a06=-2.9967785627100754e-02; - a06=(a06*a04); - a00=(a00-a06); - a06=-3.0801331505514837e-03; - a06=(a06*a09); - a00=(a00+a06); - a06=5.5581350085139543e-06; - a06=(a06*a12); - a00=(a00+a06); - if (res[0]!=0) res[0][2]=a00; - a00=9.8084735444364535e-08; - a00=(a00*a01); - a06=5.4404333259807062e-05; - a06=(a06*a14); - a00=(a00-a06); - a06=-2.9967785627100469e-02; - a06=(a06*a17); - a00=(a00-a06); - a06=1.4970303990827358e+00; - a06=(a06*a04); - a00=(a00-a06); - a06=2.7177645446042520e-03; - a06=(a06*a09); - a00=(a00+a06); - a06=-4.9042367722181902e-06; - a06=(a06*a12); - a00=(a00+a06); - if (res[0]!=0) res[0][3]=a00; - a00=1.0920100546139210e-05; - a00=(a00*a01); - a06=6.0570157695918657e-03; - a06=(a06*a14); - a00=(a00-a06); - a06=-3.0801331505514781e-03; - a06=(a06*a17); - a00=(a00-a06); - a06=2.7177645446042498e-03; - a06=(a06*a04); - a00=(a00-a06); - a06=3.0257778596593993e-01; - a06=(a06*a09); - a00=(a00+a06); - a06=-5.4600502730695923e-04; - a16=(a06*a12); - a00=(a00+a16); - if (res[0]!=0) res[0][4]=a00; - a00=-6.0150572994295635e-03; - a00=(a00*a01); - a01=-3.0184487502609133e-03; - a01=(a01*a14); - a00=(a00-a01); - a01=5.5581350085139340e-06; - a01=(a01*a17); - a00=(a00-a01); - a01=-4.9042367722181935e-06; - a01=(a01*a04); - a00=(a00-a01); - a06=(a06*a09); - a00=(a00+a06); - a06=3.0075286497147785e-01; - a06=(a06*a12); - a00=(a00+a06); - if (res[0]!=0) res[0][5]=a00; - a00=arg[0]? arg[0][6] : 0; - a06=sin(a00); - a12=arg[0]? arg[0][7] : 0; - a09=tan(a12); - a01=(a06*a09); - a01=(a01*a05); - a13=(a13+a01); - a00=cos(a00); - a09=(a00*a09); - a09=(a09*a10); - a13=(a13+a09); - if (res[0]!=0) res[0][6]=a13; - a13=(a00*a05); - a09=(a06*a10); - a13=(a13-a09); - if (res[0]!=0) res[0][7]=a13; - a12=cos(a12); - a06=(a06/a12); - a06=(a06*a05); - a00=(a00/a12); - a00=(a00*a10); - a06=(a06+a00); - if (res[0]!=0) res[0][8]=a06; - return 0; -} - -CASADI_SYMBOL_EXPORT int auv_model_expl_ode_fun(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ - return casadi_f0(arg, res, iw, w, mem); -} - -CASADI_SYMBOL_EXPORT int auv_model_expl_ode_fun_alloc_mem(void) { - return 0; -} - -CASADI_SYMBOL_EXPORT int auv_model_expl_ode_fun_init_mem(int mem) { - return 0; -} - -CASADI_SYMBOL_EXPORT void auv_model_expl_ode_fun_free_mem(int mem) { -} - -CASADI_SYMBOL_EXPORT int auv_model_expl_ode_fun_checkout(void) { - return 0; -} - -CASADI_SYMBOL_EXPORT void auv_model_expl_ode_fun_release(int mem) { -} - -CASADI_SYMBOL_EXPORT void auv_model_expl_ode_fun_incref(void) { -} - -CASADI_SYMBOL_EXPORT void auv_model_expl_ode_fun_decref(void) { -} - -CASADI_SYMBOL_EXPORT casadi_int auv_model_expl_ode_fun_n_in(void) { return 3;} - -CASADI_SYMBOL_EXPORT casadi_int auv_model_expl_ode_fun_n_out(void) { return 1;} - -CASADI_SYMBOL_EXPORT casadi_real auv_model_expl_ode_fun_default_in(casadi_int i) { - switch (i) { - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const char* auv_model_expl_ode_fun_name_in(casadi_int i) { - switch (i) { - case 0: return "i0"; - case 1: return "i1"; - case 2: return "i2"; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const char* auv_model_expl_ode_fun_name_out(casadi_int i) { - switch (i) { - case 0: return "o0"; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const casadi_int* auv_model_expl_ode_fun_sparsity_in(casadi_int i) { - switch (i) { - case 0: return casadi_s0; - case 1: return casadi_s1; - case 2: return casadi_s2; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const casadi_int* auv_model_expl_ode_fun_sparsity_out(casadi_int i) { - switch (i) { - case 0: return casadi_s0; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT int auv_model_expl_ode_fun_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { - if (sz_arg) *sz_arg = 3; - if (sz_res) *sz_res = 1; - if (sz_iw) *sz_iw = 0; - if (sz_w) *sz_w = 0; - return 0; -} - -CASADI_SYMBOL_EXPORT int auv_model_expl_ode_fun_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { - if (sz_arg) *sz_arg = 3*sizeof(const casadi_real*); - if (sz_res) *sz_res = 1*sizeof(casadi_real*); - if (sz_iw) *sz_iw = 0*sizeof(casadi_int); - if (sz_w) *sz_w = 0*sizeof(casadi_real); - return 0; -} - - -#ifdef __cplusplus -} /* extern "C" */ -#endif diff --git a/control/velocity_controller/src/build_auv_solver/auv_model_model/auv_model_expl_ode_fun.o b/control/velocity_controller/src/build_auv_solver/auv_model_model/auv_model_expl_ode_fun.o deleted file mode 100644 index 6cd530b508e0bcfd0ca6b946a18b25895a29592c..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 8232 zcmb`M32>BW8G!!~jx?qFZz$Rn6t;FL3zqHfh9g)@c1Z%O6FX^}(A_bREXl6PkgM50 z5)f$^5(Wm5RH?&E6=6EoI@W^II)hlrmITUCAmyf14jU*4!h| z16GL2(m+&u2y%WKL|#Xao>5|lIQUhp1`v`8y9>xDlVlXw!7?Myo(6HX9n6B`Oi6H; zu&xBsDXhwbMtzSSc)v$4>AI>f6pfGQFCKxfQ?yE$WJ8>1_d<1fcH3F>hK>Co&z^~S z)a%5)JO~PbOkn{r=(_UVy+R#TwhH7BW#P{+KqmRi#rEde3(-+JND8s*3l9q&Yg{Ly zU+NZm0n{4+(lQ(;v$qxYCQ_`?B6+$a=5PI6Z*le)0|i3?4r@OI?FzdX*5f^Sy4A2} zVp1T6aCby^9Khn2uwR&2fP)z*u-Bu%?nu7wxQN)C12L`_GzOAty90)Jf~ZCh-i10o0&3V@*v8$cy#tu(&P(nB zAET@NSlS0}j)B4rQJ)_XOfLF>>dVX`G?Nxd07r3wy%Jg;`~-zJun!Mm<=|%@HhwQy z?=`yu@>_^bVRRh1ur5TyyI>eN1^tNY21H|Tnj-@SyRni3pd0uQE2)6N08jyguMgdU z_<|To$d#N&1+$4*6Y{@9RWOTK^EQk=#pnb?fi0jMIE|xo32iT!&I2g_4Bg&_(Z3`b ziYtDB-3#@74vqf;8vBgdQ|K00Ucu-pXrBRzn1F~Lyo~T9m~=<<64?ZH^nK+UO8HJi zs=~i~IQY6PBPid}Ga6x><8_H1W1aBPev_*x=>QJZ2E`h@zq*nwz@Z62n!Ee#-UklN zF?aj@Z3t`y4(&3}2vZj5gmzi=jC`BF^_bo=0**QuzFe1o#e4RpFkt=_@7kLo6*~$f zs|1CNiD45K;X!150S^ZQ=hv}CKrdUEauDHM8E_s!0{aORcfyCm4_n4sgy%|dJAB(< z62xQ%FJf*lM%!Qt3t)@VAqNK<b22j(vBguf#XgH_KPr5or@Hq4k$w zSwR(<(1rux@*sTG=r*tuzTM}4tB-5}V=NXVN(1;*6Cum7H;K#)cU& zxX>{e_p);@-R$j+M4VigKGG}h8+Qy=57m$Nky`GMZ2l9us;`zerb5H8$~;ZfzY#A? zsSKkPgS)U9sF3?th!wxMH^mqc&-XKiwDwYI6KeYP{4E`A*uK_Y6PRX#-`BsmkN=my z)?Q=a2ZOLz{?_Y|9f%$Bx9)>%Khd<-9gr2z0vy|WGY}E&^_%v1Es{OXb5i>%msaK0 zsyteqSOuDRZwTjLg^j>Gmmg5dy=HhBupFAXaH1pq-GjDuIon6x%*9o;wT(A6(;f=_gIFM7JF{hVvprncNj&gv5_En4B>DUYn)JL9jf9N7G4s(&oi zJBr$IGsmpIfb5&euABS}C;P07J(KEY?$0S~d-8a8x38Vk=+-?~mgelwF~@0&m(rM- zPNMR3@5T|W=bzh{9$UHPnxkZ8y0-(9EvMVH>AAVzusMonH`W@BHg}fO?R1UHZWKlL zLXXGk8b5}xa0$;3cpYjQuKelG7HhDMV#siQF$^^mEoJ`mT1t~OW%%GhX>Bm+X!8}4 ziSr)v1@O^Q(Lw{EFwJ6J*>9>P^T(M3hl9u}*m}4Ey@L?EaCW5V^yv*JtCU!97(e}LrdR+#!wYf~x(s zE4_(?sqtNXPdW^;DcV5M7o>wpG@dvW0q|VAKP(W%G)14B+3Kg=OUXuL|k3b z4-;oSP9G)CX9M$*z=ueKz-Q{y#I2+#&Q-`i51bkMz%-bI7boE}$$v9-fX~#0#M>0U zhWJ|IeBN#*zCqDHPCTaQ-yzjo&Y=sjCQAmxi0_t4kqQ z8fmO`mM0OEM%_szo}?0&<}AqdU^EdUQOv^Jc#I}sGzp_@j3#5`7CLTW<8})o4BW!P zElk|PnOhjSg}p~eJ*KAc;}M!3vjIZiBfNP`ec|6DJY<=zGNr;cD`|%!q1n+Sozh;@B^|Q@EX1B`%M{cvOUdiQF$d zH!)yhfsg(CUgEmMC(#cG6yrU``maemL*jps_z;Qjk@&+BKQ3{6`?CLw630JqFuyDD zFH1ZFCjxPe^>O#IXtdJ7lrKxjnBaoPXoKorK%KCkAXkejM<35eDY` zR%-L+KV{O!uj2`LE-TW!5N)bV}xdF&ZyB~rj?w7YdPdJ z!cB(etOyw)%{e<7)tn7=SU`2R6CNjKkyB@R)WByDDM1O|GUatOHQ`$DQd?()o$yzc zaZw{wK8MQRt8H}FH`LXK8;p5ih;@c5OCt?%yV6|zFNqC|vFo(m;;y`u%fdrl3cM~E z;TeT{+RK1a7apb%Dp3dl?*Oca-SO{a+ - -#ifndef casadi_real -#define casadi_real double -#endif - -#ifndef casadi_int -#define casadi_int int -#endif - -/* Add prefix to internal symbols */ -#define casadi_f0 CASADI_PREFIX(f0) -#define casadi_fabs CASADI_PREFIX(fabs) -#define casadi_s0 CASADI_PREFIX(s0) -#define casadi_s1 CASADI_PREFIX(s1) -#define casadi_s2 CASADI_PREFIX(s2) -#define casadi_s3 CASADI_PREFIX(s3) -#define casadi_sign CASADI_PREFIX(sign) -#define casadi_sq CASADI_PREFIX(sq) - -/* Symbol visibility in DLLs */ -#ifndef CASADI_SYMBOL_EXPORT - #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) - #if defined(STATIC_LINKED) - #define CASADI_SYMBOL_EXPORT - #else - #define CASADI_SYMBOL_EXPORT __declspec(dllexport) - #endif - #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) - #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default"))) - #else - #define CASADI_SYMBOL_EXPORT - #endif -#endif - -casadi_real casadi_fabs(casadi_real x) { -/* Pre-c99 compatibility */ -#if __STDC_VERSION__ < 199901L - return x>0 ? x : -x; -#else - return fabs(x); -#endif -} - -casadi_real casadi_sign(casadi_real x) { return x<0 ? -1 : x>0 ? 1 : x;} - -casadi_real casadi_sq(casadi_real x) { return x*x;} - -static const casadi_int casadi_s0[3] = {9, 1, 1}; -static const casadi_int casadi_s1[3] = {3, 1, 1}; -static const casadi_int casadi_s2[3] = {0, 1, 1}; -static const casadi_int casadi_s3[3] = {12, 1, 1}; - -/* auv_model_expl_vde_adj:(i0[9],i1[9],i2[3],i3[0])->(o0[12]) */ -static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { - casadi_real a00, a01, a02, a03, a04, a05, a06, a07, a08, a09, a10, a11; - casadi_real a12, a13, a14, a15, a16, a17, a18, a19, a20, a21, a22, a23; - casadi_real a24, a25, a26, a27, a28; - a00=-30.; - a01=arg[0]? arg[0][1] : 0; - a02=3.0075286497147785e-01; - a03=arg[1]? arg[1][5] : 0; - a02=(a02*a03); - a04=-5.4600502730695923e-04; - a05=arg[1]? arg[1][4] : 0; - a06=(a04*a05); - a02=(a02+a06); - a06=-4.9042367722181902e-06; - a07=arg[1]? arg[1][3] : 0; - a06=(a06*a07); - a02=(a02+a06); - a06=5.5581350085139543e-06; - a08=arg[1]? arg[1][2] : 0; - a06=(a06*a08); - a02=(a02+a06); - a06=-3.0184487502609172e-03; - a09=arg[1]? arg[1][1] : 0; - a06=(a06*a09); - a02=(a02+a06); - a06=-6.0150572994295574e-03; - a10=arg[1]? arg[1][0] : 0; - a06=(a06*a10); - a02=(a02+a06); - a06=(a01*a02); - a06=(a00*a06); - a11=30.; - a12=(a11*a01); - a13=(a12*a02); - a06=(a06+a13); - a13=arg[0]? arg[0][2] : 0; - a04=(a04*a03); - a14=3.0257778596593993e-01; - a14=(a14*a05); - a04=(a04+a14); - a14=2.7177645446042520e-03; - a14=(a14*a07); - a04=(a04+a14); - a14=-3.0801331505514837e-03; - a14=(a14*a08); - a04=(a04+a14); - a14=6.0570157695918683e-03; - a14=(a14*a09); - a04=(a04+a14); - a14=1.0920100546139184e-05; - a14=(a14*a10); - a04=(a04+a14); - a14=(a13*a04); - a14=(a11*a14); - a06=(a06+a14); - a14=(a00*a13); - a15=(a14*a04); - a06=(a06+a15); - a15=arg[0]? arg[0][4] : 0; - a16=5.5581350085139340e-06; - a16=(a16*a03); - a17=-3.0801331505514781e-03; - a17=(a17*a05); - a16=(a16+a17); - a17=-2.9967785627100469e-02; - a17=(a17*a07); - a16=(a16+a17); - a17=3.3963490377380855e-02; - a17=(a17*a08); - a16=(a16+a17); - a17=-6.1658244361114702e-05; - a17=(a17*a09); - a16=(a16+a17); - a17=-1.1116270017027866e-07; - a17=(a17*a10); - a16=(a16+a17); - a17=(a15*a16); - a17=(a00*a17); - a06=(a06+a17); - a17=arg[0]? arg[0][5] : 0; - a18=-3.0184487502609133e-03; - a18=(a18*a03); - a19=6.0570157695918657e-03; - a19=(a19*a05); - a18=(a18+a19); - a19=5.4404333259807062e-05; - a19=(a19*a07); - a18=(a18+a19); - a19=-6.1658244361114783e-05; - a19=(a19*a08); - a18=(a18+a19); - a19=3.3484658136227786e-02; - a19=(a19*a09); - a18=(a18+a19); - a19=6.0368975005218254e-05; - a19=(a19*a10); - a18=(a18+a19); - a19=(a17*a18); - a19=(a11*a19); - a06=(a06+a19); - a19=arg[0]? arg[0][0] : 0; - a20=casadi_fabs(a19); - a21=-6.0150572994295635e-03; - a21=(a21*a03); - a22=1.0920100546139210e-05; - a22=(a22*a05); - a21=(a21+a22); - a22=9.8084735444364535e-08; - a22=(a22*a07); - a21=(a21+a22); - a22=-1.1116270017027936e-07; - a22=(a22*a08); - a21=(a21+a22); - a22=6.0368975005218423e-05; - a22=(a22*a09); - a21=(a21+a22); - a22=3.3453634479321918e-02; - a22=(a22*a10); - a21=(a21+a22); - a20=(a20*a21); - a06=(a06+a20); - a20=casadi_sign(a19); - a22=(a19*a21); - a20=(a20*a22); - a06=(a06+a20); - a20=23.; - a20=(a20*a21); - a06=(a06+a20); - a06=(-a06); - if (res[0]!=0) res[0][0]=a06; - a06=(a00*a19); - a20=(a06*a02); - a22=(a19*a02); - a22=(a11*a22); - a20=(a20+a22); - a22=-4.9042367722181935e-06; - a22=(a22*a03); - a03=2.7177645446042498e-03; - a03=(a03*a05); - a22=(a22+a03); - a03=1.4970303990827358e+00; - a03=(a03*a07); - a22=(a22+a03); - a03=-2.9967785627100754e-02; - a03=(a03*a08); - a22=(a22+a03); - a03=5.4404333259807184e-05; - a03=(a03*a09); - a22=(a22+a03); - a03=9.8084735444363873e-08; - a03=(a03*a10); - a22=(a22+a03); - a03=(a13*a22); - a03=(a00*a03); - a20=(a20+a03); - a03=(a11*a13); - a10=(a03*a22); - a20=(a20+a10); - a10=arg[0]? arg[0][3] : 0; - a09=(a10*a16); - a09=(a11*a09); - a20=(a20+a09); - a09=casadi_fabs(a01); - a09=(a09*a18); - a20=(a20+a09); - a09=casadi_sign(a01); - a08=(a01*a18); - a09=(a09*a08); - a20=(a20+a09); - a09=46.; - a08=(a09*a18); - a20=(a20+a08); - a08=(a17*a21); - a08=(a00*a08); - a20=(a20+a08); - a20=(-a20); - if (res[0]!=0) res[0][1]=a20; - a20=(a11*a19); - a08=(a20*a04); - a19=(a19*a04); - a19=(a00*a19); - a08=(a08+a19); - a19=(a00*a01); - a07=(a19*a22); - a08=(a08+a07); - a01=(a01*a22); - a01=(a11*a01); - a08=(a08+a01); - a01=casadi_fabs(a13); - a01=(a01*a16); - a08=(a08+a01); - a01=casadi_sign(a13); - a13=(a13*a16); - a01=(a01*a13); - a08=(a08+a01); - a01=(a09*a16); - a08=(a08+a01); - a01=(a10*a18); - a00=(a00*a01); - a08=(a08+a00); - a00=(a15*a21); - a11=(a11*a00); - a08=(a08+a11); - a08=(-a08); - if (res[0]!=0) res[0][2]=a08; - a08=arg[1]? arg[1][6] : 0; - a11=6.8000000000000005e-01; - a00=(a15*a02); - a00=(a11*a00); - a00=(a08-a00); - a01=-3.3199999999999998e+00; - a13=(a01*a15); - a13=(a13*a02); - a00=(a00-a13); - a13=-6.8000000000000005e-01; - a07=(a17*a04); - a07=(a13*a07); - a00=(a00-a07); - a07=3.3399999999999999e+00; - a05=(a07*a17); - a05=(a05*a04); - a00=(a00-a05); - a05=casadi_fabs(a10); - a05=(a05*a22); - a00=(a00-a05); - a05=casadi_sign(a10); - a23=(a10*a22); - a05=(a05*a23); - a00=(a00-a05); - a05=(a09*a22); - a00=(a00-a05); - a12=(a12*a16); - a00=(a00-a12); - a14=(a14*a18); - a00=(a00-a14); - if (res[0]!=0) res[0][3]=a00; - a00=arg[0]? arg[0][6] : 0; - a14=sin(a00); - a12=arg[0]? arg[0][7] : 0; - a05=cos(a12); - a23=(a14/a05); - a24=arg[1]? arg[1][8] : 0; - a25=(a23*a24); - a00=cos(a00); - a26=arg[1]? arg[1][7] : 0; - a27=(a00*a26); - a25=(a25+a27); - a27=tan(a12); - a28=(a14*a27); - a28=(a28*a08); - a25=(a25+a28); - a11=(a11*a10); - a11=(a11*a02); - a25=(a25-a11); - a11=(a10*a02); - a01=(a01*a11); - a25=(a25-a01); - a01=casadi_fabs(a15); - a01=(a01*a04); - a25=(a25-a01); - a01=casadi_sign(a15); - a11=(a15*a04); - a01=(a01*a11); - a25=(a25-a01); - a01=(a09*a04); - a25=(a25-a01); - a01=3.3199999999999998e+00; - a11=(a17*a22); - a11=(a01*a11); - a25=(a25-a11); - a11=-3.3399999999999999e+00; - a28=(a11*a17); - a28=(a28*a22); - a25=(a25-a28); - a06=(a06*a16); - a25=(a25-a06); - a03=(a03*a21); - a25=(a25-a03); - if (res[0]!=0) res[0][4]=a25; - a25=(a00/a05); - a03=(a25*a24); - a06=(a14*a26); - a03=(a03-a06); - a06=(a00*a27); - a06=(a06*a08); - a03=(a03+a06); - a06=casadi_fabs(a17); - a06=(a06*a02); - a03=(a03-a06); - a06=casadi_sign(a17); - a16=(a17*a02); - a06=(a06*a16); - a03=(a03-a06); - a09=(a09*a02); - a03=(a03-a09); - a13=(a13*a10); - a13=(a13*a04); - a03=(a03-a13); - a10=(a10*a04); - a07=(a07*a10); - a03=(a03-a07); - a01=(a01*a15); - a01=(a01*a22); - a03=(a03-a01); - a22=(a15*a22); - a11=(a11*a22); - a03=(a03-a11); - a20=(a20*a18); - a03=(a03-a20); - a19=(a19*a21); - a03=(a03-a19); - if (res[0]!=0) res[0][5]=a03; - a03=(a15*a24); - a19=(a03/a05); - a19=(a00*a19); - a24=(a17*a24); - a20=(a24/a05); - a20=(a14*a20); - a19=(a19-a20); - a20=(a17*a26); - a20=(a00*a20); - a19=(a19-a20); - a26=(a15*a26); - a26=(a14*a26); - a19=(a19-a26); - a17=(a17*a08); - a26=(a27*a17); - a26=(a14*a26); - a19=(a19-a26); - a15=(a15*a08); - a27=(a27*a15); - a27=(a00*a27); - a19=(a19+a27); - if (res[0]!=0) res[0][6]=a19; - a12=sin(a12); - a25=(a25/a05); - a25=(a25*a24); - a25=(a12*a25); - a23=(a23/a05); - a23=(a23*a03); - a12=(a12*a23); - a25=(a25+a12); - a00=(a00*a17); - a05=casadi_sq(a05); - a00=(a00/a05); - a25=(a25+a00); - a14=(a14*a15); - a14=(a14/a05); - a25=(a25+a14); - if (res[0]!=0) res[0][7]=a25; - a25=0.; - if (res[0]!=0) res[0][8]=a25; - if (res[0]!=0) res[0][9]=a21; - if (res[0]!=0) res[0][10]=a04; - if (res[0]!=0) res[0][11]=a02; - return 0; -} - -CASADI_SYMBOL_EXPORT int auv_model_expl_vde_adj(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ - return casadi_f0(arg, res, iw, w, mem); -} - -CASADI_SYMBOL_EXPORT int auv_model_expl_vde_adj_alloc_mem(void) { - return 0; -} - -CASADI_SYMBOL_EXPORT int auv_model_expl_vde_adj_init_mem(int mem) { - return 0; -} - -CASADI_SYMBOL_EXPORT void auv_model_expl_vde_adj_free_mem(int mem) { -} - -CASADI_SYMBOL_EXPORT int auv_model_expl_vde_adj_checkout(void) { - return 0; -} - -CASADI_SYMBOL_EXPORT void auv_model_expl_vde_adj_release(int mem) { -} - -CASADI_SYMBOL_EXPORT void auv_model_expl_vde_adj_incref(void) { -} - -CASADI_SYMBOL_EXPORT void auv_model_expl_vde_adj_decref(void) { -} - -CASADI_SYMBOL_EXPORT casadi_int auv_model_expl_vde_adj_n_in(void) { return 4;} - -CASADI_SYMBOL_EXPORT casadi_int auv_model_expl_vde_adj_n_out(void) { return 1;} - -CASADI_SYMBOL_EXPORT casadi_real auv_model_expl_vde_adj_default_in(casadi_int i) { - switch (i) { - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const char* auv_model_expl_vde_adj_name_in(casadi_int i) { - switch (i) { - case 0: return "i0"; - case 1: return "i1"; - case 2: return "i2"; - case 3: return "i3"; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const char* auv_model_expl_vde_adj_name_out(casadi_int i) { - switch (i) { - case 0: return "o0"; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const casadi_int* auv_model_expl_vde_adj_sparsity_in(casadi_int i) { - switch (i) { - case 0: return casadi_s0; - case 1: return casadi_s0; - case 2: return casadi_s1; - case 3: return casadi_s2; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const casadi_int* auv_model_expl_vde_adj_sparsity_out(casadi_int i) { - switch (i) { - case 0: return casadi_s3; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT int auv_model_expl_vde_adj_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { - if (sz_arg) *sz_arg = 4; - if (sz_res) *sz_res = 1; - if (sz_iw) *sz_iw = 0; - if (sz_w) *sz_w = 0; - return 0; -} - -CASADI_SYMBOL_EXPORT int auv_model_expl_vde_adj_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { - if (sz_arg) *sz_arg = 4*sizeof(const casadi_real*); - if (sz_res) *sz_res = 1*sizeof(casadi_real*); - if (sz_iw) *sz_iw = 0*sizeof(casadi_int); - if (sz_w) *sz_w = 0*sizeof(casadi_real); - return 0; -} - - -#ifdef __cplusplus -} /* extern "C" */ -#endif diff --git a/control/velocity_controller/src/build_auv_solver/auv_model_model/auv_model_expl_vde_adj.o b/control/velocity_controller/src/build_auv_solver/auv_model_model/auv_model_expl_vde_adj.o deleted file mode 100644 index 669b0a2701794ffd0473b5ba7e704e90de694d2e..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 12672 zcmbuF4|LVVmB)V}C@IqX-fOjHi^XUCp$~+Z#N5SPTl0X1$pE$y;4o?4G~J(X@bE2~%@1SO(kR#0(eg|ll_ z#yqyCopavrH}jc$@66npxii1Oipr|1CuLhW>!VL~5+)IaUn=#^U-<37u`d9>_+Y;`gT?zNbZ4eJTx&nnBvO)Gn z+-TY?m)}@kvnaCqFDUC`6w{<|KYkkGQq}1F0MGM#|mV4?uo%$^hhcq5ik1 z|3{yy2O1nF@El6DJ0P(nM@vZa5PkmBXt%WE@Aksb;%=kWj|rb3%c`4Denue|k~DHH zi9XiuG|HHkapjM!^2b*B6RQj__$xfatWRQSJr@v2rSS zA@zC>3nL$5j`7$|gCen$R3ayxK!3vUEc%Hh8LgoCkS-~YcEKrdz7Mz?LxCE30&4y( zUK$NRzmz5NkgM?U^vApoJU4lpx!V7rH=r^#UOjk~NAhz#+9{;XW#1eRSqLQl-d#=$ zNhx6|e}Lcr0*skk>#b+SsB(zwZz8i%SsDIf7{Vs%S52<-R&l2GBbdPwS4zka!X)=| z{IWsKel11ARFd$S8^n^)jj-3$Si8Ma9!O+OHY`%(m}>Ngj3+AGU>aG~3Ct|=x@SH$ zjMUd?YHP?PSfJ`?{X6=8&o!vB{6d@=I)V>Fj^6D|3 z)KoOn3@J<}51@d3q2~~*UHb<9+s8|L3-<+UIskXh!bO$hBg@ zxzW97^A(n;M`<^HI)Fw2Bw>^FTvDy#F}dA*5+X zA(klxUWw44wJuH6QfM54d#l*JksR&^dJ0p`z$wO?c$HaWZ~oNSx2mx>ah$2*u`5lO zYvo0Ze!?jQXdg~ij?q0K*F+!n=sZA|;01Ur473*`rcQ1n7eJpJR6C4nbPy$S28~rF zc5JNT9P+OoDv2s5m4d2xu!Ak@kP`MqcEW||0*zFLmcPSfE@7o{^AmC5~5b5Q^0;4*JLd>fs(}!g~-Cl=!>H4ZM zE_8x+W<4zjY<>iCv9LiRf0R}c#)IkbuhZdvEr)wGu@QGU)+R}CSVwUDw30$_S~#Nb@k9y@+eDchw3VtjDMhM=m>T}E&mA%7$Q zUO@}cUqfLI4ONf0i(_(HhAn6L3}-=p%u`>=#?;`rcu-9sauoFOdQ|xiIa~_pc&9kR zJpw48W8_7mZ-TZ`FQA7h!)*XHhmk?Zpz*=G=qMry97YEKxd<2HpyVxA1;-pR;LwtZ z!XdF85dWPxhIT*28|7AgsSqE@9z}KZ0Dd^dD8R8pu>jc~scw+-8v!Lk$0;pCx`vP= zeKaQ8w{UUnIW$ZYw^kw~!Z>+x5^Wp<~d z5efx;ue-fJz=(MVa~;IEA2nyFDTxQi!q^455js9 ziwqGe!XtGT;}#Wd;wX3!p2lgp7~`x(NW%odSN@wIh%A{f#)UNY-*f2JD7m6w@9m)D= zbQIr{Xk#X7oyx-Zt5$E~U3uN=_o`OobB-R(P*5LrO`afc-(RParg zKe(bN;0O9J(KlKwUnPra$7huu=4^6qB44|=pT2(!)A+;?e@|2%G!*`ldRyKuZ=Q_7 zP?MM!vbQ;Fe3_hReve1)=J>I4bW-Yy6&FdVZ#0#^ay07+@{^#5Te)59tsKo3sw7lZ znA__c{gl?Y@f+=3vf>NlWfxKl==a}bAALV_=M?fnr0;!X&m;$6^B_Bk$J6&LvIMb7 zP46Xmz9LFLfNIgFwUbf5rNC(^bXtm>mXOn407**t(*ytAg&ScqzKnBeT>WRPvZy$t zDy`6EdH0MjAEkRT($O;YUJy<`z5Cr&WqZo1`{%q-dZ>Ksx$9rN@i+JF{nh#dy}eHL zs|$MMU3JlpLw8=$vUTRr?r8@fyMODG-+O)Fk;Vtg!dET-m*H8ytykXR``y%6|LIrR z;o3zrpJ?8CVdKRQE%-^-){uYqy!M;-lpQH}?uq_|iL$8J3I4cZ*H>QbD*O4b_ZK{N z`0#?6QvR?^>96_X?1#_1Yg*aWV*ljR^B+6xJy7=8j2ABb{zI^q}J zYHIFS@Ga@jkjNhxoVw}dzyqQ;FFakwc}V*8(rYixeC62jvfE^Q|0eBE7JJHH*M#rg z{^wxq-l^%$Gy2~C`sS(OK`L&4vrOJwr9Le6Y3G`S3oj1@7cE)VmRc4l4CNQ*7tAeP z#>K+rMMe1q#j^x+6SNp99??#_=K4hr)?u0$tpiGDtCy2?#|=(amoIDjv?)3LSUjY2 zp44%KL!OvkXY-|w7`6>3T1%9!#IKffj&mp%nX7Ys_e{DfH*alrd2ZmY$rZW5m1kU= z+dnCL@3g78!G*bj^4z?ua(!3j=2YU(Dcla-^(jqJFJ_NQ-ehTE97&L;{_GWI%Dpv# z+{Y6zpKKh{o+oUo$Wt4o6R;{_+7BdGFRaSYO$j^KV5@|Y?kJ4FXdv&O3g>g( zsQa1lQiD-;qwp}lPd)xqc<>bB+>^q!pLO*%;Smkwy;FF##a|Gv_oc4hBfMS%dA}yS z+2RL;cUb&Q;awJgTX>Jf-xa>f;>U&eTl}Q(^%l?OwV&fWZ1K+sAF#M9e51wB5x&{t z(}kxkeu3~Ii(f2!*y3}A@3VMF_(6-$7yg#T%Y+}bc%|?Y7XPYnduxAPcn-f$DSbn@ zj)S`9X5o33e53Gy#ao02E#5A?z~ak<>$s}h{f_XkC4Z;zh{eAvyxQV-3$L~K_k`D5 z{D;DuE&gNS9Txv7m(TcAB>Q*adOZ653*q`4*8Fkd`V7(hDdA?^q`E=y&l!`e&*}rh zePTeL*B=S57OvYl2NwgfQ!kv)ZRF>GYa3FXgx6cV zTX=_XeNJx>-ebvc5#Ddfzb<^h;vWd#Xz_C~QPj`0aD5jD2_Lq2weW+&wVykM9~E9D zPQ6cfSe&iz2EPzqFI?Xb_6i@c~D;L^wI#IG9E+-X?$?YgDyP4U*ccvnYj z-M5?Kbq!75%x}z4QI{;tSW}R(rYK`g$jPr-SWJ)0=utqAB6<|kql6x%^q9wtVIkMg zXTb7>Y^0E_6tYSo8!J>wtY66TMan*#DPl84tXsrdMQovnb&FWHh;@rtH^jOj)(x?4 zh;>7(6=Dk^)(x?4h;>7(Tg7*LkD4Rj5uA#NHy|Hd-d}$_umbRAE z>FN`m@%ZWL8=K>ex3(`!WwPHHZ;dx3ZK(eEuGue{5{WK=VL{4Mow)W0jGr5N9>XzJ|iYF05awKjcTsjmB zKbAijTw-wgmeX8+Ba%FwVVbLZyF=fl4xL?^uS1>^af+Rqzd;a*IDJ}b{(FK*#3{6D zK0*+QIE6&b^#>4f3W=JZ#pkS&|D3_kHu&ca9yU0AOKUp~2B&|;(tNqWzhLnD3{J6M z%Rgf9JcB=J@V_(o9)r`jo3`^?gHJd3F@v9H@N9An620hO@3o!J8+?YrFEsf11}`yq zz~Gezzrf&&4E|+<#|?g=!Iv4_Gx$RWpK0*j8Tfpho0O>9l)qm%j!&lyKkbJDhWtea z|AWCVHu%|O0Ez4e@zZu@8+?|*FE{vXgWq6q+H2a*EyC#)Hu!EsevZM98+@+8M-6WJ z!Ig^^^*`T`pKkC=4Su1)3k-gV!3zx@GI)`}lftQ6rvFzOJZ#9nX4si&@PPd3f%;E( zAMLj*4Q|S}3%A={VeqhF=a~$4rpblYZueq?o9!kIPJd(3{k+%U^iSNH|3tXm&;KyE z+0Um8JC_-D&Xs^p{hVj;YYlGt?U%x--Gzqys|K$y_;G_*8oXOB#&-YL8a!;se$4~QL z8vJU5$K->K+9eli`MV50?%Xsuxk}47(P4>1cBpHbKWT8&z9-)&ByXI2G27%eY@y(^9@e_`$_Zb4ZgtOw-}t3Fr8JD?yxw;I7;_eT-@vY+~T_30Bb4# z&=_>0HAaaT{Y2e~5Jb|vS{0p`yfv?v@&=1niyt>yyk7VYi+2due?QXpdxXDZ$@dG# zCo3l{KOo$^&rsb);rj12T0SlOO3Ti$aQ$}{Eq_pWlO=ytc$dY);?T7guNOXG@d4qR zES?rVWO2USkq7dVw=YdKEWta~sou>xhYyB^{8YRv<>WUtq#B(3CCQ|d-`P$D5--fh zXO}AKcYb3sMIV_Wgc82k8rzpHjkm!{TYD;=kAH2Mn@lw{-YVr=+Lq;abhdZIJ5#qq zk=l$m*Cje}@8=o%+a3X^)6nG@8Z#D_Tu8qAb%nS~7ah72QcMbu5tTW5fqD;1{HAb0 z?MTs~J1M1$@FOnQA3~gxR=b*$teI%vlFh$XpY9T5lkN^?((TLg8>`ao>vtG6l&5uU zrd85^id|;X7<+YyX-N;Z}w*9wW^aEN@-n!o!uS14x+WMPCf1lN$ zt$#0cCtAPh*{X$uqEG908h%<|uiuH - -#ifndef casadi_real -#define casadi_real double -#endif - -#ifndef casadi_int -#define casadi_int int -#endif - -/* Add prefix to internal symbols */ -#define casadi_f0 CASADI_PREFIX(f0) -#define casadi_fabs CASADI_PREFIX(fabs) -#define casadi_s0 CASADI_PREFIX(s0) -#define casadi_s1 CASADI_PREFIX(s1) -#define casadi_s2 CASADI_PREFIX(s2) -#define casadi_s3 CASADI_PREFIX(s3) -#define casadi_s4 CASADI_PREFIX(s4) -#define casadi_sign CASADI_PREFIX(sign) -#define casadi_sq CASADI_PREFIX(sq) - -/* Symbol visibility in DLLs */ -#ifndef CASADI_SYMBOL_EXPORT - #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) - #if defined(STATIC_LINKED) - #define CASADI_SYMBOL_EXPORT - #else - #define CASADI_SYMBOL_EXPORT __declspec(dllexport) - #endif - #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) - #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default"))) - #else - #define CASADI_SYMBOL_EXPORT - #endif -#endif - -casadi_real casadi_fabs(casadi_real x) { -/* Pre-c99 compatibility */ -#if __STDC_VERSION__ < 199901L - return x>0 ? x : -x; -#else - return fabs(x); -#endif -} - -casadi_real casadi_sign(casadi_real x) { return x<0 ? -1 : x>0 ? 1 : x;} - -casadi_real casadi_sq(casadi_real x) { return x*x;} - -static const casadi_int casadi_s0[3] = {9, 1, 1}; -static const casadi_int casadi_s1[3] = {9, 9, 1}; -static const casadi_int casadi_s2[3] = {9, 3, 1}; -static const casadi_int casadi_s3[3] = {3, 1, 1}; -static const casadi_int casadi_s4[3] = {0, 1, 1}; - -/* auv_model_expl_vde_forw:(i0[9],i1[9x9],i2[9x3],i3[3],i4[0])->(o0[9],o1[9x9],o2[9x3]) */ -static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { - casadi_real a000, a001, a002, a003, a004, a005, a006, a007, a008, a009, a010, a011; - casadi_real a012, a013, a014, a015, a016, a017, a018, a019, a020, a021, a022, a023; - casadi_real a024, a025, a026, a027, a028, a029, a030, a031, a032, a033, a034, a035; - casadi_real a036, a037, a038, a039, a040, a041, a042, a043, a044, a045, a046, a047; - casadi_real a048, a049, a050, a051, a052, a053, a054, a055, a056, a057, a058, a059; - casadi_real a060, a061, a062, a063, a064, a065, a066, a067, a068, a069, a070, a071; - casadi_real a072, a073, a074, a075, a076, a077, a078, a079, a080, a081, a082, a083; - casadi_real a084, a085, a086, a087, a088, a089, a090, a091, a092, a093, a094, a095; - casadi_real a096, a097, a098, a099, a100, a101, a102, a103, a104, a105; - a000=3.3453634479321918e-02; - a001=arg[3]? arg[3][0] : 0; - a002=30.; - a003=arg[0]? arg[0][2] : 0; - a004=(a002*a003); - a005=arg[0]? arg[0][4] : 0; - a006=(a004*a005); - a007=-30.; - a008=arg[0]? arg[0][1] : 0; - a009=(a007*a008); - a010=arg[0]? arg[0][5] : 0; - a011=(a009*a010); - a006=(a006+a011); - a001=(a001-a006); - a006=23.; - a011=arg[0]? arg[0][0] : 0; - a012=(a006*a011); - a013=casadi_fabs(a011); - a014=(a013*a011); - a012=(a012+a014); - a001=(a001-a012); - a012=(a000*a001); - a014=6.0368975005218254e-05; - a015=(a007*a003); - a016=arg[0]? arg[0][3] : 0; - a017=(a015*a016); - a018=(a002*a011); - a019=(a018*a010); - a017=(a017+a019); - a019=46.; - a020=(a019*a008); - a021=casadi_fabs(a008); - a022=(a021*a008); - a020=(a020+a022); - a017=(a017+a020); - a020=(a014*a017); - a012=(a012-a020); - a020=-1.1116270017027866e-07; - a022=(a002*a008); - a023=(a022*a016); - a024=(a007*a011); - a025=(a024*a005); - a023=(a023+a025); - a025=(a019*a003); - a026=casadi_fabs(a003); - a027=(a026*a003); - a025=(a025+a027); - a023=(a023+a025); - a025=(a020*a023); - a012=(a012-a025); - a025=9.8084735444363873e-08; - a027=(a004*a008); - a028=(a009*a003); - a027=(a027+a028); - a028=-3.3399999999999999e+00; - a029=(a028*a010); - a030=(a029*a005); - a027=(a027+a030); - a030=3.3199999999999998e+00; - a031=(a030*a005); - a032=(a031*a010); - a027=(a027+a032); - a032=(a019*a016); - a033=casadi_fabs(a016); - a034=(a033*a016); - a032=(a032+a034); - a027=(a027+a032); - a032=(a025*a027); - a012=(a012-a032); - a032=1.0920100546139184e-05; - a034=arg[3]? arg[3][1] : 0; - a035=(a015*a011); - a036=(a018*a003); - a035=(a035+a036); - a036=3.3399999999999999e+00; - a037=(a036*a010); - a038=(a037*a016); - a035=(a035+a038); - a038=-6.8000000000000005e-01; - a039=(a038*a016); - a040=(a039*a010); - a035=(a035+a040); - a034=(a034-a035); - a035=(a019*a005); - a040=casadi_fabs(a005); - a041=(a040*a005); - a035=(a035+a041); - a034=(a034-a035); - a035=(a032*a034); - a012=(a012+a035); - a035=-6.0150572994295574e-03; - a041=arg[3]? arg[3][2] : 0; - a042=(a022*a011); - a043=(a024*a008); - a042=(a042+a043); - a043=-3.3199999999999998e+00; - a044=(a043*a005); - a045=(a044*a016); - a042=(a042+a045); - a045=6.8000000000000005e-01; - a046=(a045*a016); - a047=(a046*a005); - a042=(a042+a047); - a041=(a041-a042); - a042=(a019*a010); - a047=casadi_fabs(a010); - a048=(a047*a010); - a042=(a042+a048); - a041=(a041-a042); - a042=(a035*a041); - a012=(a012+a042); - if (res[0]!=0) res[0][0]=a012; - a012=6.0368975005218423e-05; - a042=(a012*a001); - a048=3.3484658136227786e-02; - a049=(a048*a017); - a042=(a042-a049); - a049=-6.1658244361114702e-05; - a050=(a049*a023); - a042=(a042-a050); - a050=5.4404333259807184e-05; - a051=(a050*a027); - a042=(a042-a051); - a051=6.0570157695918683e-03; - a052=(a051*a034); - a042=(a042+a052); - a052=-3.0184487502609172e-03; - a053=(a052*a041); - a042=(a042+a053); - if (res[0]!=0) res[0][1]=a042; - a042=-1.1116270017027936e-07; - a053=(a042*a001); - a054=-6.1658244361114783e-05; - a055=(a054*a017); - a053=(a053-a055); - a055=3.3963490377380855e-02; - a056=(a055*a023); - a053=(a053-a056); - a056=-2.9967785627100754e-02; - a057=(a056*a027); - a053=(a053-a057); - a057=-3.0801331505514837e-03; - a058=(a057*a034); - a053=(a053+a058); - a058=5.5581350085139543e-06; - a059=(a058*a041); - a053=(a053+a059); - if (res[0]!=0) res[0][2]=a053; - a053=9.8084735444364535e-08; - a059=(a053*a001); - a060=5.4404333259807062e-05; - a061=(a060*a017); - a059=(a059-a061); - a061=-2.9967785627100469e-02; - a062=(a061*a023); - a059=(a059-a062); - a062=1.4970303990827358e+00; - a063=(a062*a027); - a059=(a059-a063); - a063=2.7177645446042520e-03; - a064=(a063*a034); - a059=(a059+a064); - a064=-4.9042367722181902e-06; - a065=(a064*a041); - a059=(a059+a065); - if (res[0]!=0) res[0][3]=a059; - a059=1.0920100546139210e-05; - a065=(a059*a001); - a066=6.0570157695918657e-03; - a067=(a066*a017); - a065=(a065-a067); - a067=-3.0801331505514781e-03; - a068=(a067*a023); - a065=(a065-a068); - a068=2.7177645446042498e-03; - a069=(a068*a027); - a065=(a065-a069); - a069=3.0257778596593993e-01; - a070=(a069*a034); - a065=(a065+a070); - a070=-5.4600502730695923e-04; - a071=(a070*a041); - a065=(a065+a071); - if (res[0]!=0) res[0][4]=a065; - a065=-6.0150572994295635e-03; - a001=(a065*a001); - a071=-3.0184487502609133e-03; - a017=(a071*a017); - a001=(a001-a017); - a017=5.5581350085139340e-06; - a023=(a017*a023); - a001=(a001-a023); - a023=-4.9042367722181935e-06; - a027=(a023*a027); - a001=(a001-a027); - a034=(a070*a034); - a001=(a001+a034); - a034=3.0075286497147785e-01; - a041=(a034*a041); - a001=(a001+a041); - if (res[0]!=0) res[0][5]=a001; - a001=arg[0]? arg[0][6] : 0; - a041=sin(a001); - a027=arg[0]? arg[0][7] : 0; - a072=tan(a027); - a073=(a041*a072); - a074=(a073*a005); - a074=(a016+a074); - a001=cos(a001); - a075=(a001*a072); - a076=(a075*a010); - a074=(a074+a076); - if (res[0]!=0) res[0][6]=a074; - a074=(a001*a005); - a076=(a041*a010); - a074=(a074-a076); - if (res[0]!=0) res[0][7]=a074; - a074=cos(a027); - a076=(a041/a074); - a077=(a076*a005); - a078=(a001/a074); - a079=(a078*a010); - a077=(a077+a079); - if (res[0]!=0) res[0][8]=a077; - a077=arg[1]? arg[1][2] : 0; - a079=(a002*a077); - a080=(a005*a079); - a081=arg[1]? arg[1][4] : 0; - a082=(a004*a081); - a080=(a080+a082); - a082=arg[1]? arg[1][1] : 0; - a083=(a007*a082); - a084=(a010*a083); - a085=arg[1]? arg[1][5] : 0; - a086=(a009*a085); - a084=(a084+a086); - a080=(a080+a084); - a084=arg[1]? arg[1][0] : 0; - a086=(a006*a084); - a087=casadi_sign(a011); - a088=(a087*a084); - a088=(a011*a088); - a089=(a013*a084); - a088=(a088+a089); - a086=(a086+a088); - a080=(a080+a086); - a086=(a000*a080); - a088=(a007*a077); - a089=(a016*a088); - a090=arg[1]? arg[1][3] : 0; - a091=(a015*a090); - a089=(a089+a091); - a091=(a002*a084); - a092=(a010*a091); - a093=(a018*a085); - a092=(a092+a093); - a089=(a089+a092); - a092=(a019*a082); - a093=casadi_sign(a008); - a094=(a093*a082); - a094=(a008*a094); - a095=(a021*a082); - a094=(a094+a095); - a092=(a092+a094); - a089=(a089+a092); - a092=(a014*a089); - a086=(a086+a092); - a092=(a002*a082); - a094=(a016*a092); - a095=(a022*a090); - a094=(a094+a095); - a095=(a007*a084); - a096=(a005*a095); - a097=(a024*a081); - a096=(a096+a097); - a094=(a094+a096); - a096=(a019*a077); - a097=casadi_sign(a003); - a098=(a097*a077); - a098=(a003*a098); - a099=(a026*a077); - a098=(a098+a099); - a096=(a096+a098); - a094=(a094+a096); - a096=(a020*a094); - a086=(a086+a096); - a079=(a008*a079); - a096=(a004*a082); - a079=(a079+a096); - a083=(a003*a083); - a096=(a009*a077); - a083=(a083+a096); - a079=(a079+a083); - a083=(a028*a085); - a083=(a005*a083); - a096=(a029*a081); - a083=(a083+a096); - a079=(a079+a083); - a083=(a030*a081); - a083=(a010*a083); - a096=(a031*a085); - a083=(a083+a096); - a079=(a079+a083); - a083=(a019*a090); - a096=casadi_sign(a016); - a098=(a096*a090); - a098=(a016*a098); - a099=(a033*a090); - a098=(a098+a099); - a083=(a083+a098); - a079=(a079+a083); - a083=(a025*a079); - a086=(a086+a083); - a088=(a011*a088); - a083=(a015*a084); - a088=(a088+a083); - a091=(a003*a091); - a077=(a018*a077); - a091=(a091+a077); - a088=(a088+a091); - a091=(a036*a085); - a091=(a016*a091); - a077=(a037*a090); - a091=(a091+a077); - a088=(a088+a091); - a091=(a038*a090); - a091=(a010*a091); - a077=(a039*a085); - a091=(a091+a077); - a088=(a088+a091); - a091=(a019*a081); - a077=casadi_sign(a005); - a083=(a077*a081); - a083=(a005*a083); - a098=(a040*a081); - a083=(a083+a098); - a091=(a091+a083); - a088=(a088+a091); - a091=(a032*a088); - a086=(a086+a091); - a092=(a011*a092); - a084=(a022*a084); - a092=(a092+a084); - a095=(a008*a095); - a082=(a024*a082); - a095=(a095+a082); - a092=(a092+a095); - a095=(a043*a081); - a095=(a016*a095); - a082=(a044*a090); - a095=(a095+a082); - a092=(a092+a095); - a095=(a045*a090); - a095=(a005*a095); - a082=(a046*a081); - a095=(a095+a082); - a092=(a092+a095); - a095=(a019*a085); - a082=casadi_sign(a010); - a084=(a082*a085); - a084=(a010*a084); - a091=(a047*a085); - a084=(a084+a091); - a095=(a095+a084); - a092=(a092+a095); - a095=(a035*a092); - a086=(a086+a095); - a086=(-a086); - if (res[1]!=0) res[1][0]=a086; - a086=(a012*a080); - a095=(a048*a089); - a086=(a086+a095); - a095=(a049*a094); - a086=(a086+a095); - a095=(a050*a079); - a086=(a086+a095); - a095=(a051*a088); - a086=(a086+a095); - a095=(a052*a092); - a086=(a086+a095); - a086=(-a086); - if (res[1]!=0) res[1][1]=a086; - a086=(a042*a080); - a095=(a054*a089); - a086=(a086+a095); - a095=(a055*a094); - a086=(a086+a095); - a095=(a056*a079); - a086=(a086+a095); - a095=(a057*a088); - a086=(a086+a095); - a095=(a058*a092); - a086=(a086+a095); - a086=(-a086); - if (res[1]!=0) res[1][2]=a086; - a086=(a053*a080); - a095=(a060*a089); - a086=(a086+a095); - a095=(a061*a094); - a086=(a086+a095); - a095=(a062*a079); - a086=(a086+a095); - a095=(a063*a088); - a086=(a086+a095); - a095=(a064*a092); - a086=(a086+a095); - a086=(-a086); - if (res[1]!=0) res[1][3]=a086; - a086=(a059*a080); - a095=(a066*a089); - a086=(a086+a095); - a095=(a067*a094); - a086=(a086+a095); - a095=(a068*a079); - a086=(a086+a095); - a095=(a069*a088); - a086=(a086+a095); - a095=(a070*a092); - a086=(a086+a095); - a086=(-a086); - if (res[1]!=0) res[1][4]=a086; - a080=(a065*a080); - a089=(a071*a089); - a080=(a080+a089); - a094=(a017*a094); - a080=(a080+a094); - a079=(a023*a079); - a080=(a080+a079); - a088=(a070*a088); - a080=(a080+a088); - a092=(a034*a092); - a080=(a080+a092); - a080=(-a080); - if (res[1]!=0) res[1][5]=a080; - a080=arg[1]? arg[1][6] : 0; - a092=(a001*a080); - a088=(a072*a092); - a079=arg[1]? arg[1][7] : 0; - a094=casadi_sq(a074); - a089=(a079/a094); - a086=(a041*a089); - a088=(a088+a086); - a088=(a005*a088); - a086=(a073*a081); - a088=(a088+a086); - a090=(a090+a088); - a089=(a001*a089); - a080=(a041*a080); - a088=(a072*a080); - a089=(a089-a088); - a089=(a010*a089); - a088=(a075*a085); - a089=(a089+a088); - a090=(a090+a089); - if (res[1]!=0) res[1][6]=a090; - a090=(a001*a081); - a089=(a005*a080); - a090=(a090-a089); - a089=(a010*a092); - a088=(a041*a085); - a089=(a089+a088); - a090=(a090-a089); - if (res[1]!=0) res[1][7]=a090; - a092=(a092/a074); - a090=(a076/a074); - a027=sin(a027); - a079=(a027*a079); - a089=(a090*a079); - a092=(a092+a089); - a092=(a005*a092); - a081=(a076*a081); - a092=(a092+a081); - a081=(a078/a074); - a079=(a081*a079); - a080=(a080/a074); - a079=(a079-a080); - a079=(a010*a079); - a085=(a078*a085); - a079=(a079+a085); - a092=(a092+a079); - if (res[1]!=0) res[1][8]=a092; - a092=arg[1]? arg[1][11] : 0; - a079=(a002*a092); - a085=(a005*a079); - a080=arg[1]? arg[1][13] : 0; - a089=(a004*a080); - a085=(a085+a089); - a089=arg[1]? arg[1][10] : 0; - a088=(a007*a089); - a086=(a010*a088); - a095=arg[1]? arg[1][14] : 0; - a084=(a009*a095); - a086=(a086+a084); - a085=(a085+a086); - a086=arg[1]? arg[1][9] : 0; - a084=(a006*a086); - a091=(a087*a086); - a091=(a011*a091); - a083=(a013*a086); - a091=(a091+a083); - a084=(a084+a091); - a085=(a085+a084); - a084=(a000*a085); - a091=(a007*a092); - a083=(a016*a091); - a098=arg[1]? arg[1][12] : 0; - a099=(a015*a098); - a083=(a083+a099); - a099=(a002*a086); - a100=(a010*a099); - a101=(a018*a095); - a100=(a100+a101); - a083=(a083+a100); - a100=(a019*a089); - a101=(a093*a089); - a101=(a008*a101); - a102=(a021*a089); - a101=(a101+a102); - a100=(a100+a101); - a083=(a083+a100); - a100=(a014*a083); - a084=(a084+a100); - a100=(a002*a089); - a101=(a016*a100); - a102=(a022*a098); - a101=(a101+a102); - a102=(a007*a086); - a103=(a005*a102); - a104=(a024*a080); - a103=(a103+a104); - a101=(a101+a103); - a103=(a019*a092); - a104=(a097*a092); - a104=(a003*a104); - a105=(a026*a092); - a104=(a104+a105); - a103=(a103+a104); - a101=(a101+a103); - a103=(a020*a101); - a084=(a084+a103); - a079=(a008*a079); - a103=(a004*a089); - a079=(a079+a103); - a088=(a003*a088); - a103=(a009*a092); - a088=(a088+a103); - a079=(a079+a088); - a088=(a028*a095); - a088=(a005*a088); - a103=(a029*a080); - a088=(a088+a103); - a079=(a079+a088); - a088=(a030*a080); - a088=(a010*a088); - a103=(a031*a095); - a088=(a088+a103); - a079=(a079+a088); - a088=(a019*a098); - a103=(a096*a098); - a103=(a016*a103); - a104=(a033*a098); - a103=(a103+a104); - a088=(a088+a103); - a079=(a079+a088); - a088=(a025*a079); - a084=(a084+a088); - a091=(a011*a091); - a088=(a015*a086); - a091=(a091+a088); - a099=(a003*a099); - a092=(a018*a092); - a099=(a099+a092); - a091=(a091+a099); - a099=(a036*a095); - a099=(a016*a099); - a092=(a037*a098); - a099=(a099+a092); - a091=(a091+a099); - a099=(a038*a098); - a099=(a010*a099); - a092=(a039*a095); - a099=(a099+a092); - a091=(a091+a099); - a099=(a019*a080); - a092=(a077*a080); - a092=(a005*a092); - a088=(a040*a080); - a092=(a092+a088); - a099=(a099+a092); - a091=(a091+a099); - a099=(a032*a091); - a084=(a084+a099); - a100=(a011*a100); - a086=(a022*a086); - a100=(a100+a086); - a102=(a008*a102); - a089=(a024*a089); - a102=(a102+a089); - a100=(a100+a102); - a102=(a043*a080); - a102=(a016*a102); - a089=(a044*a098); - a102=(a102+a089); - a100=(a100+a102); - a102=(a045*a098); - a102=(a005*a102); - a089=(a046*a080); - a102=(a102+a089); - a100=(a100+a102); - a102=(a019*a095); - a089=(a082*a095); - a089=(a010*a089); - a086=(a047*a095); - a089=(a089+a086); - a102=(a102+a089); - a100=(a100+a102); - a102=(a035*a100); - a084=(a084+a102); - a084=(-a084); - if (res[1]!=0) res[1][9]=a084; - a084=(a012*a085); - a102=(a048*a083); - a084=(a084+a102); - a102=(a049*a101); - a084=(a084+a102); - a102=(a050*a079); - a084=(a084+a102); - a102=(a051*a091); - a084=(a084+a102); - a102=(a052*a100); - a084=(a084+a102); - a084=(-a084); - if (res[1]!=0) res[1][10]=a084; - a084=(a042*a085); - a102=(a054*a083); - a084=(a084+a102); - a102=(a055*a101); - a084=(a084+a102); - a102=(a056*a079); - a084=(a084+a102); - a102=(a057*a091); - a084=(a084+a102); - a102=(a058*a100); - a084=(a084+a102); - a084=(-a084); - if (res[1]!=0) res[1][11]=a084; - a084=(a053*a085); - a102=(a060*a083); - a084=(a084+a102); - a102=(a061*a101); - a084=(a084+a102); - a102=(a062*a079); - a084=(a084+a102); - a102=(a063*a091); - a084=(a084+a102); - a102=(a064*a100); - a084=(a084+a102); - a084=(-a084); - if (res[1]!=0) res[1][12]=a084; - a084=(a059*a085); - a102=(a066*a083); - a084=(a084+a102); - a102=(a067*a101); - a084=(a084+a102); - a102=(a068*a079); - a084=(a084+a102); - a102=(a069*a091); - a084=(a084+a102); - a102=(a070*a100); - a084=(a084+a102); - a084=(-a084); - if (res[1]!=0) res[1][13]=a084; - a085=(a065*a085); - a083=(a071*a083); - a085=(a085+a083); - a101=(a017*a101); - a085=(a085+a101); - a079=(a023*a079); - a085=(a085+a079); - a091=(a070*a091); - a085=(a085+a091); - a100=(a034*a100); - a085=(a085+a100); - a085=(-a085); - if (res[1]!=0) res[1][14]=a085; - a085=arg[1]? arg[1][15] : 0; - a100=(a001*a085); - a091=(a072*a100); - a079=arg[1]? arg[1][16] : 0; - a101=(a079/a094); - a083=(a041*a101); - a091=(a091+a083); - a091=(a005*a091); - a083=(a073*a080); - a091=(a091+a083); - a098=(a098+a091); - a101=(a001*a101); - a085=(a041*a085); - a091=(a072*a085); - a101=(a101-a091); - a101=(a010*a101); - a091=(a075*a095); - a101=(a101+a091); - a098=(a098+a101); - if (res[1]!=0) res[1][15]=a098; - a098=(a001*a080); - a101=(a005*a085); - a098=(a098-a101); - a101=(a010*a100); - a091=(a041*a095); - a101=(a101+a091); - a098=(a098-a101); - if (res[1]!=0) res[1][16]=a098; - a100=(a100/a074); - a079=(a027*a079); - a098=(a090*a079); - a100=(a100+a098); - a100=(a005*a100); - a080=(a076*a080); - a100=(a100+a080); - a079=(a081*a079); - a085=(a085/a074); - a079=(a079-a085); - a079=(a010*a079); - a095=(a078*a095); - a079=(a079+a095); - a100=(a100+a079); - if (res[1]!=0) res[1][17]=a100; - a100=arg[1]? arg[1][20] : 0; - a079=(a002*a100); - a095=(a005*a079); - a085=arg[1]? arg[1][22] : 0; - a080=(a004*a085); - a095=(a095+a080); - a080=arg[1]? arg[1][19] : 0; - a098=(a007*a080); - a101=(a010*a098); - a091=arg[1]? arg[1][23] : 0; - a083=(a009*a091); - a101=(a101+a083); - a095=(a095+a101); - a101=arg[1]? arg[1][18] : 0; - a083=(a006*a101); - a084=(a087*a101); - a084=(a011*a084); - a102=(a013*a101); - a084=(a084+a102); - a083=(a083+a084); - a095=(a095+a083); - a083=(a000*a095); - a084=(a007*a100); - a102=(a016*a084); - a089=arg[1]? arg[1][21] : 0; - a086=(a015*a089); - a102=(a102+a086); - a086=(a002*a101); - a099=(a010*a086); - a092=(a018*a091); - a099=(a099+a092); - a102=(a102+a099); - a099=(a019*a080); - a092=(a093*a080); - a092=(a008*a092); - a088=(a021*a080); - a092=(a092+a088); - a099=(a099+a092); - a102=(a102+a099); - a099=(a014*a102); - a083=(a083+a099); - a099=(a002*a080); - a092=(a016*a099); - a088=(a022*a089); - a092=(a092+a088); - a088=(a007*a101); - a103=(a005*a088); - a104=(a024*a085); - a103=(a103+a104); - a092=(a092+a103); - a103=(a019*a100); - a104=(a097*a100); - a104=(a003*a104); - a105=(a026*a100); - a104=(a104+a105); - a103=(a103+a104); - a092=(a092+a103); - a103=(a020*a092); - a083=(a083+a103); - a079=(a008*a079); - a103=(a004*a080); - a079=(a079+a103); - a098=(a003*a098); - a103=(a009*a100); - a098=(a098+a103); - a079=(a079+a098); - a098=(a028*a091); - a098=(a005*a098); - a103=(a029*a085); - a098=(a098+a103); - a079=(a079+a098); - a098=(a030*a085); - a098=(a010*a098); - a103=(a031*a091); - a098=(a098+a103); - a079=(a079+a098); - a098=(a019*a089); - a103=(a096*a089); - a103=(a016*a103); - a104=(a033*a089); - a103=(a103+a104); - a098=(a098+a103); - a079=(a079+a098); - a098=(a025*a079); - a083=(a083+a098); - a084=(a011*a084); - a098=(a015*a101); - a084=(a084+a098); - a086=(a003*a086); - a100=(a018*a100); - a086=(a086+a100); - a084=(a084+a086); - a086=(a036*a091); - a086=(a016*a086); - a100=(a037*a089); - a086=(a086+a100); - a084=(a084+a086); - a086=(a038*a089); - a086=(a010*a086); - a100=(a039*a091); - a086=(a086+a100); - a084=(a084+a086); - a086=(a019*a085); - a100=(a077*a085); - a100=(a005*a100); - a098=(a040*a085); - a100=(a100+a098); - a086=(a086+a100); - a084=(a084+a086); - a086=(a032*a084); - a083=(a083+a086); - a099=(a011*a099); - a101=(a022*a101); - a099=(a099+a101); - a088=(a008*a088); - a080=(a024*a080); - a088=(a088+a080); - a099=(a099+a088); - a088=(a043*a085); - a088=(a016*a088); - a080=(a044*a089); - a088=(a088+a080); - a099=(a099+a088); - a088=(a045*a089); - a088=(a005*a088); - a080=(a046*a085); - a088=(a088+a080); - a099=(a099+a088); - a088=(a019*a091); - a080=(a082*a091); - a080=(a010*a080); - a101=(a047*a091); - a080=(a080+a101); - a088=(a088+a080); - a099=(a099+a088); - a088=(a035*a099); - a083=(a083+a088); - a083=(-a083); - if (res[1]!=0) res[1][18]=a083; - a083=(a012*a095); - a088=(a048*a102); - a083=(a083+a088); - a088=(a049*a092); - a083=(a083+a088); - a088=(a050*a079); - a083=(a083+a088); - a088=(a051*a084); - a083=(a083+a088); - a088=(a052*a099); - a083=(a083+a088); - a083=(-a083); - if (res[1]!=0) res[1][19]=a083; - a083=(a042*a095); - a088=(a054*a102); - a083=(a083+a088); - a088=(a055*a092); - a083=(a083+a088); - a088=(a056*a079); - a083=(a083+a088); - a088=(a057*a084); - a083=(a083+a088); - a088=(a058*a099); - a083=(a083+a088); - a083=(-a083); - if (res[1]!=0) res[1][20]=a083; - a083=(a053*a095); - a088=(a060*a102); - a083=(a083+a088); - a088=(a061*a092); - a083=(a083+a088); - a088=(a062*a079); - a083=(a083+a088); - a088=(a063*a084); - a083=(a083+a088); - a088=(a064*a099); - a083=(a083+a088); - a083=(-a083); - if (res[1]!=0) res[1][21]=a083; - a083=(a059*a095); - a088=(a066*a102); - a083=(a083+a088); - a088=(a067*a092); - a083=(a083+a088); - a088=(a068*a079); - a083=(a083+a088); - a088=(a069*a084); - a083=(a083+a088); - a088=(a070*a099); - a083=(a083+a088); - a083=(-a083); - if (res[1]!=0) res[1][22]=a083; - a095=(a065*a095); - a102=(a071*a102); - a095=(a095+a102); - a092=(a017*a092); - a095=(a095+a092); - a079=(a023*a079); - a095=(a095+a079); - a084=(a070*a084); - a095=(a095+a084); - a099=(a034*a099); - a095=(a095+a099); - a095=(-a095); - if (res[1]!=0) res[1][23]=a095; - a095=arg[1]? arg[1][24] : 0; - a099=(a001*a095); - a084=(a072*a099); - a079=arg[1]? arg[1][25] : 0; - a092=(a079/a094); - a102=(a041*a092); - a084=(a084+a102); - a084=(a005*a084); - a102=(a073*a085); - a084=(a084+a102); - a089=(a089+a084); - a092=(a001*a092); - a095=(a041*a095); - a084=(a072*a095); - a092=(a092-a084); - a092=(a010*a092); - a084=(a075*a091); - a092=(a092+a084); - a089=(a089+a092); - if (res[1]!=0) res[1][24]=a089; - a089=(a001*a085); - a092=(a005*a095); - a089=(a089-a092); - a092=(a010*a099); - a084=(a041*a091); - a092=(a092+a084); - a089=(a089-a092); - if (res[1]!=0) res[1][25]=a089; - a099=(a099/a074); - a079=(a027*a079); - a089=(a090*a079); - a099=(a099+a089); - a099=(a005*a099); - a085=(a076*a085); - a099=(a099+a085); - a079=(a081*a079); - a095=(a095/a074); - a079=(a079-a095); - a079=(a010*a079); - a091=(a078*a091); - a079=(a079+a091); - a099=(a099+a079); - if (res[1]!=0) res[1][26]=a099; - a099=arg[1]? arg[1][29] : 0; - a079=(a002*a099); - a091=(a005*a079); - a095=arg[1]? arg[1][31] : 0; - a085=(a004*a095); - a091=(a091+a085); - a085=arg[1]? arg[1][28] : 0; - a089=(a007*a085); - a092=(a010*a089); - a084=arg[1]? arg[1][32] : 0; - a102=(a009*a084); - a092=(a092+a102); - a091=(a091+a092); - a092=arg[1]? arg[1][27] : 0; - a102=(a006*a092); - a083=(a087*a092); - a083=(a011*a083); - a088=(a013*a092); - a083=(a083+a088); - a102=(a102+a083); - a091=(a091+a102); - a102=(a000*a091); - a083=(a007*a099); - a088=(a016*a083); - a080=arg[1]? arg[1][30] : 0; - a101=(a015*a080); - a088=(a088+a101); - a101=(a002*a092); - a086=(a010*a101); - a100=(a018*a084); - a086=(a086+a100); - a088=(a088+a086); - a086=(a019*a085); - a100=(a093*a085); - a100=(a008*a100); - a098=(a021*a085); - a100=(a100+a098); - a086=(a086+a100); - a088=(a088+a086); - a086=(a014*a088); - a102=(a102+a086); - a086=(a002*a085); - a100=(a016*a086); - a098=(a022*a080); - a100=(a100+a098); - a098=(a007*a092); - a103=(a005*a098); - a104=(a024*a095); - a103=(a103+a104); - a100=(a100+a103); - a103=(a019*a099); - a104=(a097*a099); - a104=(a003*a104); - a105=(a026*a099); - a104=(a104+a105); - a103=(a103+a104); - a100=(a100+a103); - a103=(a020*a100); - a102=(a102+a103); - a079=(a008*a079); - a103=(a004*a085); - a079=(a079+a103); - a089=(a003*a089); - a103=(a009*a099); - a089=(a089+a103); - a079=(a079+a089); - a089=(a028*a084); - a089=(a005*a089); - a103=(a029*a095); - a089=(a089+a103); - a079=(a079+a089); - a089=(a030*a095); - a089=(a010*a089); - a103=(a031*a084); - a089=(a089+a103); - a079=(a079+a089); - a089=(a019*a080); - a103=(a096*a080); - a103=(a016*a103); - a104=(a033*a080); - a103=(a103+a104); - a089=(a089+a103); - a079=(a079+a089); - a089=(a025*a079); - a102=(a102+a089); - a083=(a011*a083); - a089=(a015*a092); - a083=(a083+a089); - a101=(a003*a101); - a099=(a018*a099); - a101=(a101+a099); - a083=(a083+a101); - a101=(a036*a084); - a101=(a016*a101); - a099=(a037*a080); - a101=(a101+a099); - a083=(a083+a101); - a101=(a038*a080); - a101=(a010*a101); - a099=(a039*a084); - a101=(a101+a099); - a083=(a083+a101); - a101=(a019*a095); - a099=(a077*a095); - a099=(a005*a099); - a089=(a040*a095); - a099=(a099+a089); - a101=(a101+a099); - a083=(a083+a101); - a101=(a032*a083); - a102=(a102+a101); - a086=(a011*a086); - a092=(a022*a092); - a086=(a086+a092); - a098=(a008*a098); - a085=(a024*a085); - a098=(a098+a085); - a086=(a086+a098); - a098=(a043*a095); - a098=(a016*a098); - a085=(a044*a080); - a098=(a098+a085); - a086=(a086+a098); - a098=(a045*a080); - a098=(a005*a098); - a085=(a046*a095); - a098=(a098+a085); - a086=(a086+a098); - a098=(a019*a084); - a085=(a082*a084); - a085=(a010*a085); - a092=(a047*a084); - a085=(a085+a092); - a098=(a098+a085); - a086=(a086+a098); - a098=(a035*a086); - a102=(a102+a098); - a102=(-a102); - if (res[1]!=0) res[1][27]=a102; - a102=(a012*a091); - a098=(a048*a088); - a102=(a102+a098); - a098=(a049*a100); - a102=(a102+a098); - a098=(a050*a079); - a102=(a102+a098); - a098=(a051*a083); - a102=(a102+a098); - a098=(a052*a086); - a102=(a102+a098); - a102=(-a102); - if (res[1]!=0) res[1][28]=a102; - a102=(a042*a091); - a098=(a054*a088); - a102=(a102+a098); - a098=(a055*a100); - a102=(a102+a098); - a098=(a056*a079); - a102=(a102+a098); - a098=(a057*a083); - a102=(a102+a098); - a098=(a058*a086); - a102=(a102+a098); - a102=(-a102); - if (res[1]!=0) res[1][29]=a102; - a102=(a053*a091); - a098=(a060*a088); - a102=(a102+a098); - a098=(a061*a100); - a102=(a102+a098); - a098=(a062*a079); - a102=(a102+a098); - a098=(a063*a083); - a102=(a102+a098); - a098=(a064*a086); - a102=(a102+a098); - a102=(-a102); - if (res[1]!=0) res[1][30]=a102; - a102=(a059*a091); - a098=(a066*a088); - a102=(a102+a098); - a098=(a067*a100); - a102=(a102+a098); - a098=(a068*a079); - a102=(a102+a098); - a098=(a069*a083); - a102=(a102+a098); - a098=(a070*a086); - a102=(a102+a098); - a102=(-a102); - if (res[1]!=0) res[1][31]=a102; - a091=(a065*a091); - a088=(a071*a088); - a091=(a091+a088); - a100=(a017*a100); - a091=(a091+a100); - a079=(a023*a079); - a091=(a091+a079); - a083=(a070*a083); - a091=(a091+a083); - a086=(a034*a086); - a091=(a091+a086); - a091=(-a091); - if (res[1]!=0) res[1][32]=a091; - a091=arg[1]? arg[1][33] : 0; - a086=(a001*a091); - a083=(a072*a086); - a079=arg[1]? arg[1][34] : 0; - a100=(a079/a094); - a088=(a041*a100); - a083=(a083+a088); - a083=(a005*a083); - a088=(a073*a095); - a083=(a083+a088); - a080=(a080+a083); - a100=(a001*a100); - a091=(a041*a091); - a083=(a072*a091); - a100=(a100-a083); - a100=(a010*a100); - a083=(a075*a084); - a100=(a100+a083); - a080=(a080+a100); - if (res[1]!=0) res[1][33]=a080; - a080=(a001*a095); - a100=(a005*a091); - a080=(a080-a100); - a100=(a010*a086); - a083=(a041*a084); - a100=(a100+a083); - a080=(a080-a100); - if (res[1]!=0) res[1][34]=a080; - a086=(a086/a074); - a079=(a027*a079); - a080=(a090*a079); - a086=(a086+a080); - a086=(a005*a086); - a095=(a076*a095); - a086=(a086+a095); - a079=(a081*a079); - a091=(a091/a074); - a079=(a079-a091); - a079=(a010*a079); - a084=(a078*a084); - a079=(a079+a084); - a086=(a086+a079); - if (res[1]!=0) res[1][35]=a086; - a086=arg[1]? arg[1][38] : 0; - a079=(a002*a086); - a084=(a005*a079); - a091=arg[1]? arg[1][40] : 0; - a095=(a004*a091); - a084=(a084+a095); - a095=arg[1]? arg[1][37] : 0; - a080=(a007*a095); - a100=(a010*a080); - a083=arg[1]? arg[1][41] : 0; - a088=(a009*a083); - a100=(a100+a088); - a084=(a084+a100); - a100=arg[1]? arg[1][36] : 0; - a088=(a006*a100); - a102=(a087*a100); - a102=(a011*a102); - a098=(a013*a100); - a102=(a102+a098); - a088=(a088+a102); - a084=(a084+a088); - a088=(a000*a084); - a102=(a007*a086); - a098=(a016*a102); - a085=arg[1]? arg[1][39] : 0; - a092=(a015*a085); - a098=(a098+a092); - a092=(a002*a100); - a101=(a010*a092); - a099=(a018*a083); - a101=(a101+a099); - a098=(a098+a101); - a101=(a019*a095); - a099=(a093*a095); - a099=(a008*a099); - a089=(a021*a095); - a099=(a099+a089); - a101=(a101+a099); - a098=(a098+a101); - a101=(a014*a098); - a088=(a088+a101); - a101=(a002*a095); - a099=(a016*a101); - a089=(a022*a085); - a099=(a099+a089); - a089=(a007*a100); - a103=(a005*a089); - a104=(a024*a091); - a103=(a103+a104); - a099=(a099+a103); - a103=(a019*a086); - a104=(a097*a086); - a104=(a003*a104); - a105=(a026*a086); - a104=(a104+a105); - a103=(a103+a104); - a099=(a099+a103); - a103=(a020*a099); - a088=(a088+a103); - a079=(a008*a079); - a103=(a004*a095); - a079=(a079+a103); - a080=(a003*a080); - a103=(a009*a086); - a080=(a080+a103); - a079=(a079+a080); - a080=(a028*a083); - a080=(a005*a080); - a103=(a029*a091); - a080=(a080+a103); - a079=(a079+a080); - a080=(a030*a091); - a080=(a010*a080); - a103=(a031*a083); - a080=(a080+a103); - a079=(a079+a080); - a080=(a019*a085); - a103=(a096*a085); - a103=(a016*a103); - a104=(a033*a085); - a103=(a103+a104); - a080=(a080+a103); - a079=(a079+a080); - a080=(a025*a079); - a088=(a088+a080); - a102=(a011*a102); - a080=(a015*a100); - a102=(a102+a080); - a092=(a003*a092); - a086=(a018*a086); - a092=(a092+a086); - a102=(a102+a092); - a092=(a036*a083); - a092=(a016*a092); - a086=(a037*a085); - a092=(a092+a086); - a102=(a102+a092); - a092=(a038*a085); - a092=(a010*a092); - a086=(a039*a083); - a092=(a092+a086); - a102=(a102+a092); - a092=(a019*a091); - a086=(a077*a091); - a086=(a005*a086); - a080=(a040*a091); - a086=(a086+a080); - a092=(a092+a086); - a102=(a102+a092); - a092=(a032*a102); - a088=(a088+a092); - a101=(a011*a101); - a100=(a022*a100); - a101=(a101+a100); - a089=(a008*a089); - a095=(a024*a095); - a089=(a089+a095); - a101=(a101+a089); - a089=(a043*a091); - a089=(a016*a089); - a095=(a044*a085); - a089=(a089+a095); - a101=(a101+a089); - a089=(a045*a085); - a089=(a005*a089); - a095=(a046*a091); - a089=(a089+a095); - a101=(a101+a089); - a089=(a019*a083); - a095=(a082*a083); - a095=(a010*a095); - a100=(a047*a083); - a095=(a095+a100); - a089=(a089+a095); - a101=(a101+a089); - a089=(a035*a101); - a088=(a088+a089); - a088=(-a088); - if (res[1]!=0) res[1][36]=a088; - a088=(a012*a084); - a089=(a048*a098); - a088=(a088+a089); - a089=(a049*a099); - a088=(a088+a089); - a089=(a050*a079); - a088=(a088+a089); - a089=(a051*a102); - a088=(a088+a089); - a089=(a052*a101); - a088=(a088+a089); - a088=(-a088); - if (res[1]!=0) res[1][37]=a088; - a088=(a042*a084); - a089=(a054*a098); - a088=(a088+a089); - a089=(a055*a099); - a088=(a088+a089); - a089=(a056*a079); - a088=(a088+a089); - a089=(a057*a102); - a088=(a088+a089); - a089=(a058*a101); - a088=(a088+a089); - a088=(-a088); - if (res[1]!=0) res[1][38]=a088; - a088=(a053*a084); - a089=(a060*a098); - a088=(a088+a089); - a089=(a061*a099); - a088=(a088+a089); - a089=(a062*a079); - a088=(a088+a089); - a089=(a063*a102); - a088=(a088+a089); - a089=(a064*a101); - a088=(a088+a089); - a088=(-a088); - if (res[1]!=0) res[1][39]=a088; - a088=(a059*a084); - a089=(a066*a098); - a088=(a088+a089); - a089=(a067*a099); - a088=(a088+a089); - a089=(a068*a079); - a088=(a088+a089); - a089=(a069*a102); - a088=(a088+a089); - a089=(a070*a101); - a088=(a088+a089); - a088=(-a088); - if (res[1]!=0) res[1][40]=a088; - a084=(a065*a084); - a098=(a071*a098); - a084=(a084+a098); - a099=(a017*a099); - a084=(a084+a099); - a079=(a023*a079); - a084=(a084+a079); - a102=(a070*a102); - a084=(a084+a102); - a101=(a034*a101); - a084=(a084+a101); - a084=(-a084); - if (res[1]!=0) res[1][41]=a084; - a084=arg[1]? arg[1][42] : 0; - a101=(a001*a084); - a102=(a072*a101); - a079=arg[1]? arg[1][43] : 0; - a099=(a079/a094); - a098=(a041*a099); - a102=(a102+a098); - a102=(a005*a102); - a098=(a073*a091); - a102=(a102+a098); - a085=(a085+a102); - a099=(a001*a099); - a084=(a041*a084); - a102=(a072*a084); - a099=(a099-a102); - a099=(a010*a099); - a102=(a075*a083); - a099=(a099+a102); - a085=(a085+a099); - if (res[1]!=0) res[1][42]=a085; - a085=(a001*a091); - a099=(a005*a084); - a085=(a085-a099); - a099=(a010*a101); - a102=(a041*a083); - a099=(a099+a102); - a085=(a085-a099); - if (res[1]!=0) res[1][43]=a085; - a101=(a101/a074); - a079=(a027*a079); - a085=(a090*a079); - a101=(a101+a085); - a101=(a005*a101); - a091=(a076*a091); - a101=(a101+a091); - a079=(a081*a079); - a084=(a084/a074); - a079=(a079-a084); - a079=(a010*a079); - a083=(a078*a083); - a079=(a079+a083); - a101=(a101+a079); - if (res[1]!=0) res[1][44]=a101; - a101=arg[1]? arg[1][47] : 0; - a079=(a002*a101); - a083=(a005*a079); - a084=arg[1]? arg[1][49] : 0; - a091=(a004*a084); - a083=(a083+a091); - a091=arg[1]? arg[1][46] : 0; - a085=(a007*a091); - a099=(a010*a085); - a102=arg[1]? arg[1][50] : 0; - a098=(a009*a102); - a099=(a099+a098); - a083=(a083+a099); - a099=arg[1]? arg[1][45] : 0; - a098=(a006*a099); - a088=(a087*a099); - a088=(a011*a088); - a089=(a013*a099); - a088=(a088+a089); - a098=(a098+a088); - a083=(a083+a098); - a098=(a000*a083); - a088=(a007*a101); - a089=(a016*a088); - a095=arg[1]? arg[1][48] : 0; - a100=(a015*a095); - a089=(a089+a100); - a100=(a002*a099); - a092=(a010*a100); - a086=(a018*a102); - a092=(a092+a086); - a089=(a089+a092); - a092=(a019*a091); - a086=(a093*a091); - a086=(a008*a086); - a080=(a021*a091); - a086=(a086+a080); - a092=(a092+a086); - a089=(a089+a092); - a092=(a014*a089); - a098=(a098+a092); - a092=(a002*a091); - a086=(a016*a092); - a080=(a022*a095); - a086=(a086+a080); - a080=(a007*a099); - a103=(a005*a080); - a104=(a024*a084); - a103=(a103+a104); - a086=(a086+a103); - a103=(a019*a101); - a104=(a097*a101); - a104=(a003*a104); - a105=(a026*a101); - a104=(a104+a105); - a103=(a103+a104); - a086=(a086+a103); - a103=(a020*a086); - a098=(a098+a103); - a079=(a008*a079); - a103=(a004*a091); - a079=(a079+a103); - a085=(a003*a085); - a103=(a009*a101); - a085=(a085+a103); - a079=(a079+a085); - a085=(a028*a102); - a085=(a005*a085); - a103=(a029*a084); - a085=(a085+a103); - a079=(a079+a085); - a085=(a030*a084); - a085=(a010*a085); - a103=(a031*a102); - a085=(a085+a103); - a079=(a079+a085); - a085=(a019*a095); - a103=(a096*a095); - a103=(a016*a103); - a104=(a033*a095); - a103=(a103+a104); - a085=(a085+a103); - a079=(a079+a085); - a085=(a025*a079); - a098=(a098+a085); - a088=(a011*a088); - a085=(a015*a099); - a088=(a088+a085); - a100=(a003*a100); - a101=(a018*a101); - a100=(a100+a101); - a088=(a088+a100); - a100=(a036*a102); - a100=(a016*a100); - a101=(a037*a095); - a100=(a100+a101); - a088=(a088+a100); - a100=(a038*a095); - a100=(a010*a100); - a101=(a039*a102); - a100=(a100+a101); - a088=(a088+a100); - a100=(a019*a084); - a101=(a077*a084); - a101=(a005*a101); - a085=(a040*a084); - a101=(a101+a085); - a100=(a100+a101); - a088=(a088+a100); - a100=(a032*a088); - a098=(a098+a100); - a092=(a011*a092); - a099=(a022*a099); - a092=(a092+a099); - a080=(a008*a080); - a091=(a024*a091); - a080=(a080+a091); - a092=(a092+a080); - a080=(a043*a084); - a080=(a016*a080); - a091=(a044*a095); - a080=(a080+a091); - a092=(a092+a080); - a080=(a045*a095); - a080=(a005*a080); - a091=(a046*a084); - a080=(a080+a091); - a092=(a092+a080); - a080=(a019*a102); - a091=(a082*a102); - a091=(a010*a091); - a099=(a047*a102); - a091=(a091+a099); - a080=(a080+a091); - a092=(a092+a080); - a080=(a035*a092); - a098=(a098+a080); - a098=(-a098); - if (res[1]!=0) res[1][45]=a098; - a098=(a012*a083); - a080=(a048*a089); - a098=(a098+a080); - a080=(a049*a086); - a098=(a098+a080); - a080=(a050*a079); - a098=(a098+a080); - a080=(a051*a088); - a098=(a098+a080); - a080=(a052*a092); - a098=(a098+a080); - a098=(-a098); - if (res[1]!=0) res[1][46]=a098; - a098=(a042*a083); - a080=(a054*a089); - a098=(a098+a080); - a080=(a055*a086); - a098=(a098+a080); - a080=(a056*a079); - a098=(a098+a080); - a080=(a057*a088); - a098=(a098+a080); - a080=(a058*a092); - a098=(a098+a080); - a098=(-a098); - if (res[1]!=0) res[1][47]=a098; - a098=(a053*a083); - a080=(a060*a089); - a098=(a098+a080); - a080=(a061*a086); - a098=(a098+a080); - a080=(a062*a079); - a098=(a098+a080); - a080=(a063*a088); - a098=(a098+a080); - a080=(a064*a092); - a098=(a098+a080); - a098=(-a098); - if (res[1]!=0) res[1][48]=a098; - a098=(a059*a083); - a080=(a066*a089); - a098=(a098+a080); - a080=(a067*a086); - a098=(a098+a080); - a080=(a068*a079); - a098=(a098+a080); - a080=(a069*a088); - a098=(a098+a080); - a080=(a070*a092); - a098=(a098+a080); - a098=(-a098); - if (res[1]!=0) res[1][49]=a098; - a083=(a065*a083); - a089=(a071*a089); - a083=(a083+a089); - a086=(a017*a086); - a083=(a083+a086); - a079=(a023*a079); - a083=(a083+a079); - a088=(a070*a088); - a083=(a083+a088); - a092=(a034*a092); - a083=(a083+a092); - a083=(-a083); - if (res[1]!=0) res[1][50]=a083; - a083=arg[1]? arg[1][51] : 0; - a092=(a001*a083); - a088=(a072*a092); - a079=arg[1]? arg[1][52] : 0; - a086=(a079/a094); - a089=(a041*a086); - a088=(a088+a089); - a088=(a005*a088); - a089=(a073*a084); - a088=(a088+a089); - a095=(a095+a088); - a086=(a001*a086); - a083=(a041*a083); - a088=(a072*a083); - a086=(a086-a088); - a086=(a010*a086); - a088=(a075*a102); - a086=(a086+a088); - a095=(a095+a086); - if (res[1]!=0) res[1][51]=a095; - a095=(a001*a084); - a086=(a005*a083); - a095=(a095-a086); - a086=(a010*a092); - a088=(a041*a102); - a086=(a086+a088); - a095=(a095-a086); - if (res[1]!=0) res[1][52]=a095; - a092=(a092/a074); - a079=(a027*a079); - a095=(a090*a079); - a092=(a092+a095); - a092=(a005*a092); - a084=(a076*a084); - a092=(a092+a084); - a079=(a081*a079); - a083=(a083/a074); - a079=(a079-a083); - a079=(a010*a079); - a102=(a078*a102); - a079=(a079+a102); - a092=(a092+a079); - if (res[1]!=0) res[1][53]=a092; - a092=arg[1]? arg[1][56] : 0; - a079=(a002*a092); - a102=(a005*a079); - a083=arg[1]? arg[1][58] : 0; - a084=(a004*a083); - a102=(a102+a084); - a084=arg[1]? arg[1][55] : 0; - a095=(a007*a084); - a086=(a010*a095); - a088=arg[1]? arg[1][59] : 0; - a089=(a009*a088); - a086=(a086+a089); - a102=(a102+a086); - a086=arg[1]? arg[1][54] : 0; - a089=(a006*a086); - a098=(a087*a086); - a098=(a011*a098); - a080=(a013*a086); - a098=(a098+a080); - a089=(a089+a098); - a102=(a102+a089); - a089=(a000*a102); - a098=(a007*a092); - a080=(a016*a098); - a091=arg[1]? arg[1][57] : 0; - a099=(a015*a091); - a080=(a080+a099); - a099=(a002*a086); - a100=(a010*a099); - a101=(a018*a088); - a100=(a100+a101); - a080=(a080+a100); - a100=(a019*a084); - a101=(a093*a084); - a101=(a008*a101); - a085=(a021*a084); - a101=(a101+a085); - a100=(a100+a101); - a080=(a080+a100); - a100=(a014*a080); - a089=(a089+a100); - a100=(a002*a084); - a101=(a016*a100); - a085=(a022*a091); - a101=(a101+a085); - a085=(a007*a086); - a103=(a005*a085); - a104=(a024*a083); - a103=(a103+a104); - a101=(a101+a103); - a103=(a019*a092); - a104=(a097*a092); - a104=(a003*a104); - a105=(a026*a092); - a104=(a104+a105); - a103=(a103+a104); - a101=(a101+a103); - a103=(a020*a101); - a089=(a089+a103); - a079=(a008*a079); - a103=(a004*a084); - a079=(a079+a103); - a095=(a003*a095); - a103=(a009*a092); - a095=(a095+a103); - a079=(a079+a095); - a095=(a028*a088); - a095=(a005*a095); - a103=(a029*a083); - a095=(a095+a103); - a079=(a079+a095); - a095=(a030*a083); - a095=(a010*a095); - a103=(a031*a088); - a095=(a095+a103); - a079=(a079+a095); - a095=(a019*a091); - a103=(a096*a091); - a103=(a016*a103); - a104=(a033*a091); - a103=(a103+a104); - a095=(a095+a103); - a079=(a079+a095); - a095=(a025*a079); - a089=(a089+a095); - a098=(a011*a098); - a095=(a015*a086); - a098=(a098+a095); - a099=(a003*a099); - a092=(a018*a092); - a099=(a099+a092); - a098=(a098+a099); - a099=(a036*a088); - a099=(a016*a099); - a092=(a037*a091); - a099=(a099+a092); - a098=(a098+a099); - a099=(a038*a091); - a099=(a010*a099); - a092=(a039*a088); - a099=(a099+a092); - a098=(a098+a099); - a099=(a019*a083); - a092=(a077*a083); - a092=(a005*a092); - a095=(a040*a083); - a092=(a092+a095); - a099=(a099+a092); - a098=(a098+a099); - a099=(a032*a098); - a089=(a089+a099); - a100=(a011*a100); - a086=(a022*a086); - a100=(a100+a086); - a085=(a008*a085); - a084=(a024*a084); - a085=(a085+a084); - a100=(a100+a085); - a085=(a043*a083); - a085=(a016*a085); - a084=(a044*a091); - a085=(a085+a084); - a100=(a100+a085); - a085=(a045*a091); - a085=(a005*a085); - a084=(a046*a083); - a085=(a085+a084); - a100=(a100+a085); - a085=(a019*a088); - a084=(a082*a088); - a084=(a010*a084); - a086=(a047*a088); - a084=(a084+a086); - a085=(a085+a084); - a100=(a100+a085); - a085=(a035*a100); - a089=(a089+a085); - a089=(-a089); - if (res[1]!=0) res[1][54]=a089; - a089=(a012*a102); - a085=(a048*a080); - a089=(a089+a085); - a085=(a049*a101); - a089=(a089+a085); - a085=(a050*a079); - a089=(a089+a085); - a085=(a051*a098); - a089=(a089+a085); - a085=(a052*a100); - a089=(a089+a085); - a089=(-a089); - if (res[1]!=0) res[1][55]=a089; - a089=(a042*a102); - a085=(a054*a080); - a089=(a089+a085); - a085=(a055*a101); - a089=(a089+a085); - a085=(a056*a079); - a089=(a089+a085); - a085=(a057*a098); - a089=(a089+a085); - a085=(a058*a100); - a089=(a089+a085); - a089=(-a089); - if (res[1]!=0) res[1][56]=a089; - a089=(a053*a102); - a085=(a060*a080); - a089=(a089+a085); - a085=(a061*a101); - a089=(a089+a085); - a085=(a062*a079); - a089=(a089+a085); - a085=(a063*a098); - a089=(a089+a085); - a085=(a064*a100); - a089=(a089+a085); - a089=(-a089); - if (res[1]!=0) res[1][57]=a089; - a089=(a059*a102); - a085=(a066*a080); - a089=(a089+a085); - a085=(a067*a101); - a089=(a089+a085); - a085=(a068*a079); - a089=(a089+a085); - a085=(a069*a098); - a089=(a089+a085); - a085=(a070*a100); - a089=(a089+a085); - a089=(-a089); - if (res[1]!=0) res[1][58]=a089; - a102=(a065*a102); - a080=(a071*a080); - a102=(a102+a080); - a101=(a017*a101); - a102=(a102+a101); - a079=(a023*a079); - a102=(a102+a079); - a098=(a070*a098); - a102=(a102+a098); - a100=(a034*a100); - a102=(a102+a100); - a102=(-a102); - if (res[1]!=0) res[1][59]=a102; - a102=arg[1]? arg[1][60] : 0; - a100=(a001*a102); - a098=(a072*a100); - a079=arg[1]? arg[1][61] : 0; - a101=(a079/a094); - a080=(a041*a101); - a098=(a098+a080); - a098=(a005*a098); - a080=(a073*a083); - a098=(a098+a080); - a091=(a091+a098); - a101=(a001*a101); - a102=(a041*a102); - a098=(a072*a102); - a101=(a101-a098); - a101=(a010*a101); - a098=(a075*a088); - a101=(a101+a098); - a091=(a091+a101); - if (res[1]!=0) res[1][60]=a091; - a091=(a001*a083); - a101=(a005*a102); - a091=(a091-a101); - a101=(a010*a100); - a098=(a041*a088); - a101=(a101+a098); - a091=(a091-a101); - if (res[1]!=0) res[1][61]=a091; - a100=(a100/a074); - a079=(a027*a079); - a091=(a090*a079); - a100=(a100+a091); - a100=(a005*a100); - a083=(a076*a083); - a100=(a100+a083); - a079=(a081*a079); - a102=(a102/a074); - a079=(a079-a102); - a079=(a010*a079); - a088=(a078*a088); - a079=(a079+a088); - a100=(a100+a079); - if (res[1]!=0) res[1][62]=a100; - a100=arg[1]? arg[1][65] : 0; - a079=(a002*a100); - a088=(a005*a079); - a102=arg[1]? arg[1][67] : 0; - a083=(a004*a102); - a088=(a088+a083); - a083=arg[1]? arg[1][64] : 0; - a091=(a007*a083); - a101=(a010*a091); - a098=arg[1]? arg[1][68] : 0; - a080=(a009*a098); - a101=(a101+a080); - a088=(a088+a101); - a101=arg[1]? arg[1][63] : 0; - a080=(a006*a101); - a089=(a087*a101); - a089=(a011*a089); - a085=(a013*a101); - a089=(a089+a085); - a080=(a080+a089); - a088=(a088+a080); - a080=(a000*a088); - a089=(a007*a100); - a085=(a016*a089); - a084=arg[1]? arg[1][66] : 0; - a086=(a015*a084); - a085=(a085+a086); - a086=(a002*a101); - a099=(a010*a086); - a092=(a018*a098); - a099=(a099+a092); - a085=(a085+a099); - a099=(a019*a083); - a092=(a093*a083); - a092=(a008*a092); - a095=(a021*a083); - a092=(a092+a095); - a099=(a099+a092); - a085=(a085+a099); - a099=(a014*a085); - a080=(a080+a099); - a099=(a002*a083); - a092=(a016*a099); - a095=(a022*a084); - a092=(a092+a095); - a095=(a007*a101); - a103=(a005*a095); - a104=(a024*a102); - a103=(a103+a104); - a092=(a092+a103); - a103=(a019*a100); - a104=(a097*a100); - a104=(a003*a104); - a105=(a026*a100); - a104=(a104+a105); - a103=(a103+a104); - a092=(a092+a103); - a103=(a020*a092); - a080=(a080+a103); - a079=(a008*a079); - a103=(a004*a083); - a079=(a079+a103); - a091=(a003*a091); - a103=(a009*a100); - a091=(a091+a103); - a079=(a079+a091); - a091=(a028*a098); - a091=(a005*a091); - a103=(a029*a102); - a091=(a091+a103); - a079=(a079+a091); - a091=(a030*a102); - a091=(a010*a091); - a103=(a031*a098); - a091=(a091+a103); - a079=(a079+a091); - a091=(a019*a084); - a103=(a096*a084); - a103=(a016*a103); - a104=(a033*a084); - a103=(a103+a104); - a091=(a091+a103); - a079=(a079+a091); - a091=(a025*a079); - a080=(a080+a091); - a089=(a011*a089); - a091=(a015*a101); - a089=(a089+a091); - a086=(a003*a086); - a100=(a018*a100); - a086=(a086+a100); - a089=(a089+a086); - a086=(a036*a098); - a086=(a016*a086); - a100=(a037*a084); - a086=(a086+a100); - a089=(a089+a086); - a086=(a038*a084); - a086=(a010*a086); - a100=(a039*a098); - a086=(a086+a100); - a089=(a089+a086); - a086=(a019*a102); - a100=(a077*a102); - a100=(a005*a100); - a091=(a040*a102); - a100=(a100+a091); - a086=(a086+a100); - a089=(a089+a086); - a086=(a032*a089); - a080=(a080+a086); - a099=(a011*a099); - a101=(a022*a101); - a099=(a099+a101); - a095=(a008*a095); - a083=(a024*a083); - a095=(a095+a083); - a099=(a099+a095); - a095=(a043*a102); - a095=(a016*a095); - a083=(a044*a084); - a095=(a095+a083); - a099=(a099+a095); - a095=(a045*a084); - a095=(a005*a095); - a083=(a046*a102); - a095=(a095+a083); - a099=(a099+a095); - a095=(a019*a098); - a083=(a082*a098); - a083=(a010*a083); - a101=(a047*a098); - a083=(a083+a101); - a095=(a095+a083); - a099=(a099+a095); - a095=(a035*a099); - a080=(a080+a095); - a080=(-a080); - if (res[1]!=0) res[1][63]=a080; - a080=(a012*a088); - a095=(a048*a085); - a080=(a080+a095); - a095=(a049*a092); - a080=(a080+a095); - a095=(a050*a079); - a080=(a080+a095); - a095=(a051*a089); - a080=(a080+a095); - a095=(a052*a099); - a080=(a080+a095); - a080=(-a080); - if (res[1]!=0) res[1][64]=a080; - a080=(a042*a088); - a095=(a054*a085); - a080=(a080+a095); - a095=(a055*a092); - a080=(a080+a095); - a095=(a056*a079); - a080=(a080+a095); - a095=(a057*a089); - a080=(a080+a095); - a095=(a058*a099); - a080=(a080+a095); - a080=(-a080); - if (res[1]!=0) res[1][65]=a080; - a080=(a053*a088); - a095=(a060*a085); - a080=(a080+a095); - a095=(a061*a092); - a080=(a080+a095); - a095=(a062*a079); - a080=(a080+a095); - a095=(a063*a089); - a080=(a080+a095); - a095=(a064*a099); - a080=(a080+a095); - a080=(-a080); - if (res[1]!=0) res[1][66]=a080; - a080=(a059*a088); - a095=(a066*a085); - a080=(a080+a095); - a095=(a067*a092); - a080=(a080+a095); - a095=(a068*a079); - a080=(a080+a095); - a095=(a069*a089); - a080=(a080+a095); - a095=(a070*a099); - a080=(a080+a095); - a080=(-a080); - if (res[1]!=0) res[1][67]=a080; - a088=(a065*a088); - a085=(a071*a085); - a088=(a088+a085); - a092=(a017*a092); - a088=(a088+a092); - a079=(a023*a079); - a088=(a088+a079); - a089=(a070*a089); - a088=(a088+a089); - a099=(a034*a099); - a088=(a088+a099); - a088=(-a088); - if (res[1]!=0) res[1][68]=a088; - a088=arg[1]? arg[1][69] : 0; - a099=(a001*a088); - a089=(a072*a099); - a079=arg[1]? arg[1][70] : 0; - a092=(a079/a094); - a085=(a041*a092); - a089=(a089+a085); - a089=(a005*a089); - a085=(a073*a102); - a089=(a089+a085); - a084=(a084+a089); - a092=(a001*a092); - a088=(a041*a088); - a089=(a072*a088); - a092=(a092-a089); - a092=(a010*a092); - a089=(a075*a098); - a092=(a092+a089); - a084=(a084+a092); - if (res[1]!=0) res[1][69]=a084; - a084=(a001*a102); - a092=(a005*a088); - a084=(a084-a092); - a092=(a010*a099); - a089=(a041*a098); - a092=(a092+a089); - a084=(a084-a092); - if (res[1]!=0) res[1][70]=a084; - a099=(a099/a074); - a079=(a027*a079); - a084=(a090*a079); - a099=(a099+a084); - a099=(a005*a099); - a102=(a076*a102); - a099=(a099+a102); - a079=(a081*a079); - a088=(a088/a074); - a079=(a079-a088); - a079=(a010*a079); - a098=(a078*a098); - a079=(a079+a098); - a099=(a099+a079); - if (res[1]!=0) res[1][71]=a099; - a099=arg[1]? arg[1][74] : 0; - a079=(a002*a099); - a098=(a005*a079); - a088=arg[1]? arg[1][76] : 0; - a102=(a004*a088); - a098=(a098+a102); - a102=arg[1]? arg[1][73] : 0; - a084=(a007*a102); - a092=(a010*a084); - a089=arg[1]? arg[1][77] : 0; - a085=(a009*a089); - a092=(a092+a085); - a098=(a098+a092); - a092=arg[1]? arg[1][72] : 0; - a085=(a006*a092); - a080=(a087*a092); - a080=(a011*a080); - a095=(a013*a092); - a080=(a080+a095); - a085=(a085+a080); - a098=(a098+a085); - a085=(a000*a098); - a080=(a007*a099); - a095=(a016*a080); - a083=arg[1]? arg[1][75] : 0; - a101=(a015*a083); - a095=(a095+a101); - a101=(a002*a092); - a086=(a010*a101); - a100=(a018*a089); - a086=(a086+a100); - a095=(a095+a086); - a086=(a019*a102); - a100=(a093*a102); - a100=(a008*a100); - a091=(a021*a102); - a100=(a100+a091); - a086=(a086+a100); - a095=(a095+a086); - a086=(a014*a095); - a085=(a085+a086); - a086=(a002*a102); - a100=(a016*a086); - a091=(a022*a083); - a100=(a100+a091); - a091=(a007*a092); - a103=(a005*a091); - a104=(a024*a088); - a103=(a103+a104); - a100=(a100+a103); - a103=(a019*a099); - a104=(a097*a099); - a104=(a003*a104); - a105=(a026*a099); - a104=(a104+a105); - a103=(a103+a104); - a100=(a100+a103); - a103=(a020*a100); - a085=(a085+a103); - a079=(a008*a079); - a103=(a004*a102); - a079=(a079+a103); - a084=(a003*a084); - a103=(a009*a099); - a084=(a084+a103); - a079=(a079+a084); - a084=(a028*a089); - a084=(a005*a084); - a103=(a029*a088); - a084=(a084+a103); - a079=(a079+a084); - a084=(a030*a088); - a084=(a010*a084); - a103=(a031*a089); - a084=(a084+a103); - a079=(a079+a084); - a084=(a019*a083); - a103=(a096*a083); - a103=(a016*a103); - a104=(a033*a083); - a103=(a103+a104); - a084=(a084+a103); - a079=(a079+a084); - a084=(a025*a079); - a085=(a085+a084); - a080=(a011*a080); - a084=(a015*a092); - a080=(a080+a084); - a101=(a003*a101); - a099=(a018*a099); - a101=(a101+a099); - a080=(a080+a101); - a101=(a036*a089); - a101=(a016*a101); - a099=(a037*a083); - a101=(a101+a099); - a080=(a080+a101); - a101=(a038*a083); - a101=(a010*a101); - a099=(a039*a089); - a101=(a101+a099); - a080=(a080+a101); - a101=(a019*a088); - a099=(a077*a088); - a099=(a005*a099); - a084=(a040*a088); - a099=(a099+a084); - a101=(a101+a099); - a080=(a080+a101); - a101=(a032*a080); - a085=(a085+a101); - a086=(a011*a086); - a092=(a022*a092); - a086=(a086+a092); - a091=(a008*a091); - a102=(a024*a102); - a091=(a091+a102); - a086=(a086+a091); - a091=(a043*a088); - a091=(a016*a091); - a102=(a044*a083); - a091=(a091+a102); - a086=(a086+a091); - a091=(a045*a083); - a091=(a005*a091); - a102=(a046*a088); - a091=(a091+a102); - a086=(a086+a091); - a091=(a019*a089); - a102=(a082*a089); - a102=(a010*a102); - a092=(a047*a089); - a102=(a102+a092); - a091=(a091+a102); - a086=(a086+a091); - a091=(a035*a086); - a085=(a085+a091); - a085=(-a085); - if (res[1]!=0) res[1][72]=a085; - a085=(a012*a098); - a091=(a048*a095); - a085=(a085+a091); - a091=(a049*a100); - a085=(a085+a091); - a091=(a050*a079); - a085=(a085+a091); - a091=(a051*a080); - a085=(a085+a091); - a091=(a052*a086); - a085=(a085+a091); - a085=(-a085); - if (res[1]!=0) res[1][73]=a085; - a085=(a042*a098); - a091=(a054*a095); - a085=(a085+a091); - a091=(a055*a100); - a085=(a085+a091); - a091=(a056*a079); - a085=(a085+a091); - a091=(a057*a080); - a085=(a085+a091); - a091=(a058*a086); - a085=(a085+a091); - a085=(-a085); - if (res[1]!=0) res[1][74]=a085; - a085=(a053*a098); - a091=(a060*a095); - a085=(a085+a091); - a091=(a061*a100); - a085=(a085+a091); - a091=(a062*a079); - a085=(a085+a091); - a091=(a063*a080); - a085=(a085+a091); - a091=(a064*a086); - a085=(a085+a091); - a085=(-a085); - if (res[1]!=0) res[1][75]=a085; - a085=(a059*a098); - a091=(a066*a095); - a085=(a085+a091); - a091=(a067*a100); - a085=(a085+a091); - a091=(a068*a079); - a085=(a085+a091); - a091=(a069*a080); - a085=(a085+a091); - a091=(a070*a086); - a085=(a085+a091); - a085=(-a085); - if (res[1]!=0) res[1][76]=a085; - a098=(a065*a098); - a095=(a071*a095); - a098=(a098+a095); - a100=(a017*a100); - a098=(a098+a100); - a079=(a023*a079); - a098=(a098+a079); - a080=(a070*a080); - a098=(a098+a080); - a086=(a034*a086); - a098=(a098+a086); - a098=(-a098); - if (res[1]!=0) res[1][77]=a098; - a098=arg[1]? arg[1][78] : 0; - a086=(a001*a098); - a080=(a072*a086); - a079=arg[1]? arg[1][79] : 0; - a100=(a079/a094); - a095=(a041*a100); - a080=(a080+a095); - a080=(a005*a080); - a095=(a073*a088); - a080=(a080+a095); - a083=(a083+a080); - a100=(a001*a100); - a098=(a041*a098); - a080=(a072*a098); - a100=(a100-a080); - a100=(a010*a100); - a080=(a075*a089); - a100=(a100+a080); - a083=(a083+a100); - if (res[1]!=0) res[1][78]=a083; - a083=(a001*a088); - a100=(a005*a098); - a083=(a083-a100); - a100=(a010*a086); - a080=(a041*a089); - a100=(a100+a080); - a083=(a083-a100); - if (res[1]!=0) res[1][79]=a083; - a086=(a086/a074); - a079=(a027*a079); - a083=(a090*a079); - a086=(a086+a083); - a086=(a005*a086); - a088=(a076*a088); - a086=(a086+a088); - a079=(a081*a079); - a098=(a098/a074); - a079=(a079-a098); - a079=(a010*a079); - a089=(a078*a089); - a079=(a079+a089); - a086=(a086+a079); - if (res[1]!=0) res[1][80]=a086; - a086=arg[2]? arg[2][2] : 0; - a079=(a002*a086); - a089=(a005*a079); - a098=arg[2]? arg[2][4] : 0; - a088=(a004*a098); - a089=(a089+a088); - a088=arg[2]? arg[2][1] : 0; - a083=(a007*a088); - a100=(a010*a083); - a080=arg[2]? arg[2][5] : 0; - a095=(a009*a080); - a100=(a100+a095); - a089=(a089+a100); - a100=arg[2]? arg[2][0] : 0; - a095=(a006*a100); - a085=(a087*a100); - a085=(a011*a085); - a091=(a013*a100); - a085=(a085+a091); - a095=(a095+a085); - a089=(a089+a095); - a095=(a000*a089); - a085=(a007*a086); - a091=(a016*a085); - a102=arg[2]? arg[2][3] : 0; - a092=(a015*a102); - a091=(a091+a092); - a092=(a002*a100); - a101=(a010*a092); - a099=(a018*a080); - a101=(a101+a099); - a091=(a091+a101); - a101=(a019*a088); - a099=(a093*a088); - a099=(a008*a099); - a084=(a021*a088); - a099=(a099+a084); - a101=(a101+a099); - a091=(a091+a101); - a101=(a014*a091); - a095=(a095+a101); - a101=(a002*a088); - a099=(a016*a101); - a084=(a022*a102); - a099=(a099+a084); - a084=(a007*a100); - a103=(a005*a084); - a104=(a024*a098); - a103=(a103+a104); - a099=(a099+a103); - a103=(a019*a086); - a104=(a097*a086); - a104=(a003*a104); - a105=(a026*a086); - a104=(a104+a105); - a103=(a103+a104); - a099=(a099+a103); - a103=(a020*a099); - a095=(a095+a103); - a079=(a008*a079); - a103=(a004*a088); - a079=(a079+a103); - a083=(a003*a083); - a103=(a009*a086); - a083=(a083+a103); - a079=(a079+a083); - a083=(a028*a080); - a083=(a005*a083); - a103=(a029*a098); - a083=(a083+a103); - a079=(a079+a083); - a083=(a030*a098); - a083=(a010*a083); - a103=(a031*a080); - a083=(a083+a103); - a079=(a079+a083); - a083=(a019*a102); - a103=(a096*a102); - a103=(a016*a103); - a104=(a033*a102); - a103=(a103+a104); - a083=(a083+a103); - a079=(a079+a083); - a083=(a025*a079); - a095=(a095+a083); - a085=(a011*a085); - a083=(a015*a100); - a085=(a085+a083); - a092=(a003*a092); - a086=(a018*a086); - a092=(a092+a086); - a085=(a085+a092); - a092=(a036*a080); - a092=(a016*a092); - a086=(a037*a102); - a092=(a092+a086); - a085=(a085+a092); - a092=(a038*a102); - a092=(a010*a092); - a086=(a039*a080); - a092=(a092+a086); - a085=(a085+a092); - a092=(a019*a098); - a086=(a077*a098); - a086=(a005*a086); - a083=(a040*a098); - a086=(a086+a083); - a092=(a092+a086); - a085=(a085+a092); - a092=(a032*a085); - a095=(a095+a092); - a101=(a011*a101); - a100=(a022*a100); - a101=(a101+a100); - a084=(a008*a084); - a088=(a024*a088); - a084=(a084+a088); - a101=(a101+a084); - a084=(a043*a098); - a084=(a016*a084); - a088=(a044*a102); - a084=(a084+a088); - a101=(a101+a084); - a084=(a045*a102); - a084=(a005*a084); - a088=(a046*a098); - a084=(a084+a088); - a101=(a101+a084); - a084=(a019*a080); - a088=(a082*a080); - a088=(a010*a088); - a100=(a047*a080); - a088=(a088+a100); - a084=(a084+a088); - a101=(a101+a084); - a084=(a035*a101); - a095=(a095+a084); - a095=(a000-a095); - if (res[2]!=0) res[2][0]=a095; - a095=(a012*a089); - a084=(a048*a091); - a095=(a095+a084); - a084=(a049*a099); - a095=(a095+a084); - a084=(a050*a079); - a095=(a095+a084); - a084=(a051*a085); - a095=(a095+a084); - a084=(a052*a101); - a095=(a095+a084); - a095=(a012-a095); - if (res[2]!=0) res[2][1]=a095; - a095=(a042*a089); - a084=(a054*a091); - a095=(a095+a084); - a084=(a055*a099); - a095=(a095+a084); - a084=(a056*a079); - a095=(a095+a084); - a084=(a057*a085); - a095=(a095+a084); - a084=(a058*a101); - a095=(a095+a084); - a095=(a042-a095); - if (res[2]!=0) res[2][2]=a095; - a095=(a053*a089); - a084=(a060*a091); - a095=(a095+a084); - a084=(a061*a099); - a095=(a095+a084); - a084=(a062*a079); - a095=(a095+a084); - a084=(a063*a085); - a095=(a095+a084); - a084=(a064*a101); - a095=(a095+a084); - a095=(a053-a095); - if (res[2]!=0) res[2][3]=a095; - a095=(a059*a089); - a084=(a066*a091); - a095=(a095+a084); - a084=(a067*a099); - a095=(a095+a084); - a084=(a068*a079); - a095=(a095+a084); - a084=(a069*a085); - a095=(a095+a084); - a084=(a070*a101); - a095=(a095+a084); - a095=(a059-a095); - if (res[2]!=0) res[2][4]=a095; - a089=(a065*a089); - a091=(a071*a091); - a089=(a089+a091); - a099=(a017*a099); - a089=(a089+a099); - a079=(a023*a079); - a089=(a089+a079); - a085=(a070*a085); - a089=(a089+a085); - a101=(a034*a101); - a089=(a089+a101); - a089=(a065-a089); - if (res[2]!=0) res[2][5]=a089; - a089=arg[2]? arg[2][6] : 0; - a101=(a001*a089); - a085=(a072*a101); - a079=arg[2]? arg[2][7] : 0; - a099=(a079/a094); - a091=(a041*a099); - a085=(a085+a091); - a085=(a005*a085); - a091=(a073*a098); - a085=(a085+a091); - a102=(a102+a085); - a099=(a001*a099); - a089=(a041*a089); - a085=(a072*a089); - a099=(a099-a085); - a099=(a010*a099); - a085=(a075*a080); - a099=(a099+a085); - a102=(a102+a099); - if (res[2]!=0) res[2][6]=a102; - a102=(a001*a098); - a099=(a005*a089); - a102=(a102-a099); - a099=(a010*a101); - a085=(a041*a080); - a099=(a099+a085); - a102=(a102-a099); - if (res[2]!=0) res[2][7]=a102; - a101=(a101/a074); - a079=(a027*a079); - a102=(a090*a079); - a101=(a101+a102); - a101=(a005*a101); - a098=(a076*a098); - a101=(a101+a098); - a079=(a081*a079); - a089=(a089/a074); - a079=(a079-a089); - a079=(a010*a079); - a080=(a078*a080); - a079=(a079+a080); - a101=(a101+a079); - if (res[2]!=0) res[2][8]=a101; - a101=arg[2]? arg[2][11] : 0; - a079=(a002*a101); - a080=(a005*a079); - a089=arg[2]? arg[2][13] : 0; - a098=(a004*a089); - a080=(a080+a098); - a098=arg[2]? arg[2][10] : 0; - a102=(a007*a098); - a099=(a010*a102); - a085=arg[2]? arg[2][14] : 0; - a091=(a009*a085); - a099=(a099+a091); - a080=(a080+a099); - a099=arg[2]? arg[2][9] : 0; - a091=(a006*a099); - a095=(a087*a099); - a095=(a011*a095); - a084=(a013*a099); - a095=(a095+a084); - a091=(a091+a095); - a080=(a080+a091); - a091=(a000*a080); - a095=(a007*a101); - a084=(a016*a095); - a088=arg[2]? arg[2][12] : 0; - a100=(a015*a088); - a084=(a084+a100); - a100=(a002*a099); - a092=(a010*a100); - a086=(a018*a085); - a092=(a092+a086); - a084=(a084+a092); - a092=(a019*a098); - a086=(a093*a098); - a086=(a008*a086); - a083=(a021*a098); - a086=(a086+a083); - a092=(a092+a086); - a084=(a084+a092); - a092=(a014*a084); - a091=(a091+a092); - a092=(a002*a098); - a086=(a016*a092); - a083=(a022*a088); - a086=(a086+a083); - a083=(a007*a099); - a103=(a005*a083); - a104=(a024*a089); - a103=(a103+a104); - a086=(a086+a103); - a103=(a019*a101); - a104=(a097*a101); - a104=(a003*a104); - a105=(a026*a101); - a104=(a104+a105); - a103=(a103+a104); - a086=(a086+a103); - a103=(a020*a086); - a091=(a091+a103); - a079=(a008*a079); - a103=(a004*a098); - a079=(a079+a103); - a102=(a003*a102); - a103=(a009*a101); - a102=(a102+a103); - a079=(a079+a102); - a102=(a028*a085); - a102=(a005*a102); - a103=(a029*a089); - a102=(a102+a103); - a079=(a079+a102); - a102=(a030*a089); - a102=(a010*a102); - a103=(a031*a085); - a102=(a102+a103); - a079=(a079+a102); - a102=(a019*a088); - a103=(a096*a088); - a103=(a016*a103); - a104=(a033*a088); - a103=(a103+a104); - a102=(a102+a103); - a079=(a079+a102); - a102=(a025*a079); - a091=(a091+a102); - a095=(a011*a095); - a102=(a015*a099); - a095=(a095+a102); - a100=(a003*a100); - a101=(a018*a101); - a100=(a100+a101); - a095=(a095+a100); - a100=(a036*a085); - a100=(a016*a100); - a101=(a037*a088); - a100=(a100+a101); - a095=(a095+a100); - a100=(a038*a088); - a100=(a010*a100); - a101=(a039*a085); - a100=(a100+a101); - a095=(a095+a100); - a100=(a019*a089); - a101=(a077*a089); - a101=(a005*a101); - a102=(a040*a089); - a101=(a101+a102); - a100=(a100+a101); - a095=(a095+a100); - a100=(a032*a095); - a091=(a091+a100); - a092=(a011*a092); - a099=(a022*a099); - a092=(a092+a099); - a083=(a008*a083); - a098=(a024*a098); - a083=(a083+a098); - a092=(a092+a083); - a083=(a043*a089); - a083=(a016*a083); - a098=(a044*a088); - a083=(a083+a098); - a092=(a092+a083); - a083=(a045*a088); - a083=(a005*a083); - a098=(a046*a089); - a083=(a083+a098); - a092=(a092+a083); - a083=(a019*a085); - a098=(a082*a085); - a098=(a010*a098); - a099=(a047*a085); - a098=(a098+a099); - a083=(a083+a098); - a092=(a092+a083); - a083=(a035*a092); - a091=(a091+a083); - a091=(a032-a091); - if (res[2]!=0) res[2][9]=a091; - a091=(a012*a080); - a083=(a048*a084); - a091=(a091+a083); - a083=(a049*a086); - a091=(a091+a083); - a083=(a050*a079); - a091=(a091+a083); - a083=(a051*a095); - a091=(a091+a083); - a083=(a052*a092); - a091=(a091+a083); - a091=(a051-a091); - if (res[2]!=0) res[2][10]=a091; - a091=(a042*a080); - a083=(a054*a084); - a091=(a091+a083); - a083=(a055*a086); - a091=(a091+a083); - a083=(a056*a079); - a091=(a091+a083); - a083=(a057*a095); - a091=(a091+a083); - a083=(a058*a092); - a091=(a091+a083); - a091=(a057-a091); - if (res[2]!=0) res[2][11]=a091; - a091=(a053*a080); - a083=(a060*a084); - a091=(a091+a083); - a083=(a061*a086); - a091=(a091+a083); - a083=(a062*a079); - a091=(a091+a083); - a083=(a063*a095); - a091=(a091+a083); - a083=(a064*a092); - a091=(a091+a083); - a091=(a063-a091); - if (res[2]!=0) res[2][12]=a091; - a091=(a059*a080); - a083=(a066*a084); - a091=(a091+a083); - a083=(a067*a086); - a091=(a091+a083); - a083=(a068*a079); - a091=(a091+a083); - a083=(a069*a095); - a091=(a091+a083); - a083=(a070*a092); - a091=(a091+a083); - a091=(a069-a091); - if (res[2]!=0) res[2][13]=a091; - a080=(a065*a080); - a084=(a071*a084); - a080=(a080+a084); - a086=(a017*a086); - a080=(a080+a086); - a079=(a023*a079); - a080=(a080+a079); - a095=(a070*a095); - a080=(a080+a095); - a092=(a034*a092); - a080=(a080+a092); - a080=(a070-a080); - if (res[2]!=0) res[2][14]=a080; - a080=arg[2]? arg[2][15] : 0; - a092=(a001*a080); - a095=(a072*a092); - a079=arg[2]? arg[2][16] : 0; - a086=(a079/a094); - a084=(a041*a086); - a095=(a095+a084); - a095=(a005*a095); - a084=(a073*a089); - a095=(a095+a084); - a088=(a088+a095); - a086=(a001*a086); - a080=(a041*a080); - a095=(a072*a080); - a086=(a086-a095); - a086=(a010*a086); - a095=(a075*a085); - a086=(a086+a095); - a088=(a088+a086); - if (res[2]!=0) res[2][15]=a088; - a088=(a001*a089); - a086=(a005*a080); - a088=(a088-a086); - a086=(a010*a092); - a095=(a041*a085); - a086=(a086+a095); - a088=(a088-a086); - if (res[2]!=0) res[2][16]=a088; - a092=(a092/a074); - a079=(a027*a079); - a088=(a090*a079); - a092=(a092+a088); - a092=(a005*a092); - a089=(a076*a089); - a092=(a092+a089); - a079=(a081*a079); - a080=(a080/a074); - a079=(a079-a080); - a079=(a010*a079); - a085=(a078*a085); - a079=(a079+a085); - a092=(a092+a079); - if (res[2]!=0) res[2][17]=a092; - a092=arg[2]? arg[2][20] : 0; - a079=(a002*a092); - a085=(a005*a079); - a080=arg[2]? arg[2][22] : 0; - a089=(a004*a080); - a085=(a085+a089); - a089=arg[2]? arg[2][19] : 0; - a088=(a007*a089); - a086=(a010*a088); - a095=arg[2]? arg[2][23] : 0; - a084=(a009*a095); - a086=(a086+a084); - a085=(a085+a086); - a086=arg[2]? arg[2][18] : 0; - a006=(a006*a086); - a087=(a087*a086); - a087=(a011*a087); - a013=(a013*a086); - a087=(a087+a013); - a006=(a006+a087); - a085=(a085+a006); - a000=(a000*a085); - a006=(a007*a092); - a087=(a016*a006); - a013=arg[2]? arg[2][21] : 0; - a084=(a015*a013); - a087=(a087+a084); - a084=(a002*a086); - a091=(a010*a084); - a083=(a018*a095); - a091=(a091+a083); - a087=(a087+a091); - a091=(a019*a089); - a093=(a093*a089); - a093=(a008*a093); - a021=(a021*a089); - a093=(a093+a021); - a091=(a091+a093); - a087=(a087+a091); - a014=(a014*a087); - a000=(a000+a014); - a002=(a002*a089); - a014=(a016*a002); - a091=(a022*a013); - a014=(a014+a091); - a007=(a007*a086); - a091=(a005*a007); - a093=(a024*a080); - a091=(a091+a093); - a014=(a014+a091); - a091=(a019*a092); - a097=(a097*a092); - a097=(a003*a097); - a026=(a026*a092); - a097=(a097+a026); - a091=(a091+a097); - a014=(a014+a091); - a020=(a020*a014); - a000=(a000+a020); - a079=(a008*a079); - a004=(a004*a089); - a079=(a079+a004); - a088=(a003*a088); - a009=(a009*a092); - a088=(a088+a009); - a079=(a079+a088); - a028=(a028*a095); - a028=(a005*a028); - a029=(a029*a080); - a028=(a028+a029); - a079=(a079+a028); - a030=(a030*a080); - a030=(a010*a030); - a031=(a031*a095); - a030=(a030+a031); - a079=(a079+a030); - a030=(a019*a013); - a096=(a096*a013); - a096=(a016*a096); - a033=(a033*a013); - a096=(a096+a033); - a030=(a030+a096); - a079=(a079+a030); - a025=(a025*a079); - a000=(a000+a025); - a006=(a011*a006); - a015=(a015*a086); - a006=(a006+a015); - a003=(a003*a084); - a018=(a018*a092); - a003=(a003+a018); - a006=(a006+a003); - a036=(a036*a095); - a036=(a016*a036); - a037=(a037*a013); - a036=(a036+a037); - a006=(a006+a036); - a038=(a038*a013); - a038=(a010*a038); - a039=(a039*a095); - a038=(a038+a039); - a006=(a006+a038); - a038=(a019*a080); - a077=(a077*a080); - a077=(a005*a077); - a040=(a040*a080); - a077=(a077+a040); - a038=(a038+a077); - a006=(a006+a038); - a032=(a032*a006); - a000=(a000+a032); - a011=(a011*a002); - a022=(a022*a086); - a011=(a011+a022); - a008=(a008*a007); - a024=(a024*a089); - a008=(a008+a024); - a011=(a011+a008); - a043=(a043*a080); - a016=(a016*a043); - a044=(a044*a013); - a016=(a016+a044); - a011=(a011+a016); - a045=(a045*a013); - a045=(a005*a045); - a046=(a046*a080); - a045=(a045+a046); - a011=(a011+a045); - a019=(a019*a095); - a082=(a082*a095); - a082=(a010*a082); - a047=(a047*a095); - a082=(a082+a047); - a019=(a019+a082); - a011=(a011+a019); - a019=(a035*a011); - a000=(a000+a019); - a035=(a035-a000); - if (res[2]!=0) res[2][18]=a035; - a012=(a012*a085); - a048=(a048*a087); - a012=(a012+a048); - a049=(a049*a014); - a012=(a012+a049); - a050=(a050*a079); - a012=(a012+a050); - a051=(a051*a006); - a012=(a012+a051); - a051=(a052*a011); - a012=(a012+a051); - a052=(a052-a012); - if (res[2]!=0) res[2][19]=a052; - a042=(a042*a085); - a054=(a054*a087); - a042=(a042+a054); - a055=(a055*a014); - a042=(a042+a055); - a056=(a056*a079); - a042=(a042+a056); - a057=(a057*a006); - a042=(a042+a057); - a057=(a058*a011); - a042=(a042+a057); - a058=(a058-a042); - if (res[2]!=0) res[2][20]=a058; - a053=(a053*a085); - a060=(a060*a087); - a053=(a053+a060); - a061=(a061*a014); - a053=(a053+a061); - a062=(a062*a079); - a053=(a053+a062); - a063=(a063*a006); - a053=(a053+a063); - a063=(a064*a011); - a053=(a053+a063); - a064=(a064-a053); - if (res[2]!=0) res[2][21]=a064; - a059=(a059*a085); - a066=(a066*a087); - a059=(a059+a066); - a067=(a067*a014); - a059=(a059+a067); - a068=(a068*a079); - a059=(a059+a068); - a069=(a069*a006); - a059=(a059+a069); - a069=(a070*a011); - a059=(a059+a069); - a059=(a070-a059); - if (res[2]!=0) res[2][22]=a059; - a065=(a065*a085); - a071=(a071*a087); - a065=(a065+a071); - a017=(a017*a014); - a065=(a065+a017); - a023=(a023*a079); - a065=(a065+a023); - a070=(a070*a006); - a065=(a065+a070); - a011=(a034*a011); - a065=(a065+a011); - a034=(a034-a065); - if (res[2]!=0) res[2][23]=a034; - a034=arg[2]? arg[2][24] : 0; - a065=(a001*a034); - a011=(a072*a065); - a070=arg[2]? arg[2][25] : 0; - a094=(a070/a094); - a006=(a041*a094); - a011=(a011+a006); - a011=(a005*a011); - a073=(a073*a080); - a011=(a011+a073); - a013=(a013+a011); - a094=(a001*a094); - a034=(a041*a034); - a072=(a072*a034); - a094=(a094-a072); - a094=(a010*a094); - a075=(a075*a095); - a094=(a094+a075); - a013=(a013+a094); - if (res[2]!=0) res[2][24]=a013; - a001=(a001*a080); - a013=(a005*a034); - a001=(a001-a013); - a013=(a010*a065); - a041=(a041*a095); - a013=(a013+a041); - a001=(a001-a013); - if (res[2]!=0) res[2][25]=a001; - a065=(a065/a074); - a027=(a027*a070); - a090=(a090*a027); - a065=(a065+a090); - a005=(a005*a065); - a076=(a076*a080); - a005=(a005+a076); - a081=(a081*a027); - a034=(a034/a074); - a081=(a081-a034); - a010=(a010*a081); - a078=(a078*a095); - a010=(a010+a078); - a005=(a005+a010); - if (res[2]!=0) res[2][26]=a005; - return 0; -} - -CASADI_SYMBOL_EXPORT int auv_model_expl_vde_forw(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ - return casadi_f0(arg, res, iw, w, mem); -} - -CASADI_SYMBOL_EXPORT int auv_model_expl_vde_forw_alloc_mem(void) { - return 0; -} - -CASADI_SYMBOL_EXPORT int auv_model_expl_vde_forw_init_mem(int mem) { - return 0; -} - -CASADI_SYMBOL_EXPORT void auv_model_expl_vde_forw_free_mem(int mem) { -} - -CASADI_SYMBOL_EXPORT int auv_model_expl_vde_forw_checkout(void) { - return 0; -} - -CASADI_SYMBOL_EXPORT void auv_model_expl_vde_forw_release(int mem) { -} - -CASADI_SYMBOL_EXPORT void auv_model_expl_vde_forw_incref(void) { -} - -CASADI_SYMBOL_EXPORT void auv_model_expl_vde_forw_decref(void) { -} - -CASADI_SYMBOL_EXPORT casadi_int auv_model_expl_vde_forw_n_in(void) { return 5;} - -CASADI_SYMBOL_EXPORT casadi_int auv_model_expl_vde_forw_n_out(void) { return 3;} - -CASADI_SYMBOL_EXPORT casadi_real auv_model_expl_vde_forw_default_in(casadi_int i) { - switch (i) { - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const char* auv_model_expl_vde_forw_name_in(casadi_int i) { - switch (i) { - case 0: return "i0"; - case 1: return "i1"; - case 2: return "i2"; - case 3: return "i3"; - case 4: return "i4"; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const char* auv_model_expl_vde_forw_name_out(casadi_int i) { - switch (i) { - case 0: return "o0"; - case 1: return "o1"; - case 2: return "o2"; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const casadi_int* auv_model_expl_vde_forw_sparsity_in(casadi_int i) { - switch (i) { - case 0: return casadi_s0; - case 1: return casadi_s1; - case 2: return casadi_s2; - case 3: return casadi_s3; - case 4: return casadi_s4; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const casadi_int* auv_model_expl_vde_forw_sparsity_out(casadi_int i) { - switch (i) { - case 0: return casadi_s0; - case 1: return casadi_s1; - case 2: return casadi_s2; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT int auv_model_expl_vde_forw_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { - if (sz_arg) *sz_arg = 5; - if (sz_res) *sz_res = 3; - if (sz_iw) *sz_iw = 0; - if (sz_w) *sz_w = 0; - return 0; -} - -CASADI_SYMBOL_EXPORT int auv_model_expl_vde_forw_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { - if (sz_arg) *sz_arg = 5*sizeof(const casadi_real*); - if (sz_res) *sz_res = 3*sizeof(casadi_real*); - if (sz_iw) *sz_iw = 0*sizeof(casadi_int); - if (sz_w) *sz_w = 0*sizeof(casadi_real); - return 0; -} - - -#ifdef __cplusplus -} /* extern "C" */ -#endif diff --git a/control/velocity_controller/src/build_auv_solver/auv_model_model/auv_model_expl_vde_forw.o b/control/velocity_controller/src/build_auv_solver/auv_model_model/auv_model_expl_vde_forw.o deleted file mode 100644 index 6559bfa668a750a75297b25f33717bd1f0432f33..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 49648 zcmeHw4S1Brx%Q%{@sHhDY*V$G;&NPU#RRb8X_X+M#uodf6=nAr`4JLB(o|)%;X5QI52M_uDab3EE{HIIkicsdxqoGjELz(Lmxhx5t5y}tk z4WE0UDm?TALMy`Ox%*v@R)u3tRpE;pZ#{QUc=_5cd%_Wkf~d=?!q=^>3SYkr(Mv); z05?g>pu9cdM9SU|ynf(ygNt_#9D4OV`>rdy=g_rfcfDS8Tqsm_?^Ok5Gg7rDLr7lL z)!{pyD7$xX2uQf@m(xNNeL6+ryE=k@HF@|~65A8b9~Uh_9<8{ljp9CWiAKwx6;E1j!RFZ`_sd4c{Sm zRpDW+_}d-sH6&Uj5=Y{HZ8VOA5H;7*s zokn8!NqT)0KPUTL751WC}k*s=3WytiZWJY&r=imL8Vb$ht1haQuh;{jL!%@23=Sv$jDW;4G~-i94X#Cmq##EM z*mnHAi+rAyLN&9t5WfM))6C+}jDlpZ@@6B@*ATA)G()6k5VnOJODC7sMaX(4#~;K* z=0%fE=(CUmc_`LYqGzl1WUD%cW6rf-+dz3eYG1E|lWhBhiNL|0M1&M?FEW z#34w=@K^Dc!QQd=L+%~Vcn2+pdkmv-n+7H9aDOPA-`8Y&k;|?PN!`$Dv#eMJ9N8v% zYP=3%D}f{X#3!5ocr9KB$Du-DKStsG68aPQ?yWDG+p730)gjf|?r z-q;f!z8`-rkg_z|hyYwHjK+{>*^K9E$0K7EPTI)$h*80nXbd_vSb`xw1}(;3;thK+ zI#psHSTj_I3yfgkYB)xiy=x=>(4i}~0^fl0u0)R4mW1;M6(L}jlpG^!URC&Gv!YX_ z9GKufJynz-6B}nM9Y$r2LviD(qM;nW6ARs}20f;vKImLik;md_jYLaNf6$Twt0c5h zZ#JP=;JnV9b!L}K6<>~ER8l6sS<$41x~T!GO$mUkm*4{R!PJ8xSY|;{>>>?c z9Gxi_s79eQlh$|_MkxqkL;2CZnp4ZdXoHU)bFGSKMII~3VEsK7$zUZO%gbP89xD*+ z>U(8c!c?LeL3%l2pb+7@H__ctVOLquzzzMgoIY6xgfCWpAzUYmd0IZ)&vOj|KI3j2 zSdpGuz?dxrr(r0>*Bn(h#zPFB5gB3dJwK{i!$?UE#&o*iC>TyW#sTj|GLK_tVjH9+ z4xx#fh=W=2Inokxp=}di0V(s;7GU;hq&-L)-vM}N6twI9W)()wY0;u^uf?Jcijtzw zazsnK1$iqm=ngx`k#`6GEo07WAnav?ZN>Lmmgh;WTpZYTS2QR}+8&{pW8=-pwp6Cf z=W#VZDh-I@U}(g20G%vV79;vrO2i1A7()X_SU5{<72uCDuH#{RHC4nx8Y4)2AHKIC z)eeHYS=Bbit&0jTP-@UW-iAo)7-1DF|2i^9w~fePitoCp>l`$lkeN)@710tT!mx$l zCZSUTwiuH?4cyJ48aterqhvvD7!?=?V|5I5k2BsJ*SQdjJsBbBWk4IG%K=Tv1*%20 ztL!;qf1z5AGtzk#-=IC{D#=)Cc=( zw1%}Ly%<=iIm)(&8eESIWF|(Eil?CN#3p1A$C!+S5zRC47GvU(N@Hpy=)$qOfU!^% zH*`}n^It(Z?4N44Kr*cx@E)?Hd2o@yG=|;}cz*|Y8SYHlaAN=AaaIQ7yeo029u@ES zGi-l2pC*7epE7m{@-TK4r$!hbD9hU{i}Y+I_ zO46wimJPIthdb#sAU+Z{hx11ka#E5h;J?O^l##L`{40%?54pFxKRAkM5c7l^o7Hr| zvzoLnh6EZJh~TCT&S7kLH``ROi-9-F1wXe?9QhB+BV0G>tCHtpu>lZy8muIkI~c5n zYb;lfK!SjraS)f*-O2o(zr|AQejMC%m-mfB;mmwSjRlEf@FHN5p@Ymvu zwa9DX{9CBVh0fd>zR>H+sECm_QvVV$CfQ+ZQKhn`(3w^XI%B^@4wz1cb8N%li6W|M zhd+)s@YY6R4?@wGDlm~^94-(ki$pf#aYkV$7uZ#KmMf%~L}_b?GX`>Dg8V@xLXnS; zm2QVxEfHl!;v70)3rvN$vTAS&&n4b$D=$Y*2Z_A`>>?x}y%tB4a)F5sDf!5XFK@Oc zUS&7HtV|FUVoY4=V1a(YDJdcbK15@Z1m8qI2x6=^DS~NglMXD4f*$vV{3_v28TvYj}R0}g$g&3|q5OsdE#bwOtZx}VE zwjatmBz%X=COy#*GTv@u9WaNXZp}^35kZ%*MXwNz=WmB7{q8|_%rFV5wStzz!07kuCR@29&S_ z!1fSp0v2Da?TutK-jL=H!V`^>kZD*x05Yk*OGP~g#kzSL?bik^ERIU=fKiH|rO^h~ zoQgwtIY1F}geQmy9}SW&(C{{fV@z_X+7zX80`X8V4A~G?#sWKGV$C=koF#n*te!x( z33$SM#_3bS;;5%~t#(TESF|8oZ47;jP+kqIPcI3IBSs!nLTpJ zH~Td3g5Y+m6ffk9hXU(^gP2}I={TMu7>vm9;esn9xJY8gZm@)WSj7lF2zkzuLjsK8 zczg#d5%M%b=8J0%AT0Ixd}ThgPe+rQsu3*+Ms7UPusfs^7HCK_fS0K-Oh%9(ok>9Zf3IDV(@m0@wf@($GUBBNzu-{b|hr)Y{Bs zdr^Hj%Hnvk8Fnz7|BY&y7pD(nQ$^yO24TXG88ee81TVy9LLY|EHM~#TI<--mFwxk< z^^u67ZX$bf?%!>z9X=I9DKMcED6bTDkgiN#{%AYPrgc!2Hz zI^SD7`Up~}ghB^^2eXl71&<(!o{?>+DV;`%*N|}q^1y_zM#=C)Y{6zI06eImsjtLM zLRvU~H0wVJ;@( z+ZuBloJQ5$n8`lMf@Um^dS^L(0E~{L8b~?pj)r=z%6pq0WNF%5IHJWtRfy>Tdlbe2 zW>hsPaFy6dF&TpacQ7!*kp(h{OH58`>@3dEz?Ik$YQk{p;N%BprL^GQz(D(W+rkCWW=0Dd8ENa||Bb0}>WRv-PE&)Ot<%&fq!x0T8qUDLm?ZWF zM&T@01_bQn0KJ+2wzB8ZC~>PUAsGq99_=(WXi8wpai+x4ni{iZ@bnWlMj*8%Fl2o= ztc}?X97BEiF&iF>!SN@~dNM!m_>xGOn^I}s?2xG(N(q`BbG(Zsr`dr-bgH00VmhX+ zkM%N4W2XL~njH@~6}(T&qseRbg=>PAM+g8DZO11^m#COr{&S<p{T22WoMj4>ZJ%R}ZdU;bmYJUHu#|Et__Di1P|c30fZi#14-K zoC@Bjy;xC!O`Bekk#!i1 zux4ci!Hw{8{)3lSYIS*KVS0ImyE)Js;T6udydrCb&fS3ubG1di8(AD5^m(u!J9h`Y zADFY3>LypN{y>49%PW7K&qGE|GeoB?59|Pcj};bf_Daap5H8GG3E27fUtz(rJXcs| z%RJ`Gf2@`Vd!NjY*M3edk8*a$%u-8Sd*@-6$I@f)dC2bVw9n%i^8ACm)bfB+!TYp4 zMtaS*4pciMV|h$j=0WGi zj`y{(0~)mE2keO~Mu)dYI#*k)VSyR;Z?v&vwhW$r=7zbOwmd#0t1U8*`SKsD<-u7` zERQeuQ_EwnOf+eqM}rRdhglx~%svmNg7;~8EbyAWxPQ>{NI9iC%cBXmv^=oO40_JP z|N142fi()kufSCE??j+o3V?6*js%40i#Cko-5ZIuyzAiOkYx|NIe`6(vTu^Rbdp@Z z0AKV4Dp`VPbJ^l4t{KHhZq|FXI$Go=j$IaMw~FjBZCJzr3{!k}58K2FQ9$v3=q^(( z67QF%CKmcN-pY2np}J!aFFD4!ixS?K{VK=rq{Y@rvJdO0yQl{4pj#;0GjKNyZ~Gyf zIcpfLS>pQzucxH+t2fx_p4nH5IBv5lv-Df=xVv;gBU5o@hy~RZ92RW8?gYhi-wp%} z+Y#6_nr%M^^@ef>w~jWs;^x{qDy@#1q_>VriKGamjBFhh-v*nnh^0k4b7Kd-=Be|F zy9~jg1Rv`Y+vKqwJ4WaE9i!NV8Dq#0*+dz~i#IAIf$es9&M)Akp58u+c%mxVV7i~l z-1bpqw^Uy}0(O9Do@N8P_cBEJz+^WmcZedAzB!AXp}-H{Pnyov?IA@ZYgweXl0wjX zY#(LIsehx56@#F|{2PvCIsF@upnE#Cqqr@!hdVWPGp{F^g><($x{DGIpnO|8xP25F zTc~gFpboNmVmDL1!G9Z}AcU5=mS|-Uky86Tq@d(RkZuC^JmK9G*+fb&2V21tn7d-B zX1Y2enN6YI>4AjYA8NLiG8cJwWHVEA+~$cOI5x0LmpRZYj)bz~75hEoW}heGVQ-q- z>?vIJ8$`K?kGQ&bF2b7my{7CMxX~S;G7$QVLK5QS7icte^F*@_8QhxMq*2T!RKNce z;~00DnysVKncjQrs17lI*R7+o7fG?x<8#Ks_>lNEw3*D}6p7dqP0YdfJvNa3iESnp z#+pyD;Eu;5o!3gAKGJZUTp zH@Y3Yg~8!YhNTW>hOD57b%7b=7_Ey!`t|&qD?)>C7VE-~*T-&M$mnz|R>aX75ZpEF z-_i*h5#CuT`%D9tM9{?PXk4&UwSE9BjFYqP80A2T*3y|A=0*|kQRHp>7kD>Cr(w)n zVl_O=SZgLDl^t3a$@(L4|!7^VVCO&Lw#aHxWa7JhND0|w^b`|qqP4(RY4DPT3^j=C>>zJt$Z;Lsz(8hL<3W6R zHzjCRAT-B*(RED1w^uOx9>JDKn+}H?6kIa#<_fd})5*PIq6P$Cxx)J?o2ew!A!C$; zRBgr<;qprdQv~t(K8rO$(iTKV^P{6Vfix<%0D0im0zP-p1o;5l7mj>>V9wYV9}*9S z%vS#S6`DS#{i2-P#J(7CKD$z6ZcL_?vJJIuFK3+y?B6GRFAFdoZfC81Lc1DADSC=sg&5)&t{c!0z~9c`$y{mCWe$V8B>7 z;tmZl4KSN`dNBA_EUb`D4@TB~p)}0#`lHPL9bP!WO!~K4Bas2p&(N4m(`zIj5)X#V zSHAqmYGH786ANP-=RtVqYh+S^<*P5kPSx4NSs3iDiPhq(y^vNaw3S{gnWk^V;*gEu z^Yj!ZgyPsiM&fxrDA^{?qcB?WJ|gb;=pe6|9-#dzn~#>`wNkw98r#os9LdqW9gPpy zm^}Gs-iXCZvza$y-$AM8`{O=3yb+5tK5*ojdm|R-cF9J`j&H>N1tpRM}4 z;(G^2M9sY~@M?4JjFA!KSYC*KLvVNx^g|bKshr6Xr?Z>Y<4K&*qT0aW#Bx4@j?;5C z+~FGt)k8R`Q~H$k5uToFZi{)6DPZiwVUVZoyEK}lQc?0(p(SzJ^C=b!0@CGNI%Li| zM9N|ivRl%-hdO#&#M}M^bCl65-o{HV`l>H{8FOF9oayk!fu$JlSKtbMdOnZES0j8F z8VnRlu`H7NB1bD4UnpD~HIkwXagopkddFMD~j0C|lYjvYR(GRHdqQ0uxq-fG#-?6xxoo&E_PAOf|}ha>7_savY93E#mtUHp2E ztX(1U8!Yr82pFdA1D7@$5o&K{@^Fl+L-49JvRH`O&J6HuhX{NQ||4&sab`s@~(fcxVuo@!&n-ksfi`x$TZWL()ymQ%);kfpA0wNym6x?851cQn4~J z1EpV&TM!xlbV&{BcWeh43mI1^fzUFRoNUMH3`eIL{2zz4EAcuYo}N?0EVx4ya%5Qq z*{)(j9*~CvQ4yJ*g1DT4!+0Ivi6yb)IL!~5ckT^3G}Jg}GL9sf<2OX5?qDiN*W)*I zATM|Nhj%2w(R>UJZqq|JZ~y_8lJXeNjuwvl(;?+BSu6u;2u5JIRiuZJ5lmB{A=nZq zi@I)jKzjBv2x~?F&*zzd^D?^c#2;pZ*nB0UUku>-i+|e{J(tD@V~H$u)C7)SFfsJ# zjZ6eg5FHZW04kJBm$9+T6VC5}Ta|^uXQ^fl&FGMZ5*}rtkVJ%b$V^Iy5N}0gDk8;n zCvMpKNO9e^(++sYVncNpeK=L#gd@zlrxpe#q~Y#-F+_G2hI{!{HiFW^5U&=G z>_C6yQ5YyfiVUZPfu@LYC6ecmu(`4H?ktR1SnhY++bRIX4sj(3qxEhiYR0(p2K zTMSze@Zu%T+*pk}F)DCoNd=}~FmSHK9}dT91Z2ZporE-~jg`-s7pNJz2t#M4WY+P_ z4S4P?UEC^3r%jQ|rf_YGvv4rLi7A3b1~g!H zn}THG2y{f7LOK9=^!K(Y$TybyC~XQy^!H;^AhWb3fw54m3k=M(m5oi|UObU?CKi#q zZ?fAIq&(BAO+kLEB=ogovMFfi;~YX~B;;wPYFMx&}XDbPT2AjkAUoFGEEruyV#?tD6Wi@C~_Q{g2j_DCf>m$&3$%iQ1FBc*cZIRj0l55&+!^`D%E~;=Xf2JO;Mp< zi(|7XR{TwE3LJxZ%r?bTDB);q3Rs|A4a{Owd}k)i6)`KqG}6$o-0BFf_+XB&WP?~V zk-4!dV)83}_IG2lQ>jO}*G8NOd|QvuCd$S@$7iVKga7WJx{0!J5Yz5 z2H-p=M8Yu{e%-FO6mtjH9^?E>TXq=tu;;`3g8;?&O980;laRd%Z%c81oF2>-&c{Ld z7zkJ$*+#%e$NV|E-V0UW`JHv)3Cb~=I_W;jZFg`d;U8=v2%!ofM6<5IZ(g7*n<-0O zHH`QIF*++yG#R-dZeBErgbbC)IN_b65(15S=y(_(Au)ll!uifGLN^5597#kI zlj9FuI6*8mIkZXPByjD|AH_jn#;t(|Sv=WGgQF(m)8JNOl#dbdj4S>JdA#Ofp@8JE z{25A_CJdI%9vD&F4#j2^gwbAy#_*7cXgsJA09yik83AC1C*V%D1?ryO`%`zg701IE zJY7TQY-~9p6kjhhkgbFIv@jcl?$NP7BYm06lU_t5mwM!h zls1(j2Xtc3C>o%Fad`%zv=eHBBbr_EQdN6}?4Z!?A4BM(LKD?cC^Hd42A4Wdska&=qIL~>s7EOike z-^`X!=0g23GIHSan|M5D`4?XwV{%cA8+LFYA|o?ZiWdlO)EvuMb==ApaCO$AjLQ+o zXEs#wQKVwifgU77o@x&dg>a1|DT_7@3V_iB!37#D-tqkia)Y7{Ul5XFUZj5dcw@ez z><le-R!0KZO?^Re~(#_3Ni7L?#55f`d$E+vNd<$>E*$9CaMs z>5dI$#WTr~5ZFh+Mz=? zzJoR22diT@{;&d3u`-;0H%TTF<@Nb&zDPrgf;u=faGGeNfcoOV8X4|B#kNJuE!3A> zaRomHvc|_Qj!=VYeXd7A8@BL$C(PiIRZB#%1I{*rs9gn5z4_o z9LJCsN3HUIz_OOzwXz5g+{LLIZX8%K@Wz3|S5);5pI3(vR%#cYcBv#>;%H=yDCMY^ zOiqb4t&3K)xAS-1-Yee9=Z`Ml=bk>gc)xw@=;D2Rr^bWg;K6N?IAs-6j_S z?kRogdcb-*3FoK>!XYbm7R^rC4P#uFhQYqgD$cO4QIVr$UxUOxjD5GS$1qc$oqwJB z!#auYy*c!^xiPo?I>$fS3uz7J82|b`(%FBHV)n?vzq6i6^KUA8F}*!vaB+VM`NtpM zfx`+TToSVm2EWHR(BRPh7fAFGBF{u*KD*7>@$Vg5r5MIA zf7O?~cJmT_GP`JGlX(2d)Q(3ju2;^BCIvB5d)Ynk^&l#r#^jt`KKg7S>TOA0JI2G5 zq-f5%{D60V8?zEmXZcw&235wp3lI*h?nUKYYHU9MPT0Z`5 z#Ety2BSf)diUho-gP@({GW~wBCt6?qA&vGV#PazorFw-ogg+E^#}iUhrv!P00Yzo2 zwB9(I$@KY7+CiR+1{bGr!@Y4Zwkj}yToazq#%u9c23n_e$UXd}G2fNZNUqZ^8*&eR zKN`8PZTJ(B_*F#=3$_Ia%Fh_CX%W&93QB8a-RVe?>g*RyV8Ki2 z!@;DD5FT=0XEJFDG>Y@N;wtSkOAlcX>KbYzGv$&L1{gd?&y@DzGc1^ye-oaBnmNDo$DsMk$um}etGzw#Jz&p;MSBQWGf&cm(pJMWv>E!6&1w?#H@wz@8 znrP&GP39im18%J(@?k&<-~8~}w-vABE&;ET>BM|=PFoS_=X-tn5{;v?p`XExhT(~g z*t%USq2tru@VN(yQ&{$M<5jl%6;+x3cQoO__`&-nC!luB61=CpCC7KX=UBVKjk^q= z-rn`r5oPy@Px_wrve8{1EbG#AMSC;-VrBPHSP0RzK7Z{!bdWBOlQF%$OYOPo>*H$A zn9=U;9%Pnf;layS)PEStS2m-)_QW+OQBh?x-o)jJR5(1jxWvrB%SUl3lp+^0tz9Mc zbLlw-y7U|VDbIZK;gT!w_(4;z$Rl09(cU_A=$77YX)Su}!TLc}1LnR|R~IUO=8BKaee(K0fB)%+ zA9DF$=<@C9%5h+T=0C)xPr7ukI5}s#^nLT!T{`i`4Fk&SKmB6=R|Y=fzbCI+w|nM* z4QYLN{ixpB9&z6T^!thu_np%3OG}oVJL9!4FF7Ukz2$$2RebN1k`??~zRjh(sw8yP zph5py7`yJKDHCd^6c+dGUEI6qqW)9lw)l=deR>!5>*Zh(*%kjI_#>&W`qFirQBuUn z34XfkSE$SFUkP=Y8tHQOX(#1z1ZJ9ZT$s!OxF%NQu*=*xF$_=gA(!~!eO#_Wp_7uh z_caOWWTfu$D5AIett7~bu*tbq7?N&7X#H+eBcRhZ@_*1$S4eAyf*sTz8uIv`M zvRmF4vCBj9Yrr=@C+U}9i`})w8XYF19Ijce=d;9X%M!1vBfm{skOlj+!%lHBm`6z# zY^cM$J{YgsVYisH(;RlO!TjuJn#G;o#x;X%5$NWfGm?@S+SP zSK9yb0%$1o35kDlC>YYi;lTtKI6M*v(7t?fs33qByZD6x{4)-Z1@O;0JXrpL4iEC@ zDu+teG{Di|t2JjIMuLUi5<-g|eMS=LYIy@rZnZR#4 zJe7guYP!P<1M%;1cvB$$_Z(gni2uJGUJ}56B=;wz=QA(vCl2@b{`+SR_v4Pof9dex z2=_aO`?1`||D(gPbFRbZ5r>NrZbDW$JZvytJ_h^@qkKQtwm3ZE;`{mb1We$JU+!>U zu0n^8bhymBxaJ86UWN;juK_vm&*s1f<-o53&hpeGRl1*($2vS2z`x`0dWZWt_ZJRt z2*iKf;fn(CUw3#afS(G(hUIAr;1@c)+2MW;zslil0ld=Tp}Ruz@p_)=@QB0xJiWl- zNr(G6{ZWT63dG;(@WQ)QzK?%KSJ%D{_jCEh4zCa3p;1%5K4SdDF_q&+R8FlKH{$DK zDo0dJoOEmN(YZoKOfJqHRFpfYU+$njxr6$KdJh@YkN^7fU*A6bUo2=}`Clx7eJ>&2 zr%2*_M*d$ah+b>j%ZT)oIK_HhEQrYLBNF;)ej-yMYJQ9+;UZ2Xi-2MkCSr;;szfW6 zc*PR2STgS;ar$VXG(*KD!#*Odk5)x1A=mvxPCqT07Fi_pleCwJ%u7VtCAl?UHR`6x zxfMM2<_Wn2zm_W~GWG0OZn~|uaniy!@XU{L=>imBBAE_`?RrS(9lwYYomh*vo&`;6(=CZt!A*ziDu~@VuN0 zok7WFD>3*r1|MkfUmE;MgRgRi72Efd2LG6Q5F*~g;6okGd@ncn^c?sPbKt+ofj^Q1 z-;e_z?+hrG)AIR#XNVDxnsV+kxb2UZI>Vjudz$!ToZ(E|^6k3@x4c?maNCbI8Qk`x zod&o4sNLW(Q=WV##)svx{pfsy+kSM3!Ko_GpD!5P_OI&=Zu`+_gWLXco53$K^6xdc z<^PWjZh5}M;I`kT4Bp$wZ#1~=M@XBby)U z+4hU?8@va?eEj$v2?>{QA;G~Q%&t`-7 zHTWwAw|x7H!7blTAnEa8zLsxi8r<@&r@<}XE;Bfr!k6<(gHwebA7gM{dVG?>ZNIzI z;Fh2F8Qk*fM+UdNU0`s_x8EDw@_CuTEf1eGc(Ez}^9Hy4eAD2hfUj>?=SZUdyBmD% z$8m!Xam&vWsT6#O_rRZ*)7{{fpS|g5#fR}N4@Vf>^6*xJTOJnDIKzi>EDr}6-16{$ z8Qk)4q`@r@YYc9Am^8TMVZFgE4}WHG%fm$mw>(^La5jmZfhE4>;l&2GJiOfCmWN+3xaHx^2Ddz%VsOjD(jFoCu>6*X zNrUs!>+QP+w>(@;^5H`{mWNvmZh5%d;FgE~NiyL>IhKbH8Qk*lQG;6^K5KBx!&eP% zd3eC!mWQV>F+MDh<>4m{Zh3g2!7UGm7~Jyk-wbYfc(cJR52qO1@^HGrEf4DrZh81) zgIgZ{+~Agne=xY^;Yx#B9&R4-aTOK}4rQ<_ATORH=cn^em9(JKp@L_z*!xAb5 zAL5pW*BadN@D_tx9(HHt@u3{c!%Gcrc{s@6mWM+P&Z_(Jk1@FAVXeU}59L|++}df!$StQJUoS@!H4>^Jp7o!Ef3E(I7NEB6&swF z9>2oimWMYO-0~1REab!TSRVFgqvAu{^03z6JrL&ez1!fHhY$9nl*4)WguyKjw;9~> z@VjoIh~=?7{Efja4<9nP<>5MmTOPh>aLdCt3~qV&uE8x2&vXkBET`q+xdzWinAcm0 z!7UHJWN^#F8w_rFIM(2nhqoHs^03a}mWSUrxaHxG4Q_e(3xktNzFtcVZh5%T;FgCk z8{G2ncb|n&e1bfD#^9ER+YD}bct4$APLAbUi^1(Uc@>=kF23bqTZzDjjfWo@n87U% z&oj8?VUfWt4@(Sgd01|6%fpcdw>-Sn;FgC;gIgZ{)Zmtfiwtgg_>jRZ4<9qQ<>3~C zTOMvVxaHwqgIgZ98{G2n^eZ#^w>&({;FgD%8r<@5u)!@4&n(Tz|0FIw&#y3e4}-Sl;FgE=2DdzHFnBMM?;?X+9;OU_ zvWfqU!7UG)4Q_dO+!r$SvOJ6!-16`f2Dd!?l))_zFEhC1;pYu*d3deCEe}T+-12a| z!7UGOH@M~D4-Ia4_}>P%Je)ElqaVw||1h}a;m-_i`8MjBj2z3i|6_2=+mpYPiEnxM z+b?Hu%fpQZw>;cxaLdCz2Ddys?kgGjy>RLK-5CbI(BM4`Zh3f_!7UH3GO4HfvXc{%U83~qUN(BPJbU9Zo?w>-Se;FgCI4Q_dO{=a49aBqp{$yW^C z&EUfgZh1J-;FgC;gIgZf8{G1+!QiKvd>0y=u1a6dKN-Bh;Oh+j5rgkAc*Nj)4Q_eZ z3Vew><1?aLdC12Dd!?qQNZ>hZ)@VyU_;!IO^c_Hqqc` z8~jd#pJnjAH{u2#>cNhmBMokOJ9M~2m-v>qHyYgX_6G*HylphN9=hqwD@_ekpEziGUaBC0z$l%r<_?5vwWy-nK;Fh-=3~qV*lEE!+ zcN*OC_JF}Hukyxa^lABgmci{j9y7R|$FDND<^S#p8Tsd$`qtHC@RJR`@@v`g(B$lR zNo{t#Zc27M_4Vxd?pw3trMG3ryWFmLS9}e^pYLDKIs7u?**gG@0ZI?@v8QkW3&RrS&cwGAW_BD7vgAX$JB?iCN z;GZ%0XoFvB@W}?JtIX&7ZG&HC@Yx3cSA+k=;GZ@4QiHQzoO}4$5Wt<88hSZ^kJ0F% zy#btKF+a!Ot>0{S$}iR1&_@C|b;r*I0o>=?H-LLN0|U4(k4jVfYjMmoHEAbzvMIfnD&<3H_ij@A5lJnwG3FVwFzevZSV0X&o= zzKa&}diC;sxyu4_%H4fM03Ye@=LhhbqykF?m%`iafj`~qbH6ZUpWDyCQPiY?2T9XE}C3BYVCJ^5hIXMazds4ML6{@6x7y)V}_z?eU6Ci`h{`Iq~G zxNT$9{Zq{5Q=ne{JK9ap^6_>tNiw zfSrWDp#G~}`a*=+k1w~!{sR}38B9OZrEd;o7)(D0X|uIo%Y&MOPtW$_^NCOI+b>(* zOj-SH{ttzkSvh>zUR?4Crl-vm%iokz4qE`uf6u#Yp=cbK4OE1Fv#!o^M*qf7Zztcn;s=k+y5RCBoBmRC z`Q@FFrqw%sHK+fsqfWo!w_lv|VWfHE=)ZqdIQ#qWHUD(TzrK*T`u=hK+vg;VbUQ``Zt0pYv}|{rLTiQ-9a-qv78!n!odt`bQrv zx^(l&y>IVLtY16f*6kZ#sz2J-a>chMPVc^V!~WB9zP0xE-`Mik9~^zF@5fU&zW>?F z&s{s|(+NGr53RWB@n1gm#;~89(6+NZ%Q!FKy6)(Oqi5DU(e~?iA1i!g*x!qPv83>p zM-%TEQtlyT=8i6FO~#H-+tS))??Om6fJa+z1_Cvgbeon*yIew6wH!x5JgKxv2 z9SnY59Q^$lB!l69h&l&@kHx?o41Q@G`W)ZhLHK_b19~tz+v32_iqkG{9QnKxhfZG{ zKAYmScj2(X`3%H?hvU#MNFJQd-Z=9AMx6F0#i8@}IB?q3!Q@{O2mfbr;OEAnlO3nM z*T(6WOF`!hd}BYi#HsfQAP%M{^WxN-9fwY49DVph96ICT$bV%V{5^5{@tL86^Z#6& zcHIz%&%QYNc5NK|4RP=n#=(Cd{G`Tq3eu;}6Q zTt8gFmwuqqs7^k!MQ+Eryq~JHHP7BI^&V$WfSiZkA|Z;@~HTq zInI^qS8)0?{9LK&EYfr=`Mjn1so$*d&QllHoie_Ta}`%8z_1o|{gucca5@n9{0Mmb ztkCkGujOyad4;C$()8&&@Kd4r@3>y2%P=1Jsnq>4U-N0n;WW{I)T{9={(q(GZHcM( z4o%;y@Y0xm{DtOoehi;K({k9Y z@ooM!pNlmAR(pT1AGJ^WAt{k=HK;Q zMfeQJi=W#y{~a;>zozLg)%30Lc$(IeTrFo*C!Yx#Uaa9(|L#Msv`Y)PMC*B_Zm;KB zRX#&qTn(0dwA?KEk7<4u#k6<1ZdbSF-|F`bXdmk>)_QC8``wz)`WQa%)O;=-rx>xu z`+iMlsm8bH-=+C!iQ(sE&3|qT|6BBU&(`=z%I8h3Z`nC2ZOQF&4R6tUYuTlr1C8zK z)_gvzin(?tD)>@uS1;8awpPp8rS-uYzxQc)u9pAxx?jGl$Kz5hH>=(oG<>&)Tl#RH z)^nHU(~|Q>jbE?(#p=hOYItsiVjN%(leK(g@rbG|c`nfWEZVI2xmn9$w52DyUQ0he*X`}r^ez4$*8SBH zqgTgkcxep(T<6O8)g1yf^$7;2�>nHipX@!oi>`SU7o7up-nDnqJ))4mC`kG_j_x zHZ-~H#+s1I8$2gi-dq-}tgbDqslGkrDr>qmIHRs2R1+*KFRQ3)43;;9%EF;wd0Bm9 z69~;HtEs6gKNPL9ArwVAgh*3;1&M^KXN16RsJ_uvS6&~it*H-Iga1(2PL|g-hIRb} zH-^!k23IgxS>I4y8?FqNSKWMQ-I_%d2-eq>)w*VcX0UKDSk~AWf@mtss%s8cs;NF$ z*;HE|uCA+XbcLEBI|#!D1?%d=jbt(yu7h|h%bIG!4r-z1`kLUa6`^2dUBhkh;K9cF zvWCX$@JzHfE>7*?GHtQZ)&`k-X!C3cSS*`?j&3ZgsMg&c%PSOv za)|?u@^NS@bxCj$KX0%$TIFm34$?#mN3}4je72CvDsGBpnLP&Zp!`^QgYjeK9j>m! z$-yc=pglp3;qsfQ-CwG#mI?-?yFyUbMn~K9poO+Vsnz(h$5zG6+Oiqd<&Dwt6^!Y7 zc5S&D;MVvo$CwB=lwt5W$_-kfRL9=2isD|zcCq`lfKmF5&UAFi_lPasH$3iQrXIRG0E-f&Qe3*dX ztr)?F=1i}tyRoe1P`E>GgJm{G#nf0=b8DzUSs7)?>;ld#S~RN#G4Nm+1RXA$9+e|S zA43acb|}s17n^vB&Czs%L^_U@odw7(Ml&+KNzZd39)z%B4jE#e8T3Kpuk&?VnK=Zm2E~ zHA)?oW#!?z22P=NFMSE#A(G)78ZGTmi;57&VBJhR4`PS~vBzU!x_@H9qSdiLJt$(q zoKJ1KwzaTk%UJuN=MHJdA+3|)Og#-phtc%G6r{YVFm7s#6c3@gwi+H*{Csib4wGMA6)L}ZpnM1L*$}D;l?@n`1G3S8hEQcZ<|;ym z&lTtEFdhUZxX_nnABJ~M_`w@zh6k7i+l!!shv^yJ5F>le>5|!4J^hf5iDury?u%PC zZpYaK;`N-35U=ZOgm`^tBgE-EJAd%rv$F^9K1X)E{We-^%*e^(`9yc zoE~Goqr+;dZ&aS$=*Bu15;Jfe{cYZrtGmmJ3NM)$Jb(20qsO|e`wQ*63r2GXdWb7Q z{td-Gxw=ab*bqruxe3@Uw{lfJQ_);2Zzy)z)xFB&UuQ`R@2h=P;R5pb4uS{PK3CI8 z)rf!$b@l0h0$XP4e3lqdU5$uJlIsf%3!g~*L|3v~(NN`w*yXKrBvxE)zK&Ov>T@C% zyx0mXov#937Tl%N#(4^ku#LU~07AY6t#C2V92;CAY@`|F#3)bilJ6@ZAphB@XyL2Yk8% zo}R7rgmJ`walpL}xWZ7GnGX062Y$8#euo1-&H+b=&;H~(;0S@*AHxB+Vvo!$cEG9A z*2nLFTd`2)l{(<|*Z{oB0k?(_@#`IMEA~iuvjdKhru}Jgz_ImUf95*iR&1G>^Br() zDx}Op2OK8B{w#99tyn8FmpI_g{nMomxO4w{xdZ+Un>zH?0k`%DnBC=or#bMu9q?~D z;2RxqzXRUmfLpO&W}6Opx&wc=177cd?{mOUa==|1D@=Fk95F24*01Kc)A1b zb-=w2_zeztrUM>yz_T6j(;e_}4)_@kc&-C}ivw;r;AcAE#SXa70rxxL*4`hpOC9hG z2Y!_UewG7X?|`4}fHynfwGMcT18(i{F?+59p6S4!?|_eTz!y5;-*UhgIpF6w;7c6v zW(Rz!1AeXpzT5%Na=<$r@X-!$A}TA7cS=?Qy{M)`jGn4*2a3 z{M`=t`40F#2i)4ja%6vf`?UnVmcZ8%_{vCNkNed9#_a9MMqASOejGz??FbL)?>1(y zNnR~H_K$rJ;J*Hh_i=Hb<|2L-bNYJU?eFiOFKJHgeZ4Q)X*!yHy{qgr9m&4lzu0Ly zkbS+svD0)M`+EP)PSauR>%GTL(^2f}y~R$`LG0@-v(s3iA$_%-rbF1*n`fu#2=?`k zvD0(_`+85e({%j$dQ`!9)A)G#xv%-%it^L;LMC9XYh$ zPSb%y`|UIxH?-eQ(_usV?KB-SwBJtC5kvd!G#7Mezn!Mzh4$NNI$UVKo#uiL?YGl( zywH9-O@|BZx6`Ld`lEwZ`@NEW*G|)cLi_DB9VfKkPSY_$`|UIxBDCL5)8Rq;?KB-7 zwBJtC!9n}&G#wSR-%is(LHq4A9TT+QPSYVl`|UIx4z%A+)6qct?KB+>wBJtCu|WTS zZnd8aIP|}rrXzv&+i5xkXuqANBY^hXX@+djemgx@(*I_sxo|}L?KBs9Xuq8vC+RXf zO-B;#x6|K|be^65uB6A4##{zl%x#GA8}wm~?wgx-}+!S4_GoCOso29g0cc5R;w~lb#fl zE{I9z#H7c@q_bku88PXRG3m6JbV^J*DJFez|3H2CBqqHhCcQZ({dP?H)tL12G3m82 z=_g{+kH(}Qib?-2CjHBp^p9iG?J?=rnDkvS>86R_z;VB&;)D`XW361adTi6|IX?3^=z z=`tdl!^4fXoF4*iMh^7%n-!Q`*Cn+gop1xn+V!)1?`_D`{4l+e4o7bT%vGiM8}wJq zGhv;_bcmx;SvlH$+8p2@Y(xCKsd?AtMfT=R$(uao8s*$vmYLs{J1%eh?xtIfwz}-1 zw&~-F+Rkg;JL`K!+(CA+IFwhP-KcK{PMd1)0s zwB3=J7ujCac1LpIst<---kx}N;rPzRqp!e?F}|~5a#7^HF?-F;pY`|4T~Vap+=xV8 zC!IFe?1Ks6(L zlez6zO3#e)bwzEr9m00_XwAU31$j>-f|1?}phHG`O38CB*W(M=u=cV0x_HSs_G$Fi z$KZYQ~lnaKf_fpX2%})tdy3IIlsJ?a#nSp&X}$V9`4bZ#4lafALc zD&2MYJ}>?j`8-7lyP&eTAK@#!XKgpy;ZD6Q+nstlL|-9m~t83UX3IPA>GB?|~FJ@spDjyZ^wCAGMbVKNSvs z&W-XD1V7WkPhGK@;@~GI9hKPpq^$3w7;+wwOUk*zz@0uXSj;(F62pDSeudei?(})g z?VQ4|-ivWiQAPQ=#v3S6bY zzp=pkkOuZrT+HE`jaT+sY#3x?Y%Ck({zQxWooFDWwh5P91Akgr5wGNfS2AM$1ubI> zXWFE>ZZqEicwh^aY@S>rjU>%XO}n17ma|Yu)5@}GB@_HZfb*#_iXfzwCpQ0L7|*oe10vM*NIh3y5HX*U^>M4#CQ zCW|7Untud~6v_X>RWZHsBSJK7&7+onWD|Lxim}JGOpq}M4#7ZA2dCy-4_YGR?-%mB z%qvKi0V?wzkX4rPDAj26fpTG_(`Yx+E8QbHFb*o+ znMu2V%{qv&4fiT5*PC};-u3xyHJDafcesD}I*1vOB40Wjfg<#prwE$owbla^Zp18- z`JzAUO9$)cf^{W3^KEczXCvzbJF6&ik?Q`0=w@4<|fPH}%%gxhakfa}UcG~2A+ZOMgF=ybu>vF+wPAjVc7jh4>7 z)0dn8Ju%vIAzq9yOb17~=d=NpO-z<1<~@;swDv2V?aZ4BX2*b8&E}`5+mB{=R5M0O zGxQjvHGK{F*HJi16exxA2cFahV3YYb?Cf1Mq z2z=3xMEj#&a_<;m&W8>>2HnIY44vOw)Si`&YS_YTqaA-**U!4ph``S&h{ZL^bUuN2aYW1wKpdi<^9I0twYVByOd^KAtRwvYn9U2TraM0 znsRgTEylML-+GMh7Rb{cvq{wG< zKH8HeKH;#Rd9&9g6wdBUHg5pm1>--tYoi#QOoP0$0m>orm??^Dba?L?SnA68RH1!4 zIX?T%-MA_hItzDdXsLnJY37441lC&(w@=nsV?J)<$^>vS5T$YtEDKO#CV<{d zED&_b7L-(|y**$CtC2rLa~}U!_HO&wn^y3WbNypb>B=2&Hg5s%di^TAVG(ONmFZlE z&}&pKxr&g6AZ5vOT4U^@~QB z(@|FIx5j|}o@eKa#!Ljxbr_+jtLd+@sE%y1sO6(`M<}h|`*$?w1mHq{GY9JLEoh{@ zemB~t!VUfxqwVf=bKLg+{??ABVR|_)^E?FfGDu;0d>wMt)bGVzjK6))_H;@U5*UYJ zJ=uWO@H?FEIj{mGGW&}W;Tzy$H~BGsKH9={c;506)Wr6*A#ku9tMQw+aXhMmj-}mN zm}KLgHLPtpzK5*SIiAP0mG~yZI_Dza1+$Qi{*Sy<7`ZePYZCK(qzc<`KYM%b9(Nta zNum!|12X{DZMZA;x>F}}fm7$l*eda5n^&R1kYg@LU{YJn()n%mX?hc&g`m+ViupaOg@fC$*mv!A)WNg%M0qg{!t6WvqHP(TH5=YY#u< znw0S3r1r$4jq(=~_7=8Z6lovsTGcbeD1Vv~O(sN!YWqbyq)DHR>P22*`*4775;@~)43kXHOhAVPu=~y5=l%0!gq8YTZfZ{YBU>G{-kn1Uv-A)N(;VQ%EdXust1PGPJlvcNZ&aH$&NNc)@!xH(>K z&^HN4jF1G^jHP*PEtD8yMN78q%4cs;Hnyz4_i&iVIS~7+&``Dg^vAEG^fW zbCCnnq>LMK|Abt#QPAf}`|X?s%n1nkyEgFq1mU_eT#8|0uyfWr z$`$C`bUU}}(J0lkb*{(G{h^~Q+kaJi;(2yvlgiASy#|x^+Cy=ok{h8vhd{z`0?h_9|%C;llh3$ z33tit-OQ4hu0XCZQai4&ZC19q4B3Tk)8IwU^F4sU4ds4f8K;v=y%<$8Z;8@(&XH_j zxboXd$C=yTXH$~0pq+|466jF`7XF36Ig&J21F^8}Rxk4xF<%By1?m7v0{Ah1>A+eH zfWj(KSOoB6-li(u2(7V5nt+4nNx)j?G0z4TxOh+{T?MTl zxPN#u%s$+wqV{D1ho?OXrRJj}Cq;JK)@L|g*TjdQjU^hzeqX{qx0<%Ft^m#D; zyWZ37UpL3c2&Gk)_b7Y4s(cP}V3!V+-(0Ia6lu5`Vbmok%084pubvAE>{2Gp+uw!8 zu^Z?4Rxq2GX{Q1M%oD(mIVFnpbEJVWOla%`iv+mL3oMM3&;M#s*^fK(Br9W{uY*N} z{!Zk8J^}O`%ujl3Cf3+UuP7t}INh4vQKT*#>F)}O0P2bPrzjG(Ds*+fRY(NzW8ND@ z>PA}e6!^37L;yc#Srq9PNTbcQAOl>qnE;nLff#K`b43&)K{5)&QiVtv6JmZdO6nDo z5*Y*(Qv{q(Y9DQ~Ww6mk%2h}Nhzy=161wUw!N9;|Z;et&go!l&H<94^V7sLe-#fC+ z&BCclV3Q%)d_{89fxZk3E{_Erw)5JLK@H^JvcY&ge1IJLutu<-qcZckM47y&$ z#$$MC=0!GQ0xyd6O~Eqs zcMrjcAfOW2YIdNIorkP;JIjb5hWDG8>=z*iRZ_CQACvva!0hsv?4J(I&PTQ|wUZpQ zr(7>}_iI9;?+mGvq#v+z@Erg1N zUV6;7alSf_LxQbOH=Ttbz_`N5OGeuX2%xpyjVNN$KG*=dyiFjWm(P)Br2ob|jdc6i zHGjlNJ>hMu_uhVMK=0Z6=?G@5Cwb5&<-Fp;$kUS|-3-*NGc!?0h^McX_4}BP)%JcL z^~JH4@7+GI0=3?cuJwCY43dv+dLwf18WrQX+Y4PW*HR#Dce%_5)J?Lv2L%D~m>;T} zbTgHsyX~%V<`OgyH`(TWDl606i<{OrXI)~HuO9QJ(LN%#sNFXo``L+zpWS5`37gCj zs*Ioa$?Coo_r^V|X-~~hK%j8^TTR`-FE#(B=o;pq)s5f$9d524(}6vH=VqYZA9U>3 z-?N=ho*A=;`w%^dd%ql35PfwKbZS8GVhQXl1dokrgh`C{$-WWh=WnvDcS!0Klu1Py zbE^Z0oUua*Tn`|^pr?4;_pL{htU_Moe~3ZN#dNay5RhOyM!;&a;9JNL?U~(?FuvP; zWjDg=gKQP)yt|1weM&uBSjthHmdrE6Z^I2TKU>D)s zZ({MHbg@I6+k~BFJ18-Rbjn@tpt=+&r$#8mV!^CK?xgYCoBl|n%*NXRn5GM1iFk)aHzqwUCY|X_SGp%Bg&}*ugdge~ zg0~VaG$Nl|Lxs6#`$DjTA(hM{y+8UJq&)Q(L4F$kGXAPG10Q$K%P@0Ua+6}lf zy!ay#%@Q9rwgMw3L6ZRDCRc?IyEuW2i2`BBN}`Mb28FG&k#VmIZ^0MB)|E-ViGzAImK-Htk(p`W1gmQ zD&4z25IjHel8{?UIYH9_t?)IQdpDA~8LrBFcV>ryO_VvK;miRAq5#3#=Wtu$6S2P2 zm+$sqYk-#`5#jULqj@6JxyulxikCw3fzGY zA;Z@}y~sBXLoDzD#QZAgttP_jguH>&Cydr3r^c5F5s&p_m?8BA>UarPgvrR`^Td7{ zFhwp`lk>kpQZF;L9%SAG!e>&SK@d8ZG*Er1N`lRi1{egKQoIu#pYIO5#mvvxhLj7W zuScvW^DhGHMb#AKA0X&m{&|ORDP7EhU!b7>U9`i-^$WB6m1I!hc~2>_`KL2dOAOBN8_HU+icCqYsMcmK) ztt9(7q>n9IG}8+~B9MUkc7XX$!E8?IAE?!67D|4^KYM_Uqd1vkx5CmOXE%X8DDpw2 zyF`lYR2lnY(x1ho4_4|bh;Ou=3|gOCpaY7;0e5N>>c|}Xrb>^TsJPh;+@!PRUR9~` zF>Sd6s5X#xjwA0yDO>Us;9wz;a+)J=waQxsIM@kfeM9n8%l{^M{qFjC<=_>HkpRVa}A9HVuNVm zQv%F2|N63&Q2EGWUtv#;7mOz$K7%jCk0IlkOuG{3!gn1yK{_+AlXoxj&pLD=_CIBC zXX9@T@BLl6G7L4d;1#J1$C`u~S>gz!9-+XrjkuFS)4EYbiP#DG8$h%eTCf@9Fx{f- zVHr}Vg>3RPe?7q$Xp9AE5>g3L4cKELfx)_er4wC3MK zD_|#cK~8S!t(#=)CfmBMA7Y&2lOM7qGA2WUC%6t>+|HY=AE|IJ+ zV*JI}{4Yx{=X#1Rr}l?sR(7Nztq$k>h$U0{@305lpAbG3_ini#54iK&XdPI@(`XB7w{AbEg{A}Ah6xkV8z z@)g5JNjXzmKeZPV=tn$*nJpAc@i*-Aqo53TdMca(+@l`!m7>4Qto3vZ&?pQ?8EU|v zqb!|vT@1w}8z}YCTY>Wi2CUYAAH)3MB{4X-1p}&f6-Y%nagu;lZ$7E8Qr3Z4P=LLV z`b?;U;Gq@>X^~2%EU~cI=y|>+0GoG$0vJ~1b^~I0Muzkd{0omFV6lPPRq=hA>U9=X zRoPNfJxf!iW(h|UXVSiuLF0_w4P+G2PxL$}QB$djJfS8Nc@O$LkSG#e(iCzyPquT- zu5N~sPw@$5?#RmAawAm;#vf8;FMonkdNu*D!e<~&rxoV@B{fxuOC~-RY;!T)lDC*q z12@c_`56)4A@F@QqsYXVJ`aam2O& zPWS5#VXB9c1NoN$d4*NHhisXtn1HGGZv$Ql)C@@QssgW5$L!+LyU1D3;`oA$M{O<< zpZ*`{fvskg`oOA3AuZB1Z?cO~;KZQ|rf{ePtP@`)Z{%zuWALZZ2;f3lr6p`Rk`yOY z&yVWfL>fFnM2jVmXgf~bN^DdP!qy?_e~R6-0VPVh>Da9ytdkWTBvo;vy6LV74E_jw zNKoIWC{Z^^38yr)e@e&GkECaXk2*mbIJO!a+S>btGez6sidFCy{bax zm)OqJ&mDOetNS&o`oPE^I`Ym}_qHSVeMu`8&QkZV=*p41)z0%j<4$daLw1FC5|=H% zs}f0h3dAGJlsLpPpBGni{c~c0=J^Pfu}bIMGie+eMLRm{1@pXTw2>+X4044Du99Io zU+K_CNPrr9gB~5p0uibP8(Iu)+DHw+SV?`r*MSnq+lJ2Qr8VtF+I&&b7i5?rMHTjx zeiM)(aV(VF)Ar$Og31bC7cy}3pf6Kdh42g+8yy5gDaAg*DrksKN|5C&C0sAn~pJ7JuIs~L4H^VJJlVC>3Y>SlH;0M8*U z^L42N*71JmyqY=Z_~t9<9A68Cz$K0>#AV`}=W9{9DNTwZb~prps+Z#i?ZMOo6D+f! z2zIH;U*?-DH=F>?u1=Xl&|-S6=pS~JwQ{ba;eOD!$U@IFe+G0x%eG*-W~&7oXTdz? zEfy@-g5ms^1v4yIx>*QVA;VB;v{ez9C0v}O{YUt5JiRMzC(3sZ=yc4Dnmjz1Pu!me!=bZh3boyeSk{T=}bud#+ z7qaXKJWGT34(ZT#1~!9A;2^rFn{a5Be}VLb+^D|suLhL|d_7k6=%hXr^=}1Otugk} zYT}fsd(<+KJ4p%a^R%J`J_WrsFuGF@(DH8OpH-|G$NVtgsm1p*a6TJY}P=Eh!AiY3@wQT%$+H{c5cnF() zxX!T#2VEyHA(J(Z4xtdt7P7lVodWR1So{^h^&G0O-GK#4Sx_6S0&y@!WTNeU;w|vC zuocAONJb7u8K8?SXouXOSrogXy+HggDKnpyOAkiH1;GN5{>1{>l4U-fM0y7lh^kh6 zFyw#{_i6Adq*U2(fe@`fQ}?-3=>p_@U!lP*3XI&K zh#;jH%%Oa@2oWk2KsQbuJSh0QKI9H3r-r z7^`Ut3osQYe*sdXo8OHuOoT+S+t;+#^VeDwJM(7sl>zqvpf01-VPo(FCg z$Wj!7Ko4YY1U+?b0mR<{pa<7E+5>_~g<=5+myFaI!DG0rdh47gWBDi_MjyzEut!jh{&%}{(_h(4S;P?LS;)KGu14}jKV_=m`{^}0^QXuQWO&%N-`XQ zi{-WkKIAaKtV|#pL`>YJ9k^d^WJ>ak{Ddi)B%yC2AB<}D3O58&7yI@}V*`7@J37+C z2ajN(DWToW!DvIuza3en21>w<#UIAVVFSIqaB&vc&IzA$ECc`^QZDR z4sXIkL~WNeH-1`7)VG{{1_A73v*>`pj|TGd{Id%r0()?Wg*=e4z9%v>2)I5<5sZ z1I_vFsdSCFhCx1|70?j?Rw%CmcZ9OWkM5HD2)L|H}hZPAH6>BQ5p!v z15D4;>=Q*UnNYS15|hbuncay82xW8v7*gQ^<@BMFw^{gQ{$Zp9`IH`T>j`?DfK3Ff zP+lKk+ynBz!=C$8W#p0r!1@U52F$-q^*4&qc|)0l$VVuj1zCpt0U(p=7Zen>tT4bt zUD1ELAq71;;FKb3g>MmCPQhWg>?er@q9;H^j0Q!QsC;hMK-;ZSyA_nd3E+V-m~0R; z$gn0R-i-Fa$?0p*>RN_PxWR{dYSb!WnXTGkH7GG&(Sz)@3dR;8ym~gDQ4+8tP99oI z-GSf6>Pt9C_acBd&(JF%|2$Js#doGJTFsv31;(Ccn`>~2(Hjwv=u28{+5-Zlm z67=B}Bl`g8X-^IXV8O9`hgKrz733_I&>Vp9)cuQ<=0p0MkW#LyM==7+jU`Q&By_=a zm4nggf1d(lC~C1@h)UtKmF>b;0Dd|5&PK=hQ4NL(0WoC_hSf#Mo1HGiH6(WkE)M96;%b1`# zxdS=B7*Hlhmtlk`W4@h5<$60XO0^VGM(4C)2jCgXb4OF%z9VO)ZI-TZ7VQ zx&&3BB``cl(LEN22Q(1ihqP(oKxl_y8{>8p} zk_JC?K#&bzYpzr_?QJM6w}=BMh)rp6)Rc?jw;YYxobyGdKc!63Zrj&bxJXM>xf^pi zM#*T@N8Mc$;*Ly}$8~I34%0EF>=Gdt{4H;>!GpoDO*+9c?#dQrxX1V*4G7 z=BuV)Dn$i{b3NF}wz~i+K_GEF4vIot2l%6K4#Wvq%6d`5t0WV37?6U4;r1*MBQBWp zfyn`;9Sz)xAE8_rP92>5pjjP?am&E~J6W|qkV-It+l~ZrO?nTqKIGC&JRR+M6rccE z=pmZJ$rwvQ)_e~e2}SvT?rI#1rzF6}*RFSSD3LpvC-N#bw73J>CCfWQvu zu74x{yvvbCr$q0%{I_s5CbvRcFgg$8YJ7GtXJh4RWC0!4De7wQElQY?Y*$04ZC695 zZC7K(9O7H9#{FVEF=1zr4UF9+AL$*|+xUwaW zqZ^VF$-X^Fi7@JUm|CdX^Wfk|S2;b89%4Ad9l5imd;YIHkKrr}p)1ehD4}%Zo`*Iu zaXgQWswsnb9<)^S#u2FCe2qVdeBh7M>FAH~p_%pu&o3R`^SFB!#<=!89sxQ`RMhkMD*+DAL#J)e zL#J)e;}XHQJr6OSVB=3@12!h*1 zmZ;~U=5*;9=WYuC>U=p$f5=+X7O*64M&7S(;D9h_CEmahhCK|$O&sULt6CHI9JLG# zdtiv*3>!ugH-G*~&tsgb<-gAJD48j8bndoX4bWhoM;=kX_B_5Ko(C0BZEuK&3q90l zFF5X?v(bGPY`7uU+FL=-Vn+qz#NLsSeHhqw$Ic3}yWm;;7jLhG)%Hqs=a8p4Fd7kQ z8*F<;_6l{l0}bYGi;8Xx68K=ugX36-I~e_dYOhd-T)F%6|MT`rX&4%Cc+Vs0R?f^S z<}n`V13Zs$1UNhoowhv>owhxXu14b9o`+aYnCLNN1135toAjWrH~}7Khh;A*K`SU$2?x6o`0od z9$Ezdq~}p;Nwzmn=~dM8Xx2j2p2u7y(N#{*qn;Q}&!eb8x+Us)P*acah(|ihLg*^y zalBAE@|eeb#Y!Bvqe?a9|9#A(MOE})=XtzWucYXRdAtVDV4g=OQNQ*)z9OCnN6HFy zSeIT0r_ulM2Rk4Lr~82!Y7nObCgZ>PZp)UNF;EZhdEnt2+w)inbk6Pe!Hy>haCja% zZF?R%ZF?S93cl@mi1EbC{Wr2f&*R^jf7qDEoy36Wq0V-^pzxLFaSs!==kc;iX)mHl zAu7+K%USAXCUne0w@;TT(`Cf-SjL1nG8jJAZi~!g;D0JIqn^jr3iZok9-Q^W^SJs# zv6c%L%0y!y@>rw{{vn>nf9IHo7QsL1c`UIcd)_#uS5eQSLkm@V9^FXN^S~)H$T=B* z=PxG)_9zJcfJ&M25>KYmBTNVUU8<)(MXDaOAx_$(>hx;h8Ky=)tW%6}$hHTb9KiWS zIXB5uIxXD4fTpSksALObx2_hexE@s2vQed#*I`P+&o>`fVHG)Jx@ajgV5a!&9*&85 zQJ=&=)G1R`frIGcDTC>4Ov-UPobup;5)V20d5RJn8~YR;Pgat&6-OtjeK@kSi(=pj zx(DTW22xW$Lt9p;!-$fHntKV~CvfF0d2*Iji5T}e&Xod3A68|R&eNq!x>!G0+C%B8 z9UL;YSe*n#3$`L->elF?(U6eGrXr(G&hs3U+aO0rt&Wa&bX0mBlrK>;r71DhfC5pUwe=mXY?QJlieWKNMBqV(gz8%2dr zeo8wqC5eN|3W+_ap5Q?nWIF$dC3o`AtB?$G4c!yH93jHBb%qow z`6`qSZzDlrK6nOvMmg9EmICusEXC|)X7o^~74$$so)2{#EoCk8?8ruz71?&4j`a74xw7-PhrP+%G7amR0fkecbYb*9OZxl7a;cNs4_8u zv{4}rSryy30q8I$Q4gbp0EdU6)3%49)3%4P{YK*39>!8(+mkkhY(UyclSvQi6F;_b z5;4&6Dz;Ih@Rf&g8WZ47o#(hkrL?#34TY#Yj7n#zqnXeihHjrO(@$ohAmU+s&n71p zTaLcoue!;T_6aaWjA%q4V;9SPd2EAyCLTuTIqW>o!X70J>pX|HK>wpW3@w6x(!-#M z6UnxZR(cinFtl0F53bBjA0l1l^f0t_b9fkaWzs+KJPhiqn5DEUi3km#-IE;@@h&if z{DaIuMPe!eWdAq24BSmE^@b5mvBL&Y8w|5)K#V z3>F^fTu_?5kh0Um7%FxA=Xw}N4}M~lh7!G{W^%Y2*-V4w&HNMdYzlQH>NUKR)_LfWyDgY1_ZhY1_Y;C-}C1p_MP~6M!kuq>q^oIiN^J`s3{EI$C99i%$tO$n8R#?CO z7?~nQG$N33pF;hz2nOdinFS}DiPG>dGG$t^BN*9CLMXAPHvT*BooEsKlm5jxOR_sg zDG^5fi()NQ9l3J2BN#h$>Ys>UR4F#T#K*{0 z&H0auU`!j=i655Owdi4g>G4KSO39l_vJv6vD5Cr2=z`W^<&;XRD$lWA&{hw%%ba}KxnNPbR$ z!^6;N+r!Xl+r#*l;M*RC7)aRX`^g49j2|%{9!C5KhMyR)MJj^vM}@CEj2oD+J&cD{ zN_z`eD@5gCEOwTw==SL{Il7E^81tDBhs6=WkogMyUx3W0hmoaFzs$qn>?R(@ zX3m3%&ezMN0?$`H2-~b?5A9)axCYiqtoB>3*ct;iISUSScQN&D_U)ypm%?KZJtN7*_#~Ct|k%YCjQ+ zHx?si=M|?J@1<}&5sP;{h?Uo@D!D^M!jc4zkCC^ z>Lr|%DPzi>k#m4dN0i56UStV4`}D#x!sgV2Wd!%gdxybpAshT=umG9L2|0RTOsFXS1I9UEaDAAHp>+` z*#f9fr~AOixDnXF9m}NR=U}vkCzYcR_dm9WoUO=*g zNi>X`O^9?+V8RYK=|)x|e$D_2g(#j`g%0Bn zUn+$zk${h$g`7JOJ*rRC3oK-FLwM2eKSTKsSeopDQZ6p&N8Ni^aVIlVzehuuS9>^= zAVd9*=sp4ofR5OKt(*sCe}y=jDcN6n$H8SitCN_E1Rj>zui4-`wfw>*pUQ>+aw%eH znf-DNJhu)MDTaUmtCT~ZA2Xi7Z5xPJwggxX_Nbq1$ZKSJNRWFF(1>o5#QVyKgk%>n z^$Ru@X?gKI@Dk(Eug_RQIC~i-ck!h`*v3Qjgd2qZ4@ny2bkPm0Q{GlWhVY6ADjo5- z*`?m7zgYHC$o+bt$bl=pr3Ue9ZHF>?Nmx_@xdvIKwqtjO)~OzUYp{0(-sO>c^`0VD z!4smOvyc3s+O;gm7v$lEs6g)FA9)83@diEd`Nz0(JHJ4Q0rSbpb?2#F|&|4dL<>jA5G!M(2H^Dz3gZ;P4$%7$1fAY&rB@V^S_vnbksjcp{*CU`Gmy+Unnf{C)jlKc z0y)zZM$aKkZ5A@B%3j_JQz$a>yTX8~e3Q<3kHpf(s2pj?t5Tk9269F@N<6RNU^kF; zx&s~(xMKS;+>>p5lODyrJM~7q!fXWImXewDOX5Kn_})dlRTqC=25Wx4OBT->maKKB zUgDLR@Pp$?#N*G#AH83~Fa{LE7ze`9cW&6hj2h)(U_zRv-!BGa?P2JLU*#YuJq+rq zvh{f59ya2^LI*XR9tPT$$(>00VeD>fy>}~}#j9YU5r7VO=LRB#P;0N!6%xnmT?Aj= zV7L~_!xyq;^5un`=}OiiB~Ar=Hoh1i8n{sKry+3afwDpMTR;tBW86R~Q(DZo5qYW%v_$;^#($Y(K?{#=)Q&~&WlFjkm+{8l3j&us zTTow0J9&s>v5zf6mn0p7Lh&67T1b&6{x}_r57dKNYDSOmScufnP`Cy>r==8_)qXDsBfIykI<*E+~a@3 zr*L`_u!Z7XVCc|ScK8(f!4uhMB8xnIGq_Jdm1kMyQ&7JZC5*Ly$fw{`i&p|^Cm~r? zsvHXr1?2E8^)Zfxj4-QAx+)`%1;-d%NO%<-Vw!B$hCJTmVKO-}F@jT%_Hr@5u0kf} zMZC*5Gh;39F>*0EqifumS!*-^>oWydO#tM^po6?*H$#<25eiaTg9(STRmR_di)wq^ zsW%x&XFaUaWW&Ki)YSkp3pDwhZM@N3-N$?33KfrhSLFzMtvZJ1c^!CvovOS_b4K4!~iN^!Y*^R_}@kvlBW$Og#?A27( zkCF4BuSL;WKA%@;&2!As$3OAfWXYAl4c~c;O;n&vmgNoBpDH-Pcizhq-J1O5McWw< zuqt4)new&>zPHRook=$-Cgh3LjR5G#38rx9oGj_|skaW+4qbbe^D}+fsXW7;is%nA z6pX(VfY!eV+H3K&6wk-07jwB&@uK_$QD3-9;DbxY)CKB{7({{fJE44oawcOao~$=7 z7TNIF9l}ZY178S4C<4etRb7JLyZ|p7NlS%li1;>P3|4?Ov0NaBAQlQTSCH|BcQC_x zM&7}CQ1u&Z5F-Jgw3M0Z=L3L}QKeBfqxI4wQ~4`UD8tj6!W3KP&U{vtmhJ_n#Deg4 zU2xrj%4re#qT{JtLc+&qgHv@}gv()sIa+`wF2~=vaRMyma;PpvkU;l8e-sCq(Xa*< zvRK(mhl4ALY49j9_#-0LxDtPm<+VQTDa-QLNaaq^U^(moi=s~`_J9$V+zyFhkq9)t zs1gA71kN%7fT#fh`pve0>ec%=$__~hJXBzH4ViZnQ~j0-6cCy09kj<}HOSvb7#I0b zs?r63?q@(k-l>gWdr*DGC@I|dToRR&6PjAC%EnPw-MKiK0Tf6YU5giY!*Ws~@v2Z( zOG^S}mOuJ9#uDA?XUL9LdPCRAC?bkWJJ9K%vRhG+7j)vxC^}#faT#%yenM;Tie_Cr zU+rs@$O#H{{G*5mFwjNyBubrNY!Tej++*>Cvl8MO=ujbb%QNyP-$4yP-$)Kuk$*Xu z!QYDjls`29tg$V`LS7eT%g`Xk^itinr z(n4DFX}|!i39{2wwnWGGAxj&@r0;6N9juG0?@8$%kHVna1#(GeA|GRu;jEk>GEngc zK-KkZ1XY^Yk&~GPalM5RI^qj4$oRuKH8aPMVgY$U`T@bdln~VLI|v-KB|<*uU{gOl+l_9&3+)Gg(1a%KZe)w5x&@gC|To9{XUgUCdzA5K>*WU2MG-bXy9$4T>?}O z4s?pSJBNLXo_kO|D|=c?~Qb0C;IS zHu%BdZZsJ|4n!e1kp=KOnewvn1y*3A18u^OL4YB&X$1ELP)(`Q3C0vKaJ*7!fT0FV zC&D^n^=gU_VA94!hXh~Zq#_+436bIT{us?3L_^T1g(wB-bVy;4tQV3De;}OB8s{ni zVh9K$f=?8~EBD+b&mot_Uw(trk5;Arh`i#L1hWyQkN%S2>gex#<2PBj+NF?ii&rDV zq7NdiARF>*JqCm@5CJn4}!z&3NB$KpbhEr zlWAxbB5S*L@VD0475tz}-?Dx{2&h^2 zL|cK)6wCEdTQ}lp(RC10B(90bK-k9-t*@W(Dh(uxngpG<%i9}>l!m(>dk#b ze$MJEFTc6xe>LTI?;qmHj~0k7JvD^F7kz}z@P|pLWwuhyTbXckh-PrF%tVwCt4nGW3A`bgHUWJt!Qi;$TtLDeZG z#?>zis*nw5Tg;Q%;NnJwVli{V5%s7CaDfAZsv3Z*JETT=Vi*Qg5`d~3agp=s2hj4$ z?8~1L0?!hjMMLEIA7xLGv8^|CS3LduYK`Ayd7KRmRTIls#(&xWSCOCk$s_%1P1ELS zXBORB4;3<>4X7V$M3<`lT&$^V9JTW*IxrW=T)VYy{mH;y83Sinym08Xo|hPt*+nOt zipP&k?O1AYzfyGL|FmM_VBt?_UrNt&~+A`Jys6K;%K zdCMqm8jh@6uu9{m3aHsLdk^Y>dcF;=vo0Vt(e`eEVl;H1eq;|a&4U&fYy9coXSn=( z6$$xeN08#c6bb=p>JI8M<9?aX_kQ?OI_)iN7G#Q*BISx`2!AMyzfKL_v=HPG1|-=l zy*J#?VtA)EK42A#T$U^t(}4uf#=+StfdQH1uHDW141cCt0y%o8w%?cTu|?eDM@WmHMV8Q{s~AX;<|uy+M`zTr zJsK0~Ei7`kvzT-Rq^@JGxU*wsfGIhmCv-=>%1XH<#bt#Zr-t9=U(FosKI>>zhe!%X zPyyWlCcBhDLF$Fx7hi|n9&O1JlH5z7sr52XN8qd{O{PQ^*7csc!Cq&jxNusL3rIgZ zf;YvyK|3XshkkF7q$g#VbO54UNHbrVYW~Xj6KAQCKu|Zy)=jf@lWX00c_TkX-h1b4 z@a#S6g_$`bX4VB_hu0H!1GQnGo^ZVI zbEuxM_v7gam6Opgr6-~kh^096ga(7bkhppxnkKrVeRkVb{}0g^J2V=X;JsH%MpPh^?V&C#loC*y;@-ditJSX{0S^Ly z)jGa|#pl|^F}~9Y;eg)6rh-CE@@BI#9Ep}lpojoaO zXpJzt6d9nVR$n4<6(+&SA($t2V(%8!h&fWp9`~vH$8=!ZPur`((yJ?@q~ig;-*C*x;Ar{L#8uU!S1Y&U^sv<;KM_N&7=96VJVsbjt9IvDi~(@TUYfFZ;P?;U43 z+^Jz-vwKtp4*b)vCTZ-4;xGadOOcSU_Xb7maxaM8lu6EZf+DWx8-bR?ROci206t8V@d#>Y8 zo+BScWhIjG3ymOKLEO8(kq;{fh9~7-o%vi8bcY5p2+w*_@8tVQJUKTL&VzLTb05Kz zRHiZ4DG)K)@|>#`Cd$eo+MGg_-fyMzR2r)$l|NpkdEN!*RXjOkR2okT3`oPrOqHXw z_&r;NK}SJK=(JsfJEn$VF*Wo<+0YhU1E?BokiE)9E-b3Lrrq z_le@X#p4(#RL@;gxNx#lH*(8q3JRkSFm$GhQzB1g!w>4B(!gbbWt$3L1I?KuKUGK= zWr#k;9~ff%)Gt7M_8R9zDQ)~vC?pF$p^|VfbIH;jtV*@N8M+4HdjP%@O+hhAf>AO3 z+ud%@IKfBo=lODAl-5vLeip@9z!302&6#T&XH&?KpptQ7wo8> zl-@w6^BgdKd>Kq;Ju1ED$$1n7jLt+@bjm4^@;kusG!5_5ej_-6k@}jUuVdBKkM#w* z8(qp79^FqtL433V*sQ1Wm7+c2W58<_u+W{-fNV2xIh+*ThtPKOXGlXfkmE*C?G?fc zblQso*s8%_9*f~-KI*{sCS98<+;MeS1BJuW^QaSkIWS6*;))~X%~PaB*6Wn;3(t5s z0C;#I64hOK_QSywcEob=7kgu}Lnq@;%pFWxAZxKA16`1%fM_C0c#xOUEO~S?{ap|* z956J~z!*`SqPMC5hW`)nGSM>ghC*lCtIS^_?QHu>)pj1VU@O^nRT4LHc!PXGDA}kV zLRNwar0hId8Vk|jv^D1gummZ94>paP7-Vu4MS$`YkSuv*RT!W!*w(Sj6&0(kS~!$% zO1os?9oLqS#sRAA!5X1#nUq`)o!V@$Kt1BGXNQjkBUNBnO=g~v)l^b-ip1?nL=V6= zsj=u8ImOEJ2$-&|Im(F2EYWA)grpiuDdz#(R-3;`v(aR)NQm8_Y$=T+o{qy8idq8u zhS8?#BT(s#fMth)`V~l9dlGrn$jZTH27b(1YY|FH?g(lSX^*{Kig2%37i$rQ2btQY)B{Z>P8=K{=Sx&W^4Wz1r5awqu^o+D{CclcRE)?P)@gv0_;SrN(W6me zdCYR8m9Y40zn1MB3D23+#bVWgxYG}RD4&s8`bW-aM}#8)vN>nG$Vq=QpnOjp%c0BQ z4IpO=ZeTLlK1dgg!jeE>oB${yIK(((C-;Gj@Cgo4W!eH+q}mICl;E)qo}LlGaP%|I ztHc^ey7okt)-uv#6xn=%3VjL=K`ED%rmA649ek!iF*u!44e5qZkRG;);}#+=c@`DJ zWrLP4RosX%2m8Q5J5qBqfq-L%^7F1Rt^pI@ZVG@$a^C@l%-wwB0S{5pp2;Jn+t5#Z zhYgNU;?G-3Vz`rIB;b77P864O81je82d(1JR~my{tthERHu%GEHOF4U-aQ$W&=j=3 z>7Mgb@GK+PEL3Ax_`p=qY;;b{MJ5D?-pR#Zp3ienqivJrgPtuhjKMdD<1}%&3d|`6 zi(~2i0p!I;zc{Ns$Bayiu2*5;xk4O&Wlk;uy^C<9!j2z+{|br+?ME}wtZX1}M}t4a zmv(#)zOx`}z(ET$ODWQ9@Iz3_ zDv)I>s|ewZhc}Fxtz{~iDIf+qn1Y2u3XY;2cu*mJCRamU^-ETPE1s?_xNwG&4MKkh za^|BysMQJ=-Ht^Mriczu?EMyn8(omgdi*~^+NW$PRB zQ7{K0WPRt{{Y?w^I{+E(i6jGkQuL1k>~Qoqu*+HXK>8|vLMiC@2L%v6{t@l20r3-W zv^;euZh2|5F?)NmF?&t&YG8i-`C0;BOWd@X^mCGfQb{%1*`tm)R^jJk?YO|Y!I ztfHIp77hi&)iXlD#&D><@jKq>)whOfy|qmngp>^GcpKGsl}zG0Iy} zU0E4wX!KS#)XngQt3uxLriO-4ZMbHpx2&e7t{fCA4oBehV~T4+WsMYPqx+3H)6NFns<>5M;FYsJ8qrUOCMUUmY3BxHr0f(F0QSq4>s1-+!|^K*4LEPW?hUQrBt6oNnl8@j(9=^kB9Rl)J zH+rv}QdH!KV#~L@uC^k0W#%YvZC%(~J)^!RGy@6|syNr%SY2Bl0@zz$ zhFZ#M(4N|gP%XgIy{dC4(Wb^w#p%ab-RK%v4N8{HfM$jpX0lYc&f6FYdo!v@vTQ~O zvW6}~DMHP-twj};O||9JpT^PNf>@Zhs;qHfSt3=l-$vW@L#@KW+Te_`W-nEq9ER&q z?F{Jmx#(W13-q@dZLJk02#3&SXtLJCV=9_v%$RBO0hF>Z^s?4_BL)j-V0@t9jiK;u zA&fOG24=7xuNsuj)}e9Gm{3DCaJ(3?W!_L_Wp#PA8gw8v5xh@sbTuL$4YA~>mEYF( z(d8}~A%hHzP;-4v@Yae@5Q4nTB`N4#5Rz00W|dXkDCRgq4uG&UdZ9V=6 zm8QCC^-YkCb=87;Er``N@}^PlrV|=+)y{O)RsmM!Qd(UVYHW1LFh=8>s06`AXj|~M zhHy~nk;=vBY=X&)$r@M&>KzW&l+6gvsIGOz456l)8a0AAK3o+uYs+R-mp2B>>y!ax zz2TYlp=b{JPg&SlP}%fQqXQPYEsV0&(Bwma;W|f6H5>cxla)yn@00hrv<>jFoZKYfeXt?@Tv0TA$6^d5X)l{%L(&qr<{cU9p z6b=l8s~$EFLYE2~Yg~=cq_P>V`f8Uw)`QY@&=)9`FsTMB+KDkcLoU^gVRVLzxkrOM zb9iO?7*|Hc(Jol2DltH?BQrzc(bmYfM~XE+!q7A+{e=p+T!-ZAc>`!N#aMRoDBSS- z^Plna*aQXNt*^e^J4XEZvUhKvGvV_;e6)Vjjl~mwyx}bK*wLvMXH7h1+k4+WN#m(f zj1COJ7qMa&g9Zra3+-XJ%BRQQp|81>^4YsmUnf*&yQ;^ys?T@TWh-3#B|U!mAx&rc z=$LfB`rJ)i$EU@*-?aLeWv_aFGokC$=g<4e@7)s~*4Kk;$3MKq_nQe9-R{|X%xiZ& znS05sA9auNtW3P4zjyNF+b{m*N#9y>)V;$e{Q2FtvLD{E<>Jg==RcG4V)KME%g_G( z#lLM{IW*TlCFAj`mAB~f-`3?W)pXi)`M=-)=Au88|7Jo)*9*gY9{$zJ+w^t)>+3T% z?D=#;;H)(t-1FV)mA(I`o$HT{r1ME)D>hcGkqn69HtO_{od?*d%JVHa@&9F0gm79H*em&nfGR9xi@o(SARZU*S%}v z*JsbFmVbNjixY1g`j7Kh|M?eO|0b??Bir#`yPW!)x%>#1yT*F%HD7i@K2{#&OCGZx0OepS($u^ z;PQJf_HI|bRjPlh>ffe|48N7n_uw;u&lEnR_>|*}-VNXeU===V@gZTaKZE=$9LbWu z#piPGEie8Q|B{SSOD|&Uimx7Gz2&41;wVWSaxeWO{U!bo|44sJ|BHXc-x5y}UlK18 zKjIhhyTpOSnZ%vxZ;303Gl^sA2Z?*mp?18s2349BE*|Q;^U&eK+GM|s3quYShz{Sn^d<$?H*a@uIlFvU*Jn(H`>i&Gb z5);VqR^$Umfy2Q02lDwSCY9-(`TPKI88`+^;fwy8z&2nMli4V618@_a(BR@Ddc`4QkSZ~{0Ad;vHCoB>V&uL7rmi@;gnP2fB* zidUSAKzdES46Fu5TJrg3;P9b*{sM3mm;+7#uK=fj^T27~b>J+J0&yO=7Pttk1TF)2 z0wa&0|9}-hED5)I9V7 zr-8%3S>PCO9ykeH1YQI#180E|>`;CiSOHuDRszehf2bN*0Zaim1E+yW;3BXU*!J(3 zBY>m8$APoJabN^<#1yascnR1BoCDH)a*ghPgMP(;?g3T>%i29=q4b}4%^f6*sSX?JBrU3IJ#CTE4tT;=<<`eeHQ$y#5Y>eXG&|WiXW6U zSh1(eYpu$$6*X4k+-)^h^_ko2tz_xoN~^lgO4L}DwN?z0wN|v&iZrb1u&Nu2=oINh zTgx})^G9fcEa*q+K>f4Oj~WgAHLFxLiZ???#W(W#_4LpHT~Cv(V`Vi~>|8lYoLSLi z4V2cev?{^W6l_z}C$#5yHRcNpd!3%e-wWU(;0_=bKh?I8lCq%{)@Vt}8ZRlUEWuT+ zHBxdMxDU)AupXEOHi9pG0vCHh9OR56j5e@=3ZkL)MGb9SMLw-U`)2+qpZ_&ks{5>o zd=lJeEu{@s+g>Yu&}yr<(rK%$#%kHCnYN%B==-;4p+C6;V~q487QVI8ilB?t3Ebw8 z*NMDFir-uNtI3)xEj@{L6nnmL)k$l-tRDhpPpk@b=^oTaexZ539$zF&Y!g<2OM=^o zYqigcQlF&29Rc@kH;!)ACx2nJ?Xl9Of3JIEkG?_wpl1#Tg)jBV6zRuShJ#cm`lOfp zgz~N+ZxVU;J9!l?R_qXlkG7x2?nUHbI@gKpzX2`>PLDO*6BogHbjib2f?Ee}7TjfA zQ(upjB5Kc-)myPM<)uHfD(dv$!aRoT6l70eU&>#*?bqWpC?=Y$=~%Nhox~^IY%O(_ zeXS(W1>|x)zx)`wbJ)yNO?IH|{b(H7dfw< zH^nm={mPi`gBj?%ign)W->cx}3~mwJYv2GnQD4zB#;hSn&l^_^IePY(F}P}Qmkh2M z+%!0vt|(C*X>b<}E(S_w+4;KPj~BpI8(a=t6*#XS zuYgM!a`WIe8*9P2$8k-GxK3~b z;JiK?1~+ENje%P<{TS8ZRxox*Dr@oqzRq+L%K5&F}dAcLZFe!3}`h0M6^bv*2Qe z+yuCFhTIF_DhzH0+**UX3N8xH>%T>Cs}1fZxCl6aPUNd73YCMSxr!2T8^9&OdBw(XD;D6f#kcD5eo}gq z|GAX=cZ~Z*zF+N#V?l8)WbL@|JLeu7ZFOqf;%LJDex0y{==3GVH0Q75d&&RpC;5J& zmhgWIC7Jhg*r7%Vk0d%xaR#PFoz|{#ke&}HRXCvV=4z9tVq@Zsqd4#WBM;@=Gb$g`VCBDD%6(|4qeDB)n(EsoM_HcZd z9g*O;OER`FrWpqqM;ON$CmC~$GmLYL3ye#Q3ctrhDasgQOfV)HTNu-f1B@e#<c6O1%XP(sP65qY+1s%<~p*Nslu4O6Fq*Ukg4+KfNy`J<*l8Kob3@M32GK zdl8af_?%z?68;oq=OE;jx*5E@4}9$KvYxNNf=BYnLk_Rs1LATW^Xa1w zFYE74=2fS|%m1f`nIAFgJ*9a^*8BJSEWbowP$9|iexCW6-#NTYL$8MDd6(srf8xmN zch0yh!F#eh9m@Cjz7W1Tgs1m|L3)mc@aIGLztg;>Cb|E`FaN^)K*}*Q$=3dc`5bqQ zJRe2ixERZL;fvsd?A)YzOAW9e#qYbBA2*T7Gh*nc=fWU+E{E`Mv7Y4n zPOHT4%fcIRSb+!7ApL)=dBndF=WWc77;#1K^n&#K70X9ikHr5}h&+9d8Kmc(5dLG< zpKf#7EB^f&Dx&c?a?RnTe@E{*#|CcpysTsCY?AgV<>TncbhSkYteuVuc?fNeB zEr%UB{hl9}6U;{sIs8Tk)GwJ|;{KOczn-c@8`_@ z%;yZh_c5O~;`u4fdo?@PKVtbA9@qMPJ+h{mS6r{W=bR7G^9lI%#p=~P=va!+Nn_sF zq50wh^gTAo=h$B|uFo+aV_y2+Yl(-u@eio3m+h4Q zkGE>R*iQOhoaE=&zhaNgyfXAZ5u)b;%jZ%~9s2t|T+T8-^1j1MKYYmi0=E~<(dlz| zFw_0~h;x6C`AX*XmsjA#&i%|MjrpWY^IX^ikFop!*Nfq))88^5`@p%^@0D@+4e~spyI=z1%VfjfTZoeBMe?EkNf%VK8{rn2^Nh6-$ zXFkEt6)V`NyY+mFIAQzc`DZKh14f+hV}6qRLHygxe1hx6^rF*~n&%r2OoZ_N$a-=- zKQwche--`4yz!sRk25d!tXbpuW!#8|gyubl@MROr$9P=G`}FUG$d9soveQw3=|!jK zm`@x1{}S^92LCSeGi;~$r4;`f1Pts(DK-v40UBIz7t#B(H1eUY*V} zpX7d)@%1z2V+Q{w_&d0)7p}3qI^?jp)#)blBRt+OGk+(P(D-WMe!hLg$=IZM7V^M@ zEI-MCtoM^3-)26+aVz$Zh<@$|nQwp0{CJb2T<@{Kh#5 z9=01F-e1?c`@nwNj&Fs!Vm9H~I_%wCk9HRJ6niu5%+}DHZJ{|u)yR(i;(8<}prS%n zs6IPWWg1Z^Rb*cwYkN>BoGS>n4;dlm>6}38bWWfJI%hlF)A&%$z6QHtf4yzvu4(_n zc0-EERQ*Fr2aB?snh#*FynUdt@nA!%-C9%I+(6>3`|8}Hotb{S2Rq$QbSb<3@cx>8 zyXzEhqBjp(9m5WBecGUZ2wUvkT=&=o8P`e2p}IY}9C_F^-&ItIPiNR2JzX8gLzL24 z4m;C(>}0V{vWIri_YY(DY#x*swF${EveUlG)kYr*)C>I1MnEQQ7vB zKI<`}^+AGebkA`&XM4Pg4o2uRLUgi4pAoWMMxQXSJK8htoxQfRvEN@MbD`erZXY_4 zh5tRh!)@ea0);a$#UkB|pvt=iD)cy;n_F}U)m2Z&HeKtD zR!c~Bb_I+(Ul`$RR@bfCgX2~KW7au|L}!OGp(fMeBIkgqa{$sK==aR08_APDxzHDj zx+g!SAN_URs<}E$cn}-?NsBajmvd!G;1Igyv=G{CZ3;TqonzfuSJneN+Ybk84`S}< z7;dN4myW9Xd!b+}$BfKp6d?iX-3on^I(4pTtAvqf=_*v_3nRS>^=41|qs8C%#S@Ny z1;C<)^JLNs-oSG8=n+N>9#s`E3dP{=_M?SW)>$vZ+9qx=8YUih14exAPEc?@W{!4D z=w4I(O$s|(~GIiTByw6k0k0enJBd<_0jI2hU+onf3DRnlTN$UL&dG_ zLr>6&_5O^15~jzDj^p$+>3`&@3bXE0hMc=n2Y8hlXO05D-`r<@aT@H!gA!VgL&?5p zY`2_mR`IyoGs17JK#zvZqn`G%^Te}G;DqNp3yCM+wN0z$^-o%a2~CD(NAUxiNO7?0 zuoQ90L!^auhOW>UbpBA32k{X1xDE)(cOP6cSv&*{`eLb2#8;)KW0((Gtz?wB15-aC z6^%%rX6HXCS|Ar0HQDAn^!5+E7x$my{v6Dk_{CLIy{(m{G;x2V` zsP{zYmflXK1@J2oJt~e_9m*Wc4mvsbIgt#G_xo;axLNk`wSC#qBk?BkaxeH6GVoQCTVD1JrvHcKKR{WE zU8!I8Z}o8b1;-3`U$xXLex!YdBqHTg_d5krf8gLW^1MmP3oaqkTi)G=7IVtGj$tKI zPVo0;dEQT{);r}zPHZsmX@e(mV!!OijD(bDt3;hD2SHrIWt#e}@*XkX~&l?4dTlL?^t7+f> diff --git a/control/velocity_controller/src/test_VC.cpp b/control/velocity_controller/src/test_VC.cpp index cb413c72e..daf614f64 100644 --- a/control/velocity_controller/src/test_VC.cpp +++ b/control/velocity_controller/src/test_VC.cpp @@ -45,7 +45,7 @@ void test_VC::send_guidance() /*time1+=0.2; reference_msg.yaw=0.6*sin(time1*std::numbers::pi/9); reference_msg.pitch=0.3*sin(time1*std::numbers::pi/9);*/ - reference_msg.surge=1.0;reference_msg.pitch=0.3;reference_msg.yaw=-1.57; //Surge, pitch, yaw + reference_msg.surge=0.20;reference_msg.pitch=-0.4;reference_msg.yaw=0.0; //Surge, pitch, yaw //RCLCPP_INFO(this->get_logger(), "guidance callback: %f, %f, %f",reference_msg.surge,reference_msg.pitch,reference_msg.yaw); publisher_guidance->publish(reference_msg); diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index b54e6cae9..ce3d45c38 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -19,7 +19,7 @@ //Konstruktør -Velocity_node::Velocity_node() : Node("velocity_controller_node"), PID_surge(100,5,0), PID_yaw(35,1,0), PID_pitch(35,1,0), lqr_controller() +Velocity_node::Velocity_node() : Node("velocity_controller_node"), PID_surge(600,50,0), PID_yaw(120,20,0), PID_pitch(35,1,0), lqr_controller() { //Dytter info til log RCLCPP_INFO(this->get_logger(), "Velocity control node has been started."); @@ -51,20 +51,18 @@ Velocity_node::Velocity_node() : Node("velocity_controller_node"), PID_surge(100 //NMPC controller - /* - NMPC.set_matrices(Q2,R2, inertia_matrix, max_force, dampening_matrix_low, dampening_matrix_high); + + NMPC.set_matrices(Q3,R3, inertia_matrix, max_force, dampening_matrix_low, dampening_matrix_high); NMPC.set_interval(publish_rate/1000.0); - NMPC.initialize_MPC(); - */ + //NMPC.initialize_MPC(); + //NMPC acados controller NMPC_acados.init(); NMPC_acados.set_max_force(max_force); std::vector W=Q2; W.insert(W.end(),R2.begin(),R2.end()); std::vector We=Q2; - for (int i=0;i<(int)We.size();i++){ - We[i]+=1e-6; - } + NMPC_acados.set_weights(W, We); //Timer @@ -87,7 +85,7 @@ Velocity_node::Velocity_node() : Node("velocity_controller_node"), PID_surge(100 //Publish/timer functions void Velocity_node::publish_thrust(){ - RCLCPP_INFO(this->get_logger(),"sending thrust"); + //RCLCPP_INFO(this->get_logger(),"sending thrust"); publisher_thrust->publish(thrust_out); } @@ -133,12 +131,13 @@ void Velocity_node::calc_thrust() case 3:{ RCLCPP_INFO(this->get_logger(),"Guidance: %f, %f, %f",guidance_values.surge,guidance_values.pitch,guidance_values.yaw); Eigen::Matrix u; - u=NMPC.calculate_thrust(guidance_values, current_state); - if (u==Eigen::Matrix{9999,9999,9999}){ + if (NMPC.calculate_thrust(guidance_values, current_state)){ controller_type=1; RCLCPP_ERROR(this->get_logger(),"Switching to PID"); + rclcpp::shutdown(); } else{ + u=NMPC.get_thrust(); thrust_out.wrench.force.x=u[0]; thrust_out.wrench.torque.y=u[1]; thrust_out.wrench.torque.z=u[2]; @@ -149,15 +148,15 @@ void Velocity_node::calc_thrust() } case 4:{ std::vectoru; - std::array x_ref={guidance_values.surge,guidance_values.sway,guidance_values.heave,guidance_values.roll_rate,guidance_values.pitch_rate,guidance_values.yaw_rate,guidance_values.roll,guidance_values.pitch,guidance_values.yaw}; - std::array u_ref={0,0,0}; + std::array x_ref={guidance_values.surge,0.0,0.0,guidance_values.pitch,guidance_values.yaw}; //surge, pitch_rate, yaw_rate, pitch, yaw - NMPC_acados.setReference(x_ref,u_ref); + NMPC_acados.setReference(x_ref); std::array state_array={current_state.surge,current_state.sway,current_state.heave,current_state.roll_rate,current_state.pitch_rate,current_state.yaw_rate,current_state.roll,current_state.pitch,current_state.yaw}; NMPC_acados.setState(state_array); int status=NMPC_acados.solve_once(); + RCLCPP_INFO(this->get_logger(),"Status %i",status); if(status){ - RCLCPP_ERROR(this->get_logger(),"Error status %i",status); + rclcpp::shutdown(); }; u=NMPC_acados.getU0(); @@ -251,16 +250,17 @@ void Velocity_node::get_new_parameters(){ this->dampening_matrix_low=this->get_parameter("dampening_matrix_low").as_double_array(); this->dampening_matrix_high=this->get_parameter("dampening_matrix_high").as_double_array(); - //NMPC Parameters + //NMPC acados Parameters + this->declare_parameter>("NMPCA_params.Q"); + this->declare_parameter>("NMPCA_params.R"); + Q2=this->get_parameter("NMPCA_params.Q").as_double_array(); + R2=this->get_parameter("NMPCA_params.R").as_double_array(); + //NMPC this->declare_parameter>("NMPC_params.Q"); this->declare_parameter>("NMPC_params.R"); - Q2=this->get_parameter("NMPC_params.Q").as_double_array(); - R2=this->get_parameter("NMPC_params.R").as_double_array(); - - - + Q3=this->get_parameter("NMPC_params.Q").as_double_array(); + R3=this->get_parameter("NMPC_params.R").as_double_array(); - } int main(int argc, char * argv[]) From 4beeab0b66c3681276798ee8132a42590d9f15c9 Mon Sep 17 00:00:00 2001 From: Henrik Date: Mon, 9 Mar 2026 11:34:41 +0100 Subject: [PATCH 077/128] Changed how one verifies the control output from LQR --- .../include/velocity_controller/LQR_setup.hpp | 15 +++------------ control/velocity_controller/src/LQR_setup.cpp | 11 ++++++++--- .../src/velocity_controller.cpp | 9 +++++---- control/velocity_controller/tests/test_LQR.cpp | 9 ++++++--- 4 files changed, 22 insertions(+), 22 deletions(-) diff --git a/control/velocity_controller/include/velocity_controller/LQR_setup.hpp b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp index fd756be43..bd252f5e3 100644 --- a/control/velocity_controller/include/velocity_controller/LQR_setup.hpp +++ b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp @@ -45,28 +45,18 @@ class LQRController{ LQRController(); bool set_matrices(std::vector Q_,std::vector R_,std::vector inertia_matrix, double max_force,std::vector water_r_low,std::vector water_r_high); void reset_controller(); - Eigen::Vector calculate_thrust(State states, Guidance_data guidance_values); + bool calculate_thrust(State states, Guidance_data guidance_values); int set_interval(double interval); - + Eigen::Vector get_thrust(); private: - //void set_params(LQRparameters params); - //Eigen::Matrix3d calculate_coriolis_matrix(double pitchrate, double yaw_rate, double sway_vel, double heave_vel); Eigen::Matrix linearize(State states); - //angle quaternion_to_euler_angle(double w, double x, double y, double z); - //double ssa(double angle); - std::tuple saturate (double value, bool windup, double limit); double anti_windup(double error, double integral_sum, bool windup); Eigen::Vector saturate_input(Eigen::Vector u); Eigen::Vector update_error(Guidance_data guidance_values, State states); - //LQRsolveResult solve_lqr(const Eigen::MatrixXd &A,const Eigen::MatrixXd &B,const Eigen::MatrixXd &Q, const Eigen::MatrixXd &R); - - //Resets controller - - // VariablesEigen::Matrix3d vector_to_matrix3d(const std::vector &other_matrix) double interval_; double integral_error_surge; double integral_error_pitch; double integral_error_yaw; bool surge_windup; bool pitch_windup; bool yaw_windup; @@ -79,6 +69,7 @@ class LQRController{ Eigen::Matrix3d input_weight_matrix; Eigen::Matrix augmented_system_matrix; Eigen::Matrix augmented_input_matrix; + Eigen::Vector u; }; diff --git a/control/velocity_controller/src/LQR_setup.cpp b/control/velocity_controller/src/LQR_setup.cpp index 00065adc7..f4f1153c8 100644 --- a/control/velocity_controller/src/LQR_setup.cpp +++ b/control/velocity_controller/src/LQR_setup.cpp @@ -176,7 +176,7 @@ Eigen::Vector LQRController::saturate_input(Eigen::Vector u) std::tie(yaw_windup, torque_z) = saturate(u[2], yaw_windup, max_force); return {force_x, torque_y, torque_z}; } -Eigen::Vector LQRController::calculate_thrust(State state, Guidance_data guidance_values){ +bool LQRController::calculate_thrust(State state, Guidance_data guidance_values){ ct::optcon::LQR<8,3> lqr; Eigen::Matrix K_l; /*RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"A matrix: %f, %f, %f, %f, %f, %f, %f, %f; %f, %f, %f, %f, %f, %f, %f, %f; ...",linearize(state)(0,0),linearize(state)(0,1),linearize(state)(0,2),linearize(state)(0,3),linearize(state)(0,4),linearize(state)(0,5),linearize(state)(0,6),linearize(state)(0,7), @@ -188,7 +188,7 @@ Eigen::Vector LQRController::calculate_thrust(State state, Guidance_da */ bool INFO= lqr.compute(Q,R,linearize(state),B,K_l,true,false); if(INFO==0){ - return {9999,9999,9999}; //Need to fix + return false; } /* Eigen::Matrix K; @@ -207,7 +207,8 @@ Eigen::Vector LQRController::calculate_thrust(State state, Guidance_da K_l(1,0),K_l(1,1),K_l(1,2),K_l(1,3),K_l(1,4),K_l(1,5),K_l(1,6),K_l(1,7), K_l(2,0),K_l(2,1),K_l(2,2),K_l(2,3),K_l(2,4),K_l(2,5),K_l(2,6),K_l(2,7)); */ - return saturate_input( (K_l*state_error)); + u=saturate_input( (K_l*state_error)); + return true; } void LQRController::reset_controller(){ integral_error_surge=0.0; @@ -255,4 +256,8 @@ Eigen::Matrix3d vector2d_to_matrix3d(const std::vector> &oth for (int j = 0; j < 3; ++j) mat(i, j) = other_matrix[i][j]; return mat; +} + +Eigen::Vector LQRController::get_thrust(){ + return u; } \ No newline at end of file diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index ce3d45c38..d67889708 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -95,11 +95,11 @@ void Velocity_node::calc_thrust() angle NED_error={guidance_values.roll-current_state.roll,guidance_values.pitch-current_state.pitch,guidance_values.yaw-current_state.yaw}; angle error=NED_to_BODY(NED_error,current_state); Guidance_data mod_g_values=guidance_values; - if (error.psit<3.14/2 && error.thetat<3.14/2){ //Need to fix to pi + if (abs(error.psit)<3.14/2 || abs(error.thetat)<3.14/2){ //Need to fix to pi mod_g_values.surge=guidance_values.surge*cos(error.psit)*cos(error.thetat); } else{ - mod_g_values.surge=0; + mod_g_values.surge=current_state.surge; //Only focus on rotating? Or is 0 maybe TODO: Decide. Potentially set the u.surge to 0. Then remember to fix the integral anti wind up } switch (controller_type) { @@ -116,12 +116,13 @@ void Velocity_node::calc_thrust() } case 2:{ - Eigen::Vector3d u=lqr_controller.calculate_thrust(current_state,mod_g_values); - if (u==Eigen::Vector3d{9999,9999,9999}){ + + if (!lqr_controller.calculate_thrust(current_state,mod_g_values)){ controller_type=1; RCLCPP_ERROR(this->get_logger(),"Switching to PID"); } else{ + Eigen::Vector3d u=lqr_controller.get_thrust(); thrust_out.wrench.force.x=u[0]; thrust_out.wrench.torque.y=u[1]; thrust_out.wrench.torque.z=u[2]; diff --git a/control/velocity_controller/tests/test_LQR.cpp b/control/velocity_controller/tests/test_LQR.cpp index f97f112bd..9317da845 100644 --- a/control/velocity_controller/tests/test_LQR.cpp +++ b/control/velocity_controller/tests/test_LQR.cpp @@ -64,7 +64,8 @@ TEST_F(LQR_test,Direction){ Guidance_data value; State state{}; value.surge=0.2; - Eigen::Vector result=controller.calculate_thrust(state,value); + controller.calculate_thrust(state,value); + Eigen::Vector result=controller.get_thrust(); EXPECT_TRUE(result(0)>0); } @@ -77,7 +78,8 @@ TEST_F(LQR_test,zero_input){ value.surge=1.0; value.yaw=0.2; value.pitch=0.3; - Eigen::Vector result=controller.calculate_thrust(states,value); + controller.calculate_thrust(states,value); + Eigen::Vector result=controller.get_thrust(); EXPECT_NEAR(result(0),0,delta); EXPECT_NEAR(result(1),0,delta); EXPECT_NEAR(result(2),0,delta); @@ -88,7 +90,8 @@ TEST_F(LQR_test,zero_input){ value.surge=0; value.pitch=0; value.yaw=0; - result=controller.calculate_thrust(states, value); + controller.calculate_thrust(states, value); + result=controller.get_thrust(); EXPECT_NEAR(result(0),0,delta); EXPECT_NEAR(result(1),0,delta); EXPECT_NEAR(result(2),0,delta); From c8a1da7a6b364130ab6aef4ce87b99a23d9200c0 Mon Sep 17 00:00:00 2001 From: Anbit Date: Mon, 9 Mar 2026 14:20:35 +0100 Subject: [PATCH 078/128] Fix issues with new operation_manager --- .../los_guidance/config/guidance_params.yaml | 4 ++-- .../launch/guidance_test.launch.py | 24 ++++++++++++++++--- 2 files changed, 23 insertions(+), 5 deletions(-) diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 35ac01df6..63ab43b8f 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -26,8 +26,8 @@ vector_field_los: # Common Guidance Parameters common: - active_los_method: 2 - u_desired: 0.3 + active_los_method: 2 # 0: Adaptive LOS, 1: Proportional LOS, 2: Integral LOS, 3: Vector Field LOS + u_desired: 0.3 goal_reached_tol: 0.35 # Debug Settings diff --git a/guidance/los_guidance/launch/guidance_test.launch.py b/guidance/los_guidance/launch/guidance_test.launch.py index 8dd953671..03d404982 100644 --- a/guidance/los_guidance/launch/guidance_test.launch.py +++ b/guidance/los_guidance/launch/guidance_test.launch.py @@ -22,7 +22,7 @@ def generate_launch_description(): ), launch_arguments={ 'scenario': 'tacc', - 'rendering': 'true', + 'rendering': 'false', }.items(), ) @@ -34,6 +34,16 @@ def generate_launch_description(): ) ) + operation_mode_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("operation_mode_manager"), + "launch", + "operation_mode_manager.launch.py", + ) + ) +) + los_guidance_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(los_guidance_dir, 'launch', 'los_guidance.launch.py') @@ -66,8 +76,15 @@ def generate_launch_description(): cmd=[ "bash", "-lc", - "ros2 topic pub --once /orca/killswitch std_msgs/msg/Bool \"{data: false}\"; " - "ros2 topic pub --once /orca/operation_mode std_msgs/msg/String \"{data: 'autonomous mode'}\"; ", + ( + "ros2 service call /orca/set_killswitch " + "vortex_msgs/srv/SetKillswitch " + "\"{killswitch_on: false}\" " + "&& " + "ros2 service call /orca/set_operation_mode " + "vortex_msgs/srv/SetOperationMode " + "\"{requested_operation_mode: {operation_mode: 1}}\"" + ), ], output="screen", ), @@ -92,6 +109,7 @@ def generate_launch_description(): [ stonefish_sim, vortex_sim_interface, + operation_mode_launch, los_guidance_launch, velocity_controller_launch, orca_sim, From a2927addaf9cfa7a3dbf1b34e80bae64dc0b235d Mon Sep 17 00:00:00 2001 From: Anbit Date: Mon, 9 Mar 2026 15:06:08 +0100 Subject: [PATCH 079/128] Add clamp for pitch --- guidance/los_guidance/config/guidance_params.yaml | 3 ++- .../include/los_guidance/los_guidance_ros.hpp | 1 + guidance/los_guidance/launch/guidance_test.launch.py | 12 ++++++------ guidance/los_guidance/src/los_guidance_ros.cpp | 5 ++++- 4 files changed, 13 insertions(+), 8 deletions(-) diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 63ab43b8f..6dd71b6a4 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -27,8 +27,9 @@ vector_field_los: # Common Guidance Parameters common: active_los_method: 2 # 0: Adaptive LOS, 1: Proportional LOS, 2: Integral LOS, 3: Vector Field LOS - u_desired: 0.3 + u_desired: 0.3 goal_reached_tol: 0.35 + max_pitch_angle: 0.785 # 45 degrees # Debug Settings debug: diff --git a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp index 54ff2e24c..39e4edca8 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -110,6 +110,7 @@ class LosGuidanceNode : public rclcpp::Node { types::Inputs path_inputs_{}; double u_desired_{}; double goal_reached_tol_{}; + double max_pitch_angle_{}; types::ActiveLosMethod method_{}; // Guidance Modules diff --git a/guidance/los_guidance/launch/guidance_test.launch.py b/guidance/los_guidance/launch/guidance_test.launch.py index 03d404982..38a50e97c 100644 --- a/guidance/los_guidance/launch/guidance_test.launch.py +++ b/guidance/los_guidance/launch/guidance_test.launch.py @@ -35,14 +35,14 @@ def generate_launch_description(): ) operation_mode_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join( - get_package_share_directory("operation_mode_manager"), - "launch", - "operation_mode_manager.launch.py", + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("operation_mode_manager"), + "launch", + "operation_mode_manager.launch.py", + ) ) ) -) los_guidance_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 40210dae6..372152025 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -252,7 +252,9 @@ void LosGuidanceNode::set_los_mode( vortex_msgs::msg::LOSGuidance LosGuidanceNode::fill_los_reference( types::Outputs outputs) { vortex_msgs::msg::LOSGuidance reference_msg; - reference_msg.pitch = outputs.theta_d; + const double clamped_pitch = + std::clamp(outputs.theta_d, -max_pitch_angle_, max_pitch_angle_); + reference_msg.pitch = clamped_pitch; reference_msg.yaw = outputs.psi_d; reference_msg.surge = u_desired_; @@ -267,6 +269,7 @@ YAML::Node LosGuidanceNode::get_los_config(std::string yaml_file_path) { void LosGuidanceNode::parse_common_config(YAML::Node common_config) { std::lock_guard lock(mutex_); u_desired_ = common_config["u_desired"].as(); + max_pitch_angle_ = common_config["max_pitch_angle"].as(); goal_reached_tol_ = common_config["goal_reached_tol"].as(); method_ = static_cast( common_config["active_los_method"].as()); From 7f7fdded7ae295fa87014103ff623ef5f37c4aef Mon Sep 17 00:00:00 2001 From: Anbit Date: Mon, 9 Mar 2026 15:17:11 +0100 Subject: [PATCH 080/128] Fix some duplicate issues --- guidance/los_guidance/launch/guidance_test.launch.py | 4 ++-- guidance/los_guidance/src/los_guidance_ros.cpp | 11 +++++------ 2 files changed, 7 insertions(+), 8 deletions(-) diff --git a/guidance/los_guidance/launch/guidance_test.launch.py b/guidance/los_guidance/launch/guidance_test.launch.py index 38a50e97c..3a85dfc7d 100644 --- a/guidance/los_guidance/launch/guidance_test.launch.py +++ b/guidance/los_guidance/launch/guidance_test.launch.py @@ -21,8 +21,8 @@ def generate_launch_description(): os.path.join(stonefish_dir, 'launch', 'simulation.launch.py') ), launch_arguments={ - 'scenario': 'tacc', - 'rendering': 'false', + 'scenario': 'default', + 'rendering': 'true', }.items(), ) diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 372152025..de851d950 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -334,16 +334,16 @@ void LosGuidanceNode::execute( switch (method_) { case types::ActiveLosMethod::ADAPTIVE: - outputs = adaptive_los_->calculate_outputs(path_inputs_); + outputs = adaptive_los_->calculate_outputs(inputs_copy); break; case types::ActiveLosMethod::PROPORTIONAL: - outputs = proportional_los_->calculate_outputs(path_inputs_); + outputs = proportional_los_->calculate_outputs(inputs_copy); break; case types::ActiveLosMethod::INTEGRAL: - outputs = integral_los_->calculate_outputs(path_inputs_); + outputs = integral_los_->calculate_outputs(inputs_copy); break; case types::ActiveLosMethod::VECTOR_FIELD: - outputs = vector_field_los_->calculate_outputs(path_inputs_); + outputs = vector_field_los_->calculate_outputs(inputs_copy); break; default: spdlog::error("Invalid LOS method selected"); @@ -375,8 +375,7 @@ void LosGuidanceNode::execute( state_debug_pub_->publish(state_debug_msg); goal_handle->publish_feedback(feedback); reference_pub_->publish(reference_msg); - - if ((path_inputs_.current_position - path_inputs_.next_point) + if ((inputs_copy.current_position - inputs_copy.next_point) .as_vector() .norm() < goal_reached_tol_) { auto stop_ref = reference_msg; From f64e566ebe06a60fc58fc9d1c63c31b447f338ec Mon Sep 17 00:00:00 2001 From: Anbit Date: Mon, 9 Mar 2026 15:44:40 +0100 Subject: [PATCH 081/128] Fix max_pitch to 55 degres --- guidance/los_guidance/config/guidance_params.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 6dd71b6a4..0914cd9ff 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -29,7 +29,7 @@ common: active_los_method: 2 # 0: Adaptive LOS, 1: Proportional LOS, 2: Integral LOS, 3: Vector Field LOS u_desired: 0.3 goal_reached_tol: 0.35 - max_pitch_angle: 0.785 # 45 degrees + max_pitch_angle: 0.96 # 55 degrees # Debug Settings debug: From 7a39ece3d4571a45643e6505dbe83a65d64400ec Mon Sep 17 00:00:00 2001 From: Anbit Date: Thu, 12 Mar 2026 16:09:42 +0100 Subject: [PATCH 082/128] Fix: merge issiue --- .../launch/los_guidance.launch.py | 62 ++++++++++++------- 1 file changed, 40 insertions(+), 22 deletions(-) diff --git a/guidance/los_guidance/launch/los_guidance.launch.py b/guidance/los_guidance/launch/los_guidance.launch.py index bf653b641..5ee9a3d1d 100644 --- a/guidance/los_guidance/launch/los_guidance.launch.py +++ b/guidance/los_guidance/launch/los_guidance.launch.py @@ -5,29 +5,47 @@ from launch.actions import OpaqueFunction from launch_ros.actions import Node -adapt_params = path.join( - get_package_share_directory("los_guidance"), - "config", - "guidance_params.yaml", -) -orca_params = path.join( - get_package_share_directory("auv_setup"), - "config", - "robots", - "orca.yaml", +from auv_setup.launch_arg_common import ( + declare_drone_and_namespace_args, + resolve_drone_and_namespace, ) -def generate_launch_description(): - los_guidance_node = Node( - package="los_guidance", - executable="los_guidance_node", - name="los_guidance_node", - namespace="orca", - parameters=[ - orca_params, - adapt_params, - ], - output="screen", +def launch_setup(context, *args, **kwargs): + drone, namespace = resolve_drone_and_namespace(context) + + guidance_config = os.path.join( + get_package_share_directory("los_guidance"), + "config", + "guidance_params.yaml", + ) + + drone_params = os.path.join( + get_package_share_directory("auv_setup"), + "config", + "robots", + f"{drone}.yaml", ) - return LaunchDescription([los_guidance_node]) + + return [ + Node( + package="los_guidance", + executable="los_guidance_node", + name="los_guidance_node", + namespace=namespace, + parameters=[ + drone_params, + { + "los_config_file": guidance_config, + "time_step": 0.1, + }, + ], + output="screen", + ) + ] + + +def generate_launch_description(): + return LaunchDescription( + declare_drone_and_namespace_args() + [OpaqueFunction(function=launch_setup)] + ) \ No newline at end of file From 0431417b0da2021f77e7b2088f60890f2251b63f Mon Sep 17 00:00:00 2001 From: Anbit Date: Mon, 16 Mar 2026 13:13:55 +0100 Subject: [PATCH 083/128] Start: Test libraary for los --- guidance/los_guidance/CMakeLists.txt | 2 +- .../launch/guidance_test.launch.py | 103 +++++++----- guidance/los_guidance/scripts/square_test.py | 90 ----------- .../los_guidance/scripts/test_scenarios.py | 150 ++++++++++++++++++ 4 files changed, 217 insertions(+), 128 deletions(-) delete mode 100644 guidance/los_guidance/scripts/square_test.py create mode 100644 guidance/los_guidance/scripts/test_scenarios.py diff --git a/guidance/los_guidance/CMakeLists.txt b/guidance/los_guidance/CMakeLists.txt index d6bdb26bf..00c00a391 100644 --- a/guidance/los_guidance/CMakeLists.txt +++ b/guidance/los_guidance/CMakeLists.txt @@ -87,7 +87,7 @@ install(DIRECTORY ) install(PROGRAMS - scripts/square_test.py + scripts/test_scenarios.py DESTINATION share/${PROJECT_NAME}/scripts ) diff --git a/guidance/los_guidance/launch/guidance_test.launch.py b/guidance/los_guidance/launch/guidance_test.launch.py index 3a85dfc7d..fa78df486 100644 --- a/guidance/los_guidance/launch/guidance_test.launch.py +++ b/guidance/los_guidance/launch/guidance_test.launch.py @@ -3,59 +3,74 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import ( + DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription, + OpaqueFunction, TimerAction, ) from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from auv_setup.launch_arg_common import ( + declare_drone_and_namespace_args, + resolve_drone_and_namespace, +) -def generate_launch_description(): - stonefish_dir = get_package_share_directory('stonefish_sim') - vortex_sim_interface_dir = get_package_share_directory('vortex_sim_interface') - los_guidance_dir = get_package_share_directory('los_guidance') - velocity_controller_dir = get_package_share_directory('velocity_controller_lqr') + +def launch_setup(context, *args, **kwargs): + drone, namespace = resolve_drone_and_namespace(context) + test_scenario = LaunchConfiguration("test_scenario").perform(context) + + stonefish_dir = get_package_share_directory("stonefish_sim") + vortex_sim_interface_dir = get_package_share_directory("vortex_sim_interface") + los_guidance_dir = get_package_share_directory("los_guidance") + operation_mode_manager_dir = get_package_share_directory("operation_mode_manager") stonefish_sim = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(stonefish_dir, 'launch', 'simulation.launch.py') + os.path.join(stonefish_dir, "launch", "simulation.launch.py") ), launch_arguments={ - 'scenario': 'default', - 'rendering': 'true', + "scenario": "default", + "rendering": "false", }.items(), ) vortex_sim_interface = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join( - vortex_sim_interface_dir, 'launch', 'vortex_sim_interface.launch.py' + vortex_sim_interface_dir, "launch", "vortex_sim_interface.launch.py" ) - ) + ), + launch_arguments={ + "drone": drone, + "namespace": namespace, + }.items(), ) operation_mode_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join( - get_package_share_directory("operation_mode_manager"), + operation_mode_manager_dir, "launch", "operation_mode_manager.launch.py", ) - ) + ), + launch_arguments={ + "drone": drone, + "namespace": namespace, + }.items(), ) los_guidance_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(los_guidance_dir, 'launch', 'los_guidance.launch.py') - ) - ) - - velocity_controller_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join( - velocity_controller_dir, 'launch', 'velocity_controller_lqr.launch.py' - ) - ) + os.path.join(los_guidance_dir, "launch", "los_guidance.launch.py") + ), + launch_arguments={ + "drone": drone, + "namespace": namespace, + }.items(), ) orca_sim = TimerAction( @@ -63,7 +78,7 @@ def generate_launch_description(): actions=[ IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(stonefish_dir, 'launch', 'orca_sim.launch.py') + os.path.join(stonefish_dir, "launch", "orca_sim.launch.py") ) ) ], @@ -77,11 +92,11 @@ def generate_launch_description(): "bash", "-lc", ( - "ros2 service call /orca/set_killswitch " + f"ros2 service call /{namespace}/set_killswitch " "vortex_msgs/srv/SetKillswitch " "\"{killswitch_on: false}\" " "&& " - "ros2 service call /orca/set_operation_mode " + f"ros2 service call /{namespace}/set_operation_mode " "vortex_msgs/srv/SetOperationMode " "\"{requested_operation_mode: {operation_mode: 1}}\"" ), @@ -91,29 +106,43 @@ def generate_launch_description(): ], ) - square_test = TimerAction( + test_scenarios = TimerAction( period=20.0, actions=[ ExecuteProcess( cmd=[ "bash", "-lc", - f"python3 {os.path.join(los_guidance_dir, 'scripts', 'square_test.py')}", + ( + f"python3 {os.path.join(los_guidance_dir, 'scripts', 'test_scenario.py')} " + f"--ros-args -p drone:={drone} -p test_scenario:={test_scenario}" + ), ], output="screen", ) ], ) + return [ + stonefish_sim, + vortex_sim_interface, + operation_mode_launch, + los_guidance_launch, + orca_sim, + set_autonomy, + test_scenarios, + ] + + +def generate_launch_description(): return LaunchDescription( - [ - stonefish_sim, - vortex_sim_interface, - operation_mode_launch, - los_guidance_launch, - velocity_controller_launch, - orca_sim, - set_autonomy, - square_test, + declare_drone_and_namespace_args(default_drone="orca") + + [ + DeclareLaunchArgument( + "test_scenario", + default_value="square", + description="Scenario to run: square, circle, test_pitch, opposite_point", + ), + OpaqueFunction(function=launch_setup), ] - ) + ) \ No newline at end of file diff --git a/guidance/los_guidance/scripts/square_test.py b/guidance/los_guidance/scripts/square_test.py deleted file mode 100644 index 954530849..000000000 --- a/guidance/los_guidance/scripts/square_test.py +++ /dev/null @@ -1,90 +0,0 @@ -import rclpy -from rclpy.action import ActionClient -from rclpy.node import Node -from std_msgs.msg import Header -from vortex_msgs.action import LOSGuidance - - -class SquareTest(Node): - def __init__(self): - super().__init__('square_test_client') - - self.get_logger().info("Square test started") - - self._action_client = ActionClient(self, LOSGuidance, '/orca/los_guidance') - - self.depth = 2.5 - self.size = 10.0 - - self.waypoints = [ - (self.size, 0.0, self.depth), - (self.size, self.size, self.depth), - (0.0, self.size, self.depth), - (0.0, 0.0, self.depth), - ] - - self.current_index = 0 - - self.send_next_goal() - - def send_next_goal(self): - if self.current_index >= len(self.waypoints): - self.get_logger().info("Square test completed!") - rclpy.shutdown() - return - - self._action_client.wait_for_server() - - goal_msg = LOSGuidance.Goal() - - header = Header() - header.frame_id = "world_ned" - - goal_msg.goal.header = header - - x, y, z = self.waypoints[self.current_index] - - goal_msg.goal.point.x = x - goal_msg.goal.point.y = y - goal_msg.goal.point.z = z - - self.get_logger().info( - f"Sending waypoint {self.current_index + 1}: x={x}, y={y}, z={z}" - ) - - self._send_goal_future = self._action_client.send_goal_async( - goal_msg, feedback_callback=self.feedback_callback - ) - - self._send_goal_future.add_done_callback(self.goal_response_callback) - - def goal_response_callback(self, future): - goal_handle = future.result() - - if not goal_handle.accepted: - self.get_logger().info('Goal rejected') - return - - self.get_logger().info('Goal accepted') - - self._get_result_future = goal_handle.get_result_async() - self._get_result_future.add_done_callback(self.result_callback) - - def result_callback(self, future): - self.get_logger().info("Waypoint reached") - - self.current_index += 1 - self.send_next_goal() - - def feedback_callback(self, feedback_msg): - pass - - -def main(args=None): - rclpy.init(args=args) - node = SquareTest() - rclpy.spin(node) - - -if __name__ == '__main__': - main() diff --git a/guidance/los_guidance/scripts/test_scenarios.py b/guidance/los_guidance/scripts/test_scenarios.py new file mode 100644 index 000000000..0d6a2d328 --- /dev/null +++ b/guidance/los_guidance/scripts/test_scenarios.py @@ -0,0 +1,150 @@ +import math + +import rclpy +from rclpy.action import ActionClient +from rclpy.node import Node +from std_msgs.msg import Header +from vortex_msgs.action import LOSGuidance + + +class WaypointTest(Node): + def __init__(self): + super().__init__("waypoint_test_client") + + self.declare_parameter("test_scenario", "square") + self.declare_parameter("drone", "orca") + + self.test_scenario = self.get_parameter("test_scenario").value + self.drone = self.get_parameter("drone").value + + self._action_client = ActionClient( + self, + LOSGuidance, + f"/{self.drone}/los_guidance", + ) + + self.depth = 2.5 + self.square_size = 10.0 + + self.circle_radius = 8.0 + self.circle_points = 16 + self.circle_center_x = 0.0 + self.circle_center_y = 0.0 + + self.waypoints = self.generate_waypoints(self.test_scenario) + self.current_index = 0 + + self.get_logger().info(f"Starting test scenario: {self.test_scenario}") + self.get_logger().info(f"Using drone namespace: {self.drone}") + self.get_logger().info(f"Number of waypoints: {len(self.waypoints)}") + + self.send_next_goal() + + def generate_waypoints(self, test_scenario): + if test_scenario == "square": + s = self.square_size + d = self.depth + return [ + (s, 0.0, d), + (s, s, d), + (0.0, s, d), + (0.0, 0.0, d), + ] + + elif test_scenario == "circle": + d = self.depth + waypoints = [] + + for i in range(self.circle_points): + theta = 2.0 * math.pi * i / self.circle_points + x = self.circle_center_x + self.circle_radius * math.cos(theta) + y = self.circle_center_y + self.circle_radius * math.sin(theta) + waypoints.append((x, y, d)) + + waypoints.append(waypoints[0]) + return waypoints + + elif test_scenario == "test_pitch": + # 0 = water surface, do not go above + # 3 = seabed/ground, do not touch + # Keep all depths safely between these + return [ + (3.0, 0.0, 1.0), # slight up + (6.0, 0.0, 2.0), # slight down + (9.0, 0.0, 1.0), # up again + (12.0, 0.0, 2.0), # down again + ] + + elif test_scenario == "opposite_point": + # Go to one point, then the exact opposite point + return [ + (6.0, 4.0, self.depth), + (-6.0, -4.0, self.depth), + ] + + else: + self.get_logger().warn( + f"Unknown test_scenario '{test_scenario}', defaulting to square" + ) + return self.generate_waypoints("square") + + def send_next_goal(self): + if self.current_index >= len(self.waypoints): + self.get_logger().info(f"{self.test_scenario} test completed!") + rclpy.shutdown() + return + + self._action_client.wait_for_server() + + goal_msg = LOSGuidance.Goal() + + header = Header() + header.frame_id = "world_ned" + goal_msg.goal.header = header + + x, y, z = self.waypoints[self.current_index] + goal_msg.goal.point.x = float(x) + goal_msg.goal.point.y = float(y) + goal_msg.goal.point.z = float(z) + + self.get_logger().info( + f"Sending waypoint {self.current_index + 1}/{len(self.waypoints)}: " + f"x={x:.2f}, y={y:.2f}, z={z:.2f}" + ) + + self._send_goal_future = self._action_client.send_goal_async( + goal_msg, + feedback_callback=self.feedback_callback, + ) + self._send_goal_future.add_done_callback(self.goal_response_callback) + + def goal_response_callback(self, future): + goal_handle = future.result() + + if not goal_handle.accepted: + self.get_logger().error("Goal rejected") + rclpy.shutdown() + return + + self.get_logger().info("Goal accepted") + + self._get_result_future = goal_handle.get_result_async() + self._get_result_future.add_done_callback(self.result_callback) + + def result_callback(self, future): + self.get_logger().info("Waypoint reached") + self.current_index += 1 + self.send_next_goal() + + def feedback_callback(self, feedback_msg): + pass + + +def main(args=None): + rclpy.init(args=args) + node = WaypointTest() + rclpy.spin(node) + + +if __name__ == "__main__": + main() \ No newline at end of file From f495f8b4fc88d9891c70334ddca15598967415c3 Mon Sep 17 00:00:00 2001 From: Henrik Date: Mon, 16 Mar 2026 17:46:03 +0100 Subject: [PATCH 084/128] Initial fixes --- .../velocity_controller/config/parameters.yaml | 7 +++++-- .../src/velocity_controller.cpp | 2 +- .../los_guidance/launch/guidance_test.launch.py | 17 +++++++++-------- 3 files changed, 15 insertions(+), 11 deletions(-) diff --git a/control/velocity_controller/config/parameters.yaml b/control/velocity_controller/config/parameters.yaml index 88ad626bc..2fefc0f78 100644 --- a/control/velocity_controller/config/parameters.yaml +++ b/control/velocity_controller/config/parameters.yaml @@ -5,13 +5,13 @@ odom_topic: /orca/odom #Odometry twist_topic: /dvl/twist #Twist pose_topic: /dvl/pose #Pose - guidance_topic: /guidance/los #Guidance + guidance_topic: /orca/guidance/los #Guidance thrust_topic: /orca/wrench_input #Thrust softwareoperation_topic: /softwareOperationMode #Software Operation killswitch_topic: /softwareKillSwitch #Kill Switch LQR_params: - Q: [300.0,32.84,32.84,32.84,32.84,100.0,32.84,32.84] + Q: [200.0,32.84,32.84,15.0,15.0,100.0,32.84,32.84] R: [0.02,3.1,3.10] NMPCA_params: Q: [1.0,1.0,1.0,5.0,5.0] # u,q,r,theta,psi @@ -33,3 +33,6 @@ #Q: [300.0,0.01,0.01,0.01,32.84,32.84,32.84,32.84,32.84] # u,v,w,p,q,r,phi,theta,psi # R: [0.02,3.1,3.10] # u_surge, u_theta, u_psi + + #Fixes: reduce oscillations, follows angles close to wrap around, model restoring forces in pitch, test different references, try to find out why the fuck it went backwards?, + diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index d67889708..31b746731 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -19,7 +19,7 @@ //Konstruktør -Velocity_node::Velocity_node() : Node("velocity_controller_node"), PID_surge(600,50,0), PID_yaw(120,20,0), PID_pitch(35,1,0), lqr_controller() +Velocity_node::Velocity_node() : Node("velocity_controller_node"), PID_surge(300,10,5), PID_yaw(60,8,5), PID_pitch(10,1,3), lqr_controller() { //Dytter info til log RCLCPP_INFO(this->get_logger(), "Velocity control node has been started."); diff --git a/guidance/los_guidance/launch/guidance_test.launch.py b/guidance/los_guidance/launch/guidance_test.launch.py index 3a85dfc7d..93328c679 100644 --- a/guidance/los_guidance/launch/guidance_test.launch.py +++ b/guidance/los_guidance/launch/guidance_test.launch.py @@ -14,7 +14,7 @@ def generate_launch_description(): stonefish_dir = get_package_share_directory('stonefish_sim') vortex_sim_interface_dir = get_package_share_directory('vortex_sim_interface') los_guidance_dir = get_package_share_directory('los_guidance') - velocity_controller_dir = get_package_share_directory('velocity_controller_lqr') + velocity_controller_dir = get_package_share_directory('velocity_controller') stonefish_sim = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -51,12 +51,12 @@ def generate_launch_description(): ) velocity_controller_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join( - velocity_controller_dir, 'launch', 'velocity_controller_lqr.launch.py' + PythonLaunchDescriptionSource( + os.path.join( + velocity_controller_dir, 'launch', 'velocity_controller.launch.py' + ) + ) ) - ) - ) orca_sim = TimerAction( period=12.0, @@ -111,9 +111,10 @@ def generate_launch_description(): vortex_sim_interface, operation_mode_launch, los_guidance_launch, - velocity_controller_launch, orca_sim, set_autonomy, - square_test, + velocity_controller_launch, + #square_test, + ] ) From 2e73cff3c439c996097622585f9c0789c45207d9 Mon Sep 17 00:00:00 2001 From: Henrik Date: Thu, 19 Mar 2026 14:16:19 +0100 Subject: [PATCH 085/128] changed to lifecycle node --- control/velocity_controller/CMakeLists.txt | 5 + .../velocity_controller.hpp | 19 ++- .../launch/velocity_controller.launch.py | 6 +- control/velocity_controller/package.xml | 2 + .../src/velocity_controller.cpp | 142 +++++++++++------- 5 files changed, 111 insertions(+), 63 deletions(-) diff --git a/control/velocity_controller/CMakeLists.txt b/control/velocity_controller/CMakeLists.txt index 8247d1976..9abb901a0 100644 --- a/control/velocity_controller/CMakeLists.txt +++ b/control/velocity_controller/CMakeLists.txt @@ -12,6 +12,8 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +#find_package(lifecycle_msgs REQUIRED) find_package(std_msgs REQUIRED) find_package(vortex_msgs REQUIRED) find_package(vortex_utils REQUIRED) @@ -74,6 +76,8 @@ add_executable(velocity_controller_node # ) ament_target_dependencies(velocity_controller_node rclcpp + rclcpp_lifecycle + #rclcpp_msgs std_msgs vortex_msgs geometry_msgs @@ -120,6 +124,7 @@ target_link_libraries(test_VC_node Eigen3::Eigen ct_optcon ct_core) ament_target_dependencies(test_VC_node rclcpp +rclcpp_lifecycle std_msgs vortex_msgs geometry_msgs diff --git a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp index 87741af89..06de8b774 100644 --- a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp +++ b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp @@ -13,21 +13,22 @@ #include "vortex_msgs/msg/los_guidance.hpp" #include "velocity_controller/NMPC_setup.hpp" #include "velocity_controller/NMPC_acados.hpp" +#include -class Velocity_node : public rclcpp::Node{ + +class Velocity_node : public rclcpp_lifecycle::LifecycleNode{ public: Velocity_node(); //Different initializatin functions void get_new_parameters(); //Timer functions - void publish_thrust(); void calc_thrust(); //Callback functions void guidance_callback(const vortex_msgs::msg::LOSGuidance::SharedPtr msg_ptr); - void killswitch_callback(const std_msgs::msg::Bool::SharedPtr msg_ptr); + //void killswitch_callback(const std_msgs::msg::Bool::SharedPtr msg_ptr); void odometry_callback(const nav_msgs::msg::Odometry::SharedPtr msg_ptr); //Publisher instance @@ -40,7 +41,7 @@ class Velocity_node : public rclcpp::Node{ //Subscriber instance rclcpp::Subscription::SharedPtr subscriber_Odometry; rclcpp::Subscription::SharedPtr subscriber_guidance; - rclcpp::Subscription::SharedPtr subscriber_killswitch; + //rclcpp::Subscription::SharedPtr subscriber_killswitch; //Variables for topics @@ -87,10 +88,14 @@ class Velocity_node : public rclcpp::Node{ std::vector Q3; std::vector R3; - //Test - rclcpp::Publisher::SharedPtr publisher_reference; - + std::atomic_bool should_exit_{false}; + //States + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure(const rclcpp_lifecycle::State &) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; }; diff --git a/control/velocity_controller/launch/velocity_controller.launch.py b/control/velocity_controller/launch/velocity_controller.launch.py index 148a697a0..f536fa356 100644 --- a/control/velocity_controller/launch/velocity_controller.launch.py +++ b/control/velocity_controller/launch/velocity_controller.launch.py @@ -9,7 +9,9 @@ def generate_launch_description(): pkg_share = get_package_share_directory('velocity_controller') - config_path = os.path.join(pkg_share, 'config', 'parameters.yaml') + global_share = get_package_share_directory('auv_setup') + config_path_local = os.path.join(pkg_share, 'config', 'parameters.yaml') + config_path_global = os.path.join(global_share,'config','robots','orca.yaml') node_name_arg = DeclareLaunchArgument( 'node_name', default_value='velocity_controller_node', @@ -25,5 +27,5 @@ def generate_launch_description(): executable='velocity_controller_node', name=velocity_controller_name, output='screen', - parameters=[config_path]) + parameters=[config_path_local,config_path_global]) ]) \ No newline at end of file diff --git a/control/velocity_controller/package.xml b/control/velocity_controller/package.xml index 897f5f19f..d9acc271e 100644 --- a/control/velocity_controller/package.xml +++ b/control/velocity_controller/package.xml @@ -17,6 +17,8 @@ ct_optcon ct_core vortex_utils + rclcpp_lifecycle + ament_cmake_gtest diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index 31b746731..45e6a9053 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -12,63 +12,32 @@ #include #include #include +#include #include #include "vortex_msgs/msg/los_guidance.hpp" #include "vortex/utils/math.hpp" #include "velocity_controller/utilities.hpp" +#include //Konstruktør -Velocity_node::Velocity_node() : Node("velocity_controller_node"), PID_surge(300,10,5), PID_yaw(60,8,5), PID_pitch(10,1,3), lqr_controller() +Velocity_node::Velocity_node() : rclcpp_lifecycle::LifecycleNode("velocity_controller_lifecycle"), PID_surge(300,10,5), PID_yaw(60,8,5), PID_pitch(10,1,3), lqr_controller() { - //Dytter info til log RCLCPP_INFO(this->get_logger(), "Velocity control node has been started."); - - //Parameter from config. get_new_parameters(); - - - // Publishers - use TRANSIENT_LOCAL for internal topics - rclcpp::QoS pub_QoS(10); - pub_QoS.keep_last(10).reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT).durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); - - publisher_thrust = create_publisher(topic_thrust, pub_QoS); - publisher_reference = create_publisher("/reference", pub_QoS); - - //Subscribers - use VOLATILE for external topics (simulator, sensors) - rclcpp::QoS sub_QoS(10); - sub_QoS.keep_last(10).reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT).durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); - - subscriber_Odometry = this->create_subscription( - topic_odometry,sub_QoS, - std::bind(&Velocity_node::odometry_callback,this,std::placeholders::_1)); - subscriber_guidance = this->create_subscription( - topic_guidance,sub_QoS, - std::bind(&Velocity_node::guidance_callback,this, std::placeholders::_1)); - subscriber_killswitch = this->create_subscription( - topic_killswitch,sub_QoS, - std::bind(&Velocity_node::killswitch_callback,this, std::placeholders::_1)); - - - //NMPC controller - + //NMPC controller NMPC.set_matrices(Q3,R3, inertia_matrix, max_force, dampening_matrix_low, dampening_matrix_high); NMPC.set_interval(publish_rate/1000.0); - //NMPC.initialize_MPC(); - + //NMPC.initialize_MPC(); //NMPC acados controller NMPC_acados.init(); NMPC_acados.set_max_force(max_force); std::vector W=Q2; W.insert(W.end(),R2.begin(),R2.end()); std::vector We=Q2; - NMPC_acados.set_weights(W, We); - //Timer - timer_calculation = this->create_wall_timer(std::chrono::milliseconds(publish_rate), std::bind(&Velocity_node::calc_thrust, this)); - timer_publish = this->create_wall_timer(std::chrono::milliseconds(publish_rate), std::bind(&Velocity_node::publish_thrust, this)); //Controllers PID_surge.set_output_limits(-max_force, max_force); PID_pitch.set_output_limits(-max_force, max_force); @@ -77,21 +46,19 @@ Velocity_node::Velocity_node() : Node("velocity_controller_node"), PID_surge(300 controller_type=1; RCLCPP_INFO(this->get_logger(),"Switching to PID"); }; + return; } -//Publish/timer functions -void Velocity_node::publish_thrust(){ - //RCLCPP_INFO(this->get_logger(),"sending thrust"); - publisher_thrust->publish(thrust_out); -} + //** må forbedre integrasjon og derivasjons beregningene void Velocity_node::calc_thrust() { + RCLCPP_INFO(get_logger(),"Calculating thrust"); angle NED_error={guidance_values.roll-current_state.roll,guidance_values.pitch-current_state.pitch,guidance_values.yaw-current_state.yaw}; angle error=NED_to_BODY(NED_error,current_state); Guidance_data mod_g_values=guidance_values; @@ -172,9 +139,7 @@ void Velocity_node::calc_thrust() break; } } - std_msgs::msg::Float64MultiArray msg; - msg.data={guidance_values.surge,guidance_values.pitch,guidance_values.yaw}; - publisher_reference->publish(msg); + publisher_thrust->publish(thrust_out); return; } @@ -183,14 +148,13 @@ void Velocity_node::calc_thrust() //Callback functions void Velocity_node::guidance_callback(const vortex_msgs::msg::LOSGuidance::SharedPtr msg_ptr){ guidance_values = *msg_ptr; - //RCLCPP_INFO(this->get_logger(), "Guidance received: surge=%.3f pitch=%.3f yaw=%.3f", - // guidance_values.surge, guidance_values.pitch, guidance_values.yaw); + RCLCPP_INFO(this->get_logger(), "Guidance received: surge=%.3f pitch=%.3f yaw=%.3f",guidance_values.surge, guidance_values.pitch, guidance_values.yaw); //RCLCPP_INFO(this->get_logger(),"message: s: %f, p:%f, y:%f", msg_ptr->surge,msg_ptr->pitch,msg_ptr->yaw); return; } void Velocity_node::odometry_callback(const nav_msgs::msg::Odometry::SharedPtr msg_ptr){ - //RCLCPP_INFO(this->get_logger(),"Recieved odometry"); + RCLCPP_INFO(this->get_logger(),"Recieved odometry"); angle temp=quaternion_to_euler_angle(msg_ptr->pose.pose.orientation.w, msg_ptr->pose.pose.orientation.x, msg_ptr->pose.pose.orientation.y, msg_ptr->pose.pose.orientation.z); //angles current_state.roll = temp.phit; @@ -208,7 +172,7 @@ void Velocity_node::odometry_callback(const nav_msgs::msg::Odometry::SharedPtr m } //**Needs to update to shutdown the node -void Velocity_node::killswitch_callback(const std_msgs::msg::Bool::SharedPtr msg_ptr){ +/*void Velocity_node::killswitch_callback(const std_msgs::msg::Bool::SharedPtr msg_ptr){ RCLCPP_INFO(this->get_logger(), "Received killswitch: '%d'", msg_ptr->data); if(msg_ptr->data == true){ guidance_values = Guidance_data(); @@ -216,10 +180,11 @@ void Velocity_node::killswitch_callback(const std_msgs::msg::Bool::SharedPtr msg RCLCPP_INFO(this->get_logger(), "Killswitch activated, reference and current state set to zero"); } return; -} +}*/ void Velocity_node::get_new_parameters(){ + //topics this->declare_parameter("topics.thrust_topic"); this->topic_thrust = this->get_parameter("topics.thrust_topic").as_string(); this->declare_parameter("topics.guidance_topic"); @@ -228,6 +193,7 @@ void Velocity_node::get_new_parameters(){ this->topic_odometry = this->get_parameter("topics.odom_topic").as_string(); this->declare_parameter("topics.killswitch_topic"); this->topic_killswitch = this->get_parameter("topics.killswitch_topic").as_string(); + //variables this->declare_parameter("max_force"); this->max_force = this->get_parameter("max_force").as_double(); this->declare_parameter("publish_rate"); @@ -264,14 +230,82 @@ void Velocity_node::get_new_parameters(){ } +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Velocity_node::on_configure(const rclcpp_lifecycle::State &){ + RCLCPP_INFO(get_logger(), "Configure VC"); + + // Publishers + rclcpp::QoS pub_QoS(10); + pub_QoS.keep_last(10).reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT).durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); + publisher_thrust = create_publisher(topic_thrust, pub_QoS); + + //Subscribers + rclcpp::QoS sub_QoS(10); + sub_QoS.keep_last(10).reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT).durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); + subscriber_Odometry = this->create_subscription( topic_odometry,sub_QoS, std::bind(&Velocity_node::odometry_callback,this,std::placeholders::_1)); + subscriber_guidance = this->create_subscription( topic_guidance,sub_QoS,std::bind(&Velocity_node::guidance_callback,this, std::placeholders::_1)); + //subscriber_killswitch = this->create_subscription(topic_killswitch,sub_QoS,std::bind(&Velocity_node::killswitch_callback,this, std::placeholders::_1)); + //Timer + return CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +Velocity_node::on_activate(const rclcpp_lifecycle::State & state) +{ + RCLCPP_INFO(get_logger(), "Activating..."); + timer_calculation = this->create_wall_timer(std::chrono::milliseconds(publish_rate), std::bind(&Velocity_node::calc_thrust, this)); + //LifecycleNode::on_activate(state); + //timer_calculation->reset(); + + + return CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +Velocity_node::on_deactivate(const rclcpp_lifecycle::State & state) +{ + RCLCPP_INFO(get_logger(), "Deactivating..."); + auto ret = LifecycleNode::on_deactivate(state); + //timer_calculation->cancel(); + return ret; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +Velocity_node::on_cleanup(const rclcpp_lifecycle::State &) +{ + RCLCPP_INFO(get_logger(), "Cleaning up..."); + timer_calculation.reset(); + publisher_thrust.reset(); + subscriber_guidance.reset(); + subscriber_Odometry.reset(); + return CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +Velocity_node::on_shutdown(const rclcpp_lifecycle::State & state) +{ + RCLCPP_INFO(get_logger(), "Shutting down from state %s", state.label().c_str()); + if(timer_calculation) timer_calculation->cancel(); + timer_calculation.reset(); + publisher_thrust.reset(); + subscriber_guidance.reset(); + subscriber_Odometry.reset(); + should_exit_=true; + return CallbackReturn::SUCCESS; +} + + int main(int argc, char * argv[]) { rclcpp::init(argc, argv); - auto node = std::make_shared(); - rclcpp::executors::MultiThreadedExecutor exec; - exec.add_node(node); - exec.spin(); - rclcpp::shutdown(); + auto lc_node = std::make_shared(); + + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(lc_node->get_node_base_interface()); + + while (rclcpp::ok()&&!lc_node->should_exit_){ + exec.spin_some(); + } + //rclcpp::shutdown(); return 0; } From b6f144b7aa7a0faaff7dd68d746108f12cc1df08 Mon Sep 17 00:00:00 2001 From: Henrik Date: Thu, 19 Mar 2026 14:21:35 +0100 Subject: [PATCH 086/128] changed to global parameter file --- control/velocity_controller/src/velocity_controller.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index 45e6a9053..25b885fff 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -58,7 +58,7 @@ Velocity_node::Velocity_node() : rclcpp_lifecycle::LifecycleNode("velocity_contr //** må forbedre integrasjon og derivasjons beregningene void Velocity_node::calc_thrust() { - RCLCPP_INFO(get_logger(),"Calculating thrust"); + //RCLCPP_INFO(get_logger(),"Calculating thrust"); angle NED_error={guidance_values.roll-current_state.roll,guidance_values.pitch-current_state.pitch,guidance_values.yaw-current_state.yaw}; angle error=NED_to_BODY(NED_error,current_state); Guidance_data mod_g_values=guidance_values; @@ -208,8 +208,8 @@ void Velocity_node::get_new_parameters(){ Q=this->get_parameter("LQR_params.Q").as_double_array(); this->declare_parameter>("LQR_params.R"); R=this->get_parameter("LQR_params.R").as_double_array(); - this->declare_parameter>("inertia_matrix"); - this->get_parameter("inertia_matrix", inertia_matrix); + this->declare_parameter>("physical.mass_matrix"); + this->get_parameter("physical.mass_matrix", inertia_matrix); //D this->declare_parameter>("dampening_matrix_low"); From c6a20477a8d6d385c965ecc6c73970476d90d6ab Mon Sep 17 00:00:00 2001 From: Henrik Date: Thu, 19 Mar 2026 14:37:10 +0100 Subject: [PATCH 087/128] Changed more to global paramter file --- .../src/velocity_controller.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index 25b885fff..e5ee3e75c 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -184,15 +184,15 @@ void Velocity_node::odometry_callback(const nav_msgs::msg::Odometry::SharedPtr m void Velocity_node::get_new_parameters(){ - //topics - this->declare_parameter("topics.thrust_topic"); - this->topic_thrust = this->get_parameter("topics.thrust_topic").as_string(); - this->declare_parameter("topics.guidance_topic"); - this->topic_guidance = this->get_parameter("topics.guidance_topic").as_string(); - this->declare_parameter("topics.odom_topic"); - this->topic_odometry = this->get_parameter("topics.odom_topic").as_string(); + //topics //TODO: check what happens when same parameter in global and local file + this->declare_parameter("topics.wrench_input"); + this->topic_thrust = this->get_parameter("topics.wrench_input").as_string(); + this->declare_parameter("topics.guidance.los"); + this->topic_guidance = this->get_parameter("topics.guidance.los").as_string(); + this->declare_parameter("topics.odom"); + this->topic_odometry = this->get_parameter("topics.odom").as_string(); this->declare_parameter("topics.killswitch_topic"); - this->topic_killswitch = this->get_parameter("topics.killswitch_topic").as_string(); + this->topic_killswitch = this->get_parameter("topics.killswitch").as_string(); //variables this->declare_parameter("max_force"); this->max_force = this->get_parameter("max_force").as_double(); From 4818dae0ae9046180cca06c8f500d75ed31d572b Mon Sep 17 00:00:00 2001 From: Henrik Date: Thu, 19 Mar 2026 14:47:24 +0100 Subject: [PATCH 088/128] removed unneccessary parameters --- .../velocity_controller/config/parameters.yaml | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/control/velocity_controller/config/parameters.yaml b/control/velocity_controller/config/parameters.yaml index 2fefc0f78..1c0e5ae3a 100644 --- a/control/velocity_controller/config/parameters.yaml +++ b/control/velocity_controller/config/parameters.yaml @@ -1,14 +1,14 @@ /**: ros__parameters: - topics: - odom_topic: /orca/odom #Odometry - twist_topic: /dvl/twist #Twist - pose_topic: /dvl/pose #Pose - guidance_topic: /orca/guidance/los #Guidance - thrust_topic: /orca/wrench_input #Thrust - softwareoperation_topic: /softwareOperationMode #Software Operation - killswitch_topic: /softwareKillSwitch #Kill Switch + #topics: + # odom_topic: /orca/odom #Odometry + # twist_topic: /dvl/twist #Twist + # pose_topic: /dvl/pose #Pose + # guidance_topic: /orca/guidance/los #Guidance + # thrust_topic: /orca/wrench_input #Thrust + # softwareoperation_topic: /softwareOperationMode #Software Operation + # killswitch_topic: /softwareKillSwitch #Kill Switch LQR_params: Q: [200.0,32.84,32.84,15.0,15.0,100.0,32.84,32.84] From ef4517efd5bbffb2b7d2030843b9cf8e2a360f69 Mon Sep 17 00:00:00 2001 From: Anbit Date: Mon, 23 Mar 2026 12:14:30 +0100 Subject: [PATCH 089/128] Add: Diffrent test scenraios for autopilot test --- guidance/los_guidance/config/guidance_params.yaml | 3 ++- .../include/los_guidance/los_guidance_ros.hpp | 4 +++- guidance/los_guidance/launch/guidance_test.launch.py | 8 ++++---- guidance/los_guidance/launch/los_guidance.launch.py | 2 +- guidance/los_guidance/scripts/test_scenarios.py | 8 ++++---- guidance/los_guidance/src/los_guidance_ros.cpp | 11 ++++++++--- 6 files changed, 22 insertions(+), 14 deletions(-) diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 0914cd9ff..0a546a4a8 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -28,8 +28,9 @@ vector_field_los: common: active_los_method: 2 # 0: Adaptive LOS, 1: Proportional LOS, 2: Integral LOS, 3: Vector Field LOS u_desired: 0.3 - goal_reached_tol: 0.35 + goal_reached_tol: 0.15 max_pitch_angle: 0.96 # 55 degrees + slow_down_distance: 0.40 # Debug Settings debug: diff --git a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp index 39e4edca8..a996c42b9 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -75,7 +75,8 @@ class LosGuidanceNode : public rclcpp::Node { void publish_state_debug( const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr current_pose); - vortex_msgs::msg::LOSGuidance fill_los_reference(types::Outputs output); + vortex_msgs::msg::LOSGuidance fill_los_reference(types::Outputs output, + types::Inputs inputs); // State Flags bool has_active_segment_{false}; @@ -111,6 +112,7 @@ class LosGuidanceNode : public rclcpp::Node { double u_desired_{}; double goal_reached_tol_{}; double max_pitch_angle_{}; + double slow_down_distance_{}; types::ActiveLosMethod method_{}; // Guidance Modules diff --git a/guidance/los_guidance/launch/guidance_test.launch.py b/guidance/los_guidance/launch/guidance_test.launch.py index fa78df486..68e739ab7 100644 --- a/guidance/los_guidance/launch/guidance_test.launch.py +++ b/guidance/los_guidance/launch/guidance_test.launch.py @@ -106,7 +106,7 @@ def launch_setup(context, *args, **kwargs): ], ) - test_scenarios = TimerAction( + run_test_scenario = TimerAction( period=20.0, actions=[ ExecuteProcess( @@ -114,7 +114,7 @@ def launch_setup(context, *args, **kwargs): "bash", "-lc", ( - f"python3 {os.path.join(los_guidance_dir, 'scripts', 'test_scenario.py')} " + f"python3 {os.path.join(los_guidance_dir, 'scripts', 'test_scenarios.py')} " f"--ros-args -p drone:={drone} -p test_scenario:={test_scenario}" ), ], @@ -130,7 +130,7 @@ def launch_setup(context, *args, **kwargs): los_guidance_launch, orca_sim, set_autonomy, - test_scenarios, + run_test_scenario, ] @@ -145,4 +145,4 @@ def generate_launch_description(): ), OpaqueFunction(function=launch_setup), ] - ) \ No newline at end of file + ) diff --git a/guidance/los_guidance/launch/los_guidance.launch.py b/guidance/los_guidance/launch/los_guidance.launch.py index 5ee9a3d1d..d616e0853 100644 --- a/guidance/los_guidance/launch/los_guidance.launch.py +++ b/guidance/los_guidance/launch/los_guidance.launch.py @@ -48,4 +48,4 @@ def launch_setup(context, *args, **kwargs): def generate_launch_description(): return LaunchDescription( declare_drone_and_namespace_args() + [OpaqueFunction(function=launch_setup)] - ) \ No newline at end of file + ) diff --git a/guidance/los_guidance/scripts/test_scenarios.py b/guidance/los_guidance/scripts/test_scenarios.py index 0d6a2d328..95edef2d6 100644 --- a/guidance/los_guidance/scripts/test_scenarios.py +++ b/guidance/los_guidance/scripts/test_scenarios.py @@ -69,9 +69,9 @@ def generate_waypoints(self, test_scenario): # 3 = seabed/ground, do not touch # Keep all depths safely between these return [ - (3.0, 0.0, 1.0), # slight up - (6.0, 0.0, 2.0), # slight down - (9.0, 0.0, 1.0), # up again + (3.0, 0.0, 1.0), # slight up + (6.0, 0.0, 2.0), # slight down + (9.0, 0.0, 1.0), # up again (12.0, 0.0, 2.0), # down again ] @@ -147,4 +147,4 @@ def main(args=None): if __name__ == "__main__": - main() \ No newline at end of file + main() diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index de851d950..09042ae74 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -250,13 +250,17 @@ void LosGuidanceNode::set_los_mode( // Message Helpers vortex_msgs::msg::LOSGuidance LosGuidanceNode::fill_los_reference( - types::Outputs outputs) { + types::Outputs outputs, + types::Inputs inputs) { vortex_msgs::msg::LOSGuidance reference_msg; const double clamped_pitch = std::clamp(outputs.theta_d, -max_pitch_angle_, max_pitch_angle_); reference_msg.pitch = clamped_pitch; reference_msg.yaw = outputs.psi_d; - reference_msg.surge = u_desired_; + if ((inputs.current_position - inputs.next_point).as_vector().norm() <= + slow_down_distance_) { + reference_msg.surge = (u_desired_ / 2.0); + } return reference_msg; } @@ -271,6 +275,7 @@ void LosGuidanceNode::parse_common_config(YAML::Node common_config) { u_desired_ = common_config["u_desired"].as(); max_pitch_angle_ = common_config["max_pitch_angle"].as(); goal_reached_tol_ = common_config["goal_reached_tol"].as(); + slow_down_distance_ = common_config["slow_down_distance"].as(); method_ = static_cast( common_config["active_los_method"].as()); } @@ -353,7 +358,7 @@ void LosGuidanceNode::execute( } vortex_msgs::msg::LOSGuidance reference_msg = - fill_los_reference(outputs); + fill_los_reference(outputs, inputs_copy); feedback->feedback = reference_msg; los_debug_pub_->publish(reference_msg); From 1a9af0999dacc982ffa439a3f639edc9d1d87510 Mon Sep 17 00:00:00 2001 From: Henrik Date: Mon, 23 Mar 2026 12:54:20 +0100 Subject: [PATCH 090/128] changed State overload with msg_ptr, added reset function, changed reset function in LQR, and added OMM to launch file --- .../index/LQR_setup.cpp.2C597C3359569B5F.idx | Bin 0 -> 10314 bytes .../index/LQR_setup.hpp.7B50E8B15298D255.idx | Bin 0 -> 4224 bytes .../NMPC_acados.cpp.326F2FC10CEB64DA.idx | Bin 0 -> 6250 bytes .../NMPC_acados.hpp.29D23C04AD60A55C.idx | Bin 0 -> 3236 bytes .../index/NMPC_setup.cpp.86F9F0381989E206.idx | Bin 0 -> 14226 bytes .../index/NMPC_setup.hpp.CAD16FEB6583910A.idx | Bin 0 -> 2236 bytes .../index/PID_setup.cpp.3C9DCE2555B8A965.idx | Bin 0 -> 2170 bytes .../index/PID_setup.hpp.486A8D0A6ED861A2.idx | Bin 0 -> 1230 bytes ...im_solver_auv_model.c.53A9281F3B2EA7B4.idx | Bin 0 -> 5454 bytes ...im_solver_auv_model.h.995A50C901E2AF1C.idx | Bin 0 -> 3536 bytes ...os_solver_auv_model.c.5778764B3A2FDFAE.idx | Bin 0 -> 12412 bytes ...os_solver_auv_model.h.F9AC8187D4C1016F.idx | Bin 0 -> 5168 bytes ..._model_impl_dae_fun.c.CDE429F376604CAB.idx | Bin 0 -> 2238 bytes ...ae_fun_jac_x_xdot_u.c.6132B1FDE79246FD.idx | Bin 0 -> 2716 bytes ..._fun_jac_x_xdot_u_z.c.ED4BE4E791391EA0.idx | Bin 0 -> 2796 bytes ...ae_fun_jac_x_xdot_z.c.59CA37EE71B4BA5D.idx | Bin 0 -> 2718 bytes ..._dae_jac_x_xdot_u_z.c.A6D29D52260DC082.idx | Bin 0 -> 2772 bytes .../auv_model_model.h.60CCFC5FA93A2F4B.idx | Bin 0 -> 2004 bytes ...ct_instantiations.cpp.CECDE40637351DC9.idx | Bin 0 -> 1054 bytes .../main_auv_model.c.FC541DFFAD75A3A0.idx | Bin 0 -> 1648 bytes .../main_sim_auv_model.c.E637641F2593FF77.idx | Bin 0 -> 1122 bytes .../index/test_LQR.cpp.46B8F24A8C36EC93.idx | Bin 0 -> 8694 bytes .../index/test_PID.cpp.575590D7897A814B.idx | Bin 0 -> 8678 bytes .../index/test_VC.cpp.74BA115DB14CB17C.idx | Bin 0 -> 4538 bytes .../index/test_VC.hpp.C3EBE494A18C2184.idx | Bin 0 -> 1946 bytes .../index/utilities.cpp.7F99D4E1DE20E3AB.idx | Bin 0 -> 2778 bytes .../index/utilities.hpp.77C0A5FDF681DAA0.idx | Bin 0 -> 2878 bytes ...locity_controller.cpp.DFC34CF5F86A4B55.idx | Bin 0 -> 16350 bytes ...locity_controller.hpp.3E0346F5513060F5.idx | Bin 0 -> 4310 bytes .../include/velocity_controller/LQR_setup.hpp | 2 +- .../include/velocity_controller/utilities.hpp | 10 +- .../velocity_controller.hpp | 17 +-- .../launch/VCnTest.launch.py | 41 ++++---- control/velocity_controller/src/LQR_setup.cpp | 22 ++-- control/velocity_controller/src/test_VC.cpp | 8 +- control/velocity_controller/src/utilities.cpp | 40 ++++--- .../src/velocity_controller.cpp | 99 ++++++++++++------ 37 files changed, 149 insertions(+), 90 deletions(-) create mode 100644 .cache/clangd/index/LQR_setup.cpp.2C597C3359569B5F.idx create mode 100644 .cache/clangd/index/LQR_setup.hpp.7B50E8B15298D255.idx create mode 100644 .cache/clangd/index/NMPC_acados.cpp.326F2FC10CEB64DA.idx create mode 100644 .cache/clangd/index/NMPC_acados.hpp.29D23C04AD60A55C.idx create mode 100644 .cache/clangd/index/NMPC_setup.cpp.86F9F0381989E206.idx create mode 100644 .cache/clangd/index/NMPC_setup.hpp.CAD16FEB6583910A.idx create mode 100644 .cache/clangd/index/PID_setup.cpp.3C9DCE2555B8A965.idx create mode 100644 .cache/clangd/index/PID_setup.hpp.486A8D0A6ED861A2.idx create mode 100644 .cache/clangd/index/acados_sim_solver_auv_model.c.53A9281F3B2EA7B4.idx create mode 100644 .cache/clangd/index/acados_sim_solver_auv_model.h.995A50C901E2AF1C.idx create mode 100644 .cache/clangd/index/acados_solver_auv_model.c.5778764B3A2FDFAE.idx create mode 100644 .cache/clangd/index/acados_solver_auv_model.h.F9AC8187D4C1016F.idx create mode 100644 .cache/clangd/index/auv_model_impl_dae_fun.c.CDE429F376604CAB.idx create mode 100644 .cache/clangd/index/auv_model_impl_dae_fun_jac_x_xdot_u.c.6132B1FDE79246FD.idx create mode 100644 .cache/clangd/index/auv_model_impl_dae_fun_jac_x_xdot_u_z.c.ED4BE4E791391EA0.idx create mode 100644 .cache/clangd/index/auv_model_impl_dae_fun_jac_x_xdot_z.c.59CA37EE71B4BA5D.idx create mode 100644 .cache/clangd/index/auv_model_impl_dae_jac_x_xdot_u_z.c.A6D29D52260DC082.idx create mode 100644 .cache/clangd/index/auv_model_model.h.60CCFC5FA93A2F4B.idx create mode 100644 .cache/clangd/index/ct_instantiations.cpp.CECDE40637351DC9.idx create mode 100644 .cache/clangd/index/main_auv_model.c.FC541DFFAD75A3A0.idx create mode 100644 .cache/clangd/index/main_sim_auv_model.c.E637641F2593FF77.idx create mode 100644 .cache/clangd/index/test_LQR.cpp.46B8F24A8C36EC93.idx create mode 100644 .cache/clangd/index/test_PID.cpp.575590D7897A814B.idx create mode 100644 .cache/clangd/index/test_VC.cpp.74BA115DB14CB17C.idx create mode 100644 .cache/clangd/index/test_VC.hpp.C3EBE494A18C2184.idx create mode 100644 .cache/clangd/index/utilities.cpp.7F99D4E1DE20E3AB.idx create mode 100644 .cache/clangd/index/utilities.hpp.77C0A5FDF681DAA0.idx create mode 100644 .cache/clangd/index/velocity_controller.cpp.DFC34CF5F86A4B55.idx create mode 100644 .cache/clangd/index/velocity_controller.hpp.3E0346F5513060F5.idx diff --git a/.cache/clangd/index/LQR_setup.cpp.2C597C3359569B5F.idx b/.cache/clangd/index/LQR_setup.cpp.2C597C3359569B5F.idx new file mode 100644 index 0000000000000000000000000000000000000000..7e4238ef0862dda27c1e010f1d3d55d83aefcaed GIT binary patch literal 10314 zcmb`NcU%<57r^hGOT7gSIQp^lD!n6OfukNMA`k`CRqTRF5ygV29H)o@8#WLTBPb#& z7%{=blGqCSi2hJ4M2QXKZ%#8e>;Llkn9qK1W@qNTdGp?zw~3Dm4-awV@j@p@ zEy$RXGm+x)cp~&KJ11*~xeAZxug~Kxs5mwzW6m8v;f;}=xA#w9d;a2uO(#-{TfV;}Ha9K6~%CWMD&Ps7>Uf-3d74;W? zNRF^A^ELhEqDk*lm;J{^Y4Zl-#?4$TQ_st~! zXJ^lUuzNW@9eQC>@A2QzAnda{t_3?+HyH?!E z|M+g|%6&=MZRrs?ZBOn>j@|D%dZ}4=i`k+ZWu4lCn(y@Zr~Le9%O6M6J9Fi)9zGVV z+5hg3%ZauA>Do04KdL+kEr*}bLXitxLB%j&h0bLq!puWg%NeQjD)XZMqb zJG^%U?b>}l;Emy-#nE%`4yv);vOFQR#l%%Z?_xgFv*$vVO>@)a4FZ$VlF{|Hv)^0x zpWO9J(BkZ(`=diIdO6HJ+^d&6)qL0Ry5(&JiAV1XPoJ!43`~d!N?TiIxZN^qR(rDb z^_Ay<2`=ZY&DvTt+AQmVe7c&~z!xnyYI8^2UpFPHr0KBsIICKRzV+`)ckQ^EW&3O0 z4$C0*-sx2iWAk%z7K+oyZk72xn`=N1-q$s|<*N3hrp93h`+W|d%QD*6_K)aFWm)c~ za}n*^BeF*Cd%dCYcUSeN|K^6ewxr%%vfR4GWxUar2Tr@MZamfPQQnn$WMceTvBB69 z?}n>g(;i5QBGsl|3`qZ`d%Z*HrKEg*3w8P8Uh`AqWE1C}{4@Uy_1B|k7s3qkPsc~q zmi@Z%faeR7O>r5)Y1d{J+&@%&iQ0V=Nf6peiLa+}cLG=;mA2iuco=At|-pTej8p?90zeZCf(Y zb%yQfErnZq8^<)ep8GrD%Gu=J^l=9p=Q`PD`6p;?o#@+`HvdKQ;*P|&NnOK zzupz9O}St(Zi4fQ4+}CXe(<-;xJMsoo>L&W>(p9r{&W2?$NfXSmwr5T_uqxR|J2@V zmxs3XX3)uSs-HOk@7j~gPahstqx&XF-kCPz{Ji64jcyB#vSmgqR)_rcepBy-EA3<7 z290msG^WJadi9)=*X~vEe;Sx3E&6q=LEhXD|Hpsqw!0VecdD0KM{LaOaUQ4JeAEM* z&kWaea{Bx7o3qy)7Ub+bY~Fch_5;@==jMC=_T%FtKOMQ0-|=s!{Sr^nh=LDGV~1pD zy-pgJb97sB=ekHu>!~ZwZN2v1C$BVofz4}GuOZbfPe%9e{VgJ`eT@tAlN~?1CAIff z#OzUqps^MJY?z#V->Q-w#d(Pxw?slj7CyHYZ=_YoldtF($YfYH$4R*+# zv)@He9qV%b%y=*bm|F}fR&qN(MLBA6$ zQa#i?M7Tpu%A52jgN}|C@@JtAnNFErsospY18%OouVyQszcP{L&FAshE)aHsb!x0I z)|zLc!XF*~4Wz2<8>OnYQ2snk$e)Ksr~row@JT9AQ|xJv-*?e_`^c{;=!B4;g*rHkoGnvbHCzpFb5PtR&l7HOCuv>=lHe6sy#n9V zkyaypc^VY(a>gJDHg}JU_7r?~PRO5&MyLmedep^V?C*@n*dJVREZA9gMaZ9xI&^@j z1F6|f!%eB?pPMHZd|W*A2F=Swlk|hIA9aWm#@X^TR0r@rscDv$gMM`Sx~GQx1T==R zjunVh&c+0nVNKbhvVbN&uLyd8leY7Jz$?YLNvnu(=xNiqGj^3rcBMgWWnQY z+BRozp!W$%oo0(+crirbO{Ce)V6hn-2zP;67w9N=k$nh)hoDcm2Yh;9uyS&wpU_tz zRq70hqSg;OO7YkaAou|Km=i|mIs9+!U=@9ub+&hQ!8?H{*g2F#3C_tJS`E%?fy4FS ze2~L&8gV+LHO?T;j8F+^lzIJgxOIUKS3H#-5D@X6g2q$OA^ZwdUV(s! z`wDblff3<8(C7mlE*tcL8J7q3!7#%8;L#6$SUogS-{5`-!TiIHz@bHrVs*X;CiVXXt{6Z3mThP{sZwdhG6NI(1j| zfM!!vQZ(_AJvK|=@Qt&Ykt)raQ|5+bio{`w@nnh`plZMn`w5z@0goC8AzTMlPzMKr z{Bjehn_!5KU19&(-Z1`5lK~<&+&$a}4=iFMG$WLzB4Q&2kwVYkAgx?pjp6Z?)@>TccP@sAshl16EmDIoC z(t*P$mV;2vZI^?i90rp1Di~G; zp+ur8h^>M+%;&GG4X)Ps+YMPt19&umCwA>3c0YLT2QR{n;NA!xm?uu-EqeS9&s~N3 z!VIV{%z$FKWr5u+YD~RQ^WvXQ=C8F7ELAEYlFCvF*S3-*fi#TnC zASeV5uLQwLE^h$A1}-;)pc!Q&eBbQ}CSqO>CN)jh=G_ez+cR~_2D;imk)_yR-)}mm){SZRD z>-`YqmFr+e9c1G5Vhw@me>vf$WcRC)9$T(gwGZffS8W)A78Ye6rBq5@bHn4xwE_w9 z`+Z>72c9^h;q(y=^f`CL16^7vFgR2^DH09T3i_>Jil;PI5ueO8=!irj&eNR73DryV zmnf~AX7ly(jfhoLf>R~9;nmY@B?MGLFy?CubL7VYHphIWvw7#>FOqU_ayV0ay<>tKv+>pm2f7pA~S6k#GXA~f;1DEw(g3J66R zMi~*x)yUPv>K3ti8hJ{>zw%8#CI5JT729gj3XXVNvFAsI@c9yeZIGQYF=Zj$G6rc= zVk$AmQ_^ghX&8sX{lWuDTZC?eGGfwfq+X-}Zqu1yHPKSz$7E#EO(1LneY}A6!xl5~ z^Fqh-#cXy^_E5ZRG0L2z#V`_cZe*u{;KIX+Xb)syD&s~L8FT8VS$te`>RJSI1@@A+T5pAT}$`4rP_8^8`LP!ZGZ_Jcj)MzCoFTg)5(dNZ#e zwk}s7W=E=z)WWlg*#y-D3cuD-ovckN=I21lBd=6=g2#E+M z6e)>}#AGOYesA~oHw!gT`sgPO!as}HSSHqr&?07$G6NH_g}@Y&6^huEz^vr*24FUT z6_!}UHUrZPR(Q8W=!z+N2u9dph|rZ$^Z|@;s+hg^YU{d{>=F$z>mqPbX76IwL(@YW zFGuSd+5f<-APFKlAA z3@pkBKfEjXR(7hZ2&IQl!T2dELi|@&Py}}0J_x{p-9L6l+OCA9a@0|xE78N_i`X#T zFeN$ul$btkdDTia5j)aoq%wqw*aU+FL-NWhFj)2FT{f^71`z{U4F;>Z?d4!pPBLB* zIs!(U35U48dive_xZFRaJ_#7!m&c!*3ic)HpX1Pw1W^)C(O2>n|tRRm=OVyT|V2%4G zS+zH3l$N7VDFcxVl!=ytU1a14;A=b*Mp1Sn!Zd(=1Gr(MRs<@C{oqcx5$qbl74yx1 zPY0zR(%bTtqRbMfT-TsS$G4*8r>dt4@FKh6{;G_p%~Q|4vkPB!Ev06RTnESUt)G6sBa_{GP_r`-Ya{n_k1M{JWg!e zc~EAs7bSaEl$BCSnjJ3~Pia%s72UspaSDF1Eua z^N(k0Z6+Q@hOiynwu2`=Jz};BT&lnwvu~BFXv66C?@>pIs)Q3o!c@bQ)UaXd>Hp2r zIf~LLYsy+3>z!tkB9f%onbGVLdWjGpBy>#?=$VEE>L$t3tyb@z(_bnWm+j(^Xy5tmN%Y27p^*Rssm>%N_%wAlTwM^$*;6w*xjS) zKUcnS+HRxvjPl*J=@+E3GSDm|8Hj@I%fJci0PhmYb3Yfxu?S+6rm41r2nYUsW(Avf zi{zJ3#FBxA3}`%pqU2#%gxueMB@8jjyfx)y#zm>!v7Tq%UP4Ft1vtF`XA-mCfbAQw z!$RPoiiG)Gg2ziaGBAEZN0(I_x{PiHr_JDrr#k%egS_3Z5<5^upaOg zhhQjPeDHB6Z?EOdbrX~*B}fzTt|3a2e#4gCP-NhO_HW@XtHz) zho(xWacG8gCWmH8(>atOoz0=S(o7CzNwYbWBb~>g`O*a(%9Ac6Bm;G{TUcKrv~`xK zV8!JC83Yn80u5Ba;$*+39N2Mda{hnr3o7epG;Q1=x{2uLt9La(z(PfXX4z!!|%8t5WFz zJ#1z)DtSR)BtDv;nii-^cZlGM<)!fwq>r~8ontO7{*W8ywf#!i6 z3N{Z`63+aPb$|YTgpX=V_{IhMhdsKba0!mC%FGv4s2x2y3mp+Lt#4h1^~D`~0V zVyorum$$!CLd37$m@L0Jw-cS-T+LkN#R(M|H1m`+e)&2TS09^u$law0(rCPU>vcK} zVqT~IiGKdFDF1!;k-p4#CJ(;S8gD;}*VpGDCFYiWtP_=YHX0>_)J|^MXD9gV z{Icw`3Vj5rZf8*dVCidTn79Iq_2Ao7n7;Is{j=iK^| zQj$0hs}zh%N!l$!Qkj;LZ)pnlE(ITaR2A%3O6n&f6eo5%-{@CiRi};$e|#o98k_6M5!&99n9! z)ReTfgK0aMVV{Dt40NcuF74ma(yYVc`%h6WVnf+*N;@G`2&b4Mp(Gq?1+!KVgi zgPMItX*~N%iNesmCzjsK-|`F{#RDKXK&nu9e~<*9KT2b_bo0)do>R58z37dlDod50 zqTIgKR0;U$-s`V)bd3&N{r#En!8@cWJIaousTeATqniXOfkUm}(F&eeQ<)2fToIqY z`ax5~##zS=B%2!N5~n=msLds|^wjpstNm~h6myl0VBDMrvNKd8?T1#6;sH>+hGB7kUF=p%-TgH#^XCfIF#)ENUTp4F(FcZjxFaw!j zW;ip58ODS%62_D9Vtg21#*K+$0+^o@i literal 0 HcmV?d00001 diff --git a/.cache/clangd/index/LQR_setup.hpp.7B50E8B15298D255.idx b/.cache/clangd/index/LQR_setup.hpp.7B50E8B15298D255.idx new file mode 100644 index 0000000000000000000000000000000000000000..24523a4ad7a3f1d91aa220fc01c52e8125ebd4bb GIT binary patch literal 4224 zcmYk82~-nT7sqEtAWbF=2_)>3$WrzIVKpTr?3=QvC2rLo5L61O3b?h^;))(bELx!! z=pqFx;!eNgrxfs0Xsu`~w1QTtwGk0ei&POozW1hHk#o3b-v58!owv>VWmbGl%vupa zL}tXV)o13X84&~_LH~LAxmmg?1hIop5Nj(Mb^7Huf<$+F#{^%y3+x^pI`yo^EBsgu zyD3M%pNKwgCGuMJ$@ZDm5wgLI@E6aMj(%squDkH70!69W!>SM0YUCG|_{eWnU+gXz zYrNc2(|6Ro@%xZVBXzU>yi=i{Yjo*qK3`Bdq0IaH^NkiSE~b%-_7-1z|1tj>zvpJe zw)UlgGv;gL8!m51e^_nkDL5lo_3gyAos~NxdW}t6>*u=mk9F{Re=O|TJFWj%_VCo7 zrkf@31_w6Dy+(?jJT9#NqUly;S)wi@>b95HDdpq_+56 zM}LRz&mT8@H(?k%_FOjn`0~D?gnjz-j$4Wo+zD zVRD;ke|grEEqB+_@f|(QXSzT7d&`HlhM4o~K2V-1_wim@_Bguu(*3pytvYDlnO{t< zI>!sb!<9nO;|2Hr3Gs*GlbL=TYn8PnUMSg#! zWk;7}7+MZR812n8?v5K^VvY(gux1k%N=DzdTV3Ayci%*{UNcmZRyqIR^ikg|j}X-q zy8=VO`UM^O*e{Rw*##W=;j4qe2W@=yEeG~3w5wgzl%a20UO)Oo6CiqY>seKY zW5wZXg#X012)FL3dJwCd^)M}S%^rUK*ZMAtpKt7J4hc8ak=fMKIi332JKxdkZ$$O~ z`p>5Mrm}&@A%|FqP5FlXxn=y7p*wCOyd5=&J7=*uO;kbii zQL4@&_*we+XZ??#p1#*Cj#_%(<^{QMdwOr)I(_;hV}kG%3=z#`4J1QZnh|WYEIN=2 z#2e@fM1P;Jw(~t9>Y2h&A!35HR9R}c0JcFAJa5x)k7$MpK(@7`wI>(Awl>+5u&vr4 zVkiMBu9az>xB&JC>qn<{#x1g7C@HdIWHA9;0J|}pcAewFvtlT3WUHsE7jgk?GpG2m zUezG7-O;c(9o^mi+?JuDQSVs$Se6T5XPsp$_V#a3Fw|RkVzi|>7r@qx z%MVDbrD}!>M7=}hp`Kg-o6bqko294;7g4rm#MBd{}lZaxQ>r_4eaCzx|=BPDH(ldT*gz zw@^w}X|Hs7m9Te7>%DBV(+s7={n;=FE`S~QUc$_=fZHEnId1*lNow=h9TfAgb=(x4aAk*;;9La=?X2P7N`t!NDO*m^}osZl~F zDd|KXhBx2))rZkO-{)$a?`OaAD25-rU3UD*l~Ep`Q(`xrw<27jO+bu11@ORBByV+rv=P@Yz*K zHC%u}>im^X-s)TfLwTW9RC}tmTmYNwUt2!C!MmTKqL3ZR50!EO>}*q4i-ergK}(83 zQ?xd-wr9PJyu|nf(CEf|QBsoIV1Ij%q0CWljZ7ow0`z`I*Zig1{reAvgddvT04W(D zUDzZ^k_)#75jJQA+RyKrqnn>K%uqpi^o~+@Ei0@zl^ zuKxZ&^w%gu@$mcK(JbTw*twZY@@h;b?|o9h-S@C9aNi>tU-q!lWuGrUr1EbuG8VE? zbd&{rfI>ErmuU9tGGdbglGLvz$Rqzcv!%WO{NW$>LJ%aCW9uUa`UqKnH-817LddG(REcoy1K&_33^#beMUO|$Jio#q0qRTpizoYP zXpI<-u8`GuYLvhS)QZ<6!?6;wT3RcHH9##dtsk&J$VSi+Vz@wsY$P2i1`r6@XgXR9 zzcz$y3>_neB?H)^v0~D~tYMm@4Tn~c0j)^JKn3~6@kVFcfqhBnh$?BN7;ai28%zg_ z;od?mzu;+;EfuX2!-@&nSUOe=I{LM9q7P4Vsk&N*QGLRL?7^@%yQIU*s3bo-wMl!}IWCKBw zjPVKCb^V*y&AVN(ixjef{6Gd!2tY-%#2|$ta1KxrOi_@b7s|%q1R21IvN1J5hHXUI z7?ZE$mzc!ah>;)z9+8aM2r^t6s0hX($Z!WBnSADP2#y6LQ-XgP_;LV3jwfKkZG>LHQSfCF3=54C z1MsCv0Qr!Nu?JZ|JS1b>L2V!%k}>8$2Ew6NVzPk@*8|GNSOXcZS(MFj?B!5^Vt6D7 zVK5S)7m_i$K!)oD*G9;K+PRrIX}Dfq?&7@J_(6ulzkh7_=5_%Q9zWZ}(aAyPEGHbn TAqe^8qr>E*^W>w`}WFzuMaAdZXQU(taoBZ26(T{PVo$ywB}C z=Q(HI?1F+?y-1W>KfANJp}j^T5{Y>HZ*_Z{BUUUDDSSkt&OL|9OIL&@tFOIwe%7v? zfraL`UgMwfcQi)4Z;dfNimP|5nV)BFPP=Tp=RSY#_zK?zNn7j0v&O#e9KQ6_@e-M_ zyR>CP^y$W$YZsT5EV^J@6TVRP&R^c)Tip-lDz*LF{@fTk_w>^BM?c^G(coH1Ucr^I zTi-?vT{tT{@i1hCvoP!Mr;Bf*&66Ki?>n4*_*&{K5&Jp+S-Wxu^le*~sBR6nem(b7 z!Em_-2|w{GZR;v{E&jcQ`5!8Ve_c~^yXRkf*L1!;`^u}nW4UX7XFK$Ep4b%=tiC>S zExW6t;`KFK&knWrzxu>3>x`BY)e|qaPr3Z9TQ^0K+nV$Dh@oB>gVXMVk&)#Ut;+Kh!FHUn`T-KXmcWt|O^4qVbssCMF5dZkbwJ-fn4sN{g zD63!E|Iw*%<#$~Z4f;EOj5%}a^3Om2{o0usDPgDoT9G&Ly0)@@mpd`|!};@{S!*@? zsYA4}`QrNRP49fm`!3)3(sr$+ZTaBG-3w>#dZVTE`@SF=W`XqmVu-Sz8t9iRPTC>-UyrwttgzExGy5eZ!(F+pYZ@Onpago^jMYEy}2|yd+w^wz+mX zh(u%7x_=*j?>7Nju`y7Dd{w@I!aVgny-0?03V)K$`4+vEv{kPbM`5u^WiklUPfTe_+GU*7K@8iMWKR4ZV4o#%kBluXnyMb5w$o5iw8k92!TSPJW=BrkN-q3G=_e0 zbaKvVwKx(#5Tpvy3UTr{rAHQw#l^3*2K|En;HFv}jm4v&8if!cNuCryUhqY1_0aL= zg#Qf^oA3iFxk@2~OTwi@-21x(`Qf=|zfp@Luy{A%zevHZwGzqD1RsJB5=R~#fq)|r zNchfc$qPPvu-Q+_A=7MAF-cVpZ3oqMP!o4>Xb0$aKmg@YNFRj^G7X0gLf}Er6Mir1 zDd(mc<^C!G>C$u=BuNBhC^pO?DJLL%mOY1rNkBQ-Ik}Wd@=Iou z=|mX;Ibj<(;RT?-xS-JmO_ckfxDVz~c7xpw*~Ci%8UcO;^pwZJJ`UN$9045z{unjy z=%PQR{L1C8RdGnC)M{fmRNKhVlr=M?gt=4CG^=BqnfZ95mw)K=}wLkAQ~qF_0gl&y#gFCFF;K zRu-`D^R7kv>jjiA$>(U43MGYJBFO_GKNLAK#D4PX$GiNW3>I@JRuLOOqK^s1#t~X} zz4epJrDb6<4yA>sMUoeAC_N-ypkz<6TPfx1^Ft^tRxj34Z*Bqg7GQW3tfOF~)*b}i zLF$dO@0RU+`Nbn~VgXrXmLOt@fD&X0URrC~-JN%*!3+X2s*EP0`WI^Hi1gEy2}%wv zku6bB6{=;`3@w!{WvE70!%(fPmZ4@@Gea%17KT>JRx-3owu+%vSt~;aKymPKJFQaxM{9*;q-e53!9yW}P{JxPwE>^~*!3f$i3I8>J3w2Sb}7 zcr%0&=?gwrn+IA{sn~TInI?uTm`K~+Gjhjp8gM9566r_YIQxmZoC%?C##+mcIO zkE4;7@C2oNbv{qRFQCfCN@l%RHC8d?C~y=JRip!tOv&p%v*B<>OQPlEogA`+TVlxM z97+gJV5q`ekwhloP^Gz&p(=9~Lyd8b3G_)voP(kE==OLj>xk}Ps0Si?z(}RN5Y-ED zl=}hoLnPrR4oI_g8_wIY5mAyTuj8;0dP^8%gg$|xipUD)g_V(&3{^!|G1M5=$f)87 zb1>8%+U`|@Lmi{ad7{?yW?B^D4X@s{Cf ze2JDsjDPktyyWLU2cG|aU3U07?B_ylVIUcdlcKPrn2cF{;PTn+&ZS*a0VzTh2I3R} zrK{7Ka)EnL@4h0SEOizm%hBg}WpCy^sOKL4W;57f=gD^j7T)i}OK_TcnudggM>DiDbc7a4%JL_+-5tbZ_5S*)WNh2q;f#>S z&3p0BF{+q&@-s;}9DL7lh6KOk`J7jyYY)2d(}G6uHV(w^_~{={H{^MXbN~69lXnha z8x2Z>hRn{POl785PY%sg&h*mvCwJU_x~}pdeuY}5Mq~xuHGK5oj#+mM{~WD~Hj|kW z|NhptJ^!OVCBdz8vMH6uw%os*Y3s_P%429$o(0{rw2dZHVdXt1FQR((Q?>dvAM81Y z%S2^E<#IZ@s-cRZy&%~OKEweW8iF8v7@#~1I(!%)oEvs2erQwBdAw!#%ly^EI1Z@| zY9l3$Q8Sg2R;Fd1)JybC=?InwGhP+qg=At5-p!{*+9*X$jbbR;5^bfj7*k9NC8ODx zN@*j+ZKO#XJXvsa{rmSWP7zRrq#}ZN1@HWlN`|T=RbJ|R^W(p*lU#b&6QXESJPna) zx+#a!AVdyAEO8WvkGaNCFq7hCE7v{wmzKlt;qXVQBE6aa*1drZgLiG0e0gNZFxZLK zJhCU+lS$_DC?_$8At&Ir-@8Hbs1Gvwz)nozksHF?5J5EIanBw$4iQ9?vDX{Y=8OM! zUCE)d%dSy9tWq!1$9JEPa5U+fmd%&8l}1+ zjrtp(3rE18@;Ic9LmIJ<$ERE6G4LmR=^wvcc{=;*1m5y?K*SE(z%Bmj{{H(fAHItZ zX-0{WDFqgzWhz}x3C0A5D(n@xL{nVP?3D~v*{c}pfw&&J+&$f5bU)2%;$_hOaLYtu z@nZC*k<|<5eug7is4VoBa@?8D>L?)Z%``l`u)?S2A;JOE>Uc604w%-!(|~1aGrft0 zlP=8eZ6bLzQ#+H9<%H!hvJz8?g@&(GS?X;jdDIQ6ZhDX638vNwJWdBT=_PT(jTE0 zSjXmVADFVB=@E|i4hY5hNfhJ$>)w#`rg-)KcJ` zAYVkHaPL<@r{?O8PaAK3S|WrQ!bO5tBC2byZwkVw3kVQTkw|@fWpY0Sr&MXxLH-)x GMgIqBpVBY@ literal 0 HcmV?d00001 diff --git a/.cache/clangd/index/NMPC_acados.hpp.29D23C04AD60A55C.idx b/.cache/clangd/index/NMPC_acados.hpp.29D23C04AD60A55C.idx new file mode 100644 index 0000000000000000000000000000000000000000..c4754ccd0b6d25f937bfb5457578579e95628a28 GIT binary patch literal 3236 zcmY+H4^&gv9mij;3E_pu|{4YdBLIttZl3$1N;-=ja%b@O?5{3%bulOnQrs*&56(4`~1emB_99P3e({~bhKW( z-0Cm>PxeFN(l-moWGShMlMmMTBERX>j1DuMt?M>!p1ZO6yI0R2+u!7BZ`w0EKYM(C z$(H;z+uY3u&W4s$ZP~oxtM}UN>WpW#h97 zd{o>lnBZnIPWwqY9{KfHk`<07!T>AXN zZPS$JXPq3ZXuR=y@}J)rp?lU{eD~PHN3R|0S5JPh>aX692iOG-io?5Fm_=s$(L3^2 z-&j6eH(j-4O=0wuO;g8yzidv=^p#iI+&M=spNw)Rh2A>Tn08K5SrW=ecUo=W& zK@$oaU;-T2&TjdvGtNeiL3*&=q?-rZbK>J`yJGx2jUsXK!pK6E7{Ffq$;$s;@D;~t zkPO=#&*{Yg_I*ayCi`NAokpqHE;E+Ji2>}-uAGP)kL`BQNRI7Uk+T$H0DF4LGYOg3 z=DV~=f^B28Q7s0r6Vmr|kxSp6N~2_K=c;q{VgOs#{2;}(W;C5f(b)E?y}TH}E;uBc zq5ACy9vV^D&Qs^<#Q?Va%5{0uDhE;QsE*03+ z+*i8Z=k8W$Py}wx^Sn_EU>9pzzigP-zDSPn)nQc;s%S9~q_{s4R<`$@j2G;Weo@V! zL`ZKVZE9%0ZDB)CY4_GT1|@-=5t*R``|9^Sz42YjZ8S3C-t*P@dNF{Vvmv|PF@XK%bNc(o9~HgNpjb$rOeRNzJ!ADkZq=tP z-88b{)}?%@UJPL8#0^Z@yI%JJjiRufWy(qv1K6{tZ|wW0weRl?io>=w+Nu!)*e4W! zk_^fEhG;}$+h_8b#Q=8Uft9+K)t?SAC?1C9B%K_xyx+Sm>)UGQmWus1^u)j@ zDArH0C5m;XIiG}G zqF8sX+Y4-?*fPFM4=|$GkT%4@EvQU0e4X68pTfNqNDE?M9}~VhZ+vC!yc)~-(tH}O z6pU2Dix7p*p(88{@B|s)i73_-VPb)23K5}D2@s0sBk&VsJRgVC6`qez=i}jsQmil5 zX9$$A?dmKf#{}C8#4; zOi=ZwgG~M33@O$uGgDBN5+U`olG4Hh$tcz;wMGFvaLH)3KnadN9<;z`NC14svcP4K zuNL)`v%wu}@j7uP?(sUo*}@AVuo`N^0mZVwXpj$h<%5UX`kQc)ENPYjmGikOP$#o>1;v*aH)S))a4a)+|L59VbC`EpQT;F@`#!J$H ze#|L{(UA}ymowg(7`{jDIJX5xiN~Dbo)#|oNFN=(Ri$Jp4I{$@AJ#s{!MIF7Xlw4+Y4ceY*?~o@%|0mmpV~tAv{d+`&nN#{+7^yCGp&;k^JE-V2;t zcqc%H8-SAtj|9j7R4faR1IR#RoJ4pJKwep&dCu_Rh3}D?^&9*~$f;(7reGYTs;OMI zKsX+?H5IiRgb!K>6F((<5JXJ)Qx*Oc{^Rh|PYWLe!G%BB!%c_2EvR~UPN7n3b)1Hw PW0Zt0>}$fGO!)JEs|c=R literal 0 HcmV?d00001 diff --git a/.cache/clangd/index/NMPC_setup.cpp.86F9F0381989E206.idx b/.cache/clangd/index/NMPC_setup.cpp.86F9F0381989E206.idx new file mode 100644 index 0000000000000000000000000000000000000000..ee7f67abe0f8bdc2e31b66d091ed7824685d5605 GIT binary patch literal 14226 zcmchdhgVcb7r>cuaoOc9W%seWz|z~&q=+3AR8S)h#uB5L1SJ+gz_wsw1rdy&J&F<~ z6oY~V>_H@g4N;>3NsuU_*hq{q5n}B6&6@MxT>k-cPLlm`Uzs;^`^@O!k&!EVnV1az zeE9sNsq-dFOiWC8`tRI%bK>uHF)?YfFfp0G?T3%9r`+mo_0zz)(c}I;vZ&9vTj}`v zMTgHa_Wja-LalAaxU>IWB@-W(R}Wu`7g*tmdox#rc5eOa(W+&E+{wWGMJD#s`lhFT znWoA7q5X@H-x{vyUq@~~H(%NK>%w5$=(x`f{SK0VqJs^e+pWH}`N)`B`Q6PPzB@W6 zj<{F#xc0+juIRJA2P>{^ zz1%v`t?!D+;*6BMzZPB1J5%f#O1j?ey(aRr_$5Do;rP#jVc(>-&kos=u+*GAqh_V-+MSLKeXnwhtwD-L(AGcB|jWKP<125&LD zHegQ4^3)txk58(?KYv(1(dTi-x!fx`#bMw19oRR^W$5A+sbPHzHznGX70o&~_M6y4 zm$v)fX*+JX@%-zVCG!?+n6kgnxu02H^wrYye?4?OE}1_1U}J6nX(vuj?-p?C@FQ*X znDmj)ya77-Xw9DLyX z!R49DuKm7$%eB~_CI^rDR=z2=#87#DUDc=+|HN)yb?S>vcmKKbRsRdoXztBFD_etR z`tDh#v9Ndain0woyJPRE@(G^t$ftCRy;opAHytCh9ecp~i z#S4lL|K_8}nvk>Ujm0IeE)9krH}=^-n-OJ*PO#EE{q@Bc-=>yMon|O>zWUXW(OYk4 z{%A6O^qAOBaL-ZIJ#Ws5`#a;)rUjDYJGy1{rc@MA7KK5c`cc%yIqgUVQneTS=l;iCsp`o|ewhUB$8&us_b~VK7 z_sve1PhB$`8p!3G`yuk)$F4{6-~8HtwZFZ_u*|0F8|(2a`glJb_jHal=kT*_X;;ja z)Zbk&TK3(|wDIBk2O7^a)nWT<(DCCZoCb89kDIzT_CVj*qM*?W|qjTj7tXxCQ&S;9gN?&SpNs1?v;L zxF%*^y=jFf(F+u6g=>^tCKn%{@Y@fSu7@puv%({&I1j7yuxFG^CKHD@XX>=MML{Me zH8F=i8QX4(BsxMIh>I{R9T{sk){#++T@0Wp4pYP{#}cL2Z|g%%tvI64E4m5eqBKA; z1khN;L_l95#aaZo6e-F92e<@yF=?PI(2`ML5VD>R>?8cG332ckyJL%{VoqNj8%J#cAVg1=%IH1uHTJjIl&IQi**l zaey$?D}@iDRV!+(5Kmf=rUhw*JGA4Ge8|iUYY9huCB8CYa2%Penk-(#ktv!fI^hT6F+l5D?zqq zHR3jDX4}$dcOXdz97HMBl(Iq0?5I(!cRy<@=g9)c1@6LFd9u)cp{U(F$wq_ZdtLUD;C%mIm;(hJJ=nCQCI<1y`nOQblX!Nt$UIkfnL0>4p2s zNO|s?^I=Y1I1(TU=qe})N0ObBU4@G{lH!~q(gzmxrF{&|&eZtxaAY1HA?QHbs+6E- zwOibz9O)+MX2zZsZxOF#&x&`A_hgiyNU#RvngC>p7Kz}#MAt-7*1&};CKnfaVxHt8 zhg{?)Jc1`BNLPZy!A=`}tIMU@ET6Z2CL87Bl}H$_(bKe{0(?jO%jD@M3-14G5+Iel zV=ob~*sg8ogG@Q%CGi3??=A5LG*LCtM))|6 z#Asqfzt53Ls!5{kywBh6(%|f#^l?7sKGtmVeTMlEM!v4TdPaV3e%_2)u(Sm$guG*B zdqvj8p0YkJ92v|F7EOne{a{h)ITFD|09k}>gs9jYiR2=IEYda-$eNH<69TK-g18n0 zu5LljEo>P%avgEkq5L{>z7FNBh--!NR^-`=L^bEg4aD7m@*BwW29&oUZ5t8|&lowB z9f)T$--(o+kXK=D6$ay7g`HPnQSCUg8f#Z$@VvFyWi1xflq2h~%R0y#v34W8J_m>9 z-~sIY`IyVc7EpJ&0?37!D}-Ext&6aVUB4f5`=Pu7%PTO)B(E-o(u=aNBn!*g==WgDJ=jKgy2Y_COK)5%7(g9` zF*5y^Et{BdZ|#hc(asnd?TnGp&KMc(jM0B~Ch+6zwZp>_UkrHb0Ve0;cwrPq`_H5H zUo6_cS%S}QkAGqY(p+PSe2KX*e4Z>t@}&rHI+CX|7V>7scRChAf=I3E=XTTvJyTTdpD!_gMZ+*qj=;d5Cg64%ySki>$ zf`B7eu;dElW-Mujd>xx#$JT=HX#8?s_7A@nGGkCQT{T~suUwdCniIp)sX_^Uw{IHHPDel)VmP%Wn6@M7eU^RLbkK1)sZ^XuMYJW zuCKflH;+&9|4`161)d9hg+Fs-q1!^>jIvShY<3ezGt7^((I@QcYGhZ<5;=}$ly)_c zPawM!kSnl91@>X9bs9UIhRa-s9qJgz#!rsabQD^9%PTztzG8`FN>l zc1AN4?5X znD9|LvJds##}fQwUS0EA`*oj19m;-ezaKjY3vPdm%#!L7X3T~nN8*xkR8v&@V0v;E@n@mD9%<_lNFA>rn=43cC~)L1 zvb_u6cOPl*!|OYcQwMSuX27UtPMwf9VcSjbrZ2WRkPl;Lt=8X6)@t zxFw=_8fy;eQUs(+5x10?FHh2uG#$#*5tk0-*(iWEF4@zwQ8aB_GOk9}v~kI}23ga_ zCF2vwnl>&O-$JrmNG$1$?nHJQas}4W#-^a=#;zt$8=H*lFi#tsjGM4y6LfZ0u;UfT z&DgP-@s{y@qpi2i-tyMI!t(KO;X%gaaYg~2EwEz0lPccft1VVMO}oqHb-1m|L$9(j%Rbe=s ztL8|!CS3Hgv_0e&A=(fnj|j1a$B}JFw~Zx|#++tIIf@Y0iz5}twgToPN0IGO$koWL z8s8csr4< z6Y?tTvkHTnZ!Pv+i~ZR59LCDSuqAT}Tb{yJg5q*?eSJ=|aL~o1gmgtfQ?n~m5fGo)|N!)|sNivrVWXVp+Vv@|0 z6fOnGQXEsjeFnKf%z}7g;0@qw4Pl0GL2YOllBa?zQ+27J_DYkd0a=<~n!j-0p(|4i zeeW+WpIo_=FR7PE9{=p@zT#0~ z>3dNZ(&lkj*DH57-Eb(zTrpM(nx3)x&E$d2WA@NSMgg)ffVN$T>l(GcNAo-`x3W+bK}u@^a? zaev2^)3B9^xJ(vLIr0Q?PZ*C%`D$G0qBG^R^_GjcTv*EGB9B}ot}913IJgohFG21l zNOUtNo|-+@2L~PDd9oZym%|Jp3rVveKSk1~ke?yxGsw@8^f}}gNcsZuDnvKD)+#iEYu=oW;>I>S#OFb6<9-)NI@?2^19M< zB&G_`d+cr6n-@-9aHOAUKc2N52bvBPr_Z=OX`06pD2^VY(Y0V-p-J8F!Gk~64pE+b zFFq_c{`!sZH`Qlo^bV2)!N!4elBgPVpH-0}wh?0A7j$tU`D~p^Yl6E`VH!)rE|oJaYZbhga3p#uMF**~4_*8sBD z+XHgaI{|Xjy8#MUg^M4|lOd`hfT*ut&Nh{JLVa}>Uxc}SfH zxdi!_AW)W8Aie^2pN=B_DCA1SS3;(-;VFDSjSbHr)7bDFGK~!{Ak*0J3NAm54X;_M zXY91e-!OM(R1f)53<^#f8`9Z1EMu=!o`FG@L}SA;)?zev>g1UiWLR0)E{pBR()q-0 zS?nmyW5+JYG~94y27G8`yq zAWwVvj{8{)GI(+6ewPi6SE+5LI;4sngC*uaRyptJy!u|cK&!sXxt_A{_iOY#R6t-r zFsl>-wSi(gohMP+D6n;K0NETspcirm*_>ffh$n4G*TxbeBYU;MzQk&*TaCpkjVBwi zZX*^WDNm2v*AiqS`FCiCWRWUR4dFRJzH?PfY zdD~E!m5=8M3uA039HF~WmEvwx*^GZfB3^B7)$;Egf}_nbMZ9)`4icpMv zi`o7C4VKs5H1)k9(~)q?aM2Cu$PmjRfVLs!Hny*!qfSqm2e}-1m$Qc&chL0vkXV!J zXiveag6+)c$Wdf<6mli9s$?u!01bb}^R+_o`*ZHGBfIB&wb4DTT4Y|!oS2TBLzd^5 z6E)^smTOteGv-{D>mcV~pB&bO{@SwDChEzgThs}>yP;kS^IrVJbKf-)H%!Lgp;2=$=JqmYceQ=AVspv;J8z9?R6d?449{px8Pu3o ziN-XqpZ9@>Q=Z(RiG!cSPu!7jxOXz8a^TPH);w`lyNbJYJn5$CrWKaK*fMu4XB*$f zmbt?|Se?}(Z7tijqAfDit&Z*P8@Wdv+r8)Ma_P>&7}bFI2BuCtxs3SBkgp(~?wm1u zIgMqf*)a%XOHNh?`63Rzi2Dn(!P89zC%QAmxE_brLwN&sqB~Q9T=+j#zyHo}3kn!A z^_$wDhS?8j;p-{!1lgosq6aiaHAWmON5*Q#iYX-b&W>-8N#8>uAIcuSmrVHS_z@|c zKP`Jm>#7#y-h#Y@+0@a7oBMUfeH|a2PwYDR(OaG8Dp_ULQds=!-KO?O^kB?L+mT{7 zL`TM`$B2PRM<%K#0-9_)S-dje;g92Kn=~DC2(8Gc6?J1nxPg3bFs?W=ujhnq`p5Lb zgUItBbR>t6=OM;zJBih$Pum|W=%F{aNHKa*@`w~;8%>pwE|003ao2Y6KGr;-hpCZm zB}?Iq9ZKI5%$sp^v;nOwb{Y@01>AtV4*RZy9mgCTnS;f>586@1gDUV~L9L7(M%%Nj z0^_Lq`qZ(CEo5WxSbh4>PiW)t6>@sTwtIQ94C|J$T&vp}xoPj?H=n#0C$x3r)K>qp zfiK=Rwls7XLHHbFV@pGK5g0dP4c$c$ICz)5YFpyWm$b#=FYyPb-bboNisdawh=z#S z4EN44iP`f9{G>a7`4u%2dK#br`Wbo}pb#=W4NwG`o(9+sxe95jSeP*SCA&H}zeU)A zR@1`B=S-bB*?637?wq)}YmGm~82|I%Kcj>nCMF}<&++W%hvH8}IzwMwDv$YYsrl$J zW5<0o{=?`|Bcn_{5-%_r$9|4xKgY74QS4_l`x%$?`OLW#oo7ejE)vtOQZt#{oKsj> zT3M@URrl;Pi=Q?sMbTX#=!+ w?O<)NHeB07+eh15>#z0JhG_?CUA05Bk=mi!VOpKmfwxiH+YZ;*As3VX1M_btSO5S3 literal 0 HcmV?d00001 diff --git a/.cache/clangd/index/NMPC_setup.hpp.CAD16FEB6583910A.idx b/.cache/clangd/index/NMPC_setup.hpp.CAD16FEB6583910A.idx new file mode 100644 index 0000000000000000000000000000000000000000..4f031ff590a1bdd32b4210039fed6ed331362fd5 GIT binary patch literal 2236 zcmYjS3s6*57{2!ayL<7&SM|(NRgXRz{Ns%SI(M0Uv;*jH4w! z7&RMlus~6BG#h-NmY61tV~LiUQaWZ2Uz0OpQKn;K25SGg&TrlszS;YI|2cEdfBx@d zY)*FeK8qv`doyR=jKVpS<0VOQ!e9BEvgt&YBo~pSc@3>KnPa|sR$F?LFVai$m0{I+ zy(23Ne~Cm(_eL)N_;T0prCXd6j-HydVPkjq;4bfkpU3?$tT>u7so#;`EIS-KdoLxv z3p+nQbN={8J+lh_-L>Adzb&&fqh-Rp`FrHFgqs(py}cr@t9HcI#U5Y5O96A=7nTj* z)MG^DKJ(tTk+-)U-8$gn!Pif%nQ1M%v-NndhRs)^H*cM3y;oGV?s(V2BHkuSHDSI(F+HBORh$2RsW zx_;KA5uXALf`_Y$>Xo2*;_5LFX;GYyis5gQx9WGt_BkBm{DR2kZu* z{QfGLu1zfUGU5g~$b)tRP;QFtu77x9NPv+zkUgr$W&p}_-}e;;xho{j1M|{-BlbzG?I+xrV4eT zy#BuPt>=PGqZtXn>QsMf!~m4%2KPH=FKW+c#09cTbvX?{`KjmwJ>BIl(1;T^w<)$n zlymFyb8-%^Dq>mt7xugBnf1n z#itm6^5UoK{hm=1*D+!RIn2Wj15lpSmvmkl*WO|#Fh1O5GPw+p9yLMc3H8%2ULN}J za2q4>u->bBZ3f`_Td^DUx4%z6rx6GC%@VYvqg+!Q7cO4%(-n>QQMM}9$9FboV9!tg zoO1UXBQk96@H-*~;Oe4|)-S@%yh9@lS1XG0c+&e8t~-|bU1=vHDy)VJ2^xT_uYMBK zPQP*eZ$>;ICtH#g15nP|Zf$8OF6m~(1hVee!v>&SRyei%S8&@aF?(7^&7gz?%B?Bx3G4)uQ9g zQ_fYc<6Tg$shW;EP_C=Gj-965uG)1xC*=;+q2mNlo~owm=;4y<2bO)?Ux}J1_ftPZ z2T-0uQy6-Y@&FAm><{HZ8f4fO%0o27(3X^kX_%oCFlOL*B4hA)7%y-wB36*bPl9TZ zDM(|S04=fvX^aurM8pTu_{mT$vIA-SKA=TrAdPVWw8#piF(!Z(8G$r52WXKENMkGj zEiwUVYy!|C3y{v8wS0Wzg0|f-AQ2jIVvA6QY7q^O{=rBRW&+S65U>g-0uC=S0O{t# z*DAN&8GlX;Uy_oB!-w{OY7q%ojROIDi#$LYS3$Li0Hks3pj!AJX|z7j!uLqyAObD? sjnC5 zYb~5^7q2T517U-TqrS64ar8Z;A^Q2?+Cxz9`>{p&j~jqi#Oa`V4Yq-6R%}{4?a)8M zM)^SXZ2Wxbk43L7tGc3{4Tb)ZZ!0Y)I#(D9F1)t6G36Rn3NI7t-)a+Q7n7O|Ae2v^J zuvIeu`|o)!-8v(-_X`MjqDsGCGcfhqc*#bo*Qph+kgE5#isr~z+ekyDF z9RJAE>7kIE4lU;kZ8xJwcun8U_`wQYPm*s*vDayx!6E6z{6vE#9QU>jkz>CE+rKUE z`a#m2U?#51+dt}A@E(~;Pw?dShc%bovhq*Px#xPk?!@G`?*bN*8Wep~{Jg(&GXF4> zc}$L7YHMox6V?{be4U^2b7uDVrzW|Q5vOzh_*E~fv%AAvcH|~j%3o&7I=8l8N(}9~ zzAv@%i(S{vWc1l@G{eghSDq(asOsBP9OYDUMY_iQK>~Zyd@Js*zry+0RsJ?cA(L2W z@<(R8OHBjo+}cL%zt`*_wuP>C%L&bnn!Xj$F_Qkx&hddp!Q4u&WoX7-Rs`vp#9r~R zW#_lM(?=VQBzd1(HqhwSV{>-gbJW3`eeZJ?xv}PAyRnh4R^^#}?GY6(zH)lIa_@Sh zVdnANNvM*qqZw&flL&l+|^_Tjo=lQ|#kPR#{sFbsYD z>yhafsV{9yNt;~SLnm(+L5Gcg?My8TTBr&%9o%-7awE7Vgw^GE@)ySs={|$B0E69v zcg3H*DsaD9d*Y|mT@t_S!M6O%S*HEG5bfWRKjTfA9_*GKQCu^**c|P?$aGt5q}HB! zz*Vq9Z=X2$uw_E_=xlf3_QkY4|13N;yAL%YbvH)hmno-xvQ^8zMPB>JgYVm?$vIr03$!bi7 z6mu05lfXn&BNNz!j0u}1XG?zIuHB=Qk_Kns)EF!&994`n(fOm65+;?}`DCa9-4s^f z;2O0cJp(Qz_AoC4@W%A6g>!9{90L#PN#MNgyA)N*DE+kVU$ zCJ)Vmy1s;{=m_QAlqT$wIw(6rRv=kkh z#c$Hkgz;M11RWxYOwpwxG(9>n1t!1|H~~*!4VD25zyX$E6<7||0xPf@xPcYG6}STr zzy-Fz9ykCS;02t)2Cx#W0WQD}ECuVqIy4Jp3t#(9X0d zb9Q=OGF>7q{h9J|Gh`BkS}{WA>duOw6WtpXrTKM6I;^aAr{k`M3$sBxDt^tI%MP?fC5NAyO&@$<@Y_OcN!K^~oL>3#r6suXy!G+= zyy^I9K1P0Nf4OySirFg9{^8ma``o>v*EcKHKX+nka3sK1e|IkM&0KJ0?}ZoAnU`(* z4&9y~>eG)A!%5?lG+RT-pT^23HJr(9yH~@-->X8)-7-5zm%nP78NFv|n_F?8vsg-B z4@rw7uPl(gnx5{2;M(mOiP!vlY$aSl#OJD7_vYOnI%XcZ_gEfgG`|nM;aMB$>~X(i z>g)IZWyVZ;0bZ*BmY&Sa(7bLWhWh1PgSxi{PRW3*cErY3Ac~fyW6}P^=ZSL@a=- z^h)Kkk>MJGRTLWqqfRV{NTU|inV~1GUZahGbNVB zy32-E71jWDk~tXxd-b_C^Evaq1gmLpouD&{1#sNft3^eVE&Yy|3gDy;Qmt5!Oq{ud z<8N-c^J!CpdYE7(-ER;KIx#EX%iz>#zj6%Q?9kFv?9oYC5*at;8rgg|^^S zA{Sjk3@&NRL_!P>Y0Lyd4DRUJ%o)Vsj6ThTK@6^F%tS#9j%dsTK@4u_*~|&V;DpA^ z1;pTj#>@f4;DDaZ1V9Y`SZ<%W{WV7lj!h6C%%ARM!-p8=PCG4rn$9~bnVn!q=Z0S^ literal 0 HcmV?d00001 diff --git a/.cache/clangd/index/acados_sim_solver_auv_model.c.53A9281F3B2EA7B4.idx b/.cache/clangd/index/acados_sim_solver_auv_model.c.53A9281F3B2EA7B4.idx new file mode 100644 index 0000000000000000000000000000000000000000..795aba4db8c99b5dd262916d8b663c4962e96b29 GIT binary patch literal 5454 zcmY*d3p`cX8sBTRm9%#g=R|az;}zl{oU@NdMG&u+5oo5`+sX>`3Ne!BquBi+$ zrV>#M(~O#=L}f;=nbhUdb7pdjOcG|g&0TWW+57Gve*6D@-~PUDed~Ly-?; z@Q0tF(CUh-vRlwMp_OO)*4gY}l9l9GH@UiaQ^kw28?RdyJ-O5Ic&w~C?~WoT#{G^s zuzyEv&bcB<)Z~)MRIlrPtzFi_*!1}Icf1##T5q$>CBQVM^V`IUvgKQA2jpkIyD#ra z-s1Ig>9e(V*NktxEaR&!9)8-{<0cFL;cWFlp6*}Tfu6f`{xQc|S=pstZc_HOv)JsA zpQXUQckPnsN8+!&M;oB?VAjIwV3%tzPyUs0Zu@sppEPW)+dtwLez~T>H0xyij9!iT z7n$BuppMQDcX5U2M2N~*2CfM!& zc4)xS`)F8=G$j1?ZC@Y@O4yiszOZLh>R12e**Fiwc{WADHGVz0-^?yw9+%XkWut3! zWXJVWKR$a&4;@PkjF*pTTd1{?;Rn4r|H}w&-;v!TLSO9n&M&UatTzS(!~*~N?In^V3ww*0Bx@mTCfsZXl@Q~dAV(lq1XD|U#;raK3w0rqb+~AZl-m3-5b}}?MJP8 zha?>l%$u%Xk2ZQgdS+Iu27J;pm5~<#L3s7K>L?SF^e93^5KJ|kQw_~!AqF8X#B34} z%fvvY<~2^q$ytj{DZ&(qIFrs|S)^{H`#XW9OOU{+@Wj1>VZJX#m?423klX=HWdXVY z&ctjrNW2rddG-GG{G+uBidcw54nX<<6w88)g4_vR5;(h9Oljfk|B*>~lu(2r5~+i8 z>!6h^)F^b?kh?V!v1u9qAt8Lrw-lj|L=M5(hoFgUgK&f6J6~Kak;noUMTCW~&vz6d zL?VrFRwFc)g$cqO-idhFAd#xvIbOHBGcHnu2@>fAWG@iP{B`~9*%dvoYTKhZXg5F+ zMo7ei^bpC`>#mou0zbNDwf-qAeoherBv1usRUuD;1;Nvvyff2Qx<#j@*7FrbEJ7l7 zB0Dcxgd`%A(48@3rukyz$E&Js!!HNY$0))W2|NYlQ=l(X=ql`3GXraX> zKqBQ(qZ~zcqi7?FEZQA6E2M=;i;hXRnnw`9k1F=O`0X}93K+=(i4dC?F#ewYUU)$Q zM(!t9ut?#f@W)6yOefp;=0gAk%nLxi0DSD=tFNRz&;LF<6EU6v@)^*^4C@8f#fgrM z>WJ|R(EJ66u&S@GH+@j(vyq-bF(1<(3o)5uMu5Qxu*Kg~ObO(dz(qJaLfWf-`?aW< z1{C8(da0wf`R>t9+k4RbAD(j&xd@O&U;);rSuv>Ay(ycA><)k zCxO-^uwro`qzj?Bs)U$AxU3Lvz_=jFWI;l?-U773)4=RBaKMIe-2%P;6)g@Hu2#gD zDpgE2jj~!@-C$*gVwg#0&c|vFoB#`wWR7M?f0A305|dkgzS!&V;H(5mXU)#yVe^QZz~lllmy34;ayJ*}0Wy!pbA;sNm%sJ9p*Zye&3>SV zo5<^?*0a83fIDJD*hIXK^%qQTj#je^T8_xi0r?zg;SJ{v*JqgM+*^SdsZOa8SYFF% zq^tA!m)>aMML=8xoG{~1dbE}2WzV%}>EWVq3rwb%IB}eeMJX03QjBo!pbVJq#W;*N zAIjgjwV2A0?L>#fAS{oo56MqAW-NdF(Qe*k>m9{Rit#t}UxwuoTIZk4BDuAE4F`qF zx3fqgR(Nwzup$eiS+<4aAE)mLKnpt#^o|2nHc`w8pm&0cYk^)Zi)}uZtZF>4F91?Z z1t2TH9ITN-4U#B_vvAXtNi`$Oc^v_0xzWzio3NO7^5D##FRq0na=3N4sxuy0{^s0CO2%iEcoK6(lVzV946FWnpEwnlS zz3`q=Xi6?R1l?7W6UE4^9D3pyQD}B8s)FvgU?|j07FWX+xV=zl5L?$lZ`E){Bh{o4 zy5TX~FeEIW415rcf|=}|tV#;|(}&xBdPb%qay~H52X=TpH4jod8ZY#vq9})mLOAIh zEso}-v(iGTO6S)*14nOIuTMj|l#a?Utm%A$QF2vT@-{?{@rX&qWShIrZ9V4(x1)-$ z0%R4Kix~puJRr|=aVH=(2*H&lE8G0J)DHCjw?aAop`|F(8Y%xCD?TTzn9a2f6q#AP=*+ zDEF~tO5Hyj(Rs(8_SeVuDJGZ>=AaNdgo8rqP!0;C!#F6M4(FfEpXppNwmm7(?|K&F?LNvnadnHp-^eyw$4RpqO2dzY7}Rpt{C> zpRn5N?LDM4#W6)S!Ga&(Jz5jq(2H7LBcK}r=bF?HO!|Qo?-GTsT?Ws%*QQgDe+n+b z9eGjdV8WS}J$*?3P9WL|tg(M(zZ%uo8H@gfEF1y!5ukE>s;Qa`09)3=b0ByQ7PHYU zh5S-zh${grMENpx8O95{7uCu0>HKT}g(XNc$QVm)Ie8%Ore5U$3QIPbt(w47XA|N8 z)WBzxtwo=Hp?2ob6C_p+$a0{8zn?m^5QpGQY#kp?koD=CHn!j3p5U@iKj8)1rLG?U17ncY6uR&tOlyB*2 zo6<9;UYw@>Og~cmLJ+p>tBiehU|${CS3A`!VE#EM*B9(v{ZPZvNg|cl+SxnE2#JbM m*sESQDWek6ze@=np04_AjX4xOciwxNTDOa?Tx~@biC8oNnPs$CoZa0pWG>KR}VxzT&{e0_i(FwZ@=r~#A{3V?eo8N zKKj6Z)AY#sfi~Sk5AGNTWlzSs_j*h^{zqY4qTjkNDm40t1DkgHYn3yf&AC0c>9PA9 zzw*p(^|?24O4A~@hL^i^m&Yp}+`L@#v7sq^m;HqE{)Kxl9;$AogMEgAUyX{!bKSwo zKV9xKbT_}~>KyMnkkISxVKz6ZKv5CagQ%r(c3xj)@LU# zwO)17db=LlJ9XKzW#0$XJT{&y_PgFy{oN` z&riAWLFLQ6yA}sUPgC1Xcihgs=B7J#ac;o%)uXNlT$s4?ruL4oaQDg2Px%+ubltBB z4!&Joy(Ce)#`x&Py1_0*dt1l3(IG4E8W*HKJGJTc%`dh#9*$Gi|LAi3VX~(>wz22p zzv|Clek*?8s=rkYJn8h(7md1?a=zv5U3UgPS?;he{=;bFFYc%NHViqvS5|R?l&vW( z_!!1w|K-o;)~4n-@??}d5jA5plbIp*FfH)=w0KF$jE=VPJP8MPyedA18FVl1x%42m z;i!TqQ^BoM>9owC+aW-{%Inf~a0h@pMimpz47!tU=_ZtxES*HjxZ%@?X!JBj5gBJg zA}8C3Bgf6FXkIrt$Ac$9kUvv3Q_YMe|2dt%TM1lDQih|czzOM@MdqR)gR%UvL zJPCpPS*lrDW-$NF-1ds1!rEy(2?TeFDkYW~bWeLRx%=JwAL)2v5AHCBFaatQ%%J=Df|AW{{Vua9nS{k+%{I-08R@d{d?6rx;OREuX7!(WG8!sS^J;fyEETx8 zKUqWdJ1Kkm(ix&jFbyA|odWsW3SDok(stDIghGCZV~8s=m|tqDXz_pVR3lF`;7(8_ zXqZ8FUFE&0%a473k|&|yj#b5~nL&5p+2<`w7L=c+WDH&}+ao)IeVhnSIDKN2A>VIy zYBMEcF(A(^FPsJFydYrhpLBmIDtXq%lVCU?S(QA489ZQL<>;hq->hz@WCG@AC^AA` zef@dP4V5uE`BywqLw=kpPR|VHSFiZ1UAcbC*F13mceq3NL}t)^EzaTb%76NF*|F{i zIt5M@CG)EL>-R05N`3!xgD2Ai*4(!G@(E{;#6G2DA_nA#=9`FY#0cW%0o{MPZ*BiTZ`mLv z<1xUXFa*DgC|i?Vw5T>j2SF( z&!dO;cJ5jJKc4u5TdUG)m_avLQM9P+3JiK|o~kMHppDFtis>X>QZe?UVg|{uS{aDJ zYL!VctyWni%W9QPvaMD*B*$u%OLDDNMq;#DnTW}1l}GZdR{12~YE?iAEGr~f8wW? z8}C**54Cn1oT#Yfv;xiz6{EN)0f&r=I!-5G`=}VrMGN>ssTjk>2-rg^#&WR&vKJNO zxHti+f{O86ynx?^iV0kTfHX+OL@rUlcSpsU+)M#GO~qs`SwO;~;w)~KfPJN63YQ{a zJE@q;r3%=Wz>3hslRG}hw54JQ7cv?NnTk2`95;Mmr~|yiF#ns|0?$F6KF2~7Vn%32 znuQZZLx^Fvz0huq0s!XQZ`ms9SgJ8o)mGK9(cV4KNck^WOMS0G4zN zqfo*Cmh=l@lrDfJ-9i{A5$a}AW!@Vf3`9#Rh0*0lcWk;k>C;~LvKmgqBTK??%CnR> zoU7_;J{oyu_k9Qo=7JsYV3=8Xwkv)EoKHBHNu2rIM-6StMLV0GKyD}(%Hv&NZsjH= zjttHy}mE=pAWh)BHrbz$z YNM`sa7HecO069|a^EWdHyG literal 0 HcmV?d00001 diff --git a/.cache/clangd/index/acados_solver_auv_model.c.5778764B3A2FDFAE.idx b/.cache/clangd/index/acados_solver_auv_model.c.5778764B3A2FDFAE.idx new file mode 100644 index 0000000000000000000000000000000000000000..3b988c84ae8b5ecba6a34e2d18efb6aa21c525be GIT binary patch literal 12412 zcmb7~cUTlh8^CXOXLn{{=>i^{BE8$df(RW{zGLRy>)7mO+DhKy?Lo7U;;H6sfkRQk?Euv{!9}~) zyQny%-&QMiXy|p5`qy2PY{tw_I=5&MV`$qg{I80)>$?pwd15+bB5&_H(mvhlq|Lrr zv-WFm-a22N+5ZGzzIu*RwsTHWv3dVfcAK~yZ_5jYE)Ii#8lIXZ0N+_J*N!$y=yp5I zCp|S{^g|=T?Oki{-L7TFjYkExjX00m`;9Z?&7bwtDPGd{)%{EN&;Rx3ZvR~&TZ;>8 zv`SnmC;z#$<$nJKcRj-8TlHg%%B*@HZB^j^acJ4$In6BgS}mTUWpaIr*2UGijy%-3}J^v2y+po3s`rL)4s!8TnqUGfN$8UAC?i6!NXSaDpfz~<) z{ei#Q^)d3-n>QybJZ}A(f{qS_*C(yZUfOG`d%AXrS;4`FXV%Qy=l8_bVC2#tvql#V zt64YZT#j4f)my*xec`bvef1zi^B+nBx@CIQFP(qHdhi*3MqUTQ{KH==Qac5j#coMU z4fkF9i`;92@#LZJUK$_X@Tcvz_~5nLnVq^{R(*F-_6l}87i zSTrcly=$Ro=xkVX{C!}bpgZrDrZ2D4yz=O^{cOyXKEGe>ANH;)a>r1?>&2VGA$x-1 zywIn9oqz9hCEqVHX;$d?ifsohQ+tl8c06`>+_gdl5x+~bi>7zRa55FKNVY_ zC=YnV{NeBXw7PZB}{fs;$qLb0I?9n}@Dv&j?y3uWs4oTErxMFsaP* z^VXj9p!){vJ|BkdtI^AU{PA*`-R#}I14|#B-jseRdCv3ng|nKw4f_)sd%1)kX+5Cx z_{`_*mcF4sL{1n#)%1?u68q#&12&AX@3qixq`_qW;kKQNzHGJ9ZT9lP)0Y=xs%_>) zU-Iq!G}_)`#k9DGGc2>8X7{}jFk|Emqls=Yg+aR)ye(T5dBa?L?5&g~f|Vwa-?}d( z)@=GHozA5vXI$z~{(fJt=Gn1sGbPtI#jkhO_?NZafBD|p$Y#|CA3n?t@BDty?MLyu zpMRdZd{1lFLhp*%r}74#ExB{LUCq*g3#~r%a<=nt@!R#ar9Yc{rbhpg9slyL^**Q1 zW)7TUzVVjP_#1Pa_AEZs!#JbDf7956`%IttHWt;!9i)M)!op>VS*`2(-c8d=cI|PX`q+;Hx~Ws&e*BQ# zN-uol2=AaW5MO=KJOhn-uT?TG@qGNJW#I*1j{Utd^zX3v>Jw*L=IXyHo|~o_)N+Ma z)gPAcPU)W4{%P{>Th2c@A3nXX{_3z@SJ^u~MlbAZA=WMVBQkw-v+C7fzUbT!xRZMZ2u?WPX?!Ma?^)_ahO4Q|5V}~)myjgYFupaJ5^(0km zEOdsK&hReZE;zw`{V;#;tZi{nHj`9+u*hKq4kLHp2u2;j_^3o3(AZCvdwa8^o6?;m zRevmW7lOOc#W$2uhcdlXY6HZ6z1b3{$Upl&p5`p6x?rJbQ72k-^i5!_5||!edzv^1 zk7%1vJa_vqx^9xH0~VSNrd#^1P_O8y+-l;0(lZ@v9(R>gE?6J}f(U5$?Lvm!wxx9l z*r+o^QuW3nYdC%lXZ7tuM2eT6j*-`DPm@%AvB)k2b|Ew0a7G=@v{7!gaq>8I_!;em zlBy>b`XKNh1j}z1($8GuVT)eqbYCK=+^|Tn2!cf`-&jT+%XCt-%=O=!n-l(Eouu-@ zLSYDmAxGb>jCyOs#G7e1s_@;E%#F)ab_GkSj#wy8)Q=O}_$D#7Nlf3bBO3Pu){f)5 zM;^gDrbS9B4=fZf3h|<~Z!%+^%yj-LG<7Hz8Uu#(s|cx#kyOrDXq@@DF22j#E+4E? zOOmnA04(4)bccu8`?;}_sxuad5~V27-Z!4HjAy!i?PbD`SZMqYH;1BDK5>!?FMaYc z#`qZ1{o931e&Oa`$809=OORAfSY$4kYwo*>QLkd!Dk{`C>trtLu`f|lwZS4G3?IVS ze!GxfybJ=icDjjnNvif(Bng5f*v5A~qh60EuGWXlSKs-3Wb31qQ%3BQRPI0+mj~|7sBrnllVnRBWAuM zNdTV&J*A^62&$l-(owXiiWaq-GCKAWRg9=jJZIe5#PX+`({%(%uH!)+j})EM@y2z$ zTHycz1PE07J^}2bIYR&$G?xjWOkoqhb>R+M`{-hwmhqsBZ%$Ojs-{9Nb;5`(+&9}Qv^F6-ZJ(^4TcBM3DK%ES{9U}=6``x6=aF)Vn zk>gp^t|>R-1z}XYIEgG0QIq|nU-wFFhNqSl7WQB}E)` zz&SDqq9ABW6snPfAqu8>GX$HVk+4Pjf9q zwKV?&rGKC~>7VT5usIqxal{-X(s4uM#*H*gJ$@%NIQ6azHu(hvE}-UQ&UY-625uPk zrlBDNHqU_ViQCT4Sd}*79}`y&V694}iu!^4n3W#W{BQG2nxR_utX9?LTb@;%wlU9= z{>cs!54B?-N}233$8H z_>trc!8Svn`YRLc$^^W7Yy8+=N*sQDdR!+hNnUKf*g={2a>wOP3T zlZ=hBcx16IS*$CWog}a2oYr#AiVtV=F4?>*NfJ193odnnD{)0hE{87V(3QA~BxfU+ zY~)Jp{hNW;Pk-kWch%9z>ln0-X-NjAkxz5dY0iq=NF!h4z%`Cqxl1@uLi1A&Jf-;s z2VT%z!=W0krPALjfK~yDg8&MmITD~qnqvWorFRa=03_3V0DuDulLH4{2pUft#4LCs z(BK+$BaY{`yiKiN3ms3Knd+caM@LyblLFz%6;guRUV}eQ+E{LW4+XP4rQ+;-Htsq;3On z!+8YGBQxR&RiCe&s(BnRKwHAUaE*kW$S)+kfazR@rbJ%FQNuu9hC7dWqcQ+gI0DsT%P@q`vab80RjceRE-{2x6t3T=tsQvhjO-|oC|SE zN&Y0*d=i|M=!t~GBjIR86#@!4jig*9`wqC zgA~3D2VI7;!j;gj61G@oWNKoDDC%-w(%MFsBkK_ zLK?J4Q`TkTYIxmjtcMWGAAH#|=AV%dr+t?*_!ZOlyg2>mOzdPkS+J8;D_LhZi*~au z6i#PBy3%fVcjBD{qokSK|0lrzxAXt+(3tb>Pp)CM|B5{&nQ=~Lx{_H2bZ9p=%l&#o zLnLDrscaWZ4}|3WxY1+&S3McWFfGV08uX-&V=NU;V2l%#_S)4WYp%2&?)TlS)x&y} z+6*oB!?O-#P#|MMd`}}Ma#A9_P;EwFGcqK$Cq9TntFe|u6s(cYLU0zgB2Kj6)Pux+ zZM+xZ$rQ78#cb2YAjzTgyGg&Qe&;pvRu*if6}*iF+h`7BK^V>9EC{DLf&~!@lQI^z zePa!a4DIx6t9gmh^Ok)VfeRPyST;GlXeD-|2m~TfE8@{%y^n@nblbcd4?b7$%oT={ zyl`nr>d+ZOE!KY5m0hK|Rv!b;b-1fgwq+>Wj<^l!5|65}Xhuf0r@Pwx=ZgvJ@jap$ z<7nk5{mONNb6x5$!9atT22g?Y$kKpQ4g!@lS0PYEa}5GD3X>x;zI9_Mj@)|ll=czo z>oeJ7Nyh$)QtF9^e$vuEXi{kwh)w8kl=6JFuxdhSvz|HC$IE+n(WF&+z?-$tCRQRv&~e#PcP*gR+l+ zp2{ki1luM-Z{qk8K6SX>h5g7e1h2!;ybQW2*)#xI2cWJbq9p7WPGP7waTE#Hc6!7j znG}0)2?m+(LT)6rN%)Z2;V}A9Sw%CESte?)sQwAEdV;zTuaR&xSO<$;NqFEDO>~G7 zdy?}mc0bW6TI@|$U7U+W=NPdMF&!R{*fv)5CL0W#bHuiBqBrp_37^<);>B*{#4h2Y zrAva?mt=nl*8^K8ie6-!_o}FyP2PpG;W&i;y>TQRV0UdS3KMtL{aBEa<8>qw$JS%k z<1ssyn6k=|U)WQ3`*s00@aQq`Ky0Rwt9VpJ^J^Zxrn!bkH8lUtqrYkX$fJ)6ukZgR z%+xSv`*&kb|FR-@==3X5xJXpOT9&ZhWM~aMx`bWLD1RtpTbHpTXr58V22%p%tV=mN zToEW|rzKo(8$MO^W(55@mQx;bGlaq z^YQu(wL51#WF?d`7^e)zo%GaDyK@TQ+?7zt;hl1LccOL)*Y2F^1$QM>jzFCw^jE-i$Pxk)0A+5y&Y5 zxhtVnfNTory?2~AEl!+AT$v;ktjdN50Zsb)R{zI}Y%KmaHOT1@q$^c4>($#o9XBpc z*4M~IoN*CnP86k)Z*T@TxTZp`MlR)yN;$RCUcnhv(EN%sdPQ?Br(esNDDrE7!5X?y z90CkNXpRQP(SW*t0x(RV3(q@&(M~|!KMyp^Q;v%o`86)~o&R}(A(q8w!1>zqI}(GKeS4%r6rVC^%#v394EExTaFqpD zSw|8y2epF_UYXu(?|17n$&T2xjSpT(S5KbT^>=^2bRWLs60}5*xV{D#zR@nIM+S7h z{fZ}Bf$;&nSzm0j*p`S%^77`(?G-w~v^l}_P~s(vHP2$5$mkl%73#H|12H2mKk(++ zyps|?b%J@F;6&7li|kM*4Qj|BBlxX$y!_=~An-)9jxi!6vbPwpHW zHvIAFga6sk_trkWmHC)Ij5j$k5XC@=ETa{g z=BEAU$&#%GP9Tks;mA-l@Rn?P(t$;0KU+?iU+HBYJ3qqh@d%=2az;~Ko9~Ei7So# z1%fXSkvPHW8G#!JDUNUxftxhnLg1FdEtX&Y^~v^2={Qb)ZSiYUb@tkjXBD2i4jgmT z$b~#8SX!Fq&fo5JPjk0OD!hA%Gn;rwAa0=G_9=P4j*M?5Fvl z01nceCV(`Wj|$)@%_jtKg62~KI7Ksl!z_#DvjR9vbDjY5Xf6;y0nL{LaEaz30Tj`E zT>#fX|5DNCCx7c@Pg*o0(h-(!Wh-b z>Gk=?@Ud+-W4xQOA$eVr_cQAKjDy0djB%>cermAYwnW$6$GJ&wA@UDyt(+6HW z@!dL0(lpMxjjt1sR7ci8oC6zQAs`L790@@IbS8_~(zIU2U|^fm_(`%q1N<3%WpzEq zz+;RN$%43C$%0T;U*SLw1akT$3pSL^z(+x!M7<83gV$R zzQI67ICIU&w8E@zng2QO|Iu&SPaoDC&cx~~6x<7io+Ke$(A)H;z;D)>@6Hp$WQ%9> zgMQD(3vD`To{l`pDQ|q*6hK%GDEj$W+>r7@dF*^EFW|c->Q^#DP?;T3#mVPJ@})jst`Zuy9mHVU`Z5;(;5JU zG+zSX63zDjxJPq6K=pun{ox}(A8Gys&?lO=@@Okh$#3J)Hk!kD6sGXbu3Ik6uDNsx z&ma$)Uk`A{G=W zPxJ2Vi_6+UqlC8>o>$RGvcgF6O=Nfz zITLn_UE9?)sc$i!Ngyuzn@sH{5+s){LOKX zz9tW+-Ha=7poZkpdOz)nsf<&q(%v;GT=TAX94>m}Mvt@!;j(vN+Cg&?i;`$gW>GTD zdswiCZADUxM&8SUy)>t?D3z8!$fAQZA7;T}T0V^hX$l{!Rl|WJIy}eb-iw;=MYg24 zGt_Hp%o6>HFR*zILY;%qh`4m;bFDs{T44PKH$*bVk&Fegf`cY}zhRW=d+d_stU)>J zptxWqYhKB=SNJ05b&(sVxa2kDc@4?L1@Qx6WOz#{;i1`eCgUr;Kj<{PU}3$6aU{Wm z3rd`IFz2Xr^jWa_EI256HWK!ZgxKSw(J}b$a zQQysI09i_K!3_|FB(8}hEGLkd^(y+Ql%uHL#@ssJ7r=~;!L+&B%v{sLqGc;fD;q~^TN8WL{{T7Dx3vHO literal 0 HcmV?d00001 diff --git a/.cache/clangd/index/acados_solver_auv_model.h.F9AC8187D4C1016F.idx b/.cache/clangd/index/acados_solver_auv_model.h.F9AC8187D4C1016F.idx new file mode 100644 index 0000000000000000000000000000000000000000..b6757e3c58bd722f1275da28396a9ce646c3f43a GIT binary patch literal 5168 zcmYjT2Ut|c6W@hHAM!Y92izebRisFjc07vILlNmsBm!rNpixvL2qM_9Km-(&&QTP> zC?ZDGXvD6l31Ept1&k#se~A<|ivPXi?epHpw;%JHxtZDB*_mIkz{kf#4FJzX!G>hv z+QlpY01`pU+BM5h$pYZ72*8H2=J0_0NA^lR56{Q5)BSdLng$f^+AMRL17&pocs}p$ zvup8l_mYB#j}<>X;ny0*jQZ!MI%HXtz8`Wd8}n+3yimF9t{LPHdql~$$vhs6OfdTX zid?b1Ro`CUEg8*TiVIhqnW|*b3ODD3hHcdeV0GDR4a@6w-}4c+Rn7A{KX9;r#Y*+z zOM1Ioj6(0_R2}}V^v2V@roZF_ax{BS9O5PF)hKLgI-faC_WD^FnAy4P#?$6P7t4ID?2 zA^gQZ$5UNZtMcSm7{;WAWCUbwFjo%Fy|iP;y(ho5-;DO`+D2%xjO6FZA61o~>D%hs z_d1<$6y#K|n`CP3ZFynY)pnHwU1hv+xrEmNS*NNFji}#EkFMizBc5MLIac&~%rs^G z?zRU}Hpk!DHxKl5jy(yVF*F=P49+>Cy#Gt(*F|w!8x$O#JZY{?cDU}Z-T!d-zn!6S z%dgoTY`9q0yDs0>z4FLgryU8g<7?in@@;qSbeY^?Ajm2IiI-F%tE&H`|FX;p&6c+r z_Z7y1pV=)kO?&CFJ$Iqbrk7e3aTlA6KYdOj=ddq3#zUS_q z;We#uKQ63v(mVLN(6+kapA-I{{ysfJMDBF#J6#=C&R(Q((}+}Bo^(CbKz50r?+n4=>;6I0;sKam1#x)w>O7HPH zo^z-DL(aTMSJIX`2RuKgZO}EIWjrr|A81$j>|S>0za>7u z>0H^hcG1-tp9XjO2b~s1-D~@;#b==BeBG%&?MIW_%yKk@_m2m2t7CFIj_vW^Zu6K= zrjDcpm6!~<-;edI(K@=vJT6P%_CdclhHKE>nVDh9vD#11ZqsqUv)w*L3DrB(8hW;d&@w zv9Y?r!)Yh~P%!+W88L0F-owa1d13FeT`776ZC82I6kqvl%*^qo<8su#{RP%q@*RskD|w%u8{v@pu#Iz5*VXbE&ix|kh>2C19`R_nBxd3xexIR zZkih?sdwM5;&@Kup-fv4&CYW8pj!%$NWGRRXysPypl=SrZ=0o)=Ev{hgNy z8@M75i57`Yk_CHsWoN@{+tX@F0yFHP0CEeU2jD6bGLjt9Ob=}F8W5V1U^+IbhWgde zRT5;r9ZGZGr0(Q{E7`Ur(84B}+)Vp#S-cwmMJA?l!39dwk02ys9m zVpt2jU+jHZP67>Vl3|q*2;@l;%(6lTYl1{hypiMz66j!qDyUZlT_ipNY>|oS^d#y< zo?I0hc%n3;)uMGI3r=(0PadZXpj| zrrJURZS0{Q8q`B~iHCVMXeUF*&#Hc{oOFT&hS;PW^2*^vbp|^j6S*t?Yq( zzWfFD$fRXscfru=hj&O|j!p6)HxKdwcj_cr$(cNCs+^XdcD{=Z+|Y)F7D6A%f;T)h zGo<^F#>e|4Fv1?PAvarG>nidSsqz~=jefr@ctHXKY*GXDYoMDXRmpqRXN$g=o0D3F#5At;oViy$bHmiIxhPg*X9pjcWifuKZME`y*>maC;mg^y?mzEnKX!s^?w7J4M=;5g#LsHSKXcgQn zNU9ccYM}vcgF>5Wqb3~MIh(n0aok%FGAv;xmnq>#*08jjR#6!j-Z+9UtwggKMVf~UU(=Q(_I zLa;Gm%w)59AaZMx?F$cdQc=8ebZM#d8_Ms~c*y z(=^WoBG{g=#~mKvNsVCfn8wzoN)>;m^7yd`HY3cK!@g`#|M5&`ni%#Y{P3^>yFT?4 zY!1I1kKmcaOlBW_rJ~D|>W(EK*n+TNoZ#t?&WN7+^eJsCzmfbPWpWa-_aS^3?>Hv} zi+KQT3|{U3Q@-lminR##C48APJ@fn1G~F&@1AYvmGZ9}j%Va)_Bp5*so@t}kL`gY`dx%Jh1ki9qI%>>2I5ZTi#gto^|57~<3{=mbz6xs6$K4ahdH8U@0$(>TvcoSKPX1E`bl#nGf!wiF@mavwX;bBfv zOIb_JFfqWljRH!jBOMMO3xa9FNGtFJKzWF{BCaT07Ra8ahqOJe2c)1$A+5j{g%mUu zq!qYrp$K9Yh$HM&)Xd!`G(C!755j{vfcadvB!?+)k0E#lF@t$;_}GzmnkLfTaXlaf zO%7?rqp;dttAD9zLyl$>vzZs#T(hfqC_b?r!48B2)BbSTA~!UFq}}{@!h^$_XL%0w z;22hn5^j?uN`q4m^>ALe4AAdsc1i!fWrrv`&~Qoziswi;GQA9!9kQpXCbp-ECQ{G@ zlU9VLRQt%@7`=-exezYQ!Qh%eu$Xe<(+xJ5dN@#xeGkFTgfnxx)^bCQ-9Dei@El@} zD(xEJM~{NioRrq^Ym281?hyN_T+sNhK>emoozG{CzknR{}H{l^yr$gLCM#N>wG z8gfhXSlTUDoulx0Yvv1N??$*W+2I;Su$bB6y+yK}8xa5o<%OmZ}KK zc!QHx3LaR7f|IH!f_8#{s3_XeBDEqQs02hPbw=%)_x=r?v6;z!v+w)%zsK9(TM#mP zw$2ivnK2>h39%_F`3NBq`6s6&#b4kd6k&l-dO>4ELr(KY_Al+c8!6{)zw&_hLxuR#>bE`F#!Jdn^E(@yM`830K zcAunZ$>RuhOxsr{~Ea>&IXMp5kD6{sj-1|v>e=T z>lxXzi$Y3bzro@+*oPCS&~wvww7&kzK?;39A_lB5;JKVYWJhrC0cXd{M<_IwMB1>T z4To?75o`W#o$9Wv$Og$t#NWkVYHVN~lLnqzA9|rFrjVG}^;lYu137^T&IM!o;{G-} zL7`D3(t@Qe*q;-KH1X-+(1egO3b~R4h{c0=3MUYGUVm_J`vSS1Le3;|4vWu`LuUmd;or|2yM9Y}1BIMO z{pmfs8Y$#LB3H2M74mjjfym@rNo%w9hT9Z!Cy{b2E+-SQ0+C~L zDW8&#bwf5tK_USz0a9ZF8%9a7tCP13qk`x9KP(XAzXYtU{wDpbU-LvPVe+sb4-4UJ zC5#pew5C3wf-#E-w=_P}iRn%(fvW+?kcn7cU<&|>g(C|H_gut838EZeQV|m?h#l5b z3)I7U8i8h5FPA6C=Ae58#n?u++gi05@W=tbqZL5rZWR3_y#IS){nbcd1V>{bcykx-s2t>I0Re9*ZQ@0~U$Ff(Qn@Fzm>Z$2}J@C0JO39RUm= zk&I_Cgz-b(inH5lw^xxp`v`q(;Bo+#B+oJld4Q1^??;drEPpWK^vKLZ#iFc6GD9{N zWSdSEh$JK4&ml41PanDZ@6dx8_)Hh3yG*A@C|tktotd1pDw%vU5Rc)}g=hArye{UA lL*rl=LaP#D){_qga-eB!#y1yOSXv3KDH}VHtv$~X{R8qrn^FJ( literal 0 HcmV?d00001 diff --git a/.cache/clangd/index/auv_model_impl_dae_fun_jac_x_xdot_u.c.6132B1FDE79246FD.idx b/.cache/clangd/index/auv_model_impl_dae_fun_jac_x_xdot_u.c.6132B1FDE79246FD.idx new file mode 100644 index 0000000000000000000000000000000000000000..4d1476f88c492a1240ca1131d36dfd3086071ca0 GIT binary patch literal 2716 zcmZ{le^iWF7{~8S^WGZX(bi1NWLER8AL$ofXHnSNj7F)5C0S0PiIqZS6}9JB+w?xlCoBoO*tJN5(4Hu@cjkQ`Gk@4Qr{_7(`~BW~-+SNte3~1i z(d-(4kXjp*5+Ac}sR$t?BY#Qj5?8hg5LzWhDCIzNUTDspadgJJOl9&>mtE8BW1ch) zn`y6p(&D{sN6qj>k(Zh-_w{V_t*Q*uuWwZ|-eoTXZ+JKatSox{<6gJTdzrk_NxGC( zF0I8GIg>8ee%qStO(^QnjGO_+=E*G*wsg+HHLY2L^xsV#u(%6)Vq!9 z2cKux=FJ|QJ0?^uZ8{mVq-y`_Yo{lzFr4lf_j*NeYLPa;Tim-yoZfd+)}nNYmg*jL zRezJEFy3p}7F_OkpxQQ7u5UZ?>Z0f35SJaDk$2M)Qfnl3X^FGgp5pnvy4;RHv9~yS z(Ul9g?dDAHQS=tUPf2ItQp!D1lZT{1RcrZ$kv_107bDFDvP!n|Aci~Fg z9XYCP8?{Hi9jNlg4wuT8cWN@?V_H`!{?%lh&RhCi8u`#ite-d{i`GV9)5k`*6cGqkK6Awx4YhYaafVGA%2`lqi8gQSh^`iHxrSI##L`JAI*El#V5y8|*7zQB z^`emnv8ZYF5OYALYfF9;(pM)8yENWQf<_Q|hI|HN4j@L8`T%TpSiXzf<|6MF2k;6cmbbafmcJWm9KT%uvmgziDjmICSwjT!@-ie z-}goN#?k0wBKK3y{nT_m!3_8>daEjHKP303HPx&tYTBxp3T1j~aNbAS!&ANegVJi4igMq`K^KnFOP z1IVKrD_%a>H}xuwh7x%)J$Z;ZfP6i*u=}{Cx|K#_iCl{nwb-9euzvn8>N{TZ+uCU4 zLM%G0(&0cpfu%Sp>dpO~FNtL^u^1_Zk(?-(z>;^i_jOo8)Sonx6U%1oyctjD6IkNQ zAAS*fX!1Waawe9@fsrzEfLC1}x4bDU>(yfk8cF0pc_3pBAScEwPdZ9&IrOW*`G2~U z2waQ8y3m_M;2IMI=tv@P?Fa(&ArU^@eYTEV*YN{n?0F$RFLZz%%GhT@{LH%EV}tkD zSm_NmxWT&45#bz>mEI=8ZI&9kS|sGUnO|1K9>daOSP6U*v3e}kTh|p>T7i|&bzgr$b5Ei~uwcf#U)Q6T+F6%J|&f0wr4{#6_0F1X7R@9424{ z@PX{g1P6F5Vk@z<(sG#HKCU(Czv=49#C~Ew32*`MA#~)h!4H?Q$0&S^vWHK5V(6y) zqq59q^34rGY_PnRk}aihDfRviz`UdbOt68sk+I1*+3NnL3vjx?s@^WZ+pX&Y0WPqv ziv_sYx~>=CdP@xyBWJ@Q43-6U5rG322tX_%e3(;2;2;Gf;3UXFna~7}Wo$boZl@gK z^8lsD*DxUpY$CQ0OA9S`EHNf-sYy#pj7~Bgl7N*;*_#W8HkAm*qwz2bq3C#R9Qn_H Z9HdgAjfldwb_2u%X^FjT&_@DC^dA7nOpX8m literal 0 HcmV?d00001 diff --git a/.cache/clangd/index/auv_model_impl_dae_fun_jac_x_xdot_u_z.c.ED4BE4E791391EA0.idx b/.cache/clangd/index/auv_model_impl_dae_fun_jac_x_xdot_u_z.c.ED4BE4E791391EA0.idx new file mode 100644 index 0000000000000000000000000000000000000000..661e76b4e4254023002383b8857427fc6bcaba19 GIT binary patch literal 2796 zcmZ`(2~bm46nzQ*KNf#P_yH0Ok|+vDars)swRBXFMJbMeP*GHHX~7CuE7lF!CIUJj zI&LVmLP0hK1_RTwDMnG;s-sjuT&hl8u+qvhRr`|vuSsWYW^(V`f6jaN{`cQGi+sGi z9$6qXC(>tgOw`5|6hcT!{_z{*)^&>zx?zsc=A&%~7NmDfU{ZV2tiLNfF5TjC$##p& zET+KwzK8DvhxXR6g_~*`>W}Spcb5jsx42Nw!N)=lL~0iX-41%&o}svwA=`KF$hpcC z)5Mnfap$MsJuP!|XmcoEF}~ByDYcWzDs(&19jp$kyfJF|{w|LJ*QEtNAFtou(fP7l zeyiE_VSY{TN`?05!OOeOU3ucIk(uoqCoUKtdodu?f5!KJ=p6X#{$7v659<4pYVT(cOm$6S&XpB}KaMn=JlC(tt~fkl zxm};EB|c|$*1ndk%sC<1IZH=QxVxZgUDu&1n{l&Vzi3~yETnGaQR{!cG|w6QdiUBr zd-I*L?3|;OukOFDX}z57|Ln)u^16YQef<~NR-X}PA01xQ zyJPm+v-@}74E*Bp9tVo??aF-mX6BmFUHN`@O73@E$!YQ~$lH-m$GC+}R+Sfd4_x@$ zeDGM7k8SG~h}W~%aS7T@?G*Ry?XQin!uw%^($lv#=@)FBB3_K&afdw7zk~ z@AeFGCzK-WT!fbh2`E<-k>{0p14;=RO(;GxAC*2plUeczx$AD8uk?uP%gg77<;pB>O={0Bb0DixJn7de4Ddv^B81D z;6YkGNP7zjb{Kvse~snU&IJiXIpoxT{#d0n7 z5fV_UYh!C2cL%5$G=WeWu)G1!5fV_&tgVWE`g%(NgGLd`JKFvonT1b4$+TNibv{H} z!Jy%UGJ~06rw{NpPIyEAt;54kNstqP=ga1+^a0?TNioXE{ne)>$eF-?GC!3*06Z!( zE92qmK#c^AA+WE^SEUaCkF6|t)N1;tmO%~#PQ>=2l_?OL(C^Dcwrgp!U`={QhGKq)yf(|t|K z>kbAf2&Ip>x6sN(g z1}mUBXqc?$1q|y!xdh;43jpYqY^)P2Jf^r(%$8yWtOMPW^}J|-2x=t&FI50Qqhw>9 zK;bdP)nT^IF!5K7Nt-pZZ%z~|xGFKO5}U&@E4W56ZZy;&YSM^TH0%gcCXIADhQ}0l z2D4`jjX=_3YhKC%Ps$b3xR|ztwjgcN=Hr4U0Iz0f1TrQ7uVDZ{yaeEt3&5wrs_WG; zdk=^dT!k1{7~T=IOjdkc!KT){az6~8fD^1>bSalA!l@#oI$MOZjq5xS&NHr0iSQ}o zx>1B14K?@zCFcD=Xi9M>F?-T*H7KqGvn9rL4Q6W$2M_*1zK_m4;1DRT9JA$yW#A(w zV%|SKbSuMbnSn-e6_~9s)cw<(HObnPdUBSY=AII;I6~Yj`_wC(?+wCLn?*5r8)&0Kka|@NxekA6aKU zz?X6t#Q1{P=(D~O<5$M@ArpMa#0YOT!Og~XCWSL8BfNvc9flgN4-tmW)C_H()8@}< zYj{Dh7&10*Za^fi7QI4O#K*0Q*G(8js68qhFs17 literal 0 HcmV?d00001 diff --git a/.cache/clangd/index/auv_model_impl_dae_fun_jac_x_xdot_z.c.59CA37EE71B4BA5D.idx b/.cache/clangd/index/auv_model_impl_dae_fun_jac_x_xdot_z.c.59CA37EE71B4BA5D.idx new file mode 100644 index 0000000000000000000000000000000000000000..c81bc5f734ed7c400f75ecc2e979fc2111f73279 GIT binary patch literal 2718 zcmZ`(3s_8P82;zXoWI){(M*~dLq-yoID6HEPEs56|;MAI1hANN&5+yf`<%GPE%@BFi!&A@F>S^kLq&b4nDh zHSRBPm}GlJcYb)B_$+t7+(#_)C|Ng%2pplu_kJbs?qO=1!M?<5V{5azU`VNzd5q&^c$!tgCObJO0w8 zUzAgAQh`P9wQOWtg|+UYo3K{VQY(IXury^rTjNT{nVBUaA5g*GeT67ybyQfk5TVuO zI@{}!Hz@}7M~KbD@=WZ@Cvp#rzqc9pBi}YZ!3=UDl-so8Hto$PpzO}Q7Mk6+%8@|| zLYZzeU1|)-aG%KUb)j{1lJ``12FVFrPpj(b06uX9gjD*5MRf(90Srx zhELF;wPBOGPhJllgSrsP6V#6CmGb0Q0nNeb+jL!K2dZ-t1qb= zi^Zrnp-huaQyBy7V@Si3_QMvm6*I_zz?HPTk__PzblBzG;#<+!d`FC22xY8ntjZXm z!{Ii|lbPyee~3{J0{hAQRK@^su)b&MpAA_J43ZJ}KCQS&tRmK1vJm>2M&OB1uBt|_6JWe)FWefoOc)Z@++LZo|L7fR)O)IL&04~9R=5}#7qWpN`^roVFGaa1ptIg08X_4fLsZ{X%zqvDFHq&O$$3tlKcWGTSnnBssrp< z%08v=Q}cSK5bqS4;irW7lzE*f!igd?yh?(0NE3O(>wr!f>bvP zqf*laN;ZeWIi`;ZU!SbtEC5!3_Q^pTgb$CU>(PVMF^#9t9J!wHM+4szF)6QE69~*INh+yaPndg zPkt~qpmk9F^{6RJ%PwBnzh&e|sjp3j6IO>0zLw!|!0G6&oE_Gpb&K5}s`o99YDsv1QX7@pC_K7-%#_(1i^q5VQfixa#&h|V z1^F}Y=iRH2O)ZSmoJ89#{;G{(%_6f`xmmqc7W`U}BV1bcN4ChiDqv;WPv5Qcxu$DU z|2=bfe)##AL#a%G$MTx!D%+l_gM}?`-FDj)UB9@*E6zJ%P)FbRF_{x*9*Bu}`zGek zr(4HPYn|J&^<>-Yr+?!dIbaI)A)Lzpuy@Gb2eBmw zo>ugfSuD{scpOW+)v(4ZHS|g1%F!`<_MDvW!!;G}b6seWOHsKqL$oVG9>1Szk?0qF zSSKlaxv^qXf4ao>a_4*(>Xa}y-+EQ$zTTI?{1k=AwXR8Ye^2;}ib==b$ z7yn|4%WAjQZ9D1e%=qv#2|Ys@&$i$YGky-mCRm`~-97uO_do zJ)wJ6i8Asktml+{KjKnR>4T0+_Yd`meAagOYClo)KpUM!Beptw3hn_ze;W19Z zB3A|=TruF4rY;(z-?I3hQBl>em_DxjtFE6$9Sk&G?GyE((1i*s4)R$ z)6V*cM!WOQV&p_9L9!r~AwZLg$kL^|>o>WwXfS~vVfiEMV@yz?G;Li|q#{_uA~~V- zU{wzeHYT97IS)O$Eim1OMKVHpg%z*JUGNDgl8&S4_SSWiSn}G4dx!1b;mO7Xl$w^X zciv|m!&o$sP@1s336C=-pk!HExr|qL%wUm%P-gX;B{c+in>y8B5RGa^ZXe z%EtK9YgW7r+{Pjmp{%7I|<&}6S~KDlpL1&f9d%2rynl@2r}pxiPm zwv!BUye&o!gyJvrR~Z5{nec7-0zdzrIu_Xy_zkvugMEz&Dx6z=&3cqgas!Lp38kEt zm(!Ds2`KL>v|l?9ywJ=dYeH$q@^*4vd;-dlmdefHYezm5BWFU1mPM-!0nX8B#f=lj zc6W=>Fan3k!c>L;FiMV}n{u8g67(!k{Ev!-6t7-mU(9J~iI!G^1~HdIOL9zWP&Z}C ztJ=6t%4sp9#TGEhpmNfdmpHr@)Fl8fYyd#f6vZp~<9UwIk|VSdrWlk>_Vcoa{Rx-W zq?b>)?M^AV(?aI7Py|O&a^*s%+_VPalM1}*p#sRA0G}2)0eFSO4v;nhcwGYky~!=< z)eK)txn?YC#+I-Tgx$xI7qpQl<>oQ-20#rdS0Z3a1ijYx1kAnO>kSmMf$D{KQcP#> z^$j6&L--K~yC6;VUST@h0%pNSl9VeIFr~elGz*yK-s>!i$?Dyti(J!A~^z5`!lE|-?%n#K-u`Lrb8v<6Ee^>{ymjbKCs__X~HfcGBQ@maX4 z_EgfAY=M$HE@Y0I1_j)ROr+k5fRDK%T2f@1NN^=W(_4~>mPbqSOeY3+BK<$@Nd({x z2`c>Sp)Oy0b=?JWFpWqfhTpBkCUbXzbbU1$^im;HYC5lyJB*pb_~YP%laYhzZ4Am{ zZa*#AZ#oz_8tKp584$t82*8^d0N`E(__Sw{j(WoaKIV#PNwMk1z`F=dZ(Jr?AuTC1 z(TIm7&enq|$+0Q=ITdhE8=40`{wJhHFba)=Z3x9C#U+yeRy05&p@f)b`k3_<^<%{r KQuF=-OY|S9;~VDy literal 0 HcmV?d00001 diff --git a/.cache/clangd/index/auv_model_model.h.60CCFC5FA93A2F4B.idx b/.cache/clangd/index/auv_model_model.h.60CCFC5FA93A2F4B.idx new file mode 100644 index 0000000000000000000000000000000000000000..1bf1d161709426d37f7086f5e4cec6f64a66f075 GIT binary patch literal 2004 zcmYk6drZ?;6vuDnhgy3@C?JKQEfi@*z=CC#w$4IJeT4ClF+ki9wbTcqw$7F@Go8XR z6)+>dnB8=~nQm-294I&e&3Gt`5yq%nROXypR0x)^NMx7m@1$+gKhCG$+~2vs^Eq9y zOt0^lf{=FIGV5l;_FM)bVdt{!^&y60`YAaS2K5wd9 z^t>mzdg{?zr9=AKAa3f*hW&d)<)7B`%YXlE$hsp!lzeI+JSi@FN$FA_i+mq{_`S-m zpSR7n9Q5kd?$6xV@$inZy=?29oPlZU^rzx|&P5G%fBr|6W9UKfyS3Y{pWqJFTVhTg zDG9yo=x!G5@JPAwYj(fm-&KJXr!QJL$5K|!%)VKp8@$%zCL2#$(fH^E4xF!5tTOwD zl<1`Ic>Yr0z4nZ1Z)=6Oq+F}^`ollF8gq&ea!G? zfSvF7`;lE`>kh=}(U#x={?44Y*X}bORdU})XFGk0K0cIqx#8q?h5IEB#x7ZWMIdQG z=EBWu1a1h4LSB6EP5FswZAZZXe!TaeYYlvT80!D!lSi~%v=L~e+Pao6Ng{ET^ z009M~n1lS?ITCup!WgeLHt(Amm?|`p5l1mm;fOnV(yK@GUTHh9d~1IuM$_R0El;a* zHo1JuToo-ndIbGS1y&4iH<0S>oc0Tb7pL!0a= zx-eR$ztV@12L$4nI2Rw@F#fP<>9Ut3@EQcvjG7D!nmpQ~y&I9<-;dEW2NbnP zY=WuLEC!zmrXI5xEGC%R%VKbsVCpQ3!B~Q+sVoLh38sFs80;jNTFGK?l3?m0i@`*K zsevp8{|Khuu^6l)nA*l-aE)N<7>mVO_fB6Nur+d~iG@>zATk2+NSX@Ara?D?sb(w& z$q1%$u^7}Mm@36$5Q<NM1w_GtWYX6E>)@`!&zw}HF6jb7Hia+c$X?olLeK@ke@rzyV)9qx7TJkHJ9|m`PdsbQ z1YttpZ$b?rtZylP{#B($h(b zZAU?N5ISE>OI6?8+zj8SY*T!Q!V+FeO)5?{vUGRoBQoQy;x*?Ol zRr^@Uk7_+qzPDQtim5nyiOs+^4IJ z%nOA`4|c~moLAXiqP(zY`6XZ~ygl?ghTVmMdf~9|@uF#Sz_EAx@CQ~&+JGv0VLRXl zE}`GLVog85a@ofllcyH;%bk9dE=!@xSEaq^V;T&ztZLs5&L;!WeKqgcqkJ^xI4(Vi z0e<^7$z7h7$X=b7$g`r z7&90=7$q1t7&{m=7&;g|7(N&)7%Uhr7%vzo7(*B`7(o~{7&#a?7%dnn7(f_57$O)% Y7)BUJ7)Tf#7#$Q978@B!7a9Q}0O;q%b^rhX literal 0 HcmV?d00001 diff --git a/.cache/clangd/index/main_auv_model.c.FC541DFFAD75A3A0.idx b/.cache/clangd/index/main_auv_model.c.FC541DFFAD75A3A0.idx new file mode 100644 index 0000000000000000000000000000000000000000..01481f6f9239137c7f13b9cba7727d0693207af0 GIT binary patch literal 1648 zcmY+Ddr%Ws6vp?;1r}Hc7$70LfN{WrJPH9KBve6?AhuNjTbKdl8C$5NHds^;{lg$2 zBah(?2@C^41{^H%&{m*U$4+$s5gD|#)e*EB9Z?4atEj!yxkOi)9Un(RB!XEu{_9-*#06`QY?|^!EXHxf+pY53I)}s6t$@6eu%Zg3}ZCi-Mz?6u3SdjohAgDrk{SWx8=aYM)c?SK8k26=t5K zOFf7qJT+{$^I&Pz>UKKVM&r@U`}bNt<38SgR#bm=&_<}VsIBM@6PwPdcZ;J!WBnJKzM9^k$$Ig6Z0eu*ql$!< zj6M9Q*zhw+9?uRxX^Vcnk-6uoyPqsAWkLp;SwRj*+)|n&#ih~{3EGn$d}-$Hko<~* z&|Fdb&ZmTnYh%&h)`>%kf#7~l`LVG?@M?*)p93vQ-=*n;b! z1&h}f{QWGvJ--{h^+<5GFX+mGChmE;Z|!@# z9Myst-6xYtLr=6y*W2AzvsvdKTvMwD=R{YVik~^$5Z%0-*Lw7Q^cxmG`BUG#ksetV zXRBN`0N4Ta!(T`rDJ|&eEJGbc@FT+T zcAYZwJ6+nbDl~i{6fzilF3g0KwaFV$_7;aN0*n*j4(Y?nM;e5M0m2xJmCyqDR{ z2uq!$ff&o|j#Y1%COZ)p+lfOlem77(b0qzXQwaA0>t3)H8@CG|+#H!TJ%fxeGK|9b zk;|WRvtH~KEOC~IFn-ZH#D3pm)`xH>fSq6sHk=E__ug+D>qn0yau)IN*Ob>6?f^gg zco<;`l-S~Ir)AmQLle4DG{9t-%*U9aQ(%f-31ivY{!*MCN3|mPk^15Jo4)9pk}Xf8 zErY3G4z9!tjQPCo!p2(&H-XhnhR($8b$I#<~l@F5~=e0K1KKA0YdT^UneDoIw*x z`JU93XUtKi=X|pgIIrA3Tn;!O2q#0x7(%Wg)DMav^k;Y k1DC^dBGR)n_N>&fW3yM8KvOew($d0;vS!)a*n-u>zc_0G4M zDCgy|uzk{Dp(2q@HoN7Fu4zc!^!;!5_%81f)qwTPtjUYbtMZ@OdiNIh{$_0M;b30o zX?m>Fpuld=TcBqNos-v7sAFs)R0&6P_L8sV*ZO7Hf*{ibD;oOvfX zVVn8BD?)E(Z{%M&X=!f1$+`unLX>vRJ9XXuS%z<&Kl={R;(rUSJvnfR^VN!txK2+qd3Gae!F0`Sm)`h3VteqY+B`JL z^rF?4!|JX@3q!WA{=w0$d-=d7e!+0f+He2oMCIpU&8mx@4<* zrTL|J)7|`GF;`}V3wh}VGZa_mCdmV1<8Sz@>W54D+jtmc7#Jk=SoAo+1W*P7ic-^x z1(_HaJiqgVvaoBaG0I4GGO~0s>cBK!IB~zGYssoGpoEo*l`~9Ozs|&c-m=O!$HBfp)={M)!cKjd+bTV8W%VC%0XY zT5u1j+KR=B4<=kTp>rDRwi!==!qzO-TrlC&c2)a+$xVC%G{jcM))FRsU{<*eY#it6)4=o$lJoG29xMMbVBoUb@MNvgpHhy6-?Ml;LzNyGt2+6 z$w-=Un8Dp5BU!*x01r_a$tsR2B)M9iS|qtfMwUiKxN~JB+ZZ|87~!fJic)hDL20D8 zD7iQo#E=FN5FiR;FffQCm=Xx46oM%RXEI9K6|2nf6gd^dCM7K%hU@LV5MJ!^qsHg~z6s<_rs`a~x-$@RAuWLR3c>P1x;;j6#lYP!UcTeXe!hgbq zz7Z6PI>SG2M&_JUltQ5h$p4%tZ!KFaUfbnTc=tx(=xep@ zsvD-UvEesfyANY8d_KR(?NVdL_@*hYTZ7Jo;)jmmTPqg3_6E7$^q6()@vV6kCs(_> zxnFL)n{n;4-Y*Y@IGcajH@$90&b@|#GvQh69Z$wh{rOk7pE|$f593VmYE<8U;{Iy| zacTMWrkUlL%YJ%N?eS=lO|&T0N7R!Xxa!ADVs^V54ftX2>)OX-i- zD;;}E?S_weFsZz!ga38yx3g=}`kPBUmru#sv;H1!$A)C+7rg(iRu*Rh#6uD{LYUiX1=pt8==$YnAF* zdNr+?QJHXPjs>$c$W-OpJ^q+?)`8;3-D4vnAI`Ns{cVNSxWcWiPngk(i8mLm2`N9j z-?>jVhn0P2TfwJyW!sJ*1Dh|mnbtRSd{ZMh*ih70-t>)QXZM+htgO8~i*_vE$a%LQ zzEpLStPjxE$mGgk+*j5_2t}QWn!ne%NVF=gQ8! z?>D=po=%(LJvOoUlY2JV7QD$XdmfDmt^ZMe*d=Fa#}^3^@u_DWs0AVMDUk`v_Jc!e zo6Vgz_AlU`D1F#@JlWkQcXJJ7ClsLzU=6RWe#>)n%8fs?%XYqPsr?nNtBBJD@j zTZq^BF}Kz~KB#nCeM+>Z+kEzP^UoS*Jzlvl;IIun{ow0!Nw<^yy3-tYHk3PdTzX)}gC^WXGDrcJznDn6v8I(cPCj7e0Bm^nBUv^#&Dh-lfT_lCJRB$O2UQhW`y1Nb&K_gH@T<*cD}B$yQ2Qfv*i z0}#{3RBiT{;5mYWLdEUy0d;?4*veM>8b7;@jciI1`R#Nx-AFrtx&2&QZtpJ`dgX5(=%WozKTJ1B zeZfP*$(5TjOgY*Ce48>mb5hR6p?bfd8fzB_mfUQS*Tu-=7>p7SB0G^N?S_|$;rX5w zB#A`|h{lNAAySKiAqo*q)RWj=EQUxT9s`k5>;aLFI0T|lF`*}ky#&%Ckw_pd5~T#v zBJq(xS|p(oNQ=~73Tcr_q>vV=QtGOoPwFA{hbTZAr6-xa4ALT#$RI5;r3_k&%tr=k zkp;*gEpmG~q(v@~Lt5lYIiyAIBZn-Khsq%>3VQ{lMIli@S`5D+n14sGHwYv@OT$`ddbVfSbHOBR!hzuc zkt4$qA}5BEjzC62nS(MBo3N$ZazVmj6Yg|(E^t~l;Y;`Bf?QSAc4cht{;7PbfJkH} zvT_#{WWV!F%LHNsd7r}mKO#^q0in@~lBoGV?J}hb34RDa0;GXGJlB$CtaNh@83(9H zw^RWkM#Vf}qJWT~5{RUz6e1ZagGi3bAyS|Uhq zNQk1)D2S5KB#4sHWQbDG6di#*BsW5nu|Z})wFHC`(KyrSv<9IeTWcfu5L;`bo-{V< zNn@j)G&br7&Em+1Gok0mPVMd-pS02 z%pfv1GKWaS6AcAVhqQw)w$PJ=FM&wPmqH}t%OH~TZ;pQXYHPJvPhn^n z2N=U9!mY!_dWuG)Az1>N08t_<(G+~LvcUH$(~84N(GiLv)ZEqJ!KJC15v13D^x$0(L``fZcFXL>KMinH3Hwn+RY9 zK=y|)LxuqR*~CQFL`W9TjMvG|_-3Bs8OO+I0{a2%5!^SyVjl_ zM`mdbkHZI^4cay%rnX(vKOqjA!v!BAbAS~#N}mI;Pq8eBj!>~WxJ-SHV9Ve#Rf}Nt zFt4Oxt8`^+VNMM@yYMrQ5AZ71^t7A^QH2*xNO*I-EkOnl5@FmhOW;*PB7vKrBjBD0 zbx`+(45GEWFIswxJ>f$ZBXbjTOJL#ne{KKn!p+M*A#H6#l-yMiv~}aT#x3I0@gFvuhW!u{$47XTmxBw&`yUk zLv=gNCSqB!kSvZFr;~j<@8Rm*t@5-F2m`Q12!Bxg56%WfOXxeVx=oux9bC}BW|QC5 z9G5_2H~4HaY5T5Mqs?YcBU1+7pC1TJXSk&I<4I+CwD%nhr~~MrI`F>D19jk3XrRqv zHNN;SISGi#sDv}v`-TzZvy7Cw`Hu#|KPR&v&w^(SRsoKLe^HrbPEAi~EnoTp`&e^$ zLt*NnYfH%q=%eVi;F0>w9y`rv&4cA+HsF|ZtiZe>dz5jNrJVc^us-ywfzp9F zGFh4C;JgWXBk~!*iF$C_KK*mc)p#GCKv;b>E!XwYl09?*;e+{@ zfpkFn4;Ent5(|0!!6G4w!lEEb!jd3L#*!gQ!BTWoDSW$n@40LH$!e{Zs8wJpArUSK zSAnU7L?V`G1*qdz)1^HpXI7CB$uwsQfn}Ss42qu`udOBvhUQcVEMse1XFxpx9S>BS zzOUwd3_W|yv*}^(tf{phg)!h7?k{fqRXQWf$LZy9GCO7r$*=&~(aHGLedojD8jXYm zW`gOOF(Dzc5ZQv%DI~-~v7MeILJ34tp%fySPzI4)D2GTPROkpSfUNMEyFlPqx3`w& zo?Y$J`T=S1fLr--H;4RIL`ayKnCbdwArZ}u)^(;eKXlKpcplwOI<*Jt0d+w^XppW8 zddV&C%~`##{R4^)xVHRDE-5&cJ^qD#_7xg=zXM%HkoP-Kt|0Gs$O8qiyd_T3<|o~J zg^WfP%Ho52g53M`%rmxQuYH6^<=P1w^A334zlM@$r4bwlQ2GV`^|=@HL}X(Fm-hD; zK;T67roH^F;YGu)4|u5E*KclWJMqHNWMqK9>BaG!76oU zFjC%!U{7KGfrd5e&SbQ4yJ{4@+j|890vm-{gK0=|z#JiR!`vYX#DXBo!m=UC#pXh^ zl8SwyTO6sigo>5ITt~%@!F+;>HNt$EinYVM48fM`j3M?F)r5bS4|SJAN5y3HZ=ucu+fFz=#a6*@ltZoIr#w&k~f{AiN-n_ZjF$i@9+tLS0u z0oC*%;~-r%PP*vz?Y3gp?;l~*0tX`#x8`RU(8s3+`6r7<7Qg-oo6%W$?ybV+wzp)S zvL9}53F@GLa1c5S)05*UM{7Mf4R_LglKgdQdg>%H(Syy>{Q#+Z17s`R8=!^sSNBF- z71`VC0&#_{gJryHdkz(x*}=g|r#)wOdQK!c8ogV0a|L%I=z$zl&7FR}P0#sXZ8e}Y@#mbe7> z#BFg0+#0vTN8@8~8SaYP;7VMD%W)Tc96la*#GP;vF2?O~FI#m69W3a- literal 0 HcmV?d00001 diff --git a/.cache/clangd/index/test_PID.cpp.575590D7897A814B.idx b/.cache/clangd/index/test_PID.cpp.575590D7897A814B.idx new file mode 100644 index 0000000000000000000000000000000000000000..0472c685d287e2225c328b78f9ade7d1b82cd907 GIT binary patch literal 8678 zcmd^@d0Z1m8^@P0B!NV-z=A78L^%VIE71@_00|J0BjAB9`dTcYRph9k1@XcIZ`7hf z1<%&v!BJ5w;DMszQI9HEsftysMbTEg@#xFDiSMuzTmR_i!(Wxphk@@tJF~mbJo9^= znc-ofp*QChgpno$mr%lc`#IVV|Ff6O2Ja%kat1o_`Yo~eIL66-@&?)|v*v_gP=_emnHMAtsf-tzS;#zJRQ67j!jb604Gp0%q3b?F)!Z zj;xLhEs(A})%D};>X);nLc9Al0pWKpWvaHWJe>75qq*`tm=LkP7OMhDC*Ag3Yq=y- zyS0j)SL}^Y3?K@V*oE=3>u$kIe#>*6T!C+YJ78-~VyeOGT^;K~i&|aJ{4()!v1#eE z6(hHoFAnJM=2pM|No@G=+@^7zmMyqLbE<7-)7@&z28FlBqH`|Wb~c)4ycw}JCw-1{ z!T6%J)%E?-`ux}CbkP&mBf;GI@{+{^xYO~t^xA#G`iSPmN{;~}kKpqA3AfjLrLw4L z%5W+jKX$$0nkv`&vx|hU`d>4+yD-*$*NqMJjw7=3Oy#R8Rv)~6Z}96-YuA>C-xSG+ zM9$WyKdUOv78f7%IJWd@*3|}OUg~Z88&A_y{2CfZZINfhRnICOt=N~}y1n=1jgsv2 zlMinA%s$q4e~D)8%zntO9NpSz*=#l1@VT2|g_IqB_s~scM^fzp!SV;+xbOX`zdGNf|CPWIcY43d zw!79^c6{fSlsD54I!e;nRRe#ORG0Og*SLDY)mq27;f`|(14Hs=M~Xp7$B{p(U)3%j z+$-UFTll0i<6P<=@2S0cD4D%x!RuO$#I3HWJvsBu_N`a#KUdlJ$rD;PeDe7X-)(2> z%TC{Heh{!{bQWP=(|mUF?~w~LW>waxeUDb&+Sv8@^SND*!#L8Gq?8v2mvmxgU5kdA z;%7eL=JxfOG~Muo=^fKs>Y{^Hoe>6gl9Q(2tg4EcV=`yre4?oDJ(*>qoswsj75{SK ztZUahTq{h)8#A-yQ@6+ zb(m)s!`Q7)Eh@j<`u7QK^J$n4J7qe1)tyqy!7&RCh%+@u~ zJZZ|L>J;VglqqW{J-D%jv6ZfYgSS+VO>5hinZjXd=)v7Fcebv9y=U`=UK#H9Xeoyk zNt^#N_`%EbmxYE{erd%TG4gL-ZS&2u7DvlZ7kt1iuaXlNoiA9l7H7rMuV!j&YNl)8 ztIfYuD$vBAE8(!h>1T&wVQgIkU(Ie$i|WZO*^Uo9n*c#S$wEf#=CESu-hY{rUzMp| zjsbBMAMn}|MY<)o?am%6mL~dd+)`@^rpJqlYd&BO8`ao*(@ptdD^_^K-@N&fi=zEQ zUDh4rV`J%8O9Dw;T?4;+OI@Dr`JrR4f1tOu-K_S;vY+m7SYdQ;Hk)m#YhZ6|TGGUf z&#W~DQAtWIr2uTSkLOxs{azggH z@`po57zrs~t}jAD3lwDjG`vgh%L%8$}!L8i{JNY zI?9DqjAcwW!6&iI^c<+~PWZv}P1?l+my@|34LTNITH99Jn+0?}hwcPApF`S#`>tQW zOCZe&N(4j#c$uKYK#Y(l@D#v@5|kJ4LP!E62uXnyAvur>;1>~;0w@qt0wqHJz#pL? z5QLBls1OPPAqc5~8lflNh~4eghQh zH$b6&0~G2vp!;m-DWK;!K%ssE6zaFp*}KQ_*DZEn8R!KbOOIfdq7^}_hfpC!%kt(q z8+E#{g%pp^vxeD7NI7sFtY8=lDG#oP6%0Hf<<0d*NXC^}!Qc{7L%2h_38p^WM3?fQ zc=0Kby~qU)&ZoroVuU=!o?h@#e9BAgg^)xd@nLLIZ>fwSxxL&4z6PICI4WEjQaUOT z@^|-_Ft#A?AQ?j{vC4~~5NU{8EkhxWTzm6GP-4?|j+ zh7|E!ZS$5e6(aOhXs*Sx4L+U2HFPr6mTRzpGuNY5d10>k>DbtRFmBX| z-TAlDo{y&IQd|DQDTbx3lZmdRh2g=ccw8P2#txt2bNL8400$1EJC48+At&I3kTY;b zNQ8?ln0~In6|sqNF=F!o9vnuKJaJD8hP-euguH<_a;F5BAT}v3MMwr@97ctFfDb}) zTyDYGe1R`wQ{W23<_G)`n-W(dHhYuPD9f{b>&^^q`#R zKH=BQ6X_5{LmNf}9r0*n>c=T83TeI4)`)O{0lnl5g`dvgXmV!h5zH#|P3Ys^=^$Jd z#y$y8`;0HnGI}S7u+O{4!Au17G7-?rL_jYS!Tdx&6a8|Q9;K273Go+8=J$}d_wQZ1 zY(iD@d;>nkwr4xTl^wNXP+WPT>7pL`GR@}ah3Ch#;csfUGckpnkz8h%6{m<&lYkMdl z*4V{8i;nnX-A7#x9r!VM*!Vb6(Z4X}V5vZ_IjzVrYYvqh*^F@C&g7tN93dqlM7B_S zg_M{OBP1u}wlE(GDFvZGNJ%IWiXmfjc2qRdWri^@Hi!F(PC?1Tj1{|@dZ^4; zT$|?iQj~VZh_5T!;J5Jgib1|!t-;q*LB5_uOmE{PH8{(fKk6W>4qca35!lw;LxBK~ z+}{h%w@oE~vM;b(dngcKZNxUwJF%z3f@3d$)jJ&)9CrcSGSFec87zQBCmj}?FagY! zbXagA1Tc}nLnXRQNA><)4GRaDGT{*GQq`0W`a99gak$IR`=Qr{$NykUKK{axvP}iN z7aw&LPKXHm#C<@)n~%CGot89p!V+54!c-53UmYkC8Niw%&)K!#Sl^}$1Gq(iyE+Um z1o#(&!QXuW#((z(7{T2a;40|802e~Ww1fc`(uYl=|>JAhmav;C@CcS5|&mrebl|J I4fV!Z literal 0 HcmV?d00001 diff --git a/.cache/clangd/index/test_VC.cpp.74BA115DB14CB17C.idx b/.cache/clangd/index/test_VC.cpp.74BA115DB14CB17C.idx new file mode 100644 index 0000000000000000000000000000000000000000..5c01a6f3a96edbe4ed0c17507e5203e90fa63659 GIT binary patch literal 4538 zcmZ8j2V7HE7f&!C;~|esUPwZKAVVOCMji=!?}rQlfz|=ESa5(=MyqwSI8Yo2D&Rl_ zx9p{&fPx~Uj)GPhS`kq}st9hsoA$d8N`Aj2_kZ>|_ndogfTx>VsV0Tu9Oao5AH8}p zjY6R?!8dVrLTn@ecQh!Jq}+-H@hcwVXghJT+%`6(yDvUfnzyUgL~$%j=2b(ltR=Em z{dV=#(hHRg_Svz^y_>Vyr|1f`d!06ZcPRPq)}*HTk9Twi+Cm2yDuxTZ-@p0m#$d*F z$u;GUJBot|z6Z0tju?=I((>@Q!uSF~Jd%F8t zV&+Jr+rwAz`kRCwynkR;$i~axbo?@wx+$Q(tFy{HJ|jPXVV?H+g3)vJFGso^UljHZ z2?y(Jccy&4waqDeEM=4Zg`wL%=$OZ;IYMKjS%R&93~y}bnQbY{wP;^5w8cN->1^J{ zNV5!7_n`Kv0&Azt*PAxa+*4!x>{w+scX8=4(c#v}AMWyR*#`VtDX2>`6)ShOEH&Pv zQ1G;&V{n_WzVpqILi6Uc7CpN?&7*uYee<$HA2`0dQgGGkr}3;uT@J089Ig1DHs{DR z!qDf!^1ju}tv8nxhlJ0N{N5fUD%jDyIi0`IyWo(8_3J-NXO7uZRuz?w^Nikm321Ry45!% znvOSC{9S#xR^jHzomK;JaIxcQ<3p*s-FAD=mtMO5B)sOF@wZ`S%5VE@K6ft99NN0b z-^*9jHs|FoZTCA%XNJhSf4$+5Y!a0I@yzYu;byg8Jv4r7!Cx*2tMn5*$ooXIp!Bq# z<~JT}5G6e-{0{6O_7$5qfc1HCI{rP}W#B$_N`{$IZD`|u4Y7%*dYxY-guZ0qUl*n~v-1=x0}UK2pA2 zDeX3oj_JU9zSl`zxk^HD7f(OG#`W`4FPnY`*YxfnAH_45I`Rw{N0pmj3@#~QQ+EGS z`>^mp4N*}Wm%D!9?VBaF`Mb5&(z#7KtDmVCrSt@+hCI0tA0`A zqCiWXKk{_&ZC}c1#$R0fu46AQC;qnYXp8C3>p~hV96XvfoUm?We|jIO>9BLtmd96{ z-JVN+PGPGQK6Bc;iyydL!nSI9eJa3$yU2SeIy#Oz;wtK~*c;K;o#w26?e@vagm-4c zzCF^YuP5uf8dJ|!w-^6ZVWPjZe43PD=c=t(mY|OBN4(;X- zS)XF;`z}X3>~n4xt90xq#y1qt^0`vGXx8<%nRPjB+0OhZpA!zvvykRQ+xj~CqOKi> zHNuKbZoRF#k9<)S@*apNiOKOx^1x~1*j3WLYpu8tp)90O2z#2n@;^U-^Uvpmn#jjEuip9W1s4YEU$A8L0fI=(}xISrXt)c>tVCpbUp5) zHI|xCd4x<^Mu%j5L&Fa}>fg@-i3ywpUeGo|NF)+lD73IGMs6{ zlKOm%BA%kV?8>I3YP-9$r|}3+)RO^sfk$|w-jk3o>Pw<0#mi3$hXO3+iCE(EpaXz{ z@X$%PK)Nbj8|v~1hAM+Z{SVpd4}+7Wz^{lQ;z1!R{r=FZC0Ff$Noy@@0hw|6 zL#g9$dQS2NGLo4?a%$Dg!|O!8E^;zYrVGgutcJFeYUK{dLMNdMv|Z`$R+8On>J4(@ z(zpyrMhF|44Fl??Y=7m*B}Tm!m;@VTbD$8D;GsMc`M*s$5V4XK41yQv2rQwH)^LAp zQAhSdkhoAqs1C_ILSJ296C(erQNN4}2g2o*oyqZmWKh|ed=e#93GO|sIUf$@$V$)3 z00tMP_t)<-L(U>V)>YFbXY}{m-+IQJSrI*D>PBOi9Z>(?ARc(am*GD^BM1?o6fhmO zrl;*~y~s3b0;rXUCL-s%Brl>#k>iyt*F8tY&y&4|5J5^oq_%1MoTo2mvr<9E96wGd zOom7B{rDt`ewlU8GH>pBxh)Ueb0UuUO7lre{WlevqZPI5K}nt2&bqJ-*w!NkxxkKppSrVs_HxL%5nMt6b;>AG}7NJa>TE`vl2rHW(j zYeII)huj8tpC|$T4IVoQ{X)gJPbxK@PsPuY*uz~ix_+cj?un(!BF}4t(4pzjq2GvI zof}m1a(7Ls#TxIm*- zSJ#&&SzIaoz?5dYTl8Rt&)$_e@=oA#44@^@30yvjT3(cMPY-wHfDD+b%t;ww1QrQZ zUR&}Pf7=w7E4PGj$V&=XLii-gIa!+*%~+i~C5i;@eC#;0=&<}i5X&rJny{Ow-2XfR z91p~MKJs=f1mWQ6RD1_nX@m$=3M5Yo z4dJ9o&}Zm#CnQZ2O%NCYxG@za2#gmYob{ZIVJWr;jg+r^8dnD*a6w#{kQ{z)wR`Ek z`*k2!@hq_gv~2s zLU+|}`;yK%4BS&<5-1lnf@CH^H>TT7Aciqx-UMQrF`Xt5%Zw$RKwKYg5Jc zeIR%WZZ)PBlf3+y1WSuWqV$ig{Q)Lz&*XdWB6f#+590&FGr0}n1p>(O7cRW~8~4Y5 z7rp4<5HNz%a1|KWY>Kpe?tGQuDGl@}BQm$^XyFKu7SIG_Q@b|jOusVf@&=G6zg5h4 zsw(1jedAM-t00CNr?u+8lgCXLb70)}t25W4Z(`m9m;Ut4i7^Yz7<0kqfJ1&BCdI5VbIb&D#2hev Q%$3FF@VH)D+SBy@2c>}iN&o-= literal 0 HcmV?d00001 diff --git a/.cache/clangd/index/test_VC.hpp.C3EBE494A18C2184.idx b/.cache/clangd/index/test_VC.hpp.C3EBE494A18C2184.idx new file mode 100644 index 0000000000000000000000000000000000000000..27b853ec84ddf9e4b5f8341c878f2c48ec8ba3d0 GIT binary patch literal 1946 zcmYjS2~bm46n*(1`AL2jNLYS82_$F|N0C@9W2u0MtlCL&K@nv@q3%VABdeo>3!(^$ zRqHNJDjF<%z0QT5+Uk-J(T`z!brSt)fz`()YjI;-ATVFZbMg?)~?jmrRZd4=?5; z6q+2Bu_R@Ak|RP$fj{QuX$xw22*nISC}Z#Wsb6h+WR%@E2^I^_{&Xy|vZwKiOI+s<8o5YS6f~Qb}wGoGTOEhoU`&xPOd3YzRc^$h8zCrvqJUO zDBk^xqYgmA>L zTBY{k48Z4xxbr$ItJX>gIfm(R^kmKeEVZrC#@X8PB!n8ndYPW#48ULR4oWRO>0Tfq zTrsTCYWz3@aAd^iJ*&DJ3nhdK!v>i_%^84iJpWl)*70DEgz&_$PNq|H2H+#234POk z4JwlnZuG}VkS+ojH-Iw@6XUK_lIPM{hUdAyW1Ey~(n!Wcy z$>A;u;f}j!0vQu$ARncrd}G!+AVl9O8diRK3Pn&P?N0~8z$sGcuMCE>t;jz(gZFLcD{MXZQC6&9%8!CX*a5Sqkhz5yaH>)pjA9~rvtcZ&@V-kj9 zJfI}J8;)k>2GO82>}F*KZcrF@v+@G>6hYYeB`LB(2a41QbW%_fMd}55DNs?QL12)A z2w}xctP+7K-+t8h;?t5Hn29#fqo6x{JO~y3f3au6OUy0y692D=9!tpOU7ou>U^g?084Jm2DT|YwF(G6j zph4!ex#l%&DN%ntyhsMN_MMonCPb>M zebK`GDpVvs$`Z;|qVcszNqtB6IWu#fXXd=W-}`(2`+5Dn7A$DdhamSDuMJ7DsgXDc zg2?Dkk(v_EMuD>-1Z^m+3i`SBg(Ioyc=n~+Z+(-J`4f>9t!4fTUsii+C(NsyjG;fk zRqh*kVQ779?9QDeS@t`izqvF`N!@T*bd^PJn-(V1EtXKVs@|{$Z2wgXfuB))!9Q zzj2EDMo>u13M6^%J@m8e>epwTc}t3wbq5%((f6AI8=h3xxi=}p_ta(@z1&eB1RLts z;U1joOHMAzD(~FoR6~03bkuQAexpoyxv$07+b`R5UDiZVWZ49(xu&;THpCtOHblt{ zs>@6^_3+~A={_x}XN_(PUR`+QNCNRqW|ttq0QFa8!~s63RcH8l=Q8C$jiuPiHa@O@GK9O zG+2ty@chRp!}*(Y_5GJc^FKK)v)OW>C79$CITYS_EaBY8_3=IL+_;8Ox7^p1?upKG z<*%B)yq(T&-lI4F`mtARcj?6lT*iy)*5nZpgBn&sYwi$eB`LpN^_V`sD(?wtyiLSz zS@%%x-1Vd?`ud=-$^KDA?b5%?x(ciCt++WR#V5tX=Ci7MtxXbqJP4sjVhDORjgtkR z`46{@)|Cugh(0C@U00q}R9Kr(m9w+84l?~ql$b3=V;ri?Y^|4fO|tVi)<@(X@a4ciPvYg zxoi)}^AEWnJC=Bf5`Nmv#QnpE&gzkRC^h^swZA5+^~MX1-A=O)-y?7OJby%T>GmVV zZWLVL_hvn!`;=_?JxwooZg-e0G>$l-xY}(u+L<`#rDIRoXZhNUSVEru3Z99-C^6ZWxhWt`7f!2kL1Mxu6>E7TOx@J#V%e~ zxG?JU-G-Z~8AWA-zg5KCeD1hnwDI1J*C)wH|C)Kqm2sg~yTh8Ez8D=|vEM1Z9>37u zhbW3XYTuVWIx!w6P`3T2XLmM8*N-4Le-%f2Xb||#*iL<8sxuBD-drc-}LX-Sb?~e} zBON0aq^G0(kKi1w{yGzG9Qelqi^O6E@<==;q=(ajmIj~}pp@8EieNkh9X__U!a;Tq zH;sU>4cI2CgNV#Prb4D(JAE#XLnJK%!q8(7vH1RMX|qqOj5pRIBTPI~2ctjw`fS|% z-Ij$TBMdx47o*7tN0+0AIUpkfU4aG?>xxy#+pzhJLt2M98WWXE6=4Pl2+Nt}h8egh zKXiOyR+A;_+}Xg{7^7GD_iI17ze9kAYDu?*F*@OH&W3BQtu{I^!ZG5^z-SoZvbdIN zBp?Y?NyCVUBjT&~BxDIyjigpmE`}~pYl1)QYPZ1=5V`@~L?d2ZmJudGqvx}1)JSe1 zSGlsEIS_m`>Mz%+`jQwF%z(|0a|O5ed3tNXh!M>Q#=7qFI5%f&eRu#$%V)|(n91~o zJ*EbYe@CK5ESMJN7%iFiWw`HmX2LWwB7kDvR&C*T0Fi9Z5f?RH^l-V zBWyC8qCUc=u^Ac&W~e|lERlv%OcZOxBAy{u6_1RFEySv#lMx9~qM?+)5;L_2iAbW6pOh%o zP)cE`N}1XaNIRTY(1lKtIo(`E@6KLrg$Wvw|)2N7CB9~(J>W(C>j$L_lM4N!%A-wwfMW_%J zk9k2aD;iZb`6;o9ky9QODbWh2sYS@t->*jtF?oBy?_yJ{5*HX85(EWe8xRzvUIwd| z(Md6h3KVkIEt;;UZ$Ka#8j*}=n2=2=R5}f?ffbMgd$16QfE4h7H4p%2-~i?UA#ee1 zz!A&^?!W`s09!B%*a0!{0<(c9Z~}9HE06&RSODe&9`FV}z!xkAmS84>30tt1n3?Nx Gq5lJWZb&u& literal 0 HcmV?d00001 diff --git a/.cache/clangd/index/utilities.hpp.77C0A5FDF681DAA0.idx b/.cache/clangd/index/utilities.hpp.77C0A5FDF681DAA0.idx new file mode 100644 index 0000000000000000000000000000000000000000..c8067cfb154433005caca6247586ce791ace32f1 GIT binary patch literal 2878 zcmY*b3s_TS82&jMTiXt2jB~b~amLui0UIbn4vSF28!%LSP*W~h5}-tw!9={^Ez3(i z(p^J*EK1BwQ4^6u(bNJFH9!-e(jsVyC5h!FP5aLopTj*5?{@z8|GxA7-~WB@Lt65L z2{8(U#^xlK7UsHUixEN${wsDBt^GsD)(Qmgk{A62cml&bFXxXs;E()Y0J~I$n4E0HYdbc-Hqy) z?d{_%t}wUnktxS3sXK|ggX-IT?^w7y`USQxrZq4L6Sq|v7uJOv^bhvWDhquwJ@Nbg z!b2Ha*Us`09dgEZBm4O-DuXR+J5Uw4XL4WEIeki`%1FHLD0%w}+5N|}N4)GPY1c71 zW%HQdo}K1$#x}KPPn=Cvy%tCKC3HR=-SElGiio_HuXE?Q>eH90k6!)tyD$6yxZgrt zRv%s{+PZghuDkKv@2AGe1 zoL`X@(<)xES|@Gj-Tv=^c@;DEt=L$$Hbr%dTtZ&b9hmWLa!|&l@HhJ>X69$_n6>}J zM}EV`938T)V2txb@4;gQtf(|Ut>@h{jkgoRz8;+z;%IP8UVUriQthO(S*4K|A1EaU zpURwc&k|R+yKRFuqNZZkd%lj9zhte}9Zg#iw%|dtsla-3{lV(Y%RNnH#!0(G`L4GX zB-W?aT}Cf%O^x|aXYV?=XkmQ*%@193*B&0TzRvmA((9$oEA`1K8LuSOh_*CE(!;8v zix(AUmx~Z;?kuSB#$p->6{^uCK5 z>fM@I0scgw%o9Lc1FV5SSJb&n&o~&CB6K)8V`QuX0PUpC?s#&+1*4R}%}c8!Dun>Z zlgfwoJssZtQ&-gdP=A8M@tm9s5CG!y(z-;azkS_K5jIS#jcSVkKu0$O)SN4SEgVOK zxDYyo1A3&TXJ%dY@zE4v#YZC*q)Gsw|5;O>{B*%xOxtfM$L5-;}63k|@H$ z(+b940H8~fE~ynQzs;tIAYPhU9Vh_M*UZvSvg#(x*AS1%5N#HlRRRFrbd9(gx>dHw zLz~1V70mdH z%}4g5B$9jj0ns*zO@(B>1fo>K;jTF zCxZ}@HgESV&GFFSByC|VI;aUr+d^#-4|{~R$JrC$LLj$~XU5^h zFykV*Fx#+<)upl1VGyvorQCAJ0CSIv!OuI$+@n1-vwz(WQ3OfbrFJE}7eCqTQSbz& zfa}XU0@N4GfG;F=0l_qP z#4UsnsH{(*U>R^aC_i52tpb)IhdA(&>(oDTW%@_1O#jH0ftm0p$4=r^$VG6&LDRbP zJBA*)^6>{gxD|$&B0&o{4IhhbW>9QAmjtop2HI`5hJA`-L!v{*!sUSt;fvrs2rdHb zhaU^Hn5d~87Un|Dw3q}f}4=L<5`^b8aP$|#n5Ozq(jl}PEeX4-~qzEC?nv#c!$ zp1}P0`eWwb%2&dT3`^;cz^y6K8#_l literal 0 HcmV?d00001 diff --git a/.cache/clangd/index/velocity_controller.cpp.DFC34CF5F86A4B55.idx b/.cache/clangd/index/velocity_controller.cpp.DFC34CF5F86A4B55.idx new file mode 100644 index 0000000000000000000000000000000000000000..4f24fba6820e43a14936740914ff82fcd4ba0df0 GIT binary patch literal 16350 zcmds;XINCn7x4Ft3ofwq1@6Maf^?(`3W7=zR79mJS3#xN5F3I=Fb+SEI2@ESSV@f`}SLqe5cIJFEZM8S~+Nzr7#yc@pn$&)hO+&N*}D%or6qY*?GW zKrnPl=%U$EGZKXYfk1-)OV5~}auUNI1_HsNZH=*$=ic|xyEOb>d-0yTv$K4+JH7e} zHC}L0i#a~7dy4d@(0#r&*B2)I8ouyZ_sKa00bjY=z<#GAR>gl`kmvv8X?Ur7@SS|| z3?-*ypW;2cecJ#hvIeRmyY=?X{e1d zbDbD}C}>cL_VWtwKVLPsWV$bTa$&fYLXmmw*p&>gC8l{NCUK)qb=$?=+rm%ZRY()QR$!G6+M2^=gi#9%UiXl9}S#Yxif^_CMQEys=e&yf&Iya9epo( zpWkyc;=%BnH|0K8)%-ldBjj`RisBA!5*@Kjda%JtV`z}G=dzTgnoHzQ+*5uisHv~E zU8&h;y=Y;o`O&ZE9H_s3%Re&YYeG&hQ-TG%^>Ia=ko_;H;X+WIj!%HvcMDEcOR-`>wY3m6d^BUEg0zDXz-3@}2FYH+8Yip))&U zrinA7;}RwcoGRi{r={<|S2p1BU_nWFmiMxFy9LEZjL&4mh$@Q4Zftkcq6kUZn<%MAI<1lKm4NU zUhKX&Ys8h*%}$GNG`^4dd5v-X&Yhx#_fvx=?_3&Z^Vg_@KL?uq>Hlr)>(grU@?UB= z?>zWL;+Tq5n~CkOcB;9yjI_Tvb#X+SS(nc=_1D+#75I%2{tIavvvSPbG*QBQq;$V<4EwrA*yv2)LDSkTnoHEoKc{PuL4JvMn8@O!A zptENFqu>0}Z}a2)?+0qD%V$^3==Dv=dBwIf$-ll2TX+4!wEgR1dUYJ`OdlF->k~88V?!V30cj~JXC4aT&*$14{_+$Frg{Zd*(ihK8N&$f&Tfe6Pu z{Kszp33G$hmkjmLT-=}(>6Ri{?Bwvt@uY!4cKLn(Igjm(Q3P%v)s@O(jf6(}0!|

0dk?=GU z{qHY)`^P`?r%p?M?JW?THCuFFW08)USi;LCa$}-COeS(ELuPVEhP>rr42_m2QL^Q1 zO)1$+?ae5;^>MRi$i~f48H#aBA=KQg|I$a|sMbryhwFq}knxi75jqhJ zMT#S3L>4o~!2aR4gVe-g-nO@`HhGemx9@GwP^LzvF1ba_w}ZSLY{_7V`3{hGu-pmq zPL`j7{3*-NK>m#7S0I1I@@tU4X88@s-?01^1&qn#DF6>SA7+ES*KW;EbHL zD1{5dwP_ckETbGLm4JE)=o0%B^W_MYBQ4@ksR`vX$EeTJ6v_Ap;P3#vNzY~cLvVP= zauc2X1vR zfH;hpuLt*f@TS}Vt_{$iuyfqsT9E~5T1d>Zb|%LCK@Xr8dEfoEXA@`V`sjd+zlfw4 zkvs90<6hOi$vdiaLChB-zd|&e__|SO&Zr|j0zE9K0a*=Lk~bC&ed~3jMqeL)Y5|fK zAWITm#C$E1)*?$1&cytWNctnPB%XWt@-^=TdsFm5!p~Eir%uL*GzZiFg}zzh9Y<5- zwWrA-VZq0cM+82$_xtbn{8pkS=G}$vhSU(kG{dCiv0^@6J>G~?s(LCzcR_R)Oo(=h zYC9HP>M!B25*2ZZ1bR~o=(T_m>G|^UGZqeW8N(^faWzQS1AVg#a$yWILKwjqWVBVZ z6Mf_^P~XKGWEnzb)F3U=%|_cS8EuLrd>%5+LncJI5(TCK{9Q zF95w@c?Ci%kT(6?EeLHv+Qgb9{BDGHBW*IK621zdDx^&YU&1#c)QGf+t-RfE;ZXkG zd*+{M_tMU*6>YA*V2us#8i=leG103S4|7yLN+5bPxpwZKG4UI0gxJ{t6@Y5^#C$G7 zxyXPt6!X;xRU-qUP%+6JAC1E ziQlPr4%pV~z_AX;od3<>%7Ell4{nqjz@Y(LC^vz96F5_D2D@hHOSuK?Td4d;*F;A& zvX>){GuuIJJ9Uj|QTKaYJhjTHx0nxD5U_|?BBpEzSPhi3re@8cq__==dyKRUq-7wZ z6F?cblz}_tqo8*b49VA7+uQ$hTYE-dJTO(@R|P|fK#ct{Fb{`SDdazOPa*%QdkX*Kp2GjQr|>`S8DFmV-galWvvSs9@)r8(AXYGNB zk6p2K6@a7w%*ZgEz1DlsPK_6?>JmQDDzOjwS;8mvNwTJt+$Wi#GZhHvPUuB z1)?sHk+?EA?EIq16TlrilcmJcl0*Z1({^yHWG1rkVc}6ktztgHIl_lhq(h_^rQ0C7 z4HEJKW%@9p5XF%AF|%d)qN78udLS7e;u7LcZo@Pgv%KE5_G==Q_=x3zY5e>fj(gp9dMNWxXL1NcEj_u?)#~C zJh8)MBh73&QHuGsNOLXa$Y-)DSMyFU>@tlgun`R-0sO2}mrd_}*#oeyQnXT3Z+tj3 zB#W?V~0B5SPdrO8g-%QBuYwN*~h*${*tr<&S9t<&SZR^2fMD`D5BZ z`D5BZ`D5BZ*la>t+4I$N2V&pILnsd!(Z1CnRD+D@^mrVh}{#a zzR7cu>4*9OF??lSkyeP7khq{VD=9t7r7RHN9mmBvkh|tx+fr_-%>^lg8nfue@rhX= zi}^?3`UnOQRf(%_`Uc`+D88u)T$;d>e4?0d2IpqzN4W)DTBzK9h_88UXJD|7jK2cr zSHOupQHIko`89B+d>!Q1S-y##AMC08HkjXL@4pKcccCwpKLE`KbfKYKR%$+ExeKhj zz@4^#4mQuhgK{@$bpuDa2ef)veg|6bSbh&$?a)@vyoram!InC|+F=C)=#<1mkJI zPTNk8%sZH@>}&}2m;Qj)8s5XP>E)pc$U}R8`fWZs0Qu;U>X+enF4;9URgF+C!WK!F zk!Of`SLCWeDN-1zMT|?#$5_RvObyonK)oEaX$n|@PzBN;OA}5_c6_1H)e$)Ch3JPE z(}#rEhd5IjVK{8GopiLXJN;rCOv?9Xk7f){G4O)3y*O)#L1Cdo!(@LEE$?9 znkl9Vou@uelhBzBBgcz=GMb=)4QD$@_o{3Z2N`uY34Ng3Y_}{*aT;+ZiV`bC69A8c7-|XK)eAxSwy}zgo8=VDRCE)9}!-rDiez z9?*Mg>|gqxCx zgc)qZ_$u5!XwHqfVB3>L-ULb(*=rQk}r49v^u{cDq!wv20Boq<*KC768)w&b|u8MfR(f zBE~D9-7PEb$;9`hSft3wJDe|iK6v6`l!a%=95gBijUzrP!_GWv9p%%hvJdYZRt>|+(6^scle|1qw+!EL4jG?AR>X?d7d#T&yQ{WR z6Ayl0xoQIfuZmVi`<2gxj4BnGFNj~JKECK!PSG3TYtA9 zlx~9PCg_unoL>@Z9G(47E)MZg#!;$bftXL^63vOe#C)1@8Y9aF<7`@32y%J(=XY-+ zR)3~z|M5P1T?VI5UxUMkVuWH8)k!nxHG_lmkdyP|-!Bj08s=^XFhe%tFF>PD+@$;IcyxQ+ET=cv~dhWNn8@M{7vSP8JcfApJ|zHoX$`V80P@P zxnP`2`M1RyjiWZNU#}~})+@RUQu0+YY`vnZEVqHEjpZ94y20`-5Zz+=4v6ls+zz64 zmODVy!Ez^vI$3@SqNglB1JN^O7>#L-{J3-V*B@ zuD>m9i!vn6RWREGb)Z)VQu3hd?@#&7sxm12O!G?OBG|KKNTyoMnEvl11<~G)u>PL^ zjJGN>#VMOZKz}-a1Vjc*VrX%|DkiZHvMmQe z6m|+vhI|#n85*saz)+%M4nvC-?8hhyKvBYcoT3sGH7qxP;tb2Jpt!|w4WJs(C1!#5 ztO3=sd=$`8mg@l3v0M+Rp5+EW4JCgX!e!K#V{ zrar-{G*reHL+@hHB+eqk>wQrvXi+W$Q5of{P%{S^vJExYQQlfvw|%N%#%M zqLN5)6@jaEep86V>qqjKYjg6xh7 z*XAg1aTYt9le}KWyNF#Fa@BNI^*Ex_WXSBS*6*+)mgy~1RnP(pXNTSk3oQGvhFOjC z$Tg}w()-=|@`n*u%dn1O%ww1gBUzrzl;+dT(-~PVnCG&k`38KVhCM{n0$wdtM~z|4 zr83#Ha%K74Aeb%rd58GXNEB%uX;0}Wm>mTZoPWOmvwd$+do z^e=m{Z6w$ysHU8`eSB6AG;rUijh#dor9;$*Q&V9yLovb_h7#2i^=Qi^^(2Op)sq=Y z6Q(iu%@@vRC|#J&P&Np&f#Dnw=CGU#!d#XMKvn>%xw2sC73)%+R|l{oo}Ei&?^B;8s~R`-Y%BQL8qLS++=(i5|<$v$<}1}w41n` zo) zM(IYWHf!-P=teUXqZ`9eqJAP%Jx$V2VklWZnV~e@H0Hkfy7L)I*G*?A8+5aQ;T+J- zVL2Ccb183}VfNzi+weMURMtAy1|&w^yJXbO$4#uqKNlfc5wa&K@0ysd>Uj@lHQ_mN zw$5x-ERgXlKxYN0c<7HgMof$gx}KYm4|xRM)It4AkuMoBF+LsH z?-24PBZkjmAi^~XJ5-&DG?(qXpp+gLRCZo)h#nVIc3x0Vj|+-%fwRv!W!FF1W z4=K`p$_WFF9o<*9ox!m!7y0I*;p7t|^Z9N4+kZQcGduPyH$E-RoaGkdN)*DyDAMmm zC+_``Dg5~Y4(VAMSsg3)f4E5N9N+ccz9vM=@YC>vF$e0)w-dsmq6LO^7j8HQ&Bph{S@HYQxf3#th7C~4M z#E|~AEuOG=qef2~&JM3Y&=m+HYE$mA2VaBHl&`~}>$LiGc}kLZV`kd$0P?|u(kp$O`>@U-E0uucGzE>cCyibDbNcLA*`89C4 z20>Kb2KH^>Mfp0oT&IV*CHxKOa|2wc{3bZu1W(GhKz@sr--f=o>0xgPe+T4u=&rv6 zSKplP!eDxTJGiy8`~bKIU`OR0;MT#)9|HG~m3M+$C)K9}Zx|RqqNf-n{8Mmy%F3UC z+cOwOpWg)@T`-*TbLjURLMgujw^!g#`8Bw`X1N=r-QY;&Js|C2`3<7Tn&l z{0^k=z=6u&gY-S+Wm@MOEX)4-?K3UtuHC)q*4!nXH*h4+G|5ygkuuJXOR-o%TP z0P6r|Ardekz?TGjTm-8HMg1rB4!s(>Gte<_-~E~<>22(+RS;4IV@OC3t4K3ZNUq<( z!DXr5QdMpC+tgltQdj(T7ym5pEms}(Is0T)=hbg=e#f790Xbiw|D2$vv-VZmKhi%w z)1isl^em&z$m|b1OGrJX0VLqy$cMCxkSPtR-y!YqXrldFN0j#FibsFo5Ut1t#kb7x z)Pr6egrYyOJ_ArdZ)(MX zD3GCQK-JKjD9Jys0+L31!Qyq^(9G7SpF}TX>E&d4IfY)1rD3mPi_!r%XxEtoEPWI4d#Y$eK}{&m2=}9xS?D>E|BZb4di?{ i7tWCj;QYBhTrf9`8_td3WSqIg$k@DVopbMfcl*yFAv`3cTR;#~ zv%`z?atif4f*>sLZ$V-HJP$+=o5vAEacx_YcJmE)s(-?Kvzubs!n|tq*Z0{EBRYhi zh*TSn1Yhg?p&@(9DV|wx^V}014vrsJc)mW=zP#jZ-6M2;ZdbVWZzCovFASg9mj7gN z6_E3_r)#<-TU#5+GXDl{wRY^c6|b?cQ3li~|IN^?{CytWK(`?-6?u!kys zU0ZMwZQr4&^VEb4qwwasrc#rK8~9YJ-5>i7*1zk2RGcs+|5cw?bk7(c?MRqg^!;IC z`l`lxYR!#}HpVaA_q+CMn`pmbY0F}s=-kDXOW^B+! z{TbUks{~&jEnEKd)GfZvAH6mk&&>7>SHHWps^)@gcUH$rE7^|H(6BKmi?!gMxI#qvGZ}|0OZvv zbBjV;Vx%-0gXKU`pbZBgFS%^Iv%2{wdm8y-*+t+Y=Ky3$f7eJ%+BOFod0<%)tVrYl zWMTQid%A^>m6nKtm5X^|bC55%>YY%;7$+L>@#qvXMZ^KfX$J*w9rwjL(}=`!s5I1y z1CSqi4=u2NGvr1i3oJ)UBb_+_SsQt|I5XAAoks3h_B8Rd-~i-guQR8z%|2Jt2w^#n zij!~v@-&ZEo)0#xRSS>{tUNs?Jr!i@)FlU=RxHxc$P15FYL#&ufPA*D+p(bcjz5h| zu$(AOWHMo!-+(o0104nhGZ;2UfjRqAY&#CqLB)Zj*vzuIRH8IPLHy!y(x@F z##jyz1uz_dd~;!6!*X>+JdMUfAa`zyNHMu2 zPNmUDSPm8j+j0PM-|4ylZDUa`jZCqeBu!#C0C{uSmX?MuPA?FkNwBeu*o+US7QeP7 zvUOG0A{vSC=qPEF3kNV-v_U7nctVm-BPEt63nwc$0C{WWBjWC_#%pQhgk`m<+JXa+ zo9@-dvoOLJ5cGJiY%WeWUIR_wD9I5`-z6sTPXylD$)l}6C z4nU6D(YT`4`>(w;nu29#fwK(-ggFO(|9 zC_hrVgVn-4NAeDk<0^5!+d+e)2_`kp+{iNVbEz0>8&ovStrc zPA8|e<>d%A@hHZbbmoJ3Qj81f!iQH%F|MR5ADok7+(2&&Nm~Q>RVV58Oy{~Ev#mLmMk6{n^ASbDO;2=Tj>(`>>44RwxBn~gW1;m|P6#v0J@MNG4C1~gm-rr8(+8twtpYc=_?kr zvjsyrh@oKi9@Hn?Eo}HZd-nzD#iK5(AftHHWi8Dt9d%jxS)Yx%tO9*8YzP*C#=nol z_y5xq)c<*5s>agEeeFOeE)B&*#bhv2iqS|lj&KGLQh0Z4c*4YSQ+BRgIaGfYzXlyo zCxir|m;hdY5S)c#0(pT#aQT)W?w1@GPU^!upUj(V4)qErfIq;-FMQyqRJSW(?lt|m zcQ7y591JC)Q(LRcy+43yS%54YUg7VHkKH@jZgm^eazFW0cz6j_gM|ZP>-(4%xrzK? z?&1A`=W1+f-m;#cR{D?f1dS>P9u|?G^Qk@;ADp0FZFl@=in$RS_3j4)ug0&JEj5Ch z*mFvK)p=m`QX@G2S##o@&-Lc-G=h7)+arECb5{BpBe+Uf83Ai5$p56^1ba<(|GO*v z$I8&OG&JRgCgWKDU3x|7Gc7&A-cBL2wz0C5i=`4 Q_,std::vector R_,std::vector inertia_matrix, double max_force,std::vector water_r_low,std::vector water_r_high); - void reset_controller(); + void reset_controller(int nr=0); bool calculate_thrust(State states, Guidance_data guidance_values); int set_interval(double interval); Eigen::Vector get_thrust(); diff --git a/control/velocity_controller/include/velocity_controller/utilities.hpp b/control/velocity_controller/include/velocity_controller/utilities.hpp index 796701f1c..543174581 100644 --- a/control/velocity_controller/include/velocity_controller/utilities.hpp +++ b/control/velocity_controller/include/velocity_controller/utilities.hpp @@ -1,4 +1,5 @@ #pragma once +#include #include #include #include "std_msgs/msg/float64_multi_array.hpp" @@ -7,8 +8,7 @@ #include -class angle{ - public: +struct angle{ double phit=0.0; double thetat=0.0; double psit=0.0; @@ -20,12 +20,15 @@ class State{ public: double surge=0.0, sway=0.0, heave=0.0, roll_rate=0.0, pitch_rate=0.0, yaw_rate=0.0; //roll_rate=0.0, pitch_rate=0.0, yaw_rate=0.0; double roll=0.0, pitch=0.0, yaw=0.0; //phi, theta, psi + double w=0.0, x=0.0,y=0.0,z=0.0; //double integral_surge=0.0; double integral_pitch=0.0; double integral_yaw=0.0; State(double surge=0,double pitch=0, double yaw=0):surge{surge}, pitch{pitch},yaw{yaw}{}; + //State(){}; State operator=(int n){if (n){surge=0.0,sway=0.0,heave=0.0,roll_rate=0.0,pitch_rate=0.0,yaw_rate=0.0,roll=0.0,pitch=0.0,yaw=0.0;} return *this;}; + State operator=(nav_msgs::msg::Odometry::SharedPtr rhs); }; - +//TODO: fix these so the initializing is correct, and that changing the quaternions changes the angles, so that the state is always consistent class Guidance_data:public State{ public: //double surge; double pitch; double yaw; @@ -40,5 +43,4 @@ class Guidance_data:public State{ angle NED_to_BODY(const angle &a,const State &s); Eigen::Vector3d NED_to_BODY(const Eigen::Vector3d &a, const State &s); -//casadi::MX mtimes(const casadi::MX& A, const casadi::MX& B); diff --git a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp index 06de8b774..09e6474ef 100644 --- a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp +++ b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp @@ -28,16 +28,13 @@ class Velocity_node : public rclcpp_lifecycle::LifecycleNode{ //Callback functions void guidance_callback(const vortex_msgs::msg::LOSGuidance::SharedPtr msg_ptr); - //void killswitch_callback(const std_msgs::msg::Bool::SharedPtr msg_ptr); void odometry_callback(const nav_msgs::msg::Odometry::SharedPtr msg_ptr); //Publisher instance rclcpp::Publisher::SharedPtr publisher_thrust; //Timer instance - rclcpp::TimerBase::SharedPtr timer_calculation; - rclcpp::TimerBase::SharedPtr timer_publish; - + rclcpp::TimerBase::SharedPtr timer_calculation; //Subscriber instance rclcpp::Subscription::SharedPtr subscriber_Odometry; rclcpp::Subscription::SharedPtr subscriber_guidance; @@ -57,11 +54,11 @@ class Velocity_node : public rclcpp_lifecycle::LifecycleNode{ //Stored wrenches values vortex_msgs::msg::LOSGuidance reference_in; Guidance_data guidance_values; - Guidance_data current_state; + State current_state; geometry_msgs::msg::WrenchStamped thrust_out; - int controller_type; //1 PID, 2 LQR + int controller_type; //1 PID, 2 LQR, 3 NMPC, 4 NMPC acados //PID controllers PID_controller PID_surge; @@ -89,6 +86,9 @@ class Velocity_node : public rclcpp_lifecycle::LifecycleNode{ std::vector R3; std::atomic_bool should_exit_{false}; + //VC settings + bool anti_swing=1; + bool anti_overshoot=0; //States rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure(const rclcpp_lifecycle::State &) override; @@ -96,8 +96,9 @@ class Velocity_node : public rclcpp_lifecycle::LifecycleNode{ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) override; rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; -}; - + //TODO: reset function that resets all controllers, easier to do, and be able to pass an argument so that u can reset either Surge, Pitch and Yaw + void reset_controllers(int nr=0); +}; diff --git a/control/velocity_controller/launch/VCnTest.launch.py b/control/velocity_controller/launch/VCnTest.launch.py index 6e2defd8f..eb56112cf 100644 --- a/control/velocity_controller/launch/VCnTest.launch.py +++ b/control/velocity_controller/launch/VCnTest.launch.py @@ -9,8 +9,12 @@ from launch.substitutions import LaunchConfiguration def generate_launch_description(): - pkg_share = get_package_share_directory('velocity_controller') - config_path = os.path.join(pkg_share, 'config', 'parameters.yaml') + global_share = get_package_share_directory('auv_setup') + config_path_global = os.path.join(global_share,'config','robots','orca.yaml') + common_launch_args = { + "drone": "orca", + "namespace": "orca", + }.items() stonefish_dir = get_package_share_directory('stonefish_sim') @@ -30,36 +34,31 @@ def generate_launch_description(): ) ] ) + operation_mode_manager_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("operation_mode_manager"), + "launch", + "operation_mode_manager.launch.py", + ) + ), + launch_arguments=common_launch_args, + ) - - #node_name_arg = DeclareLaunchArgument( - # 'node_name', default_value='velocity_controller_node', - # description='Name of the velocity controller node' - #) - - node_name_arg2 = DeclareLaunchArgument( + node_name_arg = DeclareLaunchArgument( 'node_name_1', default_value='test_VC_node', description='Name of the test VC node' ) - - #velocity_controller_name = LaunchConfiguration('node_name') test_VC_name = LaunchConfiguration('node_name_1') return LaunchDescription([ stonefish_sim, orca_sim, - #node_name_arg, - node_name_arg2, - #Node(package='velocity_controller', - # executable='velocity_controller_node', - # name=velocity_controller_name, - # output='screen', - # parameters=[config_path] - #arguments=['--ros-args','--log-level','debug'] - # ), + node_name_arg, + operation_mode_manager_launch, Node(package='velocity_controller', executable='test_VC_node', name=test_VC_name, output='screen', - parameters=[config_path]) + parameters=[config_path_global]) ]) \ No newline at end of file diff --git a/control/velocity_controller/src/LQR_setup.cpp b/control/velocity_controller/src/LQR_setup.cpp index f4f1153c8..81de3af27 100644 --- a/control/velocity_controller/src/LQR_setup.cpp +++ b/control/velocity_controller/src/LQR_setup.cpp @@ -210,14 +210,22 @@ bool LQRController::calculate_thrust(State state, Guidance_data guidance_values) u=saturate_input( (K_l*state_error)); return true; } -void LQRController::reset_controller(){ - integral_error_surge=0.0; - integral_error_pitch=0.0; - integral_error_yaw=0.0; +void LQRController::reset_controller(int nr){ + if(nr==0||nr==1){ + integral_error_surge=0.0; + surge_windup=false; + } + if(nr==0||nr==2){ + integral_error_pitch=0.0; + pitch_windup=false; + } + if(nr==0||nr==3){ + integral_error_yaw=0.0; + yaw_windup=false; + + + } - surge_windup=false; - pitch_windup=false; - yaw_windup=false; return; } int LQRController::set_interval(double interval){ diff --git a/control/velocity_controller/src/test_VC.cpp b/control/velocity_controller/src/test_VC.cpp index daf614f64..06e33c79a 100644 --- a/control/velocity_controller/src/test_VC.cpp +++ b/control/velocity_controller/src/test_VC.cpp @@ -16,10 +16,10 @@ test_VC::test_VC() : Node("test_VC_node") { - this->declare_parameter("topics.guidance_topic"); - this->declare_parameter("topics.odom_topic"); - this->topic_guidance=this->get_parameter("topics.guidance_topic").as_string(); - this->topic_odometry=this->get_parameter("topics.odom_topic").as_string(); + this->declare_parameter("topics.guidance.los"); + this->declare_parameter("topics.odom"); + this->topic_guidance=this->get_parameter("topics.guidance.los").as_string(); + this->topic_odometry=this->get_parameter("topics.odom").as_string(); rclcpp::QoS pub_QoS(10); pub_QoS.keep_last(10).reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT).durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); diff --git a/control/velocity_controller/src/utilities.cpp b/control/velocity_controller/src/utilities.cpp index b51cf687e..dae729e68 100644 --- a/control/velocity_controller/src/utilities.cpp +++ b/control/velocity_controller/src/utilities.cpp @@ -2,6 +2,8 @@ #include "Eigen/Dense" #include #include +#include +#include angle quaternion_to_euler_angle(double w, double x, double y, double z){ double ysqr = y * y; @@ -61,19 +63,27 @@ Eigen::Vector3d NED_to_BODY(const Eigen::Vector3d &a, const State &s){ return v_body; } -/* -casadi::MX mtimes(const casadi::MX& A, const casadi::MX& B){ - if (A.size2()!=B.size1()){ - throw std::invalid_argument("Wrong dimensions size. A has %f columns and B has %f rows"); - } - casadi::MX result=casadi::MX::zeros(A.size1(),B.size2()); - for (int i=0;ipose.pose.orientation.w; + x=rhs->pose.pose.orientation.x; + y=rhs->pose.pose.orientation.y; + z=rhs->pose.pose.orientation.z; + + auto [r,p,y_]=quaternion_to_euler_angle(w, x, y, z); + roll=r; + pitch=p; + yaw=y_; + + //angular velocity + roll_rate=rhs->twist.twist.angular.x; + pitch_rate=rhs->twist.twist.angular.y; + yaw_rate=rhs->twist.twist.angular.z; + //velocity + surge = rhs->twist.twist.linear.x; + sway = rhs->twist.twist.linear.y; + heave = rhs->twist.twist.linear.z; + + return (*this); + } -*/ + diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index e5ee3e75c..2e7384360 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -4,13 +4,13 @@ #include #include "geometry_msgs/msg/wrench_stamped.hpp" #include "std_msgs/msg/float64_multi_array.hpp" -//#include -//#include #include "std_msgs/msg/bool.hpp" +#include "velocity_controller/NMPC_setup.hpp" #include "velocity_controller/PID_setup.hpp" #include #include #include +#include #include #include #include @@ -21,10 +21,11 @@ //Konstruktør -Velocity_node::Velocity_node() : rclcpp_lifecycle::LifecycleNode("velocity_controller_lifecycle"), PID_surge(300,10,5), PID_yaw(60,8,5), PID_pitch(10,1,3), lqr_controller() +Velocity_node::Velocity_node() : rclcpp_lifecycle::LifecycleNode("velocity_controller_lifecycle"), PID_surge(300,10,5), PID_yaw(60,8,12), PID_pitch(10,1,5), lqr_controller() { RCLCPP_INFO(this->get_logger(), "Velocity control node has been started."); get_new_parameters(); + //TODO: dont need to save the Q3 R3 and the other matries, just use #define //NMPC controller NMPC.set_matrices(Q3,R3, inertia_matrix, max_force, dampening_matrix_low, dampening_matrix_high); NMPC.set_interval(publish_rate/1000.0); @@ -62,11 +63,13 @@ void Velocity_node::calc_thrust() angle NED_error={guidance_values.roll-current_state.roll,guidance_values.pitch-current_state.pitch,guidance_values.yaw-current_state.yaw}; angle error=NED_to_BODY(NED_error,current_state); Guidance_data mod_g_values=guidance_values; - if (abs(error.psit)<3.14/2 || abs(error.thetat)<3.14/2){ //Need to fix to pi - mod_g_values.surge=guidance_values.surge*cos(error.psit)*cos(error.thetat); - } - else{ - mod_g_values.surge=current_state.surge; //Only focus on rotating? Or is 0 maybe TODO: Decide. Potentially set the u.surge to 0. Then remember to fix the integral anti wind up + if(anti_overshoot){ + if (abs(error.psit)<3.14/2 || abs(error.thetat)<3.14/2){ //Need to fix to pi + mod_g_values.surge=guidance_values.surge*cos(error.psit)*cos(error.thetat); + } + else{ + mod_g_values.surge=current_state.surge; //Only focus on rotating? Or is 0 maybe //TODO: Decide. Potentially set the u.surge to 0. Then remember to fix the integral anti wind up + } } switch (controller_type) { @@ -97,7 +100,7 @@ void Velocity_node::calc_thrust() break; } case 3:{ - RCLCPP_INFO(this->get_logger(),"Guidance: %f, %f, %f",guidance_values.surge,guidance_values.pitch,guidance_values.yaw); + //RCLCPP_INFO(this->get_logger(),"Guidance: %f, %f, %f",guidance_values.surge,guidance_values.pitch,guidance_values.yaw); Eigen::Matrix u; if (NMPC.calculate_thrust(guidance_values, current_state)){ controller_type=1; @@ -146,28 +149,31 @@ void Velocity_node::calc_thrust() //Callback functions +//TODO: odometry dropout void Velocity_node::guidance_callback(const vortex_msgs::msg::LOSGuidance::SharedPtr msg_ptr){ + Guidance_data old_guidance=guidance_values; guidance_values = *msg_ptr; - RCLCPP_INFO(this->get_logger(), "Guidance received: surge=%.3f pitch=%.3f yaw=%.3f",guidance_values.surge, guidance_values.pitch, guidance_values.yaw); + if(anti_swing){ + if(abs(old_guidance.surge-guidance_values.surge)>=0.5){ + reset_controllers(1); + } + if (abs(old_guidance.pitch-guidance_values.pitch)>std::numbers::pi/4) { + reset_controllers(2); + + } + if (abs(old_guidance.yaw-guidance_values.yaw)get_logger(), "Guidance received: surge=%.3f pitch=%.3f yaw=%.3f",guidance_values.surge, guidance_values.pitch, guidance_values.yaw); //RCLCPP_INFO(this->get_logger(),"message: s: %f, p:%f, y:%f", msg_ptr->surge,msg_ptr->pitch,msg_ptr->yaw); return; } - +//TODO: update to also update the quaternions void Velocity_node::odometry_callback(const nav_msgs::msg::Odometry::SharedPtr msg_ptr){ - RCLCPP_INFO(this->get_logger(),"Recieved odometry"); - angle temp=quaternion_to_euler_angle(msg_ptr->pose.pose.orientation.w, msg_ptr->pose.pose.orientation.x, msg_ptr->pose.pose.orientation.y, msg_ptr->pose.pose.orientation.z); - //angles - current_state.roll = temp.phit; - current_state.pitch = temp.thetat; - current_state.yaw = temp.psit; - //angular velocity - current_state.roll_rate=msg_ptr->twist.twist.angular.x; - current_state.pitch_rate=msg_ptr->twist.twist.angular.y; - current_state.yaw_rate=msg_ptr->twist.twist.angular.z; - //velocity - current_state.surge = msg_ptr->twist.twist.linear.x; - current_state.sway = msg_ptr->twist.twist.linear.y; - current_state.heave = msg_ptr->twist.twist.linear.z; + //RCLCPP_INFO(this->get_logger(),"Recieved odometry"); + current_state=msg_ptr; //overloaded to fix all the internal states return; } @@ -191,9 +197,9 @@ void Velocity_node::get_new_parameters(){ this->topic_guidance = this->get_parameter("topics.guidance.los").as_string(); this->declare_parameter("topics.odom"); this->topic_odometry = this->get_parameter("topics.odom").as_string(); - this->declare_parameter("topics.killswitch_topic"); - this->topic_killswitch = this->get_parameter("topics.killswitch").as_string(); + //variables + this->declare_parameter("max_force"); this->max_force = this->get_parameter("max_force").as_double(); this->declare_parameter("publish_rate"); @@ -203,19 +209,21 @@ void Velocity_node::get_new_parameters(){ //LQR Parameters - + this->declare_parameter>("LQR_params.Q"); Q=this->get_parameter("LQR_params.Q").as_double_array(); this->declare_parameter>("LQR_params.R"); R=this->get_parameter("LQR_params.R").as_double_array(); this->declare_parameter>("physical.mass_matrix"); this->get_parameter("physical.mass_matrix", inertia_matrix); + RCLCPP_INFO(get_logger(),"1"); //D this->declare_parameter>("dampening_matrix_low"); this->declare_parameter>("dampening_matrix_high"); this->dampening_matrix_low=this->get_parameter("dampening_matrix_low").as_double_array(); this->dampening_matrix_high=this->get_parameter("dampening_matrix_high").as_double_array(); + RCLCPP_INFO(get_logger(),"1"); //NMPC acados Parameters this->declare_parameter>("NMPCA_params.Q"); @@ -223,10 +231,12 @@ void Velocity_node::get_new_parameters(){ Q2=this->get_parameter("NMPCA_params.Q").as_double_array(); R2=this->get_parameter("NMPCA_params.R").as_double_array(); //NMPC + this->declare_parameter>("NMPC_params.Q"); this->declare_parameter>("NMPC_params.R"); Q3=this->get_parameter("NMPC_params.Q").as_double_array(); R3=this->get_parameter("NMPC_params.R").as_double_array(); + } @@ -253,11 +263,11 @@ Velocity_node::on_activate(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Activating..."); timer_calculation = this->create_wall_timer(std::chrono::milliseconds(publish_rate), std::bind(&Velocity_node::calc_thrust, this)); - //LifecycleNode::on_activate(state); + auto ret = LifecycleNode::on_activate(state); //timer_calculation->reset(); - return CallbackReturn::SUCCESS; + return ret; } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn @@ -265,6 +275,8 @@ Velocity_node::on_deactivate(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Deactivating..."); auto ret = LifecycleNode::on_deactivate(state); + reset_controllers(); + //TODO: reset NMPCs //timer_calculation->cancel(); return ret; } @@ -293,6 +305,33 @@ Velocity_node::on_shutdown(const rclcpp_lifecycle::State & state) return CallbackReturn::SUCCESS; } +void Velocity_node::reset_controllers(int nr){ + switch (nr) { + case 0: + PID_pitch.reset_controller(); + PID_surge.reset_controller(); + PID_yaw.reset_controller(); + lqr_controller.reset_controller(); + break; + case 1: + PID_surge.reset_controller(); + lqr_controller.reset_controller(1); + + break; + + case 2: + PID_pitch.reset_controller(); + lqr_controller.reset_controller(2); + + break; + + case 3: + PID_yaw.reset_controller(); + lqr_controller.reset_controller(3); + break; + + } +} int main(int argc, char * argv[]) { From c4e0770d3f8006b844fffc2a733730a43acff8d1 Mon Sep 17 00:00:00 2001 From: Henrik Date: Mon, 23 Mar 2026 14:14:34 +0100 Subject: [PATCH 091/128] changed the PID class, better interface and more checks --- .../config/parameters.yaml | 2 +- .../include/velocity_controller/PID_setup.hpp | 16 +++--- .../launch/VCnTest.launch.py | 3 +- .../launch/velocity_controller.launch.py | 1 + control/velocity_controller/src/PID_setup.cpp | 42 +++++++++------ .../src/velocity_controller.cpp | 13 +++-- .../velocity_controller/tests/test_PID.cpp | 54 ++++++++++--------- 7 files changed, 76 insertions(+), 55 deletions(-) diff --git a/control/velocity_controller/config/parameters.yaml b/control/velocity_controller/config/parameters.yaml index 1c0e5ae3a..19aedf287 100644 --- a/control/velocity_controller/config/parameters.yaml +++ b/control/velocity_controller/config/parameters.yaml @@ -29,7 +29,7 @@ publish_rate: 100 #ms #Clamp parameter max_force: 99.5 #should maybe be 99.5 - controller_type: 2 #1 PID 2 LQR 3 NMPC 4NMPC fast + controller_type: 1 #1 PID 2 LQR 3 NMPC 4NMPC fast #Q: [300.0,0.01,0.01,0.01,32.84,32.84,32.84,32.84,32.84] # u,v,w,p,q,r,phi,theta,psi # R: [0.02,3.1,3.10] # u_surge, u_theta, u_psi diff --git a/control/velocity_controller/include/velocity_controller/PID_setup.hpp b/control/velocity_controller/include/velocity_controller/PID_setup.hpp index a690fce04..0f0f9d86e 100644 --- a/control/velocity_controller/include/velocity_controller/PID_setup.hpp +++ b/control/velocity_controller/include/velocity_controller/PID_setup.hpp @@ -4,25 +4,23 @@ #include #include #include -#include "utilities.hpp" class PID_controller { public: - PID_controller( double k_p=0, double k_i=0, double k_d=0, double max_output=100, double min_output=-100); + //PID_controller(double Kp=0, double Ki=0, double Kd=0, double max_output=0, double min_output=0, double dt=0); //PID_controller(double k_p, double k_i, double k_d) : PID_controller(k_p, k_i, k_d, 100.0, -100.0) {}; - double calculate_thrust(double error, double dt); + bool calculate_thrust(double error); void reset_controller(); double get_output(); bool set_output_limits(double min_output, double max_output); - void set_parameters(double k_p,double k_i, double k_d); + bool set_parameters(double k_p,double k_i, double k_d, double dt); + bool set_dt(double dt); private: - double k_p; - double k_i; - double k_d; + double Kp_, Ki_, Kd_, dt_; double integral=0; double previous_error=0; double output=0; - double max_output_; - double min_output_; + double max_output_, min_output_; + bool init=false; }; diff --git a/control/velocity_controller/launch/VCnTest.launch.py b/control/velocity_controller/launch/VCnTest.launch.py index eb56112cf..d20d1e9ad 100644 --- a/control/velocity_controller/launch/VCnTest.launch.py +++ b/control/velocity_controller/launch/VCnTest.launch.py @@ -22,7 +22,7 @@ def generate_launch_description(): PythonLaunchDescriptionSource( os.path.join(stonefish_dir, 'launch', 'simulation.launch.py') ), - launch_arguments={'rendering_quality': 'low','rendering':'false'}.items(), + launch_arguments={'rendering_quality': 'low','rendering':'true'}.items(), ) orca_sim = TimerAction( period=12.0, @@ -59,6 +59,7 @@ def generate_launch_description(): Node(package='velocity_controller', executable='test_VC_node', name=test_VC_name, + namespace='orca', output='screen', parameters=[config_path_global]) ]) \ No newline at end of file diff --git a/control/velocity_controller/launch/velocity_controller.launch.py b/control/velocity_controller/launch/velocity_controller.launch.py index f536fa356..4318ca4af 100644 --- a/control/velocity_controller/launch/velocity_controller.launch.py +++ b/control/velocity_controller/launch/velocity_controller.launch.py @@ -26,6 +26,7 @@ def generate_launch_description(): Node(package='velocity_controller', executable='velocity_controller_node', name=velocity_controller_name, + namespace='orca', output='screen', parameters=[config_path_local,config_path_global]) ]) \ No newline at end of file diff --git a/control/velocity_controller/src/PID_setup.cpp b/control/velocity_controller/src/PID_setup.cpp index 5d795f294..0c0b444f0 100644 --- a/control/velocity_controller/src/PID_setup.cpp +++ b/control/velocity_controller/src/PID_setup.cpp @@ -1,17 +1,16 @@ #include "velocity_controller/PID_setup.hpp" -#include "velocity_controller/LQR_setup.hpp" -#include "velocity_controller/utilities.hpp" -PID_controller::PID_controller( double k_p, double k_i, double k_d, double max_output, double min_output):k_p(k_p), k_i(k_i), k_d(k_d), max_output_(max_output), min_output_(min_output) { +/* +PID_controller::PID_controller( double Kp, double Ki, double Kd, double max_output, double min_output, double dt):Kp_(Kp), Ki_(Ki), Kd_(Kd), max_output_(max_output), min_output_(min_output), dt_(dt) { integral = 0.0; previous_error = 0.0; -}; -double PID_controller::calculate_thrust(double error, double dt){ - if (dt<=0){ - return 0; - } +};*/ +//TODO: kanskje forbedre integrasjon og derivasjons beregningene +//TODO: check for more errors, f.example Nan or very high intergral +bool PID_controller::calculate_thrust(double error){ + if(!init)return false; //P + I + D - output=k_p*error+k_i*integral + k_d * (error - previous_error) / dt; + output=Kp_*error+Ki_*integral + Kd_ * (error - previous_error) / dt_; //Saturation if (output>max_output_){ @@ -21,11 +20,11 @@ double PID_controller::calculate_thrust(double error, double dt){ output = min_output_; } else{ - integral+=error*dt; //anti-wind up + integral+=error*dt_; //anti-wind up } previous_error = error; - return output; + return true; }; void PID_controller::reset_controller(){ integral = 0.0; @@ -45,8 +44,21 @@ bool PID_controller::set_output_limits(double min_output, double max_output){ max_output_ = max_output; return true; }; -void PID_controller::set_parameters(double k_p,double k_i, double k_d){ - this->k_p=k_p; - this->k_i=k_i; - this->k_d=k_d; +bool PID_controller::set_parameters(double Kp,double Ki, double Kd, double dt){ + Kp_=Kp; + Ki_=Ki; + Kd_=Kd; + if(set_dt(dt)){ + init=true; + return true; + }; + return false; +}; + +bool PID_controller::set_dt(double dt){ + if (dt<=0){ + return false; + } + dt_=dt; + return true; } diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index 2e7384360..81bbc0f92 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -21,7 +21,7 @@ //Konstruktør -Velocity_node::Velocity_node() : rclcpp_lifecycle::LifecycleNode("velocity_controller_lifecycle"), PID_surge(300,10,5), PID_yaw(60,8,12), PID_pitch(10,1,5), lqr_controller() +Velocity_node::Velocity_node() : rclcpp_lifecycle::LifecycleNode("velocity_controller_lifecycle"), lqr_controller() { RCLCPP_INFO(this->get_logger(), "Velocity control node has been started."); get_new_parameters(); @@ -43,6 +43,10 @@ Velocity_node::Velocity_node() : rclcpp_lifecycle::LifecycleNode("velocity_contr PID_surge.set_output_limits(-max_force, max_force); PID_pitch.set_output_limits(-max_force, max_force); PID_yaw.set_output_limits(-max_force, max_force); + PID_surge.set_parameters(300,10,5,publish_rate/1000.0); + PID_surge.set_parameters(60,8,12,publish_rate/1000.0); + PID_surge.set_parameters(10,1,5,publish_rate/1000.0); + if(!lqr_controller.set_matrices(Q,R,inertia_matrix,max_force,dampening_matrix_low,dampening_matrix_high)||!lqr_controller.set_interval(static_cast(publish_rate)/1000)){ controller_type=1; RCLCPP_INFO(this->get_logger(),"Switching to PID"); @@ -56,7 +60,6 @@ Velocity_node::Velocity_node() : rclcpp_lifecycle::LifecycleNode("velocity_contr -//** må forbedre integrasjon og derivasjons beregningene void Velocity_node::calc_thrust() { //RCLCPP_INFO(get_logger(),"Calculating thrust"); @@ -75,9 +78,9 @@ void Velocity_node::calc_thrust() { case 1:{ - PID_surge.calculate_thrust(mod_g_values.surge-current_state.surge,publish_rate/1000.0); - PID_pitch.calculate_thrust(error.thetat,publish_rate/1000.0); - PID_yaw.calculate_thrust(error.psit,publish_rate/1000.0); + PID_surge.calculate_thrust(mod_g_values.surge-current_state.surge); + PID_pitch.calculate_thrust(error.thetat); + PID_yaw.calculate_thrust(error.psit); thrust_out.wrench.force.x = PID_surge.get_output(); thrust_out.wrench.torque.y = PID_pitch.get_output(); thrust_out.wrench.torque.z = PID_yaw.get_output(); diff --git a/control/velocity_controller/tests/test_PID.cpp b/control/velocity_controller/tests/test_PID.cpp index 9fcd1a0df..0d449c15f 100644 --- a/control/velocity_controller/tests/test_PID.cpp +++ b/control/velocity_controller/tests/test_PID.cpp @@ -9,7 +9,7 @@ class PID_test : public ::testing::Test{ double delta=0.0001; PID_controller PID; void SetUp() override{ - PID.set_parameters(0,0,0); + PID.set_parameters(0,0,0,1); PID.reset_controller(); } void TearDown() override{ @@ -34,52 +34,58 @@ class Node_test : public ::testing:Test{ */ TEST_F(PID_test,reset_controller){ - PID.set_parameters(0,1,0); - PID.calculate_thrust(100,100); - PID.calculate_thrust(0,1); + PID.set_parameters(0,1,0,100); + PID.calculate_thrust(100); + PID.set_parameters(0,1,0,1); + PID.calculate_thrust(0); PID.reset_controller(); SCOPED_TRACE("Scenario: reset"); EXPECT_NEAR(PID.get_output(),0,delta); - PID.calculate_thrust(0,1); + PID.calculate_thrust(0); SCOPED_TRACE("Scenario: reset2"); EXPECT_NEAR(PID.get_output(),0,delta); } TEST_F(PID_test,P){ - PID.set_parameters(1,0,0); - EXPECT_NEAR(PID.calculate_thrust(1,1),1,delta); - EXPECT_NEAR(PID.calculate_thrust(2,1),2,delta); - PID.set_parameters(1.2,0,0); - EXPECT_NEAR(PID.calculate_thrust(-2.2,1),-2.64,delta); - EXPECT_NEAR(PID.calculate_thrust(-1.5,1),-1.8,delta); + PID.set_parameters(1,0,0,1); + EXPECT_NEAR(PID.calculate_thrust(1),1,delta); + EXPECT_NEAR(PID.calculate_thrust(2),2,delta); + PID.set_parameters(1.2,0,0,1); + EXPECT_NEAR(PID.calculate_thrust(-2.2),-2.64,delta); + EXPECT_NEAR(PID.calculate_thrust(-1.5),-1.8,delta); } TEST_F(PID_test,I){ - PID.set_parameters(0,1.1,0); - PID.calculate_thrust(1,1); + PID.set_parameters(0,1.1,0,1); + PID.calculate_thrust(1); EXPECT_NEAR(PID.get_output(),0,delta); - PID.calculate_thrust(1,1); + PID.calculate_thrust(1); EXPECT_NEAR(PID.get_output(),1.1,delta); - PID.calculate_thrust(-1,1); + PID.calculate_thrust(-1); EXPECT_NEAR(PID.get_output(),2.2,delta); - EXPECT_NEAR(PID.calculate_thrust(0,1),1.1,delta); + EXPECT_NEAR(PID.calculate_thrust(0),1.1,delta); PID.set_output_limits(-101,101); PID.reset_controller(); - PID.set_parameters(1,1,0); - EXPECT_NEAR(PID.calculate_thrust(1000,10),101,delta); - EXPECT_NEAR(PID.calculate_thrust(0,1),0,delta); + PID.set_parameters(1,1,0,10); + EXPECT_NEAR(PID.calculate_thrust(1000),101,delta); + PID.set_parameters(1,1,0,1); + EXPECT_NEAR(PID.calculate_thrust(0),0,delta); PID.reset_controller(); - EXPECT_NEAR(PID.calculate_thrust(-10000,1),-101,delta); - PID.calculate_thrust(-50,1); - EXPECT_NEAR(PID.calculate_thrust(1,1),-49,delta); + EXPECT_NEAR(PID.calculate_thrust(-10000),-101,delta); + PID.calculate_thrust(-50); + EXPECT_NEAR(PID.calculate_thrust(1),-49,delta); } TEST_F(PID_test,D){ } TEST_F(PID_test,illegal_inputs){ double temp=PID.get_output(); - EXPECT_FALSE(PID.calculate_thrust(1,0)); + PID.set_parameters(1,1,0,0); + + EXPECT_FALSE(PID.calculate_thrust(1)); EXPECT_NEAR(PID.get_output(),temp,delta); EXPECT_FALSE(PID.set_output_limits(1,-1)); - EXPECT_FALSE(PID.calculate_thrust(1,-1)); + PID.set_parameters(1,1,0,-1); + + EXPECT_FALSE(PID.calculate_thrust(1)); } /* TEST(PID,BASIC){ From 44de4c7f50199b29ceb8dfdd60b44868504b15e4 Mon Sep 17 00:00:00 2001 From: Henrik Date: Mon, 23 Mar 2026 14:31:13 +0100 Subject: [PATCH 092/128] set new controller in launch file --- auv_setup/launch/autopilot.launch.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/auv_setup/launch/autopilot.launch.py b/auv_setup/launch/autopilot.launch.py index a6bd585c8..92f153001 100644 --- a/auv_setup/launch/autopilot.launch.py +++ b/auv_setup/launch/autopilot.launch.py @@ -19,8 +19,8 @@ def generate_launch_description() -> LaunchDescription: autopilot_controller = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join( - get_package_share_directory("velocity_controller_lqr"), - "launch/velocity_controller_lqr.launch.py", + get_package_share_directory("velocity_controller"), + "launch/velocity_controller.launch.py", ) ) ) From 17b261c4bc63e30c265d7081fd3762ab1164c842 Mon Sep 17 00:00:00 2001 From: Henrik Date: Mon, 23 Mar 2026 15:13:18 +0100 Subject: [PATCH 093/128] changed autopilot global launch file to Composable Node --- auv_setup/config/robots/orca.yaml | 1 - auv_setup/launch/autopilot.launch.py | 83 +++++++++++++++++----------- 2 files changed, 52 insertions(+), 32 deletions(-) diff --git a/auv_setup/config/robots/orca.yaml b/auv_setup/config/robots/orca.yaml index aa0a883ab..5fd09ccf8 100644 --- a/auv_setup/config/robots/orca.yaml +++ b/auv_setup/config/robots/orca.yaml @@ -127,7 +127,6 @@ pose: "pose" twist: "twist" odom: "odom" - odom: "odom" operation_mode: "operation_mode" killswitch: "killswitch" reference_pose: "reference_pose" diff --git a/auv_setup/launch/autopilot.launch.py b/auv_setup/launch/autopilot.launch.py index 12ed49687..9dedc7e1a 100644 --- a/auv_setup/launch/autopilot.launch.py +++ b/auv_setup/launch/autopilot.launch.py @@ -9,44 +9,65 @@ ) from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration - - +from auv_setup.launch_arg_common import ( + declare_drone_and_namespace_args, + resolve_drone_and_namespace, +) +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode def launch_setup(context, *args, **kwargs): - drone = LaunchConfiguration("drone").perform(context) - - los_guidance_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join( - get_package_share_directory("los_guidance"), - "launch", - "los_guidance.launch.py", - ) - ), - launch_arguments={"drone": drone}.items(), + drone, namespace = resolve_drone_and_namespace(context) + drone_params = os.path.join( + get_package_share_directory("auv_setup"), + "config", + "robots", + f"{drone}.yaml", ) - - autopilot_controller_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join( - get_package_share_directory("velocity_controller"), - "launch", - "velocity_controller.launch.py", - ) - ), - launch_arguments={"drone": drone}.items(), + VC_params = os.path.join( + get_package_share_directory("velocity_controller"), + "config", + "parameters.yaml", ) + adapt_params = os.path.join( + get_package_share_directory("los_guidance"), + "config", + "guidance_params.yaml", + ) + container=ComposableNodeContainer( + name="autopilot_container", + namespace=namespace, + package="rclcpp_components", + executable="component_container_mt", + composable_node_description=[ + ComposableNode( + package="velocity_controller", + plugin="velocity_controller_node", + name="velocity_controller_node", + namespace=namespace, + parameters=[VC_params,drone_params], + extra_arguments=[{"use_intra_process_comms":True}], + ), + ComposableNode( + package="los_guidance", + plugin="los_guidance_node", + name="los_guidance_node", + namespace=namespace, + parameters=[adapt_params,drone_params], + extra_arguments=[{"use_intra_process_comms":True}], + ), - return [los_guidance_launch, autopilot_controller_launch] + ], + output="screen", + arguments=["--ros-args","--log-level","error"], + ) + return [container] + -def generate_launch_description() -> LaunchDescription: +def generate_launch_description(): return LaunchDescription( - [ - DeclareLaunchArgument( - "drone", - default_value="orca", - description="Drone name / namespace", - ), + declare_drone_and_namespace_args() + + [ OpaqueFunction(function=launch_setup), ] ) From 94b1d70ef7772b769c90e040c8e3bcbfa06202a4 Mon Sep 17 00:00:00 2001 From: Henrik Date: Tue, 24 Mar 2026 10:52:50 +0100 Subject: [PATCH 094/128] changed launch files, works now --- .../launch/VCnTest.launch.py | 21 +++++++++---- .../launch/velocity_controller.launch.py | 31 ++++++++++--------- control/velocity_controller/package.xml | 5 +-- 3 files changed, 32 insertions(+), 25 deletions(-) diff --git a/control/velocity_controller/launch/VCnTest.launch.py b/control/velocity_controller/launch/VCnTest.launch.py index d20d1e9ad..dcc50fc1f 100644 --- a/control/velocity_controller/launch/VCnTest.launch.py +++ b/control/velocity_controller/launch/VCnTest.launch.py @@ -7,13 +7,19 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource from ament_index_python.packages import get_package_share_directory from launch.substitutions import LaunchConfiguration +from auv_setup.launch_arg_common import ( + declare_drone_and_namespace_args, + resolve_drone_and_namespace, +) +from launch.actions import OpaqueFunction -def generate_launch_description(): +def launch_setup(context,*args,**kwargs): + drone, namespace=resolve_drone_and_namespace(context) global_share = get_package_share_directory('auv_setup') - config_path_global = os.path.join(global_share,'config','robots','orca.yaml') + config_path_global = os.path.join(global_share,'config','robots',f'{drone}.yaml') common_launch_args = { - "drone": "orca", - "namespace": "orca", + "drone": drone, + "namespace": namespace, }.items() stonefish_dir = get_package_share_directory('stonefish_sim') @@ -59,7 +65,10 @@ def generate_launch_description(): Node(package='velocity_controller', executable='test_VC_node', name=test_VC_name, - namespace='orca', + namespace=namespace, output='screen', parameters=[config_path_global]) - ]) \ No newline at end of file + ]) + +def generate_launch_description(): + return LaunchDescription(declare_drone_and_namespace_args()+[OpaqueFunction(function=launch_setup)]) \ No newline at end of file diff --git a/control/velocity_controller/launch/velocity_controller.launch.py b/control/velocity_controller/launch/velocity_controller.launch.py index 4318ca4af..470613742 100644 --- a/control/velocity_controller/launch/velocity_controller.launch.py +++ b/control/velocity_controller/launch/velocity_controller.launch.py @@ -6,27 +6,28 @@ #from launch.launch_description_sources import PythonLaunchDescriptionSource from ament_index_python.packages import get_package_share_directory from launch.substitutions import LaunchConfiguration +from auv_setup.launch_arg_common import ( + declare_drone_and_namespace_args, + resolve_drone_and_namespace, +) +from launch.actions import OpaqueFunction + + +def launch_setup(context,*args,**kwargs): + drone, namespace=resolve_drone_and_namespace(context) -def generate_launch_description(): pkg_share = get_package_share_directory('velocity_controller') global_share = get_package_share_directory('auv_setup') config_path_local = os.path.join(pkg_share, 'config', 'parameters.yaml') - config_path_global = os.path.join(global_share,'config','robots','orca.yaml') - - node_name_arg = DeclareLaunchArgument( - 'node_name', default_value='velocity_controller_node', - description='Name of the velocity controller node' - ) + config_path_global = os.path.join(global_share,'config','robots',f"{drone}.yaml") - velocity_controller_name = LaunchConfiguration('node_name') - - - return LaunchDescription([ - node_name_arg, + return [ Node(package='velocity_controller', executable='velocity_controller_node', - name=velocity_controller_name, - namespace='orca', + name="velocity_controller_node", + namespace=namespace, output='screen', parameters=[config_path_local,config_path_global]) - ]) \ No newline at end of file + ] +def generate_launch_description(): + return LaunchDescription(declare_drone_and_namespace_args()+[OpaqueFunction(function=launch_setup)]) \ No newline at end of file diff --git a/control/velocity_controller/package.xml b/control/velocity_controller/package.xml index d9acc271e..90dd8db86 100644 --- a/control/velocity_controller/package.xml +++ b/control/velocity_controller/package.xml @@ -18,12 +18,9 @@ ct_core vortex_utils rclcpp_lifecycle + auv_setup ament_cmake_gtest - - - - ament_lint_auto ament_lint_common From 5b68001487cddae9347480701edfbe86b553cfa6 Mon Sep 17 00:00:00 2001 From: Henrik Date: Tue, 24 Mar 2026 10:53:30 +0100 Subject: [PATCH 095/128] minor bug fixes --- auv_setup/launch/autopilot.launch.py | 2 +- control/velocity_controller/launch/VCnTest.launch.py | 8 ++++---- control/velocity_controller/src/velocity_controller.cpp | 6 +++--- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/auv_setup/launch/autopilot.launch.py b/auv_setup/launch/autopilot.launch.py index 9dedc7e1a..69267c3d4 100644 --- a/auv_setup/launch/autopilot.launch.py +++ b/auv_setup/launch/autopilot.launch.py @@ -38,7 +38,7 @@ def launch_setup(context, *args, **kwargs): namespace=namespace, package="rclcpp_components", executable="component_container_mt", - composable_node_description=[ + composable_node_descriptions=[ ComposableNode( package="velocity_controller", plugin="velocity_controller_node", diff --git a/control/velocity_controller/launch/VCnTest.launch.py b/control/velocity_controller/launch/VCnTest.launch.py index dcc50fc1f..44ff3a8a4 100644 --- a/control/velocity_controller/launch/VCnTest.launch.py +++ b/control/velocity_controller/launch/VCnTest.launch.py @@ -35,7 +35,7 @@ def launch_setup(context,*args,**kwargs): actions=[ IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(stonefish_dir, 'launch', 'orca_sim.launch.py') + os.path.join(stonefish_dir, 'launch', 'drone_sim.launch.py') ) ) ] @@ -57,18 +57,18 @@ def launch_setup(context,*args,**kwargs): ) test_VC_name = LaunchConfiguration('node_name_1') - return LaunchDescription([ + return [ stonefish_sim, orca_sim, node_name_arg, - operation_mode_manager_launch, + #operation_mode_manager_launch, Node(package='velocity_controller', executable='test_VC_node', name=test_VC_name, namespace=namespace, output='screen', parameters=[config_path_global]) - ]) + ] def generate_launch_description(): return LaunchDescription(declare_drone_and_namespace_args()+[OpaqueFunction(function=launch_setup)]) \ No newline at end of file diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index 81bbc0f92..66ba48d9c 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -44,8 +44,8 @@ Velocity_node::Velocity_node() : rclcpp_lifecycle::LifecycleNode("velocity_contr PID_pitch.set_output_limits(-max_force, max_force); PID_yaw.set_output_limits(-max_force, max_force); PID_surge.set_parameters(300,10,5,publish_rate/1000.0); - PID_surge.set_parameters(60,8,12,publish_rate/1000.0); - PID_surge.set_parameters(10,1,5,publish_rate/1000.0); + PID_pitch.set_parameters(60,8,12,publish_rate/1000.0); + PID_yaw.set_parameters(10,1,5,publish_rate/1000.0); if(!lqr_controller.set_matrices(Q,R,inertia_matrix,max_force,dampening_matrix_low,dampening_matrix_high)||!lqr_controller.set_interval(static_cast(publish_rate)/1000)){ controller_type=1; @@ -173,7 +173,7 @@ void Velocity_node::guidance_callback(const vortex_msgs::msg::LOSGuidance::Share //RCLCPP_INFO(this->get_logger(),"message: s: %f, p:%f, y:%f", msg_ptr->surge,msg_ptr->pitch,msg_ptr->yaw); return; } -//TODO: update to also update the quaternions + void Velocity_node::odometry_callback(const nav_msgs::msg::Odometry::SharedPtr msg_ptr){ //RCLCPP_INFO(this->get_logger(),"Recieved odometry"); current_state=msg_ptr; //overloaded to fix all the internal states From b29e76dece40a1d22a23dabb09540693d3014678 Mon Sep 17 00:00:00 2001 From: Henrik Date: Tue, 24 Mar 2026 11:07:32 +0100 Subject: [PATCH 096/128] untracked som files --- .../index/LQR_setup.cpp.2C597C3359569B5F.idx | Bin 10314 -> 0 bytes .../index/LQR_setup.hpp.7B50E8B15298D255.idx | Bin 4224 -> 0 bytes .../NMPC_acados.cpp.326F2FC10CEB64DA.idx | Bin 6250 -> 0 bytes .../NMPC_acados.hpp.29D23C04AD60A55C.idx | Bin 3236 -> 0 bytes .../index/NMPC_setup.cpp.86F9F0381989E206.idx | Bin 14226 -> 0 bytes .../index/NMPC_setup.hpp.CAD16FEB6583910A.idx | Bin 2236 -> 0 bytes .../index/PID_setup.cpp.3C9DCE2555B8A965.idx | Bin 2170 -> 0 bytes .../index/PID_setup.hpp.486A8D0A6ED861A2.idx | Bin 1230 -> 0 bytes ...im_solver_auv_model.c.53A9281F3B2EA7B4.idx | Bin 5454 -> 0 bytes ...im_solver_auv_model.h.995A50C901E2AF1C.idx | Bin 3536 -> 0 bytes ...os_solver_auv_model.c.5778764B3A2FDFAE.idx | Bin 12412 -> 0 bytes ...os_solver_auv_model.h.F9AC8187D4C1016F.idx | Bin 5168 -> 0 bytes ..._model_impl_dae_fun.c.CDE429F376604CAB.idx | Bin 2238 -> 0 bytes ...ae_fun_jac_x_xdot_u.c.6132B1FDE79246FD.idx | Bin 2716 -> 0 bytes ..._fun_jac_x_xdot_u_z.c.ED4BE4E791391EA0.idx | Bin 2796 -> 0 bytes ...ae_fun_jac_x_xdot_z.c.59CA37EE71B4BA5D.idx | Bin 2718 -> 0 bytes ..._dae_jac_x_xdot_u_z.c.A6D29D52260DC082.idx | Bin 2772 -> 0 bytes .../auv_model_model.h.60CCFC5FA93A2F4B.idx | Bin 2004 -> 0 bytes ...ct_instantiations.cpp.CECDE40637351DC9.idx | Bin 1054 -> 0 bytes .../main_auv_model.c.FC541DFFAD75A3A0.idx | Bin 1648 -> 0 bytes .../main_sim_auv_model.c.E637641F2593FF77.idx | Bin 1122 -> 0 bytes .../index/test_LQR.cpp.46B8F24A8C36EC93.idx | Bin 8694 -> 0 bytes .../index/test_PID.cpp.575590D7897A814B.idx | Bin 8678 -> 0 bytes .../index/test_VC.cpp.74BA115DB14CB17C.idx | Bin 4538 -> 0 bytes .../index/test_VC.hpp.C3EBE494A18C2184.idx | Bin 1946 -> 0 bytes .../index/utilities.cpp.7F99D4E1DE20E3AB.idx | Bin 2778 -> 0 bytes .../index/utilities.hpp.77C0A5FDF681DAA0.idx | Bin 2878 -> 0 bytes ...locity_controller.cpp.DFC34CF5F86A4B55.idx | Bin 16350 -> 0 bytes ...locity_controller.hpp.3E0346F5513060F5.idx | Bin 4310 -> 0 bytes .gitignore | 61 ------------------ 30 files changed, 61 deletions(-) delete mode 100644 .cache/clangd/index/LQR_setup.cpp.2C597C3359569B5F.idx delete mode 100644 .cache/clangd/index/LQR_setup.hpp.7B50E8B15298D255.idx delete mode 100644 .cache/clangd/index/NMPC_acados.cpp.326F2FC10CEB64DA.idx delete mode 100644 .cache/clangd/index/NMPC_acados.hpp.29D23C04AD60A55C.idx delete mode 100644 .cache/clangd/index/NMPC_setup.cpp.86F9F0381989E206.idx delete mode 100644 .cache/clangd/index/NMPC_setup.hpp.CAD16FEB6583910A.idx delete mode 100644 .cache/clangd/index/PID_setup.cpp.3C9DCE2555B8A965.idx delete mode 100644 .cache/clangd/index/PID_setup.hpp.486A8D0A6ED861A2.idx delete mode 100644 .cache/clangd/index/acados_sim_solver_auv_model.c.53A9281F3B2EA7B4.idx delete mode 100644 .cache/clangd/index/acados_sim_solver_auv_model.h.995A50C901E2AF1C.idx delete mode 100644 .cache/clangd/index/acados_solver_auv_model.c.5778764B3A2FDFAE.idx delete mode 100644 .cache/clangd/index/acados_solver_auv_model.h.F9AC8187D4C1016F.idx delete mode 100644 .cache/clangd/index/auv_model_impl_dae_fun.c.CDE429F376604CAB.idx delete mode 100644 .cache/clangd/index/auv_model_impl_dae_fun_jac_x_xdot_u.c.6132B1FDE79246FD.idx delete mode 100644 .cache/clangd/index/auv_model_impl_dae_fun_jac_x_xdot_u_z.c.ED4BE4E791391EA0.idx delete mode 100644 .cache/clangd/index/auv_model_impl_dae_fun_jac_x_xdot_z.c.59CA37EE71B4BA5D.idx delete mode 100644 .cache/clangd/index/auv_model_impl_dae_jac_x_xdot_u_z.c.A6D29D52260DC082.idx delete mode 100644 .cache/clangd/index/auv_model_model.h.60CCFC5FA93A2F4B.idx delete mode 100644 .cache/clangd/index/ct_instantiations.cpp.CECDE40637351DC9.idx delete mode 100644 .cache/clangd/index/main_auv_model.c.FC541DFFAD75A3A0.idx delete mode 100644 .cache/clangd/index/main_sim_auv_model.c.E637641F2593FF77.idx delete mode 100644 .cache/clangd/index/test_LQR.cpp.46B8F24A8C36EC93.idx delete mode 100644 .cache/clangd/index/test_PID.cpp.575590D7897A814B.idx delete mode 100644 .cache/clangd/index/test_VC.cpp.74BA115DB14CB17C.idx delete mode 100644 .cache/clangd/index/test_VC.hpp.C3EBE494A18C2184.idx delete mode 100644 .cache/clangd/index/utilities.cpp.7F99D4E1DE20E3AB.idx delete mode 100644 .cache/clangd/index/utilities.hpp.77C0A5FDF681DAA0.idx delete mode 100644 .cache/clangd/index/velocity_controller.cpp.DFC34CF5F86A4B55.idx delete mode 100644 .cache/clangd/index/velocity_controller.hpp.3E0346F5513060F5.idx delete mode 100644 .gitignore diff --git a/.cache/clangd/index/LQR_setup.cpp.2C597C3359569B5F.idx b/.cache/clangd/index/LQR_setup.cpp.2C597C3359569B5F.idx deleted file mode 100644 index 7e4238ef0862dda27c1e010f1d3d55d83aefcaed..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 10314 zcmb`NcU%<57r^hGOT7gSIQp^lD!n6OfukNMA`k`CRqTRF5ygV29H)o@8#WLTBPb#& z7%{=blGqCSi2hJ4M2QXKZ%#8e>;Llkn9qK1W@qNTdGp?zw~3Dm4-awV@j@p@ zEy$RXGm+x)cp~&KJ11*~xeAZxug~Kxs5mwzW6m8v;f;}=xA#w9d;a2uO(#-{TfV;}Ha9K6~%CWMD&Ps7>Uf-3d74;W? zNRF^A^ELhEqDk*lm;J{^Y4Zl-#?4$TQ_st~! zXJ^lUuzNW@9eQC>@A2QzAnda{t_3?+HyH?!E z|M+g|%6&=MZRrs?ZBOn>j@|D%dZ}4=i`k+ZWu4lCn(y@Zr~Le9%O6M6J9Fi)9zGVV z+5hg3%ZauA>Do04KdL+kEr*}bLXitxLB%j&h0bLq!puWg%NeQjD)XZMqb zJG^%U?b>}l;Emy-#nE%`4yv);vOFQR#l%%Z?_xgFv*$vVO>@)a4FZ$VlF{|Hv)^0x zpWO9J(BkZ(`=diIdO6HJ+^d&6)qL0Ry5(&JiAV1XPoJ!43`~d!N?TiIxZN^qR(rDb z^_Ay<2`=ZY&DvTt+AQmVe7c&~z!xnyYI8^2UpFPHr0KBsIICKRzV+`)ckQ^EW&3O0 z4$C0*-sx2iWAk%z7K+oyZk72xn`=N1-q$s|<*N3hrp93h`+W|d%QD*6_K)aFWm)c~ za}n*^BeF*Cd%dCYcUSeN|K^6ewxr%%vfR4GWxUar2Tr@MZamfPQQnn$WMceTvBB69 z?}n>g(;i5QBGsl|3`qZ`d%Z*HrKEg*3w8P8Uh`AqWE1C}{4@Uy_1B|k7s3qkPsc~q zmi@Z%faeR7O>r5)Y1d{J+&@%&iQ0V=Nf6peiLa+}cLG=;mA2iuco=At|-pTej8p?90zeZCf(Y zb%yQfErnZq8^<)ep8GrD%Gu=J^l=9p=Q`PD`6p;?o#@+`HvdKQ;*P|&NnOK zzupz9O}St(Zi4fQ4+}CXe(<-;xJMsoo>L&W>(p9r{&W2?$NfXSmwr5T_uqxR|J2@V zmxs3XX3)uSs-HOk@7j~gPahstqx&XF-kCPz{Ji64jcyB#vSmgqR)_rcepBy-EA3<7 z290msG^WJadi9)=*X~vEe;Sx3E&6q=LEhXD|Hpsqw!0VecdD0KM{LaOaUQ4JeAEM* z&kWaea{Bx7o3qy)7Ub+bY~Fch_5;@==jMC=_T%FtKOMQ0-|=s!{Sr^nh=LDGV~1pD zy-pgJb97sB=ekHu>!~ZwZN2v1C$BVofz4}GuOZbfPe%9e{VgJ`eT@tAlN~?1CAIff z#OzUqps^MJY?z#V->Q-w#d(Pxw?slj7CyHYZ=_YoldtF($YfYH$4R*+# zv)@He9qV%b%y=*bm|F}fR&qN(MLBA6$ zQa#i?M7Tpu%A52jgN}|C@@JtAnNFErsospY18%OouVyQszcP{L&FAshE)aHsb!x0I z)|zLc!XF*~4Wz2<8>OnYQ2snk$e)Ksr~row@JT9AQ|xJv-*?e_`^c{;=!B4;g*rHkoGnvbHCzpFb5PtR&l7HOCuv>=lHe6sy#n9V zkyaypc^VY(a>gJDHg}JU_7r?~PRO5&MyLmedep^V?C*@n*dJVREZA9gMaZ9xI&^@j z1F6|f!%eB?pPMHZd|W*A2F=Swlk|hIA9aWm#@X^TR0r@rscDv$gMM`Sx~GQx1T==R zjunVh&c+0nVNKbhvVbN&uLyd8leY7Jz$?YLNvnu(=xNiqGj^3rcBMgWWnQY z+BRozp!W$%oo0(+crirbO{Ce)V6hn-2zP;67w9N=k$nh)hoDcm2Yh;9uyS&wpU_tz zRq70hqSg;OO7YkaAou|Km=i|mIs9+!U=@9ub+&hQ!8?H{*g2F#3C_tJS`E%?fy4FS ze2~L&8gV+LHO?T;j8F+^lzIJgxOIUKS3H#-5D@X6g2q$OA^ZwdUV(s! z`wDblff3<8(C7mlE*tcL8J7q3!7#%8;L#6$SUogS-{5`-!TiIHz@bHrVs*X;CiVXXt{6Z3mThP{sZwdhG6NI(1j| zfM!!vQZ(_AJvK|=@Qt&Ykt)raQ|5+bio{`w@nnh`plZMn`w5z@0goC8AzTMlPzMKr z{Bjehn_!5KU19&(-Z1`5lK~<&+&$a}4=iFMG$WLzB4Q&2kwVYkAgx?pjp6Z?)@>TccP@sAshl16EmDIoC z(t*P$mV;2vZI^?i90rp1Di~G; zp+ur8h^>M+%;&GG4X)Ps+YMPt19&umCwA>3c0YLT2QR{n;NA!xm?uu-EqeS9&s~N3 z!VIV{%z$FKWr5u+YD~RQ^WvXQ=C8F7ELAEYlFCvF*S3-*fi#TnC zASeV5uLQwLE^h$A1}-;)pc!Q&eBbQ}CSqO>CN)jh=G_ez+cR~_2D;imk)_yR-)}mm){SZRD z>-`YqmFr+e9c1G5Vhw@me>vf$WcRC)9$T(gwGZffS8W)A78Ye6rBq5@bHn4xwE_w9 z`+Z>72c9^h;q(y=^f`CL16^7vFgR2^DH09T3i_>Jil;PI5ueO8=!irj&eNR73DryV zmnf~AX7ly(jfhoLf>R~9;nmY@B?MGLFy?CubL7VYHphIWvw7#>FOqU_ayV0ay<>tKv+>pm2f7pA~S6k#GXA~f;1DEw(g3J66R zMi~*x)yUPv>K3ti8hJ{>zw%8#CI5JT729gj3XXVNvFAsI@c9yeZIGQYF=Zj$G6rc= zVk$AmQ_^ghX&8sX{lWuDTZC?eGGfwfq+X-}Zqu1yHPKSz$7E#EO(1LneY}A6!xl5~ z^Fqh-#cXy^_E5ZRG0L2z#V`_cZe*u{;KIX+Xb)syD&s~L8FT8VS$te`>RJSI1@@A+T5pAT}$`4rP_8^8`LP!ZGZ_Jcj)MzCoFTg)5(dNZ#e zwk}s7W=E=z)WWlg*#y-D3cuD-ovckN=I21lBd=6=g2#E+M z6e)>}#AGOYesA~oHw!gT`sgPO!as}HSSHqr&?07$G6NH_g}@Y&6^huEz^vr*24FUT z6_!}UHUrZPR(Q8W=!z+N2u9dph|rZ$^Z|@;s+hg^YU{d{>=F$z>mqPbX76IwL(@YW zFGuSd+5f<-APFKlAA z3@pkBKfEjXR(7hZ2&IQl!T2dELi|@&Py}}0J_x{p-9L6l+OCA9a@0|xE78N_i`X#T zFeN$ul$btkdDTia5j)aoq%wqw*aU+FL-NWhFj)2FT{f^71`z{U4F;>Z?d4!pPBLB* zIs!(U35U48dive_xZFRaJ_#7!m&c!*3ic)HpX1Pw1W^)C(O2>n|tRRm=OVyT|V2%4G zS+zH3l$N7VDFcxVl!=ytU1a14;A=b*Mp1Sn!Zd(=1Gr(MRs<@C{oqcx5$qbl74yx1 zPY0zR(%bTtqRbMfT-TsS$G4*8r>dt4@FKh6{;G_p%~Q|4vkPB!Ev06RTnESUt)G6sBa_{GP_r`-Ya{n_k1M{JWg!e zc~EAs7bSaEl$BCSnjJ3~Pia%s72UspaSDF1Eua z^N(k0Z6+Q@hOiynwu2`=Jz};BT&lnwvu~BFXv66C?@>pIs)Q3o!c@bQ)UaXd>Hp2r zIf~LLYsy+3>z!tkB9f%onbGVLdWjGpBy>#?=$VEE>L$t3tyb@z(_bnWm+j(^Xy5tmN%Y27p^*Rssm>%N_%wAlTwM^$*;6w*xjS) zKUcnS+HRxvjPl*J=@+E3GSDm|8Hj@I%fJci0PhmYb3Yfxu?S+6rm41r2nYUsW(Avf zi{zJ3#FBxA3}`%pqU2#%gxueMB@8jjyfx)y#zm>!v7Tq%UP4Ft1vtF`XA-mCfbAQw z!$RPoiiG)Gg2ziaGBAEZN0(I_x{PiHr_JDrr#k%egS_3Z5<5^upaOg zhhQjPeDHB6Z?EOdbrX~*B}fzTt|3a2e#4gCP-NhO_HW@XtHz) zho(xWacG8gCWmH8(>atOoz0=S(o7CzNwYbWBb~>g`O*a(%9Ac6Bm;G{TUcKrv~`xK zV8!JC83Yn80u5Ba;$*+39N2Mda{hnr3o7epG;Q1=x{2uLt9La(z(PfXX4z!!|%8t5WFz zJ#1z)DtSR)BtDv;nii-^cZlGM<)!fwq>r~8ontO7{*W8ywf#!i6 z3N{Z`63+aPb$|YTgpX=V_{IhMhdsKba0!mC%FGv4s2x2y3mp+Lt#4h1^~D`~0V zVyorum$$!CLd37$m@L0Jw-cS-T+LkN#R(M|H1m`+e)&2TS09^u$law0(rCPU>vcK} zVqT~IiGKdFDF1!;k-p4#CJ(;S8gD;}*VpGDCFYiWtP_=YHX0>_)J|^MXD9gV z{Icw`3Vj5rZf8*dVCidTn79Iq_2Ao7n7;Is{j=iK^| zQj$0hs}zh%N!l$!Qkj;LZ)pnlE(ITaR2A%3O6n&f6eo5%-{@CiRi};$e|#o98k_6M5!&99n9! z)ReTfgK0aMVV{Dt40NcuF74ma(yYVc`%h6WVnf+*N;@G`2&b4Mp(Gq?1+!KVgi zgPMItX*~N%iNesmCzjsK-|`F{#RDKXK&nu9e~<*9KT2b_bo0)do>R58z37dlDod50 zqTIgKR0;U$-s`V)bd3&N{r#En!8@cWJIaousTeATqniXOfkUm}(F&eeQ<)2fToIqY z`ax5~##zS=B%2!N5~n=msLds|^wjpstNm~h6myl0VBDMrvNKd8?T1#6;sH>+hGB7kUF=p%-TgH#^XCfIF#)ENUTp4F(FcZjxFaw!j zW;ip58ODS%62_D9Vtg21#*K+$0+^o@i diff --git a/.cache/clangd/index/LQR_setup.hpp.7B50E8B15298D255.idx b/.cache/clangd/index/LQR_setup.hpp.7B50E8B15298D255.idx deleted file mode 100644 index 24523a4ad7a3f1d91aa220fc01c52e8125ebd4bb..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 4224 zcmYk82~-nT7sqEtAWbF=2_)>3$WrzIVKpTr?3=QvC2rLo5L61O3b?h^;))(bELx!! z=pqFx;!eNgrxfs0Xsu`~w1QTtwGk0ei&POozW1hHk#o3b-v58!owv>VWmbGl%vupa zL}tXV)o13X84&~_LH~LAxmmg?1hIop5Nj(Mb^7Huf<$+F#{^%y3+x^pI`yo^EBsgu zyD3M%pNKwgCGuMJ$@ZDm5wgLI@E6aMj(%squDkH70!69W!>SM0YUCG|_{eWnU+gXz zYrNc2(|6Ro@%xZVBXzU>yi=i{Yjo*qK3`Bdq0IaH^NkiSE~b%-_7-1z|1tj>zvpJe zw)UlgGv;gL8!m51e^_nkDL5lo_3gyAos~NxdW}t6>*u=mk9F{Re=O|TJFWj%_VCo7 zrkf@31_w6Dy+(?jJT9#NqUly;S)wi@>b95HDdpq_+56 zM}LRz&mT8@H(?k%_FOjn`0~D?gnjz-j$4Wo+zD zVRD;ke|grEEqB+_@f|(QXSzT7d&`HlhM4o~K2V-1_wim@_Bguu(*3pytvYDlnO{t< zI>!sb!<9nO;|2Hr3Gs*GlbL=TYn8PnUMSg#! zWk;7}7+MZR812n8?v5K^VvY(gux1k%N=DzdTV3Ayci%*{UNcmZRyqIR^ikg|j}X-q zy8=VO`UM^O*e{Rw*##W=;j4qe2W@=yEeG~3w5wgzl%a20UO)Oo6CiqY>seKY zW5wZXg#X012)FL3dJwCd^)M}S%^rUK*ZMAtpKt7J4hc8ak=fMKIi332JKxdkZ$$O~ z`p>5Mrm}&@A%|FqP5FlXxn=y7p*wCOyd5=&J7=*uO;kbii zQL4@&_*we+XZ??#p1#*Cj#_%(<^{QMdwOr)I(_;hV}kG%3=z#`4J1QZnh|WYEIN=2 z#2e@fM1P;Jw(~t9>Y2h&A!35HR9R}c0JcFAJa5x)k7$MpK(@7`wI>(Awl>+5u&vr4 zVkiMBu9az>xB&JC>qn<{#x1g7C@HdIWHA9;0J|}pcAewFvtlT3WUHsE7jgk?GpG2m zUezG7-O;c(9o^mi+?JuDQSVs$Se6T5XPsp$_V#a3Fw|RkVzi|>7r@qx z%MVDbrD}!>M7=}hp`Kg-o6bqko294;7g4rm#MBd{}lZaxQ>r_4eaCzx|=BPDH(ldT*gz zw@^w}X|Hs7m9Te7>%DBV(+s7={n;=FE`S~QUc$_=fZHEnId1*lNow=h9TfAgb=(x4aAk*;;9La=?X2P7N`t!NDO*m^}osZl~F zDd|KXhBx2))rZkO-{)$a?`OaAD25-rU3UD*l~Ep`Q(`xrw<27jO+bu11@ORBByV+rv=P@Yz*K zHC%u}>im^X-s)TfLwTW9RC}tmTmYNwUt2!C!MmTKqL3ZR50!EO>}*q4i-ergK}(83 zQ?xd-wr9PJyu|nf(CEf|QBsoIV1Ij%q0CWljZ7ow0`z`I*Zig1{reAvgddvT04W(D zUDzZ^k_)#75jJQA+RyKrqnn>K%uqpi^o~+@Ei0@zl^ zuKxZ&^w%gu@$mcK(JbTw*twZY@@h;b?|o9h-S@C9aNi>tU-q!lWuGrUr1EbuG8VE? zbd&{rfI>ErmuU9tGGdbglGLvz$Rqzcv!%WO{NW$>LJ%aCW9uUa`UqKnH-817LddG(REcoy1K&_33^#beMUO|$Jio#q0qRTpizoYP zXpI<-u8`GuYLvhS)QZ<6!?6;wT3RcHH9##dtsk&J$VSi+Vz@wsY$P2i1`r6@XgXR9 zzcz$y3>_neB?H)^v0~D~tYMm@4Tn~c0j)^JKn3~6@kVFcfqhBnh$?BN7;ai28%zg_ z;od?mzu;+;EfuX2!-@&nSUOe=I{LM9q7P4Vsk&N*QGLRL?7^@%yQIU*s3bo-wMl!}IWCKBw zjPVKCb^V*y&AVN(ixjef{6Gd!2tY-%#2|$ta1KxrOi_@b7s|%q1R21IvN1J5hHXUI z7?ZE$mzc!ah>;)z9+8aM2r^t6s0hX($Z!WBnSADP2#y6LQ-XgP_;LV3jwfKkZG>LHQSfCF3=54C z1MsCv0Qr!Nu?JZ|JS1b>L2V!%k}>8$2Ew6NVzPk@*8|GNSOXcZS(MFj?B!5^Vt6D7 zVK5S)7m_i$K!)oD*G9;K+PRrIX}Dfq?&7@J_(6ulzkh7_=5_%Q9zWZ}(aAyPEGHbn TAqe^8qr>E*^W>w`}WFzuMaAdZXQU(taoBZ26(T{PVo$ywB}C z=Q(HI?1F+?y-1W>KfANJp}j^T5{Y>HZ*_Z{BUUUDDSSkt&OL|9OIL&@tFOIwe%7v? zfraL`UgMwfcQi)4Z;dfNimP|5nV)BFPP=Tp=RSY#_zK?zNn7j0v&O#e9KQ6_@e-M_ zyR>CP^y$W$YZsT5EV^J@6TVRP&R^c)Tip-lDz*LF{@fTk_w>^BM?c^G(coH1Ucr^I zTi-?vT{tT{@i1hCvoP!Mr;Bf*&66Ki?>n4*_*&{K5&Jp+S-Wxu^le*~sBR6nem(b7 z!Em_-2|w{GZR;v{E&jcQ`5!8Ve_c~^yXRkf*L1!;`^u}nW4UX7XFK$Ep4b%=tiC>S zExW6t;`KFK&knWrzxu>3>x`BY)e|qaPr3Z9TQ^0K+nV$Dh@oB>gVXMVk&)#Ut;+Kh!FHUn`T-KXmcWt|O^4qVbssCMF5dZkbwJ-fn4sN{g zD63!E|Iw*%<#$~Z4f;EOj5%}a^3Om2{o0usDPgDoT9G&Ly0)@@mpd`|!};@{S!*@? zsYA4}`QrNRP49fm`!3)3(sr$+ZTaBG-3w>#dZVTE`@SF=W`XqmVu-Sz8t9iRPTC>-UyrwttgzExGy5eZ!(F+pYZ@Onpago^jMYEy}2|yd+w^wz+mX zh(u%7x_=*j?>7Nju`y7Dd{w@I!aVgny-0?03V)K$`4+vEv{kPbM`5u^WiklUPfTe_+GU*7K@8iMWKR4ZV4o#%kBluXnyMb5w$o5iw8k92!TSPJW=BrkN-q3G=_e0 zbaKvVwKx(#5Tpvy3UTr{rAHQw#l^3*2K|En;HFv}jm4v&8if!cNuCryUhqY1_0aL= zg#Qf^oA3iFxk@2~OTwi@-21x(`Qf=|zfp@Luy{A%zevHZwGzqD1RsJB5=R~#fq)|r zNchfc$qPPvu-Q+_A=7MAF-cVpZ3oqMP!o4>Xb0$aKmg@YNFRj^G7X0gLf}Er6Mir1 zDd(mc<^C!G>C$u=BuNBhC^pO?DJLL%mOY1rNkBQ-Ik}Wd@=Iou z=|mX;Ibj<(;RT?-xS-JmO_ckfxDVz~c7xpw*~Ci%8UcO;^pwZJJ`UN$9045z{unjy z=%PQR{L1C8RdGnC)M{fmRNKhVlr=M?gt=4CG^=BqnfZ95mw)K=}wLkAQ~qF_0gl&y#gFCFF;K zRu-`D^R7kv>jjiA$>(U43MGYJBFO_GKNLAK#D4PX$GiNW3>I@JRuLOOqK^s1#t~X} zz4epJrDb6<4yA>sMUoeAC_N-ypkz<6TPfx1^Ft^tRxj34Z*Bqg7GQW3tfOF~)*b}i zLF$dO@0RU+`Nbn~VgXrXmLOt@fD&X0URrC~-JN%*!3+X2s*EP0`WI^Hi1gEy2}%wv zku6bB6{=;`3@w!{WvE70!%(fPmZ4@@Gea%17KT>JRx-3owu+%vSt~;aKymPKJFQaxM{9*;q-e53!9yW}P{JxPwE>^~*!3f$i3I8>J3w2Sb}7 zcr%0&=?gwrn+IA{sn~TInI?uTm`K~+Gjhjp8gM9566r_YIQxmZoC%?C##+mcIO zkE4;7@C2oNbv{qRFQCfCN@l%RHC8d?C~y=JRip!tOv&p%v*B<>OQPlEogA`+TVlxM z97+gJV5q`ekwhloP^Gz&p(=9~Lyd8b3G_)voP(kE==OLj>xk}Ps0Si?z(}RN5Y-ED zl=}hoLnPrR4oI_g8_wIY5mAyTuj8;0dP^8%gg$|xipUD)g_V(&3{^!|G1M5=$f)87 zb1>8%+U`|@Lmi{ad7{?yW?B^D4X@s{Cf ze2JDsjDPktyyWLU2cG|aU3U07?B_ylVIUcdlcKPrn2cF{;PTn+&ZS*a0VzTh2I3R} zrK{7Ka)EnL@4h0SEOizm%hBg}WpCy^sOKL4W;57f=gD^j7T)i}OK_TcnudggM>DiDbc7a4%JL_+-5tbZ_5S*)WNh2q;f#>S z&3p0BF{+q&@-s;}9DL7lh6KOk`J7jyYY)2d(}G6uHV(w^_~{={H{^MXbN~69lXnha z8x2Z>hRn{POl785PY%sg&h*mvCwJU_x~}pdeuY}5Mq~xuHGK5oj#+mM{~WD~Hj|kW z|NhptJ^!OVCBdz8vMH6uw%os*Y3s_P%429$o(0{rw2dZHVdXt1FQR((Q?>dvAM81Y z%S2^E<#IZ@s-cRZy&%~OKEweW8iF8v7@#~1I(!%)oEvs2erQwBdAw!#%ly^EI1Z@| zY9l3$Q8Sg2R;Fd1)JybC=?InwGhP+qg=At5-p!{*+9*X$jbbR;5^bfj7*k9NC8ODx zN@*j+ZKO#XJXvsa{rmSWP7zRrq#}ZN1@HWlN`|T=RbJ|R^W(p*lU#b&6QXESJPna) zx+#a!AVdyAEO8WvkGaNCFq7hCE7v{wmzKlt;qXVQBE6aa*1drZgLiG0e0gNZFxZLK zJhCU+lS$_DC?_$8At&Ir-@8Hbs1Gvwz)nozksHF?5J5EIanBw$4iQ9?vDX{Y=8OM! zUCE)d%dSy9tWq!1$9JEPa5U+fmd%&8l}1+ zjrtp(3rE18@;Ic9LmIJ<$ERE6G4LmR=^wvcc{=;*1m5y?K*SE(z%Bmj{{H(fAHItZ zX-0{WDFqgzWhz}x3C0A5D(n@xL{nVP?3D~v*{c}pfw&&J+&$f5bU)2%;$_hOaLYtu z@nZC*k<|<5eug7is4VoBa@?8D>L?)Z%``l`u)?S2A;JOE>Uc604w%-!(|~1aGrft0 zlP=8eZ6bLzQ#+H9<%H!hvJz8?g@&(GS?X;jdDIQ6ZhDX638vNwJWdBT=_PT(jTE0 zSjXmVADFVB=@E|i4hY5hNfhJ$>)w#`rg-)KcJ` zAYVkHaPL<@r{?O8PaAK3S|WrQ!bO5tBC2byZwkVw3kVQTkw|@fWpY0Sr&MXxLH-)x GMgIqBpVBY@ diff --git a/.cache/clangd/index/NMPC_acados.hpp.29D23C04AD60A55C.idx b/.cache/clangd/index/NMPC_acados.hpp.29D23C04AD60A55C.idx deleted file mode 100644 index c4754ccd0b6d25f937bfb5457578579e95628a28..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 3236 zcmY+H4^&gv9mij;3E_pu|{4YdBLIttZl3$1N;-=ja%b@O?5{3%bulOnQrs*&56(4`~1emB_99P3e({~bhKW( z-0Cm>PxeFN(l-moWGShMlMmMTBERX>j1DuMt?M>!p1ZO6yI0R2+u!7BZ`w0EKYM(C z$(H;z+uY3u&W4s$ZP~oxtM}UN>WpW#h97 zd{o>lnBZnIPWwqY9{KfHk`<07!T>AXN zZPS$JXPq3ZXuR=y@}J)rp?lU{eD~PHN3R|0S5JPh>aX692iOG-io?5Fm_=s$(L3^2 z-&j6eH(j-4O=0wuO;g8yzidv=^p#iI+&M=spNw)Rh2A>Tn08K5SrW=ecUo=W& zK@$oaU;-T2&TjdvGtNeiL3*&=q?-rZbK>J`yJGx2jUsXK!pK6E7{Ffq$;$s;@D;~t zkPO=#&*{Yg_I*ayCi`NAokpqHE;E+Ji2>}-uAGP)kL`BQNRI7Uk+T$H0DF4LGYOg3 z=DV~=f^B28Q7s0r6Vmr|kxSp6N~2_K=c;q{VgOs#{2;}(W;C5f(b)E?y}TH}E;uBc zq5ACy9vV^D&Qs^<#Q?Va%5{0uDhE;QsE*03+ z+*i8Z=k8W$Py}wx^Sn_EU>9pzzigP-zDSPn)nQc;s%S9~q_{s4R<`$@j2G;Weo@V! zL`ZKVZE9%0ZDB)CY4_GT1|@-=5t*R``|9^Sz42YjZ8S3C-t*P@dNF{Vvmv|PF@XK%bNc(o9~HgNpjb$rOeRNzJ!ADkZq=tP z-88b{)}?%@UJPL8#0^Z@yI%JJjiRufWy(qv1K6{tZ|wW0weRl?io>=w+Nu!)*e4W! zk_^fEhG;}$+h_8b#Q=8Uft9+K)t?SAC?1C9B%K_xyx+Sm>)UGQmWus1^u)j@ zDArH0C5m;XIiG}G zqF8sX+Y4-?*fPFM4=|$GkT%4@EvQU0e4X68pTfNqNDE?M9}~VhZ+vC!yc)~-(tH}O z6pU2Dix7p*p(88{@B|s)i73_-VPb)23K5}D2@s0sBk&VsJRgVC6`qez=i}jsQmil5 zX9$$A?dmKf#{}C8#4; zOi=ZwgG~M33@O$uGgDBN5+U`olG4Hh$tcz;wMGFvaLH)3KnadN9<;z`NC14svcP4K zuNL)`v%wu}@j7uP?(sUo*}@AVuo`N^0mZVwXpj$h<%5UX`kQc)ENPYjmGikOP$#o>1;v*aH)S))a4a)+|L59VbC`EpQT;F@`#!J$H ze#|L{(UA}ymowg(7`{jDIJX5xiN~Dbo)#|oNFN=(Ri$Jp4I{$@AJ#s{!MIF7Xlw4+Y4ceY*?~o@%|0mmpV~tAv{d+`&nN#{+7^yCGp&;k^JE-V2;t zcqc%H8-SAtj|9j7R4faR1IR#RoJ4pJKwep&dCu_Rh3}D?^&9*~$f;(7reGYTs;OMI zKsX+?H5IiRgb!K>6F((<5JXJ)Qx*Oc{^Rh|PYWLe!G%BB!%c_2EvR~UPN7n3b)1Hw PW0Zt0>}$fGO!)JEs|c=R diff --git a/.cache/clangd/index/NMPC_setup.cpp.86F9F0381989E206.idx b/.cache/clangd/index/NMPC_setup.cpp.86F9F0381989E206.idx deleted file mode 100644 index ee7f67abe0f8bdc2e31b66d091ed7824685d5605..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 14226 zcmchdhgVcb7r>cuaoOc9W%seWz|z~&q=+3AR8S)h#uB5L1SJ+gz_wsw1rdy&J&F<~ z6oY~V>_H@g4N;>3NsuU_*hq{q5n}B6&6@MxT>k-cPLlm`Uzs;^`^@O!k&!EVnV1az zeE9sNsq-dFOiWC8`tRI%bK>uHF)?YfFfp0G?T3%9r`+mo_0zz)(c}I;vZ&9vTj}`v zMTgHa_Wja-LalAaxU>IWB@-W(R}Wu`7g*tmdox#rc5eOa(W+&E+{wWGMJD#s`lhFT znWoA7q5X@H-x{vyUq@~~H(%NK>%w5$=(x`f{SK0VqJs^e+pWH}`N)`B`Q6PPzB@W6 zj<{F#xc0+juIRJA2P>{^ zz1%v`t?!D+;*6BMzZPB1J5%f#O1j?ey(aRr_$5Do;rP#jVc(>-&kos=u+*GAqh_V-+MSLKeXnwhtwD-L(AGcB|jWKP<125&LD zHegQ4^3)txk58(?KYv(1(dTi-x!fx`#bMw19oRR^W$5A+sbPHzHznGX70o&~_M6y4 zm$v)fX*+JX@%-zVCG!?+n6kgnxu02H^wrYye?4?OE}1_1U}J6nX(vuj?-p?C@FQ*X znDmj)ya77-Xw9DLyX z!R49DuKm7$%eB~_CI^rDR=z2=#87#DUDc=+|HN)yb?S>vcmKKbRsRdoXztBFD_etR z`tDh#v9Ndain0woyJPRE@(G^t$ftCRy;opAHytCh9ecp~i z#S4lL|K_8}nvk>Ujm0IeE)9krH}=^-n-OJ*PO#EE{q@Bc-=>yMon|O>zWUXW(OYk4 z{%A6O^qAOBaL-ZIJ#Ws5`#a;)rUjDYJGy1{rc@MA7KK5c`cc%yIqgUVQneTS=l;iCsp`o|ewhUB$8&us_b~VK7 z_sve1PhB$`8p!3G`yuk)$F4{6-~8HtwZFZ_u*|0F8|(2a`glJb_jHal=kT*_X;;ja z)Zbk&TK3(|wDIBk2O7^a)nWT<(DCCZoCb89kDIzT_CVj*qM*?W|qjTj7tXxCQ&S;9gN?&SpNs1?v;L zxF%*^y=jFf(F+u6g=>^tCKn%{@Y@fSu7@puv%({&I1j7yuxFG^CKHD@XX>=MML{Me zH8F=i8QX4(BsxMIh>I{R9T{sk){#++T@0Wp4pYP{#}cL2Z|g%%tvI64E4m5eqBKA; z1khN;L_l95#aaZo6e-F92e<@yF=?PI(2`ML5VD>R>?8cG332ckyJL%{VoqNj8%J#cAVg1=%IH1uHTJjIl&IQi**l zaey$?D}@iDRV!+(5Kmf=rUhw*JGA4Ge8|iUYY9huCB8CYa2%Penk-(#ktv!fI^hT6F+l5D?zqq zHR3jDX4}$dcOXdz97HMBl(Iq0?5I(!cRy<@=g9)c1@6LFd9u)cp{U(F$wq_ZdtLUD;C%mIm;(hJJ=nCQCI<1y`nOQblX!Nt$UIkfnL0>4p2s zNO|s?^I=Y1I1(TU=qe})N0ObBU4@G{lH!~q(gzmxrF{&|&eZtxaAY1HA?QHbs+6E- zwOibz9O)+MX2zZsZxOF#&x&`A_hgiyNU#RvngC>p7Kz}#MAt-7*1&};CKnfaVxHt8 zhg{?)Jc1`BNLPZy!A=`}tIMU@ET6Z2CL87Bl}H$_(bKe{0(?jO%jD@M3-14G5+Iel zV=ob~*sg8ogG@Q%CGi3??=A5LG*LCtM))|6 z#Asqfzt53Ls!5{kywBh6(%|f#^l?7sKGtmVeTMlEM!v4TdPaV3e%_2)u(Sm$guG*B zdqvj8p0YkJ92v|F7EOne{a{h)ITFD|09k}>gs9jYiR2=IEYda-$eNH<69TK-g18n0 zu5LljEo>P%avgEkq5L{>z7FNBh--!NR^-`=L^bEg4aD7m@*BwW29&oUZ5t8|&lowB z9f)T$--(o+kXK=D6$ay7g`HPnQSCUg8f#Z$@VvFyWi1xflq2h~%R0y#v34W8J_m>9 z-~sIY`IyVc7EpJ&0?37!D}-Ext&6aVUB4f5`=Pu7%PTO)B(E-o(u=aNBn!*g==WgDJ=jKgy2Y_COK)5%7(g9` zF*5y^Et{BdZ|#hc(asnd?TnGp&KMc(jM0B~Ch+6zwZp>_UkrHb0Ve0;cwrPq`_H5H zUo6_cS%S}QkAGqY(p+PSe2KX*e4Z>t@}&rHI+CX|7V>7scRChAf=I3E=XTTvJyTTdpD!_gMZ+*qj=;d5Cg64%ySki>$ zf`B7eu;dElW-Mujd>xx#$JT=HX#8?s_7A@nGGkCQT{T~suUwdCniIp)sX_^Uw{IHHPDel)VmP%Wn6@M7eU^RLbkK1)sZ^XuMYJW zuCKflH;+&9|4`161)d9hg+Fs-q1!^>jIvShY<3ezGt7^((I@QcYGhZ<5;=}$ly)_c zPawM!kSnl91@>X9bs9UIhRa-s9qJgz#!rsabQD^9%PTztzG8`FN>l zc1AN4?5X znD9|LvJds##}fQwUS0EA`*oj19m;-ezaKjY3vPdm%#!L7X3T~nN8*xkR8v&@V0v;E@n@mD9%<_lNFA>rn=43cC~)L1 zvb_u6cOPl*!|OYcQwMSuX27UtPMwf9VcSjbrZ2WRkPl;Lt=8X6)@t zxFw=_8fy;eQUs(+5x10?FHh2uG#$#*5tk0-*(iWEF4@zwQ8aB_GOk9}v~kI}23ga_ zCF2vwnl>&O-$JrmNG$1$?nHJQas}4W#-^a=#;zt$8=H*lFi#tsjGM4y6LfZ0u;UfT z&DgP-@s{y@qpi2i-tyMI!t(KO;X%gaaYg~2EwEz0lPccft1VVMO}oqHb-1m|L$9(j%Rbe=s ztL8|!CS3Hgv_0e&A=(fnj|j1a$B}JFw~Zx|#++tIIf@Y0iz5}twgToPN0IGO$koWL z8s8csr4< z6Y?tTvkHTnZ!Pv+i~ZR59LCDSuqAT}Tb{yJg5q*?eSJ=|aL~o1gmgtfQ?n~m5fGo)|N!)|sNivrVWXVp+Vv@|0 z6fOnGQXEsjeFnKf%z}7g;0@qw4Pl0GL2YOllBa?zQ+27J_DYkd0a=<~n!j-0p(|4i zeeW+WpIo_=FR7PE9{=p@zT#0~ z>3dNZ(&lkj*DH57-Eb(zTrpM(nx3)x&E$d2WA@NSMgg)ffVN$T>l(GcNAo-`x3W+bK}u@^a? zaev2^)3B9^xJ(vLIr0Q?PZ*C%`D$G0qBG^R^_GjcTv*EGB9B}ot}913IJgohFG21l zNOUtNo|-+@2L~PDd9oZym%|Jp3rVveKSk1~ke?yxGsw@8^f}}gNcsZuDnvKD)+#iEYu=oW;>I>S#OFb6<9-)NI@?2^19M< zB&G_`d+cr6n-@-9aHOAUKc2N52bvBPr_Z=OX`06pD2^VY(Y0V-p-J8F!Gk~64pE+b zFFq_c{`!sZH`Qlo^bV2)!N!4elBgPVpH-0}wh?0A7j$tU`D~p^Yl6E`VH!)rE|oJaYZbhga3p#uMF**~4_*8sBD z+XHgaI{|Xjy8#MUg^M4|lOd`hfT*ut&Nh{JLVa}>Uxc}SfH zxdi!_AW)W8Aie^2pN=B_DCA1SS3;(-;VFDSjSbHr)7bDFGK~!{Ak*0J3NAm54X;_M zXY91e-!OM(R1f)53<^#f8`9Z1EMu=!o`FG@L}SA;)?zev>g1UiWLR0)E{pBR()q-0 zS?nmyW5+JYG~94y27G8`yq zAWwVvj{8{)GI(+6ewPi6SE+5LI;4sngC*uaRyptJy!u|cK&!sXxt_A{_iOY#R6t-r zFsl>-wSi(gohMP+D6n;K0NETspcirm*_>ffh$n4G*TxbeBYU;MzQk&*TaCpkjVBwi zZX*^WDNm2v*AiqS`FCiCWRWUR4dFRJzH?PfY zdD~E!m5=8M3uA039HF~WmEvwx*^GZfB3^B7)$;Egf}_nbMZ9)`4icpMv zi`o7C4VKs5H1)k9(~)q?aM2Cu$PmjRfVLs!Hny*!qfSqm2e}-1m$Qc&chL0vkXV!J zXiveag6+)c$Wdf<6mli9s$?u!01bb}^R+_o`*ZHGBfIB&wb4DTT4Y|!oS2TBLzd^5 z6E)^smTOteGv-{D>mcV~pB&bO{@SwDChEzgThs}>yP;kS^IrVJbKf-)H%!Lgp;2=$=JqmYceQ=AVspv;J8z9?R6d?449{px8Pu3o ziN-XqpZ9@>Q=Z(RiG!cSPu!7jxOXz8a^TPH);w`lyNbJYJn5$CrWKaK*fMu4XB*$f zmbt?|Se?}(Z7tijqAfDit&Z*P8@Wdv+r8)Ma_P>&7}bFI2BuCtxs3SBkgp(~?wm1u zIgMqf*)a%XOHNh?`63Rzi2Dn(!P89zC%QAmxE_brLwN&sqB~Q9T=+j#zyHo}3kn!A z^_$wDhS?8j;p-{!1lgosq6aiaHAWmON5*Q#iYX-b&W>-8N#8>uAIcuSmrVHS_z@|c zKP`Jm>#7#y-h#Y@+0@a7oBMUfeH|a2PwYDR(OaG8Dp_ULQds=!-KO?O^kB?L+mT{7 zL`TM`$B2PRM<%K#0-9_)S-dje;g92Kn=~DC2(8Gc6?J1nxPg3bFs?W=ujhnq`p5Lb zgUItBbR>t6=OM;zJBih$Pum|W=%F{aNHKa*@`w~;8%>pwE|003ao2Y6KGr;-hpCZm zB}?Iq9ZKI5%$sp^v;nOwb{Y@01>AtV4*RZy9mgCTnS;f>586@1gDUV~L9L7(M%%Nj z0^_Lq`qZ(CEo5WxSbh4>PiW)t6>@sTwtIQ94C|J$T&vp}xoPj?H=n#0C$x3r)K>qp zfiK=Rwls7XLHHbFV@pGK5g0dP4c$c$ICz)5YFpyWm$b#=FYyPb-bboNisdawh=z#S z4EN44iP`f9{G>a7`4u%2dK#br`Wbo}pb#=W4NwG`o(9+sxe95jSeP*SCA&H}zeU)A zR@1`B=S-bB*?637?wq)}YmGm~82|I%Kcj>nCMF}<&++W%hvH8}IzwMwDv$YYsrl$J zW5<0o{=?`|Bcn_{5-%_r$9|4xKgY74QS4_l`x%$?`OLW#oo7ejE)vtOQZt#{oKsj> zT3M@URrl;Pi=Q?sMbTX#=!+ w?O<)NHeB07+eh15>#z0JhG_?CUA05Bk=mi!VOpKmfwxiH+YZ;*As3VX1M_btSO5S3 diff --git a/.cache/clangd/index/NMPC_setup.hpp.CAD16FEB6583910A.idx b/.cache/clangd/index/NMPC_setup.hpp.CAD16FEB6583910A.idx deleted file mode 100644 index 4f031ff590a1bdd32b4210039fed6ed331362fd5..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 2236 zcmYjS3s6*57{2!ayL<7&SM|(NRgXRz{Ns%SI(M0Uv;*jH4w! z7&RMlus~6BG#h-NmY61tV~LiUQaWZ2Uz0OpQKn;K25SGg&TrlszS;YI|2cEdfBx@d zY)*FeK8qv`doyR=jKVpS<0VOQ!e9BEvgt&YBo~pSc@3>KnPa|sR$F?LFVai$m0{I+ zy(23Ne~Cm(_eL)N_;T0prCXd6j-HydVPkjq;4bfkpU3?$tT>u7so#;`EIS-KdoLxv z3p+nQbN={8J+lh_-L>Adzb&&fqh-Rp`FrHFgqs(py}cr@t9HcI#U5Y5O96A=7nTj* z)MG^DKJ(tTk+-)U-8$gn!Pif%nQ1M%v-NndhRs)^H*cM3y;oGV?s(V2BHkuSHDSI(F+HBORh$2RsW zx_;KA5uXALf`_Y$>Xo2*;_5LFX;GYyis5gQx9WGt_BkBm{DR2kZu* z{QfGLu1zfUGU5g~$b)tRP;QFtu77x9NPv+zkUgr$W&p}_-}e;;xho{j1M|{-BlbzG?I+xrV4eT zy#BuPt>=PGqZtXn>QsMf!~m4%2KPH=FKW+c#09cTbvX?{`KjmwJ>BIl(1;T^w<)$n zlymFyb8-%^Dq>mt7xugBnf1n z#itm6^5UoK{hm=1*D+!RIn2Wj15lpSmvmkl*WO|#Fh1O5GPw+p9yLMc3H8%2ULN}J za2q4>u->bBZ3f`_Td^DUx4%z6rx6GC%@VYvqg+!Q7cO4%(-n>QQMM}9$9FboV9!tg zoO1UXBQk96@H-*~;Oe4|)-S@%yh9@lS1XG0c+&e8t~-|bU1=vHDy)VJ2^xT_uYMBK zPQP*eZ$>;ICtH#g15nP|Zf$8OF6m~(1hVee!v>&SRyei%S8&@aF?(7^&7gz?%B?Bx3G4)uQ9g zQ_fYc<6Tg$shW;EP_C=Gj-965uG)1xC*=;+q2mNlo~owm=;4y<2bO)?Ux}J1_ftPZ z2T-0uQy6-Y@&FAm><{HZ8f4fO%0o27(3X^kX_%oCFlOL*B4hA)7%y-wB36*bPl9TZ zDM(|S04=fvX^aurM8pTu_{mT$vIA-SKA=TrAdPVWw8#piF(!Z(8G$r52WXKENMkGj zEiwUVYy!|C3y{v8wS0Wzg0|f-AQ2jIVvA6QY7q^O{=rBRW&+S65U>g-0uC=S0O{t# z*DAN&8GlX;Uy_oB!-w{OY7q%ojROIDi#$LYS3$Li0Hks3pj!AJX|z7j!uLqyAObD? sjnC5 zYb~5^7q2T517U-TqrS64ar8Z;A^Q2?+Cxz9`>{p&j~jqi#Oa`V4Yq-6R%}{4?a)8M zM)^SXZ2Wxbk43L7tGc3{4Tb)ZZ!0Y)I#(D9F1)t6G36Rn3NI7t-)a+Q7n7O|Ae2v^J zuvIeu`|o)!-8v(-_X`MjqDsGCGcfhqc*#bo*Qph+kgE5#isr~z+ekyDF z9RJAE>7kIE4lU;kZ8xJwcun8U_`wQYPm*s*vDayx!6E6z{6vE#9QU>jkz>CE+rKUE z`a#m2U?#51+dt}A@E(~;Pw?dShc%bovhq*Px#xPk?!@G`?*bN*8Wep~{Jg(&GXF4> zc}$L7YHMox6V?{be4U^2b7uDVrzW|Q5vOzh_*E~fv%AAvcH|~j%3o&7I=8l8N(}9~ zzAv@%i(S{vWc1l@G{eghSDq(asOsBP9OYDUMY_iQK>~Zyd@Js*zry+0RsJ?cA(L2W z@<(R8OHBjo+}cL%zt`*_wuP>C%L&bnn!Xj$F_Qkx&hddp!Q4u&WoX7-Rs`vp#9r~R zW#_lM(?=VQBzd1(HqhwSV{>-gbJW3`eeZJ?xv}PAyRnh4R^^#}?GY6(zH)lIa_@Sh zVdnANNvM*qqZw&flL&l+|^_Tjo=lQ|#kPR#{sFbsYD z>yhafsV{9yNt;~SLnm(+L5Gcg?My8TTBr&%9o%-7awE7Vgw^GE@)ySs={|$B0E69v zcg3H*DsaD9d*Y|mT@t_S!M6O%S*HEG5bfWRKjTfA9_*GKQCu^**c|P?$aGt5q}HB! zz*Vq9Z=X2$uw_E_=xlf3_QkY4|13N;yAL%YbvH)hmno-xvQ^8zMPB>JgYVm?$vIr03$!bi7 z6mu05lfXn&BNNz!j0u}1XG?zIuHB=Qk_Kns)EF!&994`n(fOm65+;?}`DCa9-4s^f z;2O0cJp(Qz_AoC4@W%A6g>!9{90L#PN#MNgyA)N*DE+kVU$ zCJ)Vmy1s;{=m_QAlqT$wIw(6rRv=kkh z#c$Hkgz;M11RWxYOwpwxG(9>n1t!1|H~~*!4VD25zyX$E6<7||0xPf@xPcYG6}STr zzy-Fz9ykCS;02t)2Cx#W0WQD}ECuVqIy4Jp3t#(9X0d zb9Q=OGF>7q{h9J|Gh`BkS}{WA>duOw6WtpXrTKM6I;^aAr{k`M3$sBxDt^tI%MP?fC5NAyO&@$<@Y_OcN!K^~oL>3#r6suXy!G+= zyy^I9K1P0Nf4OySirFg9{^8ma``o>v*EcKHKX+nka3sK1e|IkM&0KJ0?}ZoAnU`(* z4&9y~>eG)A!%5?lG+RT-pT^23HJr(9yH~@-->X8)-7-5zm%nP78NFv|n_F?8vsg-B z4@rw7uPl(gnx5{2;M(mOiP!vlY$aSl#OJD7_vYOnI%XcZ_gEfgG`|nM;aMB$>~X(i z>g)IZWyVZ;0bZ*BmY&Sa(7bLWhWh1PgSxi{PRW3*cErY3Ac~fyW6}P^=ZSL@a=- z^h)Kkk>MJGRTLWqqfRV{NTU|inV~1GUZahGbNVB zy32-E71jWDk~tXxd-b_C^Evaq1gmLpouD&{1#sNft3^eVE&Yy|3gDy;Qmt5!Oq{ud z<8N-c^J!CpdYE7(-ER;KIx#EX%iz>#zj6%Q?9kFv?9oYC5*at;8rgg|^^S zA{Sjk3@&NRL_!P>Y0Lyd4DRUJ%o)Vsj6ThTK@6^F%tS#9j%dsTK@4u_*~|&V;DpA^ z1;pTj#>@f4;DDaZ1V9Y`SZ<%W{WV7lj!h6C%%ARM!-p8=PCG4rn$9~bnVn!q=Z0S^ diff --git a/.cache/clangd/index/acados_sim_solver_auv_model.c.53A9281F3B2EA7B4.idx b/.cache/clangd/index/acados_sim_solver_auv_model.c.53A9281F3B2EA7B4.idx deleted file mode 100644 index 795aba4db8c99b5dd262916d8b663c4962e96b29..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 5454 zcmY*d3p`cX8sBTRm9%#g=R|az;}zl{oU@NdMG&u+5oo5`+sX>`3Ne!BquBi+$ zrV>#M(~O#=L}f;=nbhUdb7pdjOcG|g&0TWW+57Gve*6D@-~PUDed~Ly-?; z@Q0tF(CUh-vRlwMp_OO)*4gY}l9l9GH@UiaQ^kw28?RdyJ-O5Ic&w~C?~WoT#{G^s zuzyEv&bcB<)Z~)MRIlrPtzFi_*!1}Icf1##T5q$>CBQVM^V`IUvgKQA2jpkIyD#ra z-s1Ig>9e(V*NktxEaR&!9)8-{<0cFL;cWFlp6*}Tfu6f`{xQc|S=pstZc_HOv)JsA zpQXUQckPnsN8+!&M;oB?VAjIwV3%tzPyUs0Zu@sppEPW)+dtwLez~T>H0xyij9!iT z7n$BuppMQDcX5U2M2N~*2CfM!& zc4)xS`)F8=G$j1?ZC@Y@O4yiszOZLh>R12e**Fiwc{WADHGVz0-^?yw9+%XkWut3! zWXJVWKR$a&4;@PkjF*pTTd1{?;Rn4r|H}w&-;v!TLSO9n&M&UatTzS(!~*~N?In^V3ww*0Bx@mTCfsZXl@Q~dAV(lq1XD|U#;raK3w0rqb+~AZl-m3-5b}}?MJP8 zha?>l%$u%Xk2ZQgdS+Iu27J;pm5~<#L3s7K>L?SF^e93^5KJ|kQw_~!AqF8X#B34} z%fvvY<~2^q$ytj{DZ&(qIFrs|S)^{H`#XW9OOU{+@Wj1>VZJX#m?423klX=HWdXVY z&ctjrNW2rddG-GG{G+uBidcw54nX<<6w88)g4_vR5;(h9Oljfk|B*>~lu(2r5~+i8 z>!6h^)F^b?kh?V!v1u9qAt8Lrw-lj|L=M5(hoFgUgK&f6J6~Kak;noUMTCW~&vz6d zL?VrFRwFc)g$cqO-idhFAd#xvIbOHBGcHnu2@>fAWG@iP{B`~9*%dvoYTKhZXg5F+ zMo7ei^bpC`>#mou0zbNDwf-qAeoherBv1usRUuD;1;Nvvyff2Qx<#j@*7FrbEJ7l7 zB0Dcxgd`%A(48@3rukyz$E&Js!!HNY$0))W2|NYlQ=l(X=ql`3GXraX> zKqBQ(qZ~zcqi7?FEZQA6E2M=;i;hXRnnw`9k1F=O`0X}93K+=(i4dC?F#ewYUU)$Q zM(!t9ut?#f@W)6yOefp;=0gAk%nLxi0DSD=tFNRz&;LF<6EU6v@)^*^4C@8f#fgrM z>WJ|R(EJ66u&S@GH+@j(vyq-bF(1<(3o)5uMu5Qxu*Kg~ObO(dz(qJaLfWf-`?aW< z1{C8(da0wf`R>t9+k4RbAD(j&xd@O&U;);rSuv>Ay(ycA><)k zCxO-^uwro`qzj?Bs)U$AxU3Lvz_=jFWI;l?-U773)4=RBaKMIe-2%P;6)g@Hu2#gD zDpgE2jj~!@-C$*gVwg#0&c|vFoB#`wWR7M?f0A305|dkgzS!&V;H(5mXU)#yVe^QZz~lllmy34;ayJ*}0Wy!pbA;sNm%sJ9p*Zye&3>SV zo5<^?*0a83fIDJD*hIXK^%qQTj#je^T8_xi0r?zg;SJ{v*JqgM+*^SdsZOa8SYFF% zq^tA!m)>aMML=8xoG{~1dbE}2WzV%}>EWVq3rwb%IB}eeMJX03QjBo!pbVJq#W;*N zAIjgjwV2A0?L>#fAS{oo56MqAW-NdF(Qe*k>m9{Rit#t}UxwuoTIZk4BDuAE4F`qF zx3fqgR(Nwzup$eiS+<4aAE)mLKnpt#^o|2nHc`w8pm&0cYk^)Zi)}uZtZF>4F91?Z z1t2TH9ITN-4U#B_vvAXtNi`$Oc^v_0xzWzio3NO7^5D##FRq0na=3N4sxuy0{^s0CO2%iEcoK6(lVzV946FWnpEwnlS zz3`q=Xi6?R1l?7W6UE4^9D3pyQD}B8s)FvgU?|j07FWX+xV=zl5L?$lZ`E){Bh{o4 zy5TX~FeEIW415rcf|=}|tV#;|(}&xBdPb%qay~H52X=TpH4jod8ZY#vq9})mLOAIh zEso}-v(iGTO6S)*14nOIuTMj|l#a?Utm%A$QF2vT@-{?{@rX&qWShIrZ9V4(x1)-$ z0%R4Kix~puJRr|=aVH=(2*H&lE8G0J)DHCjw?aAop`|F(8Y%xCD?TTzn9a2f6q#AP=*+ zDEF~tO5Hyj(Rs(8_SeVuDJGZ>=AaNdgo8rqP!0;C!#F6M4(FfEpXppNwmm7(?|K&F?LNvnadnHp-^eyw$4RpqO2dzY7}Rpt{C> zpRn5N?LDM4#W6)S!Ga&(Jz5jq(2H7LBcK}r=bF?HO!|Qo?-GTsT?Ws%*QQgDe+n+b z9eGjdV8WS}J$*?3P9WL|tg(M(zZ%uo8H@gfEF1y!5ukE>s;Qa`09)3=b0ByQ7PHYU zh5S-zh${grMENpx8O95{7uCu0>HKT}g(XNc$QVm)Ie8%Ore5U$3QIPbt(w47XA|N8 z)WBzxtwo=Hp?2ob6C_p+$a0{8zn?m^5QpGQY#kp?koD=CHn!j3p5U@iKj8)1rLG?U17ncY6uR&tOlyB*2 zo6<9;UYw@>Og~cmLJ+p>tBiehU|${CS3A`!VE#EM*B9(v{ZPZvNg|cl+SxnE2#JbM m*sESQDWek6ze@=np04_AjX4xOciwxNTDOa?Tx~@biC8oNnPs$CoZa0pWG>KR}VxzT&{e0_i(FwZ@=r~#A{3V?eo8N zKKj6Z)AY#sfi~Sk5AGNTWlzSs_j*h^{zqY4qTjkNDm40t1DkgHYn3yf&AC0c>9PA9 zzw*p(^|?24O4A~@hL^i^m&Yp}+`L@#v7sq^m;HqE{)Kxl9;$AogMEgAUyX{!bKSwo zKV9xKbT_}~>KyMnkkISxVKz6ZKv5CagQ%r(c3xj)@LU# zwO)17db=LlJ9XKzW#0$XJT{&y_PgFy{oN` z&riAWLFLQ6yA}sUPgC1Xcihgs=B7J#ac;o%)uXNlT$s4?ruL4oaQDg2Px%+ubltBB z4!&Joy(Ce)#`x&Py1_0*dt1l3(IG4E8W*HKJGJTc%`dh#9*$Gi|LAi3VX~(>wz22p zzv|Clek*?8s=rkYJn8h(7md1?a=zv5U3UgPS?;he{=;bFFYc%NHViqvS5|R?l&vW( z_!!1w|K-o;)~4n-@??}d5jA5plbIp*FfH)=w0KF$jE=VPJP8MPyedA18FVl1x%42m z;i!TqQ^BoM>9owC+aW-{%Inf~a0h@pMimpz47!tU=_ZtxES*HjxZ%@?X!JBj5gBJg zA}8C3Bgf6FXkIrt$Ac$9kUvv3Q_YMe|2dt%TM1lDQih|czzOM@MdqR)gR%UvL zJPCpPS*lrDW-$NF-1ds1!rEy(2?TeFDkYW~bWeLRx%=JwAL)2v5AHCBFaatQ%%J=Df|AW{{Vua9nS{k+%{I-08R@d{d?6rx;OREuX7!(WG8!sS^J;fyEETx8 zKUqWdJ1Kkm(ix&jFbyA|odWsW3SDok(stDIghGCZV~8s=m|tqDXz_pVR3lF`;7(8_ zXqZ8FUFE&0%a473k|&|yj#b5~nL&5p+2<`w7L=c+WDH&}+ao)IeVhnSIDKN2A>VIy zYBMEcF(A(^FPsJFydYrhpLBmIDtXq%lVCU?S(QA489ZQL<>;hq->hz@WCG@AC^AA` zef@dP4V5uE`BywqLw=kpPR|VHSFiZ1UAcbC*F13mceq3NL}t)^EzaTb%76NF*|F{i zIt5M@CG)EL>-R05N`3!xgD2Ai*4(!G@(E{;#6G2DA_nA#=9`FY#0cW%0o{MPZ*BiTZ`mLv z<1xUXFa*DgC|i?Vw5T>j2SF( z&!dO;cJ5jJKc4u5TdUG)m_avLQM9P+3JiK|o~kMHppDFtis>X>QZe?UVg|{uS{aDJ zYL!VctyWni%W9QPvaMD*B*$u%OLDDNMq;#DnTW}1l}GZdR{12~YE?iAEGr~f8wW? z8}C**54Cn1oT#Yfv;xiz6{EN)0f&r=I!-5G`=}VrMGN>ssTjk>2-rg^#&WR&vKJNO zxHti+f{O86ynx?^iV0kTfHX+OL@rUlcSpsU+)M#GO~qs`SwO;~;w)~KfPJN63YQ{a zJE@q;r3%=Wz>3hslRG}hw54JQ7cv?NnTk2`95;Mmr~|yiF#ns|0?$F6KF2~7Vn%32 znuQZZLx^Fvz0huq0s!XQZ`ms9SgJ8o)mGK9(cV4KNck^WOMS0G4zN zqfo*Cmh=l@lrDfJ-9i{A5$a}AW!@Vf3`9#Rh0*0lcWk;k>C;~LvKmgqBTK??%CnR> zoU7_;J{oyu_k9Qo=7JsYV3=8Xwkv)EoKHBHNu2rIM-6StMLV0GKyD}(%Hv&NZsjH= zjttHy}mE=pAWh)BHrbz$z YNM`sa7HecO069|a^EWdHyG diff --git a/.cache/clangd/index/acados_solver_auv_model.c.5778764B3A2FDFAE.idx b/.cache/clangd/index/acados_solver_auv_model.c.5778764B3A2FDFAE.idx deleted file mode 100644 index 3b988c84ae8b5ecba6a34e2d18efb6aa21c525be..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 12412 zcmb7~cUTlh8^CXOXLn{{=>i^{BE8$df(RW{zGLRy>)7mO+DhKy?Lo7U;;H6sfkRQk?Euv{!9}~) zyQny%-&QMiXy|p5`qy2PY{tw_I=5&MV`$qg{I80)>$?pwd15+bB5&_H(mvhlq|Lrr zv-WFm-a22N+5ZGzzIu*RwsTHWv3dVfcAK~yZ_5jYE)Ii#8lIXZ0N+_J*N!$y=yp5I zCp|S{^g|=T?Oki{-L7TFjYkExjX00m`;9Z?&7bwtDPGd{)%{EN&;Rx3ZvR~&TZ;>8 zv`SnmC;z#$<$nJKcRj-8TlHg%%B*@HZB^j^acJ4$In6BgS}mTUWpaIr*2UGijy%-3}J^v2y+po3s`rL)4s!8TnqUGfN$8UAC?i6!NXSaDpfz~<) z{ei#Q^)d3-n>QybJZ}A(f{qS_*C(yZUfOG`d%AXrS;4`FXV%Qy=l8_bVC2#tvql#V zt64YZT#j4f)my*xec`bvef1zi^B+nBx@CIQFP(qHdhi*3MqUTQ{KH==Qac5j#coMU z4fkF9i`;92@#LZJUK$_X@Tcvz_~5nLnVq^{R(*F-_6l}87i zSTrcly=$Ro=xkVX{C!}bpgZrDrZ2D4yz=O^{cOyXKEGe>ANH;)a>r1?>&2VGA$x-1 zywIn9oqz9hCEqVHX;$d?ifsohQ+tl8c06`>+_gdl5x+~bi>7zRa55FKNVY_ zC=YnV{NeBXw7PZB}{fs;$qLb0I?9n}@Dv&j?y3uWs4oTErxMFsaP* z^VXj9p!){vJ|BkdtI^AU{PA*`-R#}I14|#B-jseRdCv3ng|nKw4f_)sd%1)kX+5Cx z_{`_*mcF4sL{1n#)%1?u68q#&12&AX@3qixq`_qW;kKQNzHGJ9ZT9lP)0Y=xs%_>) zU-Iq!G}_)`#k9DGGc2>8X7{}jFk|Emqls=Yg+aR)ye(T5dBa?L?5&g~f|Vwa-?}d( z)@=GHozA5vXI$z~{(fJt=Gn1sGbPtI#jkhO_?NZafBD|p$Y#|CA3n?t@BDty?MLyu zpMRdZd{1lFLhp*%r}74#ExB{LUCq*g3#~r%a<=nt@!R#ar9Yc{rbhpg9slyL^**Q1 zW)7TUzVVjP_#1Pa_AEZs!#JbDf7956`%IttHWt;!9i)M)!op>VS*`2(-c8d=cI|PX`q+;Hx~Ws&e*BQ# zN-uol2=AaW5MO=KJOhn-uT?TG@qGNJW#I*1j{Utd^zX3v>Jw*L=IXyHo|~o_)N+Ma z)gPAcPU)W4{%P{>Th2c@A3nXX{_3z@SJ^u~MlbAZA=WMVBQkw-v+C7fzUbT!xRZMZ2u?WPX?!Ma?^)_ahO4Q|5V}~)myjgYFupaJ5^(0km zEOdsK&hReZE;zw`{V;#;tZi{nHj`9+u*hKq4kLHp2u2;j_^3o3(AZCvdwa8^o6?;m zRevmW7lOOc#W$2uhcdlXY6HZ6z1b3{$Upl&p5`p6x?rJbQ72k-^i5!_5||!edzv^1 zk7%1vJa_vqx^9xH0~VSNrd#^1P_O8y+-l;0(lZ@v9(R>gE?6J}f(U5$?Lvm!wxx9l z*r+o^QuW3nYdC%lXZ7tuM2eT6j*-`DPm@%AvB)k2b|Ew0a7G=@v{7!gaq>8I_!;em zlBy>b`XKNh1j}z1($8GuVT)eqbYCK=+^|Tn2!cf`-&jT+%XCt-%=O=!n-l(Eouu-@ zLSYDmAxGb>jCyOs#G7e1s_@;E%#F)ab_GkSj#wy8)Q=O}_$D#7Nlf3bBO3Pu){f)5 zM;^gDrbS9B4=fZf3h|<~Z!%+^%yj-LG<7Hz8Uu#(s|cx#kyOrDXq@@DF22j#E+4E? zOOmnA04(4)bccu8`?;}_sxuad5~V27-Z!4HjAy!i?PbD`SZMqYH;1BDK5>!?FMaYc z#`qZ1{o931e&Oa`$809=OORAfSY$4kYwo*>QLkd!Dk{`C>trtLu`f|lwZS4G3?IVS ze!GxfybJ=icDjjnNvif(Bng5f*v5A~qh60EuGWXlSKs-3Wb31qQ%3BQRPI0+mj~|7sBrnllVnRBWAuM zNdTV&J*A^62&$l-(owXiiWaq-GCKAWRg9=jJZIe5#PX+`({%(%uH!)+j})EM@y2z$ zTHycz1PE07J^}2bIYR&$G?xjWOkoqhb>R+M`{-hwmhqsBZ%$Ojs-{9Nb;5`(+&9}Qv^F6-ZJ(^4TcBM3DK%ES{9U}=6``x6=aF)Vn zk>gp^t|>R-1z}XYIEgG0QIq|nU-wFFhNqSl7WQB}E)` zz&SDqq9ABW6snPfAqu8>GX$HVk+4Pjf9q zwKV?&rGKC~>7VT5usIqxal{-X(s4uM#*H*gJ$@%NIQ6azHu(hvE}-UQ&UY-625uPk zrlBDNHqU_ViQCT4Sd}*79}`y&V694}iu!^4n3W#W{BQG2nxR_utX9?LTb@;%wlU9= z{>cs!54B?-N}233$8H z_>trc!8Svn`YRLc$^^W7Yy8+=N*sQDdR!+hNnUKf*g={2a>wOP3T zlZ=hBcx16IS*$CWog}a2oYr#AiVtV=F4?>*NfJ193odnnD{)0hE{87V(3QA~BxfU+ zY~)Jp{hNW;Pk-kWch%9z>ln0-X-NjAkxz5dY0iq=NF!h4z%`Cqxl1@uLi1A&Jf-;s z2VT%z!=W0krPALjfK~yDg8&MmITD~qnqvWorFRa=03_3V0DuDulLH4{2pUft#4LCs z(BK+$BaY{`yiKiN3ms3Knd+caM@LyblLFz%6;guRUV}eQ+E{LW4+XP4rQ+;-Htsq;3On z!+8YGBQxR&RiCe&s(BnRKwHAUaE*kW$S)+kfazR@rbJ%FQNuu9hC7dWqcQ+gI0DsT%P@q`vab80RjceRE-{2x6t3T=tsQvhjO-|oC|SE zN&Y0*d=i|M=!t~GBjIR86#@!4jig*9`wqC zgA~3D2VI7;!j;gj61G@oWNKoDDC%-w(%MFsBkK_ zLK?J4Q`TkTYIxmjtcMWGAAH#|=AV%dr+t?*_!ZOlyg2>mOzdPkS+J8;D_LhZi*~au z6i#PBy3%fVcjBD{qokSK|0lrzxAXt+(3tb>Pp)CM|B5{&nQ=~Lx{_H2bZ9p=%l&#o zLnLDrscaWZ4}|3WxY1+&S3McWFfGV08uX-&V=NU;V2l%#_S)4WYp%2&?)TlS)x&y} z+6*oB!?O-#P#|MMd`}}Ma#A9_P;EwFGcqK$Cq9TntFe|u6s(cYLU0zgB2Kj6)Pux+ zZM+xZ$rQ78#cb2YAjzTgyGg&Qe&;pvRu*if6}*iF+h`7BK^V>9EC{DLf&~!@lQI^z zePa!a4DIx6t9gmh^Ok)VfeRPyST;GlXeD-|2m~TfE8@{%y^n@nblbcd4?b7$%oT={ zyl`nr>d+ZOE!KY5m0hK|Rv!b;b-1fgwq+>Wj<^l!5|65}Xhuf0r@Pwx=ZgvJ@jap$ z<7nk5{mONNb6x5$!9atT22g?Y$kKpQ4g!@lS0PYEa}5GD3X>x;zI9_Mj@)|ll=czo z>oeJ7Nyh$)QtF9^e$vuEXi{kwh)w8kl=6JFuxdhSvz|HC$IE+n(WF&+z?-$tCRQRv&~e#PcP*gR+l+ zp2{ki1luM-Z{qk8K6SX>h5g7e1h2!;ybQW2*)#xI2cWJbq9p7WPGP7waTE#Hc6!7j znG}0)2?m+(LT)6rN%)Z2;V}A9Sw%CESte?)sQwAEdV;zTuaR&xSO<$;NqFEDO>~G7 zdy?}mc0bW6TI@|$U7U+W=NPdMF&!R{*fv)5CL0W#bHuiBqBrp_37^<);>B*{#4h2Y zrAva?mt=nl*8^K8ie6-!_o}FyP2PpG;W&i;y>TQRV0UdS3KMtL{aBEa<8>qw$JS%k z<1ssyn6k=|U)WQ3`*s00@aQq`Ky0Rwt9VpJ^J^Zxrn!bkH8lUtqrYkX$fJ)6ukZgR z%+xSv`*&kb|FR-@==3X5xJXpOT9&ZhWM~aMx`bWLD1RtpTbHpTXr58V22%p%tV=mN zToEW|rzKo(8$MO^W(55@mQx;bGlaq z^YQu(wL51#WF?d`7^e)zo%GaDyK@TQ+?7zt;hl1LccOL)*Y2F^1$QM>jzFCw^jE-i$Pxk)0A+5y&Y5 zxhtVnfNTory?2~AEl!+AT$v;ktjdN50Zsb)R{zI}Y%KmaHOT1@q$^c4>($#o9XBpc z*4M~IoN*CnP86k)Z*T@TxTZp`MlR)yN;$RCUcnhv(EN%sdPQ?Br(esNDDrE7!5X?y z90CkNXpRQP(SW*t0x(RV3(q@&(M~|!KMyp^Q;v%o`86)~o&R}(A(q8w!1>zqI}(GKeS4%r6rVC^%#v394EExTaFqpD zSw|8y2epF_UYXu(?|17n$&T2xjSpT(S5KbT^>=^2bRWLs60}5*xV{D#zR@nIM+S7h z{fZ}Bf$;&nSzm0j*p`S%^77`(?G-w~v^l}_P~s(vHP2$5$mkl%73#H|12H2mKk(++ zyps|?b%J@F;6&7li|kM*4Qj|BBlxX$y!_=~An-)9jxi!6vbPwpHW zHvIAFga6sk_trkWmHC)Ij5j$k5XC@=ETa{g z=BEAU$&#%GP9Tks;mA-l@Rn?P(t$;0KU+?iU+HBYJ3qqh@d%=2az;~Ko9~Ei7So# z1%fXSkvPHW8G#!JDUNUxftxhnLg1FdEtX&Y^~v^2={Qb)ZSiYUb@tkjXBD2i4jgmT z$b~#8SX!Fq&fo5JPjk0OD!hA%Gn;rwAa0=G_9=P4j*M?5Fvl z01nceCV(`Wj|$)@%_jtKg62~KI7Ksl!z_#DvjR9vbDjY5Xf6;y0nL{LaEaz30Tj`E zT>#fX|5DNCCx7c@Pg*o0(h-(!Wh-b z>Gk=?@Ud+-W4xQOA$eVr_cQAKjDy0djB%>cermAYwnW$6$GJ&wA@UDyt(+6HW z@!dL0(lpMxjjt1sR7ci8oC6zQAs`L790@@IbS8_~(zIU2U|^fm_(`%q1N<3%WpzEq zz+;RN$%43C$%0T;U*SLw1akT$3pSL^z(+x!M7<83gV$R zzQI67ICIU&w8E@zng2QO|Iu&SPaoDC&cx~~6x<7io+Ke$(A)H;z;D)>@6Hp$WQ%9> zgMQD(3vD`To{l`pDQ|q*6hK%GDEj$W+>r7@dF*^EFW|c->Q^#DP?;T3#mVPJ@})jst`Zuy9mHVU`Z5;(;5JU zG+zSX63zDjxJPq6K=pun{ox}(A8Gys&?lO=@@Okh$#3J)Hk!kD6sGXbu3Ik6uDNsx z&ma$)Uk`A{G=W zPxJ2Vi_6+UqlC8>o>$RGvcgF6O=Nfz zITLn_UE9?)sc$i!Ngyuzn@sH{5+s){LOKX zz9tW+-Ha=7poZkpdOz)nsf<&q(%v;GT=TAX94>m}Mvt@!;j(vN+Cg&?i;`$gW>GTD zdswiCZADUxM&8SUy)>t?D3z8!$fAQZA7;T}T0V^hX$l{!Rl|WJIy}eb-iw;=MYg24 zGt_Hp%o6>HFR*zILY;%qh`4m;bFDs{T44PKH$*bVk&Fegf`cY}zhRW=d+d_stU)>J zptxWqYhKB=SNJ05b&(sVxa2kDc@4?L1@Qx6WOz#{;i1`eCgUr;Kj<{PU}3$6aU{Wm z3rd`IFz2Xr^jWa_EI256HWK!ZgxKSw(J}b$a zQQysI09i_K!3_|FB(8}hEGLkd^(y+Ql%uHL#@ssJ7r=~;!L+&B%v{sLqGc;fD;q~^TN8WL{{T7Dx3vHO diff --git a/.cache/clangd/index/acados_solver_auv_model.h.F9AC8187D4C1016F.idx b/.cache/clangd/index/acados_solver_auv_model.h.F9AC8187D4C1016F.idx deleted file mode 100644 index b6757e3c58bd722f1275da28396a9ce646c3f43a..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 5168 zcmYjT2Ut|c6W@hHAM!Y92izebRisFjc07vILlNmsBm!rNpixvL2qM_9Km-(&&QTP> zC?ZDGXvD6l31Ept1&k#se~A<|ivPXi?epHpw;%JHxtZDB*_mIkz{kf#4FJzX!G>hv z+QlpY01`pU+BM5h$pYZ72*8H2=J0_0NA^lR56{Q5)BSdLng$f^+AMRL17&pocs}p$ zvup8l_mYB#j}<>X;ny0*jQZ!MI%HXtz8`Wd8}n+3yimF9t{LPHdql~$$vhs6OfdTX zid?b1Ro`CUEg8*TiVIhqnW|*b3ODD3hHcdeV0GDR4a@6w-}4c+Rn7A{KX9;r#Y*+z zOM1Ioj6(0_R2}}V^v2V@roZF_ax{BS9O5PF)hKLgI-faC_WD^FnAy4P#?$6P7t4ID?2 zA^gQZ$5UNZtMcSm7{;WAWCUbwFjo%Fy|iP;y(ho5-;DO`+D2%xjO6FZA61o~>D%hs z_d1<$6y#K|n`CP3ZFynY)pnHwU1hv+xrEmNS*NNFji}#EkFMizBc5MLIac&~%rs^G z?zRU}Hpk!DHxKl5jy(yVF*F=P49+>Cy#Gt(*F|w!8x$O#JZY{?cDU}Z-T!d-zn!6S z%dgoTY`9q0yDs0>z4FLgryU8g<7?in@@;qSbeY^?Ajm2IiI-F%tE&H`|FX;p&6c+r z_Z7y1pV=)kO?&CFJ$Iqbrk7e3aTlA6KYdOj=ddq3#zUS_q z;We#uKQ63v(mVLN(6+kapA-I{{ysfJMDBF#J6#=C&R(Q((}+}Bo^(CbKz50r?+n4=>;6I0;sKam1#x)w>O7HPH zo^z-DL(aTMSJIX`2RuKgZO}EIWjrr|A81$j>|S>0za>7u z>0H^hcG1-tp9XjO2b~s1-D~@;#b==BeBG%&?MIW_%yKk@_m2m2t7CFIj_vW^Zu6K= zrjDcpm6!~<-;edI(K@=vJT6P%_CdclhHKE>nVDh9vD#11ZqsqUv)w*L3DrB(8hW;d&@w zv9Y?r!)Yh~P%!+W88L0F-owa1d13FeT`776ZC82I6kqvl%*^qo<8su#{RP%q@*RskD|w%u8{v@pu#Iz5*VXbE&ix|kh>2C19`R_nBxd3xexIR zZkih?sdwM5;&@Kup-fv4&CYW8pj!%$NWGRRXysPypl=SrZ=0o)=Ev{hgNy z8@M75i57`Yk_CHsWoN@{+tX@F0yFHP0CEeU2jD6bGLjt9Ob=}F8W5V1U^+IbhWgde zRT5;r9ZGZGr0(Q{E7`Ur(84B}+)Vp#S-cwmMJA?l!39dwk02ys9m zVpt2jU+jHZP67>Vl3|q*2;@l;%(6lTYl1{hypiMz66j!qDyUZlT_ipNY>|oS^d#y< zo?I0hc%n3;)uMGI3r=(0PadZXpj| zrrJURZS0{Q8q`B~iHCVMXeUF*&#Hc{oOFT&hS;PW^2*^vbp|^j6S*t?Yq( zzWfFD$fRXscfru=hj&O|j!p6)HxKdwcj_cr$(cNCs+^XdcD{=Z+|Y)F7D6A%f;T)h zGo<^F#>e|4Fv1?PAvarG>nidSsqz~=jefr@ctHXKY*GXDYoMDXRmpqRXN$g=o0D3F#5At;oViy$bHmiIxhPg*X9pjcWifuKZME`y*>maC;mg^y?mzEnKX!s^?w7J4M=;5g#LsHSKXcgQn zNU9ccYM}vcgF>5Wqb3~MIh(n0aok%FGAv;xmnq>#*08jjR#6!j-Z+9UtwggKMVf~UU(=Q(_I zLa;Gm%w)59AaZMx?F$cdQc=8ebZM#d8_Ms~c*y z(=^WoBG{g=#~mKvNsVCfn8wzoN)>;m^7yd`HY3cK!@g`#|M5&`ni%#Y{P3^>yFT?4 zY!1I1kKmcaOlBW_rJ~D|>W(EK*n+TNoZ#t?&WN7+^eJsCzmfbPWpWa-_aS^3?>Hv} zi+KQT3|{U3Q@-lminR##C48APJ@fn1G~F&@1AYvmGZ9}j%Va)_Bp5*so@t}kL`gY`dx%Jh1ki9qI%>>2I5ZTi#gto^|57~<3{=mbz6xs6$K4ahdH8U@0$(>TvcoSKPX1E`bl#nGf!wiF@mavwX;bBfv zOIb_JFfqWljRH!jBOMMO3xa9FNGtFJKzWF{BCaT07Ra8ahqOJe2c)1$A+5j{g%mUu zq!qYrp$K9Yh$HM&)Xd!`G(C!755j{vfcadvB!?+)k0E#lF@t$;_}GzmnkLfTaXlaf zO%7?rqp;dttAD9zLyl$>vzZs#T(hfqC_b?r!48B2)BbSTA~!UFq}}{@!h^$_XL%0w z;22hn5^j?uN`q4m^>ALe4AAdsc1i!fWrrv`&~Qoziswi;GQA9!9kQpXCbp-ECQ{G@ zlU9VLRQt%@7`=-exezYQ!Qh%eu$Xe<(+xJ5dN@#xeGkFTgfnxx)^bCQ-9Dei@El@} zD(xEJM~{NioRrq^Ym281?hyN_T+sNhK>emoozG{CzknR{}H{l^yr$gLCM#N>wG z8gfhXSlTUDoulx0Yvv1N??$*W+2I;Su$bB6y+yK}8xa5o<%OmZ}KK zc!QHx3LaR7f|IH!f_8#{s3_XeBDEqQs02hPbw=%)_x=r?v6;z!v+w)%zsK9(TM#mP zw$2ivnK2>h39%_F`3NBq`6s6&#b4kd6k&l-dO>4ELr(KY_Al+c8!6{)zw&_hLxuR#>bE`F#!Jdn^E(@yM`830K zcAunZ$>RuhOxsr{~Ea>&IXMp5kD6{sj-1|v>e=T z>lxXzi$Y3bzro@+*oPCS&~wvww7&kzK?;39A_lB5;JKVYWJhrC0cXd{M<_IwMB1>T z4To?75o`W#o$9Wv$Og$t#NWkVYHVN~lLnqzA9|rFrjVG}^;lYu137^T&IM!o;{G-} zL7`D3(t@Qe*q;-KH1X-+(1egO3b~R4h{c0=3MUYGUVm_J`vSS1Le3;|4vWu`LuUmd;or|2yM9Y}1BIMO z{pmfs8Y$#LB3H2M74mjjfym@rNo%w9hT9Z!Cy{b2E+-SQ0+C~L zDW8&#bwf5tK_USz0a9ZF8%9a7tCP13qk`x9KP(XAzXYtU{wDpbU-LvPVe+sb4-4UJ zC5#pew5C3wf-#E-w=_P}iRn%(fvW+?kcn7cU<&|>g(C|H_gut838EZeQV|m?h#l5b z3)I7U8i8h5FPA6C=Ae58#n?u++gi05@W=tbqZL5rZWR3_y#IS){nbcd1V>{bcykx-s2t>I0Re9*ZQ@0~U$Ff(Qn@Fzm>Z$2}J@C0JO39RUm= zk&I_Cgz-b(inH5lw^xxp`v`q(;Bo+#B+oJld4Q1^??;drEPpWK^vKLZ#iFc6GD9{N zWSdSEh$JK4&ml41PanDZ@6dx8_)Hh3yG*A@C|tktotd1pDw%vU5Rc)}g=hArye{UA lL*rl=LaP#D){_qga-eB!#y1yOSXv3KDH}VHtv$~X{R8qrn^FJ( diff --git a/.cache/clangd/index/auv_model_impl_dae_fun_jac_x_xdot_u.c.6132B1FDE79246FD.idx b/.cache/clangd/index/auv_model_impl_dae_fun_jac_x_xdot_u.c.6132B1FDE79246FD.idx deleted file mode 100644 index 4d1476f88c492a1240ca1131d36dfd3086071ca0..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 2716 zcmZ{le^iWF7{~8S^WGZX(bi1NWLER8AL$ofXHnSNj7F)5C0S0PiIqZS6}9JB+w?xlCoBoO*tJN5(4Hu@cjkQ`Gk@4Qr{_7(`~BW~-+SNte3~1i z(d-(4kXjp*5+Ac}sR$t?BY#Qj5?8hg5LzWhDCIzNUTDspadgJJOl9&>mtE8BW1ch) zn`y6p(&D{sN6qj>k(Zh-_w{V_t*Q*uuWwZ|-eoTXZ+JKatSox{<6gJTdzrk_NxGC( zF0I8GIg>8ee%qStO(^QnjGO_+=E*G*wsg+HHLY2L^xsV#u(%6)Vq!9 z2cKux=FJ|QJ0?^uZ8{mVq-y`_Yo{lzFr4lf_j*NeYLPa;Tim-yoZfd+)}nNYmg*jL zRezJEFy3p}7F_OkpxQQ7u5UZ?>Z0f35SJaDk$2M)Qfnl3X^FGgp5pnvy4;RHv9~yS z(Ul9g?dDAHQS=tUPf2ItQp!D1lZT{1RcrZ$kv_107bDFDvP!n|Aci~Fg z9XYCP8?{Hi9jNlg4wuT8cWN@?V_H`!{?%lh&RhCi8u`#ite-d{i`GV9)5k`*6cGqkK6Awx4YhYaafVGA%2`lqi8gQSh^`iHxrSI##L`JAI*El#V5y8|*7zQB z^`emnv8ZYF5OYALYfF9;(pM)8yENWQf<_Q|hI|HN4j@L8`T%TpSiXzf<|6MF2k;6cmbbafmcJWm9KT%uvmgziDjmICSwjT!@-ie z-}goN#?k0wBKK3y{nT_m!3_8>daEjHKP303HPx&tYTBxp3T1j~aNbAS!&ANegVJi4igMq`K^KnFOP z1IVKrD_%a>H}xuwh7x%)J$Z;ZfP6i*u=}{Cx|K#_iCl{nwb-9euzvn8>N{TZ+uCU4 zLM%G0(&0cpfu%Sp>dpO~FNtL^u^1_Zk(?-(z>;^i_jOo8)Sonx6U%1oyctjD6IkNQ zAAS*fX!1Waawe9@fsrzEfLC1}x4bDU>(yfk8cF0pc_3pBAScEwPdZ9&IrOW*`G2~U z2waQ8y3m_M;2IMI=tv@P?Fa(&ArU^@eYTEV*YN{n?0F$RFLZz%%GhT@{LH%EV}tkD zSm_NmxWT&45#bz>mEI=8ZI&9kS|sGUnO|1K9>daOSP6U*v3e}kTh|p>T7i|&bzgr$b5Ei~uwcf#U)Q6T+F6%J|&f0wr4{#6_0F1X7R@9424{ z@PX{g1P6F5Vk@z<(sG#HKCU(Czv=49#C~Ew32*`MA#~)h!4H?Q$0&S^vWHK5V(6y) zqq59q^34rGY_PnRk}aihDfRviz`UdbOt68sk+I1*+3NnL3vjx?s@^WZ+pX&Y0WPqv ziv_sYx~>=CdP@xyBWJ@Q43-6U5rG322tX_%e3(;2;2;Gf;3UXFna~7}Wo$boZl@gK z^8lsD*DxUpY$CQ0OA9S`EHNf-sYy#pj7~Bgl7N*;*_#W8HkAm*qwz2bq3C#R9Qn_H Z9HdgAjfldwb_2u%X^FjT&_@DC^dA7nOpX8m diff --git a/.cache/clangd/index/auv_model_impl_dae_fun_jac_x_xdot_u_z.c.ED4BE4E791391EA0.idx b/.cache/clangd/index/auv_model_impl_dae_fun_jac_x_xdot_u_z.c.ED4BE4E791391EA0.idx deleted file mode 100644 index 661e76b4e4254023002383b8857427fc6bcaba19..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 2796 zcmZ`(2~bm46nzQ*KNf#P_yH0Ok|+vDars)swRBXFMJbMeP*GHHX~7CuE7lF!CIUJj zI&LVmLP0hK1_RTwDMnG;s-sjuT&hl8u+qvhRr`|vuSsWYW^(V`f6jaN{`cQGi+sGi z9$6qXC(>tgOw`5|6hcT!{_z{*)^&>zx?zsc=A&%~7NmDfU{ZV2tiLNfF5TjC$##p& zET+KwzK8DvhxXR6g_~*`>W}Spcb5jsx42Nw!N)=lL~0iX-41%&o}svwA=`KF$hpcC z)5Mnfap$MsJuP!|XmcoEF}~ByDYcWzDs(&19jp$kyfJF|{w|LJ*QEtNAFtou(fP7l zeyiE_VSY{TN`?05!OOeOU3ucIk(uoqCoUKtdodu?f5!KJ=p6X#{$7v659<4pYVT(cOm$6S&XpB}KaMn=JlC(tt~fkl zxm};EB|c|$*1ndk%sC<1IZH=QxVxZgUDu&1n{l&Vzi3~yETnGaQR{!cG|w6QdiUBr zd-I*L?3|;OukOFDX}z57|Ln)u^16YQef<~NR-X}PA01xQ zyJPm+v-@}74E*Bp9tVo??aF-mX6BmFUHN`@O73@E$!YQ~$lH-m$GC+}R+Sfd4_x@$ zeDGM7k8SG~h}W~%aS7T@?G*Ry?XQin!uw%^($lv#=@)FBB3_K&afdw7zk~ z@AeFGCzK-WT!fbh2`E<-k>{0p14;=RO(;GxAC*2plUeczx$AD8uk?uP%gg77<;pB>O={0Bb0DixJn7de4Ddv^B81D z;6YkGNP7zjb{Kvse~snU&IJiXIpoxT{#d0n7 z5fV_UYh!C2cL%5$G=WeWu)G1!5fV_&tgVWE`g%(NgGLd`JKFvonT1b4$+TNibv{H} z!Jy%UGJ~06rw{NpPIyEAt;54kNstqP=ga1+^a0?TNioXE{ne)>$eF-?GC!3*06Z!( zE92qmK#c^AA+WE^SEUaCkF6|t)N1;tmO%~#PQ>=2l_?OL(C^Dcwrgp!U`={QhGKq)yf(|t|K z>kbAf2&Ip>x6sN(g z1}mUBXqc?$1q|y!xdh;43jpYqY^)P2Jf^r(%$8yWtOMPW^}J|-2x=t&FI50Qqhw>9 zK;bdP)nT^IF!5K7Nt-pZZ%z~|xGFKO5}U&@E4W56ZZy;&YSM^TH0%gcCXIADhQ}0l z2D4`jjX=_3YhKC%Ps$b3xR|ztwjgcN=Hr4U0Iz0f1TrQ7uVDZ{yaeEt3&5wrs_WG; zdk=^dT!k1{7~T=IOjdkc!KT){az6~8fD^1>bSalA!l@#oI$MOZjq5xS&NHr0iSQ}o zx>1B14K?@zCFcD=Xi9M>F?-T*H7KqGvn9rL4Q6W$2M_*1zK_m4;1DRT9JA$yW#A(w zV%|SKbSuMbnSn-e6_~9s)cw<(HObnPdUBSY=AII;I6~Yj`_wC(?+wCLn?*5r8)&0Kka|@NxekA6aKU zz?X6t#Q1{P=(D~O<5$M@ArpMa#0YOT!Og~XCWSL8BfNvc9flgN4-tmW)C_H()8@}< zYj{Dh7&10*Za^fi7QI4O#K*0Q*G(8js68qhFs17 diff --git a/.cache/clangd/index/auv_model_impl_dae_fun_jac_x_xdot_z.c.59CA37EE71B4BA5D.idx b/.cache/clangd/index/auv_model_impl_dae_fun_jac_x_xdot_z.c.59CA37EE71B4BA5D.idx deleted file mode 100644 index c81bc5f734ed7c400f75ecc2e979fc2111f73279..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 2718 zcmZ`(3s_8P82;zXoWI){(M*~dLq-yoID6HEPEs56|;MAI1hANN&5+yf`<%GPE%@BFi!&A@F>S^kLq&b4nDh zHSRBPm}GlJcYb)B_$+t7+(#_)C|Ng%2pplu_kJbs?qO=1!M?<5V{5azU`VNzd5q&^c$!tgCObJO0w8 zUzAgAQh`P9wQOWtg|+UYo3K{VQY(IXury^rTjNT{nVBUaA5g*GeT67ybyQfk5TVuO zI@{}!Hz@}7M~KbD@=WZ@Cvp#rzqc9pBi}YZ!3=UDl-so8Hto$PpzO}Q7Mk6+%8@|| zLYZzeU1|)-aG%KUb)j{1lJ``12FVFrPpj(b06uX9gjD*5MRf(90Srx zhELF;wPBOGPhJllgSrsP6V#6CmGb0Q0nNeb+jL!K2dZ-t1qb= zi^Zrnp-huaQyBy7V@Si3_QMvm6*I_zz?HPTk__PzblBzG;#<+!d`FC22xY8ntjZXm z!{Ii|lbPyee~3{J0{hAQRK@^su)b&MpAA_J43ZJ}KCQS&tRmK1vJm>2M&OB1uBt|_6JWe)FWefoOc)Z@++LZo|L7fR)O)IL&04~9R=5}#7qWpN`^roVFGaa1ptIg08X_4fLsZ{X%zqvDFHq&O$$3tlKcWGTSnnBssrp< z%08v=Q}cSK5bqS4;irW7lzE*f!igd?yh?(0NE3O(>wr!f>bvP zqf*laN;ZeWIi`;ZU!SbtEC5!3_Q^pTgb$CU>(PVMF^#9t9J!wHM+4szF)6QE69~*INh+yaPndg zPkt~qpmk9F^{6RJ%PwBnzh&e|sjp3j6IO>0zLw!|!0G6&oE_Gpb&K5}s`o99YDsv1QX7@pC_K7-%#_(1i^q5VQfixa#&h|V z1^F}Y=iRH2O)ZSmoJ89#{;G{(%_6f`xmmqc7W`U}BV1bcN4ChiDqv;WPv5Qcxu$DU z|2=bfe)##AL#a%G$MTx!D%+l_gM}?`-FDj)UB9@*E6zJ%P)FbRF_{x*9*Bu}`zGek zr(4HPYn|J&^<>-Yr+?!dIbaI)A)Lzpuy@Gb2eBmw zo>ugfSuD{scpOW+)v(4ZHS|g1%F!`<_MDvW!!;G}b6seWOHsKqL$oVG9>1Szk?0qF zSSKlaxv^qXf4ao>a_4*(>Xa}y-+EQ$zTTI?{1k=AwXR8Ye^2;}ib==b$ z7yn|4%WAjQZ9D1e%=qv#2|Ys@&$i$YGky-mCRm`~-97uO_do zJ)wJ6i8Asktml+{KjKnR>4T0+_Yd`meAagOYClo)KpUM!Beptw3hn_ze;W19Z zB3A|=TruF4rY;(z-?I3hQBl>em_DxjtFE6$9Sk&G?GyE((1i*s4)R$ z)6V*cM!WOQV&p_9L9!r~AwZLg$kL^|>o>WwXfS~vVfiEMV@yz?G;Li|q#{_uA~~V- zU{wzeHYT97IS)O$Eim1OMKVHpg%z*JUGNDgl8&S4_SSWiSn}G4dx!1b;mO7Xl$w^X zciv|m!&o$sP@1s336C=-pk!HExr|qL%wUm%P-gX;B{c+in>y8B5RGa^ZXe z%EtK9YgW7r+{Pjmp{%7I|<&}6S~KDlpL1&f9d%2rynl@2r}pxiPm zwv!BUye&o!gyJvrR~Z5{nec7-0zdzrIu_Xy_zkvugMEz&Dx6z=&3cqgas!Lp38kEt zm(!Ds2`KL>v|l?9ywJ=dYeH$q@^*4vd;-dlmdefHYezm5BWFU1mPM-!0nX8B#f=lj zc6W=>Fan3k!c>L;FiMV}n{u8g67(!k{Ev!-6t7-mU(9J~iI!G^1~HdIOL9zWP&Z}C ztJ=6t%4sp9#TGEhpmNfdmpHr@)Fl8fYyd#f6vZp~<9UwIk|VSdrWlk>_Vcoa{Rx-W zq?b>)?M^AV(?aI7Py|O&a^*s%+_VPalM1}*p#sRA0G}2)0eFSO4v;nhcwGYky~!=< z)eK)txn?YC#+I-Tgx$xI7qpQl<>oQ-20#rdS0Z3a1ijYx1kAnO>kSmMf$D{KQcP#> z^$j6&L--K~yC6;VUST@h0%pNSl9VeIFr~elGz*yK-s>!i$?Dyti(J!A~^z5`!lE|-?%n#K-u`Lrb8v<6Ee^>{ymjbKCs__X~HfcGBQ@maX4 z_EgfAY=M$HE@Y0I1_j)ROr+k5fRDK%T2f@1NN^=W(_4~>mPbqSOeY3+BK<$@Nd({x z2`c>Sp)Oy0b=?JWFpWqfhTpBkCUbXzbbU1$^im;HYC5lyJB*pb_~YP%laYhzZ4Am{ zZa*#AZ#oz_8tKp584$t82*8^d0N`E(__Sw{j(WoaKIV#PNwMk1z`F=dZ(Jr?AuTC1 z(TIm7&enq|$+0Q=ITdhE8=40`{wJhHFba)=Z3x9C#U+yeRy05&p@f)b`k3_<^<%{r KQuF=-OY|S9;~VDy diff --git a/.cache/clangd/index/auv_model_model.h.60CCFC5FA93A2F4B.idx b/.cache/clangd/index/auv_model_model.h.60CCFC5FA93A2F4B.idx deleted file mode 100644 index 1bf1d161709426d37f7086f5e4cec6f64a66f075..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 2004 zcmYk6drZ?;6vuDnhgy3@C?JKQEfi@*z=CC#w$4IJeT4ClF+ki9wbTcqw$7F@Go8XR z6)+>dnB8=~nQm-294I&e&3Gt`5yq%nROXypR0x)^NMx7m@1$+gKhCG$+~2vs^Eq9y zOt0^lf{=FIGV5l;_FM)bVdt{!^&y60`YAaS2K5wd9 z^t>mzdg{?zr9=AKAa3f*hW&d)<)7B`%YXlE$hsp!lzeI+JSi@FN$FA_i+mq{_`S-m zpSR7n9Q5kd?$6xV@$inZy=?29oPlZU^rzx|&P5G%fBr|6W9UKfyS3Y{pWqJFTVhTg zDG9yo=x!G5@JPAwYj(fm-&KJXr!QJL$5K|!%)VKp8@$%zCL2#$(fH^E4xF!5tTOwD zl<1`Ic>Yr0z4nZ1Z)=6Oq+F}^`ollF8gq&ea!G? zfSvF7`;lE`>kh=}(U#x={?44Y*X}bORdU})XFGk0K0cIqx#8q?h5IEB#x7ZWMIdQG z=EBWu1a1h4LSB6EP5FswZAZZXe!TaeYYlvT80!D!lSi~%v=L~e+Pao6Ng{ET^ z009M~n1lS?ITCup!WgeLHt(Amm?|`p5l1mm;fOnV(yK@GUTHh9d~1IuM$_R0El;a* zHo1JuToo-ndIbGS1y&4iH<0S>oc0Tb7pL!0a= zx-eR$ztV@12L$4nI2Rw@F#fP<>9Ut3@EQcvjG7D!nmpQ~y&I9<-;dEW2NbnP zY=WuLEC!zmrXI5xEGC%R%VKbsVCpQ3!B~Q+sVoLh38sFs80;jNTFGK?l3?m0i@`*K zsevp8{|Khuu^6l)nA*l-aE)N<7>mVO_fB6Nur+d~iG@>zATk2+NSX@Ara?D?sb(w& z$q1%$u^7}Mm@36$5Q<NM1w_GtWYX6E>)@`!&zw}HF6jb7Hia+c$X?olLeK@ke@rzyV)9qx7TJkHJ9|m`PdsbQ z1YttpZ$b?rtZylP{#B($h(b zZAU?N5ISE>OI6?8+zj8SY*T!Q!V+FeO)5?{vUGRoBQoQy;x*?Ol zRr^@Uk7_+qzPDQtim5nyiOs+^4IJ z%nOA`4|c~moLAXiqP(zY`6XZ~ygl?ghTVmMdf~9|@uF#Sz_EAx@CQ~&+JGv0VLRXl zE}`GLVog85a@ofllcyH;%bk9dE=!@xSEaq^V;T&ztZLs5&L;!WeKqgcqkJ^xI4(Vi z0e<^7$z7h7$X=b7$g`r z7&90=7$q1t7&{m=7&;g|7(N&)7%Uhr7%vzo7(*B`7(o~{7&#a?7%dnn7(f_57$O)% Y7)BUJ7)Tf#7#$Q978@B!7a9Q}0O;q%b^rhX diff --git a/.cache/clangd/index/main_auv_model.c.FC541DFFAD75A3A0.idx b/.cache/clangd/index/main_auv_model.c.FC541DFFAD75A3A0.idx deleted file mode 100644 index 01481f6f9239137c7f13b9cba7727d0693207af0..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 1648 zcmY+Ddr%Ws6vp?;1r}Hc7$70LfN{WrJPH9KBve6?AhuNjTbKdl8C$5NHds^;{lg$2 zBah(?2@C^41{^H%&{m*U$4+$s5gD|#)e*EB9Z?4atEj!yxkOi)9Un(RB!XEu{_9-*#06`QY?|^!EXHxf+pY53I)}s6t$@6eu%Zg3}ZCi-Mz?6u3SdjohAgDrk{SWx8=aYM)c?SK8k26=t5K zOFf7qJT+{$^I&Pz>UKKVM&r@U`}bNt<38SgR#bm=&_<}VsIBM@6PwPdcZ;J!WBnJKzM9^k$$Ig6Z0eu*ql$!< zj6M9Q*zhw+9?uRxX^Vcnk-6uoyPqsAWkLp;SwRj*+)|n&#ih~{3EGn$d}-$Hko<~* z&|Fdb&ZmTnYh%&h)`>%kf#7~l`LVG?@M?*)p93vQ-=*n;b! z1&h}f{QWGvJ--{h^+<5GFX+mGChmE;Z|!@# z9Myst-6xYtLr=6y*W2AzvsvdKTvMwD=R{YVik~^$5Z%0-*Lw7Q^cxmG`BUG#ksetV zXRBN`0N4Ta!(T`rDJ|&eEJGbc@FT+T zcAYZwJ6+nbDl~i{6fzilF3g0KwaFV$_7;aN0*n*j4(Y?nM;e5M0m2xJmCyqDR{ z2uq!$ff&o|j#Y1%COZ)p+lfOlem77(b0qzXQwaA0>t3)H8@CG|+#H!TJ%fxeGK|9b zk;|WRvtH~KEOC~IFn-ZH#D3pm)`xH>fSq6sHk=E__ug+D>qn0yau)IN*Ob>6?f^gg zco<;`l-S~Ir)AmQLle4DG{9t-%*U9aQ(%f-31ivY{!*MCN3|mPk^15Jo4)9pk}Xf8 zErY3G4z9!tjQPCo!p2(&H-XhnhR($8b$I#<~l@F5~=e0K1KKA0YdT^UneDoIw*x z`JU93XUtKi=X|pgIIrA3Tn;!O2q#0x7(%Wg)DMav^k;Y k1DC^dBGR)n_N>&fW3yM8KvOew($d0;vS!)a*n-u>zc_0G4M zDCgy|uzk{Dp(2q@HoN7Fu4zc!^!;!5_%81f)qwTPtjUYbtMZ@OdiNIh{$_0M;b30o zX?m>Fpuld=TcBqNos-v7sAFs)R0&6P_L8sV*ZO7Hf*{ibD;oOvfX zVVn8BD?)E(Z{%M&X=!f1$+`unLX>vRJ9XXuS%z<&Kl={R;(rUSJvnfR^VN!txK2+qd3Gae!F0`Sm)`h3VteqY+B`JL z^rF?4!|JX@3q!WA{=w0$d-=d7e!+0f+He2oMCIpU&8mx@4<* zrTL|J)7|`GF;`}V3wh}VGZa_mCdmV1<8Sz@>W54D+jtmc7#Jk=SoAo+1W*P7ic-^x z1(_HaJiqgVvaoBaG0I4GGO~0s>cBK!IB~zGYssoGpoEo*l`~9Ozs|&c-m=O!$HBfp)={M)!cKjd+bTV8W%VC%0XY zT5u1j+KR=B4<=kTp>rDRwi!==!qzO-TrlC&c2)a+$xVC%G{jcM))FRsU{<*eY#it6)4=o$lJoG29xMMbVBoUb@MNvgpHhy6-?Ml;LzNyGt2+6 z$w-=Un8Dp5BU!*x01r_a$tsR2B)M9iS|qtfMwUiKxN~JB+ZZ|87~!fJic)hDL20D8 zD7iQo#E=FN5FiR;FffQCm=Xx46oM%RXEI9K6|2nf6gd^dCM7K%hU@LV5MJ!^qsHg~z6s<_rs`a~x-$@RAuWLR3c>P1x;;j6#lYP!UcTeXe!hgbq zz7Z6PI>SG2M&_JUltQ5h$p4%tZ!KFaUfbnTc=tx(=xep@ zsvD-UvEesfyANY8d_KR(?NVdL_@*hYTZ7Jo;)jmmTPqg3_6E7$^q6()@vV6kCs(_> zxnFL)n{n;4-Y*Y@IGcajH@$90&b@|#GvQh69Z$wh{rOk7pE|$f593VmYE<8U;{Iy| zacTMWrkUlL%YJ%N?eS=lO|&T0N7R!Xxa!ADVs^V54ftX2>)OX-i- zD;;}E?S_weFsZz!ga38yx3g=}`kPBUmru#sv;H1!$A)C+7rg(iRu*Rh#6uD{LYUiX1=pt8==$YnAF* zdNr+?QJHXPjs>$c$W-OpJ^q+?)`8;3-D4vnAI`Ns{cVNSxWcWiPngk(i8mLm2`N9j z-?>jVhn0P2TfwJyW!sJ*1Dh|mnbtRSd{ZMh*ih70-t>)QXZM+htgO8~i*_vE$a%LQ zzEpLStPjxE$mGgk+*j5_2t}QWn!ne%NVF=gQ8! z?>D=po=%(LJvOoUlY2JV7QD$XdmfDmt^ZMe*d=Fa#}^3^@u_DWs0AVMDUk`v_Jc!e zo6Vgz_AlU`D1F#@JlWkQcXJJ7ClsLzU=6RWe#>)n%8fs?%XYqPsr?nNtBBJD@j zTZq^BF}Kz~KB#nCeM+>Z+kEzP^UoS*Jzlvl;IIun{ow0!Nw<^yy3-tYHk3PdTzX)}gC^WXGDrcJznDn6v8I(cPCj7e0Bm^nBUv^#&Dh-lfT_lCJRB$O2UQhW`y1Nb&K_gH@T<*cD}B$yQ2Qfv*i z0}#{3RBiT{;5mYWLdEUy0d;?4*veM>8b7;@jciI1`R#Nx-AFrtx&2&QZtpJ`dgX5(=%WozKTJ1B zeZfP*$(5TjOgY*Ce48>mb5hR6p?bfd8fzB_mfUQS*Tu-=7>p7SB0G^N?S_|$;rX5w zB#A`|h{lNAAySKiAqo*q)RWj=EQUxT9s`k5>;aLFI0T|lF`*}ky#&%Ckw_pd5~T#v zBJq(xS|p(oNQ=~73Tcr_q>vV=QtGOoPwFA{hbTZAr6-xa4ALT#$RI5;r3_k&%tr=k zkp;*gEpmG~q(v@~Lt5lYIiyAIBZn-Khsq%>3VQ{lMIli@S`5D+n14sGHwYv@OT$`ddbVfSbHOBR!hzuc zkt4$qA}5BEjzC62nS(MBo3N$ZazVmj6Yg|(E^t~l;Y;`Bf?QSAc4cht{;7PbfJkH} zvT_#{WWV!F%LHNsd7r}mKO#^q0in@~lBoGV?J}hb34RDa0;GXGJlB$CtaNh@83(9H zw^RWkM#Vf}qJWT~5{RUz6e1ZagGi3bAyS|Uhq zNQk1)D2S5KB#4sHWQbDG6di#*BsW5nu|Z})wFHC`(KyrSv<9IeTWcfu5L;`bo-{V< zNn@j)G&br7&Em+1Gok0mPVMd-pS02 z%pfv1GKWaS6AcAVhqQw)w$PJ=FM&wPmqH}t%OH~TZ;pQXYHPJvPhn^n z2N=U9!mY!_dWuG)Az1>N08t_<(G+~LvcUH$(~84N(GiLv)ZEqJ!KJC15v13D^x$0(L``fZcFXL>KMinH3Hwn+RY9 zK=y|)LxuqR*~CQFL`W9TjMvG|_-3Bs8OO+I0{a2%5!^SyVjl_ zM`mdbkHZI^4cay%rnX(vKOqjA!v!BAbAS~#N}mI;Pq8eBj!>~WxJ-SHV9Ve#Rf}Nt zFt4Oxt8`^+VNMM@yYMrQ5AZ71^t7A^QH2*xNO*I-EkOnl5@FmhOW;*PB7vKrBjBD0 zbx`+(45GEWFIswxJ>f$ZBXbjTOJL#ne{KKn!p+M*A#H6#l-yMiv~}aT#x3I0@gFvuhW!u{$47XTmxBw&`yUk zLv=gNCSqB!kSvZFr;~j<@8Rm*t@5-F2m`Q12!Bxg56%WfOXxeVx=oux9bC}BW|QC5 z9G5_2H~4HaY5T5Mqs?YcBU1+7pC1TJXSk&I<4I+CwD%nhr~~MrI`F>D19jk3XrRqv zHNN;SISGi#sDv}v`-TzZvy7Cw`Hu#|KPR&v&w^(SRsoKLe^HrbPEAi~EnoTp`&e^$ zLt*NnYfH%q=%eVi;F0>w9y`rv&4cA+HsF|ZtiZe>dz5jNrJVc^us-ywfzp9F zGFh4C;JgWXBk~!*iF$C_KK*mc)p#GCKv;b>E!XwYl09?*;e+{@ zfpkFn4;Ent5(|0!!6G4w!lEEb!jd3L#*!gQ!BTWoDSW$n@40LH$!e{Zs8wJpArUSK zSAnU7L?V`G1*qdz)1^HpXI7CB$uwsQfn}Ss42qu`udOBvhUQcVEMse1XFxpx9S>BS zzOUwd3_W|yv*}^(tf{phg)!h7?k{fqRXQWf$LZy9GCO7r$*=&~(aHGLedojD8jXYm zW`gOOF(Dzc5ZQv%DI~-~v7MeILJ34tp%fySPzI4)D2GTPROkpSfUNMEyFlPqx3`w& zo?Y$J`T=S1fLr--H;4RIL`ayKnCbdwArZ}u)^(;eKXlKpcplwOI<*Jt0d+w^XppW8 zddV&C%~`##{R4^)xVHRDE-5&cJ^qD#_7xg=zXM%HkoP-Kt|0Gs$O8qiyd_T3<|o~J zg^WfP%Ho52g53M`%rmxQuYH6^<=P1w^A334zlM@$r4bwlQ2GV`^|=@HL}X(Fm-hD; zK;T67roH^F;YGu)4|u5E*KclWJMqHNWMqK9>BaG!76oU zFjC%!U{7KGfrd5e&SbQ4yJ{4@+j|890vm-{gK0=|z#JiR!`vYX#DXBo!m=UC#pXh^ zl8SwyTO6sigo>5ITt~%@!F+;>HNt$EinYVM48fM`j3M?F)r5bS4|SJAN5y3HZ=ucu+fFz=#a6*@ltZoIr#w&k~f{AiN-n_ZjF$i@9+tLS0u z0oC*%;~-r%PP*vz?Y3gp?;l~*0tX`#x8`RU(8s3+`6r7<7Qg-oo6%W$?ybV+wzp)S zvL9}53F@GLa1c5S)05*UM{7Mf4R_LglKgdQdg>%H(Syy>{Q#+Z17s`R8=!^sSNBF- z71`VC0&#_{gJryHdkz(x*}=g|r#)wOdQK!c8ogV0a|L%I=z$zl&7FR}P0#sXZ8e}Y@#mbe7> z#BFg0+#0vTN8@8~8SaYP;7VMD%W)Tc96la*#GP;vF2?O~FI#m69W3a- diff --git a/.cache/clangd/index/test_PID.cpp.575590D7897A814B.idx b/.cache/clangd/index/test_PID.cpp.575590D7897A814B.idx deleted file mode 100644 index 0472c685d287e2225c328b78f9ade7d1b82cd907..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 8678 zcmd^@d0Z1m8^@P0B!NV-z=A78L^%VIE71@_00|J0BjAB9`dTcYRph9k1@XcIZ`7hf z1<%&v!BJ5w;DMszQI9HEsftysMbTEg@#xFDiSMuzTmR_i!(Wxphk@@tJF~mbJo9^= znc-ofp*QChgpno$mr%lc`#IVV|Ff6O2Ja%kat1o_`Yo~eIL66-@&?)|v*v_gP=_emnHMAtsf-tzS;#zJRQ67j!jb604Gp0%q3b?F)!Z zj;xLhEs(A})%D};>X);nLc9Al0pWKpWvaHWJe>75qq*`tm=LkP7OMhDC*Ag3Yq=y- zyS0j)SL}^Y3?K@V*oE=3>u$kIe#>*6T!C+YJ78-~VyeOGT^;K~i&|aJ{4()!v1#eE z6(hHoFAnJM=2pM|No@G=+@^7zmMyqLbE<7-)7@&z28FlBqH`|Wb~c)4ycw}JCw-1{ z!T6%J)%E?-`ux}CbkP&mBf;GI@{+{^xYO~t^xA#G`iSPmN{;~}kKpqA3AfjLrLw4L z%5W+jKX$$0nkv`&vx|hU`d>4+yD-*$*NqMJjw7=3Oy#R8Rv)~6Z}96-YuA>C-xSG+ zM9$WyKdUOv78f7%IJWd@*3|}OUg~Z88&A_y{2CfZZINfhRnICOt=N~}y1n=1jgsv2 zlMinA%s$q4e~D)8%zntO9NpSz*=#l1@VT2|g_IqB_s~scM^fzp!SV;+xbOX`zdGNf|CPWIcY43d zw!79^c6{fSlsD54I!e;nRRe#ORG0Og*SLDY)mq27;f`|(14Hs=M~Xp7$B{p(U)3%j z+$-UFTll0i<6P<=@2S0cD4D%x!RuO$#I3HWJvsBu_N`a#KUdlJ$rD;PeDe7X-)(2> z%TC{Heh{!{bQWP=(|mUF?~w~LW>waxeUDb&+Sv8@^SND*!#L8Gq?8v2mvmxgU5kdA z;%7eL=JxfOG~Muo=^fKs>Y{^Hoe>6gl9Q(2tg4EcV=`yre4?oDJ(*>qoswsj75{SK ztZUahTq{h)8#A-yQ@6+ zb(m)s!`Q7)Eh@j<`u7QK^J$n4J7qe1)tyqy!7&RCh%+@u~ zJZZ|L>J;VglqqW{J-D%jv6ZfYgSS+VO>5hinZjXd=)v7Fcebv9y=U`=UK#H9Xeoyk zNt^#N_`%EbmxYE{erd%TG4gL-ZS&2u7DvlZ7kt1iuaXlNoiA9l7H7rMuV!j&YNl)8 ztIfYuD$vBAE8(!h>1T&wVQgIkU(Ie$i|WZO*^Uo9n*c#S$wEf#=CESu-hY{rUzMp| zjsbBMAMn}|MY<)o?am%6mL~dd+)`@^rpJqlYd&BO8`ao*(@ptdD^_^K-@N&fi=zEQ zUDh4rV`J%8O9Dw;T?4;+OI@Dr`JrR4f1tOu-K_S;vY+m7SYdQ;Hk)m#YhZ6|TGGUf z&#W~DQAtWIr2uTSkLOxs{azggH z@`po57zrs~t}jAD3lwDjG`vgh%L%8$}!L8i{JNY zI?9DqjAcwW!6&iI^c<+~PWZv}P1?l+my@|34LTNITH99Jn+0?}hwcPApF`S#`>tQW zOCZe&N(4j#c$uKYK#Y(l@D#v@5|kJ4LP!E62uXnyAvur>;1>~;0w@qt0wqHJz#pL? z5QLBls1OPPAqc5~8lflNh~4eghQh zH$b6&0~G2vp!;m-DWK;!K%ssE6zaFp*}KQ_*DZEn8R!KbOOIfdq7^}_hfpC!%kt(q z8+E#{g%pp^vxeD7NI7sFtY8=lDG#oP6%0Hf<<0d*NXC^}!Qc{7L%2h_38p^WM3?fQ zc=0Kby~qU)&ZoroVuU=!o?h@#e9BAgg^)xd@nLLIZ>fwSxxL&4z6PICI4WEjQaUOT z@^|-_Ft#A?AQ?j{vC4~~5NU{8EkhxWTzm6GP-4?|j+ zh7|E!ZS$5e6(aOhXs*Sx4L+U2HFPr6mTRzpGuNY5d10>k>DbtRFmBX| z-TAlDo{y&IQd|DQDTbx3lZmdRh2g=ccw8P2#txt2bNL8400$1EJC48+At&I3kTY;b zNQ8?ln0~In6|sqNF=F!o9vnuKJaJD8hP-euguH<_a;F5BAT}v3MMwr@97ctFfDb}) zTyDYGe1R`wQ{W23<_G)`n-W(dHhYuPD9f{b>&^^q`#R zKH=BQ6X_5{LmNf}9r0*n>c=T83TeI4)`)O{0lnl5g`dvgXmV!h5zH#|P3Ys^=^$Jd z#y$y8`;0HnGI}S7u+O{4!Au17G7-?rL_jYS!Tdx&6a8|Q9;K273Go+8=J$}d_wQZ1 zY(iD@d;>nkwr4xTl^wNXP+WPT>7pL`GR@}ah3Ch#;csfUGckpnkz8h%6{m<&lYkMdl z*4V{8i;nnX-A7#x9r!VM*!Vb6(Z4X}V5vZ_IjzVrYYvqh*^F@C&g7tN93dqlM7B_S zg_M{OBP1u}wlE(GDFvZGNJ%IWiXmfjc2qRdWri^@Hi!F(PC?1Tj1{|@dZ^4; zT$|?iQj~VZh_5T!;J5Jgib1|!t-;q*LB5_uOmE{PH8{(fKk6W>4qca35!lw;LxBK~ z+}{h%w@oE~vM;b(dngcKZNxUwJF%z3f@3d$)jJ&)9CrcSGSFec87zQBCmj}?FagY! zbXagA1Tc}nLnXRQNA><)4GRaDGT{*GQq`0W`a99gak$IR`=Qr{$NykUKK{axvP}iN z7aw&LPKXHm#C<@)n~%CGot89p!V+54!c-53UmYkC8Niw%&)K!#Sl^}$1Gq(iyE+Um z1o#(&!QXuW#((z(7{T2a;40|802e~Ww1fc`(uYl=|>JAhmav;C@CcS5|&mrebl|J I4fV!Z diff --git a/.cache/clangd/index/test_VC.cpp.74BA115DB14CB17C.idx b/.cache/clangd/index/test_VC.cpp.74BA115DB14CB17C.idx deleted file mode 100644 index 5c01a6f3a96edbe4ed0c17507e5203e90fa63659..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 4538 zcmZ8j2V7HE7f&!C;~|esUPwZKAVVOCMji=!?}rQlfz|=ESa5(=MyqwSI8Yo2D&Rl_ zx9p{&fPx~Uj)GPhS`kq}st9hsoA$d8N`Aj2_kZ>|_ndogfTx>VsV0Tu9Oao5AH8}p zjY6R?!8dVrLTn@ecQh!Jq}+-H@hcwVXghJT+%`6(yDvUfnzyUgL~$%j=2b(ltR=Em z{dV=#(hHRg_Svz^y_>Vyr|1f`d!06ZcPRPq)}*HTk9Twi+Cm2yDuxTZ-@p0m#$d*F z$u;GUJBot|z6Z0tju?=I((>@Q!uSF~Jd%F8t zV&+Jr+rwAz`kRCwynkR;$i~axbo?@wx+$Q(tFy{HJ|jPXVV?H+g3)vJFGso^UljHZ z2?y(Jccy&4waqDeEM=4Zg`wL%=$OZ;IYMKjS%R&93~y}bnQbY{wP;^5w8cN->1^J{ zNV5!7_n`Kv0&Azt*PAxa+*4!x>{w+scX8=4(c#v}AMWyR*#`VtDX2>`6)ShOEH&Pv zQ1G;&V{n_WzVpqILi6Uc7CpN?&7*uYee<$HA2`0dQgGGkr}3;uT@J089Ig1DHs{DR z!qDf!^1ju}tv8nxhlJ0N{N5fUD%jDyIi0`IyWo(8_3J-NXO7uZRuz?w^Nikm321Ry45!% znvOSC{9S#xR^jHzomK;JaIxcQ<3p*s-FAD=mtMO5B)sOF@wZ`S%5VE@K6ft99NN0b z-^*9jHs|FoZTCA%XNJhSf4$+5Y!a0I@yzYu;byg8Jv4r7!Cx*2tMn5*$ooXIp!Bq# z<~JT}5G6e-{0{6O_7$5qfc1HCI{rP}W#B$_N`{$IZD`|u4Y7%*dYxY-guZ0qUl*n~v-1=x0}UK2pA2 zDeX3oj_JU9zSl`zxk^HD7f(OG#`W`4FPnY`*YxfnAH_45I`Rw{N0pmj3@#~QQ+EGS z`>^mp4N*}Wm%D!9?VBaF`Mb5&(z#7KtDmVCrSt@+hCI0tA0`A zqCiWXKk{_&ZC}c1#$R0fu46AQC;qnYXp8C3>p~hV96XvfoUm?We|jIO>9BLtmd96{ z-JVN+PGPGQK6Bc;iyydL!nSI9eJa3$yU2SeIy#Oz;wtK~*c;K;o#w26?e@vagm-4c zzCF^YuP5uf8dJ|!w-^6ZVWPjZe43PD=c=t(mY|OBN4(;X- zS)XF;`z}X3>~n4xt90xq#y1qt^0`vGXx8<%nRPjB+0OhZpA!zvvykRQ+xj~CqOKi> zHNuKbZoRF#k9<)S@*apNiOKOx^1x~1*j3WLYpu8tp)90O2z#2n@;^U-^Uvpmn#jjEuip9W1s4YEU$A8L0fI=(}xISrXt)c>tVCpbUp5) zHI|xCd4x<^Mu%j5L&Fa}>fg@-i3ywpUeGo|NF)+lD73IGMs6{ zlKOm%BA%kV?8>I3YP-9$r|}3+)RO^sfk$|w-jk3o>Pw<0#mi3$hXO3+iCE(EpaXz{ z@X$%PK)Nbj8|v~1hAM+Z{SVpd4}+7Wz^{lQ;z1!R{r=FZC0Ff$Noy@@0hw|6 zL#g9$dQS2NGLo4?a%$Dg!|O!8E^;zYrVGgutcJFeYUK{dLMNdMv|Z`$R+8On>J4(@ z(zpyrMhF|44Fl??Y=7m*B}Tm!m;@VTbD$8D;GsMc`M*s$5V4XK41yQv2rQwH)^LAp zQAhSdkhoAqs1C_ILSJ296C(erQNN4}2g2o*oyqZmWKh|ed=e#93GO|sIUf$@$V$)3 z00tMP_t)<-L(U>V)>YFbXY}{m-+IQJSrI*D>PBOi9Z>(?ARc(am*GD^BM1?o6fhmO zrl;*~y~s3b0;rXUCL-s%Brl>#k>iyt*F8tY&y&4|5J5^oq_%1MoTo2mvr<9E96wGd zOom7B{rDt`ewlU8GH>pBxh)Ueb0UuUO7lre{WlevqZPI5K}nt2&bqJ-*w!NkxxkKppSrVs_HxL%5nMt6b;>AG}7NJa>TE`vl2rHW(j zYeII)huj8tpC|$T4IVoQ{X)gJPbxK@PsPuY*uz~ix_+cj?un(!BF}4t(4pzjq2GvI zof}m1a(7Ls#TxIm*- zSJ#&&SzIaoz?5dYTl8Rt&)$_e@=oA#44@^@30yvjT3(cMPY-wHfDD+b%t;ww1QrQZ zUR&}Pf7=w7E4PGj$V&=XLii-gIa!+*%~+i~C5i;@eC#;0=&<}i5X&rJny{Ow-2XfR z91p~MKJs=f1mWQ6RD1_nX@m$=3M5Yo z4dJ9o&}Zm#CnQZ2O%NCYxG@za2#gmYob{ZIVJWr;jg+r^8dnD*a6w#{kQ{z)wR`Ek z`*k2!@hq_gv~2s zLU+|}`;yK%4BS&<5-1lnf@CH^H>TT7Aciqx-UMQrF`Xt5%Zw$RKwKYg5Jc zeIR%WZZ)PBlf3+y1WSuWqV$ig{Q)Lz&*XdWB6f#+590&FGr0}n1p>(O7cRW~8~4Y5 z7rp4<5HNz%a1|KWY>Kpe?tGQuDGl@}BQm$^XyFKu7SIG_Q@b|jOusVf@&=G6zg5h4 zsw(1jedAM-t00CNr?u+8lgCXLb70)}t25W4Z(`m9m;Ut4i7^Yz7<0kqfJ1&BCdI5VbIb&D#2hev Q%$3FF@VH)D+SBy@2c>}iN&o-= diff --git a/.cache/clangd/index/test_VC.hpp.C3EBE494A18C2184.idx b/.cache/clangd/index/test_VC.hpp.C3EBE494A18C2184.idx deleted file mode 100644 index 27b853ec84ddf9e4b5f8341c878f2c48ec8ba3d0..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 1946 zcmYjS2~bm46n*(1`AL2jNLYS82_$F|N0C@9W2u0MtlCL&K@nv@q3%VABdeo>3!(^$ zRqHNJDjF<%z0QT5+Uk-J(T`z!brSt)fz`()YjI;-ATVFZbMg?)~?jmrRZd4=?5; z6q+2Bu_R@Ak|RP$fj{QuX$xw22*nISC}Z#Wsb6h+WR%@E2^I^_{&Xy|vZwKiOI+s<8o5YS6f~Qb}wGoGTOEhoU`&xPOd3YzRc^$h8zCrvqJUO zDBk^xqYgmA>L zTBY{k48Z4xxbr$ItJX>gIfm(R^kmKeEVZrC#@X8PB!n8ndYPW#48ULR4oWRO>0Tfq zTrsTCYWz3@aAd^iJ*&DJ3nhdK!v>i_%^84iJpWl)*70DEgz&_$PNq|H2H+#234POk z4JwlnZuG}VkS+ojH-Iw@6XUK_lIPM{hUdAyW1Ey~(n!Wcy z$>A;u;f}j!0vQu$ARncrd}G!+AVl9O8diRK3Pn&P?N0~8z$sGcuMCE>t;jz(gZFLcD{MXZQC6&9%8!CX*a5Sqkhz5yaH>)pjA9~rvtcZ&@V-kj9 zJfI}J8;)k>2GO82>}F*KZcrF@v+@G>6hYYeB`LB(2a41QbW%_fMd}55DNs?QL12)A z2w}xctP+7K-+t8h;?t5Hn29#fqo6x{JO~y3f3au6OUy0y692D=9!tpOU7ou>U^g?084Jm2DT|YwF(G6j zph4!ex#l%&DN%ntyhsMN_MMonCPb>M zebK`GDpVvs$`Z;|qVcszNqtB6IWu#fXXd=W-}`(2`+5Dn7A$DdhamSDuMJ7DsgXDc zg2?Dkk(v_EMuD>-1Z^m+3i`SBg(Ioyc=n~+Z+(-J`4f>9t!4fTUsii+C(NsyjG;fk zRqh*kVQ779?9QDeS@t`izqvF`N!@T*bd^PJn-(V1EtXKVs@|{$Z2wgXfuB))!9Q zzj2EDMo>u13M6^%J@m8e>epwTc}t3wbq5%((f6AI8=h3xxi=}p_ta(@z1&eB1RLts z;U1joOHMAzD(~FoR6~03bkuQAexpoyxv$07+b`R5UDiZVWZ49(xu&;THpCtOHblt{ zs>@6^_3+~A={_x}XN_(PUR`+QNCNRqW|ttq0QFa8!~s63RcH8l=Q8C$jiuPiHa@O@GK9O zG+2ty@chRp!}*(Y_5GJc^FKK)v)OW>C79$CITYS_EaBY8_3=IL+_;8Ox7^p1?upKG z<*%B)yq(T&-lI4F`mtARcj?6lT*iy)*5nZpgBn&sYwi$eB`LpN^_V`sD(?wtyiLSz zS@%%x-1Vd?`ud=-$^KDA?b5%?x(ciCt++WR#V5tX=Ci7MtxXbqJP4sjVhDORjgtkR z`46{@)|Cugh(0C@U00q}R9Kr(m9w+84l?~ql$b3=V;ri?Y^|4fO|tVi)<@(X@a4ciPvYg zxoi)}^AEWnJC=Bf5`Nmv#QnpE&gzkRC^h^swZA5+^~MX1-A=O)-y?7OJby%T>GmVV zZWLVL_hvn!`;=_?JxwooZg-e0G>$l-xY}(u+L<`#rDIRoXZhNUSVEru3Z99-C^6ZWxhWt`7f!2kL1Mxu6>E7TOx@J#V%e~ zxG?JU-G-Z~8AWA-zg5KCeD1hnwDI1J*C)wH|C)Kqm2sg~yTh8Ez8D=|vEM1Z9>37u zhbW3XYTuVWIx!w6P`3T2XLmM8*N-4Le-%f2Xb||#*iL<8sxuBD-drc-}LX-Sb?~e} zBON0aq^G0(kKi1w{yGzG9Qelqi^O6E@<==;q=(ajmIj~}pp@8EieNkh9X__U!a;Tq zH;sU>4cI2CgNV#Prb4D(JAE#XLnJK%!q8(7vH1RMX|qqOj5pRIBTPI~2ctjw`fS|% z-Ij$TBMdx47o*7tN0+0AIUpkfU4aG?>xxy#+pzhJLt2M98WWXE6=4Pl2+Nt}h8egh zKXiOyR+A;_+}Xg{7^7GD_iI17ze9kAYDu?*F*@OH&W3BQtu{I^!ZG5^z-SoZvbdIN zBp?Y?NyCVUBjT&~BxDIyjigpmE`}~pYl1)QYPZ1=5V`@~L?d2ZmJudGqvx}1)JSe1 zSGlsEIS_m`>Mz%+`jQwF%z(|0a|O5ed3tNXh!M>Q#=7qFI5%f&eRu#$%V)|(n91~o zJ*EbYe@CK5ESMJN7%iFiWw`HmX2LWwB7kDvR&C*T0Fi9Z5f?RH^l-V zBWyC8qCUc=u^Ac&W~e|lERlv%OcZOxBAy{u6_1RFEySv#lMx9~qM?+)5;L_2iAbW6pOh%o zP)cE`N}1XaNIRTY(1lKtIo(`E@6KLrg$Wvw|)2N7CB9~(J>W(C>j$L_lM4N!%A-wwfMW_%J zk9k2aD;iZb`6;o9ky9QODbWh2sYS@t->*jtF?oBy?_yJ{5*HX85(EWe8xRzvUIwd| z(Md6h3KVkIEt;;UZ$Ka#8j*}=n2=2=R5}f?ffbMgd$16QfE4h7H4p%2-~i?UA#ee1 zz!A&^?!W`s09!B%*a0!{0<(c9Z~}9HE06&RSODe&9`FV}z!xkAmS84>30tt1n3?Nx Gq5lJWZb&u& diff --git a/.cache/clangd/index/utilities.hpp.77C0A5FDF681DAA0.idx b/.cache/clangd/index/utilities.hpp.77C0A5FDF681DAA0.idx deleted file mode 100644 index c8067cfb154433005caca6247586ce791ace32f1..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 2878 zcmY*b3s_TS82&jMTiXt2jB~b~amLui0UIbn4vSF28!%LSP*W~h5}-tw!9={^Ez3(i z(p^J*EK1BwQ4^6u(bNJFH9!-e(jsVyC5h!FP5aLopTj*5?{@z8|GxA7-~WB@Lt65L z2{8(U#^xlK7UsHUixEN${wsDBt^GsD)(Qmgk{A62cml&bFXxXs;E()Y0J~I$n4E0HYdbc-Hqy) z?d{_%t}wUnktxS3sXK|ggX-IT?^w7y`USQxrZq4L6Sq|v7uJOv^bhvWDhquwJ@Nbg z!b2Ha*Us`09dgEZBm4O-DuXR+J5Uw4XL4WEIeki`%1FHLD0%w}+5N|}N4)GPY1c71 zW%HQdo}K1$#x}KPPn=Cvy%tCKC3HR=-SElGiio_HuXE?Q>eH90k6!)tyD$6yxZgrt zRv%s{+PZghuDkKv@2AGe1 zoL`X@(<)xES|@Gj-Tv=^c@;DEt=L$$Hbr%dTtZ&b9hmWLa!|&l@HhJ>X69$_n6>}J zM}EV`938T)V2txb@4;gQtf(|Ut>@h{jkgoRz8;+z;%IP8UVUriQthO(S*4K|A1EaU zpURwc&k|R+yKRFuqNZZkd%lj9zhte}9Zg#iw%|dtsla-3{lV(Y%RNnH#!0(G`L4GX zB-W?aT}Cf%O^x|aXYV?=XkmQ*%@193*B&0TzRvmA((9$oEA`1K8LuSOh_*CE(!;8v zix(AUmx~Z;?kuSB#$p->6{^uCK5 z>fM@I0scgw%o9Lc1FV5SSJb&n&o~&CB6K)8V`QuX0PUpC?s#&+1*4R}%}c8!Dun>Z zlgfwoJssZtQ&-gdP=A8M@tm9s5CG!y(z-;azkS_K5jIS#jcSVkKu0$O)SN4SEgVOK zxDYyo1A3&TXJ%dY@zE4v#YZC*q)Gsw|5;O>{B*%xOxtfM$L5-;}63k|@H$ z(+b940H8~fE~ynQzs;tIAYPhU9Vh_M*UZvSvg#(x*AS1%5N#HlRRRFrbd9(gx>dHw zLz~1V70mdH z%}4g5B$9jj0ns*zO@(B>1fo>K;jTF zCxZ}@HgESV&GFFSByC|VI;aUr+d^#-4|{~R$JrC$LLj$~XU5^h zFykV*Fx#+<)upl1VGyvorQCAJ0CSIv!OuI$+@n1-vwz(WQ3OfbrFJE}7eCqTQSbz& zfa}XU0@N4GfG;F=0l_qP z#4UsnsH{(*U>R^aC_i52tpb)IhdA(&>(oDTW%@_1O#jH0ftm0p$4=r^$VG6&LDRbP zJBA*)^6>{gxD|$&B0&o{4IhhbW>9QAmjtop2HI`5hJA`-L!v{*!sUSt;fvrs2rdHb zhaU^Hn5d~87Un|Dw3q}f}4=L<5`^b8aP$|#n5Ozq(jl}PEeX4-~qzEC?nv#c!$ zp1}P0`eWwb%2&dT3`^;cz^y6K8#_l diff --git a/.cache/clangd/index/velocity_controller.cpp.DFC34CF5F86A4B55.idx b/.cache/clangd/index/velocity_controller.cpp.DFC34CF5F86A4B55.idx deleted file mode 100644 index 4f24fba6820e43a14936740914ff82fcd4ba0df0..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 16350 zcmds;XINCn7x4Ft3ofwq1@6Maf^?(`3W7=zR79mJS3#xN5F3I=Fb+SEI2@ESSV@f`}SLqe5cIJFEZM8S~+Nzr7#yc@pn$&)hO+&N*}D%or6qY*?GW zKrnPl=%U$EGZKXYfk1-)OV5~}auUNI1_HsNZH=*$=ic|xyEOb>d-0yTv$K4+JH7e} zHC}L0i#a~7dy4d@(0#r&*B2)I8ouyZ_sKa00bjY=z<#GAR>gl`kmvv8X?Ur7@SS|| z3?-*ypW;2cecJ#hvIeRmyY=?X{e1d zbDbD}C}>cL_VWtwKVLPsWV$bTa$&fYLXmmw*p&>gC8l{NCUK)qb=$?=+rm%ZRY()QR$!G6+M2^=gi#9%UiXl9}S#Yxif^_CMQEys=e&yf&Iya9epo( zpWkyc;=%BnH|0K8)%-ldBjj`RisBA!5*@Kjda%JtV`z}G=dzTgnoHzQ+*5uisHv~E zU8&h;y=Y;o`O&ZE9H_s3%Re&YYeG&hQ-TG%^>Ia=ko_;H;X+WIj!%HvcMDEcOR-`>wY3m6d^BUEg0zDXz-3@}2FYH+8Yip))&U zrinA7;}RwcoGRi{r={<|S2p1BU_nWFmiMxFy9LEZjL&4mh$@Q4Zftkcq6kUZn<%MAI<1lKm4NU zUhKX&Ys8h*%}$GNG`^4dd5v-X&Yhx#_fvx=?_3&Z^Vg_@KL?uq>Hlr)>(grU@?UB= z?>zWL;+Tq5n~CkOcB;9yjI_Tvb#X+SS(nc=_1D+#75I%2{tIavvvSPbG*QBQq;$V<4EwrA*yv2)LDSkTnoHEoKc{PuL4JvMn8@O!A zptENFqu>0}Z}a2)?+0qD%V$^3==Dv=dBwIf$-ll2TX+4!wEgR1dUYJ`OdlF->k~88V?!V30cj~JXC4aT&*$14{_+$Frg{Zd*(ihK8N&$f&Tfe6Pu z{Kszp33G$hmkjmLT-=}(>6Ri{?Bwvt@uY!4cKLn(Igjm(Q3P%v)s@O(jf6(}0!|

0dk?=GU z{qHY)`^P`?r%p?M?JW?THCuFFW08)USi;LCa$}-COeS(ELuPVEhP>rr42_m2QL^Q1 zO)1$+?ae5;^>MRi$i~f48H#aBA=KQg|I$a|sMbryhwFq}knxi75jqhJ zMT#S3L>4o~!2aR4gVe-g-nO@`HhGemx9@GwP^LzvF1ba_w}ZSLY{_7V`3{hGu-pmq zPL`j7{3*-NK>m#7S0I1I@@tU4X88@s-?01^1&qn#DF6>SA7+ES*KW;EbHL zD1{5dwP_ckETbGLm4JE)=o0%B^W_MYBQ4@ksR`vX$EeTJ6v_Ap;P3#vNzY~cLvVP= zauc2X1vR zfH;hpuLt*f@TS}Vt_{$iuyfqsT9E~5T1d>Zb|%LCK@Xr8dEfoEXA@`V`sjd+zlfw4 zkvs90<6hOi$vdiaLChB-zd|&e__|SO&Zr|j0zE9K0a*=Lk~bC&ed~3jMqeL)Y5|fK zAWITm#C$E1)*?$1&cytWNctnPB%XWt@-^=TdsFm5!p~Eir%uL*GzZiFg}zzh9Y<5- zwWrA-VZq0cM+82$_xtbn{8pkS=G}$vhSU(kG{dCiv0^@6J>G~?s(LCzcR_R)Oo(=h zYC9HP>M!B25*2ZZ1bR~o=(T_m>G|^UGZqeW8N(^faWzQS1AVg#a$yWILKwjqWVBVZ z6Mf_^P~XKGWEnzb)F3U=%|_cS8EuLrd>%5+LncJI5(TCK{9Q zF95w@c?Ci%kT(6?EeLHv+Qgb9{BDGHBW*IK621zdDx^&YU&1#c)QGf+t-RfE;ZXkG zd*+{M_tMU*6>YA*V2us#8i=leG103S4|7yLN+5bPxpwZKG4UI0gxJ{t6@Y5^#C$G7 zxyXPt6!X;xRU-qUP%+6JAC1E ziQlPr4%pV~z_AX;od3<>%7Ell4{nqjz@Y(LC^vz96F5_D2D@hHOSuK?Td4d;*F;A& zvX>){GuuIJJ9Uj|QTKaYJhjTHx0nxD5U_|?BBpEzSPhi3re@8cq__==dyKRUq-7wZ z6F?cblz}_tqo8*b49VA7+uQ$hTYE-dJTO(@R|P|fK#ct{Fb{`SDdazOPa*%QdkX*Kp2GjQr|>`S8DFmV-galWvvSs9@)r8(AXYGNB zk6p2K6@a7w%*ZgEz1DlsPK_6?>JmQDDzOjwS;8mvNwTJt+$Wi#GZhHvPUuB z1)?sHk+?EA?EIq16TlrilcmJcl0*Z1({^yHWG1rkVc}6ktztgHIl_lhq(h_^rQ0C7 z4HEJKW%@9p5XF%AF|%d)qN78udLS7e;u7LcZo@Pgv%KE5_G==Q_=x3zY5e>fj(gp9dMNWxXL1NcEj_u?)#~C zJh8)MBh73&QHuGsNOLXa$Y-)DSMyFU>@tlgun`R-0sO2}mrd_}*#oeyQnXT3Z+tj3 zB#W?V~0B5SPdrO8g-%QBuYwN*~h*${*tr<&S9t<&SZR^2fMD`D5BZ z`D5BZ`D5BZ*la>t+4I$N2V&pILnsd!(Z1CnRD+D@^mrVh}{#a zzR7cu>4*9OF??lSkyeP7khq{VD=9t7r7RHN9mmBvkh|tx+fr_-%>^lg8nfue@rhX= zi}^?3`UnOQRf(%_`Uc`+D88u)T$;d>e4?0d2IpqzN4W)DTBzK9h_88UXJD|7jK2cr zSHOupQHIko`89B+d>!Q1S-y##AMC08HkjXL@4pKcccCwpKLE`KbfKYKR%$+ExeKhj zz@4^#4mQuhgK{@$bpuDa2ef)veg|6bSbh&$?a)@vyoram!InC|+F=C)=#<1mkJI zPTNk8%sZH@>}&}2m;Qj)8s5XP>E)pc$U}R8`fWZs0Qu;U>X+enF4;9URgF+C!WK!F zk!Of`SLCWeDN-1zMT|?#$5_RvObyonK)oEaX$n|@PzBN;OA}5_c6_1H)e$)Ch3JPE z(}#rEhd5IjVK{8GopiLXJN;rCOv?9Xk7f){G4O)3y*O)#L1Cdo!(@LEE$?9 znkl9Vou@uelhBzBBgcz=GMb=)4QD$@_o{3Z2N`uY34Ng3Y_}{*aT;+ZiV`bC69A8c7-|XK)eAxSwy}zgo8=VDRCE)9}!-rDiez z9?*Mg>|gqxCx zgc)qZ_$u5!XwHqfVB3>L-ULb(*=rQk}r49v^u{cDq!wv20Boq<*KC768)w&b|u8MfR(f zBE~D9-7PEb$;9`hSft3wJDe|iK6v6`l!a%=95gBijUzrP!_GWv9p%%hvJdYZRt>|+(6^scle|1qw+!EL4jG?AR>X?d7d#T&yQ{WR z6Ayl0xoQIfuZmVi`<2gxj4BnGFNj~JKECK!PSG3TYtA9 zlx~9PCg_unoL>@Z9G(47E)MZg#!;$bftXL^63vOe#C)1@8Y9aF<7`@32y%J(=XY-+ zR)3~z|M5P1T?VI5UxUMkVuWH8)k!nxHG_lmkdyP|-!Bj08s=^XFhe%tFF>PD+@$;IcyxQ+ET=cv~dhWNn8@M{7vSP8JcfApJ|zHoX$`V80P@P zxnP`2`M1RyjiWZNU#}~})+@RUQu0+YY`vnZEVqHEjpZ94y20`-5Zz+=4v6ls+zz64 zmODVy!Ez^vI$3@SqNglB1JN^O7>#L-{J3-V*B@ zuD>m9i!vn6RWREGb)Z)VQu3hd?@#&7sxm12O!G?OBG|KKNTyoMnEvl11<~G)u>PL^ zjJGN>#VMOZKz}-a1Vjc*VrX%|DkiZHvMmQe z6m|+vhI|#n85*saz)+%M4nvC-?8hhyKvBYcoT3sGH7qxP;tb2Jpt!|w4WJs(C1!#5 ztO3=sd=$`8mg@l3v0M+Rp5+EW4JCgX!e!K#V{ zrar-{G*reHL+@hHB+eqk>wQrvXi+W$Q5of{P%{S^vJExYQQlfvw|%N%#%M zqLN5)6@jaEep86V>qqjKYjg6xh7 z*XAg1aTYt9le}KWyNF#Fa@BNI^*Ex_WXSBS*6*+)mgy~1RnP(pXNTSk3oQGvhFOjC z$Tg}w()-=|@`n*u%dn1O%ww1gBUzrzl;+dT(-~PVnCG&k`38KVhCM{n0$wdtM~z|4 zr83#Ha%K74Aeb%rd58GXNEB%uX;0}Wm>mTZoPWOmvwd$+do z^e=m{Z6w$ysHU8`eSB6AG;rUijh#dor9;$*Q&V9yLovb_h7#2i^=Qi^^(2Op)sq=Y z6Q(iu%@@vRC|#J&P&Np&f#Dnw=CGU#!d#XMKvn>%xw2sC73)%+R|l{oo}Ei&?^B;8s~R`-Y%BQL8qLS++=(i5|<$v$<}1}w41n` zo) zM(IYWHf!-P=teUXqZ`9eqJAP%Jx$V2VklWZnV~e@H0Hkfy7L)I*G*?A8+5aQ;T+J- zVL2Ccb183}VfNzi+weMURMtAy1|&w^yJXbO$4#uqKNlfc5wa&K@0ysd>Uj@lHQ_mN zw$5x-ERgXlKxYN0c<7HgMof$gx}KYm4|xRM)It4AkuMoBF+LsH z?-24PBZkjmAi^~XJ5-&DG?(qXpp+gLRCZo)h#nVIc3x0Vj|+-%fwRv!W!FF1W z4=K`p$_WFF9o<*9ox!m!7y0I*;p7t|^Z9N4+kZQcGduPyH$E-RoaGkdN)*DyDAMmm zC+_``Dg5~Y4(VAMSsg3)f4E5N9N+ccz9vM=@YC>vF$e0)w-dsmq6LO^7j8HQ&Bph{S@HYQxf3#th7C~4M z#E|~AEuOG=qef2~&JM3Y&=m+HYE$mA2VaBHl&`~}>$LiGc}kLZV`kd$0P?|u(kp$O`>@U-E0uucGzE>cCyibDbNcLA*`89C4 z20>Kb2KH^>Mfp0oT&IV*CHxKOa|2wc{3bZu1W(GhKz@sr--f=o>0xgPe+T4u=&rv6 zSKplP!eDxTJGiy8`~bKIU`OR0;MT#)9|HG~m3M+$C)K9}Zx|RqqNf-n{8Mmy%F3UC z+cOwOpWg)@T`-*TbLjURLMgujw^!g#`8Bw`X1N=r-QY;&Js|C2`3<7Tn&l z{0^k=z=6u&gY-S+Wm@MOEX)4-?K3UtuHC)q*4!nXH*h4+G|5ygkuuJXOR-o%TP z0P6r|Ardekz?TGjTm-8HMg1rB4!s(>Gte<_-~E~<>22(+RS;4IV@OC3t4K3ZNUq<( z!DXr5QdMpC+tgltQdj(T7ym5pEms}(Is0T)=hbg=e#f790Xbiw|D2$vv-VZmKhi%w z)1isl^em&z$m|b1OGrJX0VLqy$cMCxkSPtR-y!YqXrldFN0j#FibsFo5Ut1t#kb7x z)Pr6egrYyOJ_ArdZ)(MX zD3GCQK-JKjD9Jys0+L31!Qyq^(9G7SpF}TX>E&d4IfY)1rD3mPi_!r%XxEtoEPWI4d#Y$eK}{&m2=}9xS?D>E|BZb4di?{ i7tWCj;QYBhTrf9`8_td3WSqIg$k@DVopbMfcl*yFAv`3cTR;#~ zv%`z?atif4f*>sLZ$V-HJP$+=o5vAEacx_YcJmE)s(-?Kvzubs!n|tq*Z0{EBRYhi zh*TSn1Yhg?p&@(9DV|wx^V}014vrsJc)mW=zP#jZ-6M2;ZdbVWZzCovFASg9mj7gN z6_E3_r)#<-TU#5+GXDl{wRY^c6|b?cQ3li~|IN^?{CytWK(`?-6?u!kys zU0ZMwZQr4&^VEb4qwwasrc#rK8~9YJ-5>i7*1zk2RGcs+|5cw?bk7(c?MRqg^!;IC z`l`lxYR!#}HpVaA_q+CMn`pmbY0F}s=-kDXOW^B+! z{TbUks{~&jEnEKd)GfZvAH6mk&&>7>SHHWps^)@gcUH$rE7^|H(6BKmi?!gMxI#qvGZ}|0OZvv zbBjV;Vx%-0gXKU`pbZBgFS%^Iv%2{wdm8y-*+t+Y=Ky3$f7eJ%+BOFod0<%)tVrYl zWMTQid%A^>m6nKtm5X^|bC55%>YY%;7$+L>@#qvXMZ^KfX$J*w9rwjL(}=`!s5I1y z1CSqi4=u2NGvr1i3oJ)UBb_+_SsQt|I5XAAoks3h_B8Rd-~i-guQR8z%|2Jt2w^#n zij!~v@-&ZEo)0#xRSS>{tUNs?Jr!i@)FlU=RxHxc$P15FYL#&ufPA*D+p(bcjz5h| zu$(AOWHMo!-+(o0104nhGZ;2UfjRqAY&#CqLB)Zj*vzuIRH8IPLHy!y(x@F z##jyz1uz_dd~;!6!*X>+JdMUfAa`zyNHMu2 zPNmUDSPm8j+j0PM-|4ylZDUa`jZCqeBu!#C0C{uSmX?MuPA?FkNwBeu*o+US7QeP7 zvUOG0A{vSC=qPEF3kNV-v_U7nctVm-BPEt63nwc$0C{WWBjWC_#%pQhgk`m<+JXa+ zo9@-dvoOLJ5cGJiY%WeWUIR_wD9I5`-z6sTPXylD$)l}6C z4nU6D(YT`4`>(w;nu29#fwK(-ggFO(|9 zC_hrVgVn-4NAeDk<0^5!+d+e)2_`kp+{iNVbEz0>8&ovStrc zPA8|e<>d%A@hHZbbmoJ3Qj81f!iQH%F|MR5ADok7+(2&&Nm~Q>RVV58Oy{~Ev#mLmMk6{n^ASbDO;2=Tj>(`>>44RwxBn~gW1;m|P6#v0J@MNG4C1~gm-rr8(+8twtpYc=_?kr zvjsyrh@oKi9@Hn?Eo}HZd-nzD#iK5(AftHHWi8Dt9d%jxS)Yx%tO9*8YzP*C#=nol z_y5xq)c<*5s>agEeeFOeE)B&*#bhv2iqS|lj&KGLQh0Z4c*4YSQ+BRgIaGfYzXlyo zCxir|m;hdY5S)c#0(pT#aQT)W?w1@GPU^!upUj(V4)qErfIq;-FMQyqRJSW(?lt|m zcQ7y591JC)Q(LRcy+43yS%54YUg7VHkKH@jZgm^eazFW0cz6j_gM|ZP>-(4%xrzK? z?&1A`=W1+f-m;#cR{D?f1dS>P9u|?G^Qk@;ADp0FZFl@=in$RS_3j4)ug0&JEj5Ch z*mFvK)p=m`QX@G2S##o@&-Lc-G=h7)+arECb5{BpBe+Uf83Ai5$p56^1ba<(|GO*v z$I8&OG&JRgCgWKDU3x|7Gc7&A-cBL2wz0C5i=`4 Date: Tue, 24 Mar 2026 11:56:33 +0100 Subject: [PATCH 097/128] added autostart and odometry dropout checks --- control/velocity_controller/CMakeLists.txt | 4 +-- .../velocity_controller.hpp | 17 +++++++--- control/velocity_controller/package.xml | 1 + .../src/velocity_controller.cpp | 34 ++++++++++++------- 4 files changed, 37 insertions(+), 19 deletions(-) diff --git a/control/velocity_controller/CMakeLists.txt b/control/velocity_controller/CMakeLists.txt index 9abb901a0..5540b1e01 100644 --- a/control/velocity_controller/CMakeLists.txt +++ b/control/velocity_controller/CMakeLists.txt @@ -13,7 +13,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) -#find_package(lifecycle_msgs REQUIRED) +find_package(lifecycle_msgs REQUIRED) find_package(std_msgs REQUIRED) find_package(vortex_msgs REQUIRED) find_package(vortex_utils REQUIRED) @@ -77,7 +77,7 @@ add_executable(velocity_controller_node ament_target_dependencies(velocity_controller_node rclcpp rclcpp_lifecycle - #rclcpp_msgs + lifecycle_msgs std_msgs vortex_msgs geometry_msgs diff --git a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp index 09e6474ef..4225767ab 100644 --- a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp +++ b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp @@ -19,7 +19,10 @@ class Velocity_node : public rclcpp_lifecycle::LifecycleNode{ public: - Velocity_node(); + explicit Velocity_node(); + Velocity_node(const Velocity_node&)=delete; //no copy constructor + Velocity_node& operator=(const Velocity_node&) = delete; //no copy assignment + //TODO: decide if i one should be allowed to move or transfer ownership if the class //Different initializatin functions void get_new_parameters(); @@ -34,7 +37,8 @@ class Velocity_node : public rclcpp_lifecycle::LifecycleNode{ rclcpp::Publisher::SharedPtr publisher_thrust; //Timer instance - rclcpp::TimerBase::SharedPtr timer_calculation; + rclcpp::TimerBase::SharedPtr timer_calculation; + rclcpp::TimerBase::SharedPtr startup_timer_; //Subscriber instance rclcpp::Subscription::SharedPtr subscriber_Odometry; rclcpp::Subscription::SharedPtr subscriber_guidance; @@ -87,8 +91,12 @@ class Velocity_node : public rclcpp_lifecycle::LifecycleNode{ std::atomic_bool should_exit_{false}; //VC settings - bool anti_swing=1; - bool anti_overshoot=0; + bool reset_on_new_ref=true; + bool anti_overshoot=false; + bool auto_start=true; + bool odometry_dropout_guard=true; + int publish_counter=0; + bool first_start=true; //States rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure(const rclcpp_lifecycle::State &) override; @@ -97,7 +105,6 @@ class Velocity_node : public rclcpp_lifecycle::LifecycleNode{ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) override; rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; - //TODO: reset function that resets all controllers, easier to do, and be able to pass an argument so that u can reset either Surge, Pitch and Yaw void reset_controllers(int nr=0); }; diff --git a/control/velocity_controller/package.xml b/control/velocity_controller/package.xml index 90dd8db86..f138d6dbe 100644 --- a/control/velocity_controller/package.xml +++ b/control/velocity_controller/package.xml @@ -18,6 +18,7 @@ ct_core vortex_utils rclcpp_lifecycle + lifecycle_msgs auv_setup ament_cmake_gtest diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index 66ba48d9c..f5e023606 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -8,8 +8,10 @@ #include "velocity_controller/NMPC_setup.hpp" #include "velocity_controller/PID_setup.hpp" #include +#include #include #include +#include #include #include #include @@ -51,6 +53,9 @@ Velocity_node::Velocity_node() : rclcpp_lifecycle::LifecycleNode("velocity_contr controller_type=1; RCLCPP_INFO(this->get_logger(),"Switching to PID"); }; + if(auto_start){ + startup_timer_=create_wall_timer(std::chrono::milliseconds(0), [this](){startup_timer_->cancel(); trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);}); + } return; } @@ -62,6 +67,14 @@ Velocity_node::Velocity_node() : rclcpp_lifecycle::LifecycleNode("velocity_contr void Velocity_node::calc_thrust() { + if (odometry_dropout_guard){ + publish_counter++; + if(publish_counter>=100){ + reset_controllers(); + RCLCPP_WARN(this->get_logger(),"Odometry dropout, no thrust"); + return; + } + } //RCLCPP_INFO(get_logger(),"Calculating thrust"); angle NED_error={guidance_values.roll-current_state.roll,guidance_values.pitch-current_state.pitch,guidance_values.yaw-current_state.yaw}; angle error=NED_to_BODY(NED_error,current_state); @@ -152,22 +165,15 @@ void Velocity_node::calc_thrust() //Callback functions -//TODO: odometry dropout void Velocity_node::guidance_callback(const vortex_msgs::msg::LOSGuidance::SharedPtr msg_ptr){ Guidance_data old_guidance=guidance_values; guidance_values = *msg_ptr; - if(anti_swing){ - if(abs(old_guidance.surge-guidance_values.surge)>=0.5){ - reset_controllers(1); - } - if (abs(old_guidance.pitch-guidance_values.pitch)>std::numbers::pi/4) { - reset_controllers(2); - - } - if (abs(old_guidance.yaw-guidance_values.yaw)=0.5) reset_controllers(1); + if (abs(old_guidance.pitch-guidance_values.pitch)>std::numbers::pi/4)reset_controllers(2); + if (abs(old_guidance.yaw-guidance_values.yaw)get_logger(), "Guidance received: surge=%.3f pitch=%.3f yaw=%.3f",guidance_values.surge, guidance_values.pitch, guidance_values.yaw); //RCLCPP_INFO(this->get_logger(),"message: s: %f, p:%f, y:%f", msg_ptr->surge,msg_ptr->pitch,msg_ptr->yaw); @@ -258,6 +264,10 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Veloci subscriber_guidance = this->create_subscription( topic_guidance,sub_QoS,std::bind(&Velocity_node::guidance_callback,this, std::placeholders::_1)); //subscriber_killswitch = this->create_subscription(topic_killswitch,sub_QoS,std::bind(&Velocity_node::killswitch_callback,this, std::placeholders::_1)); //Timer + if(first_start&&auto_start){ + startup_timer_=create_wall_timer(std::chrono::milliseconds(0),[this](){startup_timer_->cancel(); trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE);}); + } + first_start=false; return CallbackReturn::SUCCESS; } From 07b373722d9b0a6ea79a6c23d2a3896c9b70196a Mon Sep 17 00:00:00 2001 From: Henrik Date: Tue, 24 Mar 2026 12:24:06 +0100 Subject: [PATCH 098/128] seperate config files for seperate drones, and PID parameters from config file --- .../{parameters.yaml => nautilus_params.yaml} | 8 +++- .../config/orca_params.yaml | 41 +++++++++++++++++++ .../include/velocity_controller/PID_setup.hpp | 1 + .../velocity_controller.hpp | 5 +++ .../launch/velocity_controller.launch.py | 2 +- control/velocity_controller/src/PID_setup.cpp | 5 +++ .../src/velocity_controller.cpp | 16 +++++--- 7 files changed, 69 insertions(+), 9 deletions(-) rename control/velocity_controller/config/{parameters.yaml => nautilus_params.yaml} (80%) create mode 100644 control/velocity_controller/config/orca_params.yaml diff --git a/control/velocity_controller/config/parameters.yaml b/control/velocity_controller/config/nautilus_params.yaml similarity index 80% rename from control/velocity_controller/config/parameters.yaml rename to control/velocity_controller/config/nautilus_params.yaml index 19aedf287..f6b58cd96 100644 --- a/control/velocity_controller/config/parameters.yaml +++ b/control/velocity_controller/config/nautilus_params.yaml @@ -3,12 +3,16 @@ #topics: # odom_topic: /orca/odom #Odometry - # twist_topic: /dvl/twist #Twist + # twist_topic: /dvl/twist #Twistconfig/parameters.yaml # pose_topic: /dvl/pose #Pose # guidance_topic: /orca/guidance/los #Guidance # thrust_topic: /orca/wrench_input #Thrust # softwareoperation_topic: /softwareOperationMode #Software Operation # killswitch_topic: /softwareKillSwitch #Kill Switch + PID_params: + surge: [300.0,10.0,5.0] + pitch: [60.0,8.0,12.0] + yaw: [10.0,1.0,5.0] LQR_params: Q: [200.0,32.84,32.84,15.0,15.0,100.0,32.84,32.84] @@ -21,7 +25,7 @@ Q: [100.0,0.0,0.0,0.0,1.0,1.0,0.0,5.0,50.0] # u,q,r,theta,psi R: [0.1,0.1,0.1] # u_surge, u_theta, u_psi N: 20 - inertia_matrix: [ 30.0, 0.0, 0.0, 0.0, 0.0, 0.60, 0.0, 30.0, 0.0, 0.0, -0.60, 0.30, 0.0, 0.0, 30.0, 0.60, 0.30, 0.0, 0.0, 0.0, 0.60, 0.68, 0.0, 0.0, 0.0, -0.60, 0.30, 0.0, 3.32, 0.0, 0.60, 0.30, 0.0, 0.0, 0.0, 3.34] + #inertia_matrix: [ 30.0, 0.0, 0.0, 0.0, 0.0, 0.60, 0.0, 30.0, 0.0, 0.0, -0.60, 0.30, 0.0, 0.0, 30.0, 0.60, 0.30, 0.0, 0.0, 0.0, 0.60, 0.68, 0.0, 0.0, 0.0, -0.60, 0.30, 0.0, 3.32, 0.0, 0.60, 0.30, 0.0, 0.0, 0.0, 3.34] dampening_matrix_low: [23.0,0.0,0.0,0.0,0.0,0.0, 0.0,46.0,0.0,0.0,0.0,0.0, 0.0,0.0,46.0,0.0,0.0,0.0, 0.0,0.0,0.0,46.0,0.0,0.0, 0.0,0.0,0.0,0.0,46.0,0.0, 0.0,0.0,0.0,0.0,0.0,46.0] dampening_matrix_high: [1.0,0.0,0.0,0.0,0.0,0.0, 0.0,1.0,0.0,0.0,0.0,0.0, 0.0,0.0,1.0,0.0,0.0,0.0, 0.0,0.0,0.0,1.0,0.0,0.0, 0.0,0.0,0.0,0.0,1.0,0.0, 0.0,0.0,0.0,0.0,0.0,1.0] diff --git a/control/velocity_controller/config/orca_params.yaml b/control/velocity_controller/config/orca_params.yaml new file mode 100644 index 000000000..15411d647 --- /dev/null +++ b/control/velocity_controller/config/orca_params.yaml @@ -0,0 +1,41 @@ +/**: + ros__parameters: + #topics: + # odom_topic: /orca/odom #Odometry + # twist_topic: /dvl/twist #Twistconfig/parameters.yaml + # pose_topic: /dvl/pose #Pose + # guidance_topic: /orca/guidance/los #Guidance + # thrust_topic: /orca/wrench_input #Thrust + # softwareoperation_topic: /softwareOperationMode #Software Operation + # killswitch_topic: /softwareKillSwitch #Kill Switch + PID_params: + surge: [300.0,10.0,5.0] + pitch: [60.0,8.0,12.0] + yaw: [10.0,1.0,5.0] + + LQR_params: + Q: [200.0,32.84,32.84,15.0,15.0,100.0,32.84,32.84] + R: [0.02,3.1,3.10] + NMPCA_params: + Q: [1.0,1.0,1.0,5.0,5.0] # u,q,r,theta,psi + R: [0.1,0.1,0.1] # u_surge, u_theta, u_psi + N: 20 + NMPC_params: + Q: [100.0,0.0,0.0,0.0,1.0,1.0,0.0,5.0,50.0] # u,q,r,theta,psi + R: [0.1,0.1,0.1] # u_surge, u_theta, u_psi + N: 20 + #inertia_matrix: [ 30.0, 0.0, 0.0, 0.0, 0.0, 0.60, 0.0, 30.0, 0.0, 0.0, -0.60, 0.30, 0.0, 0.0, 30.0, 0.60, 0.30, 0.0, 0.0, 0.0, 0.60, 0.68, 0.0, 0.0, 0.0, -0.60, 0.30, 0.0, 3.32, 0.0, 0.60, 0.30, 0.0, 0.0, 0.0, 3.34] + dampening_matrix_low: [23.0,0.0,0.0,0.0,0.0,0.0, 0.0,46.0,0.0,0.0,0.0,0.0, 0.0,0.0,46.0,0.0,0.0,0.0, 0.0,0.0,0.0,46.0,0.0,0.0, 0.0,0.0,0.0,0.0,46.0,0.0, 0.0,0.0,0.0,0.0,0.0,46.0] + dampening_matrix_high: [1.0,0.0,0.0,0.0,0.0,0.0, 0.0,1.0,0.0,0.0,0.0,0.0, 0.0,0.0,1.0,0.0,0.0,0.0, 0.0,0.0,0.0,1.0,0.0,0.0, 0.0,0.0,0.0,0.0,1.0,0.0, 0.0,0.0,0.0,0.0,0.0,1.0] + + #calculation_rate: 200 #ms integer + publish_rate: 100 #ms + #Clamp parameter + max_force: 99.5 #should maybe be 99.5 + controller_type: 1 #1 PID 2 LQR 3 NMPC 4NMPC fast + + #Q: [300.0,0.01,0.01,0.01,32.84,32.84,32.84,32.84,32.84] # u,v,w,p,q,r,phi,theta,psi + # R: [0.02,3.1,3.10] # u_surge, u_theta, u_psi + + #Fixes: reduce oscillations, follows angles close to wrap around, model restoring forces in pitch, test different references, try to find out why the fuck it went backwards?, + diff --git a/control/velocity_controller/include/velocity_controller/PID_setup.hpp b/control/velocity_controller/include/velocity_controller/PID_setup.hpp index 0f0f9d86e..ae712b430 100644 --- a/control/velocity_controller/include/velocity_controller/PID_setup.hpp +++ b/control/velocity_controller/include/velocity_controller/PID_setup.hpp @@ -14,6 +14,7 @@ class PID_controller { double get_output(); bool set_output_limits(double min_output, double max_output); bool set_parameters(double k_p,double k_i, double k_d, double dt); + bool set_parameters(std::vector& params,double dt); bool set_dt(double dt); private: double Kp_, Ki_, Kd_, dt_; diff --git a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp index 4225767ab..65574982a 100644 --- a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp +++ b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp @@ -14,6 +14,7 @@ #include "velocity_controller/NMPC_setup.hpp" #include "velocity_controller/NMPC_acados.hpp" #include +#include @@ -66,8 +67,12 @@ class Velocity_node : public rclcpp_lifecycle::LifecycleNode{ //PID controllers PID_controller PID_surge; + std::vector surge_params; PID_controller PID_yaw; + std::vector yaw_params; PID_controller PID_pitch; + std::vector pitch_params; + //LQR Controller LQRController lqr_controller; diff --git a/control/velocity_controller/launch/velocity_controller.launch.py b/control/velocity_controller/launch/velocity_controller.launch.py index 470613742..00ad9cb81 100644 --- a/control/velocity_controller/launch/velocity_controller.launch.py +++ b/control/velocity_controller/launch/velocity_controller.launch.py @@ -18,7 +18,7 @@ def launch_setup(context,*args,**kwargs): pkg_share = get_package_share_directory('velocity_controller') global_share = get_package_share_directory('auv_setup') - config_path_local = os.path.join(pkg_share, 'config', 'parameters.yaml') + config_path_local = os.path.join(pkg_share, 'config', f'{drone}_params.yaml') config_path_global = os.path.join(global_share,'config','robots',f"{drone}.yaml") return [ diff --git a/control/velocity_controller/src/PID_setup.cpp b/control/velocity_controller/src/PID_setup.cpp index 0c0b444f0..6a2428168 100644 --- a/control/velocity_controller/src/PID_setup.cpp +++ b/control/velocity_controller/src/PID_setup.cpp @@ -62,3 +62,8 @@ bool PID_controller::set_dt(double dt){ dt_=dt; return true; } +bool PID_controller::set_parameters(std::vector& params,double dt){ + return set_parameters(params.at(0),params.at(1),params.at(2), dt); + +}; + diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index f5e023606..7162377ea 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -45,9 +45,9 @@ Velocity_node::Velocity_node() : rclcpp_lifecycle::LifecycleNode("velocity_contr PID_surge.set_output_limits(-max_force, max_force); PID_pitch.set_output_limits(-max_force, max_force); PID_yaw.set_output_limits(-max_force, max_force); - PID_surge.set_parameters(300,10,5,publish_rate/1000.0); - PID_pitch.set_parameters(60,8,12,publish_rate/1000.0); - PID_yaw.set_parameters(10,1,5,publish_rate/1000.0); + PID_surge.set_parameters(surge_params,publish_rate/1000.0); + PID_pitch.set_parameters(pitch_params,publish_rate/1000.0); + PID_yaw.set_parameters(yaw_params,publish_rate/1000.0); if(!lqr_controller.set_matrices(Q,R,inertia_matrix,max_force,dampening_matrix_low,dampening_matrix_high)||!lqr_controller.set_interval(static_cast(publish_rate)/1000)){ controller_type=1; @@ -216,6 +216,13 @@ void Velocity_node::get_new_parameters(){ this->declare_parameter("controller_type"); this->controller_type=this->get_parameter("controller_type").as_int(); + //PID Params + this->declare_parameter>("PID_params.surge"); + surge_params=this->get_parameter("PID_params.surge").as_double_array(); + this->declare_parameter>("PID_params.pitch"); + pitch_params=this->get_parameter("PID_params.pitch").as_double_array(); + this->declare_parameter>("PID_params.yaw"); + yaw_params=this->get_parameter("PID_params.yaw").as_double_array(); //LQR Parameters @@ -277,8 +284,6 @@ Velocity_node::on_activate(const rclcpp_lifecycle::State & state) RCLCPP_INFO(get_logger(), "Activating..."); timer_calculation = this->create_wall_timer(std::chrono::milliseconds(publish_rate), std::bind(&Velocity_node::calc_thrust, this)); auto ret = LifecycleNode::on_activate(state); - //timer_calculation->reset(); - return ret; } @@ -290,7 +295,6 @@ Velocity_node::on_deactivate(const rclcpp_lifecycle::State & state) auto ret = LifecycleNode::on_deactivate(state); reset_controllers(); //TODO: reset NMPCs - //timer_calculation->cancel(); return ret; } From 0acc8cb325c9b35b8b579019ff3825d115d98b33 Mon Sep 17 00:00:00 2001 From: Henrik Date: Tue, 24 Mar 2026 12:34:02 +0100 Subject: [PATCH 099/128] preparing to merge changes to autopilot branch and branch for pushing to main --- .../velocity_controller/config/nautilus_params.yaml | 11 +---------- .../include/velocity_controller/utilities.hpp | 2 +- control/velocity_controller/src/LQR_setup.cpp | 1 - control/velocity_controller/src/NMPC_setup.cpp | 2 +- control/velocity_controller/src/utilities.cpp | 2 +- 5 files changed, 4 insertions(+), 14 deletions(-) diff --git a/control/velocity_controller/config/nautilus_params.yaml b/control/velocity_controller/config/nautilus_params.yaml index f6b58cd96..ca12c8bca 100644 --- a/control/velocity_controller/config/nautilus_params.yaml +++ b/control/velocity_controller/config/nautilus_params.yaml @@ -1,14 +1,6 @@ /**: ros__parameters: - #topics: - # odom_topic: /orca/odom #Odometry - # twist_topic: /dvl/twist #Twistconfig/parameters.yaml - # pose_topic: /dvl/pose #Pose - # guidance_topic: /orca/guidance/los #Guidance - # thrust_topic: /orca/wrench_input #Thrust - # softwareoperation_topic: /softwareOperationMode #Software Operation - # killswitch_topic: /softwareKillSwitch #Kill Switch PID_params: surge: [300.0,10.0,5.0] pitch: [60.0,8.0,12.0] @@ -29,10 +21,9 @@ dampening_matrix_low: [23.0,0.0,0.0,0.0,0.0,0.0, 0.0,46.0,0.0,0.0,0.0,0.0, 0.0,0.0,46.0,0.0,0.0,0.0, 0.0,0.0,0.0,46.0,0.0,0.0, 0.0,0.0,0.0,0.0,46.0,0.0, 0.0,0.0,0.0,0.0,0.0,46.0] dampening_matrix_high: [1.0,0.0,0.0,0.0,0.0,0.0, 0.0,1.0,0.0,0.0,0.0,0.0, 0.0,0.0,1.0,0.0,0.0,0.0, 0.0,0.0,0.0,1.0,0.0,0.0, 0.0,0.0,0.0,0.0,1.0,0.0, 0.0,0.0,0.0,0.0,0.0,1.0] - #calculation_rate: 200 #ms integer publish_rate: 100 #ms #Clamp parameter - max_force: 99.5 #should maybe be 99.5 + max_force: 99.5 controller_type: 1 #1 PID 2 LQR 3 NMPC 4NMPC fast #Q: [300.0,0.01,0.01,0.01,32.84,32.84,32.84,32.84,32.84] # u,v,w,p,q,r,phi,theta,psi diff --git a/control/velocity_controller/include/velocity_controller/utilities.hpp b/control/velocity_controller/include/velocity_controller/utilities.hpp index 543174581..85d961da7 100644 --- a/control/velocity_controller/include/velocity_controller/utilities.hpp +++ b/control/velocity_controller/include/velocity_controller/utilities.hpp @@ -28,7 +28,7 @@ class State{ State operator=(int n){if (n){surge=0.0,sway=0.0,heave=0.0,roll_rate=0.0,pitch_rate=0.0,yaw_rate=0.0,roll=0.0,pitch=0.0,yaw=0.0;} return *this;}; State operator=(nav_msgs::msg::Odometry::SharedPtr rhs); }; -//TODO: fix these so the initializing is correct, and that changing the quaternions changes the angles, so that the state is always consistent +//TODO: fix these so that changing the quaternions changes the angles, so that the state is always consistent class Guidance_data:public State{ public: //double surge; double pitch; double yaw; diff --git a/control/velocity_controller/src/LQR_setup.cpp b/control/velocity_controller/src/LQR_setup.cpp index 81de3af27..f7b5bc991 100644 --- a/control/velocity_controller/src/LQR_setup.cpp +++ b/control/velocity_controller/src/LQR_setup.cpp @@ -149,7 +149,6 @@ Eigen::Matrix LQRController::linearize(State s){ Eigen::Matrix ret; ret.setZero(); ret.block<5,5>(0,0)=A.block<5,5>(0,0); - //legge inn integral state #TODO ret.block<3,3>(5,0)=Eigen::Matrix3d::Identity(); return ret; diff --git a/control/velocity_controller/src/NMPC_setup.cpp b/control/velocity_controller/src/NMPC_setup.cpp index 657a9e6de..86e0b3cea 100644 --- a/control/velocity_controller/src/NMPC_setup.cpp +++ b/control/velocity_controller/src/NMPC_setup.cpp @@ -102,7 +102,7 @@ bool NMPC_controller::initialize_MPC(){ } //D=mtimes(M_i,D); //Creating Coriolis matrix - SYM Cor=SYM::zeros(6,6); //TODO maybe make a general crossproduct matric generator + SYM Cor=SYM::zeros(6,6); Cor(0,1)=-mass*X(5); Cor(0,2)=mass*X(4); Cor(1,0)=mass*X(5); Cor(1,2)=-mass*X(3); Cor(2,0)=-mass*X(4); Cor(2,1)=mass*X(3); diff --git a/control/velocity_controller/src/utilities.cpp b/control/velocity_controller/src/utilities.cpp index dae729e68..61b09d8ef 100644 --- a/control/velocity_controller/src/utilities.cpp +++ b/control/velocity_controller/src/utilities.cpp @@ -24,7 +24,7 @@ angle quaternion_to_euler_angle(double w, double x, double y, double z){ return {phi, theta, psi}; }; angle NED_to_BODY(const angle &a,const State &s){ - //TODO tests for illegal angles + //TODO tests for illegal angles maybe Eigen::Vector3d q; q< Date: Tue, 24 Mar 2026 12:42:02 +0100 Subject: [PATCH 100/128] more changes to prepare for merges --- control/velocity_controller/src/LQR_setup.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/control/velocity_controller/src/LQR_setup.cpp b/control/velocity_controller/src/LQR_setup.cpp index f7b5bc991..c06b3853e 100644 --- a/control/velocity_controller/src/LQR_setup.cpp +++ b/control/velocity_controller/src/LQR_setup.cpp @@ -185,6 +185,7 @@ bool LQRController::calculate_thrust(State state, Guidance_data guidance_values) B(2,0),B(2,1),B(2,2), B(3,0),B(3,1),B(3,2)); */ + //TODO: consider making my own solver using eigen so that it does not need controll_toolbox library bool INFO= lqr.compute(Q,R,linearize(state),B,K_l,true,false); if(INFO==0){ return false; From a06855f53bf460ad235d78094c577045c7ff00d9 Mon Sep 17 00:00:00 2001 From: Henrik Date: Tue, 24 Mar 2026 12:43:03 +0100 Subject: [PATCH 101/128] more changes to prepare for merges --- control/velocity_controller/config/orca_params.yaml | 13 ++----------- 1 file changed, 2 insertions(+), 11 deletions(-) diff --git a/control/velocity_controller/config/orca_params.yaml b/control/velocity_controller/config/orca_params.yaml index 15411d647..a6b227d8e 100644 --- a/control/velocity_controller/config/orca_params.yaml +++ b/control/velocity_controller/config/orca_params.yaml @@ -1,13 +1,6 @@ /**: ros__parameters: - #topics: - # odom_topic: /orca/odom #Odometry - # twist_topic: /dvl/twist #Twistconfig/parameters.yaml - # pose_topic: /dvl/pose #Pose - # guidance_topic: /orca/guidance/los #Guidance - # thrust_topic: /orca/wrench_input #Thrust - # softwareoperation_topic: /softwareOperationMode #Software Operation - # killswitch_topic: /softwareKillSwitch #Kill Switch + PID_params: surge: [300.0,10.0,5.0] pitch: [60.0,8.0,12.0] @@ -24,14 +17,12 @@ Q: [100.0,0.0,0.0,0.0,1.0,1.0,0.0,5.0,50.0] # u,q,r,theta,psi R: [0.1,0.1,0.1] # u_surge, u_theta, u_psi N: 20 - #inertia_matrix: [ 30.0, 0.0, 0.0, 0.0, 0.0, 0.60, 0.0, 30.0, 0.0, 0.0, -0.60, 0.30, 0.0, 0.0, 30.0, 0.60, 0.30, 0.0, 0.0, 0.0, 0.60, 0.68, 0.0, 0.0, 0.0, -0.60, 0.30, 0.0, 3.32, 0.0, 0.60, 0.30, 0.0, 0.0, 0.0, 3.34] dampening_matrix_low: [23.0,0.0,0.0,0.0,0.0,0.0, 0.0,46.0,0.0,0.0,0.0,0.0, 0.0,0.0,46.0,0.0,0.0,0.0, 0.0,0.0,0.0,46.0,0.0,0.0, 0.0,0.0,0.0,0.0,46.0,0.0, 0.0,0.0,0.0,0.0,0.0,46.0] dampening_matrix_high: [1.0,0.0,0.0,0.0,0.0,0.0, 0.0,1.0,0.0,0.0,0.0,0.0, 0.0,0.0,1.0,0.0,0.0,0.0, 0.0,0.0,0.0,1.0,0.0,0.0, 0.0,0.0,0.0,0.0,1.0,0.0, 0.0,0.0,0.0,0.0,0.0,1.0] - #calculation_rate: 200 #ms integer publish_rate: 100 #ms #Clamp parameter - max_force: 99.5 #should maybe be 99.5 + max_force: 99.5 controller_type: 1 #1 PID 2 LQR 3 NMPC 4NMPC fast #Q: [300.0,0.01,0.01,0.01,32.84,32.84,32.84,32.84,32.84] # u,v,w,p,q,r,phi,theta,psi From 7ec98730afbd477839b1c070b75183e3bda69a06 Mon Sep 17 00:00:00 2001 From: Anbit Date: Tue, 24 Mar 2026 13:20:20 +0100 Subject: [PATCH 102/128] Add: Slowing down while reaching the goal --- .../los_guidance/launch/guidance_test.launch.py | 15 +++++++++++++++ guidance/los_guidance/src/los_guidance_ros.cpp | 14 +++++++++++--- 2 files changed, 26 insertions(+), 3 deletions(-) diff --git a/guidance/los_guidance/launch/guidance_test.launch.py b/guidance/los_guidance/launch/guidance_test.launch.py index 68e739ab7..7b3470932 100644 --- a/guidance/los_guidance/launch/guidance_test.launch.py +++ b/guidance/los_guidance/launch/guidance_test.launch.py @@ -26,6 +26,7 @@ def launch_setup(context, *args, **kwargs): vortex_sim_interface_dir = get_package_share_directory("vortex_sim_interface") los_guidance_dir = get_package_share_directory("los_guidance") operation_mode_manager_dir = get_package_share_directory("operation_mode_manager") + velocity_controller_dir = get_package_share_directory('velocity_controller_lqr') stonefish_sim = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -73,6 +74,19 @@ def launch_setup(context, *args, **kwargs): }.items(), ) + velocity_controller_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + velocity_controller_dir, 'launch', 'velocity_controller_lqr.launch.py' + ) + ), + launch_arguments={ + "drone": drone, + "namespace": namespace, + }.items(), + + ) + orca_sim = TimerAction( period=12.0, actions=[ @@ -128,6 +142,7 @@ def launch_setup(context, *args, **kwargs): vortex_sim_interface, operation_mode_launch, los_guidance_launch, + velocity_controller_launch, orca_sim, set_autonomy, run_test_scenario, diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 09042ae74..976d73980 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -257,9 +257,17 @@ vortex_msgs::msg::LOSGuidance LosGuidanceNode::fill_los_reference( std::clamp(outputs.theta_d, -max_pitch_angle_, max_pitch_angle_); reference_msg.pitch = clamped_pitch; reference_msg.yaw = outputs.psi_d; - if ((inputs.current_position - inputs.next_point).as_vector().norm() <= - slow_down_distance_) { - reference_msg.surge = (u_desired_ / 2.0); + + const double distance_to_goal = + (inputs.current_position - inputs.next_point).as_vector().norm(); + + const double u_slow_min = 0.2; // bytt ut dette med config + + if (distance_to_goal <= slow_down_distance_) { + const double alpha = distance_to_goal / slow_down_distance_; + reference_msg.surge = u_slow_min + alpha * (u_desired_ - u_slow_min); + } else { + reference_msg.surge = u_desired_; } return reference_msg; From 52b958b760c7a189b4a594c94135a457d7315a74 Mon Sep 17 00:00:00 2001 From: Henrik Date: Tue, 24 Mar 2026 14:23:41 +0100 Subject: [PATCH 103/128] idk what this is --- .../index/LQR_setup.cpp.2C597C3359569B5F.idx | Bin 0 -> 10314 bytes .../index/LQR_setup.hpp.7B50E8B15298D255.idx | Bin 0 -> 4224 bytes .../NMPC_acados.cpp.326F2FC10CEB64DA.idx | Bin 0 -> 6250 bytes .../NMPC_acados.hpp.29D23C04AD60A55C.idx | Bin 0 -> 3236 bytes .../index/NMPC_setup.cpp.86F9F0381989E206.idx | Bin 0 -> 14226 bytes .../index/NMPC_setup.hpp.CAD16FEB6583910A.idx | Bin 0 -> 2236 bytes .../index/PID_setup.cpp.3C9DCE2555B8A965.idx | Bin 0 -> 2132 bytes .../index/PID_setup.hpp.486A8D0A6ED861A2.idx | Bin 0 -> 1310 bytes ...im_solver_auv_model.c.53A9281F3B2EA7B4.idx | Bin 0 -> 5454 bytes ...im_solver_auv_model.h.995A50C901E2AF1C.idx | Bin 0 -> 3536 bytes ...os_solver_auv_model.c.5778764B3A2FDFAE.idx | Bin 0 -> 12412 bytes ...os_solver_auv_model.h.F9AC8187D4C1016F.idx | Bin 0 -> 5168 bytes ..._model_impl_dae_fun.c.CDE429F376604CAB.idx | Bin 0 -> 2238 bytes ...ae_fun_jac_x_xdot_u.c.6132B1FDE79246FD.idx | Bin 0 -> 2716 bytes ..._fun_jac_x_xdot_u_z.c.ED4BE4E791391EA0.idx | Bin 0 -> 2796 bytes ...ae_fun_jac_x_xdot_z.c.59CA37EE71B4BA5D.idx | Bin 0 -> 2718 bytes ..._dae_jac_x_xdot_u_z.c.A6D29D52260DC082.idx | Bin 0 -> 2772 bytes .../auv_model_model.h.60CCFC5FA93A2F4B.idx | Bin 0 -> 2004 bytes ...ct_instantiations.cpp.CECDE40637351DC9.idx | Bin 0 -> 1054 bytes .../main_auv_model.c.FC541DFFAD75A3A0.idx | Bin 0 -> 1648 bytes .../main_sim_auv_model.c.E637641F2593FF77.idx | Bin 0 -> 1122 bytes .../index/test_LQR.cpp.46B8F24A8C36EC93.idx | Bin 0 -> 8694 bytes .../index/test_PID.cpp.575590D7897A814B.idx | Bin 0 -> 8790 bytes .../index/test_VC.cpp.74BA115DB14CB17C.idx | Bin 0 -> 4538 bytes .../index/test_VC.hpp.C3EBE494A18C2184.idx | Bin 0 -> 1946 bytes .../index/utilities.cpp.7F99D4E1DE20E3AB.idx | Bin 0 -> 4004 bytes .../index/utilities.hpp.77C0A5FDF681DAA0.idx | Bin 0 -> 3028 bytes ...locity_controller.cpp.DFC34CF5F86A4B55.idx | Bin 0 -> 15478 bytes ...locity_controller.hpp.3E0346F5513060F5.idx | Bin 0 -> 4532 bytes .gitignore | 59 ++++++++++++++++++ 30 files changed, 59 insertions(+) create mode 100644 .cache/clangd/index/LQR_setup.cpp.2C597C3359569B5F.idx create mode 100644 .cache/clangd/index/LQR_setup.hpp.7B50E8B15298D255.idx create mode 100644 .cache/clangd/index/NMPC_acados.cpp.326F2FC10CEB64DA.idx create mode 100644 .cache/clangd/index/NMPC_acados.hpp.29D23C04AD60A55C.idx create mode 100644 .cache/clangd/index/NMPC_setup.cpp.86F9F0381989E206.idx create mode 100644 .cache/clangd/index/NMPC_setup.hpp.CAD16FEB6583910A.idx create mode 100644 .cache/clangd/index/PID_setup.cpp.3C9DCE2555B8A965.idx create mode 100644 .cache/clangd/index/PID_setup.hpp.486A8D0A6ED861A2.idx create mode 100644 .cache/clangd/index/acados_sim_solver_auv_model.c.53A9281F3B2EA7B4.idx create mode 100644 .cache/clangd/index/acados_sim_solver_auv_model.h.995A50C901E2AF1C.idx create mode 100644 .cache/clangd/index/acados_solver_auv_model.c.5778764B3A2FDFAE.idx create mode 100644 .cache/clangd/index/acados_solver_auv_model.h.F9AC8187D4C1016F.idx create mode 100644 .cache/clangd/index/auv_model_impl_dae_fun.c.CDE429F376604CAB.idx create mode 100644 .cache/clangd/index/auv_model_impl_dae_fun_jac_x_xdot_u.c.6132B1FDE79246FD.idx create mode 100644 .cache/clangd/index/auv_model_impl_dae_fun_jac_x_xdot_u_z.c.ED4BE4E791391EA0.idx create mode 100644 .cache/clangd/index/auv_model_impl_dae_fun_jac_x_xdot_z.c.59CA37EE71B4BA5D.idx create mode 100644 .cache/clangd/index/auv_model_impl_dae_jac_x_xdot_u_z.c.A6D29D52260DC082.idx create mode 100644 .cache/clangd/index/auv_model_model.h.60CCFC5FA93A2F4B.idx create mode 100644 .cache/clangd/index/ct_instantiations.cpp.CECDE40637351DC9.idx create mode 100644 .cache/clangd/index/main_auv_model.c.FC541DFFAD75A3A0.idx create mode 100644 .cache/clangd/index/main_sim_auv_model.c.E637641F2593FF77.idx create mode 100644 .cache/clangd/index/test_LQR.cpp.46B8F24A8C36EC93.idx create mode 100644 .cache/clangd/index/test_PID.cpp.575590D7897A814B.idx create mode 100644 .cache/clangd/index/test_VC.cpp.74BA115DB14CB17C.idx create mode 100644 .cache/clangd/index/test_VC.hpp.C3EBE494A18C2184.idx create mode 100644 .cache/clangd/index/utilities.cpp.7F99D4E1DE20E3AB.idx create mode 100644 .cache/clangd/index/utilities.hpp.77C0A5FDF681DAA0.idx create mode 100644 .cache/clangd/index/velocity_controller.cpp.DFC34CF5F86A4B55.idx create mode 100644 .cache/clangd/index/velocity_controller.hpp.3E0346F5513060F5.idx create mode 100644 .gitignore diff --git a/.cache/clangd/index/LQR_setup.cpp.2C597C3359569B5F.idx b/.cache/clangd/index/LQR_setup.cpp.2C597C3359569B5F.idx new file mode 100644 index 0000000000000000000000000000000000000000..d1c96570c7059f612ade7721ccbc7223e5d034ff GIT binary patch literal 10314 zcmb`Nd0Z3M6TmYuKp+bQ2zOZSQ|=oPQ3z*15djsVuHr4I9HMw2Dj|qa@Iq7&5i2Mn zDp;|_R%`2R1;wMx_4lHEdF%i3`Pk2XGrMo!%$qkeZ?=gsk&$6e98UPu zm_?b>awqdR9F7S6%gN22WueC51Q>8Qi)xOJ%ba)9UwCzt*NuHM*PcCpZqxC!(vEK~ z&(0BhF3y~pI2^$iVH8OuA$zs-DOZ&t8l^oMUlcdoiw z@Zrt$lD#Q8T^UiiT@PZ@wBLLbkoLo$9lsyR=*?3+yZ1m) zzVFTNza%#XWau^&0m^nUgD88zQ*m{b10bN86*vu}01cp7q}eDTENjraO=GPR2* z8tAxMnpSodCLg&iJo#fyYfw^DaQfOxqit5%bGlP) zu9TbsI;4uTHhW9)81w8qikTYTga7Wh+L$-;_PS{?Wo?IaCRjH*K41T)V(0ct*>*oS zZMO>M_s^_z9AA)|yI7ntev91y$$Ud<=-#_?J1*(;wY3gEFyMRWOt$gft{0+NM4&E>4_gI{mAfG({$3F{B@&4+2axTKK;ACP< zW982q_j~nR2!rKQ+4__RTe|=T(6{n4MHqxrt!QKB(>pH)`=ayT>BuX;& z=T7G7-G;0GrYEkretgu^!~brGr!KFzfYB*Gt=l3tn5s2y?F+ROx8bnBN8A6J*~PxO zN9X);BG==)uliyvre{`MXdlhrZB}|dyRxoAGPvf@xS|cabq?2h9{r1I-C1GL(P38j z^QKi}_DuP3Z*#}F6{RL^yG;b+7S*MNN7~qmmt1?zSaZJLJuI#9{^qSskM8rZfE{Ux_COZKV!my*7?qM*#Sw~TPFLprZ4=veQ8f}*Oc*@JinT) z(Ldi3YD_z4Ibo8^s&|VrYrYMz&-{bh-#)KUaLc)~+2V)hqfYxmeU^VXaqIo!{uhmZ zbSuKU`ZK9iI59x)hd14+RVNRPZqR#`qUcSZb#}oq^H%r8#yN81Rcpfjdb_Fr+{Nzk zuY)IcZW>qSVzXvm*-MYQ#6JzqQkMKY-Y|cDSipndciI0D_m9k5qbELY?gY=1UB3LF z_ERIYoSpyq<<;pcj*D{l9J1&=HTRC&;WG<;e*Nyj;qMP$DCl|L>#)pAG_vsB^7vty z+AmWkr?wbGp#C?s<(`QT%Xw4oA?1SxcJL0rxjkj`Ij?yqP6&*0 zGv07@w;t9XTF!LgmBj57pDV+T;PfZSqa_S>?|xXL@Ltb8_-C zr#uD@N02ckC(&)!2q8B$nZq0e(Lu12d1!i=a`-5gqQ9>if3XY=I3^Ktv(OFqz~CNO z$~^g=B7DQjg5!e49sDREcP_f22Shz+1b0n$)d&q~uQDDEIWk7bor7-3a?bLW`OrR& z`1#t~8g`0>CCL8RYHhTM+rk<)4a#PR^GmU0i%lrlYMp(RV+tWEwxnnj7x$}@X%{k3W=12QE za7;Ly1f*h%_+8oV)7P7Y+}TK6#&s;?dda-RUJm$qSN+$0{%yg>h1_g(gNw+;O6I2N zW{AbX2^YMMyC?ieaWatx17JM>ezH;4qx?9UJmBPxLmE`ypAhRM`1*{HJ0FeE435p{ zmH=^p3m)T#_Q{1GmWE!XIC*G{BCfEAYbQ$(CfIQ_)dz7tT49d1qd{!Oy2nP`Bs7MJ zu8COY%5&9IYB3sVAxk;7-RZ%DrxYgxjqnUa&%i-8QZ&-x<1BKuMWcwTt5T1Va|Lp)0p_HU`i}owJ54a&q4D!=n@_PwE++i zbq7Fi0E`L01I>4!%VxuOV9w?t?_fCLBCcl<*B_gQLgpJ<#0|sz?e?J1;#M)AD?;mL zSPDjX;!1Wb1t-jRtG1r`C;Wc~>}yVjOr5n%Ms+CW;rONlcnY=h@;H9Jq8PU_tXNJrFV7pw5@KBOqyDn7T%FZ=Lj6fx@Z`yQ;Y@A!ic;gd3bUnc|`;88o&sL0g7n=&jtu1+yr&d z1P6fp(hK}vFvGE_Xy9~z1b4RSAQ2Pk5$THu7BNv;Q7TsvG0}o(A(m})5=8v^>svD} zg*4$IWw0g`Q_dBXb157(o_zgH;`1pl%+*AU8{bWYpBFJ+d@mOH@%>m7#1CRo2tPzc z0~;>vuV}t)@r8!_H$E43K-@)+r8=@>FQIjG4&1Fu`GPS@_B1=p?6QfK*O1>&D6 z6l}-Tz&K*pgJ6FUT(B+L{5RG`7hbeMZngykTR;atC}MVlU^iJZ5%UiS{sA50YOA?| z)m$CyYSR<{9Q4b{Viy#UmUL1*((}D>oxyzN1oCGo-N>oBe#gz2VVQYn{X?5w1Ow*$ul@h9=zaq zsPR5?1KwwDz+>2Mz+>2Mz+>2MF#Fi5MIFO>ywt?V0M>G_!wQI*V$doEZNdsLRDcPa zZ4_Y7W;X@eFaKrX{`(8K3)8!NkVCu%!D}$au}s9Q;0ji7Rk7==%iney-U@#q#Y74s z^|0q4ii(UU6lD|TL@3%e+L@44AZ0x#4Uw`!db}W>CCk#tQfWjnIf5KLJYPhPIc|iO z2$ry9MIa~w7MFmagv}d3uz}6(AZQ0Y?7kFp5(Fm++n=qCvKjagjP|_5UgAa+iLj4Q z(Xe-x4I&f6p(tco@+^fUWTl8xSeDoUu{$6IyA_4JM|&TbVsBBh|2_yKd=vOLL4*U= z-yU)g50A{DAZDb?q;geq)A?@k?pwcXAEriq^fG0Eru?HmdfBIIB`Lj(kkZQtDZLES zb45kW@HuaXp{U#hvzj0a&lg(=%>JthFQj^0iuT-mrLKF>^EdTIVQ6A84lydDd2z1dXkc~R7uvKp z4;-~KdsHO`6X+3c290LWCd=OpA#DP_ zMk1dx=b4)jpG)Q?n-Djx2VOmxkeAehZ#_g4Zi1X9P_VM(Rp4I*FP+V1%2BT2w ziPDVH!sDX6s1+?B6k`-)Oejw?PYat{#N=z{s|f$fHTziftF%f1FEQ<7x3?i~9y(m>GrI={_XhSU1on$@PO7pu^ zA4Wh`pY=hC;NUXrWBfEV%>JZDF1lN-gh$ntbXGq&` zo?lNLBu2SPvjr&ZK4KJIH21N&6*OB3&mDMdyJct3R363H@$A%yJB9PY)v&#gop})~ z>I92Upz*uAE=`J=zIWcVFI1+;2Y(pul^Zt;)gO*LM=dg=XkN6+5h*60&Q~QukFb}I zZm%$(jcSk2YdWco)E6ruVsflTB5dR{_6@T?bTwSrze0F*?cdtBL%E(GTwzhgvO01%I70eL126~b^B_Wa#gd##BSTuU@Xb&PQ zhr#7!sW!C*{oCVoeHn@rRy->m;>;<&DZ}tX%0rB#0Y>88PBE>Z-3rFo#gy#Z3d0EB z0{$(~$HDr9VFYMU6!3 zM0-3RF;fAS6`(rFiJ3~UtR#HzmgFn>iMz$9ZhQ_V&ry%(zkY)<#X=JxFI*KZR_zfiW9RbE}8Ra;Z}g@(gDES}lN1{MkW z^yYy~@tjyE>KI;(%CZ#04w4j;C&*I; z9u#E-`7BzlvD_3}+%LttyFIg_3Z+Uph~%Iuw3O^BCtCoY?~yXfb{kQq1sqzy9Xqu$ zQ914d55lcr-wJM+uf0DRoN-Wp^B2l8%bfGvg8PnbLDQG?3Qo(*Ny{n!#$|jv*uZ=w4|8fyy2?(QK-%dZ-F@M9?DacWFN#6ZiV5k z5Q7<)Kg)YZj)*k|Bh!^d`~n3Sj#DGp{7vy56M#2X16Qv?O7BaGkdLyzW3 z#{Yn-A2XgAOZnOKY)2dyD5ej1eV~m`RCr*doU$XWQmuXNlLfB*G1#HjMtQjj+;k=>8?I`#Sj z+RCrM`4za3oK?uRE9BZ^B@PcxoYZsIx(l5-tHHS%yzq#Jez=pr>sfLS>Il?;Zw-vV z)}+w6*!v)a;yH#KbN2CGNnbHV^-z*D87~f^6zNzNjgyXNQJPf7q6yN8ESe-`STseN z&Z4Q(X)Ky9ox!46(%CGUBh6q@rgScg=1a3!lr7C+QLc0Wixx^3u_#}1e zP)!C_sA0wCKsf{vE(T51z+$tg7!28LRm^@~0h(x0iCh8J3b4V9U526<6OIXM=O{mq zsDzA*nv1IRKn-ZKG)ElmDQ2d|OcB{*S!P+Rj~_K)QUms6&6~lbnVb@oKA?6G^sx_6 zsFR`ALrOFXwLEzDKo9R&3PmB4GOj%iOTp=lHRoFfT}36in}wSrwyBu$vhZS&pM@Wb zf-Hhq6k-vgB3$619r@||5#Ess;6{NZrA+5|R$hR~l`&580-l|IWuOMv_5lRQfmF(v$!K{+~d?ol) zvY)R6|4R1rufXILn6hKKNg>x1$8-@>$TctIiZR2(Mb*+n?hlcbX&%k)+wxL`SYe8( zhuC`3i&0h_+AZw9Z43CdzzCeXDCQdQuaUlOXy0J9D2ezH+G!G6+g!s~-H`1Da4hox&b%O9O+Z0tu* zELU5u`V{5ma^yBvn&EpPVnpmFKnr- zMZ+$N&t7_`C1Mh65(bk+O>j+6ZE_^@PViyL0uxkiNhBMV5K3faU{gjuNr{*;a4iGX zHx4Cxmw^v)i82T*W9QH^7*+WI&J5K(oJr(z3b~v~ zE~k;p^vtOlIS3uHq+0i~^@E7a3B5E&X7o0`z}v>okF2hh>9EA2@;({8j2J(Lcj z!|1_u2t9%xLJz0IX$kE`d(*zOAMH-Z(1COm9ZZMP!)PDcgN~#lXeTPiqHB%5$t5YB6)D#1c{ke*K5D*x z&p=vzLA#?>ZuHi$^S3RoM+bIpSnc^mv0;C<-pc)J^lhz{W z4X!&x8!iZ$m;29H{e`oG+M{_39b0Ns`c&s{8+&`~J!Vz%MnX^8IrILZsMCp#E4L z`10dLvo;9+iEa_>*jxE1QakT)QtF1i{OqrEou)tCDr^p%Wuzt3sb>p1bTxOsp|{)$ z>;L7Syv0V+LpJ_Lw(IZu_))Bd<3Lc`yQh||IkigcGQBY8!;@CMsekt520a^Uwpu!2 zYgdq{H4PX|p7^x?$+I)}n?+$OA6UL1m+nmN&Dx|(9@HlYFT)|CxwL^~C^KV%4VQ-d zae7FMSTm)WnhRj}hyxbo{W?f9lrOR^>@3{40Jep}-k6W7 zdV~yRh>B~Z8hb8)y?M*njE<;frVJ%Pc7!y-mkVGwrqfOfTzTdU<$-L~Y}HaOfNg9a zJ>IJvK(-4S+gxd`<^tHCIMd->UC*r;DjfBWw25T70Cw6rwtQcIj+~+1#4kphnQ#GY z^@Qx8*g~RWC_mIYNEYPA1+eLiRCd{%9xmOk`$jL9avAkrzIrpuDL4H1T=>U zDGf@vN1EOvWvth9FEJ$K<+RY7|1&*ONU2al2Py7A6+HMJuPS7edb)4BG!n;9a{TtR zO2!4~e{ji2M*o{<<_jq=)PJqZT5r}z>f;1<#T}kQ{)U~20wRhaW(<(R0n(0*6~@{T zmim;Dz?@|DkIpsVZO(tZTu4Qr3gx6zIqAbHZIsSXj3I#|pF=v`+sQF!?K&RQY$2I^f%ITcb0R$-%Xe3h_oMeF@^<1-AU!2Q`^TP}d@ z_in?()nSX87|Ijbfpnml3t+pOjQ0AU3TzQl!Kl2e*mVXg=gTEv2PRxmw0I<(7Z5(^ z)Qpq1HFc8}h&<96@2GL$n~I}6&vlnYRytZ!+Ig8upkA*DtG z)RIoMs6v2EfGZ?iS?qZ|%Ini#coahrtc!t*DHjM!g0LY7G(gkoq}<65G`|XnNHmA- zlyE!cz$OS290O6ONVuYa_k!@~g?!pDI z&5vLI?UC@WF^1yd@!wW24+bVWmqyTYW_7)1s#@ zV8iGzQ~16FY%DL<_|;v*#`(snUcDe29~wXV)rn#gcnQX^w{OLnINkqPrB4f3xwYID zP9lEs@>EIxc*TqS{)C^O2BGpjR@8NwcIt{aMcUg5IRHz_oILfr9(w<&kNXa zI$Q+5HUw-09U+1x1K6UuV$#A}!)ub399lsJv?3V;6=VP^k}*a>zICF}!Rq1uICOFq zv_b^;tAGul14M9Jp_X^R%&C@=R*GOD1#Bc8DT0jwDx;}lB*Q4Mj%aX9VUXcGBAH_^ z_mhespfr*(Zy_5N9M#6K1sOPtWDHr@Nmz6wW4J;#tT&P|R6z!wA{oOJWZ)>0F+`y@ zeBVgM@PusOCXz8UA-lGJ+onZ#%6F3j){pPU0004_=q)imp$HrgR0OjVWax#mF)l#{ zT%v5uN|0e2Q8tF-EBPfHaW*0&$N)toVX)u8UD+_mm>){o`4Cr5gLG_;L9W! zA58}X@TE!s`H+mU2U$QoBxBq`Z6F}WFzuMaAdZXQU(taoBZ26(T{PVo$ywB}C z=Q(HI?1F+?y-1W>KfANJp}j^T5{Y>HZ*_Z{BUUUDDSSkt&OL|9OIL&@tFOIwe%7v? zfraL`UgMwfcQi)4Z;dfNimP|5nV)BFPP=Tp=RSY#_zK?zNn7j0v&O#e9KQ6_@e-M_ zyR>CP^y$W$YZsT5EV^J@6TVRP&R^c)Tip-lDz*LF{@fTk_w>^BM?c^G(coH1Ucr^I zTi-?vT{tT{@i1hCvoP!Mr;Bf*&66Ki?>n4*_*&{K5&Jp+S-Wxu^le*~sBR6nem(b7 z!Em_-2|w{GZR;v{E&jcQ`5!8Ve_c~^yXRkf*L1!;`^u}nW4UX7XFK$Ep4b%=tiC>S zExW6t;`KFK&knWrzxu>3>x`BY)e|qaPr3Z9TQ^0K+nV$Dh@oB>gVXMVk&)#Ut;+Kh!FHUn`T-KXmcWt|O^4qVbssCMF5dZkbwJ-fn4sN{g zD63!E|Iw*%<#$~Z4f;EOj5%}a^3Om2{o0usDPgDoT9G&Ly0)@@mpd`|!};@{S!*@? zsYA4}`QrNRP49fm`!3)3(sr$+ZTaBG-3w>#dZVTE`@SF=W`XqmVu-Sz8t9iRPTC>-UyrwttgzExGy5eZ!(F+pYZ@Onpago^jMYEy}2|yd+w^wz+mX zh(u%7x_=*j?>7Nju`y7Dd{w@I!aVgny-0?03V)K$`4+vEv{kPbM`5u^WiklUPfTe_+GU*7K@8iMWKR4ZV4o#%kBluXnyMb5w$o5iw8k92!TSPJW=BrkN-q3G=_e0 zbaKvVwKx(#5Tpvy3UTr{rAHQw#l^3*2K|En;HFv}jm4v&8if!cNuCryUhqY1_0aL= zg#Qf^oA3iFxk@2~OTwi@-21x(`Qf=|zfp@Luy{A%zevHZwGzqD1RsJB5=R~#fq)|r zNchfc$qPPvu-Q+_A=7MAF-cVpZ3oqMP!o4>Xb0$aKmg@YNFRj^G7X0gLf}Er6Mir1 zDd(mc<^C!G>C$u=BuNBhC^pO?DJLL%mOY1rNkBQ-Ik}Wd@=Iou z=|mX;Ibj<(;RT?-xS-JmO_ckfxDVz~c7xpw*~Ci%8UcO;^pwZJJ`UN$9045z{unjy z=%PQR{L1C8RdGnC)M{fmRNKhVlr=M?gt=4CG^=BqnfZ95mw)K=}wLkAQ~qF_0gl&y#gFCFF;K zRu-`D^R7kv>jjiA$>(U43MGYJBFO_GKNLAK#D4PX$GiNW3>I@JRuLOOqK^s1#t~X} zz4epJrDb6<4yA>sMUoeAC_N-ypkz<6TPfx1^Ft^tRxj34Z*Bqg7GQW3tfOF~)*b}i zLF$dO@0RU+`Nbn~VgXrXmLOt@fD&X0URrC~-JN%*!3+X2s*EP0`WI^Hi1gEy2}%wv zku6bB6{=;`3@w!{WvE70!%(fPmZ4@@Gea%17KT>JRx-3owu+%vSt~;aKymPKJFQaxM{9*;q-e53!9yW}P{JxPwE>^~*!3f$i3I8>J3w2Sb}7 zcr%0&=?gwrn+IA{sn~TInI?uTm`K~+Gjhjp8gM9566r_YIQxmZoC%?C##+mcIO zkE4;7@C2oNbv{qRFQCfCN@l%RHC8d?C~y=JRip!tOv&p%v*B<>OQPlEogA`+TVlxM z97+gJV5q`ekwhloP^Gz&p(=9~Lyd8b3G_)voP(kE==OLj>xk}Ps0Si?z(}RN5Y-ED zl=}hoLnPrR4oI_g8_wIY5mAyTuj8;0dP^8%gg$|xipUD)g_V(&3{^!|G1M5=$f)87 zb1>8%+U`|@Lmi{ad7{?yW?B^D4X@s{Cf ze2JDsjDPktyyWLU2cG|aU3U07?B_ylVIUcdlcKPrn2cF{;PTn+&ZS*a0VzTh2I3R} zrK{7Ka)EnL@4h0SEOizm%hBg}WpCy^sOKL4W;57f=gD^j7T)i}OK_TcnudggM>DiDbc7a4%JL_+-5tbZ_5S*)WNh2q;f#>S z&3p0BF{+q&@-s;}9DL7lh6KOk`J7jyYY)2d(}G6uHV(w^_~{={H{^MXbN~69lXnha z8x2Z>hRn{POl785PY%sg&h*mvCwJU_x~}pdeuY}5Mq~xuHGK5oj#+mM{~WD~Hj|kW z|NhptJ^!OVCBdz8vMH6uw%os*Y3s_P%429$o(0{rw2dZHVdXt1FQR((Q?>dvAM81Y z%S2^E<#IZ@s-cRZy&%~OKEweW8iF8v7@#~1I(!%)oEvs2erQwBdAw!#%ly^EI1Z@| zY9l3$Q8Sg2R;Fd1)JybC=?InwGhP+qg=At5-p!{*+9*X$jbbR;5^bfj7*k9NC8ODx zN@*j+ZKO#XJXvsa{rmSWP7zRrq#}ZN1@HWlN`|T=RbJ|R^W(p*lU#b&6QXESJPna) zx+#a!AVdyAEO8WvkGaNCFq7hCE7v{wmzKlt;qXVQBE6aa*1drZgLiG0e0gNZFxZLK zJhCU+lS$_DC?_$8At&Ir-@8Hbs1Gvwz)nozksHF?5J5EIanBw$4iQ9?vDX{Y=8OM! zUCE)d%dSy9tWq!1$9JEPa5U+fmd%&8l}1+ zjrtp(3rE18@;Ic9LmIJ<$ERE6G4LmR=^wvcc{=;*1m5y?K*SE(z%Bmj{{H(fAHItZ zX-0{WDFqgzWhz}x3C0A5D(n@xL{nVP?3D~v*{c}pfw&&J+&$f5bU)2%;$_hOaLYtu z@nZC*k<|<5eug7is4VoBa@?8D>L?)Z%``l`u)?S2A;JOE>Uc604w%-!(|~1aGrft0 zlP=8eZ6bLzQ#+H9<%H!hvJz8?g@&(GS?X;jdDIQ6ZhDX638vNwJWdBT=_PT(jTE0 zSjXmVADFVB=@E|i4hY5hNfhJ$>)w#`rg-)KcJ` zAYVkHaPL<@r{?O8PaAK3S|WrQ!bO5tBC2byZwkVw3kVQTkw|@fWpY0Sr&MXxLH-)x GMgIqBpVBY@ literal 0 HcmV?d00001 diff --git a/.cache/clangd/index/NMPC_acados.hpp.29D23C04AD60A55C.idx b/.cache/clangd/index/NMPC_acados.hpp.29D23C04AD60A55C.idx new file mode 100644 index 0000000000000000000000000000000000000000..c4754ccd0b6d25f937bfb5457578579e95628a28 GIT binary patch literal 3236 zcmY+H4^&gv9mij;3E_pu|{4YdBLIttZl3$1N;-=ja%b@O?5{3%bulOnQrs*&56(4`~1emB_99P3e({~bhKW( z-0Cm>PxeFN(l-moWGShMlMmMTBERX>j1DuMt?M>!p1ZO6yI0R2+u!7BZ`w0EKYM(C z$(H;z+uY3u&W4s$ZP~oxtM}UN>WpW#h97 zd{o>lnBZnIPWwqY9{KfHk`<07!T>AXN zZPS$JXPq3ZXuR=y@}J)rp?lU{eD~PHN3R|0S5JPh>aX692iOG-io?5Fm_=s$(L3^2 z-&j6eH(j-4O=0wuO;g8yzidv=^p#iI+&M=spNw)Rh2A>Tn08K5SrW=ecUo=W& zK@$oaU;-T2&TjdvGtNeiL3*&=q?-rZbK>J`yJGx2jUsXK!pK6E7{Ffq$;$s;@D;~t zkPO=#&*{Yg_I*ayCi`NAokpqHE;E+Ji2>}-uAGP)kL`BQNRI7Uk+T$H0DF4LGYOg3 z=DV~=f^B28Q7s0r6Vmr|kxSp6N~2_K=c;q{VgOs#{2;}(W;C5f(b)E?y}TH}E;uBc zq5ACy9vV^D&Qs^<#Q?Va%5{0uDhE;QsE*03+ z+*i8Z=k8W$Py}wx^Sn_EU>9pzzigP-zDSPn)nQc;s%S9~q_{s4R<`$@j2G;Weo@V! zL`ZKVZE9%0ZDB)CY4_GT1|@-=5t*R``|9^Sz42YjZ8S3C-t*P@dNF{Vvmv|PF@XK%bNc(o9~HgNpjb$rOeRNzJ!ADkZq=tP z-88b{)}?%@UJPL8#0^Z@yI%JJjiRufWy(qv1K6{tZ|wW0weRl?io>=w+Nu!)*e4W! zk_^fEhG;}$+h_8b#Q=8Uft9+K)t?SAC?1C9B%K_xyx+Sm>)UGQmWus1^u)j@ zDArH0C5m;XIiG}G zqF8sX+Y4-?*fPFM4=|$GkT%4@EvQU0e4X68pTfNqNDE?M9}~VhZ+vC!yc)~-(tH}O z6pU2Dix7p*p(88{@B|s)i73_-VPb)23K5}D2@s0sBk&VsJRgVC6`qez=i}jsQmil5 zX9$$A?dmKf#{}C8#4; zOi=ZwgG~M33@O$uGgDBN5+U`olG4Hh$tcz;wMGFvaLH)3KnadN9<;z`NC14svcP4K zuNL)`v%wu}@j7uP?(sUo*}@AVuo`N^0mZVwXpj$h<%5UX`kQc)ENPYjmGikOP$#o>1;v*aH)S))a4a)+|L59VbC`EpQT;F@`#!J$H ze#|L{(UA}ymowg(7`{jDIJX5xiN~Dbo)#|oNFN=(Ri$Jp4I{$@AJ#s{!MIF7Xlw4+Y4ceY*?~o@%|0mmpV~tAv{d+`&nN#{+7^yCGp&;k^JE-V2;t zcqc%H8-SAtj|9j7R4faR1IR#RoJ4pJKwep&dCu_Rh3}D?^&9*~$f;(7reGYTs;OMI zKsX+?H5IiRgb!K>6F((<5JXJ)Qx*Oc{^Rh|PYWLe!G%BB!%c_2EvR~UPN7n3b)1Hw PW0Zt0>}$fGO!)JEs|c=R literal 0 HcmV?d00001 diff --git a/.cache/clangd/index/NMPC_setup.cpp.86F9F0381989E206.idx b/.cache/clangd/index/NMPC_setup.cpp.86F9F0381989E206.idx new file mode 100644 index 0000000000000000000000000000000000000000..ee7f67abe0f8bdc2e31b66d091ed7824685d5605 GIT binary patch literal 14226 zcmchdhgVcb7r>cuaoOc9W%seWz|z~&q=+3AR8S)h#uB5L1SJ+gz_wsw1rdy&J&F<~ z6oY~V>_H@g4N;>3NsuU_*hq{q5n}B6&6@MxT>k-cPLlm`Uzs;^`^@O!k&!EVnV1az zeE9sNsq-dFOiWC8`tRI%bK>uHF)?YfFfp0G?T3%9r`+mo_0zz)(c}I;vZ&9vTj}`v zMTgHa_Wja-LalAaxU>IWB@-W(R}Wu`7g*tmdox#rc5eOa(W+&E+{wWGMJD#s`lhFT znWoA7q5X@H-x{vyUq@~~H(%NK>%w5$=(x`f{SK0VqJs^e+pWH}`N)`B`Q6PPzB@W6 zj<{F#xc0+juIRJA2P>{^ zz1%v`t?!D+;*6BMzZPB1J5%f#O1j?ey(aRr_$5Do;rP#jVc(>-&kos=u+*GAqh_V-+MSLKeXnwhtwD-L(AGcB|jWKP<125&LD zHegQ4^3)txk58(?KYv(1(dTi-x!fx`#bMw19oRR^W$5A+sbPHzHznGX70o&~_M6y4 zm$v)fX*+JX@%-zVCG!?+n6kgnxu02H^wrYye?4?OE}1_1U}J6nX(vuj?-p?C@FQ*X znDmj)ya77-Xw9DLyX z!R49DuKm7$%eB~_CI^rDR=z2=#87#DUDc=+|HN)yb?S>vcmKKbRsRdoXztBFD_etR z`tDh#v9Ndain0woyJPRE@(G^t$ftCRy;opAHytCh9ecp~i z#S4lL|K_8}nvk>Ujm0IeE)9krH}=^-n-OJ*PO#EE{q@Bc-=>yMon|O>zWUXW(OYk4 z{%A6O^qAOBaL-ZIJ#Ws5`#a;)rUjDYJGy1{rc@MA7KK5c`cc%yIqgUVQneTS=l;iCsp`o|ewhUB$8&us_b~VK7 z_sve1PhB$`8p!3G`yuk)$F4{6-~8HtwZFZ_u*|0F8|(2a`glJb_jHal=kT*_X;;ja z)Zbk&TK3(|wDIBk2O7^a)nWT<(DCCZoCb89kDIzT_CVj*qM*?W|qjTj7tXxCQ&S;9gN?&SpNs1?v;L zxF%*^y=jFf(F+u6g=>^tCKn%{@Y@fSu7@puv%({&I1j7yuxFG^CKHD@XX>=MML{Me zH8F=i8QX4(BsxMIh>I{R9T{sk){#++T@0Wp4pYP{#}cL2Z|g%%tvI64E4m5eqBKA; z1khN;L_l95#aaZo6e-F92e<@yF=?PI(2`ML5VD>R>?8cG332ckyJL%{VoqNj8%J#cAVg1=%IH1uHTJjIl&IQi**l zaey$?D}@iDRV!+(5Kmf=rUhw*JGA4Ge8|iUYY9huCB8CYa2%Penk-(#ktv!fI^hT6F+l5D?zqq zHR3jDX4}$dcOXdz97HMBl(Iq0?5I(!cRy<@=g9)c1@6LFd9u)cp{U(F$wq_ZdtLUD;C%mIm;(hJJ=nCQCI<1y`nOQblX!Nt$UIkfnL0>4p2s zNO|s?^I=Y1I1(TU=qe})N0ObBU4@G{lH!~q(gzmxrF{&|&eZtxaAY1HA?QHbs+6E- zwOibz9O)+MX2zZsZxOF#&x&`A_hgiyNU#RvngC>p7Kz}#MAt-7*1&};CKnfaVxHt8 zhg{?)Jc1`BNLPZy!A=`}tIMU@ET6Z2CL87Bl}H$_(bKe{0(?jO%jD@M3-14G5+Iel zV=ob~*sg8ogG@Q%CGi3??=A5LG*LCtM))|6 z#Asqfzt53Ls!5{kywBh6(%|f#^l?7sKGtmVeTMlEM!v4TdPaV3e%_2)u(Sm$guG*B zdqvj8p0YkJ92v|F7EOne{a{h)ITFD|09k}>gs9jYiR2=IEYda-$eNH<69TK-g18n0 zu5LljEo>P%avgEkq5L{>z7FNBh--!NR^-`=L^bEg4aD7m@*BwW29&oUZ5t8|&lowB z9f)T$--(o+kXK=D6$ay7g`HPnQSCUg8f#Z$@VvFyWi1xflq2h~%R0y#v34W8J_m>9 z-~sIY`IyVc7EpJ&0?37!D}-Ext&6aVUB4f5`=Pu7%PTO)B(E-o(u=aNBn!*g==WgDJ=jKgy2Y_COK)5%7(g9` zF*5y^Et{BdZ|#hc(asnd?TnGp&KMc(jM0B~Ch+6zwZp>_UkrHb0Ve0;cwrPq`_H5H zUo6_cS%S}QkAGqY(p+PSe2KX*e4Z>t@}&rHI+CX|7V>7scRChAf=I3E=XTTvJyTTdpD!_gMZ+*qj=;d5Cg64%ySki>$ zf`B7eu;dElW-Mujd>xx#$JT=HX#8?s_7A@nGGkCQT{T~suUwdCniIp)sX_^Uw{IHHPDel)VmP%Wn6@M7eU^RLbkK1)sZ^XuMYJW zuCKflH;+&9|4`161)d9hg+Fs-q1!^>jIvShY<3ezGt7^((I@QcYGhZ<5;=}$ly)_c zPawM!kSnl91@>X9bs9UIhRa-s9qJgz#!rsabQD^9%PTztzG8`FN>l zc1AN4?5X znD9|LvJds##}fQwUS0EA`*oj19m;-ezaKjY3vPdm%#!L7X3T~nN8*xkR8v&@V0v;E@n@mD9%<_lNFA>rn=43cC~)L1 zvb_u6cOPl*!|OYcQwMSuX27UtPMwf9VcSjbrZ2WRkPl;Lt=8X6)@t zxFw=_8fy;eQUs(+5x10?FHh2uG#$#*5tk0-*(iWEF4@zwQ8aB_GOk9}v~kI}23ga_ zCF2vwnl>&O-$JrmNG$1$?nHJQas}4W#-^a=#;zt$8=H*lFi#tsjGM4y6LfZ0u;UfT z&DgP-@s{y@qpi2i-tyMI!t(KO;X%gaaYg~2EwEz0lPccft1VVMO}oqHb-1m|L$9(j%Rbe=s ztL8|!CS3Hgv_0e&A=(fnj|j1a$B}JFw~Zx|#++tIIf@Y0iz5}twgToPN0IGO$koWL z8s8csr4< z6Y?tTvkHTnZ!Pv+i~ZR59LCDSuqAT}Tb{yJg5q*?eSJ=|aL~o1gmgtfQ?n~m5fGo)|N!)|sNivrVWXVp+Vv@|0 z6fOnGQXEsjeFnKf%z}7g;0@qw4Pl0GL2YOllBa?zQ+27J_DYkd0a=<~n!j-0p(|4i zeeW+WpIo_=FR7PE9{=p@zT#0~ z>3dNZ(&lkj*DH57-Eb(zTrpM(nx3)x&E$d2WA@NSMgg)ffVN$T>l(GcNAo-`x3W+bK}u@^a? zaev2^)3B9^xJ(vLIr0Q?PZ*C%`D$G0qBG^R^_GjcTv*EGB9B}ot}913IJgohFG21l zNOUtNo|-+@2L~PDd9oZym%|Jp3rVveKSk1~ke?yxGsw@8^f}}gNcsZuDnvKD)+#iEYu=oW;>I>S#OFb6<9-)NI@?2^19M< zB&G_`d+cr6n-@-9aHOAUKc2N52bvBPr_Z=OX`06pD2^VY(Y0V-p-J8F!Gk~64pE+b zFFq_c{`!sZH`Qlo^bV2)!N!4elBgPVpH-0}wh?0A7j$tU`D~p^Yl6E`VH!)rE|oJaYZbhga3p#uMF**~4_*8sBD z+XHgaI{|Xjy8#MUg^M4|lOd`hfT*ut&Nh{JLVa}>Uxc}SfH zxdi!_AW)W8Aie^2pN=B_DCA1SS3;(-;VFDSjSbHr)7bDFGK~!{Ak*0J3NAm54X;_M zXY91e-!OM(R1f)53<^#f8`9Z1EMu=!o`FG@L}SA;)?zev>g1UiWLR0)E{pBR()q-0 zS?nmyW5+JYG~94y27G8`yq zAWwVvj{8{)GI(+6ewPi6SE+5LI;4sngC*uaRyptJy!u|cK&!sXxt_A{_iOY#R6t-r zFsl>-wSi(gohMP+D6n;K0NETspcirm*_>ffh$n4G*TxbeBYU;MzQk&*TaCpkjVBwi zZX*^WDNm2v*AiqS`FCiCWRWUR4dFRJzH?PfY zdD~E!m5=8M3uA039HF~WmEvwx*^GZfB3^B7)$;Egf}_nbMZ9)`4icpMv zi`o7C4VKs5H1)k9(~)q?aM2Cu$PmjRfVLs!Hny*!qfSqm2e}-1m$Qc&chL0vkXV!J zXiveag6+)c$Wdf<6mli9s$?u!01bb}^R+_o`*ZHGBfIB&wb4DTT4Y|!oS2TBLzd^5 z6E)^smTOteGv-{D>mcV~pB&bO{@SwDChEzgThs}>yP;kS^IrVJbKf-)H%!Lgp;2=$=JqmYceQ=AVspv;J8z9?R6d?449{px8Pu3o ziN-XqpZ9@>Q=Z(RiG!cSPu!7jxOXz8a^TPH);w`lyNbJYJn5$CrWKaK*fMu4XB*$f zmbt?|Se?}(Z7tijqAfDit&Z*P8@Wdv+r8)Ma_P>&7}bFI2BuCtxs3SBkgp(~?wm1u zIgMqf*)a%XOHNh?`63Rzi2Dn(!P89zC%QAmxE_brLwN&sqB~Q9T=+j#zyHo}3kn!A z^_$wDhS?8j;p-{!1lgosq6aiaHAWmON5*Q#iYX-b&W>-8N#8>uAIcuSmrVHS_z@|c zKP`Jm>#7#y-h#Y@+0@a7oBMUfeH|a2PwYDR(OaG8Dp_ULQds=!-KO?O^kB?L+mT{7 zL`TM`$B2PRM<%K#0-9_)S-dje;g92Kn=~DC2(8Gc6?J1nxPg3bFs?W=ujhnq`p5Lb zgUItBbR>t6=OM;zJBih$Pum|W=%F{aNHKa*@`w~;8%>pwE|003ao2Y6KGr;-hpCZm zB}?Iq9ZKI5%$sp^v;nOwb{Y@01>AtV4*RZy9mgCTnS;f>586@1gDUV~L9L7(M%%Nj z0^_Lq`qZ(CEo5WxSbh4>PiW)t6>@sTwtIQ94C|J$T&vp}xoPj?H=n#0C$x3r)K>qp zfiK=Rwls7XLHHbFV@pGK5g0dP4c$c$ICz)5YFpyWm$b#=FYyPb-bboNisdawh=z#S z4EN44iP`f9{G>a7`4u%2dK#br`Wbo}pb#=W4NwG`o(9+sxe95jSeP*SCA&H}zeU)A zR@1`B=S-bB*?637?wq)}YmGm~82|I%Kcj>nCMF}<&++W%hvH8}IzwMwDv$YYsrl$J zW5<0o{=?`|Bcn_{5-%_r$9|4xKgY74QS4_l`x%$?`OLW#oo7ejE)vtOQZt#{oKsj> zT3M@URrl;Pi=Q?sMbTX#=!+ w?O<)NHeB07+eh15>#z0JhG_?CUA05Bk=mi!VOpKmfwxiH+YZ;*As3VX1M_btSO5S3 literal 0 HcmV?d00001 diff --git a/.cache/clangd/index/NMPC_setup.hpp.CAD16FEB6583910A.idx b/.cache/clangd/index/NMPC_setup.hpp.CAD16FEB6583910A.idx new file mode 100644 index 0000000000000000000000000000000000000000..4f031ff590a1bdd32b4210039fed6ed331362fd5 GIT binary patch literal 2236 zcmYjS3s6*57{2!ayL<7&SM|(NRgXRz{Ns%SI(M0Uv;*jH4w! z7&RMlus~6BG#h-NmY61tV~LiUQaWZ2Uz0OpQKn;K25SGg&TrlszS;YI|2cEdfBx@d zY)*FeK8qv`doyR=jKVpS<0VOQ!e9BEvgt&YBo~pSc@3>KnPa|sR$F?LFVai$m0{I+ zy(23Ne~Cm(_eL)N_;T0prCXd6j-HydVPkjq;4bfkpU3?$tT>u7so#;`EIS-KdoLxv z3p+nQbN={8J+lh_-L>Adzb&&fqh-Rp`FrHFgqs(py}cr@t9HcI#U5Y5O96A=7nTj* z)MG^DKJ(tTk+-)U-8$gn!Pif%nQ1M%v-NndhRs)^H*cM3y;oGV?s(V2BHkuSHDSI(F+HBORh$2RsW zx_;KA5uXALf`_Y$>Xo2*;_5LFX;GYyis5gQx9WGt_BkBm{DR2kZu* z{QfGLu1zfUGU5g~$b)tRP;QFtu77x9NPv+zkUgr$W&p}_-}e;;xho{j1M|{-BlbzG?I+xrV4eT zy#BuPt>=PGqZtXn>QsMf!~m4%2KPH=FKW+c#09cTbvX?{`KjmwJ>BIl(1;T^w<)$n zlymFyb8-%^Dq>mt7xugBnf1n z#itm6^5UoK{hm=1*D+!RIn2Wj15lpSmvmkl*WO|#Fh1O5GPw+p9yLMc3H8%2ULN}J za2q4>u->bBZ3f`_Td^DUx4%z6rx6GC%@VYvqg+!Q7cO4%(-n>QQMM}9$9FboV9!tg zoO1UXBQk96@H-*~;Oe4|)-S@%yh9@lS1XG0c+&e8t~-|bU1=vHDy)VJ2^xT_uYMBK zPQP*eZ$>;ICtH#g15nP|Zf$8OF6m~(1hVee!v>&SRyei%S8&@aF?(7^&7gz?%B?Bx3G4)uQ9g zQ_fYc<6Tg$shW;EP_C=Gj-965uG)1xC*=;+q2mNlo~owm=;4y<2bO)?Ux}J1_ftPZ z2T-0uQy6-Y@&FAm><{HZ8f4fO%0o27(3X^kX_%oCFlOL*B4hA)7%y-wB36*bPl9TZ zDM(|S04=fvX^aurM8pTu_{mT$vIA-SKA=TrAdPVWw8#piF(!Z(8G$r52WXKENMkGj zEiwUVYy!|C3y{v8wS0Wzg0|f-AQ2jIVvA6QY7q^O{=rBRW&+S65U>g-0uC=S0O{t# z*DAN&8GlX;Uy_oB!-w{OY7q%ojROIDi#$LYS3$Li0Hks3pj!AJX|z7j!uLqyAObD? sj~7Je}f@07{o9`=*Lep`Bb#ikEXQrX|iU}sIYcTvs-OR zDJ&rtSwGuyNtBkBY>GpG2dUFYX{?)Q0~`@Ns%J&~cSR;|H9 zP;hLhLb@|qf`cH)1i9tOvR$`zASl@of)qt{Q6q_ue8~MX3pD2`yDNll_|_SNeKf`FB>)9SHO!J+&KU=1&TzQ&ZC| zvPTk6v^?xOmh{fzTKSk`*B`N-W=GeNQ_k8w@Aff0(mic`Ini%tZA3=Yp-6t{z80$= z&i0W?+b+M4zq59FZp`=^sXXIVnRiIJr=`*TWtp8Gk1oCGmi!kkeN}GvGWlA|d->Pz zr%OGj1n;U;exYJ9R(98_au zc75Ppb^P0Y?z^GLP5mXokLY>8eZh{;{;5nGn5WiVR0M~*Z;>x& zX7@FwRvwz19FF>`)YEmSTIICygSdD6D=Gi6yr#2t-e>Qyh_^W>_U*?Q))=XoCG`aDLI!W6JG0yNc_$wyq}D zf(sr&|4L3wOjX>lJQ>Ff8+}>h@Ak~eN6(cx>j&p^_ZkQ`sbY?AeqK9Vtayj~-&WJ@wP1w#46_-h0sQ%Ng+P zA12&viYBxiY;frPBB^~{9bu@0a;N!2%Scha?OsLP0cOYfeUNj^^~*zE3!5CLe;(cF zmS4G@Jr_Q!Y>am@IXjhF_Z>r*!>w<>G*OUOlJMrIv;pxjb1_a!CSh{Qq}t7 zKhpL!^8HA$9j}zQzt7ZV(RRv+=d&rk-a{W zGA4mua3vcKY~xzjw|_YQa2Wez^eFal@yB}`#_Oj==Ypc$ax(Y))g%^9QKKII+skuA%=uTa{Z*%5F-Inx0jN>SZP+)q^JNy7!IqaUT50Dml3?M)+%di5-*%ggNw z8vGORH)c00qLXTb9TIxWXy*FH*>OmoCf(kOoba2SdG_w-eppY&t_z1m92&<~NFtCJ z5b`t7T#4uFYC2!5Q_0vh2uz?6?1e-E(G&s@6o$Yj`frMqZv%?R*#96fkH#|>k_luI z8tt7L?mL$dbe4==i@+=f%SLEOG^C+$M6T+ocyJE_MP^y>LSbvXJvrahDEkSTVr$|6(*1V7{3_ohDAyd;YA^?I9GjCN%V~|`y^+CUtw0( zRQ13^CfwO?o;@L}_+lu99wrWCF!Xes4bU-)PS^}Iv zA8mvVJ;*scI#7x*G{76c+C?6C4;W>JdJ%@2j-m{6j9uPsPqY4BvpYN30bK=)+;Q%o zcZU|jGDTc{t|6Mz>{Ij6Os5$n(zOR8B?xf}ow8`JRAVYro7V~FL_%i(l^EfqnG-Y< z3L|dKe4uVrA;hhj3)Jm$$Gk>2asH-8Fb_CTTM6I*MQd5&EGcLPG*k~35p)TQW`e29 zq@s&p5lf#%Ln*={wl15BGJ{1NT@Dr10T%Iec~n%Ss%1@2+&5505I!Ul$xO?DY)oFH z!&2PR&pJH*>vfx4ed~>;2{agJL)>s~pPK;5cJ7vFj;CC4a40r%O zum|424|oD!AOQYgDR2fH;0hc;FyMkf;01WV2e^PGU^xf?EU*Hs0;@p?U;+yh8r|G9 KlwpRkf&LFH3U9vv literal 0 HcmV?d00001 diff --git a/.cache/clangd/index/PID_setup.hpp.486A8D0A6ED861A2.idx b/.cache/clangd/index/PID_setup.hpp.486A8D0A6ED861A2.idx new file mode 100644 index 0000000000000000000000000000000000000000..7a301cdda0bba7186f497155175080db86664767 GIT binary patch literal 1310 zcmYL|c}P@I6vp3No#pw)H*fYa5x2?;Ev3yiLd7jxB{5t|M~%`r3N7gmwWvX9!AcO#r0L#Uryk5VGv9a5xtIGpFE%k`<_|owDU!u#;&upnJIDWNx z=4)E($tsCQG4ErxPE>l?`3?7+0$PiI&gGZw>Wof}=q^bfH@2r&#rnl=3cmMP44IKFGkmvTf1i058Y3! z&up*rOq?|iizP`BK~3HtFCE#({+i$TL6+$q_r-HZ?bD8-w;z=)368WLeY>M7=JwtA zsEP`&Qzw2njG3Y4dy~=?i~2K{M>Y+m1V^Pb*vwhSV``@hjwXrymTVD)mj^%07`*F9 zDeX-je3HH?y`cAch^=#v=8!(vXJm!RoNZL%yI>qqhfbcY4<$O*?PPQ_D!xZhT{An1CzHn{d+E&T6XhDN}A78c~F7aNf+uL6CaYzBOb?kR_{s$;+&2fZMMmY!vd1P-Wo#v59z1IKM&%mlB3ry*BWjKNCUB)mH#_= zz<_(3IA^Y(M{2ychSRuFf$Ht+&KDJZyE!ODc(!FyiBv;{Vd+L9nYeY;ow1-e)d-IS za&}IqqXPBz;mID;#LFojA#yQ-N=*e~CO2cRsSLjMG&u+5oo5`+sX>`3Ne!BquBi+$ zrV>#M(~O#=L}f;=nbhUdb7pdjOcG|g&0TWW+57Gve*6D@-~PUDed~Ly-?; z@Q0tF(CUh-vRlwMp_OO)*4gY}l9l9GH@UiaQ^kw28?RdyJ-O5Ic&w~C?~WoT#{G^s zuzyEv&bcB<)Z~)MRIlrPtzFi_*!1}Icf1##T5q$>CBQVM^V`IUvgKQA2jpkIyD#ra z-s1Ig>9e(V*NktxEaR&!9)8-{<0cFL;cWFlp6*}Tfu6f`{xQc|S=pstZc_HOv)JsA zpQXUQckPnsN8+!&M;oB?VAjIwV3%tzPyUs0Zu@sppEPW)+dtwLez~T>H0xyij9!iT z7n$BuppMQDcX5U2M2N~*2CfM!& zc4)xS`)F8=G$j1?ZC@Y@O4yiszOZLh>R12e**Fiwc{WADHGVz0-^?yw9+%XkWut3! zWXJVWKR$a&4;@PkjF*pTTd1{?;Rn4r|H}w&-;v!TLSO9n&M&UatTzS(!~*~N?In^V3ww*0Bx@mTCfsZXl@Q~dAV(lq1XD|U#;raK3w0rqb+~AZl-m3-5b}}?MJP8 zha?>l%$u%Xk2ZQgdS+Iu27J;pm5~<#L3s7K>L?SF^e93^5KJ|kQw_~!AqF8X#B34} z%fvvY<~2^q$ytj{DZ&(qIFrs|S)^{H`#XW9OOU{+@Wj1>VZJX#m?423klX=HWdXVY z&ctjrNW2rddG-GG{G+uBidcw54nX<<6w88)g4_vR5;(h9Oljfk|B*>~lu(2r5~+i8 z>!6h^)F^b?kh?V!v1u9qAt8Lrw-lj|L=M5(hoFgUgK&f6J6~Kak;noUMTCW~&vz6d zL?VrFRwFc)g$cqO-idhFAd#xvIbOHBGcHnu2@>fAWG@iP{B`~9*%dvoYTKhZXg5F+ zMo7ei^bpC`>#mou0zbNDwf-qAeoherBv1usRUuD;1;Nvvyff2Qx<#j@*7FrbEJ7l7 zB0Dcxgd`%A(48@3rukyz$E&Js!!HNY$0))W2|NYlQ=l(X=ql`3GXraX> zKqBQ(qZ~zcqi7?FEZQA6E2M=;i;hXRnnw`9k1F=O`0X}93K+=(i4dC?F#ewYUU)$Q zM(!t9ut?#f@W)6yOefp;=0gAk%nLxi0DSD=tFNRz&;LF<6EU6v@)^*^4C@8f#fgrM z>WJ|R(EJ66u&S@GH+@j(vyq-bF(1<(3o)5uMu5Qxu*Kg~ObO(dz(qJaLfWf-`?aW< z1{C8(da0wf`R>t9+k4RbAD(j&xd@O&U;);rSuv>Ay(ycA><)k zCxO-^uwro`qzj?Bs)U$AxU3Lvz_=jFWI;l?-U773)4=RBaKMIe-2%P;6)g@Hu2#gD zDpgE2jj~!@-C$*gVwg#0&c|vFoB#`wWR7M?f0A305|dkgzS!&V;H(5mXU)#yVe^QZz~lllmy34;ayJ*}0Wy!pbA;sNm%sJ9p*Zye&3>SV zo5<^?*0a83fIDJD*hIXK^%qQTj#je^T8_xi0r?zg;SJ{v*JqgM+*^SdsZOa8SYFF% zq^tA!m)>aMML=8xoG{~1dbE}2WzV%}>EWVq3rwb%IB}eeMJX03QjBo!pbVJq#W;*N zAIjgjwV2A0?L>#fAS{oo56MqAW-NdF(Qe*k>m9{Rit#t}UxwuoTIZk4BDuAE4F`qF zx3fqgR(Nwzup$eiS+<4aAE)mLKnpt#^o|2nHc`w8pm&0cYk^)Zi)}uZtZF>4F91?Z z1t2TH9ITN-4U#B_vvAXtNi`$Oc^v_0xzWzio3NO7^5D##FRq0na=3N4sxuy0{^s0CO2%iEcoK6(lVzV946FWnpEwnlS zz3`q=Xi6?R1l?7W6UE4^9D3pyQD}B8s)FvgU?|j07FWX+xV=zl5L?$lZ`E){Bh{o4 zy5TX~FeEIW415rcf|=}|tV#;|(}&xBdPb%qay~H52X=TpH4jod8ZY#vq9})mLOAIh zEso}-v(iGTO6S)*14nOIuTMj|l#a?Utm%A$QF2vT@-{?{@rX&qWShIrZ9V4(x1)-$ z0%R4Kix~puJRr|=aVH=(2*H&lE8G0J)DHCjw?aAop`|F(8Y%xCD?TTzn9a2f6q#AP=*+ zDEF~tO5Hyj(Rs(8_SeVuDJGZ>=AaNdgo8rqP!0;C!#F6M4(FfEpXppNwmm7(?|K&F?LNvnadnHp-^eyw$4RpqO2dzY7}Rpt{C> zpRn5N?LDM4#W6)S!Ga&(Jz5jq(2H7LBcK}r=bF?HO!|Qo?-GTsT?Ws%*QQgDe+n+b z9eGjdV8WS}J$*?3P9WL|tg(M(zZ%uo8H@gfEF1y!5ukE>s;Qa`09)3=b0ByQ7PHYU zh5S-zh${grMENpx8O95{7uCu0>HKT}g(XNc$QVm)Ie8%Ore5U$3QIPbt(w47XA|N8 z)WBzxtwo=Hp?2ob6C_p+$a0{8zn?m^5QpGQY#kp?koD=CHn!j3p5U@iKj8)1rLG?U17ncY6uR&tOlyB*2 zo6<9;UYw@>Og~cmLJ+p>tBiehU|${CS3A`!VE#EM*B9(v{ZPZvNg|cl+SxnE2#JbM m*sESQDWek6ze@=np04_AjX4xOciwxNTDOa?Tx~@biC8oNnPs$CoZa0pWG>KR}VxzT&{e0_i(FwZ@=r~#A{3V?eo8N zKKj6Z)AY#sfi~Sk5AGNTWlzSs_j*h^{zqY4qTjkNDm40t1DkgHYn3yf&AC0c>9PA9 zzw*p(^|?24O4A~@hL^i^m&Yp}+`L@#v7sq^m;HqE{)Kxl9;$AogMEgAUyX{!bKSwo zKV9xKbT_}~>KyMnkkISxVKz6ZKv5CagQ%r(c3xj)@LU# zwO)17db=LlJ9XKzW#0$XJT{&y_PgFy{oN` z&riAWLFLQ6yA}sUPgC1Xcihgs=B7J#ac;o%)uXNlT$s4?ruL4oaQDg2Px%+ubltBB z4!&Joy(Ce)#`x&Py1_0*dt1l3(IG4E8W*HKJGJTc%`dh#9*$Gi|LAi3VX~(>wz22p zzv|Clek*?8s=rkYJn8h(7md1?a=zv5U3UgPS?;he{=;bFFYc%NHViqvS5|R?l&vW( z_!!1w|K-o;)~4n-@??}d5jA5plbIp*FfH)=w0KF$jE=VPJP8MPyedA18FVl1x%42m z;i!TqQ^BoM>9owC+aW-{%Inf~a0h@pMimpz47!tU=_ZtxES*HjxZ%@?X!JBj5gBJg zA}8C3Bgf6FXkIrt$Ac$9kUvv3Q_YMe|2dt%TM1lDQih|czzOM@MdqR)gR%UvL zJPCpPS*lrDW-$NF-1ds1!rEy(2?TeFDkYW~bWeLRx%=JwAL)2v5AHCBFaatQ%%J=Df|AW{{Vua9nS{k+%{I-08R@d{d?6rx;OREuX7!(WG8!sS^J;fyEETx8 zKUqWdJ1Kkm(ix&jFbyA|odWsW3SDok(stDIghGCZV~8s=m|tqDXz_pVR3lF`;7(8_ zXqZ8FUFE&0%a473k|&|yj#b5~nL&5p+2<`w7L=c+WDH&}+ao)IeVhnSIDKN2A>VIy zYBMEcF(A(^FPsJFydYrhpLBmIDtXq%lVCU?S(QA489ZQL<>;hq->hz@WCG@AC^AA` zef@dP4V5uE`BywqLw=kpPR|VHSFiZ1UAcbC*F13mceq3NL}t)^EzaTb%76NF*|F{i zIt5M@CG)EL>-R05N`3!xgD2Ai*4(!G@(E{;#6G2DA_nA#=9`FY#0cW%0o{MPZ*BiTZ`mLv z<1xUXFa*DgC|i?Vw5T>j2SF( z&!dO;cJ5jJKc4u5TdUG)m_avLQM9P+3JiK|o~kMHppDFtis>X>QZe?UVg|{uS{aDJ zYL!VctyWni%W9QPvaMD*B*$u%OLDDNMq;#DnTW}1l}GZdR{12~YE?iAEGr~f8wW? z8}C**54Cn1oT#Yfv;xiz6{EN)0f&r=I!-5G`=}VrMGN>ssTjk>2-rg^#&WR&vKJNO zxHti+f{O86ynx?^iV0kTfHX+OL@rUlcSpsU+)M#GO~qs`SwO;~;w)~KfPJN63YQ{a zJE@q;r3%=Wz>3hslRG}hw54JQ7cv?NnTk2`95;Mmr~|yiF#ns|0?$F6KF2~7Vn%32 znuQZZLx^Fvz0huq0s!XQZ`ms9SgJ8o)mGK9(cV4KNck^WOMS0G4zN zqfo*Cmh=l@lrDfJ-9i{A5$a}AW!@Vf3`9#Rh0*0lcWk;k>C;~LvKmgqBTK??%CnR> zoU7_;J{oyu_k9Qo=7JsYV3=8Xwkv)EoKHBHNu2rIM-6StMLV0GKyD}(%Hv&NZsjH= zjttHy}mE=pAWh)BHrbz$z YNM`sa7HecO069|a^EWdHyG literal 0 HcmV?d00001 diff --git a/.cache/clangd/index/acados_solver_auv_model.c.5778764B3A2FDFAE.idx b/.cache/clangd/index/acados_solver_auv_model.c.5778764B3A2FDFAE.idx new file mode 100644 index 0000000000000000000000000000000000000000..3b988c84ae8b5ecba6a34e2d18efb6aa21c525be GIT binary patch literal 12412 zcmb7~cUTlh8^CXOXLn{{=>i^{BE8$df(RW{zGLRy>)7mO+DhKy?Lo7U;;H6sfkRQk?Euv{!9}~) zyQny%-&QMiXy|p5`qy2PY{tw_I=5&MV`$qg{I80)>$?pwd15+bB5&_H(mvhlq|Lrr zv-WFm-a22N+5ZGzzIu*RwsTHWv3dVfcAK~yZ_5jYE)Ii#8lIXZ0N+_J*N!$y=yp5I zCp|S{^g|=T?Oki{-L7TFjYkExjX00m`;9Z?&7bwtDPGd{)%{EN&;Rx3ZvR~&TZ;>8 zv`SnmC;z#$<$nJKcRj-8TlHg%%B*@HZB^j^acJ4$In6BgS}mTUWpaIr*2UGijy%-3}J^v2y+po3s`rL)4s!8TnqUGfN$8UAC?i6!NXSaDpfz~<) z{ei#Q^)d3-n>QybJZ}A(f{qS_*C(yZUfOG`d%AXrS;4`FXV%Qy=l8_bVC2#tvql#V zt64YZT#j4f)my*xec`bvef1zi^B+nBx@CIQFP(qHdhi*3MqUTQ{KH==Qac5j#coMU z4fkF9i`;92@#LZJUK$_X@Tcvz_~5nLnVq^{R(*F-_6l}87i zSTrcly=$Ro=xkVX{C!}bpgZrDrZ2D4yz=O^{cOyXKEGe>ANH;)a>r1?>&2VGA$x-1 zywIn9oqz9hCEqVHX;$d?ifsohQ+tl8c06`>+_gdl5x+~bi>7zRa55FKNVY_ zC=YnV{NeBXw7PZB}{fs;$qLb0I?9n}@Dv&j?y3uWs4oTErxMFsaP* z^VXj9p!){vJ|BkdtI^AU{PA*`-R#}I14|#B-jseRdCv3ng|nKw4f_)sd%1)kX+5Cx z_{`_*mcF4sL{1n#)%1?u68q#&12&AX@3qixq`_qW;kKQNzHGJ9ZT9lP)0Y=xs%_>) zU-Iq!G}_)`#k9DGGc2>8X7{}jFk|Emqls=Yg+aR)ye(T5dBa?L?5&g~f|Vwa-?}d( z)@=GHozA5vXI$z~{(fJt=Gn1sGbPtI#jkhO_?NZafBD|p$Y#|CA3n?t@BDty?MLyu zpMRdZd{1lFLhp*%r}74#ExB{LUCq*g3#~r%a<=nt@!R#ar9Yc{rbhpg9slyL^**Q1 zW)7TUzVVjP_#1Pa_AEZs!#JbDf7956`%IttHWt;!9i)M)!op>VS*`2(-c8d=cI|PX`q+;Hx~Ws&e*BQ# zN-uol2=AaW5MO=KJOhn-uT?TG@qGNJW#I*1j{Utd^zX3v>Jw*L=IXyHo|~o_)N+Ma z)gPAcPU)W4{%P{>Th2c@A3nXX{_3z@SJ^u~MlbAZA=WMVBQkw-v+C7fzUbT!xRZMZ2u?WPX?!Ma?^)_ahO4Q|5V}~)myjgYFupaJ5^(0km zEOdsK&hReZE;zw`{V;#;tZi{nHj`9+u*hKq4kLHp2u2;j_^3o3(AZCvdwa8^o6?;m zRevmW7lOOc#W$2uhcdlXY6HZ6z1b3{$Upl&p5`p6x?rJbQ72k-^i5!_5||!edzv^1 zk7%1vJa_vqx^9xH0~VSNrd#^1P_O8y+-l;0(lZ@v9(R>gE?6J}f(U5$?Lvm!wxx9l z*r+o^QuW3nYdC%lXZ7tuM2eT6j*-`DPm@%AvB)k2b|Ew0a7G=@v{7!gaq>8I_!;em zlBy>b`XKNh1j}z1($8GuVT)eqbYCK=+^|Tn2!cf`-&jT+%XCt-%=O=!n-l(Eouu-@ zLSYDmAxGb>jCyOs#G7e1s_@;E%#F)ab_GkSj#wy8)Q=O}_$D#7Nlf3bBO3Pu){f)5 zM;^gDrbS9B4=fZf3h|<~Z!%+^%yj-LG<7Hz8Uu#(s|cx#kyOrDXq@@DF22j#E+4E? zOOmnA04(4)bccu8`?;}_sxuad5~V27-Z!4HjAy!i?PbD`SZMqYH;1BDK5>!?FMaYc z#`qZ1{o931e&Oa`$809=OORAfSY$4kYwo*>QLkd!Dk{`C>trtLu`f|lwZS4G3?IVS ze!GxfybJ=icDjjnNvif(Bng5f*v5A~qh60EuGWXlSKs-3Wb31qQ%3BQRPI0+mj~|7sBrnllVnRBWAuM zNdTV&J*A^62&$l-(owXiiWaq-GCKAWRg9=jJZIe5#PX+`({%(%uH!)+j})EM@y2z$ zTHycz1PE07J^}2bIYR&$G?xjWOkoqhb>R+M`{-hwmhqsBZ%$Ojs-{9Nb;5`(+&9}Qv^F6-ZJ(^4TcBM3DK%ES{9U}=6``x6=aF)Vn zk>gp^t|>R-1z}XYIEgG0QIq|nU-wFFhNqSl7WQB}E)` zz&SDqq9ABW6snPfAqu8>GX$HVk+4Pjf9q zwKV?&rGKC~>7VT5usIqxal{-X(s4uM#*H*gJ$@%NIQ6azHu(hvE}-UQ&UY-625uPk zrlBDNHqU_ViQCT4Sd}*79}`y&V694}iu!^4n3W#W{BQG2nxR_utX9?LTb@;%wlU9= z{>cs!54B?-N}233$8H z_>trc!8Svn`YRLc$^^W7Yy8+=N*sQDdR!+hNnUKf*g={2a>wOP3T zlZ=hBcx16IS*$CWog}a2oYr#AiVtV=F4?>*NfJ193odnnD{)0hE{87V(3QA~BxfU+ zY~)Jp{hNW;Pk-kWch%9z>ln0-X-NjAkxz5dY0iq=NF!h4z%`Cqxl1@uLi1A&Jf-;s z2VT%z!=W0krPALjfK~yDg8&MmITD~qnqvWorFRa=03_3V0DuDulLH4{2pUft#4LCs z(BK+$BaY{`yiKiN3ms3Knd+caM@LyblLFz%6;guRUV}eQ+E{LW4+XP4rQ+;-Htsq;3On z!+8YGBQxR&RiCe&s(BnRKwHAUaE*kW$S)+kfazR@rbJ%FQNuu9hC7dWqcQ+gI0DsT%P@q`vab80RjceRE-{2x6t3T=tsQvhjO-|oC|SE zN&Y0*d=i|M=!t~GBjIR86#@!4jig*9`wqC zgA~3D2VI7;!j;gj61G@oWNKoDDC%-w(%MFsBkK_ zLK?J4Q`TkTYIxmjtcMWGAAH#|=AV%dr+t?*_!ZOlyg2>mOzdPkS+J8;D_LhZi*~au z6i#PBy3%fVcjBD{qokSK|0lrzxAXt+(3tb>Pp)CM|B5{&nQ=~Lx{_H2bZ9p=%l&#o zLnLDrscaWZ4}|3WxY1+&S3McWFfGV08uX-&V=NU;V2l%#_S)4WYp%2&?)TlS)x&y} z+6*oB!?O-#P#|MMd`}}Ma#A9_P;EwFGcqK$Cq9TntFe|u6s(cYLU0zgB2Kj6)Pux+ zZM+xZ$rQ78#cb2YAjzTgyGg&Qe&;pvRu*if6}*iF+h`7BK^V>9EC{DLf&~!@lQI^z zePa!a4DIx6t9gmh^Ok)VfeRPyST;GlXeD-|2m~TfE8@{%y^n@nblbcd4?b7$%oT={ zyl`nr>d+ZOE!KY5m0hK|Rv!b;b-1fgwq+>Wj<^l!5|65}Xhuf0r@Pwx=ZgvJ@jap$ z<7nk5{mONNb6x5$!9atT22g?Y$kKpQ4g!@lS0PYEa}5GD3X>x;zI9_Mj@)|ll=czo z>oeJ7Nyh$)QtF9^e$vuEXi{kwh)w8kl=6JFuxdhSvz|HC$IE+n(WF&+z?-$tCRQRv&~e#PcP*gR+l+ zp2{ki1luM-Z{qk8K6SX>h5g7e1h2!;ybQW2*)#xI2cWJbq9p7WPGP7waTE#Hc6!7j znG}0)2?m+(LT)6rN%)Z2;V}A9Sw%CESte?)sQwAEdV;zTuaR&xSO<$;NqFEDO>~G7 zdy?}mc0bW6TI@|$U7U+W=NPdMF&!R{*fv)5CL0W#bHuiBqBrp_37^<);>B*{#4h2Y zrAva?mt=nl*8^K8ie6-!_o}FyP2PpG;W&i;y>TQRV0UdS3KMtL{aBEa<8>qw$JS%k z<1ssyn6k=|U)WQ3`*s00@aQq`Ky0Rwt9VpJ^J^Zxrn!bkH8lUtqrYkX$fJ)6ukZgR z%+xSv`*&kb|FR-@==3X5xJXpOT9&ZhWM~aMx`bWLD1RtpTbHpTXr58V22%p%tV=mN zToEW|rzKo(8$MO^W(55@mQx;bGlaq z^YQu(wL51#WF?d`7^e)zo%GaDyK@TQ+?7zt;hl1LccOL)*Y2F^1$QM>jzFCw^jE-i$Pxk)0A+5y&Y5 zxhtVnfNTory?2~AEl!+AT$v;ktjdN50Zsb)R{zI}Y%KmaHOT1@q$^c4>($#o9XBpc z*4M~IoN*CnP86k)Z*T@TxTZp`MlR)yN;$RCUcnhv(EN%sdPQ?Br(esNDDrE7!5X?y z90CkNXpRQP(SW*t0x(RV3(q@&(M~|!KMyp^Q;v%o`86)~o&R}(A(q8w!1>zqI}(GKeS4%r6rVC^%#v394EExTaFqpD zSw|8y2epF_UYXu(?|17n$&T2xjSpT(S5KbT^>=^2bRWLs60}5*xV{D#zR@nIM+S7h z{fZ}Bf$;&nSzm0j*p`S%^77`(?G-w~v^l}_P~s(vHP2$5$mkl%73#H|12H2mKk(++ zyps|?b%J@F;6&7li|kM*4Qj|BBlxX$y!_=~An-)9jxi!6vbPwpHW zHvIAFga6sk_trkWmHC)Ij5j$k5XC@=ETa{g z=BEAU$&#%GP9Tks;mA-l@Rn?P(t$;0KU+?iU+HBYJ3qqh@d%=2az;~Ko9~Ei7So# z1%fXSkvPHW8G#!JDUNUxftxhnLg1FdEtX&Y^~v^2={Qb)ZSiYUb@tkjXBD2i4jgmT z$b~#8SX!Fq&fo5JPjk0OD!hA%Gn;rwAa0=G_9=P4j*M?5Fvl z01nceCV(`Wj|$)@%_jtKg62~KI7Ksl!z_#DvjR9vbDjY5Xf6;y0nL{LaEaz30Tj`E zT>#fX|5DNCCx7c@Pg*o0(h-(!Wh-b z>Gk=?@Ud+-W4xQOA$eVr_cQAKjDy0djB%>cermAYwnW$6$GJ&wA@UDyt(+6HW z@!dL0(lpMxjjt1sR7ci8oC6zQAs`L790@@IbS8_~(zIU2U|^fm_(`%q1N<3%WpzEq zz+;RN$%43C$%0T;U*SLw1akT$3pSL^z(+x!M7<83gV$R zzQI67ICIU&w8E@zng2QO|Iu&SPaoDC&cx~~6x<7io+Ke$(A)H;z;D)>@6Hp$WQ%9> zgMQD(3vD`To{l`pDQ|q*6hK%GDEj$W+>r7@dF*^EFW|c->Q^#DP?;T3#mVPJ@})jst`Zuy9mHVU`Z5;(;5JU zG+zSX63zDjxJPq6K=pun{ox}(A8Gys&?lO=@@Okh$#3J)Hk!kD6sGXbu3Ik6uDNsx z&ma$)Uk`A{G=W zPxJ2Vi_6+UqlC8>o>$RGvcgF6O=Nfz zITLn_UE9?)sc$i!Ngyuzn@sH{5+s){LOKX zz9tW+-Ha=7poZkpdOz)nsf<&q(%v;GT=TAX94>m}Mvt@!;j(vN+Cg&?i;`$gW>GTD zdswiCZADUxM&8SUy)>t?D3z8!$fAQZA7;T}T0V^hX$l{!Rl|WJIy}eb-iw;=MYg24 zGt_Hp%o6>HFR*zILY;%qh`4m;bFDs{T44PKH$*bVk&Fegf`cY}zhRW=d+d_stU)>J zptxWqYhKB=SNJ05b&(sVxa2kDc@4?L1@Qx6WOz#{;i1`eCgUr;Kj<{PU}3$6aU{Wm z3rd`IFz2Xr^jWa_EI256HWK!ZgxKSw(J}b$a zQQysI09i_K!3_|FB(8}hEGLkd^(y+Ql%uHL#@ssJ7r=~;!L+&B%v{sLqGc;fD;q~^TN8WL{{T7Dx3vHO literal 0 HcmV?d00001 diff --git a/.cache/clangd/index/acados_solver_auv_model.h.F9AC8187D4C1016F.idx b/.cache/clangd/index/acados_solver_auv_model.h.F9AC8187D4C1016F.idx new file mode 100644 index 0000000000000000000000000000000000000000..b6757e3c58bd722f1275da28396a9ce646c3f43a GIT binary patch literal 5168 zcmYjT2Ut|c6W@hHAM!Y92izebRisFjc07vILlNmsBm!rNpixvL2qM_9Km-(&&QTP> zC?ZDGXvD6l31Ept1&k#se~A<|ivPXi?epHpw;%JHxtZDB*_mIkz{kf#4FJzX!G>hv z+QlpY01`pU+BM5h$pYZ72*8H2=J0_0NA^lR56{Q5)BSdLng$f^+AMRL17&pocs}p$ zvup8l_mYB#j}<>X;ny0*jQZ!MI%HXtz8`Wd8}n+3yimF9t{LPHdql~$$vhs6OfdTX zid?b1Ro`CUEg8*TiVIhqnW|*b3ODD3hHcdeV0GDR4a@6w-}4c+Rn7A{KX9;r#Y*+z zOM1Ioj6(0_R2}}V^v2V@roZF_ax{BS9O5PF)hKLgI-faC_WD^FnAy4P#?$6P7t4ID?2 zA^gQZ$5UNZtMcSm7{;WAWCUbwFjo%Fy|iP;y(ho5-;DO`+D2%xjO6FZA61o~>D%hs z_d1<$6y#K|n`CP3ZFynY)pnHwU1hv+xrEmNS*NNFji}#EkFMizBc5MLIac&~%rs^G z?zRU}Hpk!DHxKl5jy(yVF*F=P49+>Cy#Gt(*F|w!8x$O#JZY{?cDU}Z-T!d-zn!6S z%dgoTY`9q0yDs0>z4FLgryU8g<7?in@@;qSbeY^?Ajm2IiI-F%tE&H`|FX;p&6c+r z_Z7y1pV=)kO?&CFJ$Iqbrk7e3aTlA6KYdOj=ddq3#zUS_q z;We#uKQ63v(mVLN(6+kapA-I{{ysfJMDBF#J6#=C&R(Q((}+}Bo^(CbKz50r?+n4=>;6I0;sKam1#x)w>O7HPH zo^z-DL(aTMSJIX`2RuKgZO}EIWjrr|A81$j>|S>0za>7u z>0H^hcG1-tp9XjO2b~s1-D~@;#b==BeBG%&?MIW_%yKk@_m2m2t7CFIj_vW^Zu6K= zrjDcpm6!~<-;edI(K@=vJT6P%_CdclhHKE>nVDh9vD#11ZqsqUv)w*L3DrB(8hW;d&@w zv9Y?r!)Yh~P%!+W88L0F-owa1d13FeT`776ZC82I6kqvl%*^qo<8su#{RP%q@*RskD|w%u8{v@pu#Iz5*VXbE&ix|kh>2C19`R_nBxd3xexIR zZkih?sdwM5;&@Kup-fv4&CYW8pj!%$NWGRRXysPypl=SrZ=0o)=Ev{hgNy z8@M75i57`Yk_CHsWoN@{+tX@F0yFHP0CEeU2jD6bGLjt9Ob=}F8W5V1U^+IbhWgde zRT5;r9ZGZGr0(Q{E7`Ur(84B}+)Vp#S-cwmMJA?l!39dwk02ys9m zVpt2jU+jHZP67>Vl3|q*2;@l;%(6lTYl1{hypiMz66j!qDyUZlT_ipNY>|oS^d#y< zo?I0hc%n3;)uMGI3r=(0PadZXpj| zrrJURZS0{Q8q`B~iHCVMXeUF*&#Hc{oOFT&hS;PW^2*^vbp|^j6S*t?Yq( zzWfFD$fRXscfru=hj&O|j!p6)HxKdwcj_cr$(cNCs+^XdcD{=Z+|Y)F7D6A%f;T)h zGo<^F#>e|4Fv1?PAvarG>nidSsqz~=jefr@ctHXKY*GXDYoMDXRmpqRXN$g=o0D3F#5At;oViy$bHmiIxhPg*X9pjcWifuKZME`y*>maC;mg^y?mzEnKX!s^?w7J4M=;5g#LsHSKXcgQn zNU9ccYM}vcgF>5Wqb3~MIh(n0aok%FGAv;xmnq>#*08jjR#6!j-Z+9UtwggKMVf~UU(=Q(_I zLa;Gm%w)59AaZMx?F$cdQc=8ebZM#d8_Ms~c*y z(=^WoBG{g=#~mKvNsVCfn8wzoN)>;m^7yd`HY3cK!@g`#|M5&`ni%#Y{P3^>yFT?4 zY!1I1kKmcaOlBW_rJ~D|>W(EK*n+TNoZ#t?&WN7+^eJsCzmfbPWpWa-_aS^3?>Hv} zi+KQT3|{U3Q@-lminR##C48APJ@fn1G~F&@1AYvmGZ9}j%Va)_Bp5*so@t}kL`gY`dx%Jh1ki9qI%>>2I5ZTi#gto^|57~<3{=mbz6xs6$K4ahdH8U@0$(>TvcoSKPX1E`bl#nGf!wiF@mavwX;bBfv zOIb_JFfqWljRH!jBOMMO3xa9FNGtFJKzWF{BCaT07Ra8ahqOJe2c)1$A+5j{g%mUu zq!qYrp$K9Yh$HM&)Xd!`G(C!755j{vfcadvB!?+)k0E#lF@t$;_}GzmnkLfTaXlaf zO%7?rqp;dttAD9zLyl$>vzZs#T(hfqC_b?r!48B2)BbSTA~!UFq}}{@!h^$_XL%0w z;22hn5^j?uN`q4m^>ALe4AAdsc1i!fWrrv`&~Qoziswi;GQA9!9kQpXCbp-ECQ{G@ zlU9VLRQt%@7`=-exezYQ!Qh%eu$Xe<(+xJ5dN@#xeGkFTgfnxx)^bCQ-9Dei@El@} zD(xEJM~{NioRrq^Ym281?hyN_T+sNhK>emoozG{CzknR{}H{l^yr$gLCM#N>wG z8gfhXSlTUDoulx0Yvv1N??$*W+2I;Su$bB6y+yK}8xa5o<%OmZ}KK zc!QHx3LaR7f|IH!f_8#{s3_XeBDEqQs02hPbw=%)_x=r?v6;z!v+w)%zsK9(TM#mP zw$2ivnK2>h39%_F`3NBq`6s6&#b4kd6k&l-dO>4ELr(KY_Al+c8!6{)zw&_hLxuR#>bE`F#!Jdn^E(@yM`830K zcAunZ$>RuhOxsr{~Ea>&IXMp5kD6{sj-1|v>e=T z>lxXzi$Y3bzro@+*oPCS&~wvww7&kzK?;39A_lB5;JKVYWJhrC0cXd{M<_IwMB1>T z4To?75o`W#o$9Wv$Og$t#NWkVYHVN~lLnqzA9|rFrjVG}^;lYu137^T&IM!o;{G-} zL7`D3(t@Qe*q;-KH1X-+(1egO3b~R4h{c0=3MUYGUVm_J`vSS1Le3;|4vWu`LuUmd;or|2yM9Y}1BIMO z{pmfs8Y$#LB3H2M74mjjfym@rNo%w9hT9Z!Cy{b2E+-SQ0+C~L zDW8&#bwf5tK_USz0a9ZF8%9a7tCP13qk`x9KP(XAzXYtU{wDpbU-LvPVe+sb4-4UJ zC5#pew5C3wf-#E-w=_P}iRn%(fvW+?kcn7cU<&|>g(C|H_gut838EZeQV|m?h#l5b z3)I7U8i8h5FPA6C=Ae58#n?u++gi05@W=tbqZL5rZWR3_y#IS){nbcd1V>{bcykx-s2t>I0Re9*ZQ@0~U$Ff(Qn@Fzm>Z$2}J@C0JO39RUm= zk&I_Cgz-b(inH5lw^xxp`v`q(;Bo+#B+oJld4Q1^??;drEPpWK^vKLZ#iFc6GD9{N zWSdSEh$JK4&ml41PanDZ@6dx8_)Hh3yG*A@C|tktotd1pDw%vU5Rc)}g=hArye{UA lL*rl=LaP#D){_qga-eB!#y1yOSXv3KDH}VHtv$~X{R8qrn^FJ( literal 0 HcmV?d00001 diff --git a/.cache/clangd/index/auv_model_impl_dae_fun_jac_x_xdot_u.c.6132B1FDE79246FD.idx b/.cache/clangd/index/auv_model_impl_dae_fun_jac_x_xdot_u.c.6132B1FDE79246FD.idx new file mode 100644 index 0000000000000000000000000000000000000000..4d1476f88c492a1240ca1131d36dfd3086071ca0 GIT binary patch literal 2716 zcmZ{le^iWF7{~8S^WGZX(bi1NWLER8AL$ofXHnSNj7F)5C0S0PiIqZS6}9JB+w?xlCoBoO*tJN5(4Hu@cjkQ`Gk@4Qr{_7(`~BW~-+SNte3~1i z(d-(4kXjp*5+Ac}sR$t?BY#Qj5?8hg5LzWhDCIzNUTDspadgJJOl9&>mtE8BW1ch) zn`y6p(&D{sN6qj>k(Zh-_w{V_t*Q*uuWwZ|-eoTXZ+JKatSox{<6gJTdzrk_NxGC( zF0I8GIg>8ee%qStO(^QnjGO_+=E*G*wsg+HHLY2L^xsV#u(%6)Vq!9 z2cKux=FJ|QJ0?^uZ8{mVq-y`_Yo{lzFr4lf_j*NeYLPa;Tim-yoZfd+)}nNYmg*jL zRezJEFy3p}7F_OkpxQQ7u5UZ?>Z0f35SJaDk$2M)Qfnl3X^FGgp5pnvy4;RHv9~yS z(Ul9g?dDAHQS=tUPf2ItQp!D1lZT{1RcrZ$kv_107bDFDvP!n|Aci~Fg z9XYCP8?{Hi9jNlg4wuT8cWN@?V_H`!{?%lh&RhCi8u`#ite-d{i`GV9)5k`*6cGqkK6Awx4YhYaafVGA%2`lqi8gQSh^`iHxrSI##L`JAI*El#V5y8|*7zQB z^`emnv8ZYF5OYALYfF9;(pM)8yENWQf<_Q|hI|HN4j@L8`T%TpSiXzf<|6MF2k;6cmbbafmcJWm9KT%uvmgziDjmICSwjT!@-ie z-}goN#?k0wBKK3y{nT_m!3_8>daEjHKP303HPx&tYTBxp3T1j~aNbAS!&ANegVJi4igMq`K^KnFOP z1IVKrD_%a>H}xuwh7x%)J$Z;ZfP6i*u=}{Cx|K#_iCl{nwb-9euzvn8>N{TZ+uCU4 zLM%G0(&0cpfu%Sp>dpO~FNtL^u^1_Zk(?-(z>;^i_jOo8)Sonx6U%1oyctjD6IkNQ zAAS*fX!1Waawe9@fsrzEfLC1}x4bDU>(yfk8cF0pc_3pBAScEwPdZ9&IrOW*`G2~U z2waQ8y3m_M;2IMI=tv@P?Fa(&ArU^@eYTEV*YN{n?0F$RFLZz%%GhT@{LH%EV}tkD zSm_NmxWT&45#bz>mEI=8ZI&9kS|sGUnO|1K9>daOSP6U*v3e}kTh|p>T7i|&bzgr$b5Ei~uwcf#U)Q6T+F6%J|&f0wr4{#6_0F1X7R@9424{ z@PX{g1P6F5Vk@z<(sG#HKCU(Czv=49#C~Ew32*`MA#~)h!4H?Q$0&S^vWHK5V(6y) zqq59q^34rGY_PnRk}aihDfRviz`UdbOt68sk+I1*+3NnL3vjx?s@^WZ+pX&Y0WPqv ziv_sYx~>=CdP@xyBWJ@Q43-6U5rG322tX_%e3(;2;2;Gf;3UXFna~7}Wo$boZl@gK z^8lsD*DxUpY$CQ0OA9S`EHNf-sYy#pj7~Bgl7N*;*_#W8HkAm*qwz2bq3C#R9Qn_H Z9HdgAjfldwb_2u%X^FjT&_@DC^dA7nOpX8m literal 0 HcmV?d00001 diff --git a/.cache/clangd/index/auv_model_impl_dae_fun_jac_x_xdot_u_z.c.ED4BE4E791391EA0.idx b/.cache/clangd/index/auv_model_impl_dae_fun_jac_x_xdot_u_z.c.ED4BE4E791391EA0.idx new file mode 100644 index 0000000000000000000000000000000000000000..661e76b4e4254023002383b8857427fc6bcaba19 GIT binary patch literal 2796 zcmZ`(2~bm46nzQ*KNf#P_yH0Ok|+vDars)swRBXFMJbMeP*GHHX~7CuE7lF!CIUJj zI&LVmLP0hK1_RTwDMnG;s-sjuT&hl8u+qvhRr`|vuSsWYW^(V`f6jaN{`cQGi+sGi z9$6qXC(>tgOw`5|6hcT!{_z{*)^&>zx?zsc=A&%~7NmDfU{ZV2tiLNfF5TjC$##p& zET+KwzK8DvhxXR6g_~*`>W}Spcb5jsx42Nw!N)=lL~0iX-41%&o}svwA=`KF$hpcC z)5Mnfap$MsJuP!|XmcoEF}~ByDYcWzDs(&19jp$kyfJF|{w|LJ*QEtNAFtou(fP7l zeyiE_VSY{TN`?05!OOeOU3ucIk(uoqCoUKtdodu?f5!KJ=p6X#{$7v659<4pYVT(cOm$6S&XpB}KaMn=JlC(tt~fkl zxm};EB|c|$*1ndk%sC<1IZH=QxVxZgUDu&1n{l&Vzi3~yETnGaQR{!cG|w6QdiUBr zd-I*L?3|;OukOFDX}z57|Ln)u^16YQef<~NR-X}PA01xQ zyJPm+v-@}74E*Bp9tVo??aF-mX6BmFUHN`@O73@E$!YQ~$lH-m$GC+}R+Sfd4_x@$ zeDGM7k8SG~h}W~%aS7T@?G*Ry?XQin!uw%^($lv#=@)FBB3_K&afdw7zk~ z@AeFGCzK-WT!fbh2`E<-k>{0p14;=RO(;GxAC*2plUeczx$AD8uk?uP%gg77<;pB>O={0Bb0DixJn7de4Ddv^B81D z;6YkGNP7zjb{Kvse~snU&IJiXIpoxT{#d0n7 z5fV_UYh!C2cL%5$G=WeWu)G1!5fV_&tgVWE`g%(NgGLd`JKFvonT1b4$+TNibv{H} z!Jy%UGJ~06rw{NpPIyEAt;54kNstqP=ga1+^a0?TNioXE{ne)>$eF-?GC!3*06Z!( zE92qmK#c^AA+WE^SEUaCkF6|t)N1;tmO%~#PQ>=2l_?OL(C^Dcwrgp!U`={QhGKq)yf(|t|K z>kbAf2&Ip>x6sN(g z1}mUBXqc?$1q|y!xdh;43jpYqY^)P2Jf^r(%$8yWtOMPW^}J|-2x=t&FI50Qqhw>9 zK;bdP)nT^IF!5K7Nt-pZZ%z~|xGFKO5}U&@E4W56ZZy;&YSM^TH0%gcCXIADhQ}0l z2D4`jjX=_3YhKC%Ps$b3xR|ztwjgcN=Hr4U0Iz0f1TrQ7uVDZ{yaeEt3&5wrs_WG; zdk=^dT!k1{7~T=IOjdkc!KT){az6~8fD^1>bSalA!l@#oI$MOZjq5xS&NHr0iSQ}o zx>1B14K?@zCFcD=Xi9M>F?-T*H7KqGvn9rL4Q6W$2M_*1zK_m4;1DRT9JA$yW#A(w zV%|SKbSuMbnSn-e6_~9s)cw<(HObnPdUBSY=AII;I6~Yj`_wC(?+wCLn?*5r8)&0Kka|@NxekA6aKU zz?X6t#Q1{P=(D~O<5$M@ArpMa#0YOT!Og~XCWSL8BfNvc9flgN4-tmW)C_H()8@}< zYj{Dh7&10*Za^fi7QI4O#K*0Q*G(8js68qhFs17 literal 0 HcmV?d00001 diff --git a/.cache/clangd/index/auv_model_impl_dae_fun_jac_x_xdot_z.c.59CA37EE71B4BA5D.idx b/.cache/clangd/index/auv_model_impl_dae_fun_jac_x_xdot_z.c.59CA37EE71B4BA5D.idx new file mode 100644 index 0000000000000000000000000000000000000000..c81bc5f734ed7c400f75ecc2e979fc2111f73279 GIT binary patch literal 2718 zcmZ`(3s_8P82;zXoWI){(M*~dLq-yoID6HEPEs56|;MAI1hANN&5+yf`<%GPE%@BFi!&A@F>S^kLq&b4nDh zHSRBPm}GlJcYb)B_$+t7+(#_)C|Ng%2pplu_kJbs?qO=1!M?<5V{5azU`VNzd5q&^c$!tgCObJO0w8 zUzAgAQh`P9wQOWtg|+UYo3K{VQY(IXury^rTjNT{nVBUaA5g*GeT67ybyQfk5TVuO zI@{}!Hz@}7M~KbD@=WZ@Cvp#rzqc9pBi}YZ!3=UDl-so8Hto$PpzO}Q7Mk6+%8@|| zLYZzeU1|)-aG%KUb)j{1lJ``12FVFrPpj(b06uX9gjD*5MRf(90Srx zhELF;wPBOGPhJllgSrsP6V#6CmGb0Q0nNeb+jL!K2dZ-t1qb= zi^Zrnp-huaQyBy7V@Si3_QMvm6*I_zz?HPTk__PzblBzG;#<+!d`FC22xY8ntjZXm z!{Ii|lbPyee~3{J0{hAQRK@^su)b&MpAA_J43ZJ}KCQS&tRmK1vJm>2M&OB1uBt|_6JWe)FWefoOc)Z@++LZo|L7fR)O)IL&04~9R=5}#7qWpN`^roVFGaa1ptIg08X_4fLsZ{X%zqvDFHq&O$$3tlKcWGTSnnBssrp< z%08v=Q}cSK5bqS4;irW7lzE*f!igd?yh?(0NE3O(>wr!f>bvP zqf*laN;ZeWIi`;ZU!SbtEC5!3_Q^pTgb$CU>(PVMF^#9t9J!wHM+4szF)6QE69~*INh+yaPndg zPkt~qpmk9F^{6RJ%PwBnzh&e|sjp3j6IO>0zLw!|!0G6&oE_Gpb&K5}s`o99YDsv1QX7@pC_K7-%#_(1i^q5VQfixa#&h|V z1^F}Y=iRH2O)ZSmoJ89#{;G{(%_6f`xmmqc7W`U}BV1bcN4ChiDqv;WPv5Qcxu$DU z|2=bfe)##AL#a%G$MTx!D%+l_gM}?`-FDj)UB9@*E6zJ%P)FbRF_{x*9*Bu}`zGek zr(4HPYn|J&^<>-Yr+?!dIbaI)A)Lzpuy@Gb2eBmw zo>ugfSuD{scpOW+)v(4ZHS|g1%F!`<_MDvW!!;G}b6seWOHsKqL$oVG9>1Szk?0qF zSSKlaxv^qXf4ao>a_4*(>Xa}y-+EQ$zTTI?{1k=AwXR8Ye^2;}ib==b$ z7yn|4%WAjQZ9D1e%=qv#2|Ys@&$i$YGky-mCRm`~-97uO_do zJ)wJ6i8Asktml+{KjKnR>4T0+_Yd`meAagOYClo)KpUM!Beptw3hn_ze;W19Z zB3A|=TruF4rY;(z-?I3hQBl>em_DxjtFE6$9Sk&G?GyE((1i*s4)R$ z)6V*cM!WOQV&p_9L9!r~AwZLg$kL^|>o>WwXfS~vVfiEMV@yz?G;Li|q#{_uA~~V- zU{wzeHYT97IS)O$Eim1OMKVHpg%z*JUGNDgl8&S4_SSWiSn}G4dx!1b;mO7Xl$w^X zciv|m!&o$sP@1s336C=-pk!HExr|qL%wUm%P-gX;B{c+in>y8B5RGa^ZXe z%EtK9YgW7r+{Pjmp{%7I|<&}6S~KDlpL1&f9d%2rynl@2r}pxiPm zwv!BUye&o!gyJvrR~Z5{nec7-0zdzrIu_Xy_zkvugMEz&Dx6z=&3cqgas!Lp38kEt zm(!Ds2`KL>v|l?9ywJ=dYeH$q@^*4vd;-dlmdefHYezm5BWFU1mPM-!0nX8B#f=lj zc6W=>Fan3k!c>L;FiMV}n{u8g67(!k{Ev!-6t7-mU(9J~iI!G^1~HdIOL9zWP&Z}C ztJ=6t%4sp9#TGEhpmNfdmpHr@)Fl8fYyd#f6vZp~<9UwIk|VSdrWlk>_Vcoa{Rx-W zq?b>)?M^AV(?aI7Py|O&a^*s%+_VPalM1}*p#sRA0G}2)0eFSO4v;nhcwGYky~!=< z)eK)txn?YC#+I-Tgx$xI7qpQl<>oQ-20#rdS0Z3a1ijYx1kAnO>kSmMf$D{KQcP#> z^$j6&L--K~yC6;VUST@h0%pNSl9VeIFr~elGz*yK-s>!i$?Dyti(J!A~^z5`!lE|-?%n#K-u`Lrb8v<6Ee^>{ymjbKCs__X~HfcGBQ@maX4 z_EgfAY=M$HE@Y0I1_j)ROr+k5fRDK%T2f@1NN^=W(_4~>mPbqSOeY3+BK<$@Nd({x z2`c>Sp)Oy0b=?JWFpWqfhTpBkCUbXzbbU1$^im;HYC5lyJB*pb_~YP%laYhzZ4Am{ zZa*#AZ#oz_8tKp584$t82*8^d0N`E(__Sw{j(WoaKIV#PNwMk1z`F=dZ(Jr?AuTC1 z(TIm7&enq|$+0Q=ITdhE8=40`{wJhHFba)=Z3x9C#U+yeRy05&p@f)b`k3_<^<%{r KQuF=-OY|S9;~VDy literal 0 HcmV?d00001 diff --git a/.cache/clangd/index/auv_model_model.h.60CCFC5FA93A2F4B.idx b/.cache/clangd/index/auv_model_model.h.60CCFC5FA93A2F4B.idx new file mode 100644 index 0000000000000000000000000000000000000000..1bf1d161709426d37f7086f5e4cec6f64a66f075 GIT binary patch literal 2004 zcmYk6drZ?;6vuDnhgy3@C?JKQEfi@*z=CC#w$4IJeT4ClF+ki9wbTcqw$7F@Go8XR z6)+>dnB8=~nQm-294I&e&3Gt`5yq%nROXypR0x)^NMx7m@1$+gKhCG$+~2vs^Eq9y zOt0^lf{=FIGV5l;_FM)bVdt{!^&y60`YAaS2K5wd9 z^t>mzdg{?zr9=AKAa3f*hW&d)<)7B`%YXlE$hsp!lzeI+JSi@FN$FA_i+mq{_`S-m zpSR7n9Q5kd?$6xV@$inZy=?29oPlZU^rzx|&P5G%fBr|6W9UKfyS3Y{pWqJFTVhTg zDG9yo=x!G5@JPAwYj(fm-&KJXr!QJL$5K|!%)VKp8@$%zCL2#$(fH^E4xF!5tTOwD zl<1`Ic>Yr0z4nZ1Z)=6Oq+F}^`ollF8gq&ea!G? zfSvF7`;lE`>kh=}(U#x={?44Y*X}bORdU})XFGk0K0cIqx#8q?h5IEB#x7ZWMIdQG z=EBWu1a1h4LSB6EP5FswZAZZXe!TaeYYlvT80!D!lSi~%v=L~e+Pao6Ng{ET^ z009M~n1lS?ITCup!WgeLHt(Amm?|`p5l1mm;fOnV(yK@GUTHh9d~1IuM$_R0El;a* zHo1JuToo-ndIbGS1y&4iH<0S>oc0Tb7pL!0a= zx-eR$ztV@12L$4nI2Rw@F#fP<>9Ut3@EQcvjG7D!nmpQ~y&I9<-;dEW2NbnP zY=WuLEC!zmrXI5xEGC%R%VKbsVCpQ3!B~Q+sVoLh38sFs80;jNTFGK?l3?m0i@`*K zsevp8{|Khuu^6l)nA*l-aE)N<7>mVO_fB6Nur+d~iG@>zATk2+NSX@Ara?D?sb(w& z$q1%$u^7}Mm@36$5Q<NM1w_GtWYX6E>)@`!&zw}HF6jb7Hia+c$X?olLeK@ke@rzyV)9qx7TJkHJ9|m`PdsbQ z1YttpZ$b?rtZylP{#B($h(b zZAU?N5ISE>OI6?8+zj8SY*T!Q!V+FeO)5?{vUGRoBQoQy;x*?Ol zRr^@Uk7_+qzPDQtim5nyiOs+^4IJ z%nOA`4|c~moLAXiqP(zY`6XZ~ygl?ghTVmMdf~9|@uF#Sz_EAx@CQ~&+JGv0VLRXl zE}`GLVog85a@ofllcyH;%bk9dE=!@xSEaq^V;T&ztZLs5&L;!WeKqgcqkJ^xI4(Vi z0e<^7$z7h7$X=b7$g`r z7&90=7$q1t7&{m=7&;g|7(N&)7%Uhr7%vzo7(*B`7(o~{7&#a?7%dnn7(f_57$O)% Y7)BUJ7)Tf#7#$Q978@B!7a9Q}0O;q%b^rhX literal 0 HcmV?d00001 diff --git a/.cache/clangd/index/main_auv_model.c.FC541DFFAD75A3A0.idx b/.cache/clangd/index/main_auv_model.c.FC541DFFAD75A3A0.idx new file mode 100644 index 0000000000000000000000000000000000000000..01481f6f9239137c7f13b9cba7727d0693207af0 GIT binary patch literal 1648 zcmY+Ddr%Ws6vp?;1r}Hc7$70LfN{WrJPH9KBve6?AhuNjTbKdl8C$5NHds^;{lg$2 zBah(?2@C^41{^H%&{m*U$4+$s5gD|#)e*EB9Z?4atEj!yxkOi)9Un(RB!XEu{_9-*#06`QY?|^!EXHxf+pY53I)}s6t$@6eu%Zg3}ZCi-Mz?6u3SdjohAgDrk{SWx8=aYM)c?SK8k26=t5K zOFf7qJT+{$^I&Pz>UKKVM&r@U`}bNt<38SgR#bm=&_<}VsIBM@6PwPdcZ;J!WBnJKzM9^k$$Ig6Z0eu*ql$!< zj6M9Q*zhw+9?uRxX^Vcnk-6uoyPqsAWkLp;SwRj*+)|n&#ih~{3EGn$d}-$Hko<~* z&|Fdb&ZmTnYh%&h)`>%kf#7~l`LVG?@M?*)p93vQ-=*n;b! z1&h}f{QWGvJ--{h^+<5GFX+mGChmE;Z|!@# z9Myst-6xYtLr=6y*W2AzvsvdKTvMwD=R{YVik~^$5Z%0-*Lw7Q^cxmG`BUG#ksetV zXRBN`0N4Ta!(T`rDJ|&eEJGbc@FT+T zcAYZwJ6+nbDl~i{6fzilF3g0KwaFV$_7;aN0*n*j4(Y?nM;e5M0m2xJmCyqDR{ z2uq!$ff&o|j#Y1%COZ)p+lfOlem77(b0qzXQwaA0>t3)H8@CG|+#H!TJ%fxeGK|9b zk;|WRvtH~KEOC~IFn-ZH#D3pm)`xH>fSq6sHk=E__ug+D>qn0yau)IN*Ob>6?f^gg zco<;`l-S~Ir)AmQLle4DG{9t-%*U9aQ(%f-31ivY{!*MCN3|mPk^15Jo4)9pk}Xf8 zErY3G4z9!tjQPCo!p2(&H-XhnhR($8b$I#<~l@F5~=e0K1KKA0YdT^UneDoIw*x z`JU93XUtKi=X|pgIIrA3Tn;!O2q#0x7(%Wg)DMav^k;Y k1DC^dBGR)n_N>&fW3yM8KvOew($d0;vS!)a*n-u>zc_0G4M zDCgy|uzk{Dp(2q@HoN7Fu4zc!^!;!5_%81f)qwTPtjUYbtMZ@OdiNIh{$_0M;b30o zX?m>Fpuld=TcBqNos-v7sAFs)R0&6P_L8sV*ZO7Hf*{ibD;oOvfX zVVn8BD?)E(Z{%M&X=!f1$+`unLX>vRJ9XXuS%z<&Kl={R;(rUSJvnfR^VN!txK2+qd3Gae!F0`Sm)`h3VteqY+B`JL z^rF?4!|JX@3q!WA{=w0$d-=d7e!+0f+He2oMCIpU&8mx@4<* zrTL|J)7|`GF;`}V3wh}VGZa_mCdmV1<8Sz@>W54D+jtmc7#Jk=SoAo+1W*P7ic-^x z1(_HaJiqgVvaoBaG0I4GGO~0s>cBK!IB~zGYssoGpoEo*l`~9Ozs|&c-m=O!$HBfp)={M)!cKjd+bTV8W%VC%0XY zT5u1j+KR=B4<=kTp>rDRwi!==!qzO-TrlC&c2)a+$xVC%G{jcM))FRsU{<*eY#it6)4=o$lJoG29xMMbVBoUb@MNvgpHhy6-?Ml;LzNyGt2+6 z$w-=Un8Dp5BU!*x01r_a$tsR2B)M9iS|qtfMwUiKxN~JB+ZZ|87~!fJic)hDL20D8 zD7iQo#E=FN5FiR;FffQCm=Xx46oM%RXEI9K6|2nf6gd^dCM7K%hU@LV5MJ!^qsHg~z6s<_rs`a~x-$@RAuWLR3c>P1x;;j6#lYP!UcTeXe!hgbq zz7Z6PI>SG2M&_JUltQ5h$p4%tZ!KFaUfbnTc=tx(=xep@ zsvD-UvEesfyANY8d_KR(?NVdL_@*hYTZ7Jo;)jmmTPqg3_6E7$^q6()@vV6kCs(_> zxnFL)n{n;4-Y*Y@IGcajH@$90&b@|#GvQh69Z$wh{rOk7pE|$f593VmYE<8U;{Iy| zacTMWrkUlL%YJ%N?eS=lO|&T0N7R!Xxa!ADVs^V54ftX2>)OX-i- zD;;}E?S_weFsZz!ga38yx3g=}`kPBUmru#sv;H1!$A)C+7rg(iRu*Rh#6uD{LYUiX1=pt8==$YnAF* zdNr+?QJHXPjs>$c$W-OpJ^q+?)`8;3-D4vnAI`Ns{cVNSxWcWiPngk(i8mLm2`N9j z-?>jVhn0P2TfwJyW!sJ*1Dh|mnbtRSd{ZMh*ih70-t>)QXZM+htgO8~i*_vE$a%LQ zzEpLStPjxE$mGgk+*j5_2t}QWn!ne%NVF=gQ8! z?>D=po=%(LJvOoUlY2JV7QD$XdmfDmt^ZMe*d=Fa#}^3^@u_DWs0AVMDUk`v_Jc!e zo6Vgz_AlU`D1F#@JlWkQcXJJ7ClsLzU=6RWe#>)n%8fs?%XYqPsr?nNtBBJD@j zTZq^BF}Kz~KB#nCeM+>Z+kEzP^UoS*Jzlvl;IIun{ow0!Nw<^yy3-tYHk3PdTzX)}gC^WXGDrcJznDn6v8I(cPCj7e0Bm^nBUv^#&Dh-lfT_lCJRB$O2UQhW`y1Nb&K_gH@T<*cD}B$yQ2Qfv*i z0}#{3RBiT{;5mYWLdEUy0d;?4*veM>8b7;@jciI1`R#Nx-AFrtx&2&QZtpJ`dgX5(=%WozKTJ1B zeZfP*$(5TjOgY*Ce48>mb5hR6p?bfd8fzB_mfUQS*Tu-=7>p7SB0G^N?S_|$;rX5w zB#A`|h{lNAAySKiAqo*q)RWj=EQUxT9s`k5>;aLFI0T|lF`*}ky#&%Ckw_pd5~T#v zBJq(xS|p(oNQ=~73Tcr_q>vV=QtGOoPwFA{hbTZAr6-xa4ALT#$RI5;r3_k&%tr=k zkp;*gEpmG~q(v@~Lt5lYIiyAIBZn-Khsq%>3VQ{lMIli@S`5D+n14sGHwYv@OT$`ddbVfSbHOBR!hzuc zkt4$qA}5BEjzC62nS(MBo3N$ZazVmj6Yg|(E^t~l;Y;`Bf?QSAc4cht{;7PbfJkH} zvT_#{WWV!F%LHNsd7r}mKO#^q0in@~lBoGV?J}hb34RDa0;GXGJlB$CtaNh@83(9H zw^RWkM#Vf}qJWT~5{RUz6e1ZagGi3bAyS|Uhq zNQk1)D2S5KB#4sHWQbDG6di#*BsW5nu|Z})wFHC`(KyrSv<9IeTWcfu5L;`bo-{V< zNn@j)G&br7&Em+1Gok0mPVMd-pS02 z%pfv1GKWaS6AcAVhqQw)w$PJ=FM&wPmqH}t%OH~TZ;pQXYHPJvPhn^n z2N=U9!mY!_dWuG)Az1>N08t_<(G+~LvcUH$(~84N(GiLv)ZEqJ!KJC15v13D^x$0(L``fZcFXL>KMinH3Hwn+RY9 zK=y|)LxuqR*~CQFL`W9TjMvG|_-3Bs8OO+I0{a2%5!^SyVjl_ zM`mdbkHZI^4cay%rnX(vKOqjA!v!BAbAS~#N}mI;Pq8eBj!>~WxJ-SHV9Ve#Rf}Nt zFt4Oxt8`^+VNMM@yYMrQ5AZ71^t7A^QH2*xNO*I-EkOnl5@FmhOW;*PB7vKrBjBD0 zbx`+(45GEWFIswxJ>f$ZBXbjTOJL#ne{KKn!p+M*A#H6#l-yMiv~}aT#x3I0@gFvuhW!u{$47XTmxBw&`yUk zLv=gNCSqB!kSvZFr;~j<@8Rm*t@5-F2m`Q12!Bxg56%WfOXxeVx=oux9bC}BW|QC5 z9G5_2H~4HaY5T5Mqs?YcBU1+7pC1TJXSk&I<4I+CwD%nhr~~MrI`F>D19jk3XrRqv zHNN;SISGi#sDv}v`-TzZvy7Cw`Hu#|KPR&v&w^(SRsoKLe^HrbPEAi~EnoTp`&e^$ zLt*NnYfH%q=%eVi;F0>w9y`rv&4cA+HsF|ZtiZe>dz5jNrJVc^us-ywfzp9F zGFh4C;JgWXBk~!*iF$C_KK*mc)p#GCKv;b>E!XwYl09?*;e+{@ zfpkFn4;Ent5(|0!!6G4w!lEEb!jd3L#*!gQ!BTWoDSW$n@40LH$!e{Zs8wJpArUSK zSAnU7L?V`G1*qdz)1^HpXI7CB$uwsQfn}Ss42qu`udOBvhUQcVEMse1XFxpx9S>BS zzOUwd3_W|yv*}^(tf{phg)!h7?k{fqRXQWf$LZy9GCO7r$*=&~(aHGLedojD8jXYm zW`gOOF(Dzc5ZQv%DI~-~v7MeILJ34tp%fySPzI4)D2GTPROkpSfUNMEyFlPqx3`w& zo?Y$J`T=S1fLr--H;4RIL`ayKnCbdwArZ}u)^(;eKXlKpcplwOI<*Jt0d+w^XppW8 zddV&C%~`##{R4^)xVHRDE-5&cJ^qD#_7xg=zXM%HkoP-Kt|0Gs$O8qiyd_T3<|o~J zg^WfP%Ho52g53M`%rmxQuYH6^<=P1w^A334zlM@$r4bwlQ2GV`^|=@HL}X(Fm-hD; zK;T67roH^F;YGu)4|u5E*KclWJMqHNWMqK9>BaG!76oU zFjC%!U{7KGfrd5e&SbQ4yJ{4@+j|890vm-{gK0=|z#JiR!`vYX#DXBo!m=UC#pXh^ zl8SwyTO6sigo>5ITt~%@!F+;>HNt$EinYVM48fM`j3M?F)r5bS4|SJAN5y3HZ=ucu+fFz=#a6*@ltZoIr#w&k~f{AiN-n_ZjF$i@9+tLS0u z0oC*%;~-r%PP*vz?Y3gp?;l~*0tX`#x8`RU(8s3+`6r7<7Qg-oo6%W$?ybV+wzp)S zvL9}53F@GLa1c5S)05*UM{7Mf4R_LglKgdQdg>%H(Syy>{Q#+Z17s`R8=!^sSNBF- z71`VC0&#_{gJryHdkz(x*}=g|r#)wOdQK!c8ogV0a|L%I=z$zl&7FR}P0#sXZ8e}Y@#mbe7> z#BFg0+#0vTN8@8~8SaYP;7VMD%W)Tc96la*#GP;vF2?O~FI#m69W3a- literal 0 HcmV?d00001 diff --git a/.cache/clangd/index/test_PID.cpp.575590D7897A814B.idx b/.cache/clangd/index/test_PID.cpp.575590D7897A814B.idx new file mode 100644 index 0000000000000000000000000000000000000000..c950a966809dbe94bc0b1892eabba8b7a5fc3e98 GIT binary patch literal 8790 zcmd^^X;c))7RQ^RoN1VrX`#UuMqJp3O<-_X1_l{n7+Gbt(ML3ZK1IL?8WcBN(72$+ z1tPd61~)LEMiy68T;dW1BM~u*F}Ot$H#EM->lt6sH70(@IedzoW8trFS9RCzd;fK- zh*k#&M-0X=Wr{i{JvDnAz%Yzr|7B%QomilcVH0~}SWZb<{OHV9AM#??PUFm4*WDv# zkImb3!P0P~!RU>4KX@s>d(f2BtS+28*LLCh9|G|m#_mtg%VfDOMs-8*6X%y2P9AlS zu(qb7b~Y8Sh<@|- zdElMW6zMGUf`%mX$a>NDIbREZJM^M{WCpQqKyJf+Mb5Viy6Us3m1(^cGmqx)SA?g9 zSA_?!k*+x1_2ccTSF@$!f%mHwp?5B4E4QvVobxuTY3~IvE^K`bRtZv1IaXY6xhzvT zwmR7@-xuaPm?})=7mk%RI}Ts+TfSssIl2Aq;H}ju8G3JawXY8@YL%S*W&D+5qtfTg zW4D(rRt$1+ZVfy#2PF4aQl2#H`JoGRJOB zQqkI~x&fK}{%dijsDt-JG`Fs-Wbt5p8ksnyX1};DtZA{I>)_ZUr1$;g+pE7)np8Jt z*_I}aUa!BpQc`zrk@)qX>w0$=#=Gpgv7yc;W=6h|_sa5Bwaxd2z6my$v^@HzNJgay zwm$n=S$?j#xYqUf7teC8)%)dV+_t*$Y)ZPkz9DXlcUEH6tl|XU{ROSt`(D}TF=NW9 zhc~=tAMbylBy#P{?P~dw2NzZC*^^RJ7_%$`-?%4FN5}?kuA{cdO(iGhHD_PVo3WL= zf82#$+#q>ae%>SZ%gy?m@(%8vetJlC`_MyEie5HVKe*-nZHC31Q-*5CC`K4ov8$#0 zZsm$=cd|{r`8-4EqnNl%f6omsR|4ST@8r2)A#j^ zf!AC2oY=V~{f}w2HXc*>mCnCNBNPzkIQI{U@K@@Y!~* zZqJ#UO%D|n2|1K;bYv$AsJ9K$)|=Tk-f@5xM42Kouja>l+| zIO}?IyZzqjy=P0p_n++Ol&(?bzp8y)@cL)6*fWve%S6&SAi4U*iL4pvpDyEJ*j>N# z9k)F!?aX*1qcJ7|i{NV;Oq0FrPq0~e`j+NGi4iYh1jgV(oYXe&j2QNnt=pKSI8zzP zi(}1(28Kr32AX#VWy!C{pOXoAF&M^JVOD%?0|(!C?8Luc&Pw+&!72za6FvqP~^zP5p{Hn5^a*>Ov@;{(s8K;TcZpqSkP-bl9hpFYX2ei?2y zip25{cx{RA6jQwIPK6mSGU_kfQez6Hjddul{(xB>*U)#p{bwpY8~FYPL6@&o|OGus1d}b$nKn zIfiW%Zk%~049E10wcibXNk%VsS?1)+JRZR~00$CwBN!*()J@}NtSfsI64Q&Nu?~42A6r_}R@0XUw2$kX^2?#XEhT9{`#4BDNT2l!gvqoq z#Ylie1TRyJGjK-84Y-NmLn+1`xFaM5QiME#Cqh2JM+9d?F>)YB$RGG4qyP$plt774 z5C}p@1yl&Bff}J`5KZP=eE$3X$o4Xdi2*Spj$%P9VoLxC-Q?=*YZ~|IA+bKeXa?Xu zjHU}mnl6yaF8H6dG(0lM80h#7==cpVsNaB&-vER94KS$R0E7As==cpVsNVpC`VBCs z-vER94KS$R0E7AsXr~Q51$6ud7}RfoLH(96WA_+h-C}E=o=)KLbO;JGn+zH$fZ_m! znKaZ2GE6OkkyuIWVHgsOvz0SKZW1?F7={GnE^$Xl>Lwk^**u4MdUE7r^+6jIthA^(kA=WnYp>9z_^Xls_neD=VYh-0_Ia%zipm(uwayzU1mYVs!rzf zz3v}%!op=+&s@7)!{C9-HIMghnf`yW0xdx!T(sX4=iICVMCQ`DiXQq*cCR0twQsww z60rfz>ME2CHlSHuCcM^;n z>1M)_JL!&)2k=1dl#)`!=1F=YOMRgA3|&|-k6LvV>?Z$cmUPSjyKWmc5! zs_S?>-B`@n;5IaTMKNQC+actLJ0j#pxmm-27BlXYJ3>-QijXJeiI6w$O~Wu1GrqVl zLVma(LJCS@&E2V_ln4b;K?tcR6+$6+2r`PAQX{r#(`XwQC}JkTG@+Xeypo^wODfLk z^=^rSj(g{GSX%&{+5+g*7C@)AfE5Spc=Yr0bSQ(?OCEcvWPT5Y+<|>-myWA!ny*JN zd@H^kl*Nc0o{43JMvHoAW|5m;6kZtBlCLLb(uL_X+y=s(85^^j4K^3E8iK~3pZVS1 z+q!bK=%2AgLN`V55aGS03LYM?m(eVRhX!obr8zNxpXxhj3;bWJuB`2$fLJ{+@p)9( z<8>c(fqU@Bv=K>(4n_aKn1jV1n{yhG;c^a@9NF|xpU$+vZ2~bPp(K`^x;sDPx zmYmwkDLF#^ls`g|RHP-Rp)piUH${E9xGnK&#x|aq3BUvTal^%$#@fOz@WkuAM0q)) zPw(~V%uXJqbHqaF9I?P1FFRpPHgOY1bL+Q5`&L@SIqhK|fS%~U=d&v>dbe&~!UHw7 zFREKF7Ub^Ox|ZGx#0)_YVpw8|8GGCwSrdoh!;qCH7!U3ycoO&CJKY^|{hoKxk^LX_ z(MNp8B0|JH34Pr5;*UqI@^x<+YbLuw{k;FffLju})FRqpan-8hfbBSX`4Sf8~&QwHKkS zT;W^@or;*ysWk{46(n>dVp^LZwcbwF^ic;{Rq(o;@_@Fc9ts3_#Q$D!zHKb|5C0;+ zwTA)$)<%3ooxc#UVIi;*!7U6M76Mlh+&8gdA&`k+(aDB|AXEg`N;WJ65hA#dz+*zT zOh@(pT@4Edm^AJX?{ej&c6OfV%5k_$?*7Pa!_&WUo{@BM*q%*mgfBnpC>$52{={YQ znm<13s&s1V9a6wcp2dEaxST!jjWdCTFs?f$2(~ zKIp|4;0A_5qPLNVH11An}_M+Wr8SO*+ x(h}N{cA}kWdpdx2rTuAldMNEl`_XcGFg=V8qJwEM-Jdcwv*@SlYpyp4`ybOEuOk2e literal 0 HcmV?d00001 diff --git a/.cache/clangd/index/test_VC.cpp.74BA115DB14CB17C.idx b/.cache/clangd/index/test_VC.cpp.74BA115DB14CB17C.idx new file mode 100644 index 0000000000000000000000000000000000000000..5c01a6f3a96edbe4ed0c17507e5203e90fa63659 GIT binary patch literal 4538 zcmZ8j2V7HE7f&!C;~|esUPwZKAVVOCMji=!?}rQlfz|=ESa5(=MyqwSI8Yo2D&Rl_ zx9p{&fPx~Uj)GPhS`kq}st9hsoA$d8N`Aj2_kZ>|_ndogfTx>VsV0Tu9Oao5AH8}p zjY6R?!8dVrLTn@ecQh!Jq}+-H@hcwVXghJT+%`6(yDvUfnzyUgL~$%j=2b(ltR=Em z{dV=#(hHRg_Svz^y_>Vyr|1f`d!06ZcPRPq)}*HTk9Twi+Cm2yDuxTZ-@p0m#$d*F z$u;GUJBot|z6Z0tju?=I((>@Q!uSF~Jd%F8t zV&+Jr+rwAz`kRCwynkR;$i~axbo?@wx+$Q(tFy{HJ|jPXVV?H+g3)vJFGso^UljHZ z2?y(Jccy&4waqDeEM=4Zg`wL%=$OZ;IYMKjS%R&93~y}bnQbY{wP;^5w8cN->1^J{ zNV5!7_n`Kv0&Azt*PAxa+*4!x>{w+scX8=4(c#v}AMWyR*#`VtDX2>`6)ShOEH&Pv zQ1G;&V{n_WzVpqILi6Uc7CpN?&7*uYee<$HA2`0dQgGGkr}3;uT@J089Ig1DHs{DR z!qDf!^1ju}tv8nxhlJ0N{N5fUD%jDyIi0`IyWo(8_3J-NXO7uZRuz?w^Nikm321Ry45!% znvOSC{9S#xR^jHzomK;JaIxcQ<3p*s-FAD=mtMO5B)sOF@wZ`S%5VE@K6ft99NN0b z-^*9jHs|FoZTCA%XNJhSf4$+5Y!a0I@yzYu;byg8Jv4r7!Cx*2tMn5*$ooXIp!Bq# z<~JT}5G6e-{0{6O_7$5qfc1HCI{rP}W#B$_N`{$IZD`|u4Y7%*dYxY-guZ0qUl*n~v-1=x0}UK2pA2 zDeX3oj_JU9zSl`zxk^HD7f(OG#`W`4FPnY`*YxfnAH_45I`Rw{N0pmj3@#~QQ+EGS z`>^mp4N*}Wm%D!9?VBaF`Mb5&(z#7KtDmVCrSt@+hCI0tA0`A zqCiWXKk{_&ZC}c1#$R0fu46AQC;qnYXp8C3>p~hV96XvfoUm?We|jIO>9BLtmd96{ z-JVN+PGPGQK6Bc;iyydL!nSI9eJa3$yU2SeIy#Oz;wtK~*c;K;o#w26?e@vagm-4c zzCF^YuP5uf8dJ|!w-^6ZVWPjZe43PD=c=t(mY|OBN4(;X- zS)XF;`z}X3>~n4xt90xq#y1qt^0`vGXx8<%nRPjB+0OhZpA!zvvykRQ+xj~CqOKi> zHNuKbZoRF#k9<)S@*apNiOKOx^1x~1*j3WLYpu8tp)90O2z#2n@;^U-^Uvpmn#jjEuip9W1s4YEU$A8L0fI=(}xISrXt)c>tVCpbUp5) zHI|xCd4x<^Mu%j5L&Fa}>fg@-i3ywpUeGo|NF)+lD73IGMs6{ zlKOm%BA%kV?8>I3YP-9$r|}3+)RO^sfk$|w-jk3o>Pw<0#mi3$hXO3+iCE(EpaXz{ z@X$%PK)Nbj8|v~1hAM+Z{SVpd4}+7Wz^{lQ;z1!R{r=FZC0Ff$Noy@@0hw|6 zL#g9$dQS2NGLo4?a%$Dg!|O!8E^;zYrVGgutcJFeYUK{dLMNdMv|Z`$R+8On>J4(@ z(zpyrMhF|44Fl??Y=7m*B}Tm!m;@VTbD$8D;GsMc`M*s$5V4XK41yQv2rQwH)^LAp zQAhSdkhoAqs1C_ILSJ296C(erQNN4}2g2o*oyqZmWKh|ed=e#93GO|sIUf$@$V$)3 z00tMP_t)<-L(U>V)>YFbXY}{m-+IQJSrI*D>PBOi9Z>(?ARc(am*GD^BM1?o6fhmO zrl;*~y~s3b0;rXUCL-s%Brl>#k>iyt*F8tY&y&4|5J5^oq_%1MoTo2mvr<9E96wGd zOom7B{rDt`ewlU8GH>pBxh)Ueb0UuUO7lre{WlevqZPI5K}nt2&bqJ-*w!NkxxkKppSrVs_HxL%5nMt6b;>AG}7NJa>TE`vl2rHW(j zYeII)huj8tpC|$T4IVoQ{X)gJPbxK@PsPuY*uz~ix_+cj?un(!BF}4t(4pzjq2GvI zof}m1a(7Ls#TxIm*- zSJ#&&SzIaoz?5dYTl8Rt&)$_e@=oA#44@^@30yvjT3(cMPY-wHfDD+b%t;ww1QrQZ zUR&}Pf7=w7E4PGj$V&=XLii-gIa!+*%~+i~C5i;@eC#;0=&<}i5X&rJny{Ow-2XfR z91p~MKJs=f1mWQ6RD1_nX@m$=3M5Yo z4dJ9o&}Zm#CnQZ2O%NCYxG@za2#gmYob{ZIVJWr;jg+r^8dnD*a6w#{kQ{z)wR`Ek z`*k2!@hq_gv~2s zLU+|}`;yK%4BS&<5-1lnf@CH^H>TT7Aciqx-UMQrF`Xt5%Zw$RKwKYg5Jc zeIR%WZZ)PBlf3+y1WSuWqV$ig{Q)Lz&*XdWB6f#+590&FGr0}n1p>(O7cRW~8~4Y5 z7rp4<5HNz%a1|KWY>Kpe?tGQuDGl@}BQm$^XyFKu7SIG_Q@b|jOusVf@&=G6zg5h4 zsw(1jedAM-t00CNr?u+8lgCXLb70)}t25W4Z(`m9m;Ut4i7^Yz7<0kqfJ1&BCdI5VbIb&D#2hev Q%$3FF@VH)D+SBy@2c>}iN&o-= literal 0 HcmV?d00001 diff --git a/.cache/clangd/index/test_VC.hpp.C3EBE494A18C2184.idx b/.cache/clangd/index/test_VC.hpp.C3EBE494A18C2184.idx new file mode 100644 index 0000000000000000000000000000000000000000..27b853ec84ddf9e4b5f8341c878f2c48ec8ba3d0 GIT binary patch literal 1946 zcmYjS2~bm46n*(1`AL2jNLYS82_$F|N0C@9W2u0MtlCL&K@nv@q3%VABdeo>3!(^$ zRqHNJDjF<%z0QT5+Uk-J(T`z!brSt)fz`()YjI;-ATVFZbMg?)~?jmrRZd4=?5; z6q+2Bu_R@Ak|RP$fj{QuX$xw22*nISC}Z#Wsb6h+WR%@E2^I^_{&Xy|vZwKiOI+s<8o5YS6f~Qb}wGoGTOEhoU`&xPOd3YzRc^$h8zCrvqJUO zDBk^xqYgmA>L zTBY{k48Z4xxbr$ItJX>gIfm(R^kmKeEVZrC#@X8PB!n8ndYPW#48ULR4oWRO>0Tfq zTrsTCYWz3@aAd^iJ*&DJ3nhdK!v>i_%^84iJpWl)*70DEgz&_$PNq|H2H+#234POk z4JwlnZuG}VkS+ojH-Iw@6XUK_lIPM{hUdAyW1Ey~(n!Wcy z$>A;u;f}j!0vQu$ARncrd}G!+AVl9O8diRK3Pn&P?N0~8z$sGcuMCE>t;jz(gZFLcD{MXZQC6&9%8!CX*a5Sqkhz5yaH>)pjA9~rvtcZ&@V-kj9 zJfI}J8;)k>2GO82>}F*KZcrF@v+@G>6hYYeB`LB(2a41QbW%_fMd}55DNs?QL12)A z2w}xctP+7K-+t8h;?t5Hn29#fqo6x{JO~y3f3au6OUy0y692D=9!tpOU7ou>U^g?084Jm2DT|YwF(G6j zph4!ex#l%&DN%ntyhsMN_E#cC-0D_}*{kH}huZ&6~HTsL7MJI08WU z%&3Jq8Tn}>002PrFE2lLt{DY)8~|Y9mVNO#U)>A^I}PfjXZf{(hc}c|{cu=9E~-o! zU1z`Vb(?aneLVNpL2gpu)021C55|Yu9oPhFiaYaCi&KjShD(N;uI-3ULLLt-Yn%M{ zT4QbOg!*E^$xexIqW{K$Zs!XU-qZaC3ytsPdfGw#h?e=WQ=a7N(z_ed+D5D-#1kCcOIC*&}*X zFj(SL(t3KneD(ZbcL!>-bHmMj4L$brZ}2RX;YUa3?hZ^o8r8XSP&9BaHS0#^ztn#H zo1>~Hy0@<%!qlL%sc&j-RU{eZkAmXfFB6^%{rf=c)T~KgG@q%vbEI|avy$7i_eaY1 z?>i=&S8lAg-+0#Ixw7oj$-}q4N_lmZ>v`u;gt41e3AQw_UY;ztb%fL9*gbVOJ+6A} zM;lIt|KR%8fSC;4vULfG=f*G-FImb`WOw;xU{Av6VFzREUBOtlbNZ^H>#u7z^}kG6 zS5eeG`WQdVQ)NEiID7ld0MhRM)h=_+FZ=m+$MbcbRUw^Ay$cdE4;Q+v36Q1J=Vbp> z`um%Yi@iuK|7!{$eNwGjkq`aMaBR-&Pk8dK{RN{*60qT&BB;Z!e9w&8XI!r?chGp# zuDR?yuU*!CMvxZyed3d@y@n;vmU4ak%Gd7e%vSHT4Gmd-Cy83WV^z|@_~A0QsV&L& zVX0F}pT4`++M%6QTg&RRZ0p{8z(_U9v$+?~&fm=)=u23ld%GZWeH^o82Uol;L^i-* zzyJ4?nYl=P#RmUy7F?*__mB#fHQYbiJvOFyu=w&8hukB#`@>?}cb!c>-*82l z@@_3uS(<+-)-L12w1?%wH65wnobe=eUb@I#x~FVx@VuRt-qN-=+bXsyKW^D&+f}tX zCe3wn-_(ZO{&knyZI-W8S`-V~)3Uy9H%g)o#|B2nT-LG2Dg+&OH$HWw?6{bFVg~QQ zFV}RNH+VV>Ua1`(pWzz*^5rDWh6jzOe3p(F`d`hD zEw@80!dHP=5#SX)e@Et_;6%&)KHtkOv;FA&jP@%zwOZbzUe<*RHp$5jrM)rL<~_T< z325U5EMG*g6>pz^Xeg|SvgU{u{HMM(eaxw$WBDbUYx?HxnRz^E+!x(1P9NR1lU;Z( zOT(KnXS$$iYS-m!gV}d~pcZzJ=YJi`kfv=ydJ6|XyseWw>uI(0e?9JoxLEb5_|3}A zi+|~51{GBV>SZ4)$)A)hE&R^GzHh_M@%||b{g^p8?u!!Ejlo=gkj({myWz&g*Y%cJ z)6I4D6K4&*S#r!&GGk$O*k9kJKeS^-wqAT(zB4D_$)Vy2Q+95yNap`>c1oT8`k($; zD?Sx0AUp2q$>YC{tu7q5WR*s%i#Ae2D{@YILfY&DE-~pRKKhPFHgRmHU3m!yh(PZb z50~FeOH3_Tb^l+-ltnpFK;EL9^fGkD)P?6<^UP~WcCt+|0f?HRX2wIKAs4_7)uF#B zk0l4)c1TXs0V4n)916!dUP6&L1NLNFz&y<+p3Ds_`rrPd=r%f#iV~<4DlguV;>ZK+ zDWm=))U(@;ct95){O5u1#P^8zb@KH9>`6Ajl&PqP+>BXy$L#^2a`RVvLIO^cZ0U%Q zCUnK^5EV^@BWIUox*yp8jvhrv;&^dBSoCy6@2MABkpVW0!BE@%Li?iL=XBJsmZT+P zVmcC`j0nIGGwS=;gKq&R8#)5nLkukK#y@+43VxgGYy%=(GM9phH#PSy>{5Yjlx-%N z?J*}W;$jySe^l~FAOevgD#iv85mjW5%rfJ;S*ClVI?pecp!F4S1Q50-9f>AIQ!x{7q42$h zF~=9JnA%4z!^BA)6+`lDJ1Ls5#z_NWVmcD;6iz@|CoKW#oOC$KJhgP;;jpU;3WSIp zMK0Lp5F+M@y{t&;B*j|@LS#NNu~nx46)Y=K3Y0<&HL#8+46V4XK%?`ZdAJgegUWZr zNa*2+`3frvrUm0$-EWxptLIabawI*P#*vU7Eu4U~G%W$?XgVDI-C|kon7Atxt*u_9 z7h4^eyiEcO1$jS6Xih&DHqyIJkQ)}}tMZDdNm(C8#0HXqjR9}5!ZA7#UggCREA#BXVc9bvuwk^esii5p_rC8Z9*ETMn zM;LaG3L`B{+Qo-yX!*Q7yge~C9f=}E;XCjpx$ z(4BxFA-;LWz{VTOtz*AM{c89c+;3v=Z+;W{WU?w?<> z|6Vu%zGW9oe8JJ0m?GC#~pn*7!V<0$PHp)QKQcYOT@PJi%2LE5s*~mXGLO` zn7|YZ#RR5UDkd-`42gr)PQsPoJ%~P3?otn{PRf(wHwe|qrTEYT5kCh%d?2ET=SN`r z3H-)by~x~U1fMdVjKGvL@uGu>f}tQV6_A2!-9aH$5b{$plmw;{ zQsT_~_G#?Oit5{F%Xo3Ta4{W;Bt_y^t8f0^dxuVNENF+SmUeWx-oHdv(Ln+tb1}FwbhgK4wx+#DQe55aGkN^bw@_Njj-mM*yl@*&_V;~ z6;Sb1gc_*?Dnd1b{e$J0zq3_2vomIFdWsHqDnPY+;K-nb~tGQa4adT9upah;z1=+^KF`4QqzZxm1I;!HjQ{`u literal 0 HcmV?d00001 diff --git a/.cache/clangd/index/utilities.hpp.77C0A5FDF681DAA0.idx b/.cache/clangd/index/utilities.hpp.77C0A5FDF681DAA0.idx new file mode 100644 index 0000000000000000000000000000000000000000..8b16d9f9241a07ec421ef8a5e4da15b9b4de06c6 GIT binary patch literal 3028 zcmY*b4Nwzj8ve3`V3K7+vPlSRAaMf;CP52@+Cvc)!4O^)e)%`k=j~|O0T07iD!{A*jT8@k6O`Q@4nse8r&J4+5Mh6VfF?&lw_b2`^V)n=v?HD;Y$(9yrA{Y**g!}7a#;@cCiemqh~ ztztgg?pPeSIe&+%bFQ(rkeE4ptMo|Una9nmYqR$Wr)=x22`zn!9hzH$|imhA$PosT@>S zA}77Kb**+r-tpq7x`01EY5d=-?nHn-)vD=6u3J^Pog zUlh5q&=?exuw>$eUrb-hzVuy|PVEm3zjfeR`L^Dvk@}YZ*vpDvJL-i{YwtSohF^wA zikL(qTCGtB@PH=Z&cc@}`JJ?eN63#Nf@Fx6N~JO$fIe|!+i90NPvJ)}Vs`*-R2Tz+ zu5EO!|97-fLlJ>^bERGx$^+0T)Q^Y$R@$leA#hvMa*xb_)n<>JKX_a1O-~s5!CdC&&Rd2H3Xb@+jO$^Xe4YwCIzV_`5 ziZJ4%ky2951JDmDs*`6HJ;Ahv)1Io#Bj@m2jh!7^!BD) zUOAr`S}V~ifL@o>rIKF$HJ>7aIpr!e&Y)?5x5n(u5tx(fE06qUV zbA!6h{56Rr?sh@6S!9+Yu{VJzIj(rn3mcqYY)EM((L{JbCX(rpSmfn?q2XL_S^fR2 zb{_(FD6JFeXdX}iLcR#$9UPakG&`$5x=9f%R-lrqWITX^8?FrBod^4RDMH5`Z<=xr z4?rK)w4P?Pmv4^~Xz{Z!NGuc}A3wbPH#Nnt7ra_~S42?w`w|}s$%Ch77b;~H%H!`3 zP=txgfK*NL03RqA3cH%XZX9vfE<_k2r#S$+u`sDT?!eGvim-Cg!D<~3K-&rm?TOx= zsTD~0X@x<8X~oOjsDf6QQh}VGPDmk4Cn4dd5;h@eCYp(X(Sucq#66p1RU!G70uqM^ z88L*Av{tAU!vv7Dj?wAin#6h={bciw}01`yfW|P_av|(t=bjvKL z4tz=A_5+Xse&8kHeE^oR)(lM+Oa<2ZCi73&!C*2y?($u35n#~NI~mI3OI z`JY`l{Ok2ofmSq5^zRqzQoOYyv< znW~vS?gT7jf)J35rE(4fA>c4vF0j48GS0;uP0F~PY|I<56VyQv$h!?38j6LNIhTQD z*h3ijOe<)gDX{jL0&AZsF!&Ic5Ntvog^ZPX4m7>naVWgyx4r+u^H*N3w&s1G<7H&WQPV9T7Nj=!JibT)GWlG%A6 z0Qyo8;5M+au>kwR%e-xkEkn+HByO{MIz;0WiiZIkJ-mvC0o#J-$tX0+VU9`K$Qq}3 zo=ifM98L%Jhcm-@9~=XB{R*ZSGUF;a(sQagpo{DJaP14wLg0;DRv)pQncOyWeVk=mcFUN}D89};{-L}-egwu+D sR^U>@4MGafE%zVz*hM>>LRsL#)CIG2`LP+De_4nNVAKHth9{>OV literal 0 HcmV?d00001 diff --git a/.cache/clangd/index/velocity_controller.cpp.DFC34CF5F86A4B55.idx b/.cache/clangd/index/velocity_controller.cpp.DFC34CF5F86A4B55.idx new file mode 100644 index 0000000000000000000000000000000000000000..7a23290a548b3a611ae7c7515e9b9fa20487e716 GIT binary patch literal 15478 zcmds-XIxZA)bMx41sAyNQWo}tyDS}~x&n%zBA_T1q=qYEq1mv}tVBR#Xg(nd3Mz^f z#EMv>SYiZ46wz1^Y(Oj!6=H&lB>K#1W@pTY_x<*K(BCiN{`btickY=p=bSk+!Tx@J z^M~?yzOnvG=EWsP@p(L+82y)&oH$!ejmL}7=kb;lp9v3I(CV&JKe4uM*s-6R-z0m4 zb@WkNwt2GVo?Vk)=Eq6%xsgXhS&Tud__+Fb@#1nY&5nG53p{_7r0C2g zOOt@RdXc@kqtiso9!}7ywhdSF9g_dOm&MK(R#PLYM%)P;Y3sj!RAg{a=?n3TW$%Or zyY|!sKVQ5jdfB?0<8?e|c1&rmbGuu8+V$M;35z|7S}h{9;-tO0YZEi}tgE-pFdT1t z<$>lQwUrwe+0U9?7yQeyBeT9qTpK+iW*>jow>rag%^ugTaQA&2exzW`C9|fkmPK(g z`;}wV)<%7^{`*SXCr@Y0^p@>dic zjr4Dx)Hv2hZS5q@)5nAxRysL*NVa$dIadF^_VJMj_^Ws9lx6v~uV1`)9kS5W)%nhz zRkrLK?O5ryyfF3#Ga%IU+Oh2X%rj19 zM!PH5)Xj}Ksyk)1x4y6D$D=P64&XgVe%hV9&uz|>DMjiR*Y-p&T@rBbs?HYq5S_TC zb`|}%hZ-^`%N;BF^W1J7a6Pj7&ccwq<_Dl;#`0g&swV#Ky1;SW>_d8E-1gbm`Tw4} zv#Em9O5Qd8&xGdU=N5~X>`RsYy!#q&Thx`}VC|mCV?UgUEh-C~&{KS*@OI&yZAOJz zK9b4WQ^oA{qN=9#?d$8-S9L_D?tQW~`tHDj3jyX6p1U-h7**-;m*&fcS)E&l9`SUV zzDP82x<=K*jN#j}$9UL12Ws`_$GJPlr34Oo{%23IQDgXJtHs5F52A}<^Ucnl|7GLr zd;h#Wq*1DqV!N#Nq}N&DcHgL121cifPe#n}T_~P^zxmG}b+r<-wrGZ=nCNZ%-o;Q) zP$`)m?LF%Ii?O?B?r+%J6k!ysb9V*1cnDXt!8+zy%Xgle!WW%g8~5Y+DT61+e|WD4 zmPLtxl=B>RL6jt*X3vXr1#Zp7(UY~v*w4& z0O8{8_v+dmJ0AXYOg{CLA@*zFOe1A7yk-agnft)r^^WBkiq^XI%x zIT>%ZcePqVPyOrW2Z=8}6dyd{1tJy=@zNSJEGBGJ zTK0qmv-kO@w#B&odS&wA{bvoD7rl<@lALY+`;VD!eOAP`%{})f(zf-(JDJ3)%4&Kv zGwIAD_D7B6)1L2}UH#KeSFIMEI_a~1XN+mbuRW}QO;LAEW{&Tf=JRIz9KX?hKCXJ- zaG}C9MmX;H{>=$btNtn6k=MRy_NtW&jrJseyLf7KgGJBV6KWOZA4MCzW6SrQyM6lG z*>+2>91j}gJZ0~OfqMiA`vMvtr|C{r@Oeo~=SAlL9&aLJ_g%%qT0I>&FNnvLKv4-a z3ZFi4`XrtJ`AqcB`oJ$Ty`1a%I&cAMPyj^*P#QjM;exc}WQS$QbWTL%WC z2E)a}<>AH}#$u&-A`<6)yY|?jtW)tia4r&OK~WYo4xc)4Y6O0Nc+X4w`_);|9%H^y2hKtbE&%ZbU>z>ekn~f&VjOzHi&f(khgVzd z(}4?7gS}9+7j-gfVw9?rS=aj?8)LivXNEV2$K!57{!J+O_Yb=L)4#;H8A*<=Jl>Ul zOIkIS2-O%dClkvIv3e0nWJVPAll7-)uxuPf6J*hZELaO;LRLmr{Rk<{6&4g(Dy%57 zR=85+r3j%YR52Sy0^R+|O7>2XxYoU#QW;m<9RunDQST4e2&2V>t{4TJkTWEd;@w9_qjsilU1;q0KD z7Wo86=tz+hbfU-^I#U$L57Z`~7h)RHpU^3&ehP}Pp)%Y#sD6%c{PeOp0qS!#1yZgN z*fxT}SSwPl3D`E#+zR|#0Y~I*U|buBAlwe@+kpq+4q)Fwb0@It1Vf0t3)poLc9`6w z6|^ov3o;yS7YynaYy^GqZLN1Yr_9TC7Xs8@Xw(P?VB@~vdekFkTcrpvTrTv?g`@G@ z-Q@eXYx8kk-98NW9W?q5nqsHGaHY_w6q;f$#BfKT(Gh5h=hNYvw+E-}o2?7PT!LDH zI@Ty|4vhcxSaX?eCuhq~T*l4{2|jx&EO57Kbvxj)R;$pDYn9B+gl%WzZF zrx*~Lt3H>an?P_ANU#z%op`q7x|^6q-)r=M7FhBbE+4>r zpc+1g>jkhE=;4M8w-dshP!B7V;SND~2wG5>2j3pi>DXI1XZ4L9<91 z&hZ?&P{*5EC+@LE^F9|!bD<+1$%{@={o&X?{n1?53e>g&5stVsf?NC4*L-iM&T!r- z-b=9MAj$;ZIe>6#T!n#&1~u0U1EiU7XOR+!T(}@@Hu)8@c%Oh_}rH^&;O6j^SLi=p3i-0 z^ZftVJpVs7&;O6jN0#Y)u-ZP+LFtjH*vz270Go(Mp7!SDiGx#qcS0kR55)PPAJ*lS z+kJ;_*LdxuF6N@mqRsKoVlKu!#)43+c`QYV@#%WGmrRQQ z1Nt(YiP*#x&m@L(v~(Om$jQRVnUJ%&vlAi37N95xgd2dM0f_Mnl%aJ@E)2zUZ_fEu zOHK~IrGQe-&(UuH9w|h_{6=7^keuOOXWvzz2^ysxrHX0cuBU&B)lPLmoi&4Is(`_8 z=FprXYiLc;3jPXJNI)|T@Y6|%K&}diGJr9DFT-U5^-Q3P8P7!ISUxWVuxly!BjIgp z{391+=RH8PhXf6V`v5dQ5Dw~;RyxVrU6G~lhu-_4A9iL}?7A%b4$K^cK5>rL9Mv12 z`;J=va`Ur6sQoRdc?$}0u3}xWZn^OrnL83RLER>(j|Ga#856daPIX6Jc@M(hgQd9B z4EGU)egA^bUn z68;=Q34acunEOQ}l>U*kUECzdw!$+CJ+j;3bM!4otfu9BEEOH zGJo3!()Osrjl+f)_HQ-xM1(3O6!Cq~_|p;<8?G`GCgj3YJRP;>MkfV3mU^SR zBiTq>eAmL;MP!e<-V$dksE65-ClQs>o9LKG&|m>N{qSMk3#4 zscor)T_PegOG`{ESNyEC?4)2cU&>WMgDSN4|F<3><<3Kc^EB5$gBqHzLW8S>-Hd+X z{{RM$Mh2P=_keV`7m(?Z0S|%M})^ui*8TVpgC&QI6RkUYfC#?OI}zrH_9k&Qnk3v>un z5Lw`ez)^%!6{&*=rMadJAyfj`5+K6~6~mPPrxM_T`E}#M!@QC8P~>ubd~$UDlQOr5;P z1e#D3FNkM|&J)xVG%?i{jGZL7WDudjaFQ5F0(pQ!T_L7uo?I3KOlg!&1JZ&V? zkqPwz{azr(PY@$VE6#-m_*IL>JT%a%dm5!&T8#4!B%kg*%{!J*>crGgLTOh(+I5wu zSDLfY4q$@KSIN#hfGcM1+f60Oiw8y{yRzh4YGTbd+??h=WB-D(sUp!wxT+ zp^Vd1!I9y*0PG^uB;7r~!Ng|hETo)9*tZdtW&f>k;sHWA-sfU14wc&u4qoi|x63?q zhqcgJRmlumeQex`m30f0p^++EqR>c{7#Xe&2-<)QM;QHFFN5kMl0+dIeSL4$x(1P_ z_gFIC-f`Z^gnVVbl&|!Y_)!xk-Xz|Uv`jTiRb|o)m+F-oK*_>VDf=o0vSQ$XEr#K? z0JsI{5-tHICBTvJ4j|tFoCuc!St+@HP4u_tzd4hWj8v2X`egts{Ms!~?!42?mZcy; zA(R%v{#N3Ztqa<^7MtmU^$Ay@4@)@;2S(8rD#%*|46uDsCMkJ>lMpU zC+`EnePDn`>q(uO_^)H9(vaXbG`tPXu&rPRr6%RJ0rhP_g1aK+b^`UCG?xMOGMbAZ zEQ0!24aWc0{jsg(uj}BoJXTwcVyVntrmAA2m5tO~RpCapUb&qup4N!$>|6-h%k3$d zy`4QJbC5Y$la>y)4hljFH^p#5^*~S$bg?3?rurL(WWLWvfg;2(M72t1xM((7hV{*G ziH3=kEDIQBkrlm<<6kd#_Xg&CrHk%K?)w~vCC$uLrsZSO#$%m?XpA~DFHYwNOjM=}t5E7( zph}&yT6}#MJG!nz{-gnTHUMv|QYm*6c-;h}FyDIkZh~c5`8u>I6KWW$@?R*cHVmUE zhK*5qI);m7V<}29OrlyQ8zxhf4GgmZ#W}z*hj7zUjWgp5^YcYgWO0IvzzFwBiY!i0 zL-S=IxJ>gkAh<^Jbs)G-^9>-lLGxW8xJ&arAh<_!GY~Y>{1^xx)7%0CEi^v^f@d^8 z2ZHA`zXXDpG`|9ZSA-9q7&F^RU$+3QY`TF(HyDE5Ju2ovi%d8K2Y6I|h8Fp72=>9K z_zNwz!Xc!FP!281;SijSi>8!B{&1$g01an1@azVvx~y_jNaH)QkD+{#XD0NgU*wq& z{plBZZiW8jMWs-q6iNt}L(g*Pk6*0ZRMI#CC784Clslep-@XB5IGe%n&A=ZUud>oU zG6PP;UyPGdWYnKGg^5w8!xiZ;Pi50Z;T{upixY~JZfilBs?dG+LyhO$O1+I=X{v~) z1e(dl&{(xlG5&Xl9BXUaI5(H~lgu{hq4dnw+YS44L_ywDDO&3NJ(ZjLSo%umfljE(L;8!newrh2!%VZvK>}vG0Kewqj+P#(pIHdY~+7TFu2Q2HjKF%y*<1K@yS#8KN}S4}G*tRPj>c!zfZ_QE4uQQm7VG*_LvNHh*I+AS_CQD%YC5GwDBHi=ec z-6#u`CQ`C2Ak6|Q9$$1gz1Z!a64du^_B`-bDf-kfSr`?VW96|_-8fm6Ov!S9EQi#M&x|{_Ln@t7hO|FQ zZxme?8|>$azX#RbEP`z?9laZ!;?c2UPaiRtGA6|z|BPrtifSc?C^#hq%dGw?bIJN+ zd-+#t>o+F#&IXckd(qOa39?O4j#pqfCZg5)=Tl4AX&;dOv!?uV<32Q`Q8rO3PrSg~ zJ!gpCfc@IYcKIPftUi>X@k1#JXlE6gH9<$LpYB!5 z_iQg<52;JhDGi;UpebImO1V=|=M>GSq3|>`A@Xxj=Nv7shC0^jZE-krJReQi>l5m<)#`D-n7f;m6<>d?SW&{!QNE|75aG+YFC{;XTxyA)rM22dNu>K%|H+5eacfVY85~a zCzA}yw$&~GJ)DdyS65@ZK{&QPN~Z_rS$@jUN9Q> zEGUmFc6-uv6-9db$SOKVN*!5cP#p%MQVjmwCBL-%wv2z{8uH|+8mUx3NYhAD(H{Zr z8(-hFyM?Aof>wgcT^&33+tSj%!fv5^eW5RPs-YObVseTpb4~fwsMZ6wke%d0m<#oZ zW_Cch1L_kqH~`fSKtpT>4BC%k55WF}4?=hl>SGSB9aii$yMt1&YT#21#^JA3?);9t1SSx!1w(5|$>e5i^x!kgmNg=WnGQ|Tp#tkxS%xvm zAbiqq{gQ(to;Au*xbc=u$Ie5HiXAo=$riL2z2CkXjKJ>`qvD564K2R}94`SMBEJl5 zE(2G>wZO5KoIn+G*MRvo;7H{4z_uQ^5WWuN*J*hJux}tIRK?s4AiqI&qQ$6W8JVLh5n zo}x8fH87|qt93DQiIPhs`x2woyrh=!ji(nDobtN$4BeOo4RqxSP zXQES3K(!=0y#JB;%uD_MMuJ{o&hNc#yEfneK z>g%y`)`T6vDp()ZnzdssSR2-o9mKk@Ls=_!Fzd;>vL5U(b~tO#IYSKO=vMF)-u(FYBKv&;S4c literal 0 HcmV?d00001 diff --git a/.cache/clangd/index/velocity_controller.hpp.3E0346F5513060F5.idx b/.cache/clangd/index/velocity_controller.hpp.3E0346F5513060F5.idx new file mode 100644 index 0000000000000000000000000000000000000000..fc7431d5ec85756dbe30346c651947b8e46651f4 GIT binary patch literal 4532 zcmY*a30PBC7Jj*c`tb1*l0ZV(URV;Aut;PrVHcDgL@_{sSg|rLltl$crlL|7trRyz z5OAqfMX89pIF@29Dk`G1E@Lg#qDa-c)MdKNeXnx{hVSF#e*b^YednHg&-s(4M?|!m z5JcGA=|zjO3-tyBL5T5xL1BK*H;5pfP9%t;hR#{h>u-BdHxgH@%rU+s*3EtIu+zJ` z>GKkrT|&0x)K{oZ2(y(M?d#6jPPgCg;B7No zx8EpW_JymqJ3QaSn%vrD@w8ZO^XS;_mYj;^fzc5S- z^tb=bd|m&vv>YATa_U^DW3;30N}ku{z7FGYw^HY4ckKT9eDI*)%U-c(8{N^@inWi=}b9nRi48OiZZx23xzV6BQ{x)rmc{{5cj;|LM-R<6T{>#9- zvhV!f{`|71=}FSKLbsKcMGB9_Z2@~Crgioh&u+B;rS{E~tmZ!-W!=l!SsxVAv9IaU z>(H*e2WwW4tG-njZ`^qP!P0A4hRgT&nxD&%m5(@VExq7W+4*U^Q-k`WT%CE#!=Ei~ zMtr5#6}P!LUo7_66VrHqSBbH`E7d+DBkxe1U-#lSQ5yC0kbW0Yoqf%aPya75hFvRO zMY=MnM`|{|kaa&XwEnc=L1pB3%ANYU{reVdi*f5ggk#NRJ6BnwW&@;eH`(w z&?8q^8Qbxb{x|JCkG`rJZS_v0RZf$)C3TITUL{!DtT9{oyYGbJ*4qv$#r#LMWK>Lp zXJ6mv;a{&xFMs#&$#LDT$uTD$wY;A+Qst35l=Tsscy-QjLGhwF<%R??LA%RqZLvK` zqX{B{2@(Zaa)!Xqoc&X!y`7Y}A#W0mNK8jaBg{DibUfv}=8L;MLIF~X2ze9YHz9K- zD?UpL4vGU`SqZDIhy=(BJ8VMyO~{hTjL%F52VaLluLJ5DF^wen0|65QB%A?yJGpd2 z=iFDOH1fl=x2?AuXMkR=%$-IuOh-$jRh$7@6Md;D zGnMB-BOgqAPV^LW2I!P2r%uc@UZJLuA*K_k1PNz=o;vxB=YzFnY5|&pGk?X?zT#yw zx;R}bn>!!pZjqX|=a;HnKNGnbqzV=E`XmD{*snE2tIVHu4)aJ+hwTFe=UZy#)kI}`rwQvp)p_^E#l(;?e0Zo7lSn4=AvcOiB9r*gVHA^0 zCi9`!DP|@)lMfAYp)sMqeva>W9>t`PDPpJ{s0Gi6Qwb;vWQk!`A^%>Zvm6vM1m01PsL6J+H9V2}sw&1wU{c1q>D zyT;o&QYa|JC@G~BCL79-EfqCNkM%NHIF4P6IE6VltE&8mJM)WGXW?(3KRErOeVm z>r#wfsn@`u=Iff$*LPm#whPk5j@d3KBX-PoL7A~*whPLN9kX4KJ{GpC9N6o6_R)?l z<0!^TZ50S-!1Z-edP99F#@ozW4pRWS3!jr!K{zLXV^ZX>`kGMnp*TpKNPlo&E9>1u4v3<6=W2RxvV9b z%g0<+e%7)vmsOxIf&)Pk$mlPb{&~N6&&!$D@yUG* zd`zH?p#t~=tSrL^dQ;u6P4lR`fxW{G!cAb#B=u+-g67>Hz_3-Y)pU4;Lm##d9zA1z z7sJ;6)?x7QlD0f895A)Gk71F!C;;|;xjUq*&a&(!s*o kN=Yb2eXK{9R-;S1(WS-c(s6XD99`OvE}ci0wxi4c0C?W7-2eap literal 0 HcmV?d00001 diff --git a/.gitignore b/.gitignore new file mode 100644 index 000000000..68e1f5f0c --- /dev/null +++ b/.gitignore @@ -0,0 +1,59 @@ +# Created by https://www.gitignore.io/api/ros +# Edit at https://www.gitignore.io/?templates=ros + +# CMake +cmake-build-debug/ + +### ROS ### +install/ +log/ +build/ +bin/ +msg_gen/ +srv_gen/ +msg/*Action.msg +msg/*ActionFeedback.msg +msg/*ActionGoal.msg +msg/*ActionResult.msg +msg/*Feedback.msg +msg/*Goal.msg +msg/*Result.msg +msg/_*.py +build_isolated/ +devel_isolated/ + +# Generated by dynamic reconfigure +*.cfgc +/cfg/cpp/ +/cfg/*.py + +# Ignore generated docs +*.dox +*.wikidoc + +# IDE stuff +.project +.cproject +.vscode +.DS_Store +.idea + +# qcreator stuff +CMakeLists.txt.user + +srv/_*.py +*.pcd +*.pyc +qtcreator-* +*.user + +/planning/cfg +/planning/docs +/planning/src + +*~ + +# Emacs +.#* + .cache +# End of https://www.gitignore.io/api/ros \ No newline at end of file From b61a37aa05ae63131dc66a1fd0414ae40c89546a Mon Sep 17 00:00:00 2001 From: Henrik Date: Tue, 24 Mar 2026 15:16:42 +0100 Subject: [PATCH 104/128] new fixes --- .ignore | 0 .../config/nautilus_params.yaml | 2 +- .../velocity_controller/config/orca_params.yaml | 2 +- .../velocity_controller/velocity_controller.hpp | 2 +- .../src/velocity_controller.cpp | 2 +- .../los_guidance/launch/guidance_test.launch.py | 14 +++++++------- 6 files changed, 11 insertions(+), 11 deletions(-) create mode 100644 .ignore diff --git a/.ignore b/.ignore new file mode 100644 index 000000000..e69de29bb diff --git a/control/velocity_controller/config/nautilus_params.yaml b/control/velocity_controller/config/nautilus_params.yaml index ca12c8bca..3d183cae5 100644 --- a/control/velocity_controller/config/nautilus_params.yaml +++ b/control/velocity_controller/config/nautilus_params.yaml @@ -24,7 +24,7 @@ publish_rate: 100 #ms #Clamp parameter max_force: 99.5 - controller_type: 1 #1 PID 2 LQR 3 NMPC 4NMPC fast + controller_type: 2 #1 PID 2 LQR 3 NMPC 4NMPC fast #Q: [300.0,0.01,0.01,0.01,32.84,32.84,32.84,32.84,32.84] # u,v,w,p,q,r,phi,theta,psi # R: [0.02,3.1,3.10] # u_surge, u_theta, u_psi diff --git a/control/velocity_controller/config/orca_params.yaml b/control/velocity_controller/config/orca_params.yaml index a6b227d8e..b7ce863f6 100644 --- a/control/velocity_controller/config/orca_params.yaml +++ b/control/velocity_controller/config/orca_params.yaml @@ -23,7 +23,7 @@ publish_rate: 100 #ms #Clamp parameter max_force: 99.5 - controller_type: 1 #1 PID 2 LQR 3 NMPC 4NMPC fast + controller_type: 2 #1 PID 2 LQR 3 NMPC 4NMPC fast #Q: [300.0,0.01,0.01,0.01,32.84,32.84,32.84,32.84,32.84] # u,v,w,p,q,r,phi,theta,psi # R: [0.02,3.1,3.10] # u_surge, u_theta, u_psi diff --git a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp index 65574982a..099c95a74 100644 --- a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp +++ b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp @@ -98,7 +98,7 @@ class Velocity_node : public rclcpp_lifecycle::LifecycleNode{ //VC settings bool reset_on_new_ref=true; bool anti_overshoot=false; - bool auto_start=true; + bool auto_start=false; bool odometry_dropout_guard=true; int publish_counter=0; bool first_start=true; diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index 7162377ea..5941a14cc 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -71,7 +71,7 @@ void Velocity_node::calc_thrust() publish_counter++; if(publish_counter>=100){ reset_controllers(); - RCLCPP_WARN(this->get_logger(),"Odometry dropout, no thrust"); + //RCLCPP_WARN(this->get_logger(),"Odometry dropout, no thrust"); return; } } diff --git a/guidance/los_guidance/launch/guidance_test.launch.py b/guidance/los_guidance/launch/guidance_test.launch.py index 7b3470932..d5dc84073 100644 --- a/guidance/los_guidance/launch/guidance_test.launch.py +++ b/guidance/los_guidance/launch/guidance_test.launch.py @@ -26,7 +26,7 @@ def launch_setup(context, *args, **kwargs): vortex_sim_interface_dir = get_package_share_directory("vortex_sim_interface") los_guidance_dir = get_package_share_directory("los_guidance") operation_mode_manager_dir = get_package_share_directory("operation_mode_manager") - velocity_controller_dir = get_package_share_directory('velocity_controller_lqr') + velocity_controller_dir = get_package_share_directory('velocity_controller') stonefish_sim = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -34,10 +34,10 @@ def launch_setup(context, *args, **kwargs): ), launch_arguments={ "scenario": "default", - "rendering": "false", + "rendering": "true", }.items(), ) - + vortex_sim_interface = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join( @@ -77,7 +77,7 @@ def launch_setup(context, *args, **kwargs): velocity_controller_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join( - velocity_controller_dir, 'launch', 'velocity_controller_lqr.launch.py' + velocity_controller_dir, 'launch', 'velocity_controller.launch.py' ) ), launch_arguments={ @@ -92,7 +92,7 @@ def launch_setup(context, *args, **kwargs): actions=[ IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(stonefish_dir, "launch", "orca_sim.launch.py") + os.path.join(stonefish_dir, "launch", "drone_sim.launch.py") ) ) ], @@ -139,8 +139,8 @@ def launch_setup(context, *args, **kwargs): return [ stonefish_sim, - vortex_sim_interface, - operation_mode_launch, + #vortex_sim_interface, + #operation_mode_launch, los_guidance_launch, velocity_controller_launch, orca_sim, From 3e6f6f10e2f1ad2a0bf4a5d3d8a03f89907fa9f2 Mon Sep 17 00:00:00 2001 From: Henrik Date: Tue, 24 Mar 2026 23:17:03 +0100 Subject: [PATCH 105/128] fixed error calculation, and NED to body calculation --- .../config/orca_params.yaml | 2 + .../include/velocity_controller/LQR_setup.hpp | 14 +- .../include/velocity_controller/utilities.hpp | 27 ++-- control/velocity_controller/src/LQR_setup.cpp | 123 ++++++++++-------- .../velocity_controller/src/NMPC_setup.cpp | 2 +- control/velocity_controller/src/test_VC.cpp | 26 +--- control/velocity_controller/src/utilities.cpp | 74 +++++++++-- .../src/velocity_controller.cpp | 59 +++------ 8 files changed, 189 insertions(+), 138 deletions(-) diff --git a/control/velocity_controller/config/orca_params.yaml b/control/velocity_controller/config/orca_params.yaml index b7ce863f6..b19015c95 100644 --- a/control/velocity_controller/config/orca_params.yaml +++ b/control/velocity_controller/config/orca_params.yaml @@ -7,6 +7,8 @@ yaw: [10.0,1.0,5.0] LQR_params: + #Q: [200.0,62.84,62.84,15.0,15.0,10.0,3.84,3.84] + #R: [8.0,100.0,100.0] Q: [200.0,32.84,32.84,15.0,15.0,100.0,32.84,32.84] R: [0.02,3.1,3.10] NMPCA_params: diff --git a/control/velocity_controller/include/velocity_controller/LQR_setup.hpp b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp index 8a39589fd..d6e73d552 100644 --- a/control/velocity_controller/include/velocity_controller/LQR_setup.hpp +++ b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp @@ -46,16 +46,18 @@ class LQRController{ bool set_matrices(std::vector Q_,std::vector R_,std::vector inertia_matrix, double max_force,std::vector water_r_low,std::vector water_r_high); void reset_controller(int nr=0); bool calculate_thrust(State states, Guidance_data guidance_values); - int set_interval(double interval); + bool set_interval(double interval); Eigen::Vector get_thrust(); private: Eigen::Matrix linearize(State states); + Eigen::Matrix coriolis(const State& s); + std::tuple saturate (double value, bool windup, double limit); double anti_windup(double error, double integral_sum, bool windup); Eigen::Vector saturate_input(Eigen::Vector u); - Eigen::Vector update_error(Guidance_data guidance_values, State states); + Eigen::Vector update_error(const Guidance_data& error, const State& state); double interval_; double integral_error_surge; double integral_error_pitch; double integral_error_yaw; @@ -75,7 +77,7 @@ class LQRController{ }; //Extra operations -Eigen::Matrix3d vector_to_matrix3d(const std::vector &other_matrix); -std::vector matrix3d_to_vector(const Eigen::Matrix3d &mat); -std::vector> matrix3d_to_vector2d(const Eigen::Matrix3d &mat); -Eigen::Matrix3d vector2d_to_matrix3d(const std::vector> &other_matrix); +//Eigen::Matrix3d vector_to_matrix3d(const std::vector &other_matrix); +//std::vector matrix3d_to_vector(const Eigen::Matrix3d &mat); +//std::vector> matrix3d_to_vector2d(const Eigen::Matrix3d &mat); +//Eigen::Matrix3d vector2d_to_matrix3d(const std::vector> &other_matrix); diff --git a/control/velocity_controller/include/velocity_controller/utilities.hpp b/control/velocity_controller/include/velocity_controller/utilities.hpp index 85d961da7..c59efcc5a 100644 --- a/control/velocity_controller/include/velocity_controller/utilities.hpp +++ b/control/velocity_controller/include/velocity_controller/utilities.hpp @@ -14,6 +14,7 @@ struct angle{ double psit=0.0; }; angle quaternion_to_euler_angle(double w, double x, double y, double z); +geometry_msgs::msg::Quaternion euler_angle_to_quaternion(double roll, double pitch, double yaw); class State{ //Dataclass to store state values for LQR controller @@ -27,20 +28,28 @@ class State{ //State(){}; State operator=(int n){if (n){surge=0.0,sway=0.0,heave=0.0,roll_rate=0.0,pitch_rate=0.0,yaw_rate=0.0,roll=0.0,pitch=0.0,yaw=0.0;} return *this;}; State operator=(nav_msgs::msg::Odometry::SharedPtr rhs); + angle get_angle(); }; //TODO: fix these so that changing the quaternions changes the angles, so that the state is always consistent -class Guidance_data:public State{ +class Guidance_data{ public: - //double surge; double pitch; double yaw; - Guidance_data(vortex_msgs::msg::LOSGuidance msg):State{msg.surge,msg.pitch,msg.yaw}{}; - Guidance_data(double surge, double pitch, double yaw):State{surge, pitch, yaw} {}; - Guidance_data():State{0, 0, 0} {}; + double surge=0.0; double pitch=0.0; double yaw=0.0; + //Guidance_data(vortex_msgs::msg::LOSGuidance msg):State{msg.surge,msg.pitch,msg.yaw}{}; + Guidance_data(double surge, double pitch, double yaw):surge{surge},pitch{pitch}, yaw{yaw} {}; + Guidance_data():surge{0.0}, pitch{0.0}, yaw{0.0} {}; + //Guidance_data():State{0, 0, 0} {}; - Guidance_data operator-(const Guidance_data& other) const; - Guidance_data& operator=(const std_msgs::msg::Float64MultiArray& msg); + //Guidance_data operator-(const Guidance_data& other) const; + Guidance_data& operator=(const vortex_msgs::msg::LOSGuidance::SharedPtr& msg); }; -angle NED_to_BODY(const angle &a,const State &s); -Eigen::Vector3d NED_to_BODY(const Eigen::Vector3d &a, const State &s); +//angle NED_to_BODY(const angle &a,const State &s); +//Eigen::Vector3d NED_to_BODY(const Eigen::Vector3d &a, const State &s); +//Eigen::Matrix3d R_ned_to_body(double roll, double pitch, double yaw); +//Eigen::Matrix3d T_euler_to_body(double roll, double pitch); +Eigen::Vector3d body_rates_to_euler_rates(double roll, double pitch,double p, double q, double r); +angle angle_NED_to_body( double roll_des, double pitch_des, double yaw_des, double roll, double pitch, double yaw); +angle angle_NED_to_body(angle desired, angle state); +inline angle angle_NED_to_body(angle desired, angle state){return angle_NED_to_body(desired.phit, desired.thetat, desired.psit, state.phit, state.thetat, state.psit);} diff --git a/control/velocity_controller/src/LQR_setup.cpp b/control/velocity_controller/src/LQR_setup.cpp index c06b3853e..3cb1cf8a3 100644 --- a/control/velocity_controller/src/LQR_setup.cpp +++ b/control/velocity_controller/src/LQR_setup.cpp @@ -121,29 +121,26 @@ Eigen::Matrix LQRController::linearize(State s){ else { D=-inertia_matrix_inv*D_high; } - Eigen::Matrix C; - C.setZero(); //Unødvendig kanskje + Eigen::Matrix C=coriolis(s); + /* //Need to decide if i want to learize around 0? C(1,5)=-mass*s.surge; C(2,4)=mass*s.surge; + */ + D-=inertia_matrix_inv*C; //To avoid unneccessary allocation - /*Eigen::Matrix T(1.0,sin(s.psi)*tan(s.theta),cos(s.psi)*tan(s.theta),s.pitch*cos(s.psi)*tan(s.theta)-s.yaw*sin(s.psi)*tan(s.theta),(s.pitch*sin(s.psi)+s.yaw*cos(s.psi))/(cos(s.theta)*cos(s.theta)), - 0,cos(s.psi),-sin(s.psi),s.yaw*sin(s.psi)+s.pitch*cos(s.psi),0,0, - 0,sin(s.psi)*1/cos(s.theta),cos(s.psi)/cos(s.theta),s.sway*cos(s.psi)/cos(s.theta)-s.pitch*sin(s.psi)/cos(s.theta),(s.yaw*sin(s.psi)+s.pitch*cos(s.psi)*sin(s.theta)/(cos(s.theta)*cos(s.theta)))); -*/ - Eigen::Matrix T; - T<<1,sin(s.yaw)*tan(s.pitch),cos(s.yaw)*tan(s.pitch), + + Eigen::Matrix T=Eigen::Matrix::Identity(); + /*T<<1,sin(s.yaw)*tan(s.pitch),cos(s.yaw)*tan(s.pitch), 0,cos(s.yaw),-sin(s.yaw), - 0,sin(s.yaw)/cos(s.pitch),cos(s.yaw)/cos(s.pitch); + 0,sin(s.yaw)/cos(s.pitch),cos(s.yaw)/cos(s.pitch);*/ Eigen::Matrix A; A.block<6,6>(0,0)=D; A.block<3,3>(0,6)=A.block<3,3>(6,0)=A.block<3,3>(6,6)=Eigen::Matrix3d::Zero(); A.block<3,3>(6,3)=T; std::vector> swaplines{{1,7},{2,8},{3,4},{4,5}}; for (long unsigned int i=0;i ret; @@ -153,19 +150,16 @@ Eigen::Matrix LQRController::linearize(State s){ return ret; }; -Eigen::Vector LQRController::update_error(Guidance_data guidance_values, State states){ - double surge_error = guidance_values.surge - states.surge; - double pitch_error = vortex::utils::math::ssa(guidance_values.pitch - states.pitch); - double yaw_error = vortex::utils::math::ssa(guidance_values.yaw - states.yaw); +Eigen::Vector LQRController::update_error(const Guidance_data& error, const State& state){ + double surge_error = error.surge; + double pitch_error = error.pitch; + double yaw_error = error.yaw; integral_error_surge = anti_windup(surge_error, integral_error_surge, surge_windup); integral_error_pitch = anti_windup(pitch_error, integral_error_pitch, pitch_windup); integral_error_yaw = anti_windup(yaw_error, integral_error_yaw, yaw_windup); - //RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"integral errors: %f, %f, %f",integral_error_surge,integral_error_pitch,integral_error_yaw); - //RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"windup status: %d, %d, %d",surge_windup,pitch_windup,yaw_windup); - //RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"pitch value n state %f, %f",guidance_values.pitch,states.pitch); - //RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"errors: %f, %f, %f",surge_error,pitch_error,yaw_error); - Eigen::Vector state_error= {surge_error, pitch_error, yaw_error, states.pitch_rate, states.yaw_rate, integral_error_surge, integral_error_pitch, integral_error_yaw}; + + Eigen::Vector state_error= {surge_error, pitch_error, yaw_error, -state.pitch_rate, -state.yaw_rate, integral_error_surge, integral_error_pitch, integral_error_yaw}; return state_error; } Eigen::Vector LQRController::saturate_input(Eigen::Vector u){ @@ -175,38 +169,18 @@ Eigen::Vector LQRController::saturate_input(Eigen::Vector u) std::tie(yaw_windup, torque_z) = saturate(u[2], yaw_windup, max_force); return {force_x, torque_y, torque_z}; } -bool LQRController::calculate_thrust(State state, Guidance_data guidance_values){ +bool LQRController::calculate_thrust(State state, Guidance_data error){ ct::optcon::LQR<8,3> lqr; Eigen::Matrix K_l; - /*RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"A matrix: %f, %f, %f, %f, %f, %f, %f, %f; %f, %f, %f, %f, %f, %f, %f, %f; ...",linearize(state)(0,0),linearize(state)(0,1),linearize(state)(0,2),linearize(state)(0,3),linearize(state)(0,4),linearize(state)(0,5),linearize(state)(0,6),linearize(state)(0,7), - linearize(state)(1,0),linearize(state)(1,1),linearize(state)(1,2),linearize(state)(1,3),linearize(state)(1,4),linearize(state)(1,5),linearize(state)(1,6),linearize(state)(1,7)); - RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"B matrix: %f, %f, %f; %f, %f, %f; %f, %f, %f; %f, %f, %f; ...",B(0,0),B(0,1),B(0,2), - B(1,0),B(1,1),B(1,2), - B(2,0),B(2,1),B(2,2), - B(3,0),B(3,1),B(3,2)); - */ + //TODO: consider making my own solver using eigen so that it does not need controll_toolbox library bool INFO= lqr.compute(Q,R,linearize(state),B,K_l,true,false); if(INFO==0){ return false; } - /* - Eigen::Matrix K; - K.block<3,3>(0,0)=K_l.block<3,3>(0,0); - K.block<3,3>(0,3)=K_l.block<3,3>(0,5); - */ + - Eigen::Matrix state_error = update_error(guidance_values, state); - /*RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"Guidance values: %f, %f, %f",guidance_values.surge,guidance_values.pitch,guidance_values.yaw); - RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"Current states: %f, %f, %f, %f, %f",state.surge,state.pitch,state.yaw,state.pitch_rate,state.yaw_rate); - RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"State error: %f, %f, %f, %f, %f, %f, %f, %f",state_error(0),state_error(1),state_error(2),state_error(3),state_error(4),state_error(5),state_error(6),state_error(7)); - RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"Control input: %f, %f, %f",- (K_l*state_error)(0),- (K_l*state_error)(1),- (K_l*state_error)(2)); - RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"saturated_input: %f, %f, %f",saturate_input(- (K_l*state_error))(0),saturate_input(- (K_l*state_error))(1),saturate_input(- (K_l*state_error))(2)); - RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"K matrix: %f, %f, %f, %f, %f, %f, %f, %f; %f, %f, %f, %f, %f, %f, %f, %f; %f, %f, %f, %f, %f, %f, %f, %f", - K_l(0,0),K_l(0,1),K_l(0,2),K_l(0,3),K_l(0,4),K_l(0,5),K_l(0,6),K_l(0,7), - K_l(1,0),K_l(1,1),K_l(1,2),K_l(1,3),K_l(1,4),K_l(1,5),K_l(1,6),K_l(1,7), - K_l(2,0),K_l(2,1),K_l(2,2),K_l(2,3),K_l(2,4),K_l(2,5),K_l(2,6),K_l(2,7)); - */ + Eigen::Matrix state_error = update_error(error, state); u=saturate_input( (K_l*state_error)); return true; } @@ -222,19 +196,21 @@ void LQRController::reset_controller(int nr){ if(nr==0||nr==3){ integral_error_yaw=0.0; yaw_windup=false; - - } return; } -int LQRController::set_interval(double interval){ +bool LQRController::set_interval(double interval){ + if(interval<=0)return false; interval_=interval; - return 1; + return true; } - +Eigen::Vector LQRController::get_thrust(){ + return u; +} //Hjelpefunksjoner for å konvertere mellom std::vector og Eigen::Matrix3d +/*/ Eigen::Matrix3d vector_to_matrix3d(const std::vector &other_matrix){ Eigen::Matrix3d mat; for (int i = 0; i < 3; ++i) @@ -265,7 +241,52 @@ Eigen::Matrix3d vector2d_to_matrix3d(const std::vector> &oth mat(i, j) = other_matrix[i][j]; return mat; } +*/ +//TODO: double check the matrices here +Eigen::Matrix LQRController::coriolis(const State& s) +{ + // Body velocities + double u = s.surge; + double v = s.sway; + double w = s.heave; + double p = s.roll_rate; + double q = s.pitch_rate; + double r = s.yaw_rate; -Eigen::Vector LQRController::get_thrust(){ - return u; + // Inertia matrix components — extract from M + // M = [m*I -m*S(r_g)] + // [m*S(r_g) I_b ] + // For simplicity assuming diagonal M (no off-diagonal inertia/CoG terms): + double m = mass; + double Ixx = 1.0 / inertia_matrix_inv(3,3); + double Iyy = 1.0 / inertia_matrix_inv(4,4); + double Izz = 1.0 / inertia_matrix_inv(5,5); + + Eigen::Matrix C = Eigen::Matrix::Zero(); + + // Standard Fossen rigid-body Coriolis (diagonal inertia, CoG at origin): + // C_RB = [ 0 m*r -m*q ] top-right 3x3 + // [ -m*r 0 m*p ] + // [ m*q -m*p 0 ] + // and + // C_RB = [ 0 -Izz*r Iyy*q ] bottom-left 3x3 (transposed of above with inertia) + // [ Izz*r 0 -Ixx*p ] + // [ -Iyy*q Ixx*p 0 ] + + // Top-right block (translational-rotational coupling) + C(0,4) = m * w; C(0,5) = -m * v; + C(1,3) = -m * w; C(1,5) = m * u; + C(2,3) = m * v; C(2,4) = -m * u; + + // Bottom-left block (rotational-translational coupling) + C(3,1) = m * w; C(3,2) = -m * v; + C(4,0) = -m * w; C(4,2) = m * u; + C(5,0) = m * v; C(5,1) = -m * u; + + // Bottom-right block (rotational-rotational coupling) + C(3,4) = Izz * r; C(3,5) = -Iyy * q; + C(4,3) = -Izz * r; C(4,5) = Ixx * p; + C(5,3) = Iyy * q; C(5,4) = -Ixx * p; + + return C; } \ No newline at end of file diff --git a/control/velocity_controller/src/NMPC_setup.cpp b/control/velocity_controller/src/NMPC_setup.cpp index 86e0b3cea..8c7df9fe1 100644 --- a/control/velocity_controller/src/NMPC_setup.cpp +++ b/control/velocity_controller/src/NMPC_setup.cpp @@ -286,7 +286,7 @@ bool NMPC_controller::initialize_MPC(){ bool NMPC_controller::calculate_thrust(Guidance_data guidance_values, State state){ casadi::DM x0_val={state.surge,state.sway,state.heave,state.roll_rate,state.pitch_rate,state.yaw_rate,state.roll,state.pitch,state.yaw}; - casadi::DM xr_val={guidance_values.surge,guidance_values.sway,guidance_values.heave,guidance_values.roll_rate,guidance_values.pitch_rate,guidance_values.yaw_rate,guidance_values.roll,guidance_values.pitch,guidance_values.yaw}; + casadi::DM xr_val={guidance_values.surge,0,0,0,0,0,0,guidance_values.pitch,guidance_values.yaw}; casadi::DM ur_val={0,0,0}; Pval=casadi::DM::vertcat({x0_val,xr_val,ur_val}); // Solve diff --git a/control/velocity_controller/src/test_VC.cpp b/control/velocity_controller/src/test_VC.cpp index 06e33c79a..42a660e20 100644 --- a/control/velocity_controller/src/test_VC.cpp +++ b/control/velocity_controller/src/test_VC.cpp @@ -10,6 +10,7 @@ #include #include #include "vortex_msgs/msg/los_guidance.hpp" +#include "velocity_controller/utilities.hpp" //#include "velocity_controller/velocity_controller.hpp" //#include "LQR_setup.hpp" //Denne noden er kun for å teste velocity_controller noden @@ -32,7 +33,7 @@ test_VC::test_VC() : Node("test_VC_node") topic_odometry,sub_QoS, std::bind(&test_VC::odometry_callback,this,std::placeholders::_1)); timer_ = this->create_wall_timer( - std::chrono::milliseconds(1000), + std::chrono::milliseconds(200), std::bind(&test_VC::send_guidance, this)); clock_ = this->get_clock(); RCLCPP_INFO(this->get_logger(), "Test_VC node has been started"); @@ -42,10 +43,10 @@ test_VC::test_VC() : Node("test_VC_node") void test_VC::send_guidance() { - /*time1+=0.2; - reference_msg.yaw=0.6*sin(time1*std::numbers::pi/9); - reference_msg.pitch=0.3*sin(time1*std::numbers::pi/9);*/ - reference_msg.surge=0.20;reference_msg.pitch=-0.4;reference_msg.yaw=0.0; //Surge, pitch, yaw + time1+=0.2; + reference_msg.yaw=std::numbers::pi*sin(time1*std::numbers::pi/9); + //reference_msg.pitch=0.3*sin(time1*std::numbers::pi/9); + reference_msg.surge=0.20;reference_msg.pitch=-0.4;//reference_msg.yaw=0.0; //Surge, pitch, yaw //RCLCPP_INFO(this->get_logger(), "guidance callback: %f, %f, %f",reference_msg.surge,reference_msg.pitch,reference_msg.yaw); publisher_guidance->publish(reference_msg); @@ -70,20 +71,5 @@ int main(int argc, char const *argv[]) return 0; } -geometry_msgs::msg::Quaternion euler_angle_to_quaternion(double roll, double pitch, double yaw){ - double cy = cos(yaw * 0.5); - double sy = sin(yaw * 0.5); - double cp = cos(pitch * 0.5); - double sp = sin(pitch * 0.5); - double cr = cos(roll * 0.5); - double sr = sin(roll * 0.5); - geometry_msgs::msg::Quaternion q; - q.w = cr * cp * cy + sr * sp * sy; - q.x = sr * cp * cy - cr * sp * sy; - q.y = cr * sp * cy + sr * cp * sy; - q.z = cr * cp * sy - sr * sp * cy; - - return q; -} \ No newline at end of file diff --git a/control/velocity_controller/src/utilities.cpp b/control/velocity_controller/src/utilities.cpp index 61b09d8ef..69242e7e0 100644 --- a/control/velocity_controller/src/utilities.cpp +++ b/control/velocity_controller/src/utilities.cpp @@ -23,14 +23,15 @@ angle quaternion_to_euler_angle(double w, double x, double y, double z){ return {phi, theta, psi}; }; +/* angle NED_to_BODY(const angle &a,const State &s){ //TODO tests for illegal angles maybe Eigen::Vector3d q; q< navigation: v_nav = R_n_b * v_body Eigen::Matrix3d R_n_b = Rz * Ry * Rx; - // To get body from navigation (NED->BODY): apply transpose (inverse) Eigen::Vector3d v_body = R_n_b.transpose() * a; - /* - Eigen::Matrix3d T; - T<pose.pose.orientation.w; x=rhs->pose.pose.orientation.x; @@ -87,3 +81,63 @@ State State::operator=(nav_msgs::msg::Odometry::SharedPtr rhs){ } +geometry_msgs::msg::Quaternion euler_angle_to_quaternion(double roll, double pitch, double yaw){ + double cy = cos(yaw * 0.5); + double sy = sin(yaw * 0.5); + double cp = cos(pitch * 0.5); + double sp = sin(pitch * 0.5); + double cr = cos(roll * 0.5); + double sr = sin(roll * 0.5); + + geometry_msgs::msg::Quaternion q; + q.w = cr * cp * cy + sr * sp * sy; + q.x = sr * cp * cy - cr * sp * sy; + q.y = cr * sp * cy + sr * cp * sy; + q.z = cr * cp * sy - sr * sp * cy; + + return q; +} + +angle angle_NED_to_body( double roll_des, double pitch_des, double yaw_des,double roll, double pitch, double yaw) +{ + double cr = std::cos(roll), sr = std::sin(roll); + double cp = std::cos(pitch), sp = std::sin(pitch); + double cy = std::cos(yaw), sy = std::sin(yaw); + + // R_current: NED to body for current attitude + Eigen::Matrix3d R_current; + R_current << cp*cy, cp*sy, -sp, + sr*sp*cy - cr*sy, sr*sp*sy + cr*cy, sr*cp, + cr*sp*cy + sr*sy, cr*sp*sy - sr*cy, cr*cp; + + double cr_d = std::cos(roll_des), sr_d = std::sin(roll_des); + double cp_d = std::cos(pitch_des), sp_d = std::sin(pitch_des); + double cy_d = std::cos(yaw_des), sy_d = std::sin(yaw_des); + + // R_desired: NED to body for desired attitude + Eigen::Matrix3d R_desired; + R_desired << cp_d*cy_d, cp_d*sy_d, -sp_d, + sr_d*sp_d*cy_d - cr_d*sy_d, sr_d*sp_d*sy_d + cr_d*cy_d, sr_d*cp_d, + cr_d*sp_d*cy_d + sr_d*sy_d, cr_d*sp_d*sy_d - sr_d*cy_d, cr_d*cp_d; + + // Error rotation matrix: how much to rotate in body to reach desired + // R_error = R_desired * R_current^T + Eigen::Matrix3d R_error = R_desired * R_current.transpose(); + + // Extract euler angles from R_error — this gives the error in body frame + double pitch_err = std::asin(-R_error(2, 0)); + double roll_err = std::atan2(R_error(2, 1), R_error(2, 2)); + double yaw_err = std::atan2(R_error(1, 0), R_error(0, 0)); + + return {roll_err, pitch_err, yaw_err}; +} + +angle State::get_angle(){ + return {roll,pitch,yaw}; +} +Guidance_data& Guidance_data::operator=(const vortex_msgs::msg::LOSGuidance::SharedPtr& msg){ + surge=msg->surge; + pitch=msg->pitch; + yaw=msg->yaw; + return *this; +} diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index 5941a14cc..f2272cb8d 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -60,11 +60,6 @@ Velocity_node::Velocity_node() : rclcpp_lifecycle::LifecycleNode("velocity_contr } - - - - - void Velocity_node::calc_thrust() { if (odometry_dropout_guard){ @@ -75,25 +70,22 @@ void Velocity_node::calc_thrust() return; } } - //RCLCPP_INFO(get_logger(),"Calculating thrust"); - angle NED_error={guidance_values.roll-current_state.roll,guidance_values.pitch-current_state.pitch,guidance_values.yaw-current_state.yaw}; - angle error=NED_to_BODY(NED_error,current_state); - Guidance_data mod_g_values=guidance_values; + //Do I need ssa here? + angle ref_in_body=angle_NED_to_body({0,vortex::utils::math::ssa(guidance_values.pitch),vortex::utils::math::ssa(guidance_values.yaw)},current_state.get_angle()); + Guidance_data error={guidance_values.surge-current_state.surge,-ref_in_body.thetat,-ref_in_body.psit}; + if(anti_overshoot){ - if (abs(error.psit)<3.14/2 || abs(error.thetat)<3.14/2){ //Need to fix to pi - mod_g_values.surge=guidance_values.surge*cos(error.psit)*cos(error.thetat); - } - else{ - mod_g_values.surge=current_state.surge; //Only focus on rotating? Or is 0 maybe //TODO: Decide. Potentially set the u.surge to 0. Then remember to fix the integral anti wind up + if (abs(error.yaw)get_logger(),"Switching to PID"); } @@ -166,37 +158,22 @@ void Velocity_node::calc_thrust() //Callback functions void Velocity_node::guidance_callback(const vortex_msgs::msg::LOSGuidance::SharedPtr msg_ptr){ - Guidance_data old_guidance=guidance_values; - guidance_values = *msg_ptr; - if(reset_on_new_ref){ - if(abs(old_guidance.surge-guidance_values.surge)>=0.5) reset_controllers(1); - if (abs(old_guidance.pitch-guidance_values.pitch)>std::numbers::pi/4)reset_controllers(2); - if (abs(old_guidance.yaw-guidance_values.yaw)surge-guidance_values.surge)>=0.1) reset_controllers(1); + if (abs(msg_ptr->pitch-guidance_values.pitch)>std::numbers::pi/4)reset_controllers(2); + if (abs(msg_ptr->yaw-guidance_values.yaw)get_logger(), "Guidance received: surge=%.3f pitch=%.3f yaw=%.3f",guidance_values.surge, guidance_values.pitch, guidance_values.yaw); - //RCLCPP_INFO(this->get_logger(),"message: s: %f, p:%f, y:%f", msg_ptr->surge,msg_ptr->pitch,msg_ptr->yaw); + guidance_values = msg_ptr; //overloaded to fix all the internal states + return; } void Velocity_node::odometry_callback(const nav_msgs::msg::Odometry::SharedPtr msg_ptr){ - //RCLCPP_INFO(this->get_logger(),"Recieved odometry"); + publish_counter=0; current_state=msg_ptr; //overloaded to fix all the internal states return; } -//**Needs to update to shutdown the node -/*void Velocity_node::killswitch_callback(const std_msgs::msg::Bool::SharedPtr msg_ptr){ - RCLCPP_INFO(this->get_logger(), "Received killswitch: '%d'", msg_ptr->data); - if(msg_ptr->data == true){ - guidance_values = Guidance_data(); - current_state = Guidance_data(); - RCLCPP_INFO(this->get_logger(), "Killswitch activated, reference and current state set to zero"); - } - return; -}*/ - void Velocity_node::get_new_parameters(){ //topics //TODO: check what happens when same parameter in global and local file @@ -293,8 +270,8 @@ Velocity_node::on_deactivate(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Deactivating..."); auto ret = LifecycleNode::on_deactivate(state); + timer_calculation.reset(); reset_controllers(); - //TODO: reset NMPCs return ret; } From df6aab033545856ee975423f74f6690a40b6b343 Mon Sep 17 00:00:00 2001 From: Henrik Date: Wed, 25 Mar 2026 11:14:10 +0100 Subject: [PATCH 106/128] Some cleanup in LQR, ros, and added PID external error --- .../include/velocity_controller/LQR_setup.hpp | 4 +- .../include/velocity_controller/PID_setup.hpp | 1 + .../velocity_controller.hpp | 4 ++ control/velocity_controller/src/LQR_setup.cpp | 48 ++++--------------- control/velocity_controller/src/PID_setup.cpp | 21 +++++++- .../src/velocity_controller.cpp | 27 +++++------ 6 files changed, 49 insertions(+), 56 deletions(-) diff --git a/control/velocity_controller/include/velocity_controller/LQR_setup.hpp b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp index d6e73d552..3ac78e980 100644 --- a/control/velocity_controller/include/velocity_controller/LQR_setup.hpp +++ b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp @@ -64,7 +64,7 @@ class LQRController{ bool surge_windup; bool pitch_windup; bool yaw_windup; Eigen::Matrix Q; Eigen::Matrix R; Eigen::Matrix B; Eigen::Matrix D_low; Eigen::Matrix D_high; - double max_force; double mass; + double max_force, mass, Ixx, Iyy,Izz; Eigen::Matrix inertia_matrix_inv; Eigen::Matrix state_weight_matrix; @@ -74,6 +74,8 @@ class LQRController{ Eigen::Vector u; + + }; //Extra operations diff --git a/control/velocity_controller/include/velocity_controller/PID_setup.hpp b/control/velocity_controller/include/velocity_controller/PID_setup.hpp index ae712b430..8168c90a0 100644 --- a/control/velocity_controller/include/velocity_controller/PID_setup.hpp +++ b/control/velocity_controller/include/velocity_controller/PID_setup.hpp @@ -10,6 +10,7 @@ class PID_controller { //PID_controller(double Kp=0, double Ki=0, double Kd=0, double max_output=0, double min_output=0, double dt=0); //PID_controller(double k_p, double k_i, double k_d) : PID_controller(k_p, k_i, k_d, 100.0, -100.0) {}; bool calculate_thrust(double error); + bool calculate_thrust(double error, double error_d); void reset_controller(); double get_output(); bool set_output_limits(double min_output, double max_output); diff --git a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp index 099c95a74..eff738c7c 100644 --- a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp +++ b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp @@ -111,6 +111,10 @@ class Velocity_node : public rclcpp_lifecycle::LifecycleNode{ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; void reset_controllers(int nr=0); + rclcpp::QoS pub_QoS; + rclcpp::QoS sub_QoS; + + }; diff --git a/control/velocity_controller/src/LQR_setup.cpp b/control/velocity_controller/src/LQR_setup.cpp index 3cb1cf8a3..0de30a5ff 100644 --- a/control/velocity_controller/src/LQR_setup.cpp +++ b/control/velocity_controller/src/LQR_setup.cpp @@ -23,16 +23,7 @@ //Eigen::IOFormat fmt(Eigen::StreamPrecision, 0, ", ", "\n", "[", "]"); LQRController::LQRController() -{ - interval_ = 0.0; - integral_error_surge = 0.0; - integral_error_pitch = 0.0; - integral_error_yaw = 0.0; - surge_windup = false; - pitch_windup = false; - yaw_windup = false; - max_force = 0.0; - mass = 0.0; +{ Q.setZero(); R.setZero(); B.setZero(); @@ -64,10 +55,10 @@ bool LQRController::set_matrices(std::vector Q_,std::vector R_,s } max_force=max_force_; // Ensure full matrices are zeroed before assigning diagonals - Q.setZero(); - R.setZero(); Q.diagonal() = Eigen::Map(Q_.data(), Q_.size()); R.diagonal() = Eigen::Map(R_.data(), R_.size()); + Ixx=inertia_matrix_.at(6*3+3); Iyy=inertia_matrix_.at(4*6+4); Izz=inertia_matrix_.at(5*6+5); mass=inertia_matrix_.at(0); + Eigen::Matrix inertia_matrix = Eigen::Map>(inertia_matrix_.data(),6,6); D_low=Eigen::Map>(water_r_low.data(),6,6); D_high=Eigen::Map>(water_r_high.data(),6,6); @@ -83,8 +74,7 @@ bool LQRController::set_matrices(std::vector Q_,std::vector R_,s B_m.row(swaplines[i][0]).swap(B_m.row(swaplines[i][1])); } B.block<5,3>(0,0)=B_m.block<5,3>(0,0); - integral_error_surge= 0.0; integral_error_pitch= 0.0; integral_error_yaw= 0.0; - surge_windup= false; pitch_windup= false; yaw_windup= false; mass=inertia_matrix_[0]; + reset_controller(); return 1; } @@ -126,7 +116,6 @@ Eigen::Matrix LQRController::linearize(State s){ C(1,5)=-mass*s.surge; C(2,4)=mass*s.surge; */ - D-=inertia_matrix_inv*C; //To avoid unneccessary allocation Eigen::Matrix T=Eigen::Matrix::Identity(); @@ -178,7 +167,6 @@ bool LQRController::calculate_thrust(State state, Guidance_data error){ if(INFO==0){ return false; } - Eigen::Matrix state_error = update_error(error, state); u=saturate_input( (K_l*state_error)); @@ -253,35 +241,19 @@ Eigen::Matrix LQRController::coriolis(const State& s) double q = s.pitch_rate; double r = s.yaw_rate; - // Inertia matrix components — extract from M - // M = [m*I -m*S(r_g)] - // [m*S(r_g) I_b ] - // For simplicity assuming diagonal M (no off-diagonal inertia/CoG terms): - double m = mass; - double Ixx = 1.0 / inertia_matrix_inv(3,3); - double Iyy = 1.0 / inertia_matrix_inv(4,4); - double Izz = 1.0 / inertia_matrix_inv(5,5); Eigen::Matrix C = Eigen::Matrix::Zero(); - // Standard Fossen rigid-body Coriolis (diagonal inertia, CoG at origin): - // C_RB = [ 0 m*r -m*q ] top-right 3x3 - // [ -m*r 0 m*p ] - // [ m*q -m*p 0 ] - // and - // C_RB = [ 0 -Izz*r Iyy*q ] bottom-left 3x3 (transposed of above with inertia) - // [ Izz*r 0 -Ixx*p ] - // [ -Iyy*q Ixx*p 0 ] // Top-right block (translational-rotational coupling) - C(0,4) = m * w; C(0,5) = -m * v; - C(1,3) = -m * w; C(1,5) = m * u; - C(2,3) = m * v; C(2,4) = -m * u; + C(0,4) = mass * w; C(0,5) = -mass * v; + C(1,3) = -mass * w; C(1,5) = mass * u; + C(2,3) = mass * v; C(2,4) = -mass * u; // Bottom-left block (rotational-translational coupling) - C(3,1) = m * w; C(3,2) = -m * v; - C(4,0) = -m * w; C(4,2) = m * u; - C(5,0) = m * v; C(5,1) = -m * u; + C(3,1) = mass * w; C(3,2) = -mass * v; + C(4,0) = -mass* w; C(4,2) = mass * u; + C(5,0) = mass * v; C(5,1) = -mass * u; // Bottom-right block (rotational-rotational coupling) C(3,4) = Izz * r; C(3,5) = -Iyy * q; diff --git a/control/velocity_controller/src/PID_setup.cpp b/control/velocity_controller/src/PID_setup.cpp index 6a2428168..3b8102214 100644 --- a/control/velocity_controller/src/PID_setup.cpp +++ b/control/velocity_controller/src/PID_setup.cpp @@ -9,9 +9,24 @@ PID_controller::PID_controller( double Kp, double Ki, double Kd, double max_outp //TODO: check for more errors, f.example Nan or very high intergral bool PID_controller::calculate_thrust(double error){ if(!init)return false; + //Saturation + if (output>max_output_){ + output = max_output_; + } + else if (output < min_output_){ + output = min_output_; + } + else{ + integral+=error*dt_; //anti-wind up + } //P + I + D - output=Kp_*error+Ki_*integral + Kd_ * (error - previous_error) / dt_; + output=Kp_*error+Ki_*integral + Kd_ * (error - previous_error) / dt_; + previous_error = error; + return true; +}; +bool PID_controller::calculate_thrust(double error, double error_d){ + if(!init)return false; //Saturation if (output>max_output_){ output = max_output_; @@ -22,10 +37,12 @@ bool PID_controller::calculate_thrust(double error){ else{ integral+=error*dt_; //anti-wind up } + //P + I + D + output=Kp_*error+Ki_*integral + Kd_ * error_d; previous_error = error; return true; -}; +} void PID_controller::reset_controller(){ integral = 0.0; previous_error = 0.0; diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index f2272cb8d..037730231 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -23,10 +23,12 @@ //Konstruktør -Velocity_node::Velocity_node() : rclcpp_lifecycle::LifecycleNode("velocity_controller_lifecycle"), lqr_controller() +Velocity_node::Velocity_node() : rclcpp_lifecycle::LifecycleNode("velocity_controller_lifecycle"), lqr_controller(), pub_QoS(10), sub_QoS(10) { - RCLCPP_INFO(this->get_logger(), "Velocity control node has been started."); get_new_parameters(); + pub_QoS.keep_last(10).reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT).durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); + sub_QoS.keep_last(10).reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT).durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); + //TODO: dont need to save the Q3 R3 and the other matries, just use #define //NMPC controller NMPC.set_matrices(Q3,R3, inertia_matrix, max_force, dampening_matrix_low, dampening_matrix_high); @@ -56,6 +58,8 @@ Velocity_node::Velocity_node() : rclcpp_lifecycle::LifecycleNode("velocity_contr if(auto_start){ startup_timer_=create_wall_timer(std::chrono::milliseconds(0), [this](){startup_timer_->cancel(); trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);}); } + RCLCPP_INFO(this->get_logger(), "Velocity control node has been started."); + return; } @@ -66,11 +70,11 @@ void Velocity_node::calc_thrust() publish_counter++; if(publish_counter>=100){ reset_controllers(); - //RCLCPP_WARN(this->get_logger(),"Odometry dropout, no thrust"); + RCLCPP_WARN(this->get_logger(),"Odometry dropout, no thrust"); return; } } - //Do I need ssa here? + //TODO: Do I need ssa here? angle ref_in_body=angle_NED_to_body({0,vortex::utils::math::ssa(guidance_values.pitch),vortex::utils::math::ssa(guidance_values.yaw)},current_state.get_angle()); Guidance_data error={guidance_values.surge-current_state.surge,-ref_in_body.thetat,-ref_in_body.psit}; @@ -82,10 +86,10 @@ void Velocity_node::calc_thrust() switch (controller_type) { case 1:{ - + //TODO: some logic for removing the change in reference PID_surge.calculate_thrust(error.surge); - PID_pitch.calculate_thrust(error.pitch); - PID_yaw.calculate_thrust(error.yaw); + PID_pitch.calculate_thrust(error.pitch,-current_state.pitch_rate); + PID_yaw.calculate_thrust(error.yaw, -current_state.yaw_rate); thrust_out.wrench.force.x = PID_surge.get_output(); thrust_out.wrench.torque.y = PID_pitch.get_output(); thrust_out.wrench.torque.z = PID_yaw.get_output(); @@ -208,15 +212,13 @@ void Velocity_node::get_new_parameters(){ this->declare_parameter>("LQR_params.R"); R=this->get_parameter("LQR_params.R").as_double_array(); this->declare_parameter>("physical.mass_matrix"); - this->get_parameter("physical.mass_matrix", inertia_matrix); - RCLCPP_INFO(get_logger(),"1"); + inertia_matrix=this->get_parameter("physical.mass_matrix").as_double_array(); //D this->declare_parameter>("dampening_matrix_low"); this->declare_parameter>("dampening_matrix_high"); this->dampening_matrix_low=this->get_parameter("dampening_matrix_low").as_double_array(); this->dampening_matrix_high=this->get_parameter("dampening_matrix_high").as_double_array(); - RCLCPP_INFO(get_logger(),"1"); //NMPC acados Parameters this->declare_parameter>("NMPCA_params.Q"); @@ -237,16 +239,11 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Veloci RCLCPP_INFO(get_logger(), "Configure VC"); // Publishers - rclcpp::QoS pub_QoS(10); - pub_QoS.keep_last(10).reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT).durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); publisher_thrust = create_publisher(topic_thrust, pub_QoS); //Subscribers - rclcpp::QoS sub_QoS(10); - sub_QoS.keep_last(10).reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT).durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); subscriber_Odometry = this->create_subscription( topic_odometry,sub_QoS, std::bind(&Velocity_node::odometry_callback,this,std::placeholders::_1)); subscriber_guidance = this->create_subscription( topic_guidance,sub_QoS,std::bind(&Velocity_node::guidance_callback,this, std::placeholders::_1)); - //subscriber_killswitch = this->create_subscription(topic_killswitch,sub_QoS,std::bind(&Velocity_node::killswitch_callback,this, std::placeholders::_1)); //Timer if(first_start&&auto_start){ startup_timer_=create_wall_timer(std::chrono::milliseconds(0),[this](){startup_timer_->cancel(); trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE);}); From 182688fc1e35affd0305996b7c4e60c2d9b086c0 Mon Sep 17 00:00:00 2001 From: Henrik Date: Wed, 25 Mar 2026 11:41:06 +0100 Subject: [PATCH 107/128] changed the file system so that test files is only built when testing --- control/velocity_controller/CMakeLists.txt | 27 ------------------- .../velocity_controller/tests/CMakeLists.txt | 26 +++++++++++++++++- .../{src => tests}/LQR_test.cpp | 0 .../{src => tests}/test_VC.cpp | 5 +--- .../velocity_controller => tests}/test_VC.hpp | 6 ----- 5 files changed, 26 insertions(+), 38 deletions(-) rename control/velocity_controller/{src => tests}/LQR_test.cpp (100%) rename control/velocity_controller/{src => tests}/test_VC.cpp (92%) rename control/velocity_controller/{include/velocity_controller => tests}/test_VC.hpp (84%) diff --git a/control/velocity_controller/CMakeLists.txt b/control/velocity_controller/CMakeLists.txt index 5540b1e01..30fd54084 100644 --- a/control/velocity_controller/CMakeLists.txt +++ b/control/velocity_controller/CMakeLists.txt @@ -107,34 +107,7 @@ if(BUILD_TESTING) add_subdirectory(tests) endif() -add_executable(test_VC_node -src/test_VC.cpp -src/utilities.cpp -src/ct_instantiations.cpp -) - -target_include_directories(test_VC_node PUBLIC -$ -$ -${EIGEN3_INCLUDE_DIR} -) - -target_link_libraries(test_VC_node Eigen3::Eigen ct_optcon ct_core) - -ament_target_dependencies(test_VC_node -rclcpp -rclcpp_lifecycle -std_msgs -vortex_msgs -geometry_msgs -nav_msgs -vortex_utils -) - -install(TARGETS test_VC_node -DESTINATION lib/${PROJECT_NAME} -) ament_package() diff --git a/control/velocity_controller/tests/CMakeLists.txt b/control/velocity_controller/tests/CMakeLists.txt index b76128d1c..0d579c4f3 100644 --- a/control/velocity_controller/tests/CMakeLists.txt +++ b/control/velocity_controller/tests/CMakeLists.txt @@ -78,6 +78,30 @@ target_link_libraries( ct_optcon ct_core ) +add_executable(test_VC_node +test_VC.cpp +../src/utilities.cpp +) + +target_include_directories(test_VC_node PUBLIC +$ +$ +${EIGEN3_INCLUDE_DIR} +) + + +target_link_libraries(test_VC_node Eigen3::Eigen ct_optcon ct_core) +ament_target_dependencies(test_VC_node +rclcpp +rclcpp_lifecycle +std_msgs +vortex_msgs +geometry_msgs +nav_msgs +vortex_utils +) -#gtest_discover_tests(${TEST_BINARY_NAME}) \ No newline at end of file +install(TARGETS test_VC_node +DESTINATION lib/${PROJECT_NAME} +) \ No newline at end of file diff --git a/control/velocity_controller/src/LQR_test.cpp b/control/velocity_controller/tests/LQR_test.cpp similarity index 100% rename from control/velocity_controller/src/LQR_test.cpp rename to control/velocity_controller/tests/LQR_test.cpp diff --git a/control/velocity_controller/src/test_VC.cpp b/control/velocity_controller/tests/test_VC.cpp similarity index 92% rename from control/velocity_controller/src/test_VC.cpp rename to control/velocity_controller/tests/test_VC.cpp index 42a660e20..93dced1f4 100644 --- a/control/velocity_controller/src/test_VC.cpp +++ b/control/velocity_controller/tests/test_VC.cpp @@ -5,7 +5,7 @@ #include #include ///#include "velocity_controller/PID_setup.hpp" -#include "velocity_controller/test_VC.hpp" +#include "test_VC.hpp" #include #include #include @@ -47,13 +47,10 @@ void test_VC::send_guidance() reference_msg.yaw=std::numbers::pi*sin(time1*std::numbers::pi/9); //reference_msg.pitch=0.3*sin(time1*std::numbers::pi/9); reference_msg.surge=0.20;reference_msg.pitch=-0.4;//reference_msg.yaw=0.0; //Surge, pitch, yaw - //RCLCPP_INFO(this->get_logger(), "guidance callback: %f, %f, %f",reference_msg.surge,reference_msg.pitch,reference_msg.yaw); - publisher_guidance->publish(reference_msg); } void test_VC::odometry_callback(const nav_msgs::msg::Odometry::SharedPtr msg_ptr){ - //RCLCPP_INFO(this->get_logger(), "odo callback"); vortex_msgs::msg::LOSGuidance msg; angle temp=quaternion_to_euler_angle(msg_ptr->pose.pose.orientation.w, msg_ptr->pose.pose.orientation.x, msg_ptr->pose.pose.orientation.y, msg_ptr->pose.pose.orientation.z); msg.set__pitch(temp.thetat); diff --git a/control/velocity_controller/include/velocity_controller/test_VC.hpp b/control/velocity_controller/tests/test_VC.hpp similarity index 84% rename from control/velocity_controller/include/velocity_controller/test_VC.hpp rename to control/velocity_controller/tests/test_VC.hpp index 711c9f334..ff82b8cd5 100644 --- a/control/velocity_controller/include/velocity_controller/test_VC.hpp +++ b/control/velocity_controller/tests/test_VC.hpp @@ -15,10 +15,8 @@ class test_VC : public rclcpp::Node{ public: test_VC(); //Callback functions - //void read_thrust(geometry_msgs::msg::WrenchStamped::SharedPtr msg); void send_guidance(); void odometry_callback(const nav_msgs::msg::Odometry::SharedPtr msg_ptr); - //void send_state(); //Variables @@ -30,19 +28,15 @@ class test_VC : public rclcpp::Node{ rclcpp::TimerBase::SharedPtr timer_; rclcpp::Clock::SharedPtr clock_; //Messages - //std::vector thrust_vector; vortex_msgs::msg::LOSGuidance reference_msg; //Topics - //std::string topic_odom; - //std::string topic_thrust; std::string topic_guidance; std::string topic_state="/state"; std::string topic_odometry; //MSGS - //nav_msgs::msg::Odometry odom_msg; double time1=0; }; From e7a3eaa63188a90f7abd4c5636b97d35d9fb2d48 Mon Sep 17 00:00:00 2001 From: Henrik Date: Wed, 25 Mar 2026 12:34:51 +0100 Subject: [PATCH 108/128] cleaned up the program for push --- control/velocity_controller/CMakeLists.txt | 50 +- .../config/nautilus_params.yaml | 25 +- .../config/orca_params.yaml | 27 +- .../include/velocity_controller/LQR_setup.hpp | 4 +- .../velocity_controller/NMPC_acados.hpp | 90 -- .../velocity_controller/NMPC_setup.hpp | 40 - .../velocity_controller.hpp | 19 +- .../velocity_controller/scripts/auv_ocp.json | 1145 ---------------- .../scripts/build_auv_solver/Makefile | 208 --- .../acados_sim_solver_auv_model.c | 310 ----- .../acados_sim_solver_auv_model.h | 105 -- .../build_auv_solver/acados_solver.pxd | 63 - .../acados_solver_auv_model.c | 1187 ----------------- .../acados_solver_auv_model.h | 174 --- .../acados_solver_auv_model.o | Bin 35504 -> 0 bytes .../auv_model_model/auv_model_impl_dae_fun.c | 394 ------ .../auv_model_model/auv_model_impl_dae_fun.o | Bin 11584 -> 0 bytes .../auv_model_impl_dae_fun_jac_x_xdot_u.c | 847 ------------ .../auv_model_impl_dae_fun_jac_x_xdot_u.o | Bin 21328 -> 0 bytes .../auv_model_impl_dae_fun_jac_x_xdot_u_z.c | 851 ------------ .../auv_model_impl_dae_fun_jac_x_xdot_u_z.o | Bin 21944 -> 0 bytes .../auv_model_impl_dae_fun_jac_x_xdot_z.c | 809 ----------- .../auv_model_impl_dae_fun_jac_x_xdot_z.o | Bin 20472 -> 0 bytes .../auv_model_impl_dae_jac_x_xdot_u_z.c | 708 ---------- .../auv_model_impl_dae_jac_x_xdot_u_z.o | Bin 17736 -> 0 bytes .../auv_model_model/auv_model_model.h | 81 -- .../libacados_ocp_solver_auv_model.so | Bin 86256 -> 0 bytes .../scripts/build_auv_solver/main_auv_model.c | 189 --- .../build_auv_solver/main_sim_auv_model.c | 141 -- .../velocity_controller/scripts/dynamics.py | 163 --- .../velocity_controller/scripts/generator.py | 223 ---- control/velocity_controller/src/LQR_setup.cpp | 95 +- .../velocity_controller/src/NMPC_acados.cpp | 183 --- .../velocity_controller/src/NMPC_setup.cpp | 351 ----- .../src/velocity_controller.cpp | 170 +-- .../velocity_controller/tests/LQR_test.cpp | 55 - .../velocity_controller/tests/test_LQR.cpp | 16 +- control/velocity_controller/tests/test_VC.cpp | 4 +- control/velocity_controller/tests/test_VC.hpp | 4 +- 39 files changed, 80 insertions(+), 8651 deletions(-) delete mode 100644 control/velocity_controller/include/velocity_controller/NMPC_acados.hpp delete mode 100644 control/velocity_controller/include/velocity_controller/NMPC_setup.hpp delete mode 100644 control/velocity_controller/scripts/auv_ocp.json delete mode 100644 control/velocity_controller/scripts/build_auv_solver/Makefile delete mode 100644 control/velocity_controller/scripts/build_auv_solver/acados_sim_solver_auv_model.c delete mode 100644 control/velocity_controller/scripts/build_auv_solver/acados_sim_solver_auv_model.h delete mode 100644 control/velocity_controller/scripts/build_auv_solver/acados_solver.pxd delete mode 100644 control/velocity_controller/scripts/build_auv_solver/acados_solver_auv_model.c delete mode 100644 control/velocity_controller/scripts/build_auv_solver/acados_solver_auv_model.h delete mode 100644 control/velocity_controller/scripts/build_auv_solver/acados_solver_auv_model.o delete mode 100644 control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun.c delete mode 100644 control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun.o delete mode 100644 control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u.c delete mode 100644 control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u.o delete mode 100644 control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u_z.c delete mode 100644 control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u_z.o delete mode 100644 control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_z.c delete mode 100644 control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_z.o delete mode 100644 control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_jac_x_xdot_u_z.c delete mode 100644 control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_jac_x_xdot_u_z.o delete mode 100644 control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_model.h delete mode 100755 control/velocity_controller/scripts/build_auv_solver/libacados_ocp_solver_auv_model.so delete mode 100644 control/velocity_controller/scripts/build_auv_solver/main_auv_model.c delete mode 100644 control/velocity_controller/scripts/build_auv_solver/main_sim_auv_model.c delete mode 100644 control/velocity_controller/scripts/dynamics.py delete mode 100644 control/velocity_controller/scripts/generator.py delete mode 100644 control/velocity_controller/src/NMPC_acados.cpp delete mode 100644 control/velocity_controller/src/NMPC_setup.cpp delete mode 100644 control/velocity_controller/tests/LQR_test.cpp diff --git a/control/velocity_controller/CMakeLists.txt b/control/velocity_controller/CMakeLists.txt index 30fd54084..04e477b4a 100644 --- a/control/velocity_controller/CMakeLists.txt +++ b/control/velocity_controller/CMakeLists.txt @@ -9,7 +9,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() -# find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) @@ -18,43 +17,20 @@ find_package(std_msgs REQUIRED) find_package(vortex_msgs REQUIRED) find_package(vortex_utils REQUIRED) find_package(Eigen3 REQUIRED) -#find_package(CasADi REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(ct_optcon REQUIRED) find_package(ct_core REQUIRED) find_package(casadi REQUIRED) -set(ACADOS_ROOT "$ENV{ACADOS_SOURCE_DIR}" CACHE PATH "acados root") include_directories( - ${ACADOS_ROOT}/include - ${ACADOS_ROOT}/include/acados - ${ACADOS_ROOT}/include/acados/ocp_nlp - ${ACADOS_ROOT}/include/acados_c - ${ACADOS_ROOT}/include/blasfeo/include - ${ACADOS_ROOT}/include/hpipm/include - ${ACADOS_ROOT}/include/qpOASES_e - ${CMAKE_CURRENT_SOURCE_DIR}/scripts/build_auv_solver - include - -) -link_directories(${ACADOS_ROOT}/lib) - - -# After setting ACADOS_ROOT etc. -file(GLOB_RECURSE GEN_SRCS - ${CMAKE_CURRENT_SOURCE_DIR}/scripts/build_auv_solver/*.c - ${CMAKE_CURRENT_SOURCE_DIR}/scripts/build_auv_solver/*/*.c -) - - - -add_library(auv_nmpc STATIC - src/NMPC_acados.cpp - ${GEN_SRCS} ) +#target_include_directories(velocity_controller_node PUBLIC +# $ +# $ +#) add_executable(velocity_controller_node src/velocity_controller.cpp @@ -62,18 +38,7 @@ add_executable(velocity_controller_node src/LQR_setup.cpp src/utilities.cpp src/ct_instantiations.cpp - src/NMPC_setup.cpp - #src/NMPC_acados.cpp ) - - - -#add_executable(test_LQR_node -# src/LQR_test.cpp -# src/LQR_setup.cpp -# src/utilities.cpp -# src/ct_instantiations.cpp -# ) ament_target_dependencies(velocity_controller_node rclcpp rclcpp_lifecycle @@ -81,13 +46,11 @@ ament_target_dependencies(velocity_controller_node std_msgs vortex_msgs geometry_msgs - # CasADi nav_msgs vortex_utils ) #target_include_directories(velocity_controller_node PRIVATE casadi Eigen3) -target_link_libraries(auv_nmpc acados hpipm blasfeo qpOASES_e m) -target_link_libraries(velocity_controller_node Eigen3::Eigen casadi::casadi ct_optcon ct_core auv_nmpc) +target_link_libraries(velocity_controller_node Eigen3::Eigen casadi::casadi ct_optcon ct_core) install(TARGETS velocity_controller_node DESTINATION lib/${PROJECT_NAME} @@ -107,7 +70,4 @@ if(BUILD_TESTING) add_subdirectory(tests) endif() - - - ament_package() diff --git a/control/velocity_controller/config/nautilus_params.yaml b/control/velocity_controller/config/nautilus_params.yaml index 3d183cae5..becd032bb 100644 --- a/control/velocity_controller/config/nautilus_params.yaml +++ b/control/velocity_controller/config/nautilus_params.yaml @@ -9,25 +9,16 @@ LQR_params: Q: [200.0,32.84,32.84,15.0,15.0,100.0,32.84,32.84] R: [0.02,3.1,3.10] - NMPCA_params: - Q: [1.0,1.0,1.0,5.0,5.0] # u,q,r,theta,psi - R: [0.1,0.1,0.1] # u_surge, u_theta, u_psi - N: 20 - NMPC_params: - Q: [100.0,0.0,0.0,0.0,1.0,1.0,0.0,5.0,50.0] # u,q,r,theta,psi - R: [0.1,0.1,0.1] # u_surge, u_theta, u_psi - N: 20 - #inertia_matrix: [ 30.0, 0.0, 0.0, 0.0, 0.0, 0.60, 0.0, 30.0, 0.0, 0.0, -0.60, 0.30, 0.0, 0.0, 30.0, 0.60, 0.30, 0.0, 0.0, 0.0, 0.60, 0.68, 0.0, 0.0, 0.0, -0.60, 0.30, 0.0, 3.32, 0.0, 0.60, 0.30, 0.0, 0.0, 0.0, 3.34] dampening_matrix_low: [23.0,0.0,0.0,0.0,0.0,0.0, 0.0,46.0,0.0,0.0,0.0,0.0, 0.0,0.0,46.0,0.0,0.0,0.0, 0.0,0.0,0.0,46.0,0.0,0.0, 0.0,0.0,0.0,0.0,46.0,0.0, 0.0,0.0,0.0,0.0,0.0,46.0] dampening_matrix_high: [1.0,0.0,0.0,0.0,0.0,0.0, 0.0,1.0,0.0,0.0,0.0,0.0, 0.0,0.0,1.0,0.0,0.0,0.0, 0.0,0.0,0.0,1.0,0.0,0.0, 0.0,0.0,0.0,0.0,1.0,0.0, 0.0,0.0,0.0,0.0,0.0,1.0] - publish_rate: 100 #ms - #Clamp parameter - max_force: 99.5 - controller_type: 2 #1 PID 2 LQR 3 NMPC 4NMPC fast - #Q: [300.0,0.01,0.01,0.01,32.84,32.84,32.84,32.84,32.84] # u,v,w,p,q,r,phi,theta,psi - # R: [0.02,3.1,3.10] # u_surge, u_theta, u_psi - - #Fixes: reduce oscillations, follows angles close to wrap around, model restoring forces in pitch, test different references, try to find out why the fuck it went backwards?, + Settings: #Settings for the controller + controller_type: 2 #1 PID 2 LQR + auto_start: false #0 for no, 1 for yes + anti_overshoot: false + reset_on_new_ref: true + odometry_dropout_guard: true + publish_rate: 100 #ms + max_force: 99.5 diff --git a/control/velocity_controller/config/orca_params.yaml b/control/velocity_controller/config/orca_params.yaml index b19015c95..9a111d51a 100644 --- a/control/velocity_controller/config/orca_params.yaml +++ b/control/velocity_controller/config/orca_params.yaml @@ -7,28 +7,21 @@ yaw: [10.0,1.0,5.0] LQR_params: - #Q: [200.0,62.84,62.84,15.0,15.0,10.0,3.84,3.84] - #R: [8.0,100.0,100.0] Q: [200.0,32.84,32.84,15.0,15.0,100.0,32.84,32.84] R: [0.02,3.1,3.10] - NMPCA_params: - Q: [1.0,1.0,1.0,5.0,5.0] # u,q,r,theta,psi - R: [0.1,0.1,0.1] # u_surge, u_theta, u_psi - N: 20 - NMPC_params: - Q: [100.0,0.0,0.0,0.0,1.0,1.0,0.0,5.0,50.0] # u,q,r,theta,psi - R: [0.1,0.1,0.1] # u_surge, u_theta, u_psi - N: 20 dampening_matrix_low: [23.0,0.0,0.0,0.0,0.0,0.0, 0.0,46.0,0.0,0.0,0.0,0.0, 0.0,0.0,46.0,0.0,0.0,0.0, 0.0,0.0,0.0,46.0,0.0,0.0, 0.0,0.0,0.0,0.0,46.0,0.0, 0.0,0.0,0.0,0.0,0.0,46.0] dampening_matrix_high: [1.0,0.0,0.0,0.0,0.0,0.0, 0.0,1.0,0.0,0.0,0.0,0.0, 0.0,0.0,1.0,0.0,0.0,0.0, 0.0,0.0,0.0,1.0,0.0,0.0, 0.0,0.0,0.0,0.0,1.0,0.0, 0.0,0.0,0.0,0.0,0.0,1.0] - publish_rate: 100 #ms - #Clamp parameter - max_force: 99.5 - controller_type: 2 #1 PID 2 LQR 3 NMPC 4NMPC fast - #Q: [300.0,0.01,0.01,0.01,32.84,32.84,32.84,32.84,32.84] # u,v,w,p,q,r,phi,theta,psi - # R: [0.02,3.1,3.10] # u_surge, u_theta, u_psi + Settings: #Settings for the controller + controller_type: 2 #1 PID 2 LQR + auto_start: false #0 for no, 1 for yes + anti_overshoot: false + reset_on_new_ref: true + odometry_dropout_guard: true + publish_rate: 100 #ms + max_force: 99.5 - #Fixes: reduce oscillations, follows angles close to wrap around, model restoring forces in pitch, test different references, try to find out why the fuck it went backwards?, + + diff --git a/control/velocity_controller/include/velocity_controller/LQR_setup.hpp b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp index 3ac78e980..e38cc68cb 100644 --- a/control/velocity_controller/include/velocity_controller/LQR_setup.hpp +++ b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp @@ -43,7 +43,7 @@ class LQRController{ public: LQRController(); - bool set_matrices(std::vector Q_,std::vector R_,std::vector inertia_matrix, double max_force,std::vector water_r_low,std::vector water_r_high); + bool set_matrices(std::vector Q_,std::vector R_,std::vector inertia_matrix, double max_force,std::vector D_low); void reset_controller(int nr=0); bool calculate_thrust(State states, Guidance_data guidance_values); bool set_interval(double interval); @@ -63,7 +63,7 @@ class LQRController{ double integral_error_surge; double integral_error_pitch; double integral_error_yaw; bool surge_windup; bool pitch_windup; bool yaw_windup; Eigen::Matrix Q; Eigen::Matrix R; Eigen::Matrix B; - Eigen::Matrix D_low; Eigen::Matrix D_high; + Eigen::Matrix D; double max_force, mass, Ixx, Iyy,Izz; Eigen::Matrix inertia_matrix_inv; diff --git a/control/velocity_controller/include/velocity_controller/NMPC_acados.hpp b/control/velocity_controller/include/velocity_controller/NMPC_acados.hpp deleted file mode 100644 index a54c495a6..000000000 --- a/control/velocity_controller/include/velocity_controller/NMPC_acados.hpp +++ /dev/null @@ -1,90 +0,0 @@ - -#pragma once -#include -#include -#include -#include -#include -#include - -// acados C API (generated) -extern "C" { -#include "acados_c/ocp_nlp_interface.h" -#include "acados/ocp_nlp/ocp_nlp_common.h" -#include "acados/utils/print.h" - -// Generated solver headers -#include "acados_solver_auv_model.h" // <-- from build_auv_solver/ -} - -class AuvNMPC { -public: - // Adjust sizes if your model differs - static constexpr int NX = AUV_MODEL_NX; // [u v w p q r phi theta psi] - static constexpr int NU = AUV_MODEL_NU; // [Fx My Mz] - static constexpr int NY = AUV_MODEL_NY; - static constexpr int NY_E = AUV_MODEL_NYN; - - -// Pass N if your generated header does not provide _acados_get_N() - explicit AuvNMPC(int N_horizon_override = -1) - : N_override_(N_horizon_override) {} - - ~AuvNMPC(); - - - // Not copyable - AuvNMPC(const AuvNMPC&) = delete; - AuvNMPC& operator=(const AuvNMPC&) = delete; - - // Lifecycle - bool init(); - - - void set_weights(const std::vector& W_diag, const std::vector& W_e_diag); - void set_max_force(double max_force); // updates bound on con_h: Fx^2+My^2+Mz^2 <= max^2 - - // Inputs - void setState(const std::array& x); - void setReference(const std::array& x_ref); - void setWeights(const std::vector& W_diag, const std::vector& W_e_diag); // sizes: NY, NY_E - void setMaxForce(double max_force); // updates con_h bounds - - - // One-shot solve: provide current state, desired state & input refs, get u0 back. - // Returns solver status (0 == success). - int solve_once(); - bool initialize_guess(std::array x,std::array u_init); - - // Outputs - std::vector getU0(); - - private: - - // generated capsule type (from acados_solver_auv_model.h) - auv_model_solver_capsule* capsule_ = nullptr; - - - // acados solver - ocp_nlp_solver* solver_ = nullptr; - ocp_nlp_config* config_ = nullptr; - ocp_nlp_dims* dims_ = nullptr; - ocp_nlp_in* nlp_in_ = nullptr; - ocp_nlp_out* nlp_out_= nullptr; - - int N_=20; - int N_override_=-1; - - std::array W_; // length NY - std::vector W_e_{NY_E*NY_E,0.0}; // length NY_E - double max_force2_ = 100*100; // squared constraint, default 100^2 - - - static void set_diag(double* M, int n, const std::vector& diag); - //U out - std::vector u0_out={0,0,0}; - //Recorded states states - std::array x0; - std::array xr; - std::array ur={0,0,0}; -}; diff --git a/control/velocity_controller/include/velocity_controller/NMPC_setup.hpp b/control/velocity_controller/include/velocity_controller/NMPC_setup.hpp deleted file mode 100644 index 19208d44c..000000000 --- a/control/velocity_controller/include/velocity_controller/NMPC_setup.hpp +++ /dev/null @@ -1,40 +0,0 @@ -#pragma once -//#include -#include -#include -#include "velocity_controller/utilities.hpp" - -class NMPC_controller{ - public: - Eigen::Matrix get_thrust(); - bool calculate_thrust(Guidance_data guidance_values, State state); - bool set_matrices(std::vector Q_,std::vector R_,std::vector inertia_matrix, double max_force,std::vector water_r_low,std::vector water_r_high); - void reset_controller(); - bool set_interval(double interval); - bool initialize_MPC(); - private: - Eigen::Matrix Q_; - Eigen::MatrixR_; - Eigen::MatrixM_inv; - Eigen::MatrixD_low; - Eigen::MatrixD_high; - //Eigen::MatrixB_; - double interval_; - double mass; - double Iz; - double Ix; - double Iy; - int N=3; - int n=9; - int m=3; - casadi::DM Z0_next; //For warm start - casadi::DM lbx; - casadi::DM ubx; - casadi::DM lbg; - casadi::DM ubg; - casadi::DM Pval; - casadi::Function solver; - Eigen::Matrixthrust; - - -}; \ No newline at end of file diff --git a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp index eff738c7c..a81483f9f 100644 --- a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp +++ b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp @@ -11,8 +11,6 @@ #include "LQR_setup.hpp" #include "nav_msgs/msg/odometry.hpp" #include "vortex_msgs/msg/los_guidance.hpp" -#include "velocity_controller/NMPC_setup.hpp" -#include "velocity_controller/NMPC_acados.hpp" #include #include @@ -84,22 +82,13 @@ class Velocity_node : public rclcpp_lifecycle::LifecycleNode{ std::vector inertia_matrix; std::vector dampening_matrix_low; std::vector dampening_matrix_high; - //NMPC controller - NMPC_controller NMPC; - //NMPC acados - AuvNMPC NMPC_acados; - std::vector Q2; - std::vector R2; - //NMPC parameters - std::vector Q3; - std::vector R3; std::atomic_bool should_exit_{false}; //VC settings - bool reset_on_new_ref=true; - bool anti_overshoot=false; - bool auto_start=false; - bool odometry_dropout_guard=true; + bool reset_on_new_ref; + bool anti_overshoot; + bool auto_start; + bool odometry_dropout_guard; int publish_counter=0; bool first_start=true; diff --git a/control/velocity_controller/scripts/auv_ocp.json b/control/velocity_controller/scripts/auv_ocp.json deleted file mode 100644 index 1becb5b1f..000000000 --- a/control/velocity_controller/scripts/auv_ocp.json +++ /dev/null @@ -1,1145 +0,0 @@ -{ - "code_gen_opts": { - "acados_include_path": "/home/henrik/ros2_ws_v/src/acados/include", - "acados_lib_path": "/home/henrik/ros2_ws_v/src/acados/lib", - "acados_link_libs": { - "clarabel": "", - "daqp": "", - "hpmpc": "", - "ooqp": "", - "openmp": "-fopenmp", - "osqp": "-losqp", - "qpdunes": "", - "qpoases": "-lqpOASES_e" - }, - "acados_version": "508482dac", - "code_export_directory": "/home/henrik/ros2_ws_v/src/vortex-auv/control/velocity_controller/scripts/build_auv_solver", - "cython_include_dirs": [ - "/usr/lib/python3/dist-packages/numpy/core/include", - "/usr/include/python3.10" - ], - "json_file": "auv_ocp.json", - "os": "unix", - "shared_lib_ext": ".so" - }, - "constraints": { - "C": [], - "C_e": [], - "D": [], - "constr_type": "BGH", - "constr_type_0": "BGH", - "constr_type_e": "BGH", - "constr_types": [ - "BGH", - "BGP" - ], - "has_x0": true, - "idxbu": [ - 0, - 1, - 2 - ], - "idxbx": [ - 7 - ], - "idxbx_0": [ - 0, - 1, - 2, - 3, - 4, - 5, - 6, - 7, - 8 - ], - "idxbx_e": [], - "idxbxe_0": [ - 0, - 1, - 2, - 3, - 4, - 5, - 6, - 7, - 8 - ], - "idxs_rev": [], - "idxs_rev_0": [], - "idxs_rev_e": [], - "idxsbu": [], - "idxsbx": [], - "idxsbx_e": [], - "idxsg": [], - "idxsg_e": [], - "idxsh": [], - "idxsh_0": [], - "idxsh_e": [], - "idxsphi": [], - "idxsphi_0": [], - "idxsphi_e": [], - "lbu": [ - -99.5, - -99.5, - -99.5 - ], - "lbx": [ - -1.4 - ], - "lbx_0": [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - "lbx_e": [], - "lg": [], - "lg_e": [], - "lh": [], - "lh_0": [], - "lh_e": [], - "lphi": [], - "lphi_0": [], - "lphi_e": [], - "ls": [], - "ls_0": [], - "ls_e": [], - "lsbu": [], - "lsbx": [], - "lsbx_e": [], - "lsg": [], - "lsg_e": [], - "lsh": [], - "lsh_0": [], - "lsh_e": [], - "lsphi": [], - "lsphi_0": [], - "lsphi_e": [], - "ubu": [ - 99.5, - 99.5, - 99.5 - ], - "ubx": [ - 1.4 - ], - "ubx_0": [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - "ubx_e": [], - "ug": [], - "ug_e": [], - "uh": [], - "uh_0": [], - "uh_e": [], - "uphi": [], - "uphi_0": [], - "uphi_e": [], - "us": [], - "us_0": [], - "us_e": [], - "usbu": [], - "usbx": [], - "usbx_e": [], - "usg": [], - "usg_e": [], - "ush": [], - "ush_0": [], - "ush_e": [], - "usphi": [], - "usphi_0": [], - "usphi_e": [] - }, - "cost": { - "Vu": [ - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 1.0, - 0.0, - 0.0 - ], - [ - 0.0, - 1.0, - 0.0 - ], - [ - 0.0, - 0.0, - 1.0 - ] - ], - "Vu_0": [ - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 1.0, - 0.0, - 0.0 - ], - [ - 0.0, - 1.0, - 0.0 - ], - [ - 0.0, - 0.0, - 1.0 - ] - ], - "Vx": [ - [ - 1.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 1.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 1.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 1.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 1.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ] - ], - "Vx_0": [ - [ - 1.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 1.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 1.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 1.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 1.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ] - ], - "Vx_e": [ - [ - 1.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 1.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 1.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 1.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 1.0 - ] - ], - "Vz": [], - "Vz_0": [], - "W": [ - [ - 100.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 1.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.1, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.1, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.1 - ] - ], - "W_0": [ - [ - 100.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 1.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.1, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.1, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.1 - ] - ], - "W_e": [ - [ - 100.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 1.0 - ] - ], - "Zl": [], - "Zl_0": [], - "Zl_e": [], - "Zu": [], - "Zu_0": [], - "Zu_e": [], - "cost_ext_fun_type": "casadi", - "cost_ext_fun_type_0": "casadi", - "cost_ext_fun_type_e": "casadi", - "cost_ext_fun_types": [ - "casadi", - "generic" - ], - "cost_function_ext_cost": null, - "cost_function_ext_cost_0": null, - "cost_function_ext_cost_e": null, - "cost_source_ext_cost": null, - "cost_source_ext_cost_0": null, - "cost_source_ext_cost_e": null, - "cost_type": "LINEAR_LS", - "cost_type_0": "LINEAR_LS", - "cost_type_e": "LINEAR_LS", - "cost_types": [ - "LINEAR_LS", - "NONLINEAR_LS", - "EXTERNAL", - "CONVEX_OVER_NONLINEAR", - "AUTO" - ], - "yref": [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - "yref_0": [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - "yref_e": [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - "zl": [], - "zl_0": [], - "zl_e": [], - "zu": [], - "zu_0": [], - "zu_e": [] - }, - "dims": { - "N": 20, - "n_global_data": 0, - "nbu": 3, - "nbx": 1, - "nbx_0": 9, - "nbx_e": 0, - "nbxe_0": 9, - "ng": 0, - "ng_e": 0, - "nh": 0, - "nh_0": 0, - "nh_e": 0, - "np": 0, - "np_global": 0, - "nphi": 0, - "nphi_0": 0, - "nphi_e": 0, - "nr": 0, - "nr_0": 0, - "nr_e": 0, - "ns": 0, - "ns_0": 0, - "ns_e": 0, - "nsbu": 0, - "nsbx": 0, - "nsbx_e": 0, - "nsg": 0, - "nsg_e": 0, - "nsh": 0, - "nsh_0": 0, - "nsh_e": 0, - "nsphi": 0, - "nsphi_0": 0, - "nsphi_e": 0, - "nu": 3, - "nx": 9, - "nx_next": 9, - "ny": 8, - "ny_0": 8, - "ny_e": 5, - "nz": 0 - }, - "external_function_files_model": [ - "auv_model_model/auv_model_impl_dae_fun.c", - "auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_z.c", - "auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u_z.c", - "auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u.c", - "auv_model_model/auv_model_impl_dae_jac_x_xdot_u_z.c" - ], - "external_function_files_ocp": [], - "hash": "78dcc9b0fd627d3908081bfa461a62e0", - "model": { - "con_h_expr": [], - "con_h_expr_0": [], - "con_h_expr_e": [], - "con_phi_expr": [], - "con_phi_expr_0": [], - "con_phi_expr_e": [], - "con_r_expr": [], - "con_r_expr_0": [], - "con_r_expr_e": [], - "con_r_in_phi": [], - "con_r_in_phi_0": [], - "con_r_in_phi_e": [], - "cost_conl_custom_outer_hess": [], - "cost_conl_custom_outer_hess_0": [], - "cost_conl_custom_outer_hess_e": [], - "cost_expr_ext_cost": [], - "cost_expr_ext_cost_0": [], - "cost_expr_ext_cost_custom_hess": [], - "cost_expr_ext_cost_custom_hess_0": [], - "cost_expr_ext_cost_custom_hess_e": [], - "cost_expr_ext_cost_e": [], - "cost_psi_expr": [], - "cost_psi_expr_0": [], - "cost_psi_expr_e": [], - "cost_r_in_psi_expr": [], - "cost_r_in_psi_expr_0": [], - "cost_r_in_psi_expr_e": [], - "cost_y_expr": [], - "cost_y_expr_0": [], - "cost_y_expr_e": [], - "disc_dyn_custom_hess_ux_expr": [], - "disc_dyn_custom_jac_ux_expr": [], - "disc_dyn_expr": [], - "dyn_disc_fun": null, - "dyn_disc_fun_jac": null, - "dyn_disc_fun_jac_hess": null, - "dyn_ext_fun_type": "casadi", - "dyn_generic_source": null, - "dyn_impl_dae_fun": null, - "dyn_impl_dae_fun_jac": null, - "dyn_impl_dae_jac": null, - "expression_names": [ - "f_expl_expr", - "f_impl_expr", - "p", - "p_global", - "pi", - "u", - "x", - "xdot", - "z" - ], - "f_expl_expr": "SX(@1=-30, @2=30, @3=((Fx-(((@1*r)*v)+((@2*q)*w)))-((23+fabs(u))*u)), @4=46, @5=((((@2*r)*u)+((@1*p)*w))+((@4+fabs(v))*v)), @6=((((@1*q)*u)+((@2*p)*v))+((@4+fabs(w))*w)), @7=((((3.34*r)*q)+((-3.32*q)*r))+((@4+fabs(p))*p)), @8=((My-(((-3.34*r)*p)+((0.68*p)*r)))-((@4+fabs(q))*q)), @9=((Mz-(((3.32*q)*p)+((-0.68*p)*q)))-((@4+fabs(r))*r)), @10=-0.000546005, [((((((0.0334536*@3)-(6.0369e-05*@5))-(-1.11163e-07*@6))-(9.80847e-08*@7))+(1.09201e-05*@8))+(-0.00601506*@9)), ((((((6.0369e-05*@3)-(0.0334847*@5))-(-6.16582e-05*@6))-(5.44043e-05*@7))+(0.00605702*@8))+(-0.00301845*@9)), ((((((-1.11163e-07*@3)-(-6.16582e-05*@5))-(0.0339635*@6))-(-0.0299678*@7))+(-0.00308013*@8))+(5.55814e-06*@9)), ((((((9.80847e-08*@3)-(5.44043e-05*@5))-(-0.0299678*@6))-(1.49703*@7))+(0.00271776*@8))+(-4.90424e-06*@9)), ((((((1.09201e-05*@3)-(0.00605702*@5))-(-0.00308013*@6))-(0.00271776*@7))+(0.302578*@8))+(@10*@9)), ((((((-0.00601506*@3)-(-0.00301845*@5))-(5.55814e-06*@6))-(-4.90424e-06*@7))+(@10*@8))+(0.300753*@9)), ((p+((sin(phi)*tan(theta))*q))+((cos(phi)*tan(theta))*r)), ((cos(phi)*q)-(sin(phi)*r)), (((sin(phi)/cos(theta))*q)+((cos(phi)/cos(theta))*r))])", - "f_impl_expr": "SX(@1=-30, @2=30, @3=((Fx-(((@1*r)*v)+((@2*q)*w)))-((23+fabs(u))*u)), @4=46, @5=((((@2*r)*u)+((@1*p)*w))+((@4+fabs(v))*v)), @6=((((@1*q)*u)+((@2*p)*v))+((@4+fabs(w))*w)), @7=((((3.34*r)*q)+((-3.32*q)*r))+((@4+fabs(p))*p)), @8=((My-(((-3.34*r)*p)+((0.68*p)*r)))-((@4+fabs(q))*q)), @9=((Mz-(((3.32*q)*p)+((-0.68*p)*q)))-((@4+fabs(r))*r)), @10=-0.000546005, [(xdot_0-((((((0.0334536*@3)-(6.0369e-05*@5))-(-1.11163e-07*@6))-(9.80847e-08*@7))+(1.09201e-05*@8))+(-0.00601506*@9))), (xdot_1-((((((6.0369e-05*@3)-(0.0334847*@5))-(-6.16582e-05*@6))-(5.44043e-05*@7))+(0.00605702*@8))+(-0.00301845*@9))), (xdot_2-((((((-1.11163e-07*@3)-(-6.16582e-05*@5))-(0.0339635*@6))-(-0.0299678*@7))+(-0.00308013*@8))+(5.55814e-06*@9))), (xdot_3-((((((9.80847e-08*@3)-(5.44043e-05*@5))-(-0.0299678*@6))-(1.49703*@7))+(0.00271776*@8))+(-4.90424e-06*@9))), (xdot_4-((((((1.09201e-05*@3)-(0.00605702*@5))-(-0.00308013*@6))-(0.00271776*@7))+(0.302578*@8))+(@10*@9))), (xdot_5-((((((-0.00601506*@3)-(-0.00301845*@5))-(5.55814e-06*@6))-(-4.90424e-06*@7))+(@10*@8))+(0.300753*@9))), (xdot_6-((p+((sin(phi)*tan(theta))*q))+((cos(phi)*tan(theta))*r))), (xdot_7-((cos(phi)*q)-(sin(phi)*r))), (xdot_8-(((sin(phi)/cos(theta))*q)+((cos(phi)/cos(theta))*r)))])", - "gnsf_model": null, - "name": "auv_model", - "nu_original": null, - "p": "SX(0x1)", - "p_global": "SX(0x1)", - "pi": "SX([pi_0, pi_1, pi_2, pi_3, pi_4, pi_5, pi_6, pi_7, pi_8])", - "serialized_expressions": "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", - "t": [], - "t0": null, - "t_label": "t", - "u": "SX([Fx, My, Mz])", - "u_labels": [ - "u0", - "u1", - "u2" - ], - "x": "SX([u, v, w, p, q, r, phi, theta, psi])", - "x_labels": [ - "x0", - "x1", - "x2", - "x3", - "x4", - "x5", - "x6", - "x7", - "x8" - ], - "xdot": "SX([xdot_0, xdot_1, xdot_2, xdot_3, xdot_4, xdot_5, xdot_6, xdot_7, xdot_8])", - "z": "SX(0x1)" - }, - "name": "auv_model", - "p_global_values": [], - "parameter_values": [], - "problem_class": "OCP", - "ros_opts": null, - "simulink_opts": null, - "solver_options": { - "N_horizon": 20, - "Tsim": 0.1, - "adaptive_levenberg_marquardt_lam": 5.0, - "adaptive_levenberg_marquardt_mu0": 0.001, - "adaptive_levenberg_marquardt_mu_min": 1e-16, - "adaptive_levenberg_marquardt_obj_scalar": 2.0, - "allow_direction_mode_switch_to_nominal": true, - "anderson_activation_threshold": 10.0, - "as_rti_iter": 1, - "as_rti_level": 4, - "byrd_omojokon_slack_relaxation_factor": 1.00001, - "collocation_type": "GAUSS_LEGENDRE", - "cost_discretization": "EULER", - "cost_scaling": [ - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 1.0 - ], - "custom_templates": [], - "custom_update_copy": true, - "custom_update_filename": "", - "custom_update_header_filename": "", - "eval_residual_at_max_iter": false, - "exact_hess_constr": 1, - "exact_hess_cost": 1, - "exact_hess_dyn": 1, - "ext_cost_num_hess": 0, - "ext_fun_compile_flags": "-O2", - "ext_fun_expand_constr": false, - "ext_fun_expand_cost": false, - "ext_fun_expand_dyn": false, - "ext_fun_expand_precompute": false, - "fixed_hess": 0, - "globalization": "FIXED_STEP", - "globalization_alpha_min": 0.05, - "globalization_alpha_reduction": 0.7, - "globalization_eps_sufficient_descent": 0.0001, - "globalization_fixed_step_length": 1.0, - "globalization_full_step_dual": 0, - "globalization_funnel_fraction_switching_condition": 0.001, - "globalization_funnel_init_increase_factor": 15.0, - "globalization_funnel_init_upper_bound": 1.0, - "globalization_funnel_initial_penalty_parameter": 1.0, - "globalization_funnel_kappa": 0.9, - "globalization_funnel_sufficient_decrease_factor": 0.9, - "globalization_funnel_use_merit_fun_only": false, - "globalization_line_search_use_sufficient_descent": 0, - "globalization_use_SOC": 0, - "hessian_approx": "GAUSS_NEWTON", - "hpipm_mode": "BALANCE", - "integrator_type": "IRK", - "levenberg_marquardt": 0.01, - "log_dual_step_norm": false, - "log_primal_step_norm": false, - "model_external_shared_lib_dir": null, - "model_external_shared_lib_name": null, - "nlp_qp_tol_min_comp": 1e-11, - "nlp_qp_tol_min_eq": 1e-10, - "nlp_qp_tol_min_ineq": 1e-10, - "nlp_qp_tol_min_stat": 1e-09, - "nlp_qp_tol_reduction_factor": 0.1, - "nlp_qp_tol_safety_factor": 0.1, - "nlp_qp_tol_strategy": "FIXED_QP_TOL", - "nlp_solver_ext_qp_res": 0, - "nlp_solver_max_iter": 100, - "nlp_solver_tol_comp": 1e-06, - "nlp_solver_tol_eq": 1e-06, - "nlp_solver_tol_ineq": 1e-06, - "nlp_solver_tol_min_step_norm": 0.0, - "nlp_solver_tol_stat": 1e-06, - "nlp_solver_type": "SQP", - "nlp_solver_warm_start_first_qp": false, - "nlp_solver_warm_start_first_qp_from_nlp": false, - "print_level": 2, - "qp_solver": "FULL_CONDENSING_HPIPM", - "qp_solver_cond_N": 20, - "qp_solver_cond_block_size": null, - "qp_solver_cond_ric_alg": 1, - "qp_solver_iter_max": 50, - "qp_solver_mu0": 0.0, - "qp_solver_ric_alg": 1, - "qp_solver_t0_init": 2, - "qp_solver_tol_comp": null, - "qp_solver_tol_eq": null, - "qp_solver_tol_ineq": null, - "qp_solver_tol_stat": null, - "qp_solver_warm_start": 1, - "qpscaling_lb_norm_inf_grad_obj": 0.0001, - "qpscaling_scale_constraints": "NO_CONSTRAINT_SCALING", - "qpscaling_scale_objective": "NO_OBJECTIVE_SCALING", - "qpscaling_ub_max_abs_eig": 100000.0, - "reg_adaptive_eps": false, - "reg_epsilon": 0.0001, - "reg_max_cond_block": 10000000.0, - "reg_min_epsilon": 1e-08, - "regularize_method": "NO_REGULARIZE", - "rti_log_only_available_residuals": 0, - "rti_log_residuals": 0, - "search_direction_mode": "NOMINAL_QP", - "sens_forw_p": false, - "shooting_nodes": [ - 0.0, - 0.1, - 0.2, - 0.30000000000000004, - 0.4, - 0.5, - 0.6, - 0.7, - 0.7999999999999999, - 0.8999999999999999, - 0.9999999999999999, - 1.0999999999999999, - 1.2, - 1.3, - 1.4000000000000001, - 1.5000000000000002, - 1.6000000000000003, - 1.7000000000000004, - 1.8000000000000005, - 1.9000000000000006, - 2.0000000000000004 - ], - "sim_method_jac_reuse": [ - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0 - ], - "sim_method_newton_iter": 3, - "sim_method_newton_tol": 0.0, - "sim_method_num_stages": [ - 2, - 2, - 2, - 2, - 2, - 2, - 2, - 2, - 2, - 2, - 2, - 2, - 2, - 2, - 2, - 2, - 2, - 2, - 2, - 2 - ], - "sim_method_num_steps": [ - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4 - ], - "solution_sens_qp_t_lam_min": 1e-09, - "store_iterates": false, - "tau_min": 0.0, - "tf": 2.0, - "time_steps": [ - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1 - ], - "timeout_heuristic": "LAST", - "timeout_max_time": 0.0, - "use_constraint_hessian_in_feas_qp": false, - "with_adaptive_levenberg_marquardt": false, - "with_anderson_acceleration": false, - "with_batch_functionality": false, - "with_solution_sens_wrt_params": false, - "with_value_sens_wrt_params": false - }, - "zoro_description": null -} \ No newline at end of file diff --git a/control/velocity_controller/scripts/build_auv_solver/Makefile b/control/velocity_controller/scripts/build_auv_solver/Makefile deleted file mode 100644 index 7e20d181d..000000000 --- a/control/velocity_controller/scripts/build_auv_solver/Makefile +++ /dev/null @@ -1,208 +0,0 @@ -# -# Copyright (c) The acados authors. -# -# This file is part of acados. -# -# The 2-Clause BSD License -# -# 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. -# -# 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 HOLDER 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.; -# - - - - - -# define sources and use make's implicit rules to generate object files (*.o) - -# model -MODEL_SRC= - -MODEL_SRC+= auv_model_model/auv_model_impl_dae_fun.c -MODEL_SRC+= auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_z.c -MODEL_SRC+= auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u_z.c -MODEL_SRC+= auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u.c -MODEL_SRC+= auv_model_model/auv_model_impl_dae_jac_x_xdot_u_z.c - -MODEL_OBJ := $(MODEL_SRC:.c=.o) -# optimal control problem - mostly CasADi exports -OCP_SRC= - - - -OCP_SRC+= acados_solver_auv_model.c - -OCP_OBJ := $(OCP_SRC:.c=.o) -# for sim solver -SIM_SRC= acados_sim_solver_auv_model.c -SIM_OBJ := $(SIM_SRC:.c=.o) - -# for target example_sim -EX_SIM_SRC= main_sim_auv_model.c -EX_SIM_OBJ := $(EX_SIM_SRC:.c=.o) -EX_SIM_EXE := $(EX_SIM_SRC:.c=) -# for target example -EX_SRC= main_auv_model.c -EX_OBJ := $(EX_SRC:.c=.o) -EX_EXE := $(EX_SRC:.c=) - - -# combine model, (potentially) sim and ocp object files -OBJ= -OBJ+= $(MODEL_OBJ) -OBJ+= $(SIM_OBJ) -OBJ+= $(OCP_OBJ) - -EXTERNAL_DIR= -EXTERNAL_LIB= - -INCLUDE_PATH = /home/henrik/ros2_ws_v/src/acados/include -LIB_PATH = /home/henrik/ros2_ws_v/src/acados/lib - -# preprocessor flags for make's implicit rules -CPPFLAGS+= -I$(INCLUDE_PATH) -CPPFLAGS+= -I$(INCLUDE_PATH)/acados -CPPFLAGS+= -I$(INCLUDE_PATH)/blasfeo/include -CPPFLAGS+= -I$(INCLUDE_PATH)/hpipm/include - - -# define the c-compiler flags for make's implicit rules -CFLAGS = -fPIC -std=c99 -O2 - -# # Debugging -# CFLAGS += -g3 -fno-diagnostics-show-line-numbers -g - -# linker flags -LDFLAGS+= -L$(LIB_PATH) - - -# link to libraries -LDLIBS+= -lacados -LDLIBS+= -lhpipm -LDLIBS+= -lblasfeo -LDLIBS+= -lm -LDLIBS+= - -# libraries -LIBACADOS_SOLVER=libacados_solver_auv_model.so -LIBACADOS_OCP_SOLVER=libacados_ocp_solver_auv_model.so -LIBACADOS_SIM_SOLVER=lib$(SIM_SRC:.c=.so) - -# virtual targets -.PHONY : all clean - - all: clean example_sim example -shared_lib: bundled_shared_lib ocp_shared_lib sim_shared_lib - -# some linker targets -example: $(EX_OBJ) $(OBJ) - $(CC) $^ -o $(EX_EXE) $(LDFLAGS) $(LDLIBS) - -example_sim: $(EX_SIM_OBJ) $(MODEL_OBJ) $(SIM_OBJ) - $(CC) $^ -o $(EX_SIM_EXE) $(LDFLAGS) $(LDLIBS) - -sim_shared_lib: $(SIM_OBJ) $(MODEL_OBJ) - $(CC) -shared $^ -o $(LIBACADOS_SIM_SOLVER) $(LDFLAGS) $(LDLIBS) -bundled_shared_lib: $(OBJ) - $(CC) -shared $^ -o $(LIBACADOS_SOLVER) $(LDFLAGS) $(LDLIBS) - -ocp_shared_lib: $(OCP_OBJ) $(MODEL_OBJ) - $(CC) -shared $^ -o $(LIBACADOS_OCP_SOLVER) $(LDFLAGS) $(LDLIBS) \ - -L$(EXTERNAL_DIR) -l$(EXTERNAL_LIB) - - -# Cython targets -ocp_cython_c: ocp_shared_lib - cython \ - -o acados_ocp_solver_pyx.c \ - -I $(INCLUDE_PATH)/../interfaces/acados_template/acados_template \ - $(INCLUDE_PATH)/../interfaces/acados_template/acados_template/acados_ocp_solver_pyx.pyx \ - -I /home/henrik/ros2_ws_v/src/vortex-auv/control/velocity_controller/scripts/build_auv_solver \ - -ocp_cython_o: ocp_cython_c - $(CC) $(ACADOS_FLAGS) -c -O2 \ - -fPIC \ - -DNPY_NO_DEPRECATED_API=NPY_1_7_API_VERSION \ - -o acados_ocp_solver_pyx.o \ - -I $(INCLUDE_PATH)/blasfeo/include/ \ - -I $(INCLUDE_PATH)/hpipm/include/ \ - -I $(INCLUDE_PATH) \ - -I /usr/lib/python3/dist-packages/numpy/core/include \ - -I /usr/include/python3.10 \ - acados_ocp_solver_pyx.c \ - -ocp_cython: ocp_cython_o - $(CC) $(ACADOS_FLAGS) -shared \ - -o acados_ocp_solver_pyx.so \ - -Wl,-rpath=$(LIB_PATH) \ - acados_ocp_solver_pyx.o \ - $(abspath .)/libacados_ocp_solver_auv_model.so \ - $(LDFLAGS) $(LDLIBS) - - -# Sim Cython targets -sim_cython_c: sim_shared_lib - cython \ - -o acados_sim_solver_pyx.c \ - -I $(INCLUDE_PATH)/../interfaces/acados_template/acados_template \ - $(INCLUDE_PATH)/../interfaces/acados_template/acados_template/acados_sim_solver_pyx.pyx \ - -I /home/henrik/ros2_ws_v/src/vortex-auv/control/velocity_controller/scripts/build_auv_solver \ - -sim_cython_o: sim_cython_c - $(CC) $(ACADOS_FLAGS) -c -O2 \ - -fPIC \ - -DNPY_NO_DEPRECATED_API=NPY_1_7_API_VERSION \ - -o acados_sim_solver_pyx.o \ - -I $(INCLUDE_PATH)/blasfeo/include/ \ - -I $(INCLUDE_PATH)/hpipm/include/ \ - -I $(INCLUDE_PATH) \ - -I /usr/lib/python3/dist-packages/numpy/core/include \ - -I /usr/include/python3.10 \ - acados_sim_solver_pyx.c \ - -sim_cython: sim_cython_o - $(CC) $(ACADOS_FLAGS) -shared \ - -o acados_sim_solver_pyx.so \ - -Wl,-rpath=$(LIB_PATH) \ - acados_sim_solver_pyx.o \ - $(abspath .)/libacados_sim_solver_auv_model.so \ - $(LDFLAGS) $(LDLIBS) - -clean: - $(RM) $(OBJ) $(EX_OBJ) $(EX_SIM_OBJ) - $(RM) $(LIBACADOS_SOLVER) $(LIBACADOS_OCP_SOLVER) $(LIBACADOS_SIM_SOLVER) - $(RM) $(EX_EXE) $(EX_SIM_EXE) -clean_ocp_shared_lib: - $(RM) $(LIBACADOS_OCP_SOLVER) - $(RM) $(OCP_OBJ) - -clean_ocp_cython: - $(RM) libacados_ocp_solver_auv_model.so - $(RM) acados_solver_auv_model.o - $(RM) acados_ocp_solver_pyx.so - $(RM) acados_ocp_solver_pyx.o - -clean_sim_cython: - $(RM) libacados_sim_solver_auv_model.so - $(RM) acados_sim_solver_auv_model.o - $(RM) acados_sim_solver_pyx.so - $(RM) acados_sim_solver_pyx.o diff --git a/control/velocity_controller/scripts/build_auv_solver/acados_sim_solver_auv_model.c b/control/velocity_controller/scripts/build_auv_solver/acados_sim_solver_auv_model.c deleted file mode 100644 index 3da4f7b13..000000000 --- a/control/velocity_controller/scripts/build_auv_solver/acados_sim_solver_auv_model.c +++ /dev/null @@ -1,310 +0,0 @@ -/* - * Copyright (c) The acados authors. - * - * This file is part of acados. - * - * The 2-Clause BSD License - * - * 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. - * - * 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 HOLDER 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.; - */ -// standard -#include -#include - -// acados -#include "acados_c/external_function_interface.h" -#include "acados_c/sim_interface.h" -#include "acados_c/external_function_interface.h" - -#include "acados/sim/sim_common.h" -#include "acados/utils/external_function_generic.h" -#include "acados/utils/print.h" - - -// example specific -#include "auv_model_model/auv_model_model.h" -#include "acados_sim_solver_auv_model.h" - - -// ** solver data ** - -auv_model_sim_solver_capsule * auv_model_acados_sim_solver_create_capsule() -{ - void* capsule_mem = malloc(sizeof(auv_model_sim_solver_capsule)); - auv_model_sim_solver_capsule *capsule = (auv_model_sim_solver_capsule *) capsule_mem; - - return capsule; -} - - -int auv_model_acados_sim_solver_free_capsule(auv_model_sim_solver_capsule * capsule) -{ - free(capsule); - return 0; -} - - -int auv_model_acados_sim_create(auv_model_sim_solver_capsule * capsule) -{ - // initialize - const int nx = AUV_MODEL_NX; - const int nu = AUV_MODEL_NU; - const int nz = AUV_MODEL_NZ; - const int np = AUV_MODEL_NP; - bool tmp_bool; - - double Tsim = 0.1; - - capsule->acados_sim_mem = NULL; - - external_function_opts ext_fun_opts; - external_function_opts_set_to_default(&ext_fun_opts); - ext_fun_opts.external_workspace = false; - - - capsule->sim_impl_dae_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); - capsule->sim_impl_dae_fun_jac_x_xdot_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); - capsule->sim_impl_dae_jac_x_xdot_u_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); - - - capsule->sim_impl_dae_jac_p = NULL; - - // external functions (implicit model) - capsule->sim_impl_dae_fun->casadi_fun = &auv_model_impl_dae_fun; - capsule->sim_impl_dae_fun->casadi_work = &auv_model_impl_dae_fun_work; - capsule->sim_impl_dae_fun->casadi_sparsity_in = &auv_model_impl_dae_fun_sparsity_in; - capsule->sim_impl_dae_fun->casadi_sparsity_out = &auv_model_impl_dae_fun_sparsity_out; - capsule->sim_impl_dae_fun->casadi_n_in = &auv_model_impl_dae_fun_n_in; - capsule->sim_impl_dae_fun->casadi_n_out = &auv_model_impl_dae_fun_n_out; - external_function_param_casadi_create(capsule->sim_impl_dae_fun, np, &ext_fun_opts); - - capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_fun = &auv_model_impl_dae_fun_jac_x_xdot_z; - capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_work = &auv_model_impl_dae_fun_jac_x_xdot_z_work; - capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_sparsity_in = &auv_model_impl_dae_fun_jac_x_xdot_z_sparsity_in; - capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_sparsity_out = &auv_model_impl_dae_fun_jac_x_xdot_z_sparsity_out; - capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_n_in = &auv_model_impl_dae_fun_jac_x_xdot_z_n_in; - capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_n_out = &auv_model_impl_dae_fun_jac_x_xdot_z_n_out; - external_function_param_casadi_create(capsule->sim_impl_dae_fun_jac_x_xdot_z, np, &ext_fun_opts); - - capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_fun = &auv_model_impl_dae_jac_x_xdot_u_z; - capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_work = &auv_model_impl_dae_jac_x_xdot_u_z_work; - capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_sparsity_in = &auv_model_impl_dae_jac_x_xdot_u_z_sparsity_in; - capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_sparsity_out = &auv_model_impl_dae_jac_x_xdot_u_z_sparsity_out; - capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_n_in = &auv_model_impl_dae_jac_x_xdot_u_z_n_in; - capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_n_out = &auv_model_impl_dae_jac_x_xdot_u_z_n_out; - external_function_param_casadi_create(capsule->sim_impl_dae_jac_x_xdot_u_z, np, &ext_fun_opts); - - - - - - // sim plan & config - sim_solver_plan_t plan; - plan.sim_solver = IRK; - - // create correct config based on plan - sim_config * auv_model_sim_config = sim_config_create(plan); - capsule->acados_sim_config = auv_model_sim_config; - - // sim dims - void *auv_model_sim_dims = sim_dims_create(auv_model_sim_config); - capsule->acados_sim_dims = auv_model_sim_dims; - sim_dims_set(auv_model_sim_config, auv_model_sim_dims, "nx", &nx); - sim_dims_set(auv_model_sim_config, auv_model_sim_dims, "nu", &nu); - sim_dims_set(auv_model_sim_config, auv_model_sim_dims, "nz", &nz); - sim_dims_set(auv_model_sim_config, auv_model_sim_dims, "np", &np); - - - // sim opts - sim_opts *auv_model_sim_opts = sim_opts_create(auv_model_sim_config, auv_model_sim_dims); - capsule->acados_sim_opts = auv_model_sim_opts; - int tmp_int = 3; - sim_opts_set(auv_model_sim_config, auv_model_sim_opts, "newton_iter", &tmp_int); - double tmp_double = 0; - sim_opts_set(auv_model_sim_config, auv_model_sim_opts, "newton_tol", &tmp_double); - sim_collocation_type collocation_type = GAUSS_LEGENDRE; - sim_opts_set(auv_model_sim_config, auv_model_sim_opts, "collocation_type", &collocation_type); - - - tmp_int = 2; - sim_opts_set(auv_model_sim_config, auv_model_sim_opts, "num_stages", &tmp_int); - tmp_int = 4; - sim_opts_set(auv_model_sim_config, auv_model_sim_opts, "num_steps", &tmp_int); - tmp_bool = 0; - sim_opts_set(auv_model_sim_config, auv_model_sim_opts, "jac_reuse", &tmp_bool); - - - // sim in / out - sim_in *auv_model_sim_in = sim_in_create(auv_model_sim_config, auv_model_sim_dims); - capsule->acados_sim_in = auv_model_sim_in; - sim_out *auv_model_sim_out = sim_out_create(auv_model_sim_config, auv_model_sim_dims); - capsule->acados_sim_out = auv_model_sim_out; - - sim_in_set(auv_model_sim_config, auv_model_sim_dims, - auv_model_sim_in, "T", &Tsim); - - // model functions - auv_model_sim_config->model_set(auv_model_sim_in->model, - "impl_ode_fun", capsule->sim_impl_dae_fun); - auv_model_sim_config->model_set(auv_model_sim_in->model, - "impl_ode_fun_jac_x_xdot", capsule->sim_impl_dae_fun_jac_x_xdot_z); - auv_model_sim_config->model_set(auv_model_sim_in->model, - "impl_ode_jac_x_xdot_u", capsule->sim_impl_dae_jac_x_xdot_u_z); - - - // sim solver - sim_solver *auv_model_sim_solver = sim_solver_create(auv_model_sim_config, - auv_model_sim_dims, auv_model_sim_opts, auv_model_sim_in); - capsule->acados_sim_solver = auv_model_sim_solver; - - capsule->acados_sim_mem = auv_model_sim_solver->mem; - - - - /* initialize input */ - // x - double x0[9]; - for (int ii = 0; ii < 9; ii++) - x0[ii] = 0.0; - - sim_in_set(auv_model_sim_config, auv_model_sim_dims, - auv_model_sim_in, "x", x0); - - - // u - double u0[3]; - for (int ii = 0; ii < 3; ii++) - u0[ii] = 0.0; - - sim_in_set(auv_model_sim_config, auv_model_sim_dims, - auv_model_sim_in, "u", u0); - - // S_forw - double S_forw[108]; - for (int ii = 0; ii < 108; ii++) - S_forw[ii] = 0.0; - for (int ii = 0; ii < 9; ii++) - S_forw[ii + ii * 9 ] = 1.0; - - - sim_in_set(auv_model_sim_config, auv_model_sim_dims, - auv_model_sim_in, "S_forw", S_forw); - - int status = sim_precompute(auv_model_sim_solver, auv_model_sim_in, auv_model_sim_out); - - return status; -} - - -int auv_model_acados_sim_solve(auv_model_sim_solver_capsule *capsule) -{ - // integrate dynamics using acados sim_solver - int status = sim_solve(capsule->acados_sim_solver, - capsule->acados_sim_in, capsule->acados_sim_out); - if (status != 0) - printf("error in auv_model_acados_sim_solve()! Exiting.\n"); - - return status; -} - - - - -int auv_model_acados_sim_free(auv_model_sim_solver_capsule *capsule) -{ - // free memory - sim_solver_destroy(capsule->acados_sim_solver); - sim_in_destroy(capsule->acados_sim_in); - sim_out_destroy(capsule->acados_sim_out); - sim_opts_destroy(capsule->acados_sim_opts); - sim_dims_destroy(capsule->acados_sim_dims); - sim_config_destroy(capsule->acados_sim_config); - - // free external function - external_function_param_casadi_free(capsule->sim_impl_dae_fun); - external_function_param_casadi_free(capsule->sim_impl_dae_fun_jac_x_xdot_z); - external_function_param_casadi_free(capsule->sim_impl_dae_jac_x_xdot_u_z); - - free(capsule->sim_impl_dae_fun); - free(capsule->sim_impl_dae_fun_jac_x_xdot_z); - free(capsule->sim_impl_dae_jac_x_xdot_u_z); - - - return 0; -} - - -int auv_model_acados_sim_update_params(auv_model_sim_solver_capsule *capsule, double *p, int np) -{ - int status = 0; - int casadi_np = AUV_MODEL_NP; - - if (casadi_np != np) { - printf("auv_model_acados_sim_update_params: trying to set %i parameters for external functions." - " External function has %i parameters. Exiting.\n", np, casadi_np); - exit(1); - } - capsule->sim_impl_dae_fun[0].set_param(capsule->sim_impl_dae_fun, p); - capsule->sim_impl_dae_fun_jac_x_xdot_z[0].set_param(capsule->sim_impl_dae_fun_jac_x_xdot_z, p); - capsule->sim_impl_dae_jac_x_xdot_u_z[0].set_param(capsule->sim_impl_dae_jac_x_xdot_u_z, p); - - - return status; -} - -/* getters pointers to C objects*/ -sim_config * auv_model_acados_get_sim_config(auv_model_sim_solver_capsule *capsule) -{ - return capsule->acados_sim_config; -}; - -sim_in * auv_model_acados_get_sim_in(auv_model_sim_solver_capsule *capsule) -{ - return capsule->acados_sim_in; -}; - -sim_out * auv_model_acados_get_sim_out(auv_model_sim_solver_capsule *capsule) -{ - return capsule->acados_sim_out; -}; - -void * auv_model_acados_get_sim_dims(auv_model_sim_solver_capsule *capsule) -{ - return capsule->acados_sim_dims; -}; - -sim_opts * auv_model_acados_get_sim_opts(auv_model_sim_solver_capsule *capsule) -{ - return capsule->acados_sim_opts; -}; - -sim_solver * auv_model_acados_get_sim_solver(auv_model_sim_solver_capsule *capsule) -{ - return capsule->acados_sim_solver; -}; - -void * auv_model_acados_get_sim_mem(auv_model_sim_solver_capsule *capsule) -{ - return capsule->acados_sim_mem; -}; - diff --git a/control/velocity_controller/scripts/build_auv_solver/acados_sim_solver_auv_model.h b/control/velocity_controller/scripts/build_auv_solver/acados_sim_solver_auv_model.h deleted file mode 100644 index beed43e59..000000000 --- a/control/velocity_controller/scripts/build_auv_solver/acados_sim_solver_auv_model.h +++ /dev/null @@ -1,105 +0,0 @@ -/* - * Copyright (c) The acados authors. - * - * This file is part of acados. - * - * The 2-Clause BSD License - * - * 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. - * - * 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 HOLDER 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.; - */ - -#ifndef ACADOS_SIM_auv_model_H_ -#define ACADOS_SIM_auv_model_H_ - -#include "acados_c/sim_interface.h" -#include "acados_c/external_function_interface.h" - -#define AUV_MODEL_NX 9 -#define AUV_MODEL_NZ 0 -#define AUV_MODEL_NU 3 -#define AUV_MODEL_NP 0 - -#ifdef __cplusplus -extern "C" { -#endif - - -// ** capsule for solver data ** -typedef struct auv_model_sim_solver_capsule -{ - // acados objects - sim_in *acados_sim_in; - sim_out *acados_sim_out; - sim_solver *acados_sim_solver; - sim_opts *acados_sim_opts; - sim_config *acados_sim_config; - void *acados_sim_dims; - void *acados_sim_mem; - - /* external functions */ - // ERK - external_function_param_casadi * sim_expl_vde_forw; - external_function_param_casadi * sim_vde_adj_casadi; - external_function_param_casadi * sim_expl_ode_fun_casadi; - external_function_param_casadi * sim_expl_ode_hess; - external_function_param_casadi * sim_expl_vde_forw_p; - - // IRK - external_function_param_casadi * sim_impl_dae_fun; - external_function_param_casadi * sim_impl_dae_fun_jac_x_xdot_z; - external_function_param_casadi * sim_impl_dae_jac_x_xdot_u_z; - external_function_param_casadi * sim_impl_dae_hess; - external_function_param_casadi * sim_impl_dae_jac_p; - - // GNSF - external_function_param_casadi * sim_gnsf_phi_fun; - external_function_param_casadi * sim_gnsf_phi_fun_jac_y; - external_function_param_casadi * sim_gnsf_phi_jac_y_uhat; - external_function_param_casadi * sim_gnsf_f_lo_jac_x1_x1dot_u_z; - external_function_param_casadi * sim_gnsf_get_matrices_fun; - -} auv_model_sim_solver_capsule; - - -ACADOS_SYMBOL_EXPORT int auv_model_acados_sim_create(auv_model_sim_solver_capsule *capsule); -ACADOS_SYMBOL_EXPORT int auv_model_acados_sim_solve(auv_model_sim_solver_capsule *capsule); - -ACADOS_SYMBOL_EXPORT int auv_model_acados_sim_free(auv_model_sim_solver_capsule *capsule); -ACADOS_SYMBOL_EXPORT int auv_model_acados_sim_update_params(auv_model_sim_solver_capsule *capsule, double *value, int np); - -ACADOS_SYMBOL_EXPORT sim_config * auv_model_acados_get_sim_config(auv_model_sim_solver_capsule *capsule); -ACADOS_SYMBOL_EXPORT sim_in * auv_model_acados_get_sim_in(auv_model_sim_solver_capsule *capsule); -ACADOS_SYMBOL_EXPORT sim_out * auv_model_acados_get_sim_out(auv_model_sim_solver_capsule *capsule); -ACADOS_SYMBOL_EXPORT void * auv_model_acados_get_sim_dims(auv_model_sim_solver_capsule *capsule); -ACADOS_SYMBOL_EXPORT sim_opts * auv_model_acados_get_sim_opts(auv_model_sim_solver_capsule *capsule); -ACADOS_SYMBOL_EXPORT sim_solver * auv_model_acados_get_sim_solver(auv_model_sim_solver_capsule *capsule); -ACADOS_SYMBOL_EXPORT void * auv_model_acados_get_sim_mem(auv_model_sim_solver_capsule *capsule); - -ACADOS_SYMBOL_EXPORT auv_model_sim_solver_capsule * auv_model_acados_sim_solver_create_capsule(void); -ACADOS_SYMBOL_EXPORT int auv_model_acados_sim_solver_free_capsule(auv_model_sim_solver_capsule *capsule); - -#ifdef __cplusplus -} -#endif - -#endif // ACADOS_SIM_auv_model_H_ diff --git a/control/velocity_controller/scripts/build_auv_solver/acados_solver.pxd b/control/velocity_controller/scripts/build_auv_solver/acados_solver.pxd deleted file mode 100644 index a6a039341..000000000 --- a/control/velocity_controller/scripts/build_auv_solver/acados_solver.pxd +++ /dev/null @@ -1,63 +0,0 @@ -# -# Copyright (c) The acados authors. -# -# This file is part of acados. -# -# The 2-Clause BSD License -# -# 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. -# -# 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 HOLDER 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.; -# - -cimport acados_solver_common - -cdef extern from "acados_solver_auv_model.h": - ctypedef struct nlp_solver_capsule "auv_model_solver_capsule": - pass - - nlp_solver_capsule * acados_create_capsule "auv_model_acados_create_capsule"() - int acados_free_capsule "auv_model_acados_free_capsule"(nlp_solver_capsule *capsule) - - int acados_create "auv_model_acados_create"(nlp_solver_capsule * capsule) - - int acados_create_with_discretization "auv_model_acados_create_with_discretization"(nlp_solver_capsule * capsule, int n_time_steps, double* new_time_steps) - int acados_update_time_steps "auv_model_acados_update_time_steps"(nlp_solver_capsule * capsule, int N, double* new_time_steps) - int acados_update_qp_solver_cond_N "auv_model_acados_update_qp_solver_cond_N"(nlp_solver_capsule * capsule, int qp_solver_cond_N) - - int acados_update_params "auv_model_acados_update_params"(nlp_solver_capsule * capsule, int stage, double *value, int np_) - int acados_update_params_sparse "auv_model_acados_update_params_sparse"(nlp_solver_capsule * capsule, int stage, int *idx, double *p, int n_update) - int acados_set_p_global_and_precompute_dependencies "auv_model_acados_set_p_global_and_precompute_dependencies"(nlp_solver_capsule * capsule, double *value, int data_len) - int acados_solve "auv_model_acados_solve"(nlp_solver_capsule * capsule) - int acados_reset "auv_model_acados_reset"(nlp_solver_capsule * capsule, int reset_qp_solver_mem) - int acados_free "auv_model_acados_free"(nlp_solver_capsule * capsule) - void acados_print_stats "auv_model_acados_print_stats"(nlp_solver_capsule * capsule) - - int acados_custom_update "auv_model_acados_custom_update"(nlp_solver_capsule* capsule, double * data, int data_len) - - acados_solver_common.ocp_nlp_in *acados_get_nlp_in "auv_model_acados_get_nlp_in"(nlp_solver_capsule * capsule) - acados_solver_common.ocp_nlp_out *acados_get_nlp_out "auv_model_acados_get_nlp_out"(nlp_solver_capsule * capsule) - acados_solver_common.ocp_nlp_out *acados_get_sens_out "auv_model_acados_get_sens_out"(nlp_solver_capsule * capsule) - acados_solver_common.ocp_nlp_solver *acados_get_nlp_solver "auv_model_acados_get_nlp_solver"(nlp_solver_capsule * capsule) - acados_solver_common.ocp_nlp_config *acados_get_nlp_config "auv_model_acados_get_nlp_config"(nlp_solver_capsule * capsule) - void *acados_get_nlp_opts "auv_model_acados_get_nlp_opts"(nlp_solver_capsule * capsule) - acados_solver_common.ocp_nlp_dims *acados_get_nlp_dims "auv_model_acados_get_nlp_dims"(nlp_solver_capsule * capsule) - acados_solver_common.ocp_nlp_plan *acados_get_nlp_plan "auv_model_acados_get_nlp_plan"(nlp_solver_capsule * capsule) diff --git a/control/velocity_controller/scripts/build_auv_solver/acados_solver_auv_model.c b/control/velocity_controller/scripts/build_auv_solver/acados_solver_auv_model.c deleted file mode 100644 index 6fbe2da06..000000000 --- a/control/velocity_controller/scripts/build_auv_solver/acados_solver_auv_model.c +++ /dev/null @@ -1,1187 +0,0 @@ -/* - * Copyright (c) The acados authors. - * - * This file is part of acados. - * - * The 2-Clause BSD License - * - * 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. - * - * 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 HOLDER 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.; - */ - -// standard -#include -#include -#include -// acados -// #include "acados/utils/print.h" -#include "acados_c/ocp_nlp_interface.h" -#include "acados_c/external_function_interface.h" - -// example specific - -#include "auv_model_model/auv_model_model.h" - - - - - -#include "acados_solver_auv_model.h" - -#define NX AUV_MODEL_NX -#define NZ AUV_MODEL_NZ -#define NU AUV_MODEL_NU -#define NP AUV_MODEL_NP -#define NP_GLOBAL AUV_MODEL_NP_GLOBAL -#define NY0 AUV_MODEL_NY0 -#define NY AUV_MODEL_NY -#define NYN AUV_MODEL_NYN - -#define NBX AUV_MODEL_NBX -#define NBX0 AUV_MODEL_NBX0 -#define NBU AUV_MODEL_NBU -#define NG AUV_MODEL_NG -#define NBXN AUV_MODEL_NBXN -#define NGN AUV_MODEL_NGN - -#define NH AUV_MODEL_NH -#define NHN AUV_MODEL_NHN -#define NH0 AUV_MODEL_NH0 -#define NPHI AUV_MODEL_NPHI -#define NPHIN AUV_MODEL_NPHIN -#define NPHI0 AUV_MODEL_NPHI0 -#define NR AUV_MODEL_NR - -#define NS AUV_MODEL_NS -#define NS0 AUV_MODEL_NS0 -#define NSN AUV_MODEL_NSN - -#define NSBX AUV_MODEL_NSBX -#define NSBU AUV_MODEL_NSBU -#define NSH0 AUV_MODEL_NSH0 -#define NSH AUV_MODEL_NSH -#define NSHN AUV_MODEL_NSHN -#define NSG AUV_MODEL_NSG -#define NSPHI0 AUV_MODEL_NSPHI0 -#define NSPHI AUV_MODEL_NSPHI -#define NSPHIN AUV_MODEL_NSPHIN -#define NSGN AUV_MODEL_NSGN -#define NSBXN AUV_MODEL_NSBXN - - - -// ** solver data ** - -auv_model_solver_capsule * auv_model_acados_create_capsule(void) -{ - void* capsule_mem = malloc(sizeof(auv_model_solver_capsule)); - auv_model_solver_capsule *capsule = (auv_model_solver_capsule *) capsule_mem; - - return capsule; -} - - -int auv_model_acados_free_capsule(auv_model_solver_capsule *capsule) -{ - free(capsule); - return 0; -} - - -int auv_model_acados_create(auv_model_solver_capsule* capsule) -{ - int N_shooting_intervals = AUV_MODEL_N; - double* new_time_steps = NULL; // NULL -> don't alter the code generated time-steps - return auv_model_acados_create_with_discretization(capsule, N_shooting_intervals, new_time_steps); -} - - -int auv_model_acados_update_time_steps(auv_model_solver_capsule* capsule, int N, double* new_time_steps) -{ - - if (N != capsule->nlp_solver_plan->N) { - fprintf(stderr, "auv_model_acados_update_time_steps: given number of time steps (= %d) " \ - "differs from the currently allocated number of " \ - "time steps (= %d)!\n" \ - "Please recreate with new discretization and provide a new vector of time_stamps!\n", - N, capsule->nlp_solver_plan->N); - return 1; - } - - ocp_nlp_config * nlp_config = capsule->nlp_config; - ocp_nlp_dims * nlp_dims = capsule->nlp_dims; - ocp_nlp_in * nlp_in = capsule->nlp_in; - - for (int i = 0; i < N; i++) - { - ocp_nlp_in_set(nlp_config, nlp_dims, nlp_in, i, "Ts", &new_time_steps[i]); - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "scaling", &new_time_steps[i]); - } - return 0; - -} - -/** - * Internal function for auv_model_acados_create: step 1 - */ -void auv_model_acados_create_set_plan(ocp_nlp_plan_t* nlp_solver_plan, const int N) -{ - assert(N == nlp_solver_plan->N); - - /************************************************ - * plan - ************************************************/ - - nlp_solver_plan->nlp_solver = SQP; - - nlp_solver_plan->ocp_qp_solver_plan.qp_solver = FULL_CONDENSING_HPIPM; - nlp_solver_plan->relaxed_ocp_qp_solver_plan.qp_solver = FULL_CONDENSING_HPIPM; - nlp_solver_plan->nlp_cost[0] = LINEAR_LS; - for (int i = 1; i < N; i++) - nlp_solver_plan->nlp_cost[i] = LINEAR_LS; - - nlp_solver_plan->nlp_cost[N] = LINEAR_LS; - - for (int i = 0; i < N; i++) - { - nlp_solver_plan->nlp_dynamics[i] = CONTINUOUS_MODEL; - nlp_solver_plan->sim_solver_plan[i].sim_solver = IRK; - } - - nlp_solver_plan->nlp_constraints[0] = BGH; - - for (int i = 1; i < N; i++) - { - nlp_solver_plan->nlp_constraints[i] = BGH; - } - nlp_solver_plan->nlp_constraints[N] = BGH; - - nlp_solver_plan->regularization = NO_REGULARIZE; - - nlp_solver_plan->globalization = FIXED_STEP; -} - - -static ocp_nlp_dims* auv_model_acados_create_setup_dimensions(auv_model_solver_capsule* capsule) -{ - ocp_nlp_plan_t* nlp_solver_plan = capsule->nlp_solver_plan; - const int N = nlp_solver_plan->N; - ocp_nlp_config* nlp_config = capsule->nlp_config; - - /************************************************ - * dimensions - ************************************************/ - #define NINTNP1MEMS 18 - int* intNp1mem = (int*)malloc( (N+1)*sizeof(int)*NINTNP1MEMS ); - - int* nx = intNp1mem + (N+1)*0; - int* nu = intNp1mem + (N+1)*1; - int* nbx = intNp1mem + (N+1)*2; - int* nbu = intNp1mem + (N+1)*3; - int* nsbx = intNp1mem + (N+1)*4; - int* nsbu = intNp1mem + (N+1)*5; - int* nsg = intNp1mem + (N+1)*6; - int* nsh = intNp1mem + (N+1)*7; - int* nsphi = intNp1mem + (N+1)*8; - int* ns = intNp1mem + (N+1)*9; - int* ng = intNp1mem + (N+1)*10; - int* nh = intNp1mem + (N+1)*11; - int* nphi = intNp1mem + (N+1)*12; - int* nz = intNp1mem + (N+1)*13; - int* ny = intNp1mem + (N+1)*14; - int* nr = intNp1mem + (N+1)*15; - int* nbxe = intNp1mem + (N+1)*16; - int* np = intNp1mem + (N+1)*17; - - for (int i = 0; i < N+1; i++) - { - // common - nx[i] = NX; - nu[i] = NU; - nz[i] = NZ; - ns[i] = NS; - // cost - ny[i] = NY; - // constraints - nbx[i] = NBX; - nbu[i] = NBU; - nsbx[i] = NSBX; - nsbu[i] = NSBU; - nsg[i] = NSG; - nsh[i] = NSH; - nsphi[i] = NSPHI; - ng[i] = NG; - nh[i] = NH; - nphi[i] = NPHI; - nr[i] = NR; - nbxe[i] = 0; - np[i] = NP; - } - - // for initial state - nbx[0] = NBX0; - nsbx[0] = 0; - ns[0] = NS0; - - nbxe[0] = 9; - - ny[0] = NY0; - nh[0] = NH0; - nsh[0] = NSH0; - nsphi[0] = NSPHI0; - nphi[0] = NPHI0; - - - // terminal - common - nu[N] = 0; - nz[N] = 0; - ns[N] = NSN; - // cost - ny[N] = NYN; - // constraint - nbx[N] = NBXN; - nbu[N] = 0; - ng[N] = NGN; - nh[N] = NHN; - nphi[N] = NPHIN; - nr[N] = 0; - - nsbx[N] = NSBXN; - nsbu[N] = 0; - nsg[N] = NSGN; - nsh[N] = NSHN; - nsphi[N] = NSPHIN; - - /* create and set ocp_nlp_dims */ - ocp_nlp_dims * nlp_dims = ocp_nlp_dims_create(nlp_config); - - ocp_nlp_dims_set_opt_vars(nlp_config, nlp_dims, "nx", nx); - ocp_nlp_dims_set_opt_vars(nlp_config, nlp_dims, "nu", nu); - ocp_nlp_dims_set_opt_vars(nlp_config, nlp_dims, "nz", nz); - ocp_nlp_dims_set_opt_vars(nlp_config, nlp_dims, "ns", ns); - ocp_nlp_dims_set_opt_vars(nlp_config, nlp_dims, "np", np); - - ocp_nlp_dims_set_global(nlp_config, nlp_dims, "np_global", 0); - ocp_nlp_dims_set_global(nlp_config, nlp_dims, "n_global_data", 0); - - for (int i = 0; i <= N; i++) - { - ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nbx", &nbx[i]); - ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nbu", &nbu[i]); - ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nsbx", &nsbx[i]); - ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nsbu", &nsbu[i]); - ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "ng", &ng[i]); - ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nsg", &nsg[i]); - ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nbxe", &nbxe[i]); - } - ocp_nlp_dims_set_cost(nlp_config, nlp_dims, 0, "ny", &ny[0]); - for (int i = 1; i < N; i++) - ocp_nlp_dims_set_cost(nlp_config, nlp_dims, i, "ny", &ny[i]); - ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, 0, "nh", &nh[0]); - ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, 0, "nsh", &nsh[0]); - - for (int i = 1; i < N; i++) - { - ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nh", &nh[i]); - ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nsh", &nsh[i]); - } - ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, N, "nh", &nh[N]); - ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, N, "nsh", &nsh[N]); - ocp_nlp_dims_set_cost(nlp_config, nlp_dims, N, "ny", &ny[N]); - free(intNp1mem); - - return nlp_dims; -} - - -/** - * Internal function for auv_model_acados_create: step 3 - */ -void auv_model_acados_create_setup_functions(auv_model_solver_capsule* capsule) -{ - const int N = capsule->nlp_solver_plan->N; - - /************************************************ - * external functions - ************************************************/ - -#define MAP_CASADI_FNC(__CAPSULE_FNC__, __MODEL_BASE_FNC__) do{ \ - capsule->__CAPSULE_FNC__.casadi_fun = & __MODEL_BASE_FNC__ ;\ - capsule->__CAPSULE_FNC__.casadi_n_in = & __MODEL_BASE_FNC__ ## _n_in; \ - capsule->__CAPSULE_FNC__.casadi_n_out = & __MODEL_BASE_FNC__ ## _n_out; \ - capsule->__CAPSULE_FNC__.casadi_sparsity_in = & __MODEL_BASE_FNC__ ## _sparsity_in; \ - capsule->__CAPSULE_FNC__.casadi_sparsity_out = & __MODEL_BASE_FNC__ ## _sparsity_out; \ - capsule->__CAPSULE_FNC__.casadi_work = & __MODEL_BASE_FNC__ ## _work; \ - external_function_external_param_casadi_create(&capsule->__CAPSULE_FNC__, &ext_fun_opts); \ - } while(false) - - external_function_opts ext_fun_opts; - external_function_opts_set_to_default(&ext_fun_opts); - - - ext_fun_opts.external_workspace = true; - if (N > 0) - { - - - - - // implicit dae - capsule->impl_dae_fun = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*N); - for (int i = 0; i < N; i++) { - MAP_CASADI_FNC(impl_dae_fun[i], auv_model_impl_dae_fun); - } - - capsule->impl_dae_fun_jac_x_xdot_z = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*N); - for (int i = 0; i < N; i++) { - MAP_CASADI_FNC(impl_dae_fun_jac_x_xdot_z[i], auv_model_impl_dae_fun_jac_x_xdot_z); - } - - capsule->impl_dae_jac_x_xdot_u_z = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*N); - for (int i = 0; i < N; i++) { - MAP_CASADI_FNC(impl_dae_jac_x_xdot_u_z[i], auv_model_impl_dae_jac_x_xdot_u_z); - } - - - - } // N > 0 - -#undef MAP_CASADI_FNC -} - - -/** - * Internal function for auv_model_acados_create: step 5 - */ -void auv_model_acados_create_set_default_parameters(auv_model_solver_capsule* capsule) -{ - - // no parameters defined - - - // no global parameters defined -} - - -/** - * Internal function for auv_model_acados_create: step 5 - */ -void auv_model_acados_setup_nlp_in(auv_model_solver_capsule* capsule, const int N, double* new_time_steps) -{ - assert(N == capsule->nlp_solver_plan->N); - ocp_nlp_config* nlp_config = capsule->nlp_config; - ocp_nlp_dims* nlp_dims = capsule->nlp_dims; - - int tmp_int = 0; - - /************************************************ - * nlp_in - ************************************************/ - ocp_nlp_in * nlp_in = capsule->nlp_in; - /************************************************ - * nlp_out - ************************************************/ - ocp_nlp_out * nlp_out = capsule->nlp_out; - - // set up time_steps and cost_scaling - - if (new_time_steps) - { - // NOTE: this sets scaling and time_steps - auv_model_acados_update_time_steps(capsule, N, new_time_steps); - } - else - { - // set time_steps - - double time_step = 0.1; - for (int i = 0; i < N; i++) - { - ocp_nlp_in_set(nlp_config, nlp_dims, nlp_in, i, "Ts", &time_step); - } - // set cost scaling - double* cost_scaling = malloc((N+1)*sizeof(double)); - cost_scaling[0] = 0.1; - cost_scaling[1] = 0.1; - cost_scaling[2] = 0.1; - cost_scaling[3] = 0.1; - cost_scaling[4] = 0.1; - cost_scaling[5] = 0.1; - cost_scaling[6] = 0.1; - cost_scaling[7] = 0.1; - cost_scaling[8] = 0.1; - cost_scaling[9] = 0.1; - cost_scaling[10] = 0.1; - cost_scaling[11] = 0.1; - cost_scaling[12] = 0.1; - cost_scaling[13] = 0.1; - cost_scaling[14] = 0.1; - cost_scaling[15] = 0.1; - cost_scaling[16] = 0.1; - cost_scaling[17] = 0.1; - cost_scaling[18] = 0.1; - cost_scaling[19] = 0.1; - cost_scaling[20] = 1; - for (int i = 0; i <= N; i++) - { - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "scaling", &cost_scaling[i]); - } - free(cost_scaling); - } - - - - /**** Dynamics ****/ - for (int i = 0; i < N; i++) - { - ocp_nlp_dynamics_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, i, "impl_dae_fun", &capsule->impl_dae_fun[i]); - ocp_nlp_dynamics_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, i, - "impl_dae_fun_jac_x_xdot_z", &capsule->impl_dae_fun_jac_x_xdot_z[i]); - ocp_nlp_dynamics_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, i, - "impl_dae_jac_x_xdot_u", &capsule->impl_dae_jac_x_xdot_u_z[i]); - - } - - /**** Cost ****/ - double* yref_0 = calloc(NY0, sizeof(double)); - // change only the non-zero elements: - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "yref", yref_0); - free(yref_0); - - double* W_0 = calloc(NY0*NY0, sizeof(double)); - // change only the non-zero elements: - W_0[0+(NY0) * 0] = 100; - W_0[4+(NY0) * 4] = 1; - W_0[5+(NY0) * 5] = 0.1; - W_0[6+(NY0) * 6] = 0.1; - W_0[7+(NY0) * 7] = 0.1; - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "W", W_0); - free(W_0); - double* Vx_0 = calloc(NY0*NX, sizeof(double)); - // change only the non-zero elements: - Vx_0[0+(NY0) * 0] = 1; - Vx_0[1+(NY0) * 4] = 1; - Vx_0[2+(NY0) * 5] = 1; - Vx_0[3+(NY0) * 7] = 1; - Vx_0[4+(NY0) * 8] = 1; - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "Vx", Vx_0); - free(Vx_0); - double* Vu_0 = calloc(NY0*NU, sizeof(double)); - // change only the non-zero elements: - Vu_0[5+(NY0) * 0] = 1; - Vu_0[6+(NY0) * 1] = 1; - Vu_0[7+(NY0) * 2] = 1; - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "Vu", Vu_0); - free(Vu_0); - double* yref = calloc(NY, sizeof(double)); - // change only the non-zero elements: - - for (int i = 1; i < N; i++) - { - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "yref", yref); - } - free(yref); - double* W = calloc(NY*NY, sizeof(double)); - // change only the non-zero elements: - W[0+(NY) * 0] = 100; - W[4+(NY) * 4] = 1; - W[5+(NY) * 5] = 0.1; - W[6+(NY) * 6] = 0.1; - W[7+(NY) * 7] = 0.1; - - for (int i = 1; i < N; i++) - { - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "W", W); - } - free(W); - double* Vx = calloc(NY*NX, sizeof(double)); - // change only the non-zero elements: - Vx[0+(NY) * 0] = 1; - Vx[1+(NY) * 4] = 1; - Vx[2+(NY) * 5] = 1; - Vx[3+(NY) * 7] = 1; - Vx[4+(NY) * 8] = 1; - for (int i = 1; i < N; i++) - { - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "Vx", Vx); - } - free(Vx); - - - double* Vu = calloc(NY*NU, sizeof(double)); - // change only the non-zero elements: - Vu[5+(NY) * 0] = 1; - Vu[6+(NY) * 1] = 1; - Vu[7+(NY) * 2] = 1; - - for (int i = 1; i < N; i++) - { - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "Vu", Vu); - } - free(Vu); - double* yref_e = calloc(NYN, sizeof(double)); - // change only the non-zero elements: - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "yref", yref_e); - free(yref_e); - - double* W_e = calloc(NYN*NYN, sizeof(double)); - // change only the non-zero elements: - W_e[0+(NYN) * 0] = 100; - W_e[4+(NYN) * 4] = 1; - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "W", W_e); - free(W_e); - double* Vx_e = calloc(NYN*NX, sizeof(double)); - // change only the non-zero elements: - Vx_e[0+(NYN) * 0] = 1; - Vx_e[1+(NYN) * 4] = 1; - Vx_e[2+(NYN) * 5] = 1; - Vx_e[3+(NYN) * 7] = 1; - Vx_e[4+(NYN) * 8] = 1; - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "Vx", Vx_e); - free(Vx_e); - - - - - - - - /**** Constraints ****/ - - // bounds for initial stage - // x0 - int* idxbx0 = malloc(NBX0 * sizeof(int)); - idxbx0[0] = 0; - idxbx0[1] = 1; - idxbx0[2] = 2; - idxbx0[3] = 3; - idxbx0[4] = 4; - idxbx0[5] = 5; - idxbx0[6] = 6; - idxbx0[7] = 7; - idxbx0[8] = 8; - - double* lubx0 = calloc(2*NBX0, sizeof(double)); - double* lbx0 = lubx0; - double* ubx0 = lubx0 + NBX0; - // change only the non-zero elements: - - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "idxbx", idxbx0); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "lbx", lbx0); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "ubx", ubx0); - free(idxbx0); - free(lubx0); - // idxbxe_0 - int* idxbxe_0 = malloc(9 * sizeof(int)); - idxbxe_0[0] = 0; - idxbxe_0[1] = 1; - idxbxe_0[2] = 2; - idxbxe_0[3] = 3; - idxbxe_0[4] = 4; - idxbxe_0[5] = 5; - idxbxe_0[6] = 6; - idxbxe_0[7] = 7; - idxbxe_0[8] = 8; - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "idxbxe", idxbxe_0); - free(idxbxe_0); - - - - - - - - - - - - - /* constraints that are the same for initial and intermediate */ - // u - int* idxbu = malloc(NBU * sizeof(int)); - idxbu[0] = 0; - idxbu[1] = 1; - idxbu[2] = 2; - double* lubu = calloc(2*NBU, sizeof(double)); - double* lbu = lubu; - double* ubu = lubu + NBU; - lbu[0] = -99.5; - ubu[0] = 99.5; - lbu[1] = -99.5; - ubu[1] = 99.5; - lbu[2] = -99.5; - ubu[2] = 99.5; - - for (int i = 0; i < N; i++) - { - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxbu", idxbu); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lbu", lbu); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "ubu", ubu); - } - free(idxbu); - free(lubu); - - - - - - - /* Path constraints */ - - // x - int* idxbx = malloc(NBX * sizeof(int)); - idxbx[0] = 7; - double* lubx = calloc(2*NBX, sizeof(double)); - double* lbx = lubx; - double* ubx = lubx + NBX; - lbx[0] = -1.4; - ubx[0] = 1.4; - - for (int i = 1; i < N; i++) - { - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxbx", idxbx); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lbx", lbx); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "ubx", ubx); - } - free(idxbx); - free(lubx); - - - - - - - - - - - - - - /* terminal constraints */ - - - - - - - - - - - - - - - - - - - - -} - - -static void auv_model_acados_create_set_opts(auv_model_solver_capsule* capsule) -{ - const int N = capsule->nlp_solver_plan->N; - ocp_nlp_config* nlp_config = capsule->nlp_config; - void *nlp_opts = capsule->nlp_opts; - - /************************************************ - * opts - ************************************************/ - - - - int fixed_hess = 0; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "fixed_hess", &fixed_hess); - - double globalization_fixed_step_length = 1; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "globalization_fixed_step_length", &globalization_fixed_step_length); - - - - - int with_solution_sens_wrt_params = false; - ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "with_solution_sens_wrt_params", &with_solution_sens_wrt_params); - - int with_value_sens_wrt_params = false; - ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "with_value_sens_wrt_params", &with_value_sens_wrt_params); - - double solution_sens_qp_t_lam_min = 0.000000001; - ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "solution_sens_qp_t_lam_min", &solution_sens_qp_t_lam_min); - - int globalization_full_step_dual = 0; - ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "globalization_full_step_dual", &globalization_full_step_dual); - - // set collocation type (relevant for implicit integrators) - sim_collocation_type collocation_type = GAUSS_LEGENDRE; - for (int i = 0; i < N; i++) - ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_collocation_type", &collocation_type); - - // set up sim_method_num_steps - // all sim_method_num_steps are identical - int sim_method_num_steps = 4; - for (int i = 0; i < N; i++) - ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_num_steps", &sim_method_num_steps); - - // set up sim_method_num_stages - // all sim_method_num_stages are identical - int sim_method_num_stages = 2; - for (int i = 0; i < N; i++) - ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_num_stages", &sim_method_num_stages); - - int newton_iter_val = 3; - for (int i = 0; i < N; i++) - ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_newton_iter", &newton_iter_val); - - double newton_tol_val = 0; - for (int i = 0; i < N; i++) - ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_newton_tol", &newton_tol_val); - - // set up sim_method_jac_reuse - bool tmp_bool = (bool) 0; - for (int i = 0; i < N; i++) - ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_jac_reuse", &tmp_bool); - - double levenberg_marquardt = 0.01; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "levenberg_marquardt", &levenberg_marquardt); - - /* options QP solver */ - - int nlp_solver_ext_qp_res = 0; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "ext_qp_res", &nlp_solver_ext_qp_res); - - bool store_iterates = false; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "store_iterates", &store_iterates); - int log_primal_step_norm = false; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "log_primal_step_norm", &log_primal_step_norm); - - int log_dual_step_norm = false; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "log_dual_step_norm", &log_dual_step_norm); - - double nlp_solver_tol_min_step_norm = 0; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "tol_min_step_norm", &nlp_solver_tol_min_step_norm); - // set HPIPM mode: should be done before setting other QP solver options - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_hpipm_mode", "BALANCE"); - - - - int qp_solver_t0_init = 2; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_t0_init", &qp_solver_t0_init); - - - - - // set SQP specific options - double nlp_solver_tol_stat = 0.000001; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "tol_stat", &nlp_solver_tol_stat); - - double nlp_solver_tol_eq = 0.000001; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "tol_eq", &nlp_solver_tol_eq); - - double nlp_solver_tol_ineq = 0.000001; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "tol_ineq", &nlp_solver_tol_ineq); - - double nlp_solver_tol_comp = 0.000001; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "tol_comp", &nlp_solver_tol_comp); - - int nlp_solver_max_iter = 100; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "max_iter", &nlp_solver_max_iter); - - // set options for adaptive Levenberg-Marquardt Update - bool with_adaptive_levenberg_marquardt = false; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "with_adaptive_levenberg_marquardt", &with_adaptive_levenberg_marquardt); - - double adaptive_levenberg_marquardt_lam = 5; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "adaptive_levenberg_marquardt_lam", &adaptive_levenberg_marquardt_lam); - - double adaptive_levenberg_marquardt_mu_min = 0.0000000000000001; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "adaptive_levenberg_marquardt_mu_min", &adaptive_levenberg_marquardt_mu_min); - - double adaptive_levenberg_marquardt_mu0 = 0.001; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "adaptive_levenberg_marquardt_mu0", &adaptive_levenberg_marquardt_mu0); - - double adaptive_levenberg_marquardt_obj_scalar = 2; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "adaptive_levenberg_marquardt_obj_scalar", &adaptive_levenberg_marquardt_obj_scalar); - - bool eval_residual_at_max_iter = false; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "eval_residual_at_max_iter", &eval_residual_at_max_iter); - - // QP scaling - double qpscaling_ub_max_abs_eig = 100000; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qpscaling_ub_max_abs_eig", &qpscaling_ub_max_abs_eig); - - double qpscaling_lb_norm_inf_grad_obj = 0.0001; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qpscaling_lb_norm_inf_grad_obj", &qpscaling_lb_norm_inf_grad_obj); - - qpscaling_scale_objective_type qpscaling_scale_objective = NO_OBJECTIVE_SCALING; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qpscaling_scale_objective", &qpscaling_scale_objective); - - ocp_nlp_qpscaling_constraint_type qpscaling_scale_constraints = NO_CONSTRAINT_SCALING; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qpscaling_scale_constraints", &qpscaling_scale_constraints); - - // NLP QP tol strategy - ocp_nlp_qp_tol_strategy_t nlp_qp_tol_strategy = FIXED_QP_TOL; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_strategy", &nlp_qp_tol_strategy); - - double nlp_qp_tol_reduction_factor = 0.1; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_reduction_factor", &nlp_qp_tol_reduction_factor); - - double nlp_qp_tol_safety_factor = 0.1; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_safety_factor", &nlp_qp_tol_safety_factor); - - double nlp_qp_tol_min_stat = 0.000000001; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_min_stat", &nlp_qp_tol_min_stat); - - double nlp_qp_tol_min_eq = 0.0000000001; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_min_eq", &nlp_qp_tol_min_eq); - - double nlp_qp_tol_min_ineq = 0.0000000001; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_min_ineq", &nlp_qp_tol_min_ineq); - - double nlp_qp_tol_min_comp = 0.00000000001; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_min_comp", &nlp_qp_tol_min_comp); - - bool with_anderson_acceleration = false; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "with_anderson_acceleration", &with_anderson_acceleration); - - double anderson_activation_threshold = 10; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "anderson_activation_threshold", &anderson_activation_threshold); - - int qp_solver_iter_max = 50; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_iter_max", &qp_solver_iter_max); - - - int qp_solver_warm_start = 1; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_warm_start", &qp_solver_warm_start); - - int print_level = 2; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "print_level", &print_level); - - int ext_cost_num_hess = 0; -} - - -/** - * Internal function for auv_model_acados_create: step 7 - */ -void auv_model_acados_set_nlp_out(auv_model_solver_capsule* capsule) -{ - const int N = capsule->nlp_solver_plan->N; - ocp_nlp_config* nlp_config = capsule->nlp_config; - ocp_nlp_dims* nlp_dims = capsule->nlp_dims; - ocp_nlp_out* nlp_out = capsule->nlp_out; - ocp_nlp_in* nlp_in = capsule->nlp_in; - - // initialize primal solution - double* xu0 = calloc(NX+NU, sizeof(double)); - double* x0 = xu0; - - // initialize with x0 - - - double* u0 = xu0 + NX; - - for (int i = 0; i < N; i++) - { - // x0 - ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, i, "x", x0); - // u0 - ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, i, "u", u0); - } - ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, N, "x", x0); - free(xu0); -} - - -/** - * Internal function for auv_model_acados_create: step 9 - */ -int auv_model_acados_create_precompute(auv_model_solver_capsule* capsule) { - int status = ocp_nlp_precompute(capsule->nlp_solver, capsule->nlp_in, capsule->nlp_out); - - if (status != ACADOS_SUCCESS) { - printf("\nocp_nlp_precompute failed!\n\n"); - exit(1); - } - - return status; -} - - -int auv_model_acados_create_with_discretization(auv_model_solver_capsule* capsule, int N, double* new_time_steps) -{ - // If N does not match the number of shooting intervals used for code generation, new_time_steps must be given. - if (N != AUV_MODEL_N && !new_time_steps) { - fprintf(stderr, "auv_model_acados_create_with_discretization: new_time_steps is NULL " \ - "but the number of shooting intervals (= %d) differs from the number of " \ - "shooting intervals (= %d) during code generation! Please provide a new vector of time_stamps!\n", \ - N, AUV_MODEL_N); - return 1; - } - - // number of expected runtime parameters - capsule->nlp_np = NP; - - // 1) create and set nlp_solver_plan; create nlp_config - capsule->nlp_solver_plan = ocp_nlp_plan_create(N); - auv_model_acados_create_set_plan(capsule->nlp_solver_plan, N); - capsule->nlp_config = ocp_nlp_config_create(*capsule->nlp_solver_plan); - - // 2) create and set dimensions - capsule->nlp_dims = auv_model_acados_create_setup_dimensions(capsule); - - // 3) create and set nlp_opts - capsule->nlp_opts = ocp_nlp_solver_opts_create(capsule->nlp_config, capsule->nlp_dims); - auv_model_acados_create_set_opts(capsule); - - // 4) create and set nlp_out - // 4.1) nlp_out - capsule->nlp_out = ocp_nlp_out_create(capsule->nlp_config, capsule->nlp_dims); - // 4.2) sens_out - capsule->sens_out = ocp_nlp_out_create(capsule->nlp_config, capsule->nlp_dims); - auv_model_acados_set_nlp_out(capsule); - - // 5) create nlp_in - capsule->nlp_in = ocp_nlp_in_create(capsule->nlp_config, capsule->nlp_dims); - - // 6) setup functions, nlp_in and default parameters - auv_model_acados_create_setup_functions(capsule); - auv_model_acados_setup_nlp_in(capsule, N, new_time_steps); - auv_model_acados_create_set_default_parameters(capsule); - - // 7) create solver - capsule->nlp_solver = ocp_nlp_solver_create(capsule->nlp_config, capsule->nlp_dims, capsule->nlp_opts, capsule->nlp_in); - - - // 8) do precomputations - int status = auv_model_acados_create_precompute(capsule); - - return status; -} - -/** - * This function is for updating an already initialized solver with a different number of qp_cond_N. It is useful for code reuse after code export. - */ -int auv_model_acados_update_qp_solver_cond_N(auv_model_solver_capsule* capsule, int qp_solver_cond_N) -{ - printf("\nacados_update_qp_solver_cond_N() not implemented, since no partial condensing solver is used!\n\n"); - exit(1); -} - - -int auv_model_acados_reset(auv_model_solver_capsule* capsule, int reset_qp_solver_mem) -{ - - // set initialization to all zeros - - const int N = capsule->nlp_solver_plan->N; - ocp_nlp_config* nlp_config = capsule->nlp_config; - ocp_nlp_dims* nlp_dims = capsule->nlp_dims; - ocp_nlp_out* nlp_out = capsule->nlp_out; - ocp_nlp_in* nlp_in = capsule->nlp_in; - ocp_nlp_solver* nlp_solver = capsule->nlp_solver; - - double* buffer = calloc(NX+NU+NZ+2*NS+2*NSN+2*NS0+NBX+NBU+NG+NH+NPHI+NBX0+NBXN+NHN+NH0+NPHIN+NGN, sizeof(double)); - - for(int i=0; inlp_config, capsule->nlp_dims, capsule->nlp_in, stage, "parameter_values", p); - - return solver_status; -} - - -int auv_model_acados_update_params_sparse(auv_model_solver_capsule * capsule, int stage, int *idx, double *p, int n_update) -{ - ocp_nlp_in_set_params_sparse(capsule->nlp_config, capsule->nlp_dims, capsule->nlp_in, stage, idx, p, n_update); - - return 0; -} - - -int auv_model_acados_set_p_global_and_precompute_dependencies(auv_model_solver_capsule* capsule, double* data, int data_len) -{ - - // printf("No global_data, auv_model_acados_set_p_global_and_precompute_dependencies does nothing.\n"); - return 0; -} - - - - -int auv_model_acados_solve(auv_model_solver_capsule* capsule) -{ - // solve NLP - int solver_status = ocp_nlp_solve(capsule->nlp_solver, capsule->nlp_in, capsule->nlp_out); - - return solver_status; -} - - - -int auv_model_acados_setup_qp_matrices_and_factorize(auv_model_solver_capsule* capsule) -{ - int solver_status = ocp_nlp_setup_qp_matrices_and_factorize(capsule->nlp_solver, capsule->nlp_in, capsule->nlp_out); - - return solver_status; -} - - - - - - -int auv_model_acados_free(auv_model_solver_capsule* capsule) -{ - // before destroying, keep some info - const int N = capsule->nlp_solver_plan->N; - // free memory - ocp_nlp_solver_opts_destroy(capsule->nlp_opts); - ocp_nlp_in_destroy(capsule->nlp_in); - ocp_nlp_out_destroy(capsule->nlp_out); - ocp_nlp_out_destroy(capsule->sens_out); - ocp_nlp_solver_destroy(capsule->nlp_solver); - ocp_nlp_dims_destroy(capsule->nlp_dims); - ocp_nlp_config_destroy(capsule->nlp_config); - ocp_nlp_plan_destroy(capsule->nlp_solver_plan); - - /* free external function */ - // dynamics - for (int i = 0; i < N; i++) - { - external_function_external_param_casadi_free(&capsule->impl_dae_fun[i]); - external_function_external_param_casadi_free(&capsule->impl_dae_fun_jac_x_xdot_z[i]); - external_function_external_param_casadi_free(&capsule->impl_dae_jac_x_xdot_u_z[i]); - - } - free(capsule->impl_dae_fun); - free(capsule->impl_dae_fun_jac_x_xdot_z); - free(capsule->impl_dae_jac_x_xdot_u_z); - - - // cost - - // constraints - - - - return 0; -} - - -void auv_model_acados_print_stats(auv_model_solver_capsule* capsule) -{ - int nlp_iter, stat_m, stat_n, tmp_int; - ocp_nlp_get(capsule->nlp_solver, "nlp_iter", &nlp_iter); - ocp_nlp_get(capsule->nlp_solver, "stat_n", &stat_n); - ocp_nlp_get(capsule->nlp_solver, "stat_m", &stat_m); - - - int stat_n_max = 16; - if (stat_n > stat_n_max) - { - printf("stat_n_max = %d is too small, increase it in the template!\n", stat_n_max); - exit(1); - } - double stat[1616]; - ocp_nlp_get(capsule->nlp_solver, "statistics", stat); - - int nrow = nlp_iter+1 < stat_m ? nlp_iter+1 : stat_m; - - - printf("iter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter\talpha"); - if (stat_n > 8) - printf("\t\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp"); - printf("\n"); - for (int i = 0; i < nrow; i++) - { - for (int j = 0; j < stat_n + 1; j++) - { - if (j == 0 || j == 5 || j == 6) - { - tmp_int = (int) stat[i + j * nrow]; - printf("%d\t", tmp_int); - } - else - { - printf("%e\t", stat[i + j * nrow]); - } - } - printf("\n"); - } -} - -int auv_model_acados_custom_update(auv_model_solver_capsule* capsule, double* data, int data_len) -{ - (void)capsule; - (void)data; - (void)data_len; - printf("\ndummy function that can be called in between solver calls to update parameters or numerical data efficiently in C.\n"); - printf("nothing set yet..\n"); - return 1; - -} - - - -ocp_nlp_in *auv_model_acados_get_nlp_in(auv_model_solver_capsule* capsule) { return capsule->nlp_in; } -ocp_nlp_out *auv_model_acados_get_nlp_out(auv_model_solver_capsule* capsule) { return capsule->nlp_out; } -ocp_nlp_out *auv_model_acados_get_sens_out(auv_model_solver_capsule* capsule) { return capsule->sens_out; } -ocp_nlp_solver *auv_model_acados_get_nlp_solver(auv_model_solver_capsule* capsule) { return capsule->nlp_solver; } -ocp_nlp_config *auv_model_acados_get_nlp_config(auv_model_solver_capsule* capsule) { return capsule->nlp_config; } -void *auv_model_acados_get_nlp_opts(auv_model_solver_capsule* capsule) { return capsule->nlp_opts; } -ocp_nlp_dims *auv_model_acados_get_nlp_dims(auv_model_solver_capsule* capsule) { return capsule->nlp_dims; } -ocp_nlp_plan_t *auv_model_acados_get_nlp_plan(auv_model_solver_capsule* capsule) { return capsule->nlp_solver_plan; } diff --git a/control/velocity_controller/scripts/build_auv_solver/acados_solver_auv_model.h b/control/velocity_controller/scripts/build_auv_solver/acados_solver_auv_model.h deleted file mode 100644 index 8fd75b39a..000000000 --- a/control/velocity_controller/scripts/build_auv_solver/acados_solver_auv_model.h +++ /dev/null @@ -1,174 +0,0 @@ -/* - * Copyright (c) The acados authors. - * - * This file is part of acados. - * - * The 2-Clause BSD License - * - * 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. - * - * 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 HOLDER 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.; - */ - -#ifndef ACADOS_SOLVER_auv_model_H_ -#define ACADOS_SOLVER_auv_model_H_ - -#include "acados/utils/types.h" - -#include "acados_c/ocp_nlp_interface.h" -#include "acados_c/external_function_interface.h" - -#define AUV_MODEL_NX 9 -#define AUV_MODEL_NZ 0 -#define AUV_MODEL_NU 3 -#define AUV_MODEL_NP 0 -#define AUV_MODEL_NP_GLOBAL 0 -#define AUV_MODEL_NBX 1 -#define AUV_MODEL_NBX0 9 -#define AUV_MODEL_NBU 3 -#define AUV_MODEL_NSBX 0 -#define AUV_MODEL_NSBU 0 -#define AUV_MODEL_NSH 0 -#define AUV_MODEL_NSH0 0 -#define AUV_MODEL_NSG 0 -#define AUV_MODEL_NSPHI 0 -#define AUV_MODEL_NSHN 0 -#define AUV_MODEL_NSGN 0 -#define AUV_MODEL_NSPHIN 0 -#define AUV_MODEL_NSPHI0 0 -#define AUV_MODEL_NSBXN 0 -#define AUV_MODEL_NS 0 -#define AUV_MODEL_NS0 0 -#define AUV_MODEL_NSN 0 -#define AUV_MODEL_NG 0 -#define AUV_MODEL_NBXN 0 -#define AUV_MODEL_NGN 0 -#define AUV_MODEL_NY0 8 -#define AUV_MODEL_NY 8 -#define AUV_MODEL_NYN 5 -#define AUV_MODEL_N 20 -#define AUV_MODEL_NH 0 -#define AUV_MODEL_NHN 0 -#define AUV_MODEL_NH0 0 -#define AUV_MODEL_NPHI0 0 -#define AUV_MODEL_NPHI 0 -#define AUV_MODEL_NPHIN 0 -#define AUV_MODEL_NR 0 - -#ifdef __cplusplus -extern "C" { -#endif - - -// ** capsule for solver data ** -typedef struct auv_model_solver_capsule -{ - // acados objects - ocp_nlp_in *nlp_in; - ocp_nlp_out *nlp_out; - ocp_nlp_out *sens_out; - ocp_nlp_solver *nlp_solver; - void *nlp_opts; - ocp_nlp_plan_t *nlp_solver_plan; - ocp_nlp_config *nlp_config; - ocp_nlp_dims *nlp_dims; - - // number of expected runtime parameters - unsigned int nlp_np; - - /* external functions */ - - // dynamics - - external_function_external_param_casadi *impl_dae_fun; - external_function_external_param_casadi *impl_dae_fun_jac_x_xdot_z; - external_function_external_param_casadi *impl_dae_jac_x_xdot_u_z; - external_function_external_param_casadi *impl_dae_jac_p; - - - - - // cost - - - - - - - // constraints - - - - - - - -} auv_model_solver_capsule; - -ACADOS_SYMBOL_EXPORT auv_model_solver_capsule * auv_model_acados_create_capsule(void); -ACADOS_SYMBOL_EXPORT int auv_model_acados_free_capsule(auv_model_solver_capsule *capsule); - -ACADOS_SYMBOL_EXPORT int auv_model_acados_create(auv_model_solver_capsule * capsule); - -ACADOS_SYMBOL_EXPORT int auv_model_acados_reset(auv_model_solver_capsule* capsule, int reset_qp_solver_mem); - -/** - * Generic version of auv_model_acados_create which allows to use a different number of shooting intervals than - * the number used for code generation. If new_time_steps=NULL and n_time_steps matches the number used for code - * generation, the time-steps from code generation is used. - */ -ACADOS_SYMBOL_EXPORT int auv_model_acados_create_with_discretization(auv_model_solver_capsule * capsule, int n_time_steps, double* new_time_steps); -/** - * Update the time step vector. Number N must be identical to the currently set number of shooting nodes in the - * nlp_solver_plan. Returns 0 if no error occurred and a otherwise a value other than 0. - */ -ACADOS_SYMBOL_EXPORT int auv_model_acados_update_time_steps(auv_model_solver_capsule * capsule, int N, double* new_time_steps); -/** - * This function is used for updating an already initialized solver with a different number of qp_cond_N. - */ -ACADOS_SYMBOL_EXPORT int auv_model_acados_update_qp_solver_cond_N(auv_model_solver_capsule * capsule, int qp_solver_cond_N); -ACADOS_SYMBOL_EXPORT int auv_model_acados_update_params(auv_model_solver_capsule * capsule, int stage, double *value, int np); -ACADOS_SYMBOL_EXPORT int auv_model_acados_update_params_sparse(auv_model_solver_capsule * capsule, int stage, int *idx, double *p, int n_update); -ACADOS_SYMBOL_EXPORT int auv_model_acados_set_p_global_and_precompute_dependencies(auv_model_solver_capsule* capsule, double* data, int data_len); - -ACADOS_SYMBOL_EXPORT int auv_model_acados_solve(auv_model_solver_capsule * capsule); -ACADOS_SYMBOL_EXPORT int auv_model_acados_setup_qp_matrices_and_factorize(auv_model_solver_capsule* capsule); - - - -ACADOS_SYMBOL_EXPORT int auv_model_acados_free(auv_model_solver_capsule * capsule); -ACADOS_SYMBOL_EXPORT void auv_model_acados_print_stats(auv_model_solver_capsule * capsule); -ACADOS_SYMBOL_EXPORT int auv_model_acados_custom_update(auv_model_solver_capsule* capsule, double* data, int data_len); - -ACADOS_SYMBOL_EXPORT ocp_nlp_in *auv_model_acados_get_nlp_in(auv_model_solver_capsule * capsule); -ACADOS_SYMBOL_EXPORT ocp_nlp_out *auv_model_acados_get_nlp_out(auv_model_solver_capsule * capsule); -ACADOS_SYMBOL_EXPORT ocp_nlp_out *auv_model_acados_get_sens_out(auv_model_solver_capsule * capsule); -ACADOS_SYMBOL_EXPORT ocp_nlp_solver *auv_model_acados_get_nlp_solver(auv_model_solver_capsule * capsule); -ACADOS_SYMBOL_EXPORT ocp_nlp_config *auv_model_acados_get_nlp_config(auv_model_solver_capsule * capsule); -ACADOS_SYMBOL_EXPORT void *auv_model_acados_get_nlp_opts(auv_model_solver_capsule * capsule); -ACADOS_SYMBOL_EXPORT ocp_nlp_dims *auv_model_acados_get_nlp_dims(auv_model_solver_capsule * capsule); -ACADOS_SYMBOL_EXPORT ocp_nlp_plan_t *auv_model_acados_get_nlp_plan(auv_model_solver_capsule * capsule); - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif // ACADOS_SOLVER_auv_model_H_ diff --git a/control/velocity_controller/scripts/build_auv_solver/acados_solver_auv_model.o b/control/velocity_controller/scripts/build_auv_solver/acados_solver_auv_model.o deleted file mode 100644 index 17dcd34d54974794d4f68bd95dce9a5b47a9c28e..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 35504 zcmcJ23w&Hvwf9Mzv}u7%3bo{7l>-i#LW!9^6TTvuHfc{_(ne{aKy^AzCJ#&|Au~f; zC{RpO8Ha)TKnp73^8*#d2iGDXDQyX;2%;j0Lcs@A9xZU?ExrG>_TDpVcIH&>cYojc zF*)b__kXRu_S$Pd&OZB;%UdE#CzO>r<|uPM>=f<{>NxdV3)fA2+2kDQOmo8Lc)9n` zefaF~s@BZE!Yfyu*XTB!^fI~hm)+A zdC|)}*_>V5@GRdVZMDL@Y=uHN4=>XkT+Ni$Y}zFi)sWK$Kv^|$i_)hTrBBbNYrRIz zTCIcF$YX7hj+!DJHF+J#a3^mx12O6K{qeh^l#;Y6AE&{|AAcX#DZMyyOq=S^>{G22l7r{ z@X&VN+Z>#13O6|~vzas;(l7a1j|2yG9H=0p${A$;QRUArN`q~)Npn8AvM3q0R2I_e zi_#!kUnDxuPM%kkyg?^JZUZIg=+QYJ!4QXZaY7{6VC!w5YE*JM(zV=G!Ocj?hvgQ& zEls?#m`Jy!iQ9^abX%IZy_iV1iU_AKvWu{&QzlO-N`}*?_~db8aUOJA+L)V)iF8|< zcxy3{Zc7t)6cg!I5mCIUw*6Cml#CWDq1)2A-B(Pc+k9?KMY$oInvhyZZnl%rp@?M9 zPWB4Pb~HVNBoR$?tE{kzj}{Z@wlr~XF_CUd6A2Yr(`{+ufnp-vmL|SlOr%>yLt@Q}z5mCl7nshR`SrlFSZ*ip0=7sJw-OG_G$5jO=Gg zWgw;_UgoWE=D)_4RRhM%V{&+`g|3|8cgg}+UyF;}*#}#5mzIZK^>#izL6xLqcX+IM zglUoNC+fZ2Y`*cbXV!YzvtWJRA7kZYl`~d8fejuWE%zGtr1zOj!ozQurKX2-D}#0( z$lw&tz#Ch_^AswgDjfpC@jC9!n_#k!pk`82jq2y_x~m)t_H`dTc+ku7AoR1(1Mt*r z#LJyV>bX230EE9XZy``TF1TJHfaOSBT&hI08yj<`R} z1l_=_mBH%3teHVo`I#l?`o^A{On%8;;&lZE&|2^p^T9~jeuN;drw3cF*hAf1VA`_! zz_b-jfobQovv{QOh4g*M{{+rIvtL=q&IhWdZZkMBFoV&+3C9aDN87S1yynbr zBAKV#aVk5Xq|#ksQfFRqOCQWsyc0 z6BxM!&GF}zFz#;p!x@C`hi)is&Fs`Uv}X3;A%hbmy7yEvO-2T#?+ezlB$4H)jU=>= zw#>el!+~kH^q>vqwYE5n-3p8^Ue_1by0|EjohPz`PMuwml`q2c)i4&PZS*ilXBz(K z{HQ*=81G7GuQ{Z4cE}#6X|Gl4+XqSK4kJ|Tm`VdTXBP!qGxvsv52E`%5fK!=3s&Ia zOO1!knSE`jOEW%^+l{B2Gtai<`pa4x_op9=WL^tr#=<E~emr@@bA- z*_u(FzKy(3=xxU3w~@=yZV)l3As;&qgop1f3pb9D9s9=_Au9zv3bKb^T>Ng-{=ZTC z|3>Zq8_EBHV>iPM|C@Yeb)l`Pd^b?b+dwt5gKB2myDyh;YXv0vmMX7wTfn4(8O62! zA9+E{IdmH`jRDPI4X+0j{A{Y_E2BoPSRM85$_}GWDM7=eO3pl~>#I4lD2Ni*;J5uI z$k6#weXge($NgHIi!8o7Xtb+XIuf_&iFuU6nS9`kHMx~IA$|kXDg)D+bT5u6SW9RW z1q`1Z$!-dwv!tMns6ic;Me@#hL-BNy=4EE9(Frs=xi!~JuD`|$@nwi8LR04aaH8bYr zyqd1S^wGdH_^y^w%EK^JQj;4#FMNJ;c6P93GnW# z;g-y^Sdf8x52i36DzA!uA)j276hREIbx}yw5Jw3@O@4KU)3LGdv)@hgHAa+I$OmH%x-fWRjB!)u;YGnR@>i}2zHJUxqKBCj*g`#A z1j8+tPj!3Sipm#0r>F?5ay86n4;b^w5u)_xkyDflfEEggnrC_Rn&;9g*2f#M-j?1L z!GLYbV^0U)LX)8BAj(~srmN?Y`~wJTl3PT!oCfkpE=94#vo18M2#@oTVExGGWmxRN zv;h6h8|cs5s0Ovw^jx7VYg`meKgn|iXrl-~D>|6lQ08?U^maZokx3?U%#?yNhilQQ zbeyReZgO$Pyr>EDqNaBI*5Wr9n6`N{oQ~5roM7RVH{y)k*x2fk(ZH2|1VDM$aOkyU z-f7G1r_j{#*OqyMCJ)wlME!s3@R*wQv^4(n(nmRH$VljUjmyI3v)TJ9UORYD>w7mb zWughi+}Dn29X(%SmdFZK&|B; zc{`sf_ox@orEYQB${uXXl~3_HeqHuzYwqw2ck{W!ot=9pcpdi}q$cymtbO9X*6Vn% zY(IJdBDCd#w|gD$l)dP6K*gflk<|~WYjG))2Cw5l*|Xku0^4%aulU5CD!N&81u>d3 z`*%K5o_T%Neo)Y*Py)nfBe5^op6v^I7=5mM6Q%MxnuCbimBAh_vnIIKgVP)6p`H4B z>J|nuxMNUgCqcX-=?gaH>&K(VhcH$V@T}zEiNC_31$zKAIMA7NV0Ea%sZ{q9siu)k zG!m``2bYY|iLe?iPS>^K+AXYBC<#&mWi&kFL?aPpY0eCzL1IY*3+925c3Npb*-$_f zl}a>7g~6f$7ONuY1F>!t2{whtCaS`aI?@+Z=gA5!qk`x)m@!I0WG{YujEV(keV!v} zjI)M2-t#uWxHyt8P)B9Bna(#6IDBn*Yy!;^_$dr2N9mL=2RAc{=kmO8Avw3E0yO<61xB0VW>BcstompB1W>x zYc#5@f^(=w+cBqV3H>#Ku&$>_-q4zP!pl4t$?P%xG9>GpX>5K1y`>%@mgmZAb#c&> zt5lr?Qa*J#bpv*4Uas0s$;(l^xra=x@l9QkEB}z47#`k@(RhyLIBlGOuBPbD#alQwI;WW-sD2MbtM))y`q2dI&3GB1+A}3H5cW zQG^IEqh4#ftiCl{)tWuqC0UaO)DDSYIsd1iV6Y5^<0A5U@BQw zFgjnWg2CcSRW?y2?7KwI18II1diduwpAWm)^zlVMvmiT zc(knXp}_KoLZji#BY91gfoooYbMkRj2&tBlR|8l72x+Yy_hNeCsTBb3)V$oxJxIWs zHm?eJSzahuo}0H98P6OU-FVV-=fH1xJWSO^GB2`De)?%GdxCSOr#*Vio4M&j;oP#f zusT~kY54WB^$)xpnEepO2$q1MpcxUSU-}zeQcb{!sMZ;1x>4EMxH~X1z!PBQ=lpB1 zbOhb~4tT!M-R~;a#AfaLCt5M9{uyf6RNuI+mbs!qnD$&kV+n{IK`#>V3ULd0QS}Ge z$=3ZcB5W?CFRQlhYpnZv>)y5Q8?5`A=wNIp)~{19iuI-AFyg?7whZ2IL11=(tUl=WGX%w z?M%n|oX(AjSbuLvGTJfF*Ei6iIH`?;@j^->-OmC^8;W(uB^FdqfFf|}@ zx**mO9g3%uai=eSQ9Q9OKGYrUj}2XzjtzCDocM-R6y^^>buu+D6laC86mI(lx}$?b zz5Ow5e_~*$pFt{!DiTA;jLI#L#oZ84h-jqWk$y7{T z#4qIg-h{sB80a5#`ePeZ(cPq}Ff|#A4}-xZ3*aI-KS1 zX{WhloK73;izQC^@N%cOf3Ppw8H-1|(uty*D8<}{=!VXLRP^FPia^uO#-VtZbB=TN z2IuUw)7!aW-3F%*zchY)7w1zNU>acBL6S3dpkpvfh71nH$vf#(-0h0>_QgAon>y7= zY;Y23CvmZpKy41O<%Acq7LCX#fKq^oY6@msec?j%s_J-F?0l6_7R z*~j{wi=DwNHoU2by4a`&^L5NyN6<()MAmJi9kd>M`n5u z^%sw(di&8Iqeo4i>UP5g2^T#kdNp^T%cV4z)7;wA+!H!yxt+aTU1&RQ*U&(}o9cPXL`NL3ix!#cjrG9}^q2|2XkXl) zktZ+&azA1GS`<}k$UoIh4Q(W;)Brck6M9Kyte+Y=VhELq@iIP?Ko;opcsxkfxh*#4 z_QaBIkt~sV$=~^At~!SV_0KMaDp{BsK(-ig`(~qBsG1RXy(nvfD=!sCnGu0Hf~cdJ zg5I7-Qoe{^IEBwNnD7PlkyFTl=!fXcBC)=~o)~;Eg^;|wzJ{Q_VkP>TN1~~n>HhwW z`EtXgSjz2)CERsrRInNC2!^hUr`E^OCbiosfy(8o#w)!EXCkuVL%ra*)HA#BuCCsW zUe!3EYDvLYRAuAZ0ea4~8V)e)Dv_?+320Fou}DYZIB@#4kDU1IVRh3!e983hFMI0Z zEvM&Rxb4Q*?!EQHiZi|G&dj4<{L#Z-Ui9|OFFoA0uKjc^+r;M!UT->O$;{`TUU1Cm z*I!4+?Wf;y*B>5xa?|6dUE8_wsmuTMtp6RETtnp+pDt3RayLnUrQm(7_| zS3hr-hFJyar};hQv26J&%6F77UTnshxBB5+cJW7@vJKT`?>&5SWM9s_u}Hbo2!~8 z3?b(wC35blYFIiBZh*?yNiU8Qs4}2GNBXat7_M@+mc#zxisq{N38Pc1YJr4JNtzFH zNOm5{;5eQtv6Sqxs=*1(mMpwLJ{Z)pyz+s2+9A0`V=fI6g|f1r$CY(+*~Fz4RacgI zRoluY)|TOFanTp&2nT)J1l|v0wZqoXjNI&ZS zys1@{VFU}s$#$K#sS-B@pOmV9#9rCuD8~4TKI5?7s%;Y{5MkosrGxVfVsT%}c z(?RE%8asi2wyNs2IMr+HGdgarfp*g-e0?(E`|OT0*d3Si^|__u75d39pVxVug=_Ll zul5V2-Kx_*q|$2ItK75EWZ2$c=`_=iP#N}W%(QdD{>#E@fmKqcfa4ze1AkvN0a3qo zVsn){Tt4B4RW(bvDLc+;B+t{__fQ=b>ZVk@F00z%Myhr+;kPzYbs#?Rvt=dw2O z54LID4Y;SW51?>l^GzD3aG-;*J2gh{@#rAzp%U0%OJJ{;zz)}qQtERpjQlvy!YFUQ zgTkcTdFf!O^og|R-#Xrl`w0T)z8Bac{K_ywxH!{soH{ul#hGw(yx&~lJW=2V?%@kl zLW4Sv8Ysu(Db05aRKB{{2j8r5iYa+)%r9ZG2f6K~2iFd{N<0yvXBv zzNv6#Nf6gx<@-v0TP>5m$u|{FvjlN{v&N$q{(X&avhW{k{0kO-yT+BiLgL*TzpDW8 z)gFyMY~jDu_}?x3H+-+!hD`c{=D#69T>n|)M_3{E7me3g_&$w)(8B3s2AWMQvhaUu ze1(O-rSX`Bt4}^FoL&o`%=Z<}dJ9)}S91TzD)pbEH2;$p|35UIweVv#{#gs>c>v1w zB?~`U^S^1~A-Ow(RZ{cEEdEB#x66Au)C7R zQT12hylLS){-VBS=y`%s^+o1YI_(C~=dWn}Ct3V&Xnd)K->mVdh5tb7r*Tjow`qRT z;@_e15ewg`^{kajaIO4U;aq3&AJBSkvGCt&Jw2A5$29*wi~pp?|8C)b)A-94{+!11 z4-7h=mo$F3o=ceQU(tq1D3Q ztNCYJ_;DH^wD1pVe5-|@!gwVXB#V!^nxD1!jT+Bc_~{zoX5lT2PvJR?%;8MU|D47D zh{mt8@N+c&6$}5E#=mCa9U8yM!h1FTeG4DZ_>U|+&G?jY{n90jqrF@8eudWapO&6a zX?%x;=QRE^3;&$fU(M%&=T!Tta2~bzU)6dZxA1Rk{0R$J?XALj%EEu5`G2+WJ2d`` zh2Nv`mn{5#jUTk|hcrG>&r?mw{-E*67XF0B54Z5YX?&W6KdJ;$F*?vJXPU*z`|9(fO(x&-WgiY2QB{lG=7qW2Q_|*g`cePkcH3D_*@Hb)c67m zZ_;?!!k1~h)xwuEKFR6fv#Bk|p^a-aUpmj?uhDpig?DKD0t>%D;}=@^g^W*fYAt+& z=5Mm_%Qb$Pg{%Iq!uh0ytNsh+vT*f`igwNV3g!Khwr9J=|GLh1tA*bTyt*KszuM`8 zKkb9R0{lok32oDlLS~KWDCAd4|1rRcf1~D`=i}?}rAR4$gXWKFzM7ZgCA*J)s=rct zuJFOX=!4(jgMSY=m$y>6OwC^%=K&x7?|ks5wVu%_N)NAT;c_Y-)k@i4?}IP%!7uQ^ zxBB2;_rdS=!T;ieABirel>G~Q@Gc+x8XtUz55CU_KMHYE%AWZ?_!=L4*a!cv5B``B zUWvG&_&J~-q~S(B7HZr*mulw*1LmZc~v&IL1A8_(Rlh)7cjmV?bhyO7jJn4gf$_M|N4}QB3{)i9$ybnGZ zYssbjaEuQ=+Xr6^ocuF-ta7~3d7j1}()bCwUWPU9xC&6uX^wNfkN$6K{*A{eKCgM= za*xq-yux|C6PM5V=&3{;lb!XNkFe)svBsM;Zul0{x4~~+QM(wxNG5u z*dR0R)ued!I53FKMbT*cM_X2|JU6;@)$%1PTURWPM(aW)(R!yYvZP@?{ax6|*9-Y? z1O08}RQ?m9`#JPyF8!HDe^_b(T`3v53Mn?zL(C7cK!_zmED~aw5DSGkqY!63hh^ue ztXOsq%g$lhIV?MeW#_Q$9G0EKvU6E>F3ZkUcCzeTmYvJ8b6IvS%g$xlxhy-6W#_T% zJeHlO3dyqbSau%E&STkmEIW^7=d+{zB$2WX?jCTF6ohnYmCgS+;?5X<(@amTF+B29|1M zypi!n#v7FlY+)noY-F8{th14IHgamHo>hnHSx2azm4xb9OQ@dJgz8yCsGe1X>RCsq zo|S~^l@=2+N{bE}9W^>^blm8`(UGG=N5xJ^1y4vtPiT&^Ma54@g-%GtQAmYQNX1Y{ z1yM-FPe=t&Xs#-Q3Z9UPo{$QwkczjE3Z@XE$!rqS+eWAXV*h5!507Gx7B)z9$I%{f z?)XHsqvsRY%-7pDe#?Pw*Lv3ub}7;pqVcq@p?ExMNaH$(ar>C^meDRsVQ9xd65GeH zr3Skt0Y{fbR$yyobj8x8XSJ-1t_&}Zv_!Fu1bYLAV0#zu>+2%x$6F*iB;E_rAMa_%3mcaO0d${`}uDfzJ)1P6!j~LMSGWa{X3UiT7BH3 zH>!!YrDI=hAm>eE!ULMFS`+Q}LoJ%VD!(s0-`t~C=+Pcq+D(Q2#t4b+%c7eoxV(@d zZ{Qde)c zjdu3-W5+yizsyTB247O&gZ;Nd$?++A`>)ZRmk@p%FGQVg9+g9gl|OmVg3x_ih?du= zkF&=+Pmd7TePwKZh`h<8q)%Fng_| z*aa(gXJarJjMG+3Z0k#o&zwt_&rRKxQjgIGzUYsouvrir9w`sC3%~c`g0V2;ki2n< zX_PL`d`JysopF2>Ft9P71~=Fc)xM4Cy0T#oI~&w-Zo@P-=k?}005kI1U4kBsxE$k3 zr*`?%&d);2=q?o?9cg^{(H~WDGQN%=!@Wn*kfVDRlSVzuI0Gn=zo_v|E$n$NElugl zQU2-tcvUiQk|y8#%c@7CORCKg!9Lu6kPT=b_PJBEn=J~w>r!{Az zC#i8;&sM$1M_}>w?4X0IgiuNFA{lVNlapZ+ld!|&6$&A(FcY3b1L&AtqqKPvdNv}gE# z^3nf>;MWNLbTSZ!tzW$lR`$~u#YTU`hre3mbUH@xFAzBG{W5w|LXWK9t%5&O@YQ?J z@%8vE!GE9N|G-E8PQj3j}!Ru0zc75PlMopK=509_z}Sm3VuxBwB%&$N%-hV3%=CzX@S#{lhN}3tg=RF5a&XUZGY zxXnLV@J|-}CV|fuc$<%&m4bhY;P(o=PT&{$=($wzX)W5=|5<_83;e4-dcH0AA;G^x z;P`eoum3(DJ%1GZxq@%@x>Bj;h8dZr8h0>Pgp@Pz_j z;G@SAd|E3u`L6QecME=_;HP}}^nQ^JD%Yv_8U3I0;a@NK|3~nDAaL4)YxLadqi2`k zOFfSYoIVmUdj964=Xt@G<9OL52;-n~(MKyrPnE{)`g)JxpDy?(3A{<*3w`u_Sn#Eu zvjiR%de-{r=@$IOg1<@NO9Y!T`Zwvev0{_1Pe^}r*3H&jE-z@OG z0{@P{pAqNeKK1!M{h~^vrAYJSg=4 zMBsY`|04o_(T9If@aY>BqyGpp0teYI_16phHl!K;VvSR|&JuW=;LHAJmB3}c*C}wR zf1}X9Qt00<@Kpl8Q|OWT?iRSr_qPI<`93T3pDpwsQ-vEGCee;8MRU@XtWb_-BsBsa|M`VenH0zFOezLXWgFA^7JC{zk$7Pl0Cy|2%8(TD#p!H)`l)ls;?Ve79Kxb$0t558F7(x2xET>5RD#;G3H;%EHub%Dz`zeVVg z^?Rq_%Q%0)hyS$T%Q%0*hhI4z#^E6Ud=5WjXS2r1emO5XTi|C1{-=bVYX$y2flI&r zMCg(FcM6<_dSg%HdvJq;>^T!ZgRjsy+5ZKBe?s80Tw8qbt9d&O-y!hZefWC?U+Vvx4}UtnoWMcl`Wb%44<~D!{L?A$If8$Oz!wSp zPJt&heiAVGJbtOr9~bhku^n4+#D`AAU;k z2L*q#5C5}*f1%*t;KRRF@P`EdP9OdQf}a%pKl|_x3VurPkE54uILM#7@iTrN^ueo@3wukykBeee;1%ko|$a5*k~Rp5JIm+{XI zf!{0e`+e}=3;d^o|Fpoz1pb1+zasE=1pZ@zPdXMiILL25!q3=wjKJ>`_!5m%J<51k zC-~Ao3Bi~3o)dgo?>7j(toQHx@b?P-dST}?f`5m=UlRD80&jA0gM<7a$I}a3LREjV z4rk+^v`eVs%kgx?WtjUPIi7xABVpgTQYV_@e^@{ zz#kL*-2(rkz#kC!;{ty~;C~YM>jM9?z{@{?8yr+#i68BQpCE8qUv)nCLLdApjZZ+n zSKw#-e5>F;DezIj|D?e8`0#%(_``z#M<4z_1%Hd+zvII{A_x)=vVR0WV}DTNj;navE!=zu`V$Lxj#vDLEZltWY5pA!V^5Rj zzi#o@YTW!g8HT@ExKsChUg)D%~*7c56V=kIoEY0Q+beX=-V;;yR!68_&Rm?7k%S(_(w@k z;SaF%eRm>VhkrPD5Z@p--V^Pje~wp`?0@&yg*Y{tNtT)>eLw5O;spKLArHP6 zS5zfUc|=`oI8U7$YSlS_6Xm0uuUQV^Y9d^Rr6S{Rua&P!Ag(-S>zgf$heR+m&%in;6Qt^N5CzXWycZ=}9D8((U zdmH&0m0U7!O1f`>B0TyYbtZr3@U!LV-HjdlBynSe{yNSdC{uCR{+ouhQu3Qel+Z0g JmM(4i{{~5-Bo6=p diff --git a/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun.c b/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun.c deleted file mode 100644 index acd0b61e9..000000000 --- a/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun.c +++ /dev/null @@ -1,394 +0,0 @@ -/* This file was automatically generated by CasADi 3.7.2. - * It consists of: - * 1) content generated by CasADi runtime: not copyrighted - * 2) template code copied from CasADi source: permissively licensed (MIT-0) - * 3) user code: owned by the user - * - */ -#ifdef __cplusplus -extern "C" { -#endif - -/* How to prefix internal symbols */ -#ifdef CASADI_CODEGEN_PREFIX - #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID) - #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID - #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID) -#else - #define CASADI_PREFIX(ID) auv_model_impl_dae_fun_ ## ID -#endif - -#include - -#ifndef casadi_real -#define casadi_real double -#endif - -#ifndef casadi_int -#define casadi_int int -#endif - -/* Add prefix to internal symbols */ -#define casadi_f0 CASADI_PREFIX(f0) -#define casadi_fabs CASADI_PREFIX(fabs) -#define casadi_s0 CASADI_PREFIX(s0) -#define casadi_s1 CASADI_PREFIX(s1) -#define casadi_s2 CASADI_PREFIX(s2) -#define casadi_s3 CASADI_PREFIX(s3) - -/* Symbol visibility in DLLs */ -#ifndef CASADI_SYMBOL_EXPORT - #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) - #if defined(STATIC_LINKED) - #define CASADI_SYMBOL_EXPORT - #else - #define CASADI_SYMBOL_EXPORT __declspec(dllexport) - #endif - #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) - #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default"))) - #else - #define CASADI_SYMBOL_EXPORT - #endif -#endif - -casadi_real casadi_fabs(casadi_real x) { -/* Pre-c99 compatibility */ -#if __STDC_VERSION__ < 199901L - return x>0 ? x : -x; -#else - return fabs(x); -#endif -} - -static const casadi_int casadi_s0[3] = {9, 1, 1}; -static const casadi_int casadi_s1[3] = {3, 1, 1}; -static const casadi_int casadi_s2[3] = {0, 1, 1}; -static const casadi_int casadi_s3[3] = {0, 0, 1}; - -/* auv_model_impl_dae_fun:(i0[9],i1[9],i2[3],i3[0],i4[],i5[0])->(o0[9]) */ -static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { - casadi_real a00, a01, a02, a03, a04, a05, a06, a07, a08, a09, a10, a11; - casadi_real a12, a13, a14; - a00=arg[1]? arg[1][0] : 0; - a01=3.3453634479321918e-02; - a02=arg[2]? arg[2][0] : 0; - a03=-30.; - a04=arg[0]? arg[0][5] : 0; - a05=(a03*a04); - a06=arg[0]? arg[0][1] : 0; - a05=(a05*a06); - a07=30.; - a08=arg[0]? arg[0][4] : 0; - a09=(a07*a08); - a10=arg[0]? arg[0][2] : 0; - a09=(a09*a10); - a05=(a05+a09); - a02=(a02-a05); - a05=23.; - a09=arg[0]? arg[0][0] : 0; - a11=casadi_fabs(a09); - a05=(a05+a11); - a05=(a05*a09); - a02=(a02-a05); - a01=(a01*a02); - a05=6.0368975005218254e-05; - a11=(a07*a04); - a11=(a11*a09); - a12=arg[0]? arg[0][3] : 0; - a13=(a03*a12); - a13=(a13*a10); - a11=(a11+a13); - a13=46.; - a14=casadi_fabs(a06); - a14=(a13+a14); - a14=(a14*a06); - a11=(a11+a14); - a05=(a05*a11); - a01=(a01-a05); - a05=-1.1116270017027866e-07; - a03=(a03*a08); - a03=(a03*a09); - a07=(a07*a12); - a07=(a07*a06); - a03=(a03+a07); - a07=casadi_fabs(a10); - a07=(a13+a07); - a07=(a07*a10); - a03=(a03+a07); - a05=(a05*a03); - a01=(a01-a05); - a05=9.8084735444363873e-08; - a07=3.3399999999999999e+00; - a07=(a07*a04); - a07=(a07*a08); - a10=-3.3199999999999998e+00; - a10=(a10*a08); - a10=(a10*a04); - a07=(a07+a10); - a10=casadi_fabs(a12); - a10=(a13+a10); - a10=(a10*a12); - a07=(a07+a10); - a05=(a05*a07); - a01=(a01-a05); - a05=1.0920100546139184e-05; - a10=arg[2]? arg[2][1] : 0; - a06=-3.3399999999999999e+00; - a06=(a06*a04); - a06=(a06*a12); - a09=6.8000000000000005e-01; - a09=(a09*a12); - a09=(a09*a04); - a06=(a06+a09); - a10=(a10-a06); - a06=casadi_fabs(a08); - a06=(a13+a06); - a06=(a06*a08); - a10=(a10-a06); - a05=(a05*a10); - a01=(a01+a05); - a05=-6.0150572994295574e-03; - a06=arg[2]? arg[2][2] : 0; - a09=3.3199999999999998e+00; - a09=(a09*a08); - a09=(a09*a12); - a14=-6.8000000000000005e-01; - a14=(a14*a12); - a14=(a14*a08); - a09=(a09+a14); - a06=(a06-a09); - a09=casadi_fabs(a04); - a13=(a13+a09); - a13=(a13*a04); - a06=(a06-a13); - a05=(a05*a06); - a01=(a01+a05); - a00=(a00-a01); - if (res[0]!=0) res[0][0]=a00; - a00=arg[1]? arg[1][1] : 0; - a01=6.0368975005218423e-05; - a01=(a01*a02); - a05=3.3484658136227786e-02; - a05=(a05*a11); - a01=(a01-a05); - a05=-6.1658244361114702e-05; - a05=(a05*a03); - a01=(a01-a05); - a05=5.4404333259807184e-05; - a05=(a05*a07); - a01=(a01-a05); - a05=6.0570157695918683e-03; - a05=(a05*a10); - a01=(a01+a05); - a05=-3.0184487502609172e-03; - a05=(a05*a06); - a01=(a01+a05); - a00=(a00-a01); - if (res[0]!=0) res[0][1]=a00; - a00=arg[1]? arg[1][2] : 0; - a01=-1.1116270017027936e-07; - a01=(a01*a02); - a05=-6.1658244361114783e-05; - a05=(a05*a11); - a01=(a01-a05); - a05=3.3963490377380855e-02; - a05=(a05*a03); - a01=(a01-a05); - a05=-2.9967785627100754e-02; - a05=(a05*a07); - a01=(a01-a05); - a05=-3.0801331505514837e-03; - a05=(a05*a10); - a01=(a01+a05); - a05=5.5581350085139543e-06; - a05=(a05*a06); - a01=(a01+a05); - a00=(a00-a01); - if (res[0]!=0) res[0][2]=a00; - a00=arg[1]? arg[1][3] : 0; - a01=9.8084735444364535e-08; - a01=(a01*a02); - a05=5.4404333259807062e-05; - a05=(a05*a11); - a01=(a01-a05); - a05=-2.9967785627100469e-02; - a05=(a05*a03); - a01=(a01-a05); - a05=1.4970303990827358e+00; - a05=(a05*a07); - a01=(a01-a05); - a05=2.7177645446042520e-03; - a05=(a05*a10); - a01=(a01+a05); - a05=-4.9042367722181902e-06; - a05=(a05*a06); - a01=(a01+a05); - a00=(a00-a01); - if (res[0]!=0) res[0][3]=a00; - a00=arg[1]? arg[1][4] : 0; - a01=1.0920100546139210e-05; - a01=(a01*a02); - a05=6.0570157695918657e-03; - a05=(a05*a11); - a01=(a01-a05); - a05=-3.0801331505514781e-03; - a05=(a05*a03); - a01=(a01-a05); - a05=2.7177645446042498e-03; - a05=(a05*a07); - a01=(a01-a05); - a05=3.0257778596593993e-01; - a05=(a05*a10); - a01=(a01+a05); - a05=-5.4600502730695923e-04; - a13=(a05*a06); - a01=(a01+a13); - a00=(a00-a01); - if (res[0]!=0) res[0][4]=a00; - a00=arg[1]? arg[1][5] : 0; - a01=-6.0150572994295635e-03; - a01=(a01*a02); - a02=-3.0184487502609133e-03; - a02=(a02*a11); - a01=(a01-a02); - a02=5.5581350085139340e-06; - a02=(a02*a03); - a01=(a01-a02); - a02=-4.9042367722181935e-06; - a02=(a02*a07); - a01=(a01-a02); - a05=(a05*a10); - a01=(a01+a05); - a05=3.0075286497147785e-01; - a05=(a05*a06); - a01=(a01+a05); - a00=(a00-a01); - if (res[0]!=0) res[0][5]=a00; - a00=arg[1]? arg[1][6] : 0; - a01=arg[0]? arg[0][6] : 0; - a05=sin(a01); - a06=arg[0]? arg[0][7] : 0; - a10=tan(a06); - a02=(a05*a10); - a02=(a02*a08); - a12=(a12+a02); - a01=cos(a01); - a10=(a01*a10); - a10=(a10*a04); - a12=(a12+a10); - a00=(a00-a12); - if (res[0]!=0) res[0][6]=a00; - a00=arg[1]? arg[1][7] : 0; - a12=(a01*a08); - a10=(a05*a04); - a12=(a12-a10); - a00=(a00-a12); - if (res[0]!=0) res[0][7]=a00; - a00=arg[1]? arg[1][8] : 0; - a06=cos(a06); - a05=(a05/a06); - a05=(a05*a08); - a01=(a01/a06); - a01=(a01*a04); - a05=(a05+a01); - a00=(a00-a05); - if (res[0]!=0) res[0][8]=a00; - return 0; -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ - return casadi_f0(arg, res, iw, w, mem); -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_alloc_mem(void) { - return 0; -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_init_mem(int mem) { - return 0; -} - -CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_free_mem(int mem) { -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_checkout(void) { - return 0; -} - -CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_release(int mem) { -} - -CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_incref(void) { -} - -CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_decref(void) { -} - -CASADI_SYMBOL_EXPORT casadi_int auv_model_impl_dae_fun_n_in(void) { return 6;} - -CASADI_SYMBOL_EXPORT casadi_int auv_model_impl_dae_fun_n_out(void) { return 1;} - -CASADI_SYMBOL_EXPORT casadi_real auv_model_impl_dae_fun_default_in(casadi_int i) { - switch (i) { - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const char* auv_model_impl_dae_fun_name_in(casadi_int i) { - switch (i) { - case 0: return "i0"; - case 1: return "i1"; - case 2: return "i2"; - case 3: return "i3"; - case 4: return "i4"; - case 5: return "i5"; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const char* auv_model_impl_dae_fun_name_out(casadi_int i) { - switch (i) { - case 0: return "o0"; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const casadi_int* auv_model_impl_dae_fun_sparsity_in(casadi_int i) { - switch (i) { - case 0: return casadi_s0; - case 1: return casadi_s0; - case 2: return casadi_s1; - case 3: return casadi_s2; - case 4: return casadi_s3; - case 5: return casadi_s2; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const casadi_int* auv_model_impl_dae_fun_sparsity_out(casadi_int i) { - switch (i) { - case 0: return casadi_s0; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { - if (sz_arg) *sz_arg = 6; - if (sz_res) *sz_res = 1; - if (sz_iw) *sz_iw = 0; - if (sz_w) *sz_w = 0; - return 0; -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { - if (sz_arg) *sz_arg = 6*sizeof(const casadi_real*); - if (sz_res) *sz_res = 1*sizeof(casadi_real*); - if (sz_iw) *sz_iw = 0*sizeof(casadi_int); - if (sz_w) *sz_w = 0*sizeof(casadi_real); - return 0; -} - - -#ifdef __cplusplus -} /* extern "C" */ -#endif diff --git a/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun.o b/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun.o deleted file mode 100644 index 0dc1612cdaca9a4f10a4ca3b8fc76bd5eb4d0921..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 11584 zcmb`N4Rq7h6~O<60&QhU>Nf3wL8d_?LQK*YDk4dsg-olb)1t|Yv>{DwN55=RDS}v} zbvQ)pWFB-*@p$yG=@B+Pj>A+kJ_J920v({DFs;~57*M7%26p$p|9#2rqv@RT&dLA1 z_j~u=_jTXL|KHwQR6Ju)TAC(9nl@ES<|Ng$xHXw}i=<&hkER_Ga$`C%uc0tON<~vY8p))_5$l$7nJ|erjB_*SoC9ROIOF z13Lg}K#VPmGBgcD>#gwkAyxtL_b4tm^%(todyLsJO9skhM2yzWkgc#-!E$XUWIqz6 zW-Nt=e}~AT+WYYPYmd>lsmB;Nbkev;G(TdzwH1a1LxWJSkI@t>?O(ZxH#-+>K`hl_K&S4t% zpmsY*iN5*7&?4&SDNGLZLJJOmOtdm!!GU}iiz~n|MAep@9*@y?qIg+2BT@uYfR6(p zbP~pP7bq56>LIris@(`VF`)hqAQd>MzPAN)qX`i9oe&BzM*epo^<#Yu{n*uo7U+%t zV5HD!g{<#gKx0L*$LNRILT#wZ3r+HGf&tf6!RlK`I>5evJ3c<`DKh#tV#7-zeLU4d{>p~wtkHhIFpOc-0rF?Fk026i6 z%J#!l`gNb_qY!u#I83Xbwl8ih01LtoQ|zZLtxp+jPkx;4vx&OE`L+N&0kS_aErhNFa2pChUD>x4 z=y`XdZN*r7HkgE3V^gr%vZ7u`Ae};xEBT9L(6XO;>~zm zIpVk7?g@Bq_ssT`LHouO3im$-JKV82I{!O3s<^^Wfo5`|jOYy63&1ejANqI&dIX^Eq#lc5^ErJjWC3X8p-SZu=M4r$NTb~R{-B$KEo||C(PAm zIPN;Z0eszf-oR|aSq@w9c6`L)k$t@xpVO`A^sN`1?S->ZzdC?lbinTj{J@CyKz9%d zVRBnw!Zi(@t@T+T-KP+@ftFag-mQg}aumRXRVGX_VU~#unXqF*xMpSdmalqyjboaT zh+`6(F6t>i1NF>-N5m%U=k@te0qe`bgs@fL{kpKFLH9op#0-6alLqc6FehT=;9?CG zNY^hFOA<D!uy|^ z-xNvq=bMzP@@jtFQ0P9bKeV?FFQ_b!ZHBvNGf;8=T+%yaUY&Qjn70Rdy`l8=-n90q zy@!lr+_2jTcLj01{pTwWV<7eiY+q~ddTa`r_8N;e_8l*5pX)oePu8cl|C`kBZS9Q> z-8u|M)!X_dB!}XFdRuox61Hkgz6D9~8h~?~oXtoZ+9!SVaMDMwPpMyJ*Qy*^l~b$A z)vEHes(h_ZEDo&c{|@}!1k*AP$Ad`Ae5R6@74_=jUYV!3xo`fsOV3Y@Q|#8`Nfgg? zuN&R^`Ons6xgXy6g*EVSR(umi8$Wd&+?4go-^|ar){NeM)nm^aT~qEheU`QFo;T7L z^mdn)-aYlDzaOx#Ied8PrJ>88pZaXmhHWHwcFWak4qKjaxu-4qb^8^j4Zj;PZuQXn zhr6s#7ruS%&L-Djw{OD`d`;Dc zl?Pi|w32;OCsMu3$lh3z+x_0|v3pN`YEhW50BzY&Li$t_>IKv3a=zy zqVW4fK0}Nl6^ZDVDfolLxh$q15g=2;9h8U1L?-hb`@bbFt^`bB6>&3Tn0!v;GZX$V zi(epam4M>4#0wPuDsivEbzjcZ+zQ`F@^ai{@iyYpk0jqgyj%i`JBU{*{C(n03O_`= zUg3v{H!J)I@udpyA>O9&Q^Z#){A=Q?6`n49KT}(!@GRo`cqa=@#MdQ}NL@rcuJBRB z^>IxWUP63-5{Z*JcN_ZP(V_9l5f@f^j@UBu;nDK!=nH!Je@5!c6CN^9Cu;z^>X zmJ2>4alXm&M~Tb4j2|a%lz`%=iK{!}bHwG|FV$WkF84>t|495o2`K(E@C(_b$TM~i zaTCdja~9J704^Cfbs_~nnu32p_K#BsH-%<@}5%-bP%W&({+-;Y9?3Jfr6lw-fj2 z#o9yCjw1gj;%kY^^LhvI;|kY;jdzvR)K!G5%c^SXtIH~a;j&0$tu2(Ipe$-nS>jAt zlAE%`q1lS3JMl3QA9j4?;Uga(lkic14~HnZS|o*zBUhw_szYcvgo;DhaR^O^E-wr? zg#o8tpU`p&3r?Zs6c(I9*C}+Jx|XnzE3|TjghayTQwInfkDtwpP9Lv>Lt7OYJn zQ5LMOt_zjbglkd>RMl3+1}cvuZX57vbLSMdN9MC*eM(W=<|0Zb%B1~4Jfp~14U zIrC%TXyQH!+m?n~5Pba+VJ__}$Z?C1JjytpC6aGt9QQWKzaWn9bol8~@)5AlV^HnO za}+tZGlk?;J2#}@`aJ>PG5Xf4pV#Md{d_!^?_ll3Gf=kw0OJP6XJf-5V1#G3l)r~@ zJSQc8oN?aY_ZiP-^77(<#du~*JCk5P$AJ9%@RNKdpIcHE3}e=A{}`&&8V+~4Li zK9<@472_7hpJ1H(+aDOeoXM|eocr6`jB|h6%Q*M9&l%@_b%t^7hv^y62naaN+^YQnW(sXyeH^kIOXmlOzQstCsq=Sb?y@w)*Sir_mB567u~Gl!6t`-_Zk1J<9u{=Xucrhjiv z8jVX diff --git a/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u.c b/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u.c deleted file mode 100644 index 0321b6e18..000000000 --- a/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u.c +++ /dev/null @@ -1,847 +0,0 @@ -/* This file was automatically generated by CasADi 3.7.2. - * It consists of: - * 1) content generated by CasADi runtime: not copyrighted - * 2) template code copied from CasADi source: permissively licensed (MIT-0) - * 3) user code: owned by the user - * - */ -#ifdef __cplusplus -extern "C" { -#endif - -/* How to prefix internal symbols */ -#ifdef CASADI_CODEGEN_PREFIX - #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID) - #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID - #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID) -#else - #define CASADI_PREFIX(ID) auv_model_impl_dae_fun_jac_x_xdot_u_ ## ID -#endif - -#include - -#ifndef casadi_real -#define casadi_real double -#endif - -#ifndef casadi_int -#define casadi_int int -#endif - -/* Add prefix to internal symbols */ -#define casadi_f0 CASADI_PREFIX(f0) -#define casadi_fabs CASADI_PREFIX(fabs) -#define casadi_s0 CASADI_PREFIX(s0) -#define casadi_s1 CASADI_PREFIX(s1) -#define casadi_s2 CASADI_PREFIX(s2) -#define casadi_s3 CASADI_PREFIX(s3) -#define casadi_s4 CASADI_PREFIX(s4) -#define casadi_s5 CASADI_PREFIX(s5) -#define casadi_s6 CASADI_PREFIX(s6) -#define casadi_sign CASADI_PREFIX(sign) -#define casadi_sq CASADI_PREFIX(sq) - -/* Symbol visibility in DLLs */ -#ifndef CASADI_SYMBOL_EXPORT - #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) - #if defined(STATIC_LINKED) - #define CASADI_SYMBOL_EXPORT - #else - #define CASADI_SYMBOL_EXPORT __declspec(dllexport) - #endif - #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) - #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default"))) - #else - #define CASADI_SYMBOL_EXPORT - #endif -#endif - -casadi_real casadi_fabs(casadi_real x) { -/* Pre-c99 compatibility */ -#if __STDC_VERSION__ < 199901L - return x>0 ? x : -x; -#else - return fabs(x); -#endif -} - -casadi_real casadi_sign(casadi_real x) { return x<0 ? -1 : x>0 ? 1 : x;} - -casadi_real casadi_sq(casadi_real x) { return x*x;} - -static const casadi_int casadi_s0[3] = {9, 1, 1}; -static const casadi_int casadi_s1[3] = {3, 1, 1}; -static const casadi_int casadi_s2[3] = {0, 1, 1}; -static const casadi_int casadi_s3[3] = {0, 0, 1}; -static const casadi_int casadi_s4[60] = - {9, 9, 0, 6, 12, 18, 25, 34, - 43, 46, 48, 48, 0, 1, 2, 3, - 4, 5, 0, 1, 2, 3, 4, 5, - 0, 1, 2, 3, 4, 5, 0, 1, - 2, 3, 4, 5, 6, 0, 1, 2, - 3, 4, 5, 6, 7, 8, 0, 1, - 2, 3, 4, 5, 6, 7, 8, 6, - 7, 8, 6, 8}; -static const casadi_int casadi_s5[21] = - {9, 9, 0, 1, 2, 3, 4, 5, - 6, 7, 8, 9, 0, 1, 2, 3, - 4, 5, 6, 7, 8}; -static const casadi_int casadi_s6[24] = - {9, 3, 0, 6, 12, 18, 0, 1, - 2, 3, 4, 5, 0, 1, 2, 3, - 4, 5, 0, 1, 2, 3, 4, 5}; - -/* auv_model_impl_dae_fun_jac_x_xdot_u:(i0[9],i1[9],i2[3],i3[0],i4[],i5[0])->(o0[9],o1[9x9,48nz],o2[9x9,9nz],o3[9x3,18nz]) */ -static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { - casadi_real a00, a01, a02, a03, a04, a05, a06, a07, a08, a09, a10, a11; - casadi_real a12, a13, a14, a15, a16, a17, a18, a19, a20, a21, a22, a23; - casadi_real a24, a25, a26, a27, a28, a29, a30, a31, a32, a33, a34, a35; - casadi_real a36, a37, a38, a39, a40, a41, a42, a43, a44, a45, a46, a47; - casadi_real a48, a49, a50, a51, a52, a53, a54, a55, a56, a57, a58, a59; - casadi_real a60, a61, a62, a63, a64, a65, a66, a67, a68, a69, a70, a71; - casadi_real a72, a73, a74, a75, a76, a77, a78; - a00=arg[1]? arg[1][0] : 0; - a01=3.3453634479321918e-02; - a02=arg[2]? arg[2][0] : 0; - a03=-30.; - a04=arg[0]? arg[0][5] : 0; - a05=(a03*a04); - a06=arg[0]? arg[0][1] : 0; - a07=(a05*a06); - a08=30.; - a09=arg[0]? arg[0][4] : 0; - a10=(a08*a09); - a11=arg[0]? arg[0][2] : 0; - a12=(a10*a11); - a07=(a07+a12); - a02=(a02-a07); - a07=23.; - a12=arg[0]? arg[0][0] : 0; - a13=casadi_fabs(a12); - a07=(a07+a13); - a13=(a07*a12); - a02=(a02-a13); - a13=(a01*a02); - a14=6.0368975005218254e-05; - a15=(a08*a04); - a16=(a15*a12); - a17=arg[0]? arg[0][3] : 0; - a18=(a03*a17); - a19=(a18*a11); - a16=(a16+a19); - a19=46.; - a20=casadi_fabs(a06); - a20=(a19+a20); - a21=(a20*a06); - a16=(a16+a21); - a21=(a14*a16); - a13=(a13-a21); - a21=-1.1116270017027866e-07; - a22=(a03*a09); - a23=(a22*a12); - a24=(a08*a17); - a25=(a24*a06); - a23=(a23+a25); - a25=casadi_fabs(a11); - a25=(a19+a25); - a26=(a25*a11); - a23=(a23+a26); - a26=(a21*a23); - a13=(a13-a26); - a26=9.8084735444363873e-08; - a27=3.3399999999999999e+00; - a28=(a27*a04); - a29=(a28*a09); - a30=-3.3199999999999998e+00; - a31=(a30*a09); - a32=(a31*a04); - a29=(a29+a32); - a32=casadi_fabs(a17); - a32=(a19+a32); - a33=(a32*a17); - a29=(a29+a33); - a33=(a26*a29); - a13=(a13-a33); - a33=1.0920100546139184e-05; - a34=arg[2]? arg[2][1] : 0; - a35=-3.3399999999999999e+00; - a36=(a35*a04); - a37=(a36*a17); - a38=6.8000000000000005e-01; - a39=(a38*a17); - a40=(a39*a04); - a37=(a37+a40); - a34=(a34-a37); - a37=casadi_fabs(a09); - a37=(a19+a37); - a40=(a37*a09); - a34=(a34-a40); - a40=(a33*a34); - a13=(a13+a40); - a40=-6.0150572994295574e-03; - a41=arg[2]? arg[2][2] : 0; - a42=3.3199999999999998e+00; - a43=(a42*a09); - a44=(a43*a17); - a45=-6.8000000000000005e-01; - a46=(a45*a17); - a47=(a46*a09); - a44=(a44+a47); - a41=(a41-a44); - a44=casadi_fabs(a04); - a19=(a19+a44); - a44=(a19*a04); - a41=(a41-a44); - a44=(a40*a41); - a13=(a13+a44); - a00=(a00-a13); - if (res[0]!=0) res[0][0]=a00; - a00=arg[1]? arg[1][1] : 0; - a13=6.0368975005218423e-05; - a44=(a13*a02); - a47=3.3484658136227786e-02; - a48=(a47*a16); - a44=(a44-a48); - a48=-6.1658244361114702e-05; - a49=(a48*a23); - a44=(a44-a49); - a49=5.4404333259807184e-05; - a50=(a49*a29); - a44=(a44-a50); - a50=6.0570157695918683e-03; - a51=(a50*a34); - a44=(a44+a51); - a51=-3.0184487502609172e-03; - a52=(a51*a41); - a44=(a44+a52); - a00=(a00-a44); - if (res[0]!=0) res[0][1]=a00; - a00=arg[1]? arg[1][2] : 0; - a44=-1.1116270017027936e-07; - a52=(a44*a02); - a53=-6.1658244361114783e-05; - a54=(a53*a16); - a52=(a52-a54); - a54=3.3963490377380855e-02; - a55=(a54*a23); - a52=(a52-a55); - a55=-2.9967785627100754e-02; - a56=(a55*a29); - a52=(a52-a56); - a56=-3.0801331505514837e-03; - a57=(a56*a34); - a52=(a52+a57); - a57=5.5581350085139543e-06; - a58=(a57*a41); - a52=(a52+a58); - a00=(a00-a52); - if (res[0]!=0) res[0][2]=a00; - a00=arg[1]? arg[1][3] : 0; - a52=9.8084735444364535e-08; - a58=(a52*a02); - a59=5.4404333259807062e-05; - a60=(a59*a16); - a58=(a58-a60); - a60=-2.9967785627100469e-02; - a61=(a60*a23); - a58=(a58-a61); - a61=1.4970303990827358e+00; - a62=(a61*a29); - a58=(a58-a62); - a62=2.7177645446042520e-03; - a63=(a62*a34); - a58=(a58+a63); - a63=-4.9042367722181902e-06; - a64=(a63*a41); - a58=(a58+a64); - a00=(a00-a58); - if (res[0]!=0) res[0][3]=a00; - a00=arg[1]? arg[1][4] : 0; - a58=1.0920100546139210e-05; - a64=(a58*a02); - a65=6.0570157695918657e-03; - a66=(a65*a16); - a64=(a64-a66); - a66=-3.0801331505514781e-03; - a67=(a66*a23); - a64=(a64-a67); - a67=2.7177645446042498e-03; - a68=(a67*a29); - a64=(a64-a68); - a68=3.0257778596593993e-01; - a69=(a68*a34); - a64=(a64+a69); - a69=-5.4600502730695923e-04; - a70=(a69*a41); - a64=(a64+a70); - a00=(a00-a64); - if (res[0]!=0) res[0][4]=a00; - a00=arg[1]? arg[1][5] : 0; - a64=-6.0150572994295635e-03; - a02=(a64*a02); - a70=-3.0184487502609133e-03; - a16=(a70*a16); - a02=(a02-a16); - a16=5.5581350085139340e-06; - a23=(a16*a23); - a02=(a02-a23); - a23=-4.9042367722181935e-06; - a29=(a23*a29); - a02=(a02-a29); - a34=(a69*a34); - a02=(a02+a34); - a34=3.0075286497147785e-01; - a41=(a34*a41); - a02=(a02+a41); - a00=(a00-a02); - if (res[0]!=0) res[0][5]=a00; - a00=arg[1]? arg[1][6] : 0; - a02=arg[0]? arg[0][6] : 0; - a41=sin(a02); - a29=arg[0]? arg[0][7] : 0; - a71=tan(a29); - a72=(a41*a71); - a73=(a72*a09); - a73=(a17+a73); - a02=cos(a02); - a74=(a02*a71); - a75=(a74*a04); - a73=(a73+a75); - a00=(a00-a73); - if (res[0]!=0) res[0][6]=a00; - a00=arg[1]? arg[1][7] : 0; - a73=(a02*a09); - a75=(a41*a04); - a73=(a73-a75); - a00=(a00-a73); - if (res[0]!=0) res[0][7]=a00; - a00=arg[1]? arg[1][8] : 0; - a73=cos(a29); - a75=(a41/a73); - a76=(a75*a09); - a77=(a02/a73); - a78=(a77*a04); - a76=(a76+a78); - a00=(a00-a76); - if (res[0]!=0) res[0][8]=a00; - a00=casadi_sign(a12); - a00=(a12*a00); - a00=(a00+a07); - a07=(a01*a00); - a76=(a14*a15); - a07=(a07+a76); - a76=(a21*a22); - a07=(a07+a76); - if (res[1]!=0) res[1][0]=a07; - a07=(a13*a00); - a76=(a47*a15); - a07=(a07+a76); - a76=(a48*a22); - a07=(a07+a76); - if (res[1]!=0) res[1][1]=a07; - a07=(a44*a00); - a76=(a53*a15); - a07=(a07+a76); - a76=(a54*a22); - a07=(a07+a76); - if (res[1]!=0) res[1][2]=a07; - a07=(a52*a00); - a76=(a59*a15); - a07=(a07+a76); - a76=(a60*a22); - a07=(a07+a76); - if (res[1]!=0) res[1][3]=a07; - a07=(a58*a00); - a76=(a65*a15); - a07=(a07+a76); - a76=(a66*a22); - a07=(a07+a76); - if (res[1]!=0) res[1][4]=a07; - a00=(a64*a00); - a15=(a70*a15); - a00=(a00+a15); - a22=(a16*a22); - a00=(a00+a22); - if (res[1]!=0) res[1][5]=a00; - a00=(a01*a05); - a22=casadi_sign(a06); - a22=(a06*a22); - a22=(a22+a20); - a20=(a14*a22); - a00=(a00+a20); - a20=(a21*a24); - a00=(a00+a20); - if (res[1]!=0) res[1][6]=a00; - a00=(a13*a05); - a20=(a47*a22); - a00=(a00+a20); - a20=(a48*a24); - a00=(a00+a20); - if (res[1]!=0) res[1][7]=a00; - a00=(a44*a05); - a20=(a53*a22); - a00=(a00+a20); - a20=(a54*a24); - a00=(a00+a20); - if (res[1]!=0) res[1][8]=a00; - a00=(a52*a05); - a20=(a59*a22); - a00=(a00+a20); - a20=(a60*a24); - a00=(a00+a20); - if (res[1]!=0) res[1][9]=a00; - a00=(a58*a05); - a20=(a65*a22); - a00=(a00+a20); - a20=(a66*a24); - a00=(a00+a20); - if (res[1]!=0) res[1][10]=a00; - a05=(a64*a05); - a22=(a70*a22); - a05=(a05+a22); - a24=(a16*a24); - a05=(a05+a24); - if (res[1]!=0) res[1][11]=a05; - a05=(a01*a10); - a24=(a14*a18); - a05=(a05+a24); - a24=casadi_sign(a11); - a24=(a11*a24); - a24=(a24+a25); - a25=(a21*a24); - a05=(a05+a25); - if (res[1]!=0) res[1][12]=a05; - a05=(a13*a10); - a25=(a47*a18); - a05=(a05+a25); - a25=(a48*a24); - a05=(a05+a25); - if (res[1]!=0) res[1][13]=a05; - a05=(a44*a10); - a25=(a53*a18); - a05=(a05+a25); - a25=(a54*a24); - a05=(a05+a25); - if (res[1]!=0) res[1][14]=a05; - a05=(a52*a10); - a25=(a59*a18); - a05=(a05+a25); - a25=(a60*a24); - a05=(a05+a25); - if (res[1]!=0) res[1][15]=a05; - a05=(a58*a10); - a25=(a65*a18); - a05=(a05+a25); - a25=(a66*a24); - a05=(a05+a25); - if (res[1]!=0) res[1][16]=a05; - a10=(a64*a10); - a18=(a70*a18); - a10=(a10+a18); - a24=(a16*a24); - a10=(a10+a24); - if (res[1]!=0) res[1][17]=a10; - a10=(a03*a11); - a24=(a14*a10); - a18=(a08*a06); - a05=(a21*a18); - a24=(a24+a05); - a05=casadi_sign(a17); - a05=(a17*a05); - a05=(a05+a32); - a32=(a26*a05); - a24=(a24+a32); - a38=(a38*a04); - a36=(a36+a38); - a38=(a33*a36); - a24=(a24+a38); - a45=(a45*a09); - a43=(a43+a45); - a45=(a40*a43); - a24=(a24+a45); - if (res[1]!=0) res[1][18]=a24; - a24=(a47*a10); - a45=(a48*a18); - a24=(a24+a45); - a45=(a49*a05); - a24=(a24+a45); - a45=(a50*a36); - a24=(a24+a45); - a45=(a51*a43); - a24=(a24+a45); - if (res[1]!=0) res[1][19]=a24; - a24=(a53*a10); - a45=(a54*a18); - a24=(a24+a45); - a45=(a55*a05); - a24=(a24+a45); - a45=(a56*a36); - a24=(a24+a45); - a45=(a57*a43); - a24=(a24+a45); - if (res[1]!=0) res[1][20]=a24; - a24=(a59*a10); - a45=(a60*a18); - a24=(a24+a45); - a45=(a61*a05); - a24=(a24+a45); - a45=(a62*a36); - a24=(a24+a45); - a45=(a63*a43); - a24=(a24+a45); - if (res[1]!=0) res[1][21]=a24; - a24=(a65*a10); - a45=(a66*a18); - a24=(a24+a45); - a45=(a67*a05); - a24=(a24+a45); - a45=(a68*a36); - a24=(a24+a45); - a45=(a69*a43); - a24=(a24+a45); - if (res[1]!=0) res[1][22]=a24; - a10=(a70*a10); - a18=(a16*a18); - a10=(a10+a18); - a05=(a23*a05); - a10=(a10+a05); - a36=(a69*a36); - a10=(a10+a36); - a43=(a34*a43); - a10=(a10+a43); - if (res[1]!=0) res[1][23]=a10; - a10=-1.; - if (res[1]!=0) res[1][24]=a10; - a11=(a08*a11); - a10=(a01*a11); - a43=(a03*a12); - a21=(a21*a43); - a10=(a10+a21); - a30=(a30*a04); - a28=(a28+a30); - a30=(a26*a28); - a10=(a10+a30); - a30=casadi_sign(a09); - a30=(a09*a30); - a30=(a30+a37); - a37=(a33*a30); - a10=(a10+a37); - a42=(a42*a17); - a42=(a42+a46); - a46=(a40*a42); - a10=(a10+a46); - if (res[1]!=0) res[1][25]=a10; - a10=(a13*a11); - a48=(a48*a43); - a10=(a10+a48); - a48=(a49*a28); - a10=(a10+a48); - a48=(a50*a30); - a10=(a10+a48); - a48=(a51*a42); - a10=(a10+a48); - if (res[1]!=0) res[1][26]=a10; - a10=(a44*a11); - a54=(a54*a43); - a10=(a10+a54); - a54=(a55*a28); - a10=(a10+a54); - a54=(a56*a30); - a10=(a10+a54); - a54=(a57*a42); - a10=(a10+a54); - if (res[1]!=0) res[1][27]=a10; - a10=(a52*a11); - a60=(a60*a43); - a10=(a10+a60); - a60=(a61*a28); - a10=(a10+a60); - a60=(a62*a30); - a10=(a10+a60); - a60=(a63*a42); - a10=(a10+a60); - if (res[1]!=0) res[1][28]=a10; - a10=(a58*a11); - a66=(a66*a43); - a10=(a10+a66); - a66=(a67*a28); - a10=(a10+a66); - a66=(a68*a30); - a10=(a10+a66); - a66=(a69*a42); - a10=(a10+a66); - if (res[1]!=0) res[1][29]=a10; - a11=(a64*a11); - a16=(a16*a43); - a11=(a11+a16); - a28=(a23*a28); - a11=(a11+a28); - a30=(a69*a30); - a11=(a11+a30); - a42=(a34*a42); - a11=(a11+a42); - if (res[1]!=0) res[1][30]=a11; - a72=(-a72); - if (res[1]!=0) res[1][31]=a72; - a72=(-a02); - if (res[1]!=0) res[1][32]=a72; - a72=(-a75); - if (res[1]!=0) res[1][33]=a72; - a03=(a03*a06); - a01=(a01*a03); - a08=(a08*a12); - a14=(a14*a08); - a01=(a01+a14); - a27=(a27*a09); - a27=(a27+a31); - a26=(a26*a27); - a01=(a01+a26); - a35=(a35*a17); - a35=(a35+a39); - a33=(a33*a35); - a01=(a01+a33); - a33=casadi_sign(a04); - a33=(a04*a33); - a33=(a33+a19); - a40=(a40*a33); - a01=(a01+a40); - if (res[1]!=0) res[1][34]=a01; - a13=(a13*a03); - a47=(a47*a08); - a13=(a13+a47); - a49=(a49*a27); - a13=(a13+a49); - a50=(a50*a35); - a13=(a13+a50); - a51=(a51*a33); - a13=(a13+a51); - if (res[1]!=0) res[1][35]=a13; - a44=(a44*a03); - a53=(a53*a08); - a44=(a44+a53); - a55=(a55*a27); - a44=(a44+a55); - a56=(a56*a35); - a44=(a44+a56); - a57=(a57*a33); - a44=(a44+a57); - if (res[1]!=0) res[1][36]=a44; - a52=(a52*a03); - a59=(a59*a08); - a52=(a52+a59); - a61=(a61*a27); - a52=(a52+a61); - a62=(a62*a35); - a52=(a52+a62); - a63=(a63*a33); - a52=(a52+a63); - if (res[1]!=0) res[1][37]=a52; - a58=(a58*a03); - a65=(a65*a08); - a58=(a58+a65); - a67=(a67*a27); - a58=(a58+a67); - a68=(a68*a35); - a58=(a58+a68); - a68=(a69*a33); - a58=(a58+a68); - if (res[1]!=0) res[1][38]=a58; - a64=(a64*a03); - a70=(a70*a08); - a64=(a64+a70); - a23=(a23*a27); - a64=(a64+a23); - a69=(a69*a35); - a64=(a64+a69); - a34=(a34*a33); - a64=(a64+a34); - if (res[1]!=0) res[1][39]=a64; - a74=(-a74); - if (res[1]!=0) res[1][40]=a74; - if (res[1]!=0) res[1][41]=a41; - a74=(-a77); - if (res[1]!=0) res[1][42]=a74; - a74=(a71*a02); - a74=(a09*a74); - a71=(a71*a41); - a71=(a04*a71); - a74=(a74-a71); - a74=(-a74); - if (res[1]!=0) res[1][43]=a74; - a74=(a09*a41); - a71=(a04*a02); - a74=(a74+a71); - if (res[1]!=0) res[1][44]=a74; - a74=(a09*a77); - a71=(a04*a75); - a74=(a74-a71); - a74=(-a74); - if (res[1]!=0) res[1][45]=a74; - a74=casadi_sq(a73); - a41=(a41/a74); - a41=(a09*a41); - a02=(a02/a74); - a02=(a04*a02); - a41=(a41+a02); - a41=(-a41); - if (res[1]!=0) res[1][46]=a41; - a75=(a75/a73); - a29=sin(a29); - a75=(a75*a29); - a09=(a09*a75); - a77=(a77/a73); - a77=(a77*a29); - a04=(a04*a77); - a09=(a09+a04); - a09=(-a09); - if (res[1]!=0) res[1][47]=a09; - a09=1.; - if (res[2]!=0) res[2][0]=a09; - if (res[2]!=0) res[2][1]=a09; - if (res[2]!=0) res[2][2]=a09; - if (res[2]!=0) res[2][3]=a09; - if (res[2]!=0) res[2][4]=a09; - if (res[2]!=0) res[2][5]=a09; - if (res[2]!=0) res[2][6]=a09; - if (res[2]!=0) res[2][7]=a09; - if (res[2]!=0) res[2][8]=a09; - a09=-3.3453634479321918e-02; - if (res[3]!=0) res[3][0]=a09; - a09=-6.0368975005218423e-05; - if (res[3]!=0) res[3][1]=a09; - a09=1.1116270017027936e-07; - if (res[3]!=0) res[3][2]=a09; - a09=-9.8084735444364535e-08; - if (res[3]!=0) res[3][3]=a09; - a09=-1.0920100546139210e-05; - if (res[3]!=0) res[3][4]=a09; - a09=6.0150572994295635e-03; - if (res[3]!=0) res[3][5]=a09; - a09=-1.0920100546139184e-05; - if (res[3]!=0) res[3][6]=a09; - a09=-6.0570157695918683e-03; - if (res[3]!=0) res[3][7]=a09; - a09=3.0801331505514837e-03; - if (res[3]!=0) res[3][8]=a09; - a09=-2.7177645446042520e-03; - if (res[3]!=0) res[3][9]=a09; - a09=-3.0257778596593993e-01; - if (res[3]!=0) res[3][10]=a09; - a09=5.4600502730695923e-04; - if (res[3]!=0) res[3][11]=a09; - a04=6.0150572994295574e-03; - if (res[3]!=0) res[3][12]=a04; - a04=3.0184487502609172e-03; - if (res[3]!=0) res[3][13]=a04; - a04=-5.5581350085139543e-06; - if (res[3]!=0) res[3][14]=a04; - a04=4.9042367722181902e-06; - if (res[3]!=0) res[3][15]=a04; - if (res[3]!=0) res[3][16]=a09; - a09=-3.0075286497147785e-01; - if (res[3]!=0) res[3][17]=a09; - return 0; -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_u(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ - return casadi_f0(arg, res, iw, w, mem); -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_u_alloc_mem(void) { - return 0; -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_u_init_mem(int mem) { - return 0; -} - -CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_jac_x_xdot_u_free_mem(int mem) { -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_u_checkout(void) { - return 0; -} - -CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_jac_x_xdot_u_release(int mem) { -} - -CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_jac_x_xdot_u_incref(void) { -} - -CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_jac_x_xdot_u_decref(void) { -} - -CASADI_SYMBOL_EXPORT casadi_int auv_model_impl_dae_fun_jac_x_xdot_u_n_in(void) { return 6;} - -CASADI_SYMBOL_EXPORT casadi_int auv_model_impl_dae_fun_jac_x_xdot_u_n_out(void) { return 4;} - -CASADI_SYMBOL_EXPORT casadi_real auv_model_impl_dae_fun_jac_x_xdot_u_default_in(casadi_int i) { - switch (i) { - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const char* auv_model_impl_dae_fun_jac_x_xdot_u_name_in(casadi_int i) { - switch (i) { - case 0: return "i0"; - case 1: return "i1"; - case 2: return "i2"; - case 3: return "i3"; - case 4: return "i4"; - case 5: return "i5"; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const char* auv_model_impl_dae_fun_jac_x_xdot_u_name_out(casadi_int i) { - switch (i) { - case 0: return "o0"; - case 1: return "o1"; - case 2: return "o2"; - case 3: return "o3"; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const casadi_int* auv_model_impl_dae_fun_jac_x_xdot_u_sparsity_in(casadi_int i) { - switch (i) { - case 0: return casadi_s0; - case 1: return casadi_s0; - case 2: return casadi_s1; - case 3: return casadi_s2; - case 4: return casadi_s3; - case 5: return casadi_s2; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const casadi_int* auv_model_impl_dae_fun_jac_x_xdot_u_sparsity_out(casadi_int i) { - switch (i) { - case 0: return casadi_s0; - case 1: return casadi_s4; - case 2: return casadi_s5; - case 3: return casadi_s6; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_u_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { - if (sz_arg) *sz_arg = 6; - if (sz_res) *sz_res = 4; - if (sz_iw) *sz_iw = 0; - if (sz_w) *sz_w = 0; - return 0; -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_u_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { - if (sz_arg) *sz_arg = 6*sizeof(const casadi_real*); - if (sz_res) *sz_res = 4*sizeof(casadi_real*); - if (sz_iw) *sz_iw = 0*sizeof(casadi_int); - if (sz_w) *sz_w = 0*sizeof(casadi_real); - return 0; -} - - -#ifdef __cplusplus -} /* extern "C" */ -#endif diff --git a/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u.o b/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u.o deleted file mode 100644 index 4f9555c24d94168607aa270aeeee8e89f9bc61ee..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 21328 zcmcJX3w+eomB;_c!${fuCl)PjT^)76U?ol-KxmarAdzW}HfoTW8caeS5``p^L1U#B zOK4TJ#_FeKw{D9IwY1AtyLG#)bxGtIK`{cpz#>|TP+tLjRnhG4oO^EOLFk`DUr-^<0{aM>rbsPWH0AqyFh1Hux38 z32pGtkoUKrXz;Up8vOISZ#?~ozkchGBYuEIAo_TNKXYq?f92y4k9al#FN;YB={w@b zyYzeUtl}$6bKfhTSzIwQdi%#$L~lPDz4eVYm>FF*BO|)BD{&Ik1RK8U-*Oq6lper)Rzzw$8T zpKkDHJ&9{ygFpI^KYmSUAM61?M4G8q?;_!Cyd1+tLLcCIg#^PW!yo@xs23Uh@vA~X zyps$JDG6mmYCq8~q#nctb7R~JbKg)FuFNT_McAkXz7Cd~`=kP1*cLQflOpqvf<+fP`> zu$CUtG91!T%2L8zqAV2QJY~zyK2qWbCPKt)+3<^YJbVn{eJrwKFe-c1plLd z{So=^V?V=(ce1ZQ5}_b!_&n*Uc2|X*hb303pdmvv`|)=r)!P{nT*;^G}3k zr%w=_7mC^p6(Nj@007MF!L}HyC7X#RAoB`vXG8+q!rz8|R z;$M9b*FG+!8uz+T6dKoXW<);5^(^q3P&ur|RD#J8>lRf3DX;Nlb_dFVT3!@+MhX~X zU>&l?QI{CaPJSHoTx>nO){U$i$#Z42#9_LDRq?0EojeL=lK04&@J(em1WLHJn@kRJ zSXl_BQjS(iZ3;vopUw4-E}Kp(zu&%rEsXPWm(AS|7CdH`&{L4=L6=$Ts9LBe zt~l1)(Or1<;Nt$%fb|&Z_wi~8$Tq;qp4|!ffOFUc&_5r1@*$_EB>OXA8i?^ zj;GL*#}I6$X?f%=<(Hb!lAjcc?Vzy@huvA%4_J|C5sr%~`8E23kYghre0p^1p-56~{$q)@m6)PAdd>7KkpBaYnnS z_~Gd79bH_i>k+$Fx!%Tx4c3&{oWk4-3Jz-4N>6 zz|}^9?V%p1r2Q6kpal1AO#9fxy+PTZq8k~vC^Q!;4&pM?L5V+lJ#vl4gcFxZW%Qc> z@h%KttSE8zGjdy(RVG4h7+M>k!OFjfyfhIh7XQ8TzE|E~8c-Oyc`$%F;p=I-7)BiZH6h-3UHpx zleE6fY7Z?2MA`=?4eK2$Fbg}8ti^JIG{=jIPfZ^@W&9jd2DzS3jAr^l0|)6ma6t%~ zvgwR!PBvoktUpuYJRB?V;NDF6w-{Lk;{v9>InI$}vb`0hkQMjaypWg7t_Y)R!a$p19uKSt6|lZ%egD^_kALD|nkyayTEc^il+h!xY(3*ws~ z35gPa)+;cx2jW{nMn8#)R-1}q*C{j6b=aL@x4g>qdb=$}*NtFHiW08w=DjScPHP=L zssvDK1#JeeuAr?e2v%B}?pPt$r5cHKlt@z?O2<6O{hXHNAT+&%i;Oo5GhE5K`7n#K zmgzjGAD#Be$CzzqVS!)T0^6}e3DB2=anWVc?xrI}Z;z4(yZkJS;YfgXYPiA>A4B3z zu!8#((v#6hK*fV99#(OvwOJ$ODqgMPEy*}=VWbaDbkG{`p3Wh9?_Vg3+VnV< z)juu#r83cL`pY!44%gF87rK7O@o-6BUNDw0m*EtIspUnqMRqsd9tXzg!L18Z5ij3( z$gvV1D(UMJjg+OU5BFiUG#{(*u@f!ND^_$1Rt%a@qg%|$%fw4U5sKKOAVx|Z%_X?U z4vjg|*Vo{m(M|&Ro@%~1qMRezg$-C+Q*ZbNLs2LRrKajq)!v?-n+u3nW z=}Y>C-`E_zi`U`X`=YhO9*qv^ncBCHzNbg;;!Y27)BeZn9X{!|PjoKnJ0bCvZ;z9= zrG1HShM>bF$Y@oP(r?%4X zH8nErmLK$t4-{P_&t+dYclC(xjw-s|q_3NFbKjwgif>N+-MxEO{G#@OqOslE zN9}v)Cq-j_TC(-Rr`w7?EQ-w>`{>-F|9R!5@IwauoCl|_Z0jr=w{iciUui5_ z{BnD{SN{A|(cfwHJNY}so^@hRr&FHQ*J<_H`dE3J-_~!K*!?e3pI?c6*<$zEV&~8Z z+utemooefQqp4@-ju&@~eeUS{MR8M~&O_mCk9JINC|WM{`rPuTuYC2MUF)R2ol^gP ze^`6>OTI6Pe16vU$<4EO7L|9Lx3_SAai@K@ZrXnMwxXT3y*i~nu6^so-|K97QonUl zzfQ5YNbK#D@@zRpQvV{Uzpc+Ysn1lgr%3GSG##v0o>RNb`_zbePL}74$lBAFzJA%- zk&(OCznfio_sFgd^jQDLB6)TSw{l(HjOo+A9L%0MXJJ!fVK6s8CpRZNp>QF;=H8N* zmlG}+FPMv>`7MBp=C~P`&ZK#-iX{*8`2gS^2JdnW^`a+QJ&KzjZYSc z6SH&86iFjiB5^y-JBJr%hQB+aB(reI ziC@cnc<9jgP9B+AI6X65oS8i>GYHvfnSp7U8PU%mJC(6o^wFG3f!JdlGxMxvJ;Lmq zPICJPk)zKV6tvtZnQSx*4#EnAjWjCQ7TVKM(5zCJ^$XcThc*f~n+NA@7e;=gKyu54 zSw9k6Gl*Q5FuR~qd+u@=P|84}!@R?Ng9!}f8TQjDLp{41B-748bO?BmHxg#OrPND> z^MZ>wt-}-+;v)H8vDNbXg$Eox8-%aGlR|}gcz?o^^jK4Sg=ax8@rb-v%d?Qn%kT>2 z-99c99=1T9(}V{dJ|hJ$6JGAfU!H=~*@RZWh$DY>3SKAN*3)LYHU&5R4f;9VN_*Z7 zR=>j+2`_i}cT(`%gjYNA-%r8s6K?ypwdkLP^O?~?JXZ*}!CyUz`->*t!&aqwpR_erpQeBfP_re=Y@oN%&%ZCNsU7f*-Vc9Qn6W@OOk8 zzb5s6n1Y*eo8jG-6y?|m9-kRro5M#5Z*lm^!ZRE@#|ZZve!6hezmm#A!k0Vp=L%o# zaQf+r#@EA16&#zCf?ptfg(E*L1&<03IOSfNf>#JP{UTY&H&XCw;rkr@4Jr6M;k}Oh zf)u<>_}Zi?9J@vML5H^s-{MP3myIe^9--r;h%_n#NoquBbng^9lwnf9(H(u`AJ^X(Q~TEcRBK>iF~;u zf0oFb`6-#0E!^%OY)X#sAlJiE&npma=7}W#vhau_9}!;e@QZ|3JG@MIi^DG$zS!Y0 z;ckDJ&3t(OzTH|{E8M=@XQOc60(rIwcXx0%2zPt#BH?y_X4Neb?)J|+gxhZ|R{kF0 z{PE91JeLdqqGA;NDfr21QLtx_ZsFEH_8f9pxa}vl9%FFWAwAWipU)$RUjS}(ND@D$ zr{F~?_>2_%VsI+Aht59;_S|xl@O=({PJ#=s({Q==t{ygD9htCoo zcKCOMM}*t+%fAUPclfiytA$%X4?~9{ds>9s^G#TIuW)Wms z&gv0v&pWRP57UP+1k1geg*Q~qZ?3DKSJgPbWnNWXO?_3v!ltTgYig_7s@m$B6IBay zYSWRbY7M6+mY1GbetKfL>4{BDPplw4u}NM|+4MZRO{N>aPNY~a-zVmB+~6?Bjhx|y zoQLnZQ#j7|JeHiy_dJ%&<0A6%NhYs=@2o%1l+QXR8qV4#8a;fU$ddU+C+o>)os(Hx zzA>Kj<{QHCi6+i@Cz)JaRz8=N&!yxW{Y(m2|3sE4VEvO=LxIt6WSC6myaim!WY$o? zr4(@9Nvx;9)W}GhJ4+TCZLGhLwG|pYtf7$atiVW+qc~r#flPB*M=s~iHFae&iQ|)u z6aY01JR3+fxv60NIGTePVvP5(Ak zrCX`G`i7c?^Ac$*u9;u|SIMSpM{7&Xg4V{wP3bD{mru8V8=DtgmsWd_VAY(P67{WE zAMN>rww?5C?wI^=Lcb?bAWq+0EiWO6K>X{tEZ;;BfjI5CEPs_C0`bdmS#Hk(#PPS( z{rnfQ-^Vt`TcG%_g_Hc{ihrH}f-8TfaC%&U%a%Jah5Us|p1vho`BKHTo+}iespQT1 z`P23+R`OcUD#fo-2_jH(7A{-g>lLT7g5|d={#nKE zQJnVjR{jCSX{%@XuN9{)n&n#+r{5VYe@SsV6IlL+;`9fAmLF66bj3$upGtx1dj>8m zf12V!#Y+@FTXEVqQy@KO;<9?KQGBf8H!4ow#I5}I6d$knj})gdXyt#cIQ`CR`9{Sj zD85Vau;Tj_&r|#z#q$-XeLMxKZvie_Zb0!FijPx#vf@(|pQw1T;?oqrMDb$9w)e z|GIFuzSk;wUEg-ab$$O`ac$3kE3WJNgyOosFDf2VcD|yx_QRWsPgn9I@9z}PRpo9`{L6}urcDEa zTi=Po$v=7>xlnPv9$l>XmB?f3yHN3%;@?wTuSY*p{BuhF=Zfp~=)V-#>&rI9^*Xgj zalP)muDD)zK2kiW?D>p*SR?<8Rs0Oa^?G!^;^!#&BE`om{&mH_qT(3Jz z6xZv>y^8B~WToPI9r>N&dOqKv_+(}0HpTTkyhCxlFMU&SJ^%k%@r#uH;qqaT{IBQ# zGZmkqUTP4QyIOBBCNxH~S^D|x-2Jf?VD>B*!`GXmN94P16yoUQm(il3)=rQ%Z+ zKUML|6{k;ZR{tEuzo7Wdihot{A1JQpjh`rk~H_yvmpR`GKcU$6L7#kVVdp5nU| zk0}0@;$@1TLYrU&@-sDs^}`jy-FCcF$y3zIuTWgiZ@*Do&nJ&5K0)c}R$QM`{-F5R zl>9r2U!wRBT5J%gUV2|WTJhOPv-W2zuJ>;<6pt$TS&Cn*c&*|&iZ?5+=k1#n*Ym&+ z6xZ|ZLyGHuyIyhKZ}%v!=g-#_*Yn#)itB!ee>lN`{HgofIK}n+lcTuqx04i)K$rD@ zRB=6DolKhp1nP$gTvonT@tYK1Bi!xJI~Av;$?DmYf)Ay|8-es(jmyesDqf{H{cjQ! zNWL1El^>^gjpBKV&ry7;;+$;&#r65_mx@nQ<^EQ2y-(Pzc!`qVp|~D*uPUy`5&e%k6v)qd-9LdQBLw1l zzj2x3dc0IBPFoD?|5b|9Qe*kUieIDn9>w*#^N!->N`53Q-UwvpWw@+<`}-zwt-n>t zYyFF8bBsXpLvdL>_bNVD@s)}tTCKhpX^!Sefr%V&Uo-YuN^HU%sHl5klb`?>v{*ZR-O zZxnekvtPbi0PTe+Sou2PL8eK*TzJUgi-gnOl7iLKA)MA|3YNF0&~K#7w=XSk>q&ba z&1plLu=BK^+tpzx5x!@>(4 z9uaQ-o;B&Wa^cmEe6{d|!&`*gzvHv^^a{V7)M2Glu1%9>x39d1CY}G^{JF?WI%%(?gDs!V3_95I?Xwh#6sP{9!#=V9 zJSQQjg%FS68q9y$7aPH#6h!``Hgf%U2^fjU_8T!10x~gnIOW^+v(Kv_K3M)0Qt_Bo zC{J6yea@BqVMp!*$#3URn=IAgGKtYzYMGUxXNW$64H8`0TOo5IE~lDaw&V{wI;`Cm z{}Upl)6E~1{0E%^-Te0 - -#ifndef casadi_real -#define casadi_real double -#endif - -#ifndef casadi_int -#define casadi_int int -#endif - -/* Add prefix to internal symbols */ -#define casadi_f0 CASADI_PREFIX(f0) -#define casadi_fabs CASADI_PREFIX(fabs) -#define casadi_s0 CASADI_PREFIX(s0) -#define casadi_s1 CASADI_PREFIX(s1) -#define casadi_s2 CASADI_PREFIX(s2) -#define casadi_s3 CASADI_PREFIX(s3) -#define casadi_s4 CASADI_PREFIX(s4) -#define casadi_s5 CASADI_PREFIX(s5) -#define casadi_s6 CASADI_PREFIX(s6) -#define casadi_s7 CASADI_PREFIX(s7) -#define casadi_sign CASADI_PREFIX(sign) -#define casadi_sq CASADI_PREFIX(sq) - -/* Symbol visibility in DLLs */ -#ifndef CASADI_SYMBOL_EXPORT - #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) - #if defined(STATIC_LINKED) - #define CASADI_SYMBOL_EXPORT - #else - #define CASADI_SYMBOL_EXPORT __declspec(dllexport) - #endif - #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) - #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default"))) - #else - #define CASADI_SYMBOL_EXPORT - #endif -#endif - -casadi_real casadi_fabs(casadi_real x) { -/* Pre-c99 compatibility */ -#if __STDC_VERSION__ < 199901L - return x>0 ? x : -x; -#else - return fabs(x); -#endif -} - -casadi_real casadi_sign(casadi_real x) { return x<0 ? -1 : x>0 ? 1 : x;} - -casadi_real casadi_sq(casadi_real x) { return x*x;} - -static const casadi_int casadi_s0[3] = {9, 1, 1}; -static const casadi_int casadi_s1[3] = {3, 1, 1}; -static const casadi_int casadi_s2[3] = {0, 1, 1}; -static const casadi_int casadi_s3[3] = {0, 0, 1}; -static const casadi_int casadi_s4[60] = - {9, 9, 0, 6, 12, 18, 25, 34, - 43, 46, 48, 48, 0, 1, 2, 3, - 4, 5, 0, 1, 2, 3, 4, 5, - 0, 1, 2, 3, 4, 5, 0, 1, - 2, 3, 4, 5, 6, 0, 1, 2, - 3, 4, 5, 6, 7, 8, 0, 1, - 2, 3, 4, 5, 6, 7, 8, 6, - 7, 8, 6, 8}; -static const casadi_int casadi_s5[21] = - {9, 9, 0, 1, 2, 3, 4, 5, - 6, 7, 8, 9, 0, 1, 2, 3, - 4, 5, 6, 7, 8}; -static const casadi_int casadi_s6[24] = - {9, 3, 0, 6, 12, 18, 0, 1, - 2, 3, 4, 5, 0, 1, 2, 3, - 4, 5, 0, 1, 2, 3, 4, 5}; -static const casadi_int casadi_s7[3] = {9, 0, 1}; - -/* auv_model_impl_dae_fun_jac_x_xdot_u_z:(i0[9],i1[9],i2[3],i3[0],i4[],i5[0])->(o0[9],o1[9x9,48nz],o2[9x9,9nz],o3[9x3,18nz],o4[9x0]) */ -static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { - casadi_real a00, a01, a02, a03, a04, a05, a06, a07, a08, a09, a10, a11; - casadi_real a12, a13, a14, a15, a16, a17, a18, a19, a20, a21, a22, a23; - casadi_real a24, a25, a26, a27, a28, a29, a30, a31, a32, a33, a34, a35; - casadi_real a36, a37, a38, a39, a40, a41, a42, a43, a44, a45, a46, a47; - casadi_real a48, a49, a50, a51, a52, a53, a54, a55, a56, a57, a58, a59; - casadi_real a60, a61, a62, a63, a64, a65, a66, a67, a68, a69, a70, a71; - casadi_real a72, a73, a74, a75, a76, a77, a78; - a00=arg[1]? arg[1][0] : 0; - a01=3.3453634479321918e-02; - a02=arg[2]? arg[2][0] : 0; - a03=-30.; - a04=arg[0]? arg[0][5] : 0; - a05=(a03*a04); - a06=arg[0]? arg[0][1] : 0; - a07=(a05*a06); - a08=30.; - a09=arg[0]? arg[0][4] : 0; - a10=(a08*a09); - a11=arg[0]? arg[0][2] : 0; - a12=(a10*a11); - a07=(a07+a12); - a02=(a02-a07); - a07=23.; - a12=arg[0]? arg[0][0] : 0; - a13=casadi_fabs(a12); - a07=(a07+a13); - a13=(a07*a12); - a02=(a02-a13); - a13=(a01*a02); - a14=6.0368975005218254e-05; - a15=(a08*a04); - a16=(a15*a12); - a17=arg[0]? arg[0][3] : 0; - a18=(a03*a17); - a19=(a18*a11); - a16=(a16+a19); - a19=46.; - a20=casadi_fabs(a06); - a20=(a19+a20); - a21=(a20*a06); - a16=(a16+a21); - a21=(a14*a16); - a13=(a13-a21); - a21=-1.1116270017027866e-07; - a22=(a03*a09); - a23=(a22*a12); - a24=(a08*a17); - a25=(a24*a06); - a23=(a23+a25); - a25=casadi_fabs(a11); - a25=(a19+a25); - a26=(a25*a11); - a23=(a23+a26); - a26=(a21*a23); - a13=(a13-a26); - a26=9.8084735444363873e-08; - a27=3.3399999999999999e+00; - a28=(a27*a04); - a29=(a28*a09); - a30=-3.3199999999999998e+00; - a31=(a30*a09); - a32=(a31*a04); - a29=(a29+a32); - a32=casadi_fabs(a17); - a32=(a19+a32); - a33=(a32*a17); - a29=(a29+a33); - a33=(a26*a29); - a13=(a13-a33); - a33=1.0920100546139184e-05; - a34=arg[2]? arg[2][1] : 0; - a35=-3.3399999999999999e+00; - a36=(a35*a04); - a37=(a36*a17); - a38=6.8000000000000005e-01; - a39=(a38*a17); - a40=(a39*a04); - a37=(a37+a40); - a34=(a34-a37); - a37=casadi_fabs(a09); - a37=(a19+a37); - a40=(a37*a09); - a34=(a34-a40); - a40=(a33*a34); - a13=(a13+a40); - a40=-6.0150572994295574e-03; - a41=arg[2]? arg[2][2] : 0; - a42=3.3199999999999998e+00; - a43=(a42*a09); - a44=(a43*a17); - a45=-6.8000000000000005e-01; - a46=(a45*a17); - a47=(a46*a09); - a44=(a44+a47); - a41=(a41-a44); - a44=casadi_fabs(a04); - a19=(a19+a44); - a44=(a19*a04); - a41=(a41-a44); - a44=(a40*a41); - a13=(a13+a44); - a00=(a00-a13); - if (res[0]!=0) res[0][0]=a00; - a00=arg[1]? arg[1][1] : 0; - a13=6.0368975005218423e-05; - a44=(a13*a02); - a47=3.3484658136227786e-02; - a48=(a47*a16); - a44=(a44-a48); - a48=-6.1658244361114702e-05; - a49=(a48*a23); - a44=(a44-a49); - a49=5.4404333259807184e-05; - a50=(a49*a29); - a44=(a44-a50); - a50=6.0570157695918683e-03; - a51=(a50*a34); - a44=(a44+a51); - a51=-3.0184487502609172e-03; - a52=(a51*a41); - a44=(a44+a52); - a00=(a00-a44); - if (res[0]!=0) res[0][1]=a00; - a00=arg[1]? arg[1][2] : 0; - a44=-1.1116270017027936e-07; - a52=(a44*a02); - a53=-6.1658244361114783e-05; - a54=(a53*a16); - a52=(a52-a54); - a54=3.3963490377380855e-02; - a55=(a54*a23); - a52=(a52-a55); - a55=-2.9967785627100754e-02; - a56=(a55*a29); - a52=(a52-a56); - a56=-3.0801331505514837e-03; - a57=(a56*a34); - a52=(a52+a57); - a57=5.5581350085139543e-06; - a58=(a57*a41); - a52=(a52+a58); - a00=(a00-a52); - if (res[0]!=0) res[0][2]=a00; - a00=arg[1]? arg[1][3] : 0; - a52=9.8084735444364535e-08; - a58=(a52*a02); - a59=5.4404333259807062e-05; - a60=(a59*a16); - a58=(a58-a60); - a60=-2.9967785627100469e-02; - a61=(a60*a23); - a58=(a58-a61); - a61=1.4970303990827358e+00; - a62=(a61*a29); - a58=(a58-a62); - a62=2.7177645446042520e-03; - a63=(a62*a34); - a58=(a58+a63); - a63=-4.9042367722181902e-06; - a64=(a63*a41); - a58=(a58+a64); - a00=(a00-a58); - if (res[0]!=0) res[0][3]=a00; - a00=arg[1]? arg[1][4] : 0; - a58=1.0920100546139210e-05; - a64=(a58*a02); - a65=6.0570157695918657e-03; - a66=(a65*a16); - a64=(a64-a66); - a66=-3.0801331505514781e-03; - a67=(a66*a23); - a64=(a64-a67); - a67=2.7177645446042498e-03; - a68=(a67*a29); - a64=(a64-a68); - a68=3.0257778596593993e-01; - a69=(a68*a34); - a64=(a64+a69); - a69=-5.4600502730695923e-04; - a70=(a69*a41); - a64=(a64+a70); - a00=(a00-a64); - if (res[0]!=0) res[0][4]=a00; - a00=arg[1]? arg[1][5] : 0; - a64=-6.0150572994295635e-03; - a02=(a64*a02); - a70=-3.0184487502609133e-03; - a16=(a70*a16); - a02=(a02-a16); - a16=5.5581350085139340e-06; - a23=(a16*a23); - a02=(a02-a23); - a23=-4.9042367722181935e-06; - a29=(a23*a29); - a02=(a02-a29); - a34=(a69*a34); - a02=(a02+a34); - a34=3.0075286497147785e-01; - a41=(a34*a41); - a02=(a02+a41); - a00=(a00-a02); - if (res[0]!=0) res[0][5]=a00; - a00=arg[1]? arg[1][6] : 0; - a02=arg[0]? arg[0][6] : 0; - a41=sin(a02); - a29=arg[0]? arg[0][7] : 0; - a71=tan(a29); - a72=(a41*a71); - a73=(a72*a09); - a73=(a17+a73); - a02=cos(a02); - a74=(a02*a71); - a75=(a74*a04); - a73=(a73+a75); - a00=(a00-a73); - if (res[0]!=0) res[0][6]=a00; - a00=arg[1]? arg[1][7] : 0; - a73=(a02*a09); - a75=(a41*a04); - a73=(a73-a75); - a00=(a00-a73); - if (res[0]!=0) res[0][7]=a00; - a00=arg[1]? arg[1][8] : 0; - a73=cos(a29); - a75=(a41/a73); - a76=(a75*a09); - a77=(a02/a73); - a78=(a77*a04); - a76=(a76+a78); - a00=(a00-a76); - if (res[0]!=0) res[0][8]=a00; - a00=casadi_sign(a12); - a00=(a12*a00); - a00=(a00+a07); - a07=(a01*a00); - a76=(a14*a15); - a07=(a07+a76); - a76=(a21*a22); - a07=(a07+a76); - if (res[1]!=0) res[1][0]=a07; - a07=(a13*a00); - a76=(a47*a15); - a07=(a07+a76); - a76=(a48*a22); - a07=(a07+a76); - if (res[1]!=0) res[1][1]=a07; - a07=(a44*a00); - a76=(a53*a15); - a07=(a07+a76); - a76=(a54*a22); - a07=(a07+a76); - if (res[1]!=0) res[1][2]=a07; - a07=(a52*a00); - a76=(a59*a15); - a07=(a07+a76); - a76=(a60*a22); - a07=(a07+a76); - if (res[1]!=0) res[1][3]=a07; - a07=(a58*a00); - a76=(a65*a15); - a07=(a07+a76); - a76=(a66*a22); - a07=(a07+a76); - if (res[1]!=0) res[1][4]=a07; - a00=(a64*a00); - a15=(a70*a15); - a00=(a00+a15); - a22=(a16*a22); - a00=(a00+a22); - if (res[1]!=0) res[1][5]=a00; - a00=(a01*a05); - a22=casadi_sign(a06); - a22=(a06*a22); - a22=(a22+a20); - a20=(a14*a22); - a00=(a00+a20); - a20=(a21*a24); - a00=(a00+a20); - if (res[1]!=0) res[1][6]=a00; - a00=(a13*a05); - a20=(a47*a22); - a00=(a00+a20); - a20=(a48*a24); - a00=(a00+a20); - if (res[1]!=0) res[1][7]=a00; - a00=(a44*a05); - a20=(a53*a22); - a00=(a00+a20); - a20=(a54*a24); - a00=(a00+a20); - if (res[1]!=0) res[1][8]=a00; - a00=(a52*a05); - a20=(a59*a22); - a00=(a00+a20); - a20=(a60*a24); - a00=(a00+a20); - if (res[1]!=0) res[1][9]=a00; - a00=(a58*a05); - a20=(a65*a22); - a00=(a00+a20); - a20=(a66*a24); - a00=(a00+a20); - if (res[1]!=0) res[1][10]=a00; - a05=(a64*a05); - a22=(a70*a22); - a05=(a05+a22); - a24=(a16*a24); - a05=(a05+a24); - if (res[1]!=0) res[1][11]=a05; - a05=(a01*a10); - a24=(a14*a18); - a05=(a05+a24); - a24=casadi_sign(a11); - a24=(a11*a24); - a24=(a24+a25); - a25=(a21*a24); - a05=(a05+a25); - if (res[1]!=0) res[1][12]=a05; - a05=(a13*a10); - a25=(a47*a18); - a05=(a05+a25); - a25=(a48*a24); - a05=(a05+a25); - if (res[1]!=0) res[1][13]=a05; - a05=(a44*a10); - a25=(a53*a18); - a05=(a05+a25); - a25=(a54*a24); - a05=(a05+a25); - if (res[1]!=0) res[1][14]=a05; - a05=(a52*a10); - a25=(a59*a18); - a05=(a05+a25); - a25=(a60*a24); - a05=(a05+a25); - if (res[1]!=0) res[1][15]=a05; - a05=(a58*a10); - a25=(a65*a18); - a05=(a05+a25); - a25=(a66*a24); - a05=(a05+a25); - if (res[1]!=0) res[1][16]=a05; - a10=(a64*a10); - a18=(a70*a18); - a10=(a10+a18); - a24=(a16*a24); - a10=(a10+a24); - if (res[1]!=0) res[1][17]=a10; - a10=(a03*a11); - a24=(a14*a10); - a18=(a08*a06); - a05=(a21*a18); - a24=(a24+a05); - a05=casadi_sign(a17); - a05=(a17*a05); - a05=(a05+a32); - a32=(a26*a05); - a24=(a24+a32); - a38=(a38*a04); - a36=(a36+a38); - a38=(a33*a36); - a24=(a24+a38); - a45=(a45*a09); - a43=(a43+a45); - a45=(a40*a43); - a24=(a24+a45); - if (res[1]!=0) res[1][18]=a24; - a24=(a47*a10); - a45=(a48*a18); - a24=(a24+a45); - a45=(a49*a05); - a24=(a24+a45); - a45=(a50*a36); - a24=(a24+a45); - a45=(a51*a43); - a24=(a24+a45); - if (res[1]!=0) res[1][19]=a24; - a24=(a53*a10); - a45=(a54*a18); - a24=(a24+a45); - a45=(a55*a05); - a24=(a24+a45); - a45=(a56*a36); - a24=(a24+a45); - a45=(a57*a43); - a24=(a24+a45); - if (res[1]!=0) res[1][20]=a24; - a24=(a59*a10); - a45=(a60*a18); - a24=(a24+a45); - a45=(a61*a05); - a24=(a24+a45); - a45=(a62*a36); - a24=(a24+a45); - a45=(a63*a43); - a24=(a24+a45); - if (res[1]!=0) res[1][21]=a24; - a24=(a65*a10); - a45=(a66*a18); - a24=(a24+a45); - a45=(a67*a05); - a24=(a24+a45); - a45=(a68*a36); - a24=(a24+a45); - a45=(a69*a43); - a24=(a24+a45); - if (res[1]!=0) res[1][22]=a24; - a10=(a70*a10); - a18=(a16*a18); - a10=(a10+a18); - a05=(a23*a05); - a10=(a10+a05); - a36=(a69*a36); - a10=(a10+a36); - a43=(a34*a43); - a10=(a10+a43); - if (res[1]!=0) res[1][23]=a10; - a10=-1.; - if (res[1]!=0) res[1][24]=a10; - a11=(a08*a11); - a10=(a01*a11); - a43=(a03*a12); - a21=(a21*a43); - a10=(a10+a21); - a30=(a30*a04); - a28=(a28+a30); - a30=(a26*a28); - a10=(a10+a30); - a30=casadi_sign(a09); - a30=(a09*a30); - a30=(a30+a37); - a37=(a33*a30); - a10=(a10+a37); - a42=(a42*a17); - a42=(a42+a46); - a46=(a40*a42); - a10=(a10+a46); - if (res[1]!=0) res[1][25]=a10; - a10=(a13*a11); - a48=(a48*a43); - a10=(a10+a48); - a48=(a49*a28); - a10=(a10+a48); - a48=(a50*a30); - a10=(a10+a48); - a48=(a51*a42); - a10=(a10+a48); - if (res[1]!=0) res[1][26]=a10; - a10=(a44*a11); - a54=(a54*a43); - a10=(a10+a54); - a54=(a55*a28); - a10=(a10+a54); - a54=(a56*a30); - a10=(a10+a54); - a54=(a57*a42); - a10=(a10+a54); - if (res[1]!=0) res[1][27]=a10; - a10=(a52*a11); - a60=(a60*a43); - a10=(a10+a60); - a60=(a61*a28); - a10=(a10+a60); - a60=(a62*a30); - a10=(a10+a60); - a60=(a63*a42); - a10=(a10+a60); - if (res[1]!=0) res[1][28]=a10; - a10=(a58*a11); - a66=(a66*a43); - a10=(a10+a66); - a66=(a67*a28); - a10=(a10+a66); - a66=(a68*a30); - a10=(a10+a66); - a66=(a69*a42); - a10=(a10+a66); - if (res[1]!=0) res[1][29]=a10; - a11=(a64*a11); - a16=(a16*a43); - a11=(a11+a16); - a28=(a23*a28); - a11=(a11+a28); - a30=(a69*a30); - a11=(a11+a30); - a42=(a34*a42); - a11=(a11+a42); - if (res[1]!=0) res[1][30]=a11; - a72=(-a72); - if (res[1]!=0) res[1][31]=a72; - a72=(-a02); - if (res[1]!=0) res[1][32]=a72; - a72=(-a75); - if (res[1]!=0) res[1][33]=a72; - a03=(a03*a06); - a01=(a01*a03); - a08=(a08*a12); - a14=(a14*a08); - a01=(a01+a14); - a27=(a27*a09); - a27=(a27+a31); - a26=(a26*a27); - a01=(a01+a26); - a35=(a35*a17); - a35=(a35+a39); - a33=(a33*a35); - a01=(a01+a33); - a33=casadi_sign(a04); - a33=(a04*a33); - a33=(a33+a19); - a40=(a40*a33); - a01=(a01+a40); - if (res[1]!=0) res[1][34]=a01; - a13=(a13*a03); - a47=(a47*a08); - a13=(a13+a47); - a49=(a49*a27); - a13=(a13+a49); - a50=(a50*a35); - a13=(a13+a50); - a51=(a51*a33); - a13=(a13+a51); - if (res[1]!=0) res[1][35]=a13; - a44=(a44*a03); - a53=(a53*a08); - a44=(a44+a53); - a55=(a55*a27); - a44=(a44+a55); - a56=(a56*a35); - a44=(a44+a56); - a57=(a57*a33); - a44=(a44+a57); - if (res[1]!=0) res[1][36]=a44; - a52=(a52*a03); - a59=(a59*a08); - a52=(a52+a59); - a61=(a61*a27); - a52=(a52+a61); - a62=(a62*a35); - a52=(a52+a62); - a63=(a63*a33); - a52=(a52+a63); - if (res[1]!=0) res[1][37]=a52; - a58=(a58*a03); - a65=(a65*a08); - a58=(a58+a65); - a67=(a67*a27); - a58=(a58+a67); - a68=(a68*a35); - a58=(a58+a68); - a68=(a69*a33); - a58=(a58+a68); - if (res[1]!=0) res[1][38]=a58; - a64=(a64*a03); - a70=(a70*a08); - a64=(a64+a70); - a23=(a23*a27); - a64=(a64+a23); - a69=(a69*a35); - a64=(a64+a69); - a34=(a34*a33); - a64=(a64+a34); - if (res[1]!=0) res[1][39]=a64; - a74=(-a74); - if (res[1]!=0) res[1][40]=a74; - if (res[1]!=0) res[1][41]=a41; - a74=(-a77); - if (res[1]!=0) res[1][42]=a74; - a74=(a71*a02); - a74=(a09*a74); - a71=(a71*a41); - a71=(a04*a71); - a74=(a74-a71); - a74=(-a74); - if (res[1]!=0) res[1][43]=a74; - a74=(a09*a41); - a71=(a04*a02); - a74=(a74+a71); - if (res[1]!=0) res[1][44]=a74; - a74=(a09*a77); - a71=(a04*a75); - a74=(a74-a71); - a74=(-a74); - if (res[1]!=0) res[1][45]=a74; - a74=casadi_sq(a73); - a41=(a41/a74); - a41=(a09*a41); - a02=(a02/a74); - a02=(a04*a02); - a41=(a41+a02); - a41=(-a41); - if (res[1]!=0) res[1][46]=a41; - a75=(a75/a73); - a29=sin(a29); - a75=(a75*a29); - a09=(a09*a75); - a77=(a77/a73); - a77=(a77*a29); - a04=(a04*a77); - a09=(a09+a04); - a09=(-a09); - if (res[1]!=0) res[1][47]=a09; - a09=1.; - if (res[2]!=0) res[2][0]=a09; - if (res[2]!=0) res[2][1]=a09; - if (res[2]!=0) res[2][2]=a09; - if (res[2]!=0) res[2][3]=a09; - if (res[2]!=0) res[2][4]=a09; - if (res[2]!=0) res[2][5]=a09; - if (res[2]!=0) res[2][6]=a09; - if (res[2]!=0) res[2][7]=a09; - if (res[2]!=0) res[2][8]=a09; - a09=-3.3453634479321918e-02; - if (res[3]!=0) res[3][0]=a09; - a09=-6.0368975005218423e-05; - if (res[3]!=0) res[3][1]=a09; - a09=1.1116270017027936e-07; - if (res[3]!=0) res[3][2]=a09; - a09=-9.8084735444364535e-08; - if (res[3]!=0) res[3][3]=a09; - a09=-1.0920100546139210e-05; - if (res[3]!=0) res[3][4]=a09; - a09=6.0150572994295635e-03; - if (res[3]!=0) res[3][5]=a09; - a09=-1.0920100546139184e-05; - if (res[3]!=0) res[3][6]=a09; - a09=-6.0570157695918683e-03; - if (res[3]!=0) res[3][7]=a09; - a09=3.0801331505514837e-03; - if (res[3]!=0) res[3][8]=a09; - a09=-2.7177645446042520e-03; - if (res[3]!=0) res[3][9]=a09; - a09=-3.0257778596593993e-01; - if (res[3]!=0) res[3][10]=a09; - a09=5.4600502730695923e-04; - if (res[3]!=0) res[3][11]=a09; - a04=6.0150572994295574e-03; - if (res[3]!=0) res[3][12]=a04; - a04=3.0184487502609172e-03; - if (res[3]!=0) res[3][13]=a04; - a04=-5.5581350085139543e-06; - if (res[3]!=0) res[3][14]=a04; - a04=4.9042367722181902e-06; - if (res[3]!=0) res[3][15]=a04; - if (res[3]!=0) res[3][16]=a09; - a09=-3.0075286497147785e-01; - if (res[3]!=0) res[3][17]=a09; - return 0; -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_u_z(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ - return casadi_f0(arg, res, iw, w, mem); -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_u_z_alloc_mem(void) { - return 0; -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_u_z_init_mem(int mem) { - return 0; -} - -CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_jac_x_xdot_u_z_free_mem(int mem) { -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_u_z_checkout(void) { - return 0; -} - -CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_jac_x_xdot_u_z_release(int mem) { -} - -CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_jac_x_xdot_u_z_incref(void) { -} - -CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_jac_x_xdot_u_z_decref(void) { -} - -CASADI_SYMBOL_EXPORT casadi_int auv_model_impl_dae_fun_jac_x_xdot_u_z_n_in(void) { return 6;} - -CASADI_SYMBOL_EXPORT casadi_int auv_model_impl_dae_fun_jac_x_xdot_u_z_n_out(void) { return 5;} - -CASADI_SYMBOL_EXPORT casadi_real auv_model_impl_dae_fun_jac_x_xdot_u_z_default_in(casadi_int i) { - switch (i) { - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const char* auv_model_impl_dae_fun_jac_x_xdot_u_z_name_in(casadi_int i) { - switch (i) { - case 0: return "i0"; - case 1: return "i1"; - case 2: return "i2"; - case 3: return "i3"; - case 4: return "i4"; - case 5: return "i5"; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const char* auv_model_impl_dae_fun_jac_x_xdot_u_z_name_out(casadi_int i) { - switch (i) { - case 0: return "o0"; - case 1: return "o1"; - case 2: return "o2"; - case 3: return "o3"; - case 4: return "o4"; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const casadi_int* auv_model_impl_dae_fun_jac_x_xdot_u_z_sparsity_in(casadi_int i) { - switch (i) { - case 0: return casadi_s0; - case 1: return casadi_s0; - case 2: return casadi_s1; - case 3: return casadi_s2; - case 4: return casadi_s3; - case 5: return casadi_s2; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const casadi_int* auv_model_impl_dae_fun_jac_x_xdot_u_z_sparsity_out(casadi_int i) { - switch (i) { - case 0: return casadi_s0; - case 1: return casadi_s4; - case 2: return casadi_s5; - case 3: return casadi_s6; - case 4: return casadi_s7; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_u_z_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { - if (sz_arg) *sz_arg = 6; - if (sz_res) *sz_res = 5; - if (sz_iw) *sz_iw = 0; - if (sz_w) *sz_w = 0; - return 0; -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_u_z_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { - if (sz_arg) *sz_arg = 6*sizeof(const casadi_real*); - if (sz_res) *sz_res = 5*sizeof(casadi_real*); - if (sz_iw) *sz_iw = 0*sizeof(casadi_int); - if (sz_w) *sz_w = 0*sizeof(casadi_real); - return 0; -} - - -#ifdef __cplusplus -} /* extern "C" */ -#endif diff --git a/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u_z.o b/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u_z.o deleted file mode 100644 index dc3e918bb13381d14374a20d16c91be469187efa..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 21944 zcmchf3wTwt>2LXIlvAO@XW=*m(a(crp z?R?+fd*(N5)~s1Gdmj4)zExCw`62L&?=N@qsy`ShB|Jx3#VyxjsbBa3GwMHY3%PlK93-Ism0?~g1l z@)u>iGYGx`=z;j!-c9wW#+EeP!c?H)eZT_2m)+C zR%vhy?j6A-)Q|j1Kbras7YQI4KUmPGqvh{WE?``7%md}2-mA57(yMs39Ey>DBzpW z5ey(rGBBhtmo8lO1+QOh8HZKg8K+`6eRTvB* z_9izfVk#g5U`*;>s=OUas2V@=xZ*KI8GVE5L2xV$nWd+ZWGdH!Otz zF}~hF`G?rg@Zr7eE0B0FfEK<$ddj)cj|53Ka+U{|8NGgLf9w?!DTU#96bD<17te)@ z%3p)74Y*#d^PRO1&z&e(L&GxmJg&FVnjIjI@UUPdzUZGJ_8eewunB|;bckzX`;mx~ zW1~AD^zxAcU+hmvP$kb%Vh7zYRz)c}naGRC|D>+Km$n;O9~0RF@;pVh6UN9ci({N4 zjhC$aMe+t-hVsK~MHn$-B)OY}P-pp8@9bemZ$z*r*DAfC}PpT{QrL8?)&1N{9 z)K+qh7{{M&W;wqRC$X~FznJckW@1DhuTtQC=i0X&{TbuAX>gJ9jo|3pn7qVVUHJxZ z^mFVoQ|X|*P2hw3ywDeWiXqtGo~ma`Dv5Lkx! zCrq=`UlE-XjMxIDLA(`y0GQc>Ya^tw7-ACKg%)DE{gejmQ#9wc0@z|)0PjB)HWr{v zVK92ccf$c(d%2QI+-rgnXk5;P5&0C?AAy$#OJFso5=@p@x2Orod6Orz@1P#25R)}d$&ZHdzC`y$x>0lkd9Ik2I7~ONDz=T>$#20-@*X)8zA5j9Kq0qw zqbWfSD-Oa`DiQ4_!6KBmU4ba%)41J{#nWl!_qDBO3uC<8rSb5C1*`26dKPj$=rT(k zH4E*;6~lU)+J$EiE*?L1SdWqa5U-YiTvDLKV?DlDbtHubQxbt1nUas zds?88A$CQv_W=wz(3cVpaMQ*NZrT&JX=6}qpe4%~qpN9$fgFB+@$BH$1?|G-E4JIT zkG2fd#?dhAm6w*#l0P66-ANfx5JrnixJ72I z@9(U~jp+{sWglVt%j{9CS?GW463~F{+;g-8KqXjjY_CPPSW&QDs8pw4g!3zrFS>0< zQrUhFXnB;kJsRDDvP~!pS?XD8OlzyLxrUZ(jLtEzj(3L`iZt-=m5m_YE(l=Q4|JlH z$GaKeu^Kvs-=-XB#Gvfo>9C^y84=63KNaf_#w;(jz8Kc37_STE<+Mzj7U8uhrL{at zJJ?A^V+`+m^j)N-hS*Uj|Ibc7G^Z?UA8ZY6AO3Eg5B~#nE;%VWQ6s|jyXY=l z*c+v^oBqfCFAbVX=}3a{MWd`ExWp9a-7HQd{SyaN@&N@S)r`3x(JPAG25rTBu@OPo z_9BG(HE^?0VOy{VDrvt(11Qd88`C~Eac@)cXX!@9Eey_viUYXJbWrF^U58Ssm~dh; zsieLG5bMGVj1?uuenx5QvhsMa1+Uh6Xt44hpe#*9ipBoqq+gcwOM@!I``Jxq`_k5# z39tWE#%nDWVwr+|ZYOkTjs-Txua3i?0ykzGdD4yjw$bfNd6<{u0$&tOj6!!5dZJIk z6zT;yPv%KlU&`8ot$@h;$mC(YLj%gN6G>^66Qns_lzwLV;3?x5m@>%qd}1`y4>~wN z=YdOu(3D1JRCBTsi_88(nR9Tgz=KCKmEUA!fp}h|Jn?=Rwiq(`px1igwnNC}!wD_J z=Dj#l#9Qtdoy}P%M4~us$V`KRH1Ny$a4`-oB^c)bf5adI0K-^9gOHde_5ntW_AF-Q)$+ zGvFJ5qJvQvBVWqha*nli;nWcw@PjY*3DSRsm&Dz%_h2{e1hy&$FGjaQ6|MBvY>XfF zj2%bd3wf`D3e1}W#n0lxLYGQMopRc)MV~>egExTWKD-raJ7H)Uj(t(uq0;vSYz1fs zN2zBqtjwMRyD%CUVJO=G7-mb#pF{a=RQ?!AZzC5Sqa;>tdV|tlfOroIw(&L)QxGer zqZhWmw=BHNkf5Q2g}eV0>h;^t%~Ik$WQK!7g74-r=yHc51jn z5FbP04X}d86!Hh6VZVw8R6L~OP-}~ZOH{m4#hV7=ybX!mv4OsNWprJ4eL;D_^#wN+ zRM_uiXw%DeMZQB_G|Zo-j_Sn@0qcMNzG4@;FeT1k8+ho#J$Q}up@|M!gVIAeM8Etu z>Y_e9iFNf)3xBIljGF#B&8)-iw9|!d-$^`N(w7&!OPI@W3c}Q~8-0=1jnrCTygj&e zVJhO~8xJ{F;zK2UeWEvI(XzvRSS`)RDtzoj&-02E*@P8?Ce+9#bMi9r!eE#p_9%!q zrH1Ab+@lA_9O>(;^PSg50{EV4zB!_vF0C^_nJ6$K$X_Gr`&};xJ@;~sv4koxlNk0K z=8NV)22C_vXNff&sX~5l&6`C!dkFsq%uV9`l%(a(DO`VM~hS2nefnuoU* z6?-reYn}a`h!jtnlx|mcv06Gr9Q+Q{;=42X5yDs`%FPsjnFPBX8uO*WT-aC2b8RV& z|0pEN>9Du<;8J+Kt}fkrtFE2CKWB7dYUuahXVdFdiQ~z;`TtjX)hM)#s)Qd%-zpln zDouT7-*d{pvv1@rjgk9!9lob8Qa$2{$grNgzPj1w_bB=_q8QyY z*Xz+^#;24Iu_TvI<7k5N^&zi5!>iBq>a)E1Y_C4YYYcgf8D3+i*O=usW>Yo)^l+Ng z$c_9PwUgX%$tQ=Mj_V9bQ(5vC<&VI{BOlKb;iO;N;_9P1PsNpV0&)=T4_d#r{|k^f zAJ@gW(s5B|(naA!>_1U?vLR6!-PWPVXnBg0k8IHWN^zT}zS3#(k52cQ@GYyydA3U0 zKAr5|{1xXud&&2I?#uslOMCuX>`N27FAzJ2huQv4X>Xow?=7aCo!fVB zANTyx5A$QDJ)H+bTb^j2UYGx{wCnQ^Z@c!j2X?NJ_I679`~6|<-6!SF7Wu4{ty3DW z-;rO^KJn$;eFdHN*}QS<;k)y9*#7F2_PF+~5r40-^-25INc%d)-h8pQQ|hzzSyGBfRR|MQ>fTYIOMib?>K@-#@x*Jw4XF zkuT3q;a0B8n=yU*mjY=sXU%Vj&ktl|rDvpvCgslOWXA28ndzbI34*yQn&14mXpWol zwV5>UbrG|3BYwZ-FzmLgyVP~IyYEnC1EGl!38Vux3Ky(;*B`t&qwBAy<8W-`f!>Rqm>C-j^E8l@Q z@e+qWD?BW1>!m^nAK)T+>nE${JjfB>OF{^x=HXF45pR`rr95wDy-8lKr0wGy!b29w zbAj-H!&?*ZMZ!xQ`NawN1H!|O{0|cF9}BmBvBj1q;J>tb_-Um*Z$$$BTj3>+{F4dz zTH%$ByxE;5c`koek*^oN(hAD6F#(@1e2F8! zAOXKaxZkPwt_1u(;XRK0g9-Q#h3|Fbf0}^*On9#&|LX+&G2yG6_WoY@0f+xV_+E#v z7vAmgr-gSpe4FrHj-7uL{*lA?2yb%us|omBj@9;B)k8$`Hga;gcp71n>j}snp_{G9=9iG7)^R~mMhBR^B*OC0&@M4k`$Dz8eopZjMZ<3`~DhtCym_b0YQ zvv9kgvHVuycHd(8w}e+Z`WFdra`&;Jzevp}A| z6z=ZqRtR@T?r(+L{gzd?S~!1Xv=Gnr!tFO0o3&Z^xfaOtIpOX&-zEHPNB$M?Gt^>X z&n%-y7;gP$&n-E^?f9|nyFqxR=;!kb;%(qohpu{Y0)AHlet!b~AUN6IL+2#~d(K%e ze6Pcg2=5hc&n#mRVLyC+u$~g(e&JUBcHses|5|v+;k$%~h1>Jaa10!(x5VL>2(J`w z{ah)$Nw_@^eOGv|aC;{DgViGgB5IoF9TMIp+{&L12a-J@`e24&&qr4XZ*sU-HUAqG za~o@F=Ty|sZJJY2Q&n40H@~6c>s8eiEfp;_jq!^46}P2V4^6J3IW$zk%%KWq4OK8> zsDjx;6`V3u!JMHAPWIA^r)Sb_3f(xFL$M6L=VWl);1I`+oZ-1#hVL0uInMV?mYl-( zOqR^#Dl)T3CNrDwtUuG#&pLAqXKgt~58rcGGRx>>Jz1=C3Tw+U#&g*$LpYve;#_vJ zDaCbVaa~zlOP0~kB%Af;uuL}VpUfJvjeaA;WD1wf=31t(hHS1So6AmSJ=vy3M$+6_ zGS_Hh{kg0y*XUslxqN2@MuHs0r@+7l3Ns2H-p8!Cq0yqem&s`+!` zC!oA)ZtV$558ayPrmA_(_3>MWUO|6}p;vNC=Ev55^UL zOwJ)Be~sc_Ab@~vnWyE=cNkMIeS@_6a}vm3CY&BKmAv_W@>xCCDEVuZd`klTtx8_o z)1i2o((^iQ*}WZ&W;_ z_=}2172mJ;1&Y6~_!PyH;uG_m?@o~z|R~6TOct`Oj-L4YFwI3EJPTzX0 zpBF2x?f;SDy1h#k*Y>YaT(|d0#WPjCn-u?d#Zy0r1cKY%9O2|2y^dU_xL%LGs<>Vs z=PMqAF6)Q86xZv~4-_Ax
KUXOmSxL#kjD6ZG3J&Nmf=Pktps@_i&*Xz#bPw}AQmnfd5c)sEnDSoBm6BVyeT(1|+itBafPQ~>)@}S~+9a*ZlUPqo(T+iq0 z70*?6Zc#j}_;$thzVsc%_5A-A#b+ozBjp1o`CrffV-=4m`ALfF^{YT}y&l~y+Ku{o!8;A&mM>8J zD~f+laXoMRMDYnq{#S}$rugp^zgY2gisvi7Rq;uRzo>YD;_oVch2m$?;RJ#FOd__u z*9dp}@m?jb_s>fd*Yn$N6xZ{~YQ@u4z1@n>RQz?tzpD6qihoV_Y~Lj?Fz;9xLv2X9=CfG*YoFF zitG996UFs7#6KwEK>pO@ZM@=o{z+F{kK4(L>+u;;T+df$&|pHKarg~fw!f+sU!eGM z;qG|ep*T&AR?nUUd^l}N5J*o2E-Rm`c%|a>zidz-`6^sie!Swd6wg$=TJb!^YZU*A z;wUsgitBZDyW;x2drfiu9?}0aM1lOQ*ZosylZ`+;jLZ7-D#i8tQm!~H-B!Lsahj4X zU#a+P#rG(#*PZtize>rErb!cl?7SM6)o*`CC9d^1D|xMdA#DN>NPajjtLH(*>lI(B z_}3Nxjp8>czFzS;itkW-uHvsN-k|s~#mP?TKJQH0+#`_vjkv7+7bxDOc$VTfD?VNE zd5T}7c(dYj6eoM=ou_c8!^_Dqga;jdsqm!^pD6q{4lfq|jKjmM-Sb{|_@%>4#!-h` zdxq0ui9l+}{!&UHoaykF4E8Q?_(I|KL3-#~BK<_`69vm35}ssmKkpV!?-T_q|B`T7 ze*3xoz1sG_mER!p(%gRefPiz1`hK1&JmBz%aGLWeSpDU~Z9iKc7EW_G1DZS@|yE8ytE2cO3Tb0G)Zhg$(P@ zt8R{G;Fn~PL<&DVS2xa`TiXCt4UO^Ibo`TwNzL)9>YF5eW5fLPrg@D`we#Y)iq6{E z6?OCQyPkC5-}wljJh^6+qbDrsr83@awjIwlO=k|81H%@`vxGKF2qm%})82sUNg{|K z-NN63h|}Dx!|P7HHcz_ICOw*uNaz2zJnbb(XC^KkZ2e<3MBLVIpR_llIE^120`$=h zVUCj#U^;LMBYHglEs=@J{(T|&kNU{ye1Hz0 z2vPIszuWrlb2f6Qe5hZ-L|UXg^^XoDOTj*AZ(`$|A-4`GuNlQ%**m~a#pN{9TPEcL z$kV~vZP^bIA)RjdZYh7jsn9L|5Xv5}|5kp@)Ua2|Q~%Mv*_OBC|9Ev%$(3X2?s)+! ugh2fj$K{r%&-L!u2l9H->F#;AlXFyXnuGs2_4wu6OH84qsw_p_^8X71$7nqO diff --git a/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_z.c b/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_z.c deleted file mode 100644 index 7cbf6b868..000000000 --- a/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_z.c +++ /dev/null @@ -1,809 +0,0 @@ -/* This file was automatically generated by CasADi 3.7.2. - * It consists of: - * 1) content generated by CasADi runtime: not copyrighted - * 2) template code copied from CasADi source: permissively licensed (MIT-0) - * 3) user code: owned by the user - * - */ -#ifdef __cplusplus -extern "C" { -#endif - -/* How to prefix internal symbols */ -#ifdef CASADI_CODEGEN_PREFIX - #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID) - #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID - #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID) -#else - #define CASADI_PREFIX(ID) auv_model_impl_dae_fun_jac_x_xdot_z_ ## ID -#endif - -#include - -#ifndef casadi_real -#define casadi_real double -#endif - -#ifndef casadi_int -#define casadi_int int -#endif - -/* Add prefix to internal symbols */ -#define casadi_f0 CASADI_PREFIX(f0) -#define casadi_fabs CASADI_PREFIX(fabs) -#define casadi_s0 CASADI_PREFIX(s0) -#define casadi_s1 CASADI_PREFIX(s1) -#define casadi_s2 CASADI_PREFIX(s2) -#define casadi_s3 CASADI_PREFIX(s3) -#define casadi_s4 CASADI_PREFIX(s4) -#define casadi_s5 CASADI_PREFIX(s5) -#define casadi_s6 CASADI_PREFIX(s6) -#define casadi_sign CASADI_PREFIX(sign) -#define casadi_sq CASADI_PREFIX(sq) - -/* Symbol visibility in DLLs */ -#ifndef CASADI_SYMBOL_EXPORT - #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) - #if defined(STATIC_LINKED) - #define CASADI_SYMBOL_EXPORT - #else - #define CASADI_SYMBOL_EXPORT __declspec(dllexport) - #endif - #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) - #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default"))) - #else - #define CASADI_SYMBOL_EXPORT - #endif -#endif - -casadi_real casadi_fabs(casadi_real x) { -/* Pre-c99 compatibility */ -#if __STDC_VERSION__ < 199901L - return x>0 ? x : -x; -#else - return fabs(x); -#endif -} - -casadi_real casadi_sign(casadi_real x) { return x<0 ? -1 : x>0 ? 1 : x;} - -casadi_real casadi_sq(casadi_real x) { return x*x;} - -static const casadi_int casadi_s0[3] = {9, 1, 1}; -static const casadi_int casadi_s1[3] = {3, 1, 1}; -static const casadi_int casadi_s2[3] = {0, 1, 1}; -static const casadi_int casadi_s3[3] = {0, 0, 1}; -static const casadi_int casadi_s4[60] = - {9, 9, 0, 6, 12, 18, 25, 34, - 43, 46, 48, 48, 0, 1, 2, 3, - 4, 5, 0, 1, 2, 3, 4, 5, - 0, 1, 2, 3, 4, 5, 0, 1, - 2, 3, 4, 5, 6, 0, 1, 2, - 3, 4, 5, 6, 7, 8, 0, 1, - 2, 3, 4, 5, 6, 7, 8, 6, - 7, 8, 6, 8}; -static const casadi_int casadi_s5[21] = - {9, 9, 0, 1, 2, 3, 4, 5, - 6, 7, 8, 9, 0, 1, 2, 3, - 4, 5, 6, 7, 8}; -static const casadi_int casadi_s6[3] = {9, 0, 1}; - -/* auv_model_impl_dae_fun_jac_x_xdot_z:(i0[9],i1[9],i2[3],i3[0],i4[],i5[0])->(o0[9],o1[9x9,48nz],o2[9x9,9nz],o3[9x0]) */ -static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { - casadi_real a00, a01, a02, a03, a04, a05, a06, a07, a08, a09, a10, a11; - casadi_real a12, a13, a14, a15, a16, a17, a18, a19, a20, a21, a22, a23; - casadi_real a24, a25, a26, a27, a28, a29, a30, a31, a32, a33, a34, a35; - casadi_real a36, a37, a38, a39, a40, a41, a42, a43, a44, a45, a46, a47; - casadi_real a48, a49, a50, a51, a52, a53, a54, a55, a56, a57, a58, a59; - casadi_real a60, a61, a62, a63, a64, a65, a66, a67, a68, a69, a70, a71; - casadi_real a72, a73, a74, a75, a76, a77, a78; - a00=arg[1]? arg[1][0] : 0; - a01=3.3453634479321918e-02; - a02=arg[2]? arg[2][0] : 0; - a03=-30.; - a04=arg[0]? arg[0][5] : 0; - a05=(a03*a04); - a06=arg[0]? arg[0][1] : 0; - a07=(a05*a06); - a08=30.; - a09=arg[0]? arg[0][4] : 0; - a10=(a08*a09); - a11=arg[0]? arg[0][2] : 0; - a12=(a10*a11); - a07=(a07+a12); - a02=(a02-a07); - a07=23.; - a12=arg[0]? arg[0][0] : 0; - a13=casadi_fabs(a12); - a07=(a07+a13); - a13=(a07*a12); - a02=(a02-a13); - a13=(a01*a02); - a14=6.0368975005218254e-05; - a15=(a08*a04); - a16=(a15*a12); - a17=arg[0]? arg[0][3] : 0; - a18=(a03*a17); - a19=(a18*a11); - a16=(a16+a19); - a19=46.; - a20=casadi_fabs(a06); - a20=(a19+a20); - a21=(a20*a06); - a16=(a16+a21); - a21=(a14*a16); - a13=(a13-a21); - a21=-1.1116270017027866e-07; - a22=(a03*a09); - a23=(a22*a12); - a24=(a08*a17); - a25=(a24*a06); - a23=(a23+a25); - a25=casadi_fabs(a11); - a25=(a19+a25); - a26=(a25*a11); - a23=(a23+a26); - a26=(a21*a23); - a13=(a13-a26); - a26=9.8084735444363873e-08; - a27=3.3399999999999999e+00; - a28=(a27*a04); - a29=(a28*a09); - a30=-3.3199999999999998e+00; - a31=(a30*a09); - a32=(a31*a04); - a29=(a29+a32); - a32=casadi_fabs(a17); - a32=(a19+a32); - a33=(a32*a17); - a29=(a29+a33); - a33=(a26*a29); - a13=(a13-a33); - a33=1.0920100546139184e-05; - a34=arg[2]? arg[2][1] : 0; - a35=-3.3399999999999999e+00; - a36=(a35*a04); - a37=(a36*a17); - a38=6.8000000000000005e-01; - a39=(a38*a17); - a40=(a39*a04); - a37=(a37+a40); - a34=(a34-a37); - a37=casadi_fabs(a09); - a37=(a19+a37); - a40=(a37*a09); - a34=(a34-a40); - a40=(a33*a34); - a13=(a13+a40); - a40=-6.0150572994295574e-03; - a41=arg[2]? arg[2][2] : 0; - a42=3.3199999999999998e+00; - a43=(a42*a09); - a44=(a43*a17); - a45=-6.8000000000000005e-01; - a46=(a45*a17); - a47=(a46*a09); - a44=(a44+a47); - a41=(a41-a44); - a44=casadi_fabs(a04); - a19=(a19+a44); - a44=(a19*a04); - a41=(a41-a44); - a44=(a40*a41); - a13=(a13+a44); - a00=(a00-a13); - if (res[0]!=0) res[0][0]=a00; - a00=arg[1]? arg[1][1] : 0; - a13=6.0368975005218423e-05; - a44=(a13*a02); - a47=3.3484658136227786e-02; - a48=(a47*a16); - a44=(a44-a48); - a48=-6.1658244361114702e-05; - a49=(a48*a23); - a44=(a44-a49); - a49=5.4404333259807184e-05; - a50=(a49*a29); - a44=(a44-a50); - a50=6.0570157695918683e-03; - a51=(a50*a34); - a44=(a44+a51); - a51=-3.0184487502609172e-03; - a52=(a51*a41); - a44=(a44+a52); - a00=(a00-a44); - if (res[0]!=0) res[0][1]=a00; - a00=arg[1]? arg[1][2] : 0; - a44=-1.1116270017027936e-07; - a52=(a44*a02); - a53=-6.1658244361114783e-05; - a54=(a53*a16); - a52=(a52-a54); - a54=3.3963490377380855e-02; - a55=(a54*a23); - a52=(a52-a55); - a55=-2.9967785627100754e-02; - a56=(a55*a29); - a52=(a52-a56); - a56=-3.0801331505514837e-03; - a57=(a56*a34); - a52=(a52+a57); - a57=5.5581350085139543e-06; - a58=(a57*a41); - a52=(a52+a58); - a00=(a00-a52); - if (res[0]!=0) res[0][2]=a00; - a00=arg[1]? arg[1][3] : 0; - a52=9.8084735444364535e-08; - a58=(a52*a02); - a59=5.4404333259807062e-05; - a60=(a59*a16); - a58=(a58-a60); - a60=-2.9967785627100469e-02; - a61=(a60*a23); - a58=(a58-a61); - a61=1.4970303990827358e+00; - a62=(a61*a29); - a58=(a58-a62); - a62=2.7177645446042520e-03; - a63=(a62*a34); - a58=(a58+a63); - a63=-4.9042367722181902e-06; - a64=(a63*a41); - a58=(a58+a64); - a00=(a00-a58); - if (res[0]!=0) res[0][3]=a00; - a00=arg[1]? arg[1][4] : 0; - a58=1.0920100546139210e-05; - a64=(a58*a02); - a65=6.0570157695918657e-03; - a66=(a65*a16); - a64=(a64-a66); - a66=-3.0801331505514781e-03; - a67=(a66*a23); - a64=(a64-a67); - a67=2.7177645446042498e-03; - a68=(a67*a29); - a64=(a64-a68); - a68=3.0257778596593993e-01; - a69=(a68*a34); - a64=(a64+a69); - a69=-5.4600502730695923e-04; - a70=(a69*a41); - a64=(a64+a70); - a00=(a00-a64); - if (res[0]!=0) res[0][4]=a00; - a00=arg[1]? arg[1][5] : 0; - a64=-6.0150572994295635e-03; - a02=(a64*a02); - a70=-3.0184487502609133e-03; - a16=(a70*a16); - a02=(a02-a16); - a16=5.5581350085139340e-06; - a23=(a16*a23); - a02=(a02-a23); - a23=-4.9042367722181935e-06; - a29=(a23*a29); - a02=(a02-a29); - a34=(a69*a34); - a02=(a02+a34); - a34=3.0075286497147785e-01; - a41=(a34*a41); - a02=(a02+a41); - a00=(a00-a02); - if (res[0]!=0) res[0][5]=a00; - a00=arg[1]? arg[1][6] : 0; - a02=arg[0]? arg[0][6] : 0; - a41=sin(a02); - a29=arg[0]? arg[0][7] : 0; - a71=tan(a29); - a72=(a41*a71); - a73=(a72*a09); - a73=(a17+a73); - a02=cos(a02); - a74=(a02*a71); - a75=(a74*a04); - a73=(a73+a75); - a00=(a00-a73); - if (res[0]!=0) res[0][6]=a00; - a00=arg[1]? arg[1][7] : 0; - a73=(a02*a09); - a75=(a41*a04); - a73=(a73-a75); - a00=(a00-a73); - if (res[0]!=0) res[0][7]=a00; - a00=arg[1]? arg[1][8] : 0; - a73=cos(a29); - a75=(a41/a73); - a76=(a75*a09); - a77=(a02/a73); - a78=(a77*a04); - a76=(a76+a78); - a00=(a00-a76); - if (res[0]!=0) res[0][8]=a00; - a00=casadi_sign(a12); - a00=(a12*a00); - a00=(a00+a07); - a07=(a01*a00); - a76=(a14*a15); - a07=(a07+a76); - a76=(a21*a22); - a07=(a07+a76); - if (res[1]!=0) res[1][0]=a07; - a07=(a13*a00); - a76=(a47*a15); - a07=(a07+a76); - a76=(a48*a22); - a07=(a07+a76); - if (res[1]!=0) res[1][1]=a07; - a07=(a44*a00); - a76=(a53*a15); - a07=(a07+a76); - a76=(a54*a22); - a07=(a07+a76); - if (res[1]!=0) res[1][2]=a07; - a07=(a52*a00); - a76=(a59*a15); - a07=(a07+a76); - a76=(a60*a22); - a07=(a07+a76); - if (res[1]!=0) res[1][3]=a07; - a07=(a58*a00); - a76=(a65*a15); - a07=(a07+a76); - a76=(a66*a22); - a07=(a07+a76); - if (res[1]!=0) res[1][4]=a07; - a00=(a64*a00); - a15=(a70*a15); - a00=(a00+a15); - a22=(a16*a22); - a00=(a00+a22); - if (res[1]!=0) res[1][5]=a00; - a00=(a01*a05); - a22=casadi_sign(a06); - a22=(a06*a22); - a22=(a22+a20); - a20=(a14*a22); - a00=(a00+a20); - a20=(a21*a24); - a00=(a00+a20); - if (res[1]!=0) res[1][6]=a00; - a00=(a13*a05); - a20=(a47*a22); - a00=(a00+a20); - a20=(a48*a24); - a00=(a00+a20); - if (res[1]!=0) res[1][7]=a00; - a00=(a44*a05); - a20=(a53*a22); - a00=(a00+a20); - a20=(a54*a24); - a00=(a00+a20); - if (res[1]!=0) res[1][8]=a00; - a00=(a52*a05); - a20=(a59*a22); - a00=(a00+a20); - a20=(a60*a24); - a00=(a00+a20); - if (res[1]!=0) res[1][9]=a00; - a00=(a58*a05); - a20=(a65*a22); - a00=(a00+a20); - a20=(a66*a24); - a00=(a00+a20); - if (res[1]!=0) res[1][10]=a00; - a05=(a64*a05); - a22=(a70*a22); - a05=(a05+a22); - a24=(a16*a24); - a05=(a05+a24); - if (res[1]!=0) res[1][11]=a05; - a05=(a01*a10); - a24=(a14*a18); - a05=(a05+a24); - a24=casadi_sign(a11); - a24=(a11*a24); - a24=(a24+a25); - a25=(a21*a24); - a05=(a05+a25); - if (res[1]!=0) res[1][12]=a05; - a05=(a13*a10); - a25=(a47*a18); - a05=(a05+a25); - a25=(a48*a24); - a05=(a05+a25); - if (res[1]!=0) res[1][13]=a05; - a05=(a44*a10); - a25=(a53*a18); - a05=(a05+a25); - a25=(a54*a24); - a05=(a05+a25); - if (res[1]!=0) res[1][14]=a05; - a05=(a52*a10); - a25=(a59*a18); - a05=(a05+a25); - a25=(a60*a24); - a05=(a05+a25); - if (res[1]!=0) res[1][15]=a05; - a05=(a58*a10); - a25=(a65*a18); - a05=(a05+a25); - a25=(a66*a24); - a05=(a05+a25); - if (res[1]!=0) res[1][16]=a05; - a10=(a64*a10); - a18=(a70*a18); - a10=(a10+a18); - a24=(a16*a24); - a10=(a10+a24); - if (res[1]!=0) res[1][17]=a10; - a10=(a03*a11); - a24=(a14*a10); - a18=(a08*a06); - a05=(a21*a18); - a24=(a24+a05); - a05=casadi_sign(a17); - a05=(a17*a05); - a05=(a05+a32); - a32=(a26*a05); - a24=(a24+a32); - a38=(a38*a04); - a36=(a36+a38); - a38=(a33*a36); - a24=(a24+a38); - a45=(a45*a09); - a43=(a43+a45); - a45=(a40*a43); - a24=(a24+a45); - if (res[1]!=0) res[1][18]=a24; - a24=(a47*a10); - a45=(a48*a18); - a24=(a24+a45); - a45=(a49*a05); - a24=(a24+a45); - a45=(a50*a36); - a24=(a24+a45); - a45=(a51*a43); - a24=(a24+a45); - if (res[1]!=0) res[1][19]=a24; - a24=(a53*a10); - a45=(a54*a18); - a24=(a24+a45); - a45=(a55*a05); - a24=(a24+a45); - a45=(a56*a36); - a24=(a24+a45); - a45=(a57*a43); - a24=(a24+a45); - if (res[1]!=0) res[1][20]=a24; - a24=(a59*a10); - a45=(a60*a18); - a24=(a24+a45); - a45=(a61*a05); - a24=(a24+a45); - a45=(a62*a36); - a24=(a24+a45); - a45=(a63*a43); - a24=(a24+a45); - if (res[1]!=0) res[1][21]=a24; - a24=(a65*a10); - a45=(a66*a18); - a24=(a24+a45); - a45=(a67*a05); - a24=(a24+a45); - a45=(a68*a36); - a24=(a24+a45); - a45=(a69*a43); - a24=(a24+a45); - if (res[1]!=0) res[1][22]=a24; - a10=(a70*a10); - a18=(a16*a18); - a10=(a10+a18); - a05=(a23*a05); - a10=(a10+a05); - a36=(a69*a36); - a10=(a10+a36); - a43=(a34*a43); - a10=(a10+a43); - if (res[1]!=0) res[1][23]=a10; - a10=-1.; - if (res[1]!=0) res[1][24]=a10; - a11=(a08*a11); - a10=(a01*a11); - a43=(a03*a12); - a21=(a21*a43); - a10=(a10+a21); - a30=(a30*a04); - a28=(a28+a30); - a30=(a26*a28); - a10=(a10+a30); - a30=casadi_sign(a09); - a30=(a09*a30); - a30=(a30+a37); - a37=(a33*a30); - a10=(a10+a37); - a42=(a42*a17); - a42=(a42+a46); - a46=(a40*a42); - a10=(a10+a46); - if (res[1]!=0) res[1][25]=a10; - a10=(a13*a11); - a48=(a48*a43); - a10=(a10+a48); - a48=(a49*a28); - a10=(a10+a48); - a48=(a50*a30); - a10=(a10+a48); - a48=(a51*a42); - a10=(a10+a48); - if (res[1]!=0) res[1][26]=a10; - a10=(a44*a11); - a54=(a54*a43); - a10=(a10+a54); - a54=(a55*a28); - a10=(a10+a54); - a54=(a56*a30); - a10=(a10+a54); - a54=(a57*a42); - a10=(a10+a54); - if (res[1]!=0) res[1][27]=a10; - a10=(a52*a11); - a60=(a60*a43); - a10=(a10+a60); - a60=(a61*a28); - a10=(a10+a60); - a60=(a62*a30); - a10=(a10+a60); - a60=(a63*a42); - a10=(a10+a60); - if (res[1]!=0) res[1][28]=a10; - a10=(a58*a11); - a66=(a66*a43); - a10=(a10+a66); - a66=(a67*a28); - a10=(a10+a66); - a66=(a68*a30); - a10=(a10+a66); - a66=(a69*a42); - a10=(a10+a66); - if (res[1]!=0) res[1][29]=a10; - a11=(a64*a11); - a16=(a16*a43); - a11=(a11+a16); - a28=(a23*a28); - a11=(a11+a28); - a30=(a69*a30); - a11=(a11+a30); - a42=(a34*a42); - a11=(a11+a42); - if (res[1]!=0) res[1][30]=a11; - a72=(-a72); - if (res[1]!=0) res[1][31]=a72; - a72=(-a02); - if (res[1]!=0) res[1][32]=a72; - a72=(-a75); - if (res[1]!=0) res[1][33]=a72; - a03=(a03*a06); - a01=(a01*a03); - a08=(a08*a12); - a14=(a14*a08); - a01=(a01+a14); - a27=(a27*a09); - a27=(a27+a31); - a26=(a26*a27); - a01=(a01+a26); - a35=(a35*a17); - a35=(a35+a39); - a33=(a33*a35); - a01=(a01+a33); - a33=casadi_sign(a04); - a33=(a04*a33); - a33=(a33+a19); - a40=(a40*a33); - a01=(a01+a40); - if (res[1]!=0) res[1][34]=a01; - a13=(a13*a03); - a47=(a47*a08); - a13=(a13+a47); - a49=(a49*a27); - a13=(a13+a49); - a50=(a50*a35); - a13=(a13+a50); - a51=(a51*a33); - a13=(a13+a51); - if (res[1]!=0) res[1][35]=a13; - a44=(a44*a03); - a53=(a53*a08); - a44=(a44+a53); - a55=(a55*a27); - a44=(a44+a55); - a56=(a56*a35); - a44=(a44+a56); - a57=(a57*a33); - a44=(a44+a57); - if (res[1]!=0) res[1][36]=a44; - a52=(a52*a03); - a59=(a59*a08); - a52=(a52+a59); - a61=(a61*a27); - a52=(a52+a61); - a62=(a62*a35); - a52=(a52+a62); - a63=(a63*a33); - a52=(a52+a63); - if (res[1]!=0) res[1][37]=a52; - a58=(a58*a03); - a65=(a65*a08); - a58=(a58+a65); - a67=(a67*a27); - a58=(a58+a67); - a68=(a68*a35); - a58=(a58+a68); - a68=(a69*a33); - a58=(a58+a68); - if (res[1]!=0) res[1][38]=a58; - a64=(a64*a03); - a70=(a70*a08); - a64=(a64+a70); - a23=(a23*a27); - a64=(a64+a23); - a69=(a69*a35); - a64=(a64+a69); - a34=(a34*a33); - a64=(a64+a34); - if (res[1]!=0) res[1][39]=a64; - a74=(-a74); - if (res[1]!=0) res[1][40]=a74; - if (res[1]!=0) res[1][41]=a41; - a74=(-a77); - if (res[1]!=0) res[1][42]=a74; - a74=(a71*a02); - a74=(a09*a74); - a71=(a71*a41); - a71=(a04*a71); - a74=(a74-a71); - a74=(-a74); - if (res[1]!=0) res[1][43]=a74; - a74=(a09*a41); - a71=(a04*a02); - a74=(a74+a71); - if (res[1]!=0) res[1][44]=a74; - a74=(a09*a77); - a71=(a04*a75); - a74=(a74-a71); - a74=(-a74); - if (res[1]!=0) res[1][45]=a74; - a74=casadi_sq(a73); - a41=(a41/a74); - a41=(a09*a41); - a02=(a02/a74); - a02=(a04*a02); - a41=(a41+a02); - a41=(-a41); - if (res[1]!=0) res[1][46]=a41; - a75=(a75/a73); - a29=sin(a29); - a75=(a75*a29); - a09=(a09*a75); - a77=(a77/a73); - a77=(a77*a29); - a04=(a04*a77); - a09=(a09+a04); - a09=(-a09); - if (res[1]!=0) res[1][47]=a09; - a09=1.; - if (res[2]!=0) res[2][0]=a09; - if (res[2]!=0) res[2][1]=a09; - if (res[2]!=0) res[2][2]=a09; - if (res[2]!=0) res[2][3]=a09; - if (res[2]!=0) res[2][4]=a09; - if (res[2]!=0) res[2][5]=a09; - if (res[2]!=0) res[2][6]=a09; - if (res[2]!=0) res[2][7]=a09; - if (res[2]!=0) res[2][8]=a09; - return 0; -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_z(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ - return casadi_f0(arg, res, iw, w, mem); -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_z_alloc_mem(void) { - return 0; -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_z_init_mem(int mem) { - return 0; -} - -CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_jac_x_xdot_z_free_mem(int mem) { -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_z_checkout(void) { - return 0; -} - -CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_jac_x_xdot_z_release(int mem) { -} - -CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_jac_x_xdot_z_incref(void) { -} - -CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_jac_x_xdot_z_decref(void) { -} - -CASADI_SYMBOL_EXPORT casadi_int auv_model_impl_dae_fun_jac_x_xdot_z_n_in(void) { return 6;} - -CASADI_SYMBOL_EXPORT casadi_int auv_model_impl_dae_fun_jac_x_xdot_z_n_out(void) { return 4;} - -CASADI_SYMBOL_EXPORT casadi_real auv_model_impl_dae_fun_jac_x_xdot_z_default_in(casadi_int i) { - switch (i) { - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const char* auv_model_impl_dae_fun_jac_x_xdot_z_name_in(casadi_int i) { - switch (i) { - case 0: return "i0"; - case 1: return "i1"; - case 2: return "i2"; - case 3: return "i3"; - case 4: return "i4"; - case 5: return "i5"; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const char* auv_model_impl_dae_fun_jac_x_xdot_z_name_out(casadi_int i) { - switch (i) { - case 0: return "o0"; - case 1: return "o1"; - case 2: return "o2"; - case 3: return "o3"; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const casadi_int* auv_model_impl_dae_fun_jac_x_xdot_z_sparsity_in(casadi_int i) { - switch (i) { - case 0: return casadi_s0; - case 1: return casadi_s0; - case 2: return casadi_s1; - case 3: return casadi_s2; - case 4: return casadi_s3; - case 5: return casadi_s2; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const casadi_int* auv_model_impl_dae_fun_jac_x_xdot_z_sparsity_out(casadi_int i) { - switch (i) { - case 0: return casadi_s0; - case 1: return casadi_s4; - case 2: return casadi_s5; - case 3: return casadi_s6; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_z_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { - if (sz_arg) *sz_arg = 6; - if (sz_res) *sz_res = 4; - if (sz_iw) *sz_iw = 0; - if (sz_w) *sz_w = 0; - return 0; -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_z_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { - if (sz_arg) *sz_arg = 6*sizeof(const casadi_real*); - if (sz_res) *sz_res = 4*sizeof(casadi_real*); - if (sz_iw) *sz_iw = 0*sizeof(casadi_int); - if (sz_w) *sz_w = 0*sizeof(casadi_real); - return 0; -} - - -#ifdef __cplusplus -} /* extern "C" */ -#endif diff --git a/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_z.o b/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_z.o deleted file mode 100644 index 1ffd6fd57b05f22d4aee0d6afbc8d7620d5dfd28..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 20472 zcmcJV4|LSkmB%N75GkAAMA5QUaasoqD(U1OBvr`-5}4L#qXwDLND`8aL?MY}DABSQ zYiPA-jayE&d)SIw+NGrvg$dZ`~Px7=>FrOdyaI#%+Si&nV}W&*jZ5Hi+<5rekioE!dsE| z`yuchKo5x1)iTaQHBO|O@b1XLPtb>-B15v5`7=@bDUbzT{8S;8J<<>KD>Ue3B&z-6 z@!ROHLTxSYAiB_x@{zsBK8n;ve;8&V@{B)|Baxl~C%k>Ysd*C;yU0Wo@}dZIi6)xOO<=M^0oeMt^pN~ugDu5QXki~&Sv{0&vt>a2~{?d6$hQJ@14!#Y8m{G;qm**m5ZoV ztJv3w#QZ*Z^8w>6=&9k+wwF|14%57={ZtD?-Xv5FoAIjjcfcdRi#q2X#4vgvxqBem zNaHZ_8d5JH^`w6tyDD-7t$Bqt(Ga0>095AJfR+9>5GuV^TpKxrM2s98eg#5r43s&M z-y-oAmp}r#L?8K=vJW8Z&(Sg`XFsw|68R0tYZTc{_!ve_%paiiN3iz*Ibts&#wzkJ zWucN9su4$^MM+`ZTmi4MukaeH)R9tku6TnIuTDgdZtoNEgi2AXQ`_Wqw z2tBybzW@_I^x!l8I=rz=Jm9CjlUE8faZxdTj0_@cSqMWOEy6Mzq!sE9h&KA`Y=LS_ z1e)1(FtZoWR!F}OkuLu}cnM4H5gM?+qII_uz!u{IUZ_N2V*$#P`@;jyyc2l(xW167 z3K}*CCZ7si`et< zSPv9#C9hRt{+oI2M6hYrY~~rTpFBs7gkNfUAW-gyRgrC`1Uajcn~oCU9ullTi)=Sg zZYo9UQ=9G<&)Z{$4%?ZUw;vfs3iI+)scF1>krBLK` zS9-sbT@|p^B`s$^NxP}Mw1l?)q)-?qpzvlW2%<$*+*!V%Eqcb7zREA>h-81okz>um zfGDS3a@=n2x%Heyn@)rdL2*>F!-|6KbEOZ!`E|$_-L@mCY@(-K&xgJ=#Lc0*%h`u&%~$T1Nx_0XgLHxf>$iW0|^ z4-g-RnT9dT`>hkfK4s2EHGKXtEyA>@roB9j9X6rqJ*2UqBz8=!nnUujk_^d*=InLd zL#;vmL(2SJIv@Ri(7EQc=*(Ux^T*W-=G+-tDf27>H$;ww?(dFst!|IleQJm|X^*0H z@FR>Mcshd1fbFXxa>vQ*G^$n49X34^rWLgh&mPEhoBqckFbA5dY0zPOVHjqw_OCI; z`813BNMf2nC0|c4Qq7qA9!*i4H0V6$L`WV-Oer1ndnwDz6r8&LKxa*!<%nQdVzHY` z!=cOHi|Wv-cerKPJj?047-1J;N*)0|1x=Bck-$|DbF>0K7@{$M2a<1N>dE30;X$9A zisL@UaWDL*q!|@Xq{oGzco?*krJU>yJ;t^_!w9SqTtAMZi3(+<3)98Pu7Gc&Y~%!4 zY}d48F3jme@e_FdI+HucUSlJlK*st2%hKE~eRt@qG?uO9GD-inFM}2jrAG&kss%kDbV$9q> zQ6_ELc=3Ry^4p9o5O+A`iErlOx-F+M_@y6?I*MHE@c7wNBS{(aM>b7>`Y=b#Jl*ugx)$xS`vqR8ahx<3!bsj34fBP z!3qBwap80sFk=bt9Z<`s>hOC|CnFeWJJq`dhGV-bbH??M7fhdmZv_euMR7(>_Ibjo6MI}T_#EU)FEo;a0a=mv2jMGf??tWEbRL{n zO($0nY__!Gu|jSOoHMVHGHI1V?HG_e&S_rwpy_oyWIS)Fbnax`e4oWt%M2d0k1qV= z<1Lu9*yC5U!FHS|z4Y;5QfQ@&F*A@t_lL-XaVHxyJLsju8lFJto~<1yl^4JHma>|% zTgv8@)!Oe>Q2quMz@u>*Z!eO^`r&u1`iUFQKKL>_#)oP%d(1^yUrw=^=*tXUa)zXb zUTk>Ipw`rNk#kRDU5WYmx9UXqCF(TOpWA8Yt!~e0yyW4N2n}j1D!7qgj_*fb|9#VLff#Qn~1puSs0{lRHirK5=Q=<`tD%=(Lm$nl$48QnHiQiRxNt_3KQ>26Z;Za8h2l3sP{D<~om98;!5L1~>92o1> z{$h-UYQEc5)3^^qqJ}P9n-5woKJD5TDMr%aKsd(Pneed815i}AeE6DAo_LTmE?XmW` z+e`jB-S&pi)=}F=Q?G?qe2nNQ>afs?-H776M9~)!#pt9pTKBdYUs686l3d=$(G=yI z1D@tQPjkMfxxmw0=xHwUv<5t_d7jpMPip}Rp4M=dXk+6Mr?h6&z7cq+#Wd2$38V33 z;z?HyqKTmOldiFN@Dbbd&v-tE=Q2FGc*t4wP$1jfcAPFwHl)&}+nSDymZv!R$Oh|Q zN>kjX$+tR9{?X|{6Yd5cE_4dCZC+Ql{`_}-=#>8Dg_5Ta`5!O+-Ig)Wyy|*VW~z+B;qJZ?gJbF<^OV?^ zBX(abc8&2P4jv)wbJrH@KGKmF*g z8{c|(&vPz+lpYkh^CiC^d*`&)TV5%x>c0Gq;)7+I?7RJioyYDgeWlcNX#Xbjo*tKX z(7dO~dvwMoq9V)p;K&(S zIS-s!mKFH=sPe4hdq;mMYyF53%g-K@RXj5*P?nW5Bg+Tb8Cl*LS(%|vp*YpCPV~_l zMuFHyj+u4MvR+}lyCP0<2Ziafbo-{D<;Ke7*335yD-<@ysNgz6!>}4*)-PlW-6AM_ z)x5ZDmoV}h1(JJInDryECx?-X3$q&x_2(WJ14PV(zre2efP-W00M%kw_otY6ao^3Fzi;sHrp z{d5+k6~0)weO)Eo%3D4|xX%K4&rZQBg;%-cuTR0l!h?6wr#jJHfr zkn^py$MY4d-^G^+uX6FPrQr7ouXD+ND+OOA+>UE&(Z31jyOV`@uMuwNtIhhMmFKsm zc8+9vj9-)dCn8UOiPK?&@J3%!+szbD-I zDJlPl6#PTsc7E9sCsS}fz+t}JX9eXwiVk+*9WFjrc$5r!13x%6;k<`Cf_=Y6nScjE&@#R*(i+2g{aq({okGpuc@O>`(za#vFi?0^m@8bU< zywkvZ#VFbUezqs+G<#NmK)CgfJ+J>%xb>%P$LqrD zL_eR~DUQp6)gf8@8l8fVNWnjqf;-@3f7N+LyFH^{CA`kXZxh}o+@94R5#H&N-zvP@ zCEqW6jf=c~RTK+Q#~(+USy&+S}?I zYCCE>8e3zvcjY#uBURfTNKY(3J+XrH#PZS;D@sqSFg>xUp4`fr`Sdf5emGe~u{{1Q z%Hz1f0gf9v!;85L|K^o&oPYCKavJ~Uvt&M3kzYVE`Gx$;`twcwth2~))>dTn@NW@I z78sqZp@4NxWBCGOypb`v%!^E%`BauE;JOO9t^%&5!02aE$oh*|rjYedWetT!zmZ`w zjms8tErqPVkV{Qv4TYvHM#lVdEz|h7*l6S5V%Ap7dZw|4V%ElL*a9xjyi?-CJ>ED*xbQ{&!6s=#fFqXFB z`bACuC)srEXm6`u+}<3!GhO8g`E)B-+Pe7mwEBYtYvge_U}onpys1#p!Qj zmTy;_zN=gQy5e-UxBQ6W^uIrrpHh5+;$v}sq(JSx2#=M&Kyjbq<%(ad_*}(5tN5*o zPgH!V;`9yJ*82^`CoBGl;xq@X{EroIU_(8?<6{qtx1@cb;9;<&O z&czgn7vizptN1L%Cn-Kn@e;+06faY}Oz~?J4=Nr}obGJao@T{oD!xSVD->U$c!lB* zD^CBiXZ8O;ar)zx<>TZ+M#FBA;uVUwD85YbR>j+7AtpVyDZWxT`Je8>w%*l>)BV%( z&5D0s@z)fetN2@re@^jZiceL1j9k>no=J*dta!2F#fq0JezkCSd!tHTx3^Pq-QFK5 zuI>4$;<~*rD6ZT4s^WfS=Wi9)emJc73?+YtTzttty1f?)cenQnC9m66skrvTQpM?S zS+?K5p}4mH5yf?TA5&c0|AgYYy+2nxPu06c@h>Rur9%UPyS;hB$v=7@DN$VSN3#{z z`{NyohoQ^*;al!ZuJ@6L6xaL6V~Xp2Um6Yy}tcKalM}WLh&g|&o;&NJ>@OMzog`kD}IgQ_M*X!+5itF*YS#do+4=AqJ z&-WGA>)T1i^?1voPDLQU>2WntalQUbQCyGDBE|K1D^pyrSJ`wpK%jAWJ09C#^A&GX z{0ZS?ryl3K6{oGq>N${tUq+iZ0_oBBpy`V1ed=1p^}aSwaqa&W#r6KRTyedBJ)*dt zFaM>u9&gVo9zYvxd$%i|ulNDQ3lx7}alNm7thnAUK1GK%1oE4nM-vp+<9w3hL6os} z<}0rEo#~3}{q$PJ^?r1d;(C1Ernnw&Ur}7|kKa^W@5}oY*W>D-;9std_&mjLR$PzgcEzide3#Ag4T?7^zEtrh#aAdERs2!KZ&my$#TO|4lH%kC=~mC%F1|4X znFz;Se6{cqv{)lpe?B1m92d9$<9CUR)0u!mk&D}ULoUABhs<1E!ecJpFZ@0i zx8ual8*8Wi`}ca6y#4#P`MWj@s2Gv?*%r7zZKQz z#+o`}p4`UzSiL8AetWwocX2BvNIWkWf9zu+!xDK7?Xf)k@lYg@!k-KqS{E&9YJsYj z)>u<6{sntVd#t|Uc1hpbvLv@{acf)C;@F*{vuQzXbTR%?BOUk;zI-T8E*s_26O{DZ zcH&9ej%O=QXJ=Xyf)>bo105a^#>@Ue$8f60t`k=6B;mAn>TofAj2I?J_a<^^ev;0= zw|pMTl1?!fF%xY4bhg&P)^Fc*HlR3-A06t%{+FzvWcsua;z2ya`LCA_zX(1lPyVAm za{KQZFcOhtDqJ<=y`$=`Ozt zWryp({*bAmPYkC1qw|w3Z^!>|byLYWeJn#5Ki0gdznby5%bx}29{W^cA6*U*4!9<^ Zm}4^WOV_I5%J - -#ifndef casadi_real -#define casadi_real double -#endif - -#ifndef casadi_int -#define casadi_int int -#endif - -/* Add prefix to internal symbols */ -#define casadi_f0 CASADI_PREFIX(f0) -#define casadi_fabs CASADI_PREFIX(fabs) -#define casadi_s0 CASADI_PREFIX(s0) -#define casadi_s1 CASADI_PREFIX(s1) -#define casadi_s2 CASADI_PREFIX(s2) -#define casadi_s3 CASADI_PREFIX(s3) -#define casadi_s4 CASADI_PREFIX(s4) -#define casadi_s5 CASADI_PREFIX(s5) -#define casadi_s6 CASADI_PREFIX(s6) -#define casadi_s7 CASADI_PREFIX(s7) -#define casadi_sign CASADI_PREFIX(sign) -#define casadi_sq CASADI_PREFIX(sq) - -/* Symbol visibility in DLLs */ -#ifndef CASADI_SYMBOL_EXPORT - #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) - #if defined(STATIC_LINKED) - #define CASADI_SYMBOL_EXPORT - #else - #define CASADI_SYMBOL_EXPORT __declspec(dllexport) - #endif - #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) - #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default"))) - #else - #define CASADI_SYMBOL_EXPORT - #endif -#endif - -casadi_real casadi_sign(casadi_real x) { return x<0 ? -1 : x>0 ? 1 : x;} - -casadi_real casadi_fabs(casadi_real x) { -/* Pre-c99 compatibility */ -#if __STDC_VERSION__ < 199901L - return x>0 ? x : -x; -#else - return fabs(x); -#endif -} - -casadi_real casadi_sq(casadi_real x) { return x*x;} - -static const casadi_int casadi_s0[3] = {9, 1, 1}; -static const casadi_int casadi_s1[3] = {3, 1, 1}; -static const casadi_int casadi_s2[3] = {0, 1, 1}; -static const casadi_int casadi_s3[3] = {0, 0, 1}; -static const casadi_int casadi_s4[60] = - {9, 9, 0, 6, 12, 18, 25, 34, - 43, 46, 48, 48, 0, 1, 2, 3, - 4, 5, 0, 1, 2, 3, 4, 5, - 0, 1, 2, 3, 4, 5, 0, 1, - 2, 3, 4, 5, 6, 0, 1, 2, - 3, 4, 5, 6, 7, 8, 0, 1, - 2, 3, 4, 5, 6, 7, 8, 6, - 7, 8, 6, 8}; -static const casadi_int casadi_s5[21] = - {9, 9, 0, 1, 2, 3, 4, 5, - 6, 7, 8, 9, 0, 1, 2, 3, - 4, 5, 6, 7, 8}; -static const casadi_int casadi_s6[24] = - {9, 3, 0, 6, 12, 18, 0, 1, - 2, 3, 4, 5, 0, 1, 2, 3, - 4, 5, 0, 1, 2, 3, 4, 5}; -static const casadi_int casadi_s7[3] = {9, 0, 1}; - -/* auv_model_impl_dae_jac_x_xdot_u_z:(i0[9],i1[9],i2[3],i3[0],i4[],i5[0])->(o0[9x9,48nz],o1[9x9,9nz],o2[9x3,18nz],o3[9x0]) */ -static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { - casadi_real a00, a01, a02, a03, a04, a05, a06, a07, a08, a09, a10, a11; - casadi_real a12, a13, a14, a15, a16, a17, a18, a19, a20, a21, a22, a23; - casadi_real a24, a25, a26, a27, a28, a29, a30, a31, a32, a33, a34, a35; - casadi_real a36, a37, a38, a39, a40, a41, a42, a43, a44, a45, a46, a47; - casadi_real a48, a49, a50, a51, a52, a53; - a00=3.3453634479321918e-02; - a01=arg[0]? arg[0][0] : 0; - a02=casadi_sign(a01); - a02=(a01*a02); - a03=23.; - a04=casadi_fabs(a01); - a03=(a03+a04); - a02=(a02+a03); - a03=(a00*a02); - a04=6.0368975005218254e-05; - a05=30.; - a06=arg[0]? arg[0][5] : 0; - a07=(a05*a06); - a08=(a04*a07); - a03=(a03+a08); - a08=-1.1116270017027866e-07; - a09=-30.; - a10=arg[0]? arg[0][4] : 0; - a11=(a09*a10); - a12=(a08*a11); - a03=(a03+a12); - if (res[0]!=0) res[0][0]=a03; - a03=6.0368975005218423e-05; - a12=(a03*a02); - a13=3.3484658136227786e-02; - a14=(a13*a07); - a12=(a12+a14); - a14=-6.1658244361114702e-05; - a15=(a14*a11); - a12=(a12+a15); - if (res[0]!=0) res[0][1]=a12; - a12=-1.1116270017027936e-07; - a15=(a12*a02); - a16=-6.1658244361114783e-05; - a17=(a16*a07); - a15=(a15+a17); - a17=3.3963490377380855e-02; - a18=(a17*a11); - a15=(a15+a18); - if (res[0]!=0) res[0][2]=a15; - a15=9.8084735444364535e-08; - a18=(a15*a02); - a19=5.4404333259807062e-05; - a20=(a19*a07); - a18=(a18+a20); - a20=-2.9967785627100469e-02; - a21=(a20*a11); - a18=(a18+a21); - if (res[0]!=0) res[0][3]=a18; - a18=1.0920100546139210e-05; - a21=(a18*a02); - a22=6.0570157695918657e-03; - a23=(a22*a07); - a21=(a21+a23); - a23=-3.0801331505514781e-03; - a24=(a23*a11); - a21=(a21+a24); - if (res[0]!=0) res[0][4]=a21; - a21=-6.0150572994295635e-03; - a02=(a21*a02); - a24=-3.0184487502609133e-03; - a07=(a24*a07); - a02=(a02+a07); - a07=5.5581350085139340e-06; - a11=(a07*a11); - a02=(a02+a11); - if (res[0]!=0) res[0][5]=a02; - a02=(a09*a06); - a11=(a00*a02); - a25=arg[0]? arg[0][1] : 0; - a26=casadi_sign(a25); - a26=(a25*a26); - a27=46.; - a28=casadi_fabs(a25); - a28=(a27+a28); - a26=(a26+a28); - a28=(a04*a26); - a11=(a11+a28); - a28=arg[0]? arg[0][3] : 0; - a29=(a05*a28); - a30=(a08*a29); - a11=(a11+a30); - if (res[0]!=0) res[0][6]=a11; - a11=(a03*a02); - a30=(a13*a26); - a11=(a11+a30); - a30=(a14*a29); - a11=(a11+a30); - if (res[0]!=0) res[0][7]=a11; - a11=(a12*a02); - a30=(a16*a26); - a11=(a11+a30); - a30=(a17*a29); - a11=(a11+a30); - if (res[0]!=0) res[0][8]=a11; - a11=(a15*a02); - a30=(a19*a26); - a11=(a11+a30); - a30=(a20*a29); - a11=(a11+a30); - if (res[0]!=0) res[0][9]=a11; - a11=(a18*a02); - a30=(a22*a26); - a11=(a11+a30); - a30=(a23*a29); - a11=(a11+a30); - if (res[0]!=0) res[0][10]=a11; - a02=(a21*a02); - a26=(a24*a26); - a02=(a02+a26); - a29=(a07*a29); - a02=(a02+a29); - if (res[0]!=0) res[0][11]=a02; - a02=(a05*a10); - a29=(a00*a02); - a26=(a09*a28); - a11=(a04*a26); - a29=(a29+a11); - a11=arg[0]? arg[0][2] : 0; - a30=casadi_sign(a11); - a30=(a11*a30); - a31=casadi_fabs(a11); - a31=(a27+a31); - a30=(a30+a31); - a31=(a08*a30); - a29=(a29+a31); - if (res[0]!=0) res[0][12]=a29; - a29=(a03*a02); - a31=(a13*a26); - a29=(a29+a31); - a31=(a14*a30); - a29=(a29+a31); - if (res[0]!=0) res[0][13]=a29; - a29=(a12*a02); - a31=(a16*a26); - a29=(a29+a31); - a31=(a17*a30); - a29=(a29+a31); - if (res[0]!=0) res[0][14]=a29; - a29=(a15*a02); - a31=(a19*a26); - a29=(a29+a31); - a31=(a20*a30); - a29=(a29+a31); - if (res[0]!=0) res[0][15]=a29; - a29=(a18*a02); - a31=(a22*a26); - a29=(a29+a31); - a31=(a23*a30); - a29=(a29+a31); - if (res[0]!=0) res[0][16]=a29; - a02=(a21*a02); - a26=(a24*a26); - a02=(a02+a26); - a30=(a07*a30); - a02=(a02+a30); - if (res[0]!=0) res[0][17]=a02; - a02=(a09*a11); - a30=(a04*a02); - a26=(a05*a25); - a29=(a08*a26); - a30=(a30+a29); - a29=9.8084735444363873e-08; - a31=casadi_sign(a28); - a31=(a28*a31); - a32=casadi_fabs(a28); - a32=(a27+a32); - a31=(a31+a32); - a32=(a29*a31); - a30=(a30+a32); - a32=1.0920100546139184e-05; - a33=-3.3399999999999999e+00; - a34=(a33*a06); - a35=6.8000000000000005e-01; - a36=(a35*a06); - a34=(a34+a36); - a36=(a32*a34); - a30=(a30+a36); - a36=-6.0150572994295574e-03; - a37=3.3199999999999998e+00; - a38=(a37*a10); - a39=-6.8000000000000005e-01; - a40=(a39*a10); - a38=(a38+a40); - a40=(a36*a38); - a30=(a30+a40); - if (res[0]!=0) res[0][18]=a30; - a30=(a13*a02); - a40=(a14*a26); - a30=(a30+a40); - a40=5.4404333259807184e-05; - a41=(a40*a31); - a30=(a30+a41); - a41=6.0570157695918683e-03; - a42=(a41*a34); - a30=(a30+a42); - a42=-3.0184487502609172e-03; - a43=(a42*a38); - a30=(a30+a43); - if (res[0]!=0) res[0][19]=a30; - a30=(a16*a02); - a43=(a17*a26); - a30=(a30+a43); - a43=-2.9967785627100754e-02; - a44=(a43*a31); - a30=(a30+a44); - a44=-3.0801331505514837e-03; - a45=(a44*a34); - a30=(a30+a45); - a45=5.5581350085139543e-06; - a46=(a45*a38); - a30=(a30+a46); - if (res[0]!=0) res[0][20]=a30; - a30=(a19*a02); - a46=(a20*a26); - a30=(a30+a46); - a46=1.4970303990827358e+00; - a47=(a46*a31); - a30=(a30+a47); - a47=2.7177645446042520e-03; - a48=(a47*a34); - a30=(a30+a48); - a48=-4.9042367722181902e-06; - a49=(a48*a38); - a30=(a30+a49); - if (res[0]!=0) res[0][21]=a30; - a30=(a22*a02); - a49=(a23*a26); - a30=(a30+a49); - a49=2.7177645446042498e-03; - a50=(a49*a31); - a30=(a30+a50); - a50=3.0257778596593993e-01; - a51=(a50*a34); - a30=(a30+a51); - a51=-5.4600502730695923e-04; - a52=(a51*a38); - a30=(a30+a52); - if (res[0]!=0) res[0][22]=a30; - a02=(a24*a02); - a26=(a07*a26); - a02=(a02+a26); - a26=-4.9042367722181935e-06; - a31=(a26*a31); - a02=(a02+a31); - a34=(a51*a34); - a02=(a02+a34); - a34=3.0075286497147785e-01; - a38=(a34*a38); - a02=(a02+a38); - if (res[0]!=0) res[0][23]=a02; - a02=-1.; - if (res[0]!=0) res[0][24]=a02; - a11=(a05*a11); - a02=(a00*a11); - a38=(a09*a01); - a08=(a08*a38); - a02=(a02+a08); - a08=3.3399999999999999e+00; - a31=(a08*a06); - a30=-3.3199999999999998e+00; - a52=(a30*a06); - a31=(a31+a52); - a52=(a29*a31); - a02=(a02+a52); - a52=casadi_sign(a10); - a52=(a10*a52); - a53=casadi_fabs(a10); - a53=(a27+a53); - a52=(a52+a53); - a53=(a32*a52); - a02=(a02+a53); - a37=(a37*a28); - a39=(a39*a28); - a37=(a37+a39); - a39=(a36*a37); - a02=(a02+a39); - if (res[0]!=0) res[0][25]=a02; - a02=(a03*a11); - a14=(a14*a38); - a02=(a02+a14); - a14=(a40*a31); - a02=(a02+a14); - a14=(a41*a52); - a02=(a02+a14); - a14=(a42*a37); - a02=(a02+a14); - if (res[0]!=0) res[0][26]=a02; - a02=(a12*a11); - a17=(a17*a38); - a02=(a02+a17); - a17=(a43*a31); - a02=(a02+a17); - a17=(a44*a52); - a02=(a02+a17); - a17=(a45*a37); - a02=(a02+a17); - if (res[0]!=0) res[0][27]=a02; - a02=(a15*a11); - a20=(a20*a38); - a02=(a02+a20); - a20=(a46*a31); - a02=(a02+a20); - a20=(a47*a52); - a02=(a02+a20); - a20=(a48*a37); - a02=(a02+a20); - if (res[0]!=0) res[0][28]=a02; - a02=(a18*a11); - a23=(a23*a38); - a02=(a02+a23); - a23=(a49*a31); - a02=(a02+a23); - a23=(a50*a52); - a02=(a02+a23); - a23=(a51*a37); - a02=(a02+a23); - if (res[0]!=0) res[0][29]=a02; - a11=(a21*a11); - a07=(a07*a38); - a11=(a11+a07); - a31=(a26*a31); - a11=(a11+a31); - a52=(a51*a52); - a11=(a11+a52); - a37=(a34*a37); - a11=(a11+a37); - if (res[0]!=0) res[0][30]=a11; - a11=arg[0]? arg[0][6] : 0; - a37=sin(a11); - a52=arg[0]? arg[0][7] : 0; - a31=tan(a52); - a07=(a37*a31); - a07=(-a07); - if (res[0]!=0) res[0][31]=a07; - a11=cos(a11); - a07=(-a11); - if (res[0]!=0) res[0][32]=a07; - a07=cos(a52); - a38=(a37/a07); - a02=(-a38); - if (res[0]!=0) res[0][33]=a02; - a09=(a09*a25); - a00=(a00*a09); - a05=(a05*a01); - a04=(a04*a05); - a00=(a00+a04); - a08=(a08*a10); - a30=(a30*a10); - a08=(a08+a30); - a29=(a29*a08); - a00=(a00+a29); - a33=(a33*a28); - a35=(a35*a28); - a33=(a33+a35); - a32=(a32*a33); - a00=(a00+a32); - a32=casadi_sign(a06); - a32=(a06*a32); - a35=casadi_fabs(a06); - a27=(a27+a35); - a32=(a32+a27); - a36=(a36*a32); - a00=(a00+a36); - if (res[0]!=0) res[0][34]=a00; - a03=(a03*a09); - a13=(a13*a05); - a03=(a03+a13); - a40=(a40*a08); - a03=(a03+a40); - a41=(a41*a33); - a03=(a03+a41); - a42=(a42*a32); - a03=(a03+a42); - if (res[0]!=0) res[0][35]=a03; - a12=(a12*a09); - a16=(a16*a05); - a12=(a12+a16); - a43=(a43*a08); - a12=(a12+a43); - a44=(a44*a33); - a12=(a12+a44); - a45=(a45*a32); - a12=(a12+a45); - if (res[0]!=0) res[0][36]=a12; - a15=(a15*a09); - a19=(a19*a05); - a15=(a15+a19); - a46=(a46*a08); - a15=(a15+a46); - a47=(a47*a33); - a15=(a15+a47); - a48=(a48*a32); - a15=(a15+a48); - if (res[0]!=0) res[0][37]=a15; - a18=(a18*a09); - a22=(a22*a05); - a18=(a18+a22); - a49=(a49*a08); - a18=(a18+a49); - a50=(a50*a33); - a18=(a18+a50); - a50=(a51*a32); - a18=(a18+a50); - if (res[0]!=0) res[0][38]=a18; - a21=(a21*a09); - a24=(a24*a05); - a21=(a21+a24); - a26=(a26*a08); - a21=(a21+a26); - a51=(a51*a33); - a21=(a21+a51); - a34=(a34*a32); - a21=(a21+a34); - if (res[0]!=0) res[0][39]=a21; - a21=(a11*a31); - a21=(-a21); - if (res[0]!=0) res[0][40]=a21; - if (res[0]!=0) res[0][41]=a37; - a21=(a11/a07); - a34=(-a21); - if (res[0]!=0) res[0][42]=a34; - a34=(a31*a11); - a34=(a10*a34); - a31=(a31*a37); - a31=(a06*a31); - a34=(a34-a31); - a34=(-a34); - if (res[0]!=0) res[0][43]=a34; - a34=(a10*a37); - a31=(a06*a11); - a34=(a34+a31); - if (res[0]!=0) res[0][44]=a34; - a34=(a10*a21); - a31=(a06*a38); - a34=(a34-a31); - a34=(-a34); - if (res[0]!=0) res[0][45]=a34; - a34=casadi_sq(a07); - a37=(a37/a34); - a37=(a10*a37); - a11=(a11/a34); - a11=(a06*a11); - a37=(a37+a11); - a37=(-a37); - if (res[0]!=0) res[0][46]=a37; - a38=(a38/a07); - a52=sin(a52); - a38=(a38*a52); - a10=(a10*a38); - a21=(a21/a07); - a21=(a21*a52); - a06=(a06*a21); - a10=(a10+a06); - a10=(-a10); - if (res[0]!=0) res[0][47]=a10; - a10=1.; - if (res[1]!=0) res[1][0]=a10; - if (res[1]!=0) res[1][1]=a10; - if (res[1]!=0) res[1][2]=a10; - if (res[1]!=0) res[1][3]=a10; - if (res[1]!=0) res[1][4]=a10; - if (res[1]!=0) res[1][5]=a10; - if (res[1]!=0) res[1][6]=a10; - if (res[1]!=0) res[1][7]=a10; - if (res[1]!=0) res[1][8]=a10; - a10=-3.3453634479321918e-02; - if (res[2]!=0) res[2][0]=a10; - a10=-6.0368975005218423e-05; - if (res[2]!=0) res[2][1]=a10; - a10=1.1116270017027936e-07; - if (res[2]!=0) res[2][2]=a10; - a10=-9.8084735444364535e-08; - if (res[2]!=0) res[2][3]=a10; - a10=-1.0920100546139210e-05; - if (res[2]!=0) res[2][4]=a10; - a10=6.0150572994295635e-03; - if (res[2]!=0) res[2][5]=a10; - a10=-1.0920100546139184e-05; - if (res[2]!=0) res[2][6]=a10; - a10=-6.0570157695918683e-03; - if (res[2]!=0) res[2][7]=a10; - a10=3.0801331505514837e-03; - if (res[2]!=0) res[2][8]=a10; - a10=-2.7177645446042520e-03; - if (res[2]!=0) res[2][9]=a10; - a10=-3.0257778596593993e-01; - if (res[2]!=0) res[2][10]=a10; - a10=5.4600502730695923e-04; - if (res[2]!=0) res[2][11]=a10; - a06=6.0150572994295574e-03; - if (res[2]!=0) res[2][12]=a06; - a06=3.0184487502609172e-03; - if (res[2]!=0) res[2][13]=a06; - a06=-5.5581350085139543e-06; - if (res[2]!=0) res[2][14]=a06; - a06=4.9042367722181902e-06; - if (res[2]!=0) res[2][15]=a06; - if (res[2]!=0) res[2][16]=a10; - a10=-3.0075286497147785e-01; - if (res[2]!=0) res[2][17]=a10; - return 0; -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_jac_x_xdot_u_z(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ - return casadi_f0(arg, res, iw, w, mem); -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_jac_x_xdot_u_z_alloc_mem(void) { - return 0; -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_jac_x_xdot_u_z_init_mem(int mem) { - return 0; -} - -CASADI_SYMBOL_EXPORT void auv_model_impl_dae_jac_x_xdot_u_z_free_mem(int mem) { -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_jac_x_xdot_u_z_checkout(void) { - return 0; -} - -CASADI_SYMBOL_EXPORT void auv_model_impl_dae_jac_x_xdot_u_z_release(int mem) { -} - -CASADI_SYMBOL_EXPORT void auv_model_impl_dae_jac_x_xdot_u_z_incref(void) { -} - -CASADI_SYMBOL_EXPORT void auv_model_impl_dae_jac_x_xdot_u_z_decref(void) { -} - -CASADI_SYMBOL_EXPORT casadi_int auv_model_impl_dae_jac_x_xdot_u_z_n_in(void) { return 6;} - -CASADI_SYMBOL_EXPORT casadi_int auv_model_impl_dae_jac_x_xdot_u_z_n_out(void) { return 4;} - -CASADI_SYMBOL_EXPORT casadi_real auv_model_impl_dae_jac_x_xdot_u_z_default_in(casadi_int i) { - switch (i) { - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const char* auv_model_impl_dae_jac_x_xdot_u_z_name_in(casadi_int i) { - switch (i) { - case 0: return "i0"; - case 1: return "i1"; - case 2: return "i2"; - case 3: return "i3"; - case 4: return "i4"; - case 5: return "i5"; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const char* auv_model_impl_dae_jac_x_xdot_u_z_name_out(casadi_int i) { - switch (i) { - case 0: return "o0"; - case 1: return "o1"; - case 2: return "o2"; - case 3: return "o3"; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const casadi_int* auv_model_impl_dae_jac_x_xdot_u_z_sparsity_in(casadi_int i) { - switch (i) { - case 0: return casadi_s0; - case 1: return casadi_s0; - case 2: return casadi_s1; - case 3: return casadi_s2; - case 4: return casadi_s3; - case 5: return casadi_s2; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const casadi_int* auv_model_impl_dae_jac_x_xdot_u_z_sparsity_out(casadi_int i) { - switch (i) { - case 0: return casadi_s4; - case 1: return casadi_s5; - case 2: return casadi_s6; - case 3: return casadi_s7; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_jac_x_xdot_u_z_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { - if (sz_arg) *sz_arg = 6; - if (sz_res) *sz_res = 4; - if (sz_iw) *sz_iw = 0; - if (sz_w) *sz_w = 0; - return 0; -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_jac_x_xdot_u_z_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { - if (sz_arg) *sz_arg = 6*sizeof(const casadi_real*); - if (sz_res) *sz_res = 4*sizeof(casadi_real*); - if (sz_iw) *sz_iw = 0*sizeof(casadi_int); - if (sz_w) *sz_w = 0*sizeof(casadi_real); - return 0; -} - - -#ifdef __cplusplus -} /* extern "C" */ -#endif diff --git a/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_jac_x_xdot_u_z.o b/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_jac_x_xdot_u_z.o deleted file mode 100644 index 1fde7b8e4e1dfd14abd342bbdf246919a4508b66..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 17736 zcmcJV4Rlr2mB()cG*FuRQe#V7icfvmU@^Xge6=EZfgn#Oc2Wg-Z;T`%$t$TLiR3XN zwHO!B1qH|SV;t?+8LUog*Eq^j*DR`x315=>fe0vQ)zX3#sul|MWBh35zxUnyWphfF z&$e^cy6>LzJNxW&&ffc+d+tqco>_b8X*oHbrW|jYH&jyUc>}Sbe2|TKpYrm(6aIwv zQvT6jr2N92DgUB@8zvn0H$Rzk+z*YS1}T5`rj$SD$&`QfrY-*2F;6Q{=plii{cIKn zCHs=T=``J*Rs1ocQVwuZpie5x?b)4QLu)b4Gl7O(aHY;w$^pW1ze6xS!u1 zi6MuI!&HOpO_3OecNl7q;=UaslOs}>;Es>`iIEWqUc#!@%*3z@_d)zuQ0uR_^OLOQ zLeUmSAE7cS)PI%Q(P^#K5c$s9PsP901!At(6yCS_%7hiK` zeB~~G#VEKBJ)9*Ygy``aKQxhx$OJtjBLc~tR2`TB8O#;UYy9M|>E}iId5(TI(a$gN zllaY$a&DWz=uT3W*mV-{ld3#{FeuzjI{G0YmQQXuUhOBgVkY|)`uXQ#!dq|!gGD_H zy#%2xe*W~33PNHtu3#pR+C-`lh6S4;6{g?_(R3&{K7lp~cqhL;Ld{Wwd~Ku)zmG(E zAcV|KkpzC*BaIq3Es+pLd>CE4f%{gj9LJS$1PNRxb4b)g#z86Cqbp>f0>7#^gl^D$ zC)6M`67Tsd8pg7V8mZCjNF9DP9`>M*mdH4Ww?x8dfGN-YO{&H(+=HKjT{Zrs*)-UnZugU~p*A@{KNx=U7&3{cF{+8s%x8da}BQdUfIHwOGii~%eZFme+F$g*m}p-T7>!cU>@ zbzDI}`ib8|$pB{jJUWV*j!5|)$Ztm7Cb%6d?daR&@lg=!BLBUB>j_#VJ(x|(|MYD3 zeL9jLV`hAfGTCuE35$zIKm%dF5OTZ~Jk1&k168PH1JO!+79-t(k??HL;PsRQZ^}RG zP1Ip_m=y${!7!hu%pqj@aUFzYpawQaOUWnqr%?9uyA zhuA`tBmYBOR}7Cnz6O?qD3+VXLlEG>>fk<-1rZMQI)BANY*=IwM4T?D(Bue_KS50` zFD)p8tp-t+pb8Yy%HZk8*GQZsvAZT(u-_tgQQvXYjEM5e>;p05I>w@1So7 z?2{(jvKLqb^6*NsjHHl%(-@AVg(r`}JGT6mQ%1NC)%*FYXoIF1Jqw+9Rwno5WgWIb-|O{^ifC=Mso zi_hUMJi!{-EuoBqx3PnB)xi+1U2`5*eyCm?1EUtcf5pm!*n5-jqw!6VE`P=Ir*k)x z;biP^6vMPriSIIA=D40)OAoxjvhg(w$?7lS!dgh4IPTZ~j?DHv8sp&#B!ORmMU}wb zFY$%0ccV=|;9lrrA4B+maj92#!M5+1x(}W5yG|LWn#4QR^mG^y-=TE@{Hn2$z|%Jq z5AeTu#6O86PBQD$${z>kHHThar5RFJcSZJ5SFja&g0xPSR(1j#2kn1}Cn30&k1z3+ zZ~80l8p(#98I9k%F>)W8$8UWkvJQ86;$B|*>bZrLFxR~-gD&7!JZNI*df8Ex*vJi0 zc?^{`{0!qDgz?jKrh;p+SJ3(>+>wg=%P{Ez1oYein!t8{(%MK5I#z9@owgrhtS@xj zpSK?fTL!wuwmP?lo^coxtqz{nyCjR;ibyV>VlagsLVjpuaAKqJQbbA-I}Pv4ua1Ba zVlvn^o~QMc7TrO62|2_GLJ6CCC00)1@2Sa66jpQJ0OioaToKq9^j@@&vPbc-XoUaUdGiQ%9s~-kzMWLQ z0*@roKDm>u4}}F=(K5y@>z^Y{^q^(ekn@rskQhDR@$@fz9^%xFV!Qq+Kfkw*+F=Xa zPEy#d>3q=BsKW@hxcHhcq3HmxB??1Kdp5s6VDgy9b12u+gKXZr=$Ov0czsCH%K{xz zB!Fva1D%J*3no^}3uR>&df`=}s{tI1ccSqCRPqCM9v?#a1j}7GYV%hvfs*)|8M$>0r!sBy{B9`-6jcFN6@kQ_Tb8nsGz*i__jgyg%_GomoulmAiv~ z4%DtLPbr)6yiZ+QNs_DEP?o6v$Q36dEw92$lys$HEX_v0a1cjSl12M6g1#lf;W z2$o?s-X7Gd>sx8#y6p#pTDaI1M8`;yzVfc7~>}fpzmA^n1yB}>S*88bm9#7Cd%8j^vn6tyQZ;N_u#a>&9 z*EYp#EA`sSyw0fCS?qO|c%4(w@Nb$%i#8rS@>Is*gcD5|Z~pjh79^6P`BFVufFlq0 zkHbm7?bFpqeV&0U_hZOGG?T1;JO0l?;#^!`!c~NeoIw|*k2C&{D^Ft>u8o~*HZoS8 z@?;}T>OU$|Ud!YwTPFM1GKxsYs)Bn(?E0ml-;UY)t;a8D{^C!j-Ls@G;Im>ICaiq@ zYa7P&-?sU!!us3B#J13F^J`Ud?-SlHaGQQaKW#8L5Dby$9omT$M!6UP0FP(O2z^m4L?t=SH|Mu9b$G-o| zbw6vqyK3UVjN zM2!3IlkxZI_-~ML?|O09#1~GyQ~mXzz?v)g3a<&)o6s)xY`fp2wZ}^c@tr z&rAK3{2djY*Y2*W>z({+<-zJcy?1Zh@rPAayEFDxd#n9*K59?3x7y#eui8)TsrFX; zsJ%pgpY-SY^KtQ)#$%P(?{TqTpV*^H?9nIl(|J{ieXGR2YOlw|Uejd$RWkoRZ`O<% zUkMk^nZLLry*OMvrKq?ldST^aE*38J__7wv_!zB-5Y$9`hEhZZ1D zDQD@GUe1zG&gaLTk=u&}NNvV*jpuz??!8o_sv?A`DBXwfB5*hG%s}9 zY18w@{nLo)dEr|}PR}b`JE}S_`t8$e@+y~~ae3acoDtvq6^ig=dnT<*^Ls(-62fvNbLSo$n^`;2QT?^kHdh5O(Z(f`@`U=1V->q z{O2_zJbh{m)lEb;2Ruvcc6{5v9Fr-E{)J%RMQ{C44SNB%D1haLW7 z;rktapYSN(hbkW!hCd>F!H_8D9u?l`@P6S-9R7sxE{8uYyw~BohT;2!uXE&I9flti z9{6pj(eH%oKA|-sAA)!s{HqT6nL+Zxi0=@P8J* z&f)*cJlBh#G%L@$TjbX}@;?>6)8YRmyx-vu3qR)Yp9|mb@IK-CJknupW1jnG``c3@ zug@cue@?hQ@09Nq9>l3q&wE99jPFDIfbcqp9}(W@@T0=J9R8N@9*4g#yw~A5d;lEn zt#kMp!rgT=mieg6xlw2EDdAey`#9lr{Ih9-aQ6fe5w2$?ZF7NeeNHQn3U}9axp4g^ zpz>3N^G7O$xK9`UdBZ3h2mh2e6g+wc?i3yqc|8Z;F1${-&f_uRjl%gnjQlI$u8m3L z;4pmuF#PZ^{0(p#e@HsfbMrVH{)vYjK0|m^xSoMKgvT8DJB8Od@>_&AI{XdcT@F7B z9npAtgzK4ls_I#`T8;8(fq&xwZY9hB)ms>LU;iJlg6{(k)I(v zDuJcv?QaP0a=6#D_#Yb4wD(ON*L6TBe~p znvGCNHbPUf5h~6`s5~2?(rkpvvJtBAifU&>>4$UWlq;j3FVoMI68bIX`cnQaE@r~K zl$#V!VM+ciF6CNQP|CWBS*9##RmqZpOrW17OIY(1mMme(DJ)sSI?DnXZdJnhDXh69 z_~lkpSX)V;KhPHR$Q{xTR}}|McirQB(0Fa>T^%93TQwv6S=SWj6n zykLUdwv6S=SiYR)%UQmh^_R0uIkzfj{pGBuoHdkls|s#a!SWT{wu0p=xNQZus^C@? z+=_8!(1;aO280zuB!m z_R>uql>O)i4NdLsoy`pkTNh>_+t$&R&QdhBsI@gq(dGrM&DV7QIMXl|vP2H_o zNWu(@T2omFwzOs`*wK)cJv$n*n@US-s%de1I!mQZ3tK;iWOgIy?rK`p-Il&FJH;94 z?3CZox#+qqtWRPM^KVSIc6*+lbLiMtjOzpW0A4E3KjL)OQm*HA;w6Uv2zg3)tb6lu zsXTqBp+vkHm-0;nkw_0cJC*Msh(w$o-O67lh(w$ot;*jeh(w&ebt#`9M>XPf##3Ht z_-72)?{y?kXH=E{p^^Wr;XgC{bA~@;IDIcwJ&ze4HhjSF^9_H_@QC5>7*0M?{lWWs zV225Yf0ibMM7MKssr*F4=`Z@qgZJY=p5ASh>-z{bn`C(K{u;>B->OvpMx%eS;VTWN zziX-d3&P#_IbpbspFyLi+~}cmIwh*M{=L(1>&Lqd52KBa=K;g5fB)NXYo9*Dt>5WC zrzp{QF2beyPZ%CGd=wb~iR7&x^9{H5|AOJ?qK)b=HaueZbi-}DebsR5$N7d^e=RlK z`ggN%y3IHJzF>H>;e$qxwR7+9rgz@_6k$M8zS zCmBxft}0(=xV7^l!@pwWml|&MEH~WR?K_6s=g32bTYElj_{FB*eTIM9@PmfavrOl8 z%mQ8BZkHGyk{4p)^hZ?H6E^${hEFy8i-uorc$MMT7#=gc&G2f&ml{6Z@KuJ- zF#Jx#YYcx;xEohT47YJrl#9klWQUo!)DGo_+c>%0@Jo&S^@h(f{1(G)+}>_@+{pjf z@XHK;$Z+f5&4ye5?lRo^_m76t-==iF`cEHZ2kXbN@_~VP%*c;3-1;$M_;e#5HQf4f zs^Qj;^9=txqvsys$y z@R%9ry@uO3S#P+FlZ}SkI2kb9#>oN0&oljwq|FzJ>^Tva+HIq7@_UQn=g5al;%$bv z3nxAHeIY#zzsvBi8a+QX{O=8a*zh{T`wah@;ZGTUrQv%Gzsm3fhST4G)ec7upJVv@ zhF@)Xgcc8yYlo?ZQ&ZJH%W%pnUuC#`@A$sq_Pypl!{-`3>kX%@>fdI#eQ$c%aQl8V zo(6$Lez*I{G{fzF(r&ojH-0Rf>|@8d!EigyJ%-!3+9=;|NzVveI+m;n0{PtzA1iu(>u}mzDZS70G7j0sGJL#nX_n#o-Cyme`bUa9y(3UkzE1e~0B88U!ov>V zCp_Ztjlv5ZzEil`L;Kx2jDBhEl{$K2QjR;kPRjKTZ!`ANTa4;J!d}`0(0Plx|&;_KI4X(oJ5` z{O)eAXi+B>NW8cR-^^JkV41q+?leB@iy#X0M}+3ig$r9dpsJ%Y-CBfy{kyO`-PC-Y zl)v7wxTtGUXIJZ@^o^pkbwNXF5k7&6WB-VB7L(jPVkfN-w`h^SQ_`Lg z%a9F13gBlAKfQ(J)bX}_D-7)IQ?p!xKWE9bwfJ;()Pt@OFBz%*`)nXun>9e zU+?rTMtO=io5J)F0co;R5oS8{6GQe?_6yUY7%40rk^RU=uKlh6BN5kJb#!<@qGzy8 z+FtMU%so~6_0oR5Gk*2I-WN#wsO<##Mscw2pIR-`;Xa97k4u@#(0#22h=s_zvbTWI zE3-39uUFcKQD>9RTiKn+kWRP#dTD>y>CkQe1GGJr|6=s=g0x@SlmF=4sqJ-or|O&9 z-9aDSkhB!}?D75m|G&SZ zo!3>bURAw%_0H;UZq3fUqEA$mqW$_RBb6|uC{CFX$O&VeO1g49a?Vr|nKoK0+(TM$ znLui(ViPcNv(T-R{@~~2zk2%ULju-PV>y=BCh1LS-me8-OO56FAr>fug|OjTKl z4y`g$&ps$aoXXWwUT^JSwwE+7+md*Y&y2ltq5|_Y zlP-Ow0(2iIVagx%`>c$=9lWjjn}0+%4S0C>Yje(e_6;KS;Y!0ryawPp`?#~CZ3`@x ze&-h`3HE-`cE#o#V2`^*$-Sfh0K5C%sJ|*oOw;XsLbpf9oqx_raVwRq`N>x-UGe2f zw!Y3kMcew>PduSk={q*s1{ITB1-@M8Hm7s&`ASmE>T{G~Co0Y;#TngHdt$Uhv8USm z*dY_T)}DBu(sYM%e;?B2Qe5e{sH=>?H4@ioT-mtjcZEn47t$Pg{~M&^aOFyPJkl$1 zU4`pvToZ6j#5D;Q{oJ@bxUMI_@Vf!`UISc!d$LiX0jJ&iaWfTH39eFHH{mM7MZa>9 zC>2O6aRnq?g>)LO=@OoSbQZ2#aLvJWE3Vsc{SH?huKBpM-|e{hJ+3=&g>c=4s{t4N z?#6WwuKNkV??GID#Pw$a@LP!M5nPWFfFGJsc?{PgTuDYwt40 zH4V?5*7k<`e=?(Mp5AU5_jr8U?`Mr(cjv4})r-CJF3Wp-^{rj^z1@0=_ocXoS+{?9 z;H+gGBUWc74)@;HC;Qc4?8SR04_o%}xR(n5ym`ZiC3`Ra$M0Ww=(|hb`*2_9@GBl` zIzR4<@4x%Q##z>F?bnRD{IvTnTGKEoD(%pDYkV&_Mj_uh?%eG|9nnv8S?ep)rZhk5H^sDc=W}G@acGsqN18-h(!QTe!8f8@FTS~O->9~{yJx*%eQ(30@hg5c`jWN&?OT66 zW@_t{)f*O7T{Pt%$2b1r!2GT`{kqB?tiRBio3P-5nPs1E|J_YL-*`pxE9yNrXTAK` zhmW1|T=uf|q@Yo?Gxnr+={8h)q#-tC> z{5}2(qZH-D9&)Hp^@P9exSsKe7$ke5Z^Yo!6Mk4Pa%cAGnZAE7{J}=C_n`mXmY(@M z(+hq{FZ^%pMbAfj;X~s?PxW2i3;u$bp6U1WV&_YH(evY8_}tZt{U7dy&!2nYliG_u zSM~2%f0}pfxXi!KUie?w3!foiU^g=X{B1AxZ|bGJu0WY{df;a_!tU{C%~)(fANz2JY>3qI6K`_jD8liZVgsh7PMd*0DYy-w|gzN{De>w2M2fj)zJ zXqV4>p)VWQbG@GJMef5`ES;#Fpfs-)LTG(Jzs|k_Z{H%|i$$WWKUv@lHVFjO;9t%u zl76#*b0z*)@dCf3MIbajv~VN2Hd+MWhi2lR3mt>;PYGDVhfWaqwg&`$xJZTPlRmsa0G-BX2K&hgit`#S zuABpT^t+D7ffJNkS`^`Tp-7Y@Y0o^VzsBbg_Wu)<08J?PY4*G-R>-ZD_0sSorT$HF zvG%zrrYzuo3VmqNg&#ovZHJSS{(&_D*7!`Z3Vie5Bk@;CJr${^){gPgPue6OjsH~1 zzl|19_!YuG==bjNf`60A{vXPEwaR+QY^CfZNpI4#Qrf50vc5KH2aQh>2-qK_ z{WX7hPV#A%{Sn=nfBDirc2hsimi;AF+D-F^fQFa+HMtK+{hLhndP}x%?L;BZ4m$ds zF2@1oDFJJGPU3M4#w!!}l_F8zlydV-^}0~nxzW_`MnEyLgI%_x?8Zu8*)N*@E*NY2 z+ofOGO@5Uk$A{FX1-)D9^Qjyk+N9kye6_5XUDiuhSGiBx-zn*}esPlQ7quooyjSYk zYSQy0Y3Ev#zpa=4pqTt2UGsk_P;2*Asi#xwsp<2I>=y+leJ+;zEHUZxH|cN9vRyPk zc}ez{Jd>Wk)%?VSUnKQ+%KoeI3CM9NAm^`KY5(0)pAM;y=09i2d8$d;L95r3+W0K_ zKaTSBJ0$JUEbVZUKr1gvJ(rmLd6V?3W=RP3__smYx#@lZYyJ>_yzs*|$w$+(S?b?r z(toA&hh}Mi&3{gj?b7&+;6Gd>idXt)qx4Tr|Jl<1snX8qR{T4p*|SB!7fbxrQcp$t zr>5t7l23u;qqWO4*C>^~1JVD8m^-7ZX zx1|1dX-~~RN6YcScC{c(l6JTWh9ds81p@CBi85REqej_}K*qm=(w=RyVKskSEBQA| z`+O=JX8%CJzd-UqasK@Ue@U)Q)(gWX|At9Bx0VSw9)3)}ak70=WqWCS9+CW=CjQht zdAybUH9a>;yE&!Z(B1gwJW+M%j*<9Xh2xjZzjyzY0iy zPL=+w(GQdO4wFA*O8*RO7W%oRo^MM20h8bEmi|*K^+)sZ??o7v?eL(0HMxJ3^;%-8 z*I(rLlloL7{UkXac1XX{`g zORIzastM!AlvP&vC-|n6`9)sOIo`qGY{y?>^St%@GP_(eJIw<86x;hBUR6)z) zKvii)u$c7hE?cUI0^UHGuRNbhmj9kF;Lb7yu~#Yg~8IwifY9_1FZnF z>L}fs-e4sfs@PXk7BuKkS{^9#7Ww={-wWDX9q?6Emj-8gOM4}$I85I3$|_^6jS|DG zD{Few%3I;3%yfca_gWg$3Llai8_qrQ-LfNAv zJe*Y`IO^6vjEN!&2z!N{SU0$Vs;1}aM^f}!853D=RWXqrrCvuexru8p=85e7)VHMX zd)Zt3=q1$#2ax2VH_=dpR@6qNqL~%G^3uZUaL4wVT#x!rp%_`TLWSsz!73kmy`fx> z0(s=4w$+N_o(Gk13r1EZP~|VIEDzKK!vm)Q<@FlI8vl$^W4DUrGrcre;w>tz#`qH~ zo#mtPL)U=@BCRWFdZ9|irBlt=qSA70l+{Z!j%<#R+&c|}NB5kmWtCHWW!>=xyY-Y= z9X6A3vJgY4UVtoULSdjbVPtuU?Zw#LTTIRcH|w(%G%oU9*`2V;4-4oj(S0`=k{5eq z58fD&ExJXx$;Hegycx444blGY5~vXZUSVvn4~m981sx*buPE|Y6qfpX5YFZat4}j? zkL)q~z!v4cU{z_MznWzf`wD}VRS^?A&r-e9b+Zi3$C1M2vY^P1ZdEx`&x0AvC~AE( zPByj~&5mkDNe33Vwv-?iOudltfb7L;syBktic+lBde7%Y;bHO%OZ@}du^~t!kejyd$Udi=@A<`g8E3U9zlF07lg68$l_C6OP4 zV-oo>xF(Swy>klH4>weq>~ho&N}P{!Hzi1q&fkbu+X@`j_`=K&KbV@T$UWugwb6Gl zN3D)t;^>9+wnyv1bC0n<@@#zMp}*$n`(pJ`^~9seEK;efT-a$*mah`Bh02;!W_h(JOh550q-{8p`iXNFyLhfkMl|l_=!3- zp8Gc7Pcz_W81VF5w)U$v;1e|@(mM?JMgzXlfT!o=wci2*-mW2$HW~1>2K-_JKEr@t zV!+GL8|N-H;Ij<$%?A8s2K+h$-eq zvFg7(10Eq+{pU8|wU{tv78vl<2ee;_0k6fbDKB8aYki6EGYt4Y6GHxK4R|fKOj&mr z@Yia@NE;1!gqro=0s|f)bp6+4z-uvN%3N%~Bjl_9mKg9)=vYNrYQS3!_+|rMi@j6! zIs+b|Z2i}2z#HEo*<`>Ybg%!~40wdN^`C0MYtI5uW`_Zf5V!u@Z@?qeAO4*k_FFmh za$bx9pAe>2YzF+P27H16|3?GfZor>zz$Y881StI{2d1TCIkL? z1HR3GPch(C1O5U7zQcgO(171>z)vyY6*=$I7?x_l#~APv40xLXe~|&7V8B0Qz}pS@ zu?Bpy0e`UppK8F4<`7YqEq+XCNO&phC&+Zp4kx8&fR64(6ZV4tp5 zSKaEERjhbd+Lu6gb|rm<#Cn!O^d*$j+5SaWS63s$v=r-Xe^-ZT!_e8jLWgM~-`W10 z4$}g@v;9#WriFWF`@K3$3--?T+jN*Fxz6^Rb(j|Do$WpyrVVRn`&Bwj3-ZqPOdX~L zcW3)h9j1kLXZzVYObhJJ_CY#K3+vAIC>^GSb7%YBLt1@l!HoLrFfEi(e;uZUFzT-{yIzxT-0BOX<>`{>o6^7QGXq#jW_DA!?b`!{dJfY zuBg8b)5aV1*I`<~qW(I37Q=fFYW24>{DltF0v7exVOpS~{yIzxQ`BFFX@QCQ>o6@K zQGXq#g(K>(!?a*T{dJfYim1O1(?SsS*I`-!qW(He3p~_chiPGl`s*+)&`^IJriB^m zufwz;L;L@%)t?qzXn!51jXdhF!?eIc{dJfYN~pgM(*g;% z*5P!9eL74J0i*spOa~FDzYbr{@K7Bd!SLBSOa~ySzYf#E3hJ-JqZr=%lU9Ex!(Zrd zCd2RQ@Mwlt=w6Qt%LH#T!JnGo4@~e|CU}hre$52G zWP+bI!H=8ZhfVNl)cz)TmkHixflz= zo^OKZnBW=>b~R+o#J#J*Qboz|{lR!FW&ZG?SjzP8YO|hIi_u3}B~#GnFn!u+N(LWt zg*sd-c8_wc*x%O`wbu3SpJTFa+^qpn9;T|Sj>pN>qaPv&G-L;UnVC`#s}%!xUngP9X1 zUgNrLX9AW~u6l2(tG?1+I>4< zWe2*tD9h;Ln7)e&;JEqqmgV1z+)e1h#E2q)HDN+1i@pk;6ZB_MU6*31kyr8$)3N}dUr0? zeAz>HTQe$A??BP3Wz5)k3ttDkaI|{Dk zf=;EUa;*@mm2=PGUaY12TJfq-5F4&`< z$y&L!S&rj0VaK6N8kebs20fPR_)*pmt#igB)atp8fb@fg_~TKS8YiGG*e`r!h@RIM zdC-jyywsU8J4Vmm^MIm2ml#F8Ugo9hd0!$AMfnU$9VxSi>Dik^_At^}JxgZKkl83V zgEHVft3aoA{XmMvuE)L3U^QiI=ZsuOx{m%Q&?ru?u_|f{W!GC$9}bth%TX&BKg`VL zF{nNw2#<>(yh9Mqk%R=QHG=T7g+`8zg77*?NT8Z42w#dITp$Q9mxKhW=Mo_rWQ1OY zOOTB!j3}b)Xqnw#&mMr|Xc)b}qN@A2dY%XwO+v;%P|>(Rpt@WTrbiGi7KF!3LITx4 z2*Oh$2$u*#l1m&3RLce7zCRo7uv8HCmxKhWSwx8H4bb(!;(peDfTAV~Ca**=X%-xBD82l7Tk3AeL@QY)jUDCmq%6_fn5!EIW`HxFC-y>>aPT0Z3JPP zAbeL65~v>fjt%g)2trj5E|Y`=svii#xeQuqR9>GMRnW!OtmSjSpI#M#p)tSshHrgfkB9d{I%+AxZi$!)G z3M;B2vnS};6Gip}?&V*6OU4mXV3F(BVot2dbltX^X3OiY&&ga4 zDG&8XdFa-MU>;lBH)5z&7sEfTXU&5OVn4FH1UK!^Lz`kg-=OmmwHj?Ww3!b^iifAGA8fPzRZa{dGle)*yOBwXL{!Fj+$wh!c%kWr>5uDUpRl?+^b!o_p*m}VNoIE z<%Bk8g?`L!uuQubA9D+RlN;LZ3hhxRLYmCdaG7;2QLegGF`yj}TJ=O|ojv?e&1Rc0`$_go-GJ?m_F5a7BhsPq)tB!zC@} zFMxtrj<`ZUWrn_M8EXR??sHsbi-^zE9g4Evjd?_AnADZiFeiHGKG%vb`>3aPq9%o_ zGh4FeQvtBJDb>|5gaKFmxMWxTHPBwSM@uwmrL;u%VTJ3Oqg}(-)U<0tGV6Yh3dUtN zOmG-wyu7!o3j$~x50zTR#Bqu$irUfo%T>5mS3whx+N1^tXsma<_F6Q`wNJ(343p4f z7;j$12n`=k+jb8n8T*l_*P%WJ2Qb(x^b;J#Wmv6YdUDl|#VVo5aY=6IYgb`Ox|$2) z!}TtSn!oGTb6xcy(Oua(e*@}_mT~E7J0Ui!oR$FJTIa^Ddz0E{ZhZO%P7@WRu7@){_&z0R&gF4$gJht%D~xY}UbNIMh#IDpzO>hQ5{^$B|v? zZ*dh{E74o<7wf^?s6B8&+{Za`Zd*gcoORIHRO_JMI<12y=c&6oy1H_Q@2Xh`J(7}z z1fgBZokF{`t67Vr>o`f-`35PIHiw|WMWjg?X)?^AUM)1iSkST~R&RgklLzVftWS|a zpCMs=ywGPV^r_5KuQ%wE(F$3pv9wknRv@F06^LC($>7-zI~31&nSp^0>&Y4U5^qyi zf)y--g|kT?H}nazKC=z_M2Gd64t;KgK4bILxAwr>$v*9NoSvyyMmmc;KV4<^9~wWj z!NV0A=IHK2!vpAu8*t7Ehr(GcHU^;|F|c4z`4z3{?5_D6t@Ao?PFg?A;R?-m#Hf#< zI-OQq+BLKYsn2!90La*fk)YE$DCkJWU)07`80Wxfgu-{Dcf)uS90}t?2XaF1Xyenk zhUlxu*ZYb=ih?v_<9VKTTXw}GI-6<_23PrIiVGh z@G@BBgw|mCRlBG>M$Ze8h+N%$+Eg+18mY#3sM{lCH-MmB9AQ|5$(rC%cJ6ZR#|PhZ6T`o7*G7zHj=xajm4C;9^sAZN?%QWiz4}^^MO6$&F-Mdq( zk%i1Ug-FnN#=h`(Fd_5eFd+1!+5uIl)tBl8Po;u`nfH%cIqD3P8p#1|`FtKA?CfV& zqFEkprgD!;cBMKynsC)EK+_V2yQE-(jw!T2dz}S7S)maQ)Hngxg2gC9=_Z+`Qi}sF zK&pX0SrLPH$tYEyQJ$j)iV#?*;Jj%bJa zD3ko4dY~ei`6{GB>FM7XcuO65yrPyN-TR3;#O}VE8XX!!fdP;}RX|Oc{b**tM?C;$ z%LkGz?6?!d8U@HuX|&u^;E>sJMGUQwb3*;pT$Dm1SY8A^C)DCV)1P*JMz#&9IFNyP zyMmYy8)7H_UNfEA02}Dp$QrF@<-!Nl3S@gxDuHMkVv~^zHu~v~dL{C4a+B$%fhE(E zHGGJprh&xAV1lt>kWqKGlWDuutw@*CdQBaGJ7E>FL5qSRjB2iq1~01fs}1^oZhGABdHqTBV_J&Oi#Lw&YDLu?*$gdv3E zsP`cMS~7|-0k9~40aIbV`dqC@=h#`24_iQJ;hQtaiHv zhb@vH`P2erP#ZKN6+ZQk?M9y>Z~6|5QO))|EbC~0iB+On0D9qhJk-An5;O;zAI3n1 z0yNYIXsBO8Lw$QX_L?MaFp9Mg=JTG|M9eC$-cIe@kalYwB^g`bFY5Q(uzIwCA@46H zvlPQ3F@g3K@_ZO;z^O4=^)qOL$>T&88ai4XBNTSkHDkbn%V*E;n(Ib;M)YGGthU35 z7$zHHr;~X02=!8wB|nEBOA6#)tLL-FoCKO{(L*7t=6UKha3zgf7WOqbIVSHv8>Q5* zQDZoAGHP#dJr=u`(#ea2_2G4=-YNK06oSn970TRxa$tZ1%HF(aZNTx%gRLd^@eOge-L&Qe0!G|M6wc6|6<oRJsNp5ECevQ zV^YYYf(Vh6Ih=a?xM=&3iBp9Bpv8{gg7-1#CYmM(N{I(fhb&5rfcsKQBb&&munjc0 zWgI1hOOVCsKG|?@51&?u62t_|`iX>QA`+>kj7EpVkp>RTt@HEfqycq91yNP1QGhCg zLjxRENhMbdR&O%2mW3eyS3x~5C@-5&gDxY;S7iq~l zg;N*SUodHwO)|c9P&T`{l zCiI&?4_&Y0o*>*u;B3GxB-|?C&~7ejGPE12m|HKbMzh*P7>8Ok4{NIIp<8p|)~V#l zGjc+oxk5W~Lu<5g86{J*Xm0)tqb1Iv#)J8l4bjQ!)nAZ)7|BI0vMxauh&Xo`a<8Oe zK+ioxge&I?YZ?uI5VK*usFz?2DLLVklK3cAoFhJJ|Cfc;VzJJuB#Kw)>amD~n(NY+YhgsD&hibEUF z3tfoVMhdBap#o4cIXkpF3k6-pjl2K>_pDH-s5P-yr-6`iAh%7=p-nBbaU&OtnwnLl zuATJ5=`vdc;Y8_~R1q>A+VOz;D^?U;1_0#~XXvR~CB7hxPaync7LPGpZTO6)ql)yL z`njnnsa{VdLO)=Jva46Yk#g#%ah@7Ygc3k*jQZ`T8l_FW6h_uE)zh?$erhEO(OBn3 z6>`B=9r+0tQu{#+iOi`#A*X(loyw{kK`cZk$VoXoAGgm(L2hUG&=t3YeTX`qQ}ZI> zK(mMsnFpk2nn=^NMivdchg1azy@B`6lB3KDZdCPcCV-XOZ#g8^fwcY@y76^dP_#EA zmte7Ap(Rm6nge|ei>p3BjN$ta{}Vv%;Mp2fSgbW`@8$m~;1 z+3!VWAAl8%%wLYo{+LWiyT|QB)DYV?gVg#4=e8_+8L~Y$3`hOu{+3piIw!6JJ|v~p z;eHVEyn%HN#?XfNNyt+hVBHApIKoZ`)_rZ=swm=XZD>10Ds-t$L|nHzAtHEUIGd$) zKD8(5F@Jr{d1!(3oY1?j`cn{AslOE)Ez5V%o+hBj!AdSyLvL_H4_yzqA?+><5$dH| zwAMekIimIT_!71Z8;L!56X(ploY4C5p;pKN^K=wq<_W8Ky*#uzydGu!!fWUDArNJ# zf>>W0@^?lm!1)MUxk3l85qpYT?eH}<9{%d8pQETd*`RY`)N{cY7@K;UxJgiN7BuOq zDln<)J75l)Wc3nY=6^hQw5xE{(2rdWan9TZM`O;4tbmL% zIutxr)7$-0%qM88^y!LXIC`JwSd6G843EGLwk0kEo#NCHtide|oq;m3D5L($fJ9DO z*EAqc1hNiKv)HWnOhnH_V1o#Ls~IM2ybsDGAc3-MVNffyQoodjZ|}JGp4dAH3hl6h{+kg z+B(0oiy&cVJq%Wpi{XALEFT{3SAo+Q48>BE`t^s{L4S{4%!pUf!qj|^qN`)5eFK?{ zR~vDsz6lc>I)v7rL4US^ZIG3;$=UC@{ z=&H}gx!%?YShslZ}6te(PNldTZt8dfx0DoRJnI8FrnwEMu4p#Yes1vSXcJnT66P+QTWpg_!( zamng(4dG-&g8B@(0W8E+E;7;p+Y=Aa@4JL0?!0v9%eh zj|oqQT~rt5=Xp0{A4rx$$FXtU+_AgivV?lSGYf`R0U>WTo!a3TiL}Ii3w7H-*+pQC zqztvqw7U1^*85|Mv(W_f=MxZp3){YnB6#Kt&^r*j5)d2I^FA;|WK{PXjB^qT5WDws z0ocg%Chiiau?8NhDznq8D_=|c>%t)4Z+;3K-*M0YK$%M29w z9CsNzG2ue5L&(bigiQA}IOMDJ=PMZC)8b5PG6nKHI|=nAp-?R4X;DlCyocCG_<#mK zF7kiW;D-hLGb+mT4+{7vzz0bxqIiPQT_WQU+WglfC@5fEkc4$XWg=e}R3u#{x*urBLE0YjE2QRt;rGC`BMN|3NjAZ;oC zBM@m@L<-p+%UZq{Nr^e;!AQFc)aCJI9#!(agmSI!oeuqgIVY`b9gJu&7AQyjm zFcD)c4>a^|+W?K@e*DE|7L`CR@_YuJ^Lea9uXm%m4@x?vxLd$5Gm;0RJ%{Zy#RIq31I0XQUdT5ni~s}jH7F_kg8_ym{hY?*WSpc>!^+^#C4TiWukl zntH)IFve<3cU$2A?sl4Q)&aAMhTym8Pc!}u8fT}!W0J|~=O2HNJvR3Cn`xR)ejnFP zTws#?3BR|cpVyMJ&3_E!?fdbkxDoY?Us{Fr93I#!Kri8&WbKAldW^AaH+1;Pm{hu9 z*DPsY^FuhkL(?QwXE_BwL(UZZiHM)YyQDm~#w$VR6=;8rd7&12K4$KA)B!UM)+lHb z?m5(13)9svH^^R!PJx*bVOJ_aPEXD+Twb`paQyZ@sBj~wnn53%A7D#u0(UHmlGXN$ zpr%~knfH5U*ti9v_3e@J&Gcq?DV-M678B=a#E)0O2AFg!T-CipB9e4+`q5R5wN`pxfpm0PaC(oqR#ux76$ zWy9-r(r*;>qud_!JKMy6FUXAgzDuws5Bh&$%qRPV<$s8*?($vOTlH%>j;!jMzYn+l z=x`n}DcoYHrrY(n%=#Tt&>*322S)So5h8(06;B-a(Q?&Szm{H-ha&CO>cPr zVaK{@?sl>N*hNcb+LSPIo4)_3h@(DiOIlmODLpJ>b5egwTgDiyt79M?W2mi!2H`wF zh_L~+78Sz68nY(4B_`H9TBU8nEl-i2kC2UVZO}4@8*1U^MShb81<^OTKYjsT4t~nf25X>9JFX4D5q=Nu_ge=qa&&NB+>b=( zgh1=yWe$4I3HfbckM#nmw-RMDDAlR!Xy|ZTJ}*bJVlM{$#1s86lXYH{Fv<+7m{^t1 zM)j=8Ezl;5bdohomjb|A#q~7_x3Ta=jz)-K!3nex`k_V0n+|y_Yq6GXS3l-FTT&tC zxh>aliroWDZOfP_Aby-jyduC{(H5|I2`h;AOC{?F`H_7odkeCDqLm9)R_+6Y`52fF z>1HFL_rh!NkO;xwL*FfA2yF23g;r!=%7T!5k_aZKpp#IvQ2drSuojrw^0B|SrQO6G zlX>wLQo@^%{~6`~B=W!0;L9-(p<+Ln;7(}E^fI58sNa%qKng|!G(mWH=L~9%#2A6m zMJ+Ii+9Y6Il!SFr65czW*6Clxl3wm_QB~XZyw$>bNJvgFNM@I?CB7uwRu?T{y_S!g zL`zr~En%)DON1{AVY0t{4=k3}2WCSlD2)`dAu!X&H4;1kv?J(qhE%H{2cJ^e@xTcBvSlrWwL|5<~ z!eOCFE7wlkU~NgQU`Km&;EuK*?ze?O?39z83~nn3fsvH<1cV%)%KNRhSdu08Tl6@o z3;Qj4xDcbzT@D2|>f+VBQsXVxG6z3$N?neK;PR*uhFK>~1@x?L5#DC}XoXzbIl&nS zsM(5(mb2gR;I@f}g}9yMuxn}Dtp!4DUCG6`0Cr@=s z7&l+zLeIpUMbD>-@nz5=t^yBccyvesOB@W~FOA5ydHnVfG($1kySjOwg+)9a!C3Rx zf#GVh8Y0x7rhZMQG%zLifX!q%GGsc@f&kHM5YW~aC4fYU0#Sm`ZEV>nuQda1L7%K{ zK%HInW9Zb;I&T$g=;7%uIX3e~w(lZs=8Sg&0oBA*2i<-~9dXgwRa`UDR$NJ!b164g zB*_o(Yzbui$zf#t`F!tb#dDL$vW#Ppres-$23jg}qAX(_CE_Dr+Jq(z|$>ftj)iIvZjA|xy)1aGpDc^fOK^+(#E8xv|wmX%?{a(PaUXhZWEOmTO^lniTn);Cu z`x}vg6_9%c7&uX*Jg$*lJjPPfU>e> zsWP>-5Rba>DJi@&0ovu`n2}u3hWoqZA`b&88j-ea>X2s9h@fG5D$Bl_Dh^H3#z7h~ zQ|_NCD+@-pB!3Y@J8&O|o(7-g`PS;8L6z4=`Mmz|#Gi|rqgza%Q&=}5$>2O_aigeUO02*^z9@50z&cL}>pUf_^Ncs}Tp)R7nt1Lt z@szO6Q^GpW;Rc>flIMfPnw^B6kDGW(Sm!BW$#c-+#ez@THs)hXDHmzR)8ON(kAGdT zgBd)Cj5aniwUAV;OY&GM`CeG_CV_`U9tXdN2SJ(#n*mS!R0G{Op*c>`2Q4PArj|QJ z;MzsYxM@Uf!}UIB_`M3y@EI32_i#G+mmPl>I+{drUd>`ZqAq<+_c`94!1IL9Jwja+ z=UTKLvwDagOH5fB=XR78`;#9uSat2DwWj3*A2h>EiDfU=apVs39IX(CT5aQ+Ve-tP z^`a;3uTUOcawpI~gOdkm?r{gWk7lEz!bS1)5x}p|#WHRuAy!Yy+agUmS(M!e!R{Yf zjd_%VGx>F4)NO#-{s1bq^529aTjG&EE#?6d# ze}%H1{lwVv7lGm2Lb``KZ79AUm*qY~<|J{C#<5vu6%b3$AzW#!4-ZR|=Q(Pj3&xE7 zNI-|ljRj{~Cu8sE-i~IcSsZVLs7x@ZR=gbo!WgtFp4#LVPEf!3r(qiS8%7PX47UZzRVb2Y@t0*G00(FmDM^><0KyrXzO)V`@zm}+GM&XHZn%79yXd&xO z$YwiSCSaVWy3zK@X%|sJEKX4oEGXP>6OQ?_72R)x6;1ZU>QX_PG)knFv4Ry?dlJv( zdp8B`%4qrHCO<|FCJu5Sii<50%PyGjQwVt{$cK;O| zc11oJCXP1zWMiD+O~xEQKLFja?6T3}0Up7L?WC>%NBRb-E94>zHskdcENk$XwRr6S z;RHH;iU=pj^C~d^Ophn5CySvQDz(?lZn^_ zy)N?g9j1i!jg5pcl|!mg)|?1gWZblC1&TK1ENhEN*0m;C64qr&m=+|lcQdU?=FkY4 zXg*uo2?Av~QY8O`@Xha1E_Mg_6S>u1R8!!q*W$F!OEWE?Q$?GyJAn>=bS(0xcw zjX?!F67=!SS~Ot=&~RgpCSPFmPGkjp47my%C)7{L|cc*R|w$2r*fclV(d^c2E;} zw1eKAP9+8%z);%8IkxygC`LzkG>T9v`mimjKkz8|fIMon#VZ1Vy#ml;Izh!)mpmq< znWnvs-dDo=K;<#C8=IemmsZ=m@QjN%hlg%_{+JxU1>J;dYl9jrR}P3tw|RtqjjSJb zinNe#r&sB44lK`nF-qr)K8UVND-QVj1m4?;2L&e3WakD&K5e*Gw4W9E0<$=eHpdst zu<-=@zY>p#Mh%Z59?=Ga^l;w4Y;xrJ{*xwnu?fD%1pm$i*AT452dKV?5x60`uB%V* z@>|aXifOhsP9xz9F@M$pxCP{)~Ijycn39D@9~Nw$2;tYE#FhTqi7Nv z<#6$iCt=PW;~nnnxNI$i{+Ht&yXDb71_Q)7Xv(D>V^YgR8bL&KgCiaouP{<@+#~6E z&Jo}Xk8@b@dp$&BVH_+{IJd7|1 z;vKl!Ftv6Mb>LhKn<$QT;IJl{h_D`y`+|TL*uyH zvSR{Vqg%{l&JklCIMl+)m_5tuGPB30Pku zN?2dENtl+1k@1exG_jbXMZ6=yBxFDSlrg$+kL5VN7kXYJ?)9bcPux94-VY*AUkdLrl%w$tb&XgGe@!?n zgZ~rp4mY>sVdEW3|ISP1Iq9h9A}!wWlos!RW|%8D-hur*1~ZrX^TrgxS zF1+ELjME4Vf^<@X;g%nTM7(2}qZSCY?MW`i1!!*zaoWxj%U(COhbpc%7E-`cv4c() z!4ljY#PtEtPigTE%numi6*0trZH#&Fq$pytddvew=`lAM^T0UQd&~pp1v2I_k*wD} z=COv>#m1NiO7tG{aR1G4BD4yYfF?5LG5#u^pO*2a0riW|1p>VEmQ_SN`Sw@d}&;*S`k=S^V5gGHaOC)Wh(RP?P)j3ZX zszxh4t{Uy`Cd;~2yD6h{~EH3HUS z9`_44_9l_yc*A44hwpocc}x-+I5z1P^Z5JYh8n%dHNu1_*AcN#|5mErj2zS<@h%{^ z&URE}Be&cV&gvfX7$h{?1fd)Q`GR?cV;;8&IDWH8(c+$@hxtB&9_2ljZ4%#m%tI{i zM9hO%hiEShs^ORi6Tpda{=7(38Eq|J_c}#{hWTyP(N?BQ2m?Yjjjv-9nJ;XeC zUdYGWybu|GA!?2;Wr%r9WRBsOhlFv?B2MciEYJOIX`c$o7_!4Lk5dJXmJhTlPU&yr zDPf(bgms>08+a}d4V^OH#PgtlF)oQ{frNFQ64rTU8+bNJo{x^#>?HJj&csu~I!_5p zp5d6sUgl#o75oMGk2UJ?4@1SKa3#V;+B|5eR3!TFe8T3{RF|UXmO#I&q8x zfikXv^%e!q5Dh_PFzjPnvXhFxMSn0EyJtH}z(U1kY0hQVaVzQA6G2u_;_HO5#_a;a z(~KL;D4w(sYKYPvOR8{Nj)U;?(20)-dYmfpK!h`fN((%flTs6L%`!QiRu>cxJt;<10Fd7)&m}yRL~UgVE$~yh=9ixf;8z$ky<7( zD?Fo8fXZ;#V+dm??D1a@c)SDGo%}O3M)!aRo?7^K10LTzOz!(140x12%FA3nNdC71 z9<5{8$&M88h(hnh5NP=kU81*uNBtPQgwY(Vph2b-?v8*6-B$3ela7QeRpQ=G_tV6E zD&5Z#_fEQ>EAI2?{&(WOfbMS>_W`=UlkaP5F}D0`ah93Xwdw7yaGd2I0)=T_Y^aI_ ztnc_Hu?fNt4N6$w@kyArmyvOn_y}3FtV!$4*1D{aRb`UZDPVmUEn!`jgn3vvkM%Oi zTrUL3`qAbx?FoUh{6m-RN!h~pLWh0gUf*0kV#s@l^$EvW?h`P_i0&5m5u3|9h5r9; zoMo~2Xl7DfmKJBp2;lZeaTc)^#B36dvm{UqK|7zON2)hD$?rMdk}**7w8Nd?6&!Ct zpo7luI)I_HjdN^Ck2(on#_<+VF4E#FNeg5KZEjd{9&Da;C$NCJM_t5GX?sy*yybCx z3$y(PJ>H^6Sm5$fA39G2l;lCTCA}J@Uj2ojMZAeyu+n;RI0$i7_T{>sc<;j%Ff?oyk;cIXz`i@!%?wc;x)(oq6!vXhmY56 zhKGth8D3%O7O#0zYbs=!Zt)tw$cT*B z1SHZBuSqmk?NzO69IshTP!LFxx%{xLHSlMHFlkY{mrbXZ(v3tB`m4WVW zLi31tO|`)NyYZT>^}5d;W4vbcg=EfuC0=8L>Nxy1#A{99-(HrA6O@e`l*Nh{~kHM1G>e?z?HwE5(||G{|8E5GMu?!Om$vibeCat&9_WhcxZ0V=V_N1K8_hNH-g}B!@ zm%Eu>H+(&FKw=nMCelJFg*SrTO74OAlG~@O^5wA)975Tl0*W^o{ zcDNI~;&Abrz30(UWe@QhRc1uSYqkLUUy0W|Ihgba$7?)7(Er8pnrO7mQN(N7&>MP) zmn?Mfar!)JVSb;)1m9tTD^2he6FiaNhGAtto6n|h-oy95Qn9vx0c*+dWDgKoqR(YqukjZC$9TdYBVI~vR(~9gMowZw z4=W77NSllvU&c=gBSWpcm1k=D`U)QD!kj~Ieq*p4gny9j1OfJ@eN#Cd@Ngy%etYRE&dD) ziqMjnxp7Qli$7OXuoXy4B~c@T-T{1%Zkj=1k@D%=JoGupxHdu#fq3hnb5IJOKlLOg zkv7O@B7Te4I%qUas$LgUEP{vy&6}=%%FDl2a1eIM6xs!7ok1}qw4nEyM#rfG* zJj@yZ=0!&>JP0?l`AuZJm91VfM=tSf&{&0tok%eQqa_p6naC6oE4O97ol4>p2{g^4 zJVfD!U+lJNd=Xusg%eoS%`QN(XM^W(O3$Tay& z+AqCN8{3Up^ztZlc55F%pm(6RldIsHccS?Za$Rgmm!m_0ecClhrP*z<+r`=-{&mWv zW^Z6Bd{r5(yN>=qee9FaL>o{zX(VQ9AjlWE@je0&dlghAKjbej!zi|-vp{AYbag83 z<8RV9;?cviM5W$?!gy^yc^mRmc91VnGl3OZ)^ZLPaC7D*;+~Er>8Y_DB<>=C!&94h zgWnqW0R%Zo(1}72F_*8i$E`rKqo?iJM=fpvQGnWyKAMJ?3KlwQVVYv=$^7y;-pF>7 zF$>h++`?@QnCytHdGcEI55T$V`+@_Zt;wJROY6K1%pqB@LT=K7Sg27?#Pj_860#+D zCYyT)He!jbJOIY;LLO>|?-GEJzM8RtZnop585iDf@+95Plx`deCMTW7BY~$4&s^eF zd;Yu)zOMwT*d=gtVEJM2XhFq?g)w~3cs!v0bj2n=k2cjUuFxILXfd)qc;FwcK}|EZfE*ZnBrlG>;eh9cWh=xk z0SDZpRXk~{#Xa5=_rxydd-27G4JZv<2ik$Z03l&K5*-1<-32@Z^!Q*xBh;W8;gt%0 zeF0&1PpqAIp(}K7HC8fb_dBFHsbJX~k^O?mrjJ)FK6pBTk5AR`#2lg)%J-m>KY=80_awe8#s&0v`oukrf9^Lx zco%;+lE1jIFq#c5J_k;b82UV26H>H<*rKTYr*X^i=jj&lYpPHj&5CArqF!_NsAH>{ zVi^}iW(P8}=-FMHW%jAW$`hnD6`B@f1@({Qw~JlkwOx9*m;S|tiWckOyBxI;tPTz` zH{wpOgc6SV+3000plBY&_YzPri(e@gYI3#q6D{uMX&>5JB#YxQ3H8yMutx&y1c;G_ z$K}uZq8#QkF>HK5{fS>e#cKey#1aF`#|g~Z16RVJo{oMeK7l0P0RT2}9gi&d;#UH+ zo9p=3z_v03ghx*yZ~px>9Txikk(6>B|VlhmLba4>byNE zFY>*>&k%UX@FYHEzy|~#vM~N@cqtP{Em+T>xjktfWI-~0mV)Ywmd8Ii0V~i{MnjDy zLFhoAJ;KODuZQ1IM#BQV-{7W~Cz3xxd9gdkSW45M9WP>->(uMv!VGNC*tB`Hbev8&PU{|F%KGpD~teU7_Di2 zP+LGqE7Y*5jaB49zedq@+?QyOM%D!0KcMoc$Gd=YX;n|3R`k&NmCjmdvp`?PlMbBA z?;>J)LB;sZU-6L{?iFgi=v%;}FfDr4v2;}b2Iv5oR#CFhp`sR!bRR-xxHFw0Z zewJFKy&nYfhi&-5?IrMMc@X##--?64SH!&?`dD5Q_ZWtgEi1%5>1kOb?(^t=y|^!+ z`?th>fbQSr`&#N?I0#&dB;o@(;yWX#Y+CC{$l68AaW{ydI@ZTv5NpTCoD)KG+br?% zR5dI=T>6%dL0kLQpZNJ#5Fryr<2QFen9zO+9RO3A<*!IG41DZ_F;HFyF2_}f4&%V z;?bN}1>nFvoLB7}#77bH$X;|_wb=x3GQrDD@bf14ae}q;AbI|C2h1Nn{|SM@bpBHW zg6`)(6M*f0{-d2&EyY0HLw%malEZjjHPZxp2-ftY=ZB5?Qs4Pt9oF|!7|x{cY7lhS zcO7~IE zZ*^r^O|Z1G!dvaHsP;~;3VH*+Dqp$Go#rd6@psFLEOT?f8}yd>%Dv^K70T4I$|=6G z(pf&{SzJ?A=B*C;1Ky$K1gyBk?a9n?OOkYKjzp5IteT9Yo zGC#C~B8mZv+DQ*8fvrj^%ZkX#8lX5aIQ0A*h8F{5rS{^9#7Ww?%;+l#Gz)P++!#ks> zGU%NZ&M{zXl$lljVr8;2X@)YXMky_tF=d8QhN}h_Lw-(bfUE(sMj23981Rx3fhs>) zrY7jO7yC-f{6%LE7@$60iux-_{{BiuWv~PuYp?bP?KAzs3oaNyW14B8;8BT3iZXx2)L@AioP0&T068%? zoVoLgk@0XOG%9K1=V9r}Yk2S?+9S}V9u+_IJ}vPtLF>6%Do*l@=GH zx7drTD$DJ`62HB$rmD(c5iFZ&_lcp_UvxMIXAj6L^ZTm(cKBJ9AM)+g2kjO9>0n(A zR8S7-c62~{psI3OX_4RVV}fZIXexERsK1p5s?X*|hOxLG75W0zHD&&kkv()MY0z$V z5PEodEQr+MG7@VrkiE3pe&xj6T>F%oAXh$YDGUphL2^Fyet#8Qx4Ku09aW(k3?ozR zg&5K8Q~eckP&(T#jdv_|84%gr7_H<$gYmh@du8%qJNk{CMjL-Q8q!}h#9m!mQRoNS zj?Ne?^_4*njFA;UqjT{HMwY-_$D@O0*9fl?bNglXVAV`26|Cf5nN&(FedW}@;X`Ol zOpN}j3W&fc$1_0n1@>$+&R*iHwnvmDqEfQA=;Ne<7#?Uuwv(%phQUgR#mrhZ1kFOt z48JQyT`Rcpf_~H)9w|RKu>9ntoWjWTc>pb1n;Q`s4E(7moP($-NuLPstl4 z%Q`u|albQV%o#hryyUb|5B-ULuZ;TVYaea?eD=o~4;Ib*;?{$YX9#+S6Y(U4F7fj? zwL40qaP`53Ab>(gKNeg>dmJwQwYi&R>MZ8pzU49*+g_4%p+FMeQubkwtF zzIEXrp0JJ@G0XP#fRE<99(_w!`-BOzMn3b!C#lbF-#+r(!e2cx^067q*GamAYlc6& z-SOxs=jgfjwGOr|fB3ZX7x%k;;Hc!kWWAB`&Wurgo$iTAua+#oS@OF=(*5)PkDHz> ze00?Gb#D)Bd-jp#p7U03z3uYS(p^00?Mo}Ke{WP?yFN$}0ii=G%|^7@1N# z($CngMqF=x8G|xMOUDA_A1b4jBOk|;Bnpme6@=*&9H%RI9znsAJ_YyCpw zr`1QJ*UD@4)B3;GueEVN>)+aVp!IW2o>pI3|B+h%)%v^EkF|cU^=GaBoBOr2$0)5o zYyH&RueJWI^%+kzia)M;GX)>vEq&6 zLr?sSeaxt{d3-R^kq+9=$ls{{FXNA}5ZQ<9uK9u1KeYa$^-s;;wei5zU$yyHn^(2@ zQkw@~*UHQO`nv3|+B~bxpUY+cSuXp{DA}L2d3%)XkFU%AI8vjN{cE|_?po^Hi1`s$ z0j>b9T3mU!RCy)5hxrlrF*4Q4X(`#~7ylR5=&65F&avRB57MuvGUh&fBpM?<-TxQn zUuDUcU0qGMT5+}E>cG{E%l(zO?m$|CYlcjXV-(f%7yl9a8?r@D?Rl*D|Bv`jE!9g= zI^=j{-v4O(ppnhm`bJyNY5N^*{S$S1-^(4?B-Vjj?O5#JaNjx(K8LgoX)UFB*!v+3 zxG}CFtws7X(k7&F*jxrYU0p$>P1kmHEkW9vk2L_&Hl#eMPepkcwhifir1lb&!)CN8 zfHeiuB}f+_ZA1DL(%LGlL(tjUkPbm=!y`j3q;{nFNS#OnNDGk8Lz;>Q?3N(S!-IVT z(IFaLYO-H%}X)e-cqy2hxQ|mFeJv)P{5&QajR52!9*&zyX^L zsS~Lk=>(*yNK249kg!Uj9sSOLLB}g4en~|m?ZAF@k zv<+zi(hj6Sq{>66KT;dgg-Gp4mmp0=x*F*cq??c`e@1&EEkLTo0*^EfX$R6|q^S#G z7o<+46Oh&-EkW9dbT(4u1=Js@4JrNe!FHtIAx%XZa{|gCJp*YT(p01cNVAXzkkaS( zYmt^AZA4m&vv^Nz11 z8&qu80!&7Ln_j7z5eXb;aEf3^bFw5`HnKTv5d?8(PnKj$Shfz`DcFL*wRnI`1SrHz zndFa(32jVqK#AN@;6?(_D&&I<3PT4Nx;7sNd&(pJ(;%d%N~JlmD7` zMtXKX`@GNlywCe%-#zWUYFdKpkjo)c&mu06X~=HKnO9MN$P(lXWEt`xWCijNWEJul zr1O2$zX|n#Tnd?hY=KNcUIUqi%s^%!2O!IkyC4&<&CN|h79ppJLp}}J^aJ!W$P8o! zatiW&$Z5z%yaF==xePMtiLUuuxAvZ&&eu92R9C8n23Gxw0N_b67rMiiYPTj7ix(gPc-Z+H` zcR46tYH?cIQFqJndroU}Tkk%7ja#m7NS=O% z+X~j^wm@f<+l)M`+{R7}4Qe*$AbyIQ=H}2PRHCvE;pYx8RlhE`=~}l@`BMeo1O5tW zuBNhK?}3I+DtlG1><%|wKZcUqLrU&spuCgFbF^n}?m~J<3Cp|2&DD2Uc@YJQ!7TC~{^Hymre~G* z;`d>&LtqKaNk&~&oNMY@tZP`k(7ms&%blugXsN@|Dz{j73*>r`F~|t;8(y&Y~g z>-M&}nQIm44J)A;Cf|Vl3hbxJKHC1~Gu*~Dbe0jER&i}XALcLISB#j1wLkh^-Cd|g zZ9}hHH0(~**ONlS;_%M74&_n1WbqKNba;Mj5G#N+fn9`SXT95$g}4vw60omxzj+YW zWlTiIr2a`2j&&*=cd6r>!ul)hQ(TUrjGxdGr(Qu_ib!Y{94orellAEJ_cV04iMton|E1g9 zu7)z^30$6p?yl`~bLXOsjJ65uuWQ@`iEj5m8b8@?w>sGHP+jO8LCvn_C|ciKvjg`B zoKxLLQ8}uIg8?%DPTheLFK~}_MD|R0(QV+MX;wWwik@nb~J5KqB8b@?YG!-VEe$# zb}EBC26ns3sq-EIqqQGijaG^5(Ry_m81*|!#1^9hLzZ3wtNbEcu;&a*??vrF3Z5bRCfA8eCj zVE0*!9)9Kk<*`5nfV98)&a~b$iaI-yAU`N2r_Ur&_ zdT`!vDz6u8F<3v|N{`c?AD*N=_rcyJ_!m5ALY1h_&w;gAtPHjs%xupiU*`5vt%t0`-J(q%2!NhNB&la#_V85>R+y#5| znY!7YlVFV&n+96|X13?kV2-6X3r3&Qn|g=Aj$5n>_P)gyEJl675FV8%R?T4adB4dn z0XqsNaUs1l*by+ZJ$t}1@UL5&Q?VZcn*=l4^A4~>mfjTD(!UF~=VM@LgOOR)|G|gA z&9*Cp?FBR2_z2h(nE2g|{Kvs2!TyY%|K{(X)>?z_pVhjCht3S0Z~gmc7YaP~{a|}; z20Lo89M};svz_*URV=+nzz$n_`@s%btOWLk#ohp;XH>K8j)J{vv5&!K!6Yse(z|zZty$>eilgcQ7CGZR^4CpuMHxW<~$gS0ZNU@~H zAxe+(;!VzPHWrk&z zWsYT$t{uq?4Gv#hYJ zvUFB)`79GGQ!LXgGc2<#b1aK2r&vz2oMBmFS!P*bS!L<8bNMV2EK@AgEHf;#EORW2 zET>pbvz%dBVp(QcVOeGAba44B6D(6K(=6$KyHLuqY`w5fov&$czdF%!!mjBjza^JX&d85VGGC%%b zLH&N_&6fUGz{AS@HtQGP4cd8`dDi0ZG0#}MxnBF9v-lU7J3rI*d$e%2GS4!vGQXdB z^;poKr@_PgKP>it9@Jmhp#7=*Pmm{>mo0uH^Qy&*V&^?=NBn%8`Edr)UavDRvcA;! z7t9mPv)Y_PpJj&CYaKZC2j|0}|F<%)d=TXHcT>_YTm0{tS1tYy^Wyv3j`(>d77%2o z!d&XRl6iu8R%dg*1Rm!9U96uz9`utwJ0$xFi~l3@CX2tzJZ157upS{hS&onR)5<*Y zzq(xU{|}j$SYPV<$ILTUoYBog>-8O_f40-Th%|yJEDp~=-*L`zrrCeVF$-SrEOBHV zDEu{LAFHZF&^~=;Scg@b%zL6=WiIo$@Lwu^u4AtYPQwDN-oYv`Et*==XN_lLeoH;1 z&t)BVHS>&GWzN3?+snNCL{R@;=A{=k@72Qj9`oX`2I}t$IDC!yv}NZL(YMN7h6Ux> zY||8%DegMeotn1t#SP4J>`yoI+av6JgY`@754xX9&oi&=)#vhz|1;)Ohcy2!Eu8al zK=CX-se!Ef+lAktx%yie4*Qr-ar~v+N#==%wLVT&dPX=uN651)y+;rhpO0AIVgDr# zpFvj$)9;Aj^gaUBYntmT&*gVU=+ozBVfJ5&;BT@0Vpi8Pt%dUmbNb)rl$L7YEL()3 z!pElgEI9dD+OO$$zIe0ZrlvmL!TO0e^|{1{-hUuF)h9HUc6ovMl+{1qW}bOL>!Ta1 zbk<^Rr)=4$&#TCOncGGB$#%s#vmxj+s<3!I5W)Ac{jBBBzeMQ2%lZ|ozRTbk`8oAn z?aX%e={n|Fs~^()7^Gj}a;4us!Mx1lLip>78x`flpGWZ1-0(Og6-T+;F_B%R8=23% zq|ar%+!JAk-uoc`r`e9^Kg&FQhqj~sK7zv^ihZu{LVej;xJ27Y@pzH`kW!qL4AI5< zmE9VOoo&K7@Y4SuiLmq2=J5DD6`}up1pn6v{vV3FPI|xgQ|5stELp<(=Q8kwQBOH+ zRr&~?6(4%9h2o#)@%{>DxR?11_fLuEvx=a`pAG*^EU#^FZhnPJT@(!#l2aXvA`q}bv17611$pW=By`rBK~XDt0+ zfF}&Qa(FI+PjM@9JTYym^m*nvtG>g`QxjTG#^cu%=gfw9l=ZXLxcJ)${kK`aWcBBU zbM*IS4AbiH)8J~{as0)fYZSM&9cP&JtCszHnP+Bo9`(684j*Tp-lX}3cz&StPs}S% z>T~HQA22UIq`CBy1O_0gR+BDM9Q~H#DdS>8g#LEccUWKSf0cPDtBX+Yhv4uT=2bR) z6?^dS%o996aP^#DGA}Z}jP-x-T<+)0p{CN+%%{1&(tr9DXC*`IV*T8k8us$FKWCoi z_)9y!!aTwKQ|kL+g#G28(tc*y9~m##Gq1j&3zRs2g?X8|`2QW|P1Zc~y5dHK`0yy} zCtlK^oB4TInp6KQvp-U9C-ah3?oQ?@o+l*^f66?;T>N}R@mha=62U)aJ4G&6;&XAc z_A_mbqt(o(SbrfGHO4%5g9gGMWuDoq`4z1HGV>JM$1+%@cNFInLwpp$m!7BnoMAh1 zzfLi4;&CDI=~mn@%!mD~?{GZD|0(7@5ZkocC0x3mEvsF5HGU+4BHWZs?00gPb5C)-~&nWvuUF? zqTUb1;Y#Myp62SENE}`-`qq4PhvHh-{5{F~GkkxM{`O*o{*PEcXZ0W2HJJQN->$7o zKTk5xTJdrE#|rtvICe(tcd*gS;5O`9g56CDJG}mp(Vf@<$nyq9{e4uh!9^b1!|ZZs zkAM;ElQ57>g==_QvB{6uH#XL{+rxMK$96kgX=ep&fP`%Yki|Gt+W`esqqn1}*%rX~ z+W%+{TlsiiW@Beh&rROy8#c7}ti5i7=OtH!(v-2sfVrCu0x@5TdBMv^W%2TtN9MUA z^2&;+E5=?q6Vcw7S|@NVs21sGP%YBwpjxaNc=^x8si>D<8Bgu^W2lXL@d5|Ec%f5@ zae}852M*oAgcyZ3hi*l@PibCTx zMLsla0rw-iXiz62WVTXh*R+*F!(}Ul2Fq58QMrgf*_J}*23zS$OyH|Gwyp2CTM%orhZD{GKZZsHX!>S&FCOytCfxyi@4g8PQ4ndRXqM zK7ekFb)edja4SZ_cHhPo$(6JvrP_Im_7~K9FZx8)!|U(M_YDktdj02DRXN9OKJwC6 z)s}9?CW1x~SVi@!e!Wc_yxX>eb_AuJ6EW+|+w-esMU0xKrhCutSr}#^EHH5=gb8$0 ztK)DrXey#%OyLMCwZ`wTDZ#I$?%AsvV)s*1To~1hrwY@5il?iNn zFM_&P3wPCwC+$ec-)8UDDlZG`X z`n{;8YDOgd@@f0NsJIyVCy(4jl$AhpL{Gc{HRDDtp8B&O?=>3XQEgWW+#{kRE#+Wu z!>z;HaEHKVwWGWJ#>EsNV@5Oy*=q4`r!pgpHOwH6x3*_JOv;A>SVYlB0zG|OuyhKk zuNAVeDTOOvv$toMxtjh$(;W|R@sf7>^D7#b_y{#^7yl9T< zU~SRXJS6qjmiRzqa4;|mBPwtTi^K`<v2k~*6 z+S1=;JEFJL%)!C>g=~Bq<%JQkngxGwEb67OVJGZz?Mj)o)G#(OM+XsGv+wlhY!wul zp+kaUxUbgvviJ@>?)hce+Hsx={4Ou8>hDBPo3mp(b}SqHPOX>O=sU4$dJ2=&xLzZN zSIh3JUgS(;1;C%y{JP0=O8;m+-b3XnE^K+NAG;ng73p#ClpaFiN7(Z;&z}+wb2)h8 zsWlMTkap!!AX8!7rlfzA$p!P z6RpnUuEuND1l%+{P~_u@h)Nkg({1F$3qng-#%R8PW&c=UyqvtWV8ui9t-?+;o-Eby z0bVnWah#&oTb5BZW<%VxTGaSRCyugzABv-V;+fv^m%D}ZDD~`96N2dO8#!dDzW%K* zaFYu5J6av6b?;rN2*F^KR3dGQlOvHUIV9ft0*GcAg?$^IV4B;hX#YmZ9B)4_&*6b zeAu8(n;blxowmW@`L?OvVf(Ylcc}XLwi*HkCf!#5dUR_dE)R?3&}4~uN#43f_-|61fDv5!cx9`KUm}tXAu8IPvj8t(|3>MT)uxS zGRuXL|D+@SGj?vn0oj)P@_lEK@_lD1U-C=+|A_Oi5`!!GfTeujn)1^-FLwT~fl(Fl zH6NYQK}r3~OyY;IdvIXpPo(t~k!p`UQHGTFp+tTi%*>zb(8?l3PfW0F))sjmUU4&%^dDhAzyYby7^EcMgY7+&tj_m0 zbz+>0owvZ2;79Vy_inQ>^ZzIEQs0&Q@;$X7&R@}P`1=`1xzdggBQL2){;uEA8M>Be zc>veUB>6>Fana20f8VZ2=WmsUVIz`H4^N= zs#wF^13)Y_kJORm|E$F&zbUOMsvJ2k{}H_ZObM?!I)8v4@n5<%y{9O?l5&O7yPRS9y)LcE-9Xe;UHuar{2yICD8>K) diff --git a/control/velocity_controller/scripts/build_auv_solver/main_auv_model.c b/control/velocity_controller/scripts/build_auv_solver/main_auv_model.c deleted file mode 100644 index 800de47b4..000000000 --- a/control/velocity_controller/scripts/build_auv_solver/main_auv_model.c +++ /dev/null @@ -1,189 +0,0 @@ -/* - * Copyright (c) The acados authors. - * - * This file is part of acados. - * - * The 2-Clause BSD License - * - * 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. - * - * 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 HOLDER 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.; - */ - - -// standard -#include -#include -// acados -#include "acados/utils/print.h" -#include "acados/utils/math.h" -#include "acados_c/ocp_nlp_interface.h" -#include "acados_c/external_function_interface.h" -#include "acados_solver_auv_model.h" - -// blasfeo -#include "blasfeo_d_aux_ext_dep.h" - -#define NX AUV_MODEL_NX -#define NP AUV_MODEL_NP -#define NU AUV_MODEL_NU -#define NBX0 AUV_MODEL_NBX0 -#define NP_GLOBAL AUV_MODEL_NP_GLOBAL - - -int main() -{ - - auv_model_solver_capsule *acados_ocp_capsule = auv_model_acados_create_capsule(); - // there is an opportunity to change the number of shooting intervals in C without new code generation - int N = AUV_MODEL_N; - // allocate the array and fill it accordingly - double* new_time_steps = NULL; - int status = auv_model_acados_create_with_discretization(acados_ocp_capsule, N, new_time_steps); - - if (status) - { - printf("auv_model_acados_create() returned status %d. Exiting.\n", status); - exit(1); - } - - ocp_nlp_config *nlp_config = auv_model_acados_get_nlp_config(acados_ocp_capsule); - ocp_nlp_dims *nlp_dims = auv_model_acados_get_nlp_dims(acados_ocp_capsule); - ocp_nlp_in *nlp_in = auv_model_acados_get_nlp_in(acados_ocp_capsule); - ocp_nlp_out *nlp_out = auv_model_acados_get_nlp_out(acados_ocp_capsule); - ocp_nlp_solver *nlp_solver = auv_model_acados_get_nlp_solver(acados_ocp_capsule); - void *nlp_opts = auv_model_acados_get_nlp_opts(acados_ocp_capsule); - // initial condition - double lbx0[NBX0]; - double ubx0[NBX0]; - lbx0[0] = 0; - ubx0[0] = 0; - lbx0[1] = 0; - ubx0[1] = 0; - lbx0[2] = 0; - ubx0[2] = 0; - lbx0[3] = 0; - ubx0[3] = 0; - lbx0[4] = 0; - ubx0[4] = 0; - lbx0[5] = 0; - ubx0[5] = 0; - lbx0[6] = 0; - ubx0[6] = 0; - lbx0[7] = 0; - ubx0[7] = 0; - lbx0[8] = 0; - ubx0[8] = 0; - - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "lbx", lbx0); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "ubx", ubx0); - - // initialization for state values - double x_init[NX]; - x_init[0] = 0.0; - x_init[1] = 0.0; - x_init[2] = 0.0; - x_init[3] = 0.0; - x_init[4] = 0.0; - x_init[5] = 0.0; - x_init[6] = 0.0; - x_init[7] = 0.0; - x_init[8] = 0.0; - - // initial value for control input - double u0[NU]; - u0[0] = 0.0; - u0[1] = 0.0; - u0[2] = 0.0; - - // prepare evaluation - int NTIMINGS = 1; - double min_time = 1e12; - double kkt_norm_inf; - double elapsed_time; - int sqp_iter; - - double xtraj[NX * (N+1)]; - double utraj[NU * N]; - - // solve ocp in loop - for (int ii = 0; ii < NTIMINGS; ii++) - { - // initialize solution - for (int i = 0; i < N; i++) - { - ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, i, "x", x_init); - ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, i, "u", u0); - } - ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, N, "x", x_init); - status = auv_model_acados_solve(acados_ocp_capsule); - ocp_nlp_get(nlp_solver, "time_tot", &elapsed_time); - min_time = MIN(elapsed_time, min_time); - } - - /* print solution and statistics */ - for (int ii = 0; ii <= nlp_dims->N; ii++) - ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "x", &xtraj[ii*NX]); - for (int ii = 0; ii < nlp_dims->N; ii++) - ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "u", &utraj[ii*NU]); - - printf("\n--- xtraj ---\n"); - d_print_exp_tran_mat( NX, N+1, xtraj, NX); - printf("\n--- utraj ---\n"); - d_print_exp_tran_mat( NU, N, utraj, NU ); - // ocp_nlp_out_print(nlp_solver->dims, nlp_out); - - printf("\nsolved ocp %d times, solution printed above\n\n", NTIMINGS); - - if (status == ACADOS_SUCCESS) - { - printf("auv_model_acados_solve(): SUCCESS!\n"); - } - else - { - printf("auv_model_acados_solve() failed with status %d.\n", status); - } - - // get solution - ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "kkt_norm_inf", &kkt_norm_inf); - ocp_nlp_get(nlp_solver, "sqp_iter", &sqp_iter); - - auv_model_acados_print_stats(acados_ocp_capsule); - - printf("\nSolver info:\n"); - printf(" SQP iterations %2d\n minimum time for %d solve %f [ms]\n KKT %e\n", - sqp_iter, NTIMINGS, min_time*1000, kkt_norm_inf); - - - - // free solver - status = auv_model_acados_free(acados_ocp_capsule); - if (status) { - printf("auv_model_acados_free() returned status %d. \n", status); - } - // free solver capsule - status = auv_model_acados_free_capsule(acados_ocp_capsule); - if (status) { - printf("auv_model_acados_free_capsule() returned status %d. \n", status); - } - - return status; -} diff --git a/control/velocity_controller/scripts/build_auv_solver/main_sim_auv_model.c b/control/velocity_controller/scripts/build_auv_solver/main_sim_auv_model.c deleted file mode 100644 index 3b036370b..000000000 --- a/control/velocity_controller/scripts/build_auv_solver/main_sim_auv_model.c +++ /dev/null @@ -1,141 +0,0 @@ -/* - * Copyright (c) The acados authors. - * - * This file is part of acados. - * - * The 2-Clause BSD License - * - * 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. - * - * 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 HOLDER 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.; - */ - - -// standard -#include -#include -// acados -#include "acados/utils/print.h" -#include "acados/utils/math.h" -#include "acados_c/sim_interface.h" -#include "acados_sim_solver_auv_model.h" - -#define NX AUV_MODEL_NX -#define NZ AUV_MODEL_NZ -#define NU AUV_MODEL_NU -#define NP AUV_MODEL_NP - - -int main() -{ - int status = 0; - auv_model_sim_solver_capsule *capsule = auv_model_acados_sim_solver_create_capsule(); - status = auv_model_acados_sim_create(capsule); - - if (status) - { - printf("acados_create() returned status %d. Exiting.\n", status); - exit(1); - } - - sim_config *acados_sim_config = auv_model_acados_get_sim_config(capsule); - sim_in *acados_sim_in = auv_model_acados_get_sim_in(capsule); - sim_out *acados_sim_out = auv_model_acados_get_sim_out(capsule); - void *acados_sim_dims = auv_model_acados_get_sim_dims(capsule); - - // initial condition - double x_current[NX]; - x_current[0] = 0.0; - x_current[1] = 0.0; - x_current[2] = 0.0; - x_current[3] = 0.0; - x_current[4] = 0.0; - x_current[5] = 0.0; - x_current[6] = 0.0; - x_current[7] = 0.0; - x_current[8] = 0.0; - - - x_current[0] = 0; - x_current[1] = 0; - x_current[2] = 0; - x_current[3] = 0; - x_current[4] = 0; - x_current[5] = 0; - x_current[6] = 0; - x_current[7] = 0; - x_current[8] = 0; - - - - - // initial value for control input - double u0[NU]; - u0[0] = 0.0; - u0[1] = 0.0; - u0[2] = 0.0; - - - - - int n_sim_steps = 3; - // solve ocp in loop - for (int ii = 0; ii < n_sim_steps; ii++) - { - // set inputs - sim_in_set(acados_sim_config, acados_sim_dims, - acados_sim_in, "x", x_current); - sim_in_set(acados_sim_config, acados_sim_dims, - acados_sim_in, "u", u0); - - // solve - status = auv_model_acados_sim_solve(capsule); - if (status != ACADOS_SUCCESS) - { - printf("acados_solve() failed with status %d.\n", status); - } - - // get outputs - sim_out_get(acados_sim_config, acados_sim_dims, - acados_sim_out, "x", x_current); - - - - // print solution - printf("\nx_current, %d\n", ii); - for (int jj = 0; jj < NX; jj++) - { - printf("%e\n", x_current[jj]); - } - } - - printf("\nPerformed %d simulation steps with acados integrator successfully.\n\n", n_sim_steps); - - // free solver - status = auv_model_acados_sim_free(capsule); - if (status) { - printf("auv_model_acados_sim_free() returned status %d. \n", status); - } - - auv_model_acados_sim_solver_free_capsule(capsule); - - return status; -} diff --git a/control/velocity_controller/scripts/dynamics.py b/control/velocity_controller/scripts/dynamics.py deleted file mode 100644 index cfd1e220b..000000000 --- a/control/velocity_controller/scripts/dynamics.py +++ /dev/null @@ -1,163 +0,0 @@ - -# auv_model.py -from casadi import SX, vertcat, diag, cos, sin, tan, fabs, inv -import numpy as np -import yaml - -def euler_kinematics_T(phi, theta): - """ - Euler ZYX rate mapping: [phi_dot, theta_dot, psi_dot] = T(phi,theta) * [p q r] - Avoid singularity at cos(theta)=0 with a tiny epsilon if needed (handled later in OCP). - """ - T = SX.zeros(3, 3) - T[0, 0] = 1.0 - T[0, 1] = sin(phi) * tan(theta) - T[0, 2] = cos(phi) * tan(theta) - T[1, 0] = 0.0 - T[1, 1] = cos(phi) - T[1, 2] = -sin(phi) - T[2, 0] = 0.0 - T[2, 1] = sin(phi) / cos(theta) - T[2, 2] = cos(phi) / cos(theta) - return T - -def coriolis_rb_diag(m, Ix, Iy, Iz, u, v, w, p, q, r): - """ - Rigid-body Coriolis/centripetal matrix C_RB for diagonal inertia matrices. - Fossen 2011-style, for 6DOF body velocities [u v w p q r]. - """ - C = SX.zeros(6, 6) - - # Angular velocity block - C[0, 1] = -m * r - C[0, 2] = m * q - C[1, 0] = m * r - C[1, 2] = -m * p - C[2, 0] = -m * q - C[2, 1] = m * p - - # Angular-Angular block (J*omega x omega) - C[3, 4] = Iz * r - C[3, 5] = -Iy * q - C[4, 3] = -Iz * r - C[4, 5] = Ix * p - C[5, 3] = Iy * q - C[5, 4] = -Ix * p - return C - -def dampening(linear_diag, quad_diag, u, v, w, p, q, r): - """ - Linear + quadratic diagonal damping D(v)*v = (Dl + Dq*|v|) v. - Inputs Dl, Dq are 6-vectors (or lists) for [u v w p q r]. - """ - Dl = SX(linear_diag) - Dq_vec = SX(6, 1) - vel = vertcat(u, v, w, p, q, r) - abs_vel = SX(6, 1) - for i in range(6): - abs_vel[i] = fabs(vel[i]) - Dq = SX(quad_diag) # elementwise times abs_vel when applied - # Effective damping operator when multiplying right by vel: - # (Dl + Dq*|vel|) * vel - return Dl, Dq, abs_vel - -def make_auv_model(mass_inertia_matrix,D_lin,D_quad): - """ - Build symbolic CasADi model for a 9x3 AUV suitable for acados codegen. - - params (dict) expected keys: - - m, Ix, Iy, Iz : scalars - - D_lin: length-6 list/tuple : linear damping diag for [u v w p q r] - - D_quad: length-6 list/tuple : quadratic damping diag - - g_vec: length-6 list/tuple : restoring forces/torques in body (optional; use zeros if unknown) - Returns: - dict with keys: x, u, f_expl_expr, name - """ - # Unpack parameters - M=SX(mass_inertia_matrix) - Ix = mass_inertia_matrix[3][3] - Iy = mass_inertia_matrix[4][4] - Iz = mass_inertia_matrix[5][5] - m=M[0][0] - #D_lin = params.get('D_lin') - #D_quad = params.get('D_quad') - - # States: body velocities and Euler angles - u = SX.sym('u') # surge - v = SX.sym('v') # sway - w = SX.sym('w') # heave - p = SX.sym('p') # roll rate NED - q = SX.sym('q') # pitch rate NED - r = SX.sym('r') # yaw rate NED - phi = SX.sym('phi') #Body - theta = SX.sym('theta') #Body - psi = SX.sym('psi') #Body - - x = vertcat(u, v, w, p, q, r, phi, theta, psi) - - # Inputs: surge force, pitch & yaw moments - Fx = SX.sym('Fx') - My = SX.sym('My') - Mz = SX.sym('Mz') - u_in = vertcat(Fx, My, Mz) - - # Inertia (diagonal for now) - #M = diag(SX([m, m, m, Ix, Iy, Iz])) - - # Coriolis matrix for diagonal inertia - C = coriolis_rb_diag(m, Ix, Iy, Iz, u, v, w, p, q, r) - - # Damping (linear + quadratic diagonal) - Dl, Dq, abs_vel = dampening(D_lin, D_quad, u, v, w, p, q, r) - #TODO: blend the dampening functions - # Generalized input vector tau: map [Fx, My, Mz] to 6x1 wrench - # tau = [Fx, 0, 0, 0, My, Mz]^T - tau = vertcat(Fx, 0.0, 0.0, 0.0, My, Mz) - - - # 6DOF body dynamics: nu_dot = M^{-1} (tau - C*nu - (Dl + Dq*|nu|) nu - g) - nu = vertcat(u, v, w, p, q, r) - D_eff_times_nu = (Dl + diag(Dq @ abs_vel)) @ nu # elementwise: (Dq*|nu|) * nu - rhs_nu = SX(6, 1) - rhs_nu = inv(M) @ (tau - C @ nu - D_eff_times_nu) # - print(D_eff_times_nu) - # Kinematics: eta_dot = T(eta) * omega - T = euler_kinematics_T(phi, theta) - eta_dot = T @ vertcat(p, q, r) - - xdot = vertcat(rhs_nu, eta_dot) - xdot_sym = SX.sym("xdot", x.size()[0]) - model = { - "name": "auv_model", - "x": x, - "xdot": xdot_sym, - "u": u_in, - "f_expl_expr": xdot, # explicit ODE f(x,u) - # implicit form (optional in acados): f_impl = xdot - f(x,u) - "f_impl_expr": xdot_sym - xdot - } - return model - -if __name__ == "__main__": - # Quick smoke test - m=[[6,0,0,0,0,0,], - [0,5,0,0,0,0,], - [0,0,4,0,0,0,], - [0,0,0,3,0,0,], - [0,0,0,0,5,0,], - [0,0,0,0,0,9]] - D_lin=[[6,0,0,0,0,0,], - [0,5,0,0,0,0,], - [0,0,4,0,0,0,], - [0,0,0,3,0,0,], - [0,0,0,0,5,0,], - [0,0,0,0,0,9]] - D_quad=[[6,0,0,0,0,0,], - [0,5,0,0,0,0,], - [0,0,4,0,0,0,], - [0,0,0,3,0,0,], - [0,0,0,0,5,0,], - [0,0,0,0,0,9]] - mdl = make_auv_model(m,D_lin,D_quad) - print("Model:", mdl["name"]) - print("x dim:", mdl["x"].numel(), "u dim:", mdl["u"].numel()) diff --git a/control/velocity_controller/scripts/generator.py b/control/velocity_controller/scripts/generator.py deleted file mode 100644 index cbd2ee512..000000000 --- a/control/velocity_controller/scripts/generator.py +++ /dev/null @@ -1,223 +0,0 @@ - -#!/usr/bin/env python3 -""" -AUV NMPC OCP generator for acados -N = 20, Tf = 0.05 (2.5 ms steps) -Nonlinear thrust magnitude constraint. -Diagonal Q/R/Qe loaded from weights.yaml or defaults. - -Generates a solver in ./build_auv_solver/ -""" - -import numpy as np -import yaml -from pathlib import Path -import scipy.linalg - -from acados_template import AcadosOcp, AcadosOcpSolver, AcadosModel -from casadi import SX, vertcat - -# Import the underwater vehicle model from Step 1 -from dynamics import make_auv_model - - -# ----------------------------- -# Load weighting matrices -# ----------------------------- -def load_matrices(path="../config/parameters.yaml"): - if Path(path).exists(): - with open(path, "r") as f: - data = yaml.safe_load(f) - node_key = next(iter(data.keys())) - print("Top-level keys:", list(data.keys())) - print("[INFO] Loading weights from", path) - Q = np.diag(data[node_key]["ros__parameters"]["NMPC_params"]["Q"]) - R = np.diag(data[node_key]["ros__parameters"]["NMPC_params"]["R"]) - Qe = np.diag(data[node_key]["ros__parameters"]["NMPC_params"]["Q"]) - inertia_M=np.reshape(data[node_key]["ros__parameters"]["inertia_matrix"],(6,6)) - D_lin = np.reshape(data[node_key]["ros__parameters"]["dampening_matrix_low"],(6,6)) - D_quad = np.reshape(data[node_key]["ros__parameters"]["dampening_matrix_high"],(6,6)) - N=data[node_key]["ros__parameters"]["NMPC_params"]["N"] - delta_t=data[node_key]["ros__parameters"]["publish_rate"] - max_force=data[node_key]["ros__parameters"]["max_force"] - else: - print("[ERROR], yaml file not found") - - return Q, R, Qe, inertia_M, D_lin, D_quad,N,delta_t,max_force - - -# ----------------------------- -# Build the OCP -# ----------------------------- -def create_auv_ocp(): - # Load weights - Q, R, Qe, inertia_Matrix, D_lin, D_quad, N, delta_t, max_force = load_matrices() - - # Load dynamical model - mdl = make_auv_model(inertia_Matrix, D_lin, D_quad) - - # Wrap into acados model format - acados_model = AcadosModel() - acados_model.name = mdl["name"] - acados_model.x = mdl["x"] - acados_model.xdot=mdl["xdot"] - acados_model.u = mdl["u"] - acados_model.f_expl_expr = mdl["f_expl_expr"] - acados_model.f_impl_expr = mdl["f_impl_expr"] - - # Create OCP - ocp = AcadosOcp() - ocp.model = acados_model - - # Horizon settings - Tf = (delta_t*N)/1000 # total horizon [seconds] - ocp.dims.N = N - ocp.solver_options.tf = Tf - - ocp.solver_options.integrator_type="IRK" - ocp.solver_options.sim_method_num_stages=2 - ocp.solver_options.sim_method_num_steps=4 - - nx = acados_model.x.size()[0] - nu = acados_model.u.size()[0] - - # ---------------------------------- - # Cost: LINEAR_LS (yref-based) - # ---------------------------------- - # ---------------------------------- - ocp.cost.cost_type = "LINEAR_LS" - ocp.cost.cost_type_e = "LINEAR_LS" - - # States you care about: u=0, q=4, r=5 - idx_states = [0, 4, 5, 7, 8] - idx_controls = [0, 1, 2] - - n_y = len(idx_states) + len(idx_controls) # 8 - n_ye = len(idx_states) # 5 - - # Vx: (8, nx) — selects only u, q, r from state vector - Vx = np.zeros((n_y, nx)) - for i, idx in enumerate(idx_states): - Vx[i, idx] = 1.0 - - # Vu: (8, nu) — selects all 3 controls, placed in lower block - Vu = np.zeros((n_y, nu)) - for i, idx in enumerate(idx_controls): - Vu[len(idx_states) + i, idx] = 1.0 - - # W: (8, 8) — only track what you care about, well conditioned - Q_tracked = np.diag([ - Q[0, 0], # u - Q[1, 1], # q - Q[2, 2], # r - Q[3, 3], # theta - Q[4, 4], # psi -]) - R_tracked = np.diag([R[0,0], R[1,1], R[2,2]]) # weights for controls - - W = scipy.linalg.block_diag(Q_tracked, R_tracked) - - ocp.cost.Vx = Vx # (8, nx) - ocp.cost.Vu = Vu # (8, nu) - ocp.cost.W = W # (8, 8) - - # Terminal cost — same state selection, no controls - Vx_e = np.zeros((n_ye, nx)) - for i, idx in enumerate(idx_states): - Vx_e[i, idx] = 1.0 - - Q_e_tracked = np.diag([Qe[0,0], Qe[1,1], Qe[2,2], Qe[3,3],Qe[4,4]]) - - ocp.cost.Vx_e = Vx_e # (5, nx) - ocp.cost.W_e = Q_e_tracked # (5, 5) - - # References must match ny=6 and ny_e=3 - ocp.cost.yref = np.zeros(n_y) # [u, q, r, theta, psi, u1, u2, u3] - ocp.cost.yref_e = np.zeros(n_ye) # [u, q, r, theta, psi] - - # ---------------------------------- - # Nonlinear input constraint: - # Fx^2 + My^2 + Mz^2 <= 10000 - # ---------------------------------- - #Fx, My, Mz = acados_model.u[0], acados_model.u[1], acados_model.u[2] - #h_expr = Fx**2 + My**2 + Mz**2 - - #ocp.model.con_h_expr = vertcat(h_expr) # 1 constraint - #ocp.dims.nh=1 - #ocp.constraints.lh = np.array([0.0]) # lower bound: h >= 0 (redundant) - #ocp.constraints.uh = np.array([max_force**2]) # upper bound: magnitude <= 100 - - # No bounds on slack variables (we don't use slacks) - #ocp.constraints.idxsh = np.array([], dtype=int) - #ocp.constraints.lsh = np.array([]) - #ocp.constraints.ush = np.array([]) - - # No box-constraints on h (handled via lh/uh) - #ocp.constraints.idxbh = np.array([], dtype=int) - #ocp.constraints.lbh = np.array([]) - #ocp.constraints.ubh = np.array([]) - - u_max = max_force # from YAML - - ocp.constraints.lbu = -u_max * np.ones(nu) - ocp.constraints.ubu = u_max * np.ones(nu) - ocp.constraints.idxbu = np.arange(nu, dtype=int) - ocp.constraints.idxbx = np.array([7]) - ocp.constraints.lbx = -1.4 - ocp.constraints.ubx = 1.4 - ocp.dims.nbx = 1 - - # ---------------------------------- - # Initial state constraint (must be updated before solve) - # ---------------------------------- - ocp.constraints.x0 = np.zeros(nx) - - # ---------------------------------- - # Solver options - # ---------------------------------- - print("W shape:", ocp.cost.W.shape) - print("W diagonal:", np.diag(ocp.cost.W)) - print("W_e shape:", ocp.cost.W_e.shape) - print("W_e diagonal:", np.diag(ocp.cost.W_e)) - print("Vx shape:", ocp.cost.Vx.shape) - print("Vx_e shape:", ocp.cost.Vx_e.shape) - print("yref:", ocp.cost.yref) - print("yref_e:", ocp.cost.yref_e) - print("ny:", ocp.dims.ny) - print("ny_e:", ocp.dims.ny_e) - print("VX",ocp.cost.Vx) - print("VU", ocp.cost.Vu) - ocp.solver_options.qp_solver = "FULL_CONDENSING_HPIPM" - ocp.solver_options.qp_solver_warm_start=1 - ocp.solver_options.hessian_approx = "GAUSS_NEWTON" - #ocp.solver_options.integrator_type = "ERK" - ocp.solver_options.nlp_solver_type = "SQP" # fast real-time iteration - #ocp.solver_options.nlp_solver_max_iter = 100 - - #ocp.solver_options.globalization = 'MERIT_BACKTRACKING' - ocp.solver_options.levenberg_marquardt = 1e-2 - ocp.solver_options.print_level = 2 - - # ---------------------------------- - # Output directory - # ---------------------------------- - ocp.code_gen_opts.code_export_directory = "build_auv_solver" - - print("nh:", ocp.dims.nh) - print("lh:", ocp.constraints.lh) - print("uh:", ocp.constraints.uh) - - #print("idxbh:", ocp.constraints.idxbh) - print("idxsh:", ocp.constraints.idxsh) - - return ocp - - -# ----------------------------- -# Main entry: generate solver -# ----------------------------- -if __name__ == "__main__": - ocp = create_auv_ocp() - print("[INFO] Generating AUV NMPC solver...") - AcadosOcpSolver(ocp, json_file="auv_ocp.json") - print("[INFO] Done. Solver code is in ./build_auv_solver/") diff --git a/control/velocity_controller/src/LQR_setup.cpp b/control/velocity_controller/src/LQR_setup.cpp index 0de30a5ff..f6314cac6 100644 --- a/control/velocity_controller/src/LQR_setup.cpp +++ b/control/velocity_controller/src/LQR_setup.cpp @@ -1,4 +1,3 @@ - #include "velocity_controller/LQR_setup.hpp" #include "rclcpp/rclcpp.hpp" #include @@ -17,22 +16,16 @@ //#include #include "vortex/utils/math.hpp" #include "ct/optcon/lqr/LQR.hpp" -#include "velocity_controller/NMPC_setup.hpp" - -//Eigen::IOFormat fmt(Eigen::StreamPrecision, 0, ", ", "\n", "[", "]"); -LQRController::LQRController() -{ +LQRController::LQRController(){ Q.setZero(); R.setZero(); B.setZero(); - D_low.setZero(); - D_high.setZero(); + D.setZero(); inertia_matrix_inv.setZero(); }; -bool LQRController::set_matrices(std::vector Q_,std::vector R_,std::vector inertia_matrix_,double max_force_, std::vector water_r_low,std::vector water_r_high){ - //Possible error handling here to check for size and allowed values. +bool LQRController::set_matrices(std::vector Q_,std::vector R_,std::vector inertia_matrix_,double max_force_, std::vector D_low){ if (Q_.size()!=8){ RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The Q matrix has the wrong amount of elements"); return 0; @@ -45,29 +38,21 @@ bool LQRController::set_matrices(std::vector Q_,std::vector R_,s RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The M matrix has the wrong amount of elements"); return 0; } - if(water_r_low.size()!=36||water_r_high.size()!=36){ - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The D matrix has the wrong amount of elements"); - return 0; - } if (max_force_<0){ RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The max_force need to be >0"); return 0; } max_force=max_force_; - // Ensure full matrices are zeroed before assigning diagonals Q.diagonal() = Eigen::Map(Q_.data(), Q_.size()); R.diagonal() = Eigen::Map(R_.data(), R_.size()); Ixx=inertia_matrix_.at(6*3+3); Iyy=inertia_matrix_.at(4*6+4); Izz=inertia_matrix_.at(5*6+5); mass=inertia_matrix_.at(0); Eigen::Matrix inertia_matrix = Eigen::Map>(inertia_matrix_.data(),6,6); - D_low=Eigen::Map>(water_r_low.data(),6,6); - D_high=Eigen::Map>(water_r_high.data(),6,6); + D=Eigen::Map>(D_low.data(),6,6); inertia_matrix_inv=inertia_matrix.inverse(); Eigen::MatrixB_t=inertia_matrix_inv*(Eigen::Matrix()<<1,0,0, 0,0,0, 0,0,0, 0,0,0, 0,1,0, 0,0,1).finished(); - B.setZero(); - Eigen::Matrix B_m; - B_m.setZero(); + Eigen::Matrix B_m=Eigen::Matrix::Zero(); B_m.block<6,3>(0,0)=B_t; std::vector> swaplines{{1,7},{2,8},{3,4},{4,5}}; for (long unsigned int i=0;i Q_,std::vector R_,s return 1; } -//Can be optimized std::tuple LQRController::saturate (double value, bool windup, double limit){ if (abs(value) > limit){ windup=true; @@ -102,28 +86,17 @@ double LQRController::anti_windup(double error, double integral_sum, bool windup Eigen::Matrix LQRController::linearize(State s){ - //Eigen::Matrix A; - Eigen::Matrix D=Eigen::Matrix::Zero(); + Eigen::Matrix D_=Eigen::Matrix::Zero(); - if (s.surge<100){ //Threshold tbd - D=-inertia_matrix_inv*D_low; - } - else { - D=-inertia_matrix_inv*D_high; - } + D_=-inertia_matrix_inv*D; //Assuming linear dampening for now + Eigen::Matrix C=coriolis(s); - /* //Need to decide if i want to learize around 0? - C(1,5)=-mass*s.surge; - C(2,4)=mass*s.surge; - */ - D-=inertia_matrix_inv*C; //To avoid unneccessary allocation + + D_-=inertia_matrix_inv*C; //To avoid unneccessary allocation Eigen::Matrix T=Eigen::Matrix::Identity(); - /*T<<1,sin(s.yaw)*tan(s.pitch),cos(s.yaw)*tan(s.pitch), - 0,cos(s.yaw),-sin(s.yaw), - 0,sin(s.yaw)/cos(s.pitch),cos(s.yaw)/cos(s.pitch);*/ Eigen::Matrix A; - A.block<6,6>(0,0)=D; + A.block<6,6>(0,0)=D_; A.block<3,3>(0,6)=A.block<3,3>(6,0)=A.block<3,3>(6,6)=Eigen::Matrix3d::Zero(); A.block<3,3>(6,3)=T; std::vector> swaplines{{1,7},{2,8},{3,4},{4,5}}; @@ -161,13 +134,8 @@ Eigen::Vector LQRController::saturate_input(Eigen::Vector u) bool LQRController::calculate_thrust(State state, Guidance_data error){ ct::optcon::LQR<8,3> lqr; Eigen::Matrix K_l; - - //TODO: consider making my own solver using eigen so that it does not need controll_toolbox library bool INFO= lqr.compute(Q,R,linearize(state),B,K_l,true,false); - if(INFO==0){ - return false; - } - + if(INFO==0)return false; Eigen::Matrix state_error = update_error(error, state); u=saturate_input( (K_l*state_error)); return true; @@ -197,54 +165,17 @@ bool LQRController::set_interval(double interval){ Eigen::Vector LQRController::get_thrust(){ return u; } -//Hjelpefunksjoner for å konvertere mellom std::vector og Eigen::Matrix3d -/*/ -Eigen::Matrix3d vector_to_matrix3d(const std::vector &other_matrix){ - Eigen::Matrix3d mat; - for (int i = 0; i < 3; ++i) - for (int j = 0; j < 3; ++j) - mat(i, j) = other_matrix[i * 3 + j]; - return mat; -} -std::vector matrix3d_to_vector(const Eigen::Matrix3d &mat){ - std::vector other_matrix(9); - for (int i = 0; i < 3; ++i) - for (int j = 0; j < 3; ++j) - other_matrix[i * 3 + j] = mat(i, j); - return other_matrix; -} - -std::vector> matrix3d_to_vector2d(const Eigen::Matrix3d &mat){ - std::vector> other_matrix(3, std::vector(3)); - for (int i = 0; i < 3; ++i) - for (int j = 0; j < 3; ++j) - other_matrix[i][j] = mat(i, j); - return other_matrix; -} -Eigen::Matrix3d vector2d_to_matrix3d(const std::vector> &other_matrix){ - Eigen::Matrix3d mat; - for (int i = 0; i < 3; ++i) - for (int j = 0; j < 3; ++j) - mat(i, j) = other_matrix[i][j]; - return mat; -} -*/ //TODO: double check the matrices here -Eigen::Matrix LQRController::coriolis(const State& s) -{ - // Body velocities +Eigen::Matrix LQRController::coriolis(const State& s){ double u = s.surge; double v = s.sway; double w = s.heave; double p = s.roll_rate; double q = s.pitch_rate; double r = s.yaw_rate; - - Eigen::Matrix C = Eigen::Matrix::Zero(); - // Top-right block (translational-rotational coupling) C(0,4) = mass * w; C(0,5) = -mass * v; C(1,3) = -mass * w; C(1,5) = mass * u; diff --git a/control/velocity_controller/src/NMPC_acados.cpp b/control/velocity_controller/src/NMPC_acados.cpp deleted file mode 100644 index 9f7e99a11..000000000 --- a/control/velocity_controller/src/NMPC_acados.cpp +++ /dev/null @@ -1,183 +0,0 @@ -//#include "rclcpp/rclcpp.hpp" -#include "velocity_controller/NMPC_acados.hpp" -#include // memcpy -#include -//#include "acados_solver_auv_model.h" -#include - -void AuvNMPC::set_diag(double* M, int n, const std::vector& diag) { - for (int i = 0; i < n; ++i) { - std::memset(M + i*n, 0, n * sizeof(double)); - if (i < (int)diag.size()) M[i*n + i] = diag[i]; - } -} - -AuvNMPC::~AuvNMPC() { - if (capsule_) { - auv_model_acados_free(capsule_); - auv_model_acados_free_capsule(capsule_); - capsule_ = nullptr; - std::cout << "Destroying AuvNMPC..." << std::endl; - } -} - -bool AuvNMPC::init() { - capsule_ = auv_model_acados_create_capsule(); - if (!capsule_) return false; - - - int status = auv_model_acados_create(capsule_); - if (status) { - std::cerr << "[AuvNMPC] create failed, status: " << status << std::endl; - return false; - } - - solver_ = auv_model_acados_get_nlp_solver(capsule_); - config_ = auv_model_acados_get_nlp_config(capsule_); - dims_ = auv_model_acados_get_nlp_dims(capsule_); - nlp_in_ = auv_model_acados_get_nlp_in(capsule_); - nlp_out_= auv_model_acados_get_nlp_out(capsule_); - N_ = (N_override_ > 0) ? N_override_ : AUV_MODEL_N; // fallback - return true; -} -bool AuvNMPC::initialize_guess(std::array x,std::array u_init){ - for (int i=0;i& Wd, const std::vector& We) { - std::vector W_diag_(NY, 0.0); - std::vector W_e_diag_(NY_E, 0.0); - if ((int)Wd.size() == NY) { - W_diag_ = Wd; - } else { - std::cerr << "[AuvNMPC] set_weights: W size mismatch, got " - << Wd.size() << " expected " << NY << std::endl; - } - if ((int)We.size() == NY_E) { - W_e_diag_ = We; - } else { - std::cerr << "[AuvNMPC] set_weights: W_e size mismatch, got " - << We.size() << " expected " << NY_E << std::endl; - } - // Build W and W_e from current diagonals (could cache) - - set_diag(W_.data(), NY, W_diag_); - - set_diag(W_e_.data(), NY_E, W_e_diag_); -} - -void AuvNMPC::set_max_force(double max_force) { - max_force2_ = max_force; - if (std::isnan(max_force2_)) std::cout<<"Max force is Nan"<(x0.data())); - ocp_nlp_constraints_model_set(config_, dims_, nlp_in_, nlp_out_, 0, "ubx", const_cast(x0.data())); - - - - // Update stages - // Stage yref: [u, q, r, theta, psi, tau1, tau2, tau3] - // Matches Vx selecting states [0,4,5,7,8] and Vu selecting all 3 inputs - double yref[NY] = { - xr[0], // u (surge velocity) - xr[1], // q (pitch rate) - xr[2], // r (yaw rate) - xr[3], // theta (pitch) - xr[4], // psi (yaw) - ur[0], // tau_surge - ur[1], // tau_pitch - ur[2] // tau_yaw - }; - for (int k = 0; k < N_; ++k) { - ocp_nlp_cost_model_set(config_, dims_, nlp_in_, k, "yref", yref); - ocp_nlp_cost_model_set(config_, dims_, nlp_in_, k, "W", W_.data()); - } - // Terminal yref_e: [u, q, r, theta, psi] - double yref_e[NY_E] = { - xr[0], // u - xr[1], // q - xr[2], // r - xr[3], // theta - xr[4] // psi - }; - - ocp_nlp_cost_model_set(config_, dims_, nlp_in_, N_, "yref_e", yref_e); - ocp_nlp_cost_model_set(config_, dims_, nlp_in_, N_, "W_e", W_e_.data()); - - // Solve (blocking) - /* - for (int k = 0; k <= N_; ++k) { - ocp_nlp_out_set(config_, dims_, nlp_out_, nlp_in_,k, "x", const_cast(x0.data())); - } - double u_init[NU] = {0.0, 0.0, 0.0}; - for (int k = 0; k < N_; ++k) { - ocp_nlp_out_set(config_, dims_, nlp_out_,nlp_in_, k, "u", u_init); - }*/ - - int status = auv_model_acados_solve(capsule_); - - if (status == 0) { - std::cout << "--- Predicted Trajectory ---" << std::endl; - for (int k = 0; k <= N_; ++k) { - double x_k[NX]; - ocp_nlp_out_get(config_, dims_, nlp_out_, k, "x", x_k); - std::cout << "Step " << k << ": " < AuvNMPC::getU0(){ - return u0_out; -} - -void AuvNMPC::setState(const std::array& x){ - x0=x; - for (int i=0;i& x_ref){ //surge, pitch_rate, yaw_rate, pitch, yaw - xr=x_ref; - std::cout<<"xr: "; - for (int i=0;i -#include -#include -#include -#include -#include -#include -#include -#include "rclcpp/rclcpp.hpp" -#include "velocity_controller/utilities.hpp" -#include "velocity_controller/NMPC_setup.hpp" - - -bool NMPC_controller::set_matrices(std::vector Q,std::vector R,std::vector inertia_matrix, double max_force,std::vector water_r_low,std::vector water_r_high){ - if (Q.size()!=9){ - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The Q matrix has the wrong amount of elements"); - return 0; - } - if(R.size()!=3){ - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The R matrix has the wrong amount of elements"); - return 0; - } - if(inertia_matrix.size()!=36){ - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The M matrix has the wrong amount of elements"); - return 0; - } - if(water_r_low.size()!=36||water_r_high.size()!=36){ - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The D matrix has the wrong amount of elements"); - return 0; - } - if (max_force<0){ - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The max_force need to be >0"); - return 0; - } - if (inertia_matrix[0]<0){ - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"Negative mass?"); - return 0; - } - Q_.setZero(); - R_.setZero(); - Q_.diagonal() = Eigen::Map(Q.data(), Q.size()); - R_.diagonal() = Eigen::Map(R.data(), R.size()); - Eigen::Matrix inertia_matrix_ = Eigen::Map>(inertia_matrix.data(),6,6); - D_low=Eigen::Map>(water_r_low.data(),6,6); - D_high=Eigen::Map>(water_r_high.data(),6,6); - M_inv=inertia_matrix_.inverse(); - mass=inertia_matrix[0]; - Ix=inertia_matrix_(3,3);std::vector Q2; - std::vector R2; - Iy=inertia_matrix_(4,4); - Iz=inertia_matrix_(5,5); - - //B_=M_inv*(Eigen::Matrix() << 1,0,0,0,1,0,0,0,1,0,0,0,0,0,0,0,0,0).finished(); - - return true; -}; -void NMPC_controller::reset_controller(){ - return; -} -bool NMPC_controller::set_interval(double interval){ - interval_=interval; - return true; -} -bool NMPC_controller::initialize_MPC(){ - using SYM=casadi::MX; - SYM X=SYM::sym("X",n,1); //u,v,w,p,q,r,phi,theta,psi - //SYM A=SYM::zeros(n,n); - casadi::DM M_i=casadi::DM::zeros(6,6); - SYM U=SYM::sym("U",3,1); - casadi::DM Q=casadi::DM::zeros(n,n); - casadi::DM R=casadi::DM::zeros(m,m); - /*U(0,0)=SYM::sym("u_surge"); - U(1,0)=SYM::sym("u_pitch"); - U(2,0)=SYM::sym("u_yaw");*/ - - //Creating M_i matrixstd::vector Q2; - std::vector R2; - for (int i=0;i X_v(N+1), U_v(N); - for (int i=0;i<=N;i++) X_v[i] = SYM::sym("X_"+std::to_string(i),n); - for (int i=0;i z_parts; - z_parts.insert(z_parts.end(), X_v.begin(), X_v.end()); - z_parts.insert(z_parts.end(), U_v.begin(), U_v.end()); - SYM Z = vertcat(z_parts); - - //Initial state - SYM x0=SYM::sym("x0",n); - SYM xr=SYM::sym("xr",n); - SYM ur=SYM::sym("ur",m); - - SYM P=SYM::vertcat({x0,xr,ur}); - - - auto p_x0 = P(casadi::Slice(0, n)); // x0 - auto p_xr = P(casadi::Slice(n, 2*n)); // xr - auto p_ur = P(casadi::Slice(2*n, 2*n + m)); // ur - - //Dynamic constraints - - std::vector g_list; - g_list.push_back(X_v[0]-p_x0); - for (int i=0; i lbx_parts, ubx_parts; - x_min(7)=-1.4; - x_max(7)=1.4; - // X0..XN - for (int k = 0; k <= N; ++k) { - lbx_parts.push_back(x_min); - ubx_parts.push_back(x_max); - } - // U0..U_{N-1} - for (int k = 0; k < N; ++k) { - lbx_parts.push_back(u_min); - ubx_parts.push_back(u_max); - } - lbx = vertcat(lbx_parts); - ubx = vertcat(ubx_parts); - - // Equality constraints: G == 0 - lbg = casadi::DM::zeros(n*(N+1)); - ubg = casadi::DM::zeros(n*(N+1)); - - - //building NLP - casadi::MXDict nlp; - nlp["x"]=Z; - nlp["f"]=J; - nlp["g"]=G; - nlp["p"]=P; - casadi::Dict opts1; - opts1["ipopt.print_level"]=2; - opts1["print_time"]=false; - opts1["ipopt.sb"]="yes"; - opts1["expand"]=true; - opts1["jit"]=false; - opts1["ipopt.tol"]=1e-4; - opts1["ipopt.max_iter"]=100; - opts1["ipopt.linear_solver"]="mumps"; //robust default - - solver=casadi::nlpsol("solver","ipopt",nlp,opts1); - - // -------------------------------------------------------- - // Prepare parameter vector Pval and initial guess Z0 - // -------------------------------------------------------- - // Example numeric values: - casadi::DM x0_val = casadi::DM::zeros(n,1); - std::vector Pval_parts; - Pval_parts.push_back(x0_val); - Pval_parts.push_back(casadi::DM::zeros(n,1)); - Pval_parts.push_back(casadi::DM::zeros(m,1)); - Pval = vertcat(Pval_parts); - - // Initial guess: Z0 (X guesses then U guesses) - std::vector Z0_parts; - for (int k = 0; k <= N; ++k) Z0_parts.push_back(x0_val); // start with x0 everywhere - for (int k = 0; k < N; ++k) Z0_parts.push_back(casadi::DM::zeros(m,1)); - Z0_next = vertcat(Z0_parts); - - - return true; -} - -bool NMPC_controller::calculate_thrust(Guidance_data guidance_values, State state){ - - casadi::DM x0_val={state.surge,state.sway,state.heave,state.roll_rate,state.pitch_rate,state.yaw_rate,state.roll,state.pitch,state.yaw}; - casadi::DM xr_val={guidance_values.surge,0,0,0,0,0,0,guidance_values.pitch,guidance_values.yaw}; - casadi::DM ur_val={0,0,0}; - Pval=casadi::DM::vertcat({x0_val,xr_val,ur_val}); - // Solve - - casadi::DMDict solver_in; - solver_in["x0"] = Z0_next; - solver_in["lbx"] = lbx; - solver_in["ubx"] = ubx; - solver_in["lbg"] = lbg; - solver_in["ubg"] = ubg; - solver_in["p"] = Pval; - auto sol = solver(solver_in); - if (sol.count("x") == 0) { - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "NLP solver failed"); - return 1; - } - casadi::DM Zstar = sol.at("x"); // optimal stacked decision vector - //TODO: check to see NAN or INF values in solution - - // index of U0 start: - int offset_u0 = n*(N+1); - // Extract u0* (first control block after all states) - // Z = [X0; X1; ...; XN; U0; U1; ...; U_{N-1}] - casadi::DM u0_star = Zstar(casadi::Slice(offset_u0, offset_u0 + m)); - - std::cout << "u0* = " << u0_star << std::endl; - - - // Warm-start shift for next iteration - // Build new Z0_next = [X1*; X2*; ...; XN*; XN*; U1*; ...; U_{N-1}*; U_{N-1}*] - // (X0 will be re-anchored by the new measured x0 in constraints) - std::vector Xstar(N+1), Ustar(N); - // Unstack from Zstar: - for (int k = 0; k <= N; ++k) { - int i0 = k*n; - Xstar[k] = Zstar(casadi::Slice(i0, i0+n)); - } - for (int k = 0; k < N; ++k) { - int i0 = n*(N+1) + k*m; - Ustar[k] = Zstar(casadi::Slice(i0, i0+m)); - } - - std::vector Z0_next_parts; - // shifted states: X1..XN, repeat XN at end - for (int k = 1; k <= N; ++k) Z0_next_parts.push_back(Xstar[k]); - Z0_next_parts.push_back(Xstar[N]); - // shifted inputs: U1..U_{N-1}, repeat last - for (int k = 1; k < N; ++k) Z0_next_parts.push_back(Ustar[k]); - Z0_next_parts.push_back(Ustar[N-1]); - - Z0_next = vertcat(Z0_next_parts); - - - thrust(0) = double(u0_star(0)); - thrust(1) = double(u0_star(1)); - thrust(2) = double(u0_star(2)); - return 0; -} - -Eigen::Matrix NMPC_controller::get_thrust(){ - return thrust; -} \ No newline at end of file diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index 037730231..c577a73af 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -1,13 +1,6 @@ -#include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/string.hpp" #include "velocity_controller/velocity_controller.hpp" #include -#include "geometry_msgs/msg/wrench_stamped.hpp" -#include "std_msgs/msg/float64_multi_array.hpp" -#include "std_msgs/msg/bool.hpp" -#include "velocity_controller/NMPC_setup.hpp" #include "velocity_controller/PID_setup.hpp" -#include #include #include #include @@ -16,34 +9,19 @@ #include #include #include -#include "vortex_msgs/msg/los_guidance.hpp" #include "vortex/utils/math.hpp" #include "velocity_controller/utilities.hpp" #include -//Konstruktør + Velocity_node::Velocity_node() : rclcpp_lifecycle::LifecycleNode("velocity_controller_lifecycle"), lqr_controller(), pub_QoS(10), sub_QoS(10) { get_new_parameters(); pub_QoS.keep_last(10).reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT).durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); sub_QoS.keep_last(10).reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT).durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); - //TODO: dont need to save the Q3 R3 and the other matries, just use #define - //NMPC controller - NMPC.set_matrices(Q3,R3, inertia_matrix, max_force, dampening_matrix_low, dampening_matrix_high); - NMPC.set_interval(publish_rate/1000.0); - //NMPC.initialize_MPC(); - //NMPC acados controller - NMPC_acados.init(); - NMPC_acados.set_max_force(max_force); - std::vector W=Q2; - W.insert(W.end(),R2.begin(),R2.end()); - std::vector We=Q2; - NMPC_acados.set_weights(W, We); - - - //Controllers + //PID initialization PID_surge.set_output_limits(-max_force, max_force); PID_pitch.set_output_limits(-max_force, max_force); PID_yaw.set_output_limits(-max_force, max_force); @@ -51,10 +29,12 @@ Velocity_node::Velocity_node() : rclcpp_lifecycle::LifecycleNode("velocity_contr PID_pitch.set_parameters(pitch_params,publish_rate/1000.0); PID_yaw.set_parameters(yaw_params,publish_rate/1000.0); - if(!lqr_controller.set_matrices(Q,R,inertia_matrix,max_force,dampening_matrix_low,dampening_matrix_high)||!lqr_controller.set_interval(static_cast(publish_rate)/1000)){ + //LQR + if(!lqr_controller.set_matrices(Q,R,inertia_matrix,max_force,dampening_matrix_low)||!lqr_controller.set_interval(static_cast(publish_rate)/1000)){ controller_type=1; RCLCPP_INFO(this->get_logger(),"Switching to PID"); }; + //Automaticly start in activate if auto_start is true if(auto_start){ startup_timer_=create_wall_timer(std::chrono::milliseconds(0), [this](){startup_timer_->cancel(); trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);}); } @@ -86,7 +66,6 @@ void Velocity_node::calc_thrust() switch (controller_type) { case 1:{ - //TODO: some logic for removing the change in reference PID_surge.calculate_thrust(error.surge); PID_pitch.calculate_thrust(error.pitch,-current_state.pitch_rate); PID_yaw.calculate_thrust(error.yaw, -current_state.yaw_rate); @@ -111,47 +90,11 @@ void Velocity_node::calc_thrust() } break; } - case 3:{ - //RCLCPP_INFO(this->get_logger(),"Guidance: %f, %f, %f",guidance_values.surge,guidance_values.pitch,guidance_values.yaw); - Eigen::Matrix u; - if (NMPC.calculate_thrust(guidance_values, current_state)){ - controller_type=1; - RCLCPP_ERROR(this->get_logger(),"Switching to PID"); - rclcpp::shutdown(); - } - else{ - u=NMPC.get_thrust(); - thrust_out.wrench.force.x=u[0]; - thrust_out.wrench.torque.y=u[1]; - thrust_out.wrench.torque.z=u[2]; - RCLCPP_INFO(this->get_logger(),"NMPC: surge: %f, pitch %f, yaw %f",u(0),u(1),u(2)); - } - - break; - } - case 4:{ - std::vectoru; - std::array x_ref={guidance_values.surge,0.0,0.0,guidance_values.pitch,guidance_values.yaw}; //surge, pitch_rate, yaw_rate, pitch, yaw - - NMPC_acados.setReference(x_ref); - std::array state_array={current_state.surge,current_state.sway,current_state.heave,current_state.roll_rate,current_state.pitch_rate,current_state.yaw_rate,current_state.roll,current_state.pitch,current_state.yaw}; - NMPC_acados.setState(state_array); - int status=NMPC_acados.solve_once(); - RCLCPP_INFO(this->get_logger(),"Status %i",status); - if(status){ - - rclcpp::shutdown(); - }; - u=NMPC_acados.getU0(); - thrust_out.wrench.force.x=u[0]; - thrust_out.wrench.torque.y=u[1]; - thrust_out.wrench.torque.z=u[2]; - break; - } default:{ - //Some crash handling here - RCLCPP_ERROR(this->get_logger(),"Unknown controller set"); - break; + RCLCPP_ERROR(this->get_logger(),"Unknown controller set, switching to PID"); + controller_type=1; + calc_thrust(); + return; } } publisher_thrust->publish(thrust_out); @@ -180,22 +123,27 @@ void Velocity_node::odometry_callback(const nav_msgs::msg::Odometry::SharedPtr m void Velocity_node::get_new_parameters(){ - //topics //TODO: check what happens when same parameter in global and local file + //topics this->declare_parameter("topics.wrench_input"); - this->topic_thrust = this->get_parameter("topics.wrench_input").as_string(); + topic_thrust = this->get_parameter("topics.wrench_input").as_string(); this->declare_parameter("topics.guidance.los"); - this->topic_guidance = this->get_parameter("topics.guidance.los").as_string(); + topic_guidance = this->get_parameter("topics.guidance.los").as_string(); this->declare_parameter("topics.odom"); - this->topic_odometry = this->get_parameter("topics.odom").as_string(); + topic_odometry = this->get_parameter("topics.odom").as_string(); - //variables - - this->declare_parameter("max_force"); - this->max_force = this->get_parameter("max_force").as_double(); - this->declare_parameter("publish_rate"); - this->publish_rate = this->get_parameter("publish_rate").as_int(); - this->declare_parameter("controller_type"); - this->controller_type=this->get_parameter("controller_type").as_int(); + //Settings + this->declare_parameter("Settings.max_force"); + max_force = this->get_parameter("Settings.max_force").as_double(); + this->declare_parameter("Settings.publish_rate"); + publish_rate = this->get_parameter("Settings.publish_rate").as_int(); + this->declare_parameter("Settings.controller_type"); + controller_type=this->get_parameter("Settings.controller_type").as_int(); + this->declare_parameters("Settings", {{"auto_start", false}, {"reset_on_new_ref", true}, {"anti_overshoot", true},{"odometry_dropout_guard", true}}); + auto settings = this->get_parameters({"Settings.auto_start", "Settings.reset_on_new_ref", "Settings.anti_overshoot", "Settings.odometry_dropout_guard"}); + auto_start = settings[0].as_bool(); + reset_on_new_ref = settings[1].as_bool(); + anti_overshoot = settings[2].as_bool(); + odometry_dropout_guard = settings[3].as_bool(); //PID Params this->declare_parameter>("PID_params.surge"); @@ -220,18 +168,7 @@ void Velocity_node::get_new_parameters(){ this->dampening_matrix_low=this->get_parameter("dampening_matrix_low").as_double_array(); this->dampening_matrix_high=this->get_parameter("dampening_matrix_high").as_double_array(); - //NMPC acados Parameters - this->declare_parameter>("NMPCA_params.Q"); - this->declare_parameter>("NMPCA_params.R"); - Q2=this->get_parameter("NMPCA_params.Q").as_double_array(); - R2=this->get_parameter("NMPCA_params.R").as_double_array(); - //NMPC - - this->declare_parameter>("NMPC_params.Q"); - this->declare_parameter>("NMPC_params.R"); - Q3=this->get_parameter("NMPC_params.Q").as_double_array(); - R3=this->get_parameter("NMPC_params.R").as_double_array(); - + } @@ -307,20 +244,17 @@ void Velocity_node::reset_controllers(int nr){ case 1: PID_surge.reset_controller(); lqr_controller.reset_controller(1); - break; case 2: PID_pitch.reset_controller(); lqr_controller.reset_controller(2); - break; case 3: PID_yaw.reset_controller(); lqr_controller.reset_controller(3); break; - } } @@ -335,57 +269,7 @@ int main(int argc, char * argv[]) while (rclcpp::ok()&&!lc_node->should_exit_){ exec.spin_some(); } - //rclcpp::shutdown(); return 0; } -//---------------------------------------------------------------------------------------------------------------- -//Operator overloading for geometry_msgs::msg::WrenchStamped -/* -geometry_msgs::msg::WrenchStamped operator-(const geometry_msgs::msg::WrenchStamped & a, const geometry_msgs::msg::WrenchStamped & b) -{ - geometry_msgs::msg::WrenchStamped result; - result.wrench.force.x = a.wrench.force.x - b.wrench.force.x; - result.wrench.force.y = a.wrench.force.y - b.wrench.force.y; - result.wrench.force.z = a.wrench.force.z - b.wrench.force.z; - result.wrench.torque.x = a.wrench.torque.x - b.wrench.torque.x; - result.wrench.torque.y = a.wrench.torque.y - b.wrench.torque.y; - result.wrench.torque.z = a.wrench.torque.z - b.wrench.torque.z; - return result; -} -geometry_msgs::msg::WrenchStamped operator+(const geometry_msgs::msg::WrenchStamped & a, const geometry_msgs::msg::WrenchStamped & b) -{ - geometry_msgs::msg::WrenchStamped result; - result.wrench.force.x = a.wrench.force.x + b.wrench.force.x; - result.wrench.force.y = a.wrench.force.y + b.wrench.force.y; - result.wrench.force.z = a.wrench.force.z + b.wrench.force.z; - result.wrench.torque.x = a.wrench.torque.x + b.wrench.torque.x; - result.wrench.torque.y = a.wrench.torque.y + b.wrench.torque.y; - result.wrench.torque.z = a.wrench.torque.z + b.wrench.torque.z; - return result; -} -//operator overloading for guidance_data -Guidance_data Guidance_data::operator-(const Guidance_data & b) const -{ - Guidance_data result; - result.surge = this->surge - b.surge; - result.pitch = this->pitch - b.pitch; - result.yaw = this->yaw - b.yaw; - return result; -} - -Guidance_data& Guidance_data::operator=(const std_msgs::msg::Float64MultiArray& msg) -{ - if (msg.data.size()>=3){ - surge=msg.data[0]; - pitch=msg.data[1]; - yaw=msg.data[2]; - } - else{ - //throw std::runtime_error("Guidance message too short, needs at least 3 values"); - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Guidance message too short, needs at least 3 values"); - } - return *this; -} -*/ diff --git a/control/velocity_controller/tests/LQR_test.cpp b/control/velocity_controller/tests/LQR_test.cpp deleted file mode 100644 index 392103cff..000000000 --- a/control/velocity_controller/tests/LQR_test.cpp +++ /dev/null @@ -1,55 +0,0 @@ -#include "rclcpp/rclcpp.hpp" -#include -#include "velocity_controller/LQR_setup.hpp" -#include "ct/optcon/lqr/LQR.hpp" - -class test_LQR_node : public rclcpp::Node{ - public: - double q_surge =75.0, q_pitch= 175.0, q_yaw= 175.0, r_surge= 0.3, r_pitch= 0.4, r_yaw= 0.4, i_surge= 0.3, - i_pitch= 0.4, i_yaw= 0.3, i_weight= 0.5, dt= 0.1; - LQRparameters param={q_surge, q_pitch, q_yaw, r_surge, r_pitch, r_yaw, i_surge, i_pitch, i_yaw, i_weight}; - LQRController controller; - test_LQR_node():Node("test_LQR_node"), controller(){ - RCLCPP_INFO(this->get_logger(),"LQR test node started"); - Eigen::Matrix Q; - Eigen::VectorXd qdiag(8); - qdiag << 75, 175, 175, 1, 0.3, 0.4, 0.3, 1; - Q.diagonal() = qdiag; - Eigen::Matrix R; - Eigen::VectorXd rdiag(3); - rdiag << 0.3, 0.4, 0.4; - R.diagonal() = rdiag; - Eigen::Matrix A=(Eigen::Matrix()<<5,7,23,0,0,0,1,1,0,45,21,4,3,4,3,2,0,23,1,7,6,5,5,7,5,7,6,3,5,7,0,9,2,2,3,2,1,0,3,7,0,0,8,7,6,5,8,5,4,7,1,5,6,2,4,7,6,2,4,5,2,12,5,6).finished(); - Eigen::Matrix B=(Eigen::Matrix()<<2,0,0,3,0,2,0,3,0,1,2,0,3,4,0,3,5,0,6,3,4,1,2,3).finished(); - /*Eigen::Matrix3d inertia_matrix=(Eigen::Matrix3d()<<30.0, 0.6, 0.0, 0.6, 1.629, 0.0, 0.0, 0.0, 1.729).finished(); - Eigen::Matrix3d inertia_matrix_inv=inertia_matrix.inverse(); - Eigen::Matrix3d coriolis_matrix=(Eigen::Matrix3d()<<0.2,-30*2*0.01,-30*2*0.0,30 * 2*0.01,0,1.629 * 2,30 * 2*0.01,1.769 * 2,0).finished(); - Eigen::Matrix3d system_matrix=inertia_matrix.inverse()*coriolis_matrix; - Eigen::Matrix augmented_system_matrix =(Eigen::Matrix()< augmented_input_matrix=(Eigen::Matrix()<< inertia_matrix_inv(0,0),inertia_matrix_inv(0,1),inertia_matrix_inv(0,2),0,0,0, - inertia_matrix_inv(1,0),inertia_matrix_inv(1,1),inertia_matrix_inv(1,2),0,0,0, - inertia_matrix_inv(2,0),inertia_matrix_inv(2,1),inertia_matrix_inv(2,2),0,0,0).finished();*/ - ct::optcon::LQR<8,3> lqr_solver; - Eigen::Matrix K; - lqr_solver.compute(Q,R,A,B,K,true,false); - RCLCPP_INFO(this->get_logger(),"LQR Gain K matrix:"); - RCLCPP_INFO(this->get_logger(),"\n%f %f %f %f %f %f %f %f\n%f %f %f %f %f %f %f %f\n%f %f %f %f %f %f %f %f", - K(0,0),K(0,1),K(0,2),K(0,3),K(0,4),K(0,5),K(0,6),K(0,7), - K(1,0),K(1,1),K(1,2),K(1,3),K(1,4),K(1,5),K(1,6),K(1,7), - K(2,0),K(2,1),K(2,2),K(2,3),K(2,4),K(2,5),K(2,6),K(2,7)); - - } -}; - -int main(int argc, char const *argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/control/velocity_controller/tests/test_LQR.cpp b/control/velocity_controller/tests/test_LQR.cpp index 9317da845..f2355ab44 100644 --- a/control/velocity_controller/tests/test_LQR.cpp +++ b/control/velocity_controller/tests/test_LQR.cpp @@ -20,7 +20,7 @@ class LQR_test : public ::testing::Test{ }; void SetUp() override{ - controller.set_matrices(cfg["/**"]["ros__parameters"]["LQR_params"]["Q"].as>(),cfg["/**"]["ros__parameters"]["LQR_params"]["R"].as>(),cfg["/**"]["ros__parameters"]["inertia_matrix"].as>(),cfg["/**"]["ros__parameters"]["max_force"].as(),cfg["/**"]["ros__parameters"]["dampening_matrix_low"].as>(),cfg["/**"]["ros__parameters"]["dampening_matrix_high"].as>()); + controller.set_matrices(cfg["/**"]["ros__parameters"]["LQR_params"]["Q"].as>(),cfg["/**"]["ros__parameters"]["LQR_params"]["R"].as>(),cfg["/**"]["ros__parameters"]["inertia_matrix"].as>(),cfg["/**"]["ros__parameters"]["max_force"].as(),cfg["/**"]["ros__parameters"]["dampening_matrix_low"].as>()); controller.reset_controller(); controller.set_interval(0.01); } @@ -43,13 +43,13 @@ TEST_F(LQR_test,wrong_setup){ std::vector six={1,2,3,4,5,6}; std::vector thirty_six={1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24,25,26,27,28,29,30,31,32,33,34,35,36}; std::vector three={1,2,3}; - EXPECT_TRUE(controller.set_matrices(eight,three,thirty_six,100,thirty_six,thirty_six)); - EXPECT_FALSE(controller.set_matrices(eight,eight,thirty_six,100,thirty_six,thirty_six)); - EXPECT_FALSE(controller.set_matrices(three,three,thirty_six,100,thirty_six,thirty_six)); - EXPECT_FALSE(controller.set_matrices(eight,three,eight,100,thirty_six,thirty_six)); - EXPECT_FALSE(controller.set_matrices(eight,three,thirty_six,100,eight,thirty_six)); - EXPECT_FALSE(controller.set_matrices(eight,three,thirty_six,100,thirty_six,eight)); - EXPECT_FALSE(controller.set_matrices(eight,three,thirty_six,-100,thirty_six,thirty_six)); + EXPECT_TRUE(controller.set_matrices(eight,three,thirty_six,100,thirty_six)); + EXPECT_FALSE(controller.set_matrices(eight,eight,thirty_six,100,thirty_six)); + EXPECT_FALSE(controller.set_matrices(three,three,thirty_six,100,thirty_six)); + EXPECT_FALSE(controller.set_matrices(eight,three,eight,100,thirty_six)); + EXPECT_FALSE(controller.set_matrices(eight,three,thirty_six,100,eight)); + EXPECT_FALSE(controller.set_matrices(eight,three,thirty_six,100,eight)); + EXPECT_FALSE(controller.set_matrices(eight,three,thirty_six,-100,thirty_six)); }; /* TEST_F(LQR_test,solve){ diff --git a/control/velocity_controller/tests/test_VC.cpp b/control/velocity_controller/tests/test_VC.cpp index 93dced1f4..9e32436f7 100644 --- a/control/velocity_controller/tests/test_VC.cpp +++ b/control/velocity_controller/tests/test_VC.cpp @@ -4,15 +4,13 @@ #include #include #include -///#include "velocity_controller/PID_setup.hpp" #include "test_VC.hpp" #include #include #include #include "vortex_msgs/msg/los_guidance.hpp" #include "velocity_controller/utilities.hpp" -//#include "velocity_controller/velocity_controller.hpp" -//#include "LQR_setup.hpp" + //Denne noden er kun for å teste velocity_controller noden test_VC::test_VC() : Node("test_VC_node") diff --git a/control/velocity_controller/tests/test_VC.hpp b/control/velocity_controller/tests/test_VC.hpp index ff82b8cd5..d5270b509 100644 --- a/control/velocity_controller/tests/test_VC.hpp +++ b/control/velocity_controller/tests/test_VC.hpp @@ -5,11 +5,11 @@ #include #include #include -#include "velocity_controller/PID_setup.hpp" -#include "velocity_controller/velocity_controller.hpp" #include #include #include "vortex_msgs/msg/los_guidance.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "velocity_controller/utilities.hpp" class test_VC : public rclcpp::Node{ public: From f4d9c574c4380522985297e876959eeed3620017 Mon Sep 17 00:00:00 2001 From: Henrik Date: Wed, 25 Mar 2026 14:32:28 +0100 Subject: [PATCH 109/128] Cleaned up a bit more, removed comments, Guidance_values class and LQRParameters class --- .../include/velocity_controller/LQR_setup.hpp | 38 ------------------- .../include/velocity_controller/utilities.hpp | 12 ------ .../velocity_controller.hpp | 2 +- 3 files changed, 1 insertion(+), 51 deletions(-) diff --git a/control/velocity_controller/include/velocity_controller/LQR_setup.hpp b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp index e38cc68cb..2b4adfadd 100644 --- a/control/velocity_controller/include/velocity_controller/LQR_setup.hpp +++ b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp @@ -3,42 +3,9 @@ #include #include #include -#include "PID_setup.hpp" #include #include "velocity_controller/utilities.hpp" - - -/*class Guidance_values{ - //Dataclass to store guidance values for LQR controller - public: - double surge=0.0; double pitch=0.0; double yaw=0.0; - double integral_surge=0.0; double integral_pitch=0.0; double integral_yaw=0.0; -}; -*/ -class LQRparameters{ - //Dataclass to store LQR parameters - public: - double q_surge=0.0; double q_pitch=0.0; double q_yaw=0.0; - double r_surge=0.0; double r_pitch=0.0; double r_yaw=0.0; - double i_surge=0.0; double i_pitch=0.0; double i_yaw=0.0; - double i_weight=0.0; double max_force=0.0; -}; - -/*class angle{ - public: - double phit=0.0; - double thetat=0.0; - double psit=0.0; - -};*/ -/* -struct LQRsolveResult{ - Eigen::MatrixXd K; - Eigen::MatrixXd P; - int INFO=0; - LQRsolveResult(Eigen::MatrixXd K=Eigen::MatrixXd::Zero(),Eigen::MatrixXd P=Eigen::MatrixXd::Zero(), int INFO=0):K(K),P(P),INFO(INFO) {}; -};*/ class LQRController{ public: @@ -78,8 +45,3 @@ class LQRController{ }; -//Extra operations -//Eigen::Matrix3d vector_to_matrix3d(const std::vector &other_matrix); -//std::vector matrix3d_to_vector(const Eigen::Matrix3d &mat); -//std::vector> matrix3d_to_vector2d(const Eigen::Matrix3d &mat); -//Eigen::Matrix3d vector2d_to_matrix3d(const std::vector> &other_matrix); diff --git a/control/velocity_controller/include/velocity_controller/utilities.hpp b/control/velocity_controller/include/velocity_controller/utilities.hpp index c59efcc5a..70512f5cf 100644 --- a/control/velocity_controller/include/velocity_controller/utilities.hpp +++ b/control/velocity_controller/include/velocity_controller/utilities.hpp @@ -17,36 +17,24 @@ angle quaternion_to_euler_angle(double w, double x, double y, double z); geometry_msgs::msg::Quaternion euler_angle_to_quaternion(double roll, double pitch, double yaw); class State{ - //Dataclass to store state values for LQR controller public: double surge=0.0, sway=0.0, heave=0.0, roll_rate=0.0, pitch_rate=0.0, yaw_rate=0.0; //roll_rate=0.0, pitch_rate=0.0, yaw_rate=0.0; double roll=0.0, pitch=0.0, yaw=0.0; //phi, theta, psi double w=0.0, x=0.0,y=0.0,z=0.0; - //double integral_surge=0.0; double integral_pitch=0.0; double integral_yaw=0.0; State(double surge=0,double pitch=0, double yaw=0):surge{surge}, pitch{pitch},yaw{yaw}{}; - //State(){}; State operator=(int n){if (n){surge=0.0,sway=0.0,heave=0.0,roll_rate=0.0,pitch_rate=0.0,yaw_rate=0.0,roll=0.0,pitch=0.0,yaw=0.0;} return *this;}; State operator=(nav_msgs::msg::Odometry::SharedPtr rhs); angle get_angle(); }; -//TODO: fix these so that changing the quaternions changes the angles, so that the state is always consistent class Guidance_data{ public: double surge=0.0; double pitch=0.0; double yaw=0.0; - //Guidance_data(vortex_msgs::msg::LOSGuidance msg):State{msg.surge,msg.pitch,msg.yaw}{}; Guidance_data(double surge, double pitch, double yaw):surge{surge},pitch{pitch}, yaw{yaw} {}; Guidance_data():surge{0.0}, pitch{0.0}, yaw{0.0} {}; - //Guidance_data():State{0, 0, 0} {}; - - //Guidance_data operator-(const Guidance_data& other) const; Guidance_data& operator=(const vortex_msgs::msg::LOSGuidance::SharedPtr& msg); }; -//angle NED_to_BODY(const angle &a,const State &s); -//Eigen::Vector3d NED_to_BODY(const Eigen::Vector3d &a, const State &s); -//Eigen::Matrix3d R_ned_to_body(double roll, double pitch, double yaw); -//Eigen::Matrix3d T_euler_to_body(double roll, double pitch); Eigen::Vector3d body_rates_to_euler_rates(double roll, double pitch,double p, double q, double r); angle angle_NED_to_body( double roll_des, double pitch_des, double yaw_des, double roll, double pitch, double yaw); angle angle_NED_to_body(angle desired, angle state); diff --git a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp index a81483f9f..9bdd2864f 100644 --- a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp +++ b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp @@ -61,7 +61,7 @@ class Velocity_node : public rclcpp_lifecycle::LifecycleNode{ geometry_msgs::msg::WrenchStamped thrust_out; - int controller_type; //1 PID, 2 LQR, 3 NMPC, 4 NMPC acados + int controller_type; //1 PID, 2 LQR //PID controllers PID_controller PID_surge; From c01882122c47e20ac32a10b2228cf7738233a0a6 Mon Sep 17 00:00:00 2001 From: Henrik Date: Wed, 25 Mar 2026 14:32:36 +0100 Subject: [PATCH 110/128] Added ReadMe file --- control/velocity_controller/README.md | 201 ++++++++++++++++++++++++++ 1 file changed, 201 insertions(+) create mode 100644 control/velocity_controller/README.md diff --git a/control/velocity_controller/README.md b/control/velocity_controller/README.md new file mode 100644 index 000000000..25556cb17 --- /dev/null +++ b/control/velocity_controller/README.md @@ -0,0 +1,201 @@ +# velocity_controller + +ROS2 lifecycle node for velocity control of an AUV (autonomous underwater vehicle). Supports two control strategies — a PID controller and an LQR controller. + +--- + +## Overview + +The package implements a `Velocity_node` that subscribes to odometry and guidance inputs, computes thrust commands, and publishes them as `WrenchStamped` messages. The node is managed as a ROS2 lifecycle node, meaning it can be managed by a lifecycle manager, however if you do not want to use a lifecycle manager you can change the parameter autostart in the parameter file so that it automaticly goes into active state. + +The LQR controller linearizes the vehicle dynamics around the current state at each timestep (gain-scheduled LQR), using a body-frame model that includes linear hydrodynamic damping, Coriolis effects, and integral action for steady-state error rejection. The PID controller serves as a simpler backup. + +--- + +## Dependencies + +| Dependency | Purpose | +|---|---| +| `rclcpp` / `rclcpp_lifecycle` | ROS2 node and lifecycle management | +| `Eigen3` | Matrix math for LQR | +| `control_toolbox` (`ct::optcon`) | Riccati equation solver for LQR gain | +| `CasADi` | Used in utilities (NMPC-related) | +| `vortex_msgs` | Custom guidance message (`LOSGuidance`) | +| `nav_msgs` | Odometry input | +| `geometry_msgs` | Thrust output (`WrenchStamped`) | + +--- + +## Topics + +| Topic | Type | Direction | Description | +|---|---|---|---| +| `topic_thrust` | `geometry_msgs/WrenchStamped` | Published | Force and torque commands to thruster allocator | +| `topic_guidance` | `vortex_msgs/LOSGuidance` | Subscribed | Desired surge, pitch, yaw from guidance system | +| `topic_odometry` | `nav_msgs/Odometry` | Subscribed | Current vehicle state from state estimator | + +Topic names are configurable via ROS2 global parameter file. + +--- + +## Parameters + +All parameters are loaded in the constructor via `get_new_parameters()`. + +### Controller selection + +| Parameter | Type | Description | +|---|---|---| +| `controller_type` | `int` | `1` = PID, `2` = LQR | +| `publish_rate` | `int` | Control loop frequency in Hz | +| `max_force` | `double` | Saturation limit applied to all outputs (N / Nm) | + +### LQR parameters + +| Parameter | Type | Size | Description | +|---|---|---|---| +| `Q` | `double[]` | 8 | Diagonal of state weight matrix. States: `[surge_err, pitch_err, yaw_err, pitch_rate_err, yaw_rate_err, ∫surge, ∫pitch, ∫yaw]` | +| `R` | `double[]` | 3 | Diagonal of input weight matrix. Inputs: `[Fx, Ty, Tz]` | +| `inertia_matrix` | `double[]` | 36 | Row-major 6x6 rigid body inertia matrix | +| `dampening_matrix_low` | `double[]` | 36 | Row-major 6x6 hydrodynamic damping matrix at low speed | + +### PID parameters + +Each axis (surge, pitch, yaw) takes a parameter vector of the form `[Kp, Ki, Kd]`. + +| Parameter | Description | +|---|---| +| `surge_params` | PID gains for surge | +| `pitch_params` | PID gains for pitch | +| `yaw_params` | PID gains for yaw | + +### Behaviour flags + +| Parameter | Type | Description | +|---|---|---| +| `reset_on_new_ref` | `bool` | Reset integrators when a new guidance reference arrives | +| `anti_overshoot` | `bool` | Enable anti-overshoot logic | +| `odometry_dropout_guard` | `bool` | Stop publishing if odometry stops arriving | +| `auto_start` | `bool` | If true, node self-transitions to active on startup | + + +--- + +## Lifecycle states + +The node uses the standard ROS2 managed lifecycle: + +``` +Unconfigured → [configure] → Inactive → [activate] → Active + ← [deactivate] ← +``` + +If `auto_start` is set to `true` in the parameters, the node will automatically call configure and activate itself after startup without needing an external lifecycle manager. + +To manually manage the node: + +```bash +# Configure +ros2 lifecycle set /velocity_controller configure + +# Activate +ros2 lifecycle set /velocity_controller activate + +# Deactivate +ros2 lifecycle set /velocity_controller deactivate + +# Cleanup +ros2 lifecycle set /velocity_controller cleanup + +# Shutdown +ros2 lifecycle set /velocity_controller shutdown + +``` + +--- + +## Controller details + +### LQR + +The LQR controller uses an 8-state augmented model in the body frame: + +``` +x = [surge_err, pitch_err, yaw_err, pitch_rate_err, yaw_rate_err, ∫surge, ∫pitch, ∫yaw] +u = [Fx, Ty, Tz] +``` + +The system matrix `A` is re-linearized around the current state every control timestep. Guidance references in NED are converted to body-frame errors using the rotation matrix method before being passed to the controller — not by angle subtraction. + +The gain `K` is computed by solving the continuous-time algebraic Riccati equation via `ct::optcon::LQR`. The control law is: + +``` +u = K * x_error +``` + +where `ct::optcon` produces `K` such that this is equivalent to `u = -K * (x - x_ref)`. + +If the Riccati solver fails (e.g. due to an unstabilizable operating point), the node automatically falls back to PID and logs an error. + +### PID + +Three independent PID controllers run on surge, pitch, and yaw. Each supports anti-windup via integrator clamping. The derivative term can be computed either from the error signal or from a separately provided error derivative, depending on which `calculate_thrust` overload is called. + +--- + +## Building + +```bash +colcon build --packages-select velocity_controller --symlink-install +source install/setup.bash +``` +or with + +```bash +colcon build --packages-up-to velocity_controller --symlink-install +source install/setup.bash +``` +Done in root of workspace +--- + +## Running + +Via a launch file with a parameter file: + +```bash +ros2 launch velocity_controller velocity_controller.launch.py +``` + +--- + +## Tests +There are system tests and a helper node that generates a reference for the controller to follow. +Tests are build like this: +```bash +colcon build --packages-select velocity_controller --symlink-install --cmake-args -DBUILD_TESTING=ON +source install/setup.bash +``` +System tests are run with the command + +```bash +colcon test +``` + +Helper node is run with + +```bash +ros2 launch velocity_controller VCnTest.launch.py +``` + +## Notes for new team members + +- The guidance input is expected in NED frame (north-east-down). The controller handles the NED-to-body conversion internally. +- All angle errors are wrapped to `[-π, π]` using `ssa()` (smallest signed angle) before being fed to the controller. +- The LQR Q matrix ordering matters — the 8 diagonal values correspond exactly to `[surge_err, pitch_err, yaw_err, pitch_rate_err, yaw_rate_err, ∫surge, ∫pitch, ∫yaw]` in that order. +- If the vehicle behaves oddly, check that `interval_` (the control timestep) is being set correctly — a value of `0` disables integral action silently. + +## Adding new controllers +After adding the hpp file, add the calculation to calc_thrust function in a new switch case, add to the reset_controller function, with options to reset only one integral, lastly update documentation. Remeber to intialize correctly, either in 'on_configure' or in constructor, add the appropriate parameters, and update alle the {drone}_params.yaml files. + +## Adding new drones +Copy a {drone}_params.yaml file and change the name to the new name of the drone. Add the appropriate matrices, and tune to satisfying behaviour. \ No newline at end of file From 31a3a0efea8c4ffafa97fc7dea58eda2a402fbc6 Mon Sep 17 00:00:00 2001 From: Henrik Date: Sun, 29 Mar 2026 12:55:22 +0200 Subject: [PATCH 111/128] updated autopilot.launch.py --- auv_setup/launch/autopilot.launch.py | 52 +++++++++++++--------------- 1 file changed, 25 insertions(+), 27 deletions(-) diff --git a/auv_setup/launch/autopilot.launch.py b/auv_setup/launch/autopilot.launch.py index 69267c3d4..2f527b00c 100644 --- a/auv_setup/launch/autopilot.launch.py +++ b/auv_setup/launch/autopilot.launch.py @@ -3,8 +3,6 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import ( - DeclareLaunchArgument, - IncludeLaunchDescription, OpaqueFunction, ) from launch.launch_description_sources import PythonLaunchDescriptionSource @@ -18,47 +16,47 @@ def launch_setup(context, *args, **kwargs): drone, namespace = resolve_drone_and_namespace(context) drone_params = os.path.join( - get_package_share_directory("auv_setup"), - "config", - "robots", - f"{drone}.yaml", + get_package_share_directory('auv_setup'), + 'config', + 'robots', + f'{drone}.yaml', ) - VC_params = os.path.join( - get_package_share_directory("velocity_controller"), - "config", - "parameters.yaml", + velocity_control_params = os.path.join( + get_package_share_directory('velocity_controller'), + 'config', + f'{drone}_params.yaml', ) - adapt_params = os.path.join( - get_package_share_directory("los_guidance"), - "config", - "guidance_params.yaml", + los_config = os.path.join( + get_package_share_directory('los_guidance'), + 'config', + 'guidance_params.yaml', ) container=ComposableNodeContainer( - name="autopilot_container", + name='autopilot_container', namespace=namespace, - package="rclcpp_components", - executable="component_container_mt", + package='rclcpp_components', + executable='component_container_mt', composable_node_descriptions=[ ComposableNode( - package="velocity_controller", - plugin="velocity_controller_node", - name="velocity_controller_node", + package='velocity_controller', + plugin='velocity_node', + name='velocity_controller_node', namespace=namespace, - parameters=[VC_params,drone_params], + parameters=[velocity_control_params,drone_params], extra_arguments=[{"use_intra_process_comms":True}], ), ComposableNode( - package="los_guidance", - plugin="los_guidance_node", - name="los_guidance_node", + package='los_guidance', + plugin='vortex::guidance::los::LosGuidanceNode', + name='los_guidance_node', namespace=namespace, - parameters=[adapt_params,drone_params], + parameters=[drone_params,{"los_config_file":los_config,"time_step":0.1},], extra_arguments=[{"use_intra_process_comms":True}], ), ], - output="screen", - arguments=["--ros-args","--log-level","error"], + output='screen', + arguments=['--ros-args','--log-level','error'], ) return [container] From cb88a384fad5a014a2fcd7be33496c6bf9924977 Mon Sep 17 00:00:00 2001 From: Henrik Date: Sun, 29 Mar 2026 13:19:38 +0200 Subject: [PATCH 112/128] updated to be on point with easter testing branch --- control/velocity_controller/CMakeLists.txt | 20 +++++++++---------- .../velocity_controller.hpp | 2 +- control/velocity_controller/package.xml | 3 +-- .../src/velocity_controller.cpp | 17 +++------------- dependencies.repos | 3 --- .../los_guidance/config/guidance_params.yaml | 8 ++++++-- 6 files changed, 21 insertions(+), 32 deletions(-) diff --git a/control/velocity_controller/CMakeLists.txt b/control/velocity_controller/CMakeLists.txt index 04e477b4a..65a831a39 100644 --- a/control/velocity_controller/CMakeLists.txt +++ b/control/velocity_controller/CMakeLists.txt @@ -12,6 +12,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) +find_package(rclcpp_components REQUIRED) find_package(lifecycle_msgs REQUIRED) find_package(std_msgs REQUIRED) find_package(vortex_msgs REQUIRED) @@ -27,20 +28,18 @@ include_directories( include ) -#target_include_directories(velocity_controller_node PUBLIC -# $ -# $ -#) -add_executable(velocity_controller_node +set(LIB_NAME velocity_controller_component) +add_library(${LIB_NAME} SHARED src/velocity_controller.cpp src/PID_setup.cpp src/LQR_setup.cpp src/utilities.cpp src/ct_instantiations.cpp ) -ament_target_dependencies(velocity_controller_node +ament_target_dependencies(${LIB_NAME} rclcpp + rclcpp_components rclcpp_lifecycle lifecycle_msgs std_msgs @@ -49,11 +48,12 @@ ament_target_dependencies(velocity_controller_node nav_msgs vortex_utils ) -#target_include_directories(velocity_controller_node PRIVATE casadi Eigen3) -target_link_libraries(velocity_controller_node Eigen3::Eigen casadi::casadi ct_optcon ct_core) +target_link_libraries(${LIB_NAME} Eigen3::Eigen casadi::casadi ct_optcon ct_core) install(TARGETS - velocity_controller_node - DESTINATION lib/${PROJECT_NAME} + ${LIB_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) install( diff --git a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp index 9bdd2864f..6d356cea5 100644 --- a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp +++ b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp @@ -18,7 +18,7 @@ class Velocity_node : public rclcpp_lifecycle::LifecycleNode{ public: - explicit Velocity_node(); + explicit Velocity_node(const rclcpp::NodeOptions & options); Velocity_node(const Velocity_node&)=delete; //no copy constructor Velocity_node& operator=(const Velocity_node&) = delete; //no copy assignment //TODO: decide if i one should be allowed to move or transfer ownership if the class diff --git a/control/velocity_controller/package.xml b/control/velocity_controller/package.xml index f138d6dbe..b55753631 100644 --- a/control/velocity_controller/package.xml +++ b/control/velocity_controller/package.xml @@ -14,10 +14,9 @@ vortex_msgs geometry_msgs nav_msgs - ct_optcon - ct_core vortex_utils rclcpp_lifecycle + rclcpp_components lifecycle_msgs auv_setup diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index c577a73af..5256a61ee 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -12,10 +12,11 @@ #include "vortex/utils/math.hpp" #include "velocity_controller/utilities.hpp" #include +#include -Velocity_node::Velocity_node() : rclcpp_lifecycle::LifecycleNode("velocity_controller_lifecycle"), lqr_controller(), pub_QoS(10), sub_QoS(10) +Velocity_node::Velocity_node(const rclcpp::NodeOptions & options) : rclcpp_lifecycle::LifecycleNode("velocity_controller_lifecycle", options), lqr_controller(), pub_QoS(10), sub_QoS(10) { get_new_parameters(); pub_QoS.keep_last(10).reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT).durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); @@ -258,18 +259,6 @@ void Velocity_node::reset_controllers(int nr){ } } -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - auto lc_node = std::make_shared(); - - rclcpp::executors::SingleThreadedExecutor exec; - exec.add_node(lc_node->get_node_base_interface()); - - while (rclcpp::ok()&&!lc_node->should_exit_){ - exec.spin_some(); - } - return 0; -} +RCLCPP_COMPONENTS_REGISTER_NODE(Velocity_node) diff --git a/dependencies.repos b/dependencies.repos index b925a14d8..a62354be6 100644 --- a/dependencies.repos +++ b/dependencies.repos @@ -5,9 +5,6 @@ repositories: vortex-vkf: type: git url: https://github.com/vortexntnu/vortex-vkf.git - control-toolbox: - type: git - url: https://github.com/ethz-adrl/control-toolbox.git vortex-utils: type: git url: https://github.com/vortexntnu/vortex-utils.git diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 0a546a4a8..89f59b119 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -28,9 +28,13 @@ vector_field_los: common: active_los_method: 2 # 0: Adaptive LOS, 1: Proportional LOS, 2: Integral LOS, 3: Vector Field LOS u_desired: 0.3 - goal_reached_tol: 0.15 + goal_reached_tol: 0.20 max_pitch_angle: 0.96 # 55 degrees - slow_down_distance: 0.40 + slow_approach: true + slow_down_distance: 1.5 + u_slow_min: 0.1 + surge_rate_limit: 0.1 + surge_initialization: false # Debug Settings debug: From e02dc4764841df754c77f8cce8296835711adbd9 Mon Sep 17 00:00:00 2001 From: Henrik Date: Sun, 29 Mar 2026 13:28:17 +0200 Subject: [PATCH 113/128] Removed .cache file from tracking --- .../index/LQR_setup.cpp.2C597C3359569B5F.idx | Bin 10314 -> 0 bytes .../index/LQR_setup.hpp.7B50E8B15298D255.idx | Bin 4224 -> 0 bytes .../index/NMPC_acados.cpp.326F2FC10CEB64DA.idx | Bin 6250 -> 0 bytes .../index/NMPC_acados.hpp.29D23C04AD60A55C.idx | Bin 3236 -> 0 bytes .../index/NMPC_setup.cpp.86F9F0381989E206.idx | Bin 14226 -> 0 bytes .../index/NMPC_setup.hpp.CAD16FEB6583910A.idx | Bin 2236 -> 0 bytes .../index/PID_setup.cpp.3C9DCE2555B8A965.idx | Bin 2132 -> 0 bytes .../index/PID_setup.hpp.486A8D0A6ED861A2.idx | Bin 1310 -> 0 bytes ..._sim_solver_auv_model.c.53A9281F3B2EA7B4.idx | Bin 5454 -> 0 bytes ..._sim_solver_auv_model.h.995A50C901E2AF1C.idx | Bin 3536 -> 0 bytes ...ados_solver_auv_model.c.5778764B3A2FDFAE.idx | Bin 12412 -> 0 bytes ...ados_solver_auv_model.h.F9AC8187D4C1016F.idx | Bin 5168 -> 0 bytes ...uv_model_impl_dae_fun.c.CDE429F376604CAB.idx | Bin 2238 -> 0 bytes ..._dae_fun_jac_x_xdot_u.c.6132B1FDE79246FD.idx | Bin 2716 -> 0 bytes ...ae_fun_jac_x_xdot_u_z.c.ED4BE4E791391EA0.idx | Bin 2796 -> 0 bytes ..._dae_fun_jac_x_xdot_z.c.59CA37EE71B4BA5D.idx | Bin 2718 -> 0 bytes ...pl_dae_jac_x_xdot_u_z.c.A6D29D52260DC082.idx | Bin 2772 -> 0 bytes .../auv_model_model.h.60CCFC5FA93A2F4B.idx | Bin 2004 -> 0 bytes .../ct_instantiations.cpp.CECDE40637351DC9.idx | Bin 1054 -> 0 bytes .../index/main_auv_model.c.FC541DFFAD75A3A0.idx | Bin 1648 -> 0 bytes .../main_sim_auv_model.c.E637641F2593FF77.idx | Bin 1122 -> 0 bytes .../index/test_LQR.cpp.46B8F24A8C36EC93.idx | Bin 8694 -> 0 bytes .../index/test_PID.cpp.575590D7897A814B.idx | Bin 8790 -> 0 bytes .../index/test_VC.cpp.74BA115DB14CB17C.idx | Bin 4538 -> 0 bytes .../index/test_VC.hpp.C3EBE494A18C2184.idx | Bin 1946 -> 0 bytes .../index/utilities.cpp.7F99D4E1DE20E3AB.idx | Bin 4004 -> 0 bytes .../index/utilities.hpp.77C0A5FDF681DAA0.idx | Bin 3028 -> 0 bytes ...velocity_controller.cpp.DFC34CF5F86A4B55.idx | Bin 15478 -> 0 bytes ...velocity_controller.hpp.3E0346F5513060F5.idx | Bin 4532 -> 0 bytes .gitignore | 2 +- 30 files changed, 1 insertion(+), 1 deletion(-) delete mode 100644 .cache/clangd/index/LQR_setup.cpp.2C597C3359569B5F.idx delete mode 100644 .cache/clangd/index/LQR_setup.hpp.7B50E8B15298D255.idx delete mode 100644 .cache/clangd/index/NMPC_acados.cpp.326F2FC10CEB64DA.idx delete mode 100644 .cache/clangd/index/NMPC_acados.hpp.29D23C04AD60A55C.idx delete mode 100644 .cache/clangd/index/NMPC_setup.cpp.86F9F0381989E206.idx delete mode 100644 .cache/clangd/index/NMPC_setup.hpp.CAD16FEB6583910A.idx delete mode 100644 .cache/clangd/index/PID_setup.cpp.3C9DCE2555B8A965.idx delete mode 100644 .cache/clangd/index/PID_setup.hpp.486A8D0A6ED861A2.idx delete mode 100644 .cache/clangd/index/acados_sim_solver_auv_model.c.53A9281F3B2EA7B4.idx delete mode 100644 .cache/clangd/index/acados_sim_solver_auv_model.h.995A50C901E2AF1C.idx delete mode 100644 .cache/clangd/index/acados_solver_auv_model.c.5778764B3A2FDFAE.idx delete mode 100644 .cache/clangd/index/acados_solver_auv_model.h.F9AC8187D4C1016F.idx delete mode 100644 .cache/clangd/index/auv_model_impl_dae_fun.c.CDE429F376604CAB.idx delete mode 100644 .cache/clangd/index/auv_model_impl_dae_fun_jac_x_xdot_u.c.6132B1FDE79246FD.idx delete mode 100644 .cache/clangd/index/auv_model_impl_dae_fun_jac_x_xdot_u_z.c.ED4BE4E791391EA0.idx delete mode 100644 .cache/clangd/index/auv_model_impl_dae_fun_jac_x_xdot_z.c.59CA37EE71B4BA5D.idx delete mode 100644 .cache/clangd/index/auv_model_impl_dae_jac_x_xdot_u_z.c.A6D29D52260DC082.idx delete mode 100644 .cache/clangd/index/auv_model_model.h.60CCFC5FA93A2F4B.idx delete mode 100644 .cache/clangd/index/ct_instantiations.cpp.CECDE40637351DC9.idx delete mode 100644 .cache/clangd/index/main_auv_model.c.FC541DFFAD75A3A0.idx delete mode 100644 .cache/clangd/index/main_sim_auv_model.c.E637641F2593FF77.idx delete mode 100644 .cache/clangd/index/test_LQR.cpp.46B8F24A8C36EC93.idx delete mode 100644 .cache/clangd/index/test_PID.cpp.575590D7897A814B.idx delete mode 100644 .cache/clangd/index/test_VC.cpp.74BA115DB14CB17C.idx delete mode 100644 .cache/clangd/index/test_VC.hpp.C3EBE494A18C2184.idx delete mode 100644 .cache/clangd/index/utilities.cpp.7F99D4E1DE20E3AB.idx delete mode 100644 .cache/clangd/index/utilities.hpp.77C0A5FDF681DAA0.idx delete mode 100644 .cache/clangd/index/velocity_controller.cpp.DFC34CF5F86A4B55.idx delete mode 100644 .cache/clangd/index/velocity_controller.hpp.3E0346F5513060F5.idx diff --git a/.cache/clangd/index/LQR_setup.cpp.2C597C3359569B5F.idx b/.cache/clangd/index/LQR_setup.cpp.2C597C3359569B5F.idx deleted file mode 100644 index d1c96570c7059f612ade7721ccbc7223e5d034ff..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 10314 zcmb`Nd0Z3M6TmYuKp+bQ2zOZSQ|=oPQ3z*15djsVuHr4I9HMw2Dj|qa@Iq7&5i2Mn zDp;|_R%`2R1;wMx_4lHEdF%i3`Pk2XGrMo!%$qkeZ?=gsk&$6e98UPu zm_?b>awqdR9F7S6%gN22WueC51Q>8Qi)xOJ%ba)9UwCzt*NuHM*PcCpZqxC!(vEK~ z&(0BhF3y~pI2^$iVH8OuA$zs-DOZ&t8l^oMUlcdoiw z@Zrt$lD#Q8T^UiiT@PZ@wBLLbkoLo$9lsyR=*?3+yZ1m) zzVFTNza%#XWau^&0m^nUgD88zQ*m{b10bN86*vu}01cp7q}eDTENjraO=GPR2* z8tAxMnpSodCLg&iJo#fyYfw^DaQfOxqit5%bGlP) zu9TbsI;4uTHhW9)81w8qikTYTga7Wh+L$-;_PS{?Wo?IaCRjH*K41T)V(0ct*>*oS zZMO>M_s^_z9AA)|yI7ntev91y$$Ud<=-#_?J1*(;wY3gEFyMRWOt$gft{0+NM4&E>4_gI{mAfG({$3F{B@&4+2axTKK;ACP< zW982q_j~nR2!rKQ+4__RTe|=T(6{n4MHqxrt!QKB(>pH)`=ayT>BuX;& z=T7G7-G;0GrYEkretgu^!~brGr!KFzfYB*Gt=l3tn5s2y?F+ROx8bnBN8A6J*~PxO zN9X);BG==)uliyvre{`MXdlhrZB}|dyRxoAGPvf@xS|cabq?2h9{r1I-C1GL(P38j z^QKi}_DuP3Z*#}F6{RL^yG;b+7S*MNN7~qmmt1?zSaZJLJuI#9{^qSskM8rZfE{Ux_COZKV!my*7?qM*#Sw~TPFLprZ4=veQ8f}*Oc*@JinT) z(Ldi3YD_z4Ibo8^s&|VrYrYMz&-{bh-#)KUaLc)~+2V)hqfYxmeU^VXaqIo!{uhmZ zbSuKU`ZK9iI59x)hd14+RVNRPZqR#`qUcSZb#}oq^H%r8#yN81Rcpfjdb_Fr+{Nzk zuY)IcZW>qSVzXvm*-MYQ#6JzqQkMKY-Y|cDSipndciI0D_m9k5qbELY?gY=1UB3LF z_ERIYoSpyq<<;pcj*D{l9J1&=HTRC&;WG<;e*Nyj;qMP$DCl|L>#)pAG_vsB^7vty z+AmWkr?wbGp#C?s<(`QT%Xw4oA?1SxcJL0rxjkj`Ij?yqP6&*0 zGv07@w;t9XTF!LgmBj57pDV+T;PfZSqa_S>?|xXL@Ltb8_-C zr#uD@N02ckC(&)!2q8B$nZq0e(Lu12d1!i=a`-5gqQ9>if3XY=I3^Ktv(OFqz~CNO z$~^g=B7DQjg5!e49sDREcP_f22Shz+1b0n$)d&q~uQDDEIWk7bor7-3a?bLW`OrR& z`1#t~8g`0>CCL8RYHhTM+rk<)4a#PR^GmU0i%lrlYMp(RV+tWEwxnnj7x$}@X%{k3W=12QE za7;Ly1f*h%_+8oV)7P7Y+}TK6#&s;?dda-RUJm$qSN+$0{%yg>h1_g(gNw+;O6I2N zW{AbX2^YMMyC?ieaWatx17JM>ezH;4qx?9UJmBPxLmE`ypAhRM`1*{HJ0FeE435p{ zmH=^p3m)T#_Q{1GmWE!XIC*G{BCfEAYbQ$(CfIQ_)dz7tT49d1qd{!Oy2nP`Bs7MJ zu8COY%5&9IYB3sVAxk;7-RZ%DrxYgxjqnUa&%i-8QZ&-x<1BKuMWcwTt5T1Va|Lp)0p_HU`i}owJ54a&q4D!=n@_PwE++i zbq7Fi0E`L01I>4!%VxuOV9w?t?_fCLBCcl<*B_gQLgpJ<#0|sz?e?J1;#M)AD?;mL zSPDjX;!1Wb1t-jRtG1r`C;Wc~>}yVjOr5n%Ms+CW;rONlcnY=h@;H9Jq8PU_tXNJrFV7pw5@KBOqyDn7T%FZ=Lj6fx@Z`yQ;Y@A!ic;gd3bUnc|`;88o&sL0g7n=&jtu1+yr&d z1P6fp(hK}vFvGE_Xy9~z1b4RSAQ2Pk5$THu7BNv;Q7TsvG0}o(A(m})5=8v^>svD} zg*4$IWw0g`Q_dBXb157(o_zgH;`1pl%+*AU8{bWYpBFJ+d@mOH@%>m7#1CRo2tPzc z0~;>vuV}t)@r8!_H$E43K-@)+r8=@>FQIjG4&1Fu`GPS@_B1=p?6QfK*O1>&D6 z6l}-Tz&K*pgJ6FUT(B+L{5RG`7hbeMZngykTR;atC}MVlU^iJZ5%UiS{sA50YOA?| z)m$CyYSR<{9Q4b{Viy#UmUL1*((}D>oxyzN1oCGo-N>oBe#gz2VVQYn{X?5w1Ow*$ul@h9=zaq zsPR5?1KwwDz+>2Mz+>2Mz+>2MF#Fi5MIFO>ywt?V0M>G_!wQI*V$doEZNdsLRDcPa zZ4_Y7W;X@eFaKrX{`(8K3)8!NkVCu%!D}$au}s9Q;0ji7Rk7==%iney-U@#q#Y74s z^|0q4ii(UU6lD|TL@3%e+L@44AZ0x#4Uw`!db}W>CCk#tQfWjnIf5KLJYPhPIc|iO z2$ry9MIa~w7MFmagv}d3uz}6(AZQ0Y?7kFp5(Fm++n=qCvKjagjP|_5UgAa+iLj4Q z(Xe-x4I&f6p(tco@+^fUWTl8xSeDoUu{$6IyA_4JM|&TbVsBBh|2_yKd=vOLL4*U= z-yU)g50A{DAZDb?q;geq)A?@k?pwcXAEriq^fG0Eru?HmdfBIIB`Lj(kkZQtDZLES zb45kW@HuaXp{U#hvzj0a&lg(=%>JthFQj^0iuT-mrLKF>^EdTIVQ6A84lydDd2z1dXkc~R7uvKp z4;-~KdsHO`6X+3c290LWCd=OpA#DP_ zMk1dx=b4)jpG)Q?n-Djx2VOmxkeAehZ#_g4Zi1X9P_VM(Rp4I*FP+V1%2BT2w ziPDVH!sDX6s1+?B6k`-)Oejw?PYat{#N=z{s|f$fHTziftF%f1FEQ<7x3?i~9y(m>GrI={_XhSU1on$@PO7pu^ zA4Wh`pY=hC;NUXrWBfEV%>JZDF1lN-gh$ntbXGq&` zo?lNLBu2SPvjr&ZK4KJIH21N&6*OB3&mDMdyJct3R363H@$A%yJB9PY)v&#gop})~ z>I92Upz*uAE=`J=zIWcVFI1+;2Y(pul^Zt;)gO*LM=dg=XkN6+5h*60&Q~QukFb}I zZm%$(jcSk2YdWco)E6ruVsflTB5dR{_6@T?bTwSrze0F*?cdtBL%E(GTwzhgvO01%I70eL126~b^B_Wa#gd##BSTuU@Xb&PQ zhr#7!sW!C*{oCVoeHn@rRy->m;>;<&DZ}tX%0rB#0Y>88PBE>Z-3rFo#gy#Z3d0EB z0{$(~$HDr9VFYMU6!3 zM0-3RF;fAS6`(rFiJ3~UtR#HzmgFn>iMz$9ZhQ_V&ry%(zkY)<#X=JxFI*KZR_zfiW9RbE}8Ra;Z}g@(gDES}lN1{MkW z^yYy~@tjyE>KI;(%CZ#04w4j;C&*I; z9u#E-`7BzlvD_3}+%LttyFIg_3Z+Uph~%Iuw3O^BCtCoY?~yXfb{kQq1sqzy9Xqu$ zQ914d55lcr-wJM+uf0DRoN-Wp^B2l8%bfGvg8PnbLDQG?3Qo(*Ny{n!#$|jv*uZ=w4|8fyy2?(QK-%dZ-F@M9?DacWFN#6ZiV5k z5Q7<)Kg)YZj)*k|Bh!^d`~n3Sj#DGp{7vy56M#2X16Qv?O7BaGkdLyzW3 z#{Yn-A2XgAOZnOKY)2dyD5ej1eV~m`RCr*doU$XWQmuXNlLfB*G1#HjMtQjj+;k=>8?I`#Sj z+RCrM`4za3oK?uRE9BZ^B@PcxoYZsIx(l5-tHHS%yzq#Jez=pr>sfLS>Il?;Zw-vV z)}+w6*!v)a;yH#KbN2CGNnbHV^-z*D87~f^6zNzNjgyXNQJPf7q6yN8ESe-`STseN z&Z4Q(X)Ky9ox!46(%CGUBh6q@rgScg=1a3!lr7C+QLc0Wixx^3u_#}1e zP)!C_sA0wCKsf{vE(T51z+$tg7!28LRm^@~0h(x0iCh8J3b4V9U526<6OIXM=O{mq zsDzA*nv1IRKn-ZKG)ElmDQ2d|OcB{*S!P+Rj~_K)QUms6&6~lbnVb@oKA?6G^sx_6 zsFR`ALrOFXwLEzDKo9R&3PmB4GOj%iOTp=lHRoFfT}36in}wSrwyBu$vhZS&pM@Wb zf-Hhq6k-vgB3$619r@||5#Ess;6{NZrA+5|R$hR~l`&580-l|IWuOMv_5lRQfmF(v$!K{+~d?ol) zvY)R6|4R1rufXILn6hKKNg>x1$8-@>$TctIiZR2(Mb*+n?hlcbX&%k)+wxL`SYe8( zhuC`3i&0h_+AZw9Z43CdzzCeXDCQdQuaUlOXy0J9D2ezH+G!G6+g!s~-H`1Da4hox&b%O9O+Z0tu* zELU5u`V{5ma^yBvn&EpPVnpmFKnr- zMZ+$N&t7_`C1Mh65(bk+O>j+6ZE_^@PViyL0uxkiNhBMV5K3faU{gjuNr{*;a4iGX zHx4Cxmw^v)i82T*W9QH^7*+WI&J5K(oJr(z3b~v~ zE~k;p^vtOlIS3uHq+0i~^@E7a3B5E&X7o0`z}v>okF2hh>9EA2@;({8j2J(Lcj z!|1_u2t9%xLJz0IX$kE`d(*zOAMH-Z(1COm9ZZMP!)PDcgN~#lXeTPiqHB%5$t5YB6)D#1c{ke*K5D*x z&p=vzLA#?>ZuHi$^S3RoM+bIpSnc^mv0;C<-pc)J^lhz{W z4X!&x8!iZ$m;29H{e`oG+M{_39b0Ns`c&s{8+&`~J!Vz%MnX^8IrILZsMCp#E4L z`10dLvo;9+iEa_>*jxE1QakT)QtF1i{OqrEou)tCDr^p%Wuzt3sb>p1bTxOsp|{)$ z>;L7Syv0V+LpJ_Lw(IZu_))Bd<3Lc`yQh||IkigcGQBY8!;@CMsekt520a^Uwpu!2 zYgdq{H4PX|p7^x?$+I)}n?+$OA6UL1m+nmN&Dx|(9@HlYFT)|CxwL^~C^KV%4VQ-d zae7FMSTm)WnhRj}hyxbo{W?f9lrOR^>@3{40Jep}-k6W7 zdV~yRh>B~Z8hb8)y?M*njE<;frVJ%Pc7!y-mkVGwrqfOfTzTdU<$-L~Y}HaOfNg9a zJ>IJvK(-4S+gxd`<^tHCIMd->UC*r;DjfBWw25T70Cw6rwtQcIj+~+1#4kphnQ#GY z^@Qx8*g~RWC_mIYNEYPA1+eLiRCd{%9xmOk`$jL9avAkrzIrpuDL4H1T=>U zDGf@vN1EOvWvth9FEJ$K<+RY7|1&*ONU2al2Py7A6+HMJuPS7edb)4BG!n;9a{TtR zO2!4~e{ji2M*o{<<_jq=)PJqZT5r}z>f;1<#T}kQ{)U~20wRhaW(<(R0n(0*6~@{T zmim;Dz?@|DkIpsVZO(tZTu4Qr3gx6zIqAbHZIsSXj3I#|pF=v`+sQF!?K&RQY$2I^f%ITcb0R$-%Xe3h_oMeF@^<1-AU!2Q`^TP}d@ z_in?()nSX87|Ijbfpnml3t+pOjQ0AU3TzQl!Kl2e*mVXg=gTEv2PRxmw0I<(7Z5(^ z)Qpq1HFc8}h&<96@2GL$n~I}6&vlnYRytZ!+Ig8upkA*DtG z)RIoMs6v2EfGZ?iS?qZ|%Ini#coahrtc!t*DHjM!g0LY7G(gkoq}<65G`|XnNHmA- zlyE!cz$OS290O6ONVuYa_k!@~g?!pDI z&5vLI?UC@WF^1yd@!wW24+bVWmqyTYW_7)1s#@ zV8iGzQ~16FY%DL<_|;v*#`(snUcDe29~wXV)rn#gcnQX^w{OLnINkqPrB4f3xwYID zP9lEs@>EIxc*TqS{)C^O2BGpjR@8NwcIt{aMcUg5IRHz_oILfr9(w<&kNXa zI$Q+5HUw-09U+1x1K6UuV$#A}!)ub399lsJv?3V;6=VP^k}*a>zICF}!Rq1uICOFq zv_b^;tAGul14M9Jp_X^R%&C@=R*GOD1#Bc8DT0jwDx;}lB*Q4Mj%aX9VUXcGBAH_^ z_mhespfr*(Zy_5N9M#6K1sOPtWDHr@Nmz6wW4J;#tT&P|R6z!wA{oOJWZ)>0F+`y@ zeBVgM@PusOCXz8UA-lGJ+onZ#%6F3j){pPU0004_=q)imp$HrgR0OjVWax#mF)l#{ zT%v5uN|0e2Q8tF-EBPfHaW*0&$N)toVX)u8UD+_mm>){o`4Cr5gLG_;L9W! zA58}X@TE!s`H+mU2U$QoBxBq`Z6F}WFzuMaAdZXQU(taoBZ26(T{PVo$ywB}C z=Q(HI?1F+?y-1W>KfANJp}j^T5{Y>HZ*_Z{BUUUDDSSkt&OL|9OIL&@tFOIwe%7v? zfraL`UgMwfcQi)4Z;dfNimP|5nV)BFPP=Tp=RSY#_zK?zNn7j0v&O#e9KQ6_@e-M_ zyR>CP^y$W$YZsT5EV^J@6TVRP&R^c)Tip-lDz*LF{@fTk_w>^BM?c^G(coH1Ucr^I zTi-?vT{tT{@i1hCvoP!Mr;Bf*&66Ki?>n4*_*&{K5&Jp+S-Wxu^le*~sBR6nem(b7 z!Em_-2|w{GZR;v{E&jcQ`5!8Ve_c~^yXRkf*L1!;`^u}nW4UX7XFK$Ep4b%=tiC>S zExW6t;`KFK&knWrzxu>3>x`BY)e|qaPr3Z9TQ^0K+nV$Dh@oB>gVXMVk&)#Ut;+Kh!FHUn`T-KXmcWt|O^4qVbssCMF5dZkbwJ-fn4sN{g zD63!E|Iw*%<#$~Z4f;EOj5%}a^3Om2{o0usDPgDoT9G&Ly0)@@mpd`|!};@{S!*@? zsYA4}`QrNRP49fm`!3)3(sr$+ZTaBG-3w>#dZVTE`@SF=W`XqmVu-Sz8t9iRPTC>-UyrwttgzExGy5eZ!(F+pYZ@Onpago^jMYEy}2|yd+w^wz+mX zh(u%7x_=*j?>7Nju`y7Dd{w@I!aVgny-0?03V)K$`4+vEv{kPbM`5u^WiklUPfTe_+GU*7K@8iMWKR4ZV4o#%kBluXnyMb5w$o5iw8k92!TSPJW=BrkN-q3G=_e0 zbaKvVwKx(#5Tpvy3UTr{rAHQw#l^3*2K|En;HFv}jm4v&8if!cNuCryUhqY1_0aL= zg#Qf^oA3iFxk@2~OTwi@-21x(`Qf=|zfp@Luy{A%zevHZwGzqD1RsJB5=R~#fq)|r zNchfc$qPPvu-Q+_A=7MAF-cVpZ3oqMP!o4>Xb0$aKmg@YNFRj^G7X0gLf}Er6Mir1 zDd(mc<^C!G>C$u=BuNBhC^pO?DJLL%mOY1rNkBQ-Ik}Wd@=Iou z=|mX;Ibj<(;RT?-xS-JmO_ckfxDVz~c7xpw*~Ci%8UcO;^pwZJJ`UN$9045z{unjy z=%PQR{L1C8RdGnC)M{fmRNKhVlr=M?gt=4CG^=BqnfZ95mw)K=}wLkAQ~qF_0gl&y#gFCFF;K zRu-`D^R7kv>jjiA$>(U43MGYJBFO_GKNLAK#D4PX$GiNW3>I@JRuLOOqK^s1#t~X} zz4epJrDb6<4yA>sMUoeAC_N-ypkz<6TPfx1^Ft^tRxj34Z*Bqg7GQW3tfOF~)*b}i zLF$dO@0RU+`Nbn~VgXrXmLOt@fD&X0URrC~-JN%*!3+X2s*EP0`WI^Hi1gEy2}%wv zku6bB6{=;`3@w!{WvE70!%(fPmZ4@@Gea%17KT>JRx-3owu+%vSt~;aKymPKJFQaxM{9*;q-e53!9yW}P{JxPwE>^~*!3f$i3I8>J3w2Sb}7 zcr%0&=?gwrn+IA{sn~TInI?uTm`K~+Gjhjp8gM9566r_YIQxmZoC%?C##+mcIO zkE4;7@C2oNbv{qRFQCfCN@l%RHC8d?C~y=JRip!tOv&p%v*B<>OQPlEogA`+TVlxM z97+gJV5q`ekwhloP^Gz&p(=9~Lyd8b3G_)voP(kE==OLj>xk}Ps0Si?z(}RN5Y-ED zl=}hoLnPrR4oI_g8_wIY5mAyTuj8;0dP^8%gg$|xipUD)g_V(&3{^!|G1M5=$f)87 zb1>8%+U`|@Lmi{ad7{?yW?B^D4X@s{Cf ze2JDsjDPktyyWLU2cG|aU3U07?B_ylVIUcdlcKPrn2cF{;PTn+&ZS*a0VzTh2I3R} zrK{7Ka)EnL@4h0SEOizm%hBg}WpCy^sOKL4W;57f=gD^j7T)i}OK_TcnudggM>DiDbc7a4%JL_+-5tbZ_5S*)WNh2q;f#>S z&3p0BF{+q&@-s;}9DL7lh6KOk`J7jyYY)2d(}G6uHV(w^_~{={H{^MXbN~69lXnha z8x2Z>hRn{POl785PY%sg&h*mvCwJU_x~}pdeuY}5Mq~xuHGK5oj#+mM{~WD~Hj|kW z|NhptJ^!OVCBdz8vMH6uw%os*Y3s_P%429$o(0{rw2dZHVdXt1FQR((Q?>dvAM81Y z%S2^E<#IZ@s-cRZy&%~OKEweW8iF8v7@#~1I(!%)oEvs2erQwBdAw!#%ly^EI1Z@| zY9l3$Q8Sg2R;Fd1)JybC=?InwGhP+qg=At5-p!{*+9*X$jbbR;5^bfj7*k9NC8ODx zN@*j+ZKO#XJXvsa{rmSWP7zRrq#}ZN1@HWlN`|T=RbJ|R^W(p*lU#b&6QXESJPna) zx+#a!AVdyAEO8WvkGaNCFq7hCE7v{wmzKlt;qXVQBE6aa*1drZgLiG0e0gNZFxZLK zJhCU+lS$_DC?_$8At&Ir-@8Hbs1Gvwz)nozksHF?5J5EIanBw$4iQ9?vDX{Y=8OM! zUCE)d%dSy9tWq!1$9JEPa5U+fmd%&8l}1+ zjrtp(3rE18@;Ic9LmIJ<$ERE6G4LmR=^wvcc{=;*1m5y?K*SE(z%Bmj{{H(fAHItZ zX-0{WDFqgzWhz}x3C0A5D(n@xL{nVP?3D~v*{c}pfw&&J+&$f5bU)2%;$_hOaLYtu z@nZC*k<|<5eug7is4VoBa@?8D>L?)Z%``l`u)?S2A;JOE>Uc604w%-!(|~1aGrft0 zlP=8eZ6bLzQ#+H9<%H!hvJz8?g@&(GS?X;jdDIQ6ZhDX638vNwJWdBT=_PT(jTE0 zSjXmVADFVB=@E|i4hY5hNfhJ$>)w#`rg-)KcJ` zAYVkHaPL<@r{?O8PaAK3S|WrQ!bO5tBC2byZwkVw3kVQTkw|@fWpY0Sr&MXxLH-)x GMgIqBpVBY@ diff --git a/.cache/clangd/index/NMPC_acados.hpp.29D23C04AD60A55C.idx b/.cache/clangd/index/NMPC_acados.hpp.29D23C04AD60A55C.idx deleted file mode 100644 index c4754ccd0b6d25f937bfb5457578579e95628a28..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 3236 zcmY+H4^&gv9mij;3E_pu|{4YdBLIttZl3$1N;-=ja%b@O?5{3%bulOnQrs*&56(4`~1emB_99P3e({~bhKW( z-0Cm>PxeFN(l-moWGShMlMmMTBERX>j1DuMt?M>!p1ZO6yI0R2+u!7BZ`w0EKYM(C z$(H;z+uY3u&W4s$ZP~oxtM}UN>WpW#h97 zd{o>lnBZnIPWwqY9{KfHk`<07!T>AXN zZPS$JXPq3ZXuR=y@}J)rp?lU{eD~PHN3R|0S5JPh>aX692iOG-io?5Fm_=s$(L3^2 z-&j6eH(j-4O=0wuO;g8yzidv=^p#iI+&M=spNw)Rh2A>Tn08K5SrW=ecUo=W& zK@$oaU;-T2&TjdvGtNeiL3*&=q?-rZbK>J`yJGx2jUsXK!pK6E7{Ffq$;$s;@D;~t zkPO=#&*{Yg_I*ayCi`NAokpqHE;E+Ji2>}-uAGP)kL`BQNRI7Uk+T$H0DF4LGYOg3 z=DV~=f^B28Q7s0r6Vmr|kxSp6N~2_K=c;q{VgOs#{2;}(W;C5f(b)E?y}TH}E;uBc zq5ACy9vV^D&Qs^<#Q?Va%5{0uDhE;QsE*03+ z+*i8Z=k8W$Py}wx^Sn_EU>9pzzigP-zDSPn)nQc;s%S9~q_{s4R<`$@j2G;Weo@V! zL`ZKVZE9%0ZDB)CY4_GT1|@-=5t*R``|9^Sz42YjZ8S3C-t*P@dNF{Vvmv|PF@XK%bNc(o9~HgNpjb$rOeRNzJ!ADkZq=tP z-88b{)}?%@UJPL8#0^Z@yI%JJjiRufWy(qv1K6{tZ|wW0weRl?io>=w+Nu!)*e4W! zk_^fEhG;}$+h_8b#Q=8Uft9+K)t?SAC?1C9B%K_xyx+Sm>)UGQmWus1^u)j@ zDArH0C5m;XIiG}G zqF8sX+Y4-?*fPFM4=|$GkT%4@EvQU0e4X68pTfNqNDE?M9}~VhZ+vC!yc)~-(tH}O z6pU2Dix7p*p(88{@B|s)i73_-VPb)23K5}D2@s0sBk&VsJRgVC6`qez=i}jsQmil5 zX9$$A?dmKf#{}C8#4; zOi=ZwgG~M33@O$uGgDBN5+U`olG4Hh$tcz;wMGFvaLH)3KnadN9<;z`NC14svcP4K zuNL)`v%wu}@j7uP?(sUo*}@AVuo`N^0mZVwXpj$h<%5UX`kQc)ENPYjmGikOP$#o>1;v*aH)S))a4a)+|L59VbC`EpQT;F@`#!J$H ze#|L{(UA}ymowg(7`{jDIJX5xiN~Dbo)#|oNFN=(Ri$Jp4I{$@AJ#s{!MIF7Xlw4+Y4ceY*?~o@%|0mmpV~tAv{d+`&nN#{+7^yCGp&;k^JE-V2;t zcqc%H8-SAtj|9j7R4faR1IR#RoJ4pJKwep&dCu_Rh3}D?^&9*~$f;(7reGYTs;OMI zKsX+?H5IiRgb!K>6F((<5JXJ)Qx*Oc{^Rh|PYWLe!G%BB!%c_2EvR~UPN7n3b)1Hw PW0Zt0>}$fGO!)JEs|c=R diff --git a/.cache/clangd/index/NMPC_setup.cpp.86F9F0381989E206.idx b/.cache/clangd/index/NMPC_setup.cpp.86F9F0381989E206.idx deleted file mode 100644 index ee7f67abe0f8bdc2e31b66d091ed7824685d5605..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 14226 zcmchdhgVcb7r>cuaoOc9W%seWz|z~&q=+3AR8S)h#uB5L1SJ+gz_wsw1rdy&J&F<~ z6oY~V>_H@g4N;>3NsuU_*hq{q5n}B6&6@MxT>k-cPLlm`Uzs;^`^@O!k&!EVnV1az zeE9sNsq-dFOiWC8`tRI%bK>uHF)?YfFfp0G?T3%9r`+mo_0zz)(c}I;vZ&9vTj}`v zMTgHa_Wja-LalAaxU>IWB@-W(R}Wu`7g*tmdox#rc5eOa(W+&E+{wWGMJD#s`lhFT znWoA7q5X@H-x{vyUq@~~H(%NK>%w5$=(x`f{SK0VqJs^e+pWH}`N)`B`Q6PPzB@W6 zj<{F#xc0+juIRJA2P>{^ zz1%v`t?!D+;*6BMzZPB1J5%f#O1j?ey(aRr_$5Do;rP#jVc(>-&kos=u+*GAqh_V-+MSLKeXnwhtwD-L(AGcB|jWKP<125&LD zHegQ4^3)txk58(?KYv(1(dTi-x!fx`#bMw19oRR^W$5A+sbPHzHznGX70o&~_M6y4 zm$v)fX*+JX@%-zVCG!?+n6kgnxu02H^wrYye?4?OE}1_1U}J6nX(vuj?-p?C@FQ*X znDmj)ya77-Xw9DLyX z!R49DuKm7$%eB~_CI^rDR=z2=#87#DUDc=+|HN)yb?S>vcmKKbRsRdoXztBFD_etR z`tDh#v9Ndain0woyJPRE@(G^t$ftCRy;opAHytCh9ecp~i z#S4lL|K_8}nvk>Ujm0IeE)9krH}=^-n-OJ*PO#EE{q@Bc-=>yMon|O>zWUXW(OYk4 z{%A6O^qAOBaL-ZIJ#Ws5`#a;)rUjDYJGy1{rc@MA7KK5c`cc%yIqgUVQneTS=l;iCsp`o|ewhUB$8&us_b~VK7 z_sve1PhB$`8p!3G`yuk)$F4{6-~8HtwZFZ_u*|0F8|(2a`glJb_jHal=kT*_X;;ja z)Zbk&TK3(|wDIBk2O7^a)nWT<(DCCZoCb89kDIzT_CVj*qM*?W|qjTj7tXxCQ&S;9gN?&SpNs1?v;L zxF%*^y=jFf(F+u6g=>^tCKn%{@Y@fSu7@puv%({&I1j7yuxFG^CKHD@XX>=MML{Me zH8F=i8QX4(BsxMIh>I{R9T{sk){#++T@0Wp4pYP{#}cL2Z|g%%tvI64E4m5eqBKA; z1khN;L_l95#aaZo6e-F92e<@yF=?PI(2`ML5VD>R>?8cG332ckyJL%{VoqNj8%J#cAVg1=%IH1uHTJjIl&IQi**l zaey$?D}@iDRV!+(5Kmf=rUhw*JGA4Ge8|iUYY9huCB8CYa2%Penk-(#ktv!fI^hT6F+l5D?zqq zHR3jDX4}$dcOXdz97HMBl(Iq0?5I(!cRy<@=g9)c1@6LFd9u)cp{U(F$wq_ZdtLUD;C%mIm;(hJJ=nCQCI<1y`nOQblX!Nt$UIkfnL0>4p2s zNO|s?^I=Y1I1(TU=qe})N0ObBU4@G{lH!~q(gzmxrF{&|&eZtxaAY1HA?QHbs+6E- zwOibz9O)+MX2zZsZxOF#&x&`A_hgiyNU#RvngC>p7Kz}#MAt-7*1&};CKnfaVxHt8 zhg{?)Jc1`BNLPZy!A=`}tIMU@ET6Z2CL87Bl}H$_(bKe{0(?jO%jD@M3-14G5+Iel zV=ob~*sg8ogG@Q%CGi3??=A5LG*LCtM))|6 z#Asqfzt53Ls!5{kywBh6(%|f#^l?7sKGtmVeTMlEM!v4TdPaV3e%_2)u(Sm$guG*B zdqvj8p0YkJ92v|F7EOne{a{h)ITFD|09k}>gs9jYiR2=IEYda-$eNH<69TK-g18n0 zu5LljEo>P%avgEkq5L{>z7FNBh--!NR^-`=L^bEg4aD7m@*BwW29&oUZ5t8|&lowB z9f)T$--(o+kXK=D6$ay7g`HPnQSCUg8f#Z$@VvFyWi1xflq2h~%R0y#v34W8J_m>9 z-~sIY`IyVc7EpJ&0?37!D}-Ext&6aVUB4f5`=Pu7%PTO)B(E-o(u=aNBn!*g==WgDJ=jKgy2Y_COK)5%7(g9` zF*5y^Et{BdZ|#hc(asnd?TnGp&KMc(jM0B~Ch+6zwZp>_UkrHb0Ve0;cwrPq`_H5H zUo6_cS%S}QkAGqY(p+PSe2KX*e4Z>t@}&rHI+CX|7V>7scRChAf=I3E=XTTvJyTTdpD!_gMZ+*qj=;d5Cg64%ySki>$ zf`B7eu;dElW-Mujd>xx#$JT=HX#8?s_7A@nGGkCQT{T~suUwdCniIp)sX_^Uw{IHHPDel)VmP%Wn6@M7eU^RLbkK1)sZ^XuMYJW zuCKflH;+&9|4`161)d9hg+Fs-q1!^>jIvShY<3ezGt7^((I@QcYGhZ<5;=}$ly)_c zPawM!kSnl91@>X9bs9UIhRa-s9qJgz#!rsabQD^9%PTztzG8`FN>l zc1AN4?5X znD9|LvJds##}fQwUS0EA`*oj19m;-ezaKjY3vPdm%#!L7X3T~nN8*xkR8v&@V0v;E@n@mD9%<_lNFA>rn=43cC~)L1 zvb_u6cOPl*!|OYcQwMSuX27UtPMwf9VcSjbrZ2WRkPl;Lt=8X6)@t zxFw=_8fy;eQUs(+5x10?FHh2uG#$#*5tk0-*(iWEF4@zwQ8aB_GOk9}v~kI}23ga_ zCF2vwnl>&O-$JrmNG$1$?nHJQas}4W#-^a=#;zt$8=H*lFi#tsjGM4y6LfZ0u;UfT z&DgP-@s{y@qpi2i-tyMI!t(KO;X%gaaYg~2EwEz0lPccft1VVMO}oqHb-1m|L$9(j%Rbe=s ztL8|!CS3Hgv_0e&A=(fnj|j1a$B}JFw~Zx|#++tIIf@Y0iz5}twgToPN0IGO$koWL z8s8csr4< z6Y?tTvkHTnZ!Pv+i~ZR59LCDSuqAT}Tb{yJg5q*?eSJ=|aL~o1gmgtfQ?n~m5fGo)|N!)|sNivrVWXVp+Vv@|0 z6fOnGQXEsjeFnKf%z}7g;0@qw4Pl0GL2YOllBa?zQ+27J_DYkd0a=<~n!j-0p(|4i zeeW+WpIo_=FR7PE9{=p@zT#0~ z>3dNZ(&lkj*DH57-Eb(zTrpM(nx3)x&E$d2WA@NSMgg)ffVN$T>l(GcNAo-`x3W+bK}u@^a? zaev2^)3B9^xJ(vLIr0Q?PZ*C%`D$G0qBG^R^_GjcTv*EGB9B}ot}913IJgohFG21l zNOUtNo|-+@2L~PDd9oZym%|Jp3rVveKSk1~ke?yxGsw@8^f}}gNcsZuDnvKD)+#iEYu=oW;>I>S#OFb6<9-)NI@?2^19M< zB&G_`d+cr6n-@-9aHOAUKc2N52bvBPr_Z=OX`06pD2^VY(Y0V-p-J8F!Gk~64pE+b zFFq_c{`!sZH`Qlo^bV2)!N!4elBgPVpH-0}wh?0A7j$tU`D~p^Yl6E`VH!)rE|oJaYZbhga3p#uMF**~4_*8sBD z+XHgaI{|Xjy8#MUg^M4|lOd`hfT*ut&Nh{JLVa}>Uxc}SfH zxdi!_AW)W8Aie^2pN=B_DCA1SS3;(-;VFDSjSbHr)7bDFGK~!{Ak*0J3NAm54X;_M zXY91e-!OM(R1f)53<^#f8`9Z1EMu=!o`FG@L}SA;)?zev>g1UiWLR0)E{pBR()q-0 zS?nmyW5+JYG~94y27G8`yq zAWwVvj{8{)GI(+6ewPi6SE+5LI;4sngC*uaRyptJy!u|cK&!sXxt_A{_iOY#R6t-r zFsl>-wSi(gohMP+D6n;K0NETspcirm*_>ffh$n4G*TxbeBYU;MzQk&*TaCpkjVBwi zZX*^WDNm2v*AiqS`FCiCWRWUR4dFRJzH?PfY zdD~E!m5=8M3uA039HF~WmEvwx*^GZfB3^B7)$;Egf}_nbMZ9)`4icpMv zi`o7C4VKs5H1)k9(~)q?aM2Cu$PmjRfVLs!Hny*!qfSqm2e}-1m$Qc&chL0vkXV!J zXiveag6+)c$Wdf<6mli9s$?u!01bb}^R+_o`*ZHGBfIB&wb4DTT4Y|!oS2TBLzd^5 z6E)^smTOteGv-{D>mcV~pB&bO{@SwDChEzgThs}>yP;kS^IrVJbKf-)H%!Lgp;2=$=JqmYceQ=AVspv;J8z9?R6d?449{px8Pu3o ziN-XqpZ9@>Q=Z(RiG!cSPu!7jxOXz8a^TPH);w`lyNbJYJn5$CrWKaK*fMu4XB*$f zmbt?|Se?}(Z7tijqAfDit&Z*P8@Wdv+r8)Ma_P>&7}bFI2BuCtxs3SBkgp(~?wm1u zIgMqf*)a%XOHNh?`63Rzi2Dn(!P89zC%QAmxE_brLwN&sqB~Q9T=+j#zyHo}3kn!A z^_$wDhS?8j;p-{!1lgosq6aiaHAWmON5*Q#iYX-b&W>-8N#8>uAIcuSmrVHS_z@|c zKP`Jm>#7#y-h#Y@+0@a7oBMUfeH|a2PwYDR(OaG8Dp_ULQds=!-KO?O^kB?L+mT{7 zL`TM`$B2PRM<%K#0-9_)S-dje;g92Kn=~DC2(8Gc6?J1nxPg3bFs?W=ujhnq`p5Lb zgUItBbR>t6=OM;zJBih$Pum|W=%F{aNHKa*@`w~;8%>pwE|003ao2Y6KGr;-hpCZm zB}?Iq9ZKI5%$sp^v;nOwb{Y@01>AtV4*RZy9mgCTnS;f>586@1gDUV~L9L7(M%%Nj z0^_Lq`qZ(CEo5WxSbh4>PiW)t6>@sTwtIQ94C|J$T&vp}xoPj?H=n#0C$x3r)K>qp zfiK=Rwls7XLHHbFV@pGK5g0dP4c$c$ICz)5YFpyWm$b#=FYyPb-bboNisdawh=z#S z4EN44iP`f9{G>a7`4u%2dK#br`Wbo}pb#=W4NwG`o(9+sxe95jSeP*SCA&H}zeU)A zR@1`B=S-bB*?637?wq)}YmGm~82|I%Kcj>nCMF}<&++W%hvH8}IzwMwDv$YYsrl$J zW5<0o{=?`|Bcn_{5-%_r$9|4xKgY74QS4_l`x%$?`OLW#oo7ejE)vtOQZt#{oKsj> zT3M@URrl;Pi=Q?sMbTX#=!+ w?O<)NHeB07+eh15>#z0JhG_?CUA05Bk=mi!VOpKmfwxiH+YZ;*As3VX1M_btSO5S3 diff --git a/.cache/clangd/index/NMPC_setup.hpp.CAD16FEB6583910A.idx b/.cache/clangd/index/NMPC_setup.hpp.CAD16FEB6583910A.idx deleted file mode 100644 index 4f031ff590a1bdd32b4210039fed6ed331362fd5..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 2236 zcmYjS3s6*57{2!ayL<7&SM|(NRgXRz{Ns%SI(M0Uv;*jH4w! z7&RMlus~6BG#h-NmY61tV~LiUQaWZ2Uz0OpQKn;K25SGg&TrlszS;YI|2cEdfBx@d zY)*FeK8qv`doyR=jKVpS<0VOQ!e9BEvgt&YBo~pSc@3>KnPa|sR$F?LFVai$m0{I+ zy(23Ne~Cm(_eL)N_;T0prCXd6j-HydVPkjq;4bfkpU3?$tT>u7so#;`EIS-KdoLxv z3p+nQbN={8J+lh_-L>Adzb&&fqh-Rp`FrHFgqs(py}cr@t9HcI#U5Y5O96A=7nTj* z)MG^DKJ(tTk+-)U-8$gn!Pif%nQ1M%v-NndhRs)^H*cM3y;oGV?s(V2BHkuSHDSI(F+HBORh$2RsW zx_;KA5uXALf`_Y$>Xo2*;_5LFX;GYyis5gQx9WGt_BkBm{DR2kZu* z{QfGLu1zfUGU5g~$b)tRP;QFtu77x9NPv+zkUgr$W&p}_-}e;;xho{j1M|{-BlbzG?I+xrV4eT zy#BuPt>=PGqZtXn>QsMf!~m4%2KPH=FKW+c#09cTbvX?{`KjmwJ>BIl(1;T^w<)$n zlymFyb8-%^Dq>mt7xugBnf1n z#itm6^5UoK{hm=1*D+!RIn2Wj15lpSmvmkl*WO|#Fh1O5GPw+p9yLMc3H8%2ULN}J za2q4>u->bBZ3f`_Td^DUx4%z6rx6GC%@VYvqg+!Q7cO4%(-n>QQMM}9$9FboV9!tg zoO1UXBQk96@H-*~;Oe4|)-S@%yh9@lS1XG0c+&e8t~-|bU1=vHDy)VJ2^xT_uYMBK zPQP*eZ$>;ICtH#g15nP|Zf$8OF6m~(1hVee!v>&SRyei%S8&@aF?(7^&7gz?%B?Bx3G4)uQ9g zQ_fYc<6Tg$shW;EP_C=Gj-965uG)1xC*=;+q2mNlo~owm=;4y<2bO)?Ux}J1_ftPZ z2T-0uQy6-Y@&FAm><{HZ8f4fO%0o27(3X^kX_%oCFlOL*B4hA)7%y-wB36*bPl9TZ zDM(|S04=fvX^aurM8pTu_{mT$vIA-SKA=TrAdPVWw8#piF(!Z(8G$r52WXKENMkGj zEiwUVYy!|C3y{v8wS0Wzg0|f-AQ2jIVvA6QY7q^O{=rBRW&+S65U>g-0uC=S0O{t# z*DAN&8GlX;Uy_oB!-w{OY7q%ojROIDi#$LYS3$Li0Hks3pj!AJX|z7j!uLqyAObD? sj~7Je}f@07{o9`=*Lep`Bb#ikEXQrX|iU}sIYcTvs-OR zDJ&rtSwGuyNtBkBY>GpG2dUFYX{?)Q0~`@Ns%J&~cSR;|H9 zP;hLhLb@|qf`cH)1i9tOvR$`zASl@of)qt{Q6q_ue8~MX3pD2`yDNll_|_SNeKf`FB>)9SHO!J+&KU=1&TzQ&ZC| zvPTk6v^?xOmh{fzTKSk`*B`N-W=GeNQ_k8w@Aff0(mic`Ini%tZA3=Yp-6t{z80$= z&i0W?+b+M4zq59FZp`=^sXXIVnRiIJr=`*TWtp8Gk1oCGmi!kkeN}GvGWlA|d->Pz zr%OGj1n;U;exYJ9R(98_au zc75Ppb^P0Y?z^GLP5mXokLY>8eZh{;{;5nGn5WiVR0M~*Z;>x& zX7@FwRvwz19FF>`)YEmSTIICygSdD6D=Gi6yr#2t-e>Qyh_^W>_U*?Q))=XoCG`aDLI!W6JG0yNc_$wyq}D zf(sr&|4L3wOjX>lJQ>Ff8+}>h@Ak~eN6(cx>j&p^_ZkQ`sbY?AeqK9Vtayj~-&WJ@wP1w#46_-h0sQ%Ng+P zA12&viYBxiY;frPBB^~{9bu@0a;N!2%Scha?OsLP0cOYfeUNj^^~*zE3!5CLe;(cF zmS4G@Jr_Q!Y>am@IXjhF_Z>r*!>w<>G*OUOlJMrIv;pxjb1_a!CSh{Qq}t7 zKhpL!^8HA$9j}zQzt7ZV(RRv+=d&rk-a{W zGA4mua3vcKY~xzjw|_YQa2Wez^eFal@yB}`#_Oj==Ypc$ax(Y))g%^9QKKII+skuA%=uTa{Z*%5F-Inx0jN>SZP+)q^JNy7!IqaUT50Dml3?M)+%di5-*%ggNw z8vGORH)c00qLXTb9TIxWXy*FH*>OmoCf(kOoba2SdG_w-eppY&t_z1m92&<~NFtCJ z5b`t7T#4uFYC2!5Q_0vh2uz?6?1e-E(G&s@6o$Yj`frMqZv%?R*#96fkH#|>k_luI z8tt7L?mL$dbe4==i@+=f%SLEOG^C+$M6T+ocyJE_MP^y>LSbvXJvrahDEkSTVr$|6(*1V7{3_ohDAyd;YA^?I9GjCN%V~|`y^+CUtw0( zRQ13^CfwO?o;@L}_+lu99wrWCF!Xes4bU-)PS^}Iv zA8mvVJ;*scI#7x*G{76c+C?6C4;W>JdJ%@2j-m{6j9uPsPqY4BvpYN30bK=)+;Q%o zcZU|jGDTc{t|6Mz>{Ij6Os5$n(zOR8B?xf}ow8`JRAVYro7V~FL_%i(l^EfqnG-Y< z3L|dKe4uVrA;hhj3)Jm$$Gk>2asH-8Fb_CTTM6I*MQd5&EGcLPG*k~35p)TQW`e29 zq@s&p5lf#%Ln*={wl15BGJ{1NT@Dr10T%Iec~n%Ss%1@2+&5505I!Ul$xO?DY)oFH z!&2PR&pJH*>vfx4ed~>;2{agJL)>s~pPK;5cJ7vFj;CC4a40r%O zum|424|oD!AOQYgDR2fH;0hc;FyMkf;01WV2e^PGU^xf?EU*Hs0;@p?U;+yh8r|G9 KlwpRkf&LFH3U9vv diff --git a/.cache/clangd/index/PID_setup.hpp.486A8D0A6ED861A2.idx b/.cache/clangd/index/PID_setup.hpp.486A8D0A6ED861A2.idx deleted file mode 100644 index 7a301cdda0bba7186f497155175080db86664767..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 1310 zcmYL|c}P@I6vp3No#pw)H*fYa5x2?;Ev3yiLd7jxB{5t|M~%`r3N7gmwWvX9!AcO#r0L#Uryk5VGv9a5xtIGpFE%k`<_|owDU!u#;&upnJIDWNx z=4)E($tsCQG4ErxPE>l?`3?7+0$PiI&gGZw>Wof}=q^bfH@2r&#rnl=3cmMP44IKFGkmvTf1i058Y3! z&up*rOq?|iizP`BK~3HtFCE#({+i$TL6+$q_r-HZ?bD8-w;z=)368WLeY>M7=JwtA zsEP`&Qzw2njG3Y4dy~=?i~2K{M>Y+m1V^Pb*vwhSV``@hjwXrymTVD)mj^%07`*F9 zDeX-je3HH?y`cAch^=#v=8!(vXJm!RoNZL%yI>qqhfbcY4<$O*?PPQ_D!xZhT{An1CzHn{d+E&T6XhDN}A78c~F7aNf+uL6CaYzBOb?kR_{s$;+&2fZMMmY!vd1P-Wo#v59z1IKM&%mlB3ry*BWjKNCUB)mH#_= zz<_(3IA^Y(M{2ychSRuFf$Ht+&KDJZyE!ODc(!FyiBv;{Vd+L9nYeY;ow1-e)d-IS za&}IqqXPBz;mID;#LFojA#yQ-N=*e~CO2cRsSLjMG&u+5oo5`+sX>`3Ne!BquBi+$ zrV>#M(~O#=L}f;=nbhUdb7pdjOcG|g&0TWW+57Gve*6D@-~PUDed~Ly-?; z@Q0tF(CUh-vRlwMp_OO)*4gY}l9l9GH@UiaQ^kw28?RdyJ-O5Ic&w~C?~WoT#{G^s zuzyEv&bcB<)Z~)MRIlrPtzFi_*!1}Icf1##T5q$>CBQVM^V`IUvgKQA2jpkIyD#ra z-s1Ig>9e(V*NktxEaR&!9)8-{<0cFL;cWFlp6*}Tfu6f`{xQc|S=pstZc_HOv)JsA zpQXUQckPnsN8+!&M;oB?VAjIwV3%tzPyUs0Zu@sppEPW)+dtwLez~T>H0xyij9!iT z7n$BuppMQDcX5U2M2N~*2CfM!& zc4)xS`)F8=G$j1?ZC@Y@O4yiszOZLh>R12e**Fiwc{WADHGVz0-^?yw9+%XkWut3! zWXJVWKR$a&4;@PkjF*pTTd1{?;Rn4r|H}w&-;v!TLSO9n&M&UatTzS(!~*~N?In^V3ww*0Bx@mTCfsZXl@Q~dAV(lq1XD|U#;raK3w0rqb+~AZl-m3-5b}}?MJP8 zha?>l%$u%Xk2ZQgdS+Iu27J;pm5~<#L3s7K>L?SF^e93^5KJ|kQw_~!AqF8X#B34} z%fvvY<~2^q$ytj{DZ&(qIFrs|S)^{H`#XW9OOU{+@Wj1>VZJX#m?423klX=HWdXVY z&ctjrNW2rddG-GG{G+uBidcw54nX<<6w88)g4_vR5;(h9Oljfk|B*>~lu(2r5~+i8 z>!6h^)F^b?kh?V!v1u9qAt8Lrw-lj|L=M5(hoFgUgK&f6J6~Kak;noUMTCW~&vz6d zL?VrFRwFc)g$cqO-idhFAd#xvIbOHBGcHnu2@>fAWG@iP{B`~9*%dvoYTKhZXg5F+ zMo7ei^bpC`>#mou0zbNDwf-qAeoherBv1usRUuD;1;Nvvyff2Qx<#j@*7FrbEJ7l7 zB0Dcxgd`%A(48@3rukyz$E&Js!!HNY$0))W2|NYlQ=l(X=ql`3GXraX> zKqBQ(qZ~zcqi7?FEZQA6E2M=;i;hXRnnw`9k1F=O`0X}93K+=(i4dC?F#ewYUU)$Q zM(!t9ut?#f@W)6yOefp;=0gAk%nLxi0DSD=tFNRz&;LF<6EU6v@)^*^4C@8f#fgrM z>WJ|R(EJ66u&S@GH+@j(vyq-bF(1<(3o)5uMu5Qxu*Kg~ObO(dz(qJaLfWf-`?aW< z1{C8(da0wf`R>t9+k4RbAD(j&xd@O&U;);rSuv>Ay(ycA><)k zCxO-^uwro`qzj?Bs)U$AxU3Lvz_=jFWI;l?-U773)4=RBaKMIe-2%P;6)g@Hu2#gD zDpgE2jj~!@-C$*gVwg#0&c|vFoB#`wWR7M?f0A305|dkgzS!&V;H(5mXU)#yVe^QZz~lllmy34;ayJ*}0Wy!pbA;sNm%sJ9p*Zye&3>SV zo5<^?*0a83fIDJD*hIXK^%qQTj#je^T8_xi0r?zg;SJ{v*JqgM+*^SdsZOa8SYFF% zq^tA!m)>aMML=8xoG{~1dbE}2WzV%}>EWVq3rwb%IB}eeMJX03QjBo!pbVJq#W;*N zAIjgjwV2A0?L>#fAS{oo56MqAW-NdF(Qe*k>m9{Rit#t}UxwuoTIZk4BDuAE4F`qF zx3fqgR(Nwzup$eiS+<4aAE)mLKnpt#^o|2nHc`w8pm&0cYk^)Zi)}uZtZF>4F91?Z z1t2TH9ITN-4U#B_vvAXtNi`$Oc^v_0xzWzio3NO7^5D##FRq0na=3N4sxuy0{^s0CO2%iEcoK6(lVzV946FWnpEwnlS zz3`q=Xi6?R1l?7W6UE4^9D3pyQD}B8s)FvgU?|j07FWX+xV=zl5L?$lZ`E){Bh{o4 zy5TX~FeEIW415rcf|=}|tV#;|(}&xBdPb%qay~H52X=TpH4jod8ZY#vq9})mLOAIh zEso}-v(iGTO6S)*14nOIuTMj|l#a?Utm%A$QF2vT@-{?{@rX&qWShIrZ9V4(x1)-$ z0%R4Kix~puJRr|=aVH=(2*H&lE8G0J)DHCjw?aAop`|F(8Y%xCD?TTzn9a2f6q#AP=*+ zDEF~tO5Hyj(Rs(8_SeVuDJGZ>=AaNdgo8rqP!0;C!#F6M4(FfEpXppNwmm7(?|K&F?LNvnadnHp-^eyw$4RpqO2dzY7}Rpt{C> zpRn5N?LDM4#W6)S!Ga&(Jz5jq(2H7LBcK}r=bF?HO!|Qo?-GTsT?Ws%*QQgDe+n+b z9eGjdV8WS}J$*?3P9WL|tg(M(zZ%uo8H@gfEF1y!5ukE>s;Qa`09)3=b0ByQ7PHYU zh5S-zh${grMENpx8O95{7uCu0>HKT}g(XNc$QVm)Ie8%Ore5U$3QIPbt(w47XA|N8 z)WBzxtwo=Hp?2ob6C_p+$a0{8zn?m^5QpGQY#kp?koD=CHn!j3p5U@iKj8)1rLG?U17ncY6uR&tOlyB*2 zo6<9;UYw@>Og~cmLJ+p>tBiehU|${CS3A`!VE#EM*B9(v{ZPZvNg|cl+SxnE2#JbM m*sESQDWek6ze@=np04_AjX4xOciwxNTDOa?Tx~@biC8oNnPs$CoZa0pWG>KR}VxzT&{e0_i(FwZ@=r~#A{3V?eo8N zKKj6Z)AY#sfi~Sk5AGNTWlzSs_j*h^{zqY4qTjkNDm40t1DkgHYn3yf&AC0c>9PA9 zzw*p(^|?24O4A~@hL^i^m&Yp}+`L@#v7sq^m;HqE{)Kxl9;$AogMEgAUyX{!bKSwo zKV9xKbT_}~>KyMnkkISxVKz6ZKv5CagQ%r(c3xj)@LU# zwO)17db=LlJ9XKzW#0$XJT{&y_PgFy{oN` z&riAWLFLQ6yA}sUPgC1Xcihgs=B7J#ac;o%)uXNlT$s4?ruL4oaQDg2Px%+ubltBB z4!&Joy(Ce)#`x&Py1_0*dt1l3(IG4E8W*HKJGJTc%`dh#9*$Gi|LAi3VX~(>wz22p zzv|Clek*?8s=rkYJn8h(7md1?a=zv5U3UgPS?;he{=;bFFYc%NHViqvS5|R?l&vW( z_!!1w|K-o;)~4n-@??}d5jA5plbIp*FfH)=w0KF$jE=VPJP8MPyedA18FVl1x%42m z;i!TqQ^BoM>9owC+aW-{%Inf~a0h@pMimpz47!tU=_ZtxES*HjxZ%@?X!JBj5gBJg zA}8C3Bgf6FXkIrt$Ac$9kUvv3Q_YMe|2dt%TM1lDQih|czzOM@MdqR)gR%UvL zJPCpPS*lrDW-$NF-1ds1!rEy(2?TeFDkYW~bWeLRx%=JwAL)2v5AHCBFaatQ%%J=Df|AW{{Vua9nS{k+%{I-08R@d{d?6rx;OREuX7!(WG8!sS^J;fyEETx8 zKUqWdJ1Kkm(ix&jFbyA|odWsW3SDok(stDIghGCZV~8s=m|tqDXz_pVR3lF`;7(8_ zXqZ8FUFE&0%a473k|&|yj#b5~nL&5p+2<`w7L=c+WDH&}+ao)IeVhnSIDKN2A>VIy zYBMEcF(A(^FPsJFydYrhpLBmIDtXq%lVCU?S(QA489ZQL<>;hq->hz@WCG@AC^AA` zef@dP4V5uE`BywqLw=kpPR|VHSFiZ1UAcbC*F13mceq3NL}t)^EzaTb%76NF*|F{i zIt5M@CG)EL>-R05N`3!xgD2Ai*4(!G@(E{;#6G2DA_nA#=9`FY#0cW%0o{MPZ*BiTZ`mLv z<1xUXFa*DgC|i?Vw5T>j2SF( z&!dO;cJ5jJKc4u5TdUG)m_avLQM9P+3JiK|o~kMHppDFtis>X>QZe?UVg|{uS{aDJ zYL!VctyWni%W9QPvaMD*B*$u%OLDDNMq;#DnTW}1l}GZdR{12~YE?iAEGr~f8wW? z8}C**54Cn1oT#Yfv;xiz6{EN)0f&r=I!-5G`=}VrMGN>ssTjk>2-rg^#&WR&vKJNO zxHti+f{O86ynx?^iV0kTfHX+OL@rUlcSpsU+)M#GO~qs`SwO;~;w)~KfPJN63YQ{a zJE@q;r3%=Wz>3hslRG}hw54JQ7cv?NnTk2`95;Mmr~|yiF#ns|0?$F6KF2~7Vn%32 znuQZZLx^Fvz0huq0s!XQZ`ms9SgJ8o)mGK9(cV4KNck^WOMS0G4zN zqfo*Cmh=l@lrDfJ-9i{A5$a}AW!@Vf3`9#Rh0*0lcWk;k>C;~LvKmgqBTK??%CnR> zoU7_;J{oyu_k9Qo=7JsYV3=8Xwkv)EoKHBHNu2rIM-6StMLV0GKyD}(%Hv&NZsjH= zjttHy}mE=pAWh)BHrbz$z YNM`sa7HecO069|a^EWdHyG diff --git a/.cache/clangd/index/acados_solver_auv_model.c.5778764B3A2FDFAE.idx b/.cache/clangd/index/acados_solver_auv_model.c.5778764B3A2FDFAE.idx deleted file mode 100644 index 3b988c84ae8b5ecba6a34e2d18efb6aa21c525be..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 12412 zcmb7~cUTlh8^CXOXLn{{=>i^{BE8$df(RW{zGLRy>)7mO+DhKy?Lo7U;;H6sfkRQk?Euv{!9}~) zyQny%-&QMiXy|p5`qy2PY{tw_I=5&MV`$qg{I80)>$?pwd15+bB5&_H(mvhlq|Lrr zv-WFm-a22N+5ZGzzIu*RwsTHWv3dVfcAK~yZ_5jYE)Ii#8lIXZ0N+_J*N!$y=yp5I zCp|S{^g|=T?Oki{-L7TFjYkExjX00m`;9Z?&7bwtDPGd{)%{EN&;Rx3ZvR~&TZ;>8 zv`SnmC;z#$<$nJKcRj-8TlHg%%B*@HZB^j^acJ4$In6BgS}mTUWpaIr*2UGijy%-3}J^v2y+po3s`rL)4s!8TnqUGfN$8UAC?i6!NXSaDpfz~<) z{ei#Q^)d3-n>QybJZ}A(f{qS_*C(yZUfOG`d%AXrS;4`FXV%Qy=l8_bVC2#tvql#V zt64YZT#j4f)my*xec`bvef1zi^B+nBx@CIQFP(qHdhi*3MqUTQ{KH==Qac5j#coMU z4fkF9i`;92@#LZJUK$_X@Tcvz_~5nLnVq^{R(*F-_6l}87i zSTrcly=$Ro=xkVX{C!}bpgZrDrZ2D4yz=O^{cOyXKEGe>ANH;)a>r1?>&2VGA$x-1 zywIn9oqz9hCEqVHX;$d?ifsohQ+tl8c06`>+_gdl5x+~bi>7zRa55FKNVY_ zC=YnV{NeBXw7PZB}{fs;$qLb0I?9n}@Dv&j?y3uWs4oTErxMFsaP* z^VXj9p!){vJ|BkdtI^AU{PA*`-R#}I14|#B-jseRdCv3ng|nKw4f_)sd%1)kX+5Cx z_{`_*mcF4sL{1n#)%1?u68q#&12&AX@3qixq`_qW;kKQNzHGJ9ZT9lP)0Y=xs%_>) zU-Iq!G}_)`#k9DGGc2>8X7{}jFk|Emqls=Yg+aR)ye(T5dBa?L?5&g~f|Vwa-?}d( z)@=GHozA5vXI$z~{(fJt=Gn1sGbPtI#jkhO_?NZafBD|p$Y#|CA3n?t@BDty?MLyu zpMRdZd{1lFLhp*%r}74#ExB{LUCq*g3#~r%a<=nt@!R#ar9Yc{rbhpg9slyL^**Q1 zW)7TUzVVjP_#1Pa_AEZs!#JbDf7956`%IttHWt;!9i)M)!op>VS*`2(-c8d=cI|PX`q+;Hx~Ws&e*BQ# zN-uol2=AaW5MO=KJOhn-uT?TG@qGNJW#I*1j{Utd^zX3v>Jw*L=IXyHo|~o_)N+Ma z)gPAcPU)W4{%P{>Th2c@A3nXX{_3z@SJ^u~MlbAZA=WMVBQkw-v+C7fzUbT!xRZMZ2u?WPX?!Ma?^)_ahO4Q|5V}~)myjgYFupaJ5^(0km zEOdsK&hReZE;zw`{V;#;tZi{nHj`9+u*hKq4kLHp2u2;j_^3o3(AZCvdwa8^o6?;m zRevmW7lOOc#W$2uhcdlXY6HZ6z1b3{$Upl&p5`p6x?rJbQ72k-^i5!_5||!edzv^1 zk7%1vJa_vqx^9xH0~VSNrd#^1P_O8y+-l;0(lZ@v9(R>gE?6J}f(U5$?Lvm!wxx9l z*r+o^QuW3nYdC%lXZ7tuM2eT6j*-`DPm@%AvB)k2b|Ew0a7G=@v{7!gaq>8I_!;em zlBy>b`XKNh1j}z1($8GuVT)eqbYCK=+^|Tn2!cf`-&jT+%XCt-%=O=!n-l(Eouu-@ zLSYDmAxGb>jCyOs#G7e1s_@;E%#F)ab_GkSj#wy8)Q=O}_$D#7Nlf3bBO3Pu){f)5 zM;^gDrbS9B4=fZf3h|<~Z!%+^%yj-LG<7Hz8Uu#(s|cx#kyOrDXq@@DF22j#E+4E? zOOmnA04(4)bccu8`?;}_sxuad5~V27-Z!4HjAy!i?PbD`SZMqYH;1BDK5>!?FMaYc z#`qZ1{o931e&Oa`$809=OORAfSY$4kYwo*>QLkd!Dk{`C>trtLu`f|lwZS4G3?IVS ze!GxfybJ=icDjjnNvif(Bng5f*v5A~qh60EuGWXlSKs-3Wb31qQ%3BQRPI0+mj~|7sBrnllVnRBWAuM zNdTV&J*A^62&$l-(owXiiWaq-GCKAWRg9=jJZIe5#PX+`({%(%uH!)+j})EM@y2z$ zTHycz1PE07J^}2bIYR&$G?xjWOkoqhb>R+M`{-hwmhqsBZ%$Ojs-{9Nb;5`(+&9}Qv^F6-ZJ(^4TcBM3DK%ES{9U}=6``x6=aF)Vn zk>gp^t|>R-1z}XYIEgG0QIq|nU-wFFhNqSl7WQB}E)` zz&SDqq9ABW6snPfAqu8>GX$HVk+4Pjf9q zwKV?&rGKC~>7VT5usIqxal{-X(s4uM#*H*gJ$@%NIQ6azHu(hvE}-UQ&UY-625uPk zrlBDNHqU_ViQCT4Sd}*79}`y&V694}iu!^4n3W#W{BQG2nxR_utX9?LTb@;%wlU9= z{>cs!54B?-N}233$8H z_>trc!8Svn`YRLc$^^W7Yy8+=N*sQDdR!+hNnUKf*g={2a>wOP3T zlZ=hBcx16IS*$CWog}a2oYr#AiVtV=F4?>*NfJ193odnnD{)0hE{87V(3QA~BxfU+ zY~)Jp{hNW;Pk-kWch%9z>ln0-X-NjAkxz5dY0iq=NF!h4z%`Cqxl1@uLi1A&Jf-;s z2VT%z!=W0krPALjfK~yDg8&MmITD~qnqvWorFRa=03_3V0DuDulLH4{2pUft#4LCs z(BK+$BaY{`yiKiN3ms3Knd+caM@LyblLFz%6;guRUV}eQ+E{LW4+XP4rQ+;-Htsq;3On z!+8YGBQxR&RiCe&s(BnRKwHAUaE*kW$S)+kfazR@rbJ%FQNuu9hC7dWqcQ+gI0DsT%P@q`vab80RjceRE-{2x6t3T=tsQvhjO-|oC|SE zN&Y0*d=i|M=!t~GBjIR86#@!4jig*9`wqC zgA~3D2VI7;!j;gj61G@oWNKoDDC%-w(%MFsBkK_ zLK?J4Q`TkTYIxmjtcMWGAAH#|=AV%dr+t?*_!ZOlyg2>mOzdPkS+J8;D_LhZi*~au z6i#PBy3%fVcjBD{qokSK|0lrzxAXt+(3tb>Pp)CM|B5{&nQ=~Lx{_H2bZ9p=%l&#o zLnLDrscaWZ4}|3WxY1+&S3McWFfGV08uX-&V=NU;V2l%#_S)4WYp%2&?)TlS)x&y} z+6*oB!?O-#P#|MMd`}}Ma#A9_P;EwFGcqK$Cq9TntFe|u6s(cYLU0zgB2Kj6)Pux+ zZM+xZ$rQ78#cb2YAjzTgyGg&Qe&;pvRu*if6}*iF+h`7BK^V>9EC{DLf&~!@lQI^z zePa!a4DIx6t9gmh^Ok)VfeRPyST;GlXeD-|2m~TfE8@{%y^n@nblbcd4?b7$%oT={ zyl`nr>d+ZOE!KY5m0hK|Rv!b;b-1fgwq+>Wj<^l!5|65}Xhuf0r@Pwx=ZgvJ@jap$ z<7nk5{mONNb6x5$!9atT22g?Y$kKpQ4g!@lS0PYEa}5GD3X>x;zI9_Mj@)|ll=czo z>oeJ7Nyh$)QtF9^e$vuEXi{kwh)w8kl=6JFuxdhSvz|HC$IE+n(WF&+z?-$tCRQRv&~e#PcP*gR+l+ zp2{ki1luM-Z{qk8K6SX>h5g7e1h2!;ybQW2*)#xI2cWJbq9p7WPGP7waTE#Hc6!7j znG}0)2?m+(LT)6rN%)Z2;V}A9Sw%CESte?)sQwAEdV;zTuaR&xSO<$;NqFEDO>~G7 zdy?}mc0bW6TI@|$U7U+W=NPdMF&!R{*fv)5CL0W#bHuiBqBrp_37^<);>B*{#4h2Y zrAva?mt=nl*8^K8ie6-!_o}FyP2PpG;W&i;y>TQRV0UdS3KMtL{aBEa<8>qw$JS%k z<1ssyn6k=|U)WQ3`*s00@aQq`Ky0Rwt9VpJ^J^Zxrn!bkH8lUtqrYkX$fJ)6ukZgR z%+xSv`*&kb|FR-@==3X5xJXpOT9&ZhWM~aMx`bWLD1RtpTbHpTXr58V22%p%tV=mN zToEW|rzKo(8$MO^W(55@mQx;bGlaq z^YQu(wL51#WF?d`7^e)zo%GaDyK@TQ+?7zt;hl1LccOL)*Y2F^1$QM>jzFCw^jE-i$Pxk)0A+5y&Y5 zxhtVnfNTory?2~AEl!+AT$v;ktjdN50Zsb)R{zI}Y%KmaHOT1@q$^c4>($#o9XBpc z*4M~IoN*CnP86k)Z*T@TxTZp`MlR)yN;$RCUcnhv(EN%sdPQ?Br(esNDDrE7!5X?y z90CkNXpRQP(SW*t0x(RV3(q@&(M~|!KMyp^Q;v%o`86)~o&R}(A(q8w!1>zqI}(GKeS4%r6rVC^%#v394EExTaFqpD zSw|8y2epF_UYXu(?|17n$&T2xjSpT(S5KbT^>=^2bRWLs60}5*xV{D#zR@nIM+S7h z{fZ}Bf$;&nSzm0j*p`S%^77`(?G-w~v^l}_P~s(vHP2$5$mkl%73#H|12H2mKk(++ zyps|?b%J@F;6&7li|kM*4Qj|BBlxX$y!_=~An-)9jxi!6vbPwpHW zHvIAFga6sk_trkWmHC)Ij5j$k5XC@=ETa{g z=BEAU$&#%GP9Tks;mA-l@Rn?P(t$;0KU+?iU+HBYJ3qqh@d%=2az;~Ko9~Ei7So# z1%fXSkvPHW8G#!JDUNUxftxhnLg1FdEtX&Y^~v^2={Qb)ZSiYUb@tkjXBD2i4jgmT z$b~#8SX!Fq&fo5JPjk0OD!hA%Gn;rwAa0=G_9=P4j*M?5Fvl z01nceCV(`Wj|$)@%_jtKg62~KI7Ksl!z_#DvjR9vbDjY5Xf6;y0nL{LaEaz30Tj`E zT>#fX|5DNCCx7c@Pg*o0(h-(!Wh-b z>Gk=?@Ud+-W4xQOA$eVr_cQAKjDy0djB%>cermAYwnW$6$GJ&wA@UDyt(+6HW z@!dL0(lpMxjjt1sR7ci8oC6zQAs`L790@@IbS8_~(zIU2U|^fm_(`%q1N<3%WpzEq zz+;RN$%43C$%0T;U*SLw1akT$3pSL^z(+x!M7<83gV$R zzQI67ICIU&w8E@zng2QO|Iu&SPaoDC&cx~~6x<7io+Ke$(A)H;z;D)>@6Hp$WQ%9> zgMQD(3vD`To{l`pDQ|q*6hK%GDEj$W+>r7@dF*^EFW|c->Q^#DP?;T3#mVPJ@})jst`Zuy9mHVU`Z5;(;5JU zG+zSX63zDjxJPq6K=pun{ox}(A8Gys&?lO=@@Okh$#3J)Hk!kD6sGXbu3Ik6uDNsx z&ma$)Uk`A{G=W zPxJ2Vi_6+UqlC8>o>$RGvcgF6O=Nfz zITLn_UE9?)sc$i!Ngyuzn@sH{5+s){LOKX zz9tW+-Ha=7poZkpdOz)nsf<&q(%v;GT=TAX94>m}Mvt@!;j(vN+Cg&?i;`$gW>GTD zdswiCZADUxM&8SUy)>t?D3z8!$fAQZA7;T}T0V^hX$l{!Rl|WJIy}eb-iw;=MYg24 zGt_Hp%o6>HFR*zILY;%qh`4m;bFDs{T44PKH$*bVk&Fegf`cY}zhRW=d+d_stU)>J zptxWqYhKB=SNJ05b&(sVxa2kDc@4?L1@Qx6WOz#{;i1`eCgUr;Kj<{PU}3$6aU{Wm z3rd`IFz2Xr^jWa_EI256HWK!ZgxKSw(J}b$a zQQysI09i_K!3_|FB(8}hEGLkd^(y+Ql%uHL#@ssJ7r=~;!L+&B%v{sLqGc;fD;q~^TN8WL{{T7Dx3vHO diff --git a/.cache/clangd/index/acados_solver_auv_model.h.F9AC8187D4C1016F.idx b/.cache/clangd/index/acados_solver_auv_model.h.F9AC8187D4C1016F.idx deleted file mode 100644 index b6757e3c58bd722f1275da28396a9ce646c3f43a..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 5168 zcmYjT2Ut|c6W@hHAM!Y92izebRisFjc07vILlNmsBm!rNpixvL2qM_9Km-(&&QTP> zC?ZDGXvD6l31Ept1&k#se~A<|ivPXi?epHpw;%JHxtZDB*_mIkz{kf#4FJzX!G>hv z+QlpY01`pU+BM5h$pYZ72*8H2=J0_0NA^lR56{Q5)BSdLng$f^+AMRL17&pocs}p$ zvup8l_mYB#j}<>X;ny0*jQZ!MI%HXtz8`Wd8}n+3yimF9t{LPHdql~$$vhs6OfdTX zid?b1Ro`CUEg8*TiVIhqnW|*b3ODD3hHcdeV0GDR4a@6w-}4c+Rn7A{KX9;r#Y*+z zOM1Ioj6(0_R2}}V^v2V@roZF_ax{BS9O5PF)hKLgI-faC_WD^FnAy4P#?$6P7t4ID?2 zA^gQZ$5UNZtMcSm7{;WAWCUbwFjo%Fy|iP;y(ho5-;DO`+D2%xjO6FZA61o~>D%hs z_d1<$6y#K|n`CP3ZFynY)pnHwU1hv+xrEmNS*NNFji}#EkFMizBc5MLIac&~%rs^G z?zRU}Hpk!DHxKl5jy(yVF*F=P49+>Cy#Gt(*F|w!8x$O#JZY{?cDU}Z-T!d-zn!6S z%dgoTY`9q0yDs0>z4FLgryU8g<7?in@@;qSbeY^?Ajm2IiI-F%tE&H`|FX;p&6c+r z_Z7y1pV=)kO?&CFJ$Iqbrk7e3aTlA6KYdOj=ddq3#zUS_q z;We#uKQ63v(mVLN(6+kapA-I{{ysfJMDBF#J6#=C&R(Q((}+}Bo^(CbKz50r?+n4=>;6I0;sKam1#x)w>O7HPH zo^z-DL(aTMSJIX`2RuKgZO}EIWjrr|A81$j>|S>0za>7u z>0H^hcG1-tp9XjO2b~s1-D~@;#b==BeBG%&?MIW_%yKk@_m2m2t7CFIj_vW^Zu6K= zrjDcpm6!~<-;edI(K@=vJT6P%_CdclhHKE>nVDh9vD#11ZqsqUv)w*L3DrB(8hW;d&@w zv9Y?r!)Yh~P%!+W88L0F-owa1d13FeT`776ZC82I6kqvl%*^qo<8su#{RP%q@*RskD|w%u8{v@pu#Iz5*VXbE&ix|kh>2C19`R_nBxd3xexIR zZkih?sdwM5;&@Kup-fv4&CYW8pj!%$NWGRRXysPypl=SrZ=0o)=Ev{hgNy z8@M75i57`Yk_CHsWoN@{+tX@F0yFHP0CEeU2jD6bGLjt9Ob=}F8W5V1U^+IbhWgde zRT5;r9ZGZGr0(Q{E7`Ur(84B}+)Vp#S-cwmMJA?l!39dwk02ys9m zVpt2jU+jHZP67>Vl3|q*2;@l;%(6lTYl1{hypiMz66j!qDyUZlT_ipNY>|oS^d#y< zo?I0hc%n3;)uMGI3r=(0PadZXpj| zrrJURZS0{Q8q`B~iHCVMXeUF*&#Hc{oOFT&hS;PW^2*^vbp|^j6S*t?Yq( zzWfFD$fRXscfru=hj&O|j!p6)HxKdwcj_cr$(cNCs+^XdcD{=Z+|Y)F7D6A%f;T)h zGo<^F#>e|4Fv1?PAvarG>nidSsqz~=jefr@ctHXKY*GXDYoMDXRmpqRXN$g=o0D3F#5At;oViy$bHmiIxhPg*X9pjcWifuKZME`y*>maC;mg^y?mzEnKX!s^?w7J4M=;5g#LsHSKXcgQn zNU9ccYM}vcgF>5Wqb3~MIh(n0aok%FGAv;xmnq>#*08jjR#6!j-Z+9UtwggKMVf~UU(=Q(_I zLa;Gm%w)59AaZMx?F$cdQc=8ebZM#d8_Ms~c*y z(=^WoBG{g=#~mKvNsVCfn8wzoN)>;m^7yd`HY3cK!@g`#|M5&`ni%#Y{P3^>yFT?4 zY!1I1kKmcaOlBW_rJ~D|>W(EK*n+TNoZ#t?&WN7+^eJsCzmfbPWpWa-_aS^3?>Hv} zi+KQT3|{U3Q@-lminR##C48APJ@fn1G~F&@1AYvmGZ9}j%Va)_Bp5*so@t}kL`gY`dx%Jh1ki9qI%>>2I5ZTi#gto^|57~<3{=mbz6xs6$K4ahdH8U@0$(>TvcoSKPX1E`bl#nGf!wiF@mavwX;bBfv zOIb_JFfqWljRH!jBOMMO3xa9FNGtFJKzWF{BCaT07Ra8ahqOJe2c)1$A+5j{g%mUu zq!qYrp$K9Yh$HM&)Xd!`G(C!755j{vfcadvB!?+)k0E#lF@t$;_}GzmnkLfTaXlaf zO%7?rqp;dttAD9zLyl$>vzZs#T(hfqC_b?r!48B2)BbSTA~!UFq}}{@!h^$_XL%0w z;22hn5^j?uN`q4m^>ALe4AAdsc1i!fWrrv`&~Qoziswi;GQA9!9kQpXCbp-ECQ{G@ zlU9VLRQt%@7`=-exezYQ!Qh%eu$Xe<(+xJ5dN@#xeGkFTgfnxx)^bCQ-9Dei@El@} zD(xEJM~{NioRrq^Ym281?hyN_T+sNhK>emoozG{CzknR{}H{l^yr$gLCM#N>wG z8gfhXSlTUDoulx0Yvv1N??$*W+2I;Su$bB6y+yK}8xa5o<%OmZ}KK zc!QHx3LaR7f|IH!f_8#{s3_XeBDEqQs02hPbw=%)_x=r?v6;z!v+w)%zsK9(TM#mP zw$2ivnK2>h39%_F`3NBq`6s6&#b4kd6k&l-dO>4ELr(KY_Al+c8!6{)zw&_hLxuR#>bE`F#!Jdn^E(@yM`830K zcAunZ$>RuhOxsr{~Ea>&IXMp5kD6{sj-1|v>e=T z>lxXzi$Y3bzro@+*oPCS&~wvww7&kzK?;39A_lB5;JKVYWJhrC0cXd{M<_IwMB1>T z4To?75o`W#o$9Wv$Og$t#NWkVYHVN~lLnqzA9|rFrjVG}^;lYu137^T&IM!o;{G-} zL7`D3(t@Qe*q;-KH1X-+(1egO3b~R4h{c0=3MUYGUVm_J`vSS1Le3;|4vWu`LuUmd;or|2yM9Y}1BIMO z{pmfs8Y$#LB3H2M74mjjfym@rNo%w9hT9Z!Cy{b2E+-SQ0+C~L zDW8&#bwf5tK_USz0a9ZF8%9a7tCP13qk`x9KP(XAzXYtU{wDpbU-LvPVe+sb4-4UJ zC5#pew5C3wf-#E-w=_P}iRn%(fvW+?kcn7cU<&|>g(C|H_gut838EZeQV|m?h#l5b z3)I7U8i8h5FPA6C=Ae58#n?u++gi05@W=tbqZL5rZWR3_y#IS){nbcd1V>{bcykx-s2t>I0Re9*ZQ@0~U$Ff(Qn@Fzm>Z$2}J@C0JO39RUm= zk&I_Cgz-b(inH5lw^xxp`v`q(;Bo+#B+oJld4Q1^??;drEPpWK^vKLZ#iFc6GD9{N zWSdSEh$JK4&ml41PanDZ@6dx8_)Hh3yG*A@C|tktotd1pDw%vU5Rc)}g=hArye{UA lL*rl=LaP#D){_qga-eB!#y1yOSXv3KDH}VHtv$~X{R8qrn^FJ( diff --git a/.cache/clangd/index/auv_model_impl_dae_fun_jac_x_xdot_u.c.6132B1FDE79246FD.idx b/.cache/clangd/index/auv_model_impl_dae_fun_jac_x_xdot_u.c.6132B1FDE79246FD.idx deleted file mode 100644 index 4d1476f88c492a1240ca1131d36dfd3086071ca0..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 2716 zcmZ{le^iWF7{~8S^WGZX(bi1NWLER8AL$ofXHnSNj7F)5C0S0PiIqZS6}9JB+w?xlCoBoO*tJN5(4Hu@cjkQ`Gk@4Qr{_7(`~BW~-+SNte3~1i z(d-(4kXjp*5+Ac}sR$t?BY#Qj5?8hg5LzWhDCIzNUTDspadgJJOl9&>mtE8BW1ch) zn`y6p(&D{sN6qj>k(Zh-_w{V_t*Q*uuWwZ|-eoTXZ+JKatSox{<6gJTdzrk_NxGC( zF0I8GIg>8ee%qStO(^QnjGO_+=E*G*wsg+HHLY2L^xsV#u(%6)Vq!9 z2cKux=FJ|QJ0?^uZ8{mVq-y`_Yo{lzFr4lf_j*NeYLPa;Tim-yoZfd+)}nNYmg*jL zRezJEFy3p}7F_OkpxQQ7u5UZ?>Z0f35SJaDk$2M)Qfnl3X^FGgp5pnvy4;RHv9~yS z(Ul9g?dDAHQS=tUPf2ItQp!D1lZT{1RcrZ$kv_107bDFDvP!n|Aci~Fg z9XYCP8?{Hi9jNlg4wuT8cWN@?V_H`!{?%lh&RhCi8u`#ite-d{i`GV9)5k`*6cGqkK6Awx4YhYaafVGA%2`lqi8gQSh^`iHxrSI##L`JAI*El#V5y8|*7zQB z^`emnv8ZYF5OYALYfF9;(pM)8yENWQf<_Q|hI|HN4j@L8`T%TpSiXzf<|6MF2k;6cmbbafmcJWm9KT%uvmgziDjmICSwjT!@-ie z-}goN#?k0wBKK3y{nT_m!3_8>daEjHKP303HPx&tYTBxp3T1j~aNbAS!&ANegVJi4igMq`K^KnFOP z1IVKrD_%a>H}xuwh7x%)J$Z;ZfP6i*u=}{Cx|K#_iCl{nwb-9euzvn8>N{TZ+uCU4 zLM%G0(&0cpfu%Sp>dpO~FNtL^u^1_Zk(?-(z>;^i_jOo8)Sonx6U%1oyctjD6IkNQ zAAS*fX!1Waawe9@fsrzEfLC1}x4bDU>(yfk8cF0pc_3pBAScEwPdZ9&IrOW*`G2~U z2waQ8y3m_M;2IMI=tv@P?Fa(&ArU^@eYTEV*YN{n?0F$RFLZz%%GhT@{LH%EV}tkD zSm_NmxWT&45#bz>mEI=8ZI&9kS|sGUnO|1K9>daOSP6U*v3e}kTh|p>T7i|&bzgr$b5Ei~uwcf#U)Q6T+F6%J|&f0wr4{#6_0F1X7R@9424{ z@PX{g1P6F5Vk@z<(sG#HKCU(Czv=49#C~Ew32*`MA#~)h!4H?Q$0&S^vWHK5V(6y) zqq59q^34rGY_PnRk}aihDfRviz`UdbOt68sk+I1*+3NnL3vjx?s@^WZ+pX&Y0WPqv ziv_sYx~>=CdP@xyBWJ@Q43-6U5rG322tX_%e3(;2;2;Gf;3UXFna~7}Wo$boZl@gK z^8lsD*DxUpY$CQ0OA9S`EHNf-sYy#pj7~Bgl7N*;*_#W8HkAm*qwz2bq3C#R9Qn_H Z9HdgAjfldwb_2u%X^FjT&_@DC^dA7nOpX8m diff --git a/.cache/clangd/index/auv_model_impl_dae_fun_jac_x_xdot_u_z.c.ED4BE4E791391EA0.idx b/.cache/clangd/index/auv_model_impl_dae_fun_jac_x_xdot_u_z.c.ED4BE4E791391EA0.idx deleted file mode 100644 index 661e76b4e4254023002383b8857427fc6bcaba19..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 2796 zcmZ`(2~bm46nzQ*KNf#P_yH0Ok|+vDars)swRBXFMJbMeP*GHHX~7CuE7lF!CIUJj zI&LVmLP0hK1_RTwDMnG;s-sjuT&hl8u+qvhRr`|vuSsWYW^(V`f6jaN{`cQGi+sGi z9$6qXC(>tgOw`5|6hcT!{_z{*)^&>zx?zsc=A&%~7NmDfU{ZV2tiLNfF5TjC$##p& zET+KwzK8DvhxXR6g_~*`>W}Spcb5jsx42Nw!N)=lL~0iX-41%&o}svwA=`KF$hpcC z)5Mnfap$MsJuP!|XmcoEF}~ByDYcWzDs(&19jp$kyfJF|{w|LJ*QEtNAFtou(fP7l zeyiE_VSY{TN`?05!OOeOU3ucIk(uoqCoUKtdodu?f5!KJ=p6X#{$7v659<4pYVT(cOm$6S&XpB}KaMn=JlC(tt~fkl zxm};EB|c|$*1ndk%sC<1IZH=QxVxZgUDu&1n{l&Vzi3~yETnGaQR{!cG|w6QdiUBr zd-I*L?3|;OukOFDX}z57|Ln)u^16YQef<~NR-X}PA01xQ zyJPm+v-@}74E*Bp9tVo??aF-mX6BmFUHN`@O73@E$!YQ~$lH-m$GC+}R+Sfd4_x@$ zeDGM7k8SG~h}W~%aS7T@?G*Ry?XQin!uw%^($lv#=@)FBB3_K&afdw7zk~ z@AeFGCzK-WT!fbh2`E<-k>{0p14;=RO(;GxAC*2plUeczx$AD8uk?uP%gg77<;pB>O={0Bb0DixJn7de4Ddv^B81D z;6YkGNP7zjb{Kvse~snU&IJiXIpoxT{#d0n7 z5fV_UYh!C2cL%5$G=WeWu)G1!5fV_&tgVWE`g%(NgGLd`JKFvonT1b4$+TNibv{H} z!Jy%UGJ~06rw{NpPIyEAt;54kNstqP=ga1+^a0?TNioXE{ne)>$eF-?GC!3*06Z!( zE92qmK#c^AA+WE^SEUaCkF6|t)N1;tmO%~#PQ>=2l_?OL(C^Dcwrgp!U`={QhGKq)yf(|t|K z>kbAf2&Ip>x6sN(g z1}mUBXqc?$1q|y!xdh;43jpYqY^)P2Jf^r(%$8yWtOMPW^}J|-2x=t&FI50Qqhw>9 zK;bdP)nT^IF!5K7Nt-pZZ%z~|xGFKO5}U&@E4W56ZZy;&YSM^TH0%gcCXIADhQ}0l z2D4`jjX=_3YhKC%Ps$b3xR|ztwjgcN=Hr4U0Iz0f1TrQ7uVDZ{yaeEt3&5wrs_WG; zdk=^dT!k1{7~T=IOjdkc!KT){az6~8fD^1>bSalA!l@#oI$MOZjq5xS&NHr0iSQ}o zx>1B14K?@zCFcD=Xi9M>F?-T*H7KqGvn9rL4Q6W$2M_*1zK_m4;1DRT9JA$yW#A(w zV%|SKbSuMbnSn-e6_~9s)cw<(HObnPdUBSY=AII;I6~Yj`_wC(?+wCLn?*5r8)&0Kka|@NxekA6aKU zz?X6t#Q1{P=(D~O<5$M@ArpMa#0YOT!Og~XCWSL8BfNvc9flgN4-tmW)C_H()8@}< zYj{Dh7&10*Za^fi7QI4O#K*0Q*G(8js68qhFs17 diff --git a/.cache/clangd/index/auv_model_impl_dae_fun_jac_x_xdot_z.c.59CA37EE71B4BA5D.idx b/.cache/clangd/index/auv_model_impl_dae_fun_jac_x_xdot_z.c.59CA37EE71B4BA5D.idx deleted file mode 100644 index c81bc5f734ed7c400f75ecc2e979fc2111f73279..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 2718 zcmZ`(3s_8P82;zXoWI){(M*~dLq-yoID6HEPEs56|;MAI1hANN&5+yf`<%GPE%@BFi!&A@F>S^kLq&b4nDh zHSRBPm}GlJcYb)B_$+t7+(#_)C|Ng%2pplu_kJbs?qO=1!M?<5V{5azU`VNzd5q&^c$!tgCObJO0w8 zUzAgAQh`P9wQOWtg|+UYo3K{VQY(IXury^rTjNT{nVBUaA5g*GeT67ybyQfk5TVuO zI@{}!Hz@}7M~KbD@=WZ@Cvp#rzqc9pBi}YZ!3=UDl-so8Hto$PpzO}Q7Mk6+%8@|| zLYZzeU1|)-aG%KUb)j{1lJ``12FVFrPpj(b06uX9gjD*5MRf(90Srx zhELF;wPBOGPhJllgSrsP6V#6CmGb0Q0nNeb+jL!K2dZ-t1qb= zi^Zrnp-huaQyBy7V@Si3_QMvm6*I_zz?HPTk__PzblBzG;#<+!d`FC22xY8ntjZXm z!{Ii|lbPyee~3{J0{hAQRK@^su)b&MpAA_J43ZJ}KCQS&tRmK1vJm>2M&OB1uBt|_6JWe)FWefoOc)Z@++LZo|L7fR)O)IL&04~9R=5}#7qWpN`^roVFGaa1ptIg08X_4fLsZ{X%zqvDFHq&O$$3tlKcWGTSnnBssrp< z%08v=Q}cSK5bqS4;irW7lzE*f!igd?yh?(0NE3O(>wr!f>bvP zqf*laN;ZeWIi`;ZU!SbtEC5!3_Q^pTgb$CU>(PVMF^#9t9J!wHM+4szF)6QE69~*INh+yaPndg zPkt~qpmk9F^{6RJ%PwBnzh&e|sjp3j6IO>0zLw!|!0G6&oE_Gpb&K5}s`o99YDsv1QX7@pC_K7-%#_(1i^q5VQfixa#&h|V z1^F}Y=iRH2O)ZSmoJ89#{;G{(%_6f`xmmqc7W`U}BV1bcN4ChiDqv;WPv5Qcxu$DU z|2=bfe)##AL#a%G$MTx!D%+l_gM}?`-FDj)UB9@*E6zJ%P)FbRF_{x*9*Bu}`zGek zr(4HPYn|J&^<>-Yr+?!dIbaI)A)Lzpuy@Gb2eBmw zo>ugfSuD{scpOW+)v(4ZHS|g1%F!`<_MDvW!!;G}b6seWOHsKqL$oVG9>1Szk?0qF zSSKlaxv^qXf4ao>a_4*(>Xa}y-+EQ$zTTI?{1k=AwXR8Ye^2;}ib==b$ z7yn|4%WAjQZ9D1e%=qv#2|Ys@&$i$YGky-mCRm`~-97uO_do zJ)wJ6i8Asktml+{KjKnR>4T0+_Yd`meAagOYClo)KpUM!Beptw3hn_ze;W19Z zB3A|=TruF4rY;(z-?I3hQBl>em_DxjtFE6$9Sk&G?GyE((1i*s4)R$ z)6V*cM!WOQV&p_9L9!r~AwZLg$kL^|>o>WwXfS~vVfiEMV@yz?G;Li|q#{_uA~~V- zU{wzeHYT97IS)O$Eim1OMKVHpg%z*JUGNDgl8&S4_SSWiSn}G4dx!1b;mO7Xl$w^X zciv|m!&o$sP@1s336C=-pk!HExr|qL%wUm%P-gX;B{c+in>y8B5RGa^ZXe z%EtK9YgW7r+{Pjmp{%7I|<&}6S~KDlpL1&f9d%2rynl@2r}pxiPm zwv!BUye&o!gyJvrR~Z5{nec7-0zdzrIu_Xy_zkvugMEz&Dx6z=&3cqgas!Lp38kEt zm(!Ds2`KL>v|l?9ywJ=dYeH$q@^*4vd;-dlmdefHYezm5BWFU1mPM-!0nX8B#f=lj zc6W=>Fan3k!c>L;FiMV}n{u8g67(!k{Ev!-6t7-mU(9J~iI!G^1~HdIOL9zWP&Z}C ztJ=6t%4sp9#TGEhpmNfdmpHr@)Fl8fYyd#f6vZp~<9UwIk|VSdrWlk>_Vcoa{Rx-W zq?b>)?M^AV(?aI7Py|O&a^*s%+_VPalM1}*p#sRA0G}2)0eFSO4v;nhcwGYky~!=< z)eK)txn?YC#+I-Tgx$xI7qpQl<>oQ-20#rdS0Z3a1ijYx1kAnO>kSmMf$D{KQcP#> z^$j6&L--K~yC6;VUST@h0%pNSl9VeIFr~elGz*yK-s>!i$?Dyti(J!A~^z5`!lE|-?%n#K-u`Lrb8v<6Ee^>{ymjbKCs__X~HfcGBQ@maX4 z_EgfAY=M$HE@Y0I1_j)ROr+k5fRDK%T2f@1NN^=W(_4~>mPbqSOeY3+BK<$@Nd({x z2`c>Sp)Oy0b=?JWFpWqfhTpBkCUbXzbbU1$^im;HYC5lyJB*pb_~YP%laYhzZ4Am{ zZa*#AZ#oz_8tKp584$t82*8^d0N`E(__Sw{j(WoaKIV#PNwMk1z`F=dZ(Jr?AuTC1 z(TIm7&enq|$+0Q=ITdhE8=40`{wJhHFba)=Z3x9C#U+yeRy05&p@f)b`k3_<^<%{r KQuF=-OY|S9;~VDy diff --git a/.cache/clangd/index/auv_model_model.h.60CCFC5FA93A2F4B.idx b/.cache/clangd/index/auv_model_model.h.60CCFC5FA93A2F4B.idx deleted file mode 100644 index 1bf1d161709426d37f7086f5e4cec6f64a66f075..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 2004 zcmYk6drZ?;6vuDnhgy3@C?JKQEfi@*z=CC#w$4IJeT4ClF+ki9wbTcqw$7F@Go8XR z6)+>dnB8=~nQm-294I&e&3Gt`5yq%nROXypR0x)^NMx7m@1$+gKhCG$+~2vs^Eq9y zOt0^lf{=FIGV5l;_FM)bVdt{!^&y60`YAaS2K5wd9 z^t>mzdg{?zr9=AKAa3f*hW&d)<)7B`%YXlE$hsp!lzeI+JSi@FN$FA_i+mq{_`S-m zpSR7n9Q5kd?$6xV@$inZy=?29oPlZU^rzx|&P5G%fBr|6W9UKfyS3Y{pWqJFTVhTg zDG9yo=x!G5@JPAwYj(fm-&KJXr!QJL$5K|!%)VKp8@$%zCL2#$(fH^E4xF!5tTOwD zl<1`Ic>Yr0z4nZ1Z)=6Oq+F}^`ollF8gq&ea!G? zfSvF7`;lE`>kh=}(U#x={?44Y*X}bORdU})XFGk0K0cIqx#8q?h5IEB#x7ZWMIdQG z=EBWu1a1h4LSB6EP5FswZAZZXe!TaeYYlvT80!D!lSi~%v=L~e+Pao6Ng{ET^ z009M~n1lS?ITCup!WgeLHt(Amm?|`p5l1mm;fOnV(yK@GUTHh9d~1IuM$_R0El;a* zHo1JuToo-ndIbGS1y&4iH<0S>oc0Tb7pL!0a= zx-eR$ztV@12L$4nI2Rw@F#fP<>9Ut3@EQcvjG7D!nmpQ~y&I9<-;dEW2NbnP zY=WuLEC!zmrXI5xEGC%R%VKbsVCpQ3!B~Q+sVoLh38sFs80;jNTFGK?l3?m0i@`*K zsevp8{|Khuu^6l)nA*l-aE)N<7>mVO_fB6Nur+d~iG@>zATk2+NSX@Ara?D?sb(w& z$q1%$u^7}Mm@36$5Q<NM1w_GtWYX6E>)@`!&zw}HF6jb7Hia+c$X?olLeK@ke@rzyV)9qx7TJkHJ9|m`PdsbQ z1YttpZ$b?rtZylP{#B($h(b zZAU?N5ISE>OI6?8+zj8SY*T!Q!V+FeO)5?{vUGRoBQoQy;x*?Ol zRr^@Uk7_+qzPDQtim5nyiOs+^4IJ z%nOA`4|c~moLAXiqP(zY`6XZ~ygl?ghTVmMdf~9|@uF#Sz_EAx@CQ~&+JGv0VLRXl zE}`GLVog85a@ofllcyH;%bk9dE=!@xSEaq^V;T&ztZLs5&L;!WeKqgcqkJ^xI4(Vi z0e<^7$z7h7$X=b7$g`r z7&90=7$q1t7&{m=7&;g|7(N&)7%Uhr7%vzo7(*B`7(o~{7&#a?7%dnn7(f_57$O)% Y7)BUJ7)Tf#7#$Q978@B!7a9Q}0O;q%b^rhX diff --git a/.cache/clangd/index/main_auv_model.c.FC541DFFAD75A3A0.idx b/.cache/clangd/index/main_auv_model.c.FC541DFFAD75A3A0.idx deleted file mode 100644 index 01481f6f9239137c7f13b9cba7727d0693207af0..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 1648 zcmY+Ddr%Ws6vp?;1r}Hc7$70LfN{WrJPH9KBve6?AhuNjTbKdl8C$5NHds^;{lg$2 zBah(?2@C^41{^H%&{m*U$4+$s5gD|#)e*EB9Z?4atEj!yxkOi)9Un(RB!XEu{_9-*#06`QY?|^!EXHxf+pY53I)}s6t$@6eu%Zg3}ZCi-Mz?6u3SdjohAgDrk{SWx8=aYM)c?SK8k26=t5K zOFf7qJT+{$^I&Pz>UKKVM&r@U`}bNt<38SgR#bm=&_<}VsIBM@6PwPdcZ;J!WBnJKzM9^k$$Ig6Z0eu*ql$!< zj6M9Q*zhw+9?uRxX^Vcnk-6uoyPqsAWkLp;SwRj*+)|n&#ih~{3EGn$d}-$Hko<~* z&|Fdb&ZmTnYh%&h)`>%kf#7~l`LVG?@M?*)p93vQ-=*n;b! z1&h}f{QWGvJ--{h^+<5GFX+mGChmE;Z|!@# z9Myst-6xYtLr=6y*W2AzvsvdKTvMwD=R{YVik~^$5Z%0-*Lw7Q^cxmG`BUG#ksetV zXRBN`0N4Ta!(T`rDJ|&eEJGbc@FT+T zcAYZwJ6+nbDl~i{6fzilF3g0KwaFV$_7;aN0*n*j4(Y?nM;e5M0m2xJmCyqDR{ z2uq!$ff&o|j#Y1%COZ)p+lfOlem77(b0qzXQwaA0>t3)H8@CG|+#H!TJ%fxeGK|9b zk;|WRvtH~KEOC~IFn-ZH#D3pm)`xH>fSq6sHk=E__ug+D>qn0yau)IN*Ob>6?f^gg zco<;`l-S~Ir)AmQLle4DG{9t-%*U9aQ(%f-31ivY{!*MCN3|mPk^15Jo4)9pk}Xf8 zErY3G4z9!tjQPCo!p2(&H-XhnhR($8b$I#<~l@F5~=e0K1KKA0YdT^UneDoIw*x z`JU93XUtKi=X|pgIIrA3Tn;!O2q#0x7(%Wg)DMav^k;Y k1DC^dBGR)n_N>&fW3yM8KvOew($d0;vS!)a*n-u>zc_0G4M zDCgy|uzk{Dp(2q@HoN7Fu4zc!^!;!5_%81f)qwTPtjUYbtMZ@OdiNIh{$_0M;b30o zX?m>Fpuld=TcBqNos-v7sAFs)R0&6P_L8sV*ZO7Hf*{ibD;oOvfX zVVn8BD?)E(Z{%M&X=!f1$+`unLX>vRJ9XXuS%z<&Kl={R;(rUSJvnfR^VN!txK2+qd3Gae!F0`Sm)`h3VteqY+B`JL z^rF?4!|JX@3q!WA{=w0$d-=d7e!+0f+He2oMCIpU&8mx@4<* zrTL|J)7|`GF;`}V3wh}VGZa_mCdmV1<8Sz@>W54D+jtmc7#Jk=SoAo+1W*P7ic-^x z1(_HaJiqgVvaoBaG0I4GGO~0s>cBK!IB~zGYssoGpoEo*l`~9Ozs|&c-m=O!$HBfp)={M)!cKjd+bTV8W%VC%0XY zT5u1j+KR=B4<=kTp>rDRwi!==!qzO-TrlC&c2)a+$xVC%G{jcM))FRsU{<*eY#it6)4=o$lJoG29xMMbVBoUb@MNvgpHhy6-?Ml;LzNyGt2+6 z$w-=Un8Dp5BU!*x01r_a$tsR2B)M9iS|qtfMwUiKxN~JB+ZZ|87~!fJic)hDL20D8 zD7iQo#E=FN5FiR;FffQCm=Xx46oM%RXEI9K6|2nf6gd^dCM7K%hU@LV5MJ!^qsHg~z6s<_rs`a~x-$@RAuWLR3c>P1x;;j6#lYP!UcTeXe!hgbq zz7Z6PI>SG2M&_JUltQ5h$p4%tZ!KFaUfbnTc=tx(=xep@ zsvD-UvEesfyANY8d_KR(?NVdL_@*hYTZ7Jo;)jmmTPqg3_6E7$^q6()@vV6kCs(_> zxnFL)n{n;4-Y*Y@IGcajH@$90&b@|#GvQh69Z$wh{rOk7pE|$f593VmYE<8U;{Iy| zacTMWrkUlL%YJ%N?eS=lO|&T0N7R!Xxa!ADVs^V54ftX2>)OX-i- zD;;}E?S_weFsZz!ga38yx3g=}`kPBUmru#sv;H1!$A)C+7rg(iRu*Rh#6uD{LYUiX1=pt8==$YnAF* zdNr+?QJHXPjs>$c$W-OpJ^q+?)`8;3-D4vnAI`Ns{cVNSxWcWiPngk(i8mLm2`N9j z-?>jVhn0P2TfwJyW!sJ*1Dh|mnbtRSd{ZMh*ih70-t>)QXZM+htgO8~i*_vE$a%LQ zzEpLStPjxE$mGgk+*j5_2t}QWn!ne%NVF=gQ8! z?>D=po=%(LJvOoUlY2JV7QD$XdmfDmt^ZMe*d=Fa#}^3^@u_DWs0AVMDUk`v_Jc!e zo6Vgz_AlU`D1F#@JlWkQcXJJ7ClsLzU=6RWe#>)n%8fs?%XYqPsr?nNtBBJD@j zTZq^BF}Kz~KB#nCeM+>Z+kEzP^UoS*Jzlvl;IIun{ow0!Nw<^yy3-tYHk3PdTzX)}gC^WXGDrcJznDn6v8I(cPCj7e0Bm^nBUv^#&Dh-lfT_lCJRB$O2UQhW`y1Nb&K_gH@T<*cD}B$yQ2Qfv*i z0}#{3RBiT{;5mYWLdEUy0d;?4*veM>8b7;@jciI1`R#Nx-AFrtx&2&QZtpJ`dgX5(=%WozKTJ1B zeZfP*$(5TjOgY*Ce48>mb5hR6p?bfd8fzB_mfUQS*Tu-=7>p7SB0G^N?S_|$;rX5w zB#A`|h{lNAAySKiAqo*q)RWj=EQUxT9s`k5>;aLFI0T|lF`*}ky#&%Ckw_pd5~T#v zBJq(xS|p(oNQ=~73Tcr_q>vV=QtGOoPwFA{hbTZAr6-xa4ALT#$RI5;r3_k&%tr=k zkp;*gEpmG~q(v@~Lt5lYIiyAIBZn-Khsq%>3VQ{lMIli@S`5D+n14sGHwYv@OT$`ddbVfSbHOBR!hzuc zkt4$qA}5BEjzC62nS(MBo3N$ZazVmj6Yg|(E^t~l;Y;`Bf?QSAc4cht{;7PbfJkH} zvT_#{WWV!F%LHNsd7r}mKO#^q0in@~lBoGV?J}hb34RDa0;GXGJlB$CtaNh@83(9H zw^RWkM#Vf}qJWT~5{RUz6e1ZagGi3bAyS|Uhq zNQk1)D2S5KB#4sHWQbDG6di#*BsW5nu|Z})wFHC`(KyrSv<9IeTWcfu5L;`bo-{V< zNn@j)G&br7&Em+1Gok0mPVMd-pS02 z%pfv1GKWaS6AcAVhqQw)w$PJ=FM&wPmqH}t%OH~TZ;pQXYHPJvPhn^n z2N=U9!mY!_dWuG)Az1>N08t_<(G+~LvcUH$(~84N(GiLv)ZEqJ!KJC15v13D^x$0(L``fZcFXL>KMinH3Hwn+RY9 zK=y|)LxuqR*~CQFL`W9TjMvG|_-3Bs8OO+I0{a2%5!^SyVjl_ zM`mdbkHZI^4cay%rnX(vKOqjA!v!BAbAS~#N}mI;Pq8eBj!>~WxJ-SHV9Ve#Rf}Nt zFt4Oxt8`^+VNMM@yYMrQ5AZ71^t7A^QH2*xNO*I-EkOnl5@FmhOW;*PB7vKrBjBD0 zbx`+(45GEWFIswxJ>f$ZBXbjTOJL#ne{KKn!p+M*A#H6#l-yMiv~}aT#x3I0@gFvuhW!u{$47XTmxBw&`yUk zLv=gNCSqB!kSvZFr;~j<@8Rm*t@5-F2m`Q12!Bxg56%WfOXxeVx=oux9bC}BW|QC5 z9G5_2H~4HaY5T5Mqs?YcBU1+7pC1TJXSk&I<4I+CwD%nhr~~MrI`F>D19jk3XrRqv zHNN;SISGi#sDv}v`-TzZvy7Cw`Hu#|KPR&v&w^(SRsoKLe^HrbPEAi~EnoTp`&e^$ zLt*NnYfH%q=%eVi;F0>w9y`rv&4cA+HsF|ZtiZe>dz5jNrJVc^us-ywfzp9F zGFh4C;JgWXBk~!*iF$C_KK*mc)p#GCKv;b>E!XwYl09?*;e+{@ zfpkFn4;Ent5(|0!!6G4w!lEEb!jd3L#*!gQ!BTWoDSW$n@40LH$!e{Zs8wJpArUSK zSAnU7L?V`G1*qdz)1^HpXI7CB$uwsQfn}Ss42qu`udOBvhUQcVEMse1XFxpx9S>BS zzOUwd3_W|yv*}^(tf{phg)!h7?k{fqRXQWf$LZy9GCO7r$*=&~(aHGLedojD8jXYm zW`gOOF(Dzc5ZQv%DI~-~v7MeILJ34tp%fySPzI4)D2GTPROkpSfUNMEyFlPqx3`w& zo?Y$J`T=S1fLr--H;4RIL`ayKnCbdwArZ}u)^(;eKXlKpcplwOI<*Jt0d+w^XppW8 zddV&C%~`##{R4^)xVHRDE-5&cJ^qD#_7xg=zXM%HkoP-Kt|0Gs$O8qiyd_T3<|o~J zg^WfP%Ho52g53M`%rmxQuYH6^<=P1w^A334zlM@$r4bwlQ2GV`^|=@HL}X(Fm-hD; zK;T67roH^F;YGu)4|u5E*KclWJMqHNWMqK9>BaG!76oU zFjC%!U{7KGfrd5e&SbQ4yJ{4@+j|890vm-{gK0=|z#JiR!`vYX#DXBo!m=UC#pXh^ zl8SwyTO6sigo>5ITt~%@!F+;>HNt$EinYVM48fM`j3M?F)r5bS4|SJAN5y3HZ=ucu+fFz=#a6*@ltZoIr#w&k~f{AiN-n_ZjF$i@9+tLS0u z0oC*%;~-r%PP*vz?Y3gp?;l~*0tX`#x8`RU(8s3+`6r7<7Qg-oo6%W$?ybV+wzp)S zvL9}53F@GLa1c5S)05*UM{7Mf4R_LglKgdQdg>%H(Syy>{Q#+Z17s`R8=!^sSNBF- z71`VC0&#_{gJryHdkz(x*}=g|r#)wOdQK!c8ogV0a|L%I=z$zl&7FR}P0#sXZ8e}Y@#mbe7> z#BFg0+#0vTN8@8~8SaYP;7VMD%W)Tc96la*#GP;vF2?O~FI#m69W3a- diff --git a/.cache/clangd/index/test_PID.cpp.575590D7897A814B.idx b/.cache/clangd/index/test_PID.cpp.575590D7897A814B.idx deleted file mode 100644 index c950a966809dbe94bc0b1892eabba8b7a5fc3e98..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 8790 zcmd^^X;c))7RQ^RoN1VrX`#UuMqJp3O<-_X1_l{n7+Gbt(ML3ZK1IL?8WcBN(72$+ z1tPd61~)LEMiy68T;dW1BM~u*F}Ot$H#EM->lt6sH70(@IedzoW8trFS9RCzd;fK- zh*k#&M-0X=Wr{i{JvDnAz%Yzr|7B%QomilcVH0~}SWZb<{OHV9AM#??PUFm4*WDv# zkImb3!P0P~!RU>4KX@s>d(f2BtS+28*LLCh9|G|m#_mtg%VfDOMs-8*6X%y2P9AlS zu(qb7b~Y8Sh<@|- zdElMW6zMGUf`%mX$a>NDIbREZJM^M{WCpQqKyJf+Mb5Viy6Us3m1(^cGmqx)SA?g9 zSA_?!k*+x1_2ccTSF@$!f%mHwp?5B4E4QvVobxuTY3~IvE^K`bRtZv1IaXY6xhzvT zwmR7@-xuaPm?})=7mk%RI}Ts+TfSssIl2Aq;H}ju8G3JawXY8@YL%S*W&D+5qtfTg zW4D(rRt$1+ZVfy#2PF4aQl2#H`JoGRJOB zQqkI~x&fK}{%dijsDt-JG`Fs-Wbt5p8ksnyX1};DtZA{I>)_ZUr1$;g+pE7)np8Jt z*_I}aUa!BpQc`zrk@)qX>w0$=#=Gpgv7yc;W=6h|_sa5Bwaxd2z6my$v^@HzNJgay zwm$n=S$?j#xYqUf7teC8)%)dV+_t*$Y)ZPkz9DXlcUEH6tl|XU{ROSt`(D}TF=NW9 zhc~=tAMbylBy#P{?P~dw2NzZC*^^RJ7_%$`-?%4FN5}?kuA{cdO(iGhHD_PVo3WL= zf82#$+#q>ae%>SZ%gy?m@(%8vetJlC`_MyEie5HVKe*-nZHC31Q-*5CC`K4ov8$#0 zZsm$=cd|{r`8-4EqnNl%f6omsR|4ST@8r2)A#j^ zf!AC2oY=V~{f}w2HXc*>mCnCNBNPzkIQI{U@K@@Y!~* zZqJ#UO%D|n2|1K;bYv$AsJ9K$)|=Tk-f@5xM42Kouja>l+| zIO}?IyZzqjy=P0p_n++Ol&(?bzp8y)@cL)6*fWve%S6&SAi4U*iL4pvpDyEJ*j>N# z9k)F!?aX*1qcJ7|i{NV;Oq0FrPq0~e`j+NGi4iYh1jgV(oYXe&j2QNnt=pKSI8zzP zi(}1(28Kr32AX#VWy!C{pOXoAF&M^JVOD%?0|(!C?8Luc&Pw+&!72za6FvqP~^zP5p{Hn5^a*>Ov@;{(s8K;TcZpqSkP-bl9hpFYX2ei?2y zip25{cx{RA6jQwIPK6mSGU_kfQez6Hjddul{(xB>*U)#p{bwpY8~FYPL6@&o|OGus1d}b$nKn zIfiW%Zk%~049E10wcibXNk%VsS?1)+JRZR~00$CwBN!*()J@}NtSfsI64Q&Nu?~42A6r_}R@0XUw2$kX^2?#XEhT9{`#4BDNT2l!gvqoq z#Ylie1TRyJGjK-84Y-NmLn+1`xFaM5QiME#Cqh2JM+9d?F>)YB$RGG4qyP$plt774 z5C}p@1yl&Bff}J`5KZP=eE$3X$o4Xdi2*Spj$%P9VoLxC-Q?=*YZ~|IA+bKeXa?Xu zjHU}mnl6yaF8H6dG(0lM80h#7==cpVsNaB&-vER94KS$R0E7As==cpVsNVpC`VBCs z-vER94KS$R0E7AsXr~Q51$6ud7}RfoLH(96WA_+h-C}E=o=)KLbO;JGn+zH$fZ_m! znKaZ2GE6OkkyuIWVHgsOvz0SKZW1?F7={GnE^$Xl>Lwk^**u4MdUE7r^+6jIthA^(kA=WnYp>9z_^Xls_neD=VYh-0_Ia%zipm(uwayzU1mYVs!rzf zz3v}%!op=+&s@7)!{C9-HIMghnf`yW0xdx!T(sX4=iICVMCQ`DiXQq*cCR0twQsww z60rfz>ME2CHlSHuCcM^;n z>1M)_JL!&)2k=1dl#)`!=1F=YOMRgA3|&|-k6LvV>?Z$cmUPSjyKWmc5! zs_S?>-B`@n;5IaTMKNQC+actLJ0j#pxmm-27BlXYJ3>-QijXJeiI6w$O~Wu1GrqVl zLVma(LJCS@&E2V_ln4b;K?tcR6+$6+2r`PAQX{r#(`XwQC}JkTG@+Xeypo^wODfLk z^=^rSj(g{GSX%&{+5+g*7C@)AfE5Spc=Yr0bSQ(?OCEcvWPT5Y+<|>-myWA!ny*JN zd@H^kl*Nc0o{43JMvHoAW|5m;6kZtBlCLLb(uL_X+y=s(85^^j4K^3E8iK~3pZVS1 z+q!bK=%2AgLN`V55aGS03LYM?m(eVRhX!obr8zNxpXxhj3;bWJuB`2$fLJ{+@p)9( z<8>c(fqU@Bv=K>(4n_aKn1jV1n{yhG;c^a@9NF|xpU$+vZ2~bPp(K`^x;sDPx zmYmwkDLF#^ls`g|RHP-Rp)piUH${E9xGnK&#x|aq3BUvTal^%$#@fOz@WkuAM0q)) zPw(~V%uXJqbHqaF9I?P1FFRpPHgOY1bL+Q5`&L@SIqhK|fS%~U=d&v>dbe&~!UHw7 zFREKF7Ub^Ox|ZGx#0)_YVpw8|8GGCwSrdoh!;qCH7!U3ycoO&CJKY^|{hoKxk^LX_ z(MNp8B0|JH34Pr5;*UqI@^x<+YbLuw{k;FffLju})FRqpan-8hfbBSX`4Sf8~&QwHKkS zT;W^@or;*ysWk{46(n>dVp^LZwcbwF^ic;{Rq(o;@_@Fc9ts3_#Q$D!zHKb|5C0;+ zwTA)$)<%3ooxc#UVIi;*!7U6M76Mlh+&8gdA&`k+(aDB|AXEg`N;WJ65hA#dz+*zT zOh@(pT@4Edm^AJX?{ej&c6OfV%5k_$?*7Pa!_&WUo{@BM*q%*mgfBnpC>$52{={YQ znm<13s&s1V9a6wcp2dEaxST!jjWdCTFs?f$2(~ zKIp|4;0A_5qPLNVH11An}_M+Wr8SO*+ x(h}N{cA}kWdpdx2rTuAldMNEl`_XcGFg=V8qJwEM-Jdcwv*@SlYpyp4`ybOEuOk2e diff --git a/.cache/clangd/index/test_VC.cpp.74BA115DB14CB17C.idx b/.cache/clangd/index/test_VC.cpp.74BA115DB14CB17C.idx deleted file mode 100644 index 5c01a6f3a96edbe4ed0c17507e5203e90fa63659..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 4538 zcmZ8j2V7HE7f&!C;~|esUPwZKAVVOCMji=!?}rQlfz|=ESa5(=MyqwSI8Yo2D&Rl_ zx9p{&fPx~Uj)GPhS`kq}st9hsoA$d8N`Aj2_kZ>|_ndogfTx>VsV0Tu9Oao5AH8}p zjY6R?!8dVrLTn@ecQh!Jq}+-H@hcwVXghJT+%`6(yDvUfnzyUgL~$%j=2b(ltR=Em z{dV=#(hHRg_Svz^y_>Vyr|1f`d!06ZcPRPq)}*HTk9Twi+Cm2yDuxTZ-@p0m#$d*F z$u;GUJBot|z6Z0tju?=I((>@Q!uSF~Jd%F8t zV&+Jr+rwAz`kRCwynkR;$i~axbo?@wx+$Q(tFy{HJ|jPXVV?H+g3)vJFGso^UljHZ z2?y(Jccy&4waqDeEM=4Zg`wL%=$OZ;IYMKjS%R&93~y}bnQbY{wP;^5w8cN->1^J{ zNV5!7_n`Kv0&Azt*PAxa+*4!x>{w+scX8=4(c#v}AMWyR*#`VtDX2>`6)ShOEH&Pv zQ1G;&V{n_WzVpqILi6Uc7CpN?&7*uYee<$HA2`0dQgGGkr}3;uT@J089Ig1DHs{DR z!qDf!^1ju}tv8nxhlJ0N{N5fUD%jDyIi0`IyWo(8_3J-NXO7uZRuz?w^Nikm321Ry45!% znvOSC{9S#xR^jHzomK;JaIxcQ<3p*s-FAD=mtMO5B)sOF@wZ`S%5VE@K6ft99NN0b z-^*9jHs|FoZTCA%XNJhSf4$+5Y!a0I@yzYu;byg8Jv4r7!Cx*2tMn5*$ooXIp!Bq# z<~JT}5G6e-{0{6O_7$5qfc1HCI{rP}W#B$_N`{$IZD`|u4Y7%*dYxY-guZ0qUl*n~v-1=x0}UK2pA2 zDeX3oj_JU9zSl`zxk^HD7f(OG#`W`4FPnY`*YxfnAH_45I`Rw{N0pmj3@#~QQ+EGS z`>^mp4N*}Wm%D!9?VBaF`Mb5&(z#7KtDmVCrSt@+hCI0tA0`A zqCiWXKk{_&ZC}c1#$R0fu46AQC;qnYXp8C3>p~hV96XvfoUm?We|jIO>9BLtmd96{ z-JVN+PGPGQK6Bc;iyydL!nSI9eJa3$yU2SeIy#Oz;wtK~*c;K;o#w26?e@vagm-4c zzCF^YuP5uf8dJ|!w-^6ZVWPjZe43PD=c=t(mY|OBN4(;X- zS)XF;`z}X3>~n4xt90xq#y1qt^0`vGXx8<%nRPjB+0OhZpA!zvvykRQ+xj~CqOKi> zHNuKbZoRF#k9<)S@*apNiOKOx^1x~1*j3WLYpu8tp)90O2z#2n@;^U-^Uvpmn#jjEuip9W1s4YEU$A8L0fI=(}xISrXt)c>tVCpbUp5) zHI|xCd4x<^Mu%j5L&Fa}>fg@-i3ywpUeGo|NF)+lD73IGMs6{ zlKOm%BA%kV?8>I3YP-9$r|}3+)RO^sfk$|w-jk3o>Pw<0#mi3$hXO3+iCE(EpaXz{ z@X$%PK)Nbj8|v~1hAM+Z{SVpd4}+7Wz^{lQ;z1!R{r=FZC0Ff$Noy@@0hw|6 zL#g9$dQS2NGLo4?a%$Dg!|O!8E^;zYrVGgutcJFeYUK{dLMNdMv|Z`$R+8On>J4(@ z(zpyrMhF|44Fl??Y=7m*B}Tm!m;@VTbD$8D;GsMc`M*s$5V4XK41yQv2rQwH)^LAp zQAhSdkhoAqs1C_ILSJ296C(erQNN4}2g2o*oyqZmWKh|ed=e#93GO|sIUf$@$V$)3 z00tMP_t)<-L(U>V)>YFbXY}{m-+IQJSrI*D>PBOi9Z>(?ARc(am*GD^BM1?o6fhmO zrl;*~y~s3b0;rXUCL-s%Brl>#k>iyt*F8tY&y&4|5J5^oq_%1MoTo2mvr<9E96wGd zOom7B{rDt`ewlU8GH>pBxh)Ueb0UuUO7lre{WlevqZPI5K}nt2&bqJ-*w!NkxxkKppSrVs_HxL%5nMt6b;>AG}7NJa>TE`vl2rHW(j zYeII)huj8tpC|$T4IVoQ{X)gJPbxK@PsPuY*uz~ix_+cj?un(!BF}4t(4pzjq2GvI zof}m1a(7Ls#TxIm*- zSJ#&&SzIaoz?5dYTl8Rt&)$_e@=oA#44@^@30yvjT3(cMPY-wHfDD+b%t;ww1QrQZ zUR&}Pf7=w7E4PGj$V&=XLii-gIa!+*%~+i~C5i;@eC#;0=&<}i5X&rJny{Ow-2XfR z91p~MKJs=f1mWQ6RD1_nX@m$=3M5Yo z4dJ9o&}Zm#CnQZ2O%NCYxG@za2#gmYob{ZIVJWr;jg+r^8dnD*a6w#{kQ{z)wR`Ek z`*k2!@hq_gv~2s zLU+|}`;yK%4BS&<5-1lnf@CH^H>TT7Aciqx-UMQrF`Xt5%Zw$RKwKYg5Jc zeIR%WZZ)PBlf3+y1WSuWqV$ig{Q)Lz&*XdWB6f#+590&FGr0}n1p>(O7cRW~8~4Y5 z7rp4<5HNz%a1|KWY>Kpe?tGQuDGl@}BQm$^XyFKu7SIG_Q@b|jOusVf@&=G6zg5h4 zsw(1jedAM-t00CNr?u+8lgCXLb70)}t25W4Z(`m9m;Ut4i7^Yz7<0kqfJ1&BCdI5VbIb&D#2hev Q%$3FF@VH)D+SBy@2c>}iN&o-= diff --git a/.cache/clangd/index/test_VC.hpp.C3EBE494A18C2184.idx b/.cache/clangd/index/test_VC.hpp.C3EBE494A18C2184.idx deleted file mode 100644 index 27b853ec84ddf9e4b5f8341c878f2c48ec8ba3d0..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 1946 zcmYjS2~bm46n*(1`AL2jNLYS82_$F|N0C@9W2u0MtlCL&K@nv@q3%VABdeo>3!(^$ zRqHNJDjF<%z0QT5+Uk-J(T`z!brSt)fz`()YjI;-ATVFZbMg?)~?jmrRZd4=?5; z6q+2Bu_R@Ak|RP$fj{QuX$xw22*nISC}Z#Wsb6h+WR%@E2^I^_{&Xy|vZwKiOI+s<8o5YS6f~Qb}wGoGTOEhoU`&xPOd3YzRc^$h8zCrvqJUO zDBk^xqYgmA>L zTBY{k48Z4xxbr$ItJX>gIfm(R^kmKeEVZrC#@X8PB!n8ndYPW#48ULR4oWRO>0Tfq zTrsTCYWz3@aAd^iJ*&DJ3nhdK!v>i_%^84iJpWl)*70DEgz&_$PNq|H2H+#234POk z4JwlnZuG}VkS+ojH-Iw@6XUK_lIPM{hUdAyW1Ey~(n!Wcy z$>A;u;f}j!0vQu$ARncrd}G!+AVl9O8diRK3Pn&P?N0~8z$sGcuMCE>t;jz(gZFLcD{MXZQC6&9%8!CX*a5Sqkhz5yaH>)pjA9~rvtcZ&@V-kj9 zJfI}J8;)k>2GO82>}F*KZcrF@v+@G>6hYYeB`LB(2a41QbW%_fMd}55DNs?QL12)A z2w}xctP+7K-+t8h;?t5Hn29#fqo6x{JO~y3f3au6OUy0y692D=9!tpOU7ou>U^g?084Jm2DT|YwF(G6j zph4!ex#l%&DN%ntyhsMN_E#cC-0D_}*{kH}huZ&6~HTsL7MJI08WU z%&3Jq8Tn}>002PrFE2lLt{DY)8~|Y9mVNO#U)>A^I}PfjXZf{(hc}c|{cu=9E~-o! zU1z`Vb(?aneLVNpL2gpu)021C55|Yu9oPhFiaYaCi&KjShD(N;uI-3ULLLt-Yn%M{ zT4QbOg!*E^$xexIqW{K$Zs!XU-qZaC3ytsPdfGw#h?e=WQ=a7N(z_ed+D5D-#1kCcOIC*&}*X zFj(SL(t3KneD(ZbcL!>-bHmMj4L$brZ}2RX;YUa3?hZ^o8r8XSP&9BaHS0#^ztn#H zo1>~Hy0@<%!qlL%sc&j-RU{eZkAmXfFB6^%{rf=c)T~KgG@q%vbEI|avy$7i_eaY1 z?>i=&S8lAg-+0#Ixw7oj$-}q4N_lmZ>v`u;gt41e3AQw_UY;ztb%fL9*gbVOJ+6A} zM;lIt|KR%8fSC;4vULfG=f*G-FImb`WOw;xU{Av6VFzREUBOtlbNZ^H>#u7z^}kG6 zS5eeG`WQdVQ)NEiID7ld0MhRM)h=_+FZ=m+$MbcbRUw^Ay$cdE4;Q+v36Q1J=Vbp> z`um%Yi@iuK|7!{$eNwGjkq`aMaBR-&Pk8dK{RN{*60qT&BB;Z!e9w&8XI!r?chGp# zuDR?yuU*!CMvxZyed3d@y@n;vmU4ak%Gd7e%vSHT4Gmd-Cy83WV^z|@_~A0QsV&L& zVX0F}pT4`++M%6QTg&RRZ0p{8z(_U9v$+?~&fm=)=u23ld%GZWeH^o82Uol;L^i-* zzyJ4?nYl=P#RmUy7F?*__mB#fHQYbiJvOFyu=w&8hukB#`@>?}cb!c>-*82l z@@_3uS(<+-)-L12w1?%wH65wnobe=eUb@I#x~FVx@VuRt-qN-=+bXsyKW^D&+f}tX zCe3wn-_(ZO{&knyZI-W8S`-V~)3Uy9H%g)o#|B2nT-LG2Dg+&OH$HWw?6{bFVg~QQ zFV}RNH+VV>Ua1`(pWzz*^5rDWh6jzOe3p(F`d`hD zEw@80!dHP=5#SX)e@Et_;6%&)KHtkOv;FA&jP@%zwOZbzUe<*RHp$5jrM)rL<~_T< z325U5EMG*g6>pz^Xeg|SvgU{u{HMM(eaxw$WBDbUYx?HxnRz^E+!x(1P9NR1lU;Z( zOT(KnXS$$iYS-m!gV}d~pcZzJ=YJi`kfv=ydJ6|XyseWw>uI(0e?9JoxLEb5_|3}A zi+|~51{GBV>SZ4)$)A)hE&R^GzHh_M@%||b{g^p8?u!!Ejlo=gkj({myWz&g*Y%cJ z)6I4D6K4&*S#r!&GGk$O*k9kJKeS^-wqAT(zB4D_$)Vy2Q+95yNap`>c1oT8`k($; zD?Sx0AUp2q$>YC{tu7q5WR*s%i#Ae2D{@YILfY&DE-~pRKKhPFHgRmHU3m!yh(PZb z50~FeOH3_Tb^l+-ltnpFK;EL9^fGkD)P?6<^UP~WcCt+|0f?HRX2wIKAs4_7)uF#B zk0l4)c1TXs0V4n)916!dUP6&L1NLNFz&y<+p3Ds_`rrPd=r%f#iV~<4DlguV;>ZK+ zDWm=))U(@;ct95){O5u1#P^8zb@KH9>`6Ajl&PqP+>BXy$L#^2a`RVvLIO^cZ0U%Q zCUnK^5EV^@BWIUox*yp8jvhrv;&^dBSoCy6@2MABkpVW0!BE@%Li?iL=XBJsmZT+P zVmcC`j0nIGGwS=;gKq&R8#)5nLkukK#y@+43VxgGYy%=(GM9phH#PSy>{5Yjlx-%N z?J*}W;$jySe^l~FAOevgD#iv85mjW5%rfJ;S*ClVI?pecp!F4S1Q50-9f>AIQ!x{7q42$h zF~=9JnA%4z!^BA)6+`lDJ1Ls5#z_NWVmcD;6iz@|CoKW#oOC$KJhgP;;jpU;3WSIp zMK0Lp5F+M@y{t&;B*j|@LS#NNu~nx46)Y=K3Y0<&HL#8+46V4XK%?`ZdAJgegUWZr zNa*2+`3frvrUm0$-EWxptLIabawI*P#*vU7Eu4U~G%W$?XgVDI-C|kon7Atxt*u_9 z7h4^eyiEcO1$jS6Xih&DHqyIJkQ)}}tMZDdNm(C8#0HXqjR9}5!ZA7#UggCREA#BXVc9bvuwk^esii5p_rC8Z9*ETMn zM;LaG3L`B{+Qo-yX!*Q7yge~C9f=}E;XCjpx$ z(4BxFA-;LWz{VTOtz*AM{c89c+;3v=Z+;W{WU?w?<> z|6Vu%zGW9oe8JJ0m?GC#~pn*7!V<0$PHp)QKQcYOT@PJi%2LE5s*~mXGLO` zn7|YZ#RR5UDkd-`42gr)PQsPoJ%~P3?otn{PRf(wHwe|qrTEYT5kCh%d?2ET=SN`r z3H-)by~x~U1fMdVjKGvL@uGu>f}tQV6_A2!-9aH$5b{$plmw;{ zQsT_~_G#?Oit5{F%Xo3Ta4{W;Bt_y^t8f0^dxuVNENF+SmUeWx-oHdv(Ln+tb1}FwbhgK4wx+#DQe55aGkN^bw@_Njj-mM*yl@*&_V;~ z6;Sb1gc_*?Dnd1b{e$J0zq3_2vomIFdWsHqDnPY+;K-nb~tGQa4adT9upah;z1=+^KF`4QqzZxm1I;!HjQ{`u diff --git a/.cache/clangd/index/utilities.hpp.77C0A5FDF681DAA0.idx b/.cache/clangd/index/utilities.hpp.77C0A5FDF681DAA0.idx deleted file mode 100644 index 8b16d9f9241a07ec421ef8a5e4da15b9b4de06c6..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 3028 zcmY*b4Nwzj8ve3`V3K7+vPlSRAaMf;CP52@+Cvc)!4O^)e)%`k=j~|O0T07iD!{A*jT8@k6O`Q@4nse8r&J4+5Mh6VfF?&lw_b2`^V)n=v?HD;Y$(9yrA{Y**g!}7a#;@cCiemqh~ ztztgg?pPeSIe&+%bFQ(rkeE4ptMo|Una9nmYqR$Wr)=x22`zn!9hzH$|imhA$PosT@>S zA}77Kb**+r-tpq7x`01EY5d=-?nHn-)vD=6u3J^Pog zUlh5q&=?exuw>$eUrb-hzVuy|PVEm3zjfeR`L^Dvk@}YZ*vpDvJL-i{YwtSohF^wA zikL(qTCGtB@PH=Z&cc@}`JJ?eN63#Nf@Fx6N~JO$fIe|!+i90NPvJ)}Vs`*-R2Tz+ zu5EO!|97-fLlJ>^bERGx$^+0T)Q^Y$R@$leA#hvMa*xb_)n<>JKX_a1O-~s5!CdC&&Rd2H3Xb@+jO$^Xe4YwCIzV_`5 ziZJ4%ky2951JDmDs*`6HJ;Ahv)1Io#Bj@m2jh!7^!BD) zUOAr`S}V~ifL@o>rIKF$HJ>7aIpr!e&Y)?5x5n(u5tx(fE06qUV zbA!6h{56Rr?sh@6S!9+Yu{VJzIj(rn3mcqYY)EM((L{JbCX(rpSmfn?q2XL_S^fR2 zb{_(FD6JFeXdX}iLcR#$9UPakG&`$5x=9f%R-lrqWITX^8?FrBod^4RDMH5`Z<=xr z4?rK)w4P?Pmv4^~Xz{Z!NGuc}A3wbPH#Nnt7ra_~S42?w`w|}s$%Ch77b;~H%H!`3 zP=txgfK*NL03RqA3cH%XZX9vfE<_k2r#S$+u`sDT?!eGvim-Cg!D<~3K-&rm?TOx= zsTD~0X@x<8X~oOjsDf6QQh}VGPDmk4Cn4dd5;h@eCYp(X(Sucq#66p1RU!G70uqM^ z88L*Av{tAU!vv7Dj?wAin#6h={bciw}01`yfW|P_av|(t=bjvKL z4tz=A_5+Xse&8kHeE^oR)(lM+Oa<2ZCi73&!C*2y?($u35n#~NI~mI3OI z`JY`l{Ok2ofmSq5^zRqzQoOYyv< znW~vS?gT7jf)J35rE(4fA>c4vF0j48GS0;uP0F~PY|I<56VyQv$h!?38j6LNIhTQD z*h3ijOe<)gDX{jL0&AZsF!&Ic5Ntvog^ZPX4m7>naVWgyx4r+u^H*N3w&s1G<7H&WQPV9T7Nj=!JibT)GWlG%A6 z0Qyo8;5M+au>kwR%e-xkEkn+HByO{MIz;0WiiZIkJ-mvC0o#J-$tX0+VU9`K$Qq}3 zo=ifM98L%Jhcm-@9~=XB{R*ZSGUF;a(sQagpo{DJaP14wLg0;DRv)pQncOyWeVk=mcFUN}D89};{-L}-egwu+D sR^U>@4MGafE%zVz*hM>>LRsL#)CIG2`LP+De_4nNVAKHth9{>OV diff --git a/.cache/clangd/index/velocity_controller.cpp.DFC34CF5F86A4B55.idx b/.cache/clangd/index/velocity_controller.cpp.DFC34CF5F86A4B55.idx deleted file mode 100644 index 7a23290a548b3a611ae7c7515e9b9fa20487e716..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 15478 zcmds-XIxZA)bMx41sAyNQWo}tyDS}~x&n%zBA_T1q=qYEq1mv}tVBR#Xg(nd3Mz^f z#EMv>SYiZ46wz1^Y(Oj!6=H&lB>K#1W@pTY_x<*K(BCiN{`btickY=p=bSk+!Tx@J z^M~?yzOnvG=EWsP@p(L+82y)&oH$!ejmL}7=kb;lp9v3I(CV&JKe4uM*s-6R-z0m4 zb@WkNwt2GVo?Vk)=Eq6%xsgXhS&Tud__+Fb@#1nY&5nG53p{_7r0C2g zOOt@RdXc@kqtiso9!}7ywhdSF9g_dOm&MK(R#PLYM%)P;Y3sj!RAg{a=?n3TW$%Or zyY|!sKVQ5jdfB?0<8?e|c1&rmbGuu8+V$M;35z|7S}h{9;-tO0YZEi}tgE-pFdT1t z<$>lQwUrwe+0U9?7yQeyBeT9qTpK+iW*>jow>rag%^ugTaQA&2exzW`C9|fkmPK(g z`;}wV)<%7^{`*SXCr@Y0^p@>dic zjr4Dx)Hv2hZS5q@)5nAxRysL*NVa$dIadF^_VJMj_^Ws9lx6v~uV1`)9kS5W)%nhz zRkrLK?O5ryyfF3#Ga%IU+Oh2X%rj19 zM!PH5)Xj}Ksyk)1x4y6D$D=P64&XgVe%hV9&uz|>DMjiR*Y-p&T@rBbs?HYq5S_TC zb`|}%hZ-^`%N;BF^W1J7a6Pj7&ccwq<_Dl;#`0g&swV#Ky1;SW>_d8E-1gbm`Tw4} zv#Em9O5Qd8&xGdU=N5~X>`RsYy!#q&Thx`}VC|mCV?UgUEh-C~&{KS*@OI&yZAOJz zK9b4WQ^oA{qN=9#?d$8-S9L_D?tQW~`tHDj3jyX6p1U-h7**-;m*&fcS)E&l9`SUV zzDP82x<=K*jN#j}$9UL12Ws`_$GJPlr34Oo{%23IQDgXJtHs5F52A}<^Ucnl|7GLr zd;h#Wq*1DqV!N#Nq}N&DcHgL121cifPe#n}T_~P^zxmG}b+r<-wrGZ=nCNZ%-o;Q) zP$`)m?LF%Ii?O?B?r+%J6k!ysb9V*1cnDXt!8+zy%Xgle!WW%g8~5Y+DT61+e|WD4 zmPLtxl=B>RL6jt*X3vXr1#Zp7(UY~v*w4& z0O8{8_v+dmJ0AXYOg{CLA@*zFOe1A7yk-agnft)r^^WBkiq^XI%x zIT>%ZcePqVPyOrW2Z=8}6dyd{1tJy=@zNSJEGBGJ zTK0qmv-kO@w#B&odS&wA{bvoD7rl<@lALY+`;VD!eOAP`%{})f(zf-(JDJ3)%4&Kv zGwIAD_D7B6)1L2}UH#KeSFIMEI_a~1XN+mbuRW}QO;LAEW{&Tf=JRIz9KX?hKCXJ- zaG}C9MmX;H{>=$btNtn6k=MRy_NtW&jrJseyLf7KgGJBV6KWOZA4MCzW6SrQyM6lG z*>+2>91j}gJZ0~OfqMiA`vMvtr|C{r@Oeo~=SAlL9&aLJ_g%%qT0I>&FNnvLKv4-a z3ZFi4`XrtJ`AqcB`oJ$Ty`1a%I&cAMPyj^*P#QjM;exc}WQS$QbWTL%WC z2E)a}<>AH}#$u&-A`<6)yY|?jtW)tia4r&OK~WYo4xc)4Y6O0Nc+X4w`_);|9%H^y2hKtbE&%ZbU>z>ekn~f&VjOzHi&f(khgVzd z(}4?7gS}9+7j-gfVw9?rS=aj?8)LivXNEV2$K!57{!J+O_Yb=L)4#;H8A*<=Jl>Ul zOIkIS2-O%dClkvIv3e0nWJVPAll7-)uxuPf6J*hZELaO;LRLmr{Rk<{6&4g(Dy%57 zR=85+r3j%YR52Sy0^R+|O7>2XxYoU#QW;m<9RunDQST4e2&2V>t{4TJkTWEd;@w9_qjsilU1;q0KD z7Wo86=tz+hbfU-^I#U$L57Z`~7h)RHpU^3&ehP}Pp)%Y#sD6%c{PeOp0qS!#1yZgN z*fxT}SSwPl3D`E#+zR|#0Y~I*U|buBAlwe@+kpq+4q)Fwb0@It1Vf0t3)poLc9`6w z6|^ov3o;yS7YynaYy^GqZLN1Yr_9TC7Xs8@Xw(P?VB@~vdekFkTcrpvTrTv?g`@G@ z-Q@eXYx8kk-98NW9W?q5nqsHGaHY_w6q;f$#BfKT(Gh5h=hNYvw+E-}o2?7PT!LDH zI@Ty|4vhcxSaX?eCuhq~T*l4{2|jx&EO57Kbvxj)R;$pDYn9B+gl%WzZF zrx*~Lt3H>an?P_ANU#z%op`q7x|^6q-)r=M7FhBbE+4>r zpc+1g>jkhE=;4M8w-dshP!B7V;SND~2wG5>2j3pi>DXI1XZ4L9<91 z&hZ?&P{*5EC+@LE^F9|!bD<+1$%{@={o&X?{n1?53e>g&5stVsf?NC4*L-iM&T!r- z-b=9MAj$;ZIe>6#T!n#&1~u0U1EiU7XOR+!T(}@@Hu)8@c%Oh_}rH^&;O6j^SLi=p3i-0 z^ZftVJpVs7&;O6jN0#Y)u-ZP+LFtjH*vz270Go(Mp7!SDiGx#qcS0kR55)PPAJ*lS z+kJ;_*LdxuF6N@mqRsKoVlKu!#)43+c`QYV@#%WGmrRQQ z1Nt(YiP*#x&m@L(v~(Om$jQRVnUJ%&vlAi37N95xgd2dM0f_Mnl%aJ@E)2zUZ_fEu zOHK~IrGQe-&(UuH9w|h_{6=7^keuOOXWvzz2^ysxrHX0cuBU&B)lPLmoi&4Is(`_8 z=FprXYiLc;3jPXJNI)|T@Y6|%K&}diGJr9DFT-U5^-Q3P8P7!ISUxWVuxly!BjIgp z{391+=RH8PhXf6V`v5dQ5Dw~;RyxVrU6G~lhu-_4A9iL}?7A%b4$K^cK5>rL9Mv12 z`;J=va`Ur6sQoRdc?$}0u3}xWZn^OrnL83RLER>(j|Ga#856daPIX6Jc@M(hgQd9B z4EGU)egA^bUn z68;=Q34acunEOQ}l>U*kUECzdw!$+CJ+j;3bM!4otfu9BEEOH zGJo3!()Osrjl+f)_HQ-xM1(3O6!Cq~_|p;<8?G`GCgj3YJRP;>MkfV3mU^SR zBiTq>eAmL;MP!e<-V$dksE65-ClQs>o9LKG&|m>N{qSMk3#4 zscor)T_PegOG`{ESNyEC?4)2cU&>WMgDSN4|F<3><<3Kc^EB5$gBqHzLW8S>-Hd+X z{{RM$Mh2P=_keV`7m(?Z0S|%M})^ui*8TVpgC&QI6RkUYfC#?OI}zrH_9k&Qnk3v>un z5Lw`ez)^%!6{&*=rMadJAyfj`5+K6~6~mPPrxM_T`E}#M!@QC8P~>ubd~$UDlQOr5;P z1e#D3FNkM|&J)xVG%?i{jGZL7WDudjaFQ5F0(pQ!T_L7uo?I3KOlg!&1JZ&V? zkqPwz{azr(PY@$VE6#-m_*IL>JT%a%dm5!&T8#4!B%kg*%{!J*>crGgLTOh(+I5wu zSDLfY4q$@KSIN#hfGcM1+f60Oiw8y{yRzh4YGTbd+??h=WB-D(sUp!wxT+ zp^Vd1!I9y*0PG^uB;7r~!Ng|hETo)9*tZdtW&f>k;sHWA-sfU14wc&u4qoi|x63?q zhqcgJRmlumeQex`m30f0p^++EqR>c{7#Xe&2-<)QM;QHFFN5kMl0+dIeSL4$x(1P_ z_gFIC-f`Z^gnVVbl&|!Y_)!xk-Xz|Uv`jTiRb|o)m+F-oK*_>VDf=o0vSQ$XEr#K? z0JsI{5-tHICBTvJ4j|tFoCuc!St+@HP4u_tzd4hWj8v2X`egts{Ms!~?!42?mZcy; zA(R%v{#N3Ztqa<^7MtmU^$Ay@4@)@;2S(8rD#%*|46uDsCMkJ>lMpU zC+`EnePDn`>q(uO_^)H9(vaXbG`tPXu&rPRr6%RJ0rhP_g1aK+b^`UCG?xMOGMbAZ zEQ0!24aWc0{jsg(uj}BoJXTwcVyVntrmAA2m5tO~RpCapUb&qup4N!$>|6-h%k3$d zy`4QJbC5Y$la>y)4hljFH^p#5^*~S$bg?3?rurL(WWLWvfg;2(M72t1xM((7hV{*G ziH3=kEDIQBkrlm<<6kd#_Xg&CrHk%K?)w~vCC$uLrsZSO#$%m?XpA~DFHYwNOjM=}t5E7( zph}&yT6}#MJG!nz{-gnTHUMv|QYm*6c-;h}FyDIkZh~c5`8u>I6KWW$@?R*cHVmUE zhK*5qI);m7V<}29OrlyQ8zxhf4GgmZ#W}z*hj7zUjWgp5^YcYgWO0IvzzFwBiY!i0 zL-S=IxJ>gkAh<^Jbs)G-^9>-lLGxW8xJ&arAh<_!GY~Y>{1^xx)7%0CEi^v^f@d^8 z2ZHA`zXXDpG`|9ZSA-9q7&F^RU$+3QY`TF(HyDE5Ju2ovi%d8K2Y6I|h8Fp72=>9K z_zNwz!Xc!FP!281;SijSi>8!B{&1$g01an1@azVvx~y_jNaH)QkD+{#XD0NgU*wq& z{plBZZiW8jMWs-q6iNt}L(g*Pk6*0ZRMI#CC784Clslep-@XB5IGe%n&A=ZUud>oU zG6PP;UyPGdWYnKGg^5w8!xiZ;Pi50Z;T{upixY~JZfilBs?dG+LyhO$O1+I=X{v~) z1e(dl&{(xlG5&Xl9BXUaI5(H~lgu{hq4dnw+YS44L_ywDDO&3NJ(ZjLSo%umfljE(L;8!newrh2!%VZvK>}vG0Kewqj+P#(pIHdY~+7TFu2Q2HjKF%y*<1K@yS#8KN}S4}G*tRPj>c!zfZ_QE4uQQm7VG*_LvNHh*I+AS_CQD%YC5GwDBHi=ec z-6#u`CQ`C2Ak6|Q9$$1gz1Z!a64du^_B`-bDf-kfSr`?VW96|_-8fm6Ov!S9EQi#M&x|{_Ln@t7hO|FQ zZxme?8|>$azX#RbEP`z?9laZ!;?c2UPaiRtGA6|z|BPrtifSc?C^#hq%dGw?bIJN+ zd-+#t>o+F#&IXckd(qOa39?O4j#pqfCZg5)=Tl4AX&;dOv!?uV<32Q`Q8rO3PrSg~ zJ!gpCfc@IYcKIPftUi>X@k1#JXlE6gH9<$LpYB!5 z_iQg<52;JhDGi;UpebImO1V=|=M>GSq3|>`A@Xxj=Nv7shC0^jZE-krJReQi>l5m<)#`D-n7f;m6<>d?SW&{!QNE|75aG+YFC{;XTxyA)rM22dNu>K%|H+5eacfVY85~a zCzA}yw$&~GJ)DdyS65@ZK{&QPN~Z_rS$@jUN9Q> zEGUmFc6-uv6-9db$SOKVN*!5cP#p%MQVjmwCBL-%wv2z{8uH|+8mUx3NYhAD(H{Zr z8(-hFyM?Aof>wgcT^&33+tSj%!fv5^eW5RPs-YObVseTpb4~fwsMZ6wke%d0m<#oZ zW_Cch1L_kqH~`fSKtpT>4BC%k55WF}4?=hl>SGSB9aii$yMt1&YT#21#^JA3?);9t1SSx!1w(5|$>e5i^x!kgmNg=WnGQ|Tp#tkxS%xvm zAbiqq{gQ(to;Au*xbc=u$Ie5HiXAo=$riL2z2CkXjKJ>`qvD564K2R}94`SMBEJl5 zE(2G>wZO5KoIn+G*MRvo;7H{4z_uQ^5WWuN*J*hJux}tIRK?s4AiqI&qQ$6W8JVLh5n zo}x8fH87|qt93DQiIPhs`x2woyrh=!ji(nDobtN$4BeOo4RqxSP zXQES3K(!=0y#JB;%uD_MMuJ{o&hNc#yEfneK z>g%y`)`T6vDp()ZnzdssSR2-o9mKk@Ls=_!Fzd;>vL5U(b~tO#IYSKO=vMF)-u(FYBKv&;S4c diff --git a/.cache/clangd/index/velocity_controller.hpp.3E0346F5513060F5.idx b/.cache/clangd/index/velocity_controller.hpp.3E0346F5513060F5.idx deleted file mode 100644 index fc7431d5ec85756dbe30346c651947b8e46651f4..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 4532 zcmY*a30PBC7Jj*c`tb1*l0ZV(URV;Aut;PrVHcDgL@_{sSg|rLltl$crlL|7trRyz z5OAqfMX89pIF@29Dk`G1E@Lg#qDa-c)MdKNeXnx{hVSF#e*b^YednHg&-s(4M?|!m z5JcGA=|zjO3-tyBL5T5xL1BK*H;5pfP9%t;hR#{h>u-BdHxgH@%rU+s*3EtIu+zJ` z>GKkrT|&0x)K{oZ2(y(M?d#6jPPgCg;B7No zx8EpW_JymqJ3QaSn%vrD@w8ZO^XS;_mYj;^fzc5S- z^tb=bd|m&vv>YATa_U^DW3;30N}ku{z7FGYw^HY4ckKT9eDI*)%U-c(8{N^@inWi=}b9nRi48OiZZx23xzV6BQ{x)rmc{{5cj;|LM-R<6T{>#9- zvhV!f{`|71=}FSKLbsKcMGB9_Z2@~Crgioh&u+B;rS{E~tmZ!-W!=l!SsxVAv9IaU z>(H*e2WwW4tG-njZ`^qP!P0A4hRgT&nxD&%m5(@VExq7W+4*U^Q-k`WT%CE#!=Ei~ zMtr5#6}P!LUo7_66VrHqSBbH`E7d+DBkxe1U-#lSQ5yC0kbW0Yoqf%aPya75hFvRO zMY=MnM`|{|kaa&XwEnc=L1pB3%ANYU{reVdi*f5ggk#NRJ6BnwW&@;eH`(w z&?8q^8Qbxb{x|JCkG`rJZS_v0RZf$)C3TITUL{!DtT9{oyYGbJ*4qv$#r#LMWK>Lp zXJ6mv;a{&xFMs#&$#LDT$uTD$wY;A+Qst35l=Tsscy-QjLGhwF<%R??LA%RqZLvK` zqX{B{2@(Zaa)!Xqoc&X!y`7Y}A#W0mNK8jaBg{DibUfv}=8L;MLIF~X2ze9YHz9K- zD?UpL4vGU`SqZDIhy=(BJ8VMyO~{hTjL%F52VaLluLJ5DF^wen0|65QB%A?yJGpd2 z=iFDOH1fl=x2?AuXMkR=%$-IuOh-$jRh$7@6Md;D zGnMB-BOgqAPV^LW2I!P2r%uc@UZJLuA*K_k1PNz=o;vxB=YzFnY5|&pGk?X?zT#yw zx;R}bn>!!pZjqX|=a;HnKNGnbqzV=E`XmD{*snE2tIVHu4)aJ+hwTFe=UZy#)kI}`rwQvp)p_^E#l(;?e0Zo7lSn4=AvcOiB9r*gVHA^0 zCi9`!DP|@)lMfAYp)sMqeva>W9>t`PDPpJ{s0Gi6Qwb;vWQk!`A^%>Zvm6vM1m01PsL6J+H9V2}sw&1wU{c1q>D zyT;o&QYa|JC@G~BCL79-EfqCNkM%NHIF4P6IE6VltE&8mJM)WGXW?(3KRErOeVm z>r#wfsn@`u=Iff$*LPm#whPk5j@d3KBX-PoL7A~*whPLN9kX4KJ{GpC9N6o6_R)?l z<0!^TZ50S-!1Z-edP99F#@ozW4pRWS3!jr!K{zLXV^ZX>`kGMnp*TpKNPlo&E9>1u4v3<6=W2RxvV9b z%g0<+e%7)vmsOxIf&)Pk$mlPb{&~N6&&!$D@yUG* zd`zH?p#t~=tSrL^dQ;u6P4lR`fxW{G!cAb#B=u+-g67>Hz_3-Y)pU4;Lm##d9zA1z z7sJ;6)?x7QlD0f895A)Gk71F!C;;|;xjUq*&a&(!s*o kN=Yb2eXK{9R-;S1(WS-c(s6XD99`OvE}ci0wxi4c0C?W7-2eap diff --git a/.gitignore b/.gitignore index 68e1f5f0c..04ae9a3fc 100644 --- a/.gitignore +++ b/.gitignore @@ -55,5 +55,5 @@ qtcreator-* # Emacs .#* - .cache +.cache # End of https://www.gitignore.io/api/ros \ No newline at end of file From de375d3715af01d2ce005963d6d8e5f38ddc3fa8 Mon Sep 17 00:00:00 2001 From: Henrik Date: Sun, 29 Mar 2026 13:45:05 +0200 Subject: [PATCH 114/128] removed the changes in los_guidance --- guidance/los_guidance/CMakeLists.txt | 57 +-- guidance/los_guidance/README.md | 365 +----------------- .../los_guidance/config/guidance_params.yaml | 50 +-- .../include/los_guidance/los_guidance.hpp | 75 ++++ .../include/los_guidance/los_guidance_ros.hpp | 135 +++---- .../launch/los_guidance.launch.py | 63 ++- guidance/los_guidance/package.xml | 7 +- guidance/los_guidance/src/los_guidance.cpp | 59 +++ .../los_guidance/src/los_guidance_node.cpp | 4 +- .../los_guidance/src/los_guidance_ros.cpp | 332 ++++------------ 10 files changed, 331 insertions(+), 816 deletions(-) create mode 100644 guidance/los_guidance/include/los_guidance/los_guidance.hpp create mode 100644 guidance/los_guidance/src/los_guidance.cpp diff --git a/guidance/los_guidance/CMakeLists.txt b/guidance/los_guidance/CMakeLists.txt index 00c00a391..dd243a31e 100644 --- a/guidance/los_guidance/CMakeLists.txt +++ b/guidance/los_guidance/CMakeLists.txt @@ -13,62 +13,34 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(vortex_msgs REQUIRED) -find_package(vortex_utils_ros REQUIRED) find_package(geometry_msgs REQUIRED) -find_package(nav_msgs REQUIRED) find_package(Eigen3 REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) find_package(spdlog REQUIRED) find_package(fmt REQUIRED) -find_package(yaml-cpp REQUIRED) - include_directories(include) -set(LIB_NAME los_guidance_lib) -add_library(${LIB_NAME} SHARED - src/lib/proportional_los.cpp - src/lib/integral_los.cpp - src/lib/adaptive_los.cpp +add_executable(los_guidance_node + src/los_guidance_node.cpp src/los_guidance_ros.cpp - src/lib/vector_field_los.cpp + src/los_guidance.cpp ) -ament_target_dependencies(${LIB_NAME} +ament_target_dependencies(los_guidance_node rclcpp rclcpp_action geometry_msgs - nav_msgs vortex_msgs - vortex_utils_ros Eigen3 + tf2 + tf2_geometry_msgs spdlog fmt ) -add_executable(los_guidance_node - src/los_guidance_node.cpp -) - -ament_target_dependencies(los_guidance_node - rclcpp - rclcpp_action - geometry_msgs - vortex_msgs - vortex_utils_ros -) - -target_link_libraries(los_guidance_node - ${LIB_NAME} - yaml-cpp -) - -install(TARGETS - ${LIB_NAME} - EXPORT export_${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) +target_link_libraries(los_guidance_node fmt::fmt) install(TARGETS los_guidance_node @@ -77,7 +49,7 @@ install(TARGETS install( DIRECTORY include/ - DESTINATION include/ + DESTINATION include ) install(DIRECTORY @@ -86,13 +58,4 @@ install(DIRECTORY DESTINATION share/${PROJECT_NAME}/ ) -install(PROGRAMS - scripts/test_scenarios.py - DESTINATION share/${PROJECT_NAME}/scripts -) - -if(BUILD_TESTING) - add_subdirectory(test) -endif() - ament_package() diff --git a/guidance/los_guidance/README.md b/guidance/los_guidance/README.md index af54fdb90..33c616c59 100644 --- a/guidance/los_guidance/README.md +++ b/guidance/los_guidance/README.md @@ -1,181 +1,22 @@ -# 3D LOS Guidance Library +## ALSO Guidance Law for 3D Path Following -This package implements several **Line-of-Sight (LOS) guidance algorithms** for **3D path following**. - -The guidance system computes the **desired yaw** $\psi_d$ and **desired pitch** $\theta_d$ that allow a vehicle to follow a path between waypoints. - -The node receives - -- vehicle pose -- waypoint information - -and computes guidance references based on the selected LOS algorithm. The resulting guidance commands consist of - -- desired yaw -- desired pitch -- desired surge velocity. - -The vehicle surge speed is kept **constant during path following** and is defined in the configuration file using the parameter `u_desired`. - ---- - -# Implemented LOS Methods - -The library supports four LOS guidance algorithms. - -| Mode | Method | -|-----|------| -| 0 | Proportional LOS | -| 1 | Integral LOS | -| 2 | Adaptive LOS | -| 3 | Vector Field LOS | - -The guidance method can be **changed during runtime** using a ROS service. - ---- - - -# Sending a LOS Goal - -A waypoint can be sent to the guidance node using the action interface: - -``` -ros2 action send_goal /orca/los_guidance \ -vortex_msgs/action/LOSGuidance \ -"{goal: {header: {frame_id: world_ned}, point: {x: 0.0, y: 0.0, z: 0.0}}}" -``` - -This command instructs the guidance node to start following a path toward the waypoint. - ---- - -# Switching LOS Method - -The active LOS guidance method can be changed during runtime. - -``` -ros2 service call /orca/set_los_mode \ -vortex_msgs/srv/SetLosMode "{mode: X}" -``` - -Where - -| X | Method | -|---|---| -| 0 | Proportional LOS | -| 1 | Integral LOS | -| 2 | Adaptive LOS | -| 3 | Vector Field LOS | - -Example: - -``` -ros2 service call /orca/set_los_mode vortex_msgs/srv/SetLosMode "{mode: 2}" -``` - -This switches the guidance system to **Adaptive LOS**. - ---- -## Path Geometry - -The path between two waypoints defines the **path direction angles** used by the guidance law. - -```math -\pi_h = -\text{atan2}(y_{i+1}^n - y_i^n, x_{i+1}^n - x_i^n) -``` - -```math -\pi_v = -\text{atan2} -\left( --(z_{i+1}^n - z_i^n), -\sqrt{(x_{i+1}^n - x_i^n)^2 + -(y_{i+1}^n - y_i^n)^2} -\right) -``` - -where - -- $\pi_h$ is the **horizontal path angle (azimuth angle)** -- $\pi_v$ is the **vertical path angle (elevation angle)** - -These angles define the **direction of the current path segment** between two waypoints. - -Additionally - -- $P_i^n = (x_i^n, y_i^n, z_i^n)$ is the previous waypoint in the north-east-down (NED) frame -- $P_{i+1}^n = (x_{i+1}^n, y_{i+1}^n, z_{i+1}^n)$ is the next waypoint. - ---- - -## Path Frame Errors - -The along-, cross- and vertical-track errors in the path-tangential frame are found by - -```math -\begin{bmatrix} -x_e^p \\ -y_e^p \\ -z_e^p -\end{bmatrix} -= -\mathbf{R}_{y,\pi_v}^\top -\mathbf{R}_{z,\pi_h}^\top -\left( -\begin{bmatrix} -x^n \\ -y^n \\ -z^n -\end{bmatrix} -- -\begin{bmatrix} -x_i^n \\ -y_i^n \\ -z_i^n -\end{bmatrix} -\right) -``` - -where - -- $x_e^p$ is the along-track error -- $y_e^p$ is the cross-track error -- $z_e^p$ is the vertical-track error - -and - -- $P^n = (x^n, y^n, z^n)$ is the current position of the vehicle. - ---- - -## Adaptive LOS (ALSO) - -Adaptive LOS estimates **crab angles caused by disturbances** such as ocean currents or wind. +The guidance law gives calculates the desired heading angle $\psi_d$ and desired pitch angle $\theta_d$. The crab angles $\beta_c$ and $\alpha_c$ are estimated adaptively. The guidance law looks like ```math \psi_d = \pi_h - \hat{\beta}_c - \tan^{-1}\left(\frac{y_e^p}{\Delta_h}\right) + ``` ```math -\dot{\hat{\beta}}_c = -\gamma_h -\frac{\Delta_h}{\sqrt{\Delta_h^2 + (y_e^p)^2}} -y_e^p +\dot{\hat{\beta}}_c = \gamma_h \frac{\Delta_h}{\sqrt{\Delta_h^2 + (y_e^p)^2}} y_e^p ``` ```math -\theta_d = -\pi_v + -\hat{\alpha}_c + -\tan^{-1}\left(\frac{z_e^p}{\Delta_v}\right) +\theta_d = \pi_v + \hat{\alpha}_c + \tan^{-1}\left(\frac{z_e^p}{\Delta_v}\right) ``` ```math -\dot{\hat{\alpha}}_c = -\gamma_v -\frac{\Delta_v}{\sqrt{\Delta_v^2 + (z_e^p)^2}} -z_e^p +\dot{\hat{\alpha}}_c = \gamma_v \frac{\Delta_v}{\sqrt{\Delta_v^2 + (z_e^p)^2}} z_e^p ``` where @@ -186,193 +27,27 @@ where - $y_e^p$ is the cross-track error - $z_e^p$ is the vertical-track error -The terms - -- $\hat{\beta}_c$ is the **estimated horizontal crab angle** -- $\hat{\alpha}_c$ is the **estimated vertical crab angle** - -These angles represent the **estimated disturbance-induced deviation** between the vehicle heading and the actual direction of motion. -They allow the guidance system to **compensate for disturbances such as currents or wind**. - -Adaptive LOS is generally the **most robust method** and works well for - -- curved trajectories -- long paths -- environments with disturbances. - ---- - -## Proportional LOS (PLOS) - -Proportional LOS is the simplest LOS guidance law. +The azimuth angle $\pi_v$ and the elevation angle $\pi_h$ can be found by ```math -\psi_d = -\pi_h - -\tan^{-1}\left(\frac{y_e^p}{\Delta_h}\right) +\pi_h = \text{atan2}(y_{i+1}^n - y_i^n, x_{i+1}^n, - x_i^n) ``` - ```math -\theta_d = -\pi_v + -\tan^{-1}\left(\frac{z_e^p}{\Delta_v}\right) +\pi_v = \text{atan2}(-(z_{i+1}^n - z_i^n), \sqrt{(x_{i+1}^n - x_i^n)^2 + (y_{i+1}^n - y_i^n)^2}) ``` -### Parameters - -- $\Delta_h$ — horizontal lookahead distance -- $\Delta_v$ — vertical lookahead distance - -The lookahead distances determine **how aggressively the vehicle corrects path errors**. - -- small values → aggressive corrections -- large values → smoother but slower convergence - -### Use case - -PLOS works best for - -- simple waypoint following -- environments with minimal disturbances. - -However, it may suffer from **steady-state tracking error** when disturbances are present. - ---- - -## Integral LOS (ILOS) - -Integral LOS adds integral action to remove steady-state error. - -```math -u_h = -k_{p,h}y_e^p + -k_{i,h}\int y_e^p dt -``` - -```math -u_v = -k_{p,v}z_e^p + -k_{i,v}\int z_e^p dt -``` - -```math -\psi_d = -\pi_h - -\tan^{-1}(u_h) -``` - -```math -\theta_d = -\pi_v + -\tan^{-1}(u_v) -``` - -### Parameters - -- $k_{p,h}$ — horizontal proportional gain -- $k_{p,v}$ — vertical proportional gain -- $k_{i,h}$ — horizontal integral gain -- $k_{i,v}$ — vertical integral gain +where $P_i^n = (x_i^n, y_i^n, z_i^n)$ is the previous waypoint in the north-east-down frame and $P_{i+1}^n = (x_{i+1}^n, y_{i+1}^n, z_{i+1}^n)$ is the next waypoint in north-east-down frame. -The integral term allows the controller to **eliminate steady-state cross-track errors** caused by constant disturbances. - -### Use case - -ILOS works well when there are **persistent disturbances**, such as - -- steady ocean currents -- constant wind disturbances. - ---- - -## Vector Field LOS (VF-LOS) - -Vector Field LOS generates a **bounded approach angle** toward the path. - -```math -\psi_d = \pi_h - \chi_h -``` +The along-, cross- and vertical-track errors in the path-tangential frame are found by ```math -\chi_h = -\psi_{max} -\frac{2}{\pi} -\tan^{-1}(k_p y_e^p) -``` - -### Parameters - -- $\psi_{max}$ — maximum allowed approach angle -- $k_p$ — proportional gain controlling path convergence - -The bounded approach angle prevents excessively aggressive heading changes. - -### Use case - -VF-LOS works best for - -- long straight path following -- corridor tracking -- inspection missions along pipelines or cables. - -However it performs worse when the path contains **sharp turns or rapidly changing path segments**. - ---- - -## ROS Interfaces - -| Interface | Name | Type | Message-Type | -|----------|------|------|---------| -| Action Server | `/orca/los_guidance` | Goal input | `vortex_msgs/action/LOSGuidance` | -| Subscriber | `/orca/waypoint` | Waypoint input | `geometry_msgs/PointStamped` | -| Subscriber | `/orca/pose` | Vehicle pose | `geometry_msgs/PoseWithCovarianceStamped` | -| Subscriber | `/orca/odom` | Vehicle velocity | `nav_msgs/Odometry` | -| Publisher | `/orca/guidance/los` | Guidance reference (yaw, pitch, surge) | `vortex_msgs/LOSGuidance` | -| Publisher | `/los_debug` | LOS debug output | `vortex_msgs/LOSGuidance` | -| Publisher | `/state_debug` | Vehicle state debug | `vortex_msgs/LOSGuidance` | - ---- - -## Guidance Node Architecture -The LOS guidance node computes reference commands for the vehicle based on -the current vehicle state and the active waypoint. - -The process works as follows - -1. The node receives the **vehicle pose**. -2. The current **path segment** is defined between two waypoints. -3. The **path geometry** is computed to determine the path direction. -4. The **cross-track and vertical-track errors** are calculated in the path frame. -5. The selected **LOS guidance algorithm** computes desired yaw and pitch. -6. The node publishes the resulting **guidance reference**. - -### Data Flow - -``` -Vehicle Pose + Waypoint - │ - ▼ - Path Geometry - (π_h , π_v angles) - │ - ▼ - Path Frame Errors - (x_e^p , y_e^p , z_e^p) - │ - ▼ - LOS Algorithm - (PLOS / ILOS / ALSO / VF-LOS) - │ - ▼ - Guidance Output - (ψ_d , θ_d , u_desired) +\begin{bmatrix} +x_e^p \\ y_e^p \\ z_e^p +\end{bmatrix} = \mathbf{R}_{y, \pi_v}^\top \mathbf{R}_{z, \pi_h}^\top \left( \begin{bmatrix} +x^n \\ y^n \\ z^n +\end{bmatrix} - \begin{bmatrix} +x_i^n \\ y_i^n \\ z_i^n +\end{bmatrix} +\right) ``` - -The resulting guidance command is published as a `vortex_msgs/LOSGuidance` -message containing - -- desired yaw -- desired pitch -- desired surge velocity. - ---- +where $P^n = (x^n, y^n, z^n)$ is the current position of the drone. diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 89f59b119..0a04dec0a 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -1,42 +1,8 @@ -# Adaptive LOS Parameters -adaptive_los: - lookahead_distance_h: 0.9 - lookahead_distance_v: 1.4 - gamma_h: 0.03 - gamma_v: 0.02 - -# Proportional LOS Parameters -prop_los: - lookahead_distance_h: 0.74 - lookahead_distance_v: 0.8 - -# Integral LOS Parameters -integer_los: - k_p_h: 0.5 - k_p_v: 0.5 - k_i_h: 0.1 - k_i_v: 0.1 - -# Vector Field LOS Parameters -vector_field_los: - max_approach_angle_h: 1.0 - max_approach_angle_v: 1.0 - k_p_h: 1.5 - k_p_v: 0.9 - -# Common Guidance Parameters -common: - active_los_method: 2 # 0: Adaptive LOS, 1: Proportional LOS, 2: Integral LOS, 3: Vector Field LOS - u_desired: 0.3 - goal_reached_tol: 0.20 - max_pitch_angle: 0.96 # 55 degrees - slow_approach: true - slow_down_distance: 1.5 - u_slow_min: 0.1 - surge_rate_limit: 0.1 - surge_initialization: false - -# Debug Settings -debug: - enable_debug: true - debug_topic_name: "/los_guidance_debug" +/**: + ros__parameters: + lookahead_distance_h: 1.5 + lookahead_distance_v: 0.6 + gamma_h: 0.05 + gamma_v: 0.1 + time_step: 0.01 + u_desired: 0.3 diff --git a/guidance/los_guidance/include/los_guidance/los_guidance.hpp b/guidance/los_guidance/include/los_guidance/los_guidance.hpp new file mode 100644 index 000000000..d73830ba8 --- /dev/null +++ b/guidance/los_guidance/include/los_guidance/los_guidance.hpp @@ -0,0 +1,75 @@ +#ifndef LOS_GUIDANCE_HPP +#define LOS_GUIDANCE_HPP + +#include + +namespace LOS { +struct Point { + double x; + double y; + double z; + + Point operator-(const Point& other) const { + return Point{x - other.x, y - other.y, z - other.z}; + } + + Eigen::Vector3d as_vector() const { return Eigen::Vector3d(x, y, z); } +}; + +struct CrossTrackError { + double x_e; + double y_e; + double z_e; + + inline static CrossTrackError from_vector(const Eigen::Vector3d& vector) { + return CrossTrackError{vector.x(), vector.y(), vector.z()}; + } +}; + +struct Params { + double lookahead_distance_h; + double lookahead_distance_v; + double gamma_h; + double gamma_v; + double time_step; +}; +}; // namespace LOS + +/** + * @brief Adaptive Line-of-Sight (LOS) guidance algorithm based on slide 113 + * in "Fossen 2024 Lecture on 2D and 3D path-following control". + */ +class AdaptiveLOSGuidance { + public: + AdaptiveLOSGuidance(const LOS::Params& params); + ~AdaptiveLOSGuidance() = default; + + void update_angles(const LOS::Point& prev_point, + const LOS::Point& next_point); + + LOS::CrossTrackError calculate_crosstrack_error( + const LOS::Point& prev_point, + const LOS::Point& current_position); + + double calculate_psi_d(const double& y_e); + + double calculate_theta_d(const double& z_e); + + void update_adaptive_estimates( + const LOS::CrossTrackError& crosstrack_error); + + private: + double time_step_; + double lookahead_distance_h_; + double lookahead_distance_v_; + double gamma_h_; + double gamma_v_; + Eigen::Matrix3d rotation_y_; + Eigen::Matrix3d rotation_z_; + double pi_h_; + double pi_v_; + double beta_c_hat_ = 0.0; + double alpha_c_hat_ = 0.0; +}; + +#endif // LOS_GUIDANCE_HPP diff --git a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp index a996c42b9..674996a08 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -1,130 +1,115 @@ -#ifndef LOS_GUIDANCE__LOS_GUIDANCE_ROS_HPP_ -#define LOS_GUIDANCE__LOS_GUIDANCE_ROS_HPP_ +#ifndef REFERENCE_FILTER_ROS_HPP +#define REFERENCE_FILTER_ROS_HPP -#include +#include +#include #include #include #include -#include +#include #include #include -#include +#include #include -#include #include #include -#include +#include "los_guidance.hpp" -#include -#include - -#include "los_guidance/lib/adaptive_los.hpp" -#include "los_guidance/lib/integral_los.hpp" -#include "los_guidance/lib/proportional_los.hpp" -#include "los_guidance/lib/types.hpp" -#include "los_guidance/lib/vector_field_los.hpp" - -namespace vortex::guidance::los { - -// LOS Guidance ROS Node -class LosGuidanceNode : public rclcpp::Node { +class LOSGuidanceNode : public rclcpp::Node { public: - // Constructor - LosGuidanceNode(); + explicit LOSGuidanceNode(); private: - // Setup Functions + // @brief Set the subscribers and publishers void set_subscribers_and_publisher(); + + // @brief Set the action server void set_action_server(); - void set_service_server(); - // Configuration Functions - void set_adaptive_los_guidance(YAML::Node config); - void set_proportional_los_guidance(YAML::Node config); - void set_integral_los_guidance(YAML::Node config); - void set_vector_field_guidance(YAML::Node config); - YAML::Node get_los_config(std::string yaml_file_path); - void parse_common_config(YAML::Node common_config); + // @brief Set the adaptive LOS guidance parameters + void set_adaptive_los_guidance(); - // Callback Functions + // @brief Callback for the waypoint topic + // @param msg The reference message void waypoint_callback( const geometry_msgs::msg::PointStamped::SharedPtr msg); + + // @brief Callback for the pose topic + // @param msg The pose message void pose_callback( const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); - void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg); - // Action Server Functions + // @brief Handle the goal request + // @param uuid The goal UUID + // @param goal The goal message + // @return The goal response rclcpp_action::GoalResponse handle_goal( const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal); + + // @brief Handle the cancel request + // @param goal_handle The goal handle + // @return The cancel response rclcpp_action::CancelResponse handle_cancel( const std::shared_ptr< rclcpp_action::ServerGoalHandle> goal_handle); + + // @brief Handle the accepted request + // @param goal_handle The goal handle void handle_accepted(const std::shared_ptr> goal_handle); + + // @brief Execute the goal + // @param goal_handle The goal handle void execute(const std::shared_ptr> goal_handle); - // Service Functions - void set_los_mode( - const std::shared_ptr request, - std::shared_ptr response); - - // Publish Functions - void publish_state_debug( - const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr - current_pose); - vortex_msgs::msg::LOSGuidance fill_los_reference(types::Outputs output, - types::Inputs inputs); + // @brief Fill the lost waypoints + // @param goal The goal message + void fill_los_waypoints( + const geometry_msgs::msg::PointStamped& los_waypoint); - // State Flags - bool has_active_segment_{false}; + vortex_msgs::msg::LOSGuidance fill_los_reference(); - // ROS Interfaces rclcpp_action::Server::SharedPtr action_server_; - rclcpp::Service::SharedPtr los_mode_service_; + rclcpp::Publisher::SharedPtr reference_pub_; - rclcpp::Publisher::SharedPtr los_debug_pub_; - rclcpp::Publisher::SharedPtr - state_debug_pub_; + rclcpp::Subscription::SharedPtr waypoint_sub_; + rclcpp::Subscription< geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_; - rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::TimerBase::SharedPtr reference_pub_timer_; - rclcpp::CallbackGroup::SharedPtr cb_group_; - // Timing and Synchronization std::chrono::milliseconds time_step_; + std::mutex mutex_; - // Action State rclcpp_action::GoalUUID preempted_goal_id_; + std::shared_ptr< rclcpp_action::ServerGoalHandle> goal_handle_; - // Guidance State - types::Inputs path_inputs_{}; - double u_desired_{}; - double goal_reached_tol_{}; - double max_pitch_angle_{}; - double slow_down_distance_{}; - types::ActiveLosMethod method_{}; - - // Guidance Modules - std::unique_ptr adaptive_los_{}; - std::unique_ptr integral_los_{}; - std::unique_ptr proportional_los_{}; - std::unique_ptr vector_field_los_{}; - - // Debug Data - nav_msgs::msg::Odometry::SharedPtr debug_current_odom_{}; -}; + rclcpp::CallbackGroup::SharedPtr cb_group_; + + LOS::Point eta_; -} // namespace vortex::guidance::los + LOS::Point last_point_; + + LOS::Point next_point_; + + std::unique_ptr adaptive_los_guidance_; + + double yaw_d_; + + double pitch_d_; + + double u_desired_; +}; -#endif // LOS_GUIDANCE__LOS_GUIDANCE_ROS_HPP_ +#endif diff --git a/guidance/los_guidance/launch/los_guidance.launch.py b/guidance/los_guidance/launch/los_guidance.launch.py index d616e0853..03366e897 100644 --- a/guidance/los_guidance/launch/los_guidance.launch.py +++ b/guidance/los_guidance/launch/los_guidance.launch.py @@ -1,51 +1,32 @@ -import os +from os import path from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import OpaqueFunction from launch_ros.actions import Node -from auv_setup.launch_arg_common import ( - declare_drone_and_namespace_args, - resolve_drone_and_namespace, +adapt_params = path.join( + get_package_share_directory("los_guidance"), + "config", + "guidance_params.yaml", +) +orca_params = path.join( + get_package_share_directory("auv_setup"), + "config", + "robots", + "orca.yaml", ) - - -def launch_setup(context, *args, **kwargs): - drone, namespace = resolve_drone_and_namespace(context) - - guidance_config = os.path.join( - get_package_share_directory("los_guidance"), - "config", - "guidance_params.yaml", - ) - - drone_params = os.path.join( - get_package_share_directory("auv_setup"), - "config", - "robots", - f"{drone}.yaml", - ) - - return [ - Node( - package="los_guidance", - executable="los_guidance_node", - name="los_guidance_node", - namespace=namespace, - parameters=[ - drone_params, - { - "los_config_file": guidance_config, - "time_step": 0.1, - }, - ], - output="screen", - ) - ] def generate_launch_description(): - return LaunchDescription( - declare_drone_and_namespace_args() + [OpaqueFunction(function=launch_setup)] + los_guidance_node = Node( + package="los_guidance", + executable="los_guidance_node", + name="los_guidance_node", + namespace="orca", + parameters=[ + orca_params, + adapt_params, + ], + output="screen", ) + return LaunchDescription([los_guidance_node]) diff --git a/guidance/los_guidance/package.xml b/guidance/los_guidance/package.xml index 5f696a725..a3d2a9a8a 100644 --- a/guidance/los_guidance/package.xml +++ b/guidance/los_guidance/package.xml @@ -3,7 +3,7 @@ los_guidance 0.0.0 - LOS Guidance module for guidance of AUV + LOS Guidance modual for guidance of AUV talhanc MIT @@ -14,9 +14,10 @@ geometry_msgs vortex_msgs nav_msgs - vortex_utils_ros eigen - yaml-cpp + tf2 + tf2_geometry_msgs + ament_cmake diff --git a/guidance/los_guidance/src/los_guidance.cpp b/guidance/los_guidance/src/los_guidance.cpp new file mode 100644 index 000000000..49a521ff2 --- /dev/null +++ b/guidance/los_guidance/src/los_guidance.cpp @@ -0,0 +1,59 @@ +#include + +AdaptiveLOSGuidance::AdaptiveLOSGuidance(const LOS::Params& params) + : time_step_(params.time_step), + lookahead_distance_h_(params.lookahead_distance_h), + lookahead_distance_v_(params.lookahead_distance_v), + gamma_h_(params.gamma_h), + gamma_v_(params.gamma_v) {} + +void AdaptiveLOSGuidance::update_angles(const LOS::Point& prev_point, + const LOS::Point& next_point) { + const LOS::Point difference = next_point - prev_point; + + pi_h_ = atan2(difference.y, difference.x); + pi_v_ = atan2(-difference.z, sqrt(difference.x * difference.x + + difference.y * difference.y)); + + rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); + rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); +} + +LOS::CrossTrackError AdaptiveLOSGuidance::calculate_crosstrack_error( + const LOS::Point& prev_point, + const LOS::Point& current_position) { + const LOS::Point difference = current_position - prev_point; + const Eigen::Vector3d difference_vector = difference.as_vector(); + + const Eigen::Vector3d cross_track_error = + rotation_y_.transpose() * rotation_z_.transpose() * difference_vector; + + return LOS::CrossTrackError::from_vector(cross_track_error); +} + +double AdaptiveLOSGuidance::calculate_psi_d(const double& y_e) { + return pi_h_ - beta_c_hat_ - atan(y_e / lookahead_distance_h_); +} + +double AdaptiveLOSGuidance::calculate_theta_d(const double& z_e) { + return pi_v_ + alpha_c_hat_ + atan(z_e / lookahead_distance_v_); +} + +void AdaptiveLOSGuidance::update_adaptive_estimates( + const LOS::CrossTrackError& crosstrack_error) { + double beta_c_hat_dot = + gamma_h_ * + (lookahead_distance_h_ / + sqrt(lookahead_distance_h_ * lookahead_distance_h_ + + crosstrack_error.y_e * crosstrack_error.y_e)) * + crosstrack_error.y_e; + double alpha_c_hat_dot = + gamma_v_ * + (lookahead_distance_v_ / + sqrt(lookahead_distance_v_ * lookahead_distance_v_ + + crosstrack_error.z_e * crosstrack_error.z_e)) * + crosstrack_error.z_e; + + beta_c_hat_ += beta_c_hat_dot * time_step_; + alpha_c_hat_ += alpha_c_hat_dot * time_step_; +} diff --git a/guidance/los_guidance/src/los_guidance_node.cpp b/guidance/los_guidance/src/los_guidance_node.cpp index 31ad2eb74..420b97565 100644 --- a/guidance/los_guidance/src/los_guidance_node.cpp +++ b/guidance/los_guidance/src/los_guidance_node.cpp @@ -1,12 +1,10 @@ #include - #include "los_guidance/los_guidance_ros.hpp" -// Main Entry Point int main(int argc, char** argv) { rclcpp::init(argc, argv); - auto node = std::make_shared(); + auto node = std::make_shared(); rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(node); diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 976d73980..0b8215d6e 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -1,214 +1,108 @@ -#include "los_guidance/los_guidance_ros.hpp" - -#include #include -#include -#include - -#include "los_guidance/lib/types.hpp" - -const auto start_message = R"( - _ ___ ____ ____ _ _ - | | / _ \/ ___| / ___|_ _(_) __| | __ _ _ __ ___ ___ - | | | | | \___ \ | | _| | | | |/ _` |/ _` | '_ \ / __/ _ \ - | |__| |_| |___) | | |_| | |_| | | (_| | (_| | | | | (_| __/ - |_____\___/|____/ \____|\__,_|_|\__,_|\__,_|_| |_|\___\___| - -)"; +#include -namespace vortex::guidance::los { +LOSGuidanceNode::LOSGuidanceNode() : Node("los_guidance_node") { + time_step_ = std::chrono::milliseconds(10); -// Constructor -LosGuidanceNode::LosGuidanceNode() : Node("los_guidance_node") { - double time_step_s = this->declare_parameter("time_step"); - time_step_ = - std::chrono::milliseconds(static_cast(time_step_s * 1000)); - - const std::string yaml_path = - this->declare_parameter("los_config_file"); - - YAML::Node config = get_los_config(yaml_path); - - parse_common_config(config["common"]); set_subscribers_and_publisher(); + set_action_server(); - set_service_server(); - set_adaptive_los_guidance(config); - set_proportional_los_guidance(config); - set_integral_los_guidance(config); - set_vector_field_guidance(config); - spdlog::info(start_message); + set_adaptive_los_guidance(); + + spdlog::info("LOS guidance node initialized"); } -// ROS Setup -void LosGuidanceNode::set_subscribers_and_publisher() { +void LOSGuidanceNode::set_subscribers_and_publisher() { this->declare_parameter("topics.pose"); this->declare_parameter("topics.guidance.los"); this->declare_parameter("topics.waypoint"); - this->declare_parameter("topics.odom"); std::string pose_topic = this->get_parameter("topics.pose").as_string(); std::string guidance_topic = this->get_parameter("topics.guidance.los").as_string(); std::string waypoint_topic = this->get_parameter("topics.waypoint").as_string(); - std::string odom_topic = this->get_parameter("topics.odom").as_string(); - auto qos_sensor_data = vortex::utils::qos_profiles::sensor_data_profile(1); + rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; + auto qos_sensor_data = rclcpp::QoS( + rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); reference_pub_ = this->create_publisher( guidance_topic, qos_sensor_data); - los_debug_pub_ = this->create_publisher( - "los_debug", qos_sensor_data); - - state_debug_pub_ = this->create_publisher( - "state_debug", qos_sensor_data); - waypoint_sub_ = this->create_subscription( waypoint_topic, qos_sensor_data, - std::bind(&LosGuidanceNode::waypoint_callback, this, + std::bind(&LOSGuidanceNode::waypoint_callback, this, std::placeholders::_1)); pose_sub_ = this->create_subscription< geometry_msgs::msg::PoseWithCovarianceStamped>( pose_topic, qos_sensor_data, - std::bind(&LosGuidanceNode::pose_callback, this, - std::placeholders::_1)); - - odom_sub_ = this->create_subscription( - odom_topic, qos_sensor_data, - std::bind(&LosGuidanceNode::odom_callback, this, + std::bind(&LOSGuidanceNode::pose_callback, this, std::placeholders::_1)); } -void LosGuidanceNode::set_action_server() { +void LOSGuidanceNode::set_action_server() { this->declare_parameter("action_servers.los"); std::string action_server_name = this->get_parameter("action_servers.los").as_string(); - cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); action_server_ = rclcpp_action::create_server( this, action_server_name, - std::bind(&LosGuidanceNode::handle_goal, this, + std::bind(&LOSGuidanceNode::handle_goal, this, std::placeholders::_1, std::placeholders::_2), - std::bind(&LosGuidanceNode::handle_cancel, this, + std::bind(&LOSGuidanceNode::handle_cancel, this, std::placeholders::_1), - std::bind(&LosGuidanceNode::handle_accepted, this, + std::bind(&LOSGuidanceNode::handle_accepted, this, std::placeholders::_1), rcl_action_server_get_default_options(), cb_group_); } -void LosGuidanceNode::set_service_server() { - this->declare_parameter("services.los_mode", "set_los_mode"); - std::string service_name = - this->get_parameter("services.los_mode").as_string(); - - los_mode_service_ = this->create_service( - service_name, std::bind(&LosGuidanceNode::set_los_mode, this, - std::placeholders::_1, std::placeholders::_2)); -} +void LOSGuidanceNode::set_adaptive_los_guidance() { + this->declare_parameter("lookahead_distance_h"); + this->declare_parameter("lookahead_distance_v"); + this->declare_parameter("gamma_h"); + this->declare_parameter("gamma_v"); + this->declare_parameter("time_step"); + this->declare_parameter("u_desired"); -// Guidance Configuration -void LosGuidanceNode::set_adaptive_los_guidance(YAML::Node config) { - auto adaptive_los_config = config["adaptive_los"]; - auto params = AdaptiveLosParams{}; + u_desired_ = this->get_parameter("u_desired").as_double(); + LOS::Params params; params.lookahead_distance_h = - adaptive_los_config["lookahead_distance_h"].as(); + this->get_parameter("lookahead_distance_h").as_double(); params.lookahead_distance_v = - adaptive_los_config["lookahead_distance_v"].as(); - params.gamma_h = adaptive_los_config["gamma_h"].as(); - params.gamma_v = adaptive_los_config["gamma_v"].as(); - params.time_step = static_cast(time_step_.count()) / 1000.0; + this->get_parameter("lookahead_distance_v").as_double(); + params.gamma_h = this->get_parameter("gamma_h").as_double(); + params.gamma_v = this->get_parameter("gamma_v").as_double(); + params.time_step = this->get_parameter("time_step").as_double(); - adaptive_los_ = std::make_unique(params); + adaptive_los_guidance_ = std::make_unique(params); } -void LosGuidanceNode::set_proportional_los_guidance(YAML::Node config) { - auto proportional_los_config = config["prop_los"]; - auto params = ProportionalLosParams{}; - - params.lookahead_distance_h = - proportional_los_config["lookahead_distance_h"].as(); - params.lookahead_distance_v = - proportional_los_config["lookahead_distance_v"].as(); - - proportional_los_ = std::make_unique(params); -} - -void LosGuidanceNode::set_integral_los_guidance(YAML::Node config) { - auto integral_los_config = config["integer_los"]; - auto params = IntegralLosParams{}; - - params.k_p_h = integral_los_config["k_p_h"].as(); - params.k_p_v = integral_los_config["k_p_v"].as(); - params.k_i_h = integral_los_config["k_i_h"].as(); - params.k_i_v = integral_los_config["k_i_v"].as(); - params.time_step = static_cast(time_step_.count()) / 1000.0; - - integral_los_ = std::make_unique(params); -} - -void LosGuidanceNode::set_vector_field_guidance(YAML::Node config) { - auto vector_field_config = config["vector_field_los"]; - auto params = VectorFieldLosParams{}; - - params.max_approach_angle_h = - vector_field_config["max_approach_angle_h"].as(); - params.max_approach_angle_v = - vector_field_config["max_approach_angle_v"].as(); - params.k_p_h = vector_field_config["k_p_h"].as(); - params.k_p_v = vector_field_config["k_p_v"].as(); - params.time_step = static_cast(time_step_.count()) / 1000.0; - - vector_field_los_ = std::make_unique(params); -} - -// Topic Callbacks -void LosGuidanceNode::waypoint_callback( - const geometry_msgs::msg::PointStamped::SharedPtr wp_msg) { - std::lock_guard lock(mutex_); - - const auto new_wp = types::Point::point_from_ros(wp_msg->point); - - if (!has_active_segment_) { - path_inputs_.prev_point = path_inputs_.current_position; - path_inputs_.next_point = new_wp; - has_active_segment_ = true; - } else { - path_inputs_.prev_point = path_inputs_.next_point; - path_inputs_.next_point = new_wp; - } - - spdlog::info("Received waypoint: ({}, {}, {})", new_wp.x, new_wp.y, - new_wp.z); +void LOSGuidanceNode::waypoint_callback( + const geometry_msgs::msg::PointStamped::SharedPtr los_waypoint) { + fill_los_waypoints(*los_waypoint); + adaptive_los_guidance_->update_angles(last_point_, next_point_); + spdlog::info("Received waypoint"); } -void LosGuidanceNode::pose_callback( +void LOSGuidanceNode::pose_callback( const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr current_pose) { - std::lock_guard lock(mutex_); - path_inputs_.current_position = - types::Point::point_from_ros(current_pose->pose.pose.position); -} - -void LosGuidanceNode::odom_callback( - const nav_msgs::msg::Odometry::SharedPtr msg) { - std::lock_guard lock(mutex_); - debug_current_odom_ = msg; + eta_.x = current_pose->pose.pose.position.x; + eta_.y = current_pose->pose.pose.position.y; + eta_.z = current_pose->pose.pose.position.z; } -// Action Server Callbacks -rclcpp_action::GoalResponse LosGuidanceNode::handle_goal( +rclcpp_action::GoalResponse LOSGuidanceNode::handle_goal( const rclcpp_action::GoalUUID&, std::shared_ptr goal) { (void)goal; - { std::lock_guard lock(mutex_); if (goal_handle_) { @@ -218,12 +112,11 @@ rclcpp_action::GoalResponse LosGuidanceNode::handle_goal( } } } - spdlog::info("Accepted goal request"); return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } -rclcpp_action::CancelResponse LosGuidanceNode::handle_cancel( +rclcpp_action::CancelResponse LOSGuidanceNode::handle_cancel( const std::shared_ptr< rclcpp_action::ServerGoalHandle> goal_handle) { @@ -232,64 +125,34 @@ rclcpp_action::CancelResponse LosGuidanceNode::handle_cancel( return rclcpp_action::CancelResponse::ACCEPT; } -void LosGuidanceNode::handle_accepted( +void LOSGuidanceNode::handle_accepted( const std::shared_ptr< rclcpp_action::ServerGoalHandle> goal_handle) { execute(goal_handle); } -// Service Callback -void LosGuidanceNode::set_los_mode( - const std::shared_ptr request, - std::shared_ptr response) { - method_ = static_cast(request->mode); - spdlog::info("LOS mode set to {}", static_cast(method_)); - response->success = true; +void LOSGuidanceNode::fill_los_waypoints( + const geometry_msgs::msg::PointStamped& los_waypoint) { + last_point_.x = eta_.x; + last_point_.y = eta_.y; + last_point_.z = eta_.z; + + next_point_.x = los_waypoint.point.x; + next_point_.y = los_waypoint.point.y; + next_point_.z = los_waypoint.point.z; } -// Message Helpers -vortex_msgs::msg::LOSGuidance LosGuidanceNode::fill_los_reference( - types::Outputs outputs, - types::Inputs inputs) { +vortex_msgs::msg::LOSGuidance LOSGuidanceNode::fill_los_reference() { vortex_msgs::msg::LOSGuidance reference_msg; - const double clamped_pitch = - std::clamp(outputs.theta_d, -max_pitch_angle_, max_pitch_angle_); - reference_msg.pitch = clamped_pitch; - reference_msg.yaw = outputs.psi_d; - - const double distance_to_goal = - (inputs.current_position - inputs.next_point).as_vector().norm(); - - const double u_slow_min = 0.2; // bytt ut dette med config - - if (distance_to_goal <= slow_down_distance_) { - const double alpha = distance_to_goal / slow_down_distance_; - reference_msg.surge = u_slow_min + alpha * (u_desired_ - u_slow_min); - } else { - reference_msg.surge = u_desired_; - } + reference_msg.pitch = pitch_d_; + reference_msg.yaw = yaw_d_; + reference_msg.surge = u_desired_; return reference_msg; } -YAML::Node LosGuidanceNode::get_los_config(std::string yaml_file_path) { - YAML::Node config = YAML::LoadFile(yaml_file_path); - return config; -} - -void LosGuidanceNode::parse_common_config(YAML::Node common_config) { - std::lock_guard lock(mutex_); - u_desired_ = common_config["u_desired"].as(); - max_pitch_angle_ = common_config["max_pitch_angle"].as(); - goal_reached_tol_ = common_config["goal_reached_tol"].as(); - slow_down_distance_ = common_config["slow_down_distance"].as(); - method_ = static_cast( - common_config["active_los_method"].as()); -} - -// Goal Execution -void LosGuidanceNode::execute( +void LOSGuidanceNode::execute( const std::shared_ptr< rclcpp_action::ServerGoalHandle> goal_handle) { @@ -303,16 +166,9 @@ void LosGuidanceNode::execute( const geometry_msgs::msg::PointStamped los_waypoint = goal_handle->get_goal()->goal; - const auto new_wp = types::Point::point_from_ros(los_waypoint.point); + fill_los_waypoints(los_waypoint); - if (!has_active_segment_) { - path_inputs_.prev_point = path_inputs_.current_position; - path_inputs_.next_point = new_wp; - has_active_segment_ = true; - } else { - path_inputs_.prev_point = path_inputs_.next_point; - path_inputs_.next_point = new_wp; - } + adaptive_los_guidance_->update_angles(last_point_, next_point_); auto feedback = std::make_shared(); @@ -329,7 +185,6 @@ void LosGuidanceNode::execute( return; } } - if (goal_handle->is_canceling()) { result->success = false; goal_handle->canceled(result); @@ -337,68 +192,27 @@ void LosGuidanceNode::execute( return; } - types::Inputs inputs_copy; - { - std::lock_guard lock(mutex_); - inputs_copy = path_inputs_; - } + LOS::CrossTrackError errors = + adaptive_los_guidance_->calculate_crosstrack_error(last_point_, + eta_); - types::Outputs outputs; - - switch (method_) { - case types::ActiveLosMethod::ADAPTIVE: - outputs = adaptive_los_->calculate_outputs(inputs_copy); - break; - case types::ActiveLosMethod::PROPORTIONAL: - outputs = proportional_los_->calculate_outputs(inputs_copy); - break; - case types::ActiveLosMethod::INTEGRAL: - outputs = integral_los_->calculate_outputs(inputs_copy); - break; - case types::ActiveLosMethod::VECTOR_FIELD: - outputs = vector_field_los_->calculate_outputs(inputs_copy); - break; - default: - spdlog::error("Invalid LOS method selected"); - result->success = false; - goal_handle->abort(result); - return; - } - - vortex_msgs::msg::LOSGuidance reference_msg = - fill_los_reference(outputs, inputs_copy); - feedback->feedback = reference_msg; - - los_debug_pub_->publish(reference_msg); + yaw_d_ = adaptive_los_guidance_->calculate_psi_d(errors.y_e); + pitch_d_ = adaptive_los_guidance_->calculate_theta_d(errors.z_e); - const auto& v = debug_current_odom_->twist.twist.linear; - double surge = std::sqrt(v.x * v.x + v.y * v.y + v.z * v.z); + adaptive_los_guidance_->update_adaptive_estimates(errors); - vortex_msgs::msg::LOSGuidance state_debug_msg; - Eigen::Vector3d euler = vortex::utils::math::quat_to_euler( - Eigen::Quaterniond(debug_current_odom_->pose.pose.orientation.w, - debug_current_odom_->pose.pose.orientation.x, - debug_current_odom_->pose.pose.orientation.y, - debug_current_odom_->pose.pose.orientation.z)); + vortex_msgs::msg::LOSGuidance reference_msg = fill_los_reference(); - state_debug_msg.pitch = euler.y(); - state_debug_msg.yaw = euler.z(); - state_debug_msg.surge = surge; + feedback->feedback = reference_msg; - state_debug_pub_->publish(state_debug_msg); goal_handle->publish_feedback(feedback); reference_pub_->publish(reference_msg); - if ((inputs_copy.current_position - inputs_copy.next_point) - .as_vector() - .norm() < goal_reached_tol_) { - auto stop_ref = reference_msg; - stop_ref.surge = 0.0; - stop_ref.pitch = 0.0; - stop_ref.yaw = reference_msg.yaw; - - reference_pub_->publish(stop_ref); + + if ((eta_ - next_point_).as_vector().norm() < 0.5) { result->success = true; goal_handle->succeed(result); + vortex_msgs::msg::LOSGuidance reference_msg = fill_los_reference(); + reference_pub_->publish(reference_msg); spdlog::info("Goal reached"); return; } @@ -406,5 +220,3 @@ void LosGuidanceNode::execute( loop_rate.sleep(); } } - -} // namespace vortex::guidance::los From 0c11cffed3e11c0949b059a7a1c1018ff8a8a104 Mon Sep 17 00:00:00 2001 From: Henrik Date: Sun, 29 Mar 2026 14:33:52 +0200 Subject: [PATCH 115/128] Minor changes here and there --- auv_setup/launch/autopilot.launch.py | 1 - .../launch/VCnTest.launch.py | 25 ++++--------- .../src/ct_instantiations.cpp | 9 ----- control/velocity_controller/src/utilities.cpp | 36 +------------------ 4 files changed, 8 insertions(+), 63 deletions(-) diff --git a/auv_setup/launch/autopilot.launch.py b/auv_setup/launch/autopilot.launch.py index 9c6e6591c..2f527b00c 100644 --- a/auv_setup/launch/autopilot.launch.py +++ b/auv_setup/launch/autopilot.launch.py @@ -62,7 +62,6 @@ def launch_setup(context, *args, **kwargs): -def generate_launch_description(): def generate_launch_description(): return LaunchDescription( declare_drone_and_namespace_args() diff --git a/control/velocity_controller/launch/VCnTest.launch.py b/control/velocity_controller/launch/VCnTest.launch.py index 44ff3a8a4..f6f7b6f56 100644 --- a/control/velocity_controller/launch/VCnTest.launch.py +++ b/control/velocity_controller/launch/VCnTest.launch.py @@ -17,10 +17,6 @@ def launch_setup(context,*args,**kwargs): drone, namespace=resolve_drone_and_namespace(context) global_share = get_package_share_directory('auv_setup') config_path_global = os.path.join(global_share,'config','robots',f'{drone}.yaml') - common_launch_args = { - "drone": drone, - "namespace": namespace, - }.items() stonefish_dir = get_package_share_directory('stonefish_sim') @@ -28,9 +24,13 @@ def launch_setup(context,*args,**kwargs): PythonLaunchDescriptionSource( os.path.join(stonefish_dir, 'launch', 'simulation.launch.py') ), - launch_arguments={'rendering_quality': 'low','rendering':'true'}.items(), + launch_arguments={ + 'scenario': 'nautilus_no_gpu.scn', + 'rendering_quality': 'low', + 'rendering':'true', + }.items(), ) - orca_sim = TimerAction( + drone_sim = TimerAction( period=12.0, actions=[ IncludeLaunchDescription( @@ -40,16 +40,6 @@ def launch_setup(context,*args,**kwargs): ) ] ) - operation_mode_manager_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join( - get_package_share_directory("operation_mode_manager"), - "launch", - "operation_mode_manager.launch.py", - ) - ), - launch_arguments=common_launch_args, - ) node_name_arg = DeclareLaunchArgument( 'node_name_1', default_value='test_VC_node', @@ -59,9 +49,8 @@ def launch_setup(context,*args,**kwargs): return [ stonefish_sim, - orca_sim, + drone_sim, node_name_arg, - #operation_mode_manager_launch, Node(package='velocity_controller', executable='test_VC_node', name=test_VC_name, diff --git a/control/velocity_controller/src/ct_instantiations.cpp b/control/velocity_controller/src/ct_instantiations.cpp index 124a25e99..19794350f 100644 --- a/control/velocity_controller/src/ct_instantiations.cpp +++ b/control/velocity_controller/src/ct_instantiations.cpp @@ -1,16 +1,7 @@ -// src/ct_instantiations.cpp // This file exists ONLY to emit Control Toolbox symbols -//#include #include -//#define CT_OPTCON_ENABLE_LQR -//#define CT_CORE_ENABLE_CARE -//#include -//#include - -//#include -//#include template class ct::optcon::LQR<8, 3>; template class ct::optcon::CARE<8, 3>; \ No newline at end of file diff --git a/control/velocity_controller/src/utilities.cpp b/control/velocity_controller/src/utilities.cpp index 69242e7e0..f0f894f8f 100644 --- a/control/velocity_controller/src/utilities.cpp +++ b/control/velocity_controller/src/utilities.cpp @@ -23,40 +23,7 @@ angle quaternion_to_euler_angle(double w, double x, double y, double z){ return {phi, theta, psi}; }; -/* -angle NED_to_BODY(const angle &a,const State &s){ - //TODO tests for illegal angles maybe - Eigen::Vector3d q; - q<pose.pose.orientation.w; x=rhs->pose.pose.orientation.x; @@ -120,7 +87,6 @@ angle angle_NED_to_body( double roll_des, double pitch_des, double yaw_des,doubl sr_d*sp_d*cy_d - cr_d*sy_d, sr_d*sp_d*sy_d + cr_d*cy_d, sr_d*cp_d, cr_d*sp_d*cy_d + sr_d*sy_d, cr_d*sp_d*sy_d - sr_d*cy_d, cr_d*cp_d; - // Error rotation matrix: how much to rotate in body to reach desired // R_error = R_desired * R_current^T Eigen::Matrix3d R_error = R_desired * R_current.transpose(); From 92a43c36c258346929fa6ac1c904d1180cdcde96 Mon Sep 17 00:00:00 2001 From: Henrik Date: Sun, 29 Mar 2026 15:31:33 +0200 Subject: [PATCH 116/128] some pre commit hooks auto changes --- .gitignore | 22 +- auv_setup/launch/autopilot.launch.py | 26 +- control/velocity_controller/CMakeLists.txt | 4 +- control/velocity_controller/README.md | 6 +- .../config/nautilus_params.yaml | 7 +- .../config/orca_params.yaml | 12 +- .../include/velocity_controller/LQR_setup.hpp | 71 +-- .../include/velocity_controller/PID_setup.hpp | 28 +- .../include/velocity_controller/utilities.hpp | 77 ++- .../velocity_controller.hpp | 121 ++--- .../launch/VCnTest.launch.py | 53 +- .../launch/velocity_controller.launch.py | 41 +- control/velocity_controller/package.xml | 2 +- control/velocity_controller/src/LQR_setup.cpp | 265 +++++----- control/velocity_controller/src/PID_setup.cpp | 95 ++-- .../src/ct_instantiations.cpp | 5 +- control/velocity_controller/src/utilities.cpp | 91 ++-- .../src/velocity_controller.cpp | 451 ++++++++++-------- .../velocity_controller/tests/CMakeLists.txt | 10 +- .../velocity_controller/tests/test_LQR.cpp | 127 ++--- .../velocity_controller/tests/test_PID.cpp | 97 ++-- control/velocity_controller/tests/test_VC.cpp | 82 ++-- control/velocity_controller/tests/test_VC.hpp | 44 +- .../launch/guidance_test.launch.py | 9 +- .../launch/los_guidance.launch.py | 2 - 25 files changed, 939 insertions(+), 809 deletions(-) diff --git a/.gitignore b/.gitignore index 04ae9a3fc..c5f5f029b 100644 --- a/.gitignore +++ b/.gitignore @@ -1,9 +1,9 @@ # Created by https://www.gitignore.io/api/ros # Edit at https://www.gitignore.io/?templates=ros - + # CMake cmake-build-debug/ - + ### ROS ### install/ log/ @@ -21,39 +21,39 @@ msg/*Result.msg msg/_*.py build_isolated/ devel_isolated/ - + # Generated by dynamic reconfigure *.cfgc /cfg/cpp/ /cfg/*.py - + # Ignore generated docs *.dox *.wikidoc - + # IDE stuff .project .cproject .vscode .DS_Store .idea - + # qcreator stuff CMakeLists.txt.user - + srv/_*.py *.pcd *.pyc qtcreator-* *.user - + /planning/cfg /planning/docs /planning/src - + *~ - + # Emacs .#* .cache -# End of https://www.gitignore.io/api/ros \ No newline at end of file +# End of https://www.gitignore.io/api/ros diff --git a/auv_setup/launch/autopilot.launch.py b/auv_setup/launch/autopilot.launch.py index 2f527b00c..a2111627e 100644 --- a/auv_setup/launch/autopilot.launch.py +++ b/auv_setup/launch/autopilot.launch.py @@ -5,14 +5,15 @@ from launch.actions import ( OpaqueFunction, ) -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + from auv_setup.launch_arg_common import ( declare_drone_and_namespace_args, resolve_drone_and_namespace, ) -from launch_ros.actions import ComposableNodeContainer -from launch_ros.descriptions import ComposableNode + + def launch_setup(context, *args, **kwargs): drone, namespace = resolve_drone_and_namespace(context) drone_params = os.path.join( @@ -31,7 +32,7 @@ def launch_setup(context, *args, **kwargs): 'config', 'guidance_params.yaml', ) - container=ComposableNodeContainer( + container = ComposableNodeContainer( name='autopilot_container', namespace=namespace, package='rclcpp_components', @@ -42,24 +43,25 @@ def launch_setup(context, *args, **kwargs): plugin='velocity_node', name='velocity_controller_node', namespace=namespace, - parameters=[velocity_control_params,drone_params], - extra_arguments=[{"use_intra_process_comms":True}], + parameters=[velocity_control_params, drone_params], + extra_arguments=[{"use_intra_process_comms": True}], ), ComposableNode( package='los_guidance', plugin='vortex::guidance::los::LosGuidanceNode', name='los_guidance_node', namespace=namespace, - parameters=[drone_params,{"los_config_file":los_config,"time_step":0.1},], - extra_arguments=[{"use_intra_process_comms":True}], + parameters=[ + drone_params, + {"los_config_file": los_config, "time_step": 0.1}, + ], + extra_arguments=[{"use_intra_process_comms": True}], ), - ], output='screen', - arguments=['--ros-args','--log-level','error'], + arguments=['--ros-args', '--log-level', 'error'], ) return [container] - def generate_launch_description(): diff --git a/control/velocity_controller/CMakeLists.txt b/control/velocity_controller/CMakeLists.txt index 65a831a39..cb691bf72 100644 --- a/control/velocity_controller/CMakeLists.txt +++ b/control/velocity_controller/CMakeLists.txt @@ -63,11 +63,11 @@ install( install(DIRECTORY launch - config + config DESTINATION share/${PROJECT_NAME}/ ) if(BUILD_TESTING) - add_subdirectory(tests) + add_subdirectory(tests) endif() ament_package() diff --git a/control/velocity_controller/README.md b/control/velocity_controller/README.md index 25556cb17..fad6a11f3 100644 --- a/control/velocity_controller/README.md +++ b/control/velocity_controller/README.md @@ -6,7 +6,7 @@ ROS2 lifecycle node for velocity control of an AUV (autonomous underwater vehicl ## Overview -The package implements a `Velocity_node` that subscribes to odometry and guidance inputs, computes thrust commands, and publishes them as `WrenchStamped` messages. The node is managed as a ROS2 lifecycle node, meaning it can be managed by a lifecycle manager, however if you do not want to use a lifecycle manager you can change the parameter autostart in the parameter file so that it automaticly goes into active state. +The package implements a `Velocity_node` that subscribes to odometry and guidance inputs, computes thrust commands, and publishes them as `WrenchStamped` messages. The node is managed as a ROS2 lifecycle node, meaning it can be managed by a lifecycle manager, however if you do not want to use a lifecycle manager you can change the parameter autostart in the parameter file so that it automatically goes into active state. The LQR controller linearizes the vehicle dynamics around the current state at each timestep (gain-scheduled LQR), using a body-frame model that includes linear hydrodynamic damping, Coriolis effects, and integral action for steady-state error rejection. The PID controller serves as a simpler backup. @@ -195,7 +195,7 @@ ros2 launch velocity_controller VCnTest.launch.py - If the vehicle behaves oddly, check that `interval_` (the control timestep) is being set correctly — a value of `0` disables integral action silently. ## Adding new controllers -After adding the hpp file, add the calculation to calc_thrust function in a new switch case, add to the reset_controller function, with options to reset only one integral, lastly update documentation. Remeber to intialize correctly, either in 'on_configure' or in constructor, add the appropriate parameters, and update alle the {drone}_params.yaml files. +After adding the hpp file, add the calculation to calc_thrust function in a new switch case, add to the reset_controller function, with options to reset only one integral, lastly update documentation. Remember to initialize correctly, either in 'on_configure' or in constructor, add the appropriate parameters, and update alle the {drone}_params.yaml files. ## Adding new drones -Copy a {drone}_params.yaml file and change the name to the new name of the drone. Add the appropriate matrices, and tune to satisfying behaviour. \ No newline at end of file +Copy a {drone}_params.yaml file and change the name to the new name of the drone. Add the appropriate matrices, and tune to satisfying behaviour. diff --git a/control/velocity_controller/config/nautilus_params.yaml b/control/velocity_controller/config/nautilus_params.yaml index becd032bb..88d0855a7 100644 --- a/control/velocity_controller/config/nautilus_params.yaml +++ b/control/velocity_controller/config/nautilus_params.yaml @@ -12,13 +12,12 @@ dampening_matrix_low: [23.0,0.0,0.0,0.0,0.0,0.0, 0.0,46.0,0.0,0.0,0.0,0.0, 0.0,0.0,46.0,0.0,0.0,0.0, 0.0,0.0,0.0,46.0,0.0,0.0, 0.0,0.0,0.0,0.0,46.0,0.0, 0.0,0.0,0.0,0.0,0.0,46.0] dampening_matrix_high: [1.0,0.0,0.0,0.0,0.0,0.0, 0.0,1.0,0.0,0.0,0.0,0.0, 0.0,0.0,1.0,0.0,0.0,0.0, 0.0,0.0,0.0,1.0,0.0,0.0, 0.0,0.0,0.0,0.0,1.0,0.0, 0.0,0.0,0.0,0.0,0.0,1.0] - + Settings: #Settings for the controller - controller_type: 2 #1 PID 2 LQR + controller_type: 2 #1 PID 2 LQR auto_start: false #0 for no, 1 for yes anti_overshoot: false reset_on_new_ref: true odometry_dropout_guard: true publish_rate: 100 #ms - max_force: 99.5 - + max_force: 99.5 diff --git a/control/velocity_controller/config/orca_params.yaml b/control/velocity_controller/config/orca_params.yaml index 9a111d51a..88d0855a7 100644 --- a/control/velocity_controller/config/orca_params.yaml +++ b/control/velocity_controller/config/orca_params.yaml @@ -1,6 +1,6 @@ /**: ros__parameters: - + PID_params: surge: [300.0,10.0,5.0] pitch: [60.0,8.0,12.0] @@ -12,16 +12,12 @@ dampening_matrix_low: [23.0,0.0,0.0,0.0,0.0,0.0, 0.0,46.0,0.0,0.0,0.0,0.0, 0.0,0.0,46.0,0.0,0.0,0.0, 0.0,0.0,0.0,46.0,0.0,0.0, 0.0,0.0,0.0,0.0,46.0,0.0, 0.0,0.0,0.0,0.0,0.0,46.0] dampening_matrix_high: [1.0,0.0,0.0,0.0,0.0,0.0, 0.0,1.0,0.0,0.0,0.0,0.0, 0.0,0.0,1.0,0.0,0.0,0.0, 0.0,0.0,0.0,1.0,0.0,0.0, 0.0,0.0,0.0,0.0,1.0,0.0, 0.0,0.0,0.0,0.0,0.0,1.0] - + Settings: #Settings for the controller - controller_type: 2 #1 PID 2 LQR + controller_type: 2 #1 PID 2 LQR auto_start: false #0 for no, 1 for yes anti_overshoot: false reset_on_new_ref: true odometry_dropout_guard: true publish_rate: 100 #ms - max_force: 99.5 - - - - + max_force: 99.5 diff --git a/control/velocity_controller/include/velocity_controller/LQR_setup.hpp b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp index 2b4adfadd..b96dba61e 100644 --- a/control/velocity_controller/include/velocity_controller/LQR_setup.hpp +++ b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp @@ -1,47 +1,54 @@ #pragma once +#include +#include #include #include -#include -#include #include "velocity_controller/utilities.hpp" -class LQRController{ - - public: +class LQRController { + public: LQRController(); - bool set_matrices(std::vector Q_,std::vector R_,std::vector inertia_matrix, double max_force,std::vector D_low); - void reset_controller(int nr=0); + bool set_matrices(std::vector Q_, + std::vector R_, + std::vector inertia_matrix, + double max_force, + std::vector D_low); + void reset_controller(int nr = 0); bool calculate_thrust(State states, Guidance_data guidance_values); bool set_interval(double interval); - Eigen::Vector get_thrust(); - private: - Eigen::Matrix linearize(State states); - Eigen::Matrix coriolis(const State& s); + Eigen::Vector get_thrust(); + private: + Eigen::Matrix linearize(State states); + Eigen::Matrix coriolis(const State& s); - std::tuple saturate (double value, bool windup, double limit); - double anti_windup(double error, double integral_sum, bool windup); - Eigen::Vector saturate_input(Eigen::Vector u); + std::tuple saturate(double value, + bool windup, + double limit); + double anti_windup(double error, double integral_sum, bool windup); + Eigen::Vector saturate_input(Eigen::Vector u); - Eigen::Vector update_error(const Guidance_data& error, const State& state); - - double interval_; - double integral_error_surge; double integral_error_pitch; double integral_error_yaw; - bool surge_windup; bool pitch_windup; bool yaw_windup; - Eigen::Matrix Q; Eigen::Matrix R; Eigen::Matrix B; - Eigen::Matrix D; - double max_force, mass, Ixx, Iyy,Izz; + Eigen::Vector update_error(const Guidance_data& error, + const State& state); - Eigen::Matrix inertia_matrix_inv; - Eigen::Matrix state_weight_matrix; + double interval_; + double integral_error_surge; + double integral_error_pitch; + double integral_error_yaw; + bool surge_windup; + bool pitch_windup; + bool yaw_windup; + Eigen::Matrix Q; + Eigen::Matrix R; + Eigen::Matrix B; + Eigen::Matrix D; + double max_force, mass, Ixx, Iyy, Izz; + + Eigen::Matrix inertia_matrix_inv; + Eigen::Matrix state_weight_matrix; Eigen::Matrix3d input_weight_matrix; - Eigen::Matrix augmented_system_matrix; - Eigen::Matrix augmented_input_matrix; - Eigen::Vector u; - - - - + Eigen::Matrix augmented_system_matrix; + Eigen::Matrix augmented_input_matrix; + Eigen::Vector u; }; - diff --git a/control/velocity_controller/include/velocity_controller/PID_setup.hpp b/control/velocity_controller/include/velocity_controller/PID_setup.hpp index 8168c90a0..5d6384078 100644 --- a/control/velocity_controller/include/velocity_controller/PID_setup.hpp +++ b/control/velocity_controller/include/velocity_controller/PID_setup.hpp @@ -1,28 +1,30 @@ #pragma once +#include #include -#include #include -#include +#include class PID_controller { - public: - //PID_controller(double Kp=0, double Ki=0, double Kd=0, double max_output=0, double min_output=0, double dt=0); - //PID_controller(double k_p, double k_i, double k_d) : PID_controller(k_p, k_i, k_d, 100.0, -100.0) {}; + public: + // PID_controller(double Kp=0, double Ki=0, double Kd=0, double + // max_output=0, double min_output=0, double dt=0); PID_controller(double + // k_p, double k_i, double k_d) : PID_controller(k_p, k_i, k_d, 100.0, + // -100.0) {}; bool calculate_thrust(double error); bool calculate_thrust(double error, double error_d); void reset_controller(); double get_output(); bool set_output_limits(double min_output, double max_output); - bool set_parameters(double k_p,double k_i, double k_d, double dt); - bool set_parameters(std::vector& params,double dt); + bool set_parameters(double k_p, double k_i, double k_d, double dt); + bool set_parameters(std::vector& params, double dt); bool set_dt(double dt); - private: + + private: double Kp_, Ki_, Kd_, dt_; - double integral=0; - double previous_error=0; - double output=0; + double integral = 0; + double previous_error = 0; + double output = 0; double max_output_, min_output_; - bool init=false; + bool init = false; }; - diff --git a/control/velocity_controller/include/velocity_controller/utilities.hpp b/control/velocity_controller/include/velocity_controller/utilities.hpp index 70512f5cf..11a9edf33 100644 --- a/control/velocity_controller/include/velocity_controller/utilities.hpp +++ b/control/velocity_controller/include/velocity_controller/utilities.hpp @@ -1,43 +1,68 @@ #pragma once +#include +#include #include #include #include #include "std_msgs/msg/float64_multi_array.hpp" #include "vortex_msgs/msg/los_guidance.hpp" -#include -#include - -struct angle{ - double phit=0.0; - double thetat=0.0; - double psit=0.0; +struct angle { + double phit = 0.0; + double thetat = 0.0; + double psit = 0.0; }; angle quaternion_to_euler_angle(double w, double x, double y, double z); -geometry_msgs::msg::Quaternion euler_angle_to_quaternion(double roll, double pitch, double yaw); +geometry_msgs::msg::Quaternion euler_angle_to_quaternion(double roll, + double pitch, + double yaw); -class State{ - public: - double surge=0.0, sway=0.0, heave=0.0, roll_rate=0.0, pitch_rate=0.0, yaw_rate=0.0; //roll_rate=0.0, pitch_rate=0.0, yaw_rate=0.0; - double roll=0.0, pitch=0.0, yaw=0.0; //phi, theta, psi - double w=0.0, x=0.0,y=0.0,z=0.0; - State(double surge=0,double pitch=0, double yaw=0):surge{surge}, pitch{pitch},yaw{yaw}{}; +class State { + public: + double surge = 0.0, sway = 0.0, heave = 0.0, roll_rate = 0.0, + pitch_rate = 0.0, + yaw_rate = 0.0; // roll_rate=0.0, pitch_rate=0.0, yaw_rate=0.0; + double roll = 0.0, pitch = 0.0, yaw = 0.0; // phi, theta, psi + double w = 0.0, x = 0.0, y = 0.0, z = 0.0; + State(double surge = 0, double pitch = 0, double yaw = 0) + : surge{surge}, pitch{pitch}, yaw{yaw} {}; - State operator=(int n){if (n){surge=0.0,sway=0.0,heave=0.0,roll_rate=0.0,pitch_rate=0.0,yaw_rate=0.0,roll=0.0,pitch=0.0,yaw=0.0;} return *this;}; + State operator=(int n) { + if (n) { + surge = 0.0, sway = 0.0, heave = 0.0, roll_rate = 0.0, + pitch_rate = 0.0, yaw_rate = 0.0, roll = 0.0, pitch = 0.0, + yaw = 0.0; + } + return *this; + }; State operator=(nav_msgs::msg::Odometry::SharedPtr rhs); angle get_angle(); }; -class Guidance_data{ - public: - double surge=0.0; double pitch=0.0; double yaw=0.0; - Guidance_data(double surge, double pitch, double yaw):surge{surge},pitch{pitch}, yaw{yaw} {}; - Guidance_data():surge{0.0}, pitch{0.0}, yaw{0.0} {}; - Guidance_data& operator=(const vortex_msgs::msg::LOSGuidance::SharedPtr& msg); +class Guidance_data { + public: + double surge = 0.0; + double pitch = 0.0; + double yaw = 0.0; + Guidance_data(double surge, double pitch, double yaw) + : surge{surge}, pitch{pitch}, yaw{yaw} {}; + Guidance_data() : surge{0.0}, pitch{0.0}, yaw{0.0} {}; + Guidance_data& operator=( + const vortex_msgs::msg::LOSGuidance::SharedPtr& msg); }; -Eigen::Vector3d body_rates_to_euler_rates(double roll, double pitch,double p, double q, double r); -angle angle_NED_to_body( double roll_des, double pitch_des, double yaw_des, double roll, double pitch, double yaw); +Eigen::Vector3d body_rates_to_euler_rates(double roll, + double pitch, + double p, + double q, + double r); +angle angle_NED_to_body(double roll_des, + double pitch_des, + double yaw_des, + double roll, + double pitch, + double yaw); angle angle_NED_to_body(angle desired, angle state); -inline angle angle_NED_to_body(angle desired, angle state){return angle_NED_to_body(desired.phit, desired.thetat, desired.psit, state.phit, state.thetat, state.psit);} - - +inline angle angle_NED_to_body(angle desired, angle state) { + return angle_NED_to_body(desired.phit, desired.thetat, desired.psit, + state.phit, state.thetat, state.psit); +} diff --git a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp index 6d356cea5..b732481b7 100644 --- a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp +++ b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp @@ -1,69 +1,72 @@ #pragma once -#include -#include -#include -#include -#include #include #include -#include "velocity_controller/PID_setup.hpp" -#include "LQR_setup.hpp" -#include "nav_msgs/msg/odometry.hpp" -#include "vortex_msgs/msg/los_guidance.hpp" +#include +#include #include +#include +#include +#include #include - - - -class Velocity_node : public rclcpp_lifecycle::LifecycleNode{ - public: - explicit Velocity_node(const rclcpp::NodeOptions & options); - Velocity_node(const Velocity_node&)=delete; //no copy constructor - Velocity_node& operator=(const Velocity_node&) = delete; //no copy assignment - //TODO: decide if i one should be allowed to move or transfer ownership if the class - //Different initializatin functions +#include "LQR_setup.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "velocity_controller/PID_setup.hpp" +#include "vortex_msgs/msg/los_guidance.hpp" + +class Velocity_node : public rclcpp_lifecycle::LifecycleNode { + public: + explicit Velocity_node(const rclcpp::NodeOptions& options); + Velocity_node(const Velocity_node&) = delete; // no copy constructor + Velocity_node& operator=(const Velocity_node&) = + delete; // no copy assignment + // TODO: decide if i one should be allowed to move or transfer ownership if + // the class Different initialization functions void get_new_parameters(); - //Timer functions + // Timer functions void calc_thrust(); - //Callback functions - void guidance_callback(const vortex_msgs::msg::LOSGuidance::SharedPtr msg_ptr); + // Callback functions + void guidance_callback( + const vortex_msgs::msg::LOSGuidance::SharedPtr msg_ptr); void odometry_callback(const nav_msgs::msg::Odometry::SharedPtr msg_ptr); - //Publisher instance - rclcpp::Publisher::SharedPtr publisher_thrust; - - //Timer instance - rclcpp::TimerBase::SharedPtr timer_calculation; + // Publisher instance + rclcpp::Publisher::SharedPtr + publisher_thrust; + + // Timer instance + rclcpp::TimerBase::SharedPtr timer_calculation; rclcpp::TimerBase::SharedPtr startup_timer_; - //Subscriber instance - rclcpp::Subscription::SharedPtr subscriber_Odometry; - rclcpp::Subscription::SharedPtr subscriber_guidance; - //rclcpp::Subscription::SharedPtr subscriber_killswitch; + // Subscriber instance + rclcpp::Subscription::SharedPtr + subscriber_Odometry; + rclcpp::Subscription::SharedPtr + subscriber_guidance; + // rclcpp::Subscription::SharedPtr + // subscriber_killswitch; - //Variables for topics + // Variables for topics std::string topic_thrust; std::string topic_guidance; std::string topic_killswitch; std::string topic_odometry; - //Variables for timers + // Variables for timers int publish_rate; double max_force; - //Stored wrenches values + // Stored wrenches values vortex_msgs::msg::LOSGuidance reference_in; Guidance_data guidance_values; State current_state; geometry_msgs::msg::WrenchStamped thrust_out; + int controller_type; // 1 PID, 2 LQR - int controller_type; //1 PID, 2 LQR - - //PID controllers + // PID controllers PID_controller PID_surge; std::vector surge_params; PID_controller PID_yaw; @@ -71,39 +74,39 @@ class Velocity_node : public rclcpp_lifecycle::LifecycleNode{ PID_controller PID_pitch; std::vector pitch_params; - - //LQR Controller + // LQR Controller LQRController lqr_controller; - //LQRparameters lqr_parameters; + // LQRparameters lqr_parameters; std::vector Q; std::vector R; - //std::vector Qi; - //std::vector Ri; + // std::vector Qi; + // std::vector Ri; std::vector inertia_matrix; std::vector dampening_matrix_low; std::vector dampening_matrix_high; - + std::atomic_bool should_exit_{false}; - //VC settings + // VC settings bool reset_on_new_ref; bool anti_overshoot; bool auto_start; bool odometry_dropout_guard; - int publish_counter=0; - bool first_start=true; - - //States - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure(const rclcpp_lifecycle::State &) override; - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) override; - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; - - void reset_controllers(int nr=0); + int publish_counter = 0; + bool first_start = true; + + // States + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_configure(const rclcpp_lifecycle::State&) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_activate(const rclcpp_lifecycle::State& state) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_deactivate(const rclcpp_lifecycle::State& state) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_cleanup(const rclcpp_lifecycle::State&) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_shutdown(const rclcpp_lifecycle::State& state) override; + + void reset_controllers(int nr = 0); rclcpp::QoS pub_QoS; rclcpp::QoS sub_QoS; - - }; - - diff --git a/control/velocity_controller/launch/VCnTest.launch.py b/control/velocity_controller/launch/VCnTest.launch.py index f6f7b6f56..bca0e911f 100644 --- a/control/velocity_controller/launch/VCnTest.launch.py +++ b/control/velocity_controller/launch/VCnTest.launch.py @@ -1,22 +1,27 @@ -from launch import LaunchDescription -from launch_ros.actions import Node import os -from launch.actions import IncludeLaunchDescription, TimerAction -from launch.actions import IncludeLaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.launch_description_sources import PythonLaunchDescriptionSource + from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + IncludeLaunchDescription, + OpaqueFunction, + TimerAction, +) +from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + from auv_setup.launch_arg_common import ( declare_drone_and_namespace_args, resolve_drone_and_namespace, ) -from launch.actions import OpaqueFunction -def launch_setup(context,*args,**kwargs): - drone, namespace=resolve_drone_and_namespace(context) + +def launch_setup(context, *args, **kwargs): + drone, namespace = resolve_drone_and_namespace(context) global_share = get_package_share_directory('auv_setup') - config_path_global = os.path.join(global_share,'config','robots',f'{drone}.yaml') + config_path_global = os.path.join(global_share, 'config', 'robots', f'{drone}.yaml') stonefish_dir = get_package_share_directory('stonefish_sim') @@ -27,7 +32,7 @@ def launch_setup(context,*args,**kwargs): launch_arguments={ 'scenario': 'nautilus_no_gpu.scn', 'rendering_quality': 'low', - 'rendering':'true', + 'rendering': 'true', }.items(), ) drone_sim = TimerAction( @@ -38,12 +43,13 @@ def launch_setup(context,*args,**kwargs): os.path.join(stonefish_dir, 'launch', 'drone_sim.launch.py') ) ) - ] + ], ) node_name_arg = DeclareLaunchArgument( - 'node_name_1', default_value='test_VC_node', - description='Name of the test VC node' + 'node_name_1', + default_value='test_vc_node', + description='Name of the test velocity controller node', ) test_VC_name = LaunchConfiguration('node_name_1') @@ -51,13 +57,18 @@ def launch_setup(context,*args,**kwargs): stonefish_sim, drone_sim, node_name_arg, - Node(package='velocity_controller', - executable='test_VC_node', - name=test_VC_name, - namespace=namespace, - output='screen', - parameters=[config_path_global]) + Node( + package='velocity_controller', + executable='test_vc_node', + name=test_VC_name, + namespace=namespace, + output='screen', + parameters=[config_path_global], + ), ] + def generate_launch_description(): - return LaunchDescription(declare_drone_and_namespace_args()+[OpaqueFunction(function=launch_setup)]) \ No newline at end of file + return LaunchDescription( + declare_drone_and_namespace_args() + [OpaqueFunction(function=launch_setup)] + ) diff --git a/control/velocity_controller/launch/velocity_controller.launch.py b/control/velocity_controller/launch/velocity_controller.launch.py index 00ad9cb81..666cfe414 100644 --- a/control/velocity_controller/launch/velocity_controller.launch.py +++ b/control/velocity_controller/launch/velocity_controller.launch.py @@ -1,33 +1,40 @@ -from launch import LaunchDescription -from launch_ros.actions import Node import os -#from launch.actions import IncludeLaunchDescription -from launch.actions import DeclareLaunchArgument -#from launch.launch_description_sources import PythonLaunchDescriptionSource + +# from launch.launch_description_sources import PythonLaunchDescriptionSource from ament_index_python.packages import get_package_share_directory -from launch.substitutions import LaunchConfiguration +from launch import LaunchDescription + +# from launch.actions import IncludeLaunchDescription +from launch.actions import OpaqueFunction +from launch_ros.actions import Node + from auv_setup.launch_arg_common import ( declare_drone_and_namespace_args, resolve_drone_and_namespace, ) -from launch.actions import OpaqueFunction -def launch_setup(context,*args,**kwargs): - drone, namespace=resolve_drone_and_namespace(context) +def launch_setup(context, *args, **kwargs): + drone, namespace = resolve_drone_and_namespace(context) pkg_share = get_package_share_directory('velocity_controller') global_share = get_package_share_directory('auv_setup') config_path_local = os.path.join(pkg_share, 'config', f'{drone}_params.yaml') - config_path_global = os.path.join(global_share,'config','robots',f"{drone}.yaml") + config_path_global = os.path.join(global_share, 'config', 'robots', f"{drone}.yaml") return [ - Node(package='velocity_controller', - executable='velocity_controller_node', - name="velocity_controller_node", - namespace=namespace, - output='screen', - parameters=[config_path_local,config_path_global]) + Node( + package='velocity_controller', + executable='velocity_controller_node', + name="velocity_controller_node", + namespace=namespace, + output='screen', + parameters=[config_path_local, config_path_global], + ) ] + + def generate_launch_description(): - return LaunchDescription(declare_drone_and_namespace_args()+[OpaqueFunction(function=launch_setup)]) \ No newline at end of file + return LaunchDescription( + declare_drone_and_namespace_args() + [OpaqueFunction(function=launch_setup)] + ) diff --git a/control/velocity_controller/package.xml b/control/velocity_controller/package.xml index b55753631..c467e56c6 100644 --- a/control/velocity_controller/package.xml +++ b/control/velocity_controller/package.xml @@ -19,7 +19,7 @@ rclcpp_components lifecycle_msgs auv_setup - + ament_cmake_gtest ament_lint_auto ament_lint_common diff --git a/control/velocity_controller/src/LQR_setup.cpp b/control/velocity_controller/src/LQR_setup.cpp index f6314cac6..406741226 100644 --- a/control/velocity_controller/src/LQR_setup.cpp +++ b/control/velocity_controller/src/LQR_setup.cpp @@ -1,195 +1,228 @@ #include "velocity_controller/LQR_setup.hpp" -#include "rclcpp/rclcpp.hpp" +#include +#include #include +#include #include -#include #include -#include -#include -//#include -//#include -//#include -//#include +#include "rclcpp/rclcpp.hpp" +// #include +// #include +// #include +// #include +#include #include "velocity_controller/PID_setup.hpp" #include "velocity_controller/utilities.hpp" -#include -//#include +// #include +#include "ct/optcon/lqr/LQR.hpp" #include "vortex/utils/math.hpp" -#include "ct/optcon/lqr/LQR.hpp" - -LQRController::LQRController(){ +LQRController::LQRController() { Q.setZero(); R.setZero(); B.setZero(); D.setZero(); inertia_matrix_inv.setZero(); }; -bool LQRController::set_matrices(std::vector Q_,std::vector R_,std::vector inertia_matrix_,double max_force_, std::vector D_low){ - if (Q_.size()!=8){ - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The Q matrix has the wrong amount of elements"); +bool LQRController::set_matrices(std::vector Q_, + std::vector R_, + std::vector inertia_matrix_, + double max_force_, + std::vector D_low) { + if (Q_.size() != 8) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), + "The Q matrix has the wrong amount of elements"); return 0; } - if(R_.size()!=3){ - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The R matrix has the wrong amount of elements"); + if (R_.size() != 3) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), + "The R matrix has the wrong amount of elements"); return 0; } - if(inertia_matrix_.size()!=36){ - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The M matrix has the wrong amount of elements"); + if (inertia_matrix_.size() != 36) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), + "The M matrix has the wrong amount of elements"); return 0; } - if (max_force_<0){ - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The max_force need to be >0"); + if (max_force_ < 0) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), + "The max_force need to be >0"); return 0; } - max_force=max_force_; + max_force = max_force_; Q.diagonal() = Eigen::Map(Q_.data(), Q_.size()); R.diagonal() = Eigen::Map(R_.data(), R_.size()); - Ixx=inertia_matrix_.at(6*3+3); Iyy=inertia_matrix_.at(4*6+4); Izz=inertia_matrix_.at(5*6+5); mass=inertia_matrix_.at(0); - - Eigen::Matrix inertia_matrix = Eigen::Map>(inertia_matrix_.data(),6,6); - D=Eigen::Map>(D_low.data(),6,6); - inertia_matrix_inv=inertia_matrix.inverse(); - - Eigen::MatrixB_t=inertia_matrix_inv*(Eigen::Matrix()<<1,0,0, 0,0,0, 0,0,0, 0,0,0, 0,1,0, 0,0,1).finished(); - Eigen::Matrix B_m=Eigen::Matrix::Zero(); - B_m.block<6,3>(0,0)=B_t; - std::vector> swaplines{{1,7},{2,8},{3,4},{4,5}}; - for (long unsigned int i=0;i inertia_matrix = + Eigen::Map>(inertia_matrix_.data(), 6, + 6); + D = Eigen::Map>(D_low.data(), 6, 6); + inertia_matrix_inv = inertia_matrix.inverse(); + + Eigen::Matrix B_t = + inertia_matrix_inv * (Eigen::Matrix() << 1, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1) + .finished(); + Eigen::Matrix B_m = Eigen::Matrix::Zero(); + B_m.block<6, 3>(0, 0) = B_t; + std::vector> swaplines{{1, 7}, {2, 8}, {3, 4}, {4, 5}}; + for (long unsigned int i = 0; i < swaplines.size(); i++) { B_m.row(swaplines[i][0]).swap(B_m.row(swaplines[i][1])); } - B.block<5,3>(0,0)=B_m.block<5,3>(0,0); + B.block<5, 3>(0, 0) = B_m.block<5, 3>(0, 0); reset_controller(); return 1; } -std::tuple LQRController::saturate (double value, bool windup, double limit){ - if (abs(value) > limit){ - windup=true; - value = limit * (value/abs(value)); - } - else { - windup=false; +std::tuple LQRController::saturate(double value, + bool windup, + double limit) { + if (abs(value) > limit) { + windup = true; + value = limit * (value / abs(value)); + } else { + windup = false; } - return {windup,value}; + return {windup, value}; } - - -double LQRController::anti_windup(double error, double integral_sum, bool windup){ - if (!windup){ - integral_sum += error*interval_; +double LQRController::anti_windup(double error, + double integral_sum, + bool windup) { + if (!windup) { + integral_sum += error * interval_; } return integral_sum; } +Eigen::Matrix LQRController::linearize(State s) { + Eigen::Matrix D_ = Eigen::Matrix::Zero(); + + D_ = -inertia_matrix_inv * D; // Assuming linear dampening for now + Eigen::Matrix C = coriolis(s); -Eigen::Matrix LQRController::linearize(State s){ - Eigen::Matrix D_=Eigen::Matrix::Zero(); + D_ -= inertia_matrix_inv * C; // To avoid unnecessary allocation - D_=-inertia_matrix_inv*D; //Assuming linear dampening for now - - Eigen::Matrix C=coriolis(s); - - D_-=inertia_matrix_inv*C; //To avoid unneccessary allocation - - Eigen::Matrix T=Eigen::Matrix::Identity(); - Eigen::Matrix A; - A.block<6,6>(0,0)=D_; - A.block<3,3>(0,6)=A.block<3,3>(6,0)=A.block<3,3>(6,6)=Eigen::Matrix3d::Zero(); - A.block<3,3>(6,3)=T; - std::vector> swaplines{{1,7},{2,8},{3,4},{4,5}}; - for (long unsigned int i=0;i T = Eigen::Matrix::Identity(); + Eigen::Matrix A; + A.block<6, 6>(0, 0) = D_; + A.block<3, 3>(0, 6) = A.block<3, 3>(6, 0) = A.block<3, 3>(6, 6) = + Eigen::Matrix3d::Zero(); + A.block<3, 3>(6, 3) = T; + std::vector> swaplines{{1, 7}, {2, 8}, {3, 4}, {4, 5}}; + for (long unsigned int i = 0; i < swaplines.size(); i++) { A.row(swaplines[i][0]).swap(A.row(swaplines[i][1])); A.col(swaplines[i][0]).swap(A.col(swaplines[i][1])); } - - Eigen::Matrix ret; + + Eigen::Matrix ret; ret.setZero(); - ret.block<5,5>(0,0)=A.block<5,5>(0,0); - ret.block<3,3>(5,0)=Eigen::Matrix3d::Identity(); - + ret.block<5, 5>(0, 0) = A.block<5, 5>(0, 0); + ret.block<3, 3>(5, 0) = Eigen::Matrix3d::Identity(); + return ret; }; -Eigen::Vector LQRController::update_error(const Guidance_data& error, const State& state){ +Eigen::Vector LQRController::update_error(const Guidance_data& error, + const State& state) { double surge_error = error.surge; double pitch_error = error.pitch; - double yaw_error = error.yaw; - - integral_error_surge = anti_windup(surge_error, integral_error_surge, surge_windup); - integral_error_pitch = anti_windup(pitch_error, integral_error_pitch, pitch_windup); + double yaw_error = error.yaw; + + integral_error_surge = + anti_windup(surge_error, integral_error_surge, surge_windup); + integral_error_pitch = + anti_windup(pitch_error, integral_error_pitch, pitch_windup); integral_error_yaw = anti_windup(yaw_error, integral_error_yaw, yaw_windup); - - Eigen::Vector state_error= {surge_error, pitch_error, yaw_error, -state.pitch_rate, -state.yaw_rate, integral_error_surge, integral_error_pitch, integral_error_yaw}; + + Eigen::Vector state_error = { + surge_error, pitch_error, yaw_error, + -state.pitch_rate, -state.yaw_rate, integral_error_surge, + integral_error_pitch, integral_error_yaw}; return state_error; } -Eigen::Vector LQRController::saturate_input(Eigen::Vector u){ +Eigen::Vector LQRController::saturate_input( + Eigen::Vector u) { double force_x, torque_y, torque_z; std::tie(surge_windup, force_x) = saturate(u[0], surge_windup, max_force); std::tie(pitch_windup, torque_y) = saturate(u[1], pitch_windup, max_force); std::tie(yaw_windup, torque_z) = saturate(u[2], yaw_windup, max_force); return {force_x, torque_y, torque_z}; } -bool LQRController::calculate_thrust(State state, Guidance_data error){ - ct::optcon::LQR<8,3> lqr; - Eigen::Matrix K_l; - bool INFO= lqr.compute(Q,R,linearize(state),B,K_l,true,false); - if(INFO==0)return false; - Eigen::Matrix state_error = update_error(error, state); - u=saturate_input( (K_l*state_error)); +bool LQRController::calculate_thrust(State state, Guidance_data error) { + ct::optcon::LQR<8, 3> lqr; + Eigen::Matrix K_l; + bool INFO = lqr.compute(Q, R, linearize(state), B, K_l, true, false); + if (INFO == 0) + return false; + Eigen::Matrix state_error = update_error(error, state); + u = saturate_input((K_l * state_error)); return true; } -void LQRController::reset_controller(int nr){ - if(nr==0||nr==1){ - integral_error_surge=0.0; - surge_windup=false; +void LQRController::reset_controller(int nr) { + if (nr == 0 || nr == 1) { + integral_error_surge = 0.0; + surge_windup = false; } - if(nr==0||nr==2){ - integral_error_pitch=0.0; - pitch_windup=false; + if (nr == 0 || nr == 2) { + integral_error_pitch = 0.0; + pitch_windup = false; } - if(nr==0||nr==3){ - integral_error_yaw=0.0; - yaw_windup=false; + if (nr == 0 || nr == 3) { + integral_error_yaw = 0.0; + yaw_windup = false; } return; } -bool LQRController::set_interval(double interval){ - if(interval<=0)return false; - interval_=interval; +bool LQRController::set_interval(double interval) { + if (interval <= 0) + return false; + interval_ = interval; return true; } -Eigen::Vector LQRController::get_thrust(){ +Eigen::Vector LQRController::get_thrust() { return u; } -//TODO: double check the matrices here -Eigen::Matrix LQRController::coriolis(const State& s){ - double u = s.surge; - double v = s.sway; - double w = s.heave; - double p = s.roll_rate; - double q = s.pitch_rate; - double r = s.yaw_rate; - Eigen::Matrix C = Eigen::Matrix::Zero(); +// TODO: double check the matrices here +Eigen::Matrix LQRController::coriolis(const State& s) { + double u = s.surge; + double v = s.sway; + double w = s.heave; + double p = s.roll_rate; + double q = s.pitch_rate; + double r = s.yaw_rate; + Eigen::Matrix C = Eigen::Matrix::Zero(); // Top-right block (translational-rotational coupling) - C(0,4) = mass * w; C(0,5) = -mass * v; - C(1,3) = -mass * w; C(1,5) = mass * u; - C(2,3) = mass * v; C(2,4) = -mass * u; + C(0, 4) = mass * w; + C(0, 5) = -mass * v; + C(1, 3) = -mass * w; + C(1, 5) = mass * u; + C(2, 3) = mass * v; + C(2, 4) = -mass * u; // Bottom-left block (rotational-translational coupling) - C(3,1) = mass * w; C(3,2) = -mass * v; - C(4,0) = -mass* w; C(4,2) = mass * u; - C(5,0) = mass * v; C(5,1) = -mass * u; + C(3, 1) = mass * w; + C(3, 2) = -mass * v; + C(4, 0) = -mass * w; + C(4, 2) = mass * u; + C(5, 0) = mass * v; + C(5, 1) = -mass * u; // Bottom-right block (rotational-rotational coupling) - C(3,4) = Izz * r; C(3,5) = -Iyy * q; - C(4,3) = -Izz * r; C(4,5) = Ixx * p; - C(5,3) = Iyy * q; C(5,4) = -Ixx * p; + C(3, 4) = Izz * r; + C(3, 5) = -Iyy * q; + C(4, 3) = -Izz * r; + C(4, 5) = Ixx * p; + C(5, 3) = Iyy * q; + C(5, 4) = -Ixx * p; return C; -} \ No newline at end of file +} diff --git a/control/velocity_controller/src/PID_setup.cpp b/control/velocity_controller/src/PID_setup.cpp index 3b8102214..c4ac03844 100644 --- a/control/velocity_controller/src/PID_setup.cpp +++ b/control/velocity_controller/src/PID_setup.cpp @@ -1,86 +1,87 @@ #include "velocity_controller/PID_setup.hpp" /* -PID_controller::PID_controller( double Kp, double Ki, double Kd, double max_output, double min_output, double dt):Kp_(Kp), Ki_(Ki), Kd_(Kd), max_output_(max_output), min_output_(min_output), dt_(dt) { - integral = 0.0; +PID_controller::PID_controller( double Kp, double Ki, double Kd, double +max_output, double min_output, double dt):Kp_(Kp), Ki_(Ki), Kd_(Kd), +max_output_(max_output), min_output_(min_output), dt_(dt) { integral = 0.0; previous_error = 0.0; };*/ -//TODO: kanskje forbedre integrasjon og derivasjons beregningene -//TODO: check for more errors, f.example Nan or very high intergral -bool PID_controller::calculate_thrust(double error){ - if(!init)return false; - //Saturation - if (output>max_output_){ +// TODO: kanskje forbedre integrasjon og derivasjons beregningene +// TODO: check for more errors, f.example Nan or very high integral +bool PID_controller::calculate_thrust(double error) { + if (!init) + return false; + // Saturation + if (output > max_output_) { output = max_output_; - } - else if (output < min_output_){ + } else if (output < min_output_) { output = min_output_; + } else { + integral += error * dt_; // anti-wind up } - else{ - integral+=error*dt_; //anti-wind up - } - //P + I + D - output=Kp_*error+Ki_*integral + Kd_ * (error - previous_error) / dt_; - previous_error = error; + // P + I + D + output = + Kp_ * error + Ki_ * integral + Kd_ * (error - previous_error) / dt_; + previous_error = error; return true; -}; -bool PID_controller::calculate_thrust(double error, double error_d){ - if(!init)return false; - //Saturation - if (output>max_output_){ +}; +bool PID_controller::calculate_thrust(double error, double error_d) { + if (!init) + return false; + // Saturation + if (output > max_output_) { output = max_output_; - } - else if (output < min_output_){ + } else if (output < min_output_) { output = min_output_; + } else { + integral += error * dt_; // anti-wind up } - else{ - integral+=error*dt_; //anti-wind up - } - //P + I + D - output=Kp_*error+Ki_*integral + Kd_ * error_d; - previous_error = error; + // P + I + D + output = Kp_ * error + Ki_ * integral + Kd_ * error_d; + previous_error = error; return true; } -void PID_controller::reset_controller(){ +void PID_controller::reset_controller() { integral = 0.0; previous_error = 0.0; - output=0.0; + output = 0.0; } -double PID_controller::get_output(){ +double PID_controller::get_output() { return output; }; -bool PID_controller::set_output_limits(double min_output, double max_output){ - if (max_output& params,double dt){ - return set_parameters(params.at(0),params.at(1),params.at(2), dt); - +bool PID_controller::set_parameters(std::vector& params, double dt) { + return set_parameters(params.at(0), params.at(1), params.at(2), dt); }; - diff --git a/control/velocity_controller/src/ct_instantiations.cpp b/control/velocity_controller/src/ct_instantiations.cpp index 19794350f..ac8ff543c 100644 --- a/control/velocity_controller/src/ct_instantiations.cpp +++ b/control/velocity_controller/src/ct_instantiations.cpp @@ -2,6 +2,5 @@ #include - -template class ct::optcon::LQR<8, 3>; -template class ct::optcon::CARE<8, 3>; \ No newline at end of file +template class ct::optcon::LQR<8, 3>; +template class ct::optcon::CARE<8, 3>; diff --git a/control/velocity_controller/src/utilities.cpp b/control/velocity_controller/src/utilities.cpp index f0f894f8f..85d8145a5 100644 --- a/control/velocity_controller/src/utilities.cpp +++ b/control/velocity_controller/src/utilities.cpp @@ -1,11 +1,11 @@ #include "velocity_controller/utilities.hpp" -#include "Eigen/Dense" +#include #include -#include -#include #include +#include +#include "Eigen/Dense" -angle quaternion_to_euler_angle(double w, double x, double y, double z){ +angle quaternion_to_euler_angle(double w, double x, double y, double z) { double ysqr = y * y; double t0 = +2.0 * (w * x + y * z); @@ -24,31 +24,32 @@ angle quaternion_to_euler_angle(double w, double x, double y, double z){ return {phi, theta, psi}; }; -State State::operator=(nav_msgs::msg::Odometry::SharedPtr rhs){ - w=rhs->pose.pose.orientation.w; - x=rhs->pose.pose.orientation.x; - y=rhs->pose.pose.orientation.y; - z=rhs->pose.pose.orientation.z; - - auto [r,p,y_]=quaternion_to_euler_angle(w, x, y, z); - roll=r; - pitch=p; - yaw=y_; - - //angular velocity - roll_rate=rhs->twist.twist.angular.x; - pitch_rate=rhs->twist.twist.angular.y; - yaw_rate=rhs->twist.twist.angular.z; - //velocity +State State::operator=(nav_msgs::msg::Odometry::SharedPtr rhs) { + w = rhs->pose.pose.orientation.w; + x = rhs->pose.pose.orientation.x; + y = rhs->pose.pose.orientation.y; + z = rhs->pose.pose.orientation.z; + + auto [r, p, y_] = quaternion_to_euler_angle(w, x, y, z); + roll = r; + pitch = p; + yaw = y_; + + // angular velocity + roll_rate = rhs->twist.twist.angular.x; + pitch_rate = rhs->twist.twist.angular.y; + yaw_rate = rhs->twist.twist.angular.z; + // velocity surge = rhs->twist.twist.linear.x; sway = rhs->twist.twist.linear.y; heave = rhs->twist.twist.linear.z; return (*this); - } -geometry_msgs::msg::Quaternion euler_angle_to_quaternion(double roll, double pitch, double yaw){ +geometry_msgs::msg::Quaternion euler_angle_to_quaternion(double roll, + double pitch, + double yaw) { double cy = cos(yaw * 0.5); double sy = sin(yaw * 0.5); double cp = cos(pitch * 0.5); @@ -65,45 +66,51 @@ geometry_msgs::msg::Quaternion euler_angle_to_quaternion(double roll, double pit return q; } -angle angle_NED_to_body( double roll_des, double pitch_des, double yaw_des,double roll, double pitch, double yaw) -{ - double cr = std::cos(roll), sr = std::sin(roll); +angle angle_NED_to_body(double roll_des, + double pitch_des, + double yaw_des, + double roll, + double pitch, + double yaw) { + double cr = std::cos(roll), sr = std::sin(roll); double cp = std::cos(pitch), sp = std::sin(pitch); - double cy = std::cos(yaw), sy = std::sin(yaw); + double cy = std::cos(yaw), sy = std::sin(yaw); // R_current: NED to body for current attitude Eigen::Matrix3d R_current; - R_current << cp*cy, cp*sy, -sp, - sr*sp*cy - cr*sy, sr*sp*sy + cr*cy, sr*cp, - cr*sp*cy + sr*sy, cr*sp*sy - sr*cy, cr*cp; + R_current << cp * cy, cp * sy, -sp, sr * sp * cy - cr * sy, + sr * sp * sy + cr * cy, sr * cp, cr * sp * cy + sr * sy, + cr * sp * sy - sr * cy, cr * cp; - double cr_d = std::cos(roll_des), sr_d = std::sin(roll_des); + double cr_d = std::cos(roll_des), sr_d = std::sin(roll_des); double cp_d = std::cos(pitch_des), sp_d = std::sin(pitch_des); - double cy_d = std::cos(yaw_des), sy_d = std::sin(yaw_des); + double cy_d = std::cos(yaw_des), sy_d = std::sin(yaw_des); // R_desired: NED to body for desired attitude Eigen::Matrix3d R_desired; - R_desired << cp_d*cy_d, cp_d*sy_d, -sp_d, - sr_d*sp_d*cy_d - cr_d*sy_d, sr_d*sp_d*sy_d + cr_d*cy_d, sr_d*cp_d, - cr_d*sp_d*cy_d + sr_d*sy_d, cr_d*sp_d*sy_d - sr_d*cy_d, cr_d*cp_d; + R_desired << cp_d * cy_d, cp_d * sy_d, -sp_d, + sr_d * sp_d * cy_d - cr_d * sy_d, sr_d * sp_d * sy_d + cr_d * cy_d, + sr_d * cp_d, cr_d * sp_d * cy_d + sr_d * sy_d, + cr_d * sp_d * sy_d - sr_d * cy_d, cr_d * cp_d; // R_error = R_desired * R_current^T Eigen::Matrix3d R_error = R_desired * R_current.transpose(); // Extract euler angles from R_error — this gives the error in body frame double pitch_err = std::asin(-R_error(2, 0)); - double roll_err = std::atan2(R_error(2, 1), R_error(2, 2)); - double yaw_err = std::atan2(R_error(1, 0), R_error(0, 0)); + double roll_err = std::atan2(R_error(2, 1), R_error(2, 2)); + double yaw_err = std::atan2(R_error(1, 0), R_error(0, 0)); return {roll_err, pitch_err, yaw_err}; } -angle State::get_angle(){ - return {roll,pitch,yaw}; +angle State::get_angle() { + return {roll, pitch, yaw}; } -Guidance_data& Guidance_data::operator=(const vortex_msgs::msg::LOSGuidance::SharedPtr& msg){ - surge=msg->surge; - pitch=msg->pitch; - yaw=msg->yaw; +Guidance_data& Guidance_data::operator=( + const vortex_msgs::msg::LOSGuidance::SharedPtr& msg) { + surge = msg->surge; + pitch = msg->pitch; + yaw = msg->yaw; return *this; } diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index 5256a61ee..1fd0a33c7 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -1,264 +1,299 @@ #include "velocity_controller/velocity_controller.hpp" #include -#include "velocity_controller/PID_setup.hpp" +#include #include #include -#include #include #include #include +#include +#include #include #include -#include "vortex/utils/math.hpp" +#include "velocity_controller/PID_setup.hpp" #include "velocity_controller/utilities.hpp" -#include -#include - - +#include "vortex/utils/math.hpp" -Velocity_node::Velocity_node(const rclcpp::NodeOptions & options) : rclcpp_lifecycle::LifecycleNode("velocity_controller_lifecycle", options), lqr_controller(), pub_QoS(10), sub_QoS(10) -{ - get_new_parameters(); - pub_QoS.keep_last(10).reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT).durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); - sub_QoS.keep_last(10).reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT).durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); +Velocity_node::Velocity_node(const rclcpp::NodeOptions& options) + : rclcpp_lifecycle::LifecycleNode("velocity_controller_lifecycle", options), + lqr_controller(), + pub_QoS(10), + sub_QoS(10) { + get_new_parameters(); + pub_QoS.keep_last(10) + .reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT) + .durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); + sub_QoS.keep_last(10) + .reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT) + .durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); - //PID initialization - PID_surge.set_output_limits(-max_force, max_force); - PID_pitch.set_output_limits(-max_force, max_force); - PID_yaw.set_output_limits(-max_force, max_force); - PID_surge.set_parameters(surge_params,publish_rate/1000.0); - PID_pitch.set_parameters(pitch_params,publish_rate/1000.0); - PID_yaw.set_parameters(yaw_params,publish_rate/1000.0); + // PID initialization + PID_surge.set_output_limits(-max_force, max_force); + PID_pitch.set_output_limits(-max_force, max_force); + PID_yaw.set_output_limits(-max_force, max_force); + PID_surge.set_parameters(surge_params, publish_rate / 1000.0); + PID_pitch.set_parameters(pitch_params, publish_rate / 1000.0); + PID_yaw.set_parameters(yaw_params, publish_rate / 1000.0); - //LQR - if(!lqr_controller.set_matrices(Q,R,inertia_matrix,max_force,dampening_matrix_low)||!lqr_controller.set_interval(static_cast(publish_rate)/1000)){ - controller_type=1; - RCLCPP_INFO(this->get_logger(),"Switching to PID"); - }; - //Automaticly start in activate if auto_start is true - if(auto_start){ - startup_timer_=create_wall_timer(std::chrono::milliseconds(0), [this](){startup_timer_->cancel(); trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);}); - } - RCLCPP_INFO(this->get_logger(), "Velocity control node has been started."); + // LQR + if (!lqr_controller.set_matrices(Q, R, inertia_matrix, max_force, + dampening_matrix_low) || + !lqr_controller.set_interval(static_cast(publish_rate) / + 1000)) { + controller_type = 1; + RCLCPP_INFO(this->get_logger(), "Switching to PID"); + }; + // Automatically start in activate if auto_start is true + if (auto_start) { + startup_timer_ = + create_wall_timer(std::chrono::milliseconds(0), [this]() { + startup_timer_->cancel(); + trigger_transition( + lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); + }); + } + RCLCPP_INFO(this->get_logger(), "Velocity control node has been started."); - return; + return; } - -void Velocity_node::calc_thrust() -{ - if (odometry_dropout_guard){ - publish_counter++; - if(publish_counter>=100){ - reset_controllers(); - RCLCPP_WARN(this->get_logger(),"Odometry dropout, no thrust"); - return; +void Velocity_node::calc_thrust() { + if (odometry_dropout_guard) { + publish_counter++; + if (publish_counter >= 100) { + reset_controllers(); + RCLCPP_WARN(this->get_logger(), "Odometry dropout, no thrust"); + return; + } } - } - //TODO: Do I need ssa here? - angle ref_in_body=angle_NED_to_body({0,vortex::utils::math::ssa(guidance_values.pitch),vortex::utils::math::ssa(guidance_values.yaw)},current_state.get_angle()); - Guidance_data error={guidance_values.surge-current_state.surge,-ref_in_body.thetat,-ref_in_body.psit}; + // TODO: Do I need ssa here? + angle ref_in_body = + angle_NED_to_body({0, vortex::utils::math::ssa(guidance_values.pitch), + vortex::utils::math::ssa(guidance_values.yaw)}, + current_state.get_angle()); + Guidance_data error = {guidance_values.surge - current_state.surge, + -ref_in_body.thetat, -ref_in_body.psit}; - if(anti_overshoot){ - if (abs(error.yaw)get_logger(),"Switching to PID"); + if (anti_overshoot) { + if (abs(error.yaw) < std::numbers::pi / 2 || + abs(error.yaw) < std::numbers::pi / 2) { + error.surge = + guidance_values.surge * cos(error.yaw) * cos(error.pitch); + } } - else{ - Eigen::Vector3d u=lqr_controller.get_thrust(); - thrust_out.wrench.force.x=u[0]; - thrust_out.wrench.torque.y=u[1]; - thrust_out.wrench.torque.z=u[2]; + switch (controller_type) { + case 1: { + PID_surge.calculate_thrust(error.surge); + PID_pitch.calculate_thrust(error.pitch, -current_state.pitch_rate); + PID_yaw.calculate_thrust(error.yaw, -current_state.yaw_rate); + thrust_out.wrench.force.x = PID_surge.get_output(); + thrust_out.wrench.torque.y = PID_pitch.get_output(); + thrust_out.wrench.torque.z = PID_yaw.get_output(); + + break; + } + case 2: { + if (!lqr_controller.calculate_thrust(current_state, error)) { + controller_type = 1; + RCLCPP_ERROR(this->get_logger(), "Switching to PID"); + } else { + Eigen::Vector3d u = lqr_controller.get_thrust(); + thrust_out.wrench.force.x = u[0]; + thrust_out.wrench.torque.y = u[1]; + thrust_out.wrench.torque.z = u[2]; + } + break; + } + default: { + RCLCPP_ERROR(this->get_logger(), + "Unknown controller set, switching to PID"); + controller_type = 1; + calc_thrust(); + return; + } } - break; - } - default:{ - RCLCPP_ERROR(this->get_logger(),"Unknown controller set, switching to PID"); - controller_type=1; - calc_thrust(); + publisher_thrust->publish(thrust_out); return; - } - } - publisher_thrust->publish(thrust_out); - return; } +// Callback functions +void Velocity_node::guidance_callback( + const vortex_msgs::msg::LOSGuidance::SharedPtr msg_ptr) { + if (reset_on_new_ref) { // On big step changes, reset the controllers to + // avoid big overshoots + if (abs(msg_ptr->surge - guidance_values.surge) >= 0.1) + reset_controllers(1); + if (abs(msg_ptr->pitch - guidance_values.pitch) > std::numbers::pi / 4) + reset_controllers(2); + if (abs(msg_ptr->yaw - guidance_values.yaw) < std::numbers::pi / 4) + reset_controllers(3); + } + guidance_values = msg_ptr; // overloaded to fix all the internal states - -//Callback functions -void Velocity_node::guidance_callback(const vortex_msgs::msg::LOSGuidance::SharedPtr msg_ptr){ - if(reset_on_new_ref){ //On big step changes, reset the controllers to avoid big overshoots - if(abs(msg_ptr->surge-guidance_values.surge)>=0.1) reset_controllers(1); - if (abs(msg_ptr->pitch-guidance_values.pitch)>std::numbers::pi/4)reset_controllers(2); - if (abs(msg_ptr->yaw-guidance_values.yaw)declare_parameter("topics.wrench_input"); + topic_thrust = this->get_parameter("topics.wrench_input").as_string(); + this->declare_parameter("topics.guidance.los"); + topic_guidance = this->get_parameter("topics.guidance.los").as_string(); + this->declare_parameter("topics.odom"); + topic_odometry = this->get_parameter("topics.odom").as_string(); -void Velocity_node::get_new_parameters(){ - //topics - this->declare_parameter("topics.wrench_input"); - topic_thrust = this->get_parameter("topics.wrench_input").as_string(); - this->declare_parameter("topics.guidance.los"); - topic_guidance = this->get_parameter("topics.guidance.los").as_string(); - this->declare_parameter("topics.odom"); - topic_odometry = this->get_parameter("topics.odom").as_string(); - - //Settings - this->declare_parameter("Settings.max_force"); - max_force = this->get_parameter("Settings.max_force").as_double(); - this->declare_parameter("Settings.publish_rate"); - publish_rate = this->get_parameter("Settings.publish_rate").as_int(); - this->declare_parameter("Settings.controller_type"); - controller_type=this->get_parameter("Settings.controller_type").as_int(); - this->declare_parameters("Settings", {{"auto_start", false}, {"reset_on_new_ref", true}, {"anti_overshoot", true},{"odometry_dropout_guard", true}}); - auto settings = this->get_parameters({"Settings.auto_start", "Settings.reset_on_new_ref", "Settings.anti_overshoot", "Settings.odometry_dropout_guard"}); - auto_start = settings[0].as_bool(); - reset_on_new_ref = settings[1].as_bool(); - anti_overshoot = settings[2].as_bool(); - odometry_dropout_guard = settings[3].as_bool(); - - //PID Params - this->declare_parameter>("PID_params.surge"); - surge_params=this->get_parameter("PID_params.surge").as_double_array(); - this->declare_parameter>("PID_params.pitch"); - pitch_params=this->get_parameter("PID_params.pitch").as_double_array(); - this->declare_parameter>("PID_params.yaw"); - yaw_params=this->get_parameter("PID_params.yaw").as_double_array(); + // Settings + this->declare_parameter("Settings.max_force"); + max_force = this->get_parameter("Settings.max_force").as_double(); + this->declare_parameter("Settings.publish_rate"); + publish_rate = this->get_parameter("Settings.publish_rate").as_int(); + this->declare_parameter("Settings.controller_type"); + controller_type = this->get_parameter("Settings.controller_type").as_int(); + this->declare_parameters("Settings", + {{"auto_start", false}, + {"reset_on_new_ref", true}, + {"anti_overshoot", true}, + {"odometry_dropout_guard", true}}); + auto settings = this->get_parameters( + {"Settings.auto_start", "Settings.reset_on_new_ref", + "Settings.anti_overshoot", "Settings.odometry_dropout_guard"}); + auto_start = settings[0].as_bool(); + reset_on_new_ref = settings[1].as_bool(); + anti_overshoot = settings[2].as_bool(); + odometry_dropout_guard = settings[3].as_bool(); - //LQR Parameters + // PID Params + this->declare_parameter>("PID_params.surge"); + surge_params = this->get_parameter("PID_params.surge").as_double_array(); + this->declare_parameter>("PID_params.pitch"); + pitch_params = this->get_parameter("PID_params.pitch").as_double_array(); + this->declare_parameter>("PID_params.yaw"); + yaw_params = this->get_parameter("PID_params.yaw").as_double_array(); - this->declare_parameter>("LQR_params.Q"); - Q=this->get_parameter("LQR_params.Q").as_double_array(); - this->declare_parameter>("LQR_params.R"); - R=this->get_parameter("LQR_params.R").as_double_array(); - this->declare_parameter>("physical.mass_matrix"); - inertia_matrix=this->get_parameter("physical.mass_matrix").as_double_array(); + // LQR Parameters - //D - this->declare_parameter>("dampening_matrix_low"); - this->declare_parameter>("dampening_matrix_high"); - this->dampening_matrix_low=this->get_parameter("dampening_matrix_low").as_double_array(); - this->dampening_matrix_high=this->get_parameter("dampening_matrix_high").as_double_array(); + this->declare_parameter>("LQR_params.Q"); + Q = this->get_parameter("LQR_params.Q").as_double_array(); + this->declare_parameter>("LQR_params.R"); + R = this->get_parameter("LQR_params.R").as_double_array(); + this->declare_parameter>("physical.mass_matrix"); + inertia_matrix = + this->get_parameter("physical.mass_matrix").as_double_array(); - - + // D + this->declare_parameter>("dampening_matrix_low"); + this->declare_parameter>("dampening_matrix_high"); + this->dampening_matrix_low = + this->get_parameter("dampening_matrix_low").as_double_array(); + this->dampening_matrix_high = + this->get_parameter("dampening_matrix_high").as_double_array(); } -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Velocity_node::on_configure(const rclcpp_lifecycle::State &){ - RCLCPP_INFO(get_logger(), "Configure VC"); - - // Publishers - publisher_thrust = create_publisher(topic_thrust, pub_QoS); - - //Subscribers - subscriber_Odometry = this->create_subscription( topic_odometry,sub_QoS, std::bind(&Velocity_node::odometry_callback,this,std::placeholders::_1)); - subscriber_guidance = this->create_subscription( topic_guidance,sub_QoS,std::bind(&Velocity_node::guidance_callback,this, std::placeholders::_1)); - //Timer - if(first_start&&auto_start){ - startup_timer_=create_wall_timer(std::chrono::milliseconds(0),[this](){startup_timer_->cancel(); trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE);}); - } - first_start=false; - return CallbackReturn::SUCCESS; +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +Velocity_node::on_configure(const rclcpp_lifecycle::State&) { + RCLCPP_INFO(get_logger(), "Configure VC"); + + // Publishers + publisher_thrust = create_publisher( + topic_thrust, pub_QoS); + + // Subscribers + subscriber_Odometry = this->create_subscription( + topic_odometry, sub_QoS, + std::bind(&Velocity_node::odometry_callback, this, + std::placeholders::_1)); + subscriber_guidance = + this->create_subscription( + topic_guidance, sub_QoS, + std::bind(&Velocity_node::guidance_callback, this, + std::placeholders::_1)); + // Timer + if (first_start && auto_start) { + startup_timer_ = + create_wall_timer(std::chrono::milliseconds(0), [this]() { + startup_timer_->cancel(); + trigger_transition( + lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); + }); + } + first_start = false; + return CallbackReturn::SUCCESS; } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -Velocity_node::on_activate(const rclcpp_lifecycle::State & state) -{ - RCLCPP_INFO(get_logger(), "Activating..."); - timer_calculation = this->create_wall_timer(std::chrono::milliseconds(publish_rate), std::bind(&Velocity_node::calc_thrust, this)); - auto ret = LifecycleNode::on_activate(state); - - return ret; +Velocity_node::on_activate(const rclcpp_lifecycle::State& state) { + RCLCPP_INFO(get_logger(), "Activating..."); + timer_calculation = + this->create_wall_timer(std::chrono::milliseconds(publish_rate), + std::bind(&Velocity_node::calc_thrust, this)); + auto ret = LifecycleNode::on_activate(state); + + return ret; } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -Velocity_node::on_deactivate(const rclcpp_lifecycle::State & state) -{ - RCLCPP_INFO(get_logger(), "Deactivating..."); - auto ret = LifecycleNode::on_deactivate(state); - timer_calculation.reset(); - reset_controllers(); - return ret; +Velocity_node::on_deactivate(const rclcpp_lifecycle::State& state) { + RCLCPP_INFO(get_logger(), "Deactivating..."); + auto ret = LifecycleNode::on_deactivate(state); + timer_calculation.reset(); + reset_controllers(); + return ret; } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -Velocity_node::on_cleanup(const rclcpp_lifecycle::State &) -{ - RCLCPP_INFO(get_logger(), "Cleaning up..."); - timer_calculation.reset(); - publisher_thrust.reset(); - subscriber_guidance.reset(); - subscriber_Odometry.reset(); - return CallbackReturn::SUCCESS; +Velocity_node::on_cleanup(const rclcpp_lifecycle::State&) { + RCLCPP_INFO(get_logger(), "Cleaning up..."); + timer_calculation.reset(); + publisher_thrust.reset(); + subscriber_guidance.reset(); + subscriber_Odometry.reset(); + return CallbackReturn::SUCCESS; } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -Velocity_node::on_shutdown(const rclcpp_lifecycle::State & state) -{ - RCLCPP_INFO(get_logger(), "Shutting down from state %s", state.label().c_str()); - if(timer_calculation) timer_calculation->cancel(); - timer_calculation.reset(); - publisher_thrust.reset(); - subscriber_guidance.reset(); - subscriber_Odometry.reset(); - should_exit_=true; - return CallbackReturn::SUCCESS; +Velocity_node::on_shutdown(const rclcpp_lifecycle::State& state) { + RCLCPP_INFO(get_logger(), "Shutting down from state %s", + state.label().c_str()); + if (timer_calculation) + timer_calculation->cancel(); + timer_calculation.reset(); + publisher_thrust.reset(); + subscriber_guidance.reset(); + subscriber_Odometry.reset(); + should_exit_ = true; + return CallbackReturn::SUCCESS; } -void Velocity_node::reset_controllers(int nr){ +void Velocity_node::reset_controllers(int nr) { switch (nr) { - case 0: - PID_pitch.reset_controller(); - PID_surge.reset_controller(); - PID_yaw.reset_controller(); - lqr_controller.reset_controller(); - break; - case 1: - PID_surge.reset_controller(); - lqr_controller.reset_controller(1); - break; + case 0: + PID_pitch.reset_controller(); + PID_surge.reset_controller(); + PID_yaw.reset_controller(); + lqr_controller.reset_controller(); + break; + case 1: + PID_surge.reset_controller(); + lqr_controller.reset_controller(1); + break; - case 2: - PID_pitch.reset_controller(); - lqr_controller.reset_controller(2); - break; + case 2: + PID_pitch.reset_controller(); + lqr_controller.reset_controller(2); + break; - case 3: - PID_yaw.reset_controller(); - lqr_controller.reset_controller(3); - break; + case 3: + PID_yaw.reset_controller(); + lqr_controller.reset_controller(3); + break; } } RCLCPP_COMPONENTS_REGISTER_NODE(Velocity_node) - - diff --git a/control/velocity_controller/tests/CMakeLists.txt b/control/velocity_controller/tests/CMakeLists.txt index 0d579c4f3..61f7e7586 100644 --- a/control/velocity_controller/tests/CMakeLists.txt +++ b/control/velocity_controller/tests/CMakeLists.txt @@ -49,7 +49,7 @@ ament_target_dependencies(LQR_test vortex_msgs nav_msgs ) -target_compile_definitions(LQR_test PRIVATE +target_compile_definitions(LQR_test PRIVATE YAML_PATH="${PROJECT_SOURCE_DIR}/config/parameters.yaml") target_link_libraries(${TEST_BINARY_NAME} @@ -74,8 +74,8 @@ target_link_libraries( #yaml-cpp #GTest::GTest #spdlog::spdlog - Eigen3::Eigen - ct_optcon + Eigen3::Eigen + ct_optcon ct_core ) add_executable(test_VC_node @@ -86,7 +86,7 @@ test_VC.cpp target_include_directories(test_VC_node PUBLIC $ $ -${EIGEN3_INCLUDE_DIR} +${EIGEN3_INCLUDE_DIR} ) @@ -104,4 +104,4 @@ vortex_utils install(TARGETS test_VC_node DESTINATION lib/${PROJECT_NAME} -) \ No newline at end of file +) diff --git a/control/velocity_controller/tests/test_LQR.cpp b/control/velocity_controller/tests/test_LQR.cpp index f2355ab44..fdb646bd9 100644 --- a/control/velocity_controller/tests/test_LQR.cpp +++ b/control/velocity_controller/tests/test_LQR.cpp @@ -1,32 +1,38 @@ #include +#include #include -#include #include +#include #include "velocity_controller/utilities.hpp" -#include -class LQR_test : public ::testing::Test{ - protected: - double delta=0.0001; +class LQR_test : public ::testing::Test { + protected: + double delta = 0.0001; static inline YAML::Node cfg; LQRController controller; - static void SetUpTestSuite() { + static void SetUpTestSuite() { try { cfg = YAML::LoadFile(YAML_PATH); - } - catch (const YAML::Exception& e) { - FAIL() << "Failed to load YAML from '" << YAML_PATH << "': " << e.what(); + } catch (const YAML::Exception& e) { + FAIL() << "Failed to load YAML from '" << YAML_PATH + << "': " << e.what(); } - }; - void SetUp() override{ - controller.set_matrices(cfg["/**"]["ros__parameters"]["LQR_params"]["Q"].as>(),cfg["/**"]["ros__parameters"]["LQR_params"]["R"].as>(),cfg["/**"]["ros__parameters"]["inertia_matrix"].as>(),cfg["/**"]["ros__parameters"]["max_force"].as(),cfg["/**"]["ros__parameters"]["dampening_matrix_low"].as>()); + void SetUp() override { + controller.set_matrices( + cfg["/**"]["ros__parameters"]["LQR_params"]["Q"] + .as>(), + cfg["/**"]["ros__parameters"]["LQR_params"]["R"] + .as>(), + cfg["/**"]["ros__parameters"]["inertia_matrix"] + .as>(), + cfg["/**"]["ros__parameters"]["max_force"].as(), + cfg["/**"]["ros__parameters"]["dampening_matrix_low"] + .as>()); controller.reset_controller(); controller.set_interval(0.01); } - void TearDown() override{ - - } + void TearDown() override {} }; /* TEST(LQR,setup){ @@ -37,19 +43,25 @@ TEST(LQR,setup){ controller.set_matrices(cfg["LQR"]["Q"],cfg["LQR"]["Q"],cfg["LQR"]["Q"],cfg["LQR"]["Q"],100); }; */ -TEST_F(LQR_test,wrong_setup){ - //LQRController controller; - std::vector eight={1,2,3,4,5,6,7,8}; - std::vector six={1,2,3,4,5,6}; - std::vector thirty_six={1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24,25,26,27,28,29,30,31,32,33,34,35,36}; - std::vector three={1,2,3}; - EXPECT_TRUE(controller.set_matrices(eight,three,thirty_six,100,thirty_six)); - EXPECT_FALSE(controller.set_matrices(eight,eight,thirty_six,100,thirty_six)); - EXPECT_FALSE(controller.set_matrices(three,three,thirty_six,100,thirty_six)); - EXPECT_FALSE(controller.set_matrices(eight,three,eight,100,thirty_six)); - EXPECT_FALSE(controller.set_matrices(eight,three,thirty_six,100,eight)); - EXPECT_FALSE(controller.set_matrices(eight,three,thirty_six,100,eight)); - EXPECT_FALSE(controller.set_matrices(eight,three,thirty_six,-100,thirty_six)); +TEST_F(LQR_test, wrong_setup) { + // LQRController controller; + std::vector eight = {1, 2, 3, 4, 5, 6, 7, 8}; + std::vector six = {1, 2, 3, 4, 5, 6}; + std::vector thirty_six = { + 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, + 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36}; + std::vector three = {1, 2, 3}; + EXPECT_TRUE( + controller.set_matrices(eight, three, thirty_six, 100, thirty_six)); + EXPECT_FALSE( + controller.set_matrices(eight, eight, thirty_six, 100, thirty_six)); + EXPECT_FALSE( + controller.set_matrices(three, three, thirty_six, 100, thirty_six)); + EXPECT_FALSE(controller.set_matrices(eight, three, eight, 100, thirty_six)); + EXPECT_FALSE(controller.set_matrices(eight, three, thirty_six, 100, eight)); + EXPECT_FALSE(controller.set_matrices(eight, three, thirty_six, 100, eight)); + EXPECT_FALSE( + controller.set_matrices(eight, three, thirty_six, -100, thirty_six)); }; /* TEST_F(LQR_test,solve){ @@ -60,44 +72,43 @@ TEST_F(LQR_test,solve){ EXPECT_NEAR(result(1),0,delta); EXPECT_NEAR(result(2),0,delta); };*/ -TEST_F(LQR_test,Direction){ +TEST_F(LQR_test, Direction) { Guidance_data value; State state{}; - value.surge=0.2; - controller.calculate_thrust(state,value); - Eigen::Vector result=controller.get_thrust(); - EXPECT_TRUE(result(0)>0); - + value.surge = 0.2; + controller.calculate_thrust(state, value); + Eigen::Vector result = controller.get_thrust(); + EXPECT_TRUE(result(0) > 0); } -TEST_F(LQR_test,zero_input){ +TEST_F(LQR_test, zero_input) { State states{}; - states.surge=1; - states.yaw=0.2; - states.pitch=0.3; + states.surge = 1; + states.yaw = 0.2; + states.pitch = 0.3; Guidance_data value; - value.surge=1.0; - value.yaw=0.2; - value.pitch=0.3; - controller.calculate_thrust(states,value); - Eigen::Vector result=controller.get_thrust(); - EXPECT_NEAR(result(0),0,delta); - EXPECT_NEAR(result(1),0,delta); - EXPECT_NEAR(result(2),0,delta); + value.surge = 1.0; + value.yaw = 0.2; + value.pitch = 0.3; + controller.calculate_thrust(states, value); + Eigen::Vector result = controller.get_thrust(); + EXPECT_NEAR(result(0), 0, delta); + EXPECT_NEAR(result(1), 0, delta); + EXPECT_NEAR(result(2), 0, delta); controller.reset_controller(); - states.surge=0; - states.pitch=0; - states.yaw=0; - value.surge=0; - value.pitch=0; - value.yaw=0; + states.surge = 0; + states.pitch = 0; + states.yaw = 0; + value.surge = 0; + value.pitch = 0; + value.yaw = 0; controller.calculate_thrust(states, value); - result=controller.get_thrust(); - EXPECT_NEAR(result(0),0,delta); - EXPECT_NEAR(result(1),0,delta); - EXPECT_NEAR(result(2),0,delta); + result = controller.get_thrust(); + EXPECT_NEAR(result(0), 0, delta); + EXPECT_NEAR(result(1), 0, delta); + EXPECT_NEAR(result(2), 0, delta); } -int main(int argc,char** argv){ - testing::InitGoogleTest(&argc,argv); +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } diff --git a/control/velocity_controller/tests/test_PID.cpp b/control/velocity_controller/tests/test_PID.cpp index 0d449c15f..8d1a10ec0 100644 --- a/control/velocity_controller/tests/test_PID.cpp +++ b/control/velocity_controller/tests/test_PID.cpp @@ -1,21 +1,18 @@ #include -//#include -//#include +// #include +// #include #include -//#include +// #include -class PID_test : public ::testing::Test{ - protected: - double delta=0.0001; +class PID_test : public ::testing::Test { + protected: + double delta = 0.0001; PID_controller PID; - void SetUp() override{ - PID.set_parameters(0,0,0,1); + void SetUp() override { + PID.set_parameters(0, 0, 0, 1); PID.reset_controller(); } - void TearDown() override{ - - } - + void TearDown() override {} }; /* class Node_test : public ::testing:Test{ @@ -33,57 +30,55 @@ class Node_test : public ::testing:Test{ */ -TEST_F(PID_test,reset_controller){ - PID.set_parameters(0,1,0,100); +TEST_F(PID_test, reset_controller) { + PID.set_parameters(0, 1, 0, 100); PID.calculate_thrust(100); - PID.set_parameters(0,1,0,1); + PID.set_parameters(0, 1, 0, 1); PID.calculate_thrust(0); PID.reset_controller(); SCOPED_TRACE("Scenario: reset"); - EXPECT_NEAR(PID.get_output(),0,delta); + EXPECT_NEAR(PID.get_output(), 0, delta); PID.calculate_thrust(0); - SCOPED_TRACE("Scenario: reset2"); - EXPECT_NEAR(PID.get_output(),0,delta); + SCOPED_TRACE("Scenario: reset2"); + EXPECT_NEAR(PID.get_output(), 0, delta); } -TEST_F(PID_test,P){ - PID.set_parameters(1,0,0,1); - EXPECT_NEAR(PID.calculate_thrust(1),1,delta); - EXPECT_NEAR(PID.calculate_thrust(2),2,delta); - PID.set_parameters(1.2,0,0,1); - EXPECT_NEAR(PID.calculate_thrust(-2.2),-2.64,delta); - EXPECT_NEAR(PID.calculate_thrust(-1.5),-1.8,delta); +TEST_F(PID_test, P) { + PID.set_parameters(1, 0, 0, 1); + EXPECT_NEAR(PID.calculate_thrust(1), 1, delta); + EXPECT_NEAR(PID.calculate_thrust(2), 2, delta); + PID.set_parameters(1.2, 0, 0, 1); + EXPECT_NEAR(PID.calculate_thrust(-2.2), -2.64, delta); + EXPECT_NEAR(PID.calculate_thrust(-1.5), -1.8, delta); } -TEST_F(PID_test,I){ - PID.set_parameters(0,1.1,0,1); +TEST_F(PID_test, I) { + PID.set_parameters(0, 1.1, 0, 1); PID.calculate_thrust(1); - EXPECT_NEAR(PID.get_output(),0,delta); + EXPECT_NEAR(PID.get_output(), 0, delta); PID.calculate_thrust(1); - EXPECT_NEAR(PID.get_output(),1.1,delta); + EXPECT_NEAR(PID.get_output(), 1.1, delta); PID.calculate_thrust(-1); - EXPECT_NEAR(PID.get_output(),2.2,delta); - EXPECT_NEAR(PID.calculate_thrust(0),1.1,delta); - PID.set_output_limits(-101,101); + EXPECT_NEAR(PID.get_output(), 2.2, delta); + EXPECT_NEAR(PID.calculate_thrust(0), 1.1, delta); + PID.set_output_limits(-101, 101); PID.reset_controller(); - PID.set_parameters(1,1,0,10); - EXPECT_NEAR(PID.calculate_thrust(1000),101,delta); - PID.set_parameters(1,1,0,1); - EXPECT_NEAR(PID.calculate_thrust(0),0,delta); + PID.set_parameters(1, 1, 0, 10); + EXPECT_NEAR(PID.calculate_thrust(1000), 101, delta); + PID.set_parameters(1, 1, 0, 1); + EXPECT_NEAR(PID.calculate_thrust(0), 0, delta); PID.reset_controller(); - EXPECT_NEAR(PID.calculate_thrust(-10000),-101,delta); + EXPECT_NEAR(PID.calculate_thrust(-10000), -101, delta); PID.calculate_thrust(-50); - EXPECT_NEAR(PID.calculate_thrust(1),-49,delta); -} -TEST_F(PID_test,D){ - + EXPECT_NEAR(PID.calculate_thrust(1), -49, delta); } -TEST_F(PID_test,illegal_inputs){ - double temp=PID.get_output(); - PID.set_parameters(1,1,0,0); +TEST_F(PID_test, D) {} +TEST_F(PID_test, illegal_inputs) { + double temp = PID.get_output(); + PID.set_parameters(1, 1, 0, 0); EXPECT_FALSE(PID.calculate_thrust(1)); - EXPECT_NEAR(PID.get_output(),temp,delta); - EXPECT_FALSE(PID.set_output_limits(1,-1)); - PID.set_parameters(1,1,0,-1); + EXPECT_NEAR(PID.get_output(), temp, delta); + EXPECT_FALSE(PID.set_output_limits(1, -1)); + PID.set_parameters(1, 1, 0, -1); EXPECT_FALSE(PID.calculate_thrust(1)); } @@ -103,9 +98,7 @@ TEST(PID,BASIC){ } */ - - -int main(int argc,char** argv){ - testing::InitGoogleTest(&argc,argv); +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); -} \ No newline at end of file +} diff --git a/control/velocity_controller/tests/test_VC.cpp b/control/velocity_controller/tests/test_VC.cpp index 9e32436f7..40610792e 100644 --- a/control/velocity_controller/tests/test_VC.cpp +++ b/control/velocity_controller/tests/test_VC.cpp @@ -1,70 +1,70 @@ -#include -#include -#include -#include -#include -#include #include "test_VC.hpp" #include +#include #include #include -#include "vortex_msgs/msg/los_guidance.hpp" +#include +#include +#include +#include +#include #include "velocity_controller/utilities.hpp" +#include "vortex_msgs/msg/los_guidance.hpp" -//Denne noden er kun for å teste velocity_controller noden +// Denne noden er kun for å teste velocity_controller noden -test_VC::test_VC() : Node("test_VC_node") -{ +test_VC::test_VC() : Node("test_VC_node") { this->declare_parameter("topics.guidance.los"); this->declare_parameter("topics.odom"); - this->topic_guidance=this->get_parameter("topics.guidance.los").as_string(); - this->topic_odometry=this->get_parameter("topics.odom").as_string(); + this->topic_guidance = + this->get_parameter("topics.guidance.los").as_string(); + this->topic_odometry = this->get_parameter("topics.odom").as_string(); rclcpp::QoS pub_QoS(10); - pub_QoS.keep_last(10).reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT).durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); - - publisher_guidance = this->create_publisher(topic_guidance, pub_QoS); - publisher_state = this->create_publisher("/state", pub_QoS); - + pub_QoS.keep_last(10) + .reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT) + .durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); + + publisher_guidance = this->create_publisher( + topic_guidance, pub_QoS); + publisher_state = this->create_publisher( + "/state", pub_QoS); + rclcpp::QoS sub_QoS(10); - sub_QoS.keep_last(10).reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT).durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); + sub_QoS.keep_last(10) + .reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT) + .durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); subscription_state = this->create_subscription( - topic_odometry,sub_QoS, - std::bind(&test_VC::odometry_callback,this,std::placeholders::_1)); - timer_ = this->create_wall_timer( - std::chrono::milliseconds(200), - std::bind(&test_VC::send_guidance, this)); + topic_odometry, sub_QoS, + std::bind(&test_VC::odometry_callback, this, std::placeholders::_1)); + timer_ = this->create_wall_timer(std::chrono::milliseconds(200), + std::bind(&test_VC::send_guidance, this)); clock_ = this->get_clock(); RCLCPP_INFO(this->get_logger(), "Test_VC node has been started"); - - -} +} -void test_VC::send_guidance() -{ - time1+=0.2; - reference_msg.yaw=std::numbers::pi*sin(time1*std::numbers::pi/9); - //reference_msg.pitch=0.3*sin(time1*std::numbers::pi/9); - reference_msg.surge=0.20;reference_msg.pitch=-0.4;//reference_msg.yaw=0.0; //Surge, pitch, yaw +void test_VC::send_guidance() { + time1 += 0.2; + reference_msg.yaw = std::numbers::pi * sin(time1 * std::numbers::pi / 9); + // reference_msg.pitch=0.3*sin(time1*std::numbers::pi/9); + reference_msg.surge = 0.20; + reference_msg.pitch = -0.4; // reference_msg.yaw=0.0; //Surge, pitch, yaw publisher_guidance->publish(reference_msg); } -void test_VC::odometry_callback(const nav_msgs::msg::Odometry::SharedPtr msg_ptr){ +void test_VC::odometry_callback( + const nav_msgs::msg::Odometry::SharedPtr msg_ptr) { vortex_msgs::msg::LOSGuidance msg; - angle temp=quaternion_to_euler_angle(msg_ptr->pose.pose.orientation.w, msg_ptr->pose.pose.orientation.x, msg_ptr->pose.pose.orientation.y, msg_ptr->pose.pose.orientation.z); + angle temp = quaternion_to_euler_angle( + msg_ptr->pose.pose.orientation.w, msg_ptr->pose.pose.orientation.x, + msg_ptr->pose.pose.orientation.y, msg_ptr->pose.pose.orientation.z); msg.set__pitch(temp.thetat); msg.set__yaw(temp.psit); msg.set__surge(msg_ptr->twist.twist.linear.x); publisher_state->publish(msg); - } -int main(int argc, char const *argv[]) -{ - +int main(int argc, char const* argv[]) { rclcpp::init(argc, argv); rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; } - - - \ No newline at end of file diff --git a/control/velocity_controller/tests/test_VC.hpp b/control/velocity_controller/tests/test_VC.hpp index d5270b509..d79f6dbd7 100644 --- a/control/velocity_controller/tests/test_VC.hpp +++ b/control/velocity_controller/tests/test_VC.hpp @@ -1,43 +1,45 @@ #pragma once -#include -#include -#include #include -#include #include #include -#include "vortex_msgs/msg/los_guidance.hpp" +#include +#include +#include +#include #include "nav_msgs/msg/odometry.hpp" #include "velocity_controller/utilities.hpp" +#include "vortex_msgs/msg/los_guidance.hpp" -class test_VC : public rclcpp::Node{ - public: +class test_VC : public rclcpp::Node { + public: test_VC(); - //Callback functions + // Callback functions void send_guidance(); void odometry_callback(const nav_msgs::msg::Odometry::SharedPtr msg_ptr); - - //Variables - - //Subscribers and publishers - rclcpp::Publisher::SharedPtr publisher_guidance; + + // Variables + + // Subscribers and publishers + rclcpp::Publisher::SharedPtr + publisher_guidance; rclcpp::Publisher::SharedPtr publisher_state; rclcpp::Subscription::SharedPtr subscription_state; - //Timers + // Timers rclcpp::TimerBase::SharedPtr timer_; rclcpp::Clock::SharedPtr clock_; - //Messages + // Messages vortex_msgs::msg::LOSGuidance reference_msg; - //Topics + // Topics std::string topic_guidance; - std::string topic_state="/state"; + std::string topic_state = "/state"; std::string topic_odometry; - - //MSGS - double time1=0; + // MSGS + double time1 = 0; }; -geometry_msgs::msg::Quaternion euler_angle_to_quaternion(double roll, double pitch, double yaw); \ No newline at end of file +geometry_msgs::msg::Quaternion euler_angle_to_quaternion(double roll, + double pitch, + double yaw); diff --git a/guidance/los_guidance/launch/guidance_test.launch.py b/guidance/los_guidance/launch/guidance_test.launch.py index d5dc84073..fa65290e6 100644 --- a/guidance/los_guidance/launch/guidance_test.launch.py +++ b/guidance/los_guidance/launch/guidance_test.launch.py @@ -37,7 +37,7 @@ def launch_setup(context, *args, **kwargs): "rendering": "true", }.items(), ) - + vortex_sim_interface = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join( @@ -80,11 +80,10 @@ def launch_setup(context, *args, **kwargs): velocity_controller_dir, 'launch', 'velocity_controller.launch.py' ) ), - launch_arguments={ + launch_arguments={ "drone": drone, "namespace": namespace, }.items(), - ) orca_sim = TimerAction( @@ -139,8 +138,8 @@ def launch_setup(context, *args, **kwargs): return [ stonefish_sim, - #vortex_sim_interface, - #operation_mode_launch, + # vortex_sim_interface, + # operation_mode_launch, los_guidance_launch, velocity_controller_launch, orca_sim, diff --git a/guidance/los_guidance/launch/los_guidance.launch.py b/guidance/los_guidance/launch/los_guidance.launch.py index b922c08bc..a761e6484 100644 --- a/guidance/los_guidance/launch/los_guidance.launch.py +++ b/guidance/los_guidance/launch/los_guidance.launch.py @@ -1,5 +1,3 @@ -from os import path - from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch_ros.actions import Node From e198b168387073143dec58e4c805ba445d51d3bd Mon Sep 17 00:00:00 2001 From: Henrik Date: Sun, 29 Mar 2026 15:32:58 +0200 Subject: [PATCH 117/128] updated guidance repositry from main --- .../launch/los_guidance.launch.py | 56 ++-- .../los_guidance/src/los_guidance_ros.cpp | 22 +- guidance/reference_filter_dp/CMakeLists.txt | 64 +--- guidance/reference_filter_dp/README.md | 20 +- .../config/reference_filter_params.yaml | 1 - .../reference_filter_dp/eigen_typedefs.hpp | 19 ++ .../reference_filter_dp/reference_filter.hpp | 44 +++ .../reference_filter_ros.hpp | 126 ++++++++ .../reference_filter_utils.hpp | 26 ++ .../launch/reference_filter_dp.launch.py | 54 ++-- guidance/reference_filter_dp/package.xml | 4 +- .../src/reference_filter.cpp | 38 +++ .../src/reference_filter_ros.cpp | 294 ++++++++++++++++++ .../src/reference_filter_utils.cpp | 55 ++++ 14 files changed, 677 insertions(+), 146 deletions(-) create mode 100644 guidance/reference_filter_dp/include/reference_filter_dp/eigen_typedefs.hpp create mode 100644 guidance/reference_filter_dp/include/reference_filter_dp/reference_filter.hpp create mode 100644 guidance/reference_filter_dp/include/reference_filter_dp/reference_filter_ros.hpp create mode 100644 guidance/reference_filter_dp/include/reference_filter_dp/reference_filter_utils.hpp create mode 100644 guidance/reference_filter_dp/src/reference_filter.cpp create mode 100644 guidance/reference_filter_dp/src/reference_filter_ros.cpp create mode 100644 guidance/reference_filter_dp/src/reference_filter_utils.cpp diff --git a/guidance/los_guidance/launch/los_guidance.launch.py b/guidance/los_guidance/launch/los_guidance.launch.py index a761e6484..03366e897 100644 --- a/guidance/los_guidance/launch/los_guidance.launch.py +++ b/guidance/los_guidance/launch/los_guidance.launch.py @@ -1,42 +1,32 @@ +from os import path + from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch_ros.actions import Node -from auv_setup.launch_arg_common import ( - declare_drone_and_namespace_args, - resolve_drone_and_namespace, +adapt_params = path.join( + get_package_share_directory("los_guidance"), + "config", + "guidance_params.yaml", +) +orca_params = path.join( + get_package_share_directory("auv_setup"), + "config", + "robots", + "orca.yaml", ) - - -def launch_setup(context, *args, **kwargs): - drone, namespace = resolve_drone_and_namespace(context) - - los_config = os.path.join( - get_package_share_directory("los_guidance"), - "config", - "guidance_params.yaml", - ) - - drone_params = os.path.join( - get_package_share_directory("auv_setup"), - "config", - "robots", - f"{drone}.yaml", - ) - - return [ - Node( - package="los_guidance", - executable="los_guidance_node", - name="los_guidance_node", - namespace=namespace, - parameters=[drone_params, los_config], - output="screen", - ) - ] def generate_launch_description(): - return LaunchDescription( - declare_drone_and_namespace_args() + [OpaqueFunction(function=launch_setup)] + los_guidance_node = Node( + package="los_guidance", + executable="los_guidance_node", + name="los_guidance_node", + namespace="orca", + parameters=[ + orca_params, + adapt_params, + ], + output="screen", ) + return LaunchDescription([los_guidance_node]) diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index e0131ed10..0b8215d6e 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -170,6 +170,8 @@ void LOSGuidanceNode::execute( adaptive_los_guidance_->update_angles(last_point_, next_point_); + auto feedback = + std::make_shared(); auto result = std::make_shared(); rclcpp::Rate loop_rate(1000.0 / time_step_.count()); @@ -199,22 +201,22 @@ void LOSGuidanceNode::execute( adaptive_los_guidance_->update_adaptive_estimates(errors); - if ((eta_ - next_point_).as_vector().norm() < goal_reached_tol_) { + vortex_msgs::msg::LOSGuidance reference_msg = fill_los_reference(); + + feedback->feedback = reference_msg; + + goal_handle->publish_feedback(feedback); + reference_pub_->publish(reference_msg); + + if ((eta_ - next_point_).as_vector().norm() < 0.5) { result->success = true; goal_handle->succeed(result); - u_desired_ = 0.0; - auto final_reference_msg = - std::make_unique( - fill_los_reference()); - reference_pub_->publish(std::move(final_reference_msg)); + vortex_msgs::msg::LOSGuidance reference_msg = fill_los_reference(); + reference_pub_->publish(reference_msg); spdlog::info("Goal reached"); return; } - auto reference_msg = std::make_unique( - fill_los_reference()); - reference_pub_->publish(std::move(reference_msg)); - loop_rate.sleep(); } } diff --git a/guidance/reference_filter_dp/CMakeLists.txt b/guidance/reference_filter_dp/CMakeLists.txt index b5213e31e..f02cfb95b 100644 --- a/guidance/reference_filter_dp/CMakeLists.txt +++ b/guidance/reference_filter_dp/CMakeLists.txt @@ -14,67 +14,47 @@ find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(rclcpp_components REQUIRED) find_package(vortex_msgs REQUIRED) -find_package(vortex_utils REQUIRED) -find_package(vortex_utils_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(Eigen3 REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) find_package(spdlog REQUIRED) find_package(fmt REQUIRED) include_directories(include) -set(CORE_LIB_NAME "${PROJECT_NAME}") +set(LIB_NAME "${PROJECT_NAME}_component") -add_library(${CORE_LIB_NAME} SHARED - src/lib/reference_filter.cpp - src/lib/waypoint_utils.cpp - src/lib/waypoint_follower.cpp -) - -target_include_directories(${CORE_LIB_NAME} - PUBLIC - $ - $ -) - -ament_target_dependencies(${CORE_LIB_NAME} - Eigen3 - vortex_utils -) - -set(COMPONENT_LIB_NAME "${PROJECT_NAME}_component") - -add_library(${COMPONENT_LIB_NAME} SHARED - src/ros/reference_filter_ros.cpp -) +add_library(${LIB_NAME} SHARED + src/reference_filter.cpp + src/reference_filter_ros.cpp + src/reference_filter_utils.cpp) -ament_target_dependencies(${COMPONENT_LIB_NAME} +ament_target_dependencies(${LIB_NAME} PUBLIC rclcpp rclcpp_components rclcpp_action geometry_msgs nav_msgs + Eigen3 + tf2 + tf2_geometry_msgs vortex_msgs - vortex_utils - vortex_utils_ros spdlog fmt ) -target_link_libraries(${COMPONENT_LIB_NAME} ${CORE_LIB_NAME}) - rclcpp_components_register_node( - ${COMPONENT_LIB_NAME} + ${LIB_NAME} PLUGIN "ReferenceFilterNode" EXECUTABLE ${PROJECT_NAME}_node ) -install( - TARGETS - ${CORE_LIB_NAME} - ${COMPONENT_LIB_NAME} - EXPORT export_${PROJECT_NAME} +ament_export_targets(export_${LIB_NAME}) + +install(TARGETS ${LIB_NAME} + EXPORT export_${LIB_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -91,16 +71,4 @@ install(DIRECTORY DESTINATION share/${PROJECT_NAME}/ ) -ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) -ament_export_dependencies( - Eigen3 - vortex_utils -) -ament_export_include_directories(include) -ament_export_libraries(${CORE_LIB_NAME}) - -if(BUILD_TESTING) - add_subdirectory(test) -endif() - ament_package() diff --git a/guidance/reference_filter_dp/README.md b/guidance/reference_filter_dp/README.md index 4fe004335..0ccfbf28c 100644 --- a/guidance/reference_filter_dp/README.md +++ b/guidance/reference_filter_dp/README.md @@ -33,28 +33,10 @@ The state at the next time step is calculated using forward Euler integration x_{i+1} = x_i + \dot{x}_i * dt ``` -## Waypoint modes - -Each waypoint has a mode that determines which degrees of freedom are controlled by the reference filter. The mode also determines how convergence is measured. - -| Mode | Controlled DOFs | Convergence metric | -|---|---|---| -| `FULL_POSE` | All 6 DOF (position + orientation) | Euclidean norm of position and angle errors | -| `ONLY_POSITION` | x, y, z (orientation holds current value) | Euclidean norm of position error | -| `FORWARD_HEADING` | x, y, z + yaw toward target (roll/pitch = 0) | Euclidean norm of position error and yaw error | -| `ONLY_ORIENTATION` | roll, pitch, yaw (position holds current value) | Euclidean norm of angle errors | - -For all modes, convergence is reached when the error metric drops below the `convergence_threshold` specified in the action goal. - ## Action Server - -The action server is responsible for handling goal requests and publishing guidance commands. The server will always prioritize new goal requests, and will abort ongoing requests when getting a new request. The action definition can be found [here](https://github.com/vortexntnu/vortex-msgs/blob/main/action/ReferenceFilterWaypoint.action). +The action server is responsible for handling goal requests and publishing guidance commands. The server will always prioritize new goal request, and will abort ongoing request when getting a new request. The action definition can be found [here](https://github.com/vortexntnu/vortex-msgs/blob/main/action/ReferenceFilterWaypoint.action). - Action name: /reference_filter - Goal type: PoseStamped - Result type: bool - Guidance topic: /dp/reference - -### Overwriting the reference during an action - -While an action is executing, the reference goal can be updated at any time by publishing a `geometry_msgs/msg/PoseStamped` to the reference pose topic. This allows external nodes to adjust the target pose mid-action without canceling and resending the goal. The convergence check will use the updated reference, so the action completes when the vehicle reaches the latest reference. diff --git a/guidance/reference_filter_dp/config/reference_filter_params.yaml b/guidance/reference_filter_dp/config/reference_filter_params.yaml index 1aef0ec13..a9cf24aae 100644 --- a/guidance/reference_filter_dp/config/reference_filter_params.yaml +++ b/guidance/reference_filter_dp/config/reference_filter_params.yaml @@ -2,4 +2,3 @@ ros__parameters: zeta: [1., 1., 1., 1., 1., 1.] omega: [0.2, 0.2, 0.2, 0.2, 0.2, 0.2] - time_step_ms: 10 diff --git a/guidance/reference_filter_dp/include/reference_filter_dp/eigen_typedefs.hpp b/guidance/reference_filter_dp/include/reference_filter_dp/eigen_typedefs.hpp new file mode 100644 index 000000000..c7d795e15 --- /dev/null +++ b/guidance/reference_filter_dp/include/reference_filter_dp/eigen_typedefs.hpp @@ -0,0 +1,19 @@ +/** + * @file eigen_typedefs.hpp + * @brief Contains the typedef for a 6x1 Eigen vector, 6x6 Eigen matrix. + * and 18x18 Eigen matrix. + */ + +#ifndef VORTEX_EIGEN_TYPEDEFS_H +#define VORTEX_EIGEN_TYPEDEFS_H + +#include + +typedef Eigen::Matrix Matrix18d; +typedef Eigen::Matrix Matrix18x6d; +typedef Eigen::Matrix Matrix6d; +typedef Eigen::Matrix Vector6d; +typedef Eigen::Matrix Vector18d; +typedef Eigen::Matrix Matrix3d; + +#endif diff --git a/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter.hpp b/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter.hpp new file mode 100644 index 000000000..6c1c8c4f2 --- /dev/null +++ b/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter.hpp @@ -0,0 +1,44 @@ +#ifndef REFERENCE_FILTER_HPP +#define REFERENCE_FILTER_HPP + +#include "reference_filter_dp/eigen_typedefs.hpp" + +class ReferenceFilter { + public: + explicit ReferenceFilter(); + + // @brief Calculate the state derivative + // @param x The state vector 18x1 + // @param r The reference vector 6x1 + // @return The state derivative 18x1 + // REF: Handbook of Marine Craft Hydrodynamics and Motion Control, Fossen + // 2021 p. 337 eq: 12.11 + Vector18d calculate_x_dot(const Vector18d& x, const Vector6d& r); + + // @brief Calculate the state transition matrix + // REF: Handbook of Marine Craft Hydrodynamics and Motion Control, Fossen + // 2021 p. 337 eq: 12.12 + void calculate_Ad(); + + // @brief Calculate the input matrix + // REF: Handbook of Marine Craft Hydrodynamics and Motion Control, Fossen + // 2021 p. 337 eq: 12.12 + void calculate_Bd(); + + // @brief Set the omega matrix + // @param omega The omega matrix 6x1 vector + void set_omega(const Vector6d& omega); + + // @brief Set the delta matrix + // @param zeta The delta matrix 6x1 vector + void set_delta(const Vector6d& zeta); + + private: + Matrix18d Ad_; + Matrix18x6d Bd_; + Matrix6d Omega_; + Matrix6d Delta_; + Matrix6d identity_matrix_; +}; + +#endif diff --git a/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter_ros.hpp b/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter_ros.hpp new file mode 100644 index 000000000..997360807 --- /dev/null +++ b/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter_ros.hpp @@ -0,0 +1,126 @@ +#ifndef REFERENCE_FILTER_ROS_HPP +#define REFERENCE_FILTER_ROS_HPP + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class ReferenceFilterNode : public rclcpp::Node { + public: + explicit ReferenceFilterNode( + const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); + + private: + // @brief Set the subscribers and publishers + void set_subscribers_and_publisher(); + + // @brief Set the action server + void set_action_server(); + + // @brief Initializes the reference filter with ROS parameters. + void set_refererence_filter(); + + // @brief Callback for the reference topic + // @param msg The reference message + void reference_callback( + const geometry_msgs::msg::PoseStamped::SharedPtr msg); + + // @brief Callback for the pose topic + // @param msg The pose message + void pose_callback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); + + // @brief Callback for the twist topic + // @param msg The twist message + void twist_callback( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); + + // @brief Handle the goal request + // @param uuid The goal UUID + // @param goal The goal message + // @return The goal response + rclcpp_action::GoalResponse handle_goal( + const rclcpp_action::GoalUUID& uuid, + std::shared_ptr< + const vortex_msgs::action::ReferenceFilterWaypoint::Goal> goal); + + // @brief Handle the cancel request + // @param goal_handle The goal handle + // @return The cancel response + rclcpp_action::CancelResponse handle_cancel( + const std::shared_ptr> goal_handle); + + // @brief Handle the accepted request + // @param goal_handle The goal handle + void handle_accepted( + const std::shared_ptr> goal_handle); + + // @brief Execute the goal + // @param goal_handle The goal handle + void execute( + const std::shared_ptr> goal_handle); + + Vector18d fill_reference_state(); + + Vector6d fill_reference_goal(const geometry_msgs::msg::PoseStamped& goal); + + vortex_msgs::msg::ReferenceFilter fill_reference_msg(); + + rclcpp_action::Server< + vortex_msgs::action::ReferenceFilterWaypoint>::SharedPtr action_server_; + + ReferenceFilter reference_filter_; + + rclcpp::Publisher::SharedPtr + reference_pub_; + + rclcpp::Subscription::SharedPtr + reference_sub_; + + rclcpp::Subscription< + geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_; + + rclcpp::Subscription< + geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr twist_sub_; + + rclcpp::TimerBase::SharedPtr reference_pub_timer_; + + std::chrono::milliseconds time_step_; + + geometry_msgs::msg::PoseWithCovarianceStamped current_pose_; + + geometry_msgs::msg::TwistWithCovarianceStamped current_twist_; + + // x is [eta, eta_dot, eta_dot_dot] (ref. page 337 in Fossen, 2021 + // nu and eta are 6 degrees of freedom (position and orientation in 3D + // space) + Vector18d x_; + + // The reference signal vector with 6 degrees of freedom [eta] + Vector6d r_; + + std::mutex mutex_; + + rclcpp_action::GoalUUID preempted_goal_id_; + + std::shared_ptr> + goal_handle_; + + rclcpp::CallbackGroup::SharedPtr cb_group_; +}; + +#endif diff --git a/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter_utils.hpp b/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter_utils.hpp new file mode 100644 index 000000000..e1ccf3f04 --- /dev/null +++ b/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter_utils.hpp @@ -0,0 +1,26 @@ +#ifndef REFERENCE_FILTER_UTILS_HPP +#define REFERENCE_FILTER_UTILS_HPP + +#include + +// @brief Calculate the rotation matrix from the Euler angles +// @param eta 6x1 vector containing [surge, sway, heave, roll, pitch, yaw] +// @return The rotation matrix +Matrix3d calculate_R(const Vector6d& eta); + +// @brief Calculate the transformation matrix from the Euler angles +// @param eta 6x1 vector containing [surge, sway, heave, roll, pitch, yaw] +// @return The transformation matrix +Matrix3d calculate_T(const Vector6d& eta); + +// @brief Calculate the Jacobian matrix from the Euler angles +// @param eta 6x1 vector containing [surge, sway, heave, roll, pitch, yaw] +// @return The Jacobian matrix +Matrix6d calculate_J(const Vector6d& eta); + +// @brief Calculate the smallest signed angle +// @param angle the angle given in radians +// @return The smallest signed angle +double ssa(double angle); + +#endif diff --git a/guidance/reference_filter_dp/launch/reference_filter_dp.launch.py b/guidance/reference_filter_dp/launch/reference_filter_dp.launch.py index 7f25ac155..4581abbfb 100644 --- a/guidance/reference_filter_dp/launch/reference_filter_dp.launch.py +++ b/guidance/reference_filter_dp/launch/reference_filter_dp.launch.py @@ -2,44 +2,32 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import OpaqueFunction from launch_ros.actions import Node -from auv_setup.launch_arg_common import ( - declare_drone_and_namespace_args, - resolve_drone_and_namespace, +config_file_path = os.path.join( + get_package_share_directory('reference_filter_dp'), + 'config', + 'reference_filter_params.yaml', ) - -def launch_setup(context, *args, **kwargs): - drone, namespace = resolve_drone_and_namespace(context) - - config_file_path = os.path.join( - get_package_share_directory("reference_filter_dp"), - "config", - "reference_filter_params.yaml", - ) - - drone_params = os.path.join( - get_package_share_directory("auv_setup"), - "config", - "robots", - f"{drone}.yaml", - ) - - return [ - Node( - package="reference_filter_dp", - executable="reference_filter_dp_node", - name="reference_filter_node", - namespace=namespace, - parameters=[config_file_path, drone_params], - output="screen", - ) - ] +orca_config = os.path.join( + get_package_share_directory('auv_setup'), + 'config', + 'robots', + 'orca.yaml', +) def generate_launch_description(): - return LaunchDescription( - declare_drone_and_namespace_args() + [OpaqueFunction(function=launch_setup)] + reference_filter_node = Node( + package='reference_filter_dp', + executable='reference_filter_dp_node', + name='reference_filter_node', + namespace='orca', + parameters=[ + config_file_path, + orca_config, + ], + output='screen', ) + return LaunchDescription([reference_filter_node]) diff --git a/guidance/reference_filter_dp/package.xml b/guidance/reference_filter_dp/package.xml index 40793be91..a7f097fde 100644 --- a/guidance/reference_filter_dp/package.xml +++ b/guidance/reference_filter_dp/package.xml @@ -13,10 +13,10 @@ rclcpp_action geometry_msgs vortex_msgs - vortex_utils - vortex_utils_ros nav_msgs eigen + tf2 + tf2_geometry_msgs ament_cmake diff --git a/guidance/reference_filter_dp/src/reference_filter.cpp b/guidance/reference_filter_dp/src/reference_filter.cpp new file mode 100644 index 000000000..4ebdc2e1d --- /dev/null +++ b/guidance/reference_filter_dp/src/reference_filter.cpp @@ -0,0 +1,38 @@ +#include + +ReferenceFilter::ReferenceFilter() + : Ad_(Matrix18d::Zero()), + Bd_(Matrix18x6d::Zero()), + Omega_(Matrix6d::Identity()), + Delta_(Matrix6d::Identity()), + identity_matrix_(Matrix6d::Identity()) {} + +Vector18d ReferenceFilter::calculate_x_dot(const Vector18d& x, + const Vector6d& r) { + Vector18d x_dot = Ad_ * x + Bd_ * r; + + return x_dot; +} + +void ReferenceFilter::calculate_Ad() { + Matrix6d OmegaCubed = Omega_ * Omega_ * Omega_; + Matrix6d OmegaSquared = Omega_ * Omega_; + Ad_.block<6, 6>(0, 6) = identity_matrix_; + Ad_.block<6, 6>(12, 0) = -OmegaCubed; + Ad_.block<6, 6>(12, 6) = -(2 * Delta_ + identity_matrix_) * OmegaSquared; + Ad_.block<6, 6>(12, 12) = -(2 * Delta_ + identity_matrix_) * Omega_; + Ad_.block<6, 6>(6, 12) = identity_matrix_; +} + +void ReferenceFilter::calculate_Bd() { + Matrix6d OmegaCubed = Omega_ * Omega_ * Omega_; + Bd_.block<6, 6>(12, 0) = OmegaCubed; +} + +void ReferenceFilter::set_omega(const Vector6d& omega) { + Omega_ = omega.asDiagonal(); +} + +void ReferenceFilter::set_delta(const Vector6d& zeta) { + Delta_ = zeta.asDiagonal(); +} diff --git a/guidance/reference_filter_dp/src/reference_filter_ros.cpp b/guidance/reference_filter_dp/src/reference_filter_ros.cpp new file mode 100644 index 000000000..22c68dfe8 --- /dev/null +++ b/guidance/reference_filter_dp/src/reference_filter_ros.cpp @@ -0,0 +1,294 @@ +#include +#include +#include + +ReferenceFilterNode::ReferenceFilterNode(const rclcpp::NodeOptions& options) + : Node("reference_filter_node", options) { + time_step_ = std::chrono::milliseconds(10); + + set_subscribers_and_publisher(); + + set_action_server(); + + set_refererence_filter(); + + x_ = Vector18d::Zero(); + + spdlog::info("Reference filter node initialized"); +} + +void ReferenceFilterNode::set_subscribers_and_publisher() { + this->declare_parameter("topics.pose"); + this->declare_parameter("topics.twist"); + this->declare_parameter("topics.guidance.dp"); + this->declare_parameter("topics.aruco_board_pose_camera"); + + std::string pose_topic = this->get_parameter("topics.pose").as_string(); + std::string twist_topic = this->get_parameter("topics.twist").as_string(); + std::string guidance_topic = + this->get_parameter("topics.guidance.dp").as_string(); + std::string aruco_board_pose_camera_topic = + this->get_parameter("topics.aruco_board_pose_camera").as_string(); + + rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; + auto qos_sensor_data = rclcpp::QoS( + rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); + reference_pub_ = this->create_publisher( + guidance_topic, qos_sensor_data); + reference_sub_ = this->create_subscription( + aruco_board_pose_camera_topic, qos_sensor_data, + std::bind(&ReferenceFilterNode::reference_callback, this, + std::placeholders::_1)); + pose_sub_ = this->create_subscription< + geometry_msgs::msg::PoseWithCovarianceStamped>( + pose_topic, qos_sensor_data, + std::bind(&ReferenceFilterNode::pose_callback, this, + std::placeholders::_1)); + twist_sub_ = this->create_subscription< + geometry_msgs::msg::TwistWithCovarianceStamped>( + twist_topic, qos_sensor_data, + std::bind(&ReferenceFilterNode::twist_callback, this, + std::placeholders::_1)); +} + +void ReferenceFilterNode::set_action_server() { + this->declare_parameter("action_servers.reference_filter"); + std::string action_server_name = + this->get_parameter("action_servers.reference_filter").as_string(); + cb_group_ = + this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + + action_server_ = rclcpp_action::create_server< + vortex_msgs::action::ReferenceFilterWaypoint>( + this, action_server_name, + std::bind(&ReferenceFilterNode::handle_goal, this, + std::placeholders::_1, std::placeholders::_2), + std::bind(&ReferenceFilterNode::handle_cancel, this, + std::placeholders::_1), + std::bind(&ReferenceFilterNode::handle_accepted, this, + std::placeholders::_1), + rcl_action_server_get_default_options(), cb_group_); +} + +void ReferenceFilterNode::set_refererence_filter() { + this->declare_parameter>( + "zeta", {0.707, 0.707, 0.707, 0.707, 0.707, 0.707}); + this->declare_parameter>( + "omega", {0.1, 0.1, 0.1, 0.1, 0.1, 0.1}); + + std::vector zeta = this->get_parameter("zeta").as_double_array(); + std::vector omega = this->get_parameter("omega").as_double_array(); + + const Vector6d zeta_eigen = Eigen::Map(zeta.data()); + const Vector6d omega_eigen = Eigen::Map(omega.data()); + + reference_filter_.set_delta(zeta_eigen); + reference_filter_.set_omega(omega_eigen); + + reference_filter_.calculate_Ad(); + reference_filter_.calculate_Bd(); +} + +void ReferenceFilterNode::reference_callback( + const geometry_msgs::msg::PoseStamped::SharedPtr msg) { + double x = msg->pose.position.x; + double y = msg->pose.position.y; + double z = msg->pose.position.z; + + tf2::Quaternion q; + q.setX(msg->pose.orientation.x); + q.setY(msg->pose.orientation.y); + q.setZ(msg->pose.orientation.z); + q.setW(msg->pose.orientation.w); + + tf2::Matrix3x3 m(q); + double roll, pitch, yaw; + m.getRPY(roll, pitch, yaw); + + r_ << x, y, z, roll, pitch, yaw; +} + +void ReferenceFilterNode::pose_callback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { + current_pose_ = *msg; +} + +void ReferenceFilterNode::twist_callback( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { + current_twist_ = *msg; +} + +rclcpp_action::GoalResponse ReferenceFilterNode::handle_goal( + const rclcpp_action::GoalUUID& uuid, + std::shared_ptr + goal) { + (void)uuid; + (void)goal; + { + std::lock_guard lock(mutex_); + if (goal_handle_) { + if (goal_handle_->is_active()) { + spdlog::info("Aborting current goal and accepting new goal"); + preempted_goal_id_ = goal_handle_->get_goal_id(); + } + } + } + spdlog::info("Accepted goal request"); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse ReferenceFilterNode::handle_cancel( + const std::shared_ptr> goal_handle) { + spdlog::info("Received request to cancel goal"); + (void)goal_handle; + return rclcpp_action::CancelResponse::ACCEPT; +} + +void ReferenceFilterNode::handle_accepted( + const std::shared_ptr> goal_handle) { + execute(goal_handle); +} + +Vector18d ReferenceFilterNode::fill_reference_state() { + Vector18d x = Vector18d::Zero(); + x(0) = current_pose_.pose.pose.position.x; + x(1) = current_pose_.pose.pose.position.y; + x(2) = current_pose_.pose.pose.position.z; + + tf2::Quaternion q; + tf2::fromMsg(current_pose_.pose.pose.orientation, q); + tf2::Matrix3x3 m(q); + double roll, pitch, yaw; + m.getRPY(roll, pitch, yaw); + + x(3) = ssa(roll); + x(4) = ssa(pitch); + x(5) = ssa(yaw); + + Vector6d eta; + eta << current_pose_.pose.pose.position.x, + current_pose_.pose.pose.position.y, current_pose_.pose.pose.position.z, + roll, pitch, yaw; + Matrix6d J = calculate_J(eta); + Vector6d nu; + nu << current_twist_.twist.twist.linear.x, + current_twist_.twist.twist.linear.y, + current_twist_.twist.twist.linear.z, + current_twist_.twist.twist.angular.x, + current_twist_.twist.twist.angular.y, + current_twist_.twist.twist.angular.z; + Vector6d eta_dot = J * nu; + + x(6) = eta_dot(0); + x(7) = eta_dot(1); + x(8) = eta_dot(2); + x(9) = eta_dot(3); + x(10) = eta_dot(4); + x(11) = eta_dot(5); + + return x; +} + +Vector6d ReferenceFilterNode::fill_reference_goal( + const geometry_msgs::msg::PoseStamped& goal) { + double x = goal.pose.position.x; + double y = goal.pose.position.y; + double z = goal.pose.position.z; + + tf2::Quaternion q_goal; + tf2::fromMsg(goal.pose.orientation, q_goal); + + tf2::Matrix3x3 m_goal(q_goal); + double roll_goal, pitch_goal, yaw_goal; + m_goal.getRPY(roll_goal, pitch_goal, yaw_goal); + + Vector6d r; + r << x, y, z, roll_goal, pitch_goal, yaw_goal; + + return r; +} + +vortex_msgs::msg::ReferenceFilter ReferenceFilterNode::fill_reference_msg() { + vortex_msgs::msg::ReferenceFilter feedback_msg; + feedback_msg.x = x_(0); + feedback_msg.y = x_(1); + feedback_msg.z = x_(2); + feedback_msg.roll = ssa(x_(3)); + feedback_msg.pitch = ssa(x_(4)); + feedback_msg.yaw = ssa(x_(5)); + feedback_msg.x_dot = x_(6); + feedback_msg.y_dot = x_(7); + feedback_msg.z_dot = x_(8); + feedback_msg.roll_dot = x_(9); + feedback_msg.pitch_dot = x_(10); + feedback_msg.yaw_dot = x_(11); + + return feedback_msg; +} + +void ReferenceFilterNode::execute( + const std::shared_ptr> goal_handle) { + { + std::lock_guard lock(mutex_); + this->goal_handle_ = goal_handle; + } + + spdlog::info("Executing goal"); + + x_ = fill_reference_state(); + + const geometry_msgs::msg::PoseStamped goal = goal_handle->get_goal()->goal; + + r_ = fill_reference_goal(goal); + + auto feedback = std::make_shared< + vortex_msgs::action::ReferenceFilterWaypoint::Feedback>(); + auto result = std::make_shared< + vortex_msgs::action::ReferenceFilterWaypoint::Result>(); + + rclcpp::Rate loop_rate(1000.0 / time_step_.count()); + + while (rclcpp::ok()) { + { + std::lock_guard lock(mutex_); + if (goal_handle->get_goal_id() == preempted_goal_id_) { + result->success = false; + goal_handle->abort(result); + return; + } + } + if (goal_handle->is_canceling()) { + result->success = false; + goal_handle->canceled(result); + spdlog::info("Goal canceled"); + return; + } + Vector18d x_dot = reference_filter_.calculate_x_dot(x_, r_); + x_ += x_dot * time_step_.count() / 1000.0; + + vortex_msgs::msg::ReferenceFilter feedback_msg = fill_reference_msg(); + + feedback->feedback = feedback_msg; + + goal_handle->publish_feedback(feedback); + reference_pub_->publish(feedback_msg); + + if ((x_.head(6) - r_.head(6)).norm() < 0.1) { + result->success = true; + goal_handle->succeed(result); + x_.head(6) = r_.head(6); + vortex_msgs::msg::ReferenceFilter feedback_msg = + fill_reference_msg(); + reference_pub_->publish(feedback_msg); + spdlog::info("Goal reached"); + return; + } + + loop_rate.sleep(); + } +} + +RCLCPP_COMPONENTS_REGISTER_NODE(ReferenceFilterNode) diff --git a/guidance/reference_filter_dp/src/reference_filter_utils.cpp b/guidance/reference_filter_dp/src/reference_filter_utils.cpp new file mode 100644 index 000000000..d7a5be273 --- /dev/null +++ b/guidance/reference_filter_dp/src/reference_filter_utils.cpp @@ -0,0 +1,55 @@ +#include + +Matrix3d calculate_R(const Vector6d& eta) { + const double roll = eta(3); + const double pitch = eta(4); + const double yaw = eta(5); + + const Eigen::AngleAxisd roll_angle(roll, Eigen::Vector3d::UnitX()); + const Eigen::AngleAxisd pitch_angle(pitch, Eigen::Vector3d::UnitY()); + const Eigen::AngleAxisd yaw_angle(yaw, Eigen::Vector3d::UnitZ()); + + const Eigen::Quaterniond q = yaw_angle * pitch_angle * roll_angle; + + return q.toRotationMatrix(); +} + +Matrix3d calculate_T(const Vector6d& eta) { + Matrix3d T; + double phi = eta(3); + double theta = eta(4); + + double cphi = cos(phi); + double sphi = sin(phi); + double ctheta = cos(theta); + double stheta = sin(theta); + + // Manually added the transformation matrix from Fossen 2021 p.29 eq: 2.41 + T(0, 0) = 1; + T(0, 1) = sphi * stheta / ctheta; + T(0, 2) = cphi * stheta / ctheta; + T(1, 0) = 0; + T(1, 1) = cphi; + T(1, 2) = -sphi; + T(2, 0) = 0; + T(2, 1) = sphi / ctheta; + T(2, 2) = cphi / ctheta; + + return T; +} + +Matrix6d calculate_J(const Vector6d& eta) { + Matrix3d R = calculate_R(eta); + Matrix3d T = calculate_T(eta); + + Matrix6d J = Matrix6d::Zero(); + J.block<3, 3>(0, 0) = R; + J.block<3, 3>(3, 3) = T; + + return J; +} + +double ssa(double angle) { + double angle_ssa = fmod(angle + M_PI, 2 * M_PI) - M_PI; + return angle_ssa; +} From 6a90ce72c420170211be5e8377796939172b29e9 Mon Sep 17 00:00:00 2001 From: Henrik Date: Sun, 29 Mar 2026 15:49:36 +0200 Subject: [PATCH 118/128] no move contructor and no move assignment --- .../velocity_controller/velocity_controller.hpp | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp index b732481b7..708b0acf8 100644 --- a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp +++ b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp @@ -10,18 +10,17 @@ #include #include #include "LQR_setup.hpp" -#include "nav_msgs/msg/odometry.hpp" +//#include "nav_msgs/msg/odometry.hpp" #include "velocity_controller/PID_setup.hpp" -#include "vortex_msgs/msg/los_guidance.hpp" +//#include "vortex_msgs/msg/los_guidance.hpp" class Velocity_node : public rclcpp_lifecycle::LifecycleNode { public: explicit Velocity_node(const rclcpp::NodeOptions& options); Velocity_node(const Velocity_node&) = delete; // no copy constructor - Velocity_node& operator=(const Velocity_node&) = - delete; // no copy assignment - // TODO: decide if i one should be allowed to move or transfer ownership if - // the class Different initialization functions + Velocity_node& operator=(const Velocity_node&) = delete; // no copy assignment + Velocity_node(Velocity_node&&) = delete; // no move constructor + Velocity_node& operator=(Velocity_node&&) = delete; // no move assignment void get_new_parameters(); // Timer functions From a42b6c39394e9f3402cf47ad80a5a6cf7b8f713c Mon Sep 17 00:00:00 2001 From: Henrik Date: Sun, 29 Mar 2026 17:30:12 +0200 Subject: [PATCH 119/128] fixed some pre commit hooks --- .pre-commit-config.yaml | 4 +- control/velocity_controller/README.md | 2 +- .../{ => include}/tests/test_VC.hpp | 2 + .../include/velocity_controller/LQR_setup.hpp | 5 +- .../include/velocity_controller/PID_setup.hpp | 3 +- .../include/velocity_controller/utilities.hpp | 2 +- .../velocity_controller.hpp | 13 +-- .../launch/VCnTest.launch.py | 4 +- control/velocity_controller/src/LQR_setup.cpp | 23 ++--- control/velocity_controller/src/PID_setup.cpp | 14 +-- control/velocity_controller/src/utilities.cpp | 94 +++++++++---------- .../src/velocity_controller.cpp | 2 +- .../velocity_controller/tests/test_LQR.cpp | 2 +- control/velocity_controller/tests/test_VC.cpp | 6 +- 14 files changed, 87 insertions(+), 89 deletions(-) rename control/velocity_controller/{ => include}/tests/test_VC.hpp (98%) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index ef82f0dfb..51b80195c 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -57,7 +57,7 @@ repos: "--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D401", "--fix", ] - stages: [commit] + stages: [pre-commit] pass_filenames: true - id: ruff name: ruff-check @@ -66,7 +66,7 @@ repos: "--ignore=T201,N812,B006,S101,S311,S607,S603", "--fix" ] - stages: [commit] + stages: [pre-commit] pass_filenames: true # C++ hooks - repo: https://github.com/pre-commit/mirrors-clang-format diff --git a/control/velocity_controller/README.md b/control/velocity_controller/README.md index fad6a11f3..c659bcdcb 100644 --- a/control/velocity_controller/README.md +++ b/control/velocity_controller/README.md @@ -195,7 +195,7 @@ ros2 launch velocity_controller VCnTest.launch.py - If the vehicle behaves oddly, check that `interval_` (the control timestep) is being set correctly — a value of `0` disables integral action silently. ## Adding new controllers -After adding the hpp file, add the calculation to calc_thrust function in a new switch case, add to the reset_controller function, with options to reset only one integral, lastly update documentation. Remember to initialize correctly, either in 'on_configure' or in constructor, add the appropriate parameters, and update alle the {drone}_params.yaml files. +After adding the hpp file, add the calculation to calc_thrust function in a new switch case, add to the reset_controller function, with options to reset only one integral, lastly update documentation. Remember to initialize correctly, either in 'on_configure' or in constructor, add the appropriate parameters, and update all the {drone}_params.yaml files. ## Adding new drones Copy a {drone}_params.yaml file and change the name to the new name of the drone. Add the appropriate matrices, and tune to satisfying behaviour. diff --git a/control/velocity_controller/tests/test_VC.hpp b/control/velocity_controller/include/tests/test_VC.hpp similarity index 98% rename from control/velocity_controller/tests/test_VC.hpp rename to control/velocity_controller/include/tests/test_VC.hpp index d79f6dbd7..3fc2ced74 100644 --- a/control/velocity_controller/tests/test_VC.hpp +++ b/control/velocity_controller/include/tests/test_VC.hpp @@ -10,6 +10,8 @@ #include "nav_msgs/msg/odometry.hpp" #include "velocity_controller/utilities.hpp" #include "vortex_msgs/msg/los_guidance.hpp" +#include + class test_VC : public rclcpp::Node { public: diff --git a/control/velocity_controller/include/velocity_controller/LQR_setup.hpp b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp index b96dba61e..85e872059 100644 --- a/control/velocity_controller/include/velocity_controller/LQR_setup.hpp +++ b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp @@ -1,10 +1,13 @@ #pragma once - #include #include #include #include #include "velocity_controller/utilities.hpp" +#include +#include + + class LQRController { public: diff --git a/control/velocity_controller/include/velocity_controller/PID_setup.hpp b/control/velocity_controller/include/velocity_controller/PID_setup.hpp index 5d6384078..11dfbd698 100644 --- a/control/velocity_controller/include/velocity_controller/PID_setup.hpp +++ b/control/velocity_controller/include/velocity_controller/PID_setup.hpp @@ -1,9 +1,10 @@ #pragma once - #include #include #include #include +#include + class PID_controller { public: diff --git a/control/velocity_controller/include/velocity_controller/utilities.hpp b/control/velocity_controller/include/velocity_controller/utilities.hpp index 11a9edf33..15069a3a3 100644 --- a/control/velocity_controller/include/velocity_controller/utilities.hpp +++ b/control/velocity_controller/include/velocity_controller/utilities.hpp @@ -24,7 +24,7 @@ class State { yaw_rate = 0.0; // roll_rate=0.0, pitch_rate=0.0, yaw_rate=0.0; double roll = 0.0, pitch = 0.0, yaw = 0.0; // phi, theta, psi double w = 0.0, x = 0.0, y = 0.0, z = 0.0; - State(double surge = 0, double pitch = 0, double yaw = 0) + explicit State(double surge = 0, double pitch = 0, double yaw = 0) : surge{surge}, pitch{pitch}, yaw{yaw} {}; State operator=(int n) { diff --git a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp index 708b0acf8..d04ad2268 100644 --- a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp +++ b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp @@ -1,5 +1,5 @@ -#pragma once - +#ifndef VELOCITY_CONTROLLER__VELOCITY_CONTROLLER_HPP_ +#define VELOCITY_CONTROLLER__VELOCITY_CONTROLLER_HPP_ #include #include #include @@ -13,6 +13,8 @@ //#include "nav_msgs/msg/odometry.hpp" #include "velocity_controller/PID_setup.hpp" //#include "vortex_msgs/msg/los_guidance.hpp" +#include +#endif // VELOCITY_CONTROLLER__VELOCITY_CONTROLLER_HPP_ class Velocity_node : public rclcpp_lifecycle::LifecycleNode { public: @@ -21,6 +23,8 @@ class Velocity_node : public rclcpp_lifecycle::LifecycleNode { Velocity_node& operator=(const Velocity_node&) = delete; // no copy assignment Velocity_node(Velocity_node&&) = delete; // no move constructor Velocity_node& operator=(Velocity_node&&) = delete; // no move assignment + + private: void get_new_parameters(); // Timer functions @@ -43,11 +47,8 @@ class Velocity_node : public rclcpp_lifecycle::LifecycleNode { subscriber_Odometry; rclcpp::Subscription::SharedPtr subscriber_guidance; - // rclcpp::Subscription::SharedPtr - // subscriber_killswitch; // Variables for topics - std::string topic_thrust; std::string topic_guidance; std::string topic_killswitch; @@ -63,7 +64,6 @@ class Velocity_node : public rclcpp_lifecycle::LifecycleNode { State current_state; geometry_msgs::msg::WrenchStamped thrust_out; - int controller_type; // 1 PID, 2 LQR // PID controllers PID_controller PID_surge; @@ -92,6 +92,7 @@ class Velocity_node : public rclcpp_lifecycle::LifecycleNode { bool odometry_dropout_guard; int publish_counter = 0; bool first_start = true; + int controller_type; // 1 PID, 2 LQR // States rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn diff --git a/control/velocity_controller/launch/VCnTest.launch.py b/control/velocity_controller/launch/VCnTest.launch.py index bca0e911f..47ab29d48 100644 --- a/control/velocity_controller/launch/VCnTest.launch.py +++ b/control/velocity_controller/launch/VCnTest.launch.py @@ -51,7 +51,7 @@ def launch_setup(context, *args, **kwargs): default_value='test_vc_node', description='Name of the test velocity controller node', ) - test_VC_name = LaunchConfiguration('node_name_1') + test_vc_name = LaunchConfiguration('node_name_1') return [ stonefish_sim, @@ -60,7 +60,7 @@ def launch_setup(context, *args, **kwargs): Node( package='velocity_controller', executable='test_vc_node', - name=test_VC_name, + name=test_vc_name, namespace=namespace, output='screen', parameters=[config_path_global], diff --git a/control/velocity_controller/src/LQR_setup.cpp b/control/velocity_controller/src/LQR_setup.cpp index 406741226..2ab02211c 100644 --- a/control/velocity_controller/src/LQR_setup.cpp +++ b/control/velocity_controller/src/LQR_setup.cpp @@ -2,20 +2,15 @@ #include #include #include -#include +//#include #include #include -#include "rclcpp/rclcpp.hpp" -// #include -// #include -// #include -// #include +//#include "rclcpp/rclcpp.hpp" #include -#include "velocity_controller/PID_setup.hpp" +//#include "velocity_controller/PID_setup.hpp" #include "velocity_controller/utilities.hpp" -// #include #include "ct/optcon/lqr/LQR.hpp" -#include "vortex/utils/math.hpp" +//#include "vortex/utils/math.hpp" LQRController::LQRController() { Q.setZero(); @@ -23,7 +18,7 @@ LQRController::LQRController() { B.setZero(); D.setZero(); inertia_matrix_inv.setZero(); -}; +} bool LQRController::set_matrices(std::vector Q_, std::vector R_, std::vector inertia_matrix_, @@ -70,7 +65,7 @@ bool LQRController::set_matrices(std::vector Q_, Eigen::Matrix B_m = Eigen::Matrix::Zero(); B_m.block<6, 3>(0, 0) = B_t; std::vector> swaplines{{1, 7}, {2, 8}, {3, 4}, {4, 5}}; - for (long unsigned int i = 0; i < swaplines.size(); i++) { + for (int64_t i = 0; i < swaplines.size(); i++) { B_m.row(swaplines[i][0]).swap(B_m.row(swaplines[i][1])); } B.block<5, 3>(0, 0) = B_m.block<5, 3>(0, 0); @@ -115,7 +110,7 @@ Eigen::Matrix LQRController::linearize(State s) { Eigen::Matrix3d::Zero(); A.block<3, 3>(6, 3) = T; std::vector> swaplines{{1, 7}, {2, 8}, {3, 4}, {4, 5}}; - for (long unsigned int i = 0; i < swaplines.size(); i++) { + for (int64_t i = 0; i < swaplines.size(); i++) { A.row(swaplines[i][0]).swap(A.row(swaplines[i][1])); A.col(swaplines[i][0]).swap(A.col(swaplines[i][1])); } @@ -126,7 +121,7 @@ Eigen::Matrix LQRController::linearize(State s) { ret.block<3, 3>(5, 0) = Eigen::Matrix3d::Identity(); return ret; -}; +} Eigen::Vector LQRController::update_error(const Guidance_data& error, const State& state) { double surge_error = error.surge; @@ -190,7 +185,7 @@ Eigen::Vector LQRController::get_thrust() { return u; } -// TODO: double check the matrices here +// TODO(henrimha): double check the matrices here Eigen::Matrix LQRController::coriolis(const State& s) { double u = s.surge; double v = s.sway; diff --git a/control/velocity_controller/src/PID_setup.cpp b/control/velocity_controller/src/PID_setup.cpp index c4ac03844..d8003b228 100644 --- a/control/velocity_controller/src/PID_setup.cpp +++ b/control/velocity_controller/src/PID_setup.cpp @@ -6,8 +6,8 @@ max_output, double min_output, double dt):Kp_(Kp), Ki_(Ki), Kd_(Kd), max_output_(max_output), min_output_(min_output), dt_(dt) { integral = 0.0; previous_error = 0.0; };*/ -// TODO: kanskje forbedre integrasjon og derivasjons beregningene -// TODO: check for more errors, f.example Nan or very high integral +// TODO(henrimha): kanskje forbedre integrasjon og derivasjons beregningene +// TODO(henrimha): check for more errors, f.example Nan or very high integral bool PID_controller::calculate_thrust(double error) { if (!init) return false; @@ -25,7 +25,7 @@ bool PID_controller::calculate_thrust(double error) { previous_error = error; return true; -}; +} bool PID_controller::calculate_thrust(double error, double error_d) { if (!init) return false; @@ -51,7 +51,7 @@ void PID_controller::reset_controller() { double PID_controller::get_output() { return output; -}; +} bool PID_controller::set_output_limits(double min_output, double max_output) { if (max_output < min_output) { @@ -60,7 +60,7 @@ bool PID_controller::set_output_limits(double min_output, double max_output) { min_output_ = min_output; max_output_ = max_output; return true; -}; +} bool PID_controller::set_parameters(double Kp, double Ki, double Kd, @@ -73,7 +73,7 @@ bool PID_controller::set_parameters(double Kp, return true; }; return false; -}; +} bool PID_controller::set_dt(double dt) { if (dt <= 0) { @@ -84,4 +84,4 @@ bool PID_controller::set_dt(double dt) { } bool PID_controller::set_parameters(std::vector& params, double dt) { return set_parameters(params.at(0), params.at(1), params.at(2), dt); -}; +} diff --git a/control/velocity_controller/src/utilities.cpp b/control/velocity_controller/src/utilities.cpp index 85d8145a5..bfe632d81 100644 --- a/control/velocity_controller/src/utilities.cpp +++ b/control/velocity_controller/src/utilities.cpp @@ -1,11 +1,10 @@ #include "velocity_controller/utilities.hpp" -#include +#include "Eigen/Dense" #include +#include #include -#include -#include "Eigen/Dense" -angle quaternion_to_euler_angle(double w, double x, double y, double z) { +angle quaternion_to_euler_angle(double w, double x, double y, double z){ double ysqr = y * y; double t0 = +2.0 * (w * x + y * z); @@ -22,34 +21,33 @@ angle quaternion_to_euler_angle(double w, double x, double y, double z) { double psi = std::atan2(t3, t4); return {phi, theta, psi}; -}; - -State State::operator=(nav_msgs::msg::Odometry::SharedPtr rhs) { - w = rhs->pose.pose.orientation.w; - x = rhs->pose.pose.orientation.x; - y = rhs->pose.pose.orientation.y; - z = rhs->pose.pose.orientation.z; - - auto [r, p, y_] = quaternion_to_euler_angle(w, x, y, z); - roll = r; - pitch = p; - yaw = y_; - - // angular velocity - roll_rate = rhs->twist.twist.angular.x; - pitch_rate = rhs->twist.twist.angular.y; - yaw_rate = rhs->twist.twist.angular.z; - // velocity +} + +State State::operator=(nav_msgs::msg::Odometry::SharedPtr rhs){ + w=rhs->pose.pose.orientation.w; + x=rhs->pose.pose.orientation.x; + y=rhs->pose.pose.orientation.y; + z=rhs->pose.pose.orientation.z; + + auto [r,p,y_]=quaternion_to_euler_angle(w, x, y, z); + roll=r; + pitch=p; + yaw=y_; + + // Angular velocity + roll_rate=rhs->twist.twist.angular.x; + pitch_rate=rhs->twist.twist.angular.y; + yaw_rate=rhs->twist.twist.angular.z; + // Velocity surge = rhs->twist.twist.linear.x; sway = rhs->twist.twist.linear.y; heave = rhs->twist.twist.linear.z; return (*this); + } -geometry_msgs::msg::Quaternion euler_angle_to_quaternion(double roll, - double pitch, - double yaw) { +geometry_msgs::msg::Quaternion euler_angle_to_quaternion(double roll, double pitch, double yaw){ double cy = cos(yaw * 0.5); double sy = sin(yaw * 0.5); double cp = cos(pitch * 0.5); @@ -66,51 +64,45 @@ geometry_msgs::msg::Quaternion euler_angle_to_quaternion(double roll, return q; } -angle angle_NED_to_body(double roll_des, - double pitch_des, - double yaw_des, - double roll, - double pitch, - double yaw) { - double cr = std::cos(roll), sr = std::sin(roll); +angle angle_NED_to_body( double roll_des, double pitch_des, double yaw_des,double roll, double pitch, double yaw) +{ + double cr = std::cos(roll), sr = std::sin(roll); double cp = std::cos(pitch), sp = std::sin(pitch); - double cy = std::cos(yaw), sy = std::sin(yaw); + double cy = std::cos(yaw), sy = std::sin(yaw); // R_current: NED to body for current attitude Eigen::Matrix3d R_current; - R_current << cp * cy, cp * sy, -sp, sr * sp * cy - cr * sy, - sr * sp * sy + cr * cy, sr * cp, cr * sp * cy + sr * sy, - cr * sp * sy - sr * cy, cr * cp; + R_current << cp*cy, cp*sy, -sp, + sr*sp*cy - cr*sy, sr*sp*sy + cr*cy, sr*cp, + cr*sp*cy + sr*sy, cr*sp*sy - sr*cy, cr*cp; - double cr_d = std::cos(roll_des), sr_d = std::sin(roll_des); + double cr_d = std::cos(roll_des), sr_d = std::sin(roll_des); double cp_d = std::cos(pitch_des), sp_d = std::sin(pitch_des); - double cy_d = std::cos(yaw_des), sy_d = std::sin(yaw_des); + double cy_d = std::cos(yaw_des), sy_d = std::sin(yaw_des); // R_desired: NED to body for desired attitude Eigen::Matrix3d R_desired; - R_desired << cp_d * cy_d, cp_d * sy_d, -sp_d, - sr_d * sp_d * cy_d - cr_d * sy_d, sr_d * sp_d * sy_d + cr_d * cy_d, - sr_d * cp_d, cr_d * sp_d * cy_d + sr_d * sy_d, - cr_d * sp_d * sy_d - sr_d * cy_d, cr_d * cp_d; + R_desired << cp_d*cy_d, cp_d*sy_d, -sp_d, + sr_d*sp_d*cy_d - cr_d*sy_d, sr_d*sp_d*sy_d + cr_d*cy_d, sr_d*cp_d, + cr_d*sp_d*cy_d + sr_d*sy_d, cr_d*sp_d*sy_d - sr_d*cy_d, cr_d*cp_d; // R_error = R_desired * R_current^T Eigen::Matrix3d R_error = R_desired * R_current.transpose(); // Extract euler angles from R_error — this gives the error in body frame double pitch_err = std::asin(-R_error(2, 0)); - double roll_err = std::atan2(R_error(2, 1), R_error(2, 2)); - double yaw_err = std::atan2(R_error(1, 0), R_error(0, 0)); + double roll_err = std::atan2(R_error(2, 1), R_error(2, 2)); + double yaw_err = std::atan2(R_error(1, 0), R_error(0, 0)); return {roll_err, pitch_err, yaw_err}; } -angle State::get_angle() { - return {roll, pitch, yaw}; +angle State::get_angle(){ + return {roll,pitch,yaw}; } -Guidance_data& Guidance_data::operator=( - const vortex_msgs::msg::LOSGuidance::SharedPtr& msg) { - surge = msg->surge; - pitch = msg->pitch; - yaw = msg->yaw; +Guidance_data& Guidance_data::operator=(const vortex_msgs::msg::LOSGuidance::SharedPtr& msg){ + surge=msg->surge; + pitch=msg->pitch; + yaw=msg->yaw; return *this; } diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index 1fd0a33c7..b02334ab3 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -66,7 +66,7 @@ void Velocity_node::calc_thrust() { return; } } - // TODO: Do I need ssa here? + // TODO(henrimha): Do I need ssa here? angle ref_in_body = angle_NED_to_body({0, vortex::utils::math::ssa(guidance_values.pitch), vortex::utils::math::ssa(guidance_values.yaw)}, diff --git a/control/velocity_controller/tests/test_LQR.cpp b/control/velocity_controller/tests/test_LQR.cpp index fdb646bd9..e224c1946 100644 --- a/control/velocity_controller/tests/test_LQR.cpp +++ b/control/velocity_controller/tests/test_LQR.cpp @@ -78,7 +78,7 @@ TEST_F(LQR_test, Direction) { value.surge = 0.2; controller.calculate_thrust(state, value); Eigen::Vector result = controller.get_thrust(); - EXPECT_TRUE(result(0) > 0); + EXPECT_GT(result(0), 0); } TEST_F(LQR_test, zero_input) { State states{}; diff --git a/control/velocity_controller/tests/test_VC.cpp b/control/velocity_controller/tests/test_VC.cpp index 40610792e..4aef803d4 100644 --- a/control/velocity_controller/tests/test_VC.cpp +++ b/control/velocity_controller/tests/test_VC.cpp @@ -1,4 +1,4 @@ -#include "test_VC.hpp" +#include "tests/test_VC.hpp" #include #include #include @@ -10,6 +10,10 @@ #include #include "velocity_controller/utilities.hpp" #include "vortex_msgs/msg/los_guidance.hpp" +#include +#include + + // Denne noden er kun for å teste velocity_controller noden From 69689522c6c430123d220a88d603d34382c7b3a6 Mon Sep 17 00:00:00 2001 From: Henrik Date: Sun, 29 Mar 2026 17:38:44 +0200 Subject: [PATCH 120/128] more precommit hooks fixes --- .../include/tests/test_VC.hpp | 3 +-- .../include/velocity_controller/LQR_setup.hpp | 6 ++--- .../include/velocity_controller/PID_setup.hpp | 1 - .../include/velocity_controller/utilities.hpp | 4 ++-- .../velocity_controller.hpp | 12 +++++----- control/velocity_controller/src/LQR_setup.cpp | 10 ++++----- control/velocity_controller/src/PID_setup.cpp | 2 +- control/velocity_controller/src/utilities.cpp | 2 +- .../src/velocity_controller.cpp | 2 +- .../velocity_controller/tests/test_LQR.cpp | 22 +++---------------- control/velocity_controller/tests/test_VC.cpp | 6 ++--- 11 files changed, 24 insertions(+), 46 deletions(-) diff --git a/control/velocity_controller/include/tests/test_VC.hpp b/control/velocity_controller/include/tests/test_VC.hpp index 3fc2ced74..fd9a2ecce 100644 --- a/control/velocity_controller/include/tests/test_VC.hpp +++ b/control/velocity_controller/include/tests/test_VC.hpp @@ -7,11 +7,10 @@ #include #include #include +#include #include "nav_msgs/msg/odometry.hpp" #include "velocity_controller/utilities.hpp" #include "vortex_msgs/msg/los_guidance.hpp" -#include - class test_VC : public rclcpp::Node { public: diff --git a/control/velocity_controller/include/velocity_controller/LQR_setup.hpp b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp index 85e872059..cdc8491ce 100644 --- a/control/velocity_controller/include/velocity_controller/LQR_setup.hpp +++ b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp @@ -3,11 +3,9 @@ #include #include #include -#include "velocity_controller/utilities.hpp" -#include #include - - +#include +#include "velocity_controller/utilities.hpp" class LQRController { public: diff --git a/control/velocity_controller/include/velocity_controller/PID_setup.hpp b/control/velocity_controller/include/velocity_controller/PID_setup.hpp index 11dfbd698..4f4166ae7 100644 --- a/control/velocity_controller/include/velocity_controller/PID_setup.hpp +++ b/control/velocity_controller/include/velocity_controller/PID_setup.hpp @@ -5,7 +5,6 @@ #include #include - class PID_controller { public: // PID_controller(double Kp=0, double Ki=0, double Kd=0, double diff --git a/control/velocity_controller/include/velocity_controller/utilities.hpp b/control/velocity_controller/include/velocity_controller/utilities.hpp index 15069a3a3..9f6f8e1d5 100644 --- a/control/velocity_controller/include/velocity_controller/utilities.hpp +++ b/control/velocity_controller/include/velocity_controller/utilities.hpp @@ -25,7 +25,7 @@ class State { double roll = 0.0, pitch = 0.0, yaw = 0.0; // phi, theta, psi double w = 0.0, x = 0.0, y = 0.0, z = 0.0; explicit State(double surge = 0, double pitch = 0, double yaw = 0) - : surge{surge}, pitch{pitch}, yaw{yaw} {}; + : surge{surge}, pitch{pitch}, yaw{yaw} {} State operator=(int n) { if (n) { @@ -34,7 +34,7 @@ class State { yaw = 0.0; } return *this; - }; + } State operator=(nav_msgs::msg::Odometry::SharedPtr rhs); angle get_angle(); }; diff --git a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp index d04ad2268..4d636d699 100644 --- a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp +++ b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp @@ -10,9 +10,9 @@ #include #include #include "LQR_setup.hpp" -//#include "nav_msgs/msg/odometry.hpp" +// #include "nav_msgs/msg/odometry.hpp" #include "velocity_controller/PID_setup.hpp" -//#include "vortex_msgs/msg/los_guidance.hpp" +// #include "vortex_msgs/msg/los_guidance.hpp" #include #endif // VELOCITY_CONTROLLER__VELOCITY_CONTROLLER_HPP_ @@ -20,11 +20,12 @@ class Velocity_node : public rclcpp_lifecycle::LifecycleNode { public: explicit Velocity_node(const rclcpp::NodeOptions& options); Velocity_node(const Velocity_node&) = delete; // no copy constructor - Velocity_node& operator=(const Velocity_node&) = delete; // no copy assignment - Velocity_node(Velocity_node&&) = delete; // no move constructor + Velocity_node& operator=(const Velocity_node&) = + delete; // no copy assignment + Velocity_node(Velocity_node&&) = delete; // no move constructor Velocity_node& operator=(Velocity_node&&) = delete; // no move assignment - private: + private: void get_new_parameters(); // Timer functions @@ -64,7 +65,6 @@ class Velocity_node : public rclcpp_lifecycle::LifecycleNode { State current_state; geometry_msgs::msg::WrenchStamped thrust_out; - // PID controllers PID_controller PID_surge; std::vector surge_params; diff --git a/control/velocity_controller/src/LQR_setup.cpp b/control/velocity_controller/src/LQR_setup.cpp index 2ab02211c..5b323bd9b 100644 --- a/control/velocity_controller/src/LQR_setup.cpp +++ b/control/velocity_controller/src/LQR_setup.cpp @@ -2,15 +2,15 @@ #include #include #include -//#include +// #include #include #include -//#include "rclcpp/rclcpp.hpp" +// #include "rclcpp/rclcpp.hpp" #include -//#include "velocity_controller/PID_setup.hpp" -#include "velocity_controller/utilities.hpp" +// #include "velocity_controller/PID_setup.hpp" #include "ct/optcon/lqr/LQR.hpp" -//#include "vortex/utils/math.hpp" +#include "velocity_controller/utilities.hpp" +// #include "vortex/utils/math.hpp" LQRController::LQRController() { Q.setZero(); diff --git a/control/velocity_controller/src/PID_setup.cpp b/control/velocity_controller/src/PID_setup.cpp index d8003b228..ad730fe63 100644 --- a/control/velocity_controller/src/PID_setup.cpp +++ b/control/velocity_controller/src/PID_setup.cpp @@ -71,7 +71,7 @@ bool PID_controller::set_parameters(double Kp, if (set_dt(dt)) { init = true; return true; - }; + } return false; } diff --git a/control/velocity_controller/src/utilities.cpp b/control/velocity_controller/src/utilities.cpp index bfe632d81..fd4125459 100644 --- a/control/velocity_controller/src/utilities.cpp +++ b/control/velocity_controller/src/utilities.cpp @@ -1,7 +1,7 @@ #include "velocity_controller/utilities.hpp" #include "Eigen/Dense" #include -#include +#include #include angle quaternion_to_euler_angle(double w, double x, double y, double z){ diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index b02334ab3..3f91eef24 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -42,7 +42,7 @@ Velocity_node::Velocity_node(const rclcpp::NodeOptions& options) 1000)) { controller_type = 1; RCLCPP_INFO(this->get_logger(), "Switching to PID"); - }; + } // Automatically start in activate if auto_start is true if (auto_start) { startup_timer_ = diff --git a/control/velocity_controller/tests/test_LQR.cpp b/control/velocity_controller/tests/test_LQR.cpp index e224c1946..b99754aa5 100644 --- a/control/velocity_controller/tests/test_LQR.cpp +++ b/control/velocity_controller/tests/test_LQR.cpp @@ -17,7 +17,7 @@ class LQR_test : public ::testing::Test { FAIL() << "Failed to load YAML from '" << YAML_PATH << "': " << e.what(); } - }; + } void SetUp() override { controller.set_matrices( cfg["/**"]["ros__parameters"]["LQR_params"]["Q"] @@ -34,15 +34,7 @@ class LQR_test : public ::testing::Test { } void TearDown() override {} }; -/* -TEST(LQR,setup){ - LQRController controller; - controller.set_interval(1); - YAML::Node cfg; - ASSERT_NO_THROW(cfg=YAML::LoadFile(YAML_PATH)); - controller.set_matrices(cfg["LQR"]["Q"],cfg["LQR"]["Q"],cfg["LQR"]["Q"],cfg["LQR"]["Q"],100); -}; -*/ + TEST_F(LQR_test, wrong_setup) { // LQRController controller; std::vector eight = {1, 2, 3, 4, 5, 6, 7, 8}; @@ -63,15 +55,7 @@ TEST_F(LQR_test, wrong_setup) { EXPECT_FALSE( controller.set_matrices(eight, three, thirty_six, -100, thirty_six)); }; -/* -TEST_F(LQR_test,solve){ - State states{1,1,1,2,2,2,1,2,1}; - Guidance_data value{1,3,2}; - Eigen::Vector result=controller.calculate_thrust(states,value); - EXPECT_NEAR(result(0),0,delta); - EXPECT_NEAR(result(1),0,delta); - EXPECT_NEAR(result(2),0,delta); -};*/ + TEST_F(LQR_test, Direction) { Guidance_data value; State state{}; diff --git a/control/velocity_controller/tests/test_VC.cpp b/control/velocity_controller/tests/test_VC.cpp index 4aef803d4..b33b157df 100644 --- a/control/velocity_controller/tests/test_VC.cpp +++ b/control/velocity_controller/tests/test_VC.cpp @@ -4,16 +4,14 @@ #include #include #include +#include #include #include #include #include +#include #include "velocity_controller/utilities.hpp" #include "vortex_msgs/msg/los_guidance.hpp" -#include -#include - - // Denne noden er kun for å teste velocity_controller noden From 6eb2d7ed5aa04abbc01cd0c4aed5a54c34bbb6f1 Mon Sep 17 00:00:00 2001 From: Henrik Date: Sun, 29 Mar 2026 17:42:17 +0200 Subject: [PATCH 121/128] final pre commit hooks --- control/velocity_controller/src/utilities.cpp | 81 ++++++++++--------- .../velocity_controller/tests/test_LQR.cpp | 2 +- 2 files changed, 45 insertions(+), 38 deletions(-) diff --git a/control/velocity_controller/src/utilities.cpp b/control/velocity_controller/src/utilities.cpp index fd4125459..31a27453c 100644 --- a/control/velocity_controller/src/utilities.cpp +++ b/control/velocity_controller/src/utilities.cpp @@ -1,10 +1,10 @@ #include "velocity_controller/utilities.hpp" -#include "Eigen/Dense" -#include #include +#include #include +#include "Eigen/Dense" -angle quaternion_to_euler_angle(double w, double x, double y, double z){ +angle quaternion_to_euler_angle(double w, double x, double y, double z) { double ysqr = y * y; double t0 = +2.0 * (w * x + y * z); @@ -23,31 +23,32 @@ angle quaternion_to_euler_angle(double w, double x, double y, double z){ return {phi, theta, psi}; } -State State::operator=(nav_msgs::msg::Odometry::SharedPtr rhs){ - w=rhs->pose.pose.orientation.w; - x=rhs->pose.pose.orientation.x; - y=rhs->pose.pose.orientation.y; - z=rhs->pose.pose.orientation.z; +State State::operator=(nav_msgs::msg::Odometry::SharedPtr rhs) { + w = rhs->pose.pose.orientation.w; + x = rhs->pose.pose.orientation.x; + y = rhs->pose.pose.orientation.y; + z = rhs->pose.pose.orientation.z; - auto [r,p,y_]=quaternion_to_euler_angle(w, x, y, z); - roll=r; - pitch=p; - yaw=y_; + auto [r, p, y_] = quaternion_to_euler_angle(w, x, y, z); + roll = r; + pitch = p; + yaw = y_; // Angular velocity - roll_rate=rhs->twist.twist.angular.x; - pitch_rate=rhs->twist.twist.angular.y; - yaw_rate=rhs->twist.twist.angular.z; + roll_rate = rhs->twist.twist.angular.x; + pitch_rate = rhs->twist.twist.angular.y; + yaw_rate = rhs->twist.twist.angular.z; // Velocity surge = rhs->twist.twist.linear.x; sway = rhs->twist.twist.linear.y; heave = rhs->twist.twist.linear.z; return (*this); - } -geometry_msgs::msg::Quaternion euler_angle_to_quaternion(double roll, double pitch, double yaw){ +geometry_msgs::msg::Quaternion euler_angle_to_quaternion(double roll, + double pitch, + double yaw) { double cy = cos(yaw * 0.5); double sy = sin(yaw * 0.5); double cp = cos(pitch * 0.5); @@ -64,45 +65,51 @@ geometry_msgs::msg::Quaternion euler_angle_to_quaternion(double roll, double pit return q; } -angle angle_NED_to_body( double roll_des, double pitch_des, double yaw_des,double roll, double pitch, double yaw) -{ - double cr = std::cos(roll), sr = std::sin(roll); +angle angle_NED_to_body(double roll_des, + double pitch_des, + double yaw_des, + double roll, + double pitch, + double yaw) { + double cr = std::cos(roll), sr = std::sin(roll); double cp = std::cos(pitch), sp = std::sin(pitch); - double cy = std::cos(yaw), sy = std::sin(yaw); + double cy = std::cos(yaw), sy = std::sin(yaw); // R_current: NED to body for current attitude Eigen::Matrix3d R_current; - R_current << cp*cy, cp*sy, -sp, - sr*sp*cy - cr*sy, sr*sp*sy + cr*cy, sr*cp, - cr*sp*cy + sr*sy, cr*sp*sy - sr*cy, cr*cp; + R_current << cp * cy, cp * sy, -sp, sr * sp * cy - cr * sy, + sr * sp * sy + cr * cy, sr * cp, cr * sp * cy + sr * sy, + cr * sp * sy - sr * cy, cr * cp; - double cr_d = std::cos(roll_des), sr_d = std::sin(roll_des); + double cr_d = std::cos(roll_des), sr_d = std::sin(roll_des); double cp_d = std::cos(pitch_des), sp_d = std::sin(pitch_des); - double cy_d = std::cos(yaw_des), sy_d = std::sin(yaw_des); + double cy_d = std::cos(yaw_des), sy_d = std::sin(yaw_des); // R_desired: NED to body for desired attitude Eigen::Matrix3d R_desired; - R_desired << cp_d*cy_d, cp_d*sy_d, -sp_d, - sr_d*sp_d*cy_d - cr_d*sy_d, sr_d*sp_d*sy_d + cr_d*cy_d, sr_d*cp_d, - cr_d*sp_d*cy_d + sr_d*sy_d, cr_d*sp_d*sy_d - sr_d*cy_d, cr_d*cp_d; + R_desired << cp_d * cy_d, cp_d * sy_d, -sp_d, + sr_d * sp_d * cy_d - cr_d * sy_d, sr_d * sp_d * sy_d + cr_d * cy_d, + sr_d * cp_d, cr_d * sp_d * cy_d + sr_d * sy_d, + cr_d * sp_d * sy_d - sr_d * cy_d, cr_d * cp_d; // R_error = R_desired * R_current^T Eigen::Matrix3d R_error = R_desired * R_current.transpose(); // Extract euler angles from R_error — this gives the error in body frame double pitch_err = std::asin(-R_error(2, 0)); - double roll_err = std::atan2(R_error(2, 1), R_error(2, 2)); - double yaw_err = std::atan2(R_error(1, 0), R_error(0, 0)); + double roll_err = std::atan2(R_error(2, 1), R_error(2, 2)); + double yaw_err = std::atan2(R_error(1, 0), R_error(0, 0)); return {roll_err, pitch_err, yaw_err}; } -angle State::get_angle(){ - return {roll,pitch,yaw}; +angle State::get_angle() { + return {roll, pitch, yaw}; } -Guidance_data& Guidance_data::operator=(const vortex_msgs::msg::LOSGuidance::SharedPtr& msg){ - surge=msg->surge; - pitch=msg->pitch; - yaw=msg->yaw; +Guidance_data& Guidance_data::operator=( + const vortex_msgs::msg::LOSGuidance::SharedPtr& msg) { + surge = msg->surge; + pitch = msg->pitch; + yaw = msg->yaw; return *this; } diff --git a/control/velocity_controller/tests/test_LQR.cpp b/control/velocity_controller/tests/test_LQR.cpp index b99754aa5..aa2330d7d 100644 --- a/control/velocity_controller/tests/test_LQR.cpp +++ b/control/velocity_controller/tests/test_LQR.cpp @@ -54,7 +54,7 @@ TEST_F(LQR_test, wrong_setup) { EXPECT_FALSE(controller.set_matrices(eight, three, thirty_six, 100, eight)); EXPECT_FALSE( controller.set_matrices(eight, three, thirty_six, -100, thirty_six)); -}; +} TEST_F(LQR_test, Direction) { Guidance_data value; From 786b98ec53c5dbe9b04b07f57967dc41f7a91fd2 Mon Sep 17 00:00:00 2001 From: Henrik Date: Sun, 29 Mar 2026 17:56:42 +0200 Subject: [PATCH 122/128] reverted guidance to main --- .../include/los_guidance/lib/adaptive_los.hpp | 57 ----- .../include/los_guidance/lib/integral_los.hpp | 55 ---- .../los_guidance/lib/proportional_los.hpp | 48 ---- .../include/los_guidance/lib/types.hpp | 60 ----- .../los_guidance/lib/vector_field_los.hpp | 51 ---- .../launch/guidance_test.launch.py | 162 ------------ .../los_guidance/scripts/test_scenarios.py | 150 ----------- .../los_guidance/src/lib/adaptive_los.cpp | 78 ------ .../los_guidance/src/lib/integral_los.cpp | 56 ---- .../los_guidance/src/lib/proportional_los.cpp | 52 ---- .../los_guidance/src/lib/vector_field_los.cpp | 55 ---- guidance/los_guidance/test/CMakeLists.txt | 29 --- .../los_guidance/test/adaptive_los_test.cpp | 173 ------------- .../los_guidance/test/integral_los_test.cpp | 108 -------- .../test/proportional_los_test.cpp | 104 -------- guidance/los_guidance/test/test_main.cpp | 8 - .../test/vector_field_los_test.cpp | 111 -------- .../lib/eigen_typedefs.hpp | 21 -- .../lib/reference_filter.hpp | 50 ---- .../lib/waypoint_follower.hpp | 83 ------ .../lib/waypoint_types.hpp | 34 --- .../lib/waypoint_utils.hpp | 42 --- .../ros/reference_filter_ros.hpp | 97 ------- .../ros/reference_filter_ros_utils.hpp | 69 ----- .../src/lib/reference_filter.cpp | 37 --- .../src/lib/waypoint_follower.cpp | 73 ------ .../src/lib/waypoint_utils.cpp | 86 ------- .../src/ros/reference_filter_ros.cpp | 239 ------------------ .../reference_filter_dp/test/CMakeLists.txt | 57 ----- .../test/test_reference_filter.cpp | 58 ----- .../test/test_waypoint_follower.cpp | 111 -------- .../test/test_waypoint_utils.cpp | 174 ------------- .../reference_filter_dp_quat/CMakeLists.txt | 105 -------- guidance/reference_filter_dp_quat/README.md | 106 -------- .../config/reference_filter_params.yaml | 5 - .../lib/eigen_typedefs.hpp | 21 -- .../lib/reference_filter.hpp | 50 ---- .../lib/waypoint_follower.hpp | 112 -------- .../ros/reference_filter_ros.hpp | 95 ------- .../ros/reference_filter_ros_utils.hpp | 58 ----- .../launch/reference_filter_dp_quat.launch.py | 64 ----- guidance/reference_filter_dp_quat/package.xml | 24 -- .../src/lib/reference_filter.cpp | 37 --- .../src/lib/waypoint_follower.cpp | 98 ------- .../src/ros/reference_filter_ros.cpp | 237 ----------------- .../test/CMakeLists.txt | 39 --- .../test/test_reference_filter.cpp | 58 ----- .../test/test_waypoint_follower.cpp | 112 -------- 48 files changed, 3809 deletions(-) delete mode 100644 guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp delete mode 100644 guidance/los_guidance/include/los_guidance/lib/integral_los.hpp delete mode 100644 guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp delete mode 100644 guidance/los_guidance/include/los_guidance/lib/types.hpp delete mode 100644 guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp delete mode 100644 guidance/los_guidance/launch/guidance_test.launch.py delete mode 100644 guidance/los_guidance/scripts/test_scenarios.py delete mode 100644 guidance/los_guidance/src/lib/adaptive_los.cpp delete mode 100644 guidance/los_guidance/src/lib/integral_los.cpp delete mode 100644 guidance/los_guidance/src/lib/proportional_los.cpp delete mode 100644 guidance/los_guidance/src/lib/vector_field_los.cpp delete mode 100644 guidance/los_guidance/test/CMakeLists.txt delete mode 100644 guidance/los_guidance/test/adaptive_los_test.cpp delete mode 100644 guidance/los_guidance/test/integral_los_test.cpp delete mode 100644 guidance/los_guidance/test/proportional_los_test.cpp delete mode 100644 guidance/los_guidance/test/test_main.cpp delete mode 100644 guidance/los_guidance/test/vector_field_los_test.cpp delete mode 100644 guidance/reference_filter_dp/include/reference_filter_dp/lib/eigen_typedefs.hpp delete mode 100644 guidance/reference_filter_dp/include/reference_filter_dp/lib/reference_filter.hpp delete mode 100644 guidance/reference_filter_dp/include/reference_filter_dp/lib/waypoint_follower.hpp delete mode 100644 guidance/reference_filter_dp/include/reference_filter_dp/lib/waypoint_types.hpp delete mode 100644 guidance/reference_filter_dp/include/reference_filter_dp/lib/waypoint_utils.hpp delete mode 100644 guidance/reference_filter_dp/include/reference_filter_dp/ros/reference_filter_ros.hpp delete mode 100644 guidance/reference_filter_dp/include/reference_filter_dp/ros/reference_filter_ros_utils.hpp delete mode 100644 guidance/reference_filter_dp/src/lib/reference_filter.cpp delete mode 100644 guidance/reference_filter_dp/src/lib/waypoint_follower.cpp delete mode 100644 guidance/reference_filter_dp/src/lib/waypoint_utils.cpp delete mode 100644 guidance/reference_filter_dp/src/ros/reference_filter_ros.cpp delete mode 100644 guidance/reference_filter_dp/test/CMakeLists.txt delete mode 100644 guidance/reference_filter_dp/test/test_reference_filter.cpp delete mode 100644 guidance/reference_filter_dp/test/test_waypoint_follower.cpp delete mode 100644 guidance/reference_filter_dp/test/test_waypoint_utils.cpp delete mode 100644 guidance/reference_filter_dp_quat/CMakeLists.txt delete mode 100644 guidance/reference_filter_dp_quat/README.md delete mode 100644 guidance/reference_filter_dp_quat/config/reference_filter_params.yaml delete mode 100644 guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/eigen_typedefs.hpp delete mode 100644 guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/reference_filter.hpp delete mode 100644 guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_follower.hpp delete mode 100644 guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros.hpp delete mode 100644 guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros_utils.hpp delete mode 100644 guidance/reference_filter_dp_quat/launch/reference_filter_dp_quat.launch.py delete mode 100644 guidance/reference_filter_dp_quat/package.xml delete mode 100644 guidance/reference_filter_dp_quat/src/lib/reference_filter.cpp delete mode 100644 guidance/reference_filter_dp_quat/src/lib/waypoint_follower.cpp delete mode 100644 guidance/reference_filter_dp_quat/src/ros/reference_filter_ros.cpp delete mode 100644 guidance/reference_filter_dp_quat/test/CMakeLists.txt delete mode 100644 guidance/reference_filter_dp_quat/test/test_reference_filter.cpp delete mode 100644 guidance/reference_filter_dp_quat/test/test_waypoint_follower.cpp diff --git a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp deleted file mode 100644 index 1f9784178..000000000 --- a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp +++ /dev/null @@ -1,57 +0,0 @@ -#ifndef LOS_GUIDANCE__LIB__ADAPTIVE_LOS_HPP_ -#define LOS_GUIDANCE__LIB__ADAPTIVE_LOS_HPP_ - -#include -#include -#include - -#include "los_guidance/lib/types.hpp" - -namespace vortex::guidance::los { - -// Parameter Structure -struct AdaptiveLosParams { - double lookahead_distance_h{}; - double lookahead_distance_v{}; - double gamma_h{}; - double gamma_v{}; - double time_step{}; -}; - -// Adaptive LOS Guidance Class -class AdaptiveLOSGuidance { - public: - // Constructor / Destructor - explicit AdaptiveLOSGuidance(const AdaptiveLosParams& params); - ~AdaptiveLOSGuidance() = default; - - // Main Output Calculation - types::Outputs calculate_outputs(const types::Inputs& inputs); - - private: - // Internal Update Functions - void update_angles(const types::Inputs& inputs); - const types::CrossTrackError calculate_crosstrack_error( - const types::Inputs& inputs); - void update_adaptive_estimates( - const types::CrossTrackError& cross_track_error); - - // Parameters - AdaptiveLosParams params_{}; - - // Rotation Matrices - Eigen::Matrix3d rotation_y_ = Eigen::Matrix3d::Zero(); - Eigen::Matrix3d rotation_z_ = Eigen::Matrix3d::Zero(); - - // Path Angles - double pi_h_{}; - double pi_v_{}; - - // Adaptive Estimates - double beta_c_hat_ = 0.0; - double alpha_c_hat_ = 0.0; -}; - -} // namespace vortex::guidance::los - -#endif // LOS_GUIDANCE__LIB__ADAPTIVE_LOS_HPP_ diff --git a/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp b/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp deleted file mode 100644 index bde7d0d5f..000000000 --- a/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp +++ /dev/null @@ -1,55 +0,0 @@ -#ifndef LOS_GUIDANCE__LIB__INTEGRAL_LOS_HPP_ -#define LOS_GUIDANCE__LIB__INTEGRAL_LOS_HPP_ - -#include -#include -#include - -#include "los_guidance/lib/types.hpp" - -namespace vortex::guidance::los { - -// Parameter Structure -struct IntegralLosParams { - double k_p_h{}; - double k_p_v{}; - double k_i_h{}; - double k_i_v{}; - double time_step{}; -}; - -// Integral LOS Guidance Class -class IntegralLOSGuidance { - public: - // Constructor / Destructor - explicit IntegralLOSGuidance(const IntegralLosParams& params); - ~IntegralLOSGuidance() = default; - - // Main Output Calculation - types::Outputs calculate_outputs(const types::Inputs& inputs); - - private: - // Internal Update Functions - void update_angles(const types::Inputs& inputs); - types::CrossTrackError calculate_crosstrack_error( - const types::Inputs& inputs); - - // Parameters - IntegralLosParams m_params{}; - - // Integral States - double int_h{}; - double int_v{}; - - // Path Angles - double pi_h_{}; - double pi_v_{}; - - // Rotation Representation - Eigen::AngleAxisd rotation_y_{0.0, Eigen::Vector3d::UnitY()}; - Eigen::AngleAxisd rotation_z_{0.0, Eigen::Vector3d::UnitZ()}; -}; - -} // namespace vortex::guidance::los - -#endif // LOS_GUIDANCE__LIB__INTEGRAL_LOS_HPP_ diff --git a/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp b/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp deleted file mode 100644 index a2e9663e1..000000000 --- a/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp +++ /dev/null @@ -1,48 +0,0 @@ -#ifndef LOS_GUIDANCE__LIB__PROPORTIONAL_LOS_HPP_ -#define LOS_GUIDANCE__LIB__PROPORTIONAL_LOS_HPP_ - -#include -#include -#include - -#include "los_guidance/lib/types.hpp" - -namespace vortex::guidance::los { - -// Parameter Structure -struct ProportionalLosParams { - double lookahead_distance_h{}; - double lookahead_distance_v{}; -}; - -// Proportional LOS Guidance Class -class ProportionalLOSGuidance { - public: - // Constructor / Destructor - explicit ProportionalLOSGuidance(const ProportionalLosParams& params); - ~ProportionalLOSGuidance() = default; - - // Main Output Calculation - types::Outputs calculate_outputs(const types::Inputs& inputs); - - private: - // Internal Update Functions - void update_angles(const types::Inputs& inputs); - types::CrossTrackError calculate_crosstrack_error( - const types::Inputs& inputs) const; - - // Parameters - ProportionalLosParams m_params{}; - - // Path Angles - double pi_h_{0.0}; - double pi_v_{0.0}; - - // Rotation Representation - Eigen::AngleAxisd rotation_y_{0.0, Eigen::Vector3d::UnitY()}; - Eigen::AngleAxisd rotation_z_{0.0, Eigen::Vector3d::UnitZ()}; -}; - -} // namespace vortex::guidance::los - -#endif // LOS_GUIDANCE__LIB__PROPORTIONAL_LOS_HPP_ diff --git a/guidance/los_guidance/include/los_guidance/lib/types.hpp b/guidance/los_guidance/include/los_guidance/lib/types.hpp deleted file mode 100644 index 52aab3d48..000000000 --- a/guidance/los_guidance/include/los_guidance/lib/types.hpp +++ /dev/null @@ -1,60 +0,0 @@ -#ifndef LOS_GUIDANCE__LIB__TYPES_HPP_ -#define LOS_GUIDANCE__LIB__TYPES_HPP_ - -#include -#include -#include -#include -#include - -namespace vortex::guidance::los::types { - -// Point Representation -struct Point { - double x{}; - double y{}; - double z{}; - - // Point Operations - Point operator-(const Point& other) const { - return Point{x - other.x, y - other.y, z - other.z}; - } - - // Conversion Functions - Eigen::Vector3d as_vector() const { return Eigen::Vector3d(x, y, z); } - - static Point point_from_ros(const geometry_msgs::msg::Point& msg) { - return Point{msg.x, msg.y, msg.z}; - } -}; - -// Cross Track Error -struct CrossTrackError { - double x_e{}; - double y_e{}; - double z_e{}; - - inline static CrossTrackError from_vector(const Eigen::Vector3d& vector) { - return CrossTrackError{vector.x(), vector.y(), vector.z()}; - } -}; - -// Guidance Outputs -struct Outputs { - double psi_d{}; - double theta_d{}; -}; - -// Guidance Inputs -struct Inputs { - Point prev_point{}; - Point next_point{}; - Point current_position{}; -}; - -// Active LOS Method -enum class ActiveLosMethod { PROPORTIONAL, INTEGRAL, ADAPTIVE, VECTOR_FIELD }; - -} // namespace vortex::guidance::los::types - -#endif // LOS_GUIDANCE__LIB__TYPES_HPP_ diff --git a/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp b/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp deleted file mode 100644 index 60d332929..000000000 --- a/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp +++ /dev/null @@ -1,51 +0,0 @@ -#ifndef LOS_GUIDANCE__LIB__VECTOR_FIELD_LOS_HPP_ -#define LOS_GUIDANCE__LIB__VECTOR_FIELD_LOS_HPP_ - -#include -#include -#include - -#include "los_guidance/lib/types.hpp" - -namespace vortex::guidance::los { - -// Parameter Structure -struct VectorFieldLosParams { - double max_approach_angle_h{}; - double max_approach_angle_v{}; - double k_p_h{}; - double k_p_v{}; - double time_step{}; -}; - -// Vector Field LOS Guidance Class -class VectorFieldLOSGuidance { - public: - // Constructor / Destructor - explicit VectorFieldLOSGuidance(const VectorFieldLosParams& params); - ~VectorFieldLOSGuidance() = default; - - // Main Output Calculation - types::Outputs calculate_outputs(const types::Inputs& inputs); - - private: - // Internal Update Functions - void update_angles(const types::Inputs& inputs); - types::CrossTrackError calculate_crosstrack_error( - const types::Inputs& inputs) const; - - // Parameters - VectorFieldLosParams m_params{}; - - // Path Angles - double pi_h_{0.0}; - double pi_v_{0.0}; - - // Rotation Representation - Eigen::AngleAxisd rotation_y_{0.0, Eigen::Vector3d::UnitY()}; - Eigen::AngleAxisd rotation_z_{0.0, Eigen::Vector3d::UnitZ()}; -}; - -} // namespace vortex::guidance::los - -#endif // LOS_GUIDANCE__LIB__VECTOR_FIELD_LOS_HPP_ diff --git a/guidance/los_guidance/launch/guidance_test.launch.py b/guidance/los_guidance/launch/guidance_test.launch.py deleted file mode 100644 index fa65290e6..000000000 --- a/guidance/los_guidance/launch/guidance_test.launch.py +++ /dev/null @@ -1,162 +0,0 @@ -import os - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import ( - DeclareLaunchArgument, - ExecuteProcess, - IncludeLaunchDescription, - OpaqueFunction, - TimerAction, -) -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration - -from auv_setup.launch_arg_common import ( - declare_drone_and_namespace_args, - resolve_drone_and_namespace, -) - - -def launch_setup(context, *args, **kwargs): - drone, namespace = resolve_drone_and_namespace(context) - test_scenario = LaunchConfiguration("test_scenario").perform(context) - - stonefish_dir = get_package_share_directory("stonefish_sim") - vortex_sim_interface_dir = get_package_share_directory("vortex_sim_interface") - los_guidance_dir = get_package_share_directory("los_guidance") - operation_mode_manager_dir = get_package_share_directory("operation_mode_manager") - velocity_controller_dir = get_package_share_directory('velocity_controller') - - stonefish_sim = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(stonefish_dir, "launch", "simulation.launch.py") - ), - launch_arguments={ - "scenario": "default", - "rendering": "true", - }.items(), - ) - - vortex_sim_interface = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join( - vortex_sim_interface_dir, "launch", "vortex_sim_interface.launch.py" - ) - ), - launch_arguments={ - "drone": drone, - "namespace": namespace, - }.items(), - ) - - operation_mode_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join( - operation_mode_manager_dir, - "launch", - "operation_mode_manager.launch.py", - ) - ), - launch_arguments={ - "drone": drone, - "namespace": namespace, - }.items(), - ) - - los_guidance_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(los_guidance_dir, "launch", "los_guidance.launch.py") - ), - launch_arguments={ - "drone": drone, - "namespace": namespace, - }.items(), - ) - - velocity_controller_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join( - velocity_controller_dir, 'launch', 'velocity_controller.launch.py' - ) - ), - launch_arguments={ - "drone": drone, - "namespace": namespace, - }.items(), - ) - - orca_sim = TimerAction( - period=12.0, - actions=[ - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(stonefish_dir, "launch", "drone_sim.launch.py") - ) - ) - ], - ) - - set_autonomy = TimerAction( - period=12.0, - actions=[ - ExecuteProcess( - cmd=[ - "bash", - "-lc", - ( - f"ros2 service call /{namespace}/set_killswitch " - "vortex_msgs/srv/SetKillswitch " - "\"{killswitch_on: false}\" " - "&& " - f"ros2 service call /{namespace}/set_operation_mode " - "vortex_msgs/srv/SetOperationMode " - "\"{requested_operation_mode: {operation_mode: 1}}\"" - ), - ], - output="screen", - ), - ], - ) - - run_test_scenario = TimerAction( - period=20.0, - actions=[ - ExecuteProcess( - cmd=[ - "bash", - "-lc", - ( - f"python3 {os.path.join(los_guidance_dir, 'scripts', 'test_scenarios.py')} " - f"--ros-args -p drone:={drone} -p test_scenario:={test_scenario}" - ), - ], - output="screen", - ) - ], - ) - - return [ - stonefish_sim, - # vortex_sim_interface, - # operation_mode_launch, - los_guidance_launch, - velocity_controller_launch, - orca_sim, - set_autonomy, - run_test_scenario, - ] - - -def generate_launch_description(): - return LaunchDescription( - declare_drone_and_namespace_args(default_drone="orca") - + [ - DeclareLaunchArgument( - "test_scenario", - default_value="square", - description="Scenario to run: square, circle, test_pitch, opposite_point", - ), - OpaqueFunction(function=launch_setup), - ] - ) diff --git a/guidance/los_guidance/scripts/test_scenarios.py b/guidance/los_guidance/scripts/test_scenarios.py deleted file mode 100644 index 95edef2d6..000000000 --- a/guidance/los_guidance/scripts/test_scenarios.py +++ /dev/null @@ -1,150 +0,0 @@ -import math - -import rclpy -from rclpy.action import ActionClient -from rclpy.node import Node -from std_msgs.msg import Header -from vortex_msgs.action import LOSGuidance - - -class WaypointTest(Node): - def __init__(self): - super().__init__("waypoint_test_client") - - self.declare_parameter("test_scenario", "square") - self.declare_parameter("drone", "orca") - - self.test_scenario = self.get_parameter("test_scenario").value - self.drone = self.get_parameter("drone").value - - self._action_client = ActionClient( - self, - LOSGuidance, - f"/{self.drone}/los_guidance", - ) - - self.depth = 2.5 - self.square_size = 10.0 - - self.circle_radius = 8.0 - self.circle_points = 16 - self.circle_center_x = 0.0 - self.circle_center_y = 0.0 - - self.waypoints = self.generate_waypoints(self.test_scenario) - self.current_index = 0 - - self.get_logger().info(f"Starting test scenario: {self.test_scenario}") - self.get_logger().info(f"Using drone namespace: {self.drone}") - self.get_logger().info(f"Number of waypoints: {len(self.waypoints)}") - - self.send_next_goal() - - def generate_waypoints(self, test_scenario): - if test_scenario == "square": - s = self.square_size - d = self.depth - return [ - (s, 0.0, d), - (s, s, d), - (0.0, s, d), - (0.0, 0.0, d), - ] - - elif test_scenario == "circle": - d = self.depth - waypoints = [] - - for i in range(self.circle_points): - theta = 2.0 * math.pi * i / self.circle_points - x = self.circle_center_x + self.circle_radius * math.cos(theta) - y = self.circle_center_y + self.circle_radius * math.sin(theta) - waypoints.append((x, y, d)) - - waypoints.append(waypoints[0]) - return waypoints - - elif test_scenario == "test_pitch": - # 0 = water surface, do not go above - # 3 = seabed/ground, do not touch - # Keep all depths safely between these - return [ - (3.0, 0.0, 1.0), # slight up - (6.0, 0.0, 2.0), # slight down - (9.0, 0.0, 1.0), # up again - (12.0, 0.0, 2.0), # down again - ] - - elif test_scenario == "opposite_point": - # Go to one point, then the exact opposite point - return [ - (6.0, 4.0, self.depth), - (-6.0, -4.0, self.depth), - ] - - else: - self.get_logger().warn( - f"Unknown test_scenario '{test_scenario}', defaulting to square" - ) - return self.generate_waypoints("square") - - def send_next_goal(self): - if self.current_index >= len(self.waypoints): - self.get_logger().info(f"{self.test_scenario} test completed!") - rclpy.shutdown() - return - - self._action_client.wait_for_server() - - goal_msg = LOSGuidance.Goal() - - header = Header() - header.frame_id = "world_ned" - goal_msg.goal.header = header - - x, y, z = self.waypoints[self.current_index] - goal_msg.goal.point.x = float(x) - goal_msg.goal.point.y = float(y) - goal_msg.goal.point.z = float(z) - - self.get_logger().info( - f"Sending waypoint {self.current_index + 1}/{len(self.waypoints)}: " - f"x={x:.2f}, y={y:.2f}, z={z:.2f}" - ) - - self._send_goal_future = self._action_client.send_goal_async( - goal_msg, - feedback_callback=self.feedback_callback, - ) - self._send_goal_future.add_done_callback(self.goal_response_callback) - - def goal_response_callback(self, future): - goal_handle = future.result() - - if not goal_handle.accepted: - self.get_logger().error("Goal rejected") - rclpy.shutdown() - return - - self.get_logger().info("Goal accepted") - - self._get_result_future = goal_handle.get_result_async() - self._get_result_future.add_done_callback(self.result_callback) - - def result_callback(self, future): - self.get_logger().info("Waypoint reached") - self.current_index += 1 - self.send_next_goal() - - def feedback_callback(self, feedback_msg): - pass - - -def main(args=None): - rclpy.init(args=args) - node = WaypointTest() - rclpy.spin(node) - - -if __name__ == "__main__": - main() diff --git a/guidance/los_guidance/src/lib/adaptive_los.cpp b/guidance/los_guidance/src/lib/adaptive_los.cpp deleted file mode 100644 index db6177b1c..000000000 --- a/guidance/los_guidance/src/lib/adaptive_los.cpp +++ /dev/null @@ -1,78 +0,0 @@ -#include - -#include "los_guidance/lib/types.hpp" - -namespace vortex::guidance::los { - -// Constructor -AdaptiveLOSGuidance::AdaptiveLOSGuidance(const AdaptiveLosParams& params) - : params_{params} {} - -// Angle Update -void AdaptiveLOSGuidance::update_angles(const types::Inputs& inputs) { - const double dx = inputs.next_point.x - inputs.prev_point.x; - const double dy = inputs.next_point.y - inputs.prev_point.y; - const double dz = inputs.next_point.z - inputs.prev_point.z; - - pi_h_ = std::atan2(dy, dx); - pi_v_ = std::atan2(-dz, std::sqrt(dx * dx + dy * dy)); - - rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); - rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); -} - -// Cross-Track Error Calculation -const types::CrossTrackError AdaptiveLOSGuidance::calculate_crosstrack_error( - const types::Inputs& inputs) { - const types::Point difference = inputs.current_position - inputs.prev_point; - const Eigen::Vector3d difference_vector = difference.as_vector(); - - const Eigen::Vector3d cross_track_error = - rotation_y_.transpose() * rotation_z_.transpose() * difference_vector; - - return types::CrossTrackError::from_vector(cross_track_error); -} - -// Adaptive Estimate Update -void AdaptiveLOSGuidance::update_adaptive_estimates( - const types::CrossTrackError& cross_track_error) { - const double denom_h = - std::sqrt(params_.lookahead_distance_h * params_.lookahead_distance_h + - cross_track_error.y_e * cross_track_error.y_e); - const double denom_v = - std::sqrt(params_.lookahead_distance_v * params_.lookahead_distance_v + - cross_track_error.z_e * cross_track_error.z_e); - - const double beta_dot = params_.gamma_h * - (params_.lookahead_distance_h / denom_h) * - cross_track_error.y_e; - const double alpha_dot = params_.gamma_v * - (params_.lookahead_distance_v / denom_v) * - cross_track_error.z_e; - - beta_c_hat_ += beta_dot * params_.time_step; - alpha_c_hat_ += alpha_dot * params_.time_step; -} - -// Output Calculation -types::Outputs AdaptiveLOSGuidance::calculate_outputs( - const types::Inputs& inputs) { - update_angles(inputs); - - const types::CrossTrackError cross_track_error = - calculate_crosstrack_error(inputs); - - update_adaptive_estimates(cross_track_error); - - const double psi_d = - pi_h_ - beta_c_hat_ - - std::atan(cross_track_error.y_e / params_.lookahead_distance_h); - - const double theta_d = - pi_v_ + alpha_c_hat_ + - std::atan(cross_track_error.z_e / params_.lookahead_distance_v); - - return types::Outputs{psi_d, theta_d}; -} - -} // namespace vortex::guidance::los diff --git a/guidance/los_guidance/src/lib/integral_los.cpp b/guidance/los_guidance/src/lib/integral_los.cpp deleted file mode 100644 index c8f0b4d59..000000000 --- a/guidance/los_guidance/src/lib/integral_los.cpp +++ /dev/null @@ -1,56 +0,0 @@ -#include - -namespace vortex::guidance::los { - -// Constructor -IntegralLOSGuidance::IntegralLOSGuidance(const IntegralLosParams& params) - : m_params{params} {} - -// Angle Update -void IntegralLOSGuidance::update_angles(const types::Inputs& inputs) { - const types::Point difference = inputs.next_point - inputs.prev_point; - - pi_h_ = std::atan2(difference.y, difference.x); - pi_v_ = std::atan2(-difference.z, std::sqrt(difference.x * difference.x + - difference.y * difference.y)); - - rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); - rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); -} - -// Cross-Track Error Calculation -types::CrossTrackError IntegralLOSGuidance::calculate_crosstrack_error( - const types::Inputs& inputs) { - const Eigen::Vector3d diff_vec = - (inputs.current_position - inputs.prev_point).as_vector(); - - const Eigen::Vector3d path_frame_error = - rotation_y_.toRotationMatrix().transpose() * - rotation_z_.toRotationMatrix().transpose() * diff_vec; - - return types::CrossTrackError::from_vector(path_frame_error); -} - -// Output Calculation -types::Outputs IntegralLOSGuidance::calculate_outputs( - const types::Inputs& inputs) { - update_angles(inputs); - - const types::CrossTrackError cross_track_error = - calculate_crosstrack_error(inputs); - - int_h += cross_track_error.y_e * m_params.time_step; - int_v += cross_track_error.z_e * m_params.time_step; - - const double u_h = - m_params.k_p_h * cross_track_error.y_e + m_params.k_i_h * int_h; - const double u_v = - m_params.k_p_v * cross_track_error.z_e + m_params.k_i_v * int_v; - - const double psi_d = pi_h_ - std::atan(u_h); - const double theta_d = pi_v_ + std::atan(u_v); - - return types::Outputs{psi_d, theta_d}; -} - -} // namespace vortex::guidance::los diff --git a/guidance/los_guidance/src/lib/proportional_los.cpp b/guidance/los_guidance/src/lib/proportional_los.cpp deleted file mode 100644 index f76df385c..000000000 --- a/guidance/los_guidance/src/lib/proportional_los.cpp +++ /dev/null @@ -1,52 +0,0 @@ -#include - -namespace vortex::guidance::los { - -// Constructor -ProportionalLOSGuidance::ProportionalLOSGuidance( - const ProportionalLosParams& params) - : m_params{params} {} - -// Angle Update -void ProportionalLOSGuidance::update_angles(const types::Inputs& inputs) { - const types::Point difference = inputs.next_point - inputs.prev_point; - - pi_h_ = std::atan2(difference.y, difference.x); - pi_v_ = std::atan2(-difference.z, std::sqrt(difference.x * difference.x + - difference.y * difference.y)); - - rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); - rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); -} - -// Cross-Track Error Calculation -types::CrossTrackError ProportionalLOSGuidance::calculate_crosstrack_error( - const types::Inputs& inputs) const { - const Eigen::Vector3d diff_vec = - (inputs.current_position - inputs.prev_point).as_vector(); - - const Eigen::Vector3d path_frame_error = - rotation_y_.toRotationMatrix().transpose() * - rotation_z_.toRotationMatrix().transpose() * diff_vec; - - return types::CrossTrackError::from_vector(path_frame_error); -} - -// Output Calculation -types::Outputs ProportionalLOSGuidance::calculate_outputs( - const types::Inputs& inputs) { - update_angles(inputs); - - const types::CrossTrackError cross_track_error = - calculate_crosstrack_error(inputs); - - const double k_p_h = 1.0 / m_params.lookahead_distance_h; - const double k_p_v = 1.0 / m_params.lookahead_distance_v; - - const double psi_d = pi_h_ - std::atan(k_p_h * cross_track_error.y_e); - const double theta_d = pi_v_ + std::atan(k_p_v * cross_track_error.z_e); - - return types::Outputs{psi_d, theta_d}; -} - -} // namespace vortex::guidance::los diff --git a/guidance/los_guidance/src/lib/vector_field_los.cpp b/guidance/los_guidance/src/lib/vector_field_los.cpp deleted file mode 100644 index edf352a6e..000000000 --- a/guidance/los_guidance/src/lib/vector_field_los.cpp +++ /dev/null @@ -1,55 +0,0 @@ -#include - -namespace vortex::guidance::los { - -// Constructor -VectorFieldLOSGuidance::VectorFieldLOSGuidance( - const VectorFieldLosParams& params) - : m_params{params} {} - -// Angle Update -void VectorFieldLOSGuidance::update_angles(const types::Inputs& inputs) { - const types::Point difference = inputs.next_point - inputs.prev_point; - - pi_h_ = std::atan2(difference.y, difference.x); - pi_v_ = std::atan2(-difference.z, std::sqrt(difference.x * difference.x + - difference.y * difference.y)); - - rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); - rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); -} - -// Cross-Track Error Calculation -types::CrossTrackError VectorFieldLOSGuidance::calculate_crosstrack_error( - const types::Inputs& inputs) const { - const Eigen::Vector3d diff_vec = - (inputs.current_position - inputs.prev_point).as_vector(); - - const Eigen::Vector3d path_frame_error = - rotation_y_.toRotationMatrix().transpose() * - rotation_z_.toRotationMatrix().transpose() * diff_vec; - - return types::CrossTrackError::from_vector(path_frame_error); -} - -// Output Calculation -types::Outputs VectorFieldLOSGuidance::calculate_outputs( - const types::Inputs& inputs) { - update_angles(inputs); - - const types::CrossTrackError cross_track_error = - calculate_crosstrack_error(inputs); - - const double approach_h = m_params.max_approach_angle_h * (2.0 / M_PI) * - std::atan(m_params.k_p_h * cross_track_error.y_e); - - const double approach_v = m_params.max_approach_angle_v * (2.0 / M_PI) * - std::atan(m_params.k_p_v * cross_track_error.z_e); - - const double psi_d = pi_h_ - approach_h; - const double theta_d = pi_v_ - approach_v; - - return types::Outputs{psi_d, theta_d}; -} - -} // namespace vortex::guidance::los diff --git a/guidance/los_guidance/test/CMakeLists.txt b/guidance/los_guidance/test/CMakeLists.txt deleted file mode 100644 index c0cdc1ae7..000000000 --- a/guidance/los_guidance/test/CMakeLists.txt +++ /dev/null @@ -1,29 +0,0 @@ -cmake_minimum_required(VERSION 3.8) - -find_package(GTest REQUIRED) -include(GoogleTest) - -set(TEST_BINARY_NAME ${PROJECT_NAME}_test) -add_executable( - ${TEST_BINARY_NAME} - test_main.cpp - adaptive_los_test.cpp - proportional_los_test.cpp - integral_los_test.cpp - vector_field_los_test.cpp - -) - -target_link_libraries(${TEST_BINARY_NAME} - PRIVATE - ${LIB_NAME} - yaml-cpp - Eigen3::Eigen - spdlog::spdlog - fmt::fmt - GTest::GTest -) - -ament_target_dependencies(${TEST_BINARY_NAME} PUBLIC Eigen3) - -gtest_discover_tests(${TEST_BINARY_NAME}) diff --git a/guidance/los_guidance/test/adaptive_los_test.cpp b/guidance/los_guidance/test/adaptive_los_test.cpp deleted file mode 100644 index 5fd71027a..000000000 --- a/guidance/los_guidance/test/adaptive_los_test.cpp +++ /dev/null @@ -1,173 +0,0 @@ -#include "los_guidance/lib/adaptive_los.hpp" -#include - -namespace vortex::guidance::los { // new namespace added los - -class AdaptiveLosTest : public ::testing::Test { - protected: - AdaptiveLosTest() : los_{get_params()} {} - - AdaptiveLosParams get_params() { - AdaptiveLosParams p; - p.lookahead_distance_h = 1.0; - p.lookahead_distance_v = 1.0; - p.gamma_h = 1.0; - p.gamma_v = 1.0; - p.time_step = 0.01; - return p; - } - - AdaptiveLOSGuidance los_; - const double tol = 1e-9; -}; -/* -TEST_F(AdaptiveLosTest, T01_test_cross_track_error_on_track){ - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.0, 0.0}; - - const types::Output O = los_.calculate_outputs(inputs); - - EXPECT_NEAR(e.x_e, 0.0, tol); - EXPECT_NEAR(e.y_e, 0.0, tol); - EXPECT_NEAR(e.z_e, 0.0, tol); -} - -TEST_F(AdaptiveLosTests, T02_test_cross_track_error_right_off_track) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.5, 0.0}; - - const types::Output O = los_.calculate_outputs(inputs); - - EXPECT_NEAR(e.x_e, 0.0, tol); - EXPECT_NEAR(e.y_e, 0.5, tol); - EXPECT_NEAR(e.z_e, 0.0, tol); -} - -TEST_F(AdaptiveLosTests, T03_test_cross_track_error_left_off_track) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, -0.5, 0.0}; - - const types::Output O = los_.calculate_outputs(inputs); - - EXPECT_NEAR(e.x_e, 0.0, tol); - EXPECT_NEAR(e.y_e, -0.5, tol); - EXPECT_NEAR(e.z_e, 0.0, tol); -} - -TEST_F(AdaptiveLosTests, T04_test_cross_track_error_under_track) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.0, 0.5}; - - const types::Output O = los_.calculate_outputs(inputs); - - EXPECT_NEAR(e.x_e, 0.0, tol); - EXPECT_NEAR(e.y_e, 0.0, tol); - EXPECT_NEAR(e.z_e, 0.5, tol); -} - -TEST_F(AdaptiveLosTests, T05_test_cross_track_error_over_track) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.0, -0.5}; - - const types::Output O = los_.calculate_outputs(inputs); - - EXPECT_NEAR(e.x_e, 0.0, tol); - EXPECT_NEAR(e.y_e, 0.0, tol); - EXPECT_NEAR(e.z_e, -0.5, tol); -} -*/ - -// Test commanded angles when drone is to the right of the track -TEST_F(AdaptiveLosTest, T06_test_commanded_angles) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.5, 0.0}; - - const types::Outputs O = los_.calculate_outputs(inputs); - - // Heading cmd should be between -pi/2 and 0 - EXPECT_LT(O.psi_d, 0.0); - EXPECT_GT(O.psi_d, -1.57); - - // Pitch cmd should be zero - EXPECT_NEAR(O.theta_d, 0.0, tol); -} - -// Test commanded angles when drone is to the left of the track -TEST_F(AdaptiveLosTest, T07_test_commanded_angles) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, -0.5, 0.0}; - - const types::Outputs O = los_.calculate_outputs(inputs); - - // Heading cmd should be between 0 and pi/2 - EXPECT_GT(O.psi_d, 0.0); - EXPECT_LT(O.psi_d, 1.57); - // Pitch cmd should be zero - EXPECT_NEAR(O.theta_d, 0.0, tol); -} - -// Test commanded angles when drone is under the track -TEST_F(AdaptiveLosTest, T08_test_commanded_angles) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.0, 0.5}; - - const types::Outputs O = los_.calculate_outputs(inputs); - - // Heading cmd should be 0 - EXPECT_NEAR(O.psi_d, 0.0, tol); - // Pitch cmd should be between 0 and pi/2 - EXPECT_GT(O.theta_d, 0.0); - EXPECT_LT(O.theta_d, 1.57); -} - -// Test commanded angles when drone is over the track -TEST_F(AdaptiveLosTest, T09_test_commanded_angles) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.0, -0.5}; - - const types::Outputs O = los_.calculate_outputs(inputs); - - // Heading cmd should be 0 - EXPECT_NEAR(O.psi_d, 0.0, tol); - // Pitch cmd should be between -pi/2 and 0 - EXPECT_LT(O.theta_d, 0.0); - EXPECT_GT(O.theta_d, -1.57); -} - -// Test commanded angles when drone is over and to the right of the track - -TEST_F(AdaptiveLosTest, T10_test_commanded_angles) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.5, -0.5}; - - const types::Outputs O = los_.calculate_outputs(inputs); - - // Heading cmd should be between -pi/2 and 0 - EXPECT_LT(O.psi_d, 0.0); - EXPECT_GT(O.psi_d, -1.57); - // Pitch cmd should be between -pi/2 and 0 - EXPECT_LT(O.theta_d, 0.0); - EXPECT_GT(O.theta_d, -1.57); -} - -} // namespace vortex::guidance::los diff --git a/guidance/los_guidance/test/integral_los_test.cpp b/guidance/los_guidance/test/integral_los_test.cpp deleted file mode 100644 index 1999abac8..000000000 --- a/guidance/los_guidance/test/integral_los_test.cpp +++ /dev/null @@ -1,108 +0,0 @@ -#include "los_guidance/lib/integral_los.hpp" -#include -#include "los_guidance/lib/adaptive_los.hpp" - -namespace vortex::guidance::los { - -class IntegralLosTest : public ::testing::Test { - protected: - IntegralLosTest() : Ilos_{get_params()} {} - - IntegralLosParams get_params() { - IntegralLosParams params; - params.k_i_h = 0.1; // needs tuning - params.k_i_v = 0.1; // needs tuning - params.k_p_h = 0.667; // needs tuning - params.k_p_v = 0.582; // needs tuning - params.time_step = 0.01; - return params; - } - - IntegralLOSGuidance Ilos_; - const double tol = 1e-9; -}; - -// Test commanded angles when drone is to the right of the track -TEST_F(IntegralLosTest, T06_test_commanded_angles) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.5, 0.0}; - - const types::Outputs O = Ilos_.calculate_outputs(inputs); - - // Heading cmd should be between -pi/2 and 0 - EXPECT_LT(O.psi_d, 0.0); - EXPECT_GT(O.psi_d, -1.57); - - // Pitch cmd should be zero - EXPECT_NEAR(O.theta_d, 0.0, tol); -} - -// Test commanded angles when drone is to the left of the track -TEST_F(IntegralLosTest, T07_test_commanded_angles) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, -0.5, 0.0}; - - const types::Outputs O = Ilos_.calculate_outputs(inputs); - - // Heading cmd should be between 0 and pi/2 - EXPECT_GT(O.psi_d, 0.0); - EXPECT_LT(O.psi_d, 1.57); - // Pitch cmd should be zero - EXPECT_NEAR(O.theta_d, 0.0, tol); -} - -// Test commanded angles when drone is under the track -TEST_F(IntegralLosTest, T08_test_commanded_angles) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.0, 0.5}; - - const types::Outputs O = Ilos_.calculate_outputs(inputs); - - // Heading cmd should be 0 - EXPECT_NEAR(O.psi_d, 0.0, tol); - // Pitch cmd should be between 0 and pi/2 - EXPECT_GT(O.theta_d, 0.0); - EXPECT_LT(O.theta_d, 1.57); -} - -// Test commanded angles when drone is over the track -TEST_F(IntegralLosTest, T09_test_commanded_angles) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.0, -0.5}; - - const types::Outputs O = Ilos_.calculate_outputs(inputs); - - // Heading cmd should be 0 - EXPECT_NEAR(O.psi_d, 0.0, tol); - // Pitch cmd should be between -pi/2 and 0 - EXPECT_LT(O.theta_d, 0.0); - EXPECT_GT(O.theta_d, -1.57); -} - -// Test commanded angles when drone is over and to the right of the track - -TEST_F(IntegralLosTest, T10_test_commanded_angles) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.5, -0.5}; - - const types::Outputs O = Ilos_.calculate_outputs(inputs); - - // Heading cmd should be between -pi/2 and 0 - EXPECT_LT(O.psi_d, 0.0); - EXPECT_GT(O.psi_d, -1.57); - // Pitch cmd should be between -pi/2 and 0 - EXPECT_LT(O.theta_d, 0.0); - EXPECT_GT(O.theta_d, -1.57); -} - -} // namespace vortex::guidance::los diff --git a/guidance/los_guidance/test/proportional_los_test.cpp b/guidance/los_guidance/test/proportional_los_test.cpp deleted file mode 100644 index b35a3ba6f..000000000 --- a/guidance/los_guidance/test/proportional_los_test.cpp +++ /dev/null @@ -1,104 +0,0 @@ -#include "los_guidance/lib/proportional_los.hpp" -#include - -namespace vortex::guidance::los { - -class ProportionalLosTest : public ::testing::Test { - protected: - ProportionalLosTest() : Plos_{get_params()} {} - - ProportionalLosParams get_params() { - ProportionalLosParams params; - params.lookahead_distance_h = 10.0; - params.lookahead_distance_v = 10.0; - return params; - } - - ProportionalLOSGuidance Plos_; - const double tol = 1e-9; -}; - -// Test commanded angles when drone is to the right of the track -TEST_F(ProportionalLosTest, T06_test_commanded_angles) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.5, 0.0}; - - const types::Outputs O = Plos_.calculate_outputs(inputs); - - // Heading cmd should be between -pi/2 and 0 - EXPECT_LT(O.psi_d, 0.0); - EXPECT_GT(O.psi_d, -1.57); - - // Pitch cmd should be zero - EXPECT_NEAR(O.theta_d, 0.0, tol); -} - -// Test commanded angles when drone is to the left of the track -TEST_F(ProportionalLosTest, T07_test_commanded_angles) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, -0.5, 0.0}; - - const types::Outputs O = Plos_.calculate_outputs(inputs); - - // Heading cmd should be between 0 and pi/2 - EXPECT_GT(O.psi_d, 0.0); - EXPECT_LT(O.psi_d, 1.57); - // Pitch cmd should be zero - EXPECT_NEAR(O.theta_d, 0.0, tol); -} - -// Test commanded angles when drone is under the track -TEST_F(ProportionalLosTest, T08_test_commanded_angles) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.0, 0.5}; - - const types::Outputs O = Plos_.calculate_outputs(inputs); - - // Heading cmd should be 0 - EXPECT_NEAR(O.psi_d, 0.0, tol); - // Pitch cmd should be between 0 and pi/2 - EXPECT_GT(O.theta_d, 0.0); - EXPECT_LT(O.theta_d, 1.57); -} - -// Test commanded angles when drone is over the track -TEST_F(ProportionalLosTest, T09_test_commanded_angles) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.0, -0.5}; - - const types::Outputs O = Plos_.calculate_outputs(inputs); - - // Heading cmd should be 0 - EXPECT_NEAR(O.psi_d, 0.0, tol); - // Pitch cmd should be between -pi/2 and 0 - EXPECT_LT(O.theta_d, 0.0); - EXPECT_GT(O.theta_d, -1.57); -} - -// Test commanded angles when drone is over and to the right of the track - -TEST_F(ProportionalLosTest, T10_test_commanded_angles) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.5, -0.5}; - - const types::Outputs O = Plos_.calculate_outputs(inputs); - - // Heading cmd should be between -pi/2 and 0 - EXPECT_LT(O.psi_d, 0.0); - EXPECT_GT(O.psi_d, -1.57); - // Pitch cmd should be between -pi/2 and 0 - EXPECT_LT(O.theta_d, 0.0); - EXPECT_GT(O.theta_d, -1.57); -} - -} // namespace vortex::guidance::los diff --git a/guidance/los_guidance/test/test_main.cpp b/guidance/los_guidance/test/test_main.cpp deleted file mode 100644 index af89a87e6..000000000 --- a/guidance/los_guidance/test/test_main.cpp +++ /dev/null @@ -1,8 +0,0 @@ -// test/test_main.cpp -#include - -int main(int argc, char** argv) { - testing::InitGoogleTest(&argc, argv); - - return RUN_ALL_TESTS(); -} diff --git a/guidance/los_guidance/test/vector_field_los_test.cpp b/guidance/los_guidance/test/vector_field_los_test.cpp deleted file mode 100644 index 956e9b5f0..000000000 --- a/guidance/los_guidance/test/vector_field_los_test.cpp +++ /dev/null @@ -1,111 +0,0 @@ -#include "los_guidance/lib/vector_field_los.hpp" -#include -#include "los_guidance/lib/proportional_los.hpp" - -namespace vortex::guidance::los { - -class VectorFieldLosTest : public ::testing::Test { - protected: - VectorFieldLosTest() : Vflos_{get_params()} {} - - VectorFieldLosParams get_params() { - VectorFieldLosParams params; - params.max_approach_angle_h = 30.0 * M_PI / 180.0; // 30 degrees in rad - params.max_approach_angle_v = 20.0 * M_PI / 180.0; // 20 degrees in rad - params.k_p_h = 0.1; // needs tuning - params.k_p_v = 0.1; // needs tuning - params.time_step = 0.01; - return params; - } - - VectorFieldLOSGuidance Vflos_; - const double tol = 1e-9; -}; - -// Test commanded angles when drone is to the right of the track -TEST_F(VectorFieldLosTest, T06_test_commanded_angles) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.5, 0.0}; - - const types::Outputs O = Vflos_.calculate_outputs(inputs); - - // Heading cmd should be between -pi/2 and 0 - EXPECT_LT(O.psi_d, 0.0); - EXPECT_GT(O.psi_d, -1.57); - - // Pitch cmd should be zero - EXPECT_NEAR(O.theta_d, 0.0, tol); -} - -// Test commanded angles when drone is to the left of the track -TEST_F(VectorFieldLosTest, T07_test_commanded_angles) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, -0.5, 0.0}; - - const types::Outputs O = Vflos_.calculate_outputs(inputs); - - // Heading cmd should be between 0 and pi/2 - EXPECT_GT(O.psi_d, 0.0); - EXPECT_LT(O.psi_d, 1.57); - - // Pitch cmd should be zero - EXPECT_NEAR(O.theta_d, 0.0, tol); -} - -// Test commanded angles when drone is under the track -TEST_F(VectorFieldLosTest, T08_test_commanded_angles) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.0, 0.5}; - - const types::Outputs O = Vflos_.calculate_outputs(inputs); - - // Heading cmd should be 0 - EXPECT_NEAR(O.psi_d, 0.0, tol); - - // Pitch cmd should be between 0 and pi/2 - EXPECT_GT(O.theta_d, 0.0); - EXPECT_LT(O.theta_d, 1.57); -} - -// Test commanded angles when drone is over the track -TEST_F(VectorFieldLosTest, T09_test_commanded_angles) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.0, -0.5}; - - const types::Outputs O = Vflos_.calculate_outputs(inputs); - - // Heading cmd should be 0 - EXPECT_NEAR(O.psi_d, 0.0, tol); - - // Pitch cmd should be between -pi/2 and 0 - EXPECT_LT(O.theta_d, 0.0); - EXPECT_GT(O.theta_d, -1.57); -} - -// Test commanded angles when drone is over and to the right of the track -TEST_F(VectorFieldLosTest, T10_test_commanded_angles) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.5, -0.5}; - - const types::Outputs O = Vflos_.calculate_outputs(inputs); - - // Heading cmd should be between -pi/2 and 0 - EXPECT_LT(O.psi_d, 0.0); - EXPECT_GT(O.psi_d, -1.57); - - // Pitch cmd should be between -pi/2 and 0 - EXPECT_LT(O.theta_d, 0.0); - EXPECT_GT(O.theta_d, -1.57); -} - -} // namespace vortex::guidance::los diff --git a/guidance/reference_filter_dp/include/reference_filter_dp/lib/eigen_typedefs.hpp b/guidance/reference_filter_dp/include/reference_filter_dp/lib/eigen_typedefs.hpp deleted file mode 100644 index bd9a358e7..000000000 --- a/guidance/reference_filter_dp/include/reference_filter_dp/lib/eigen_typedefs.hpp +++ /dev/null @@ -1,21 +0,0 @@ -/** - * @file eigen_typedefs.hpp - * @brief Contains Eigen typedefs used in this package. - */ - -#ifndef REFERENCE_FILTER_DP__LIB__EIGEN_TYPEDEFS_HPP_ -#define REFERENCE_FILTER_DP__LIB__EIGEN_TYPEDEFS_HPP_ - -#include - -namespace Eigen { - -typedef Eigen::Matrix Matrix18d; -typedef Eigen::Matrix Matrix18x6d; -typedef Eigen::Matrix Matrix6d; -typedef Eigen::Matrix Vector6d; -typedef Eigen::Matrix Vector18d; - -} // namespace Eigen - -#endif // REFERENCE_FILTER_DP__LIB__EIGEN_TYPEDEFS_HPP_ diff --git a/guidance/reference_filter_dp/include/reference_filter_dp/lib/reference_filter.hpp b/guidance/reference_filter_dp/include/reference_filter_dp/lib/reference_filter.hpp deleted file mode 100644 index c9e3973db..000000000 --- a/guidance/reference_filter_dp/include/reference_filter_dp/lib/reference_filter.hpp +++ /dev/null @@ -1,50 +0,0 @@ -#ifndef REFERENCE_FILTER_DP__LIB__REFERENCE_FILTER_HPP_ -#define REFERENCE_FILTER_DP__LIB__REFERENCE_FILTER_HPP_ - -#include "reference_filter_dp/lib/eigen_typedefs.hpp" - -namespace vortex::guidance { - -struct ReferenceFilterParams { - Eigen::Vector6d omega = Eigen::Vector6d::Zero(); - Eigen::Vector6d zeta = Eigen::Vector6d::Zero(); -}; - -/** - * @brief Stateless third-order reference filter. - * - * Computes the state derivative x_dot = Ad * x + Bd * r, where the state - * x = [eta, eta_dot, eta_ddot] is 18-dimensional and r is the 6D reference. - */ -class ReferenceFilter { - public: - explicit ReferenceFilter(const ReferenceFilterParams& params); - - // @brief Calculate the state derivative - // @param x The state vector 18x1 - // @param r The reference vector 6x1 - // @return The state derivative 18x1 - // REF: Handbook of Marine Craft Hydrodynamics and Motion Control, Fossen - // 2021 p. 337 eq: 12.11 - Eigen::Vector18d calculate_x_dot(const Eigen::Vector18d& x, - const Eigen::Vector6d& r); - - // @brief Calculate the state transition matrix - // REF: Handbook of Marine Craft Hydrodynamics and Motion Control, Fossen - // 2021 p. 337 eq: 12.12 - void calculate_Ad(const Eigen::Vector6d& omega, - const Eigen::Vector6d& zeta); - - // @brief Calculate the input matrix - // REF: Handbook of Marine Craft Hydrodynamics and Motion Control, Fossen - // 2021 p. 337 eq: 12.12 - void calculate_Bd(const Eigen::Vector6d& omega); - - private: - Eigen::Matrix18d Ad_ = Eigen::Matrix18d::Zero(); - Eigen::Matrix18x6d Bd_ = Eigen::Matrix18x6d::Zero(); -}; - -} // namespace vortex::guidance - -#endif // REFERENCE_FILTER_DP__LIB__REFERENCE_FILTER_HPP_ diff --git a/guidance/reference_filter_dp/include/reference_filter_dp/lib/waypoint_follower.hpp b/guidance/reference_filter_dp/include/reference_filter_dp/lib/waypoint_follower.hpp deleted file mode 100644 index 121a9f431..000000000 --- a/guidance/reference_filter_dp/include/reference_filter_dp/lib/waypoint_follower.hpp +++ /dev/null @@ -1,83 +0,0 @@ -#ifndef REFERENCE_FILTER_DP__LIB__WAYPOINT_FOLLOWER_HPP_ -#define REFERENCE_FILTER_DP__LIB__WAYPOINT_FOLLOWER_HPP_ - -#include -#include -#include "reference_filter_dp/lib/eigen_typedefs.hpp" -#include "reference_filter_dp/lib/reference_filter.hpp" -#include "reference_filter_dp/lib/waypoint_types.hpp" - -namespace vortex::guidance { - -using vortex::utils::types::PoseEuler; -using vortex::utils::types::Twist; - -/** - * @brief Manages reference filter state and waypoint following logic. - * - * Thread-safe: all public methods acquire a mutex. - */ -class WaypointFollower { - public: - WaypointFollower(const ReferenceFilterParams& params, double dt_seconds); - - /** - * @brief Initialize the follower with the current vehicle state and target. - * @param pose Current vehicle pose. - * @param twist Current vehicle twist (body frame). - * @param waypoint Target waypoint with mode. - * @param convergence_threshold Max error norm to consider target reached. - */ - void start(const PoseEuler& pose, - const Twist& twist, - const Waypoint& waypoint, - double convergence_threshold); - - /** - * @brief Advance the filter by one time step. - * @return The updated filter state. - */ - Eigen::Vector18d step(); - - /** - * @brief Check if the measured pose has converged to the reference goal. - * @param measured_pose Current measured pose. - * @return True if the error norm is within the convergence threshold. - */ - bool within_convergance(const Eigen::Vector6d& measured_pose) const; - - /** - * @brief Update the reference goal pose mid-sequence. - * @param reference_goal_pose The new reference pose. - */ - void set_reference(const PoseEuler& reference_goal_pose); - - /** - * @brief Snap the position component of the filter state to the reference. - * - * Useful after convergence to eliminate any remaining steady-state offset - */ - void snap_state_to_reference(); - - /// @brief Get the current 18D filter state. - Eigen::Vector18d state() const; - - /// @brief Get the current 6D reference goal pose. - Eigen::Vector6d reference() const; - - private: - Eigen::Vector18d compute_initial_state(const PoseEuler& pose, - const Twist& twist); - - mutable std::mutex mutex_; - ReferenceFilter filter_; - double dt_seconds_{0.01}; - Eigen::Vector18d state_ = Eigen::Vector18d::Zero(); - Eigen::Vector6d reference_goal_ = Eigen::Vector6d::Zero(); - WaypointMode waypoint_mode_{WaypointMode::FULL_POSE}; - double convergence_threshold_{0.1}; -}; - -} // namespace vortex::guidance - -#endif // REFERENCE_FILTER_DP__LIB__WAYPOINT_FOLLOWER_HPP_ diff --git a/guidance/reference_filter_dp/include/reference_filter_dp/lib/waypoint_types.hpp b/guidance/reference_filter_dp/include/reference_filter_dp/lib/waypoint_types.hpp deleted file mode 100644 index ed8e8e2db..000000000 --- a/guidance/reference_filter_dp/include/reference_filter_dp/lib/waypoint_types.hpp +++ /dev/null @@ -1,34 +0,0 @@ -#ifndef REFERENCE_FILTER_DP__LIB__WAYPOINT_TYPES_HPP_ -#define REFERENCE_FILTER_DP__LIB__WAYPOINT_TYPES_HPP_ - -#include -#include - -namespace vortex::guidance { - -using vortex::utils::types::PoseEuler; - -/** - * @brief Determines which degrees of freedom the reference filter controls. - * - * The mode affects both the reference goal computation (via apply_mode_logic) - * and the convergence check (via has_converged). - */ -enum class WaypointMode : uint8_t { - FULL_POSE = 0, ///< Control all 6 DOF. - ONLY_POSITION = 1, ///< Control x, y, z; hold current orientation. - FORWARD_HEADING = 2, ///< Control x, y, z with yaw toward target. - ONLY_ORIENTATION = 3, ///< Control roll, pitch, yaw; hold current position. -}; - -/** - * @brief A target pose with an associated waypoint mode. - */ -struct Waypoint { - PoseEuler pose{}; - WaypointMode mode = WaypointMode::FULL_POSE; -}; - -} // namespace vortex::guidance - -#endif // REFERENCE_FILTER_DP__LIB__WAYPOINT_TYPES_HPP_ diff --git a/guidance/reference_filter_dp/include/reference_filter_dp/lib/waypoint_utils.hpp b/guidance/reference_filter_dp/include/reference_filter_dp/lib/waypoint_utils.hpp deleted file mode 100644 index 0db3340ad..000000000 --- a/guidance/reference_filter_dp/include/reference_filter_dp/lib/waypoint_utils.hpp +++ /dev/null @@ -1,42 +0,0 @@ -#ifndef REFERENCE_FILTER_DP__LIB__WAYPOINT_UTILS_HPP_ -#define REFERENCE_FILTER_DP__LIB__WAYPOINT_UTILS_HPP_ - -#include "reference_filter_dp/lib/eigen_typedefs.hpp" -#include "reference_filter_dp/lib/waypoint_types.hpp" - -namespace vortex::guidance { - -/** - * @brief Apply waypoint mode logic to a reference pose. - * - * For modes that don't control all DOFs, the uncontrolled components are - * replaced with values from @p current_state so the filter holds them steady. - * - * @param r_in The raw reference pose (6D). - * @param mode The waypoint mode. - * @param current_state The current filter state pose (6D). - * @return The adjusted reference pose. - */ -Eigen::Vector6d apply_mode_logic(const Eigen::Vector6d& r_in, - WaypointMode mode, - const Eigen::Vector6d& current_state); - -/** - * @brief Check whether the measured pose has converged to the reference. - * - * Only the DOFs relevant to the waypoint mode are included in the error norm. - * - * @param measured_pose The current measured pose (6D). - * @param reference The reference goal pose (6D). - * @param mode The waypoint mode. - * @param convergence_threshold The maximum allowed error norm. - * @return True if the error is below the threshold. - */ -bool has_converged(const Eigen::Vector6d& measured_pose, - const Eigen::Vector6d& reference, - WaypointMode mode, - double convergence_threshold); - -} // namespace vortex::guidance - -#endif // REFERENCE_FILTER_DP__LIB__WAYPOINT_UTILS_HPP_ diff --git a/guidance/reference_filter_dp/include/reference_filter_dp/ros/reference_filter_ros.hpp b/guidance/reference_filter_dp/include/reference_filter_dp/ros/reference_filter_ros.hpp deleted file mode 100644 index 636d10a34..000000000 --- a/guidance/reference_filter_dp/include/reference_filter_dp/ros/reference_filter_ros.hpp +++ /dev/null @@ -1,97 +0,0 @@ -#ifndef REFERENCE_FILTER_DP__ROS__REFERENCE_FILTER_ROS_HPP_ -#define REFERENCE_FILTER_DP__ROS__REFERENCE_FILTER_ROS_HPP_ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "reference_filter_dp/lib/waypoint_follower.hpp" - -namespace vortex::guidance { - -class ReferenceFilterNode : public rclcpp::Node { - public: - explicit ReferenceFilterNode( - const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); - - ~ReferenceFilterNode(); - - private: - // @brief Set the subscribers and publishers - void set_subscribers_and_publisher(); - - // @brief Set the action server - void set_action_server(); - - // @brief Initializes the reference filter with ROS parameters. - void set_refererence_filter(); - - /// @brief Accept all incoming goals unconditionally. - rclcpp_action::GoalResponse handle_goal( - const rclcpp_action::GoalUUID& uuid, - std::shared_ptr< - const vortex_msgs::action::ReferenceFilterWaypoint::Goal> goal); - - /// @brief Accept all cancel requests. - rclcpp_action::CancelResponse handle_cancel( - const std::shared_ptr> goal_handle); - - /// @brief Join the old execution thread and spawn a new one for the goal. - void handle_accepted( - const std::shared_ptr> goal_handle); - - /** - * @brief Execute the action goal in a loop until convergence or - * preemption. - * @param goal_handle The goal handle. - */ - void execute( - const std::shared_ptr> goal_handle); - - rclcpp_action::Server< - vortex_msgs::action::ReferenceFilterWaypoint>::SharedPtr action_server_; - - ReferenceFilterParams filter_params_; - - std::unique_ptr follower_; - - rclcpp::Publisher::SharedPtr - reference_pub_; - - rclcpp::Subscription::SharedPtr - reference_sub_; - - rclcpp::Subscription< - geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_; - - rclcpp::Subscription< - geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr twist_sub_; - - rclcpp::TimerBase::SharedPtr reference_pub_timer_; - - std::chrono::milliseconds time_step_{}; - - vortex::utils::types::PoseEuler current_pose_; - - vortex::utils::types::Twist current_twist_; - - std::mutex sensor_mutex_; - - std::atomic preempted_{false}; - std::mutex execute_mutex_; - std::thread execute_thread_; -}; - -} // namespace vortex::guidance - -#endif // REFERENCE_FILTER_DP__ROS__REFERENCE_FILTER_ROS_HPP_ diff --git a/guidance/reference_filter_dp/include/reference_filter_dp/ros/reference_filter_ros_utils.hpp b/guidance/reference_filter_dp/include/reference_filter_dp/ros/reference_filter_ros_utils.hpp deleted file mode 100644 index 35c8a9927..000000000 --- a/guidance/reference_filter_dp/include/reference_filter_dp/ros/reference_filter_ros_utils.hpp +++ /dev/null @@ -1,69 +0,0 @@ -#ifndef REFERENCE_FILTER_DP__ROS__REFERENCE_FILTER_ROS_UTILS_HPP_ -#define REFERENCE_FILTER_DP__ROS__REFERENCE_FILTER_ROS_UTILS_HPP_ - -#include -#include -#include -#include -#include -#include -#include -#include "reference_filter_dp/lib/eigen_typedefs.hpp" -#include "reference_filter_dp/lib/waypoint_types.hpp" - -namespace vortex::guidance { - -/** - * @brief Convert a ROS waypoint mode to a WaypointMode enum. - * @throws std::invalid_argument if the mode value is not recognized. - */ -inline WaypointMode waypoint_mode_from_ros( - const vortex_msgs::msg::WaypointMode& mode_msg) { - switch (mode_msg.mode) { - case vortex_msgs::msg::WaypointMode::FULL_POSE: - return WaypointMode::FULL_POSE; - case vortex_msgs::msg::WaypointMode::ONLY_POSITION: - return WaypointMode::ONLY_POSITION; - case vortex_msgs::msg::WaypointMode::FORWARD_HEADING: - return WaypointMode::FORWARD_HEADING; - case vortex_msgs::msg::WaypointMode::ONLY_ORIENTATION: - return WaypointMode::ONLY_ORIENTATION; - default: - throw std::invalid_argument("Invalid ROS waypoint mode: " + - std::to_string(mode_msg.mode)); - } -} - -/// @brief Convert a ROS Waypoint message to an internal Waypoint struct. -inline vortex::guidance::Waypoint waypoint_from_ros( - const vortex_msgs::msg::Waypoint& ros_wp) { - Waypoint wp; - wp.pose = - vortex::utils::ros_conversions::ros_pose_to_pose_euler(ros_wp.pose); - wp.mode = waypoint_mode_from_ros(ros_wp.waypoint_mode); - return wp; -} - -/// @brief Fill a ReferenceFilter message from an 18D state vector. -inline vortex_msgs::msg::ReferenceFilter fill_reference_msg( - const Eigen::Vector18d& x) { - using vortex::utils::math::ssa; - vortex_msgs::msg::ReferenceFilter msg; - msg.x = x(0); - msg.y = x(1); - msg.z = x(2); - msg.roll = ssa(x(3)); - msg.pitch = ssa(x(4)); - msg.yaw = ssa(x(5)); - msg.x_dot = x(6); - msg.y_dot = x(7); - msg.z_dot = x(8); - msg.roll_dot = x(9); - msg.pitch_dot = x(10); - msg.yaw_dot = x(11); - return msg; -} - -} // namespace vortex::guidance - -#endif // REFERENCE_FILTER_DP__ROS__REFERENCE_FILTER_ROS_UTILS_HPP_ diff --git a/guidance/reference_filter_dp/src/lib/reference_filter.cpp b/guidance/reference_filter_dp/src/lib/reference_filter.cpp deleted file mode 100644 index e5fc606b6..000000000 --- a/guidance/reference_filter_dp/src/lib/reference_filter.cpp +++ /dev/null @@ -1,37 +0,0 @@ -#include "reference_filter_dp/lib/reference_filter.hpp" - -namespace vortex::guidance { - -ReferenceFilter::ReferenceFilter(const ReferenceFilterParams& params) { - calculate_Ad(params.omega, params.zeta); - calculate_Bd(params.omega); -} - -Eigen::Vector18d ReferenceFilter::calculate_x_dot(const Eigen::Vector18d& x, - const Eigen::Vector6d& r) { - Eigen::Vector18d x_dot = Ad_ * x + Bd_ * r; - - return x_dot; -} - -void ReferenceFilter::calculate_Ad(const Eigen::Vector6d& omega, - const Eigen::Vector6d& zeta) { - Eigen::Matrix6d omega_diag = omega.asDiagonal(); - Eigen::Matrix6d delta = zeta.asDiagonal(); - Eigen::Matrix6d omega_diag_squared = omega_diag * omega_diag; - Eigen::Matrix6d omega_diag_cubed = omega_diag * omega_diag * omega_diag; - Ad_.block<6, 6>(0, 6) = Eigen::Matrix6d::Identity(); - Ad_.block<6, 6>(12, 0) = -omega_diag_cubed; - Ad_.block<6, 6>(12, 6) = - -(2 * delta + Eigen::Matrix6d::Identity()) * omega_diag_squared; - Ad_.block<6, 6>(12, 12) = - -(2 * delta + Eigen::Matrix6d::Identity()) * omega_diag; - Ad_.block<6, 6>(6, 12) = Eigen::Matrix6d::Identity(); -} - -void ReferenceFilter::calculate_Bd(const Eigen::Vector6d& omega) { - Eigen::Matrix6d omega_diag = omega.asDiagonal(); - Bd_.block<6, 6>(12, 0) = omega_diag * omega_diag * omega_diag; -} - -} // namespace vortex::guidance diff --git a/guidance/reference_filter_dp/src/lib/waypoint_follower.cpp b/guidance/reference_filter_dp/src/lib/waypoint_follower.cpp deleted file mode 100644 index 4da1d7c31..000000000 --- a/guidance/reference_filter_dp/src/lib/waypoint_follower.cpp +++ /dev/null @@ -1,73 +0,0 @@ -#include "reference_filter_dp/lib/waypoint_follower.hpp" -#include -#include "reference_filter_dp/lib/eigen_typedefs.hpp" -#include "reference_filter_dp/lib/waypoint_utils.hpp" - -namespace vortex::guidance { - -WaypointFollower::WaypointFollower(const ReferenceFilterParams& params, - double dt_seconds) - : filter_(params), dt_seconds_(dt_seconds) {} - -void WaypointFollower::start(const PoseEuler& pose, - const Twist& twist, - const Waypoint& waypoint, - double convergence_threshold) { - std::lock_guard lock(mutex_); - state_ = compute_initial_state(pose, twist); - waypoint_mode_ = waypoint.mode; - convergence_threshold_ = convergence_threshold; - reference_goal_ = apply_mode_logic(waypoint.pose.to_vector(), - waypoint_mode_, state_.head<6>()); -} - -Eigen::Vector18d WaypointFollower::compute_initial_state(const PoseEuler& pose, - const Twist& twist) { - Eigen::Vector18d x = Eigen::Vector18d::Zero(); - - x.head<6>() = pose.to_vector(); - - Eigen::Matrix J = pose.as_j_matrix(); - x.segment<6>(6) = J * twist.to_vector(); - - return x; -} - -Eigen::Vector18d WaypointFollower::step() { - std::lock_guard lock(mutex_); - Eigen::Vector18d state_dot_ = - filter_.calculate_x_dot(state_, reference_goal_); - state_ += state_dot_ * dt_seconds_; - - return state_; -} - -bool WaypointFollower::within_convergance( - const Eigen::Vector6d& measured_pose) const { - std::lock_guard lock(mutex_); - return has_converged(measured_pose, reference_goal_, waypoint_mode_, - convergence_threshold_); -} - -void WaypointFollower::set_reference(const PoseEuler& reference_goal_pose) { - std::lock_guard lock(mutex_); - reference_goal_ = apply_mode_logic(reference_goal_pose.to_vector(), - waypoint_mode_, state_.head<6>()); -} - -Eigen::Vector18d WaypointFollower::state() const { - std::lock_guard lock(mutex_); - return state_; -} - -Eigen::Vector6d WaypointFollower::reference() const { - std::lock_guard lock(mutex_); - return reference_goal_; -} - -void WaypointFollower::snap_state_to_reference() { - std::lock_guard lock(mutex_); - state_.head<6>() = reference_goal_; -} - -} // namespace vortex::guidance diff --git a/guidance/reference_filter_dp/src/lib/waypoint_utils.cpp b/guidance/reference_filter_dp/src/lib/waypoint_utils.cpp deleted file mode 100644 index fe72ebf13..000000000 --- a/guidance/reference_filter_dp/src/lib/waypoint_utils.cpp +++ /dev/null @@ -1,86 +0,0 @@ -#include "reference_filter_dp/lib/waypoint_utils.hpp" -#include -#include - -namespace vortex::guidance { - -Eigen::Vector6d apply_mode_logic(const Eigen::Vector6d& reference_in, - WaypointMode mode, - const Eigen::Vector6d& current_state) { - Eigen::Vector6d reference_out = reference_in; - - using vortex::utils::math::ssa; - - switch (mode) { - case WaypointMode::FULL_POSE: - reference_out(3) = - current_state(3) + ssa(reference_in(3) - current_state(3)); - reference_out(4) = - current_state(4) + ssa(reference_in(4) - current_state(4)); - reference_out(5) = - current_state(5) + ssa(reference_in(5) - current_state(5)); - break; - - case WaypointMode::ONLY_POSITION: - reference_out(3) = current_state(3); - reference_out(4) = current_state(4); - reference_out(5) = current_state(5); - break; - - case WaypointMode::FORWARD_HEADING: { - double dx = reference_in(0) - current_state(0); - double dy = reference_in(1) - current_state(1); - double forward_heading = std::atan2(dy, dx); - - reference_out(3) = 0.0; - reference_out(4) = 0.0; - reference_out(5) = vortex::utils::math::ssa(forward_heading); - break; - } - - case WaypointMode::ONLY_ORIENTATION: - reference_out(0) = current_state(0); - reference_out(1) = current_state(1); - reference_out(2) = current_state(2); - reference_out(3) = - current_state(3) + ssa(reference_in(3) - current_state(3)); - reference_out(4) = - current_state(4) + ssa(reference_in(4) - current_state(4)); - reference_out(5) = - current_state(5) + ssa(reference_in(5) - current_state(5)); - break; - } - - return reference_out; -} - -bool has_converged(const Eigen::Vector6d& measured_pose, - const Eigen::Vector6d& reference, - WaypointMode mode, - double convergence_threshold) { - using vortex::utils::math::ssa; - const Eigen::Vector3d ep = measured_pose.head<3>() - reference.head<3>(); - - Eigen::Vector3d ea; - ea(0) = ssa(measured_pose(3) - reference(3)); - ea(1) = ssa(measured_pose(4) - reference(4)); - ea(2) = ssa(measured_pose(5) - reference(5)); - - const double err = [&] { - switch (mode) { - case WaypointMode::ONLY_POSITION: - return ep.norm(); - case WaypointMode::ONLY_ORIENTATION: - return ea.norm(); - case WaypointMode::FORWARD_HEADING: - return std::sqrt(ep.squaredNorm() + ea(2) * ea(2)); - case WaypointMode::FULL_POSE: - default: - return std::sqrt(ep.squaredNorm() + ea.squaredNorm()); - } - }(); - - return err < convergence_threshold; -} - -} // namespace vortex::guidance diff --git a/guidance/reference_filter_dp/src/ros/reference_filter_ros.cpp b/guidance/reference_filter_dp/src/ros/reference_filter_ros.cpp deleted file mode 100644 index 152a59aab..000000000 --- a/guidance/reference_filter_dp/src/ros/reference_filter_ros.cpp +++ /dev/null @@ -1,239 +0,0 @@ -#include "reference_filter_dp/ros/reference_filter_ros.hpp" -#include -#include -#include -#include -#include -#include -#include "reference_filter_dp/ros/reference_filter_ros_utils.hpp" - -const auto start_message = R"( - ____ __ _____ _ _ _ - | _ \ ___ / _| ___ _ __ ___ _ __ ___ ___ | ___(_) | |_ ___ _ __ - | |_) / _ \ |_ / _ \ '__/ _ \ '_ \ / __/ _ \ | |_ | | | __/ _ \ '__| - | _ < __/ _| __/ | | __/ | | | (_| __/ | _| | | | || __/ | - |_| \_\___|_| \___|_| \___|_| |_|\___\___| |_| |_|_|\__\___|_| - - )"; - -namespace vortex::guidance { - -ReferenceFilterNode::ReferenceFilterNode(const rclcpp::NodeOptions& options) - : Node("reference_filter_node", options) { - int time_step_ms = this->declare_parameter("time_step_ms"); - time_step_ = std::chrono::milliseconds(time_step_ms); - - set_subscribers_and_publisher(); - - set_action_server(); - - set_refererence_filter(); - - spdlog::info(start_message); -} - -ReferenceFilterNode::~ReferenceFilterNode() { - preempted_ = true; - if (execute_thread_.joinable()) { - execute_thread_.join(); - } -} - -void ReferenceFilterNode::set_subscribers_and_publisher() { - this->declare_parameter("topics.pose"); - this->declare_parameter("topics.twist"); - this->declare_parameter("topics.guidance.dp"); - this->declare_parameter("topics.reference_pose"); - - std::string pose_topic = this->get_parameter("topics.pose").as_string(); - std::string twist_topic = this->get_parameter("topics.twist").as_string(); - std::string guidance_topic = - this->get_parameter("topics.guidance.dp").as_string(); - std::string reference_pose_topic = - this->get_parameter("topics.reference_pose").as_string(); - - auto qos_sensor_data = vortex::utils::qos_profiles::sensor_data_profile(1); - reference_pub_ = this->create_publisher( - guidance_topic, qos_sensor_data); - - reference_sub_ = this->create_subscription( - reference_pose_topic, qos_sensor_data, - [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg) { - follower_->set_reference( - vortex::utils::ros_conversions::ros_pose_to_pose_euler( - msg->pose)); - }); - - pose_sub_ = this->create_subscription< - geometry_msgs::msg::PoseWithCovarianceStamped>( - pose_topic, qos_sensor_data, - [this](const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr - msg) { - std::lock_guard lock(sensor_mutex_); - current_pose_ = - vortex::utils::ros_conversions::ros_pose_to_pose_euler( - msg->pose.pose); - }); - - twist_sub_ = this->create_subscription< - geometry_msgs::msg::TwistWithCovarianceStamped>( - twist_topic, qos_sensor_data, - [this](const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr - msg) { - std::lock_guard lock(sensor_mutex_); - current_twist_ = vortex::utils::ros_conversions::ros_twist_to_twist( - msg->twist.twist); - }); -} - -void ReferenceFilterNode::set_action_server() { - this->declare_parameter("action_servers.reference_filter"); - std::string action_server_name = - this->get_parameter("action_servers.reference_filter").as_string(); - - action_server_ = rclcpp_action::create_server< - vortex_msgs::action::ReferenceFilterWaypoint>( - this, action_server_name, - [this](const auto& uuid, auto goal) { - return handle_goal(uuid, std::move(goal)); - }, - [this](auto goal_handle) { return handle_cancel(goal_handle); }, - [this](auto goal_handle) { handle_accepted(goal_handle); }); -} - -void ReferenceFilterNode::set_refererence_filter() { - this->declare_parameter>("zeta"); - this->declare_parameter>("omega"); - - std::vector zeta = this->get_parameter("zeta").as_double_array(); - std::vector omega = this->get_parameter("omega").as_double_array(); - - Eigen::Vector6d zeta_eigen = Eigen::Map(zeta.data()); - Eigen::Vector6d omega_eigen = Eigen::Map(omega.data()); - - filter_params_ = ReferenceFilterParams{omega_eigen, zeta_eigen}; - - double dt_seconds = time_step_.count() / 1000.0; - follower_ = std::make_unique(filter_params_, dt_seconds); -} - -rclcpp_action::GoalResponse ReferenceFilterNode::handle_goal( - const rclcpp_action::GoalUUID& /*uuid*/, - std::shared_ptr - /*goal*/) { - spdlog::info("Accepted goal request"); - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; -} - -rclcpp_action::CancelResponse ReferenceFilterNode::handle_cancel( - const std::shared_ptr> /*goal_handle*/) { - spdlog::info("Received request to cancel goal"); - return rclcpp_action::CancelResponse::ACCEPT; -} - -void ReferenceFilterNode::handle_accepted( - const std::shared_ptr> goal_handle) { - std::lock_guard lock(execute_mutex_); - preempted_ = true; - if (execute_thread_.joinable()) { - execute_thread_.join(); - } - preempted_ = false; - - execute_thread_ = - std::thread([this, goal_handle]() { execute(goal_handle); }); -} - -void ReferenceFilterNode::execute( - const std::shared_ptr> goal_handle) { - spdlog::info("Executing goal"); - - double convergence_threshold = - goal_handle->get_goal()->convergence_threshold; - - if (convergence_threshold <= 0.0) { - convergence_threshold = 0.1; - spdlog::warn( - "ReferenceFilter: Invalid convergence_threshold received (<= 0). " - "Using default 0.1"); - } - - const auto wp = waypoint_from_ros(goal_handle->get_goal()->waypoint); - - const auto [pose, twist] = [this] { - std::lock_guard lock(sensor_mutex_); - return std::pair{current_pose_, current_twist_}; - }(); - - follower_->start(pose, twist, wp, convergence_threshold); - - auto result = std::make_shared< - vortex_msgs::action::ReferenceFilterWaypoint::Result>(); - - rclcpp::Rate loop_rate(1000.0 / time_step_.count()); - - while (rclcpp::ok()) { - if (preempted_.load()) { - result->success = false; - goal_handle->abort(result); - spdlog::info("Goal preempted by newer goal"); - return; - } - - if (goal_handle->is_canceling()) { - result->success = false; - goal_handle->canceled(result); - spdlog::info("Goal canceled"); - return; - } - - Eigen::Vector18d filter_state = follower_->step(); - - const auto current_pose_vector = [this] { - std::lock_guard lock(sensor_mutex_); - return current_pose_.to_vector(); - }(); - - bool target_reached = - follower_->within_convergance(current_pose_vector); - - if (target_reached) { - follower_->snap_state_to_reference(); - - auto final_reference_msg = - std::make_unique( - fill_reference_msg(follower_->state())); - - reference_pub_->publish(std::move(final_reference_msg)); - - result->success = true; - goal_handle->succeed(result); - spdlog::info("Goal reached"); - return; - } - - auto reference_msg = - std::make_unique( - fill_reference_msg(filter_state)); - reference_pub_->publish(std::move(reference_msg)); - loop_rate.sleep(); - } - if (!rclcpp::ok() && goal_handle->is_active()) { - auto result = std::make_shared< - vortex_msgs::action::ReferenceFilterWaypoint::Result>(); - result->success = false; - - try { - goal_handle->abort(result); - } catch (...) { - // Ignore exceptions during shutdown - } - } -} - -RCLCPP_COMPONENTS_REGISTER_NODE(ReferenceFilterNode) - -} // namespace vortex::guidance diff --git a/guidance/reference_filter_dp/test/CMakeLists.txt b/guidance/reference_filter_dp/test/CMakeLists.txt deleted file mode 100644 index 2b8229c0c..000000000 --- a/guidance/reference_filter_dp/test/CMakeLists.txt +++ /dev/null @@ -1,57 +0,0 @@ -cmake_minimum_required(VERSION 3.8) - -find_package(GTest REQUIRED) -include(GoogleTest) - -set(TEST_BINARY_NAME ${PROJECT_NAME}_test) -add_executable( - ${TEST_BINARY_NAME} - test_reference_filter.cpp -) - -target_link_libraries( - ${TEST_BINARY_NAME} - PRIVATE - ${CORE_LIB_NAME} - GTest::GTest -) - -ament_target_dependencies(${TEST_BINARY_NAME} PUBLIC Eigen3) - -gtest_discover_tests(${TEST_BINARY_NAME}) - -# Waypoint utils tests -set(TEST_WAYPOINT_UTILS ${PROJECT_NAME}_test_waypoint_utils) -add_executable( - ${TEST_WAYPOINT_UTILS} - test_waypoint_utils.cpp -) - -target_link_libraries( - ${TEST_WAYPOINT_UTILS} - PRIVATE - ${CORE_LIB_NAME} - GTest::GTest -) - -ament_target_dependencies(${TEST_WAYPOINT_UTILS} PUBLIC Eigen3 vortex_utils) - -gtest_discover_tests(${TEST_WAYPOINT_UTILS}) - -# Waypoint follower tests -set(TEST_WAYPOINT_FOLLOWER ${PROJECT_NAME}_test_waypoint_follower) -add_executable( - ${TEST_WAYPOINT_FOLLOWER} - test_waypoint_follower.cpp -) - -target_link_libraries( - ${TEST_WAYPOINT_FOLLOWER} - PRIVATE - ${CORE_LIB_NAME} - GTest::GTest -) - -ament_target_dependencies(${TEST_WAYPOINT_FOLLOWER} PUBLIC Eigen3 vortex_utils) - -gtest_discover_tests(${TEST_WAYPOINT_FOLLOWER}) diff --git a/guidance/reference_filter_dp/test/test_reference_filter.cpp b/guidance/reference_filter_dp/test/test_reference_filter.cpp deleted file mode 100644 index 3716ddaa9..000000000 --- a/guidance/reference_filter_dp/test/test_reference_filter.cpp +++ /dev/null @@ -1,58 +0,0 @@ -#include - -#include "reference_filter_dp/lib/eigen_typedefs.hpp" -#include "reference_filter_dp/lib/reference_filter.hpp" - -namespace vortex::guidance { - -class ReferenceFilterTests : public ::testing::Test { - protected: - ReferenceFilterTests() : reference_filter_{get_filter_params()} {} - - ReferenceFilterParams get_filter_params() { - ReferenceFilterParams params; - params.omega = Eigen::Vector6d::Ones(); - params.zeta = Eigen::Vector6d::Ones(); - return params; - } - - ReferenceFilter reference_filter_; -}; - -TEST_F(ReferenceFilterTests, T01_positive_command_positive_xdot) { - Eigen::Vector18d x = Eigen::Vector18d::Zero(); - Eigen::Vector6d r = Eigen::Vector6d::Ones(); - Eigen::Vector18d xdot = reference_filter_.calculate_x_dot(x, r); - - for (int i = 0; i < xdot.size(); ++i) { - EXPECT_GE(xdot(i), 0.0); - } -} - -TEST_F(ReferenceFilterTests, T02_negative_command_negative_xdot) { - Eigen::Vector18d x = Eigen::Vector18d::Zero(); - Eigen::Vector6d r = -Eigen::Vector6d::Ones(); - Eigen::Vector18d xdot = reference_filter_.calculate_x_dot(x, r); - - for (int i = 0; i < xdot.size(); ++i) { - EXPECT_LE(xdot(i), 0.0); - } -} - -TEST_F(ReferenceFilterTests, T03_zero_command_zero_xdot) { - Eigen::Vector18d x = Eigen::Vector18d::Zero(); - Eigen::Vector6d r = Eigen::Vector6d::Zero(); - Eigen::Vector18d xdot = reference_filter_.calculate_x_dot(x, r); - - for (int i = 0; i < xdot.size(); ++i) { - EXPECT_DOUBLE_EQ(xdot(i), 0.0); - } -} - -} // namespace vortex::guidance - -int main(int argc, char** argv) { - testing::InitGoogleTest(&argc, argv); - - return RUN_ALL_TESTS(); -} diff --git a/guidance/reference_filter_dp/test/test_waypoint_follower.cpp b/guidance/reference_filter_dp/test/test_waypoint_follower.cpp deleted file mode 100644 index 06e063d0f..000000000 --- a/guidance/reference_filter_dp/test/test_waypoint_follower.cpp +++ /dev/null @@ -1,111 +0,0 @@ -#include -#include "reference_filter_dp/lib/waypoint_follower.hpp" -#include "reference_filter_dp/lib/waypoint_utils.hpp" - -namespace vortex::guidance { - -class WaypointFollowerTests : public ::testing::Test { - protected: - ReferenceFilterParams get_params() { - ReferenceFilterParams params; - params.omega = Eigen::Vector6d::Ones(); - params.zeta = Eigen::Vector6d::Ones(); - return params; - } - - PoseEuler zero_pose() { return PoseEuler{}; } - Twist zero_twist() { return Twist{}; } -}; - -TEST_F(WaypointFollowerTests, StartAndStepConverges) { - WaypointFollower follower(get_params(), 0.01); - - Waypoint wp; - wp.pose = PoseEuler{1.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - wp.mode = WaypointMode::FULL_POSE; - - follower.start(zero_pose(), zero_twist(), wp, 0.1); - - Eigen::Vector18d state = follower.step(); - - // Simulate the measured pose being at the reference - Eigen::Vector6d measured_at_ref; - measured_at_ref << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0; - - EXPECT_TRUE(follower.within_convergance(measured_at_ref)); - EXPECT_EQ(state.size(), 18); -} - -TEST_F(WaypointFollowerTests, StepDoesNotConvergeWhenFar) { - WaypointFollower follower(get_params(), 0.01); - - Waypoint wp; - wp.pose = PoseEuler{10.0, 10.0, 0.0, 0.0, 0.0, 0.0}; - wp.mode = WaypointMode::FULL_POSE; - - follower.start(zero_pose(), zero_twist(), wp, 0.1); - - follower.step(); - - Eigen::Vector6d measured_far = Eigen::Vector6d::Zero(); - EXPECT_FALSE(follower.within_convergance(measured_far)); -} - -TEST_F(WaypointFollowerTests, SetReferenceUpdatesMidSequence) { - WaypointFollower follower(get_params(), 0.01); - - Waypoint wp; - wp.pose = PoseEuler{1.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - wp.mode = WaypointMode::FULL_POSE; - - follower.start(zero_pose(), zero_twist(), wp, 0.1); - - PoseEuler new_ref{5.0, 5.0, 0.0, 0.0, 0.0, 0.0}; - follower.set_reference(new_ref); - - EXPECT_DOUBLE_EQ(follower.reference()(0), 5.0); - EXPECT_DOUBLE_EQ(follower.reference()(1), 5.0); -} - -TEST_F(WaypointFollowerTests, SnapStateToReference) { - WaypointFollower follower(get_params(), 0.01); - - Waypoint wp; - wp.pose = PoseEuler{3.0, 4.0, 5.0, 0.1, 0.2, 0.3}; - wp.mode = WaypointMode::FULL_POSE; - - follower.start(zero_pose(), zero_twist(), wp, 0.1); - follower.snap_state_to_reference(); - - Eigen::Vector18d state = follower.state(); - Eigen::Vector6d ref = follower.reference(); - - for (int i = 0; i < 6; ++i) { - EXPECT_DOUBLE_EQ(state(i), ref(i)); - } -} - -TEST_F(WaypointFollowerTests, StateEvolvesWithStep) { - WaypointFollower follower(get_params(), 0.01); - - Waypoint wp; - wp.pose = PoseEuler{1.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - wp.mode = WaypointMode::FULL_POSE; - - follower.start(zero_pose(), zero_twist(), wp, 0.1); - - // Run several steps — state should move toward reference - for (int i = 0; i < 100; ++i) { - follower.step(); - } - - // x position should have moved toward 1.0 - EXPECT_GT(follower.state()(0), 0.0); -} - -} // namespace vortex::guidance - -int main(int argc, char** argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/guidance/reference_filter_dp/test/test_waypoint_utils.cpp b/guidance/reference_filter_dp/test/test_waypoint_utils.cpp deleted file mode 100644 index 6e9de1b82..000000000 --- a/guidance/reference_filter_dp/test/test_waypoint_utils.cpp +++ /dev/null @@ -1,174 +0,0 @@ -#include -#include -#include "reference_filter_dp/lib/waypoint_utils.hpp" - -namespace vortex::guidance { - -// --- apply_mode_logic tests --- - -TEST(ApplyModeLogic, FullPoseWrapsAnglesToShortestPath) { - Eigen::Vector6d r_in; - r_in << 1.0, 2.0, 3.0, 0.1, 0.2, 0.3; - Eigen::Vector6d state = Eigen::Vector6d::Zero(); - - Eigen::Vector6d result = - apply_mode_logic(r_in, WaypointMode::FULL_POSE, state); - - // Position unchanged - EXPECT_DOUBLE_EQ(result(0), 1.0); - EXPECT_DOUBLE_EQ(result(1), 2.0); - EXPECT_DOUBLE_EQ(result(2), 3.0); - - // Small angles: wrapping has no effect - EXPECT_NEAR(result(3), 0.1, 1e-12); - EXPECT_NEAR(result(4), 0.2, 1e-12); - EXPECT_NEAR(result(5), 0.3, 1e-12); -} - -TEST(ApplyModeLogic, FullPoseTakesShortestRotation) { - // Current yaw ~170°, reference yaw ~-170° => only ~20° apart - // Without wrapping, the filter would see a ~340° difference - Eigen::Vector6d state; - state << 0.0, 0.0, 0.0, 0.0, 0.0, 2.96; // ~170° - Eigen::Vector6d r_in; - r_in << 0.0, 0.0, 0.0, 0.0, 0.0, -2.96; // ~-170° - - Eigen::Vector6d result = - apply_mode_logic(r_in, WaypointMode::FULL_POSE, state); - - // The wrapped reference should be close to the current state + short - // rotation - double yaw_error = result(5) - state(5); - EXPECT_NEAR(yaw_error, -2.96 - 2.96 + 2.0 * M_PI, 1e-12); - EXPECT_LT(std::abs(yaw_error), M_PI + 1e-9); -} - -TEST(ApplyModeLogic, OnlyPositionKeepsOrientationFromState) { - Eigen::Vector6d r_in; - r_in << 1.0, 2.0, 3.0, 0.1, 0.2, 0.3; - Eigen::Vector6d state; - state << 10.0, 20.0, 30.0, 0.4, 0.5, 0.6; - - Eigen::Vector6d result = - apply_mode_logic(r_in, WaypointMode::ONLY_POSITION, state); - - EXPECT_DOUBLE_EQ(result(0), 1.0); - EXPECT_DOUBLE_EQ(result(1), 2.0); - EXPECT_DOUBLE_EQ(result(2), 3.0); - EXPECT_DOUBLE_EQ(result(3), 0.4); - EXPECT_DOUBLE_EQ(result(4), 0.5); - EXPECT_DOUBLE_EQ(result(5), 0.6); -} - -TEST(ApplyModeLogic, ForwardHeadingComputesYawFromDelta) { - Eigen::Vector6d r_in; - r_in << 1.0, 1.0, 0.0, 0.0, 0.0, 0.0; - Eigen::Vector6d state = Eigen::Vector6d::Zero(); - - Eigen::Vector6d result = - apply_mode_logic(r_in, WaypointMode::FORWARD_HEADING, state); - - double expected_yaw = std::atan2(1.0, 1.0); // pi/4 - EXPECT_DOUBLE_EQ(result(0), 1.0); - EXPECT_DOUBLE_EQ(result(1), 1.0); - EXPECT_DOUBLE_EQ(result(3), 0.0); - EXPECT_DOUBLE_EQ(result(4), 0.0); - EXPECT_NEAR(result(5), expected_yaw, 1e-12); -} - -TEST(ApplyModeLogic, OnlyOrientationKeepsPositionFromState) { - Eigen::Vector6d r_in; - r_in << 1.0, 2.0, 3.0, 0.1, 0.2, 0.3; - Eigen::Vector6d state; - state << 10.0, 20.0, 30.0, 0.4, 0.5, 0.6; - - Eigen::Vector6d result = - apply_mode_logic(r_in, WaypointMode::ONLY_ORIENTATION, state); - - EXPECT_DOUBLE_EQ(result(0), 10.0); - EXPECT_DOUBLE_EQ(result(1), 20.0); - EXPECT_DOUBLE_EQ(result(2), 30.0); - - // Small angle differences: wrapping has no effect, result matches input - EXPECT_NEAR(result(3), 0.1, 1e-12); - EXPECT_NEAR(result(4), 0.2, 1e-12); - EXPECT_NEAR(result(5), 0.3, 1e-12); -} - -TEST(ApplyModeLogic, OnlyOrientationTakesShortestRotation) { - Eigen::Vector6d state; - state << 10.0, 20.0, 30.0, 0.0, 0.0, 2.96; - Eigen::Vector6d r_in; - r_in << 1.0, 2.0, 3.0, 0.0, 0.0, -2.96; - - Eigen::Vector6d result = - apply_mode_logic(r_in, WaypointMode::ONLY_ORIENTATION, state); - - // Position from state - EXPECT_DOUBLE_EQ(result(0), 10.0); - EXPECT_DOUBLE_EQ(result(1), 20.0); - EXPECT_DOUBLE_EQ(result(2), 30.0); - - // Yaw should take the shortest path - double yaw_error = result(5) - state(5); - EXPECT_LT(std::abs(yaw_error), M_PI + 1e-9); -} - -// --- has_converged tests --- - -TEST(HasConverged, FullPoseBelowThreshold) { - Eigen::Vector6d measured; - measured << 1.0, 2.0, 3.0, 0.1, 0.2, 0.3; - Eigen::Vector6d reference = measured; - reference(0) += 0.001; - - EXPECT_TRUE( - has_converged(measured, reference, WaypointMode::FULL_POSE, 0.1)); -} - -TEST(HasConverged, FullPoseAboveThreshold) { - Eigen::Vector6d measured = Eigen::Vector6d::Zero(); - Eigen::Vector6d reference; - reference << 1.0, 1.0, 1.0, 0.0, 0.0, 0.0; - - EXPECT_FALSE( - has_converged(measured, reference, WaypointMode::FULL_POSE, 0.1)); -} - -TEST(HasConverged, OnlyPositionIgnoresOrientation) { - Eigen::Vector6d measured; - measured << 1.0, 2.0, 3.0, 0.0, 0.0, 0.0; - Eigen::Vector6d reference; - reference << 1.0, 2.0, 3.0, 1.0, 1.0, 1.0; - - EXPECT_TRUE( - has_converged(measured, reference, WaypointMode::ONLY_POSITION, 0.1)); -} - -TEST(HasConverged, OnlyOrientationIgnoresPosition) { - Eigen::Vector6d measured; - measured << 100.0, 200.0, 300.0, 0.1, 0.2, 0.3; - Eigen::Vector6d reference; - reference << 0.0, 0.0, 0.0, 0.1, 0.2, 0.3; - - EXPECT_TRUE(has_converged(measured, reference, - WaypointMode::ONLY_ORIENTATION, 0.1)); -} - -TEST(HasConverged, ForwardHeadingUsesPositionAndYawOnly) { - Eigen::Vector6d measured; - measured << 1.0, 2.0, 3.0, 0.5, 0.5, 0.1; - Eigen::Vector6d reference; - reference << 1.0, 2.0, 3.0, 0.0, 0.0, 0.1; - - // Roll and pitch differ but should be ignored - EXPECT_TRUE( - has_converged(measured, reference, WaypointMode::FORWARD_HEADING, 0.1)); -} - -} // namespace vortex::guidance - -int main(int argc, char** argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/guidance/reference_filter_dp_quat/CMakeLists.txt b/guidance/reference_filter_dp_quat/CMakeLists.txt deleted file mode 100644 index cfe51ca33..000000000 --- a/guidance/reference_filter_dp_quat/CMakeLists.txt +++ /dev/null @@ -1,105 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(reference_filter_dp_quat) - -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 20) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_action REQUIRED) -find_package(rclcpp_components REQUIRED) -find_package(vortex_msgs REQUIRED) -find_package(vortex_utils REQUIRED) -find_package(vortex_utils_ros REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(Eigen3 REQUIRED) -find_package(spdlog REQUIRED) -find_package(fmt REQUIRED) - -include_directories(include) - -set(CORE_LIB_NAME "${PROJECT_NAME}") - -add_library(${CORE_LIB_NAME} SHARED - src/lib/reference_filter.cpp - src/lib/waypoint_follower.cpp -) - -target_include_directories(${CORE_LIB_NAME} - PUBLIC - $ - $ -) - -ament_target_dependencies(${CORE_LIB_NAME} - Eigen3 - vortex_utils -) - -set(COMPONENT_LIB_NAME "${PROJECT_NAME}_component") - -add_library(${COMPONENT_LIB_NAME} SHARED - src/ros/reference_filter_ros.cpp -) - -ament_target_dependencies(${COMPONENT_LIB_NAME} - rclcpp - rclcpp_components - rclcpp_action - geometry_msgs - nav_msgs - vortex_msgs - vortex_utils - vortex_utils_ros - spdlog - fmt -) - -target_link_libraries(${COMPONENT_LIB_NAME} ${CORE_LIB_NAME}) - -rclcpp_components_register_node( - ${COMPONENT_LIB_NAME} - PLUGIN "ReferenceFilterNode" - EXECUTABLE ${PROJECT_NAME}_node -) - -install( - TARGETS - ${CORE_LIB_NAME} - ${COMPONENT_LIB_NAME} - EXPORT export_${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) - -install( - DIRECTORY include/ - DESTINATION include -) - -install(DIRECTORY - launch - config - DESTINATION share/${PROJECT_NAME}/ -) - -ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) -ament_export_dependencies( - Eigen3 - vortex_utils -) -ament_export_include_directories(include) -ament_export_libraries(${CORE_LIB_NAME}) - -if(BUILD_TESTING) - add_subdirectory(test) -endif() - -ament_package() diff --git a/guidance/reference_filter_dp_quat/README.md b/guidance/reference_filter_dp_quat/README.md deleted file mode 100644 index 1573eb2e3..000000000 --- a/guidance/reference_filter_dp_quat/README.md +++ /dev/null @@ -1,106 +0,0 @@ -## Reference filter (quaternion) - -### Third-order reference filter - -The underlying filter is the third-order model (Fossen, 2021): -```math -\dot{x}_d = A_d x_d + B_d r -``` -where -```math -A_d = \begin{bmatrix} -0_{n\times n} & I_n & 0_{n\times n} \\ -0_{n\times n} & 0_{n \times n} & I_n \\ --\Omega^3 & -(2 \Delta + I_n) \Omega^2 & -(2 \Delta + I_n) \Omega -\end{bmatrix} -``` -and -```math -B_d = \begin{bmatrix} -0_{n \times n} \\ -0_{n \times n} \\ -\Omega^3 -\end{bmatrix}. -``` - -The state is integrated using forward Euler: $x_{i+1} = x_i + \dot{x}_i \cdot dt$. - -### Error-state formulation - -This package avoids the singularity issues of Euler angles by using a quaternion-based error state. - -**Nominal pose** — a `Pose` (position + quaternion) representing the current best estimate of the reference trajectory. This is the actual output. - -**Error state** — the 18D filter state, where the first 6 elements are small errors relative to the nominal: -```math -x = \begin{bmatrix} \delta p \\ \delta \phi \\ \dot{\eta} \\ \ddot{\eta} \end{bmatrix} \in \mathbb{R}^{18} -``` - -| Indices | Meaning | -|---|---| -| `x[0:3]` | Position error $\delta p$ from nominal (meters) | -| `x[3:6]` | Orientation error $\delta \phi$ from nominal (rotation vector, radians) | -| `x[6:9]` | World-frame linear velocity (m/s) | -| `x[9:12]` | World-frame angular velocity (rad/s) | -| `x[12:18]` | Accelerations (linear + angular) | - -### Step cycle - -Each time step performs three operations: - -1. **Compute reference error** — The 6D reference $r$ is the error between the waypoint goal and the nominal pose: - - $r_{0:3} = p_{goal} - p_{nominal}$ - - $r_{3:6} = q_{\mathrm{err}}(q_{nominal}, q_{goal})$ - - The `quaternion_error` function returns $2 \cdot \text{vec}(q_{nominal}^{-1} \otimes q_{goal})$, which for small angles approximates the rotation vector from nominal to goal. - -2. **Integrate** — The standard filter step: - ```math - \dot{x} = A_d x + B_d r, \quad x \leftarrow x + \dot{x} \cdot dt - ``` - -3. **Reset** — Absorb position and orientation errors into the nominal pose, then zero them: - - $p_{nominal} \leftarrow p_{nominal} + \delta p, \quad \delta p \leftarrow 0$ - - $q_{nominal} \leftarrow q_{nominal} \otimes \exp(\delta \phi), \quad \delta \phi \leftarrow 0$ - - where $\exp(\delta \phi)$ converts the rotation vector to a quaternion via `AngleAxis`. - -The reset step keeps $\delta p$ and $\delta \phi$ near zero at all times, ensuring the linearized quaternion error remains accurate. The velocity and acceleration states ($x_{6:18}$) are **not** reset — they carry over to provide smooth, continuous motion. - -### Output message (`ReferenceFilterQuat`) - -| Field | Source | Meaning | -|---|---|---| -| `x`, `y`, `z` | Nominal pose position | Desired position in world frame (m) | -| `qw`, `qx`, `qy`, `qz` | Nominal pose quaternion | Desired orientation (unit quaternion) | -| `x_dot`, `y_dot`, `z_dot` | `x[6:9]` | World-frame linear velocity (m/s) | -| `roll_dot`, `pitch_dot`, `yaw_dot` | `x[9:12]` | World-frame angular velocity (rad/s) | - - - - -### Waypoint modes - -Each waypoint has a mode that determines which degrees of freedom are controlled by the reference filter. The mode also determines how convergence is measured. - -| Mode | Controlled DOFs | Convergence metric | -|---|---|---| -| `FULL_POSE` | All 6 DOF (position + orientation) | Position error norm + quaternion error norm | -| `ONLY_POSITION` | x, y, z (orientation holds current value) | Position error norm | -| `FORWARD_HEADING` | x, y, z + yaw toward target | Position error norm + yaw component of quaternion error | -| `ONLY_ORIENTATION` | Orientation only (position holds current value) | Quaternion error norm | - -For all modes, convergence is reached when the error metric drops below the `convergence_threshold` specified in the action goal. - -### Action Server - -The action server handles goal requests and publishes guidance commands. The server always prioritizes new goal requests, aborting ongoing requests when a new one arrives. - -- Action name: `/reference_filter` -- Goal type: Waypoint (pose + mode + convergence threshold) -- Result type: bool -- Guidance topic: `/dp/reference` - -### Overwriting the reference during an action - -While an action is executing, the reference goal can be updated by publishing a `geometry_msgs/msg/PoseStamped` to the reference pose topic. The convergence check will use the updated reference. diff --git a/guidance/reference_filter_dp_quat/config/reference_filter_params.yaml b/guidance/reference_filter_dp_quat/config/reference_filter_params.yaml deleted file mode 100644 index 1aef0ec13..000000000 --- a/guidance/reference_filter_dp_quat/config/reference_filter_params.yaml +++ /dev/null @@ -1,5 +0,0 @@ -/**: - ros__parameters: - zeta: [1., 1., 1., 1., 1., 1.] - omega: [0.2, 0.2, 0.2, 0.2, 0.2, 0.2] - time_step_ms: 10 diff --git a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/eigen_typedefs.hpp b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/eigen_typedefs.hpp deleted file mode 100644 index 1c9ce6734..000000000 --- a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/eigen_typedefs.hpp +++ /dev/null @@ -1,21 +0,0 @@ -/** - * @file eigen_typedefs.hpp - * @brief Contains Eigen typedefs used in this package. - */ - -#ifndef REFERENCE_FILTER_DP_QUAT__LIB__EIGEN_TYPEDEFS_HPP_ -#define REFERENCE_FILTER_DP_QUAT__LIB__EIGEN_TYPEDEFS_HPP_ - -#include - -namespace Eigen { - -typedef Eigen::Matrix Matrix18d; -typedef Eigen::Matrix Matrix18x6d; -typedef Eigen::Matrix Matrix6d; -typedef Eigen::Matrix Vector6d; -typedef Eigen::Matrix Vector18d; - -} // namespace Eigen - -#endif // REFERENCE_FILTER_DP_QUAT__LIB__EIGEN_TYPEDEFS_HPP_ diff --git a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/reference_filter.hpp b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/reference_filter.hpp deleted file mode 100644 index 9eefc4a55..000000000 --- a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/reference_filter.hpp +++ /dev/null @@ -1,50 +0,0 @@ -#ifndef REFERENCE_FILTER_DP_QUAT__LIB__REFERENCE_FILTER_HPP_ -#define REFERENCE_FILTER_DP_QUAT__LIB__REFERENCE_FILTER_HPP_ - -#include "reference_filter_dp_quat/lib/eigen_typedefs.hpp" - -namespace vortex::guidance { - -struct ReferenceFilterParams { - Eigen::Vector6d omega = Eigen::Vector6d::Zero(); - Eigen::Vector6d zeta = Eigen::Vector6d::Zero(); -}; - -/** - * @brief Stateless third-order reference filter. - * - * Computes the state derivative x_dot = Ad * x + Bd * r, where the state - * x = [eta, eta_dot, eta_ddot] is 18-dimensional and r is the 6D reference. - */ -class ReferenceFilter { - public: - explicit ReferenceFilter(const ReferenceFilterParams& params); - - // @brief Calculate the state derivative - // @param x The state vector 18x1 - // @param r The reference vector 6x1 - // @return The state derivative 18x1 - // REF: Handbook of Marine Craft Hydrodynamics and Motion Control, Fossen - // 2021 p. 337 eq: 12.11 - Eigen::Vector18d calculate_x_dot(const Eigen::Vector18d& x, - const Eigen::Vector6d& r); - - // @brief Calculate the state transition matrix - // REF: Handbook of Marine Craft Hydrodynamics and Motion Control, Fossen - // 2021 p. 337 eq: 12.12 - void calculate_Ad(const Eigen::Vector6d& omega, - const Eigen::Vector6d& zeta); - - // @brief Calculate the input matrix - // REF: Handbook of Marine Craft Hydrodynamics and Motion Control, Fossen - // 2021 p. 337 eq: 12.12 - void calculate_Bd(const Eigen::Vector6d& omega); - - private: - Eigen::Matrix18d Ad_ = Eigen::Matrix18d::Zero(); - Eigen::Matrix18x6d Bd_ = Eigen::Matrix18x6d::Zero(); -}; - -} // namespace vortex::guidance - -#endif // REFERENCE_FILTER_DP_QUAT__LIB__REFERENCE_FILTER_HPP_ diff --git a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_follower.hpp b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_follower.hpp deleted file mode 100644 index 6505036cd..000000000 --- a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_follower.hpp +++ /dev/null @@ -1,112 +0,0 @@ -#ifndef REFERENCE_FILTER_DP_QUAT__LIB__WAYPOINT_FOLLOWER_HPP_ -#define REFERENCE_FILTER_DP_QUAT__LIB__WAYPOINT_FOLLOWER_HPP_ - -#include -#include -#include -#include "reference_filter_dp_quat/lib/eigen_typedefs.hpp" -#include "reference_filter_dp_quat/lib/reference_filter.hpp" - -namespace vortex::guidance { - -using vortex::utils::types::Pose; -using vortex::utils::types::Twist; -using vortex::utils::types::Waypoint; -using vortex::utils::types::WaypointMode; - -/** - * @brief Manages reference filter state and waypoint following logic. - * - * Uses an error-state formulation: the 18D state vector holds - * [δp(3), δφ(3), velocity(6), acceleration(6)], while a separate - * nominal Pose tracks the full quaternion orientation. Each step, - * position/orientation errors are absorbed into the nominal and reset. - * - * Thread-safe: all public methods acquire a mutex. - */ -class WaypointFollower { - public: - WaypointFollower(const ReferenceFilterParams& params, double dt_seconds); - - /** - * @brief Initialize the follower with the current vehicle state and target. - * @param pose Current vehicle pose. - * @param twist Current vehicle twist (body frame). - * @param waypoint Target waypoint with mode. - * @param convergence_threshold Max error norm to consider target reached. - */ - void start(const Pose& pose, - const Twist& twist, - const Waypoint& waypoint, - double convergence_threshold); - - /** - * @brief Advance the filter by one time step. - * - * Computes the error-state reference, integrates, then absorbs - * position/orientation errors into the nominal pose. - */ - void step(); - - /** - * @brief Check if the measured pose has converged to the waypoint goal. - * @param measured_pose Current measured pose. - * @return True if the error norm is within the convergence threshold. - */ - bool within_convergance(const Pose& measured_pose) const; - - /** - * @brief Update the reference goal pose mid-sequence. - * @param reference_goal_pose The new reference pose. - */ - void set_reference(const Pose& reference_goal_pose); - - /** - * @brief Snap the nominal pose to the waypoint goal and zero - * errors/velocity. - * - * Useful after convergence to eliminate any remaining steady-state offset. - */ - void snap_state_to_reference(); - - /** - * @brief Get the current nominal pose (position + quaternion). - */ - Pose pose() const; - - /** - * @brief Get the current world-frame velocity (linear + angular). - */ - Eigen::Vector6d velocity() const; - - /** - * @brief Get the current waypoint goal. - */ - Pose waypoint_goal() const; - - private: - /** - * @brief Compute error-state reference based on the current nominal pose - * and waypoint goal. - */ - Eigen::Vector6d update_reference() const; - - /** - * @brief Absorb position/orientation errors into the nominal pose and - * reset the error states. - */ - void inject_and_reset(); - - mutable std::mutex mutex_; - ReferenceFilter filter_; - double dt_seconds_{0.01}; - Pose nominal_pose_; - Eigen::Vector18d state_ = Eigen::Vector18d::Zero(); - Pose waypoint_goal_; - WaypointMode waypoint_mode_{WaypointMode::FULL_POSE}; - double convergence_threshold_{0.1}; -}; - -} // namespace vortex::guidance - -#endif // REFERENCE_FILTER_DP_QUAT__LIB__WAYPOINT_FOLLOWER_HPP_ diff --git a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros.hpp b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros.hpp deleted file mode 100644 index 87df8e7e6..000000000 --- a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros.hpp +++ /dev/null @@ -1,95 +0,0 @@ -#ifndef REFERENCE_FILTER_DP_QUAT__ROS__REFERENCE_FILTER_ROS_HPP_ -#define REFERENCE_FILTER_DP_QUAT__ROS__REFERENCE_FILTER_ROS_HPP_ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "reference_filter_dp_quat/lib/waypoint_follower.hpp" - -namespace vortex::guidance { - -class ReferenceFilterNode : public rclcpp::Node { - public: - explicit ReferenceFilterNode( - const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); - - ~ReferenceFilterNode(); - - private: - // @brief Set the subscribers and publishers - void set_subscribers_and_publisher(); - - // @brief Set the action server - void set_action_server(); - - // @brief Initializes the reference filter with ROS parameters. - void set_refererence_filter(); - - /// @brief Accept all incoming goals unconditionally. - rclcpp_action::GoalResponse handle_goal( - const rclcpp_action::GoalUUID& uuid, - std::shared_ptr< - const vortex_msgs::action::ReferenceFilterQuatWaypoint::Goal> goal); - - /// @brief Accept all cancel requests. - rclcpp_action::CancelResponse handle_cancel( - const std::shared_ptr> goal_handle); - - /// @brief Join the old execution thread and spawn a new one for the goal. - void handle_accepted( - const std::shared_ptr> goal_handle); - - /** - * @brief Execute the action goal in a loop until convergence or - * preemption. - * @param goal_handle The goal handle. - */ - void execute( - const std::shared_ptr> goal_handle); - - rclcpp_action::Server:: - SharedPtr action_server_; - - ReferenceFilterParams filter_params_; - - std::unique_ptr follower_; - - rclcpp::Publisher::SharedPtr - reference_pub_; - - rclcpp::Subscription::SharedPtr - reference_sub_; - - rclcpp::Subscription< - geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_; - - rclcpp::Subscription< - geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr twist_sub_; - - std::chrono::milliseconds time_step_{}; - - vortex::utils::types::Pose current_pose_; - - vortex::utils::types::Twist current_twist_; - - std::mutex sensor_mutex_; - - std::atomic preempted_{false}; - std::mutex execute_mutex_; - std::thread execute_thread_; -}; - -} // namespace vortex::guidance - -#endif // REFERENCE_FILTER_DP_QUAT__ROS__REFERENCE_FILTER_ROS_HPP_ diff --git a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros_utils.hpp b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros_utils.hpp deleted file mode 100644 index c51fc75f0..000000000 --- a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros_utils.hpp +++ /dev/null @@ -1,58 +0,0 @@ -#ifndef REFERENCE_FILTER_DP_QUAT__ROS__REFERENCE_FILTER_ROS_UTILS_HPP_ -#define REFERENCE_FILTER_DP_QUAT__ROS__REFERENCE_FILTER_ROS_UTILS_HPP_ - -#include -#include -#include -#include -#include -#include "reference_filter_dp_quat/lib/eigen_typedefs.hpp" - -namespace vortex::guidance { - -/// @brief Fill a ReferenceFilterQuat message from a Pose and velocity vector. -inline vortex_msgs::msg::ReferenceFilterQuat fill_reference_msg( - const vortex::utils::types::Pose& pose, - const Eigen::Vector6d& velocity) { - vortex_msgs::msg::ReferenceFilterQuat msg; - msg.x = pose.x; - msg.y = pose.y; - msg.z = pose.z; - msg.qw = pose.qw; - msg.qx = pose.qx; - msg.qy = pose.qy; - msg.qz = pose.qz; - msg.x_dot = velocity(0); - msg.y_dot = velocity(1); - msg.z_dot = velocity(2); - msg.roll_dot = velocity(3); - msg.pitch_dot = velocity(4); - msg.yaw_dot = velocity(5); - return msg; -} - -/// @brief Fill an RPY ReferenceFilter message from a Pose and velocity vector. -inline vortex_msgs::msg::ReferenceFilter fill_reference_rpy_msg( - const vortex::utils::types::Pose& pose, - const Eigen::Vector6d& velocity) { - using vortex::utils::math::ssa; - auto euler = pose.as_pose_euler(); - vortex_msgs::msg::ReferenceFilter msg; - msg.x = euler.x; - msg.y = euler.y; - msg.z = euler.z; - msg.roll = ssa(euler.roll); - msg.pitch = ssa(euler.pitch); - msg.yaw = ssa(euler.yaw); - msg.x_dot = velocity(0); - msg.y_dot = velocity(1); - msg.z_dot = velocity(2); - msg.roll_dot = velocity(3); - msg.pitch_dot = velocity(4); - msg.yaw_dot = velocity(5); - return msg; -} - -} // namespace vortex::guidance - -#endif // REFERENCE_FILTER_DP_QUAT__ROS__REFERENCE_FILTER_ROS_UTILS_HPP_ diff --git a/guidance/reference_filter_dp_quat/launch/reference_filter_dp_quat.launch.py b/guidance/reference_filter_dp_quat/launch/reference_filter_dp_quat.launch.py deleted file mode 100644 index a610a8ef3..000000000 --- a/guidance/reference_filter_dp_quat/launch/reference_filter_dp_quat.launch.py +++ /dev/null @@ -1,64 +0,0 @@ -import os - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, OpaqueFunction -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node - -from auv_setup.launch_arg_common import ( - declare_drone_and_namespace_args, - resolve_drone_and_namespace, -) - - -def launch_setup(context, *args, **kwargs): - drone, namespace = resolve_drone_and_namespace(context) - rpy_publish = LaunchConfiguration("rpy_publish").perform(context) == "true" - - config_file_path = os.path.join( - get_package_share_directory("reference_filter_dp_quat"), - "config", - "reference_filter_params.yaml", - ) - - drone_params = os.path.join( - get_package_share_directory("auv_setup"), - "config", - "robots", - f"{drone}.yaml", - ) - - extra_params = {} - if rpy_publish: - extra_params = { - "publish_rpy_debug": True, - "topics.guidance.dp_rpy": "guidance/dp", - "topics.guidance.dp": "guidance/dp_quat", - } - - return [ - Node( - package="reference_filter_dp_quat", - executable="reference_filter_dp_quat_node", - name="reference_filter_node", - namespace=namespace, - parameters=[config_file_path, drone_params, extra_params], - output="screen", - ) - ] - - -def generate_launch_description(): - return LaunchDescription( - declare_drone_and_namespace_args() - + [ - DeclareLaunchArgument( - "rpy_publish", - default_value="false", - description="When true, publish RPY ReferenceFilter on guidance/dp " - "for compatibility with controllers expecting RPY messages.", - ), - OpaqueFunction(function=launch_setup), - ] - ) diff --git a/guidance/reference_filter_dp_quat/package.xml b/guidance/reference_filter_dp_quat/package.xml deleted file mode 100644 index d7aa188a2..000000000 --- a/guidance/reference_filter_dp_quat/package.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - - reference_filter_dp_quat - 0.0.0 - Quaternion Reference model for the DP controller - jorgenfj - MIT - - ament_cmake - - rclcpp - rclcpp_action - geometry_msgs - vortex_msgs - vortex_utils - vortex_utils_ros - nav_msgs - eigen - - - ament_cmake - - diff --git a/guidance/reference_filter_dp_quat/src/lib/reference_filter.cpp b/guidance/reference_filter_dp_quat/src/lib/reference_filter.cpp deleted file mode 100644 index c7d1f0cb5..000000000 --- a/guidance/reference_filter_dp_quat/src/lib/reference_filter.cpp +++ /dev/null @@ -1,37 +0,0 @@ -#include "reference_filter_dp_quat/lib/reference_filter.hpp" - -namespace vortex::guidance { - -ReferenceFilter::ReferenceFilter(const ReferenceFilterParams& params) { - calculate_Ad(params.omega, params.zeta); - calculate_Bd(params.omega); -} - -Eigen::Vector18d ReferenceFilter::calculate_x_dot(const Eigen::Vector18d& x, - const Eigen::Vector6d& r) { - Eigen::Vector18d x_dot = Ad_ * x + Bd_ * r; - - return x_dot; -} - -void ReferenceFilter::calculate_Ad(const Eigen::Vector6d& omega, - const Eigen::Vector6d& zeta) { - Eigen::Matrix6d omega_diag = omega.asDiagonal(); - Eigen::Matrix6d delta = zeta.asDiagonal(); - Eigen::Matrix6d omega_diag_squared = omega_diag * omega_diag; - Eigen::Matrix6d omega_diag_cubed = omega_diag * omega_diag * omega_diag; - Ad_.block<6, 6>(0, 6) = Eigen::Matrix6d::Identity(); - Ad_.block<6, 6>(12, 0) = -omega_diag_cubed; - Ad_.block<6, 6>(12, 6) = - -(2 * delta + Eigen::Matrix6d::Identity()) * omega_diag_squared; - Ad_.block<6, 6>(12, 12) = - -(2 * delta + Eigen::Matrix6d::Identity()) * omega_diag; - Ad_.block<6, 6>(6, 12) = Eigen::Matrix6d::Identity(); -} - -void ReferenceFilter::calculate_Bd(const Eigen::Vector6d& omega) { - Eigen::Matrix6d omega_diag = omega.asDiagonal(); - Bd_.block<6, 6>(12, 0) = omega_diag * omega_diag * omega_diag; -} - -} // namespace vortex::guidance diff --git a/guidance/reference_filter_dp_quat/src/lib/waypoint_follower.cpp b/guidance/reference_filter_dp_quat/src/lib/waypoint_follower.cpp deleted file mode 100644 index ce58bb3d3..000000000 --- a/guidance/reference_filter_dp_quat/src/lib/waypoint_follower.cpp +++ /dev/null @@ -1,98 +0,0 @@ -#include "reference_filter_dp_quat/lib/waypoint_follower.hpp" -#include -#include -#include "reference_filter_dp_quat/lib/eigen_typedefs.hpp" - -namespace vortex::guidance { - -WaypointFollower::WaypointFollower(const ReferenceFilterParams& params, - double dt_seconds) - : filter_(params), dt_seconds_(dt_seconds) {} - -void WaypointFollower::start(const Pose& pose, - const Twist& twist, - const Waypoint& waypoint, - double convergence_threshold) { - std::lock_guard lock(mutex_); - - nominal_pose_ = pose; - state_ = Eigen::Vector18d::Zero(); - - Eigen::Matrix3d R = pose.as_rotation_matrix(); - Eigen::Vector6d twist_vec = twist.to_vector(); - state_.segment<3>(6) = R * twist_vec.head<3>(); - state_.segment<3>(9) = R * twist_vec.tail<3>(); - - waypoint_mode_ = waypoint.mode; - convergence_threshold_ = convergence_threshold; - waypoint_goal_ = vortex::utils::waypoints::compute_waypoint_goal( - waypoint.pose, waypoint_mode_, nominal_pose_); -} - -void WaypointFollower::step() { - std::lock_guard lock(mutex_); - const Eigen::Vector6d filter_reference = update_reference(); - - const Eigen::Vector18d state_derivative = - filter_.calculate_x_dot(state_, filter_reference); - state_ += state_derivative * dt_seconds_; - inject_and_reset(); -} - -Eigen::Vector6d WaypointFollower::update_reference() const { - Eigen::Vector6d filter_reference; - filter_reference.head<3>() = - waypoint_goal_.pos_vector() - nominal_pose_.pos_vector(); - filter_reference.tail<3>() = vortex::utils::math::quaternion_error( - nominal_pose_.ori_quaternion(), waypoint_goal_.ori_quaternion()); - return filter_reference; -} - -void WaypointFollower::inject_and_reset() { - nominal_pose_.set_pos(nominal_pose_.pos_vector() + state_.head<3>()); - state_.head<3>().setZero(); - - const Eigen::Vector3d delta_orientation = state_.segment<3>(3); - const double angle = delta_orientation.norm(); - if (angle >= 1e-10) { - Eigen::Quaterniond delta_quat( - Eigen::AngleAxisd(angle, delta_orientation.normalized())); - nominal_pose_.set_ori(nominal_pose_.ori_quaternion() * delta_quat); - state_.segment<3>(3).setZero(); - } -} - -bool WaypointFollower::within_convergance(const Pose& measured_pose) const { - std::lock_guard lock(mutex_); - return vortex::utils::waypoints::has_converged( - measured_pose, waypoint_goal_, waypoint_mode_, convergence_threshold_); -} - -void WaypointFollower::set_reference(const Pose& reference_goal_pose) { - std::lock_guard lock(mutex_); - waypoint_goal_ = vortex::utils::waypoints::compute_waypoint_goal( - reference_goal_pose, waypoint_mode_, nominal_pose_); -} - -void WaypointFollower::snap_state_to_reference() { - std::lock_guard lock(mutex_); - nominal_pose_ = waypoint_goal_; - state_.head<12>().setZero(); -} - -Pose WaypointFollower::pose() const { - std::lock_guard lock(mutex_); - return nominal_pose_; -} - -Eigen::Vector6d WaypointFollower::velocity() const { - std::lock_guard lock(mutex_); - return state_.segment<6>(6); -} - -Pose WaypointFollower::waypoint_goal() const { - std::lock_guard lock(mutex_); - return waypoint_goal_; -} - -} // namespace vortex::guidance diff --git a/guidance/reference_filter_dp_quat/src/ros/reference_filter_ros.cpp b/guidance/reference_filter_dp_quat/src/ros/reference_filter_ros.cpp deleted file mode 100644 index a129d2aef..000000000 --- a/guidance/reference_filter_dp_quat/src/ros/reference_filter_ros.cpp +++ /dev/null @@ -1,237 +0,0 @@ -#include "reference_filter_dp_quat/ros/reference_filter_ros.hpp" -#include -#include -#include -#include -#include -#include -#include "reference_filter_dp_quat/ros/reference_filter_ros_utils.hpp" - -const auto start_message = R"( - ____ __ _____ _ _ _ - | _ \ ___ / _| ___ _ __ ___ _ __ ___ ___ | ___(_) | |_ ___ _ __ - | |_) / _ \ |_ / _ \ '__/ _ \ '_ \ / __/ _ \ | |_ | | | __/ _ \ '__| - | _ < __/ _| __/ | | __/ | | | (_| __/ | _| | | | || __/ | - |_| \_\___|_| \___|_| \___|_| |_|\___\___| |_| |_|_|\__\___|_| - - )"; - -namespace vortex::guidance { - -ReferenceFilterNode::ReferenceFilterNode(const rclcpp::NodeOptions& options) - : Node("reference_filter_node", options) { - int time_step_ms = this->declare_parameter("time_step_ms"); - time_step_ = std::chrono::milliseconds(time_step_ms); - - set_subscribers_and_publisher(); - - set_action_server(); - - set_refererence_filter(); - - spdlog::info(start_message); -} - -ReferenceFilterNode::~ReferenceFilterNode() { - preempted_ = true; - if (execute_thread_.joinable()) { - execute_thread_.join(); - } -} - -void ReferenceFilterNode::set_subscribers_and_publisher() { - this->declare_parameter("topics.pose"); - this->declare_parameter("topics.twist"); - this->declare_parameter("topics.guidance.dp_quat"); - this->declare_parameter("topics.reference_pose"); - - std::string pose_topic = this->get_parameter("topics.pose").as_string(); - std::string twist_topic = this->get_parameter("topics.twist").as_string(); - std::string guidance_topic = - this->get_parameter("topics.guidance.dp_quat").as_string(); - std::string reference_pose_topic = - this->get_parameter("topics.reference_pose").as_string(); - - auto qos_sensor_data = vortex::utils::qos_profiles::sensor_data_profile(1); - reference_pub_ = - this->create_publisher( - guidance_topic, qos_sensor_data); - - reference_sub_ = this->create_subscription( - reference_pose_topic, qos_sensor_data, - [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg) { - follower_->set_reference( - vortex::utils::ros_conversions::ros_pose_to_pose(msg->pose)); - }); - - pose_sub_ = this->create_subscription< - geometry_msgs::msg::PoseWithCovarianceStamped>( - pose_topic, qos_sensor_data, - [this](const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr - msg) { - std::lock_guard lock(sensor_mutex_); - current_pose_ = vortex::utils::ros_conversions::ros_pose_to_pose( - msg->pose.pose); - }); - - twist_sub_ = this->create_subscription< - geometry_msgs::msg::TwistWithCovarianceStamped>( - twist_topic, qos_sensor_data, - [this](const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr - msg) { - std::lock_guard lock(sensor_mutex_); - current_twist_ = vortex::utils::ros_conversions::ros_twist_to_twist( - msg->twist.twist); - }); -} - -void ReferenceFilterNode::set_action_server() { - this->declare_parameter("action_servers.reference_filter"); - std::string action_server_name = - this->get_parameter("action_servers.reference_filter").as_string(); - - action_server_ = rclcpp_action::create_server< - vortex_msgs::action::ReferenceFilterQuatWaypoint>( - this, action_server_name, - [this](const auto& uuid, auto goal) { - return handle_goal(uuid, std::move(goal)); - }, - [this](auto goal_handle) { return handle_cancel(goal_handle); }, - [this](auto goal_handle) { handle_accepted(goal_handle); }); -} - -void ReferenceFilterNode::set_refererence_filter() { - this->declare_parameter>("zeta"); - this->declare_parameter>("omega"); - - std::vector zeta = this->get_parameter("zeta").as_double_array(); - std::vector omega = this->get_parameter("omega").as_double_array(); - - Eigen::Vector6d zeta_eigen = Eigen::Map(zeta.data()); - Eigen::Vector6d omega_eigen = Eigen::Map(omega.data()); - - filter_params_ = ReferenceFilterParams{omega_eigen, zeta_eigen}; - - double dt_seconds = time_step_.count() / 1000.0; - follower_ = std::make_unique(filter_params_, dt_seconds); -} - -rclcpp_action::GoalResponse ReferenceFilterNode::handle_goal( - const rclcpp_action::GoalUUID& /*uuid*/, - std::shared_ptr< - const vortex_msgs::action::ReferenceFilterQuatWaypoint::Goal> - /*goal*/) { - spdlog::info("Accepted goal request"); - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; -} - -rclcpp_action::CancelResponse ReferenceFilterNode::handle_cancel( - const std::shared_ptr> /*goal_handle*/) { - spdlog::info("Received request to cancel goal"); - return rclcpp_action::CancelResponse::ACCEPT; -} - -void ReferenceFilterNode::handle_accepted( - const std::shared_ptr> goal_handle) { - std::lock_guard lock(execute_mutex_); - preempted_ = true; - if (execute_thread_.joinable()) { - execute_thread_.join(); - } - preempted_ = false; - - execute_thread_ = - std::thread([this, goal_handle]() { execute(goal_handle); }); -} - -void ReferenceFilterNode::execute( - const std::shared_ptr> goal_handle) { - spdlog::info("Executing goal"); - - double convergence_threshold = - goal_handle->get_goal()->convergence_threshold; - - if (convergence_threshold <= 0.0) { - convergence_threshold = 0.1; - spdlog::warn( - "ReferenceFilter: Invalid convergence_threshold received (<= 0). " - "Using default 0.1"); - } - - const auto wp = vortex::utils::waypoints::waypoint_from_ros( - goal_handle->get_goal()->waypoint); - - const auto [pose, twist] = [this] { - std::lock_guard lock(sensor_mutex_); - return std::pair{current_pose_, current_twist_}; - }(); - - follower_->start(pose, twist, wp, convergence_threshold); - - auto result = std::make_shared< - vortex_msgs::action::ReferenceFilterQuatWaypoint::Result>(); - - rclcpp::Rate loop_rate(1000.0 / time_step_.count()); - - while (rclcpp::ok()) { - if (preempted_.load()) { - result->success = false; - goal_handle->abort(result); - spdlog::info("Goal preempted by newer goal"); - return; - } - - if (goal_handle->is_canceling()) { - result->success = false; - goal_handle->canceled(result); - spdlog::info("Goal canceled"); - return; - } - - follower_->step(); - - const auto current_pose = [this] { - std::lock_guard lock(sensor_mutex_); - return current_pose_; - }(); - - bool target_reached = follower_->within_convergance(current_pose); - - if (target_reached) { - follower_->snap_state_to_reference(); - - auto final_reference_msg = - fill_reference_msg(follower_->pose(), follower_->velocity()); - - reference_pub_->publish(final_reference_msg); - - result->success = true; - goal_handle->succeed(result); - spdlog::info("Goal reached"); - return; - } - - auto reference_msg = - fill_reference_msg(follower_->pose(), follower_->velocity()); - reference_pub_->publish(reference_msg); - loop_rate.sleep(); - } - if (!rclcpp::ok() && goal_handle->is_active()) { - auto result = std::make_shared< - vortex_msgs::action::ReferenceFilterQuatWaypoint::Result>(); - result->success = false; - - try { - goal_handle->abort(result); - } catch (...) { - // Ignore exceptions during shutdown - } - } -} - -RCLCPP_COMPONENTS_REGISTER_NODE(ReferenceFilterNode) - -} // namespace vortex::guidance diff --git a/guidance/reference_filter_dp_quat/test/CMakeLists.txt b/guidance/reference_filter_dp_quat/test/CMakeLists.txt deleted file mode 100644 index 9295a4300..000000000 --- a/guidance/reference_filter_dp_quat/test/CMakeLists.txt +++ /dev/null @@ -1,39 +0,0 @@ -cmake_minimum_required(VERSION 3.8) - -find_package(GTest REQUIRED) -include(GoogleTest) - -set(TEST_BINARY_NAME ${PROJECT_NAME}_test) -add_executable( - ${TEST_BINARY_NAME} - test_reference_filter.cpp -) - -target_link_libraries( - ${TEST_BINARY_NAME} - PRIVATE - ${CORE_LIB_NAME} - GTest::GTest -) - -ament_target_dependencies(${TEST_BINARY_NAME} PUBLIC Eigen3) - -gtest_discover_tests(${TEST_BINARY_NAME}) - -# Waypoint follower tests -set(TEST_WAYPOINT_FOLLOWER ${PROJECT_NAME}_test_waypoint_follower) -add_executable( - ${TEST_WAYPOINT_FOLLOWER} - test_waypoint_follower.cpp -) - -target_link_libraries( - ${TEST_WAYPOINT_FOLLOWER} - PRIVATE - ${CORE_LIB_NAME} - GTest::GTest -) - -ament_target_dependencies(${TEST_WAYPOINT_FOLLOWER} PUBLIC Eigen3 vortex_utils) - -gtest_discover_tests(${TEST_WAYPOINT_FOLLOWER}) diff --git a/guidance/reference_filter_dp_quat/test/test_reference_filter.cpp b/guidance/reference_filter_dp_quat/test/test_reference_filter.cpp deleted file mode 100644 index d5fab4c19..000000000 --- a/guidance/reference_filter_dp_quat/test/test_reference_filter.cpp +++ /dev/null @@ -1,58 +0,0 @@ -#include - -#include "reference_filter_dp_quat/lib/eigen_typedefs.hpp" -#include "reference_filter_dp_quat/lib/reference_filter.hpp" - -namespace vortex::guidance { - -class ReferenceFilterTests : public ::testing::Test { - protected: - ReferenceFilterTests() : reference_filter_{get_filter_params()} {} - - ReferenceFilterParams get_filter_params() { - ReferenceFilterParams params; - params.omega = Eigen::Vector6d::Ones(); - params.zeta = Eigen::Vector6d::Ones(); - return params; - } - - ReferenceFilter reference_filter_; -}; - -TEST_F(ReferenceFilterTests, T01_positive_command_positive_xdot) { - Eigen::Vector18d x = Eigen::Vector18d::Zero(); - Eigen::Vector6d r = Eigen::Vector6d::Ones(); - Eigen::Vector18d xdot = reference_filter_.calculate_x_dot(x, r); - - for (int i = 0; i < xdot.size(); ++i) { - EXPECT_GE(xdot(i), 0.0); - } -} - -TEST_F(ReferenceFilterTests, T02_negative_command_negative_xdot) { - Eigen::Vector18d x = Eigen::Vector18d::Zero(); - Eigen::Vector6d r = -Eigen::Vector6d::Ones(); - Eigen::Vector18d xdot = reference_filter_.calculate_x_dot(x, r); - - for (int i = 0; i < xdot.size(); ++i) { - EXPECT_LE(xdot(i), 0.0); - } -} - -TEST_F(ReferenceFilterTests, T03_zero_command_zero_xdot) { - Eigen::Vector18d x = Eigen::Vector18d::Zero(); - Eigen::Vector6d r = Eigen::Vector6d::Zero(); - Eigen::Vector18d xdot = reference_filter_.calculate_x_dot(x, r); - - for (int i = 0; i < xdot.size(); ++i) { - EXPECT_DOUBLE_EQ(xdot(i), 0.0); - } -} - -} // namespace vortex::guidance - -int main(int argc, char** argv) { - testing::InitGoogleTest(&argc, argv); - - return RUN_ALL_TESTS(); -} diff --git a/guidance/reference_filter_dp_quat/test/test_waypoint_follower.cpp b/guidance/reference_filter_dp_quat/test/test_waypoint_follower.cpp deleted file mode 100644 index 2c482c9dc..000000000 --- a/guidance/reference_filter_dp_quat/test/test_waypoint_follower.cpp +++ /dev/null @@ -1,112 +0,0 @@ -#include -#include -#include "reference_filter_dp_quat/lib/waypoint_follower.hpp" - -namespace vortex::guidance { - -class WaypointFollowerTests : public ::testing::Test { - protected: - ReferenceFilterParams get_params() { - ReferenceFilterParams params; - params.omega = Eigen::Vector6d::Ones(); - params.zeta = Eigen::Vector6d::Ones(); - return params; - } - - Pose zero_pose() { return Pose{}; } - Twist zero_twist() { return Twist{}; } -}; - -TEST_F(WaypointFollowerTests, StartAndStepConverges) { - WaypointFollower follower(get_params(), 0.01); - - Waypoint wp; - wp.pose = Pose{1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0}; - wp.mode = WaypointMode::FULL_POSE; - - follower.start(zero_pose(), zero_twist(), wp, 0.1); - - follower.step(); - - // Simulate the measured pose being at the reference - Pose measured_at_ref{1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0}; - - EXPECT_TRUE(follower.within_convergance(measured_at_ref)); -} - -TEST_F(WaypointFollowerTests, StepDoesNotConvergeWhenFar) { - WaypointFollower follower(get_params(), 0.01); - - Waypoint wp; - wp.pose = Pose{10.0, 10.0, 0.0, 1.0, 0.0, 0.0, 0.0}; - wp.mode = WaypointMode::FULL_POSE; - - follower.start(zero_pose(), zero_twist(), wp, 0.1); - - follower.step(); - - EXPECT_FALSE(follower.within_convergance(zero_pose())); -} - -TEST_F(WaypointFollowerTests, SetReferenceUpdatesMidSequence) { - WaypointFollower follower(get_params(), 0.01); - - Waypoint wp; - wp.pose = Pose{1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0}; - wp.mode = WaypointMode::FULL_POSE; - - follower.start(zero_pose(), zero_twist(), wp, 0.1); - - Pose new_ref{5.0, 5.0, 0.0, 1.0, 0.0, 0.0, 0.0}; - follower.set_reference(new_ref); - - EXPECT_DOUBLE_EQ(follower.waypoint_goal().x, 5.0); - EXPECT_DOUBLE_EQ(follower.waypoint_goal().y, 5.0); -} - -TEST_F(WaypointFollowerTests, SnapStateToReference) { - WaypointFollower follower(get_params(), 0.01); - - Waypoint wp; - wp.pose = Pose{3.0, 4.0, 5.0, 1.0, 0.0, 0.0, 0.0}; - wp.mode = WaypointMode::FULL_POSE; - - follower.start(zero_pose(), zero_twist(), wp, 0.1); - follower.snap_state_to_reference(); - - Pose pose = follower.pose(); - Pose goal = follower.waypoint_goal(); - - EXPECT_DOUBLE_EQ(pose.x, goal.x); - EXPECT_DOUBLE_EQ(pose.y, goal.y); - EXPECT_DOUBLE_EQ(pose.z, goal.z); - EXPECT_DOUBLE_EQ(pose.qw, goal.qw); - EXPECT_DOUBLE_EQ(pose.qx, goal.qx); - EXPECT_DOUBLE_EQ(pose.qy, goal.qy); - EXPECT_DOUBLE_EQ(pose.qz, goal.qz); -} - -TEST_F(WaypointFollowerTests, StateEvolvesWithStep) { - WaypointFollower follower(get_params(), 0.01); - - Waypoint wp; - wp.pose = Pose{1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0}; - wp.mode = WaypointMode::FULL_POSE; - - follower.start(zero_pose(), zero_twist(), wp, 0.1); - - // Run several steps — state should move toward reference - for (int i = 0; i < 100; ++i) { - follower.step(); - } - - // x position should have moved toward 1.0 - EXPECT_GT(follower.pose().x, 0.0); -} - -} // namespace vortex::guidance - -int main(int argc, char** argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} From c106bbbce852d40f18db697299337ae298da5086 Mon Sep 17 00:00:00 2001 From: Henrik Date: Sun, 29 Mar 2026 18:03:46 +0200 Subject: [PATCH 123/128] new try at reverting guidance to version from main --- guidance/los_guidance/CMakeLists.txt | 32 ++- .../los_guidance/config/guidance_params.yaml | 14 +- .../include/los_guidance/los_guidance.hpp | 62 ++--- .../include/los_guidance/los_guidance_ros.hpp | 24 +- .../launch/los_guidance.launch.py | 57 +++-- guidance/los_guidance/package.xml | 6 +- guidance/los_guidance/src/los_guidance.cpp | 38 +-- .../los_guidance/src/los_guidance_node.cpp | 2 +- .../los_guidance/src/los_guidance_ros.cpp | 72 +++--- guidance/los_guidance/test/CMakeLists.txt | 21 ++ .../los_guidance/test/test_los_guidance.cpp | 182 +++++++++++++ guidance/reference_filter_dp/CMakeLists.txt | 64 +++-- guidance/reference_filter_dp/README.md | 20 +- .../config/reference_filter_params.yaml | 1 + .../lib/eigen_typedefs.hpp | 21 ++ .../lib/reference_filter.hpp | 50 ++++ .../lib/waypoint_follower.hpp | 83 ++++++ .../lib/waypoint_types.hpp | 34 +++ .../lib/waypoint_utils.hpp | 42 +++ .../ros/reference_filter_ros.hpp | 97 +++++++ .../ros/reference_filter_ros_utils.hpp | 69 +++++ .../launch/reference_filter_dp.launch.py | 54 ++-- guidance/reference_filter_dp/package.xml | 4 +- .../src/lib/reference_filter.cpp | 37 +++ .../src/lib/waypoint_follower.cpp | 73 ++++++ .../src/lib/waypoint_utils.cpp | 86 +++++++ .../src/ros/reference_filter_ros.cpp | 239 ++++++++++++++++++ .../reference_filter_dp/test/CMakeLists.txt | 57 +++++ .../test/test_reference_filter.cpp | 58 +++++ .../test/test_waypoint_follower.cpp | 111 ++++++++ .../test/test_waypoint_utils.cpp | 174 +++++++++++++ .../reference_filter_dp_quat/CMakeLists.txt | 105 ++++++++ guidance/reference_filter_dp_quat/README.md | 106 ++++++++ .../config/reference_filter_params.yaml | 5 + .../lib/eigen_typedefs.hpp | 21 ++ .../lib/reference_filter.hpp | 50 ++++ .../lib/waypoint_follower.hpp | 112 ++++++++ .../ros/reference_filter_ros.hpp | 95 +++++++ .../ros/reference_filter_ros_utils.hpp | 58 +++++ .../launch/reference_filter_dp_quat.launch.py | 64 +++++ guidance/reference_filter_dp_quat/package.xml | 24 ++ .../src/lib/reference_filter.cpp | 37 +++ .../src/lib/waypoint_follower.cpp | 98 +++++++ .../src/ros/reference_filter_ros.cpp | 237 +++++++++++++++++ .../test/CMakeLists.txt | 39 +++ .../test/test_reference_filter.cpp | 58 +++++ .../test/test_waypoint_follower.cpp | 112 ++++++++ 47 files changed, 2934 insertions(+), 171 deletions(-) create mode 100644 guidance/los_guidance/test/CMakeLists.txt create mode 100644 guidance/los_guidance/test/test_los_guidance.cpp create mode 100644 guidance/reference_filter_dp/include/reference_filter_dp/lib/eigen_typedefs.hpp create mode 100644 guidance/reference_filter_dp/include/reference_filter_dp/lib/reference_filter.hpp create mode 100644 guidance/reference_filter_dp/include/reference_filter_dp/lib/waypoint_follower.hpp create mode 100644 guidance/reference_filter_dp/include/reference_filter_dp/lib/waypoint_types.hpp create mode 100644 guidance/reference_filter_dp/include/reference_filter_dp/lib/waypoint_utils.hpp create mode 100644 guidance/reference_filter_dp/include/reference_filter_dp/ros/reference_filter_ros.hpp create mode 100644 guidance/reference_filter_dp/include/reference_filter_dp/ros/reference_filter_ros_utils.hpp create mode 100644 guidance/reference_filter_dp/src/lib/reference_filter.cpp create mode 100644 guidance/reference_filter_dp/src/lib/waypoint_follower.cpp create mode 100644 guidance/reference_filter_dp/src/lib/waypoint_utils.cpp create mode 100644 guidance/reference_filter_dp/src/ros/reference_filter_ros.cpp create mode 100644 guidance/reference_filter_dp/test/CMakeLists.txt create mode 100644 guidance/reference_filter_dp/test/test_reference_filter.cpp create mode 100644 guidance/reference_filter_dp/test/test_waypoint_follower.cpp create mode 100644 guidance/reference_filter_dp/test/test_waypoint_utils.cpp create mode 100644 guidance/reference_filter_dp_quat/CMakeLists.txt create mode 100644 guidance/reference_filter_dp_quat/README.md create mode 100644 guidance/reference_filter_dp_quat/config/reference_filter_params.yaml create mode 100644 guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/eigen_typedefs.hpp create mode 100644 guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/reference_filter.hpp create mode 100644 guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_follower.hpp create mode 100644 guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros.hpp create mode 100644 guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros_utils.hpp create mode 100644 guidance/reference_filter_dp_quat/launch/reference_filter_dp_quat.launch.py create mode 100644 guidance/reference_filter_dp_quat/package.xml create mode 100644 guidance/reference_filter_dp_quat/src/lib/reference_filter.cpp create mode 100644 guidance/reference_filter_dp_quat/src/lib/waypoint_follower.cpp create mode 100644 guidance/reference_filter_dp_quat/src/ros/reference_filter_ros.cpp create mode 100644 guidance/reference_filter_dp_quat/test/CMakeLists.txt create mode 100644 guidance/reference_filter_dp_quat/test/test_reference_filter.cpp create mode 100644 guidance/reference_filter_dp_quat/test/test_waypoint_follower.cpp diff --git a/guidance/los_guidance/CMakeLists.txt b/guidance/los_guidance/CMakeLists.txt index dd243a31e..90b51de3a 100644 --- a/guidance/los_guidance/CMakeLists.txt +++ b/guidance/los_guidance/CMakeLists.txt @@ -13,34 +13,44 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(vortex_msgs REQUIRED) +find_package(vortex_utils_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(Eigen3 REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_geometry_msgs REQUIRED) find_package(spdlog REQUIRED) find_package(fmt REQUIRED) include_directories(include) -add_executable(los_guidance_node - src/los_guidance_node.cpp - src/los_guidance_ros.cpp +set(LIB_NAME los_guidance_lib) +add_library(${LIB_NAME} SHARED src/los_guidance.cpp + src/los_guidance_ros.cpp ) -ament_target_dependencies(los_guidance_node +ament_target_dependencies(${LIB_NAME} rclcpp rclcpp_action geometry_msgs vortex_msgs + vortex_utils_ros Eigen3 - tf2 - tf2_geometry_msgs spdlog fmt ) -target_link_libraries(los_guidance_node fmt::fmt) +add_executable(los_guidance_node + src/los_guidance_node.cpp +) + +target_link_libraries(los_guidance_node ${LIB_NAME}) + +install(TARGETS + ${LIB_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) install(TARGETS los_guidance_node @@ -58,4 +68,8 @@ install(DIRECTORY DESTINATION share/${PROJECT_NAME}/ ) +if(BUILD_TESTING) + add_subdirectory(test) +endif() + ament_package() diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 0a04dec0a..a580cf57c 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -1,8 +1,10 @@ /**: ros__parameters: - lookahead_distance_h: 1.5 - lookahead_distance_v: 0.6 - gamma_h: 0.05 - gamma_v: 0.1 - time_step: 0.01 - u_desired: 0.3 + los: + lookahead_distance_h: 1.5 + lookahead_distance_v: 0.6 + gamma_h: 0.05 + gamma_v: 0.1 + time_step: 0.01 + u_desired: 0.3 + goal_reached_tol: 1.0 diff --git a/guidance/los_guidance/include/los_guidance/los_guidance.hpp b/guidance/los_guidance/include/los_guidance/los_guidance.hpp index d73830ba8..71ddfdb11 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance.hpp @@ -1,13 +1,16 @@ -#ifndef LOS_GUIDANCE_HPP -#define LOS_GUIDANCE_HPP +#ifndef LOS_GUIDANCE__LOS_GUIDANCE_HPP_ +#define LOS_GUIDANCE__LOS_GUIDANCE_HPP_ #include +namespace vortex::guidance { + namespace LOS { + struct Point { - double x; - double y; - double z; + double x{}; + double y{}; + double z{}; Point operator-(const Point& other) const { return Point{x - other.x, y - other.y, z - other.z}; @@ -17,9 +20,9 @@ struct Point { }; struct CrossTrackError { - double x_e; - double y_e; - double z_e; + double x_e{}; + double y_e{}; + double z_e{}; inline static CrossTrackError from_vector(const Eigen::Vector3d& vector) { return CrossTrackError{vector.x(), vector.y(), vector.z()}; @@ -27,13 +30,14 @@ struct CrossTrackError { }; struct Params { - double lookahead_distance_h; - double lookahead_distance_v; - double gamma_h; - double gamma_v; - double time_step; + double lookahead_distance_h{}; + double lookahead_distance_v{}; + double gamma_h{}; + double gamma_v{}; + double time_step{}; }; -}; // namespace LOS + +} // namespace LOS /** * @brief Adaptive Line-of-Sight (LOS) guidance algorithm based on slide 113 @@ -41,7 +45,7 @@ struct Params { */ class AdaptiveLOSGuidance { public: - AdaptiveLOSGuidance(const LOS::Params& params); + explicit AdaptiveLOSGuidance(const LOS::Params& params); ~AdaptiveLOSGuidance() = default; void update_angles(const LOS::Point& prev_point, @@ -49,27 +53,25 @@ class AdaptiveLOSGuidance { LOS::CrossTrackError calculate_crosstrack_error( const LOS::Point& prev_point, - const LOS::Point& current_position); + const LOS::Point& current_position) const; - double calculate_psi_d(const double& y_e); + double calculate_psi_d(const double& y_e) const; - double calculate_theta_d(const double& z_e); + double calculate_theta_d(const double& z_e) const; void update_adaptive_estimates( const LOS::CrossTrackError& crosstrack_error); private: - double time_step_; - double lookahead_distance_h_; - double lookahead_distance_v_; - double gamma_h_; - double gamma_v_; - Eigen::Matrix3d rotation_y_; - Eigen::Matrix3d rotation_z_; - double pi_h_; - double pi_v_; - double beta_c_hat_ = 0.0; - double alpha_c_hat_ = 0.0; + LOS::Params params_; + Eigen::Matrix3d rotation_y_ = Eigen::Matrix3d::Zero(); + Eigen::Matrix3d rotation_z_ = Eigen::Matrix3d::Zero(); + double pi_h_{}; + double pi_v_{}; + double beta_c_hat_{}; + double alpha_c_hat_{}; }; -#endif // LOS_GUIDANCE_HPP +} // namespace vortex::guidance + +#endif // LOS_GUIDANCE__LOS_GUIDANCE_HPP_ diff --git a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp index 674996a08..623dac5da 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -1,23 +1,23 @@ -#ifndef REFERENCE_FILTER_ROS_HPP -#define REFERENCE_FILTER_ROS_HPP +#ifndef LOS_GUIDANCE__LOS_GUIDANCE_ROS_HPP_ +#define LOS_GUIDANCE__LOS_GUIDANCE_ROS_HPP_ -#include -#include #include #include #include #include +#include #include #include -#include #include #include #include #include "los_guidance.hpp" +namespace vortex::guidance { + class LOSGuidanceNode : public rclcpp::Node { public: - explicit LOSGuidanceNode(); + LOSGuidanceNode(); private: // @brief Set the subscribers and publishers @@ -105,11 +105,15 @@ class LOSGuidanceNode : public rclcpp::Node { std::unique_ptr adaptive_los_guidance_; - double yaw_d_; + double yaw_d_{}; + + double pitch_d_{}; - double pitch_d_; + double u_desired_{}; - double u_desired_; + double goal_reached_tol_{}; }; -#endif +} // namespace vortex::guidance + +#endif // LOS_GUIDANCE__LOS_GUIDANCE_ROS_HPP_ diff --git a/guidance/los_guidance/launch/los_guidance.launch.py b/guidance/los_guidance/launch/los_guidance.launch.py index 03366e897..fb1749a3f 100644 --- a/guidance/los_guidance/launch/los_guidance.launch.py +++ b/guidance/los_guidance/launch/los_guidance.launch.py @@ -1,32 +1,45 @@ -from os import path +import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription +from launch.actions import OpaqueFunction from launch_ros.actions import Node -adapt_params = path.join( - get_package_share_directory("los_guidance"), - "config", - "guidance_params.yaml", -) -orca_params = path.join( - get_package_share_directory("auv_setup"), - "config", - "robots", - "orca.yaml", +from auv_setup.launch_arg_common import ( + declare_drone_and_namespace_args, + resolve_drone_and_namespace, ) +def launch_setup(context, *args, **kwargs): + drone, namespace = resolve_drone_and_namespace(context) + + los_config = os.path.join( + get_package_share_directory("los_guidance"), + "config", + "guidance_params.yaml", + ) + + drone_params = os.path.join( + get_package_share_directory("auv_setup"), + "config", + "robots", + f"{drone}.yaml", + ) + + return [ + Node( + package="los_guidance", + executable="los_guidance_node", + name="los_guidance_node", + namespace=namespace, + parameters=[drone_params, los_config], + output="screen", + ) + ] + + def generate_launch_description(): - los_guidance_node = Node( - package="los_guidance", - executable="los_guidance_node", - name="los_guidance_node", - namespace="orca", - parameters=[ - orca_params, - adapt_params, - ], - output="screen", + return LaunchDescription( + declare_drone_and_namespace_args() + [OpaqueFunction(function=launch_setup)] ) - return LaunchDescription([los_guidance_node]) diff --git a/guidance/los_guidance/package.xml b/guidance/los_guidance/package.xml index a3d2a9a8a..5ff6218fe 100644 --- a/guidance/los_guidance/package.xml +++ b/guidance/los_guidance/package.xml @@ -3,7 +3,7 @@ los_guidance 0.0.0 - LOS Guidance modual for guidance of AUV + LOS Guidance module for guidance of AUV talhanc MIT @@ -13,10 +13,8 @@ rclcpp_action geometry_msgs vortex_msgs - nav_msgs + vortex_utils_ros eigen - tf2 - tf2_geometry_msgs ament_cmake diff --git a/guidance/los_guidance/src/los_guidance.cpp b/guidance/los_guidance/src/los_guidance.cpp index 49a521ff2..3171d5442 100644 --- a/guidance/los_guidance/src/los_guidance.cpp +++ b/guidance/los_guidance/src/los_guidance.cpp @@ -1,11 +1,9 @@ -#include +#include "los_guidance/los_guidance.hpp" + +namespace vortex::guidance { AdaptiveLOSGuidance::AdaptiveLOSGuidance(const LOS::Params& params) - : time_step_(params.time_step), - lookahead_distance_h_(params.lookahead_distance_h), - lookahead_distance_v_(params.lookahead_distance_v), - gamma_h_(params.gamma_h), - gamma_v_(params.gamma_v) {} + : params_(params) {} void AdaptiveLOSGuidance::update_angles(const LOS::Point& prev_point, const LOS::Point& next_point) { @@ -21,7 +19,7 @@ void AdaptiveLOSGuidance::update_angles(const LOS::Point& prev_point, LOS::CrossTrackError AdaptiveLOSGuidance::calculate_crosstrack_error( const LOS::Point& prev_point, - const LOS::Point& current_position) { + const LOS::Point& current_position) const { const LOS::Point difference = current_position - prev_point; const Eigen::Vector3d difference_vector = difference.as_vector(); @@ -31,29 +29,31 @@ LOS::CrossTrackError AdaptiveLOSGuidance::calculate_crosstrack_error( return LOS::CrossTrackError::from_vector(cross_track_error); } -double AdaptiveLOSGuidance::calculate_psi_d(const double& y_e) { - return pi_h_ - beta_c_hat_ - atan(y_e / lookahead_distance_h_); +double AdaptiveLOSGuidance::calculate_psi_d(const double& y_e) const { + return pi_h_ - beta_c_hat_ - atan(y_e / params_.lookahead_distance_h); } -double AdaptiveLOSGuidance::calculate_theta_d(const double& z_e) { - return pi_v_ + alpha_c_hat_ + atan(z_e / lookahead_distance_v_); +double AdaptiveLOSGuidance::calculate_theta_d(const double& z_e) const { + return pi_v_ + alpha_c_hat_ + atan(z_e / params_.lookahead_distance_v); } void AdaptiveLOSGuidance::update_adaptive_estimates( const LOS::CrossTrackError& crosstrack_error) { double beta_c_hat_dot = - gamma_h_ * - (lookahead_distance_h_ / - sqrt(lookahead_distance_h_ * lookahead_distance_h_ + + params_.gamma_h * + (params_.lookahead_distance_h / + sqrt(params_.lookahead_distance_h * params_.lookahead_distance_h + crosstrack_error.y_e * crosstrack_error.y_e)) * crosstrack_error.y_e; double alpha_c_hat_dot = - gamma_v_ * - (lookahead_distance_v_ / - sqrt(lookahead_distance_v_ * lookahead_distance_v_ + + params_.gamma_v * + (params_.lookahead_distance_v / + sqrt(params_.lookahead_distance_v * params_.lookahead_distance_v + crosstrack_error.z_e * crosstrack_error.z_e)) * crosstrack_error.z_e; - beta_c_hat_ += beta_c_hat_dot * time_step_; - alpha_c_hat_ += alpha_c_hat_dot * time_step_; + beta_c_hat_ += beta_c_hat_dot * params_.time_step; + alpha_c_hat_ += alpha_c_hat_dot * params_.time_step; } + +} // namespace vortex::guidance diff --git a/guidance/los_guidance/src/los_guidance_node.cpp b/guidance/los_guidance/src/los_guidance_node.cpp index 420b97565..931a57ded 100644 --- a/guidance/los_guidance/src/los_guidance_node.cpp +++ b/guidance/los_guidance/src/los_guidance_node.cpp @@ -4,7 +4,7 @@ int main(int argc, char** argv) { rclcpp::init(argc, argv); - auto node = std::make_shared(); + auto node = std::make_shared(); rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(node); diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 0b8215d6e..dcb8d309d 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -1,5 +1,17 @@ +#include "los_guidance/los_guidance_ros.hpp" #include -#include +#include + +const auto start_message = R"( + _ ___ ____ ____ _ _ + | | / _ \/ ___| / ___|_ _(_) __| | __ _ _ __ ___ ___ + | | | | | \___ \ | | _| | | | |/ _` |/ _` | '_ \ / __/ _ \ + | |__| |_| |___) | | |_| | |_| | | (_| | (_| | | | | (_| __/ + |_____\___/|____/ \____|\__,_|_|\__,_|\__,_|_| |_|\___\___| + +)"; + +namespace vortex::guidance { LOSGuidanceNode::LOSGuidanceNode() : Node("los_guidance_node") { time_step_ = std::chrono::milliseconds(10); @@ -10,7 +22,7 @@ LOSGuidanceNode::LOSGuidanceNode() : Node("los_guidance_node") { set_adaptive_los_guidance(); - spdlog::info("LOS guidance node initialized"); + spdlog::info(start_message); } void LOSGuidanceNode::set_subscribers_and_publisher() { @@ -24,9 +36,7 @@ void LOSGuidanceNode::set_subscribers_and_publisher() { std::string waypoint_topic = this->get_parameter("topics.waypoint").as_string(); - rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; - auto qos_sensor_data = rclcpp::QoS( - rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); + auto qos_sensor_data = vortex::utils::qos_profiles::sensor_data_profile(1); reference_pub_ = this->create_publisher( guidance_topic, qos_sensor_data); @@ -63,23 +73,22 @@ void LOSGuidanceNode::set_action_server() { } void LOSGuidanceNode::set_adaptive_los_guidance() { - this->declare_parameter("lookahead_distance_h"); - this->declare_parameter("lookahead_distance_v"); - this->declare_parameter("gamma_h"); - this->declare_parameter("gamma_v"); - this->declare_parameter("time_step"); - this->declare_parameter("u_desired"); - - u_desired_ = this->get_parameter("u_desired").as_double(); + this->declare_parameter("los.lookahead_distance_h"); + this->declare_parameter("los.lookahead_distance_v"); + this->declare_parameter("los.gamma_h"); + this->declare_parameter("los.gamma_v"); + this->declare_parameter("los.time_step"); + this->declare_parameter("los.u_desired"); + this->declare_parameter("los.goal_reached_tol"); LOS::Params params; params.lookahead_distance_h = - this->get_parameter("lookahead_distance_h").as_double(); + this->get_parameter("los.lookahead_distance_h").as_double(); params.lookahead_distance_v = - this->get_parameter("lookahead_distance_v").as_double(); - params.gamma_h = this->get_parameter("gamma_h").as_double(); - params.gamma_v = this->get_parameter("gamma_v").as_double(); - params.time_step = this->get_parameter("time_step").as_double(); + this->get_parameter("los.lookahead_distance_v").as_double(); + params.gamma_h = this->get_parameter("los.gamma_h").as_double(); + params.gamma_v = this->get_parameter("los.gamma_v").as_double(); + params.time_step = this->get_parameter("los.time_step").as_double(); adaptive_los_guidance_ = std::make_unique(params); } @@ -161,6 +170,9 @@ void LOSGuidanceNode::execute( this->goal_handle_ = goal_handle; } + u_desired_ = this->get_parameter("los.u_desired").as_double(); + goal_reached_tol_ = this->get_parameter("los.goal_reached_tol").as_double(); + spdlog::info("Executing goal"); const geometry_msgs::msg::PointStamped los_waypoint = @@ -170,8 +182,6 @@ void LOSGuidanceNode::execute( adaptive_los_guidance_->update_angles(last_point_, next_point_); - auto feedback = - std::make_shared(); auto result = std::make_shared(); rclcpp::Rate loop_rate(1000.0 / time_step_.count()); @@ -201,22 +211,24 @@ void LOSGuidanceNode::execute( adaptive_los_guidance_->update_adaptive_estimates(errors); - vortex_msgs::msg::LOSGuidance reference_msg = fill_los_reference(); - - feedback->feedback = reference_msg; - - goal_handle->publish_feedback(feedback); - reference_pub_->publish(reference_msg); - - if ((eta_ - next_point_).as_vector().norm() < 0.5) { + if ((eta_ - next_point_).as_vector().norm() < goal_reached_tol_) { result->success = true; goal_handle->succeed(result); - vortex_msgs::msg::LOSGuidance reference_msg = fill_los_reference(); - reference_pub_->publish(reference_msg); + u_desired_ = 0.0; + auto final_reference_msg = + std::make_unique( + fill_los_reference()); + reference_pub_->publish(std::move(final_reference_msg)); spdlog::info("Goal reached"); return; } + auto reference_msg = std::make_unique( + fill_los_reference()); + reference_pub_->publish(std::move(reference_msg)); + loop_rate.sleep(); } } + +} // namespace vortex::guidance diff --git a/guidance/los_guidance/test/CMakeLists.txt b/guidance/los_guidance/test/CMakeLists.txt new file mode 100644 index 000000000..69c0dfa52 --- /dev/null +++ b/guidance/los_guidance/test/CMakeLists.txt @@ -0,0 +1,21 @@ +cmake_minimum_required(VERSION 3.8) + +find_package(GTest REQUIRED) +include(GoogleTest) + +set(TEST_BINARY_NAME ${PROJECT_NAME}_test) +add_executable( + ${TEST_BINARY_NAME} + test_los_guidance.cpp +) + +target_link_libraries( + ${TEST_BINARY_NAME} + PRIVATE + ${LIB_NAME} + GTest::GTest +) + +ament_target_dependencies(${TEST_BINARY_NAME} PUBLIC Eigen3) + +gtest_discover_tests(${TEST_BINARY_NAME}) diff --git a/guidance/los_guidance/test/test_los_guidance.cpp b/guidance/los_guidance/test/test_los_guidance.cpp new file mode 100644 index 000000000..78b3d4fe1 --- /dev/null +++ b/guidance/los_guidance/test/test_los_guidance.cpp @@ -0,0 +1,182 @@ +#include + +#include "los_guidance/los_guidance.hpp" + +namespace vortex::guidance { + +class LOSTests : public ::testing::Test { + protected: + LOSTests() : los_guidance_{get_los_params()} {} + + LOS::Params get_los_params() { + LOS::Params params; + params.lookahead_distance_h = 1.0; + params.lookahead_distance_v = 1.0; + params.gamma_h = 1.0; + params.gamma_v = 1.0; + params.time_step = 0.01; + return params; + } + + AdaptiveLOSGuidance los_guidance_; + const double tol = 1e-9; +}; + +TEST_F(LOSTests, T01_test_cross_track_error_on_track) { + LOS::Point prev{0.0, 0.0, 0.0}; + LOS::Point next{1.0, 0.0, 0.0}; + los_guidance_.update_angles(prev, next); + LOS::Point current{0.0, 0.0, 0.0}; + LOS::CrossTrackError e = + los_guidance_.calculate_crosstrack_error(prev, current); + + EXPECT_NEAR(e.x_e, 0.0, tol); + EXPECT_NEAR(e.y_e, 0.0, tol); + EXPECT_NEAR(e.z_e, 0.0, tol); +} + +TEST_F(LOSTests, T02_test_cross_track_error_right_off_track) { + LOS::Point prev{0.0, 0.0, 0.0}; + LOS::Point next{1.0, 0.0, 0.0}; + los_guidance_.update_angles(prev, next); + LOS::Point current{0.0, 0.5, 0.0}; + LOS::CrossTrackError e = + los_guidance_.calculate_crosstrack_error(prev, current); + + EXPECT_NEAR(e.x_e, 0.0, tol); + EXPECT_NEAR(e.y_e, 0.5, tol); + EXPECT_NEAR(e.z_e, 0.0, tol); +} + +TEST_F(LOSTests, T03_test_cross_track_error_left_off_track) { + LOS::Point prev{0.0, 0.0, 0.0}; + LOS::Point next{1.0, 0.0, 0.0}; + los_guidance_.update_angles(prev, next); + LOS::Point current{0.0, -0.5, 0.0}; + LOS::CrossTrackError e = + los_guidance_.calculate_crosstrack_error(prev, current); + + EXPECT_NEAR(e.x_e, 0.0, tol); + EXPECT_NEAR(e.y_e, -0.5, tol); + EXPECT_NEAR(e.z_e, 0.0, tol); +} + +TEST_F(LOSTests, T04_test_cross_track_error_under_track) { + LOS::Point prev{0.0, 0.0, 0.0}; + LOS::Point next{1.0, 0.0, 0.0}; + los_guidance_.update_angles(prev, next); + LOS::Point current{0.0, 0.0, 0.5}; + LOS::CrossTrackError e = + los_guidance_.calculate_crosstrack_error(prev, current); + + EXPECT_NEAR(e.x_e, 0.0, tol); + EXPECT_NEAR(e.y_e, 0.0, tol); + EXPECT_NEAR(e.z_e, 0.5, tol); +} + +TEST_F(LOSTests, T05_test_cross_track_error_over_track) { + LOS::Point prev{0.0, 0.0, 0.0}; + LOS::Point next{1.0, 0.0, 0.0}; + los_guidance_.update_angles(prev, next); + LOS::Point current{0.0, 0.0, -0.5}; + LOS::CrossTrackError e = + los_guidance_.calculate_crosstrack_error(prev, current); + + EXPECT_NEAR(e.x_e, 0.0, tol); + EXPECT_NEAR(e.y_e, 0.0, tol); + EXPECT_NEAR(e.z_e, -0.5, tol); +} + +// Test commanded angles when drone is to the right of the track +TEST_F(LOSTests, T06_test_commanded_angles) { + LOS::Point prev{0.0, 0.0, 0.0}; + LOS::Point next{1.0, 0.0, 0.0}; + los_guidance_.update_angles(prev, next); + LOS::Point current{0.0, 0.5, 0.0}; + LOS::CrossTrackError e = + los_guidance_.calculate_crosstrack_error(prev, current); + double psi_d{los_guidance_.calculate_psi_d(e.y_e)}; + double theta_d{los_guidance_.calculate_theta_d(e.z_e)}; + // Heading cmd should be between -pi/2 and 0 + EXPECT_LT(psi_d, 0.0); + EXPECT_GT(psi_d, -1.57); + // Pitch cmd should be zero + EXPECT_NEAR(theta_d, 0.0, tol); +} + +// Test commanded angles when drone is to the left of the track +TEST_F(LOSTests, T07_test_commanded_angles) { + LOS::Point prev{0.0, 0.0, 0.0}; + LOS::Point next{1.0, 0.0, 0.0}; + los_guidance_.update_angles(prev, next); + LOS::Point current{0.0, -0.5, 0.0}; + LOS::CrossTrackError e = + los_guidance_.calculate_crosstrack_error(prev, current); + double psi_d{los_guidance_.calculate_psi_d(e.y_e)}; + double theta_d{los_guidance_.calculate_theta_d(e.z_e)}; + // Heading cmd should be between 0 and pi/2 + EXPECT_GT(psi_d, 0.0); + EXPECT_LT(psi_d, 1.57); + // Pitch cmd should be zero + EXPECT_NEAR(theta_d, 0.0, tol); +} + +// Test commanded angles when drone is under the track +TEST_F(LOSTests, T08_test_commanded_angles) { + LOS::Point prev{0.0, 0.0, 0.0}; + LOS::Point next{1.0, 0.0, 0.0}; + los_guidance_.update_angles(prev, next); + LOS::Point current{0.0, 0.0, 0.5}; + LOS::CrossTrackError e = + los_guidance_.calculate_crosstrack_error(prev, current); + double psi_d{los_guidance_.calculate_psi_d(e.y_e)}; + double theta_d{los_guidance_.calculate_theta_d(e.z_e)}; + // Heading cmd should be 0 + EXPECT_NEAR(psi_d, 0.0, tol); + // Pitch cmd should be between 0 and pi/2 + EXPECT_GT(theta_d, 0.0); + EXPECT_LT(theta_d, 1.57); +} + +// Test commanded angles when drone is over the track +TEST_F(LOSTests, T09_test_commanded_angles) { + LOS::Point prev{0.0, 0.0, 0.0}; + LOS::Point next{1.0, 0.0, 0.0}; + los_guidance_.update_angles(prev, next); + LOS::Point current{0.0, 0.0, -0.5}; + LOS::CrossTrackError e = + los_guidance_.calculate_crosstrack_error(prev, current); + double psi_d{los_guidance_.calculate_psi_d(e.y_e)}; + double theta_d{los_guidance_.calculate_theta_d(e.z_e)}; + // Heading cmd should be 0 + EXPECT_NEAR(psi_d, 0.0, tol); + // Pitch cmd should be between -pi/2 and 0 + EXPECT_LT(theta_d, 0.0); + EXPECT_GT(theta_d, -1.57); +} + +// Test commanded angles when drone is over and to the right of the track +TEST_F(LOSTests, T10_test_commanded_angles) { + LOS::Point prev{0.0, 0.0, 0.0}; + LOS::Point next{1.0, 0.0, 0.0}; + los_guidance_.update_angles(prev, next); + LOS::Point current{0.0, 0.5, -0.5}; + LOS::CrossTrackError e = + los_guidance_.calculate_crosstrack_error(prev, current); + double psi_d{los_guidance_.calculate_psi_d(e.y_e)}; + double theta_d{los_guidance_.calculate_theta_d(e.z_e)}; + // Heading cmd should be between -pi/2 and 0 + EXPECT_LT(psi_d, 0.0); + EXPECT_GT(psi_d, -1.57); + // Pitch cmd should be between -pi/2 and 0 + EXPECT_LT(theta_d, 0.0); + EXPECT_GT(theta_d, -1.57); +} + +} // namespace vortex::guidance + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} diff --git a/guidance/reference_filter_dp/CMakeLists.txt b/guidance/reference_filter_dp/CMakeLists.txt index f02cfb95b..b5213e31e 100644 --- a/guidance/reference_filter_dp/CMakeLists.txt +++ b/guidance/reference_filter_dp/CMakeLists.txt @@ -14,47 +14,67 @@ find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(rclcpp_components REQUIRED) find_package(vortex_msgs REQUIRED) +find_package(vortex_utils REQUIRED) +find_package(vortex_utils_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(Eigen3 REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_geometry_msgs REQUIRED) find_package(spdlog REQUIRED) find_package(fmt REQUIRED) include_directories(include) -set(LIB_NAME "${PROJECT_NAME}_component") +set(CORE_LIB_NAME "${PROJECT_NAME}") -add_library(${LIB_NAME} SHARED - src/reference_filter.cpp - src/reference_filter_ros.cpp - src/reference_filter_utils.cpp) +add_library(${CORE_LIB_NAME} SHARED + src/lib/reference_filter.cpp + src/lib/waypoint_utils.cpp + src/lib/waypoint_follower.cpp +) + +target_include_directories(${CORE_LIB_NAME} + PUBLIC + $ + $ +) + +ament_target_dependencies(${CORE_LIB_NAME} + Eigen3 + vortex_utils +) + +set(COMPONENT_LIB_NAME "${PROJECT_NAME}_component") + +add_library(${COMPONENT_LIB_NAME} SHARED + src/ros/reference_filter_ros.cpp +) -ament_target_dependencies(${LIB_NAME} PUBLIC +ament_target_dependencies(${COMPONENT_LIB_NAME} rclcpp rclcpp_components rclcpp_action geometry_msgs nav_msgs - Eigen3 - tf2 - tf2_geometry_msgs vortex_msgs + vortex_utils + vortex_utils_ros spdlog fmt ) +target_link_libraries(${COMPONENT_LIB_NAME} ${CORE_LIB_NAME}) + rclcpp_components_register_node( - ${LIB_NAME} + ${COMPONENT_LIB_NAME} PLUGIN "ReferenceFilterNode" EXECUTABLE ${PROJECT_NAME}_node ) -ament_export_targets(export_${LIB_NAME}) - -install(TARGETS ${LIB_NAME} - EXPORT export_${LIB_NAME} +install( + TARGETS + ${CORE_LIB_NAME} + ${COMPONENT_LIB_NAME} + EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -71,4 +91,16 @@ install(DIRECTORY DESTINATION share/${PROJECT_NAME}/ ) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + Eigen3 + vortex_utils +) +ament_export_include_directories(include) +ament_export_libraries(${CORE_LIB_NAME}) + +if(BUILD_TESTING) + add_subdirectory(test) +endif() + ament_package() diff --git a/guidance/reference_filter_dp/README.md b/guidance/reference_filter_dp/README.md index 0ccfbf28c..4fe004335 100644 --- a/guidance/reference_filter_dp/README.md +++ b/guidance/reference_filter_dp/README.md @@ -33,10 +33,28 @@ The state at the next time step is calculated using forward Euler integration x_{i+1} = x_i + \dot{x}_i * dt ``` +## Waypoint modes + +Each waypoint has a mode that determines which degrees of freedom are controlled by the reference filter. The mode also determines how convergence is measured. + +| Mode | Controlled DOFs | Convergence metric | +|---|---|---| +| `FULL_POSE` | All 6 DOF (position + orientation) | Euclidean norm of position and angle errors | +| `ONLY_POSITION` | x, y, z (orientation holds current value) | Euclidean norm of position error | +| `FORWARD_HEADING` | x, y, z + yaw toward target (roll/pitch = 0) | Euclidean norm of position error and yaw error | +| `ONLY_ORIENTATION` | roll, pitch, yaw (position holds current value) | Euclidean norm of angle errors | + +For all modes, convergence is reached when the error metric drops below the `convergence_threshold` specified in the action goal. + ## Action Server -The action server is responsible for handling goal requests and publishing guidance commands. The server will always prioritize new goal request, and will abort ongoing request when getting a new request. The action definition can be found [here](https://github.com/vortexntnu/vortex-msgs/blob/main/action/ReferenceFilterWaypoint.action). + +The action server is responsible for handling goal requests and publishing guidance commands. The server will always prioritize new goal requests, and will abort ongoing requests when getting a new request. The action definition can be found [here](https://github.com/vortexntnu/vortex-msgs/blob/main/action/ReferenceFilterWaypoint.action). - Action name: /reference_filter - Goal type: PoseStamped - Result type: bool - Guidance topic: /dp/reference + +### Overwriting the reference during an action + +While an action is executing, the reference goal can be updated at any time by publishing a `geometry_msgs/msg/PoseStamped` to the reference pose topic. This allows external nodes to adjust the target pose mid-action without canceling and resending the goal. The convergence check will use the updated reference, so the action completes when the vehicle reaches the latest reference. diff --git a/guidance/reference_filter_dp/config/reference_filter_params.yaml b/guidance/reference_filter_dp/config/reference_filter_params.yaml index a9cf24aae..1aef0ec13 100644 --- a/guidance/reference_filter_dp/config/reference_filter_params.yaml +++ b/guidance/reference_filter_dp/config/reference_filter_params.yaml @@ -2,3 +2,4 @@ ros__parameters: zeta: [1., 1., 1., 1., 1., 1.] omega: [0.2, 0.2, 0.2, 0.2, 0.2, 0.2] + time_step_ms: 10 diff --git a/guidance/reference_filter_dp/include/reference_filter_dp/lib/eigen_typedefs.hpp b/guidance/reference_filter_dp/include/reference_filter_dp/lib/eigen_typedefs.hpp new file mode 100644 index 000000000..bd9a358e7 --- /dev/null +++ b/guidance/reference_filter_dp/include/reference_filter_dp/lib/eigen_typedefs.hpp @@ -0,0 +1,21 @@ +/** + * @file eigen_typedefs.hpp + * @brief Contains Eigen typedefs used in this package. + */ + +#ifndef REFERENCE_FILTER_DP__LIB__EIGEN_TYPEDEFS_HPP_ +#define REFERENCE_FILTER_DP__LIB__EIGEN_TYPEDEFS_HPP_ + +#include + +namespace Eigen { + +typedef Eigen::Matrix Matrix18d; +typedef Eigen::Matrix Matrix18x6d; +typedef Eigen::Matrix Matrix6d; +typedef Eigen::Matrix Vector6d; +typedef Eigen::Matrix Vector18d; + +} // namespace Eigen + +#endif // REFERENCE_FILTER_DP__LIB__EIGEN_TYPEDEFS_HPP_ diff --git a/guidance/reference_filter_dp/include/reference_filter_dp/lib/reference_filter.hpp b/guidance/reference_filter_dp/include/reference_filter_dp/lib/reference_filter.hpp new file mode 100644 index 000000000..c9e3973db --- /dev/null +++ b/guidance/reference_filter_dp/include/reference_filter_dp/lib/reference_filter.hpp @@ -0,0 +1,50 @@ +#ifndef REFERENCE_FILTER_DP__LIB__REFERENCE_FILTER_HPP_ +#define REFERENCE_FILTER_DP__LIB__REFERENCE_FILTER_HPP_ + +#include "reference_filter_dp/lib/eigen_typedefs.hpp" + +namespace vortex::guidance { + +struct ReferenceFilterParams { + Eigen::Vector6d omega = Eigen::Vector6d::Zero(); + Eigen::Vector6d zeta = Eigen::Vector6d::Zero(); +}; + +/** + * @brief Stateless third-order reference filter. + * + * Computes the state derivative x_dot = Ad * x + Bd * r, where the state + * x = [eta, eta_dot, eta_ddot] is 18-dimensional and r is the 6D reference. + */ +class ReferenceFilter { + public: + explicit ReferenceFilter(const ReferenceFilterParams& params); + + // @brief Calculate the state derivative + // @param x The state vector 18x1 + // @param r The reference vector 6x1 + // @return The state derivative 18x1 + // REF: Handbook of Marine Craft Hydrodynamics and Motion Control, Fossen + // 2021 p. 337 eq: 12.11 + Eigen::Vector18d calculate_x_dot(const Eigen::Vector18d& x, + const Eigen::Vector6d& r); + + // @brief Calculate the state transition matrix + // REF: Handbook of Marine Craft Hydrodynamics and Motion Control, Fossen + // 2021 p. 337 eq: 12.12 + void calculate_Ad(const Eigen::Vector6d& omega, + const Eigen::Vector6d& zeta); + + // @brief Calculate the input matrix + // REF: Handbook of Marine Craft Hydrodynamics and Motion Control, Fossen + // 2021 p. 337 eq: 12.12 + void calculate_Bd(const Eigen::Vector6d& omega); + + private: + Eigen::Matrix18d Ad_ = Eigen::Matrix18d::Zero(); + Eigen::Matrix18x6d Bd_ = Eigen::Matrix18x6d::Zero(); +}; + +} // namespace vortex::guidance + +#endif // REFERENCE_FILTER_DP__LIB__REFERENCE_FILTER_HPP_ diff --git a/guidance/reference_filter_dp/include/reference_filter_dp/lib/waypoint_follower.hpp b/guidance/reference_filter_dp/include/reference_filter_dp/lib/waypoint_follower.hpp new file mode 100644 index 000000000..121a9f431 --- /dev/null +++ b/guidance/reference_filter_dp/include/reference_filter_dp/lib/waypoint_follower.hpp @@ -0,0 +1,83 @@ +#ifndef REFERENCE_FILTER_DP__LIB__WAYPOINT_FOLLOWER_HPP_ +#define REFERENCE_FILTER_DP__LIB__WAYPOINT_FOLLOWER_HPP_ + +#include +#include +#include "reference_filter_dp/lib/eigen_typedefs.hpp" +#include "reference_filter_dp/lib/reference_filter.hpp" +#include "reference_filter_dp/lib/waypoint_types.hpp" + +namespace vortex::guidance { + +using vortex::utils::types::PoseEuler; +using vortex::utils::types::Twist; + +/** + * @brief Manages reference filter state and waypoint following logic. + * + * Thread-safe: all public methods acquire a mutex. + */ +class WaypointFollower { + public: + WaypointFollower(const ReferenceFilterParams& params, double dt_seconds); + + /** + * @brief Initialize the follower with the current vehicle state and target. + * @param pose Current vehicle pose. + * @param twist Current vehicle twist (body frame). + * @param waypoint Target waypoint with mode. + * @param convergence_threshold Max error norm to consider target reached. + */ + void start(const PoseEuler& pose, + const Twist& twist, + const Waypoint& waypoint, + double convergence_threshold); + + /** + * @brief Advance the filter by one time step. + * @return The updated filter state. + */ + Eigen::Vector18d step(); + + /** + * @brief Check if the measured pose has converged to the reference goal. + * @param measured_pose Current measured pose. + * @return True if the error norm is within the convergence threshold. + */ + bool within_convergance(const Eigen::Vector6d& measured_pose) const; + + /** + * @brief Update the reference goal pose mid-sequence. + * @param reference_goal_pose The new reference pose. + */ + void set_reference(const PoseEuler& reference_goal_pose); + + /** + * @brief Snap the position component of the filter state to the reference. + * + * Useful after convergence to eliminate any remaining steady-state offset + */ + void snap_state_to_reference(); + + /// @brief Get the current 18D filter state. + Eigen::Vector18d state() const; + + /// @brief Get the current 6D reference goal pose. + Eigen::Vector6d reference() const; + + private: + Eigen::Vector18d compute_initial_state(const PoseEuler& pose, + const Twist& twist); + + mutable std::mutex mutex_; + ReferenceFilter filter_; + double dt_seconds_{0.01}; + Eigen::Vector18d state_ = Eigen::Vector18d::Zero(); + Eigen::Vector6d reference_goal_ = Eigen::Vector6d::Zero(); + WaypointMode waypoint_mode_{WaypointMode::FULL_POSE}; + double convergence_threshold_{0.1}; +}; + +} // namespace vortex::guidance + +#endif // REFERENCE_FILTER_DP__LIB__WAYPOINT_FOLLOWER_HPP_ diff --git a/guidance/reference_filter_dp/include/reference_filter_dp/lib/waypoint_types.hpp b/guidance/reference_filter_dp/include/reference_filter_dp/lib/waypoint_types.hpp new file mode 100644 index 000000000..ed8e8e2db --- /dev/null +++ b/guidance/reference_filter_dp/include/reference_filter_dp/lib/waypoint_types.hpp @@ -0,0 +1,34 @@ +#ifndef REFERENCE_FILTER_DP__LIB__WAYPOINT_TYPES_HPP_ +#define REFERENCE_FILTER_DP__LIB__WAYPOINT_TYPES_HPP_ + +#include +#include + +namespace vortex::guidance { + +using vortex::utils::types::PoseEuler; + +/** + * @brief Determines which degrees of freedom the reference filter controls. + * + * The mode affects both the reference goal computation (via apply_mode_logic) + * and the convergence check (via has_converged). + */ +enum class WaypointMode : uint8_t { + FULL_POSE = 0, ///< Control all 6 DOF. + ONLY_POSITION = 1, ///< Control x, y, z; hold current orientation. + FORWARD_HEADING = 2, ///< Control x, y, z with yaw toward target. + ONLY_ORIENTATION = 3, ///< Control roll, pitch, yaw; hold current position. +}; + +/** + * @brief A target pose with an associated waypoint mode. + */ +struct Waypoint { + PoseEuler pose{}; + WaypointMode mode = WaypointMode::FULL_POSE; +}; + +} // namespace vortex::guidance + +#endif // REFERENCE_FILTER_DP__LIB__WAYPOINT_TYPES_HPP_ diff --git a/guidance/reference_filter_dp/include/reference_filter_dp/lib/waypoint_utils.hpp b/guidance/reference_filter_dp/include/reference_filter_dp/lib/waypoint_utils.hpp new file mode 100644 index 000000000..0db3340ad --- /dev/null +++ b/guidance/reference_filter_dp/include/reference_filter_dp/lib/waypoint_utils.hpp @@ -0,0 +1,42 @@ +#ifndef REFERENCE_FILTER_DP__LIB__WAYPOINT_UTILS_HPP_ +#define REFERENCE_FILTER_DP__LIB__WAYPOINT_UTILS_HPP_ + +#include "reference_filter_dp/lib/eigen_typedefs.hpp" +#include "reference_filter_dp/lib/waypoint_types.hpp" + +namespace vortex::guidance { + +/** + * @brief Apply waypoint mode logic to a reference pose. + * + * For modes that don't control all DOFs, the uncontrolled components are + * replaced with values from @p current_state so the filter holds them steady. + * + * @param r_in The raw reference pose (6D). + * @param mode The waypoint mode. + * @param current_state The current filter state pose (6D). + * @return The adjusted reference pose. + */ +Eigen::Vector6d apply_mode_logic(const Eigen::Vector6d& r_in, + WaypointMode mode, + const Eigen::Vector6d& current_state); + +/** + * @brief Check whether the measured pose has converged to the reference. + * + * Only the DOFs relevant to the waypoint mode are included in the error norm. + * + * @param measured_pose The current measured pose (6D). + * @param reference The reference goal pose (6D). + * @param mode The waypoint mode. + * @param convergence_threshold The maximum allowed error norm. + * @return True if the error is below the threshold. + */ +bool has_converged(const Eigen::Vector6d& measured_pose, + const Eigen::Vector6d& reference, + WaypointMode mode, + double convergence_threshold); + +} // namespace vortex::guidance + +#endif // REFERENCE_FILTER_DP__LIB__WAYPOINT_UTILS_HPP_ diff --git a/guidance/reference_filter_dp/include/reference_filter_dp/ros/reference_filter_ros.hpp b/guidance/reference_filter_dp/include/reference_filter_dp/ros/reference_filter_ros.hpp new file mode 100644 index 000000000..636d10a34 --- /dev/null +++ b/guidance/reference_filter_dp/include/reference_filter_dp/ros/reference_filter_ros.hpp @@ -0,0 +1,97 @@ +#ifndef REFERENCE_FILTER_DP__ROS__REFERENCE_FILTER_ROS_HPP_ +#define REFERENCE_FILTER_DP__ROS__REFERENCE_FILTER_ROS_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "reference_filter_dp/lib/waypoint_follower.hpp" + +namespace vortex::guidance { + +class ReferenceFilterNode : public rclcpp::Node { + public: + explicit ReferenceFilterNode( + const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); + + ~ReferenceFilterNode(); + + private: + // @brief Set the subscribers and publishers + void set_subscribers_and_publisher(); + + // @brief Set the action server + void set_action_server(); + + // @brief Initializes the reference filter with ROS parameters. + void set_refererence_filter(); + + /// @brief Accept all incoming goals unconditionally. + rclcpp_action::GoalResponse handle_goal( + const rclcpp_action::GoalUUID& uuid, + std::shared_ptr< + const vortex_msgs::action::ReferenceFilterWaypoint::Goal> goal); + + /// @brief Accept all cancel requests. + rclcpp_action::CancelResponse handle_cancel( + const std::shared_ptr> goal_handle); + + /// @brief Join the old execution thread and spawn a new one for the goal. + void handle_accepted( + const std::shared_ptr> goal_handle); + + /** + * @brief Execute the action goal in a loop until convergence or + * preemption. + * @param goal_handle The goal handle. + */ + void execute( + const std::shared_ptr> goal_handle); + + rclcpp_action::Server< + vortex_msgs::action::ReferenceFilterWaypoint>::SharedPtr action_server_; + + ReferenceFilterParams filter_params_; + + std::unique_ptr follower_; + + rclcpp::Publisher::SharedPtr + reference_pub_; + + rclcpp::Subscription::SharedPtr + reference_sub_; + + rclcpp::Subscription< + geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_; + + rclcpp::Subscription< + geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr twist_sub_; + + rclcpp::TimerBase::SharedPtr reference_pub_timer_; + + std::chrono::milliseconds time_step_{}; + + vortex::utils::types::PoseEuler current_pose_; + + vortex::utils::types::Twist current_twist_; + + std::mutex sensor_mutex_; + + std::atomic preempted_{false}; + std::mutex execute_mutex_; + std::thread execute_thread_; +}; + +} // namespace vortex::guidance + +#endif // REFERENCE_FILTER_DP__ROS__REFERENCE_FILTER_ROS_HPP_ diff --git a/guidance/reference_filter_dp/include/reference_filter_dp/ros/reference_filter_ros_utils.hpp b/guidance/reference_filter_dp/include/reference_filter_dp/ros/reference_filter_ros_utils.hpp new file mode 100644 index 000000000..35c8a9927 --- /dev/null +++ b/guidance/reference_filter_dp/include/reference_filter_dp/ros/reference_filter_ros_utils.hpp @@ -0,0 +1,69 @@ +#ifndef REFERENCE_FILTER_DP__ROS__REFERENCE_FILTER_ROS_UTILS_HPP_ +#define REFERENCE_FILTER_DP__ROS__REFERENCE_FILTER_ROS_UTILS_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include "reference_filter_dp/lib/eigen_typedefs.hpp" +#include "reference_filter_dp/lib/waypoint_types.hpp" + +namespace vortex::guidance { + +/** + * @brief Convert a ROS waypoint mode to a WaypointMode enum. + * @throws std::invalid_argument if the mode value is not recognized. + */ +inline WaypointMode waypoint_mode_from_ros( + const vortex_msgs::msg::WaypointMode& mode_msg) { + switch (mode_msg.mode) { + case vortex_msgs::msg::WaypointMode::FULL_POSE: + return WaypointMode::FULL_POSE; + case vortex_msgs::msg::WaypointMode::ONLY_POSITION: + return WaypointMode::ONLY_POSITION; + case vortex_msgs::msg::WaypointMode::FORWARD_HEADING: + return WaypointMode::FORWARD_HEADING; + case vortex_msgs::msg::WaypointMode::ONLY_ORIENTATION: + return WaypointMode::ONLY_ORIENTATION; + default: + throw std::invalid_argument("Invalid ROS waypoint mode: " + + std::to_string(mode_msg.mode)); + } +} + +/// @brief Convert a ROS Waypoint message to an internal Waypoint struct. +inline vortex::guidance::Waypoint waypoint_from_ros( + const vortex_msgs::msg::Waypoint& ros_wp) { + Waypoint wp; + wp.pose = + vortex::utils::ros_conversions::ros_pose_to_pose_euler(ros_wp.pose); + wp.mode = waypoint_mode_from_ros(ros_wp.waypoint_mode); + return wp; +} + +/// @brief Fill a ReferenceFilter message from an 18D state vector. +inline vortex_msgs::msg::ReferenceFilter fill_reference_msg( + const Eigen::Vector18d& x) { + using vortex::utils::math::ssa; + vortex_msgs::msg::ReferenceFilter msg; + msg.x = x(0); + msg.y = x(1); + msg.z = x(2); + msg.roll = ssa(x(3)); + msg.pitch = ssa(x(4)); + msg.yaw = ssa(x(5)); + msg.x_dot = x(6); + msg.y_dot = x(7); + msg.z_dot = x(8); + msg.roll_dot = x(9); + msg.pitch_dot = x(10); + msg.yaw_dot = x(11); + return msg; +} + +} // namespace vortex::guidance + +#endif // REFERENCE_FILTER_DP__ROS__REFERENCE_FILTER_ROS_UTILS_HPP_ diff --git a/guidance/reference_filter_dp/launch/reference_filter_dp.launch.py b/guidance/reference_filter_dp/launch/reference_filter_dp.launch.py index 4581abbfb..7f25ac155 100644 --- a/guidance/reference_filter_dp/launch/reference_filter_dp.launch.py +++ b/guidance/reference_filter_dp/launch/reference_filter_dp.launch.py @@ -2,32 +2,44 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription +from launch.actions import OpaqueFunction from launch_ros.actions import Node -config_file_path = os.path.join( - get_package_share_directory('reference_filter_dp'), - 'config', - 'reference_filter_params.yaml', +from auv_setup.launch_arg_common import ( + declare_drone_and_namespace_args, + resolve_drone_and_namespace, ) -orca_config = os.path.join( - get_package_share_directory('auv_setup'), - 'config', - 'robots', - 'orca.yaml', -) + +def launch_setup(context, *args, **kwargs): + drone, namespace = resolve_drone_and_namespace(context) + + config_file_path = os.path.join( + get_package_share_directory("reference_filter_dp"), + "config", + "reference_filter_params.yaml", + ) + + drone_params = os.path.join( + get_package_share_directory("auv_setup"), + "config", + "robots", + f"{drone}.yaml", + ) + + return [ + Node( + package="reference_filter_dp", + executable="reference_filter_dp_node", + name="reference_filter_node", + namespace=namespace, + parameters=[config_file_path, drone_params], + output="screen", + ) + ] def generate_launch_description(): - reference_filter_node = Node( - package='reference_filter_dp', - executable='reference_filter_dp_node', - name='reference_filter_node', - namespace='orca', - parameters=[ - config_file_path, - orca_config, - ], - output='screen', + return LaunchDescription( + declare_drone_and_namespace_args() + [OpaqueFunction(function=launch_setup)] ) - return LaunchDescription([reference_filter_node]) diff --git a/guidance/reference_filter_dp/package.xml b/guidance/reference_filter_dp/package.xml index a7f097fde..40793be91 100644 --- a/guidance/reference_filter_dp/package.xml +++ b/guidance/reference_filter_dp/package.xml @@ -13,10 +13,10 @@ rclcpp_action geometry_msgs vortex_msgs + vortex_utils + vortex_utils_ros nav_msgs eigen - tf2 - tf2_geometry_msgs ament_cmake diff --git a/guidance/reference_filter_dp/src/lib/reference_filter.cpp b/guidance/reference_filter_dp/src/lib/reference_filter.cpp new file mode 100644 index 000000000..e5fc606b6 --- /dev/null +++ b/guidance/reference_filter_dp/src/lib/reference_filter.cpp @@ -0,0 +1,37 @@ +#include "reference_filter_dp/lib/reference_filter.hpp" + +namespace vortex::guidance { + +ReferenceFilter::ReferenceFilter(const ReferenceFilterParams& params) { + calculate_Ad(params.omega, params.zeta); + calculate_Bd(params.omega); +} + +Eigen::Vector18d ReferenceFilter::calculate_x_dot(const Eigen::Vector18d& x, + const Eigen::Vector6d& r) { + Eigen::Vector18d x_dot = Ad_ * x + Bd_ * r; + + return x_dot; +} + +void ReferenceFilter::calculate_Ad(const Eigen::Vector6d& omega, + const Eigen::Vector6d& zeta) { + Eigen::Matrix6d omega_diag = omega.asDiagonal(); + Eigen::Matrix6d delta = zeta.asDiagonal(); + Eigen::Matrix6d omega_diag_squared = omega_diag * omega_diag; + Eigen::Matrix6d omega_diag_cubed = omega_diag * omega_diag * omega_diag; + Ad_.block<6, 6>(0, 6) = Eigen::Matrix6d::Identity(); + Ad_.block<6, 6>(12, 0) = -omega_diag_cubed; + Ad_.block<6, 6>(12, 6) = + -(2 * delta + Eigen::Matrix6d::Identity()) * omega_diag_squared; + Ad_.block<6, 6>(12, 12) = + -(2 * delta + Eigen::Matrix6d::Identity()) * omega_diag; + Ad_.block<6, 6>(6, 12) = Eigen::Matrix6d::Identity(); +} + +void ReferenceFilter::calculate_Bd(const Eigen::Vector6d& omega) { + Eigen::Matrix6d omega_diag = omega.asDiagonal(); + Bd_.block<6, 6>(12, 0) = omega_diag * omega_diag * omega_diag; +} + +} // namespace vortex::guidance diff --git a/guidance/reference_filter_dp/src/lib/waypoint_follower.cpp b/guidance/reference_filter_dp/src/lib/waypoint_follower.cpp new file mode 100644 index 000000000..4da1d7c31 --- /dev/null +++ b/guidance/reference_filter_dp/src/lib/waypoint_follower.cpp @@ -0,0 +1,73 @@ +#include "reference_filter_dp/lib/waypoint_follower.hpp" +#include +#include "reference_filter_dp/lib/eigen_typedefs.hpp" +#include "reference_filter_dp/lib/waypoint_utils.hpp" + +namespace vortex::guidance { + +WaypointFollower::WaypointFollower(const ReferenceFilterParams& params, + double dt_seconds) + : filter_(params), dt_seconds_(dt_seconds) {} + +void WaypointFollower::start(const PoseEuler& pose, + const Twist& twist, + const Waypoint& waypoint, + double convergence_threshold) { + std::lock_guard lock(mutex_); + state_ = compute_initial_state(pose, twist); + waypoint_mode_ = waypoint.mode; + convergence_threshold_ = convergence_threshold; + reference_goal_ = apply_mode_logic(waypoint.pose.to_vector(), + waypoint_mode_, state_.head<6>()); +} + +Eigen::Vector18d WaypointFollower::compute_initial_state(const PoseEuler& pose, + const Twist& twist) { + Eigen::Vector18d x = Eigen::Vector18d::Zero(); + + x.head<6>() = pose.to_vector(); + + Eigen::Matrix J = pose.as_j_matrix(); + x.segment<6>(6) = J * twist.to_vector(); + + return x; +} + +Eigen::Vector18d WaypointFollower::step() { + std::lock_guard lock(mutex_); + Eigen::Vector18d state_dot_ = + filter_.calculate_x_dot(state_, reference_goal_); + state_ += state_dot_ * dt_seconds_; + + return state_; +} + +bool WaypointFollower::within_convergance( + const Eigen::Vector6d& measured_pose) const { + std::lock_guard lock(mutex_); + return has_converged(measured_pose, reference_goal_, waypoint_mode_, + convergence_threshold_); +} + +void WaypointFollower::set_reference(const PoseEuler& reference_goal_pose) { + std::lock_guard lock(mutex_); + reference_goal_ = apply_mode_logic(reference_goal_pose.to_vector(), + waypoint_mode_, state_.head<6>()); +} + +Eigen::Vector18d WaypointFollower::state() const { + std::lock_guard lock(mutex_); + return state_; +} + +Eigen::Vector6d WaypointFollower::reference() const { + std::lock_guard lock(mutex_); + return reference_goal_; +} + +void WaypointFollower::snap_state_to_reference() { + std::lock_guard lock(mutex_); + state_.head<6>() = reference_goal_; +} + +} // namespace vortex::guidance diff --git a/guidance/reference_filter_dp/src/lib/waypoint_utils.cpp b/guidance/reference_filter_dp/src/lib/waypoint_utils.cpp new file mode 100644 index 000000000..fe72ebf13 --- /dev/null +++ b/guidance/reference_filter_dp/src/lib/waypoint_utils.cpp @@ -0,0 +1,86 @@ +#include "reference_filter_dp/lib/waypoint_utils.hpp" +#include +#include + +namespace vortex::guidance { + +Eigen::Vector6d apply_mode_logic(const Eigen::Vector6d& reference_in, + WaypointMode mode, + const Eigen::Vector6d& current_state) { + Eigen::Vector6d reference_out = reference_in; + + using vortex::utils::math::ssa; + + switch (mode) { + case WaypointMode::FULL_POSE: + reference_out(3) = + current_state(3) + ssa(reference_in(3) - current_state(3)); + reference_out(4) = + current_state(4) + ssa(reference_in(4) - current_state(4)); + reference_out(5) = + current_state(5) + ssa(reference_in(5) - current_state(5)); + break; + + case WaypointMode::ONLY_POSITION: + reference_out(3) = current_state(3); + reference_out(4) = current_state(4); + reference_out(5) = current_state(5); + break; + + case WaypointMode::FORWARD_HEADING: { + double dx = reference_in(0) - current_state(0); + double dy = reference_in(1) - current_state(1); + double forward_heading = std::atan2(dy, dx); + + reference_out(3) = 0.0; + reference_out(4) = 0.0; + reference_out(5) = vortex::utils::math::ssa(forward_heading); + break; + } + + case WaypointMode::ONLY_ORIENTATION: + reference_out(0) = current_state(0); + reference_out(1) = current_state(1); + reference_out(2) = current_state(2); + reference_out(3) = + current_state(3) + ssa(reference_in(3) - current_state(3)); + reference_out(4) = + current_state(4) + ssa(reference_in(4) - current_state(4)); + reference_out(5) = + current_state(5) + ssa(reference_in(5) - current_state(5)); + break; + } + + return reference_out; +} + +bool has_converged(const Eigen::Vector6d& measured_pose, + const Eigen::Vector6d& reference, + WaypointMode mode, + double convergence_threshold) { + using vortex::utils::math::ssa; + const Eigen::Vector3d ep = measured_pose.head<3>() - reference.head<3>(); + + Eigen::Vector3d ea; + ea(0) = ssa(measured_pose(3) - reference(3)); + ea(1) = ssa(measured_pose(4) - reference(4)); + ea(2) = ssa(measured_pose(5) - reference(5)); + + const double err = [&] { + switch (mode) { + case WaypointMode::ONLY_POSITION: + return ep.norm(); + case WaypointMode::ONLY_ORIENTATION: + return ea.norm(); + case WaypointMode::FORWARD_HEADING: + return std::sqrt(ep.squaredNorm() + ea(2) * ea(2)); + case WaypointMode::FULL_POSE: + default: + return std::sqrt(ep.squaredNorm() + ea.squaredNorm()); + } + }(); + + return err < convergence_threshold; +} + +} // namespace vortex::guidance diff --git a/guidance/reference_filter_dp/src/ros/reference_filter_ros.cpp b/guidance/reference_filter_dp/src/ros/reference_filter_ros.cpp new file mode 100644 index 000000000..152a59aab --- /dev/null +++ b/guidance/reference_filter_dp/src/ros/reference_filter_ros.cpp @@ -0,0 +1,239 @@ +#include "reference_filter_dp/ros/reference_filter_ros.hpp" +#include +#include +#include +#include +#include +#include +#include "reference_filter_dp/ros/reference_filter_ros_utils.hpp" + +const auto start_message = R"( + ____ __ _____ _ _ _ + | _ \ ___ / _| ___ _ __ ___ _ __ ___ ___ | ___(_) | |_ ___ _ __ + | |_) / _ \ |_ / _ \ '__/ _ \ '_ \ / __/ _ \ | |_ | | | __/ _ \ '__| + | _ < __/ _| __/ | | __/ | | | (_| __/ | _| | | | || __/ | + |_| \_\___|_| \___|_| \___|_| |_|\___\___| |_| |_|_|\__\___|_| + + )"; + +namespace vortex::guidance { + +ReferenceFilterNode::ReferenceFilterNode(const rclcpp::NodeOptions& options) + : Node("reference_filter_node", options) { + int time_step_ms = this->declare_parameter("time_step_ms"); + time_step_ = std::chrono::milliseconds(time_step_ms); + + set_subscribers_and_publisher(); + + set_action_server(); + + set_refererence_filter(); + + spdlog::info(start_message); +} + +ReferenceFilterNode::~ReferenceFilterNode() { + preempted_ = true; + if (execute_thread_.joinable()) { + execute_thread_.join(); + } +} + +void ReferenceFilterNode::set_subscribers_and_publisher() { + this->declare_parameter("topics.pose"); + this->declare_parameter("topics.twist"); + this->declare_parameter("topics.guidance.dp"); + this->declare_parameter("topics.reference_pose"); + + std::string pose_topic = this->get_parameter("topics.pose").as_string(); + std::string twist_topic = this->get_parameter("topics.twist").as_string(); + std::string guidance_topic = + this->get_parameter("topics.guidance.dp").as_string(); + std::string reference_pose_topic = + this->get_parameter("topics.reference_pose").as_string(); + + auto qos_sensor_data = vortex::utils::qos_profiles::sensor_data_profile(1); + reference_pub_ = this->create_publisher( + guidance_topic, qos_sensor_data); + + reference_sub_ = this->create_subscription( + reference_pose_topic, qos_sensor_data, + [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg) { + follower_->set_reference( + vortex::utils::ros_conversions::ros_pose_to_pose_euler( + msg->pose)); + }); + + pose_sub_ = this->create_subscription< + geometry_msgs::msg::PoseWithCovarianceStamped>( + pose_topic, qos_sensor_data, + [this](const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr + msg) { + std::lock_guard lock(sensor_mutex_); + current_pose_ = + vortex::utils::ros_conversions::ros_pose_to_pose_euler( + msg->pose.pose); + }); + + twist_sub_ = this->create_subscription< + geometry_msgs::msg::TwistWithCovarianceStamped>( + twist_topic, qos_sensor_data, + [this](const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr + msg) { + std::lock_guard lock(sensor_mutex_); + current_twist_ = vortex::utils::ros_conversions::ros_twist_to_twist( + msg->twist.twist); + }); +} + +void ReferenceFilterNode::set_action_server() { + this->declare_parameter("action_servers.reference_filter"); + std::string action_server_name = + this->get_parameter("action_servers.reference_filter").as_string(); + + action_server_ = rclcpp_action::create_server< + vortex_msgs::action::ReferenceFilterWaypoint>( + this, action_server_name, + [this](const auto& uuid, auto goal) { + return handle_goal(uuid, std::move(goal)); + }, + [this](auto goal_handle) { return handle_cancel(goal_handle); }, + [this](auto goal_handle) { handle_accepted(goal_handle); }); +} + +void ReferenceFilterNode::set_refererence_filter() { + this->declare_parameter>("zeta"); + this->declare_parameter>("omega"); + + std::vector zeta = this->get_parameter("zeta").as_double_array(); + std::vector omega = this->get_parameter("omega").as_double_array(); + + Eigen::Vector6d zeta_eigen = Eigen::Map(zeta.data()); + Eigen::Vector6d omega_eigen = Eigen::Map(omega.data()); + + filter_params_ = ReferenceFilterParams{omega_eigen, zeta_eigen}; + + double dt_seconds = time_step_.count() / 1000.0; + follower_ = std::make_unique(filter_params_, dt_seconds); +} + +rclcpp_action::GoalResponse ReferenceFilterNode::handle_goal( + const rclcpp_action::GoalUUID& /*uuid*/, + std::shared_ptr + /*goal*/) { + spdlog::info("Accepted goal request"); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse ReferenceFilterNode::handle_cancel( + const std::shared_ptr> /*goal_handle*/) { + spdlog::info("Received request to cancel goal"); + return rclcpp_action::CancelResponse::ACCEPT; +} + +void ReferenceFilterNode::handle_accepted( + const std::shared_ptr> goal_handle) { + std::lock_guard lock(execute_mutex_); + preempted_ = true; + if (execute_thread_.joinable()) { + execute_thread_.join(); + } + preempted_ = false; + + execute_thread_ = + std::thread([this, goal_handle]() { execute(goal_handle); }); +} + +void ReferenceFilterNode::execute( + const std::shared_ptr> goal_handle) { + spdlog::info("Executing goal"); + + double convergence_threshold = + goal_handle->get_goal()->convergence_threshold; + + if (convergence_threshold <= 0.0) { + convergence_threshold = 0.1; + spdlog::warn( + "ReferenceFilter: Invalid convergence_threshold received (<= 0). " + "Using default 0.1"); + } + + const auto wp = waypoint_from_ros(goal_handle->get_goal()->waypoint); + + const auto [pose, twist] = [this] { + std::lock_guard lock(sensor_mutex_); + return std::pair{current_pose_, current_twist_}; + }(); + + follower_->start(pose, twist, wp, convergence_threshold); + + auto result = std::make_shared< + vortex_msgs::action::ReferenceFilterWaypoint::Result>(); + + rclcpp::Rate loop_rate(1000.0 / time_step_.count()); + + while (rclcpp::ok()) { + if (preempted_.load()) { + result->success = false; + goal_handle->abort(result); + spdlog::info("Goal preempted by newer goal"); + return; + } + + if (goal_handle->is_canceling()) { + result->success = false; + goal_handle->canceled(result); + spdlog::info("Goal canceled"); + return; + } + + Eigen::Vector18d filter_state = follower_->step(); + + const auto current_pose_vector = [this] { + std::lock_guard lock(sensor_mutex_); + return current_pose_.to_vector(); + }(); + + bool target_reached = + follower_->within_convergance(current_pose_vector); + + if (target_reached) { + follower_->snap_state_to_reference(); + + auto final_reference_msg = + std::make_unique( + fill_reference_msg(follower_->state())); + + reference_pub_->publish(std::move(final_reference_msg)); + + result->success = true; + goal_handle->succeed(result); + spdlog::info("Goal reached"); + return; + } + + auto reference_msg = + std::make_unique( + fill_reference_msg(filter_state)); + reference_pub_->publish(std::move(reference_msg)); + loop_rate.sleep(); + } + if (!rclcpp::ok() && goal_handle->is_active()) { + auto result = std::make_shared< + vortex_msgs::action::ReferenceFilterWaypoint::Result>(); + result->success = false; + + try { + goal_handle->abort(result); + } catch (...) { + // Ignore exceptions during shutdown + } + } +} + +RCLCPP_COMPONENTS_REGISTER_NODE(ReferenceFilterNode) + +} // namespace vortex::guidance diff --git a/guidance/reference_filter_dp/test/CMakeLists.txt b/guidance/reference_filter_dp/test/CMakeLists.txt new file mode 100644 index 000000000..2b8229c0c --- /dev/null +++ b/guidance/reference_filter_dp/test/CMakeLists.txt @@ -0,0 +1,57 @@ +cmake_minimum_required(VERSION 3.8) + +find_package(GTest REQUIRED) +include(GoogleTest) + +set(TEST_BINARY_NAME ${PROJECT_NAME}_test) +add_executable( + ${TEST_BINARY_NAME} + test_reference_filter.cpp +) + +target_link_libraries( + ${TEST_BINARY_NAME} + PRIVATE + ${CORE_LIB_NAME} + GTest::GTest +) + +ament_target_dependencies(${TEST_BINARY_NAME} PUBLIC Eigen3) + +gtest_discover_tests(${TEST_BINARY_NAME}) + +# Waypoint utils tests +set(TEST_WAYPOINT_UTILS ${PROJECT_NAME}_test_waypoint_utils) +add_executable( + ${TEST_WAYPOINT_UTILS} + test_waypoint_utils.cpp +) + +target_link_libraries( + ${TEST_WAYPOINT_UTILS} + PRIVATE + ${CORE_LIB_NAME} + GTest::GTest +) + +ament_target_dependencies(${TEST_WAYPOINT_UTILS} PUBLIC Eigen3 vortex_utils) + +gtest_discover_tests(${TEST_WAYPOINT_UTILS}) + +# Waypoint follower tests +set(TEST_WAYPOINT_FOLLOWER ${PROJECT_NAME}_test_waypoint_follower) +add_executable( + ${TEST_WAYPOINT_FOLLOWER} + test_waypoint_follower.cpp +) + +target_link_libraries( + ${TEST_WAYPOINT_FOLLOWER} + PRIVATE + ${CORE_LIB_NAME} + GTest::GTest +) + +ament_target_dependencies(${TEST_WAYPOINT_FOLLOWER} PUBLIC Eigen3 vortex_utils) + +gtest_discover_tests(${TEST_WAYPOINT_FOLLOWER}) diff --git a/guidance/reference_filter_dp/test/test_reference_filter.cpp b/guidance/reference_filter_dp/test/test_reference_filter.cpp new file mode 100644 index 000000000..3716ddaa9 --- /dev/null +++ b/guidance/reference_filter_dp/test/test_reference_filter.cpp @@ -0,0 +1,58 @@ +#include + +#include "reference_filter_dp/lib/eigen_typedefs.hpp" +#include "reference_filter_dp/lib/reference_filter.hpp" + +namespace vortex::guidance { + +class ReferenceFilterTests : public ::testing::Test { + protected: + ReferenceFilterTests() : reference_filter_{get_filter_params()} {} + + ReferenceFilterParams get_filter_params() { + ReferenceFilterParams params; + params.omega = Eigen::Vector6d::Ones(); + params.zeta = Eigen::Vector6d::Ones(); + return params; + } + + ReferenceFilter reference_filter_; +}; + +TEST_F(ReferenceFilterTests, T01_positive_command_positive_xdot) { + Eigen::Vector18d x = Eigen::Vector18d::Zero(); + Eigen::Vector6d r = Eigen::Vector6d::Ones(); + Eigen::Vector18d xdot = reference_filter_.calculate_x_dot(x, r); + + for (int i = 0; i < xdot.size(); ++i) { + EXPECT_GE(xdot(i), 0.0); + } +} + +TEST_F(ReferenceFilterTests, T02_negative_command_negative_xdot) { + Eigen::Vector18d x = Eigen::Vector18d::Zero(); + Eigen::Vector6d r = -Eigen::Vector6d::Ones(); + Eigen::Vector18d xdot = reference_filter_.calculate_x_dot(x, r); + + for (int i = 0; i < xdot.size(); ++i) { + EXPECT_LE(xdot(i), 0.0); + } +} + +TEST_F(ReferenceFilterTests, T03_zero_command_zero_xdot) { + Eigen::Vector18d x = Eigen::Vector18d::Zero(); + Eigen::Vector6d r = Eigen::Vector6d::Zero(); + Eigen::Vector18d xdot = reference_filter_.calculate_x_dot(x, r); + + for (int i = 0; i < xdot.size(); ++i) { + EXPECT_DOUBLE_EQ(xdot(i), 0.0); + } +} + +} // namespace vortex::guidance + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} diff --git a/guidance/reference_filter_dp/test/test_waypoint_follower.cpp b/guidance/reference_filter_dp/test/test_waypoint_follower.cpp new file mode 100644 index 000000000..06e063d0f --- /dev/null +++ b/guidance/reference_filter_dp/test/test_waypoint_follower.cpp @@ -0,0 +1,111 @@ +#include +#include "reference_filter_dp/lib/waypoint_follower.hpp" +#include "reference_filter_dp/lib/waypoint_utils.hpp" + +namespace vortex::guidance { + +class WaypointFollowerTests : public ::testing::Test { + protected: + ReferenceFilterParams get_params() { + ReferenceFilterParams params; + params.omega = Eigen::Vector6d::Ones(); + params.zeta = Eigen::Vector6d::Ones(); + return params; + } + + PoseEuler zero_pose() { return PoseEuler{}; } + Twist zero_twist() { return Twist{}; } +}; + +TEST_F(WaypointFollowerTests, StartAndStepConverges) { + WaypointFollower follower(get_params(), 0.01); + + Waypoint wp; + wp.pose = PoseEuler{1.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + wp.mode = WaypointMode::FULL_POSE; + + follower.start(zero_pose(), zero_twist(), wp, 0.1); + + Eigen::Vector18d state = follower.step(); + + // Simulate the measured pose being at the reference + Eigen::Vector6d measured_at_ref; + measured_at_ref << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0; + + EXPECT_TRUE(follower.within_convergance(measured_at_ref)); + EXPECT_EQ(state.size(), 18); +} + +TEST_F(WaypointFollowerTests, StepDoesNotConvergeWhenFar) { + WaypointFollower follower(get_params(), 0.01); + + Waypoint wp; + wp.pose = PoseEuler{10.0, 10.0, 0.0, 0.0, 0.0, 0.0}; + wp.mode = WaypointMode::FULL_POSE; + + follower.start(zero_pose(), zero_twist(), wp, 0.1); + + follower.step(); + + Eigen::Vector6d measured_far = Eigen::Vector6d::Zero(); + EXPECT_FALSE(follower.within_convergance(measured_far)); +} + +TEST_F(WaypointFollowerTests, SetReferenceUpdatesMidSequence) { + WaypointFollower follower(get_params(), 0.01); + + Waypoint wp; + wp.pose = PoseEuler{1.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + wp.mode = WaypointMode::FULL_POSE; + + follower.start(zero_pose(), zero_twist(), wp, 0.1); + + PoseEuler new_ref{5.0, 5.0, 0.0, 0.0, 0.0, 0.0}; + follower.set_reference(new_ref); + + EXPECT_DOUBLE_EQ(follower.reference()(0), 5.0); + EXPECT_DOUBLE_EQ(follower.reference()(1), 5.0); +} + +TEST_F(WaypointFollowerTests, SnapStateToReference) { + WaypointFollower follower(get_params(), 0.01); + + Waypoint wp; + wp.pose = PoseEuler{3.0, 4.0, 5.0, 0.1, 0.2, 0.3}; + wp.mode = WaypointMode::FULL_POSE; + + follower.start(zero_pose(), zero_twist(), wp, 0.1); + follower.snap_state_to_reference(); + + Eigen::Vector18d state = follower.state(); + Eigen::Vector6d ref = follower.reference(); + + for (int i = 0; i < 6; ++i) { + EXPECT_DOUBLE_EQ(state(i), ref(i)); + } +} + +TEST_F(WaypointFollowerTests, StateEvolvesWithStep) { + WaypointFollower follower(get_params(), 0.01); + + Waypoint wp; + wp.pose = PoseEuler{1.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + wp.mode = WaypointMode::FULL_POSE; + + follower.start(zero_pose(), zero_twist(), wp, 0.1); + + // Run several steps — state should move toward reference + for (int i = 0; i < 100; ++i) { + follower.step(); + } + + // x position should have moved toward 1.0 + EXPECT_GT(follower.state()(0), 0.0); +} + +} // namespace vortex::guidance + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/guidance/reference_filter_dp/test/test_waypoint_utils.cpp b/guidance/reference_filter_dp/test/test_waypoint_utils.cpp new file mode 100644 index 000000000..6e9de1b82 --- /dev/null +++ b/guidance/reference_filter_dp/test/test_waypoint_utils.cpp @@ -0,0 +1,174 @@ +#include +#include +#include "reference_filter_dp/lib/waypoint_utils.hpp" + +namespace vortex::guidance { + +// --- apply_mode_logic tests --- + +TEST(ApplyModeLogic, FullPoseWrapsAnglesToShortestPath) { + Eigen::Vector6d r_in; + r_in << 1.0, 2.0, 3.0, 0.1, 0.2, 0.3; + Eigen::Vector6d state = Eigen::Vector6d::Zero(); + + Eigen::Vector6d result = + apply_mode_logic(r_in, WaypointMode::FULL_POSE, state); + + // Position unchanged + EXPECT_DOUBLE_EQ(result(0), 1.0); + EXPECT_DOUBLE_EQ(result(1), 2.0); + EXPECT_DOUBLE_EQ(result(2), 3.0); + + // Small angles: wrapping has no effect + EXPECT_NEAR(result(3), 0.1, 1e-12); + EXPECT_NEAR(result(4), 0.2, 1e-12); + EXPECT_NEAR(result(5), 0.3, 1e-12); +} + +TEST(ApplyModeLogic, FullPoseTakesShortestRotation) { + // Current yaw ~170°, reference yaw ~-170° => only ~20° apart + // Without wrapping, the filter would see a ~340° difference + Eigen::Vector6d state; + state << 0.0, 0.0, 0.0, 0.0, 0.0, 2.96; // ~170° + Eigen::Vector6d r_in; + r_in << 0.0, 0.0, 0.0, 0.0, 0.0, -2.96; // ~-170° + + Eigen::Vector6d result = + apply_mode_logic(r_in, WaypointMode::FULL_POSE, state); + + // The wrapped reference should be close to the current state + short + // rotation + double yaw_error = result(5) - state(5); + EXPECT_NEAR(yaw_error, -2.96 - 2.96 + 2.0 * M_PI, 1e-12); + EXPECT_LT(std::abs(yaw_error), M_PI + 1e-9); +} + +TEST(ApplyModeLogic, OnlyPositionKeepsOrientationFromState) { + Eigen::Vector6d r_in; + r_in << 1.0, 2.0, 3.0, 0.1, 0.2, 0.3; + Eigen::Vector6d state; + state << 10.0, 20.0, 30.0, 0.4, 0.5, 0.6; + + Eigen::Vector6d result = + apply_mode_logic(r_in, WaypointMode::ONLY_POSITION, state); + + EXPECT_DOUBLE_EQ(result(0), 1.0); + EXPECT_DOUBLE_EQ(result(1), 2.0); + EXPECT_DOUBLE_EQ(result(2), 3.0); + EXPECT_DOUBLE_EQ(result(3), 0.4); + EXPECT_DOUBLE_EQ(result(4), 0.5); + EXPECT_DOUBLE_EQ(result(5), 0.6); +} + +TEST(ApplyModeLogic, ForwardHeadingComputesYawFromDelta) { + Eigen::Vector6d r_in; + r_in << 1.0, 1.0, 0.0, 0.0, 0.0, 0.0; + Eigen::Vector6d state = Eigen::Vector6d::Zero(); + + Eigen::Vector6d result = + apply_mode_logic(r_in, WaypointMode::FORWARD_HEADING, state); + + double expected_yaw = std::atan2(1.0, 1.0); // pi/4 + EXPECT_DOUBLE_EQ(result(0), 1.0); + EXPECT_DOUBLE_EQ(result(1), 1.0); + EXPECT_DOUBLE_EQ(result(3), 0.0); + EXPECT_DOUBLE_EQ(result(4), 0.0); + EXPECT_NEAR(result(5), expected_yaw, 1e-12); +} + +TEST(ApplyModeLogic, OnlyOrientationKeepsPositionFromState) { + Eigen::Vector6d r_in; + r_in << 1.0, 2.0, 3.0, 0.1, 0.2, 0.3; + Eigen::Vector6d state; + state << 10.0, 20.0, 30.0, 0.4, 0.5, 0.6; + + Eigen::Vector6d result = + apply_mode_logic(r_in, WaypointMode::ONLY_ORIENTATION, state); + + EXPECT_DOUBLE_EQ(result(0), 10.0); + EXPECT_DOUBLE_EQ(result(1), 20.0); + EXPECT_DOUBLE_EQ(result(2), 30.0); + + // Small angle differences: wrapping has no effect, result matches input + EXPECT_NEAR(result(3), 0.1, 1e-12); + EXPECT_NEAR(result(4), 0.2, 1e-12); + EXPECT_NEAR(result(5), 0.3, 1e-12); +} + +TEST(ApplyModeLogic, OnlyOrientationTakesShortestRotation) { + Eigen::Vector6d state; + state << 10.0, 20.0, 30.0, 0.0, 0.0, 2.96; + Eigen::Vector6d r_in; + r_in << 1.0, 2.0, 3.0, 0.0, 0.0, -2.96; + + Eigen::Vector6d result = + apply_mode_logic(r_in, WaypointMode::ONLY_ORIENTATION, state); + + // Position from state + EXPECT_DOUBLE_EQ(result(0), 10.0); + EXPECT_DOUBLE_EQ(result(1), 20.0); + EXPECT_DOUBLE_EQ(result(2), 30.0); + + // Yaw should take the shortest path + double yaw_error = result(5) - state(5); + EXPECT_LT(std::abs(yaw_error), M_PI + 1e-9); +} + +// --- has_converged tests --- + +TEST(HasConverged, FullPoseBelowThreshold) { + Eigen::Vector6d measured; + measured << 1.0, 2.0, 3.0, 0.1, 0.2, 0.3; + Eigen::Vector6d reference = measured; + reference(0) += 0.001; + + EXPECT_TRUE( + has_converged(measured, reference, WaypointMode::FULL_POSE, 0.1)); +} + +TEST(HasConverged, FullPoseAboveThreshold) { + Eigen::Vector6d measured = Eigen::Vector6d::Zero(); + Eigen::Vector6d reference; + reference << 1.0, 1.0, 1.0, 0.0, 0.0, 0.0; + + EXPECT_FALSE( + has_converged(measured, reference, WaypointMode::FULL_POSE, 0.1)); +} + +TEST(HasConverged, OnlyPositionIgnoresOrientation) { + Eigen::Vector6d measured; + measured << 1.0, 2.0, 3.0, 0.0, 0.0, 0.0; + Eigen::Vector6d reference; + reference << 1.0, 2.0, 3.0, 1.0, 1.0, 1.0; + + EXPECT_TRUE( + has_converged(measured, reference, WaypointMode::ONLY_POSITION, 0.1)); +} + +TEST(HasConverged, OnlyOrientationIgnoresPosition) { + Eigen::Vector6d measured; + measured << 100.0, 200.0, 300.0, 0.1, 0.2, 0.3; + Eigen::Vector6d reference; + reference << 0.0, 0.0, 0.0, 0.1, 0.2, 0.3; + + EXPECT_TRUE(has_converged(measured, reference, + WaypointMode::ONLY_ORIENTATION, 0.1)); +} + +TEST(HasConverged, ForwardHeadingUsesPositionAndYawOnly) { + Eigen::Vector6d measured; + measured << 1.0, 2.0, 3.0, 0.5, 0.5, 0.1; + Eigen::Vector6d reference; + reference << 1.0, 2.0, 3.0, 0.0, 0.0, 0.1; + + // Roll and pitch differ but should be ignored + EXPECT_TRUE( + has_converged(measured, reference, WaypointMode::FORWARD_HEADING, 0.1)); +} + +} // namespace vortex::guidance + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/guidance/reference_filter_dp_quat/CMakeLists.txt b/guidance/reference_filter_dp_quat/CMakeLists.txt new file mode 100644 index 000000000..cfe51ca33 --- /dev/null +++ b/guidance/reference_filter_dp_quat/CMakeLists.txt @@ -0,0 +1,105 @@ +cmake_minimum_required(VERSION 3.8) +project(reference_filter_dp_quat) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 20) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(vortex_msgs REQUIRED) +find_package(vortex_utils REQUIRED) +find_package(vortex_utils_ros REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(spdlog REQUIRED) +find_package(fmt REQUIRED) + +include_directories(include) + +set(CORE_LIB_NAME "${PROJECT_NAME}") + +add_library(${CORE_LIB_NAME} SHARED + src/lib/reference_filter.cpp + src/lib/waypoint_follower.cpp +) + +target_include_directories(${CORE_LIB_NAME} + PUBLIC + $ + $ +) + +ament_target_dependencies(${CORE_LIB_NAME} + Eigen3 + vortex_utils +) + +set(COMPONENT_LIB_NAME "${PROJECT_NAME}_component") + +add_library(${COMPONENT_LIB_NAME} SHARED + src/ros/reference_filter_ros.cpp +) + +ament_target_dependencies(${COMPONENT_LIB_NAME} + rclcpp + rclcpp_components + rclcpp_action + geometry_msgs + nav_msgs + vortex_msgs + vortex_utils + vortex_utils_ros + spdlog + fmt +) + +target_link_libraries(${COMPONENT_LIB_NAME} ${CORE_LIB_NAME}) + +rclcpp_components_register_node( + ${COMPONENT_LIB_NAME} + PLUGIN "ReferenceFilterNode" + EXECUTABLE ${PROJECT_NAME}_node +) + +install( + TARGETS + ${CORE_LIB_NAME} + ${COMPONENT_LIB_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install( + DIRECTORY include/ + DESTINATION include +) + +install(DIRECTORY + launch + config + DESTINATION share/${PROJECT_NAME}/ +) + +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + Eigen3 + vortex_utils +) +ament_export_include_directories(include) +ament_export_libraries(${CORE_LIB_NAME}) + +if(BUILD_TESTING) + add_subdirectory(test) +endif() + +ament_package() diff --git a/guidance/reference_filter_dp_quat/README.md b/guidance/reference_filter_dp_quat/README.md new file mode 100644 index 000000000..1573eb2e3 --- /dev/null +++ b/guidance/reference_filter_dp_quat/README.md @@ -0,0 +1,106 @@ +## Reference filter (quaternion) + +### Third-order reference filter + +The underlying filter is the third-order model (Fossen, 2021): +```math +\dot{x}_d = A_d x_d + B_d r +``` +where +```math +A_d = \begin{bmatrix} +0_{n\times n} & I_n & 0_{n\times n} \\ +0_{n\times n} & 0_{n \times n} & I_n \\ +-\Omega^3 & -(2 \Delta + I_n) \Omega^2 & -(2 \Delta + I_n) \Omega +\end{bmatrix} +``` +and +```math +B_d = \begin{bmatrix} +0_{n \times n} \\ +0_{n \times n} \\ +\Omega^3 +\end{bmatrix}. +``` + +The state is integrated using forward Euler: $x_{i+1} = x_i + \dot{x}_i \cdot dt$. + +### Error-state formulation + +This package avoids the singularity issues of Euler angles by using a quaternion-based error state. + +**Nominal pose** — a `Pose` (position + quaternion) representing the current best estimate of the reference trajectory. This is the actual output. + +**Error state** — the 18D filter state, where the first 6 elements are small errors relative to the nominal: +```math +x = \begin{bmatrix} \delta p \\ \delta \phi \\ \dot{\eta} \\ \ddot{\eta} \end{bmatrix} \in \mathbb{R}^{18} +``` + +| Indices | Meaning | +|---|---| +| `x[0:3]` | Position error $\delta p$ from nominal (meters) | +| `x[3:6]` | Orientation error $\delta \phi$ from nominal (rotation vector, radians) | +| `x[6:9]` | World-frame linear velocity (m/s) | +| `x[9:12]` | World-frame angular velocity (rad/s) | +| `x[12:18]` | Accelerations (linear + angular) | + +### Step cycle + +Each time step performs three operations: + +1. **Compute reference error** — The 6D reference $r$ is the error between the waypoint goal and the nominal pose: + - $r_{0:3} = p_{goal} - p_{nominal}$ + - $r_{3:6} = q_{\mathrm{err}}(q_{nominal}, q_{goal})$ + + The `quaternion_error` function returns $2 \cdot \text{vec}(q_{nominal}^{-1} \otimes q_{goal})$, which for small angles approximates the rotation vector from nominal to goal. + +2. **Integrate** — The standard filter step: + ```math + \dot{x} = A_d x + B_d r, \quad x \leftarrow x + \dot{x} \cdot dt + ``` + +3. **Reset** — Absorb position and orientation errors into the nominal pose, then zero them: + - $p_{nominal} \leftarrow p_{nominal} + \delta p, \quad \delta p \leftarrow 0$ + - $q_{nominal} \leftarrow q_{nominal} \otimes \exp(\delta \phi), \quad \delta \phi \leftarrow 0$ + + where $\exp(\delta \phi)$ converts the rotation vector to a quaternion via `AngleAxis`. + +The reset step keeps $\delta p$ and $\delta \phi$ near zero at all times, ensuring the linearized quaternion error remains accurate. The velocity and acceleration states ($x_{6:18}$) are **not** reset — they carry over to provide smooth, continuous motion. + +### Output message (`ReferenceFilterQuat`) + +| Field | Source | Meaning | +|---|---|---| +| `x`, `y`, `z` | Nominal pose position | Desired position in world frame (m) | +| `qw`, `qx`, `qy`, `qz` | Nominal pose quaternion | Desired orientation (unit quaternion) | +| `x_dot`, `y_dot`, `z_dot` | `x[6:9]` | World-frame linear velocity (m/s) | +| `roll_dot`, `pitch_dot`, `yaw_dot` | `x[9:12]` | World-frame angular velocity (rad/s) | + + + + +### Waypoint modes + +Each waypoint has a mode that determines which degrees of freedom are controlled by the reference filter. The mode also determines how convergence is measured. + +| Mode | Controlled DOFs | Convergence metric | +|---|---|---| +| `FULL_POSE` | All 6 DOF (position + orientation) | Position error norm + quaternion error norm | +| `ONLY_POSITION` | x, y, z (orientation holds current value) | Position error norm | +| `FORWARD_HEADING` | x, y, z + yaw toward target | Position error norm + yaw component of quaternion error | +| `ONLY_ORIENTATION` | Orientation only (position holds current value) | Quaternion error norm | + +For all modes, convergence is reached when the error metric drops below the `convergence_threshold` specified in the action goal. + +### Action Server + +The action server handles goal requests and publishes guidance commands. The server always prioritizes new goal requests, aborting ongoing requests when a new one arrives. + +- Action name: `/reference_filter` +- Goal type: Waypoint (pose + mode + convergence threshold) +- Result type: bool +- Guidance topic: `/dp/reference` + +### Overwriting the reference during an action + +While an action is executing, the reference goal can be updated by publishing a `geometry_msgs/msg/PoseStamped` to the reference pose topic. The convergence check will use the updated reference. diff --git a/guidance/reference_filter_dp_quat/config/reference_filter_params.yaml b/guidance/reference_filter_dp_quat/config/reference_filter_params.yaml new file mode 100644 index 000000000..1aef0ec13 --- /dev/null +++ b/guidance/reference_filter_dp_quat/config/reference_filter_params.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + zeta: [1., 1., 1., 1., 1., 1.] + omega: [0.2, 0.2, 0.2, 0.2, 0.2, 0.2] + time_step_ms: 10 diff --git a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/eigen_typedefs.hpp b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/eigen_typedefs.hpp new file mode 100644 index 000000000..1c9ce6734 --- /dev/null +++ b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/eigen_typedefs.hpp @@ -0,0 +1,21 @@ +/** + * @file eigen_typedefs.hpp + * @brief Contains Eigen typedefs used in this package. + */ + +#ifndef REFERENCE_FILTER_DP_QUAT__LIB__EIGEN_TYPEDEFS_HPP_ +#define REFERENCE_FILTER_DP_QUAT__LIB__EIGEN_TYPEDEFS_HPP_ + +#include + +namespace Eigen { + +typedef Eigen::Matrix Matrix18d; +typedef Eigen::Matrix Matrix18x6d; +typedef Eigen::Matrix Matrix6d; +typedef Eigen::Matrix Vector6d; +typedef Eigen::Matrix Vector18d; + +} // namespace Eigen + +#endif // REFERENCE_FILTER_DP_QUAT__LIB__EIGEN_TYPEDEFS_HPP_ diff --git a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/reference_filter.hpp b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/reference_filter.hpp new file mode 100644 index 000000000..9eefc4a55 --- /dev/null +++ b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/reference_filter.hpp @@ -0,0 +1,50 @@ +#ifndef REFERENCE_FILTER_DP_QUAT__LIB__REFERENCE_FILTER_HPP_ +#define REFERENCE_FILTER_DP_QUAT__LIB__REFERENCE_FILTER_HPP_ + +#include "reference_filter_dp_quat/lib/eigen_typedefs.hpp" + +namespace vortex::guidance { + +struct ReferenceFilterParams { + Eigen::Vector6d omega = Eigen::Vector6d::Zero(); + Eigen::Vector6d zeta = Eigen::Vector6d::Zero(); +}; + +/** + * @brief Stateless third-order reference filter. + * + * Computes the state derivative x_dot = Ad * x + Bd * r, where the state + * x = [eta, eta_dot, eta_ddot] is 18-dimensional and r is the 6D reference. + */ +class ReferenceFilter { + public: + explicit ReferenceFilter(const ReferenceFilterParams& params); + + // @brief Calculate the state derivative + // @param x The state vector 18x1 + // @param r The reference vector 6x1 + // @return The state derivative 18x1 + // REF: Handbook of Marine Craft Hydrodynamics and Motion Control, Fossen + // 2021 p. 337 eq: 12.11 + Eigen::Vector18d calculate_x_dot(const Eigen::Vector18d& x, + const Eigen::Vector6d& r); + + // @brief Calculate the state transition matrix + // REF: Handbook of Marine Craft Hydrodynamics and Motion Control, Fossen + // 2021 p. 337 eq: 12.12 + void calculate_Ad(const Eigen::Vector6d& omega, + const Eigen::Vector6d& zeta); + + // @brief Calculate the input matrix + // REF: Handbook of Marine Craft Hydrodynamics and Motion Control, Fossen + // 2021 p. 337 eq: 12.12 + void calculate_Bd(const Eigen::Vector6d& omega); + + private: + Eigen::Matrix18d Ad_ = Eigen::Matrix18d::Zero(); + Eigen::Matrix18x6d Bd_ = Eigen::Matrix18x6d::Zero(); +}; + +} // namespace vortex::guidance + +#endif // REFERENCE_FILTER_DP_QUAT__LIB__REFERENCE_FILTER_HPP_ diff --git a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_follower.hpp b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_follower.hpp new file mode 100644 index 000000000..6505036cd --- /dev/null +++ b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_follower.hpp @@ -0,0 +1,112 @@ +#ifndef REFERENCE_FILTER_DP_QUAT__LIB__WAYPOINT_FOLLOWER_HPP_ +#define REFERENCE_FILTER_DP_QUAT__LIB__WAYPOINT_FOLLOWER_HPP_ + +#include +#include +#include +#include "reference_filter_dp_quat/lib/eigen_typedefs.hpp" +#include "reference_filter_dp_quat/lib/reference_filter.hpp" + +namespace vortex::guidance { + +using vortex::utils::types::Pose; +using vortex::utils::types::Twist; +using vortex::utils::types::Waypoint; +using vortex::utils::types::WaypointMode; + +/** + * @brief Manages reference filter state and waypoint following logic. + * + * Uses an error-state formulation: the 18D state vector holds + * [δp(3), δφ(3), velocity(6), acceleration(6)], while a separate + * nominal Pose tracks the full quaternion orientation. Each step, + * position/orientation errors are absorbed into the nominal and reset. + * + * Thread-safe: all public methods acquire a mutex. + */ +class WaypointFollower { + public: + WaypointFollower(const ReferenceFilterParams& params, double dt_seconds); + + /** + * @brief Initialize the follower with the current vehicle state and target. + * @param pose Current vehicle pose. + * @param twist Current vehicle twist (body frame). + * @param waypoint Target waypoint with mode. + * @param convergence_threshold Max error norm to consider target reached. + */ + void start(const Pose& pose, + const Twist& twist, + const Waypoint& waypoint, + double convergence_threshold); + + /** + * @brief Advance the filter by one time step. + * + * Computes the error-state reference, integrates, then absorbs + * position/orientation errors into the nominal pose. + */ + void step(); + + /** + * @brief Check if the measured pose has converged to the waypoint goal. + * @param measured_pose Current measured pose. + * @return True if the error norm is within the convergence threshold. + */ + bool within_convergance(const Pose& measured_pose) const; + + /** + * @brief Update the reference goal pose mid-sequence. + * @param reference_goal_pose The new reference pose. + */ + void set_reference(const Pose& reference_goal_pose); + + /** + * @brief Snap the nominal pose to the waypoint goal and zero + * errors/velocity. + * + * Useful after convergence to eliminate any remaining steady-state offset. + */ + void snap_state_to_reference(); + + /** + * @brief Get the current nominal pose (position + quaternion). + */ + Pose pose() const; + + /** + * @brief Get the current world-frame velocity (linear + angular). + */ + Eigen::Vector6d velocity() const; + + /** + * @brief Get the current waypoint goal. + */ + Pose waypoint_goal() const; + + private: + /** + * @brief Compute error-state reference based on the current nominal pose + * and waypoint goal. + */ + Eigen::Vector6d update_reference() const; + + /** + * @brief Absorb position/orientation errors into the nominal pose and + * reset the error states. + */ + void inject_and_reset(); + + mutable std::mutex mutex_; + ReferenceFilter filter_; + double dt_seconds_{0.01}; + Pose nominal_pose_; + Eigen::Vector18d state_ = Eigen::Vector18d::Zero(); + Pose waypoint_goal_; + WaypointMode waypoint_mode_{WaypointMode::FULL_POSE}; + double convergence_threshold_{0.1}; +}; + +} // namespace vortex::guidance + +#endif // REFERENCE_FILTER_DP_QUAT__LIB__WAYPOINT_FOLLOWER_HPP_ diff --git a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros.hpp b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros.hpp new file mode 100644 index 000000000..87df8e7e6 --- /dev/null +++ b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros.hpp @@ -0,0 +1,95 @@ +#ifndef REFERENCE_FILTER_DP_QUAT__ROS__REFERENCE_FILTER_ROS_HPP_ +#define REFERENCE_FILTER_DP_QUAT__ROS__REFERENCE_FILTER_ROS_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "reference_filter_dp_quat/lib/waypoint_follower.hpp" + +namespace vortex::guidance { + +class ReferenceFilterNode : public rclcpp::Node { + public: + explicit ReferenceFilterNode( + const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); + + ~ReferenceFilterNode(); + + private: + // @brief Set the subscribers and publishers + void set_subscribers_and_publisher(); + + // @brief Set the action server + void set_action_server(); + + // @brief Initializes the reference filter with ROS parameters. + void set_refererence_filter(); + + /// @brief Accept all incoming goals unconditionally. + rclcpp_action::GoalResponse handle_goal( + const rclcpp_action::GoalUUID& uuid, + std::shared_ptr< + const vortex_msgs::action::ReferenceFilterQuatWaypoint::Goal> goal); + + /// @brief Accept all cancel requests. + rclcpp_action::CancelResponse handle_cancel( + const std::shared_ptr> goal_handle); + + /// @brief Join the old execution thread and spawn a new one for the goal. + void handle_accepted( + const std::shared_ptr> goal_handle); + + /** + * @brief Execute the action goal in a loop until convergence or + * preemption. + * @param goal_handle The goal handle. + */ + void execute( + const std::shared_ptr> goal_handle); + + rclcpp_action::Server:: + SharedPtr action_server_; + + ReferenceFilterParams filter_params_; + + std::unique_ptr follower_; + + rclcpp::Publisher::SharedPtr + reference_pub_; + + rclcpp::Subscription::SharedPtr + reference_sub_; + + rclcpp::Subscription< + geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_; + + rclcpp::Subscription< + geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr twist_sub_; + + std::chrono::milliseconds time_step_{}; + + vortex::utils::types::Pose current_pose_; + + vortex::utils::types::Twist current_twist_; + + std::mutex sensor_mutex_; + + std::atomic preempted_{false}; + std::mutex execute_mutex_; + std::thread execute_thread_; +}; + +} // namespace vortex::guidance + +#endif // REFERENCE_FILTER_DP_QUAT__ROS__REFERENCE_FILTER_ROS_HPP_ diff --git a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros_utils.hpp b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros_utils.hpp new file mode 100644 index 000000000..c51fc75f0 --- /dev/null +++ b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros_utils.hpp @@ -0,0 +1,58 @@ +#ifndef REFERENCE_FILTER_DP_QUAT__ROS__REFERENCE_FILTER_ROS_UTILS_HPP_ +#define REFERENCE_FILTER_DP_QUAT__ROS__REFERENCE_FILTER_ROS_UTILS_HPP_ + +#include +#include +#include +#include +#include +#include "reference_filter_dp_quat/lib/eigen_typedefs.hpp" + +namespace vortex::guidance { + +/// @brief Fill a ReferenceFilterQuat message from a Pose and velocity vector. +inline vortex_msgs::msg::ReferenceFilterQuat fill_reference_msg( + const vortex::utils::types::Pose& pose, + const Eigen::Vector6d& velocity) { + vortex_msgs::msg::ReferenceFilterQuat msg; + msg.x = pose.x; + msg.y = pose.y; + msg.z = pose.z; + msg.qw = pose.qw; + msg.qx = pose.qx; + msg.qy = pose.qy; + msg.qz = pose.qz; + msg.x_dot = velocity(0); + msg.y_dot = velocity(1); + msg.z_dot = velocity(2); + msg.roll_dot = velocity(3); + msg.pitch_dot = velocity(4); + msg.yaw_dot = velocity(5); + return msg; +} + +/// @brief Fill an RPY ReferenceFilter message from a Pose and velocity vector. +inline vortex_msgs::msg::ReferenceFilter fill_reference_rpy_msg( + const vortex::utils::types::Pose& pose, + const Eigen::Vector6d& velocity) { + using vortex::utils::math::ssa; + auto euler = pose.as_pose_euler(); + vortex_msgs::msg::ReferenceFilter msg; + msg.x = euler.x; + msg.y = euler.y; + msg.z = euler.z; + msg.roll = ssa(euler.roll); + msg.pitch = ssa(euler.pitch); + msg.yaw = ssa(euler.yaw); + msg.x_dot = velocity(0); + msg.y_dot = velocity(1); + msg.z_dot = velocity(2); + msg.roll_dot = velocity(3); + msg.pitch_dot = velocity(4); + msg.yaw_dot = velocity(5); + return msg; +} + +} // namespace vortex::guidance + +#endif // REFERENCE_FILTER_DP_QUAT__ROS__REFERENCE_FILTER_ROS_UTILS_HPP_ diff --git a/guidance/reference_filter_dp_quat/launch/reference_filter_dp_quat.launch.py b/guidance/reference_filter_dp_quat/launch/reference_filter_dp_quat.launch.py new file mode 100644 index 000000000..a610a8ef3 --- /dev/null +++ b/guidance/reference_filter_dp_quat/launch/reference_filter_dp_quat.launch.py @@ -0,0 +1,64 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +from auv_setup.launch_arg_common import ( + declare_drone_and_namespace_args, + resolve_drone_and_namespace, +) + + +def launch_setup(context, *args, **kwargs): + drone, namespace = resolve_drone_and_namespace(context) + rpy_publish = LaunchConfiguration("rpy_publish").perform(context) == "true" + + config_file_path = os.path.join( + get_package_share_directory("reference_filter_dp_quat"), + "config", + "reference_filter_params.yaml", + ) + + drone_params = os.path.join( + get_package_share_directory("auv_setup"), + "config", + "robots", + f"{drone}.yaml", + ) + + extra_params = {} + if rpy_publish: + extra_params = { + "publish_rpy_debug": True, + "topics.guidance.dp_rpy": "guidance/dp", + "topics.guidance.dp": "guidance/dp_quat", + } + + return [ + Node( + package="reference_filter_dp_quat", + executable="reference_filter_dp_quat_node", + name="reference_filter_node", + namespace=namespace, + parameters=[config_file_path, drone_params, extra_params], + output="screen", + ) + ] + + +def generate_launch_description(): + return LaunchDescription( + declare_drone_and_namespace_args() + + [ + DeclareLaunchArgument( + "rpy_publish", + default_value="false", + description="When true, publish RPY ReferenceFilter on guidance/dp " + "for compatibility with controllers expecting RPY messages.", + ), + OpaqueFunction(function=launch_setup), + ] + ) diff --git a/guidance/reference_filter_dp_quat/package.xml b/guidance/reference_filter_dp_quat/package.xml new file mode 100644 index 000000000..d7aa188a2 --- /dev/null +++ b/guidance/reference_filter_dp_quat/package.xml @@ -0,0 +1,24 @@ + + + + reference_filter_dp_quat + 0.0.0 + Quaternion Reference model for the DP controller + jorgenfj + MIT + + ament_cmake + + rclcpp + rclcpp_action + geometry_msgs + vortex_msgs + vortex_utils + vortex_utils_ros + nav_msgs + eigen + + + ament_cmake + + diff --git a/guidance/reference_filter_dp_quat/src/lib/reference_filter.cpp b/guidance/reference_filter_dp_quat/src/lib/reference_filter.cpp new file mode 100644 index 000000000..c7d1f0cb5 --- /dev/null +++ b/guidance/reference_filter_dp_quat/src/lib/reference_filter.cpp @@ -0,0 +1,37 @@ +#include "reference_filter_dp_quat/lib/reference_filter.hpp" + +namespace vortex::guidance { + +ReferenceFilter::ReferenceFilter(const ReferenceFilterParams& params) { + calculate_Ad(params.omega, params.zeta); + calculate_Bd(params.omega); +} + +Eigen::Vector18d ReferenceFilter::calculate_x_dot(const Eigen::Vector18d& x, + const Eigen::Vector6d& r) { + Eigen::Vector18d x_dot = Ad_ * x + Bd_ * r; + + return x_dot; +} + +void ReferenceFilter::calculate_Ad(const Eigen::Vector6d& omega, + const Eigen::Vector6d& zeta) { + Eigen::Matrix6d omega_diag = omega.asDiagonal(); + Eigen::Matrix6d delta = zeta.asDiagonal(); + Eigen::Matrix6d omega_diag_squared = omega_diag * omega_diag; + Eigen::Matrix6d omega_diag_cubed = omega_diag * omega_diag * omega_diag; + Ad_.block<6, 6>(0, 6) = Eigen::Matrix6d::Identity(); + Ad_.block<6, 6>(12, 0) = -omega_diag_cubed; + Ad_.block<6, 6>(12, 6) = + -(2 * delta + Eigen::Matrix6d::Identity()) * omega_diag_squared; + Ad_.block<6, 6>(12, 12) = + -(2 * delta + Eigen::Matrix6d::Identity()) * omega_diag; + Ad_.block<6, 6>(6, 12) = Eigen::Matrix6d::Identity(); +} + +void ReferenceFilter::calculate_Bd(const Eigen::Vector6d& omega) { + Eigen::Matrix6d omega_diag = omega.asDiagonal(); + Bd_.block<6, 6>(12, 0) = omega_diag * omega_diag * omega_diag; +} + +} // namespace vortex::guidance diff --git a/guidance/reference_filter_dp_quat/src/lib/waypoint_follower.cpp b/guidance/reference_filter_dp_quat/src/lib/waypoint_follower.cpp new file mode 100644 index 000000000..ce58bb3d3 --- /dev/null +++ b/guidance/reference_filter_dp_quat/src/lib/waypoint_follower.cpp @@ -0,0 +1,98 @@ +#include "reference_filter_dp_quat/lib/waypoint_follower.hpp" +#include +#include +#include "reference_filter_dp_quat/lib/eigen_typedefs.hpp" + +namespace vortex::guidance { + +WaypointFollower::WaypointFollower(const ReferenceFilterParams& params, + double dt_seconds) + : filter_(params), dt_seconds_(dt_seconds) {} + +void WaypointFollower::start(const Pose& pose, + const Twist& twist, + const Waypoint& waypoint, + double convergence_threshold) { + std::lock_guard lock(mutex_); + + nominal_pose_ = pose; + state_ = Eigen::Vector18d::Zero(); + + Eigen::Matrix3d R = pose.as_rotation_matrix(); + Eigen::Vector6d twist_vec = twist.to_vector(); + state_.segment<3>(6) = R * twist_vec.head<3>(); + state_.segment<3>(9) = R * twist_vec.tail<3>(); + + waypoint_mode_ = waypoint.mode; + convergence_threshold_ = convergence_threshold; + waypoint_goal_ = vortex::utils::waypoints::compute_waypoint_goal( + waypoint.pose, waypoint_mode_, nominal_pose_); +} + +void WaypointFollower::step() { + std::lock_guard lock(mutex_); + const Eigen::Vector6d filter_reference = update_reference(); + + const Eigen::Vector18d state_derivative = + filter_.calculate_x_dot(state_, filter_reference); + state_ += state_derivative * dt_seconds_; + inject_and_reset(); +} + +Eigen::Vector6d WaypointFollower::update_reference() const { + Eigen::Vector6d filter_reference; + filter_reference.head<3>() = + waypoint_goal_.pos_vector() - nominal_pose_.pos_vector(); + filter_reference.tail<3>() = vortex::utils::math::quaternion_error( + nominal_pose_.ori_quaternion(), waypoint_goal_.ori_quaternion()); + return filter_reference; +} + +void WaypointFollower::inject_and_reset() { + nominal_pose_.set_pos(nominal_pose_.pos_vector() + state_.head<3>()); + state_.head<3>().setZero(); + + const Eigen::Vector3d delta_orientation = state_.segment<3>(3); + const double angle = delta_orientation.norm(); + if (angle >= 1e-10) { + Eigen::Quaterniond delta_quat( + Eigen::AngleAxisd(angle, delta_orientation.normalized())); + nominal_pose_.set_ori(nominal_pose_.ori_quaternion() * delta_quat); + state_.segment<3>(3).setZero(); + } +} + +bool WaypointFollower::within_convergance(const Pose& measured_pose) const { + std::lock_guard lock(mutex_); + return vortex::utils::waypoints::has_converged( + measured_pose, waypoint_goal_, waypoint_mode_, convergence_threshold_); +} + +void WaypointFollower::set_reference(const Pose& reference_goal_pose) { + std::lock_guard lock(mutex_); + waypoint_goal_ = vortex::utils::waypoints::compute_waypoint_goal( + reference_goal_pose, waypoint_mode_, nominal_pose_); +} + +void WaypointFollower::snap_state_to_reference() { + std::lock_guard lock(mutex_); + nominal_pose_ = waypoint_goal_; + state_.head<12>().setZero(); +} + +Pose WaypointFollower::pose() const { + std::lock_guard lock(mutex_); + return nominal_pose_; +} + +Eigen::Vector6d WaypointFollower::velocity() const { + std::lock_guard lock(mutex_); + return state_.segment<6>(6); +} + +Pose WaypointFollower::waypoint_goal() const { + std::lock_guard lock(mutex_); + return waypoint_goal_; +} + +} // namespace vortex::guidance diff --git a/guidance/reference_filter_dp_quat/src/ros/reference_filter_ros.cpp b/guidance/reference_filter_dp_quat/src/ros/reference_filter_ros.cpp new file mode 100644 index 000000000..a129d2aef --- /dev/null +++ b/guidance/reference_filter_dp_quat/src/ros/reference_filter_ros.cpp @@ -0,0 +1,237 @@ +#include "reference_filter_dp_quat/ros/reference_filter_ros.hpp" +#include +#include +#include +#include +#include +#include +#include "reference_filter_dp_quat/ros/reference_filter_ros_utils.hpp" + +const auto start_message = R"( + ____ __ _____ _ _ _ + | _ \ ___ / _| ___ _ __ ___ _ __ ___ ___ | ___(_) | |_ ___ _ __ + | |_) / _ \ |_ / _ \ '__/ _ \ '_ \ / __/ _ \ | |_ | | | __/ _ \ '__| + | _ < __/ _| __/ | | __/ | | | (_| __/ | _| | | | || __/ | + |_| \_\___|_| \___|_| \___|_| |_|\___\___| |_| |_|_|\__\___|_| + + )"; + +namespace vortex::guidance { + +ReferenceFilterNode::ReferenceFilterNode(const rclcpp::NodeOptions& options) + : Node("reference_filter_node", options) { + int time_step_ms = this->declare_parameter("time_step_ms"); + time_step_ = std::chrono::milliseconds(time_step_ms); + + set_subscribers_and_publisher(); + + set_action_server(); + + set_refererence_filter(); + + spdlog::info(start_message); +} + +ReferenceFilterNode::~ReferenceFilterNode() { + preempted_ = true; + if (execute_thread_.joinable()) { + execute_thread_.join(); + } +} + +void ReferenceFilterNode::set_subscribers_and_publisher() { + this->declare_parameter("topics.pose"); + this->declare_parameter("topics.twist"); + this->declare_parameter("topics.guidance.dp_quat"); + this->declare_parameter("topics.reference_pose"); + + std::string pose_topic = this->get_parameter("topics.pose").as_string(); + std::string twist_topic = this->get_parameter("topics.twist").as_string(); + std::string guidance_topic = + this->get_parameter("topics.guidance.dp_quat").as_string(); + std::string reference_pose_topic = + this->get_parameter("topics.reference_pose").as_string(); + + auto qos_sensor_data = vortex::utils::qos_profiles::sensor_data_profile(1); + reference_pub_ = + this->create_publisher( + guidance_topic, qos_sensor_data); + + reference_sub_ = this->create_subscription( + reference_pose_topic, qos_sensor_data, + [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg) { + follower_->set_reference( + vortex::utils::ros_conversions::ros_pose_to_pose(msg->pose)); + }); + + pose_sub_ = this->create_subscription< + geometry_msgs::msg::PoseWithCovarianceStamped>( + pose_topic, qos_sensor_data, + [this](const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr + msg) { + std::lock_guard lock(sensor_mutex_); + current_pose_ = vortex::utils::ros_conversions::ros_pose_to_pose( + msg->pose.pose); + }); + + twist_sub_ = this->create_subscription< + geometry_msgs::msg::TwistWithCovarianceStamped>( + twist_topic, qos_sensor_data, + [this](const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr + msg) { + std::lock_guard lock(sensor_mutex_); + current_twist_ = vortex::utils::ros_conversions::ros_twist_to_twist( + msg->twist.twist); + }); +} + +void ReferenceFilterNode::set_action_server() { + this->declare_parameter("action_servers.reference_filter"); + std::string action_server_name = + this->get_parameter("action_servers.reference_filter").as_string(); + + action_server_ = rclcpp_action::create_server< + vortex_msgs::action::ReferenceFilterQuatWaypoint>( + this, action_server_name, + [this](const auto& uuid, auto goal) { + return handle_goal(uuid, std::move(goal)); + }, + [this](auto goal_handle) { return handle_cancel(goal_handle); }, + [this](auto goal_handle) { handle_accepted(goal_handle); }); +} + +void ReferenceFilterNode::set_refererence_filter() { + this->declare_parameter>("zeta"); + this->declare_parameter>("omega"); + + std::vector zeta = this->get_parameter("zeta").as_double_array(); + std::vector omega = this->get_parameter("omega").as_double_array(); + + Eigen::Vector6d zeta_eigen = Eigen::Map(zeta.data()); + Eigen::Vector6d omega_eigen = Eigen::Map(omega.data()); + + filter_params_ = ReferenceFilterParams{omega_eigen, zeta_eigen}; + + double dt_seconds = time_step_.count() / 1000.0; + follower_ = std::make_unique(filter_params_, dt_seconds); +} + +rclcpp_action::GoalResponse ReferenceFilterNode::handle_goal( + const rclcpp_action::GoalUUID& /*uuid*/, + std::shared_ptr< + const vortex_msgs::action::ReferenceFilterQuatWaypoint::Goal> + /*goal*/) { + spdlog::info("Accepted goal request"); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse ReferenceFilterNode::handle_cancel( + const std::shared_ptr> /*goal_handle*/) { + spdlog::info("Received request to cancel goal"); + return rclcpp_action::CancelResponse::ACCEPT; +} + +void ReferenceFilterNode::handle_accepted( + const std::shared_ptr> goal_handle) { + std::lock_guard lock(execute_mutex_); + preempted_ = true; + if (execute_thread_.joinable()) { + execute_thread_.join(); + } + preempted_ = false; + + execute_thread_ = + std::thread([this, goal_handle]() { execute(goal_handle); }); +} + +void ReferenceFilterNode::execute( + const std::shared_ptr> goal_handle) { + spdlog::info("Executing goal"); + + double convergence_threshold = + goal_handle->get_goal()->convergence_threshold; + + if (convergence_threshold <= 0.0) { + convergence_threshold = 0.1; + spdlog::warn( + "ReferenceFilter: Invalid convergence_threshold received (<= 0). " + "Using default 0.1"); + } + + const auto wp = vortex::utils::waypoints::waypoint_from_ros( + goal_handle->get_goal()->waypoint); + + const auto [pose, twist] = [this] { + std::lock_guard lock(sensor_mutex_); + return std::pair{current_pose_, current_twist_}; + }(); + + follower_->start(pose, twist, wp, convergence_threshold); + + auto result = std::make_shared< + vortex_msgs::action::ReferenceFilterQuatWaypoint::Result>(); + + rclcpp::Rate loop_rate(1000.0 / time_step_.count()); + + while (rclcpp::ok()) { + if (preempted_.load()) { + result->success = false; + goal_handle->abort(result); + spdlog::info("Goal preempted by newer goal"); + return; + } + + if (goal_handle->is_canceling()) { + result->success = false; + goal_handle->canceled(result); + spdlog::info("Goal canceled"); + return; + } + + follower_->step(); + + const auto current_pose = [this] { + std::lock_guard lock(sensor_mutex_); + return current_pose_; + }(); + + bool target_reached = follower_->within_convergance(current_pose); + + if (target_reached) { + follower_->snap_state_to_reference(); + + auto final_reference_msg = + fill_reference_msg(follower_->pose(), follower_->velocity()); + + reference_pub_->publish(final_reference_msg); + + result->success = true; + goal_handle->succeed(result); + spdlog::info("Goal reached"); + return; + } + + auto reference_msg = + fill_reference_msg(follower_->pose(), follower_->velocity()); + reference_pub_->publish(reference_msg); + loop_rate.sleep(); + } + if (!rclcpp::ok() && goal_handle->is_active()) { + auto result = std::make_shared< + vortex_msgs::action::ReferenceFilterQuatWaypoint::Result>(); + result->success = false; + + try { + goal_handle->abort(result); + } catch (...) { + // Ignore exceptions during shutdown + } + } +} + +RCLCPP_COMPONENTS_REGISTER_NODE(ReferenceFilterNode) + +} // namespace vortex::guidance diff --git a/guidance/reference_filter_dp_quat/test/CMakeLists.txt b/guidance/reference_filter_dp_quat/test/CMakeLists.txt new file mode 100644 index 000000000..9295a4300 --- /dev/null +++ b/guidance/reference_filter_dp_quat/test/CMakeLists.txt @@ -0,0 +1,39 @@ +cmake_minimum_required(VERSION 3.8) + +find_package(GTest REQUIRED) +include(GoogleTest) + +set(TEST_BINARY_NAME ${PROJECT_NAME}_test) +add_executable( + ${TEST_BINARY_NAME} + test_reference_filter.cpp +) + +target_link_libraries( + ${TEST_BINARY_NAME} + PRIVATE + ${CORE_LIB_NAME} + GTest::GTest +) + +ament_target_dependencies(${TEST_BINARY_NAME} PUBLIC Eigen3) + +gtest_discover_tests(${TEST_BINARY_NAME}) + +# Waypoint follower tests +set(TEST_WAYPOINT_FOLLOWER ${PROJECT_NAME}_test_waypoint_follower) +add_executable( + ${TEST_WAYPOINT_FOLLOWER} + test_waypoint_follower.cpp +) + +target_link_libraries( + ${TEST_WAYPOINT_FOLLOWER} + PRIVATE + ${CORE_LIB_NAME} + GTest::GTest +) + +ament_target_dependencies(${TEST_WAYPOINT_FOLLOWER} PUBLIC Eigen3 vortex_utils) + +gtest_discover_tests(${TEST_WAYPOINT_FOLLOWER}) diff --git a/guidance/reference_filter_dp_quat/test/test_reference_filter.cpp b/guidance/reference_filter_dp_quat/test/test_reference_filter.cpp new file mode 100644 index 000000000..d5fab4c19 --- /dev/null +++ b/guidance/reference_filter_dp_quat/test/test_reference_filter.cpp @@ -0,0 +1,58 @@ +#include + +#include "reference_filter_dp_quat/lib/eigen_typedefs.hpp" +#include "reference_filter_dp_quat/lib/reference_filter.hpp" + +namespace vortex::guidance { + +class ReferenceFilterTests : public ::testing::Test { + protected: + ReferenceFilterTests() : reference_filter_{get_filter_params()} {} + + ReferenceFilterParams get_filter_params() { + ReferenceFilterParams params; + params.omega = Eigen::Vector6d::Ones(); + params.zeta = Eigen::Vector6d::Ones(); + return params; + } + + ReferenceFilter reference_filter_; +}; + +TEST_F(ReferenceFilterTests, T01_positive_command_positive_xdot) { + Eigen::Vector18d x = Eigen::Vector18d::Zero(); + Eigen::Vector6d r = Eigen::Vector6d::Ones(); + Eigen::Vector18d xdot = reference_filter_.calculate_x_dot(x, r); + + for (int i = 0; i < xdot.size(); ++i) { + EXPECT_GE(xdot(i), 0.0); + } +} + +TEST_F(ReferenceFilterTests, T02_negative_command_negative_xdot) { + Eigen::Vector18d x = Eigen::Vector18d::Zero(); + Eigen::Vector6d r = -Eigen::Vector6d::Ones(); + Eigen::Vector18d xdot = reference_filter_.calculate_x_dot(x, r); + + for (int i = 0; i < xdot.size(); ++i) { + EXPECT_LE(xdot(i), 0.0); + } +} + +TEST_F(ReferenceFilterTests, T03_zero_command_zero_xdot) { + Eigen::Vector18d x = Eigen::Vector18d::Zero(); + Eigen::Vector6d r = Eigen::Vector6d::Zero(); + Eigen::Vector18d xdot = reference_filter_.calculate_x_dot(x, r); + + for (int i = 0; i < xdot.size(); ++i) { + EXPECT_DOUBLE_EQ(xdot(i), 0.0); + } +} + +} // namespace vortex::guidance + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} diff --git a/guidance/reference_filter_dp_quat/test/test_waypoint_follower.cpp b/guidance/reference_filter_dp_quat/test/test_waypoint_follower.cpp new file mode 100644 index 000000000..2c482c9dc --- /dev/null +++ b/guidance/reference_filter_dp_quat/test/test_waypoint_follower.cpp @@ -0,0 +1,112 @@ +#include +#include +#include "reference_filter_dp_quat/lib/waypoint_follower.hpp" + +namespace vortex::guidance { + +class WaypointFollowerTests : public ::testing::Test { + protected: + ReferenceFilterParams get_params() { + ReferenceFilterParams params; + params.omega = Eigen::Vector6d::Ones(); + params.zeta = Eigen::Vector6d::Ones(); + return params; + } + + Pose zero_pose() { return Pose{}; } + Twist zero_twist() { return Twist{}; } +}; + +TEST_F(WaypointFollowerTests, StartAndStepConverges) { + WaypointFollower follower(get_params(), 0.01); + + Waypoint wp; + wp.pose = Pose{1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0}; + wp.mode = WaypointMode::FULL_POSE; + + follower.start(zero_pose(), zero_twist(), wp, 0.1); + + follower.step(); + + // Simulate the measured pose being at the reference + Pose measured_at_ref{1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0}; + + EXPECT_TRUE(follower.within_convergance(measured_at_ref)); +} + +TEST_F(WaypointFollowerTests, StepDoesNotConvergeWhenFar) { + WaypointFollower follower(get_params(), 0.01); + + Waypoint wp; + wp.pose = Pose{10.0, 10.0, 0.0, 1.0, 0.0, 0.0, 0.0}; + wp.mode = WaypointMode::FULL_POSE; + + follower.start(zero_pose(), zero_twist(), wp, 0.1); + + follower.step(); + + EXPECT_FALSE(follower.within_convergance(zero_pose())); +} + +TEST_F(WaypointFollowerTests, SetReferenceUpdatesMidSequence) { + WaypointFollower follower(get_params(), 0.01); + + Waypoint wp; + wp.pose = Pose{1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0}; + wp.mode = WaypointMode::FULL_POSE; + + follower.start(zero_pose(), zero_twist(), wp, 0.1); + + Pose new_ref{5.0, 5.0, 0.0, 1.0, 0.0, 0.0, 0.0}; + follower.set_reference(new_ref); + + EXPECT_DOUBLE_EQ(follower.waypoint_goal().x, 5.0); + EXPECT_DOUBLE_EQ(follower.waypoint_goal().y, 5.0); +} + +TEST_F(WaypointFollowerTests, SnapStateToReference) { + WaypointFollower follower(get_params(), 0.01); + + Waypoint wp; + wp.pose = Pose{3.0, 4.0, 5.0, 1.0, 0.0, 0.0, 0.0}; + wp.mode = WaypointMode::FULL_POSE; + + follower.start(zero_pose(), zero_twist(), wp, 0.1); + follower.snap_state_to_reference(); + + Pose pose = follower.pose(); + Pose goal = follower.waypoint_goal(); + + EXPECT_DOUBLE_EQ(pose.x, goal.x); + EXPECT_DOUBLE_EQ(pose.y, goal.y); + EXPECT_DOUBLE_EQ(pose.z, goal.z); + EXPECT_DOUBLE_EQ(pose.qw, goal.qw); + EXPECT_DOUBLE_EQ(pose.qx, goal.qx); + EXPECT_DOUBLE_EQ(pose.qy, goal.qy); + EXPECT_DOUBLE_EQ(pose.qz, goal.qz); +} + +TEST_F(WaypointFollowerTests, StateEvolvesWithStep) { + WaypointFollower follower(get_params(), 0.01); + + Waypoint wp; + wp.pose = Pose{1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0}; + wp.mode = WaypointMode::FULL_POSE; + + follower.start(zero_pose(), zero_twist(), wp, 0.1); + + // Run several steps — state should move toward reference + for (int i = 0; i < 100; ++i) { + follower.step(); + } + + // x position should have moved toward 1.0 + EXPECT_GT(follower.pose().x, 0.0); +} + +} // namespace vortex::guidance + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From edf1e1bd5cf265e146dd790bde5fc6b6a8f2b85e Mon Sep 17 00:00:00 2001 From: Henrik Date: Sun, 29 Mar 2026 18:13:05 +0200 Subject: [PATCH 124/128] thougts and prayers --- .pre-commit-config.yaml | 4 +- .../reference_filter_dp/eigen_typedefs.hpp | 19 -- .../reference_filter_dp/reference_filter.hpp | 44 --- .../reference_filter_ros.hpp | 126 -------- .../reference_filter_utils.hpp | 26 -- .../src/reference_filter.cpp | 38 --- .../src/reference_filter_ros.cpp | 294 ------------------ .../src/reference_filter_utils.cpp | 55 ---- 8 files changed, 2 insertions(+), 604 deletions(-) delete mode 100644 guidance/reference_filter_dp/include/reference_filter_dp/eigen_typedefs.hpp delete mode 100644 guidance/reference_filter_dp/include/reference_filter_dp/reference_filter.hpp delete mode 100644 guidance/reference_filter_dp/include/reference_filter_dp/reference_filter_ros.hpp delete mode 100644 guidance/reference_filter_dp/include/reference_filter_dp/reference_filter_utils.hpp delete mode 100644 guidance/reference_filter_dp/src/reference_filter.cpp delete mode 100644 guidance/reference_filter_dp/src/reference_filter_ros.cpp delete mode 100644 guidance/reference_filter_dp/src/reference_filter_utils.cpp diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 51b80195c..ef82f0dfb 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -57,7 +57,7 @@ repos: "--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D401", "--fix", ] - stages: [pre-commit] + stages: [commit] pass_filenames: true - id: ruff name: ruff-check @@ -66,7 +66,7 @@ repos: "--ignore=T201,N812,B006,S101,S311,S607,S603", "--fix" ] - stages: [pre-commit] + stages: [commit] pass_filenames: true # C++ hooks - repo: https://github.com/pre-commit/mirrors-clang-format diff --git a/guidance/reference_filter_dp/include/reference_filter_dp/eigen_typedefs.hpp b/guidance/reference_filter_dp/include/reference_filter_dp/eigen_typedefs.hpp deleted file mode 100644 index c7d795e15..000000000 --- a/guidance/reference_filter_dp/include/reference_filter_dp/eigen_typedefs.hpp +++ /dev/null @@ -1,19 +0,0 @@ -/** - * @file eigen_typedefs.hpp - * @brief Contains the typedef for a 6x1 Eigen vector, 6x6 Eigen matrix. - * and 18x18 Eigen matrix. - */ - -#ifndef VORTEX_EIGEN_TYPEDEFS_H -#define VORTEX_EIGEN_TYPEDEFS_H - -#include - -typedef Eigen::Matrix Matrix18d; -typedef Eigen::Matrix Matrix18x6d; -typedef Eigen::Matrix Matrix6d; -typedef Eigen::Matrix Vector6d; -typedef Eigen::Matrix Vector18d; -typedef Eigen::Matrix Matrix3d; - -#endif diff --git a/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter.hpp b/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter.hpp deleted file mode 100644 index 6c1c8c4f2..000000000 --- a/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter.hpp +++ /dev/null @@ -1,44 +0,0 @@ -#ifndef REFERENCE_FILTER_HPP -#define REFERENCE_FILTER_HPP - -#include "reference_filter_dp/eigen_typedefs.hpp" - -class ReferenceFilter { - public: - explicit ReferenceFilter(); - - // @brief Calculate the state derivative - // @param x The state vector 18x1 - // @param r The reference vector 6x1 - // @return The state derivative 18x1 - // REF: Handbook of Marine Craft Hydrodynamics and Motion Control, Fossen - // 2021 p. 337 eq: 12.11 - Vector18d calculate_x_dot(const Vector18d& x, const Vector6d& r); - - // @brief Calculate the state transition matrix - // REF: Handbook of Marine Craft Hydrodynamics and Motion Control, Fossen - // 2021 p. 337 eq: 12.12 - void calculate_Ad(); - - // @brief Calculate the input matrix - // REF: Handbook of Marine Craft Hydrodynamics and Motion Control, Fossen - // 2021 p. 337 eq: 12.12 - void calculate_Bd(); - - // @brief Set the omega matrix - // @param omega The omega matrix 6x1 vector - void set_omega(const Vector6d& omega); - - // @brief Set the delta matrix - // @param zeta The delta matrix 6x1 vector - void set_delta(const Vector6d& zeta); - - private: - Matrix18d Ad_; - Matrix18x6d Bd_; - Matrix6d Omega_; - Matrix6d Delta_; - Matrix6d identity_matrix_; -}; - -#endif diff --git a/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter_ros.hpp b/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter_ros.hpp deleted file mode 100644 index 997360807..000000000 --- a/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter_ros.hpp +++ /dev/null @@ -1,126 +0,0 @@ -#ifndef REFERENCE_FILTER_ROS_HPP -#define REFERENCE_FILTER_ROS_HPP - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -class ReferenceFilterNode : public rclcpp::Node { - public: - explicit ReferenceFilterNode( - const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); - - private: - // @brief Set the subscribers and publishers - void set_subscribers_and_publisher(); - - // @brief Set the action server - void set_action_server(); - - // @brief Initializes the reference filter with ROS parameters. - void set_refererence_filter(); - - // @brief Callback for the reference topic - // @param msg The reference message - void reference_callback( - const geometry_msgs::msg::PoseStamped::SharedPtr msg); - - // @brief Callback for the pose topic - // @param msg The pose message - void pose_callback( - const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); - - // @brief Callback for the twist topic - // @param msg The twist message - void twist_callback( - const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); - - // @brief Handle the goal request - // @param uuid The goal UUID - // @param goal The goal message - // @return The goal response - rclcpp_action::GoalResponse handle_goal( - const rclcpp_action::GoalUUID& uuid, - std::shared_ptr< - const vortex_msgs::action::ReferenceFilterWaypoint::Goal> goal); - - // @brief Handle the cancel request - // @param goal_handle The goal handle - // @return The cancel response - rclcpp_action::CancelResponse handle_cancel( - const std::shared_ptr> goal_handle); - - // @brief Handle the accepted request - // @param goal_handle The goal handle - void handle_accepted( - const std::shared_ptr> goal_handle); - - // @brief Execute the goal - // @param goal_handle The goal handle - void execute( - const std::shared_ptr> goal_handle); - - Vector18d fill_reference_state(); - - Vector6d fill_reference_goal(const geometry_msgs::msg::PoseStamped& goal); - - vortex_msgs::msg::ReferenceFilter fill_reference_msg(); - - rclcpp_action::Server< - vortex_msgs::action::ReferenceFilterWaypoint>::SharedPtr action_server_; - - ReferenceFilter reference_filter_; - - rclcpp::Publisher::SharedPtr - reference_pub_; - - rclcpp::Subscription::SharedPtr - reference_sub_; - - rclcpp::Subscription< - geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_; - - rclcpp::Subscription< - geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr twist_sub_; - - rclcpp::TimerBase::SharedPtr reference_pub_timer_; - - std::chrono::milliseconds time_step_; - - geometry_msgs::msg::PoseWithCovarianceStamped current_pose_; - - geometry_msgs::msg::TwistWithCovarianceStamped current_twist_; - - // x is [eta, eta_dot, eta_dot_dot] (ref. page 337 in Fossen, 2021 - // nu and eta are 6 degrees of freedom (position and orientation in 3D - // space) - Vector18d x_; - - // The reference signal vector with 6 degrees of freedom [eta] - Vector6d r_; - - std::mutex mutex_; - - rclcpp_action::GoalUUID preempted_goal_id_; - - std::shared_ptr> - goal_handle_; - - rclcpp::CallbackGroup::SharedPtr cb_group_; -}; - -#endif diff --git a/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter_utils.hpp b/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter_utils.hpp deleted file mode 100644 index e1ccf3f04..000000000 --- a/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter_utils.hpp +++ /dev/null @@ -1,26 +0,0 @@ -#ifndef REFERENCE_FILTER_UTILS_HPP -#define REFERENCE_FILTER_UTILS_HPP - -#include - -// @brief Calculate the rotation matrix from the Euler angles -// @param eta 6x1 vector containing [surge, sway, heave, roll, pitch, yaw] -// @return The rotation matrix -Matrix3d calculate_R(const Vector6d& eta); - -// @brief Calculate the transformation matrix from the Euler angles -// @param eta 6x1 vector containing [surge, sway, heave, roll, pitch, yaw] -// @return The transformation matrix -Matrix3d calculate_T(const Vector6d& eta); - -// @brief Calculate the Jacobian matrix from the Euler angles -// @param eta 6x1 vector containing [surge, sway, heave, roll, pitch, yaw] -// @return The Jacobian matrix -Matrix6d calculate_J(const Vector6d& eta); - -// @brief Calculate the smallest signed angle -// @param angle the angle given in radians -// @return The smallest signed angle -double ssa(double angle); - -#endif diff --git a/guidance/reference_filter_dp/src/reference_filter.cpp b/guidance/reference_filter_dp/src/reference_filter.cpp deleted file mode 100644 index 4ebdc2e1d..000000000 --- a/guidance/reference_filter_dp/src/reference_filter.cpp +++ /dev/null @@ -1,38 +0,0 @@ -#include - -ReferenceFilter::ReferenceFilter() - : Ad_(Matrix18d::Zero()), - Bd_(Matrix18x6d::Zero()), - Omega_(Matrix6d::Identity()), - Delta_(Matrix6d::Identity()), - identity_matrix_(Matrix6d::Identity()) {} - -Vector18d ReferenceFilter::calculate_x_dot(const Vector18d& x, - const Vector6d& r) { - Vector18d x_dot = Ad_ * x + Bd_ * r; - - return x_dot; -} - -void ReferenceFilter::calculate_Ad() { - Matrix6d OmegaCubed = Omega_ * Omega_ * Omega_; - Matrix6d OmegaSquared = Omega_ * Omega_; - Ad_.block<6, 6>(0, 6) = identity_matrix_; - Ad_.block<6, 6>(12, 0) = -OmegaCubed; - Ad_.block<6, 6>(12, 6) = -(2 * Delta_ + identity_matrix_) * OmegaSquared; - Ad_.block<6, 6>(12, 12) = -(2 * Delta_ + identity_matrix_) * Omega_; - Ad_.block<6, 6>(6, 12) = identity_matrix_; -} - -void ReferenceFilter::calculate_Bd() { - Matrix6d OmegaCubed = Omega_ * Omega_ * Omega_; - Bd_.block<6, 6>(12, 0) = OmegaCubed; -} - -void ReferenceFilter::set_omega(const Vector6d& omega) { - Omega_ = omega.asDiagonal(); -} - -void ReferenceFilter::set_delta(const Vector6d& zeta) { - Delta_ = zeta.asDiagonal(); -} diff --git a/guidance/reference_filter_dp/src/reference_filter_ros.cpp b/guidance/reference_filter_dp/src/reference_filter_ros.cpp deleted file mode 100644 index 22c68dfe8..000000000 --- a/guidance/reference_filter_dp/src/reference_filter_ros.cpp +++ /dev/null @@ -1,294 +0,0 @@ -#include -#include -#include - -ReferenceFilterNode::ReferenceFilterNode(const rclcpp::NodeOptions& options) - : Node("reference_filter_node", options) { - time_step_ = std::chrono::milliseconds(10); - - set_subscribers_and_publisher(); - - set_action_server(); - - set_refererence_filter(); - - x_ = Vector18d::Zero(); - - spdlog::info("Reference filter node initialized"); -} - -void ReferenceFilterNode::set_subscribers_and_publisher() { - this->declare_parameter("topics.pose"); - this->declare_parameter("topics.twist"); - this->declare_parameter("topics.guidance.dp"); - this->declare_parameter("topics.aruco_board_pose_camera"); - - std::string pose_topic = this->get_parameter("topics.pose").as_string(); - std::string twist_topic = this->get_parameter("topics.twist").as_string(); - std::string guidance_topic = - this->get_parameter("topics.guidance.dp").as_string(); - std::string aruco_board_pose_camera_topic = - this->get_parameter("topics.aruco_board_pose_camera").as_string(); - - rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; - auto qos_sensor_data = rclcpp::QoS( - rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); - reference_pub_ = this->create_publisher( - guidance_topic, qos_sensor_data); - reference_sub_ = this->create_subscription( - aruco_board_pose_camera_topic, qos_sensor_data, - std::bind(&ReferenceFilterNode::reference_callback, this, - std::placeholders::_1)); - pose_sub_ = this->create_subscription< - geometry_msgs::msg::PoseWithCovarianceStamped>( - pose_topic, qos_sensor_data, - std::bind(&ReferenceFilterNode::pose_callback, this, - std::placeholders::_1)); - twist_sub_ = this->create_subscription< - geometry_msgs::msg::TwistWithCovarianceStamped>( - twist_topic, qos_sensor_data, - std::bind(&ReferenceFilterNode::twist_callback, this, - std::placeholders::_1)); -} - -void ReferenceFilterNode::set_action_server() { - this->declare_parameter("action_servers.reference_filter"); - std::string action_server_name = - this->get_parameter("action_servers.reference_filter").as_string(); - cb_group_ = - this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); - - action_server_ = rclcpp_action::create_server< - vortex_msgs::action::ReferenceFilterWaypoint>( - this, action_server_name, - std::bind(&ReferenceFilterNode::handle_goal, this, - std::placeholders::_1, std::placeholders::_2), - std::bind(&ReferenceFilterNode::handle_cancel, this, - std::placeholders::_1), - std::bind(&ReferenceFilterNode::handle_accepted, this, - std::placeholders::_1), - rcl_action_server_get_default_options(), cb_group_); -} - -void ReferenceFilterNode::set_refererence_filter() { - this->declare_parameter>( - "zeta", {0.707, 0.707, 0.707, 0.707, 0.707, 0.707}); - this->declare_parameter>( - "omega", {0.1, 0.1, 0.1, 0.1, 0.1, 0.1}); - - std::vector zeta = this->get_parameter("zeta").as_double_array(); - std::vector omega = this->get_parameter("omega").as_double_array(); - - const Vector6d zeta_eigen = Eigen::Map(zeta.data()); - const Vector6d omega_eigen = Eigen::Map(omega.data()); - - reference_filter_.set_delta(zeta_eigen); - reference_filter_.set_omega(omega_eigen); - - reference_filter_.calculate_Ad(); - reference_filter_.calculate_Bd(); -} - -void ReferenceFilterNode::reference_callback( - const geometry_msgs::msg::PoseStamped::SharedPtr msg) { - double x = msg->pose.position.x; - double y = msg->pose.position.y; - double z = msg->pose.position.z; - - tf2::Quaternion q; - q.setX(msg->pose.orientation.x); - q.setY(msg->pose.orientation.y); - q.setZ(msg->pose.orientation.z); - q.setW(msg->pose.orientation.w); - - tf2::Matrix3x3 m(q); - double roll, pitch, yaw; - m.getRPY(roll, pitch, yaw); - - r_ << x, y, z, roll, pitch, yaw; -} - -void ReferenceFilterNode::pose_callback( - const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { - current_pose_ = *msg; -} - -void ReferenceFilterNode::twist_callback( - const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { - current_twist_ = *msg; -} - -rclcpp_action::GoalResponse ReferenceFilterNode::handle_goal( - const rclcpp_action::GoalUUID& uuid, - std::shared_ptr - goal) { - (void)uuid; - (void)goal; - { - std::lock_guard lock(mutex_); - if (goal_handle_) { - if (goal_handle_->is_active()) { - spdlog::info("Aborting current goal and accepting new goal"); - preempted_goal_id_ = goal_handle_->get_goal_id(); - } - } - } - spdlog::info("Accepted goal request"); - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; -} - -rclcpp_action::CancelResponse ReferenceFilterNode::handle_cancel( - const std::shared_ptr> goal_handle) { - spdlog::info("Received request to cancel goal"); - (void)goal_handle; - return rclcpp_action::CancelResponse::ACCEPT; -} - -void ReferenceFilterNode::handle_accepted( - const std::shared_ptr> goal_handle) { - execute(goal_handle); -} - -Vector18d ReferenceFilterNode::fill_reference_state() { - Vector18d x = Vector18d::Zero(); - x(0) = current_pose_.pose.pose.position.x; - x(1) = current_pose_.pose.pose.position.y; - x(2) = current_pose_.pose.pose.position.z; - - tf2::Quaternion q; - tf2::fromMsg(current_pose_.pose.pose.orientation, q); - tf2::Matrix3x3 m(q); - double roll, pitch, yaw; - m.getRPY(roll, pitch, yaw); - - x(3) = ssa(roll); - x(4) = ssa(pitch); - x(5) = ssa(yaw); - - Vector6d eta; - eta << current_pose_.pose.pose.position.x, - current_pose_.pose.pose.position.y, current_pose_.pose.pose.position.z, - roll, pitch, yaw; - Matrix6d J = calculate_J(eta); - Vector6d nu; - nu << current_twist_.twist.twist.linear.x, - current_twist_.twist.twist.linear.y, - current_twist_.twist.twist.linear.z, - current_twist_.twist.twist.angular.x, - current_twist_.twist.twist.angular.y, - current_twist_.twist.twist.angular.z; - Vector6d eta_dot = J * nu; - - x(6) = eta_dot(0); - x(7) = eta_dot(1); - x(8) = eta_dot(2); - x(9) = eta_dot(3); - x(10) = eta_dot(4); - x(11) = eta_dot(5); - - return x; -} - -Vector6d ReferenceFilterNode::fill_reference_goal( - const geometry_msgs::msg::PoseStamped& goal) { - double x = goal.pose.position.x; - double y = goal.pose.position.y; - double z = goal.pose.position.z; - - tf2::Quaternion q_goal; - tf2::fromMsg(goal.pose.orientation, q_goal); - - tf2::Matrix3x3 m_goal(q_goal); - double roll_goal, pitch_goal, yaw_goal; - m_goal.getRPY(roll_goal, pitch_goal, yaw_goal); - - Vector6d r; - r << x, y, z, roll_goal, pitch_goal, yaw_goal; - - return r; -} - -vortex_msgs::msg::ReferenceFilter ReferenceFilterNode::fill_reference_msg() { - vortex_msgs::msg::ReferenceFilter feedback_msg; - feedback_msg.x = x_(0); - feedback_msg.y = x_(1); - feedback_msg.z = x_(2); - feedback_msg.roll = ssa(x_(3)); - feedback_msg.pitch = ssa(x_(4)); - feedback_msg.yaw = ssa(x_(5)); - feedback_msg.x_dot = x_(6); - feedback_msg.y_dot = x_(7); - feedback_msg.z_dot = x_(8); - feedback_msg.roll_dot = x_(9); - feedback_msg.pitch_dot = x_(10); - feedback_msg.yaw_dot = x_(11); - - return feedback_msg; -} - -void ReferenceFilterNode::execute( - const std::shared_ptr> goal_handle) { - { - std::lock_guard lock(mutex_); - this->goal_handle_ = goal_handle; - } - - spdlog::info("Executing goal"); - - x_ = fill_reference_state(); - - const geometry_msgs::msg::PoseStamped goal = goal_handle->get_goal()->goal; - - r_ = fill_reference_goal(goal); - - auto feedback = std::make_shared< - vortex_msgs::action::ReferenceFilterWaypoint::Feedback>(); - auto result = std::make_shared< - vortex_msgs::action::ReferenceFilterWaypoint::Result>(); - - rclcpp::Rate loop_rate(1000.0 / time_step_.count()); - - while (rclcpp::ok()) { - { - std::lock_guard lock(mutex_); - if (goal_handle->get_goal_id() == preempted_goal_id_) { - result->success = false; - goal_handle->abort(result); - return; - } - } - if (goal_handle->is_canceling()) { - result->success = false; - goal_handle->canceled(result); - spdlog::info("Goal canceled"); - return; - } - Vector18d x_dot = reference_filter_.calculate_x_dot(x_, r_); - x_ += x_dot * time_step_.count() / 1000.0; - - vortex_msgs::msg::ReferenceFilter feedback_msg = fill_reference_msg(); - - feedback->feedback = feedback_msg; - - goal_handle->publish_feedback(feedback); - reference_pub_->publish(feedback_msg); - - if ((x_.head(6) - r_.head(6)).norm() < 0.1) { - result->success = true; - goal_handle->succeed(result); - x_.head(6) = r_.head(6); - vortex_msgs::msg::ReferenceFilter feedback_msg = - fill_reference_msg(); - reference_pub_->publish(feedback_msg); - spdlog::info("Goal reached"); - return; - } - - loop_rate.sleep(); - } -} - -RCLCPP_COMPONENTS_REGISTER_NODE(ReferenceFilterNode) diff --git a/guidance/reference_filter_dp/src/reference_filter_utils.cpp b/guidance/reference_filter_dp/src/reference_filter_utils.cpp deleted file mode 100644 index d7a5be273..000000000 --- a/guidance/reference_filter_dp/src/reference_filter_utils.cpp +++ /dev/null @@ -1,55 +0,0 @@ -#include - -Matrix3d calculate_R(const Vector6d& eta) { - const double roll = eta(3); - const double pitch = eta(4); - const double yaw = eta(5); - - const Eigen::AngleAxisd roll_angle(roll, Eigen::Vector3d::UnitX()); - const Eigen::AngleAxisd pitch_angle(pitch, Eigen::Vector3d::UnitY()); - const Eigen::AngleAxisd yaw_angle(yaw, Eigen::Vector3d::UnitZ()); - - const Eigen::Quaterniond q = yaw_angle * pitch_angle * roll_angle; - - return q.toRotationMatrix(); -} - -Matrix3d calculate_T(const Vector6d& eta) { - Matrix3d T; - double phi = eta(3); - double theta = eta(4); - - double cphi = cos(phi); - double sphi = sin(phi); - double ctheta = cos(theta); - double stheta = sin(theta); - - // Manually added the transformation matrix from Fossen 2021 p.29 eq: 2.41 - T(0, 0) = 1; - T(0, 1) = sphi * stheta / ctheta; - T(0, 2) = cphi * stheta / ctheta; - T(1, 0) = 0; - T(1, 1) = cphi; - T(1, 2) = -sphi; - T(2, 0) = 0; - T(2, 1) = sphi / ctheta; - T(2, 2) = cphi / ctheta; - - return T; -} - -Matrix6d calculate_J(const Vector6d& eta) { - Matrix3d R = calculate_R(eta); - Matrix3d T = calculate_T(eta); - - Matrix6d J = Matrix6d::Zero(); - J.block<3, 3>(0, 0) = R; - J.block<3, 3>(3, 3) = T; - - return J; -} - -double ssa(double angle) { - double angle_ssa = fmod(angle + M_PI, 2 * M_PI) - M_PI; - return angle_ssa; -} From c1ae0acf4332f9d261ed40cd217ac6e4448279e8 Mon Sep 17 00:00:00 2001 From: Henrik Date: Wed, 1 Apr 2026 21:38:43 +0200 Subject: [PATCH 125/128] first draft of fixing PR comments, and added control manager and generic controller type --- .gitignore | 4 +- .ignore | 0 auv_setup/config/robots/orca.yaml | 46 +-- control/velocity_controller/CMakeLists.txt | 8 +- control/velocity_controller/README.md | 6 +- .../config/nautilus_params.yaml | 11 +- .../config/orca_params.yaml | 8 +- .../include/tests/test_VC.hpp | 44 ++- .../include/velocity_controller/PID_setup.hpp | 30 -- .../velocity_controller/control_manager.hpp | 29 ++ .../velocity_controller/lib/3DOF_PID.hpp | 27 ++ .../{ => lib}/LQR_setup.hpp | 37 ++- .../lib/PID_controller.hpp | 38 +++ .../velocity_controller/lib/controller.hpp | 20 ++ .../include/velocity_controller/utilities.hpp | 7 +- ...roller.hpp => velocity_controller_ros.hpp} | 87 +++-- .../launch/VCnTest.launch.py | 4 +- .../launch/velocity_controller.launch.py | 38 ++- control/velocity_controller/package.xml | 8 +- control/velocity_controller/src/3DOF_PID.cpp | 39 +++ control/velocity_controller/src/LQR_setup.cpp | 102 +++--- .../src/PID_controller.cpp | 74 +++++ control/velocity_controller/src/PID_setup.cpp | 87 ----- .../src/control_manager.cpp | 74 +++++ .../src/velocity_controller.cpp | 299 ------------------ .../src/velocity_controller_ros.cpp | 223 +++++++++++++ .../velocity_controller/tests/CMakeLists.txt | 14 +- .../velocity_controller/tests/test_LQR.cpp | 7 +- .../velocity_controller/tests/test_PID.cpp | 37 +-- control/velocity_controller/tests/test_VC.cpp | 19 +- 30 files changed, 759 insertions(+), 668 deletions(-) delete mode 100644 .ignore delete mode 100644 control/velocity_controller/include/velocity_controller/PID_setup.hpp create mode 100644 control/velocity_controller/include/velocity_controller/control_manager.hpp create mode 100644 control/velocity_controller/include/velocity_controller/lib/3DOF_PID.hpp rename control/velocity_controller/include/velocity_controller/{ => lib}/LQR_setup.hpp (59%) create mode 100644 control/velocity_controller/include/velocity_controller/lib/PID_controller.hpp create mode 100644 control/velocity_controller/include/velocity_controller/lib/controller.hpp rename control/velocity_controller/include/velocity_controller/{velocity_controller.hpp => velocity_controller_ros.hpp} (68%) create mode 100644 control/velocity_controller/src/3DOF_PID.cpp create mode 100644 control/velocity_controller/src/PID_controller.cpp delete mode 100644 control/velocity_controller/src/PID_setup.cpp create mode 100644 control/velocity_controller/src/control_manager.cpp delete mode 100644 control/velocity_controller/src/velocity_controller.cpp create mode 100644 control/velocity_controller/src/velocity_controller_ros.cpp diff --git a/.gitignore b/.gitignore index c5f5f029b..41eed4018 100644 --- a/.gitignore +++ b/.gitignore @@ -21,6 +21,7 @@ msg/*Result.msg msg/_*.py build_isolated/ devel_isolated/ +*.cache/ # Generated by dynamic reconfigure *.cfgc @@ -46,6 +47,7 @@ srv/_*.py *.pyc qtcreator-* *.user +*hintmux.sh /planning/cfg /planning/docs @@ -55,5 +57,5 @@ qtcreator-* # Emacs .#* -.cache + # End of https://www.gitignore.io/api/ros diff --git a/.ignore b/.ignore deleted file mode 100644 index e69de29bb..000000000 diff --git a/auv_setup/config/robots/orca.yaml b/auv_setup/config/robots/orca.yaml index bff7c14be..47039a4b9 100644 --- a/auv_setup/config/robots/orca.yaml +++ b/auv_setup/config/robots/orca.yaml @@ -17,9 +17,8 @@ /**: ros__parameters: physical: - center_of_mass: [0.0, 0.0, 0.035] # Position (x,y,z) in meters (M) + center_of_mass: [0.0, 0.0, 0.035] # m (x,y,z) mass_matrix : [ 30.0, 0.0, 0.0, 0.0, 0.0, 0.6, 0.0, 30.0, 0.0, 0.0, -0.6, 0.3, 0.0, 0.0, 30.0, 0.6, 0.3, 0.0, 0.0, 0.0, 0.6, 0.68, 0.0, 0.0, 0.0, -0.6, 0.3, 0.0, 3.32, 0.0, 0.6, 0.3, 0.0, 0.0, 0.0, 3.34] - # 6x6 mass_inertia_matrix propulsion: dofs: @@ -28,7 +27,9 @@ num: 3 thrusters: num: 8 - thruster_force_direction: [ # Direction of forces X,Y,Z, same logic as thruster_position thruster0 produces thrust in (X0,Y0,Z0) and ||(X0,Y0,Z0)|| = 1 + min: -100 + max: 100 + thruster_force_direction: [ 0.70711, 0.00000, 0.00000, @@ -52,9 +53,9 @@ 0.00000, 1.00000, 1.00000, - 0.00000, # Heave - ] - thruster_position: [ # Position (x0,x1 ... x7,y1,y2, ...,y7,z1,z2, ... ,z7) in meters (M). i.e thruster0 has position (x0,y0,z0) + 0.00000, + ] # Heave + thruster_position: [ 0.41500, 0.28400, -0.31800, @@ -78,14 +79,8 @@ 0.07600, 0.08200, 0.08200, - 0.07600, # z-position of thrusters - ] - - constraints: - max_force: 100.0 - min_force: -100.0 - input_matrix_weights: [1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0] - slack_matrix_weights: [2000.0,2000.0,2000.0,2000.0,2000.0,2000.0] + 0.07600, + ] # z-position of thrusters rate_of_change: max: 1 # Maximum rate of change in newton per second for a thruster @@ -126,39 +121,20 @@ joy: "joy" pose: "pose" twist: "twist" - odom: "odom" operation_mode: "operation_mode" killswitch: "killswitch" - reference_pose: "reference_pose" - landmarks: "landmarks" + aruco_board_pose_camera: "aruco_board_pose_camera" waypoint: "waypoint" + waypoint_list: "waypoint_list" guidance: los: "guidance/los" dp: "guidance/dp" - dp_quat: "guidance/dp_quat" fsm: active_controller: "fsm/active_controller" - dvl_twist: "dvl/twist" - dvl_altitude: "dvl/altitude" - imu: "imu/data_raw" - pressure_sensor: "pressure_sensor" - gripper_servos: "gripper_servos" action_servers: reference_filter: "reference_filter" los: "los_guidance" - landmark_polling: "landmark_polling" - landmark_convergence: "landmark_convergence" - waypoint_manager: "waypoint_manager" - - services: - set_operation_mode: "set_operation_mode" - set_killswitch: "set_killswitch" - toggle_killswitch: "toggle_killswitch" - get_operation_mode: "get_operation_mode" - los_mode: "set_los_mode" - waypoint_addition: "waypoint_addition" - start_mission: "start_mission" fsm: docking: diff --git a/control/velocity_controller/CMakeLists.txt b/control/velocity_controller/CMakeLists.txt index cb691bf72..4184c36ff 100644 --- a/control/velocity_controller/CMakeLists.txt +++ b/control/velocity_controller/CMakeLists.txt @@ -31,12 +31,15 @@ include_directories( set(LIB_NAME velocity_controller_component) add_library(${LIB_NAME} SHARED - src/velocity_controller.cpp - src/PID_setup.cpp + src/velocity_controller_ros.cpp + src/PID_controller.cpp src/LQR_setup.cpp src/utilities.cpp src/ct_instantiations.cpp + src/control_manager.cpp + src/3DOF_PID.cpp ) +rclcpp_components_register_nodes(${LIB_NAME} "Velocity_node") ament_target_dependencies(${LIB_NAME} rclcpp rclcpp_components @@ -66,6 +69,7 @@ install(DIRECTORY config DESTINATION share/${PROJECT_NAME}/ ) + if(BUILD_TESTING) add_subdirectory(tests) endif() diff --git a/control/velocity_controller/README.md b/control/velocity_controller/README.md index c659bcdcb..214dc58fe 100644 --- a/control/velocity_controller/README.md +++ b/control/velocity_controller/README.md @@ -13,7 +13,7 @@ The LQR controller linearizes the vehicle dynamics around the current state at e --- ## Dependencies - +//TODO(henrimha): fix the dependencies | Dependency | Purpose | |---|---| | `rclcpp` / `rclcpp_lifecycle` | ROS2 node and lifecycle management | @@ -52,7 +52,7 @@ All parameters are loaded in the constructor via `get_new_parameters()`. ### LQR parameters -| Parameter | Type | Size | Description | +| Parameter | Type | Dimension | Description | |---|---|---|---| | `Q` | `double[]` | 8 | Diagonal of state weight matrix. States: `[surge_err, pitch_err, yaw_err, pitch_rate_err, yaw_rate_err, ∫surge, ∫pitch, ∫yaw]` | | `R` | `double[]` | 3 | Diagonal of input weight matrix. Inputs: `[Fx, Ty, Tz]` | @@ -132,7 +132,7 @@ The gain `K` is computed by solving the continuous-time algebraic Riccati equati ``` u = K * x_error ``` - +//TODO(henrimha): give the riccatti equation where `ct::optcon` produces `K` such that this is equivalent to `u = -K * (x - x_ref)`. If the Riccati solver fails (e.g. due to an unstabilizable operating point), the node automatically falls back to PID and logs an error. diff --git a/control/velocity_controller/config/nautilus_params.yaml b/control/velocity_controller/config/nautilus_params.yaml index 88d0855a7..1cddf0706 100644 --- a/control/velocity_controller/config/nautilus_params.yaml +++ b/control/velocity_controller/config/nautilus_params.yaml @@ -9,15 +9,18 @@ LQR_params: Q: [200.0,32.84,32.84,15.0,15.0,100.0,32.84,32.84] R: [0.02,3.1,3.10] + #TODO(henrimha): move these to the global parameter file dampening_matrix_low: [23.0,0.0,0.0,0.0,0.0,0.0, 0.0,46.0,0.0,0.0,0.0,0.0, 0.0,0.0,46.0,0.0,0.0,0.0, 0.0,0.0,0.0,46.0,0.0,0.0, 0.0,0.0,0.0,0.0,46.0,0.0, 0.0,0.0,0.0,0.0,0.0,46.0] dampening_matrix_high: [1.0,0.0,0.0,0.0,0.0,0.0, 0.0,1.0,0.0,0.0,0.0,0.0, 0.0,0.0,1.0,0.0,0.0,0.0, 0.0,0.0,0.0,1.0,0.0,0.0, 0.0,0.0,0.0,0.0,1.0,0.0, 0.0,0.0,0.0,0.0,0.0,1.0] - Settings: #Settings for the controller - controller_type: 2 #1 PID 2 LQR - auto_start: false #0 for no, 1 for yes - anti_overshoot: false + Node_settings: #Settings for the controller + auto_start: true #0 for no, 1 for yes reset_on_new_ref: true odometry_dropout_guard: true + Control_manager_settings: + anti_overshoot: false publish_rate: 100 #ms max_force: 99.5 + controller_type: 2 #1 PID 2 LQR + diff --git a/control/velocity_controller/config/orca_params.yaml b/control/velocity_controller/config/orca_params.yaml index 88d0855a7..8085fc720 100644 --- a/control/velocity_controller/config/orca_params.yaml +++ b/control/velocity_controller/config/orca_params.yaml @@ -9,15 +9,17 @@ LQR_params: Q: [200.0,32.84,32.84,15.0,15.0,100.0,32.84,32.84] R: [0.02,3.1,3.10] + #TODO(henrimha): move these to the global parameter file dampening_matrix_low: [23.0,0.0,0.0,0.0,0.0,0.0, 0.0,46.0,0.0,0.0,0.0,0.0, 0.0,0.0,46.0,0.0,0.0,0.0, 0.0,0.0,0.0,46.0,0.0,0.0, 0.0,0.0,0.0,0.0,46.0,0.0, 0.0,0.0,0.0,0.0,0.0,46.0] dampening_matrix_high: [1.0,0.0,0.0,0.0,0.0,0.0, 0.0,1.0,0.0,0.0,0.0,0.0, 0.0,0.0,1.0,0.0,0.0,0.0, 0.0,0.0,0.0,1.0,0.0,0.0, 0.0,0.0,0.0,0.0,1.0,0.0, 0.0,0.0,0.0,0.0,0.0,1.0] - Settings: #Settings for the controller - controller_type: 2 #1 PID 2 LQR + Node_settings: #Settings for the controller auto_start: false #0 for no, 1 for yes - anti_overshoot: false reset_on_new_ref: true odometry_dropout_guard: true + Control_manager_settings: + anti_overshoot: false publish_rate: 100 #ms max_force: 99.5 + controller_type: 2 #1 PID 2 LQR diff --git a/control/velocity_controller/include/tests/test_VC.hpp b/control/velocity_controller/include/tests/test_VC.hpp index fd9a2ecce..a757daa03 100644 --- a/control/velocity_controller/include/tests/test_VC.hpp +++ b/control/velocity_controller/include/tests/test_VC.hpp @@ -1,4 +1,5 @@ -#pragma once +#ifndef TEST_VC_HPP_ +#define TEST_VC_HPP_ #include #include @@ -12,18 +13,29 @@ #include "velocity_controller/utilities.hpp" #include "vortex_msgs/msg/los_guidance.hpp" -class test_VC : public rclcpp::Node { - public: - test_VC(); - // Callback functions - void send_guidance(); +/** + @brief A class that sends a reference signal to the velocity controller +*/ +class test_velocity_controller : public rclcpp::Node { + public: + explicit test_velocity_controller(); + test_velocity_controller(const test_velocity_controller&) = delete; // no copy constructor + test_velocity_controller& operator=(const test_velocity_controller&) = delete; // no copy assignment + test_velocity_controller(test_velocity_controller&&) = delete; // no move constructor + test_velocity_controller& operator=(test_velocity_controller&&) = delete; // no move assignment + ~test_velocity_controller()=default; + private: + /** + * @brief Publishes a reference signal to the reference topic of the velocity controller. + */ + void send_reference(); + /** + * @brief Subscribes to the odometry topic and prints the current state (in euler angles) of the vehicle for debugging. + */ void odometry_callback(const nav_msgs::msg::Odometry::SharedPtr msg_ptr); - // Variables - // Subscribers and publishers - rclcpp::Publisher::SharedPtr - publisher_guidance; + rclcpp::Publisher::SharedPtr publisher_guidance; rclcpp::Publisher::SharedPtr publisher_state; rclcpp::Subscription::SharedPtr subscription_state; // Timers @@ -31,16 +43,16 @@ class test_VC : public rclcpp::Node { rclcpp::Clock::SharedPtr clock_; // Messages vortex_msgs::msg::LOSGuidance reference_msg; - // Topics std::string topic_guidance; std::string topic_state = "/state"; std::string topic_odometry; + + /** + * @brief The total time elapsed since the start of the simulation. Used to calculate the reference signal as a function of time. + */ - // MSGS - double time1 = 0; + double totaltime = 0; }; -geometry_msgs::msg::Quaternion euler_angle_to_quaternion(double roll, - double pitch, - double yaw); +#endif // TEST_VC_HPP_ \ No newline at end of file diff --git a/control/velocity_controller/include/velocity_controller/PID_setup.hpp b/control/velocity_controller/include/velocity_controller/PID_setup.hpp deleted file mode 100644 index 4f4166ae7..000000000 --- a/control/velocity_controller/include/velocity_controller/PID_setup.hpp +++ /dev/null @@ -1,30 +0,0 @@ -#pragma once -#include -#include -#include -#include -#include - -class PID_controller { - public: - // PID_controller(double Kp=0, double Ki=0, double Kd=0, double - // max_output=0, double min_output=0, double dt=0); PID_controller(double - // k_p, double k_i, double k_d) : PID_controller(k_p, k_i, k_d, 100.0, - // -100.0) {}; - bool calculate_thrust(double error); - bool calculate_thrust(double error, double error_d); - void reset_controller(); - double get_output(); - bool set_output_limits(double min_output, double max_output); - bool set_parameters(double k_p, double k_i, double k_d, double dt); - bool set_parameters(std::vector& params, double dt); - bool set_dt(double dt); - - private: - double Kp_, Ki_, Kd_, dt_; - double integral = 0; - double previous_error = 0; - double output = 0; - double max_output_, min_output_; - bool init = false; -}; diff --git a/control/velocity_controller/include/velocity_controller/control_manager.hpp b/control/velocity_controller/include/velocity_controller/control_manager.hpp new file mode 100644 index 000000000..25b757d0a --- /dev/null +++ b/control/velocity_controller/include/velocity_controller/control_manager.hpp @@ -0,0 +1,29 @@ +#ifndef VELOCITY_CONTROLLER__CONTROL_MANAGER_HPP_ +#define VELOCITY_CONTROLLER__CONTROL_MANAGER_HPP_ +#include "velocity_controller/lib/controller.hpp" +#include "velocity_controller/utilities.hpp" +#include "velocity_controller/lib/3DOF_PID.hpp" +#include "velocity_controller/lib/LQR_setup.hpp" +struct control_manager_params{ + int control_type; // 1 3DOF PID, 2 3DOF LQR + bool anti_overshoot; + bool fallback; +}; +class control_manager{ + public: + control_manager(control_manager_params params); + bool switch_controller(); + void shutdown_controller(); + geometry_msgs::msg::WrenchStamped get_output(Guidance_data guidanceState, State State); + void initialize_3DOF_controller(PID_3DOF_params params); + void initialize_LQR_controller(LQR_params params); + bool get_validity(); + void reset_controllers(int nr = 0); + private: + control_manager_params params_; + std::unique_ptr controller_3DOF=nullptr; + std::unique_ptr controller_LQR=nullptr; + + +}; +#endif // VELOCITY_CONTROLLER__CONTROL_MANAGER_HPP_ \ No newline at end of file diff --git a/control/velocity_controller/include/velocity_controller/lib/3DOF_PID.hpp b/control/velocity_controller/include/velocity_controller/lib/3DOF_PID.hpp new file mode 100644 index 000000000..8da74b266 --- /dev/null +++ b/control/velocity_controller/include/velocity_controller/lib/3DOF_PID.hpp @@ -0,0 +1,27 @@ +#ifndef VELOCITY_CONTROLLER__3DOF_PID_HPP_ +#define VELOCITY_CONTROLLER__3DOF_PID_HPP_ + +#include "velocity_controller/lib/controller.hpp" +#include "velocity_controller/utilities.hpp" +#include +#include "velocity_controller/lib/PID_controller.hpp" +struct PID_3DOF_params{ + PID_params surge_params; + PID_params pitch_params; + PID_params yaw_params; +}; +class PID_3DOF : public controller { + public: + PID_3DOF(PID_3DOF_params params); + geometry_msgs::msg::WrenchStamped calculate_thrust(State state, State error_state) override; + void reset_controller(int nr=0) override; + ///bool set_parameters(PID_params surge_params, PID_params pitch_params, PID_params yaw_params); + + private: + PID_controller surge_controller; + PID_controller pitch_controller; + PID_controller yaw_controller; +}; + + +#endif // VELOCITY_CONTROLLER__3DOF_PID_HPP_ \ No newline at end of file diff --git a/control/velocity_controller/include/velocity_controller/LQR_setup.hpp b/control/velocity_controller/include/velocity_controller/lib/LQR_setup.hpp similarity index 59% rename from control/velocity_controller/include/velocity_controller/LQR_setup.hpp rename to control/velocity_controller/include/velocity_controller/lib/LQR_setup.hpp index cdc8491ce..4a509dbf2 100644 --- a/control/velocity_controller/include/velocity_controller/LQR_setup.hpp +++ b/control/velocity_controller/include/velocity_controller/lib/LQR_setup.hpp @@ -1,24 +1,29 @@ #pragma once #include +#include #include #include #include #include #include +#include "velocity_controller/lib/controller.hpp" #include "velocity_controller/utilities.hpp" - -class LQRController { +//TODO(henrimha): Make the constructor take in a LQR_params struct and make a LQR_params struct in the header file +//TODO(henrimha): figure out how to hold the matrices in the class, and what to take in as parameters +struct LQR_params{ + std::vector Q; + std::vector R; + std::vector inertia_matrix; + double max_force; + std::vector D_low; + std::vector D_high; + double interval; +}; +class LQRController : public controller{ public: - LQRController(); - bool set_matrices(std::vector Q_, - std::vector R_, - std::vector inertia_matrix, - double max_force, - std::vector D_low); - void reset_controller(int nr = 0); - bool calculate_thrust(State states, Guidance_data guidance_values); - bool set_interval(double interval); - Eigen::Vector get_thrust(); + LQRController(LQR_params params); + void reset_controller(int nr = 0) override; + geometry_msgs::msg::WrenchStamped calculate_thrust(State state, State error_state) override; private: Eigen::Matrix linearize(State states); @@ -30,10 +35,9 @@ class LQRController { double anti_windup(double error, double integral_sum, bool windup); Eigen::Vector saturate_input(Eigen::Vector u); - Eigen::Vector update_error(const Guidance_data& error, + Eigen::Vector update_error(const State& error_state, const State& state); - - double interval_; + LQR_params params_; double integral_error_surge; double integral_error_pitch; double integral_error_yaw; @@ -44,12 +48,11 @@ class LQRController { Eigen::Matrix R; Eigen::Matrix B; Eigen::Matrix D; - double max_force, mass, Ixx, Iyy, Izz; + double mass, Ixx, Iyy, Izz; Eigen::Matrix inertia_matrix_inv; Eigen::Matrix state_weight_matrix; Eigen::Matrix3d input_weight_matrix; Eigen::Matrix augmented_system_matrix; Eigen::Matrix augmented_input_matrix; - Eigen::Vector u; }; diff --git a/control/velocity_controller/include/velocity_controller/lib/PID_controller.hpp b/control/velocity_controller/include/velocity_controller/lib/PID_controller.hpp new file mode 100644 index 000000000..e5b6cf81a --- /dev/null +++ b/control/velocity_controller/include/velocity_controller/lib/PID_controller.hpp @@ -0,0 +1,38 @@ +#pragma once +#include +#include +#include +#include +#include +//TODO(henrimha): Make the constructor take in a PID_params struct and make a PID_params struct in the header file +struct PID_params{ + double k_p, k_i, k_d; + double dt; + double max_output; + double min_output; + const std::vector& operator=(const std::vector& params); + PID_params() = default; + +}; + +class PID_controller { + public: + explicit PID_controller(PID_params params); + /** @brief Calculates the thrust based on the error and internal parameters, with a default derivative of 0*/ + /** @brief Calculates the thrust based on the error and internal parameters*/ + double calculate_thrust(double error); + /** @brief Calculates the thrust based on the error with external derivative */ + double calculate_thrust(double error, double error_d); + /** @brief Resets the all internal states, inlcuding integral, output and previous error */ + void reset_controller(); + /** @brief Returns the validity of the controller */ + bool get_validity(){return valid;}; + + private: + /** @brief internal variables for calculation */ + PID_params params_; + double integral = 0; + double previous_error = 0; + /** @brief Indicates if the controller is initialized properly*/ + bool valid = false; +}; diff --git a/control/velocity_controller/include/velocity_controller/lib/controller.hpp b/control/velocity_controller/include/velocity_controller/lib/controller.hpp new file mode 100644 index 000000000..a9388b6d5 --- /dev/null +++ b/control/velocity_controller/include/velocity_controller/lib/controller.hpp @@ -0,0 +1,20 @@ +#ifndef VELOCITY_CONTROLLER__CONTROLLER_HPP_ +#define VELOCITY_CONTROLLER__CONTROLLER_HPP_ +#include +#include "velocity_controller/utilities.hpp" +#include "geometry_msgs/msg/wrench_stamped.hpp" + +class controller{ + public: + controller()=default; + virtual geometry_msgs::msg::WrenchStamped calculate_thrust(State state, State error_state) = 0; + virtual void reset_controller(int nr=0) = 0; + bool get_validity(){return valid;}; + + protected: + bool valid=false; +}; + + + +#endif // VELOCITY_CONTROLLER__CONTROLLER_HPP_ \ No newline at end of file diff --git a/control/velocity_controller/include/velocity_controller/utilities.hpp b/control/velocity_controller/include/velocity_controller/utilities.hpp index 9f6f8e1d5..9060bc5da 100644 --- a/control/velocity_controller/include/velocity_controller/utilities.hpp +++ b/control/velocity_controller/include/velocity_controller/utilities.hpp @@ -1,4 +1,5 @@ -#pragma once +#ifndef UTILITIES_HPP_ +#define UTILITIES_HPP_ #include #include #include @@ -13,9 +14,6 @@ struct angle { double psit = 0.0; }; angle quaternion_to_euler_angle(double w, double x, double y, double z); -geometry_msgs::msg::Quaternion euler_angle_to_quaternion(double roll, - double pitch, - double yaw); class State { public: @@ -66,3 +64,4 @@ inline angle angle_NED_to_body(angle desired, angle state) { return angle_NED_to_body(desired.phit, desired.thetat, desired.psit, state.phit, state.thetat, state.psit); } +#endif // UTILITIES_HPP_ \ No newline at end of file diff --git a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp b/control/velocity_controller/include/velocity_controller/velocity_controller_ros.hpp similarity index 68% rename from control/velocity_controller/include/velocity_controller/velocity_controller.hpp rename to control/velocity_controller/include/velocity_controller/velocity_controller_ros.hpp index 4d636d699..3ae26300c 100644 --- a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp +++ b/control/velocity_controller/include/velocity_controller/velocity_controller_ros.hpp @@ -1,35 +1,48 @@ #ifndef VELOCITY_CONTROLLER__VELOCITY_CONTROLLER_HPP_ #define VELOCITY_CONTROLLER__VELOCITY_CONTROLLER_HPP_ -#include -#include +//#include +//#include #include #include #include -#include -#include -#include +//#include +//#include +//#include #include -#include "LQR_setup.hpp" -// #include "nav_msgs/msg/odometry.hpp" -#include "velocity_controller/PID_setup.hpp" -// #include "vortex_msgs/msg/los_guidance.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "vortex_msgs/msg/los_guidance.hpp" #include -#endif // VELOCITY_CONTROLLER__VELOCITY_CONTROLLER_HPP_ +#include "velocity_controller/control_manager.hpp" + +struct Node_settings{ + //special settings + bool auto_start; + bool reset_on_new_ref; + bool odometry_dropout_guard; + + // Variables for timers + int publish_rate=100; //ms + // Variables for topics + std::string topic_thrust; + std::string topic_guidance; + std::string topic_killswitch; + std::string topic_odometry; +}; class Velocity_node : public rclcpp_lifecycle::LifecycleNode { public: explicit Velocity_node(const rclcpp::NodeOptions& options); Velocity_node(const Velocity_node&) = delete; // no copy constructor - Velocity_node& operator=(const Velocity_node&) = - delete; // no copy assignment + Velocity_node& operator=(const Velocity_node&) =delete; // no copy assignment Velocity_node(Velocity_node&&) = delete; // no move constructor Velocity_node& operator=(Velocity_node&&) = delete; // no move assignment private: void get_new_parameters(); + void initialize_controllers(); // Timer functions - void calc_thrust(); + void publish_thrust(); // Callback functions void guidance_callback( @@ -49,50 +62,21 @@ class Velocity_node : public rclcpp_lifecycle::LifecycleNode { rclcpp::Subscription::SharedPtr subscriber_guidance; - // Variables for topics - std::string topic_thrust; - std::string topic_guidance; - std::string topic_killswitch; - std::string topic_odometry; - - // Variables for timers - int publish_rate; - double max_force; - + + Node_settings node_settings; + // Control manager instance + std::unique_ptr control_manager_ptr; // Stored wrenches values vortex_msgs::msg::LOSGuidance reference_in; + geometry_msgs::msg::WrenchStamped thrust_out; Guidance_data guidance_values; State current_state; - geometry_msgs::msg::WrenchStamped thrust_out; - - // PID controllers - PID_controller PID_surge; - std::vector surge_params; - PID_controller PID_yaw; - std::vector yaw_params; - PID_controller PID_pitch; - std::vector pitch_params; - - // LQR Controller - LQRController lqr_controller; - // LQRparameters lqr_parameters; - std::vector Q; - std::vector R; - // std::vector Qi; - // std::vector Ri; - std::vector inertia_matrix; - std::vector dampening_matrix_low; - std::vector dampening_matrix_high; - + std::atomic_bool should_exit_{false}; - // VC settings - bool reset_on_new_ref; - bool anti_overshoot; - bool auto_start; - bool odometry_dropout_guard; + //bool anti_overshoot; int publish_counter = 0; bool first_start = true; - int controller_type; // 1 PID, 2 LQR + //int controller_type; // 1 PID, 2 LQR // States rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn @@ -106,7 +90,8 @@ class Velocity_node : public rclcpp_lifecycle::LifecycleNode { rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_shutdown(const rclcpp_lifecycle::State& state) override; - void reset_controllers(int nr = 0); + //void reset_controllers(int nr = 0); rclcpp::QoS pub_QoS; rclcpp::QoS sub_QoS; }; +#endif // VELOCITY_CONTROLLER__VELOCITY_CONTROLLER_HPP_ diff --git a/control/velocity_controller/launch/VCnTest.launch.py b/control/velocity_controller/launch/VCnTest.launch.py index 47ab29d48..efda55f7f 100644 --- a/control/velocity_controller/launch/VCnTest.launch.py +++ b/control/velocity_controller/launch/VCnTest.launch.py @@ -30,11 +30,13 @@ def launch_setup(context, *args, **kwargs): os.path.join(stonefish_dir, 'launch', 'simulation.launch.py') ), launch_arguments={ - 'scenario': 'nautilus_no_gpu.scn', + "drone": drone, + 'scenario': 'nautilus_no_gpu', 'rendering_quality': 'low', 'rendering': 'true', }.items(), ) + drone_sim = TimerAction( period=12.0, actions=[ diff --git a/control/velocity_controller/launch/velocity_controller.launch.py b/control/velocity_controller/launch/velocity_controller.launch.py index 666cfe414..50c7aba59 100644 --- a/control/velocity_controller/launch/velocity_controller.launch.py +++ b/control/velocity_controller/launch/velocity_controller.launch.py @@ -12,7 +12,8 @@ declare_drone_and_namespace_args, resolve_drone_and_namespace, ) - +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode def launch_setup(context, *args, **kwargs): drone, namespace = resolve_drone_and_namespace(context) @@ -22,16 +23,33 @@ def launch_setup(context, *args, **kwargs): config_path_local = os.path.join(pkg_share, 'config', f'{drone}_params.yaml') config_path_global = os.path.join(global_share, 'config', 'robots', f"{drone}.yaml") + #return [ + # Node( + # package='velocity_controller', + # executable='velocity_node', + # name="velocity_controller_node", + # namespace=namespace, + # parameters=[config_path_local, config_path_global], + # ) + #] return [ - Node( - package='velocity_controller', - executable='velocity_controller_node', - name="velocity_controller_node", - namespace=namespace, - output='screen', - parameters=[config_path_local, config_path_global], - ) - ] + ComposableNodeContainer( + name='velocity_container', + namespace=namespace, + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[ + ComposableNode( + package='velocity_controller', + plugin='Velocity_node', # must match the class name registered + name='velocity_controller_node', + namespace=namespace, + parameters=[config_path_local, config_path_global], + ), + ], + output='screen', + ) +] def generate_launch_description(): diff --git a/control/velocity_controller/package.xml b/control/velocity_controller/package.xml index c467e56c6..3eea35266 100644 --- a/control/velocity_controller/package.xml +++ b/control/velocity_controller/package.xml @@ -5,7 +5,7 @@ 0.0.0 TODO: Package description henrik - TODO: License declaration + TODO: License declaration ament_cmake @@ -19,6 +19,8 @@ rclcpp_components lifecycle_msgs auv_setup + ct_optcon + ct_core ament_cmake_gtest ament_lint_auto @@ -28,3 +30,7 @@ ament_cmake + + + + diff --git a/control/velocity_controller/src/3DOF_PID.cpp b/control/velocity_controller/src/3DOF_PID.cpp new file mode 100644 index 000000000..6263afefc --- /dev/null +++ b/control/velocity_controller/src/3DOF_PID.cpp @@ -0,0 +1,39 @@ +#include "velocity_controller/lib/3DOF_PID.hpp" +//#include "velocity_controller/lib/PID_controller.hpp" +#include + +PID_3DOF::PID_3DOF(PID_3DOF_params params) + : surge_controller(params.surge_params), pitch_controller(params.pitch_params), yaw_controller(params.yaw_params) {}; + +geometry_msgs::msg::WrenchStamped PID_3DOF::calculate_thrust(State state, State error_state) { + geometry_msgs::msg::WrenchStamped u; + u.wrench.force.set__x(surge_controller.calculate_thrust(error_state.surge)); + u.wrench.torque.set__y(pitch_controller.calculate_thrust(error_state.pitch,error_state.yaw_rate)); + u.wrench.torque.set__z(yaw_controller.calculate_thrust(error_state.yaw, error_state.yaw_rate)); + if(surge_controller.get_validity() && pitch_controller.get_validity() && yaw_controller.get_validity()){ + valid = true; + } else { + valid = false; + } + return u; +} + +void PID_3DOF::reset_controller(int nr){ + switch (nr){ + case 0: + surge_controller.reset_controller(); + pitch_controller.reset_controller(); + yaw_controller.reset_controller(); + break; + case 1: + surge_controller.reset_controller(); + break; + case 2: + pitch_controller.reset_controller(); + break; + case 3: + yaw_controller.reset_controller(); + break; + } + return; +} \ No newline at end of file diff --git a/control/velocity_controller/src/LQR_setup.cpp b/control/velocity_controller/src/LQR_setup.cpp index 5b323bd9b..5ae2ea166 100644 --- a/control/velocity_controller/src/LQR_setup.cpp +++ b/control/velocity_controller/src/LQR_setup.cpp @@ -1,5 +1,6 @@ -#include "velocity_controller/LQR_setup.hpp" +#include "velocity_controller/lib/LQR_setup.hpp" #include +#include #include #include // #include @@ -12,50 +13,49 @@ #include "velocity_controller/utilities.hpp" // #include "vortex/utils/math.hpp" -LQRController::LQRController() { - Q.setZero(); - R.setZero(); - B.setZero(); - D.setZero(); +LQRController::LQRController(LQR_params params) { + params_=params; inertia_matrix_inv.setZero(); -} -bool LQRController::set_matrices(std::vector Q_, - std::vector R_, - std::vector inertia_matrix_, - double max_force_, - std::vector D_low) { - if (Q_.size() != 8) { + if (params_.interval <= 0){ + valid = false; + return; + } + else if (params_.Q.size() != 8) { RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "The Q matrix has the wrong amount of elements"); - return 0; + valid = false; + return; } - if (R_.size() != 3) { + else if (params_.R.size() != 3) { RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "The R matrix has the wrong amount of elements"); - return 0; + valid = false; + return; } - if (inertia_matrix_.size() != 36) { + else if (params_.inertia_matrix.size() != 36) { RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "The M matrix has the wrong amount of elements"); - return 0; + valid = false; + return; + } - if (max_force_ < 0) { + else if (params_.max_force < 0) { RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "The max_force need to be >0"); - return 0; + valid = false; + return; } - max_force = max_force_; - Q.diagonal() = Eigen::Map(Q_.data(), Q_.size()); - R.diagonal() = Eigen::Map(R_.data(), R_.size()); - Ixx = inertia_matrix_.at(6 * 3 + 3); - Iyy = inertia_matrix_.at(4 * 6 + 4); - Izz = inertia_matrix_.at(5 * 6 + 5); - mass = inertia_matrix_.at(0); + Q.diagonal() = Eigen::Map(params_.Q.data(), params_.Q.size()); + R.diagonal() = Eigen::Map(params_.R.data(), params_.R.size()); + Ixx = params_.inertia_matrix.at(6 * 3 + 3); + Iyy = params_.inertia_matrix.at(4 * 6 + 4); + Izz = params_.inertia_matrix.at(5 * 6 + 5); + mass = params_.inertia_matrix.at(0); Eigen::Matrix inertia_matrix = - Eigen::Map>(inertia_matrix_.data(), 6, + Eigen::Map>(params_.inertia_matrix.data(), 6, 6); - D = Eigen::Map>(D_low.data(), 6, 6); + D = Eigen::Map>(params_.D_low.data(), 6, 6); inertia_matrix_inv = inertia_matrix.inverse(); Eigen::Matrix B_t = @@ -70,7 +70,7 @@ bool LQRController::set_matrices(std::vector Q_, } B.block<5, 3>(0, 0) = B_m.block<5, 3>(0, 0); reset_controller(); - return 1; + valid = true; } std::tuple LQRController::saturate(double value, @@ -89,7 +89,7 @@ double LQRController::anti_windup(double error, double integral_sum, bool windup) { if (!windup) { - integral_sum += error * interval_; + integral_sum += error * params_.interval; } return integral_sum; } @@ -122,12 +122,11 @@ Eigen::Matrix LQRController::linearize(State s) { return ret; } -Eigen::Vector LQRController::update_error(const Guidance_data& error, +Eigen::Vector LQRController::update_error(const State& error_state, const State& state) { - double surge_error = error.surge; - double pitch_error = error.pitch; - double yaw_error = error.yaw; - + double surge_error = error_state.surge; + double pitch_error = error_state.pitch; + double yaw_error = error_state.yaw; integral_error_surge = anti_windup(surge_error, integral_error_surge, surge_windup); integral_error_pitch = @@ -143,20 +142,25 @@ Eigen::Vector LQRController::update_error(const Guidance_data& error, Eigen::Vector LQRController::saturate_input( Eigen::Vector u) { double force_x, torque_y, torque_z; - std::tie(surge_windup, force_x) = saturate(u[0], surge_windup, max_force); - std::tie(pitch_windup, torque_y) = saturate(u[1], pitch_windup, max_force); - std::tie(yaw_windup, torque_z) = saturate(u[2], yaw_windup, max_force); + std::tie(surge_windup, force_x) = saturate(u[0], surge_windup, params_.max_force); + std::tie(pitch_windup, torque_y) = saturate(u[1], pitch_windup, params_.max_force); + std::tie(yaw_windup, torque_z) = saturate(u[2], yaw_windup, params_.max_force); return {force_x, torque_y, torque_z}; } -bool LQRController::calculate_thrust(State state, Guidance_data error) { +geometry_msgs::msg::WrenchStamped LQRController::calculate_thrust(State state, State error_state) { ct::optcon::LQR<8, 3> lqr; Eigen::Matrix K_l; bool INFO = lqr.compute(Q, R, linearize(state), B, K_l, true, false); if (INFO == 0) - return false; - Eigen::Matrix state_error = update_error(error, state); - u = saturate_input((K_l * state_error)); - return true; + valid=false; + Eigen::Matrix state_error = update_error(error_state, state); + //TODO:(henrimha) fix how i return the value here + Eigen::Vectoru = saturate_input((K_l * state_error)); + geometry_msgs::msg::WrenchStamped wrench; + wrench.wrench.force.x = u[0]; + wrench.wrench.torque.y = u[1]; + wrench.wrench.torque.z = u[2]; + return wrench; } void LQRController::reset_controller(int nr) { if (nr == 0 || nr == 1) { @@ -174,16 +178,6 @@ void LQRController::reset_controller(int nr) { return; } -bool LQRController::set_interval(double interval) { - if (interval <= 0) - return false; - interval_ = interval; - return true; -} - -Eigen::Vector LQRController::get_thrust() { - return u; -} // TODO(henrimha): double check the matrices here Eigen::Matrix LQRController::coriolis(const State& s) { diff --git a/control/velocity_controller/src/PID_controller.cpp b/control/velocity_controller/src/PID_controller.cpp new file mode 100644 index 000000000..577582614 --- /dev/null +++ b/control/velocity_controller/src/PID_controller.cpp @@ -0,0 +1,74 @@ +#include "velocity_controller/lib/PID_controller.hpp" + +// TODO(henrimha): kanskje forbedre integrasjon og derivasjons beregningene +// TODO(henrimha): check for more errors, f.example Nan or very high integral +double PID_controller::calculate_thrust(double error) { + if (!valid) + return 0; + // P + I + D + integral += error * params_.dt; + double output = + params_.k_p * error + params_.k_i * integral + params_.k_d * (error - previous_error) / params_.dt; + previous_error = error; + // Saturation + //TODO(henrimha): add feature that allows integral to compound if the output is saturated, but only if the error is in the same direction as the output, otherwise it should anti-windup + if (output > params_.max_output) { + output = params_.max_output; + integral -= error * params_.dt; // anti-wind up + + } else if (output < params_.min_output) { + output = params_.min_output; + integral -= error * params_.dt; // anti-wind up + } + return output; +} +double PID_controller::calculate_thrust(double error, double error_d) { + if (!valid) + return 0; + integral += error * params_.dt; // anti-wind up + // P + I + D + double output = params_.k_p * error + params_.k_i * integral + params_.k_d * error_d; + previous_error = error; + // Saturation + if (output > params_.max_output) { + output = params_.max_output; + integral -= error * params_.dt; // anti-wind up + + } else if (output < params_.min_output) { + output = params_.min_output; + integral -= error * params_.dt; // anti-wind up + } + return output; +} +void PID_controller::reset_controller() { + integral = 0.0; + previous_error = 0.0; +} + +PID_controller::PID_controller(PID_params params){ + if (params.dt <= 0&¶ms.max_output < params.min_output) { + valid = false; + //TODO:(henrimha) throw an error or log an error message + return; + } + params_ = params; + valid = true; +} + +const std::vector& PID_params::operator=(const std::vector& params) { + if (params.size() == 6){ + throw std::invalid_argument("Invalid parameter size for PID_params"); + k_p = params[0]; + k_i = params[1]; + k_d = params[2]; + dt = params[3]; + max_output = params[4]; + min_output = params[5]; + } + else if(params.size() == 3) { + k_p = params[0]; + k_i = params[1]; + k_d = params[2]; + } + return params; +} \ No newline at end of file diff --git a/control/velocity_controller/src/PID_setup.cpp b/control/velocity_controller/src/PID_setup.cpp deleted file mode 100644 index ad730fe63..000000000 --- a/control/velocity_controller/src/PID_setup.cpp +++ /dev/null @@ -1,87 +0,0 @@ -#include "velocity_controller/PID_setup.hpp" - -/* -PID_controller::PID_controller( double Kp, double Ki, double Kd, double -max_output, double min_output, double dt):Kp_(Kp), Ki_(Ki), Kd_(Kd), -max_output_(max_output), min_output_(min_output), dt_(dt) { integral = 0.0; - previous_error = 0.0; -};*/ -// TODO(henrimha): kanskje forbedre integrasjon og derivasjons beregningene -// TODO(henrimha): check for more errors, f.example Nan or very high integral -bool PID_controller::calculate_thrust(double error) { - if (!init) - return false; - // Saturation - if (output > max_output_) { - output = max_output_; - } else if (output < min_output_) { - output = min_output_; - } else { - integral += error * dt_; // anti-wind up - } - // P + I + D - output = - Kp_ * error + Ki_ * integral + Kd_ * (error - previous_error) / dt_; - previous_error = error; - - return true; -} -bool PID_controller::calculate_thrust(double error, double error_d) { - if (!init) - return false; - // Saturation - if (output > max_output_) { - output = max_output_; - } else if (output < min_output_) { - output = min_output_; - } else { - integral += error * dt_; // anti-wind up - } - // P + I + D - output = Kp_ * error + Ki_ * integral + Kd_ * error_d; - previous_error = error; - - return true; -} -void PID_controller::reset_controller() { - integral = 0.0; - previous_error = 0.0; - output = 0.0; -} - -double PID_controller::get_output() { - return output; -} - -bool PID_controller::set_output_limits(double min_output, double max_output) { - if (max_output < min_output) { - return false; - } - min_output_ = min_output; - max_output_ = max_output; - return true; -} -bool PID_controller::set_parameters(double Kp, - double Ki, - double Kd, - double dt) { - Kp_ = Kp; - Ki_ = Ki; - Kd_ = Kd; - if (set_dt(dt)) { - init = true; - return true; - } - return false; -} - -bool PID_controller::set_dt(double dt) { - if (dt <= 0) { - return false; - } - dt_ = dt; - return true; -} -bool PID_controller::set_parameters(std::vector& params, double dt) { - return set_parameters(params.at(0), params.at(1), params.at(2), dt); -} diff --git a/control/velocity_controller/src/control_manager.cpp b/control/velocity_controller/src/control_manager.cpp new file mode 100644 index 000000000..e10bc1ec1 --- /dev/null +++ b/control/velocity_controller/src/control_manager.cpp @@ -0,0 +1,74 @@ +#include "velocity_controller/control_manager.hpp" +#include "velocity_controller/lib/3DOF_PID.hpp" +#include "velocity_controller/lib/controller.hpp" +#include "vortex_msgs/msg/los_guidance.hpp" +#include "vortex/utils/math.hpp" + +control_manager::control_manager(control_manager_params params) : params_(params) {} + +bool control_manager::switch_controller() { + // Implementation for switching controllers + return true; +} + +void control_manager::shutdown_controller() { + controller_3DOF.reset(); + controller_LQR.reset(); +} + +geometry_msgs::msg::WrenchStamped control_manager::get_output(Guidance_data guidance_values, State current_state) { + // TODO(henrimha): Do I need ssa here? + angle ref_in_body = + angle_NED_to_body({0, vortex::utils::math::ssa(guidance_values.pitch), + vortex::utils::math::ssa(guidance_values.yaw)}, + current_state.get_angle()); + State error_state_body; + error_state_body.surge = guidance_values.surge - current_state.surge; + error_state_body.pitch = -ref_in_body.thetat; + error_state_body.yaw = -ref_in_body.psit; + + if (params_.anti_overshoot) { + if (abs(error_state_body.yaw) < std::numbers::pi / 2 || + abs(error_state_body.pitch) < std::numbers::pi / 2) { + error_state_body.surge = + guidance_values.surge * cos(error_state_body.yaw) * cos(error_state_body.pitch); + } + } + switch (params_.control_type) { + case 1: + return controller_3DOF->calculate_thrust(current_state, error_state_body); + case 2: + return controller_LQR->calculate_thrust(current_state, error_state_body); + default: + return geometry_msgs::msg::WrenchStamped(); + } +} +void control_manager::initialize_3DOF_controller(PID_3DOF_params params) { + controller_3DOF = std::make_unique(params); +} + +void control_manager::initialize_LQR_controller(LQR_params params) { + controller_LQR = std::make_unique(params); +} + +bool control_manager::get_validity() { + switch (params_.control_type) { + case 1: + return controller_3DOF->get_validity(); + case 2: + return controller_LQR->get_validity(); + default: + return false; + } +} + +void control_manager::reset_controllers(int nr) { + switch (params_.control_type) { + case 1: + controller_3DOF->reset_controller(nr); + break; + case 2: + controller_LQR->reset_controller(nr); + break; + } +} \ No newline at end of file diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp deleted file mode 100644 index 3f91eef24..000000000 --- a/control/velocity_controller/src/velocity_controller.cpp +++ /dev/null @@ -1,299 +0,0 @@ -#include "velocity_controller/velocity_controller.hpp" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "velocity_controller/PID_setup.hpp" -#include "velocity_controller/utilities.hpp" -#include "vortex/utils/math.hpp" - -Velocity_node::Velocity_node(const rclcpp::NodeOptions& options) - : rclcpp_lifecycle::LifecycleNode("velocity_controller_lifecycle", options), - lqr_controller(), - pub_QoS(10), - sub_QoS(10) { - get_new_parameters(); - pub_QoS.keep_last(10) - .reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT) - .durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); - sub_QoS.keep_last(10) - .reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT) - .durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); - - // PID initialization - PID_surge.set_output_limits(-max_force, max_force); - PID_pitch.set_output_limits(-max_force, max_force); - PID_yaw.set_output_limits(-max_force, max_force); - PID_surge.set_parameters(surge_params, publish_rate / 1000.0); - PID_pitch.set_parameters(pitch_params, publish_rate / 1000.0); - PID_yaw.set_parameters(yaw_params, publish_rate / 1000.0); - - // LQR - if (!lqr_controller.set_matrices(Q, R, inertia_matrix, max_force, - dampening_matrix_low) || - !lqr_controller.set_interval(static_cast(publish_rate) / - 1000)) { - controller_type = 1; - RCLCPP_INFO(this->get_logger(), "Switching to PID"); - } - // Automatically start in activate if auto_start is true - if (auto_start) { - startup_timer_ = - create_wall_timer(std::chrono::milliseconds(0), [this]() { - startup_timer_->cancel(); - trigger_transition( - lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); - }); - } - RCLCPP_INFO(this->get_logger(), "Velocity control node has been started."); - - return; -} - -void Velocity_node::calc_thrust() { - if (odometry_dropout_guard) { - publish_counter++; - if (publish_counter >= 100) { - reset_controllers(); - RCLCPP_WARN(this->get_logger(), "Odometry dropout, no thrust"); - return; - } - } - // TODO(henrimha): Do I need ssa here? - angle ref_in_body = - angle_NED_to_body({0, vortex::utils::math::ssa(guidance_values.pitch), - vortex::utils::math::ssa(guidance_values.yaw)}, - current_state.get_angle()); - Guidance_data error = {guidance_values.surge - current_state.surge, - -ref_in_body.thetat, -ref_in_body.psit}; - - if (anti_overshoot) { - if (abs(error.yaw) < std::numbers::pi / 2 || - abs(error.yaw) < std::numbers::pi / 2) { - error.surge = - guidance_values.surge * cos(error.yaw) * cos(error.pitch); - } - } - switch (controller_type) { - case 1: { - PID_surge.calculate_thrust(error.surge); - PID_pitch.calculate_thrust(error.pitch, -current_state.pitch_rate); - PID_yaw.calculate_thrust(error.yaw, -current_state.yaw_rate); - thrust_out.wrench.force.x = PID_surge.get_output(); - thrust_out.wrench.torque.y = PID_pitch.get_output(); - thrust_out.wrench.torque.z = PID_yaw.get_output(); - - break; - } - case 2: { - if (!lqr_controller.calculate_thrust(current_state, error)) { - controller_type = 1; - RCLCPP_ERROR(this->get_logger(), "Switching to PID"); - } else { - Eigen::Vector3d u = lqr_controller.get_thrust(); - thrust_out.wrench.force.x = u[0]; - thrust_out.wrench.torque.y = u[1]; - thrust_out.wrench.torque.z = u[2]; - } - break; - } - default: { - RCLCPP_ERROR(this->get_logger(), - "Unknown controller set, switching to PID"); - controller_type = 1; - calc_thrust(); - return; - } - } - publisher_thrust->publish(thrust_out); - return; -} - -// Callback functions -void Velocity_node::guidance_callback( - const vortex_msgs::msg::LOSGuidance::SharedPtr msg_ptr) { - if (reset_on_new_ref) { // On big step changes, reset the controllers to - // avoid big overshoots - if (abs(msg_ptr->surge - guidance_values.surge) >= 0.1) - reset_controllers(1); - if (abs(msg_ptr->pitch - guidance_values.pitch) > std::numbers::pi / 4) - reset_controllers(2); - if (abs(msg_ptr->yaw - guidance_values.yaw) < std::numbers::pi / 4) - reset_controllers(3); - } - guidance_values = msg_ptr; // overloaded to fix all the internal states - - return; -} - -void Velocity_node::odometry_callback( - const nav_msgs::msg::Odometry::SharedPtr msg_ptr) { - publish_counter = 0; - current_state = msg_ptr; // overloaded to fix all the internal states - return; -} - -void Velocity_node::get_new_parameters() { - // topics - this->declare_parameter("topics.wrench_input"); - topic_thrust = this->get_parameter("topics.wrench_input").as_string(); - this->declare_parameter("topics.guidance.los"); - topic_guidance = this->get_parameter("topics.guidance.los").as_string(); - this->declare_parameter("topics.odom"); - topic_odometry = this->get_parameter("topics.odom").as_string(); - - // Settings - this->declare_parameter("Settings.max_force"); - max_force = this->get_parameter("Settings.max_force").as_double(); - this->declare_parameter("Settings.publish_rate"); - publish_rate = this->get_parameter("Settings.publish_rate").as_int(); - this->declare_parameter("Settings.controller_type"); - controller_type = this->get_parameter("Settings.controller_type").as_int(); - this->declare_parameters("Settings", - {{"auto_start", false}, - {"reset_on_new_ref", true}, - {"anti_overshoot", true}, - {"odometry_dropout_guard", true}}); - auto settings = this->get_parameters( - {"Settings.auto_start", "Settings.reset_on_new_ref", - "Settings.anti_overshoot", "Settings.odometry_dropout_guard"}); - auto_start = settings[0].as_bool(); - reset_on_new_ref = settings[1].as_bool(); - anti_overshoot = settings[2].as_bool(); - odometry_dropout_guard = settings[3].as_bool(); - - // PID Params - this->declare_parameter>("PID_params.surge"); - surge_params = this->get_parameter("PID_params.surge").as_double_array(); - this->declare_parameter>("PID_params.pitch"); - pitch_params = this->get_parameter("PID_params.pitch").as_double_array(); - this->declare_parameter>("PID_params.yaw"); - yaw_params = this->get_parameter("PID_params.yaw").as_double_array(); - - // LQR Parameters - - this->declare_parameter>("LQR_params.Q"); - Q = this->get_parameter("LQR_params.Q").as_double_array(); - this->declare_parameter>("LQR_params.R"); - R = this->get_parameter("LQR_params.R").as_double_array(); - this->declare_parameter>("physical.mass_matrix"); - inertia_matrix = - this->get_parameter("physical.mass_matrix").as_double_array(); - - // D - this->declare_parameter>("dampening_matrix_low"); - this->declare_parameter>("dampening_matrix_high"); - this->dampening_matrix_low = - this->get_parameter("dampening_matrix_low").as_double_array(); - this->dampening_matrix_high = - this->get_parameter("dampening_matrix_high").as_double_array(); -} - -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -Velocity_node::on_configure(const rclcpp_lifecycle::State&) { - RCLCPP_INFO(get_logger(), "Configure VC"); - - // Publishers - publisher_thrust = create_publisher( - topic_thrust, pub_QoS); - - // Subscribers - subscriber_Odometry = this->create_subscription( - topic_odometry, sub_QoS, - std::bind(&Velocity_node::odometry_callback, this, - std::placeholders::_1)); - subscriber_guidance = - this->create_subscription( - topic_guidance, sub_QoS, - std::bind(&Velocity_node::guidance_callback, this, - std::placeholders::_1)); - // Timer - if (first_start && auto_start) { - startup_timer_ = - create_wall_timer(std::chrono::milliseconds(0), [this]() { - startup_timer_->cancel(); - trigger_transition( - lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); - }); - } - first_start = false; - return CallbackReturn::SUCCESS; -} - -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -Velocity_node::on_activate(const rclcpp_lifecycle::State& state) { - RCLCPP_INFO(get_logger(), "Activating..."); - timer_calculation = - this->create_wall_timer(std::chrono::milliseconds(publish_rate), - std::bind(&Velocity_node::calc_thrust, this)); - auto ret = LifecycleNode::on_activate(state); - - return ret; -} - -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -Velocity_node::on_deactivate(const rclcpp_lifecycle::State& state) { - RCLCPP_INFO(get_logger(), "Deactivating..."); - auto ret = LifecycleNode::on_deactivate(state); - timer_calculation.reset(); - reset_controllers(); - return ret; -} - -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -Velocity_node::on_cleanup(const rclcpp_lifecycle::State&) { - RCLCPP_INFO(get_logger(), "Cleaning up..."); - timer_calculation.reset(); - publisher_thrust.reset(); - subscriber_guidance.reset(); - subscriber_Odometry.reset(); - return CallbackReturn::SUCCESS; -} - -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -Velocity_node::on_shutdown(const rclcpp_lifecycle::State& state) { - RCLCPP_INFO(get_logger(), "Shutting down from state %s", - state.label().c_str()); - if (timer_calculation) - timer_calculation->cancel(); - timer_calculation.reset(); - publisher_thrust.reset(); - subscriber_guidance.reset(); - subscriber_Odometry.reset(); - should_exit_ = true; - return CallbackReturn::SUCCESS; -} - -void Velocity_node::reset_controllers(int nr) { - switch (nr) { - case 0: - PID_pitch.reset_controller(); - PID_surge.reset_controller(); - PID_yaw.reset_controller(); - lqr_controller.reset_controller(); - break; - case 1: - PID_surge.reset_controller(); - lqr_controller.reset_controller(1); - break; - - case 2: - PID_pitch.reset_controller(); - lqr_controller.reset_controller(2); - break; - - case 3: - PID_yaw.reset_controller(); - lqr_controller.reset_controller(3); - break; - } -} - -RCLCPP_COMPONENTS_REGISTER_NODE(Velocity_node) diff --git a/control/velocity_controller/src/velocity_controller_ros.cpp b/control/velocity_controller/src/velocity_controller_ros.cpp new file mode 100644 index 000000000..15070d57a --- /dev/null +++ b/control/velocity_controller/src/velocity_controller_ros.cpp @@ -0,0 +1,223 @@ +#include "velocity_controller/velocity_controller_ros.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "velocity_controller/control_manager.hpp" +#include "velocity_controller/utilities.hpp" +#include "velocity_controller/lib/3DOF_PID.hpp" + +Velocity_node::Velocity_node(const rclcpp::NodeOptions& options) + : rclcpp_lifecycle::LifecycleNode("velocity_controller_lifecycle", options), + pub_QoS(10), + sub_QoS(10) { + get_new_parameters(); + initialize_controllers(); + pub_QoS.keep_last(10) + .reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT) + .durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); + sub_QoS.keep_last(10) + .reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT) + .durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); + + // Automatically start in activate if auto_start is true + if (node_settings.auto_start) { + startup_timer_ = + create_wall_timer(std::chrono::milliseconds(0), [this]() { + startup_timer_->cancel(); + trigger_transition( + lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); + }); + } + RCLCPP_INFO(this->get_logger(), "Velocity control node has been started."); + + return; +} +//TODO(henrimha): Split the ROS part and the c++ part into seperate files/classes +void Velocity_node::publish_thrust() { + if (node_settings.odometry_dropout_guard) { + publish_counter++; + if (publish_counter >= 100) { + control_manager_ptr->reset_controllers(); + RCLCPP_WARN(this->get_logger(), "Odometry dropout, no thrust"); + return; + } + } + // TODO(henrimha): Do I need ssa here? + thrust_out = control_manager_ptr->get_output(guidance_values, current_state); + publisher_thrust->publish(thrust_out); + return; +} + +// Callback functions +void Velocity_node::guidance_callback( + const vortex_msgs::msg::LOSGuidance::SharedPtr msg_ptr) { + if (node_settings.reset_on_new_ref) { // On big step changes, reset the controllers to + // avoid big overshoots + if (abs(msg_ptr->surge - guidance_values.surge) >= 0.1) + control_manager_ptr->reset_controllers(1); + if (abs(msg_ptr->pitch - guidance_values.pitch) > std::numbers::pi / 4) + control_manager_ptr->reset_controllers(2); + if (abs(msg_ptr->yaw - guidance_values.yaw) < std::numbers::pi / 4) + control_manager_ptr->reset_controllers(3); + } + guidance_values = msg_ptr; // overloaded to fix all the internal states + + return; +} + +void Velocity_node::odometry_callback( + const nav_msgs::msg::Odometry::SharedPtr msg_ptr) { + publish_counter = 0; + current_state = msg_ptr; // overloaded to fix all the internal states + return; +} + +void Velocity_node::get_new_parameters() { + + //Eigen::Matrix3d inertia_matrix; + //std::vector dampening_matrix_low, dampening_matrix_high; + // topics + this->declare_parameter("topics.wrench_input"); + node_settings.topic_thrust = this->get_parameter("topics.wrench_input").as_string(); + this->declare_parameter("topics.guidance.los"); + node_settings.topic_guidance = this->get_parameter("topics.guidance.los").as_string(); + this->declare_parameter("topics.odom"); + node_settings.topic_odometry = this->get_parameter("topics.odom").as_string(); + // Settings + this->declare_parameter("Control_manager_settings.max_force"); + + this->declare_parameter("Control_manager_settings.publish_rate"); + this->declare_parameter("Control_manager_settings.controller_type"); + this->declare_parameter("Node_settings.auto_start", false); + this->declare_parameter("Node_settings.reset_on_new_ref", true); + this->declare_parameter("Control_manager_settings.anti_overshoot", true); + this->declare_parameter("Node_settings.odometry_dropout_guard", true); + + // PID Params + this->declare_parameter>("PID_params.surge"); + this->declare_parameter>("PID_params.pitch"); + this->declare_parameter>("PID_params.yaw"); + + // LQR Parameters + this->declare_parameter>("LQR_params.Q"); + this->declare_parameter>("LQR_params.R"); + this->declare_parameter>("physical.mass_matrix"); + + // D + this->declare_parameter>("dampening_matrix_low"); + this->declare_parameter>("dampening_matrix_high"); + +} +void Velocity_node::initialize_controllers() { + control_manager_params control_params; + PID_3DOF_params pid_3dof_params; + LQR_params lqr_params; + pid_3dof_params.surge_params.max_output=pid_3dof_params.pitch_params.max_output=pid_3dof_params.yaw_params.max_output=lqr_params.max_force= this->get_parameter("Control_manager_settings.max_force").as_double(); + pid_3dof_params.surge_params.min_output=pid_3dof_params.pitch_params.min_output=pid_3dof_params.yaw_params.min_output= -this->get_parameter("Control_manager_settings.max_force").as_double(); + pid_3dof_params.surge_params.dt=pid_3dof_params.pitch_params.dt=pid_3dof_params.yaw_params.dt=lqr_params.interval= this->get_parameter("Control_manager_settings.publish_rate").as_int()/1000.0; + control_params.control_type = this->get_parameter("Control_manager_settings.controller_type").as_int(); + node_settings.auto_start = this->get_parameter("Node_settings.auto_start").as_bool(); + node_settings.reset_on_new_ref = this->get_parameter("Node_settings.reset_on_new_ref").as_bool(); + control_params.anti_overshoot = this->get_parameter("Control_manager_settings.anti_overshoot").as_bool(); + node_settings.odometry_dropout_guard = this->get_parameter("Node_settings.odometry_dropout_guard").as_bool(); + pid_3dof_params.surge_params = this->get_parameter("PID_params.surge").as_double_array(); + pid_3dof_params.pitch_params = this->get_parameter("PID_params.pitch").as_double_array(); + pid_3dof_params.yaw_params = this->get_parameter("PID_params.yaw").as_double_array(); + + lqr_params.Q = this->get_parameter("LQR_params.Q").as_double_array(); + lqr_params.R = this->get_parameter("LQR_params.R").as_double_array(); + lqr_params.inertia_matrix = this->get_parameter("physical.mass_matrix").as_double_array(); + lqr_params.D_low =this->get_parameter("dampening_matrix_low").as_double_array(); + lqr_params.D_high =this->get_parameter("dampening_matrix_high").as_double_array(); + + + control_manager_ptr = std::make_unique(control_params); + control_manager_ptr->initialize_3DOF_controller(pid_3dof_params); + control_manager_ptr->initialize_LQR_controller(lqr_params); + return; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +Velocity_node::on_configure(const rclcpp_lifecycle::State&) { + RCLCPP_INFO(get_logger(), "Configure VC"); + + // Publishers + publisher_thrust = create_publisher( + node_settings.topic_thrust, pub_QoS); + + // Subscribers + subscriber_Odometry = this->create_subscription( + node_settings.topic_odometry, sub_QoS, + std::bind(&Velocity_node::odometry_callback, this, + std::placeholders::_1)); + subscriber_guidance = + this->create_subscription( + node_settings.topic_guidance, sub_QoS, + std::bind(&Velocity_node::guidance_callback, this, + std::placeholders::_1)); + // Timer + if (first_start && node_settings.auto_start) { + startup_timer_ = + create_wall_timer(std::chrono::milliseconds(0), [this]() { + startup_timer_->cancel(); + trigger_transition( + lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); + }); + } + first_start = false; + return CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +Velocity_node::on_activate(const rclcpp_lifecycle::State& state) { + RCLCPP_INFO(get_logger(), "Activating..."); + timer_calculation = + this->create_wall_timer(std::chrono::milliseconds(node_settings.publish_rate), + std::bind(&Velocity_node::publish_thrust, this)); + auto ret = LifecycleNode::on_activate(state); + + return ret; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +Velocity_node::on_deactivate(const rclcpp_lifecycle::State& state) { + RCLCPP_INFO(get_logger(), "Deactivating..."); + auto ret = LifecycleNode::on_deactivate(state); + timer_calculation.reset(); + control_manager_ptr->reset_controllers(); + return ret; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +Velocity_node::on_cleanup(const rclcpp_lifecycle::State&) { + RCLCPP_INFO(get_logger(), "Cleaning up..."); + timer_calculation.reset(); + publisher_thrust.reset(); + subscriber_guidance.reset(); + subscriber_Odometry.reset(); + return CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +Velocity_node::on_shutdown(const rclcpp_lifecycle::State& state) { + RCLCPP_INFO(get_logger(), "Shutting down from state %s", + state.label().c_str()); + if (timer_calculation) + timer_calculation->cancel(); + timer_calculation.reset(); + publisher_thrust.reset(); + subscriber_guidance.reset(); + subscriber_Odometry.reset(); + should_exit_ = true; + return CallbackReturn::SUCCESS; +} + +RCLCPP_COMPONENTS_REGISTER_NODE(Velocity_node) diff --git a/control/velocity_controller/tests/CMakeLists.txt b/control/velocity_controller/tests/CMakeLists.txt index 61f7e7586..adbd6fe82 100644 --- a/control/velocity_controller/tests/CMakeLists.txt +++ b/control/velocity_controller/tests/CMakeLists.txt @@ -12,7 +12,7 @@ ament_add_gtest(${TEST_BINARY_NAME} test_PID.cpp ../src/utilities.cpp ../src/ct_instantiations.cpp -../src/PID_setup.cpp +../src/PID_controller.cpp ) ament_add_gtest(LQR_test @@ -22,7 +22,7 @@ test_LQR.cpp ../src/LQR_setup.cpp ) -#To DO need to clean up the include directories/dependencies and such +#TODO(hemrimha): need to clean up the include directories/dependencies and such target_include_directories(${TEST_BINARY_NAME} PUBLIC $ $ @@ -78,21 +78,21 @@ target_link_libraries( ct_optcon ct_core ) -add_executable(test_VC_node +add_executable(test_vc_node test_VC.cpp ../src/utilities.cpp ) -target_include_directories(test_VC_node PUBLIC +target_include_directories(test_vc_node PUBLIC $ $ ${EIGEN3_INCLUDE_DIR} ) -target_link_libraries(test_VC_node Eigen3::Eigen ct_optcon ct_core) +target_link_libraries(test_vc_node Eigen3::Eigen ct_optcon ct_core) -ament_target_dependencies(test_VC_node +ament_target_dependencies(test_vc_node rclcpp rclcpp_lifecycle std_msgs @@ -102,6 +102,6 @@ nav_msgs vortex_utils ) -install(TARGETS test_VC_node +install(TARGETS test_vc_node DESTINATION lib/${PROJECT_NAME} ) diff --git a/control/velocity_controller/tests/test_LQR.cpp b/control/velocity_controller/tests/test_LQR.cpp index aa2330d7d..39a28f69e 100644 --- a/control/velocity_controller/tests/test_LQR.cpp +++ b/control/velocity_controller/tests/test_LQR.cpp @@ -1,10 +1,14 @@ #include #include #include -#include +#include #include #include "velocity_controller/utilities.hpp" +//TODO(henrimha): Use standard convention for test, number_type_purpose like thrust allocator +//TODO(henrimha): Need to implement more tests, use claude or other +//TODO(henrimha): fix testing suit +/* class LQR_test : public ::testing::Test { protected: double delta = 0.0001; @@ -96,3 +100,4 @@ int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } +*/ \ No newline at end of file diff --git a/control/velocity_controller/tests/test_PID.cpp b/control/velocity_controller/tests/test_PID.cpp index 8d1a10ec0..e767d1287 100644 --- a/control/velocity_controller/tests/test_PID.cpp +++ b/control/velocity_controller/tests/test_PID.cpp @@ -1,9 +1,11 @@ #include // #include // #include -#include +#include // #include - +//TODO(henrimha): Add more tests +//TODO(henrimha): fix testing suit +/* class PID_test : public ::testing::Test { protected: double delta = 0.0001; @@ -14,22 +16,7 @@ class PID_test : public ::testing::Test { } void TearDown() override {} }; -/* -class Node_test : public ::testing:Test{ - protected: - static void SetUpTestSuite(){ - int argc=0; - char ** argv==nullptr; - rclcpp::init(); - } - static void TearDownTestSuite(){ - rclcpp::shutdown(); - } -}; - - -*/ TEST_F(PID_test, reset_controller) { PID.set_parameters(0, 1, 0, 100); PID.calculate_thrust(100); @@ -82,23 +69,9 @@ TEST_F(PID_test, illegal_inputs) { EXPECT_FALSE(PID.calculate_thrust(1)); } -/* -TEST(PID,BASIC){ - PID_controller foo (1,1,0); - ASSERT_NO_THROW(foo.set_output_limits(-10,100)); - ASSERT_NO_THROW(foo.calculate_thrust(1,1)); - EXPECT_TRUE(foo.get_output()>0); - EXPECT_NEAR(foo.get_output(),1,0.01); - ASSERT_NO_THROW(foo.calculate_thrust(1,1)); - EXPECT_NEAR(foo.get_output(),2,0.01); - ASSERT_NO_THROW(foo.calculate_thrust(1000000,1)); - EXPECT_EQ(foo.get_output(),100); - ASSERT_NO_THROW(foo.calculate_thrust(-100000,1)); - EXPECT_EQ(foo.get_output(),-10); -} -*/ int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } +*/ \ No newline at end of file diff --git a/control/velocity_controller/tests/test_VC.cpp b/control/velocity_controller/tests/test_VC.cpp index b33b157df..d85d42c86 100644 --- a/control/velocity_controller/tests/test_VC.cpp +++ b/control/velocity_controller/tests/test_VC.cpp @@ -14,8 +14,7 @@ #include "vortex_msgs/msg/los_guidance.hpp" // Denne noden er kun for å teste velocity_controller noden - -test_VC::test_VC() : Node("test_VC_node") { +test_velocity_controller::test_velocity_controller() : Node("test_VC_node") { this->declare_parameter("topics.guidance.los"); this->declare_parameter("topics.odom"); this->topic_guidance = @@ -37,23 +36,23 @@ test_VC::test_VC() : Node("test_VC_node") { .durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); subscription_state = this->create_subscription( topic_odometry, sub_QoS, - std::bind(&test_VC::odometry_callback, this, std::placeholders::_1)); + std::bind(&test_velocity_controller::odometry_callback, this, std::placeholders::_1)); timer_ = this->create_wall_timer(std::chrono::milliseconds(200), - std::bind(&test_VC::send_guidance, this)); + std::bind(&test_velocity_controller::send_reference, this)); clock_ = this->get_clock(); - RCLCPP_INFO(this->get_logger(), "Test_VC node has been started"); + RCLCPP_INFO(this->get_logger(), "Test_velocity_controller node has been started"); } -void test_VC::send_guidance() { - time1 += 0.2; - reference_msg.yaw = std::numbers::pi * sin(time1 * std::numbers::pi / 9); +void test_velocity_controller::send_reference() { + totaltime += 0.2; + reference_msg.yaw = std::numbers::pi * sin(totaltime * std::numbers::pi / 9); // reference_msg.pitch=0.3*sin(time1*std::numbers::pi/9); reference_msg.surge = 0.20; reference_msg.pitch = -0.4; // reference_msg.yaw=0.0; //Surge, pitch, yaw publisher_guidance->publish(reference_msg); } -void test_VC::odometry_callback( +void test_velocity_controller::odometry_callback( const nav_msgs::msg::Odometry::SharedPtr msg_ptr) { vortex_msgs::msg::LOSGuidance msg; angle temp = quaternion_to_euler_angle( @@ -66,7 +65,7 @@ void test_VC::odometry_callback( } int main(int argc, char const* argv[]) { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; } From e438f33998281b85db163a826380aff5b109f28b Mon Sep 17 00:00:00 2001 From: Henrik Date: Thu, 2 Apr 2026 11:49:23 +0200 Subject: [PATCH 126/128] minor tuning --- .../config/nautilus_params.yaml | 4 +- .../velocity_controller/lib/controller.hpp | 2 +- .../src/velocity_controller_ros.cpp | 44 +++++++++++++++---- control/velocity_controller/tests/test_VC.cpp | 3 +- 4 files changed, 41 insertions(+), 12 deletions(-) diff --git a/control/velocity_controller/config/nautilus_params.yaml b/control/velocity_controller/config/nautilus_params.yaml index 1cddf0706..9d6dd1a17 100644 --- a/control/velocity_controller/config/nautilus_params.yaml +++ b/control/velocity_controller/config/nautilus_params.yaml @@ -2,7 +2,7 @@ ros__parameters: PID_params: - surge: [300.0,10.0,5.0] + surge: [500.0,50.0,5.0] pitch: [60.0,8.0,12.0] yaw: [10.0,1.0,5.0] @@ -22,5 +22,5 @@ anti_overshoot: false publish_rate: 100 #ms max_force: 99.5 - controller_type: 2 #1 PID 2 LQR + controller_type: 1 #1 PID 2 LQR diff --git a/control/velocity_controller/include/velocity_controller/lib/controller.hpp b/control/velocity_controller/include/velocity_controller/lib/controller.hpp index a9388b6d5..6a4b622cc 100644 --- a/control/velocity_controller/include/velocity_controller/lib/controller.hpp +++ b/control/velocity_controller/include/velocity_controller/lib/controller.hpp @@ -10,7 +10,7 @@ class controller{ virtual geometry_msgs::msg::WrenchStamped calculate_thrust(State state, State error_state) = 0; virtual void reset_controller(int nr=0) = 0; bool get_validity(){return valid;}; - + ~controller()=default; protected: bool valid=false; }; diff --git a/control/velocity_controller/src/velocity_controller_ros.cpp b/control/velocity_controller/src/velocity_controller_ros.cpp index 15070d57a..d8c9d4817 100644 --- a/control/velocity_controller/src/velocity_controller_ros.cpp +++ b/control/velocity_controller/src/velocity_controller_ros.cpp @@ -50,8 +50,9 @@ void Velocity_node::publish_thrust() { return; } } - // TODO(henrimha): Do I need ssa here? thrust_out = control_manager_ptr->get_output(guidance_values, current_state); + /*geometry_msgs::msg::WrenchStamped test; + test.wrench.force.set__y(25);*/ publisher_thrust->publish(thrust_out); return; } @@ -120,17 +121,44 @@ void Velocity_node::initialize_controllers() { control_manager_params control_params; PID_3DOF_params pid_3dof_params; LQR_params lqr_params; - pid_3dof_params.surge_params.max_output=pid_3dof_params.pitch_params.max_output=pid_3dof_params.yaw_params.max_output=lqr_params.max_force= this->get_parameter("Control_manager_settings.max_force").as_double(); - pid_3dof_params.surge_params.min_output=pid_3dof_params.pitch_params.min_output=pid_3dof_params.yaw_params.min_output= -this->get_parameter("Control_manager_settings.max_force").as_double(); - pid_3dof_params.surge_params.dt=pid_3dof_params.pitch_params.dt=pid_3dof_params.yaw_params.dt=lqr_params.interval= this->get_parameter("Control_manager_settings.publish_rate").as_int()/1000.0; + double max_force = this->get_parameter("Control_manager_settings.max_force").as_double(); + double dt = this->get_parameter("Control_manager_settings.publish_rate").as_int() / 1000.0; // Convert ms to seconds + // Set max/min/dt FIRST + pid_3dof_params.surge_params.max_output = max_force; + pid_3dof_params.surge_params.min_output = -max_force; + pid_3dof_params.surge_params.dt = dt; + + pid_3dof_params.pitch_params.max_output = max_force; + pid_3dof_params.pitch_params.min_output = -max_force; + pid_3dof_params.pitch_params.dt = dt; + + pid_3dof_params.yaw_params.max_output = max_force; + pid_3dof_params.yaw_params.min_output = -max_force; + pid_3dof_params.yaw_params.dt = dt; + + // NOW set gains WITHOUT overwriting other fields + auto surge_gains = this->get_parameter("PID_params.surge").as_double_array(); + pid_3dof_params.surge_params.k_p = surge_gains[0]; + pid_3dof_params.surge_params.k_i = surge_gains[1]; + pid_3dof_params.surge_params.k_d = surge_gains[2]; + + auto pitch_gains = this->get_parameter("PID_params.pitch").as_double_array(); + pid_3dof_params.pitch_params.k_p = pitch_gains[0]; + pid_3dof_params.pitch_params.k_i = pitch_gains[1]; + pid_3dof_params.pitch_params.k_d = pitch_gains[2]; + + auto yaw_gains = this->get_parameter("PID_params.yaw").as_double_array(); + pid_3dof_params.yaw_params.k_p = yaw_gains[0]; + pid_3dof_params.yaw_params.k_i = yaw_gains[1]; + pid_3dof_params.yaw_params.k_d = yaw_gains[2]; + lqr_params.max_force = max_force; + lqr_params.interval = dt; + control_params.control_type = this->get_parameter("Control_manager_settings.controller_type").as_int(); node_settings.auto_start = this->get_parameter("Node_settings.auto_start").as_bool(); node_settings.reset_on_new_ref = this->get_parameter("Node_settings.reset_on_new_ref").as_bool(); control_params.anti_overshoot = this->get_parameter("Control_manager_settings.anti_overshoot").as_bool(); node_settings.odometry_dropout_guard = this->get_parameter("Node_settings.odometry_dropout_guard").as_bool(); - pid_3dof_params.surge_params = this->get_parameter("PID_params.surge").as_double_array(); - pid_3dof_params.pitch_params = this->get_parameter("PID_params.pitch").as_double_array(); - pid_3dof_params.yaw_params = this->get_parameter("PID_params.yaw").as_double_array(); lqr_params.Q = this->get_parameter("LQR_params.Q").as_double_array(); lqr_params.R = this->get_parameter("LQR_params.R").as_double_array(); @@ -138,7 +166,7 @@ void Velocity_node::initialize_controllers() { lqr_params.D_low =this->get_parameter("dampening_matrix_low").as_double_array(); lqr_params.D_high =this->get_parameter("dampening_matrix_high").as_double_array(); - + node_settings.publish_rate = this->get_parameter("Control_manager_settings.publish_rate").as_int(); control_manager_ptr = std::make_unique(control_params); control_manager_ptr->initialize_3DOF_controller(pid_3dof_params); control_manager_ptr->initialize_LQR_controller(lqr_params); diff --git a/control/velocity_controller/tests/test_VC.cpp b/control/velocity_controller/tests/test_VC.cpp index d85d42c86..9791bb299 100644 --- a/control/velocity_controller/tests/test_VC.cpp +++ b/control/velocity_controller/tests/test_VC.cpp @@ -45,8 +45,9 @@ test_velocity_controller::test_velocity_controller() : Node("test_VC_node") { void test_velocity_controller::send_reference() { totaltime += 0.2; - reference_msg.yaw = std::numbers::pi * sin(totaltime * std::numbers::pi / 9); + //reference_msg.yaw = std::numbers::pi * sin(totaltime * std::numbers::pi / 9); // reference_msg.pitch=0.3*sin(time1*std::numbers::pi/9); + reference_msg.yaw = 0.4; reference_msg.surge = 0.20; reference_msg.pitch = -0.4; // reference_msg.yaw=0.0; //Surge, pitch, yaw publisher_guidance->publish(reference_msg); From f08052545a7f456f1f4f3b7d055de4e84ccc3e23 Mon Sep 17 00:00:00 2001 From: Henrik Date: Fri, 10 Apr 2026 12:17:51 +0200 Subject: [PATCH 127/128] Changed initialization of controllers --- .../config/nautilus_params.yaml | 4 +- .../config/orca_params.yaml | 4 +- .../velocity_controller/control_manager.hpp | 3 + .../velocity_controller/lib/3DOF_PID.hpp | 9 +- .../velocity_controller/lib/LQR_setup.hpp | 4 + .../lib/PID_controller.hpp | 5 + control/velocity_controller/src/3DOF_PID.cpp | 2 +- control/velocity_controller/src/LQR_setup.cpp | 3 +- .../src/velocity_controller_ros.cpp | 91 ++++++++----------- 9 files changed, 61 insertions(+), 64 deletions(-) diff --git a/control/velocity_controller/config/nautilus_params.yaml b/control/velocity_controller/config/nautilus_params.yaml index 9d6dd1a17..f3513edd2 100644 --- a/control/velocity_controller/config/nautilus_params.yaml +++ b/control/velocity_controller/config/nautilus_params.yaml @@ -1,7 +1,7 @@ /**: ros__parameters: - PID_params: + 3DOF_PID_params: surge: [500.0,50.0,5.0] pitch: [60.0,8.0,12.0] yaw: [10.0,1.0,5.0] @@ -10,7 +10,7 @@ Q: [200.0,32.84,32.84,15.0,15.0,100.0,32.84,32.84] R: [0.02,3.1,3.10] #TODO(henrimha): move these to the global parameter file - dampening_matrix_low: [23.0,0.0,0.0,0.0,0.0,0.0, 0.0,46.0,0.0,0.0,0.0,0.0, 0.0,0.0,46.0,0.0,0.0,0.0, 0.0,0.0,0.0,46.0,0.0,0.0, 0.0,0.0,0.0,0.0,46.0,0.0, 0.0,0.0,0.0,0.0,0.0,46.0] + dampening_matrix_low: [104.0,0.0,0.0,0.0,0.0,0.0, 0.0,46.0,0.0,0.0,0.0,0.0, 0.0,0.0,46.0,0.0,0.0,0.0, 0.0,0.0,0.0,46.0,0.0,0.0, 0.0,0.0,0.0,0.0,46.0,0.0, 0.0,0.0,0.0,0.0,0.0,46.0] dampening_matrix_high: [1.0,0.0,0.0,0.0,0.0,0.0, 0.0,1.0,0.0,0.0,0.0,0.0, 0.0,0.0,1.0,0.0,0.0,0.0, 0.0,0.0,0.0,1.0,0.0,0.0, 0.0,0.0,0.0,0.0,1.0,0.0, 0.0,0.0,0.0,0.0,0.0,1.0] diff --git a/control/velocity_controller/config/orca_params.yaml b/control/velocity_controller/config/orca_params.yaml index 8085fc720..479649d35 100644 --- a/control/velocity_controller/config/orca_params.yaml +++ b/control/velocity_controller/config/orca_params.yaml @@ -1,14 +1,14 @@ /**: ros__parameters: - PID_params: + 3DOF_PID_params: surge: [300.0,10.0,5.0] pitch: [60.0,8.0,12.0] yaw: [10.0,1.0,5.0] LQR_params: Q: [200.0,32.84,32.84,15.0,15.0,100.0,32.84,32.84] - R: [0.02,3.1,3.10] + R: [0.02,3.1,3.1] #TODO(henrimha): move these to the global parameter file dampening_matrix_low: [23.0,0.0,0.0,0.0,0.0,0.0, 0.0,46.0,0.0,0.0,0.0,0.0, 0.0,0.0,46.0,0.0,0.0,0.0, 0.0,0.0,0.0,46.0,0.0,0.0, 0.0,0.0,0.0,0.0,46.0,0.0, 0.0,0.0,0.0,0.0,0.0,46.0] dampening_matrix_high: [1.0,0.0,0.0,0.0,0.0,0.0, 0.0,1.0,0.0,0.0,0.0,0.0, 0.0,0.0,1.0,0.0,0.0,0.0, 0.0,0.0,0.0,1.0,0.0,0.0, 0.0,0.0,0.0,0.0,1.0,0.0, 0.0,0.0,0.0,0.0,0.0,1.0] diff --git a/control/velocity_controller/include/velocity_controller/control_manager.hpp b/control/velocity_controller/include/velocity_controller/control_manager.hpp index 25b757d0a..5346ceb7d 100644 --- a/control/velocity_controller/include/velocity_controller/control_manager.hpp +++ b/control/velocity_controller/include/velocity_controller/control_manager.hpp @@ -8,6 +8,9 @@ struct control_manager_params{ int control_type; // 1 3DOF PID, 2 3DOF LQR bool anti_overshoot; bool fallback; + control_manager_params(int control_type_val, bool anti_overshoot_val, bool fallback_val) + : control_type(control_type_val), anti_overshoot(anti_overshoot_val), fallback(fallback_val) {} + control_manager_params() = default; }; class control_manager{ public: diff --git a/control/velocity_controller/include/velocity_controller/lib/3DOF_PID.hpp b/control/velocity_controller/include/velocity_controller/lib/3DOF_PID.hpp index 8da74b266..7851df7c5 100644 --- a/control/velocity_controller/include/velocity_controller/lib/3DOF_PID.hpp +++ b/control/velocity_controller/include/velocity_controller/lib/3DOF_PID.hpp @@ -6,9 +6,12 @@ #include #include "velocity_controller/lib/PID_controller.hpp" struct PID_3DOF_params{ - PID_params surge_params; - PID_params pitch_params; - PID_params yaw_params; + std::vector surge; + std::vector pitch; + std::vector yaw; + double dt; + double max_force; + double min_force; }; class PID_3DOF : public controller { public: diff --git a/control/velocity_controller/include/velocity_controller/lib/LQR_setup.hpp b/control/velocity_controller/include/velocity_controller/lib/LQR_setup.hpp index 4a509dbf2..940042658 100644 --- a/control/velocity_controller/include/velocity_controller/lib/LQR_setup.hpp +++ b/control/velocity_controller/include/velocity_controller/lib/LQR_setup.hpp @@ -1,5 +1,6 @@ #pragma once #include +#include #include #include #include @@ -18,6 +19,9 @@ struct LQR_params{ std::vector D_low; std::vector D_high; double interval; + LQR_params(std::vector Q_val, std::vector R_val, std::vector inertia_matrix_val, double max_force_val, std::vector D_low_val, std::vector D_high_val, double interval_val) + : Q(Q_val), R(R_val), inertia_matrix(inertia_matrix_val), max_force(max_force_val), D_low(D_low_val), D_high(D_high_val), interval(interval_val) {}; + LQR_params() = default; }; class LQRController : public controller{ public: diff --git a/control/velocity_controller/include/velocity_controller/lib/PID_controller.hpp b/control/velocity_controller/include/velocity_controller/lib/PID_controller.hpp index e5b6cf81a..51d2b1105 100644 --- a/control/velocity_controller/include/velocity_controller/lib/PID_controller.hpp +++ b/control/velocity_controller/include/velocity_controller/lib/PID_controller.hpp @@ -11,6 +11,11 @@ struct PID_params{ double max_output; double min_output; const std::vector& operator=(const std::vector& params); + // Constructor from gains vector and other params + //TODO(henrimha): change the vector to an array + PID_params(const std::vector& gains, double dt_val, double max_out, double min_out) + : k_p(gains[0]), k_i(gains[1]), k_d(gains[2]), dt(dt_val), max_output(max_out), min_output(min_out) {} + PID_params() = default; }; diff --git a/control/velocity_controller/src/3DOF_PID.cpp b/control/velocity_controller/src/3DOF_PID.cpp index 6263afefc..c69301a35 100644 --- a/control/velocity_controller/src/3DOF_PID.cpp +++ b/control/velocity_controller/src/3DOF_PID.cpp @@ -3,7 +3,7 @@ #include PID_3DOF::PID_3DOF(PID_3DOF_params params) - : surge_controller(params.surge_params), pitch_controller(params.pitch_params), yaw_controller(params.yaw_params) {}; + : surge_controller(PID_params(params.surge,params.dt,params.max_force,params.min_force)), pitch_controller(PID_params(params.pitch,params.dt,params.max_force,params.min_force)), yaw_controller(PID_params(params.yaw,params.dt,params.max_force,params.min_force)) {}; geometry_msgs::msg::WrenchStamped PID_3DOF::calculate_thrust(State state, State error_state) { geometry_msgs::msg::WrenchStamped u; diff --git a/control/velocity_controller/src/LQR_setup.cpp b/control/velocity_controller/src/LQR_setup.cpp index 5ae2ea166..a0df84d1f 100644 --- a/control/velocity_controller/src/LQR_setup.cpp +++ b/control/velocity_controller/src/LQR_setup.cpp @@ -13,8 +13,7 @@ #include "velocity_controller/utilities.hpp" // #include "vortex/utils/math.hpp" -LQRController::LQRController(LQR_params params) { - params_=params; +LQRController::LQRController(LQR_params params) : params_(params) { inertia_matrix_inv.setZero(); if (params_.interval <= 0){ valid = false; diff --git a/control/velocity_controller/src/velocity_controller_ros.cpp b/control/velocity_controller/src/velocity_controller_ros.cpp index d8c9d4817..07267d9e5 100644 --- a/control/velocity_controller/src/velocity_controller_ros.cpp +++ b/control/velocity_controller/src/velocity_controller_ros.cpp @@ -66,7 +66,7 @@ void Velocity_node::guidance_callback( control_manager_ptr->reset_controllers(1); if (abs(msg_ptr->pitch - guidance_values.pitch) > std::numbers::pi / 4) control_manager_ptr->reset_controllers(2); - if (abs(msg_ptr->yaw - guidance_values.yaw) < std::numbers::pi / 4) + if (abs(msg_ptr->yaw - guidance_values.yaw) > std::numbers::pi / 4) control_manager_ptr->reset_controllers(3); } guidance_values = msg_ptr; // overloaded to fix all the internal states @@ -82,9 +82,6 @@ void Velocity_node::odometry_callback( } void Velocity_node::get_new_parameters() { - - //Eigen::Matrix3d inertia_matrix; - //std::vector dampening_matrix_low, dampening_matrix_high; // topics this->declare_parameter("topics.wrench_input"); node_settings.topic_thrust = this->get_parameter("topics.wrench_input").as_string(); @@ -92,20 +89,22 @@ void Velocity_node::get_new_parameters() { node_settings.topic_guidance = this->get_parameter("topics.guidance.los").as_string(); this->declare_parameter("topics.odom"); node_settings.topic_odometry = this->get_parameter("topics.odom").as_string(); - // Settings - this->declare_parameter("Control_manager_settings.max_force"); + // Control manager settings + this->declare_parameter("Control_manager_settings.max_force"); this->declare_parameter("Control_manager_settings.publish_rate"); this->declare_parameter("Control_manager_settings.controller_type"); + this->declare_parameter("Control_manager_settings.anti_overshoot", true); + + //Node settings this->declare_parameter("Node_settings.auto_start", false); this->declare_parameter("Node_settings.reset_on_new_ref", true); - this->declare_parameter("Control_manager_settings.anti_overshoot", true); this->declare_parameter("Node_settings.odometry_dropout_guard", true); // PID Params - this->declare_parameter>("PID_params.surge"); - this->declare_parameter>("PID_params.pitch"); - this->declare_parameter>("PID_params.yaw"); + this->declare_parameter>("3DOF_PID_params.surge"); + this->declare_parameter>("3DOF_PID_params.pitch"); + this->declare_parameter>("3DOF_PID_params.yaw"); // LQR Parameters this->declare_parameter>("LQR_params.Q"); @@ -118,55 +117,39 @@ void Velocity_node::get_new_parameters() { } void Velocity_node::initialize_controllers() { - control_manager_params control_params; - PID_3DOF_params pid_3dof_params; - LQR_params lqr_params; - double max_force = this->get_parameter("Control_manager_settings.max_force").as_double(); - double dt = this->get_parameter("Control_manager_settings.publish_rate").as_int() / 1000.0; // Convert ms to seconds - // Set max/min/dt FIRST - pid_3dof_params.surge_params.max_output = max_force; - pid_3dof_params.surge_params.min_output = -max_force; - pid_3dof_params.surge_params.dt = dt; - - pid_3dof_params.pitch_params.max_output = max_force; - pid_3dof_params.pitch_params.min_output = -max_force; - pid_3dof_params.pitch_params.dt = dt; - - pid_3dof_params.yaw_params.max_output = max_force; - pid_3dof_params.yaw_params.min_output = -max_force; - pid_3dof_params.yaw_params.dt = dt; - - // NOW set gains WITHOUT overwriting other fields - auto surge_gains = this->get_parameter("PID_params.surge").as_double_array(); - pid_3dof_params.surge_params.k_p = surge_gains[0]; - pid_3dof_params.surge_params.k_i = surge_gains[1]; - pid_3dof_params.surge_params.k_d = surge_gains[2]; - - auto pitch_gains = this->get_parameter("PID_params.pitch").as_double_array(); - pid_3dof_params.pitch_params.k_p = pitch_gains[0]; - pid_3dof_params.pitch_params.k_i = pitch_gains[1]; - pid_3dof_params.pitch_params.k_d = pitch_gains[2]; - - auto yaw_gains = this->get_parameter("PID_params.yaw").as_double_array(); - pid_3dof_params.yaw_params.k_p = yaw_gains[0]; - pid_3dof_params.yaw_params.k_i = yaw_gains[1]; - pid_3dof_params.yaw_params.k_d = yaw_gains[2]; - lqr_params.max_force = max_force; - lqr_params.interval = dt; - - control_params.control_type = this->get_parameter("Control_manager_settings.controller_type").as_int(); + //TODO(henrimha): parameter validation or in control manager + //Initialize Node node_settings.auto_start = this->get_parameter("Node_settings.auto_start").as_bool(); node_settings.reset_on_new_ref = this->get_parameter("Node_settings.reset_on_new_ref").as_bool(); - control_params.anti_overshoot = this->get_parameter("Control_manager_settings.anti_overshoot").as_bool(); + node_settings.publish_rate = this->get_parameter("Control_manager_settings.publish_rate").as_int(); node_settings.odometry_dropout_guard = this->get_parameter("Node_settings.odometry_dropout_guard").as_bool(); - lqr_params.Q = this->get_parameter("LQR_params.Q").as_double_array(); - lqr_params.R = this->get_parameter("LQR_params.R").as_double_array(); - lqr_params.inertia_matrix = this->get_parameter("physical.mass_matrix").as_double_array(); - lqr_params.D_low =this->get_parameter("dampening_matrix_low").as_double_array(); - lqr_params.D_high =this->get_parameter("dampening_matrix_high").as_double_array(); + + //Initialize control manager parameters + auto control_type = this->get_parameter("Control_manager_settings.controller_type").as_int(); + auto anti_overshoot = this->get_parameter("Control_manager_settings.anti_overshoot").as_bool(); + control_manager_params control_params(control_type, anti_overshoot, 1); + + + //Some general parameters + double max_force = this->get_parameter("Control_manager_settings.max_force").as_double(); + double dt = node_settings.publish_rate / 1000.0; // Convert ms to seconds - node_settings.publish_rate = this->get_parameter("Control_manager_settings.publish_rate").as_int(); + // Initialize 3DOF_PID params + auto surge_gains = this->get_parameter("3DOF_PID_params.surge").as_double_array(); + auto pitch_gains = this->get_parameter("3DOF_PID_params.pitch").as_double_array(); + auto yaw_gains = this->get_parameter("3DOF_PID_params.yaw").as_double_array(); + PID_3DOF_params pid_3dof_params(surge_gains, pitch_gains, yaw_gains, dt, max_force, -max_force); + + //Initialize LQR controller params + auto Q = this->get_parameter("LQR_params.Q").as_double_array(); + auto R = this->get_parameter("LQR_params.R").as_double_array(); + auto inertia_matrix = this->get_parameter("physical.mass_matrix").as_double_array(); + auto D_low = this->get_parameter("dampening_matrix_low").as_double_array(); + auto D_high = this->get_parameter("dampening_matrix_high").as_double_array(); + LQR_params lqr_params(Q, R, inertia_matrix, max_force, D_low, D_high, dt); + + //Initalize all the controllers in control manager control_manager_ptr = std::make_unique(control_params); control_manager_ptr->initialize_3DOF_controller(pid_3dof_params); control_manager_ptr->initialize_LQR_controller(lqr_params); From e29606d523b33755b3463449adcccf0227bafc4b Mon Sep 17 00:00:00 2001 From: Henrik Date: Fri, 10 Apr 2026 12:41:25 +0200 Subject: [PATCH 128/128] removed changes to config files --- auv_setup/config/robots/orca.yaml | 45 +++++++++++++++++++++++-------- 1 file changed, 34 insertions(+), 11 deletions(-) diff --git a/auv_setup/config/robots/orca.yaml b/auv_setup/config/robots/orca.yaml index 47039a4b9..93425c34f 100644 --- a/auv_setup/config/robots/orca.yaml +++ b/auv_setup/config/robots/orca.yaml @@ -17,8 +17,9 @@ /**: ros__parameters: physical: - center_of_mass: [0.0, 0.0, 0.035] # m (x,y,z) + center_of_mass: [0.0, 0.0, 0.035] # Position (x,y,z) in meters (M) mass_matrix : [ 30.0, 0.0, 0.0, 0.0, 0.0, 0.6, 0.0, 30.0, 0.0, 0.0, -0.6, 0.3, 0.0, 0.0, 30.0, 0.6, 0.3, 0.0, 0.0, 0.0, 0.6, 0.68, 0.0, 0.0, 0.0, -0.6, 0.3, 0.0, 3.32, 0.0, 0.6, 0.3, 0.0, 0.0, 0.0, 3.34] + # 6x6 mass_inertia_matrix propulsion: dofs: @@ -27,9 +28,7 @@ num: 3 thrusters: num: 8 - min: -100 - max: 100 - thruster_force_direction: [ + thruster_force_direction: [ # Direction of forces X,Y,Z, same logic as thruster_position thruster0 produces thrust in (X0,Y0,Z0) and ||(X0,Y0,Z0)|| = 1 0.70711, 0.00000, 0.00000, @@ -53,9 +52,9 @@ 0.00000, 1.00000, 1.00000, - 0.00000, - ] # Heave - thruster_position: [ + 0.00000, # Heave + ] + thruster_position: [ # Position (x0,x1 ... x7,y1,y2, ...,y7,z1,z2, ... ,z7) in meters (M). i.e thruster0 has position (x0,y0,z0) 0.41500, 0.28400, -0.31800, @@ -79,8 +78,14 @@ 0.07600, 0.08200, 0.08200, - 0.07600, - ] # z-position of thrusters + 0.07600, # z-position of thrusters + ] + + constraints: + max_force: 100.0 + min_force: -100.0 + input_matrix_weights: [1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0] + slack_matrix_weights: [2000.0,2000.0,2000.0,2000.0,2000.0,2000.0] rate_of_change: max: 1 # Maximum rate of change in newton per second for a thruster @@ -121,20 +126,38 @@ joy: "joy" pose: "pose" twist: "twist" + odom: "odom" operation_mode: "operation_mode" killswitch: "killswitch" - aruco_board_pose_camera: "aruco_board_pose_camera" + reference_pose: "reference_pose" + landmarks: "landmarks" waypoint: "waypoint" - waypoint_list: "waypoint_list" guidance: los: "guidance/los" dp: "guidance/dp" + dp_quat: "guidance/dp_quat" fsm: active_controller: "fsm/active_controller" + dvl_twist: "dvl/twist" + dvl_altitude: "dvl/altitude" + imu: "imu/data_raw" + pressure_sensor: "pressure_sensor" + gripper_servos: "gripper_servos" action_servers: reference_filter: "reference_filter" los: "los_guidance" + landmark_polling: "landmark_polling" + landmark_convergence: "landmark_convergence" + waypoint_manager: "waypoint_manager" + + services: + set_operation_mode: "set_operation_mode" + set_killswitch: "set_killswitch" + toggle_killswitch: "toggle_killswitch" + get_operation_mode: "get_operation_mode" + waypoint_addition: "waypoint_addition" + start_mission: "start_mission" fsm: docking: