From ab3bd3a7a2daf786e2e13d8ad71b405545d46fd4 Mon Sep 17 00:00:00 2001 From: Henrik Date: Wed, 24 Sep 2025 13:41:00 +0200 Subject: [PATCH 001/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] [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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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/107] 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; };