From ab3bd3a7a2daf786e2e13d8ad71b405545d46fd4 Mon Sep 17 00:00:00 2001 From: Henrik Date: Wed, 24 Sep 2025 13:41:00 +0200 Subject: [PATCH 001/290] 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/290] 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/290] 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/290] 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/290] 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/290] 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/290] 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/290] 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/290] 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/290] 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 16f8668dc66e2afc7558325dffec17cb1952fed4 Mon Sep 17 00:00:00 2001 From: akira Date: Wed, 15 Oct 2025 15:51:03 +0200 Subject: [PATCH 011/290] feat: add debug parameters and logging for PID controller calculations --- .../pid_controller_dp/pid_controller.hpp | 15 ++ .../pid_controller_dp/src/pid_controller.cpp | 32 ++- .../src/pid_controller_ros.cpp | 194 +++++++++++++++++- 3 files changed, 230 insertions(+), 11 deletions(-) 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..5f1d76b90 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 @@ -36,6 +36,21 @@ class PIDController { // @param dt: Time step void set_time_step(double dt); + // parameters for debug + types::Eta eta_error_debug; + types::Vector6d nu_d_debug; + types::Vector6d error_nu_debug; + types::Vector6d P_debug; + types::Vector6d I_debug; + types::Vector6d D_debug; + types::Vector6d tau_debug; + types::Matrix6x7d J_inv_debug; + + // debug gain + types::Matrix6d Kp_debug; + types::Matrix6d Ki_debug; + types::Matrix6d Kd_debug; + private: types::Matrix6d Kp_; types::Matrix6d Ki_; diff --git a/control/pid_controller_dp/src/pid_controller.cpp b/control/pid_controller_dp/src/pid_controller.cpp index 427c7dcdf..5fa89ec38 100644 --- a/control/pid_controller_dp/src/pid_controller.cpp +++ b/control/pid_controller_dp/src/pid_controller.cpp @@ -12,21 +12,37 @@ types::Vector6d PIDController::calculate_tau(const types::Eta& eta, const types::Eta& eta_d, const types::Nu& nu, const types::Eta& eta_dot_d) { - types::Eta error = error_eta(eta, eta_d); + types::Eta error = error_eta(eta, eta_d); // calculate eta error - types::Matrix6x7d J_inv = calculate_J_sudo_inv(error); + // debug + eta_error_debug = error; - types::Vector6d nu_d = J_inv * eta_dot_d.as_vector(); + types::Matrix6x7d J_inv = calculate_J_sudo_inv(error); // calculate J pseudo inverse + J_inv_debug = J_inv; + + types::Vector6d nu_d = J_inv * eta_dot_d.as_vector(); // calculate velocity + nu_d_debug = nu_d; - types::Vector6d error_nu = nu.as_vector() - nu_d; + types::Vector6d error_nu = nu.as_vector() - nu_d; // calculate vel error + error_nu_debug = error_nu; - types::Vector6d P = Kp_ * J_inv * error.as_vector(); + types::Vector6d P = Kp_ * J_inv * error.as_vector(); /// P term + P_debug = P; + Kp_debug = Kp_; - types::Vector6d I = Ki_ * J_inv * integral_; + types::Vector6d I = Ki_ * J_inv * integral_; // I term + I_debug = I; + Ki_debug = Ki_; + + types::Vector6d D = Kd_ * error_nu; // D term + D_debug = D; + Kd_debug = Kd_; + types::Vector6d tau = -clamp_values((P + I + D), -80.0, 80.0); + // types::Vector6d tau = -clamp_values((P), -80.0, 80.0); - types::Vector6d D = Kd_ * error_nu; - types::Vector6d tau = -clamp_values((P + I + D), -80.0, 80.0); + //debug: tau = 0 + // types::Vector6d tau = types::Vector6d::Zero(); integral_ = anti_windup(dt_, error, integral_); diff --git a/control/pid_controller_dp/src/pid_controller_ros.cpp b/control/pid_controller_dp/src/pid_controller_ros.cpp index 163d221b8..e095bf2d2 100644 --- a/control/pid_controller_dp/src/pid_controller_ros.cpp +++ b/control/pid_controller_dp/src/pid_controller_ros.cpp @@ -100,6 +100,136 @@ void PIDControllerNode::publish_tau() { types::Vector6d tau = pid_controller_.calculate_tau(eta_, eta_d_, nu_, eta_dot_d_); + // print for debug + RCLCPP_INFO_STREAM(this->get_logger(), "Tau: [" << tau(0) << ", " << tau(1) + << ", " << tau(2) << ", " + << tau(3) << ", " << tau(4) + << ", " << tau(5) << "]"); + // print debug + RCLCPP_INFO_STREAM(this->get_logger(), "Eta error: [" + << pid_controller_.eta_error_debug.pos(0) << ", " + << pid_controller_.eta_error_debug.pos(1) << ", " + << pid_controller_.eta_error_debug.pos(2) << ", " + << pid_controller_.eta_error_debug.ori.x() << ", " + << pid_controller_.eta_error_debug.ori.y() << ", " + << pid_controller_.eta_error_debug.ori.z() << ", " + << pid_controller_.eta_error_debug.ori.w() << "]"); + RCLCPP_INFO_STREAM(this->get_logger(), "Nu desired: [" + << pid_controller_.nu_d_debug(0) << ", " + << pid_controller_.nu_d_debug(1) << ", " + << pid_controller_.nu_d_debug(2) << ", " + << pid_controller_.nu_d_debug(3) << ", " + << pid_controller_.nu_d_debug(4) << ", " + << pid_controller_.nu_d_debug(5) << "]"); + RCLCPP_INFO_STREAM(this->get_logger(), "Error nu: [" + << pid_controller_.error_nu_debug(0) << ", " + << pid_controller_.error_nu_debug(1) << ", " + << pid_controller_.error_nu_debug(2) << ", " + << pid_controller_.error_nu_debug(3) << ", " + << pid_controller_.error_nu_debug(4) << ", " + << pid_controller_.error_nu_debug(5) << "]"); + RCLCPP_INFO_STREAM(this->get_logger(), "P term: [" + << pid_controller_.P_debug(0) << ", " + << pid_controller_.P_debug(1) << ", " + << pid_controller_.P_debug(2) << ", " + << pid_controller_.P_debug(3) << ", " + << pid_controller_.P_debug(4) << ", " + << pid_controller_.P_debug(5) << "]"); + RCLCPP_INFO_STREAM(this->get_logger(), "Kp: [" + << pid_controller_.Kp_debug(0,0) << ", " + << pid_controller_.Kp_debug(0,1) << ", " + << pid_controller_.Kp_debug(0,2) << ", " + << pid_controller_.Kp_debug(0,3) << ", " + << pid_controller_.Kp_debug(0,4) << ", " + << pid_controller_.Kp_debug(0,5) << "; " + << pid_controller_.Kp_debug(1,0) << ", " + << pid_controller_.Kp_debug(1,1) << ", " + << pid_controller_.Kp_debug(1,2) << ", " + << pid_controller_.Kp_debug(1,3) << ", " + << pid_controller_.Kp_debug(1,4) << ", " + << pid_controller_.Kp_debug(1,5) << "; " + << pid_controller_.Kp_debug(2,0) << ", " + << pid_controller_.Kp_debug(2,1) << ", " + << pid_controller_.Kp_debug(2,2) << ", " + << pid_controller_.Kp_debug(2,3) << ", " + << pid_controller_.Kp_debug(2,4) << ", " + << pid_controller_.Kp_debug(2,5) << "; " + << pid_controller_.Kp_debug(3,0) << ", " + << pid_controller_.Kp_debug(3,1) << ", " + << pid_controller_.Kp_debug(3,2) << ", " + << pid_controller_.Kp_debug(3,3) << ", " + << pid_controller_.Kp_debug(3,4) << ", " + << pid_controller_.Kp_debug(3,5) << "; " + << pid_controller_.Kp_debug(4,0) << ", " + << pid_controller_.Kp_debug(4,1) << ", " + << pid_controller_.Kp_debug(4,2) << ", " + << pid_controller_.Kp_debug(4,3) << ", " + << pid_controller_.Kp_debug(4,4) << ", " + << pid_controller_.Kp_debug(4,5) << "; " + << pid_controller_.Kp_debug(5,0) << ", " + << pid_controller_.Kp_debug(5,1) << ", " + << pid_controller_.Kp_debug(5,2) << ", " + << pid_controller_.Kp_debug(5,3) << ", " + << pid_controller_.Kp_debug(5,4) << ", " + << pid_controller_.Kp_debug(5,5) << "]"); + RCLCPP_INFO_STREAM(this->get_logger(), "I term: [" + << pid_controller_.I_debug(0) << ", " + << pid_controller_.I_debug(1) << ", " + << pid_controller_.I_debug(2) << ", " + << pid_controller_.I_debug(3) << ", " + << pid_controller_.I_debug(4) << ", " + << pid_controller_.I_debug(5) << "]"); + RCLCPP_INFO_STREAM(this->get_logger(), "D term: [" + << pid_controller_.D_debug(0) << ", " + << pid_controller_.D_debug(1) << ", " + << pid_controller_.D_debug(2) << ", " + << pid_controller_.D_debug(3) << ", " + << pid_controller_.D_debug(4) << ", " + << pid_controller_.D_debug(5) << "]"); + RCLCPP_INFO_STREAM(this->get_logger(), "J inv: [" + << pid_controller_.J_inv_debug(0,0) << ", " + << pid_controller_.J_inv_debug(0,1) << ", " + << pid_controller_.J_inv_debug(0,2) << ", " + << pid_controller_.J_inv_debug(0,3) << ", " + << pid_controller_.J_inv_debug(0,4) << ", " + << pid_controller_.J_inv_debug(0,5) << ", " + << pid_controller_.J_inv_debug(0,6) << "; " + << pid_controller_.J_inv_debug(1,0) << ", " + << pid_controller_.J_inv_debug(1,1) << ", " + << pid_controller_.J_inv_debug(1,2) << ", " + << pid_controller_.J_inv_debug(1,3) << ", " + << pid_controller_.J_inv_debug(1,4) << ", " + << pid_controller_.J_inv_debug(1,5) << ", " + << pid_controller_.J_inv_debug(1,6) << "; " + << pid_controller_.J_inv_debug(2,0) << ", " + << pid_controller_.J_inv_debug(2,1) << ", " + << pid_controller_.J_inv_debug(2,2) << ", " + << pid_controller_.J_inv_debug(2,3) << ", " + << pid_controller_.J_inv_debug(2,4) << ", " + << pid_controller_.J_inv_debug(2,5) << ", " + << pid_controller_.J_inv_debug(2,6) << "; " + << pid_controller_.J_inv_debug(3,0) << ", " + << pid_controller_.J_inv_debug(3,1) << ", " + << pid_controller_.J_inv_debug(3,2) << ", " + << pid_controller_.J_inv_debug(3,3) << ", " + << pid_controller_.J_inv_debug(3,4) << ", " + << pid_controller_.J_inv_debug(3,5) << ", " + << pid_controller_.J_inv_debug(3,6) << "; " + << pid_controller_.J_inv_debug(4,0) << ", " + << pid_controller_.J_inv_debug(4,1) << ", " + << pid_controller_.J_inv_debug(4,2) << ", " + << pid_controller_.J_inv_debug(4,3) << ", " + << pid_controller_.J_inv_debug(4,4) << ", " + << pid_controller_.J_inv_debug(4,5) << ", " + << pid_controller_.J_inv_debug(4,6) << "; " + << pid_controller_.J_inv_debug(5,0) << ", " + << pid_controller_.J_inv_debug(5,1) << ", " + << pid_controller_.J_inv_debug(5,2) << ", " + << pid_controller_.J_inv_debug(5,3) << ", " + << pid_controller_.J_inv_debug(5,4) << ", " + << pid_controller_.J_inv_debug(5,5) << ", " + << pid_controller_.J_inv_debug(5,6) << "]"); + geometry_msgs::msg::WrenchStamped tau_msg; tau_msg.header.stamp = this->now(); tau_msg.header.frame_id = "base_link"; @@ -121,13 +251,71 @@ void PIDControllerNode::set_pid_params() { this->declare_parameter>( "Kd", {0.1, 0.1, 0.1, 0.1, 0.1, 0.1}); + // this->declare_parameter>( + // "Kp", {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}); + // this->declare_parameter>( + // "Ki", {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}); + // this->declare_parameter>( + // "Kd", {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}); + std::vector Kp_vec = this->get_parameter("Kp").as_double_array(); std::vector Ki_vec = this->get_parameter("Ki").as_double_array(); std::vector Kd_vec = this->get_parameter("Kd").as_double_array(); - types::Matrix6d Kp_eigen = Eigen::Map(Kp_vec.data()); - types::Matrix6d Ki_eigen = Eigen::Map(Ki_vec.data()); - types::Matrix6d Kd_eigen = Eigen::Map(Kd_vec.data()); + // TODO use type::vector6d instead of std::vector + types::Vector6d Kp_vec_eigen(Kp_vec.data()); + types::Vector6d Ki_vec_eigen(Ki_vec.data()); + types::Vector6d Kd_vec_eigen(Kd_vec.data()); + + // print out for debug + RCLCPP_INFO_STREAM( + this->get_logger(), "Kp: [" << Kp_vec[0] << ", " << Kp_vec[1] << ", " + << Kp_vec[2] << ", " << Kp_vec[3] << ", " + << Kp_vec[4] << ", " << Kp_vec[5] + << "]"); + + RCLCPP_INFO_STREAM( + this->get_logger(), "Ki: [" << Ki_vec[0] << ", " << Ki_vec[1] << ", " + << Ki_vec[2] << ", " << Ki_vec[3] << ", " + << Ki_vec[4] << ", " << Ki_vec[5] + << "]"); + RCLCPP_INFO_STREAM( + this->get_logger(), "Kd: [" << Kd_vec[0] << ", " << Kd_vec[1] << ", " + << Kd_vec[2] << ", " << Kd_vec[3] << ", " + << Kd_vec[4] << ", " << Kd_vec[5] + << "]"); + + + // types::Matrix6d Kp_eigen = Eigen::Map(Kp_vec.data()); + // types::Matrix6d Ki_eigen = Eigen::Map(Ki_vec.data()); + // types::Matrix6d Kd_eigen = Eigen::Map(Kd_vec.data()); + + // TODO: use as diagnonal to avoid cross-coupling + types::Matrix6d Kp_eigen = Kp_vec_eigen.asDiagonal().toDenseMatrix(); + types::Matrix6d Ki_eigen = Ki_vec_eigen.asDiagonal().toDenseMatrix(); + types::Matrix6d Kd_eigen = Kd_vec_eigen.asDiagonal().toDenseMatrix(); + + // print out for debug + RCLCPP_INFO_STREAM( + this->get_logger(), "Kp_eigen: [" << Kp_eigen(0,0) << ", " << Kp_eigen(0,1) << ", " + << Kp_eigen(0,2) << ", " << Kp_eigen(0,3) << ", " + << Kp_eigen(0,4) << ", " << Kp_eigen(0,5) << "; " + << Kp_eigen(1,0) << ", " << Kp_eigen(1,1) << ", " + << Kp_eigen(1,2) << ", " << Kp_eigen(1,3) << ", " + << Kp_eigen(1,4) << ", " << Kp_eigen(1,5) << "; " + << Kp_eigen(2,0) << ", " << Kp_eigen(2,1) << ", " + << Kp_eigen(2,2) << ", " << Kp_eigen(2,3) << ", " + << Kp_eigen(2,4) << ", " << Kp_eigen(2,5) << "; " + << Kp_eigen(3,0) << ", " << Kp_eigen(3,1) << ", " + << Kp_eigen(3,2) << ", " << Kp_eigen(3,3) << ", " + << Kp_eigen(3,4) << ", " << Kp_eigen(3,5) << "; " + << Kp_eigen(4,0) << ", " << Kp_eigen(4,1) << ", " + << Kp_eigen(4,2) << ", " << Kp_eigen(4,3) << ", " + << Kp_eigen(4,4) << ", " << Kp_eigen(4,5) << "; " + << Kp_eigen(5,0) << ", " << Kp_eigen(5,1) << ", " + << Kp_eigen(5,2) << ", " << Kp_eigen(5,3) << ", " + << Kp_eigen(5,4) << ", " << Kp_eigen(5,5) + << "]"); pid_controller_.set_kp(Kp_eigen); pid_controller_.set_ki(Ki_eigen); From 2fb9d916e804f2275905e41cbd49010993198868 Mon Sep 17 00:00:00 2001 From: Anbit Date: Sun, 19 Oct 2025 15:56:52 +0200 Subject: [PATCH 012/290] 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 013/290] 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 d333822f90a84999f9cf023e3d8bd5eeaca9146f Mon Sep 17 00:00:00 2001 From: ppakr Date: Wed, 22 Oct 2025 11:26:46 +0200 Subject: [PATCH 014/290] feat: add rcl_interfaces dependency and parameter callback for dynamic reconfiguring --- control/pid_controller_dp/CMakeLists.txt | 2 + .../pid_controller_dp/pid_controller_ros.hpp | 7 + control/pid_controller_dp/package.xml | 1 + .../src/pid_controller_ros.cpp | 231 ++++-------------- 4 files changed, 57 insertions(+), 184 deletions(-) diff --git a/control/pid_controller_dp/CMakeLists.txt b/control/pid_controller_dp/CMakeLists.txt index fc9d851d9..f61e0d258 100644 --- a/control/pid_controller_dp/CMakeLists.txt +++ b/control/pid_controller_dp/CMakeLists.txt @@ -16,6 +16,7 @@ find_package(geometry_msgs REQUIRED) find_package(Eigen3 REQUIRED) find_package(tf2 REQUIRED) find_package(vortex_msgs REQUIRED) +find_package(rcl_interfaces REQUIRED) include_directories(include) @@ -34,6 +35,7 @@ ament_target_dependencies(pid_controller_node Eigen3 tf2 vortex_msgs + rcl_interfaces ) install(TARGETS 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..73e48372d 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 @@ -7,6 +7,7 @@ #include #include #include +#include #include #include #include @@ -55,6 +56,10 @@ class PIDControllerNode : public rclcpp::Node { void guidance_callback( const vortex_msgs::msg::ReferenceFilter::SharedPtr msg); + // TODO: parameter callback for dynamic reconfigure of PID gains + rcl_interfaces::msg::SetParametersResult parametersCallback( + const std::vector& parameters); + PIDController pid_controller_; rclcpp::Subscription::SharedPtr killswitch_sub_; @@ -93,6 +98,8 @@ class PIDControllerNode : public rclcpp::Node { bool killswitch_on_; std::string software_mode_; + + OnSetParametersCallbackHandle::SharedPtr callback_handle_; }; #endif diff --git a/control/pid_controller_dp/package.xml b/control/pid_controller_dp/package.xml index c1200d187..0337991d7 100644 --- a/control/pid_controller_dp/package.xml +++ b/control/pid_controller_dp/package.xml @@ -15,6 +15,7 @@ eigen tf2 vortex_msgs + rcl_interfaces ament_cmake diff --git a/control/pid_controller_dp/src/pid_controller_ros.cpp b/control/pid_controller_dp/src/pid_controller_ros.cpp index e095bf2d2..4f9023f0c 100644 --- a/control/pid_controller_dp/src/pid_controller_ros.cpp +++ b/control/pid_controller_dp/src/pid_controller_ros.cpp @@ -1,4 +1,5 @@ #include +#include #include #include "pid_controller_dp/pid_controller_conversions.hpp" #include "pid_controller_dp/pid_controller_utils.hpp" @@ -12,6 +13,9 @@ PIDControllerNode::PIDControllerNode() : Node("pid_controller_node") { tau_pub_timer_ = this->create_wall_timer( time_step_, std::bind(&PIDControllerNode::publish_tau, this)); set_pid_params(); + + callback_handle_ = this->add_on_set_parameters_callback(std::bind( + &PIDControllerNode::parametersCallback, this, std::placeholders::_1)); } void PIDControllerNode::set_subscribers_and_publisher() { @@ -100,136 +104,6 @@ void PIDControllerNode::publish_tau() { types::Vector6d tau = pid_controller_.calculate_tau(eta_, eta_d_, nu_, eta_dot_d_); - // print for debug - RCLCPP_INFO_STREAM(this->get_logger(), "Tau: [" << tau(0) << ", " << tau(1) - << ", " << tau(2) << ", " - << tau(3) << ", " << tau(4) - << ", " << tau(5) << "]"); - // print debug - RCLCPP_INFO_STREAM(this->get_logger(), "Eta error: [" - << pid_controller_.eta_error_debug.pos(0) << ", " - << pid_controller_.eta_error_debug.pos(1) << ", " - << pid_controller_.eta_error_debug.pos(2) << ", " - << pid_controller_.eta_error_debug.ori.x() << ", " - << pid_controller_.eta_error_debug.ori.y() << ", " - << pid_controller_.eta_error_debug.ori.z() << ", " - << pid_controller_.eta_error_debug.ori.w() << "]"); - RCLCPP_INFO_STREAM(this->get_logger(), "Nu desired: [" - << pid_controller_.nu_d_debug(0) << ", " - << pid_controller_.nu_d_debug(1) << ", " - << pid_controller_.nu_d_debug(2) << ", " - << pid_controller_.nu_d_debug(3) << ", " - << pid_controller_.nu_d_debug(4) << ", " - << pid_controller_.nu_d_debug(5) << "]"); - RCLCPP_INFO_STREAM(this->get_logger(), "Error nu: [" - << pid_controller_.error_nu_debug(0) << ", " - << pid_controller_.error_nu_debug(1) << ", " - << pid_controller_.error_nu_debug(2) << ", " - << pid_controller_.error_nu_debug(3) << ", " - << pid_controller_.error_nu_debug(4) << ", " - << pid_controller_.error_nu_debug(5) << "]"); - RCLCPP_INFO_STREAM(this->get_logger(), "P term: [" - << pid_controller_.P_debug(0) << ", " - << pid_controller_.P_debug(1) << ", " - << pid_controller_.P_debug(2) << ", " - << pid_controller_.P_debug(3) << ", " - << pid_controller_.P_debug(4) << ", " - << pid_controller_.P_debug(5) << "]"); - RCLCPP_INFO_STREAM(this->get_logger(), "Kp: [" - << pid_controller_.Kp_debug(0,0) << ", " - << pid_controller_.Kp_debug(0,1) << ", " - << pid_controller_.Kp_debug(0,2) << ", " - << pid_controller_.Kp_debug(0,3) << ", " - << pid_controller_.Kp_debug(0,4) << ", " - << pid_controller_.Kp_debug(0,5) << "; " - << pid_controller_.Kp_debug(1,0) << ", " - << pid_controller_.Kp_debug(1,1) << ", " - << pid_controller_.Kp_debug(1,2) << ", " - << pid_controller_.Kp_debug(1,3) << ", " - << pid_controller_.Kp_debug(1,4) << ", " - << pid_controller_.Kp_debug(1,5) << "; " - << pid_controller_.Kp_debug(2,0) << ", " - << pid_controller_.Kp_debug(2,1) << ", " - << pid_controller_.Kp_debug(2,2) << ", " - << pid_controller_.Kp_debug(2,3) << ", " - << pid_controller_.Kp_debug(2,4) << ", " - << pid_controller_.Kp_debug(2,5) << "; " - << pid_controller_.Kp_debug(3,0) << ", " - << pid_controller_.Kp_debug(3,1) << ", " - << pid_controller_.Kp_debug(3,2) << ", " - << pid_controller_.Kp_debug(3,3) << ", " - << pid_controller_.Kp_debug(3,4) << ", " - << pid_controller_.Kp_debug(3,5) << "; " - << pid_controller_.Kp_debug(4,0) << ", " - << pid_controller_.Kp_debug(4,1) << ", " - << pid_controller_.Kp_debug(4,2) << ", " - << pid_controller_.Kp_debug(4,3) << ", " - << pid_controller_.Kp_debug(4,4) << ", " - << pid_controller_.Kp_debug(4,5) << "; " - << pid_controller_.Kp_debug(5,0) << ", " - << pid_controller_.Kp_debug(5,1) << ", " - << pid_controller_.Kp_debug(5,2) << ", " - << pid_controller_.Kp_debug(5,3) << ", " - << pid_controller_.Kp_debug(5,4) << ", " - << pid_controller_.Kp_debug(5,5) << "]"); - RCLCPP_INFO_STREAM(this->get_logger(), "I term: [" - << pid_controller_.I_debug(0) << ", " - << pid_controller_.I_debug(1) << ", " - << pid_controller_.I_debug(2) << ", " - << pid_controller_.I_debug(3) << ", " - << pid_controller_.I_debug(4) << ", " - << pid_controller_.I_debug(5) << "]"); - RCLCPP_INFO_STREAM(this->get_logger(), "D term: [" - << pid_controller_.D_debug(0) << ", " - << pid_controller_.D_debug(1) << ", " - << pid_controller_.D_debug(2) << ", " - << pid_controller_.D_debug(3) << ", " - << pid_controller_.D_debug(4) << ", " - << pid_controller_.D_debug(5) << "]"); - RCLCPP_INFO_STREAM(this->get_logger(), "J inv: [" - << pid_controller_.J_inv_debug(0,0) << ", " - << pid_controller_.J_inv_debug(0,1) << ", " - << pid_controller_.J_inv_debug(0,2) << ", " - << pid_controller_.J_inv_debug(0,3) << ", " - << pid_controller_.J_inv_debug(0,4) << ", " - << pid_controller_.J_inv_debug(0,5) << ", " - << pid_controller_.J_inv_debug(0,6) << "; " - << pid_controller_.J_inv_debug(1,0) << ", " - << pid_controller_.J_inv_debug(1,1) << ", " - << pid_controller_.J_inv_debug(1,2) << ", " - << pid_controller_.J_inv_debug(1,3) << ", " - << pid_controller_.J_inv_debug(1,4) << ", " - << pid_controller_.J_inv_debug(1,5) << ", " - << pid_controller_.J_inv_debug(1,6) << "; " - << pid_controller_.J_inv_debug(2,0) << ", " - << pid_controller_.J_inv_debug(2,1) << ", " - << pid_controller_.J_inv_debug(2,2) << ", " - << pid_controller_.J_inv_debug(2,3) << ", " - << pid_controller_.J_inv_debug(2,4) << ", " - << pid_controller_.J_inv_debug(2,5) << ", " - << pid_controller_.J_inv_debug(2,6) << "; " - << pid_controller_.J_inv_debug(3,0) << ", " - << pid_controller_.J_inv_debug(3,1) << ", " - << pid_controller_.J_inv_debug(3,2) << ", " - << pid_controller_.J_inv_debug(3,3) << ", " - << pid_controller_.J_inv_debug(3,4) << ", " - << pid_controller_.J_inv_debug(3,5) << ", " - << pid_controller_.J_inv_debug(3,6) << "; " - << pid_controller_.J_inv_debug(4,0) << ", " - << pid_controller_.J_inv_debug(4,1) << ", " - << pid_controller_.J_inv_debug(4,2) << ", " - << pid_controller_.J_inv_debug(4,3) << ", " - << pid_controller_.J_inv_debug(4,4) << ", " - << pid_controller_.J_inv_debug(4,5) << ", " - << pid_controller_.J_inv_debug(4,6) << "; " - << pid_controller_.J_inv_debug(5,0) << ", " - << pid_controller_.J_inv_debug(5,1) << ", " - << pid_controller_.J_inv_debug(5,2) << ", " - << pid_controller_.J_inv_debug(5,3) << ", " - << pid_controller_.J_inv_debug(5,4) << ", " - << pid_controller_.J_inv_debug(5,5) << ", " - << pid_controller_.J_inv_debug(5,6) << "]"); - geometry_msgs::msg::WrenchStamped tau_msg; tau_msg.header.stamp = this->now(); tau_msg.header.frame_id = "base_link"; @@ -251,72 +125,18 @@ void PIDControllerNode::set_pid_params() { this->declare_parameter>( "Kd", {0.1, 0.1, 0.1, 0.1, 0.1, 0.1}); - // this->declare_parameter>( - // "Kp", {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}); - // this->declare_parameter>( - // "Ki", {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}); - // this->declare_parameter>( - // "Kd", {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}); - std::vector Kp_vec = this->get_parameter("Kp").as_double_array(); std::vector Ki_vec = this->get_parameter("Ki").as_double_array(); std::vector Kd_vec = this->get_parameter("Kd").as_double_array(); - // TODO use type::vector6d instead of std::vector types::Vector6d Kp_vec_eigen(Kp_vec.data()); types::Vector6d Ki_vec_eigen(Ki_vec.data()); types::Vector6d Kd_vec_eigen(Kd_vec.data()); - // print out for debug - RCLCPP_INFO_STREAM( - this->get_logger(), "Kp: [" << Kp_vec[0] << ", " << Kp_vec[1] << ", " - << Kp_vec[2] << ", " << Kp_vec[3] << ", " - << Kp_vec[4] << ", " << Kp_vec[5] - << "]"); - - RCLCPP_INFO_STREAM( - this->get_logger(), "Ki: [" << Ki_vec[0] << ", " << Ki_vec[1] << ", " - << Ki_vec[2] << ", " << Ki_vec[3] << ", " - << Ki_vec[4] << ", " << Ki_vec[5] - << "]"); - RCLCPP_INFO_STREAM( - this->get_logger(), "Kd: [" << Kd_vec[0] << ", " << Kd_vec[1] << ", " - << Kd_vec[2] << ", " << Kd_vec[3] << ", " - << Kd_vec[4] << ", " << Kd_vec[5] - << "]"); - - - // types::Matrix6d Kp_eigen = Eigen::Map(Kp_vec.data()); - // types::Matrix6d Ki_eigen = Eigen::Map(Ki_vec.data()); - // types::Matrix6d Kd_eigen = Eigen::Map(Kd_vec.data()); - - // TODO: use as diagnonal to avoid cross-coupling types::Matrix6d Kp_eigen = Kp_vec_eigen.asDiagonal().toDenseMatrix(); types::Matrix6d Ki_eigen = Ki_vec_eigen.asDiagonal().toDenseMatrix(); types::Matrix6d Kd_eigen = Kd_vec_eigen.asDiagonal().toDenseMatrix(); - // print out for debug - RCLCPP_INFO_STREAM( - this->get_logger(), "Kp_eigen: [" << Kp_eigen(0,0) << ", " << Kp_eigen(0,1) << ", " - << Kp_eigen(0,2) << ", " << Kp_eigen(0,3) << ", " - << Kp_eigen(0,4) << ", " << Kp_eigen(0,5) << "; " - << Kp_eigen(1,0) << ", " << Kp_eigen(1,1) << ", " - << Kp_eigen(1,2) << ", " << Kp_eigen(1,3) << ", " - << Kp_eigen(1,4) << ", " << Kp_eigen(1,5) << "; " - << Kp_eigen(2,0) << ", " << Kp_eigen(2,1) << ", " - << Kp_eigen(2,2) << ", " << Kp_eigen(2,3) << ", " - << Kp_eigen(2,4) << ", " << Kp_eigen(2,5) << "; " - << Kp_eigen(3,0) << ", " << Kp_eigen(3,1) << ", " - << Kp_eigen(3,2) << ", " << Kp_eigen(3,3) << ", " - << Kp_eigen(3,4) << ", " << Kp_eigen(3,5) << "; " - << Kp_eigen(4,0) << ", " << Kp_eigen(4,1) << ", " - << Kp_eigen(4,2) << ", " << Kp_eigen(4,3) << ", " - << Kp_eigen(4,4) << ", " << Kp_eigen(4,5) << "; " - << Kp_eigen(5,0) << ", " << Kp_eigen(5,1) << ", " - << Kp_eigen(5,2) << ", " << Kp_eigen(5,3) << ", " - << Kp_eigen(5,4) << ", " << Kp_eigen(5,5) - << "]"); - pid_controller_.set_kp(Kp_eigen); pid_controller_.set_ki(Ki_eigen); pid_controller_.set_kd(Kd_eigen); @@ -334,3 +154,46 @@ void PIDControllerNode::guidance_callback( Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()); } + +// TODO: set parameter functions +rcl_interfaces::msg::SetParametersResult PIDControllerNode::parametersCallback( + const std::vector& parameters) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + + bool kp_updated = false; + bool ki_updated = false; + bool kd_updated = false; + + types::Vector6d Kp_vec_eigen; + types::Vector6d Ki_vec_eigen; + types::Vector6d Kd_vec_eigen; + + // Only set the gains if the parameter update was successful + if (result.successful) { + if (kp_updated) { + types::Matrix6d Kp_eigen = + Kp_vec_eigen.asDiagonal().toDenseMatrix(); + pid_controller_.set_kp(Kp_eigen); + } + if (ki_updated) { + types::Matrix6d Ki_eigen = + Ki_vec_eigen.asDiagonal().toDenseMatrix(); + pid_controller_.set_ki(Ki_eigen); + } + if (kd_updated) { + types::Matrix6d Kd_eigen = + Kd_vec_eigen.asDiagonal().toDenseMatrix(); + pid_controller_.set_kd(Kd_eigen); + } + } + + // print + for (const auto& param : parameters) { + RCLCPP_INFO(this->get_logger(), "%s", param.get_name().c_str()); + RCLCPP_INFO(this->get_logger(), "%s", param.get_type_name().c_str()); + RCLCPP_INFO(this->get_logger(), "%s", param.value_to_string().c_str()); + } + return result; +} \ No newline at end of file From 9141eca39ffbd505c01b408bdfb08c2822b939bb Mon Sep 17 00:00:00 2001 From: Henrik Date: Wed, 22 Oct 2025 11:59:53 +0200 Subject: [PATCH 015/290] 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 a27998d5ab0f0a31c0873b19339bcd72c0ffdf1b Mon Sep 17 00:00:00 2001 From: ppakr Date: Sat, 25 Oct 2025 10:46:57 +0200 Subject: [PATCH 016/290] feat: update PID controller parameters --- .../pid_controller_dp/config/pid_params.yaml | 24 +- .../pid_controller_dp/pid_controller_ros.hpp | 2 + .../pid_controller_dp/src/pid_controller.cpp | 24 +- .../src/pid_controller_ros.cpp | 219 +++++++++++++++++- 4 files changed, 244 insertions(+), 25 deletions(-) diff --git a/control/pid_controller_dp/config/pid_params.yaml b/control/pid_controller_dp/config/pid_params.yaml index 2a77ffd59..5bd733a61 100644 --- a/control/pid_controller_dp/config/pid_params.yaml +++ b/control/pid_controller_dp/config/pid_params.yaml @@ -1,5 +1,23 @@ /**: ros__parameters: - Kp: [70.0, 70.0, 70.0, 12.0, 12.0, 12.0] - Ki: [2.0, 2.0, 2.0, 0.12, 0.12, 0.12] - Kd: [10.0, 10.0, 10.0, 4.0, 5.0, 4.0] + # Kp: [70.0, 70.0, 70.0, 12.0, 12.0, 12.0] + # Ki: [2.0, 2.0, 2.0, 0.12, 0.12, 0.12] + # Kd: [10.0, 10.0, 10.0, 4.0, 5.0, 4.0] + Kp_x: 10.0 + Kp_y: 0.0 + Kp_z: 0.0 + Kp_roll: 0.0 + Kp_pitch: 0.0 + Kp_yaw: 0.0 + Ki_x: 0.0 + Ki_y: 0.0 + Ki_z: 0.0 + Ki_roll: 0.0 + Ki_pitch: 0.0 + Ki_yaw: 0.0 + Kd_x: 0.0 + Kd_y: 0.0 + Kd_z: 0.0 + Kd_roll: 0.0 + Kd_pitch: 0.0 + Kd_yaw: 0.0 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 73e48372d..ba525a826 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 @@ -57,6 +57,8 @@ class PIDControllerNode : public rclcpp::Node { const vortex_msgs::msg::ReferenceFilter::SharedPtr msg); // TODO: parameter callback for dynamic reconfigure of PID gains + //@brief Callback function for parameter updates + // @param parameters: vector of parameters to be set rcl_interfaces::msg::SetParametersResult parametersCallback( const std::vector& parameters); diff --git a/control/pid_controller_dp/src/pid_controller.cpp b/control/pid_controller_dp/src/pid_controller.cpp index 5fa89ec38..a4099a0c2 100644 --- a/control/pid_controller_dp/src/pid_controller.cpp +++ b/control/pid_controller_dp/src/pid_controller.cpp @@ -12,37 +12,37 @@ types::Vector6d PIDController::calculate_tau(const types::Eta& eta, const types::Eta& eta_d, const types::Nu& nu, const types::Eta& eta_dot_d) { - types::Eta error = error_eta(eta, eta_d); // calculate eta error + types::Eta error = error_eta(eta, eta_d); // calculate eta error // debug eta_error_debug = error; - types::Matrix6x7d J_inv = calculate_J_sudo_inv(error); // calculate J pseudo inverse + types::Matrix6x7d J_inv = + calculate_J_sudo_inv(error); // calculate J pseudo inverse J_inv_debug = J_inv; - - types::Vector6d nu_d = J_inv * eta_dot_d.as_vector(); // calculate velocity + + types::Vector6d nu_d = J_inv * eta_dot_d.as_vector(); // calculate velocity nu_d_debug = nu_d; - types::Vector6d error_nu = nu.as_vector() - nu_d; // calculate vel error + types::Vector6d error_nu = nu.as_vector() - nu_d; // calculate vel error error_nu_debug = error_nu; - types::Vector6d P = Kp_ * J_inv * error.as_vector(); /// P term + types::Vector6d P = Kp_ * J_inv * error.as_vector(); /// P term P_debug = P; Kp_debug = Kp_; - types::Vector6d I = Ki_ * J_inv * integral_; // I term + types::Vector6d I = Ki_ * J_inv * integral_; // I term I_debug = I; Ki_debug = Ki_; - - types::Vector6d D = Kd_ * error_nu; // D term + + types::Vector6d D = Kd_ * error_nu; // D term D_debug = D; Kd_debug = Kd_; types::Vector6d tau = -clamp_values((P + I + D), -80.0, 80.0); // types::Vector6d tau = -clamp_values((P), -80.0, 80.0); - - //debug: tau = 0 - // types::Vector6d tau = types::Vector6d::Zero(); + // debug: tau = 0 + // types::Vector6d tau = types::Vector6d::Zero(); integral_ = anti_windup(dt_, error, integral_); diff --git a/control/pid_controller_dp/src/pid_controller_ros.cpp b/control/pid_controller_dp/src/pid_controller_ros.cpp index 4f9023f0c..597bbc2d7 100644 --- a/control/pid_controller_dp/src/pid_controller_ros.cpp +++ b/control/pid_controller_dp/src/pid_controller_ros.cpp @@ -103,6 +103,137 @@ void PIDControllerNode::publish_tau() { types::Vector6d tau = pid_controller_.calculate_tau(eta_, eta_d_, nu_, eta_dot_d_); + // print for debug + RCLCPP_INFO_STREAM(this->get_logger(), "Tau: [" << tau(0) << ", " << tau(1) + << ", " << tau(2) << ", " + << tau(3) << ", " << tau(4) + << ", " << tau(5) << "]"); + // print debug + RCLCPP_INFO_STREAM(this->get_logger(), + "Eta error: [" + << pid_controller_.eta_error_debug.pos(0) << ", " + << pid_controller_.eta_error_debug.pos(1) << ", " + << pid_controller_.eta_error_debug.pos(2) << ", " + << pid_controller_.eta_error_debug.ori.x() << ", " + << pid_controller_.eta_error_debug.ori.y() << ", " + << pid_controller_.eta_error_debug.ori.z() << ", " + << pid_controller_.eta_error_debug.ori.w() << "]"); + RCLCPP_INFO_STREAM(this->get_logger(), + "Nu desired: [" << pid_controller_.nu_d_debug(0) << ", " + << pid_controller_.nu_d_debug(1) << ", " + << pid_controller_.nu_d_debug(2) << ", " + << pid_controller_.nu_d_debug(3) << ", " + << pid_controller_.nu_d_debug(4) << ", " + << pid_controller_.nu_d_debug(5) << "]"); + RCLCPP_INFO_STREAM(this->get_logger(), + "Error nu: [" + << pid_controller_.error_nu_debug(0) << ", " + << pid_controller_.error_nu_debug(1) << ", " + << pid_controller_.error_nu_debug(2) << ", " + << pid_controller_.error_nu_debug(3) << ", " + << pid_controller_.error_nu_debug(4) << ", " + << pid_controller_.error_nu_debug(5) << "]"); + RCLCPP_INFO_STREAM(this->get_logger(), + "P term: [" << pid_controller_.P_debug(0) << ", " + << pid_controller_.P_debug(1) << ", " + << pid_controller_.P_debug(2) << ", " + << pid_controller_.P_debug(3) << ", " + << pid_controller_.P_debug(4) << ", " + << pid_controller_.P_debug(5) << "]"); + RCLCPP_INFO_STREAM(this->get_logger(), + "Kp: [" << pid_controller_.Kp_debug(0, 0) << ", " + << pid_controller_.Kp_debug(0, 1) << ", " + << pid_controller_.Kp_debug(0, 2) << ", " + << pid_controller_.Kp_debug(0, 3) << ", " + << pid_controller_.Kp_debug(0, 4) << ", " + << pid_controller_.Kp_debug(0, 5) << "; " + << pid_controller_.Kp_debug(1, 0) << ", " + << pid_controller_.Kp_debug(1, 1) << ", " + << pid_controller_.Kp_debug(1, 2) << ", " + << pid_controller_.Kp_debug(1, 3) << ", " + << pid_controller_.Kp_debug(1, 4) << ", " + << pid_controller_.Kp_debug(1, 5) << "; " + << pid_controller_.Kp_debug(2, 0) << ", " + << pid_controller_.Kp_debug(2, 1) << ", " + << pid_controller_.Kp_debug(2, 2) << ", " + << pid_controller_.Kp_debug(2, 3) << ", " + << pid_controller_.Kp_debug(2, 4) << ", " + << pid_controller_.Kp_debug(2, 5) << "; " + << pid_controller_.Kp_debug(3, 0) << ", " + << pid_controller_.Kp_debug(3, 1) << ", " + << pid_controller_.Kp_debug(3, 2) << ", " + << pid_controller_.Kp_debug(3, 3) << ", " + << pid_controller_.Kp_debug(3, 4) << ", " + << pid_controller_.Kp_debug(3, 5) << "; " + << pid_controller_.Kp_debug(4, 0) << ", " + << pid_controller_.Kp_debug(4, 1) << ", " + << pid_controller_.Kp_debug(4, 2) << ", " + << pid_controller_.Kp_debug(4, 3) << ", " + << pid_controller_.Kp_debug(4, 4) << ", " + << pid_controller_.Kp_debug(4, 5) << "; " + << pid_controller_.Kp_debug(5, 0) << ", " + << pid_controller_.Kp_debug(5, 1) << ", " + << pid_controller_.Kp_debug(5, 2) << ", " + << pid_controller_.Kp_debug(5, 3) << ", " + << pid_controller_.Kp_debug(5, 4) << ", " + << pid_controller_.Kp_debug(5, 5) << "]"); + RCLCPP_INFO_STREAM(this->get_logger(), + "I term: [" << pid_controller_.I_debug(0) << ", " + << pid_controller_.I_debug(1) << ", " + << pid_controller_.I_debug(2) << ", " + << pid_controller_.I_debug(3) << ", " + << pid_controller_.I_debug(4) << ", " + << pid_controller_.I_debug(5) << "]"); + RCLCPP_INFO_STREAM(this->get_logger(), + "D term: [" << pid_controller_.D_debug(0) << ", " + << pid_controller_.D_debug(1) << ", " + << pid_controller_.D_debug(2) << ", " + << pid_controller_.D_debug(3) << ", " + << pid_controller_.D_debug(4) << ", " + << pid_controller_.D_debug(5) << "]"); + RCLCPP_INFO_STREAM(this->get_logger(), + "J inv: [" << pid_controller_.J_inv_debug(0, 0) << ", " + << pid_controller_.J_inv_debug(0, 1) << ", " + << pid_controller_.J_inv_debug(0, 2) << ", " + << pid_controller_.J_inv_debug(0, 3) << ", " + << pid_controller_.J_inv_debug(0, 4) << ", " + << pid_controller_.J_inv_debug(0, 5) << ", " + << pid_controller_.J_inv_debug(0, 6) << "; " + << pid_controller_.J_inv_debug(1, 0) << ", " + << pid_controller_.J_inv_debug(1, 1) << ", " + << pid_controller_.J_inv_debug(1, 2) << ", " + << pid_controller_.J_inv_debug(1, 3) << ", " + << pid_controller_.J_inv_debug(1, 4) << ", " + << pid_controller_.J_inv_debug(1, 5) << ", " + << pid_controller_.J_inv_debug(1, 6) << "; " + << pid_controller_.J_inv_debug(2, 0) << ", " + << pid_controller_.J_inv_debug(2, 1) << ", " + << pid_controller_.J_inv_debug(2, 2) << ", " + << pid_controller_.J_inv_debug(2, 3) << ", " + << pid_controller_.J_inv_debug(2, 4) << ", " + << pid_controller_.J_inv_debug(2, 5) << ", " + << pid_controller_.J_inv_debug(2, 6) << "; " + << pid_controller_.J_inv_debug(3, 0) << ", " + << pid_controller_.J_inv_debug(3, 1) << ", " + << pid_controller_.J_inv_debug(3, 2) << ", " + << pid_controller_.J_inv_debug(3, 3) << ", " + << pid_controller_.J_inv_debug(3, 4) << ", " + << pid_controller_.J_inv_debug(3, 5) << ", " + << pid_controller_.J_inv_debug(3, 6) << "; " + << pid_controller_.J_inv_debug(4, 0) << ", " + << pid_controller_.J_inv_debug(4, 1) << ", " + << pid_controller_.J_inv_debug(4, 2) << ", " + << pid_controller_.J_inv_debug(4, 3) << ", " + << pid_controller_.J_inv_debug(4, 4) << ", " + << pid_controller_.J_inv_debug(4, 5) << ", " + << pid_controller_.J_inv_debug(4, 6) << "; " + << pid_controller_.J_inv_debug(5, 0) << ", " + << pid_controller_.J_inv_debug(5, 1) << ", " + << pid_controller_.J_inv_debug(5, 2) << ", " + << pid_controller_.J_inv_debug(5, 3) << ", " + << pid_controller_.J_inv_debug(5, 4) << ", " + << pid_controller_.J_inv_debug(5, 5) << ", " + << pid_controller_.J_inv_debug(5, 6) << "]"); geometry_msgs::msg::WrenchStamped tau_msg; tau_msg.header.stamp = this->now(); @@ -118,16 +249,62 @@ void PIDControllerNode::publish_tau() { } void PIDControllerNode::set_pid_params() { - this->declare_parameter>( - "Kp", {1.0, 1.0, 1.0, 1.0, 1.0, 1.0}); - this->declare_parameter>( - "Ki", {0.1, 0.1, 0.1, 0.1, 0.1, 0.1}); - this->declare_parameter>( - "Kd", {0.1, 0.1, 0.1, 0.1, 0.1, 0.1}); - - std::vector Kp_vec = this->get_parameter("Kp").as_double_array(); - std::vector Ki_vec = this->get_parameter("Ki").as_double_array(); - std::vector Kd_vec = this->get_parameter("Kd").as_double_array(); + // TODO: edit parameter declaration + this->declare_parameter("Kp_x", 1.0); + this->declare_parameter("Kp_y", 1.0); + this->declare_parameter("Kp_z", 1.0); + this->declare_parameter("Kp_roll", 1.0); + this->declare_parameter("Kp_pitch", 1.0); + this->declare_parameter("Kp_yaw", 1.0); + this->declare_parameter("Ki_x", 0.1); + this->declare_parameter("Ki_y", 0.1); + this->declare_parameter("Ki_z", 0.1); + this->declare_parameter("Ki_roll", 0.1); + this->declare_parameter("Ki_pitch", 0.1); + this->declare_parameter("Ki_yaw", 0.1); + this->declare_parameter("Kd_x", 0.1); + this->declare_parameter("Kd_y", 0.1); + this->declare_parameter("Kd_z", 0.1); + this->declare_parameter("Kd_roll", 0.1); + this->declare_parameter("Kd_pitch", 0.1); + this->declare_parameter("Kd_yaw", 0.1); + + // this->declare_parameter>( + // "Kp", {1.0, 1.0, 1.0, 1.0, 1.0, 1.0}); + // this->declare_parameter>( + // "Ki", {0.1, 0.1, 0.1, 0.1, 0.1, 0.1}); + // this->declare_parameter>( + // "Kd", {0.1, 0.1, 0.1, 0.1, 0.1, 0.1}); + + // std::vector Kp_vec = this->get_parameter("Kp").as_double_array(); + // std::vector Ki_vec = this->get_parameter("Ki").as_double_array(); + // std::vector Kd_vec = this->get_parameter("Kd").as_double_array(); + + // TODO: construct vector from parameters + std::vector Kp_vec = { + this->get_parameter("Kp_x").as_double(), + this->get_parameter("Kp_y").as_double(), + this->get_parameter("Kp_z").as_double(), + this->get_parameter("Kp_roll").as_double(), + this->get_parameter("Kp_pitch").as_double(), + this->get_parameter("Kp_yaw").as_double(), + }; + std::vector Ki_vec = { + this->get_parameter("Ki_x").as_double(), + this->get_parameter("Ki_y").as_double(), + this->get_parameter("Ki_z").as_double(), + this->get_parameter("Ki_roll").as_double(), + this->get_parameter("Ki_pitch").as_double(), + this->get_parameter("Ki_yaw").as_double(), + }; + std::vector Kd_vec = { + this->get_parameter("Kd_x").as_double(), + this->get_parameter("Kd_y").as_double(), + this->get_parameter("Kd_z").as_double(), + this->get_parameter("Kd_roll").as_double(), + this->get_parameter("Kd_pitch").as_double(), + this->get_parameter("Kd_yaw").as_double(), + }; types::Vector6d Kp_vec_eigen(Kp_vec.data()); types::Vector6d Ki_vec_eigen(Ki_vec.data()); @@ -137,6 +314,28 @@ void PIDControllerNode::set_pid_params() { types::Matrix6d Ki_eigen = Ki_vec_eigen.asDiagonal().toDenseMatrix(); types::Matrix6d Kd_eigen = Kd_vec_eigen.asDiagonal().toDenseMatrix(); + // print out for debug + RCLCPP_INFO_STREAM(this->get_logger(), + "Kp_eigen: [" + << Kp_eigen(0, 0) << ", " << Kp_eigen(0, 1) << ", " + << Kp_eigen(0, 2) << ", " << Kp_eigen(0, 3) << ", " + << Kp_eigen(0, 4) << ", " << Kp_eigen(0, 5) << "; " + << Kp_eigen(1, 0) << ", " << Kp_eigen(1, 1) << ", " + << Kp_eigen(1, 2) << ", " << Kp_eigen(1, 3) << ", " + << Kp_eigen(1, 4) << ", " << Kp_eigen(1, 5) << "; " + << Kp_eigen(2, 0) << ", " << Kp_eigen(2, 1) << ", " + << Kp_eigen(2, 2) << ", " << Kp_eigen(2, 3) << ", " + << Kp_eigen(2, 4) << ", " << Kp_eigen(2, 5) << "; " + << Kp_eigen(3, 0) << ", " << Kp_eigen(3, 1) << ", " + << Kp_eigen(3, 2) << ", " << Kp_eigen(3, 3) << ", " + << Kp_eigen(3, 4) << ", " << Kp_eigen(3, 5) << "; " + << Kp_eigen(4, 0) << ", " << Kp_eigen(4, 1) << ", " + << Kp_eigen(4, 2) << ", " << Kp_eigen(4, 3) << ", " + << Kp_eigen(4, 4) << ", " << Kp_eigen(4, 5) << "; " + << Kp_eigen(5, 0) << ", " << Kp_eigen(5, 1) << ", " + << Kp_eigen(5, 2) << ", " << Kp_eigen(5, 3) << ", " + << Kp_eigen(5, 4) << ", " << Kp_eigen(5, 5) << "]"); + pid_controller_.set_kp(Kp_eigen); pid_controller_.set_ki(Ki_eigen); pid_controller_.set_kd(Kd_eigen); From 064adef0ab5fbfdcbc4af38df6f85c3d71b607ca Mon Sep 17 00:00:00 2001 From: Anbit Date: Sun, 26 Oct 2025 13:52:33 +0100 Subject: [PATCH 017/290] 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 018/290] 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 019/290] 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 020/290] 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 021/290] 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 022/290] 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 023/290] 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 71df096965f1891ddd88a4ca76d9070f5e94767b Mon Sep 17 00:00:00 2001 From: ppakr Date: Sun, 2 Nov 2025 12:25:21 +0100 Subject: [PATCH 024/290] feat: add parameter handling with individual gains for x, y, z, roll, pitch, and yaw --- .../src/pid_controller_ros.cpp | 389 ++++++++++++------ 1 file changed, 258 insertions(+), 131 deletions(-) diff --git a/control/pid_controller_dp/src/pid_controller_ros.cpp b/control/pid_controller_dp/src/pid_controller_ros.cpp index 597bbc2d7..3f6f1cb6f 100644 --- a/control/pid_controller_dp/src/pid_controller_ros.cpp +++ b/control/pid_controller_dp/src/pid_controller_ros.cpp @@ -109,131 +109,180 @@ void PIDControllerNode::publish_tau() { << tau(3) << ", " << tau(4) << ", " << tau(5) << "]"); // print debug - RCLCPP_INFO_STREAM(this->get_logger(), - "Eta error: [" - << pid_controller_.eta_error_debug.pos(0) << ", " - << pid_controller_.eta_error_debug.pos(1) << ", " - << pid_controller_.eta_error_debug.pos(2) << ", " - << pid_controller_.eta_error_debug.ori.x() << ", " - << pid_controller_.eta_error_debug.ori.y() << ", " - << pid_controller_.eta_error_debug.ori.z() << ", " - << pid_controller_.eta_error_debug.ori.w() << "]"); - RCLCPP_INFO_STREAM(this->get_logger(), - "Nu desired: [" << pid_controller_.nu_d_debug(0) << ", " - << pid_controller_.nu_d_debug(1) << ", " - << pid_controller_.nu_d_debug(2) << ", " - << pid_controller_.nu_d_debug(3) << ", " - << pid_controller_.nu_d_debug(4) << ", " - << pid_controller_.nu_d_debug(5) << "]"); - RCLCPP_INFO_STREAM(this->get_logger(), - "Error nu: [" - << pid_controller_.error_nu_debug(0) << ", " - << pid_controller_.error_nu_debug(1) << ", " - << pid_controller_.error_nu_debug(2) << ", " - << pid_controller_.error_nu_debug(3) << ", " - << pid_controller_.error_nu_debug(4) << ", " - << pid_controller_.error_nu_debug(5) << "]"); - RCLCPP_INFO_STREAM(this->get_logger(), - "P term: [" << pid_controller_.P_debug(0) << ", " - << pid_controller_.P_debug(1) << ", " - << pid_controller_.P_debug(2) << ", " - << pid_controller_.P_debug(3) << ", " - << pid_controller_.P_debug(4) << ", " - << pid_controller_.P_debug(5) << "]"); - RCLCPP_INFO_STREAM(this->get_logger(), - "Kp: [" << pid_controller_.Kp_debug(0, 0) << ", " - << pid_controller_.Kp_debug(0, 1) << ", " - << pid_controller_.Kp_debug(0, 2) << ", " - << pid_controller_.Kp_debug(0, 3) << ", " - << pid_controller_.Kp_debug(0, 4) << ", " - << pid_controller_.Kp_debug(0, 5) << "; " - << pid_controller_.Kp_debug(1, 0) << ", " - << pid_controller_.Kp_debug(1, 1) << ", " - << pid_controller_.Kp_debug(1, 2) << ", " - << pid_controller_.Kp_debug(1, 3) << ", " - << pid_controller_.Kp_debug(1, 4) << ", " - << pid_controller_.Kp_debug(1, 5) << "; " - << pid_controller_.Kp_debug(2, 0) << ", " - << pid_controller_.Kp_debug(2, 1) << ", " - << pid_controller_.Kp_debug(2, 2) << ", " - << pid_controller_.Kp_debug(2, 3) << ", " - << pid_controller_.Kp_debug(2, 4) << ", " - << pid_controller_.Kp_debug(2, 5) << "; " - << pid_controller_.Kp_debug(3, 0) << ", " - << pid_controller_.Kp_debug(3, 1) << ", " - << pid_controller_.Kp_debug(3, 2) << ", " - << pid_controller_.Kp_debug(3, 3) << ", " - << pid_controller_.Kp_debug(3, 4) << ", " - << pid_controller_.Kp_debug(3, 5) << "; " - << pid_controller_.Kp_debug(4, 0) << ", " - << pid_controller_.Kp_debug(4, 1) << ", " - << pid_controller_.Kp_debug(4, 2) << ", " - << pid_controller_.Kp_debug(4, 3) << ", " - << pid_controller_.Kp_debug(4, 4) << ", " - << pid_controller_.Kp_debug(4, 5) << "; " - << pid_controller_.Kp_debug(5, 0) << ", " - << pid_controller_.Kp_debug(5, 1) << ", " - << pid_controller_.Kp_debug(5, 2) << ", " - << pid_controller_.Kp_debug(5, 3) << ", " - << pid_controller_.Kp_debug(5, 4) << ", " - << pid_controller_.Kp_debug(5, 5) << "]"); - RCLCPP_INFO_STREAM(this->get_logger(), - "I term: [" << pid_controller_.I_debug(0) << ", " - << pid_controller_.I_debug(1) << ", " - << pid_controller_.I_debug(2) << ", " - << pid_controller_.I_debug(3) << ", " - << pid_controller_.I_debug(4) << ", " - << pid_controller_.I_debug(5) << "]"); - RCLCPP_INFO_STREAM(this->get_logger(), - "D term: [" << pid_controller_.D_debug(0) << ", " - << pid_controller_.D_debug(1) << ", " - << pid_controller_.D_debug(2) << ", " - << pid_controller_.D_debug(3) << ", " - << pid_controller_.D_debug(4) << ", " - << pid_controller_.D_debug(5) << "]"); - RCLCPP_INFO_STREAM(this->get_logger(), - "J inv: [" << pid_controller_.J_inv_debug(0, 0) << ", " - << pid_controller_.J_inv_debug(0, 1) << ", " - << pid_controller_.J_inv_debug(0, 2) << ", " - << pid_controller_.J_inv_debug(0, 3) << ", " - << pid_controller_.J_inv_debug(0, 4) << ", " - << pid_controller_.J_inv_debug(0, 5) << ", " - << pid_controller_.J_inv_debug(0, 6) << "; " - << pid_controller_.J_inv_debug(1, 0) << ", " - << pid_controller_.J_inv_debug(1, 1) << ", " - << pid_controller_.J_inv_debug(1, 2) << ", " - << pid_controller_.J_inv_debug(1, 3) << ", " - << pid_controller_.J_inv_debug(1, 4) << ", " - << pid_controller_.J_inv_debug(1, 5) << ", " - << pid_controller_.J_inv_debug(1, 6) << "; " - << pid_controller_.J_inv_debug(2, 0) << ", " - << pid_controller_.J_inv_debug(2, 1) << ", " - << pid_controller_.J_inv_debug(2, 2) << ", " - << pid_controller_.J_inv_debug(2, 3) << ", " - << pid_controller_.J_inv_debug(2, 4) << ", " - << pid_controller_.J_inv_debug(2, 5) << ", " - << pid_controller_.J_inv_debug(2, 6) << "; " - << pid_controller_.J_inv_debug(3, 0) << ", " - << pid_controller_.J_inv_debug(3, 1) << ", " - << pid_controller_.J_inv_debug(3, 2) << ", " - << pid_controller_.J_inv_debug(3, 3) << ", " - << pid_controller_.J_inv_debug(3, 4) << ", " - << pid_controller_.J_inv_debug(3, 5) << ", " - << pid_controller_.J_inv_debug(3, 6) << "; " - << pid_controller_.J_inv_debug(4, 0) << ", " - << pid_controller_.J_inv_debug(4, 1) << ", " - << pid_controller_.J_inv_debug(4, 2) << ", " - << pid_controller_.J_inv_debug(4, 3) << ", " - << pid_controller_.J_inv_debug(4, 4) << ", " - << pid_controller_.J_inv_debug(4, 5) << ", " - << pid_controller_.J_inv_debug(4, 6) << "; " - << pid_controller_.J_inv_debug(5, 0) << ", " - << pid_controller_.J_inv_debug(5, 1) << ", " - << pid_controller_.J_inv_debug(5, 2) << ", " - << pid_controller_.J_inv_debug(5, 3) << ", " - << pid_controller_.J_inv_debug(5, 4) << ", " - << pid_controller_.J_inv_debug(5, 5) << ", " - << pid_controller_.J_inv_debug(5, 6) << "]"); + // RCLCPP_INFO_STREAM(this->get_logger(), + // "Eta error: [" + // << pid_controller_.eta_error_debug.pos(0) << ", " + // << pid_controller_.eta_error_debug.pos(1) << ", " + // << pid_controller_.eta_error_debug.pos(2) << ", " + // << pid_controller_.eta_error_debug.ori.x() << ", " + // << pid_controller_.eta_error_debug.ori.y() << ", " + // << pid_controller_.eta_error_debug.ori.z() << ", " + // << pid_controller_.eta_error_debug.ori.w() << + // "]"); + // RCLCPP_INFO_STREAM(this->get_logger(), + // "Nu desired: [" << pid_controller_.nu_d_debug(0) << ", + // " + // << pid_controller_.nu_d_debug(1) << ", + // " + // << pid_controller_.nu_d_debug(2) << ", + // " + // << pid_controller_.nu_d_debug(3) << ", + // " + // << pid_controller_.nu_d_debug(4) << ", + // " + // << pid_controller_.nu_d_debug(5) << + // "]"); + // RCLCPP_INFO_STREAM(this->get_logger(), + // "Error nu: [" + // << pid_controller_.error_nu_debug(0) << ", " + // << pid_controller_.error_nu_debug(1) << ", " + // << pid_controller_.error_nu_debug(2) << ", " + // << pid_controller_.error_nu_debug(3) << ", " + // << pid_controller_.error_nu_debug(4) << ", " + // << pid_controller_.error_nu_debug(5) << "]"); + // RCLCPP_INFO_STREAM(this->get_logger(), + // "P term: [" << pid_controller_.P_debug(0) << ", " + // << pid_controller_.P_debug(1) << ", " + // << pid_controller_.P_debug(2) << ", " + // << pid_controller_.P_debug(3) << ", " + // << pid_controller_.P_debug(4) << ", " + // << pid_controller_.P_debug(5) << "]"); + // RCLCPP_INFO_STREAM(this->get_logger(), + // "Kp: [" << pid_controller_.Kp_debug(0, 0) << ", " + // << pid_controller_.Kp_debug(0, 1) << ", " + // << pid_controller_.Kp_debug(0, 2) << ", " + // << pid_controller_.Kp_debug(0, 3) << ", " + // << pid_controller_.Kp_debug(0, 4) << ", " + // << pid_controller_.Kp_debug(0, 5) << "; " + // << pid_controller_.Kp_debug(1, 0) << ", " + // << pid_controller_.Kp_debug(1, 1) << ", " + // << pid_controller_.Kp_debug(1, 2) << ", " + // << pid_controller_.Kp_debug(1, 3) << ", " + // << pid_controller_.Kp_debug(1, 4) << ", " + // << pid_controller_.Kp_debug(1, 5) << "; " + // << pid_controller_.Kp_debug(2, 0) << ", " + // << pid_controller_.Kp_debug(2, 1) << ", " + // << pid_controller_.Kp_debug(2, 2) << ", " + // << pid_controller_.Kp_debug(2, 3) << ", " + // << pid_controller_.Kp_debug(2, 4) << ", " + // << pid_controller_.Kp_debug(2, 5) << "; " + // << pid_controller_.Kp_debug(3, 0) << ", " + // << pid_controller_.Kp_debug(3, 1) << ", " + // << pid_controller_.Kp_debug(3, 2) << ", " + // << pid_controller_.Kp_debug(3, 3) << ", " + // << pid_controller_.Kp_debug(3, 4) << ", " + // << pid_controller_.Kp_debug(3, 5) << "; " + // << pid_controller_.Kp_debug(4, 0) << ", " + // << pid_controller_.Kp_debug(4, 1) << ", " + // << pid_controller_.Kp_debug(4, 2) << ", " + // << pid_controller_.Kp_debug(4, 3) << ", " + // << pid_controller_.Kp_debug(4, 4) << ", " + // << pid_controller_.Kp_debug(4, 5) << "; " + // << pid_controller_.Kp_debug(5, 0) << ", " + // << pid_controller_.Kp_debug(5, 1) << ", " + // << pid_controller_.Kp_debug(5, 2) << ", " + // << pid_controller_.Kp_debug(5, 3) << ", " + // << pid_controller_.Kp_debug(5, 4) << ", " + // << pid_controller_.Kp_debug(5, 5) << "]"); + // RCLCPP_INFO_STREAM(this->get_logger(), + // "I term: [" << pid_controller_.I_debug(0) << ", " + // << pid_controller_.I_debug(1) << ", " + // << pid_controller_.I_debug(2) << ", " + // << pid_controller_.I_debug(3) << ", " + // << pid_controller_.I_debug(4) << ", " + // << pid_controller_.I_debug(5) << "]"); + // RCLCPP_INFO_STREAM(this->get_logger(), + // "D term: [" << pid_controller_.D_debug(0) << ", " + // << pid_controller_.D_debug(1) << ", " + // << pid_controller_.D_debug(2) << ", " + // << pid_controller_.D_debug(3) << ", " + // << pid_controller_.D_debug(4) << ", " + // << pid_controller_.D_debug(5) << "]"); + // RCLCPP_INFO_STREAM(this->get_logger(), + // "J inv: [" << pid_controller_.J_inv_debug(0, 0) << ", + // " + // << pid_controller_.J_inv_debug(0, 1) << ", + // " + // << pid_controller_.J_inv_debug(0, 2) << ", + // " + // << pid_controller_.J_inv_debug(0, 3) << ", + // " + // << pid_controller_.J_inv_debug(0, 4) << ", + // " + // << pid_controller_.J_inv_debug(0, 5) << ", + // " + // << pid_controller_.J_inv_debug(0, 6) << "; + // " + // << pid_controller_.J_inv_debug(1, 0) << ", + // " + // << pid_controller_.J_inv_debug(1, 1) << ", + // " + // << pid_controller_.J_inv_debug(1, 2) << ", + // " + // << pid_controller_.J_inv_debug(1, 3) << ", + // " + // << pid_controller_.J_inv_debug(1, 4) << ", + // " + // << pid_controller_.J_inv_debug(1, 5) << ", + // " + // << pid_controller_.J_inv_debug(1, 6) << "; + // " + // << pid_controller_.J_inv_debug(2, 0) << ", + // " + // << pid_controller_.J_inv_debug(2, 1) << ", + // " + // << pid_controller_.J_inv_debug(2, 2) << ", + // " + // << pid_controller_.J_inv_debug(2, 3) << ", + // " + // << pid_controller_.J_inv_debug(2, 4) << ", + // " + // << pid_controller_.J_inv_debug(2, 5) << ", + // " + // << pid_controller_.J_inv_debug(2, 6) << "; + // " + // << pid_controller_.J_inv_debug(3, 0) << ", + // " + // << pid_controller_.J_inv_debug(3, 1) << ", + // " + // << pid_controller_.J_inv_debug(3, 2) << ", + // " + // << pid_controller_.J_inv_debug(3, 3) << ", + // " + // << pid_controller_.J_inv_debug(3, 4) << ", + // " + // << pid_controller_.J_inv_debug(3, 5) << ", + // " + // << pid_controller_.J_inv_debug(3, 6) << "; + // " + // << pid_controller_.J_inv_debug(4, 0) << ", + // " + // << pid_controller_.J_inv_debug(4, 1) << ", + // " + // << pid_controller_.J_inv_debug(4, 2) << ", + // " + // << pid_controller_.J_inv_debug(4, 3) << ", + // " + // << pid_controller_.J_inv_debug(4, 4) << ", + // " + // << pid_controller_.J_inv_debug(4, 5) << ", + // " + // << pid_controller_.J_inv_debug(4, 6) << "; + // " + // << pid_controller_.J_inv_debug(5, 0) << ", + // " + // << pid_controller_.J_inv_debug(5, 1) << ", + // " + // << pid_controller_.J_inv_debug(5, 2) << ", + // " + // << pid_controller_.J_inv_debug(5, 3) << ", + // " + // << pid_controller_.J_inv_debug(5, 4) << ", + // " + // << pid_controller_.J_inv_debug(5, 5) << ", + // " + // << pid_controller_.J_inv_debug(5, 6) << + // "]"); geometry_msgs::msg::WrenchStamped tau_msg; tau_msg.header.stamp = this->now(); @@ -361,27 +410,105 @@ rcl_interfaces::msg::SetParametersResult PIDControllerNode::parametersCallback( result.successful = true; result.reason = "success"; - bool kp_updated = false; - bool ki_updated = false; - bool kd_updated = false; + bool kp_x_updated = false; + bool kp_y_updated = false; + bool kp_z_updated = false; + bool kp_roll_updated = false; + bool kp_pitch_updated = false; + bool kp_yaw_updated = false; + + bool ki_x_updated = false; + bool ki_y_updated = false; + bool ki_z_updated = false; + bool ki_roll_updated = false; + bool ki_pitch_updated = false; + bool ki_yaw_updated = false; + + bool kd_x_updated = false; + bool kd_y_updated = false; + bool kd_z_updated = false; + bool kd_roll_updated = false; + bool kd_pitch_updated = false; + bool kd_yaw_updated = false; types::Vector6d Kp_vec_eigen; types::Vector6d Ki_vec_eigen; types::Vector6d Kd_vec_eigen; + for (const auto& param : parameters) { + if (param.get_name() == "Kp_x") { + Kp_vec_eigen(0) = param.as_double(); + kp_x_updated = true; + } else if (param.get_name() == "Kp_y") { + Kp_vec_eigen(1) = param.as_double(); + kp_y_updated = true; + } else if (param.get_name() == "Kp_z") { + Kp_vec_eigen(2) = param.as_double(); + kp_z_updated = true; + } else if (param.get_name() == "Kp_roll") { + Kp_vec_eigen(3) = param.as_double(); + kp_roll_updated = true; + } else if (param.get_name() == "Kp_pitch") { + Kp_vec_eigen(4) = param.as_double(); + kp_pitch_updated = true; + } else if (param.get_name() == "Kp_yaw") { + Kp_vec_eigen(5) = param.as_double(); + kp_yaw_updated = true; + } else if (param.get_name() == "Ki_x") { + Ki_vec_eigen(0) = param.as_double(); + ki_x_updated = true; + } else if (param.get_name() == "Ki_y") { + Ki_vec_eigen(1) = param.as_double(); + ki_y_updated = true; + } else if (param.get_name() == "Ki_z") { + Ki_vec_eigen(2) = param.as_double(); + ki_z_updated = true; + } else if (param.get_name() == "Ki_roll") { + Ki_vec_eigen(3) = param.as_double(); + ki_roll_updated = true; + } else if (param.get_name() == "Ki_pitch") { + Ki_vec_eigen(4) = param.as_double(); + ki_pitch_updated = true; + } else if (param.get_name() == "Ki_yaw") { + Ki_vec_eigen(5) = param.as_double(); + ki_yaw_updated = true; + } else if (param.get_name() == "Kd_x") { + Kd_vec_eigen(0) = param.as_double(); + kd_x_updated = true; + } else if (param.get_name() == "Kd_y") { + Kd_vec_eigen(1) = param.as_double(); + kd_y_updated = true; + } else if (param.get_name() == "Kd_z") { + Kd_vec_eigen(2) = param.as_double(); + kd_z_updated = true; + } else if (param.get_name() == "Kd_roll") { + Kd_vec_eigen(3) = param.as_double(); + kd_roll_updated = true; + } else if (param.get_name() == "Kd_pitch") { + Kd_vec_eigen(4) = param.as_double(); + kd_pitch_updated = true; + } else if (param.get_name() == "Kd_yaw") { + Kd_vec_eigen(5) = param.as_double(); + kd_yaw_updated = true; + } + } + // Only set the gains if the parameter update was successful if (result.successful) { - if (kp_updated) { + if (kp_x_updated || kp_y_updated || kp_z_updated || kp_roll_updated || + kp_pitch_updated || kp_yaw_updated) { types::Matrix6d Kp_eigen = Kp_vec_eigen.asDiagonal().toDenseMatrix(); pid_controller_.set_kp(Kp_eigen); } - if (ki_updated) { + if (ki_x_updated || ki_y_updated || ki_z_updated || ki_roll_updated || + ki_pitch_updated || ki_yaw_updated) { types::Matrix6d Ki_eigen = Ki_vec_eigen.asDiagonal().toDenseMatrix(); pid_controller_.set_ki(Ki_eigen); } - if (kd_updated) { + if (kd_x_updated || kd_y_updated || kd_z_updated || kd_roll_updated || + kd_pitch_updated || kd_yaw_updated) { types::Matrix6d Kd_eigen = Kd_vec_eigen.asDiagonal().toDenseMatrix(); pid_controller_.set_kd(Kd_eigen); From 3bb892627bd89d847961fe2da87048f7a998d616 Mon Sep 17 00:00:00 2001 From: Anbit Date: Sun, 2 Nov 2025 12:36:03 +0100 Subject: [PATCH 025/290] 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 026/290] 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 027/290] 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 028/290] 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 029/290] 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 030/290] 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 031/290] 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 032/290] 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 033/290] [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 edcd63bb7555226ff682add1fa1a3d4337fd2e6a Mon Sep 17 00:00:00 2001 From: ppakr Date: Wed, 12 Nov 2025 20:55:01 +0100 Subject: [PATCH 034/290] feat: update CMake configuration and add logging for PID controller with new test setup --- control/pid_controller_dp/CMakeLists.txt | 58 +- .../pid_controller_dp/pid_controller.hpp | 3 +- .../pid_controller_utils.hpp | 4 + control/pid_controller_dp/package.xml | 1 + .../pid_controller_dp/src/pid_controller.cpp | 102 +++- .../src/pid_controller_node.cpp | 12 +- .../src/pid_controller_ros.cpp | 132 +++-- .../src/pid_controller_utils.cpp | 57 +- control/pid_controller_dp/test/CMakeLists.txt | 22 + .../test/pid_controller_tests.cpp | 514 ++++++++++++++++++ 10 files changed, 830 insertions(+), 75 deletions(-) create mode 100644 control/pid_controller_dp/test/CMakeLists.txt create mode 100644 control/pid_controller_dp/test/pid_controller_tests.cpp diff --git a/control/pid_controller_dp/CMakeLists.txt b/control/pid_controller_dp/CMakeLists.txt index f61e0d258..6186ef2dd 100644 --- a/control/pid_controller_dp/CMakeLists.txt +++ b/control/pid_controller_dp/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.8) project(pid_controller_dp) if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD 20) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") @@ -17,17 +17,43 @@ find_package(Eigen3 REQUIRED) find_package(tf2 REQUIRED) find_package(vortex_msgs REQUIRED) find_package(rcl_interfaces REQUIRED) +find_package(vortex_utils REQUIRED) +find_package(spdlog REQUIRED) +find_package(fmt REQUIRED) include_directories(include) +set(LIB_NAME ${PROJECT_NAME}_lib) -add_executable(pid_controller_node - src/pid_controller_node.cpp - src/pid_controller_ros.cpp +add_library(${LIB_NAME} SHARED src/pid_controller.cpp src/pid_controller_utils.cpp src/pid_controller_conversions.cpp ) +ament_target_dependencies(${LIB_NAME} PUBLIC + rclcpp + geometry_msgs + nav_msgs + Eigen3 + tf2 + vortex_msgs + rcl_interfaces + vortex_utils + spdlog + fmt +) + + +install(TARGETS + ${LIB_NAME} + DESTINATION lib/${PROJECT_NAME} +) + +add_executable(pid_controller_node + src/pid_controller_node.cpp + src/pid_controller_ros.cpp +) + ament_target_dependencies(pid_controller_node rclcpp geometry_msgs @@ -36,6 +62,25 @@ ament_target_dependencies(pid_controller_node tf2 vortex_msgs rcl_interfaces + vortex_utils + spdlog + fmt +) + +target_link_libraries( + pid_controller_node + ${LIB_NAME} + spdlog::spdlog + # vortex_utilis::vortex_utils +) + +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 @@ -48,4 +93,9 @@ install(DIRECTORY DESTINATION share/${PROJECT_NAME}/ ) + +if(BUILD_TESTING) + add_subdirectory(test) +endif() + ament_package() 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 5f1d76b90..d2d45891e 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,6 +1,7 @@ #ifndef PID_CONTROLLER_HPP #define PID_CONTROLLER_HPP +#include #include "pid_controller_dp/typedefs.hpp" class PIDController { @@ -45,7 +46,7 @@ class PIDController { types::Vector6d D_debug; types::Vector6d tau_debug; types::Matrix6x7d J_inv_debug; - + // debug gain types::Matrix6d Kp_debug; types::Matrix6d Ki_debug; 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..27d20f952 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,11 +1,13 @@ #ifndef PID_UTILS_HPP #define PID_UTILS_HPP +#include #include #include #include #include #include +#include #include "pid_controller_dp/typedefs.hpp" // @brief Calculate the sine of an angle in degrees @@ -71,4 +73,6 @@ types::Vector7d anti_windup(const double dt, const types::Eta& error, const types::Vector7d& integral); +// void print_J_transformation(const types::J_transformation& J); +// void print_Jinv_transformation(const types::Matrix6x7d& J_inv); #endif diff --git a/control/pid_controller_dp/package.xml b/control/pid_controller_dp/package.xml index 0337991d7..d21a2fa92 100644 --- a/control/pid_controller_dp/package.xml +++ b/control/pid_controller_dp/package.xml @@ -16,6 +16,7 @@ tf2 vortex_msgs rcl_interfaces + vortex_utils ament_cmake diff --git a/control/pid_controller_dp/src/pid_controller.cpp b/control/pid_controller_dp/src/pid_controller.cpp index a4099a0c2..e44698e2d 100644 --- a/control/pid_controller_dp/src/pid_controller.cpp +++ b/control/pid_controller_dp/src/pid_controller.cpp @@ -1,6 +1,71 @@ #include "pid_controller_dp/pid_controller.hpp" #include "pid_controller_dp/pid_controller_utils.hpp" +void print_eta(const types::Eta& eta) { + // spdlog::info("Eta values:"); + spdlog::info("Position - North: {}, East: {}, Down: {}", eta.pos[0], + eta.pos[1], eta.pos[2]); + spdlog::info("Orientation - w: {}, x: {}, y: {}, z: {}", eta.ori.w(), + eta.ori.x(), eta.ori.y(), eta.ori.z()); +} + +void print_nu(const types::Nu& nu) { + spdlog::info("Nu values:"); + spdlog::info("Linear Speed - u: {}, v: {}, w: {}", nu.linear_speed[0], + nu.linear_speed[1], nu.linear_speed[2]); + spdlog::info("Angular Speed - p: {}, q: {}, r: {}", nu.angular_speed[0], + nu.angular_speed[1], nu.angular_speed[2]); +} + +void print_vect_6d(const types::Vector6d& vec) { + spdlog::info("Vector6d values:"); + for (int i = 0; i < 6; ++i) { + spdlog::info("Element[{}]: {}", i, vec[i]); + } +} + +void print_J_transformation(const types::J_transformation& J) { + spdlog::info("J_transformation:"); + + spdlog::info("R (3x3) elements:"); + for (int i = 0; i < J.R.rows(); ++i) { + for (int j = 0; j < J.R.cols(); ++j) { + spdlog::info("R[{},{}] = {}", i, j, J.R(i, j)); + } + } + + spdlog::info("T (4x3) elements:"); + for (int i = 0; i < J.T.rows(); ++i) { + for (int j = 0; j < J.T.cols(); ++j) { + spdlog::info("T[{},{}] = {}", i, j, J.T(i, j)); + } + } + + spdlog::info("Combined Matrix (7x6) elements:"); + auto M = J.as_matrix(); + for (int i = 0; i < M.rows(); ++i) { + for (int j = 0; j < M.cols(); ++j) { + spdlog::info("M[{},{}] = {}", i, j, M(i, j)); + } + } +} + +void print_Jinv_transformation(const types::Matrix6x7d& J_inv) { + spdlog::info("J_pseudo_inverse (6x7):"); + for (int i = 0; i < J_inv.rows(); ++i) { + std::string row; + row.reserve(128); + row += "["; + for (int j = 0; j < J_inv.cols(); ++j) { + row += std::to_string(J_inv(i, j)); + if (j < J_inv.cols() - 1) + row += ", "; + } + row += "]"; + spdlog::info("{}", row); + } +} + PIDController::PIDController() : Kp_(types::Matrix6d::Identity()), Ki_(types::Matrix6d::Zero()), @@ -14,30 +79,45 @@ types::Vector6d PIDController::calculate_tau(const types::Eta& eta, const types::Eta& eta_dot_d) { types::Eta error = error_eta(eta, eta_d); // calculate eta error + // set w = 0 + error.ori.w() = 0.0; // only use vector part of quaternion for error + + auto eta_dot_d_copy = eta_dot_d; + eta_dot_d_copy.ori.w() = 0.0; // set w = 0 for desired eta_dot // debug - eta_error_debug = error; + // eta_error_debug = error; + spdlog::info("Eta: "); + print_eta(eta); + spdlog::info("Eta desired: "); + print_eta(eta_d); + spdlog::info("Eta error:"); + print_eta(error); types::Matrix6x7d J_inv = - calculate_J_sudo_inv(error); // calculate J pseudo inverse + calculate_J_sudo_inv(eta); // calculate J pseudo inverse J_inv_debug = J_inv; + print_Jinv_transformation(J_inv); - types::Vector6d nu_d = J_inv * eta_dot_d.as_vector(); // calculate velocity - nu_d_debug = nu_d; + types::Vector6d nu_d = + J_inv * eta_dot_d_copy.as_vector(); // calculate velocity + // nu_d_debug = nu_d; + // print_nu(nu_d); types::Vector6d error_nu = nu.as_vector() - nu_d; // calculate vel error - error_nu_debug = error_nu; + // error_nu_debug = error_nu; + // print_vect_6d(error_nu); - types::Vector6d P = Kp_ * J_inv * error.as_vector(); /// P term - P_debug = P; + types::Vector6d P = Kp_ * J_inv * error.as_vector(); // P term + // P_debug = P; Kp_debug = Kp_; types::Vector6d I = Ki_ * J_inv * integral_; // I term - I_debug = I; - Ki_debug = Ki_; + // I_debug = I; + // Ki_debug = Ki_; types::Vector6d D = Kd_ * error_nu; // D term - D_debug = D; - Kd_debug = Kd_; + // D_debug = D; + // Kd_debug = Kd_; types::Vector6d tau = -clamp_values((P + I + D), -80.0, 80.0); // types::Vector6d tau = -clamp_values((P), -80.0, 80.0); diff --git a/control/pid_controller_dp/src/pid_controller_node.cpp b/control/pid_controller_dp/src/pid_controller_node.cpp index f9bf44663..88ac81095 100644 --- a/control/pid_controller_dp/src/pid_controller_node.cpp +++ b/control/pid_controller_dp/src/pid_controller_node.cpp @@ -1,8 +1,18 @@ +#include #include "pid_controller_dp/pid_controller_ros.hpp" +auto start_msg = R"( + ____ ___ ____ ____ _ _ _ + | _ \_ _| _ \ / ___|___ _ __ | |_ _ __ ___ | | | ___ _ __ + | |_) | || | | | | | / _ \| '_ \| __| '__/ _ \| | |/ _ \ '__| + | __/| || |_| | | |__| (_) | | | | |_| | | (_) | | | __/ | + |_| |___|____/ \____\___/|_| |_|\__|_| \___/|_|_|\___|_| +)"; + int main(int argc, char** argv) { rclcpp::init(argc, argv); - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Started PID Controller Node"); + // RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Started PID Controller Node"); + spdlog::info(start_msg); rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; diff --git a/control/pid_controller_dp/src/pid_controller_ros.cpp b/control/pid_controller_dp/src/pid_controller_ros.cpp index 3f6f1cb6f..2d9fc6233 100644 --- a/control/pid_controller_dp/src/pid_controller_ros.cpp +++ b/control/pid_controller_dp/src/pid_controller_ros.cpp @@ -147,43 +147,43 @@ void PIDControllerNode::publish_tau() { // << pid_controller_.P_debug(3) << ", " // << pid_controller_.P_debug(4) << ", " // << pid_controller_.P_debug(5) << "]"); - // RCLCPP_INFO_STREAM(this->get_logger(), - // "Kp: [" << pid_controller_.Kp_debug(0, 0) << ", " - // << pid_controller_.Kp_debug(0, 1) << ", " - // << pid_controller_.Kp_debug(0, 2) << ", " - // << pid_controller_.Kp_debug(0, 3) << ", " - // << pid_controller_.Kp_debug(0, 4) << ", " - // << pid_controller_.Kp_debug(0, 5) << "; " - // << pid_controller_.Kp_debug(1, 0) << ", " - // << pid_controller_.Kp_debug(1, 1) << ", " - // << pid_controller_.Kp_debug(1, 2) << ", " - // << pid_controller_.Kp_debug(1, 3) << ", " - // << pid_controller_.Kp_debug(1, 4) << ", " - // << pid_controller_.Kp_debug(1, 5) << "; " - // << pid_controller_.Kp_debug(2, 0) << ", " - // << pid_controller_.Kp_debug(2, 1) << ", " - // << pid_controller_.Kp_debug(2, 2) << ", " - // << pid_controller_.Kp_debug(2, 3) << ", " - // << pid_controller_.Kp_debug(2, 4) << ", " - // << pid_controller_.Kp_debug(2, 5) << "; " - // << pid_controller_.Kp_debug(3, 0) << ", " - // << pid_controller_.Kp_debug(3, 1) << ", " - // << pid_controller_.Kp_debug(3, 2) << ", " - // << pid_controller_.Kp_debug(3, 3) << ", " - // << pid_controller_.Kp_debug(3, 4) << ", " - // << pid_controller_.Kp_debug(3, 5) << "; " - // << pid_controller_.Kp_debug(4, 0) << ", " - // << pid_controller_.Kp_debug(4, 1) << ", " - // << pid_controller_.Kp_debug(4, 2) << ", " - // << pid_controller_.Kp_debug(4, 3) << ", " - // << pid_controller_.Kp_debug(4, 4) << ", " - // << pid_controller_.Kp_debug(4, 5) << "; " - // << pid_controller_.Kp_debug(5, 0) << ", " - // << pid_controller_.Kp_debug(5, 1) << ", " - // << pid_controller_.Kp_debug(5, 2) << ", " - // << pid_controller_.Kp_debug(5, 3) << ", " - // << pid_controller_.Kp_debug(5, 4) << ", " - // << pid_controller_.Kp_debug(5, 5) << "]"); + RCLCPP_INFO_STREAM(this->get_logger(), + "Kp: [" << pid_controller_.Kp_debug(0, 0) << ", " + << pid_controller_.Kp_debug(0, 1) << ", " + << pid_controller_.Kp_debug(0, 2) << ", " + << pid_controller_.Kp_debug(0, 3) << ", " + << pid_controller_.Kp_debug(0, 4) << ", " + << pid_controller_.Kp_debug(0, 5) << "; " + << pid_controller_.Kp_debug(1, 0) << ", " + << pid_controller_.Kp_debug(1, 1) << ", " + << pid_controller_.Kp_debug(1, 2) << ", " + << pid_controller_.Kp_debug(1, 3) << ", " + << pid_controller_.Kp_debug(1, 4) << ", " + << pid_controller_.Kp_debug(1, 5) << "; " + << pid_controller_.Kp_debug(2, 0) << ", " + << pid_controller_.Kp_debug(2, 1) << ", " + << pid_controller_.Kp_debug(2, 2) << ", " + << pid_controller_.Kp_debug(2, 3) << ", " + << pid_controller_.Kp_debug(2, 4) << ", " + << pid_controller_.Kp_debug(2, 5) << "; " + << pid_controller_.Kp_debug(3, 0) << ", " + << pid_controller_.Kp_debug(3, 1) << ", " + << pid_controller_.Kp_debug(3, 2) << ", " + << pid_controller_.Kp_debug(3, 3) << ", " + << pid_controller_.Kp_debug(3, 4) << ", " + << pid_controller_.Kp_debug(3, 5) << "; " + << pid_controller_.Kp_debug(4, 0) << ", " + << pid_controller_.Kp_debug(4, 1) << ", " + << pid_controller_.Kp_debug(4, 2) << ", " + << pid_controller_.Kp_debug(4, 3) << ", " + << pid_controller_.Kp_debug(4, 4) << ", " + << pid_controller_.Kp_debug(4, 5) << "; " + << pid_controller_.Kp_debug(5, 0) << ", " + << pid_controller_.Kp_debug(5, 1) << ", " + << pid_controller_.Kp_debug(5, 2) << ", " + << pid_controller_.Kp_debug(5, 3) << ", " + << pid_controller_.Kp_debug(5, 4) << ", " + << pid_controller_.Kp_debug(5, 5) << "]"); // RCLCPP_INFO_STREAM(this->get_logger(), // "I term: [" << pid_controller_.I_debug(0) << ", " // << pid_controller_.I_debug(1) << ", " @@ -364,26 +364,44 @@ void PIDControllerNode::set_pid_params() { types::Matrix6d Kd_eigen = Kd_vec_eigen.asDiagonal().toDenseMatrix(); // print out for debug - RCLCPP_INFO_STREAM(this->get_logger(), - "Kp_eigen: [" - << Kp_eigen(0, 0) << ", " << Kp_eigen(0, 1) << ", " - << Kp_eigen(0, 2) << ", " << Kp_eigen(0, 3) << ", " - << Kp_eigen(0, 4) << ", " << Kp_eigen(0, 5) << "; " - << Kp_eigen(1, 0) << ", " << Kp_eigen(1, 1) << ", " - << Kp_eigen(1, 2) << ", " << Kp_eigen(1, 3) << ", " - << Kp_eigen(1, 4) << ", " << Kp_eigen(1, 5) << "; " - << Kp_eigen(2, 0) << ", " << Kp_eigen(2, 1) << ", " - << Kp_eigen(2, 2) << ", " << Kp_eigen(2, 3) << ", " - << Kp_eigen(2, 4) << ", " << Kp_eigen(2, 5) << "; " - << Kp_eigen(3, 0) << ", " << Kp_eigen(3, 1) << ", " - << Kp_eigen(3, 2) << ", " << Kp_eigen(3, 3) << ", " - << Kp_eigen(3, 4) << ", " << Kp_eigen(3, 5) << "; " - << Kp_eigen(4, 0) << ", " << Kp_eigen(4, 1) << ", " - << Kp_eigen(4, 2) << ", " << Kp_eigen(4, 3) << ", " - << Kp_eigen(4, 4) << ", " << Kp_eigen(4, 5) << "; " - << Kp_eigen(5, 0) << ", " << Kp_eigen(5, 1) << ", " - << Kp_eigen(5, 2) << ", " << Kp_eigen(5, 3) << ", " - << Kp_eigen(5, 4) << ", " << Kp_eigen(5, 5) << "]"); + // RCLCPP_INFO_STREAM(this->get_logger(), + // "Kp_eigen: [" + // << Kp_eigen(0, 0) << ", " << Kp_eigen(0, 1) << ", + // " + // << Kp_eigen(0, 2) << ", " << Kp_eigen(0, 3) << ", + // " + // << Kp_eigen(0, 4) << ", " << Kp_eigen(0, 5) << "; + // " + // << Kp_eigen(1, 0) << ", " << Kp_eigen(1, 1) << ", + // " + // << Kp_eigen(1, 2) << ", " << Kp_eigen(1, 3) << ", + // " + // << Kp_eigen(1, 4) << ", " << Kp_eigen(1, 5) << "; + // " + // << Kp_eigen(2, 0) << ", " << Kp_eigen(2, 1) << ", + // " + // << Kp_eigen(2, 2) << ", " << Kp_eigen(2, 3) << ", + // " + // << Kp_eigen(2, 4) << ", " << Kp_eigen(2, 5) << "; + // " + // << Kp_eigen(3, 0) << ", " << Kp_eigen(3, 1) << ", + // " + // << Kp_eigen(3, 2) << ", " << Kp_eigen(3, 3) << ", + // " + // << Kp_eigen(3, 4) << ", " << Kp_eigen(3, 5) << "; + // " + // << Kp_eigen(4, 0) << ", " << Kp_eigen(4, 1) << ", + // " + // << Kp_eigen(4, 2) << ", " << Kp_eigen(4, 3) << ", + // " + // << Kp_eigen(4, 4) << ", " << Kp_eigen(4, 5) << "; + // " + // << Kp_eigen(5, 0) << ", " << Kp_eigen(5, 1) << ", + // " + // << Kp_eigen(5, 2) << ", " << Kp_eigen(5, 3) << ", + // " + // << Kp_eigen(5, 4) << ", " << Kp_eigen(5, 5) << + // "]"); pid_controller_.set_kp(Kp_eigen); pid_controller_.set_ki(Ki_eigen); diff --git a/control/pid_controller_dp/src/pid_controller_utils.cpp b/control/pid_controller_dp/src/pid_controller_utils.cpp index 246e15500..97d3e607b 100644 --- a/control/pid_controller_dp/src/pid_controller_utils.cpp +++ b/control/pid_controller_dp/src/pid_controller_utils.cpp @@ -1,8 +1,45 @@ #include "pid_controller_dp/pid_controller_utils.hpp" #include +#include +#include #include "pid_controller_dp/pid_controller_conversions.hpp" #include "pid_controller_dp/typedefs.hpp" +// void print_J_transformation(const types::J_transformation& J) { +// spdlog::info("J_transformation:"); + +// spdlog::info("R (3x3) elements:"); +// for (int i = 0; i < J.R.rows(); ++i) { +// for (int j = 0; j < J.R.cols(); ++j) { +// spdlog::info("R[{},{}] = {}", i, j, J.R(i, j)); +// } +// } + +// spdlog::info("T (4x3) elements:"); +// for (int i = 0; i < J.T.rows(); ++i) { +// for (int j = 0; j < J.T.cols(); ++j) { +// spdlog::info("T[{},{}] = {}", i, j, J.T(i, j)); +// } +// } + +// spdlog::info("Combined Matrix (7x6) elements:"); +// auto M = J.as_matrix(); +// for (int i = 0; i < M.rows(); ++i) { +// for (int j = 0; j < M.cols(); ++j) { +// spdlog::info("M[{},{}] = {}", i, j, M(i, j)); +// } +// } +// } + +// void print_Jinv_transformation(const types::Matrix6x7d& J_inv) { +// spdlog::info("J (6x7) elements:"); +// for (int i = 0; i < J_inv.rows(); ++i) { +// for (int j = 0; j < J_inv.cols(); ++j) { +// spdlog::info("J_inv[{},{}] = {}", i, j, J_inv(i, j)); +// } +// } +// } + types::Matrix3d calculate_R_quat(const types::Eta& eta) { return eta.ori.normalized().toRotationMatrix(); } @@ -18,6 +55,7 @@ types::Matrix4x3d calculate_T_quat(const types::Eta& eta) { types::Matrix4x3d transformation_matrix; transformation_matrix << -x, -y, -z, w, -z, y, z, w, -x, -y, x, w; + // transformation_matrix << -x, -y, -z, w, -z, y, -z, w, -x, -y, x, w; return transformation_matrix * 0.5; } @@ -34,16 +72,30 @@ types::Matrix6x7d calculate_J_sudo_inv(const types::Eta& eta) { types::J_transformation J; J.R = R; J.T = T; + // print_J_transformation(J); types::Matrix6x7d J_transpose = J.as_matrix().transpose(); + // spdlog::info("J_transpose (6x7) elements:"); + // print_Jinv_transformation(J_transpose); + // types::Matrix6x7d J_inv = (J.as_matrix() * J_transpose).inverse(); + + // spdlog::info(""); + // print_Jinv_transformation(J_inv); + + // (J_transpose * J.as_matrix()).inverse() * J_transpose + + // types::Matrix6x7d J_pseudo_inv = + // (J_transpose * J.as_matrix()).inverse() * J_transpose; + types::Matrix6x7d J_pseudo_inv = - (J_transpose * J.as_matrix()).inverse() * J_transpose; + vortex::utils::math::pseudo_inverse(J.as_matrix()); return J_pseudo_inv; } types::Eta error_eta(const types::Eta& eta, const types::Eta& eta_d) { types::Eta eta_error; + // vortex::utils::types::EtaQuat eta_error; eta_error.pos = eta.pos - eta_d.pos; eta_error.ori = eta_d.ori.conjugate() * eta.ori; @@ -57,6 +109,7 @@ Eigen::VectorXd clamp_values(const Eigen::VectorXd& values, double min_val, double max_val) { return values.cwiseMax(min_val).cwiseMin(max_val); + // return vortex::utils::math::clamp_values(values, min_val, max_val); } types::Vector7d anti_windup(const double dt, @@ -72,4 +125,6 @@ types::Vector7d anti_windup(const double dt, integral_anti_windup = clamp_values(integral_anti_windup, -80.0, 80.0); return integral_anti_windup; + // return vortex::utils::math::anti_windup(dt, error_norm.as_vector(), + // integral, -80.0, 80.0); } diff --git a/control/pid_controller_dp/test/CMakeLists.txt b/control/pid_controller_dp/test/CMakeLists.txt new file mode 100644 index 000000000..d47998914 --- /dev/null +++ b/control/pid_controller_dp/test/CMakeLists.txt @@ -0,0 +1,22 @@ +cmake_minimum_required(VERSION 3.8) + +find_package(GTest REQUIRED) +include(GoogleTest) + +set(TEST_BINARY_NAME ${PROJECT_NAME}_test) +add_executable( + ${TEST_BINARY_NAME} + pid_controller_tests.cpp +) + +target_link_libraries( + ${TEST_BINARY_NAME} + PRIVATE + ${LIB_NAME} + GTest::GTest + spdlog::spdlog +) + +ament_target_dependencies(${TEST_BINARY_NAME} PUBLIC Eigen3 tf2 vortex_utils) + +gtest_discover_tests(${TEST_BINARY_NAME}) diff --git a/control/pid_controller_dp/test/pid_controller_tests.cpp b/control/pid_controller_dp/test/pid_controller_tests.cpp new file mode 100644 index 000000000..83a2614d6 --- /dev/null +++ b/control/pid_controller_dp/test/pid_controller_tests.cpp @@ -0,0 +1,514 @@ +// #include +#include +#include +// #include +// #include + +// #include "dp_adapt_backs_controller/dp_adapt_backs_controller.hpp" +// #include "dp_adapt_backs_controller/pid_controller_utils.hpp" +// #include "dp_adapt_backs_controller/typedefs.hpp" +#include +#include "pid_controller_dp/pid_controller.hpp" +#include "pid_controller_dp/pid_controller_utils.hpp" +#include "pid_controller_dp/typedefs.hpp" + +void print_tau(const types::Vector6d& tau) { + spdlog::info("Tau values:"); + spdlog::info("Surge: {}", tau[0]); + spdlog::info("Sway: {}", tau[1]); + spdlog::info("Heave: {}", tau[2]); + spdlog::info("Roll: {}", tau[3]); + spdlog::info("Pitch: {}", tau[4]); + spdlog::info("Yaw: {}", tau[5]); +} + +class PIDControllerTests : public ::testing::Test { + protected: + PIDControllerTests() : pid_controller_() { + // Set PID gains for testing + types::Matrix6d Kp = types::Matrix6d::Identity() * 10.0; + types::Matrix6d Ki = types::Matrix6d::Identity() * 0.5; + types::Matrix6d Kd = types::Matrix6d::Identity() * 2.0; + + pid_controller_.set_kp(Kp); + pid_controller_.set_ki(Ki); + pid_controller_.set_kd(Kd); + } + + types::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) { + types::Eta current_pose; + current_pose.pos = types::Vector3d(north_pos, east_pos, down_pos); + current_pose.ori = vortex::utils::math::euler_to_quat( + roll_angle, pitch_angle, yaw_angle); + + return current_pose; + } + + types::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) { + types::Eta reference_pose; + reference_pose.pos = types::Vector3d(north_pos, east_pos, down_pos); + reference_pose.ori = vortex::utils::math::euler_to_quat( + roll_angle, pitch_angle, yaw_angle); + return reference_pose; + } + + types::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) { + types::Nu current_velocity; + current_velocity.linear_speed = + types::Vector3d(surge_vel, sway_vel, heave_vel); + current_velocity.angular_speed = + types::Vector3d(roll_rate, pitch_rate, yaw_rate); + return current_velocity; + } + + PIDController pid_controller_; +}; + +/* +Test that negative north error only (in body) gives positive surge command only. +*/ + +TEST_F(PIDControllerTests, + T01_neg_north_error_with_zero_heading_gives_surge_only_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(10.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + print_tau(tau); + + EXPECT_GT(tau[0], 0.0); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative north error with positive heading gives a positive surge +command and negative sway command. +*/ + +TEST_F( + PIDControllerTests, + T02_neg_north_error_with_positive_heading_gives_pos_surge_and_neg_sway_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 1.5)}; + types::Eta eta_d{generate_reference_pose(10.0, 0.0, 0.0, 0.0, 0.0, 1.5)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + print_tau(tau); + EXPECT_GT(tau[0], 0.0); + EXPECT_LT(tau[1], 0.0); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative north error with negative heading gives a positive surge +command and positive sway command. +*/ + +TEST_F( + PIDControllerTests, + T03_neg_north_error_with_negative_heading_gives_pos_surge_and_pos_sway_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, -1.5)}; + types::Eta eta_d{generate_reference_pose(10.0, 0.0, 0.0, 0.0, 0.0, -1.5)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + print_tau(tau); + + EXPECT_GT(tau[0], 0.0); + EXPECT_GT(tau[1], 0.0); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative down error with zero roll and pitch gives a positive heave +command. +*/ + +TEST_F( + PIDControllerTests, + T04_neg_down_error_with_zero_roll_and_pitch_gives_positive_heave_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 0.0, 2.0, 0.0, 0.0, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + print_tau(tau); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_GT(tau[2], 0.0); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative down error with zero roll and negative pitch gives a positive +heave and positive surge command. +*/ + +TEST_F( + PIDControllerTests, + T05_neg_down_error_with_zero_roll_and_neg_pitch_gives_positive_heave_and_positive_surge_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, -0.5, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 0.0, 2.0, 0.0, -0.5, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + print_tau(tau); + + EXPECT_GT(tau[0], 0.0); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_GT(tau[2], 0.0); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative down error with zero roll and positive pitch gives a positive +heave and negative surge command. +*/ + +TEST_F( + PIDControllerTests, + T06_neg_down_error_with_zero_roll_and_pos_pitch_gives_positive_heave_and_negative_surge_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.5, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 0.0, 2.0, 0.0, 0.5, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + print_tau(tau); + + EXPECT_LT(tau[0], 0.0); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_GT(tau[2], 0.0); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative east error with zero heading gives a positive sway command. +*/ + +TEST_F(PIDControllerTests, + T07_neg_east_error_with_zero_heading_gives_positive_sway_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 10.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + print_tau(tau); + + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_GT(tau[1], 0.0); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that positive east error with zero heading gives a negative sway command. +*/ + +TEST_F(PIDControllerTests, + T08_pos_east_error_with_zero_heading_gives_pos_sway_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, -10.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + print_tau(tau); + + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_LT(tau[1], 0.0); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative east error with positive heading gives a positive surge and +sway command. +*/ + +TEST_F( + PIDControllerTests, + T09_neg_east_error_with_positive_heading_gives_pos_sway_and_pos_surge_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 1.5)}; + types::Eta eta_d{generate_reference_pose(0.0, 10.0, 0.0, 0.0, 0.0, 1.5)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + EXPECT_GT(tau[0], 0.0); + EXPECT_GT(tau[1], 0.0); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative east error with negative heading gives a negative surge and +positive sway command. +*/ + +TEST_F( + PIDControllerTests, + T10_neg_east_error_with_negative_heading_gives_pos_sway_and_neg_surge_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, -1.5)}; + types::Eta eta_d{generate_reference_pose(0.0, 10.0, 0.0, 0.0, 0.0, -1.5)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + print_tau(tau); + + EXPECT_LT(tau[0], 0.0); + EXPECT_GT(tau[1], 0.0); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative roll error gives positive roll command. +*/ + +TEST_F(PIDControllerTests, T11_neg_roll_error_gives_positive_roll_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 1.0, 0.0, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + print_tau(tau); + + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_GT(tau[3], 0.0); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that positive roll error gives negative roll command. +*/ + +TEST_F(PIDControllerTests, T12_pos_roll_error_gives_neg_roll_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, -1.0, 0.0, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_LT(tau[3], 0.0); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative pitch error gives positive pitch command. +*/ + +TEST_F(PIDControllerTests, T13_neg_pitch_error_gives_pos_pitch_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 1.0, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_GT(tau[4], 0.0); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that positive pitch error gives negative pitch command. +*/ + +TEST_F(PIDControllerTests, T14_pos_pitch_error_gives_neg_pitch_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, -1.0, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_LT(tau[4], 0.0); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative yaw error gives positive yaw command. +*/ + +TEST_F(PIDControllerTests, T15_neg_yaw_error_gives_pos_yaw_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, 1.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + print_tau(tau); + + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_GT(tau[5], 0.0); +} + +/* +Test that positive yaw error gives negative yaw command. +*/ + +TEST_F(PIDControllerTests, T16_pos_yaw_error_gives_neg_yaw_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, -1.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + print_tau(tau); + + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_LT(tau[5], 0.0); +} + +/* +Test that positive surge velocity only results in negative surge command +(breaking effect). +*/ + +TEST_F(PIDControllerTests, T17_pos_surge_vel_gives_negative_surge_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(1.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + EXPECT_LT(tau[0], 0.0); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that positive sway velocity only results in negative sway command (breaking +effect). +*/ + +TEST_F(PIDControllerTests, T18_pos_sway_vel_gives_negative_sway_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 1.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_LT(tau[1], 0.0); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that positive heave velocity only results in negative heave command +(breaking effect). +*/ + +TEST_F(PIDControllerTests, T19_pos_heave_vel_gives_negative_heave_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 1.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_LT(tau[2], 0.0); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} From 1ed1d42aa8f92f18d9185ab6ce814ccbfcd575fb 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 20:21:30 +0000 Subject: [PATCH 035/290] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- control/pid_controller_dp/CMakeLists.txt | 2 +- control/pid_controller_dp/src/pid_controller_node.cpp | 8 ++++---- control/pid_controller_dp/src/pid_controller_ros.cpp | 2 +- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/control/pid_controller_dp/CMakeLists.txt b/control/pid_controller_dp/CMakeLists.txt index 6186ef2dd..409bce891 100644 --- a/control/pid_controller_dp/CMakeLists.txt +++ b/control/pid_controller_dp/CMakeLists.txt @@ -68,7 +68,7 @@ ament_target_dependencies(pid_controller_node ) target_link_libraries( - pid_controller_node + pid_controller_node ${LIB_NAME} spdlog::spdlog # vortex_utilis::vortex_utils diff --git a/control/pid_controller_dp/src/pid_controller_node.cpp b/control/pid_controller_dp/src/pid_controller_node.cpp index 88ac81095..bee6500fc 100644 --- a/control/pid_controller_dp/src/pid_controller_node.cpp +++ b/control/pid_controller_dp/src/pid_controller_node.cpp @@ -2,11 +2,11 @@ #include "pid_controller_dp/pid_controller_ros.hpp" auto start_msg = R"( - ____ ___ ____ ____ _ _ _ - | _ \_ _| _ \ / ___|___ _ __ | |_ _ __ ___ | | | ___ _ __ + ____ ___ ____ ____ _ _ _ + | _ \_ _| _ \ / ___|___ _ __ | |_ _ __ ___ | | | ___ _ __ | |_) | || | | | | | / _ \| '_ \| __| '__/ _ \| | |/ _ \ '__| - | __/| || |_| | | |__| (_) | | | | |_| | | (_) | | | __/ | - |_| |___|____/ \____\___/|_| |_|\__|_| \___/|_|_|\___|_| + | __/| || |_| | | |__| (_) | | | | |_| | | (_) | | | __/ | + |_| |___|____/ \____\___/|_| |_|\__|_| \___/|_|_|\___|_| )"; int main(int argc, char** argv) { diff --git a/control/pid_controller_dp/src/pid_controller_ros.cpp b/control/pid_controller_dp/src/pid_controller_ros.cpp index 2d9fc6233..ece588b8e 100644 --- a/control/pid_controller_dp/src/pid_controller_ros.cpp +++ b/control/pid_controller_dp/src/pid_controller_ros.cpp @@ -540,4 +540,4 @@ rcl_interfaces::msg::SetParametersResult PIDControllerNode::parametersCallback( RCLCPP_INFO(this->get_logger(), "%s", param.value_to_string().c_str()); } return result; -} \ 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 036/290] 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 037/290] 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 038/290] 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 039/290] 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 040/290] 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 041/290] 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 98c5d572ab31740418188bdba7e2ed8fb4d097d8 Mon Sep 17 00:00:00 2001 From: ppakr Date: Mon, 5 Jan 2026 12:09:52 +0100 Subject: [PATCH 042/290] WIP: before types migration --- .gitignore | 5 +++++ .../src/dp_adapt_backs_controller.cpp | 2 ++ control/pid_controller_dp/config/pid_params.yaml | 6 +++--- .../include/pid_controller_dp/pid_controller_utils.hpp | 1 + scripts/ci_install_dependencies.sh | 0 5 files changed, 11 insertions(+), 3 deletions(-) mode change 100644 => 100755 scripts/ci_install_dependencies.sh diff --git a/.gitignore b/.gitignore index f86dbd841..4d6551eb9 100644 --- a/.gitignore +++ b/.gitignore @@ -61,3 +61,8 @@ qtcreator-* COLCON_IGNORE # End of https://www.gitignore.io/api/ros +control/pid_controller_dp/test/test_main.cpp +control/pid_controller_dp/test/test_pid_basic.cpp +control/pid_controller_dp/test/test_pid_controller.cpp +control/pid_controller_dp/test/test_type_casting.cpp +scripts/ci_install_dependencies.sh 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 cec31e6d3..effb56e17 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 @@ -48,6 +48,8 @@ dp_types::Vector6d DPAdaptBacksController::calculate_tau( dp_types::Vector6d F_est = Y_v * adap_param_; + + // control equation dp_types::Vector6d tau = (M_ * alpha_dot) + (C * nu.as_vector()) - (J.transpose() * z_1) - (K2_ * z_2) - F_est - d_est_; diff --git a/control/pid_controller_dp/config/pid_params.yaml b/control/pid_controller_dp/config/pid_params.yaml index 5bd733a61..c6efcb8bd 100644 --- a/control/pid_controller_dp/config/pid_params.yaml +++ b/control/pid_controller_dp/config/pid_params.yaml @@ -3,13 +3,13 @@ # Kp: [70.0, 70.0, 70.0, 12.0, 12.0, 12.0] # Ki: [2.0, 2.0, 2.0, 0.12, 0.12, 0.12] # Kd: [10.0, 10.0, 10.0, 4.0, 5.0, 4.0] - Kp_x: 10.0 + Kp_x: 20.0 Kp_y: 0.0 - Kp_z: 0.0 + Kp_z: 50.0 Kp_roll: 0.0 Kp_pitch: 0.0 Kp_yaw: 0.0 - Ki_x: 0.0 + Ki_x: 0.08 Ki_y: 0.0 Ki_z: 0.0 Ki_roll: 0.0 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 27d20f952..f2d5f8ca6 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 @@ -9,6 +9,7 @@ #include #include #include "pid_controller_dp/typedefs.hpp" +#include "typedefs.hpp" // @brief Calculate the sine of an angle in degrees // @param angle: Angle in degrees diff --git a/scripts/ci_install_dependencies.sh b/scripts/ci_install_dependencies.sh old mode 100644 new mode 100755 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 043/290] 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 044/290] 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 85f9b1ae980772862c1f7a4d7c429339c100020b Mon Sep 17 00:00:00 2001 From: ppakr Date: Mon, 5 Jan 2026 15:30:15 +0100 Subject: [PATCH 045/290] refactor: update typedefs and conversions for Eta and Nu structures --- .../include/pid_controller_dp/typedefs.hpp | 57 +++++------- .../pid_controller_dp/src/pid_controller.cpp | 30 ++++--- .../src/pid_controller_conversions.cpp | 26 +++--- .../src/pid_controller_ros.cpp | 17 +++- .../src/pid_controller_utils.cpp | 87 +++++-------------- .../test/pid_controller_tests.cpp | 38 ++++---- 6 files changed, 110 insertions(+), 145 deletions(-) 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..039170736 100644 --- a/control/pid_controller_dp/include/pid_controller_dp/typedefs.hpp +++ b/control/pid_controller_dp/include/pid_controller_dp/typedefs.hpp @@ -7,48 +7,31 @@ #define VORTEX_EIGEN_TYPEDEFS_H #include +#include namespace types { -typedef Eigen::Matrix Vector3d; -typedef Eigen::Matrix Vector6d; -typedef Eigen::Matrix Vector7d; -typedef Eigen::Matrix Vector4d; -typedef Eigen::Matrix Matrix6d; -typedef Eigen::Matrix Matrix3d; -typedef Eigen::Matrix Matrix4x3d; -typedef Eigen::Matrix Matrix7x6d; -typedef Eigen::Matrix Matrix6x7d; -typedef Eigen::Matrix Matrix7d; -typedef Eigen::Quaterniond Quaterniond; - -struct Eta { - Eigen::Vector3d pos = Eigen::Vector3d::Zero(); - Eigen::Quaterniond ori = Eigen::Quaterniond::Identity(); - - types::Vector7d as_vector() const { - types::Vector7d vec; - vec << pos, ori.w(), ori.x(), ori.y(), ori.z(); - return vec; - } -}; - -struct Nu { - Eigen::Vector3d linear_speed = types::Vector3d::Zero(); - Eigen::Vector3d angular_speed = types::Vector3d::Zero(); - - types::Vector6d as_vector() const { - types::Vector6d vec; - vec << linear_speed, angular_speed; - return vec; - } -}; +using Vector3d = Eigen::Matrix; +using Vector4d = Eigen::Matrix; +using Vector6d = Eigen::Matrix; +using Vector7d = Eigen::Matrix; +using Matrix3d = Eigen::Matrix; +using Matrix4x3d = Eigen::Matrix; +using Matrix6d = Eigen::Matrix; +using Matrix7x6d = Eigen::Matrix; +using Matrix6x7d = Eigen::Matrix; +using Matrix7d = Eigen::Matrix; +using Quaterniond = Eigen::Quaterniond; + +// Alias canonical types from vortex utils +using Eta = ::vortex::utils::types::EtaQuat; +using Nu = ::vortex::utils::types::Nu; struct J_transformation { - Eigen::Matrix3d R = types::Matrix3d::Identity(); - types::Matrix4x3d T = types::Matrix4x3d::Zero(); + Matrix3d R = Matrix3d::Identity(); + Matrix4x3d T = Matrix4x3d::Zero(); - types::Matrix7x6d as_matrix() const { - types::Matrix7x6d mat = types::Matrix7x6d::Zero(); + Matrix7x6d as_matrix() const { + Matrix7x6d mat = Matrix7x6d::Zero(); mat.block<3, 3>(0, 0) = R; mat.block<4, 3>(3, 3) = T; return mat; diff --git a/control/pid_controller_dp/src/pid_controller.cpp b/control/pid_controller_dp/src/pid_controller.cpp index e44698e2d..a4d6e1feb 100644 --- a/control/pid_controller_dp/src/pid_controller.cpp +++ b/control/pid_controller_dp/src/pid_controller.cpp @@ -3,18 +3,19 @@ void print_eta(const types::Eta& eta) { // spdlog::info("Eta values:"); - spdlog::info("Position - North: {}, East: {}, Down: {}", eta.pos[0], - eta.pos[1], eta.pos[2]); - spdlog::info("Orientation - w: {}, x: {}, y: {}, z: {}", eta.ori.w(), - eta.ori.x(), eta.ori.y(), eta.ori.z()); + auto pos = eta.pos_vector(); + auto ori = eta.ori_quaternion(); + spdlog::info("Position - North: {}, East: {}, Down: {}", pos[0], pos[1], + pos[2]); + spdlog::info("Orientation - w: {}, x: {}, y: {}, z: {}", ori.w(), ori.x(), + ori.y(), ori.z()); } void print_nu(const types::Nu& nu) { spdlog::info("Nu values:"); - spdlog::info("Linear Speed - u: {}, v: {}, w: {}", nu.linear_speed[0], - nu.linear_speed[1], nu.linear_speed[2]); - spdlog::info("Angular Speed - p: {}, q: {}, r: {}", nu.angular_speed[0], - nu.angular_speed[1], nu.angular_speed[2]); + auto v = nu.to_vector(); + spdlog::info("Linear Speed - u: {}, v: {}, w: {}", v(0), v(1), v(2)); + spdlog::info("Angular Speed - p: {}, q: {}, r: {}", v(3), v(4), v(5)); } void print_vect_6d(const types::Vector6d& vec) { @@ -79,11 +80,12 @@ types::Vector6d PIDController::calculate_tau(const types::Eta& eta, const types::Eta& eta_dot_d) { types::Eta error = error_eta(eta, eta_d); // calculate eta error - // set w = 0 - error.ori.w() = 0.0; // only use vector part of quaternion for error + // set quaternion scalar part w = 0 (only use vector part of quaternion for + // error) + error.qw = 0.0; auto eta_dot_d_copy = eta_dot_d; - eta_dot_d_copy.ori.w() = 0.0; // set w = 0 for desired eta_dot + eta_dot_d_copy.qw = 0.0; // set w = 0 for desired eta_dot // debug // eta_error_debug = error; spdlog::info("Eta: "); @@ -99,15 +101,15 @@ types::Vector6d PIDController::calculate_tau(const types::Eta& eta, print_Jinv_transformation(J_inv); types::Vector6d nu_d = - J_inv * eta_dot_d_copy.as_vector(); // calculate velocity + J_inv * eta_dot_d_copy.to_vector(); // calculate velocity // nu_d_debug = nu_d; // print_nu(nu_d); - types::Vector6d error_nu = nu.as_vector() - nu_d; // calculate vel error + types::Vector6d error_nu = nu.to_vector() - nu_d; // calculate vel error // error_nu_debug = error_nu; // print_vect_6d(error_nu); - types::Vector6d P = Kp_ * J_inv * error.as_vector(); // P term + types::Vector6d P = Kp_ * J_inv * error.to_vector(); // P term // P_debug = P; Kp_debug = Kp_; diff --git a/control/pid_controller_dp/src/pid_controller_conversions.cpp b/control/pid_controller_dp/src/pid_controller_conversions.cpp index 8f6ff1970..d13099cac 100644 --- a/control/pid_controller_dp/src/pid_controller_conversions.cpp +++ b/control/pid_controller_dp/src/pid_controller_conversions.cpp @@ -7,12 +7,14 @@ types::Eta eta_convert_from_ros_to_eigen( const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { types::Eta eta; - eta.pos << msg->pose.pose.position.x, msg->pose.pose.position.y, - msg->pose.pose.position.z; - eta.ori.w() = msg->pose.pose.orientation.w; - eta.ori.x() = msg->pose.pose.orientation.x; - eta.ori.y() = msg->pose.pose.orientation.y; - eta.ori.z() = msg->pose.pose.orientation.z; + eta.x = msg->pose.pose.position.x; + eta.y = msg->pose.pose.position.y; + eta.z = msg->pose.pose.position.z; + + eta.qw = msg->pose.pose.orientation.w; + eta.qx = msg->pose.pose.orientation.x; + eta.qy = msg->pose.pose.orientation.y; + eta.qz = msg->pose.pose.orientation.z; return eta; } @@ -20,9 +22,13 @@ types::Eta eta_convert_from_ros_to_eigen( types::Nu nu_convert_from_ros_to_eigen( const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { types::Nu nu; - nu.linear_speed << msg->twist.twist.linear.x, msg->twist.twist.linear.y, - msg->twist.twist.linear.z; - nu.angular_speed << msg->twist.twist.angular.x, msg->twist.twist.angular.y, - msg->twist.twist.angular.z; + 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; + return nu; } diff --git a/control/pid_controller_dp/src/pid_controller_ros.cpp b/control/pid_controller_dp/src/pid_controller_ros.cpp index ece588b8e..876052b68 100644 --- a/control/pid_controller_dp/src/pid_controller_ros.cpp +++ b/control/pid_controller_dp/src/pid_controller_ros.cpp @@ -410,15 +410,24 @@ void PIDControllerNode::set_pid_params() { void PIDControllerNode::guidance_callback( const vortex_msgs::msg::ReferenceFilter::SharedPtr msg) { - eta_d_.pos << msg->x, msg->y, msg->z; + // Set desired position + eta_d_.x = msg->x; + eta_d_.y = msg->y; + eta_d_.z = msg->z; + // Convert desired attitude (roll, pitch, yaw) to quaternion and store double roll = msg->roll; double pitch = msg->pitch; double yaw = msg->yaw; - eta_d_.ori = Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX()) * - Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()); + Eigen::Quaterniond quat = Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX()) * + Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()); + + eta_d_.qw = quat.w(); + eta_d_.qx = quat.x(); + eta_d_.qy = quat.y(); + eta_d_.qz = quat.z(); } // TODO: set parameter functions diff --git a/control/pid_controller_dp/src/pid_controller_utils.cpp b/control/pid_controller_dp/src/pid_controller_utils.cpp index 97d3e607b..d85c9c4fa 100644 --- a/control/pid_controller_dp/src/pid_controller_utils.cpp +++ b/control/pid_controller_dp/src/pid_controller_utils.cpp @@ -41,90 +41,47 @@ // } types::Matrix3d calculate_R_quat(const types::Eta& eta) { - return eta.ori.normalized().toRotationMatrix(); + return eta.as_rotation_matrix(); } types::Matrix4x3d calculate_T_quat(const types::Eta& eta) { - types::Quaterniond quaternion_norm = eta.ori.normalized(); - - double w = quaternion_norm.w(); - double x = quaternion_norm.x(); - double y = quaternion_norm.y(); - double z = quaternion_norm.z(); - - types::Matrix4x3d transformation_matrix; - - transformation_matrix << -x, -y, -z, w, -z, y, z, w, -x, -y, x, w; - // transformation_matrix << -x, -y, -z, w, -z, y, -z, w, -x, -y, x, w; - - return transformation_matrix * 0.5; + return eta.as_transformation_matrix(); } types::Matrix6x7d calculate_J_sudo_inv(const types::Eta& eta) { - types::Eta eta_norm; - - eta_norm.pos = eta.pos; - eta_norm.ori = eta.ori; - - types::Matrix3d R = calculate_R_quat(eta_norm); - types::Matrix4x3d T = calculate_T_quat(eta_norm); - - types::J_transformation J; - J.R = R; - J.T = T; - // print_J_transformation(J); - - types::Matrix6x7d J_transpose = J.as_matrix().transpose(); - // spdlog::info("J_transpose (6x7) elements:"); - // print_Jinv_transformation(J_transpose); - // types::Matrix6x7d J_inv = (J.as_matrix() * J_transpose).inverse(); - - // spdlog::info(""); - // print_Jinv_transformation(J_inv); - - // (J_transpose * J.as_matrix()).inverse() * J_transpose - - // types::Matrix6x7d J_pseudo_inv = - // (J_transpose * J.as_matrix()).inverse() * J_transpose; - - types::Matrix6x7d J_pseudo_inv = - vortex::utils::math::pseudo_inverse(J.as_matrix()); + auto J_matrix = eta.as_j_matrix(); + Eigen::MatrixXd J_pseudo_inv_dynamic = + vortex::utils::math::pseudo_inverse(J_matrix); + types::Matrix6x7d J_pseudo_inv; + J_pseudo_inv = J_pseudo_inv_dynamic; return J_pseudo_inv; } types::Eta error_eta(const types::Eta& eta, const types::Eta& eta_d) { - types::Eta eta_error; - // vortex::utils::types::EtaQuat eta_error; - - eta_error.pos = eta.pos - eta_d.pos; - eta_error.ori = eta_d.ori.conjugate() * eta.ori; - - eta_error.ori = eta_error.ori.normalized(); - - return eta_error; + return eta - eta_d; } Eigen::VectorXd clamp_values(const Eigen::VectorXd& values, double min_val, double max_val) { - return values.cwiseMax(min_val).cwiseMin(max_val); - // return vortex::utils::math::clamp_values(values, min_val, max_val); + // return values.cwiseMax(min_val).cwiseMin(max_val); + return vortex::utils::math::clamp_values(values, min_val, max_val); } types::Vector7d anti_windup(const double dt, const types::Eta& error, const types::Vector7d& integral) { - types::Eta error_norm; - - error_norm.pos = error.pos; - error_norm.ori = error.ori; - - types::Vector7d integral_anti_windup = - integral + (error_norm.as_vector() * dt); - - integral_anti_windup = clamp_values(integral_anti_windup, -80.0, 80.0); - return integral_anti_windup; - // return vortex::utils::math::anti_windup(dt, error_norm.as_vector(), - // integral, -80.0, 80.0); + // Eigen::VectorXd integral_eig = integral; + // Eigen::VectorXd updated = vortex::utils::math::anti_windup( + // dt, error.to_vector(), integral_eig, -80.0, 80.0); + + // types::Vector7d integral_anti_windup; + // for (int i = 0; i < 7; ++i) { + // integral_anti_windup(i) = updated(i); + // } + + // return integral_anti_windup; + return vortex::utils::math::anti_windup(dt, error.to_vector(), integral, + -80.0, 80.0); } diff --git a/control/pid_controller_dp/test/pid_controller_tests.cpp b/control/pid_controller_dp/test/pid_controller_tests.cpp index 83a2614d6..18b2a7e6f 100644 --- a/control/pid_controller_dp/test/pid_controller_tests.cpp +++ b/control/pid_controller_dp/test/pid_controller_tests.cpp @@ -1,13 +1,8 @@ -// #include #include #include -// #include -// #include -// #include "dp_adapt_backs_controller/dp_adapt_backs_controller.hpp" -// #include "dp_adapt_backs_controller/pid_controller_utils.hpp" -// #include "dp_adapt_backs_controller/typedefs.hpp" #include +#include #include "pid_controller_dp/pid_controller.hpp" #include "pid_controller_dp/pid_controller_utils.hpp" #include "pid_controller_dp/typedefs.hpp" @@ -42,10 +37,15 @@ class PIDControllerTests : public ::testing::Test { const double pitch_angle, const double yaw_angle) { types::Eta current_pose; - current_pose.pos = types::Vector3d(north_pos, east_pos, down_pos); - current_pose.ori = vortex::utils::math::euler_to_quat( + current_pose.x = north_pos; + current_pose.y = east_pos; + current_pose.z = down_pos; + Eigen::Quaterniond q = vortex::utils::math::euler_to_quat( roll_angle, pitch_angle, yaw_angle); - + current_pose.qw = q.w(); + current_pose.qx = q.x(); + current_pose.qy = q.y(); + current_pose.qz = q.z(); return current_pose; } @@ -56,9 +56,15 @@ class PIDControllerTests : public ::testing::Test { const double pitch_angle, const double yaw_angle) { types::Eta reference_pose; - reference_pose.pos = types::Vector3d(north_pos, east_pos, down_pos); - reference_pose.ori = vortex::utils::math::euler_to_quat( + reference_pose.x = north_pos; + reference_pose.y = east_pos; + reference_pose.z = down_pos; + Eigen::Quaterniond q = vortex::utils::math::euler_to_quat( roll_angle, pitch_angle, yaw_angle); + reference_pose.qw = q.w(); + reference_pose.qx = q.x(); + reference_pose.qy = q.y(); + reference_pose.qz = q.z(); return reference_pose; } @@ -69,10 +75,12 @@ class PIDControllerTests : public ::testing::Test { const double pitch_rate, const double yaw_rate) { types::Nu current_velocity; - current_velocity.linear_speed = - types::Vector3d(surge_vel, sway_vel, heave_vel); - current_velocity.angular_speed = - types::Vector3d(roll_rate, pitch_rate, yaw_rate); + current_velocity.u = surge_vel; + current_velocity.v = sway_vel; + current_velocity.w = heave_vel; + current_velocity.p = roll_rate; + current_velocity.q = pitch_rate; + current_velocity.r = yaw_rate; return current_velocity; } From 457c2af38aa2ea83a33dfb8f3146b001bfa5d5d6 Mon Sep 17 00:00:00 2001 From: ppakr Date: Mon, 5 Jan 2026 20:35:32 +0100 Subject: [PATCH 046/290] fix: correct rqt gain update --- .../pid_controller_dp/pid_controller.hpp | 4 + .../pid_controller_dp/src/pid_controller.cpp | 10 + .../src/pid_controller_ros.cpp | 193 +----------------- 3 files changed, 21 insertions(+), 186 deletions(-) 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 d2d45891e..71a24d106 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 @@ -37,6 +37,10 @@ class PIDController { // @param dt: Time step void set_time_step(double dt); + types::Matrix6d get_kp(); + types::Matrix6d get_ki(); + types::Matrix6d get_kd(); + // parameters for debug types::Eta eta_error_debug; types::Vector6d nu_d_debug; diff --git a/control/pid_controller_dp/src/pid_controller.cpp b/control/pid_controller_dp/src/pid_controller.cpp index a4d6e1feb..60edc2014 100644 --- a/control/pid_controller_dp/src/pid_controller.cpp +++ b/control/pid_controller_dp/src/pid_controller.cpp @@ -146,3 +146,13 @@ void PIDController::set_kd(const types::Matrix6d& Kd) { void PIDController::set_time_step(double dt) { this->dt_ = dt; } + +types::Matrix6d PIDController::get_kp() { + return this->Kp_; +} +types::Matrix6d PIDController::get_ki() { + return this->Ki_; +} +types::Matrix6d PIDController::get_kd() { + return this->Kd_; +} \ No newline at end of file diff --git a/control/pid_controller_dp/src/pid_controller_ros.cpp b/control/pid_controller_dp/src/pid_controller_ros.cpp index 876052b68..e85892a03 100644 --- a/control/pid_controller_dp/src/pid_controller_ros.cpp +++ b/control/pid_controller_dp/src/pid_controller_ros.cpp @@ -108,45 +108,6 @@ void PIDControllerNode::publish_tau() { << ", " << tau(2) << ", " << tau(3) << ", " << tau(4) << ", " << tau(5) << "]"); - // print debug - // RCLCPP_INFO_STREAM(this->get_logger(), - // "Eta error: [" - // << pid_controller_.eta_error_debug.pos(0) << ", " - // << pid_controller_.eta_error_debug.pos(1) << ", " - // << pid_controller_.eta_error_debug.pos(2) << ", " - // << pid_controller_.eta_error_debug.ori.x() << ", " - // << pid_controller_.eta_error_debug.ori.y() << ", " - // << pid_controller_.eta_error_debug.ori.z() << ", " - // << pid_controller_.eta_error_debug.ori.w() << - // "]"); - // RCLCPP_INFO_STREAM(this->get_logger(), - // "Nu desired: [" << pid_controller_.nu_d_debug(0) << ", - // " - // << pid_controller_.nu_d_debug(1) << ", - // " - // << pid_controller_.nu_d_debug(2) << ", - // " - // << pid_controller_.nu_d_debug(3) << ", - // " - // << pid_controller_.nu_d_debug(4) << ", - // " - // << pid_controller_.nu_d_debug(5) << - // "]"); - // RCLCPP_INFO_STREAM(this->get_logger(), - // "Error nu: [" - // << pid_controller_.error_nu_debug(0) << ", " - // << pid_controller_.error_nu_debug(1) << ", " - // << pid_controller_.error_nu_debug(2) << ", " - // << pid_controller_.error_nu_debug(3) << ", " - // << pid_controller_.error_nu_debug(4) << ", " - // << pid_controller_.error_nu_debug(5) << "]"); - // RCLCPP_INFO_STREAM(this->get_logger(), - // "P term: [" << pid_controller_.P_debug(0) << ", " - // << pid_controller_.P_debug(1) << ", " - // << pid_controller_.P_debug(2) << ", " - // << pid_controller_.P_debug(3) << ", " - // << pid_controller_.P_debug(4) << ", " - // << pid_controller_.P_debug(5) << "]"); RCLCPP_INFO_STREAM(this->get_logger(), "Kp: [" << pid_controller_.Kp_debug(0, 0) << ", " << pid_controller_.Kp_debug(0, 1) << ", " @@ -184,105 +145,6 @@ void PIDControllerNode::publish_tau() { << pid_controller_.Kp_debug(5, 3) << ", " << pid_controller_.Kp_debug(5, 4) << ", " << pid_controller_.Kp_debug(5, 5) << "]"); - // RCLCPP_INFO_STREAM(this->get_logger(), - // "I term: [" << pid_controller_.I_debug(0) << ", " - // << pid_controller_.I_debug(1) << ", " - // << pid_controller_.I_debug(2) << ", " - // << pid_controller_.I_debug(3) << ", " - // << pid_controller_.I_debug(4) << ", " - // << pid_controller_.I_debug(5) << "]"); - // RCLCPP_INFO_STREAM(this->get_logger(), - // "D term: [" << pid_controller_.D_debug(0) << ", " - // << pid_controller_.D_debug(1) << ", " - // << pid_controller_.D_debug(2) << ", " - // << pid_controller_.D_debug(3) << ", " - // << pid_controller_.D_debug(4) << ", " - // << pid_controller_.D_debug(5) << "]"); - // RCLCPP_INFO_STREAM(this->get_logger(), - // "J inv: [" << pid_controller_.J_inv_debug(0, 0) << ", - // " - // << pid_controller_.J_inv_debug(0, 1) << ", - // " - // << pid_controller_.J_inv_debug(0, 2) << ", - // " - // << pid_controller_.J_inv_debug(0, 3) << ", - // " - // << pid_controller_.J_inv_debug(0, 4) << ", - // " - // << pid_controller_.J_inv_debug(0, 5) << ", - // " - // << pid_controller_.J_inv_debug(0, 6) << "; - // " - // << pid_controller_.J_inv_debug(1, 0) << ", - // " - // << pid_controller_.J_inv_debug(1, 1) << ", - // " - // << pid_controller_.J_inv_debug(1, 2) << ", - // " - // << pid_controller_.J_inv_debug(1, 3) << ", - // " - // << pid_controller_.J_inv_debug(1, 4) << ", - // " - // << pid_controller_.J_inv_debug(1, 5) << ", - // " - // << pid_controller_.J_inv_debug(1, 6) << "; - // " - // << pid_controller_.J_inv_debug(2, 0) << ", - // " - // << pid_controller_.J_inv_debug(2, 1) << ", - // " - // << pid_controller_.J_inv_debug(2, 2) << ", - // " - // << pid_controller_.J_inv_debug(2, 3) << ", - // " - // << pid_controller_.J_inv_debug(2, 4) << ", - // " - // << pid_controller_.J_inv_debug(2, 5) << ", - // " - // << pid_controller_.J_inv_debug(2, 6) << "; - // " - // << pid_controller_.J_inv_debug(3, 0) << ", - // " - // << pid_controller_.J_inv_debug(3, 1) << ", - // " - // << pid_controller_.J_inv_debug(3, 2) << ", - // " - // << pid_controller_.J_inv_debug(3, 3) << ", - // " - // << pid_controller_.J_inv_debug(3, 4) << ", - // " - // << pid_controller_.J_inv_debug(3, 5) << ", - // " - // << pid_controller_.J_inv_debug(3, 6) << "; - // " - // << pid_controller_.J_inv_debug(4, 0) << ", - // " - // << pid_controller_.J_inv_debug(4, 1) << ", - // " - // << pid_controller_.J_inv_debug(4, 2) << ", - // " - // << pid_controller_.J_inv_debug(4, 3) << ", - // " - // << pid_controller_.J_inv_debug(4, 4) << ", - // " - // << pid_controller_.J_inv_debug(4, 5) << ", - // " - // << pid_controller_.J_inv_debug(4, 6) << "; - // " - // << pid_controller_.J_inv_debug(5, 0) << ", - // " - // << pid_controller_.J_inv_debug(5, 1) << ", - // " - // << pid_controller_.J_inv_debug(5, 2) << ", - // " - // << pid_controller_.J_inv_debug(5, 3) << ", - // " - // << pid_controller_.J_inv_debug(5, 4) << ", - // " - // << pid_controller_.J_inv_debug(5, 5) << ", - // " - // << pid_controller_.J_inv_debug(5, 6) << - // "]"); geometry_msgs::msg::WrenchStamped tau_msg; tau_msg.header.stamp = this->now(); @@ -298,7 +160,6 @@ void PIDControllerNode::publish_tau() { } void PIDControllerNode::set_pid_params() { - // TODO: edit parameter declaration this->declare_parameter("Kp_x", 1.0); this->declare_parameter("Kp_y", 1.0); this->declare_parameter("Kp_z", 1.0); @@ -329,7 +190,6 @@ void PIDControllerNode::set_pid_params() { // std::vector Ki_vec = this->get_parameter("Ki").as_double_array(); // std::vector Kd_vec = this->get_parameter("Kd").as_double_array(); - // TODO: construct vector from parameters std::vector Kp_vec = { this->get_parameter("Kp_x").as_double(), this->get_parameter("Kp_y").as_double(), @@ -363,46 +223,6 @@ void PIDControllerNode::set_pid_params() { types::Matrix6d Ki_eigen = Ki_vec_eigen.asDiagonal().toDenseMatrix(); types::Matrix6d Kd_eigen = Kd_vec_eigen.asDiagonal().toDenseMatrix(); - // print out for debug - // RCLCPP_INFO_STREAM(this->get_logger(), - // "Kp_eigen: [" - // << Kp_eigen(0, 0) << ", " << Kp_eigen(0, 1) << ", - // " - // << Kp_eigen(0, 2) << ", " << Kp_eigen(0, 3) << ", - // " - // << Kp_eigen(0, 4) << ", " << Kp_eigen(0, 5) << "; - // " - // << Kp_eigen(1, 0) << ", " << Kp_eigen(1, 1) << ", - // " - // << Kp_eigen(1, 2) << ", " << Kp_eigen(1, 3) << ", - // " - // << Kp_eigen(1, 4) << ", " << Kp_eigen(1, 5) << "; - // " - // << Kp_eigen(2, 0) << ", " << Kp_eigen(2, 1) << ", - // " - // << Kp_eigen(2, 2) << ", " << Kp_eigen(2, 3) << ", - // " - // << Kp_eigen(2, 4) << ", " << Kp_eigen(2, 5) << "; - // " - // << Kp_eigen(3, 0) << ", " << Kp_eigen(3, 1) << ", - // " - // << Kp_eigen(3, 2) << ", " << Kp_eigen(3, 3) << ", - // " - // << Kp_eigen(3, 4) << ", " << Kp_eigen(3, 5) << "; - // " - // << Kp_eigen(4, 0) << ", " << Kp_eigen(4, 1) << ", - // " - // << Kp_eigen(4, 2) << ", " << Kp_eigen(4, 3) << ", - // " - // << Kp_eigen(4, 4) << ", " << Kp_eigen(4, 5) << "; - // " - // << Kp_eigen(5, 0) << ", " << Kp_eigen(5, 1) << ", - // " - // << Kp_eigen(5, 2) << ", " << Kp_eigen(5, 3) << ", - // " - // << Kp_eigen(5, 4) << ", " << Kp_eigen(5, 5) << - // "]"); - pid_controller_.set_kp(Kp_eigen); pid_controller_.set_ki(Ki_eigen); pid_controller_.set_kd(Kd_eigen); @@ -420,9 +240,10 @@ void PIDControllerNode::guidance_callback( double pitch = msg->pitch; double yaw = msg->yaw; - Eigen::Quaterniond quat = Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX()) * - Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()); + Eigen::Quaterniond quat = + Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX()) * + Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()); eta_d_.qw = quat.w(); eta_d_.qx = quat.x(); @@ -458,9 +279,9 @@ rcl_interfaces::msg::SetParametersResult PIDControllerNode::parametersCallback( bool kd_pitch_updated = false; bool kd_yaw_updated = false; - types::Vector6d Kp_vec_eigen; - types::Vector6d Ki_vec_eigen; - types::Vector6d Kd_vec_eigen; + types::Vector6d Kp_vec_eigen = pid_controller_.get_kp().diagonal(); + types::Vector6d Ki_vec_eigen = pid_controller_.get_ki().diagonal(); + types::Vector6d Kd_vec_eigen = pid_controller_.get_kd().diagonal(); for (const auto& param : parameters) { if (param.get_name() == "Kp_x") { From cb4156b15a5b7a2bf537ab1d267ce66c81dde215 Mon Sep 17 00:00:00 2001 From: Anbit Date: Tue, 6 Jan 2026 12:32:51 +0100 Subject: [PATCH 047/290] 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 048/290] 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 049/290] 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 050/290] 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 051/290] 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 052/290] 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 053/290] 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 054/290] 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 055/290] 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 056/290] 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 057/290] 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 058/290] 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 059/290] 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 060/290] 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 061/290] 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 062/290] 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 063/290] 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 064/290] 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 065/290] 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 d7eee6493ce2c5a82ebe5bc59d5d1e05746a4d87 Mon Sep 17 00:00:00 2001 From: forrisdahl Date: Sun, 1 Feb 2026 14:55:51 +0100 Subject: [PATCH 066/290] refactor: remove exceptions, use memcpy instead of for loop --- .../thruster_interface_auv_driver.hpp | 9 +++- .../src/thruster_interface_auv_driver.cpp | 53 ++++++++----------- 2 files changed, 30 insertions(+), 32 deletions(-) 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 6e2f0ebd4..5b4320a43 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 @@ -65,6 +65,12 @@ class ThrusterInterfaceAUVDriver { int pico_i2c_address, const std::vector& thruster_parameters, const std::vector>& poly_coeffs); + + /** + * @brief initializes i2c + * @return 0 on success, negative number on failure + */ + int init_i2c(); /** * @brief calls both 1) interpolate_forces_to_pwm() to * convert the thruster forces to PWM values and 2) send_data_to_escs() to @@ -126,8 +132,9 @@ class ThrusterInterfaceAUVDriver { * to the ESCs via I2C * * @param thruster_pwm_array vector of pwm values to send + * @return 0 on success, -1 on failure */ - void send_data_to_escs(const std::vector& thruster_pwm_array); + int send_data_to_escs(const std::vector& thruster_pwm_array); /** * @brief convert Newtons to Kg 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 86834a8e7..26b803ef7 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp @@ -1,6 +1,7 @@ #include "thruster_interface_auv/thruster_interface_auv_driver.hpp" #include #include +#include #include #include @@ -13,18 +14,22 @@ ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver( pico_i2c_address_(pico_i2c_address), thruster_parameters_(thruster_parameters), poly_coeffs_(poly_coeffs) { + idle_pwm_value_ = + (calc_poly(0, poly_coeffs_[LEFT]) + calc_poly(0, poly_coeffs_[RIGHT])) / + 2; +} + +int ThrusterInterfaceAUVDriver::init_i2c() { std::string i2c_filename = std::format("/dev/i2c-{}", i2c_bus_); - bus_fd_ = - open(i2c_filename.c_str(), - O_RDWR); // Open the i2c bus for reading and writing (0_RDWR) + bus_fd_ = open(i2c_filename.c_str(), O_RDWR); if (bus_fd_ < 0) { - std::runtime_error(std::format("ERROR: Failed to open I2C bus {} : {}", - i2c_bus_, strerror(errno))); + return bus_fd_; } - idle_pwm_value_ = - (calc_poly(0, poly_coeffs_[LEFT]) + calc_poly(0, poly_coeffs_[RIGHT])) / - 2; + if (ioctl(bus_fd_, I2C_SLAVE, pico_i2c_address_) < 0) { + return -1; + } + return 0; } ThrusterInterfaceAUVDriver::~ThrusterInterfaceAUVDriver() { @@ -65,31 +70,21 @@ std::uint16_t ThrusterInterfaceAUVDriver::calc_poly( coeffs[2] * force + coeffs[3]); } -void ThrusterInterfaceAUVDriver::send_data_to_escs( +int ThrusterInterfaceAUVDriver::send_data_to_escs( const std::vector& thruster_pwm_array) { constexpr std::size_t i2c_data_size = 1 + 8 * 2; // 8 thrusters * (1xMSB + 1xLSB) - std::vector i2c_data_array; - i2c_data_array.reserve(i2c_data_size); + std::array i2c_data_array; - i2c_data_array.push_back(0x00); // Start byte - std::ranges::for_each(thruster_pwm_array, [&](std::uint16_t pwm) { - std::array bytes = pwm_to_i2c_data(pwm); - std::ranges::copy(bytes, std::back_inserter(i2c_data_array)); - }); + i2c_data_array[0] = 0; - // Set the I2C slave address - if (ioctl(bus_fd_, I2C_SLAVE, pico_i2c_address_) < 0) { - throw std::runtime_error(std::format("Failed to open I2C bus {} : {}", - i2c_bus_, strerror(errno))); - return; - } + std::memcpy(i2c_data_array.data() + 1, thruster_pwm_array.data(), + i2c_data_size - 1); - // Write data to the I2C device if (write(bus_fd_, i2c_data_array.data(), i2c_data_size) != i2c_data_size) { - throw std::runtime_error(std::format( - "ERROR: Failed to write to I2C device : {}", strerror(errno))); + return -1; } + return 0; } std::vector ThrusterInterfaceAUVDriver::drive_thrusters( @@ -116,12 +111,8 @@ std::vector ThrusterInterfaceAUVDriver::drive_thrusters( return result; }); - try { - send_data_to_escs(thruster_pwm_array); - } catch (const std::exception& e) { - spdlog::error("ERROR: Failed to send PWM values - {}", e.what()); - } catch (...) { - spdlog::error("ERROR: Failed to send PWM values - Unknown exception"); + if (send_data_to_escs(thruster_pwm_array)){ + return {}; } return thruster_pwm_array; From 3f953f1e21b2de5fc017129001f9463387e71497 Mon Sep 17 00:00:00 2001 From: forrisdahl Date: Sun, 1 Feb 2026 14:56:08 +0100 Subject: [PATCH 067/290] refactor: remove clamping Clamping will be done on the MCU --- .../src/thruster_interface_auv_driver.cpp | 9 --------- 1 file changed, 9 deletions(-) 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 26b803ef7..be0028e0e 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp @@ -102,15 +102,6 @@ std::vector ThrusterInterfaceAUVDriver::drive_thrusters( std::vector thruster_pwm_array = interpolate_forces_to_pwm(mapped_forces); - std::ranges::transform(thruster_pwm_array, thruster_pwm_array.begin(), - [this, i = 0](auto pwm) mutable { - auto result = std::clamp( - pwm, thruster_parameters_[i].pwm_min, - thruster_parameters_[i].pwm_max); - ++i; - return result; - }); - if (send_data_to_escs(thruster_pwm_array)){ return {}; } From 87d8c536848b6e966a2f8053b909405ec14afa42 Mon Sep 17 00:00:00 2001 From: forrisdahl Date: Sun, 1 Feb 2026 15:01:07 +0100 Subject: [PATCH 068/290] refactor: simplify code by using for loop --- .../src/thruster_interface_auv_driver.cpp | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) 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 be0028e0e..93c95fa6a 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp @@ -84,6 +84,7 @@ int ThrusterInterfaceAUVDriver::send_data_to_escs( if (write(bus_fd_, i2c_data_array.data(), i2c_data_size) != i2c_data_size) { return -1; } + return 0; } @@ -92,17 +93,20 @@ std::vector ThrusterInterfaceAUVDriver::drive_thrusters( // Apply thruster mapping and direction std::vector mapped_forces(thruster_forces_array.size()); - std::ranges::transform(thruster_parameters_, mapped_forces.begin(), - [this, &thruster_forces_array](const auto& param) { - return thruster_forces_array[param.mapping] * - param.direction; - }); + for (std::size_t i = 0; i < thruster_parameters_.size(); ++i) { + const auto& param = thruster_parameters_[i]; + + const std::size_t idx = param.mapping; + + const double raw_force = thruster_forces_array[idx]; + mapped_forces[i] = raw_force * param.direction; + } // Convert forces to PWM std::vector thruster_pwm_array = interpolate_forces_to_pwm(mapped_forces); - if (send_data_to_escs(thruster_pwm_array)){ + if (send_data_to_escs(thruster_pwm_array)) { return {}; } From 86e9bc8eaca63e4fc486039960bcac15300080c7 Mon Sep 17 00:00:00 2001 From: forrisdahl Date: Sun, 1 Feb 2026 15:05:15 +0100 Subject: [PATCH 069/290] refactor: use simple for loop in interpolate_forces_to_pwm function --- .../src/thruster_interface_auv_driver.cpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) 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 93c95fa6a..693c002b4 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp @@ -42,12 +42,15 @@ ThrusterInterfaceAUVDriver::~ThrusterInterfaceAUVDriver() { std::vector ThrusterInterfaceAUVDriver::interpolate_forces_to_pwm( const std::vector& thruster_forces_array) { - // Convert Newtons to Kg (since the thruster datasheet is in Kg) - auto pwm_view = thruster_forces_array | std::views::transform(to_kg) | - std::views::transform([this](double force_in_kg) { - return force_to_pwm(force_in_kg, poly_coeffs_); - }); - return std::vector(pwm_view.begin(), pwm_view.end()); + std::vector pwm; + pwm.resize(thruster_forces_array.size()); // exactly one allocation + + for (std::size_t i = 0; i < thruster_forces_array.size(); ++i) { + const double force_in_kg = to_kg(thruster_forces_array[i]); + pwm[i] = force_to_pwm(force_in_kg, poly_coeffs_); + } + + return pwm; } std::uint16_t ThrusterInterfaceAUVDriver::force_to_pwm( From 77e7d226227de43516fbb8ab1fa4168c1753e68c Mon Sep 17 00:00:00 2001 From: forrisdahl Date: Sun, 1 Feb 2026 15:05:57 +0100 Subject: [PATCH 070/290] refactor: use init_i2c in node --- motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp | 1 + 1 file changed, 1 insertion(+) 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 603b2d46e..9e921b264 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp @@ -33,6 +33,7 @@ ThrusterInterfaceAUVNode::ThrusterInterfaceAUVNode( thruster_driver_ = std::make_unique( i2c_bus_, i2c_address_, thruster_parameters_, poly_coeffs_); + thruster_driver_.init_i2c(); thruster_forces_array_ = std::vector(8, 0.00); From 6af0cdad1f76e6a92a2c16fd1dcb69d723e33441 Mon Sep 17 00:00:00 2001 From: forrisdahl Date: Sun, 1 Feb 2026 18:20:46 +0100 Subject: [PATCH 071/290] refactor: use two vectors in stead of a 2d vector --- .../thruster_interface_auv_driver.hpp | 34 ++++++------------- .../thruster_interface_auv_ros.hpp | 3 +- .../src/thruster_interface_auv_driver.cpp | 21 ++++++------ .../src/thruster_interface_auv_ros.cpp | 9 ++--- 4 files changed, 25 insertions(+), 42 deletions(-) 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 5b4320a43..8b11f913d 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 @@ -57,14 +57,16 @@ class ThrusterInterfaceAUVDriver { * @param pico_i2c_address i2c address of the ESC that drive the * @param thruster_parameters describe mapping, direction, min and max pwm * value for each thruster - * @param poly_coeffs LEFT(<0) and RIGHT(>0) third order + * @param left_coeffs LEFT(<0) and RIGHT(>0) third order + * @param right_coeffs * polynomial coefficients */ - ThrusterInterfaceAUVDriver( + ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver( std::int16_t i2c_bus, int pico_i2c_address, const std::vector& thruster_parameters, - const std::vector>& poly_coeffs); + const std::vector& right_coeffs, + const std::vector& left_coeffs); /** * @brief initializes i2c @@ -84,15 +86,13 @@ class ThrusterInterfaceAUVDriver { const std::vector& thruster_forces_array); private: - int bus_fd_; ///< file descriptor for the I2C bus (integer >0 that uniquely - ///< identifies the device. -1 if it fails) - + int bus_fd_; int i2c_bus_; int pico_i2c_address_; std::vector thruster_parameters_; - std::vector> poly_coeffs_; - - uint16_t idle_pwm_value_; ///< pwm value when force = 0.00 + std::vector right_coeffs_; + std::vector left_coeffs_; + uint16_t idle_pwm_value_; /** * @brief only take the thruster forces and return PWM values @@ -115,8 +115,7 @@ class ThrusterInterfaceAUVDriver { * * @return std::uint16_t scalar pwm value */ - std::uint16_t force_to_pwm(double force, - const std::vector>& coeffs); + std::uint16_t force_to_pwm(double force); /** * @brief compute y = a*x^3 + b*x^2 + c*x + d @@ -144,19 +143,6 @@ class ThrusterInterfaceAUVDriver { * @return double Kg */ static constexpr double to_kg(double force) { return force / 9.80665; } - - /** - * @brief convert pwm values to i2c bytes - * - * @param pwm pwm value - * - * @return std::array i2c data - */ - static constexpr std::array pwm_to_i2c_data( - std::uint16_t pwm) { - return {static_cast((pwm >> 8) & 0xFF), - static_cast(pwm & 0xFF)}; - } }; #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 58ca254fb..8c3752769 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 @@ -57,7 +57,8 @@ class ThrusterInterfaceAUVNode : public rclcpp::Node { std::string subscriber_topic_name_; std::string publisher_topic_name_; std::vector thruster_parameters_; - std::vector> poly_coeffs_; + std::vector> left_coeffs_; + std::vector> right_coeffs_; std::vector thruster_forces_array_; bool debug_flag_; 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 693c002b4..9357fda18 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp @@ -9,14 +9,15 @@ ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver( std::int16_t i2c_bus, int pico_i2c_address, const std::vector& thruster_parameters, - const std::vector>& poly_coeffs) + const std::vector& right_coeffs, + const std::vector& left_coeffs) : i2c_bus_(i2c_bus), pico_i2c_address_(pico_i2c_address), thruster_parameters_(thruster_parameters), - poly_coeffs_(poly_coeffs) { + right_coeffs_(right_coeffs), + left_coeffs_(left_coeffs) { idle_pwm_value_ = - (calc_poly(0, poly_coeffs_[LEFT]) + calc_poly(0, poly_coeffs_[RIGHT])) / - 2; + (calc_poly(0, left_coeffs_) + calc_poly(0, right_coeffs_)) / 2; } int ThrusterInterfaceAUVDriver::init_i2c() { @@ -43,23 +44,21 @@ ThrusterInterfaceAUVDriver::~ThrusterInterfaceAUVDriver() { std::vector ThrusterInterfaceAUVDriver::interpolate_forces_to_pwm( const std::vector& thruster_forces_array) { std::vector pwm; - pwm.resize(thruster_forces_array.size()); // exactly one allocation + pwm.resize(thruster_forces_array.size()); for (std::size_t i = 0; i < thruster_forces_array.size(); ++i) { const double force_in_kg = to_kg(thruster_forces_array[i]); - pwm[i] = force_to_pwm(force_in_kg, poly_coeffs_); + pwm[i] = force_to_pwm(force_in_kg); } return pwm; } -std::uint16_t ThrusterInterfaceAUVDriver::force_to_pwm( - double force, - const std::vector>& coeffs) { +std::uint16_t ThrusterInterfaceAUVDriver::force_to_pwm(double force) { if (force < 0) { - return calc_poly(force, coeffs[LEFT]); + return calc_poly(force, left_coeffs_); } else if (force > 0) { - return calc_poly(force, coeffs[RIGHT]); + return calc_poly(force, right_coeffs_); } else { return idle_pwm_value_; // 1500 } 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 9e921b264..412e7746b 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp @@ -32,7 +32,7 @@ ThrusterInterfaceAUVNode::ThrusterInterfaceAUVNode( vortex::utils::qos_profiles::reliable_profile(1)); thruster_driver_ = std::make_unique( - i2c_bus_, i2c_address_, thruster_parameters_, poly_coeffs_); + i2c_bus_, i2c_address_, thruster_parameters_, right_coeffs_, left_coeffs_); thruster_driver_.init_i2c(); thruster_forces_array_ = std::vector(8, 0.00); @@ -132,9 +132,9 @@ void ThrusterInterfaceAUVNode::extract_all_parameters() { this->get_parameter("propulsion.thrusters.thruster_PWM_max") .as_integer_array(); - std::vector left_coeffs = + this->left_coeffs_ = this->get_parameter("coeffs.16V.LEFT").as_double_array(); - std::vector right_coeffs = + this->right_coeffs_ = this->get_parameter("coeffs.16V.RIGHT").as_double_array(); this->i2c_bus_ = this->get_parameter("i2c.bus").as_int(); @@ -160,9 +160,6 @@ void ThrusterInterfaceAUVNode::extract_all_parameters() { std::back_inserter(this->thruster_parameters_), create_thruster_parameters); - this->poly_coeffs_.push_back(left_coeffs); - this->poly_coeffs_.push_back(right_coeffs); - double timout_treshold_param = this->get_parameter("propulsion.thrusters.watchdog_timeout") .as_double(); From d1a449516061d48c4b995d04d0a77476f3b896d4 Mon Sep 17 00:00:00 2001 From: forrisdahl Date: Sun, 1 Feb 2026 18:21:43 +0100 Subject: [PATCH 072/290] refactor: remove start byte start byte is not needed and makes parsing harder for no benifit --- .../src/thruster_interface_auv_driver.cpp | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) 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 9357fda18..50e778438 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp @@ -74,14 +74,11 @@ std::uint16_t ThrusterInterfaceAUVDriver::calc_poly( int ThrusterInterfaceAUVDriver::send_data_to_escs( const std::vector& thruster_pwm_array) { - constexpr std::size_t i2c_data_size = - 1 + 8 * 2; // 8 thrusters * (1xMSB + 1xLSB) + constexpr std::size_t i2c_data_size = 8 * 2; std::array i2c_data_array; - i2c_data_array[0] = 0; - - std::memcpy(i2c_data_array.data() + 1, thruster_pwm_array.data(), - i2c_data_size - 1); + std::memcpy(i2c_data_array.data(), thruster_pwm_array.data(), + i2c_data_size); if (write(bus_fd_, i2c_data_array.data(), i2c_data_size) != i2c_data_size) { return -1; @@ -92,7 +89,7 @@ int ThrusterInterfaceAUVDriver::send_data_to_escs( std::vector ThrusterInterfaceAUVDriver::drive_thrusters( const std::vector& thruster_forces_array) { - // Apply thruster mapping and direction + std::vector mapped_forces(thruster_forces_array.size()); for (std::size_t i = 0; i < thruster_parameters_.size(); ++i) { @@ -104,7 +101,6 @@ std::vector ThrusterInterfaceAUVDriver::drive_thrusters( mapped_forces[i] = raw_force * param.direction; } - // Convert forces to PWM std::vector thruster_pwm_array = interpolate_forces_to_pwm(mapped_forces); From 68ed08223e034e82bef396d9b4c0293739c45748 Mon Sep 17 00:00:00 2001 From: forrisdahl Date: Sun, 1 Feb 2026 18:26:39 +0100 Subject: [PATCH 073/290] fix: syntax error ros.hpp --- .../thruster_interface_auv/thruster_interface_auv_ros.hpp | 4 ++-- .../src/thruster_interface_auv_driver.cpp | 1 - .../thruster_interface_auv/src/thruster_interface_auv_ros.cpp | 3 ++- 3 files changed, 4 insertions(+), 4 deletions(-) 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 8c3752769..9050610f2 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 @@ -57,8 +57,8 @@ class ThrusterInterfaceAUVNode : public rclcpp::Node { std::string subscriber_topic_name_; std::string publisher_topic_name_; std::vector thruster_parameters_; - std::vector> left_coeffs_; - std::vector> right_coeffs_; + std::vector left_coeffs_; + std::vector right_coeffs_; std::vector thruster_forces_array_; bool debug_flag_; 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 50e778438..c8c06a8c4 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp @@ -89,7 +89,6 @@ int ThrusterInterfaceAUVDriver::send_data_to_escs( std::vector ThrusterInterfaceAUVDriver::drive_thrusters( const std::vector& thruster_forces_array) { - std::vector mapped_forces(thruster_forces_array.size()); for (std::size_t i = 0; i < thruster_parameters_.size(); ++i) { 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 412e7746b..08337cd43 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp @@ -32,7 +32,8 @@ ThrusterInterfaceAUVNode::ThrusterInterfaceAUVNode( vortex::utils::qos_profiles::reliable_profile(1)); thruster_driver_ = std::make_unique( - i2c_bus_, i2c_address_, thruster_parameters_, right_coeffs_, left_coeffs_); + i2c_bus_, i2c_address_, thruster_parameters_, right_coeffs_, + left_coeffs_); thruster_driver_.init_i2c(); thruster_forces_array_ = std::vector(8, 0.00); From 392c408da26bd3508142600c749f87a0f96e4637 Mon Sep 17 00:00:00 2001 From: forrisdahl Date: Tue, 3 Feb 2026 16:23:31 +0100 Subject: [PATCH 074/290] refactor: add optional for handling message failure --- .../src/thruster_interface_auv_driver.cpp | 4 +--- .../src/thruster_interface_auv_ros.cpp | 9 +++++++-- 2 files changed, 8 insertions(+), 5 deletions(-) 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 c8c06a8c4..b82584a59 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp @@ -1,9 +1,7 @@ #include "thruster_interface_auv/thruster_interface_auv_driver.hpp" -#include #include #include #include -#include ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver( std::int16_t i2c_bus, @@ -87,7 +85,7 @@ int ThrusterInterfaceAUVDriver::send_data_to_escs( return 0; } -std::vector ThrusterInterfaceAUVDriver::drive_thrusters( +std::optional> ThrusterInterfaceAUVDriver::drive_thrusters( const std::vector& thruster_forces_array) { std::vector mapped_forces(thruster_forces_array.size()); 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 08337cd43..8938e333f 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp @@ -61,10 +61,15 @@ void ThrusterInterfaceAUVNode::pwm_callback() { std::vector thruster_pwm_array = thruster_driver_->drive_thrusters(this->thruster_forces_array_); + if (thruster_pwm_array.has_value() == false){ + spdlog::warn("Sending PWM values to thrusters failed"); + } + + if (debug_flag_) { std_msgs::msg::Int16MultiArray pwm_message; - pwm_message.data = std::vector(thruster_pwm_array.begin(), - thruster_pwm_array.end()); + pwm_message.data = std::vector(thruster_pwm_array.value().begin(), + thruster_pwm_array.value().end()); thruster_pwm_publisher_->publish(pwm_message); } } From 6a13ca136e74bb2c0aae3073b21c641674cb2aea Mon Sep 17 00:00:00 2001 From: Anbit Date: Wed, 4 Feb 2026 11:46:25 +0100 Subject: [PATCH 075/290] 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 b4aefb94026bb455164644a6a936fed0f3ee3d20 Mon Sep 17 00:00:00 2001 From: ppakr Date: Thu, 5 Feb 2026 21:39:40 +0100 Subject: [PATCH 076/290] fix: update type aliases for Eta and Nu to use Pose and Twist --- .../pid_controller_dp/include/pid_controller_dp/typedefs.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 0fe90cc90..d2f6a8707 100644 --- a/control/pid_controller_dp/include/pid_controller_dp/typedefs.hpp +++ b/control/pid_controller_dp/include/pid_controller_dp/typedefs.hpp @@ -23,8 +23,8 @@ using Matrix7d = Eigen::Matrix; using Quaterniond = Eigen::Quaterniond; // Alias canonical types from vortex utils -using Eta = ::vortex::utils::types::EtaQuat; -using Nu = ::vortex::utils::types::Nu; +using Eta = ::vortex::utils::types::Pose; +using Nu = ::vortex::utils::types::Twist; struct J_transformation { Matrix3d R = Matrix3d::Identity(); From 5d314097f54a8750551a80b84308db19a2f3ab4f Mon Sep 17 00:00:00 2001 From: Henrik Date: Mon, 9 Feb 2026 18:42:06 +0100 Subject: [PATCH 077/290] 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 078/290] 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 079/290] 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 080/290] 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 081/290] 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 f8e4c239f29764e1b4f98cd71900d6abb80652e4 Mon Sep 17 00:00:00 2001 From: Talha Nauman Choudhry Date: Sun, 9 Feb 2025 19:47:52 +0100 Subject: [PATCH 082/290] feat: added the math and setup of the ESKF, need some changes --- navigation/eskf_python/CMakeLists.txt | 33 ++ navigation/eskf_python/README.md | 0 .../eskf_python/config/eskf_python.yaml | 3 + .../eskf_python/eskf_python/__init__.py | 0 .../eskf_python/eskf_python_filter.py | 511 ++++++++++++++++++ .../eskf_python/eskf_python_node.py | 103 ++++ navigation/eskf_python/launch/eskf.launch.py | 22 + navigation/eskf_python/package.xml | 23 + 8 files changed, 695 insertions(+) create mode 100644 navigation/eskf_python/CMakeLists.txt create mode 100644 navigation/eskf_python/README.md create mode 100644 navigation/eskf_python/config/eskf_python.yaml create mode 100644 navigation/eskf_python/eskf_python/__init__.py create mode 100644 navigation/eskf_python/eskf_python/eskf_python_filter.py create mode 100644 navigation/eskf_python/eskf_python/eskf_python_node.py create mode 100644 navigation/eskf_python/launch/eskf.launch.py create mode 100644 navigation/eskf_python/package.xml diff --git a/navigation/eskf_python/CMakeLists.txt b/navigation/eskf_python/CMakeLists.txt new file mode 100644 index 000000000..b4fc9118c --- /dev/null +++ b/navigation/eskf_python/CMakeLists.txt @@ -0,0 +1,33 @@ +cmake_minimum_required(VERSION 3.8) +project(eskf_python) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake_python REQUIRED) +find_package(rclpy REQUIRED) +find_package(vortex_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) + +ament_python_install_package(${PROJECT_NAME}) + +install(DIRECTORY + launch + config + DESTINATION share/${PROJECT_NAME} +) + +install(PROGRAMS + eskf_python/eskf_python_node.py + DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + find_package(ament_cmake_pytest REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) +endif() + +ament_package() diff --git a/navigation/eskf_python/README.md b/navigation/eskf_python/README.md new file mode 100644 index 000000000..e69de29bb diff --git a/navigation/eskf_python/config/eskf_python.yaml b/navigation/eskf_python/config/eskf_python.yaml new file mode 100644 index 000000000..0d80b90df --- /dev/null +++ b/navigation/eskf_python/config/eskf_python.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + eskf_python_node: diff --git a/navigation/eskf_python/eskf_python/__init__.py b/navigation/eskf_python/eskf_python/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/navigation/eskf_python/eskf_python/eskf_python_filter.py b/navigation/eskf_python/eskf_python/eskf_python_filter.py new file mode 100644 index 000000000..286747467 --- /dev/null +++ b/navigation/eskf_python/eskf_python/eskf_python_filter.py @@ -0,0 +1,511 @@ +from dataclasses import dataclass, field +from typing import tuple + +import numpy as np + + +@dataclass +class StateVector_quaternion: + position: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Position vector (x, y, z) + velocity: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Velocity vector (u, v, w) + orientation: np.ndarray = field( + default_factory=lambda: np.zeros(4) + ) # Orientation quaternion (w, x, y, z) + acceleration_bias: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Acceleration bias vector (b_ax, b_ay, b_az) + gyro_bias: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Gyro bias vector (b_gx, b_gy, b_gz) + + def R_q(self) -> np.ndarray: + """Calculates the rotation matrix from the orientation quaternion. + + Returns: + np.ndarray: The rotation matrix. + """ + q0, q1, q2, q3 = self.orientation + R = np.array( + [ + [ + 1 - 2 * q2**2 - 2 * q3**2, + 2 * (q1 * q2 - q0 * q3), + 2 * (q0 * q2 + q1 * q3), + ], + [ + 2 * (q1 * q2 + q0 * q3), + 1 - 2 * q1**2 - 2 * q3**2, + 2 * (q2 * q3 - q0 * q1), + ], + [ + 2 * (q1 * q3 - q0 * q2), + 2 * (q0 * q1 + q2 * q3), + 1 - 2 * q1**2 - 2 * q2**2, + ], + ] + ) + + return R + + def euler_forward( + self, current_state: 'StateVector_quaternion', dt: float + ) -> 'StateVector_quaternion': + # Define the new state + new_state = StateVector_quaternion() + + # Define the state derivatives + new_state.position = current_state.position + self.position * dt + new_state.velocity = current_state.velocity + self.velocity * dt + new_state.orientation = current_state.orientation + self.orientation * dt + new_state.acceleration_bias = ( + current_state.acceleration_bias + self.acceleration_bias * dt + ) + new_state.gyro_bias = current_state.gyro_bias + self.gyro_bias * dt + + # Normalize the orientation quaternion + new_state.orientation /= np.linalg.norm(new_state.orientation) + + return new_state + + +@dataclass +class StateVector_euler: + position: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Position vector (x, y, z) + velocity: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Velocity vector (u, v, w) + orientation: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Orientation angles (roll, pitch, yaw) + acceleration_bias: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Acceleration bias vector (b_ax, b_ay, b_az) + gyro_bias: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Gyro bias vector (b_gx, b_gy, b_gz) + covariance: np.ndarray = field( + default_factory=lambda: np.zeros((15, 15)) + ) # Covariance matrix + + def fill_states(self, state: np.ndarray) -> None: + """Fills the state vector with the values from a numpy array. + + Args: + state (np.ndarray): The state vector. + """ + self.position = state[0:3] + self.velocity = state[3:6] + self.orientation = state[6:9] + self.acceleration_bias = state[9:12] + self.gyro_bias = state[12:15] + + def copy_state(self, wanted_state: 'StateVector_euler') -> None: + """Copies the state from a StateVector object into the current StateVector object. + + Args: + wanted_state (StateVector_euler): The quaternion state to copy from. + """ + self.position = wanted_state.position + self.velocity = wanted_state.velocity + self.orientation = wanted_state.orientation + self.acceleration_bias = wanted_state.acceleration_bias + self.gyro_bias = wanted_state.gyro_bias + + +@dataclass +class MeasurementModel: + measurement: np.ndarray = field( + default_factory=lambda: np.zeros(6) + ) # Measurement vector + measurement_matrix: np.ndarray = field( + default_factory=lambda: np.zeros((6, 15)) + ) # Measurement matrix + measurement_covariance: np.ndarray = field( + default_factory=lambda: np.zeros((6, 6)) + ) # Measurement noise matrix + + +class ErrorStateKalmanFilter: + def __init__( + self, + P_ab: np.ndarray, + P_wb: np.ndarray, + Q: np.ndarray, + lever_arm: np.array, + R: np.ndarray, + g: float, + dt: float, + ) -> None: + self.P_ab = P_ab + self.P_wb = P_wb + self.Q_process_noise = Q + self.lever_arm = lever_arm + self.R = R + self.g = np.array([0, 0, g]) + self.dt = dt + + def skew_symmetric(self, vector: np.ndarray) -> np.ndarray: + """Calculates the skew symmetric matrix of a vector. + + Args: + vector (np.ndarray): The vector. + + Returns: + np.ndarray: The skew symmetric matrix. + """ + return np.array( + [ + [0, -vector[2], vector[1]], + [vector[2], 0, -vector[0]], + [-vector[1], vector[0], 0], + ] + ) + + def quaternion_super_product(self, q1: np.ndarray, q2: np.ndarray) -> np.ndarray: + """Calculates the quaternion super product of two quaternions. + + Args: + q1 (np.ndarray): The first quaternion. + q2 (np.ndarray): The second quaternion. + + Returns: + np.ndarray: The quaternion super product. + """ + nu_0, eta_0_x, eta_0_y, eta_0_z = q1 + nu_1, eta_1_x, eta_1_y, eta_1_z = q2 + + eta_0 = np.array([[eta_1_x, eta_1_y, eta_1_z]]).T + eta_1 = np.array([[eta_0_x, eta_0_y, eta_0_z]]).T + + eta_new = ( + nu_1 * eta_0 + nu_0 * eta_1 + np.dot(self.skew_symmetric(eta_0), eta_1) + ) + nu_new = nu_0 * nu_1 - np.dot(eta_0.T, eta_1) + + q_new = np.array([nu_new, eta_new[0], eta_new[1], eta_new[2]]) + q_new /= np.linalg.norm(q_new) + + return q_new + + def van_loan_discretization( + self, A_c: np.ndarray, G_c: np.ndarray + ) -> tuple[np.ndarray, np.ndarray]: + """Calculates the Van Loan discretization of a continuous-time system. + + Args: + A_c (np.ndarray): The A matrix. + G_c (np.ndarray): The G matrix. + + Returns: + tuple: The A_d and GQG_d matrices. + """ + GQG_T = np.dot(np.dot(G_c, self.Q_process_noise), G_c.T) * self.dt + + matrix_exp = ( + np.block([[A_c, GQG_T], [np.zeros((A_c.shape[0], A_c.shape[0])), A_c.T]]) + * self.dt + ) + + van_loan_matrix = np.linalg.expm(matrix_exp) + + V1 = van_loan_matrix[A_c.shape[0] :, A_c.shape[0] :] + V2 = van_loan_matrix[: A_c.shape[0], A_c.shape[0] :] + + A_d = V1.T + GQG_d = A_d @ V2 + + return A_d, GQG_d + + def nominal_state_update( + self, current_state: StateVector_quaternion, imu_reading: np.ndarray + ) -> StateVector_quaternion: + """Updates the nominal state of the system. + + Args: + current_state (np.ndarray): The current state of the system. + imu_reading (np.ndarray): The IMU reading. + + Returns: + np.ndarray: The updated nominal state. + """ + # Defining the IMU readings + imu_acceleration = imu_reading[0:3] + imu_gyro = imu_reading[3:6] + + # Define the derivative of the state + current_state_dot = StateVector_quaternion() + + # Define the state derivates + current_state_dot.position = current_state.velocity + current_state_dot.velocity = ( + np.dot( + current_state.R_q(), + (imu_acceleration - current_state.acceleration_bias), + ) + + self.g + ) + + # Define the quaternion derivatives + current_state_dot.orientation = 0.5 * self.quaternion_super_product( + current_state.orientation, + np.array([0, imu_gyro[0], imu_gyro[1], imu_gyro[2]]), + ) + + # Define the bias + current_state_dot.acceleration_bias = ( + -np.dot(self.P_ab, np.eye(3)) @ current_state.acceleration_bias + ) + current_state_dot.gyro_bias = ( + -np.dot(self.P_wb, np.eye(3)) @ current_state.gyro_bias + ) + + return current_state_dot.euler_forward(current_state, self.dt) + + def error_state_update( + self, + current_error_state: StateVector_euler, + current_state: StateVector_quaternion, + imu_reading: np.ndarray, + ) -> StateVector_euler: + """Updates the error state of the system. + + Args: + current_error_state (np.ndarray): The current error state of the system. + current_state (np.ndarray): The current state of the system. + imu_reading (np.ndarray): The IMU reading. + + Returns: + np.ndarray: The updated error state. + """ + # Define the derivative of the state + next_error_state = StateVector_euler() + + # Defining the IMU readings + imu_acceleration = imu_reading[0:3] + imu_gyro = imu_reading[3:6] + + A_c = np.zeros((15, 15)) + A_c[0:3, 3:6] = np.eye(3) + A_c[3:6, 6:9] = -np.dot( + current_state.R_q(), + self.skew_symmetric(imu_acceleration - current_state.acceleration_bias), + ) + A_c[6:9, 6:9] = -self.skew_symmetric(imu_gyro - current_state.gyro_bias) + A_c[3:6, 9:12] = -current_state.R_q() + A_c[6:9, 12:15] = -np.eye(3) + A_c[9:12, 9:12] = -self.P_ab * np.eye(3) + A_c[12:15, 12:15] = -self.P_wb * np.eye(3) + + G_c = np.zeros((15, 12)) + G_c[3:6, 0:3] = -current_state.R_q() + G_c[6:9, 3:6] = -np.eye(3) + G_c[9:12, 6:9] = np.eye(3) + G_c[12:15, 9:12] = np.eye(3) + + # Van loan discretization + A_d, GQG_d = self.van_loan_discretization(A_c, G_c, self.dt) + + # Inserting the new state and covariance + next_error_state.copy_state(current_error_state) + next_error_state.covariance = ( + np.dot(np.dot(A_d, current_error_state.covariance), A_d.T) + GQG_d + ) + + return next_error_state + + def H(self) -> np.ndarray: + """Calculates the measurement matrix. + + Returns: + np.ndarray: The measurement matrix. + """ + # Define the measurement matrix + H = np.zeros((3, 15)) + + # For now assume only velocity is measured + H[0:3, 3:6] = np.eye(3) + + return H + + def prediction_from_estimates( + self, + current_state: StateVector_quaternion, + current_error_state: StateVector_euler, + imu_reading: np.ndarray, + ) -> StateVector_euler: + """Predicts the measurement from the current state and error state. + + Args: + current_state (StateVector_quaternion): The current state of the system. + current_error_state (StateVector_euler): The current error state of the system. + imu_reading (np.ndarray): The IMU reading. + + Returns: + StateVector_euler: The predicted measurement. + """ + # Define the z_pred matrix + z_pred = MeasurementModel() + + # Define the z_pred values separately + z_pred_1 = np.hstack((current_state.position, current_state.velocity)) + z_pred_2 = np.hstack( + np.dot(current_state.R_q(), self.lever_arm), + np.dot( + current_state.R_q, + np.dot( + self.skew_symmetric(current_state.angular_velocity), self.lever_arm + ), + ), + ) + + # Combine the z_pred values + z_pred.measurement = z_pred_1 + z_pred_2 + + # Define the H matrix + z_pred.measurement_matrix = self.H() + R = self.R + z_pred.measurement_covariance = ( + np.dot( + np.dot(z_pred.measurement_matrix, current_error_state.covariance), + z_pred.measurement_matrix.T, + ) + + R + ) + + return z_pred + + def measurement_update( + self, + error_state_pred: StateVector_euler, + z_pred: MeasurementModel, + dvl_measure: np.array, + ) -> StateVector_euler: + """Updates the error state of the system. + + Args: + current_error_state (np.ndarray): The current error state of the system. + measurement (np.ndarray): The measurement. + + Returns: + np.ndarray: The updated error state. + """ + # Define new error state value + new_error_state = StateVector_euler() + + # Define the measurement matrix + innovation = dvl_measure - z_pred.measurement + H = z_pred.measurement_matrix + R = self.R + P = error_state_pred.covariance + S = z_pred.measurement_covariance + + # Kalman gain calculation + W = np.dot(P, np.linalg.solve(S, H).T) + new_error_state.fill_states(np.dot(W, innovation)) + + I_WH = np.eye(15) - np.dot(W, H) + new_error_state.covariance = np.dot(np.dot(I_WH, P), I_WH.T) + np.dot( + np.dot(W, R), W.T + ) + + return new_error_state + + def imu_update_states( + self, + current_pred_nom: StateVector_quaternion, + current_pred_err: StateVector_euler, + imu_readings: np.array, + ) -> tuple[StateVector_quaternion, StateVector_euler]: + """Calculates the predicted state using the IMU readings. + + Args: + current_pred_nom (StateVector_quaternion): The current nominal state. + current_pred_err (StateVector_euler): The current error state. + imu_readings (np.array): The IMU readings. + + Returns: + tuple: The predicted nominal state and the predicted error state. + """ + pred_nom_state = self.nominal_state_update(current_pred_nom, imu_readings) + pred_err_state = self.error_state_update( + current_pred_err, current_pred_nom, imu_readings + ) + + return pred_nom_state, pred_err_state + + def dvl_update_states( + self, + current_pred_nom: StateVector_quaternion, + current_pred_err: StateVector_euler, + dvl_measure: np.array, + ) -> tuple[StateVector_quaternion, StateVector_euler]: + """Calculates the predicted state using the DVL readings. + + Args: + current_pred_nom (StateVector_quaternion): The current nominal state. + current_pred_err (StateVector_euler): The current error state. + dvl_measure (np.array): The DVL readings. + + Returns: + tuple: The predicted nominal state and the predicted error state. + """ + z_pred = self.prediction_from_estimates( + current_pred_nom, current_pred_err, dvl_measure + ) + new_error_state = self.measurement_update(current_pred_err, z_pred, dvl_measure) + + return current_pred_nom, new_error_state + + def injection_and_reset( + self, next_state: StateVector_quaternion, next_error_state: StateVector_euler + ) -> tuple[StateVector_quaternion, StateVector_euler]: + """Injects the error state into the nominal state and resets the error state. + + Args: + next_state (StateVector_quaternion): The next nominal state. + next_error_state (StateVector_euler): The next error state. + + Returns: + tuple: The injected nominal state and the reset error state. + """ + # Define the new state + inj_state = StateVector_quaternion() + + # Injecting the error state + inj_state.position = next_state.position + next_error_state.position + inj_state.velocity = next_state.velocity + next_error_state.velocity + inj_state.orientation = self.quaternion_super_product( + next_state.orientation, + 0.5 + * np.array( + [ + 2, + next_error_state.orientation[0], + next_error_state.orientation[1], + next_error_state.orientation[2], + ] + ), + ) + inj_state.acceleration_bias = ( + next_state.acceleration_bias + next_error_state.acceleration_bias + ) + inj_state.gyro_bias = next_state.gyro_bias + next_error_state.gyro_bias + + # Resetting the error state + G = np.eye(15) + G[6:9, 6:9] = np.eye(3) - self.skew_symmetric( + 0.5 * next_error_state.orientation + ) + + next_error_state.covariance = np.dot( + np.dot(G, next_error_state.covariance), G.T + ) + next_error_state.fill_states(np.zeros(15)) + + return inj_state, next_error_state diff --git a/navigation/eskf_python/eskf_python/eskf_python_node.py b/navigation/eskf_python/eskf_python/eskf_python_node.py new file mode 100644 index 000000000..25e5b6ca9 --- /dev/null +++ b/navigation/eskf_python/eskf_python/eskf_python_node.py @@ -0,0 +1,103 @@ +#!/usr/bin/env python3 + +import rclpy +from nav_msgs.msg import Odometry +from rclpy.node import Node +from rclpy.qos import QoSProfile, qos_profile_sensor_data + +# NEED TO CHANGE THIS TO THE CORRECT PATH +from eskf_python.eskf_python_filter import ( + ErrorStateKalmanFilter, + MeasurementModel, + StateVector_euler, + StateVector_quaternion, +) + +qos_profile = QoSProfile( + depth=1, + history=qos_profile_sensor_data.history, + reliability=qos_profile_sensor_data.reliability, +) + + +class ESKalmanFilterNode(Node): + def __init__(self): + super().__init__("eskf_python_node") + + # This callback will supply information from the IMU (Inertial Measurement Unit) 1000 Hz + # TEMPORARILY ADDED FOR TESTING + self.imu_subscriber_ = self.create_subscription( + Odometry, '/orca/imu', self.state_callback, qos_profile=qos_profile + ) + + # This publisher will publish the estimtaed state of the vehicle + self.state_publisher_ = self.create_publisher( + Odometry, '/orca/odom', qos_profile=qos_profile + ) + + self.eskf_modual = ErrorStateKalmanFilter() + self.current_state_nom = StateVector_quaternion() + self.current_state_error = StateVector_euler() + self.measurement_pred = MeasurementModel() + + self.get_logger().info("hybridpath_controller_node started") + + def imu_callback(self, msg: Odometry): + self.get_logger().info(f"Received IMU message: {msg}") + + # Get the IMU data + + # SOME CONVERSION HERE TO SUITABLE TYPE + imu_data = "something" + + # Update the filter with the IMU data + self.current_state_nom, self.current_state_error = ( + ErrorStateKalmanFilter.imu_update_states( + self.current_state_nom, self.current_state_error, imu_data + ) + ) + + # Publish the estimated state + """ + Some conversion function from the custom state type to odometry message + This needs to be worked on + """ + + def filter_callback(self): + """Callback function for the filter measurement update, + this will be called when the filter needs to be updated with the DVL data. + """ + self.get_logger().info("Filter callback, got DVL data") + + # Get the DVL data + dvl_data = "something" + + # Update the filter with the DVL data + self.current_state_nom, self.current_state_error = ( + ErrorStateKalmanFilter.dvl_update_states( + self.current_state_nom, self.current_state_error, dvl_data + ) + ) + self.current_state_nom, self.current_state_error = ( + ErrorStateKalmanFilter.injection_and_reset( + self.current_state_nom, self.current_state_error + ) + ) + + # Publish the estimated state + """ + Some conversion function from the custom state type to odometry message + This needs to be worked on + """ + + +def main(args=None): + rclpy.init(args=args) + node = ESKalmanFilterNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/navigation/eskf_python/launch/eskf.launch.py b/navigation/eskf_python/launch/eskf.launch.py new file mode 100644 index 000000000..3cae83dce --- /dev/null +++ b/navigation/eskf_python/launch/eskf.launch.py @@ -0,0 +1,22 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + eskf_python_node = Node( + package='eskf_python', + executable='eskf_python_node.py', + name='eskf_python_node', + parameters=[ + os.path.join( + get_package_share_directory('eskf_python'), + 'config', + 'eskf_python.yaml', + ), + ], + output='screen', + ) + return LaunchDescription([eskf_python_node]) diff --git a/navigation/eskf_python/package.xml b/navigation/eskf_python/package.xml new file mode 100644 index 000000000..980653c40 --- /dev/null +++ b/navigation/eskf_python/package.xml @@ -0,0 +1,23 @@ + + + + eskf_python + 1.0.0 + This package provides the implementation of a error-state kalman filter in python + talhanc + MIT + + ament_cmake_python + + rclpy + python-transforms3d-pip + geometry_msgs + vortex_msgs + + python3-pytest + + + + ament_cmake + + From d11a26723b8917b060aabaa75887249fe1ea320c Mon Sep 17 00:00:00 2001 From: Talha Nauman Choudhry Date: Mon, 10 Feb 2025 14:28:18 +0100 Subject: [PATCH 083/290] feat: added in the IMU msg and DVL msg, pluss ros2 setup --- .../eskf_python/eskf_python_filter.py | 12 +-- .../eskf_python/eskf_python_node.py | 74 ++++++++++++++----- 2 files changed, 56 insertions(+), 30 deletions(-) diff --git a/navigation/eskf_python/eskf_python/eskf_python_filter.py b/navigation/eskf_python/eskf_python/eskf_python_filter.py index 286747467..392c705c2 100644 --- a/navigation/eskf_python/eskf_python/eskf_python_filter.py +++ b/navigation/eskf_python/eskf_python/eskf_python_filter.py @@ -353,16 +353,8 @@ def prediction_from_estimates( z_pred = MeasurementModel() # Define the z_pred values separately - z_pred_1 = np.hstack((current_state.position, current_state.velocity)) - z_pred_2 = np.hstack( - np.dot(current_state.R_q(), self.lever_arm), - np.dot( - current_state.R_q, - np.dot( - self.skew_symmetric(current_state.angular_velocity), self.lever_arm - ), - ), - ) + z_pred_1 = current_state.velocity + z_pred_2 = 0 # Currently assuming no lever arm compensation # Combine the z_pred values z_pred.measurement = z_pred_1 + z_pred_2 diff --git a/navigation/eskf_python/eskf_python/eskf_python_node.py b/navigation/eskf_python/eskf_python/eskf_python_node.py index 25e5b6ca9..5b860582e 100644 --- a/navigation/eskf_python/eskf_python/eskf_python_node.py +++ b/navigation/eskf_python/eskf_python/eskf_python_node.py @@ -4,6 +4,9 @@ from nav_msgs.msg import Odometry from rclpy.node import Node from rclpy.qos import QoSProfile, qos_profile_sensor_data +from sensor_msgs.msg import Imu, +import numpy as np +from geometry_msgs.msg import TwistWithCovarianceStamped # NEED TO CHANGE THIS TO THE CORRECT PATH from eskf_python.eskf_python_filter import ( @@ -25,9 +28,12 @@ def __init__(self): super().__init__("eskf_python_node") # This callback will supply information from the IMU (Inertial Measurement Unit) 1000 Hz - # TEMPORARILY ADDED FOR TESTING self.imu_subscriber_ = self.create_subscription( - Odometry, '/orca/imu', self.state_callback, qos_profile=qos_profile + Imu, '/orca/imu', self.imu_callback, qos_profile=qos_profile + ) + + self.twist_dvl_subscriber_ = self.create_subscription( + TwistWithCovarianceStamped, '/dvl/twist', self.filter_callback, qos_profile=qos_profile ) # This publisher will publish the estimtaed state of the vehicle @@ -39,16 +45,19 @@ def __init__(self): self.current_state_nom = StateVector_quaternion() self.current_state_error = StateVector_euler() self.measurement_pred = MeasurementModel() + self.odom_msg = Odometry() - self.get_logger().info("hybridpath_controller_node started") + self.get_logger().info("Error State Kalman Filter started") - def imu_callback(self, msg: Odometry): - self.get_logger().info(f"Received IMU message: {msg}") + def imu_callback(self, msg: Imu): # Get the IMU data - # SOME CONVERSION HERE TO SUITABLE TYPE - imu_data = "something" + imu_acceleartion = msg.linear_acceleration + imu_angular_velocity = msg.angular_velocity + + # Combine the IMU data + imu_data = np.array([imu_acceleartion.x, imu_acceleartion.y, imu_acceleartion.z, imu_angular_velocity.x, imu_angular_velocity.y, imu_angular_velocity.z]) # Update the filter with the IMU data self.current_state_nom, self.current_state_error = ( @@ -57,20 +66,34 @@ def imu_callback(self, msg: Odometry): ) ) - # Publish the estimated state - """ - Some conversion function from the custom state type to odometry message - This needs to be worked on - """ + # Inserting the nominal state into the msg + self.odom_msg.pose.pose.position.x = self.current_state_nom.position[0] + self.odom_msg.pose.pose.position.y = self.current_state_nom.position[1] + self.odom_msg.pose.pose.position.z = self.current_state_nom.position[2] + self.odom_msg.pose.pose.orientation.x = self.current_state_nom.orientation[0] + self.odom_msg.pose.pose.orientation.y = self.current_state_nom.orientation[1] + self.odom_msg.pose.pose.orientation.z = self.current_state_nom.orientation[2] + self.odom_msg.pose.pose.orientation.w = self.current_state_nom.orientation[3] + self.odom_msg.twist.twist.linear.x = self.current_state_nom.velocity[0] + self.odom_msg.twist.twist.linear.y = self.current_state_nom.velocity[1] + self.odom_msg.twist.twist.linear.z = self.current_state_nom.velocity[2] + self.odom_msg.twist.twist.angular.x = imu_angular_velocity.x + self.odom_msg.twist.twist.angular.y = imu_angular_velocity.y + self.odom_msg.twist.twist.angular.z = imu_angular_velocity.z + + # Publish + self.state_publisher_.publish(self.odom_msg) - def filter_callback(self): + + + def filter_callback(self, msg: TwistWithCovarianceStamped): """Callback function for the filter measurement update, this will be called when the filter needs to be updated with the DVL data. """ self.get_logger().info("Filter callback, got DVL data") - # Get the DVL data - dvl_data = "something" + # Get the DVL data (linear velocity) + dvl_data = np.array([msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z]) # Update the filter with the DVL data self.current_state_nom, self.current_state_error = ( @@ -84,11 +107,22 @@ def filter_callback(self): ) ) - # Publish the estimated state - """ - Some conversion function from the custom state type to odometry message - This needs to be worked on - """ + # Inserting data into the msg + self.odom_msg.pose.pose.position.x = self.current_state_nom.position[0] + self.odom_msg.pose.pose.position.y = self.current_state_nom.position[1] + self.odom_msg.pose.pose.position.z = self.current_state_nom.position[2] + self.odom_msg.pose.pose.orientation.x = self.current_state_nom.orientation[0] + self.odom_msg.pose.pose.orientation.y = self.current_state_nom.orientation[1] + self.odom_msg.pose.pose.orientation.z = self.current_state_nom.orientation[2] + self.odom_msg.pose.pose.orientation.w = self.current_state_nom.orientation[3] + self.odom_msg.twist.twist.linear.x = self.current_state_nom.velocity[0] + self.odom_msg.twist.twist.linear.y = self.current_state_nom.velocity[1] + self.odom_msg.twist.twist.linear.z = self.current_state_nom.velocity[2] + self.odom_msg.twist.twist.linear.z = self.current_state_nom.velocity[2] + + # Publishing the data + self.state_publisher_.publish(self.odom_msg) + def main(args=None): From 648e39ff60b0fe2fc317f3316bc7a8b7a42609cc Mon Sep 17 00:00:00 2001 From: Talha Nauman Choudhry Date: Fri, 21 Feb 2025 17:07:58 +0100 Subject: [PATCH 084/290] feat: added ES-UKF filter --- .../eskf_python/eskf_python_filter.py | 4 +- navigation/sp_ukf_python/CMakeLists.txt | 33 ++ navigation/sp_ukf_python/README.md | 0 .../sp_ukf_python/config/sp_ukf_python.yaml | 3 + navigation/sp_ukf_python/launch/ukf.launch.py | 22 + navigation/sp_ukf_python/package.xml | 23 + .../sp_ukf_python/sp_ukf_python/__init__.py | 0 .../sp_ukf_python/sp_ukf_python.py | 440 ++++++++++++++++++ .../sp_ukf_python/sp_ukf_python_class.py | 280 +++++++++++ .../sp_ukf_python/sp_ukf_python_node.py | 137 ++++++ .../sp_ukf_python/sp_ukf_python_utils.py | 107 +++++ .../sp_ukf_python/sp_ukf_python/test_ukf.py | 218 +++++++++ 12 files changed, 1265 insertions(+), 2 deletions(-) create mode 100644 navigation/sp_ukf_python/CMakeLists.txt create mode 100644 navigation/sp_ukf_python/README.md create mode 100644 navigation/sp_ukf_python/config/sp_ukf_python.yaml create mode 100644 navigation/sp_ukf_python/launch/ukf.launch.py create mode 100644 navigation/sp_ukf_python/package.xml create mode 100644 navigation/sp_ukf_python/sp_ukf_python/__init__.py create mode 100644 navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python.py create mode 100644 navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_class.py create mode 100644 navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_node.py create mode 100644 navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_utils.py create mode 100644 navigation/sp_ukf_python/sp_ukf_python/test_ukf.py diff --git a/navigation/eskf_python/eskf_python/eskf_python_filter.py b/navigation/eskf_python/eskf_python/eskf_python_filter.py index 392c705c2..edf39dcc0 100644 --- a/navigation/eskf_python/eskf_python/eskf_python_filter.py +++ b/navigation/eskf_python/eskf_python/eskf_python_filter.py @@ -2,7 +2,7 @@ from typing import tuple import numpy as np - +from scipy.linalg import expm @dataclass class StateVector_quaternion: @@ -212,7 +212,7 @@ def van_loan_discretization( * self.dt ) - van_loan_matrix = np.linalg.expm(matrix_exp) + van_loan_matrix = expm(matrix_exp) V1 = van_loan_matrix[A_c.shape[0] :, A_c.shape[0] :] V2 = van_loan_matrix[: A_c.shape[0], A_c.shape[0] :] diff --git a/navigation/sp_ukf_python/CMakeLists.txt b/navigation/sp_ukf_python/CMakeLists.txt new file mode 100644 index 000000000..a40f065cd --- /dev/null +++ b/navigation/sp_ukf_python/CMakeLists.txt @@ -0,0 +1,33 @@ +cmake_minimum_required(VERSION 3.8) +project(sp_ukf_python) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake_python REQUIRED) +find_package(rclpy REQUIRED) +find_package(vortex_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) + +ament_python_install_package(${PROJECT_NAME}) + +install(DIRECTORY + launch + config + DESTINATION share/${PROJECT_NAME} +) + +install(PROGRAMS + sp_ukf_python/sp_ukf_python_node.py + DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + find_package(ament_cmake_pytest REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) +endif() + +ament_package() diff --git a/navigation/sp_ukf_python/README.md b/navigation/sp_ukf_python/README.md new file mode 100644 index 000000000..e69de29bb diff --git a/navigation/sp_ukf_python/config/sp_ukf_python.yaml b/navigation/sp_ukf_python/config/sp_ukf_python.yaml new file mode 100644 index 000000000..d3d18145d --- /dev/null +++ b/navigation/sp_ukf_python/config/sp_ukf_python.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + sp_ukf_python_node: diff --git a/navigation/sp_ukf_python/launch/ukf.launch.py b/navigation/sp_ukf_python/launch/ukf.launch.py new file mode 100644 index 000000000..fdd3f07e6 --- /dev/null +++ b/navigation/sp_ukf_python/launch/ukf.launch.py @@ -0,0 +1,22 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + sp_ukf_python_node = Node( + package='sp_ukf_python', + executable='sp_ukf_python_node.py', + name='sp_ukf_python_node', + parameters=[ + os.path.join( + get_package_share_directory('sp_ukf_python'), + 'config', + 'sp_ukf_python.yaml', + ), + ], + output='screen', + ) + return LaunchDescription([sp_ukf_python_node]) diff --git a/navigation/sp_ukf_python/package.xml b/navigation/sp_ukf_python/package.xml new file mode 100644 index 000000000..6aa4edbc0 --- /dev/null +++ b/navigation/sp_ukf_python/package.xml @@ -0,0 +1,23 @@ + + + + sp_ukf_python + 1.0.0 + This package provides the implementation of a sigma point based Unscented Error-state Kalman Filter + talhanc + MIT + + ament_cmake_python + + rclpy + python-transforms3d-pip + geometry_msgs + vortex_msgs + + python3-pytest + + + + ament_cmake + + diff --git a/navigation/sp_ukf_python/sp_ukf_python/__init__.py b/navigation/sp_ukf_python/sp_ukf_python/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python.py b/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python.py new file mode 100644 index 000000000..6f9d1b2f6 --- /dev/null +++ b/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python.py @@ -0,0 +1,440 @@ +from dataclasses import dataclass +from typing import Tuple +from sp_ukf_python_class import StateVector_quaternion, StateVector_euler +from sp_ukf_python_utils import skew_symmetric, quaternion_super_product +from scipy.linalg import expm + +import numpy as np + +class ErrorStateUnscentedKalmanFilter: + def __init__( + self, + P_ab: float, + P_wb: float, + Q: np.ndarray, + lever_arm: np.array, + R: np.ndarray, + g: float, + dt: float, + ) -> None: + self.P_ab = P_ab + self.P_wb = P_wb + self.Q_process_noise = Q + self.lever_arm = lever_arm + self.R = R + self.g = np.array([0, 0, g]) + self.dt = dt + self.y_i = np.zeros((15, 2*15)) + + def mean_set(self, set: np.ndarray) -> np.ndarray: + """ + Calculates the mean of a set of values. + + Args: + set (np.ndarray): The set of values. + + Returns: + np.ndarray: The mean of the set. + """ + # Define the number of columns + n = set.shape[0] + + # Calculate the mean value + mean_value = (1 / (2 * n)) * np.sum(set, axis=1) + + return mean_value + + def weighted_mean_set(self, set: np.ndarray, weight: np.ndarray) -> np.ndarray: + """ + Calculates the mean of a set of values. + + Args: + set (np.ndarray): The set of values. + + Returns: + np.ndarray: The mean of the set. + """ + # Define the number of columns + n = set.shape[0] + mean_value = np.zeros(n) + + for i in range(2*n + 1): + mean_value += weight[i] * set[:, i] + + mean_value = (1 / (2 * n + 1)) * mean_value + + return mean_value + + def covariance_set(self, mean: np.ndarray, set: np.ndarray, mean_2: np.ndarray = None, set_2: np.ndarray = None) -> np.ndarray: + """ + Calculate the covarince of a set of sigmapoints + + Args: + mean (np.ndarray): The mean of the set. + set (np.ndarray): The set of values. + + Returns: + np.ndarray: The covariance of the set. + """ + + if mean_2 is not None: + + n = set.shape[0] + n2 = set_2.shape[0] + covariance_set = np.zeros((n, n2)) + + for i in range(2*n): + vector = StateVector_euler() + vector.position = set[:, i][:3] + vector.velocity = set[:, i][3:6] + vector.orientation = set[:, i][6:9] + vector.acceleration_bias = set[:, i][9:12] + vector.gyro_bias = set[:, i][12:] + + vector_2 = StateVector_euler() + vector_2.position = set_2[:, i][:3] + vector_2.velocity = set_2[:, i][3:6] + vector_2.orientation = set_2[:, i][6:9] + vector_2.acceleration_bias = set_2[:, i][9:12] + vector_2.gyro_bias = set_2[:, i][12:] + W_i = vector - mean + W_i_2 = vector_2 - mean_2 + + covariance_set += (1 / (2*n)) * np.outer(W_i, W_i_2) + + return covariance_set + else: + + n = set.shape[0] + covariance_set = np.zeros((n, n)) + + for i in range(2*n): + vector = StateVector_euler() + vector.position = set[:, i][:3] + vector.velocity = set[:, i][3:6] + vector.orientation = set[:, i][6:9] + vector.acceleration_bias = set[:, i][9:12] + vector.gyro_bias = set[:, i][12:] + + W_i = vector - mean + covariance_set += (1 / (2*n)) * np.outer(W_i, W_i) + + return covariance_set + + def weighted_covariance_set(self, mean: np.ndarray, set: np.ndarray, weight: np.ndarray) -> np.ndarray: + """ + Calculate the covarince of a set of sigmapoints + + Args: + mean (np.ndarray): The mean of the set. + set (np.ndarray): The set of values. + + Returns: + np.ndarray: The covariance of the set. + """ + + n = set.shape[0] + covariance_set = np.zeros((n, n)) + + for i in range(2*n): + vector = StateVector_euler() + vector.position = set[:, i][:3] + vector.velocity = set[:, i][3:6] + vector.orientation = set[:, i][6:9] + vector.acceleration_bias = set[:, i][9:12] + vector.gyro_bias = set[:, i][12:] + + W_i = vector - mean + covariance_set += weight[i] * np.outer(W_i, W_i) + + return covariance_set + + + + def generate_sigma_points(self, error_state: StateVector_euler, Q_process_noise) -> tuple[list[StateVector_euler], np.ndarray]: + """ + Generates the sigma points for the UKF + This is done using the Cholesky decomposition method + """ + + # Define n + n = len(error_state.covariance) + kappa = 3 - n + + # Computing S matrix using cholensky decomposition + S = np.linalg.cholesky(error_state.covariance + Q_process_noise) + + S_scaled = np.sqrt(n + kappa) * S + + weighted_points = np.concatenate((S_scaled , -S_scaled), axis=1) + + sigma_points = [StateVector_euler() for _ in range(2 * n + 1)] + + sigma_points[0].fill_states(error_state.as_vector()) + for i in range(2*n): + sigma_points[i + 1].fill_states(error_state + weighted_points[:,i]) + + W = np.zeros(2*n + 1) + W[0] = kappa / (n + kappa) + + for i in range(2*n): + W[i + 1] = 1 / (2 * (n + kappa)) + + return sigma_points, W + + def nominal_state_update( + self, current_state: StateVector_quaternion, imu_reading: np.ndarray + ) -> StateVector_quaternion: + """Updates the nominal state of the system. + + Args: + current_state (np.ndarray): The current state of the system. + imu_reading (np.ndarray): The IMU reading. + + Returns: + np.ndarray: The updated nominal state. + """ + # Defining the IMU readings + imu_acceleration = imu_reading[0:3] + imu_gyro = imu_reading[3:6] + + # Define the derivative of the state + current_state_dot = StateVector_quaternion() + + # Define the state derivates + current_state_dot.position = current_state.velocity + current_state_dot.velocity = ( + np.dot( + current_state.R_q(), + (imu_acceleration - current_state.acceleration_bias), + ) + + self.g + ) + + # Define the quaternion derivatives + current_state_dot.orientation = 0.5 * quaternion_super_product( + current_state.orientation, + np.array([0, imu_gyro[0] - current_state.gyro_bias[0], imu_gyro[1] - current_state.gyro_bias[1], imu_gyro[2] - current_state.gyro_bias[2]]), + ) + + # Define the bias + current_state_dot.acceleration_bias = ( + -np.dot(self.P_ab, np.eye(3)) @ current_state.acceleration_bias + ) + current_state_dot.gyro_bias = ( + -np.dot(self.P_wb, np.eye(3)) @ current_state.gyro_bias + ) + + return current_state_dot.euler_forward(current_state, self.dt) + + def error_state_update( + self, + current_error_state: StateVector_euler, + current_state: StateVector_quaternion, + imu_reading: np.ndarray, + ) -> np.ndarray: + """Updates the error state of the system. + + Args: + current_error_state (np.ndarray): The current error state of the system. + current_state (np.ndarray): The current state of the system. + imu_reading (np.ndarray): The IMU reading. + + Returns: + np.ndarray: The updated error state. + """ + # Defining the IMU readings + imu_acceleration = imu_reading[0:3] + imu_gyro = imu_reading[3:6] + + A_c = np.zeros((15, 15)) + A_c[0:3, 3:6] = np.eye(3) + A_c[3:6, 6:9] = -np.dot( + current_state.R_q(), + skew_symmetric(imu_acceleration - current_state.acceleration_bias), + ) + A_c[6:9, 6:9] = -skew_symmetric(imu_gyro - current_state.gyro_bias) + A_c[3:6, 9:12] = -current_state.R_q() + A_c[6:9, 12:15] = -np.eye(3) + A_c[9:12, 9:12] = -self.P_ab * np.eye(3) + A_c[12:15, 12:15] = -self.P_wb * np.eye(3) + + # Exact matrix exponential + A_d = expm(A_c * self.dt) + + next_error_state = A_d @ current_error_state.as_vector() + + return next_error_state + + def unscented_transform(self, sigma_points: list[StateVector_euler], current_state: StateVector_quaternion, + imu_reading: np.ndarray,) -> StateVector_euler: + """ + Performs the Unscented Transform + This is the corresponding to a preditction step in the EKF + """ + + n = len(sigma_points[0].as_vector()) + + self.y_i = np.zeros((n, 2*n)) + + for i in range(2*n): + self.y_i[:, i] = self.error_state_update(sigma_points[i], current_state, imu_reading) + + error_state_estimate = StateVector_euler() + + x = self.mean_set(self.y_i) + + error_state_estimate.fill_states(x) + error_state_estimate.covariance = self.covariance_set(x, self.y_i) + + return error_state_estimate + + def H(self) -> np.ndarray: + """Calculates the measurement matrix. + + Returns: + np.ndarray: The measurement matrix. + """ + # Define the measurement matrix + H = np.zeros((3, 15)) + + # For now assume only velocity is measured + H[0:3, 3:6] = np.eye(3) + + return H + + def measurement_update(self, sigma_points: list[StateVector_euler], current_error_state: StateVector_euler, dvl_data: np.ndarray, Weight: np.ndarray) -> StateVector_euler: + """ + Updates the state vector with the DVL data + """ + + H = self.H() + R = self.R + + n = len(sigma_points[0].as_vector()) + + Z_i = np.zeros((H.shape[0], 2 * n)) + + for i in range(2*n): + Z_i[:, i] = np.dot(H, sigma_points[i].as_vector()) + + z = self.weighted_mean_set(Z_i, Weight) + S = self.weighted_covariance_set(z, Z_i, Weight) + + x = self.mean_set(self.y_i) + + # Calculate the rest + innovation = dvl_data - z + + P_innovation = S + R + + P_xz = self.covariance_set(x, self.y_i, z, Z_i) + + # Kalman gain + K_k = np.dot(P_xz, np.linalg.inv(P_innovation)) + + updated_error_state = StateVector_euler() + + # Update the state + updated_error_state.fill_states(x + np.dot(K_k, innovation)) + + # Update the covariance + updated_error_state.covariance = current_error_state.covariance - np.dot(K_k, np.dot(P_innovation, K_k.T)) + + return updated_error_state + + def imu_update_states(self, current_state_nom: StateVector_quaternion, current_state_error: StateVector_euler, imu_data: np.ndarray) -> tuple[StateVector_quaternion, StateVector_euler]: + """ + Updates the state vector with the IMU data + + Args: + current_state_nom (StateVector_quaternion): The current nominal state + current_state_error (StateVector_euler): The current error state + imu_data (np.ndarray): The IMU data + + Returns: + tuple[StateVector_quaternion, StateVector_euler]: The updated nominal and error states + + """ + + # Update the nominal state + current_state_nom = self.nominal_state_update(current_state_nom, imu_data) + + # Generate the sigma points + sigma_points, _ = self.generate_sigma_points(current_state_error, self.Q_process_noise) + + # Update the error state + current_state_error = self.unscented_transform(sigma_points, current_state_nom, imu_data) + + return current_state_nom, current_state_error + + def dvl_update_states(self, current_state_nom: StateVector_quaternion, current_state_error: StateVector_euler, dvl_data: np.ndarray) -> tuple[StateVector_quaternion, StateVector_euler]: + """ + Update the error state given the DVL data + + Args: + current_state_nom (StateVector_quaternion): The current nominal state + current_state_error (StateVector_euler): The current error state + dvl_data (np.ndarray): The DVL data to update the state with + + Returns: + tuple[StateVector_quaternion, StateVector_euler]: The updated nominal and error states + """ + + # Generate the sigma points + sigma_points, weight = self.generate_sigma_points(current_state_error, self.Q_process_noise) + + # Update the error state + current_state_error = self.measurement_update(sigma_points, current_state_error, dvl_data, weight) + + return current_state_nom, current_state_error + + def inject_and_reset(self, current_state_nom: StateVector_quaternion, current_state_error: StateVector_euler) -> tuple[StateVector_quaternion, StateVector_euler]: + """ + Injects the error state into the nominal state and resets the error state + + Args: + current_state_nom (StateVector_quaternion): The current nominal state + current_state_error (StateVector_euler): The current error state + + Returns: + tuple[StateVector_quaternion, StateVector_euler]: The updated nominal and error states + """ + + inj_state = StateVector_quaternion() + + inj_state.position = current_state_nom.position + current_state_error.position + inj_state.velocity = current_state_nom.velocity + current_state_error.velocity + inj_state.orientation = quaternion_super_product( + current_state_nom.orientation, + 0.5 + * np.array( + [ + 2, + current_state_error.orientation[0], + current_state_error.orientation[1], + current_state_error.orientation[2], + ] + ), + ) + inj_state.acceleration_bias = ( + current_state_nom.acceleration_bias + current_state_error.acceleration_bias + ) + inj_state.gyro_bias = current_state_nom.gyro_bias + current_state_error.gyro_bias + + + # Resetting the error state + G = np.eye(15) + G[6:9, 6:9] = np.eye(3) - skew_symmetric( + 0.5 * current_state_error.orientation + ) + + current_state_error.covariance = np.dot( + np.dot(G, current_state_error.covariance), G.T + ) + + current_state_error.fill_states(np.zeros(15)) + + + return inj_state, current_state_error + diff --git a/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_class.py b/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_class.py new file mode 100644 index 000000000..629ce460e --- /dev/null +++ b/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_class.py @@ -0,0 +1,280 @@ +import numpy as np +from dataclasses import dataclass, field +from sp_ukf_python_utils import quaternion_super_product, quaternion_error, euler_rotation_quaternion, ssa + +@dataclass +class StateVector_quaternion: + position: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Position vector (x, y, z) + velocity: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Velocity vector (u, v, w) + orientation: np.ndarray = field( + default_factory=lambda: np.zeros(4) + ) # Orientation quaternion (w, x, y, z) + acceleration_bias: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Acceleration bias vector (b_ax, b_ay, b_az) + gyro_bias: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Gyro bias vector (b_gx, b_gy, b_gz) + + def as_vector(self) -> np.ndarray: + """Calculates the state vector. + + Returns: + np.ndarray: The state vector. + """ + return np.concatenate( + [ + self.position, + self.velocity, + self.orientation, + self.acceleration_bias, + self.gyro_bias, + ] + ) + + def fill_states(self, state: np.ndarray) -> None: + """Fills the state vector with the values from a numpy array. + + Args: + state (np.ndarray): The state vector. + """ + if len(state) == 15: + self.position = state[0:3] + self.velocity = state[3:6] + self.orientation = state[6:10] + self.acceleration_bias = state[10:13] + self.gyro_bias = state[13:] + else: + self.position = state[0:3] + self.velocity = state[3:6] + self.orientation = euler_rotation_quaternion(state[6:9]) + self.acceleration_bias = state[9:12] + self.gyro_bias = state[12:] + + def R_q(self) -> np.ndarray: + """Calculates the rotation matrix from the orientation quaternion. + + Returns: + np.ndarray: The rotation matrix. + """ + q0, q1, q2, q3 = self.orientation + R = np.array( + [ + [ + 1 - 2 * q2**2 - 2 * q3**2, + 2 * (q1 * q2 - q0 * q3), + 2 * (q0 * q2 + q1 * q3), + ], + [ + 2 * (q1 * q2 + q0 * q3), + 1 - 2 * q1**2 - 2 * q3**2, + 2 * (q2 * q3 - q0 * q1), + ], + [ + 2 * (q1 * q3 - q0 * q2), + 2 * (q0 * q1 + q2 * q3), + 1 - 2 * q1**2 - 2 * q2**2, + ], + ] + ) + + return R + + def euler_forward( + self, current_state: 'StateVector_quaternion', dt: float + ) -> 'StateVector_quaternion': + # Define the new state + new_state = StateVector_quaternion() + + # Define the state derivatives + new_state.position = current_state.position + self.position * dt + new_state.velocity = current_state.velocity + self.velocity * dt + new_state.orientation = current_state.orientation + self.orientation * dt + new_state.acceleration_bias = ( + current_state.acceleration_bias + self.acceleration_bias * dt + ) + new_state.gyro_bias = current_state.gyro_bias + self.gyro_bias * dt + + # Normalize the orientation quaternion + new_state.orientation /= np.linalg.norm(new_state.orientation) + + return new_state + + def __sub__(self, other: 'StateVector_quaternion') -> np.ndarray: + """Subtracts two StateVector_quaternion objects. + + Args: + other (StateVector_quaternion): The other StateVector_quaternion object. + + Returns: + np.ndarray: The difference between the two StateVector_quaternion objects. + """ + position_diff = self.position - other.position + velocity_diff = self.velocity - other.velocity + orientation_diff = quaternion_error(self.orientation, other.orientation) + acceleration_bias_diff = self.acceleration_bias - other.acceleration_bias + gyro_bias_diff = self.gyro_bias - other.gyro_bias + + return np.concatenate( + [ + position_diff, + velocity_diff, + orientation_diff, + acceleration_bias_diff, + gyro_bias_diff, + ] + ) + + def __add__(self, other: 'np.ndarray') -> 'np.ndarray': + """Adds a numpy array to this StateVector_quaternion. + + Args: + other (np.ndarray): The numpy array to add. + + Returns: + np.ndarray: The result of the addition. + """ + # Construct the quaternion from the array + add_to_position = other[:3] + add_to_orientation = euler_rotation_quaternion(other[6:10]) + + new_position = self.position + add_to_position + new_velcoity = self.velocity + other[3:6] + new_orientation = quaternion_super_product(self.orientation, add_to_orientation) + new_acceleration_bias = self.acceleration_bias + other[10:13] + new_gyro_bias = self.gyro_bias + other[13:] + + return np.concatenate( + [ + new_position, + new_velcoity, + new_orientation, + new_acceleration_bias, + new_gyro_bias, + ] + ) + + +@dataclass +class StateVector_euler: + position: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Position vector (x, y, z) + velocity: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Velocity vector (u, v, w) + orientation: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Orientation angles (roll, pitch, yaw) + acceleration_bias: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Acceleration bias vector (b_ax, b_ay, b_az) + gyro_bias: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Gyro bias vector (b_gx, b_gy, b_gz) + covariance: np.ndarray = field( + default_factory=lambda: np.zeros((15, 15)) + ) # Covariance matrix + + def as_vector(self) -> np.ndarray: + """Calculates the state estimate vector. + + Returns: + np.ndarray: The state estimate vector. + """ + return np.concatenate( + [ + self.position, + self.velocity, + self.orientation, + self.acceleration_bias, + self.gyro_bias, + ] + ) + + def fill_states(self, state: np.ndarray) -> None: + """Fills the state vector with the values from a numpy array. + + Args: + state (np.ndarray): The state vector. + """ + self.position = state[0:3] + self.velocity = state[3:6] + self.orientation = state[6:9] + self.acceleration_bias = state[9:12] + self.gyro_bias = state[12:15] + + def copy_state(self, wanted_state: 'StateVector_euler') -> None: + """Copies the state from a StateVector object into the current StateVector object. + + Args: + wanted_state (StateVector_euler): The quaternion state to copy from. + """ + self.position = wanted_state.position + self.velocity = wanted_state.velocity + self.orientation = wanted_state.orientation + self.acceleration_bias = wanted_state.acceleration_bias + self.gyro_bias = wanted_state.gyro_bias + + def __add__(self, other: 'np.ndarray') -> 'np.ndarray': + """Adds a numpy array to this StateVector_quaternion. + + Args: + other (np.ndarray): The numpy array to add. + + Returns: + np.ndarray: The result of the addition. + """ + + new_position = self.position + other[:3] + new_velcoity = self.velocity + other[3:6] + new_orientation = self.orientation + other[6:9] + new_acceleration_bias = self.acceleration_bias + other[9:12] + new_gyro_bias = self.gyro_bias + other[12:] + + return np.concatenate( + [ + new_position, + new_velcoity, + new_orientation, + new_acceleration_bias, + new_gyro_bias, + ] + ) + + def __sub__(self, other_state: 'StateVector_euler') -> 'StateVector_euler': + """ + Subtracts two StateVector_euler objects. + + Args: + other (StateVector_euler): The other StateVector_euler object. + + Returns: + StateVector_euler: The difference between the two StateVector_euler objects. + """ + position_diff = self.position - other_state[:3] + velocity_diff = self.velocity - other_state[3:6] + orientation_diff = ssa(self.orientation - other_state[6:9]) + acceleration_bias_diff = self.acceleration_bias - other_state[9:12] + gyro_bias_diff = self.gyro_bias - other_state[12:] + + return np.concatenate( + (position_diff, velocity_diff, orientation_diff, acceleration_bias_diff, gyro_bias_diff) + ) + + +@dataclass +class MeasurementModel: + measurement: np.ndarray = field( + default_factory=lambda: np.zeros(6) + ) # Measurement vector + measurement_matrix: np.ndarray = field( + default_factory=lambda: np.zeros((6, 15)) + ) # Measurement matrix + measurement_covariance: np.ndarray = field( + default_factory=lambda: np.zeros((6, 6)) + ) # Measurement noise matrix \ No newline at end of file diff --git a/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_node.py b/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_node.py new file mode 100644 index 000000000..103528ef2 --- /dev/null +++ b/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_node.py @@ -0,0 +1,137 @@ +#!/usr/bin/env python3 + +import rclpy +from nav_msgs.msg import Odometry +from rclpy.node import Node +from rclpy.qos import QoSProfile, qos_profile_sensor_data +from sensor_msgs.msg import Imu, +import numpy as np +from geometry_msgs.msg import TwistWithCovarianceStamped + +# NEED TO CHANGE THIS TO THE CORRECT PATH +from eskf_python.eskf_python_filter import ( + ErrorStateKalmanFilter, + MeasurementModel, + StateVector_euler, + StateVector_quaternion, +) + +qos_profile = QoSProfile( + depth=1, + history=qos_profile_sensor_data.history, + reliability=qos_profile_sensor_data.reliability, +) + + +class ESKalmanFilterNode(Node): + def __init__(self): + super().__init__("sp_ukf_python_node") + + # This callback will supply information from the IMU (Inertial Measurement Unit) 1000 Hz + self.imu_subscriber_ = self.create_subscription( + Imu, '/orca/imu', self.imu_callback, qos_profile=qos_profile + ) + + self.twist_dvl_subscriber_ = self.create_subscription( + TwistWithCovarianceStamped, '/dvl/twist', self.filter_callback, qos_profile=qos_profile + ) + + # This publisher will publish the estimtaed state of the vehicle + self.state_publisher_ = self.create_publisher( + Odometry, '/orca/odom', qos_profile=qos_profile + ) + + self.eskf_modual = ErrorStateKalmanFilter() + self.current_state_nom = StateVector_quaternion() + self.current_state_error = StateVector_euler() + self.measurement_pred = MeasurementModel() + self.odom_msg = Odometry() + + self.get_logger().info("Unscented Kalman Filter started") + + def imu_callback(self, msg: Imu): + + # Get the IMU data + + imu_acceleartion = msg.linear_acceleration + imu_angular_velocity = msg.angular_velocity + + # Combine the IMU data + imu_data = np.array([imu_acceleartion.x, imu_acceleartion.y, imu_acceleartion.z, imu_angular_velocity.x, imu_angular_velocity.y, imu_angular_velocity.z]) + + # Update the filter with the IMU data + self.current_state_nom, self.current_state_error = ( + ErrorStateKalmanFilter.imu_update_states( + self.current_state_nom, self.current_state_error, imu_data + ) + ) + + # Inserting the nominal state into the msg + self.odom_msg.pose.pose.position.x = self.current_state_nom.position[0] + self.odom_msg.pose.pose.position.y = self.current_state_nom.position[1] + self.odom_msg.pose.pose.position.z = self.current_state_nom.position[2] + self.odom_msg.pose.pose.orientation.x = self.current_state_nom.orientation[0] + self.odom_msg.pose.pose.orientation.y = self.current_state_nom.orientation[1] + self.odom_msg.pose.pose.orientation.z = self.current_state_nom.orientation[2] + self.odom_msg.pose.pose.orientation.w = self.current_state_nom.orientation[3] + self.odom_msg.twist.twist.linear.x = self.current_state_nom.velocity[0] + self.odom_msg.twist.twist.linear.y = self.current_state_nom.velocity[1] + self.odom_msg.twist.twist.linear.z = self.current_state_nom.velocity[2] + self.odom_msg.twist.twist.angular.x = imu_angular_velocity.x + self.odom_msg.twist.twist.angular.y = imu_angular_velocity.y + self.odom_msg.twist.twist.angular.z = imu_angular_velocity.z + + # Publish + self.state_publisher_.publish(self.odom_msg) + + + + def filter_callback(self, msg: TwistWithCovarianceStamped): + """Callback function for the filter measurement update, + this will be called when the filter needs to be updated with the DVL data. + """ + self.get_logger().info("Filter callback, got DVL data") + + # Get the DVL data (linear velocity) + dvl_data = np.array([msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z]) + + # Update the filter with the DVL data + self.current_state_nom, self.current_state_error = ( + ErrorStateKalmanFilter.dvl_update_states( + self.current_state_nom, self.current_state_error, dvl_data + ) + ) + self.current_state_nom, self.current_state_error = ( + ErrorStateKalmanFilter.injection_and_reset( + self.current_state_nom, self.current_state_error + ) + ) + + # Inserting data into the msg + self.odom_msg.pose.pose.position.x = self.current_state_nom.position[0] + self.odom_msg.pose.pose.position.y = self.current_state_nom.position[1] + self.odom_msg.pose.pose.position.z = self.current_state_nom.position[2] + self.odom_msg.pose.pose.orientation.x = self.current_state_nom.orientation[0] + self.odom_msg.pose.pose.orientation.y = self.current_state_nom.orientation[1] + self.odom_msg.pose.pose.orientation.z = self.current_state_nom.orientation[2] + self.odom_msg.pose.pose.orientation.w = self.current_state_nom.orientation[3] + self.odom_msg.twist.twist.linear.x = self.current_state_nom.velocity[0] + self.odom_msg.twist.twist.linear.y = self.current_state_nom.velocity[1] + self.odom_msg.twist.twist.linear.z = self.current_state_nom.velocity[2] + self.odom_msg.twist.twist.linear.z = self.current_state_nom.velocity[2] + + # Publishing the data + self.state_publisher_.publish(self.odom_msg) + + + +def main(args=None): + rclpy.init(args=args) + node = ESKalmanFilterNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_utils.py b/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_utils.py new file mode 100644 index 000000000..071180fc7 --- /dev/null +++ b/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_utils.py @@ -0,0 +1,107 @@ +import numpy as np + +def quaternion_super_product(q1: np.ndarray, q2: np.ndarray) -> np.ndarray: + """Calculates the quaternion super product of two quaternions. + + Args: + q1 (np.ndarray): The first quaternion. + q2 (np.ndarray): The second quaternion. + + Returns: + np.ndarray: The quaternion super product. + """ + eta_0, e_0_x, e_0_y, e_0_z = q1 + eta_1, e_1_x, e_1_y, e_1_z = q2 + + e_0 = np.array([e_0_x, e_0_y, e_0_z]) + e_1 = np.array([e_1_x, e_1_y, e_1_z]) + + eta_new = eta_0 * eta_1 - (e_0_x * e_1_x + e_0_y * e_1_y + e_0_z * e_1_z) + nu_new = e_1 * eta_0 + e_0 * eta_1 + np.dot(skew_symmetric(e_0), e_1) + + q_new = np.array([eta_new, nu_new[0], nu_new[1], nu_new[2]]) + q_new /= np.linalg.norm(q_new) + + return q_new + +def quaternion_error(quat_1: np.ndarray, quat_2: np.ndarray) -> np.ndarray: + """ + Calculates the error between two quaternions + """ + + quat_2_inv = np.array([quat_2[0], -quat_2[1], -quat_2[2], -quat_2[3]]) + + error_quat = quaternion_super_product(quat_1, quat_2_inv) + + return error_quat + +def euler_rotation_quaternion(self, euler_angles: np.ndarray) -> np.ndarray: + """ + Converts An vector assumed to be rotation vector to quaternion + + Args: + euler_angles (np.ndarray): Rotation vector + + Returns: + np.ndarray: Quaternion representation of the rotation vector + """ + + angle = np.linalg.norm(euler_angles) + + if angle == 0: + axis = np.array([0, 0, 0]) + else: + axis = euler_angles / angle + + quaternion = np.zeros(4) + quaternion[0] = np.cos(angle / 2) + quaternion[1:] = np.sin(angle / 2) * axis + + return quaternion + +def quaternion_rotation_euler(self, quaternion: np.ndarray) -> np.ndarray: + """ + Converts a quaternion to an euler rotation vector + Used to generate the covarince matrix + + Args: + quaternion (np.ndarray): The quaternion to convert + + Returns: + np.ndarray: The euler rotation vector + """ + nu, eta_x, eta_y, eta_z = quaternion + + phi = np.arctan2(2 * (nu * eta_x + eta_y * eta_z), 1 - 2 * (eta_x ** 2 + eta_y ** 2)) + theta = -np.arcsin(2 * (eta_z * eta_x - nu * eta_y)) + psi = np.arctan2(2 * (nu * eta_z + eta_x * eta_y), 1 - 2 * (eta_y ** 2 + eta_z ** 2)) + + return np.array([phi, theta, psi]) + +def skew_symmetric(vector: np.ndarray) -> np.ndarray: + """Calculates the skew symmetric matrix of a vector. + + Args: + vector (np.ndarray): The vector. + + Returns: + np.ndarray: The skew symmetric matrix. + """ + return np.array( + [ + [0, -vector[2], vector[1]], + [vector[2], 0, -vector[0]], + [-vector[1], vector[0], 0], + ] + ) + +def ssa(angle: np.ndarray) -> np.ndarray: + """ + smallest signed angle between two angles + """ + ssa_vector = np.zeros(len(angle)) + + for i in range(len(angle)): + ssa_vector[i] = (angle[i] + np.pi) % (2 * np.pi) - np.pi + + return ssa_vector \ No newline at end of file diff --git a/navigation/sp_ukf_python/sp_ukf_python/test_ukf.py b/navigation/sp_ukf_python/sp_ukf_python/test_ukf.py new file mode 100644 index 000000000..59e08aa55 --- /dev/null +++ b/navigation/sp_ukf_python/sp_ukf_python/test_ukf.py @@ -0,0 +1,218 @@ +import numpy as np +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D # for 3D plotting + +# (Assuming the following have been imported from your modules) +from sp_ukf_python_class import StateVector_quaternion, StateVector_euler +from sp_ukf_python_utils import skew_symmetric, quaternion_super_product +from sp_ukf_python import ErrorStateUnscentedKalmanFilter + +def quat_to_yaw(q: np.ndarray) -> float: + """ + Convert a quaternion (assumed [w, x, y, z]) with zero roll and pitch + into a yaw angle. + """ + return 2 * np.arctan2(q[3], q[0]) + +def run_ESUKF_simulation(): + # Simulation parameters + dt = 0.01 # time step [s] + T = 60.0 # total simulation time [s] + num_steps = int(T/dt) + g_val = 9.81 # gravitational acceleration + + # Define noise and covariance matrices + Q = np.diag([0.1]*15) # Process noise covariance (15x15) + R_meas = np.diag([0.08]*3) # DVL measurement noise (velocity noise) + P_ab = 0.005 # Accelerometer bias dynamics matrix + P_wb = 0.005 # Gyro bias dynamics matrix + lever_arm = np.array([0.0, 0.0, 0.0]) # Assume sensor is at the center of mass + + # Create ESUKF instance + esukf = ErrorStateUnscentedKalmanFilter(P_ab, P_wb, Q, lever_arm, R_meas, g_val, dt) + + # Initialize true state (StateVector_quaternion) with no biases. + true_state = StateVector_quaternion() + true_state.position = np.array([20.0, 0.0, 0.0]) + true_state.velocity = np.array([0.0, 1.0, 0.0]) + true_state.orientation = np.array([1.0, 0.0, 0.0, 0.0]) # No initial rotation + true_state.acceleration_bias = np.zeros(3) + true_state.gyro_bias = np.zeros(3) + + # Initialize estimated (nominal) state with a small offset. + est_state_nom = StateVector_quaternion() + est_state_nom.position = true_state.position + np.array([0.1, -0.1, 0.05]) + est_state_nom.velocity = true_state.velocity + np.array([0.05, 0.05, -0.05]) + est_state_nom.orientation = true_state.orientation.copy() + est_state_nom.acceleration_bias = np.zeros(3) + est_state_nom.gyro_bias = np.zeros(3) + + # Initialize error state (StateVector_euler) as zero with some initial covariance. + est_state_error = StateVector_euler() + est_state_error.fill_states(np.zeros(15)) + est_state_error.covariance = 0.1 * np.eye(15) + + # Prepare histories for plotting + time_hist = [] + true_pos_hist = [] + est_pos_hist = [] + true_vel_hist = [] + est_vel_hist = [] + true_yaw_hist = [] + est_yaw_hist = [] + + # For the true trajectory, we simulate a circle in the horizontal plane. + R_circle = 20.0 # circle radius [m] + omega = 0.05 # angular speed [rad/s] + + t = 0.0 + for step in range(num_steps): + # --- True State Generation --- + # Circular trajectory: position = [R*cos(omega*t), R*sin(omega*t), 0] + pos_true = np.array([R_circle * np.cos(omega * t), + R_circle * np.sin(omega * t), + 0.0]) + # Velocity is the derivative of position. + vel_true = np.array([-R_circle * omega * np.sin(omega * t), + R_circle * omega * np.cos(omega * t), + 0.0]) + # Acceleration is the second derivative. + acc_true = np.array([-R_circle * omega**2 * np.cos(omega * t), + -R_circle * omega**2 * np.sin(omega * t), + 0.0]) + # Update the true state. + true_state.position = pos_true + true_state.velocity = vel_true + # Compute heading (yaw) tangent to the path. + yaw_true = np.arctan2(vel_true[1], vel_true[0]) + # For simplicity, assume roll and pitch are zero. + true_state.orientation = np.array([np.cos(yaw_true/2), 0.0, 0.0, np.sin(yaw_true/2)]) + # Biases remain zero for the true state. + + # --- Simulated IMU Measurements --- + # The nominal state propagation uses: + # velocity_dot = R_q() @ (imu_acc - bias) + g + # Therefore, the ideal accelerometer measurement is: + # imu_acc = R_true.T @ (acc_true - g_vector) + R_true = true_state.R_q() # rotation matrix from quaternion + imu_acc_ideal = np.dot(R_true.T, (acc_true - np.array([0.0, 0.0, g_val]))) + # Add noise (e.g., 0.1 m/s^2 std dev). + imu_acc_noise = np.random.normal(0.0, 0.1, 3) + imu_acc_meas = imu_acc_ideal + imu_acc_noise + + # For the gyro: the true angular velocity in body frame. + # For a circular path with constant yaw rate, the ideal gyro reading is: + imu_gyro_ideal = np.array([0.0, 0.0, omega]) + # Add noise (e.g., 0.01 rad/s std dev). + imu_gyro_noise = np.random.normal(0.0, 0.01, 3) + imu_gyro_meas = imu_gyro_ideal + imu_gyro_noise + + # Combine to form the complete IMU measurement vector. + imu_meas = np.hstack((imu_acc_meas, imu_gyro_meas)) + + # --- Simulated DVL Measurement --- + # DVL measures velocity (here assumed in the inertial frame). + dvl_noise = np.random.normal(0.0, 0.05, 3) + dvl_meas = vel_true + dvl_noise + + # --- Filter Updates --- + # 1. Propagate the nominal state with IMU data. + est_state_nom, est_state_error = esukf.imu_update_states(est_state_nom, est_state_error, imu_meas) + # 2. Incorporate DVL measurement. + est_state_nom, est_state_error = esukf.dvl_update_states(est_state_nom, est_state_error, dvl_meas) + # 3. Inject the error state into the nominal state and reset the error state. + est_state_nom, est_state_error = esukf.inject_and_reset(est_state_nom, est_state_error) + + # --- Store Histories --- + time_hist.append(t) + true_pos_hist.append(pos_true) + est_pos_hist.append(est_state_nom.position.copy()) + true_vel_hist.append(vel_true) + est_vel_hist.append(est_state_nom.velocity.copy()) + true_yaw_hist.append(yaw_true) + est_yaw_hist.append(quat_to_yaw(est_state_nom.orientation)) + + t += dt + + # Convert histories to NumPy arrays. + true_pos_hist = np.array(true_pos_hist) + est_pos_hist = np.array(est_pos_hist) + true_vel_hist = np.array(true_vel_hist) + est_vel_hist = np.array(est_vel_hist) + true_yaw_hist = np.array(true_yaw_hist) + est_yaw_hist = np.array(est_yaw_hist) + time_hist = np.array(time_hist) + + # --- Plotting Results --- + + # Plot positions (each axis separately) + plt.figure(figsize=(10, 8)) + plt.subplot(3, 1, 1) + plt.plot(time_hist, true_pos_hist[:, 0], label='True X') + plt.plot(time_hist, est_pos_hist[:, 0], '--', label='Estimated X') + plt.ylabel('X Position (m)') + plt.legend() + + plt.subplot(3, 1, 2) + plt.plot(time_hist, true_pos_hist[:, 1], label='True Y') + plt.plot(time_hist, est_pos_hist[:, 1], '--', label='Estimated Y') + plt.ylabel('Y Position (m)') + plt.legend() + + plt.subplot(3, 1, 3) + plt.plot(time_hist, true_pos_hist[:, 2], label='True Z') + plt.plot(time_hist, est_pos_hist[:, 2], '--', label='Estimated Z') + plt.xlabel('Time (s)') + plt.ylabel('Z Position (m)') + plt.legend() + plt.tight_layout() + plt.show() + + # Plot velocities + plt.figure(figsize=(10, 8)) + plt.subplot(3, 1, 1) + plt.plot(time_hist, true_vel_hist[:, 0], label='True Vx') + plt.plot(time_hist, est_vel_hist[:, 0], '--', label='Estimated Vx') + plt.ylabel('Vx (m/s)') + plt.legend() + + plt.subplot(3, 1, 2) + plt.plot(time_hist, true_vel_hist[:, 1], label='True Vy') + plt.plot(time_hist, est_vel_hist[:, 1], '--', label='Estimated Vy') + plt.ylabel('Vy (m/s)') + plt.legend() + + plt.subplot(3, 1, 3) + plt.plot(time_hist, true_vel_hist[:, 2], label='True Vz') + plt.plot(time_hist, est_vel_hist[:, 2], '--', label='Estimated Vz') + plt.xlabel('Time (s)') + plt.ylabel('Vz (m/s)') + plt.legend() + plt.tight_layout() + plt.show() + + # Plot heading (yaw) + plt.figure(figsize=(10, 4)) + plt.plot(time_hist, np.degrees(true_yaw_hist), label='True Yaw') + plt.plot(time_hist, np.degrees(est_yaw_hist), '--', label='Estimated Yaw') + plt.xlabel('Time (s)') + plt.ylabel('Yaw (deg)') + plt.legend() + plt.title('Heading Comparison') + plt.tight_layout() + plt.show() + + # Plot 3D Trajectory + fig = plt.figure(figsize=(8, 6)) + ax = fig.add_subplot(111, projection='3d') + ax.plot(true_pos_hist[:, 0], true_pos_hist[:, 1], true_pos_hist[:, 2], label='True Trajectory', linewidth=2) + ax.plot(est_pos_hist[:, 0], est_pos_hist[:, 1], est_pos_hist[:, 2], '--', label='Estimated Trajectory', linewidth=2) + ax.set_xlabel('X (m)') + ax.set_ylabel('Y (m)') + ax.set_zlabel('Z (m)') + ax.legend() + plt.title('3D Trajectory') + plt.show() + +if __name__ == '__main__': + run_ESUKF_simulation() From 59164c31f7f19d686ea93be1022db169f392f8ee Mon Sep 17 00:00:00 2001 From: Talha Nauman Choudhry Date: Thu, 27 Feb 2025 16:18:15 +0100 Subject: [PATCH 085/290] feat: added UKF fix injection step --- .../sp_ukf_python/sp_ukf_python.py | 377 ++++++++++-------- .../sp_ukf_python/sp_ukf_python_class.py | 33 +- .../sp_ukf_python/sp_ukf_python/test_ukf.py | 295 +++++++++----- 3 files changed, 434 insertions(+), 271 deletions(-) diff --git a/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python.py b/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python.py index 6f9d1b2f6..8d2ff6265 100644 --- a/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python.py +++ b/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python.py @@ -1,10 +1,9 @@ -from dataclasses import dataclass -from typing import Tuple -from sp_ukf_python_class import StateVector_quaternion, StateVector_euler -from sp_ukf_python_utils import skew_symmetric, quaternion_super_product -from scipy.linalg import expm import numpy as np +from scipy.linalg import expm +from sp_ukf_python_class import StateVector_euler, StateVector_quaternion +from sp_ukf_python_utils import quaternion_super_product, skew_symmetric + class ErrorStateUnscentedKalmanFilter: def __init__( @@ -24,11 +23,10 @@ def __init__( self.R = R self.g = np.array([0, 0, g]) self.dt = dt - self.y_i = np.zeros((15, 2*15)) - + self.y_i = np.zeros((15, 2 * 15)) + def mean_set(self, set: np.ndarray) -> np.ndarray: - """ - Calculates the mean of a set of values. + """Calculates the mean of a set of values. Args: set (np.ndarray): The set of values. @@ -38,15 +36,14 @@ def mean_set(self, set: np.ndarray) -> np.ndarray: """ # Define the number of columns n = set.shape[0] - + # Calculate the mean value mean_value = (1 / (2 * n)) * np.sum(set, axis=1) return mean_value - + def weighted_mean_set(self, set: np.ndarray, weight: np.ndarray) -> np.ndarray: - """ - Calculates the mean of a set of values. + """Calculates the mean of a set of values. Args: set (np.ndarray): The set of values. @@ -57,86 +54,94 @@ def weighted_mean_set(self, set: np.ndarray, weight: np.ndarray) -> np.ndarray: # Define the number of columns n = set.shape[0] mean_value = np.zeros(n) - - for i in range(2*n + 1): + + for i in range(2 * n + 1): mean_value += weight[i] * set[:, i] - mean_value = (1 / (2 * n + 1)) * mean_value + mean_value = (1 / (2 * n + 1)) * mean_value return mean_value - - def covariance_set(self, mean: np.ndarray, set: np.ndarray, mean_2: np.ndarray = None, set_2: np.ndarray = None) -> np.ndarray: - """ - Calculate the covarince of a set of sigmapoints - + + def covariance_set(self, mean: np.ndarray, set: np.ndarray) -> np.ndarray: + """Calculate the covarince of a set of sigmapoints + Args: mean (np.ndarray): The mean of the set. set (np.ndarray): The set of values. - + Returns: np.ndarray: The covariance of the set. """ + n = set.shape[0] + covariance_set = np.zeros((n, n)) - if mean_2 is not None: - - n = set.shape[0] - n2 = set_2.shape[0] - covariance_set = np.zeros((n, n2)) - - for i in range(2*n): - vector = StateVector_euler() - vector.position = set[:, i][:3] - vector.velocity = set[:, i][3:6] - vector.orientation = set[:, i][6:9] - vector.acceleration_bias = set[:, i][9:12] - vector.gyro_bias = set[:, i][12:] - - vector_2 = StateVector_euler() - vector_2.position = set_2[:, i][:3] - vector_2.velocity = set_2[:, i][3:6] - vector_2.orientation = set_2[:, i][6:9] - vector_2.acceleration_bias = set_2[:, i][9:12] - vector_2.gyro_bias = set_2[:, i][12:] - W_i = vector - mean - W_i_2 = vector_2 - mean_2 - - covariance_set += (1 / (2*n)) * np.outer(W_i, W_i_2) - - return covariance_set - else: - - n = set.shape[0] - covariance_set = np.zeros((n, n)) - - for i in range(2*n): - vector = StateVector_euler() - vector.position = set[:, i][:3] - vector.velocity = set[:, i][3:6] - vector.orientation = set[:, i][6:9] - vector.acceleration_bias = set[:, i][9:12] - vector.gyro_bias = set[:, i][12:] - - W_i = vector - mean - covariance_set += (1 / (2*n)) * np.outer(W_i, W_i) - - return covariance_set - - def weighted_covariance_set(self, mean: np.ndarray, set: np.ndarray, weight: np.ndarray) -> np.ndarray: + for i in range(2 * n + 1): + vector = StateVector_euler() + vector.position = set[:, i][:3] + vector.velocity = set[:, i][3:6] + vector.orientation = set[:, i][6:9] + vector.acceleration_bias = set[:, i][9:12] + vector.gyro_bias = set[:, i][12:] + + W_i = vector - mean + + covariance_set += (1 / (2 * n + 1)) * np.outer(W_i, W_i) + + return covariance_set + + def cross_covariance_set( + self, + mean: np.ndarray, + set: np.ndarray, + mean_2: np.ndarray, + set_2: np.ndarray, + weight: np.ndarray, + ) -> np.ndarray: + """Calculate the cross covariance of a set of sigmapoints + + Args: + mean (np.ndarray): The mean of the set. + set (np.ndarray): The set of values. + mean_2 (np.ndarray): The mean of the second set. + set_2 (np.ndarray): The second set of values. + + Returns: + np.ndarray: The cross covariance of the set. """ - Calculate the covarince of a set of sigmapoints - + n_x = set.shape[0] + n_z = set_2.shape[0] + covariance_mat = np.zeros((n_x, n_z)) + + for i in range(2 * n_x + 1): + # parse the 15-dim error state + err_vec = set[:, i] # shape (15,) + W_i = err_vec - mean # shape (15,) + + # parse the 3-dim measurement + meas_vec = set_2[:, i] # shape (3,) + W_i_2 = meas_vec - mean_2 # shape (3,) + + # outer product -> shape (15,3) + covariance_mat += weight[i] * np.outer(W_i, W_i_2) + + return covariance_mat + + def weighted_covariance_set( + self, mean: np.ndarray, set: np.ndarray, weight: np.ndarray + ) -> np.ndarray: + """Calculate the covarince of a set of sigmapoints + Args: mean (np.ndarray): The mean of the set. set (np.ndarray): The set of values. - + Returns: np.ndarray: The covariance of the set. """ - n = set.shape[0] covariance_set = np.zeros((n, n)) - for i in range(2*n): + for i in range(2 * n + 1): vector = StateVector_euler() vector.position = set[:, i][:3] vector.velocity = set[:, i][3:6] @@ -146,38 +151,38 @@ def weighted_covariance_set(self, mean: np.ndarray, set: np.ndarray, weight: np. W_i = vector - mean covariance_set += weight[i] * np.outer(W_i, W_i) - - return covariance_set - + return covariance_set - def generate_sigma_points(self, error_state: StateVector_euler, Q_process_noise) -> tuple[list[StateVector_euler], np.ndarray]: - """ - Generates the sigma points for the UKF + def generate_sigma_points( + self, error_state: StateVector_euler, Q_process_noise + ) -> tuple[list[StateVector_euler], np.ndarray]: + """Generates the sigma points for the UKF This is done using the Cholesky decomposition method """ - # Define n n = len(error_state.covariance) kappa = 3 - n # Computing S matrix using cholensky decomposition + # print(error_state.covariance + Q_process_noise) S = np.linalg.cholesky(error_state.covariance + Q_process_noise) + # print(S) S_scaled = np.sqrt(n + kappa) * S - weighted_points = np.concatenate((S_scaled , -S_scaled), axis=1) + weighted_points = np.concatenate((S_scaled, -S_scaled), axis=1) sigma_points = [StateVector_euler() for _ in range(2 * n + 1)] sigma_points[0].fill_states(error_state.as_vector()) - for i in range(2*n): - sigma_points[i + 1].fill_states(error_state + weighted_points[:,i]) + for i in range(2 * n): + sigma_points[i + 1].fill_states(error_state + weighted_points[:, i]) - W = np.zeros(2*n + 1) + W = np.zeros(2 * n + 1) W[0] = kappa / (n + kappa) - for i in range(2*n): + for i in range(2 * n): W[i + 1] = 1 / (2 * (n + kappa)) return sigma_points, W @@ -214,7 +219,14 @@ def nominal_state_update( # Define the quaternion derivatives current_state_dot.orientation = 0.5 * quaternion_super_product( current_state.orientation, - np.array([0, imu_gyro[0] - current_state.gyro_bias[0], imu_gyro[1] - current_state.gyro_bias[1], imu_gyro[2] - current_state.gyro_bias[2]]), + np.array( + [ + 0, + imu_gyro[0] - current_state.gyro_bias[0], + imu_gyro[1] - current_state.gyro_bias[1], + imu_gyro[2] - current_state.gyro_bias[2], + ] + ), ) # Define the bias @@ -265,20 +277,24 @@ def error_state_update( next_error_state = A_d @ current_error_state.as_vector() return next_error_state - - def unscented_transform(self, sigma_points: list[StateVector_euler], current_state: StateVector_quaternion, - imu_reading: np.ndarray,) -> StateVector_euler: - """ - Performs the Unscented Transform + + def unscented_transform( + self, + sigma_points: list[StateVector_euler], + current_state: StateVector_quaternion, + imu_reading: np.ndarray, + ) -> StateVector_euler: + """Performs the Unscented Transform This is the corresponding to a preditction step in the EKF """ - n = len(sigma_points[0].as_vector()) - self.y_i = np.zeros((n, 2*n)) + self.y_i = np.zeros((n, 2 * n + 1)) - for i in range(2*n): - self.y_i[:, i] = self.error_state_update(sigma_points[i], current_state, imu_reading) + for i in range(2 * n + 1): + self.y_i[:, i] = self.error_state_update( + sigma_points[i], current_state, imu_reading + ) error_state_estimate = StateVector_euler() @@ -286,9 +302,9 @@ def unscented_transform(self, sigma_points: list[StateVector_euler], current_sta error_state_estimate.fill_states(x) error_state_estimate.covariance = self.covariance_set(x, self.y_i) - + return error_state_estimate - + def H(self) -> np.ndarray: """Calculates the measurement matrix. @@ -296,39 +312,83 @@ def H(self) -> np.ndarray: np.ndarray: The measurement matrix. """ # Define the measurement matrix - H = np.zeros((3, 15)) + H = np.zeros((3, 16)) # For now assume only velocity is measured - H[0:3, 3:6] = np.eye(3) + H[:, 3:6] = np.eye(3) return H - - def measurement_update(self, sigma_points: list[StateVector_euler], current_error_state: StateVector_euler, dvl_data: np.ndarray, Weight: np.ndarray) -> StateVector_euler: - """ - Updates the state vector with the DVL data + + def injection( + self, + current_state_nom: StateVector_quaternion, + current_state_error: StateVector_euler, + ) -> StateVector_quaternion: + """Injects the error state into the nominal state + + Args: + current_state_nom (StateVector_quaternion): The current nominal state + current_state_error (StateVector_euler): The current error state + + Returns: + StateVector_quaternion: The updated nominal state """ + inj_state = StateVector_quaternion() + inj_state.position = current_state_nom.position + current_state_error.position + inj_state.velocity = current_state_nom.velocity + current_state_error.velocity + inj_state.orientation = quaternion_super_product( + current_state_nom.orientation, + 0.5 + * np.array( + [ + 2, + current_state_error.orientation[0], + current_state_error.orientation[1], + current_state_error.orientation[2], + ] + ), + ) + inj_state.acceleration_bias = ( + current_state_nom.acceleration_bias + current_state_error.acceleration_bias + ) + inj_state.gyro_bias = ( + current_state_nom.gyro_bias + current_state_error.gyro_bias + ) + + return inj_state + + def measurement_update( + self, + sigma_points: list[StateVector_euler], + current_nom_state: StateVector_quaternion, + current_error_state: StateVector_euler, + dvl_data: np.ndarray, + Weight: np.ndarray, + ) -> StateVector_euler: + """Updates the state vector with the DVL data + """ H = self.H() R = self.R n = len(sigma_points[0].as_vector()) - Z_i = np.zeros((H.shape[0], 2 * n)) + Z_i = np.zeros((H.shape[0], 2 * n + 1)) + + for i in range(2 * n + 1): + injected_state = self.injection(current_nom_state, sigma_points[i]) + Z_i[:, i] = np.dot(H, injected_state.as_vector()) - for i in range(2*n): - Z_i[:, i] = np.dot(H, sigma_points[i].as_vector()) - z = self.weighted_mean_set(Z_i, Weight) S = self.weighted_covariance_set(z, Z_i, Weight) x = self.mean_set(self.y_i) - # Calculate the rest innovation = dvl_data - z P_innovation = S + R - P_xz = self.covariance_set(x, self.y_i, z, Z_i) + P_xz = self.cross_covariance_set(x, self.y_i, z, Z_i, Weight) # Kalman gain K_k = np.dot(P_xz, np.linalg.inv(P_innovation)) @@ -339,102 +399,99 @@ def measurement_update(self, sigma_points: list[StateVector_euler], current_erro updated_error_state.fill_states(x + np.dot(K_k, innovation)) # Update the covariance - updated_error_state.covariance = current_error_state.covariance - np.dot(K_k, np.dot(P_innovation, K_k.T)) + updated_error_state.covariance = current_error_state.covariance - np.dot( + K_k, np.dot(P_innovation, K_k.T) + ) - return updated_error_state + return updated_error_state + + def imu_update_states( + self, + current_state_nom: StateVector_quaternion, + current_state_error: StateVector_euler, + imu_data: np.ndarray, + ) -> tuple[StateVector_quaternion, StateVector_euler]: + """Updates the state vector with the IMU data - def imu_update_states(self, current_state_nom: StateVector_quaternion, current_state_error: StateVector_euler, imu_data: np.ndarray) -> tuple[StateVector_quaternion, StateVector_euler]: - """ - Updates the state vector with the IMU data - Args: current_state_nom (StateVector_quaternion): The current nominal state current_state_error (StateVector_euler): The current error state imu_data (np.ndarray): The IMU data - + Returns: tuple[StateVector_quaternion, StateVector_euler]: The updated nominal and error states """ - # Update the nominal state current_state_nom = self.nominal_state_update(current_state_nom, imu_data) # Generate the sigma points - sigma_points, _ = self.generate_sigma_points(current_state_error, self.Q_process_noise) + sigma_points, _ = self.generate_sigma_points( + current_state_error, self.Q_process_noise + ) # Update the error state - current_state_error = self.unscented_transform(sigma_points, current_state_nom, imu_data) + current_state_error = self.unscented_transform( + sigma_points, current_state_nom, imu_data + ) return current_state_nom, current_state_error - - def dvl_update_states(self, current_state_nom: StateVector_quaternion, current_state_error: StateVector_euler, dvl_data: np.ndarray) -> tuple[StateVector_quaternion, StateVector_euler]: - """ - Update the error state given the DVL data + + def dvl_update_states( + self, + current_state_nom: StateVector_quaternion, + current_state_error: StateVector_euler, + dvl_data: np.ndarray, + ) -> tuple[StateVector_quaternion, StateVector_euler]: + """Update the error state given the DVL data Args: current_state_nom (StateVector_quaternion): The current nominal state current_state_error (StateVector_euler): The current error state dvl_data (np.ndarray): The DVL data to update the state with - + Returns: tuple[StateVector_quaternion, StateVector_euler]: The updated nominal and error states """ - # Generate the sigma points - sigma_points, weight = self.generate_sigma_points(current_state_error, self.Q_process_noise) + sigma_points, weight = self.generate_sigma_points( + current_state_error, self.Q_process_noise + ) # Update the error state - current_state_error = self.measurement_update(sigma_points, current_state_error, dvl_data, weight) + current_state_error = self.measurement_update( + sigma_points, current_state_nom, current_state_error, dvl_data, weight + ) return current_state_nom, current_state_error - - def inject_and_reset(self, current_state_nom: StateVector_quaternion, current_state_error: StateVector_euler) -> tuple[StateVector_quaternion, StateVector_euler]: - """ - Injects the error state into the nominal state and resets the error state + + def inject_and_reset( + self, + current_state_nom: StateVector_quaternion, + current_state_error: StateVector_euler, + ) -> tuple[StateVector_quaternion, StateVector_euler]: + """Injects the error state into the nominal state and resets the error state Args: current_state_nom (StateVector_quaternion): The current nominal state current_state_error (StateVector_euler): The current error state - - Returns: + + Returns: tuple[StateVector_quaternion, StateVector_euler]: The updated nominal and error states """ + inj_state = self.injection(current_state_nom, current_state_error) - inj_state = StateVector_quaternion() - - inj_state.position = current_state_nom.position + current_state_error.position - inj_state.velocity = current_state_nom.velocity + current_state_error.velocity - inj_state.orientation = quaternion_super_product( - current_state_nom.orientation, - 0.5 - * np.array( - [ - 2, - current_state_error.orientation[0], - current_state_error.orientation[1], - current_state_error.orientation[2], - ] - ), - ) - inj_state.acceleration_bias = ( - current_state_nom.acceleration_bias + current_state_error.acceleration_bias - ) - inj_state.gyro_bias = current_state_nom.gyro_bias + current_state_error.gyro_bias - - - # Resetting the error state G = np.eye(15) - G[6:9, 6:9] = np.eye(3) - skew_symmetric( - 0.5 * current_state_error.orientation - ) + G[6:9, 6:9] = np.eye(3) - skew_symmetric(0.5 * current_state_error.orientation) current_state_error.covariance = np.dot( np.dot(G, current_state_error.covariance), G.T ) + current_state_error.covariance += np.eye(15) * 1e-4 + + eigvals = np.linalg.eigvals(current_state_error.covariance) + print("Min eigenvalue:", np.min(eigvals)) current_state_error.fill_states(np.zeros(15)) - return inj_state, current_state_error - diff --git a/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_class.py b/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_class.py index 629ce460e..b9a76e26f 100644 --- a/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_class.py +++ b/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_class.py @@ -1,6 +1,13 @@ -import numpy as np from dataclasses import dataclass, field -from sp_ukf_python_utils import quaternion_super_product, quaternion_error, euler_rotation_quaternion, ssa + +import numpy as np +from sp_ukf_python_utils import ( + euler_rotation_quaternion, + quaternion_error, + quaternion_super_product, + ssa, +) + @dataclass class StateVector_quaternion: @@ -48,7 +55,7 @@ def fill_states(self, state: np.ndarray) -> None: self.orientation = state[6:10] self.acceleration_bias = state[10:13] self.gyro_bias = state[13:] - else: + else: self.position = state[0:3] self.velocity = state[3:6] self.orientation = euler_rotation_quaternion(state[6:9]) @@ -103,7 +110,7 @@ def euler_forward( new_state.orientation /= np.linalg.norm(new_state.orientation) return new_state - + def __sub__(self, other: 'StateVector_quaternion') -> np.ndarray: """Subtracts two StateVector_quaternion objects. @@ -128,7 +135,7 @@ def __sub__(self, other: 'StateVector_quaternion') -> np.ndarray: gyro_bias_diff, ] ) - + def __add__(self, other: 'np.ndarray') -> 'np.ndarray': """Adds a numpy array to this StateVector_quaternion. @@ -229,7 +236,6 @@ def __add__(self, other: 'np.ndarray') -> 'np.ndarray': Returns: np.ndarray: The result of the addition. """ - new_position = self.position + other[:3] new_velcoity = self.velocity + other[3:6] new_orientation = self.orientation + other[6:9] @@ -247,9 +253,8 @@ def __add__(self, other: 'np.ndarray') -> 'np.ndarray': ) def __sub__(self, other_state: 'StateVector_euler') -> 'StateVector_euler': - """ - Subtracts two StateVector_euler objects. - + """Subtracts two StateVector_euler objects. + Args: other (StateVector_euler): The other StateVector_euler object. @@ -263,7 +268,13 @@ def __sub__(self, other_state: 'StateVector_euler') -> 'StateVector_euler': gyro_bias_diff = self.gyro_bias - other_state[12:] return np.concatenate( - (position_diff, velocity_diff, orientation_diff, acceleration_bias_diff, gyro_bias_diff) + [ + position_diff, + velocity_diff, + orientation_diff, + acceleration_bias_diff, + gyro_bias_diff, + ] ) @@ -277,4 +288,4 @@ class MeasurementModel: ) # Measurement matrix measurement_covariance: np.ndarray = field( default_factory=lambda: np.zeros((6, 6)) - ) # Measurement noise matrix \ No newline at end of file + ) # Measurement noise matrix diff --git a/navigation/sp_ukf_python/sp_ukf_python/test_ukf.py b/navigation/sp_ukf_python/sp_ukf_python/test_ukf.py index 59e08aa55..d46962797 100644 --- a/navigation/sp_ukf_python/sp_ukf_python/test_ukf.py +++ b/navigation/sp_ukf_python/sp_ukf_python/test_ukf.py @@ -1,45 +1,78 @@ -import numpy as np import matplotlib.pyplot as plt -from mpl_toolkits.mplot3d import Axes3D # for 3D plotting +import numpy as np # (Assuming the following have been imported from your modules) -from sp_ukf_python_class import StateVector_quaternion, StateVector_euler -from sp_ukf_python_utils import skew_symmetric, quaternion_super_product +from sp_ukf_python_class import StateVector_euler, StateVector_quaternion + from sp_ukf_python import ErrorStateUnscentedKalmanFilter + def quat_to_yaw(q: np.ndarray) -> float: - """ - Convert a quaternion (assumed [w, x, y, z]) with zero roll and pitch - into a yaw angle. + """Convert a quaternion (assumed [w, x, y, z]) into a yaw angle. + In NED, yaw is typically around the z-down axis. """ return 2 * np.arctan2(q[3], q[0]) + def run_ESUKF_simulation(): + # ------------------------------------------------------------------------- # Simulation parameters - dt = 0.01 # time step [s] - T = 60.0 # total simulation time [s] - num_steps = int(T/dt) - g_val = 9.81 # gravitational acceleration + # ------------------------------------------------------------------------- + dt = 0.01 # time step [s] + T = 60.0 # total simulation time [s] + num_steps = int(T / dt) + # In an NED frame, gravity is +9.81 in the z (down) direction. + g_val = 9.81 + + # ------------------------------------------------------------------------- # Define noise and covariance matrices - Q = np.diag([0.1]*15) # Process noise covariance (15x15) - R_meas = np.diag([0.08]*3) # DVL measurement noise (velocity noise) - P_ab = 0.005 # Accelerometer bias dynamics matrix - P_wb = 0.005 # Gyro bias dynamics matrix - lever_arm = np.array([0.0, 0.0, 0.0]) # Assume sensor is at the center of mass + # ------------------------------------------------------------------------- + Q = np.diag( + [ + 0.06, + 0.06, + 0.06, # position error + 0.04, + 0.04, + 0.04, # velocity error + 0.003, + 0.003, + 0.003, # orientation error + 0.02, + 0.02, + 0.02, # accelerometer bias error + 0.02, + 0.02, + 0.02, # gyro bias error + ] + ) + + R_meas = np.diag([0.52, 0.52, 0.52]) # Increased DVL measurement noise - # Create ESUKF instance + # Bias dynamics tuning remains the same here: + P_ab = 0.002 + P_wb = 0.002 + lever_arm = np.array([0.0, 0.0, 0.0]) # Sensor at the vehicle CG + + # Create the Error-State UKF instance (NED convention) esukf = ErrorStateUnscentedKalmanFilter(P_ab, P_wb, Q, lever_arm, R_meas, g_val, dt) - # Initialize true state (StateVector_quaternion) with no biases. + # ------------------------------------------------------------------------- + # Initialize the true state in NED + # ------------------------------------------------------------------------- + # We treat x as North, y as East, z as Down. + # We'll do a circular path in the horizontal plane (z=0). true_state = StateVector_quaternion() - true_state.position = np.array([20.0, 0.0, 0.0]) - true_state.velocity = np.array([0.0, 1.0, 0.0]) + true_state.position = np.array([20.0, 0.0, 0.0]) # [N, E, D]=[20, 0, 0] + true_state.velocity = np.array([0.0, 1.0, 0.0]) # 1 m/s in the East direction true_state.orientation = np.array([1.0, 0.0, 0.0, 0.0]) # No initial rotation true_state.acceleration_bias = np.zeros(3) true_state.gyro_bias = np.zeros(3) - # Initialize estimated (nominal) state with a small offset. + # ------------------------------------------------------------------------- + # Initialize the estimated state + # ------------------------------------------------------------------------- est_state_nom = StateVector_quaternion() est_state_nom.position = true_state.position + np.array([0.1, -0.1, 0.05]) est_state_nom.velocity = true_state.velocity + np.array([0.05, 0.05, -0.05]) @@ -47,12 +80,14 @@ def run_ESUKF_simulation(): est_state_nom.acceleration_bias = np.zeros(3) est_state_nom.gyro_bias = np.zeros(3) - # Initialize error state (StateVector_euler) as zero with some initial covariance. + # Initialize error state (Euler) with some covariance est_state_error = StateVector_euler() est_state_error.fill_states(np.zeros(15)) - est_state_error.covariance = 0.1 * np.eye(15) + est_state_error.covariance = 0.5 * np.eye(15) + # ------------------------------------------------------------------------- # Prepare histories for plotting + # ------------------------------------------------------------------------- time_hist = [] true_pos_hist = [] est_pos_hist = [] @@ -61,67 +96,110 @@ def run_ESUKF_simulation(): true_yaw_hist = [] est_yaw_hist = [] - # For the true trajectory, we simulate a circle in the horizontal plane. - R_circle = 20.0 # circle radius [m] - omega = 0.05 # angular speed [rad/s] + # ------------------------------------------------------------------------- + # Define the "circular" trajectory in the horizontal plane (z=0) + # in NED: x=North, y=East, z=Down + # We'll revolve in the XY-plane, at D=0, with radius=20 m, angular speed=0.05 rad/s + # ------------------------------------------------------------------------- + R_circle = 20.0 + omega = 0.05 + # ------------------------------------------------------------------------- + # Main simulation loop + # ------------------------------------------------------------------------- t = 0.0 for step in range(num_steps): - # --- True State Generation --- - # Circular trajectory: position = [R*cos(omega*t), R*sin(omega*t), 0] - pos_true = np.array([R_circle * np.cos(omega * t), - R_circle * np.sin(omega * t), - 0.0]) - # Velocity is the derivative of position. - vel_true = np.array([-R_circle * omega * np.sin(omega * t), - R_circle * omega * np.cos(omega * t), - 0.0]) - # Acceleration is the second derivative. - acc_true = np.array([-R_circle * omega**2 * np.cos(omega * t), - -R_circle * omega**2 * np.sin(omega * t), - 0.0]) - # Update the true state. + # --- True State Generation (NED) --- + # Position: circle in x-y plane at z=0 + pos_true = np.array( + [ + R_circle * np.cos(omega * t), # N + R_circle * np.sin(omega * t), # E + 0.0, # D + ] + ) + # Velocity: derivative of pos + vel_true = np.array( + [ + -R_circle * omega * np.sin(omega * t), # d/dt of cos => -sin + R_circle * omega * np.cos(omega * t), # d/dt of sin => cos + 0.0, + ] + ) + # Acceleration: second derivative + acc_true = np.array( + [ + -R_circle * omega**2 * np.cos(omega * t), + -R_circle * omega**2 * np.sin(omega * t), + 0.0, + ] + ) + + # Update the "true" state in NED true_state.position = pos_true true_state.velocity = vel_true - # Compute heading (yaw) tangent to the path. + + # Compute full quaternion from Euler angles (roll, pitch, yaw) + roll_true = 0.0 + pitch_true = 0.0 yaw_true = np.arctan2(vel_true[1], vel_true[0]) - # For simplicity, assume roll and pitch are zero. - true_state.orientation = np.array([np.cos(yaw_true/2), 0.0, 0.0, np.sin(yaw_true/2)]) - # Biases remain zero for the true state. - - # --- Simulated IMU Measurements --- - # The nominal state propagation uses: - # velocity_dot = R_q() @ (imu_acc - bias) + g - # Therefore, the ideal accelerometer measurement is: - # imu_acc = R_true.T @ (acc_true - g_vector) - R_true = true_state.R_q() # rotation matrix from quaternion - imu_acc_ideal = np.dot(R_true.T, (acc_true - np.array([0.0, 0.0, g_val]))) - # Add noise (e.g., 0.1 m/s^2 std dev). - imu_acc_noise = np.random.normal(0.0, 0.1, 3) + cy = np.cos(yaw_true * 0.5) + sy = np.sin(yaw_true * 0.5) + cp = np.cos(pitch_true * 0.5) + sp = np.sin(pitch_true * 0.5) + cr = np.cos(roll_true * 0.5) + sr = np.sin(roll_true * 0.5) + true_state.orientation = np.array( + [ + cr * cp * cy + sr * sp * sy, # w + sr * cp * cy - cr * sp * sy, # x + cr * sp * cy + sr * cp * sy, # y + cr * cp * sy - sr * sp * cy, # z + ] + ) + + # --- Simulated IMU Measurements (NED) --- + # Gravity is +9.81 in the down (z) direction in NED + R_true = true_state.R_q() # rotation from body to inertial + # The "specific force" in body frame is (acc_inertial - gravity_inertial) rotated to body + imu_acc_ideal = R_true.T @ ( + acc_true - np.array([0.0, 0.0, g_val]) + ) + np.random.normal(0.01, 0.01, 3) # [rad/s] + + # Add small noise + imu_acc_noise = np.random.normal(0.0, 0.05, 3) # [m/s^2] imu_acc_meas = imu_acc_ideal + imu_acc_noise - # For the gyro: the true angular velocity in body frame. - # For a circular path with constant yaw rate, the ideal gyro reading is: - imu_gyro_ideal = np.array([0.0, 0.0, omega]) - # Add noise (e.g., 0.01 rad/s std dev). - imu_gyro_noise = np.random.normal(0.0, 0.01, 3) + # Gyro: angular velocity about body axes. Yaw rate is ~omega for a flat circle + imu_gyro_ideal = np.array([0.0, 0.0, omega]) + np.random.normal( + 0.01, 0.01, 3 + ) # [rad/s] + imu_gyro_noise = np.random.normal(0.0, 0.05, 3) # [rad/s] imu_gyro_meas = imu_gyro_ideal + imu_gyro_noise - # Combine to form the complete IMU measurement vector. + # Combine imu_meas = np.hstack((imu_acc_meas, imu_gyro_meas)) # --- Simulated DVL Measurement --- - # DVL measures velocity (here assumed in the inertial frame). + # Velocity in inertial frame (NED) with zero noise for this test dvl_noise = np.random.normal(0.0, 0.05, 3) dvl_meas = vel_true + dvl_noise - # --- Filter Updates --- - # 1. Propagate the nominal state with IMU data. - est_state_nom, est_state_error = esukf.imu_update_states(est_state_nom, est_state_error, imu_meas) - # 2. Incorporate DVL measurement. - est_state_nom, est_state_error = esukf.dvl_update_states(est_state_nom, est_state_error, dvl_meas) - # 3. Inject the error state into the nominal state and reset the error state. - est_state_nom, est_state_error = esukf.inject_and_reset(est_state_nom, est_state_error) + # --------------------------------------------------------------------- + # Filter Updates + # --------------------------------------------------------------------- + # 1. IMU update (prediction) + est_state_nom, est_state_error = esukf.imu_update_states( + est_state_nom, est_state_error, imu_meas + ) + # 2. DVL update (measurement) + est_state_nom, est_state_error = esukf.dvl_update_states( + est_state_nom, est_state_error, dvl_meas + ) + # 3. Inject error state + est_state_nom, est_state_error = esukf.inject_and_reset( + est_state_nom, est_state_error + ) # --- Store Histories --- time_hist.append(t) @@ -134,7 +212,9 @@ def run_ESUKF_simulation(): t += dt - # Convert histories to NumPy arrays. + # ------------------------------------------------------------------------- + # Convert histories to arrays + # ------------------------------------------------------------------------- true_pos_hist = np.array(true_pos_hist) est_pos_hist = np.array(est_pos_hist) true_vel_hist = np.array(true_vel_hist) @@ -143,76 +223,91 @@ def run_ESUKF_simulation(): est_yaw_hist = np.array(est_yaw_hist) time_hist = np.array(time_hist) - # --- Plotting Results --- - - # Plot positions (each axis separately) + # ------------------------------------------------------------------------- + # Plotting + # ------------------------------------------------------------------------- + # Positions plt.figure(figsize=(10, 8)) plt.subplot(3, 1, 1) - plt.plot(time_hist, true_pos_hist[:, 0], label='True X') - plt.plot(time_hist, est_pos_hist[:, 0], '--', label='Estimated X') - plt.ylabel('X Position (m)') + plt.plot(time_hist, true_pos_hist[:, 0], label='True N') + plt.plot(time_hist, est_pos_hist[:, 0], '--', label='Estimated N') + plt.ylabel('N (m)') plt.legend() plt.subplot(3, 1, 2) - plt.plot(time_hist, true_pos_hist[:, 1], label='True Y') - plt.plot(time_hist, est_pos_hist[:, 1], '--', label='Estimated Y') - plt.ylabel('Y Position (m)') + plt.plot(time_hist, true_pos_hist[:, 1], label='True E') + plt.plot(time_hist, est_pos_hist[:, 1], '--', label='Estimated E') + plt.ylabel('E (m)') plt.legend() plt.subplot(3, 1, 3) - plt.plot(time_hist, true_pos_hist[:, 2], label='True Z') - plt.plot(time_hist, est_pos_hist[:, 2], '--', label='Estimated Z') + plt.plot(time_hist, true_pos_hist[:, 2], label='True D') + plt.plot(time_hist, est_pos_hist[:, 2], '--', label='Estimated D') plt.xlabel('Time (s)') - plt.ylabel('Z Position (m)') + plt.ylabel('D (m)') plt.legend() plt.tight_layout() plt.show() - # Plot velocities + # Velocities plt.figure(figsize=(10, 8)) plt.subplot(3, 1, 1) - plt.plot(time_hist, true_vel_hist[:, 0], label='True Vx') - plt.plot(time_hist, est_vel_hist[:, 0], '--', label='Estimated Vx') - plt.ylabel('Vx (m/s)') + plt.plot(time_hist, true_vel_hist[:, 0], label='True Vn') + plt.plot(time_hist, est_vel_hist[:, 0], '--', label='Estimated Vn') + plt.ylabel('Vn (m/s)') plt.legend() plt.subplot(3, 1, 2) - plt.plot(time_hist, true_vel_hist[:, 1], label='True Vy') - plt.plot(time_hist, est_vel_hist[:, 1], '--', label='Estimated Vy') - plt.ylabel('Vy (m/s)') + plt.plot(time_hist, true_vel_hist[:, 1], label='True Ve') + plt.plot(time_hist, est_vel_hist[:, 1], '--', label='Estimated Ve') + plt.ylabel('Ve (m/s)') plt.legend() plt.subplot(3, 1, 3) - plt.plot(time_hist, true_vel_hist[:, 2], label='True Vz') - plt.plot(time_hist, est_vel_hist[:, 2], '--', label='Estimated Vz') + plt.plot(time_hist, true_vel_hist[:, 2], label='True Vd') + plt.plot(time_hist, est_vel_hist[:, 2], '--', label='Estimated Vd') plt.xlabel('Time (s)') - plt.ylabel('Vz (m/s)') + plt.ylabel('Vd (m/s)') plt.legend() plt.tight_layout() plt.show() - # Plot heading (yaw) + # Heading (Yaw) plt.figure(figsize=(10, 4)) plt.plot(time_hist, np.degrees(true_yaw_hist), label='True Yaw') plt.plot(time_hist, np.degrees(est_yaw_hist), '--', label='Estimated Yaw') plt.xlabel('Time (s)') plt.ylabel('Yaw (deg)') plt.legend() - plt.title('Heading Comparison') + plt.title('Heading Comparison (NED)') plt.tight_layout() plt.show() - # Plot 3D Trajectory + # 3D Trajectory fig = plt.figure(figsize=(8, 6)) ax = fig.add_subplot(111, projection='3d') - ax.plot(true_pos_hist[:, 0], true_pos_hist[:, 1], true_pos_hist[:, 2], label='True Trajectory', linewidth=2) - ax.plot(est_pos_hist[:, 0], est_pos_hist[:, 1], est_pos_hist[:, 2], '--', label='Estimated Trajectory', linewidth=2) - ax.set_xlabel('X (m)') - ax.set_ylabel('Y (m)') - ax.set_zlabel('Z (m)') + ax.plot( + true_pos_hist[:, 0], + true_pos_hist[:, 1], + true_pos_hist[:, 2], + label='True Trajectory', + linewidth=2, + ) + ax.plot( + est_pos_hist[:, 0], + est_pos_hist[:, 1], + est_pos_hist[:, 2], + '--', + label='Estimated Trajectory', + linewidth=2, + ) + ax.set_xlabel('North (m)') + ax.set_ylabel('East (m)') + ax.set_zlabel('Down (m)') ax.legend() - plt.title('3D Trajectory') + plt.title('3D Trajectory (NED Frame)') plt.show() + if __name__ == '__main__': run_ESUKF_simulation() From b5e1453c702f295575bb83798af74e1f9cac2874 Mon Sep 17 00:00:00 2001 From: Talha Nauman Choudhry Date: Sun, 9 Mar 2025 01:32:46 +0100 Subject: [PATCH 086/290] feat: working ukf filter is added, some issue in the ESUKF --- .../sp_ukf_python/sp_ukf_python.py | 40 +- .../sp_ukf_python/sp_ukf_python_class.py | 3 +- .../sp_ukf_python/sp_ukf_python_utils.py | 11 +- .../sp_ukf_python/sp_ukf_python/test_ukf.py | 2 +- navigation/ukf_okid/ukf_python/__ini__.py | 0 navigation/ukf_okid/ukf_python/ukf_okid.py | 376 ++++++++++++ .../ukf_python/ukf_okid_class copy.py | 568 ++++++++++++++++++ .../ukf_okid/ukf_python/ukf_okid_class.py | 464 ++++++++++++++ navigation/ukf_okid/ukf_python/ukf_utils.py | 36 ++ 9 files changed, 1476 insertions(+), 24 deletions(-) create mode 100644 navigation/ukf_okid/ukf_python/__ini__.py create mode 100644 navigation/ukf_okid/ukf_python/ukf_okid.py create mode 100644 navigation/ukf_okid/ukf_python/ukf_okid_class copy.py create mode 100644 navigation/ukf_okid/ukf_python/ukf_okid_class.py create mode 100644 navigation/ukf_okid/ukf_python/ukf_utils.py diff --git a/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python.py b/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python.py index 8d2ff6265..819cb737d 100644 --- a/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python.py +++ b/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python.py @@ -24,6 +24,7 @@ def __init__( self.g = np.array([0, 0, g]) self.dt = dt self.y_i = np.zeros((15, 2 * 15)) + self.W = np.zeros(2 * 15 + 1) def mean_set(self, set: np.ndarray) -> np.ndarray: """Calculates the mean of a set of values. @@ -34,11 +35,12 @@ def mean_set(self, set: np.ndarray) -> np.ndarray: Returns: np.ndarray: The mean of the set. """ - # Define the number of columns + # Define the number of sigma points based on columns n = set.shape[0] + mean_value = np.zeros(n) - # Calculate the mean value - mean_value = (1 / (2 * n)) * np.sum(set, axis=1) + for i in range(2 * n + 1): + mean_value += (1/(2 * n + 1)) * set[:, i] return mean_value @@ -58,8 +60,6 @@ def weighted_mean_set(self, set: np.ndarray, weight: np.ndarray) -> np.ndarray: for i in range(2 * n + 1): mean_value += weight[i] * set[:, i] - mean_value = (1 / (2 * n + 1)) * mean_value - return mean_value def covariance_set(self, mean: np.ndarray, set: np.ndarray) -> np.ndarray: @@ -160,31 +160,26 @@ def generate_sigma_points( """Generates the sigma points for the UKF This is done using the Cholesky decomposition method """ - # Define n n = len(error_state.covariance) kappa = 3 - n - # Computing S matrix using cholensky decomposition - # print(error_state.covariance + Q_process_noise) S = np.linalg.cholesky(error_state.covariance + Q_process_noise) - # print(S) S_scaled = np.sqrt(n + kappa) * S weighted_points = np.concatenate((S_scaled, -S_scaled), axis=1) sigma_points = [StateVector_euler() for _ in range(2 * n + 1)] + W = np.zeros(2 * n + 1) sigma_points[0].fill_states(error_state.as_vector()) - for i in range(2 * n): - sigma_points[i + 1].fill_states(error_state + weighted_points[:, i]) - - W = np.zeros(2 * n + 1) W[0] = kappa / (n + kappa) for i in range(2 * n): + sigma_points[i + 1].fill_states(error_state + weighted_points[:, i]) W[i + 1] = 1 / (2 * (n + kappa)) + self.W = W return sigma_points, W def nominal_state_update( @@ -298,10 +293,10 @@ def unscented_transform( error_state_estimate = StateVector_euler() - x = self.mean_set(self.y_i) + x = self.weighted_mean_set(self.y_i, self.W) error_state_estimate.fill_states(x) - error_state_estimate.covariance = self.covariance_set(x, self.y_i) + error_state_estimate.covariance = self.weighted_covariance_set(x, self.y_i, self.W) return error_state_estimate @@ -311,10 +306,10 @@ def H(self) -> np.ndarray: Returns: np.ndarray: The measurement matrix. """ - # Define the measurement matrix + # Define the measurement matrix (error state is 15-dim) H = np.zeros((3, 16)) - # For now assume only velocity is measured + # For now assume only velocity is measured (located at indices 3:6) H[:, 3:6] = np.eye(3) return H @@ -442,6 +437,7 @@ def dvl_update_states( current_state_nom: StateVector_quaternion, current_state_error: StateVector_euler, dvl_data: np.ndarray, + imu_data: np.ndarray, ) -> tuple[StateVector_quaternion, StateVector_euler]: """Update the error state given the DVL data @@ -458,6 +454,11 @@ def dvl_update_states( current_state_error, self.Q_process_noise ) + # Update the error state + current_state_error = self.unscented_transform( + sigma_points, current_state_nom, imu_data + ) + # Update the error state current_state_error = self.measurement_update( sigma_points, current_state_nom, current_state_error, dvl_data, weight @@ -487,10 +488,7 @@ def inject_and_reset( current_state_error.covariance = np.dot( np.dot(G, current_state_error.covariance), G.T ) - current_state_error.covariance += np.eye(15) * 1e-4 - - eigvals = np.linalg.eigvals(current_state_error.covariance) - print("Min eigenvalue:", np.min(eigvals)) + current_state_error.covariance += np.eye(15) current_state_error.fill_states(np.zeros(15)) diff --git a/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_class.py b/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_class.py index b9a76e26f..f8ede884d 100644 --- a/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_class.py +++ b/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_class.py @@ -6,6 +6,7 @@ quaternion_error, quaternion_super_product, ssa, + quat_norm, ) @@ -100,7 +101,7 @@ def euler_forward( # Define the state derivatives new_state.position = current_state.position + self.position * dt new_state.velocity = current_state.velocity + self.velocity * dt - new_state.orientation = current_state.orientation + self.orientation * dt + new_state.orientation = quat_norm(current_state.orientation + self.orientation * dt) new_state.acceleration_bias = ( current_state.acceleration_bias + self.acceleration_bias * dt ) diff --git a/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_utils.py b/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_utils.py index 071180fc7..56285f031 100644 --- a/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_utils.py +++ b/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_utils.py @@ -104,4 +104,13 @@ def ssa(angle: np.ndarray) -> np.ndarray: for i in range(len(angle)): ssa_vector[i] = (angle[i] + np.pi) % (2 * np.pi) - np.pi - return ssa_vector \ No newline at end of file + return ssa_vector + +def quat_norm(quat: np.ndarray) -> np.ndarray: + """ + Function that normalizes a quaternion + """ + + quat = quat / np.linalg.norm(quat) + + return quat diff --git a/navigation/sp_ukf_python/sp_ukf_python/test_ukf.py b/navigation/sp_ukf_python/sp_ukf_python/test_ukf.py index d46962797..1d8c723b1 100644 --- a/navigation/sp_ukf_python/sp_ukf_python/test_ukf.py +++ b/navigation/sp_ukf_python/sp_ukf_python/test_ukf.py @@ -194,7 +194,7 @@ def run_ESUKF_simulation(): ) # 2. DVL update (measurement) est_state_nom, est_state_error = esukf.dvl_update_states( - est_state_nom, est_state_error, dvl_meas + est_state_nom, est_state_error, dvl_meas, imu_meas ) # 3. Inject error state est_state_nom, est_state_error = esukf.inject_and_reset( diff --git a/navigation/ukf_okid/ukf_python/__ini__.py b/navigation/ukf_okid/ukf_python/__ini__.py new file mode 100644 index 000000000..e69de29bb diff --git a/navigation/ukf_okid/ukf_python/ukf_okid.py b/navigation/ukf_okid/ukf_python/ukf_okid.py new file mode 100644 index 000000000..c588d579e --- /dev/null +++ b/navigation/ukf_okid/ukf_python/ukf_okid.py @@ -0,0 +1,376 @@ +from ukf_okid_class import * +import numpy as np +import time +import matplotlib.pyplot as plt + + +class UKF: + def __init__(self, process_model: process_model, x_0, P_0, Q, R): + self.x = x_0 + self.P = P_0 + self.Q = Q + self.R = R + self.process_model = process_model + self.sigma_points_list = None + self.y_i = None + self.weight = None + + def sigma_points(self, current_state: StateQuat) -> tuple[list[StateQuat], np.ndarray]: + """ + Functions that generate the sigma points for the UKF + """ + n = len(current_state.covariance) + kappa = 3 - n + + S = np.linalg.cholesky(current_state.covariance + self.Q) + S_scaled = np.sqrt(n + kappa) * S + + weighted_points = np.concatenate([S_scaled, -S_scaled], axis=1) + + self.sigma_points_list = [StateQuat() for _ in range(2 * n + 1)] + W = np.zeros(2 * n + 1) + + self.sigma_points_list [0].fill_states(current_state.as_vector()) + W[0] = kappa / (n + kappa) + for i in range(2 * n): + self.sigma_points_list [i + 1].fill_states(current_state.insert_weights(weighted_points[:, i])) + W[i + 1] = 1 / (2 * (n + kappa)) + + self.weight = W + + return self.sigma_points_list , self.weight + + + def unscented_transform(self, current_state: StateQuat) -> StateQuat: + """ + The unscented transform function generates the priori state estimate + """ + + _ , _ = self.sigma_points(current_state) + n = len(current_state.covariance) + + self.y_i = [StateQuat() for _ in range(2 * n + 1)] + + for i in range(2 * n + 1): + self.process_model.model_prediction(self.sigma_points_list[i]) + self.y_i[i] = self.process_model.euler_forward() + + state_estimate = StateQuat() + x = mean_set(self.y_i, self.weight) + + state_estimate.fill_states(x) + state_estimate.covariance = covariance_set(self.y_i, x, self.weight) + return state_estimate + + def measurement_update(self, current_state: StateQuat, measurement: MeasModel) -> tuple[MeasModel, np.ndarray]: + """ + Function that updates the state estimate with a measurement + Hopefully this is the DVL or GNSS + """ + + n = len(current_state.covariance) + z_i = [MeasModel() for _ in range(2 * n + 1)] + + for i in range(2 * n + 1): + z_i[i] = measurement.H(self.sigma_points_list[i]) + + meas_update = MeasModel() + + meas_update.measurement = mean_measurement(z_i, self.weight) + + meas_update.covariance = covariance_measurement(z_i, meas_update.measurement, self.weight) + + cross_correlation = cross_covariance(self.y_i, current_state.as_vector(), z_i, meas_update.measurement, self.weight) + + return meas_update, cross_correlation + + def posteriori_estimate(self, current_state: StateQuat, cross_correlation: np.ndarray, measurement: MeasModel, ex_measuremnt: MeasModel) -> StateQuat: + """ + Calculates the posteriori estimate using measurment and the prior estimate + """ + + nu_k = MeasModel() + + nu_k.measurement = measurement.measurement - ex_measuremnt.measurement + nu_k.covariance = ex_measuremnt.covariance + measurement.covariance + + K_k = np.dot(cross_correlation, np.linalg.inv(nu_k.covariance)) + + posteriori_estimate = StateQuat() + + posteriori_estimate.fill_states_different_dim(current_state.as_vector(), np.dot(K_k, nu_k.measurement)) + posteriori_estimate.covariance = current_state.covariance - np.dot(K_k, np.dot(nu_k.covariance, np.transpose(K_k))) + + self.process_model.state_vector_prev = posteriori_estimate + + return posteriori_estimate + +def add_quaternion_noise(q, noise_std): + + noise = np.random.normal(0, noise_std, 3) + + theta = np.linalg.norm(noise) + + if theta > 0: + + axis = noise / theta + + q_noise = np.hstack((np.cos(theta/2), np.sin(theta/2) * axis)) + + else: + + q_noise = np.array([1.0, 0.0, 0.0, 0.0]) + + q_new = quaternion_super_product(q, q_noise) + + return q_new / np.linalg.norm(q_new) + + +if __name__ == '__main__': + + # Create initial state vector and covariance matrix. + x0 = np.zeros(13) + x0[0:3] = [0.3, 0.3, 0.3] + x0[3] = 1 + x0[7:10] = [0.2, 0.2, 0.2] + dt = 0.01 + R = (0.1 / dt) * np.eye(3) + + Q = 0.1 * np.eye(12) + P0 = np.eye(12) * 0.1 + + model = process_model() + model.dt = 0.01 + model.mass_interia_matrix = np.array([ + [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] + ]) + model.m = 30.0 + model.r_b_bg = np.array([0.01, 0.0, 0.02]) + model.inertia = np.diag([0.68, 3.32, 3.34]) + model.damping_linear = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) + model.damping_nonlinear = np.array([0.3, 0.3, 0.3, 0.3, 0.3, 0.3]) + model.added_mass = np.diag([1.0, 1.0, 1.0, 2.0, 2.0, 2.0]) + + model_ukf = model + + # Simulation parameters + simulation_time = 40 # seconds + num_steps = int(simulation_time / dt) + + # Initialize a dummy StateQuat. + test_state = StateQuat() + test_state.fill_states(x0) + test_state.covariance = P0 + + # Initialize a estimated state + estimated_state = StateQuat() + estimated_state.fill_states(x0) + estimated_state.covariance = P0 + + # Initialize a estimated state + noisy_state = StateQuat() + noisy_state.fill_states(x0) + noisy_state.covariance = P0 + + measurment_model = MeasModel() + measurment_model.measurement = np.array([0.0, 0.0, 0.0]) + measurment_model.covariance = R + + # Initialize arrays to store the results + positions = np.zeros((num_steps, 3)) + orientations = np.zeros((num_steps, 3)) + velocities = np.zeros((num_steps, 3)) + angular_velocities = np.zeros((num_steps, 3)) + + # Initialize arrays to store the estimates + positions_est = np.zeros((num_steps, 3)) + orientations_est = np.zeros((num_steps, 3)) + velocities_est = np.zeros((num_steps, 3)) + angular_velocities_est = np.zeros((num_steps, 3)) + + # Initialize the okid params + okid_params = np.zeros((num_steps, 21)) + + model.state_vector_prev = test_state + model.state_vector = test_state + + model_ukf.state_vector_prev = test_state + model_ukf.state_vector = test_state + + # initialize the ukf + ukf = UKF(model_ukf, x0, P0, Q, R) + + # Test + ukf.unscented_transform(test_state) + + elapsed_times = [] + + u = lambda t: np.array([2 * np.sin(1 * t), 2 * np.sin(1 * t), 2 * np.sin(1 * t), 0.2 * np.cos(1 * t), 0.2 * np.cos(1 * t), 0.2 * np.cos(1 * t)]) + + # Run the simulation + for step in range(num_steps): + # Insert control input + model.Control_input = u(step * dt) + model_ukf.Control_input = u(step * dt) + + # Perform the unscented transform + model.model_prediction(test_state) + new_state = model.euler_forward() + + # Adding noise in the state vector + noisy_state.position = new_state.position + np.random.normal(0, 0.1, 3) + noisy_state.orientation = add_quaternion_noise(new_state.orientation, 0.1) + noisy_state.velocity = new_state.velocity + np.random.normal(0, 0.1, 3) + noisy_state.angular_velocity = new_state.angular_velocity + np.random.normal(0, 0.1, 3) + + start_time = time.time() + estimated_state = ukf.unscented_transform(noisy_state) + elapsed_time = time.time() - start_time + elapsed_times.append(elapsed_time) + + if step % 20 == 0: + measurment_model.measurement = new_state.velocity + np.random.normal(0, 0.2, 3) + meas_update, covariance_matrix = ukf.measurement_update(estimated_state, measurment_model) + estimated_state = ukf.posteriori_estimate(estimated_state, covariance_matrix, measurment_model, meas_update) + + + positions[step, :] = new_state.position + orientations[step, :] = quat_to_euler(new_state.orientation) + velocities[step, :] = new_state.velocity + angular_velocities[step, :] = new_state.angular_velocity + + positions_est[step, :] = estimated_state.position + orientations_est[step, :] = quat_to_euler(estimated_state.orientation) + velocities_est[step, :] = estimated_state.velocity + angular_velocities_est[step, :] = estimated_state.angular_velocity + + # Update the state for the next iteration + model.state_vector_prev = new_state + + print('Average elapsed time: ', np.mean(elapsed_times)) + print('Max elapsed time: ', np.max(elapsed_times)) + print('Min elapsed time: ', np.min(elapsed_times)) + print('median elapsed time: ', np.median(elapsed_times)) + # Plot the results + time = np.linspace(0, simulation_time, num_steps) + + # Plot positions + plt.figure() + plt.subplot(3, 1, 1) + plt.plot(time, positions[:, 0], label='True') + plt.plot(time, positions_est[:, 0], label='Estimated') + plt.title('Position X') + plt.xlabel('Time [s]') + plt.ylabel('Position X [m]') + plt.legend() + + plt.subplot(3, 1, 2) + plt.plot(time, positions[:, 1], label='True') + plt.plot(time, positions_est[:, 1], label='Estimated') + plt.title('Position Y') + plt.xlabel('Time [s]') + plt.ylabel('Position Y [m]') + plt.legend() + + plt.subplot(3, 1, 3) + plt.plot(time, positions[:, 2], label='True') + plt.plot(time, positions_est[:, 2], label='Estimated') + plt.title('Position Z') + plt.xlabel('Time [s]') + plt.ylabel('Position Z [m]') + plt.legend() + + plt.tight_layout() + plt.show() + + # Plot orientations (Euler angles) + plt.figure() + plt.subplot(3, 1, 1) + plt.plot(time, orientations[:, 0], label='True') + plt.plot(time, orientations_est[:, 0], label='Estimated') + plt.title('Orientation Roll') + plt.xlabel('Time [s]') + plt.ylabel('Roll [rad]') + plt.legend() + + plt.subplot(3, 1, 2) + plt.plot(time, orientations[:, 1], label='True') + plt.plot(time, orientations_est[:, 1], label='Estimated') + plt.title('Orientation Pitch') + plt.xlabel('Time [s]') + plt.ylabel('Pitch [rad]') + plt.legend() + + plt.subplot(3, 1, 3) + plt.plot(time, orientations[:, 2], label='True') + plt.plot(time, orientations_est[:, 2], label='Estimated') + plt.title('Orientation Yaw') + plt.xlabel('Time [s]') + plt.ylabel('Yaw [rad]') + plt.legend() + + plt.tight_layout() + plt.show() + + # Plot velocities + plt.figure() + plt.subplot(3, 1, 1) + plt.plot(time, velocities[:, 0], label='True') + plt.plot(time, velocities_est[:, 0], label='Estimated') + plt.title('Velocity X') + plt.xlabel('Time [s]') + plt.ylabel('Velocity X [m/s]') + plt.legend() + + plt.subplot(3, 1, 2) + plt.plot(time, velocities[:, 1], label='True') + plt.plot(time, velocities_est[:, 1], label='Estimated') + plt.title('Velocity Y') + plt.xlabel('Time [s]') + plt.ylabel('Velocity Y [m/s]') + plt.legend() + + plt.subplot(3, 1, 3) + plt.plot(time, velocities[:, 2], label='True') + plt.plot(time, velocities_est[:, 2], label='Estimated') + plt.title('Velocity Z') + plt.xlabel('Time [s]') + plt.ylabel('Velocity Z [m/s]') + plt.legend() + + plt.tight_layout() + plt.show() + + # Plot angular velocities + plt.figure() + plt.subplot(3, 1, 1) + plt.plot(time, angular_velocities[:, 0], label='True') + plt.plot(time, angular_velocities_est[:, 0], label='Estimated') + plt.title('Angular Velocity X') + plt.xlabel('Time [s]') + plt.ylabel('Angular Velocity X [rad/s]') + plt.legend() + + plt.subplot(3, 1, 2) + plt.plot(time, angular_velocities[:, 1], label='True') + plt.plot(time, angular_velocities_est[:, 1], label='Estimated') + plt.title('Angular Velocity Y') + plt.xlabel('Time [s]') + plt.ylabel('Angular Velocity Y [rad/s]') + plt.legend() + + plt.subplot(3, 1, 3) + plt.plot(time, angular_velocities[:, 2], label='True') + plt.plot(time, angular_velocities_est[:, 2], label='Estimated') + plt.title('Angular Velocity Z') + plt.xlabel('Time [s]') + plt.ylabel('Angular Velocity Z [rad/s]') + plt.legend() + + plt.tight_layout() + plt.show() \ No newline at end of file diff --git a/navigation/ukf_okid/ukf_python/ukf_okid_class copy.py b/navigation/ukf_okid/ukf_python/ukf_okid_class copy.py new file mode 100644 index 000000000..86b7beb49 --- /dev/null +++ b/navigation/ukf_okid/ukf_python/ukf_okid_class copy.py @@ -0,0 +1,568 @@ +from dataclasses import dataclass, field +import numpy as np + + +from dataclasses import dataclass, field +import numpy as np + +@dataclass +class StateQuat: + """ + A class to represent the state to be estimated by the UKF. + """ + position: np.ndarray = field(default_factory=lambda: np.zeros(3)) + orientation: np.ndarray = field(default_factory=lambda: np.array([1, 0, 0, 0])) + velocity: np.ndarray = field(default_factory=lambda: np.zeros(3)) + angular_velocity: np.ndarray = field(default_factory=lambda: np.zeros(3)) + okid_params: np.ndarray = field(default_factory=lambda: np.zeros(21)) + covariance: np.ndarray = field(default_factory=lambda: np.zeros((33, 33))) + + def as_vector(self) -> np.ndarray: + """Returns the StateVector as a numpy array.""" + return np.concatenate([self.position, self.orientation, self.velocity, self.angular_velocity, self.okid_params]) + + def nu(self) -> np.ndarray: + """Calculates the nu vector.""" + return np.concatenate([self.velocity, self.angular_velocity]) + + def R_q(self) -> np.ndarray: + """Calculates the rotation matrix from the orientation quaternion.""" + q0, q1, q2, q3 = self.orientation + R = np.array([ + [1 - 2 * q2**2 - 2 * q3**2, 2 * (q1 * q2 - q0 * q3), 2 * (q0 * q2 + q1 * q3)], + [2 * (q1 * q2 + q0 * q3), 1 - 2 * q1**2 - 2 * q3**2, 2 * (q2 * q3 - q0 * q1)], + [2 * (q1 * q3 - q0 * q2), 2 * (q0 * q1 + q2 * q3), 1 - 2 * q1**2 - 2 * q2**2] + ]) + return R + + def fill_states(self, state: np.ndarray) -> None: + """Fills the state vector with the values from a numpy array.""" + self.position = state[0:3] + self.orientation = state[3:7] + self.velocity = state[7:10] + self.angular_velocity = state[10:13] + + if len(state) > 13: + self.okid_params = state[13:] + + def fill_states_different_dim(self, state: np.ndarray, state_euler: np.ndarray) -> None: + """Fills states when the state vector has different dimensions than the default state vector.""" + self.position = state[0:3] + state_euler[0:3] + self.orientation = quaternion_super_product(state[3:7], euler_to_quat(state_euler[3:6])) + self.velocity = state[7:10] + state_euler[6:9] + self.angular_velocity = state[10:13] + state_euler[9:12] + + if len(state) > 13: + self.okid_params = state[13:] + + def subtract(self, other: 'StateQuat') -> np.ndarray: + """Subtracts two StateQuat objects, returning the difference with Euler angles.""" + new_array = np.zeros(len(self.as_vector()) - 1) + new_array[:3] = self.position - other.position + new_array[3:6] = quat_to_euler(quaternion_error(self.orientation, other.orientation)) + new_array[6:9] = self.velocity - other.velocity + new_array[9:12] = self.angular_velocity - other.angular_velocity + + new_array[12:] = self.okid_params - other.okid_params + + return new_array + + def __add__(self, other: 'StateQuat') -> 'StateQuat': + """Adds two StateQuat objects.""" + new_state = StateQuat() + new_state.position = self.position + other.position + new_state.orientation = quaternion_super_product(self.orientation, other.orientation) + new_state.velocity = self.velocity + other.velocity + new_state.angular_velocity = self.angular_velocity + other.angular_velocity + + new_state.okid_params = self.okid_params + other.okid_params + + return new_state + + def __sub__(self, other: 'StateQuat') -> 'StateQuat': + """Subtracts two StateQuat objects.""" + new_state = StateQuat() + new_state.position = self.position - other.position + new_state.orientation = quaternion_error(self.orientation, other.orientation) + new_state.velocity = self.velocity - other.velocity + new_state.angular_velocity = self.angular_velocity - other.angular_velocity + + new_state.okid_params = self.okid_params - other.okid_params + + return new_state.as_vector() + + def __rmul__(self, scalar: float) -> 'StateQuat': + """Multiplies the StateQuat object by a scalar.""" + new_state = StateQuat() + new_state.position = scalar * self.position + new_state.orientation = quat_norm(scalar * self.orientation) + new_state.velocity = scalar * self.velocity + new_state.angular_velocity = scalar * self.angular_velocity + + new_state.okid_params = scalar * self.okid_params + + return new_state + + def insert_weights(self, weights: np.ndarray) -> np.ndarray: + """Inserts the weights into the covariance matrix.""" + new_state = StateQuat() + new_state.position = self.position - weights[:3] + new_state.orientation = quaternion_error(self.orientation, euler_to_quat(weights[3:6])) + new_state.velocity = self.velocity - weights[6:9] + new_state.angular_velocity = self.angular_velocity - weights[9:12] + new_state.okid_params = self.okid_params - weights[12:] + + return new_state.as_vector() + + def add_without_quaternions(self, other: 'StateQuat') -> None: + """Adds elements into the state vector without considering the quaternions.""" + self.position += other.position + self.velocity += other.velocity + self.angular_velocity += other.angular_velocity + self.okid_params += other.okid_params + +@dataclass +class MeasModel: + """ + A class defined for a general measurement model. + """ + measurement: np.ndarray = field(default_factory=lambda: np.zeros(3)) + covariance: np.ndarray = field(default_factory=lambda: np.zeros((3, 3))) + + def H(self, state: StateQuat) -> 'MeasModel': + """Calculates the measurement matrix.""" + H = np.zeros((3, 34)) + H[:, 7:10] = np.eye(3) + z_i = MeasModel() + z_i.measurement = np.dot(H, state.as_vector()) + return z_i + + def __add__(self, other: 'MeasModel') -> 'MeasModel': + """Defines the addition operation between two MeasModel objects.""" + result = MeasModel() + result.measurement = self.measurement + other.measurement + return result + + def __rmul__(self, scalar: float) -> 'MeasModel': + """Defines multiplication between scalar value and MeasModel object.""" + result = MeasModel() + result.measurement = scalar * self.measurement + return result + + def __sub__(self, other: 'MeasModel') -> 'MeasModel': + """Defines the subtraction between two MeasModel objects.""" + result = MeasModel() + result.measurement = self.measurement - other.measurement + return result + +@dataclass +class process_model: + """ + A class defined for a general process model. + """ + state_vector: StateQuat = field(default_factory=StateQuat) + state_vector_dot: StateQuat = field(default_factory=StateQuat) + state_vector_prev: StateQuat = field(default_factory=StateQuat) + Control_input: np.ndarray = field(default_factory=lambda: np.zeros(6)) + mass_interia_matrix: np.ndarray = field(default_factory=lambda: np.zeros((6, 6))) + added_mass: np.ndarray = field(default_factory=lambda: np.zeros(6)) + damping_linear: np.ndarray = field(default_factory=lambda: np.zeros(6)) + damping_nonlinear: np.ndarray = field(default_factory=lambda: np.zeros(6)) + m: float = 0.0 + inertia: np.ndarray = field(default_factory=lambda: np.zeros((3,3))) + r_b_bg: np.ndarray = field(default_factory=lambda: np.zeros(3)) + dt: float = 0.0 + integral_error_position: np.ndarray = field(default_factory=lambda: np.zeros(3)) + integral_error_orientation: np.ndarray = field(default_factory=lambda: np.zeros(4)) + prev_position_error: np.ndarray = field(default_factory=lambda: np.zeros(3)) + prev_orientation_error: np.ndarray = field(default_factory=lambda: np.zeros(3)) + + def R(self) -> np.ndarray: + """Calculates the rotation matrix.""" + nu, e_1, e_2, e_3 = self.state_vector.orientation + R = np.array([ + [1 - 2 * e_2 ** 2 - 2 * e_3 ** 2, 2 * e_1 * e_2 - 2 * nu * e_3, 2 * e_1 * e_3 + 2 * nu * e_2], + [2 * e_1 * e_2 + 2 * nu * e_3, 1 - 2 * e_1 ** 2 - 2 * e_3 ** 2, 2 * e_2 * e_3 - 2 * nu * e_1], + [2 * e_1 * e_3 - 2 * nu * e_2, 2 * e_2 * e_3 + 2 * nu * e_1, 1 - 2 * e_1 ** 2 - 2 * e_2 ** 2] + ]) + return R + + def T(self) -> np.ndarray: + """Calculates the transformation matrix.""" + nu, e_1, e_2, e_3 = self.state_vector.orientation + T = 0.5 * np.array([ + [-e_1, -e_2, -e_3], + [nu, -e_3, e_2], + [e_3, nu, -e_1], + [-e_2, e_1, nu] + ]) + return T + + def Crb(self) -> np.ndarray: + """Calculates the Coriolis matrix.""" + ang_vel = self.state_vector.angular_velocity + ang_vel_skew = skew_symmetric(ang_vel) + lever_arm_skew = skew_symmetric(self.r_b_bg) + Crb = np.zeros((6, 6)) + Crb[0:3, 0:3] = self.m * ang_vel_skew + Crb[3:6, 3:6] = -skew_symmetric(np.dot(self.inertia, ang_vel)) + Crb[0:3, 3:6] = -self.m * np.dot(ang_vel_skew, lever_arm_skew) + Crb[3:6, 0:3] = self.m * np.dot(lever_arm_skew, ang_vel_skew) + return Crb + + def D(self) -> np.ndarray: + """Calculates the damping matrix.""" + D_l = -np.diag(self.damping_linear) + D_nl = -np.diag(self.damping_nonlinear) * np.abs(self.state_vector.nu()) + return D_l + D_nl + + def model_prediction(self, state: StateQuat) -> None: + """Calculates the model of the system.""" + self.state_vector = state + self.state_vector_dot.position = np.dot(self.R(), self.state_vector.velocity) + self.state_vector_dot.orientation = np.dot(self.T(), self.state_vector.angular_velocity) + Nu = np.linalg.inv(self.mass_interia_matrix + np.diag(self.added_mass)) @ (self.Control_input - np.dot(self.Crb(), self.state_vector.nu()) - np.dot(self.D(), self.state_vector.nu())) + self.state_vector_dot.velocity = Nu[:3] + self.state_vector_dot.angular_velocity = Nu[3:] + + def euler_forward(self) -> StateQuat: + """Calculates the forward Euler integration.""" + self.state_vector.position = self.state_vector_prev.position + self.state_vector_dot.position * self.dt + self.state_vector.orientation = quat_norm(self.state_vector_prev.orientation + self.state_vector_dot.orientation * self.dt) + self.state_vector.velocity = self.state_vector_prev.velocity + self.state_vector_dot.velocity * self.dt + self.state_vector.angular_velocity = self.state_vector_prev.angular_velocity + self.state_vector_dot.angular_velocity * self.dt + return self.state_vector + +@dataclass +class okid_model: + """ + A class defined for a general process model. + """ + state_vector: StateQuat = field(default_factory=StateQuat) + state_vector_dot: StateQuat = field(default_factory=StateQuat) + state_vector_prev: StateQuat = field(default_factory=StateQuat) + Control_input: np.ndarray = field(default_factory=lambda: np.zeros(6)) + mass_interia_matrix: np.ndarray = field(default_factory=lambda: np.zeros((6, 6))) + m: float = 0.0 + inertia: np.ndarray = field(default_factory=lambda: np.zeros((3,3))) + r_b_bg: np.ndarray = field(default_factory=lambda: np.zeros(3)) + dt: float = 0.0 + prev_position_error: np.ndarray = field(default_factory=lambda: np.zeros(3)) + prev_orientation_error: np.ndarray = field(default_factory=lambda: np.zeros(3)) + D_matrix: np.ndarray = field(default_factory=lambda: np.zeros((6, 6))) + added_mass: np.ndarray = field(default_factory=lambda: np.zeros(6)) + + def R(self) -> np.ndarray: + """Calculates the rotation matrix.""" + nu, e_1, e_2, e_3 = self.state_vector.orientation + R = np.array([ + [1 - 2 * e_2 ** 2 - 2 * e_3 ** 2, 2 * e_1 * e_2 - 2 * nu * e_3, 2 * e_1 * e_3 + 2 * nu * e_2], + [2 * e_1 * e_2 + 2 * nu * e_3, 1 - 2 * e_1 ** 2 - 2 * e_3 ** 2, 2 * e_2 * e_3 - 2 * nu * e_1], + [2 * e_1 * e_3 - 2 * nu * e_2, 2 * e_2 * e_3 + 2 * nu * e_1, 1 - 2 * e_1 ** 2 - 2 * e_2 ** 2] + ]) + return R + + def T(self) -> np.ndarray: + """Calculates the transformation matrix.""" + nu, e_1, e_2, e_3 = self.state_vector.orientation + T = 0.5 * np.array([ + [-e_1, -e_2, -e_3], + [nu, -e_3, e_2], + [e_3, nu, -e_1], + [-e_2, e_1, nu] + ]) + return T + + def Crb(self) -> np.ndarray: + """Calculates the Coriolis matrix.""" + ang_vel = self.state_vector.angular_velocity + ang_vel_skew = skew_symmetric(ang_vel) + lever_arm_skew = skew_symmetric(self.r_b_bg) + Crb = np.zeros((6, 6)) + Crb[0:3, 0:3] = self.m * ang_vel_skew + Crb[3:6, 3:6] = -skew_symmetric(np.dot(self.inertia, ang_vel)) + Crb[0:3, 3:6] = -self.m * np.dot(ang_vel_skew, lever_arm_skew) + Crb[3:6, 0:3] = self.m * np.dot(lever_arm_skew, ang_vel_skew) + return Crb + + def D(self, linear_damping: np.ndarray, nonlinear_damping: np.ndarray) -> np.ndarray: + """Calculates the damping matrix.""" + D_l = -np.diag(linear_damping) + D_nl = -np.diag(nonlinear_damping) * np.abs(self.state_vector.nu()) + return D_l + D_nl + + def model_prediction(self, state: StateQuat) -> None: + """Calculates the model of the system.""" + self.state_vector = state + + self.inertia = np.diag(self.state_vector.okid_params[:3]) + self.mass_interia_matrix[3:6, 3:6] = self.inertia + self.D_matrix = self.D(self.state_vector.okid_params[3:9], self.state_vector.okid_params[9:15]) + self.added_mass = self.state_vector.okid_params[15:21] + + self.state_vector_dot.position = np.dot(self.R(), self.state_vector.velocity) + self.state_vector_dot.orientation = np.dot(self.T(), self.state_vector.angular_velocity) + + Nu = np.linalg.inv(self.mass_interia_matrix + np.diag(self.added_mass)) @ (self.Control_input - np.dot(self.Crb(), self.state_vector.nu()) - np.dot(self.D_matrix, self.state_vector.nu())) + self.state_vector_dot.velocity = Nu[:3] + self.state_vector_dot.angular_velocity = Nu[3:] + + self.state_vector_dot.okid_params = np.zeros(21) + + def euler_forward(self) -> StateQuat: + """Calculates the forward Euler integration.""" + self.state_vector.position = self.state_vector_prev.position + self.state_vector_dot.position * self.dt + self.state_vector.orientation = quat_norm(self.state_vector_prev.orientation + self.state_vector_dot.orientation * self.dt) + self.state_vector.velocity = self.state_vector_prev.velocity + self.state_vector_dot.velocity * self.dt + self.state_vector.angular_velocity = self.state_vector_prev.angular_velocity + self.state_vector_dot.angular_velocity * self.dt + self.state_vector.okid_params = self.state_vector_prev.okid_params + return self.state_vector + + + +def euler_to_quat(euler_angles: np.ndarray) -> np.ndarray: + """ + Converts Euler angles to a quaternion + """ + psi, theta, phi = euler_angles + c_psi = np.cos(psi / 2) + s_psi = np.sin(psi / 2) + c_theta = np.cos(theta / 2) + s_theta = np.sin(theta / 2) + c_phi = np.cos(phi / 2) + s_phi = np.sin(phi / 2) + + quat = np.array([ + c_psi * c_theta * c_phi + s_psi * s_theta * s_phi, + c_psi * c_theta * s_phi - s_psi * s_theta * c_phi, + s_psi * c_theta * s_phi + c_psi * s_theta * c_phi, + s_psi * c_theta * c_phi - c_psi * s_theta * s_phi + ]) + + return quat + +def quat_to_euler(quat: np.ndarray) -> np.ndarray: + """ + Converts a quaternion to Euler angles + """ + nu, eta_1, eta_2, eta_3 = quat + + phi = np.arctan2(2*(eta_2 * eta_3 + nu * eta_1), 1 - 2 * (eta_1 ** 2 + eta_2 ** 2)) + theta = -np.arcsin(2 * (eta_1 * eta_3 - nu * eta_2)) + psi = np.arctan2(2 * (nu * eta_3 + eta_1 * eta_2), 1 - 2 * (eta_2 ** 2 + eta_3 ** 2)) + + return np.array([phi, theta, psi]) + +def quat_norm(quat: np.ndarray) -> np.ndarray: + """ + Function that normalizes a quaternion + """ + + quat = quat / np.linalg.norm(quat) + + return quat + +def skew_symmetric(vector: np.ndarray) -> np.ndarray: + """Calculates the skew symmetric matrix of a vector. + + Args: + vector (np.ndarray): The vector. + + Returns: + np.ndarray: The skew symmetric matrix. + """ + return np.array( + [ + [0, -vector[2], vector[1]], + [vector[2], 0, -vector[0]], + [-vector[1], vector[0], 0], + ] + ) + +def quaternion_super_product(q1: np.ndarray, q2: np.ndarray) -> np.ndarray: + """Calculates the quaternion super product of two quaternions. + + Args: + q1 (np.ndarray): The first quaternion. + q2 (np.ndarray): The second quaternion. + + Returns: + np.ndarray: The quaternion super product. + """ + eta_0, e_0_x, e_0_y, e_0_z = q1 + eta_1, e_1_x, e_1_y, e_1_z = q2 + + e_0 = np.array([e_0_x, e_0_y, e_0_z]) + e_1 = np.array([e_1_x, e_1_y, e_1_z]) + + eta_new = eta_0 * eta_1 - (e_0_x * e_1_x + e_0_y * e_1_y + e_0_z * e_1_z) + nu_new = e_1 * eta_0 + e_0 * eta_1 + np.dot(skew_symmetric(e_0), e_1) + + q_new = quat_norm(np.array([eta_new, nu_new[0], nu_new[1], nu_new[2]])) + + return q_new + +def quaternion_error(quat_1: np.ndarray, quat_2: np.ndarray) -> np.ndarray: + """ + Calculates the error between two quaternions + """ + + quat_2_inv = np.array([quat_2[0], -quat_2[1], -quat_2[2], -quat_2[3]]) + + error_quat = quaternion_super_product(quat_1, quat_2_inv) + + return error_quat + +def iterative_quaternion_mean_statequat(state_list: list[StateQuat], weights: np.ndarray, tol: float = 1e-6, max_iter: int = 100) -> np.ndarray: + """ + Computes the weighted mean of the quaternion orientations from a list of StateQuat objects + using an iterative approach, without requiring the caller to manually extract the quaternion. + + Parameters: + state_list (list[StateQuat]): List of StateQuat objects. + weights (np.ndarray): Weights for each state. + tol (float): Convergence tolerance. + max_iter (int): Maximum number of iterations. + + Returns: + np.ndarray: The averaged quaternion as a 4-element numpy array. + """ + # Internally extract the quaternion from each state + sigma_quats = [state.orientation for state in state_list] + + # Initialize the mean quaternion with the first quaternion + mean_q = sigma_quats[0].copy() + + for _ in range(max_iter): + weighted_error_vectors = [] + for i, q in enumerate(sigma_quats): + # Compute the error quaternion: e = q * inv(mean_q) + # For unit quaternions, the inverse is the conjugate. + mean_q_conj = np.array([mean_q[0], -mean_q[1], -mean_q[2], -mean_q[3]]) + e = quaternion_super_product(q, mean_q_conj) + + # Clip to avoid numerical issues + e0_clipped = np.clip(e[0], -1.0, 1.0) + angle = 2 * np.arccos(e0_clipped) + if np.abs(angle) < 1e-8: + error_vec = np.zeros(3) + else: + # Compute the full rotation vector (angle * axis) + error_vec = (angle / np.sin(angle / 2)) * e[1:4] + weighted_error_vectors.append(weights[i] * error_vec) + + error_avg = np.sum(weighted_error_vectors, axis=0) + if np.linalg.norm(error_avg) < tol: + break + + error_norm = np.linalg.norm(error_avg) + delta_q = (np.array([np.cos(error_norm / 2), + *(np.sin(error_norm / 2) * (error_avg / error_norm))]) + if error_norm > 0 else np.array([1.0, 0.0, 0.0, 0.0])) + mean_q = quaternion_super_product(delta_q, mean_q) + mean_q = quat_norm(mean_q) + + return mean_q + + + +def mean_set(set_points: list[StateQuat], weights: np.ndarray = None) -> np.ndarray: + """ + Function that calculates the mean of a set of points + """ + n = len(set_points[0].as_vector()) - 1 + mean_value = StateQuat() + + if weights is None: + for i in range(2 * n + 1): + weight_temp_list = (1/ (2 * n + 1)) * np.ones(2 * n + 1) + mean_value.add_without_quaternions(weight_temp_list[i] * set_points[i]) + + mean_value.orientation = iterative_quaternion_mean_statequat(set_points, weight_temp_list) + + else: + for i in range(2 * n + 1): + mean_value.add_without_quaternions(weights[i] * set_points[i]) + + mean_value.orientation = iterative_quaternion_mean_statequat(set_points, weights) + + return mean_value.as_vector() + +def mean_measurement(set_points: list[MeasModel], weights: np.ndarray = None) -> np.ndarray: + """ + Function that calculates the mean of a set of points + """ + n = len(set_points) + mean_value = MeasModel() + + if weights is None: + for i in range(n): + mean_value = mean_value + set_points[i] + else: + for i in range(n): + mean_value = mean_value + (weights[i] * set_points[i]) + + return mean_value.measurement + +def covariance_set(set_points: list[StateQuat], mean: np.ndarray, weights: np.ndarray = None) -> np.ndarray: + """ + Function that calculates the covariance of a set of points + """ + n = len(set_points[0].as_vector()) - 1 + covariance = np.zeros((n, n)) + mean_quat = StateQuat() + mean_quat.fill_states(mean) + + if weights is None: + for i in range(2 * n + 1): + covariance += np.outer(set_points[i].subtract(mean_quat), set_points[i].subtract(mean_quat)) + + covariance = (1 / (2 * n + 1)) * covariance + + else: + for i in range(2 * n + 1): + covariance += weights[i] * np.outer(set_points[i].subtract(mean_quat), set_points[i].subtract(mean_quat)) + + return covariance + +def covariance_measurement(set_points: list[MeasModel], mean: np.ndarray, weights: np.ndarray = None) -> np.ndarray: + """ + Function that calculates the covariance of a set of points + """ + n = len(set_points) + co_size = len(set_points[0].measurement) + covariance = np.zeros((co_size, co_size)) + mean_meas = MeasModel() + mean_meas.measurement = mean + + if weights is None: + for i in range(n): + temp_model = set_points[i] - mean_meas + covariance += np.outer(temp_model.measurement, temp_model.measurement) + + covariance = (1 / (n)) * covariance + + else: + for i in range(n): + temp_model = set_points[i] - mean_meas + covariance += weights[i] * np.outer(temp_model.measurement, temp_model.measurement) + + return covariance + +def cross_covariance(set_y: list[StateQuat], mean_y: np.ndarray, set_z: list[MeasModel], mean_z: np.ndarray, weights: np.ndarray) -> np.ndarray: + """ + Calculates the cross covariance between the measurement and state prediction + """ + + n = len(mean_y) - 1 + m = len(mean_z) + cross_covariance = np.zeros((n,m)) + mean_quat = StateQuat() + mean_quat.fill_states(mean_y) + + for i in range(n): + cross_covariance += np.outer(set_y[i].subtract(mean_quat), set_z[i].measurement - mean_z) + + cross_covariance = (1 / len(set_y)) * cross_covariance + + return cross_covariance diff --git a/navigation/ukf_okid/ukf_python/ukf_okid_class.py b/navigation/ukf_okid/ukf_python/ukf_okid_class.py new file mode 100644 index 000000000..8444fd82e --- /dev/null +++ b/navigation/ukf_okid/ukf_python/ukf_okid_class.py @@ -0,0 +1,464 @@ +from dataclasses import dataclass, field +import numpy as np + + +from dataclasses import dataclass, field +import numpy as np + +@dataclass +class StateQuat: + """ + A class to represent the state to be estimated by the UKF. + """ + position: np.ndarray = field(default_factory=lambda: np.zeros(3)) + orientation: np.ndarray = field(default_factory=lambda: np.array([1, 0, 0, 0])) + velocity: np.ndarray = field(default_factory=lambda: np.zeros(3)) + angular_velocity: np.ndarray = field(default_factory=lambda: np.zeros(3)) + covariance: np.ndarray = field(default_factory=lambda: np.zeros((12, 12))) + + def as_vector(self) -> np.ndarray: + """Returns the StateVector as a numpy array.""" + return np.concatenate([self.position, self.orientation, self.velocity, self.angular_velocity]) + + def nu(self) -> np.ndarray: + """Calculates the nu vector.""" + return np.concatenate([self.velocity, self.angular_velocity]) + + def R_q(self) -> np.ndarray: + """Calculates the rotation matrix from the orientation quaternion.""" + q0, q1, q2, q3 = self.orientation + R = np.array([ + [1 - 2 * q2**2 - 2 * q3**2, 2 * (q1 * q2 - q0 * q3), 2 * (q0 * q2 + q1 * q3)], + [2 * (q1 * q2 + q0 * q3), 1 - 2 * q1**2 - 2 * q3**2, 2 * (q2 * q3 - q0 * q1)], + [2 * (q1 * q3 - q0 * q2), 2 * (q0 * q1 + q2 * q3), 1 - 2 * q1**2 - 2 * q2**2] + ]) + return R + + def fill_states(self, state: np.ndarray) -> None: + """Fills the state vector with the values from a numpy array.""" + self.position = state[0:3] + self.orientation = state[3:7] + self.velocity = state[7:10] + self.angular_velocity = state[10:13] + + def fill_states_different_dim(self, state: np.ndarray, state_euler: np.ndarray) -> None: + """Fills states when the state vector has different dimensions than the default state vector.""" + self.position = state[0:3] + state_euler[0:3] + self.orientation = quaternion_super_product(state[3:7], euler_to_quat(state_euler[3:6])) + self.velocity = state[7:10] + state_euler[6:9] + self.angular_velocity = state[10:13] + state_euler[9:12] + + def subtract(self, other: 'StateQuat') -> np.ndarray: + """Subtracts two StateQuat objects, returning the difference with Euler angles.""" + new_array = np.zeros(len(self.as_vector()) - 1) + new_array[:3] = self.position - other.position + new_array[3:6] = quat_to_euler(quaternion_error(self.orientation, other.orientation)) + new_array[6:9] = self.velocity - other.velocity + new_array[9:12] = self.angular_velocity - other.angular_velocity + + return new_array + + def __add__(self, other: 'StateQuat') -> 'StateQuat': + """Adds two StateQuat objects.""" + new_state = StateQuat() + new_state.position = self.position + other.position + new_state.orientation = quaternion_super_product(self.orientation, other.orientation) + new_state.velocity = self.velocity + other.velocity + new_state.angular_velocity = self.angular_velocity + other.angular_velocity + + return new_state + + def __sub__(self, other: 'StateQuat') -> 'StateQuat': + """Subtracts two StateQuat objects.""" + new_state = StateQuat() + new_state.position = self.position - other.position + new_state.orientation = quaternion_error(self.orientation, other.orientation) + new_state.velocity = self.velocity - other.velocity + new_state.angular_velocity = self.angular_velocity - other.angular_velocity + + return new_state.as_vector() + + def __rmul__(self, scalar: float) -> 'StateQuat': + """Multiplies the StateQuat object by a scalar.""" + new_state = StateQuat() + new_state.position = scalar * self.position + new_state.orientation = quat_norm(scalar * self.orientation) + new_state.velocity = scalar * self.velocity + new_state.angular_velocity = scalar * self.angular_velocity + + return new_state + + def insert_weights(self, weights: np.ndarray) -> np.ndarray: + """Inserts the weights into the covariance matrix.""" + new_state = StateQuat() + new_state.position = self.position - weights[:3] + new_state.orientation = quaternion_error(self.orientation, euler_to_quat(weights[3:6])) + new_state.velocity = self.velocity - weights[6:9] + new_state.angular_velocity = self.angular_velocity - weights[9:12] + + return new_state.as_vector() + + def add_without_quaternions(self, other: 'StateQuat') -> None: + """Adds elements into the state vector without considering the quaternions.""" + self.position += other.position + self.velocity += other.velocity + self.angular_velocity += other.angular_velocity + +@dataclass +class MeasModel: + """ + A class defined for a general measurement model. + """ + measurement: np.ndarray = field(default_factory=lambda: np.zeros(3)) + covariance: np.ndarray = field(default_factory=lambda: np.zeros((3, 3))) + + def H(self, state: StateQuat) -> 'MeasModel': + """Calculates the measurement matrix.""" + H = np.zeros((3, 13)) + H[:, 7:10] = np.eye(3) + z_i = MeasModel() + z_i.measurement = np.dot(H, state.as_vector()) + return z_i + + def __add__(self, other: 'MeasModel') -> 'MeasModel': + """Defines the addition operation between two MeasModel objects.""" + result = MeasModel() + result.measurement = self.measurement + other.measurement + return result + + def __rmul__(self, scalar: float) -> 'MeasModel': + """Defines multiplication between scalar value and MeasModel object.""" + result = MeasModel() + result.measurement = scalar * self.measurement + return result + + def __sub__(self, other: 'MeasModel') -> 'MeasModel': + """Defines the subtraction between two MeasModel objects.""" + result = MeasModel() + result.measurement = self.measurement - other.measurement + return result + +@dataclass +class process_model: + """ + A class defined for a general process model. + """ + state_vector: StateQuat = field(default_factory=StateQuat) + state_vector_dot: StateQuat = field(default_factory=StateQuat) + state_vector_prev: StateQuat = field(default_factory=StateQuat) + Control_input: np.ndarray = field(default_factory=lambda: np.zeros(6)) + mass_interia_matrix: np.ndarray = field(default_factory=lambda: np.zeros((6, 6))) + added_mass: np.ndarray = field(default_factory=lambda: np.zeros(6)) + damping_linear: np.ndarray = field(default_factory=lambda: np.zeros(6)) + damping_nonlinear: np.ndarray = field(default_factory=lambda: np.zeros(6)) + m: float = 0.0 + inertia: np.ndarray = field(default_factory=lambda: np.zeros((3,3))) + r_b_bg: np.ndarray = field(default_factory=lambda: np.zeros(3)) + dt: float = 0.0 + integral_error_position: np.ndarray = field(default_factory=lambda: np.zeros(3)) + integral_error_orientation: np.ndarray = field(default_factory=lambda: np.zeros(4)) + prev_position_error: np.ndarray = field(default_factory=lambda: np.zeros(3)) + prev_orientation_error: np.ndarray = field(default_factory=lambda: np.zeros(3)) + + def R(self) -> np.ndarray: + """Calculates the rotation matrix.""" + nu, e_1, e_2, e_3 = self.state_vector.orientation + R = np.array([ + [1 - 2 * e_2 ** 2 - 2 * e_3 ** 2, 2 * e_1 * e_2 - 2 * nu * e_3, 2 * e_1 * e_3 + 2 * nu * e_2], + [2 * e_1 * e_2 + 2 * nu * e_3, 1 - 2 * e_1 ** 2 - 2 * e_3 ** 2, 2 * e_2 * e_3 - 2 * nu * e_1], + [2 * e_1 * e_3 - 2 * nu * e_2, 2 * e_2 * e_3 + 2 * nu * e_1, 1 - 2 * e_1 ** 2 - 2 * e_2 ** 2] + ]) + return R + + def T(self) -> np.ndarray: + """Calculates the transformation matrix.""" + nu, e_1, e_2, e_3 = self.state_vector.orientation + T = 0.5 * np.array([ + [-e_1, -e_2, -e_3], + [nu, -e_3, e_2], + [e_3, nu, -e_1], + [-e_2, e_1, nu] + ]) + return T + + def Crb(self) -> np.ndarray: + """Calculates the Coriolis matrix.""" + ang_vel = self.state_vector.angular_velocity + ang_vel_skew = skew_symmetric(ang_vel) + lever_arm_skew = skew_symmetric(self.r_b_bg) + Crb = np.zeros((6, 6)) + Crb[0:3, 0:3] = self.m * ang_vel_skew + Crb[3:6, 3:6] = -skew_symmetric(np.dot(self.inertia, ang_vel)) + Crb[0:3, 3:6] = -self.m * np.dot(ang_vel_skew, lever_arm_skew) + Crb[3:6, 0:3] = self.m * np.dot(lever_arm_skew, ang_vel_skew) + return Crb + + def D(self) -> np.ndarray: + """Calculates the damping matrix.""" + D_l = -np.diag(self.damping_linear) + D_nl = -np.diag(self.damping_nonlinear) * np.abs(self.state_vector.nu()) + return D_l + D_nl + + def model_prediction(self, state: StateQuat) -> None: + """Calculates the model of the system.""" + self.state_vector = state + self.state_vector_dot.position = np.dot(self.R(), self.state_vector.velocity) + self.state_vector_dot.orientation = np.dot(self.T(), self.state_vector.angular_velocity) + Nu = np.linalg.inv(self.mass_interia_matrix + np.diag(self.added_mass)) @ (self.Control_input - np.dot(self.Crb(), self.state_vector.nu()) - np.dot(self.D(), self.state_vector.nu())) + self.state_vector_dot.velocity = Nu[:3] + self.state_vector_dot.angular_velocity = Nu[3:] + + def euler_forward(self) -> StateQuat: + """Calculates the forward Euler integration.""" + self.state_vector.position = self.state_vector_prev.position + self.state_vector_dot.position * self.dt + self.state_vector.orientation = quat_norm(self.state_vector_prev.orientation + self.state_vector_dot.orientation * self.dt) + self.state_vector.velocity = self.state_vector_prev.velocity + self.state_vector_dot.velocity * self.dt + self.state_vector.angular_velocity = self.state_vector_prev.angular_velocity + self.state_vector_dot.angular_velocity * self.dt + return self.state_vector + +def euler_to_quat(euler_angles: np.ndarray) -> np.ndarray: + """ + Converts Euler angles to a quaternion + """ + psi, theta, phi = euler_angles + c_psi = np.cos(psi / 2) + s_psi = np.sin(psi / 2) + c_theta = np.cos(theta / 2) + s_theta = np.sin(theta / 2) + c_phi = np.cos(phi / 2) + s_phi = np.sin(phi / 2) + + quat = np.array([ + c_psi * c_theta * c_phi + s_psi * s_theta * s_phi, + c_psi * c_theta * s_phi - s_psi * s_theta * c_phi, + s_psi * c_theta * s_phi + c_psi * s_theta * c_phi, + s_psi * c_theta * c_phi - c_psi * s_theta * s_phi + ]) + + return quat + +def quat_to_euler(quat: np.ndarray) -> np.ndarray: + """ + Converts a quaternion to Euler angles + """ + nu, eta_1, eta_2, eta_3 = quat + + phi = np.arctan2(2*(eta_2 * eta_3 + nu * eta_1), 1 - 2 * (eta_1 ** 2 + eta_2 ** 2)) + theta = -np.arcsin(2 * (eta_1 * eta_3 - nu * eta_2)) + psi = np.arctan2(2 * (nu * eta_3 + eta_1 * eta_2), 1 - 2 * (eta_2 ** 2 + eta_3 ** 2)) + + return np.array([phi, theta, psi]) + +def quat_norm(quat: np.ndarray) -> np.ndarray: + """ + Function that normalizes a quaternion + """ + + quat = quat / np.linalg.norm(quat) + + return quat + +def skew_symmetric(vector: np.ndarray) -> np.ndarray: + """Calculates the skew symmetric matrix of a vector. + + Args: + vector (np.ndarray): The vector. + + Returns: + np.ndarray: The skew symmetric matrix. + """ + return np.array( + [ + [0, -vector[2], vector[1]], + [vector[2], 0, -vector[0]], + [-vector[1], vector[0], 0], + ] + ) + +def quaternion_super_product(q1: np.ndarray, q2: np.ndarray) -> np.ndarray: + """Calculates the quaternion super product of two quaternions. + + Args: + q1 (np.ndarray): The first quaternion. + q2 (np.ndarray): The second quaternion. + + Returns: + np.ndarray: The quaternion super product. + """ + eta_0, e_0_x, e_0_y, e_0_z = q1 + eta_1, e_1_x, e_1_y, e_1_z = q2 + + e_0 = np.array([e_0_x, e_0_y, e_0_z]) + e_1 = np.array([e_1_x, e_1_y, e_1_z]) + + eta_new = eta_0 * eta_1 - (e_0_x * e_1_x + e_0_y * e_1_y + e_0_z * e_1_z) + nu_new = e_1 * eta_0 + e_0 * eta_1 + np.dot(skew_symmetric(e_0), e_1) + + q_new = quat_norm(np.array([eta_new, nu_new[0], nu_new[1], nu_new[2]])) + + return q_new + +def quaternion_error(quat_1: np.ndarray, quat_2: np.ndarray) -> np.ndarray: + """ + Calculates the error between two quaternions + """ + + quat_2_inv = np.array([quat_2[0], -quat_2[1], -quat_2[2], -quat_2[3]]) + + error_quat = quaternion_super_product(quat_1, quat_2_inv) + + return error_quat + +def iterative_quaternion_mean_statequat(state_list: list[StateQuat], weights: np.ndarray, tol: float = 1e-6, max_iter: int = 100) -> np.ndarray: + """ + Computes the weighted mean of the quaternion orientations from a list of StateQuat objects + using an iterative approach, without requiring the caller to manually extract the quaternion. + + Parameters: + state_list (list[StateQuat]): List of StateQuat objects. + weights (np.ndarray): Weights for each state. + tol (float): Convergence tolerance. + max_iter (int): Maximum number of iterations. + + Returns: + np.ndarray: The averaged quaternion as a 4-element numpy array. + """ + # Internally extract the quaternion from each state + sigma_quats = [state.orientation for state in state_list] + + # Initialize the mean quaternion with the first quaternion + mean_q = sigma_quats[0].copy() + + for _ in range(max_iter): + weighted_error_vectors = [] + for i, q in enumerate(sigma_quats): + # Compute the error quaternion: e = q * inv(mean_q) + # For unit quaternions, the inverse is the conjugate. + mean_q_conj = np.array([mean_q[0], -mean_q[1], -mean_q[2], -mean_q[3]]) + e = quaternion_super_product(q, mean_q_conj) + + # Clip to avoid numerical issues + e0_clipped = np.clip(e[0], -1.0, 1.0) + angle = 2 * np.arccos(e0_clipped) + if np.abs(angle) < 1e-8: + error_vec = np.zeros(3) + else: + # Compute the full rotation vector (angle * axis) + error_vec = (angle / np.sin(angle / 2)) * e[1:4] + weighted_error_vectors.append(weights[i] * error_vec) + + error_avg = np.sum(weighted_error_vectors, axis=0) + if np.linalg.norm(error_avg) < tol: + break + + error_norm = np.linalg.norm(error_avg) + delta_q = (np.array([np.cos(error_norm / 2), + *(np.sin(error_norm / 2) * (error_avg / error_norm))]) + if error_norm > 0 else np.array([1.0, 0.0, 0.0, 0.0])) + mean_q = quaternion_super_product(delta_q, mean_q) + mean_q = quat_norm(mean_q) + + return mean_q + + + +def mean_set(set_points: list[StateQuat], weights: np.ndarray = None) -> np.ndarray: + """ + Function that calculates the mean of a set of points + """ + n = len(set_points[0].as_vector()) - 1 + mean_value = StateQuat() + + if weights is None: + for i in range(2 * n + 1): + weight_temp_list = (1/ (2 * n + 1)) * np.ones(2 * n + 1) + mean_value.add_without_quaternions(weight_temp_list[i] * set_points[i]) + + mean_value.orientation = iterative_quaternion_mean_statequat(set_points, weight_temp_list) + + else: + for i in range(2 * n + 1): + mean_value.add_without_quaternions(weights[i] * set_points[i]) + + mean_value.orientation = iterative_quaternion_mean_statequat(set_points, weights) + + return mean_value.as_vector() + +def mean_measurement(set_points: list[MeasModel], weights: np.ndarray = None) -> np.ndarray: + """ + Function that calculates the mean of a set of points + """ + n = len(set_points) + mean_value = MeasModel() + + if weights is None: + for i in range(n): + mean_value = mean_value + set_points[i] + else: + for i in range(n): + mean_value = mean_value + (weights[i] * set_points[i]) + + return mean_value.measurement + +def covariance_set(set_points: list[StateQuat], mean: np.ndarray, weights: np.ndarray = None) -> np.ndarray: + """ + Function that calculates the covariance of a set of points + """ + n = len(set_points[0].as_vector()) - 1 + covariance = np.zeros((n, n)) + mean_quat = StateQuat() + mean_quat.fill_states(mean) + + if weights is None: + for i in range(2 * n + 1): + covariance += np.outer(set_points[i].subtract(mean_quat), set_points[i].subtract(mean_quat)) + + covariance = (1 / (2 * n + 1)) * covariance + + else: + for i in range(2 * n + 1): + covariance += weights[i] * np.outer(set_points[i].subtract(mean_quat), set_points[i].subtract(mean_quat)) + + return covariance + +def covariance_measurement(set_points: list[MeasModel], mean: np.ndarray, weights: np.ndarray = None) -> np.ndarray: + """ + Function that calculates the covariance of a set of points + """ + n = len(set_points) + co_size = len(set_points[0].measurement) + covariance = np.zeros((co_size, co_size)) + mean_meas = MeasModel() + mean_meas.measurement = mean + + if weights is None: + for i in range(n): + temp_model = set_points[i] - mean_meas + covariance += np.outer(temp_model.measurement, temp_model.measurement) + + covariance = (1 / (n)) * covariance + + else: + for i in range(n): + temp_model = set_points[i] - mean_meas + covariance += weights[i] * np.outer(temp_model.measurement, temp_model.measurement) + + return covariance + +def cross_covariance(set_y: list[StateQuat], mean_y: np.ndarray, set_z: list[MeasModel], mean_z: np.ndarray, weights: np.ndarray) -> np.ndarray: + """ + Calculates the cross covariance between the measurement and state prediction + """ + + n = len(mean_y) - 1 + m = len(mean_z) + cross_covariance = np.zeros((n,m)) + mean_quat = StateQuat() + mean_quat.fill_states(mean_y) + + for i in range(n): + cross_covariance += np.outer(set_y[i].subtract(mean_quat), set_z[i].measurement - mean_z) + + cross_covariance = (1 / len(set_y)) * cross_covariance + + return cross_covariance diff --git a/navigation/ukf_okid/ukf_python/ukf_utils.py b/navigation/ukf_okid/ukf_python/ukf_utils.py new file mode 100644 index 000000000..f52f2eb62 --- /dev/null +++ b/navigation/ukf_okid/ukf_python/ukf_utils.py @@ -0,0 +1,36 @@ +import numpy as np +from dataclasses import dataclass +from ukf_okid_class import StateQuat + +def print_StateQuat_list(state_list: list[StateQuat], name="StateQuat List", print_covariance=True): + """ + Custom print function to print a list of StateQuat objects in a formatted form. + """ + print(f"{name}:") + for i, state in enumerate(state_list): + print(f"Index {i}:") + print_StateQuat(state, f"StateQuat {i}", print_covariance) + +def print_StateQuat(state: StateQuat, name="StateQuat", print_covariance=True): + """ + Custom print function to print StateQuat objects in a formatted form. + """ + print(f"{name}:") + print(f" Position: {state.position}") + print(f" Orientation: {state.orientation}") + print(f" Velocity: {state.velocity}") + print(f" Angular Velocity: {state.angular_velocity}") + print(f" okid_params: {state.okid_params}") + if print_covariance: + print_matrix(state.covariance, "Covariance") + +def print_matrix(matrix, name="Matrix"): + """ + Custom print function to print matrices in a formatted form. + """ + print(f"{name}: {matrix.shape}") + if isinstance(matrix, np.ndarray): + for row in matrix: + print(" ".join(f"{val:.2f}" for val in row)) + else: + print(matrix) From 707c4f468c7d2608116a3075318bed1f1164ed97 Mon Sep 17 00:00:00 2001 From: Talha Nauman Choudhry Date: Fri, 14 Mar 2025 23:04:40 +0100 Subject: [PATCH 087/290] added in some changes to eskf and the ukf algorithm --- .../eskf_python/eskf_python_class.py | 215 +++++++ .../eskf_python/eskf_python_filter.py | 600 +++++------------- .../eskf_python/eskf_python_node.py | 2 +- .../eskf_python/eskf_python_utils.py | 95 +++ .../eskf_python/ukf_okid_class.py} | 110 +--- navigation/sp_ukf_python/CMakeLists.txt | 33 - navigation/sp_ukf_python/README.md | 0 .../sp_ukf_python/config/sp_ukf_python.yaml | 3 - navigation/sp_ukf_python/launch/ukf.launch.py | 22 - navigation/sp_ukf_python/package.xml | 23 - .../sp_ukf_python/sp_ukf_python/__init__.py | 0 .../sp_ukf_python/sp_ukf_python.py | 495 --------------- .../sp_ukf_python/sp_ukf_python_class.py | 292 --------- .../sp_ukf_python/sp_ukf_python_node.py | 137 ---- .../sp_ukf_python/sp_ukf_python_utils.py | 116 ---- .../sp_ukf_python/sp_ukf_python/test_ukf.py | 313 --------- navigation/ukf_okid/ukf_python/ukf_okid.py | 87 ++- 17 files changed, 551 insertions(+), 1992 deletions(-) create mode 100644 navigation/eskf_python/eskf_python/eskf_python_class.py create mode 100644 navigation/eskf_python/eskf_python/eskf_python_utils.py rename navigation/{ukf_okid/ukf_python/ukf_okid_class copy.py => eskf_python/eskf_python/ukf_okid_class.py} (78%) delete mode 100644 navigation/sp_ukf_python/CMakeLists.txt delete mode 100644 navigation/sp_ukf_python/README.md delete mode 100644 navigation/sp_ukf_python/config/sp_ukf_python.yaml delete mode 100644 navigation/sp_ukf_python/launch/ukf.launch.py delete mode 100644 navigation/sp_ukf_python/package.xml delete mode 100644 navigation/sp_ukf_python/sp_ukf_python/__init__.py delete mode 100644 navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python.py delete mode 100644 navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_class.py delete mode 100644 navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_node.py delete mode 100644 navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_utils.py delete mode 100644 navigation/sp_ukf_python/sp_ukf_python/test_ukf.py diff --git a/navigation/eskf_python/eskf_python/eskf_python_class.py b/navigation/eskf_python/eskf_python/eskf_python_class.py new file mode 100644 index 000000000..949ab996d --- /dev/null +++ b/navigation/eskf_python/eskf_python/eskf_python_class.py @@ -0,0 +1,215 @@ +from dataclasses import dataclass, field +from typing import Tuple, List +from scipy.linalg import expm +import numpy as np +from eskf_python_utils import skew_matrix, quaternion_product + + +@dataclass +class StateQuat: + position: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Position vector (x, y, z) + velocity: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Velocity vector (u, v, w) + orientation: np.ndarray = field( + default_factory=lambda: np.array([1, 0, 0, 0]) + ) # Orientation quaternion (w, x, y, z) + acceleration_bias: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Acceleration bias vector (b_ax, b_ay, b_az) + gyro_bias: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Gyro bias vector (b_gx, b_gy, b_gz) + g: np.ndarray = field( + default_factory=lambda: np.array([0, 0, 0]) + ) # Gravity vector + + def as_vector(self) -> np.ndarray: + """Returns the state vector as a numpy array. + + Returns: + np.ndarray: The state vector. + """ + return np.concatenate( + [ + self.position, + self.velocity, + self.orientation, + self.acceleration_bias, + self.gyro_bias, + self.g, + ] + ) + + def fill_states(self, state: np.ndarray) -> None: + """Fills the state vector with the values from a numpy array. + + Args: + state (np.ndarray): The state vector. + """ + self.position = state[0:3] + self.velocity = state[3:6] + self.orientation = state[6:10] + self.acceleration_bias = state[10:13] + self.gyro_bias = state[13:16] + self.g = state[16:19] + + def R_q(self) -> np.ndarray: + """Calculates the rotation matrix from the orientation quaternion. + + Returns: + np.ndarray: The rotation matrix. + """ + q0, q1, q2, q3 = self.orientation + R = np.array( + [ + [ + 1 - 2 * q2**2 - 2 * q3**2, + 2 * (q1 * q2 - q0 * q3), + 2 * (q0 * q2 + q1 * q3), + ], + [ + 2 * (q1 * q2 + q0 * q3), + 1 - 2 * q1**2 - 2 * q3**2, + 2 * (q2 * q3 - q0 * q1), + ], + [ + 2 * (q1 * q3 - q0 * q2), + 2 * (q0 * q1 + q2 * q3), + 1 - 2 * q1**2 - 2 * q2**2, + ], + ] + ) + + return R + + # def inject(self, EulerState: 'StateEuler') -> 'StateQuat': + # inj_state = StateQuat() + + # # Injecting the error state + # inj_state.position = self.position + EulerState.position + # inj_state.velocity = self.velocity + EulerState.velocity + # inj_state.orientation = quaternion_product( + # self.orientation, + # 0.5 + # * np.array( + # [ + # 2, + # EulerState.orientation[0], + # EulerState.orientation[1], + # EulerState.orientation[2], + # ] + # ), + # ) + # inj_state.acceleration_bias = self.acceleration_bias + EulerState.acceleration_bias + # inj_state.gyro_bias = self.gyro_bias + EulerState.gyro_bias + + # return inj_state + + + +@dataclass +class StateEuler: + position: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Position vector (x, y, z) + velocity: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Velocity vector (u, v, w) + orientation: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Orientation angles (roll, pitch, yaw) + acceleration_bias: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Acceleration bias vector (b_ax, b_ay, b_az) + gyro_bias: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) # Gyro bias vector (b_gx, b_gy, b_gz) + g: np.ndarray = field( + default_factory=lambda: np.array([0, 0, 9.81]) + ) # Gravity vector + covariance: np.ndarray = field( + default_factory=lambda: np.zeros((18, 18)) + ) # Covariance matrix + + def as_vector(self) -> np.ndarray: + """Returns the state vector as a numpy array. + + Returns: + np.ndarray: The state vector. + """ + return np.concatenate( + [ + self.position, + self.velocity, + self.orientation, + self.acceleration_bias, + self.gyro_bias, + self.g, + ] + ) + + def fill_states(self, state: np.ndarray) -> None: + """Fills the state vector with the values from a numpy array. + + Args: + state (np.ndarray): The state vector. + """ + self.position = state[0:3] + self.velocity = state[3:6] + self.orientation = state[6:9] + self.acceleration_bias = state[9:12] + self.gyro_bias = state[12:15] + self.g = state[15:18] + + def copy_state(self, wanted_state: 'StateEuler') -> None: + """Copies the state from a StateVector object into the current StateVector object. + + Args: + wanted_state (StateVector_euler): The quaternion state to copy from. + """ + self.position = wanted_state.position + self.velocity = wanted_state.velocity + self.orientation = wanted_state.orientation + self.acceleration_bias = wanted_state.acceleration_bias + self.gyro_bias = wanted_state.gyro_bias + + +@dataclass +class MeasurementModel: + measurement: np.ndarray = field( + default_factory=lambda: np.zeros(6) + ) + measurement_covariance: np.ndarray = field( + default_factory=lambda: np.zeros((6, 6)) + ) + + def H(self) -> np.ndarray: + """Calculates the measurement matrix. + + Returns: + np.ndarray: The measurement matrix. + """ + H = np.zeros((3, 15)) + + H[0:3, 3:6] = np.eye(3) + + return H + +@dataclass +class Measurement: + acceleration: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) + angular_velocity: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) + aiding: np.ndarray = field( + default_factory=lambda: np.zeros(3) + ) + + aiding_covariance: np.ndarray = field( + default_factory=lambda: np.zeros((3, 3)) + ) \ No newline at end of file diff --git a/navigation/eskf_python/eskf_python/eskf_python_filter.py b/navigation/eskf_python/eskf_python/eskf_python_filter.py index edf39dcc0..a9c772e47 100644 --- a/navigation/eskf_python/eskf_python/eskf_python_filter.py +++ b/navigation/eskf_python/eskf_python/eskf_python_filter.py @@ -1,503 +1,249 @@ -from dataclasses import dataclass, field -from typing import tuple +# from dataclasses import dataclass +from typing import Tuple import numpy as np from scipy.linalg import expm +from eskf_python_class import StateEuler, StateQuat, Measurement +from eskf_python_utils import skew_matrix, quaternion_product, R_from_angle_axis, angle_axis_to_quaternion +from ukf_okid_class import euler_to_quat +from scipy.linalg import block_diag + +class ESKF: + def __init__(self, Q: np.ndarray, P0, Hx, nom_state: StateQuat, p_accBias, p_gyroBias, dt): + self.Q = Q + self.Hx = Hx # Jacobian of the measurement model + self.dt = dt + self.nom_state = nom_state + self.error_state = StateEuler() + self.error_state.covariance = P0 + self.p_accBias = p_accBias + self.p_gyroBias = p_gyroBias + + def Fx(self, imu_data: Measurement) -> np.ndarray: + """Calculates the state transition matrix. -@dataclass -class StateVector_quaternion: - position: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Position vector (x, y, z) - velocity: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Velocity vector (u, v, w) - orientation: np.ndarray = field( - default_factory=lambda: np.zeros(4) - ) # Orientation quaternion (w, x, y, z) - acceleration_bias: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Acceleration bias vector (b_ax, b_ay, b_az) - gyro_bias: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Gyro bias vector (b_gx, b_gy, b_gz) - - def R_q(self) -> np.ndarray: - """Calculates the rotation matrix from the orientation quaternion. + Args: + imu_data (np.ndarray): The IMU data. Returns: - np.ndarray: The rotation matrix. + np.ndarray: The state transition matrix. """ - q0, q1, q2, q3 = self.orientation - R = np.array( - [ - [ - 1 - 2 * q2**2 - 2 * q3**2, - 2 * (q1 * q2 - q0 * q3), - 2 * (q0 * q2 + q1 * q3), - ], - [ - 2 * (q1 * q2 + q0 * q3), - 1 - 2 * q1**2 - 2 * q3**2, - 2 * (q2 * q3 - q0 * q1), - ], - [ - 2 * (q1 * q3 - q0 * q2), - 2 * (q0 * q1 + q2 * q3), - 1 - 2 * q1**2 - 2 * q2**2, - ], - ] - ) - return R + F_x = np.zeros((18, 18)) + I = np.eye(3) + + F_x[0:3, 0:3] = I + F_x[0:3, 3:6] = self.dt * I + F_x[3:6, 3:6] = I + F_x[3:6, 6:9] = -self.nom_state.R_q() @ skew_matrix(imu_data.acceleration - self.nom_state.acceleration_bias) * self.dt + F_x[6:9, 6:9] = R_from_angle_axis((imu_data.angular_velocity - self.nom_state.gyro_bias) * self.dt).T + F_x[3:6, 9:12] = -self.nom_state.R_q() * self.dt + F_x[3:6, 15:18] = I * self.dt + F_x[6:9, 12:15] = -I * self.dt + F_x[9:12, 9:12] = I + F_x[12:15, 12:15] = I + F_x[15:18, 15:18] = I + + return F_x + + def Fi(self) -> np.ndarray: + """Calculates the input matrix. + + Returns: + np.ndarray: The input matrix. + """ - def euler_forward( - self, current_state: 'StateVector_quaternion', dt: float - ) -> 'StateVector_quaternion': - # Define the new state - new_state = StateVector_quaternion() + F_i = np.zeros((18, 12)) + I = np.eye(3) - # Define the state derivatives - new_state.position = current_state.position + self.position * dt - new_state.velocity = current_state.velocity + self.velocity * dt - new_state.orientation = current_state.orientation + self.orientation * dt - new_state.acceleration_bias = ( - current_state.acceleration_bias + self.acceleration_bias * dt - ) - new_state.gyro_bias = current_state.gyro_bias + self.gyro_bias * dt - - # Normalize the orientation quaternion - new_state.orientation /= np.linalg.norm(new_state.orientation) - - return new_state - - -@dataclass -class StateVector_euler: - position: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Position vector (x, y, z) - velocity: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Velocity vector (u, v, w) - orientation: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Orientation angles (roll, pitch, yaw) - acceleration_bias: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Acceleration bias vector (b_ax, b_ay, b_az) - gyro_bias: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Gyro bias vector (b_gx, b_gy, b_gz) - covariance: np.ndarray = field( - default_factory=lambda: np.zeros((15, 15)) - ) # Covariance matrix - - def fill_states(self, state: np.ndarray) -> None: - """Fills the state vector with the values from a numpy array. + F_i[3:6, 0:3] = I + F_i[6:9, 3:6] = I + F_i[9:12, 6:9] = I + F_i[12:15, 9:12] = I - Args: - state (np.ndarray): The state vector. + return F_i + + def Q_delta_theta(self) -> np.ndarray: + """ + Calculates the Q_delta_theta matrix. + See Joan Solà. Quaternion kinematics for the error-state Kalman filter. + chapter: 6.1.1 eq. 281 """ - self.position = state[0:3] - self.velocity = state[3:6] - self.orientation = state[6:9] - self.acceleration_bias = state[9:12] - self.gyro_bias = state[12:15] - def copy_state(self, wanted_state: 'StateVector_euler') -> None: - """Copies the state from a StateVector object into the current StateVector object. + qw, qx, qy, qz = self.nom_state.orientation - Args: - wanted_state (StateVector_euler): The quaternion state to copy from. - """ - self.position = wanted_state.position - self.velocity = wanted_state.velocity - self.orientation = wanted_state.orientation - self.acceleration_bias = wanted_state.acceleration_bias - self.gyro_bias = wanted_state.gyro_bias - - -@dataclass -class MeasurementModel: - measurement: np.ndarray = field( - default_factory=lambda: np.zeros(6) - ) # Measurement vector - measurement_matrix: np.ndarray = field( - default_factory=lambda: np.zeros((6, 15)) - ) # Measurement matrix - measurement_covariance: np.ndarray = field( - default_factory=lambda: np.zeros((6, 6)) - ) # Measurement noise matrix - - -class ErrorStateKalmanFilter: - def __init__( - self, - P_ab: np.ndarray, - P_wb: np.ndarray, - Q: np.ndarray, - lever_arm: np.array, - R: np.ndarray, - g: float, - dt: float, - ) -> None: - self.P_ab = P_ab - self.P_wb = P_wb - self.Q_process_noise = Q - self.lever_arm = lever_arm - self.R = R - self.g = np.array([0, 0, g]) - self.dt = dt + Q_delta_theta = 0.5 * np.array([ + [-qx, -qy, -qz], + [qw, -qz, qy], + [qz, qw, -qx], + [-qy, qx, qw], + ]) - def skew_symmetric(self, vector: np.ndarray) -> np.ndarray: - """Calculates the skew symmetric matrix of a vector. + return Q_delta_theta - Args: - vector (np.ndarray): The vector. + def H(self) -> np.ndarray: + """Calculates the measurement matrix. Returns: - np.ndarray: The skew symmetric matrix. + np.ndarray: The measurement matrix. """ - return np.array( - [ - [0, -vector[2], vector[1]], - [vector[2], 0, -vector[0]], - [-vector[1], vector[0], 0], - ] - ) - def quaternion_super_product(self, q1: np.ndarray, q2: np.ndarray) -> np.ndarray: - """Calculates the quaternion super product of two quaternions. + X_deltax = block_diag(np.eye(6), self.Q_delta_theta(), np.eye(9)) - Args: - q1 (np.ndarray): The first quaternion. - q2 (np.ndarray): The second quaternion. + H = self.Hx @ X_deltax + + return H + + def h(self) -> np.ndarray: + """ + Calculates the measurement model. Returns: - np.ndarray: The quaternion super product. + np.ndarray: The measurement model. """ - nu_0, eta_0_x, eta_0_y, eta_0_z = q1 - nu_1, eta_1_x, eta_1_y, eta_1_z = q2 + return self.nom_state.velocity - eta_0 = np.array([[eta_1_x, eta_1_y, eta_1_z]]).T - eta_1 = np.array([[eta_0_x, eta_0_y, eta_0_z]]).T - eta_new = ( - nu_1 * eta_0 + nu_0 * eta_1 + np.dot(self.skew_symmetric(eta_0), eta_1) - ) - nu_new = nu_0 * nu_1 - np.dot(eta_0.T, eta_1) - q_new = np.array([nu_new, eta_new[0], eta_new[1], eta_new[2]]) - q_new /= np.linalg.norm(q_new) + def nominal_state_discrete(self, imu_data: Measurement) -> None: + """ + Calculates the next nominal state using the discrete-time process model defined in: + Joan Solà. Quaternion kinematics for the error-state Kalman filter. + Chapter: 5.4.1 The nominal state kinematics - return q_new + Args: + imu_data (np.ndarray): The IMU data. + """ - def van_loan_discretization( - self, A_c: np.ndarray, G_c: np.ndarray - ) -> tuple[np.ndarray, np.ndarray]: - """Calculates the Van Loan discretization of a continuous-time system. + # Rectify measurements. + acc_rect = imu_data.acceleration - self.nom_state.acceleration_bias + gyro_rect = imu_data.angular_velocity - self.nom_state.gyro_bias + + R = self.nom_state.R_q() + + self.nom_state.position = self.nom_state.position + self.nom_state.velocity * self.dt + 0.5 * (R @ acc_rect + self.nom_state.g) * self.dt**2 + self.nom_state.velocity = self.nom_state.velocity + (R @ acc_rect + self.nom_state.g) * self.dt + self.nom_state.orientation = quaternion_product(self.nom_state.orientation, angle_axis_to_quaternion(gyro_rect * self.dt)) + self.nom_state.acceleration_bias = np.exp(-self.p_accBias * self.dt) * self.nom_state.acceleration_bias + self.nom_state.gyro_bias = np.exp(-self.p_gyroBias * self.dt) * self.nom_state.gyro_bias + self.nom_state.g = self.nom_state.g + + def van_loan_discretization(self, A_c, G_c) -> Tuple[np.ndarray, np.ndarray]: + """ + Calculates the Van Loan discretization of a continuous-time system. Args: A_c (np.ndarray): The A matrix. G_c (np.ndarray): The G matrix. Returns: - tuple: The A_d and GQG_d matrices. + Tuple: The A_d and GQG_d matrices. """ - GQG_T = np.dot(np.dot(G_c, self.Q_process_noise), G_c.T) * self.dt + + GQG_T = np.dot(np.dot(G_c, self.Q), G_c.T) matrix_exp = ( - np.block([[A_c, GQG_T], [np.zeros((A_c.shape[0], A_c.shape[0])), A_c.T]]) + np.block([[- A_c, GQG_T], [np.zeros((A_c.shape[0], A_c.shape[0])), np.transpose(A_c)]]) * self.dt ) van_loan_matrix = expm(matrix_exp) - V1 = van_loan_matrix[A_c.shape[0] :, A_c.shape[0] :] - V2 = van_loan_matrix[: A_c.shape[0], A_c.shape[0] :] + V1 = van_loan_matrix[A_c.shape[0]:, A_c.shape[0]:] + V2 = van_loan_matrix[:A_c.shape[0], A_c.shape[0]:] A_d = V1.T GQG_d = A_d @ V2 return A_d, GQG_d - def nominal_state_update( - self, current_state: StateVector_quaternion, imu_reading: np.ndarray - ) -> StateVector_quaternion: - """Updates the nominal state of the system. + def error_state_prediction(self, imu_data: Measurement) -> None: - Args: - current_state (np.ndarray): The current state of the system. - imu_reading (np.ndarray): The IMU reading. + # Rectify measurements. + acc_rect = imu_data.acceleration - self.nom_state.acceleration_bias + gyro_rect = imu_data.angular_velocity - self.nom_state.gyro_bias - Returns: - np.ndarray: The updated nominal state. - """ - # Defining the IMU readings - imu_acceleration = imu_reading[0:3] - imu_gyro = imu_reading[3:6] - - # Define the derivative of the state - current_state_dot = StateVector_quaternion() - - # Define the state derivates - current_state_dot.position = current_state.velocity - current_state_dot.velocity = ( - np.dot( - current_state.R_q(), - (imu_acceleration - current_state.acceleration_bias), - ) - + self.g - ) + R = self.nom_state.R_q() - # Define the quaternion derivatives - current_state_dot.orientation = 0.5 * self.quaternion_super_product( - current_state.orientation, - np.array([0, imu_gyro[0], imu_gyro[1], imu_gyro[2]]), - ) + A_c = np.zeros((18, 18)) - # Define the bias - current_state_dot.acceleration_bias = ( - -np.dot(self.P_ab, np.eye(3)) @ current_state.acceleration_bias - ) - current_state_dot.gyro_bias = ( - -np.dot(self.P_wb, np.eye(3)) @ current_state.gyro_bias - ) - - return current_state_dot.euler_forward(current_state, self.dt) - - def error_state_update( - self, - current_error_state: StateVector_euler, - current_state: StateVector_quaternion, - imu_reading: np.ndarray, - ) -> StateVector_euler: - """Updates the error state of the system. - - Args: - current_error_state (np.ndarray): The current error state of the system. - current_state (np.ndarray): The current state of the system. - imu_reading (np.ndarray): The IMU reading. - - Returns: - np.ndarray: The updated error state. - """ - # Define the derivative of the state - next_error_state = StateVector_euler() - - # Defining the IMU readings - imu_acceleration = imu_reading[0:3] - imu_gyro = imu_reading[3:6] - - A_c = np.zeros((15, 15)) A_c[0:3, 3:6] = np.eye(3) - A_c[3:6, 6:9] = -np.dot( - current_state.R_q(), - self.skew_symmetric(imu_acceleration - current_state.acceleration_bias), - ) - A_c[6:9, 6:9] = -self.skew_symmetric(imu_gyro - current_state.gyro_bias) - A_c[3:6, 9:12] = -current_state.R_q() + A_c[3:6, 6:9] = - R @ skew_matrix(acc_rect) + A_c[6:9, 6:9] = - skew_matrix(gyro_rect) + A_c[3:6, 9:12] = - R + A_c[9:12, 9:12] = -self.p_accBias * np.eye(3) + A_c[12:15, 12:15] = -self.p_gyroBias * np.eye(3) A_c[6:9, 12:15] = -np.eye(3) - A_c[9:12, 9:12] = -self.P_ab * np.eye(3) - A_c[12:15, 12:15] = -self.P_wb * np.eye(3) + A_c[3:6, 15:18] = np.eye(3) + + G_c = np.zeros((18, 12)) - G_c = np.zeros((15, 12)) - G_c[3:6, 0:3] = -current_state.R_q() + G_c[3:6, 0:3] = -R G_c[6:9, 3:6] = -np.eye(3) G_c[9:12, 6:9] = np.eye(3) G_c[12:15, 9:12] = np.eye(3) - # Van loan discretization - A_d, GQG_d = self.van_loan_discretization(A_c, G_c, self.dt) + A_d, GQG_d = self.van_loan_discretization(A_c, G_c) - # Inserting the new state and covariance - next_error_state.copy_state(current_error_state) - next_error_state.covariance = ( - np.dot(np.dot(A_d, current_error_state.covariance), A_d.T) + GQG_d - ) - - return next_error_state - - def H(self) -> np.ndarray: - """Calculates the measurement matrix. + self.error_state.covariance = (A_d @ self.error_state.covariance @ A_d.T + GQG_d) - Returns: - np.ndarray: The measurement matrix. + def measurement_update(self, dvl_measurement:Measurement) -> None: """ - # Define the measurement matrix - H = np.zeros((3, 15)) - - # For now assume only velocity is measured - H[0:3, 3:6] = np.eye(3) - - return H - - def prediction_from_estimates( - self, - current_state: StateVector_quaternion, - current_error_state: StateVector_euler, - imu_reading: np.ndarray, - ) -> StateVector_euler: - """Predicts the measurement from the current state and error state. + Updates the error state using the DVL measurement. + Joan Solà. Quaternion kinematics for the error-state Kalman filter. + Chapter: 6.1 eq. 274-276 Args: - current_state (StateVector_quaternion): The current state of the system. - current_error_state (StateVector_euler): The current error state of the system. - imu_reading (np.ndarray): The IMU reading. - - Returns: - StateVector_euler: The predicted measurement. + dvl_measurement (np.ndarray): The DVL measurement. """ - # Define the z_pred matrix - z_pred = MeasurementModel() - - # Define the z_pred values separately - z_pred_1 = current_state.velocity - z_pred_2 = 0 # Currently assuming no lever arm compensation - - # Combine the z_pred values - z_pred.measurement = z_pred_1 + z_pred_2 - - # Define the H matrix - z_pred.measurement_matrix = self.H() - R = self.R - z_pred.measurement_covariance = ( - np.dot( - np.dot(z_pred.measurement_matrix, current_error_state.covariance), - z_pred.measurement_matrix.T, - ) - + R - ) - - return z_pred - def measurement_update( - self, - error_state_pred: StateVector_euler, - z_pred: MeasurementModel, - dvl_measure: np.array, - ) -> StateVector_euler: - """Updates the error state of the system. + H = self.H() + P = self.error_state.covariance + R= dvl_measurement.aiding_covariance + K = P @ H.T @ np.linalg.inv(H @ P @ H.T + R) + self.error_state.fill_states(K @ (dvl_measurement.aiding - self.h())) + self.error_state.covariance = (np.eye(18) - K @ H) @ P - Args: - current_error_state (np.ndarray): The current error state of the system. - measurement (np.ndarray): The measurement. - - Returns: - np.ndarray: The updated error state. + def injection(self) -> None: """ - # Define new error state value - new_error_state = StateVector_euler() - - # Define the measurement matrix - innovation = dvl_measure - z_pred.measurement - H = z_pred.measurement_matrix - R = self.R - P = error_state_pred.covariance - S = z_pred.measurement_covariance - - # Kalman gain calculation - W = np.dot(P, np.linalg.solve(S, H).T) - new_error_state.fill_states(np.dot(W, innovation)) - - I_WH = np.eye(15) - np.dot(W, H) - new_error_state.covariance = np.dot(np.dot(I_WH, P), I_WH.T) + np.dot( - np.dot(W, R), W.T - ) - - return new_error_state - - def imu_update_states( - self, - current_pred_nom: StateVector_quaternion, - current_pred_err: StateVector_euler, - imu_readings: np.array, - ) -> tuple[StateVector_quaternion, StateVector_euler]: - """Calculates the predicted state using the IMU readings. - - Args: - current_pred_nom (StateVector_quaternion): The current nominal state. - current_pred_err (StateVector_euler): The current error state. - imu_readings (np.array): The IMU readings. - - Returns: - tuple: The predicted nominal state and the predicted error state. + Injects the error state into the nominal state to produce the estimated state. + Joan Solà. Quaternion kinematics for the error-state Kalman filter. + Chapter 6.2 eq. 282-283 + """ - pred_nom_state = self.nominal_state_update(current_pred_nom, imu_readings) - pred_err_state = self.error_state_update( - current_pred_err, current_pred_nom, imu_readings - ) - - return pred_nom_state, pred_err_state - - def dvl_update_states( - self, - current_pred_nom: StateVector_quaternion, - current_pred_err: StateVector_euler, - dvl_measure: np.array, - ) -> tuple[StateVector_quaternion, StateVector_euler]: - """Calculates the predicted state using the DVL readings. - - Args: - current_pred_nom (StateVector_quaternion): The current nominal state. - current_pred_err (StateVector_euler): The current error state. - dvl_measure (np.array): The DVL readings. - - Returns: - tuple: The predicted nominal state and the predicted error state. + + self.nom_state.position = self.nom_state.position + self.error_state.position + self.nom_state.velocity = self.nom_state.velocity + self.error_state.velocity + self.nom_state.orientation = quaternion_product(self.nom_state.orientation, euler_to_quat(self.error_state.orientation)) + self.nom_state.acceleration_bias = self.nom_state.acceleration_bias + self.error_state.acceleration_bias + self.nom_state.gyro_bias = self.nom_state.gyro_bias + self.error_state.gyro_bias + self.nom_state.g = self.nom_state.g + self.error_state.g + + def reset_error_state(self) -> None: + """ + Resets the error state after injection. + Joan Solà. Quaternion kinematics for the error-state Kalman filter. + Chapter 6.3 eq. 284-286 """ - z_pred = self.prediction_from_estimates( - current_pred_nom, current_pred_err, dvl_measure - ) - new_error_state = self.measurement_update(current_pred_err, z_pred, dvl_measure) - return current_pred_nom, new_error_state + G = np.eye(18) # Neglecting the delta_theta as this is most common in practice - def injection_and_reset( - self, next_state: StateVector_quaternion, next_error_state: StateVector_euler - ) -> tuple[StateVector_quaternion, StateVector_euler]: - """Injects the error state into the nominal state and resets the error state. + self.error_state.covariance = G @ self.error_state.covariance @ G.T + self.error_state.fill_states(np.zeros(18)) - Args: - next_state (StateVector_quaternion): The next nominal state. - next_error_state (StateVector_euler): The next error state. - - Returns: - tuple: The injected nominal state and the reset error state. + def imu_update(self, imu_data: Measurement) -> None: + """ + Updates the state using the IMU data. """ - # Define the new state - inj_state = StateVector_quaternion() - - # Injecting the error state - inj_state.position = next_state.position + next_error_state.position - inj_state.velocity = next_state.velocity + next_error_state.velocity - inj_state.orientation = self.quaternion_super_product( - next_state.orientation, - 0.5 - * np.array( - [ - 2, - next_error_state.orientation[0], - next_error_state.orientation[1], - next_error_state.orientation[2], - ] - ), - ) - inj_state.acceleration_bias = ( - next_state.acceleration_bias + next_error_state.acceleration_bias - ) - inj_state.gyro_bias = next_state.gyro_bias + next_error_state.gyro_bias - - # Resetting the error state - G = np.eye(15) - G[6:9, 6:9] = np.eye(3) - self.skew_symmetric( - 0.5 * next_error_state.orientation - ) - - next_error_state.covariance = np.dot( - np.dot(G, next_error_state.covariance), G.T - ) - next_error_state.fill_states(np.zeros(15)) - return inj_state, next_error_state + self.nominal_state_discrete(imu_data) + self.error_state_prediction(imu_data) + + def dvl_update(self, dvl_measurement: Measurement) -> None: + """ + Updates the state using the DVL measurement. + """ + + self.measurement_update(dvl_measurement) + self.injection() + self.reset_error_state() \ No newline at end of file diff --git a/navigation/eskf_python/eskf_python/eskf_python_node.py b/navigation/eskf_python/eskf_python/eskf_python_node.py index 5b860582e..ec206ab64 100644 --- a/navigation/eskf_python/eskf_python/eskf_python_node.py +++ b/navigation/eskf_python/eskf_python/eskf_python_node.py @@ -4,7 +4,7 @@ from nav_msgs.msg import Odometry from rclpy.node import Node from rclpy.qos import QoSProfile, qos_profile_sensor_data -from sensor_msgs.msg import Imu, +from sensor_msgs.msg import Imu import numpy as np from geometry_msgs.msg import TwistWithCovarianceStamped diff --git a/navigation/eskf_python/eskf_python/eskf_python_utils.py b/navigation/eskf_python/eskf_python/eskf_python_utils.py new file mode 100644 index 000000000..9ea439982 --- /dev/null +++ b/navigation/eskf_python/eskf_python/eskf_python_utils.py @@ -0,0 +1,95 @@ +import numpy as np + +def skew_matrix(vector: np.ndarray) -> np.ndarray: + """ + Returns the skew symmetric matrix of a 3x1 vector. + """ + return np.array( + [ + [0, -vector[2], vector[1]], + [vector[2], 0, -vector[0]], + [-vector[1], vector[0], 0] + ] + ) + +def quaternion_product(q1: np.ndarray, q2: np.ndarray) -> np.ndarray: + """Calculates the quaternion super product of two quaternions. + + Args: + q1 (np.ndarray): The first quaternion. + q2 (np.ndarray): The second quaternion. + + Returns: + np.ndarray: The quaternion super product. + """ + + eta_0, e_0_x, e_0_y, e_0_z = q1 + eta_1, e_1_x, e_1_y, e_1_z = q2 + + e_0 = np.array([e_0_x, e_0_y, e_0_z]) + e_1 = np.array([e_1_x, e_1_y, e_1_z]) + + eta_new = eta_0 * eta_1 - np.dot(e_0, e_1) + nu_new = e_1 * eta_0 + e_0 * eta_1 + np.cross(e_0, e_1) + + q_new = np.array([eta_new, nu_new[0], nu_new[1], nu_new[2]]) + q_new = q_new / np.linalg.norm(q_new) + + return q_new + +def angle_axis_to_quaternion(vector: np.ndarray) -> np.ndarray: + """Converts an angle-axis representation to a quaternion. + + Args: + vector (np.ndarray): The angle-axis representation. + + Returns: + np.ndarray: The quaternion representation. + """ + angle = np.linalg.norm(vector) + if angle < 1e-8: + return np.array([1, 0, 0, 0]) + else: + axis = vector / angle + + + q = np.zeros(4) + q[0] = np.cos(angle / 2) + q[1:] = np.sin(angle / 2) * axis + + return q + + +def R_from_angle_axis(vector: np.ndarray) -> np.ndarray: + """Calculates the rotation matrix from the angle-axis representation. + + Args: + vector (np.ndarray): The angle-axis representation. + + Returns: + np.ndarray: The rotation matrix. + """ + quaternion = angle_axis_to_quaternion(vector) + q0, q1, q2, q3 = quaternion + + R = np.array( + [ + [ + 1 - 2 * q2**2 - 2 * q3**2, + 2 * (q1 * q2 - q0 * q3), + 2 * (q0 * q2 + q1 * q3), + ], + [ + 2 * (q1 * q2 + q0 * q3), + 1 - 2 * q1**2 - 2 * q3**2, + 2 * (q2 * q3 - q0 * q1), + ], + [ + 2 * (q1 * q3 - q0 * q2), + 2 * (q0 * q1 + q2 * q3), + 1 - 2 * q1**2 - 2 * q2**2, + ], + ] + ) + + return R diff --git a/navigation/ukf_okid/ukf_python/ukf_okid_class copy.py b/navigation/eskf_python/eskf_python/ukf_okid_class.py similarity index 78% rename from navigation/ukf_okid/ukf_python/ukf_okid_class copy.py rename to navigation/eskf_python/eskf_python/ukf_okid_class.py index 86b7beb49..8444fd82e 100644 --- a/navigation/ukf_okid/ukf_python/ukf_okid_class copy.py +++ b/navigation/eskf_python/eskf_python/ukf_okid_class.py @@ -14,12 +14,11 @@ class StateQuat: orientation: np.ndarray = field(default_factory=lambda: np.array([1, 0, 0, 0])) velocity: np.ndarray = field(default_factory=lambda: np.zeros(3)) angular_velocity: np.ndarray = field(default_factory=lambda: np.zeros(3)) - okid_params: np.ndarray = field(default_factory=lambda: np.zeros(21)) - covariance: np.ndarray = field(default_factory=lambda: np.zeros((33, 33))) + covariance: np.ndarray = field(default_factory=lambda: np.zeros((12, 12))) def as_vector(self) -> np.ndarray: """Returns the StateVector as a numpy array.""" - return np.concatenate([self.position, self.orientation, self.velocity, self.angular_velocity, self.okid_params]) + return np.concatenate([self.position, self.orientation, self.velocity, self.angular_velocity]) def nu(self) -> np.ndarray: """Calculates the nu vector.""" @@ -42,9 +41,6 @@ def fill_states(self, state: np.ndarray) -> None: self.velocity = state[7:10] self.angular_velocity = state[10:13] - if len(state) > 13: - self.okid_params = state[13:] - def fill_states_different_dim(self, state: np.ndarray, state_euler: np.ndarray) -> None: """Fills states when the state vector has different dimensions than the default state vector.""" self.position = state[0:3] + state_euler[0:3] @@ -52,9 +48,6 @@ def fill_states_different_dim(self, state: np.ndarray, state_euler: np.ndarray) self.velocity = state[7:10] + state_euler[6:9] self.angular_velocity = state[10:13] + state_euler[9:12] - if len(state) > 13: - self.okid_params = state[13:] - def subtract(self, other: 'StateQuat') -> np.ndarray: """Subtracts two StateQuat objects, returning the difference with Euler angles.""" new_array = np.zeros(len(self.as_vector()) - 1) @@ -63,8 +56,6 @@ def subtract(self, other: 'StateQuat') -> np.ndarray: new_array[6:9] = self.velocity - other.velocity new_array[9:12] = self.angular_velocity - other.angular_velocity - new_array[12:] = self.okid_params - other.okid_params - return new_array def __add__(self, other: 'StateQuat') -> 'StateQuat': @@ -75,8 +66,6 @@ def __add__(self, other: 'StateQuat') -> 'StateQuat': new_state.velocity = self.velocity + other.velocity new_state.angular_velocity = self.angular_velocity + other.angular_velocity - new_state.okid_params = self.okid_params + other.okid_params - return new_state def __sub__(self, other: 'StateQuat') -> 'StateQuat': @@ -87,8 +76,6 @@ def __sub__(self, other: 'StateQuat') -> 'StateQuat': new_state.velocity = self.velocity - other.velocity new_state.angular_velocity = self.angular_velocity - other.angular_velocity - new_state.okid_params = self.okid_params - other.okid_params - return new_state.as_vector() def __rmul__(self, scalar: float) -> 'StateQuat': @@ -99,8 +86,6 @@ def __rmul__(self, scalar: float) -> 'StateQuat': new_state.velocity = scalar * self.velocity new_state.angular_velocity = scalar * self.angular_velocity - new_state.okid_params = scalar * self.okid_params - return new_state def insert_weights(self, weights: np.ndarray) -> np.ndarray: @@ -110,7 +95,6 @@ def insert_weights(self, weights: np.ndarray) -> np.ndarray: new_state.orientation = quaternion_error(self.orientation, euler_to_quat(weights[3:6])) new_state.velocity = self.velocity - weights[6:9] new_state.angular_velocity = self.angular_velocity - weights[9:12] - new_state.okid_params = self.okid_params - weights[12:] return new_state.as_vector() @@ -119,7 +103,6 @@ def add_without_quaternions(self, other: 'StateQuat') -> None: self.position += other.position self.velocity += other.velocity self.angular_velocity += other.angular_velocity - self.okid_params += other.okid_params @dataclass class MeasModel: @@ -131,7 +114,7 @@ class MeasModel: def H(self, state: StateQuat) -> 'MeasModel': """Calculates the measurement matrix.""" - H = np.zeros((3, 34)) + H = np.zeros((3, 13)) H[:, 7:10] = np.eye(3) z_i = MeasModel() z_i.measurement = np.dot(H, state.as_vector()) @@ -233,93 +216,6 @@ def euler_forward(self) -> StateQuat: self.state_vector.angular_velocity = self.state_vector_prev.angular_velocity + self.state_vector_dot.angular_velocity * self.dt return self.state_vector -@dataclass -class okid_model: - """ - A class defined for a general process model. - """ - state_vector: StateQuat = field(default_factory=StateQuat) - state_vector_dot: StateQuat = field(default_factory=StateQuat) - state_vector_prev: StateQuat = field(default_factory=StateQuat) - Control_input: np.ndarray = field(default_factory=lambda: np.zeros(6)) - mass_interia_matrix: np.ndarray = field(default_factory=lambda: np.zeros((6, 6))) - m: float = 0.0 - inertia: np.ndarray = field(default_factory=lambda: np.zeros((3,3))) - r_b_bg: np.ndarray = field(default_factory=lambda: np.zeros(3)) - dt: float = 0.0 - prev_position_error: np.ndarray = field(default_factory=lambda: np.zeros(3)) - prev_orientation_error: np.ndarray = field(default_factory=lambda: np.zeros(3)) - D_matrix: np.ndarray = field(default_factory=lambda: np.zeros((6, 6))) - added_mass: np.ndarray = field(default_factory=lambda: np.zeros(6)) - - def R(self) -> np.ndarray: - """Calculates the rotation matrix.""" - nu, e_1, e_2, e_3 = self.state_vector.orientation - R = np.array([ - [1 - 2 * e_2 ** 2 - 2 * e_3 ** 2, 2 * e_1 * e_2 - 2 * nu * e_3, 2 * e_1 * e_3 + 2 * nu * e_2], - [2 * e_1 * e_2 + 2 * nu * e_3, 1 - 2 * e_1 ** 2 - 2 * e_3 ** 2, 2 * e_2 * e_3 - 2 * nu * e_1], - [2 * e_1 * e_3 - 2 * nu * e_2, 2 * e_2 * e_3 + 2 * nu * e_1, 1 - 2 * e_1 ** 2 - 2 * e_2 ** 2] - ]) - return R - - def T(self) -> np.ndarray: - """Calculates the transformation matrix.""" - nu, e_1, e_2, e_3 = self.state_vector.orientation - T = 0.5 * np.array([ - [-e_1, -e_2, -e_3], - [nu, -e_3, e_2], - [e_3, nu, -e_1], - [-e_2, e_1, nu] - ]) - return T - - def Crb(self) -> np.ndarray: - """Calculates the Coriolis matrix.""" - ang_vel = self.state_vector.angular_velocity - ang_vel_skew = skew_symmetric(ang_vel) - lever_arm_skew = skew_symmetric(self.r_b_bg) - Crb = np.zeros((6, 6)) - Crb[0:3, 0:3] = self.m * ang_vel_skew - Crb[3:6, 3:6] = -skew_symmetric(np.dot(self.inertia, ang_vel)) - Crb[0:3, 3:6] = -self.m * np.dot(ang_vel_skew, lever_arm_skew) - Crb[3:6, 0:3] = self.m * np.dot(lever_arm_skew, ang_vel_skew) - return Crb - - def D(self, linear_damping: np.ndarray, nonlinear_damping: np.ndarray) -> np.ndarray: - """Calculates the damping matrix.""" - D_l = -np.diag(linear_damping) - D_nl = -np.diag(nonlinear_damping) * np.abs(self.state_vector.nu()) - return D_l + D_nl - - def model_prediction(self, state: StateQuat) -> None: - """Calculates the model of the system.""" - self.state_vector = state - - self.inertia = np.diag(self.state_vector.okid_params[:3]) - self.mass_interia_matrix[3:6, 3:6] = self.inertia - self.D_matrix = self.D(self.state_vector.okid_params[3:9], self.state_vector.okid_params[9:15]) - self.added_mass = self.state_vector.okid_params[15:21] - - self.state_vector_dot.position = np.dot(self.R(), self.state_vector.velocity) - self.state_vector_dot.orientation = np.dot(self.T(), self.state_vector.angular_velocity) - - Nu = np.linalg.inv(self.mass_interia_matrix + np.diag(self.added_mass)) @ (self.Control_input - np.dot(self.Crb(), self.state_vector.nu()) - np.dot(self.D_matrix, self.state_vector.nu())) - self.state_vector_dot.velocity = Nu[:3] - self.state_vector_dot.angular_velocity = Nu[3:] - - self.state_vector_dot.okid_params = np.zeros(21) - - def euler_forward(self) -> StateQuat: - """Calculates the forward Euler integration.""" - self.state_vector.position = self.state_vector_prev.position + self.state_vector_dot.position * self.dt - self.state_vector.orientation = quat_norm(self.state_vector_prev.orientation + self.state_vector_dot.orientation * self.dt) - self.state_vector.velocity = self.state_vector_prev.velocity + self.state_vector_dot.velocity * self.dt - self.state_vector.angular_velocity = self.state_vector_prev.angular_velocity + self.state_vector_dot.angular_velocity * self.dt - self.state_vector.okid_params = self.state_vector_prev.okid_params - return self.state_vector - - - def euler_to_quat(euler_angles: np.ndarray) -> np.ndarray: """ Converts Euler angles to a quaternion diff --git a/navigation/sp_ukf_python/CMakeLists.txt b/navigation/sp_ukf_python/CMakeLists.txt deleted file mode 100644 index a40f065cd..000000000 --- a/navigation/sp_ukf_python/CMakeLists.txt +++ /dev/null @@ -1,33 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(sp_ukf_python) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake_python REQUIRED) -find_package(rclpy REQUIRED) -find_package(vortex_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) - -ament_python_install_package(${PROJECT_NAME}) - -install(DIRECTORY - launch - config - DESTINATION share/${PROJECT_NAME} -) - -install(PROGRAMS - sp_ukf_python/sp_ukf_python_node.py - DESTINATION lib/${PROJECT_NAME} -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - find_package(ament_cmake_pytest REQUIRED) - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) -endif() - -ament_package() diff --git a/navigation/sp_ukf_python/README.md b/navigation/sp_ukf_python/README.md deleted file mode 100644 index e69de29bb..000000000 diff --git a/navigation/sp_ukf_python/config/sp_ukf_python.yaml b/navigation/sp_ukf_python/config/sp_ukf_python.yaml deleted file mode 100644 index d3d18145d..000000000 --- a/navigation/sp_ukf_python/config/sp_ukf_python.yaml +++ /dev/null @@ -1,3 +0,0 @@ -/**: - ros__parameters: - sp_ukf_python_node: diff --git a/navigation/sp_ukf_python/launch/ukf.launch.py b/navigation/sp_ukf_python/launch/ukf.launch.py deleted file mode 100644 index fdd3f07e6..000000000 --- a/navigation/sp_ukf_python/launch/ukf.launch.py +++ /dev/null @@ -1,22 +0,0 @@ -import os - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch_ros.actions import Node - - -def generate_launch_description(): - sp_ukf_python_node = Node( - package='sp_ukf_python', - executable='sp_ukf_python_node.py', - name='sp_ukf_python_node', - parameters=[ - os.path.join( - get_package_share_directory('sp_ukf_python'), - 'config', - 'sp_ukf_python.yaml', - ), - ], - output='screen', - ) - return LaunchDescription([sp_ukf_python_node]) diff --git a/navigation/sp_ukf_python/package.xml b/navigation/sp_ukf_python/package.xml deleted file mode 100644 index 6aa4edbc0..000000000 --- a/navigation/sp_ukf_python/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - sp_ukf_python - 1.0.0 - This package provides the implementation of a sigma point based Unscented Error-state Kalman Filter - talhanc - MIT - - ament_cmake_python - - rclpy - python-transforms3d-pip - geometry_msgs - vortex_msgs - - python3-pytest - - - - ament_cmake - - diff --git a/navigation/sp_ukf_python/sp_ukf_python/__init__.py b/navigation/sp_ukf_python/sp_ukf_python/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python.py b/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python.py deleted file mode 100644 index 819cb737d..000000000 --- a/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python.py +++ /dev/null @@ -1,495 +0,0 @@ - -import numpy as np -from scipy.linalg import expm -from sp_ukf_python_class import StateVector_euler, StateVector_quaternion -from sp_ukf_python_utils import quaternion_super_product, skew_symmetric - - -class ErrorStateUnscentedKalmanFilter: - def __init__( - self, - P_ab: float, - P_wb: float, - Q: np.ndarray, - lever_arm: np.array, - R: np.ndarray, - g: float, - dt: float, - ) -> None: - self.P_ab = P_ab - self.P_wb = P_wb - self.Q_process_noise = Q - self.lever_arm = lever_arm - self.R = R - self.g = np.array([0, 0, g]) - self.dt = dt - self.y_i = np.zeros((15, 2 * 15)) - self.W = np.zeros(2 * 15 + 1) - - def mean_set(self, set: np.ndarray) -> np.ndarray: - """Calculates the mean of a set of values. - - Args: - set (np.ndarray): The set of values. - - Returns: - np.ndarray: The mean of the set. - """ - # Define the number of sigma points based on columns - n = set.shape[0] - mean_value = np.zeros(n) - - for i in range(2 * n + 1): - mean_value += (1/(2 * n + 1)) * set[:, i] - - return mean_value - - def weighted_mean_set(self, set: np.ndarray, weight: np.ndarray) -> np.ndarray: - """Calculates the mean of a set of values. - - Args: - set (np.ndarray): The set of values. - - Returns: - np.ndarray: The mean of the set. - """ - # Define the number of columns - n = set.shape[0] - mean_value = np.zeros(n) - - for i in range(2 * n + 1): - mean_value += weight[i] * set[:, i] - - return mean_value - - def covariance_set(self, mean: np.ndarray, set: np.ndarray) -> np.ndarray: - """Calculate the covarince of a set of sigmapoints - - Args: - mean (np.ndarray): The mean of the set. - set (np.ndarray): The set of values. - - Returns: - np.ndarray: The covariance of the set. - """ - n = set.shape[0] - covariance_set = np.zeros((n, n)) - - for i in range(2 * n + 1): - vector = StateVector_euler() - vector.position = set[:, i][:3] - vector.velocity = set[:, i][3:6] - vector.orientation = set[:, i][6:9] - vector.acceleration_bias = set[:, i][9:12] - vector.gyro_bias = set[:, i][12:] - - W_i = vector - mean - - covariance_set += (1 / (2 * n + 1)) * np.outer(W_i, W_i) - - return covariance_set - - def cross_covariance_set( - self, - mean: np.ndarray, - set: np.ndarray, - mean_2: np.ndarray, - set_2: np.ndarray, - weight: np.ndarray, - ) -> np.ndarray: - """Calculate the cross covariance of a set of sigmapoints - - Args: - mean (np.ndarray): The mean of the set. - set (np.ndarray): The set of values. - mean_2 (np.ndarray): The mean of the second set. - set_2 (np.ndarray): The second set of values. - - Returns: - np.ndarray: The cross covariance of the set. - """ - n_x = set.shape[0] - n_z = set_2.shape[0] - covariance_mat = np.zeros((n_x, n_z)) - - for i in range(2 * n_x + 1): - # parse the 15-dim error state - err_vec = set[:, i] # shape (15,) - W_i = err_vec - mean # shape (15,) - - # parse the 3-dim measurement - meas_vec = set_2[:, i] # shape (3,) - W_i_2 = meas_vec - mean_2 # shape (3,) - - # outer product -> shape (15,3) - covariance_mat += weight[i] * np.outer(W_i, W_i_2) - - return covariance_mat - - def weighted_covariance_set( - self, mean: np.ndarray, set: np.ndarray, weight: np.ndarray - ) -> np.ndarray: - """Calculate the covarince of a set of sigmapoints - - Args: - mean (np.ndarray): The mean of the set. - set (np.ndarray): The set of values. - - Returns: - np.ndarray: The covariance of the set. - """ - n = set.shape[0] - covariance_set = np.zeros((n, n)) - - for i in range(2 * n + 1): - vector = StateVector_euler() - vector.position = set[:, i][:3] - vector.velocity = set[:, i][3:6] - vector.orientation = set[:, i][6:9] - vector.acceleration_bias = set[:, i][9:12] - vector.gyro_bias = set[:, i][12:] - - W_i = vector - mean - covariance_set += weight[i] * np.outer(W_i, W_i) - - return covariance_set - - def generate_sigma_points( - self, error_state: StateVector_euler, Q_process_noise - ) -> tuple[list[StateVector_euler], np.ndarray]: - """Generates the sigma points for the UKF - This is done using the Cholesky decomposition method - """ - n = len(error_state.covariance) - kappa = 3 - n - - S = np.linalg.cholesky(error_state.covariance + Q_process_noise) - - S_scaled = np.sqrt(n + kappa) * S - - weighted_points = np.concatenate((S_scaled, -S_scaled), axis=1) - - sigma_points = [StateVector_euler() for _ in range(2 * n + 1)] - W = np.zeros(2 * n + 1) - - sigma_points[0].fill_states(error_state.as_vector()) - W[0] = kappa / (n + kappa) - - for i in range(2 * n): - sigma_points[i + 1].fill_states(error_state + weighted_points[:, i]) - W[i + 1] = 1 / (2 * (n + kappa)) - - self.W = W - return sigma_points, W - - def nominal_state_update( - self, current_state: StateVector_quaternion, imu_reading: np.ndarray - ) -> StateVector_quaternion: - """Updates the nominal state of the system. - - Args: - current_state (np.ndarray): The current state of the system. - imu_reading (np.ndarray): The IMU reading. - - Returns: - np.ndarray: The updated nominal state. - """ - # Defining the IMU readings - imu_acceleration = imu_reading[0:3] - imu_gyro = imu_reading[3:6] - - # Define the derivative of the state - current_state_dot = StateVector_quaternion() - - # Define the state derivates - current_state_dot.position = current_state.velocity - current_state_dot.velocity = ( - np.dot( - current_state.R_q(), - (imu_acceleration - current_state.acceleration_bias), - ) - + self.g - ) - - # Define the quaternion derivatives - current_state_dot.orientation = 0.5 * quaternion_super_product( - current_state.orientation, - np.array( - [ - 0, - imu_gyro[0] - current_state.gyro_bias[0], - imu_gyro[1] - current_state.gyro_bias[1], - imu_gyro[2] - current_state.gyro_bias[2], - ] - ), - ) - - # Define the bias - current_state_dot.acceleration_bias = ( - -np.dot(self.P_ab, np.eye(3)) @ current_state.acceleration_bias - ) - current_state_dot.gyro_bias = ( - -np.dot(self.P_wb, np.eye(3)) @ current_state.gyro_bias - ) - - return current_state_dot.euler_forward(current_state, self.dt) - - def error_state_update( - self, - current_error_state: StateVector_euler, - current_state: StateVector_quaternion, - imu_reading: np.ndarray, - ) -> np.ndarray: - """Updates the error state of the system. - - Args: - current_error_state (np.ndarray): The current error state of the system. - current_state (np.ndarray): The current state of the system. - imu_reading (np.ndarray): The IMU reading. - - Returns: - np.ndarray: The updated error state. - """ - # Defining the IMU readings - imu_acceleration = imu_reading[0:3] - imu_gyro = imu_reading[3:6] - - A_c = np.zeros((15, 15)) - A_c[0:3, 3:6] = np.eye(3) - A_c[3:6, 6:9] = -np.dot( - current_state.R_q(), - skew_symmetric(imu_acceleration - current_state.acceleration_bias), - ) - A_c[6:9, 6:9] = -skew_symmetric(imu_gyro - current_state.gyro_bias) - A_c[3:6, 9:12] = -current_state.R_q() - A_c[6:9, 12:15] = -np.eye(3) - A_c[9:12, 9:12] = -self.P_ab * np.eye(3) - A_c[12:15, 12:15] = -self.P_wb * np.eye(3) - - # Exact matrix exponential - A_d = expm(A_c * self.dt) - - next_error_state = A_d @ current_error_state.as_vector() - - return next_error_state - - def unscented_transform( - self, - sigma_points: list[StateVector_euler], - current_state: StateVector_quaternion, - imu_reading: np.ndarray, - ) -> StateVector_euler: - """Performs the Unscented Transform - This is the corresponding to a preditction step in the EKF - """ - n = len(sigma_points[0].as_vector()) - - self.y_i = np.zeros((n, 2 * n + 1)) - - for i in range(2 * n + 1): - self.y_i[:, i] = self.error_state_update( - sigma_points[i], current_state, imu_reading - ) - - error_state_estimate = StateVector_euler() - - x = self.weighted_mean_set(self.y_i, self.W) - - error_state_estimate.fill_states(x) - error_state_estimate.covariance = self.weighted_covariance_set(x, self.y_i, self.W) - - return error_state_estimate - - def H(self) -> np.ndarray: - """Calculates the measurement matrix. - - Returns: - np.ndarray: The measurement matrix. - """ - # Define the measurement matrix (error state is 15-dim) - H = np.zeros((3, 16)) - - # For now assume only velocity is measured (located at indices 3:6) - H[:, 3:6] = np.eye(3) - - return H - - def injection( - self, - current_state_nom: StateVector_quaternion, - current_state_error: StateVector_euler, - ) -> StateVector_quaternion: - """Injects the error state into the nominal state - - Args: - current_state_nom (StateVector_quaternion): The current nominal state - current_state_error (StateVector_euler): The current error state - - Returns: - StateVector_quaternion: The updated nominal state - """ - inj_state = StateVector_quaternion() - - inj_state.position = current_state_nom.position + current_state_error.position - inj_state.velocity = current_state_nom.velocity + current_state_error.velocity - inj_state.orientation = quaternion_super_product( - current_state_nom.orientation, - 0.5 - * np.array( - [ - 2, - current_state_error.orientation[0], - current_state_error.orientation[1], - current_state_error.orientation[2], - ] - ), - ) - inj_state.acceleration_bias = ( - current_state_nom.acceleration_bias + current_state_error.acceleration_bias - ) - inj_state.gyro_bias = ( - current_state_nom.gyro_bias + current_state_error.gyro_bias - ) - - return inj_state - - def measurement_update( - self, - sigma_points: list[StateVector_euler], - current_nom_state: StateVector_quaternion, - current_error_state: StateVector_euler, - dvl_data: np.ndarray, - Weight: np.ndarray, - ) -> StateVector_euler: - """Updates the state vector with the DVL data - """ - H = self.H() - R = self.R - - n = len(sigma_points[0].as_vector()) - - Z_i = np.zeros((H.shape[0], 2 * n + 1)) - - for i in range(2 * n + 1): - injected_state = self.injection(current_nom_state, sigma_points[i]) - Z_i[:, i] = np.dot(H, injected_state.as_vector()) - - z = self.weighted_mean_set(Z_i, Weight) - S = self.weighted_covariance_set(z, Z_i, Weight) - - x = self.mean_set(self.y_i) - - innovation = dvl_data - z - - P_innovation = S + R - - P_xz = self.cross_covariance_set(x, self.y_i, z, Z_i, Weight) - - # Kalman gain - K_k = np.dot(P_xz, np.linalg.inv(P_innovation)) - - updated_error_state = StateVector_euler() - - # Update the state - updated_error_state.fill_states(x + np.dot(K_k, innovation)) - - # Update the covariance - updated_error_state.covariance = current_error_state.covariance - np.dot( - K_k, np.dot(P_innovation, K_k.T) - ) - - return updated_error_state - - def imu_update_states( - self, - current_state_nom: StateVector_quaternion, - current_state_error: StateVector_euler, - imu_data: np.ndarray, - ) -> tuple[StateVector_quaternion, StateVector_euler]: - """Updates the state vector with the IMU data - - Args: - current_state_nom (StateVector_quaternion): The current nominal state - current_state_error (StateVector_euler): The current error state - imu_data (np.ndarray): The IMU data - - Returns: - tuple[StateVector_quaternion, StateVector_euler]: The updated nominal and error states - - """ - # Update the nominal state - current_state_nom = self.nominal_state_update(current_state_nom, imu_data) - - # Generate the sigma points - sigma_points, _ = self.generate_sigma_points( - current_state_error, self.Q_process_noise - ) - - # Update the error state - current_state_error = self.unscented_transform( - sigma_points, current_state_nom, imu_data - ) - - return current_state_nom, current_state_error - - def dvl_update_states( - self, - current_state_nom: StateVector_quaternion, - current_state_error: StateVector_euler, - dvl_data: np.ndarray, - imu_data: np.ndarray, - ) -> tuple[StateVector_quaternion, StateVector_euler]: - """Update the error state given the DVL data - - Args: - current_state_nom (StateVector_quaternion): The current nominal state - current_state_error (StateVector_euler): The current error state - dvl_data (np.ndarray): The DVL data to update the state with - - Returns: - tuple[StateVector_quaternion, StateVector_euler]: The updated nominal and error states - """ - # Generate the sigma points - sigma_points, weight = self.generate_sigma_points( - current_state_error, self.Q_process_noise - ) - - # Update the error state - current_state_error = self.unscented_transform( - sigma_points, current_state_nom, imu_data - ) - - # Update the error state - current_state_error = self.measurement_update( - sigma_points, current_state_nom, current_state_error, dvl_data, weight - ) - - return current_state_nom, current_state_error - - def inject_and_reset( - self, - current_state_nom: StateVector_quaternion, - current_state_error: StateVector_euler, - ) -> tuple[StateVector_quaternion, StateVector_euler]: - """Injects the error state into the nominal state and resets the error state - - Args: - current_state_nom (StateVector_quaternion): The current nominal state - current_state_error (StateVector_euler): The current error state - - Returns: - tuple[StateVector_quaternion, StateVector_euler]: The updated nominal and error states - """ - inj_state = self.injection(current_state_nom, current_state_error) - - G = np.eye(15) - G[6:9, 6:9] = np.eye(3) - skew_symmetric(0.5 * current_state_error.orientation) - - current_state_error.covariance = np.dot( - np.dot(G, current_state_error.covariance), G.T - ) - current_state_error.covariance += np.eye(15) - - current_state_error.fill_states(np.zeros(15)) - - return inj_state, current_state_error diff --git a/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_class.py b/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_class.py deleted file mode 100644 index f8ede884d..000000000 --- a/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_class.py +++ /dev/null @@ -1,292 +0,0 @@ -from dataclasses import dataclass, field - -import numpy as np -from sp_ukf_python_utils import ( - euler_rotation_quaternion, - quaternion_error, - quaternion_super_product, - ssa, - quat_norm, -) - - -@dataclass -class StateVector_quaternion: - position: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Position vector (x, y, z) - velocity: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Velocity vector (u, v, w) - orientation: np.ndarray = field( - default_factory=lambda: np.zeros(4) - ) # Orientation quaternion (w, x, y, z) - acceleration_bias: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Acceleration bias vector (b_ax, b_ay, b_az) - gyro_bias: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Gyro bias vector (b_gx, b_gy, b_gz) - - def as_vector(self) -> np.ndarray: - """Calculates the state vector. - - Returns: - np.ndarray: The state vector. - """ - return np.concatenate( - [ - self.position, - self.velocity, - self.orientation, - self.acceleration_bias, - self.gyro_bias, - ] - ) - - def fill_states(self, state: np.ndarray) -> None: - """Fills the state vector with the values from a numpy array. - - Args: - state (np.ndarray): The state vector. - """ - if len(state) == 15: - self.position = state[0:3] - self.velocity = state[3:6] - self.orientation = state[6:10] - self.acceleration_bias = state[10:13] - self.gyro_bias = state[13:] - else: - self.position = state[0:3] - self.velocity = state[3:6] - self.orientation = euler_rotation_quaternion(state[6:9]) - self.acceleration_bias = state[9:12] - self.gyro_bias = state[12:] - - def R_q(self) -> np.ndarray: - """Calculates the rotation matrix from the orientation quaternion. - - Returns: - np.ndarray: The rotation matrix. - """ - q0, q1, q2, q3 = self.orientation - R = np.array( - [ - [ - 1 - 2 * q2**2 - 2 * q3**2, - 2 * (q1 * q2 - q0 * q3), - 2 * (q0 * q2 + q1 * q3), - ], - [ - 2 * (q1 * q2 + q0 * q3), - 1 - 2 * q1**2 - 2 * q3**2, - 2 * (q2 * q3 - q0 * q1), - ], - [ - 2 * (q1 * q3 - q0 * q2), - 2 * (q0 * q1 + q2 * q3), - 1 - 2 * q1**2 - 2 * q2**2, - ], - ] - ) - - return R - - def euler_forward( - self, current_state: 'StateVector_quaternion', dt: float - ) -> 'StateVector_quaternion': - # Define the new state - new_state = StateVector_quaternion() - - # Define the state derivatives - new_state.position = current_state.position + self.position * dt - new_state.velocity = current_state.velocity + self.velocity * dt - new_state.orientation = quat_norm(current_state.orientation + self.orientation * dt) - new_state.acceleration_bias = ( - current_state.acceleration_bias + self.acceleration_bias * dt - ) - new_state.gyro_bias = current_state.gyro_bias + self.gyro_bias * dt - - # Normalize the orientation quaternion - new_state.orientation /= np.linalg.norm(new_state.orientation) - - return new_state - - def __sub__(self, other: 'StateVector_quaternion') -> np.ndarray: - """Subtracts two StateVector_quaternion objects. - - Args: - other (StateVector_quaternion): The other StateVector_quaternion object. - - Returns: - np.ndarray: The difference between the two StateVector_quaternion objects. - """ - position_diff = self.position - other.position - velocity_diff = self.velocity - other.velocity - orientation_diff = quaternion_error(self.orientation, other.orientation) - acceleration_bias_diff = self.acceleration_bias - other.acceleration_bias - gyro_bias_diff = self.gyro_bias - other.gyro_bias - - return np.concatenate( - [ - position_diff, - velocity_diff, - orientation_diff, - acceleration_bias_diff, - gyro_bias_diff, - ] - ) - - def __add__(self, other: 'np.ndarray') -> 'np.ndarray': - """Adds a numpy array to this StateVector_quaternion. - - Args: - other (np.ndarray): The numpy array to add. - - Returns: - np.ndarray: The result of the addition. - """ - # Construct the quaternion from the array - add_to_position = other[:3] - add_to_orientation = euler_rotation_quaternion(other[6:10]) - - new_position = self.position + add_to_position - new_velcoity = self.velocity + other[3:6] - new_orientation = quaternion_super_product(self.orientation, add_to_orientation) - new_acceleration_bias = self.acceleration_bias + other[10:13] - new_gyro_bias = self.gyro_bias + other[13:] - - return np.concatenate( - [ - new_position, - new_velcoity, - new_orientation, - new_acceleration_bias, - new_gyro_bias, - ] - ) - - -@dataclass -class StateVector_euler: - position: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Position vector (x, y, z) - velocity: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Velocity vector (u, v, w) - orientation: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Orientation angles (roll, pitch, yaw) - acceleration_bias: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Acceleration bias vector (b_ax, b_ay, b_az) - gyro_bias: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Gyro bias vector (b_gx, b_gy, b_gz) - covariance: np.ndarray = field( - default_factory=lambda: np.zeros((15, 15)) - ) # Covariance matrix - - def as_vector(self) -> np.ndarray: - """Calculates the state estimate vector. - - Returns: - np.ndarray: The state estimate vector. - """ - return np.concatenate( - [ - self.position, - self.velocity, - self.orientation, - self.acceleration_bias, - self.gyro_bias, - ] - ) - - def fill_states(self, state: np.ndarray) -> None: - """Fills the state vector with the values from a numpy array. - - Args: - state (np.ndarray): The state vector. - """ - self.position = state[0:3] - self.velocity = state[3:6] - self.orientation = state[6:9] - self.acceleration_bias = state[9:12] - self.gyro_bias = state[12:15] - - def copy_state(self, wanted_state: 'StateVector_euler') -> None: - """Copies the state from a StateVector object into the current StateVector object. - - Args: - wanted_state (StateVector_euler): The quaternion state to copy from. - """ - self.position = wanted_state.position - self.velocity = wanted_state.velocity - self.orientation = wanted_state.orientation - self.acceleration_bias = wanted_state.acceleration_bias - self.gyro_bias = wanted_state.gyro_bias - - def __add__(self, other: 'np.ndarray') -> 'np.ndarray': - """Adds a numpy array to this StateVector_quaternion. - - Args: - other (np.ndarray): The numpy array to add. - - Returns: - np.ndarray: The result of the addition. - """ - new_position = self.position + other[:3] - new_velcoity = self.velocity + other[3:6] - new_orientation = self.orientation + other[6:9] - new_acceleration_bias = self.acceleration_bias + other[9:12] - new_gyro_bias = self.gyro_bias + other[12:] - - return np.concatenate( - [ - new_position, - new_velcoity, - new_orientation, - new_acceleration_bias, - new_gyro_bias, - ] - ) - - def __sub__(self, other_state: 'StateVector_euler') -> 'StateVector_euler': - """Subtracts two StateVector_euler objects. - - Args: - other (StateVector_euler): The other StateVector_euler object. - - Returns: - StateVector_euler: The difference between the two StateVector_euler objects. - """ - position_diff = self.position - other_state[:3] - velocity_diff = self.velocity - other_state[3:6] - orientation_diff = ssa(self.orientation - other_state[6:9]) - acceleration_bias_diff = self.acceleration_bias - other_state[9:12] - gyro_bias_diff = self.gyro_bias - other_state[12:] - - return np.concatenate( - [ - position_diff, - velocity_diff, - orientation_diff, - acceleration_bias_diff, - gyro_bias_diff, - ] - ) - - -@dataclass -class MeasurementModel: - measurement: np.ndarray = field( - default_factory=lambda: np.zeros(6) - ) # Measurement vector - measurement_matrix: np.ndarray = field( - default_factory=lambda: np.zeros((6, 15)) - ) # Measurement matrix - measurement_covariance: np.ndarray = field( - default_factory=lambda: np.zeros((6, 6)) - ) # Measurement noise matrix diff --git a/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_node.py b/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_node.py deleted file mode 100644 index 103528ef2..000000000 --- a/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_node.py +++ /dev/null @@ -1,137 +0,0 @@ -#!/usr/bin/env python3 - -import rclpy -from nav_msgs.msg import Odometry -from rclpy.node import Node -from rclpy.qos import QoSProfile, qos_profile_sensor_data -from sensor_msgs.msg import Imu, -import numpy as np -from geometry_msgs.msg import TwistWithCovarianceStamped - -# NEED TO CHANGE THIS TO THE CORRECT PATH -from eskf_python.eskf_python_filter import ( - ErrorStateKalmanFilter, - MeasurementModel, - StateVector_euler, - StateVector_quaternion, -) - -qos_profile = QoSProfile( - depth=1, - history=qos_profile_sensor_data.history, - reliability=qos_profile_sensor_data.reliability, -) - - -class ESKalmanFilterNode(Node): - def __init__(self): - super().__init__("sp_ukf_python_node") - - # This callback will supply information from the IMU (Inertial Measurement Unit) 1000 Hz - self.imu_subscriber_ = self.create_subscription( - Imu, '/orca/imu', self.imu_callback, qos_profile=qos_profile - ) - - self.twist_dvl_subscriber_ = self.create_subscription( - TwistWithCovarianceStamped, '/dvl/twist', self.filter_callback, qos_profile=qos_profile - ) - - # This publisher will publish the estimtaed state of the vehicle - self.state_publisher_ = self.create_publisher( - Odometry, '/orca/odom', qos_profile=qos_profile - ) - - self.eskf_modual = ErrorStateKalmanFilter() - self.current_state_nom = StateVector_quaternion() - self.current_state_error = StateVector_euler() - self.measurement_pred = MeasurementModel() - self.odom_msg = Odometry() - - self.get_logger().info("Unscented Kalman Filter started") - - def imu_callback(self, msg: Imu): - - # Get the IMU data - - imu_acceleartion = msg.linear_acceleration - imu_angular_velocity = msg.angular_velocity - - # Combine the IMU data - imu_data = np.array([imu_acceleartion.x, imu_acceleartion.y, imu_acceleartion.z, imu_angular_velocity.x, imu_angular_velocity.y, imu_angular_velocity.z]) - - # Update the filter with the IMU data - self.current_state_nom, self.current_state_error = ( - ErrorStateKalmanFilter.imu_update_states( - self.current_state_nom, self.current_state_error, imu_data - ) - ) - - # Inserting the nominal state into the msg - self.odom_msg.pose.pose.position.x = self.current_state_nom.position[0] - self.odom_msg.pose.pose.position.y = self.current_state_nom.position[1] - self.odom_msg.pose.pose.position.z = self.current_state_nom.position[2] - self.odom_msg.pose.pose.orientation.x = self.current_state_nom.orientation[0] - self.odom_msg.pose.pose.orientation.y = self.current_state_nom.orientation[1] - self.odom_msg.pose.pose.orientation.z = self.current_state_nom.orientation[2] - self.odom_msg.pose.pose.orientation.w = self.current_state_nom.orientation[3] - self.odom_msg.twist.twist.linear.x = self.current_state_nom.velocity[0] - self.odom_msg.twist.twist.linear.y = self.current_state_nom.velocity[1] - self.odom_msg.twist.twist.linear.z = self.current_state_nom.velocity[2] - self.odom_msg.twist.twist.angular.x = imu_angular_velocity.x - self.odom_msg.twist.twist.angular.y = imu_angular_velocity.y - self.odom_msg.twist.twist.angular.z = imu_angular_velocity.z - - # Publish - self.state_publisher_.publish(self.odom_msg) - - - - def filter_callback(self, msg: TwistWithCovarianceStamped): - """Callback function for the filter measurement update, - this will be called when the filter needs to be updated with the DVL data. - """ - self.get_logger().info("Filter callback, got DVL data") - - # Get the DVL data (linear velocity) - dvl_data = np.array([msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z]) - - # Update the filter with the DVL data - self.current_state_nom, self.current_state_error = ( - ErrorStateKalmanFilter.dvl_update_states( - self.current_state_nom, self.current_state_error, dvl_data - ) - ) - self.current_state_nom, self.current_state_error = ( - ErrorStateKalmanFilter.injection_and_reset( - self.current_state_nom, self.current_state_error - ) - ) - - # Inserting data into the msg - self.odom_msg.pose.pose.position.x = self.current_state_nom.position[0] - self.odom_msg.pose.pose.position.y = self.current_state_nom.position[1] - self.odom_msg.pose.pose.position.z = self.current_state_nom.position[2] - self.odom_msg.pose.pose.orientation.x = self.current_state_nom.orientation[0] - self.odom_msg.pose.pose.orientation.y = self.current_state_nom.orientation[1] - self.odom_msg.pose.pose.orientation.z = self.current_state_nom.orientation[2] - self.odom_msg.pose.pose.orientation.w = self.current_state_nom.orientation[3] - self.odom_msg.twist.twist.linear.x = self.current_state_nom.velocity[0] - self.odom_msg.twist.twist.linear.y = self.current_state_nom.velocity[1] - self.odom_msg.twist.twist.linear.z = self.current_state_nom.velocity[2] - self.odom_msg.twist.twist.linear.z = self.current_state_nom.velocity[2] - - # Publishing the data - self.state_publisher_.publish(self.odom_msg) - - - -def main(args=None): - rclpy.init(args=args) - node = ESKalmanFilterNode() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_utils.py b/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_utils.py deleted file mode 100644 index 56285f031..000000000 --- a/navigation/sp_ukf_python/sp_ukf_python/sp_ukf_python_utils.py +++ /dev/null @@ -1,116 +0,0 @@ -import numpy as np - -def quaternion_super_product(q1: np.ndarray, q2: np.ndarray) -> np.ndarray: - """Calculates the quaternion super product of two quaternions. - - Args: - q1 (np.ndarray): The first quaternion. - q2 (np.ndarray): The second quaternion. - - Returns: - np.ndarray: The quaternion super product. - """ - eta_0, e_0_x, e_0_y, e_0_z = q1 - eta_1, e_1_x, e_1_y, e_1_z = q2 - - e_0 = np.array([e_0_x, e_0_y, e_0_z]) - e_1 = np.array([e_1_x, e_1_y, e_1_z]) - - eta_new = eta_0 * eta_1 - (e_0_x * e_1_x + e_0_y * e_1_y + e_0_z * e_1_z) - nu_new = e_1 * eta_0 + e_0 * eta_1 + np.dot(skew_symmetric(e_0), e_1) - - q_new = np.array([eta_new, nu_new[0], nu_new[1], nu_new[2]]) - q_new /= np.linalg.norm(q_new) - - return q_new - -def quaternion_error(quat_1: np.ndarray, quat_2: np.ndarray) -> np.ndarray: - """ - Calculates the error between two quaternions - """ - - quat_2_inv = np.array([quat_2[0], -quat_2[1], -quat_2[2], -quat_2[3]]) - - error_quat = quaternion_super_product(quat_1, quat_2_inv) - - return error_quat - -def euler_rotation_quaternion(self, euler_angles: np.ndarray) -> np.ndarray: - """ - Converts An vector assumed to be rotation vector to quaternion - - Args: - euler_angles (np.ndarray): Rotation vector - - Returns: - np.ndarray: Quaternion representation of the rotation vector - """ - - angle = np.linalg.norm(euler_angles) - - if angle == 0: - axis = np.array([0, 0, 0]) - else: - axis = euler_angles / angle - - quaternion = np.zeros(4) - quaternion[0] = np.cos(angle / 2) - quaternion[1:] = np.sin(angle / 2) * axis - - return quaternion - -def quaternion_rotation_euler(self, quaternion: np.ndarray) -> np.ndarray: - """ - Converts a quaternion to an euler rotation vector - Used to generate the covarince matrix - - Args: - quaternion (np.ndarray): The quaternion to convert - - Returns: - np.ndarray: The euler rotation vector - """ - nu, eta_x, eta_y, eta_z = quaternion - - phi = np.arctan2(2 * (nu * eta_x + eta_y * eta_z), 1 - 2 * (eta_x ** 2 + eta_y ** 2)) - theta = -np.arcsin(2 * (eta_z * eta_x - nu * eta_y)) - psi = np.arctan2(2 * (nu * eta_z + eta_x * eta_y), 1 - 2 * (eta_y ** 2 + eta_z ** 2)) - - return np.array([phi, theta, psi]) - -def skew_symmetric(vector: np.ndarray) -> np.ndarray: - """Calculates the skew symmetric matrix of a vector. - - Args: - vector (np.ndarray): The vector. - - Returns: - np.ndarray: The skew symmetric matrix. - """ - return np.array( - [ - [0, -vector[2], vector[1]], - [vector[2], 0, -vector[0]], - [-vector[1], vector[0], 0], - ] - ) - -def ssa(angle: np.ndarray) -> np.ndarray: - """ - smallest signed angle between two angles - """ - ssa_vector = np.zeros(len(angle)) - - for i in range(len(angle)): - ssa_vector[i] = (angle[i] + np.pi) % (2 * np.pi) - np.pi - - return ssa_vector - -def quat_norm(quat: np.ndarray) -> np.ndarray: - """ - Function that normalizes a quaternion - """ - - quat = quat / np.linalg.norm(quat) - - return quat diff --git a/navigation/sp_ukf_python/sp_ukf_python/test_ukf.py b/navigation/sp_ukf_python/sp_ukf_python/test_ukf.py deleted file mode 100644 index 1d8c723b1..000000000 --- a/navigation/sp_ukf_python/sp_ukf_python/test_ukf.py +++ /dev/null @@ -1,313 +0,0 @@ -import matplotlib.pyplot as plt -import numpy as np - -# (Assuming the following have been imported from your modules) -from sp_ukf_python_class import StateVector_euler, StateVector_quaternion - -from sp_ukf_python import ErrorStateUnscentedKalmanFilter - - -def quat_to_yaw(q: np.ndarray) -> float: - """Convert a quaternion (assumed [w, x, y, z]) into a yaw angle. - In NED, yaw is typically around the z-down axis. - """ - return 2 * np.arctan2(q[3], q[0]) - - -def run_ESUKF_simulation(): - # ------------------------------------------------------------------------- - # Simulation parameters - # ------------------------------------------------------------------------- - dt = 0.01 # time step [s] - T = 60.0 # total simulation time [s] - num_steps = int(T / dt) - - # In an NED frame, gravity is +9.81 in the z (down) direction. - g_val = 9.81 - - # ------------------------------------------------------------------------- - # Define noise and covariance matrices - # ------------------------------------------------------------------------- - Q = np.diag( - [ - 0.06, - 0.06, - 0.06, # position error - 0.04, - 0.04, - 0.04, # velocity error - 0.003, - 0.003, - 0.003, # orientation error - 0.02, - 0.02, - 0.02, # accelerometer bias error - 0.02, - 0.02, - 0.02, # gyro bias error - ] - ) - - R_meas = np.diag([0.52, 0.52, 0.52]) # Increased DVL measurement noise - - # Bias dynamics tuning remains the same here: - P_ab = 0.002 - P_wb = 0.002 - lever_arm = np.array([0.0, 0.0, 0.0]) # Sensor at the vehicle CG - - # Create the Error-State UKF instance (NED convention) - esukf = ErrorStateUnscentedKalmanFilter(P_ab, P_wb, Q, lever_arm, R_meas, g_val, dt) - - # ------------------------------------------------------------------------- - # Initialize the true state in NED - # ------------------------------------------------------------------------- - # We treat x as North, y as East, z as Down. - # We'll do a circular path in the horizontal plane (z=0). - true_state = StateVector_quaternion() - true_state.position = np.array([20.0, 0.0, 0.0]) # [N, E, D]=[20, 0, 0] - true_state.velocity = np.array([0.0, 1.0, 0.0]) # 1 m/s in the East direction - true_state.orientation = np.array([1.0, 0.0, 0.0, 0.0]) # No initial rotation - true_state.acceleration_bias = np.zeros(3) - true_state.gyro_bias = np.zeros(3) - - # ------------------------------------------------------------------------- - # Initialize the estimated state - # ------------------------------------------------------------------------- - est_state_nom = StateVector_quaternion() - est_state_nom.position = true_state.position + np.array([0.1, -0.1, 0.05]) - est_state_nom.velocity = true_state.velocity + np.array([0.05, 0.05, -0.05]) - est_state_nom.orientation = true_state.orientation.copy() - est_state_nom.acceleration_bias = np.zeros(3) - est_state_nom.gyro_bias = np.zeros(3) - - # Initialize error state (Euler) with some covariance - est_state_error = StateVector_euler() - est_state_error.fill_states(np.zeros(15)) - est_state_error.covariance = 0.5 * np.eye(15) - - # ------------------------------------------------------------------------- - # Prepare histories for plotting - # ------------------------------------------------------------------------- - time_hist = [] - true_pos_hist = [] - est_pos_hist = [] - true_vel_hist = [] - est_vel_hist = [] - true_yaw_hist = [] - est_yaw_hist = [] - - # ------------------------------------------------------------------------- - # Define the "circular" trajectory in the horizontal plane (z=0) - # in NED: x=North, y=East, z=Down - # We'll revolve in the XY-plane, at D=0, with radius=20 m, angular speed=0.05 rad/s - # ------------------------------------------------------------------------- - R_circle = 20.0 - omega = 0.05 - - # ------------------------------------------------------------------------- - # Main simulation loop - # ------------------------------------------------------------------------- - t = 0.0 - for step in range(num_steps): - # --- True State Generation (NED) --- - # Position: circle in x-y plane at z=0 - pos_true = np.array( - [ - R_circle * np.cos(omega * t), # N - R_circle * np.sin(omega * t), # E - 0.0, # D - ] - ) - # Velocity: derivative of pos - vel_true = np.array( - [ - -R_circle * omega * np.sin(omega * t), # d/dt of cos => -sin - R_circle * omega * np.cos(omega * t), # d/dt of sin => cos - 0.0, - ] - ) - # Acceleration: second derivative - acc_true = np.array( - [ - -R_circle * omega**2 * np.cos(omega * t), - -R_circle * omega**2 * np.sin(omega * t), - 0.0, - ] - ) - - # Update the "true" state in NED - true_state.position = pos_true - true_state.velocity = vel_true - - # Compute full quaternion from Euler angles (roll, pitch, yaw) - roll_true = 0.0 - pitch_true = 0.0 - yaw_true = np.arctan2(vel_true[1], vel_true[0]) - cy = np.cos(yaw_true * 0.5) - sy = np.sin(yaw_true * 0.5) - cp = np.cos(pitch_true * 0.5) - sp = np.sin(pitch_true * 0.5) - cr = np.cos(roll_true * 0.5) - sr = np.sin(roll_true * 0.5) - true_state.orientation = np.array( - [ - cr * cp * cy + sr * sp * sy, # w - sr * cp * cy - cr * sp * sy, # x - cr * sp * cy + sr * cp * sy, # y - cr * cp * sy - sr * sp * cy, # z - ] - ) - - # --- Simulated IMU Measurements (NED) --- - # Gravity is +9.81 in the down (z) direction in NED - R_true = true_state.R_q() # rotation from body to inertial - # The "specific force" in body frame is (acc_inertial - gravity_inertial) rotated to body - imu_acc_ideal = R_true.T @ ( - acc_true - np.array([0.0, 0.0, g_val]) - ) + np.random.normal(0.01, 0.01, 3) # [rad/s] - - # Add small noise - imu_acc_noise = np.random.normal(0.0, 0.05, 3) # [m/s^2] - imu_acc_meas = imu_acc_ideal + imu_acc_noise - - # Gyro: angular velocity about body axes. Yaw rate is ~omega for a flat circle - imu_gyro_ideal = np.array([0.0, 0.0, omega]) + np.random.normal( - 0.01, 0.01, 3 - ) # [rad/s] - imu_gyro_noise = np.random.normal(0.0, 0.05, 3) # [rad/s] - imu_gyro_meas = imu_gyro_ideal + imu_gyro_noise - - # Combine - imu_meas = np.hstack((imu_acc_meas, imu_gyro_meas)) - - # --- Simulated DVL Measurement --- - # Velocity in inertial frame (NED) with zero noise for this test - dvl_noise = np.random.normal(0.0, 0.05, 3) - dvl_meas = vel_true + dvl_noise - - # --------------------------------------------------------------------- - # Filter Updates - # --------------------------------------------------------------------- - # 1. IMU update (prediction) - est_state_nom, est_state_error = esukf.imu_update_states( - est_state_nom, est_state_error, imu_meas - ) - # 2. DVL update (measurement) - est_state_nom, est_state_error = esukf.dvl_update_states( - est_state_nom, est_state_error, dvl_meas, imu_meas - ) - # 3. Inject error state - est_state_nom, est_state_error = esukf.inject_and_reset( - est_state_nom, est_state_error - ) - - # --- Store Histories --- - time_hist.append(t) - true_pos_hist.append(pos_true) - est_pos_hist.append(est_state_nom.position.copy()) - true_vel_hist.append(vel_true) - est_vel_hist.append(est_state_nom.velocity.copy()) - true_yaw_hist.append(yaw_true) - est_yaw_hist.append(quat_to_yaw(est_state_nom.orientation)) - - t += dt - - # ------------------------------------------------------------------------- - # Convert histories to arrays - # ------------------------------------------------------------------------- - true_pos_hist = np.array(true_pos_hist) - est_pos_hist = np.array(est_pos_hist) - true_vel_hist = np.array(true_vel_hist) - est_vel_hist = np.array(est_vel_hist) - true_yaw_hist = np.array(true_yaw_hist) - est_yaw_hist = np.array(est_yaw_hist) - time_hist = np.array(time_hist) - - # ------------------------------------------------------------------------- - # Plotting - # ------------------------------------------------------------------------- - # Positions - plt.figure(figsize=(10, 8)) - plt.subplot(3, 1, 1) - plt.plot(time_hist, true_pos_hist[:, 0], label='True N') - plt.plot(time_hist, est_pos_hist[:, 0], '--', label='Estimated N') - plt.ylabel('N (m)') - plt.legend() - - plt.subplot(3, 1, 2) - plt.plot(time_hist, true_pos_hist[:, 1], label='True E') - plt.plot(time_hist, est_pos_hist[:, 1], '--', label='Estimated E') - plt.ylabel('E (m)') - plt.legend() - - plt.subplot(3, 1, 3) - plt.plot(time_hist, true_pos_hist[:, 2], label='True D') - plt.plot(time_hist, est_pos_hist[:, 2], '--', label='Estimated D') - plt.xlabel('Time (s)') - plt.ylabel('D (m)') - plt.legend() - plt.tight_layout() - plt.show() - - # Velocities - plt.figure(figsize=(10, 8)) - plt.subplot(3, 1, 1) - plt.plot(time_hist, true_vel_hist[:, 0], label='True Vn') - plt.plot(time_hist, est_vel_hist[:, 0], '--', label='Estimated Vn') - plt.ylabel('Vn (m/s)') - plt.legend() - - plt.subplot(3, 1, 2) - plt.plot(time_hist, true_vel_hist[:, 1], label='True Ve') - plt.plot(time_hist, est_vel_hist[:, 1], '--', label='Estimated Ve') - plt.ylabel('Ve (m/s)') - plt.legend() - - plt.subplot(3, 1, 3) - plt.plot(time_hist, true_vel_hist[:, 2], label='True Vd') - plt.plot(time_hist, est_vel_hist[:, 2], '--', label='Estimated Vd') - plt.xlabel('Time (s)') - plt.ylabel('Vd (m/s)') - plt.legend() - plt.tight_layout() - plt.show() - - # Heading (Yaw) - plt.figure(figsize=(10, 4)) - plt.plot(time_hist, np.degrees(true_yaw_hist), label='True Yaw') - plt.plot(time_hist, np.degrees(est_yaw_hist), '--', label='Estimated Yaw') - plt.xlabel('Time (s)') - plt.ylabel('Yaw (deg)') - plt.legend() - plt.title('Heading Comparison (NED)') - plt.tight_layout() - plt.show() - - # 3D Trajectory - fig = plt.figure(figsize=(8, 6)) - ax = fig.add_subplot(111, projection='3d') - ax.plot( - true_pos_hist[:, 0], - true_pos_hist[:, 1], - true_pos_hist[:, 2], - label='True Trajectory', - linewidth=2, - ) - ax.plot( - est_pos_hist[:, 0], - est_pos_hist[:, 1], - est_pos_hist[:, 2], - '--', - label='Estimated Trajectory', - linewidth=2, - ) - ax.set_xlabel('North (m)') - ax.set_ylabel('East (m)') - ax.set_zlabel('Down (m)') - ax.legend() - plt.title('3D Trajectory (NED Frame)') - plt.show() - - -if __name__ == '__main__': - run_ESUKF_simulation() diff --git a/navigation/ukf_okid/ukf_python/ukf_okid.py b/navigation/ukf_okid/ukf_python/ukf_okid.py index c588d579e..dd52c6589 100644 --- a/navigation/ukf_okid/ukf_python/ukf_okid.py +++ b/navigation/ukf_okid/ukf_python/ukf_okid.py @@ -14,6 +14,31 @@ def __init__(self, process_model: process_model, x_0, P_0, Q, R): self.sigma_points_list = None self.y_i = None self.weight = None + # self.T = self.generate_T_matrix(len(P_0)) + + def generate_T_matrix(n): + """ + Generates the orthonormal transformation matrix T used in the TUKF sigma point generation. + + Parameters: + n (int): The state dimension. + + Returns: + T (np.ndarray): An n x 2n orthonormal transformation matrix used to generate TUKF sigma points. + """ + T = np.zeros((n, 2 * n)) + + for i in range(1, 2 * n + 1): # indexing matches equation (1, ..., 2n) + for j in range(1, (n // 2) + 1): + T[2 * j - 2, i - 1] = np.sqrt(2) * np.cos(((2 * j - 1) * i * np.pi) / n) + T[2 * j - 1, i - 1] = np.sqrt(2) * np.sin(((2 * j - 1) * i * np.pi) / n) + + if n % 2 == 1: # if n is odd, add the last term as described in the paper + T[n - 1, i - 1] = (-1) ** i + + T = T / np.sqrt(2) # Normalize matrix for orthonormality (unit scaling) + + return T def sigma_points(self, current_state: StateQuat) -> tuple[list[StateQuat], np.ndarray]: """ @@ -134,10 +159,10 @@ def add_quaternion_noise(q, noise_std): x0[3] = 1 x0[7:10] = [0.2, 0.2, 0.2] dt = 0.01 - R = (0.1 / dt) * np.eye(3) + R = (0.01) * np.eye(3) - Q = 0.1 * np.eye(12) - P0 = np.eye(12) * 0.1 + Q = 0.00015 * np.eye(12) + P0 = np.eye(12) * 0.0001 model = process_model() model.dt = 0.01 @@ -156,16 +181,35 @@ def add_quaternion_noise(q, noise_std): model.damping_nonlinear = np.array([0.3, 0.3, 0.3, 0.3, 0.3, 0.3]) model.added_mass = np.diag([1.0, 1.0, 1.0, 2.0, 2.0, 2.0]) - model_ukf = model + model_ukf = process_model() + model_ukf.dt = 0.01 + model_ukf.mass_interia_matrix = np.array([ + [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] + ]) + model_ukf.m = 30.0 + model_ukf.r_b_bg = np.array([0.01, 0.0, 0.02]) + model_ukf.inertia = np.diag([0.68, 3.32, 3.34]) + model_ukf.damping_linear = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) + model_ukf.damping_nonlinear = np.array([0.3, 0.3, 0.3, 0.3, 0.3, 0.3]) + model_ukf.added_mass = np.diag([1.0, 1.0, 1.0, 2.0, 2.0, 2.0]) # Simulation parameters - simulation_time = 40 # seconds + simulation_time = 20 # seconds num_steps = int(simulation_time / dt) # Initialize a dummy StateQuat. - test_state = StateQuat() - test_state.fill_states(x0) - test_state.covariance = P0 + new_state = StateQuat() + new_state.fill_states(x0) + new_state.covariance = P0 + + test_state_x = StateQuat() + test_state_x.fill_states(x0) + test_state_x.covariance = P0 # Initialize a estimated state estimated_state = StateQuat() @@ -196,18 +240,15 @@ def add_quaternion_noise(q, noise_std): # Initialize the okid params okid_params = np.zeros((num_steps, 21)) - model.state_vector_prev = test_state - model.state_vector = test_state + model.state_vector_prev = new_state + model.state_vector = new_state - model_ukf.state_vector_prev = test_state - model_ukf.state_vector = test_state + model_ukf.state_vector_prev = test_state_x + model_ukf.state_vector = test_state_x # initialize the ukf ukf = UKF(model_ukf, x0, P0, Q, R) - # Test - ukf.unscented_transform(test_state) - elapsed_times = [] u = lambda t: np.array([2 * np.sin(1 * t), 2 * np.sin(1 * t), 2 * np.sin(1 * t), 0.2 * np.cos(1 * t), 0.2 * np.cos(1 * t), 0.2 * np.cos(1 * t)]) @@ -219,22 +260,22 @@ def add_quaternion_noise(q, noise_std): model_ukf.Control_input = u(step * dt) # Perform the unscented transform - model.model_prediction(test_state) + model.model_prediction(new_state) new_state = model.euler_forward() # Adding noise in the state vector - noisy_state.position = new_state.position + np.random.normal(0, 0.1, 3) - noisy_state.orientation = add_quaternion_noise(new_state.orientation, 0.1) - noisy_state.velocity = new_state.velocity + np.random.normal(0, 0.1, 3) - noisy_state.angular_velocity = new_state.angular_velocity + np.random.normal(0, 0.1, 3) + estimated_state.position = estimated_state.position # + np.random.normal(0, 0.01, 3) + estimated_state.orientation = estimated_state.orientation #add_quaternion_noise(estimated_state.orientation, 0.01) + estimated_state.velocity = estimated_state.velocity # + np.random.normal(0, 0.01, 3) + estimated_state.angular_velocity = estimated_state.angular_velocity # + np.random.normal(0, 0.01, 3) start_time = time.time() - estimated_state = ukf.unscented_transform(noisy_state) + estimated_state = ukf.unscented_transform(estimated_state) elapsed_time = time.time() - start_time elapsed_times.append(elapsed_time) - if step % 20 == 0: - measurment_model.measurement = new_state.velocity + np.random.normal(0, 0.2, 3) + if step % 10 == 0: + measurment_model.measurement = new_state.velocity # + np.random.normal(0, 0.01, 3) meas_update, covariance_matrix = ukf.measurement_update(estimated_state, measurment_model) estimated_state = ukf.posteriori_estimate(estimated_state, covariance_matrix, measurment_model, meas_update) From 3861aab8f8c69553c752fe44d513ad0ddf7ab970 Mon Sep 17 00:00:00 2001 From: Talha Nauman Choudhry Date: Fri, 14 Mar 2025 23:17:19 +0100 Subject: [PATCH 088/290] added test script --- .../eskf_python/eskf_python/eskf_test.py | 235 ++++++++++++++++++ 1 file changed, 235 insertions(+) create mode 100644 navigation/eskf_python/eskf_python/eskf_test.py diff --git a/navigation/eskf_python/eskf_python/eskf_test.py b/navigation/eskf_python/eskf_python/eskf_test.py new file mode 100644 index 000000000..4623b7b86 --- /dev/null +++ b/navigation/eskf_python/eskf_python/eskf_test.py @@ -0,0 +1,235 @@ + +from eskf_python_class import StateEuler, StateQuat, MeasurementModel, Measurement +import numpy as np +from eskf_python_utils import skew_matrix, quaternion_product, R_from_angle_axis, angle_axis_to_quaternion +from ukf_okid_class import process_model, quat_to_euler, euler_to_quat +from ukf_okid_class import StateQuat as StateQuatmodel +from scipy.linalg import block_diag +import matplotlib.pyplot as plt +from eskf_python_filter import ESKF + + +def fancy_print_state_quat(state: StateQuat) -> None: + print("Nominal State (Quaternion):") + print(f" Position : {np.array2string(state.position, precision=3, separator=', ')}") + print(f" Velocity : {np.array2string(state.velocity, precision=3, separator=', ')}") + print(f" Orientation (Quat): {np.array2string(state.orientation, precision=3, separator=', ')}") + print(f" Acceleration Bias : {np.array2string(state.acceleration_bias, precision=3, separator=', ')}") + print(f" Gyro Bias : {np.array2string(state.gyro_bias, precision=3, separator=', ')}") + print(f" Gravity : {np.array2string(state.g, precision=3, separator=', ')}\n") + + +def fancy_print_state_euler(state: StateEuler) -> None: + print("Error State (Euler):") + print(f" Position Error : {np.array2string(state.position, precision=3, separator=', ')}") + print(f" Velocity Error : {np.array2string(state.velocity, precision=3, separator=', ')}") + print(f" Orientation Error : {np.array2string(state.orientation, precision=3, separator=', ')}") + print(f" Acceleration Bias Error: {np.array2string(state.acceleration_bias, precision=3, separator=', ')}") + print(f" Gyro Bias Error : {np.array2string(state.gyro_bias, precision=3, separator=', ')}") + print(f" Gravity Error : {np.array2string(state.g, precision=3, separator=', ')}\n") + +def fancy_print_matrix(matrix: np.ndarray) -> None: + print(f"Matrix shape: {matrix.shape}") + print("========== Matrix ==========") + for row in matrix: + print(" ".join(f"{value:8.3f}" for value in row)) + print("======== End Matrix ========") + +if __name__ == "__main__": + + # Simulation parameters + simulation_time = 20.0 # seconds + dt = 0.01 + num_steps = int(simulation_time / dt) + time = np.linspace(0, simulation_time, num_steps) + + # ----------------------- Setup Initial States, Filter & Model ----------------------- + # True initial state + true_state_init = StateQuat() + true_state_init.position = np.array([0.1, 0.0, 0.0]) + true_state_init.velocity = np.array([0.1, 0.0, 0.0]) + P0 = np.diag([ + 1.0, 1.0, 1.0, # Position + 0.2, 0.2, 0.2, # Velocity + 0.01, 0.01, 0.01, # Orientation + 0.00001, 0.00001, 0.00001, # Acceleration bias + 0.00001, 0.00001, 0.00001, # Gyro bias + 0.00001, 0.00001, 0.00001 # Gravity + ]) + + # Estimated initial state (for filter) + est_state_init = StateQuat() + est_state_init.position = np.array([0.1, 0.0, 0.0]) + est_state_init.velocity = np.array([0.1, 0.0, 0.0]) + + # Noise parameters + Q = np.diag([ + (0.02**2) / dt, (0.02**2) / dt, (0.02**2) / dt, # Accelerometer noise + (0.001**2) / dt, (0.001**2) / dt, (0.001**2) / dt, # Gyroscope noise + 0.0001, 0.0001, 0.0001, # Acceleration bias random walk + 0.00001, 0.00001, 0.00001 # Gyro bias random walk + ]) + + Hx = np.zeros((3, 19)) + Hx[0:3, 6:9] = np.eye(3) + + # Create filter object + eskf = ESKF(Q, P0, Hx, true_state_init, 1e-13, 1e-13, dt) + + imu_data = Measurement() + dvl_data = Measurement() + dvl_data.aiding_covariance = np.eye(3) * 0.2 + + # Setup the process model + model = process_model() + model.dt = dt + model.mass_interia_matrix = np.array([ + [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] + ]) + model.m = 30.0 + model.r_b_bg = np.array([0.01, 0.0, 0.02]) + model.inertia = np.diag([0.68, 3.32, 3.34]) + model.damping_linear = np.diag([0.03, 0.03, 0.03, 0.03, 0.03, 0.03]) + + # Initialize a dummy state for simulation dynamics. + new_state = StateQuatmodel() + new_state.position = np.array([0.1, 0.0, 0.0]) + new_state.velocity = np.array([0.1, 0.0, 0.0]) + new_state_prev = StateQuatmodel() + new_state_prev.position = np.array([0.1, 0.0, 0.0]) + new_state_prev.velocity = np.array([0.1, 0.0, 0.0]) + + model.state_vector = new_state + model.state_vector_prev = new_state_prev + + # ----------------------- Data Storage Arrays ----------------------- + true_positions = np.zeros((num_steps, 3)) + true_orientations = np.zeros((num_steps, 3)) + true_velocities = np.zeros((num_steps, 3)) + + est_positions = np.zeros((num_steps, 3)) + est_orientations = np.zeros((num_steps, 3)) + est_velocities = np.zeros((num_steps, 3)) + + # We'll record the filter’s covariance diagonal for each state component. + pos_cov = np.zeros((num_steps, 3)) # covariance for position (indices 0:3) + vel_cov = np.zeros((num_steps, 3)) # covariance for velocity (indices 3:6) + ori_cov = np.zeros((num_steps, 3)) # covariance for orientation (indices 6:9) + + prev_velocity = np.zeros(3) + u = lambda t: np.array([ + 0.5 * np.sin(0.1 * t), + 0.5 * np.sin(0.1 * t + 0.3), + 0.5 * np.sin(0.1 * t + 0.6), + 0.05 * np.cos(0.1 * t), + 0.05 * np.cos(0.1 * t + 0.3), + 0.05 * np.cos(0.1 * t + 0.6) + ]) + + # ----------------------- Simulation Loop ----------------------- + for step in range(num_steps): + t = step * dt + + model.Control_input = u(t) + model.model_prediction(new_state) + new_state = model.euler_forward() + + # Simulate IMU measurements (with noise) + imu_data.acceleration = ((new_state.velocity - prev_velocity) / dt) + np.random.normal(0, 0.13, 3) + imu_data.angular_velocity = new_state.angular_velocity + np.random.normal(0, 0.13, 3) + + eskf.imu_update(imu_data) + + # DVL update every 20 time-steps + if step % 20 == 0: + dvl_data.aiding = new_state.velocity + np.random.normal(0, 0.01, 3) + eskf.dvl_update(dvl_data) + + # Store True data (from the simulated dynamics) + true_positions[step, :] = np.copy(new_state.position) + true_orientations[step, :] = quat_to_euler(np.copy(new_state.orientation)) + true_velocities[step, :] = np.copy(new_state.velocity) + + # Store estimated state (from the filter) + est_positions[step, :] = np.copy(eskf.nom_state.position) + est_orientations[step, :] = quat_to_euler(np.copy(eskf.nom_state.orientation)) + est_velocities[step, :] = np.copy(eskf.nom_state.velocity) + + # Record covariance diagonal (assumed ordering: pos (0:3), vel (3:6), orientation (6:9)) + P_diag = np.diag(eskf.error_state.covariance) + pos_cov[step, :] = P_diag[0:3] + vel_cov[step, :] = P_diag[3:6] + ori_cov[step, :] = P_diag[6:9] + + prev_velocity = new_state.velocity + model.state_vector_prev = new_state + + # ----------------------- New Plotting Scheme ----------------------- + # Create 3 separate figures, each corresponding to one degree of freedom: + # For position and velocity: X, Y, Z. + # For orientation: Roll, Pitch, Yaw. + axis_labels_pos = ["X", "Y", "Z"] + axis_labels_vel = ["X", "Y", "Z"] + axis_labels_ori = ["Roll", "Pitch", "Yaw"] + + # Plot Position + fig_pos, axs_pos = plt.subplots(3, 1, figsize=(10, 12)) + fig_pos.suptitle("True Data vs Filter Estimates for Position") + for i in range(3): + ax_pos = axs_pos[i] + ax_pos.plot(time, true_positions[:, i], label=f"True Pos {axis_labels_pos[i]}", color=f"C{i}", linestyle='-') + ax_pos.plot(time, est_positions[:, i], label=f"Est Pos {axis_labels_pos[i]}", color=f"C{i}", linestyle='--') + sigma_pos = np.sqrt(pos_cov[:, i]) + ax_pos.fill_between(time, est_positions[:, i] - sigma_pos, est_positions[:, i] + sigma_pos, + color=f"C{i}", alpha=0.2) + ax_pos.set_title(f"Position [{axis_labels_pos[i]}] [m]") + ax_pos.set_xlabel("Time [s]") + ax_pos.set_ylabel("Position") + ax_pos.grid(True) + ax_pos.legend() + + plt.tight_layout(rect=[0, 0, 1, 0.96]) + plt.show() + + # Plot Velocity + fig_vel, axs_vel = plt.subplots(3, 1, figsize=(10, 12)) + fig_vel.suptitle("True Data vs Filter Estimates for Velocity") + for i in range(3): + ax_vel = axs_vel[i] + ax_vel.plot(time, true_velocities[:, i], label=f"True Vel {axis_labels_vel[i]}", color=f"C{i}", linestyle='-') + ax_vel.plot(time, est_velocities[:, i], label=f"Est Vel {axis_labels_vel[i]}", color=f"C{i}", linestyle='--') + sigma_vel = np.sqrt(vel_cov[:, i]) + ax_vel.fill_between(time, est_velocities[:, i] - sigma_vel, est_velocities[:, i] + sigma_vel, + color=f"C{i}", alpha=0.2) + ax_vel.set_title(f"Velocity [{axis_labels_vel[i]}] [m/s]") + ax_vel.set_xlabel("Time [s]") + ax_vel.set_ylabel("Velocity") + ax_vel.grid(True) + ax_vel.legend() + + plt.tight_layout(rect=[0, 0, 1, 0.96]) + plt.show() + + # Plot Orientation + fig_ori, axs_ori = plt.subplots(3, 1, figsize=(10, 12)) + fig_ori.suptitle("True Data vs Filter Estimates for Orientation") + for i in range(3): + ax_ori = axs_ori[i] + ax_ori.plot(time, true_orientations[:, i], label=f"True Ori {axis_labels_ori[i]}", color=f"C{i}", linestyle='-') + ax_ori.plot(time, est_orientations[:, i], label=f"Est Ori {axis_labels_ori[i]}", color=f"C{i}", linestyle='--') + sigma_ori = np.sqrt(ori_cov[:, i]) + ax_ori.fill_between(time, est_orientations[:, i] - sigma_ori, est_orientations[:, i] + sigma_ori, + color=f"C{i}", alpha=0.2) + ax_ori.set_title(f"Orientation [{axis_labels_ori[i]}] [rad]") + ax_ori.set_xlabel("Time [s]") + ax_ori.set_ylabel("Orientation") + ax_ori.grid(True) + ax_ori.legend() + + plt.tight_layout(rect=[0, 0, 1, 0.96]) + plt.show() From 2ede07e8f2c8e636951bd7ac4bd07445d3239fbf Mon Sep 17 00:00:00 2001 From: Talha Nauman Choudhry Date: Sat, 15 Mar 2025 00:02:45 +0100 Subject: [PATCH 089/290] fixed some errors, code runs from eskf_test now --- .../eskf_python/eskf_python_class.py | 28 -- .../eskf_python/eskf_python_filter.py | 22 +- .../eskf_python/eskf_python_utils.py | 53 ++ .../eskf_python/eskf_python/eskf_test.py | 83 +--- .../eskf_python/eskf_test_utils.py | 184 +++++++ .../eskf_python/eskf_python/ukf_okid_class.py | 464 ------------------ 6 files changed, 272 insertions(+), 562 deletions(-) create mode 100644 navigation/eskf_python/eskf_python/eskf_test_utils.py delete mode 100644 navigation/eskf_python/eskf_python/ukf_okid_class.py diff --git a/navigation/eskf_python/eskf_python/eskf_python_class.py b/navigation/eskf_python/eskf_python/eskf_python_class.py index 949ab996d..c2ffc8e02 100644 --- a/navigation/eskf_python/eskf_python/eskf_python_class.py +++ b/navigation/eskf_python/eskf_python/eskf_python_class.py @@ -1,8 +1,5 @@ from dataclasses import dataclass, field -from typing import Tuple, List -from scipy.linalg import expm import numpy as np -from eskf_python_utils import skew_matrix, quaternion_product @dataclass @@ -85,31 +82,6 @@ def R_q(self) -> np.ndarray: return R - # def inject(self, EulerState: 'StateEuler') -> 'StateQuat': - # inj_state = StateQuat() - - # # Injecting the error state - # inj_state.position = self.position + EulerState.position - # inj_state.velocity = self.velocity + EulerState.velocity - # inj_state.orientation = quaternion_product( - # self.orientation, - # 0.5 - # * np.array( - # [ - # 2, - # EulerState.orientation[0], - # EulerState.orientation[1], - # EulerState.orientation[2], - # ] - # ), - # ) - # inj_state.acceleration_bias = self.acceleration_bias + EulerState.acceleration_bias - # inj_state.gyro_bias = self.gyro_bias + EulerState.gyro_bias - - # return inj_state - - - @dataclass class StateEuler: position: np.ndarray = field( diff --git a/navigation/eskf_python/eskf_python/eskf_python_filter.py b/navigation/eskf_python/eskf_python/eskf_python_filter.py index a9c772e47..b1efeb0a9 100644 --- a/navigation/eskf_python/eskf_python/eskf_python_filter.py +++ b/navigation/eskf_python/eskf_python/eskf_python_filter.py @@ -4,9 +4,9 @@ import numpy as np from scipy.linalg import expm from eskf_python_class import StateEuler, StateQuat, Measurement -from eskf_python_utils import skew_matrix, quaternion_product, R_from_angle_axis, angle_axis_to_quaternion -from ukf_okid_class import euler_to_quat +from eskf_python_utils import skew_matrix, quaternion_product, R_from_angle_axis, angle_axis_to_quaternion, euler_to_quat from scipy.linalg import block_diag +from scipy.spatial.transform import Rotation as R_scipy class ESKF: def __init__(self, Q: np.ndarray, P0, Hx, nom_state: StateQuat, p_accBias, p_gyroBias, dt): @@ -103,8 +103,6 @@ def h(self) -> np.ndarray: """ return self.nom_state.velocity - - def nominal_state_discrete(self, imu_data: Measurement) -> None: """ Calculates the next nominal state using the discrete-time process model defined in: @@ -196,13 +194,18 @@ def measurement_update(self, dvl_measurement:Measurement) -> None: Args: dvl_measurement (np.ndarray): The DVL measurement. """ - + H = self.H() P = self.error_state.covariance - R= dvl_measurement.aiding_covariance - K = P @ H.T @ np.linalg.inv(H @ P @ H.T + R) - self.error_state.fill_states(K @ (dvl_measurement.aiding - self.h())) - self.error_state.covariance = (np.eye(18) - K @ H) @ P + R = dvl_measurement.aiding_covariance + + S = H @ P @ H.T + R + K = P @ H.T @ np.linalg.inv(S) + innovation = dvl_measurement.aiding - self.h() + self.error_state.fill_states(K @ innovation) + + I_KH = np.eye(18) - K @ H + self.error_state.covariance = I_KH @ P @ I_KH.T + K @ R @ K.T # Joseph form for more stability def injection(self) -> None: """ @@ -211,7 +214,6 @@ def injection(self) -> None: Chapter 6.2 eq. 282-283 """ - self.nom_state.position = self.nom_state.position + self.error_state.position self.nom_state.velocity = self.nom_state.velocity + self.error_state.velocity self.nom_state.orientation = quaternion_product(self.nom_state.orientation, euler_to_quat(self.error_state.orientation)) diff --git a/navigation/eskf_python/eskf_python/eskf_python_utils.py b/navigation/eskf_python/eskf_python/eskf_python_utils.py index 9ea439982..aaef5f8d6 100644 --- a/navigation/eskf_python/eskf_python/eskf_python_utils.py +++ b/navigation/eskf_python/eskf_python/eskf_python_utils.py @@ -12,6 +12,14 @@ def skew_matrix(vector: np.ndarray) -> np.ndarray: ] ) +def quat_norm(quat: np.ndarray) -> np.ndarray: + """ + Function that normalizes a quaternion + """ + quat = quat / np.linalg.norm(quat) + + return quat + def quaternion_product(q1: np.ndarray, q2: np.ndarray) -> np.ndarray: """Calculates the quaternion super product of two quaternions. @@ -37,6 +45,17 @@ def quaternion_product(q1: np.ndarray, q2: np.ndarray) -> np.ndarray: return q_new +def quaternion_error(quat_1: np.ndarray, quat_2: np.ndarray) -> np.ndarray: + """ + Calculates the error between two quaternions + """ + + quat_2_inv = np.array([quat_2[0], -quat_2[1], -quat_2[2], -quat_2[3]]) + + error_quat = quaternion_product(quat_1, quat_2_inv) + + return error_quat + def angle_axis_to_quaternion(vector: np.ndarray) -> np.ndarray: """Converts an angle-axis representation to a quaternion. @@ -93,3 +112,37 @@ def R_from_angle_axis(vector: np.ndarray) -> np.ndarray: ) return R + +def euler_to_quat(euler_angles: np.ndarray) -> np.ndarray: + """ + Converts Euler angles to a quaternion + """ + psi, theta, phi = euler_angles + c_psi = np.cos(psi / 2) + s_psi = np.sin(psi / 2) + c_theta = np.cos(theta / 2) + s_theta = np.sin(theta / 2) + c_phi = np.cos(phi / 2) + s_phi = np.sin(phi / 2) + + quat = np.array([ + c_psi * c_theta * c_phi + s_psi * s_theta * s_phi, + c_psi * c_theta * s_phi - s_psi * s_theta * c_phi, + s_psi * c_theta * s_phi + c_psi * s_theta * c_phi, + s_psi * c_theta * c_phi - c_psi * s_theta * s_phi + ]) + + return quat + +def quat_to_euler(quat: np.ndarray) -> np.ndarray: + """ + Converts a quaternion to Euler angles + """ + nu, eta_1, eta_2, eta_3 = quat + + phi = np.arctan2(2*(eta_2 * eta_3 + nu * eta_1), 1 - 2 * (eta_1 ** 2 + eta_2 ** 2)) + theta = -np.arcsin(2 * (eta_1 * eta_3 - nu * eta_2)) + psi = np.arctan2(2 * (nu * eta_3 + eta_1 * eta_2), 1 - 2 * (eta_2 ** 2 + eta_3 ** 2)) + + return np.array([phi, theta, psi]) + diff --git a/navigation/eskf_python/eskf_python/eskf_test.py b/navigation/eskf_python/eskf_python/eskf_test.py index 4623b7b86..bf4cc8f8b 100644 --- a/navigation/eskf_python/eskf_python/eskf_test.py +++ b/navigation/eskf_python/eskf_python/eskf_test.py @@ -1,40 +1,11 @@ from eskf_python_class import StateEuler, StateQuat, MeasurementModel, Measurement +from eskf_python_utils import quat_to_euler +from eskf_test_utils import process_model, StateQuatModel import numpy as np -from eskf_python_utils import skew_matrix, quaternion_product, R_from_angle_axis, angle_axis_to_quaternion -from ukf_okid_class import process_model, quat_to_euler, euler_to_quat -from ukf_okid_class import StateQuat as StateQuatmodel -from scipy.linalg import block_diag import matplotlib.pyplot as plt from eskf_python_filter import ESKF - -def fancy_print_state_quat(state: StateQuat) -> None: - print("Nominal State (Quaternion):") - print(f" Position : {np.array2string(state.position, precision=3, separator=', ')}") - print(f" Velocity : {np.array2string(state.velocity, precision=3, separator=', ')}") - print(f" Orientation (Quat): {np.array2string(state.orientation, precision=3, separator=', ')}") - print(f" Acceleration Bias : {np.array2string(state.acceleration_bias, precision=3, separator=', ')}") - print(f" Gyro Bias : {np.array2string(state.gyro_bias, precision=3, separator=', ')}") - print(f" Gravity : {np.array2string(state.g, precision=3, separator=', ')}\n") - - -def fancy_print_state_euler(state: StateEuler) -> None: - print("Error State (Euler):") - print(f" Position Error : {np.array2string(state.position, precision=3, separator=', ')}") - print(f" Velocity Error : {np.array2string(state.velocity, precision=3, separator=', ')}") - print(f" Orientation Error : {np.array2string(state.orientation, precision=3, separator=', ')}") - print(f" Acceleration Bias Error: {np.array2string(state.acceleration_bias, precision=3, separator=', ')}") - print(f" Gyro Bias Error : {np.array2string(state.gyro_bias, precision=3, separator=', ')}") - print(f" Gravity Error : {np.array2string(state.g, precision=3, separator=', ')}\n") - -def fancy_print_matrix(matrix: np.ndarray) -> None: - print(f"Matrix shape: {matrix.shape}") - print("========== Matrix ==========") - for row in matrix: - print(" ".join(f"{value:8.3f}" for value in row)) - print("======== End Matrix ========") - if __name__ == "__main__": # Simulation parameters @@ -51,23 +22,18 @@ def fancy_print_matrix(matrix: np.ndarray) -> None: P0 = np.diag([ 1.0, 1.0, 1.0, # Position 0.2, 0.2, 0.2, # Velocity - 0.01, 0.01, 0.01, # Orientation + 0.2, 0.2, 0.2, # Orientation 0.00001, 0.00001, 0.00001, # Acceleration bias 0.00001, 0.00001, 0.00001, # Gyro bias 0.00001, 0.00001, 0.00001 # Gravity ]) - # Estimated initial state (for filter) - est_state_init = StateQuat() - est_state_init.position = np.array([0.1, 0.0, 0.0]) - est_state_init.velocity = np.array([0.1, 0.0, 0.0]) - # Noise parameters Q = np.diag([ - (0.02**2) / dt, (0.02**2) / dt, (0.02**2) / dt, # Accelerometer noise - (0.001**2) / dt, (0.001**2) / dt, (0.001**2) / dt, # Gyroscope noise - 0.0001, 0.0001, 0.0001, # Acceleration bias random walk - 0.00001, 0.00001, 0.00001 # Gyro bias random walk + (0.05**2) / dt, (0.05**2) / dt, (0.05**2) / dt, # Accelerometer noise + (0.004**2) / dt, (0.004**2) / dt, (0.004**2) / dt, # Gyroscope noise + 0.0002, 0.0002, 0.0002, # Acceleration bias random walk + 0.0001, 0.0001, 0.0001 # Gyro bias random walk ]) Hx = np.zeros((3, 19)) @@ -76,11 +42,14 @@ def fancy_print_matrix(matrix: np.ndarray) -> None: # Create filter object eskf = ESKF(Q, P0, Hx, true_state_init, 1e-13, 1e-13, dt) + # Create measurement objects imu_data = Measurement() dvl_data = Measurement() - dvl_data.aiding_covariance = np.eye(3) * 0.2 - # Setup the process model + # R matrix for DVL aiding + dvl_data.aiding_covariance = np.diag([(0.5)**2, (0.5)**2, (0.5)**2]) + + # Setup the process model for simulation of AUV model = process_model() model.dt = dt model.mass_interia_matrix = np.array([ @@ -97,17 +66,19 @@ def fancy_print_matrix(matrix: np.ndarray) -> None: model.damping_linear = np.diag([0.03, 0.03, 0.03, 0.03, 0.03, 0.03]) # Initialize a dummy state for simulation dynamics. - new_state = StateQuatmodel() + # Two where made since there seems to be an issue with declaring two identical objects. + new_state = StateQuatModel() new_state.position = np.array([0.1, 0.0, 0.0]) new_state.velocity = np.array([0.1, 0.0, 0.0]) - new_state_prev = StateQuatmodel() + + new_state_prev = StateQuatModel() new_state_prev.position = np.array([0.1, 0.0, 0.0]) new_state_prev.velocity = np.array([0.1, 0.0, 0.0]) model.state_vector = new_state model.state_vector_prev = new_state_prev - # ----------------------- Data Storage Arrays ----------------------- + # Initialize arrays to store true and estimated states true_positions = np.zeros((num_steps, 3)) true_orientations = np.zeros((num_steps, 3)) true_velocities = np.zeros((num_steps, 3)) @@ -116,10 +87,10 @@ def fancy_print_matrix(matrix: np.ndarray) -> None: est_orientations = np.zeros((num_steps, 3)) est_velocities = np.zeros((num_steps, 3)) - # We'll record the filter’s covariance diagonal for each state component. - pos_cov = np.zeros((num_steps, 3)) # covariance for position (indices 0:3) - vel_cov = np.zeros((num_steps, 3)) # covariance for velocity (indices 3:6) - ori_cov = np.zeros((num_steps, 3)) # covariance for orientation (indices 6:9) + # covariance arrays + pos_cov = np.zeros((num_steps, 3)) + vel_cov = np.zeros((num_steps, 3)) + ori_cov = np.zeros((num_steps, 3)) prev_velocity = np.zeros(3) u = lambda t: np.array([ @@ -131,7 +102,7 @@ def fancy_print_matrix(matrix: np.ndarray) -> None: 0.05 * np.cos(0.1 * t + 0.6) ]) - # ----------------------- Simulation Loop ----------------------- + # Sim for step in range(num_steps): t = step * dt @@ -139,28 +110,23 @@ def fancy_print_matrix(matrix: np.ndarray) -> None: model.model_prediction(new_state) new_state = model.euler_forward() - # Simulate IMU measurements (with noise) imu_data.acceleration = ((new_state.velocity - prev_velocity) / dt) + np.random.normal(0, 0.13, 3) imu_data.angular_velocity = new_state.angular_velocity + np.random.normal(0, 0.13, 3) eskf.imu_update(imu_data) - # DVL update every 20 time-steps if step % 20 == 0: dvl_data.aiding = new_state.velocity + np.random.normal(0, 0.01, 3) eskf.dvl_update(dvl_data) - # Store True data (from the simulated dynamics) true_positions[step, :] = np.copy(new_state.position) true_orientations[step, :] = quat_to_euler(np.copy(new_state.orientation)) true_velocities[step, :] = np.copy(new_state.velocity) - # Store estimated state (from the filter) est_positions[step, :] = np.copy(eskf.nom_state.position) est_orientations[step, :] = quat_to_euler(np.copy(eskf.nom_state.orientation)) est_velocities[step, :] = np.copy(eskf.nom_state.velocity) - # Record covariance diagonal (assumed ordering: pos (0:3), vel (3:6), orientation (6:9)) P_diag = np.diag(eskf.error_state.covariance) pos_cov[step, :] = P_diag[0:3] vel_cov[step, :] = P_diag[3:6] @@ -169,10 +135,7 @@ def fancy_print_matrix(matrix: np.ndarray) -> None: prev_velocity = new_state.velocity model.state_vector_prev = new_state - # ----------------------- New Plotting Scheme ----------------------- - # Create 3 separate figures, each corresponding to one degree of freedom: - # For position and velocity: X, Y, Z. - # For orientation: Roll, Pitch, Yaw. + # Plotting axis_labels_pos = ["X", "Y", "Z"] axis_labels_vel = ["X", "Y", "Z"] axis_labels_ori = ["Roll", "Pitch", "Yaw"] diff --git a/navigation/eskf_python/eskf_python/eskf_test_utils.py b/navigation/eskf_python/eskf_python/eskf_test_utils.py new file mode 100644 index 000000000..34abe8eda --- /dev/null +++ b/navigation/eskf_python/eskf_python/eskf_test_utils.py @@ -0,0 +1,184 @@ +import numpy as np +from dataclasses import dataclass, field +from typing import Tuple +from eskf_python_utils import quaternion_product, euler_to_quat, quat_to_euler, quaternion_error, quat_norm, skew_matrix + +# This was the original code from the ukf_okid.py file + +@dataclass +class StateQuatModel: + """ + A class to represent the state to be estimated by the UKF. + """ + position: np.ndarray = field(default_factory=lambda: np.zeros(3)) + orientation: np.ndarray = field(default_factory=lambda: np.array([1, 0, 0, 0])) + velocity: np.ndarray = field(default_factory=lambda: np.zeros(3)) + angular_velocity: np.ndarray = field(default_factory=lambda: np.zeros(3)) + covariance: np.ndarray = field(default_factory=lambda: np.zeros((12, 12))) + + def as_vector(self) -> np.ndarray: + """Returns the StateVector as a numpy array.""" + return np.concatenate([self.position, self.orientation, self.velocity, self.angular_velocity]) + + def nu(self) -> np.ndarray: + """Calculates the nu vector.""" + return np.concatenate([self.velocity, self.angular_velocity]) + + def R_q(self) -> np.ndarray: + """Calculates the rotation matrix from the orientation quaternion.""" + q0, q1, q2, q3 = self.orientation + R = np.array([ + [1 - 2 * q2**2 - 2 * q3**2, 2 * (q1 * q2 - q0 * q3), 2 * (q0 * q2 + q1 * q3)], + [2 * (q1 * q2 + q0 * q3), 1 - 2 * q1**2 - 2 * q3**2, 2 * (q2 * q3 - q0 * q1)], + [2 * (q1 * q3 - q0 * q2), 2 * (q0 * q1 + q2 * q3), 1 - 2 * q1**2 - 2 * q2**2] + ]) + return R + + def fill_states(self, state: np.ndarray) -> None: + """Fills the state vector with the values from a numpy array.""" + self.position = state[0:3] + self.orientation = state[3:7] + self.velocity = state[7:10] + self.angular_velocity = state[10:13] + + def fill_states_different_dim(self, state: np.ndarray, state_euler: np.ndarray) -> None: + """Fills states when the state vector has different dimensions than the default state vector.""" + self.position = state[0:3] + state_euler[0:3] + self.orientation = quaternion_product(state[3:7], euler_to_quat(state_euler[3:6])) + self.velocity = state[7:10] + state_euler[6:9] + self.angular_velocity = state[10:13] + state_euler[9:12] + + def subtract(self, other: 'StateQuatModel') -> np.ndarray: + """Subtracts two StateQuatModel objects, returning the difference with Euler angles.""" + new_array = np.zeros(len(self.as_vector()) - 1) + new_array[:3] = self.position - other.position + new_array[3:6] = quat_to_euler(quaternion_error(self.orientation, other.orientation)) + new_array[6:9] = self.velocity - other.velocity + new_array[9:12] = self.angular_velocity - other.angular_velocity + + return new_array + + def __add__(self, other: 'StateQuatModel') -> 'StateQuatModel': + """Adds two StateQuatModel objects.""" + new_state = StateQuatModel() + new_state.position = self.position + other.position + new_state.orientation = quaternion_product(self.orientation, other.orientation) + new_state.velocity = self.velocity + other.velocity + new_state.angular_velocity = self.angular_velocity + other.angular_velocity + + return new_state + + def __sub__(self, other: 'StateQuatModel') -> 'StateQuatModel': + """Subtracts two StateQuatModel objects.""" + new_state = StateQuatModel() + new_state.position = self.position - other.position + new_state.orientation = quaternion_error(self.orientation, other.orientation) + new_state.velocity = self.velocity - other.velocity + new_state.angular_velocity = self.angular_velocity - other.angular_velocity + + return new_state.as_vector() + + def __rmul__(self, scalar: float) -> 'StateQuatModel': + """Multiplies the StateQuatModel object by a scalar.""" + new_state = StateQuatModel() + new_state.position = scalar * self.position + new_state.orientation = quat_norm(scalar * self.orientation) + new_state.velocity = scalar * self.velocity + new_state.angular_velocity = scalar * self.angular_velocity + + return new_state + + def insert_weights(self, weights: np.ndarray) -> np.ndarray: + """Inserts the weights into the covariance matrix.""" + new_state = StateQuatModel() + new_state.position = self.position - weights[:3] + new_state.orientation = quaternion_error(self.orientation, euler_to_quat(weights[3:6])) + new_state.velocity = self.velocity - weights[6:9] + new_state.angular_velocity = self.angular_velocity - weights[9:12] + + return new_state.as_vector() + + def add_without_quaternions(self, other: 'StateQuatModel') -> None: + """Adds elements into the state vector without considering the quaternions.""" + self.position += other.position + self.velocity += other.velocity + self.angular_velocity += other.angular_velocity + + +@dataclass +class process_model: + """ + A class defined for a general process model. + """ + state_vector: StateQuatModel = field(default_factory=StateQuatModel) + state_vector_dot: StateQuatModel = field(default_factory=StateQuatModel) + state_vector_prev: StateQuatModel = field(default_factory=StateQuatModel) + Control_input: np.ndarray = field(default_factory=lambda: np.zeros(6)) + mass_interia_matrix: np.ndarray = field(default_factory=lambda: np.zeros((6, 6))) + added_mass: np.ndarray = field(default_factory=lambda: np.zeros(6)) + damping_linear: np.ndarray = field(default_factory=lambda: np.zeros(6)) + damping_nonlinear: np.ndarray = field(default_factory=lambda: np.zeros(6)) + m: float = 0.0 + inertia: np.ndarray = field(default_factory=lambda: np.zeros((3,3))) + r_b_bg: np.ndarray = field(default_factory=lambda: np.zeros(3)) + dt: float = 0.0 + integral_error_position: np.ndarray = field(default_factory=lambda: np.zeros(3)) + integral_error_orientation: np.ndarray = field(default_factory=lambda: np.zeros(4)) + prev_position_error: np.ndarray = field(default_factory=lambda: np.zeros(3)) + prev_orientation_error: np.ndarray = field(default_factory=lambda: np.zeros(3)) + + def R(self) -> np.ndarray: + """Calculates the rotation matrix.""" + nu, e_1, e_2, e_3 = self.state_vector.orientation + R = np.array([ + [1 - 2 * e_2 ** 2 - 2 * e_3 ** 2, 2 * e_1 * e_2 - 2 * nu * e_3, 2 * e_1 * e_3 + 2 * nu * e_2], + [2 * e_1 * e_2 + 2 * nu * e_3, 1 - 2 * e_1 ** 2 - 2 * e_3 ** 2, 2 * e_2 * e_3 - 2 * nu * e_1], + [2 * e_1 * e_3 - 2 * nu * e_2, 2 * e_2 * e_3 + 2 * nu * e_1, 1 - 2 * e_1 ** 2 - 2 * e_2 ** 2] + ]) + return R + + def T(self) -> np.ndarray: + """Calculates the transformation matrix.""" + nu, e_1, e_2, e_3 = self.state_vector.orientation + T = 0.5 * np.array([ + [-e_1, -e_2, -e_3], + [nu, -e_3, e_2], + [e_3, nu, -e_1], + [-e_2, e_1, nu] + ]) + return T + + def Crb(self) -> np.ndarray: + """Calculates the Coriolis matrix.""" + ang_vel = self.state_vector.angular_velocity + ang_vel_skew = skew_matrix(ang_vel) + lever_arm_skew = skew_matrix(self.r_b_bg) + Crb = np.zeros((6, 6)) + Crb[0:3, 0:3] = self.m * ang_vel_skew + Crb[3:6, 3:6] = -skew_matrix(np.dot(self.inertia, ang_vel)) + Crb[0:3, 3:6] = -self.m * np.dot(ang_vel_skew, lever_arm_skew) + Crb[3:6, 0:3] = self.m * np.dot(lever_arm_skew, ang_vel_skew) + return Crb + + def D(self) -> np.ndarray: + """Calculates the damping matrix.""" + D_l = -np.diag(self.damping_linear) + D_nl = -np.diag(self.damping_nonlinear) * np.abs(self.state_vector.nu()) + return D_l + D_nl + + def model_prediction(self, state: StateQuatModel) -> None: + """Calculates the model of the system.""" + self.state_vector = state + self.state_vector_dot.position = np.dot(self.R(), self.state_vector.velocity) + self.state_vector_dot.orientation = np.dot(self.T(), self.state_vector.angular_velocity) + Nu = np.linalg.inv(self.mass_interia_matrix + np.diag(self.added_mass)) @ (self.Control_input - np.dot(self.Crb(), self.state_vector.nu()) - np.dot(self.D(), self.state_vector.nu())) + self.state_vector_dot.velocity = Nu[:3] + self.state_vector_dot.angular_velocity = Nu[3:] + + def euler_forward(self) -> StateQuatModel: + """Calculates the forward Euler integration.""" + self.state_vector.position = self.state_vector_prev.position + self.state_vector_dot.position * self.dt + self.state_vector.orientation = quat_norm(self.state_vector_prev.orientation + self.state_vector_dot.orientation * self.dt) + self.state_vector.velocity = self.state_vector_prev.velocity + self.state_vector_dot.velocity * self.dt + self.state_vector.angular_velocity = self.state_vector_prev.angular_velocity + self.state_vector_dot.angular_velocity * self.dt + return self.state_vector \ No newline at end of file diff --git a/navigation/eskf_python/eskf_python/ukf_okid_class.py b/navigation/eskf_python/eskf_python/ukf_okid_class.py deleted file mode 100644 index 8444fd82e..000000000 --- a/navigation/eskf_python/eskf_python/ukf_okid_class.py +++ /dev/null @@ -1,464 +0,0 @@ -from dataclasses import dataclass, field -import numpy as np - - -from dataclasses import dataclass, field -import numpy as np - -@dataclass -class StateQuat: - """ - A class to represent the state to be estimated by the UKF. - """ - position: np.ndarray = field(default_factory=lambda: np.zeros(3)) - orientation: np.ndarray = field(default_factory=lambda: np.array([1, 0, 0, 0])) - velocity: np.ndarray = field(default_factory=lambda: np.zeros(3)) - angular_velocity: np.ndarray = field(default_factory=lambda: np.zeros(3)) - covariance: np.ndarray = field(default_factory=lambda: np.zeros((12, 12))) - - def as_vector(self) -> np.ndarray: - """Returns the StateVector as a numpy array.""" - return np.concatenate([self.position, self.orientation, self.velocity, self.angular_velocity]) - - def nu(self) -> np.ndarray: - """Calculates the nu vector.""" - return np.concatenate([self.velocity, self.angular_velocity]) - - def R_q(self) -> np.ndarray: - """Calculates the rotation matrix from the orientation quaternion.""" - q0, q1, q2, q3 = self.orientation - R = np.array([ - [1 - 2 * q2**2 - 2 * q3**2, 2 * (q1 * q2 - q0 * q3), 2 * (q0 * q2 + q1 * q3)], - [2 * (q1 * q2 + q0 * q3), 1 - 2 * q1**2 - 2 * q3**2, 2 * (q2 * q3 - q0 * q1)], - [2 * (q1 * q3 - q0 * q2), 2 * (q0 * q1 + q2 * q3), 1 - 2 * q1**2 - 2 * q2**2] - ]) - return R - - def fill_states(self, state: np.ndarray) -> None: - """Fills the state vector with the values from a numpy array.""" - self.position = state[0:3] - self.orientation = state[3:7] - self.velocity = state[7:10] - self.angular_velocity = state[10:13] - - def fill_states_different_dim(self, state: np.ndarray, state_euler: np.ndarray) -> None: - """Fills states when the state vector has different dimensions than the default state vector.""" - self.position = state[0:3] + state_euler[0:3] - self.orientation = quaternion_super_product(state[3:7], euler_to_quat(state_euler[3:6])) - self.velocity = state[7:10] + state_euler[6:9] - self.angular_velocity = state[10:13] + state_euler[9:12] - - def subtract(self, other: 'StateQuat') -> np.ndarray: - """Subtracts two StateQuat objects, returning the difference with Euler angles.""" - new_array = np.zeros(len(self.as_vector()) - 1) - new_array[:3] = self.position - other.position - new_array[3:6] = quat_to_euler(quaternion_error(self.orientation, other.orientation)) - new_array[6:9] = self.velocity - other.velocity - new_array[9:12] = self.angular_velocity - other.angular_velocity - - return new_array - - def __add__(self, other: 'StateQuat') -> 'StateQuat': - """Adds two StateQuat objects.""" - new_state = StateQuat() - new_state.position = self.position + other.position - new_state.orientation = quaternion_super_product(self.orientation, other.orientation) - new_state.velocity = self.velocity + other.velocity - new_state.angular_velocity = self.angular_velocity + other.angular_velocity - - return new_state - - def __sub__(self, other: 'StateQuat') -> 'StateQuat': - """Subtracts two StateQuat objects.""" - new_state = StateQuat() - new_state.position = self.position - other.position - new_state.orientation = quaternion_error(self.orientation, other.orientation) - new_state.velocity = self.velocity - other.velocity - new_state.angular_velocity = self.angular_velocity - other.angular_velocity - - return new_state.as_vector() - - def __rmul__(self, scalar: float) -> 'StateQuat': - """Multiplies the StateQuat object by a scalar.""" - new_state = StateQuat() - new_state.position = scalar * self.position - new_state.orientation = quat_norm(scalar * self.orientation) - new_state.velocity = scalar * self.velocity - new_state.angular_velocity = scalar * self.angular_velocity - - return new_state - - def insert_weights(self, weights: np.ndarray) -> np.ndarray: - """Inserts the weights into the covariance matrix.""" - new_state = StateQuat() - new_state.position = self.position - weights[:3] - new_state.orientation = quaternion_error(self.orientation, euler_to_quat(weights[3:6])) - new_state.velocity = self.velocity - weights[6:9] - new_state.angular_velocity = self.angular_velocity - weights[9:12] - - return new_state.as_vector() - - def add_without_quaternions(self, other: 'StateQuat') -> None: - """Adds elements into the state vector without considering the quaternions.""" - self.position += other.position - self.velocity += other.velocity - self.angular_velocity += other.angular_velocity - -@dataclass -class MeasModel: - """ - A class defined for a general measurement model. - """ - measurement: np.ndarray = field(default_factory=lambda: np.zeros(3)) - covariance: np.ndarray = field(default_factory=lambda: np.zeros((3, 3))) - - def H(self, state: StateQuat) -> 'MeasModel': - """Calculates the measurement matrix.""" - H = np.zeros((3, 13)) - H[:, 7:10] = np.eye(3) - z_i = MeasModel() - z_i.measurement = np.dot(H, state.as_vector()) - return z_i - - def __add__(self, other: 'MeasModel') -> 'MeasModel': - """Defines the addition operation between two MeasModel objects.""" - result = MeasModel() - result.measurement = self.measurement + other.measurement - return result - - def __rmul__(self, scalar: float) -> 'MeasModel': - """Defines multiplication between scalar value and MeasModel object.""" - result = MeasModel() - result.measurement = scalar * self.measurement - return result - - def __sub__(self, other: 'MeasModel') -> 'MeasModel': - """Defines the subtraction between two MeasModel objects.""" - result = MeasModel() - result.measurement = self.measurement - other.measurement - return result - -@dataclass -class process_model: - """ - A class defined for a general process model. - """ - state_vector: StateQuat = field(default_factory=StateQuat) - state_vector_dot: StateQuat = field(default_factory=StateQuat) - state_vector_prev: StateQuat = field(default_factory=StateQuat) - Control_input: np.ndarray = field(default_factory=lambda: np.zeros(6)) - mass_interia_matrix: np.ndarray = field(default_factory=lambda: np.zeros((6, 6))) - added_mass: np.ndarray = field(default_factory=lambda: np.zeros(6)) - damping_linear: np.ndarray = field(default_factory=lambda: np.zeros(6)) - damping_nonlinear: np.ndarray = field(default_factory=lambda: np.zeros(6)) - m: float = 0.0 - inertia: np.ndarray = field(default_factory=lambda: np.zeros((3,3))) - r_b_bg: np.ndarray = field(default_factory=lambda: np.zeros(3)) - dt: float = 0.0 - integral_error_position: np.ndarray = field(default_factory=lambda: np.zeros(3)) - integral_error_orientation: np.ndarray = field(default_factory=lambda: np.zeros(4)) - prev_position_error: np.ndarray = field(default_factory=lambda: np.zeros(3)) - prev_orientation_error: np.ndarray = field(default_factory=lambda: np.zeros(3)) - - def R(self) -> np.ndarray: - """Calculates the rotation matrix.""" - nu, e_1, e_2, e_3 = self.state_vector.orientation - R = np.array([ - [1 - 2 * e_2 ** 2 - 2 * e_3 ** 2, 2 * e_1 * e_2 - 2 * nu * e_3, 2 * e_1 * e_3 + 2 * nu * e_2], - [2 * e_1 * e_2 + 2 * nu * e_3, 1 - 2 * e_1 ** 2 - 2 * e_3 ** 2, 2 * e_2 * e_3 - 2 * nu * e_1], - [2 * e_1 * e_3 - 2 * nu * e_2, 2 * e_2 * e_3 + 2 * nu * e_1, 1 - 2 * e_1 ** 2 - 2 * e_2 ** 2] - ]) - return R - - def T(self) -> np.ndarray: - """Calculates the transformation matrix.""" - nu, e_1, e_2, e_3 = self.state_vector.orientation - T = 0.5 * np.array([ - [-e_1, -e_2, -e_3], - [nu, -e_3, e_2], - [e_3, nu, -e_1], - [-e_2, e_1, nu] - ]) - return T - - def Crb(self) -> np.ndarray: - """Calculates the Coriolis matrix.""" - ang_vel = self.state_vector.angular_velocity - ang_vel_skew = skew_symmetric(ang_vel) - lever_arm_skew = skew_symmetric(self.r_b_bg) - Crb = np.zeros((6, 6)) - Crb[0:3, 0:3] = self.m * ang_vel_skew - Crb[3:6, 3:6] = -skew_symmetric(np.dot(self.inertia, ang_vel)) - Crb[0:3, 3:6] = -self.m * np.dot(ang_vel_skew, lever_arm_skew) - Crb[3:6, 0:3] = self.m * np.dot(lever_arm_skew, ang_vel_skew) - return Crb - - def D(self) -> np.ndarray: - """Calculates the damping matrix.""" - D_l = -np.diag(self.damping_linear) - D_nl = -np.diag(self.damping_nonlinear) * np.abs(self.state_vector.nu()) - return D_l + D_nl - - def model_prediction(self, state: StateQuat) -> None: - """Calculates the model of the system.""" - self.state_vector = state - self.state_vector_dot.position = np.dot(self.R(), self.state_vector.velocity) - self.state_vector_dot.orientation = np.dot(self.T(), self.state_vector.angular_velocity) - Nu = np.linalg.inv(self.mass_interia_matrix + np.diag(self.added_mass)) @ (self.Control_input - np.dot(self.Crb(), self.state_vector.nu()) - np.dot(self.D(), self.state_vector.nu())) - self.state_vector_dot.velocity = Nu[:3] - self.state_vector_dot.angular_velocity = Nu[3:] - - def euler_forward(self) -> StateQuat: - """Calculates the forward Euler integration.""" - self.state_vector.position = self.state_vector_prev.position + self.state_vector_dot.position * self.dt - self.state_vector.orientation = quat_norm(self.state_vector_prev.orientation + self.state_vector_dot.orientation * self.dt) - self.state_vector.velocity = self.state_vector_prev.velocity + self.state_vector_dot.velocity * self.dt - self.state_vector.angular_velocity = self.state_vector_prev.angular_velocity + self.state_vector_dot.angular_velocity * self.dt - return self.state_vector - -def euler_to_quat(euler_angles: np.ndarray) -> np.ndarray: - """ - Converts Euler angles to a quaternion - """ - psi, theta, phi = euler_angles - c_psi = np.cos(psi / 2) - s_psi = np.sin(psi / 2) - c_theta = np.cos(theta / 2) - s_theta = np.sin(theta / 2) - c_phi = np.cos(phi / 2) - s_phi = np.sin(phi / 2) - - quat = np.array([ - c_psi * c_theta * c_phi + s_psi * s_theta * s_phi, - c_psi * c_theta * s_phi - s_psi * s_theta * c_phi, - s_psi * c_theta * s_phi + c_psi * s_theta * c_phi, - s_psi * c_theta * c_phi - c_psi * s_theta * s_phi - ]) - - return quat - -def quat_to_euler(quat: np.ndarray) -> np.ndarray: - """ - Converts a quaternion to Euler angles - """ - nu, eta_1, eta_2, eta_3 = quat - - phi = np.arctan2(2*(eta_2 * eta_3 + nu * eta_1), 1 - 2 * (eta_1 ** 2 + eta_2 ** 2)) - theta = -np.arcsin(2 * (eta_1 * eta_3 - nu * eta_2)) - psi = np.arctan2(2 * (nu * eta_3 + eta_1 * eta_2), 1 - 2 * (eta_2 ** 2 + eta_3 ** 2)) - - return np.array([phi, theta, psi]) - -def quat_norm(quat: np.ndarray) -> np.ndarray: - """ - Function that normalizes a quaternion - """ - - quat = quat / np.linalg.norm(quat) - - return quat - -def skew_symmetric(vector: np.ndarray) -> np.ndarray: - """Calculates the skew symmetric matrix of a vector. - - Args: - vector (np.ndarray): The vector. - - Returns: - np.ndarray: The skew symmetric matrix. - """ - return np.array( - [ - [0, -vector[2], vector[1]], - [vector[2], 0, -vector[0]], - [-vector[1], vector[0], 0], - ] - ) - -def quaternion_super_product(q1: np.ndarray, q2: np.ndarray) -> np.ndarray: - """Calculates the quaternion super product of two quaternions. - - Args: - q1 (np.ndarray): The first quaternion. - q2 (np.ndarray): The second quaternion. - - Returns: - np.ndarray: The quaternion super product. - """ - eta_0, e_0_x, e_0_y, e_0_z = q1 - eta_1, e_1_x, e_1_y, e_1_z = q2 - - e_0 = np.array([e_0_x, e_0_y, e_0_z]) - e_1 = np.array([e_1_x, e_1_y, e_1_z]) - - eta_new = eta_0 * eta_1 - (e_0_x * e_1_x + e_0_y * e_1_y + e_0_z * e_1_z) - nu_new = e_1 * eta_0 + e_0 * eta_1 + np.dot(skew_symmetric(e_0), e_1) - - q_new = quat_norm(np.array([eta_new, nu_new[0], nu_new[1], nu_new[2]])) - - return q_new - -def quaternion_error(quat_1: np.ndarray, quat_2: np.ndarray) -> np.ndarray: - """ - Calculates the error between two quaternions - """ - - quat_2_inv = np.array([quat_2[0], -quat_2[1], -quat_2[2], -quat_2[3]]) - - error_quat = quaternion_super_product(quat_1, quat_2_inv) - - return error_quat - -def iterative_quaternion_mean_statequat(state_list: list[StateQuat], weights: np.ndarray, tol: float = 1e-6, max_iter: int = 100) -> np.ndarray: - """ - Computes the weighted mean of the quaternion orientations from a list of StateQuat objects - using an iterative approach, without requiring the caller to manually extract the quaternion. - - Parameters: - state_list (list[StateQuat]): List of StateQuat objects. - weights (np.ndarray): Weights for each state. - tol (float): Convergence tolerance. - max_iter (int): Maximum number of iterations. - - Returns: - np.ndarray: The averaged quaternion as a 4-element numpy array. - """ - # Internally extract the quaternion from each state - sigma_quats = [state.orientation for state in state_list] - - # Initialize the mean quaternion with the first quaternion - mean_q = sigma_quats[0].copy() - - for _ in range(max_iter): - weighted_error_vectors = [] - for i, q in enumerate(sigma_quats): - # Compute the error quaternion: e = q * inv(mean_q) - # For unit quaternions, the inverse is the conjugate. - mean_q_conj = np.array([mean_q[0], -mean_q[1], -mean_q[2], -mean_q[3]]) - e = quaternion_super_product(q, mean_q_conj) - - # Clip to avoid numerical issues - e0_clipped = np.clip(e[0], -1.0, 1.0) - angle = 2 * np.arccos(e0_clipped) - if np.abs(angle) < 1e-8: - error_vec = np.zeros(3) - else: - # Compute the full rotation vector (angle * axis) - error_vec = (angle / np.sin(angle / 2)) * e[1:4] - weighted_error_vectors.append(weights[i] * error_vec) - - error_avg = np.sum(weighted_error_vectors, axis=0) - if np.linalg.norm(error_avg) < tol: - break - - error_norm = np.linalg.norm(error_avg) - delta_q = (np.array([np.cos(error_norm / 2), - *(np.sin(error_norm / 2) * (error_avg / error_norm))]) - if error_norm > 0 else np.array([1.0, 0.0, 0.0, 0.0])) - mean_q = quaternion_super_product(delta_q, mean_q) - mean_q = quat_norm(mean_q) - - return mean_q - - - -def mean_set(set_points: list[StateQuat], weights: np.ndarray = None) -> np.ndarray: - """ - Function that calculates the mean of a set of points - """ - n = len(set_points[0].as_vector()) - 1 - mean_value = StateQuat() - - if weights is None: - for i in range(2 * n + 1): - weight_temp_list = (1/ (2 * n + 1)) * np.ones(2 * n + 1) - mean_value.add_without_quaternions(weight_temp_list[i] * set_points[i]) - - mean_value.orientation = iterative_quaternion_mean_statequat(set_points, weight_temp_list) - - else: - for i in range(2 * n + 1): - mean_value.add_without_quaternions(weights[i] * set_points[i]) - - mean_value.orientation = iterative_quaternion_mean_statequat(set_points, weights) - - return mean_value.as_vector() - -def mean_measurement(set_points: list[MeasModel], weights: np.ndarray = None) -> np.ndarray: - """ - Function that calculates the mean of a set of points - """ - n = len(set_points) - mean_value = MeasModel() - - if weights is None: - for i in range(n): - mean_value = mean_value + set_points[i] - else: - for i in range(n): - mean_value = mean_value + (weights[i] * set_points[i]) - - return mean_value.measurement - -def covariance_set(set_points: list[StateQuat], mean: np.ndarray, weights: np.ndarray = None) -> np.ndarray: - """ - Function that calculates the covariance of a set of points - """ - n = len(set_points[0].as_vector()) - 1 - covariance = np.zeros((n, n)) - mean_quat = StateQuat() - mean_quat.fill_states(mean) - - if weights is None: - for i in range(2 * n + 1): - covariance += np.outer(set_points[i].subtract(mean_quat), set_points[i].subtract(mean_quat)) - - covariance = (1 / (2 * n + 1)) * covariance - - else: - for i in range(2 * n + 1): - covariance += weights[i] * np.outer(set_points[i].subtract(mean_quat), set_points[i].subtract(mean_quat)) - - return covariance - -def covariance_measurement(set_points: list[MeasModel], mean: np.ndarray, weights: np.ndarray = None) -> np.ndarray: - """ - Function that calculates the covariance of a set of points - """ - n = len(set_points) - co_size = len(set_points[0].measurement) - covariance = np.zeros((co_size, co_size)) - mean_meas = MeasModel() - mean_meas.measurement = mean - - if weights is None: - for i in range(n): - temp_model = set_points[i] - mean_meas - covariance += np.outer(temp_model.measurement, temp_model.measurement) - - covariance = (1 / (n)) * covariance - - else: - for i in range(n): - temp_model = set_points[i] - mean_meas - covariance += weights[i] * np.outer(temp_model.measurement, temp_model.measurement) - - return covariance - -def cross_covariance(set_y: list[StateQuat], mean_y: np.ndarray, set_z: list[MeasModel], mean_z: np.ndarray, weights: np.ndarray) -> np.ndarray: - """ - Calculates the cross covariance between the measurement and state prediction - """ - - n = len(mean_y) - 1 - m = len(mean_z) - cross_covariance = np.zeros((n,m)) - mean_quat = StateQuat() - mean_quat.fill_states(mean_y) - - for i in range(n): - cross_covariance += np.outer(set_y[i].subtract(mean_quat), set_z[i].measurement - mean_z) - - cross_covariance = (1 / len(set_y)) * cross_covariance - - return cross_covariance From 60e533e79bb39144161fcc7c7c66d951cb1df9d9 Mon Sep 17 00:00:00 2001 From: Talha Nauman Choudhry Date: Thu, 20 Mar 2025 16:29:42 +0100 Subject: [PATCH 090/290] added changes to ukf --- .../eskf_python/eskf_python_class.py | 23 +- .../eskf_python/eskf_python_filter.py | 28 +- .../eskf_python/eskf_python/eskf_test.py | 185 ++++++---- navigation/ukf_okid/ukf_python/rest.py | 37 ++ navigation/ukf_okid/ukf_python/ukf_okid.py | 324 +----------------- .../ukf_okid/ukf_python/ukf_okid_class.py | 143 ++++---- navigation/ukf_okid/ukf_python/ukf_test.py | 323 +++++++++++++++++ navigation/ukf_okid/ukf_python/ukf_utils.py | 2 +- 8 files changed, 614 insertions(+), 451 deletions(-) create mode 100644 navigation/ukf_okid/ukf_python/rest.py create mode 100644 navigation/ukf_okid/ukf_python/ukf_test.py diff --git a/navigation/eskf_python/eskf_python/eskf_python_class.py b/navigation/eskf_python/eskf_python/eskf_python_class.py index c2ffc8e02..0ba96ba1f 100644 --- a/navigation/eskf_python/eskf_python/eskf_python_class.py +++ b/navigation/eskf_python/eskf_python/eskf_python_class.py @@ -1,6 +1,6 @@ from dataclasses import dataclass, field import numpy as np - +from eskf_python_utils import quaternion_error @dataclass class StateQuat: @@ -82,6 +82,27 @@ def R_q(self) -> np.ndarray: return R + def __sub__(self, other: 'StateQuat') -> 'StateQuat': + """Subtracts the values of two state vectors. + + Args: + other (StateQuat): The state vector to subtract. + + Returns: + np.ndarray: The difference between the two state vectors. + """ + result = StateQuat() + result.position = self.position - other.position + result.velocity = self.velocity - other.velocity + result.orientation = quaternion_error(self.orientation, other.orientation) + result.acceleration_bias = self.acceleration_bias - other.acceleration_bias + result.gyro_bias = self.gyro_bias - other.gyro_bias + result.g = self.g - other.g + + return result + + + @dataclass class StateEuler: position: np.ndarray = field( diff --git a/navigation/eskf_python/eskf_python/eskf_python_filter.py b/navigation/eskf_python/eskf_python/eskf_python_filter.py index b1efeb0a9..8c4b01963 100644 --- a/navigation/eskf_python/eskf_python/eskf_python_filter.py +++ b/navigation/eskf_python/eskf_python/eskf_python_filter.py @@ -185,7 +185,7 @@ def error_state_prediction(self, imu_data: Measurement) -> None: self.error_state.covariance = (A_d @ self.error_state.covariance @ A_d.T + GQG_d) - def measurement_update(self, dvl_measurement:Measurement) -> None: + def measurement_update(self, dvl_measurement:Measurement) -> float: """ Updates the error state using the DVL measurement. Joan Solà. Quaternion kinematics for the error-state Kalman filter. @@ -202,11 +202,16 @@ def measurement_update(self, dvl_measurement:Measurement) -> None: S = H @ P @ H.T + R K = P @ H.T @ np.linalg.inv(S) innovation = dvl_measurement.aiding - self.h() + + NIS_value = self.NIS(S, innovation) + self.error_state.fill_states(K @ innovation) I_KH = np.eye(18) - K @ H self.error_state.covariance = I_KH @ P @ I_KH.T + K @ R @ K.T # Joseph form for more stability + return NIS_value + def injection(self) -> None: """ Injects the error state into the nominal state to produce the estimated state. @@ -241,11 +246,26 @@ def imu_update(self, imu_data: Measurement) -> None: self.nominal_state_discrete(imu_data) self.error_state_prediction(imu_data) - def dvl_update(self, dvl_measurement: Measurement) -> None: + def dvl_update(self, dvl_measurement: Measurement) -> float: """ Updates the state using the DVL measurement. """ - self.measurement_update(dvl_measurement) + NIS = self.measurement_update(dvl_measurement) self.injection() - self.reset_error_state() \ No newline at end of file + self.reset_error_state() + + return NIS + + # functions for tuning the filter + def NIS(self, S: np.ndarray, innovation: np.ndarray) -> float: + """ + Calculates the Normalized Innovation Squared (NIS) value. + """ + return innovation.T @ np.linalg.inv(S) @ innovation + + def NEES(self, P: np.ndarray, true_state: StateQuat, estimate_state: StateQuat) -> float: + """ + Calculates the Normalized Estimation Error Squared (NEES) value. + """ + return (true_state - estimate_state).as_vector().T @ np.linalg.inv(P) @ (true_state - estimate_state).as_vector() \ No newline at end of file diff --git a/navigation/eskf_python/eskf_python/eskf_test.py b/navigation/eskf_python/eskf_python/eskf_test.py index bf4cc8f8b..95f5e996b 100644 --- a/navigation/eskf_python/eskf_python/eskf_test.py +++ b/navigation/eskf_python/eskf_python/eskf_test.py @@ -5,8 +5,9 @@ import numpy as np import matplotlib.pyplot as plt from eskf_python_filter import ESKF +from scipy.stats import chi2 -if __name__ == "__main__": +def simulate_eskf(): # Simulation parameters simulation_time = 20.0 # seconds @@ -20,7 +21,7 @@ true_state_init.position = np.array([0.1, 0.0, 0.0]) true_state_init.velocity = np.array([0.1, 0.0, 0.0]) P0 = np.diag([ - 1.0, 1.0, 1.0, # Position + 0.5, 0.5, 0.5, # Position 0.2, 0.2, 0.2, # Velocity 0.2, 0.2, 0.2, # Orientation 0.00001, 0.00001, 0.00001, # Acceleration bias @@ -30,14 +31,14 @@ # Noise parameters Q = np.diag([ - (0.05**2) / dt, (0.05**2) / dt, (0.05**2) / dt, # Accelerometer noise - (0.004**2) / dt, (0.004**2) / dt, (0.004**2) / dt, # Gyroscope noise - 0.0002, 0.0002, 0.0002, # Acceleration bias random walk - 0.0001, 0.0001, 0.0001 # Gyro bias random walk + (0.034**2) / dt, (0.034**2) / dt, (0.034**2) / dt, # Accelerometer noise + (0.002**2) / dt, (0.002**2) / dt, (0.002**2) / dt, # Gyroscope noise + 0.00001, 0.00001, 0.00001, # Acceleration bias random walk + 0.00001, 0.00001, 0.00001 # Gyro bias random walk ]) Hx = np.zeros((3, 19)) - Hx[0:3, 6:9] = np.eye(3) + Hx[0:3, 3:6] = np.eye(3) # Create filter object eskf = ESKF(Q, P0, Hx, true_state_init, 1e-13, 1e-13, dt) @@ -47,7 +48,7 @@ dvl_data = Measurement() # R matrix for DVL aiding - dvl_data.aiding_covariance = np.diag([(0.5)**2, (0.5)**2, (0.5)**2]) + dvl_data.aiding_covariance = np.diag([(0.01)**2, (0.01)**2, (0.01)**2]) # Setup the process model for simulation of AUV model = process_model() @@ -102,6 +103,9 @@ 0.05 * np.cos(0.1 * t + 0.6) ]) + NIS_list = [] + NIS_value = 0.0 + # Sim for step in range(num_steps): t = step * dt @@ -117,7 +121,8 @@ if step % 20 == 0: dvl_data.aiding = new_state.velocity + np.random.normal(0, 0.01, 3) - eskf.dvl_update(dvl_data) + NIS_value = eskf.dvl_update(dvl_data) + NIS_list.append(NIS_value) true_positions[step, :] = np.copy(new_state.position) true_orientations[step, :] = quat_to_euler(np.copy(new_state.orientation)) @@ -135,64 +140,104 @@ prev_velocity = new_state.velocity model.state_vector_prev = new_state - # Plotting - axis_labels_pos = ["X", "Y", "Z"] - axis_labels_vel = ["X", "Y", "Z"] - axis_labels_ori = ["Roll", "Pitch", "Yaw"] - - # Plot Position - fig_pos, axs_pos = plt.subplots(3, 1, figsize=(10, 12)) - fig_pos.suptitle("True Data vs Filter Estimates for Position") - for i in range(3): - ax_pos = axs_pos[i] - ax_pos.plot(time, true_positions[:, i], label=f"True Pos {axis_labels_pos[i]}", color=f"C{i}", linestyle='-') - ax_pos.plot(time, est_positions[:, i], label=f"Est Pos {axis_labels_pos[i]}", color=f"C{i}", linestyle='--') - sigma_pos = np.sqrt(pos_cov[:, i]) - ax_pos.fill_between(time, est_positions[:, i] - sigma_pos, est_positions[:, i] + sigma_pos, - color=f"C{i}", alpha=0.2) - ax_pos.set_title(f"Position [{axis_labels_pos[i]}] [m]") - ax_pos.set_xlabel("Time [s]") - ax_pos.set_ylabel("Position") - ax_pos.grid(True) - ax_pos.legend() - - plt.tight_layout(rect=[0, 0, 1, 0.96]) - plt.show() - - # Plot Velocity - fig_vel, axs_vel = plt.subplots(3, 1, figsize=(10, 12)) - fig_vel.suptitle("True Data vs Filter Estimates for Velocity") - for i in range(3): - ax_vel = axs_vel[i] - ax_vel.plot(time, true_velocities[:, i], label=f"True Vel {axis_labels_vel[i]}", color=f"C{i}", linestyle='-') - ax_vel.plot(time, est_velocities[:, i], label=f"Est Vel {axis_labels_vel[i]}", color=f"C{i}", linestyle='--') - sigma_vel = np.sqrt(vel_cov[:, i]) - ax_vel.fill_between(time, est_velocities[:, i] - sigma_vel, est_velocities[:, i] + sigma_vel, - color=f"C{i}", alpha=0.2) - ax_vel.set_title(f"Velocity [{axis_labels_vel[i]}] [m/s]") - ax_vel.set_xlabel("Time [s]") - ax_vel.set_ylabel("Velocity") - ax_vel.grid(True) - ax_vel.legend() - - plt.tight_layout(rect=[0, 0, 1, 0.96]) - plt.show() - - # Plot Orientation - fig_ori, axs_ori = plt.subplots(3, 1, figsize=(10, 12)) - fig_ori.suptitle("True Data vs Filter Estimates for Orientation") - for i in range(3): - ax_ori = axs_ori[i] - ax_ori.plot(time, true_orientations[:, i], label=f"True Ori {axis_labels_ori[i]}", color=f"C{i}", linestyle='-') - ax_ori.plot(time, est_orientations[:, i], label=f"Est Ori {axis_labels_ori[i]}", color=f"C{i}", linestyle='--') - sigma_ori = np.sqrt(ori_cov[:, i]) - ax_ori.fill_between(time, est_orientations[:, i] - sigma_ori, est_orientations[:, i] + sigma_ori, - color=f"C{i}", alpha=0.2) - ax_ori.set_title(f"Orientation [{axis_labels_ori[i]}] [rad]") - ax_ori.set_xlabel("Time [s]") - ax_ori.set_ylabel("Orientation") - ax_ori.grid(True) - ax_ori.legend() - - plt.tight_layout(rect=[0, 0, 1, 0.96]) - plt.show() + return time, true_positions, true_orientations, true_velocities, est_positions, est_orientations, est_velocities, pos_cov, vel_cov, ori_cov, NIS_list + +time, true_positions, true_orientations, true_velocities, est_positions, est_orientations, est_velocities, pos_cov, vel_cov, ori_cov, _ = simulate_eskf() + +# Plotting +axis_labels_pos = ["X", "Y", "Z"] +axis_labels_vel = ["X", "Y", "Z"] +axis_labels_ori = ["Roll", "Pitch", "Yaw"] + +# Plot Position +fig_pos, axs_pos = plt.subplots(3, 1, figsize=(10, 12)) +fig_pos.suptitle("True Data vs Filter Estimates for Position") +for i in range(3): + ax_pos = axs_pos[i] + ax_pos.plot(time, true_positions[:, i], label=f"True Pos {axis_labels_pos[i]}", color=f"C{i}", linestyle='-') + ax_pos.plot(time, est_positions[:, i], label=f"Est Pos {axis_labels_pos[i]}", color=f"C{i}", linestyle='--') + sigma_pos = np.sqrt(pos_cov[:, i]) + ax_pos.fill_between(time, est_positions[:, i] - sigma_pos, est_positions[:, i] + sigma_pos, + color=f"C{i}", alpha=0.2) + ax_pos.set_title(f"Position [{axis_labels_pos[i]}] [m]") + ax_pos.set_xlabel("Time [s]") + ax_pos.set_ylabel("Position") + ax_pos.grid(True) + ax_pos.legend() + +plt.tight_layout(rect=[0, 0, 1, 0.96]) +plt.show() + +# Plot Velocity +fig_vel, axs_vel = plt.subplots(3, 1, figsize=(10, 12)) +fig_vel.suptitle("True Data vs Filter Estimates for Velocity") +for i in range(3): + ax_vel = axs_vel[i] + ax_vel.plot(time, true_velocities[:, i], label=f"True Vel {axis_labels_vel[i]}", color=f"C{i}", linestyle='-') + ax_vel.plot(time, est_velocities[:, i], label=f"Est Vel {axis_labels_vel[i]}", color=f"C{i}", linestyle='--') + sigma_vel = np.sqrt(vel_cov[:, i]) + ax_vel.fill_between(time, est_velocities[:, i] - sigma_vel, est_velocities[:, i] + sigma_vel, + color=f"C{i}", alpha=0.2) + ax_vel.set_title(f"Velocity [{axis_labels_vel[i]}] [m/s]") + ax_vel.set_xlabel("Time [s]") + ax_vel.set_ylabel("Velocity") + ax_vel.grid(True) + ax_vel.legend() + +plt.tight_layout(rect=[0, 0, 1, 0.96]) +plt.show() + +# Plot Orientation +fig_ori, axs_ori = plt.subplots(3, 1, figsize=(10, 12)) +fig_ori.suptitle("True Data vs Filter Estimates for Orientation") +for i in range(3): + ax_ori = axs_ori[i] + ax_ori.plot(time, true_orientations[:, i], label=f"True Ori {axis_labels_ori[i]}", color=f"C{i}", linestyle='-') + ax_ori.plot(time, est_orientations[:, i], label=f"Est Ori {axis_labels_ori[i]}", color=f"C{i}", linestyle='--') + sigma_ori = np.sqrt(ori_cov[:, i]) + ax_ori.fill_between(time, est_orientations[:, i] - sigma_ori, est_orientations[:, i] + sigma_ori, + color=f"C{i}", alpha=0.2) + ax_ori.set_title(f"Orientation [{axis_labels_ori[i]}] [rad]") + ax_ori.set_xlabel("Time [s]") + ax_ori.set_ylabel("Orientation") + ax_ori.grid(True) + ax_ori.legend() + +plt.tight_layout(rect=[0, 0, 1, 0.96]) +plt.show() + + +### _______ NIS AND NEES _______ + +num_simulations = 10 +NIS_runs = [] + +for sim in range(num_simulations): + time, true_positions, true_orientations, true_velocities, \ + est_positions, est_orientations, est_velocities, \ + pos_cov, vel_cov, ori_cov, NIS_list = simulate_eskf() + + NIS_runs.append(np.array(NIS_list)) + +NIS_runs = np.vstack(NIS_runs) +ANIS = np.mean(NIS_runs, axis=0) + +measurement_dimension = 3 + +chi2_lower = chi2.ppf(0.025, measurement_dimension) / num_simulations +chi2_upper = chi2.ppf(0.975, measurement_dimension) / num_simulations + +time_steps = np.arange(len(ANIS)) * 0.01 * 20 + +fig, ax = plt.subplots(figsize=(10, 6)) +ax.plot(time_steps, ANIS, label="ANIS", color="C0") +ax.axhline(chi2_lower, color="C1", linestyle="--", label="95% CI Lower") +ax.axhline(chi2_upper, color="C2", linestyle="--", label="95% CI Upper") +ax.set_title("Average Normalized Innovation Squared (ANIS)") +ax.set_xlabel("Time [s]") +ax.set_ylabel("ANIS") +ax.grid(True) +ax.legend() + +plt.tight_layout() +plt.show() \ No newline at end of file diff --git a/navigation/ukf_okid/ukf_python/rest.py b/navigation/ukf_okid/ukf_python/rest.py new file mode 100644 index 000000000..df7392bd4 --- /dev/null +++ b/navigation/ukf_okid/ukf_python/rest.py @@ -0,0 +1,37 @@ +def mean_set(set_points: list[StateQuat], weights: np.ndarray = None) -> np.ndarray: + """ + Function that calculates the mean of a set of points + """ + n = len(set_points[0].as_vector()) - 1 + mean_value = StateQuat() + + if weights is None: + for i in range(2 * n + 1): + weight_temp_list = (1/ (2 * n + 1)) * np.ones(2 * n + 1) + mean_value.add_without_quaternions(weight_temp_list[i] * set_points[i]) + + mean_value.orientation = iterative_quaternion_mean_statequat(set_points, weight_temp_list) + + else: + for i in range(2 * n + 1): + mean_value.add_without_quaternions(weights[i] * set_points[i]) + + mean_value.orientation = iterative_quaternion_mean_statequat(set_points, weights) + + return mean_value.as_vector() + +def mean_measurement(set_points: list[MeasModel], weights: np.ndarray = None) -> np.ndarray: + """ + Function that calculates the mean of a set of points + """ + n = len(set_points) + mean_value = MeasModel() + + if weights is None: + for i in range(n): + mean_value = mean_value + set_points[i] + else: + for i in range(n): + mean_value = mean_value + (weights[i] * set_points[i]) + + return mean_value.measurement \ No newline at end of file diff --git a/navigation/ukf_okid/ukf_python/ukf_okid.py b/navigation/ukf_okid/ukf_python/ukf_okid.py index dd52c6589..1ede3f22b 100644 --- a/navigation/ukf_okid/ukf_python/ukf_okid.py +++ b/navigation/ukf_okid/ukf_python/ukf_okid.py @@ -14,9 +14,9 @@ def __init__(self, process_model: process_model, x_0, P_0, Q, R): self.sigma_points_list = None self.y_i = None self.weight = None - # self.T = self.generate_T_matrix(len(P_0)) + self.T = self.generate_T_matrix(len(P_0)) - def generate_T_matrix(n): + def generate_T_matrix(n: float) -> np.ndarray: """ Generates the orthonormal transformation matrix T used in the TUKF sigma point generation. @@ -28,42 +28,36 @@ def generate_T_matrix(n): """ T = np.zeros((n, 2 * n)) - for i in range(1, 2 * n + 1): # indexing matches equation (1, ..., 2n) + for i in range(1, 2 * n + 1): for j in range(1, (n // 2) + 1): T[2 * j - 2, i - 1] = np.sqrt(2) * np.cos(((2 * j - 1) * i * np.pi) / n) T[2 * j - 1, i - 1] = np.sqrt(2) * np.sin(((2 * j - 1) * i * np.pi) / n) - if n % 2 == 1: # if n is odd, add the last term as described in the paper + if n % 2 == 1: # if n is odd T[n - 1, i - 1] = (-1) ** i - T = T / np.sqrt(2) # Normalize matrix for orthonormality (unit scaling) + T = T / np.sqrt(2) return T - def sigma_points(self, current_state: StateQuat) -> tuple[list[StateQuat], np.ndarray]: + def sigma_points(self, current_state: StateQuat) -> list[StateQuat]: """ Functions that generate the sigma points for the UKF """ n = len(current_state.covariance) - kappa = 3 - n - S = np.linalg.cholesky(current_state.covariance + self.Q) - S_scaled = np.sqrt(n + kappa) * S - - weighted_points = np.concatenate([S_scaled, -S_scaled], axis=1) + I = np.hstack([np.eye(n), -np.eye(n)]) + my = np.sqrt(n) * I + delta = self.T @ my - self.sigma_points_list = [StateQuat() for _ in range(2 * n + 1)] - W = np.zeros(2 * n + 1) - - self.sigma_points_list [0].fill_states(current_state.as_vector()) - W[0] = kappa / (n + kappa) - for i in range(2 * n): - self.sigma_points_list [i + 1].fill_states(current_state.insert_weights(weighted_points[:, i])) - W[i + 1] = 1 / (2 * (n + kappa)) + S = np.linalg.cholesky(current_state.covariance + self.Q) - self.weight = W + self.sigma_points_list = [StateQuat() for _ in range(2 * n)] + + for state in self.sigma_points_list: + state.fill_states_different_dim(current_state.as_vector(), delta[:, self.sigma_points_list.index - return self.sigma_points_list , self.weight + return self.sigma_points_list def unscented_transform(self, current_state: StateQuat) -> StateQuat: @@ -128,290 +122,4 @@ def posteriori_estimate(self, current_state: StateQuat, cross_correlation: np.nd self.process_model.state_vector_prev = posteriori_estimate - return posteriori_estimate - -def add_quaternion_noise(q, noise_std): - - noise = np.random.normal(0, noise_std, 3) - - theta = np.linalg.norm(noise) - - if theta > 0: - - axis = noise / theta - - q_noise = np.hstack((np.cos(theta/2), np.sin(theta/2) * axis)) - - else: - - q_noise = np.array([1.0, 0.0, 0.0, 0.0]) - - q_new = quaternion_super_product(q, q_noise) - - return q_new / np.linalg.norm(q_new) - - -if __name__ == '__main__': - - # Create initial state vector and covariance matrix. - x0 = np.zeros(13) - x0[0:3] = [0.3, 0.3, 0.3] - x0[3] = 1 - x0[7:10] = [0.2, 0.2, 0.2] - dt = 0.01 - R = (0.01) * np.eye(3) - - Q = 0.00015 * np.eye(12) - P0 = np.eye(12) * 0.0001 - - model = process_model() - model.dt = 0.01 - model.mass_interia_matrix = np.array([ - [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] - ]) - model.m = 30.0 - model.r_b_bg = np.array([0.01, 0.0, 0.02]) - model.inertia = np.diag([0.68, 3.32, 3.34]) - model.damping_linear = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) - model.damping_nonlinear = np.array([0.3, 0.3, 0.3, 0.3, 0.3, 0.3]) - model.added_mass = np.diag([1.0, 1.0, 1.0, 2.0, 2.0, 2.0]) - - model_ukf = process_model() - model_ukf.dt = 0.01 - model_ukf.mass_interia_matrix = np.array([ - [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] - ]) - model_ukf.m = 30.0 - model_ukf.r_b_bg = np.array([0.01, 0.0, 0.02]) - model_ukf.inertia = np.diag([0.68, 3.32, 3.34]) - model_ukf.damping_linear = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) - model_ukf.damping_nonlinear = np.array([0.3, 0.3, 0.3, 0.3, 0.3, 0.3]) - model_ukf.added_mass = np.diag([1.0, 1.0, 1.0, 2.0, 2.0, 2.0]) - - # Simulation parameters - simulation_time = 20 # seconds - num_steps = int(simulation_time / dt) - - # Initialize a dummy StateQuat. - new_state = StateQuat() - new_state.fill_states(x0) - new_state.covariance = P0 - - test_state_x = StateQuat() - test_state_x.fill_states(x0) - test_state_x.covariance = P0 - - # Initialize a estimated state - estimated_state = StateQuat() - estimated_state.fill_states(x0) - estimated_state.covariance = P0 - - # Initialize a estimated state - noisy_state = StateQuat() - noisy_state.fill_states(x0) - noisy_state.covariance = P0 - - measurment_model = MeasModel() - measurment_model.measurement = np.array([0.0, 0.0, 0.0]) - measurment_model.covariance = R - - # Initialize arrays to store the results - positions = np.zeros((num_steps, 3)) - orientations = np.zeros((num_steps, 3)) - velocities = np.zeros((num_steps, 3)) - angular_velocities = np.zeros((num_steps, 3)) - - # Initialize arrays to store the estimates - positions_est = np.zeros((num_steps, 3)) - orientations_est = np.zeros((num_steps, 3)) - velocities_est = np.zeros((num_steps, 3)) - angular_velocities_est = np.zeros((num_steps, 3)) - - # Initialize the okid params - okid_params = np.zeros((num_steps, 21)) - - model.state_vector_prev = new_state - model.state_vector = new_state - - model_ukf.state_vector_prev = test_state_x - model_ukf.state_vector = test_state_x - - # initialize the ukf - ukf = UKF(model_ukf, x0, P0, Q, R) - - elapsed_times = [] - - u = lambda t: np.array([2 * np.sin(1 * t), 2 * np.sin(1 * t), 2 * np.sin(1 * t), 0.2 * np.cos(1 * t), 0.2 * np.cos(1 * t), 0.2 * np.cos(1 * t)]) - - # Run the simulation - for step in range(num_steps): - # Insert control input - model.Control_input = u(step * dt) - model_ukf.Control_input = u(step * dt) - - # Perform the unscented transform - model.model_prediction(new_state) - new_state = model.euler_forward() - - # Adding noise in the state vector - estimated_state.position = estimated_state.position # + np.random.normal(0, 0.01, 3) - estimated_state.orientation = estimated_state.orientation #add_quaternion_noise(estimated_state.orientation, 0.01) - estimated_state.velocity = estimated_state.velocity # + np.random.normal(0, 0.01, 3) - estimated_state.angular_velocity = estimated_state.angular_velocity # + np.random.normal(0, 0.01, 3) - - start_time = time.time() - estimated_state = ukf.unscented_transform(estimated_state) - elapsed_time = time.time() - start_time - elapsed_times.append(elapsed_time) - - if step % 10 == 0: - measurment_model.measurement = new_state.velocity # + np.random.normal(0, 0.01, 3) - meas_update, covariance_matrix = ukf.measurement_update(estimated_state, measurment_model) - estimated_state = ukf.posteriori_estimate(estimated_state, covariance_matrix, measurment_model, meas_update) - - - positions[step, :] = new_state.position - orientations[step, :] = quat_to_euler(new_state.orientation) - velocities[step, :] = new_state.velocity - angular_velocities[step, :] = new_state.angular_velocity - - positions_est[step, :] = estimated_state.position - orientations_est[step, :] = quat_to_euler(estimated_state.orientation) - velocities_est[step, :] = estimated_state.velocity - angular_velocities_est[step, :] = estimated_state.angular_velocity - - # Update the state for the next iteration - model.state_vector_prev = new_state - - print('Average elapsed time: ', np.mean(elapsed_times)) - print('Max elapsed time: ', np.max(elapsed_times)) - print('Min elapsed time: ', np.min(elapsed_times)) - print('median elapsed time: ', np.median(elapsed_times)) - # Plot the results - time = np.linspace(0, simulation_time, num_steps) - - # Plot positions - plt.figure() - plt.subplot(3, 1, 1) - plt.plot(time, positions[:, 0], label='True') - plt.plot(time, positions_est[:, 0], label='Estimated') - plt.title('Position X') - plt.xlabel('Time [s]') - plt.ylabel('Position X [m]') - plt.legend() - - plt.subplot(3, 1, 2) - plt.plot(time, positions[:, 1], label='True') - plt.plot(time, positions_est[:, 1], label='Estimated') - plt.title('Position Y') - plt.xlabel('Time [s]') - plt.ylabel('Position Y [m]') - plt.legend() - - plt.subplot(3, 1, 3) - plt.plot(time, positions[:, 2], label='True') - plt.plot(time, positions_est[:, 2], label='Estimated') - plt.title('Position Z') - plt.xlabel('Time [s]') - plt.ylabel('Position Z [m]') - plt.legend() - - plt.tight_layout() - plt.show() - - # Plot orientations (Euler angles) - plt.figure() - plt.subplot(3, 1, 1) - plt.plot(time, orientations[:, 0], label='True') - plt.plot(time, orientations_est[:, 0], label='Estimated') - plt.title('Orientation Roll') - plt.xlabel('Time [s]') - plt.ylabel('Roll [rad]') - plt.legend() - - plt.subplot(3, 1, 2) - plt.plot(time, orientations[:, 1], label='True') - plt.plot(time, orientations_est[:, 1], label='Estimated') - plt.title('Orientation Pitch') - plt.xlabel('Time [s]') - plt.ylabel('Pitch [rad]') - plt.legend() - - plt.subplot(3, 1, 3) - plt.plot(time, orientations[:, 2], label='True') - plt.plot(time, orientations_est[:, 2], label='Estimated') - plt.title('Orientation Yaw') - plt.xlabel('Time [s]') - plt.ylabel('Yaw [rad]') - plt.legend() - - plt.tight_layout() - plt.show() - - # Plot velocities - plt.figure() - plt.subplot(3, 1, 1) - plt.plot(time, velocities[:, 0], label='True') - plt.plot(time, velocities_est[:, 0], label='Estimated') - plt.title('Velocity X') - plt.xlabel('Time [s]') - plt.ylabel('Velocity X [m/s]') - plt.legend() - - plt.subplot(3, 1, 2) - plt.plot(time, velocities[:, 1], label='True') - plt.plot(time, velocities_est[:, 1], label='Estimated') - plt.title('Velocity Y') - plt.xlabel('Time [s]') - plt.ylabel('Velocity Y [m/s]') - plt.legend() - - plt.subplot(3, 1, 3) - plt.plot(time, velocities[:, 2], label='True') - plt.plot(time, velocities_est[:, 2], label='Estimated') - plt.title('Velocity Z') - plt.xlabel('Time [s]') - plt.ylabel('Velocity Z [m/s]') - plt.legend() - - plt.tight_layout() - plt.show() - - # Plot angular velocities - plt.figure() - plt.subplot(3, 1, 1) - plt.plot(time, angular_velocities[:, 0], label='True') - plt.plot(time, angular_velocities_est[:, 0], label='Estimated') - plt.title('Angular Velocity X') - plt.xlabel('Time [s]') - plt.ylabel('Angular Velocity X [rad/s]') - plt.legend() - - plt.subplot(3, 1, 2) - plt.plot(time, angular_velocities[:, 1], label='True') - plt.plot(time, angular_velocities_est[:, 1], label='Estimated') - plt.title('Angular Velocity Y') - plt.xlabel('Time [s]') - plt.ylabel('Angular Velocity Y [rad/s]') - plt.legend() - - plt.subplot(3, 1, 3) - plt.plot(time, angular_velocities[:, 2], label='True') - plt.plot(time, angular_velocities_est[:, 2], label='Estimated') - plt.title('Angular Velocity Z') - plt.xlabel('Time [s]') - plt.ylabel('Angular Velocity Z [rad/s]') - plt.legend() - - plt.tight_layout() - plt.show() \ No newline at end of file + return posteriori_estimate \ No newline at end of file diff --git a/navigation/ukf_okid/ukf_python/ukf_okid_class.py b/navigation/ukf_okid/ukf_python/ukf_okid_class.py index 8444fd82e..50f1988b4 100644 --- a/navigation/ukf_okid/ukf_python/ukf_okid_class.py +++ b/navigation/ukf_okid/ukf_python/ukf_okid_class.py @@ -48,11 +48,11 @@ def fill_states_different_dim(self, state: np.ndarray, state_euler: np.ndarray) self.velocity = state[7:10] + state_euler[6:9] self.angular_velocity = state[10:13] + state_euler[9:12] - def subtract(self, other: 'StateQuat') -> np.ndarray: + def subtract(self, other: 'StateQuat', error_ori: 'np.ndarray') -> np.ndarray: """Subtracts two StateQuat objects, returning the difference with Euler angles.""" new_array = np.zeros(len(self.as_vector()) - 1) new_array[:3] = self.position - other.position - new_array[3:6] = quat_to_euler(quaternion_error(self.orientation, other.orientation)) + new_array[3:6] = error_ori new_array[6:9] = self.velocity - other.velocity new_array[9:12] = self.angular_velocity - other.angular_velocity @@ -115,7 +115,7 @@ class MeasModel: def H(self, state: StateQuat) -> 'MeasModel': """Calculates the measurement matrix.""" H = np.zeros((3, 13)) - H[:, 7:10] = np.eye(3) + H[0:3, 7:10] = np.eye(3) z_i = MeasModel() z_i.measurement = np.dot(H, state.as_vector()) return z_i @@ -309,7 +309,7 @@ def quaternion_error(quat_1: np.ndarray, quat_2: np.ndarray) -> np.ndarray: return error_quat -def iterative_quaternion_mean_statequat(state_list: list[StateQuat], weights: np.ndarray, tol: float = 1e-6, max_iter: int = 100) -> np.ndarray: +def iterative_quaternion_mean_statequat(state_list: list[StateQuat], tol: float = 1e-6, max_iter: int = 100) -> np.ndarray: """ Computes the weighted mean of the quaternion orientations from a list of StateQuat objects using an iterative approach, without requiring the caller to manually extract the quaternion. @@ -323,142 +323,151 @@ def iterative_quaternion_mean_statequat(state_list: list[StateQuat], weights: np Returns: np.ndarray: The averaged quaternion as a 4-element numpy array. """ - # Internally extract the quaternion from each state + sigma_quats = [state.orientation for state in state_list] - - # Initialize the mean quaternion with the first quaternion + n = len(state_list) + mean_q = sigma_quats[0].copy() for _ in range(max_iter): weighted_error_vectors = [] for i, q in enumerate(sigma_quats): - # Compute the error quaternion: e = q * inv(mean_q) - # For unit quaternions, the inverse is the conjugate. mean_q_conj = np.array([mean_q[0], -mean_q[1], -mean_q[2], -mean_q[3]]) e = quaternion_super_product(q, mean_q_conj) - # Clip to avoid numerical issues e0_clipped = np.clip(e[0], -1.0, 1.0) angle = 2 * np.arccos(e0_clipped) if np.abs(angle) < 1e-8: error_vec = np.zeros(3) else: - # Compute the full rotation vector (angle * axis) error_vec = (angle / np.sin(angle / 2)) * e[1:4] - weighted_error_vectors.append(weights[i] * error_vec) + weighted_error_vectors.append(error_vec) - error_avg = np.sum(weighted_error_vectors, axis=0) + error_avg = (1 / n) * np.sum(weighted_error_vectors, axis=0) if np.linalg.norm(error_avg) < tol: break error_norm = np.linalg.norm(error_avg) - delta_q = (np.array([np.cos(error_norm / 2), - *(np.sin(error_norm / 2) * (error_avg / error_norm))]) - if error_norm > 0 else np.array([1.0, 0.0, 0.0, 0.0])) + if error_norm > 0: + delta_q = np.array([np.cos(error_norm / 2), + *(np.sin(error_norm / 2) * (error_avg / error_norm))]) + else: + delta_q = np.array([1.0, 0.0, 0.0, 0.0]) + mean_q = quaternion_super_product(delta_q, mean_q) mean_q = quat_norm(mean_q) return mean_q - - -def mean_set(set_points: list[StateQuat], weights: np.ndarray = None) -> np.ndarray: +def mean_set(set_points: list[StateQuat]) -> np.ndarray: """ - Function that calculates the mean of a set of points + Functio calculates the mean vector of a set of points + + Args: + set_points (list[StateQuat]): List of StateQuat objects + + Returns: + np.ndarray: The mean vector """ - n = len(set_points[0].as_vector()) - 1 + n = len(set_points) mean_value = StateQuat() - if weights is None: - for i in range(2 * n + 1): - weight_temp_list = (1/ (2 * n + 1)) * np.ones(2 * n + 1) - mean_value.add_without_quaternions(weight_temp_list[i] * set_points[i]) - - mean_value.orientation = iterative_quaternion_mean_statequat(set_points, weight_temp_list) + for state in set_points: + mean_value.add_without_quaternions(state) - else: - for i in range(2 * n + 1): - mean_value.add_without_quaternions(weights[i] * set_points[i]) + mean_value = (1 / (n)) * mean_value + + mean_value.orientation = iterative_quaternion_mean_statequat(set_points) - mean_value.orientation = iterative_quaternion_mean_statequat(set_points, weights) - return mean_value.as_vector() -def mean_measurement(set_points: list[MeasModel], weights: np.ndarray = None) -> np.ndarray: +def mean_measurement(set_points: list[MeasModel]) -> np.ndarray: """ Function that calculates the mean of a set of points """ n = len(set_points) mean_value = MeasModel() - if weights is None: - for i in range(n): - mean_value = mean_value + set_points[i] - else: - for i in range(n): - mean_value = mean_value + (weights[i] * set_points[i]) + for state in set_points: + mean_value = mean_value + state + mean_value = (1 / n) * mean_value + return mean_value.measurement -def covariance_set(set_points: list[StateQuat], mean: np.ndarray, weights: np.ndarray = None) -> np.ndarray: +def covariance_set(set_points: list[StateQuat], mean: StateQuat) -> np.ndarray: """ Function that calculates the covariance of a set of points """ - n = len(set_points[0].as_vector()) - 1 - covariance = np.zeros((n, n)) + n = len(set_points) + covariance = np.zeros(set_points[0].covariance.shape) + mean_quat = StateQuat() - mean_quat.fill_states(mean) + mean_quat.fill_states(mean.as_vector()) - if weights is None: - for i in range(2 * n + 1): - covariance += np.outer(set_points[i].subtract(mean_quat), set_points[i].subtract(mean_quat)) + mean_q = mean.orientation - covariance = (1 / (2 * n + 1)) * covariance + for state in set_points: + q = state.orientation + diff_q = quaternion_error(q, mean_q) + + e0_clipped = np.clip(diff_q[0], -1.0, 1.0) + angle = 2.0 * np.arccos(e0_clipped) + if abs(angle) < 1e-8: + e_vec = np.zeros(3) + else: + e_vec = (angle / np.sin(angle/2)) * diff_q[1:4] - else: - for i in range(2 * n + 1): - covariance += weights[i] * np.outer(set_points[i].subtract(mean_quat), set_points[i].subtract(mean_quat)) + covariance += np.outer(state.subtract(mean_quat, e_vec), state.subtract(mean_quat, e_vec)) + + covariance = (1 / (n)) * covariance return covariance -def covariance_measurement(set_points: list[MeasModel], mean: np.ndarray, weights: np.ndarray = None) -> np.ndarray: +def covariance_measurement(set_points: list[MeasModel], mean: np.ndarray) -> np.ndarray: """ Function that calculates the covariance of a set of points """ n = len(set_points) co_size = len(set_points[0].measurement) covariance = np.zeros((co_size, co_size)) + mean_meas = MeasModel() mean_meas.measurement = mean - if weights is None: - for i in range(n): - temp_model = set_points[i] - mean_meas - covariance += np.outer(temp_model.measurement, temp_model.measurement) - - covariance = (1 / (n)) * covariance + for state in set_points: + temp_state = state - mean_meas + covariance += np.outer(temp_state.measurement, temp_state.measurement) - else: - for i in range(n): - temp_model = set_points[i] - mean_meas - covariance += weights[i] * np.outer(temp_model.measurement, temp_model.measurement) + covariance = (1 / n) * covariance return covariance -def cross_covariance(set_y: list[StateQuat], mean_y: np.ndarray, set_z: list[MeasModel], mean_z: np.ndarray, weights: np.ndarray) -> np.ndarray: +def cross_covariance(set_y: list[StateQuat], mean_y: np.ndarray, set_z: list[MeasModel], mean_z: np.ndarray) -> np.ndarray: """ Calculates the cross covariance between the measurement and state prediction """ + n = len(set_y) - n = len(mean_y) - 1 - m = len(mean_z) - cross_covariance = np.zeros((n,m)) + cross_covariance = np.zeros((len(mean_y) - 1, len(mean_z))) mean_quat = StateQuat() mean_quat.fill_states(mean_y) + mean_q = mean_quat.orientation + for i in range(n): - cross_covariance += np.outer(set_y[i].subtract(mean_quat), set_z[i].measurement - mean_z) + q = set_y[i].orientation + diff_q = quaternion_error(q, mean_q) + + e0_clipped = np.clip(diff_q[0], -1.0, 1.0) + angle = 2.0 * np.arccos(e0_clipped) + if abs(angle) < 1e-8: + e_vec = np.zeros(3) + else: + e_vec = (angle / np.sin(angle/2)) * diff_q[1:4] + + cross_covariance += np.outer(set_y[i].subtract(mean_quat, e_vec), set_z[i].measurement - mean_z) - cross_covariance = (1 / len(set_y)) * cross_covariance + cross_covariance = (1 / n) * cross_covariance return cross_covariance diff --git a/navigation/ukf_okid/ukf_python/ukf_test.py b/navigation/ukf_okid/ukf_python/ukf_test.py new file mode 100644 index 000000000..5a7e9eaba --- /dev/null +++ b/navigation/ukf_okid/ukf_python/ukf_test.py @@ -0,0 +1,323 @@ +from ukf_okid import UKF +from ukf_okid_class import StateQuat, process_model, MeasModel +import numpy as np +import time +import matplotlib.pyplot as plt +from ukf_utils import print_StateQuat_list, print_StateQuat +from ukf_okid_class import quaternion_super_product, quat_to_euler, mean_set, covariance_set + + + +def add_quaternion_noise(q, noise_std): + + noise = np.random.normal(0, noise_std, 3) + + theta = np.linalg.norm(noise) + + if theta > 0: + + axis = noise / theta + + q_noise = np.hstack((np.cos(theta/2), np.sin(theta/2) * axis)) + + else: + + q_noise = np.array([1.0, 0.0, 0.0, 0.0]) + + q_new = quaternion_super_product(q, q_noise) + + return q_new / np.linalg.norm(q_new) + + +if __name__ == '__main__': + + # Define a mean StateQuat + mean_state = StateQuat() + mean_state.position = np.array([1.0, 2.0, 3.0]) + mean_state.orientation = np.array([1.0, 0.0, 0.0, 0.0]) # Quaternion + mean_state.velocity = np.array([0.5, 0.5, 0.5]) + mean_state.angular_velocity = np.array([0.1, 0.1, 0.1]) + + test_state = StateQuat() + test_state.position = np.array([1.0, 1.0, 1.0]) + test_state.orientation = np.array([0.0, 1.0, 0.0, 0.0]) # Quaternion + test_state.velocity = np.array([0.2, 0.2, 0.2]) + test_state.angular_velocity = np.array([0.2, 0.2, 0.2]) + + # Create a set with only one element + state_set = list() + state_set.append(test_state) + print(len(state_set)) + + # Compute the mean + mean = mean_set(state_set) + + # Compute the covariance + mean_state.covariance = covariance_set(state_set, mean_state) + + # Print the results + print("Mean State:") + print_StateQuat(mean_state) + + # # Create initial state vector and covariance matrix. + # x0 = np.zeros(13) + # x0[0:3] = [0.3, 0.3, 0.3] + # x0[3] = 1 + # x0[7:10] = [0.2, 0.2, 0.2] + # dt = 0.01 + # R = (0.01) * np.eye(3) + + # Q = 0.00015 * np.eye(12) + # P0 = np.eye(12) * 0.0001 + + # model = process_model() + # model.dt = 0.01 + # model.mass_interia_matrix = np.array([ + # [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] + # ]) + # model.m = 30.0 + # model.r_b_bg = np.array([0.01, 0.0, 0.02]) + # model.inertia = np.diag([0.68, 3.32, 3.34]) + # model.damping_linear = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) + # model.damping_nonlinear = np.array([0.3, 0.3, 0.3, 0.3, 0.3, 0.3]) + # model.added_mass = np.diag([1.0, 1.0, 1.0, 2.0, 2.0, 2.0]) + + # model_ukf = process_model() + # model_ukf.dt = 0.01 + # model_ukf.mass_interia_matrix = np.array([ + # [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] + # ]) + # model_ukf.m = 30.0 + # model_ukf.r_b_bg = np.array([0.01, 0.0, 0.02]) + # model_ukf.inertia = np.diag([0.68, 3.32, 3.34]) + # model_ukf.damping_linear = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) + # model_ukf.damping_nonlinear = np.array([0.3, 0.3, 0.3, 0.3, 0.3, 0.3]) + # model_ukf.added_mass = np.diag([1.0, 1.0, 1.0, 2.0, 2.0, 2.0]) + + # # Simulation parameters + # simulation_time = 20 # seconds + # num_steps = int(simulation_time / dt) + + # # Initialize a dummy StateQuat. + # new_state = StateQuat() + # new_state.fill_states(x0) + # new_state.covariance = P0 + + # test_state_x = StateQuat() + # test_state_x.fill_states(x0) + # test_state_x.covariance = P0 + + # # Initialize a estimated state + # estimated_state = StateQuat() + # estimated_state.fill_states(x0) + # estimated_state.covariance = P0 + + # # Initialize a estimated state + # noisy_state = StateQuat() + # noisy_state.fill_states(x0) + # noisy_state.covariance = P0 + + # measurment_model = MeasModel() + # measurment_model.measurement = np.array([0.0, 0.0, 0.0]) + # measurment_model.covariance = R + + # # Initialize arrays to store the results + # positions = np.zeros((num_steps, 3)) + # orientations = np.zeros((num_steps, 3)) + # velocities = np.zeros((num_steps, 3)) + # angular_velocities = np.zeros((num_steps, 3)) + + # # Initialize arrays to store the estimates + # positions_est = np.zeros((num_steps, 3)) + # orientations_est = np.zeros((num_steps, 3)) + # velocities_est = np.zeros((num_steps, 3)) + # angular_velocities_est = np.zeros((num_steps, 3)) + + # # Initialize the okid params + # okid_params = np.zeros((num_steps, 21)) + + # model.state_vector_prev = new_state + # model.state_vector = new_state + + # model_ukf.state_vector_prev = test_state_x + # model_ukf.state_vector = test_state_x + + # # initialize the ukf + # ukf = UKF(model_ukf, x0, P0, Q, R) + + # elapsed_times = [] + + # u = lambda t: np.array([2 * np.sin(1 * t), 2 * np.sin(1 * t), 2 * np.sin(1 * t), 0.2 * np.cos(1 * t), 0.2 * np.cos(1 * t), 0.2 * np.cos(1 * t)]) + + # # Run the simulation + # for step in range(num_steps): + # # Insert control input + # model.Control_input = u(step * dt) + # model_ukf.Control_input = u(step * dt) + + # # Perform the unscented transform + # model.model_prediction(new_state) + # new_state = model.euler_forward() + + # # Adding noise in the state vector + # estimated_state.position = estimated_state.position # + np.random.normal(0, 0.01, 3) + # estimated_state.orientation = estimated_state.orientation #add_quaternion_noise(estimated_state.orientation, 0.01) + # estimated_state.velocity = estimated_state.velocity # + np.random.normal(0, 0.01, 3) + # estimated_state.angular_velocity = estimated_state.angular_velocity # + np.random.normal(0, 0.01, 3) + + # start_time = time.time() + # estimated_state = ukf.unscented_transform(estimated_state) + # elapsed_time = time.time() - start_time + # elapsed_times.append(elapsed_time) + + # if step % 10 == 0: + # measurment_model.measurement = new_state.velocity # + np.random.normal(0, 0.01, 3) + # meas_update, covariance_matrix = ukf.measurement_update(estimated_state, measurment_model) + # estimated_state = ukf.posteriori_estimate(estimated_state, covariance_matrix, measurment_model, meas_update) + + + # positions[step, :] = new_state.position + # orientations[step, :] = quat_to_euler(new_state.orientation) + # velocities[step, :] = new_state.velocity + # angular_velocities[step, :] = new_state.angular_velocity + + # positions_est[step, :] = estimated_state.position + # orientations_est[step, :] = quat_to_euler(estimated_state.orientation) + # velocities_est[step, :] = estimated_state.velocity + # angular_velocities_est[step, :] = estimated_state.angular_velocity + + # # Update the state for the next iteration + # model.state_vector_prev = new_state + + # print('Average elapsed time: ', np.mean(elapsed_times)) + # print('Max elapsed time: ', np.max(elapsed_times)) + # print('Min elapsed time: ', np.min(elapsed_times)) + # print('median elapsed time: ', np.median(elapsed_times)) + # # Plot the results + # time = np.linspace(0, simulation_time, num_steps) + + # # Plot positions + # plt.figure() + # plt.subplot(3, 1, 1) + # plt.plot(time, positions[:, 0], label='True') + # plt.plot(time, positions_est[:, 0], label='Estimated') + # plt.title('Position X') + # plt.xlabel('Time [s]') + # plt.ylabel('Position X [m]') + # plt.legend() + + # plt.subplot(3, 1, 2) + # plt.plot(time, positions[:, 1], label='True') + # plt.plot(time, positions_est[:, 1], label='Estimated') + # plt.title('Position Y') + # plt.xlabel('Time [s]') + # plt.ylabel('Position Y [m]') + # plt.legend() + + # plt.subplot(3, 1, 3) + # plt.plot(time, positions[:, 2], label='True') + # plt.plot(time, positions_est[:, 2], label='Estimated') + # plt.title('Position Z') + # plt.xlabel('Time [s]') + # plt.ylabel('Position Z [m]') + # plt.legend() + + # plt.tight_layout() + # plt.show() + + # # Plot orientations (Euler angles) + # plt.figure() + # plt.subplot(3, 1, 1) + # plt.plot(time, orientations[:, 0], label='True') + # plt.plot(time, orientations_est[:, 0], label='Estimated') + # plt.title('Orientation Roll') + # plt.xlabel('Time [s]') + # plt.ylabel('Roll [rad]') + # plt.legend() + + # plt.subplot(3, 1, 2) + # plt.plot(time, orientations[:, 1], label='True') + # plt.plot(time, orientations_est[:, 1], label='Estimated') + # plt.title('Orientation Pitch') + # plt.xlabel('Time [s]') + # plt.ylabel('Pitch [rad]') + # plt.legend() + + # plt.subplot(3, 1, 3) + # plt.plot(time, orientations[:, 2], label='True') + # plt.plot(time, orientations_est[:, 2], label='Estimated') + # plt.title('Orientation Yaw') + # plt.xlabel('Time [s]') + # plt.ylabel('Yaw [rad]') + # plt.legend() + + # plt.tight_layout() + # plt.show() + + # # Plot velocities + # plt.figure() + # plt.subplot(3, 1, 1) + # plt.plot(time, velocities[:, 0], label='True') + # plt.plot(time, velocities_est[:, 0], label='Estimated') + # plt.title('Velocity X') + # plt.xlabel('Time [s]') + # plt.ylabel('Velocity X [m/s]') + # plt.legend() + + # plt.subplot(3, 1, 2) + # plt.plot(time, velocities[:, 1], label='True') + # plt.plot(time, velocities_est[:, 1], label='Estimated') + # plt.title('Velocity Y') + # plt.xlabel('Time [s]') + # plt.ylabel('Velocity Y [m/s]') + # plt.legend() + + # plt.subplot(3, 1, 3) + # plt.plot(time, velocities[:, 2], label='True') + # plt.plot(time, velocities_est[:, 2], label='Estimated') + # plt.title('Velocity Z') + # plt.xlabel('Time [s]') + # plt.ylabel('Velocity Z [m/s]') + # plt.legend() + + # plt.tight_layout() + # plt.show() + + # # Plot angular velocities + # plt.figure() + # plt.subplot(3, 1, 1) + # plt.plot(time, angular_velocities[:, 0], label='True') + # plt.plot(time, angular_velocities_est[:, 0], label='Estimated') + # plt.title('Angular Velocity X') + # plt.xlabel('Time [s]') + # plt.ylabel('Angular Velocity X [rad/s]') + # plt.legend() + + # plt.subplot(3, 1, 2) + # plt.plot(time, angular_velocities[:, 1], label='True') + # plt.plot(time, angular_velocities_est[:, 1], label='Estimated') + # plt.title('Angular Velocity Y') + # plt.xlabel('Time [s]') + # plt.ylabel('Angular Velocity Y [rad/s]') + # plt.legend() + + # plt.subplot(3, 1, 3) + # plt.plot(time, angular_velocities[:, 2], label='True') + # plt.plot(time, angular_velocities_est[:, 2], label='Estimated') + # plt.title('Angular Velocity Z') + # plt.xlabel('Time [s]') + # plt.ylabel('Angular Velocity Z [rad/s]') + # plt.legend() + + # plt.tight_layout() + # plt.show() \ No newline at end of file diff --git a/navigation/ukf_okid/ukf_python/ukf_utils.py b/navigation/ukf_okid/ukf_python/ukf_utils.py index f52f2eb62..ad5871567 100644 --- a/navigation/ukf_okid/ukf_python/ukf_utils.py +++ b/navigation/ukf_okid/ukf_python/ukf_utils.py @@ -20,7 +20,7 @@ def print_StateQuat(state: StateQuat, name="StateQuat", print_covariance=True): print(f" Orientation: {state.orientation}") print(f" Velocity: {state.velocity}") print(f" Angular Velocity: {state.angular_velocity}") - print(f" okid_params: {state.okid_params}") + # print(f" okid_params: {state.okid_params}") if print_covariance: print_matrix(state.covariance, "Covariance") From b569a60db422e3b86a703ce0353d5232ace814a7 Mon Sep 17 00:00:00 2001 From: Talha Nauman Choudhry Date: Fri, 28 Mar 2025 21:41:33 +0100 Subject: [PATCH 091/290] feat: Added ESKF in cpp using .hpp and .cpp, current implementation uses msg imu/data_raw and /orca/pose as the dvl info --- navigation/eskf/CMakeLists.txt | 53 ++++ navigation/eskf/config/eskf_params.yaml | 5 + navigation/eskf/include/eskf/eskf.hpp | 92 +++++++ navigation/eskf/include/eskf/eskf_ros.hpp | 69 +++++ navigation/eskf/include/eskf/eskf_utils.hpp | 11 + navigation/eskf/include/eskf/typedefs.hpp | 100 +++++++ navigation/eskf/launch/eskf.launch.py | 22 ++ navigation/eskf/package.xml | 22 ++ navigation/eskf/src/eskf.cpp | 240 ++++++++++++++++ navigation/eskf/src/eskf_node.cpp | 9 + navigation/eskf/src/eskf_ros.cpp | 118 ++++++++ navigation/eskf/src/eskf_utils.cpp | 13 + .../eskf_python/eskf_python_filter.py | 193 +++++++------ .../eskf_python/eskf_python/eskf_test.py | 257 +++++++++++++----- navigation/ukf_okid/ukf_python/ukf_okid.py | 22 +- 15 files changed, 1067 insertions(+), 159 deletions(-) create mode 100644 navigation/eskf/CMakeLists.txt create mode 100644 navigation/eskf/config/eskf_params.yaml create mode 100644 navigation/eskf/include/eskf/eskf.hpp create mode 100644 navigation/eskf/include/eskf/eskf_ros.hpp create mode 100644 navigation/eskf/include/eskf/eskf_utils.hpp create mode 100644 navigation/eskf/include/eskf/typedefs.hpp create mode 100644 navigation/eskf/launch/eskf.launch.py create mode 100644 navigation/eskf/package.xml create mode 100644 navigation/eskf/src/eskf.cpp create mode 100644 navigation/eskf/src/eskf_node.cpp create mode 100644 navigation/eskf/src/eskf_ros.cpp create mode 100644 navigation/eskf/src/eskf_utils.cpp diff --git a/navigation/eskf/CMakeLists.txt b/navigation/eskf/CMakeLists.txt new file mode 100644 index 000000000..2809f9ed9 --- /dev/null +++ b/navigation/eskf/CMakeLists.txt @@ -0,0 +1,53 @@ +cmake_minimum_required(VERSION 3.8) +project(eskf) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +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(nav_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(tf2 REQUIRED) +find_package(vortex_msgs REQUIRED) + +if(NOT DEFINED EIGEN3_INCLUDE_DIR) + set(EIGEN3_INCLUDE_DIR ${EIGEN3_INCLUDE_DIRS}) +endif() +include_directories(${EIGEN3_INCLUDE_DIR}) + +include_directories(include) + +add_executable(eskf_node + src/eskf.cpp + src/eskf_ros.cpp + src/eskf_node.cpp + src/eskf_utils.cpp +) + +ament_target_dependencies(eskf_node + rclcpp + geometry_msgs + nav_msgs + Eigen3 + tf2 + vortex_msgs +) + +install(TARGETS + eskf_node + DESTINATION lib/${PROJECT_NAME}) + +install(DIRECTORY + config + launch + DESTINATION share/${PROJECT_NAME}/ +) + +ament_package() diff --git a/navigation/eskf/config/eskf_params.yaml b/navigation/eskf/config/eskf_params.yaml new file mode 100644 index 000000000..f3402f4a9 --- /dev/null +++ b/navigation/eskf/config/eskf_params.yaml @@ -0,0 +1,5 @@ +eskf_node: + ros__parameters: + imu_topic: imu/date_raw + dvl_twist: /orca/twist + odom_topic: odom diff --git a/navigation/eskf/include/eskf/eskf.hpp b/navigation/eskf/include/eskf/eskf.hpp new file mode 100644 index 000000000..ee47277ea --- /dev/null +++ b/navigation/eskf/include/eskf/eskf.hpp @@ -0,0 +1,92 @@ +#ifndef ESKF_HPP +#define ESKF_HPP + +#include +#include +#include "eskf/typedefs.hpp" +#include "typedefs.hpp" + +class ESKF { + public: + ESKF(const eskf_params& params); + + std::pair imu_update( + const state_quat& nom_state, + const state_euler& error_state, + const imu_measurement& imu_meas, + const double dt); + + std::pair dvl_update( + const state_quat& nom_state, + const state_euler& error_state, + const dvl_measurement& dvl_meas); + + private: + // @brief Predict the nominal state + // @param nom_state: Nominal state + // @param imu_meas: IMU measurement + // @return Predicted nominal state + state_quat nominal_state_discrete(const state_quat& nom_state, + const imu_measurement& imu_meas, + const double dt); + + // @brief Predict the error state + // @param error_state: Error state + // @param nom_state: Nominal state + // @param imu_meas: IMU measurement + // @return Predicted error state + state_euler error_state_prediction(const state_euler& error_state, + const state_quat& nom_state, + const imu_measurement& imu_meas, + const double dt); + + // @brief Update the error state + // @param error_state: Error state + // @param dvl_meas: DVL measurement + // @return Updated error state + state_euler measurement_update(const state_quat& nom_state, + const state_euler& error_state, + const dvl_measurement& dvl_meas); + + // @brief Inject the error state into the nominal state and reset the error + // state + // @param nom_state: Nominal state + // @param error_state: Error state + // @return Injected and reset state + std::pair injection_and_reset( + const state_quat& nom_state, + const state_euler& error_state); + + // @brief Van Loan discretization + // @param A_c: Continuous state transition matrix + // @param G_c: Continuous input matrix + // @return Discrete state transition matrix and discrete input matrix + std::pair van_loan_discretization( + const Eigen::Matrix18d& A_c, + const Eigen::Matrix18x12d& G_c, + const double dt); + + // @brief Calculate the delta quaternion matrix + // @param nom_state: Nominal state + // @return Delta quaternion matrix + Eigen::Matrix4x3d calculate_Q_delta(const state_quat& nom_state); + + // @brief Calculate the measurement matrix jakobian + // @param nom_state: Nominal state + // @return Measurement matrix + Eigen::Matrix3x19d calculate_Hx(const state_quat& nom_state); + + // @brief Calculate the full measurement matrix + // @param nom_state: Nominal state + // @return Measurement matrix + Eigen::Matrix3x18d calculate_H(const state_quat& nom_state); + + // @brief Calculate the measurement + // @param nom_state: Nominal state + // @return Measurement + Eigen::Vector3d calculate_h(const state_quat& nom_state); + + Eigen::Matrix12d Q_; +}; + +#endif // ESKF_HPP diff --git a/navigation/eskf/include/eskf/eskf_ros.hpp b/navigation/eskf/include/eskf/eskf_ros.hpp new file mode 100644 index 000000000..88f97459f --- /dev/null +++ b/navigation/eskf/include/eskf/eskf_ros.hpp @@ -0,0 +1,69 @@ +#ifndef ESKF_ROS_HPP +#define ESKF_ROS_HPP + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "eskf/eskf.hpp" +#include "eskf/typedefs.hpp" +#include "typedefs.hpp" + +class ESKFNode : public rclcpp::Node { + public: + explicit ESKFNode(); + + private: + // @brief Callback function for the imu topic + // @param msg: Imu message containing the imu data + void imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg); + + // @brief Callback function for the dvl topic + // @param msg: TwistWithCovarianceStamped message containing the dvl data + void dvl_callback( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); + + // @brief Publish the odometry message + void publish_odom(); + + // @brief Set the subscriber and publisher for the node + void set_subscribers_and_publisher(); + + // @brief Set the parameters for the eskf + void set_parameters(); + + rclcpp::Subscription::SharedPtr imu_sub_; + + rclcpp::Subscription< + geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr dvl_sub_; + + rclcpp::Publisher::SharedPtr odom_pub_; + + std::chrono::milliseconds time_step; + + rclcpp::TimerBase::SharedPtr odom_pub_timer_; + + state_quat nom_state_; + + state_euler error_state_; + + imu_measurement imu_meas_; + + dvl_measurement dvl_meas_; + + eskf_params eskf_params_; + + std::unique_ptr eskf_; + + rclcpp::Time last_imu_time_; + + bool first_imu_msg_received_ = false; +}; + +#endif // ESKF_ROS_HPP diff --git a/navigation/eskf/include/eskf/eskf_utils.hpp b/navigation/eskf/include/eskf/eskf_utils.hpp new file mode 100644 index 000000000..afd871772 --- /dev/null +++ b/navigation/eskf/include/eskf/eskf_utils.hpp @@ -0,0 +1,11 @@ +#ifndef ESKF_UTILS_HPP +#define ESKF_UTILS_HPP + +#include "eigen3/Eigen/Dense" +#include "eskf/typedefs.hpp" + +Eigen::Matrix3d skew(const Eigen::Vector3d& v); + +double sq(const double& value); + +#endif // ESKF_UTILS_HPP diff --git a/navigation/eskf/include/eskf/typedefs.hpp b/navigation/eskf/include/eskf/typedefs.hpp new file mode 100644 index 000000000..c93c92402 --- /dev/null +++ b/navigation/eskf/include/eskf/typedefs.hpp @@ -0,0 +1,100 @@ +/** + * @file typedefs.hpp + * @brief Contains the typedef and structs for the eskf. + */ +#ifndef ESKF_TYPEDEFS_H +#define ESKF_TYPEDEFS_H + +#include +#include + +namespace Eigen { +typedef Eigen::Matrix Vector19d; +typedef Eigen::Matrix Vector18d; +typedef Eigen::Matrix Matrix18d; +typedef Eigen::Matrix Matrix19d; +typedef Eigen::Matrix Matrix18x12d; +typedef Eigen::Matrix Matrix4x3d; +typedef Eigen::Matrix Matrix3x19d; +typedef Eigen::Matrix Matrix3x18d; +typedef Eigen::Matrix Matrix12d; +typedef Eigen::Matrix Matrix18d; +typedef Eigen::Matrix Matrix3x1d; +typedef Eigen::Matrix Matrix19x18d; +typedef Eigen::Matrix Matrix18x3d; +typedef Eigen::Matrix Matrix36d; +typedef Eigen::Matrix Matrix6d; +typedef Eigen::Matrix Matrix9d; +} // namespace Eigen + +struct state_quat { + Eigen::Vector3d pos = Eigen::Vector3d::Zero(); + Eigen::Vector3d vel = Eigen::Vector3d::Zero(); + Eigen::Quaterniond quat = Eigen::Quaterniond::Identity(); + Eigen::Vector3d gyro_bias = Eigen::Vector3d::Zero(); + Eigen::Vector3d accel_bias = Eigen::Vector3d::Zero(); + Eigen::Vector3d gravity = Eigen::Vector3d(0, 0, 9.81); + + Eigen::Vector19d as_vector() const { + Eigen::Vector19d vec; + vec << pos, vel, quat.w(), quat.x(), quat.y(), quat.z(), gyro_bias, + accel_bias; + return vec; + } + + state_quat operator-(const state_quat& other) const { + state_quat diff; + diff.pos = pos - other.pos; + diff.vel = vel - other.vel; + diff.quat = quat * other.quat.inverse(); + diff.gyro_bias = gyro_bias - other.gyro_bias; + diff.accel_bias = accel_bias - other.accel_bias; + return diff; + } + + Eigen::Matrix3d get_R() const { return quat.toRotationMatrix(); } +}; + +struct state_euler { + Eigen::Vector3d pos = Eigen::Vector3d::Zero(); + Eigen::Vector3d vel = Eigen::Vector3d::Zero(); + Eigen::Vector3d euler = Eigen::Vector3d::Zero(); + Eigen::Vector3d gyro_bias = Eigen::Vector3d::Zero(); + Eigen::Vector3d accel_bias = Eigen::Vector3d::Zero(); + Eigen::Vector3d gravity = Eigen::Vector3d(0, 0, 9.81); + + Eigen::Matrix18d covariance = Eigen::Matrix18d::Zero(); + + Eigen::Vector18d as_vector() const { + Eigen::Vector18d vec; + vec << pos, vel, euler, gyro_bias, accel_bias, gravity; + return vec; + } + + void set_from_vector(const Eigen::Vector18d& vec) { + pos = vec.block<3, 1>(0, 0); + vel = vec.block<3, 1>(3, 0); + euler = vec.block<3, 1>(6, 0); + gyro_bias = vec.block<3, 1>(9, 0); + accel_bias = vec.block<3, 1>(12, 0); + gravity = vec.block<3, 1>(15, 0); + } +}; + +struct imu_measurement { + Eigen::Vector3d accel = Eigen::Vector3d::Zero(); + Eigen::Vector3d gyro = Eigen::Vector3d::Zero(); +}; + +struct dvl_measurement { + Eigen::Vector3d vel = Eigen::Vector3d::Zero(); + Eigen::Matrix3d cov = Eigen::Matrix3d::Zero(); +}; + +struct eskf_params { + double temp = 0.0; + Eigen::Matrix12d Q = Eigen::Matrix12d::Zero(); + double dt = 0.0; +}; + +#endif // ESKF_TYPEDEFS_H diff --git a/navigation/eskf/launch/eskf.launch.py b/navigation/eskf/launch/eskf.launch.py new file mode 100644 index 000000000..84284f804 --- /dev/null +++ b/navigation/eskf/launch/eskf.launch.py @@ -0,0 +1,22 @@ +from os import path + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + +eskf_params = path.join( + get_package_share_directory("eskf"), "config", "eskf_params.yaml" +) + + +def generate_launch_description(): + eskf_node = Node( + package="eskf", + executable="eskf_node", + name="eskf_node", + parameters=[ + eskf_params, + ], + output="screen", + ) + return LaunchDescription([eskf_node]) diff --git a/navigation/eskf/package.xml b/navigation/eskf/package.xml new file mode 100644 index 000000000..d3d8dc416 --- /dev/null +++ b/navigation/eskf/package.xml @@ -0,0 +1,22 @@ + + + + eskf + 1.0.0 + Error-state Kalman filter + talhanc + MIT + + ament_cmake + + rclcpp + geometry_msgs + nav_msgs + eigen + tf2 + vortex_msgs + + + ament_cmake + + diff --git a/navigation/eskf/src/eskf.cpp b/navigation/eskf/src/eskf.cpp new file mode 100644 index 000000000..8218bf8d0 --- /dev/null +++ b/navigation/eskf/src/eskf.cpp @@ -0,0 +1,240 @@ +#include "eskf/eskf.hpp" +#include +#include +#include +#include +#include "eskf/eskf_utils.hpp" +#include "eskf/typedefs.hpp" + +ESKF::ESKF(const eskf_params& params) : Q_(params.Q) {} + +std::pair ESKF::van_loan_discretization( + const Eigen::Matrix18d& A_c, + const Eigen::Matrix18x12d& G_c, + const double dt) { + Eigen::Matrix18d GQG_T = G_c * Q_ * G_c.transpose(); + Eigen::Matrix36d vanLoanMat = Eigen::Matrix36d::Zero(); + + vanLoanMat.topLeftCorner<18, 18>() = -A_c; + vanLoanMat.topRightCorner<18, 18>() = GQG_T; + vanLoanMat.bottomRightCorner<18, 18>() = A_c.transpose(); + + Eigen::Matrix36d vanLoanExp = (vanLoanMat * dt).exp(); + + Eigen::Matrix18d V1 = vanLoanExp.bottomRightCorner<18, 18>().transpose(); + Eigen::Matrix18d V2 = vanLoanExp.topRightCorner<18, 18>(); + + Eigen::Matrix18d A_d = V1; + Eigen::Matrix18d GQG_d = A_d * V2; + + return {A_d, GQG_d}; +} + +Eigen::Matrix4x3d ESKF::calculate_Q_delta(const state_quat& nom_state) { + Eigen::Matrix4x3d Q_delta_theta = Eigen::Matrix4x3d::Zero(); + double qw = nom_state.quat.w(); + double qx = nom_state.quat.x(); + double qy = nom_state.quat.y(); + double qz = nom_state.quat.z(); + + Q_delta_theta << -qx, -qy, -qz, qw, -qz, qy, qz, qw, -qx, -qy, qx, qw; + + Q_delta_theta *= 0.5; + return Q_delta_theta; +} + +Eigen::Matrix3x19d ESKF::calculate_Hx(const state_quat& nom_state) { + Eigen::Matrix3x19d Hx = Eigen::Matrix3x19d::Zero(); + + Eigen::Matrix3d R_bn = + nom_state.quat.normalized().toRotationMatrix().transpose(); + + // normal measurement of the velocity + Hx.block<3, 3>(0, 3) = R_bn; + + Eigen::Vector3d v_n = nom_state.vel; + + Eigen::Matrix dR_dq; + Eigen::Quaterniond q = nom_state.quat.normalized(); + double qw = q.w(); + double qx = q.x(); + double qy = q.y(); + double qz = q.z(); + + dR_dq.col(0) = + 2 * Eigen::Vector3d(qw * v_n.x() - qz * v_n.y() + qy * v_n.z(), + qz * v_n.x() + qw * v_n.y() - qx * v_n.z(), + -qy * v_n.x() + qx * v_n.y() + qw * v_n.z()); + + dR_dq.col(1) = + 2 * Eigen::Vector3d(qx * v_n.x() + qy * v_n.y() + qz * v_n.z(), + qy * v_n.x() - qx * v_n.y() - qw * v_n.z(), + qz * v_n.x() + qw * v_n.y() - qx * v_n.z()); + + dR_dq.col(2) = + 2 * Eigen::Vector3d(-qy * v_n.x() + qx * v_n.y() + qw * v_n.z(), + qx * v_n.x() + qy * v_n.y() + qz * v_n.z(), + -qw * v_n.x() + qz * v_n.y() - qy * v_n.z()); + + dR_dq.col(3) = + 2 * Eigen::Vector3d(-qz * v_n.x() - qw * v_n.y() + qx * v_n.z(), + qw * v_n.x() - qz * v_n.y() + qy * v_n.z(), + qx * v_n.x() + qy * v_n.y() + qz * v_n.z()); + + Hx.block<3, 4>(0, 6) = dR_dq; + + return Hx; +} + +Eigen::Matrix3x18d ESKF::calculate_H(const state_quat& nom_state) { + Eigen::Matrix19x18d X_delta = Eigen::Matrix19x18d::Zero(); + X_delta.block<6, 6>(0, 0) = Eigen::Matrix6d::Identity(); + X_delta.block<4, 3>(6, 6) = calculate_Q_delta(nom_state); + X_delta.block<9, 9>(10, 9) = Eigen::Matrix9d::Identity(); + + Eigen::Matrix3x18d H = calculate_Hx(nom_state) * X_delta; + return H; +} + +Eigen::Matrix3x1d ESKF::calculate_h(const state_quat& nom_state) { + Eigen::Matrix3x1d h; + Eigen::Matrix3d R_bn = + nom_state.quat.normalized().toRotationMatrix().transpose(); + + h = R_bn * nom_state.vel; + + return h; +} + +state_quat ESKF::nominal_state_discrete(const state_quat& nom_state, + const imu_measurement& imu_meas, + const double dt) { + Eigen::Vector3d acc = + nom_state.get_R() * (imu_meas.accel - nom_state.accel_bias) + + nom_state.gravity; + Eigen::Vector3d gyro = (imu_meas.gyro - nom_state.gyro_bias) * dt; + + state_quat next_nom_state; + next_nom_state.pos = + nom_state.pos + nom_state.vel * dt + 0.5 * sq(dt) * acc; + next_nom_state.vel = nom_state.vel + dt * acc; + next_nom_state.quat = + (nom_state.quat * + Eigen::Quaterniond(0, 0.5 * gyro.x(), 0.5 * gyro.y(), 0.5 * gyro.z())); + next_nom_state.quat.normalize(); + next_nom_state.gyro_bias = nom_state.gyro_bias; + next_nom_state.accel_bias = nom_state.accel_bias; + next_nom_state.gravity = nom_state.gravity; + + return next_nom_state; +} + +state_euler ESKF::error_state_prediction(const state_euler& error_state, + const state_quat& nom_state, + const imu_measurement& imu_meas, + const double dt) { + Eigen::Matrix3d R = nom_state.get_R(); + Eigen::Vector3d acc = (imu_meas.accel - nom_state.accel_bias); + Eigen::Vector3d gyro = imu_meas.gyro - nom_state.gyro_bias; + + Eigen::Matrix18d A_c = Eigen::Matrix18d::Zero(); + A_c.block<3, 3>(0, 3) = Eigen::Matrix3d::Identity(); + A_c.block<3, 3>(3, 6) = -R * skew(acc); + A_c.block<3, 3>(6, 6) = -skew(gyro); + A_c.block<3, 3>(3, 9) = -R; + A_c.block<3, 3>(9, 9) = -Eigen::Matrix3d::Identity(); + A_c.block<3, 3>(12, 12) = -Eigen::Matrix3d::Identity(); + A_c.block<3, 3>(6, 12) = -Eigen::Matrix3d::Identity(); + A_c.block<3, 3>(3, 15) = Eigen::Matrix3d::Identity(); + + Eigen::Matrix18x12d G_c = Eigen::Matrix18x12d::Zero(); + G_c.block<3, 3>(3, 0) = -R; + G_c.block<3, 3>(6, 3) = -Eigen::Matrix3d::Identity(); + G_c.block<3, 3>(9, 6) = Eigen::Matrix3d::Identity(); + G_c.block<3, 3>(12, 9) = Eigen::Matrix3d::Identity(); + + auto [A_d, GQG_d] = van_loan_discretization(A_c, G_c, dt); + + state_euler next_error_state; + next_error_state.covariance = + A_d * error_state.covariance * A_d.transpose() + GQG_d; + + return next_error_state; +} + +state_euler ESKF::measurement_update(const state_quat& nom_state, + const state_euler& error_state, + const dvl_measurement& dvl_meas) { + state_euler new_error_state; + + Eigen::Matrix3x18d H = calculate_H(nom_state); + Eigen::Matrix18d P = error_state.covariance; + Eigen::Matrix3d R = dvl_meas.cov; + + Eigen::Matrix3d S = H * P * H.transpose() + R; + Eigen::Matrix18x3d K = P * H.transpose() * S.inverse(); + Eigen::Vector3d innovation = dvl_meas.vel - calculate_h(nom_state); + new_error_state.set_from_vector(K * innovation); + + Eigen::Matrix18d I_KH = Eigen::Matrix18d::Identity() - K * H; + new_error_state.covariance = + I_KH * P * I_KH.transpose() + + K * R * K.transpose(); // Used joseph form for more stable calculations + + return new_error_state; +} + +std::pair ESKF::injection_and_reset( + const state_quat& nom_state, + const state_euler& error_state) { + state_quat next_nom_state; + + next_nom_state.pos = nom_state.pos + error_state.pos; + next_nom_state.vel = nom_state.vel + error_state.vel; + next_nom_state.quat = + nom_state.quat * Eigen::Quaterniond(1, 0.5 * error_state.euler.x(), + 0.5 * error_state.euler.y(), + 0.5 * error_state.euler.z()); + next_nom_state.quat.normalize(); + next_nom_state.gyro_bias = nom_state.gyro_bias + error_state.gyro_bias; + next_nom_state.accel_bias = nom_state.accel_bias + error_state.accel_bias; + next_nom_state.gravity = nom_state.gravity + error_state.gravity; + + state_euler new_error_state; + + Eigen::Matrix18d G = Eigen::Matrix18d::Identity(); + + new_error_state.covariance = G * error_state.covariance * G.transpose(); + new_error_state.pos = Eigen::Vector3d::Zero(); + new_error_state.vel = Eigen::Vector3d::Zero(); + new_error_state.euler = Eigen::Vector3d::Zero(); + new_error_state.gyro_bias = Eigen::Vector3d::Zero(); + new_error_state.accel_bias = Eigen::Vector3d::Zero(); + new_error_state.gravity = Eigen::Vector3d::Zero(); + + return {next_nom_state, new_error_state}; +} + +std::pair ESKF::imu_update( + const state_quat& nom_state, + const state_euler& error_state, + const imu_measurement& imu_meas, + const double dt) { + state_quat next_nom_state = nominal_state_discrete(nom_state, imu_meas, dt); + state_euler next_error_state = + error_state_prediction(error_state, nom_state, imu_meas, dt); + + return {next_nom_state, next_error_state}; +} + +std::pair ESKF::dvl_update( + const state_quat& nom_state, + const state_euler& error_state, + const dvl_measurement& dvl_meas) { + state_euler new_error_state = + measurement_update(nom_state, error_state, dvl_meas); + auto [updated_nom_state, updated_error_state] = + injection_and_reset(nom_state, new_error_state); + + return {updated_nom_state, updated_error_state}; +} diff --git a/navigation/eskf/src/eskf_node.cpp b/navigation/eskf/src/eskf_node.cpp new file mode 100644 index 000000000..e90cebde3 --- /dev/null +++ b/navigation/eskf/src/eskf_node.cpp @@ -0,0 +1,9 @@ +#include "eskf/eskf_ros.hpp" + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Started ESKF Node"); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp new file mode 100644 index 000000000..a3b9c5cf3 --- /dev/null +++ b/navigation/eskf/src/eskf_ros.cpp @@ -0,0 +1,118 @@ +#include "eskf/eskf_ros.hpp" +#include +#include +#include "eskf/eskf_utils.hpp" +#include "eskf/typedefs.hpp" + +ESKFNode::ESKFNode() : Node("eskf_node") { + time_step = std::chrono::milliseconds(1); + odom_pub_timer_ = this->create_wall_timer( + time_step, std::bind(&ESKFNode::publish_odom, this)); + + set_subscribers_and_publisher(); + + set_parameters(); +} + +void ESKFNode::set_subscribers_and_publisher() { + rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; + auto qos_sensor_data = rclcpp::QoS( + rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); + + this->declare_parameter("imu_topic", "imu/data_raw"); + std::string imu_topic = this->get_parameter("imu_topic").as_string(); + imu_sub_ = this->create_subscription( + imu_topic, qos_sensor_data, + std::bind(&ESKFNode::imu_callback, this, std::placeholders::_1)); + + this->declare_parameter("dvl_topic", "/orca/twist"); + std::string dvl_topic = this->get_parameter("dvl_topic").as_string(); + dvl_sub_ = this->create_subscription< + geometry_msgs::msg::TwistWithCovarianceStamped>( + dvl_topic, qos_sensor_data, + std::bind(&ESKFNode::dvl_callback, this, std::placeholders::_1)); + + this->declare_parameter("odom_topic", "odom"); + std::string odom_topic = this->get_parameter("odom_topic").as_string(); + odom_pub_ = this->create_publisher( + odom_topic, qos_sensor_data); +} + +void ESKFNode::set_parameters() { + Eigen::Matrix12d Q; + Q.setZero(); + Q.diagonal() << sq(0.0103), sq(0.0118), sq(0.0043), // acceleration noise + sq(0.00193), sq(0.00306), sq(0.00118), // gyroscope noise + sq(0.05), sq(0.05), sq(0.05), // acceleration bias noise + sq(0.03), sq(0.03), sq(0.03); // gyroscope bias noise + + eskf_params_.Q = Q; + + eskf_ = std::make_unique(eskf_params_); + + Eigen::Matrix18d P; + P.setZero(); + P.diagonal() << 0.1, 0.1, 0.1, // position + 0.1, 0.1, 0.1, // velocity + 0.1, 0.1, 0.1, // euler angles + 0.01, 0.01, 0.01, // accel bias + 0.01, 0.01, 0.01, // gyro bias + 0.001, 0.001, 0.001; // gravity + + error_state_.covariance = P; +} + +void ESKFNode::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { + rclcpp::Time current_time = msg->header.stamp; + + if (!first_imu_msg_received_) { + last_imu_time_ = current_time; + first_imu_msg_received_ = true; + return; + } + + double dt = (current_time - last_imu_time_).seconds(); + last_imu_time_ = current_time; + + imu_meas_.accel << msg->linear_acceleration.x, msg->linear_acceleration.y, + msg->linear_acceleration.z; + imu_meas_.gyro << msg->angular_velocity.x, msg->angular_velocity.y, + msg->angular_velocity.z; + + std::tie(nom_state_, error_state_) = + eskf_->imu_update(nom_state_, error_state_, imu_meas_, dt); +} + +void ESKFNode::dvl_callback( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { + dvl_meas_.vel << msg->twist.twist.linear.x, msg->twist.twist.linear.y, + msg->twist.twist.linear.z; + dvl_meas_.cov << msg->twist.covariance[0], msg->twist.covariance[1], + msg->twist.covariance[2], msg->twist.covariance[6], + msg->twist.covariance[7], msg->twist.covariance[8], + msg->twist.covariance[12], msg->twist.covariance[13], + msg->twist.covariance[14]; + + std::tie(nom_state_, error_state_) = + eskf_->dvl_update(nom_state_, error_state_, dvl_meas_); +} + +void ESKFNode::publish_odom() { + nav_msgs::msg::Odometry odom_msg; + + odom_msg.pose.pose.position.x = nom_state_.pos.x(); + odom_msg.pose.pose.position.y = nom_state_.pos.y(); + odom_msg.pose.pose.position.z = nom_state_.pos.z(); + + odom_msg.pose.pose.orientation.w = nom_state_.quat.w(); + odom_msg.pose.pose.orientation.x = nom_state_.quat.x(); + odom_msg.pose.pose.orientation.y = nom_state_.quat.y(); + odom_msg.pose.pose.orientation.z = nom_state_.quat.z(); + + odom_msg.twist.twist.linear.x = nom_state_.vel.x(); + odom_msg.twist.twist.linear.y = nom_state_.vel.y(); + odom_msg.twist.twist.linear.z = nom_state_.vel.z(); + + odom_msg.header.stamp = this->now(); // Add timestamp to the message + odom_pub_->publish(odom_msg); +} diff --git a/navigation/eskf/src/eskf_utils.cpp b/navigation/eskf/src/eskf_utils.cpp new file mode 100644 index 000000000..4d33ec7ce --- /dev/null +++ b/navigation/eskf/src/eskf_utils.cpp @@ -0,0 +1,13 @@ + +#include "eskf/eskf_utils.hpp" +#include "eskf/typedefs.hpp" + +Eigen::Matrix3d skew(const Eigen::Vector3d& v) { + Eigen::Matrix3d S; + S << 0, -v.z(), v.y(), v.z(), 0, -v.x(), -v.y(), v.x(), 0; + return S; +} + +double sq(const double& value) { + return value * value; +} diff --git a/navigation/eskf_python/eskf_python/eskf_python_filter.py b/navigation/eskf_python/eskf_python/eskf_python_filter.py index 8c4b01963..245d7ece4 100644 --- a/navigation/eskf_python/eskf_python/eskf_python_filter.py +++ b/navigation/eskf_python/eskf_python/eskf_python_filter.py @@ -2,16 +2,22 @@ from typing import Tuple import numpy as np -from scipy.linalg import expm -from eskf_python_class import StateEuler, StateQuat, Measurement -from eskf_python_utils import skew_matrix, quaternion_product, R_from_angle_axis, angle_axis_to_quaternion, euler_to_quat -from scipy.linalg import block_diag -from scipy.spatial.transform import Rotation as R_scipy +from eskf_python_class import Measurement, StateEuler, StateQuat +from eskf_python_utils import ( + R_from_angle_axis, + angle_axis_to_quaternion, + euler_to_quat, + quaternion_product, + skew_matrix, +) +from scipy.linalg import block_diag, expm + class ESKF: - def __init__(self, Q: np.ndarray, P0, Hx, nom_state: StateQuat, p_accBias, p_gyroBias, dt): + def __init__( + self, Q: np.ndarray, P0, nom_state: StateQuat, p_accBias, p_gyroBias, dt + ): self.Q = Q - self.Hx = Hx # Jacobian of the measurement model self.dt = dt self.nom_state = nom_state self.error_state = StateEuler() @@ -28,15 +34,20 @@ def Fx(self, imu_data: Measurement) -> np.ndarray: Returns: np.ndarray: The state transition matrix. """ - F_x = np.zeros((18, 18)) I = np.eye(3) F_x[0:3, 0:3] = I F_x[0:3, 3:6] = self.dt * I F_x[3:6, 3:6] = I - F_x[3:6, 6:9] = -self.nom_state.R_q() @ skew_matrix(imu_data.acceleration - self.nom_state.acceleration_bias) * self.dt - F_x[6:9, 6:9] = R_from_angle_axis((imu_data.angular_velocity - self.nom_state.gyro_bias) * self.dt).T + F_x[3:6, 6:9] = ( + -self.nom_state.R_q() + @ skew_matrix(imu_data.acceleration - self.nom_state.acceleration_bias) + * self.dt + ) + F_x[6:9, 6:9] = R_from_angle_axis( + (imu_data.angular_velocity - self.nom_state.gyro_bias) * self.dt + ).T F_x[3:6, 9:12] = -self.nom_state.R_q() * self.dt F_x[3:6, 15:18] = I * self.dt F_x[6:9, 12:15] = -I * self.dt @@ -45,14 +56,13 @@ def Fx(self, imu_data: Measurement) -> np.ndarray: F_x[15:18, 15:18] = I return F_x - + def Fi(self) -> np.ndarray: """Calculates the input matrix. Returns: np.ndarray: The input matrix. """ - F_i = np.zeros((18, 12)) I = np.eye(3) @@ -62,73 +72,99 @@ def Fi(self) -> np.ndarray: F_i[12:15, 9:12] = I return F_i - + def Q_delta_theta(self) -> np.ndarray: - """ - Calculates the Q_delta_theta matrix. + """Calculates the Q_delta_theta matrix. See Joan Solà. Quaternion kinematics for the error-state Kalman filter. chapter: 6.1.1 eq. 281 """ - qw, qx, qy, qz = self.nom_state.orientation - Q_delta_theta = 0.5 * np.array([ - [-qx, -qy, -qz], - [qw, -qz, qy], - [qz, qw, -qx], - [-qy, qx, qw], - ]) + Q_delta_theta = 0.5 * np.array( + [ + [-qx, -qy, -qz], + [qw, -qz, qy], + [qz, qw, -qx], + [-qy, qx, qw], + ] + ) return Q_delta_theta + def Hx(self) -> np.ndarray: + """Calculates the Jacobian of the measurement model. + + Returns: + np.ndarray: The Jacobian of the measurement model. + """ + Hx = np.zeros((3, 19)) + + q0, q1, q2, q3 = self.nom_state.orientation + e = np.array([q1, q2, q3]) + v = self.nom_state.velocity + + temp_nu = -2 * skew_matrix(e) @ v + temp_eps = 2 * (q0 * np.eye(3) + skew_matrix(e)) @ skew_matrix(v) + + Hx[0:3, 3:6] = self.nom_state.R_q() + Hx[0:3, 6:10] = np.vstack([temp_nu, temp_eps]).T + + Hx = np.zeros((3, 19)) + Hx[0:3, 3:6] = np.eye(3) + + return Hx + def H(self) -> np.ndarray: """Calculates the measurement matrix. Returns: np.ndarray: The measurement matrix. """ - X_deltax = block_diag(np.eye(6), self.Q_delta_theta(), np.eye(9)) - H = self.Hx @ X_deltax + H = self.Hx() @ X_deltax return H - + def h(self) -> np.ndarray: - """ - Calculates the measurement model. + """Calculates the measurement model. Returns: np.ndarray: The measurement model. """ - return self.nom_state.velocity + return self.nom_state.velocity # self.nom_state.R_q() @ self.nom_state.velocity def nominal_state_discrete(self, imu_data: Measurement) -> None: - """ - Calculates the next nominal state using the discrete-time process model defined in: + """Calculates the next nominal state using the discrete-time process model defined in: Joan Solà. Quaternion kinematics for the error-state Kalman filter. Chapter: 5.4.1 The nominal state kinematics - Args: + Args: imu_data (np.ndarray): The IMU data. """ - # Rectify measurements. acc_rect = imu_data.acceleration - self.nom_state.acceleration_bias gyro_rect = imu_data.angular_velocity - self.nom_state.gyro_bias R = self.nom_state.R_q() - self.nom_state.position = self.nom_state.position + self.nom_state.velocity * self.dt + 0.5 * (R @ acc_rect + self.nom_state.g) * self.dt**2 - self.nom_state.velocity = self.nom_state.velocity + (R @ acc_rect + self.nom_state.g) * self.dt - self.nom_state.orientation = quaternion_product(self.nom_state.orientation, angle_axis_to_quaternion(gyro_rect * self.dt)) - self.nom_state.acceleration_bias = np.exp(-self.p_accBias * self.dt) * self.nom_state.acceleration_bias - self.nom_state.gyro_bias = np.exp(-self.p_gyroBias * self.dt) * self.nom_state.gyro_bias + self.nom_state.position = ( + self.nom_state.position + + self.nom_state.velocity * self.dt + + 0.5 * (R @ acc_rect + self.nom_state.g) * self.dt**2 + ) + self.nom_state.velocity = ( + self.nom_state.velocity + (R @ acc_rect + self.nom_state.g) * self.dt + ) + self.nom_state.orientation = quaternion_product( + self.nom_state.orientation, angle_axis_to_quaternion(gyro_rect * self.dt) + ) + self.nom_state.acceleration_bias = self.nom_state.acceleration_bias + self.nom_state.gyro_bias = self.nom_state.gyro_bias self.nom_state.g = self.nom_state.g def van_loan_discretization(self, A_c, G_c) -> Tuple[np.ndarray, np.ndarray]: - """ - Calculates the Van Loan discretization of a continuous-time system. + """Calculates the Van Loan discretization of a continuous-time system. Args: A_c (np.ndarray): The A matrix. @@ -137,18 +173,22 @@ def van_loan_discretization(self, A_c, G_c) -> Tuple[np.ndarray, np.ndarray]: Returns: Tuple: The A_d and GQG_d matrices. """ - GQG_T = np.dot(np.dot(G_c, self.Q), G_c.T) matrix_exp = ( - np.block([[- A_c, GQG_T], [np.zeros((A_c.shape[0], A_c.shape[0])), np.transpose(A_c)]]) + np.block( + [ + [-A_c, GQG_T], + [np.zeros((A_c.shape[0], A_c.shape[0])), np.transpose(A_c)], + ] + ) * self.dt ) van_loan_matrix = expm(matrix_exp) - V1 = van_loan_matrix[A_c.shape[0]:, A_c.shape[0]:] - V2 = van_loan_matrix[:A_c.shape[0], A_c.shape[0]:] + V1 = van_loan_matrix[A_c.shape[0] :, A_c.shape[0] :] + V2 = van_loan_matrix[: A_c.shape[0], A_c.shape[0] :] A_d = V1.T GQG_d = A_d @ V2 @@ -156,7 +196,6 @@ def van_loan_discretization(self, A_c, G_c) -> Tuple[np.ndarray, np.ndarray]: return A_d, GQG_d def error_state_prediction(self, imu_data: Measurement) -> None: - # Rectify measurements. acc_rect = imu_data.acceleration - self.nom_state.acceleration_bias gyro_rect = imu_data.angular_velocity - self.nom_state.gyro_bias @@ -166,9 +205,9 @@ def error_state_prediction(self, imu_data: Measurement) -> None: A_c = np.zeros((18, 18)) A_c[0:3, 3:6] = np.eye(3) - A_c[3:6, 6:9] = - R @ skew_matrix(acc_rect) - A_c[6:9, 6:9] = - skew_matrix(gyro_rect) - A_c[3:6, 9:12] = - R + A_c[3:6, 6:9] = -R @ skew_matrix(acc_rect) + A_c[6:9, 6:9] = -skew_matrix(gyro_rect) + A_c[3:6, 9:12] = -R A_c[9:12, 9:12] = -self.p_accBias * np.eye(3) A_c[12:15, 12:15] = -self.p_gyroBias * np.eye(3) A_c[6:9, 12:15] = -np.eye(3) @@ -183,18 +222,16 @@ def error_state_prediction(self, imu_data: Measurement) -> None: A_d, GQG_d = self.van_loan_discretization(A_c, G_c) - self.error_state.covariance = (A_d @ self.error_state.covariance @ A_d.T + GQG_d) + self.error_state.covariance = A_d @ self.error_state.covariance @ A_d.T + GQG_d - def measurement_update(self, dvl_measurement:Measurement) -> float: - """ - Updates the error state using the DVL measurement. + def measurement_update(self, dvl_measurement: Measurement) -> float: + """Updates the error state using the DVL measurement. Joan Solà. Quaternion kinematics for the error-state Kalman filter. Chapter: 6.1 eq. 274-276 Args: dvl_measurement (np.ndarray): The DVL measurement. """ - H = self.H() P = self.error_state.covariance R = dvl_measurement.aiding_covariance @@ -208,49 +245,47 @@ def measurement_update(self, dvl_measurement:Measurement) -> float: self.error_state.fill_states(K @ innovation) I_KH = np.eye(18) - K @ H - self.error_state.covariance = I_KH @ P @ I_KH.T + K @ R @ K.T # Joseph form for more stability - + self.error_state.covariance = ( + I_KH @ P @ I_KH.T + K @ R @ K.T + ) # Joseph form for more stability return NIS_value def injection(self) -> None: - """ - Injects the error state into the nominal state to produce the estimated state. + """Injects the error state into the nominal state to produce the estimated state. Joan Solà. Quaternion kinematics for the error-state Kalman filter. Chapter 6.2 eq. 282-283 - + """ self.nom_state.position = self.nom_state.position + self.error_state.position self.nom_state.velocity = self.nom_state.velocity + self.error_state.velocity - self.nom_state.orientation = quaternion_product(self.nom_state.orientation, euler_to_quat(self.error_state.orientation)) - self.nom_state.acceleration_bias = self.nom_state.acceleration_bias + self.error_state.acceleration_bias + self.nom_state.orientation = quaternion_product( + self.nom_state.orientation, euler_to_quat(self.error_state.orientation) + ) + self.nom_state.acceleration_bias = ( + self.nom_state.acceleration_bias + self.error_state.acceleration_bias + ) self.nom_state.gyro_bias = self.nom_state.gyro_bias + self.error_state.gyro_bias self.nom_state.g = self.nom_state.g + self.error_state.g def reset_error_state(self) -> None: - """ - Resets the error state after injection. + """Resets the error state after injection. Joan Solà. Quaternion kinematics for the error-state Kalman filter. Chapter 6.3 eq. 284-286 """ - - G = np.eye(18) # Neglecting the delta_theta as this is most common in practice + G = np.eye(18) # Neglecting the delta_theta as this is most common in practice self.error_state.covariance = G @ self.error_state.covariance @ G.T self.error_state.fill_states(np.zeros(18)) def imu_update(self, imu_data: Measurement) -> None: + """Updates the state using the IMU data. """ - Updates the state using the IMU data. - """ - self.nominal_state_discrete(imu_data) self.error_state_prediction(imu_data) - + def dvl_update(self, dvl_measurement: Measurement) -> float: + """Updates the state using the DVL measurement. """ - Updates the state using the DVL measurement. - """ - NIS = self.measurement_update(dvl_measurement) self.injection() self.reset_error_state() @@ -259,13 +294,17 @@ def dvl_update(self, dvl_measurement: Measurement) -> float: # functions for tuning the filter def NIS(self, S: np.ndarray, innovation: np.ndarray) -> float: - """ - Calculates the Normalized Innovation Squared (NIS) value. + """Calculates the Normalized Innovation Squared (NIS) value. """ return innovation.T @ np.linalg.inv(S) @ innovation - - def NEES(self, P: np.ndarray, true_state: StateQuat, estimate_state: StateQuat) -> float: - """ - Calculates the Normalized Estimation Error Squared (NEES) value. + + def NEEDS( + self, P: np.ndarray, true_state: StateQuat, estimate_state: StateQuat + ) -> float: + """Calculates the Normalized Estimation Error Squared (NEEDS) value. """ - return (true_state - estimate_state).as_vector().T @ np.linalg.inv(P) @ (true_state - estimate_state).as_vector() \ No newline at end of file + return ( + (true_state - estimate_state).as_vector().T + @ np.linalg.inv(P) + @ (true_state - estimate_state).as_vector() + ) diff --git a/navigation/eskf_python/eskf_python/eskf_test.py b/navigation/eskf_python/eskf_python/eskf_test.py index 95f5e996b..53ad83958 100644 --- a/navigation/eskf_python/eskf_python/eskf_test.py +++ b/navigation/eskf_python/eskf_python/eskf_test.py @@ -1,14 +1,13 @@ - -from eskf_python_class import StateEuler, StateQuat, MeasurementModel, Measurement -from eskf_python_utils import quat_to_euler -from eskf_test_utils import process_model, StateQuatModel -import numpy as np import matplotlib.pyplot as plt +import numpy as np +from eskf_python_class import Measurement, StateQuat from eskf_python_filter import ESKF +from eskf_python_utils import quat_to_euler +from eskf_test_utils import StateQuatModel, process_model from scipy.stats import chi2 -def simulate_eskf(): +def simulate_eskf(): # Simulation parameters simulation_time = 20.0 # seconds dt = 0.01 @@ -20,47 +19,71 @@ def simulate_eskf(): true_state_init = StateQuat() true_state_init.position = np.array([0.1, 0.0, 0.0]) true_state_init.velocity = np.array([0.1, 0.0, 0.0]) - P0 = np.diag([ - 0.5, 0.5, 0.5, # Position - 0.2, 0.2, 0.2, # Velocity - 0.2, 0.2, 0.2, # Orientation - 0.00001, 0.00001, 0.00001, # Acceleration bias - 0.00001, 0.00001, 0.00001, # Gyro bias - 0.00001, 0.00001, 0.00001 # Gravity - ]) - + P0 = np.diag( + [ + 0.3, + 0.3, + 0.3, # Position + 0.2, + 0.2, + 0.2, # Velocity + 0.2, + 0.2, + 0.2, # Orientation + 0.0001, + 0.0001, + 0.0001, # Acceleration bias + 0.00001, + 0.00001, + 0.00001, # Gyro bias + 0.00001, + 0.00001, + 0.00001, # Gravity + ] + ) # Noise parameters - Q = np.diag([ - (0.034**2) / dt, (0.034**2) / dt, (0.034**2) / dt, # Accelerometer noise - (0.002**2) / dt, (0.002**2) / dt, (0.002**2) / dt, # Gyroscope noise - 0.00001, 0.00001, 0.00001, # Acceleration bias random walk - 0.00001, 0.00001, 0.00001 # Gyro bias random walk - ]) - - Hx = np.zeros((3, 19)) - Hx[0:3, 3:6] = np.eye(3) + Q = np.diag( + [ + (0.13**2), + (0.13**2), + (0.13**2), # Adjusted Accelerometer noise + (0.13**2), + (0.13**2), + (0.13**2), # Adjusted Gyroscope noise + 0.0001, + 0.0001, + 0.0001, # Adjusted Acceleration bias random walk + 0.0001, + 0.0001, + 0.0001, # Adjusted Gyro bias random walk + ] + ) # Create filter object - eskf = ESKF(Q, P0, Hx, true_state_init, 1e-13, 1e-13, dt) + eskf = ESKF(Q, P0, true_state_init, 1e-13, 1e-13, dt) # Create measurement objects imu_data = Measurement() dvl_data = Measurement() # R matrix for DVL aiding - dvl_data.aiding_covariance = np.diag([(0.01)**2, (0.01)**2, (0.01)**2]) + dvl_data.aiding_covariance = np.diag( + [(0.01) ** 2, (0.01) ** 2, (0.01) ** 2] + ) # Adjusted DVL aiding covariance # Setup the process model for simulation of AUV model = process_model() model.dt = dt - model.mass_interia_matrix = np.array([ - [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] - ]) + model.mass_interia_matrix = np.array( + [ + [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], + ] + ) model.m = 30.0 model.r_b_bg = np.array([0.01, 0.0, 0.02]) model.inertia = np.diag([0.68, 3.32, 3.34]) @@ -89,19 +112,21 @@ def simulate_eskf(): est_velocities = np.zeros((num_steps, 3)) # covariance arrays - pos_cov = np.zeros((num_steps, 3)) - vel_cov = np.zeros((num_steps, 3)) - ori_cov = np.zeros((num_steps, 3)) + pos_cov = np.zeros((num_steps, 3)) + vel_cov = np.zeros((num_steps, 3)) + ori_cov = np.zeros((num_steps, 3)) prev_velocity = np.zeros(3) - u = lambda t: np.array([ - 0.5 * np.sin(0.1 * t), - 0.5 * np.sin(0.1 * t + 0.3), - 0.5 * np.sin(0.1 * t + 0.6), - 0.05 * np.cos(0.1 * t), - 0.05 * np.cos(0.1 * t + 0.3), - 0.05 * np.cos(0.1 * t + 0.6) - ]) + u = lambda t: np.array( + [ + 0.5 * np.sin(0.1 * t), + 0.5 * np.sin(0.1 * t + 0.3), + 0.5 * np.sin(0.1 * t + 0.6), + 0.05 * np.cos(0.1 * t), + 0.05 * np.cos(0.1 * t + 0.3), + 0.05 * np.cos(0.1 * t + 0.6), + ] + ) NIS_list = [] NIS_value = 0.0 @@ -114,12 +139,16 @@ def simulate_eskf(): model.model_prediction(new_state) new_state = model.euler_forward() - imu_data.acceleration = ((new_state.velocity - prev_velocity) / dt) + np.random.normal(0, 0.13, 3) - imu_data.angular_velocity = new_state.angular_velocity + np.random.normal(0, 0.13, 3) + imu_data.acceleration = ( + (new_state.velocity - prev_velocity) / dt + ) + np.random.normal(0, 0.13, 3) + imu_data.angular_velocity = new_state.angular_velocity + np.random.normal( + 0, 0.13, 3 + ) eskf.imu_update(imu_data) - if step % 20 == 0: + if step % 200 == 0: dvl_data.aiding = new_state.velocity + np.random.normal(0, 0.01, 3) NIS_value = eskf.dvl_update(dvl_data) NIS_list.append(NIS_value) @@ -140,9 +169,34 @@ def simulate_eskf(): prev_velocity = new_state.velocity model.state_vector_prev = new_state - return time, true_positions, true_orientations, true_velocities, est_positions, est_orientations, est_velocities, pos_cov, vel_cov, ori_cov, NIS_list - -time, true_positions, true_orientations, true_velocities, est_positions, est_orientations, est_velocities, pos_cov, vel_cov, ori_cov, _ = simulate_eskf() + return ( + time, + true_positions, + true_orientations, + true_velocities, + est_positions, + est_orientations, + est_velocities, + pos_cov, + vel_cov, + ori_cov, + NIS_list, + ) + + +( + time, + true_positions, + true_orientations, + true_velocities, + est_positions, + est_orientations, + est_velocities, + pos_cov, + vel_cov, + ori_cov, + _, +) = simulate_eskf() # Plotting axis_labels_pos = ["X", "Y", "Z"] @@ -154,11 +208,28 @@ def simulate_eskf(): fig_pos.suptitle("True Data vs Filter Estimates for Position") for i in range(3): ax_pos = axs_pos[i] - ax_pos.plot(time, true_positions[:, i], label=f"True Pos {axis_labels_pos[i]}", color=f"C{i}", linestyle='-') - ax_pos.plot(time, est_positions[:, i], label=f"Est Pos {axis_labels_pos[i]}", color=f"C{i}", linestyle='--') + ax_pos.plot( + time, + true_positions[:, i], + label=f"True Pos {axis_labels_pos[i]}", + color=f"C{i}", + linestyle='-', + ) + ax_pos.plot( + time, + est_positions[:, i], + label=f"Est Pos {axis_labels_pos[i]}", + color=f"C{i}", + linestyle='--', + ) sigma_pos = np.sqrt(pos_cov[:, i]) - ax_pos.fill_between(time, est_positions[:, i] - sigma_pos, est_positions[:, i] + sigma_pos, - color=f"C{i}", alpha=0.2) + ax_pos.fill_between( + time, + est_positions[:, i] - sigma_pos, + est_positions[:, i] + sigma_pos, + color=f"C{i}", + alpha=0.2, + ) ax_pos.set_title(f"Position [{axis_labels_pos[i]}] [m]") ax_pos.set_xlabel("Time [s]") ax_pos.set_ylabel("Position") @@ -173,11 +244,28 @@ def simulate_eskf(): fig_vel.suptitle("True Data vs Filter Estimates for Velocity") for i in range(3): ax_vel = axs_vel[i] - ax_vel.plot(time, true_velocities[:, i], label=f"True Vel {axis_labels_vel[i]}", color=f"C{i}", linestyle='-') - ax_vel.plot(time, est_velocities[:, i], label=f"Est Vel {axis_labels_vel[i]}", color=f"C{i}", linestyle='--') + ax_vel.plot( + time, + true_velocities[:, i], + label=f"True Vel {axis_labels_vel[i]}", + color=f"C{i}", + linestyle='-', + ) + ax_vel.plot( + time, + est_velocities[:, i], + label=f"Est Vel {axis_labels_vel[i]}", + color=f"C{i}", + linestyle='--', + ) sigma_vel = np.sqrt(vel_cov[:, i]) - ax_vel.fill_between(time, est_velocities[:, i] - sigma_vel, est_velocities[:, i] + sigma_vel, - color=f"C{i}", alpha=0.2) + ax_vel.fill_between( + time, + est_velocities[:, i] - sigma_vel, + est_velocities[:, i] + sigma_vel, + color=f"C{i}", + alpha=0.2, + ) ax_vel.set_title(f"Velocity [{axis_labels_vel[i]}] [m/s]") ax_vel.set_xlabel("Time [s]") ax_vel.set_ylabel("Velocity") @@ -192,11 +280,28 @@ def simulate_eskf(): fig_ori.suptitle("True Data vs Filter Estimates for Orientation") for i in range(3): ax_ori = axs_ori[i] - ax_ori.plot(time, true_orientations[:, i], label=f"True Ori {axis_labels_ori[i]}", color=f"C{i}", linestyle='-') - ax_ori.plot(time, est_orientations[:, i], label=f"Est Ori {axis_labels_ori[i]}", color=f"C{i}", linestyle='--') + ax_ori.plot( + time, + true_orientations[:, i], + label=f"True Ori {axis_labels_ori[i]}", + color=f"C{i}", + linestyle='-', + ) + ax_ori.plot( + time, + est_orientations[:, i], + label=f"Est Ori {axis_labels_ori[i]}", + color=f"C{i}", + linestyle='--', + ) sigma_ori = np.sqrt(ori_cov[:, i]) - ax_ori.fill_between(time, est_orientations[:, i] - sigma_ori, est_orientations[:, i] + sigma_ori, - color=f"C{i}", alpha=0.2) + ax_ori.fill_between( + time, + est_orientations[:, i] - sigma_ori, + est_orientations[:, i] + sigma_ori, + color=f"C{i}", + alpha=0.2, + ) ax_ori.set_title(f"Orientation [{axis_labels_ori[i]}] [rad]") ax_ori.set_xlabel("Time [s]") ax_ori.set_ylabel("Orientation") @@ -207,19 +312,29 @@ def simulate_eskf(): plt.show() -### _______ NIS AND NEES _______ +### _______ NIS AND NEEDS _______ -num_simulations = 10 +num_simulations = 10 NIS_runs = [] for sim in range(num_simulations): - time, true_positions, true_orientations, true_velocities, \ - est_positions, est_orientations, est_velocities, \ - pos_cov, vel_cov, ori_cov, NIS_list = simulate_eskf() + ( + time, + true_positions, + true_orientations, + true_velocities, + est_positions, + est_orientations, + est_velocities, + pos_cov, + vel_cov, + ori_cov, + NIS_list, + ) = simulate_eskf() NIS_runs.append(np.array(NIS_list)) -NIS_runs = np.vstack(NIS_runs) +NIS_runs = np.vstack(NIS_runs) ANIS = np.mean(NIS_runs, axis=0) measurement_dimension = 3 @@ -227,7 +342,7 @@ def simulate_eskf(): chi2_lower = chi2.ppf(0.025, measurement_dimension) / num_simulations chi2_upper = chi2.ppf(0.975, measurement_dimension) / num_simulations -time_steps = np.arange(len(ANIS)) * 0.01 * 20 +time_steps = np.arange(len(ANIS)) * 0.01 * 20 fig, ax = plt.subplots(figsize=(10, 6)) ax.plot(time_steps, ANIS, label="ANIS", color="C0") @@ -240,4 +355,4 @@ def simulate_eskf(): ax.legend() plt.tight_layout() -plt.show() \ No newline at end of file +plt.show() diff --git a/navigation/ukf_okid/ukf_python/ukf_okid.py b/navigation/ukf_okid/ukf_python/ukf_okid.py index 1ede3f22b..f7000adb4 100644 --- a/navigation/ukf_okid/ukf_python/ukf_okid.py +++ b/navigation/ukf_okid/ukf_python/ukf_okid.py @@ -36,10 +36,10 @@ def generate_T_matrix(n: float) -> np.ndarray: if n % 2 == 1: # if n is odd T[n - 1, i - 1] = (-1) ** i - T = T / np.sqrt(2) + T = T / np.sqrt(2) return T - + def sigma_points(self, current_state: StateQuat) -> list[StateQuat]: """ Functions that generate the sigma points for the UKF @@ -53,12 +53,12 @@ def sigma_points(self, current_state: StateQuat) -> list[StateQuat]: S = np.linalg.cholesky(current_state.covariance + self.Q) self.sigma_points_list = [StateQuat() for _ in range(2 * n)] - + for state in self.sigma_points_list: - state.fill_states_different_dim(current_state.as_vector(), delta[:, self.sigma_points_list.index + state.fill_states_different_dim(current_state.as_vector(), return self.sigma_points_list - + def unscented_transform(self, current_state: StateQuat) -> StateQuat: """ @@ -94,18 +94,18 @@ def measurement_update(self, current_state: StateQuat, measurement: MeasModel) - z_i[i] = measurement.H(self.sigma_points_list[i]) meas_update = MeasModel() - + meas_update.measurement = mean_measurement(z_i, self.weight) - + meas_update.covariance = covariance_measurement(z_i, meas_update.measurement, self.weight) - + cross_correlation = cross_covariance(self.y_i, current_state.as_vector(), z_i, meas_update.measurement, self.weight) - + return meas_update, cross_correlation def posteriori_estimate(self, current_state: StateQuat, cross_correlation: np.ndarray, measurement: MeasModel, ex_measuremnt: MeasModel) -> StateQuat: """ - Calculates the posteriori estimate using measurment and the prior estimate + Calculates the posteriori estimate using measurement and the prior estimate """ nu_k = MeasModel() @@ -122,4 +122,4 @@ def posteriori_estimate(self, current_state: StateQuat, cross_correlation: np.nd self.process_model.state_vector_prev = posteriori_estimate - return posteriori_estimate \ No newline at end of file + return posteriori_estimate From f0ccaf082af0854a30ddf21e3caef9429c2430a6 Mon Sep 17 00:00:00 2001 From: Talha Nauman Choudhry Date: Wed, 2 Apr 2025 17:03:52 +0200 Subject: [PATCH 092/290] fix: Added correction for the IMU measurements --- navigation/eskf/CMakeLists.txt | 10 +- navigation/eskf/config/eskf_params.yaml | 3 +- navigation/eskf/include/eskf/eskf.hpp | 2 +- navigation/eskf/include/eskf/eskf_utils.hpp | 4 + navigation/eskf/include/eskf/typedefs.hpp | 48 +- navigation/eskf/src/eskf.cpp | 95 ++- navigation/eskf/src/eskf_ros.cpp | 81 ++- navigation/eskf/src/eskf_utils.cpp | 18 + .../eskf_python/eskf_python_filter.py | 116 ++-- .../eskf_python/eskf_python/eskf_test.py | 3 +- navigation/ukf_okid/ukf_python/ukf_okid.py | 33 +- .../ukf_okid/ukf_python/ukf_okid_class.py | 6 +- navigation/ukf_okid/ukf_python/ukf_test.py | 550 +++++++++--------- 13 files changed, 476 insertions(+), 493 deletions(-) diff --git a/navigation/eskf/CMakeLists.txt b/navigation/eskf/CMakeLists.txt index 2809f9ed9..6c8167609 100644 --- a/navigation/eskf/CMakeLists.txt +++ b/navigation/eskf/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.8) project(eskf) if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD 20) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") @@ -16,6 +16,8 @@ find_package(geometry_msgs REQUIRED) find_package(Eigen3 REQUIRED) find_package(tf2 REQUIRED) find_package(vortex_msgs REQUIRED) +find_package(spdlog REQUIRED) +find_package(fmt REQUIRED) if(NOT DEFINED EIGEN3_INCLUDE_DIR) set(EIGEN3_INCLUDE_DIR ${EIGEN3_INCLUDE_DIRS}) @@ -38,6 +40,12 @@ ament_target_dependencies(eskf_node Eigen3 tf2 vortex_msgs + spdlog + fmt +) + +target_link_libraries(eskf_node + fmt::fmt ) install(TARGETS diff --git a/navigation/eskf/config/eskf_params.yaml b/navigation/eskf/config/eskf_params.yaml index f3402f4a9..f89b62f79 100644 --- a/navigation/eskf/config/eskf_params.yaml +++ b/navigation/eskf/config/eskf_params.yaml @@ -1,5 +1,6 @@ eskf_node: ros__parameters: - imu_topic: imu/date_raw + imu_topic: imu/data_raw dvl_twist: /orca/twist odom_topic: odom + diag_Q_std: [0.0103, 0.0118, 0.0043, 0.00193, 0.00306, 0.00118, 0.000001, 0.000001, 0.000001, 0.000003, 0.000003, 0.000003] diff --git a/navigation/eskf/include/eskf/eskf.hpp b/navigation/eskf/include/eskf/eskf.hpp index ee47277ea..b30dc35b0 100644 --- a/navigation/eskf/include/eskf/eskf.hpp +++ b/navigation/eskf/include/eskf/eskf.hpp @@ -1,7 +1,7 @@ #ifndef ESKF_HPP #define ESKF_HPP -#include +#include #include #include "eskf/typedefs.hpp" #include "typedefs.hpp" diff --git a/navigation/eskf/include/eskf/eskf_utils.hpp b/navigation/eskf/include/eskf/eskf_utils.hpp index afd871772..100f7673d 100644 --- a/navigation/eskf/include/eskf/eskf_utils.hpp +++ b/navigation/eskf/include/eskf/eskf_utils.hpp @@ -8,4 +8,8 @@ Eigen::Matrix3d skew(const Eigen::Vector3d& v); double sq(const double& value); +Eigen::Quaterniond vector3d_to_quaternion(const Eigen::Vector3d& vector); + +Eigen::Quaterniond euler_to_quaternion(const Eigen::Vector3d& euler); + #endif // ESKF_UTILS_HPP diff --git a/navigation/eskf/include/eskf/typedefs.hpp b/navigation/eskf/include/eskf/typedefs.hpp index c93c92402..c0a1e41f9 100644 --- a/navigation/eskf/include/eskf/typedefs.hpp +++ b/navigation/eskf/include/eskf/typedefs.hpp @@ -5,26 +5,26 @@ #ifndef ESKF_TYPEDEFS_H #define ESKF_TYPEDEFS_H -#include +#include #include namespace Eigen { -typedef Eigen::Matrix Vector19d; -typedef Eigen::Matrix Vector18d; -typedef Eigen::Matrix Matrix18d; -typedef Eigen::Matrix Matrix19d; -typedef Eigen::Matrix Matrix18x12d; -typedef Eigen::Matrix Matrix4x3d; -typedef Eigen::Matrix Matrix3x19d; -typedef Eigen::Matrix Matrix3x18d; -typedef Eigen::Matrix Matrix12d; -typedef Eigen::Matrix Matrix18d; -typedef Eigen::Matrix Matrix3x1d; -typedef Eigen::Matrix Matrix19x18d; -typedef Eigen::Matrix Matrix18x3d; -typedef Eigen::Matrix Matrix36d; -typedef Eigen::Matrix Matrix6d; -typedef Eigen::Matrix Matrix9d; + typedef Eigen::Matrix Vector19d; + typedef Eigen::Matrix Vector18d; + typedef Eigen::Matrix Matrix18d; + typedef Eigen::Matrix Matrix19d; + typedef Eigen::Matrix Matrix18x12d; + typedef Eigen::Matrix Matrix4x3d; + typedef Eigen::Matrix Matrix3x19d; + typedef Eigen::Matrix Matrix3x18d; + typedef Eigen::Matrix Matrix12d; + typedef Eigen::Matrix Matrix18d; + typedef Eigen::Matrix Matrix3x1d; + typedef Eigen::Matrix Matrix19x18d; + typedef Eigen::Matrix Matrix18x3d; + typedef Eigen::Matrix Matrix36d; + typedef Eigen::Matrix Matrix6d; + typedef Eigen::Matrix Matrix9d; } // namespace Eigen struct state_quat { @@ -61,7 +61,7 @@ struct state_euler { Eigen::Vector3d euler = Eigen::Vector3d::Zero(); Eigen::Vector3d gyro_bias = Eigen::Vector3d::Zero(); Eigen::Vector3d accel_bias = Eigen::Vector3d::Zero(); - Eigen::Vector3d gravity = Eigen::Vector3d(0, 0, 9.81); + Eigen::Vector3d gravity = Eigen::Vector3d::Zero(); Eigen::Matrix18d covariance = Eigen::Matrix18d::Zero(); @@ -84,6 +84,18 @@ struct state_euler { struct imu_measurement { Eigen::Vector3d accel = Eigen::Vector3d::Zero(); Eigen::Vector3d gyro = Eigen::Vector3d::Zero(); + Eigen::Vector3d accel_uncorrected = Eigen::Vector3d::Zero(); + Eigen::Vector3d gyro_uncorrected = Eigen::Vector3d::Zero(); + + void correct() { + Eigen::Matrix3d R_nb; + R_nb << 0, 0, -1, + 0, -1, 0, + -1, 0, 0; + + accel = R_nb * accel_uncorrected; + gyro = R_nb * gyro_uncorrected; + } }; struct dvl_measurement { diff --git a/navigation/eskf/src/eskf.cpp b/navigation/eskf/src/eskf.cpp index 8218bf8d0..55d8516dd 100644 --- a/navigation/eskf/src/eskf.cpp +++ b/navigation/eskf/src/eskf.cpp @@ -6,7 +6,8 @@ #include "eskf/eskf_utils.hpp" #include "eskf/typedefs.hpp" -ESKF::ESKF(const eskf_params& params) : Q_(params.Q) {} +ESKF::ESKF(const eskf_params& params) : + Q_(params.Q) {} std::pair ESKF::van_loan_discretization( const Eigen::Matrix18d& A_c, @@ -42,44 +43,45 @@ Eigen::Matrix4x3d ESKF::calculate_Q_delta(const state_quat& nom_state) { Q_delta_theta *= 0.5; return Q_delta_theta; } - Eigen::Matrix3x19d ESKF::calculate_Hx(const state_quat& nom_state) { Eigen::Matrix3x19d Hx = Eigen::Matrix3x19d::Zero(); - Eigen::Matrix3d R_bn = - nom_state.quat.normalized().toRotationMatrix().transpose(); - - // normal measurement of the velocity - Hx.block<3, 3>(0, 3) = R_bn; - + Eigen::Quaterniond q = nom_state.quat.normalized(); + Eigen::Matrix3d R_bn = q.toRotationMatrix(); + Eigen::Vector3d v_n = nom_state.vel; + Hx.block<3, 3>(0, 3) = R_bn.transpose(); + Eigen::Matrix dR_dq; - Eigen::Quaterniond q = nom_state.quat.normalized(); double qw = q.w(); double qx = q.x(); double qy = q.y(); double qz = q.z(); - dR_dq.col(0) = - 2 * Eigen::Vector3d(qw * v_n.x() - qz * v_n.y() + qy * v_n.z(), - qz * v_n.x() + qw * v_n.y() - qx * v_n.z(), - -qy * v_n.x() + qx * v_n.y() + qw * v_n.z()); - - dR_dq.col(1) = - 2 * Eigen::Vector3d(qx * v_n.x() + qy * v_n.y() + qz * v_n.z(), - qy * v_n.x() - qx * v_n.y() - qw * v_n.z(), - qz * v_n.x() + qw * v_n.y() - qx * v_n.z()); - - dR_dq.col(2) = - 2 * Eigen::Vector3d(-qy * v_n.x() + qx * v_n.y() + qw * v_n.z(), - qx * v_n.x() + qy * v_n.y() + qz * v_n.z(), - -qw * v_n.x() + qz * v_n.y() - qy * v_n.z()); - - dR_dq.col(3) = - 2 * Eigen::Vector3d(-qz * v_n.x() - qw * v_n.y() + qx * v_n.z(), - qw * v_n.x() - qz * v_n.y() + qy * v_n.z(), - qx * v_n.x() + qy * v_n.y() + qz * v_n.z()); + dR_dq.col(0) = 2 * Eigen::Vector3d( + qw * v_n.x() + qz * v_n.y() - qy * v_n.z(), + -qz * v_n.x() + qw * v_n.y() + qx * v_n.z(), + qy * v_n.x() - qx * v_n.y() + qw * v_n.z() + ); + + dR_dq.col(1) = 2 * Eigen::Vector3d( + qx * v_n.x() + qy * v_n.y() + qz * v_n.z(), + qy * v_n.x() - qx * v_n.y() - qw * v_n.z(), + qz * v_n.x() + qw * v_n.y() - qx * v_n.z() + ); + + dR_dq.col(2) = 2 * Eigen::Vector3d( + -qy * v_n.x() + qx * v_n.y() + qw * v_n.z(), + qx * v_n.x() + qy * v_n.y() + qz * v_n.z(), + -qw * v_n.x() + qz * v_n.y() - qy * v_n.z() + ); + + dR_dq.col(3) = 2 * Eigen::Vector3d( + -qz * v_n.x() - qw * v_n.y() + qx * v_n.z(), + qw * v_n.x() - qz * v_n.y() + qy * v_n.z(), + qx * v_n.x() + qy * v_n.y() + qz * v_n.z() + ); Hx.block<3, 4>(0, 6) = dR_dq; @@ -98,8 +100,7 @@ Eigen::Matrix3x18d ESKF::calculate_H(const state_quat& nom_state) { Eigen::Matrix3x1d ESKF::calculate_h(const state_quat& nom_state) { Eigen::Matrix3x1d h; - Eigen::Matrix3d R_bn = - nom_state.quat.normalized().toRotationMatrix().transpose(); + Eigen::Matrix3d R_bn = nom_state.quat.normalized().toRotationMatrix().transpose(); h = R_bn * nom_state.vel; @@ -109,18 +110,14 @@ Eigen::Matrix3x1d ESKF::calculate_h(const state_quat& nom_state) { state_quat ESKF::nominal_state_discrete(const state_quat& nom_state, const imu_measurement& imu_meas, const double dt) { - Eigen::Vector3d acc = - nom_state.get_R() * (imu_meas.accel - nom_state.accel_bias) + - nom_state.gravity; + Eigen::Vector3d acc = nom_state.get_R() * (imu_meas.accel - nom_state.accel_bias) + nom_state.gravity; Eigen::Vector3d gyro = (imu_meas.gyro - nom_state.gyro_bias) * dt; state_quat next_nom_state; - next_nom_state.pos = - nom_state.pos + nom_state.vel * dt + 0.5 * sq(dt) * acc; + + next_nom_state.pos = nom_state.pos + nom_state.vel * dt + 0.5 * sq(dt) * acc; next_nom_state.vel = nom_state.vel + dt * acc; - next_nom_state.quat = - (nom_state.quat * - Eigen::Quaterniond(0, 0.5 * gyro.x(), 0.5 * gyro.y(), 0.5 * gyro.z())); + next_nom_state.quat = (nom_state.quat * vector3d_to_quaternion(gyro)); next_nom_state.quat.normalize(); next_nom_state.gyro_bias = nom_state.gyro_bias; next_nom_state.accel_bias = nom_state.accel_bias; @@ -135,7 +132,7 @@ state_euler ESKF::error_state_prediction(const state_euler& error_state, const double dt) { Eigen::Matrix3d R = nom_state.get_R(); Eigen::Vector3d acc = (imu_meas.accel - nom_state.accel_bias); - Eigen::Vector3d gyro = imu_meas.gyro - nom_state.gyro_bias; + Eigen::Vector3d gyro = (imu_meas.gyro - nom_state.gyro_bias); Eigen::Matrix18d A_c = Eigen::Matrix18d::Zero(); A_c.block<3, 3>(0, 3) = Eigen::Matrix3d::Identity(); @@ -191,10 +188,7 @@ std::pair ESKF::injection_and_reset( next_nom_state.pos = nom_state.pos + error_state.pos; next_nom_state.vel = nom_state.vel + error_state.vel; - next_nom_state.quat = - nom_state.quat * Eigen::Quaterniond(1, 0.5 * error_state.euler.x(), - 0.5 * error_state.euler.y(), - 0.5 * error_state.euler.z()); + next_nom_state.quat = nom_state.quat * vector3d_to_quaternion(error_state.euler); next_nom_state.quat.normalize(); next_nom_state.gyro_bias = nom_state.gyro_bias + error_state.gyro_bias; next_nom_state.accel_bias = nom_state.accel_bias + error_state.accel_bias; @@ -205,12 +199,6 @@ std::pair ESKF::injection_and_reset( Eigen::Matrix18d G = Eigen::Matrix18d::Identity(); new_error_state.covariance = G * error_state.covariance * G.transpose(); - new_error_state.pos = Eigen::Vector3d::Zero(); - new_error_state.vel = Eigen::Vector3d::Zero(); - new_error_state.euler = Eigen::Vector3d::Zero(); - new_error_state.gyro_bias = Eigen::Vector3d::Zero(); - new_error_state.accel_bias = Eigen::Vector3d::Zero(); - new_error_state.gravity = Eigen::Vector3d::Zero(); return {next_nom_state, new_error_state}; } @@ -221,8 +209,7 @@ std::pair ESKF::imu_update( const imu_measurement& imu_meas, const double dt) { state_quat next_nom_state = nominal_state_discrete(nom_state, imu_meas, dt); - state_euler next_error_state = - error_state_prediction(error_state, nom_state, imu_meas, dt); + state_euler next_error_state = error_state_prediction(error_state, next_nom_state, imu_meas, dt); return {next_nom_state, next_error_state}; } @@ -231,10 +218,8 @@ std::pair ESKF::dvl_update( const state_quat& nom_state, const state_euler& error_state, const dvl_measurement& dvl_meas) { - state_euler new_error_state = - measurement_update(nom_state, error_state, dvl_meas); - auto [updated_nom_state, updated_error_state] = - injection_and_reset(nom_state, new_error_state); + state_euler new_error_state = measurement_update(nom_state, error_state, dvl_meas); + auto [updated_nom_state, updated_error_state] = injection_and_reset(nom_state, new_error_state); return {updated_nom_state, updated_error_state}; } diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp index a3b9c5cf3..524b48c29 100644 --- a/navigation/eskf/src/eskf_ros.cpp +++ b/navigation/eskf/src/eskf_ros.cpp @@ -1,63 +1,64 @@ #include "eskf/eskf_ros.hpp" -#include -#include #include "eskf/eskf_utils.hpp" #include "eskf/typedefs.hpp" +#include ESKFNode::ESKFNode() : Node("eskf_node") { time_step = std::chrono::milliseconds(1); - odom_pub_timer_ = this->create_wall_timer( - time_step, std::bind(&ESKFNode::publish_odom, this)); + odom_pub_timer_ = this->create_wall_timer(time_step, std::bind(&ESKFNode::publish_odom, this)); set_subscribers_and_publisher(); set_parameters(); + + spdlog::info("ESKF Node Initialized"); } void ESKFNode::set_subscribers_and_publisher() { rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; - auto qos_sensor_data = rclcpp::QoS( - rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); + auto qos_sensor_data = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); this->declare_parameter("imu_topic", "imu/data_raw"); std::string imu_topic = this->get_parameter("imu_topic").as_string(); - imu_sub_ = this->create_subscription( - imu_topic, qos_sensor_data, - std::bind(&ESKFNode::imu_callback, this, std::placeholders::_1)); + imu_sub_ = this->create_subscription(imu_topic, qos_sensor_data, std::bind(&ESKFNode::imu_callback, this, std::placeholders::_1)); this->declare_parameter("dvl_topic", "/orca/twist"); std::string dvl_topic = this->get_parameter("dvl_topic").as_string(); - dvl_sub_ = this->create_subscription< - geometry_msgs::msg::TwistWithCovarianceStamped>( - dvl_topic, qos_sensor_data, - std::bind(&ESKFNode::dvl_callback, this, std::placeholders::_1)); + dvl_sub_ = this->create_subscription(dvl_topic, qos_sensor_data, std::bind(&ESKFNode::dvl_callback, this, std::placeholders::_1)); + this->declare_parameter("odom_topic", "odom"); std::string odom_topic = this->get_parameter("odom_topic").as_string(); - odom_pub_ = this->create_publisher( - odom_topic, qos_sensor_data); + odom_pub_ = this->create_publisher(odom_topic, qos_sensor_data); } void ESKFNode::set_parameters() { + + std::vector diag_Q_std; + this->declare_parameter>("diag_Q_std"); // gyroscope bias noise + + diag_Q_std = this->get_parameter("diag_Q_std").as_double_array(); + Eigen::Matrix12d Q; Q.setZero(); - Q.diagonal() << sq(0.0103), sq(0.0118), sq(0.0043), // acceleration noise - sq(0.00193), sq(0.00306), sq(0.00118), // gyroscope noise - sq(0.05), sq(0.05), sq(0.05), // acceleration bias noise - sq(0.03), sq(0.03), sq(0.03); // gyroscope bias noise - + spdlog::info("Q diagonal: {}",diag_Q_std[0]); + Q.diagonal() << + sq(diag_Q_std[0]), sq(diag_Q_std[1]), sq(diag_Q_std[2]), // acceleration noise + sq(diag_Q_std[3]), sq(diag_Q_std[4]), sq(diag_Q_std[5]), // gyroscope noise + sq(diag_Q_std[6]), sq(diag_Q_std[7]), sq(diag_Q_std[8]), // acceleration bias noise + sq(diag_Q_std[9]), sq(diag_Q_std[10]), sq(diag_Q_std[11]); // gyroscope bias noise eskf_params_.Q = Q; eskf_ = std::make_unique(eskf_params_); Eigen::Matrix18d P; P.setZero(); - P.diagonal() << 0.1, 0.1, 0.1, // position - 0.1, 0.1, 0.1, // velocity - 0.1, 0.1, 0.1, // euler angles - 0.01, 0.01, 0.01, // accel bias - 0.01, 0.01, 0.01, // gyro bias - 0.001, 0.001, 0.001; // gravity + P.diagonal() << 1.0, 1.0, 1.0, // position + 0.1, 0.1, 0.1, // velocity + 0.1, 0.1, 0.1, // euler angles + 0.001, 0.001, 0.001, // accel bias + 0.001, 0.001, 0.001, // gyro bias + 0.001, 0.001, 0.001; // gravity error_state_.covariance = P; } @@ -71,30 +72,24 @@ void ESKFNode::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { return; } - double dt = (current_time - last_imu_time_).seconds(); + double dt = (current_time - last_imu_time_).nanoseconds() * 1e-9; last_imu_time_ = current_time; - imu_meas_.accel << msg->linear_acceleration.x, msg->linear_acceleration.y, - msg->linear_acceleration.z; - imu_meas_.gyro << msg->angular_velocity.x, msg->angular_velocity.y, - msg->angular_velocity.z; + imu_meas_.accel_uncorrected << msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z; + imu_meas_.gyro_uncorrected << msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z; + imu_meas_.correct(); - std::tie(nom_state_, error_state_) = - eskf_->imu_update(nom_state_, error_state_, imu_meas_, dt); + std::tie(nom_state_, error_state_) = eskf_->imu_update(nom_state_, error_state_, imu_meas_, dt); } void ESKFNode::dvl_callback( const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { - dvl_meas_.vel << msg->twist.twist.linear.x, msg->twist.twist.linear.y, - msg->twist.twist.linear.z; - dvl_meas_.cov << msg->twist.covariance[0], msg->twist.covariance[1], - msg->twist.covariance[2], msg->twist.covariance[6], - msg->twist.covariance[7], msg->twist.covariance[8], - msg->twist.covariance[12], msg->twist.covariance[13], - msg->twist.covariance[14]; - - std::tie(nom_state_, error_state_) = - eskf_->dvl_update(nom_state_, error_state_, dvl_meas_); + dvl_meas_.vel << msg->twist.twist.linear.x, msg->twist.twist.linear.y, msg->twist.twist.linear.z; + dvl_meas_.cov << msg->twist.covariance[0], msg->twist.covariance[1], msg->twist.covariance[2], + msg->twist.covariance[6], msg->twist.covariance[7], msg->twist.covariance[8], + msg->twist.covariance[12], msg->twist.covariance[13], msg->twist.covariance[14]; + + std::tie(nom_state_, error_state_) = eskf_->dvl_update(nom_state_, error_state_, dvl_meas_); } void ESKFNode::publish_odom() { diff --git a/navigation/eskf/src/eskf_utils.cpp b/navigation/eskf/src/eskf_utils.cpp index 4d33ec7ce..7a668adfc 100644 --- a/navigation/eskf/src/eskf_utils.cpp +++ b/navigation/eskf/src/eskf_utils.cpp @@ -11,3 +11,21 @@ Eigen::Matrix3d skew(const Eigen::Vector3d& v) { double sq(const double& value) { return value * value; } + +Eigen::Quaterniond vector3d_to_quaternion(const Eigen::Vector3d& vector) { + double angle = vector.norm(); + if (angle < 1e-8) { + return Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0); + } else { + Eigen::Vector3d axis = vector / angle; + return Eigen::Quaterniond(Eigen::AngleAxisd(angle, axis)); + } +} + +Eigen::Quaterniond euler_to_quaternion(const Eigen::Vector3d& euler) { + Eigen::Quaterniond q; + q = Eigen::AngleAxisd(euler.z(), Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(euler.y(), Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(euler.x(), Eigen::Vector3d::UnitX()); + return q; +} \ No newline at end of file diff --git a/navigation/eskf_python/eskf_python/eskf_python_filter.py b/navigation/eskf_python/eskf_python/eskf_python_filter.py index 245d7ece4..890226639 100644 --- a/navigation/eskf_python/eskf_python/eskf_python_filter.py +++ b/navigation/eskf_python/eskf_python/eskf_python_filter.py @@ -25,54 +25,6 @@ def __init__( self.p_accBias = p_accBias self.p_gyroBias = p_gyroBias - def Fx(self, imu_data: Measurement) -> np.ndarray: - """Calculates the state transition matrix. - - Args: - imu_data (np.ndarray): The IMU data. - - Returns: - np.ndarray: The state transition matrix. - """ - F_x = np.zeros((18, 18)) - I = np.eye(3) - - F_x[0:3, 0:3] = I - F_x[0:3, 3:6] = self.dt * I - F_x[3:6, 3:6] = I - F_x[3:6, 6:9] = ( - -self.nom_state.R_q() - @ skew_matrix(imu_data.acceleration - self.nom_state.acceleration_bias) - * self.dt - ) - F_x[6:9, 6:9] = R_from_angle_axis( - (imu_data.angular_velocity - self.nom_state.gyro_bias) * self.dt - ).T - F_x[3:6, 9:12] = -self.nom_state.R_q() * self.dt - F_x[3:6, 15:18] = I * self.dt - F_x[6:9, 12:15] = -I * self.dt - F_x[9:12, 9:12] = I - F_x[12:15, 12:15] = I - F_x[15:18, 15:18] = I - - return F_x - - def Fi(self) -> np.ndarray: - """Calculates the input matrix. - - Returns: - np.ndarray: The input matrix. - """ - F_i = np.zeros((18, 12)) - I = np.eye(3) - - F_i[3:6, 0:3] = I - F_i[6:9, 3:6] = I - F_i[9:12, 6:9] = I - F_i[12:15, 9:12] = I - - return F_i - def Q_delta_theta(self) -> np.ndarray: """Calculates the Q_delta_theta matrix. See Joan Solà. Quaternion kinematics for the error-state Kalman filter. @@ -92,26 +44,58 @@ def Q_delta_theta(self) -> np.ndarray: return Q_delta_theta def Hx(self) -> np.ndarray: - """Calculates the Jacobian of the measurement model. - - Returns: - np.ndarray: The Jacobian of the measurement model. """ - Hx = np.zeros((3, 19)) - - q0, q1, q2, q3 = self.nom_state.orientation - e = np.array([q1, q2, q3]) - v = self.nom_state.velocity - - temp_nu = -2 * skew_matrix(e) @ v - temp_eps = 2 * (q0 * np.eye(3) + skew_matrix(e)) @ skew_matrix(v) - - Hx[0:3, 3:6] = self.nom_state.R_q() - Hx[0:3, 6:10] = np.vstack([temp_nu, temp_eps]).T + Computes the true-state measurement Jacobian for the measurement + h(x) = R(q) * velocity, + where: + - R(q) is the rotation matrix from the quaternion (q, q, q, q) with q as the scalar part, + - velocity is a 3-vector. + + The state is assumed to be ordered as: + [position (3), velocity (3), quaternion (4), ...] (total length 19). + + The Jacobian Hx is a 3x19 matrix with nonzero blocks: + - Columns 3:6 (velocity): R(q) + - Columns 6:10 (quaternion): + """ + q = self.nom_state.orientation # shape (4,) + v = self.nom_state.velocity # shape (3,) + q0, q1, q2, q3 = q + v1, v2, v3 = v + + R = self.nom_state.R_q().transpose() # shape (3, 3) + + dhdq0 = 2 * np.array([ + q0 * v1 - q3 * v2 + q2 * v3, + q3 * v1 + q0 * v2 - q1 * v3, + -q2 * v1 + q1 * v2 + q0 * v3 + ]) + + dhdq1 = 2 * np.array([ + q1 * v1 + q2 * v2 + q3 * v3, + q2 * v1 - q1 * v2 - q0 * v3, + q3 * v1 + q0 * v2 - q1 * v3 + ]) + + dhdq2 = 2 * np.array([ + -q2 * v1 + q1 * v2 + q0 * v3, + q1 * v1 + q2 * v2 + q3 * v3, + -q0 * v1 + q3 * v2 - q2 * v3 + ]) + + dhdq3 = 2 * np.array([ + -q3 * v1 - q0 * v2 + q1 * v3, + q0 * v1 - q3 * v2 + q2 * v3, + q1 * v1 + q2 * v2 + q3 * v3 + ]) + + dHdq = np.column_stack((dhdq0, dhdq1, dhdq2, dhdq3)) # shape (3, 4) + Hx = np.zeros((3, 19)) - Hx[0:3, 3:6] = np.eye(3) - + Hx[:, 3:6] = R + Hx[:, 6:10] = dHdq + return Hx def H(self) -> np.ndarray: @@ -132,7 +116,7 @@ def h(self) -> np.ndarray: Returns: np.ndarray: The measurement model. """ - return self.nom_state.velocity # self.nom_state.R_q() @ self.nom_state.velocity + return self.nom_state.R_q() @ self.nom_state.velocity def nominal_state_discrete(self, imu_data: Measurement) -> None: """Calculates the next nominal state using the discrete-time process model defined in: diff --git a/navigation/eskf_python/eskf_python/eskf_test.py b/navigation/eskf_python/eskf_python/eskf_test.py index 53ad83958..0930d9f6c 100644 --- a/navigation/eskf_python/eskf_python/eskf_test.py +++ b/navigation/eskf_python/eskf_python/eskf_test.py @@ -313,7 +313,7 @@ def simulate_eskf(): ### _______ NIS AND NEEDS _______ - +""" num_simulations = 10 NIS_runs = [] @@ -356,3 +356,4 @@ def simulate_eskf(): plt.tight_layout() plt.show() +""" \ No newline at end of file diff --git a/navigation/ukf_okid/ukf_python/ukf_okid.py b/navigation/ukf_okid/ukf_python/ukf_okid.py index f7000adb4..16312935a 100644 --- a/navigation/ukf_okid/ukf_python/ukf_okid.py +++ b/navigation/ukf_okid/ukf_python/ukf_okid.py @@ -16,7 +16,7 @@ def __init__(self, process_model: process_model, x_0, P_0, Q, R): self.weight = None self.T = self.generate_T_matrix(len(P_0)) - def generate_T_matrix(n: float) -> np.ndarray: + def generate_T_matrix(self, n: float) -> np.ndarray: """ Generates the orthonormal transformation matrix T used in the TUKF sigma point generation. @@ -26,10 +26,10 @@ def generate_T_matrix(n: float) -> np.ndarray: Returns: T (np.ndarray): An n x 2n orthonormal transformation matrix used to generate TUKF sigma points. """ - T = np.zeros((n, 2 * n)) + T = np.zeros((n, n)) - for i in range(1, 2 * n + 1): - for j in range(1, (n // 2) + 1): + for i in range(n): + for j in range(n//2): T[2 * j - 2, i - 1] = np.sqrt(2) * np.cos(((2 * j - 1) * i * np.pi) / n) T[2 * j - 1, i - 1] = np.sqrt(2) * np.sin(((2 * j - 1) * i * np.pi) / n) @@ -54,8 +54,9 @@ def sigma_points(self, current_state: StateQuat) -> list[StateQuat]: self.sigma_points_list = [StateQuat() for _ in range(2 * n)] - for state in self.sigma_points_list: - state.fill_states_different_dim(current_state.as_vector(), + for index, state in enumerate(self.sigma_points_list): + delta_x = S @ delta[:, index] + state.fill_states_different_dim(current_state.as_vector(), delta_x) return self.sigma_points_list @@ -65,20 +66,20 @@ def unscented_transform(self, current_state: StateQuat) -> StateQuat: The unscented transform function generates the priori state estimate """ - _ , _ = self.sigma_points(current_state) + _ = self.sigma_points(current_state) n = len(current_state.covariance) - self.y_i = [StateQuat() for _ in range(2 * n + 1)] + self.y_i = [StateQuat() for _ in range(2 * n)] - for i in range(2 * n + 1): + for i in range(2 * n ): self.process_model.model_prediction(self.sigma_points_list[i]) self.y_i[i] = self.process_model.euler_forward() state_estimate = StateQuat() - x = mean_set(self.y_i, self.weight) + x = mean_set(self.y_i) state_estimate.fill_states(x) - state_estimate.covariance = covariance_set(self.y_i, x, self.weight) + state_estimate.covariance = covariance_set(self.y_i, x) return state_estimate def measurement_update(self, current_state: StateQuat, measurement: MeasModel) -> tuple[MeasModel, np.ndarray]: @@ -88,18 +89,18 @@ def measurement_update(self, current_state: StateQuat, measurement: MeasModel) - """ n = len(current_state.covariance) - z_i = [MeasModel() for _ in range(2 * n + 1)] + z_i = [MeasModel() for _ in range(2 * n)] - for i in range(2 * n + 1): + for i in range(2 * n): z_i[i] = measurement.H(self.sigma_points_list[i]) meas_update = MeasModel() - meas_update.measurement = mean_measurement(z_i, self.weight) + meas_update.measurement = mean_measurement(z_i) - meas_update.covariance = covariance_measurement(z_i, meas_update.measurement, self.weight) + meas_update.covariance = covariance_measurement(z_i, meas_update.measurement) - cross_correlation = cross_covariance(self.y_i, current_state.as_vector(), z_i, meas_update.measurement, self.weight) + cross_correlation = cross_covariance(self.y_i, current_state.as_vector(), z_i, meas_update.measurement) return meas_update, cross_correlation diff --git a/navigation/ukf_okid/ukf_python/ukf_okid_class.py b/navigation/ukf_okid/ukf_python/ukf_okid_class.py index 50f1988b4..4d3281260 100644 --- a/navigation/ukf_okid/ukf_python/ukf_okid_class.py +++ b/navigation/ukf_okid/ukf_python/ukf_okid_class.py @@ -395,7 +395,7 @@ def mean_measurement(set_points: list[MeasModel]) -> np.ndarray: return mean_value.measurement -def covariance_set(set_points: list[StateQuat], mean: StateQuat) -> np.ndarray: +def covariance_set(set_points: list[StateQuat], mean: np.ndarray) -> np.ndarray: """ Function that calculates the covariance of a set of points """ @@ -403,9 +403,9 @@ def covariance_set(set_points: list[StateQuat], mean: StateQuat) -> np.ndarray: covariance = np.zeros(set_points[0].covariance.shape) mean_quat = StateQuat() - mean_quat.fill_states(mean.as_vector()) + mean_quat.fill_states(mean) - mean_q = mean.orientation + mean_q = mean_quat.orientation for state in set_points: q = state.orientation diff --git a/navigation/ukf_okid/ukf_python/ukf_test.py b/navigation/ukf_okid/ukf_python/ukf_test.py index 5a7e9eaba..3c197ed63 100644 --- a/navigation/ukf_okid/ukf_python/ukf_test.py +++ b/navigation/ukf_okid/ukf_python/ukf_test.py @@ -31,293 +31,267 @@ def add_quaternion_noise(q, noise_std): if __name__ == '__main__': - # Define a mean StateQuat - mean_state = StateQuat() - mean_state.position = np.array([1.0, 2.0, 3.0]) - mean_state.orientation = np.array([1.0, 0.0, 0.0, 0.0]) # Quaternion - mean_state.velocity = np.array([0.5, 0.5, 0.5]) - mean_state.angular_velocity = np.array([0.1, 0.1, 0.1]) - - test_state = StateQuat() - test_state.position = np.array([1.0, 1.0, 1.0]) - test_state.orientation = np.array([0.0, 1.0, 0.0, 0.0]) # Quaternion - test_state.velocity = np.array([0.2, 0.2, 0.2]) - test_state.angular_velocity = np.array([0.2, 0.2, 0.2]) - - # Create a set with only one element - state_set = list() - state_set.append(test_state) - print(len(state_set)) - - # Compute the mean - mean = mean_set(state_set) - - # Compute the covariance - mean_state.covariance = covariance_set(state_set, mean_state) - - # Print the results - print("Mean State:") - print_StateQuat(mean_state) - - # # Create initial state vector and covariance matrix. - # x0 = np.zeros(13) - # x0[0:3] = [0.3, 0.3, 0.3] - # x0[3] = 1 - # x0[7:10] = [0.2, 0.2, 0.2] - # dt = 0.01 - # R = (0.01) * np.eye(3) + # Create initial state vector and covariance matrix. + x0 = np.zeros(13) + x0[0:3] = [0.3, 0.3, 0.3] + x0[3] = 1 + x0[7:10] = [0.2, 0.2, 0.2] + dt = 0.01 + R = (0.01) * np.eye(3) - # Q = 0.00015 * np.eye(12) - # P0 = np.eye(12) * 0.0001 - - # model = process_model() - # model.dt = 0.01 - # model.mass_interia_matrix = np.array([ - # [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] - # ]) - # model.m = 30.0 - # model.r_b_bg = np.array([0.01, 0.0, 0.02]) - # model.inertia = np.diag([0.68, 3.32, 3.34]) - # model.damping_linear = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) - # model.damping_nonlinear = np.array([0.3, 0.3, 0.3, 0.3, 0.3, 0.3]) - # model.added_mass = np.diag([1.0, 1.0, 1.0, 2.0, 2.0, 2.0]) - - # model_ukf = process_model() - # model_ukf.dt = 0.01 - # model_ukf.mass_interia_matrix = np.array([ - # [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] - # ]) - # model_ukf.m = 30.0 - # model_ukf.r_b_bg = np.array([0.01, 0.0, 0.02]) - # model_ukf.inertia = np.diag([0.68, 3.32, 3.34]) - # model_ukf.damping_linear = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) - # model_ukf.damping_nonlinear = np.array([0.3, 0.3, 0.3, 0.3, 0.3, 0.3]) - # model_ukf.added_mass = np.diag([1.0, 1.0, 1.0, 2.0, 2.0, 2.0]) - - # # Simulation parameters - # simulation_time = 20 # seconds - # num_steps = int(simulation_time / dt) - - # # Initialize a dummy StateQuat. - # new_state = StateQuat() - # new_state.fill_states(x0) - # new_state.covariance = P0 - - # test_state_x = StateQuat() - # test_state_x.fill_states(x0) - # test_state_x.covariance = P0 - - # # Initialize a estimated state - # estimated_state = StateQuat() - # estimated_state.fill_states(x0) - # estimated_state.covariance = P0 - - # # Initialize a estimated state - # noisy_state = StateQuat() - # noisy_state.fill_states(x0) - # noisy_state.covariance = P0 - - # measurment_model = MeasModel() - # measurment_model.measurement = np.array([0.0, 0.0, 0.0]) - # measurment_model.covariance = R - - # # Initialize arrays to store the results - # positions = np.zeros((num_steps, 3)) - # orientations = np.zeros((num_steps, 3)) - # velocities = np.zeros((num_steps, 3)) - # angular_velocities = np.zeros((num_steps, 3)) - - # # Initialize arrays to store the estimates - # positions_est = np.zeros((num_steps, 3)) - # orientations_est = np.zeros((num_steps, 3)) - # velocities_est = np.zeros((num_steps, 3)) - # angular_velocities_est = np.zeros((num_steps, 3)) - - # # Initialize the okid params - # okid_params = np.zeros((num_steps, 21)) - - # model.state_vector_prev = new_state - # model.state_vector = new_state - - # model_ukf.state_vector_prev = test_state_x - # model_ukf.state_vector = test_state_x - - # # initialize the ukf - # ukf = UKF(model_ukf, x0, P0, Q, R) - - # elapsed_times = [] - - # u = lambda t: np.array([2 * np.sin(1 * t), 2 * np.sin(1 * t), 2 * np.sin(1 * t), 0.2 * np.cos(1 * t), 0.2 * np.cos(1 * t), 0.2 * np.cos(1 * t)]) - - # # Run the simulation - # for step in range(num_steps): - # # Insert control input - # model.Control_input = u(step * dt) - # model_ukf.Control_input = u(step * dt) - - # # Perform the unscented transform - # model.model_prediction(new_state) - # new_state = model.euler_forward() - - # # Adding noise in the state vector - # estimated_state.position = estimated_state.position # + np.random.normal(0, 0.01, 3) - # estimated_state.orientation = estimated_state.orientation #add_quaternion_noise(estimated_state.orientation, 0.01) - # estimated_state.velocity = estimated_state.velocity # + np.random.normal(0, 0.01, 3) - # estimated_state.angular_velocity = estimated_state.angular_velocity # + np.random.normal(0, 0.01, 3) - - # start_time = time.time() - # estimated_state = ukf.unscented_transform(estimated_state) - # elapsed_time = time.time() - start_time - # elapsed_times.append(elapsed_time) - - # if step % 10 == 0: - # measurment_model.measurement = new_state.velocity # + np.random.normal(0, 0.01, 3) - # meas_update, covariance_matrix = ukf.measurement_update(estimated_state, measurment_model) - # estimated_state = ukf.posteriori_estimate(estimated_state, covariance_matrix, measurment_model, meas_update) - - - # positions[step, :] = new_state.position - # orientations[step, :] = quat_to_euler(new_state.orientation) - # velocities[step, :] = new_state.velocity - # angular_velocities[step, :] = new_state.angular_velocity - - # positions_est[step, :] = estimated_state.position - # orientations_est[step, :] = quat_to_euler(estimated_state.orientation) - # velocities_est[step, :] = estimated_state.velocity - # angular_velocities_est[step, :] = estimated_state.angular_velocity + Q = 0.00015 * np.eye(12) + P0 = np.eye(12) * 0.0001 + + model = process_model() + model.dt = 0.01 + model.mass_interia_matrix = np.array([ + [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] + ]) + model.m = 30.0 + model.r_b_bg = np.array([0.01, 0.0, 0.02]) + model.inertia = np.diag([0.68, 3.32, 3.34]) + model.damping_linear = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) + model.damping_nonlinear = np.array([0.3, 0.3, 0.3, 0.3, 0.3, 0.3]) + model.added_mass = np.diag([1.0, 1.0, 1.0, 2.0, 2.0, 2.0]) + + model_ukf = process_model() + model_ukf.dt = 0.01 + model_ukf.mass_interia_matrix = np.array([ + [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] + ]) + model_ukf.m = 30.0 + model_ukf.r_b_bg = np.array([0.01, 0.0, 0.02]) + model_ukf.inertia = np.diag([0.68, 3.32, 3.34]) + model_ukf.damping_linear = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) + model_ukf.damping_nonlinear = np.array([0.3, 0.3, 0.3, 0.3, 0.3, 0.3]) + model_ukf.added_mass = np.diag([1.0, 1.0, 1.0, 2.0, 2.0, 2.0]) + + # Simulation parameters + simulation_time = 5 # seconds + num_steps = int(simulation_time / dt) + + # Initialize a dummy StateQuat. + new_state = StateQuat() + new_state.fill_states(x0) + new_state.covariance = P0 + + test_state_x = StateQuat() + test_state_x.fill_states(x0) + test_state_x.covariance = P0 + + # Initialize a estimated state + estimated_state = StateQuat() + estimated_state.fill_states(x0) + estimated_state.covariance = P0 + + # Initialize a estimated state + noisy_state = StateQuat() + noisy_state.fill_states(x0) + noisy_state.covariance = P0 + + measurment_model = MeasModel() + measurment_model.measurement = np.array([0.0, 0.0, 0.0]) + measurment_model.covariance = R + + # Initialize arrays to store the results + positions = np.zeros((num_steps, 3)) + orientations = np.zeros((num_steps, 3)) + velocities = np.zeros((num_steps, 3)) + angular_velocities = np.zeros((num_steps, 3)) + + # Initialize arrays to store the estimates + positions_est = np.zeros((num_steps, 3)) + orientations_est = np.zeros((num_steps, 3)) + velocities_est = np.zeros((num_steps, 3)) + angular_velocities_est = np.zeros((num_steps, 3)) + + # Initialize the okid params + okid_params = np.zeros((num_steps, 21)) + + model.state_vector_prev = new_state + model.state_vector = new_state + + model_ukf.state_vector_prev = test_state_x + model_ukf.state_vector = test_state_x + + # initialize the ukf + ukf = UKF(model_ukf, x0, P0, Q, R) + + elapsed_times = [] + + u = lambda t: np.array([2 * np.sin(1 * t), 2 * np.sin(1 * t), 2 * np.sin(1 * t), 0.2 * np.cos(1 * t), 0.2 * np.cos(1 * t), 0.2 * np.cos(1 * t)]) + + # Run the simulation + for step in range(num_steps): + # Insert control input + model.Control_input = u(step * dt) + model_ukf.Control_input = u(step * dt) + + # Perform the unscented transform + model.model_prediction(new_state) + new_state = model.euler_forward() + + # Adding noise in the state vector + estimated_state.position = estimated_state.position # + np.random.normal(0, 0.01, 3) + estimated_state.orientation = estimated_state.orientation #add_quaternion_noise(estimated_state.orientation, 0.01) + estimated_state.velocity = estimated_state.velocity # + np.random.normal(0, 0.01, 3) + estimated_state.angular_velocity = estimated_state.angular_velocity # + np.random.normal(0, 0.01, 3) + + start_time = time.time() + estimated_state = ukf.unscented_transform(estimated_state) + print(estimated_state.as_vector()) + break + elapsed_time = time.time() - start_time + elapsed_times.append(elapsed_time) + + if step % 20 == 0: + measurment_model.measurement = new_state.velocity # + np.random.normal(0, 0.01, 3) + meas_update, covariance_matrix = ukf.measurement_update(estimated_state, measurment_model) + estimated_state = ukf.posteriori_estimate(estimated_state, covariance_matrix, measurment_model, meas_update) + + + positions[step, :] = new_state.position + orientations[step, :] = quat_to_euler(new_state.orientation) + velocities[step, :] = new_state.velocity + angular_velocities[step, :] = new_state.angular_velocity + + positions_est[step, :] = estimated_state.position + orientations_est[step, :] = quat_to_euler(estimated_state.orientation) + velocities_est[step, :] = estimated_state.velocity + angular_velocities_est[step, :] = estimated_state.angular_velocity - # # Update the state for the next iteration - # model.state_vector_prev = new_state - - # print('Average elapsed time: ', np.mean(elapsed_times)) - # print('Max elapsed time: ', np.max(elapsed_times)) - # print('Min elapsed time: ', np.min(elapsed_times)) - # print('median elapsed time: ', np.median(elapsed_times)) - # # Plot the results - # time = np.linspace(0, simulation_time, num_steps) - - # # Plot positions - # plt.figure() - # plt.subplot(3, 1, 1) - # plt.plot(time, positions[:, 0], label='True') - # plt.plot(time, positions_est[:, 0], label='Estimated') - # plt.title('Position X') - # plt.xlabel('Time [s]') - # plt.ylabel('Position X [m]') - # plt.legend() - - # plt.subplot(3, 1, 2) - # plt.plot(time, positions[:, 1], label='True') - # plt.plot(time, positions_est[:, 1], label='Estimated') - # plt.title('Position Y') - # plt.xlabel('Time [s]') - # plt.ylabel('Position Y [m]') - # plt.legend() - - # plt.subplot(3, 1, 3) - # plt.plot(time, positions[:, 2], label='True') - # plt.plot(time, positions_est[:, 2], label='Estimated') - # plt.title('Position Z') - # plt.xlabel('Time [s]') - # plt.ylabel('Position Z [m]') - # plt.legend() - - # plt.tight_layout() - # plt.show() - - # # Plot orientations (Euler angles) - # plt.figure() - # plt.subplot(3, 1, 1) - # plt.plot(time, orientations[:, 0], label='True') - # plt.plot(time, orientations_est[:, 0], label='Estimated') - # plt.title('Orientation Roll') - # plt.xlabel('Time [s]') - # plt.ylabel('Roll [rad]') - # plt.legend() - - # plt.subplot(3, 1, 2) - # plt.plot(time, orientations[:, 1], label='True') - # plt.plot(time, orientations_est[:, 1], label='Estimated') - # plt.title('Orientation Pitch') - # plt.xlabel('Time [s]') - # plt.ylabel('Pitch [rad]') - # plt.legend() - - # plt.subplot(3, 1, 3) - # plt.plot(time, orientations[:, 2], label='True') - # plt.plot(time, orientations_est[:, 2], label='Estimated') - # plt.title('Orientation Yaw') - # plt.xlabel('Time [s]') - # plt.ylabel('Yaw [rad]') - # plt.legend() - - # plt.tight_layout() - # plt.show() - - # # Plot velocities - # plt.figure() - # plt.subplot(3, 1, 1) - # plt.plot(time, velocities[:, 0], label='True') - # plt.plot(time, velocities_est[:, 0], label='Estimated') - # plt.title('Velocity X') - # plt.xlabel('Time [s]') - # plt.ylabel('Velocity X [m/s]') - # plt.legend() - - # plt.subplot(3, 1, 2) - # plt.plot(time, velocities[:, 1], label='True') - # plt.plot(time, velocities_est[:, 1], label='Estimated') - # plt.title('Velocity Y') - # plt.xlabel('Time [s]') - # plt.ylabel('Velocity Y [m/s]') - # plt.legend() - - # plt.subplot(3, 1, 3) - # plt.plot(time, velocities[:, 2], label='True') - # plt.plot(time, velocities_est[:, 2], label='Estimated') - # plt.title('Velocity Z') - # plt.xlabel('Time [s]') - # plt.ylabel('Velocity Z [m/s]') - # plt.legend() - - # plt.tight_layout() - # plt.show() - - # # Plot angular velocities - # plt.figure() - # plt.subplot(3, 1, 1) - # plt.plot(time, angular_velocities[:, 0], label='True') - # plt.plot(time, angular_velocities_est[:, 0], label='Estimated') - # plt.title('Angular Velocity X') - # plt.xlabel('Time [s]') - # plt.ylabel('Angular Velocity X [rad/s]') - # plt.legend() - - # plt.subplot(3, 1, 2) - # plt.plot(time, angular_velocities[:, 1], label='True') - # plt.plot(time, angular_velocities_est[:, 1], label='Estimated') - # plt.title('Angular Velocity Y') - # plt.xlabel('Time [s]') - # plt.ylabel('Angular Velocity Y [rad/s]') - # plt.legend() - - # plt.subplot(3, 1, 3) - # plt.plot(time, angular_velocities[:, 2], label='True') - # plt.plot(time, angular_velocities_est[:, 2], label='Estimated') - # plt.title('Angular Velocity Z') - # plt.xlabel('Time [s]') - # plt.ylabel('Angular Velocity Z [rad/s]') - # plt.legend() - - # plt.tight_layout() - # plt.show() \ No newline at end of file + # Update the state for the next iteration + model.state_vector_prev = new_state + + print('Average elapsed time: ', np.mean(elapsed_times)) + print('Max elapsed time: ', np.max(elapsed_times)) + print('Min elapsed time: ', np.min(elapsed_times)) + print('median elapsed time: ', np.median(elapsed_times)) + # Plot the results + time = np.linspace(0, simulation_time, num_steps) + + # Plot positions + plt.figure() + plt.subplot(3, 1, 1) + plt.plot(time, positions[:, 0], label='True') + plt.plot(time, positions_est[:, 0], label='Estimated') + plt.title('Position X') + plt.xlabel('Time [s]') + plt.ylabel('Position X [m]') + plt.legend() + + plt.subplot(3, 1, 2) + plt.plot(time, positions[:, 1], label='True') + plt.plot(time, positions_est[:, 1], label='Estimated') + plt.title('Position Y') + plt.xlabel('Time [s]') + plt.ylabel('Position Y [m]') + plt.legend() + + plt.subplot(3, 1, 3) + plt.plot(time, positions[:, 2], label='True') + plt.plot(time, positions_est[:, 2], label='Estimated') + plt.title('Position Z') + plt.xlabel('Time [s]') + plt.ylabel('Position Z [m]') + plt.legend() + + plt.tight_layout() + plt.show() + + # Plot orientations (Euler angles) + plt.figure() + plt.subplot(3, 1, 1) + plt.plot(time, orientations[:, 0], label='True') + plt.plot(time, orientations_est[:, 0], label='Estimated') + plt.title('Orientation Roll') + plt.xlabel('Time [s]') + plt.ylabel('Roll [rad]') + plt.legend() + + plt.subplot(3, 1, 2) + plt.plot(time, orientations[:, 1], label='True') + plt.plot(time, orientations_est[:, 1], label='Estimated') + plt.title('Orientation Pitch') + plt.xlabel('Time [s]') + plt.ylabel('Pitch [rad]') + plt.legend() + + plt.subplot(3, 1, 3) + plt.plot(time, orientations[:, 2], label='True') + plt.plot(time, orientations_est[:, 2], label='Estimated') + plt.title('Orientation Yaw') + plt.xlabel('Time [s]') + plt.ylabel('Yaw [rad]') + plt.legend() + + plt.tight_layout() + plt.show() + + # Plot velocities + plt.figure() + plt.subplot(3, 1, 1) + plt.plot(time, velocities[:, 0], label='True') + plt.plot(time, velocities_est[:, 0], label='Estimated') + plt.title('Velocity X') + plt.xlabel('Time [s]') + plt.ylabel('Velocity X [m/s]') + plt.legend() + + plt.subplot(3, 1, 2) + plt.plot(time, velocities[:, 1], label='True') + plt.plot(time, velocities_est[:, 1], label='Estimated') + plt.title('Velocity Y') + plt.xlabel('Time [s]') + plt.ylabel('Velocity Y [m/s]') + plt.legend() + + plt.subplot(3, 1, 3) + plt.plot(time, velocities[:, 2], label='True') + plt.plot(time, velocities_est[:, 2], label='Estimated') + plt.title('Velocity Z') + plt.xlabel('Time [s]') + plt.ylabel('Velocity Z [m/s]') + plt.legend() + + plt.tight_layout() + plt.show() + + # Plot angular velocities + plt.figure() + plt.subplot(3, 1, 1) + plt.plot(time, angular_velocities[:, 0], label='True') + plt.plot(time, angular_velocities_est[:, 0], label='Estimated') + plt.title('Angular Velocity X') + plt.xlabel('Time [s]') + plt.ylabel('Angular Velocity X [rad/s]') + plt.legend() + + plt.subplot(3, 1, 2) + plt.plot(time, angular_velocities[:, 1], label='True') + plt.plot(time, angular_velocities_est[:, 1], label='Estimated') + plt.title('Angular Velocity Y') + plt.xlabel('Time [s]') + plt.ylabel('Angular Velocity Y [rad/s]') + plt.legend() + + plt.subplot(3, 1, 3) + plt.plot(time, angular_velocities[:, 2], label='True') + plt.plot(time, angular_velocities_est[:, 2], label='Estimated') + plt.title('Angular Velocity Z') + plt.xlabel('Time [s]') + plt.ylabel('Angular Velocity Z [rad/s]') + plt.legend() + + plt.tight_layout() + plt.show() \ No newline at end of file From db879dbc5dee5a756874fdaeaf330a0044845686 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 3 Apr 2025 17:22:08 +0000 Subject: [PATCH 093/290] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- navigation/eskf/include/eskf/typedefs.hpp | 40 ++- navigation/eskf/src/eskf.cpp | 69 ++-- navigation/eskf/src/eskf_ros.cpp | 79 +++-- navigation/eskf/src/eskf_utils.cpp | 2 +- .../eskf_python/eskf_python_class.py | 38 +-- .../eskf_python/eskf_python_filter.py | 87 ++--- .../eskf_python/eskf_python_node.py | 32 +- .../eskf_python/eskf_python_utils.py | 82 ++--- .../eskf_python/eskf_python/eskf_test.py | 3 +- .../eskf_python/eskf_test_utils.py | 131 ++++++-- navigation/ukf_okid/ukf_python/rest.py | 31 +- navigation/ukf_okid/ukf_python/ukf_okid.py | 54 +-- .../ukf_okid/ukf_python/ukf_okid_class.py | 307 +++++++++++------- navigation/ukf_okid/ukf_python/ukf_test.py | 106 +++--- navigation/ukf_okid/ukf_python/ukf_utils.py | 18 +- 15 files changed, 631 insertions(+), 448 deletions(-) diff --git a/navigation/eskf/include/eskf/typedefs.hpp b/navigation/eskf/include/eskf/typedefs.hpp index c0a1e41f9..925d8fd72 100644 --- a/navigation/eskf/include/eskf/typedefs.hpp +++ b/navigation/eskf/include/eskf/typedefs.hpp @@ -5,26 +5,26 @@ #ifndef ESKF_TYPEDEFS_H #define ESKF_TYPEDEFS_H -#include #include +#include namespace Eigen { - typedef Eigen::Matrix Vector19d; - typedef Eigen::Matrix Vector18d; - typedef Eigen::Matrix Matrix18d; - typedef Eigen::Matrix Matrix19d; - typedef Eigen::Matrix Matrix18x12d; - typedef Eigen::Matrix Matrix4x3d; - typedef Eigen::Matrix Matrix3x19d; - typedef Eigen::Matrix Matrix3x18d; - typedef Eigen::Matrix Matrix12d; - typedef Eigen::Matrix Matrix18d; - typedef Eigen::Matrix Matrix3x1d; - typedef Eigen::Matrix Matrix19x18d; - typedef Eigen::Matrix Matrix18x3d; - typedef Eigen::Matrix Matrix36d; - typedef Eigen::Matrix Matrix6d; - typedef Eigen::Matrix Matrix9d; +typedef Eigen::Matrix Vector19d; +typedef Eigen::Matrix Vector18d; +typedef Eigen::Matrix Matrix18d; +typedef Eigen::Matrix Matrix19d; +typedef Eigen::Matrix Matrix18x12d; +typedef Eigen::Matrix Matrix4x3d; +typedef Eigen::Matrix Matrix3x19d; +typedef Eigen::Matrix Matrix3x18d; +typedef Eigen::Matrix Matrix12d; +typedef Eigen::Matrix Matrix18d; +typedef Eigen::Matrix Matrix3x1d; +typedef Eigen::Matrix Matrix19x18d; +typedef Eigen::Matrix Matrix18x3d; +typedef Eigen::Matrix Matrix36d; +typedef Eigen::Matrix Matrix6d; +typedef Eigen::Matrix Matrix9d; } // namespace Eigen struct state_quat { @@ -89,10 +89,8 @@ struct imu_measurement { void correct() { Eigen::Matrix3d R_nb; - R_nb << 0, 0, -1, - 0, -1, 0, - -1, 0, 0; - + R_nb << 0, 0, -1, 0, -1, 0, -1, 0, 0; + accel = R_nb * accel_uncorrected; gyro = R_nb * gyro_uncorrected; } diff --git a/navigation/eskf/src/eskf.cpp b/navigation/eskf/src/eskf.cpp index 55d8516dd..58c532afa 100644 --- a/navigation/eskf/src/eskf.cpp +++ b/navigation/eskf/src/eskf.cpp @@ -6,8 +6,7 @@ #include "eskf/eskf_utils.hpp" #include "eskf/typedefs.hpp" -ESKF::ESKF(const eskf_params& params) : - Q_(params.Q) {} +ESKF::ESKF(const eskf_params& params) : Q_(params.Q) {} std::pair ESKF::van_loan_discretization( const Eigen::Matrix18d& A_c, @@ -48,7 +47,7 @@ Eigen::Matrix3x19d ESKF::calculate_Hx(const state_quat& nom_state) { Eigen::Quaterniond q = nom_state.quat.normalized(); Eigen::Matrix3d R_bn = q.toRotationMatrix(); - + Eigen::Vector3d v_n = nom_state.vel; Hx.block<3, 3>(0, 3) = R_bn.transpose(); @@ -59,29 +58,25 @@ Eigen::Matrix3x19d ESKF::calculate_Hx(const state_quat& nom_state) { double qy = q.y(); double qz = q.z(); - dR_dq.col(0) = 2 * Eigen::Vector3d( - qw * v_n.x() + qz * v_n.y() - qy * v_n.z(), - -qz * v_n.x() + qw * v_n.y() + qx * v_n.z(), - qy * v_n.x() - qx * v_n.y() + qw * v_n.z() - ); - - dR_dq.col(1) = 2 * Eigen::Vector3d( - qx * v_n.x() + qy * v_n.y() + qz * v_n.z(), - qy * v_n.x() - qx * v_n.y() - qw * v_n.z(), - qz * v_n.x() + qw * v_n.y() - qx * v_n.z() - ); - - dR_dq.col(2) = 2 * Eigen::Vector3d( - -qy * v_n.x() + qx * v_n.y() + qw * v_n.z(), - qx * v_n.x() + qy * v_n.y() + qz * v_n.z(), - -qw * v_n.x() + qz * v_n.y() - qy * v_n.z() - ); - - dR_dq.col(3) = 2 * Eigen::Vector3d( - -qz * v_n.x() - qw * v_n.y() + qx * v_n.z(), - qw * v_n.x() - qz * v_n.y() + qy * v_n.z(), - qx * v_n.x() + qy * v_n.y() + qz * v_n.z() - ); + dR_dq.col(0) = + 2 * Eigen::Vector3d(qw * v_n.x() + qz * v_n.y() - qy * v_n.z(), + -qz * v_n.x() + qw * v_n.y() + qx * v_n.z(), + qy * v_n.x() - qx * v_n.y() + qw * v_n.z()); + + dR_dq.col(1) = + 2 * Eigen::Vector3d(qx * v_n.x() + qy * v_n.y() + qz * v_n.z(), + qy * v_n.x() - qx * v_n.y() - qw * v_n.z(), + qz * v_n.x() + qw * v_n.y() - qx * v_n.z()); + + dR_dq.col(2) = + 2 * Eigen::Vector3d(-qy * v_n.x() + qx * v_n.y() + qw * v_n.z(), + qx * v_n.x() + qy * v_n.y() + qz * v_n.z(), + -qw * v_n.x() + qz * v_n.y() - qy * v_n.z()); + + dR_dq.col(3) = + 2 * Eigen::Vector3d(-qz * v_n.x() - qw * v_n.y() + qx * v_n.z(), + qw * v_n.x() - qz * v_n.y() + qy * v_n.z(), + qx * v_n.x() + qy * v_n.y() + qz * v_n.z()); Hx.block<3, 4>(0, 6) = dR_dq; @@ -100,7 +95,8 @@ Eigen::Matrix3x18d ESKF::calculate_H(const state_quat& nom_state) { Eigen::Matrix3x1d ESKF::calculate_h(const state_quat& nom_state) { Eigen::Matrix3x1d h; - Eigen::Matrix3d R_bn = nom_state.quat.normalized().toRotationMatrix().transpose(); + Eigen::Matrix3d R_bn = + nom_state.quat.normalized().toRotationMatrix().transpose(); h = R_bn * nom_state.vel; @@ -110,12 +106,15 @@ Eigen::Matrix3x1d ESKF::calculate_h(const state_quat& nom_state) { state_quat ESKF::nominal_state_discrete(const state_quat& nom_state, const imu_measurement& imu_meas, const double dt) { - Eigen::Vector3d acc = nom_state.get_R() * (imu_meas.accel - nom_state.accel_bias) + nom_state.gravity; + Eigen::Vector3d acc = + nom_state.get_R() * (imu_meas.accel - nom_state.accel_bias) + + nom_state.gravity; Eigen::Vector3d gyro = (imu_meas.gyro - nom_state.gyro_bias) * dt; state_quat next_nom_state; - next_nom_state.pos = nom_state.pos + nom_state.vel * dt + 0.5 * sq(dt) * acc; + next_nom_state.pos = + nom_state.pos + nom_state.vel * dt + 0.5 * sq(dt) * acc; next_nom_state.vel = nom_state.vel + dt * acc; next_nom_state.quat = (nom_state.quat * vector3d_to_quaternion(gyro)); next_nom_state.quat.normalize(); @@ -188,7 +187,8 @@ std::pair ESKF::injection_and_reset( next_nom_state.pos = nom_state.pos + error_state.pos; next_nom_state.vel = nom_state.vel + error_state.vel; - next_nom_state.quat = nom_state.quat * vector3d_to_quaternion(error_state.euler); + next_nom_state.quat = + nom_state.quat * vector3d_to_quaternion(error_state.euler); next_nom_state.quat.normalize(); next_nom_state.gyro_bias = nom_state.gyro_bias + error_state.gyro_bias; next_nom_state.accel_bias = nom_state.accel_bias + error_state.accel_bias; @@ -209,7 +209,8 @@ std::pair ESKF::imu_update( const imu_measurement& imu_meas, const double dt) { state_quat next_nom_state = nominal_state_discrete(nom_state, imu_meas, dt); - state_euler next_error_state = error_state_prediction(error_state, next_nom_state, imu_meas, dt); + state_euler next_error_state = + error_state_prediction(error_state, next_nom_state, imu_meas, dt); return {next_nom_state, next_error_state}; } @@ -218,8 +219,10 @@ std::pair ESKF::dvl_update( const state_quat& nom_state, const state_euler& error_state, const dvl_measurement& dvl_meas) { - state_euler new_error_state = measurement_update(nom_state, error_state, dvl_meas); - auto [updated_nom_state, updated_error_state] = injection_and_reset(nom_state, new_error_state); + state_euler new_error_state = + measurement_update(nom_state, error_state, dvl_meas); + auto [updated_nom_state, updated_error_state] = + injection_and_reset(nom_state, new_error_state); return {updated_nom_state, updated_error_state}; } diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp index 524b48c29..d679d600e 100644 --- a/navigation/eskf/src/eskf_ros.cpp +++ b/navigation/eskf/src/eskf_ros.cpp @@ -1,11 +1,12 @@ #include "eskf/eskf_ros.hpp" +#include #include "eskf/eskf_utils.hpp" #include "eskf/typedefs.hpp" -#include ESKFNode::ESKFNode() : Node("eskf_node") { time_step = std::chrono::milliseconds(1); - odom_pub_timer_ = this->create_wall_timer(time_step, std::bind(&ESKFNode::publish_odom, this)); + odom_pub_timer_ = this->create_wall_timer( + time_step, std::bind(&ESKFNode::publish_odom, this)); set_subscribers_and_publisher(); @@ -16,49 +17,58 @@ ESKFNode::ESKFNode() : Node("eskf_node") { void ESKFNode::set_subscribers_and_publisher() { rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; - auto qos_sensor_data = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); + auto qos_sensor_data = rclcpp::QoS( + rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); this->declare_parameter("imu_topic", "imu/data_raw"); std::string imu_topic = this->get_parameter("imu_topic").as_string(); - imu_sub_ = this->create_subscription(imu_topic, qos_sensor_data, std::bind(&ESKFNode::imu_callback, this, std::placeholders::_1)); + imu_sub_ = this->create_subscription( + imu_topic, qos_sensor_data, + std::bind(&ESKFNode::imu_callback, this, std::placeholders::_1)); this->declare_parameter("dvl_topic", "/orca/twist"); std::string dvl_topic = this->get_parameter("dvl_topic").as_string(); - dvl_sub_ = this->create_subscription(dvl_topic, qos_sensor_data, std::bind(&ESKFNode::dvl_callback, this, std::placeholders::_1)); - + dvl_sub_ = this->create_subscription< + geometry_msgs::msg::TwistWithCovarianceStamped>( + dvl_topic, qos_sensor_data, + std::bind(&ESKFNode::dvl_callback, this, std::placeholders::_1)); this->declare_parameter("odom_topic", "odom"); std::string odom_topic = this->get_parameter("odom_topic").as_string(); - odom_pub_ = this->create_publisher(odom_topic, qos_sensor_data); + odom_pub_ = this->create_publisher( + odom_topic, qos_sensor_data); } void ESKFNode::set_parameters() { - std::vector diag_Q_std; - this->declare_parameter>("diag_Q_std"); // gyroscope bias noise + this->declare_parameter>( + "diag_Q_std"); // gyroscope bias noise diag_Q_std = this->get_parameter("diag_Q_std").as_double_array(); - + Eigen::Matrix12d Q; Q.setZero(); - spdlog::info("Q diagonal: {}",diag_Q_std[0]); - Q.diagonal() << - sq(diag_Q_std[0]), sq(diag_Q_std[1]), sq(diag_Q_std[2]), // acceleration noise - sq(diag_Q_std[3]), sq(diag_Q_std[4]), sq(diag_Q_std[5]), // gyroscope noise - sq(diag_Q_std[6]), sq(diag_Q_std[7]), sq(diag_Q_std[8]), // acceleration bias noise - sq(diag_Q_std[9]), sq(diag_Q_std[10]), sq(diag_Q_std[11]); // gyroscope bias noise + spdlog::info("Q diagonal: {}", diag_Q_std[0]); + Q.diagonal() << sq(diag_Q_std[0]), sq(diag_Q_std[1]), + sq(diag_Q_std[2]), // acceleration noise + sq(diag_Q_std[3]), sq(diag_Q_std[4]), + sq(diag_Q_std[5]), // gyroscope noise + sq(diag_Q_std[6]), sq(diag_Q_std[7]), + sq(diag_Q_std[8]), // acceleration bias noise + sq(diag_Q_std[9]), sq(diag_Q_std[10]), + sq(diag_Q_std[11]); // gyroscope bias noise eskf_params_.Q = Q; eskf_ = std::make_unique(eskf_params_); Eigen::Matrix18d P; P.setZero(); - P.diagonal() << 1.0, 1.0, 1.0, // position - 0.1, 0.1, 0.1, // velocity - 0.1, 0.1, 0.1, // euler angles - 0.001, 0.001, 0.001, // accel bias - 0.001, 0.001, 0.001, // gyro bias - 0.001, 0.001, 0.001; // gravity + P.diagonal() << 1.0, 1.0, 1.0, // position + 0.1, 0.1, 0.1, // velocity + 0.1, 0.1, 0.1, // euler angles + 0.001, 0.001, 0.001, // accel bias + 0.001, 0.001, 0.001, // gyro bias + 0.001, 0.001, 0.001; // gravity error_state_.covariance = P; } @@ -75,21 +85,28 @@ void ESKFNode::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { double dt = (current_time - last_imu_time_).nanoseconds() * 1e-9; last_imu_time_ = current_time; - imu_meas_.accel_uncorrected << msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z; - imu_meas_.gyro_uncorrected << msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z; + imu_meas_.accel_uncorrected << msg->linear_acceleration.x, + msg->linear_acceleration.y, msg->linear_acceleration.z; + imu_meas_.gyro_uncorrected << msg->angular_velocity.x, + msg->angular_velocity.y, msg->angular_velocity.z; imu_meas_.correct(); - std::tie(nom_state_, error_state_) = eskf_->imu_update(nom_state_, error_state_, imu_meas_, dt); + std::tie(nom_state_, error_state_) = + eskf_->imu_update(nom_state_, error_state_, imu_meas_, dt); } void ESKFNode::dvl_callback( const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { - dvl_meas_.vel << msg->twist.twist.linear.x, msg->twist.twist.linear.y, msg->twist.twist.linear.z; - dvl_meas_.cov << msg->twist.covariance[0], msg->twist.covariance[1], msg->twist.covariance[2], - msg->twist.covariance[6], msg->twist.covariance[7], msg->twist.covariance[8], - msg->twist.covariance[12], msg->twist.covariance[13], msg->twist.covariance[14]; - - std::tie(nom_state_, error_state_) = eskf_->dvl_update(nom_state_, error_state_, dvl_meas_); + dvl_meas_.vel << msg->twist.twist.linear.x, msg->twist.twist.linear.y, + msg->twist.twist.linear.z; + dvl_meas_.cov << msg->twist.covariance[0], msg->twist.covariance[1], + msg->twist.covariance[2], msg->twist.covariance[6], + msg->twist.covariance[7], msg->twist.covariance[8], + msg->twist.covariance[12], msg->twist.covariance[13], + msg->twist.covariance[14]; + + std::tie(nom_state_, error_state_) = + eskf_->dvl_update(nom_state_, error_state_, dvl_meas_); } void ESKFNode::publish_odom() { diff --git a/navigation/eskf/src/eskf_utils.cpp b/navigation/eskf/src/eskf_utils.cpp index 7a668adfc..930167eaa 100644 --- a/navigation/eskf/src/eskf_utils.cpp +++ b/navigation/eskf/src/eskf_utils.cpp @@ -28,4 +28,4 @@ Eigen::Quaterniond euler_to_quaternion(const Eigen::Vector3d& euler) { Eigen::AngleAxisd(euler.y(), Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(euler.x(), Eigen::Vector3d::UnitX()); return q; -} \ No newline at end of file +} diff --git a/navigation/eskf_python/eskf_python/eskf_python_class.py b/navigation/eskf_python/eskf_python/eskf_python_class.py index 0ba96ba1f..65d41425d 100644 --- a/navigation/eskf_python/eskf_python/eskf_python_class.py +++ b/navigation/eskf_python/eskf_python/eskf_python_class.py @@ -1,7 +1,9 @@ from dataclasses import dataclass, field + import numpy as np from eskf_python_utils import quaternion_error + @dataclass class StateQuat: position: np.ndarray = field( @@ -19,10 +21,8 @@ class StateQuat: gyro_bias: np.ndarray = field( default_factory=lambda: np.zeros(3) ) # Gyro bias vector (b_gx, b_gy, b_gz) - g: np.ndarray = field( - default_factory=lambda: np.array([0, 0, 0]) - ) # Gravity vector - + g: np.ndarray = field(default_factory=lambda: np.array([0, 0, 0])) # Gravity vector + def as_vector(self) -> np.ndarray: """Returns the state vector as a numpy array. @@ -81,7 +81,7 @@ def R_q(self) -> np.ndarray: ) return R - + def __sub__(self, other: 'StateQuat') -> 'StateQuat': """Subtracts the values of two state vectors. @@ -102,7 +102,6 @@ def __sub__(self, other: 'StateQuat') -> 'StateQuat': return result - @dataclass class StateEuler: position: np.ndarray = field( @@ -172,12 +171,8 @@ def copy_state(self, wanted_state: 'StateEuler') -> None: @dataclass class MeasurementModel: - measurement: np.ndarray = field( - default_factory=lambda: np.zeros(6) - ) - measurement_covariance: np.ndarray = field( - default_factory=lambda: np.zeros((6, 6)) - ) + measurement: np.ndarray = field(default_factory=lambda: np.zeros(6)) + measurement_covariance: np.ndarray = field(default_factory=lambda: np.zeros((6, 6))) def H(self) -> np.ndarray: """Calculates the measurement matrix. @@ -190,19 +185,12 @@ def H(self) -> np.ndarray: H[0:3, 3:6] = np.eye(3) return H - + + @dataclass class Measurement: - acceleration: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) - angular_velocity: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) - aiding: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) + acceleration: np.ndarray = field(default_factory=lambda: np.zeros(3)) + angular_velocity: np.ndarray = field(default_factory=lambda: np.zeros(3)) + aiding: np.ndarray = field(default_factory=lambda: np.zeros(3)) - aiding_covariance: np.ndarray = field( - default_factory=lambda: np.zeros((3, 3)) - ) \ No newline at end of file + aiding_covariance: np.ndarray = field(default_factory=lambda: np.zeros((3, 3))) diff --git a/navigation/eskf_python/eskf_python/eskf_python_filter.py b/navigation/eskf_python/eskf_python/eskf_python_filter.py index 890226639..fac3c8526 100644 --- a/navigation/eskf_python/eskf_python/eskf_python_filter.py +++ b/navigation/eskf_python/eskf_python/eskf_python_filter.py @@ -4,7 +4,6 @@ import numpy as np from eskf_python_class import Measurement, StateEuler, StateQuat from eskf_python_utils import ( - R_from_angle_axis, angle_axis_to_quaternion, euler_to_quat, quaternion_product, @@ -44,58 +43,64 @@ def Q_delta_theta(self) -> np.ndarray: return Q_delta_theta def Hx(self) -> np.ndarray: - """ - Computes the true-state measurement Jacobian for the measurement + """Computes the true-state measurement Jacobian for the measurement h(x) = R(q) * velocity, where: - R(q) is the rotation matrix from the quaternion (q, q, q, q) with q as the scalar part, - velocity is a 3-vector. - + The state is assumed to be ordered as: [position (3), velocity (3), quaternion (4), ...] (total length 19). - + The Jacobian Hx is a 3x19 matrix with nonzero blocks: - Columns 3:6 (velocity): R(q) - - Columns 6:10 (quaternion): + - Columns 6:10 (quaternion): """ - q = self.nom_state.orientation # shape (4,) - v = self.nom_state.velocity # shape (3,) + v = self.nom_state.velocity # shape (3,) q0, q1, q2, q3 = q v1, v2, v3 = v R = self.nom_state.R_q().transpose() # shape (3, 3) - - dhdq0 = 2 * np.array([ - q0 * v1 - q3 * v2 + q2 * v3, - q3 * v1 + q0 * v2 - q1 * v3, - -q2 * v1 + q1 * v2 + q0 * v3 - ]) - - dhdq1 = 2 * np.array([ - q1 * v1 + q2 * v2 + q3 * v3, - q2 * v1 - q1 * v2 - q0 * v3, - q3 * v1 + q0 * v2 - q1 * v3 - ]) - - dhdq2 = 2 * np.array([ - -q2 * v1 + q1 * v2 + q0 * v3, - q1 * v1 + q2 * v2 + q3 * v3, - -q0 * v1 + q3 * v2 - q2 * v3 - ]) - - dhdq3 = 2 * np.array([ - -q3 * v1 - q0 * v2 + q1 * v3, - q0 * v1 - q3 * v2 + q2 * v3, - q1 * v1 + q2 * v2 + q3 * v3 - ]) - + + dhdq0 = 2 * np.array( + [ + q0 * v1 - q3 * v2 + q2 * v3, + q3 * v1 + q0 * v2 - q1 * v3, + -q2 * v1 + q1 * v2 + q0 * v3, + ] + ) + + dhdq1 = 2 * np.array( + [ + q1 * v1 + q2 * v2 + q3 * v3, + q2 * v1 - q1 * v2 - q0 * v3, + q3 * v1 + q0 * v2 - q1 * v3, + ] + ) + + dhdq2 = 2 * np.array( + [ + -q2 * v1 + q1 * v2 + q0 * v3, + q1 * v1 + q2 * v2 + q3 * v3, + -q0 * v1 + q3 * v2 - q2 * v3, + ] + ) + + dhdq3 = 2 * np.array( + [ + -q3 * v1 - q0 * v2 + q1 * v3, + q0 * v1 - q3 * v2 + q2 * v3, + q1 * v1 + q2 * v2 + q3 * v3, + ] + ) + dHdq = np.column_stack((dhdq0, dhdq1, dhdq2, dhdq3)) # shape (3, 4) - + Hx = np.zeros((3, 19)) Hx[:, 3:6] = R Hx[:, 6:10] = dHdq - + return Hx def H(self) -> np.ndarray: @@ -262,14 +267,12 @@ def reset_error_state(self) -> None: self.error_state.fill_states(np.zeros(18)) def imu_update(self, imu_data: Measurement) -> None: - """Updates the state using the IMU data. - """ + """Updates the state using the IMU data.""" self.nominal_state_discrete(imu_data) self.error_state_prediction(imu_data) def dvl_update(self, dvl_measurement: Measurement) -> float: - """Updates the state using the DVL measurement. - """ + """Updates the state using the DVL measurement.""" NIS = self.measurement_update(dvl_measurement) self.injection() self.reset_error_state() @@ -278,15 +281,13 @@ def dvl_update(self, dvl_measurement: Measurement) -> float: # functions for tuning the filter def NIS(self, S: np.ndarray, innovation: np.ndarray) -> float: - """Calculates the Normalized Innovation Squared (NIS) value. - """ + """Calculates the Normalized Innovation Squared (NIS) value.""" return innovation.T @ np.linalg.inv(S) @ innovation def NEEDS( self, P: np.ndarray, true_state: StateQuat, estimate_state: StateQuat ) -> float: - """Calculates the Normalized Estimation Error Squared (NEEDS) value. - """ + """Calculates the Normalized Estimation Error Squared (NEEDS) value.""" return ( (true_state - estimate_state).as_vector().T @ np.linalg.inv(P) diff --git a/navigation/eskf_python/eskf_python/eskf_python_node.py b/navigation/eskf_python/eskf_python/eskf_python_node.py index ec206ab64..7b300ecc6 100644 --- a/navigation/eskf_python/eskf_python/eskf_python_node.py +++ b/navigation/eskf_python/eskf_python/eskf_python_node.py @@ -1,12 +1,12 @@ #!/usr/bin/env python3 +import numpy as np import rclpy +from geometry_msgs.msg import TwistWithCovarianceStamped from nav_msgs.msg import Odometry from rclpy.node import Node from rclpy.qos import QoSProfile, qos_profile_sensor_data from sensor_msgs.msg import Imu -import numpy as np -from geometry_msgs.msg import TwistWithCovarianceStamped # NEED TO CHANGE THIS TO THE CORRECT PATH from eskf_python.eskf_python_filter import ( @@ -33,7 +33,10 @@ def __init__(self): ) self.twist_dvl_subscriber_ = self.create_subscription( - TwistWithCovarianceStamped, '/dvl/twist', self.filter_callback, qos_profile=qos_profile + TwistWithCovarianceStamped, + '/dvl/twist', + self.filter_callback, + qos_profile=qos_profile, ) # This publisher will publish the estimtaed state of the vehicle @@ -50,14 +53,22 @@ def __init__(self): self.get_logger().info("Error State Kalman Filter started") def imu_callback(self, msg: Imu): - # Get the IMU data imu_acceleartion = msg.linear_acceleration imu_angular_velocity = msg.angular_velocity # Combine the IMU data - imu_data = np.array([imu_acceleartion.x, imu_acceleartion.y, imu_acceleartion.z, imu_angular_velocity.x, imu_angular_velocity.y, imu_angular_velocity.z]) + imu_data = np.array( + [ + imu_acceleartion.x, + imu_acceleartion.y, + imu_acceleartion.z, + imu_angular_velocity.x, + imu_angular_velocity.y, + imu_angular_velocity.z, + ] + ) # Update the filter with the IMU data self.current_state_nom, self.current_state_error = ( @@ -84,8 +95,6 @@ def imu_callback(self, msg: Imu): # Publish self.state_publisher_.publish(self.odom_msg) - - def filter_callback(self, msg: TwistWithCovarianceStamped): """Callback function for the filter measurement update, this will be called when the filter needs to be updated with the DVL data. @@ -93,7 +102,13 @@ def filter_callback(self, msg: TwistWithCovarianceStamped): self.get_logger().info("Filter callback, got DVL data") # Get the DVL data (linear velocity) - dvl_data = np.array([msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z]) + dvl_data = np.array( + [ + msg.twist.twist.linear.x, + msg.twist.twist.linear.y, + msg.twist.twist.linear.z, + ] + ) # Update the filter with the DVL data self.current_state_nom, self.current_state_error = ( @@ -124,7 +139,6 @@ def filter_callback(self, msg: TwistWithCovarianceStamped): self.state_publisher_.publish(self.odom_msg) - def main(args=None): rclpy.init(args=args) node = ESKalmanFilterNode() diff --git a/navigation/eskf_python/eskf_python/eskf_python_utils.py b/navigation/eskf_python/eskf_python/eskf_python_utils.py index aaef5f8d6..bbe13d759 100644 --- a/navigation/eskf_python/eskf_python/eskf_python_utils.py +++ b/navigation/eskf_python/eskf_python/eskf_python_utils.py @@ -1,61 +1,61 @@ import numpy as np + def skew_matrix(vector: np.ndarray) -> np.ndarray: - """ - Returns the skew symmetric matrix of a 3x1 vector. + """Returns the skew symmetric matrix of a 3x1 vector. """ return np.array( [ [0, -vector[2], vector[1]], [vector[2], 0, -vector[0]], - [-vector[1], vector[0], 0] + [-vector[1], vector[0], 0], ] ) + def quat_norm(quat: np.ndarray) -> np.ndarray: - """ - Function that normalizes a quaternion + """Function that normalizes a quaternion """ quat = quat / np.linalg.norm(quat) return quat + def quaternion_product(q1: np.ndarray, q2: np.ndarray) -> np.ndarray: - """Calculates the quaternion super product of two quaternions. + """Calculates the quaternion super product of two quaternions. - Args: - q1 (np.ndarray): The first quaternion. - q2 (np.ndarray): The second quaternion. + Args: + q1 (np.ndarray): The first quaternion. + q2 (np.ndarray): The second quaternion. - Returns: - np.ndarray: The quaternion super product. - """ + Returns: + np.ndarray: The quaternion super product. + """ + eta_0, e_0_x, e_0_y, e_0_z = q1 + eta_1, e_1_x, e_1_y, e_1_z = q2 - eta_0, e_0_x, e_0_y, e_0_z = q1 - eta_1, e_1_x, e_1_y, e_1_z = q2 + e_0 = np.array([e_0_x, e_0_y, e_0_z]) + e_1 = np.array([e_1_x, e_1_y, e_1_z]) - e_0 = np.array([e_0_x, e_0_y, e_0_z]) - e_1 = np.array([e_1_x, e_1_y, e_1_z]) + eta_new = eta_0 * eta_1 - np.dot(e_0, e_1) + nu_new = e_1 * eta_0 + e_0 * eta_1 + np.cross(e_0, e_1) - eta_new = eta_0 * eta_1 - np.dot(e_0, e_1) - nu_new = e_1 * eta_0 + e_0 * eta_1 + np.cross(e_0, e_1) + q_new = np.array([eta_new, nu_new[0], nu_new[1], nu_new[2]]) + q_new = q_new / np.linalg.norm(q_new) - q_new = np.array([eta_new, nu_new[0], nu_new[1], nu_new[2]]) - q_new = q_new / np.linalg.norm(q_new) + return q_new - return q_new def quaternion_error(quat_1: np.ndarray, quat_2: np.ndarray) -> np.ndarray: + """Calculates the error between two quaternions """ - Calculates the error between two quaternions - """ - quat_2_inv = np.array([quat_2[0], -quat_2[1], -quat_2[2], -quat_2[3]]) error_quat = quaternion_product(quat_1, quat_2_inv) return error_quat + def angle_axis_to_quaternion(vector: np.ndarray) -> np.ndarray: """Converts an angle-axis representation to a quaternion. @@ -71,13 +71,12 @@ def angle_axis_to_quaternion(vector: np.ndarray) -> np.ndarray: else: axis = vector / angle - q = np.zeros(4) q[0] = np.cos(angle / 2) q[1:] = np.sin(angle / 2) * axis return q - + def R_from_angle_axis(vector: np.ndarray) -> np.ndarray: """Calculates the rotation matrix from the angle-axis representation. @@ -109,40 +108,41 @@ def R_from_angle_axis(vector: np.ndarray) -> np.ndarray: 1 - 2 * q1**2 - 2 * q2**2, ], ] - ) + ) return R + def euler_to_quat(euler_angles: np.ndarray) -> np.ndarray: - """ - Converts Euler angles to a quaternion + """Converts Euler angles to a quaternion """ psi, theta, phi = euler_angles c_psi = np.cos(psi / 2) - s_psi = np.sin(psi / 2) + s_psi = np.sin(psi / 2) c_theta = np.cos(theta / 2) s_theta = np.sin(theta / 2) c_phi = np.cos(phi / 2) s_phi = np.sin(phi / 2) - quat = np.array([ - c_psi * c_theta * c_phi + s_psi * s_theta * s_phi, - c_psi * c_theta * s_phi - s_psi * s_theta * c_phi, - s_psi * c_theta * s_phi + c_psi * s_theta * c_phi, - s_psi * c_theta * c_phi - c_psi * s_theta * s_phi - ]) + quat = np.array( + [ + c_psi * c_theta * c_phi + s_psi * s_theta * s_phi, + c_psi * c_theta * s_phi - s_psi * s_theta * c_phi, + s_psi * c_theta * s_phi + c_psi * s_theta * c_phi, + s_psi * c_theta * c_phi - c_psi * s_theta * s_phi, + ] + ) return quat + def quat_to_euler(quat: np.ndarray) -> np.ndarray: - """ - Converts a quaternion to Euler angles + """Converts a quaternion to Euler angles """ nu, eta_1, eta_2, eta_3 = quat - phi = np.arctan2(2*(eta_2 * eta_3 + nu * eta_1), 1 - 2 * (eta_1 ** 2 + eta_2 ** 2)) + phi = np.arctan2(2 * (eta_2 * eta_3 + nu * eta_1), 1 - 2 * (eta_1**2 + eta_2**2)) theta = -np.arcsin(2 * (eta_1 * eta_3 - nu * eta_2)) - psi = np.arctan2(2 * (nu * eta_3 + eta_1 * eta_2), 1 - 2 * (eta_2 ** 2 + eta_3 ** 2)) + psi = np.arctan2(2 * (nu * eta_3 + eta_1 * eta_2), 1 - 2 * (eta_2**2 + eta_3**2)) return np.array([phi, theta, psi]) - diff --git a/navigation/eskf_python/eskf_python/eskf_test.py b/navigation/eskf_python/eskf_python/eskf_test.py index 0930d9f6c..2c6d94c09 100644 --- a/navigation/eskf_python/eskf_python/eskf_test.py +++ b/navigation/eskf_python/eskf_python/eskf_test.py @@ -4,7 +4,6 @@ from eskf_python_filter import ESKF from eskf_python_utils import quat_to_euler from eskf_test_utils import StateQuatModel, process_model -from scipy.stats import chi2 def simulate_eskf(): @@ -356,4 +355,4 @@ def simulate_eskf(): plt.tight_layout() plt.show() -""" \ No newline at end of file +""" diff --git a/navigation/eskf_python/eskf_python/eskf_test_utils.py b/navigation/eskf_python/eskf_python/eskf_test_utils.py index 34abe8eda..a60e62ff0 100644 --- a/navigation/eskf_python/eskf_python/eskf_test_utils.py +++ b/navigation/eskf_python/eskf_python/eskf_test_utils.py @@ -1,15 +1,23 @@ -import numpy as np from dataclasses import dataclass, field -from typing import Tuple -from eskf_python_utils import quaternion_product, euler_to_quat, quat_to_euler, quaternion_error, quat_norm, skew_matrix + +import numpy as np +from eskf_python_utils import ( + euler_to_quat, + quat_norm, + quat_to_euler, + quaternion_error, + quaternion_product, + skew_matrix, +) # This was the original code from the ukf_okid.py file + @dataclass class StateQuatModel: + """A class to represent the state to be estimated by the UKF. """ - A class to represent the state to be estimated by the UKF. - """ + position: np.ndarray = field(default_factory=lambda: np.zeros(3)) orientation: np.ndarray = field(default_factory=lambda: np.array([1, 0, 0, 0])) velocity: np.ndarray = field(default_factory=lambda: np.zeros(3)) @@ -18,7 +26,9 @@ class StateQuatModel: def as_vector(self) -> np.ndarray: """Returns the StateVector as a numpy array.""" - return np.concatenate([self.position, self.orientation, self.velocity, self.angular_velocity]) + return np.concatenate( + [self.position, self.orientation, self.velocity, self.angular_velocity] + ) def nu(self) -> np.ndarray: """Calculates the nu vector.""" @@ -27,11 +37,25 @@ def nu(self) -> np.ndarray: def R_q(self) -> np.ndarray: """Calculates the rotation matrix from the orientation quaternion.""" q0, q1, q2, q3 = self.orientation - R = np.array([ - [1 - 2 * q2**2 - 2 * q3**2, 2 * (q1 * q2 - q0 * q3), 2 * (q0 * q2 + q1 * q3)], - [2 * (q1 * q2 + q0 * q3), 1 - 2 * q1**2 - 2 * q3**2, 2 * (q2 * q3 - q0 * q1)], - [2 * (q1 * q3 - q0 * q2), 2 * (q0 * q1 + q2 * q3), 1 - 2 * q1**2 - 2 * q2**2] - ]) + R = np.array( + [ + [ + 1 - 2 * q2**2 - 2 * q3**2, + 2 * (q1 * q2 - q0 * q3), + 2 * (q0 * q2 + q1 * q3), + ], + [ + 2 * (q1 * q2 + q0 * q3), + 1 - 2 * q1**2 - 2 * q3**2, + 2 * (q2 * q3 - q0 * q1), + ], + [ + 2 * (q1 * q3 - q0 * q2), + 2 * (q0 * q1 + q2 * q3), + 1 - 2 * q1**2 - 2 * q2**2, + ], + ] + ) return R def fill_states(self, state: np.ndarray) -> None: @@ -41,10 +65,14 @@ def fill_states(self, state: np.ndarray) -> None: self.velocity = state[7:10] self.angular_velocity = state[10:13] - def fill_states_different_dim(self, state: np.ndarray, state_euler: np.ndarray) -> None: + def fill_states_different_dim( + self, state: np.ndarray, state_euler: np.ndarray + ) -> None: """Fills states when the state vector has different dimensions than the default state vector.""" self.position = state[0:3] + state_euler[0:3] - self.orientation = quaternion_product(state[3:7], euler_to_quat(state_euler[3:6])) + self.orientation = quaternion_product( + state[3:7], euler_to_quat(state_euler[3:6]) + ) self.velocity = state[7:10] + state_euler[6:9] self.angular_velocity = state[10:13] + state_euler[9:12] @@ -52,7 +80,9 @@ def subtract(self, other: 'StateQuatModel') -> np.ndarray: """Subtracts two StateQuatModel objects, returning the difference with Euler angles.""" new_array = np.zeros(len(self.as_vector()) - 1) new_array[:3] = self.position - other.position - new_array[3:6] = quat_to_euler(quaternion_error(self.orientation, other.orientation)) + new_array[3:6] = quat_to_euler( + quaternion_error(self.orientation, other.orientation) + ) new_array[6:9] = self.velocity - other.velocity new_array[9:12] = self.angular_velocity - other.angular_velocity @@ -92,7 +122,9 @@ def insert_weights(self, weights: np.ndarray) -> np.ndarray: """Inserts the weights into the covariance matrix.""" new_state = StateQuatModel() new_state.position = self.position - weights[:3] - new_state.orientation = quaternion_error(self.orientation, euler_to_quat(weights[3:6])) + new_state.orientation = quaternion_error( + self.orientation, euler_to_quat(weights[3:6]) + ) new_state.velocity = self.velocity - weights[6:9] new_state.angular_velocity = self.angular_velocity - weights[9:12] @@ -107,9 +139,9 @@ def add_without_quaternions(self, other: 'StateQuatModel') -> None: @dataclass class process_model: + """A class defined for a general process model. """ - A class defined for a general process model. - """ + state_vector: StateQuatModel = field(default_factory=StateQuatModel) state_vector_dot: StateQuatModel = field(default_factory=StateQuatModel) state_vector_prev: StateQuatModel = field(default_factory=StateQuatModel) @@ -119,7 +151,7 @@ class process_model: damping_linear: np.ndarray = field(default_factory=lambda: np.zeros(6)) damping_nonlinear: np.ndarray = field(default_factory=lambda: np.zeros(6)) m: float = 0.0 - inertia: np.ndarray = field(default_factory=lambda: np.zeros((3,3))) + inertia: np.ndarray = field(default_factory=lambda: np.zeros((3, 3))) r_b_bg: np.ndarray = field(default_factory=lambda: np.zeros(3)) dt: float = 0.0 integral_error_position: np.ndarray = field(default_factory=lambda: np.zeros(3)) @@ -130,22 +162,33 @@ class process_model: def R(self) -> np.ndarray: """Calculates the rotation matrix.""" nu, e_1, e_2, e_3 = self.state_vector.orientation - R = np.array([ - [1 - 2 * e_2 ** 2 - 2 * e_3 ** 2, 2 * e_1 * e_2 - 2 * nu * e_3, 2 * e_1 * e_3 + 2 * nu * e_2], - [2 * e_1 * e_2 + 2 * nu * e_3, 1 - 2 * e_1 ** 2 - 2 * e_3 ** 2, 2 * e_2 * e_3 - 2 * nu * e_1], - [2 * e_1 * e_3 - 2 * nu * e_2, 2 * e_2 * e_3 + 2 * nu * e_1, 1 - 2 * e_1 ** 2 - 2 * e_2 ** 2] - ]) + R = np.array( + [ + [ + 1 - 2 * e_2**2 - 2 * e_3**2, + 2 * e_1 * e_2 - 2 * nu * e_3, + 2 * e_1 * e_3 + 2 * nu * e_2, + ], + [ + 2 * e_1 * e_2 + 2 * nu * e_3, + 1 - 2 * e_1**2 - 2 * e_3**2, + 2 * e_2 * e_3 - 2 * nu * e_1, + ], + [ + 2 * e_1 * e_3 - 2 * nu * e_2, + 2 * e_2 * e_3 + 2 * nu * e_1, + 1 - 2 * e_1**2 - 2 * e_2**2, + ], + ] + ) return R def T(self) -> np.ndarray: """Calculates the transformation matrix.""" nu, e_1, e_2, e_3 = self.state_vector.orientation - T = 0.5 * np.array([ - [-e_1, -e_2, -e_3], - [nu, -e_3, e_2], - [e_3, nu, -e_1], - [-e_2, e_1, nu] - ]) + T = 0.5 * np.array( + [[-e_1, -e_2, -e_3], [nu, -e_3, e_2], [e_3, nu, -e_1], [-e_2, e_1, nu]] + ) return T def Crb(self) -> np.ndarray: @@ -170,15 +213,31 @@ def model_prediction(self, state: StateQuatModel) -> None: """Calculates the model of the system.""" self.state_vector = state self.state_vector_dot.position = np.dot(self.R(), self.state_vector.velocity) - self.state_vector_dot.orientation = np.dot(self.T(), self.state_vector.angular_velocity) - Nu = np.linalg.inv(self.mass_interia_matrix + np.diag(self.added_mass)) @ (self.Control_input - np.dot(self.Crb(), self.state_vector.nu()) - np.dot(self.D(), self.state_vector.nu())) + self.state_vector_dot.orientation = np.dot( + self.T(), self.state_vector.angular_velocity + ) + Nu = np.linalg.inv(self.mass_interia_matrix + np.diag(self.added_mass)) @ ( + self.Control_input + - np.dot(self.Crb(), self.state_vector.nu()) + - np.dot(self.D(), self.state_vector.nu()) + ) self.state_vector_dot.velocity = Nu[:3] self.state_vector_dot.angular_velocity = Nu[3:] def euler_forward(self) -> StateQuatModel: """Calculates the forward Euler integration.""" - self.state_vector.position = self.state_vector_prev.position + self.state_vector_dot.position * self.dt - self.state_vector.orientation = quat_norm(self.state_vector_prev.orientation + self.state_vector_dot.orientation * self.dt) - self.state_vector.velocity = self.state_vector_prev.velocity + self.state_vector_dot.velocity * self.dt - self.state_vector.angular_velocity = self.state_vector_prev.angular_velocity + self.state_vector_dot.angular_velocity * self.dt - return self.state_vector \ No newline at end of file + self.state_vector.position = ( + self.state_vector_prev.position + self.state_vector_dot.position * self.dt + ) + self.state_vector.orientation = quat_norm( + self.state_vector_prev.orientation + + self.state_vector_dot.orientation * self.dt + ) + self.state_vector.velocity = ( + self.state_vector_prev.velocity + self.state_vector_dot.velocity * self.dt + ) + self.state_vector.angular_velocity = ( + self.state_vector_prev.angular_velocity + + self.state_vector_dot.angular_velocity * self.dt + ) + return self.state_vector diff --git a/navigation/ukf_okid/ukf_python/rest.py b/navigation/ukf_okid/ukf_python/rest.py index df7392bd4..52cffb2df 100644 --- a/navigation/ukf_okid/ukf_python/rest.py +++ b/navigation/ukf_okid/ukf_python/rest.py @@ -1,28 +1,33 @@ def mean_set(set_points: list[StateQuat], weights: np.ndarray = None) -> np.ndarray: - """ - Function that calculates the mean of a set of points + """Function that calculates the mean of a set of points """ n = len(set_points[0].as_vector()) - 1 mean_value = StateQuat() if weights is None: for i in range(2 * n + 1): - weight_temp_list = (1/ (2 * n + 1)) * np.ones(2 * n + 1) + weight_temp_list = (1 / (2 * n + 1)) * np.ones(2 * n + 1) mean_value.add_without_quaternions(weight_temp_list[i] * set_points[i]) - - mean_value.orientation = iterative_quaternion_mean_statequat(set_points, weight_temp_list) - + + mean_value.orientation = iterative_quaternion_mean_statequat( + set_points, weight_temp_list + ) + else: for i in range(2 * n + 1): mean_value.add_without_quaternions(weights[i] * set_points[i]) - mean_value.orientation = iterative_quaternion_mean_statequat(set_points, weights) - + mean_value.orientation = iterative_quaternion_mean_statequat( + set_points, weights + ) + return mean_value.as_vector() -def mean_measurement(set_points: list[MeasModel], weights: np.ndarray = None) -> np.ndarray: - """ - Function that calculates the mean of a set of points + +def mean_measurement( + set_points: list[MeasModel], weights: np.ndarray = None +) -> np.ndarray: + """Function that calculates the mean of a set of points """ n = len(set_points) mean_value = MeasModel() @@ -33,5 +38,5 @@ def mean_measurement(set_points: list[MeasModel], weights: np.ndarray = None) -> else: for i in range(n): mean_value = mean_value + (weights[i] * set_points[i]) - - return mean_value.measurement \ No newline at end of file + + return mean_value.measurement diff --git a/navigation/ukf_okid/ukf_python/ukf_okid.py b/navigation/ukf_okid/ukf_python/ukf_okid.py index 16312935a..80a7c939c 100644 --- a/navigation/ukf_okid/ukf_python/ukf_okid.py +++ b/navigation/ukf_okid/ukf_python/ukf_okid.py @@ -1,7 +1,6 @@ -from ukf_okid_class import * + import numpy as np -import time -import matplotlib.pyplot as plt +from ukf_okid_class import * class UKF: @@ -17,8 +16,7 @@ def __init__(self, process_model: process_model, x_0, P_0, Q, R): self.T = self.generate_T_matrix(len(P_0)) def generate_T_matrix(self, n: float) -> np.ndarray: - """ - Generates the orthonormal transformation matrix T used in the TUKF sigma point generation. + """Generates the orthonormal transformation matrix T used in the TUKF sigma point generation. Parameters: n (int): The state dimension. @@ -29,7 +27,7 @@ def generate_T_matrix(self, n: float) -> np.ndarray: T = np.zeros((n, n)) for i in range(n): - for j in range(n//2): + for j in range(n // 2): T[2 * j - 2, i - 1] = np.sqrt(2) * np.cos(((2 * j - 1) * i * np.pi) / n) T[2 * j - 1, i - 1] = np.sqrt(2) * np.sin(((2 * j - 1) * i * np.pi) / n) @@ -41,8 +39,7 @@ def generate_T_matrix(self, n: float) -> np.ndarray: return T def sigma_points(self, current_state: StateQuat) -> list[StateQuat]: - """ - Functions that generate the sigma points for the UKF + """Functions that generate the sigma points for the UKF """ n = len(current_state.covariance) @@ -60,18 +57,15 @@ def sigma_points(self, current_state: StateQuat) -> list[StateQuat]: return self.sigma_points_list - def unscented_transform(self, current_state: StateQuat) -> StateQuat: + """The unscented transform function generates the priori state estimate """ - The unscented transform function generates the priori state estimate - """ - - _ = self.sigma_points(current_state) + _ = self.sigma_points(current_state) n = len(current_state.covariance) self.y_i = [StateQuat() for _ in range(2 * n)] - for i in range(2 * n ): + for i in range(2 * n): self.process_model.model_prediction(self.sigma_points_list[i]) self.y_i[i] = self.process_model.euler_forward() @@ -82,12 +76,12 @@ def unscented_transform(self, current_state: StateQuat) -> StateQuat: state_estimate.covariance = covariance_set(self.y_i, x) return state_estimate - def measurement_update(self, current_state: StateQuat, measurement: MeasModel) -> tuple[MeasModel, np.ndarray]: - """ - Function that updates the state estimate with a measurement + def measurement_update( + self, current_state: StateQuat, measurement: MeasModel + ) -> tuple[MeasModel, np.ndarray]: + """Function that updates the state estimate with a measurement Hopefully this is the DVL or GNSS """ - n = len(current_state.covariance) z_i = [MeasModel() for _ in range(2 * n)] @@ -100,15 +94,21 @@ def measurement_update(self, current_state: StateQuat, measurement: MeasModel) - meas_update.covariance = covariance_measurement(z_i, meas_update.measurement) - cross_correlation = cross_covariance(self.y_i, current_state.as_vector(), z_i, meas_update.measurement) + cross_correlation = cross_covariance( + self.y_i, current_state.as_vector(), z_i, meas_update.measurement + ) return meas_update, cross_correlation - def posteriori_estimate(self, current_state: StateQuat, cross_correlation: np.ndarray, measurement: MeasModel, ex_measuremnt: MeasModel) -> StateQuat: - """ - Calculates the posteriori estimate using measurement and the prior estimate + def posteriori_estimate( + self, + current_state: StateQuat, + cross_correlation: np.ndarray, + measurement: MeasModel, + ex_measuremnt: MeasModel, + ) -> StateQuat: + """Calculates the posteriori estimate using measurement and the prior estimate """ - nu_k = MeasModel() nu_k.measurement = measurement.measurement - ex_measuremnt.measurement @@ -118,8 +118,12 @@ def posteriori_estimate(self, current_state: StateQuat, cross_correlation: np.nd posteriori_estimate = StateQuat() - posteriori_estimate.fill_states_different_dim(current_state.as_vector(), np.dot(K_k, nu_k.measurement)) - posteriori_estimate.covariance = current_state.covariance - np.dot(K_k, np.dot(nu_k.covariance, np.transpose(K_k))) + posteriori_estimate.fill_states_different_dim( + current_state.as_vector(), np.dot(K_k, nu_k.measurement) + ) + posteriori_estimate.covariance = current_state.covariance - np.dot( + K_k, np.dot(nu_k.covariance, np.transpose(K_k)) + ) self.process_model.state_vector_prev = posteriori_estimate diff --git a/navigation/ukf_okid/ukf_python/ukf_okid_class.py b/navigation/ukf_okid/ukf_python/ukf_okid_class.py index 4d3281260..181d1f4af 100644 --- a/navigation/ukf_okid/ukf_python/ukf_okid_class.py +++ b/navigation/ukf_okid/ukf_python/ukf_okid_class.py @@ -1,15 +1,13 @@ from dataclasses import dataclass, field -import numpy as np - -from dataclasses import dataclass, field import numpy as np + @dataclass class StateQuat: + """A class to represent the state to be estimated by the UKF. """ - A class to represent the state to be estimated by the UKF. - """ + position: np.ndarray = field(default_factory=lambda: np.zeros(3)) orientation: np.ndarray = field(default_factory=lambda: np.array([1, 0, 0, 0])) velocity: np.ndarray = field(default_factory=lambda: np.zeros(3)) @@ -18,7 +16,9 @@ class StateQuat: def as_vector(self) -> np.ndarray: """Returns the StateVector as a numpy array.""" - return np.concatenate([self.position, self.orientation, self.velocity, self.angular_velocity]) + return np.concatenate( + [self.position, self.orientation, self.velocity, self.angular_velocity] + ) def nu(self) -> np.ndarray: """Calculates the nu vector.""" @@ -27,11 +27,25 @@ def nu(self) -> np.ndarray: def R_q(self) -> np.ndarray: """Calculates the rotation matrix from the orientation quaternion.""" q0, q1, q2, q3 = self.orientation - R = np.array([ - [1 - 2 * q2**2 - 2 * q3**2, 2 * (q1 * q2 - q0 * q3), 2 * (q0 * q2 + q1 * q3)], - [2 * (q1 * q2 + q0 * q3), 1 - 2 * q1**2 - 2 * q3**2, 2 * (q2 * q3 - q0 * q1)], - [2 * (q1 * q3 - q0 * q2), 2 * (q0 * q1 + q2 * q3), 1 - 2 * q1**2 - 2 * q2**2] - ]) + R = np.array( + [ + [ + 1 - 2 * q2**2 - 2 * q3**2, + 2 * (q1 * q2 - q0 * q3), + 2 * (q0 * q2 + q1 * q3), + ], + [ + 2 * (q1 * q2 + q0 * q3), + 1 - 2 * q1**2 - 2 * q3**2, + 2 * (q2 * q3 - q0 * q1), + ], + [ + 2 * (q1 * q3 - q0 * q2), + 2 * (q0 * q1 + q2 * q3), + 1 - 2 * q1**2 - 2 * q2**2, + ], + ] + ) return R def fill_states(self, state: np.ndarray) -> None: @@ -41,10 +55,14 @@ def fill_states(self, state: np.ndarray) -> None: self.velocity = state[7:10] self.angular_velocity = state[10:13] - def fill_states_different_dim(self, state: np.ndarray, state_euler: np.ndarray) -> None: + def fill_states_different_dim( + self, state: np.ndarray, state_euler: np.ndarray + ) -> None: """Fills states when the state vector has different dimensions than the default state vector.""" self.position = state[0:3] + state_euler[0:3] - self.orientation = quaternion_super_product(state[3:7], euler_to_quat(state_euler[3:6])) + self.orientation = quaternion_super_product( + state[3:7], euler_to_quat(state_euler[3:6]) + ) self.velocity = state[7:10] + state_euler[6:9] self.angular_velocity = state[10:13] + state_euler[9:12] @@ -62,7 +80,9 @@ def __add__(self, other: 'StateQuat') -> 'StateQuat': """Adds two StateQuat objects.""" new_state = StateQuat() new_state.position = self.position + other.position - new_state.orientation = quaternion_super_product(self.orientation, other.orientation) + new_state.orientation = quaternion_super_product( + self.orientation, other.orientation + ) new_state.velocity = self.velocity + other.velocity new_state.angular_velocity = self.angular_velocity + other.angular_velocity @@ -92,7 +112,9 @@ def insert_weights(self, weights: np.ndarray) -> np.ndarray: """Inserts the weights into the covariance matrix.""" new_state = StateQuat() new_state.position = self.position - weights[:3] - new_state.orientation = quaternion_error(self.orientation, euler_to_quat(weights[3:6])) + new_state.orientation = quaternion_error( + self.orientation, euler_to_quat(weights[3:6]) + ) new_state.velocity = self.velocity - weights[6:9] new_state.angular_velocity = self.angular_velocity - weights[9:12] @@ -104,11 +126,12 @@ def add_without_quaternions(self, other: 'StateQuat') -> None: self.velocity += other.velocity self.angular_velocity += other.angular_velocity + @dataclass class MeasModel: + """A class defined for a general measurement model. """ - A class defined for a general measurement model. - """ + measurement: np.ndarray = field(default_factory=lambda: np.zeros(3)) covariance: np.ndarray = field(default_factory=lambda: np.zeros((3, 3))) @@ -138,11 +161,12 @@ def __sub__(self, other: 'MeasModel') -> 'MeasModel': result.measurement = self.measurement - other.measurement return result + @dataclass class process_model: + """A class defined for a general process model. """ - A class defined for a general process model. - """ + state_vector: StateQuat = field(default_factory=StateQuat) state_vector_dot: StateQuat = field(default_factory=StateQuat) state_vector_prev: StateQuat = field(default_factory=StateQuat) @@ -152,7 +176,7 @@ class process_model: damping_linear: np.ndarray = field(default_factory=lambda: np.zeros(6)) damping_nonlinear: np.ndarray = field(default_factory=lambda: np.zeros(6)) m: float = 0.0 - inertia: np.ndarray = field(default_factory=lambda: np.zeros((3,3))) + inertia: np.ndarray = field(default_factory=lambda: np.zeros((3, 3))) r_b_bg: np.ndarray = field(default_factory=lambda: np.zeros(3)) dt: float = 0.0 integral_error_position: np.ndarray = field(default_factory=lambda: np.zeros(3)) @@ -163,22 +187,33 @@ class process_model: def R(self) -> np.ndarray: """Calculates the rotation matrix.""" nu, e_1, e_2, e_3 = self.state_vector.orientation - R = np.array([ - [1 - 2 * e_2 ** 2 - 2 * e_3 ** 2, 2 * e_1 * e_2 - 2 * nu * e_3, 2 * e_1 * e_3 + 2 * nu * e_2], - [2 * e_1 * e_2 + 2 * nu * e_3, 1 - 2 * e_1 ** 2 - 2 * e_3 ** 2, 2 * e_2 * e_3 - 2 * nu * e_1], - [2 * e_1 * e_3 - 2 * nu * e_2, 2 * e_2 * e_3 + 2 * nu * e_1, 1 - 2 * e_1 ** 2 - 2 * e_2 ** 2] - ]) + R = np.array( + [ + [ + 1 - 2 * e_2**2 - 2 * e_3**2, + 2 * e_1 * e_2 - 2 * nu * e_3, + 2 * e_1 * e_3 + 2 * nu * e_2, + ], + [ + 2 * e_1 * e_2 + 2 * nu * e_3, + 1 - 2 * e_1**2 - 2 * e_3**2, + 2 * e_2 * e_3 - 2 * nu * e_1, + ], + [ + 2 * e_1 * e_3 - 2 * nu * e_2, + 2 * e_2 * e_3 + 2 * nu * e_1, + 1 - 2 * e_1**2 - 2 * e_2**2, + ], + ] + ) return R def T(self) -> np.ndarray: """Calculates the transformation matrix.""" nu, e_1, e_2, e_3 = self.state_vector.orientation - T = 0.5 * np.array([ - [-e_1, -e_2, -e_3], - [nu, -e_3, e_2], - [e_3, nu, -e_1], - [-e_2, e_1, nu] - ]) + T = 0.5 * np.array( + [[-e_1, -e_2, -e_3], [nu, -e_3, e_2], [e_3, nu, -e_1], [-e_2, e_1, nu]] + ) return T def Crb(self) -> np.ndarray: @@ -203,138 +238,157 @@ def model_prediction(self, state: StateQuat) -> None: """Calculates the model of the system.""" self.state_vector = state self.state_vector_dot.position = np.dot(self.R(), self.state_vector.velocity) - self.state_vector_dot.orientation = np.dot(self.T(), self.state_vector.angular_velocity) - Nu = np.linalg.inv(self.mass_interia_matrix + np.diag(self.added_mass)) @ (self.Control_input - np.dot(self.Crb(), self.state_vector.nu()) - np.dot(self.D(), self.state_vector.nu())) + self.state_vector_dot.orientation = np.dot( + self.T(), self.state_vector.angular_velocity + ) + Nu = np.linalg.inv(self.mass_interia_matrix + np.diag(self.added_mass)) @ ( + self.Control_input + - np.dot(self.Crb(), self.state_vector.nu()) + - np.dot(self.D(), self.state_vector.nu()) + ) self.state_vector_dot.velocity = Nu[:3] self.state_vector_dot.angular_velocity = Nu[3:] def euler_forward(self) -> StateQuat: """Calculates the forward Euler integration.""" - self.state_vector.position = self.state_vector_prev.position + self.state_vector_dot.position * self.dt - self.state_vector.orientation = quat_norm(self.state_vector_prev.orientation + self.state_vector_dot.orientation * self.dt) - self.state_vector.velocity = self.state_vector_prev.velocity + self.state_vector_dot.velocity * self.dt - self.state_vector.angular_velocity = self.state_vector_prev.angular_velocity + self.state_vector_dot.angular_velocity * self.dt + self.state_vector.position = ( + self.state_vector_prev.position + self.state_vector_dot.position * self.dt + ) + self.state_vector.orientation = quat_norm( + self.state_vector_prev.orientation + + self.state_vector_dot.orientation * self.dt + ) + self.state_vector.velocity = ( + self.state_vector_prev.velocity + self.state_vector_dot.velocity * self.dt + ) + self.state_vector.angular_velocity = ( + self.state_vector_prev.angular_velocity + + self.state_vector_dot.angular_velocity * self.dt + ) return self.state_vector + def euler_to_quat(euler_angles: np.ndarray) -> np.ndarray: - """ - Converts Euler angles to a quaternion + """Converts Euler angles to a quaternion """ psi, theta, phi = euler_angles c_psi = np.cos(psi / 2) - s_psi = np.sin(psi / 2) + s_psi = np.sin(psi / 2) c_theta = np.cos(theta / 2) s_theta = np.sin(theta / 2) c_phi = np.cos(phi / 2) s_phi = np.sin(phi / 2) - quat = np.array([ - c_psi * c_theta * c_phi + s_psi * s_theta * s_phi, - c_psi * c_theta * s_phi - s_psi * s_theta * c_phi, - s_psi * c_theta * s_phi + c_psi * s_theta * c_phi, - s_psi * c_theta * c_phi - c_psi * s_theta * s_phi - ]) + quat = np.array( + [ + c_psi * c_theta * c_phi + s_psi * s_theta * s_phi, + c_psi * c_theta * s_phi - s_psi * s_theta * c_phi, + s_psi * c_theta * s_phi + c_psi * s_theta * c_phi, + s_psi * c_theta * c_phi - c_psi * s_theta * s_phi, + ] + ) return quat + def quat_to_euler(quat: np.ndarray) -> np.ndarray: - """ - Converts a quaternion to Euler angles + """Converts a quaternion to Euler angles """ nu, eta_1, eta_2, eta_3 = quat - phi = np.arctan2(2*(eta_2 * eta_3 + nu * eta_1), 1 - 2 * (eta_1 ** 2 + eta_2 ** 2)) + phi = np.arctan2(2 * (eta_2 * eta_3 + nu * eta_1), 1 - 2 * (eta_1**2 + eta_2**2)) theta = -np.arcsin(2 * (eta_1 * eta_3 - nu * eta_2)) - psi = np.arctan2(2 * (nu * eta_3 + eta_1 * eta_2), 1 - 2 * (eta_2 ** 2 + eta_3 ** 2)) + psi = np.arctan2(2 * (nu * eta_3 + eta_1 * eta_2), 1 - 2 * (eta_2**2 + eta_3**2)) return np.array([phi, theta, psi]) + def quat_norm(quat: np.ndarray) -> np.ndarray: + """Function that normalizes a quaternion """ - Function that normalizes a quaternion - """ - quat = quat / np.linalg.norm(quat) return quat + def skew_symmetric(vector: np.ndarray) -> np.ndarray: - """Calculates the skew symmetric matrix of a vector. + """Calculates the skew symmetric matrix of a vector. - Args: - vector (np.ndarray): The vector. + Args: + vector (np.ndarray): The vector. + + Returns: + np.ndarray: The skew symmetric matrix. + """ + return np.array( + [ + [0, -vector[2], vector[1]], + [vector[2], 0, -vector[0]], + [-vector[1], vector[0], 0], + ] + ) - Returns: - np.ndarray: The skew symmetric matrix. - """ - return np.array( - [ - [0, -vector[2], vector[1]], - [vector[2], 0, -vector[0]], - [-vector[1], vector[0], 0], - ] - ) def quaternion_super_product(q1: np.ndarray, q2: np.ndarray) -> np.ndarray: - """Calculates the quaternion super product of two quaternions. + """Calculates the quaternion super product of two quaternions. - Args: - q1 (np.ndarray): The first quaternion. - q2 (np.ndarray): The second quaternion. + Args: + q1 (np.ndarray): The first quaternion. + q2 (np.ndarray): The second quaternion. - Returns: - np.ndarray: The quaternion super product. - """ - eta_0, e_0_x, e_0_y, e_0_z = q1 - eta_1, e_1_x, e_1_y, e_1_z = q2 + Returns: + np.ndarray: The quaternion super product. + """ + eta_0, e_0_x, e_0_y, e_0_z = q1 + eta_1, e_1_x, e_1_y, e_1_z = q2 + + e_0 = np.array([e_0_x, e_0_y, e_0_z]) + e_1 = np.array([e_1_x, e_1_y, e_1_z]) - e_0 = np.array([e_0_x, e_0_y, e_0_z]) - e_1 = np.array([e_1_x, e_1_y, e_1_z]) + eta_new = eta_0 * eta_1 - (e_0_x * e_1_x + e_0_y * e_1_y + e_0_z * e_1_z) + nu_new = e_1 * eta_0 + e_0 * eta_1 + np.dot(skew_symmetric(e_0), e_1) - eta_new = eta_0 * eta_1 - (e_0_x * e_1_x + e_0_y * e_1_y + e_0_z * e_1_z) - nu_new = e_1 * eta_0 + e_0 * eta_1 + np.dot(skew_symmetric(e_0), e_1) + q_new = quat_norm(np.array([eta_new, nu_new[0], nu_new[1], nu_new[2]])) - q_new = quat_norm(np.array([eta_new, nu_new[0], nu_new[1], nu_new[2]])) + return q_new - return q_new def quaternion_error(quat_1: np.ndarray, quat_2: np.ndarray) -> np.ndarray: + """Calculates the error between two quaternions """ - Calculates the error between two quaternions - """ - quat_2_inv = np.array([quat_2[0], -quat_2[1], -quat_2[2], -quat_2[3]]) error_quat = quaternion_super_product(quat_1, quat_2_inv) return error_quat -def iterative_quaternion_mean_statequat(state_list: list[StateQuat], tol: float = 1e-6, max_iter: int = 100) -> np.ndarray: - """ - Computes the weighted mean of the quaternion orientations from a list of StateQuat objects + +def iterative_quaternion_mean_statequat( + state_list: list[StateQuat], tol: float = 1e-6, max_iter: int = 100 +) -> np.ndarray: + """Computes the weighted mean of the quaternion orientations from a list of StateQuat objects using an iterative approach, without requiring the caller to manually extract the quaternion. - + Parameters: state_list (list[StateQuat]): List of StateQuat objects. weights (np.ndarray): Weights for each state. tol (float): Convergence tolerance. max_iter (int): Maximum number of iterations. - + Returns: np.ndarray: The averaged quaternion as a 4-element numpy array. """ - sigma_quats = [state.orientation for state in state_list] n = len(state_list) mean_q = sigma_quats[0].copy() - + for _ in range(max_iter): weighted_error_vectors = [] for i, q in enumerate(sigma_quats): mean_q_conj = np.array([mean_q[0], -mean_q[1], -mean_q[2], -mean_q[3]]) e = quaternion_super_product(q, mean_q_conj) - + e0_clipped = np.clip(e[0], -1.0, 1.0) angle = 2 * np.arccos(e0_clipped) if np.abs(angle) < 1e-8: @@ -342,28 +396,32 @@ def iterative_quaternion_mean_statequat(state_list: list[StateQuat], tol: float else: error_vec = (angle / np.sin(angle / 2)) * e[1:4] weighted_error_vectors.append(error_vec) - + error_avg = (1 / n) * np.sum(weighted_error_vectors, axis=0) if np.linalg.norm(error_avg) < tol: break - + error_norm = np.linalg.norm(error_avg) if error_norm > 0: - delta_q = np.array([np.cos(error_norm / 2), - *(np.sin(error_norm / 2) * (error_avg / error_norm))]) + delta_q = np.array( + [ + np.cos(error_norm / 2), + *(np.sin(error_norm / 2) * (error_avg / error_norm)), + ] + ) else: delta_q = np.array([1.0, 0.0, 0.0, 0.0]) mean_q = quaternion_super_product(delta_q, mean_q) mean_q = quat_norm(mean_q) - + return mean_q + def mean_set(set_points: list[StateQuat]) -> np.ndarray: - """ - Functio calculates the mean vector of a set of points - - Args: + """Function calculates the mean vector of a set of points + + Args: set_points (list[StateQuat]): List of StateQuat objects Returns: @@ -374,30 +432,30 @@ def mean_set(set_points: list[StateQuat]) -> np.ndarray: for state in set_points: mean_value.add_without_quaternions(state) - - mean_value = (1 / (n)) * mean_value + + mean_value = (1 / (n)) * mean_value mean_value.orientation = iterative_quaternion_mean_statequat(set_points) return mean_value.as_vector() + def mean_measurement(set_points: list[MeasModel]) -> np.ndarray: - """ - Function that calculates the mean of a set of points + """Function that calculates the mean of a set of points """ n = len(set_points) mean_value = MeasModel() for state in set_points: mean_value = mean_value + state - + mean_value = (1 / n) * mean_value return mean_value.measurement + def covariance_set(set_points: list[StateQuat], mean: np.ndarray) -> np.ndarray: - """ - Function that calculates the covariance of a set of points + """Function that calculates the covariance of a set of points """ n = len(set_points) covariance = np.zeros(set_points[0].covariance.shape) @@ -410,23 +468,25 @@ def covariance_set(set_points: list[StateQuat], mean: np.ndarray) -> np.ndarray: for state in set_points: q = state.orientation diff_q = quaternion_error(q, mean_q) - + e0_clipped = np.clip(diff_q[0], -1.0, 1.0) angle = 2.0 * np.arccos(e0_clipped) if abs(angle) < 1e-8: e_vec = np.zeros(3) else: - e_vec = (angle / np.sin(angle/2)) * diff_q[1:4] + e_vec = (angle / np.sin(angle / 2)) * diff_q[1:4] - covariance += np.outer(state.subtract(mean_quat, e_vec), state.subtract(mean_quat, e_vec)) + covariance += np.outer( + state.subtract(mean_quat, e_vec), state.subtract(mean_quat, e_vec) + ) covariance = (1 / (n)) * covariance return covariance + def covariance_measurement(set_points: list[MeasModel], mean: np.ndarray) -> np.ndarray: - """ - Function that calculates the covariance of a set of points + """Function that calculates the covariance of a set of points """ n = len(set_points) co_size = len(set_points[0].measurement) @@ -443,9 +503,14 @@ def covariance_measurement(set_points: list[MeasModel], mean: np.ndarray) -> np. return covariance -def cross_covariance(set_y: list[StateQuat], mean_y: np.ndarray, set_z: list[MeasModel], mean_z: np.ndarray) -> np.ndarray: - """ - Calculates the cross covariance between the measurement and state prediction + +def cross_covariance( + set_y: list[StateQuat], + mean_y: np.ndarray, + set_z: list[MeasModel], + mean_z: np.ndarray, +) -> np.ndarray: + """Calculates the cross covariance between the measurement and state prediction """ n = len(set_y) @@ -458,16 +523,18 @@ def cross_covariance(set_y: list[StateQuat], mean_y: np.ndarray, set_z: list[Mea for i in range(n): q = set_y[i].orientation diff_q = quaternion_error(q, mean_q) - + e0_clipped = np.clip(diff_q[0], -1.0, 1.0) angle = 2.0 * np.arccos(e0_clipped) if abs(angle) < 1e-8: e_vec = np.zeros(3) else: - e_vec = (angle / np.sin(angle/2)) * diff_q[1:4] + e_vec = (angle / np.sin(angle / 2)) * diff_q[1:4] + + cross_covariance += np.outer( + set_y[i].subtract(mean_quat, e_vec), set_z[i].measurement - mean_z + ) - cross_covariance += np.outer(set_y[i].subtract(mean_quat, e_vec), set_z[i].measurement - mean_z) - cross_covariance = (1 / n) * cross_covariance return cross_covariance diff --git a/navigation/ukf_okid/ukf_python/ukf_test.py b/navigation/ukf_okid/ukf_python/ukf_test.py index 3c197ed63..cebb53ac6 100644 --- a/navigation/ukf_okid/ukf_python/ukf_test.py +++ b/navigation/ukf_okid/ukf_python/ukf_test.py @@ -1,27 +1,28 @@ -from ukf_okid import UKF -from ukf_okid_class import StateQuat, process_model, MeasModel -import numpy as np import time -import matplotlib.pyplot as plt -from ukf_utils import print_StateQuat_list, print_StateQuat -from ukf_okid_class import quaternion_super_product, quat_to_euler, mean_set, covariance_set +import matplotlib.pyplot as plt +import numpy as np +from ukf_okid import UKF +from ukf_okid_class import ( + MeasModel, + StateQuat, + process_model, + quat_to_euler, + quaternion_super_product, +) def add_quaternion_noise(q, noise_std): - noise = np.random.normal(0, noise_std, 3) theta = np.linalg.norm(noise) if theta > 0: - axis = noise / theta - q_noise = np.hstack((np.cos(theta/2), np.sin(theta/2) * axis)) + q_noise = np.hstack((np.cos(theta / 2), np.sin(theta / 2) * axis)) else: - q_noise = np.array([1.0, 0.0, 0.0, 0.0]) q_new = quaternion_super_product(q, q_noise) @@ -30,7 +31,6 @@ def add_quaternion_noise(q, noise_std): if __name__ == '__main__': - # Create initial state vector and covariance matrix. x0 = np.zeros(13) x0[0:3] = [0.3, 0.3, 0.3] @@ -38,20 +38,22 @@ def add_quaternion_noise(q, noise_std): x0[7:10] = [0.2, 0.2, 0.2] dt = 0.01 R = (0.01) * np.eye(3) - + Q = 0.00015 * np.eye(12) P0 = np.eye(12) * 0.0001 model = process_model() model.dt = 0.01 - model.mass_interia_matrix = np.array([ - [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] - ]) + model.mass_interia_matrix = np.array( + [ + [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], + ] + ) model.m = 30.0 model.r_b_bg = np.array([0.01, 0.0, 0.02]) model.inertia = np.diag([0.68, 3.32, 3.34]) @@ -61,14 +63,16 @@ def add_quaternion_noise(q, noise_std): model_ukf = process_model() model_ukf.dt = 0.01 - model_ukf.mass_interia_matrix = np.array([ - [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] - ]) + model_ukf.mass_interia_matrix = np.array( + [ + [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], + ] + ) model_ukf.m = 30.0 model_ukf.r_b_bg = np.array([0.01, 0.0, 0.02]) model_ukf.inertia = np.diag([0.68, 3.32, 3.34]) @@ -101,7 +105,7 @@ def add_quaternion_noise(q, noise_std): measurment_model = MeasModel() measurment_model.measurement = np.array([0.0, 0.0, 0.0]) - measurment_model.covariance = R + measurment_model.covariance = R # Initialize arrays to store the results positions = np.zeros((num_steps, 3)) @@ -129,7 +133,16 @@ def add_quaternion_noise(q, noise_std): elapsed_times = [] - u = lambda t: np.array([2 * np.sin(1 * t), 2 * np.sin(1 * t), 2 * np.sin(1 * t), 0.2 * np.cos(1 * t), 0.2 * np.cos(1 * t), 0.2 * np.cos(1 * t)]) + u = lambda t: np.array( + [ + 2 * np.sin(1 * t), + 2 * np.sin(1 * t), + 2 * np.sin(1 * t), + 0.2 * np.cos(1 * t), + 0.2 * np.cos(1 * t), + 0.2 * np.cos(1 * t), + ] + ) # Run the simulation for step in range(num_steps): @@ -142,10 +155,18 @@ def add_quaternion_noise(q, noise_std): new_state = model.euler_forward() # Adding noise in the state vector - estimated_state.position = estimated_state.position # + np.random.normal(0, 0.01, 3) - estimated_state.orientation = estimated_state.orientation #add_quaternion_noise(estimated_state.orientation, 0.01) - estimated_state.velocity = estimated_state.velocity # + np.random.normal(0, 0.01, 3) - estimated_state.angular_velocity = estimated_state.angular_velocity # + np.random.normal(0, 0.01, 3) + estimated_state.position = ( + estimated_state.position + ) # + np.random.normal(0, 0.01, 3) + estimated_state.orientation = ( + estimated_state.orientation + ) # add_quaternion_noise(estimated_state.orientation, 0.01) + estimated_state.velocity = ( + estimated_state.velocity + ) # + np.random.normal(0, 0.01, 3) + estimated_state.angular_velocity = ( + estimated_state.angular_velocity + ) # + np.random.normal(0, 0.01, 3) start_time = time.time() estimated_state = ukf.unscented_transform(estimated_state) @@ -155,10 +176,15 @@ def add_quaternion_noise(q, noise_std): elapsed_times.append(elapsed_time) if step % 20 == 0: - measurment_model.measurement = new_state.velocity # + np.random.normal(0, 0.01, 3) - meas_update, covariance_matrix = ukf.measurement_update(estimated_state, measurment_model) - estimated_state = ukf.posteriori_estimate(estimated_state, covariance_matrix, measurment_model, meas_update) - + measurment_model.measurement = ( + new_state.velocity + ) # + np.random.normal(0, 0.01, 3) + meas_update, covariance_matrix = ukf.measurement_update( + estimated_state, measurment_model + ) + estimated_state = ukf.posteriori_estimate( + estimated_state, covariance_matrix, measurment_model, meas_update + ) positions[step, :] = new_state.position orientations[step, :] = quat_to_euler(new_state.orientation) @@ -169,7 +195,7 @@ def add_quaternion_noise(q, noise_std): orientations_est[step, :] = quat_to_euler(estimated_state.orientation) velocities_est[step, :] = estimated_state.velocity angular_velocities_est[step, :] = estimated_state.angular_velocity - + # Update the state for the next iteration model.state_vector_prev = new_state @@ -294,4 +320,4 @@ def add_quaternion_noise(q, noise_std): plt.legend() plt.tight_layout() - plt.show() \ No newline at end of file + plt.show() diff --git a/navigation/ukf_okid/ukf_python/ukf_utils.py b/navigation/ukf_okid/ukf_python/ukf_utils.py index ad5871567..da56a7dfc 100644 --- a/navigation/ukf_okid/ukf_python/ukf_utils.py +++ b/navigation/ukf_okid/ukf_python/ukf_utils.py @@ -1,19 +1,21 @@ + import numpy as np -from dataclasses import dataclass from ukf_okid_class import StateQuat -def print_StateQuat_list(state_list: list[StateQuat], name="StateQuat List", print_covariance=True): - """ - Custom print function to print a list of StateQuat objects in a formatted form. + +def print_StateQuat_list( + state_list: list[StateQuat], name="StateQuat List", print_covariance=True +): + """Custom print function to print a list of StateQuat objects in a formatted form. """ print(f"{name}:") for i, state in enumerate(state_list): print(f"Index {i}:") print_StateQuat(state, f"StateQuat {i}", print_covariance) + def print_StateQuat(state: StateQuat, name="StateQuat", print_covariance=True): - """ - Custom print function to print StateQuat objects in a formatted form. + """Custom print function to print StateQuat objects in a formatted form. """ print(f"{name}:") print(f" Position: {state.position}") @@ -24,9 +26,9 @@ def print_StateQuat(state: StateQuat, name="StateQuat", print_covariance=True): if print_covariance: print_matrix(state.covariance, "Covariance") + def print_matrix(matrix, name="Matrix"): - """ - Custom print function to print matrices in a formatted form. + """Custom print function to print matrices in a formatted form. """ print(f"{name}: {matrix.shape}") if isinstance(matrix, np.ndarray): From b0e97bded958fd2451ffd118a4ece605438a827b Mon Sep 17 00:00:00 2001 From: Talha Nauman Choudhry Date: Thu, 3 Apr 2025 19:31:45 +0200 Subject: [PATCH 094/290] refactor: remove python eskf --- navigation/eskf_python/CMakeLists.txt | 33 -- navigation/eskf_python/README.md | 0 .../eskf_python/config/eskf_python.yaml | 3 - .../eskf_python/eskf_python/__init__.py | 0 .../eskf_python/eskf_python_class.py | 196 ---------- .../eskf_python/eskf_python_filter.py | 295 --------------- .../eskf_python/eskf_python_node.py | 151 -------- .../eskf_python/eskf_python_utils.py | 148 -------- .../eskf_python/eskf_python/eskf_test.py | 358 ------------------ .../eskf_python/eskf_test_utils.py | 243 ------------ navigation/eskf_python/launch/eskf.launch.py | 22 -- navigation/eskf_python/package.xml | 23 -- navigation/ukf_okid/ukf_python/rest.py | 6 +- navigation/ukf_okid/ukf_python/ukf_okid.py | 10 +- .../ukf_okid/ukf_python/ukf_okid_class.py | 33 +- navigation/ukf_okid/ukf_python/ukf_utils.py | 10 +- 16 files changed, 19 insertions(+), 1512 deletions(-) delete mode 100644 navigation/eskf_python/CMakeLists.txt delete mode 100644 navigation/eskf_python/README.md delete mode 100644 navigation/eskf_python/config/eskf_python.yaml delete mode 100644 navigation/eskf_python/eskf_python/__init__.py delete mode 100644 navigation/eskf_python/eskf_python/eskf_python_class.py delete mode 100644 navigation/eskf_python/eskf_python/eskf_python_filter.py delete mode 100644 navigation/eskf_python/eskf_python/eskf_python_node.py delete mode 100644 navigation/eskf_python/eskf_python/eskf_python_utils.py delete mode 100644 navigation/eskf_python/eskf_python/eskf_test.py delete mode 100644 navigation/eskf_python/eskf_python/eskf_test_utils.py delete mode 100644 navigation/eskf_python/launch/eskf.launch.py delete mode 100644 navigation/eskf_python/package.xml diff --git a/navigation/eskf_python/CMakeLists.txt b/navigation/eskf_python/CMakeLists.txt deleted file mode 100644 index b4fc9118c..000000000 --- a/navigation/eskf_python/CMakeLists.txt +++ /dev/null @@ -1,33 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(eskf_python) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake_python REQUIRED) -find_package(rclpy REQUIRED) -find_package(vortex_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) - -ament_python_install_package(${PROJECT_NAME}) - -install(DIRECTORY - launch - config - DESTINATION share/${PROJECT_NAME} -) - -install(PROGRAMS - eskf_python/eskf_python_node.py - DESTINATION lib/${PROJECT_NAME} -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - find_package(ament_cmake_pytest REQUIRED) - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) -endif() - -ament_package() diff --git a/navigation/eskf_python/README.md b/navigation/eskf_python/README.md deleted file mode 100644 index e69de29bb..000000000 diff --git a/navigation/eskf_python/config/eskf_python.yaml b/navigation/eskf_python/config/eskf_python.yaml deleted file mode 100644 index 0d80b90df..000000000 --- a/navigation/eskf_python/config/eskf_python.yaml +++ /dev/null @@ -1,3 +0,0 @@ -/**: - ros__parameters: - eskf_python_node: diff --git a/navigation/eskf_python/eskf_python/__init__.py b/navigation/eskf_python/eskf_python/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/navigation/eskf_python/eskf_python/eskf_python_class.py b/navigation/eskf_python/eskf_python/eskf_python_class.py deleted file mode 100644 index 65d41425d..000000000 --- a/navigation/eskf_python/eskf_python/eskf_python_class.py +++ /dev/null @@ -1,196 +0,0 @@ -from dataclasses import dataclass, field - -import numpy as np -from eskf_python_utils import quaternion_error - - -@dataclass -class StateQuat: - position: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Position vector (x, y, z) - velocity: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Velocity vector (u, v, w) - orientation: np.ndarray = field( - default_factory=lambda: np.array([1, 0, 0, 0]) - ) # Orientation quaternion (w, x, y, z) - acceleration_bias: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Acceleration bias vector (b_ax, b_ay, b_az) - gyro_bias: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Gyro bias vector (b_gx, b_gy, b_gz) - g: np.ndarray = field(default_factory=lambda: np.array([0, 0, 0])) # Gravity vector - - def as_vector(self) -> np.ndarray: - """Returns the state vector as a numpy array. - - Returns: - np.ndarray: The state vector. - """ - return np.concatenate( - [ - self.position, - self.velocity, - self.orientation, - self.acceleration_bias, - self.gyro_bias, - self.g, - ] - ) - - def fill_states(self, state: np.ndarray) -> None: - """Fills the state vector with the values from a numpy array. - - Args: - state (np.ndarray): The state vector. - """ - self.position = state[0:3] - self.velocity = state[3:6] - self.orientation = state[6:10] - self.acceleration_bias = state[10:13] - self.gyro_bias = state[13:16] - self.g = state[16:19] - - def R_q(self) -> np.ndarray: - """Calculates the rotation matrix from the orientation quaternion. - - Returns: - np.ndarray: The rotation matrix. - """ - q0, q1, q2, q3 = self.orientation - R = np.array( - [ - [ - 1 - 2 * q2**2 - 2 * q3**2, - 2 * (q1 * q2 - q0 * q3), - 2 * (q0 * q2 + q1 * q3), - ], - [ - 2 * (q1 * q2 + q0 * q3), - 1 - 2 * q1**2 - 2 * q3**2, - 2 * (q2 * q3 - q0 * q1), - ], - [ - 2 * (q1 * q3 - q0 * q2), - 2 * (q0 * q1 + q2 * q3), - 1 - 2 * q1**2 - 2 * q2**2, - ], - ] - ) - - return R - - def __sub__(self, other: 'StateQuat') -> 'StateQuat': - """Subtracts the values of two state vectors. - - Args: - other (StateQuat): The state vector to subtract. - - Returns: - np.ndarray: The difference between the two state vectors. - """ - result = StateQuat() - result.position = self.position - other.position - result.velocity = self.velocity - other.velocity - result.orientation = quaternion_error(self.orientation, other.orientation) - result.acceleration_bias = self.acceleration_bias - other.acceleration_bias - result.gyro_bias = self.gyro_bias - other.gyro_bias - result.g = self.g - other.g - - return result - - -@dataclass -class StateEuler: - position: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Position vector (x, y, z) - velocity: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Velocity vector (u, v, w) - orientation: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Orientation angles (roll, pitch, yaw) - acceleration_bias: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Acceleration bias vector (b_ax, b_ay, b_az) - gyro_bias: np.ndarray = field( - default_factory=lambda: np.zeros(3) - ) # Gyro bias vector (b_gx, b_gy, b_gz) - g: np.ndarray = field( - default_factory=lambda: np.array([0, 0, 9.81]) - ) # Gravity vector - covariance: np.ndarray = field( - default_factory=lambda: np.zeros((18, 18)) - ) # Covariance matrix - - def as_vector(self) -> np.ndarray: - """Returns the state vector as a numpy array. - - Returns: - np.ndarray: The state vector. - """ - return np.concatenate( - [ - self.position, - self.velocity, - self.orientation, - self.acceleration_bias, - self.gyro_bias, - self.g, - ] - ) - - def fill_states(self, state: np.ndarray) -> None: - """Fills the state vector with the values from a numpy array. - - Args: - state (np.ndarray): The state vector. - """ - self.position = state[0:3] - self.velocity = state[3:6] - self.orientation = state[6:9] - self.acceleration_bias = state[9:12] - self.gyro_bias = state[12:15] - self.g = state[15:18] - - def copy_state(self, wanted_state: 'StateEuler') -> None: - """Copies the state from a StateVector object into the current StateVector object. - - Args: - wanted_state (StateVector_euler): The quaternion state to copy from. - """ - self.position = wanted_state.position - self.velocity = wanted_state.velocity - self.orientation = wanted_state.orientation - self.acceleration_bias = wanted_state.acceleration_bias - self.gyro_bias = wanted_state.gyro_bias - - -@dataclass -class MeasurementModel: - measurement: np.ndarray = field(default_factory=lambda: np.zeros(6)) - measurement_covariance: np.ndarray = field(default_factory=lambda: np.zeros((6, 6))) - - def H(self) -> np.ndarray: - """Calculates the measurement matrix. - - Returns: - np.ndarray: The measurement matrix. - """ - H = np.zeros((3, 15)) - - H[0:3, 3:6] = np.eye(3) - - return H - - -@dataclass -class Measurement: - acceleration: np.ndarray = field(default_factory=lambda: np.zeros(3)) - angular_velocity: np.ndarray = field(default_factory=lambda: np.zeros(3)) - aiding: np.ndarray = field(default_factory=lambda: np.zeros(3)) - - aiding_covariance: np.ndarray = field(default_factory=lambda: np.zeros((3, 3))) diff --git a/navigation/eskf_python/eskf_python/eskf_python_filter.py b/navigation/eskf_python/eskf_python/eskf_python_filter.py deleted file mode 100644 index fac3c8526..000000000 --- a/navigation/eskf_python/eskf_python/eskf_python_filter.py +++ /dev/null @@ -1,295 +0,0 @@ -# from dataclasses import dataclass -from typing import Tuple - -import numpy as np -from eskf_python_class import Measurement, StateEuler, StateQuat -from eskf_python_utils import ( - angle_axis_to_quaternion, - euler_to_quat, - quaternion_product, - skew_matrix, -) -from scipy.linalg import block_diag, expm - - -class ESKF: - def __init__( - self, Q: np.ndarray, P0, nom_state: StateQuat, p_accBias, p_gyroBias, dt - ): - self.Q = Q - self.dt = dt - self.nom_state = nom_state - self.error_state = StateEuler() - self.error_state.covariance = P0 - self.p_accBias = p_accBias - self.p_gyroBias = p_gyroBias - - def Q_delta_theta(self) -> np.ndarray: - """Calculates the Q_delta_theta matrix. - See Joan Solà. Quaternion kinematics for the error-state Kalman filter. - chapter: 6.1.1 eq. 281 - """ - qw, qx, qy, qz = self.nom_state.orientation - - Q_delta_theta = 0.5 * np.array( - [ - [-qx, -qy, -qz], - [qw, -qz, qy], - [qz, qw, -qx], - [-qy, qx, qw], - ] - ) - - return Q_delta_theta - - def Hx(self) -> np.ndarray: - """Computes the true-state measurement Jacobian for the measurement - h(x) = R(q) * velocity, - where: - - R(q) is the rotation matrix from the quaternion (q, q, q, q) with q as the scalar part, - - velocity is a 3-vector. - - The state is assumed to be ordered as: - [position (3), velocity (3), quaternion (4), ...] (total length 19). - - The Jacobian Hx is a 3x19 matrix with nonzero blocks: - - Columns 3:6 (velocity): R(q) - - Columns 6:10 (quaternion): - """ - q = self.nom_state.orientation # shape (4,) - v = self.nom_state.velocity # shape (3,) - q0, q1, q2, q3 = q - v1, v2, v3 = v - - R = self.nom_state.R_q().transpose() # shape (3, 3) - - dhdq0 = 2 * np.array( - [ - q0 * v1 - q3 * v2 + q2 * v3, - q3 * v1 + q0 * v2 - q1 * v3, - -q2 * v1 + q1 * v2 + q0 * v3, - ] - ) - - dhdq1 = 2 * np.array( - [ - q1 * v1 + q2 * v2 + q3 * v3, - q2 * v1 - q1 * v2 - q0 * v3, - q3 * v1 + q0 * v2 - q1 * v3, - ] - ) - - dhdq2 = 2 * np.array( - [ - -q2 * v1 + q1 * v2 + q0 * v3, - q1 * v1 + q2 * v2 + q3 * v3, - -q0 * v1 + q3 * v2 - q2 * v3, - ] - ) - - dhdq3 = 2 * np.array( - [ - -q3 * v1 - q0 * v2 + q1 * v3, - q0 * v1 - q3 * v2 + q2 * v3, - q1 * v1 + q2 * v2 + q3 * v3, - ] - ) - - dHdq = np.column_stack((dhdq0, dhdq1, dhdq2, dhdq3)) # shape (3, 4) - - Hx = np.zeros((3, 19)) - Hx[:, 3:6] = R - Hx[:, 6:10] = dHdq - - return Hx - - def H(self) -> np.ndarray: - """Calculates the measurement matrix. - - Returns: - np.ndarray: The measurement matrix. - """ - X_deltax = block_diag(np.eye(6), self.Q_delta_theta(), np.eye(9)) - - H = self.Hx() @ X_deltax - - return H - - def h(self) -> np.ndarray: - """Calculates the measurement model. - - Returns: - np.ndarray: The measurement model. - """ - return self.nom_state.R_q() @ self.nom_state.velocity - - def nominal_state_discrete(self, imu_data: Measurement) -> None: - """Calculates the next nominal state using the discrete-time process model defined in: - Joan Solà. Quaternion kinematics for the error-state Kalman filter. - Chapter: 5.4.1 The nominal state kinematics - - Args: - imu_data (np.ndarray): The IMU data. - """ - # Rectify measurements. - acc_rect = imu_data.acceleration - self.nom_state.acceleration_bias - gyro_rect = imu_data.angular_velocity - self.nom_state.gyro_bias - - R = self.nom_state.R_q() - - self.nom_state.position = ( - self.nom_state.position - + self.nom_state.velocity * self.dt - + 0.5 * (R @ acc_rect + self.nom_state.g) * self.dt**2 - ) - self.nom_state.velocity = ( - self.nom_state.velocity + (R @ acc_rect + self.nom_state.g) * self.dt - ) - self.nom_state.orientation = quaternion_product( - self.nom_state.orientation, angle_axis_to_quaternion(gyro_rect * self.dt) - ) - self.nom_state.acceleration_bias = self.nom_state.acceleration_bias - self.nom_state.gyro_bias = self.nom_state.gyro_bias - self.nom_state.g = self.nom_state.g - - def van_loan_discretization(self, A_c, G_c) -> Tuple[np.ndarray, np.ndarray]: - """Calculates the Van Loan discretization of a continuous-time system. - - Args: - A_c (np.ndarray): The A matrix. - G_c (np.ndarray): The G matrix. - - Returns: - Tuple: The A_d and GQG_d matrices. - """ - GQG_T = np.dot(np.dot(G_c, self.Q), G_c.T) - - matrix_exp = ( - np.block( - [ - [-A_c, GQG_T], - [np.zeros((A_c.shape[0], A_c.shape[0])), np.transpose(A_c)], - ] - ) - * self.dt - ) - - van_loan_matrix = expm(matrix_exp) - - V1 = van_loan_matrix[A_c.shape[0] :, A_c.shape[0] :] - V2 = van_loan_matrix[: A_c.shape[0], A_c.shape[0] :] - - A_d = V1.T - GQG_d = A_d @ V2 - - return A_d, GQG_d - - def error_state_prediction(self, imu_data: Measurement) -> None: - # Rectify measurements. - acc_rect = imu_data.acceleration - self.nom_state.acceleration_bias - gyro_rect = imu_data.angular_velocity - self.nom_state.gyro_bias - - R = self.nom_state.R_q() - - A_c = np.zeros((18, 18)) - - A_c[0:3, 3:6] = np.eye(3) - A_c[3:6, 6:9] = -R @ skew_matrix(acc_rect) - A_c[6:9, 6:9] = -skew_matrix(gyro_rect) - A_c[3:6, 9:12] = -R - A_c[9:12, 9:12] = -self.p_accBias * np.eye(3) - A_c[12:15, 12:15] = -self.p_gyroBias * np.eye(3) - A_c[6:9, 12:15] = -np.eye(3) - A_c[3:6, 15:18] = np.eye(3) - - G_c = np.zeros((18, 12)) - - G_c[3:6, 0:3] = -R - G_c[6:9, 3:6] = -np.eye(3) - G_c[9:12, 6:9] = np.eye(3) - G_c[12:15, 9:12] = np.eye(3) - - A_d, GQG_d = self.van_loan_discretization(A_c, G_c) - - self.error_state.covariance = A_d @ self.error_state.covariance @ A_d.T + GQG_d - - def measurement_update(self, dvl_measurement: Measurement) -> float: - """Updates the error state using the DVL measurement. - Joan Solà. Quaternion kinematics for the error-state Kalman filter. - Chapter: 6.1 eq. 274-276 - - Args: - dvl_measurement (np.ndarray): The DVL measurement. - """ - H = self.H() - P = self.error_state.covariance - R = dvl_measurement.aiding_covariance - - S = H @ P @ H.T + R - K = P @ H.T @ np.linalg.inv(S) - innovation = dvl_measurement.aiding - self.h() - - NIS_value = self.NIS(S, innovation) - - self.error_state.fill_states(K @ innovation) - - I_KH = np.eye(18) - K @ H - self.error_state.covariance = ( - I_KH @ P @ I_KH.T + K @ R @ K.T - ) # Joseph form for more stability - return NIS_value - - def injection(self) -> None: - """Injects the error state into the nominal state to produce the estimated state. - Joan Solà. Quaternion kinematics for the error-state Kalman filter. - Chapter 6.2 eq. 282-283 - - """ - self.nom_state.position = self.nom_state.position + self.error_state.position - self.nom_state.velocity = self.nom_state.velocity + self.error_state.velocity - self.nom_state.orientation = quaternion_product( - self.nom_state.orientation, euler_to_quat(self.error_state.orientation) - ) - self.nom_state.acceleration_bias = ( - self.nom_state.acceleration_bias + self.error_state.acceleration_bias - ) - self.nom_state.gyro_bias = self.nom_state.gyro_bias + self.error_state.gyro_bias - self.nom_state.g = self.nom_state.g + self.error_state.g - - def reset_error_state(self) -> None: - """Resets the error state after injection. - Joan Solà. Quaternion kinematics for the error-state Kalman filter. - Chapter 6.3 eq. 284-286 - """ - G = np.eye(18) # Neglecting the delta_theta as this is most common in practice - - self.error_state.covariance = G @ self.error_state.covariance @ G.T - self.error_state.fill_states(np.zeros(18)) - - def imu_update(self, imu_data: Measurement) -> None: - """Updates the state using the IMU data.""" - self.nominal_state_discrete(imu_data) - self.error_state_prediction(imu_data) - - def dvl_update(self, dvl_measurement: Measurement) -> float: - """Updates the state using the DVL measurement.""" - NIS = self.measurement_update(dvl_measurement) - self.injection() - self.reset_error_state() - - return NIS - - # functions for tuning the filter - def NIS(self, S: np.ndarray, innovation: np.ndarray) -> float: - """Calculates the Normalized Innovation Squared (NIS) value.""" - return innovation.T @ np.linalg.inv(S) @ innovation - - def NEEDS( - self, P: np.ndarray, true_state: StateQuat, estimate_state: StateQuat - ) -> float: - """Calculates the Normalized Estimation Error Squared (NEEDS) value.""" - return ( - (true_state - estimate_state).as_vector().T - @ np.linalg.inv(P) - @ (true_state - estimate_state).as_vector() - ) diff --git a/navigation/eskf_python/eskf_python/eskf_python_node.py b/navigation/eskf_python/eskf_python/eskf_python_node.py deleted file mode 100644 index 7b300ecc6..000000000 --- a/navigation/eskf_python/eskf_python/eskf_python_node.py +++ /dev/null @@ -1,151 +0,0 @@ -#!/usr/bin/env python3 - -import numpy as np -import rclpy -from geometry_msgs.msg import TwistWithCovarianceStamped -from nav_msgs.msg import Odometry -from rclpy.node import Node -from rclpy.qos import QoSProfile, qos_profile_sensor_data -from sensor_msgs.msg import Imu - -# NEED TO CHANGE THIS TO THE CORRECT PATH -from eskf_python.eskf_python_filter import ( - ErrorStateKalmanFilter, - MeasurementModel, - StateVector_euler, - StateVector_quaternion, -) - -qos_profile = QoSProfile( - depth=1, - history=qos_profile_sensor_data.history, - reliability=qos_profile_sensor_data.reliability, -) - - -class ESKalmanFilterNode(Node): - def __init__(self): - super().__init__("eskf_python_node") - - # This callback will supply information from the IMU (Inertial Measurement Unit) 1000 Hz - self.imu_subscriber_ = self.create_subscription( - Imu, '/orca/imu', self.imu_callback, qos_profile=qos_profile - ) - - self.twist_dvl_subscriber_ = self.create_subscription( - TwistWithCovarianceStamped, - '/dvl/twist', - self.filter_callback, - qos_profile=qos_profile, - ) - - # This publisher will publish the estimtaed state of the vehicle - self.state_publisher_ = self.create_publisher( - Odometry, '/orca/odom', qos_profile=qos_profile - ) - - self.eskf_modual = ErrorStateKalmanFilter() - self.current_state_nom = StateVector_quaternion() - self.current_state_error = StateVector_euler() - self.measurement_pred = MeasurementModel() - self.odom_msg = Odometry() - - self.get_logger().info("Error State Kalman Filter started") - - def imu_callback(self, msg: Imu): - # Get the IMU data - - imu_acceleartion = msg.linear_acceleration - imu_angular_velocity = msg.angular_velocity - - # Combine the IMU data - imu_data = np.array( - [ - imu_acceleartion.x, - imu_acceleartion.y, - imu_acceleartion.z, - imu_angular_velocity.x, - imu_angular_velocity.y, - imu_angular_velocity.z, - ] - ) - - # Update the filter with the IMU data - self.current_state_nom, self.current_state_error = ( - ErrorStateKalmanFilter.imu_update_states( - self.current_state_nom, self.current_state_error, imu_data - ) - ) - - # Inserting the nominal state into the msg - self.odom_msg.pose.pose.position.x = self.current_state_nom.position[0] - self.odom_msg.pose.pose.position.y = self.current_state_nom.position[1] - self.odom_msg.pose.pose.position.z = self.current_state_nom.position[2] - self.odom_msg.pose.pose.orientation.x = self.current_state_nom.orientation[0] - self.odom_msg.pose.pose.orientation.y = self.current_state_nom.orientation[1] - self.odom_msg.pose.pose.orientation.z = self.current_state_nom.orientation[2] - self.odom_msg.pose.pose.orientation.w = self.current_state_nom.orientation[3] - self.odom_msg.twist.twist.linear.x = self.current_state_nom.velocity[0] - self.odom_msg.twist.twist.linear.y = self.current_state_nom.velocity[1] - self.odom_msg.twist.twist.linear.z = self.current_state_nom.velocity[2] - self.odom_msg.twist.twist.angular.x = imu_angular_velocity.x - self.odom_msg.twist.twist.angular.y = imu_angular_velocity.y - self.odom_msg.twist.twist.angular.z = imu_angular_velocity.z - - # Publish - self.state_publisher_.publish(self.odom_msg) - - def filter_callback(self, msg: TwistWithCovarianceStamped): - """Callback function for the filter measurement update, - this will be called when the filter needs to be updated with the DVL data. - """ - self.get_logger().info("Filter callback, got DVL data") - - # Get the DVL data (linear velocity) - dvl_data = np.array( - [ - msg.twist.twist.linear.x, - msg.twist.twist.linear.y, - msg.twist.twist.linear.z, - ] - ) - - # Update the filter with the DVL data - self.current_state_nom, self.current_state_error = ( - ErrorStateKalmanFilter.dvl_update_states( - self.current_state_nom, self.current_state_error, dvl_data - ) - ) - self.current_state_nom, self.current_state_error = ( - ErrorStateKalmanFilter.injection_and_reset( - self.current_state_nom, self.current_state_error - ) - ) - - # Inserting data into the msg - self.odom_msg.pose.pose.position.x = self.current_state_nom.position[0] - self.odom_msg.pose.pose.position.y = self.current_state_nom.position[1] - self.odom_msg.pose.pose.position.z = self.current_state_nom.position[2] - self.odom_msg.pose.pose.orientation.x = self.current_state_nom.orientation[0] - self.odom_msg.pose.pose.orientation.y = self.current_state_nom.orientation[1] - self.odom_msg.pose.pose.orientation.z = self.current_state_nom.orientation[2] - self.odom_msg.pose.pose.orientation.w = self.current_state_nom.orientation[3] - self.odom_msg.twist.twist.linear.x = self.current_state_nom.velocity[0] - self.odom_msg.twist.twist.linear.y = self.current_state_nom.velocity[1] - self.odom_msg.twist.twist.linear.z = self.current_state_nom.velocity[2] - self.odom_msg.twist.twist.linear.z = self.current_state_nom.velocity[2] - - # Publishing the data - self.state_publisher_.publish(self.odom_msg) - - -def main(args=None): - rclpy.init(args=args) - node = ESKalmanFilterNode() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/navigation/eskf_python/eskf_python/eskf_python_utils.py b/navigation/eskf_python/eskf_python/eskf_python_utils.py deleted file mode 100644 index bbe13d759..000000000 --- a/navigation/eskf_python/eskf_python/eskf_python_utils.py +++ /dev/null @@ -1,148 +0,0 @@ -import numpy as np - - -def skew_matrix(vector: np.ndarray) -> np.ndarray: - """Returns the skew symmetric matrix of a 3x1 vector. - """ - return np.array( - [ - [0, -vector[2], vector[1]], - [vector[2], 0, -vector[0]], - [-vector[1], vector[0], 0], - ] - ) - - -def quat_norm(quat: np.ndarray) -> np.ndarray: - """Function that normalizes a quaternion - """ - quat = quat / np.linalg.norm(quat) - - return quat - - -def quaternion_product(q1: np.ndarray, q2: np.ndarray) -> np.ndarray: - """Calculates the quaternion super product of two quaternions. - - Args: - q1 (np.ndarray): The first quaternion. - q2 (np.ndarray): The second quaternion. - - Returns: - np.ndarray: The quaternion super product. - """ - eta_0, e_0_x, e_0_y, e_0_z = q1 - eta_1, e_1_x, e_1_y, e_1_z = q2 - - e_0 = np.array([e_0_x, e_0_y, e_0_z]) - e_1 = np.array([e_1_x, e_1_y, e_1_z]) - - eta_new = eta_0 * eta_1 - np.dot(e_0, e_1) - nu_new = e_1 * eta_0 + e_0 * eta_1 + np.cross(e_0, e_1) - - q_new = np.array([eta_new, nu_new[0], nu_new[1], nu_new[2]]) - q_new = q_new / np.linalg.norm(q_new) - - return q_new - - -def quaternion_error(quat_1: np.ndarray, quat_2: np.ndarray) -> np.ndarray: - """Calculates the error between two quaternions - """ - quat_2_inv = np.array([quat_2[0], -quat_2[1], -quat_2[2], -quat_2[3]]) - - error_quat = quaternion_product(quat_1, quat_2_inv) - - return error_quat - - -def angle_axis_to_quaternion(vector: np.ndarray) -> np.ndarray: - """Converts an angle-axis representation to a quaternion. - - Args: - vector (np.ndarray): The angle-axis representation. - - Returns: - np.ndarray: The quaternion representation. - """ - angle = np.linalg.norm(vector) - if angle < 1e-8: - return np.array([1, 0, 0, 0]) - else: - axis = vector / angle - - q = np.zeros(4) - q[0] = np.cos(angle / 2) - q[1:] = np.sin(angle / 2) * axis - - return q - - -def R_from_angle_axis(vector: np.ndarray) -> np.ndarray: - """Calculates the rotation matrix from the angle-axis representation. - - Args: - vector (np.ndarray): The angle-axis representation. - - Returns: - np.ndarray: The rotation matrix. - """ - quaternion = angle_axis_to_quaternion(vector) - q0, q1, q2, q3 = quaternion - - R = np.array( - [ - [ - 1 - 2 * q2**2 - 2 * q3**2, - 2 * (q1 * q2 - q0 * q3), - 2 * (q0 * q2 + q1 * q3), - ], - [ - 2 * (q1 * q2 + q0 * q3), - 1 - 2 * q1**2 - 2 * q3**2, - 2 * (q2 * q3 - q0 * q1), - ], - [ - 2 * (q1 * q3 - q0 * q2), - 2 * (q0 * q1 + q2 * q3), - 1 - 2 * q1**2 - 2 * q2**2, - ], - ] - ) - - return R - - -def euler_to_quat(euler_angles: np.ndarray) -> np.ndarray: - """Converts Euler angles to a quaternion - """ - psi, theta, phi = euler_angles - c_psi = np.cos(psi / 2) - s_psi = np.sin(psi / 2) - c_theta = np.cos(theta / 2) - s_theta = np.sin(theta / 2) - c_phi = np.cos(phi / 2) - s_phi = np.sin(phi / 2) - - quat = np.array( - [ - c_psi * c_theta * c_phi + s_psi * s_theta * s_phi, - c_psi * c_theta * s_phi - s_psi * s_theta * c_phi, - s_psi * c_theta * s_phi + c_psi * s_theta * c_phi, - s_psi * c_theta * c_phi - c_psi * s_theta * s_phi, - ] - ) - - return quat - - -def quat_to_euler(quat: np.ndarray) -> np.ndarray: - """Converts a quaternion to Euler angles - """ - nu, eta_1, eta_2, eta_3 = quat - - phi = np.arctan2(2 * (eta_2 * eta_3 + nu * eta_1), 1 - 2 * (eta_1**2 + eta_2**2)) - theta = -np.arcsin(2 * (eta_1 * eta_3 - nu * eta_2)) - psi = np.arctan2(2 * (nu * eta_3 + eta_1 * eta_2), 1 - 2 * (eta_2**2 + eta_3**2)) - - return np.array([phi, theta, psi]) diff --git a/navigation/eskf_python/eskf_python/eskf_test.py b/navigation/eskf_python/eskf_python/eskf_test.py deleted file mode 100644 index 2c6d94c09..000000000 --- a/navigation/eskf_python/eskf_python/eskf_test.py +++ /dev/null @@ -1,358 +0,0 @@ -import matplotlib.pyplot as plt -import numpy as np -from eskf_python_class import Measurement, StateQuat -from eskf_python_filter import ESKF -from eskf_python_utils import quat_to_euler -from eskf_test_utils import StateQuatModel, process_model - - -def simulate_eskf(): - # Simulation parameters - simulation_time = 20.0 # seconds - dt = 0.01 - num_steps = int(simulation_time / dt) - time = np.linspace(0, simulation_time, num_steps) - - # ----------------------- Setup Initial States, Filter & Model ----------------------- - # True initial state - true_state_init = StateQuat() - true_state_init.position = np.array([0.1, 0.0, 0.0]) - true_state_init.velocity = np.array([0.1, 0.0, 0.0]) - P0 = np.diag( - [ - 0.3, - 0.3, - 0.3, # Position - 0.2, - 0.2, - 0.2, # Velocity - 0.2, - 0.2, - 0.2, # Orientation - 0.0001, - 0.0001, - 0.0001, # Acceleration bias - 0.00001, - 0.00001, - 0.00001, # Gyro bias - 0.00001, - 0.00001, - 0.00001, # Gravity - ] - ) - # Noise parameters - Q = np.diag( - [ - (0.13**2), - (0.13**2), - (0.13**2), # Adjusted Accelerometer noise - (0.13**2), - (0.13**2), - (0.13**2), # Adjusted Gyroscope noise - 0.0001, - 0.0001, - 0.0001, # Adjusted Acceleration bias random walk - 0.0001, - 0.0001, - 0.0001, # Adjusted Gyro bias random walk - ] - ) - - # Create filter object - eskf = ESKF(Q, P0, true_state_init, 1e-13, 1e-13, dt) - - # Create measurement objects - imu_data = Measurement() - dvl_data = Measurement() - - # R matrix for DVL aiding - dvl_data.aiding_covariance = np.diag( - [(0.01) ** 2, (0.01) ** 2, (0.01) ** 2] - ) # Adjusted DVL aiding covariance - - # Setup the process model for simulation of AUV - model = process_model() - model.dt = dt - model.mass_interia_matrix = np.array( - [ - [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], - ] - ) - model.m = 30.0 - model.r_b_bg = np.array([0.01, 0.0, 0.02]) - model.inertia = np.diag([0.68, 3.32, 3.34]) - model.damping_linear = np.diag([0.03, 0.03, 0.03, 0.03, 0.03, 0.03]) - - # Initialize a dummy state for simulation dynamics. - # Two where made since there seems to be an issue with declaring two identical objects. - new_state = StateQuatModel() - new_state.position = np.array([0.1, 0.0, 0.0]) - new_state.velocity = np.array([0.1, 0.0, 0.0]) - - new_state_prev = StateQuatModel() - new_state_prev.position = np.array([0.1, 0.0, 0.0]) - new_state_prev.velocity = np.array([0.1, 0.0, 0.0]) - - model.state_vector = new_state - model.state_vector_prev = new_state_prev - - # Initialize arrays to store true and estimated states - true_positions = np.zeros((num_steps, 3)) - true_orientations = np.zeros((num_steps, 3)) - true_velocities = np.zeros((num_steps, 3)) - - est_positions = np.zeros((num_steps, 3)) - est_orientations = np.zeros((num_steps, 3)) - est_velocities = np.zeros((num_steps, 3)) - - # covariance arrays - pos_cov = np.zeros((num_steps, 3)) - vel_cov = np.zeros((num_steps, 3)) - ori_cov = np.zeros((num_steps, 3)) - - prev_velocity = np.zeros(3) - u = lambda t: np.array( - [ - 0.5 * np.sin(0.1 * t), - 0.5 * np.sin(0.1 * t + 0.3), - 0.5 * np.sin(0.1 * t + 0.6), - 0.05 * np.cos(0.1 * t), - 0.05 * np.cos(0.1 * t + 0.3), - 0.05 * np.cos(0.1 * t + 0.6), - ] - ) - - NIS_list = [] - NIS_value = 0.0 - - # Sim - for step in range(num_steps): - t = step * dt - - model.Control_input = u(t) - model.model_prediction(new_state) - new_state = model.euler_forward() - - imu_data.acceleration = ( - (new_state.velocity - prev_velocity) / dt - ) + np.random.normal(0, 0.13, 3) - imu_data.angular_velocity = new_state.angular_velocity + np.random.normal( - 0, 0.13, 3 - ) - - eskf.imu_update(imu_data) - - if step % 200 == 0: - dvl_data.aiding = new_state.velocity + np.random.normal(0, 0.01, 3) - NIS_value = eskf.dvl_update(dvl_data) - NIS_list.append(NIS_value) - - true_positions[step, :] = np.copy(new_state.position) - true_orientations[step, :] = quat_to_euler(np.copy(new_state.orientation)) - true_velocities[step, :] = np.copy(new_state.velocity) - - est_positions[step, :] = np.copy(eskf.nom_state.position) - est_orientations[step, :] = quat_to_euler(np.copy(eskf.nom_state.orientation)) - est_velocities[step, :] = np.copy(eskf.nom_state.velocity) - - P_diag = np.diag(eskf.error_state.covariance) - pos_cov[step, :] = P_diag[0:3] - vel_cov[step, :] = P_diag[3:6] - ori_cov[step, :] = P_diag[6:9] - - prev_velocity = new_state.velocity - model.state_vector_prev = new_state - - return ( - time, - true_positions, - true_orientations, - true_velocities, - est_positions, - est_orientations, - est_velocities, - pos_cov, - vel_cov, - ori_cov, - NIS_list, - ) - - -( - time, - true_positions, - true_orientations, - true_velocities, - est_positions, - est_orientations, - est_velocities, - pos_cov, - vel_cov, - ori_cov, - _, -) = simulate_eskf() - -# Plotting -axis_labels_pos = ["X", "Y", "Z"] -axis_labels_vel = ["X", "Y", "Z"] -axis_labels_ori = ["Roll", "Pitch", "Yaw"] - -# Plot Position -fig_pos, axs_pos = plt.subplots(3, 1, figsize=(10, 12)) -fig_pos.suptitle("True Data vs Filter Estimates for Position") -for i in range(3): - ax_pos = axs_pos[i] - ax_pos.plot( - time, - true_positions[:, i], - label=f"True Pos {axis_labels_pos[i]}", - color=f"C{i}", - linestyle='-', - ) - ax_pos.plot( - time, - est_positions[:, i], - label=f"Est Pos {axis_labels_pos[i]}", - color=f"C{i}", - linestyle='--', - ) - sigma_pos = np.sqrt(pos_cov[:, i]) - ax_pos.fill_between( - time, - est_positions[:, i] - sigma_pos, - est_positions[:, i] + sigma_pos, - color=f"C{i}", - alpha=0.2, - ) - ax_pos.set_title(f"Position [{axis_labels_pos[i]}] [m]") - ax_pos.set_xlabel("Time [s]") - ax_pos.set_ylabel("Position") - ax_pos.grid(True) - ax_pos.legend() - -plt.tight_layout(rect=[0, 0, 1, 0.96]) -plt.show() - -# Plot Velocity -fig_vel, axs_vel = plt.subplots(3, 1, figsize=(10, 12)) -fig_vel.suptitle("True Data vs Filter Estimates for Velocity") -for i in range(3): - ax_vel = axs_vel[i] - ax_vel.plot( - time, - true_velocities[:, i], - label=f"True Vel {axis_labels_vel[i]}", - color=f"C{i}", - linestyle='-', - ) - ax_vel.plot( - time, - est_velocities[:, i], - label=f"Est Vel {axis_labels_vel[i]}", - color=f"C{i}", - linestyle='--', - ) - sigma_vel = np.sqrt(vel_cov[:, i]) - ax_vel.fill_between( - time, - est_velocities[:, i] - sigma_vel, - est_velocities[:, i] + sigma_vel, - color=f"C{i}", - alpha=0.2, - ) - ax_vel.set_title(f"Velocity [{axis_labels_vel[i]}] [m/s]") - ax_vel.set_xlabel("Time [s]") - ax_vel.set_ylabel("Velocity") - ax_vel.grid(True) - ax_vel.legend() - -plt.tight_layout(rect=[0, 0, 1, 0.96]) -plt.show() - -# Plot Orientation -fig_ori, axs_ori = plt.subplots(3, 1, figsize=(10, 12)) -fig_ori.suptitle("True Data vs Filter Estimates for Orientation") -for i in range(3): - ax_ori = axs_ori[i] - ax_ori.plot( - time, - true_orientations[:, i], - label=f"True Ori {axis_labels_ori[i]}", - color=f"C{i}", - linestyle='-', - ) - ax_ori.plot( - time, - est_orientations[:, i], - label=f"Est Ori {axis_labels_ori[i]}", - color=f"C{i}", - linestyle='--', - ) - sigma_ori = np.sqrt(ori_cov[:, i]) - ax_ori.fill_between( - time, - est_orientations[:, i] - sigma_ori, - est_orientations[:, i] + sigma_ori, - color=f"C{i}", - alpha=0.2, - ) - ax_ori.set_title(f"Orientation [{axis_labels_ori[i]}] [rad]") - ax_ori.set_xlabel("Time [s]") - ax_ori.set_ylabel("Orientation") - ax_ori.grid(True) - ax_ori.legend() - -plt.tight_layout(rect=[0, 0, 1, 0.96]) -plt.show() - - -### _______ NIS AND NEEDS _______ -""" -num_simulations = 10 -NIS_runs = [] - -for sim in range(num_simulations): - ( - time, - true_positions, - true_orientations, - true_velocities, - est_positions, - est_orientations, - est_velocities, - pos_cov, - vel_cov, - ori_cov, - NIS_list, - ) = simulate_eskf() - - NIS_runs.append(np.array(NIS_list)) - -NIS_runs = np.vstack(NIS_runs) -ANIS = np.mean(NIS_runs, axis=0) - -measurement_dimension = 3 - -chi2_lower = chi2.ppf(0.025, measurement_dimension) / num_simulations -chi2_upper = chi2.ppf(0.975, measurement_dimension) / num_simulations - -time_steps = np.arange(len(ANIS)) * 0.01 * 20 - -fig, ax = plt.subplots(figsize=(10, 6)) -ax.plot(time_steps, ANIS, label="ANIS", color="C0") -ax.axhline(chi2_lower, color="C1", linestyle="--", label="95% CI Lower") -ax.axhline(chi2_upper, color="C2", linestyle="--", label="95% CI Upper") -ax.set_title("Average Normalized Innovation Squared (ANIS)") -ax.set_xlabel("Time [s]") -ax.set_ylabel("ANIS") -ax.grid(True) -ax.legend() - -plt.tight_layout() -plt.show() -""" diff --git a/navigation/eskf_python/eskf_python/eskf_test_utils.py b/navigation/eskf_python/eskf_python/eskf_test_utils.py deleted file mode 100644 index a60e62ff0..000000000 --- a/navigation/eskf_python/eskf_python/eskf_test_utils.py +++ /dev/null @@ -1,243 +0,0 @@ -from dataclasses import dataclass, field - -import numpy as np -from eskf_python_utils import ( - euler_to_quat, - quat_norm, - quat_to_euler, - quaternion_error, - quaternion_product, - skew_matrix, -) - -# This was the original code from the ukf_okid.py file - - -@dataclass -class StateQuatModel: - """A class to represent the state to be estimated by the UKF. - """ - - position: np.ndarray = field(default_factory=lambda: np.zeros(3)) - orientation: np.ndarray = field(default_factory=lambda: np.array([1, 0, 0, 0])) - velocity: np.ndarray = field(default_factory=lambda: np.zeros(3)) - angular_velocity: np.ndarray = field(default_factory=lambda: np.zeros(3)) - covariance: np.ndarray = field(default_factory=lambda: np.zeros((12, 12))) - - def as_vector(self) -> np.ndarray: - """Returns the StateVector as a numpy array.""" - return np.concatenate( - [self.position, self.orientation, self.velocity, self.angular_velocity] - ) - - def nu(self) -> np.ndarray: - """Calculates the nu vector.""" - return np.concatenate([self.velocity, self.angular_velocity]) - - def R_q(self) -> np.ndarray: - """Calculates the rotation matrix from the orientation quaternion.""" - q0, q1, q2, q3 = self.orientation - R = np.array( - [ - [ - 1 - 2 * q2**2 - 2 * q3**2, - 2 * (q1 * q2 - q0 * q3), - 2 * (q0 * q2 + q1 * q3), - ], - [ - 2 * (q1 * q2 + q0 * q3), - 1 - 2 * q1**2 - 2 * q3**2, - 2 * (q2 * q3 - q0 * q1), - ], - [ - 2 * (q1 * q3 - q0 * q2), - 2 * (q0 * q1 + q2 * q3), - 1 - 2 * q1**2 - 2 * q2**2, - ], - ] - ) - return R - - def fill_states(self, state: np.ndarray) -> None: - """Fills the state vector with the values from a numpy array.""" - self.position = state[0:3] - self.orientation = state[3:7] - self.velocity = state[7:10] - self.angular_velocity = state[10:13] - - def fill_states_different_dim( - self, state: np.ndarray, state_euler: np.ndarray - ) -> None: - """Fills states when the state vector has different dimensions than the default state vector.""" - self.position = state[0:3] + state_euler[0:3] - self.orientation = quaternion_product( - state[3:7], euler_to_quat(state_euler[3:6]) - ) - self.velocity = state[7:10] + state_euler[6:9] - self.angular_velocity = state[10:13] + state_euler[9:12] - - def subtract(self, other: 'StateQuatModel') -> np.ndarray: - """Subtracts two StateQuatModel objects, returning the difference with Euler angles.""" - new_array = np.zeros(len(self.as_vector()) - 1) - new_array[:3] = self.position - other.position - new_array[3:6] = quat_to_euler( - quaternion_error(self.orientation, other.orientation) - ) - new_array[6:9] = self.velocity - other.velocity - new_array[9:12] = self.angular_velocity - other.angular_velocity - - return new_array - - def __add__(self, other: 'StateQuatModel') -> 'StateQuatModel': - """Adds two StateQuatModel objects.""" - new_state = StateQuatModel() - new_state.position = self.position + other.position - new_state.orientation = quaternion_product(self.orientation, other.orientation) - new_state.velocity = self.velocity + other.velocity - new_state.angular_velocity = self.angular_velocity + other.angular_velocity - - return new_state - - def __sub__(self, other: 'StateQuatModel') -> 'StateQuatModel': - """Subtracts two StateQuatModel objects.""" - new_state = StateQuatModel() - new_state.position = self.position - other.position - new_state.orientation = quaternion_error(self.orientation, other.orientation) - new_state.velocity = self.velocity - other.velocity - new_state.angular_velocity = self.angular_velocity - other.angular_velocity - - return new_state.as_vector() - - def __rmul__(self, scalar: float) -> 'StateQuatModel': - """Multiplies the StateQuatModel object by a scalar.""" - new_state = StateQuatModel() - new_state.position = scalar * self.position - new_state.orientation = quat_norm(scalar * self.orientation) - new_state.velocity = scalar * self.velocity - new_state.angular_velocity = scalar * self.angular_velocity - - return new_state - - def insert_weights(self, weights: np.ndarray) -> np.ndarray: - """Inserts the weights into the covariance matrix.""" - new_state = StateQuatModel() - new_state.position = self.position - weights[:3] - new_state.orientation = quaternion_error( - self.orientation, euler_to_quat(weights[3:6]) - ) - new_state.velocity = self.velocity - weights[6:9] - new_state.angular_velocity = self.angular_velocity - weights[9:12] - - return new_state.as_vector() - - def add_without_quaternions(self, other: 'StateQuatModel') -> None: - """Adds elements into the state vector without considering the quaternions.""" - self.position += other.position - self.velocity += other.velocity - self.angular_velocity += other.angular_velocity - - -@dataclass -class process_model: - """A class defined for a general process model. - """ - - state_vector: StateQuatModel = field(default_factory=StateQuatModel) - state_vector_dot: StateQuatModel = field(default_factory=StateQuatModel) - state_vector_prev: StateQuatModel = field(default_factory=StateQuatModel) - Control_input: np.ndarray = field(default_factory=lambda: np.zeros(6)) - mass_interia_matrix: np.ndarray = field(default_factory=lambda: np.zeros((6, 6))) - added_mass: np.ndarray = field(default_factory=lambda: np.zeros(6)) - damping_linear: np.ndarray = field(default_factory=lambda: np.zeros(6)) - damping_nonlinear: np.ndarray = field(default_factory=lambda: np.zeros(6)) - m: float = 0.0 - inertia: np.ndarray = field(default_factory=lambda: np.zeros((3, 3))) - r_b_bg: np.ndarray = field(default_factory=lambda: np.zeros(3)) - dt: float = 0.0 - integral_error_position: np.ndarray = field(default_factory=lambda: np.zeros(3)) - integral_error_orientation: np.ndarray = field(default_factory=lambda: np.zeros(4)) - prev_position_error: np.ndarray = field(default_factory=lambda: np.zeros(3)) - prev_orientation_error: np.ndarray = field(default_factory=lambda: np.zeros(3)) - - def R(self) -> np.ndarray: - """Calculates the rotation matrix.""" - nu, e_1, e_2, e_3 = self.state_vector.orientation - R = np.array( - [ - [ - 1 - 2 * e_2**2 - 2 * e_3**2, - 2 * e_1 * e_2 - 2 * nu * e_3, - 2 * e_1 * e_3 + 2 * nu * e_2, - ], - [ - 2 * e_1 * e_2 + 2 * nu * e_3, - 1 - 2 * e_1**2 - 2 * e_3**2, - 2 * e_2 * e_3 - 2 * nu * e_1, - ], - [ - 2 * e_1 * e_3 - 2 * nu * e_2, - 2 * e_2 * e_3 + 2 * nu * e_1, - 1 - 2 * e_1**2 - 2 * e_2**2, - ], - ] - ) - return R - - def T(self) -> np.ndarray: - """Calculates the transformation matrix.""" - nu, e_1, e_2, e_3 = self.state_vector.orientation - T = 0.5 * np.array( - [[-e_1, -e_2, -e_3], [nu, -e_3, e_2], [e_3, nu, -e_1], [-e_2, e_1, nu]] - ) - return T - - def Crb(self) -> np.ndarray: - """Calculates the Coriolis matrix.""" - ang_vel = self.state_vector.angular_velocity - ang_vel_skew = skew_matrix(ang_vel) - lever_arm_skew = skew_matrix(self.r_b_bg) - Crb = np.zeros((6, 6)) - Crb[0:3, 0:3] = self.m * ang_vel_skew - Crb[3:6, 3:6] = -skew_matrix(np.dot(self.inertia, ang_vel)) - Crb[0:3, 3:6] = -self.m * np.dot(ang_vel_skew, lever_arm_skew) - Crb[3:6, 0:3] = self.m * np.dot(lever_arm_skew, ang_vel_skew) - return Crb - - def D(self) -> np.ndarray: - """Calculates the damping matrix.""" - D_l = -np.diag(self.damping_linear) - D_nl = -np.diag(self.damping_nonlinear) * np.abs(self.state_vector.nu()) - return D_l + D_nl - - def model_prediction(self, state: StateQuatModel) -> None: - """Calculates the model of the system.""" - self.state_vector = state - self.state_vector_dot.position = np.dot(self.R(), self.state_vector.velocity) - self.state_vector_dot.orientation = np.dot( - self.T(), self.state_vector.angular_velocity - ) - Nu = np.linalg.inv(self.mass_interia_matrix + np.diag(self.added_mass)) @ ( - self.Control_input - - np.dot(self.Crb(), self.state_vector.nu()) - - np.dot(self.D(), self.state_vector.nu()) - ) - self.state_vector_dot.velocity = Nu[:3] - self.state_vector_dot.angular_velocity = Nu[3:] - - def euler_forward(self) -> StateQuatModel: - """Calculates the forward Euler integration.""" - self.state_vector.position = ( - self.state_vector_prev.position + self.state_vector_dot.position * self.dt - ) - self.state_vector.orientation = quat_norm( - self.state_vector_prev.orientation - + self.state_vector_dot.orientation * self.dt - ) - self.state_vector.velocity = ( - self.state_vector_prev.velocity + self.state_vector_dot.velocity * self.dt - ) - self.state_vector.angular_velocity = ( - self.state_vector_prev.angular_velocity - + self.state_vector_dot.angular_velocity * self.dt - ) - return self.state_vector diff --git a/navigation/eskf_python/launch/eskf.launch.py b/navigation/eskf_python/launch/eskf.launch.py deleted file mode 100644 index 3cae83dce..000000000 --- a/navigation/eskf_python/launch/eskf.launch.py +++ /dev/null @@ -1,22 +0,0 @@ -import os - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch_ros.actions import Node - - -def generate_launch_description(): - eskf_python_node = Node( - package='eskf_python', - executable='eskf_python_node.py', - name='eskf_python_node', - parameters=[ - os.path.join( - get_package_share_directory('eskf_python'), - 'config', - 'eskf_python.yaml', - ), - ], - output='screen', - ) - return LaunchDescription([eskf_python_node]) diff --git a/navigation/eskf_python/package.xml b/navigation/eskf_python/package.xml deleted file mode 100644 index 980653c40..000000000 --- a/navigation/eskf_python/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - eskf_python - 1.0.0 - This package provides the implementation of a error-state kalman filter in python - talhanc - MIT - - ament_cmake_python - - rclpy - python-transforms3d-pip - geometry_msgs - vortex_msgs - - python3-pytest - - - - ament_cmake - - diff --git a/navigation/ukf_okid/ukf_python/rest.py b/navigation/ukf_okid/ukf_python/rest.py index 52cffb2df..c8b4f3b14 100644 --- a/navigation/ukf_okid/ukf_python/rest.py +++ b/navigation/ukf_okid/ukf_python/rest.py @@ -1,6 +1,5 @@ def mean_set(set_points: list[StateQuat], weights: np.ndarray = None) -> np.ndarray: - """Function that calculates the mean of a set of points - """ + """Function that calculates the mean of a set of points""" n = len(set_points[0].as_vector()) - 1 mean_value = StateQuat() @@ -27,8 +26,7 @@ def mean_set(set_points: list[StateQuat], weights: np.ndarray = None) -> np.ndar def mean_measurement( set_points: list[MeasModel], weights: np.ndarray = None ) -> np.ndarray: - """Function that calculates the mean of a set of points - """ + """Function that calculates the mean of a set of points""" n = len(set_points) mean_value = MeasModel() diff --git a/navigation/ukf_okid/ukf_python/ukf_okid.py b/navigation/ukf_okid/ukf_python/ukf_okid.py index 80a7c939c..50c68e08c 100644 --- a/navigation/ukf_okid/ukf_python/ukf_okid.py +++ b/navigation/ukf_okid/ukf_python/ukf_okid.py @@ -1,4 +1,3 @@ - import numpy as np from ukf_okid_class import * @@ -39,8 +38,7 @@ def generate_T_matrix(self, n: float) -> np.ndarray: return T def sigma_points(self, current_state: StateQuat) -> list[StateQuat]: - """Functions that generate the sigma points for the UKF - """ + """Functions that generate the sigma points for the UKF""" n = len(current_state.covariance) I = np.hstack([np.eye(n), -np.eye(n)]) @@ -58,8 +56,7 @@ def sigma_points(self, current_state: StateQuat) -> list[StateQuat]: return self.sigma_points_list def unscented_transform(self, current_state: StateQuat) -> StateQuat: - """The unscented transform function generates the priori state estimate - """ + """The unscented transform function generates the priori state estimate""" _ = self.sigma_points(current_state) n = len(current_state.covariance) @@ -107,8 +104,7 @@ def posteriori_estimate( measurement: MeasModel, ex_measuremnt: MeasModel, ) -> StateQuat: - """Calculates the posteriori estimate using measurement and the prior estimate - """ + """Calculates the posteriori estimate using measurement and the prior estimate""" nu_k = MeasModel() nu_k.measurement = measurement.measurement - ex_measuremnt.measurement diff --git a/navigation/ukf_okid/ukf_python/ukf_okid_class.py b/navigation/ukf_okid/ukf_python/ukf_okid_class.py index 181d1f4af..45bbffcfe 100644 --- a/navigation/ukf_okid/ukf_python/ukf_okid_class.py +++ b/navigation/ukf_okid/ukf_python/ukf_okid_class.py @@ -5,8 +5,7 @@ @dataclass class StateQuat: - """A class to represent the state to be estimated by the UKF. - """ + """A class to represent the state to be estimated by the UKF.""" position: np.ndarray = field(default_factory=lambda: np.zeros(3)) orientation: np.ndarray = field(default_factory=lambda: np.array([1, 0, 0, 0])) @@ -129,8 +128,7 @@ def add_without_quaternions(self, other: 'StateQuat') -> None: @dataclass class MeasModel: - """A class defined for a general measurement model. - """ + """A class defined for a general measurement model.""" measurement: np.ndarray = field(default_factory=lambda: np.zeros(3)) covariance: np.ndarray = field(default_factory=lambda: np.zeros((3, 3))) @@ -164,8 +162,7 @@ def __sub__(self, other: 'MeasModel') -> 'MeasModel': @dataclass class process_model: - """A class defined for a general process model. - """ + """A class defined for a general process model.""" state_vector: StateQuat = field(default_factory=StateQuat) state_vector_dot: StateQuat = field(default_factory=StateQuat) @@ -269,8 +266,7 @@ def euler_forward(self) -> StateQuat: def euler_to_quat(euler_angles: np.ndarray) -> np.ndarray: - """Converts Euler angles to a quaternion - """ + """Converts Euler angles to a quaternion""" psi, theta, phi = euler_angles c_psi = np.cos(psi / 2) s_psi = np.sin(psi / 2) @@ -292,8 +288,7 @@ def euler_to_quat(euler_angles: np.ndarray) -> np.ndarray: def quat_to_euler(quat: np.ndarray) -> np.ndarray: - """Converts a quaternion to Euler angles - """ + """Converts a quaternion to Euler angles""" nu, eta_1, eta_2, eta_3 = quat phi = np.arctan2(2 * (eta_2 * eta_3 + nu * eta_1), 1 - 2 * (eta_1**2 + eta_2**2)) @@ -304,8 +299,7 @@ def quat_to_euler(quat: np.ndarray) -> np.ndarray: def quat_norm(quat: np.ndarray) -> np.ndarray: - """Function that normalizes a quaternion - """ + """Function that normalizes a quaternion""" quat = quat / np.linalg.norm(quat) return quat @@ -354,8 +348,7 @@ def quaternion_super_product(q1: np.ndarray, q2: np.ndarray) -> np.ndarray: def quaternion_error(quat_1: np.ndarray, quat_2: np.ndarray) -> np.ndarray: - """Calculates the error between two quaternions - """ + """Calculates the error between two quaternions""" quat_2_inv = np.array([quat_2[0], -quat_2[1], -quat_2[2], -quat_2[3]]) error_quat = quaternion_super_product(quat_1, quat_2_inv) @@ -441,8 +434,7 @@ def mean_set(set_points: list[StateQuat]) -> np.ndarray: def mean_measurement(set_points: list[MeasModel]) -> np.ndarray: - """Function that calculates the mean of a set of points - """ + """Function that calculates the mean of a set of points""" n = len(set_points) mean_value = MeasModel() @@ -455,8 +447,7 @@ def mean_measurement(set_points: list[MeasModel]) -> np.ndarray: def covariance_set(set_points: list[StateQuat], mean: np.ndarray) -> np.ndarray: - """Function that calculates the covariance of a set of points - """ + """Function that calculates the covariance of a set of points""" n = len(set_points) covariance = np.zeros(set_points[0].covariance.shape) @@ -486,8 +477,7 @@ def covariance_set(set_points: list[StateQuat], mean: np.ndarray) -> np.ndarray: def covariance_measurement(set_points: list[MeasModel], mean: np.ndarray) -> np.ndarray: - """Function that calculates the covariance of a set of points - """ + """Function that calculates the covariance of a set of points""" n = len(set_points) co_size = len(set_points[0].measurement) covariance = np.zeros((co_size, co_size)) @@ -510,8 +500,7 @@ def cross_covariance( set_z: list[MeasModel], mean_z: np.ndarray, ) -> np.ndarray: - """Calculates the cross covariance between the measurement and state prediction - """ + """Calculates the cross covariance between the measurement and state prediction""" n = len(set_y) cross_covariance = np.zeros((len(mean_y) - 1, len(mean_z))) diff --git a/navigation/ukf_okid/ukf_python/ukf_utils.py b/navigation/ukf_okid/ukf_python/ukf_utils.py index da56a7dfc..cb92d9393 100644 --- a/navigation/ukf_okid/ukf_python/ukf_utils.py +++ b/navigation/ukf_okid/ukf_python/ukf_utils.py @@ -1,4 +1,3 @@ - import numpy as np from ukf_okid_class import StateQuat @@ -6,8 +5,7 @@ def print_StateQuat_list( state_list: list[StateQuat], name="StateQuat List", print_covariance=True ): - """Custom print function to print a list of StateQuat objects in a formatted form. - """ + """Custom print function to print a list of StateQuat objects in a formatted form.""" print(f"{name}:") for i, state in enumerate(state_list): print(f"Index {i}:") @@ -15,8 +13,7 @@ def print_StateQuat_list( def print_StateQuat(state: StateQuat, name="StateQuat", print_covariance=True): - """Custom print function to print StateQuat objects in a formatted form. - """ + """Custom print function to print StateQuat objects in a formatted form.""" print(f"{name}:") print(f" Position: {state.position}") print(f" Orientation: {state.orientation}") @@ -28,8 +25,7 @@ def print_StateQuat(state: StateQuat, name="StateQuat", print_covariance=True): def print_matrix(matrix, name="Matrix"): - """Custom print function to print matrices in a formatted form. - """ + """Custom print function to print matrices in a formatted form.""" print(f"{name}: {matrix.shape}") if isinstance(matrix, np.ndarray): for row in matrix: From 32e2d66a240d4a026fe7d9911bb88277a50df41b Mon Sep 17 00:00:00 2001 From: Talha Nauman Choudhry Date: Sat, 5 Apr 2025 18:27:32 +0200 Subject: [PATCH 095/290] fix: added errorstate and nominalstate variables into the eskf class --- navigation/eskf/config/eskf_params.yaml | 3 +- navigation/eskf/include/eskf/eskf.hpp | 56 +++--- navigation/eskf/include/eskf/eskf_ros.hpp | 2 +- navigation/eskf/include/eskf/typedefs.hpp | 13 +- navigation/eskf/src/eskf.cpp | 202 ++++++++++------------ navigation/eskf/src/eskf_node.cpp | 2 +- navigation/eskf/src/eskf_ros.cpp | 40 ++--- navigation/eskf/src/eskf_utils.cpp | 5 +- 8 files changed, 149 insertions(+), 174 deletions(-) diff --git a/navigation/eskf/config/eskf_params.yaml b/navigation/eskf/config/eskf_params.yaml index f89b62f79..87d50ee44 100644 --- a/navigation/eskf/config/eskf_params.yaml +++ b/navigation/eskf/config/eskf_params.yaml @@ -3,4 +3,5 @@ eskf_node: imu_topic: imu/data_raw dvl_twist: /orca/twist odom_topic: odom - diag_Q_std: [0.0103, 0.0118, 0.0043, 0.00193, 0.00306, 0.00118, 0.000001, 0.000001, 0.000001, 0.000003, 0.000003, 0.000003] + diag_Q_std: [0.0103, 0.0118, 0.0043, 0.00193, 0.00306, 0.00118, 0.000001, 0.000001, 0.000001, 0.00001, 0.00001, 0.00001] + diag_p_init: [1.0, 1.0, 1.0, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001] diff --git a/navigation/eskf/include/eskf/eskf.hpp b/navigation/eskf/include/eskf/eskf.hpp index b30dc35b0..5cc3e0708 100644 --- a/navigation/eskf/include/eskf/eskf.hpp +++ b/navigation/eskf/include/eskf/eskf.hpp @@ -10,52 +10,41 @@ class ESKF { public: ESKF(const eskf_params& params); + // @brief Update the nominal state and error state + // @param imu_meas: IMU measurement + // @param dt: Time step + // @return Updated nominal state and error state std::pair imu_update( - const state_quat& nom_state, - const state_euler& error_state, const imu_measurement& imu_meas, const double dt); + // @brief Update the nominal state and error state + // @param dvl_meas: DVL measurement + // @return Updated nominal state and error state std::pair dvl_update( - const state_quat& nom_state, - const state_euler& error_state, const dvl_measurement& dvl_meas); private: // @brief Predict the nominal state - // @param nom_state: Nominal state // @param imu_meas: IMU measurement + // @param dt: Time step // @return Predicted nominal state - state_quat nominal_state_discrete(const state_quat& nom_state, - const imu_measurement& imu_meas, - const double dt); + void nominal_state_discrete(const imu_measurement& imu_meas, + const double dt); // @brief Predict the error state - // @param error_state: Error state - // @param nom_state: Nominal state // @param imu_meas: IMU measurement + // @param dt: Time step // @return Predicted error state - state_euler error_state_prediction(const state_euler& error_state, - const state_quat& nom_state, - const imu_measurement& imu_meas, - const double dt); + void error_state_prediction(const imu_measurement& imu_meas, + const double dt); // @brief Update the error state - // @param error_state: Error state // @param dvl_meas: DVL measurement - // @return Updated error state - state_euler measurement_update(const state_quat& nom_state, - const state_euler& error_state, - const dvl_measurement& dvl_meas); + void measurement_update(const dvl_measurement& dvl_meas); // @brief Inject the error state into the nominal state and reset the error - // state - // @param nom_state: Nominal state - // @param error_state: Error state - // @return Injected and reset state - std::pair injection_and_reset( - const state_quat& nom_state, - const state_euler& error_state); + void injection_and_reset(); // @brief Van Loan discretization // @param A_c: Continuous state transition matrix @@ -69,24 +58,31 @@ class ESKF { // @brief Calculate the delta quaternion matrix // @param nom_state: Nominal state // @return Delta quaternion matrix - Eigen::Matrix4x3d calculate_Q_delta(const state_quat& nom_state); + Eigen::Matrix4x3d calculate_q_delta(); // @brief Calculate the measurement matrix jakobian // @param nom_state: Nominal state // @return Measurement matrix - Eigen::Matrix3x19d calculate_Hx(const state_quat& nom_state); + Eigen::Matrix3x19d calculate_hx(); // @brief Calculate the full measurement matrix // @param nom_state: Nominal state // @return Measurement matrix - Eigen::Matrix3x18d calculate_H(const state_quat& nom_state); + Eigen::Matrix3x18d calculate_h_jacobian(); // @brief Calculate the measurement // @param nom_state: Nominal state // @return Measurement - Eigen::Vector3d calculate_h(const state_quat& nom_state); + Eigen::Vector3d calculate_h(); + // Process noise covariance matrix Eigen::Matrix12d Q_; + + // Member variable for the current error state + state_euler current_error_state_; + + // Member variable for the current nominal state + state_quat current_nom_state_; }; #endif // ESKF_HPP diff --git a/navigation/eskf/include/eskf/eskf_ros.hpp b/navigation/eskf/include/eskf/eskf_ros.hpp index 88f97459f..b5c0b1dab 100644 --- a/navigation/eskf/include/eskf/eskf_ros.hpp +++ b/navigation/eskf/include/eskf/eskf_ros.hpp @@ -13,7 +13,7 @@ #include #include "eskf/eskf.hpp" #include "eskf/typedefs.hpp" -#include "typedefs.hpp" +#include "spdlog/spdlog.h" class ESKFNode : public rclcpp::Node { public: diff --git a/navigation/eskf/include/eskf/typedefs.hpp b/navigation/eskf/include/eskf/typedefs.hpp index 925d8fd72..9be435753 100644 --- a/navigation/eskf/include/eskf/typedefs.hpp +++ b/navigation/eskf/include/eskf/typedefs.hpp @@ -27,6 +27,13 @@ typedef Eigen::Matrix Matrix6d; typedef Eigen::Matrix Matrix9d; } // namespace Eigen +template +Eigen::Matrix createDiagonalMatrix( + const std::vector& diag) { + return Eigen::Map>(diag.data()) + .asDiagonal(); +} + struct state_quat { Eigen::Vector3d pos = Eigen::Vector3d::Zero(); Eigen::Vector3d vel = Eigen::Vector3d::Zero(); @@ -51,8 +58,6 @@ struct state_quat { diff.accel_bias = accel_bias - other.accel_bias; return diff; } - - Eigen::Matrix3d get_R() const { return quat.toRotationMatrix(); } }; struct state_euler { @@ -91,8 +96,8 @@ struct imu_measurement { Eigen::Matrix3d R_nb; R_nb << 0, 0, -1, 0, -1, 0, -1, 0, 0; - accel = R_nb * accel_uncorrected; - gyro = R_nb * gyro_uncorrected; + accel = (R_nb * accel_uncorrected); + gyro = (R_nb * gyro_uncorrected); } }; diff --git a/navigation/eskf/src/eskf.cpp b/navigation/eskf/src/eskf.cpp index 58c532afa..06c9c44c8 100644 --- a/navigation/eskf/src/eskf.cpp +++ b/navigation/eskf/src/eskf.cpp @@ -30,27 +30,27 @@ std::pair ESKF::van_loan_discretization( return {A_d, GQG_d}; } -Eigen::Matrix4x3d ESKF::calculate_Q_delta(const state_quat& nom_state) { - Eigen::Matrix4x3d Q_delta_theta = Eigen::Matrix4x3d::Zero(); - double qw = nom_state.quat.w(); - double qx = nom_state.quat.x(); - double qy = nom_state.quat.y(); - double qz = nom_state.quat.z(); +Eigen::Matrix4x3d ESKF::calculate_q_delta() { + Eigen::Matrix4x3d q_delta_theta = Eigen::Matrix4x3d::Zero(); + double qw = current_nom_state_.quat.w(); + double qx = current_nom_state_.quat.x(); + double qy = current_nom_state_.quat.y(); + double qz = current_nom_state_.quat.z(); - Q_delta_theta << -qx, -qy, -qz, qw, -qz, qy, qz, qw, -qx, -qy, qx, qw; + q_delta_theta << -qx, -qy, -qz, qw, -qz, qy, qz, qw, -qx, -qy, qx, qw; - Q_delta_theta *= 0.5; - return Q_delta_theta; + q_delta_theta *= 0.5; + return q_delta_theta; } -Eigen::Matrix3x19d ESKF::calculate_Hx(const state_quat& nom_state) { +Eigen::Matrix3x19d ESKF::calculate_hx() { Eigen::Matrix3x19d Hx = Eigen::Matrix3x19d::Zero(); - Eigen::Quaterniond q = nom_state.quat.normalized(); + Eigen::Quaterniond q = current_nom_state_.quat.normalized(); Eigen::Matrix3d R_bn = q.toRotationMatrix(); - Eigen::Vector3d v_n = nom_state.vel; + Eigen::Vector3d v_n = current_nom_state_.vel; - Hx.block<3, 3>(0, 3) = R_bn.transpose(); + Hx.block<3, 3>(0, 3) = R_bn; Eigen::Matrix dR_dq; double qw = q.w(); @@ -58,80 +58,79 @@ Eigen::Matrix3x19d ESKF::calculate_Hx(const state_quat& nom_state) { double qy = q.y(); double qz = q.z(); + Eigen::Vector3d epsilon(qx, qy, qz); + + Eigen::Vector3d e_1(1, 0, 0); + Eigen::Vector3d e_2(0, 1, 0); + Eigen::Vector3d e_3(0, 0, 1); + dR_dq.col(0) = - 2 * Eigen::Vector3d(qw * v_n.x() + qz * v_n.y() - qy * v_n.z(), - -qz * v_n.x() + qw * v_n.y() + qx * v_n.z(), - qy * v_n.x() - qx * v_n.y() + qw * v_n.z()); + ((4 * qw * Eigen::Matrix3d::Identity()) + (2 * skew(epsilon))) * v_n; - dR_dq.col(1) = - 2 * Eigen::Vector3d(qx * v_n.x() + qy * v_n.y() + qz * v_n.z(), - qy * v_n.x() - qx * v_n.y() - qw * v_n.z(), - qz * v_n.x() + qw * v_n.y() - qx * v_n.z()); + dR_dq.col(1) = 2 * + ((e_1 * epsilon.transpose()) + (epsilon * e_1.transpose()) + + (qw * skew(e_1))) * + v_n; - dR_dq.col(2) = - 2 * Eigen::Vector3d(-qy * v_n.x() + qx * v_n.y() + qw * v_n.z(), - qx * v_n.x() + qy * v_n.y() + qz * v_n.z(), - -qw * v_n.x() + qz * v_n.y() - qy * v_n.z()); + dR_dq.col(2) = 2 * + ((e_2 * epsilon.transpose()) + (epsilon * e_2.transpose()) + + (qw * skew(e_2))) * + v_n; - dR_dq.col(3) = - 2 * Eigen::Vector3d(-qz * v_n.x() - qw * v_n.y() + qx * v_n.z(), - qw * v_n.x() - qz * v_n.y() + qy * v_n.z(), - qx * v_n.x() + qy * v_n.y() + qz * v_n.z()); + dR_dq.col(3) = 2 * + ((e_3 * epsilon.transpose()) + (epsilon * e_3.transpose()) + + (qw * skew(e_3))) * + v_n; Hx.block<3, 4>(0, 6) = dR_dq; return Hx; } -Eigen::Matrix3x18d ESKF::calculate_H(const state_quat& nom_state) { - Eigen::Matrix19x18d X_delta = Eigen::Matrix19x18d::Zero(); - X_delta.block<6, 6>(0, 0) = Eigen::Matrix6d::Identity(); - X_delta.block<4, 3>(6, 6) = calculate_Q_delta(nom_state); - X_delta.block<9, 9>(10, 9) = Eigen::Matrix9d::Identity(); +Eigen::Matrix3x18d ESKF::calculate_h_jacobian() { + Eigen::Matrix19x18d x_delta = Eigen::Matrix19x18d::Zero(); + x_delta.block<6, 6>(0, 0) = Eigen::Matrix6d::Identity(); + x_delta.block<4, 3>(6, 6) = calculate_q_delta(); + x_delta.block<9, 9>(10, 9) = Eigen::Matrix9d::Identity(); - Eigen::Matrix3x18d H = calculate_Hx(nom_state) * X_delta; + Eigen::Matrix3x18d H = calculate_hx() * x_delta; return H; } -Eigen::Matrix3x1d ESKF::calculate_h(const state_quat& nom_state) { +Eigen::Matrix3x1d ESKF::calculate_h() { Eigen::Matrix3x1d h; Eigen::Matrix3d R_bn = - nom_state.quat.normalized().toRotationMatrix().transpose(); + current_nom_state_.quat.normalized().toRotationMatrix(); - h = R_bn * nom_state.vel; + h = R_bn * current_nom_state_.vel; return h; } -state_quat ESKF::nominal_state_discrete(const state_quat& nom_state, - const imu_measurement& imu_meas, - const double dt) { +void ESKF::nominal_state_discrete(const imu_measurement& imu_meas, + const double dt) { Eigen::Vector3d acc = - nom_state.get_R() * (imu_meas.accel - nom_state.accel_bias) + - nom_state.gravity; - Eigen::Vector3d gyro = (imu_meas.gyro - nom_state.gyro_bias) * dt; - - state_quat next_nom_state; - - next_nom_state.pos = - nom_state.pos + nom_state.vel * dt + 0.5 * sq(dt) * acc; - next_nom_state.vel = nom_state.vel + dt * acc; - next_nom_state.quat = (nom_state.quat * vector3d_to_quaternion(gyro)); - next_nom_state.quat.normalize(); - next_nom_state.gyro_bias = nom_state.gyro_bias; - next_nom_state.accel_bias = nom_state.accel_bias; - next_nom_state.gravity = nom_state.gravity; - - return next_nom_state; + current_nom_state_.quat.normalized().toRotationMatrix() * + (imu_meas.accel - current_nom_state_.accel_bias) + + current_nom_state_.gravity; + Eigen::Vector3d gyro = (imu_meas.gyro - current_nom_state_.gyro_bias) * dt; + + current_nom_state_.pos = current_nom_state_.pos + + current_nom_state_.vel * dt + 0.5 * sq(dt) * acc; + current_nom_state_.vel = current_nom_state_.vel + dt * acc; + current_nom_state_.quat = + (current_nom_state_.quat * vector3d_to_quaternion(gyro)); + current_nom_state_.quat.normalize(); + current_nom_state_.gyro_bias = current_nom_state_.gyro_bias; + current_nom_state_.accel_bias = current_nom_state_.accel_bias; + current_nom_state_.gravity = current_nom_state_.gravity; } -state_euler ESKF::error_state_prediction(const state_euler& error_state, - const state_quat& nom_state, - const imu_measurement& imu_meas, - const double dt) { - Eigen::Matrix3d R = nom_state.get_R(); - Eigen::Vector3d acc = (imu_meas.accel - nom_state.accel_bias); - Eigen::Vector3d gyro = (imu_meas.gyro - nom_state.gyro_bias); +void ESKF::error_state_prediction(const imu_measurement& imu_meas, + const double dt) { + Eigen::Matrix3d R = current_nom_state_.quat.normalized().toRotationMatrix(); + Eigen::Vector3d acc = (imu_meas.accel - current_nom_state_.accel_bias); + Eigen::Vector3d gyro = (imu_meas.gyro - current_nom_state_.gyro_bias); Eigen::Matrix18d A_c = Eigen::Matrix18d::Zero(); A_c.block<3, 3>(0, 3) = Eigen::Matrix3d::Identity(); @@ -152,77 +151,60 @@ state_euler ESKF::error_state_prediction(const state_euler& error_state, auto [A_d, GQG_d] = van_loan_discretization(A_c, G_c, dt); state_euler next_error_state; - next_error_state.covariance = - A_d * error_state.covariance * A_d.transpose() + GQG_d; - - return next_error_state; + current_error_state_.covariance = + A_d * current_error_state_.covariance * A_d.transpose() + GQG_d; } -state_euler ESKF::measurement_update(const state_quat& nom_state, - const state_euler& error_state, - const dvl_measurement& dvl_meas) { - state_euler new_error_state; - - Eigen::Matrix3x18d H = calculate_H(nom_state); - Eigen::Matrix18d P = error_state.covariance; +void ESKF::measurement_update(const dvl_measurement& dvl_meas) { + Eigen::Matrix3x18d H = calculate_h_jacobian(); + Eigen::Matrix18d P = current_error_state_.covariance; Eigen::Matrix3d R = dvl_meas.cov; Eigen::Matrix3d S = H * P * H.transpose() + R; Eigen::Matrix18x3d K = P * H.transpose() * S.inverse(); - Eigen::Vector3d innovation = dvl_meas.vel - calculate_h(nom_state); - new_error_state.set_from_vector(K * innovation); + Eigen::Vector3d innovation = dvl_meas.vel - calculate_h(); + current_error_state_.set_from_vector(K * innovation); Eigen::Matrix18d I_KH = Eigen::Matrix18d::Identity() - K * H; - new_error_state.covariance = + current_error_state_.covariance = I_KH * P * I_KH.transpose() + K * R * K.transpose(); // Used joseph form for more stable calculations - - return new_error_state; } -std::pair ESKF::injection_and_reset( - const state_quat& nom_state, - const state_euler& error_state) { - state_quat next_nom_state; - - next_nom_state.pos = nom_state.pos + error_state.pos; - next_nom_state.vel = nom_state.vel + error_state.vel; - next_nom_state.quat = - nom_state.quat * vector3d_to_quaternion(error_state.euler); - next_nom_state.quat.normalize(); - next_nom_state.gyro_bias = nom_state.gyro_bias + error_state.gyro_bias; - next_nom_state.accel_bias = nom_state.accel_bias + error_state.accel_bias; - next_nom_state.gravity = nom_state.gravity + error_state.gravity; - - state_euler new_error_state; +void ESKF::injection_and_reset() { + current_nom_state_.pos = current_nom_state_.pos + current_error_state_.pos; + current_nom_state_.vel = current_nom_state_.vel + current_error_state_.vel; + current_nom_state_.quat = + current_nom_state_.quat * + vector3d_to_quaternion(current_error_state_.euler); + current_nom_state_.quat.normalize(); + current_nom_state_.gyro_bias = + current_nom_state_.gyro_bias + current_error_state_.gyro_bias; + current_nom_state_.accel_bias = + current_nom_state_.accel_bias + current_error_state_.accel_bias; + current_nom_state_.gravity = + current_nom_state_.gravity + current_error_state_.gravity; Eigen::Matrix18d G = Eigen::Matrix18d::Identity(); - new_error_state.covariance = G * error_state.covariance * G.transpose(); - - return {next_nom_state, new_error_state}; + current_error_state_.covariance = + G * current_error_state_.covariance * G.transpose(); + current_error_state_.set_from_vector(Eigen::Vector18d::Zero()); } std::pair ESKF::imu_update( - const state_quat& nom_state, - const state_euler& error_state, const imu_measurement& imu_meas, const double dt) { - state_quat next_nom_state = nominal_state_discrete(nom_state, imu_meas, dt); - state_euler next_error_state = - error_state_prediction(error_state, next_nom_state, imu_meas, dt); + nominal_state_discrete(imu_meas, dt); + error_state_prediction(imu_meas, dt); - return {next_nom_state, next_error_state}; + return {current_nom_state_, current_error_state_}; } std::pair ESKF::dvl_update( - const state_quat& nom_state, - const state_euler& error_state, const dvl_measurement& dvl_meas) { - state_euler new_error_state = - measurement_update(nom_state, error_state, dvl_meas); - auto [updated_nom_state, updated_error_state] = - injection_and_reset(nom_state, new_error_state); + measurement_update(dvl_meas); + injection_and_reset(); - return {updated_nom_state, updated_error_state}; + return {current_nom_state_, current_error_state_}; } diff --git a/navigation/eskf/src/eskf_node.cpp b/navigation/eskf/src/eskf_node.cpp index e90cebde3..196fa7916 100644 --- a/navigation/eskf/src/eskf_node.cpp +++ b/navigation/eskf/src/eskf_node.cpp @@ -2,7 +2,7 @@ int main(int argc, char** argv) { rclcpp::init(argc, argv); - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Started ESKF Node"); + spdlog::info("Starting ESKF Node"); rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp index d679d600e..06bb047b9 100644 --- a/navigation/eskf/src/eskf_ros.cpp +++ b/navigation/eskf/src/eskf_ros.cpp @@ -20,20 +20,20 @@ void ESKFNode::set_subscribers_and_publisher() { auto qos_sensor_data = rclcpp::QoS( rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); - this->declare_parameter("imu_topic", "imu/data_raw"); + this->declare_parameter("imu_topic"); std::string imu_topic = this->get_parameter("imu_topic").as_string(); imu_sub_ = this->create_subscription( imu_topic, qos_sensor_data, std::bind(&ESKFNode::imu_callback, this, std::placeholders::_1)); - this->declare_parameter("dvl_topic", "/orca/twist"); + this->declare_parameter("dvl_topic"); std::string dvl_topic = this->get_parameter("dvl_topic").as_string(); dvl_sub_ = this->create_subscription< geometry_msgs::msg::TwistWithCovarianceStamped>( dvl_topic, qos_sensor_data, std::bind(&ESKFNode::dvl_callback, this, std::placeholders::_1)); - this->declare_parameter("odom_topic", "odom"); + this->declare_parameter("odom_topic"); std::string odom_topic = this->get_parameter("odom_topic").as_string(); odom_pub_ = this->create_publisher( odom_topic, qos_sensor_data); @@ -41,34 +41,24 @@ void ESKFNode::set_subscribers_and_publisher() { void ESKFNode::set_parameters() { std::vector diag_Q_std; - this->declare_parameter>( - "diag_Q_std"); // gyroscope bias noise + this->declare_parameter>("diag_Q_std"); diag_Q_std = this->get_parameter("diag_Q_std").as_double_array(); Eigen::Matrix12d Q; Q.setZero(); spdlog::info("Q diagonal: {}", diag_Q_std[0]); - Q.diagonal() << sq(diag_Q_std[0]), sq(diag_Q_std[1]), - sq(diag_Q_std[2]), // acceleration noise - sq(diag_Q_std[3]), sq(diag_Q_std[4]), - sq(diag_Q_std[5]), // gyroscope noise - sq(diag_Q_std[6]), sq(diag_Q_std[7]), - sq(diag_Q_std[8]), // acceleration bias noise - sq(diag_Q_std[9]), sq(diag_Q_std[10]), - sq(diag_Q_std[11]); // gyroscope bias noise + Q.diagonal() << sq(diag_Q_std[0]), sq(diag_Q_std[1]), sq(diag_Q_std[2]), + sq(diag_Q_std[3]), sq(diag_Q_std[4]), sq(diag_Q_std[5]), + sq(diag_Q_std[6]), sq(diag_Q_std[7]), sq(diag_Q_std[8]), + sq(diag_Q_std[9]), sq(diag_Q_std[10]), sq(diag_Q_std[11]); eskf_params_.Q = Q; eskf_ = std::make_unique(eskf_params_); - Eigen::Matrix18d P; - P.setZero(); - P.diagonal() << 1.0, 1.0, 1.0, // position - 0.1, 0.1, 0.1, // velocity - 0.1, 0.1, 0.1, // euler angles - 0.001, 0.001, 0.001, // accel bias - 0.001, 0.001, 0.001, // gyro bias - 0.001, 0.001, 0.001; // gravity + std::vector diag_p_init = + this->declare_parameter>("diag_p_init"); + Eigen::Matrix18d P = createDiagonalMatrix<18>(diag_p_init); error_state_.covariance = P; } @@ -91,8 +81,7 @@ void ESKFNode::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { msg->angular_velocity.y, msg->angular_velocity.z; imu_meas_.correct(); - std::tie(nom_state_, error_state_) = - eskf_->imu_update(nom_state_, error_state_, imu_meas_, dt); + std::tie(nom_state_, error_state_) = eskf_->imu_update(imu_meas_, dt); } void ESKFNode::dvl_callback( @@ -105,8 +94,7 @@ void ESKFNode::dvl_callback( msg->twist.covariance[12], msg->twist.covariance[13], msg->twist.covariance[14]; - std::tie(nom_state_, error_state_) = - eskf_->dvl_update(nom_state_, error_state_, dvl_meas_); + std::tie(nom_state_, error_state_) = eskf_->dvl_update(dvl_meas_); } void ESKFNode::publish_odom() { @@ -125,6 +113,6 @@ void ESKFNode::publish_odom() { odom_msg.twist.twist.linear.y = nom_state_.vel.y(); odom_msg.twist.twist.linear.z = nom_state_.vel.z(); - odom_msg.header.stamp = this->now(); // Add timestamp to the message + odom_msg.header.stamp = this->now(); odom_pub_->publish(odom_msg); } diff --git a/navigation/eskf/src/eskf_utils.cpp b/navigation/eskf/src/eskf_utils.cpp index 930167eaa..133589d05 100644 --- a/navigation/eskf/src/eskf_utils.cpp +++ b/navigation/eskf/src/eskf_utils.cpp @@ -18,7 +18,9 @@ Eigen::Quaterniond vector3d_to_quaternion(const Eigen::Vector3d& vector) { return Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0); } else { Eigen::Vector3d axis = vector / angle; - return Eigen::Quaterniond(Eigen::AngleAxisd(angle, axis)); + Eigen::Quaterniond quat = + Eigen::Quaterniond(Eigen::AngleAxisd(angle, axis)); + return quat; } } @@ -27,5 +29,6 @@ Eigen::Quaterniond euler_to_quaternion(const Eigen::Vector3d& euler) { q = Eigen::AngleAxisd(euler.z(), Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(euler.y(), Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(euler.x(), Eigen::Vector3d::UnitX()); + q.normalize(); return q; } From ed916674800f2c30d5fe44e2e575626949312e78 Mon Sep 17 00:00:00 2001 From: Talha Nauman Choudhry Date: Thu, 17 Apr 2025 18:27:14 +0200 Subject: [PATCH 096/290] feat: modified imu correction and added in tested imu noise --- navigation/eskf/config/eskf_params.yaml | 7 +- navigation/eskf/include/eskf/eskf.hpp | 1 + navigation/eskf/include/eskf/eskf_ros.hpp | 2 + navigation/eskf/include/eskf/typedefs.hpp | 16 +- navigation/eskf/src/eskf.cpp | 21 +- navigation/eskf/src/eskf_ros.cpp | 21 +- navigation/eskf/src/eskf_utils.cpp | 2 +- navigation/ukf_okid/CMakeLists.txt | 23 + navigation/ukf_okid/launch/ukf.launch.py | 16 + navigation/ukf_okid/package.xml | 22 + .../ukf_python/{__ini__.py => __init__.py} | 0 navigation/ukf_okid/ukf_python/rest.py | 40 - navigation/ukf_okid/ukf_python/ukf_okid.py | 100 +-- .../ukf_okid/ukf_python/ukf_okid_class.py | 216 +++++- navigation/ukf_okid/ukf_python/ukf_ros.py | 117 +++ navigation/ukf_okid/ukf_python/ukf_test.py | 721 ++++++++++-------- navigation/ukf_okid/ukf_python/ukf_test_2.py | 40 + navigation/ukf_okid/ukf_python/ukf_utils.py | 1 + 18 files changed, 926 insertions(+), 440 deletions(-) create mode 100644 navigation/ukf_okid/CMakeLists.txt create mode 100644 navigation/ukf_okid/launch/ukf.launch.py create mode 100644 navigation/ukf_okid/package.xml rename navigation/ukf_okid/ukf_python/{__ini__.py => __init__.py} (100%) delete mode 100644 navigation/ukf_okid/ukf_python/rest.py create mode 100755 navigation/ukf_okid/ukf_python/ukf_ros.py create mode 100644 navigation/ukf_okid/ukf_python/ukf_test_2.py diff --git a/navigation/eskf/config/eskf_params.yaml b/navigation/eskf/config/eskf_params.yaml index 87d50ee44..639af1fc9 100644 --- a/navigation/eskf/config/eskf_params.yaml +++ b/navigation/eskf/config/eskf_params.yaml @@ -1,7 +1,8 @@ eskf_node: ros__parameters: imu_topic: imu/data_raw - dvl_twist: /orca/twist + dvl_topic: /orca/twist odom_topic: odom - diag_Q_std: [0.0103, 0.0118, 0.0043, 0.00193, 0.00306, 0.00118, 0.000001, 0.000001, 0.000001, 0.00001, 0.00001, 0.00001] - diag_p_init: [1.0, 1.0, 1.0, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001] + diag_Q_std: [0.027293, 0.028089, 0.029067, 0.00255253, 0.00270035, 0.00280294, 0.000001, 0.000001, 0.000001, 0.00001, 0.00001, 0.00001] + diag_p_init: [1.0, 1.0, 0.5, 0.5, 0.5, 1.0, 0.1, 0.1, 0.1, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001] + imu_frame: [0, 0, -1, 0, -1, 0, -1, 0, 0 ] diff --git a/navigation/eskf/include/eskf/eskf.hpp b/navigation/eskf/include/eskf/eskf.hpp index 5cc3e0708..80b086d26 100644 --- a/navigation/eskf/include/eskf/eskf.hpp +++ b/navigation/eskf/include/eskf/eskf.hpp @@ -25,6 +25,7 @@ class ESKF { const dvl_measurement& dvl_meas); private: + // @brief Predict the nominal state // @param imu_meas: IMU measurement // @param dt: Time step diff --git a/navigation/eskf/include/eskf/eskf_ros.hpp b/navigation/eskf/include/eskf/eskf_ros.hpp index b5c0b1dab..e0b777c86 100644 --- a/navigation/eskf/include/eskf/eskf_ros.hpp +++ b/navigation/eskf/include/eskf/eskf_ros.hpp @@ -64,6 +64,8 @@ class ESKFNode : public rclcpp::Node { rclcpp::Time last_imu_time_; bool first_imu_msg_received_ = false; + + Eigen::Matrix3d R_imu_eskf_; }; #endif // ESKF_ROS_HPP diff --git a/navigation/eskf/include/eskf/typedefs.hpp b/navigation/eskf/include/eskf/typedefs.hpp index 9be435753..fac89baa7 100644 --- a/navigation/eskf/include/eskf/typedefs.hpp +++ b/navigation/eskf/include/eskf/typedefs.hpp @@ -7,6 +7,7 @@ #include #include +#include namespace Eigen { typedef Eigen::Matrix Vector19d; @@ -40,7 +41,11 @@ struct state_quat { Eigen::Quaterniond quat = Eigen::Quaterniond::Identity(); Eigen::Vector3d gyro_bias = Eigen::Vector3d::Zero(); Eigen::Vector3d accel_bias = Eigen::Vector3d::Zero(); - Eigen::Vector3d gravity = Eigen::Vector3d(0, 0, 9.81); + Eigen::Vector3d gravity = Eigen::Vector3d::Zero(); + + state_quat() { + gravity << 0, 0, 9.81; + } Eigen::Vector19d as_vector() const { Eigen::Vector19d vec; @@ -89,16 +94,7 @@ struct state_euler { struct imu_measurement { Eigen::Vector3d accel = Eigen::Vector3d::Zero(); Eigen::Vector3d gyro = Eigen::Vector3d::Zero(); - Eigen::Vector3d accel_uncorrected = Eigen::Vector3d::Zero(); - Eigen::Vector3d gyro_uncorrected = Eigen::Vector3d::Zero(); - void correct() { - Eigen::Matrix3d R_nb; - R_nb << 0, 0, -1, 0, -1, 0, -1, 0, 0; - - accel = (R_nb * accel_uncorrected); - gyro = (R_nb * gyro_uncorrected); - } }; struct dvl_measurement { diff --git a/navigation/eskf/src/eskf.cpp b/navigation/eskf/src/eskf.cpp index 06c9c44c8..bbd367c7f 100644 --- a/navigation/eskf/src/eskf.cpp +++ b/navigation/eskf/src/eskf.cpp @@ -107,20 +107,24 @@ Eigen::Matrix3x1d ESKF::calculate_h() { return h; } + void ESKF::nominal_state_discrete(const imu_measurement& imu_meas, const double dt) { Eigen::Vector3d acc = - current_nom_state_.quat.normalized().toRotationMatrix() * - (imu_meas.accel - current_nom_state_.accel_bias) + + current_nom_state_.quat.normalized().toRotationMatrix() * imu_meas.accel + current_nom_state_.gravity; - Eigen::Vector3d gyro = (imu_meas.gyro - current_nom_state_.gyro_bias) * dt; + Eigen::Vector3d gyro = imu_meas.gyro * dt/2; current_nom_state_.pos = current_nom_state_.pos + current_nom_state_.vel * dt + 0.5 * sq(dt) * acc; current_nom_state_.vel = current_nom_state_.vel + dt * acc; - current_nom_state_.quat = - (current_nom_state_.quat * vector3d_to_quaternion(gyro)); + + + current_nom_state_.quat = (current_nom_state_.quat * vector3d_to_quaternion(gyro)); + + current_nom_state_.quat.normalize(); + current_nom_state_.gyro_bias = current_nom_state_.gyro_bias; current_nom_state_.accel_bias = current_nom_state_.accel_bias; current_nom_state_.gravity = current_nom_state_.gravity; @@ -129,8 +133,8 @@ void ESKF::nominal_state_discrete(const imu_measurement& imu_meas, void ESKF::error_state_prediction(const imu_measurement& imu_meas, const double dt) { Eigen::Matrix3d R = current_nom_state_.quat.normalized().toRotationMatrix(); - Eigen::Vector3d acc = (imu_meas.accel - current_nom_state_.accel_bias); - Eigen::Vector3d gyro = (imu_meas.gyro - current_nom_state_.gyro_bias); + Eigen::Vector3d acc = imu_meas.accel; + Eigen::Vector3d gyro = imu_meas.gyro; Eigen::Matrix18d A_c = Eigen::Matrix18d::Zero(); A_c.block<3, 3>(0, 3) = Eigen::Matrix3d::Identity(); @@ -148,7 +152,8 @@ void ESKF::error_state_prediction(const imu_measurement& imu_meas, G_c.block<3, 3>(9, 6) = Eigen::Matrix3d::Identity(); G_c.block<3, 3>(12, 9) = Eigen::Matrix3d::Identity(); - auto [A_d, GQG_d] = van_loan_discretization(A_c, G_c, dt); + Eigen::Matrix18d A_d, GQG_d; + std::tie(A_d, GQG_d) = van_loan_discretization(A_c, G_c, dt); state_euler next_error_state; current_error_state_.covariance = diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp index 06bb047b9..a35273103 100644 --- a/navigation/eskf/src/eskf_ros.cpp +++ b/navigation/eskf/src/eskf_ros.cpp @@ -40,6 +40,11 @@ void ESKFNode::set_subscribers_and_publisher() { } void ESKFNode::set_parameters() { + std::vector R_imu_correction; + this->declare_parameter>("imu_frame"); + R_imu_correction = get_parameter("imu_rotation_matrix").as_double_array(); + R_imu_eskf_ = Eigen::Map>(R_imu_correction.data()); + std::vector diag_Q_std; this->declare_parameter>("diag_Q_std"); @@ -75,11 +80,17 @@ void ESKFNode::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { double dt = (current_time - last_imu_time_).nanoseconds() * 1e-9; last_imu_time_ = current_time; - imu_meas_.accel_uncorrected << msg->linear_acceleration.x, - msg->linear_acceleration.y, msg->linear_acceleration.z; - imu_meas_.gyro_uncorrected << msg->angular_velocity.x, - msg->angular_velocity.y, msg->angular_velocity.z; - imu_meas_.correct(); + Eigen::Vector3d raw_accel(msg->linear_acceleration.x, + msg->linear_acceleration.y, + msg->linear_acceleration.z); + + imu_meas_.accel = R_imu_eskf_ * raw_accel; + + Eigen::Vector3d raw_gyro(msg->angular_velocity.x, + msg->angular_velocity.y, + msg->angular_velocity.z); + + imu_meas_.gyro = R_imu_eskf_ * raw_gyro; std::tie(nom_state_, error_state_) = eskf_->imu_update(imu_meas_, dt); } diff --git a/navigation/eskf/src/eskf_utils.cpp b/navigation/eskf/src/eskf_utils.cpp index 133589d05..a07f3acda 100644 --- a/navigation/eskf/src/eskf_utils.cpp +++ b/navigation/eskf/src/eskf_utils.cpp @@ -20,7 +20,7 @@ Eigen::Quaterniond vector3d_to_quaternion(const Eigen::Vector3d& vector) { Eigen::Vector3d axis = vector / angle; Eigen::Quaterniond quat = Eigen::Quaterniond(Eigen::AngleAxisd(angle, axis)); - return quat; + return quat.normalized(); } } diff --git a/navigation/ukf_okid/CMakeLists.txt b/navigation/ukf_okid/CMakeLists.txt new file mode 100644 index 000000000..901ca044b --- /dev/null +++ b/navigation/ukf_okid/CMakeLists.txt @@ -0,0 +1,23 @@ +cmake_minimum_required(VERSION 3.8) +project(ukf_python) + +find_package(ament_cmake_python REQUIRED) +find_package(rclpy REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(vortex_msgs REQUIRED) + +install(DIRECTORY + launch + config + DESTINATION share/${PROJECT_NAME} +) + +ament_python_install_package(${PROJECT_NAME}) + +install(PROGRAMS + ukf_python/ukf_ros.py + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() diff --git a/navigation/ukf_okid/launch/ukf.launch.py b/navigation/ukf_okid/launch/ukf.launch.py new file mode 100644 index 000000000..62a439b94 --- /dev/null +++ b/navigation/ukf_okid/launch/ukf.launch.py @@ -0,0 +1,16 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description() -> LaunchDescription: + + ukf_node = Node( + package="ukf_python", + executable="ukf_ros.py", + name="ukf_node", + ) + + return LaunchDescription([ukf_node]) diff --git a/navigation/ukf_okid/package.xml b/navigation/ukf_okid/package.xml new file mode 100644 index 000000000..1c1b2b4cb --- /dev/null +++ b/navigation/ukf_okid/package.xml @@ -0,0 +1,22 @@ + + + + ukf_python + 1.0.0 + Uscented Kalman filter for AUV model + talha + MIT + + ament_cmake_python + + rclpy + geometry_msgs + nav_msgs + vortex_msgs + python-control-pip + std_msgs + + + ament_cmake + + diff --git a/navigation/ukf_okid/ukf_python/__ini__.py b/navigation/ukf_okid/ukf_python/__init__.py similarity index 100% rename from navigation/ukf_okid/ukf_python/__ini__.py rename to navigation/ukf_okid/ukf_python/__init__.py diff --git a/navigation/ukf_okid/ukf_python/rest.py b/navigation/ukf_okid/ukf_python/rest.py deleted file mode 100644 index c8b4f3b14..000000000 --- a/navigation/ukf_okid/ukf_python/rest.py +++ /dev/null @@ -1,40 +0,0 @@ -def mean_set(set_points: list[StateQuat], weights: np.ndarray = None) -> np.ndarray: - """Function that calculates the mean of a set of points""" - n = len(set_points[0].as_vector()) - 1 - mean_value = StateQuat() - - if weights is None: - for i in range(2 * n + 1): - weight_temp_list = (1 / (2 * n + 1)) * np.ones(2 * n + 1) - mean_value.add_without_quaternions(weight_temp_list[i] * set_points[i]) - - mean_value.orientation = iterative_quaternion_mean_statequat( - set_points, weight_temp_list - ) - - else: - for i in range(2 * n + 1): - mean_value.add_without_quaternions(weights[i] * set_points[i]) - - mean_value.orientation = iterative_quaternion_mean_statequat( - set_points, weights - ) - - return mean_value.as_vector() - - -def mean_measurement( - set_points: list[MeasModel], weights: np.ndarray = None -) -> np.ndarray: - """Function that calculates the mean of a set of points""" - n = len(set_points) - mean_value = MeasModel() - - if weights is None: - for i in range(n): - mean_value = mean_value + set_points[i] - else: - for i in range(n): - mean_value = mean_value + (weights[i] * set_points[i]) - - return mean_value.measurement diff --git a/navigation/ukf_okid/ukf_python/ukf_okid.py b/navigation/ukf_okid/ukf_python/ukf_okid.py index 50c68e08c..af61d1a79 100644 --- a/navigation/ukf_okid/ukf_python/ukf_okid.py +++ b/navigation/ukf_okid/ukf_python/ukf_okid.py @@ -1,69 +1,77 @@ import numpy as np -from ukf_okid_class import * +from ukf_utils import print_StateQuat +from ukf_okid_class import ( + MeasModel, + StateQuat, + covariance_measurement, + covariance_set, + cross_covariance, + mean_measurement, + mean_set, + process_model, + okid_process_model, +) class UKF: - def __init__(self, process_model: process_model, x_0, P_0, Q, R): + def __init__(self, process_model: okid_process_model, x_0, P_0, Q, G): self.x = x_0 self.P = P_0 self.Q = Q - self.R = R + self.G = G self.process_model = process_model self.sigma_points_list = None + self.measurement_updated = MeasModel() self.y_i = None self.weight = None - self.T = self.generate_T_matrix(len(P_0)) + self.delta = self.generate_delta_matrix(len(x_0.as_vector()) - 1) + self.cross_correlation = None - def generate_T_matrix(self, n: float) -> np.ndarray: - """Generates the orthonormal transformation matrix T used in the TUKF sigma point generation. + def generate_delta_matrix(self, n: float) -> np.ndarray: + """Generates the weight matrix used in the TUKF sigma point generation. Parameters: n (int): The state dimension. Returns: - T (np.ndarray): An n x 2n orthonormal transformation matrix used to generate TUKF sigma points. + delta (np.ndarray): An n x 2n orthonormal transformation matrix used to generate TUKF sigma points. """ - T = np.zeros((n, n)) + delta = np.zeros((n, 2 * n)) + k = 0.01 #Tuning parameter to ensure pos def - for i in range(n): + for i in range(2 * n): for j in range(n // 2): - T[2 * j - 2, i - 1] = np.sqrt(2) * np.cos(((2 * j - 1) * i * np.pi) / n) - T[2 * j - 1, i - 1] = np.sqrt(2) * np.sin(((2 * j - 1) * i * np.pi) / n) - - if n % 2 == 1: # if n is odd - T[n - 1, i - 1] = (-1) ** i - - T = T / np.sqrt(2) - - return T + delta[2 * j + 1, i] = np.sqrt(2) * np.sin((2 * j - 1)) * ((k * np.pi) / n) + delta[2 * j, i] = np.sqrt(2) * np.cos((2 * j - 1)) * ((k * np.pi) / n) + + if (n % 2) == 1: + delta[n-1, i] = (-1)**i + return delta def sigma_points(self, current_state: StateQuat) -> list[StateQuat]: - """Functions that generate the sigma points for the UKF""" + """Functions that generate the sigma points for the UKF.""" n = len(current_state.covariance) - I = np.hstack([np.eye(n), -np.eye(n)]) - my = np.sqrt(n) * I - delta = self.T @ my - S = np.linalg.cholesky(current_state.covariance + self.Q) self.sigma_points_list = [StateQuat() for _ in range(2 * n)] for index, state in enumerate(self.sigma_points_list): - delta_x = S @ delta[:, index] - state.fill_states_different_dim(current_state.as_vector(), delta_x) + delta_x = S @ self.delta[:, index] + state.fill_dynamic_states(current_state.as_vector(), delta_x) return self.sigma_points_list def unscented_transform(self, current_state: StateQuat) -> StateQuat: - """The unscented transform function generates the priori state estimate""" - _ = self.sigma_points(current_state) + """The unscented transform function generates the priori state estimate.""" + self.sigma_points(current_state) n = len(current_state.covariance) self.y_i = [StateQuat() for _ in range(2 * n)] - for i in range(2 * n): - self.process_model.model_prediction(self.sigma_points_list[i]) + for i, state in enumerate(self.sigma_points_list): + self.process_model.model_prediction(state) + self.process_model.state_vector_prev = state self.y_i[i] = self.process_model.euler_forward() state_estimate = StateQuat() @@ -75,42 +83,36 @@ def unscented_transform(self, current_state: StateQuat) -> StateQuat: def measurement_update( self, current_state: StateQuat, measurement: MeasModel - ) -> tuple[MeasModel, np.ndarray]: - """Function that updates the state estimate with a measurement + ) -> None: + """Function that updates the state estimate with a measurement. + Hopefully this is the DVL or GNSS """ n = len(current_state.covariance) z_i = [MeasModel() for _ in range(2 * n)] - for i in range(2 * n): - z_i[i] = measurement.H(self.sigma_points_list[i]) + for i, state in enumerate(self.sigma_points_list): + z_i[i] = measurement.H(state) - meas_update = MeasModel() + self.measurement_updated.measurement = mean_measurement(z_i) - meas_update.measurement = mean_measurement(z_i) + self.measurement_updated.covariance = covariance_measurement(z_i, self.measurement_updated.measurement) - meas_update.covariance = covariance_measurement(z_i, meas_update.measurement) - - cross_correlation = cross_covariance( - self.y_i, current_state.as_vector(), z_i, meas_update.measurement + self.cross_correlation = cross_covariance( + self.y_i, current_state.as_vector(), z_i, self.measurement_updated.measurement ) - return meas_update, cross_correlation - def posteriori_estimate( self, current_state: StateQuat, - cross_correlation: np.ndarray, measurement: MeasModel, - ex_measuremnt: MeasModel, ) -> StateQuat: - """Calculates the posteriori estimate using measurement and the prior estimate""" + """Calculates the posteriori estimate using measurement and the prior estimate.""" nu_k = MeasModel() + nu_k.measurement = measurement.measurement - self.measurement_updated.measurement + nu_k.covariance = self.measurement_updated.covariance + measurement.covariance - nu_k.measurement = measurement.measurement - ex_measuremnt.measurement - nu_k.covariance = ex_measuremnt.covariance + measurement.covariance - - K_k = np.dot(cross_correlation, np.linalg.inv(nu_k.covariance)) + K_k = np.dot(self.cross_correlation, np.linalg.inv(nu_k.covariance)) posteriori_estimate = StateQuat() @@ -121,6 +123,4 @@ def posteriori_estimate( K_k, np.dot(nu_k.covariance, np.transpose(K_k)) ) - self.process_model.state_vector_prev = posteriori_estimate - return posteriori_estimate diff --git a/navigation/ukf_okid/ukf_python/ukf_okid_class.py b/navigation/ukf_okid/ukf_python/ukf_okid_class.py index 45bbffcfe..5558ad754 100644 --- a/navigation/ukf_okid/ukf_python/ukf_okid_class.py +++ b/navigation/ukf_okid/ukf_python/ukf_okid_class.py @@ -2,6 +2,54 @@ import numpy as np +@dataclass +class okid: + """A class to represent the parameters for the OKID algorithm.""" + + inertia: np.ndarray = field(default_factory=lambda: np.array([0.68, 0.0, 0.0, 0.0, 3.32, 0.0, 0.0, 0.0, 3.34])) + added_mass: np.ndarray = field(default_factory=lambda: np.array([1.0,1.0,1.0,2.0,2.0,2.0])) + damping_linear: np.ndarray = field(default_factory=lambda: np.array([1.0,1.0,1.0,1.0,1.0,1.0])) + + def fill(self, state: np.ndarray) -> None: + """Fills the okid_params object with values from a numpy array.""" + self.inertia = state[0:9] + self.added_mass = state[9:15] + self.damping_linear = state[15:] + + def as_vector(self) -> np.ndarray: + """Returns the okid_params as a numpy array.""" + return np.concatenate([self.inertia, self.added_mass, self.damping_linear]) + + def __add__(self, other: 'okid') -> 'okid': + """Defines the addition operation between two okid_params objects.""" + result = okid() + result.inertia = self.inertia + other.inertia + result.added_mass = self.added_mass + other.added_mass + result.damping_linear = self.damping_linear + other.damping_linear + return result + def __sub__(self, other: 'okid') -> 'okid': + """Defines the subtraction operation between two okid_params objects.""" + result = okid() + result.inertia = self.inertia - other.inertia + result.added_mass = self.added_mass - other.added_mass + result.damping_linear = self.damping_linear - other.damping_linear + return result + + def __sub__(self, other: np.ndarray) -> 'okid': + """ Defines sub between okid_params and np.ndarray.""" + result = okid() + result.inertia = self.inertia - other[0:9] + result.added_mass = self.added_mass - other[9:15] + result.damping_linear = self.damping_linear - other[15:] + return result + + def __rmul__(self, scalar: float) -> 'okid': + """Defines the multiplication operation between a scalar and okid_params object.""" + result = okid() + result.inertia = scalar * self.inertia + result.added_mass = scalar * self.added_mass + result.damping_linear = scalar * self.damping_linear + return result @dataclass class StateQuat: @@ -11,10 +59,16 @@ class StateQuat: orientation: np.ndarray = field(default_factory=lambda: np.array([1, 0, 0, 0])) velocity: np.ndarray = field(default_factory=lambda: np.zeros(3)) angular_velocity: np.ndarray = field(default_factory=lambda: np.zeros(3)) - covariance: np.ndarray = field(default_factory=lambda: np.zeros((12, 12))) + okid_params: okid = field(default_factory=okid) + covariance: np.ndarray = field(default_factory=lambda: np.zeros((33, 33))) def as_vector(self) -> np.ndarray: """Returns the StateVector as a numpy array.""" + return np.concatenate( + [self.position, self.orientation, self.velocity, self.angular_velocity, self.okid_params.as_vector()] + ) + def dynamic_part(self) -> np.ndarray: + """Returns the dynamic part of the state vector.""" return np.concatenate( [self.position, self.orientation, self.velocity, self.angular_velocity] ) @@ -53,6 +107,16 @@ def fill_states(self, state: np.ndarray) -> None: self.orientation = state[3:7] self.velocity = state[7:10] self.angular_velocity = state[10:13] + self.okid_params.fill(state[13:]) + + def fill_dynamic_states(self, state: np.ndarray, state_euler: np.ndarray) -> None: + """Fills only the dynamic part of the state vector with the values from a numpy array.""" + self.position = state[0:3] + state_euler[0:3] + self.orientation = quaternion_super_product( + state[3:7], euler_to_quat(state_euler[3:6]) + ) + self.velocity = state[7:10] + state_euler[6:9] + self.angular_velocity = state[10:13] + state_euler[9:12] def fill_states_different_dim( self, state: np.ndarray, state_euler: np.ndarray @@ -64,6 +128,7 @@ def fill_states_different_dim( ) self.velocity = state[7:10] + state_euler[6:9] self.angular_velocity = state[10:13] + state_euler[9:12] + self.okid_params.fill(state[13:] + state_euler[12:]) def subtract(self, other: 'StateQuat', error_ori: 'np.ndarray') -> np.ndarray: """Subtracts two StateQuat objects, returning the difference with Euler angles.""" @@ -72,6 +137,7 @@ def subtract(self, other: 'StateQuat', error_ori: 'np.ndarray') -> np.ndarray: new_array[3:6] = error_ori new_array[6:9] = self.velocity - other.velocity new_array[9:12] = self.angular_velocity - other.angular_velocity + new_array[12:] = self.okid_params.as_vector() - other.okid_params.as_vector() return new_array @@ -84,6 +150,7 @@ def __add__(self, other: 'StateQuat') -> 'StateQuat': ) new_state.velocity = self.velocity + other.velocity new_state.angular_velocity = self.angular_velocity + other.angular_velocity + new_state.okid_params = self.okid_params + other.okid_params return new_state @@ -94,6 +161,7 @@ def __sub__(self, other: 'StateQuat') -> 'StateQuat': new_state.orientation = quaternion_error(self.orientation, other.orientation) new_state.velocity = self.velocity - other.velocity new_state.angular_velocity = self.angular_velocity - other.angular_velocity + new_state.okid_params = self.okid_params - other.okid_params return new_state.as_vector() @@ -104,6 +172,7 @@ def __rmul__(self, scalar: float) -> 'StateQuat': new_state.orientation = quat_norm(scalar * self.orientation) new_state.velocity = scalar * self.velocity new_state.angular_velocity = scalar * self.angular_velocity + new_state.okid_params = scalar * self.okid_params return new_state @@ -116,6 +185,7 @@ def insert_weights(self, weights: np.ndarray) -> np.ndarray: ) new_state.velocity = self.velocity - weights[6:9] new_state.angular_velocity = self.angular_velocity - weights[9:12] + new_state.okid_params = self.okid_params - weights[12:] return new_state.as_vector() @@ -124,6 +194,7 @@ def add_without_quaternions(self, other: 'StateQuat') -> None: self.position += other.position self.velocity += other.velocity self.angular_velocity += other.angular_velocity + self.okid_params += other.okid_params @dataclass @@ -136,9 +207,9 @@ class MeasModel: def H(self, state: StateQuat) -> 'MeasModel': """Calculates the measurement matrix.""" H = np.zeros((3, 13)) - H[0:3, 7:10] = np.eye(3) + H[:, 7:10] = np.eye(3) z_i = MeasModel() - z_i.measurement = np.dot(H, state.as_vector()) + z_i.measurement = np.dot(H, state.dynamic_part()) return z_i def __add__(self, other: 'MeasModel') -> 'MeasModel': @@ -263,10 +334,112 @@ def euler_forward(self) -> StateQuat: + self.state_vector_dot.angular_velocity * self.dt ) return self.state_vector +@dataclass +class okid_process_model: + state_vector: StateQuat = field(default_factory=StateQuat) + state_vector_dot: StateQuat = field(default_factory=StateQuat) + state_vector_prev: StateQuat = field(default_factory=StateQuat) + Control_input: np.ndarray = field(default_factory=lambda: np.zeros(6)) + mass_interia_matrix: np.ndarray = field(default_factory=lambda: np.zeros((6, 6))) + added_mass: np.ndarray = field(default_factory=lambda: np.zeros(6)) + damping_linear: np.ndarray = field(default_factory=lambda: np.zeros(6)) + m: float = 30.0 + inertia: np.ndarray = field(default_factory=lambda: np.zeros((3, 3))) + r_b_bg: np.ndarray = field(default_factory=lambda: np.zeros(3)) + dt: float = 0.01 + def R(self) -> np.ndarray: + """Calculates the rotation matrix.""" + nu, e_1, e_2, e_3 = self.state_vector.orientation + R = np.array( + [ + [ + 1 - 2 * e_2**2 - 2 * e_3**2, + 2 * e_1 * e_2 - 2 * nu * e_3, + 2 * e_1 * e_3 + 2 * nu * e_2, + ], + [ + 2 * e_1 * e_2 + 2 * nu * e_3, + 1 - 2 * e_1**2 - 2 * e_3**2, + 2 * e_2 * e_3 - 2 * nu * e_1, + ], + [ + 2 * e_1 * e_3 - 2 * nu * e_2, + 2 * e_2 * e_3 + 2 * nu * e_1, + 1 - 2 * e_1**2 - 2 * e_2**2, + ], + ] + ) + return R + + def T(self) -> np.ndarray: + """Calculates the transformation matrix.""" + nu, e_1, e_2, e_3 = self.state_vector.orientation + T = 0.5 * np.array( + [[-e_1, -e_2, -e_3], [nu, -e_3, e_2], [e_3, nu, -e_1], [-e_2, e_1, nu]] + ) + return T + + def Crb(self) -> np.ndarray: + """Calculates the Coriolis matrix.""" + ang_vel = self.state_vector.angular_velocity + ang_vel_skew = skew_symmetric(ang_vel) + lever_arm_skew = skew_symmetric(self.r_b_bg) + Crb = np.zeros((6, 6)) + Crb[0:3, 0:3] = self.m * ang_vel_skew + Crb[3:6, 3:6] = -skew_symmetric(np.dot(self.inertia, ang_vel)) + Crb[0:3, 3:6] = -self.m * np.dot(ang_vel_skew, lever_arm_skew) + Crb[3:6, 0:3] = self.m * np.dot(lever_arm_skew, ang_vel_skew) + return Crb + + def D(self) -> np.ndarray: + """Calculates the damping matrix.""" + D_l = -np.diag(self.damping_linear) + + return D_l + + def model_prediction(self, state: StateQuat) -> None: + """Calculates the model of the system.""" + self.state_vector = state + """ + separate out the different okid values + """ + self.inertia = state.okid_params.inertia.reshape((3, 3)) + self.added_mass = state.okid_params.added_mass + self.damping_linear = state.okid_params.damping_linear + + self.state_vector_dot.position = np.dot(self.R(), self.state_vector.velocity) + self.state_vector_dot.orientation = np.dot( + self.T(), self.state_vector.angular_velocity + ) + Nu = np.linalg.inv(self.mass_interia_matrix + np.diag(self.added_mass)) @ ( + self.Control_input + - np.dot(self.Crb(), self.state_vector.nu()) + - np.dot(self.D(), self.state_vector.nu()) + ) + self.state_vector_dot.velocity = Nu[:3] + self.state_vector_dot.angular_velocity = Nu[3:] + + def euler_forward(self) -> None: + """Calculates the forward Euler integration.""" + self.state_vector.position = ( + self.state_vector_prev.position + self.state_vector_dot.position * self.dt + ) + self.state_vector.orientation = quat_norm( + self.state_vector_prev.orientation + + self.state_vector_dot.orientation * self.dt + ) + self.state_vector.velocity = ( + self.state_vector_prev.velocity + self.state_vector_dot.velocity * self.dt + ) + self.state_vector.angular_velocity = ( + self.state_vector_prev.angular_velocity + + self.state_vector_dot.angular_velocity * self.dt + ) + return self.state_vector def euler_to_quat(euler_angles: np.ndarray) -> np.ndarray: - """Converts Euler angles to a quaternion""" + """Converts Euler angles to a quaternion.""" psi, theta, phi = euler_angles c_psi = np.cos(psi / 2) s_psi = np.sin(psi / 2) @@ -288,7 +461,7 @@ def euler_to_quat(euler_angles: np.ndarray) -> np.ndarray: def quat_to_euler(quat: np.ndarray) -> np.ndarray: - """Converts a quaternion to Euler angles""" + """Converts a quaternion to Euler angles.""" nu, eta_1, eta_2, eta_3 = quat phi = np.arctan2(2 * (eta_2 * eta_3 + nu * eta_1), 1 - 2 * (eta_1**2 + eta_2**2)) @@ -299,7 +472,7 @@ def quat_to_euler(quat: np.ndarray) -> np.ndarray: def quat_norm(quat: np.ndarray) -> np.ndarray: - """Function that normalizes a quaternion""" + """Function that normalizes a quaternion.""" quat = quat / np.linalg.norm(quat) return quat @@ -348,7 +521,7 @@ def quaternion_super_product(q1: np.ndarray, q2: np.ndarray) -> np.ndarray: def quaternion_error(quat_1: np.ndarray, quat_2: np.ndarray) -> np.ndarray: - """Calculates the error between two quaternions""" + """Calculates the error between two quaternions.""" quat_2_inv = np.array([quat_2[0], -quat_2[1], -quat_2[2], -quat_2[3]]) error_quat = quaternion_super_product(quat_1, quat_2_inv) @@ -359,17 +532,15 @@ def quaternion_error(quat_1: np.ndarray, quat_2: np.ndarray) -> np.ndarray: def iterative_quaternion_mean_statequat( state_list: list[StateQuat], tol: float = 1e-6, max_iter: int = 100 ) -> np.ndarray: - """Computes the weighted mean of the quaternion orientations from a list of StateQuat objects - using an iterative approach, without requiring the caller to manually extract the quaternion. + """Computes the iterative mean of quaternion orientations from StateQuat objects. - Parameters: - state_list (list[StateQuat]): List of StateQuat objects. - weights (np.ndarray): Weights for each state. - tol (float): Convergence tolerance. - max_iter (int): Maximum number of iterations. + Args: + state_list: List of StateQuat objects + tol: Convergence tolerance + max_iter: Maximum iterations Returns: - np.ndarray: The averaged quaternion as a 4-element numpy array. + Mean quaternion as numpy array """ sigma_quats = [state.orientation for state in state_list] n = len(state_list) @@ -412,7 +583,7 @@ def iterative_quaternion_mean_statequat( def mean_set(set_points: list[StateQuat]) -> np.ndarray: - """Function calculates the mean vector of a set of points + """Function calculates the mean vector of a set of points. Args: set_points (list[StateQuat]): List of StateQuat objects @@ -426,7 +597,10 @@ def mean_set(set_points: list[StateQuat]) -> np.ndarray: for state in set_points: mean_value.add_without_quaternions(state) - mean_value = (1 / (n)) * mean_value + mean_value.position = (1 / n) * mean_value.position + mean_value.velocity = (1 / n) * mean_value.velocity + mean_value.angular_velocity = (1 / n) * mean_value.angular_velocity + mean_value.okid_params = (1 / n) * mean_value.okid_params mean_value.orientation = iterative_quaternion_mean_statequat(set_points) @@ -434,7 +608,7 @@ def mean_set(set_points: list[StateQuat]) -> np.ndarray: def mean_measurement(set_points: list[MeasModel]) -> np.ndarray: - """Function that calculates the mean of a set of points""" + """Function that calculates the mean of a set of points.""" n = len(set_points) mean_value = MeasModel() @@ -447,7 +621,7 @@ def mean_measurement(set_points: list[MeasModel]) -> np.ndarray: def covariance_set(set_points: list[StateQuat], mean: np.ndarray) -> np.ndarray: - """Function that calculates the covariance of a set of points""" + """Function that calculates the covariance of a set of points.""" n = len(set_points) covariance = np.zeros(set_points[0].covariance.shape) @@ -477,7 +651,7 @@ def covariance_set(set_points: list[StateQuat], mean: np.ndarray) -> np.ndarray: def covariance_measurement(set_points: list[MeasModel], mean: np.ndarray) -> np.ndarray: - """Function that calculates the covariance of a set of points""" + """Function that calculates the covariance of a set of points.""" n = len(set_points) co_size = len(set_points[0].measurement) covariance = np.zeros((co_size, co_size)) @@ -500,7 +674,7 @@ def cross_covariance( set_z: list[MeasModel], mean_z: np.ndarray, ) -> np.ndarray: - """Calculates the cross covariance between the measurement and state prediction""" + """Calculates the cross covariance between the measurement and state prediction.""" n = len(set_y) cross_covariance = np.zeros((len(mean_y) - 1, len(mean_z))) diff --git a/navigation/ukf_okid/ukf_python/ukf_ros.py b/navigation/ukf_okid/ukf_python/ukf_ros.py new file mode 100755 index 000000000..66d01175c --- /dev/null +++ b/navigation/ukf_okid/ukf_python/ukf_ros.py @@ -0,0 +1,117 @@ +#!/usr/bin/env python3 +import rclpy +from geometry_msgs.msg import TwistWithCovarianceStamped, WrenchStamped +from nav_msgs.msg import Odometry +from rclpy.node import Node +from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy +from std_msgs.msg import Bool, String +from ukf_okid import UKF +from ukf_okid_class import StateQuat, process_model, MeasModel +import numpy as np + +class UKFNode(Node): + def __init__(self): + super().__init__("UKFNode") + + best_effort_qos = QoSProfile( + reliability=ReliabilityPolicy.BEST_EFFORT, + history=HistoryPolicy.KEEP_LAST, + depth=1, + ) + + #subcribers + self.dvl_subscriber = self.create_subscription(TwistWithCovarianceStamped, + "/orca/twist", + self.dvl_callback, + qos_profile=best_effort_qos, + ) + + self.control_input = self.create_subscription(WrenchStamped,"/orca/wrench_input", self.control_callback, qos_profile=best_effort_qos,) + + self.odom_publish = self.create_publisher(Odometry, "/orca/odometry", qos_profile=best_effort_qos) + dt = self.declare_parameter("dt", 0.01).get_parameter_value().double_value + self.control_timer = self.create_timer(dt, self.odom_publisher) + + self.current_state = StateQuat() + x0 = np.zeros(13) + x0[3] = 1.0 # quaternion: [1, 0, 0, 0] + P0 = np.eye(12) * 0.5 + self.ukf_model = process_model() + self.ukf_model.dt = 0.01 + self.ukf_model.mass_interia_matrix = np.array([ + [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], + ]) + self.ukf_model.m = 30.0 + self.ukf_model.r_b_bg = np.array([0.01, 0.0, 0.02]) + self.ukf_model.inertia = np.diag([0.68, 3.32, 3.34]) + self.ukf_model.damping_linear = np.array([0.01] * 6) + # self.ukf_model.added_mass = np.diag([1.0, 1.0, 1.0, 2.0, 2.0, 2.0]) + + Q = np.diag([0.02, 0.02, 0.02, + 0.02, 0.02, 0.02, + 0.1, 0.1, 0.1, + 0.1, 0.1, 0.1]) + + self.ukf = UKF(self.ukf_model, x0, P0, Q) + self.ukf_flagg = False + + def dvl_callback(self, msg: TwistWithCovarianceStamped): + # unpack msg + dvl_measurement = MeasModel() + # Print received DVL data to console + # self.get_logger().info(f"DVL data received: x={msg.twist.twist.linear.x}, y={msg.twist.twist.linear.y}, z={msg.twist.twist.linear.z}") + dvl_measurement.measurement = np.array([msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z]) + dvl_measurement.covariance = np.array([[msg.twist.covariance[0], msg.twist.covariance[1], msg.twist.covariance[3]], + [msg.twist.covariance[6], msg.twist.covariance[7], msg.twist.covariance[8]], + [msg.twist.covariance[12], msg.twist.covariance[13], msg.twist.covariance[14]]]) + + self.ukf.measurement_update(self.current_state, dvl_measurement) + self.current_state = self.ukf.posteriori_estimate(self.current_state, dvl_measurement) + self.ukf_flagg = True + + def control_callback(self, msg:WrenchStamped): + # unpack message + control_array = np.array([msg.wrench.force.x, msg.wrench.force.y, msg.wrench.force.z, msg.wrench.torque.x, msg.wrench.torque.y, msg.wrench.torque.z]) + self.ukf_model.Control_input = control_array + + def odom_publisher(self): + msg = Odometry() + + if self.ukf_flagg == False: + self.current_state = self.ukf.unscented_transform(self.current_state) + else: + self.ukf_flagg = False + + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = "odom" + msg.child_frame_id = "base_link" + + msg.pose.pose.position.x = self.current_state.position[0] + msg.pose.pose.position.y = self.current_state.position[1] + msg.pose.pose.position.z = self.current_state.position[2] + msg.pose.pose.orientation.w = self.current_state.orientation[0] + msg.pose.pose.orientation.x = self.current_state.orientation[1] + msg.pose.pose.orientation.y = self.current_state.orientation[2] + msg.pose.pose.orientation.z = self.current_state.orientation[3] + msg.twist.twist.linear.x = self.current_state.velocity[0] + msg.twist.twist.linear.y = self.current_state.velocity[1] + msg.twist.twist.linear.z = self.current_state.velocity[2] + msg.twist.twist.angular.x = self.current_state.angular_velocity[0] + msg.twist.twist.angular.y = self.current_state.angular_velocity[1] + msg.twist.twist.angular.z = self.current_state.angular_velocity[2] + + self.odom_publish.publish(msg) + +def main(args=None): + rclpy.init(args=args) + ukf_node = UKFNode() + rclpy.spin(ukf_node) + rclpy.shutdown() + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/navigation/ukf_okid/ukf_python/ukf_test.py b/navigation/ukf_okid/ukf_python/ukf_test.py index cebb53ac6..80ca90e86 100644 --- a/navigation/ukf_okid/ukf_python/ukf_test.py +++ b/navigation/ukf_okid/ukf_python/ukf_test.py @@ -1,323 +1,440 @@ -import time - -import matplotlib.pyplot as plt import numpy as np -from ukf_okid import UKF +import matplotlib.pyplot as plt +from ukf_utils import print_StateQuat +# Import your classes and functions. +# Adjust the import paths as necessary based on your module organization. from ukf_okid_class import ( - MeasModel, StateQuat, - process_model, - quat_to_euler, + MeasModel, + iterative_quaternion_mean_statequat, + mean_set, + mean_measurement, + covariance_set, + covariance_measurement, + cross_covariance, quaternion_super_product, + quaternion_error, + quat_to_euler, + quat_norm, ) - -def add_quaternion_noise(q, noise_std): - noise = np.random.normal(0, noise_std, 3) - - theta = np.linalg.norm(noise) - - if theta > 0: - axis = noise / theta - - q_noise = np.hstack((np.cos(theta / 2), np.sin(theta / 2) * axis)) - +# For testing, define a function to create a StateQuat with small perturbations. +def create_statequat(base_vector, position_perturbation, orientation_perturbation, velocity_perturbation, angular_velocity_perturbation): + """ + Creates a StateQuat object. + - base_vector: 1D numpy array for base state (13 elements: + position (3), quaternion (4), velocity (3), angular_velocity (3)) + - ..._perturbation: small perturbation vector to be added to each respective component. + Returns a StateQuat. + """ + state = StateQuat() + # Base state + state.position = base_vector[0:3] + position_perturbation + # For orientation, perturb by adding a small rotation: + base_quat = base_vector[3:7] + noise_angle = np.linalg.norm(orientation_perturbation) + if noise_angle < 1e-8: + noise_quat = np.array([1.0, 0.0, 0.0, 0.0]) else: - q_noise = np.array([1.0, 0.0, 0.0, 0.0]) - - q_new = quaternion_super_product(q, q_noise) - - return q_new / np.linalg.norm(q_new) - - -if __name__ == '__main__': - # Create initial state vector and covariance matrix. - x0 = np.zeros(13) - x0[0:3] = [0.3, 0.3, 0.3] - x0[3] = 1 - x0[7:10] = [0.2, 0.2, 0.2] - dt = 0.01 - R = (0.01) * np.eye(3) - - Q = 0.00015 * np.eye(12) - P0 = np.eye(12) * 0.0001 + noise_axis = orientation_perturbation / noise_angle + noise_quat = np.concatenate(([np.cos(noise_angle/2)], np.sin(noise_angle/2) * noise_axis)) + state.orientation = quat_norm(quaternion_super_product(base_quat, noise_quat)) + state.velocity = base_vector[7:10] + velocity_perturbation + state.angular_velocity = base_vector[10:13] + angular_velocity_perturbation + + # For the augmented parameters (OKID parameters), set a 21-element vector: + # 9 for inertia, 6 for added_mass, and 6 for damping_linear. + state.okid_params.fill(np.concatenate((np.zeros(9), np.zeros(6), np.zeros(6)))) + + # Set a default covariance (33x33 for the extended state) + state.covariance = np.eye(33) * 0.01 + return state + +# Test functions for state statistics +def test_state_statistics(): + # Define a base state vector (13 elements: position, quaternion, velocity, angular_velocity) + base_vector = np.zeros(13) + base_vector[0:3] = np.array([1.0, 2.0, 3.0]) + base_vector[3:7] = np.array([1.0, 0.0, 0.0, 0.0]) # identity quaternion + base_vector[7:10] = np.array([0.1, 0.2, 0.3]) + base_vector[10:13] = np.array([0.01, 0.02, 0.03]) + + # Create a list of StateQuat objects with small random perturbations. + np.random.seed(42) + state_list = [] + num_states = 10 + for _ in range(num_states): + pos_noise = np.random.normal(0, 0.05, 3) + ori_noise = np.random.normal(0, 0.01, 3) + vel_noise = np.random.normal(0, 0.02, 3) + ang_vel_noise = np.random.normal(0, 0.005, 3) + state_list.append(create_statequat(base_vector, pos_noise, ori_noise, vel_noise, ang_vel_noise)) + + # Compute the state mean using mean_set. + mean_state_vec = mean_set(state_list) + print("Computed mean state vector:") + print(mean_state_vec) + + # Compute the covariance of the states. + cov_state = covariance_set(state_list, mean_state_vec) + print("Computed state covariance matrix:") + print(cov_state) + + # Check symmetry of the covariance: + asym_error = np.linalg.norm(cov_state - cov_state.T) + print("Covariance symmetry error (should be near 0):", asym_error) + + # Check eigenvalues for positive semidefiniteness: + eigvals = np.linalg.eigvals(cov_state) + print("Eigenvalues of state covariance:") + print(eigvals) + +def test_measurement_statistics(): + # Create a list of measurement objects (MeasModel) with measurements in R^3. + np.random.seed(24) + meas_list = [] + num_meas = 10 + base_meas = np.array([1.0, 2.0, 3.0]) + for _ in range(num_meas): + noise = np.random.normal(0, 0.1, 3) + meas = MeasModel() + meas.measurement = base_meas + noise + meas_list.append(meas) + + # Compute the measurement mean. + mean_meas = mean_measurement(meas_list) + print("Computed measurement mean:") + print(mean_meas) + + # Compute the measurement covariance. + cov_meas = covariance_measurement(meas_list, mean_meas) + print("Computed measurement covariance:") + print(cov_meas) + + # Check symmetry and eigenvalues. + asym_error = np.linalg.norm(cov_meas - cov_meas.T) + print("Measurement covariance symmetry error:", asym_error) + eigvals = np.linalg.eigvals(cov_meas) + print("Eigenvalues of measurement covariance:") + print(eigvals) + +def test_cross_covariance(): + # Create a set of StateQuat and corresponding MeasModel objects. + np.random.seed(99) + num = 10 + state_list = [] + meas_list = [] + base_vector = np.zeros(13) + base_vector[0:3] = np.array([0.5, 1.0, -0.5]) + base_vector[3:7] = np.array([1.0, 0.0, 0.0, 0.0]) + base_vector[7:10] = np.array([0.05, 0.1, 0.15]) + base_vector[10:13] = np.array([0.005, 0.01, 0.015]) + + for _ in range(num): + pos_noise = np.random.normal(0, 0.02, 3) + ori_noise = np.random.normal(0, 0.005, 3) + vel_noise = np.random.normal(0, 0.01, 3) + ang_vel_noise = np.random.normal(0, 0.002, 3) + state = create_statequat(base_vector, pos_noise, ori_noise, vel_noise, ang_vel_noise) + state_list.append(state) + + # Generate a measurement from each state (e.g., state velocity plus noise). + meas = MeasModel() + meas.measurement = state.velocity + np.random.normal(0, 0.01, 3) + meas_list.append(meas) + + # Compute the state mean and measurement mean as vectors. + mean_state_vec = mean_set(state_list) + mean_meas = mean_measurement(meas_list) + + cross_cov = cross_covariance(state_list, mean_state_vec, meas_list, mean_meas) + print("Computed cross-covariance between state and measurement:") + print(cross_cov) - model = process_model() - model.dt = 0.01 - model.mass_interia_matrix = np.array( - [ - [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], - ] - ) - model.m = 30.0 - model.r_b_bg = np.array([0.01, 0.0, 0.02]) - model.inertia = np.diag([0.68, 3.32, 3.34]) - model.damping_linear = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) - model.damping_nonlinear = np.array([0.3, 0.3, 0.3, 0.3, 0.3, 0.3]) - model.added_mass = np.diag([1.0, 1.0, 1.0, 2.0, 2.0, 2.0]) - - model_ukf = process_model() - model_ukf.dt = 0.01 - model_ukf.mass_interia_matrix = np.array( - [ - [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], - ] - ) - model_ukf.m = 30.0 - model_ukf.r_b_bg = np.array([0.01, 0.0, 0.02]) - model_ukf.inertia = np.diag([0.68, 3.32, 3.34]) - model_ukf.damping_linear = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) - model_ukf.damping_nonlinear = np.array([0.3, 0.3, 0.3, 0.3, 0.3, 0.3]) - model_ukf.added_mass = np.diag([1.0, 1.0, 1.0, 2.0, 2.0, 2.0]) +import time +import numpy as np +import matplotlib.pyplot as plt - # Simulation parameters - simulation_time = 5 # seconds +# Import your classes and functions. +from ukf_okid_class import ( + StateQuat, + MeasModel, + iterative_quaternion_mean_statequat, + mean_set, + mean_measurement, + covariance_set, + covariance_measurement, + cross_covariance, + quaternion_super_product, + quaternion_error, + quat_norm, +) +from ukf_okid import UKF +from ukf_okid_class import process_model, okid_process_model # Your process model classes + +############################################ +# Helper function to create a StateQuat with perturbations. +############################################ +def create_statequat(base_vector, pos_noise, ori_noise, vel_noise, ang_vel_noise): + """ + Create a StateQuat object from a base vector (13 elements: + position (3), quaternion (4), velocity (3), angular_velocity (3)) + plus additive noise on each component. + + For the OKID parameters, we assume a 21-element vector: + - first 9: inertia, + - next 6: added_mass, + - last 6: damping_linear. + """ + state = StateQuat() + state.position = base_vector[0:3] + pos_noise + base_quat = base_vector[3:7] + noise_angle = np.linalg.norm(ori_noise) + if noise_angle < 1e-8: + noise_quat = np.array([1.0, 0.0, 0.0, 0.0]) + else: + noise_axis = ori_noise / noise_angle + noise_quat = np.concatenate(([np.cos(noise_angle/2)], + np.sin(noise_angle/2) * noise_axis)) + state.orientation = quat_norm(quaternion_super_product(base_quat, noise_quat)) + state.velocity = base_vector[7:10] + vel_noise + state.angular_velocity = base_vector[10:13] + ang_vel_noise + + # Set OKID parameters to exactly 21 elements (9,6,6) + state.okid_params.fill(np.concatenate((np.array([0.0, 0.0, 0.3, 0.0, 0.0, 3.3, 0.0, 0.0, 3.3]), np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0]), np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0])))) + # Set an initial covariance (33x33) for the full augmented state. + state.covariance = np.eye(33) * 0.02 + return state + +############################################ +# Full Filter Simulation Test +############################################ +def run_ukf_simulation(): + dt = 0.01 # Time step for simulation [s] + simulation_time = 10 # Total simulation time in seconds num_steps = int(simulation_time / dt) - - # Initialize a dummy StateQuat. - new_state = StateQuat() - new_state.fill_states(x0) - new_state.covariance = P0 - - test_state_x = StateQuat() - test_state_x.fill_states(x0) - test_state_x.covariance = P0 - - # Initialize a estimated state - estimated_state = StateQuat() - estimated_state.fill_states(x0) - estimated_state.covariance = P0 - - # Initialize a estimated state - noisy_state = StateQuat() - noisy_state.fill_states(x0) - noisy_state.covariance = P0 - - measurment_model = MeasModel() - measurment_model.measurement = np.array([0.0, 0.0, 0.0]) - measurment_model.covariance = R - - # Initialize arrays to store the results - positions = np.zeros((num_steps, 3)) - orientations = np.zeros((num_steps, 3)) - velocities = np.zeros((num_steps, 3)) - angular_velocities = np.zeros((num_steps, 3)) - - # Initialize arrays to store the estimates - positions_est = np.zeros((num_steps, 3)) - orientations_est = np.zeros((num_steps, 3)) - velocities_est = np.zeros((num_steps, 3)) - angular_velocities_est = np.zeros((num_steps, 3)) - - # Initialize the okid params - okid_params = np.zeros((num_steps, 21)) - - model.state_vector_prev = new_state - model.state_vector = new_state - - model_ukf.state_vector_prev = test_state_x - model_ukf.state_vector = test_state_x - - # initialize the ukf - ukf = UKF(model_ukf, x0, P0, Q, R) - - elapsed_times = [] - - u = lambda t: np.array( - [ - 2 * np.sin(1 * t), - 2 * np.sin(1 * t), - 2 * np.sin(1 * t), - 0.2 * np.cos(1 * t), - 0.2 * np.cos(1 * t), - 0.2 * np.cos(1 * t), - ] - ) - - # Run the simulation - for step in range(num_steps): - # Insert control input - model.Control_input = u(step * dt) - model_ukf.Control_input = u(step * dt) - - # Perform the unscented transform - model.model_prediction(new_state) - new_state = model.euler_forward() - - # Adding noise in the state vector - estimated_state.position = ( - estimated_state.position - ) # + np.random.normal(0, 0.01, 3) - estimated_state.orientation = ( - estimated_state.orientation - ) # add_quaternion_noise(estimated_state.orientation, 0.01) - estimated_state.velocity = ( - estimated_state.velocity - ) # + np.random.normal(0, 0.01, 3) - estimated_state.angular_velocity = ( - estimated_state.angular_velocity - ) # + np.random.normal(0, 0.01, 3) - - start_time = time.time() - estimated_state = ukf.unscented_transform(estimated_state) - print(estimated_state.as_vector()) - break - elapsed_time = time.time() - start_time - elapsed_times.append(elapsed_time) - - if step % 20 == 0: - measurment_model.measurement = ( - new_state.velocity - ) # + np.random.normal(0, 0.01, 3) - meas_update, covariance_matrix = ukf.measurement_update( - estimated_state, measurment_model - ) - estimated_state = ukf.posteriori_estimate( - estimated_state, covariance_matrix, measurment_model, meas_update - ) - - positions[step, :] = new_state.position - orientations[step, :] = quat_to_euler(new_state.orientation) - velocities[step, :] = new_state.velocity - angular_velocities[step, :] = new_state.angular_velocity - - positions_est[step, :] = estimated_state.position - orientations_est[step, :] = quat_to_euler(estimated_state.orientation) - velocities_est[step, :] = estimated_state.velocity - angular_velocities_est[step, :] = estimated_state.angular_velocity - - # Update the state for the next iteration - model.state_vector_prev = new_state - - print('Average elapsed time: ', np.mean(elapsed_times)) - print('Max elapsed time: ', np.max(elapsed_times)) - print('Min elapsed time: ', np.min(elapsed_times)) - print('median elapsed time: ', np.median(elapsed_times)) - # Plot the results - time = np.linspace(0, simulation_time, num_steps) - - # Plot positions - plt.figure() - plt.subplot(3, 1, 1) - plt.plot(time, positions[:, 0], label='True') - plt.plot(time, positions_est[:, 0], label='Estimated') - plt.title('Position X') - plt.xlabel('Time [s]') - plt.ylabel('Position X [m]') + + # Define a base state vector (13 elements: pos, quat, vel, ang_vel) + base_vector = np.zeros(13) + base_vector[0:3] = np.array([0.0, 0.0, 0.0]) + base_vector[3:7] = np.array([1.0, 0.0, 0.0, 0.0]) # identity quaternion + base_vector[7:10] = np.array([0.1, 0.0, 0.0]) # small velocity in x + base_vector[10:13] = np.array([0.0, 0.0, 0.0]) + + # Define initial covariance for state (33x33) + P0 = np.eye(33) + P0[0:3, 0:3] = np.eye(3) * 0.01 # position + P0[3:6, 3:6] = np.eye(3) * 0.01 # orientation error (quaternion) + P0[6:9, 6:9] = np.eye(3) * 0.01 # velocity + P0[9:12, 9:12] = np.eye(3) * 0.01 # angular velocity + P0[12:33, 12:33] = np.eye(21) * 0.001 # OKID parameters + + # Define process noise covariance Q (33x33) + Q = np.zeros((33, 33)) + Q[0:3, 0:3] = np.eye(3)*0.001 # for position + Q[3:6, 3:6] = np.eye(3)*0.001 # for orientation error (represented with Euler angles) + Q[6:9, 6:9] = np.eye(3)*0.001 # for velocity + Q[9:12, 9:12] = np.eye(3)*0.001 # for angular velocity + Q[12:33, 12:33] = np.eye(21) * 0.001 # OKID parameters + + + G = np.zeros((33, 12)) + G[0:3, 0:3] = np.eye(3) + G[3:6, 3:6] = np.eye(3) + G[6:9, 6:9] = np.eye(3) + G[9:12, 9:12] = np.eye(3) + + # Measurement noise covariance R (3x3), assume measurement is velocity + R = np.eye(3) * 0.01 + + # Create a simulation process model and an independent UKF process model. + sim_model = process_model() + sim_model.dt = dt + sim_model.mass_interia_matrix = np.array([ + [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], + ]) + sim_model.m = 30.0 + sim_model.r_b_bg = np.array([0.01, 0.0, 0.02]) + sim_model.inertia = np.diag([0.68, 3.32, 3.34]) + sim_model.damping_linear = np.array([0.1]*6) + sim_model.added_mass = np.array([1.0,1.0,1.0,2.0,2.0,2.0]) + + # UKF process model copy: + ukf_model = okid_process_model() + ukf_model.dt = dt + ukf_model.mass_interia_matrix = sim_model.mass_interia_matrix.copy() + ukf_model.m = sim_model.m + ukf_model.r_b_bg = sim_model.r_b_bg.copy() + ukf_model.inertia = sim_model.inertia.copy() + ukf_model.damping_linear = sim_model.damping_linear.copy() + ukf_model.added_mass = sim_model.added_mass.copy() + + # Initialize true state and filter state. + true_state = create_statequat(base_vector, + np.zeros(3), + np.zeros(3), + np.zeros(3), + np.zeros(3)) + true_state.covariance = P0.copy() + + filter_state = create_statequat(base_vector, + np.zeros(3), + np.zeros(3), + np.zeros(3), + np.zeros(3)) + filter_state.covariance = P0.copy() + + # Initialize measurement model (for example, measuring velocity only) + meas_model = MeasModel() + meas_model.covariance = R.copy() + + # Initialize UKF. + ukf = UKF(ukf_model, true_state, P0.copy(), Q.copy(), G.copy()) + + # Arrays to store time histories. + pos_true_hist = np.zeros((num_steps, 3)) + pos_est_hist = np.zeros((num_steps, 3)) + vel_true_hist = np.zeros((num_steps, 3)) + vel_est_hist = np.zeros((num_steps, 3)) + euler_true_hist = np.zeros((num_steps, 3)) + euler_est_hist = np.zeros((num_steps, 3)) + time_array = np.linspace(0, simulation_time, num_steps) + + # Control input function (example: oscillatory in all directions) + def control_input(t): + return np.array([ + 2*np.sin(t), + 2*np.sin(t+0.5), + 2*np.sin(t+1.0), + 0.2*np.cos(t), + 0.2*np.cos(t+0.5), + 0.2*np.cos(t+1.0) + ]) + + # Set previous states. + sim_model.state_vector_prev = true_state + sim_model.state_vector = true_state + ukf_model.state_vector_prev = filter_state + ukf_model.state_vector = filter_state + + # Lists for timing diagnostics. + ukf_transform_times = [] + ukf_update_times = [] + + # Simulation loop. + for i in range(num_steps): + t_current = i*dt + + # Update control inputs. + sim_model.Control_input = control_input(t_current) + ukf_model.Control_input = control_input(t_current) + + # Propagate true state using the simulation model. + sim_model.model_prediction(true_state) + true_state = sim_model.euler_forward() + + # Create a measurement from true state. + # Here we assume we measure velocity plus noise. + meas_noise = np.random.normal(0, 0.01, 3) + meas_model.measurement = true_state.velocity + meas_noise + + # UKF prediction: unscented transform. + start = time.time() + filter_state = ukf.unscented_transform(filter_state) + ukf_transform_times.append(time.time() - start) + + # UKF measurement update every few steps. + if i % 5 == 0: + try: + start = time.time() + ukf.measurement_update(filter_state, meas_model) + filter_state = ukf.posteriori_estimate(filter_state, meas_model) + ukf_update_times.append(time.time() - start) + except np.linalg.LinAlgError: + # If matrix is not PD, add jitter. + filter_state.covariance += np.eye(filter_state.covariance.shape[0])*1e-6 + + # Store true and estimated state for diagnostics. + pos_true_hist[i, :] = true_state.position + pos_est_hist[i, :] = filter_state.position + vel_true_hist[i, :] = true_state.velocity + vel_est_hist[i, :] = filter_state.velocity + # Convert quaternion to Euler angles for visualization. + # Assumes you have a function quat_to_euler. + euler_true_hist[i, :] = quat_to_euler(true_state.orientation) + euler_est_hist[i, :] = quat_to_euler(filter_state.orientation) + + # Update previous states. + sim_model.state_vector_prev = true_state + ukf_model.state_vector_prev = filter_state + + # Print timing diagnostics. + print("Average unscented transform time:", np.mean(ukf_transform_times)) + print("Average measurement update time:", np.mean(ukf_update_times)) + + # Compute error metrics. + pos_error = np.linalg.norm(pos_true_hist - pos_est_hist, axis=1) + vel_error = np.linalg.norm(vel_true_hist - vel_est_hist, axis=1) + euler_error = np.linalg.norm(euler_true_hist - euler_est_hist, axis=1) + print("Average position error:", np.mean(pos_error)) + print("Average velocity error:", np.mean(vel_error)) + print("Average orientation (Euler) error:", np.mean(euler_error)) + + # Plot estimated vs true trajectory (positions). + plt.figure(figsize=(10,8)) + plt.subplot(3,1,1) + plt.plot(time_array, pos_true_hist[:,0], label="True X") + plt.plot(time_array, pos_est_hist[:,0], label="Est X", linestyle="--") plt.legend() - - plt.subplot(3, 1, 2) - plt.plot(time, positions[:, 1], label='True') - plt.plot(time, positions_est[:, 1], label='Estimated') - plt.title('Position Y') - plt.xlabel('Time [s]') - plt.ylabel('Position Y [m]') + plt.title("Position X") + + plt.subplot(3,1,2) + plt.plot(time_array, pos_true_hist[:,1], label="True Y") + plt.plot(time_array, pos_est_hist[:,1], label="Est Y", linestyle="--") plt.legend() - - plt.subplot(3, 1, 3) - plt.plot(time, positions[:, 2], label='True') - plt.plot(time, positions_est[:, 2], label='Estimated') - plt.title('Position Z') - plt.xlabel('Time [s]') - plt.ylabel('Position Z [m]') + plt.title("Position Y") + + plt.subplot(3,1,3) + plt.plot(time_array, pos_true_hist[:,2], label="True Z") + plt.plot(time_array, pos_est_hist[:,2], label="Est Z", linestyle="--") plt.legend() - + plt.title("Position Z") plt.tight_layout() plt.show() - - # Plot orientations (Euler angles) - plt.figure() - plt.subplot(3, 1, 1) - plt.plot(time, orientations[:, 0], label='True') - plt.plot(time, orientations_est[:, 0], label='Estimated') - plt.title('Orientation Roll') - plt.xlabel('Time [s]') - plt.ylabel('Roll [rad]') - plt.legend() - - plt.subplot(3, 1, 2) - plt.plot(time, orientations[:, 1], label='True') - plt.plot(time, orientations_est[:, 1], label='Estimated') - plt.title('Orientation Pitch') - plt.xlabel('Time [s]') - plt.ylabel('Pitch [rad]') + + # Plot errors. + plt.figure(figsize=(10,4)) + plt.plot(time_array, pos_error, label="Position Error") + plt.plot(time_array, vel_error, label="Velocity Error") + plt.plot(time_array, euler_error, label="Euler Angle Error") plt.legend() - - plt.subplot(3, 1, 3) - plt.plot(time, orientations[:, 2], label='True') - plt.plot(time, orientations_est[:, 2], label='Estimated') - plt.title('Orientation Yaw') - plt.xlabel('Time [s]') - plt.ylabel('Yaw [rad]') - plt.legend() - - plt.tight_layout() + plt.title("Error Metrics over Time") + plt.xlabel("Time (s)") + plt.ylabel("Error magnitude") plt.show() - # Plot velocities - plt.figure() - plt.subplot(3, 1, 1) - plt.plot(time, velocities[:, 0], label='True') - plt.plot(time, velocities_est[:, 0], label='Estimated') - plt.title('Velocity X') - plt.xlabel('Time [s]') - plt.ylabel('Velocity X [m/s]') - plt.legend() - - plt.subplot(3, 1, 2) - plt.plot(time, velocities[:, 1], label='True') - plt.plot(time, velocities_est[:, 1], label='Estimated') - plt.title('Velocity Y') - plt.xlabel('Time [s]') - plt.ylabel('Velocity Y [m/s]') - plt.legend() +# You can also test the individual statistics functions separately: +def run_diagnostics(): + print("Testing state mean and covariance computation:") + # Call your pre-written tests: + # (Assuming these functions—test_state_statistics, test_measurement_statistics, test_cross_covariance—are defined above) + test_state_statistics() + print("\nTesting measurement mean and covariance computation:") + test_measurement_statistics() + print("\nTesting cross-covariance computation:") + test_cross_covariance() - plt.subplot(3, 1, 3) - plt.plot(time, velocities[:, 2], label='True') - plt.plot(time, velocities_est[:, 2], label='Estimated') - plt.title('Velocity Z') - plt.xlabel('Time [s]') - plt.ylabel('Velocity Z [m/s]') - plt.legend() - - plt.tight_layout() - plt.show() - - # Plot angular velocities - plt.figure() - plt.subplot(3, 1, 1) - plt.plot(time, angular_velocities[:, 0], label='True') - plt.plot(time, angular_velocities_est[:, 0], label='Estimated') - plt.title('Angular Velocity X') - plt.xlabel('Time [s]') - plt.ylabel('Angular Velocity X [rad/s]') - plt.legend() - - plt.subplot(3, 1, 2) - plt.plot(time, angular_velocities[:, 1], label='True') - plt.plot(time, angular_velocities_est[:, 1], label='Estimated') - plt.title('Angular Velocity Y') - plt.xlabel('Time [s]') - plt.ylabel('Angular Velocity Y [rad/s]') - plt.legend() +if __name__ == '__main__': + # First, run the diagnostics on the mean/covariance functions. + run_diagnostics() + + # Then run the full UKF simulation test. + print("\nRunning full UKF simulation test:") + run_ukf_simulation() - plt.subplot(3, 1, 3) - plt.plot(time, angular_velocities[:, 2], label='True') - plt.plot(time, angular_velocities_est[:, 2], label='Estimated') - plt.title('Angular Velocity Z') - plt.xlabel('Time [s]') - plt.ylabel('Angular Velocity Z [rad/s]') - plt.legend() - plt.tight_layout() - plt.show() diff --git a/navigation/ukf_okid/ukf_python/ukf_test_2.py b/navigation/ukf_okid/ukf_python/ukf_test_2.py new file mode 100644 index 000000000..bf58b7a1a --- /dev/null +++ b/navigation/ukf_okid/ukf_python/ukf_test_2.py @@ -0,0 +1,40 @@ +import numpy as np + +# Define process noise covariance Q (33x33) +Q = np.zeros((12, 12)) +Q[0:3, 0:3] = np.eye(3)*0.003 # for position +Q[3:6, 3:6] = np.eye(3)*0.003 # for orientation error (represented with Euler angles) +Q[6:9, 6:9] = np.eye(3)*0.002 # for velocity +Q[9:12, 9:12] = np.eye(3)*0.003 # for angular velocity + +G = np.zeros((33, 12)) +G[0:3, 0:3] = np.eye(3) +G[3:6, 3:6] = np.eye(3) +G[6:9, 6:9] = np.eye(3) +G[9:12, 9:12] = np.eye(3) + +GG = G @ Q @ G.T +def fancy_print_matrix(matrix, name="Matrix", precision=4): + """ + Print a matrix with fancy formatting. + + Args: + matrix: numpy array to print + name: name of the matrix to display + precision: number of decimal places to show + """ + print(f"\n{'=' * 50}") + print(f" {name} [{matrix.shape[0]}x{matrix.shape[1]}]") + print(f"{'=' * 50}") + + # Set numpy print options + with np.printoptions(precision=precision, suppress=True, linewidth=100): + # Print each row with custom formatting + for i in range(matrix.shape[0]): + row = ' '.join([f"{x:8.{precision}f}" for x in matrix[i]]) + print(f" {i:2d} | {row}") + + print(f"{'=' * 50}\n") + +# Example usage: +fancy_print_matrix(GG, name="Process Noise Covariance (GQG')", precision=3) \ No newline at end of file diff --git a/navigation/ukf_okid/ukf_python/ukf_utils.py b/navigation/ukf_okid/ukf_python/ukf_utils.py index cb92d9393..7bf7cd4e3 100644 --- a/navigation/ukf_okid/ukf_python/ukf_utils.py +++ b/navigation/ukf_okid/ukf_python/ukf_utils.py @@ -19,6 +19,7 @@ def print_StateQuat(state: StateQuat, name="StateQuat", print_covariance=True): print(f" Orientation: {state.orientation}") print(f" Velocity: {state.velocity}") print(f" Angular Velocity: {state.angular_velocity}") + print(f" okid state: {state.okid_params}") # print(f" okid_params: {state.okid_params}") if print_covariance: print_matrix(state.covariance, "Covariance") From c5f443e6d98bb1efd4da5cf46eeb65f0909f9567 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 17 Apr 2025 16:27:59 +0000 Subject: [PATCH 097/290] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- navigation/eskf/include/eskf/eskf.hpp | 1 - navigation/eskf/include/eskf/typedefs.hpp | 7 +- navigation/eskf/src/eskf.cpp | 11 +- navigation/eskf/src/eskf_ros.cpp | 18 +- navigation/ukf_okid/launch/ukf.launch.py | 3 - navigation/ukf_okid/ukf_python/ukf_okid.py | 29 +- .../ukf_okid/ukf_python/ukf_okid_class.py | 43 ++- navigation/ukf_okid/ukf_python/ukf_ros.py | 102 +++++-- navigation/ukf_okid/ukf_python/ukf_test.py | 268 ++++++++++-------- navigation/ukf_okid/ukf_python/ukf_test_2.py | 22 +- 10 files changed, 293 insertions(+), 211 deletions(-) diff --git a/navigation/eskf/include/eskf/eskf.hpp b/navigation/eskf/include/eskf/eskf.hpp index 80b086d26..5cc3e0708 100644 --- a/navigation/eskf/include/eskf/eskf.hpp +++ b/navigation/eskf/include/eskf/eskf.hpp @@ -25,7 +25,6 @@ class ESKF { const dvl_measurement& dvl_meas); private: - // @brief Predict the nominal state // @param imu_meas: IMU measurement // @param dt: Time step diff --git a/navigation/eskf/include/eskf/typedefs.hpp b/navigation/eskf/include/eskf/typedefs.hpp index fac89baa7..748804be3 100644 --- a/navigation/eskf/include/eskf/typedefs.hpp +++ b/navigation/eskf/include/eskf/typedefs.hpp @@ -5,9 +5,9 @@ #ifndef ESKF_TYPEDEFS_H #define ESKF_TYPEDEFS_H +#include #include #include -#include namespace Eigen { typedef Eigen::Matrix Vector19d; @@ -43,9 +43,7 @@ struct state_quat { Eigen::Vector3d accel_bias = Eigen::Vector3d::Zero(); Eigen::Vector3d gravity = Eigen::Vector3d::Zero(); - state_quat() { - gravity << 0, 0, 9.81; - } + state_quat() { gravity << 0, 0, 9.81; } Eigen::Vector19d as_vector() const { Eigen::Vector19d vec; @@ -94,7 +92,6 @@ struct state_euler { struct imu_measurement { Eigen::Vector3d accel = Eigen::Vector3d::Zero(); Eigen::Vector3d gyro = Eigen::Vector3d::Zero(); - }; struct dvl_measurement { diff --git a/navigation/eskf/src/eskf.cpp b/navigation/eskf/src/eskf.cpp index bbd367c7f..84fb5e8f3 100644 --- a/navigation/eskf/src/eskf.cpp +++ b/navigation/eskf/src/eskf.cpp @@ -107,21 +107,20 @@ Eigen::Matrix3x1d ESKF::calculate_h() { return h; } - void ESKF::nominal_state_discrete(const imu_measurement& imu_meas, const double dt) { Eigen::Vector3d acc = - current_nom_state_.quat.normalized().toRotationMatrix() * imu_meas.accel + + current_nom_state_.quat.normalized().toRotationMatrix() * + imu_meas.accel + current_nom_state_.gravity; - Eigen::Vector3d gyro = imu_meas.gyro * dt/2; + Eigen::Vector3d gyro = imu_meas.gyro * dt / 2; current_nom_state_.pos = current_nom_state_.pos + current_nom_state_.vel * dt + 0.5 * sq(dt) * acc; current_nom_state_.vel = current_nom_state_.vel + dt * acc; - - current_nom_state_.quat = (current_nom_state_.quat * vector3d_to_quaternion(gyro)); - + current_nom_state_.quat = + (current_nom_state_.quat * vector3d_to_quaternion(gyro)); current_nom_state_.quat.normalize(); diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp index a35273103..e805a0c39 100644 --- a/navigation/eskf/src/eskf_ros.cpp +++ b/navigation/eskf/src/eskf_ros.cpp @@ -42,8 +42,9 @@ void ESKFNode::set_subscribers_and_publisher() { void ESKFNode::set_parameters() { std::vector R_imu_correction; this->declare_parameter>("imu_frame"); - R_imu_correction = get_parameter("imu_rotation_matrix").as_double_array(); - R_imu_eskf_ = Eigen::Map>(R_imu_correction.data()); + R_imu_correction = get_parameter("imu_rotation_matrix").as_double_array(); + R_imu_eskf_ = Eigen::Map>( + R_imu_correction.data()); std::vector diag_Q_std; this->declare_parameter>("diag_Q_std"); @@ -81,15 +82,14 @@ void ESKFNode::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { last_imu_time_ = current_time; Eigen::Vector3d raw_accel(msg->linear_acceleration.x, - msg->linear_acceleration.y, - msg->linear_acceleration.z); + msg->linear_acceleration.y, + msg->linear_acceleration.z); imu_meas_.accel = R_imu_eskf_ * raw_accel; - - Eigen::Vector3d raw_gyro(msg->angular_velocity.x, - msg->angular_velocity.y, - msg->angular_velocity.z); - + + Eigen::Vector3d raw_gyro(msg->angular_velocity.x, msg->angular_velocity.y, + msg->angular_velocity.z); + imu_meas_.gyro = R_imu_eskf_ * raw_gyro; std::tie(nom_state_, error_state_) = eskf_->imu_update(imu_meas_, dt); diff --git a/navigation/ukf_okid/launch/ukf.launch.py b/navigation/ukf_okid/launch/ukf.launch.py index 62a439b94..baf6fb645 100644 --- a/navigation/ukf_okid/launch/ukf.launch.py +++ b/navigation/ukf_okid/launch/ukf.launch.py @@ -1,12 +1,9 @@ -import os -from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch_ros.actions import Node def generate_launch_description() -> LaunchDescription: - ukf_node = Node( package="ukf_python", executable="ukf_ros.py", diff --git a/navigation/ukf_okid/ukf_python/ukf_okid.py b/navigation/ukf_okid/ukf_python/ukf_okid.py index af61d1a79..474b94ac6 100644 --- a/navigation/ukf_okid/ukf_python/ukf_okid.py +++ b/navigation/ukf_okid/ukf_python/ukf_okid.py @@ -1,5 +1,4 @@ import numpy as np -from ukf_utils import print_StateQuat from ukf_okid_class import ( MeasModel, StateQuat, @@ -8,7 +7,6 @@ cross_covariance, mean_measurement, mean_set, - process_model, okid_process_model, ) @@ -37,15 +35,17 @@ def generate_delta_matrix(self, n: float) -> np.ndarray: delta (np.ndarray): An n x 2n orthonormal transformation matrix used to generate TUKF sigma points. """ delta = np.zeros((n, 2 * n)) - k = 0.01 #Tuning parameter to ensure pos def + k = 0.01 # Tuning parameter to ensure pos def - for i in range(2 * n): + for i in range(2 * n): for j in range(n // 2): - delta[2 * j + 1, i] = np.sqrt(2) * np.sin((2 * j - 1)) * ((k * np.pi) / n) - delta[2 * j, i] = np.sqrt(2) * np.cos((2 * j - 1)) * ((k * np.pi) / n) - + delta[2 * j + 1, i] = ( + np.sqrt(2) * np.sin(2 * j - 1) * ((k * np.pi) / n) + ) + delta[2 * j, i] = np.sqrt(2) * np.cos(2 * j - 1) * ((k * np.pi) / n) + if (n % 2) == 1: - delta[n-1, i] = (-1)**i + delta[n - 1, i] = (-1) ** i return delta def sigma_points(self, current_state: StateQuat) -> list[StateQuat]: @@ -96,10 +96,15 @@ def measurement_update( self.measurement_updated.measurement = mean_measurement(z_i) - self.measurement_updated.covariance = covariance_measurement(z_i, self.measurement_updated.measurement) + self.measurement_updated.covariance = covariance_measurement( + z_i, self.measurement_updated.measurement + ) self.cross_correlation = cross_covariance( - self.y_i, current_state.as_vector(), z_i, self.measurement_updated.measurement + self.y_i, + current_state.as_vector(), + z_i, + self.measurement_updated.measurement, ) def posteriori_estimate( @@ -109,7 +114,9 @@ def posteriori_estimate( ) -> StateQuat: """Calculates the posteriori estimate using measurement and the prior estimate.""" nu_k = MeasModel() - nu_k.measurement = measurement.measurement - self.measurement_updated.measurement + nu_k.measurement = ( + measurement.measurement - self.measurement_updated.measurement + ) nu_k.covariance = self.measurement_updated.covariance + measurement.covariance K_k = np.dot(self.cross_correlation, np.linalg.inv(nu_k.covariance)) diff --git a/navigation/ukf_okid/ukf_python/ukf_okid_class.py b/navigation/ukf_okid/ukf_python/ukf_okid_class.py index 5558ad754..0b329cb84 100644 --- a/navigation/ukf_okid/ukf_python/ukf_okid_class.py +++ b/navigation/ukf_okid/ukf_python/ukf_okid_class.py @@ -2,13 +2,22 @@ import numpy as np + @dataclass class okid: """A class to represent the parameters for the OKID algorithm.""" - inertia: np.ndarray = field(default_factory=lambda: np.array([0.68, 0.0, 0.0, 0.0, 3.32, 0.0, 0.0, 0.0, 3.34])) - added_mass: np.ndarray = field(default_factory=lambda: np.array([1.0,1.0,1.0,2.0,2.0,2.0])) - damping_linear: np.ndarray = field(default_factory=lambda: np.array([1.0,1.0,1.0,1.0,1.0,1.0])) + inertia: np.ndarray = field( + default_factory=lambda: np.array( + [0.68, 0.0, 0.0, 0.0, 3.32, 0.0, 0.0, 0.0, 3.34] + ) + ) + added_mass: np.ndarray = field( + default_factory=lambda: np.array([1.0, 1.0, 1.0, 2.0, 2.0, 2.0]) + ) + damping_linear: np.ndarray = field( + default_factory=lambda: np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0]) + ) def fill(self, state: np.ndarray) -> None: """Fills the okid_params object with values from a numpy array.""" @@ -27,6 +36,7 @@ def __add__(self, other: 'okid') -> 'okid': result.added_mass = self.added_mass + other.added_mass result.damping_linear = self.damping_linear + other.damping_linear return result + def __sub__(self, other: 'okid') -> 'okid': """Defines the subtraction operation between two okid_params objects.""" result = okid() @@ -34,22 +44,23 @@ def __sub__(self, other: 'okid') -> 'okid': result.added_mass = self.added_mass - other.added_mass result.damping_linear = self.damping_linear - other.damping_linear return result - + def __sub__(self, other: np.ndarray) -> 'okid': - """ Defines sub between okid_params and np.ndarray.""" + """Defines sub between okid_params and np.ndarray.""" result = okid() result.inertia = self.inertia - other[0:9] result.added_mass = self.added_mass - other[9:15] result.damping_linear = self.damping_linear - other[15:] return result - + def __rmul__(self, scalar: float) -> 'okid': """Defines the multiplication operation between a scalar and okid_params object.""" result = okid() result.inertia = scalar * self.inertia result.added_mass = scalar * self.added_mass result.damping_linear = scalar * self.damping_linear - return result + return result + @dataclass class StateQuat: @@ -65,8 +76,15 @@ class StateQuat: def as_vector(self) -> np.ndarray: """Returns the StateVector as a numpy array.""" return np.concatenate( - [self.position, self.orientation, self.velocity, self.angular_velocity, self.okid_params.as_vector()] + [ + self.position, + self.orientation, + self.velocity, + self.angular_velocity, + self.okid_params.as_vector(), + ] ) + def dynamic_part(self) -> np.ndarray: """Returns the dynamic part of the state vector.""" return np.concatenate( @@ -108,7 +126,7 @@ def fill_states(self, state: np.ndarray) -> None: self.velocity = state[7:10] self.angular_velocity = state[10:13] self.okid_params.fill(state[13:]) - + def fill_dynamic_states(self, state: np.ndarray, state_euler: np.ndarray) -> None: """Fills only the dynamic part of the state vector with the values from a numpy array.""" self.position = state[0:3] + state_euler[0:3] @@ -334,6 +352,8 @@ def euler_forward(self) -> StateQuat: + self.state_vector_dot.angular_velocity * self.dt ) return self.state_vector + + @dataclass class okid_process_model: state_vector: StateQuat = field(default_factory=StateQuat) @@ -396,8 +416,8 @@ def D(self) -> np.ndarray: """Calculates the damping matrix.""" D_l = -np.diag(self.damping_linear) - return D_l - + return D_l + def model_prediction(self, state: StateQuat) -> None: """Calculates the model of the system.""" self.state_vector = state @@ -438,6 +458,7 @@ def euler_forward(self) -> None: ) return self.state_vector + def euler_to_quat(euler_angles: np.ndarray) -> np.ndarray: """Converts Euler angles to a quaternion.""" psi, theta, phi = euler_angles diff --git a/navigation/ukf_okid/ukf_python/ukf_ros.py b/navigation/ukf_okid/ukf_python/ukf_ros.py index 66d01175c..a40c2f7c3 100755 --- a/navigation/ukf_okid/ukf_python/ukf_ros.py +++ b/navigation/ukf_okid/ukf_python/ukf_ros.py @@ -1,13 +1,13 @@ #!/usr/bin/env python3 +import numpy as np import rclpy from geometry_msgs.msg import TwistWithCovarianceStamped, WrenchStamped from nav_msgs.msg import Odometry from rclpy.node import Node from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy -from std_msgs.msg import Bool, String from ukf_okid import UKF -from ukf_okid_class import StateQuat, process_model, MeasModel -import numpy as np +from ukf_okid_class import MeasModel, StateQuat, process_model + class UKFNode(Node): def __init__(self): @@ -19,16 +19,24 @@ def __init__(self): depth=1, ) - #subcribers - self.dvl_subscriber = self.create_subscription(TwistWithCovarianceStamped, + # subscribers + self.dvl_subscriber = self.create_subscription( + TwistWithCovarianceStamped, "/orca/twist", self.dvl_callback, qos_profile=best_effort_qos, ) - self.control_input = self.create_subscription(WrenchStamped,"/orca/wrench_input", self.control_callback, qos_profile=best_effort_qos,) + self.control_input = self.create_subscription( + WrenchStamped, + "/orca/wrench_input", + self.control_callback, + qos_profile=best_effort_qos, + ) - self.odom_publish = self.create_publisher(Odometry, "/orca/odometry", qos_profile=best_effort_qos) + self.odom_publish = self.create_publisher( + Odometry, "/orca/odometry", qos_profile=best_effort_qos + ) dt = self.declare_parameter("dt", 0.01).get_parameter_value().double_value self.control_timer = self.create_timer(dt, self.odom_publisher) @@ -38,24 +46,23 @@ def __init__(self): P0 = np.eye(12) * 0.5 self.ukf_model = process_model() self.ukf_model.dt = 0.01 - self.ukf_model.mass_interia_matrix = np.array([ - [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], - ]) + self.ukf_model.mass_interia_matrix = np.array( + [ + [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], + ] + ) self.ukf_model.m = 30.0 self.ukf_model.r_b_bg = np.array([0.01, 0.0, 0.02]) self.ukf_model.inertia = np.diag([0.68, 3.32, 3.34]) self.ukf_model.damping_linear = np.array([0.01] * 6) # self.ukf_model.added_mass = np.diag([1.0, 1.0, 1.0, 2.0, 2.0, 2.0]) - Q = np.diag([0.02, 0.02, 0.02, - 0.02, 0.02, 0.02, - 0.1, 0.1, 0.1, - 0.1, 0.1, 0.1]) + Q = np.diag([0.02, 0.02, 0.02, 0.02, 0.02, 0.02, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) self.ukf = UKF(self.ukf_model, x0, P0, Q) self.ukf_flagg = False @@ -65,18 +72,51 @@ def dvl_callback(self, msg: TwistWithCovarianceStamped): dvl_measurement = MeasModel() # Print received DVL data to console # self.get_logger().info(f"DVL data received: x={msg.twist.twist.linear.x}, y={msg.twist.twist.linear.y}, z={msg.twist.twist.linear.z}") - dvl_measurement.measurement = np.array([msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z]) - dvl_measurement.covariance = np.array([[msg.twist.covariance[0], msg.twist.covariance[1], msg.twist.covariance[3]], - [msg.twist.covariance[6], msg.twist.covariance[7], msg.twist.covariance[8]], - [msg.twist.covariance[12], msg.twist.covariance[13], msg.twist.covariance[14]]]) - + dvl_measurement.measurement = np.array( + [ + msg.twist.twist.linear.x, + msg.twist.twist.linear.y, + msg.twist.twist.linear.z, + ] + ) + dvl_measurement.covariance = np.array( + [ + [ + msg.twist.covariance[0], + msg.twist.covariance[1], + msg.twist.covariance[3], + ], + [ + msg.twist.covariance[6], + msg.twist.covariance[7], + msg.twist.covariance[8], + ], + [ + msg.twist.covariance[12], + msg.twist.covariance[13], + msg.twist.covariance[14], + ], + ] + ) + self.ukf.measurement_update(self.current_state, dvl_measurement) - self.current_state = self.ukf.posteriori_estimate(self.current_state, dvl_measurement) + self.current_state = self.ukf.posteriori_estimate( + self.current_state, dvl_measurement + ) self.ukf_flagg = True - - def control_callback(self, msg:WrenchStamped): + + def control_callback(self, msg: WrenchStamped): # unpack message - control_array = np.array([msg.wrench.force.x, msg.wrench.force.y, msg.wrench.force.z, msg.wrench.torque.x, msg.wrench.torque.y, msg.wrench.torque.z]) + control_array = np.array( + [ + msg.wrench.force.x, + msg.wrench.force.y, + msg.wrench.force.z, + msg.wrench.torque.x, + msg.wrench.torque.y, + msg.wrench.torque.z, + ] + ) self.ukf_model.Control_input = control_array def odom_publisher(self): @@ -90,7 +130,7 @@ def odom_publisher(self): msg.header.stamp = self.get_clock().now().to_msg() msg.header.frame_id = "odom" msg.child_frame_id = "base_link" - + msg.pose.pose.position.x = self.current_state.position[0] msg.pose.pose.position.y = self.current_state.position[1] msg.pose.pose.position.z = self.current_state.position[2] @@ -107,11 +147,13 @@ def odom_publisher(self): self.odom_publish.publish(msg) + def main(args=None): rclpy.init(args=args) ukf_node = UKFNode() rclpy.spin(ukf_node) rclpy.shutdown() + if __name__ == "__main__": - main() \ No newline at end of file + main() diff --git a/navigation/ukf_okid/ukf_python/ukf_test.py b/navigation/ukf_okid/ukf_python/ukf_test.py index 80ca90e86..d80427364 100644 --- a/navigation/ukf_okid/ukf_python/ukf_test.py +++ b/navigation/ukf_okid/ukf_python/ukf_test.py @@ -1,27 +1,31 @@ -import numpy as np import matplotlib.pyplot as plt -from ukf_utils import print_StateQuat +import numpy as np + # Import your classes and functions. # Adjust the import paths as necessary based on your module organization. from ukf_okid_class import ( - StateQuat, MeasModel, - iterative_quaternion_mean_statequat, - mean_set, - mean_measurement, - covariance_set, + StateQuat, covariance_measurement, + covariance_set, cross_covariance, - quaternion_super_product, - quaternion_error, - quat_to_euler, + mean_measurement, + mean_set, quat_norm, + quat_to_euler, + quaternion_super_product, ) + # For testing, define a function to create a StateQuat with small perturbations. -def create_statequat(base_vector, position_perturbation, orientation_perturbation, velocity_perturbation, angular_velocity_perturbation): - """ - Creates a StateQuat object. +def create_statequat( + base_vector, + position_perturbation, + orientation_perturbation, + velocity_perturbation, + angular_velocity_perturbation, +): + """Creates a StateQuat object. - base_vector: 1D numpy array for base state (13 elements: position (3), quaternion (4), velocity (3), angular_velocity (3)) - ..._perturbation: small perturbation vector to be added to each respective component. @@ -37,19 +41,22 @@ def create_statequat(base_vector, position_perturbation, orientation_perturbatio noise_quat = np.array([1.0, 0.0, 0.0, 0.0]) else: noise_axis = orientation_perturbation / noise_angle - noise_quat = np.concatenate(([np.cos(noise_angle/2)], np.sin(noise_angle/2) * noise_axis)) + noise_quat = np.concatenate( + ([np.cos(noise_angle / 2)], np.sin(noise_angle / 2) * noise_axis) + ) state.orientation = quat_norm(quaternion_super_product(base_quat, noise_quat)) state.velocity = base_vector[7:10] + velocity_perturbation state.angular_velocity = base_vector[10:13] + angular_velocity_perturbation - - # For the augmented parameters (OKID parameters), set a 21-element vector: + + # For the augmented parameters (OKID parameters), set a 21-element vector: # 9 for inertia, 6 for added_mass, and 6 for damping_linear. state.okid_params.fill(np.concatenate((np.zeros(9), np.zeros(6), np.zeros(6)))) - + # Set a default covariance (33x33 for the extended state) state.covariance = np.eye(33) * 0.01 return state + # Test functions for state statistics def test_state_statistics(): # Define a base state vector (13 elements: position, quaternion, velocity, angular_velocity) @@ -58,7 +65,7 @@ def test_state_statistics(): base_vector[3:7] = np.array([1.0, 0.0, 0.0, 0.0]) # identity quaternion base_vector[7:10] = np.array([0.1, 0.2, 0.3]) base_vector[10:13] = np.array([0.01, 0.02, 0.03]) - + # Create a list of StateQuat objects with small random perturbations. np.random.seed(42) state_list = [] @@ -68,27 +75,32 @@ def test_state_statistics(): ori_noise = np.random.normal(0, 0.01, 3) vel_noise = np.random.normal(0, 0.02, 3) ang_vel_noise = np.random.normal(0, 0.005, 3) - state_list.append(create_statequat(base_vector, pos_noise, ori_noise, vel_noise, ang_vel_noise)) - + state_list.append( + create_statequat( + base_vector, pos_noise, ori_noise, vel_noise, ang_vel_noise + ) + ) + # Compute the state mean using mean_set. mean_state_vec = mean_set(state_list) print("Computed mean state vector:") print(mean_state_vec) - + # Compute the covariance of the states. cov_state = covariance_set(state_list, mean_state_vec) print("Computed state covariance matrix:") print(cov_state) - + # Check symmetry of the covariance: asym_error = np.linalg.norm(cov_state - cov_state.T) print("Covariance symmetry error (should be near 0):", asym_error) - + # Check eigenvalues for positive semidefiniteness: eigvals = np.linalg.eigvals(cov_state) print("Eigenvalues of state covariance:") print(eigvals) - + + def test_measurement_statistics(): # Create a list of measurement objects (MeasModel) with measurements in R^3. np.random.seed(24) @@ -100,17 +112,17 @@ def test_measurement_statistics(): meas = MeasModel() meas.measurement = base_meas + noise meas_list.append(meas) - + # Compute the measurement mean. mean_meas = mean_measurement(meas_list) print("Computed measurement mean:") print(mean_meas) - + # Compute the measurement covariance. cov_meas = covariance_measurement(meas_list, mean_meas) print("Computed measurement covariance:") print(cov_meas) - + # Check symmetry and eigenvalues. asym_error = np.linalg.norm(cov_meas - cov_meas.T) print("Measurement covariance symmetry error:", asym_error) @@ -118,6 +130,7 @@ def test_measurement_statistics(): print("Eigenvalues of measurement covariance:") print(eigvals) + def test_cross_covariance(): # Create a set of StateQuat and corresponding MeasModel objects. np.random.seed(99) @@ -129,58 +142,50 @@ def test_cross_covariance(): base_vector[3:7] = np.array([1.0, 0.0, 0.0, 0.0]) base_vector[7:10] = np.array([0.05, 0.1, 0.15]) base_vector[10:13] = np.array([0.005, 0.01, 0.015]) - + for _ in range(num): pos_noise = np.random.normal(0, 0.02, 3) ori_noise = np.random.normal(0, 0.005, 3) vel_noise = np.random.normal(0, 0.01, 3) ang_vel_noise = np.random.normal(0, 0.002, 3) - state = create_statequat(base_vector, pos_noise, ori_noise, vel_noise, ang_vel_noise) + state = create_statequat( + base_vector, pos_noise, ori_noise, vel_noise, ang_vel_noise + ) state_list.append(state) - + # Generate a measurement from each state (e.g., state velocity plus noise). meas = MeasModel() meas.measurement = state.velocity + np.random.normal(0, 0.01, 3) meas_list.append(meas) - + # Compute the state mean and measurement mean as vectors. mean_state_vec = mean_set(state_list) mean_meas = mean_measurement(meas_list) - + cross_cov = cross_covariance(state_list, mean_state_vec, meas_list, mean_meas) print("Computed cross-covariance between state and measurement:") print(cross_cov) + import time -import numpy as np -import matplotlib.pyplot as plt + +from ukf_okid import UKF # Import your classes and functions. from ukf_okid_class import ( - StateQuat, - MeasModel, - iterative_quaternion_mean_statequat, - mean_set, - mean_measurement, - covariance_set, - covariance_measurement, - cross_covariance, - quaternion_super_product, - quaternion_error, - quat_norm, -) -from ukf_okid import UKF -from ukf_okid_class import process_model, okid_process_model # Your process model classes + okid_process_model, + process_model, +) # Your process model classes + ############################################ # Helper function to create a StateQuat with perturbations. ############################################ def create_statequat(base_vector, pos_noise, ori_noise, vel_noise, ang_vel_noise): - """ - Create a StateQuat object from a base vector (13 elements: + """Create a StateQuat object from a base vector (13 elements: position (3), quaternion (4), velocity (3), angular_velocity (3)) plus additive noise on each component. - + For the OKID parameters, we assume a 21-element vector: - first 9: inertia, - next 6: added_mass, @@ -194,49 +199,60 @@ def create_statequat(base_vector, pos_noise, ori_noise, vel_noise, ang_vel_noise noise_quat = np.array([1.0, 0.0, 0.0, 0.0]) else: noise_axis = ori_noise / noise_angle - noise_quat = np.concatenate(([np.cos(noise_angle/2)], - np.sin(noise_angle/2) * noise_axis)) + noise_quat = np.concatenate( + ([np.cos(noise_angle / 2)], np.sin(noise_angle / 2) * noise_axis) + ) state.orientation = quat_norm(quaternion_super_product(base_quat, noise_quat)) state.velocity = base_vector[7:10] + vel_noise state.angular_velocity = base_vector[10:13] + ang_vel_noise # Set OKID parameters to exactly 21 elements (9,6,6) - state.okid_params.fill(np.concatenate((np.array([0.0, 0.0, 0.3, 0.0, 0.0, 3.3, 0.0, 0.0, 3.3]), np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0]), np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0])))) + state.okid_params.fill( + np.concatenate( + ( + np.array([0.0, 0.0, 0.3, 0.0, 0.0, 3.3, 0.0, 0.0, 3.3]), + np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0]), + np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0]), + ) + ) + ) # Set an initial covariance (33x33) for the full augmented state. state.covariance = np.eye(33) * 0.02 return state + ############################################ # Full Filter Simulation Test ############################################ def run_ukf_simulation(): - dt = 0.01 # Time step for simulation [s] - simulation_time = 10 # Total simulation time in seconds + dt = 0.01 # Time step for simulation [s] + simulation_time = 10 # Total simulation time in seconds num_steps = int(simulation_time / dt) - + # Define a base state vector (13 elements: pos, quat, vel, ang_vel) base_vector = np.zeros(13) - base_vector[0:3] = np.array([0.0, 0.0, 0.0]) + base_vector[0:3] = np.array([0.0, 0.0, 0.0]) base_vector[3:7] = np.array([1.0, 0.0, 0.0, 0.0]) # identity quaternion - base_vector[7:10] = np.array([0.1, 0.0, 0.0]) # small velocity in x + base_vector[7:10] = np.array([0.1, 0.0, 0.0]) # small velocity in x base_vector[10:13] = np.array([0.0, 0.0, 0.0]) - + # Define initial covariance for state (33x33) P0 = np.eye(33) P0[0:3, 0:3] = np.eye(3) * 0.01 # position P0[3:6, 3:6] = np.eye(3) * 0.01 # orientation error (quaternion) P0[6:9, 6:9] = np.eye(3) * 0.01 # velocity - P0[9:12, 9:12] = np.eye(3) * 0.01 # angular velocity - P0[12:33, 12:33] = np.eye(21) * 0.001 # OKID parameters + P0[9:12, 9:12] = np.eye(3) * 0.01 # angular velocity + P0[12:33, 12:33] = np.eye(21) * 0.001 # OKID parameters # Define process noise covariance Q (33x33) Q = np.zeros((33, 33)) - Q[0:3, 0:3] = np.eye(3)*0.001 # for position - Q[3:6, 3:6] = np.eye(3)*0.001 # for orientation error (represented with Euler angles) - Q[6:9, 6:9] = np.eye(3)*0.001 # for velocity - Q[9:12, 9:12] = np.eye(3)*0.001 # for angular velocity - Q[12:33, 12:33] = np.eye(21) * 0.001 # OKID parameters - + Q[0:3, 0:3] = np.eye(3) * 0.001 # for position + Q[3:6, 3:6] = ( + np.eye(3) * 0.001 + ) # for orientation error (represented with Euler angles) + Q[6:9, 6:9] = np.eye(3) * 0.001 # for velocity + Q[9:12, 9:12] = np.eye(3) * 0.001 # for angular velocity + Q[12:33, 12:33] = np.eye(21) * 0.001 # OKID parameters G = np.zeros((33, 12)) G[0:3, 0:3] = np.eye(3) @@ -250,19 +266,21 @@ def run_ukf_simulation(): # Create a simulation process model and an independent UKF process model. sim_model = process_model() sim_model.dt = dt - sim_model.mass_interia_matrix = np.array([ - [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], - ]) + sim_model.mass_interia_matrix = np.array( + [ + [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], + ] + ) sim_model.m = 30.0 sim_model.r_b_bg = np.array([0.01, 0.0, 0.02]) sim_model.inertia = np.diag([0.68, 3.32, 3.34]) - sim_model.damping_linear = np.array([0.1]*6) - sim_model.added_mass = np.array([1.0,1.0,1.0,2.0,2.0,2.0]) + sim_model.damping_linear = np.array([0.1] * 6) + sim_model.added_mass = np.array([1.0, 1.0, 1.0, 2.0, 2.0, 2.0]) # UKF process model copy: ukf_model = okid_process_model() @@ -275,18 +293,14 @@ def run_ukf_simulation(): ukf_model.added_mass = sim_model.added_mass.copy() # Initialize true state and filter state. - true_state = create_statequat(base_vector, - np.zeros(3), - np.zeros(3), - np.zeros(3), - np.zeros(3)) + true_state = create_statequat( + base_vector, np.zeros(3), np.zeros(3), np.zeros(3), np.zeros(3) + ) true_state.covariance = P0.copy() - - filter_state = create_statequat(base_vector, - np.zeros(3), - np.zeros(3), - np.zeros(3), - np.zeros(3)) + + filter_state = create_statequat( + base_vector, np.zeros(3), np.zeros(3), np.zeros(3), np.zeros(3) + ) filter_state.covariance = P0.copy() # Initialize measurement model (for example, measuring velocity only) @@ -307,14 +321,16 @@ def run_ukf_simulation(): # Control input function (example: oscillatory in all directions) def control_input(t): - return np.array([ - 2*np.sin(t), - 2*np.sin(t+0.5), - 2*np.sin(t+1.0), - 0.2*np.cos(t), - 0.2*np.cos(t+0.5), - 0.2*np.cos(t+1.0) - ]) + return np.array( + [ + 2 * np.sin(t), + 2 * np.sin(t + 0.5), + 2 * np.sin(t + 1.0), + 0.2 * np.cos(t), + 0.2 * np.cos(t + 0.5), + 0.2 * np.cos(t + 1.0), + ] + ) # Set previous states. sim_model.state_vector_prev = true_state @@ -325,19 +341,19 @@ def control_input(t): # Lists for timing diagnostics. ukf_transform_times = [] ukf_update_times = [] - + # Simulation loop. for i in range(num_steps): - t_current = i*dt - + t_current = i * dt + # Update control inputs. sim_model.Control_input = control_input(t_current) ukf_model.Control_input = control_input(t_current) - + # Propagate true state using the simulation model. sim_model.model_prediction(true_state) true_state = sim_model.euler_forward() - + # Create a measurement from true state. # Here we assume we measure velocity plus noise. meas_noise = np.random.normal(0, 0.01, 3) @@ -347,7 +363,7 @@ def control_input(t): start = time.time() filter_state = ukf.unscented_transform(filter_state) ukf_transform_times.append(time.time() - start) - + # UKF measurement update every few steps. if i % 5 == 0: try: @@ -357,8 +373,10 @@ def control_input(t): ukf_update_times.append(time.time() - start) except np.linalg.LinAlgError: # If matrix is not PD, add jitter. - filter_state.covariance += np.eye(filter_state.covariance.shape[0])*1e-6 - + filter_state.covariance += ( + np.eye(filter_state.covariance.shape[0]) * 1e-6 + ) + # Store true and estimated state for diagnostics. pos_true_hist[i, :] = true_state.position pos_est_hist[i, :] = filter_state.position @@ -368,7 +386,7 @@ def control_input(t): # Assumes you have a function quat_to_euler. euler_true_hist[i, :] = quat_to_euler(true_state.orientation) euler_est_hist[i, :] = quat_to_euler(filter_state.orientation) - + # Update previous states. sim_model.state_vector_prev = true_state ukf_model.state_vector_prev = filter_state @@ -376,7 +394,7 @@ def control_input(t): # Print timing diagnostics. print("Average unscented transform time:", np.mean(ukf_transform_times)) print("Average measurement update time:", np.mean(ukf_update_times)) - + # Compute error metrics. pos_error = np.linalg.norm(pos_true_hist - pos_est_hist, axis=1) vel_error = np.linalg.norm(vel_true_hist - vel_est_hist, axis=1) @@ -384,31 +402,31 @@ def control_input(t): print("Average position error:", np.mean(pos_error)) print("Average velocity error:", np.mean(vel_error)) print("Average orientation (Euler) error:", np.mean(euler_error)) - + # Plot estimated vs true trajectory (positions). - plt.figure(figsize=(10,8)) - plt.subplot(3,1,1) - plt.plot(time_array, pos_true_hist[:,0], label="True X") - plt.plot(time_array, pos_est_hist[:,0], label="Est X", linestyle="--") + plt.figure(figsize=(10, 8)) + plt.subplot(3, 1, 1) + plt.plot(time_array, pos_true_hist[:, 0], label="True X") + plt.plot(time_array, pos_est_hist[:, 0], label="Est X", linestyle="--") plt.legend() plt.title("Position X") - - plt.subplot(3,1,2) - plt.plot(time_array, pos_true_hist[:,1], label="True Y") - plt.plot(time_array, pos_est_hist[:,1], label="Est Y", linestyle="--") + + plt.subplot(3, 1, 2) + plt.plot(time_array, pos_true_hist[:, 1], label="True Y") + plt.plot(time_array, pos_est_hist[:, 1], label="Est Y", linestyle="--") plt.legend() plt.title("Position Y") - - plt.subplot(3,1,3) - plt.plot(time_array, pos_true_hist[:,2], label="True Z") - plt.plot(time_array, pos_est_hist[:,2], label="Est Z", linestyle="--") + + plt.subplot(3, 1, 3) + plt.plot(time_array, pos_true_hist[:, 2], label="True Z") + plt.plot(time_array, pos_est_hist[:, 2], label="Est Z", linestyle="--") plt.legend() plt.title("Position Z") plt.tight_layout() plt.show() - + # Plot errors. - plt.figure(figsize=(10,4)) + plt.figure(figsize=(10, 4)) plt.plot(time_array, pos_error, label="Position Error") plt.plot(time_array, vel_error, label="Velocity Error") plt.plot(time_array, euler_error, label="Euler Angle Error") @@ -418,6 +436,7 @@ def control_input(t): plt.ylabel("Error magnitude") plt.show() + # You can also test the individual statistics functions separately: def run_diagnostics(): print("Testing state mean and covariance computation:") @@ -429,12 +448,11 @@ def run_diagnostics(): print("\nTesting cross-covariance computation:") test_cross_covariance() + if __name__ == '__main__': # First, run the diagnostics on the mean/covariance functions. run_diagnostics() - + # Then run the full UKF simulation test. print("\nRunning full UKF simulation test:") run_ukf_simulation() - - diff --git a/navigation/ukf_okid/ukf_python/ukf_test_2.py b/navigation/ukf_okid/ukf_python/ukf_test_2.py index bf58b7a1a..d3a75313b 100644 --- a/navigation/ukf_okid/ukf_python/ukf_test_2.py +++ b/navigation/ukf_okid/ukf_python/ukf_test_2.py @@ -2,10 +2,10 @@ # Define process noise covariance Q (33x33) Q = np.zeros((12, 12)) -Q[0:3, 0:3] = np.eye(3)*0.003 # for position -Q[3:6, 3:6] = np.eye(3)*0.003 # for orientation error (represented with Euler angles) -Q[6:9, 6:9] = np.eye(3)*0.002 # for velocity -Q[9:12, 9:12] = np.eye(3)*0.003 # for angular velocity +Q[0:3, 0:3] = np.eye(3) * 0.003 # for position +Q[3:6, 3:6] = np.eye(3) * 0.003 # for orientation error (represented with Euler angles) +Q[6:9, 6:9] = np.eye(3) * 0.002 # for velocity +Q[9:12, 9:12] = np.eye(3) * 0.003 # for angular velocity G = np.zeros((33, 12)) G[0:3, 0:3] = np.eye(3) @@ -14,10 +14,11 @@ G[9:12, 9:12] = np.eye(3) GG = G @ Q @ G.T + + def fancy_print_matrix(matrix, name="Matrix", precision=4): - """ - Print a matrix with fancy formatting. - + """Print a matrix with fancy formatting. + Args: matrix: numpy array to print name: name of the matrix to display @@ -26,15 +27,16 @@ def fancy_print_matrix(matrix, name="Matrix", precision=4): print(f"\n{'=' * 50}") print(f" {name} [{matrix.shape[0]}x{matrix.shape[1]}]") print(f"{'=' * 50}") - + # Set numpy print options with np.printoptions(precision=precision, suppress=True, linewidth=100): # Print each row with custom formatting for i in range(matrix.shape[0]): row = ' '.join([f"{x:8.{precision}f}" for x in matrix[i]]) print(f" {i:2d} | {row}") - + print(f"{'=' * 50}\n") + # Example usage: -fancy_print_matrix(GG, name="Process Noise Covariance (GQG')", precision=3) \ No newline at end of file +fancy_print_matrix(GG, name="Process Noise Covariance (GQG')", precision=3) From 3bf8b94ad1bab19771e165d02a658c79c339d183 Mon Sep 17 00:00:00 2001 From: Talha Nauman Choudhry Date: Thu, 8 May 2025 20:19:53 +0200 Subject: [PATCH 098/290] feat: added NIS plots --- navigation/eskf/config/eskf_params.yaml | 2 +- navigation/eskf/include/eskf/eskf.hpp | 8 ++++++++ navigation/eskf/include/eskf/eskf_ros.hpp | 3 +++ navigation/eskf/src/eskf.cpp | 6 ++++++ navigation/eskf/src/eskf_ros.cpp | 9 +++++++-- 5 files changed, 25 insertions(+), 3 deletions(-) diff --git a/navigation/eskf/config/eskf_params.yaml b/navigation/eskf/config/eskf_params.yaml index 639af1fc9..961575447 100644 --- a/navigation/eskf/config/eskf_params.yaml +++ b/navigation/eskf/config/eskf_params.yaml @@ -5,4 +5,4 @@ eskf_node: odom_topic: odom diag_Q_std: [0.027293, 0.028089, 0.029067, 0.00255253, 0.00270035, 0.00280294, 0.000001, 0.000001, 0.000001, 0.00001, 0.00001, 0.00001] diag_p_init: [1.0, 1.0, 0.5, 0.5, 0.5, 1.0, 0.1, 0.1, 0.1, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001] - imu_frame: [0, 0, -1, 0, -1, 0, -1, 0, 0 ] + imu_frame: [0.0, 0.0, -1.0, 0.0, -1.0, 0.0, -1.0, 0.0, 0.0] diff --git a/navigation/eskf/include/eskf/eskf.hpp b/navigation/eskf/include/eskf/eskf.hpp index 5cc3e0708..214f462c4 100644 --- a/navigation/eskf/include/eskf/eskf.hpp +++ b/navigation/eskf/include/eskf/eskf.hpp @@ -24,6 +24,9 @@ class ESKF { std::pair dvl_update( const dvl_measurement& dvl_meas); + // NIS + double NIS_; + private: // @brief Predict the nominal state // @param imu_meas: IMU measurement @@ -39,6 +42,11 @@ class ESKF { void error_state_prediction(const imu_measurement& imu_meas, const double dt); + // @brief Calculate the NIS + // @param innovation: Innovation vector + // @param S: Innovation covariance matrix + void NIS(const Eigen::Vector3d& innovation, const Eigen::Matrix3d& S); + // @brief Update the error state // @param dvl_meas: DVL measurement void measurement_update(const dvl_measurement& dvl_meas); diff --git a/navigation/eskf/include/eskf/eskf_ros.hpp b/navigation/eskf/include/eskf/eskf_ros.hpp index e0b777c86..44743fd75 100644 --- a/navigation/eskf/include/eskf/eskf_ros.hpp +++ b/navigation/eskf/include/eskf/eskf_ros.hpp @@ -8,6 +8,7 @@ #include #include #include +#include #include #include #include @@ -45,6 +46,8 @@ class ESKFNode : public rclcpp::Node { rclcpp::Publisher::SharedPtr odom_pub_; + rclcpp::Publisher::SharedPtr nis_pub_; + std::chrono::milliseconds time_step; rclcpp::TimerBase::SharedPtr odom_pub_timer_; diff --git a/navigation/eskf/src/eskf.cpp b/navigation/eskf/src/eskf.cpp index 84fb5e8f3..c02bf57df 100644 --- a/navigation/eskf/src/eskf.cpp +++ b/navigation/eskf/src/eskf.cpp @@ -159,6 +159,11 @@ void ESKF::error_state_prediction(const imu_measurement& imu_meas, A_d * current_error_state_.covariance * A_d.transpose() + GQG_d; } +void ESKF::NIS(const Eigen::Vector3d& innovation, const Eigen::Matrix3d& S) { + Eigen::Matrix3d S_inv = S.inverse(); + NIS_ = innovation.transpose() * S_inv * innovation; +} + void ESKF::measurement_update(const dvl_measurement& dvl_meas) { Eigen::Matrix3x18d H = calculate_h_jacobian(); Eigen::Matrix18d P = current_error_state_.covariance; @@ -167,6 +172,7 @@ void ESKF::measurement_update(const dvl_measurement& dvl_meas) { Eigen::Matrix3d S = H * P * H.transpose() + R; Eigen::Matrix18x3d K = P * H.transpose() * S.inverse(); Eigen::Vector3d innovation = dvl_meas.vel - calculate_h(); + NIS(innovation, S); current_error_state_.set_from_vector(K * innovation); Eigen::Matrix18d I_KH = Eigen::Matrix18d::Identity() - K * H; diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp index e805a0c39..9d46969d9 100644 --- a/navigation/eskf/src/eskf_ros.cpp +++ b/navigation/eskf/src/eskf_ros.cpp @@ -37,12 +37,14 @@ void ESKFNode::set_subscribers_and_publisher() { std::string odom_topic = this->get_parameter("odom_topic").as_string(); odom_pub_ = this->create_publisher( odom_topic, qos_sensor_data); + + nis_pub_ = create_publisher("dvl/nis", 10); } void ESKFNode::set_parameters() { std::vector R_imu_correction; this->declare_parameter>("imu_frame"); - R_imu_correction = get_parameter("imu_rotation_matrix").as_double_array(); + R_imu_correction = get_parameter("imu_frame").as_double_array(); R_imu_eskf_ = Eigen::Map>( R_imu_correction.data()); @@ -53,7 +55,6 @@ void ESKFNode::set_parameters() { Eigen::Matrix12d Q; Q.setZero(); - spdlog::info("Q diagonal: {}", diag_Q_std[0]); Q.diagonal() << sq(diag_Q_std[0]), sq(diag_Q_std[1]), sq(diag_Q_std[2]), sq(diag_Q_std[3]), sq(diag_Q_std[4]), sq(diag_Q_std[5]), sq(diag_Q_std[6]), sq(diag_Q_std[7]), sq(diag_Q_std[8]), @@ -106,6 +107,10 @@ void ESKFNode::dvl_callback( msg->twist.covariance[14]; std::tie(nom_state_, error_state_) = eskf_->dvl_update(dvl_meas_); + + std_msgs::msg::Float64 nis_msg; + nis_msg.data = eskf_->NIS_; + nis_pub_->publish(nis_msg); } void ESKFNode::publish_odom() { From 10c8ec90a0ae26697007fad6d5f7dd3876285f7c Mon Sep 17 00:00:00 2001 From: Talha Nauman Choudhry Date: Wed, 14 May 2025 00:00:00 +0200 Subject: [PATCH 099/290] fix: issues in innovation and jacobian of the measurement --- navigation/eskf/CMakeLists.txt | 2 + navigation/eskf/config/eskf_params.yaml | 6 +- navigation/eskf/include/eskf/eskf.hpp | 8 ++ navigation/eskf/include/eskf/eskf_ros.hpp | 21 +++- navigation/eskf/include/eskf/eskf_utils.hpp | 3 + navigation/eskf/include/eskf/typedefs.hpp | 18 +++- navigation/eskf/src/eskf.cpp | 103 ++++++++++++-------- navigation/eskf/src/eskf_ros.cpp | 67 +++++++++++-- navigation/eskf/src/eskf_utils.cpp | 5 + 9 files changed, 172 insertions(+), 61 deletions(-) diff --git a/navigation/eskf/CMakeLists.txt b/navigation/eskf/CMakeLists.txt index 6c8167609..c431c235f 100644 --- a/navigation/eskf/CMakeLists.txt +++ b/navigation/eskf/CMakeLists.txt @@ -18,6 +18,7 @@ find_package(tf2 REQUIRED) find_package(vortex_msgs REQUIRED) find_package(spdlog REQUIRED) find_package(fmt REQUIRED) +find_package(stonefish_ros2 REQUIRED) if(NOT DEFINED EIGEN3_INCLUDE_DIR) set(EIGEN3_INCLUDE_DIR ${EIGEN3_INCLUDE_DIRS}) @@ -42,6 +43,7 @@ ament_target_dependencies(eskf_node vortex_msgs spdlog fmt + stonefish_ros2 ) target_link_libraries(eskf_node diff --git a/navigation/eskf/config/eskf_params.yaml b/navigation/eskf/config/eskf_params.yaml index 961575447..98be8e789 100644 --- a/navigation/eskf/config/eskf_params.yaml +++ b/navigation/eskf/config/eskf_params.yaml @@ -1,8 +1,8 @@ eskf_node: ros__parameters: imu_topic: imu/data_raw - dvl_topic: /orca/twist + dvl_topic: /dvl/sim odom_topic: odom - diag_Q_std: [0.027293, 0.028089, 0.029067, 0.00255253, 0.00270035, 0.00280294, 0.000001, 0.000001, 0.000001, 0.00001, 0.00001, 0.00001] + diag_Q_std: [0.05, 0.05, 0.05, 0.00001, 0.00001, 0.00001, 0.000001, 0.000000001, 0.000000001, 0.000000001, 0.00000001, 0.00000001] diag_p_init: [1.0, 1.0, 0.5, 0.5, 0.5, 1.0, 0.1, 0.1, 0.1, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001] - imu_frame: [0.0, 0.0, -1.0, 0.0, -1.0, 0.0, -1.0, 0.0, 0.0] + imu_frame: [0.0, 0.0, -1.0, 0.0, -1.0, 0.0, -1.0, 0.0, 0.0] diff --git a/navigation/eskf/include/eskf/eskf.hpp b/navigation/eskf/include/eskf/eskf.hpp index 214f462c4..6d8d33bd5 100644 --- a/navigation/eskf/include/eskf/eskf.hpp +++ b/navigation/eskf/include/eskf/eskf.hpp @@ -27,6 +27,12 @@ class ESKF { // NIS double NIS_; + // NEES + double NEES_; + + // ground truth + state_quat ground_truth_; + private: // @brief Predict the nominal state // @param imu_meas: IMU measurement @@ -47,6 +53,8 @@ class ESKF { // @param S: Innovation covariance matrix void NIS(const Eigen::Vector3d& innovation, const Eigen::Matrix3d& S); + void NEES(); + // @brief Update the error state // @param dvl_meas: DVL measurement void measurement_update(const dvl_measurement& dvl_meas); diff --git a/navigation/eskf/include/eskf/eskf_ros.hpp b/navigation/eskf/include/eskf/eskf_ros.hpp index 44743fd75..8cb32b173 100644 --- a/navigation/eskf/include/eskf/eskf_ros.hpp +++ b/navigation/eskf/include/eskf/eskf_ros.hpp @@ -7,11 +7,11 @@ #include #include #include +#include #include #include #include #include -#include #include "eskf/eskf.hpp" #include "eskf/typedefs.hpp" #include "spdlog/spdlog.h" @@ -21,14 +21,18 @@ class ESKFNode : public rclcpp::Node { explicit ESKFNode(); private: + + void pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); + + void twist_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); + // @brief Callback function for the imu topic // @param msg: Imu message containing the imu data void imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg); // @brief Callback function for the dvl topic // @param msg: TwistWithCovarianceStamped message containing the dvl data - void dvl_callback( - const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); + void dvl_callback(const stonefish_ros2::msg::DVL::SharedPtr msg); // @brief Publish the odometry message void publish_odom(); @@ -41,19 +45,26 @@ class ESKFNode : public rclcpp::Node { rclcpp::Subscription::SharedPtr imu_sub_; - rclcpp::Subscription< - geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr dvl_sub_; + rclcpp::Subscription::SharedPtr dvl_sub_; + + rclcpp::Subscription::SharedPtr pose_sub_; + + rclcpp::Subscription::SharedPtr twist_sub_; rclcpp::Publisher::SharedPtr odom_pub_; rclcpp::Publisher::SharedPtr nis_pub_; + rclcpp::Publisher::SharedPtr nees_pub_; + std::chrono::milliseconds time_step; rclcpp::TimerBase::SharedPtr odom_pub_timer_; state_quat nom_state_; + state_quat g_truth_; + state_euler error_state_; imu_measurement imu_meas_; diff --git a/navigation/eskf/include/eskf/eskf_utils.hpp b/navigation/eskf/include/eskf/eskf_utils.hpp index 100f7673d..0cc2f6887 100644 --- a/navigation/eskf/include/eskf/eskf_utils.hpp +++ b/navigation/eskf/include/eskf/eskf_utils.hpp @@ -3,11 +3,14 @@ #include "eigen3/Eigen/Dense" #include "eskf/typedefs.hpp" +#include Eigen::Matrix3d skew(const Eigen::Vector3d& v); double sq(const double& value); +double ssa(const double& angle); + Eigen::Quaterniond vector3d_to_quaternion(const Eigen::Vector3d& vector); Eigen::Quaterniond euler_to_quaternion(const Eigen::Vector3d& euler); diff --git a/navigation/eskf/include/eskf/typedefs.hpp b/navigation/eskf/include/eskf/typedefs.hpp index 748804be3..ab86b0a6c 100644 --- a/navigation/eskf/include/eskf/typedefs.hpp +++ b/navigation/eskf/include/eskf/typedefs.hpp @@ -48,7 +48,22 @@ struct state_quat { Eigen::Vector19d as_vector() const { Eigen::Vector19d vec; vec << pos, vel, quat.w(), quat.x(), quat.y(), quat.z(), gyro_bias, - accel_bias; + accel_bias, gravity; + return vec; + } + + Eigen::Vector18d nees_error(const state_quat& other) const { + Eigen::Vector18d vec; + Eigen::Vector3d euler_diff; + + euler_diff = (quat * other.quat.inverse()).toRotationMatrix().eulerAngles(0, 1, 2); + + vec << pos - other.pos, + vel - other.vel, + euler_diff, + gyro_bias - other.gyro_bias, + accel_bias - other.accel_bias, + gravity - other.gravity; return vec; } @@ -59,6 +74,7 @@ struct state_quat { diff.quat = quat * other.quat.inverse(); diff.gyro_bias = gyro_bias - other.gyro_bias; diff.accel_bias = accel_bias - other.accel_bias; + diff.gravity = gravity - other.gravity; return diff; } }; diff --git a/navigation/eskf/src/eskf.cpp b/navigation/eskf/src/eskf.cpp index c02bf57df..9325378bb 100644 --- a/navigation/eskf/src/eskf.cpp +++ b/navigation/eskf/src/eskf.cpp @@ -5,6 +5,7 @@ #include #include "eskf/eskf_utils.hpp" #include "eskf/typedefs.hpp" +#include "iostream" ESKF::ESKF(const eskf_params& params) : Q_(params.Q) {} @@ -50,37 +51,38 @@ Eigen::Matrix3x19d ESKF::calculate_hx() { Eigen::Vector3d v_n = current_nom_state_.vel; - Hx.block<3, 3>(0, 3) = R_bn; + Hx.block<3, 3>(0, 3) = R_bn.transpose(); - Eigen::Matrix dR_dq; double qw = q.w(); double qx = q.x(); double qy = q.y(); double qz = q.z(); - Eigen::Vector3d epsilon(qx, qy, qz); + Eigen::Matrix3d I3 = Eigen::Matrix3d::Identity(); - Eigen::Vector3d e_1(1, 0, 0); - Eigen::Vector3d e_2(0, 1, 0); - Eigen::Vector3d e_3(0, 0, 1); + Eigen::Vector3d eps(qx, qy, qz); - dR_dq.col(0) = - ((4 * qw * Eigen::Matrix3d::Identity()) + (2 * skew(epsilon))) * v_n; + Eigen::Matrix3d dR_deta = 2*qw * I3 - 2*skew(eps); - dR_dq.col(1) = 2 * - ((e_1 * epsilon.transpose()) + (epsilon * e_1.transpose()) + - (qw * skew(e_1))) * - v_n; + Eigen::Vector3d e1_vec(1,0,0), e2_vec(0,1,0), e3_vec(0,0,1); - dR_dq.col(2) = 2 * - ((e_2 * epsilon.transpose()) + (epsilon * e_2.transpose()) + - (qw * skew(e_2))) * - v_n; + Eigen::Matrix3d dR_dqx = -2*qx*I3 + + 2*(e1_vec*eps.transpose() + eps*e1_vec.transpose()) + - 2*qw*skew(e1_vec); - dR_dq.col(3) = 2 * - ((e_3 * epsilon.transpose()) + (epsilon * e_3.transpose()) + - (qw * skew(e_3))) * - v_n; + Eigen::Matrix3d dR_dqy = -2*qy*I3 + + 2*(e2_vec*eps.transpose() + eps*e2_vec.transpose()) + - 2*qw*skew(e2_vec); + + Eigen::Matrix3d dR_dqz = -2*qz*I3 + + 2*(e3_vec*eps.transpose() + eps*e3_vec.transpose()) + - 2*qw*skew(e3_vec); + + Eigen::Matrix dR_dq; + dR_dq.col(0) = dR_deta * v_n; + dR_dq.col(1) = dR_dqx * v_n; + dR_dq.col(2) = dR_dqy * v_n; + dR_dq.col(3) = dR_dqz * v_n; Hx.block<3, 4>(0, 6) = dR_dq; @@ -100,7 +102,7 @@ Eigen::Matrix3x18d ESKF::calculate_h_jacobian() { Eigen::Matrix3x1d ESKF::calculate_h() { Eigen::Matrix3x1d h; Eigen::Matrix3d R_bn = - current_nom_state_.quat.normalized().toRotationMatrix(); + current_nom_state_.quat.normalized().toRotationMatrix().transpose(); h = R_bn * current_nom_state_.vel; @@ -109,19 +111,13 @@ Eigen::Matrix3x1d ESKF::calculate_h() { void ESKF::nominal_state_discrete(const imu_measurement& imu_meas, const double dt) { - Eigen::Vector3d acc = - current_nom_state_.quat.normalized().toRotationMatrix() * - imu_meas.accel + - current_nom_state_.gravity; - Eigen::Vector3d gyro = imu_meas.gyro * dt / 2; - - current_nom_state_.pos = current_nom_state_.pos + - current_nom_state_.vel * dt + 0.5 * sq(dt) * acc; - current_nom_state_.vel = current_nom_state_.vel + dt * acc; + Eigen::Vector3d acc = current_nom_state_.quat.normalized().toRotationMatrix() * (imu_meas.accel - current_nom_state_.accel_bias) + current_nom_state_.gravity; + Eigen::Vector3d gyro = (imu_meas.gyro - current_nom_state_.gyro_bias) * dt; - current_nom_state_.quat = - (current_nom_state_.quat * vector3d_to_quaternion(gyro)); + current_nom_state_.pos = current_nom_state_.pos + current_nom_state_.vel * dt + 0.5 * sq(dt) * acc; + current_nom_state_.vel = current_nom_state_.vel + dt * acc; + current_nom_state_.quat = (current_nom_state_.quat * vector3d_to_quaternion(gyro)); current_nom_state_.quat.normalize(); current_nom_state_.gyro_bias = current_nom_state_.gyro_bias; @@ -132,8 +128,8 @@ void ESKF::nominal_state_discrete(const imu_measurement& imu_meas, void ESKF::error_state_prediction(const imu_measurement& imu_meas, const double dt) { Eigen::Matrix3d R = current_nom_state_.quat.normalized().toRotationMatrix(); - Eigen::Vector3d acc = imu_meas.accel; - Eigen::Vector3d gyro = imu_meas.gyro; + Eigen::Vector3d acc = (imu_meas.accel - current_nom_state_.accel_bias); + Eigen::Vector3d gyro = (imu_meas.gyro - current_nom_state_.gyro_bias); Eigen::Matrix18d A_c = Eigen::Matrix18d::Zero(); A_c.block<3, 3>(0, 3) = Eigen::Matrix3d::Identity(); @@ -164,6 +160,30 @@ void ESKF::NIS(const Eigen::Vector3d& innovation, const Eigen::Matrix3d& S) { NIS_ = innovation.transpose() * S_inv * innovation; } +void ESKF::NEES() { + + Eigen::Vector18d error_state = current_nom_state_.nees_error(ground_truth_); + + // Use SVD-based pseudo-inverse for better numerical stability + Eigen::JacobiSVD svd(current_error_state_.covariance, + Eigen::ComputeThinU | Eigen::ComputeThinV); + const double epsilon = 1e-10; // Threshold for singular values + Eigen::VectorXd singular_values = svd.singularValues(); + Eigen::VectorXd singular_values_inv(singular_values.size()); + + for (int i = 0; i < singular_values.size(); ++i) { + if (singular_values(i) > epsilon) { + singular_values_inv(i) = 1.0 / singular_values(i); + } else { + singular_values_inv(i) = 0.0; + } + } + + Eigen::MatrixXd cov_inv = svd.matrixV() * singular_values_inv.asDiagonal() * svd.matrixU().transpose(); + + NEES_ = error_state.transpose() * cov_inv * error_state; +} + void ESKF::measurement_update(const dvl_measurement& dvl_meas) { Eigen::Matrix3x18d H = calculate_h_jacobian(); Eigen::Matrix18d P = current_error_state_.covariance; @@ -179,21 +199,18 @@ void ESKF::measurement_update(const dvl_measurement& dvl_meas) { current_error_state_.covariance = I_KH * P * I_KH.transpose() + K * R * K.transpose(); // Used joseph form for more stable calculations + + NEES(); } void ESKF::injection_and_reset() { current_nom_state_.pos = current_nom_state_.pos + current_error_state_.pos; current_nom_state_.vel = current_nom_state_.vel + current_error_state_.vel; - current_nom_state_.quat = - current_nom_state_.quat * - vector3d_to_quaternion(current_error_state_.euler); + current_nom_state_.quat = current_nom_state_.quat * vector3d_to_quaternion(current_error_state_.euler); current_nom_state_.quat.normalize(); - current_nom_state_.gyro_bias = - current_nom_state_.gyro_bias + current_error_state_.gyro_bias; - current_nom_state_.accel_bias = - current_nom_state_.accel_bias + current_error_state_.accel_bias; - current_nom_state_.gravity = - current_nom_state_.gravity + current_error_state_.gravity; + current_nom_state_.gyro_bias = current_nom_state_.gyro_bias + current_error_state_.gyro_bias; + current_nom_state_.accel_bias = current_nom_state_.accel_bias + current_error_state_.accel_bias; + current_nom_state_.gravity = current_nom_state_.gravity + current_error_state_.gravity; Eigen::Matrix18d G = Eigen::Matrix18d::Identity(); diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp index 9d46969d9..04b6dd9c6 100644 --- a/navigation/eskf/src/eskf_ros.cpp +++ b/navigation/eskf/src/eskf_ros.cpp @@ -20,6 +20,16 @@ void ESKFNode::set_subscribers_and_publisher() { auto qos_sensor_data = rclcpp::QoS( rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); + pose_sub_ = this->create_subscription< + geometry_msgs::msg::PoseWithCovarianceStamped>( + "/orca/pose", qos_sensor_data, + std::bind(&ESKFNode::pose_callback, this, std::placeholders::_1)); + + twist_sub_ = this->create_subscription< + geometry_msgs::msg::TwistWithCovarianceStamped>( + "/orca/twist", qos_sensor_data, + std::bind(&ESKFNode::twist_callback, this, std::placeholders::_1)); + this->declare_parameter("imu_topic"); std::string imu_topic = this->get_parameter("imu_topic").as_string(); imu_sub_ = this->create_subscription( @@ -29,7 +39,7 @@ void ESKFNode::set_subscribers_and_publisher() { this->declare_parameter("dvl_topic"); std::string dvl_topic = this->get_parameter("dvl_topic").as_string(); dvl_sub_ = this->create_subscription< - geometry_msgs::msg::TwistWithCovarianceStamped>( + stonefish_ros2::msg::DVL>( dvl_topic, qos_sensor_data, std::bind(&ESKFNode::dvl_callback, this, std::placeholders::_1)); @@ -39,6 +49,7 @@ void ESKFNode::set_subscribers_and_publisher() { odom_topic, qos_sensor_data); nis_pub_ = create_publisher("dvl/nis", 10); + nees_pub_ = create_publisher("dvl/nees", 10); } void ESKFNode::set_parameters() { @@ -70,6 +81,20 @@ void ESKFNode::set_parameters() { error_state_.covariance = P; } +void ESKFNode::pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { + g_truth_.pos << msg->pose.pose.position.x, + msg->pose.pose.position.y, msg->pose.pose.position.z; + g_truth_.quat.w() = msg->pose.pose.orientation.w; + g_truth_.quat.x() = msg->pose.pose.orientation.x; + g_truth_.quat.y() = msg->pose.pose.orientation.y; + g_truth_.quat.z() = msg->pose.pose.orientation.z; +} + +void ESKFNode::twist_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { + g_truth_.vel << msg->twist.twist.linear.x, + msg->twist.twist.linear.y, msg->twist.twist.linear.z; +} + void ESKFNode::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { rclcpp::Time current_time = msg->header.stamp; @@ -97,20 +122,44 @@ void ESKFNode::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { } void ESKFNode::dvl_callback( - const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { - dvl_meas_.vel << msg->twist.twist.linear.x, msg->twist.twist.linear.y, - msg->twist.twist.linear.z; - dvl_meas_.cov << msg->twist.covariance[0], msg->twist.covariance[1], - msg->twist.covariance[2], msg->twist.covariance[6], - msg->twist.covariance[7], msg->twist.covariance[8], - msg->twist.covariance[12], msg->twist.covariance[13], - msg->twist.covariance[14]; + const stonefish_ros2::msg::DVL::SharedPtr msg) { + dvl_meas_.vel << msg->velocity.x, + msg->velocity.y, msg->velocity.z; + dvl_meas_.cov << 0.001, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.001; + + // msg->velocity_covariance[0], msg->velocity_covariance[1], msg->velocity_covariance[2], + // msg->velocity_covariance[3], msg->velocity_covariance[4], msg->velocity_covariance[5], + // msg->velocity_covariance[6], msg->velocity_covariance[7], msg->velocity_covariance[8]; + + + // Set biases and gravity as float values + float gyro_bias_x = 0.00001; + float gyro_bias_y = 0.00001; + float gyro_bias_z = 0.00001; + + float accel_bias_x = 0.00001; + float accel_bias_y = 0.00001; + float accel_bias_z = 0.00001; + + float gravity_x = 0.0; + float gravity_y = 0.0; + float gravity_z = -9.81; + + g_truth_.gyro_bias << gyro_bias_x, gyro_bias_y, gyro_bias_z; + g_truth_.accel_bias << accel_bias_x, accel_bias_y, accel_bias_z; + g_truth_.gravity << gravity_x, gravity_y, gravity_z; + + eskf_->ground_truth_ = g_truth_; std::tie(nom_state_, error_state_) = eskf_->dvl_update(dvl_meas_); std_msgs::msg::Float64 nis_msg; nis_msg.data = eskf_->NIS_; nis_pub_->publish(nis_msg); + + std_msgs::msg::Float64 nees_msg; + nees_msg.data = eskf_->NEES_; + nees_pub_->publish(nees_msg); } void ESKFNode::publish_odom() { diff --git a/navigation/eskf/src/eskf_utils.cpp b/navigation/eskf/src/eskf_utils.cpp index a07f3acda..88d04c3d5 100644 --- a/navigation/eskf/src/eskf_utils.cpp +++ b/navigation/eskf/src/eskf_utils.cpp @@ -11,6 +11,11 @@ Eigen::Matrix3d skew(const Eigen::Vector3d& v) { double sq(const double& value) { return value * value; } +double ssa(const double& angle) { + double result = fmod(angle + M_PI, 2 * M_PI); + double angle_ssa = result < 0 ? result + M_PI : result - M_PI; + return angle_ssa; +} Eigen::Quaterniond vector3d_to_quaternion(const Eigen::Vector3d& vector) { double angle = vector.norm(); From e08ef73c7b20e55f1fd23a81957674cb2ae25349 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 13 May 2025 22:01:55 +0000 Subject: [PATCH 100/290] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- navigation/eskf/include/eskf/eskf.hpp | 4 +- navigation/eskf/include/eskf/eskf_ros.hpp | 17 ++-- navigation/eskf/include/eskf/eskf_utils.hpp | 2 +- navigation/eskf/include/eskf/typedefs.hpp | 11 ++- navigation/eskf/src/eskf.cpp | 86 ++++++++++++--------- navigation/eskf/src/eskf_ros.cpp | 45 +++++------ navigation/ukf_okid/launch/ukf.launch.py | 1 - navigation/ukf_okid/ukf_python/ukf_okid.py | 4 +- 8 files changed, 92 insertions(+), 78 deletions(-) diff --git a/navigation/eskf/include/eskf/eskf.hpp b/navigation/eskf/include/eskf/eskf.hpp index 6d8d33bd5..d9b7d4fa0 100644 --- a/navigation/eskf/include/eskf/eskf.hpp +++ b/navigation/eskf/include/eskf/eskf.hpp @@ -27,7 +27,7 @@ class ESKF { // NIS double NIS_; - // NEES + // NEEDS double NEES_; // ground truth @@ -53,7 +53,7 @@ class ESKF { // @param S: Innovation covariance matrix void NIS(const Eigen::Vector3d& innovation, const Eigen::Matrix3d& S); - void NEES(); + void NEEDS(); // @brief Update the error state // @param dvl_meas: DVL measurement diff --git a/navigation/eskf/include/eskf/eskf_ros.hpp b/navigation/eskf/include/eskf/eskf_ros.hpp index 8cb32b173..c3d09e820 100644 --- a/navigation/eskf/include/eskf/eskf_ros.hpp +++ b/navigation/eskf/include/eskf/eskf_ros.hpp @@ -7,11 +7,11 @@ #include #include #include -#include #include #include #include #include +#include #include "eskf/eskf.hpp" #include "eskf/typedefs.hpp" #include "spdlog/spdlog.h" @@ -21,10 +21,11 @@ class ESKFNode : public rclcpp::Node { explicit ESKFNode(); private: + void pose_callback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); - void pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); - - void twist_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); + void twist_callback( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); // @brief Callback function for the imu topic // @param msg: Imu message containing the imu data @@ -47,9 +48,11 @@ class ESKFNode : public rclcpp::Node { rclcpp::Subscription::SharedPtr dvl_sub_; - rclcpp::Subscription::SharedPtr pose_sub_; - - rclcpp::Subscription::SharedPtr twist_sub_; + rclcpp::Subscription< + geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_; + + rclcpp::Subscription< + geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr twist_sub_; rclcpp::Publisher::SharedPtr odom_pub_; diff --git a/navigation/eskf/include/eskf/eskf_utils.hpp b/navigation/eskf/include/eskf/eskf_utils.hpp index 0cc2f6887..4fcaed412 100644 --- a/navigation/eskf/include/eskf/eskf_utils.hpp +++ b/navigation/eskf/include/eskf/eskf_utils.hpp @@ -1,9 +1,9 @@ #ifndef ESKF_UTILS_HPP #define ESKF_UTILS_HPP +#include #include "eigen3/Eigen/Dense" #include "eskf/typedefs.hpp" -#include Eigen::Matrix3d skew(const Eigen::Vector3d& v); diff --git a/navigation/eskf/include/eskf/typedefs.hpp b/navigation/eskf/include/eskf/typedefs.hpp index ab86b0a6c..47dfe06e1 100644 --- a/navigation/eskf/include/eskf/typedefs.hpp +++ b/navigation/eskf/include/eskf/typedefs.hpp @@ -56,13 +56,12 @@ struct state_quat { Eigen::Vector18d vec; Eigen::Vector3d euler_diff; - euler_diff = (quat * other.quat.inverse()).toRotationMatrix().eulerAngles(0, 1, 2); + euler_diff = (quat * other.quat.inverse()) + .toRotationMatrix() + .eulerAngles(0, 1, 2); - vec << pos - other.pos, - vel - other.vel, - euler_diff, - gyro_bias - other.gyro_bias, - accel_bias - other.accel_bias, + vec << pos - other.pos, vel - other.vel, euler_diff, + gyro_bias - other.gyro_bias, accel_bias - other.accel_bias, gravity - other.gravity; return vec; } diff --git a/navigation/eskf/src/eskf.cpp b/navigation/eskf/src/eskf.cpp index 9325378bb..e0e7c7a98 100644 --- a/navigation/eskf/src/eskf.cpp +++ b/navigation/eskf/src/eskf.cpp @@ -58,31 +58,34 @@ Eigen::Matrix3x19d ESKF::calculate_hx() { double qy = q.y(); double qz = q.z(); - Eigen::Matrix3d I3 = Eigen::Matrix3d::Identity(); + Eigen::Matrix3d I3 = Eigen::Matrix3d::Identity(); Eigen::Vector3d eps(qx, qy, qz); - Eigen::Matrix3d dR_deta = 2*qw * I3 - 2*skew(eps); + Eigen::Matrix3d dR_deta = 2 * qw * I3 - 2 * skew(eps); - Eigen::Vector3d e1_vec(1,0,0), e2_vec(0,1,0), e3_vec(0,0,1); + Eigen::Vector3d e1_vec(1, 0, 0), e2_vec(0, 1, 0), e3_vec(0, 0, 1); - Eigen::Matrix3d dR_dqx = -2*qx*I3 - + 2*(e1_vec*eps.transpose() + eps*e1_vec.transpose()) - - 2*qw*skew(e1_vec); + Eigen::Matrix3d dR_dqx = + -2 * qx * I3 + + 2 * (e1_vec * eps.transpose() + eps * e1_vec.transpose()) - + 2 * qw * skew(e1_vec); - Eigen::Matrix3d dR_dqy = -2*qy*I3 - + 2*(e2_vec*eps.transpose() + eps*e2_vec.transpose()) - - 2*qw*skew(e2_vec); + Eigen::Matrix3d dR_dqy = + -2 * qy * I3 + + 2 * (e2_vec * eps.transpose() + eps * e2_vec.transpose()) - + 2 * qw * skew(e2_vec); - Eigen::Matrix3d dR_dqz = -2*qz*I3 - + 2*(e3_vec*eps.transpose() + eps*e3_vec.transpose()) - - 2*qw*skew(e3_vec); + Eigen::Matrix3d dR_dqz = + -2 * qz * I3 + + 2 * (e3_vec * eps.transpose() + eps * e3_vec.transpose()) - + 2 * qw * skew(e3_vec); - Eigen::Matrix dR_dq; - dR_dq.col(0) = dR_deta * v_n; - dR_dq.col(1) = dR_dqx * v_n; - dR_dq.col(2) = dR_dqy * v_n; - dR_dq.col(3) = dR_dqz * v_n; + Eigen::Matrix dR_dq; + dR_dq.col(0) = dR_deta * v_n; + dR_dq.col(1) = dR_dqx * v_n; + dR_dq.col(2) = dR_dqy * v_n; + dR_dq.col(3) = dR_dqz * v_n; Hx.block<3, 4>(0, 6) = dR_dq; @@ -111,13 +114,18 @@ Eigen::Matrix3x1d ESKF::calculate_h() { void ESKF::nominal_state_discrete(const imu_measurement& imu_meas, const double dt) { - Eigen::Vector3d acc = current_nom_state_.quat.normalized().toRotationMatrix() * (imu_meas.accel - current_nom_state_.accel_bias) + current_nom_state_.gravity; + Eigen::Vector3d acc = + current_nom_state_.quat.normalized().toRotationMatrix() * + (imu_meas.accel - current_nom_state_.accel_bias) + + current_nom_state_.gravity; Eigen::Vector3d gyro = (imu_meas.gyro - current_nom_state_.gyro_bias) * dt; - current_nom_state_.pos = current_nom_state_.pos + current_nom_state_.vel * dt + 0.5 * sq(dt) * acc; + current_nom_state_.pos = current_nom_state_.pos + + current_nom_state_.vel * dt + 0.5 * sq(dt) * acc; current_nom_state_.vel = current_nom_state_.vel + dt * acc; - current_nom_state_.quat = (current_nom_state_.quat * vector3d_to_quaternion(gyro)); + current_nom_state_.quat = + (current_nom_state_.quat * vector3d_to_quaternion(gyro)); current_nom_state_.quat.normalize(); current_nom_state_.gyro_bias = current_nom_state_.gyro_bias; @@ -160,17 +168,17 @@ void ESKF::NIS(const Eigen::Vector3d& innovation, const Eigen::Matrix3d& S) { NIS_ = innovation.transpose() * S_inv * innovation; } -void ESKF::NEES() { - +void ESKF::NEEDS() { Eigen::Vector18d error_state = current_nom_state_.nees_error(ground_truth_); - + // Use SVD-based pseudo-inverse for better numerical stability - Eigen::JacobiSVD svd(current_error_state_.covariance, - Eigen::ComputeThinU | Eigen::ComputeThinV); - const double epsilon = 1e-10; // Threshold for singular values + Eigen::JacobiSVD svd( + current_error_state_.covariance, + Eigen::ComputeThinU | Eigen::ComputeThinV); + const double epsilon = 1e-10; // Threshold for singular values Eigen::VectorXd singular_values = svd.singularValues(); Eigen::VectorXd singular_values_inv(singular_values.size()); - + for (int i = 0; i < singular_values.size(); ++i) { if (singular_values(i) > epsilon) { singular_values_inv(i) = 1.0 / singular_values(i); @@ -178,9 +186,10 @@ void ESKF::NEES() { singular_values_inv(i) = 0.0; } } - - Eigen::MatrixXd cov_inv = svd.matrixV() * singular_values_inv.asDiagonal() * svd.matrixU().transpose(); - + + Eigen::MatrixXd cov_inv = svd.matrixV() * singular_values_inv.asDiagonal() * + svd.matrixU().transpose(); + NEES_ = error_state.transpose() * cov_inv * error_state; } @@ -199,18 +208,23 @@ void ESKF::measurement_update(const dvl_measurement& dvl_meas) { current_error_state_.covariance = I_KH * P * I_KH.transpose() + K * R * K.transpose(); // Used joseph form for more stable calculations - - NEES(); + + NEEDS(); } void ESKF::injection_and_reset() { current_nom_state_.pos = current_nom_state_.pos + current_error_state_.pos; current_nom_state_.vel = current_nom_state_.vel + current_error_state_.vel; - current_nom_state_.quat = current_nom_state_.quat * vector3d_to_quaternion(current_error_state_.euler); + current_nom_state_.quat = + current_nom_state_.quat * + vector3d_to_quaternion(current_error_state_.euler); current_nom_state_.quat.normalize(); - current_nom_state_.gyro_bias = current_nom_state_.gyro_bias + current_error_state_.gyro_bias; - current_nom_state_.accel_bias = current_nom_state_.accel_bias + current_error_state_.accel_bias; - current_nom_state_.gravity = current_nom_state_.gravity + current_error_state_.gravity; + current_nom_state_.gyro_bias = + current_nom_state_.gyro_bias + current_error_state_.gyro_bias; + current_nom_state_.accel_bias = + current_nom_state_.accel_bias + current_error_state_.accel_bias; + current_nom_state_.gravity = + current_nom_state_.gravity + current_error_state_.gravity; Eigen::Matrix18d G = Eigen::Matrix18d::Identity(); diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp index 04b6dd9c6..54daadb4c 100644 --- a/navigation/eskf/src/eskf_ros.cpp +++ b/navigation/eskf/src/eskf_ros.cpp @@ -24,7 +24,7 @@ void ESKFNode::set_subscribers_and_publisher() { geometry_msgs::msg::PoseWithCovarianceStamped>( "/orca/pose", qos_sensor_data, std::bind(&ESKFNode::pose_callback, this, std::placeholders::_1)); - + twist_sub_ = this->create_subscription< geometry_msgs::msg::TwistWithCovarianceStamped>( "/orca/twist", qos_sensor_data, @@ -38,8 +38,7 @@ void ESKFNode::set_subscribers_and_publisher() { this->declare_parameter("dvl_topic"); std::string dvl_topic = this->get_parameter("dvl_topic").as_string(); - dvl_sub_ = this->create_subscription< - stonefish_ros2::msg::DVL>( + dvl_sub_ = this->create_subscription( dvl_topic, qos_sensor_data, std::bind(&ESKFNode::dvl_callback, this, std::placeholders::_1)); @@ -49,7 +48,7 @@ void ESKFNode::set_subscribers_and_publisher() { odom_topic, qos_sensor_data); nis_pub_ = create_publisher("dvl/nis", 10); - nees_pub_ = create_publisher("dvl/nees", 10); + nees_pub_ = create_publisher("dvl/needs", 10); } void ESKFNode::set_parameters() { @@ -81,18 +80,20 @@ void ESKFNode::set_parameters() { error_state_.covariance = P; } -void ESKFNode::pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { - g_truth_.pos << msg->pose.pose.position.x, - msg->pose.pose.position.y, msg->pose.pose.position.z; +void ESKFNode::pose_callback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { + g_truth_.pos << msg->pose.pose.position.x, msg->pose.pose.position.y, + msg->pose.pose.position.z; g_truth_.quat.w() = msg->pose.pose.orientation.w; g_truth_.quat.x() = msg->pose.pose.orientation.x; g_truth_.quat.y() = msg->pose.pose.orientation.y; g_truth_.quat.z() = msg->pose.pose.orientation.z; } -void ESKFNode::twist_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { - g_truth_.vel << msg->twist.twist.linear.x, - msg->twist.twist.linear.y, msg->twist.twist.linear.z; +void ESKFNode::twist_callback( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { + g_truth_.vel << msg->twist.twist.linear.x, msg->twist.twist.linear.y, + msg->twist.twist.linear.z; } void ESKFNode::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { @@ -121,30 +122,30 @@ void ESKFNode::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { std::tie(nom_state_, error_state_) = eskf_->imu_update(imu_meas_, dt); } -void ESKFNode::dvl_callback( - const stonefish_ros2::msg::DVL::SharedPtr msg) { - dvl_meas_.vel << msg->velocity.x, - msg->velocity.y, msg->velocity.z; +void ESKFNode::dvl_callback(const stonefish_ros2::msg::DVL::SharedPtr msg) { + dvl_meas_.vel << msg->velocity.x, msg->velocity.y, msg->velocity.z; dvl_meas_.cov << 0.001, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.001; - - // msg->velocity_covariance[0], msg->velocity_covariance[1], msg->velocity_covariance[2], - // msg->velocity_covariance[3], msg->velocity_covariance[4], msg->velocity_covariance[5], - // msg->velocity_covariance[6], msg->velocity_covariance[7], msg->velocity_covariance[8]; - + + // msg->velocity_covariance[0], msg->velocity_covariance[1], + // msg->velocity_covariance[2], + // msg->velocity_covariance[3], msg->velocity_covariance[4], + // msg->velocity_covariance[5], + // msg->velocity_covariance[6], msg->velocity_covariance[7], + // msg->velocity_covariance[8]; // Set biases and gravity as float values float gyro_bias_x = 0.00001; float gyro_bias_y = 0.00001; float gyro_bias_z = 0.00001; - + float accel_bias_x = 0.00001; float accel_bias_y = 0.00001; float accel_bias_z = 0.00001; - + float gravity_x = 0.0; float gravity_y = 0.0; float gravity_z = -9.81; - + g_truth_.gyro_bias << gyro_bias_x, gyro_bias_y, gyro_bias_z; g_truth_.accel_bias << accel_bias_x, accel_bias_y, accel_bias_z; g_truth_.gravity << gravity_x, gravity_y, gravity_z; diff --git a/navigation/ukf_okid/launch/ukf.launch.py b/navigation/ukf_okid/launch/ukf.launch.py index baf6fb645..5d075259f 100644 --- a/navigation/ukf_okid/launch/ukf.launch.py +++ b/navigation/ukf_okid/launch/ukf.launch.py @@ -1,4 +1,3 @@ - from launch import LaunchDescription from launch_ros.actions import Node diff --git a/navigation/ukf_okid/ukf_python/ukf_okid.py b/navigation/ukf_okid/ukf_python/ukf_okid.py index 474b94ac6..e50c65c3a 100644 --- a/navigation/ukf_okid/ukf_python/ukf_okid.py +++ b/navigation/ukf_okid/ukf_python/ukf_okid.py @@ -39,9 +39,7 @@ def generate_delta_matrix(self, n: float) -> np.ndarray: for i in range(2 * n): for j in range(n // 2): - delta[2 * j + 1, i] = ( - np.sqrt(2) * np.sin(2 * j - 1) * ((k * np.pi) / n) - ) + delta[2 * j + 1, i] = np.sqrt(2) * np.sin(2 * j - 1) * ((k * np.pi) / n) delta[2 * j, i] = np.sqrt(2) * np.cos(2 * j - 1) * ((k * np.pi) / n) if (n % 2) == 1: From 4078d43cc65a52867e12bc917b9c5d74a2704eb9 Mon Sep 17 00:00:00 2001 From: Talha Nauman Choudhry Date: Wed, 14 May 2025 11:22:07 +0200 Subject: [PATCH 101/290] feat: Adding in the TUKF and the UKF --- navigation/tukf/test_tukf.py | 347 +++++++++++++ navigation/tukf/tukf.py | 126 +++++ navigation/tukf/tukf_class.py | 437 ++++++++++++++++ navigation/ukf_okid/ukf_python/ukf_okid.py | 25 +- .../ukf_okid/ukf_python/ukf_okid_class.py | 186 ++++++- navigation/ukf_okid/ukf_python/ukf_test.py | 471 +----------------- navigation/ukf_okid/ukf_python/ukf_test_2.py | 249 +++++++-- 7 files changed, 1331 insertions(+), 510 deletions(-) create mode 100644 navigation/tukf/test_tukf.py create mode 100644 navigation/tukf/tukf.py create mode 100644 navigation/tukf/tukf_class.py diff --git a/navigation/tukf/test_tukf.py b/navigation/tukf/test_tukf.py new file mode 100644 index 000000000..2998855ea --- /dev/null +++ b/navigation/tukf/test_tukf.py @@ -0,0 +1,347 @@ +import numpy as np +from tukf import TUKF +import tukf_class as ukf +from mpl_toolkits.mplot3d import Axes3D +import time +import math + +import matplotlib.pyplot as plt + +# Initialize UKF with StateQuat +initial_position = np.array([0.0, 0.0, 0.0]) # x, y, z +initial_velocity = np.array([0.0, 0.0, 0.0]) # vx, vy, vz +initial_quaternion = np.array([0.0, 0.0, 0.0]) # w, x, y, z (identity quaternion) +initial_angular_velocity = np.array([0.0, 0.0, 0.0]) # wx, wy, wz +initial_g_eta = np.array([1.2, 0.3, 0.3, 0.3]) # g_eta parameters +initial_intertia = np.array([0.68, 0.2, 0.1, + 0.2, 3.32, 0.2, + 0.1, 0.2, 3.34]) +initial_damping = np.array([0.01, 0.01, 0.01, + 0.01, 0.01, 0.01]) +initla_added_mass = np.array([0.02, 0.02, 0.02, + 0.02, 0.02, 0.02]) + +p_diag = np.concatenate([ + 2*np.ones(3), # x position + 2*np.ones(3), # orientation + 2*np.ones(3), # velocity + 2*np.ones(3), # angular velocity + 2*np.ones(9), # inertia + 2*np.ones(6), # added mass + 2*np.ones(6), # damping + 2*np.ones(4) # g_eta +]) + +initial_covariance = np.diag(p_diag) + +state = ukf.AUVState(initial_position.copy(), initial_quaternion.copy(), initial_velocity.copy(), initial_angular_velocity.copy()) +state.covariance = initial_covariance.copy() +state.inertia = np.array([0.58, 0.1, 0.05, + 0.1, 2.32, 0.1, + 0.01, 0.1, 2.34]) +state.g_eta = np.array([1.1, 0.1, 0.1, 0.1]) +state.damping = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) +state.added_mass = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) + +real_state = ukf.AUVState(initial_position.copy(), initial_quaternion.copy(), initial_velocity.copy(), initial_angular_velocity.copy()) +real_state.inertia = initial_intertia.copy() +real_state.g_eta = initial_g_eta.copy() +real_state.damping = initial_damping.copy() +real_state.added_mass = initla_added_mass.copy() + +Q_diag = np.concatenate([ + 0.01*np.ones(3), # position + 0.2*np.ones(9), # kinematic (η & ν) + 0.8*np.ones(9), # inertia + 0.8*np.ones(6), # added mass + 0.8*np.ones(6), # damping + 0.8*np.ones(4), # g_eta +]) + +UKF_model = TUKF(state, np.diag(Q_diag)) # Process noise covariance + +def dvl_h(state: ukf.AUVState) -> 'ukf.MeasModel': + H_matrix = np.zeros((3, 12)) + H_matrix[:, 6:9] = np.eye(3) + z_i = ukf.MeasModel() + z_i.measurement = np.dot(H_matrix, state.dynamic_part()) + return z_i + +dvl_measurement = ukf.MeasModel(H=dvl_h) + +def ang_h(state: ukf.AUVState) -> 'ukf.MeasModel': + H_matrix = np.zeros((3, 12)) + H_matrix[:, 9:12] = np.eye(3) + z_i = ukf.MeasModel() + z_i.measurement = np.dot(H_matrix, state.dynamic_part()) + return z_i + +ang_measurement = ukf.MeasModel(H=ang_h) + +# UKF parameters +dt = 0.01 # time step +sim_time = 50.0 # total simulation time +steps = int(sim_time / dt) + +# Storage for trajectory +positions = np.zeros((steps, 3)) +velocities = np.zeros((steps, 3)) +quaternions = np.zeros((steps, 3)) +angular_velocities = np.zeros((steps, 3)) +okid_params = np.zeros((steps, 25)) + +# Storage for trajectory +positions_est = np.zeros((steps, 3)) +velocities_est = np.zeros((steps, 3)) +quaternions_est = np.zeros((steps, 3)) +angular_velocities_est = np.zeros((steps, 3)) +okid_params_est = np.zeros((steps, 25)) + +# ---------- user‑tunable manoeuvre parameters ----------------------------- +SEG_DUR = 10.0 # [s] duration of each phase +A_F_TRANSL = 2.0 # [N] translational force amplitude +A_T_ROT = 1.0 # [N·m] rotational torque amplitude +# -------------------------------------------------------------------------- + +# Helper: build the scripted sequence as (kind, axis_idx, sign) +# kind = 'F' for force, 'T' for torque +# axes: 0‑x (surge/roll), 1‑y (sway/pitch), 2‑z (heave/yaw) +sequence = [ + ('F', 2, +1), # +z (up) + ('F', 2, -1), # –z (down) + ('F', 1, +1), # +y (right) + ('F', 1, -1), # –y (left / “back” sideways) + ('F', 0, +1), # +x (forward) + ('F', 0, -1), # –x (backward) + ('T', 2, +1), # +yaw (turn right) + ('T', 2, -1), # –yaw (turn left) + ('T', 1, +1), # +pitch (nose up) + ('T', 1, -1), # –pitch (nose down) + ('T', 0, +1), # +roll (starboard roll) + ('T', 0, -1) # –roll (port roll) +] + +TOTAL_TIME = len(sequence) * SEG_DUR # handy if you need it + +def _half_sine(local_t: float, duration: float) -> float: + """Smooth window: 0 → 1 → 0 over `duration` (half‑sine).""" + return np.sin(np.pi * local_t / duration) + +def control_inputs(t: float) -> tuple[np.ndarray, np.ndarray]: + """ + Piecewise scripted test signal: + – translations along z, y, x + – rotations about z (yaw), y (pitch), x (roll) + """ + # Default: no actuation + F = np.zeros(3) + T = np.zeros(3) + + # Past the last segment? keep everything zero + idx = int(t // SEG_DUR) + if idx >= len(sequence): + return F, T + + # Time inside current segment + tau = t - idx * SEG_DUR + window = _half_sine(tau, SEG_DUR) # 0‑to‑1‑to‑0 shape + + kind, axis, sgn = sequence[idx] + + if kind == 'F': + F[axis] = sgn * A_F_TRANSL * window + else: # 'T' + T[axis] = sgn * A_T_ROT * window + + return F, T + +# Simulation loop +for i in range(steps): + t = i * dt + + control_force, control_torque = control_inputs(t) + + control_input = np.concatenate((control_force, control_torque)) + + # Propagate state using UKF prediction + real_state = ukf.F_dynamics(real_state, dt, control_input) + state = UKF_model.unscented_transform(state, control_input) + + if UKF_model.filter_failed: + print("Filter failed, stopping simulation.") + break + + if i % 5 == 0: + # Simulate measurement update every 5 steps + ang_measurement.measurement = np.array([ + real_state.angular_velocity[0], + real_state.angular_velocity[1], + real_state.angular_velocity[2] + ]) + np.random.normal(0, 0.04, 3) + + ang_measurement.covariance = np.eye(3) * (0.03**2) # Measurement noise covariance + + UKF_model.measurement_update(state, ang_measurement) + state = UKF_model.posteriori_estimate(state, ang_measurement) + + if i % 10 == 0: + # Simulate measurement update every 10 steps + dvl_measurement.measurement = np.array([ + real_state.velocity[0], + real_state.velocity[1], + real_state.velocity[2] + ]) + np.random.normal(0, 0.04, 3) # Simulated measurement with noise + + # Simulate measurement covariance + dvl_measurement.covariance = np.eye(3) * (0.03**2) # Measurement noise covariance + + # Update UKF with measurement + UKF_model.measurement_update(state, dvl_measurement) + state = UKF_model.posteriori_estimate(state, dvl_measurement) + + # Store state for plotting + positions[i] = real_state.position + velocities[i] = real_state.velocity + quaternions[i] = real_state.orientation + angular_velocities[i] = real_state.angular_velocity + okid_params[i] = real_state.okid_part() + + # Store estimated state for plotting + positions_est[i] = state.position + velocities_est[i] = state.velocity + quaternions_est[i] = state.orientation + angular_velocities_est[i] = state.angular_velocity + okid_params_est[i] = state.okid_part() + + # Add small delay to simulate real-time execution + time.sleep(0.001) +print(state.as_vector()) +# Plotting +time_points = np.arange(0, sim_time, dt) + +# 3D trajectory plot +fig = plt.figure(figsize=(12, 10)) +ax = fig.add_subplot(111, projection='3d') +ax.plot(positions[:, 0], positions[:, 1], positions[:, 2], 'b-', label='True Trajectory') +ax.plot(positions_est[:, 0], positions_est[:, 1], positions_est[:, 2], 'r--', label='Estimated Trajectory') +ax.scatter(positions[0, 0], positions[0, 1], positions[0, 2], c='g', marker='o', s=100, label='Start') +ax.scatter(positions[-1, 0], positions[-1, 1], positions[-1, 2], c='r', marker='o', s=100, label='End') +ax.set_xlabel('X Position') +ax.set_ylabel('Y Position') +ax.set_zlabel('Z Position') +ax.set_title('3D Trajectory') +ax.legend() + +# Position plot +plt.figure(figsize=(12, 6)) +plt.subplot(311) +plt.plot(time_points, positions[:, 0], 'b-', label='True') +plt.plot(time_points, positions_est[:, 0], 'r--', label='Estimated') +plt.ylabel('X Position') +plt.legend() +plt.subplot(312) +plt.plot(time_points, positions[:, 1], 'b-', label='True') +plt.plot(time_points, positions_est[:, 1], 'r--', label='Estimated') +plt.ylabel('Y Position') +plt.legend() +plt.subplot(313) +plt.plot(time_points, positions[:, 2], 'b-', label='True') +plt.plot(time_points, positions_est[:, 2], 'r--', label='Estimated') +plt.ylabel('Z Position') +plt.xlabel('Time (s)') +plt.legend() +plt.tight_layout() + +# Velocity plot +plt.figure(figsize=(12, 6)) +plt.subplot(311) +plt.plot(time_points, velocities[:, 0], 'b-', label='True') +plt.plot(time_points, velocities_est[:, 0], 'r--', label='Estimated') +plt.ylabel('X Velocity') +plt.legend() +plt.subplot(312) +plt.plot(time_points, velocities[:, 1], 'b-', label='True') +plt.plot(time_points, velocities_est[:, 1], 'r--', label='Estimated') +plt.ylabel('Y Velocity') +plt.legend() +plt.subplot(313) +plt.plot(time_points, velocities[:, 2], 'b-', label='True') +plt.plot(time_points, velocities_est[:, 2], 'r--', label='Estimated') +plt.ylabel('Z Velocity') +plt.xlabel('Time (s)') +plt.legend() +plt.tight_layout() + +# Angular velocity plot +plt.figure(figsize=(12, 6)) +plt.subplot(311) +plt.plot(time_points, angular_velocities[:, 0], 'b-', label='True') +plt.plot(time_points, angular_velocities_est[:, 0], 'r--', label='Estimated') +plt.ylabel('Roll Rate') +plt.legend() +plt.subplot(312) +plt.plot(time_points, angular_velocities[:, 1], 'b-', label='True') +plt.plot(time_points, angular_velocities_est[:, 1], 'r--', label='Estimated') +plt.ylabel('Pitch Rate') +plt.legend() +plt.subplot(313) +plt.plot(time_points, angular_velocities[:, 2], 'b-', label='True') +plt.plot(time_points, angular_velocities_est[:, 2], 'r--', label='Estimated') +plt.ylabel('Yaw Rate') +plt.xlabel('Time (s)') +plt.legend() +plt.tight_layout() + +# OKID Inertia parameters plot (9 parameters) +plt.figure(figsize=(15, 10)) +plt.suptitle('Inertia Parameters', fontsize=16) +for i in range(9): + plt.subplot(3, 3, i+1) + plt.plot(time_points, okid_params[:, i], 'b-', label='True') + plt.plot(time_points, okid_params_est[:, i], 'r--', label='Estimated') + plt.ylabel(f'Inertia[{i}]') + if i >= 6: # Add x-label only to bottom row + plt.xlabel('Time (s)') + plt.legend() +plt.tight_layout(rect=[0, 0, 1, 0.96]) # Adjust for suptitle + +# OKID Added Mass parameters plot (6 parameters) +plt.figure(figsize=(15, 8)) +plt.suptitle('Added Mass Parameters', fontsize=16) +for i in range(6): + plt.subplot(2, 3, i+1) + plt.plot(time_points, okid_params[:, i+9], 'b-', label='True') + plt.plot(time_points, okid_params_est[:, i+9], 'r--', label='Estimated') + plt.ylabel(f'Added Mass[{i}]') + if i >= 3: # Add x-label only to bottom row + plt.xlabel('Time (s)') + plt.legend() +plt.tight_layout(rect=[0, 0, 1, 0.96]) # Adjust for suptitle + +# OKID Damping parameters plot (6 parameters) +plt.figure(figsize=(15, 8)) +plt.suptitle('Damping Parameters', fontsize=16) +for i in range(6): + plt.subplot(2, 3, i+1) + plt.plot(time_points, okid_params[:, i+15], 'b-', label='True') + plt.plot(time_points, okid_params_est[:, i+15], 'r--', label='Estimated') + plt.ylabel(f'Damping[{i}]') + if i >= 3: # Add x-label only to bottom row + plt.xlabel('Time (s)') + plt.legend() +plt.tight_layout(rect=[0, 0, 1, 0.96]) # Adjust for suptitle + +# OKID g_eta parameters plot (4 parameters) +plt.figure(figsize=(12, 8)) +plt.suptitle('g_eta Parameters', fontsize=16) +for i in range(4): + plt.subplot(2, 2, i+1) + plt.plot(time_points, okid_params[:, i+21], 'b-', label='True') + plt.plot(time_points, okid_params_est[:, i+21], 'r--', label='Estimated') + plt.ylabel(f'g_eta[{i}]') + plt.xlabel('Time (s)') + plt.legend() +plt.tight_layout(rect=[0, 0, 1, 0.96]) # Adjust for suptitle + +plt.show() + diff --git a/navigation/tukf/tukf.py b/navigation/tukf/tukf.py new file mode 100644 index 000000000..a4badd887 --- /dev/null +++ b/navigation/tukf/tukf.py @@ -0,0 +1,126 @@ +import numpy as np +from tukf_class import ( + MeasModel, + AUVState, + covariance_measurement, + covariance_set, + cross_covariance, + mean_measurement, + mean_set, + F_dynamics, + generate_delta_matrix, +) + +def print_matrix(matrix, name="Matrix"): + """Custom print function to print matrices in a formatted form.""" + print(f"{name}: {matrix.shape}") + if isinstance(matrix, np.ndarray): + for row in matrix: + print(" ".join(f"{val:.2f}" for val in row)) + else: + print(matrix) + +class TUKF: + def __init__(self, x_0: AUVState, Q): + self.x = x_0 + self.Q = Q + self.delta = generate_delta_matrix(len(x_0.as_vector())) / np.sqrt(len(x_0.as_vector())) + self.sigma_points_list = None + self.measurement_updated = MeasModel() + self.dt = 0.01 # Time step for dynamics + self.flagg = 0 + self.filter_failed = False + + def sigma_points(self, current_state: AUVState) -> list[AUVState]: + """Functions that generate the sigma points for the UKF.""" + n = len(current_state.covariance) + self.flagg += 1 + try: + S = np.linalg.cholesky(current_state.covariance) + except np.linalg.LinAlgError: + print("Cholesky decomposition failed!") + print("flagg", self.flagg) + print_matrix(current_state.covariance, "Current State Covariance") + print_matrix(self.Q, "Process Noise Covariance (Q)") + + # Set flag to indicate filter has failed + self.filter_failed = True + + # Create a valid but minimal S matrix to avoid crashing + # This allows the simulation to continue to the next step where it can be checked + S = np.eye(n) * 1e-6 + + self.sigma_points_list = [AUVState() for _ in range(2 * n)] + + for index, state in enumerate(self.sigma_points_list): + state.fill_states(current_state.as_vector() + S @ self.delta[:, index]) + + return self.sigma_points_list + + def unscented_transform(self, current_state: AUVState, control_force: np.ndarray) -> AUVState: + """The unscented transform function generates the priori state estimate.""" + self.sigma_points(current_state) + n = len(current_state.covariance) + + self.y_i = [AUVState() for _ in range(2 * n)] + + for i, sp in enumerate(self.sigma_points_list): + self.y_i[i] = F_dynamics(sp, self.dt, control_force) + + state_estimate = AUVState() + x = mean_set(self.y_i) + + state_estimate.fill_states(x) + state_estimate.covariance = covariance_set(self.y_i, x) + + self.Q + return state_estimate + + def measurement_update( + self, current_state: AUVState, measurement: MeasModel + ) -> None: + """Function that updates the state estimate with a measurement. + + Hopefully this is the DVL or GNSS + """ + n = len(current_state.covariance) + z_i = [MeasModel() for _ in range(2 * n)] + + for i, state in enumerate(self.y_i): + z_i[i] = measurement.H(state) + + self.measurement_updated.measurement = mean_measurement(z_i) + + self.measurement_updated.covariance = covariance_measurement( + z_i, self.measurement_updated.measurement + ) + + self.cross_correlation = cross_covariance( + self.y_i, + current_state.as_vector(), + z_i, + self.measurement_updated.measurement, + ) + + def posteriori_estimate( + self, + current_state: AUVState, + measurement: MeasModel, + ) -> AUVState: + """Calculates the posteriori estimate using measurement and the prior estimate.""" + nu_k = MeasModel() + nu_k.measurement = ( + measurement.measurement - self.measurement_updated.measurement + ) + nu_k.covariance = self.measurement_updated.covariance + measurement.covariance + + K_k = np.dot(self.cross_correlation, np.linalg.inv(nu_k.covariance)) + + posteriori_estimate = AUVState() + + posteriori_estimate.fill_states( + current_state.as_vector() + np.dot(K_k, nu_k.measurement) + ) + posteriori_estimate.covariance = current_state.covariance - np.dot( + K_k, np.dot(nu_k.covariance, np.transpose(K_k)) + ) + + return posteriori_estimate diff --git a/navigation/tukf/tukf_class.py b/navigation/tukf/tukf_class.py new file mode 100644 index 000000000..e3ec138fb --- /dev/null +++ b/navigation/tukf/tukf_class.py @@ -0,0 +1,437 @@ +import numpy as np +from dataclasses import dataclass, field +from typing import Callable + +@dataclass +class AUVState: + position: np.ndarray = field(default_factory=lambda: np.zeros(3)) + orientation: np.ndarray = field(default_factory=lambda: np.array(3)) + velocity: np.ndarray = field(default_factory=lambda: np.zeros(3)) + angular_velocity: np.ndarray = field(default_factory=lambda: np.zeros(3)) + inertia: np.ndarray = field(default_factory=lambda: np.zeros((9))) + added_mass: np.ndarray = field(default_factory=lambda: np.zeros((6))) + damping: np.ndarray = field(default_factory=lambda: np.zeros((6))) + g_eta: np.ndarray = field(default_factory=lambda: np.zeros((4))) + covariance: np.ndarray = field(default_factory=lambda: np.zeros((37, 37))) + + def dynamic_part(self) -> np.ndarray: + """Get the dynamic part of the AUV state.""" + return np.concatenate([ + self.position, + self.orientation, + self.velocity, + self.angular_velocity + ]) + def okid_part(self) -> np.ndarray: + """Get the OKID part of the AUV state.""" + return np.concatenate([ + self.inertia, + self.added_mass, + self.damping, + self.g_eta + ]) + def as_vector(self) -> np.ndarray: + """Convert the AUV state to a vector representation.""" + return np.concatenate([ + self.position, + self.orientation, + self.velocity, + self.angular_velocity, + self.inertia.flatten(), + self.added_mass.flatten(), + self.damping.flatten(), + self.g_eta + ]) + + def __add__(self, other: 'AUVState') -> 'AUVState': + """Add two AUV states together.""" + return AUVState( + position=self.position + other.position, + orientation=self.orientation + other.orientation, + velocity=self.velocity + other.velocity, + angular_velocity=self.angular_velocity + other.angular_velocity, + inertia=self.inertia + other.inertia, + added_mass=self.added_mass + other.added_mass, + damping=self.damping + other.damping, + g_eta=self.g_eta + other.g_eta + ) + def __sub__(self, other: 'AUVState') -> 'AUVState': + """Subtract two AUV states.""" + return AUVState( + position=self.position - other.position, + orientation=self.orientation - other.orientation, + velocity=self.velocity - other.velocity, + angular_velocity=self.angular_velocity - other.angular_velocity, + inertia=self.inertia - other.inertia, + added_mass=self.added_mass - other.added_mass, + damping=self.damping - other.damping, + g_eta=self.g_eta - other.g_eta + ) + def fill_states(self, x: np.ndarray) -> None: + """Fill the AUV state with a vector representation.""" + self.position = x[0:3] + self.orientation = x[3:6] + self.velocity = x[6:9] + self.angular_velocity = x[9:12] + self.inertia = x[12:21] + self.added_mass = x[21:27] + self.damping = x[27:33] + self.g_eta = x[33:37] + + +@dataclass +class MeasModel: + """A class defined for a general measurement model.""" + measurement: np.ndarray = field(default_factory=lambda: np.zeros(3)) + covariance: np.ndarray = field(default_factory=lambda: np.zeros((3, 3))) + H: Callable[["AUVState"], "MeasModel"] | None = None + + def __post_init__(self): + """Initialize H with a default measurement function if none provided.""" + if self.H is None: + self.H = self._default_H + + def _default_H(self, state: AUVState) -> 'MeasModel': + """Default measurement function that returns velocity.""" + H_matrix = np.zeros((3, 12)) + H_matrix[:, 6:9] = np.eye(3) + z_i = MeasModel() + z_i.measurement = np.dot(H_matrix, state.dynamic_part()) + return z_i + + def __add__(self, other: 'MeasModel') -> 'MeasModel': + """Defines the addition operation between two MeasModel objects.""" + result = MeasModel() + result.measurement = self.measurement + other.measurement + return result + + def __rmul__(self, scalar: float) -> 'MeasModel': + """Defines multiplication between scalar value and MeasModel object.""" + result = MeasModel() + result.measurement = scalar * self.measurement + return result + + def __sub__(self, other: 'MeasModel') -> 'MeasModel': + """Defines the subtraction between two MeasModel objects.""" + result = MeasModel() + result.measurement = self.measurement - other.measurement + return result + +def generate_delta_matrix_2(n: float) -> np.ndarray: + """Generates the weight matrix used in the TUKF sigma point generation. + + Parameters: + n (int): The state dimension. + + Returns: + delta (np.ndarray): An n x 2n orthonormal transformation matrix used to generate TUKF sigma points. + """ + delta = np.zeros((n, 2 * n)) + k = 0.001 # Tuning parameter to ensure pos def + + for i in range(2 * n): + for j in range(n // 2): + delta[2 * j + 1, i] = ( + np.sqrt(2) * np.sin(2 * j - 1) * ((k * np.pi) / n) + ) + delta[2 * j, i] = np.sqrt(2) * np.cos(2 * j - 1) * ((k * np.pi) / n) + + if (n % 2) == 1: + delta[n - 1, i] = np.sqrt(2) * np.cos(2 * j - 1) * ((k * np.pi) / n) + + return delta + +def generate_delta_matrix(n: int) -> np.ndarray: + if n < 1: + raise ValueError("n must be a positive integer") + + delta = np.zeros((n, 2 * n)) + r_max = n // 2 # floor(n/2) + sq2 = np.sqrt(2.0) + + for k in range(1, 2 * n + 1): # k = 1 … 2n + for r in range(1, r_max + 1): + row_cos = 2 * r - 2 # 0‑based index for γ_{k,2r‑1} + row_sin = 2 * r - 1 # 0‑based index for γ_{k,2r} + angle = (2 * r - 1) * k * np.pi / n + delta[row_cos, k - 1] = sq2 * np.cos(angle) + delta[row_sin, k - 1] = sq2 * np.sin(angle) + + if n % 2 == 1: # extra entry when n is odd + delta[n - 1, k - 1] = (-1) ** k + + return delta + +def skew_symmetric(vector: np.ndarray) -> np.ndarray: + """Calculates the skew symmetric matrix of a vector. + + Args: + vector (np.ndarray): The vector. + + Returns: + np.ndarray: The skew symmetric matrix. + """ + return np.array( + [ + [0, -vector[2], vector[1]], + [vector[2], 0, -vector[0]], + [-vector[1], vector[0], 0], + ] + ) + +def mean_set(set_points: list[AUVState]) -> np.ndarray: + """Function calculates the mean vector of a set of points. + + Args: + set_points (list[AUVState]): List of AUVState objects + + Returns: + np.ndarray: The mean vector + """ + n = len(set_points) + mean_value = np.zeros(set_points[0].as_vector().shape) + + for state in set_points: + mean_value = mean_value + state.as_vector() + + mean_value = (1 / n) * mean_value + + return mean_value + + +def mean_measurement(set_points: list[MeasModel]) -> np.ndarray: + """Function that calculates the mean of a set of points.""" + n = len(set_points) + mean_value = MeasModel() + + for state in set_points: + mean_value = mean_value + state + + mean_value = (1 / n) * mean_value + + return mean_value.measurement + + +def covariance_set(set_points: list[AUVState], mean: np.ndarray) -> np.ndarray: + """Function that calculates the covariance of a set of points.""" + n = len(set_points) + covariance = np.zeros(set_points[0].covariance.shape) + + for state in set_points: + W_i = state.as_vector() - mean + + covariance += np.outer(W_i, W_i) + + covariance = (1 / n) * covariance + + return covariance + + +def covariance_measurement(set_points: list[MeasModel], mean: np.ndarray) -> np.ndarray: + """Function that calculates the covariance of a set of points.""" + n = len(set_points) + co_size = len(set_points[0].measurement) + covariance = np.zeros((co_size, co_size)) + + mean_meas = MeasModel() + mean_meas.measurement = mean + + for state in set_points: + temp_state = state - mean_meas + covariance += np.outer(temp_state.measurement, temp_state.measurement) + + covariance = (1 / n) * covariance + + return covariance + + +def cross_covariance( + set_y: list[AUVState], + mean_y: np.ndarray, + set_z: list[MeasModel], + mean_z: np.ndarray, +) -> np.ndarray: + """Calculates the cross covariance between the measurement and state prediction.""" + n = len(set_y) + + cross_covariance = np.zeros((len(mean_y), len(mean_z))) + + for i in range(n): + state_diff = set_y[i].as_vector() - mean_y + meas_diff = set_z[i].measurement - mean_z + + cross_covariance += np.outer(state_diff, meas_diff) + + cross_covariance = (1 / n) * cross_covariance + + return cross_covariance + + +# ----------------------------------------------------------- + +def rotation_matrix(euler_angles: np.ndarray) -> np.ndarray: + """Calculates the rotation matrix from Euler angles (roll, pitch, yaw).""" + roll, pitch, yaw = euler_angles + + # Roll rotation + Rx = np.array([ + [1, 0, 0], + [0, np.cos(roll), -np.sin(roll)], + [0, np.sin(roll), np.cos(roll)] + ]) + + # Pitch rotation + Ry = np.array([ + [np.cos(pitch), 0, np.sin(pitch)], + [0, 1, 0], + [-np.sin(pitch), 0, np.cos(pitch)] + ]) + + # Yaw rotation + Rz = np.array([ + [np.cos(yaw), -np.sin(yaw), 0], + [np.sin(yaw), np.cos(yaw), 0], + [0, 0, 1] + ]) + + # Complete rotation matrix + R = Rz @ Ry @ Rx + return R + +def angular_velocity_transformation(euler_angles: np.ndarray) -> np.ndarray: + """Transformation matrix relating Euler rates to angular velocities.""" + roll, pitch, yaw = euler_angles + + T = np.array([ + [1, 0, -np.sin(pitch)], + [0, np.cos(roll), np.cos(pitch) * np.sin(roll)], + [0, -np.sin(roll), np.cos(pitch) * np.cos(roll)] + ]) + + return T + +def M_rb(inertia: np.ndarray) -> np.ndarray: + m = 30.0 + inertia = inertia.reshape((3, 3)) + r_b_bg = np.array([0.01, 0.0, 0.02]) + M_rb = np.zeros((6, 6)) + M_rb[0:3, 0:3] = m * np.eye(3) + M_rb[3:6, 3:6] = inertia + M_rb[0:3, 3:6] = -m * skew_symmetric(r_b_bg) + M_rb[3:6, 0:3] = m * skew_symmetric(r_b_bg) + return M_rb + +def M_a(added_mass: np.ndarray) -> np.ndarray: + """Calculates the added mass matrix.""" + M_a = np.zeros((6, 6)) + M_a[0:3, 0:3] = np.diag(added_mass[0:3]) + M_a[3:6, 3:6] = np.diag(added_mass[3:6]) + return M_a + +def C_rb(inertia: np.ndarray, angular_velocity: np.ndarray) -> np.ndarray: + """Calculates the Coriolis matrix.""" + m = 30.0 + r_b_bg = np.array([0.01, 0.0, 0.02]) + inertia = inertia.reshape((3, 3)) + C_rb = np.zeros((6, 6)) + + C_rb[0:3, 0:3] = m * skew_symmetric(angular_velocity) + C_rb[3:6, 3:6] = -skew_symmetric(np.dot(inertia, angular_velocity)) + C_rb[0:3, 3:6] = -m * skew_symmetric(angular_velocity) @ skew_symmetric(r_b_bg) + C_rb[3:6, 0:3] = m * skew_symmetric(r_b_bg) @ skew_symmetric(angular_velocity) + return C_rb + +def C_a(added_mass: np.ndarray, angular_velocity: np.ndarray, velocity: np.ndarray) -> np.ndarray: + """Calculates the added mass Coriolis matrix.""" + C_a = np.zeros((6, 6)) + A11 = np.diag(added_mass[0:3]) + A22 = np.diag(added_mass[3:6]) + C_a[3:6,3:6] = - skew_symmetric(A22 @ angular_velocity) + C_a[0:3,3:6] = - skew_symmetric(A11 @ velocity) + C_a[3:6,0:3] = - skew_symmetric(A11 @ velocity) + return C_a + +def D_linear(damping_linear: np.ndarray) -> np.ndarray: + """Calculates the linear damping matrix.""" + D = np.zeros((6, 6)) + D[0:3, 0:3] = -np.diag(damping_linear[0:3]) + D[3:6, 3:6] = -np.diag(damping_linear[3:6]) + return D + +def g_eta(g_eta: np.ndarray, orientation: np.ndarray) -> np.ndarray: + """Calculates the g_eta matrix using Euler angles.""" + Delta_WB = g_eta[0] + M_x = g_eta[1] + M_y = g_eta[2] + M_z = g_eta[3] + + # Get rotation matrix using Euler angles + R = rotation_matrix(orientation) + + G_eta = np.zeros((6,1)) + # Gravitational forces + G_eta[0:3] = -Delta_WB * R[:, 2].reshape(3, 1) + + # Buoyancy moments + G_eta[3] = -M_y * R[2, 2] + M_z * R[1, 2] + G_eta[4] = -M_z * R[0, 2] + M_x * R[2, 2] + G_eta[5] = -M_x * R[1, 2] + M_y * R[0, 2] + + return G_eta + +def F_dynamics( + state: AUVState, + dt: float, + control_input: np.ndarray) -> AUVState: + + """Calculates the dynamics of the system.""" + m_rb = M_rb(state.inertia) + m_a = M_a(state.added_mass) + c_rb = C_rb(state.inertia, state.angular_velocity) + c_a = C_a(state.added_mass, state.angular_velocity, state.velocity) + D_l = D_linear(state.damping) + g_eta_ = g_eta(state.g_eta, state.orientation) + + # Get rotation and transformation matrices + r = rotation_matrix(state.orientation) + t = angular_velocity_transformation(state.orientation) + + Crb = c_rb + c_a + Mrb = m_rb + m_a + M_inv = np.linalg.inv(Mrb) + + # Create a vector of velocity and angular velocity + nu = np.concatenate([state.velocity, state.angular_velocity]) + + # Calculate the new state + state_dot = AUVState() + state_dot.position = np.dot(r, state.velocity) + + # Calculate Euler angle rates from angular velocities + t_inv = np.linalg.inv(t) + euler_rates = np.dot(t_inv, state.angular_velocity) + state_dot.orientation = euler_rates + + Nu = M_inv @ (control_input - np.dot(Crb, nu) - np.dot(D_l, nu) - g_eta_.flatten()) + + state_dot.velocity = Nu[:3] + state_dot.angular_velocity = Nu[3:6] + + # Update the inertia, added mass, damping, and g_eta + state_dot.inertia = np.zeros_like(state.inertia) + state_dot.added_mass = np.zeros_like(state.added_mass) + state_dot.damping = np.zeros_like(state.damping) + state_dot.g_eta = np.zeros_like(state.g_eta) + + new_state = AUVState() + new_state.position = state.position + state_dot.position * dt + new_state.orientation = state.orientation + state_dot.orientation * dt + new_state.velocity = state.velocity + state_dot.velocity * dt + new_state.angular_velocity = state.angular_velocity + state_dot.angular_velocity * dt + new_state.inertia = state.inertia + state_dot.inertia * dt + new_state.added_mass = state.added_mass + state_dot.added_mass * dt + new_state.damping = state.damping + state_dot.damping * dt + new_state.g_eta = state.g_eta + state_dot.g_eta * dt + + return new_state +# ----------------------------------------------------------- diff --git a/navigation/ukf_okid/ukf_python/ukf_okid.py b/navigation/ukf_okid/ukf_python/ukf_okid.py index e50c65c3a..240ca42bb 100644 --- a/navigation/ukf_okid/ukf_python/ukf_okid.py +++ b/navigation/ukf_okid/ukf_python/ukf_okid.py @@ -7,23 +7,22 @@ cross_covariance, mean_measurement, mean_set, - okid_process_model, + F_dynamics, ) class UKF: - def __init__(self, process_model: okid_process_model, x_0, P_0, Q, G): + def __init__(self, x_0: StateQuat, Q): self.x = x_0 - self.P = P_0 self.Q = Q - self.G = G - self.process_model = process_model + # self.G = G self.sigma_points_list = None self.measurement_updated = MeasModel() self.y_i = None self.weight = None self.delta = self.generate_delta_matrix(len(x_0.as_vector()) - 1) self.cross_correlation = None + self.dt = 0.01 # Time step for dynamics def generate_delta_matrix(self, n: float) -> np.ndarray: """Generates the weight matrix used in the TUKF sigma point generation. @@ -35,12 +34,14 @@ def generate_delta_matrix(self, n: float) -> np.ndarray: delta (np.ndarray): An n x 2n orthonormal transformation matrix used to generate TUKF sigma points. """ delta = np.zeros((n, 2 * n)) - k = 0.01 # Tuning parameter to ensure pos def + k = 0.00000001 # Tuning parameter to ensure pos def for i in range(2 * n): for j in range(n // 2): - delta[2 * j + 1, i] = np.sqrt(2) * np.sin(2 * j - 1) * ((k * np.pi) / n) - delta[2 * j, i] = np.sqrt(2) * np.cos(2 * j - 1) * ((k * np.pi) / n) + delta[2 * j + 1, i] = ( + np.sqrt(2) * np.sin((2 * j - 1) * ((k * np.pi) / n)) + ) + delta[2 * j, i] = np.sqrt(2) * np.cos((2 * j - 1) * ((k * np.pi) / n)) if (n % 2) == 1: delta[n - 1, i] = (-1) ** i @@ -60,17 +61,15 @@ def sigma_points(self, current_state: StateQuat) -> list[StateQuat]: return self.sigma_points_list - def unscented_transform(self, current_state: StateQuat) -> StateQuat: + def unscented_transform(self, current_state: StateQuat, control_force: np.ndarray) -> StateQuat: """The unscented transform function generates the priori state estimate.""" self.sigma_points(current_state) n = len(current_state.covariance) self.y_i = [StateQuat() for _ in range(2 * n)] - for i, state in enumerate(self.sigma_points_list): - self.process_model.model_prediction(state) - self.process_model.state_vector_prev = state - self.y_i[i] = self.process_model.euler_forward() + for i, sp in enumerate(self.sigma_points_list): + self.y_i[i] = F_dynamics(sp, self.dt, control_force) state_estimate = StateQuat() x = mean_set(self.y_i) diff --git a/navigation/ukf_okid/ukf_python/ukf_okid_class.py b/navigation/ukf_okid/ukf_python/ukf_okid_class.py index 0b329cb84..f494e3280 100644 --- a/navigation/ukf_okid/ukf_python/ukf_okid_class.py +++ b/navigation/ukf_okid/ukf_python/ukf_okid_class.py @@ -1,5 +1,5 @@ from dataclasses import dataclass, field - +from typing import Callable import numpy as np @@ -13,21 +13,25 @@ class okid: ) ) added_mass: np.ndarray = field( - default_factory=lambda: np.array([1.0, 1.0, 1.0, 2.0, 2.0, 2.0]) + default_factory=lambda: np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) ) damping_linear: np.ndarray = field( - default_factory=lambda: np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0]) + default_factory=lambda: np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) + ) + g_eta: np.ndarray = field( + default_factory=lambda: np.array([0.0, 0.0, 0.0, 0.0]) ) def fill(self, state: np.ndarray) -> None: """Fills the okid_params object with values from a numpy array.""" self.inertia = state[0:9] self.added_mass = state[9:15] - self.damping_linear = state[15:] + self.damping_linear = state[15:21] + self.g_eta = state[21:25] def as_vector(self) -> np.ndarray: """Returns the okid_params as a numpy array.""" - return np.concatenate([self.inertia, self.added_mass, self.damping_linear]) + return np.concatenate([self.inertia, self.added_mass, self.damping_linear, self.g_eta]) def __add__(self, other: 'okid') -> 'okid': """Defines the addition operation between two okid_params objects.""" @@ -35,6 +39,7 @@ def __add__(self, other: 'okid') -> 'okid': result.inertia = self.inertia + other.inertia result.added_mass = self.added_mass + other.added_mass result.damping_linear = self.damping_linear + other.damping_linear + result.g_eta = self.g_eta + other.g_eta return result def __sub__(self, other: 'okid') -> 'okid': @@ -43,6 +48,7 @@ def __sub__(self, other: 'okid') -> 'okid': result.inertia = self.inertia - other.inertia result.added_mass = self.added_mass - other.added_mass result.damping_linear = self.damping_linear - other.damping_linear + result.g_eta = self.g_eta - other.g_eta return result def __sub__(self, other: np.ndarray) -> 'okid': @@ -50,7 +56,8 @@ def __sub__(self, other: np.ndarray) -> 'okid': result = okid() result.inertia = self.inertia - other[0:9] result.added_mass = self.added_mass - other[9:15] - result.damping_linear = self.damping_linear - other[15:] + result.damping_linear = self.damping_linear - other[15:21] + result.g_eta = self.g_eta - other[21:25] return result def __rmul__(self, scalar: float) -> 'okid': @@ -59,6 +66,7 @@ def __rmul__(self, scalar: float) -> 'okid': result.inertia = scalar * self.inertia result.added_mass = scalar * self.added_mass result.damping_linear = scalar * self.damping_linear + result.g_eta = scalar * self.g_eta return result @@ -71,7 +79,7 @@ class StateQuat: velocity: np.ndarray = field(default_factory=lambda: np.zeros(3)) angular_velocity: np.ndarray = field(default_factory=lambda: np.zeros(3)) okid_params: okid = field(default_factory=okid) - covariance: np.ndarray = field(default_factory=lambda: np.zeros((33, 33))) + covariance: np.ndarray = field(default_factory=lambda: np.zeros((37, 37))) def as_vector(self) -> np.ndarray: """Returns the StateVector as a numpy array.""" @@ -135,6 +143,7 @@ def fill_dynamic_states(self, state: np.ndarray, state_euler: np.ndarray) -> Non ) self.velocity = state[7:10] + state_euler[6:9] self.angular_velocity = state[10:13] + state_euler[9:12] + self.okid_params.fill(state[13:] + state_euler[12:]) def fill_states_different_dim( self, state: np.ndarray, state_euler: np.ndarray @@ -218,16 +227,21 @@ def add_without_quaternions(self, other: 'StateQuat') -> None: @dataclass class MeasModel: """A class defined for a general measurement model.""" - measurement: np.ndarray = field(default_factory=lambda: np.zeros(3)) covariance: np.ndarray = field(default_factory=lambda: np.zeros((3, 3))) - - def H(self, state: StateQuat) -> 'MeasModel': - """Calculates the measurement matrix.""" - H = np.zeros((3, 13)) - H[:, 7:10] = np.eye(3) + H: Callable[["StateQuat"], "MeasModel"] | None = None + + def __post_init__(self): + """Initialize H with a default measurement function if none provided.""" + if self.H is None: + self.H = self._default_H + + def _default_H(self, state: StateQuat) -> 'MeasModel': + """Default measurement function that returns velocity.""" + H_matrix = np.zeros((3, 13)) + H_matrix[:, 7:10] = np.eye(3) z_i = MeasModel() - z_i.measurement = np.dot(H, state.dynamic_part()) + z_i.measurement = np.dot(H_matrix, state.dynamic_part()) return z_i def __add__(self, other: 'MeasModel') -> 'MeasModel': @@ -459,6 +473,149 @@ def euler_forward(self) -> None: return self.state_vector +# ----------------------------------------------------------- + +def R_q(orientation: np.ndarray) -> np.ndarray: + """Calculates the rotation matrix from the orientation quaternion.""" + q0, q1, q2, q3 = orientation + R = np.array( + [ + [1 - 2 * q2**2 - 2 * q3**2, 2 * (q1 * q2 - q0 * q3), 2 * (q0 * q2 + q1 * q3)], + [2 * (q1 * q2 + q0 * q3), 1 - 2 * q1**2 - 2 * q3**2, 2 * (q2 * q3 - q0 * q1)], + [2 * (q1 * q3 - q0 * q2), 2 * (q0 * q1 + q2 * q3), 1 - 2 * q1**2 - 2 * q2**2], + ] + ) + return R + +def T_q(orientation: np.ndarray) -> np.ndarray: + """Calculates the transformation matrix from the orientation quaternion.""" + q0, q1, q2, q3 = orientation + T = 0.5 * np.array( + [[-q1, -q2, -q3], [q0, -q3, q2], [q3, q0, -q1], [-q2, q1, q0]] + ) + return T + +def M_rb(inertia: np.ndarray) -> np.ndarray: + m = 30.0 + inertia = inertia.reshape((3, 3)) + r_b_bg = np.array([0.01, 0.0, 0.02]) + M_rb = np.zeros((6, 6)) + M_rb[0:3, 0:3] = m * np.eye(3) + M_rb[3:6, 3:6] = inertia + M_rb[0:3, 3:6] = -m * skew_symmetric(r_b_bg) + M_rb[3:6, 0:3] = m * skew_symmetric(r_b_bg) + return M_rb + +def M_a(added_mass: np.ndarray) -> np.ndarray: + """Calculates the added mass matrix.""" + M_a = np.zeros((6, 6)) + M_a[0:3, 0:3] = np.diag(added_mass[0:3]) + M_a[3:6, 3:6] = np.diag(added_mass[3:6]) + return M_a + +def C_rb(inertia: np.ndarray, angular_velocity: np.ndarray) -> np.ndarray: + """Calculates the Coriolis matrix.""" + m = 30.0 + r_b_bg = np.array([0.01, 0.0, 0.02]) + inertia = inertia.reshape((3, 3)) + C_rb = np.zeros((6, 6)) + + C_rb[0:3, 0:3] = m * skew_symmetric(angular_velocity) + C_rb[3:6, 3:6] = -skew_symmetric(np.dot(inertia, angular_velocity)) + C_rb[0:3, 3:6] = -m * skew_symmetric(angular_velocity) @ skew_symmetric(r_b_bg) + C_rb[3:6, 0:3] = m * skew_symmetric(r_b_bg) @ skew_symmetric(angular_velocity) + return C_rb + +def C_a(added_mass: np.ndarray, angular_velocity: np.ndarray, velocity: np.ndarray) -> np.ndarray: + """Calculates the added mass Coriolis matrix.""" + C_a = np.zeros((6, 6)) + A11 = np.diag(added_mass[0:3]) + A22 = np.diag(added_mass[3:6]) + C_a[3:6,3:6] = - skew_symmetric(A22 @ angular_velocity) + C_a[0:3,3:6] = - skew_symmetric(A11 @ velocity) + C_a[3:6,0:3] = - skew_symmetric(A11 @ velocity) + return C_a + +def D_linear(damping_linear: np.ndarray) -> np.ndarray: + """Calculates the linear damping matrix.""" + D = np.zeros((6, 6)) + D[0:3, 0:3] = -np.diag(damping_linear[0:3]) + D[3:6, 3:6] = -np.diag(damping_linear[3:6]) + return D + +def g_eta(g_eta: np.ndarray, orientation: np.ndarray) -> np.ndarray: + """Calculates the g_eta matrix.""" + Delta_WB = g_eta[0] + M_x = g_eta[1] + M_y = g_eta[2] + M_z = g_eta[3] + + q_0 = orientation[0] + q_1 = orientation[1] + q_2 = orientation[2] + q_3 = orientation[3] + + R1 = (2*(q_1*q_3 - q_0*q_2)) + R2 = (2*(q_2*q_3 + q_0*q_1)) + R3 = (1 - 2*(q_1**2 + q_2**2)) + G_eta = np.zeros((6,1)) + G_eta[0] = - Delta_WB * R1 + G_eta[1] = - Delta_WB * R2 + G_eta[2] = - Delta_WB * R3 + + G_eta[3] = -M_y * R3 + M_z * R2 + G_eta[4] = -M_z * R1 + M_x * R3 + G_eta[5] = -M_x * R2 + M_y * R1 + + return G_eta + +def F_dynamics( + state: StateQuat, + dt: float, + control_input: np.ndarray) -> np.ndarray: + + """Calculates the dynamics of the system.""" + m_rb = M_rb(state.okid_params.inertia) + m_a = M_a(state.okid_params.added_mass) + c_rb = C_rb(state.okid_params.inertia, state.angular_velocity) + c_a = C_a(state.okid_params.added_mass, state.angular_velocity, state.velocity) + D_l = D_linear(state.okid_params.damping_linear) + g_eta_ = g_eta(state.okid_params.g_eta, state.orientation) + r_q = R_q(state.orientation) + t_q = T_q(state.orientation) + Crb = c_rb + c_a + Mrb = m_rb + m_a + M_inv = np.linalg.inv(Mrb) + + + # Calculate the new state + state_dot = StateQuat() + state_dot.position = np.dot(r_q, state.velocity) + state_dot.orientation = np.dot(t_q, state.angular_velocity) + Nu = M_inv @ (control_input - np.dot(Crb, state.nu()) - np.dot(D_l, state.nu()) - g_eta_.flatten()) + + state_dot.velocity = Nu[:3] + state_dot.angular_velocity = Nu[3:6] + state_dot.okid_params.added_mass = state.okid_params.added_mass + state_dot.okid_params.damping_linear = state.okid_params.damping_linear + state_dot.okid_params.inertia = state.okid_params.inertia + state_dot.okid_params.g_eta = state.okid_params.g_eta + + # Update the state using Euler integration + state.position += state_dot.position * dt + state.orientation = quat_norm( + state.orientation + state_dot.orientation * dt + ) + state.velocity += state_dot.velocity * dt + state.angular_velocity += state_dot.angular_velocity * dt + state.okid_params.added_mass = state.okid_params.added_mass + state.okid_params.damping_linear = state.okid_params.damping_linear + state.okid_params.inertia = state.okid_params.inertia + state.okid_params.g_eta = state.okid_params.g_eta + return state +# ----------------------------------------------------------- + + def euler_to_quat(euler_angles: np.ndarray) -> np.ndarray: """Converts Euler angles to a quaternion.""" psi, theta, phi = euler_angles @@ -624,7 +781,6 @@ def mean_set(set_points: list[StateQuat]) -> np.ndarray: mean_value.okid_params = (1 / n) * mean_value.okid_params mean_value.orientation = iterative_quaternion_mean_statequat(set_points) - return mean_value.as_vector() diff --git a/navigation/ukf_okid/ukf_python/ukf_test.py b/navigation/ukf_okid/ukf_python/ukf_test.py index d80427364..e664bd848 100644 --- a/navigation/ukf_okid/ukf_python/ukf_test.py +++ b/navigation/ukf_okid/ukf_python/ukf_test.py @@ -1,458 +1,27 @@ -import matplotlib.pyplot as plt import numpy as np +def generate_delta_matrix(n: int) -> np.ndarray: + if n < 1: + raise ValueError("n must be a positive integer") -# Import your classes and functions. -# Adjust the import paths as necessary based on your module organization. -from ukf_okid_class import ( - MeasModel, - StateQuat, - covariance_measurement, - covariance_set, - cross_covariance, - mean_measurement, - mean_set, - quat_norm, - quat_to_euler, - quaternion_super_product, -) + delta = np.zeros((n, 2 * n)) + r_max = n // 2 # floor(n/2) + sq2 = np.sqrt(2.0) + for k in range(1, 2 * n + 1): # k = 1 … 2n + for r in range(1, r_max + 1): + row_cos = 2 * r - 2 # 0‑based index for γ_{k,2r‑1} + row_sin = 2 * r - 1 # 0‑based index for γ_{k,2r} + angle = (2 * r - 1) * k * np.pi / n + delta[row_cos, k - 1] = sq2 * np.cos(angle) + delta[row_sin, k - 1] = sq2 * np.sin(angle) -# For testing, define a function to create a StateQuat with small perturbations. -def create_statequat( - base_vector, - position_perturbation, - orientation_perturbation, - velocity_perturbation, - angular_velocity_perturbation, -): - """Creates a StateQuat object. - - base_vector: 1D numpy array for base state (13 elements: - position (3), quaternion (4), velocity (3), angular_velocity (3)) - - ..._perturbation: small perturbation vector to be added to each respective component. - Returns a StateQuat. - """ - state = StateQuat() - # Base state - state.position = base_vector[0:3] + position_perturbation - # For orientation, perturb by adding a small rotation: - base_quat = base_vector[3:7] - noise_angle = np.linalg.norm(orientation_perturbation) - if noise_angle < 1e-8: - noise_quat = np.array([1.0, 0.0, 0.0, 0.0]) - else: - noise_axis = orientation_perturbation / noise_angle - noise_quat = np.concatenate( - ([np.cos(noise_angle / 2)], np.sin(noise_angle / 2) * noise_axis) - ) - state.orientation = quat_norm(quaternion_super_product(base_quat, noise_quat)) - state.velocity = base_vector[7:10] + velocity_perturbation - state.angular_velocity = base_vector[10:13] + angular_velocity_perturbation + if n % 2 == 1: # extra entry when n is odd + delta[n - 1, k - 1] = (-1) ** k - # For the augmented parameters (OKID parameters), set a 21-element vector: - # 9 for inertia, 6 for added_mass, and 6 for damping_linear. - state.okid_params.fill(np.concatenate((np.zeros(9), np.zeros(6), np.zeros(6)))) + return delta - # Set a default covariance (33x33 for the extended state) - state.covariance = np.eye(33) * 0.01 - return state +a1 = np.array([1, 0, 1]) +a2 = np.array([1, 1, 0]) - -# Test functions for state statistics -def test_state_statistics(): - # Define a base state vector (13 elements: position, quaternion, velocity, angular_velocity) - base_vector = np.zeros(13) - base_vector[0:3] = np.array([1.0, 2.0, 3.0]) - base_vector[3:7] = np.array([1.0, 0.0, 0.0, 0.0]) # identity quaternion - base_vector[7:10] = np.array([0.1, 0.2, 0.3]) - base_vector[10:13] = np.array([0.01, 0.02, 0.03]) - - # Create a list of StateQuat objects with small random perturbations. - np.random.seed(42) - state_list = [] - num_states = 10 - for _ in range(num_states): - pos_noise = np.random.normal(0, 0.05, 3) - ori_noise = np.random.normal(0, 0.01, 3) - vel_noise = np.random.normal(0, 0.02, 3) - ang_vel_noise = np.random.normal(0, 0.005, 3) - state_list.append( - create_statequat( - base_vector, pos_noise, ori_noise, vel_noise, ang_vel_noise - ) - ) - - # Compute the state mean using mean_set. - mean_state_vec = mean_set(state_list) - print("Computed mean state vector:") - print(mean_state_vec) - - # Compute the covariance of the states. - cov_state = covariance_set(state_list, mean_state_vec) - print("Computed state covariance matrix:") - print(cov_state) - - # Check symmetry of the covariance: - asym_error = np.linalg.norm(cov_state - cov_state.T) - print("Covariance symmetry error (should be near 0):", asym_error) - - # Check eigenvalues for positive semidefiniteness: - eigvals = np.linalg.eigvals(cov_state) - print("Eigenvalues of state covariance:") - print(eigvals) - - -def test_measurement_statistics(): - # Create a list of measurement objects (MeasModel) with measurements in R^3. - np.random.seed(24) - meas_list = [] - num_meas = 10 - base_meas = np.array([1.0, 2.0, 3.0]) - for _ in range(num_meas): - noise = np.random.normal(0, 0.1, 3) - meas = MeasModel() - meas.measurement = base_meas + noise - meas_list.append(meas) - - # Compute the measurement mean. - mean_meas = mean_measurement(meas_list) - print("Computed measurement mean:") - print(mean_meas) - - # Compute the measurement covariance. - cov_meas = covariance_measurement(meas_list, mean_meas) - print("Computed measurement covariance:") - print(cov_meas) - - # Check symmetry and eigenvalues. - asym_error = np.linalg.norm(cov_meas - cov_meas.T) - print("Measurement covariance symmetry error:", asym_error) - eigvals = np.linalg.eigvals(cov_meas) - print("Eigenvalues of measurement covariance:") - print(eigvals) - - -def test_cross_covariance(): - # Create a set of StateQuat and corresponding MeasModel objects. - np.random.seed(99) - num = 10 - state_list = [] - meas_list = [] - base_vector = np.zeros(13) - base_vector[0:3] = np.array([0.5, 1.0, -0.5]) - base_vector[3:7] = np.array([1.0, 0.0, 0.0, 0.0]) - base_vector[7:10] = np.array([0.05, 0.1, 0.15]) - base_vector[10:13] = np.array([0.005, 0.01, 0.015]) - - for _ in range(num): - pos_noise = np.random.normal(0, 0.02, 3) - ori_noise = np.random.normal(0, 0.005, 3) - vel_noise = np.random.normal(0, 0.01, 3) - ang_vel_noise = np.random.normal(0, 0.002, 3) - state = create_statequat( - base_vector, pos_noise, ori_noise, vel_noise, ang_vel_noise - ) - state_list.append(state) - - # Generate a measurement from each state (e.g., state velocity plus noise). - meas = MeasModel() - meas.measurement = state.velocity + np.random.normal(0, 0.01, 3) - meas_list.append(meas) - - # Compute the state mean and measurement mean as vectors. - mean_state_vec = mean_set(state_list) - mean_meas = mean_measurement(meas_list) - - cross_cov = cross_covariance(state_list, mean_state_vec, meas_list, mean_meas) - print("Computed cross-covariance between state and measurement:") - print(cross_cov) - - -import time - -from ukf_okid import UKF - -# Import your classes and functions. -from ukf_okid_class import ( - okid_process_model, - process_model, -) # Your process model classes - - -############################################ -# Helper function to create a StateQuat with perturbations. -############################################ -def create_statequat(base_vector, pos_noise, ori_noise, vel_noise, ang_vel_noise): - """Create a StateQuat object from a base vector (13 elements: - position (3), quaternion (4), velocity (3), angular_velocity (3)) - plus additive noise on each component. - - For the OKID parameters, we assume a 21-element vector: - - first 9: inertia, - - next 6: added_mass, - - last 6: damping_linear. - """ - state = StateQuat() - state.position = base_vector[0:3] + pos_noise - base_quat = base_vector[3:7] - noise_angle = np.linalg.norm(ori_noise) - if noise_angle < 1e-8: - noise_quat = np.array([1.0, 0.0, 0.0, 0.0]) - else: - noise_axis = ori_noise / noise_angle - noise_quat = np.concatenate( - ([np.cos(noise_angle / 2)], np.sin(noise_angle / 2) * noise_axis) - ) - state.orientation = quat_norm(quaternion_super_product(base_quat, noise_quat)) - state.velocity = base_vector[7:10] + vel_noise - state.angular_velocity = base_vector[10:13] + ang_vel_noise - - # Set OKID parameters to exactly 21 elements (9,6,6) - state.okid_params.fill( - np.concatenate( - ( - np.array([0.0, 0.0, 0.3, 0.0, 0.0, 3.3, 0.0, 0.0, 3.3]), - np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0]), - np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0]), - ) - ) - ) - # Set an initial covariance (33x33) for the full augmented state. - state.covariance = np.eye(33) * 0.02 - return state - - -############################################ -# Full Filter Simulation Test -############################################ -def run_ukf_simulation(): - dt = 0.01 # Time step for simulation [s] - simulation_time = 10 # Total simulation time in seconds - num_steps = int(simulation_time / dt) - - # Define a base state vector (13 elements: pos, quat, vel, ang_vel) - base_vector = np.zeros(13) - base_vector[0:3] = np.array([0.0, 0.0, 0.0]) - base_vector[3:7] = np.array([1.0, 0.0, 0.0, 0.0]) # identity quaternion - base_vector[7:10] = np.array([0.1, 0.0, 0.0]) # small velocity in x - base_vector[10:13] = np.array([0.0, 0.0, 0.0]) - - # Define initial covariance for state (33x33) - P0 = np.eye(33) - P0[0:3, 0:3] = np.eye(3) * 0.01 # position - P0[3:6, 3:6] = np.eye(3) * 0.01 # orientation error (quaternion) - P0[6:9, 6:9] = np.eye(3) * 0.01 # velocity - P0[9:12, 9:12] = np.eye(3) * 0.01 # angular velocity - P0[12:33, 12:33] = np.eye(21) * 0.001 # OKID parameters - - # Define process noise covariance Q (33x33) - Q = np.zeros((33, 33)) - Q[0:3, 0:3] = np.eye(3) * 0.001 # for position - Q[3:6, 3:6] = ( - np.eye(3) * 0.001 - ) # for orientation error (represented with Euler angles) - Q[6:9, 6:9] = np.eye(3) * 0.001 # for velocity - Q[9:12, 9:12] = np.eye(3) * 0.001 # for angular velocity - Q[12:33, 12:33] = np.eye(21) * 0.001 # OKID parameters - - G = np.zeros((33, 12)) - G[0:3, 0:3] = np.eye(3) - G[3:6, 3:6] = np.eye(3) - G[6:9, 6:9] = np.eye(3) - G[9:12, 9:12] = np.eye(3) - - # Measurement noise covariance R (3x3), assume measurement is velocity - R = np.eye(3) * 0.01 - - # Create a simulation process model and an independent UKF process model. - sim_model = process_model() - sim_model.dt = dt - sim_model.mass_interia_matrix = np.array( - [ - [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], - ] - ) - sim_model.m = 30.0 - sim_model.r_b_bg = np.array([0.01, 0.0, 0.02]) - sim_model.inertia = np.diag([0.68, 3.32, 3.34]) - sim_model.damping_linear = np.array([0.1] * 6) - sim_model.added_mass = np.array([1.0, 1.0, 1.0, 2.0, 2.0, 2.0]) - - # UKF process model copy: - ukf_model = okid_process_model() - ukf_model.dt = dt - ukf_model.mass_interia_matrix = sim_model.mass_interia_matrix.copy() - ukf_model.m = sim_model.m - ukf_model.r_b_bg = sim_model.r_b_bg.copy() - ukf_model.inertia = sim_model.inertia.copy() - ukf_model.damping_linear = sim_model.damping_linear.copy() - ukf_model.added_mass = sim_model.added_mass.copy() - - # Initialize true state and filter state. - true_state = create_statequat( - base_vector, np.zeros(3), np.zeros(3), np.zeros(3), np.zeros(3) - ) - true_state.covariance = P0.copy() - - filter_state = create_statequat( - base_vector, np.zeros(3), np.zeros(3), np.zeros(3), np.zeros(3) - ) - filter_state.covariance = P0.copy() - - # Initialize measurement model (for example, measuring velocity only) - meas_model = MeasModel() - meas_model.covariance = R.copy() - - # Initialize UKF. - ukf = UKF(ukf_model, true_state, P0.copy(), Q.copy(), G.copy()) - - # Arrays to store time histories. - pos_true_hist = np.zeros((num_steps, 3)) - pos_est_hist = np.zeros((num_steps, 3)) - vel_true_hist = np.zeros((num_steps, 3)) - vel_est_hist = np.zeros((num_steps, 3)) - euler_true_hist = np.zeros((num_steps, 3)) - euler_est_hist = np.zeros((num_steps, 3)) - time_array = np.linspace(0, simulation_time, num_steps) - - # Control input function (example: oscillatory in all directions) - def control_input(t): - return np.array( - [ - 2 * np.sin(t), - 2 * np.sin(t + 0.5), - 2 * np.sin(t + 1.0), - 0.2 * np.cos(t), - 0.2 * np.cos(t + 0.5), - 0.2 * np.cos(t + 1.0), - ] - ) - - # Set previous states. - sim_model.state_vector_prev = true_state - sim_model.state_vector = true_state - ukf_model.state_vector_prev = filter_state - ukf_model.state_vector = filter_state - - # Lists for timing diagnostics. - ukf_transform_times = [] - ukf_update_times = [] - - # Simulation loop. - for i in range(num_steps): - t_current = i * dt - - # Update control inputs. - sim_model.Control_input = control_input(t_current) - ukf_model.Control_input = control_input(t_current) - - # Propagate true state using the simulation model. - sim_model.model_prediction(true_state) - true_state = sim_model.euler_forward() - - # Create a measurement from true state. - # Here we assume we measure velocity plus noise. - meas_noise = np.random.normal(0, 0.01, 3) - meas_model.measurement = true_state.velocity + meas_noise - - # UKF prediction: unscented transform. - start = time.time() - filter_state = ukf.unscented_transform(filter_state) - ukf_transform_times.append(time.time() - start) - - # UKF measurement update every few steps. - if i % 5 == 0: - try: - start = time.time() - ukf.measurement_update(filter_state, meas_model) - filter_state = ukf.posteriori_estimate(filter_state, meas_model) - ukf_update_times.append(time.time() - start) - except np.linalg.LinAlgError: - # If matrix is not PD, add jitter. - filter_state.covariance += ( - np.eye(filter_state.covariance.shape[0]) * 1e-6 - ) - - # Store true and estimated state for diagnostics. - pos_true_hist[i, :] = true_state.position - pos_est_hist[i, :] = filter_state.position - vel_true_hist[i, :] = true_state.velocity - vel_est_hist[i, :] = filter_state.velocity - # Convert quaternion to Euler angles for visualization. - # Assumes you have a function quat_to_euler. - euler_true_hist[i, :] = quat_to_euler(true_state.orientation) - euler_est_hist[i, :] = quat_to_euler(filter_state.orientation) - - # Update previous states. - sim_model.state_vector_prev = true_state - ukf_model.state_vector_prev = filter_state - - # Print timing diagnostics. - print("Average unscented transform time:", np.mean(ukf_transform_times)) - print("Average measurement update time:", np.mean(ukf_update_times)) - - # Compute error metrics. - pos_error = np.linalg.norm(pos_true_hist - pos_est_hist, axis=1) - vel_error = np.linalg.norm(vel_true_hist - vel_est_hist, axis=1) - euler_error = np.linalg.norm(euler_true_hist - euler_est_hist, axis=1) - print("Average position error:", np.mean(pos_error)) - print("Average velocity error:", np.mean(vel_error)) - print("Average orientation (Euler) error:", np.mean(euler_error)) - - # Plot estimated vs true trajectory (positions). - plt.figure(figsize=(10, 8)) - plt.subplot(3, 1, 1) - plt.plot(time_array, pos_true_hist[:, 0], label="True X") - plt.plot(time_array, pos_est_hist[:, 0], label="Est X", linestyle="--") - plt.legend() - plt.title("Position X") - - plt.subplot(3, 1, 2) - plt.plot(time_array, pos_true_hist[:, 1], label="True Y") - plt.plot(time_array, pos_est_hist[:, 1], label="Est Y", linestyle="--") - plt.legend() - plt.title("Position Y") - - plt.subplot(3, 1, 3) - plt.plot(time_array, pos_true_hist[:, 2], label="True Z") - plt.plot(time_array, pos_est_hist[:, 2], label="Est Z", linestyle="--") - plt.legend() - plt.title("Position Z") - plt.tight_layout() - plt.show() - - # Plot errors. - plt.figure(figsize=(10, 4)) - plt.plot(time_array, pos_error, label="Position Error") - plt.plot(time_array, vel_error, label="Velocity Error") - plt.plot(time_array, euler_error, label="Euler Angle Error") - plt.legend() - plt.title("Error Metrics over Time") - plt.xlabel("Time (s)") - plt.ylabel("Error magnitude") - plt.show() - - -# You can also test the individual statistics functions separately: -def run_diagnostics(): - print("Testing state mean and covariance computation:") - # Call your pre-written tests: - # (Assuming these functions—test_state_statistics, test_measurement_statistics, test_cross_covariance—are defined above) - test_state_statistics() - print("\nTesting measurement mean and covariance computation:") - test_measurement_statistics() - print("\nTesting cross-covariance computation:") - test_cross_covariance() - - -if __name__ == '__main__': - # First, run the diagnostics on the mean/covariance functions. - run_diagnostics() - - # Then run the full UKF simulation test. - print("\nRunning full UKF simulation test:") - run_ukf_simulation() +a3 = np.outer(a1, a2) +print(a3) \ No newline at end of file diff --git a/navigation/ukf_okid/ukf_python/ukf_test_2.py b/navigation/ukf_okid/ukf_python/ukf_test_2.py index d3a75313b..0a2d5b2fe 100644 --- a/navigation/ukf_okid/ukf_python/ukf_test_2.py +++ b/navigation/ukf_okid/ukf_python/ukf_test_2.py @@ -1,42 +1,229 @@ import numpy as np +import ukf_okid_class as ukf +from ukf_okid import UKF +from mpl_toolkits.mplot3d import Axes3D +import time +import math +from ukf_utils import print_StateQuat, print_matrix -# Define process noise covariance Q (33x33) -Q = np.zeros((12, 12)) -Q[0:3, 0:3] = np.eye(3) * 0.003 # for position -Q[3:6, 3:6] = np.eye(3) * 0.003 # for orientation error (represented with Euler angles) -Q[6:9, 6:9] = np.eye(3) * 0.002 # for velocity -Q[9:12, 9:12] = np.eye(3) * 0.003 # for angular velocity +import matplotlib.pyplot as plt -G = np.zeros((33, 12)) -G[0:3, 0:3] = np.eye(3) -G[3:6, 3:6] = np.eye(3) -G[6:9, 6:9] = np.eye(3) -G[9:12, 9:12] = np.eye(3) +# Initialize UKF with StateQuat +initial_position = np.array([0.0, 0.0, 0.0]) # x, y, z +initial_velocity = np.array([0.0, 0.0, 0.0]) # vx, vy, vz +initial_quaternion = np.array([1.0, 0.0, 0.0, 0.0]) # w, x, y, z (identity quaternion) +initial_angular_velocity = np.array([0.0, 0.0, 0.0]) # wx, wy, wz +initial_g_eta = np.array([1.2, 0.3, 0.3, 0.3]) # g_eta parameters +initial_covariance = np.eye(37) * 20.0 # Initial covariance matrix -GG = G @ Q @ G.T +# Create StateQuat object +state = ukf.StateQuat(initial_position, initial_quaternion, initial_velocity, initial_angular_velocity) +state.okid_params.g_eta = initial_g_eta +state.covariance = initial_covariance +real_state = ukf.StateQuat(initial_position, initial_quaternion, initial_velocity, initial_angular_velocity) +real_state.okid_params.g_eta = np.array([1.2, 0.3, 0.3, 0.3]) -def fancy_print_matrix(matrix, name="Matrix", precision=4): - """Print a matrix with fancy formatting. +UKF_model = UKF(state, Q=10.0 * np.eye(37)) # Process noise covariance +dvl_measurement = ukf.MeasModel() - Args: - matrix: numpy array to print - name: name of the matrix to display - precision: number of decimal places to show - """ - print(f"\n{'=' * 50}") - print(f" {name} [{matrix.shape[0]}x{matrix.shape[1]}]") - print(f"{'=' * 50}") +def ang_h(state: ukf.StateQuat) -> 'ukf.MeasModel': + H_matrix = np.zeros((3, 13)) + H_matrix[:, 10:13] = np.eye(3) + z_i = ukf.MeasModel() + z_i.measurement = np.dot(H_matrix, state.dynamic_part()) + return z_i - # Set numpy print options - with np.printoptions(precision=precision, suppress=True, linewidth=100): - # Print each row with custom formatting - for i in range(matrix.shape[0]): - row = ' '.join([f"{x:8.{precision}f}" for x in matrix[i]]) - print(f" {i:2d} | {row}") +ang_measurement = ukf.MeasModel(H=ang_h) - print(f"{'=' * 50}\n") +# UKF parameters +dt = 0.01 # time step +sim_time = 1.0 # total simulation time +steps = int(sim_time / dt) +# Storage for trajectory +positions = np.zeros((steps, 3)) +velocities = np.zeros((steps, 3)) +quaternions = np.zeros((steps, 4)) +angular_velocities = np.zeros((steps, 3)) +okid_params = np.zeros((steps, 4)) + +# Storage for trajectory +positions_est = np.zeros((steps, 3)) +velocities_est = np.zeros((steps, 3)) +quaternions_est = np.zeros((steps, 4)) +angular_velocities_est = np.zeros((steps, 3)) +okid_params_est = np.zeros((steps, 4)) + +# Simulation loop +for i in range(steps): + t = i * dt + + # Generate control input (slow sinusoidal signals) + control_force = np.array([ + 5 * np.sin(4.0 * t), + 10 * np.sin(3.0 * t), + 10 * np.sin(2.0 * t) + ]) + + control_torque = np.array([ + 10 * np.sin(4.0 * t), + 10 * np.sin(4.0 * t), + 10 * np.sin(4.0 * t) + ]) + + control_input = np.concatenate((control_force, control_torque)) + + # Propagate state using UKF prediction + real_state = ukf.F_dynamics(real_state, dt, control_input) + state = UKF_model.unscented_transform(state, control_input) + if i % 5 == 0: + # Simulate measurement update every 5 steps + ang_measurement.measurement = np.array([ + real_state.angular_velocity[0], + real_state.angular_velocity[1], + real_state.angular_velocity[2] + ]) + np.random.normal(0, 0.03, 3) + # Simulate measurement covariance + ang_measurement.covariance = np.eye(3) * 0.04 # Measurement noise covariance + # Update UKF with measurement + UKF_model.measurement_update(state, ang_measurement) + state = UKF_model.posteriori_estimate(state, ang_measurement) + + if i % 10 == 0: + # Simulate measurement update every 10 steps + dvl_measurement.measurement = np.array([ + real_state.velocity[0], + real_state.velocity[1], + real_state.velocity[2] + ]) + np.random.normal(0, 0.03, 3) # Simulated measurement with noise + + # Simulate measurement covariance + dvl_measurement.covariance = np.eye(3) * 0.04 # Measurement noise covariance + + # Update UKF with measurement + UKF_model.measurement_update(state, dvl_measurement) + state = UKF_model.posteriori_estimate(state, dvl_measurement) + print_matrix(state.covariance) + + print("Determinant of covariance matrix:", np.linalg.det(state.covariance)) + # Store state for plotting + positions[i] = real_state.position + velocities[i] = real_state.velocity + quaternions[i] = real_state.orientation + angular_velocities[i] = real_state.angular_velocity + okid_params[i] = real_state.okid_params.g_eta + + # Store estimated state for plotting + positions_est[i] = state.position + velocities_est[i] = state.velocity + quaternions_est[i] = state.orientation + angular_velocities_est[i] = state.angular_velocity + okid_params_est[i] = state.okid_params.g_eta + + # Add small delay to simulate real-time execution + time.sleep(0.001) +print(state.as_vector()) +# Plotting +time_points = np.arange(0, sim_time, dt) + +# 3D trajectory plot +fig = plt.figure(figsize=(12, 10)) +ax = fig.add_subplot(111, projection='3d') +ax.plot(positions[:, 0], positions[:, 1], positions[:, 2], 'b-', label='True Trajectory') +ax.plot(positions_est[:, 0], positions_est[:, 1], positions_est[:, 2], 'r--', label='Estimated Trajectory') +ax.scatter(positions[0, 0], positions[0, 1], positions[0, 2], c='g', marker='o', s=100, label='Start') +ax.scatter(positions[-1, 0], positions[-1, 1], positions[-1, 2], c='r', marker='o', s=100, label='End') +ax.set_xlabel('X Position') +ax.set_ylabel('Y Position') +ax.set_zlabel('Z Position') +ax.set_title('3D Trajectory') +ax.legend() + +# Position plot +plt.figure(figsize=(12, 6)) +plt.subplot(311) +plt.plot(time_points, positions[:, 0], 'b-', label='True') +plt.plot(time_points, positions_est[:, 0], 'r--', label='Estimated') +plt.ylabel('X Position') +plt.legend() +plt.subplot(312) +plt.plot(time_points, positions[:, 1], 'b-', label='True') +plt.plot(time_points, positions_est[:, 1], 'r--', label='Estimated') +plt.ylabel('Y Position') +plt.legend() +plt.subplot(313) +plt.plot(time_points, positions[:, 2], 'b-', label='True') +plt.plot(time_points, positions_est[:, 2], 'r--', label='Estimated') +plt.ylabel('Z Position') +plt.xlabel('Time (s)') +plt.legend() +plt.tight_layout() + +# Velocity plot +plt.figure(figsize=(12, 6)) +plt.subplot(311) +plt.plot(time_points, velocities[:, 0], 'b-', label='True') +plt.plot(time_points, velocities_est[:, 0], 'r--', label='Estimated') +plt.ylabel('X Velocity') +plt.legend() +plt.subplot(312) +plt.plot(time_points, velocities[:, 1], 'b-', label='True') +plt.plot(time_points, velocities_est[:, 1], 'r--', label='Estimated') +plt.ylabel('Y Velocity') +plt.legend() +plt.subplot(313) +plt.plot(time_points, velocities[:, 2], 'b-', label='True') +plt.plot(time_points, velocities_est[:, 2], 'r--', label='Estimated') +plt.ylabel('Z Velocity') +plt.xlabel('Time (s)') +plt.legend() +plt.tight_layout() + +# Angular velocity plot +plt.figure(figsize=(12, 6)) +plt.subplot(311) +plt.plot(time_points, angular_velocities[:, 0], 'b-', label='True') +plt.plot(time_points, angular_velocities_est[:, 0], 'r--', label='Estimated') +plt.ylabel('Roll Rate') +plt.legend() +plt.subplot(312) +plt.plot(time_points, angular_velocities[:, 1], 'b-', label='True') +plt.plot(time_points, angular_velocities_est[:, 1], 'r--', label='Estimated') +plt.ylabel('Pitch Rate') +plt.legend() +plt.subplot(313) +plt.plot(time_points, angular_velocities[:, 2], 'b-', label='True') +plt.plot(time_points, angular_velocities_est[:, 2], 'r--', label='Estimated') +plt.ylabel('Yaw Rate') +plt.xlabel('Time (s)') +plt.legend() +plt.tight_layout() + +# OKID g_eta plot +plt.figure(figsize=(12, 6)) +plt.subplot(411) +plt.plot(time_points, okid_params[:, 0], 'b-', label='True') +plt.plot(time_points, okid_params_est[:, 0], 'r--', label='Estimated') +plt.ylabel('g_eta[0]') +plt.legend() +plt.subplot(412) +plt.plot(time_points, okid_params[:, 1], 'b-', label='True') +plt.plot(time_points, okid_params_est[:, 1], 'r--', label='Estimated') +plt.ylabel('g_eta[1]') +plt.legend() +plt.subplot(413) +plt.plot(time_points, okid_params[:, 2], 'b-', label='True') +plt.plot(time_points, okid_params_est[:, 2], 'r--', label='Estimated') +plt.ylabel('g_eta[2]') +plt.legend() +plt.subplot(414) +plt.plot(time_points, okid_params[:, 3], 'b-', label='True') +plt.plot(time_points, okid_params_est[:, 3], 'r--', label='Estimated') +plt.ylabel('g_eta[3]') +plt.xlabel('Time (s)') +plt.legend() +plt.tight_layout() + +plt.show() -# Example usage: -fancy_print_matrix(GG, name="Process Noise Covariance (GQG')", precision=3) From 338bb9e4ca26b675ddd566a6a2f5a59d2b5b6fa3 Mon Sep 17 00:00:00 2001 From: Talha Nauman Choudhry Date: Sun, 18 May 2025 16:40:56 +0200 Subject: [PATCH 102/290] fix: added tukf and simulation --- navigation/eskf/config/eskf_params.yaml | 6 +- navigation/eskf/include/eskf/typedefs.hpp | 6 +- navigation/eskf/src/eskf.cpp | 56 ++--- navigation/eskf/src/eskf_ros.cpp | 8 +- navigation/tukf/simulation.py | 155 +++++++++++++ navigation/tukf/simulation_re.ipynb | 265 ++++++++++++++++++++++ 6 files changed, 451 insertions(+), 45 deletions(-) create mode 100644 navigation/tukf/simulation.py create mode 100644 navigation/tukf/simulation_re.ipynb diff --git a/navigation/eskf/config/eskf_params.yaml b/navigation/eskf/config/eskf_params.yaml index 98be8e789..3b9fcdc0b 100644 --- a/navigation/eskf/config/eskf_params.yaml +++ b/navigation/eskf/config/eskf_params.yaml @@ -1,8 +1,8 @@ eskf_node: ros__parameters: - imu_topic: imu/data_raw + imu_topic: /imu/data_raw dvl_topic: /dvl/sim odom_topic: odom - diag_Q_std: [0.05, 0.05, 0.05, 0.00001, 0.00001, 0.00001, 0.000001, 0.000000001, 0.000000001, 0.000000001, 0.00000001, 0.00000001] - diag_p_init: [1.0, 1.0, 0.5, 0.5, 0.5, 1.0, 0.1, 0.1, 0.1, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001] + diag_Q_std: [0.01, 0.01, 0.01, 0.005, 0.005, 0.005, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001] + diag_p_init: [1.0, 1.0, 1.0, 0.5, 0.5, 0.5, 0.1, 0.1, 0.1, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001] imu_frame: [0.0, 0.0, -1.0, 0.0, -1.0, 0.0, -1.0, 0.0, 0.0] diff --git a/navigation/eskf/include/eskf/typedefs.hpp b/navigation/eskf/include/eskf/typedefs.hpp index 47dfe06e1..eacb32841 100644 --- a/navigation/eskf/include/eskf/typedefs.hpp +++ b/navigation/eskf/include/eskf/typedefs.hpp @@ -26,6 +26,8 @@ typedef Eigen::Matrix Matrix18x3d; typedef Eigen::Matrix Matrix36d; typedef Eigen::Matrix Matrix6d; typedef Eigen::Matrix Matrix9d; +typedef Eigen::Matrix Matrix15d; +typedef Eigen::Matrix Vector15d; } // namespace Eigen template @@ -36,7 +38,7 @@ Eigen::Matrix createDiagonalMatrix( } struct state_quat { - Eigen::Vector3d pos = Eigen::Vector3d::Zero(); + Eigen::Vector3d pos = Eigen::Vector3d(0,0,0.125); Eigen::Vector3d vel = Eigen::Vector3d::Zero(); Eigen::Quaterniond quat = Eigen::Quaterniond::Identity(); Eigen::Vector3d gyro_bias = Eigen::Vector3d::Zero(); @@ -58,7 +60,7 @@ struct state_quat { euler_diff = (quat * other.quat.inverse()) .toRotationMatrix() - .eulerAngles(0, 1, 2); + .eulerAngles(0, 1, 2) + Eigen::Vector3d(-M_PI, M_PI, -M_PI); vec << pos - other.pos, vel - other.vel, euler_diff, gyro_bias - other.gyro_bias, accel_bias - other.accel_bias, diff --git a/navigation/eskf/src/eskf.cpp b/navigation/eskf/src/eskf.cpp index e0e7c7a98..e39993abc 100644 --- a/navigation/eskf/src/eskf.cpp +++ b/navigation/eskf/src/eskf.cpp @@ -102,10 +102,9 @@ Eigen::Matrix3x18d ESKF::calculate_h_jacobian() { return H; } -Eigen::Matrix3x1d ESKF::calculate_h() { - Eigen::Matrix3x1d h; - Eigen::Matrix3d R_bn = - current_nom_state_.quat.normalized().toRotationMatrix().transpose(); +Eigen::Vector3d ESKF::calculate_h() { + Eigen::Vector3d h; + Eigen::Matrix3d R_bn = current_nom_state_.quat.normalized().toRotationMatrix().transpose(); h = R_bn * current_nom_state_.vel; @@ -114,18 +113,13 @@ Eigen::Matrix3x1d ESKF::calculate_h() { void ESKF::nominal_state_discrete(const imu_measurement& imu_meas, const double dt) { - Eigen::Vector3d acc = - current_nom_state_.quat.normalized().toRotationMatrix() * - (imu_meas.accel - current_nom_state_.accel_bias) + - current_nom_state_.gravity; + Eigen::Vector3d acc = current_nom_state_.quat.normalized().toRotationMatrix() * (imu_meas.accel - current_nom_state_.accel_bias) + current_nom_state_.gravity; Eigen::Vector3d gyro = (imu_meas.gyro - current_nom_state_.gyro_bias) * dt; - current_nom_state_.pos = current_nom_state_.pos + - current_nom_state_.vel * dt + 0.5 * sq(dt) * acc; + current_nom_state_.pos = current_nom_state_.pos + current_nom_state_.vel * dt + 0.5 * sq(dt) * acc; current_nom_state_.vel = current_nom_state_.vel + dt * acc; - current_nom_state_.quat = - (current_nom_state_.quat * vector3d_to_quaternion(gyro)); + current_nom_state_.quat = (current_nom_state_.quat * vector3d_to_quaternion(gyro)); current_nom_state_.quat.normalize(); current_nom_state_.gyro_bias = current_nom_state_.gyro_bias; @@ -159,8 +153,7 @@ void ESKF::error_state_prediction(const imu_measurement& imu_meas, std::tie(A_d, GQG_d) = van_loan_discretization(A_c, G_c, dt); state_euler next_error_state; - current_error_state_.covariance = - A_d * current_error_state_.covariance * A_d.transpose() + GQG_d; + current_error_state_.covariance = A_d * current_error_state_.covariance * A_d.transpose() + GQG_d; } void ESKF::NIS(const Eigen::Vector3d& innovation, const Eigen::Matrix3d& S) { @@ -170,27 +163,17 @@ void ESKF::NIS(const Eigen::Vector3d& innovation, const Eigen::Matrix3d& S) { void ESKF::NEEDS() { Eigen::Vector18d error_state = current_nom_state_.nees_error(ground_truth_); - - // Use SVD-based pseudo-inverse for better numerical stability - Eigen::JacobiSVD svd( - current_error_state_.covariance, - Eigen::ComputeThinU | Eigen::ComputeThinV); - const double epsilon = 1e-10; // Threshold for singular values - Eigen::VectorXd singular_values = svd.singularValues(); - Eigen::VectorXd singular_values_inv(singular_values.size()); - - for (int i = 0; i < singular_values.size(); ++i) { - if (singular_values(i) > epsilon) { - singular_values_inv(i) = 1.0 / singular_values(i); - } else { - singular_values_inv(i) = 0.0; - } + Eigen::Vector15d error_state_trim = error_state.head<15>(); + // Set the last 6 elements of error_state_trim to zero + for (int i = 9; i < 15; i++) { + error_state_trim(i) = 0; } - - Eigen::MatrixXd cov_inv = svd.matrixV() * singular_values_inv.asDiagonal() * - svd.matrixU().transpose(); - - NEES_ = error_state.transpose() * cov_inv * error_state; + error_state_trim(6) = 0.001; + error_state_trim(7) = 0.001; + error_state_trim(8) = 0.001; + Eigen::Matrix15d P = current_error_state_.covariance.block<15, 15>(0, 0); + Eigen::Matrix15d P_inv = P.inverse(); + NEES_ = error_state_trim.transpose() * P_inv * error_state_trim; } void ESKF::measurement_update(const dvl_measurement& dvl_meas) { @@ -201,6 +184,7 @@ void ESKF::measurement_update(const dvl_measurement& dvl_meas) { Eigen::Matrix3d S = H * P * H.transpose() + R; Eigen::Matrix18x3d K = P * H.transpose() * S.inverse(); Eigen::Vector3d innovation = dvl_meas.vel - calculate_h(); + NIS(innovation, S); current_error_state_.set_from_vector(K * innovation); @@ -215,9 +199,7 @@ void ESKF::measurement_update(const dvl_measurement& dvl_meas) { void ESKF::injection_and_reset() { current_nom_state_.pos = current_nom_state_.pos + current_error_state_.pos; current_nom_state_.vel = current_nom_state_.vel + current_error_state_.vel; - current_nom_state_.quat = - current_nom_state_.quat * - vector3d_to_quaternion(current_error_state_.euler); + current_nom_state_.quat = current_nom_state_.quat * vector3d_to_quaternion(current_error_state_.euler); current_nom_state_.quat.normalize(); current_nom_state_.gyro_bias = current_nom_state_.gyro_bias + current_error_state_.gyro_bias; diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp index 54daadb4c..b7cdced09 100644 --- a/navigation/eskf/src/eskf_ros.cpp +++ b/navigation/eskf/src/eskf_ros.cpp @@ -48,7 +48,7 @@ void ESKFNode::set_subscribers_and_publisher() { odom_topic, qos_sensor_data); nis_pub_ = create_publisher("dvl/nis", 10); - nees_pub_ = create_publisher("dvl/needs", 10); + nees_pub_ = create_publisher("dvl/nees", 10); } void ESKFNode::set_parameters() { @@ -123,8 +123,10 @@ void ESKFNode::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { } void ESKFNode::dvl_callback(const stonefish_ros2::msg::DVL::SharedPtr msg) { + // Log that we received a DVL message + // spdlog::info("DVL message received"); dvl_meas_.vel << msg->velocity.x, msg->velocity.y, msg->velocity.z; - dvl_meas_.cov << 0.001, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.001; + dvl_meas_.cov << 0.004*0.004, 0.0, 0.0, 0.0, 0.004*0.004, 0.0, 0.0, 0.0, 0.004*0.004; // msg->velocity_covariance[0], msg->velocity_covariance[1], // msg->velocity_covariance[2], @@ -144,7 +146,7 @@ void ESKFNode::dvl_callback(const stonefish_ros2::msg::DVL::SharedPtr msg) { float gravity_x = 0.0; float gravity_y = 0.0; - float gravity_z = -9.81; + float gravity_z = 9.81; g_truth_.gyro_bias << gyro_bias_x, gyro_bias_y, gyro_bias_z; g_truth_.accel_bias << accel_bias_x, accel_bias_y, accel_bias_z; diff --git a/navigation/tukf/simulation.py b/navigation/tukf/simulation.py new file mode 100644 index 000000000..56fb2a6a1 --- /dev/null +++ b/navigation/tukf/simulation.py @@ -0,0 +1,155 @@ +from rosbags.highlevel import AnyReader +from rosbags.typesys import get_types_from_msg, register_types +import numpy as np, pandas as pd, pathlib +from tukf import TUKF +import tukf_class as ukf + +bag_folder = pathlib.Path(r"/home/talha/vortex_auv_ws/bags/sim_data_no_dvl") + +# Initialize UKF with StateQuat +initial_position = np.array([0.0, 0.0, 0.0]) # x, y, z +initial_velocity = np.array([0.0, 0.0, 0.0]) # vx, vy, vz +initial_quaternion = np.array([0.0, 0.0, 0.0]) # w, x, y, z (identity quaternion) +initial_angular_velocity = np.array([0.0, 0.0, 0.0]) # wx, wy, wz +initial_g_eta = np.array([0.01, 0.01, 0.01, 0.01]) # g_eta parameters +initial_intertia = np.array([0.2, 0.2, 0.1, + 0.2, 0.2, 0.2, + 0.1, 0.2, 0.2]) +initial_damping = np.array([0.01, 0.01, 0.01, + 0.01, 0.01, 0.01]) +initla_added_mass = np.array([0.02, 0.02, 0.02, + 0.02, 0.02, 0.02]) + +p_diag = np.concatenate([ + 2*np.ones(3), # x position + 2*np.ones(3), # orientation + 2*np.ones(3), # velocity + 2*np.ones(3), # angular velocity + 2*np.ones(9), # inertia + 2*np.ones(6), # added mass + 2*np.ones(6), # damping + 2*np.ones(4) # g_eta +]) + +initial_covariance = np.diag(p_diag) + +state = ukf.AUVState(initial_position.copy(), initial_quaternion.copy(), initial_velocity.copy(), initial_angular_velocity.copy(), + initial_intertia.copy(), initla_added_mass.copy(), initial_damping.copy(), initial_g_eta.copy()) + +state.covariance = initial_covariance.copy() + +Q_diag = np.concatenate([ + 0.1*np.ones(3), # position + 0.1*np.ones(9), # kinematic (η & ν) + 0.001*np.ones(9), # inertia + 0.001*np.ones(6), # added mass + 0.001*np.ones(6), # damping + 0.001*np.ones(4), # g_eta +]) + +UKF_model = TUKF(state, np.diag(Q_diag)) # Process noise covariance + +def dvl_h(state: ukf.AUVState) -> 'ukf.MeasModel': + H_matrix = np.zeros((3, 12)) + H_matrix[:, 6:9] = np.eye(3) + z_i = ukf.MeasModel() + z_i.measurement = np.dot(H_matrix, state.dynamic_part()) + return z_i + +dvl_measurement = ukf.MeasModel(H=dvl_h) + +def ang_h(state: ukf.AUVState) -> 'ukf.MeasModel': + H_matrix = np.zeros((3, 12)) + H_matrix[:, 9:12] = np.eye(3) + z_i = ukf.MeasModel() + z_i.measurement = np.dot(H_matrix, state.dynamic_part()) + return z_i + +ang_measurement = ukf.MeasModel(H=ang_h) + +R_corr = np.array([[0.0, 0.0, -1.0], + [0.0, -1.0, 0.0], + [-1.0, 0.0, 0.0]]) + +# Storage for trajectory +positions = [] +velocities = [] +quaternions = [] +angular_velocities = [] +okid_params = [] + +# Storage for trajectory estimates +positions_est = [] +velocities_est = [] +quaternions_est = [] +angular_velocities_est = [] +okid_params_est = [] + +with AnyReader([bag_folder]) as reader: + # Filter topics once + conns = [c for c in reader.connections + if c.topic in ("/imu/data_raw", "/dvl_twist", "/orca/wrench_input")] + + last_time = None + log = [] + coutner = 0 + + for conn, ts_raw, raw in reader.messages(conns): + t_ns = ts_raw # already nanoseconds integer + coutner += 1 + + if conn.topic == "/orca/wrench_input": + + # Get the wrench input message + msg = reader.deserialize(raw, conn.msgtype) + wrench = msg.wrench # Extract the wrench field from the message + forces = np.array([wrench.force.x, wrench.force.y, wrench.force.z]) + torques = np.array([wrench.torque.x, wrench.torque.y, wrench.torque.z]) + + control_input = np.concatenate([forces, torques]) + + if last_time is not None: + UKF_model.dt = (t_ns - last_time) / 1e9 # Convert nanoseconds to seconds + + # 1. prediction step + state = UKF_model.unscented_transform(state, control_input) + + # 2. measurement update + msg = reader.deserialize(raw, conn.msgtype) + if conn.topic == "/imu/data_raw": + print("IMU data received") + + # Get the IMU data + measurement_imu = np.array([msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z]) + + ang_measurement.measurement = np.dot(R_corr,measurement_imu) + + ang_measurement.covariance = np.eye(3) * (0.03**2) + + UKF_model.measurement_update(state, ang_measurement) + state = UKF_model.posteriori_estimate(state, ang_measurement) + + if conn.topic == "/dvl_twist": + print("DVL data received") + + msg = reader.deserialize(raw, conn.msgtype) + dvl_measurement.measurement = np.array([msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z]) + dvl_measurement.measurement = np.dot(R_corr, dvl_measurement.measurement) + + dvl_measurement.covariance = np.eye(3) * (0.01**2) + + # Update UKF with measurement + UKF_model.measurement_update(state, dvl_measurement) + state = UKF_model.posteriori_estimate(state, dvl_measurement) + + + # Store the state estimates + positions_est.append(state.position.copy()) + velocities_est.append(state.velocity.copy()) + quaternions_est.append(state.quaternion.copy()) + angular_velocities_est.append(state.angular_velocity.copy()) + okid_params_est.append(state.okid_part().copy()) + + last_time = t_ns + + \ No newline at end of file diff --git a/navigation/tukf/simulation_re.ipynb b/navigation/tukf/simulation_re.ipynb new file mode 100644 index 000000000..e508825bf --- /dev/null +++ b/navigation/tukf/simulation_re.ipynb @@ -0,0 +1,265 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "### Simulation results for the TUKF ###\n", + "\n", + "try to simulate the path and parameters using the tukf algorithm" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [], + "source": [ + "from rosbags.highlevel import AnyReader\n", + "from rosbags.typesys import get_types_from_msg, register_types\n", + "import numpy as np, pandas as pd, pathlib\n", + "from tukf import TUKF\n", + "import tukf_class as ukf" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [], + "source": [ + "bag_folder = pathlib.Path(r\"/home/talha/vortex_auv_ws/bags/sim_data_no_dvl\")" + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "metadata": {}, + "outputs": [], + "source": [ + "# Initialize UKF with StateQuat\n", + "initial_position = np.array([0.0, 0.0, 0.0]) # x, y, z\n", + "initial_velocity = np.array([0.0, 0.0, 0.0]) # vx, vy, vz\n", + "initial_quaternion = np.array([0.0, 0.0, 0.0]) # w, x, y, z (identity quaternion)\n", + "initial_angular_velocity = np.array([0.0, 0.0, 0.0]) # wx, wy, wz\n", + "initial_g_eta = np.array([0.01, 0.01, 0.01, 0.01]) # g_eta parameters\n", + "initial_intertia = np.array([0.2, 0.2, 0.1, \n", + " 0.2, 0.2, 0.2, \n", + " 0.1, 0.2, 0.2])\n", + "initial_damping = np.array([0.01, 0.01, 0.01,\n", + " 0.01, 0.01, 0.01])\n", + "initla_added_mass = np.array([0.02, 0.02, 0.02,\n", + " 0.02, 0.02, 0.02])\n", + "\n", + "p_diag = np.concatenate([\n", + " 2*np.ones(3), # x position\n", + " 2*np.ones(3), # orientation\n", + " 2*np.ones(3), # velocity\n", + " 2*np.ones(3), # angular velocity\n", + " 2*np.ones(9), # inertia\n", + " 2*np.ones(6), # added mass\n", + " 2*np.ones(6), # damping\n", + " 2*np.ones(4) # g_eta\n", + "])\n", + "\n", + "initial_covariance = np.diag(p_diag) \n", + "\n", + "state = ukf.AUVState(initial_position.copy(), initial_quaternion.copy(), initial_velocity.copy(), initial_angular_velocity.copy(), \n", + " initial_intertia.copy(), initla_added_mass.copy(), initial_damping.copy(), initial_g_eta.copy())\n", + "\n", + "state.covariance = initial_covariance.copy()\n", + "\n", + "Q_diag = np.concatenate([\n", + " 0.1*np.ones(3), # position\n", + " 0.1*np.ones(9), # kinematic (η & ν)\n", + " 0.001*np.ones(9), # inertia\n", + " 0.001*np.ones(6), # added mass\n", + " 0.001*np.ones(6), # damping\n", + " 0.001*np.ones(4), # g_eta\n", + "])\n", + "\n", + "UKF_model = TUKF(state, np.diag(Q_diag)) # Process noise covariance\n", + "\n", + "def dvl_h(state: ukf.AUVState) -> 'ukf.MeasModel':\n", + " H_matrix = np.zeros((3, 12))\n", + " H_matrix[:, 6:9] = np.eye(3)\n", + " z_i = ukf.MeasModel()\n", + " z_i.measurement = np.dot(H_matrix, state.dynamic_part())\n", + " return z_i\n", + "\n", + "dvl_measurement = ukf.MeasModel(H=dvl_h)\n", + "\n", + "def ang_h(state: ukf.AUVState) -> 'ukf.MeasModel':\n", + " H_matrix = np.zeros((3, 12))\n", + " H_matrix[:, 9:12] = np.eye(3)\n", + " z_i = ukf.MeasModel()\n", + " z_i.measurement = np.dot(H_matrix, state.dynamic_part())\n", + " return z_i\n", + "\n", + "ang_measurement = ukf.MeasModel(H=ang_h) \n", + "\n", + "R_corr = np.array([[0.0, 0.0, -1.0],\n", + " [0.0, -1.0, 0.0],\n", + " [-1.0, 0.0, 0.0]])\n", + "\n", + "# Storage for trajectory\n", + "positions = []\n", + "velocities = []\n", + "quaternions = []\n", + "angular_velocities = []\n", + "okid_params = []\n", + "\n", + "# Storage for trajectory estimates\n", + "positions_est = []\n", + "velocities_est = []\n", + "quaternions_est = []\n", + "angular_velocities_est = []\n", + "okid_params_est = []" + ] + }, + { + "cell_type": "code", + "execution_count": 8, + "metadata": {}, + "outputs": [], + "source": [ + "with AnyReader([bag_folder]) as reader:\n", + " # Filter topics once\n", + " conns = [c for c in reader.connections\n", + " if c.topic in (\"/imu/data_raw\", \"/dvl_twist\", \"/orca/wrench_input\")]\n", + "\n", + " last_time = None\n", + " log = []\n", + " coutner = 0\n", + "\n", + " for conn, ts_raw, raw in reader.messages(conns):\n", + " t_ns = ts_raw # already nanoseconds integer\n", + " coutner += 1\n", + "\n", + " if conn.topic == \"/orca/wrench_input\":\n", + " \n", + " # Get the wrench input message\n", + " msg = reader.deserialize(raw, conn.msgtype)\n", + " wrench = msg.wrench # Extract the wrench field from the message\n", + " forces = np.array([wrench.force.x, wrench.force.y, wrench.force.z])\n", + " torques = np.array([wrench.torque.x, wrench.torque.y, wrench.torque.z])\n", + "\n", + " control_input = np.concatenate([forces, torques])\n", + "\n", + " if last_time is not None:\n", + " UKF_model.dt = (t_ns - last_time) / 1e9 # Convert nanoseconds to seconds\n", + "\n", + " # 1. prediction step\n", + " state = UKF_model.unscented_transform(state, control_input)\n", + "\n", + " # 2. measurement update\n", + " msg = reader.deserialize(raw, conn.msgtype)\n", + " if conn.topic == \"/imu/data_raw\":\n", + " # print(\"IMU data received\")\n", + "\n", + " # Get the IMU data\n", + " measurement_imu = np.array([msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z])\n", + "\n", + " ang_measurement.measurement = np.dot(R_corr,measurement_imu)\n", + "\n", + " ang_measurement.covariance = np.eye(3) * (0.03**2) \n", + "\n", + " UKF_model.measurement_update(state, ang_measurement)\n", + " state = UKF_model.posteriori_estimate(state, ang_measurement)\n", + "\n", + " if conn.topic == \"/dvl_twist\":\n", + " # print(\"DVL data received\")\n", + "\n", + " msg = reader.deserialize(raw, conn.msgtype)\n", + " dvl_measurement.measurement = np.array([msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z])\n", + " \n", + " dvl_measurement.covariance = np.eye(3) * (0.01**2) \n", + " \n", + " # Update UKF with measurement\n", + " UKF_model.measurement_update(state, dvl_measurement)\n", + " state = UKF_model.posteriori_estimate(state, dvl_measurement)\n", + "\n", + "\n", + " # Store the state estimates\n", + " positions_est.append(state.position.copy())\n", + " velocities_est.append(state.velocity.copy())\n", + " quaternions_est.append(state.orientation.copy())\n", + " angular_velocities_est.append(state.angular_velocity.copy())\n", + " okid_params_est.append(state.okid_part().copy())\n", + "\n", + " last_time = t_ns" + ] + }, + { + "cell_type": "code", + "execution_count": 9, + "metadata": {}, + "outputs": [ + { + "data": { + "image/png": "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", + "text/plain": [ + "
" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ + "import numpy as np\n", + "\n", + "import matplotlib.pyplot as plt\n", + "\n", + "# Extract x, y, z positions from positions_est\n", + "x_positions = np.array([pos[0] for pos in positions_est])\n", + "y_positions = np.array([pos[1] for pos in positions_est])\n", + "z_positions = np.array([pos[2] for pos in positions_est])\n", + "\n", + "# Create figure with three subplots\n", + "fig, axs = plt.subplots(3, 1, figsize=(12, 10), sharex=True)\n", + "fig.suptitle('AUV Position Estimates Over Time', fontsize=16)\n", + "\n", + "# Plot x position\n", + "axs[0].plot(x_positions, 'b-', linewidth=2)\n", + "axs[0].set_ylabel('X Position [m]')\n", + "axs[0].grid(True)\n", + "\n", + "# Plot y position\n", + "axs[1].plot(y_positions, 'g-', linewidth=2)\n", + "axs[1].set_ylabel('Y Position [m]')\n", + "axs[1].grid(True)\n", + "\n", + "# Plot z position\n", + "axs[2].plot(z_positions, 'r-', linewidth=2)\n", + "axs[2].set_ylabel('Z Position [m]')\n", + "axs[2].set_xlabel('Time steps')\n", + "axs[2].grid(True)\n", + "\n", + "plt.tight_layout(rect=[0, 0.03, 1, 0.95])\n", + "plt.show()" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.10.12" + } + }, + "nbformat": 4, + "nbformat_minor": 2 +} From 7a880447b9af0d8030bf096c8a372e6467309fa4 Mon Sep 17 00:00:00 2001 From: Talha Nauman Choudhry Date: Tue, 20 May 2025 13:23:59 +0200 Subject: [PATCH 103/290] fix:added in some minor fixes --- navigation/eskf/config/eskf_params.yaml | 2 +- navigation/eskf/include/eskf/eskf.hpp | 8 -- navigation/eskf/include/eskf/eskf_ros.hpp | 19 +-- navigation/eskf/src/eskf.cpp | 18 --- navigation/eskf/src/eskf_ros.cpp | 65 +-------- navigation/tukf/simulation_re.ipynb | 154 ++++++++++++++++++---- navigation/tukf/tukf_class.py | 4 +- 7 files changed, 138 insertions(+), 132 deletions(-) diff --git a/navigation/eskf/config/eskf_params.yaml b/navigation/eskf/config/eskf_params.yaml index 3b9fcdc0b..c08c21d3c 100644 --- a/navigation/eskf/config/eskf_params.yaml +++ b/navigation/eskf/config/eskf_params.yaml @@ -1,7 +1,7 @@ eskf_node: ros__parameters: imu_topic: /imu/data_raw - dvl_topic: /dvl/sim + dvl_topic: /orca/twist odom_topic: odom diag_Q_std: [0.01, 0.01, 0.01, 0.005, 0.005, 0.005, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001] diag_p_init: [1.0, 1.0, 1.0, 0.5, 0.5, 0.5, 0.1, 0.1, 0.1, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001] diff --git a/navigation/eskf/include/eskf/eskf.hpp b/navigation/eskf/include/eskf/eskf.hpp index d9b7d4fa0..214f462c4 100644 --- a/navigation/eskf/include/eskf/eskf.hpp +++ b/navigation/eskf/include/eskf/eskf.hpp @@ -27,12 +27,6 @@ class ESKF { // NIS double NIS_; - // NEEDS - double NEES_; - - // ground truth - state_quat ground_truth_; - private: // @brief Predict the nominal state // @param imu_meas: IMU measurement @@ -53,8 +47,6 @@ class ESKF { // @param S: Innovation covariance matrix void NIS(const Eigen::Vector3d& innovation, const Eigen::Matrix3d& S); - void NEEDS(); - // @brief Update the error state // @param dvl_meas: DVL measurement void measurement_update(const dvl_measurement& dvl_meas); diff --git a/navigation/eskf/include/eskf/eskf_ros.hpp b/navigation/eskf/include/eskf/eskf_ros.hpp index c3d09e820..b10995f04 100644 --- a/navigation/eskf/include/eskf/eskf_ros.hpp +++ b/navigation/eskf/include/eskf/eskf_ros.hpp @@ -21,11 +21,6 @@ class ESKFNode : public rclcpp::Node { explicit ESKFNode(); private: - void pose_callback( - const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); - - void twist_callback( - const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); // @brief Callback function for the imu topic // @param msg: Imu message containing the imu data @@ -33,7 +28,7 @@ class ESKFNode : public rclcpp::Node { // @brief Callback function for the dvl topic // @param msg: TwistWithCovarianceStamped message containing the dvl data - void dvl_callback(const stonefish_ros2::msg::DVL::SharedPtr msg); + void dvl_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); // @brief Publish the odometry message void publish_odom(); @@ -46,28 +41,18 @@ class ESKFNode : public rclcpp::Node { rclcpp::Subscription::SharedPtr imu_sub_; - rclcpp::Subscription::SharedPtr dvl_sub_; - - rclcpp::Subscription< - geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_; - - rclcpp::Subscription< - geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr twist_sub_; + rclcpp::Subscription::SharedPtr dvl_sub_; rclcpp::Publisher::SharedPtr odom_pub_; rclcpp::Publisher::SharedPtr nis_pub_; - rclcpp::Publisher::SharedPtr nees_pub_; - std::chrono::milliseconds time_step; rclcpp::TimerBase::SharedPtr odom_pub_timer_; state_quat nom_state_; - state_quat g_truth_; - state_euler error_state_; imu_measurement imu_meas_; diff --git a/navigation/eskf/src/eskf.cpp b/navigation/eskf/src/eskf.cpp index e39993abc..8d1fdb6b4 100644 --- a/navigation/eskf/src/eskf.cpp +++ b/navigation/eskf/src/eskf.cpp @@ -160,22 +160,6 @@ void ESKF::NIS(const Eigen::Vector3d& innovation, const Eigen::Matrix3d& S) { Eigen::Matrix3d S_inv = S.inverse(); NIS_ = innovation.transpose() * S_inv * innovation; } - -void ESKF::NEEDS() { - Eigen::Vector18d error_state = current_nom_state_.nees_error(ground_truth_); - Eigen::Vector15d error_state_trim = error_state.head<15>(); - // Set the last 6 elements of error_state_trim to zero - for (int i = 9; i < 15; i++) { - error_state_trim(i) = 0; - } - error_state_trim(6) = 0.001; - error_state_trim(7) = 0.001; - error_state_trim(8) = 0.001; - Eigen::Matrix15d P = current_error_state_.covariance.block<15, 15>(0, 0); - Eigen::Matrix15d P_inv = P.inverse(); - NEES_ = error_state_trim.transpose() * P_inv * error_state_trim; -} - void ESKF::measurement_update(const dvl_measurement& dvl_meas) { Eigen::Matrix3x18d H = calculate_h_jacobian(); Eigen::Matrix18d P = current_error_state_.covariance; @@ -192,8 +176,6 @@ void ESKF::measurement_update(const dvl_measurement& dvl_meas) { current_error_state_.covariance = I_KH * P * I_KH.transpose() + K * R * K.transpose(); // Used joseph form for more stable calculations - - NEEDS(); } void ESKF::injection_and_reset() { diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp index b7cdced09..cc4256c18 100644 --- a/navigation/eskf/src/eskf_ros.cpp +++ b/navigation/eskf/src/eskf_ros.cpp @@ -20,16 +20,6 @@ void ESKFNode::set_subscribers_and_publisher() { auto qos_sensor_data = rclcpp::QoS( rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); - pose_sub_ = this->create_subscription< - geometry_msgs::msg::PoseWithCovarianceStamped>( - "/orca/pose", qos_sensor_data, - std::bind(&ESKFNode::pose_callback, this, std::placeholders::_1)); - - twist_sub_ = this->create_subscription< - geometry_msgs::msg::TwistWithCovarianceStamped>( - "/orca/twist", qos_sensor_data, - std::bind(&ESKFNode::twist_callback, this, std::placeholders::_1)); - this->declare_parameter("imu_topic"); std::string imu_topic = this->get_parameter("imu_topic").as_string(); imu_sub_ = this->create_subscription( @@ -38,7 +28,7 @@ void ESKFNode::set_subscribers_and_publisher() { this->declare_parameter("dvl_topic"); std::string dvl_topic = this->get_parameter("dvl_topic").as_string(); - dvl_sub_ = this->create_subscription( + dvl_sub_ = this->create_subscription( dvl_topic, qos_sensor_data, std::bind(&ESKFNode::dvl_callback, this, std::placeholders::_1)); @@ -48,7 +38,6 @@ void ESKFNode::set_subscribers_and_publisher() { odom_topic, qos_sensor_data); nis_pub_ = create_publisher("dvl/nis", 10); - nees_pub_ = create_publisher("dvl/nees", 10); } void ESKFNode::set_parameters() { @@ -80,22 +69,6 @@ void ESKFNode::set_parameters() { error_state_.covariance = P; } -void ESKFNode::pose_callback( - const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { - g_truth_.pos << msg->pose.pose.position.x, msg->pose.pose.position.y, - msg->pose.pose.position.z; - g_truth_.quat.w() = msg->pose.pose.orientation.w; - g_truth_.quat.x() = msg->pose.pose.orientation.x; - g_truth_.quat.y() = msg->pose.pose.orientation.y; - g_truth_.quat.z() = msg->pose.pose.orientation.z; -} - -void ESKFNode::twist_callback( - const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { - g_truth_.vel << msg->twist.twist.linear.x, msg->twist.twist.linear.y, - msg->twist.twist.linear.z; -} - void ESKFNode::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { rclcpp::Time current_time = msg->header.stamp; @@ -122,37 +95,14 @@ void ESKFNode::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { std::tie(nom_state_, error_state_) = eskf_->imu_update(imu_meas_, dt); } -void ESKFNode::dvl_callback(const stonefish_ros2::msg::DVL::SharedPtr msg) { +void ESKFNode::dvl_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { // Log that we received a DVL message // spdlog::info("DVL message received"); - dvl_meas_.vel << msg->velocity.x, msg->velocity.y, msg->velocity.z; - dvl_meas_.cov << 0.004*0.004, 0.0, 0.0, 0.0, 0.004*0.004, 0.0, 0.0, 0.0, 0.004*0.004; - - // msg->velocity_covariance[0], msg->velocity_covariance[1], - // msg->velocity_covariance[2], - // msg->velocity_covariance[3], msg->velocity_covariance[4], - // msg->velocity_covariance[5], - // msg->velocity_covariance[6], msg->velocity_covariance[7], - // msg->velocity_covariance[8]; - - // Set biases and gravity as float values - float gyro_bias_x = 0.00001; - float gyro_bias_y = 0.00001; - float gyro_bias_z = 0.00001; - - float accel_bias_x = 0.00001; - float accel_bias_y = 0.00001; - float accel_bias_z = 0.00001; - - float gravity_x = 0.0; - float gravity_y = 0.0; - float gravity_z = 9.81; - - g_truth_.gyro_bias << gyro_bias_x, gyro_bias_y, gyro_bias_z; - g_truth_.accel_bias << accel_bias_x, accel_bias_y, accel_bias_z; - g_truth_.gravity << gravity_x, gravity_y, gravity_z; + dvl_meas_.vel << msg->twist.twist.linear.x, msg->twist.twist.linear.y, msg->twist.twist.linear.z; - eskf_->ground_truth_ = g_truth_; + dvl_meas_.cov << msg->twist.covariance[0], msg->twist.covariance[1], msg->twist.covariance[2], + msg->twist.covariance[6], msg->twist.covariance[7], msg->twist.covariance[8], + msg->twist.covariance[12], msg->twist.covariance[13], msg->twist.covariance[14]; std::tie(nom_state_, error_state_) = eskf_->dvl_update(dvl_meas_); @@ -160,9 +110,6 @@ void ESKFNode::dvl_callback(const stonefish_ros2::msg::DVL::SharedPtr msg) { nis_msg.data = eskf_->NIS_; nis_pub_->publish(nis_msg); - std_msgs::msg::Float64 nees_msg; - nees_msg.data = eskf_->NEES_; - nees_pub_->publish(nees_msg); } void ESKFNode::publish_odom() { diff --git a/navigation/tukf/simulation_re.ipynb b/navigation/tukf/simulation_re.ipynb index e508825bf..06f7369a6 100644 --- a/navigation/tukf/simulation_re.ipynb +++ b/navigation/tukf/simulation_re.ipynb @@ -11,7 +11,7 @@ }, { "cell_type": "code", - "execution_count": 2, + "execution_count": 1, "metadata": {}, "outputs": [], "source": [ @@ -24,7 +24,7 @@ }, { "cell_type": "code", - "execution_count": 3, + "execution_count": 2, "metadata": {}, "outputs": [], "source": [ @@ -33,33 +33,33 @@ }, { "cell_type": "code", - "execution_count": 5, + "execution_count": 3, "metadata": {}, "outputs": [], "source": [ "# Initialize UKF with StateQuat\n", - "initial_position = np.array([0.0, 0.0, 0.0]) # x, y, z\n", + "initial_position = np.array([0.0, 0.0, 0.13]) # x, y, z\n", "initial_velocity = np.array([0.0, 0.0, 0.0]) # vx, vy, vz\n", "initial_quaternion = np.array([0.0, 0.0, 0.0]) # w, x, y, z (identity quaternion)\n", "initial_angular_velocity = np.array([0.0, 0.0, 0.0]) # wx, wy, wz\n", "initial_g_eta = np.array([0.01, 0.01, 0.01, 0.01]) # g_eta parameters\n", - "initial_intertia = np.array([0.2, 0.2, 0.1, \n", - " 0.2, 0.2, 0.2, \n", - " 0.1, 0.2, 0.2])\n", + "initial_intertia = np.array([0.2, 0.05, 0.01, \n", + " 0.05, 0.2, 0.05, \n", + " 0.01, 0.05, 0.2])\n", "initial_damping = np.array([0.01, 0.01, 0.01,\n", " 0.01, 0.01, 0.01])\n", "initla_added_mass = np.array([0.02, 0.02, 0.02,\n", " 0.02, 0.02, 0.02])\n", "\n", "p_diag = np.concatenate([\n", - " 2*np.ones(3), # x position\n", - " 2*np.ones(3), # orientation\n", - " 2*np.ones(3), # velocity\n", - " 2*np.ones(3), # angular velocity\n", - " 2*np.ones(9), # inertia\n", - " 2*np.ones(6), # added mass\n", - " 2*np.ones(6), # damping\n", - " 2*np.ones(4) # g_eta\n", + " (0.8**2)*np.ones(3), # x position\n", + " (0.4**2)*np.ones(3), # orientation\n", + " (0.4**2)*np.ones(3), # velocity\n", + " (0.5**2)*np.ones(3), # angular velocity\n", + " 30*np.ones(9), # inertia\n", + " 30*np.ones(6), # added mass\n", + " 30*np.ones(6), # damping\n", + " 30*np.ones(4) # g_eta\n", "])\n", "\n", "initial_covariance = np.diag(p_diag) \n", @@ -70,12 +70,14 @@ "state.covariance = initial_covariance.copy()\n", "\n", "Q_diag = np.concatenate([\n", - " 0.1*np.ones(3), # position\n", - " 0.1*np.ones(9), # kinematic (η & ν)\n", - " 0.001*np.ones(9), # inertia\n", - " 0.001*np.ones(6), # added mass\n", - " 0.001*np.ones(6), # damping\n", - " 0.001*np.ones(4), # g_eta\n", + " (0.09**2)*np.ones(3), # position\n", + " (0.06**2)*np.ones(3), # kinematic (η & ν)\n", + " (0.06**2)*np.ones(3),\n", + " (0.06**2)*np.ones(3),\n", + " 0.000001*np.ones(9), # inertia\n", + " 0.000001*np.ones(6), # added mass\n", + " 0.000001*np.ones(6), # damping\n", + " 0.000001*np.ones(4), # g_eta\n", "])\n", "\n", "UKF_model = TUKF(state, np.diag(Q_diag)) # Process noise covariance\n", @@ -119,7 +121,7 @@ }, { "cell_type": "code", - "execution_count": 8, + "execution_count": 4, "metadata": {}, "outputs": [], "source": [ @@ -162,7 +164,7 @@ "\n", " ang_measurement.measurement = np.dot(R_corr,measurement_imu)\n", "\n", - " ang_measurement.covariance = np.eye(3) * (0.03**2) \n", + " ang_measurement.covariance = np.eye(3) * (0.005**2) \n", "\n", " UKF_model.measurement_update(state, ang_measurement)\n", " state = UKF_model.posteriori_estimate(state, ang_measurement)\n", @@ -173,7 +175,7 @@ " msg = reader.deserialize(raw, conn.msgtype)\n", " dvl_measurement.measurement = np.array([msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z])\n", " \n", - " dvl_measurement.covariance = np.eye(3) * (0.01**2) \n", + " dvl_measurement.covariance = np.eye(3) * (0.005**2) \n", " \n", " # Update UKF with measurement\n", " UKF_model.measurement_update(state, dvl_measurement)\n", @@ -192,12 +194,12 @@ }, { "cell_type": "code", - "execution_count": 9, + "execution_count": 5, "metadata": {}, "outputs": [ { "data": { - "image/png": "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", + "image/png": "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", "text/plain": [ "
" ] @@ -231,7 +233,7 @@ "axs[1].grid(True)\n", "\n", "# Plot z position\n", - "axs[2].plot(z_positions, 'r-', linewidth=2)\n", + "axs[2].plot(z_positions, 'r-', linewidth=2) \n", "axs[2].set_ylabel('Z Position [m]')\n", "axs[2].set_xlabel('Time steps')\n", "axs[2].grid(True)\n", @@ -239,6 +241,104 @@ "plt.tight_layout(rect=[0, 0.03, 1, 0.95])\n", "plt.show()" ] + }, + { + "cell_type": "code", + "execution_count": 6, + "metadata": {}, + "outputs": [ + { + "data": { + "image/png": "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", + "text/plain": [ + "
" + ] + }, + "metadata": {}, + "output_type": "display_data" + }, + { + "data": { + "image/png": "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", + "text/plain": [ + "
" + ] + }, + "metadata": {}, + "output_type": "display_data" + }, + { + "data": { + "image/png": "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", + "text/plain": [ + "
" + ] + }, + "metadata": {}, + "output_type": "display_data" + }, + { + "data": { + "image/png": "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", + "text/plain": [ + "
" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ + "# Convert okid_params_est from list of arrays to a numpy array for easier handling\n", + "okid_params_array = np.array(okid_params_est)\n", + "\n", + "# Create labels for the different parameter groups\n", + "inertia_labels = ['Ixx', 'Ixy', 'Ixz', 'Iyx', 'Iyy', 'Iyz', 'Izx', 'Izy', 'Izz']\n", + "added_mass_labels = ['Xu', 'Yv', 'Zw', 'Kp', 'Mq', 'Nr']\n", + "damping_labels = ['Xu', 'Yv', 'Zw', 'Kp', 'Mq', 'Nr']\n", + "g_eta_labels = ['g_eta1', 'g_eta2', 'g_eta3', 'g_eta4']\n", + "\n", + "# Create figures for each parameter group\n", + "plt.figure(figsize=(14, 10))\n", + "plt.suptitle('Inertia Matrix Parameters', fontsize=16)\n", + "for i in range(9):\n", + " plt.subplot(3, 3, i+1)\n", + " plt.plot(okid_params_array[:, i], 'b-')\n", + " plt.title(inertia_labels[i])\n", + " plt.grid(True)\n", + "plt.tight_layout(rect=[0, 0.03, 1, 0.95])\n", + "plt.show()\n", + "\n", + "plt.figure(figsize=(14, 8))\n", + "plt.suptitle('Added Mass Parameters', fontsize=16)\n", + "for i in range(6):\n", + " plt.subplot(2, 3, i+1)\n", + " plt.plot(okid_params_array[:, 9+i], 'g-')\n", + " plt.title(f'Added Mass: {added_mass_labels[i]}')\n", + " plt.grid(True)\n", + "plt.tight_layout(rect=[0, 0.03, 1, 0.95])\n", + "plt.show()\n", + "\n", + "plt.figure(figsize=(14, 8))\n", + "plt.suptitle('Damping Parameters', fontsize=16)\n", + "for i in range(6):\n", + " plt.subplot(2, 3, i+1)\n", + " plt.plot(okid_params_array[:, 15+i], 'r-')\n", + " plt.title(f'Damping: {damping_labels[i]}')\n", + " plt.grid(True)\n", + "plt.tight_layout(rect=[0, 0.03, 1, 0.95])\n", + "plt.show()\n", + "\n", + "plt.figure(figsize=(14, 6))\n", + "plt.suptitle('G-Eta Parameters', fontsize=16)\n", + "for i in range(4):\n", + " plt.subplot(2, 2, i+1)\n", + " plt.plot(okid_params_array[:, 21+i], 'm-')\n", + " plt.title(g_eta_labels[i])\n", + " plt.grid(True)\n", + "plt.tight_layout(rect=[0, 0.03, 1, 0.95])\n", + "plt.show()" + ] } ], "metadata": { diff --git a/navigation/tukf/tukf_class.py b/navigation/tukf/tukf_class.py index e3ec138fb..ff274680e 100644 --- a/navigation/tukf/tukf_class.py +++ b/navigation/tukf/tukf_class.py @@ -311,7 +311,7 @@ def angular_velocity_transformation(euler_angles: np.ndarray) -> np.ndarray: return T def M_rb(inertia: np.ndarray) -> np.ndarray: - m = 30.0 + m = 25.5 inertia = inertia.reshape((3, 3)) r_b_bg = np.array([0.01, 0.0, 0.02]) M_rb = np.zeros((6, 6)) @@ -330,7 +330,7 @@ def M_a(added_mass: np.ndarray) -> np.ndarray: def C_rb(inertia: np.ndarray, angular_velocity: np.ndarray) -> np.ndarray: """Calculates the Coriolis matrix.""" - m = 30.0 + m = 25.5 r_b_bg = np.array([0.01, 0.0, 0.02]) inertia = inertia.reshape((3, 3)) C_rb = np.zeros((6, 6)) From 017a1f89a271f3e3be551d7ce232777f9cf3e2c6 Mon Sep 17 00:00:00 2001 From: Talha Nauman Choudhry Date: Fri, 30 May 2025 19:18:16 +0200 Subject: [PATCH 104/290] added the cpp code --- navigation/eskf/config/eskf_params.yaml | 2 +- navigation/eskf/include/eskf/eskf_ros.hpp | 2 +- navigation/eskf/include/eskf/typedefs.hpp | 4 +- navigation/eskf/src/eskf.cpp | 43 +- navigation/eskf/src/eskf_ros.cpp | 8 + navigation/tukf/simulation.py | 155 --- navigation/tukf/simulation_re.ipynb | 365 -------- navigation/tukf/test_tukf.py | 347 ------- navigation/tukf/tukf.py | 126 --- navigation/tukf/tukf_class.py | 437 --------- navigation/tukf_rsi/CMakeLists.txt | 61 ++ .../tukf_rsi/config/tusk_rsi_params.yaml | 12 + .../tukf_rsi/include/tukf_rsi/tukf_rsi.hpp | 63 ++ .../include/tukf_rsi/tukf_rsi_model.hpp | 50 + .../include/tukf_rsi/tukf_rsi_ros.hpp | 64 ++ .../include/tukf_rsi/tukf_rsi_utils.hpp | 65 ++ .../tukf_rsi/include/tukf_rsi/typedefs.hpp | 145 +++ navigation/tukf_rsi/launch/tukf_rsi.launch.py | 22 + navigation/{ukf_okid => tukf_rsi}/package.xml | 14 +- navigation/tukf_rsi/src/tukf_rsi.cpp | 84 ++ navigation/tukf_rsi/src/tukf_rsi_model.cpp | 125 +++ navigation/tukf_rsi/src/tukf_rsi_node.cpp | 9 + navigation/tukf_rsi/src/tukf_rsi_ros.cpp | 150 +++ navigation/tukf_rsi/src/tukf_rsi_utils.cpp | 199 ++++ navigation/ukf_okid/CMakeLists.txt | 23 - navigation/ukf_okid/launch/ukf.launch.py | 12 - navigation/ukf_okid/ukf_python/__init__.py | 0 navigation/ukf_okid/ukf_python/ukf_okid.py | 130 --- .../ukf_okid/ukf_python/ukf_okid_class.py | 880 ------------------ navigation/ukf_okid/ukf_python/ukf_ros.py | 159 ---- navigation/ukf_okid/ukf_python/ukf_test.py | 27 - navigation/ukf_okid/ukf_python/ukf_test_2.py | 229 ----- navigation/ukf_okid/ukf_python/ukf_utils.py | 35 - 33 files changed, 1079 insertions(+), 2968 deletions(-) delete mode 100644 navigation/tukf/simulation.py delete mode 100644 navigation/tukf/simulation_re.ipynb delete mode 100644 navigation/tukf/test_tukf.py delete mode 100644 navigation/tukf/tukf.py delete mode 100644 navigation/tukf/tukf_class.py create mode 100644 navigation/tukf_rsi/CMakeLists.txt create mode 100644 navigation/tukf_rsi/config/tusk_rsi_params.yaml create mode 100644 navigation/tukf_rsi/include/tukf_rsi/tukf_rsi.hpp create mode 100644 navigation/tukf_rsi/include/tukf_rsi/tukf_rsi_model.hpp create mode 100644 navigation/tukf_rsi/include/tukf_rsi/tukf_rsi_ros.hpp create mode 100644 navigation/tukf_rsi/include/tukf_rsi/tukf_rsi_utils.hpp create mode 100644 navigation/tukf_rsi/include/tukf_rsi/typedefs.hpp create mode 100644 navigation/tukf_rsi/launch/tukf_rsi.launch.py rename navigation/{ukf_okid => tukf_rsi}/package.xml (56%) create mode 100644 navigation/tukf_rsi/src/tukf_rsi.cpp create mode 100644 navigation/tukf_rsi/src/tukf_rsi_model.cpp create mode 100644 navigation/tukf_rsi/src/tukf_rsi_node.cpp create mode 100644 navigation/tukf_rsi/src/tukf_rsi_ros.cpp create mode 100644 navigation/tukf_rsi/src/tukf_rsi_utils.cpp delete mode 100644 navigation/ukf_okid/CMakeLists.txt delete mode 100644 navigation/ukf_okid/launch/ukf.launch.py delete mode 100644 navigation/ukf_okid/ukf_python/__init__.py delete mode 100644 navigation/ukf_okid/ukf_python/ukf_okid.py delete mode 100644 navigation/ukf_okid/ukf_python/ukf_okid_class.py delete mode 100755 navigation/ukf_okid/ukf_python/ukf_ros.py delete mode 100644 navigation/ukf_okid/ukf_python/ukf_test.py delete mode 100644 navigation/ukf_okid/ukf_python/ukf_test_2.py delete mode 100644 navigation/ukf_okid/ukf_python/ukf_utils.py diff --git a/navigation/eskf/config/eskf_params.yaml b/navigation/eskf/config/eskf_params.yaml index c08c21d3c..f17dcd3df 100644 --- a/navigation/eskf/config/eskf_params.yaml +++ b/navigation/eskf/config/eskf_params.yaml @@ -3,6 +3,6 @@ eskf_node: imu_topic: /imu/data_raw dvl_topic: /orca/twist odom_topic: odom - diag_Q_std: [0.01, 0.01, 0.01, 0.005, 0.005, 0.005, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001] + diag_Q_std: [0.0124, 0.0124, 0.0124, 0.0026, 0.0026, 0.0026, 0.000392, 0.000392, 0.000392, 0.00001145, 0.0000145, 0.0000145] diag_p_init: [1.0, 1.0, 1.0, 0.5, 0.5, 0.5, 0.1, 0.1, 0.1, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001] imu_frame: [0.0, 0.0, -1.0, 0.0, -1.0, 0.0, -1.0, 0.0, 0.0] diff --git a/navigation/eskf/include/eskf/eskf_ros.hpp b/navigation/eskf/include/eskf/eskf_ros.hpp index b10995f04..d4a770864 100644 --- a/navigation/eskf/include/eskf/eskf_ros.hpp +++ b/navigation/eskf/include/eskf/eskf_ros.hpp @@ -21,7 +21,7 @@ class ESKFNode : public rclcpp::Node { explicit ESKFNode(); private: - + // @brief Callback function for the imu topic // @param msg: Imu message containing the imu data void imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg); diff --git a/navigation/eskf/include/eskf/typedefs.hpp b/navigation/eskf/include/eskf/typedefs.hpp index eacb32841..4dbdb6da3 100644 --- a/navigation/eskf/include/eskf/typedefs.hpp +++ b/navigation/eskf/include/eskf/typedefs.hpp @@ -38,9 +38,9 @@ Eigen::Matrix createDiagonalMatrix( } struct state_quat { - Eigen::Vector3d pos = Eigen::Vector3d(0,0,0.125); + Eigen::Vector3d pos = Eigen::Vector3d(5.58, 0.66, 0.12); Eigen::Vector3d vel = Eigen::Vector3d::Zero(); - Eigen::Quaterniond quat = Eigen::Quaterniond::Identity(); + Eigen::Quaterniond quat = Eigen::Quaterniond(0.98, -0.047, 0.028, -0.18); Eigen::Vector3d gyro_bias = Eigen::Vector3d::Zero(); Eigen::Vector3d accel_bias = Eigen::Vector3d::Zero(); Eigen::Vector3d gravity = Eigen::Vector3d::Zero(); diff --git a/navigation/eskf/src/eskf.cpp b/navigation/eskf/src/eskf.cpp index 8d1fdb6b4..3838e2f1a 100644 --- a/navigation/eskf/src/eskf.cpp +++ b/navigation/eskf/src/eskf.cpp @@ -43,6 +43,7 @@ Eigen::Matrix4x3d ESKF::calculate_q_delta() { q_delta_theta *= 0.5; return q_delta_theta; } + Eigen::Matrix3x19d ESKF::calculate_hx() { Eigen::Matrix3x19d Hx = Eigen::Matrix3x19d::Zero(); @@ -51,43 +52,21 @@ Eigen::Matrix3x19d ESKF::calculate_hx() { Eigen::Vector3d v_n = current_nom_state_.vel; + // Correct derivative w.r.t velocity (nominal state: v_n) Hx.block<3, 3>(0, 3) = R_bn.transpose(); + // Derivative w.r.t quaternion (nominal state: q) + // Compute partial derivative w.r.t quaternion directly: double qw = q.w(); - double qx = q.x(); - double qy = q.y(); - double qz = q.z(); - + Eigen::Vector3d q_vec(q.x(), q.y(), q.z()); Eigen::Matrix3d I3 = Eigen::Matrix3d::Identity(); - Eigen::Vector3d eps(qx, qy, qz); - - Eigen::Matrix3d dR_deta = 2 * qw * I3 - 2 * skew(eps); - - Eigen::Vector3d e1_vec(1, 0, 0), e2_vec(0, 1, 0), e3_vec(0, 0, 1); - - Eigen::Matrix3d dR_dqx = - -2 * qx * I3 + - 2 * (e1_vec * eps.transpose() + eps * e1_vec.transpose()) - - 2 * qw * skew(e1_vec); + Eigen::Matrix dhdq; + dhdq.col(0) = 2*( qw*v_n + q_vec.cross(v_n) ); + dhdq.block<3,3>(0,1) = 2*( q_vec.dot(v_n)*I3 + q_vec*v_n.transpose() - v_n*q_vec.transpose() - qw*skew(v_n) ); - Eigen::Matrix3d dR_dqy = - -2 * qy * I3 + - 2 * (e2_vec * eps.transpose() + eps * e2_vec.transpose()) - - 2 * qw * skew(e2_vec); - - Eigen::Matrix3d dR_dqz = - -2 * qz * I3 + - 2 * (e3_vec * eps.transpose() + eps * e3_vec.transpose()) - - 2 * qw * skew(e3_vec); - - Eigen::Matrix dR_dq; - dR_dq.col(0) = dR_deta * v_n; - dR_dq.col(1) = dR_dqx * v_n; - dR_dq.col(2) = dR_dqy * v_n; - dR_dq.col(3) = dR_dqz * v_n; - - Hx.block<3, 4>(0, 6) = dR_dq; + // Assign quaternion derivative (3x4 block at columns 6:9) + Hx.block<3,4>(0,6) = dhdq; return Hx; } @@ -107,7 +86,7 @@ Eigen::Vector3d ESKF::calculate_h() { Eigen::Matrix3d R_bn = current_nom_state_.quat.normalized().toRotationMatrix().transpose(); h = R_bn * current_nom_state_.vel; - + //0.027293, 0.028089, 0.028089, 0.00255253, 0.00270035, 0.00280294, return h; } diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp index cc4256c18..cfa85172e 100644 --- a/navigation/eskf/src/eskf_ros.cpp +++ b/navigation/eskf/src/eskf_ros.cpp @@ -128,6 +128,14 @@ void ESKFNode::publish_odom() { odom_msg.twist.twist.linear.y = nom_state_.vel.y(); odom_msg.twist.twist.linear.z = nom_state_.vel.z(); + // Add bias values to the angular velocity field of twist + odom_msg.twist.twist.angular.x = nom_state_.accel_bias.x(); + odom_msg.twist.twist.angular.y = nom_state_.accel_bias.y(); + odom_msg.twist.twist.angular.z = nom_state_.accel_bias.z(); + + // If you also want to include gyro bias, you could add it to the covariance + // matrix or publish a separate topic for biases + odom_msg.header.stamp = this->now(); odom_pub_->publish(odom_msg); } diff --git a/navigation/tukf/simulation.py b/navigation/tukf/simulation.py deleted file mode 100644 index 56fb2a6a1..000000000 --- a/navigation/tukf/simulation.py +++ /dev/null @@ -1,155 +0,0 @@ -from rosbags.highlevel import AnyReader -from rosbags.typesys import get_types_from_msg, register_types -import numpy as np, pandas as pd, pathlib -from tukf import TUKF -import tukf_class as ukf - -bag_folder = pathlib.Path(r"/home/talha/vortex_auv_ws/bags/sim_data_no_dvl") - -# Initialize UKF with StateQuat -initial_position = np.array([0.0, 0.0, 0.0]) # x, y, z -initial_velocity = np.array([0.0, 0.0, 0.0]) # vx, vy, vz -initial_quaternion = np.array([0.0, 0.0, 0.0]) # w, x, y, z (identity quaternion) -initial_angular_velocity = np.array([0.0, 0.0, 0.0]) # wx, wy, wz -initial_g_eta = np.array([0.01, 0.01, 0.01, 0.01]) # g_eta parameters -initial_intertia = np.array([0.2, 0.2, 0.1, - 0.2, 0.2, 0.2, - 0.1, 0.2, 0.2]) -initial_damping = np.array([0.01, 0.01, 0.01, - 0.01, 0.01, 0.01]) -initla_added_mass = np.array([0.02, 0.02, 0.02, - 0.02, 0.02, 0.02]) - -p_diag = np.concatenate([ - 2*np.ones(3), # x position - 2*np.ones(3), # orientation - 2*np.ones(3), # velocity - 2*np.ones(3), # angular velocity - 2*np.ones(9), # inertia - 2*np.ones(6), # added mass - 2*np.ones(6), # damping - 2*np.ones(4) # g_eta -]) - -initial_covariance = np.diag(p_diag) - -state = ukf.AUVState(initial_position.copy(), initial_quaternion.copy(), initial_velocity.copy(), initial_angular_velocity.copy(), - initial_intertia.copy(), initla_added_mass.copy(), initial_damping.copy(), initial_g_eta.copy()) - -state.covariance = initial_covariance.copy() - -Q_diag = np.concatenate([ - 0.1*np.ones(3), # position - 0.1*np.ones(9), # kinematic (η & ν) - 0.001*np.ones(9), # inertia - 0.001*np.ones(6), # added mass - 0.001*np.ones(6), # damping - 0.001*np.ones(4), # g_eta -]) - -UKF_model = TUKF(state, np.diag(Q_diag)) # Process noise covariance - -def dvl_h(state: ukf.AUVState) -> 'ukf.MeasModel': - H_matrix = np.zeros((3, 12)) - H_matrix[:, 6:9] = np.eye(3) - z_i = ukf.MeasModel() - z_i.measurement = np.dot(H_matrix, state.dynamic_part()) - return z_i - -dvl_measurement = ukf.MeasModel(H=dvl_h) - -def ang_h(state: ukf.AUVState) -> 'ukf.MeasModel': - H_matrix = np.zeros((3, 12)) - H_matrix[:, 9:12] = np.eye(3) - z_i = ukf.MeasModel() - z_i.measurement = np.dot(H_matrix, state.dynamic_part()) - return z_i - -ang_measurement = ukf.MeasModel(H=ang_h) - -R_corr = np.array([[0.0, 0.0, -1.0], - [0.0, -1.0, 0.0], - [-1.0, 0.0, 0.0]]) - -# Storage for trajectory -positions = [] -velocities = [] -quaternions = [] -angular_velocities = [] -okid_params = [] - -# Storage for trajectory estimates -positions_est = [] -velocities_est = [] -quaternions_est = [] -angular_velocities_est = [] -okid_params_est = [] - -with AnyReader([bag_folder]) as reader: - # Filter topics once - conns = [c for c in reader.connections - if c.topic in ("/imu/data_raw", "/dvl_twist", "/orca/wrench_input")] - - last_time = None - log = [] - coutner = 0 - - for conn, ts_raw, raw in reader.messages(conns): - t_ns = ts_raw # already nanoseconds integer - coutner += 1 - - if conn.topic == "/orca/wrench_input": - - # Get the wrench input message - msg = reader.deserialize(raw, conn.msgtype) - wrench = msg.wrench # Extract the wrench field from the message - forces = np.array([wrench.force.x, wrench.force.y, wrench.force.z]) - torques = np.array([wrench.torque.x, wrench.torque.y, wrench.torque.z]) - - control_input = np.concatenate([forces, torques]) - - if last_time is not None: - UKF_model.dt = (t_ns - last_time) / 1e9 # Convert nanoseconds to seconds - - # 1. prediction step - state = UKF_model.unscented_transform(state, control_input) - - # 2. measurement update - msg = reader.deserialize(raw, conn.msgtype) - if conn.topic == "/imu/data_raw": - print("IMU data received") - - # Get the IMU data - measurement_imu = np.array([msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z]) - - ang_measurement.measurement = np.dot(R_corr,measurement_imu) - - ang_measurement.covariance = np.eye(3) * (0.03**2) - - UKF_model.measurement_update(state, ang_measurement) - state = UKF_model.posteriori_estimate(state, ang_measurement) - - if conn.topic == "/dvl_twist": - print("DVL data received") - - msg = reader.deserialize(raw, conn.msgtype) - dvl_measurement.measurement = np.array([msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z]) - dvl_measurement.measurement = np.dot(R_corr, dvl_measurement.measurement) - - dvl_measurement.covariance = np.eye(3) * (0.01**2) - - # Update UKF with measurement - UKF_model.measurement_update(state, dvl_measurement) - state = UKF_model.posteriori_estimate(state, dvl_measurement) - - - # Store the state estimates - positions_est.append(state.position.copy()) - velocities_est.append(state.velocity.copy()) - quaternions_est.append(state.quaternion.copy()) - angular_velocities_est.append(state.angular_velocity.copy()) - okid_params_est.append(state.okid_part().copy()) - - last_time = t_ns - - \ No newline at end of file diff --git a/navigation/tukf/simulation_re.ipynb b/navigation/tukf/simulation_re.ipynb deleted file mode 100644 index 06f7369a6..000000000 --- a/navigation/tukf/simulation_re.ipynb +++ /dev/null @@ -1,365 +0,0 @@ -{ - "cells": [ - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "### Simulation results for the TUKF ###\n", - "\n", - "try to simulate the path and parameters using the tukf algorithm" - ] - }, - { - "cell_type": "code", - "execution_count": 1, - "metadata": {}, - "outputs": [], - "source": [ - "from rosbags.highlevel import AnyReader\n", - "from rosbags.typesys import get_types_from_msg, register_types\n", - "import numpy as np, pandas as pd, pathlib\n", - "from tukf import TUKF\n", - "import tukf_class as ukf" - ] - }, - { - "cell_type": "code", - "execution_count": 2, - "metadata": {}, - "outputs": [], - "source": [ - "bag_folder = pathlib.Path(r\"/home/talha/vortex_auv_ws/bags/sim_data_no_dvl\")" - ] - }, - { - "cell_type": "code", - "execution_count": 3, - "metadata": {}, - "outputs": [], - "source": [ - "# Initialize UKF with StateQuat\n", - "initial_position = np.array([0.0, 0.0, 0.13]) # x, y, z\n", - "initial_velocity = np.array([0.0, 0.0, 0.0]) # vx, vy, vz\n", - "initial_quaternion = np.array([0.0, 0.0, 0.0]) # w, x, y, z (identity quaternion)\n", - "initial_angular_velocity = np.array([0.0, 0.0, 0.0]) # wx, wy, wz\n", - "initial_g_eta = np.array([0.01, 0.01, 0.01, 0.01]) # g_eta parameters\n", - "initial_intertia = np.array([0.2, 0.05, 0.01, \n", - " 0.05, 0.2, 0.05, \n", - " 0.01, 0.05, 0.2])\n", - "initial_damping = np.array([0.01, 0.01, 0.01,\n", - " 0.01, 0.01, 0.01])\n", - "initla_added_mass = np.array([0.02, 0.02, 0.02,\n", - " 0.02, 0.02, 0.02])\n", - "\n", - "p_diag = np.concatenate([\n", - " (0.8**2)*np.ones(3), # x position\n", - " (0.4**2)*np.ones(3), # orientation\n", - " (0.4**2)*np.ones(3), # velocity\n", - " (0.5**2)*np.ones(3), # angular velocity\n", - " 30*np.ones(9), # inertia\n", - " 30*np.ones(6), # added mass\n", - " 30*np.ones(6), # damping\n", - " 30*np.ones(4) # g_eta\n", - "])\n", - "\n", - "initial_covariance = np.diag(p_diag) \n", - "\n", - "state = ukf.AUVState(initial_position.copy(), initial_quaternion.copy(), initial_velocity.copy(), initial_angular_velocity.copy(), \n", - " initial_intertia.copy(), initla_added_mass.copy(), initial_damping.copy(), initial_g_eta.copy())\n", - "\n", - "state.covariance = initial_covariance.copy()\n", - "\n", - "Q_diag = np.concatenate([\n", - " (0.09**2)*np.ones(3), # position\n", - " (0.06**2)*np.ones(3), # kinematic (η & ν)\n", - " (0.06**2)*np.ones(3),\n", - " (0.06**2)*np.ones(3),\n", - " 0.000001*np.ones(9), # inertia\n", - " 0.000001*np.ones(6), # added mass\n", - " 0.000001*np.ones(6), # damping\n", - " 0.000001*np.ones(4), # g_eta\n", - "])\n", - "\n", - "UKF_model = TUKF(state, np.diag(Q_diag)) # Process noise covariance\n", - "\n", - "def dvl_h(state: ukf.AUVState) -> 'ukf.MeasModel':\n", - " H_matrix = np.zeros((3, 12))\n", - " H_matrix[:, 6:9] = np.eye(3)\n", - " z_i = ukf.MeasModel()\n", - " z_i.measurement = np.dot(H_matrix, state.dynamic_part())\n", - " return z_i\n", - "\n", - "dvl_measurement = ukf.MeasModel(H=dvl_h)\n", - "\n", - "def ang_h(state: ukf.AUVState) -> 'ukf.MeasModel':\n", - " H_matrix = np.zeros((3, 12))\n", - " H_matrix[:, 9:12] = np.eye(3)\n", - " z_i = ukf.MeasModel()\n", - " z_i.measurement = np.dot(H_matrix, state.dynamic_part())\n", - " return z_i\n", - "\n", - "ang_measurement = ukf.MeasModel(H=ang_h) \n", - "\n", - "R_corr = np.array([[0.0, 0.0, -1.0],\n", - " [0.0, -1.0, 0.0],\n", - " [-1.0, 0.0, 0.0]])\n", - "\n", - "# Storage for trajectory\n", - "positions = []\n", - "velocities = []\n", - "quaternions = []\n", - "angular_velocities = []\n", - "okid_params = []\n", - "\n", - "# Storage for trajectory estimates\n", - "positions_est = []\n", - "velocities_est = []\n", - "quaternions_est = []\n", - "angular_velocities_est = []\n", - "okid_params_est = []" - ] - }, - { - "cell_type": "code", - "execution_count": 4, - "metadata": {}, - "outputs": [], - "source": [ - "with AnyReader([bag_folder]) as reader:\n", - " # Filter topics once\n", - " conns = [c for c in reader.connections\n", - " if c.topic in (\"/imu/data_raw\", \"/dvl_twist\", \"/orca/wrench_input\")]\n", - "\n", - " last_time = None\n", - " log = []\n", - " coutner = 0\n", - "\n", - " for conn, ts_raw, raw in reader.messages(conns):\n", - " t_ns = ts_raw # already nanoseconds integer\n", - " coutner += 1\n", - "\n", - " if conn.topic == \"/orca/wrench_input\":\n", - " \n", - " # Get the wrench input message\n", - " msg = reader.deserialize(raw, conn.msgtype)\n", - " wrench = msg.wrench # Extract the wrench field from the message\n", - " forces = np.array([wrench.force.x, wrench.force.y, wrench.force.z])\n", - " torques = np.array([wrench.torque.x, wrench.torque.y, wrench.torque.z])\n", - "\n", - " control_input = np.concatenate([forces, torques])\n", - "\n", - " if last_time is not None:\n", - " UKF_model.dt = (t_ns - last_time) / 1e9 # Convert nanoseconds to seconds\n", - "\n", - " # 1. prediction step\n", - " state = UKF_model.unscented_transform(state, control_input)\n", - "\n", - " # 2. measurement update\n", - " msg = reader.deserialize(raw, conn.msgtype)\n", - " if conn.topic == \"/imu/data_raw\":\n", - " # print(\"IMU data received\")\n", - "\n", - " # Get the IMU data\n", - " measurement_imu = np.array([msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z])\n", - "\n", - " ang_measurement.measurement = np.dot(R_corr,measurement_imu)\n", - "\n", - " ang_measurement.covariance = np.eye(3) * (0.005**2) \n", - "\n", - " UKF_model.measurement_update(state, ang_measurement)\n", - " state = UKF_model.posteriori_estimate(state, ang_measurement)\n", - "\n", - " if conn.topic == \"/dvl_twist\":\n", - " # print(\"DVL data received\")\n", - "\n", - " msg = reader.deserialize(raw, conn.msgtype)\n", - " dvl_measurement.measurement = np.array([msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z])\n", - " \n", - " dvl_measurement.covariance = np.eye(3) * (0.005**2) \n", - " \n", - " # Update UKF with measurement\n", - " UKF_model.measurement_update(state, dvl_measurement)\n", - " state = UKF_model.posteriori_estimate(state, dvl_measurement)\n", - "\n", - "\n", - " # Store the state estimates\n", - " positions_est.append(state.position.copy())\n", - " velocities_est.append(state.velocity.copy())\n", - " quaternions_est.append(state.orientation.copy())\n", - " angular_velocities_est.append(state.angular_velocity.copy())\n", - " okid_params_est.append(state.okid_part().copy())\n", - "\n", - " last_time = t_ns" - ] - }, - { - "cell_type": "code", - "execution_count": 5, - "metadata": {}, - "outputs": [ - { - "data": { - "image/png": "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", - "text/plain": [ - "
" - ] - }, - "metadata": {}, - "output_type": "display_data" - } - ], - "source": [ - "import numpy as np\n", - "\n", - "import matplotlib.pyplot as plt\n", - "\n", - "# Extract x, y, z positions from positions_est\n", - "x_positions = np.array([pos[0] for pos in positions_est])\n", - "y_positions = np.array([pos[1] for pos in positions_est])\n", - "z_positions = np.array([pos[2] for pos in positions_est])\n", - "\n", - "# Create figure with three subplots\n", - "fig, axs = plt.subplots(3, 1, figsize=(12, 10), sharex=True)\n", - "fig.suptitle('AUV Position Estimates Over Time', fontsize=16)\n", - "\n", - "# Plot x position\n", - "axs[0].plot(x_positions, 'b-', linewidth=2)\n", - "axs[0].set_ylabel('X Position [m]')\n", - "axs[0].grid(True)\n", - "\n", - "# Plot y position\n", - "axs[1].plot(y_positions, 'g-', linewidth=2)\n", - "axs[1].set_ylabel('Y Position [m]')\n", - "axs[1].grid(True)\n", - "\n", - "# Plot z position\n", - "axs[2].plot(z_positions, 'r-', linewidth=2) \n", - "axs[2].set_ylabel('Z Position [m]')\n", - "axs[2].set_xlabel('Time steps')\n", - "axs[2].grid(True)\n", - "\n", - "plt.tight_layout(rect=[0, 0.03, 1, 0.95])\n", - "plt.show()" - ] - }, - { - "cell_type": "code", - "execution_count": 6, - "metadata": {}, - "outputs": [ - { - "data": { - "image/png": "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", - "text/plain": [ - "
" - ] - }, - "metadata": {}, - "output_type": "display_data" - }, - { - "data": { - "image/png": "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", - "text/plain": [ - "
" - ] - }, - "metadata": {}, - "output_type": "display_data" - }, - { - "data": { - "image/png": "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", - "text/plain": [ - "
" - ] - }, - "metadata": {}, - "output_type": "display_data" - }, - { - "data": { - "image/png": "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", - "text/plain": [ - "
" - ] - }, - "metadata": {}, - "output_type": "display_data" - } - ], - "source": [ - "# Convert okid_params_est from list of arrays to a numpy array for easier handling\n", - "okid_params_array = np.array(okid_params_est)\n", - "\n", - "# Create labels for the different parameter groups\n", - "inertia_labels = ['Ixx', 'Ixy', 'Ixz', 'Iyx', 'Iyy', 'Iyz', 'Izx', 'Izy', 'Izz']\n", - "added_mass_labels = ['Xu', 'Yv', 'Zw', 'Kp', 'Mq', 'Nr']\n", - "damping_labels = ['Xu', 'Yv', 'Zw', 'Kp', 'Mq', 'Nr']\n", - "g_eta_labels = ['g_eta1', 'g_eta2', 'g_eta3', 'g_eta4']\n", - "\n", - "# Create figures for each parameter group\n", - "plt.figure(figsize=(14, 10))\n", - "plt.suptitle('Inertia Matrix Parameters', fontsize=16)\n", - "for i in range(9):\n", - " plt.subplot(3, 3, i+1)\n", - " plt.plot(okid_params_array[:, i], 'b-')\n", - " plt.title(inertia_labels[i])\n", - " plt.grid(True)\n", - "plt.tight_layout(rect=[0, 0.03, 1, 0.95])\n", - "plt.show()\n", - "\n", - "plt.figure(figsize=(14, 8))\n", - "plt.suptitle('Added Mass Parameters', fontsize=16)\n", - "for i in range(6):\n", - " plt.subplot(2, 3, i+1)\n", - " plt.plot(okid_params_array[:, 9+i], 'g-')\n", - " plt.title(f'Added Mass: {added_mass_labels[i]}')\n", - " plt.grid(True)\n", - "plt.tight_layout(rect=[0, 0.03, 1, 0.95])\n", - "plt.show()\n", - "\n", - "plt.figure(figsize=(14, 8))\n", - "plt.suptitle('Damping Parameters', fontsize=16)\n", - "for i in range(6):\n", - " plt.subplot(2, 3, i+1)\n", - " plt.plot(okid_params_array[:, 15+i], 'r-')\n", - " plt.title(f'Damping: {damping_labels[i]}')\n", - " plt.grid(True)\n", - "plt.tight_layout(rect=[0, 0.03, 1, 0.95])\n", - "plt.show()\n", - "\n", - "plt.figure(figsize=(14, 6))\n", - "plt.suptitle('G-Eta Parameters', fontsize=16)\n", - "for i in range(4):\n", - " plt.subplot(2, 2, i+1)\n", - " plt.plot(okid_params_array[:, 21+i], 'm-')\n", - " plt.title(g_eta_labels[i])\n", - " plt.grid(True)\n", - "plt.tight_layout(rect=[0, 0.03, 1, 0.95])\n", - "plt.show()" - ] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.10.12" - } - }, - "nbformat": 4, - "nbformat_minor": 2 -} diff --git a/navigation/tukf/test_tukf.py b/navigation/tukf/test_tukf.py deleted file mode 100644 index 2998855ea..000000000 --- a/navigation/tukf/test_tukf.py +++ /dev/null @@ -1,347 +0,0 @@ -import numpy as np -from tukf import TUKF -import tukf_class as ukf -from mpl_toolkits.mplot3d import Axes3D -import time -import math - -import matplotlib.pyplot as plt - -# Initialize UKF with StateQuat -initial_position = np.array([0.0, 0.0, 0.0]) # x, y, z -initial_velocity = np.array([0.0, 0.0, 0.0]) # vx, vy, vz -initial_quaternion = np.array([0.0, 0.0, 0.0]) # w, x, y, z (identity quaternion) -initial_angular_velocity = np.array([0.0, 0.0, 0.0]) # wx, wy, wz -initial_g_eta = np.array([1.2, 0.3, 0.3, 0.3]) # g_eta parameters -initial_intertia = np.array([0.68, 0.2, 0.1, - 0.2, 3.32, 0.2, - 0.1, 0.2, 3.34]) -initial_damping = np.array([0.01, 0.01, 0.01, - 0.01, 0.01, 0.01]) -initla_added_mass = np.array([0.02, 0.02, 0.02, - 0.02, 0.02, 0.02]) - -p_diag = np.concatenate([ - 2*np.ones(3), # x position - 2*np.ones(3), # orientation - 2*np.ones(3), # velocity - 2*np.ones(3), # angular velocity - 2*np.ones(9), # inertia - 2*np.ones(6), # added mass - 2*np.ones(6), # damping - 2*np.ones(4) # g_eta -]) - -initial_covariance = np.diag(p_diag) - -state = ukf.AUVState(initial_position.copy(), initial_quaternion.copy(), initial_velocity.copy(), initial_angular_velocity.copy()) -state.covariance = initial_covariance.copy() -state.inertia = np.array([0.58, 0.1, 0.05, - 0.1, 2.32, 0.1, - 0.01, 0.1, 2.34]) -state.g_eta = np.array([1.1, 0.1, 0.1, 0.1]) -state.damping = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) -state.added_mass = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) - -real_state = ukf.AUVState(initial_position.copy(), initial_quaternion.copy(), initial_velocity.copy(), initial_angular_velocity.copy()) -real_state.inertia = initial_intertia.copy() -real_state.g_eta = initial_g_eta.copy() -real_state.damping = initial_damping.copy() -real_state.added_mass = initla_added_mass.copy() - -Q_diag = np.concatenate([ - 0.01*np.ones(3), # position - 0.2*np.ones(9), # kinematic (η & ν) - 0.8*np.ones(9), # inertia - 0.8*np.ones(6), # added mass - 0.8*np.ones(6), # damping - 0.8*np.ones(4), # g_eta -]) - -UKF_model = TUKF(state, np.diag(Q_diag)) # Process noise covariance - -def dvl_h(state: ukf.AUVState) -> 'ukf.MeasModel': - H_matrix = np.zeros((3, 12)) - H_matrix[:, 6:9] = np.eye(3) - z_i = ukf.MeasModel() - z_i.measurement = np.dot(H_matrix, state.dynamic_part()) - return z_i - -dvl_measurement = ukf.MeasModel(H=dvl_h) - -def ang_h(state: ukf.AUVState) -> 'ukf.MeasModel': - H_matrix = np.zeros((3, 12)) - H_matrix[:, 9:12] = np.eye(3) - z_i = ukf.MeasModel() - z_i.measurement = np.dot(H_matrix, state.dynamic_part()) - return z_i - -ang_measurement = ukf.MeasModel(H=ang_h) - -# UKF parameters -dt = 0.01 # time step -sim_time = 50.0 # total simulation time -steps = int(sim_time / dt) - -# Storage for trajectory -positions = np.zeros((steps, 3)) -velocities = np.zeros((steps, 3)) -quaternions = np.zeros((steps, 3)) -angular_velocities = np.zeros((steps, 3)) -okid_params = np.zeros((steps, 25)) - -# Storage for trajectory -positions_est = np.zeros((steps, 3)) -velocities_est = np.zeros((steps, 3)) -quaternions_est = np.zeros((steps, 3)) -angular_velocities_est = np.zeros((steps, 3)) -okid_params_est = np.zeros((steps, 25)) - -# ---------- user‑tunable manoeuvre parameters ----------------------------- -SEG_DUR = 10.0 # [s] duration of each phase -A_F_TRANSL = 2.0 # [N] translational force amplitude -A_T_ROT = 1.0 # [N·m] rotational torque amplitude -# -------------------------------------------------------------------------- - -# Helper: build the scripted sequence as (kind, axis_idx, sign) -# kind = 'F' for force, 'T' for torque -# axes: 0‑x (surge/roll), 1‑y (sway/pitch), 2‑z (heave/yaw) -sequence = [ - ('F', 2, +1), # +z (up) - ('F', 2, -1), # –z (down) - ('F', 1, +1), # +y (right) - ('F', 1, -1), # –y (left / “back” sideways) - ('F', 0, +1), # +x (forward) - ('F', 0, -1), # –x (backward) - ('T', 2, +1), # +yaw (turn right) - ('T', 2, -1), # –yaw (turn left) - ('T', 1, +1), # +pitch (nose up) - ('T', 1, -1), # –pitch (nose down) - ('T', 0, +1), # +roll (starboard roll) - ('T', 0, -1) # –roll (port roll) -] - -TOTAL_TIME = len(sequence) * SEG_DUR # handy if you need it - -def _half_sine(local_t: float, duration: float) -> float: - """Smooth window: 0 → 1 → 0 over `duration` (half‑sine).""" - return np.sin(np.pi * local_t / duration) - -def control_inputs(t: float) -> tuple[np.ndarray, np.ndarray]: - """ - Piecewise scripted test signal: - – translations along z, y, x - – rotations about z (yaw), y (pitch), x (roll) - """ - # Default: no actuation - F = np.zeros(3) - T = np.zeros(3) - - # Past the last segment? keep everything zero - idx = int(t // SEG_DUR) - if idx >= len(sequence): - return F, T - - # Time inside current segment - tau = t - idx * SEG_DUR - window = _half_sine(tau, SEG_DUR) # 0‑to‑1‑to‑0 shape - - kind, axis, sgn = sequence[idx] - - if kind == 'F': - F[axis] = sgn * A_F_TRANSL * window - else: # 'T' - T[axis] = sgn * A_T_ROT * window - - return F, T - -# Simulation loop -for i in range(steps): - t = i * dt - - control_force, control_torque = control_inputs(t) - - control_input = np.concatenate((control_force, control_torque)) - - # Propagate state using UKF prediction - real_state = ukf.F_dynamics(real_state, dt, control_input) - state = UKF_model.unscented_transform(state, control_input) - - if UKF_model.filter_failed: - print("Filter failed, stopping simulation.") - break - - if i % 5 == 0: - # Simulate measurement update every 5 steps - ang_measurement.measurement = np.array([ - real_state.angular_velocity[0], - real_state.angular_velocity[1], - real_state.angular_velocity[2] - ]) + np.random.normal(0, 0.04, 3) - - ang_measurement.covariance = np.eye(3) * (0.03**2) # Measurement noise covariance - - UKF_model.measurement_update(state, ang_measurement) - state = UKF_model.posteriori_estimate(state, ang_measurement) - - if i % 10 == 0: - # Simulate measurement update every 10 steps - dvl_measurement.measurement = np.array([ - real_state.velocity[0], - real_state.velocity[1], - real_state.velocity[2] - ]) + np.random.normal(0, 0.04, 3) # Simulated measurement with noise - - # Simulate measurement covariance - dvl_measurement.covariance = np.eye(3) * (0.03**2) # Measurement noise covariance - - # Update UKF with measurement - UKF_model.measurement_update(state, dvl_measurement) - state = UKF_model.posteriori_estimate(state, dvl_measurement) - - # Store state for plotting - positions[i] = real_state.position - velocities[i] = real_state.velocity - quaternions[i] = real_state.orientation - angular_velocities[i] = real_state.angular_velocity - okid_params[i] = real_state.okid_part() - - # Store estimated state for plotting - positions_est[i] = state.position - velocities_est[i] = state.velocity - quaternions_est[i] = state.orientation - angular_velocities_est[i] = state.angular_velocity - okid_params_est[i] = state.okid_part() - - # Add small delay to simulate real-time execution - time.sleep(0.001) -print(state.as_vector()) -# Plotting -time_points = np.arange(0, sim_time, dt) - -# 3D trajectory plot -fig = plt.figure(figsize=(12, 10)) -ax = fig.add_subplot(111, projection='3d') -ax.plot(positions[:, 0], positions[:, 1], positions[:, 2], 'b-', label='True Trajectory') -ax.plot(positions_est[:, 0], positions_est[:, 1], positions_est[:, 2], 'r--', label='Estimated Trajectory') -ax.scatter(positions[0, 0], positions[0, 1], positions[0, 2], c='g', marker='o', s=100, label='Start') -ax.scatter(positions[-1, 0], positions[-1, 1], positions[-1, 2], c='r', marker='o', s=100, label='End') -ax.set_xlabel('X Position') -ax.set_ylabel('Y Position') -ax.set_zlabel('Z Position') -ax.set_title('3D Trajectory') -ax.legend() - -# Position plot -plt.figure(figsize=(12, 6)) -plt.subplot(311) -plt.plot(time_points, positions[:, 0], 'b-', label='True') -plt.plot(time_points, positions_est[:, 0], 'r--', label='Estimated') -plt.ylabel('X Position') -plt.legend() -plt.subplot(312) -plt.plot(time_points, positions[:, 1], 'b-', label='True') -plt.plot(time_points, positions_est[:, 1], 'r--', label='Estimated') -plt.ylabel('Y Position') -plt.legend() -plt.subplot(313) -plt.plot(time_points, positions[:, 2], 'b-', label='True') -plt.plot(time_points, positions_est[:, 2], 'r--', label='Estimated') -plt.ylabel('Z Position') -plt.xlabel('Time (s)') -plt.legend() -plt.tight_layout() - -# Velocity plot -plt.figure(figsize=(12, 6)) -plt.subplot(311) -plt.plot(time_points, velocities[:, 0], 'b-', label='True') -plt.plot(time_points, velocities_est[:, 0], 'r--', label='Estimated') -plt.ylabel('X Velocity') -plt.legend() -plt.subplot(312) -plt.plot(time_points, velocities[:, 1], 'b-', label='True') -plt.plot(time_points, velocities_est[:, 1], 'r--', label='Estimated') -plt.ylabel('Y Velocity') -plt.legend() -plt.subplot(313) -plt.plot(time_points, velocities[:, 2], 'b-', label='True') -plt.plot(time_points, velocities_est[:, 2], 'r--', label='Estimated') -plt.ylabel('Z Velocity') -plt.xlabel('Time (s)') -plt.legend() -plt.tight_layout() - -# Angular velocity plot -plt.figure(figsize=(12, 6)) -plt.subplot(311) -plt.plot(time_points, angular_velocities[:, 0], 'b-', label='True') -plt.plot(time_points, angular_velocities_est[:, 0], 'r--', label='Estimated') -plt.ylabel('Roll Rate') -plt.legend() -plt.subplot(312) -plt.plot(time_points, angular_velocities[:, 1], 'b-', label='True') -plt.plot(time_points, angular_velocities_est[:, 1], 'r--', label='Estimated') -plt.ylabel('Pitch Rate') -plt.legend() -plt.subplot(313) -plt.plot(time_points, angular_velocities[:, 2], 'b-', label='True') -plt.plot(time_points, angular_velocities_est[:, 2], 'r--', label='Estimated') -plt.ylabel('Yaw Rate') -plt.xlabel('Time (s)') -plt.legend() -plt.tight_layout() - -# OKID Inertia parameters plot (9 parameters) -plt.figure(figsize=(15, 10)) -plt.suptitle('Inertia Parameters', fontsize=16) -for i in range(9): - plt.subplot(3, 3, i+1) - plt.plot(time_points, okid_params[:, i], 'b-', label='True') - plt.plot(time_points, okid_params_est[:, i], 'r--', label='Estimated') - plt.ylabel(f'Inertia[{i}]') - if i >= 6: # Add x-label only to bottom row - plt.xlabel('Time (s)') - plt.legend() -plt.tight_layout(rect=[0, 0, 1, 0.96]) # Adjust for suptitle - -# OKID Added Mass parameters plot (6 parameters) -plt.figure(figsize=(15, 8)) -plt.suptitle('Added Mass Parameters', fontsize=16) -for i in range(6): - plt.subplot(2, 3, i+1) - plt.plot(time_points, okid_params[:, i+9], 'b-', label='True') - plt.plot(time_points, okid_params_est[:, i+9], 'r--', label='Estimated') - plt.ylabel(f'Added Mass[{i}]') - if i >= 3: # Add x-label only to bottom row - plt.xlabel('Time (s)') - plt.legend() -plt.tight_layout(rect=[0, 0, 1, 0.96]) # Adjust for suptitle - -# OKID Damping parameters plot (6 parameters) -plt.figure(figsize=(15, 8)) -plt.suptitle('Damping Parameters', fontsize=16) -for i in range(6): - plt.subplot(2, 3, i+1) - plt.plot(time_points, okid_params[:, i+15], 'b-', label='True') - plt.plot(time_points, okid_params_est[:, i+15], 'r--', label='Estimated') - plt.ylabel(f'Damping[{i}]') - if i >= 3: # Add x-label only to bottom row - plt.xlabel('Time (s)') - plt.legend() -plt.tight_layout(rect=[0, 0, 1, 0.96]) # Adjust for suptitle - -# OKID g_eta parameters plot (4 parameters) -plt.figure(figsize=(12, 8)) -plt.suptitle('g_eta Parameters', fontsize=16) -for i in range(4): - plt.subplot(2, 2, i+1) - plt.plot(time_points, okid_params[:, i+21], 'b-', label='True') - plt.plot(time_points, okid_params_est[:, i+21], 'r--', label='Estimated') - plt.ylabel(f'g_eta[{i}]') - plt.xlabel('Time (s)') - plt.legend() -plt.tight_layout(rect=[0, 0, 1, 0.96]) # Adjust for suptitle - -plt.show() - diff --git a/navigation/tukf/tukf.py b/navigation/tukf/tukf.py deleted file mode 100644 index a4badd887..000000000 --- a/navigation/tukf/tukf.py +++ /dev/null @@ -1,126 +0,0 @@ -import numpy as np -from tukf_class import ( - MeasModel, - AUVState, - covariance_measurement, - covariance_set, - cross_covariance, - mean_measurement, - mean_set, - F_dynamics, - generate_delta_matrix, -) - -def print_matrix(matrix, name="Matrix"): - """Custom print function to print matrices in a formatted form.""" - print(f"{name}: {matrix.shape}") - if isinstance(matrix, np.ndarray): - for row in matrix: - print(" ".join(f"{val:.2f}" for val in row)) - else: - print(matrix) - -class TUKF: - def __init__(self, x_0: AUVState, Q): - self.x = x_0 - self.Q = Q - self.delta = generate_delta_matrix(len(x_0.as_vector())) / np.sqrt(len(x_0.as_vector())) - self.sigma_points_list = None - self.measurement_updated = MeasModel() - self.dt = 0.01 # Time step for dynamics - self.flagg = 0 - self.filter_failed = False - - def sigma_points(self, current_state: AUVState) -> list[AUVState]: - """Functions that generate the sigma points for the UKF.""" - n = len(current_state.covariance) - self.flagg += 1 - try: - S = np.linalg.cholesky(current_state.covariance) - except np.linalg.LinAlgError: - print("Cholesky decomposition failed!") - print("flagg", self.flagg) - print_matrix(current_state.covariance, "Current State Covariance") - print_matrix(self.Q, "Process Noise Covariance (Q)") - - # Set flag to indicate filter has failed - self.filter_failed = True - - # Create a valid but minimal S matrix to avoid crashing - # This allows the simulation to continue to the next step where it can be checked - S = np.eye(n) * 1e-6 - - self.sigma_points_list = [AUVState() for _ in range(2 * n)] - - for index, state in enumerate(self.sigma_points_list): - state.fill_states(current_state.as_vector() + S @ self.delta[:, index]) - - return self.sigma_points_list - - def unscented_transform(self, current_state: AUVState, control_force: np.ndarray) -> AUVState: - """The unscented transform function generates the priori state estimate.""" - self.sigma_points(current_state) - n = len(current_state.covariance) - - self.y_i = [AUVState() for _ in range(2 * n)] - - for i, sp in enumerate(self.sigma_points_list): - self.y_i[i] = F_dynamics(sp, self.dt, control_force) - - state_estimate = AUVState() - x = mean_set(self.y_i) - - state_estimate.fill_states(x) - state_estimate.covariance = covariance_set(self.y_i, x) + + self.Q - return state_estimate - - def measurement_update( - self, current_state: AUVState, measurement: MeasModel - ) -> None: - """Function that updates the state estimate with a measurement. - - Hopefully this is the DVL or GNSS - """ - n = len(current_state.covariance) - z_i = [MeasModel() for _ in range(2 * n)] - - for i, state in enumerate(self.y_i): - z_i[i] = measurement.H(state) - - self.measurement_updated.measurement = mean_measurement(z_i) - - self.measurement_updated.covariance = covariance_measurement( - z_i, self.measurement_updated.measurement - ) - - self.cross_correlation = cross_covariance( - self.y_i, - current_state.as_vector(), - z_i, - self.measurement_updated.measurement, - ) - - def posteriori_estimate( - self, - current_state: AUVState, - measurement: MeasModel, - ) -> AUVState: - """Calculates the posteriori estimate using measurement and the prior estimate.""" - nu_k = MeasModel() - nu_k.measurement = ( - measurement.measurement - self.measurement_updated.measurement - ) - nu_k.covariance = self.measurement_updated.covariance + measurement.covariance - - K_k = np.dot(self.cross_correlation, np.linalg.inv(nu_k.covariance)) - - posteriori_estimate = AUVState() - - posteriori_estimate.fill_states( - current_state.as_vector() + np.dot(K_k, nu_k.measurement) - ) - posteriori_estimate.covariance = current_state.covariance - np.dot( - K_k, np.dot(nu_k.covariance, np.transpose(K_k)) - ) - - return posteriori_estimate diff --git a/navigation/tukf/tukf_class.py b/navigation/tukf/tukf_class.py deleted file mode 100644 index ff274680e..000000000 --- a/navigation/tukf/tukf_class.py +++ /dev/null @@ -1,437 +0,0 @@ -import numpy as np -from dataclasses import dataclass, field -from typing import Callable - -@dataclass -class AUVState: - position: np.ndarray = field(default_factory=lambda: np.zeros(3)) - orientation: np.ndarray = field(default_factory=lambda: np.array(3)) - velocity: np.ndarray = field(default_factory=lambda: np.zeros(3)) - angular_velocity: np.ndarray = field(default_factory=lambda: np.zeros(3)) - inertia: np.ndarray = field(default_factory=lambda: np.zeros((9))) - added_mass: np.ndarray = field(default_factory=lambda: np.zeros((6))) - damping: np.ndarray = field(default_factory=lambda: np.zeros((6))) - g_eta: np.ndarray = field(default_factory=lambda: np.zeros((4))) - covariance: np.ndarray = field(default_factory=lambda: np.zeros((37, 37))) - - def dynamic_part(self) -> np.ndarray: - """Get the dynamic part of the AUV state.""" - return np.concatenate([ - self.position, - self.orientation, - self.velocity, - self.angular_velocity - ]) - def okid_part(self) -> np.ndarray: - """Get the OKID part of the AUV state.""" - return np.concatenate([ - self.inertia, - self.added_mass, - self.damping, - self.g_eta - ]) - def as_vector(self) -> np.ndarray: - """Convert the AUV state to a vector representation.""" - return np.concatenate([ - self.position, - self.orientation, - self.velocity, - self.angular_velocity, - self.inertia.flatten(), - self.added_mass.flatten(), - self.damping.flatten(), - self.g_eta - ]) - - def __add__(self, other: 'AUVState') -> 'AUVState': - """Add two AUV states together.""" - return AUVState( - position=self.position + other.position, - orientation=self.orientation + other.orientation, - velocity=self.velocity + other.velocity, - angular_velocity=self.angular_velocity + other.angular_velocity, - inertia=self.inertia + other.inertia, - added_mass=self.added_mass + other.added_mass, - damping=self.damping + other.damping, - g_eta=self.g_eta + other.g_eta - ) - def __sub__(self, other: 'AUVState') -> 'AUVState': - """Subtract two AUV states.""" - return AUVState( - position=self.position - other.position, - orientation=self.orientation - other.orientation, - velocity=self.velocity - other.velocity, - angular_velocity=self.angular_velocity - other.angular_velocity, - inertia=self.inertia - other.inertia, - added_mass=self.added_mass - other.added_mass, - damping=self.damping - other.damping, - g_eta=self.g_eta - other.g_eta - ) - def fill_states(self, x: np.ndarray) -> None: - """Fill the AUV state with a vector representation.""" - self.position = x[0:3] - self.orientation = x[3:6] - self.velocity = x[6:9] - self.angular_velocity = x[9:12] - self.inertia = x[12:21] - self.added_mass = x[21:27] - self.damping = x[27:33] - self.g_eta = x[33:37] - - -@dataclass -class MeasModel: - """A class defined for a general measurement model.""" - measurement: np.ndarray = field(default_factory=lambda: np.zeros(3)) - covariance: np.ndarray = field(default_factory=lambda: np.zeros((3, 3))) - H: Callable[["AUVState"], "MeasModel"] | None = None - - def __post_init__(self): - """Initialize H with a default measurement function if none provided.""" - if self.H is None: - self.H = self._default_H - - def _default_H(self, state: AUVState) -> 'MeasModel': - """Default measurement function that returns velocity.""" - H_matrix = np.zeros((3, 12)) - H_matrix[:, 6:9] = np.eye(3) - z_i = MeasModel() - z_i.measurement = np.dot(H_matrix, state.dynamic_part()) - return z_i - - def __add__(self, other: 'MeasModel') -> 'MeasModel': - """Defines the addition operation between two MeasModel objects.""" - result = MeasModel() - result.measurement = self.measurement + other.measurement - return result - - def __rmul__(self, scalar: float) -> 'MeasModel': - """Defines multiplication between scalar value and MeasModel object.""" - result = MeasModel() - result.measurement = scalar * self.measurement - return result - - def __sub__(self, other: 'MeasModel') -> 'MeasModel': - """Defines the subtraction between two MeasModel objects.""" - result = MeasModel() - result.measurement = self.measurement - other.measurement - return result - -def generate_delta_matrix_2(n: float) -> np.ndarray: - """Generates the weight matrix used in the TUKF sigma point generation. - - Parameters: - n (int): The state dimension. - - Returns: - delta (np.ndarray): An n x 2n orthonormal transformation matrix used to generate TUKF sigma points. - """ - delta = np.zeros((n, 2 * n)) - k = 0.001 # Tuning parameter to ensure pos def - - for i in range(2 * n): - for j in range(n // 2): - delta[2 * j + 1, i] = ( - np.sqrt(2) * np.sin(2 * j - 1) * ((k * np.pi) / n) - ) - delta[2 * j, i] = np.sqrt(2) * np.cos(2 * j - 1) * ((k * np.pi) / n) - - if (n % 2) == 1: - delta[n - 1, i] = np.sqrt(2) * np.cos(2 * j - 1) * ((k * np.pi) / n) - - return delta - -def generate_delta_matrix(n: int) -> np.ndarray: - if n < 1: - raise ValueError("n must be a positive integer") - - delta = np.zeros((n, 2 * n)) - r_max = n // 2 # floor(n/2) - sq2 = np.sqrt(2.0) - - for k in range(1, 2 * n + 1): # k = 1 … 2n - for r in range(1, r_max + 1): - row_cos = 2 * r - 2 # 0‑based index for γ_{k,2r‑1} - row_sin = 2 * r - 1 # 0‑based index for γ_{k,2r} - angle = (2 * r - 1) * k * np.pi / n - delta[row_cos, k - 1] = sq2 * np.cos(angle) - delta[row_sin, k - 1] = sq2 * np.sin(angle) - - if n % 2 == 1: # extra entry when n is odd - delta[n - 1, k - 1] = (-1) ** k - - return delta - -def skew_symmetric(vector: np.ndarray) -> np.ndarray: - """Calculates the skew symmetric matrix of a vector. - - Args: - vector (np.ndarray): The vector. - - Returns: - np.ndarray: The skew symmetric matrix. - """ - return np.array( - [ - [0, -vector[2], vector[1]], - [vector[2], 0, -vector[0]], - [-vector[1], vector[0], 0], - ] - ) - -def mean_set(set_points: list[AUVState]) -> np.ndarray: - """Function calculates the mean vector of a set of points. - - Args: - set_points (list[AUVState]): List of AUVState objects - - Returns: - np.ndarray: The mean vector - """ - n = len(set_points) - mean_value = np.zeros(set_points[0].as_vector().shape) - - for state in set_points: - mean_value = mean_value + state.as_vector() - - mean_value = (1 / n) * mean_value - - return mean_value - - -def mean_measurement(set_points: list[MeasModel]) -> np.ndarray: - """Function that calculates the mean of a set of points.""" - n = len(set_points) - mean_value = MeasModel() - - for state in set_points: - mean_value = mean_value + state - - mean_value = (1 / n) * mean_value - - return mean_value.measurement - - -def covariance_set(set_points: list[AUVState], mean: np.ndarray) -> np.ndarray: - """Function that calculates the covariance of a set of points.""" - n = len(set_points) - covariance = np.zeros(set_points[0].covariance.shape) - - for state in set_points: - W_i = state.as_vector() - mean - - covariance += np.outer(W_i, W_i) - - covariance = (1 / n) * covariance - - return covariance - - -def covariance_measurement(set_points: list[MeasModel], mean: np.ndarray) -> np.ndarray: - """Function that calculates the covariance of a set of points.""" - n = len(set_points) - co_size = len(set_points[0].measurement) - covariance = np.zeros((co_size, co_size)) - - mean_meas = MeasModel() - mean_meas.measurement = mean - - for state in set_points: - temp_state = state - mean_meas - covariance += np.outer(temp_state.measurement, temp_state.measurement) - - covariance = (1 / n) * covariance - - return covariance - - -def cross_covariance( - set_y: list[AUVState], - mean_y: np.ndarray, - set_z: list[MeasModel], - mean_z: np.ndarray, -) -> np.ndarray: - """Calculates the cross covariance between the measurement and state prediction.""" - n = len(set_y) - - cross_covariance = np.zeros((len(mean_y), len(mean_z))) - - for i in range(n): - state_diff = set_y[i].as_vector() - mean_y - meas_diff = set_z[i].measurement - mean_z - - cross_covariance += np.outer(state_diff, meas_diff) - - cross_covariance = (1 / n) * cross_covariance - - return cross_covariance - - -# ----------------------------------------------------------- - -def rotation_matrix(euler_angles: np.ndarray) -> np.ndarray: - """Calculates the rotation matrix from Euler angles (roll, pitch, yaw).""" - roll, pitch, yaw = euler_angles - - # Roll rotation - Rx = np.array([ - [1, 0, 0], - [0, np.cos(roll), -np.sin(roll)], - [0, np.sin(roll), np.cos(roll)] - ]) - - # Pitch rotation - Ry = np.array([ - [np.cos(pitch), 0, np.sin(pitch)], - [0, 1, 0], - [-np.sin(pitch), 0, np.cos(pitch)] - ]) - - # Yaw rotation - Rz = np.array([ - [np.cos(yaw), -np.sin(yaw), 0], - [np.sin(yaw), np.cos(yaw), 0], - [0, 0, 1] - ]) - - # Complete rotation matrix - R = Rz @ Ry @ Rx - return R - -def angular_velocity_transformation(euler_angles: np.ndarray) -> np.ndarray: - """Transformation matrix relating Euler rates to angular velocities.""" - roll, pitch, yaw = euler_angles - - T = np.array([ - [1, 0, -np.sin(pitch)], - [0, np.cos(roll), np.cos(pitch) * np.sin(roll)], - [0, -np.sin(roll), np.cos(pitch) * np.cos(roll)] - ]) - - return T - -def M_rb(inertia: np.ndarray) -> np.ndarray: - m = 25.5 - inertia = inertia.reshape((3, 3)) - r_b_bg = np.array([0.01, 0.0, 0.02]) - M_rb = np.zeros((6, 6)) - M_rb[0:3, 0:3] = m * np.eye(3) - M_rb[3:6, 3:6] = inertia - M_rb[0:3, 3:6] = -m * skew_symmetric(r_b_bg) - M_rb[3:6, 0:3] = m * skew_symmetric(r_b_bg) - return M_rb - -def M_a(added_mass: np.ndarray) -> np.ndarray: - """Calculates the added mass matrix.""" - M_a = np.zeros((6, 6)) - M_a[0:3, 0:3] = np.diag(added_mass[0:3]) - M_a[3:6, 3:6] = np.diag(added_mass[3:6]) - return M_a - -def C_rb(inertia: np.ndarray, angular_velocity: np.ndarray) -> np.ndarray: - """Calculates the Coriolis matrix.""" - m = 25.5 - r_b_bg = np.array([0.01, 0.0, 0.02]) - inertia = inertia.reshape((3, 3)) - C_rb = np.zeros((6, 6)) - - C_rb[0:3, 0:3] = m * skew_symmetric(angular_velocity) - C_rb[3:6, 3:6] = -skew_symmetric(np.dot(inertia, angular_velocity)) - C_rb[0:3, 3:6] = -m * skew_symmetric(angular_velocity) @ skew_symmetric(r_b_bg) - C_rb[3:6, 0:3] = m * skew_symmetric(r_b_bg) @ skew_symmetric(angular_velocity) - return C_rb - -def C_a(added_mass: np.ndarray, angular_velocity: np.ndarray, velocity: np.ndarray) -> np.ndarray: - """Calculates the added mass Coriolis matrix.""" - C_a = np.zeros((6, 6)) - A11 = np.diag(added_mass[0:3]) - A22 = np.diag(added_mass[3:6]) - C_a[3:6,3:6] = - skew_symmetric(A22 @ angular_velocity) - C_a[0:3,3:6] = - skew_symmetric(A11 @ velocity) - C_a[3:6,0:3] = - skew_symmetric(A11 @ velocity) - return C_a - -def D_linear(damping_linear: np.ndarray) -> np.ndarray: - """Calculates the linear damping matrix.""" - D = np.zeros((6, 6)) - D[0:3, 0:3] = -np.diag(damping_linear[0:3]) - D[3:6, 3:6] = -np.diag(damping_linear[3:6]) - return D - -def g_eta(g_eta: np.ndarray, orientation: np.ndarray) -> np.ndarray: - """Calculates the g_eta matrix using Euler angles.""" - Delta_WB = g_eta[0] - M_x = g_eta[1] - M_y = g_eta[2] - M_z = g_eta[3] - - # Get rotation matrix using Euler angles - R = rotation_matrix(orientation) - - G_eta = np.zeros((6,1)) - # Gravitational forces - G_eta[0:3] = -Delta_WB * R[:, 2].reshape(3, 1) - - # Buoyancy moments - G_eta[3] = -M_y * R[2, 2] + M_z * R[1, 2] - G_eta[4] = -M_z * R[0, 2] + M_x * R[2, 2] - G_eta[5] = -M_x * R[1, 2] + M_y * R[0, 2] - - return G_eta - -def F_dynamics( - state: AUVState, - dt: float, - control_input: np.ndarray) -> AUVState: - - """Calculates the dynamics of the system.""" - m_rb = M_rb(state.inertia) - m_a = M_a(state.added_mass) - c_rb = C_rb(state.inertia, state.angular_velocity) - c_a = C_a(state.added_mass, state.angular_velocity, state.velocity) - D_l = D_linear(state.damping) - g_eta_ = g_eta(state.g_eta, state.orientation) - - # Get rotation and transformation matrices - r = rotation_matrix(state.orientation) - t = angular_velocity_transformation(state.orientation) - - Crb = c_rb + c_a - Mrb = m_rb + m_a - M_inv = np.linalg.inv(Mrb) - - # Create a vector of velocity and angular velocity - nu = np.concatenate([state.velocity, state.angular_velocity]) - - # Calculate the new state - state_dot = AUVState() - state_dot.position = np.dot(r, state.velocity) - - # Calculate Euler angle rates from angular velocities - t_inv = np.linalg.inv(t) - euler_rates = np.dot(t_inv, state.angular_velocity) - state_dot.orientation = euler_rates - - Nu = M_inv @ (control_input - np.dot(Crb, nu) - np.dot(D_l, nu) - g_eta_.flatten()) - - state_dot.velocity = Nu[:3] - state_dot.angular_velocity = Nu[3:6] - - # Update the inertia, added mass, damping, and g_eta - state_dot.inertia = np.zeros_like(state.inertia) - state_dot.added_mass = np.zeros_like(state.added_mass) - state_dot.damping = np.zeros_like(state.damping) - state_dot.g_eta = np.zeros_like(state.g_eta) - - new_state = AUVState() - new_state.position = state.position + state_dot.position * dt - new_state.orientation = state.orientation + state_dot.orientation * dt - new_state.velocity = state.velocity + state_dot.velocity * dt - new_state.angular_velocity = state.angular_velocity + state_dot.angular_velocity * dt - new_state.inertia = state.inertia + state_dot.inertia * dt - new_state.added_mass = state.added_mass + state_dot.added_mass * dt - new_state.damping = state.damping + state_dot.damping * dt - new_state.g_eta = state.g_eta + state_dot.g_eta * dt - - return new_state -# ----------------------------------------------------------- diff --git a/navigation/tukf_rsi/CMakeLists.txt b/navigation/tukf_rsi/CMakeLists.txt new file mode 100644 index 000000000..f1ad5e0b1 --- /dev/null +++ b/navigation/tukf_rsi/CMakeLists.txt @@ -0,0 +1,61 @@ +cmake_minimum_required(VERSION 3.8) +project(tukf_rsi) + +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(nav_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(tf2 REQUIRED) +find_package(vortex_msgs REQUIRED) +find_package(spdlog REQUIRED) +find_package(fmt REQUIRED) + +if(NOT DEFINED EIGEN3_INCLUDE_DIR) + set(EIGEN3_INCLUDE_DIR ${EIGEN3_INCLUDE_DIRS}) +endif() +include_directories(${EIGEN3_INCLUDE_DIR}) + +include_directories(include) + +add_executable(tukf_rsi_node + src/tukf_rsi.cpp + src/tukf_rsi_ros.cpp + src/tukf_rsi_node.cpp + src/tukf_rsi_utils.cpp +) + +ament_target_dependencies(tukf_rsi_node + rclcpp + geometry_msgs + nav_msgs + Eigen3 + tf2 + vortex_msgs + spdlog + fmt +) + +target_link_libraries(tukf_rsi_node + fmt::fmt +) + +install(TARGETS + tukf_rsi_node + DESTINATION lib/${PROJECT_NAME}) + +install(DIRECTORY + config + launch + DESTINATION share/${PROJECT_NAME}/ +) + +ament_package() diff --git a/navigation/tukf_rsi/config/tusk_rsi_params.yaml b/navigation/tukf_rsi/config/tusk_rsi_params.yaml new file mode 100644 index 000000000..c74717815 --- /dev/null +++ b/navigation/tukf_rsi/config/tusk_rsi_params.yaml @@ -0,0 +1,12 @@ +eskf_node: + ros__parameters: + gyro_topic: "/imu/data" + dvl_topic: "/orca/twist" + odom_topic: "/tukf/odom" + wrench_topic: "/orca/wrench_input" + diag_Q_std: [0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01] + diag_P0_std: [2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05] + x0: [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, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + diag_Rgyro_std: [0.01, 0.01, 0.01] + diag_Rdvl_std: [0.05, 0.05, 0.05] + diff --git a/navigation/tukf_rsi/include/tukf_rsi/tukf_rsi.hpp b/navigation/tukf_rsi/include/tukf_rsi/tukf_rsi.hpp new file mode 100644 index 000000000..a1e19edb5 --- /dev/null +++ b/navigation/tukf_rsi/include/tukf_rsi/tukf_rsi.hpp @@ -0,0 +1,63 @@ +// tukf.hpp +#ifndef TUKF_HPP +#define TUKF_HPP + +#include "typedefs.hpp" // includes AUVState, MeasModel, and utility functions +#include + +class TUKF { +public: + TUKF(const AUVState& x0, + const Eigen::Matrix37d& Q_in, + double dt = 0.01); + + // @brief Generate sigma points for the current state + // @param current_state: Current state of the AUV + // @return A vector of sigma points + std::vector sigma_points(const AUVState& current_state); + + // @brief Unscented transform to predict the next state + // @param current_state: Current state of the AUV + // @param control_force: Control force applied to the AUV + // @return Predicted state after applying the control force + AUVState unscented_transform(const AUVState& current_state, + const Eigen::Vector3d& control_force); + + // @brief Perform measurement update using the measurement model + // @param current_state: Current state of the AUV + // @param measurement: Measurement model containing the measurement and covariance + // @return Updated state after measurement update + void measurement_update(const AUVState& current_state, + const MeasModel& measurement); + + // @brief Posterior estimate of the state after measurement update + // @param current_state: Current state of the AUV + // @param measurement: Measurement model containing the measurement and covariance + // @return Posterior estimate of the state + AUVState posterior_estimate(const AUVState& current_state, + const MeasModel& measurement); + + // public state and flags + AUVState x; + bool filter_failed; + +private: + + Eigen::Matrix37d Q; + + Eigen::Matrix delta; + + std::vector sigma_points_list; + + std::vector y_i; + + MeasModel measurement_updated; + + Eigen::Matrix cross_correlation; + + double dt; + + int flagg; +}; + +#endif // TUKF_HPP \ No newline at end of file diff --git a/navigation/tukf_rsi/include/tukf_rsi/tukf_rsi_model.hpp b/navigation/tukf_rsi/include/tukf_rsi/tukf_rsi_model.hpp new file mode 100644 index 000000000..b536ca798 --- /dev/null +++ b/navigation/tukf_rsi/include/tukf_rsi/tukf_rsi_model.hpp @@ -0,0 +1,50 @@ +#ifndef TUKF_MODEL_HPP +#define TUKF_MODEL_HPP + +#include +#include "tukf_rsi/typedefs.hpp" + + +// @brief Tranformation matrix from quaternion orientation +// @param orientation: Quaternion representing the orientation +// @return 3x3 transformation matrix +Eigen::Matrix3d tranfromation_matrix(const Eigen::Quaterniond& orientation); + +// @brief Mass inertia system-matrix (6×6) +// @param inertia_vec: Vector containing the inertia parameters +Eigen::Matrix6d M_rb(const Eigen::Vector9d& inertia_vec); + +// @brief Added mass system-matrix (6×6) +// @param added_mass: Vector containing the added mass parameters +Eigen::Matrix6d M_a(const Eigen::Vector6d& added_mass); + +// @brief Corilos and centripetal forces system-matrix (6×6) +// @param inertia_vec: Vector containing the inertia parameters +// @param angular_velocity: Angular velocity vector +Eigen::Matrix6d C_rb(const Eigen::Vector9d& inertia_vec, + const Eigen::Vector3d& angular_velocity); + +// @brief added mass Corilos and centripetal forces system-matrix (6×6) +// @param added_mass: Vector containing the added mass parameters +// @param angular_velocity: Angular velocity vector +// @param velocity: Velocity linear vector +Eigen::Matrix6d C_a(const Eigen::Vector6d& added_mass, + const Eigen::Vector3d& angular_velocity, + const Eigen::Vector3d& velocity); + +// @brief Damping system-matrix (6×6) +// @param damping: Vector containing the damping parameters +Eigen::Matrix6d D_linear(const Eigen::Vector6d& damping); + +// @brief generalized froces (6×1) +// @param g_eta_params: Vector containing the g_eta parameters (buoyancy terms) +// @param euler_angles: Euler angles (roll, pitch, yaw) +Eigen::Vector6d G_eta(const Eigen::Vector4d& g_eta_params, + const Eigen::Vector3d& euler_angles); + +// @brief Dynamics function for the AUV +AUVState F_dynamics(const AUVState& state, + double dt, + const Eigen::Vector3d& control_input); + +#endif // TUKF_MODEL_HPP \ No newline at end of file diff --git a/navigation/tukf_rsi/include/tukf_rsi/tukf_rsi_ros.hpp b/navigation/tukf_rsi/include/tukf_rsi/tukf_rsi_ros.hpp new file mode 100644 index 000000000..98b0a547a --- /dev/null +++ b/navigation/tukf_rsi/include/tukf_rsi/tukf_rsi_ros.hpp @@ -0,0 +1,64 @@ +#ifndef TUKF_NODE_HPP +#define TUKF_NODE_HPP + +#include +#include +#include +#include +#include +#include +#include +#include +#include "tukf_rsi/tukf.hpp" +#include "tukf_rsi/tukf_rsi_utils.hpp" +#include "tukf_rsi/typedefs.hpp" + +class TUKFNode : public rclcpp::Node { +public: + TUKFNode(); + +private: + + // @brief Callback function for the gyro topic + // @param msg: Imu message containing the gyro data + void gyro_callback(const sensor_msgs::msg::Imu::SharedPtr msg); + + // @brief Callback function for the DVL topic + // @param msg: TwistWithCovarianceStamped message containing the DVL data + void dvl_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); + + // @brief Callback function for the wrench topic + // @param msg: WrenchStamped message containing the wrench data + void wrench_callback(const geometry_msgs::msg::WrenchStamped::SharedPtr msg); + + // @brief Set the subscriber and publisher for the node + void set_subscribers_and_publisher(); + + // @brief Set the parameters for the eskf + void set_parameters(); + + // @brief Publish the odometry message + void publish_odom(); + + rclcpp::Subscription::SharedPtr gyro_sub_; + + rclcpp::Subscription::SharedPtr dvl_sub_; + + rclcpp::Subscription::SharedPtr wrench_sub_; + + rclcpp::Publisher::SharedPtr odom_pub_; + + rclcpp::TimerBase::SharedPtr odom_timer_; + + std::unique_ptr tukf_; + + AUVState state_; + + double dt_; + + Eigen::Matrix3d R_gyro_; + + Eigen::Matrix3d R_dvl_; +}; + +#endif // TUKF_NODE_HPP \ No newline at end of file diff --git a/navigation/tukf_rsi/include/tukf_rsi/tukf_rsi_utils.hpp b/navigation/tukf_rsi/include/tukf_rsi/tukf_rsi_utils.hpp new file mode 100644 index 000000000..b3c31221f --- /dev/null +++ b/navigation/tukf_rsi/include/tukf_rsi/tukf_rsi_utils.hpp @@ -0,0 +1,65 @@ +#ifndef TUKF_RSI_UTILS_HPP +#define TUKF_RSI_UTILS_HPP + +#include +#include +#include "tukf_rsi/typedefs.hpp" + +// @brief Compute mean quaternion from a set of quaternions +// @param quats: Vector of quaternions +// @param tol: Tolerance for convergence +// @param maxIter: Maximum number of iterations +Eigen::Quaterniond quaternion_mean( + const std::vector& quats, + double tol = 1e-6, + int maxIter = 100 +); + +// @brief Compute mean of a set of AUV states +// @param setPoints: Vector of AUVState objects +// @param tol: Tolerance for convergence +// @param maxIter: Maximum number of iterations +Eigen::Vector37d mean_set( + const std::vector& setPoints, + double tol = 1e-6, + int maxIter = 100 +); + +// @brief Compute mean of a set of measurements +// @param setPoints: Vector of MeasModel objects +Eigen::Vector3d mean_masurement(const std::vector& setPoints); + +// @brief Compute covariance of a set of AUV states +// @param setPoints: Vector of AUVState objects +// @param meanVec: Mean vector of the set +// @param tol: Tolerance for convergence +Eigen::Matrix37d covariance_set( + const std::vector& setPoints, + const Eigen::Vector37d& meanVec, + double tol = 1e-6 +); + +// @brief Compute covariance of a set of measurements +// @param setPoints: Vector of MeasModel objects +// @param mean: Mean vector of the measurements +Eigen::Matrix3d covariance_measurement( + const std::vector& setPoints, + const Eigen::Vector3d& mean +); + +// @brief Compute cross-covariance between AUV states and measurements +// @param setY: Vector of AUVState objects +// @param meanY: Mean vector of the AUV states +// @param setZ: Vector of MeasModel objects +// @param meanZ: Mean vector of the measurements +// @param tol: Tolerance for convergence +Eigen::Matrix cross_covariance( + const std::vector& setY, + const Eigen::Vector37d& meanY, + const std::vector& setZ, + const Eigen::Vector3d& meanZ, + double tol = 1e-6 +); + +#endif // TUKF_RSI_UTILS_HPP + diff --git a/navigation/tukf_rsi/include/tukf_rsi/typedefs.hpp b/navigation/tukf_rsi/include/tukf_rsi/typedefs.hpp new file mode 100644 index 000000000..0d724577d --- /dev/null +++ b/navigation/tukf_rsi/include/tukf_rsi/typedefs.hpp @@ -0,0 +1,145 @@ +#ifndef AUV_TYPEDEFS_HPP +#define AUV_TYPEDEFS_HPP + +#include +#include +#include +#include + +namespace Eigen { + typedef Matrix Vector37d; + typedef Matrix Matrix37d; + typedef Matrix Vector25d; + typedef Matrix Vector12d; + typedef Matrix Vector9d; + typedef Matrix Vector6d; + typedef Matrix Vector4d; + typedef Matrix Matrix3x12d; + typedef Matrix Matrix3d; + typedef Matrix Matrix3x37d; + typedef Matrix Matrix37x74d; + typedef Matrix Matrix37x3d; +} + +struct AUVState { + Eigen::Vector3d position = Eigen::Vector3d::Zero(); + Eigen::Quaterniond orientation = Eigen::Quaterniond::Identity(); + Eigen::Vector3d velocity = Eigen::Vector3d::Zero(); + Eigen::Vector3d angular_velocity = Eigen::Vector3d::Zero(); + Eigen::Vector9d inertia = Eigen::Vector9d::Zero(); + Eigen::Vector6d added_mass = Eigen::Vector6d::Zero(); + Eigen::Vector6d damping = Eigen::Vector6d::Zero(); + Eigen::Vector4d g_eta = Eigen::Vector4d::Zero(); + Eigen::Matrix37d covariance = Eigen::Matrix37d::Zero(); + + Eigen::Vector3d error = Eigen::Vector3d::Zero(); + + AUVState() = default; + + Eigen::Vector12d dynamic_part() const { + Eigen::Vector12d x; + x << position, + orientation.vec(), + velocity, + angular_velocity; + return x; + } + + Eigen::Vector25d okid_part() const { + Eigen::Vector25d x; + x << inertia, + added_mass, + damping, + g_eta; + return x; + } + + Eigen::Vector37d as_vector() const { + Eigen::Vector37d x; + x << dynamic_part(), + okid_part(); + return x; + } + + AUVState operator+(const AUVState& other) const { + AUVState result; + result.position = position + other.position; + result.orientation = orientation * other.orientation; + result.velocity = velocity + other.velocity; + result.angular_velocity = angular_velocity + other.angular_velocity; + result.inertia = inertia + other.inertia; + result.added_mass = added_mass + other.added_mass; + result.damping = damping + other.damping; + result.g_eta = g_eta + other.g_eta; + return result; + } + + AUVState operator-(const AUVState& other) const { + AUVState result; + result.position = position - other.position; + result.orientation = orientation * other.orientation.inverse(); + result.velocity = velocity - other.velocity; + result.angular_velocity = angular_velocity - other.angular_velocity; + result.inertia = inertia - other.inertia; + result.added_mass = added_mass - other.added_mass; + result.damping = damping - other.damping; + result.g_eta = g_eta - other.g_eta; + return result; + } + + void fill_states(const Eigen::Vector37d& x) { + position = x.segment<3>(0); + Eigen::Vector3d ori_vec = x.segment<3>(3); + orientation = Eigen::Quaterniond(1, ori_vec.x(), ori_vec.y(), ori_vec.z()).normalized(); + velocity = x.segment<3>(6); + angular_velocity = x.segment<3>(9); + inertia = x.segment<9>(12); + added_mass = x.segment<6>(21); + damping = x.segment<6>(27); + g_eta = x.segment<4>(33); + } +}; + +struct MeasModel { + Eigen::Vector3d measurement = Eigen::Vector3d::Zero(); + Eigen::Matrix3d covariance = Eigen::Matrix3d::Zero(); + std::function H; + + MeasModel() + : H(default_h) + {} + + MeasModel(const Eigen::Vector3d& meas, + const Eigen::Matrix3d& cov, + std::function Hfunc = default_h) + : measurement(meas), covariance(cov), H(std::move(Hfunc)) + {} + + static MeasModel default_h(const AUVState& state) { + MeasModel z; + Eigen::Matrix3x12d Hmat = Eigen::Matrix3x12d::Zero(); + Hmat.block<3,3>(0,6) = Eigen::Matrix3d::Identity(); + z.measurement = Hmat * state.dynamic_part(); + return z; + } + + MeasModel operator+(const MeasModel& other) const { + MeasModel r; + r.measurement = measurement + other.measurement; + return r; + } + + MeasModel operator-(const MeasModel& other) const { + MeasModel r; + r.measurement = measurement - other.measurement; + return r; + } + + friend MeasModel operator*(double scalar, const MeasModel& m) { + MeasModel r; + r.measurement = scalar * m.measurement; + return r; + } +}; + +#endif // AUV_TYPEDEFS_HPP \ No newline at end of file diff --git a/navigation/tukf_rsi/launch/tukf_rsi.launch.py b/navigation/tukf_rsi/launch/tukf_rsi.launch.py new file mode 100644 index 000000000..e238b3e1e --- /dev/null +++ b/navigation/tukf_rsi/launch/tukf_rsi.launch.py @@ -0,0 +1,22 @@ +from os import path + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + +tukf_rsi_params = path.join( + get_package_share_directory("tukf_rsi"), "config", "tukf_rsi_params.yaml" +) + + +def generate_launch_description(): + tukf_rsi_node = Node( + package="tukf_rsi", + executable="tukf_rsi_node", + name="tukf_rsi_node", + parameters=[ + tukf_rsi_params, + ], + output="screen", + ) + return LaunchDescription([tukf_rsi_node]) diff --git a/navigation/ukf_okid/package.xml b/navigation/tukf_rsi/package.xml similarity index 56% rename from navigation/ukf_okid/package.xml rename to navigation/tukf_rsi/package.xml index 1c1b2b4cb..989241795 100644 --- a/navigation/ukf_okid/package.xml +++ b/navigation/tukf_rsi/package.xml @@ -1,20 +1,20 @@ - ukf_python + tukf_rsi 1.0.0 - Uscented Kalman filter for AUV model - talha + Transformed Unscented Kalman Filter + talhanc MIT - ament_cmake_python + ament_cmake - rclpy + rclcpp geometry_msgs nav_msgs + eigen + tf2 vortex_msgs - python-control-pip - std_msgs ament_cmake diff --git a/navigation/tukf_rsi/src/tukf_rsi.cpp b/navigation/tukf_rsi/src/tukf_rsi.cpp new file mode 100644 index 000000000..df0195d86 --- /dev/null +++ b/navigation/tukf_rsi/src/tukf_rsi.cpp @@ -0,0 +1,84 @@ +#include "tukf_rsi/tukf.hpp" +#include "tukf_rsi/tukf_rsi_utils.hpp" +#include "tukf_rsi/typedefs.hpp" +#include "tukf_rsi/tukf_rsi_model.hpp" +#include + +TUKF::TUKF(const AUVState& x0, + const Eigen::Matrix37d& Q_in, + double dt_in) + : x(x0), Q(Q_in), dt(dt_in), filter_failed(false), flagg(0) +{ + delta = generate_elta_matrix37() / std::sqrt(static_cast(x.as_sector().size())); + measurement_updated = MeasModel(); +} + +std::vector TUKF::sigma_points(const AUVState& current_state) { + int n = static_cast(current_state.covariance.rows()); + ++flagg; + Eigen::Matrix37d S; + bool chol_ok = true; + auto llt = current_state.covariance.llt(); + if(llt.info() == Eigen::NumericalIssue) { + chol_ok = false; + } else { + S = llt.matrixL(); + } + if (!chol_ok) { + filter_failed = true; + S = Eigen::Matrix37d::Identity() * 1e-6; + } + sigma_points_list.resize(2 * n); + for (int k = 0; k < 2 * n; ++k) { + Eigen::Vector37d v = current_state.as_vector() + S * delta.col(k); + sigma_points_list[k].fill_States(v); + } + return sigma_points_list; +} + +AUVState TUKF::unscented_transform(const AUVState& current_state, + const Eigen::Vector3d& control_force) { + int n = static_cast(current_state.covariance.rows()); + sigma_soints(current_state); + y_i.resize(2 * n); + for (int i = 0; i < 2 * n; ++i) { + y_i[i] = F_dynamics(sigma_points_list[i], dt, control_force); + } + AUVState state_est; + Eigen::Vector37d x_vec = mean_Set(y_i); + state_est.fill_States(x_vec); + state_est.covariance = covarianceSet(y_i, x_vec) + Q; + return state_est; +} + +void TUKF::measurement_update(const AUVState& current_state, + const MeasModel& measurement) { + int n = static_cast(current_state.covariance.rows()); + std::vector z_i(2 * n); + for (int i = 0; i < 2 * n; ++i) { + z_i[i] = measurement.H(sigma_points_list[i]); + } + measurement_updated.measurement = mean_seasurement(z_i); + measurement_updated.covariance = covariance_measurement( + z_i, measurement_updated.measurement); + cross_correlation = cross_covariance( + y_i, + current_state.as_vector(), + z_i, + measurement_updated.measurement); +} + +AUVState TUKF::posterior_estimate(const AUVState& current_state, + const MeasModel& measurement) { + MeasModel nu_k; + nu_k.measurement = measurement.measurement - measurement_updated.measurement; + nu_k.covariance = measurement_updated.covariance + measurement.covariance; + Eigen::Matrix K = cross_correlation * nu_k.covariance.inverse(); + AUVState post; + Eigen::Vector37d v = current_state.as_vector() + + K * nu_k.measurement; + post.fill_states(v); + post.covariance = current_state.covariance - + K * nu_k.covariance * K.transpose(); + return post; +} \ No newline at end of file diff --git a/navigation/tukf_rsi/src/tukf_rsi_model.cpp b/navigation/tukf_rsi/src/tukf_rsi_model.cpp new file mode 100644 index 000000000..97a91c024 --- /dev/null +++ b/navigation/tukf_rsi/src/tukf_rsi_model.cpp @@ -0,0 +1,125 @@ +#include "tukf_rsi/tukf_model.hpp" +#include "tukf_rsi/tukf_rsi_utils.hpp" +#include "tukf_rsi/typedefs.hpp" +#include +#include + + +Eigen::Matrix4x3d tranfromation_matrix(const Eigen::Quaterniond& q) { + Eigen::Matrix4x3d T; + T << -q.x(), -q.y(), -q.z(), + q.w(), -q.z(), q.y(), + q.z(), q.w(), -q.x(), + -q.y(), q.x(), q.w(); + return T; +} + +Eigen::Matrix6d M_rb(const Eigen::Vector& inertia_vec) { + const double mass = 30.0; + const Eigen::Matrix3d I_rb = Eigen::Map(inertia_vec.data()); + const Eigen::Vector3d r_b_bg(0.01, 0.0, 0.02); + + Eigen::Matrix6d M = Eigen::Matrix6d::Zero(); + M.block<3,3>(0,0) = mass * Eigen::Matrix3d::Identity(); + M.block<3,3>(0,3) = -mass * skewSymmetric(r_b_bg); + M.block<3,3>(3,0) = mass * skewSymmetric(r_b_bg); + M.block<3,3>(3,3) = I_rb; + + return M; +} + +Eigen::Matrix6d M_a(const Eigen::Vector& a_mass) { + Eigen::Matrix6d Ma = Eigen::Matrix6d::Zero(); + Ma.block<3,3>(0,0) = a_mass.head<3>().asDiagonal(); + Ma.block<3,3>(3,3) = a_mass.tail<3>().asDiagonal(); + return Ma; +} + +Eigen::Matrix6d C_rb(const Eigen::Vector& inertia_vec, const Eigen::Vector3d& w) { + const double mass = 30.0; + const Eigen::Vector3d r_b_bg(0.01, 0.0, 0.02); + const Eigen::Matrix3d I_rb = Eigen::Map(inertia_vec.data()); + + Eigen::Matrix6d C = Eigen::Matrix6d::Zero(); + C.block<3,3>(3,3) = -skewSymmetric(I_rb * w); + C.block<3,3>(0,3) = -mass * skewSymmetric(w) * skewSymmetric(r_b_bg); + C.block<3,3>(3,0) = mass * skewSymmetric(r_b_bg) * skewSymmetric(w); + + return C; +} + +Eigen::Matrix6d C_a(const Eigen::Vector& a_mass, const Eigen::Vector3d& w, const Eigen::Vector3d& v) { + Eigen::Matrix6d Ca = Eigen::Matrix6d::Zero(); + const Eigen::Matrix3d A11 = a_mass.head<3>().asDiagonal(); + const Eigen::Matrix3d A22 = a_mass.tail<3>().asDiagonal(); + + Ca.block<3,3>(0,3) = -skewSymmetric(A11 * v); + Ca.block<3,3>(3,0) = -skewSymmetric(A11 * v); + Ca.block<3,3>(3,3) = -skewSymmetric(A22 * w); + + return Ca; +} + +Eigen::Matrix6d D_linear(const Eigen::Vector& d) { + Eigen::Matrix6d D = Eigen::Matrix6d::Zero(); + D.block<3,3>(0,0) = -d.head<3>().asDiagonal(); + D.block<3,3>(3,3) = -d.tail<3>().asDiagonal(); + return D; +} + +Eigen::Vector6d G_eta(const Eigen::Vector& g_params, const Eigen::Quaterniond& q) { + const double Mx = g_params[1], My = g_params[2], Mz = g_params[3]; + const Eigen::Matrix3d R = q.toRotationMatrix(); + + Eigen::Vector6d G = Eigen::Vector6d::Zero(); + G(3) = -My*R(2,2) + Mz*R(1,2); + G(4) = -Mz*R(0,2) + Mx*R(2,2); + G(5) = -Mx*R(1,2) + My*R(0,2); + + return G; +} + +AUVState F_dynamics(const AUVState& state, double dt, const Eigen::Vector6d& u) { + const Eigen::Matrix6d Mrb = M_rb(state.inertia); + const Eigen::Matrix6d Ma = M_a(state.added_mass); + const Eigen::Matrix6d Mtotal = Mrb + Ma; + + const Eigen::Matrix6d Crb = C_rb(state.inertia, state.angular_velocity); + const Eigen::Matrix6d Ca = C_a(state.added_mass, state.angular_velocity, state.velocity); + const Eigen::Matrix6d Ctotal = Crb + Ca; + + const Eigen::Matrix6d Dl = D_linear(state.damping); + const Eigen::Vector6d G = G_eta(state.g_eta, state.orientation); + + Eigen::Vector6d nu; + nu << state.velocity, state.angular_velocity; + + AUVState sd; + sd.position = state.orientation.toRotationMatrix() * state.velocity; + sd.orientation = tranfromation_matrix(state.orientation) * state.angular_velocity; + + Eigen::Vector6d Nu = Mtotal.inverse() * (u - Ctotal*nu - Dl*nu - G); + sd.velocity = Nu.head<3>(); + sd.angular_velocity = Nu.tail<3>(); + + sd.inertia.setZero(); + sd.added_mass.setZero(); + sd.damping.setZero(); + sd.g_eta.setZero(); + + AUVState ns; + ns.position = state.position + sd.position * dt; + ns.orientation = state.orientation * Eigen::Quaterniond(1, 0.5*dt*sd.orientation.x(), + 0.5*dt*sd.orientation.y(), + 0.5*dt*sd.orientation.z()); + ns.orientation.normalize(); + ns.velocity = state.velocity + sd.velocity * dt; + ns.angular_velocity = state.angular_velocity + sd.angular_velocity * dt; + + ns.inertia = state.inertia; + ns.added_mass = state.added_mass; + ns.damping = state.damping; + ns.g_eta = state.g_eta; + + return ns; +} diff --git a/navigation/tukf_rsi/src/tukf_rsi_node.cpp b/navigation/tukf_rsi/src/tukf_rsi_node.cpp new file mode 100644 index 000000000..d4f6eed17 --- /dev/null +++ b/navigation/tukf_rsi/src/tukf_rsi_node.cpp @@ -0,0 +1,9 @@ +#include "tukf_rsi/tukf_rsi_ros.hpp" + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + spdlog::info("Starting TUFK for RSI ROS2 Node"); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/navigation/tukf_rsi/src/tukf_rsi_ros.cpp b/navigation/tukf_rsi/src/tukf_rsi_ros.cpp new file mode 100644 index 000000000..2be453191 --- /dev/null +++ b/navigation/tukf_rsi/src/tukf_rsi_ros.cpp @@ -0,0 +1,150 @@ +#include "tukf_node.hpp" +#include + +TUKFNode::TUKFNode() +: Node("tukf_node") +{ + + odom_timer_ = this->create_wall_timer( + std::chrono::duration(dt_), + std::bind(&TUKFNode::publishOdom, this)); + + set_subscribers_and_publisher(); + + set_parameters(); + + spdlog::info("TUKF Node Initialized"); +} + +void TUKFNode::set_subscribers_and_publisher() { + auto qos = rclcpp::QoS(rclcpp::SensorDataQoS()); + this->declare_parameter("gyro_topic"); + std::string gyro_topic = this->get_parameter("gyro_topic").as_string(); + gyro_sub_ = this->create_subscription( + gyro_topic, qos, + std::bind(&TUKFNode::gyroCallback, this, std::placeholders::_1)); + + this->declare_parameter("dvl_topic"); + std::string dvl_topic = this->get_parameter("dvl_topic").as_string(); + dvl_sub_ = this->create_subscription( + dvl_topic, qos, + std::bind(&TUKFNode::dvlCallback, this, std::placeholders::_1)); + + this->declare_parameter("wrench_topic"); + std::string dvl_topic = this->get_parameter("wrench_topic").as_string(); + wrench_sub_ = this->create_subscription( + wrench_sub_, qos, + std::bind(&TUKFNode::wrenchCallback, this, std::placeholders::_1)); + + this->declare_parameter("odom_topic"); + std::string odom_topic = this->get_parameter("odom_topic").as_string(); + odom_pub_ = this->create_publisher( + odom_topic, qos); +} + +void TUKFNode::set_parameters() { + this->declare_parameter>("diag_Q_std"); + auto diagQ = this->get_parameter("diag_Q_std").as_double_array(); + Eigen::Matrix37d Q = Eigen::Matrix37d::Zero(); + for (int i = 0; i < 37; ++i) { + Q(i,i) = diagQ[i] * diagQ[i]; + } + + this->declare_parameter>("diag_P0_std"); + auto diagP0 = this->get_parameter("diag_P0_std").as_double_array(); + Eigen::Matrix37d P0 = Eigen::Matrix37d::Zero(); + for (int i = 0; i < 37; ++i) { + P0(i,i) = diagP0[i] * diagP0[i]; + } + + this->declare_parameter>("x0"); + auto x0_vec = this->get_parameter("x0").as_double_array(); + Eigen::Vector37d x0_e; + for (int i = 0; i < 37; ++i) x0_e[i] = x0_vec[i]; + + this->declare_parameter("dt", 0.01); + dt_ = this->get_parameter("dt").as_double(); + + this->declare_parameter>("diag_Rgyro_std"); + auto diagRgyro = this->get_parameter("diag_Rgyro_std").as_double_array(); + R_gyro_ = Eigen::Matrix3d::Zero(); + for (int i = 0; i < 3; ++i) R_gyro_(i,i) = diagRgyro[i] * diagRgyro[i]; + + this->declare_parameter>("diag_Rdvl_std"); + auto diagRdvl = this->get_parameter("diag_Rdvl_std").as_double_array(); + R_dvl_ = Eigen::Matrix3d::Zero(); + for (int i = 0; i < 3; ++i) R_dvl_(i,i) = diagRdvl[i] * diagRdvl[i]; + + tukf_ = std::make_unique(x0, Q, dt_); + tukf_->x.covariance = P0; + tukf_->x.fill_states(x0_e); +} + +void TUKFNode::gyro_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { + Eigen::Vector3d gyro(msg->angular_velocity.x, + msg->angular_velocity.y, + msg->angular_velocity.z); + MeasModel m; + m.measurement = gyro; + m.covariance = R_gyro_; + m.H = [](const AUVState& s) { + MeasModel mm; + Eigen::Matrix3x12d Hm = Eigen::Matrix3x12d::Zero(); + Hm.block<3,3>(0,9) = Eigen::Matrix3d::Identity(); + mm.measurement = Hm * s.dynamic_part(); + return mm; + }; + + tukf_->measurement_update(state_, m); + state_ = tukf_->posterior_estimate(state_, m); +} + +void TUKFNode::dvl_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { + Eigen::Vector3d vel(msg->twist.twist.linear.x, + msg->twist.twist.linear.y, + msg->twist.twist.linear.z); + Eigen::Matrix3d cov; + cov << msg->twist.covariance[0], msg->twist.covariance[1], msg->twist.covariance[2], + msg->twist.covariance[6], msg->twist.covariance[7], msg->twist.covariance[8], + msg->twist.covariance[12], msg->twist.covariance[13], msg->twist.covariance[14]; + MeasModel m; + m.measurement = vel; + m.covariance = R_dvl_; + + tukf_->measurement_update(state_, m); + state_ = tukf_->posterior_estimate(state_, m); +} + +void TUKFNode::wrench_callback(const geometry_msgs::msg::WrenchStamped::SharedPtr msg) { + Eigen::Vector6d wrench_input(msg->wrench.force.x, + msg->wrench.force.y, + msg->wrench.force.z, + msg->wrench.torque.x, + msg->wrench.torque.y, + msg->wrench.torque.z); + + state_ = tukf_->unscented_transform(state_, control_force); +} + +void TUKFNode::publish_odom() { + nav_msgs::msg::Odometry odom; + odom.header.stamp = this->now(); + odom.header.frame_id = "odom"; + + odom.pose.pose.position.x = state_.position.x(); + odom.pose.pose.position.y = state_.position.y(); + odom.pose.pose.position.z = state_.position.z(); + odom.pose.pose.orientation.w = state_.orientation.w(); + odom.pose.pose.orientation.x = state_.orientation.x(); + odom.pose.pose.orientation.y = state_.orientation.y(); + odom.pose.pose.orientation.z = state_.orientation.z(); + + odom.twist.twist.linear.x = state_.velocity.x(); + odom.twist.twist.linear.y = state_.velocity.y(); + odom.twist.twist.linear.z = state_.velocity.z(); + odom.twist.twist.angular.x = state_.angular_velocity.x(); + odom.twist.twist.angular.y = state_.angular_velocity.y(); + odom.twist.twist.angular.z = state_.angular_velocity.z(); + + odom_pub_->publish(odom); +} diff --git a/navigation/tukf_rsi/src/tukf_rsi_utils.cpp b/navigation/tukf_rsi/src/tukf_rsi_utils.cpp new file mode 100644 index 000000000..882fe0cf8 --- /dev/null +++ b/navigation/tukf_rsi/src/tukf_rsi_utils.cpp @@ -0,0 +1,199 @@ + +#include "tukf_rsi_utils.hpp" +#include + +Eigen::Quaterniond quaternionMean( + const std::vector& quats, + double tol, + int maxIter +) { + Eigen::Quaterniond mean_q = quats.front(); + int n = int(quats.size()); + + for (int iter = 0; iter < maxIter; ++iter) { + Eigen::Vector3d errAvg = Eigen::Vector3d::Zero(); + + for (const auto& q : quats) { + + Eigen::Quaterniond e = q * mean_q.conjugate(); + + double w = std::clamp(e.w(), -1.0, 1.0); + double angle = 2 * std::acos(w); + + Eigen::Vector3d axis; + + if (std::abs(angle) < tol) { + axis.setZero(); + } else { + axis = (angle / std::sin(angle / 2.0)) * e.vec(); + } + errAvg += axis; + } + errAvg /= double(n); + + if (errAvg.norm() < tol) break; + double errNorm = errAvg.norm(); + Eigen::Quaterniond dq + + if (errNorm > tol) { + dq.w() = std::cos(errNorm/2.0); + dq.vec() = std::sin(errNorm/2.0) * (errAvg/errNorm); + } else { + dq = Eigen::Quaterniond::Identity(); + } + mean_q = dq * mean_q; + mean_q.normalize(); + } + return mean_q; +} + +Eigen::Vector37d mean_set( + const std::vector& setPoints, + double tol, + int maxIter +) { + int n = int(setPoints.size()); + + Eigen::Vector3d posAvg = Eigen::Vector3d::Zero(); + Eigen::Vector3d velAvg = Eigen::Vector3d::Zero(); + Eigen::Vector3d angVelAvg = Eigen::Vector3d::Zero(); + Eigen::Vector9d inAvg = Eigen::Vector9d::Zero(); + Eigen::Vector6d amAvg = Eigen::Vector6d::Zero(); + Eigen::Vector6d dAvg = Eigen::Vector6d::Zero(); + Eigen::Vector4d gAvg = Eigen::Vector4d::Zero(); + + std::vector quats; + quats.reserve(n); + + for (const auto& s : setPoints) { + posAvg += s.position; + velAvg += s.velocity; + angVelAvg += s.angular_velocity; + inAvg += s.inertia; + amAvg += s.added_mass; + dAvg += s.damping; + gAvg += s.g_eta; + quats.push_back(s.orientation); + } + posAvg /= double(n); + velAvg /= double(n); + angVelAvg /= double(n); + inAvg /= double(n); + amAvg /= double(n); + dAvg /= double(n); + gAvg /= double(n); + Eigen::Quaterniond qMean = quaternion_mean(quats, tol, maxIter); + + AUVState meanState; + meanState.position = posAvg; + meanState.orientation = qMean; + meanState.velocity = velAvg; + meanState.angular_velocity = angVelAvg; + meanState.inertia = inAvg; + meanState.added_mass = amAvg; + meanState.damping = dAvg; + meanState.g_eta = gAvg; + + return meanState.asVector(); +} + +Eigen::Vector3d mean_measurement(const std::vector& setPoints) { + Eigen::Vector3d avg = Eigen::Vector3d::Zero(); + for (const auto& m : setPoints) avg += m.measurement; + return avg / double(setPoints.size()); +} + +Eigen::Matrix37d covariance_set( + const std::vector& setPoints, + const Eigen::Vector37d& meanVec, + double tol +) { + int n = int(setPoints.size()); + AUVState meanState; + meanState.fillStates(meanVec); + std::vector quats; + quats.reserve(n); + + for (const auto& s : setPoints) quats.push_back(s.orientation); + meanState.orientation = quaternion_mean(quats, tol); + + Eigen::Matrix37d cov = Eigen::Matrix37d::Zero(); + for (const auto& s : setPoints) { + Eigen::Vector37d d = Eigen::Vector37d::Zero(); + + d.segment<3>(0) = s.position - meanState.position; + + Eigen::Quaterniond e = s.orientation * meanState.orientation.conjugate(); + double w = std::clamp(e.w(), -1.0, 1.0); + double angle = 2 * std::acos(w); + + + Eigen::Vector3d err; + if (std::abs(angle) < tol) err.setZero(); + else err = (angle / std::sin(angle/2.0)) * e.vec(); + + d.segment<3>(3) = err; + + d.segment<3>(6) = s.velocity - meanState.velocity; + d.segment<3>(9) = s.angular_velocity - meanState.angular_velocity; + d.segment<9>(12) = s.inertia - meanState.inertia; + d.segment<6>(21) = s.added_mass - meanState.added_mass; + d.segment<6>(27) = s.damping - meanState.damping; + d.segment<4>(33) = s.g_eta - meanState.g_eta; + cov += d * d.transpose(); + } + return cov / double(n); +} + +Eigen::Matrix3d covariance_measurement( + const std::vector& setPoints, + const Eigen::Vector3d& mean +) { + Eigen::Matrix3d cov = Eigen::Matrix3d::Zero(); + for (const auto& m : setPoints) { + Eigen::Vector3d d = m.measurement - mean; + cov += d * d.transpose(); + } + return cov / double(setPoints.size()); +} + +Eigen::Matrix cross_covariance( + const std::vector& setY, + const Eigen::Vector37d& meanY, + const std::vector& setZ, + const Eigen::Vector3d& meanZ, + double tol +) { + int n = int(setY.size()); + AUVState meanState; + meanState.fill_states(meanY); + std::vector quats; + quats.reserve(n); + for (const auto& s : setY) quats.push_back(s.orientation); + meanState.orientation = quaternion_cean(quats, tol); + + Eigen::Matrix cov = Eigen::Matrix::Zero(); + for (size_t i = 0; i < setY.size(); ++i) { + const auto& s = setY[i]; + + Eigen::Vector37d dY = Eigen::Vector37d::Zero(); + dY.segment<3>(0) = s.position - meanState.position; + Eigen::Quaterniond e = s.orientation * meanState.orientation.conjugate(); + double w = std::clamp(e.w(), -1.0, 1.0); + double angle = 2 * std::acos(w); + Eigen::Vector3d err; + if (std::abs(angle) < tol) err.setZero(); + else err = (angle / std::sin(angle/2.0)) * e.vec(); + dY.segment<3>(3) = err; + dY.segment<3>(6) = s.velocity - meanState.velocity; + dY.segment<3>(9) = s.angular_velocity - meanState.angular_velocity; + dY.segment<9>(12) = s.inertia - meanState.inertia; + dY.segment<6>(21) = s.added_mass - meanState.added_mass; + dY.segment<6>(27) = s.damping - meanState.damping; + dY.segment<4>(33) = s.g_eta - meanState.g_eta; + + Eigen::Vector3d dZ = setZ[i].measurement - meanZ; + cov += dY * dZ.transpose(); + } + return cov / double(n); +} \ No newline at end of file diff --git a/navigation/ukf_okid/CMakeLists.txt b/navigation/ukf_okid/CMakeLists.txt deleted file mode 100644 index 901ca044b..000000000 --- a/navigation/ukf_okid/CMakeLists.txt +++ /dev/null @@ -1,23 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(ukf_python) - -find_package(ament_cmake_python REQUIRED) -find_package(rclpy REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(vortex_msgs REQUIRED) - -install(DIRECTORY - launch - config - DESTINATION share/${PROJECT_NAME} -) - -ament_python_install_package(${PROJECT_NAME}) - -install(PROGRAMS - ukf_python/ukf_ros.py - DESTINATION lib/${PROJECT_NAME} -) - -ament_package() diff --git a/navigation/ukf_okid/launch/ukf.launch.py b/navigation/ukf_okid/launch/ukf.launch.py deleted file mode 100644 index 5d075259f..000000000 --- a/navigation/ukf_okid/launch/ukf.launch.py +++ /dev/null @@ -1,12 +0,0 @@ -from launch import LaunchDescription -from launch_ros.actions import Node - - -def generate_launch_description() -> LaunchDescription: - ukf_node = Node( - package="ukf_python", - executable="ukf_ros.py", - name="ukf_node", - ) - - return LaunchDescription([ukf_node]) diff --git a/navigation/ukf_okid/ukf_python/__init__.py b/navigation/ukf_okid/ukf_python/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/navigation/ukf_okid/ukf_python/ukf_okid.py b/navigation/ukf_okid/ukf_python/ukf_okid.py deleted file mode 100644 index 240ca42bb..000000000 --- a/navigation/ukf_okid/ukf_python/ukf_okid.py +++ /dev/null @@ -1,130 +0,0 @@ -import numpy as np -from ukf_okid_class import ( - MeasModel, - StateQuat, - covariance_measurement, - covariance_set, - cross_covariance, - mean_measurement, - mean_set, - F_dynamics, -) - - -class UKF: - def __init__(self, x_0: StateQuat, Q): - self.x = x_0 - self.Q = Q - # self.G = G - self.sigma_points_list = None - self.measurement_updated = MeasModel() - self.y_i = None - self.weight = None - self.delta = self.generate_delta_matrix(len(x_0.as_vector()) - 1) - self.cross_correlation = None - self.dt = 0.01 # Time step for dynamics - - def generate_delta_matrix(self, n: float) -> np.ndarray: - """Generates the weight matrix used in the TUKF sigma point generation. - - Parameters: - n (int): The state dimension. - - Returns: - delta (np.ndarray): An n x 2n orthonormal transformation matrix used to generate TUKF sigma points. - """ - delta = np.zeros((n, 2 * n)) - k = 0.00000001 # Tuning parameter to ensure pos def - - for i in range(2 * n): - for j in range(n // 2): - delta[2 * j + 1, i] = ( - np.sqrt(2) * np.sin((2 * j - 1) * ((k * np.pi) / n)) - ) - delta[2 * j, i] = np.sqrt(2) * np.cos((2 * j - 1) * ((k * np.pi) / n)) - - if (n % 2) == 1: - delta[n - 1, i] = (-1) ** i - return delta - - def sigma_points(self, current_state: StateQuat) -> list[StateQuat]: - """Functions that generate the sigma points for the UKF.""" - n = len(current_state.covariance) - - S = np.linalg.cholesky(current_state.covariance + self.Q) - - self.sigma_points_list = [StateQuat() for _ in range(2 * n)] - - for index, state in enumerate(self.sigma_points_list): - delta_x = S @ self.delta[:, index] - state.fill_dynamic_states(current_state.as_vector(), delta_x) - - return self.sigma_points_list - - def unscented_transform(self, current_state: StateQuat, control_force: np.ndarray) -> StateQuat: - """The unscented transform function generates the priori state estimate.""" - self.sigma_points(current_state) - n = len(current_state.covariance) - - self.y_i = [StateQuat() for _ in range(2 * n)] - - for i, sp in enumerate(self.sigma_points_list): - self.y_i[i] = F_dynamics(sp, self.dt, control_force) - - state_estimate = StateQuat() - x = mean_set(self.y_i) - - state_estimate.fill_states(x) - state_estimate.covariance = covariance_set(self.y_i, x) - return state_estimate - - def measurement_update( - self, current_state: StateQuat, measurement: MeasModel - ) -> None: - """Function that updates the state estimate with a measurement. - - Hopefully this is the DVL or GNSS - """ - n = len(current_state.covariance) - z_i = [MeasModel() for _ in range(2 * n)] - - for i, state in enumerate(self.sigma_points_list): - z_i[i] = measurement.H(state) - - self.measurement_updated.measurement = mean_measurement(z_i) - - self.measurement_updated.covariance = covariance_measurement( - z_i, self.measurement_updated.measurement - ) - - self.cross_correlation = cross_covariance( - self.y_i, - current_state.as_vector(), - z_i, - self.measurement_updated.measurement, - ) - - def posteriori_estimate( - self, - current_state: StateQuat, - measurement: MeasModel, - ) -> StateQuat: - """Calculates the posteriori estimate using measurement and the prior estimate.""" - nu_k = MeasModel() - nu_k.measurement = ( - measurement.measurement - self.measurement_updated.measurement - ) - nu_k.covariance = self.measurement_updated.covariance + measurement.covariance - - K_k = np.dot(self.cross_correlation, np.linalg.inv(nu_k.covariance)) - - posteriori_estimate = StateQuat() - - posteriori_estimate.fill_states_different_dim( - current_state.as_vector(), np.dot(K_k, nu_k.measurement) - ) - posteriori_estimate.covariance = current_state.covariance - np.dot( - K_k, np.dot(nu_k.covariance, np.transpose(K_k)) - ) - - return posteriori_estimate diff --git a/navigation/ukf_okid/ukf_python/ukf_okid_class.py b/navigation/ukf_okid/ukf_python/ukf_okid_class.py deleted file mode 100644 index f494e3280..000000000 --- a/navigation/ukf_okid/ukf_python/ukf_okid_class.py +++ /dev/null @@ -1,880 +0,0 @@ -from dataclasses import dataclass, field -from typing import Callable -import numpy as np - - -@dataclass -class okid: - """A class to represent the parameters for the OKID algorithm.""" - - inertia: np.ndarray = field( - default_factory=lambda: np.array( - [0.68, 0.0, 0.0, 0.0, 3.32, 0.0, 0.0, 0.0, 3.34] - ) - ) - added_mass: np.ndarray = field( - default_factory=lambda: np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) - ) - damping_linear: np.ndarray = field( - default_factory=lambda: np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) - ) - g_eta: np.ndarray = field( - default_factory=lambda: np.array([0.0, 0.0, 0.0, 0.0]) - ) - - def fill(self, state: np.ndarray) -> None: - """Fills the okid_params object with values from a numpy array.""" - self.inertia = state[0:9] - self.added_mass = state[9:15] - self.damping_linear = state[15:21] - self.g_eta = state[21:25] - - def as_vector(self) -> np.ndarray: - """Returns the okid_params as a numpy array.""" - return np.concatenate([self.inertia, self.added_mass, self.damping_linear, self.g_eta]) - - def __add__(self, other: 'okid') -> 'okid': - """Defines the addition operation between two okid_params objects.""" - result = okid() - result.inertia = self.inertia + other.inertia - result.added_mass = self.added_mass + other.added_mass - result.damping_linear = self.damping_linear + other.damping_linear - result.g_eta = self.g_eta + other.g_eta - return result - - def __sub__(self, other: 'okid') -> 'okid': - """Defines the subtraction operation between two okid_params objects.""" - result = okid() - result.inertia = self.inertia - other.inertia - result.added_mass = self.added_mass - other.added_mass - result.damping_linear = self.damping_linear - other.damping_linear - result.g_eta = self.g_eta - other.g_eta - return result - - def __sub__(self, other: np.ndarray) -> 'okid': - """Defines sub between okid_params and np.ndarray.""" - result = okid() - result.inertia = self.inertia - other[0:9] - result.added_mass = self.added_mass - other[9:15] - result.damping_linear = self.damping_linear - other[15:21] - result.g_eta = self.g_eta - other[21:25] - return result - - def __rmul__(self, scalar: float) -> 'okid': - """Defines the multiplication operation between a scalar and okid_params object.""" - result = okid() - result.inertia = scalar * self.inertia - result.added_mass = scalar * self.added_mass - result.damping_linear = scalar * self.damping_linear - result.g_eta = scalar * self.g_eta - return result - - -@dataclass -class StateQuat: - """A class to represent the state to be estimated by the UKF.""" - - position: np.ndarray = field(default_factory=lambda: np.zeros(3)) - orientation: np.ndarray = field(default_factory=lambda: np.array([1, 0, 0, 0])) - velocity: np.ndarray = field(default_factory=lambda: np.zeros(3)) - angular_velocity: np.ndarray = field(default_factory=lambda: np.zeros(3)) - okid_params: okid = field(default_factory=okid) - covariance: np.ndarray = field(default_factory=lambda: np.zeros((37, 37))) - - def as_vector(self) -> np.ndarray: - """Returns the StateVector as a numpy array.""" - return np.concatenate( - [ - self.position, - self.orientation, - self.velocity, - self.angular_velocity, - self.okid_params.as_vector(), - ] - ) - - def dynamic_part(self) -> np.ndarray: - """Returns the dynamic part of the state vector.""" - return np.concatenate( - [self.position, self.orientation, self.velocity, self.angular_velocity] - ) - - def nu(self) -> np.ndarray: - """Calculates the nu vector.""" - return np.concatenate([self.velocity, self.angular_velocity]) - - def R_q(self) -> np.ndarray: - """Calculates the rotation matrix from the orientation quaternion.""" - q0, q1, q2, q3 = self.orientation - R = np.array( - [ - [ - 1 - 2 * q2**2 - 2 * q3**2, - 2 * (q1 * q2 - q0 * q3), - 2 * (q0 * q2 + q1 * q3), - ], - [ - 2 * (q1 * q2 + q0 * q3), - 1 - 2 * q1**2 - 2 * q3**2, - 2 * (q2 * q3 - q0 * q1), - ], - [ - 2 * (q1 * q3 - q0 * q2), - 2 * (q0 * q1 + q2 * q3), - 1 - 2 * q1**2 - 2 * q2**2, - ], - ] - ) - return R - - def fill_states(self, state: np.ndarray) -> None: - """Fills the state vector with the values from a numpy array.""" - self.position = state[0:3] - self.orientation = state[3:7] - self.velocity = state[7:10] - self.angular_velocity = state[10:13] - self.okid_params.fill(state[13:]) - - def fill_dynamic_states(self, state: np.ndarray, state_euler: np.ndarray) -> None: - """Fills only the dynamic part of the state vector with the values from a numpy array.""" - self.position = state[0:3] + state_euler[0:3] - self.orientation = quaternion_super_product( - state[3:7], euler_to_quat(state_euler[3:6]) - ) - self.velocity = state[7:10] + state_euler[6:9] - self.angular_velocity = state[10:13] + state_euler[9:12] - self.okid_params.fill(state[13:] + state_euler[12:]) - - def fill_states_different_dim( - self, state: np.ndarray, state_euler: np.ndarray - ) -> None: - """Fills states when the state vector has different dimensions than the default state vector.""" - self.position = state[0:3] + state_euler[0:3] - self.orientation = quaternion_super_product( - state[3:7], euler_to_quat(state_euler[3:6]) - ) - self.velocity = state[7:10] + state_euler[6:9] - self.angular_velocity = state[10:13] + state_euler[9:12] - self.okid_params.fill(state[13:] + state_euler[12:]) - - def subtract(self, other: 'StateQuat', error_ori: 'np.ndarray') -> np.ndarray: - """Subtracts two StateQuat objects, returning the difference with Euler angles.""" - new_array = np.zeros(len(self.as_vector()) - 1) - new_array[:3] = self.position - other.position - new_array[3:6] = error_ori - new_array[6:9] = self.velocity - other.velocity - new_array[9:12] = self.angular_velocity - other.angular_velocity - new_array[12:] = self.okid_params.as_vector() - other.okid_params.as_vector() - - return new_array - - def __add__(self, other: 'StateQuat') -> 'StateQuat': - """Adds two StateQuat objects.""" - new_state = StateQuat() - new_state.position = self.position + other.position - new_state.orientation = quaternion_super_product( - self.orientation, other.orientation - ) - new_state.velocity = self.velocity + other.velocity - new_state.angular_velocity = self.angular_velocity + other.angular_velocity - new_state.okid_params = self.okid_params + other.okid_params - - return new_state - - def __sub__(self, other: 'StateQuat') -> 'StateQuat': - """Subtracts two StateQuat objects.""" - new_state = StateQuat() - new_state.position = self.position - other.position - new_state.orientation = quaternion_error(self.orientation, other.orientation) - new_state.velocity = self.velocity - other.velocity - new_state.angular_velocity = self.angular_velocity - other.angular_velocity - new_state.okid_params = self.okid_params - other.okid_params - - return new_state.as_vector() - - def __rmul__(self, scalar: float) -> 'StateQuat': - """Multiplies the StateQuat object by a scalar.""" - new_state = StateQuat() - new_state.position = scalar * self.position - new_state.orientation = quat_norm(scalar * self.orientation) - new_state.velocity = scalar * self.velocity - new_state.angular_velocity = scalar * self.angular_velocity - new_state.okid_params = scalar * self.okid_params - - return new_state - - def insert_weights(self, weights: np.ndarray) -> np.ndarray: - """Inserts the weights into the covariance matrix.""" - new_state = StateQuat() - new_state.position = self.position - weights[:3] - new_state.orientation = quaternion_error( - self.orientation, euler_to_quat(weights[3:6]) - ) - new_state.velocity = self.velocity - weights[6:9] - new_state.angular_velocity = self.angular_velocity - weights[9:12] - new_state.okid_params = self.okid_params - weights[12:] - - return new_state.as_vector() - - def add_without_quaternions(self, other: 'StateQuat') -> None: - """Adds elements into the state vector without considering the quaternions.""" - self.position += other.position - self.velocity += other.velocity - self.angular_velocity += other.angular_velocity - self.okid_params += other.okid_params - - -@dataclass -class MeasModel: - """A class defined for a general measurement model.""" - measurement: np.ndarray = field(default_factory=lambda: np.zeros(3)) - covariance: np.ndarray = field(default_factory=lambda: np.zeros((3, 3))) - H: Callable[["StateQuat"], "MeasModel"] | None = None - - def __post_init__(self): - """Initialize H with a default measurement function if none provided.""" - if self.H is None: - self.H = self._default_H - - def _default_H(self, state: StateQuat) -> 'MeasModel': - """Default measurement function that returns velocity.""" - H_matrix = np.zeros((3, 13)) - H_matrix[:, 7:10] = np.eye(3) - z_i = MeasModel() - z_i.measurement = np.dot(H_matrix, state.dynamic_part()) - return z_i - - def __add__(self, other: 'MeasModel') -> 'MeasModel': - """Defines the addition operation between two MeasModel objects.""" - result = MeasModel() - result.measurement = self.measurement + other.measurement - return result - - def __rmul__(self, scalar: float) -> 'MeasModel': - """Defines multiplication between scalar value and MeasModel object.""" - result = MeasModel() - result.measurement = scalar * self.measurement - return result - - def __sub__(self, other: 'MeasModel') -> 'MeasModel': - """Defines the subtraction between two MeasModel objects.""" - result = MeasModel() - result.measurement = self.measurement - other.measurement - return result - - -@dataclass -class process_model: - """A class defined for a general process model.""" - - state_vector: StateQuat = field(default_factory=StateQuat) - state_vector_dot: StateQuat = field(default_factory=StateQuat) - state_vector_prev: StateQuat = field(default_factory=StateQuat) - Control_input: np.ndarray = field(default_factory=lambda: np.zeros(6)) - mass_interia_matrix: np.ndarray = field(default_factory=lambda: np.zeros((6, 6))) - added_mass: np.ndarray = field(default_factory=lambda: np.zeros(6)) - damping_linear: np.ndarray = field(default_factory=lambda: np.zeros(6)) - damping_nonlinear: np.ndarray = field(default_factory=lambda: np.zeros(6)) - m: float = 0.0 - inertia: np.ndarray = field(default_factory=lambda: np.zeros((3, 3))) - r_b_bg: np.ndarray = field(default_factory=lambda: np.zeros(3)) - dt: float = 0.0 - integral_error_position: np.ndarray = field(default_factory=lambda: np.zeros(3)) - integral_error_orientation: np.ndarray = field(default_factory=lambda: np.zeros(4)) - prev_position_error: np.ndarray = field(default_factory=lambda: np.zeros(3)) - prev_orientation_error: np.ndarray = field(default_factory=lambda: np.zeros(3)) - - def R(self) -> np.ndarray: - """Calculates the rotation matrix.""" - nu, e_1, e_2, e_3 = self.state_vector.orientation - R = np.array( - [ - [ - 1 - 2 * e_2**2 - 2 * e_3**2, - 2 * e_1 * e_2 - 2 * nu * e_3, - 2 * e_1 * e_3 + 2 * nu * e_2, - ], - [ - 2 * e_1 * e_2 + 2 * nu * e_3, - 1 - 2 * e_1**2 - 2 * e_3**2, - 2 * e_2 * e_3 - 2 * nu * e_1, - ], - [ - 2 * e_1 * e_3 - 2 * nu * e_2, - 2 * e_2 * e_3 + 2 * nu * e_1, - 1 - 2 * e_1**2 - 2 * e_2**2, - ], - ] - ) - return R - - def T(self) -> np.ndarray: - """Calculates the transformation matrix.""" - nu, e_1, e_2, e_3 = self.state_vector.orientation - T = 0.5 * np.array( - [[-e_1, -e_2, -e_3], [nu, -e_3, e_2], [e_3, nu, -e_1], [-e_2, e_1, nu]] - ) - return T - - def Crb(self) -> np.ndarray: - """Calculates the Coriolis matrix.""" - ang_vel = self.state_vector.angular_velocity - ang_vel_skew = skew_symmetric(ang_vel) - lever_arm_skew = skew_symmetric(self.r_b_bg) - Crb = np.zeros((6, 6)) - Crb[0:3, 0:3] = self.m * ang_vel_skew - Crb[3:6, 3:6] = -skew_symmetric(np.dot(self.inertia, ang_vel)) - Crb[0:3, 3:6] = -self.m * np.dot(ang_vel_skew, lever_arm_skew) - Crb[3:6, 0:3] = self.m * np.dot(lever_arm_skew, ang_vel_skew) - return Crb - - def D(self) -> np.ndarray: - """Calculates the damping matrix.""" - D_l = -np.diag(self.damping_linear) - D_nl = -np.diag(self.damping_nonlinear) * np.abs(self.state_vector.nu()) - return D_l + D_nl - - def model_prediction(self, state: StateQuat) -> None: - """Calculates the model of the system.""" - self.state_vector = state - self.state_vector_dot.position = np.dot(self.R(), self.state_vector.velocity) - self.state_vector_dot.orientation = np.dot( - self.T(), self.state_vector.angular_velocity - ) - Nu = np.linalg.inv(self.mass_interia_matrix + np.diag(self.added_mass)) @ ( - self.Control_input - - np.dot(self.Crb(), self.state_vector.nu()) - - np.dot(self.D(), self.state_vector.nu()) - ) - self.state_vector_dot.velocity = Nu[:3] - self.state_vector_dot.angular_velocity = Nu[3:] - - def euler_forward(self) -> StateQuat: - """Calculates the forward Euler integration.""" - self.state_vector.position = ( - self.state_vector_prev.position + self.state_vector_dot.position * self.dt - ) - self.state_vector.orientation = quat_norm( - self.state_vector_prev.orientation - + self.state_vector_dot.orientation * self.dt - ) - self.state_vector.velocity = ( - self.state_vector_prev.velocity + self.state_vector_dot.velocity * self.dt - ) - self.state_vector.angular_velocity = ( - self.state_vector_prev.angular_velocity - + self.state_vector_dot.angular_velocity * self.dt - ) - return self.state_vector - - -@dataclass -class okid_process_model: - state_vector: StateQuat = field(default_factory=StateQuat) - state_vector_dot: StateQuat = field(default_factory=StateQuat) - state_vector_prev: StateQuat = field(default_factory=StateQuat) - Control_input: np.ndarray = field(default_factory=lambda: np.zeros(6)) - mass_interia_matrix: np.ndarray = field(default_factory=lambda: np.zeros((6, 6))) - added_mass: np.ndarray = field(default_factory=lambda: np.zeros(6)) - damping_linear: np.ndarray = field(default_factory=lambda: np.zeros(6)) - m: float = 30.0 - inertia: np.ndarray = field(default_factory=lambda: np.zeros((3, 3))) - r_b_bg: np.ndarray = field(default_factory=lambda: np.zeros(3)) - dt: float = 0.01 - - def R(self) -> np.ndarray: - """Calculates the rotation matrix.""" - nu, e_1, e_2, e_3 = self.state_vector.orientation - R = np.array( - [ - [ - 1 - 2 * e_2**2 - 2 * e_3**2, - 2 * e_1 * e_2 - 2 * nu * e_3, - 2 * e_1 * e_3 + 2 * nu * e_2, - ], - [ - 2 * e_1 * e_2 + 2 * nu * e_3, - 1 - 2 * e_1**2 - 2 * e_3**2, - 2 * e_2 * e_3 - 2 * nu * e_1, - ], - [ - 2 * e_1 * e_3 - 2 * nu * e_2, - 2 * e_2 * e_3 + 2 * nu * e_1, - 1 - 2 * e_1**2 - 2 * e_2**2, - ], - ] - ) - return R - - def T(self) -> np.ndarray: - """Calculates the transformation matrix.""" - nu, e_1, e_2, e_3 = self.state_vector.orientation - T = 0.5 * np.array( - [[-e_1, -e_2, -e_3], [nu, -e_3, e_2], [e_3, nu, -e_1], [-e_2, e_1, nu]] - ) - return T - - def Crb(self) -> np.ndarray: - """Calculates the Coriolis matrix.""" - ang_vel = self.state_vector.angular_velocity - ang_vel_skew = skew_symmetric(ang_vel) - lever_arm_skew = skew_symmetric(self.r_b_bg) - Crb = np.zeros((6, 6)) - Crb[0:3, 0:3] = self.m * ang_vel_skew - Crb[3:6, 3:6] = -skew_symmetric(np.dot(self.inertia, ang_vel)) - Crb[0:3, 3:6] = -self.m * np.dot(ang_vel_skew, lever_arm_skew) - Crb[3:6, 0:3] = self.m * np.dot(lever_arm_skew, ang_vel_skew) - return Crb - - def D(self) -> np.ndarray: - """Calculates the damping matrix.""" - D_l = -np.diag(self.damping_linear) - - return D_l - - def model_prediction(self, state: StateQuat) -> None: - """Calculates the model of the system.""" - self.state_vector = state - """ - separate out the different okid values - """ - self.inertia = state.okid_params.inertia.reshape((3, 3)) - self.added_mass = state.okid_params.added_mass - self.damping_linear = state.okid_params.damping_linear - - self.state_vector_dot.position = np.dot(self.R(), self.state_vector.velocity) - self.state_vector_dot.orientation = np.dot( - self.T(), self.state_vector.angular_velocity - ) - Nu = np.linalg.inv(self.mass_interia_matrix + np.diag(self.added_mass)) @ ( - self.Control_input - - np.dot(self.Crb(), self.state_vector.nu()) - - np.dot(self.D(), self.state_vector.nu()) - ) - self.state_vector_dot.velocity = Nu[:3] - self.state_vector_dot.angular_velocity = Nu[3:] - - def euler_forward(self) -> None: - """Calculates the forward Euler integration.""" - self.state_vector.position = ( - self.state_vector_prev.position + self.state_vector_dot.position * self.dt - ) - self.state_vector.orientation = quat_norm( - self.state_vector_prev.orientation - + self.state_vector_dot.orientation * self.dt - ) - self.state_vector.velocity = ( - self.state_vector_prev.velocity + self.state_vector_dot.velocity * self.dt - ) - self.state_vector.angular_velocity = ( - self.state_vector_prev.angular_velocity - + self.state_vector_dot.angular_velocity * self.dt - ) - return self.state_vector - - -# ----------------------------------------------------------- - -def R_q(orientation: np.ndarray) -> np.ndarray: - """Calculates the rotation matrix from the orientation quaternion.""" - q0, q1, q2, q3 = orientation - R = np.array( - [ - [1 - 2 * q2**2 - 2 * q3**2, 2 * (q1 * q2 - q0 * q3), 2 * (q0 * q2 + q1 * q3)], - [2 * (q1 * q2 + q0 * q3), 1 - 2 * q1**2 - 2 * q3**2, 2 * (q2 * q3 - q0 * q1)], - [2 * (q1 * q3 - q0 * q2), 2 * (q0 * q1 + q2 * q3), 1 - 2 * q1**2 - 2 * q2**2], - ] - ) - return R - -def T_q(orientation: np.ndarray) -> np.ndarray: - """Calculates the transformation matrix from the orientation quaternion.""" - q0, q1, q2, q3 = orientation - T = 0.5 * np.array( - [[-q1, -q2, -q3], [q0, -q3, q2], [q3, q0, -q1], [-q2, q1, q0]] - ) - return T - -def M_rb(inertia: np.ndarray) -> np.ndarray: - m = 30.0 - inertia = inertia.reshape((3, 3)) - r_b_bg = np.array([0.01, 0.0, 0.02]) - M_rb = np.zeros((6, 6)) - M_rb[0:3, 0:3] = m * np.eye(3) - M_rb[3:6, 3:6] = inertia - M_rb[0:3, 3:6] = -m * skew_symmetric(r_b_bg) - M_rb[3:6, 0:3] = m * skew_symmetric(r_b_bg) - return M_rb - -def M_a(added_mass: np.ndarray) -> np.ndarray: - """Calculates the added mass matrix.""" - M_a = np.zeros((6, 6)) - M_a[0:3, 0:3] = np.diag(added_mass[0:3]) - M_a[3:6, 3:6] = np.diag(added_mass[3:6]) - return M_a - -def C_rb(inertia: np.ndarray, angular_velocity: np.ndarray) -> np.ndarray: - """Calculates the Coriolis matrix.""" - m = 30.0 - r_b_bg = np.array([0.01, 0.0, 0.02]) - inertia = inertia.reshape((3, 3)) - C_rb = np.zeros((6, 6)) - - C_rb[0:3, 0:3] = m * skew_symmetric(angular_velocity) - C_rb[3:6, 3:6] = -skew_symmetric(np.dot(inertia, angular_velocity)) - C_rb[0:3, 3:6] = -m * skew_symmetric(angular_velocity) @ skew_symmetric(r_b_bg) - C_rb[3:6, 0:3] = m * skew_symmetric(r_b_bg) @ skew_symmetric(angular_velocity) - return C_rb - -def C_a(added_mass: np.ndarray, angular_velocity: np.ndarray, velocity: np.ndarray) -> np.ndarray: - """Calculates the added mass Coriolis matrix.""" - C_a = np.zeros((6, 6)) - A11 = np.diag(added_mass[0:3]) - A22 = np.diag(added_mass[3:6]) - C_a[3:6,3:6] = - skew_symmetric(A22 @ angular_velocity) - C_a[0:3,3:6] = - skew_symmetric(A11 @ velocity) - C_a[3:6,0:3] = - skew_symmetric(A11 @ velocity) - return C_a - -def D_linear(damping_linear: np.ndarray) -> np.ndarray: - """Calculates the linear damping matrix.""" - D = np.zeros((6, 6)) - D[0:3, 0:3] = -np.diag(damping_linear[0:3]) - D[3:6, 3:6] = -np.diag(damping_linear[3:6]) - return D - -def g_eta(g_eta: np.ndarray, orientation: np.ndarray) -> np.ndarray: - """Calculates the g_eta matrix.""" - Delta_WB = g_eta[0] - M_x = g_eta[1] - M_y = g_eta[2] - M_z = g_eta[3] - - q_0 = orientation[0] - q_1 = orientation[1] - q_2 = orientation[2] - q_3 = orientation[3] - - R1 = (2*(q_1*q_3 - q_0*q_2)) - R2 = (2*(q_2*q_3 + q_0*q_1)) - R3 = (1 - 2*(q_1**2 + q_2**2)) - G_eta = np.zeros((6,1)) - G_eta[0] = - Delta_WB * R1 - G_eta[1] = - Delta_WB * R2 - G_eta[2] = - Delta_WB * R3 - - G_eta[3] = -M_y * R3 + M_z * R2 - G_eta[4] = -M_z * R1 + M_x * R3 - G_eta[5] = -M_x * R2 + M_y * R1 - - return G_eta - -def F_dynamics( - state: StateQuat, - dt: float, - control_input: np.ndarray) -> np.ndarray: - - """Calculates the dynamics of the system.""" - m_rb = M_rb(state.okid_params.inertia) - m_a = M_a(state.okid_params.added_mass) - c_rb = C_rb(state.okid_params.inertia, state.angular_velocity) - c_a = C_a(state.okid_params.added_mass, state.angular_velocity, state.velocity) - D_l = D_linear(state.okid_params.damping_linear) - g_eta_ = g_eta(state.okid_params.g_eta, state.orientation) - r_q = R_q(state.orientation) - t_q = T_q(state.orientation) - Crb = c_rb + c_a - Mrb = m_rb + m_a - M_inv = np.linalg.inv(Mrb) - - - # Calculate the new state - state_dot = StateQuat() - state_dot.position = np.dot(r_q, state.velocity) - state_dot.orientation = np.dot(t_q, state.angular_velocity) - Nu = M_inv @ (control_input - np.dot(Crb, state.nu()) - np.dot(D_l, state.nu()) - g_eta_.flatten()) - - state_dot.velocity = Nu[:3] - state_dot.angular_velocity = Nu[3:6] - state_dot.okid_params.added_mass = state.okid_params.added_mass - state_dot.okid_params.damping_linear = state.okid_params.damping_linear - state_dot.okid_params.inertia = state.okid_params.inertia - state_dot.okid_params.g_eta = state.okid_params.g_eta - - # Update the state using Euler integration - state.position += state_dot.position * dt - state.orientation = quat_norm( - state.orientation + state_dot.orientation * dt - ) - state.velocity += state_dot.velocity * dt - state.angular_velocity += state_dot.angular_velocity * dt - state.okid_params.added_mass = state.okid_params.added_mass - state.okid_params.damping_linear = state.okid_params.damping_linear - state.okid_params.inertia = state.okid_params.inertia - state.okid_params.g_eta = state.okid_params.g_eta - return state -# ----------------------------------------------------------- - - -def euler_to_quat(euler_angles: np.ndarray) -> np.ndarray: - """Converts Euler angles to a quaternion.""" - psi, theta, phi = euler_angles - c_psi = np.cos(psi / 2) - s_psi = np.sin(psi / 2) - c_theta = np.cos(theta / 2) - s_theta = np.sin(theta / 2) - c_phi = np.cos(phi / 2) - s_phi = np.sin(phi / 2) - - quat = np.array( - [ - c_psi * c_theta * c_phi + s_psi * s_theta * s_phi, - c_psi * c_theta * s_phi - s_psi * s_theta * c_phi, - s_psi * c_theta * s_phi + c_psi * s_theta * c_phi, - s_psi * c_theta * c_phi - c_psi * s_theta * s_phi, - ] - ) - - return quat - - -def quat_to_euler(quat: np.ndarray) -> np.ndarray: - """Converts a quaternion to Euler angles.""" - nu, eta_1, eta_2, eta_3 = quat - - phi = np.arctan2(2 * (eta_2 * eta_3 + nu * eta_1), 1 - 2 * (eta_1**2 + eta_2**2)) - theta = -np.arcsin(2 * (eta_1 * eta_3 - nu * eta_2)) - psi = np.arctan2(2 * (nu * eta_3 + eta_1 * eta_2), 1 - 2 * (eta_2**2 + eta_3**2)) - - return np.array([phi, theta, psi]) - - -def quat_norm(quat: np.ndarray) -> np.ndarray: - """Function that normalizes a quaternion.""" - quat = quat / np.linalg.norm(quat) - - return quat - - -def skew_symmetric(vector: np.ndarray) -> np.ndarray: - """Calculates the skew symmetric matrix of a vector. - - Args: - vector (np.ndarray): The vector. - - Returns: - np.ndarray: The skew symmetric matrix. - """ - return np.array( - [ - [0, -vector[2], vector[1]], - [vector[2], 0, -vector[0]], - [-vector[1], vector[0], 0], - ] - ) - - -def quaternion_super_product(q1: np.ndarray, q2: np.ndarray) -> np.ndarray: - """Calculates the quaternion super product of two quaternions. - - Args: - q1 (np.ndarray): The first quaternion. - q2 (np.ndarray): The second quaternion. - - Returns: - np.ndarray: The quaternion super product. - """ - eta_0, e_0_x, e_0_y, e_0_z = q1 - eta_1, e_1_x, e_1_y, e_1_z = q2 - - e_0 = np.array([e_0_x, e_0_y, e_0_z]) - e_1 = np.array([e_1_x, e_1_y, e_1_z]) - - eta_new = eta_0 * eta_1 - (e_0_x * e_1_x + e_0_y * e_1_y + e_0_z * e_1_z) - nu_new = e_1 * eta_0 + e_0 * eta_1 + np.dot(skew_symmetric(e_0), e_1) - - q_new = quat_norm(np.array([eta_new, nu_new[0], nu_new[1], nu_new[2]])) - - return q_new - - -def quaternion_error(quat_1: np.ndarray, quat_2: np.ndarray) -> np.ndarray: - """Calculates the error between two quaternions.""" - quat_2_inv = np.array([quat_2[0], -quat_2[1], -quat_2[2], -quat_2[3]]) - - error_quat = quaternion_super_product(quat_1, quat_2_inv) - - return error_quat - - -def iterative_quaternion_mean_statequat( - state_list: list[StateQuat], tol: float = 1e-6, max_iter: int = 100 -) -> np.ndarray: - """Computes the iterative mean of quaternion orientations from StateQuat objects. - - Args: - state_list: List of StateQuat objects - tol: Convergence tolerance - max_iter: Maximum iterations - - Returns: - Mean quaternion as numpy array - """ - sigma_quats = [state.orientation for state in state_list] - n = len(state_list) - - mean_q = sigma_quats[0].copy() - - for _ in range(max_iter): - weighted_error_vectors = [] - for i, q in enumerate(sigma_quats): - mean_q_conj = np.array([mean_q[0], -mean_q[1], -mean_q[2], -mean_q[3]]) - e = quaternion_super_product(q, mean_q_conj) - - e0_clipped = np.clip(e[0], -1.0, 1.0) - angle = 2 * np.arccos(e0_clipped) - if np.abs(angle) < 1e-8: - error_vec = np.zeros(3) - else: - error_vec = (angle / np.sin(angle / 2)) * e[1:4] - weighted_error_vectors.append(error_vec) - - error_avg = (1 / n) * np.sum(weighted_error_vectors, axis=0) - if np.linalg.norm(error_avg) < tol: - break - - error_norm = np.linalg.norm(error_avg) - if error_norm > 0: - delta_q = np.array( - [ - np.cos(error_norm / 2), - *(np.sin(error_norm / 2) * (error_avg / error_norm)), - ] - ) - else: - delta_q = np.array([1.0, 0.0, 0.0, 0.0]) - - mean_q = quaternion_super_product(delta_q, mean_q) - mean_q = quat_norm(mean_q) - - return mean_q - - -def mean_set(set_points: list[StateQuat]) -> np.ndarray: - """Function calculates the mean vector of a set of points. - - Args: - set_points (list[StateQuat]): List of StateQuat objects - - Returns: - np.ndarray: The mean vector - """ - n = len(set_points) - mean_value = StateQuat() - - for state in set_points: - mean_value.add_without_quaternions(state) - - mean_value.position = (1 / n) * mean_value.position - mean_value.velocity = (1 / n) * mean_value.velocity - mean_value.angular_velocity = (1 / n) * mean_value.angular_velocity - mean_value.okid_params = (1 / n) * mean_value.okid_params - - mean_value.orientation = iterative_quaternion_mean_statequat(set_points) - return mean_value.as_vector() - - -def mean_measurement(set_points: list[MeasModel]) -> np.ndarray: - """Function that calculates the mean of a set of points.""" - n = len(set_points) - mean_value = MeasModel() - - for state in set_points: - mean_value = mean_value + state - - mean_value = (1 / n) * mean_value - - return mean_value.measurement - - -def covariance_set(set_points: list[StateQuat], mean: np.ndarray) -> np.ndarray: - """Function that calculates the covariance of a set of points.""" - n = len(set_points) - covariance = np.zeros(set_points[0].covariance.shape) - - mean_quat = StateQuat() - mean_quat.fill_states(mean) - - mean_q = mean_quat.orientation - - for state in set_points: - q = state.orientation - diff_q = quaternion_error(q, mean_q) - - e0_clipped = np.clip(diff_q[0], -1.0, 1.0) - angle = 2.0 * np.arccos(e0_clipped) - if abs(angle) < 1e-8: - e_vec = np.zeros(3) - else: - e_vec = (angle / np.sin(angle / 2)) * diff_q[1:4] - - covariance += np.outer( - state.subtract(mean_quat, e_vec), state.subtract(mean_quat, e_vec) - ) - - covariance = (1 / (n)) * covariance - - return covariance - - -def covariance_measurement(set_points: list[MeasModel], mean: np.ndarray) -> np.ndarray: - """Function that calculates the covariance of a set of points.""" - n = len(set_points) - co_size = len(set_points[0].measurement) - covariance = np.zeros((co_size, co_size)) - - mean_meas = MeasModel() - mean_meas.measurement = mean - - for state in set_points: - temp_state = state - mean_meas - covariance += np.outer(temp_state.measurement, temp_state.measurement) - - covariance = (1 / n) * covariance - - return covariance - - -def cross_covariance( - set_y: list[StateQuat], - mean_y: np.ndarray, - set_z: list[MeasModel], - mean_z: np.ndarray, -) -> np.ndarray: - """Calculates the cross covariance between the measurement and state prediction.""" - n = len(set_y) - - cross_covariance = np.zeros((len(mean_y) - 1, len(mean_z))) - mean_quat = StateQuat() - mean_quat.fill_states(mean_y) - - mean_q = mean_quat.orientation - - for i in range(n): - q = set_y[i].orientation - diff_q = quaternion_error(q, mean_q) - - e0_clipped = np.clip(diff_q[0], -1.0, 1.0) - angle = 2.0 * np.arccos(e0_clipped) - if abs(angle) < 1e-8: - e_vec = np.zeros(3) - else: - e_vec = (angle / np.sin(angle / 2)) * diff_q[1:4] - - cross_covariance += np.outer( - set_y[i].subtract(mean_quat, e_vec), set_z[i].measurement - mean_z - ) - - cross_covariance = (1 / n) * cross_covariance - - return cross_covariance diff --git a/navigation/ukf_okid/ukf_python/ukf_ros.py b/navigation/ukf_okid/ukf_python/ukf_ros.py deleted file mode 100755 index a40c2f7c3..000000000 --- a/navigation/ukf_okid/ukf_python/ukf_ros.py +++ /dev/null @@ -1,159 +0,0 @@ -#!/usr/bin/env python3 -import numpy as np -import rclpy -from geometry_msgs.msg import TwistWithCovarianceStamped, WrenchStamped -from nav_msgs.msg import Odometry -from rclpy.node import Node -from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy -from ukf_okid import UKF -from ukf_okid_class import MeasModel, StateQuat, process_model - - -class UKFNode(Node): - def __init__(self): - super().__init__("UKFNode") - - best_effort_qos = QoSProfile( - reliability=ReliabilityPolicy.BEST_EFFORT, - history=HistoryPolicy.KEEP_LAST, - depth=1, - ) - - # subscribers - self.dvl_subscriber = self.create_subscription( - TwistWithCovarianceStamped, - "/orca/twist", - self.dvl_callback, - qos_profile=best_effort_qos, - ) - - self.control_input = self.create_subscription( - WrenchStamped, - "/orca/wrench_input", - self.control_callback, - qos_profile=best_effort_qos, - ) - - self.odom_publish = self.create_publisher( - Odometry, "/orca/odometry", qos_profile=best_effort_qos - ) - dt = self.declare_parameter("dt", 0.01).get_parameter_value().double_value - self.control_timer = self.create_timer(dt, self.odom_publisher) - - self.current_state = StateQuat() - x0 = np.zeros(13) - x0[3] = 1.0 # quaternion: [1, 0, 0, 0] - P0 = np.eye(12) * 0.5 - self.ukf_model = process_model() - self.ukf_model.dt = 0.01 - self.ukf_model.mass_interia_matrix = np.array( - [ - [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], - ] - ) - self.ukf_model.m = 30.0 - self.ukf_model.r_b_bg = np.array([0.01, 0.0, 0.02]) - self.ukf_model.inertia = np.diag([0.68, 3.32, 3.34]) - self.ukf_model.damping_linear = np.array([0.01] * 6) - # self.ukf_model.added_mass = np.diag([1.0, 1.0, 1.0, 2.0, 2.0, 2.0]) - - Q = np.diag([0.02, 0.02, 0.02, 0.02, 0.02, 0.02, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) - - self.ukf = UKF(self.ukf_model, x0, P0, Q) - self.ukf_flagg = False - - def dvl_callback(self, msg: TwistWithCovarianceStamped): - # unpack msg - dvl_measurement = MeasModel() - # Print received DVL data to console - # self.get_logger().info(f"DVL data received: x={msg.twist.twist.linear.x}, y={msg.twist.twist.linear.y}, z={msg.twist.twist.linear.z}") - dvl_measurement.measurement = np.array( - [ - msg.twist.twist.linear.x, - msg.twist.twist.linear.y, - msg.twist.twist.linear.z, - ] - ) - dvl_measurement.covariance = np.array( - [ - [ - msg.twist.covariance[0], - msg.twist.covariance[1], - msg.twist.covariance[3], - ], - [ - msg.twist.covariance[6], - msg.twist.covariance[7], - msg.twist.covariance[8], - ], - [ - msg.twist.covariance[12], - msg.twist.covariance[13], - msg.twist.covariance[14], - ], - ] - ) - - self.ukf.measurement_update(self.current_state, dvl_measurement) - self.current_state = self.ukf.posteriori_estimate( - self.current_state, dvl_measurement - ) - self.ukf_flagg = True - - def control_callback(self, msg: WrenchStamped): - # unpack message - control_array = np.array( - [ - msg.wrench.force.x, - msg.wrench.force.y, - msg.wrench.force.z, - msg.wrench.torque.x, - msg.wrench.torque.y, - msg.wrench.torque.z, - ] - ) - self.ukf_model.Control_input = control_array - - def odom_publisher(self): - msg = Odometry() - - if self.ukf_flagg == False: - self.current_state = self.ukf.unscented_transform(self.current_state) - else: - self.ukf_flagg = False - - msg.header.stamp = self.get_clock().now().to_msg() - msg.header.frame_id = "odom" - msg.child_frame_id = "base_link" - - msg.pose.pose.position.x = self.current_state.position[0] - msg.pose.pose.position.y = self.current_state.position[1] - msg.pose.pose.position.z = self.current_state.position[2] - msg.pose.pose.orientation.w = self.current_state.orientation[0] - msg.pose.pose.orientation.x = self.current_state.orientation[1] - msg.pose.pose.orientation.y = self.current_state.orientation[2] - msg.pose.pose.orientation.z = self.current_state.orientation[3] - msg.twist.twist.linear.x = self.current_state.velocity[0] - msg.twist.twist.linear.y = self.current_state.velocity[1] - msg.twist.twist.linear.z = self.current_state.velocity[2] - msg.twist.twist.angular.x = self.current_state.angular_velocity[0] - msg.twist.twist.angular.y = self.current_state.angular_velocity[1] - msg.twist.twist.angular.z = self.current_state.angular_velocity[2] - - self.odom_publish.publish(msg) - - -def main(args=None): - rclpy.init(args=args) - ukf_node = UKFNode() - rclpy.spin(ukf_node) - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/navigation/ukf_okid/ukf_python/ukf_test.py b/navigation/ukf_okid/ukf_python/ukf_test.py deleted file mode 100644 index e664bd848..000000000 --- a/navigation/ukf_okid/ukf_python/ukf_test.py +++ /dev/null @@ -1,27 +0,0 @@ -import numpy as np -def generate_delta_matrix(n: int) -> np.ndarray: - if n < 1: - raise ValueError("n must be a positive integer") - - delta = np.zeros((n, 2 * n)) - r_max = n // 2 # floor(n/2) - sq2 = np.sqrt(2.0) - - for k in range(1, 2 * n + 1): # k = 1 … 2n - for r in range(1, r_max + 1): - row_cos = 2 * r - 2 # 0‑based index for γ_{k,2r‑1} - row_sin = 2 * r - 1 # 0‑based index for γ_{k,2r} - angle = (2 * r - 1) * k * np.pi / n - delta[row_cos, k - 1] = sq2 * np.cos(angle) - delta[row_sin, k - 1] = sq2 * np.sin(angle) - - if n % 2 == 1: # extra entry when n is odd - delta[n - 1, k - 1] = (-1) ** k - - return delta - -a1 = np.array([1, 0, 1]) -a2 = np.array([1, 1, 0]) - -a3 = np.outer(a1, a2) -print(a3) \ No newline at end of file diff --git a/navigation/ukf_okid/ukf_python/ukf_test_2.py b/navigation/ukf_okid/ukf_python/ukf_test_2.py deleted file mode 100644 index 0a2d5b2fe..000000000 --- a/navigation/ukf_okid/ukf_python/ukf_test_2.py +++ /dev/null @@ -1,229 +0,0 @@ -import numpy as np -import ukf_okid_class as ukf -from ukf_okid import UKF -from mpl_toolkits.mplot3d import Axes3D -import time -import math -from ukf_utils import print_StateQuat, print_matrix - -import matplotlib.pyplot as plt - -# Initialize UKF with StateQuat -initial_position = np.array([0.0, 0.0, 0.0]) # x, y, z -initial_velocity = np.array([0.0, 0.0, 0.0]) # vx, vy, vz -initial_quaternion = np.array([1.0, 0.0, 0.0, 0.0]) # w, x, y, z (identity quaternion) -initial_angular_velocity = np.array([0.0, 0.0, 0.0]) # wx, wy, wz -initial_g_eta = np.array([1.2, 0.3, 0.3, 0.3]) # g_eta parameters -initial_covariance = np.eye(37) * 20.0 # Initial covariance matrix - -# Create StateQuat object -state = ukf.StateQuat(initial_position, initial_quaternion, initial_velocity, initial_angular_velocity) -state.okid_params.g_eta = initial_g_eta -state.covariance = initial_covariance - -real_state = ukf.StateQuat(initial_position, initial_quaternion, initial_velocity, initial_angular_velocity) -real_state.okid_params.g_eta = np.array([1.2, 0.3, 0.3, 0.3]) - -UKF_model = UKF(state, Q=10.0 * np.eye(37)) # Process noise covariance -dvl_measurement = ukf.MeasModel() - -def ang_h(state: ukf.StateQuat) -> 'ukf.MeasModel': - H_matrix = np.zeros((3, 13)) - H_matrix[:, 10:13] = np.eye(3) - z_i = ukf.MeasModel() - z_i.measurement = np.dot(H_matrix, state.dynamic_part()) - return z_i - -ang_measurement = ukf.MeasModel(H=ang_h) - -# UKF parameters -dt = 0.01 # time step -sim_time = 1.0 # total simulation time -steps = int(sim_time / dt) - -# Storage for trajectory -positions = np.zeros((steps, 3)) -velocities = np.zeros((steps, 3)) -quaternions = np.zeros((steps, 4)) -angular_velocities = np.zeros((steps, 3)) -okid_params = np.zeros((steps, 4)) - -# Storage for trajectory -positions_est = np.zeros((steps, 3)) -velocities_est = np.zeros((steps, 3)) -quaternions_est = np.zeros((steps, 4)) -angular_velocities_est = np.zeros((steps, 3)) -okid_params_est = np.zeros((steps, 4)) - -# Simulation loop -for i in range(steps): - t = i * dt - - # Generate control input (slow sinusoidal signals) - control_force = np.array([ - 5 * np.sin(4.0 * t), - 10 * np.sin(3.0 * t), - 10 * np.sin(2.0 * t) - ]) - - control_torque = np.array([ - 10 * np.sin(4.0 * t), - 10 * np.sin(4.0 * t), - 10 * np.sin(4.0 * t) - ]) - - control_input = np.concatenate((control_force, control_torque)) - - # Propagate state using UKF prediction - real_state = ukf.F_dynamics(real_state, dt, control_input) - state = UKF_model.unscented_transform(state, control_input) - if i % 5 == 0: - # Simulate measurement update every 5 steps - ang_measurement.measurement = np.array([ - real_state.angular_velocity[0], - real_state.angular_velocity[1], - real_state.angular_velocity[2] - ]) + np.random.normal(0, 0.03, 3) - # Simulate measurement covariance - ang_measurement.covariance = np.eye(3) * 0.04 # Measurement noise covariance - # Update UKF with measurement - UKF_model.measurement_update(state, ang_measurement) - state = UKF_model.posteriori_estimate(state, ang_measurement) - - if i % 10 == 0: - # Simulate measurement update every 10 steps - dvl_measurement.measurement = np.array([ - real_state.velocity[0], - real_state.velocity[1], - real_state.velocity[2] - ]) + np.random.normal(0, 0.03, 3) # Simulated measurement with noise - - # Simulate measurement covariance - dvl_measurement.covariance = np.eye(3) * 0.04 # Measurement noise covariance - - # Update UKF with measurement - UKF_model.measurement_update(state, dvl_measurement) - state = UKF_model.posteriori_estimate(state, dvl_measurement) - print_matrix(state.covariance) - - print("Determinant of covariance matrix:", np.linalg.det(state.covariance)) - # Store state for plotting - positions[i] = real_state.position - velocities[i] = real_state.velocity - quaternions[i] = real_state.orientation - angular_velocities[i] = real_state.angular_velocity - okid_params[i] = real_state.okid_params.g_eta - - # Store estimated state for plotting - positions_est[i] = state.position - velocities_est[i] = state.velocity - quaternions_est[i] = state.orientation - angular_velocities_est[i] = state.angular_velocity - okid_params_est[i] = state.okid_params.g_eta - - # Add small delay to simulate real-time execution - time.sleep(0.001) -print(state.as_vector()) -# Plotting -time_points = np.arange(0, sim_time, dt) - -# 3D trajectory plot -fig = plt.figure(figsize=(12, 10)) -ax = fig.add_subplot(111, projection='3d') -ax.plot(positions[:, 0], positions[:, 1], positions[:, 2], 'b-', label='True Trajectory') -ax.plot(positions_est[:, 0], positions_est[:, 1], positions_est[:, 2], 'r--', label='Estimated Trajectory') -ax.scatter(positions[0, 0], positions[0, 1], positions[0, 2], c='g', marker='o', s=100, label='Start') -ax.scatter(positions[-1, 0], positions[-1, 1], positions[-1, 2], c='r', marker='o', s=100, label='End') -ax.set_xlabel('X Position') -ax.set_ylabel('Y Position') -ax.set_zlabel('Z Position') -ax.set_title('3D Trajectory') -ax.legend() - -# Position plot -plt.figure(figsize=(12, 6)) -plt.subplot(311) -plt.plot(time_points, positions[:, 0], 'b-', label='True') -plt.plot(time_points, positions_est[:, 0], 'r--', label='Estimated') -plt.ylabel('X Position') -plt.legend() -plt.subplot(312) -plt.plot(time_points, positions[:, 1], 'b-', label='True') -plt.plot(time_points, positions_est[:, 1], 'r--', label='Estimated') -plt.ylabel('Y Position') -plt.legend() -plt.subplot(313) -plt.plot(time_points, positions[:, 2], 'b-', label='True') -plt.plot(time_points, positions_est[:, 2], 'r--', label='Estimated') -plt.ylabel('Z Position') -plt.xlabel('Time (s)') -plt.legend() -plt.tight_layout() - -# Velocity plot -plt.figure(figsize=(12, 6)) -plt.subplot(311) -plt.plot(time_points, velocities[:, 0], 'b-', label='True') -plt.plot(time_points, velocities_est[:, 0], 'r--', label='Estimated') -plt.ylabel('X Velocity') -plt.legend() -plt.subplot(312) -plt.plot(time_points, velocities[:, 1], 'b-', label='True') -plt.plot(time_points, velocities_est[:, 1], 'r--', label='Estimated') -plt.ylabel('Y Velocity') -plt.legend() -plt.subplot(313) -plt.plot(time_points, velocities[:, 2], 'b-', label='True') -plt.plot(time_points, velocities_est[:, 2], 'r--', label='Estimated') -plt.ylabel('Z Velocity') -plt.xlabel('Time (s)') -plt.legend() -plt.tight_layout() - -# Angular velocity plot -plt.figure(figsize=(12, 6)) -plt.subplot(311) -plt.plot(time_points, angular_velocities[:, 0], 'b-', label='True') -plt.plot(time_points, angular_velocities_est[:, 0], 'r--', label='Estimated') -plt.ylabel('Roll Rate') -plt.legend() -plt.subplot(312) -plt.plot(time_points, angular_velocities[:, 1], 'b-', label='True') -plt.plot(time_points, angular_velocities_est[:, 1], 'r--', label='Estimated') -plt.ylabel('Pitch Rate') -plt.legend() -plt.subplot(313) -plt.plot(time_points, angular_velocities[:, 2], 'b-', label='True') -plt.plot(time_points, angular_velocities_est[:, 2], 'r--', label='Estimated') -plt.ylabel('Yaw Rate') -plt.xlabel('Time (s)') -plt.legend() -plt.tight_layout() - -# OKID g_eta plot -plt.figure(figsize=(12, 6)) -plt.subplot(411) -plt.plot(time_points, okid_params[:, 0], 'b-', label='True') -plt.plot(time_points, okid_params_est[:, 0], 'r--', label='Estimated') -plt.ylabel('g_eta[0]') -plt.legend() -plt.subplot(412) -plt.plot(time_points, okid_params[:, 1], 'b-', label='True') -plt.plot(time_points, okid_params_est[:, 1], 'r--', label='Estimated') -plt.ylabel('g_eta[1]') -plt.legend() -plt.subplot(413) -plt.plot(time_points, okid_params[:, 2], 'b-', label='True') -plt.plot(time_points, okid_params_est[:, 2], 'r--', label='Estimated') -plt.ylabel('g_eta[2]') -plt.legend() -plt.subplot(414) -plt.plot(time_points, okid_params[:, 3], 'b-', label='True') -plt.plot(time_points, okid_params_est[:, 3], 'r--', label='Estimated') -plt.ylabel('g_eta[3]') -plt.xlabel('Time (s)') -plt.legend() -plt.tight_layout() - -plt.show() - diff --git a/navigation/ukf_okid/ukf_python/ukf_utils.py b/navigation/ukf_okid/ukf_python/ukf_utils.py deleted file mode 100644 index 7bf7cd4e3..000000000 --- a/navigation/ukf_okid/ukf_python/ukf_utils.py +++ /dev/null @@ -1,35 +0,0 @@ -import numpy as np -from ukf_okid_class import StateQuat - - -def print_StateQuat_list( - state_list: list[StateQuat], name="StateQuat List", print_covariance=True -): - """Custom print function to print a list of StateQuat objects in a formatted form.""" - print(f"{name}:") - for i, state in enumerate(state_list): - print(f"Index {i}:") - print_StateQuat(state, f"StateQuat {i}", print_covariance) - - -def print_StateQuat(state: StateQuat, name="StateQuat", print_covariance=True): - """Custom print function to print StateQuat objects in a formatted form.""" - print(f"{name}:") - print(f" Position: {state.position}") - print(f" Orientation: {state.orientation}") - print(f" Velocity: {state.velocity}") - print(f" Angular Velocity: {state.angular_velocity}") - print(f" okid state: {state.okid_params}") - # print(f" okid_params: {state.okid_params}") - if print_covariance: - print_matrix(state.covariance, "Covariance") - - -def print_matrix(matrix, name="Matrix"): - """Custom print function to print matrices in a formatted form.""" - print(f"{name}: {matrix.shape}") - if isinstance(matrix, np.ndarray): - for row in matrix: - print(" ".join(f"{val:.2f}" for val in row)) - else: - print(matrix) From 723da45b1d12b3e7afb0eb5b54932ee22d5d5383 Mon Sep 17 00:00:00 2001 From: Talha Nauman Choudhry Date: Sun, 5 Oct 2025 20:31:07 +0200 Subject: [PATCH 105/290] fix: removing tukf and some errors in eskf --- navigation/eskf/include/eskf/eskf_ros.hpp | 11 +- navigation/eskf/src/eskf_ros.cpp | 20 +- navigation/tukf_rsi/CMakeLists.txt | 61 ------ .../tukf_rsi/config/tusk_rsi_params.yaml | 12 -- .../tukf_rsi/include/tukf_rsi/tukf_rsi.hpp | 63 ------ .../include/tukf_rsi/tukf_rsi_model.hpp | 50 ----- .../include/tukf_rsi/tukf_rsi_ros.hpp | 64 ------ .../include/tukf_rsi/tukf_rsi_utils.hpp | 65 ------ .../tukf_rsi/include/tukf_rsi/typedefs.hpp | 145 ------------- navigation/tukf_rsi/launch/tukf_rsi.launch.py | 22 -- navigation/tukf_rsi/package.xml | 22 -- navigation/tukf_rsi/src/tukf_rsi.cpp | 84 -------- navigation/tukf_rsi/src/tukf_rsi_model.cpp | 125 ----------- navigation/tukf_rsi/src/tukf_rsi_node.cpp | 9 - navigation/tukf_rsi/src/tukf_rsi_ros.cpp | 150 ------------- navigation/tukf_rsi/src/tukf_rsi_utils.cpp | 199 ------------------ 16 files changed, 17 insertions(+), 1085 deletions(-) delete mode 100644 navigation/tukf_rsi/CMakeLists.txt delete mode 100644 navigation/tukf_rsi/config/tusk_rsi_params.yaml delete mode 100644 navigation/tukf_rsi/include/tukf_rsi/tukf_rsi.hpp delete mode 100644 navigation/tukf_rsi/include/tukf_rsi/tukf_rsi_model.hpp delete mode 100644 navigation/tukf_rsi/include/tukf_rsi/tukf_rsi_ros.hpp delete mode 100644 navigation/tukf_rsi/include/tukf_rsi/tukf_rsi_utils.hpp delete mode 100644 navigation/tukf_rsi/include/tukf_rsi/typedefs.hpp delete mode 100644 navigation/tukf_rsi/launch/tukf_rsi.launch.py delete mode 100644 navigation/tukf_rsi/package.xml delete mode 100644 navigation/tukf_rsi/src/tukf_rsi.cpp delete mode 100644 navigation/tukf_rsi/src/tukf_rsi_model.cpp delete mode 100644 navigation/tukf_rsi/src/tukf_rsi_node.cpp delete mode 100644 navigation/tukf_rsi/src/tukf_rsi_ros.cpp delete mode 100644 navigation/tukf_rsi/src/tukf_rsi_utils.cpp diff --git a/navigation/eskf/include/eskf/eskf_ros.hpp b/navigation/eskf/include/eskf/eskf_ros.hpp index d4a770864..424e583f7 100644 --- a/navigation/eskf/include/eskf/eskf_ros.hpp +++ b/navigation/eskf/include/eskf/eskf_ros.hpp @@ -21,14 +21,14 @@ class ESKFNode : public rclcpp::Node { explicit ESKFNode(); private: - // @brief Callback function for the imu topic // @param msg: Imu message containing the imu data void imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg); // @brief Callback function for the dvl topic // @param msg: TwistWithCovarianceStamped message containing the dvl data - void dvl_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); + void dvl_callback( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); // @brief Publish the odometry message void publish_odom(); @@ -41,7 +41,8 @@ class ESKFNode : public rclcpp::Node { rclcpp::Subscription::SharedPtr imu_sub_; - rclcpp::Subscription::SharedPtr dvl_sub_; + rclcpp::Subscription< + geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr dvl_sub_; rclcpp::Publisher::SharedPtr odom_pub_; @@ -63,11 +64,11 @@ class ESKFNode : public rclcpp::Node { std::unique_ptr eskf_; - rclcpp::Time last_imu_time_; - bool first_imu_msg_received_ = false; Eigen::Matrix3d R_imu_eskf_; + + rclcpp::Time last_imu_time_; }; #endif // ESKF_ROS_HPP diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp index cfa85172e..c379732e3 100644 --- a/navigation/eskf/src/eskf_ros.cpp +++ b/navigation/eskf/src/eskf_ros.cpp @@ -28,7 +28,8 @@ void ESKFNode::set_subscribers_and_publisher() { this->declare_parameter("dvl_topic"); std::string dvl_topic = this->get_parameter("dvl_topic").as_string(); - dvl_sub_ = this->create_subscription( + dvl_sub_ = this->create_subscription< + geometry_msgs::msg::TwistWithCovarianceStamped>( dvl_topic, qos_sensor_data, std::bind(&ESKFNode::dvl_callback, this, std::placeholders::_1)); @@ -95,21 +96,22 @@ void ESKFNode::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { std::tie(nom_state_, error_state_) = eskf_->imu_update(imu_meas_, dt); } -void ESKFNode::dvl_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { - // Log that we received a DVL message - // spdlog::info("DVL message received"); - dvl_meas_.vel << msg->twist.twist.linear.x, msg->twist.twist.linear.y, msg->twist.twist.linear.z; +void ESKFNode::dvl_callback( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { + dvl_meas_.vel << msg->twist.twist.linear.x, msg->twist.twist.linear.y, + msg->twist.twist.linear.z; - dvl_meas_.cov << msg->twist.covariance[0], msg->twist.covariance[1], msg->twist.covariance[2], - msg->twist.covariance[6], msg->twist.covariance[7], msg->twist.covariance[8], - msg->twist.covariance[12], msg->twist.covariance[13], msg->twist.covariance[14]; + dvl_meas_.cov << msg->twist.covariance[0], msg->twist.covariance[1], + msg->twist.covariance[2], msg->twist.covariance[6], + msg->twist.covariance[7], msg->twist.covariance[8], + msg->twist.covariance[12], msg->twist.covariance[13], + msg->twist.covariance[14]; std::tie(nom_state_, error_state_) = eskf_->dvl_update(dvl_meas_); std_msgs::msg::Float64 nis_msg; nis_msg.data = eskf_->NIS_; nis_pub_->publish(nis_msg); - } void ESKFNode::publish_odom() { diff --git a/navigation/tukf_rsi/CMakeLists.txt b/navigation/tukf_rsi/CMakeLists.txt deleted file mode 100644 index f1ad5e0b1..000000000 --- a/navigation/tukf_rsi/CMakeLists.txt +++ /dev/null @@ -1,61 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(tukf_rsi) - -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(nav_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(Eigen3 REQUIRED) -find_package(tf2 REQUIRED) -find_package(vortex_msgs REQUIRED) -find_package(spdlog REQUIRED) -find_package(fmt REQUIRED) - -if(NOT DEFINED EIGEN3_INCLUDE_DIR) - set(EIGEN3_INCLUDE_DIR ${EIGEN3_INCLUDE_DIRS}) -endif() -include_directories(${EIGEN3_INCLUDE_DIR}) - -include_directories(include) - -add_executable(tukf_rsi_node - src/tukf_rsi.cpp - src/tukf_rsi_ros.cpp - src/tukf_rsi_node.cpp - src/tukf_rsi_utils.cpp -) - -ament_target_dependencies(tukf_rsi_node - rclcpp - geometry_msgs - nav_msgs - Eigen3 - tf2 - vortex_msgs - spdlog - fmt -) - -target_link_libraries(tukf_rsi_node - fmt::fmt -) - -install(TARGETS - tukf_rsi_node - DESTINATION lib/${PROJECT_NAME}) - -install(DIRECTORY - config - launch - DESTINATION share/${PROJECT_NAME}/ -) - -ament_package() diff --git a/navigation/tukf_rsi/config/tusk_rsi_params.yaml b/navigation/tukf_rsi/config/tusk_rsi_params.yaml deleted file mode 100644 index c74717815..000000000 --- a/navigation/tukf_rsi/config/tusk_rsi_params.yaml +++ /dev/null @@ -1,12 +0,0 @@ -eskf_node: - ros__parameters: - gyro_topic: "/imu/data" - dvl_topic: "/orca/twist" - odom_topic: "/tukf/odom" - wrench_topic: "/orca/wrench_input" - diag_Q_std: [0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01] - diag_P0_std: [2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05] - x0: [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, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - diag_Rgyro_std: [0.01, 0.01, 0.01] - diag_Rdvl_std: [0.05, 0.05, 0.05] - diff --git a/navigation/tukf_rsi/include/tukf_rsi/tukf_rsi.hpp b/navigation/tukf_rsi/include/tukf_rsi/tukf_rsi.hpp deleted file mode 100644 index a1e19edb5..000000000 --- a/navigation/tukf_rsi/include/tukf_rsi/tukf_rsi.hpp +++ /dev/null @@ -1,63 +0,0 @@ -// tukf.hpp -#ifndef TUKF_HPP -#define TUKF_HPP - -#include "typedefs.hpp" // includes AUVState, MeasModel, and utility functions -#include - -class TUKF { -public: - TUKF(const AUVState& x0, - const Eigen::Matrix37d& Q_in, - double dt = 0.01); - - // @brief Generate sigma points for the current state - // @param current_state: Current state of the AUV - // @return A vector of sigma points - std::vector sigma_points(const AUVState& current_state); - - // @brief Unscented transform to predict the next state - // @param current_state: Current state of the AUV - // @param control_force: Control force applied to the AUV - // @return Predicted state after applying the control force - AUVState unscented_transform(const AUVState& current_state, - const Eigen::Vector3d& control_force); - - // @brief Perform measurement update using the measurement model - // @param current_state: Current state of the AUV - // @param measurement: Measurement model containing the measurement and covariance - // @return Updated state after measurement update - void measurement_update(const AUVState& current_state, - const MeasModel& measurement); - - // @brief Posterior estimate of the state after measurement update - // @param current_state: Current state of the AUV - // @param measurement: Measurement model containing the measurement and covariance - // @return Posterior estimate of the state - AUVState posterior_estimate(const AUVState& current_state, - const MeasModel& measurement); - - // public state and flags - AUVState x; - bool filter_failed; - -private: - - Eigen::Matrix37d Q; - - Eigen::Matrix delta; - - std::vector sigma_points_list; - - std::vector y_i; - - MeasModel measurement_updated; - - Eigen::Matrix cross_correlation; - - double dt; - - int flagg; -}; - -#endif // TUKF_HPP \ No newline at end of file diff --git a/navigation/tukf_rsi/include/tukf_rsi/tukf_rsi_model.hpp b/navigation/tukf_rsi/include/tukf_rsi/tukf_rsi_model.hpp deleted file mode 100644 index b536ca798..000000000 --- a/navigation/tukf_rsi/include/tukf_rsi/tukf_rsi_model.hpp +++ /dev/null @@ -1,50 +0,0 @@ -#ifndef TUKF_MODEL_HPP -#define TUKF_MODEL_HPP - -#include -#include "tukf_rsi/typedefs.hpp" - - -// @brief Tranformation matrix from quaternion orientation -// @param orientation: Quaternion representing the orientation -// @return 3x3 transformation matrix -Eigen::Matrix3d tranfromation_matrix(const Eigen::Quaterniond& orientation); - -// @brief Mass inertia system-matrix (6×6) -// @param inertia_vec: Vector containing the inertia parameters -Eigen::Matrix6d M_rb(const Eigen::Vector9d& inertia_vec); - -// @brief Added mass system-matrix (6×6) -// @param added_mass: Vector containing the added mass parameters -Eigen::Matrix6d M_a(const Eigen::Vector6d& added_mass); - -// @brief Corilos and centripetal forces system-matrix (6×6) -// @param inertia_vec: Vector containing the inertia parameters -// @param angular_velocity: Angular velocity vector -Eigen::Matrix6d C_rb(const Eigen::Vector9d& inertia_vec, - const Eigen::Vector3d& angular_velocity); - -// @brief added mass Corilos and centripetal forces system-matrix (6×6) -// @param added_mass: Vector containing the added mass parameters -// @param angular_velocity: Angular velocity vector -// @param velocity: Velocity linear vector -Eigen::Matrix6d C_a(const Eigen::Vector6d& added_mass, - const Eigen::Vector3d& angular_velocity, - const Eigen::Vector3d& velocity); - -// @brief Damping system-matrix (6×6) -// @param damping: Vector containing the damping parameters -Eigen::Matrix6d D_linear(const Eigen::Vector6d& damping); - -// @brief generalized froces (6×1) -// @param g_eta_params: Vector containing the g_eta parameters (buoyancy terms) -// @param euler_angles: Euler angles (roll, pitch, yaw) -Eigen::Vector6d G_eta(const Eigen::Vector4d& g_eta_params, - const Eigen::Vector3d& euler_angles); - -// @brief Dynamics function for the AUV -AUVState F_dynamics(const AUVState& state, - double dt, - const Eigen::Vector3d& control_input); - -#endif // TUKF_MODEL_HPP \ No newline at end of file diff --git a/navigation/tukf_rsi/include/tukf_rsi/tukf_rsi_ros.hpp b/navigation/tukf_rsi/include/tukf_rsi/tukf_rsi_ros.hpp deleted file mode 100644 index 98b0a547a..000000000 --- a/navigation/tukf_rsi/include/tukf_rsi/tukf_rsi_ros.hpp +++ /dev/null @@ -1,64 +0,0 @@ -#ifndef TUKF_NODE_HPP -#define TUKF_NODE_HPP - -#include -#include -#include -#include -#include -#include -#include -#include -#include "tukf_rsi/tukf.hpp" -#include "tukf_rsi/tukf_rsi_utils.hpp" -#include "tukf_rsi/typedefs.hpp" - -class TUKFNode : public rclcpp::Node { -public: - TUKFNode(); - -private: - - // @brief Callback function for the gyro topic - // @param msg: Imu message containing the gyro data - void gyro_callback(const sensor_msgs::msg::Imu::SharedPtr msg); - - // @brief Callback function for the DVL topic - // @param msg: TwistWithCovarianceStamped message containing the DVL data - void dvl_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); - - // @brief Callback function for the wrench topic - // @param msg: WrenchStamped message containing the wrench data - void wrench_callback(const geometry_msgs::msg::WrenchStamped::SharedPtr msg); - - // @brief Set the subscriber and publisher for the node - void set_subscribers_and_publisher(); - - // @brief Set the parameters for the eskf - void set_parameters(); - - // @brief Publish the odometry message - void publish_odom(); - - rclcpp::Subscription::SharedPtr gyro_sub_; - - rclcpp::Subscription::SharedPtr dvl_sub_; - - rclcpp::Subscription::SharedPtr wrench_sub_; - - rclcpp::Publisher::SharedPtr odom_pub_; - - rclcpp::TimerBase::SharedPtr odom_timer_; - - std::unique_ptr tukf_; - - AUVState state_; - - double dt_; - - Eigen::Matrix3d R_gyro_; - - Eigen::Matrix3d R_dvl_; -}; - -#endif // TUKF_NODE_HPP \ No newline at end of file diff --git a/navigation/tukf_rsi/include/tukf_rsi/tukf_rsi_utils.hpp b/navigation/tukf_rsi/include/tukf_rsi/tukf_rsi_utils.hpp deleted file mode 100644 index b3c31221f..000000000 --- a/navigation/tukf_rsi/include/tukf_rsi/tukf_rsi_utils.hpp +++ /dev/null @@ -1,65 +0,0 @@ -#ifndef TUKF_RSI_UTILS_HPP -#define TUKF_RSI_UTILS_HPP - -#include -#include -#include "tukf_rsi/typedefs.hpp" - -// @brief Compute mean quaternion from a set of quaternions -// @param quats: Vector of quaternions -// @param tol: Tolerance for convergence -// @param maxIter: Maximum number of iterations -Eigen::Quaterniond quaternion_mean( - const std::vector& quats, - double tol = 1e-6, - int maxIter = 100 -); - -// @brief Compute mean of a set of AUV states -// @param setPoints: Vector of AUVState objects -// @param tol: Tolerance for convergence -// @param maxIter: Maximum number of iterations -Eigen::Vector37d mean_set( - const std::vector& setPoints, - double tol = 1e-6, - int maxIter = 100 -); - -// @brief Compute mean of a set of measurements -// @param setPoints: Vector of MeasModel objects -Eigen::Vector3d mean_masurement(const std::vector& setPoints); - -// @brief Compute covariance of a set of AUV states -// @param setPoints: Vector of AUVState objects -// @param meanVec: Mean vector of the set -// @param tol: Tolerance for convergence -Eigen::Matrix37d covariance_set( - const std::vector& setPoints, - const Eigen::Vector37d& meanVec, - double tol = 1e-6 -); - -// @brief Compute covariance of a set of measurements -// @param setPoints: Vector of MeasModel objects -// @param mean: Mean vector of the measurements -Eigen::Matrix3d covariance_measurement( - const std::vector& setPoints, - const Eigen::Vector3d& mean -); - -// @brief Compute cross-covariance between AUV states and measurements -// @param setY: Vector of AUVState objects -// @param meanY: Mean vector of the AUV states -// @param setZ: Vector of MeasModel objects -// @param meanZ: Mean vector of the measurements -// @param tol: Tolerance for convergence -Eigen::Matrix cross_covariance( - const std::vector& setY, - const Eigen::Vector37d& meanY, - const std::vector& setZ, - const Eigen::Vector3d& meanZ, - double tol = 1e-6 -); - -#endif // TUKF_RSI_UTILS_HPP - diff --git a/navigation/tukf_rsi/include/tukf_rsi/typedefs.hpp b/navigation/tukf_rsi/include/tukf_rsi/typedefs.hpp deleted file mode 100644 index 0d724577d..000000000 --- a/navigation/tukf_rsi/include/tukf_rsi/typedefs.hpp +++ /dev/null @@ -1,145 +0,0 @@ -#ifndef AUV_TYPEDEFS_HPP -#define AUV_TYPEDEFS_HPP - -#include -#include -#include -#include - -namespace Eigen { - typedef Matrix Vector37d; - typedef Matrix Matrix37d; - typedef Matrix Vector25d; - typedef Matrix Vector12d; - typedef Matrix Vector9d; - typedef Matrix Vector6d; - typedef Matrix Vector4d; - typedef Matrix Matrix3x12d; - typedef Matrix Matrix3d; - typedef Matrix Matrix3x37d; - typedef Matrix Matrix37x74d; - typedef Matrix Matrix37x3d; -} - -struct AUVState { - Eigen::Vector3d position = Eigen::Vector3d::Zero(); - Eigen::Quaterniond orientation = Eigen::Quaterniond::Identity(); - Eigen::Vector3d velocity = Eigen::Vector3d::Zero(); - Eigen::Vector3d angular_velocity = Eigen::Vector3d::Zero(); - Eigen::Vector9d inertia = Eigen::Vector9d::Zero(); - Eigen::Vector6d added_mass = Eigen::Vector6d::Zero(); - Eigen::Vector6d damping = Eigen::Vector6d::Zero(); - Eigen::Vector4d g_eta = Eigen::Vector4d::Zero(); - Eigen::Matrix37d covariance = Eigen::Matrix37d::Zero(); - - Eigen::Vector3d error = Eigen::Vector3d::Zero(); - - AUVState() = default; - - Eigen::Vector12d dynamic_part() const { - Eigen::Vector12d x; - x << position, - orientation.vec(), - velocity, - angular_velocity; - return x; - } - - Eigen::Vector25d okid_part() const { - Eigen::Vector25d x; - x << inertia, - added_mass, - damping, - g_eta; - return x; - } - - Eigen::Vector37d as_vector() const { - Eigen::Vector37d x; - x << dynamic_part(), - okid_part(); - return x; - } - - AUVState operator+(const AUVState& other) const { - AUVState result; - result.position = position + other.position; - result.orientation = orientation * other.orientation; - result.velocity = velocity + other.velocity; - result.angular_velocity = angular_velocity + other.angular_velocity; - result.inertia = inertia + other.inertia; - result.added_mass = added_mass + other.added_mass; - result.damping = damping + other.damping; - result.g_eta = g_eta + other.g_eta; - return result; - } - - AUVState operator-(const AUVState& other) const { - AUVState result; - result.position = position - other.position; - result.orientation = orientation * other.orientation.inverse(); - result.velocity = velocity - other.velocity; - result.angular_velocity = angular_velocity - other.angular_velocity; - result.inertia = inertia - other.inertia; - result.added_mass = added_mass - other.added_mass; - result.damping = damping - other.damping; - result.g_eta = g_eta - other.g_eta; - return result; - } - - void fill_states(const Eigen::Vector37d& x) { - position = x.segment<3>(0); - Eigen::Vector3d ori_vec = x.segment<3>(3); - orientation = Eigen::Quaterniond(1, ori_vec.x(), ori_vec.y(), ori_vec.z()).normalized(); - velocity = x.segment<3>(6); - angular_velocity = x.segment<3>(9); - inertia = x.segment<9>(12); - added_mass = x.segment<6>(21); - damping = x.segment<6>(27); - g_eta = x.segment<4>(33); - } -}; - -struct MeasModel { - Eigen::Vector3d measurement = Eigen::Vector3d::Zero(); - Eigen::Matrix3d covariance = Eigen::Matrix3d::Zero(); - std::function H; - - MeasModel() - : H(default_h) - {} - - MeasModel(const Eigen::Vector3d& meas, - const Eigen::Matrix3d& cov, - std::function Hfunc = default_h) - : measurement(meas), covariance(cov), H(std::move(Hfunc)) - {} - - static MeasModel default_h(const AUVState& state) { - MeasModel z; - Eigen::Matrix3x12d Hmat = Eigen::Matrix3x12d::Zero(); - Hmat.block<3,3>(0,6) = Eigen::Matrix3d::Identity(); - z.measurement = Hmat * state.dynamic_part(); - return z; - } - - MeasModel operator+(const MeasModel& other) const { - MeasModel r; - r.measurement = measurement + other.measurement; - return r; - } - - MeasModel operator-(const MeasModel& other) const { - MeasModel r; - r.measurement = measurement - other.measurement; - return r; - } - - friend MeasModel operator*(double scalar, const MeasModel& m) { - MeasModel r; - r.measurement = scalar * m.measurement; - return r; - } -}; - -#endif // AUV_TYPEDEFS_HPP \ No newline at end of file diff --git a/navigation/tukf_rsi/launch/tukf_rsi.launch.py b/navigation/tukf_rsi/launch/tukf_rsi.launch.py deleted file mode 100644 index e238b3e1e..000000000 --- a/navigation/tukf_rsi/launch/tukf_rsi.launch.py +++ /dev/null @@ -1,22 +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 - -tukf_rsi_params = path.join( - get_package_share_directory("tukf_rsi"), "config", "tukf_rsi_params.yaml" -) - - -def generate_launch_description(): - tukf_rsi_node = Node( - package="tukf_rsi", - executable="tukf_rsi_node", - name="tukf_rsi_node", - parameters=[ - tukf_rsi_params, - ], - output="screen", - ) - return LaunchDescription([tukf_rsi_node]) diff --git a/navigation/tukf_rsi/package.xml b/navigation/tukf_rsi/package.xml deleted file mode 100644 index 989241795..000000000 --- a/navigation/tukf_rsi/package.xml +++ /dev/null @@ -1,22 +0,0 @@ - - - - tukf_rsi - 1.0.0 - Transformed Unscented Kalman Filter - talhanc - MIT - - ament_cmake - - rclcpp - geometry_msgs - nav_msgs - eigen - tf2 - vortex_msgs - - - ament_cmake - - diff --git a/navigation/tukf_rsi/src/tukf_rsi.cpp b/navigation/tukf_rsi/src/tukf_rsi.cpp deleted file mode 100644 index df0195d86..000000000 --- a/navigation/tukf_rsi/src/tukf_rsi.cpp +++ /dev/null @@ -1,84 +0,0 @@ -#include "tukf_rsi/tukf.hpp" -#include "tukf_rsi/tukf_rsi_utils.hpp" -#include "tukf_rsi/typedefs.hpp" -#include "tukf_rsi/tukf_rsi_model.hpp" -#include - -TUKF::TUKF(const AUVState& x0, - const Eigen::Matrix37d& Q_in, - double dt_in) - : x(x0), Q(Q_in), dt(dt_in), filter_failed(false), flagg(0) -{ - delta = generate_elta_matrix37() / std::sqrt(static_cast(x.as_sector().size())); - measurement_updated = MeasModel(); -} - -std::vector TUKF::sigma_points(const AUVState& current_state) { - int n = static_cast(current_state.covariance.rows()); - ++flagg; - Eigen::Matrix37d S; - bool chol_ok = true; - auto llt = current_state.covariance.llt(); - if(llt.info() == Eigen::NumericalIssue) { - chol_ok = false; - } else { - S = llt.matrixL(); - } - if (!chol_ok) { - filter_failed = true; - S = Eigen::Matrix37d::Identity() * 1e-6; - } - sigma_points_list.resize(2 * n); - for (int k = 0; k < 2 * n; ++k) { - Eigen::Vector37d v = current_state.as_vector() + S * delta.col(k); - sigma_points_list[k].fill_States(v); - } - return sigma_points_list; -} - -AUVState TUKF::unscented_transform(const AUVState& current_state, - const Eigen::Vector3d& control_force) { - int n = static_cast(current_state.covariance.rows()); - sigma_soints(current_state); - y_i.resize(2 * n); - for (int i = 0; i < 2 * n; ++i) { - y_i[i] = F_dynamics(sigma_points_list[i], dt, control_force); - } - AUVState state_est; - Eigen::Vector37d x_vec = mean_Set(y_i); - state_est.fill_States(x_vec); - state_est.covariance = covarianceSet(y_i, x_vec) + Q; - return state_est; -} - -void TUKF::measurement_update(const AUVState& current_state, - const MeasModel& measurement) { - int n = static_cast(current_state.covariance.rows()); - std::vector z_i(2 * n); - for (int i = 0; i < 2 * n; ++i) { - z_i[i] = measurement.H(sigma_points_list[i]); - } - measurement_updated.measurement = mean_seasurement(z_i); - measurement_updated.covariance = covariance_measurement( - z_i, measurement_updated.measurement); - cross_correlation = cross_covariance( - y_i, - current_state.as_vector(), - z_i, - measurement_updated.measurement); -} - -AUVState TUKF::posterior_estimate(const AUVState& current_state, - const MeasModel& measurement) { - MeasModel nu_k; - nu_k.measurement = measurement.measurement - measurement_updated.measurement; - nu_k.covariance = measurement_updated.covariance + measurement.covariance; - Eigen::Matrix K = cross_correlation * nu_k.covariance.inverse(); - AUVState post; - Eigen::Vector37d v = current_state.as_vector() + - K * nu_k.measurement; - post.fill_states(v); - post.covariance = current_state.covariance - - K * nu_k.covariance * K.transpose(); - return post; -} \ No newline at end of file diff --git a/navigation/tukf_rsi/src/tukf_rsi_model.cpp b/navigation/tukf_rsi/src/tukf_rsi_model.cpp deleted file mode 100644 index 97a91c024..000000000 --- a/navigation/tukf_rsi/src/tukf_rsi_model.cpp +++ /dev/null @@ -1,125 +0,0 @@ -#include "tukf_rsi/tukf_model.hpp" -#include "tukf_rsi/tukf_rsi_utils.hpp" -#include "tukf_rsi/typedefs.hpp" -#include -#include - - -Eigen::Matrix4x3d tranfromation_matrix(const Eigen::Quaterniond& q) { - Eigen::Matrix4x3d T; - T << -q.x(), -q.y(), -q.z(), - q.w(), -q.z(), q.y(), - q.z(), q.w(), -q.x(), - -q.y(), q.x(), q.w(); - return T; -} - -Eigen::Matrix6d M_rb(const Eigen::Vector& inertia_vec) { - const double mass = 30.0; - const Eigen::Matrix3d I_rb = Eigen::Map(inertia_vec.data()); - const Eigen::Vector3d r_b_bg(0.01, 0.0, 0.02); - - Eigen::Matrix6d M = Eigen::Matrix6d::Zero(); - M.block<3,3>(0,0) = mass * Eigen::Matrix3d::Identity(); - M.block<3,3>(0,3) = -mass * skewSymmetric(r_b_bg); - M.block<3,3>(3,0) = mass * skewSymmetric(r_b_bg); - M.block<3,3>(3,3) = I_rb; - - return M; -} - -Eigen::Matrix6d M_a(const Eigen::Vector& a_mass) { - Eigen::Matrix6d Ma = Eigen::Matrix6d::Zero(); - Ma.block<3,3>(0,0) = a_mass.head<3>().asDiagonal(); - Ma.block<3,3>(3,3) = a_mass.tail<3>().asDiagonal(); - return Ma; -} - -Eigen::Matrix6d C_rb(const Eigen::Vector& inertia_vec, const Eigen::Vector3d& w) { - const double mass = 30.0; - const Eigen::Vector3d r_b_bg(0.01, 0.0, 0.02); - const Eigen::Matrix3d I_rb = Eigen::Map(inertia_vec.data()); - - Eigen::Matrix6d C = Eigen::Matrix6d::Zero(); - C.block<3,3>(3,3) = -skewSymmetric(I_rb * w); - C.block<3,3>(0,3) = -mass * skewSymmetric(w) * skewSymmetric(r_b_bg); - C.block<3,3>(3,0) = mass * skewSymmetric(r_b_bg) * skewSymmetric(w); - - return C; -} - -Eigen::Matrix6d C_a(const Eigen::Vector& a_mass, const Eigen::Vector3d& w, const Eigen::Vector3d& v) { - Eigen::Matrix6d Ca = Eigen::Matrix6d::Zero(); - const Eigen::Matrix3d A11 = a_mass.head<3>().asDiagonal(); - const Eigen::Matrix3d A22 = a_mass.tail<3>().asDiagonal(); - - Ca.block<3,3>(0,3) = -skewSymmetric(A11 * v); - Ca.block<3,3>(3,0) = -skewSymmetric(A11 * v); - Ca.block<3,3>(3,3) = -skewSymmetric(A22 * w); - - return Ca; -} - -Eigen::Matrix6d D_linear(const Eigen::Vector& d) { - Eigen::Matrix6d D = Eigen::Matrix6d::Zero(); - D.block<3,3>(0,0) = -d.head<3>().asDiagonal(); - D.block<3,3>(3,3) = -d.tail<3>().asDiagonal(); - return D; -} - -Eigen::Vector6d G_eta(const Eigen::Vector& g_params, const Eigen::Quaterniond& q) { - const double Mx = g_params[1], My = g_params[2], Mz = g_params[3]; - const Eigen::Matrix3d R = q.toRotationMatrix(); - - Eigen::Vector6d G = Eigen::Vector6d::Zero(); - G(3) = -My*R(2,2) + Mz*R(1,2); - G(4) = -Mz*R(0,2) + Mx*R(2,2); - G(5) = -Mx*R(1,2) + My*R(0,2); - - return G; -} - -AUVState F_dynamics(const AUVState& state, double dt, const Eigen::Vector6d& u) { - const Eigen::Matrix6d Mrb = M_rb(state.inertia); - const Eigen::Matrix6d Ma = M_a(state.added_mass); - const Eigen::Matrix6d Mtotal = Mrb + Ma; - - const Eigen::Matrix6d Crb = C_rb(state.inertia, state.angular_velocity); - const Eigen::Matrix6d Ca = C_a(state.added_mass, state.angular_velocity, state.velocity); - const Eigen::Matrix6d Ctotal = Crb + Ca; - - const Eigen::Matrix6d Dl = D_linear(state.damping); - const Eigen::Vector6d G = G_eta(state.g_eta, state.orientation); - - Eigen::Vector6d nu; - nu << state.velocity, state.angular_velocity; - - AUVState sd; - sd.position = state.orientation.toRotationMatrix() * state.velocity; - sd.orientation = tranfromation_matrix(state.orientation) * state.angular_velocity; - - Eigen::Vector6d Nu = Mtotal.inverse() * (u - Ctotal*nu - Dl*nu - G); - sd.velocity = Nu.head<3>(); - sd.angular_velocity = Nu.tail<3>(); - - sd.inertia.setZero(); - sd.added_mass.setZero(); - sd.damping.setZero(); - sd.g_eta.setZero(); - - AUVState ns; - ns.position = state.position + sd.position * dt; - ns.orientation = state.orientation * Eigen::Quaterniond(1, 0.5*dt*sd.orientation.x(), - 0.5*dt*sd.orientation.y(), - 0.5*dt*sd.orientation.z()); - ns.orientation.normalize(); - ns.velocity = state.velocity + sd.velocity * dt; - ns.angular_velocity = state.angular_velocity + sd.angular_velocity * dt; - - ns.inertia = state.inertia; - ns.added_mass = state.added_mass; - ns.damping = state.damping; - ns.g_eta = state.g_eta; - - return ns; -} diff --git a/navigation/tukf_rsi/src/tukf_rsi_node.cpp b/navigation/tukf_rsi/src/tukf_rsi_node.cpp deleted file mode 100644 index d4f6eed17..000000000 --- a/navigation/tukf_rsi/src/tukf_rsi_node.cpp +++ /dev/null @@ -1,9 +0,0 @@ -#include "tukf_rsi/tukf_rsi_ros.hpp" - -int main(int argc, char** argv) { - rclcpp::init(argc, argv); - spdlog::info("Starting TUFK for RSI ROS2 Node"); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} \ No newline at end of file diff --git a/navigation/tukf_rsi/src/tukf_rsi_ros.cpp b/navigation/tukf_rsi/src/tukf_rsi_ros.cpp deleted file mode 100644 index 2be453191..000000000 --- a/navigation/tukf_rsi/src/tukf_rsi_ros.cpp +++ /dev/null @@ -1,150 +0,0 @@ -#include "tukf_node.hpp" -#include - -TUKFNode::TUKFNode() -: Node("tukf_node") -{ - - odom_timer_ = this->create_wall_timer( - std::chrono::duration(dt_), - std::bind(&TUKFNode::publishOdom, this)); - - set_subscribers_and_publisher(); - - set_parameters(); - - spdlog::info("TUKF Node Initialized"); -} - -void TUKFNode::set_subscribers_and_publisher() { - auto qos = rclcpp::QoS(rclcpp::SensorDataQoS()); - this->declare_parameter("gyro_topic"); - std::string gyro_topic = this->get_parameter("gyro_topic").as_string(); - gyro_sub_ = this->create_subscription( - gyro_topic, qos, - std::bind(&TUKFNode::gyroCallback, this, std::placeholders::_1)); - - this->declare_parameter("dvl_topic"); - std::string dvl_topic = this->get_parameter("dvl_topic").as_string(); - dvl_sub_ = this->create_subscription( - dvl_topic, qos, - std::bind(&TUKFNode::dvlCallback, this, std::placeholders::_1)); - - this->declare_parameter("wrench_topic"); - std::string dvl_topic = this->get_parameter("wrench_topic").as_string(); - wrench_sub_ = this->create_subscription( - wrench_sub_, qos, - std::bind(&TUKFNode::wrenchCallback, this, std::placeholders::_1)); - - this->declare_parameter("odom_topic"); - std::string odom_topic = this->get_parameter("odom_topic").as_string(); - odom_pub_ = this->create_publisher( - odom_topic, qos); -} - -void TUKFNode::set_parameters() { - this->declare_parameter>("diag_Q_std"); - auto diagQ = this->get_parameter("diag_Q_std").as_double_array(); - Eigen::Matrix37d Q = Eigen::Matrix37d::Zero(); - for (int i = 0; i < 37; ++i) { - Q(i,i) = diagQ[i] * diagQ[i]; - } - - this->declare_parameter>("diag_P0_std"); - auto diagP0 = this->get_parameter("diag_P0_std").as_double_array(); - Eigen::Matrix37d P0 = Eigen::Matrix37d::Zero(); - for (int i = 0; i < 37; ++i) { - P0(i,i) = diagP0[i] * diagP0[i]; - } - - this->declare_parameter>("x0"); - auto x0_vec = this->get_parameter("x0").as_double_array(); - Eigen::Vector37d x0_e; - for (int i = 0; i < 37; ++i) x0_e[i] = x0_vec[i]; - - this->declare_parameter("dt", 0.01); - dt_ = this->get_parameter("dt").as_double(); - - this->declare_parameter>("diag_Rgyro_std"); - auto diagRgyro = this->get_parameter("diag_Rgyro_std").as_double_array(); - R_gyro_ = Eigen::Matrix3d::Zero(); - for (int i = 0; i < 3; ++i) R_gyro_(i,i) = diagRgyro[i] * diagRgyro[i]; - - this->declare_parameter>("diag_Rdvl_std"); - auto diagRdvl = this->get_parameter("diag_Rdvl_std").as_double_array(); - R_dvl_ = Eigen::Matrix3d::Zero(); - for (int i = 0; i < 3; ++i) R_dvl_(i,i) = diagRdvl[i] * diagRdvl[i]; - - tukf_ = std::make_unique(x0, Q, dt_); - tukf_->x.covariance = P0; - tukf_->x.fill_states(x0_e); -} - -void TUKFNode::gyro_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { - Eigen::Vector3d gyro(msg->angular_velocity.x, - msg->angular_velocity.y, - msg->angular_velocity.z); - MeasModel m; - m.measurement = gyro; - m.covariance = R_gyro_; - m.H = [](const AUVState& s) { - MeasModel mm; - Eigen::Matrix3x12d Hm = Eigen::Matrix3x12d::Zero(); - Hm.block<3,3>(0,9) = Eigen::Matrix3d::Identity(); - mm.measurement = Hm * s.dynamic_part(); - return mm; - }; - - tukf_->measurement_update(state_, m); - state_ = tukf_->posterior_estimate(state_, m); -} - -void TUKFNode::dvl_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { - Eigen::Vector3d vel(msg->twist.twist.linear.x, - msg->twist.twist.linear.y, - msg->twist.twist.linear.z); - Eigen::Matrix3d cov; - cov << msg->twist.covariance[0], msg->twist.covariance[1], msg->twist.covariance[2], - msg->twist.covariance[6], msg->twist.covariance[7], msg->twist.covariance[8], - msg->twist.covariance[12], msg->twist.covariance[13], msg->twist.covariance[14]; - MeasModel m; - m.measurement = vel; - m.covariance = R_dvl_; - - tukf_->measurement_update(state_, m); - state_ = tukf_->posterior_estimate(state_, m); -} - -void TUKFNode::wrench_callback(const geometry_msgs::msg::WrenchStamped::SharedPtr msg) { - Eigen::Vector6d wrench_input(msg->wrench.force.x, - msg->wrench.force.y, - msg->wrench.force.z, - msg->wrench.torque.x, - msg->wrench.torque.y, - msg->wrench.torque.z); - - state_ = tukf_->unscented_transform(state_, control_force); -} - -void TUKFNode::publish_odom() { - nav_msgs::msg::Odometry odom; - odom.header.stamp = this->now(); - odom.header.frame_id = "odom"; - - odom.pose.pose.position.x = state_.position.x(); - odom.pose.pose.position.y = state_.position.y(); - odom.pose.pose.position.z = state_.position.z(); - odom.pose.pose.orientation.w = state_.orientation.w(); - odom.pose.pose.orientation.x = state_.orientation.x(); - odom.pose.pose.orientation.y = state_.orientation.y(); - odom.pose.pose.orientation.z = state_.orientation.z(); - - odom.twist.twist.linear.x = state_.velocity.x(); - odom.twist.twist.linear.y = state_.velocity.y(); - odom.twist.twist.linear.z = state_.velocity.z(); - odom.twist.twist.angular.x = state_.angular_velocity.x(); - odom.twist.twist.angular.y = state_.angular_velocity.y(); - odom.twist.twist.angular.z = state_.angular_velocity.z(); - - odom_pub_->publish(odom); -} diff --git a/navigation/tukf_rsi/src/tukf_rsi_utils.cpp b/navigation/tukf_rsi/src/tukf_rsi_utils.cpp deleted file mode 100644 index 882fe0cf8..000000000 --- a/navigation/tukf_rsi/src/tukf_rsi_utils.cpp +++ /dev/null @@ -1,199 +0,0 @@ - -#include "tukf_rsi_utils.hpp" -#include - -Eigen::Quaterniond quaternionMean( - const std::vector& quats, - double tol, - int maxIter -) { - Eigen::Quaterniond mean_q = quats.front(); - int n = int(quats.size()); - - for (int iter = 0; iter < maxIter; ++iter) { - Eigen::Vector3d errAvg = Eigen::Vector3d::Zero(); - - for (const auto& q : quats) { - - Eigen::Quaterniond e = q * mean_q.conjugate(); - - double w = std::clamp(e.w(), -1.0, 1.0); - double angle = 2 * std::acos(w); - - Eigen::Vector3d axis; - - if (std::abs(angle) < tol) { - axis.setZero(); - } else { - axis = (angle / std::sin(angle / 2.0)) * e.vec(); - } - errAvg += axis; - } - errAvg /= double(n); - - if (errAvg.norm() < tol) break; - double errNorm = errAvg.norm(); - Eigen::Quaterniond dq - - if (errNorm > tol) { - dq.w() = std::cos(errNorm/2.0); - dq.vec() = std::sin(errNorm/2.0) * (errAvg/errNorm); - } else { - dq = Eigen::Quaterniond::Identity(); - } - mean_q = dq * mean_q; - mean_q.normalize(); - } - return mean_q; -} - -Eigen::Vector37d mean_set( - const std::vector& setPoints, - double tol, - int maxIter -) { - int n = int(setPoints.size()); - - Eigen::Vector3d posAvg = Eigen::Vector3d::Zero(); - Eigen::Vector3d velAvg = Eigen::Vector3d::Zero(); - Eigen::Vector3d angVelAvg = Eigen::Vector3d::Zero(); - Eigen::Vector9d inAvg = Eigen::Vector9d::Zero(); - Eigen::Vector6d amAvg = Eigen::Vector6d::Zero(); - Eigen::Vector6d dAvg = Eigen::Vector6d::Zero(); - Eigen::Vector4d gAvg = Eigen::Vector4d::Zero(); - - std::vector quats; - quats.reserve(n); - - for (const auto& s : setPoints) { - posAvg += s.position; - velAvg += s.velocity; - angVelAvg += s.angular_velocity; - inAvg += s.inertia; - amAvg += s.added_mass; - dAvg += s.damping; - gAvg += s.g_eta; - quats.push_back(s.orientation); - } - posAvg /= double(n); - velAvg /= double(n); - angVelAvg /= double(n); - inAvg /= double(n); - amAvg /= double(n); - dAvg /= double(n); - gAvg /= double(n); - Eigen::Quaterniond qMean = quaternion_mean(quats, tol, maxIter); - - AUVState meanState; - meanState.position = posAvg; - meanState.orientation = qMean; - meanState.velocity = velAvg; - meanState.angular_velocity = angVelAvg; - meanState.inertia = inAvg; - meanState.added_mass = amAvg; - meanState.damping = dAvg; - meanState.g_eta = gAvg; - - return meanState.asVector(); -} - -Eigen::Vector3d mean_measurement(const std::vector& setPoints) { - Eigen::Vector3d avg = Eigen::Vector3d::Zero(); - for (const auto& m : setPoints) avg += m.measurement; - return avg / double(setPoints.size()); -} - -Eigen::Matrix37d covariance_set( - const std::vector& setPoints, - const Eigen::Vector37d& meanVec, - double tol -) { - int n = int(setPoints.size()); - AUVState meanState; - meanState.fillStates(meanVec); - std::vector quats; - quats.reserve(n); - - for (const auto& s : setPoints) quats.push_back(s.orientation); - meanState.orientation = quaternion_mean(quats, tol); - - Eigen::Matrix37d cov = Eigen::Matrix37d::Zero(); - for (const auto& s : setPoints) { - Eigen::Vector37d d = Eigen::Vector37d::Zero(); - - d.segment<3>(0) = s.position - meanState.position; - - Eigen::Quaterniond e = s.orientation * meanState.orientation.conjugate(); - double w = std::clamp(e.w(), -1.0, 1.0); - double angle = 2 * std::acos(w); - - - Eigen::Vector3d err; - if (std::abs(angle) < tol) err.setZero(); - else err = (angle / std::sin(angle/2.0)) * e.vec(); - - d.segment<3>(3) = err; - - d.segment<3>(6) = s.velocity - meanState.velocity; - d.segment<3>(9) = s.angular_velocity - meanState.angular_velocity; - d.segment<9>(12) = s.inertia - meanState.inertia; - d.segment<6>(21) = s.added_mass - meanState.added_mass; - d.segment<6>(27) = s.damping - meanState.damping; - d.segment<4>(33) = s.g_eta - meanState.g_eta; - cov += d * d.transpose(); - } - return cov / double(n); -} - -Eigen::Matrix3d covariance_measurement( - const std::vector& setPoints, - const Eigen::Vector3d& mean -) { - Eigen::Matrix3d cov = Eigen::Matrix3d::Zero(); - for (const auto& m : setPoints) { - Eigen::Vector3d d = m.measurement - mean; - cov += d * d.transpose(); - } - return cov / double(setPoints.size()); -} - -Eigen::Matrix cross_covariance( - const std::vector& setY, - const Eigen::Vector37d& meanY, - const std::vector& setZ, - const Eigen::Vector3d& meanZ, - double tol -) { - int n = int(setY.size()); - AUVState meanState; - meanState.fill_states(meanY); - std::vector quats; - quats.reserve(n); - for (const auto& s : setY) quats.push_back(s.orientation); - meanState.orientation = quaternion_cean(quats, tol); - - Eigen::Matrix cov = Eigen::Matrix::Zero(); - for (size_t i = 0; i < setY.size(); ++i) { - const auto& s = setY[i]; - - Eigen::Vector37d dY = Eigen::Vector37d::Zero(); - dY.segment<3>(0) = s.position - meanState.position; - Eigen::Quaterniond e = s.orientation * meanState.orientation.conjugate(); - double w = std::clamp(e.w(), -1.0, 1.0); - double angle = 2 * std::acos(w); - Eigen::Vector3d err; - if (std::abs(angle) < tol) err.setZero(); - else err = (angle / std::sin(angle/2.0)) * e.vec(); - dY.segment<3>(3) = err; - dY.segment<3>(6) = s.velocity - meanState.velocity; - dY.segment<3>(9) = s.angular_velocity - meanState.angular_velocity; - dY.segment<9>(12) = s.inertia - meanState.inertia; - dY.segment<6>(21) = s.added_mass - meanState.added_mass; - dY.segment<6>(27) = s.damping - meanState.damping; - dY.segment<4>(33) = s.g_eta - meanState.g_eta; - - Eigen::Vector3d dZ = setZ[i].measurement - meanZ; - cov += dY * dZ.transpose(); - } - return cov / double(n); -} \ No newline at end of file From 1d34174c23bc8c4b01fc8bb70a8c18bb6808ef73 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Sun, 5 Oct 2025 18:54:00 +0000 Subject: [PATCH 106/290] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- navigation/eskf/include/eskf/typedefs.hpp | 3 ++- navigation/eskf/src/eskf.cpp | 33 +++++++++++++++-------- 2 files changed, 24 insertions(+), 12 deletions(-) diff --git a/navigation/eskf/include/eskf/typedefs.hpp b/navigation/eskf/include/eskf/typedefs.hpp index 4dbdb6da3..d6433d46d 100644 --- a/navigation/eskf/include/eskf/typedefs.hpp +++ b/navigation/eskf/include/eskf/typedefs.hpp @@ -60,7 +60,8 @@ struct state_quat { euler_diff = (quat * other.quat.inverse()) .toRotationMatrix() - .eulerAngles(0, 1, 2) + Eigen::Vector3d(-M_PI, M_PI, -M_PI); + .eulerAngles(0, 1, 2) + + Eigen::Vector3d(-M_PI, M_PI, -M_PI); vec << pos - other.pos, vel - other.vel, euler_diff, gyro_bias - other.gyro_bias, accel_bias - other.accel_bias, diff --git a/navigation/eskf/src/eskf.cpp b/navigation/eskf/src/eskf.cpp index 3838e2f1a..4a5501405 100644 --- a/navigation/eskf/src/eskf.cpp +++ b/navigation/eskf/src/eskf.cpp @@ -61,12 +61,14 @@ Eigen::Matrix3x19d ESKF::calculate_hx() { Eigen::Vector3d q_vec(q.x(), q.y(), q.z()); Eigen::Matrix3d I3 = Eigen::Matrix3d::Identity(); - Eigen::Matrix dhdq; - dhdq.col(0) = 2*( qw*v_n + q_vec.cross(v_n) ); - dhdq.block<3,3>(0,1) = 2*( q_vec.dot(v_n)*I3 + q_vec*v_n.transpose() - v_n*q_vec.transpose() - qw*skew(v_n) ); + Eigen::Matrix dhdq; + dhdq.col(0) = 2 * (qw * v_n + q_vec.cross(v_n)); + dhdq.block<3, 3>(0, 1) = + 2 * (q_vec.dot(v_n) * I3 + q_vec * v_n.transpose() - + v_n * q_vec.transpose() - qw * skew(v_n)); // Assign quaternion derivative (3x4 block at columns 6:9) - Hx.block<3,4>(0,6) = dhdq; + Hx.block<3, 4>(0, 6) = dhdq; return Hx; } @@ -83,22 +85,28 @@ Eigen::Matrix3x18d ESKF::calculate_h_jacobian() { Eigen::Vector3d ESKF::calculate_h() { Eigen::Vector3d h; - Eigen::Matrix3d R_bn = current_nom_state_.quat.normalized().toRotationMatrix().transpose(); + Eigen::Matrix3d R_bn = + current_nom_state_.quat.normalized().toRotationMatrix().transpose(); h = R_bn * current_nom_state_.vel; - //0.027293, 0.028089, 0.028089, 0.00255253, 0.00270035, 0.00280294, + // 0.027293, 0.028089, 0.028089, 0.00255253, 0.00270035, 0.00280294, return h; } void ESKF::nominal_state_discrete(const imu_measurement& imu_meas, const double dt) { - Eigen::Vector3d acc = current_nom_state_.quat.normalized().toRotationMatrix() * (imu_meas.accel - current_nom_state_.accel_bias) + current_nom_state_.gravity; + Eigen::Vector3d acc = + current_nom_state_.quat.normalized().toRotationMatrix() * + (imu_meas.accel - current_nom_state_.accel_bias) + + current_nom_state_.gravity; Eigen::Vector3d gyro = (imu_meas.gyro - current_nom_state_.gyro_bias) * dt; - current_nom_state_.pos = current_nom_state_.pos + current_nom_state_.vel * dt + 0.5 * sq(dt) * acc; + current_nom_state_.pos = current_nom_state_.pos + + current_nom_state_.vel * dt + 0.5 * sq(dt) * acc; current_nom_state_.vel = current_nom_state_.vel + dt * acc; - current_nom_state_.quat = (current_nom_state_.quat * vector3d_to_quaternion(gyro)); + current_nom_state_.quat = + (current_nom_state_.quat * vector3d_to_quaternion(gyro)); current_nom_state_.quat.normalize(); current_nom_state_.gyro_bias = current_nom_state_.gyro_bias; @@ -132,7 +140,8 @@ void ESKF::error_state_prediction(const imu_measurement& imu_meas, std::tie(A_d, GQG_d) = van_loan_discretization(A_c, G_c, dt); state_euler next_error_state; - current_error_state_.covariance = A_d * current_error_state_.covariance * A_d.transpose() + GQG_d; + current_error_state_.covariance = + A_d * current_error_state_.covariance * A_d.transpose() + GQG_d; } void ESKF::NIS(const Eigen::Vector3d& innovation, const Eigen::Matrix3d& S) { @@ -160,7 +169,9 @@ void ESKF::measurement_update(const dvl_measurement& dvl_meas) { void ESKF::injection_and_reset() { current_nom_state_.pos = current_nom_state_.pos + current_error_state_.pos; current_nom_state_.vel = current_nom_state_.vel + current_error_state_.vel; - current_nom_state_.quat = current_nom_state_.quat * vector3d_to_quaternion(current_error_state_.euler); + current_nom_state_.quat = + current_nom_state_.quat * + vector3d_to_quaternion(current_error_state_.euler); current_nom_state_.quat.normalize(); current_nom_state_.gyro_bias = current_nom_state_.gyro_bias + current_error_state_.gyro_bias; From 9f9c916d305693a4be9bf176a2a6e7ddcdc60e99 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Anders=20H=C3=B8gden?= Date: Tue, 7 Oct 2025 10:41:25 +0200 Subject: [PATCH 107/290] ci: add node test for eskf --- .github/workflows/ros-node-tests.yml | 5 +++ navigation/eskf/CMakeLists.txt | 2 -- navigation/eskf/include/eskf/eskf_ros.hpp | 1 - navigation/eskf/launch/eskf.launch.py | 1 + tests/ros_node_tests/eskf_node_test.sh | 39 +++++++++++++++++++++++ 5 files changed, 45 insertions(+), 3 deletions(-) create mode 100644 tests/ros_node_tests/eskf_node_test.sh diff --git a/.github/workflows/ros-node-tests.yml b/.github/workflows/ros-node-tests.yml index 4200df85d..1ae75e093 100644 --- a/.github/workflows/ros-node-tests.yml +++ b/.github/workflows/ros-node-tests.yml @@ -10,6 +10,11 @@ on: - cron: '0 1 * * *' # Runs daily at 01:00 UTC jobs: call_reusable_workflow: + strategy: + matrix: + test_script: + - "tests/ros_node_tests/dp_node_test.sh" + - "tests/ros_node_tests/eskf_node_test.sh" uses: vortexntnu/vortex-ci/.github/workflows/reusable-ros2-simulator-test.yml@main with: vcs_repos_file: "tests/dependencies.repos" diff --git a/navigation/eskf/CMakeLists.txt b/navigation/eskf/CMakeLists.txt index c431c235f..6c8167609 100644 --- a/navigation/eskf/CMakeLists.txt +++ b/navigation/eskf/CMakeLists.txt @@ -18,7 +18,6 @@ find_package(tf2 REQUIRED) find_package(vortex_msgs REQUIRED) find_package(spdlog REQUIRED) find_package(fmt REQUIRED) -find_package(stonefish_ros2 REQUIRED) if(NOT DEFINED EIGEN3_INCLUDE_DIR) set(EIGEN3_INCLUDE_DIR ${EIGEN3_INCLUDE_DIRS}) @@ -43,7 +42,6 @@ ament_target_dependencies(eskf_node vortex_msgs spdlog fmt - stonefish_ros2 ) target_link_libraries(eskf_node diff --git a/navigation/eskf/include/eskf/eskf_ros.hpp b/navigation/eskf/include/eskf/eskf_ros.hpp index 424e583f7..5975d9cc7 100644 --- a/navigation/eskf/include/eskf/eskf_ros.hpp +++ b/navigation/eskf/include/eskf/eskf_ros.hpp @@ -11,7 +11,6 @@ #include #include #include -#include #include "eskf/eskf.hpp" #include "eskf/typedefs.hpp" #include "spdlog/spdlog.h" diff --git a/navigation/eskf/launch/eskf.launch.py b/navigation/eskf/launch/eskf.launch.py index 84284f804..1e9d64759 100644 --- a/navigation/eskf/launch/eskf.launch.py +++ b/navigation/eskf/launch/eskf.launch.py @@ -14,6 +14,7 @@ def generate_launch_description(): package="eskf", executable="eskf_node", name="eskf_node", + namespace="orca", parameters=[ eskf_params, ], diff --git a/tests/ros_node_tests/eskf_node_test.sh b/tests/ros_node_tests/eskf_node_test.sh new file mode 100644 index 000000000..35ba6e91e --- /dev/null +++ b/tests/ros_node_tests/eskf_node_test.sh @@ -0,0 +1,39 @@ +#!/bin/bash +set -e +set -o pipefail + +echo "Testing that the ESKF node is able to start up and publish odom" + +# Load ROS 2 environment +echo "Setting up ROS 2 environment..." +. /opt/ros/humble/setup.sh +. "${WORKSPACE:-$HOME/ros2_ws}/install/setup.bash" + +# Function to terminate processes safely on error +cleanup() { + echo "Error detected. Cleaning up..." + kill -TERM -"$ESKF_PID" || true + exit 1 +} +trap cleanup ERR + +# Launch eskf node +setsid ros2 launch eskf eskf.launch.py & +ESKF_PID=$! +echo "Launched eskf with PID: $ESKF_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 + +# Check if eskf correctly publishes odom +echo "Waiting for odom data..." +timeout 10s ros2 topic echo /orca/odom --once +echo "Got odom data" + +# Terminate processes +kill -TERM -"$ESKF_PID" + +echo "Test completed successfully." From 587137deeace3d2708654f45706642dab61ed55e Mon Sep 17 00:00:00 2001 From: Talha Nauman Choudhry Date: Tue, 7 Oct 2025 20:28:48 +0200 Subject: [PATCH 108/290] feat: Added fancy text for starting filter --- navigation/eskf/include/eskf/eskf.hpp | 12 ++---------- navigation/eskf/include/eskf/eskf_utils.hpp | 20 ++++++++++++++++++++ navigation/eskf/include/eskf/typedefs.hpp | 11 +++++------ navigation/eskf/launch/eskf.launch.py | 2 +- navigation/eskf/package.xml | 4 ++-- navigation/eskf/src/eskf.cpp | 15 +-------------- navigation/eskf/src/eskf_ros.cpp | 10 +++++++++- navigation/eskf/src/eskf_utils.cpp | 13 +++++++++++++ 8 files changed, 53 insertions(+), 34 deletions(-) diff --git a/navigation/eskf/include/eskf/eskf.hpp b/navigation/eskf/include/eskf/eskf.hpp index 214f462c4..87221a3b6 100644 --- a/navigation/eskf/include/eskf/eskf.hpp +++ b/navigation/eskf/include/eskf/eskf.hpp @@ -24,8 +24,8 @@ class ESKF { std::pair dvl_update( const dvl_measurement& dvl_meas); - // NIS - double NIS_; + // Normalized Innovation Squared + double NIS_{}; private: // @brief Predict the nominal state @@ -63,23 +63,15 @@ class ESKF { const Eigen::Matrix18x12d& G_c, const double dt); - // @brief Calculate the delta quaternion matrix - // @param nom_state: Nominal state - // @return Delta quaternion matrix - Eigen::Matrix4x3d calculate_q_delta(); - // @brief Calculate the measurement matrix jakobian - // @param nom_state: Nominal state // @return Measurement matrix Eigen::Matrix3x19d calculate_hx(); // @brief Calculate the full measurement matrix - // @param nom_state: Nominal state // @return Measurement matrix Eigen::Matrix3x18d calculate_h_jacobian(); // @brief Calculate the measurement - // @param nom_state: Nominal state // @return Measurement Eigen::Vector3d calculate_h(); diff --git a/navigation/eskf/include/eskf/eskf_utils.hpp b/navigation/eskf/include/eskf/eskf_utils.hpp index 4fcaed412..4481f2787 100644 --- a/navigation/eskf/include/eskf/eskf_utils.hpp +++ b/navigation/eskf/include/eskf/eskf_utils.hpp @@ -5,14 +5,34 @@ #include "eigen3/Eigen/Dense" #include "eskf/typedefs.hpp" +// @brief Compute the skew-symmetric matrix of a vector +// @param v: Input vector +// @return Skew-symmetric matrix Eigen::Matrix3d skew(const Eigen::Vector3d& v); +// @brief Square a value +// @param value: Input value +// @return Squared value double sq(const double& value); +// @brief Normalize an angle to the range [-pi, pi] +// @param angle: Input angle in radians +// @return Normalized angle in radians double ssa(const double& angle); +// @brief Calculate the transformation matrix using a quaternion +// @param quat: Input quaternion +// @return Transformation matrix +Eigen::Matrix4x3d calculate_T_q(const Eigen::Quaterniond& quat); + +// @brief Convert a rotation vector to a quaternion +// @param vector: Input rotation vector +// @return Corresponding quaternion Eigen::Quaterniond vector3d_to_quaternion(const Eigen::Vector3d& vector); +// @brief Convert Euler angles to a quaternion +// @param euler: Input Euler angles (roll, pitch, yaw) in radians +// @return Corresponding quaternion Eigen::Quaterniond euler_to_quaternion(const Eigen::Vector3d& euler); #endif // ESKF_UTILS_HPP diff --git a/navigation/eskf/include/eskf/typedefs.hpp b/navigation/eskf/include/eskf/typedefs.hpp index d6433d46d..54a88032a 100644 --- a/navigation/eskf/include/eskf/typedefs.hpp +++ b/navigation/eskf/include/eskf/typedefs.hpp @@ -36,16 +36,15 @@ Eigen::Matrix createDiagonalMatrix( return Eigen::Map>(diag.data()) .asDiagonal(); } - struct state_quat { - Eigen::Vector3d pos = Eigen::Vector3d(5.58, 0.66, 0.12); + Eigen::Vector3d pos = Eigen::Vector3d::Zero(); Eigen::Vector3d vel = Eigen::Vector3d::Zero(); - Eigen::Quaterniond quat = Eigen::Quaterniond(0.98, -0.047, 0.028, -0.18); + Eigen::Quaterniond quat = Eigen::Quaterniond::Identity(); Eigen::Vector3d gyro_bias = Eigen::Vector3d::Zero(); Eigen::Vector3d accel_bias = Eigen::Vector3d::Zero(); - Eigen::Vector3d gravity = Eigen::Vector3d::Zero(); + Eigen::Vector3d gravity = Eigen::Vector3d(0, 0, 9.81); - state_quat() { gravity << 0, 0, 9.81; } + state_quat() = default; Eigen::Vector19d as_vector() const { Eigen::Vector19d vec; @@ -87,7 +86,7 @@ struct state_euler { Eigen::Vector3d euler = Eigen::Vector3d::Zero(); Eigen::Vector3d gyro_bias = Eigen::Vector3d::Zero(); Eigen::Vector3d accel_bias = Eigen::Vector3d::Zero(); - Eigen::Vector3d gravity = Eigen::Vector3d::Zero(); + Eigen::Vector3d gravity = Eigen::Vector3d(0, 0, 9.81); Eigen::Matrix18d covariance = Eigen::Matrix18d::Zero(); diff --git a/navigation/eskf/launch/eskf.launch.py b/navigation/eskf/launch/eskf.launch.py index 1e9d64759..d7b3a8a4e 100644 --- a/navigation/eskf/launch/eskf.launch.py +++ b/navigation/eskf/launch/eskf.launch.py @@ -14,7 +14,7 @@ def generate_launch_description(): package="eskf", executable="eskf_node", name="eskf_node", - namespace="orca", + # namespace="orca", parameters=[ eskf_params, ], diff --git a/navigation/eskf/package.xml b/navigation/eskf/package.xml index d3d8dc416..015e6f71c 100644 --- a/navigation/eskf/package.xml +++ b/navigation/eskf/package.xml @@ -2,9 +2,9 @@ eskf - 1.0.0 + 2.0.0 Error-state Kalman filter - talhanc + talhanc MIT ament_cmake diff --git a/navigation/eskf/src/eskf.cpp b/navigation/eskf/src/eskf.cpp index 4a5501405..8507244e9 100644 --- a/navigation/eskf/src/eskf.cpp +++ b/navigation/eskf/src/eskf.cpp @@ -31,19 +31,6 @@ std::pair ESKF::van_loan_discretization( return {A_d, GQG_d}; } -Eigen::Matrix4x3d ESKF::calculate_q_delta() { - Eigen::Matrix4x3d q_delta_theta = Eigen::Matrix4x3d::Zero(); - double qw = current_nom_state_.quat.w(); - double qx = current_nom_state_.quat.x(); - double qy = current_nom_state_.quat.y(); - double qz = current_nom_state_.quat.z(); - - q_delta_theta << -qx, -qy, -qz, qw, -qz, qy, qz, qw, -qx, -qy, qx, qw; - - q_delta_theta *= 0.5; - return q_delta_theta; -} - Eigen::Matrix3x19d ESKF::calculate_hx() { Eigen::Matrix3x19d Hx = Eigen::Matrix3x19d::Zero(); @@ -76,7 +63,7 @@ Eigen::Matrix3x19d ESKF::calculate_hx() { Eigen::Matrix3x18d ESKF::calculate_h_jacobian() { Eigen::Matrix19x18d x_delta = Eigen::Matrix19x18d::Zero(); x_delta.block<6, 6>(0, 0) = Eigen::Matrix6d::Identity(); - x_delta.block<4, 3>(6, 6) = calculate_q_delta(); + x_delta.block<4, 3>(6, 6) = calculate_T_q(current_nom_state_.quat); x_delta.block<9, 9>(10, 9) = Eigen::Matrix9d::Identity(); Eigen::Matrix3x18d H = calculate_hx() * x_delta; diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp index c379732e3..a79ae09bb 100644 --- a/navigation/eskf/src/eskf_ros.cpp +++ b/navigation/eskf/src/eskf_ros.cpp @@ -12,7 +12,15 @@ ESKFNode::ESKFNode() : Node("eskf_node") { set_parameters(); - spdlog::info("ESKF Node Initialized"); + auto start_message{R"( + ________ ______ ___ ____ ________ + |_ __ |.' ____ \ |_ ||_ _| |_ __ | + | |_ \_|| (___ \_| | |_/ / | |_ \_| + | _| _ _.____`. | __'. | _| + _| |__/ || \____) | _| | \ \_ _| |_ + |________| \______.'|____||____||_____| + )"}; + spdlog::info("\n{}", start_message); } void ESKFNode::set_subscribers_and_publisher() { diff --git a/navigation/eskf/src/eskf_utils.cpp b/navigation/eskf/src/eskf_utils.cpp index 88d04c3d5..0b9ae007f 100644 --- a/navigation/eskf/src/eskf_utils.cpp +++ b/navigation/eskf/src/eskf_utils.cpp @@ -17,6 +17,19 @@ double ssa(const double& angle) { return angle_ssa; } +Eigen::Matrix4x3d calculate_T_q(const Eigen::Quaterniond& quat) { + Eigen::Matrix4x3d T_q = Eigen::Matrix4x3d::Zero(); + double qw = quat.w(); + double qx = quat.x(); + double qy = quat.y(); + double qz = quat.z(); + + T_q << -qx, -qy, -qz, qw, -qz, qy, qz, qw, -qx, -qy, qx, qw; + + T_q *= 0.5; + return T_q; +} + Eigen::Quaterniond vector3d_to_quaternion(const Eigen::Vector3d& vector) { double angle = vector.norm(); if (angle < 1e-8) { From f3b1429859dc9fac60e1ebeda7d4e436dd551b6f Mon Sep 17 00:00:00 2001 From: Talha Nauman Choudhry Date: Tue, 7 Oct 2025 22:06:01 +0200 Subject: [PATCH 109/290] fix: moved the fancy text variable outside the initiation --- navigation/eskf/CMakeLists.txt | 34 +++++++++++++++------ navigation/eskf/include/eskf/eskf.hpp | 6 ++-- navigation/eskf/include/eskf/eskf_ros.hpp | 17 ++++++----- navigation/eskf/include/eskf/eskf_utils.hpp | 4 +-- navigation/eskf/include/eskf/typedefs.hpp | 10 +++--- navigation/eskf/src/eskf_node.cpp | 9 ------ navigation/eskf/src/eskf_ros.cpp | 25 +++++++++------ navigation/eskf/src/eskf_utils.cpp | 4 +-- 8 files changed, 61 insertions(+), 48 deletions(-) delete mode 100644 navigation/eskf/src/eskf_node.cpp diff --git a/navigation/eskf/CMakeLists.txt b/navigation/eskf/CMakeLists.txt index 6c8167609..8fc8a02cc 100644 --- a/navigation/eskf/CMakeLists.txt +++ b/navigation/eskf/CMakeLists.txt @@ -11,6 +11,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) find_package(nav_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(Eigen3 REQUIRED) @@ -22,19 +23,22 @@ find_package(fmt REQUIRED) if(NOT DEFINED EIGEN3_INCLUDE_DIR) set(EIGEN3_INCLUDE_DIR ${EIGEN3_INCLUDE_DIRS}) endif() + include_directories(${EIGEN3_INCLUDE_DIR}) include_directories(include) -add_executable(eskf_node +set(LIB_NAME "${PROJECT_NAME}_component") + +add_library(${LIB_NAME} SHARED src/eskf.cpp src/eskf_ros.cpp - src/eskf_node.cpp src/eskf_utils.cpp ) -ament_target_dependencies(eskf_node +ament_target_dependencies(${LIB_NAME} PUBLIC rclcpp + rclcpp_components geometry_msgs nav_msgs Eigen3 @@ -44,17 +48,29 @@ ament_target_dependencies(eskf_node fmt ) -target_link_libraries(eskf_node - fmt::fmt +rclcpp_components_register_node( + ${LIB_NAME} + PLUGIN "ESKFNode" + 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 - eskf_node - DESTINATION lib/${PROJECT_NAME}) +install( + DIRECTORY include/ + DESTINATION include +) install(DIRECTORY - config launch + config DESTINATION share/${PROJECT_NAME}/ ) diff --git a/navigation/eskf/include/eskf/eskf.hpp b/navigation/eskf/include/eskf/eskf.hpp index 87221a3b6..ea3a72159 100644 --- a/navigation/eskf/include/eskf/eskf.hpp +++ b/navigation/eskf/include/eskf/eskf.hpp @@ -76,13 +76,13 @@ class ESKF { Eigen::Vector3d calculate_h(); // Process noise covariance matrix - Eigen::Matrix12d Q_; + Eigen::Matrix12d Q_{}; // Member variable for the current error state - state_euler current_error_state_; + state_euler current_error_state_{}; // Member variable for the current nominal state - state_quat current_nom_state_; + state_quat current_nom_state_{}; }; #endif // ESKF_HPP diff --git a/navigation/eskf/include/eskf/eskf_ros.hpp b/navigation/eskf/include/eskf/eskf_ros.hpp index 5975d9cc7..431e93ea1 100644 --- a/navigation/eskf/include/eskf/eskf_ros.hpp +++ b/navigation/eskf/include/eskf/eskf_ros.hpp @@ -17,7 +17,8 @@ class ESKFNode : public rclcpp::Node { public: - explicit ESKFNode(); + explicit ESKFNode( + const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); private: // @brief Callback function for the imu topic @@ -51,23 +52,23 @@ class ESKFNode : public rclcpp::Node { rclcpp::TimerBase::SharedPtr odom_pub_timer_; - state_quat nom_state_; + state_quat nom_state_{}; - state_euler error_state_; + state_euler error_state_{}; - imu_measurement imu_meas_; + imu_measurement imu_meas_{}; - dvl_measurement dvl_meas_; + dvl_measurement dvl_meas_{}; - eskf_params eskf_params_; + eskf_params eskf_params_{}; std::unique_ptr eskf_; bool first_imu_msg_received_ = false; - Eigen::Matrix3d R_imu_eskf_; + Eigen::Matrix3d R_imu_eskf_{}; - rclcpp::Time last_imu_time_; + rclcpp::Time last_imu_time_{}; }; #endif // ESKF_ROS_HPP diff --git a/navigation/eskf/include/eskf/eskf_utils.hpp b/navigation/eskf/include/eskf/eskf_utils.hpp index 4481f2787..0fe06ee7a 100644 --- a/navigation/eskf/include/eskf/eskf_utils.hpp +++ b/navigation/eskf/include/eskf/eskf_utils.hpp @@ -13,12 +13,12 @@ Eigen::Matrix3d skew(const Eigen::Vector3d& v); // @brief Square a value // @param value: Input value // @return Squared value -double sq(const double& value); +double sq(double value); // @brief Normalize an angle to the range [-pi, pi] // @param angle: Input angle in radians // @return Normalized angle in radians -double ssa(const double& angle); +double ssa(double angle); // @brief Calculate the transformation matrix using a quaternion // @param quat: Input quaternion diff --git a/navigation/eskf/include/eskf/typedefs.hpp b/navigation/eskf/include/eskf/typedefs.hpp index 54a88032a..77432ecca 100644 --- a/navigation/eskf/include/eskf/typedefs.hpp +++ b/navigation/eskf/include/eskf/typedefs.hpp @@ -47,15 +47,15 @@ struct state_quat { state_quat() = default; Eigen::Vector19d as_vector() const { - Eigen::Vector19d vec; + Eigen::Vector19d vec{}; vec << pos, vel, quat.w(), quat.x(), quat.y(), quat.z(), gyro_bias, accel_bias, gravity; return vec; } Eigen::Vector18d nees_error(const state_quat& other) const { - Eigen::Vector18d vec; - Eigen::Vector3d euler_diff; + Eigen::Vector18d vec{}; + Eigen::Vector3d euler_diff{}; euler_diff = (quat * other.quat.inverse()) .toRotationMatrix() @@ -69,7 +69,7 @@ struct state_quat { } state_quat operator-(const state_quat& other) const { - state_quat diff; + state_quat diff{}; diff.pos = pos - other.pos; diff.vel = vel - other.vel; diff.quat = quat * other.quat.inverse(); @@ -91,7 +91,7 @@ struct state_euler { Eigen::Matrix18d covariance = Eigen::Matrix18d::Zero(); Eigen::Vector18d as_vector() const { - Eigen::Vector18d vec; + Eigen::Vector18d vec{}; vec << pos, vel, euler, gyro_bias, accel_bias, gravity; return vec; } diff --git a/navigation/eskf/src/eskf_node.cpp b/navigation/eskf/src/eskf_node.cpp deleted file mode 100644 index 196fa7916..000000000 --- a/navigation/eskf/src/eskf_node.cpp +++ /dev/null @@ -1,9 +0,0 @@ -#include "eskf/eskf_ros.hpp" - -int main(int argc, char** argv) { - rclcpp::init(argc, argv); - spdlog::info("Starting ESKF Node"); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp index a79ae09bb..ce9e229ed 100644 --- a/navigation/eskf/src/eskf_ros.cpp +++ b/navigation/eskf/src/eskf_ros.cpp @@ -1,9 +1,20 @@ #include "eskf/eskf_ros.hpp" #include +#include #include "eskf/eskf_utils.hpp" #include "eskf/typedefs.hpp" -ESKFNode::ESKFNode() : Node("eskf_node") { +auto start_message{R"( + ________ ______ ___ ____ ________ + |_ __ |.' ____ \ |_ ||_ _| |_ __ | + | |_ \_|| (___ \_| | |_/ / | |_ \_| + | _| _ _.____`. | __'. | _| + _| |__/ || \____) | _| | \ \_ _| |_ + |________| \______.'|____||____||_____| +)"}; + +ESKFNode::ESKFNode(const rclcpp::NodeOptions& options) + : Node("eskf_node", options) { time_step = std::chrono::milliseconds(1); odom_pub_timer_ = this->create_wall_timer( time_step, std::bind(&ESKFNode::publish_odom, this)); @@ -12,15 +23,7 @@ ESKFNode::ESKFNode() : Node("eskf_node") { set_parameters(); - auto start_message{R"( - ________ ______ ___ ____ ________ - |_ __ |.' ____ \ |_ ||_ _| |_ __ | - | |_ \_|| (___ \_| | |_/ / | |_ \_| - | _| _ _.____`. | __'. | _| - _| |__/ || \____) | _| | \ \_ _| |_ - |________| \______.'|____||____||_____| - )"}; - spdlog::info("\n{}", start_message); + spdlog::info(start_message); } void ESKFNode::set_subscribers_and_publisher() { @@ -149,3 +152,5 @@ void ESKFNode::publish_odom() { odom_msg.header.stamp = this->now(); odom_pub_->publish(odom_msg); } + +RCLCPP_COMPONENTS_REGISTER_NODE(ESKFNode) diff --git a/navigation/eskf/src/eskf_utils.cpp b/navigation/eskf/src/eskf_utils.cpp index 0b9ae007f..e7dc4e5e3 100644 --- a/navigation/eskf/src/eskf_utils.cpp +++ b/navigation/eskf/src/eskf_utils.cpp @@ -8,10 +8,10 @@ Eigen::Matrix3d skew(const Eigen::Vector3d& v) { return S; } -double sq(const double& value) { +double sq(const double value) { return value * value; } -double ssa(const double& angle) { +double ssa(const double angle) { double result = fmod(angle + M_PI, 2 * M_PI); double angle_ssa = result < 0 ? result + M_PI : result - M_PI; return angle_ssa; From 0659e428cb9f4cc2109bc906cf0263864a703e5c Mon Sep 17 00:00:00 2001 From: Ahmed Date: Wed, 15 Oct 2025 15:43:39 +0200 Subject: [PATCH 110/290] Fixed some of the suggested comments, add debug flag --- navigation/eskf/CMakeLists.txt | 4 ++++ navigation/eskf/include/eskf/typedefs.hpp | 2 +- navigation/eskf/src/eskf_ros.cpp | 8 +++++++- navigation/eskf/src/eskf_utils.cpp | 4 ++-- 4 files changed, 14 insertions(+), 4 deletions(-) diff --git a/navigation/eskf/CMakeLists.txt b/navigation/eskf/CMakeLists.txt index 8fc8a02cc..ec4feea05 100644 --- a/navigation/eskf/CMakeLists.txt +++ b/navigation/eskf/CMakeLists.txt @@ -9,6 +9,10 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release) +endif() + find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) diff --git a/navigation/eskf/include/eskf/typedefs.hpp b/navigation/eskf/include/eskf/typedefs.hpp index 77432ecca..b96f8dfbf 100644 --- a/navigation/eskf/include/eskf/typedefs.hpp +++ b/navigation/eskf/include/eskf/typedefs.hpp @@ -31,7 +31,7 @@ typedef Eigen::Matrix Vector15d; } // namespace Eigen template -Eigen::Matrix createDiagonalMatrix( +Eigen::Matrix create_diagonal_matrix( const std::vector& diag) { return Eigen::Map>(diag.data()) .asDiagonal(); diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp index ce9e229ed..5542af533 100644 --- a/navigation/eskf/src/eskf_ros.cpp +++ b/navigation/eskf/src/eskf_ros.cpp @@ -24,6 +24,9 @@ ESKFNode::ESKFNode(const rclcpp::NodeOptions& options) set_parameters(); spdlog::info(start_message); + #ifndef NDEBUG + spdlog::info("__________________________Debug mode is enabled______________________"); + #endif } void ESKFNode::set_subscribers_and_publisher() { @@ -76,7 +79,7 @@ void ESKFNode::set_parameters() { std::vector diag_p_init = this->declare_parameter>("diag_p_init"); - Eigen::Matrix18d P = createDiagonalMatrix<18>(diag_p_init); + Eigen::Matrix18d P = create_diagonal_matrix<18>(diag_p_init); error_state_.covariance = P; } @@ -120,9 +123,12 @@ void ESKFNode::dvl_callback( std::tie(nom_state_, error_state_) = eskf_->dvl_update(dvl_meas_); + #ifndef NDEBUG + // Publish NIS std_msgs::msg::Float64 nis_msg; nis_msg.data = eskf_->NIS_; nis_pub_->publish(nis_msg); + #endif } void ESKFNode::publish_odom() { diff --git a/navigation/eskf/src/eskf_utils.cpp b/navigation/eskf/src/eskf_utils.cpp index e7dc4e5e3..02e8d9229 100644 --- a/navigation/eskf/src/eskf_utils.cpp +++ b/navigation/eskf/src/eskf_utils.cpp @@ -8,10 +8,10 @@ Eigen::Matrix3d skew(const Eigen::Vector3d& v) { return S; } -double sq(const double value) { +double sq(double value) { return value * value; } -double ssa(const double angle) { +double ssa(double angle) { double result = fmod(angle + M_PI, 2 * M_PI); double angle_ssa = result < 0 ? result + M_PI : result - M_PI; return angle_ssa; From 15dd969fea3e87916aef0e453562ca100d041ebd Mon Sep 17 00:00:00 2001 From: Ahmed Date: Sun, 19 Oct 2025 14:40:25 +0200 Subject: [PATCH 111/290] updating the published topic name --- navigation/eskf/config/eskf_params.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/navigation/eskf/config/eskf_params.yaml b/navigation/eskf/config/eskf_params.yaml index f17dcd3df..2293ae1e1 100644 --- a/navigation/eskf/config/eskf_params.yaml +++ b/navigation/eskf/config/eskf_params.yaml @@ -2,7 +2,7 @@ eskf_node: ros__parameters: imu_topic: /imu/data_raw dvl_topic: /orca/twist - odom_topic: odom + odom_topic: odom_ESKF diag_Q_std: [0.0124, 0.0124, 0.0124, 0.0026, 0.0026, 0.0026, 0.000392, 0.000392, 0.000392, 0.00001145, 0.0000145, 0.0000145] diag_p_init: [1.0, 1.0, 1.0, 0.5, 0.5, 0.5, 0.1, 0.1, 0.1, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001] imu_frame: [0.0, 0.0, -1.0, 0.0, -1.0, 0.0, -1.0, 0.0, 0.0] From fe06684d755ebe55086311e6f694a63c9e323e00 Mon Sep 17 00:00:00 2001 From: Ahmed Date: Sun, 19 Oct 2025 15:53:21 +0200 Subject: [PATCH 112/290] implemented the sensor concept requirement, added eskf.tpp so that I don't put the definition in the header --- navigation/eskf/include/eskf/eskf.hpp | 32 ++++++++-------- navigation/eskf/include/eskf/eskf.tpp | 25 +++++++++++++ navigation/eskf/include/eskf/eskf_ros.hpp | 2 +- navigation/eskf/include/eskf/typedefs.hpp | 27 ++++++++++---- navigation/eskf/src/eskf.cpp | 45 +++++++++++------------ navigation/eskf/src/eskf_ros.cpp | 10 ++--- 6 files changed, 90 insertions(+), 51 deletions(-) create mode 100644 navigation/eskf/include/eskf/eskf.tpp diff --git a/navigation/eskf/include/eskf/eskf.hpp b/navigation/eskf/include/eskf/eskf.hpp index ea3a72159..f9ef6284e 100644 --- a/navigation/eskf/include/eskf/eskf.hpp +++ b/navigation/eskf/include/eskf/eskf.hpp @@ -22,7 +22,7 @@ class ESKF { // @param dvl_meas: DVL measurement // @return Updated nominal state and error state std::pair dvl_update( - const dvl_measurement& dvl_meas); + const sensor_dvl& dvl_meas); // Normalized Innovation Squared double NIS_{}; @@ -47,9 +47,11 @@ class ESKF { // @param S: Innovation covariance matrix void NIS(const Eigen::Vector3d& innovation, const Eigen::Matrix3d& S); - // @brief Update the error state - // @param dvl_meas: DVL measurement - void measurement_update(const dvl_measurement& dvl_meas); + // @brief Update the error state using a generic sensor measurement model + // @tparam SensorT Type of the sensor model (must satisfy SensorModelConcept) + // @param meas Sensor measurement instance + template + void measurement_update(const SensorT& meas); // @brief Inject the error state into the nominal state and reset the error void injection_and_reset(); @@ -63,17 +65,6 @@ class ESKF { const Eigen::Matrix18x12d& G_c, const double dt); - // @brief Calculate the measurement matrix jakobian - // @return Measurement matrix - Eigen::Matrix3x19d calculate_hx(); - - // @brief Calculate the full measurement matrix - // @return Measurement matrix - Eigen::Matrix3x18d calculate_h_jacobian(); - - // @brief Calculate the measurement - // @return Measurement - Eigen::Vector3d calculate_h(); // Process noise covariance matrix Eigen::Matrix12d Q_{}; @@ -85,4 +76,15 @@ class ESKF { state_quat current_nom_state_{}; }; +// Measurement in world frame --> h(x) +Eigen::Vector3d calculate_h(const state_quat& current_nom_state_); + +// Jacobian of h(x) with respect to the error state --> H +Eigen::Matrix3x18d calculate_h_jacobian(const state_quat& current_nom_state_); + +// Jacobian of h(x) with respect to the nominal state --> Hx +Eigen::Matrix3x19d calculate_hx(const state_quat& current_nom_state_); + +#include "eskf.tpp" // including template implementation + #endif // ESKF_HPP diff --git a/navigation/eskf/include/eskf/eskf.tpp b/navigation/eskf/include/eskf/eskf.tpp new file mode 100644 index 000000000..81e01ba8e --- /dev/null +++ b/navigation/eskf/include/eskf/eskf.tpp @@ -0,0 +1,25 @@ + + +template +void ESKF::measurement_update(const SensorT& meas) +{ + Eigen::VectorXd innovation = meas.innovation(current_nom_state_); + Eigen::MatrixXd H = meas.jacobian(current_nom_state_); + Eigen::MatrixXd R = meas.noise_covariance(); + + Eigen::MatrixXd P = current_error_state_.covariance; + + Eigen::MatrixXd S = H * P * H.transpose() + R; + Eigen::MatrixXd K = P * H.transpose() * S.inverse(); + + #ifndef NDEBUG + NIS(innovation, S); + #endif + + current_error_state_.set_from_vector(K * innovation); + + Eigen::MatrixXd I_KH = Eigen::MatrixXd::Identity(P.rows(), P.cols()) - K * H; + current_error_state_.covariance = + I_KH * P * I_KH.transpose() + + K * R * K.transpose(); // Used joseph form for more stable calculations +} \ No newline at end of file diff --git a/navigation/eskf/include/eskf/eskf_ros.hpp b/navigation/eskf/include/eskf/eskf_ros.hpp index 431e93ea1..9220ae7bf 100644 --- a/navigation/eskf/include/eskf/eskf_ros.hpp +++ b/navigation/eskf/include/eskf/eskf_ros.hpp @@ -58,7 +58,7 @@ class ESKFNode : public rclcpp::Node { imu_measurement imu_meas_{}; - dvl_measurement dvl_meas_{}; + sensor_dvl dvl_sensor_{}; eskf_params eskf_params_{}; diff --git a/navigation/eskf/include/eskf/typedefs.hpp b/navigation/eskf/include/eskf/typedefs.hpp index b96f8dfbf..5b27e1b25 100644 --- a/navigation/eskf/include/eskf/typedefs.hpp +++ b/navigation/eskf/include/eskf/typedefs.hpp @@ -8,6 +8,7 @@ #include #include #include +#include namespace Eigen { typedef Eigen::Matrix Vector19d; @@ -106,20 +107,32 @@ struct state_euler { } }; +struct eskf_params { + double temp = 0.0; + Eigen::Matrix12d Q = Eigen::Matrix12d::Zero(); + double dt = 0.0; +}; + struct imu_measurement { Eigen::Vector3d accel = Eigen::Vector3d::Zero(); Eigen::Vector3d gyro = Eigen::Vector3d::Zero(); }; -struct dvl_measurement { - Eigen::Vector3d vel = Eigen::Vector3d::Zero(); - Eigen::Matrix3d cov = Eigen::Matrix3d::Zero(); +template +concept SensorModelConcept = requires(const T &meas, const state_quat &state) +{ + { meas.innovation(state) } -> std::convertible_to; + { meas.jacobian(state) } -> std::convertible_to; + { meas.noise_covariance() } -> std::convertible_to; }; -struct eskf_params { - double temp = 0.0; - Eigen::Matrix12d Q = Eigen::Matrix12d::Zero(); - double dt = 0.0; +struct sensor_dvl { + Eigen::Vector3d measurement; + Eigen::Matrix3d measurement_noise; + Eigen::VectorXd innovation(const state_quat &state) const; + Eigen::MatrixXd jacobian(const state_quat &state) const; + Eigen::MatrixXd noise_covariance() const; }; + #endif // ESKF_TYPEDEFS_H diff --git a/navigation/eskf/src/eskf.cpp b/navigation/eskf/src/eskf.cpp index 8507244e9..7beef13cb 100644 --- a/navigation/eskf/src/eskf.cpp +++ b/navigation/eskf/src/eskf.cpp @@ -31,7 +31,7 @@ std::pair ESKF::van_loan_discretization( return {A_d, GQG_d}; } -Eigen::Matrix3x19d ESKF::calculate_hx() { +Eigen::Matrix3x19d calculate_hx(const state_quat& current_nom_state_) { Eigen::Matrix3x19d Hx = Eigen::Matrix3x19d::Zero(); Eigen::Quaterniond q = current_nom_state_.quat.normalized(); @@ -60,23 +60,22 @@ Eigen::Matrix3x19d ESKF::calculate_hx() { return Hx; } -Eigen::Matrix3x18d ESKF::calculate_h_jacobian() { +Eigen::Matrix3x18d calculate_h_jacobian(const state_quat& current_nom_state_) { Eigen::Matrix19x18d x_delta = Eigen::Matrix19x18d::Zero(); x_delta.block<6, 6>(0, 0) = Eigen::Matrix6d::Identity(); x_delta.block<4, 3>(6, 6) = calculate_T_q(current_nom_state_.quat); x_delta.block<9, 9>(10, 9) = Eigen::Matrix9d::Identity(); - Eigen::Matrix3x18d H = calculate_hx() * x_delta; + Eigen::Matrix3x18d H = calculate_hx(current_nom_state_) * x_delta; return H; } -Eigen::Vector3d ESKF::calculate_h() { +Eigen::Vector3d calculate_h(const state_quat& current_nom_state_) { Eigen::Vector3d h; Eigen::Matrix3d R_bn = current_nom_state_.quat.normalized().toRotationMatrix().transpose(); h = R_bn * current_nom_state_.vel; - // 0.027293, 0.028089, 0.028089, 0.00255253, 0.00270035, 0.00280294, return h; } @@ -135,23 +134,6 @@ void ESKF::NIS(const Eigen::Vector3d& innovation, const Eigen::Matrix3d& S) { Eigen::Matrix3d S_inv = S.inverse(); NIS_ = innovation.transpose() * S_inv * innovation; } -void ESKF::measurement_update(const dvl_measurement& dvl_meas) { - Eigen::Matrix3x18d H = calculate_h_jacobian(); - Eigen::Matrix18d P = current_error_state_.covariance; - Eigen::Matrix3d R = dvl_meas.cov; - - Eigen::Matrix3d S = H * P * H.transpose() + R; - Eigen::Matrix18x3d K = P * H.transpose() * S.inverse(); - Eigen::Vector3d innovation = dvl_meas.vel - calculate_h(); - - NIS(innovation, S); - current_error_state_.set_from_vector(K * innovation); - - Eigen::Matrix18d I_KH = Eigen::Matrix18d::Identity() - K * H; - current_error_state_.covariance = - I_KH * P * I_KH.transpose() + - K * R * K.transpose(); // Used joseph form for more stable calculations -} void ESKF::injection_and_reset() { current_nom_state_.pos = current_nom_state_.pos + current_error_state_.pos; @@ -184,9 +166,26 @@ std::pair ESKF::imu_update( } std::pair ESKF::dvl_update( - const dvl_measurement& dvl_meas) { + const sensor_dvl& dvl_meas) { measurement_update(dvl_meas); injection_and_reset(); return {current_nom_state_, current_error_state_}; } + +// DVL sensor model implementations + +Eigen::VectorXd sensor_dvl::innovation(const state_quat &state) const { + + Eigen::Vector3d innovation = this->measurement - calculate_h(state); + return innovation; +} + +Eigen::MatrixXd sensor_dvl::jacobian(const state_quat &state) const { + Eigen::Matrix3x18d H = calculate_h_jacobian(state); + return H; +} + +Eigen::MatrixXd sensor_dvl::noise_covariance() const { + return this->measurement_noise; +} \ No newline at end of file diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp index 5542af533..c95ab266f 100644 --- a/navigation/eskf/src/eskf_ros.cpp +++ b/navigation/eskf/src/eskf_ros.cpp @@ -25,7 +25,7 @@ ESKFNode::ESKFNode(const rclcpp::NodeOptions& options) spdlog::info(start_message); #ifndef NDEBUG - spdlog::info("__________________________Debug mode is enabled______________________"); + spdlog::info("______________________Debug mode is enabled______________________"); #endif } @@ -112,19 +112,19 @@ void ESKFNode::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { void ESKFNode::dvl_callback( const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { - dvl_meas_.vel << msg->twist.twist.linear.x, msg->twist.twist.linear.y, + dvl_sensor_.measurement << msg->twist.twist.linear.x, msg->twist.twist.linear.y, msg->twist.twist.linear.z; - dvl_meas_.cov << msg->twist.covariance[0], msg->twist.covariance[1], + dvl_sensor_.measurement_noise << msg->twist.covariance[0], msg->twist.covariance[1], msg->twist.covariance[2], msg->twist.covariance[6], msg->twist.covariance[7], msg->twist.covariance[8], msg->twist.covariance[12], msg->twist.covariance[13], msg->twist.covariance[14]; - std::tie(nom_state_, error_state_) = eskf_->dvl_update(dvl_meas_); + std::tie(nom_state_, error_state_) = eskf_->dvl_update(dvl_sensor_); #ifndef NDEBUG - // Publish NIS + // Publish NIS in Debug mode std_msgs::msg::Float64 nis_msg; nis_msg.data = eskf_->NIS_; nis_pub_->publish(nis_msg); From c1275ef1c7c9d4f2ae05325cacd0a58685d25de1 Mon Sep 17 00:00:00 2001 From: Ahmed Date: Sun, 26 Oct 2025 13:15:19 +0100 Subject: [PATCH 113/290] removed gravity from the state vector and put it seperately for efficiency --- navigation/eskf/include/eskf/eskf.hpp | 23 +++++++-- navigation/eskf/include/eskf/typedefs.hpp | 51 ++++++++++--------- navigation/eskf/src/eskf.cpp | 62 +++++++++++------------ navigation/eskf/src/eskf_ros.cpp | 2 +- 4 files changed, 76 insertions(+), 62 deletions(-) diff --git a/navigation/eskf/include/eskf/eskf.hpp b/navigation/eskf/include/eskf/eskf.hpp index f9ef6284e..1f0ba2ec2 100644 --- a/navigation/eskf/include/eskf/eskf.hpp +++ b/navigation/eskf/include/eskf/eskf.hpp @@ -60,9 +60,9 @@ class ESKF { // @param A_c: Continuous state transition matrix // @param G_c: Continuous input matrix // @return Discrete state transition matrix and discrete input matrix - std::pair van_loan_discretization( - const Eigen::Matrix18d& A_c, - const Eigen::Matrix18x12d& G_c, + std::pair van_loan_discretization( + const Eigen::Matrix15d& A_c, + const Eigen::Matrix15x12d& G_c, const double dt); @@ -74,16 +74,29 @@ class ESKF { // Member variable for the current nominal state state_quat current_nom_state_{}; + + // gravity + Eigen::Vector3d g_{0.0, 0.0, 9.82}; + + // accelometer noise parameters + float accm_std_{0.0}; + float accm_bias_std_{0.0}; + float accm_bias_p_{1e-16}; + + // gyroscope noise parameters + float gyro_std_{0.0}; + float gyro_bias_std_{0.0}; + float gyro_bias_p_{1e-16}; }; // Measurement in world frame --> h(x) Eigen::Vector3d calculate_h(const state_quat& current_nom_state_); // Jacobian of h(x) with respect to the error state --> H -Eigen::Matrix3x18d calculate_h_jacobian(const state_quat& current_nom_state_); +Eigen::Matrix3x15d calculate_h_jacobian(const state_quat& current_nom_state_); // Jacobian of h(x) with respect to the nominal state --> Hx -Eigen::Matrix3x19d calculate_hx(const state_quat& current_nom_state_); +Eigen::Matrix3x16d calculate_hx(const state_quat& current_nom_state_); #include "eskf.tpp" // including template implementation diff --git a/navigation/eskf/include/eskf/typedefs.hpp b/navigation/eskf/include/eskf/typedefs.hpp index 5b27e1b25..1a9963df5 100644 --- a/navigation/eskf/include/eskf/typedefs.hpp +++ b/navigation/eskf/include/eskf/typedefs.hpp @@ -29,6 +29,12 @@ typedef Eigen::Matrix Matrix6d; typedef Eigen::Matrix Matrix9d; typedef Eigen::Matrix Matrix15d; typedef Eigen::Matrix Vector15d; +typedef Eigen::Matrix Vector16d; +typedef Eigen::Matrix Matrix15x12d; +typedef Eigen::Matrix Matrix16x15d; +typedef Eigen::Matrix Matrix3x15d; +typedef Eigen::Matrix Matrix3x16d; +typedef Eigen::Matrix Matrix30d; } // namespace Eigen template @@ -43,31 +49,31 @@ struct state_quat { Eigen::Quaterniond quat = Eigen::Quaterniond::Identity(); Eigen::Vector3d gyro_bias = Eigen::Vector3d::Zero(); Eigen::Vector3d accel_bias = Eigen::Vector3d::Zero(); - Eigen::Vector3d gravity = Eigen::Vector3d(0, 0, 9.81); + // Eigen::Vector3d gravity = Eigen::Vector3d(0, 0, 9.81); state_quat() = default; - Eigen::Vector19d as_vector() const { - Eigen::Vector19d vec{}; + Eigen::Vector16d as_vector() const { + Eigen::Vector16d vec{}; vec << pos, vel, quat.w(), quat.x(), quat.y(), quat.z(), gyro_bias, - accel_bias, gravity; + accel_bias; return vec; } - Eigen::Vector18d nees_error(const state_quat& other) const { - Eigen::Vector18d vec{}; - Eigen::Vector3d euler_diff{}; + // Eigen::Vector18d nees_error(const state_quat& other) const { + // Eigen::Vector18d vec{}; + // Eigen::Vector3d euler_diff{}; - euler_diff = (quat * other.quat.inverse()) - .toRotationMatrix() - .eulerAngles(0, 1, 2) + - Eigen::Vector3d(-M_PI, M_PI, -M_PI); + // euler_diff = (quat * other.quat.inverse()) + // .toRotationMatrix() + // .eulerAngles(0, 1, 2) + + // Eigen::Vector3d(-M_PI, M_PI, -M_PI); - vec << pos - other.pos, vel - other.vel, euler_diff, - gyro_bias - other.gyro_bias, accel_bias - other.accel_bias, - gravity - other.gravity; - return vec; - } + // vec << pos - other.pos, vel - other.vel, euler_diff, + // gyro_bias - other.gyro_bias, accel_bias - other.accel_bias, + // gravity - other.gravity; + // return vec; + // } state_quat operator-(const state_quat& other) const { state_quat diff{}; @@ -76,7 +82,6 @@ struct state_quat { diff.quat = quat * other.quat.inverse(); diff.gyro_bias = gyro_bias - other.gyro_bias; diff.accel_bias = accel_bias - other.accel_bias; - diff.gravity = gravity - other.gravity; return diff; } }; @@ -87,23 +92,21 @@ struct state_euler { Eigen::Vector3d euler = Eigen::Vector3d::Zero(); Eigen::Vector3d gyro_bias = Eigen::Vector3d::Zero(); Eigen::Vector3d accel_bias = Eigen::Vector3d::Zero(); - Eigen::Vector3d gravity = Eigen::Vector3d(0, 0, 9.81); - Eigen::Matrix18d covariance = Eigen::Matrix18d::Zero(); + Eigen::Matrix15d covariance = Eigen::Matrix15d::Zero(); - Eigen::Vector18d as_vector() const { - Eigen::Vector18d vec{}; - vec << pos, vel, euler, gyro_bias, accel_bias, gravity; + Eigen::Vector15d as_vector() const { + Eigen::Vector15d vec{}; + vec << pos, vel, euler, gyro_bias, accel_bias; return vec; } - void set_from_vector(const Eigen::Vector18d& vec) { + void set_from_vector(const Eigen::Vector15d& vec) { pos = vec.block<3, 1>(0, 0); vel = vec.block<3, 1>(3, 0); euler = vec.block<3, 1>(6, 0); gyro_bias = vec.block<3, 1>(9, 0); accel_bias = vec.block<3, 1>(12, 0); - gravity = vec.block<3, 1>(15, 0); } }; diff --git a/navigation/eskf/src/eskf.cpp b/navigation/eskf/src/eskf.cpp index 7beef13cb..e08c0a90f 100644 --- a/navigation/eskf/src/eskf.cpp +++ b/navigation/eskf/src/eskf.cpp @@ -6,33 +6,34 @@ #include "eskf/eskf_utils.hpp" #include "eskf/typedefs.hpp" #include "iostream" +#include ESKF::ESKF(const eskf_params& params) : Q_(params.Q) {} -std::pair ESKF::van_loan_discretization( - const Eigen::Matrix18d& A_c, - const Eigen::Matrix18x12d& G_c, +std::pair ESKF::van_loan_discretization( + const Eigen::Matrix15d& A_c, + const Eigen::Matrix15x12d& G_c, const double dt) { - Eigen::Matrix18d GQG_T = G_c * Q_ * G_c.transpose(); - Eigen::Matrix36d vanLoanMat = Eigen::Matrix36d::Zero(); + Eigen::Matrix15d GQG_T = G_c * Q_ * G_c.transpose(); + Eigen::Matrix30d vanLoanMat = Eigen::Matrix30d::Zero(); - vanLoanMat.topLeftCorner<18, 18>() = -A_c; - vanLoanMat.topRightCorner<18, 18>() = GQG_T; - vanLoanMat.bottomRightCorner<18, 18>() = A_c.transpose(); + vanLoanMat.topLeftCorner<15, 15>() = -A_c; + vanLoanMat.topRightCorner<15, 15>() = GQG_T; + vanLoanMat.bottomRightCorner<15, 15>() = A_c.transpose(); - Eigen::Matrix36d vanLoanExp = (vanLoanMat * dt).exp(); + Eigen::Matrix30d vanLoanExp = (vanLoanMat * dt).exp(); - Eigen::Matrix18d V1 = vanLoanExp.bottomRightCorner<18, 18>().transpose(); - Eigen::Matrix18d V2 = vanLoanExp.topRightCorner<18, 18>(); + Eigen::Matrix15d V1 = vanLoanExp.bottomRightCorner<15, 15>().transpose(); + Eigen::Matrix15d V2 = vanLoanExp.topRightCorner<15, 15>(); - Eigen::Matrix18d A_d = V1; - Eigen::Matrix18d GQG_d = A_d * V2; + Eigen::Matrix15d A_d = V1; + Eigen::Matrix15d GQG_d = A_d * V2; return {A_d, GQG_d}; } -Eigen::Matrix3x19d calculate_hx(const state_quat& current_nom_state_) { - Eigen::Matrix3x19d Hx = Eigen::Matrix3x19d::Zero(); +Eigen::Matrix3x16d calculate_hx(const state_quat& current_nom_state_) { + Eigen::Matrix3x16d Hx = Eigen::Matrix3x16d::Zero(); Eigen::Quaterniond q = current_nom_state_.quat.normalized(); Eigen::Matrix3d R_bn = q.toRotationMatrix(); @@ -60,13 +61,13 @@ Eigen::Matrix3x19d calculate_hx(const state_quat& current_nom_state_) { return Hx; } -Eigen::Matrix3x18d calculate_h_jacobian(const state_quat& current_nom_state_) { - Eigen::Matrix19x18d x_delta = Eigen::Matrix19x18d::Zero(); +Eigen::Matrix3x15d calculate_h_jacobian(const state_quat& current_nom_state_) { + Eigen::Matrix16x15d x_delta = Eigen::Matrix16x15d::Zero(); x_delta.block<6, 6>(0, 0) = Eigen::Matrix6d::Identity(); x_delta.block<4, 3>(6, 6) = calculate_T_q(current_nom_state_.quat); - x_delta.block<9, 9>(10, 9) = Eigen::Matrix9d::Identity(); + x_delta.block<6, 6>(10, 9) = Eigen::Matrix6d::Identity(); - Eigen::Matrix3x18d H = calculate_hx(current_nom_state_) * x_delta; + Eigen::Matrix3x15d H = calculate_hx(current_nom_state_) * x_delta; return H; } @@ -84,7 +85,7 @@ void ESKF::nominal_state_discrete(const imu_measurement& imu_meas, Eigen::Vector3d acc = current_nom_state_.quat.normalized().toRotationMatrix() * (imu_meas.accel - current_nom_state_.accel_bias) + - current_nom_state_.gravity; + this->g_; Eigen::Vector3d gyro = (imu_meas.gyro - current_nom_state_.gyro_bias) * dt; current_nom_state_.pos = current_nom_state_.pos + @@ -95,9 +96,8 @@ void ESKF::nominal_state_discrete(const imu_measurement& imu_meas, (current_nom_state_.quat * vector3d_to_quaternion(gyro)); current_nom_state_.quat.normalize(); - current_nom_state_.gyro_bias = current_nom_state_.gyro_bias; - current_nom_state_.accel_bias = current_nom_state_.accel_bias; - current_nom_state_.gravity = current_nom_state_.gravity; + current_nom_state_.accel_bias = current_nom_state_.accel_bias*std::exp(accm_bias_p_*dt); + current_nom_state_.gyro_bias = current_nom_state_.gyro_bias*std::exp(gyro_bias_p_*dt); } void ESKF::error_state_prediction(const imu_measurement& imu_meas, @@ -106,7 +106,7 @@ void ESKF::error_state_prediction(const imu_measurement& imu_meas, Eigen::Vector3d acc = (imu_meas.accel - current_nom_state_.accel_bias); Eigen::Vector3d gyro = (imu_meas.gyro - current_nom_state_.gyro_bias); - Eigen::Matrix18d A_c = Eigen::Matrix18d::Zero(); + Eigen::Matrix15d A_c = Eigen::Matrix15d::Zero(); A_c.block<3, 3>(0, 3) = Eigen::Matrix3d::Identity(); A_c.block<3, 3>(3, 6) = -R * skew(acc); A_c.block<3, 3>(6, 6) = -skew(gyro); @@ -114,15 +114,15 @@ void ESKF::error_state_prediction(const imu_measurement& imu_meas, A_c.block<3, 3>(9, 9) = -Eigen::Matrix3d::Identity(); A_c.block<3, 3>(12, 12) = -Eigen::Matrix3d::Identity(); A_c.block<3, 3>(6, 12) = -Eigen::Matrix3d::Identity(); - A_c.block<3, 3>(3, 15) = Eigen::Matrix3d::Identity(); + // A_c.block<3, 3>(3, 15) = Eigen::Matrix3d::Identity(); - Eigen::Matrix18x12d G_c = Eigen::Matrix18x12d::Zero(); + Eigen::Matrix15x12d G_c = Eigen::Matrix15x12d::Zero(); G_c.block<3, 3>(3, 0) = -R; G_c.block<3, 3>(6, 3) = -Eigen::Matrix3d::Identity(); G_c.block<3, 3>(9, 6) = Eigen::Matrix3d::Identity(); G_c.block<3, 3>(12, 9) = Eigen::Matrix3d::Identity(); - Eigen::Matrix18d A_d, GQG_d; + Eigen::Matrix15d A_d, GQG_d; std::tie(A_d, GQG_d) = van_loan_discretization(A_c, G_c, dt); state_euler next_error_state; @@ -146,14 +146,12 @@ void ESKF::injection_and_reset() { current_nom_state_.gyro_bias + current_error_state_.gyro_bias; current_nom_state_.accel_bias = current_nom_state_.accel_bias + current_error_state_.accel_bias; - current_nom_state_.gravity = - current_nom_state_.gravity + current_error_state_.gravity; - Eigen::Matrix18d G = Eigen::Matrix18d::Identity(); + Eigen::Matrix15d G = Eigen::Matrix15d::Identity(); current_error_state_.covariance = G * current_error_state_.covariance * G.transpose(); - current_error_state_.set_from_vector(Eigen::Vector18d::Zero()); + current_error_state_.set_from_vector(Eigen::Vector15d::Zero()); } std::pair ESKF::imu_update( @@ -182,7 +180,7 @@ Eigen::VectorXd sensor_dvl::innovation(const state_quat &state) const { } Eigen::MatrixXd sensor_dvl::jacobian(const state_quat &state) const { - Eigen::Matrix3x18d H = calculate_h_jacobian(state); + Eigen::Matrix3x15d H = calculate_h_jacobian(state); return H; } diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp index c95ab266f..99e3c8cb5 100644 --- a/navigation/eskf/src/eskf_ros.cpp +++ b/navigation/eskf/src/eskf_ros.cpp @@ -79,7 +79,7 @@ void ESKFNode::set_parameters() { std::vector diag_p_init = this->declare_parameter>("diag_p_init"); - Eigen::Matrix18d P = create_diagonal_matrix<18>(diag_p_init); + Eigen::Matrix15d P = create_diagonal_matrix<15>(diag_p_init); error_state_.covariance = P; } From 4c869db5e6f0259a14205800540ea3c27f5ba05c Mon Sep 17 00:00:00 2001 From: Ahmed Date: Wed, 5 Nov 2025 11:52:17 +0100 Subject: [PATCH 114/290] added code for errors and for covariance publishing --- navigation/eskf/config/eskf_params.yaml | 3 +- navigation/eskf/include/eskf/eskf_ros.hpp | 20 +++++ navigation/eskf/include/eskf/eskf_utils.hpp | 3 + navigation/eskf/src/eskf.cpp | 1 - navigation/eskf/src/eskf_ros.cpp | 85 ++++++++++++++++++++- navigation/eskf/src/eskf_utils.cpp | 28 +++++++ 6 files changed, 136 insertions(+), 4 deletions(-) diff --git a/navigation/eskf/config/eskf_params.yaml b/navigation/eskf/config/eskf_params.yaml index 2293ae1e1..6d9f01beb 100644 --- a/navigation/eskf/config/eskf_params.yaml +++ b/navigation/eskf/config/eskf_params.yaml @@ -3,6 +3,7 @@ eskf_node: imu_topic: /imu/data_raw dvl_topic: /orca/twist odom_topic: odom_ESKF - diag_Q_std: [0.0124, 0.0124, 0.0124, 0.0026, 0.0026, 0.0026, 0.000392, 0.000392, 0.000392, 0.00001145, 0.0000145, 0.0000145] + #diag_Q_std: [0.0124, 0.0124, 0.0124, 0.0026, 0.0026, 0.0026, 0.000392, 0.000392, 0.000392, 0.00001145, 0.0000145, 0.0000145] + diag_Q_std: [0.05, 0.05, 0.1, 0.01, 0.01, 0.02, 0.001, 0.001, 0.001, 0.0001, 0.0001, 0.0001] diag_p_init: [1.0, 1.0, 1.0, 0.5, 0.5, 0.5, 0.1, 0.1, 0.1, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001] imu_frame: [0.0, 0.0, -1.0, 0.0, -1.0, 0.0, -1.0, 0.0, 0.0] diff --git a/navigation/eskf/include/eskf/eskf_ros.hpp b/navigation/eskf/include/eskf/eskf_ros.hpp index 9220ae7bf..c22371240 100644 --- a/navigation/eskf/include/eskf/eskf_ros.hpp +++ b/navigation/eskf/include/eskf/eskf_ros.hpp @@ -46,8 +46,28 @@ class ESKFNode : public rclcpp::Node { rclcpp::Publisher::SharedPtr odom_pub_; + rclcpp::Publisher::SharedPtr odom_cov_pub_; + + #ifndef NDEBUG + void ground_truth_callback(const nav_msgs::msg::Odometry::SharedPtr msg); + rclcpp::Publisher::SharedPtr nis_pub_; + rclcpp::Subscription::SharedPtr odom_ground_truth_; + + struct ESKFError { + Eigen::Vector3d pos; // position error + Eigen::Vector3d vel; // velocity error + Eigen::Vector3d ori; // orientation error (roll, pitch, yaw) + }; + + ESKFError last_error_; + rclcpp::Publisher::SharedPtr error_pub_; + #endif + + // to remove + // rclcpp::Publisher::SharedPtr incoming_imu_eskf_; + std::chrono::milliseconds time_step; rclcpp::TimerBase::SharedPtr odom_pub_timer_; diff --git a/navigation/eskf/include/eskf/eskf_utils.hpp b/navigation/eskf/include/eskf/eskf_utils.hpp index 0fe06ee7a..8dc35b0b5 100644 --- a/navigation/eskf/include/eskf/eskf_utils.hpp +++ b/navigation/eskf/include/eskf/eskf_utils.hpp @@ -35,4 +35,7 @@ Eigen::Quaterniond vector3d_to_quaternion(const Eigen::Vector3d& vector); // @return Corresponding quaternion Eigen::Quaterniond euler_to_quaternion(const Eigen::Vector3d& euler); + +Eigen::Vector3d quaternion_to_euler(const Eigen::Quaterniond& q); + #endif // ESKF_UTILS_HPP diff --git a/navigation/eskf/src/eskf.cpp b/navigation/eskf/src/eskf.cpp index e08c0a90f..be9324f60 100644 --- a/navigation/eskf/src/eskf.cpp +++ b/navigation/eskf/src/eskf.cpp @@ -114,7 +114,6 @@ void ESKF::error_state_prediction(const imu_measurement& imu_meas, A_c.block<3, 3>(9, 9) = -Eigen::Matrix3d::Identity(); A_c.block<3, 3>(12, 12) = -Eigen::Matrix3d::Identity(); A_c.block<3, 3>(6, 12) = -Eigen::Matrix3d::Identity(); - // A_c.block<3, 3>(3, 15) = Eigen::Matrix3d::Identity(); Eigen::Matrix15x12d G_c = Eigen::Matrix15x12d::Zero(); G_c.block<3, 3>(3, 0) = -R; diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp index 99e3c8cb5..80db43a89 100644 --- a/navigation/eskf/src/eskf_ros.cpp +++ b/navigation/eskf/src/eskf_ros.cpp @@ -51,8 +51,20 @@ void ESKFNode::set_subscribers_and_publisher() { std::string odom_topic = this->get_parameter("odom_topic").as_string(); odom_pub_ = this->create_publisher( odom_topic, qos_sensor_data); + #ifndef NDEBUG nis_pub_ = create_publisher("dvl/nis", 10); + + error_pub_ = create_publisher("eskf/error", 10); + + odom_cov_pub_ = create_publisher("eskf/odom_covariance", 10); + + odom_ground_truth_ = this->create_subscription( + "/orca/odom", qos_sensor_data, + std::bind(&ESKFNode::ground_truth_callback, this, + std::placeholders::_1)); + // incoming_imu_eskf_ = this->create_publisher("incoming_imu_eskf", rclcpp::SensorDataQoS() ); + #endif } void ESKFNode::set_parameters() { @@ -100,14 +112,27 @@ void ESKFNode::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { msg->linear_acceleration.y, msg->linear_acceleration.z); - imu_meas_.accel = R_imu_eskf_ * raw_accel; + // imu_meas_.accel = R_imu_eskf_ * raw_accel; + imu_meas_.accel = -raw_accel; Eigen::Vector3d raw_gyro(msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z); - imu_meas_.gyro = R_imu_eskf_ * raw_gyro; + // imu_meas_.gyro = R_imu_eskf_ * raw_gyro; + imu_meas_.gyro = raw_gyro; std::tie(nom_state_, error_state_) = eskf_->imu_update(imu_meas_, dt); + + // to remove + // sensor_msgs::msg::Imu imu_out; + // imu_out.header = msg->header; + // imu_out.linear_acceleration.x = imu_meas_.accel.x(); + // imu_out.linear_acceleration.y = imu_meas_.accel.y(); + // imu_out.linear_acceleration.z = imu_meas_.accel.z(); + // imu_out.angular_velocity.x = imu_meas_.gyro.x(); + // imu_out.angular_velocity.y = imu_meas_.gyro.y(); + // imu_out.angular_velocity.z = imu_meas_.gyro.z(); + // incoming_imu_eskf_->publish(imu_out); } void ESKFNode::dvl_callback( @@ -131,6 +156,40 @@ void ESKFNode::dvl_callback( #endif } +#ifndef NDEBUG +void ESKFNode::ground_truth_callback( + const nav_msgs::msg::Odometry::SharedPtr msg) { + + // Position error + last_error_.pos.x() = nom_state_.pos.x() - msg->pose.pose.position.x; + last_error_.pos.y() = nom_state_.pos.y() - msg->pose.pose.position.y; + last_error_.pos.z() = nom_state_.pos.z() - msg->pose.pose.position.z; + + // Velocity error + last_error_.vel.x() = nom_state_.vel.x() - msg->twist.twist.linear.x; + last_error_.vel.y() = nom_state_.vel.y() - msg->twist.twist.linear.y; + last_error_.vel.z() = nom_state_.vel.z() - msg->twist.twist.linear.z; + + // Orientation error + Eigen::Quaterniond q_nom(nom_state_.quat.x(), nom_state_.quat.y(), + nom_state_.quat.z(), nom_state_.quat.w()); + Eigen::Quaterniond q_gt(msg->pose.pose.orientation.x, + msg->pose.pose.orientation.y, + msg->pose.pose.orientation.z, + msg->pose.pose.orientation.w); + + Eigen::Quaterniond q_err = q_gt.inverse() * q_nom; // error rotation + last_error_.ori = quaternion_to_euler(q_err); + + // Publish + std_msgs::msg::Float64MultiArray error_msg; + error_msg.data = { last_error_.pos.x(), last_error_.pos.y(), last_error_.pos.z(), + last_error_.vel.x(), last_error_.vel.y(), last_error_.vel.z(), + last_error_.ori.x(), last_error_.ori.y(), last_error_.ori.z() }; + error_pub_->publish(error_msg); +} +#endif + void ESKFNode::publish_odom() { nav_msgs::msg::Odometry odom_msg; @@ -157,6 +216,28 @@ void ESKFNode::publish_odom() { odom_msg.header.stamp = this->now(); odom_pub_->publish(odom_msg); + + + // publish the covariance matrix + std_msgs::msg::Float64MultiArray msg; + + msg.layout.dim.resize(2); + msg.layout.dim[0].label = "rows"; + msg.layout.dim[0].size = 15; + msg.layout.dim[0].stride = 15 * 15; + msg.layout.dim[1].label = "cols"; + msg.layout.dim[1].size = 15; + msg.layout.dim[1].stride = 15; + + // Flatten Eigen matrix into a std::vector + msg.data.resize(15 * 15); + msg.data.assign( + error_state_.covariance.data(), + error_state_.covariance.data() + 15 * 15 + ); + + // Publish + odom_cov_pub_->publish(msg); } RCLCPP_COMPONENTS_REGISTER_NODE(ESKFNode) diff --git a/navigation/eskf/src/eskf_utils.cpp b/navigation/eskf/src/eskf_utils.cpp index 02e8d9229..5cbd704ae 100644 --- a/navigation/eskf/src/eskf_utils.cpp +++ b/navigation/eskf/src/eskf_utils.cpp @@ -50,3 +50,31 @@ Eigen::Quaterniond euler_to_quaternion(const Eigen::Vector3d& euler) { q.normalize(); return q; } + +Eigen::Vector3d quaternion_to_euler(const Eigen::Quaterniond& q) { + // Assumes quaternion is normalized + double w = q.w(); + double x = q.x(); + double y = q.y(); + double z = q.z(); + + // Roll (x-axis rotation) + double sinr_cosp = 2.0 * (w * x + y * z); + double cosr_cosp = 1.0 - 2.0 * (x * x + y * y); + double roll = std::atan2(sinr_cosp, cosr_cosp); + + // Pitch (y-axis rotation) + double sinp = 2.0 * (w * y - z * x); + double pitch; + if (std::abs(sinp) >= 1) + pitch = std::copysign(M_PI / 2.0, sinp); // use 90° if out of range + else + pitch = std::asin(sinp); + + // Yaw (z-axis rotation) + double siny_cosp = 2.0 * (w * z + x * y); + double cosy_cosp = 1.0 - 2.0 * (y * y + z * z); + double yaw = std::atan2(siny_cosp, cosy_cosp); + + return Eigen::Vector3d(roll, pitch, yaw); +} From 2b340d818bdfc9d0176889a51a84535b540745ef Mon Sep 17 00:00:00 2001 From: Ahmed Date: Sun, 9 Nov 2025 16:47:29 +0100 Subject: [PATCH 115/290] added the modifications from Talha's branch /dev/auv-navigation-eskf (removed local utils, fixed naming) --- navigation/eskf/CMakeLists.txt | 3 +- navigation/eskf/config/eskf_params.yaml | 4 +- navigation/eskf/include/eskf/eskf.hpp | 37 ++--- navigation/eskf/include/eskf/eskf.tpp | 2 +- navigation/eskf/include/eskf/eskf_ros.hpp | 32 +--- navigation/eskf/include/eskf/eskf_utils.hpp | 41 ------ navigation/eskf/include/eskf/typedefs.hpp | 52 +++---- navigation/eskf/src/eskf.cpp | 70 +++++---- navigation/eskf/src/eskf_ros.cpp | 155 +++++++------------- navigation/eskf/src/eskf_utils.cpp | 80 ---------- 10 files changed, 139 insertions(+), 337 deletions(-) delete mode 100644 navigation/eskf/include/eskf/eskf_utils.hpp delete mode 100644 navigation/eskf/src/eskf_utils.cpp diff --git a/navigation/eskf/CMakeLists.txt b/navigation/eskf/CMakeLists.txt index ec4feea05..28af1cd06 100644 --- a/navigation/eskf/CMakeLists.txt +++ b/navigation/eskf/CMakeLists.txt @@ -21,6 +21,7 @@ find_package(geometry_msgs REQUIRED) find_package(Eigen3 REQUIRED) find_package(tf2 REQUIRED) find_package(vortex_msgs REQUIRED) +find_package(vortex_utils REQUIRED) find_package(spdlog REQUIRED) find_package(fmt REQUIRED) @@ -37,7 +38,6 @@ set(LIB_NAME "${PROJECT_NAME}_component") add_library(${LIB_NAME} SHARED src/eskf.cpp src/eskf_ros.cpp - src/eskf_utils.cpp ) ament_target_dependencies(${LIB_NAME} PUBLIC @@ -48,6 +48,7 @@ ament_target_dependencies(${LIB_NAME} PUBLIC Eigen3 tf2 vortex_msgs + vortex_utils spdlog fmt ) diff --git a/navigation/eskf/config/eskf_params.yaml b/navigation/eskf/config/eskf_params.yaml index 6d9f01beb..81c38deac 100644 --- a/navigation/eskf/config/eskf_params.yaml +++ b/navigation/eskf/config/eskf_params.yaml @@ -5,5 +5,7 @@ eskf_node: odom_topic: odom_ESKF #diag_Q_std: [0.0124, 0.0124, 0.0124, 0.0026, 0.0026, 0.0026, 0.000392, 0.000392, 0.000392, 0.00001145, 0.0000145, 0.0000145] diag_Q_std: [0.05, 0.05, 0.1, 0.01, 0.01, 0.02, 0.001, 0.001, 0.001, 0.0001, 0.0001, 0.0001] + # diag_p_init: [1.0, 1.0, 1.0, 0.5, 0.5, 0.5, 0.1, 0.1, 0.1, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001] diag_p_init: [1.0, 1.0, 1.0, 0.5, 0.5, 0.5, 0.1, 0.1, 0.1, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001] - imu_frame: [0.0, 0.0, -1.0, 0.0, -1.0, 0.0, -1.0, 0.0, 0.0] + # imu_frame: [0.0, 0.0, -1.0, 0.0, -1.0, 0.0, -1.0, 0.0, 0.0] + imu_frame: [-1.0, 0.0, 0.0, -1.0, 0.0, 0.0, -1.0, 0.0, 0.0] diff --git a/navigation/eskf/include/eskf/eskf.hpp b/navigation/eskf/include/eskf/eskf.hpp index 1f0ba2ec2..8d04f12d7 100644 --- a/navigation/eskf/include/eskf/eskf.hpp +++ b/navigation/eskf/include/eskf/eskf.hpp @@ -8,38 +8,36 @@ class ESKF { public: - ESKF(const eskf_params& params); + ESKF(const EskfParams& params); // @brief Update the nominal state and error state // @param imu_meas: IMU measurement // @param dt: Time step - // @return Updated nominal state and error state - std::pair imu_update( - const imu_measurement& imu_meas, - const double dt); + void imu_update(const ImuMeasurement& imu_meas, const double dt); // @brief Update the nominal state and error state // @param dvl_meas: DVL measurement - // @return Updated nominal state and error state - std::pair dvl_update( - const sensor_dvl& dvl_meas); + void dvl_update(const SensorDVL& dvl_meas); - // Normalized Innovation Squared - double NIS_{}; + inline StateQuat get_nominal_state() const { return current_nom_state_; } + + inline StateEuler get_error_state() const { return current_error_state_; } + + inline double get_nis() const { return nis_; } private: // @brief Predict the nominal state // @param imu_meas: IMU measurement // @param dt: Time step // @return Predicted nominal state - void nominal_state_discrete(const imu_measurement& imu_meas, + void nominal_state_discrete(const ImuMeasurement& imu_meas, const double dt); // @brief Predict the error state // @param imu_meas: IMU measurement // @param dt: Time step // @return Predicted error state - void error_state_prediction(const imu_measurement& imu_meas, + void error_state_prediction(const ImuMeasurement& imu_meas, const double dt); // @brief Calculate the NIS @@ -69,11 +67,14 @@ class ESKF { // Process noise covariance matrix Eigen::Matrix12d Q_{}; + // Normalized Innovation Squared + double nis_{}; + // Member variable for the current error state - state_euler current_error_state_{}; + StateEuler current_error_state_{}; // Member variable for the current nominal state - state_quat current_nom_state_{}; + StateQuat current_nom_state_{}; // gravity Eigen::Vector3d g_{0.0, 0.0, 9.82}; @@ -90,13 +91,15 @@ class ESKF { }; // Measurement in world frame --> h(x) -Eigen::Vector3d calculate_h(const state_quat& current_nom_state_); +Eigen::Vector3d calculate_h(const StateQuat& current_nom_state_); // Jacobian of h(x) with respect to the error state --> H -Eigen::Matrix3x15d calculate_h_jacobian(const state_quat& current_nom_state_); +Eigen::Matrix3x15d calculate_h_jacobian(const StateQuat& current_nom_state_); // Jacobian of h(x) with respect to the nominal state --> Hx -Eigen::Matrix3x16d calculate_hx(const state_quat& current_nom_state_); +Eigen::Matrix3x16d calculate_hx(const StateQuat& current_nom_state_); + +double compute_nis(const Eigen::Vector3d& innovation, const Eigen::Matrix3d& S); #include "eskf.tpp" // including template implementation diff --git a/navigation/eskf/include/eskf/eskf.tpp b/navigation/eskf/include/eskf/eskf.tpp index 81e01ba8e..ba7a1f859 100644 --- a/navigation/eskf/include/eskf/eskf.tpp +++ b/navigation/eskf/include/eskf/eskf.tpp @@ -13,7 +13,7 @@ void ESKF::measurement_update(const SensorT& meas) Eigen::MatrixXd K = P * H.transpose() * S.inverse(); #ifndef NDEBUG - NIS(innovation, S); + nis_ = compute_nis(innovation, S); #endif current_error_state_.set_from_vector(K * innovation); diff --git a/navigation/eskf/include/eskf/eskf_ros.hpp b/navigation/eskf/include/eskf/eskf_ros.hpp index c22371240..a07487e3d 100644 --- a/navigation/eskf/include/eskf/eskf_ros.hpp +++ b/navigation/eskf/include/eskf/eskf_ros.hpp @@ -46,46 +46,20 @@ class ESKFNode : public rclcpp::Node { rclcpp::Publisher::SharedPtr odom_pub_; - rclcpp::Publisher::SharedPtr odom_cov_pub_; - - #ifndef NDEBUG - void ground_truth_callback(const nav_msgs::msg::Odometry::SharedPtr msg); + rclcpp::Publisher::SharedPtr cov_pub_; rclcpp::Publisher::SharedPtr nis_pub_; - rclcpp::Subscription::SharedPtr odom_ground_truth_; - - struct ESKFError { - Eigen::Vector3d pos; // position error - Eigen::Vector3d vel; // velocity error - Eigen::Vector3d ori; // orientation error (roll, pitch, yaw) - }; - - ESKFError last_error_; - rclcpp::Publisher::SharedPtr error_pub_; - #endif - - // to remove - // rclcpp::Publisher::SharedPtr incoming_imu_eskf_; - std::chrono::milliseconds time_step; rclcpp::TimerBase::SharedPtr odom_pub_timer_; - state_quat nom_state_{}; - - state_euler error_state_{}; - - imu_measurement imu_meas_{}; - - sensor_dvl dvl_sensor_{}; - - eskf_params eskf_params_{}; - std::unique_ptr eskf_; bool first_imu_msg_received_ = false; + bool debug_on_ = false; + Eigen::Matrix3d R_imu_eskf_{}; rclcpp::Time last_imu_time_{}; diff --git a/navigation/eskf/include/eskf/eskf_utils.hpp b/navigation/eskf/include/eskf/eskf_utils.hpp deleted file mode 100644 index 8dc35b0b5..000000000 --- a/navigation/eskf/include/eskf/eskf_utils.hpp +++ /dev/null @@ -1,41 +0,0 @@ -#ifndef ESKF_UTILS_HPP -#define ESKF_UTILS_HPP - -#include -#include "eigen3/Eigen/Dense" -#include "eskf/typedefs.hpp" - -// @brief Compute the skew-symmetric matrix of a vector -// @param v: Input vector -// @return Skew-symmetric matrix -Eigen::Matrix3d skew(const Eigen::Vector3d& v); - -// @brief Square a value -// @param value: Input value -// @return Squared value -double sq(double value); - -// @brief Normalize an angle to the range [-pi, pi] -// @param angle: Input angle in radians -// @return Normalized angle in radians -double ssa(double angle); - -// @brief Calculate the transformation matrix using a quaternion -// @param quat: Input quaternion -// @return Transformation matrix -Eigen::Matrix4x3d calculate_T_q(const Eigen::Quaterniond& quat); - -// @brief Convert a rotation vector to a quaternion -// @param vector: Input rotation vector -// @return Corresponding quaternion -Eigen::Quaterniond vector3d_to_quaternion(const Eigen::Vector3d& vector); - -// @brief Convert Euler angles to a quaternion -// @param euler: Input Euler angles (roll, pitch, yaw) in radians -// @return Corresponding quaternion -Eigen::Quaterniond euler_to_quaternion(const Eigen::Vector3d& euler); - - -Eigen::Vector3d quaternion_to_euler(const Eigen::Quaterniond& q); - -#endif // ESKF_UTILS_HPP diff --git a/navigation/eskf/include/eskf/typedefs.hpp b/navigation/eskf/include/eskf/typedefs.hpp index 1a9963df5..5585b0455 100644 --- a/navigation/eskf/include/eskf/typedefs.hpp +++ b/navigation/eskf/include/eskf/typedefs.hpp @@ -5,9 +5,9 @@ #ifndef ESKF_TYPEDEFS_H #define ESKF_TYPEDEFS_H -#include #include #include +#include #include namespace Eigen { @@ -28,6 +28,7 @@ typedef Eigen::Matrix Matrix36d; typedef Eigen::Matrix Matrix6d; typedef Eigen::Matrix Matrix9d; typedef Eigen::Matrix Matrix15d; +typedef Eigen::Matrix Vector12d; typedef Eigen::Matrix Vector15d; typedef Eigen::Matrix Vector16d; typedef Eigen::Matrix Matrix15x12d; @@ -38,20 +39,19 @@ typedef Eigen::Matrix Matrix30d; } // namespace Eigen template -Eigen::Matrix create_diagonal_matrix( +Eigen::Matrix createDiagonalMatrix( const std::vector& diag) { return Eigen::Map>(diag.data()) .asDiagonal(); } -struct state_quat { +struct StateQuat { Eigen::Vector3d pos = Eigen::Vector3d::Zero(); Eigen::Vector3d vel = Eigen::Vector3d::Zero(); Eigen::Quaterniond quat = Eigen::Quaterniond::Identity(); Eigen::Vector3d gyro_bias = Eigen::Vector3d::Zero(); Eigen::Vector3d accel_bias = Eigen::Vector3d::Zero(); - // Eigen::Vector3d gravity = Eigen::Vector3d(0, 0, 9.81); - state_quat() = default; + StateQuat() = default; Eigen::Vector16d as_vector() const { Eigen::Vector16d vec{}; @@ -60,23 +60,8 @@ struct state_quat { return vec; } - // Eigen::Vector18d nees_error(const state_quat& other) const { - // Eigen::Vector18d vec{}; - // Eigen::Vector3d euler_diff{}; - - // euler_diff = (quat * other.quat.inverse()) - // .toRotationMatrix() - // .eulerAngles(0, 1, 2) + - // Eigen::Vector3d(-M_PI, M_PI, -M_PI); - - // vec << pos - other.pos, vel - other.vel, euler_diff, - // gyro_bias - other.gyro_bias, accel_bias - other.accel_bias, - // gravity - other.gravity; - // return vec; - // } - - state_quat operator-(const state_quat& other) const { - state_quat diff{}; + StateQuat operator-(const StateQuat& other) const { + StateQuat diff{}; diff.pos = pos - other.pos; diff.vel = vel - other.vel; diff.quat = quat * other.quat.inverse(); @@ -86,7 +71,7 @@ struct state_quat { } }; -struct state_euler { +struct StateEuler { Eigen::Vector3d pos = Eigen::Vector3d::Zero(); Eigen::Vector3d vel = Eigen::Vector3d::Zero(); Eigen::Vector3d euler = Eigen::Vector3d::Zero(); @@ -110,30 +95,33 @@ struct state_euler { } }; -struct eskf_params { - double temp = 0.0; +struct EskfParams { Eigen::Matrix12d Q = Eigen::Matrix12d::Zero(); - double dt = 0.0; + Eigen::Matrix15d P = Eigen::Matrix15d::Zero(); }; - -struct imu_measurement { +struct ImuMeasurement { Eigen::Vector3d accel = Eigen::Vector3d::Zero(); Eigen::Vector3d gyro = Eigen::Vector3d::Zero(); }; +struct DvlMeasurement { + Eigen::Vector3d vel = Eigen::Vector3d::Zero(); + Eigen::Matrix3d cov = Eigen::Matrix3d::Zero(); +}; + template -concept SensorModelConcept = requires(const T &meas, const state_quat &state) +concept SensorModelConcept = requires(const T &meas, const StateQuat &state) { { meas.innovation(state) } -> std::convertible_to; { meas.jacobian(state) } -> std::convertible_to; { meas.noise_covariance() } -> std::convertible_to; }; -struct sensor_dvl { +struct SensorDVL { Eigen::Vector3d measurement; Eigen::Matrix3d measurement_noise; - Eigen::VectorXd innovation(const state_quat &state) const; - Eigen::MatrixXd jacobian(const state_quat &state) const; + Eigen::VectorXd innovation(const StateQuat &state) const; + Eigen::MatrixXd jacobian(const StateQuat &state) const; Eigen::MatrixXd noise_covariance() const; }; diff --git a/navigation/eskf/src/eskf.cpp b/navigation/eskf/src/eskf.cpp index be9324f60..2e623a749 100644 --- a/navigation/eskf/src/eskf.cpp +++ b/navigation/eskf/src/eskf.cpp @@ -1,14 +1,17 @@ #include "eskf/eskf.hpp" -#include #include #include #include -#include "eskf/eskf_utils.hpp" +#include #include "eskf/typedefs.hpp" -#include "iostream" -#include -ESKF::ESKF(const eskf_params& params) : Q_(params.Q) {} +double compute_nis(const Eigen::Vector3d& innovation, + const Eigen::Matrix3d& S) { + Eigen::Matrix3d S_inv = S.inverse(); + return innovation.transpose() * S_inv * innovation; +} + +ESKF::ESKF(const EskfParams& params) : Q_(params.Q) {} std::pair ESKF::van_loan_discretization( const Eigen::Matrix15d& A_c, @@ -32,7 +35,7 @@ std::pair ESKF::van_loan_discretization( return {A_d, GQG_d}; } -Eigen::Matrix3x16d calculate_hx(const state_quat& current_nom_state_) { +Eigen::Matrix3x16d calculate_hx(const StateQuat& current_nom_state_) { Eigen::Matrix3x16d Hx = Eigen::Matrix3x16d::Zero(); Eigen::Quaterniond q = current_nom_state_.quat.normalized(); @@ -53,7 +56,8 @@ Eigen::Matrix3x16d calculate_hx(const state_quat& current_nom_state_) { dhdq.col(0) = 2 * (qw * v_n + q_vec.cross(v_n)); dhdq.block<3, 3>(0, 1) = 2 * (q_vec.dot(v_n) * I3 + q_vec * v_n.transpose() - - v_n * q_vec.transpose() - qw * skew(v_n)); + v_n * q_vec.transpose() - + qw * vortex::utils::math::get_skew_symmetric_matrix(v_n)); // Assign quaternion derivative (3x4 block at columns 6:9) Hx.block<3, 4>(0, 6) = dhdq; @@ -61,17 +65,19 @@ Eigen::Matrix3x16d calculate_hx(const state_quat& current_nom_state_) { return Hx; } -Eigen::Matrix3x15d calculate_h_jacobian(const state_quat& current_nom_state_) { +Eigen::Matrix3x15d calculate_h_jacobian(const StateQuat& current_nom_state_) { Eigen::Matrix16x15d x_delta = Eigen::Matrix16x15d::Zero(); x_delta.block<6, 6>(0, 0) = Eigen::Matrix6d::Identity(); - x_delta.block<4, 3>(6, 6) = calculate_T_q(current_nom_state_.quat); + x_delta.block<4, 3>(6, 6) = + vortex::utils::math::get_transformation_matrix_attitude_quat( + current_nom_state_.quat); x_delta.block<6, 6>(10, 9) = Eigen::Matrix6d::Identity(); Eigen::Matrix3x15d H = calculate_hx(current_nom_state_) * x_delta; return H; } -Eigen::Vector3d calculate_h(const state_quat& current_nom_state_) { +Eigen::Vector3d calculate_h(const StateQuat& current_nom_state_) { Eigen::Vector3d h; Eigen::Matrix3d R_bn = current_nom_state_.quat.normalized().toRotationMatrix().transpose(); @@ -80,7 +86,7 @@ Eigen::Vector3d calculate_h(const state_quat& current_nom_state_) { return h; } -void ESKF::nominal_state_discrete(const imu_measurement& imu_meas, +void ESKF::nominal_state_discrete(const ImuMeasurement& imu_meas, const double dt) { Eigen::Vector3d acc = current_nom_state_.quat.normalized().toRotationMatrix() * @@ -89,18 +95,19 @@ void ESKF::nominal_state_discrete(const imu_measurement& imu_meas, Eigen::Vector3d gyro = (imu_meas.gyro - current_nom_state_.gyro_bias) * dt; current_nom_state_.pos = current_nom_state_.pos + - current_nom_state_.vel * dt + 0.5 * sq(dt) * acc; + current_nom_state_.vel * dt + 0.5 * dt * dt * acc; current_nom_state_.vel = current_nom_state_.vel + dt * acc; current_nom_state_.quat = - (current_nom_state_.quat * vector3d_to_quaternion(gyro)); + (current_nom_state_.quat * + vortex::utils::math::eigen_vector3d_to_quaternion(gyro)); current_nom_state_.quat.normalize(); current_nom_state_.accel_bias = current_nom_state_.accel_bias*std::exp(accm_bias_p_*dt); current_nom_state_.gyro_bias = current_nom_state_.gyro_bias*std::exp(gyro_bias_p_*dt); } -void ESKF::error_state_prediction(const imu_measurement& imu_meas, +void ESKF::error_state_prediction(const ImuMeasurement& imu_meas, const double dt) { Eigen::Matrix3d R = current_nom_state_.quat.normalized().toRotationMatrix(); Eigen::Vector3d acc = (imu_meas.accel - current_nom_state_.accel_bias); @@ -108,8 +115,10 @@ void ESKF::error_state_prediction(const imu_measurement& imu_meas, Eigen::Matrix15d A_c = Eigen::Matrix15d::Zero(); A_c.block<3, 3>(0, 3) = Eigen::Matrix3d::Identity(); - A_c.block<3, 3>(3, 6) = -R * skew(acc); - A_c.block<3, 3>(6, 6) = -skew(gyro); + A_c.block<3, 3>(3, 6) = + -R * vortex::utils::math::get_skew_symmetric_matrix(acc); + A_c.block<3, 3>(6, 6) = + -vortex::utils::math::get_skew_symmetric_matrix(gyro); A_c.block<3, 3>(3, 9) = -R; A_c.block<3, 3>(9, 9) = -Eigen::Matrix3d::Identity(); A_c.block<3, 3>(12, 12) = -Eigen::Matrix3d::Identity(); @@ -124,22 +133,18 @@ void ESKF::error_state_prediction(const imu_measurement& imu_meas, Eigen::Matrix15d A_d, GQG_d; std::tie(A_d, GQG_d) = van_loan_discretization(A_c, G_c, dt); - state_euler next_error_state; + StateEuler next_error_state; current_error_state_.covariance = A_d * current_error_state_.covariance * A_d.transpose() + GQG_d; } -void ESKF::NIS(const Eigen::Vector3d& innovation, const Eigen::Matrix3d& S) { - Eigen::Matrix3d S_inv = S.inverse(); - NIS_ = innovation.transpose() * S_inv * innovation; -} void ESKF::injection_and_reset() { current_nom_state_.pos = current_nom_state_.pos + current_error_state_.pos; current_nom_state_.vel = current_nom_state_.vel + current_error_state_.vel; - current_nom_state_.quat = - current_nom_state_.quat * - vector3d_to_quaternion(current_error_state_.euler); + current_nom_state_.quat = current_nom_state_.quat * + vortex::utils::math::eigen_vector3d_to_quaternion( + current_error_state_.euler); current_nom_state_.quat.normalize(); current_nom_state_.gyro_bias = current_nom_state_.gyro_bias + current_error_state_.gyro_bias; @@ -153,36 +158,29 @@ void ESKF::injection_and_reset() { current_error_state_.set_from_vector(Eigen::Vector15d::Zero()); } -std::pair ESKF::imu_update( - const imu_measurement& imu_meas, - const double dt) { +void ESKF::imu_update(const ImuMeasurement& imu_meas, const double dt) { nominal_state_discrete(imu_meas, dt); error_state_prediction(imu_meas, dt); - - return {current_nom_state_, current_error_state_}; } -std::pair ESKF::dvl_update( - const sensor_dvl& dvl_meas) { +void ESKF::dvl_update(const SensorDVL& dvl_meas) { measurement_update(dvl_meas); injection_and_reset(); - - return {current_nom_state_, current_error_state_}; } // DVL sensor model implementations -Eigen::VectorXd sensor_dvl::innovation(const state_quat &state) const { +Eigen::VectorXd SensorDVL::innovation(const StateQuat &state) const { Eigen::Vector3d innovation = this->measurement - calculate_h(state); return innovation; } -Eigen::MatrixXd sensor_dvl::jacobian(const state_quat &state) const { +Eigen::MatrixXd SensorDVL::jacobian(const StateQuat &state) const { Eigen::Matrix3x15d H = calculate_h_jacobian(state); return H; } -Eigen::MatrixXd sensor_dvl::noise_covariance() const { +Eigen::MatrixXd SensorDVL::noise_covariance() const { return this->measurement_noise; } \ No newline at end of file diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp index 80db43a89..9c6ca8761 100644 --- a/navigation/eskf/src/eskf_ros.cpp +++ b/navigation/eskf/src/eskf_ros.cpp @@ -1,7 +1,7 @@ #include "eskf/eskf_ros.hpp" #include #include -#include "eskf/eskf_utils.hpp" +#include #include "eskf/typedefs.hpp" auto start_message{R"( @@ -24,16 +24,14 @@ ESKFNode::ESKFNode(const rclcpp::NodeOptions& options) set_parameters(); spdlog::info(start_message); + #ifndef NDEBUG - spdlog::info("______________________Debug mode is enabled______________________"); + spdlog::info("______________________Debug mode is enabled______________________"); #endif } void ESKFNode::set_subscribers_and_publisher() { - rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; - auto qos_sensor_data = rclcpp::QoS( - rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); - + auto qos_sensor_data = vortex::utils::qos_profiles::sensor_data_profile(1); this->declare_parameter("imu_topic"); std::string imu_topic = this->get_parameter("imu_topic").as_string(); imu_sub_ = this->create_subscription( @@ -51,20 +49,12 @@ void ESKFNode::set_subscribers_and_publisher() { std::string odom_topic = this->get_parameter("odom_topic").as_string(); odom_pub_ = this->create_publisher( odom_topic, qos_sensor_data); - #ifndef NDEBUG - - nis_pub_ = create_publisher("dvl/nis", 10); - - error_pub_ = create_publisher("eskf/error", 10); - odom_cov_pub_ = create_publisher("eskf/odom_covariance", 10); - - odom_ground_truth_ = this->create_subscription( - "/orca/odom", qos_sensor_data, - std::bind(&ESKFNode::ground_truth_callback, this, - std::placeholders::_1)); - // incoming_imu_eskf_ = this->create_publisher("incoming_imu_eskf", rclcpp::SensorDataQoS() ); + #ifndef NDEBUG + nis_pub_ = create_publisher("dvl/nis", vortex::utils::qos_profiles::reliable_profile()); #endif + + cov_pub_ = create_publisher("eskf/covariance", 10); } void ESKFNode::set_parameters() { @@ -79,21 +69,26 @@ void ESKFNode::set_parameters() { diag_Q_std = this->get_parameter("diag_Q_std").as_double_array(); - Eigen::Matrix12d Q; - Q.setZero(); - Q.diagonal() << sq(diag_Q_std[0]), sq(diag_Q_std[1]), sq(diag_Q_std[2]), - sq(diag_Q_std[3]), sq(diag_Q_std[4]), sq(diag_Q_std[5]), - sq(diag_Q_std[6]), sq(diag_Q_std[7]), sq(diag_Q_std[8]), - sq(diag_Q_std[9]), sq(diag_Q_std[10]), sq(diag_Q_std[11]); - eskf_params_.Q = Q; + if (diag_Q_std.size() != 12) { + throw std::runtime_error("diag_Q_std must have length 12"); + } - eskf_ = std::make_unique(eskf_params_); + Eigen::Matrix12d Q = Eigen::Map(diag_Q_std.data()) + .array() + .square() + .matrix() + .asDiagonal(); std::vector diag_p_init = this->declare_parameter>("diag_p_init"); - Eigen::Matrix15d P = create_diagonal_matrix<15>(diag_p_init); + if (diag_p_init.size() != 15) { + throw std::runtime_error("diag_p_init must have length 15"); + } + Eigen::Matrix15d P = createDiagonalMatrix<15>(diag_p_init); - error_state_.covariance = P; + EskfParams eskf_params{.Q = Q, .P = P}; + //error_state_.covariance = P; + eskf_ = std::make_unique(eskf_params); } void ESKFNode::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { @@ -111,105 +106,64 @@ void ESKFNode::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { Eigen::Vector3d raw_accel(msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z); - - // imu_meas_.accel = R_imu_eskf_ * raw_accel; - imu_meas_.accel = -raw_accel; + + ImuMeasurement imu_measurement{}; + imu_measurement.accel = R_imu_eskf_ * raw_accel; Eigen::Vector3d raw_gyro(msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z); - // imu_meas_.gyro = R_imu_eskf_ * raw_gyro; - imu_meas_.gyro = raw_gyro; - - std::tie(nom_state_, error_state_) = eskf_->imu_update(imu_meas_, dt); - - // to remove - // sensor_msgs::msg::Imu imu_out; - // imu_out.header = msg->header; - // imu_out.linear_acceleration.x = imu_meas_.accel.x(); - // imu_out.linear_acceleration.y = imu_meas_.accel.y(); - // imu_out.linear_acceleration.z = imu_meas_.accel.z(); - // imu_out.angular_velocity.x = imu_meas_.gyro.x(); - // imu_out.angular_velocity.y = imu_meas_.gyro.y(); - // imu_out.angular_velocity.z = imu_meas_.gyro.z(); - // incoming_imu_eskf_->publish(imu_out); + // imu_measurement.gyro = R_imu_eskf_ * raw_gyro; + imu_measurement.gyro = raw_gyro; + + eskf_->imu_update(imu_measurement, dt); + + } void ESKFNode::dvl_callback( const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { - dvl_sensor_.measurement << msg->twist.twist.linear.x, msg->twist.twist.linear.y, + SensorDVL dvl_sensor; + dvl_sensor.measurement << msg->twist.twist.linear.x, msg->twist.twist.linear.y, msg->twist.twist.linear.z; - dvl_sensor_.measurement_noise << msg->twist.covariance[0], msg->twist.covariance[1], + dvl_sensor.measurement_noise << msg->twist.covariance[0], msg->twist.covariance[1], msg->twist.covariance[2], msg->twist.covariance[6], msg->twist.covariance[7], msg->twist.covariance[8], msg->twist.covariance[12], msg->twist.covariance[13], msg->twist.covariance[14]; - std::tie(nom_state_, error_state_) = eskf_->dvl_update(dvl_sensor_); + eskf_->dvl_update(dvl_sensor); #ifndef NDEBUG // Publish NIS in Debug mode std_msgs::msg::Float64 nis_msg; - nis_msg.data = eskf_->NIS_; + nis_msg.data = eskf_->get_nis(); nis_pub_->publish(nis_msg); #endif } -#ifndef NDEBUG -void ESKFNode::ground_truth_callback( - const nav_msgs::msg::Odometry::SharedPtr msg) { - - // Position error - last_error_.pos.x() = nom_state_.pos.x() - msg->pose.pose.position.x; - last_error_.pos.y() = nom_state_.pos.y() - msg->pose.pose.position.y; - last_error_.pos.z() = nom_state_.pos.z() - msg->pose.pose.position.z; - - // Velocity error - last_error_.vel.x() = nom_state_.vel.x() - msg->twist.twist.linear.x; - last_error_.vel.y() = nom_state_.vel.y() - msg->twist.twist.linear.y; - last_error_.vel.z() = nom_state_.vel.z() - msg->twist.twist.linear.z; - - // Orientation error - Eigen::Quaterniond q_nom(nom_state_.quat.x(), nom_state_.quat.y(), - nom_state_.quat.z(), nom_state_.quat.w()); - Eigen::Quaterniond q_gt(msg->pose.pose.orientation.x, - msg->pose.pose.orientation.y, - msg->pose.pose.orientation.z, - msg->pose.pose.orientation.w); - - Eigen::Quaterniond q_err = q_gt.inverse() * q_nom; // error rotation - last_error_.ori = quaternion_to_euler(q_err); - - // Publish - std_msgs::msg::Float64MultiArray error_msg; - error_msg.data = { last_error_.pos.x(), last_error_.pos.y(), last_error_.pos.z(), - last_error_.vel.x(), last_error_.vel.y(), last_error_.vel.z(), - last_error_.ori.x(), last_error_.ori.y(), last_error_.ori.z() }; - error_pub_->publish(error_msg); -} -#endif - void ESKFNode::publish_odom() { nav_msgs::msg::Odometry odom_msg; + StateQuat nom_state = eskf_->get_nominal_state(); - odom_msg.pose.pose.position.x = nom_state_.pos.x(); - odom_msg.pose.pose.position.y = nom_state_.pos.y(); - odom_msg.pose.pose.position.z = nom_state_.pos.z(); + odom_msg.pose.pose.position.x = nom_state.pos.x(); + odom_msg.pose.pose.position.y = nom_state.pos.y(); + odom_msg.pose.pose.position.z = nom_state.pos.z(); - odom_msg.pose.pose.orientation.w = nom_state_.quat.w(); - odom_msg.pose.pose.orientation.x = nom_state_.quat.x(); - odom_msg.pose.pose.orientation.y = nom_state_.quat.y(); - odom_msg.pose.pose.orientation.z = nom_state_.quat.z(); + odom_msg.pose.pose.orientation.w = nom_state.quat.w(); + odom_msg.pose.pose.orientation.x = nom_state.quat.x(); + odom_msg.pose.pose.orientation.y = nom_state.quat.y(); + odom_msg.pose.pose.orientation.z = nom_state.quat.z(); - odom_msg.twist.twist.linear.x = nom_state_.vel.x(); - odom_msg.twist.twist.linear.y = nom_state_.vel.y(); - odom_msg.twist.twist.linear.z = nom_state_.vel.z(); + odom_msg.twist.twist.linear.x = nom_state.vel.x(); + odom_msg.twist.twist.linear.y = nom_state.vel.y(); + odom_msg.twist.twist.linear.z = nom_state.vel.z(); // Add bias values to the angular velocity field of twist - odom_msg.twist.twist.angular.x = nom_state_.accel_bias.x(); - odom_msg.twist.twist.angular.y = nom_state_.accel_bias.y(); - odom_msg.twist.twist.angular.z = nom_state_.accel_bias.z(); + odom_msg.twist.twist.angular.x = nom_state.accel_bias.x(); + odom_msg.twist.twist.angular.y = nom_state.accel_bias.y(); + odom_msg.twist.twist.angular.z = nom_state.accel_bias.z(); // If you also want to include gyro bias, you could add it to the covariance // matrix or publish a separate topic for biases @@ -229,6 +183,9 @@ void ESKFNode::publish_odom() { msg.layout.dim[1].size = 15; msg.layout.dim[1].stride = 15; + // get error state covariance + StateEuler error_state_ = eskf_->get_error_state(); + // Flatten Eigen matrix into a std::vector msg.data.resize(15 * 15); msg.data.assign( @@ -236,8 +193,8 @@ void ESKFNode::publish_odom() { error_state_.covariance.data() + 15 * 15 ); - // Publish - odom_cov_pub_->publish(msg); + // Publish covariance + cov_pub_->publish(msg); } RCLCPP_COMPONENTS_REGISTER_NODE(ESKFNode) diff --git a/navigation/eskf/src/eskf_utils.cpp b/navigation/eskf/src/eskf_utils.cpp deleted file mode 100644 index 5cbd704ae..000000000 --- a/navigation/eskf/src/eskf_utils.cpp +++ /dev/null @@ -1,80 +0,0 @@ - -#include "eskf/eskf_utils.hpp" -#include "eskf/typedefs.hpp" - -Eigen::Matrix3d skew(const Eigen::Vector3d& v) { - Eigen::Matrix3d S; - S << 0, -v.z(), v.y(), v.z(), 0, -v.x(), -v.y(), v.x(), 0; - return S; -} - -double sq(double value) { - return value * value; -} -double ssa(double angle) { - double result = fmod(angle + M_PI, 2 * M_PI); - double angle_ssa = result < 0 ? result + M_PI : result - M_PI; - return angle_ssa; -} - -Eigen::Matrix4x3d calculate_T_q(const Eigen::Quaterniond& quat) { - Eigen::Matrix4x3d T_q = Eigen::Matrix4x3d::Zero(); - double qw = quat.w(); - double qx = quat.x(); - double qy = quat.y(); - double qz = quat.z(); - - T_q << -qx, -qy, -qz, qw, -qz, qy, qz, qw, -qx, -qy, qx, qw; - - T_q *= 0.5; - return T_q; -} - -Eigen::Quaterniond vector3d_to_quaternion(const Eigen::Vector3d& vector) { - double angle = vector.norm(); - if (angle < 1e-8) { - return Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0); - } else { - Eigen::Vector3d axis = vector / angle; - Eigen::Quaterniond quat = - Eigen::Quaterniond(Eigen::AngleAxisd(angle, axis)); - return quat.normalized(); - } -} - -Eigen::Quaterniond euler_to_quaternion(const Eigen::Vector3d& euler) { - Eigen::Quaterniond q; - q = Eigen::AngleAxisd(euler.z(), Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(euler.y(), Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(euler.x(), Eigen::Vector3d::UnitX()); - q.normalize(); - return q; -} - -Eigen::Vector3d quaternion_to_euler(const Eigen::Quaterniond& q) { - // Assumes quaternion is normalized - double w = q.w(); - double x = q.x(); - double y = q.y(); - double z = q.z(); - - // Roll (x-axis rotation) - double sinr_cosp = 2.0 * (w * x + y * z); - double cosr_cosp = 1.0 - 2.0 * (x * x + y * y); - double roll = std::atan2(sinr_cosp, cosr_cosp); - - // Pitch (y-axis rotation) - double sinp = 2.0 * (w * y - z * x); - double pitch; - if (std::abs(sinp) >= 1) - pitch = std::copysign(M_PI / 2.0, sinp); // use 90° if out of range - else - pitch = std::asin(sinp); - - // Yaw (z-axis rotation) - double siny_cosp = 2.0 * (w * z + x * y); - double cosy_cosp = 1.0 - 2.0 * (y * y + z * z); - double yaw = std::atan2(siny_cosp, cosy_cosp); - - return Eigen::Vector3d(roll, pitch, yaw); -} From 9771158dcb2ac7ef8c1089bd63b979a3570c584c Mon Sep 17 00:00:00 2001 From: Ahmed Date: Sun, 9 Nov 2025 16:51:23 +0100 Subject: [PATCH 116/290] removed unused variable and old comment --- navigation/eskf/include/eskf/eskf_ros.hpp | 2 -- navigation/eskf/src/eskf_ros.cpp | 2 +- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/navigation/eskf/include/eskf/eskf_ros.hpp b/navigation/eskf/include/eskf/eskf_ros.hpp index a07487e3d..2fa8191a3 100644 --- a/navigation/eskf/include/eskf/eskf_ros.hpp +++ b/navigation/eskf/include/eskf/eskf_ros.hpp @@ -58,8 +58,6 @@ class ESKFNode : public rclcpp::Node { bool first_imu_msg_received_ = false; - bool debug_on_ = false; - Eigen::Matrix3d R_imu_eskf_{}; rclcpp::Time last_imu_time_{}; diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp index 9c6ca8761..a5dbca3ae 100644 --- a/navigation/eskf/src/eskf_ros.cpp +++ b/navigation/eskf/src/eskf_ros.cpp @@ -87,7 +87,7 @@ void ESKFNode::set_parameters() { Eigen::Matrix15d P = createDiagonalMatrix<15>(diag_p_init); EskfParams eskf_params{.Q = Q, .P = P}; - //error_state_.covariance = P; + eskf_ = std::make_unique(eskf_params); } From e7e645000cbdd533dc708f490d7c041c65090ece Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Sun, 9 Nov 2025 17:42:46 +0000 Subject: [PATCH 117/290] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- navigation/eskf/include/eskf/eskf.hpp | 6 +-- navigation/eskf/include/eskf/eskf.tpp | 2 +- navigation/eskf/include/eskf/typedefs.hpp | 12 +++--- navigation/eskf/src/eskf.cpp | 30 +++++++-------- navigation/eskf/src/eskf_ros.cpp | 46 +++++++++++------------ 5 files changed, 46 insertions(+), 50 deletions(-) diff --git a/navigation/eskf/include/eskf/eskf.hpp b/navigation/eskf/include/eskf/eskf.hpp index 8d04f12d7..7fa27a3ce 100644 --- a/navigation/eskf/include/eskf/eskf.hpp +++ b/navigation/eskf/include/eskf/eskf.hpp @@ -46,10 +46,11 @@ class ESKF { void NIS(const Eigen::Vector3d& innovation, const Eigen::Matrix3d& S); // @brief Update the error state using a generic sensor measurement model - // @tparam SensorT Type of the sensor model (must satisfy SensorModelConcept) + // @tparam SensorT Type of the sensor model (must satisfy + // SensorModelConcept) // @param meas Sensor measurement instance template - void measurement_update(const SensorT& meas); + void measurement_update(const SensorT& meas); // @brief Inject the error state into the nominal state and reset the error void injection_and_reset(); @@ -63,7 +64,6 @@ class ESKF { const Eigen::Matrix15x12d& G_c, const double dt); - // Process noise covariance matrix Eigen::Matrix12d Q_{}; diff --git a/navigation/eskf/include/eskf/eskf.tpp b/navigation/eskf/include/eskf/eskf.tpp index ba7a1f859..56afcb3a3 100644 --- a/navigation/eskf/include/eskf/eskf.tpp +++ b/navigation/eskf/include/eskf/eskf.tpp @@ -22,4 +22,4 @@ void ESKF::measurement_update(const SensorT& meas) current_error_state_.covariance = I_KH * P * I_KH.transpose() + K * R * K.transpose(); // Used joseph form for more stable calculations -} \ No newline at end of file +} diff --git a/navigation/eskf/include/eskf/typedefs.hpp b/navigation/eskf/include/eskf/typedefs.hpp index 5585b0455..d8745b46e 100644 --- a/navigation/eskf/include/eskf/typedefs.hpp +++ b/navigation/eskf/include/eskf/typedefs.hpp @@ -5,10 +5,10 @@ #ifndef ESKF_TYPEDEFS_H #define ESKF_TYPEDEFS_H +#include #include #include #include -#include namespace Eigen { typedef Eigen::Matrix Vector19d; @@ -109,9 +109,8 @@ struct DvlMeasurement { Eigen::Matrix3d cov = Eigen::Matrix3d::Zero(); }; -template -concept SensorModelConcept = requires(const T &meas, const StateQuat &state) -{ +template +concept SensorModelConcept = requires(const T& meas, const StateQuat& state) { { meas.innovation(state) } -> std::convertible_to; { meas.jacobian(state) } -> std::convertible_to; { meas.noise_covariance() } -> std::convertible_to; @@ -120,10 +119,9 @@ concept SensorModelConcept = requires(const T &meas, const StateQuat &state) struct SensorDVL { Eigen::Vector3d measurement; Eigen::Matrix3d measurement_noise; - Eigen::VectorXd innovation(const StateQuat &state) const; - Eigen::MatrixXd jacobian(const StateQuat &state) const; + Eigen::VectorXd innovation(const StateQuat& state) const; + Eigen::MatrixXd jacobian(const StateQuat& state) const; Eigen::MatrixXd noise_covariance() const; }; - #endif // ESKF_TYPEDEFS_H diff --git a/navigation/eskf/src/eskf.cpp b/navigation/eskf/src/eskf.cpp index 2e623a749..1073a50cf 100644 --- a/navigation/eskf/src/eskf.cpp +++ b/navigation/eskf/src/eskf.cpp @@ -68,9 +68,9 @@ Eigen::Matrix3x16d calculate_hx(const StateQuat& current_nom_state_) { Eigen::Matrix3x15d calculate_h_jacobian(const StateQuat& current_nom_state_) { Eigen::Matrix16x15d x_delta = Eigen::Matrix16x15d::Zero(); x_delta.block<6, 6>(0, 0) = Eigen::Matrix6d::Identity(); - x_delta.block<4, 3>(6, 6) = - vortex::utils::math::get_transformation_matrix_attitude_quat( - current_nom_state_.quat); + x_delta.block<4, 3>(6, 6) = + vortex::utils::math::get_transformation_matrix_attitude_quat( + current_nom_state_.quat); x_delta.block<6, 6>(10, 9) = Eigen::Matrix6d::Identity(); Eigen::Matrix3x15d H = calculate_hx(current_nom_state_) * x_delta; @@ -99,12 +99,14 @@ void ESKF::nominal_state_discrete(const ImuMeasurement& imu_meas, current_nom_state_.vel = current_nom_state_.vel + dt * acc; current_nom_state_.quat = - (current_nom_state_.quat * - vortex::utils::math::eigen_vector3d_to_quaternion(gyro)); + (current_nom_state_.quat * + vortex::utils::math::eigen_vector3d_to_quaternion(gyro)); current_nom_state_.quat.normalize(); - current_nom_state_.accel_bias = current_nom_state_.accel_bias*std::exp(accm_bias_p_*dt); - current_nom_state_.gyro_bias = current_nom_state_.gyro_bias*std::exp(gyro_bias_p_*dt); + current_nom_state_.accel_bias = + current_nom_state_.accel_bias * std::exp(accm_bias_p_ * dt); + current_nom_state_.gyro_bias = + current_nom_state_.gyro_bias * std::exp(gyro_bias_p_ * dt); } void ESKF::error_state_prediction(const ImuMeasurement& imu_meas, @@ -117,7 +119,7 @@ void ESKF::error_state_prediction(const ImuMeasurement& imu_meas, A_c.block<3, 3>(0, 3) = Eigen::Matrix3d::Identity(); A_c.block<3, 3>(3, 6) = -R * vortex::utils::math::get_skew_symmetric_matrix(acc); - A_c.block<3, 3>(6, 6) = + A_c.block<3, 3>(6, 6) = -vortex::utils::math::get_skew_symmetric_matrix(gyro); A_c.block<3, 3>(3, 9) = -R; A_c.block<3, 3>(9, 9) = -Eigen::Matrix3d::Identity(); @@ -138,13 +140,12 @@ void ESKF::error_state_prediction(const ImuMeasurement& imu_meas, A_d * current_error_state_.covariance * A_d.transpose() + GQG_d; } - void ESKF::injection_and_reset() { current_nom_state_.pos = current_nom_state_.pos + current_error_state_.pos; current_nom_state_.vel = current_nom_state_.vel + current_error_state_.vel; current_nom_state_.quat = current_nom_state_.quat * - vortex::utils::math::eigen_vector3d_to_quaternion( - current_error_state_.euler); + vortex::utils::math::eigen_vector3d_to_quaternion( + current_error_state_.euler); current_nom_state_.quat.normalize(); current_nom_state_.gyro_bias = current_nom_state_.gyro_bias + current_error_state_.gyro_bias; @@ -170,17 +171,16 @@ void ESKF::dvl_update(const SensorDVL& dvl_meas) { // DVL sensor model implementations -Eigen::VectorXd SensorDVL::innovation(const StateQuat &state) const { - +Eigen::VectorXd SensorDVL::innovation(const StateQuat& state) const { Eigen::Vector3d innovation = this->measurement - calculate_h(state); return innovation; } -Eigen::MatrixXd SensorDVL::jacobian(const StateQuat &state) const { +Eigen::MatrixXd SensorDVL::jacobian(const StateQuat& state) const { Eigen::Matrix3x15d H = calculate_h_jacobian(state); return H; } Eigen::MatrixXd SensorDVL::noise_covariance() const { return this->measurement_noise; -} \ No newline at end of file +} diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp index a5dbca3ae..248e83434 100644 --- a/navigation/eskf/src/eskf_ros.cpp +++ b/navigation/eskf/src/eskf_ros.cpp @@ -25,9 +25,10 @@ ESKFNode::ESKFNode(const rclcpp::NodeOptions& options) spdlog::info(start_message); - #ifndef NDEBUG - spdlog::info("______________________Debug mode is enabled______________________"); - #endif +#ifndef NDEBUG + spdlog::info( + "______________________Debug mode is enabled______________________"); +#endif } void ESKFNode::set_subscribers_and_publisher() { @@ -50,11 +51,13 @@ void ESKFNode::set_subscribers_and_publisher() { odom_pub_ = this->create_publisher( odom_topic, qos_sensor_data); - #ifndef NDEBUG - nis_pub_ = create_publisher("dvl/nis", vortex::utils::qos_profiles::reliable_profile()); - #endif +#ifndef NDEBUG + nis_pub_ = create_publisher( + "dvl/nis", vortex::utils::qos_profiles::reliable_profile()); +#endif - cov_pub_ = create_publisher("eskf/covariance", 10); + cov_pub_ = create_publisher( + "eskf/covariance", 10); } void ESKFNode::set_parameters() { @@ -106,7 +109,7 @@ void ESKFNode::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { Eigen::Vector3d raw_accel(msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z); - + ImuMeasurement imu_measurement{}; imu_measurement.accel = R_imu_eskf_ * raw_accel; @@ -117,30 +120,28 @@ void ESKFNode::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { imu_measurement.gyro = raw_gyro; eskf_->imu_update(imu_measurement, dt); - - } void ESKFNode::dvl_callback( const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { SensorDVL dvl_sensor; - dvl_sensor.measurement << msg->twist.twist.linear.x, msg->twist.twist.linear.y, - msg->twist.twist.linear.z; + dvl_sensor.measurement << msg->twist.twist.linear.x, + msg->twist.twist.linear.y, msg->twist.twist.linear.z; - dvl_sensor.measurement_noise << msg->twist.covariance[0], msg->twist.covariance[1], - msg->twist.covariance[2], msg->twist.covariance[6], - msg->twist.covariance[7], msg->twist.covariance[8], - msg->twist.covariance[12], msg->twist.covariance[13], - msg->twist.covariance[14]; + dvl_sensor.measurement_noise << msg->twist.covariance[0], + msg->twist.covariance[1], msg->twist.covariance[2], + msg->twist.covariance[6], msg->twist.covariance[7], + msg->twist.covariance[8], msg->twist.covariance[12], + msg->twist.covariance[13], msg->twist.covariance[14]; eskf_->dvl_update(dvl_sensor); - #ifndef NDEBUG +#ifndef NDEBUG // Publish NIS in Debug mode std_msgs::msg::Float64 nis_msg; nis_msg.data = eskf_->get_nis(); nis_pub_->publish(nis_msg); - #endif +#endif } void ESKFNode::publish_odom() { @@ -171,7 +172,6 @@ void ESKFNode::publish_odom() { odom_msg.header.stamp = this->now(); odom_pub_->publish(odom_msg); - // publish the covariance matrix std_msgs::msg::Float64MultiArray msg; @@ -188,10 +188,8 @@ void ESKFNode::publish_odom() { // Flatten Eigen matrix into a std::vector msg.data.resize(15 * 15); - msg.data.assign( - error_state_.covariance.data(), - error_state_.covariance.data() + 15 * 15 - ); + msg.data.assign(error_state_.covariance.data(), + error_state_.covariance.data() + 15 * 15); // Publish covariance cov_pub_->publish(msg); From ed87ce65f09bec2fc26f7b46876c04187e5b17d5 Mon Sep 17 00:00:00 2001 From: Ahmed Date: Mon, 5 Jan 2026 12:00:27 +0100 Subject: [PATCH 118/290] clang-format fix --- navigation/eskf/include/eskf/eskf.tpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/navigation/eskf/include/eskf/eskf.tpp b/navigation/eskf/include/eskf/eskf.tpp index 56afcb3a3..c02d7d4f7 100644 --- a/navigation/eskf/include/eskf/eskf.tpp +++ b/navigation/eskf/include/eskf/eskf.tpp @@ -1,8 +1,7 @@ template -void ESKF::measurement_update(const SensorT& meas) -{ +void ESKF::measurement_update(const SensorT& meas) { Eigen::VectorXd innovation = meas.innovation(current_nom_state_); Eigen::MatrixXd H = meas.jacobian(current_nom_state_); Eigen::MatrixXd R = meas.noise_covariance(); @@ -12,13 +11,14 @@ void ESKF::measurement_update(const SensorT& meas) Eigen::MatrixXd S = H * P * H.transpose() + R; Eigen::MatrixXd K = P * H.transpose() * S.inverse(); - #ifndef NDEBUG +#ifndef NDEBUG nis_ = compute_nis(innovation, S); - #endif +#endif current_error_state_.set_from_vector(K * innovation); - Eigen::MatrixXd I_KH = Eigen::MatrixXd::Identity(P.rows(), P.cols()) - K * H; + Eigen::MatrixXd I_KH = + Eigen::MatrixXd::Identity(P.rows(), P.cols()) - K * H; current_error_state_.covariance = I_KH * P * I_KH.transpose() + K * R * K.transpose(); // Used joseph form for more stable calculations From 8e9fb52bf243e28491810b292c2291b865dff20a Mon Sep 17 00:00:00 2001 From: Ahmed Date: Mon, 5 Jan 2026 14:19:58 +0100 Subject: [PATCH 119/290] ament_cpplint fixes --- navigation/eskf/include/eskf/eskf.hpp | 8 ++++---- navigation/eskf/include/eskf/eskf_ros.hpp | 7 ++++--- navigation/eskf/include/eskf/typedefs.hpp | 8 ++++---- 3 files changed, 12 insertions(+), 11 deletions(-) diff --git a/navigation/eskf/include/eskf/eskf.hpp b/navigation/eskf/include/eskf/eskf.hpp index 7fa27a3ce..66fe42c0d 100644 --- a/navigation/eskf/include/eskf/eskf.hpp +++ b/navigation/eskf/include/eskf/eskf.hpp @@ -1,5 +1,5 @@ -#ifndef ESKF_HPP -#define ESKF_HPP +#ifndef ESKF__ESKF_HPP_ +#define ESKF__ESKF_HPP_ #include #include @@ -8,7 +8,7 @@ class ESKF { public: - ESKF(const EskfParams& params); + explicit ESKF(const EskfParams& params); // @brief Update the nominal state and error state // @param imu_meas: IMU measurement @@ -103,4 +103,4 @@ double compute_nis(const Eigen::Vector3d& innovation, const Eigen::Matrix3d& S); #include "eskf.tpp" // including template implementation -#endif // ESKF_HPP +#endif // ESKF__ESKF_HPP_ diff --git a/navigation/eskf/include/eskf/eskf_ros.hpp b/navigation/eskf/include/eskf/eskf_ros.hpp index 2fa8191a3..31218274f 100644 --- a/navigation/eskf/include/eskf/eskf_ros.hpp +++ b/navigation/eskf/include/eskf/eskf_ros.hpp @@ -1,9 +1,10 @@ -#ifndef ESKF_ROS_HPP -#define ESKF_ROS_HPP +#ifndef ESKF__ESKF_ROS_HPP_ +#define ESKF__ESKF_ROS_HPP_ #include #include #include +#include #include #include #include @@ -63,4 +64,4 @@ class ESKFNode : public rclcpp::Node { rclcpp::Time last_imu_time_{}; }; -#endif // ESKF_ROS_HPP +#endif // ESKF__ESKF_ROS_HPP_ diff --git a/navigation/eskf/include/eskf/typedefs.hpp b/navigation/eskf/include/eskf/typedefs.hpp index d8745b46e..56b6a224b 100644 --- a/navigation/eskf/include/eskf/typedefs.hpp +++ b/navigation/eskf/include/eskf/typedefs.hpp @@ -2,8 +2,8 @@ * @file typedefs.hpp * @brief Contains the typedef and structs for the eskf. */ -#ifndef ESKF_TYPEDEFS_H -#define ESKF_TYPEDEFS_H +#ifndef ESKF__TYPEDEFS_HPP_ +#define ESKF__TYPEDEFS_HPP_ #include #include @@ -114,7 +114,7 @@ concept SensorModelConcept = requires(const T& meas, const StateQuat& state) { { meas.innovation(state) } -> std::convertible_to; { meas.jacobian(state) } -> std::convertible_to; { meas.noise_covariance() } -> std::convertible_to; -}; +}; // NOLINT(readability/braces) struct SensorDVL { Eigen::Vector3d measurement; @@ -124,4 +124,4 @@ struct SensorDVL { Eigen::MatrixXd noise_covariance() const; }; -#endif // ESKF_TYPEDEFS_H +#endif // ESKF__TYPEDEFS_HPP_ From 56fbcccdf70ebe0c95255ba1b4da47bf8e36c342 Mon Sep 17 00:00:00 2001 From: Ahmed Date: Mon, 5 Jan 2026 15:04:49 +0100 Subject: [PATCH 120/290] missing dependency --- navigation/eskf/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/navigation/eskf/package.xml b/navigation/eskf/package.xml index 015e6f71c..588855609 100644 --- a/navigation/eskf/package.xml +++ b/navigation/eskf/package.xml @@ -15,6 +15,7 @@ eigen tf2 vortex_msgs + vortex_utils ament_cmake From 03f2daecc8c474e71a79f5d2c3df7858e3479c6d Mon Sep 17 00:00:00 2001 From: Ahmed Date: Mon, 5 Jan 2026 15:46:58 +0100 Subject: [PATCH 121/290] updating yaml, P(cov) is 15*15 now --- navigation/eskf/config/eskf_params.yaml | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/navigation/eskf/config/eskf_params.yaml b/navigation/eskf/config/eskf_params.yaml index 81c38deac..507c86de9 100644 --- a/navigation/eskf/config/eskf_params.yaml +++ b/navigation/eskf/config/eskf_params.yaml @@ -3,9 +3,6 @@ eskf_node: imu_topic: /imu/data_raw dvl_topic: /orca/twist odom_topic: odom_ESKF - #diag_Q_std: [0.0124, 0.0124, 0.0124, 0.0026, 0.0026, 0.0026, 0.000392, 0.000392, 0.000392, 0.00001145, 0.0000145, 0.0000145] diag_Q_std: [0.05, 0.05, 0.1, 0.01, 0.01, 0.02, 0.001, 0.001, 0.001, 0.0001, 0.0001, 0.0001] - # diag_p_init: [1.0, 1.0, 1.0, 0.5, 0.5, 0.5, 0.1, 0.1, 0.1, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001] - diag_p_init: [1.0, 1.0, 1.0, 0.5, 0.5, 0.5, 0.1, 0.1, 0.1, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001] - # imu_frame: [0.0, 0.0, -1.0, 0.0, -1.0, 0.0, -1.0, 0.0, 0.0] + diag_p_init: [1.0, 1.0, 1.0, 0.5, 0.5, 0.5, 0.1, 0.1, 0.1, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001] imu_frame: [-1.0, 0.0, 0.0, -1.0, 0.0, 0.0, -1.0, 0.0, 0.0] From ec0ffcf8f037720b226c96f3fe41166c672c9043 Mon Sep 17 00:00:00 2001 From: Ahmed Date: Mon, 5 Jan 2026 16:29:25 +0100 Subject: [PATCH 122/290] update for the separate vortex_utils --- navigation/eskf/CMakeLists.txt | 2 ++ navigation/eskf/package.xml | 1 + navigation/eskf/src/eskf_ros.cpp | 2 +- 3 files changed, 4 insertions(+), 1 deletion(-) diff --git a/navigation/eskf/CMakeLists.txt b/navigation/eskf/CMakeLists.txt index 28af1cd06..0028693bf 100644 --- a/navigation/eskf/CMakeLists.txt +++ b/navigation/eskf/CMakeLists.txt @@ -22,6 +22,7 @@ find_package(Eigen3 REQUIRED) find_package(tf2 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) @@ -49,6 +50,7 @@ ament_target_dependencies(${LIB_NAME} PUBLIC tf2 vortex_msgs vortex_utils + vortex_utils_ros spdlog fmt ) diff --git a/navigation/eskf/package.xml b/navigation/eskf/package.xml index 588855609..f8472c188 100644 --- a/navigation/eskf/package.xml +++ b/navigation/eskf/package.xml @@ -16,6 +16,7 @@ tf2 vortex_msgs vortex_utils + vortex_utils_ros ament_cmake diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp index 248e83434..63fe6e2aa 100644 --- a/navigation/eskf/src/eskf_ros.cpp +++ b/navigation/eskf/src/eskf_ros.cpp @@ -1,7 +1,7 @@ #include "eskf/eskf_ros.hpp" #include #include -#include +#include #include "eskf/typedefs.hpp" auto start_message{R"( From 08b88d2ca13585c5ee643a704198b2c5134448e9 Mon Sep 17 00:00:00 2001 From: Ahmed Date: Fri, 16 Jan 2026 16:36:33 +0100 Subject: [PATCH 123/290] puting the covariance in the odom msg. Fixing the orientation for IMU, it seems the accelerometer and gyro have different frames --- navigation/eskf/config/eskf_params.yaml | 7 ++- navigation/eskf/include/eskf/eskf_ros.hpp | 6 +- navigation/eskf/src/eskf_ros.cpp | 75 +++++++++++++---------- 3 files changed, 51 insertions(+), 37 deletions(-) diff --git a/navigation/eskf/config/eskf_params.yaml b/navigation/eskf/config/eskf_params.yaml index 507c86de9..9352e3f6e 100644 --- a/navigation/eskf/config/eskf_params.yaml +++ b/navigation/eskf/config/eskf_params.yaml @@ -5,4 +5,9 @@ eskf_node: odom_topic: odom_ESKF diag_Q_std: [0.05, 0.05, 0.1, 0.01, 0.01, 0.02, 0.001, 0.001, 0.001, 0.0001, 0.0001, 0.0001] diag_p_init: [1.0, 1.0, 1.0, 0.5, 0.5, 0.5, 0.1, 0.1, 0.1, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001] - imu_frame: [-1.0, 0.0, 0.0, -1.0, 0.0, 0.0, -1.0, 0.0, 0.0] + imu_acc_frame: [ -1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, -1.0 ] + imu_gyro_frame: [ 1.0, 0.0, 0.0, + 0.0, -1.0, 0.0, + 0.0, 0.0, 1.0 ] diff --git a/navigation/eskf/include/eskf/eskf_ros.hpp b/navigation/eskf/include/eskf/eskf_ros.hpp index 31218274f..9b8384608 100644 --- a/navigation/eskf/include/eskf/eskf_ros.hpp +++ b/navigation/eskf/include/eskf/eskf_ros.hpp @@ -47,8 +47,6 @@ class ESKFNode : public rclcpp::Node { rclcpp::Publisher::SharedPtr odom_pub_; - rclcpp::Publisher::SharedPtr cov_pub_; - rclcpp::Publisher::SharedPtr nis_pub_; std::chrono::milliseconds time_step; @@ -59,7 +57,9 @@ class ESKFNode : public rclcpp::Node { bool first_imu_msg_received_ = false; - Eigen::Matrix3d R_imu_eskf_{}; + Eigen::Matrix3d R_imu_acc_eskf_{}; + + Eigen::Matrix3d R_imu_gyro_eskf_{}; rclcpp::Time last_imu_time_{}; }; diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp index 63fe6e2aa..3392b3983 100644 --- a/navigation/eskf/src/eskf_ros.cpp +++ b/navigation/eskf/src/eskf_ros.cpp @@ -53,19 +53,22 @@ void ESKFNode::set_subscribers_and_publisher() { #ifndef NDEBUG nis_pub_ = create_publisher( - "dvl/nis", vortex::utils::qos_profiles::reliable_profile()); + "eskf/nis", vortex::utils::qos_profiles::reliable_profile()); #endif - - cov_pub_ = create_publisher( - "eskf/covariance", 10); } void ESKFNode::set_parameters() { - std::vector R_imu_correction; - this->declare_parameter>("imu_frame"); - R_imu_correction = get_parameter("imu_frame").as_double_array(); - R_imu_eskf_ = Eigen::Map>( - R_imu_correction.data()); + std::vector R_imu_acc_correction; + this->declare_parameter>("imu_acc_frame"); + R_imu_acc_correction = get_parameter("imu_acc_frame").as_double_array(); + R_imu_acc_eskf_ = Eigen::Map>( + R_imu_acc_correction.data()); + + std::vector R_imu_gyro_correction; + this->declare_parameter>("imu_gyro_frame"); + R_imu_gyro_correction = get_parameter("imu_gyro_frame").as_double_array(); + R_imu_gyro_eskf_ = Eigen::Map>( + R_imu_gyro_correction.data()); std::vector diag_Q_std; this->declare_parameter>("diag_Q_std"); @@ -111,13 +114,12 @@ void ESKFNode::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { msg->linear_acceleration.z); ImuMeasurement imu_measurement{}; - imu_measurement.accel = R_imu_eskf_ * raw_accel; + imu_measurement.accel = R_imu_acc_eskf_ * raw_accel; Eigen::Vector3d raw_gyro(msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z); - // imu_measurement.gyro = R_imu_eskf_ * raw_gyro; - imu_measurement.gyro = raw_gyro; + imu_measurement.gyro = R_imu_gyro_eskf_ * raw_gyro; eskf_->imu_update(imu_measurement, dt); } @@ -147,6 +149,7 @@ void ESKFNode::dvl_callback( void ESKFNode::publish_odom() { nav_msgs::msg::Odometry odom_msg; StateQuat nom_state = eskf_->get_nominal_state(); + StateEuler error_state_ = eskf_->get_error_state(); odom_msg.pose.pose.position.x = nom_state.pos.x(); odom_msg.pose.pose.position.y = nom_state.pos.y(); @@ -170,29 +173,35 @@ void ESKFNode::publish_odom() { // matrix or publish a separate topic for biases odom_msg.header.stamp = this->now(); - odom_pub_->publish(odom_msg); - - // publish the covariance matrix - std_msgs::msg::Float64MultiArray msg; - - msg.layout.dim.resize(2); - msg.layout.dim[0].label = "rows"; - msg.layout.dim[0].size = 15; - msg.layout.dim[0].stride = 15 * 15; - msg.layout.dim[1].label = "cols"; - msg.layout.dim[1].size = 15; - msg.layout.dim[1].stride = 15; - - // get error state covariance - StateEuler error_state_ = eskf_->get_error_state(); + odom_msg.header.frame_id = "odom"; + + // The covariance is published in a way that some cross terms are ignored, + // if using this cov with other estimator, the whole 15*15 should be published + // Covariance of pos and orientation needs to be mapped from 6*6 matrix to an array + // Position covariance (states 0-2) + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + odom_msg.pose.covariance[i * 6 + j] = + error_state_.covariance(i, j); + } + } - // Flatten Eigen matrix into a std::vector - msg.data.resize(15 * 15); - msg.data.assign(error_state_.covariance.data(), - error_state_.covariance.data() + 15 * 15); + // Orientation covariance (states 6–8) + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + odom_msg.pose.covariance[(i + 3) * 6 + (j + 3)] = + error_state_.covariance(i + 6, j + 6); + } + } - // Publish covariance - cov_pub_->publish(msg); + // Linear velocity covariance + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + odom_msg.twist.covariance[i * 6 + j] = + error_state_.covariance(i + 3, j + 3); + } + } + odom_pub_->publish(odom_msg); } RCLCPP_COMPONENTS_REGISTER_NODE(ESKFNode) From 0ea28f04ca2b4445b7aa6746ad93babdd5430a6b Mon Sep 17 00:00:00 2001 From: Ahmed Date: Fri, 16 Jan 2026 16:46:21 +0100 Subject: [PATCH 124/290] clnag-format fix --- navigation/eskf/src/eskf_ros.cpp | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp index 3392b3983..4a55fbbb4 100644 --- a/navigation/eskf/src/eskf_ros.cpp +++ b/navigation/eskf/src/eskf_ros.cpp @@ -175,14 +175,13 @@ void ESKFNode::publish_odom() { odom_msg.header.stamp = this->now(); odom_msg.header.frame_id = "odom"; - // The covariance is published in a way that some cross terms are ignored, - // if using this cov with other estimator, the whole 15*15 should be published - // Covariance of pos and orientation needs to be mapped from 6*6 matrix to an array - // Position covariance (states 0-2) + // Some cross terms of the covariance are ignored, and the acc/gyro biases + // cov are not published. Pos and orientation cov needs to be mapped from + // 6*6 matrix to an array (states 0-2) + for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { - odom_msg.pose.covariance[i * 6 + j] = - error_state_.covariance(i, j); + odom_msg.pose.covariance[i * 6 + j] = error_state_.covariance(i, j); } } From c945ab871776134fd28f100f32ff0932d0c85c7d Mon Sep 17 00:00:00 2001 From: Ahmed Date: Fri, 6 Feb 2026 12:42:19 +0100 Subject: [PATCH 125/290] code for debugging+NEES/RMSE test file + fix for odom ang vel output --- navigation/eskf/config/eskf_params.yaml | 15 +- navigation/eskf/include/eskf/eskf_ros.hpp | 13 +- navigation/eskf/src/eskf_ros.cpp | 76 +++++++-- navigation/eskf/test/eskf_consistency_test.py | 149 ++++++++++++++++++ 4 files changed, 229 insertions(+), 24 deletions(-) create mode 100644 navigation/eskf/test/eskf_consistency_test.py diff --git a/navigation/eskf/config/eskf_params.yaml b/navigation/eskf/config/eskf_params.yaml index 9352e3f6e..a5cb7d161 100644 --- a/navigation/eskf/config/eskf_params.yaml +++ b/navigation/eskf/config/eskf_params.yaml @@ -5,9 +5,12 @@ eskf_node: odom_topic: odom_ESKF diag_Q_std: [0.05, 0.05, 0.1, 0.01, 0.01, 0.02, 0.001, 0.001, 0.001, 0.0001, 0.0001, 0.0001] diag_p_init: [1.0, 1.0, 1.0, 0.5, 0.5, 0.5, 0.1, 0.1, 0.1, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001] - imu_acc_frame: [ -1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, -1.0 ] - imu_gyro_frame: [ 1.0, 0.0, 0.0, - 0.0, -1.0, 0.0, - 0.0, 0.0, 1.0 ] + imu_frame: [ -1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, -1.0 ] + dvl_frame: [ 0.0, 1.0, 0.0, + -1.0, 0.0, 0.0, + 0.0, 0.0, 1.0 ] + # dvl_frame: [ 1.0, 0.0, 0.0, + # 0.0, 1.0, 0.0, + # 0.0, 0.0, 1.0 ] \ No newline at end of file diff --git a/navigation/eskf/include/eskf/eskf_ros.hpp b/navigation/eskf/include/eskf/eskf_ros.hpp index 9b8384608..2407a685d 100644 --- a/navigation/eskf/include/eskf/eskf_ros.hpp +++ b/navigation/eskf/include/eskf/eskf_ros.hpp @@ -49,6 +49,12 @@ class ESKFNode : public rclcpp::Node { rclcpp::Publisher::SharedPtr nis_pub_; + // temp debug + rclcpp::Publisher::SharedPtr imu_rotated_pub_; + rclcpp::Publisher::SharedPtr dvl_rotated_pub_; + + // end temp debug + std::chrono::milliseconds time_step; rclcpp::TimerBase::SharedPtr odom_pub_timer_; @@ -57,11 +63,14 @@ class ESKFNode : public rclcpp::Node { bool first_imu_msg_received_ = false; - Eigen::Matrix3d R_imu_acc_eskf_{}; + Eigen::Matrix3d R_imu_eskf_{}; - Eigen::Matrix3d R_imu_gyro_eskf_{}; + Eigen::Matrix3d R_dvl_eskf_{}; rclcpp::Time last_imu_time_{}; + + // Latest gyro measurement (used for publishing odom output of eskf) + Eigen::Vector3d latest_gyro_measurement_{}; }; #endif // ESKF__ESKF_ROS_HPP_ diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp index 4a55fbbb4..c885fa857 100644 --- a/navigation/eskf/src/eskf_ros.cpp +++ b/navigation/eskf/src/eskf_ros.cpp @@ -51,6 +51,13 @@ void ESKFNode::set_subscribers_and_publisher() { odom_pub_ = this->create_publisher( odom_topic, qos_sensor_data); + // temp debug + imu_rotated_pub_ = this->create_publisher( + "eskf/imu_rotated", vortex::utils::qos_profiles::sensor_data_profile()); + + dvl_rotated_pub_ = this->create_publisher( + "eskf/dvl_rotated", vortex::utils::qos_profiles::sensor_data_profile()); + #ifndef NDEBUG nis_pub_ = create_publisher( "eskf/nis", vortex::utils::qos_profiles::reliable_profile()); @@ -58,17 +65,18 @@ void ESKFNode::set_subscribers_and_publisher() { } void ESKFNode::set_parameters() { - std::vector R_imu_acc_correction; - this->declare_parameter>("imu_acc_frame"); - R_imu_acc_correction = get_parameter("imu_acc_frame").as_double_array(); - R_imu_acc_eskf_ = Eigen::Map>( - R_imu_acc_correction.data()); - - std::vector R_imu_gyro_correction; - this->declare_parameter>("imu_gyro_frame"); - R_imu_gyro_correction = get_parameter("imu_gyro_frame").as_double_array(); - R_imu_gyro_eskf_ = Eigen::Map>( - R_imu_gyro_correction.data()); + + std::vector R_imu_correction; + this->declare_parameter>("imu_frame"); + R_imu_correction = get_parameter("imu_frame").as_double_array(); + R_imu_eskf_ = Eigen::Map>( + R_imu_correction.data()); + + std::vector R_dvl_correction; + this->declare_parameter>("dvl_frame"); + R_dvl_correction = get_parameter("dvl_frame").as_double_array(); + R_dvl_eskf_ = Eigen::Map>( + R_dvl_correction.data()); std::vector diag_Q_std; this->declare_parameter>("diag_Q_std"); @@ -114,12 +122,31 @@ void ESKFNode::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { msg->linear_acceleration.z); ImuMeasurement imu_measurement{}; - imu_measurement.accel = R_imu_acc_eskf_ * raw_accel; + imu_measurement.accel = R_imu_eskf_ * raw_accel; Eigen::Vector3d raw_gyro(msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z); - imu_measurement.gyro = R_imu_gyro_eskf_ * raw_gyro; + imu_measurement.gyro = R_imu_eskf_ * raw_gyro; + // imu_measurement.gyro = raw_gyro; + + // used for publishing odom output of eskf + latest_gyro_measurement_ = imu_measurement.gyro; + + // temp debug + // ========================================== + sensor_msgs::msg::Imu imu_msg_out = *msg; // Copy header and covariances + + // Overwrite vectors with corrected data + imu_msg_out.linear_acceleration.x = imu_measurement.accel.x(); + imu_msg_out.linear_acceleration.y = imu_measurement.accel.y(); + imu_msg_out.linear_acceleration.z = imu_measurement.accel.z(); + + imu_msg_out.angular_velocity.x = imu_measurement.gyro.x(); + imu_msg_out.angular_velocity.y = imu_measurement.gyro.y(); + imu_msg_out.angular_velocity.z = imu_measurement.gyro.z(); + imu_rotated_pub_->publish(imu_msg_out); + // eskf_->imu_update(imu_measurement, dt); } @@ -136,8 +163,24 @@ void ESKFNode::dvl_callback( msg->twist.covariance[8], msg->twist.covariance[12], msg->twist.covariance[13], msg->twist.covariance[14]; + // Apply the rotation correction to the DVL measurement + dvl_sensor.measurement = R_dvl_eskf_ * dvl_sensor.measurement; + dvl_sensor.measurement_noise = R_dvl_eskf_ * dvl_sensor.measurement_noise * R_dvl_eskf_.transpose(); + + // temp debug + // ========================================== + geometry_msgs::msg::TwistWithCovarianceStamped dvl_msg_out = *msg; // Copy header + + // 1. Update Linear Velocity + dvl_msg_out.twist.twist.linear.x = dvl_sensor.measurement.x(); + dvl_msg_out.twist.twist.linear.y = dvl_sensor.measurement.y(); + dvl_msg_out.twist.twist.linear.z = dvl_sensor.measurement.z(); + + dvl_rotated_pub_->publish(dvl_msg_out); + eskf_->dvl_update(dvl_sensor); + #ifndef NDEBUG // Publish NIS in Debug mode std_msgs::msg::Float64 nis_msg; @@ -165,9 +208,10 @@ void ESKFNode::publish_odom() { odom_msg.twist.twist.linear.z = nom_state.vel.z(); // Add bias values to the angular velocity field of twist - odom_msg.twist.twist.angular.x = nom_state.accel_bias.x(); - odom_msg.twist.twist.angular.y = nom_state.accel_bias.y(); - odom_msg.twist.twist.angular.z = nom_state.accel_bias.z(); + Eigen::Vector3d body_angular_vel = latest_gyro_measurement_ - nom_state.gyro_bias; + odom_msg.twist.twist.angular.x = body_angular_vel.x(); + odom_msg.twist.twist.angular.y = body_angular_vel.y(); + odom_msg.twist.twist.angular.z = body_angular_vel.z(); // If you also want to include gyro bias, you could add it to the covariance // matrix or publish a separate topic for biases diff --git a/navigation/eskf/test/eskf_consistency_test.py b/navigation/eskf/test/eskf_consistency_test.py new file mode 100644 index 000000000..5919c5738 --- /dev/null +++ b/navigation/eskf/test/eskf_consistency_test.py @@ -0,0 +1,149 @@ +import rclpy +from rclpy.node import Node +from nav_msgs.msg import Odometry +from std_msgs.msg import Float64 +import message_filters +import numpy as np +from scipy.spatial.transform import Rotation as R + +# --- IMPORT YOUR EXISTING QOS FUNCTION --- +try: + from vortex.utils.qos_profiles import sensor_data_profile +except ImportError: + # Fallback for testing without the vortex library + from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy + def sensor_data_profile(depth=5): + return QoSProfile(reliability=ReliabilityPolicy.BEST_EFFORT, history=HistoryPolicy.KEEP_LAST, depth=depth) + +class EskfValidator(Node): + def __init__(self): + super().__init__('eskf_validator') + + self.est_topic = '/odom_ESKF' + self.gt_topic = '/orca/odom' + + # --- Publishers (Foxglove Visualizers) --- + # RMSE (Running Root Mean Square Error) + self.pub_rmse_pos = self.create_publisher(Float64, 'eskf_metrics/rmse/position', 10) + self.pub_rmse_ori = self.create_publisher(Float64, 'eskf_metrics/rmse/orientation_rad', 10) + self.pub_rmse_vel = self.create_publisher(Float64, 'eskf_metrics/rmse/velocity', 10) + + # NEES (Normalized Estimation Error Squared) + self.pub_nees_pos = self.create_publisher(Float64, 'eskf_metrics/nees/position', 10) + self.pub_nees_ori = self.create_publisher(Float64, 'eskf_metrics/nees/orientation', 10) + + # --- State for Running RMSE --- + self.n_samples = 0 + self.sse_pos = 0.0 # Sum of Squared Errors + self.sse_ori = 0.0 + self.sse_vel = 0.0 + + # --- Subscribers --- + self.get_logger().info(f"Subscribing to {self.est_topic} and {self.gt_topic}...") + qos = sensor_data_profile(depth=5) + + self.sub_est = message_filters.Subscriber(self, Odometry, self.est_topic, qos_profile=qos) + self.sub_gt = message_filters.Subscriber(self, Odometry, self.gt_topic, qos_profile=qos) + + self.ts = message_filters.ApproximateTimeSynchronizer( + [self.sub_est, self.sub_gt], queue_size=10, slop=0.1 + ) + self.ts.registerCallback(self.callback) + + def callback(self, est_msg, gt_msg): + self.n_samples += 1 + + # =========================== + # 1. POSITION (Euclidean) + # =========================== + p_est = np.array([est_msg.pose.pose.position.x, est_msg.pose.pose.position.y, est_msg.pose.pose.position.z]) + p_gt = np.array([gt_msg.pose.pose.position.x, gt_msg.pose.pose.position.y, gt_msg.pose.pose.position.z]) + + err_pos_vec = p_est - p_gt + err_pos_norm = np.linalg.norm(err_pos_vec) + + # Update RMSE + self.sse_pos += err_pos_norm**2 + self.publish_float(self.pub_rmse_pos, np.sqrt(self.sse_pos / self.n_samples)) + + # =========================== + # 2. ORIENTATION (Quaternion) + # =========================== + # Extract Quaternions (x, y, z, w) + q_est = [est_msg.pose.pose.orientation.x, est_msg.pose.pose.orientation.y, est_msg.pose.pose.orientation.z, est_msg.pose.pose.orientation.w] + q_gt = [gt_msg.pose.pose.orientation.x, gt_msg.pose.pose.orientation.y, gt_msg.pose.pose.orientation.z, gt_msg.pose.pose.orientation.w] + + # Convert to Rotation objects + r_est = R.from_quat(q_est) + r_gt = R.from_quat(q_gt) + + # Calculate Error Rotation: R_err = R_gt^T * R_est + # This gives the relative rotation needed to go from GT to Est + r_err = r_gt.inv() * r_est + + # Convert to Rotation Vector (magnitude is the angle error in radians) + err_ori_vec = r_err.as_rotvec() + err_ori_norm = np.linalg.norm(err_ori_vec) + + # Update RMSE + self.sse_ori += err_ori_norm**2 + self.publish_float(self.pub_rmse_ori, np.sqrt(self.sse_ori / self.n_samples)) + + # =========================== + # 3. VELOCITY (Euclidean) + # =========================== + v_est = np.array([est_msg.twist.twist.linear.x, est_msg.twist.twist.linear.y, est_msg.twist.twist.linear.z]) + v_gt = np.array([gt_msg.twist.twist.linear.x, gt_msg.twist.twist.linear.y, gt_msg.twist.twist.linear.z]) + + err_vel_vec = v_est - v_gt + self.sse_vel += np.linalg.norm(err_vel_vec)**2 + self.publish_float(self.pub_rmse_vel, np.sqrt(self.sse_vel / self.n_samples)) + + # Note: Standard Odometry does NOT contain Linear Acceleration. + # If you need Accel RMSE, you must subscribe to the IMU topic separately. + + # =========================== + # 4. NEES CALCULATION + # =========================== + # NEES = error^T * Covariance^-1 * error + + # Reshape the 36-float array into 6x6 matrix + cov_pose = np.array(est_msg.pose.covariance).reshape(6, 6) + + # --- Position NEES (Top-Left 3x3) --- + cov_pos = cov_pose[0:3, 0:3] + try: + cov_pos_inv = np.linalg.inv(cov_pos) + nees_pos = err_pos_vec.T @ cov_pos_inv @ err_pos_vec + self.publish_float(self.pub_nees_pos, nees_pos) + except np.linalg.LinAlgError: + pass # Singular matrix, skip + + # --- Orientation NEES (Bottom-Right 3x3) --- + # Note: We use the rotation vector error (err_ori_vec) calculated earlier + cov_ori = cov_pose[3:6, 3:6] + try: + cov_ori_inv = np.linalg.inv(cov_ori) + nees_ori = err_ori_vec.T @ cov_ori_inv @ err_ori_vec + self.publish_float(self.pub_nees_ori, nees_ori) + except np.linalg.LinAlgError: + pass + + def publish_float(self, publisher, value): + msg = Float64() + msg.data = float(value) + publisher.publish(msg) + +def main(args=None): + rclpy.init(args=args) + node = EskfValidator() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file From e9c9e78bbe9e12608b41eed66c88c6905e8cc654 Mon Sep 17 00:00:00 2001 From: Ahmed Date: Tue, 10 Feb 2026 19:20:04 +0100 Subject: [PATCH 126/290] removing debugging topics, fixing the initial orientation, and the rot matrix for imu --- navigation/eskf/config/eskf_params.yaml | 9 ++--- navigation/eskf/include/eskf/eskf.hpp | 2 +- navigation/eskf/include/eskf/eskf_ros.hpp | 6 --- navigation/eskf/src/eskf.cpp | 14 ++++++- navigation/eskf/src/eskf_ros.cpp | 38 ++----------------- navigation/eskf/test/eskf_consistency_test.py | 22 +++++++++++ 6 files changed, 42 insertions(+), 49 deletions(-) diff --git a/navigation/eskf/config/eskf_params.yaml b/navigation/eskf/config/eskf_params.yaml index a5cb7d161..076e1f65b 100644 --- a/navigation/eskf/config/eskf_params.yaml +++ b/navigation/eskf/config/eskf_params.yaml @@ -8,9 +8,6 @@ eskf_node: imu_frame: [ -1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, -1.0 ] - dvl_frame: [ 0.0, 1.0, 0.0, - -1.0, 0.0, 0.0, - 0.0, 0.0, 1.0 ] - # dvl_frame: [ 1.0, 0.0, 0.0, - # 0.0, 1.0, 0.0, - # 0.0, 0.0, 1.0 ] \ No newline at end of file + dvl_frame: [ 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0 ] \ No newline at end of file diff --git a/navigation/eskf/include/eskf/eskf.hpp b/navigation/eskf/include/eskf/eskf.hpp index 66fe42c0d..cd96ff190 100644 --- a/navigation/eskf/include/eskf/eskf.hpp +++ b/navigation/eskf/include/eskf/eskf.hpp @@ -77,7 +77,7 @@ class ESKF { StateQuat current_nom_state_{}; // gravity - Eigen::Vector3d g_{0.0, 0.0, 9.82}; + Eigen::Vector3d g_{0.0, 0.0, -9.82841}; // accelometer noise parameters float accm_std_{0.0}; diff --git a/navigation/eskf/include/eskf/eskf_ros.hpp b/navigation/eskf/include/eskf/eskf_ros.hpp index 2407a685d..2e2630a10 100644 --- a/navigation/eskf/include/eskf/eskf_ros.hpp +++ b/navigation/eskf/include/eskf/eskf_ros.hpp @@ -49,12 +49,6 @@ class ESKFNode : public rclcpp::Node { rclcpp::Publisher::SharedPtr nis_pub_; - // temp debug - rclcpp::Publisher::SharedPtr imu_rotated_pub_; - rclcpp::Publisher::SharedPtr dvl_rotated_pub_; - - // end temp debug - std::chrono::milliseconds time_step; rclcpp::TimerBase::SharedPtr odom_pub_timer_; diff --git a/navigation/eskf/src/eskf.cpp b/navigation/eskf/src/eskf.cpp index 1073a50cf..7b5016771 100644 --- a/navigation/eskf/src/eskf.cpp +++ b/navigation/eskf/src/eskf.cpp @@ -11,7 +11,19 @@ double compute_nis(const Eigen::Vector3d& innovation, return innovation.transpose() * S_inv * innovation; } -ESKF::ESKF(const EskfParams& params) : Q_(params.Q) {} +ESKF::ESKF(const EskfParams& params) : Q_(params.Q) { + // Initialize Covariance + current_error_state_.covariance = params.P; + + // // Initialize Nominal Quaternion to Identity + // current_nom_state_.quat = Eigen::Quaterniond::Identity(); + + // 2. Initialize Quaternion: -90 degrees Yaw because of initial drone_orientation + Eigen::AngleAxisd init_rotation(-M_PI / 2.0, Eigen::Vector3d::UnitZ()); + + current_nom_state_.quat = Eigen::Quaterniond(init_rotation); + current_nom_state_.quat.normalize(); +} std::pair ESKF::van_loan_discretization( const Eigen::Matrix15d& A_c, diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp index c885fa857..289625ff3 100644 --- a/navigation/eskf/src/eskf_ros.cpp +++ b/navigation/eskf/src/eskf_ros.cpp @@ -51,13 +51,6 @@ void ESKFNode::set_subscribers_and_publisher() { odom_pub_ = this->create_publisher( odom_topic, qos_sensor_data); - // temp debug - imu_rotated_pub_ = this->create_publisher( - "eskf/imu_rotated", vortex::utils::qos_profiles::sensor_data_profile()); - - dvl_rotated_pub_ = this->create_publisher( - "eskf/dvl_rotated", vortex::utils::qos_profiles::sensor_data_profile()); - #ifndef NDEBUG nis_pub_ = create_publisher( "eskf/nis", vortex::utils::qos_profiles::reliable_profile()); @@ -127,27 +120,13 @@ void ESKFNode::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { Eigen::Vector3d raw_gyro(msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z); - imu_measurement.gyro = R_imu_eskf_ * raw_gyro; - // imu_measurement.gyro = raw_gyro; + // currently the gyro and the accelorometer are rotated differently. should be changed with the actual drone. + // imu_measurement.gyro = R_imu_eskf_ * raw_gyro; + imu_measurement.gyro = raw_gyro; // used for publishing odom output of eskf latest_gyro_measurement_ = imu_measurement.gyro; - // temp debug - // ========================================== - sensor_msgs::msg::Imu imu_msg_out = *msg; // Copy header and covariances - - // Overwrite vectors with corrected data - imu_msg_out.linear_acceleration.x = imu_measurement.accel.x(); - imu_msg_out.linear_acceleration.y = imu_measurement.accel.y(); - imu_msg_out.linear_acceleration.z = imu_measurement.accel.z(); - - imu_msg_out.angular_velocity.x = imu_measurement.gyro.x(); - imu_msg_out.angular_velocity.y = imu_measurement.gyro.y(); - imu_msg_out.angular_velocity.z = imu_measurement.gyro.z(); - imu_rotated_pub_->publish(imu_msg_out); - // - eskf_->imu_update(imu_measurement, dt); } @@ -167,17 +146,6 @@ void ESKFNode::dvl_callback( dvl_sensor.measurement = R_dvl_eskf_ * dvl_sensor.measurement; dvl_sensor.measurement_noise = R_dvl_eskf_ * dvl_sensor.measurement_noise * R_dvl_eskf_.transpose(); - // temp debug - // ========================================== - geometry_msgs::msg::TwistWithCovarianceStamped dvl_msg_out = *msg; // Copy header - - // 1. Update Linear Velocity - dvl_msg_out.twist.twist.linear.x = dvl_sensor.measurement.x(); - dvl_msg_out.twist.twist.linear.y = dvl_sensor.measurement.y(); - dvl_msg_out.twist.twist.linear.z = dvl_sensor.measurement.z(); - - dvl_rotated_pub_->publish(dvl_msg_out); - eskf_->dvl_update(dvl_sensor); diff --git a/navigation/eskf/test/eskf_consistency_test.py b/navigation/eskf/test/eskf_consistency_test.py index 5919c5738..a7d1495b0 100644 --- a/navigation/eskf/test/eskf_consistency_test.py +++ b/navigation/eskf/test/eskf_consistency_test.py @@ -2,6 +2,7 @@ from rclpy.node import Node from nav_msgs.msg import Odometry from std_msgs.msg import Float64 +from std_msgs.msg import Float64MultiArray import message_filters import numpy as np from scipy.spatial.transform import Rotation as R @@ -28,6 +29,10 @@ def __init__(self): self.pub_rmse_ori = self.create_publisher(Float64, 'eskf_metrics/rmse/orientation_rad', 10) self.pub_rmse_vel = self.create_publisher(Float64, 'eskf_metrics/rmse/velocity', 10) + # Euler Angles (for visual comparison) + self.pub_euler_est = self.create_publisher(Float64MultiArray, 'debug/euler/est', 10) + self.pub_euler_gt = self.create_publisher(Float64MultiArray, 'debug/euler/gt', 10) + # NEES (Normalized Estimation Error Squared) self.pub_nees_pos = self.create_publisher(Float64, 'eskf_metrics/nees/position', 10) self.pub_nees_ori = self.create_publisher(Float64, 'eskf_metrics/nees/orientation', 10) @@ -77,6 +82,23 @@ def callback(self, est_msg, gt_msg): r_est = R.from_quat(q_est) r_gt = R.from_quat(q_gt) + # --------------------------------------------------------- + # Extract Euler Angles (Roll, Pitch, Yaw) + # Using 'xyz' sequence (standard for ROS aerospace/underwater) + # degrees=True makes it easier to read in plots (0-360 or +/-180) + # --------------------------------------------------------- + euler_est = r_est.as_euler('xyz', degrees=True) # [roll, pitch, yaw] + euler_gt = r_gt.as_euler('xyz', degrees=True) # [roll, pitch, yaw] + + # Publish Estimated Euler + msg_euler_est = Float64MultiArray() + msg_euler_est.data = euler_est.tolist() + self.pub_euler_est.publish(msg_euler_est) + + # Publish GT Euler + msg_euler_gt = Float64MultiArray() + msg_euler_gt.data = euler_gt.tolist() + self.pub_euler_gt.publish(msg_euler_gt) # Calculate Error Rotation: R_err = R_gt^T * R_est # This gives the relative rotation needed to go from GT to Est r_err = r_gt.inv() * r_est From 99a506b41d459c93e1026191c337fcab8e323584 Mon Sep 17 00:00:00 2001 From: Ahmed Date: Wed, 18 Feb 2026 17:56:30 +0100 Subject: [PATCH 127/290] fixes for review findings --- navigation/eskf/include/eskf/eskf.tpp | 6 +++++- navigation/eskf/src/eskf.cpp | 18 +++++------------- 2 files changed, 10 insertions(+), 14 deletions(-) diff --git a/navigation/eskf/include/eskf/eskf.tpp b/navigation/eskf/include/eskf/eskf.tpp index c02d7d4f7..eeb579f6c 100644 --- a/navigation/eskf/include/eskf/eskf.tpp +++ b/navigation/eskf/include/eskf/eskf.tpp @@ -9,7 +9,11 @@ void ESKF::measurement_update(const SensorT& meas) { Eigen::MatrixXd P = current_error_state_.covariance; Eigen::MatrixXd S = H * P * H.transpose() + R; - Eigen::MatrixXd K = P * H.transpose() * S.inverse(); + + Eigen::MatrixXd PHt = P * H.transpose(); + + // More stable and faster than P * H.transpose() * S.inverse() + Eigen::MatrixXd K = S.ldlt().solve(PHt.transpose()).transpose(); #ifndef NDEBUG nis_ = compute_nis(innovation, S); diff --git a/navigation/eskf/src/eskf.cpp b/navigation/eskf/src/eskf.cpp index 7b5016771..fc3660522 100644 --- a/navigation/eskf/src/eskf.cpp +++ b/navigation/eskf/src/eskf.cpp @@ -15,14 +15,8 @@ ESKF::ESKF(const EskfParams& params) : Q_(params.Q) { // Initialize Covariance current_error_state_.covariance = params.P; - // // Initialize Nominal Quaternion to Identity - // current_nom_state_.quat = Eigen::Quaterniond::Identity(); - - // 2. Initialize Quaternion: -90 degrees Yaw because of initial drone_orientation - Eigen::AngleAxisd init_rotation(-M_PI / 2.0, Eigen::Vector3d::UnitZ()); - - current_nom_state_.quat = Eigen::Quaterniond(init_rotation); - current_nom_state_.quat.normalize(); + // Initialize Nominal Quaternion to Identity + current_nom_state_.quat = Eigen::Quaterniond::Identity(); } std::pair ESKF::van_loan_discretization( @@ -153,6 +147,7 @@ void ESKF::error_state_prediction(const ImuMeasurement& imu_meas, } void ESKF::injection_and_reset() { + // injection current_nom_state_.pos = current_nom_state_.pos + current_error_state_.pos; current_nom_state_.vel = current_nom_state_.vel + current_error_state_.vel; current_nom_state_.quat = current_nom_state_.quat * @@ -163,11 +158,8 @@ void ESKF::injection_and_reset() { current_nom_state_.gyro_bias + current_error_state_.gyro_bias; current_nom_state_.accel_bias = current_nom_state_.accel_bias + current_error_state_.accel_bias; - - Eigen::Matrix15d G = Eigen::Matrix15d::Identity(); - - current_error_state_.covariance = - G * current_error_state_.covariance * G.transpose(); + + // reset current_error_state_.set_from_vector(Eigen::Vector15d::Zero()); } From 0e6621d30b2deb1ca978956031e18afd46e4127d Mon Sep 17 00:00:00 2001 From: Ahmed Date: Wed, 18 Feb 2026 19:10:57 +0100 Subject: [PATCH 128/290] fixes for pre-commit --- .pre-commit-config.yaml | 2 ++ navigation/eskf/config/eskf_params.yaml | 2 +- navigation/eskf/include/eskf/eskf.tpp | 2 +- navigation/eskf/src/eskf.cpp | 6 +++--- navigation/eskf/src/eskf_ros.cpp | 15 ++++++++------- navigation/eskf/test/eskf_consistency_test.py | 6 +++--- 6 files changed, 18 insertions(+), 15 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 6609d6cbc..23e3bcd0f 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -12,6 +12,8 @@ # # See https://github.com/pre-commit/pre-commit +exclude: (^|/)(test|tests)/.* + repos: # Standard hooks - repo: https://github.com/pre-commit/pre-commit-hooks diff --git a/navigation/eskf/config/eskf_params.yaml b/navigation/eskf/config/eskf_params.yaml index 076e1f65b..c2abd772e 100644 --- a/navigation/eskf/config/eskf_params.yaml +++ b/navigation/eskf/config/eskf_params.yaml @@ -10,4 +10,4 @@ eskf_node: 0.0, 0.0, -1.0 ] dvl_frame: [ 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0 ] \ No newline at end of file + 0.0, 0.0, 1.0 ] diff --git a/navigation/eskf/include/eskf/eskf.tpp b/navigation/eskf/include/eskf/eskf.tpp index eeb579f6c..6189a609a 100644 --- a/navigation/eskf/include/eskf/eskf.tpp +++ b/navigation/eskf/include/eskf/eskf.tpp @@ -13,7 +13,7 @@ void ESKF::measurement_update(const SensorT& meas) { Eigen::MatrixXd PHt = P * H.transpose(); // More stable and faster than P * H.transpose() * S.inverse() - Eigen::MatrixXd K = S.ldlt().solve(PHt.transpose()).transpose(); + Eigen::MatrixXd K = S.ldlt().solve(PHt.transpose()).transpose(); #ifndef NDEBUG nis_ = compute_nis(innovation, S); diff --git a/navigation/eskf/src/eskf.cpp b/navigation/eskf/src/eskf.cpp index fc3660522..d904a8bb5 100644 --- a/navigation/eskf/src/eskf.cpp +++ b/navigation/eskf/src/eskf.cpp @@ -12,10 +12,10 @@ double compute_nis(const Eigen::Vector3d& innovation, } ESKF::ESKF(const EskfParams& params) : Q_(params.Q) { - // Initialize Covariance + // Initialize Covariance current_error_state_.covariance = params.P; - // Initialize Nominal Quaternion to Identity + // Initialize Nominal Quaternion to Identity current_nom_state_.quat = Eigen::Quaterniond::Identity(); } @@ -158,7 +158,7 @@ void ESKF::injection_and_reset() { current_nom_state_.gyro_bias + current_error_state_.gyro_bias; current_nom_state_.accel_bias = current_nom_state_.accel_bias + current_error_state_.accel_bias; - + // reset current_error_state_.set_from_vector(Eigen::Vector15d::Zero()); } diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp index 289625ff3..c31823e3a 100644 --- a/navigation/eskf/src/eskf_ros.cpp +++ b/navigation/eskf/src/eskf_ros.cpp @@ -58,13 +58,12 @@ void ESKFNode::set_subscribers_and_publisher() { } void ESKFNode::set_parameters() { - std::vector R_imu_correction; this->declare_parameter>("imu_frame"); R_imu_correction = get_parameter("imu_frame").as_double_array(); R_imu_eskf_ = Eigen::Map>( R_imu_correction.data()); - + std::vector R_dvl_correction; this->declare_parameter>("dvl_frame"); R_dvl_correction = get_parameter("dvl_frame").as_double_array(); @@ -120,7 +119,8 @@ void ESKFNode::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { Eigen::Vector3d raw_gyro(msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z); - // currently the gyro and the accelorometer are rotated differently. should be changed with the actual drone. + // currently the gyro and the accelorometer are rotated differently. + // should be changed with the actual drone params. // imu_measurement.gyro = R_imu_eskf_ * raw_gyro; imu_measurement.gyro = raw_gyro; @@ -142,13 +142,13 @@ void ESKFNode::dvl_callback( msg->twist.covariance[8], msg->twist.covariance[12], msg->twist.covariance[13], msg->twist.covariance[14]; - // Apply the rotation correction to the DVL measurement + // Apply the rotation correction to the DVL measurement dvl_sensor.measurement = R_dvl_eskf_ * dvl_sensor.measurement; - dvl_sensor.measurement_noise = R_dvl_eskf_ * dvl_sensor.measurement_noise * R_dvl_eskf_.transpose(); + dvl_sensor.measurement_noise = + R_dvl_eskf_ * dvl_sensor.measurement_noise * R_dvl_eskf_.transpose(); eskf_->dvl_update(dvl_sensor); - #ifndef NDEBUG // Publish NIS in Debug mode std_msgs::msg::Float64 nis_msg; @@ -176,7 +176,8 @@ void ESKFNode::publish_odom() { odom_msg.twist.twist.linear.z = nom_state.vel.z(); // Add bias values to the angular velocity field of twist - Eigen::Vector3d body_angular_vel = latest_gyro_measurement_ - nom_state.gyro_bias; + Eigen::Vector3d body_angular_vel = + latest_gyro_measurement_ - nom_state.gyro_bias; odom_msg.twist.twist.angular.x = body_angular_vel.x(); odom_msg.twist.twist.angular.y = body_angular_vel.y(); odom_msg.twist.twist.angular.z = body_angular_vel.z(); diff --git a/navigation/eskf/test/eskf_consistency_test.py b/navigation/eskf/test/eskf_consistency_test.py index a7d1495b0..0f0c8061f 100644 --- a/navigation/eskf/test/eskf_consistency_test.py +++ b/navigation/eskf/test/eskf_consistency_test.py @@ -5,7 +5,7 @@ from std_msgs.msg import Float64MultiArray import message_filters import numpy as np -from scipy.spatial.transform import Rotation as R +from scipy.spatial.transform import Rotation # --- IMPORT YOUR EXISTING QOS FUNCTION --- try: @@ -79,8 +79,8 @@ def callback(self, est_msg, gt_msg): q_gt = [gt_msg.pose.pose.orientation.x, gt_msg.pose.pose.orientation.y, gt_msg.pose.pose.orientation.z, gt_msg.pose.pose.orientation.w] # Convert to Rotation objects - r_est = R.from_quat(q_est) - r_gt = R.from_quat(q_gt) + r_est = Rotation.from_quat(q_est) + r_gt = Rotation.from_quat(q_gt) # --------------------------------------------------------- # Extract Euler Angles (Roll, Pitch, Yaw) From 2d7141163b142c3b2191298f70e2fde0c27c4140 Mon Sep 17 00:00:00 2001 From: ppakr Date: Sat, 21 Feb 2026 17:13:16 +0100 Subject: [PATCH 129/290] fix: add vortex_utils_ros to target dependencies --- control/pid_controller_dp/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/control/pid_controller_dp/CMakeLists.txt b/control/pid_controller_dp/CMakeLists.txt index 82b066266..80f9e132e 100644 --- a/control/pid_controller_dp/CMakeLists.txt +++ b/control/pid_controller_dp/CMakeLists.txt @@ -40,6 +40,7 @@ ament_target_dependencies(${LIB_NAME} PUBLIC vortex_msgs rcl_interfaces vortex_utils + vortex_utils_ros spdlog fmt ) @@ -64,6 +65,7 @@ ament_target_dependencies(pid_controller_node vortex_msgs rcl_interfaces vortex_utils + vortex_utils_ros spdlog fmt ) @@ -82,8 +84,6 @@ install(TARGETS ${LIB_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin - vortex_utils - vortex_utils_ros ) install(TARGETS From afab550b07205e387922f512ad2bd619fb333169 Mon Sep 17 00:00:00 2001 From: ppakr Date: Sat, 21 Feb 2026 17:27:39 +0100 Subject: [PATCH 130/290] refactor: remove debug parameters and logging from PID controller implementation --- .../pid_controller_dp/pid_controller.hpp | 15 ------ .../pid_controller_dp/pid_controller_ros.hpp | 3 +- .../pid_controller_utils.hpp | 2 - .../pid_controller_dp/src/pid_controller.cpp | 25 +-------- .../src/pid_controller_node.cpp | 1 - .../src/pid_controller_ros.cpp | 54 ------------------- .../src/pid_controller_utils.cpp | 46 ---------------- 7 files changed, 2 insertions(+), 144 deletions(-) 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 bd3c310de..52a855262 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 @@ -41,21 +41,6 @@ class PIDController { types::Matrix6d get_ki(); types::Matrix6d get_kd(); - // parameters for debug - types::Eta eta_error_debug; - types::Vector6d nu_d_debug; - types::Vector6d error_nu_debug; - types::Vector6d P_debug; - types::Vector6d I_debug; - types::Vector6d D_debug; - types::Vector6d tau_debug; - types::Matrix6x7d J_inv_debug; - - // debug gain - types::Matrix6d Kp_debug; - types::Matrix6d Ki_debug; - types::Matrix6d Kd_debug; - private: types::Matrix6d Kp_; types::Matrix6d Ki_; 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 a30b7d096..0f1cbdbf3 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 @@ -65,8 +65,7 @@ class PIDControllerNode : public rclcpp::Node { void guidance_callback( const vortex_msgs::msg::ReferenceFilter::SharedPtr msg); - // TODO: parameter callback for dynamic reconfigure of PID gains - //@brief Callback function for parameter updates + // @brief Callback function for parameter updates // @param parameters: vector of parameters to be set rcl_interfaces::msg::SetParametersResult parametersCallback( const std::vector& parameters); 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 a199033f4..dcd79b400 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 @@ -74,6 +74,4 @@ types::Vector7d anti_windup(const double dt, const types::Eta& error, const types::Vector7d& integral); -// void print_J_transformation(const types::J_transformation& J); -// void print_Jinv_transformation(const types::Matrix6x7d& J_inv); #endif // PID_CONTROLLER_DP__PID_CONTROLLER_UTILS_HPP_ diff --git a/control/pid_controller_dp/src/pid_controller.cpp b/control/pid_controller_dp/src/pid_controller.cpp index 60edc2014..8867b160f 100644 --- a/control/pid_controller_dp/src/pid_controller.cpp +++ b/control/pid_controller_dp/src/pid_controller.cpp @@ -86,45 +86,22 @@ types::Vector6d PIDController::calculate_tau(const types::Eta& eta, auto eta_dot_d_copy = eta_dot_d; eta_dot_d_copy.qw = 0.0; // set w = 0 for desired eta_dot - // debug - // eta_error_debug = error; - spdlog::info("Eta: "); - print_eta(eta); - spdlog::info("Eta desired: "); - print_eta(eta_d); - spdlog::info("Eta error:"); - print_eta(error); types::Matrix6x7d J_inv = calculate_J_sudo_inv(eta); // calculate J pseudo inverse - J_inv_debug = J_inv; - print_Jinv_transformation(J_inv); types::Vector6d nu_d = J_inv * eta_dot_d_copy.to_vector(); // calculate velocity - // nu_d_debug = nu_d; - // print_nu(nu_d); types::Vector6d error_nu = nu.to_vector() - nu_d; // calculate vel error - // error_nu_debug = error_nu; - // print_vect_6d(error_nu); types::Vector6d P = Kp_ * J_inv * error.to_vector(); // P term - // P_debug = P; - Kp_debug = Kp_; types::Vector6d I = Ki_ * J_inv * integral_; // I term - // I_debug = I; - // Ki_debug = Ki_; types::Vector6d D = Kd_ * error_nu; // D term - // D_debug = D; - // Kd_debug = Kd_; - types::Vector6d tau = -clamp_values((P + I + D), -80.0, 80.0); - // types::Vector6d tau = -clamp_values((P), -80.0, 80.0); - // debug: tau = 0 - // types::Vector6d tau = types::Vector6d::Zero(); + types::Vector6d tau = -clamp_values((P + I + D), -80.0, 80.0); integral_ = anti_windup(dt_, error, integral_); diff --git a/control/pid_controller_dp/src/pid_controller_node.cpp b/control/pid_controller_dp/src/pid_controller_node.cpp index bee6500fc..46d51b1ee 100644 --- a/control/pid_controller_dp/src/pid_controller_node.cpp +++ b/control/pid_controller_dp/src/pid_controller_node.cpp @@ -11,7 +11,6 @@ auto start_msg = R"( int main(int argc, char** argv) { rclcpp::init(argc, argv); - // RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Started PID Controller Node"); spdlog::info(start_msg); rclcpp::spin(std::make_shared()); rclcpp::shutdown(); diff --git a/control/pid_controller_dp/src/pid_controller_ros.cpp b/control/pid_controller_dp/src/pid_controller_ros.cpp index faf2a10b5..24b438c92 100644 --- a/control/pid_controller_dp/src/pid_controller_ros.cpp +++ b/control/pid_controller_dp/src/pid_controller_ros.cpp @@ -143,48 +143,6 @@ void PIDControllerNode::publish_tau() { types::Vector6d tau = pid_controller_.calculate_tau(eta_, eta_d_, nu_, eta_dot_d_); - // print for debug - RCLCPP_INFO_STREAM(this->get_logger(), "Tau: [" << tau(0) << ", " << tau(1) - << ", " << tau(2) << ", " - << tau(3) << ", " << tau(4) - << ", " << tau(5) << "]"); - RCLCPP_INFO_STREAM(this->get_logger(), - "Kp: [" << pid_controller_.Kp_debug(0, 0) << ", " - << pid_controller_.Kp_debug(0, 1) << ", " - << pid_controller_.Kp_debug(0, 2) << ", " - << pid_controller_.Kp_debug(0, 3) << ", " - << pid_controller_.Kp_debug(0, 4) << ", " - << pid_controller_.Kp_debug(0, 5) << "; " - << pid_controller_.Kp_debug(1, 0) << ", " - << pid_controller_.Kp_debug(1, 1) << ", " - << pid_controller_.Kp_debug(1, 2) << ", " - << pid_controller_.Kp_debug(1, 3) << ", " - << pid_controller_.Kp_debug(1, 4) << ", " - << pid_controller_.Kp_debug(1, 5) << "; " - << pid_controller_.Kp_debug(2, 0) << ", " - << pid_controller_.Kp_debug(2, 1) << ", " - << pid_controller_.Kp_debug(2, 2) << ", " - << pid_controller_.Kp_debug(2, 3) << ", " - << pid_controller_.Kp_debug(2, 4) << ", " - << pid_controller_.Kp_debug(2, 5) << "; " - << pid_controller_.Kp_debug(3, 0) << ", " - << pid_controller_.Kp_debug(3, 1) << ", " - << pid_controller_.Kp_debug(3, 2) << ", " - << pid_controller_.Kp_debug(3, 3) << ", " - << pid_controller_.Kp_debug(3, 4) << ", " - << pid_controller_.Kp_debug(3, 5) << "; " - << pid_controller_.Kp_debug(4, 0) << ", " - << pid_controller_.Kp_debug(4, 1) << ", " - << pid_controller_.Kp_debug(4, 2) << ", " - << pid_controller_.Kp_debug(4, 3) << ", " - << pid_controller_.Kp_debug(4, 4) << ", " - << pid_controller_.Kp_debug(4, 5) << "; " - << pid_controller_.Kp_debug(5, 0) << ", " - << pid_controller_.Kp_debug(5, 1) << ", " - << pid_controller_.Kp_debug(5, 2) << ", " - << pid_controller_.Kp_debug(5, 3) << ", " - << pid_controller_.Kp_debug(5, 4) << ", " - << pid_controller_.Kp_debug(5, 5) << "]"); geometry_msgs::msg::WrenchStamped tau_msg; tau_msg.header.stamp = this->now(); @@ -219,17 +177,6 @@ void PIDControllerNode::set_pid_params() { this->declare_parameter("Kd_pitch", 0.1); this->declare_parameter("Kd_yaw", 0.1); - // this->declare_parameter>( - // "Kp", {1.0, 1.0, 1.0, 1.0, 1.0, 1.0}); - // this->declare_parameter>( - // "Ki", {0.1, 0.1, 0.1, 0.1, 0.1, 0.1}); - // this->declare_parameter>( - // "Kd", {0.1, 0.1, 0.1, 0.1, 0.1, 0.1}); - - // std::vector Kp_vec = this->get_parameter("Kp").as_double_array(); - // std::vector Ki_vec = this->get_parameter("Ki").as_double_array(); - // std::vector Kd_vec = this->get_parameter("Kd").as_double_array(); - std::vector Kp_vec = { this->get_parameter("Kp_x").as_double(), this->get_parameter("Kp_y").as_double(), @@ -291,7 +238,6 @@ void PIDControllerNode::guidance_callback( eta_d_.qz = quat.z(); } -// TODO: set parameter functions rcl_interfaces::msg::SetParametersResult PIDControllerNode::parametersCallback( const std::vector& parameters) { rcl_interfaces::msg::SetParametersResult result; diff --git a/control/pid_controller_dp/src/pid_controller_utils.cpp b/control/pid_controller_dp/src/pid_controller_utils.cpp index d85c9c4fa..834322e11 100644 --- a/control/pid_controller_dp/src/pid_controller_utils.cpp +++ b/control/pid_controller_dp/src/pid_controller_utils.cpp @@ -5,41 +5,6 @@ #include "pid_controller_dp/pid_controller_conversions.hpp" #include "pid_controller_dp/typedefs.hpp" -// void print_J_transformation(const types::J_transformation& J) { -// spdlog::info("J_transformation:"); - -// spdlog::info("R (3x3) elements:"); -// for (int i = 0; i < J.R.rows(); ++i) { -// for (int j = 0; j < J.R.cols(); ++j) { -// spdlog::info("R[{},{}] = {}", i, j, J.R(i, j)); -// } -// } - -// spdlog::info("T (4x3) elements:"); -// for (int i = 0; i < J.T.rows(); ++i) { -// for (int j = 0; j < J.T.cols(); ++j) { -// spdlog::info("T[{},{}] = {}", i, j, J.T(i, j)); -// } -// } - -// spdlog::info("Combined Matrix (7x6) elements:"); -// auto M = J.as_matrix(); -// for (int i = 0; i < M.rows(); ++i) { -// for (int j = 0; j < M.cols(); ++j) { -// spdlog::info("M[{},{}] = {}", i, j, M(i, j)); -// } -// } -// } - -// void print_Jinv_transformation(const types::Matrix6x7d& J_inv) { -// spdlog::info("J (6x7) elements:"); -// for (int i = 0; i < J_inv.rows(); ++i) { -// for (int j = 0; j < J_inv.cols(); ++j) { -// spdlog::info("J_inv[{},{}] = {}", i, j, J_inv(i, j)); -// } -// } -// } - types::Matrix3d calculate_R_quat(const types::Eta& eta) { return eta.as_rotation_matrix(); } @@ -65,23 +30,12 @@ types::Eta error_eta(const types::Eta& eta, const types::Eta& eta_d) { Eigen::VectorXd clamp_values(const Eigen::VectorXd& values, double min_val, double max_val) { - // return values.cwiseMax(min_val).cwiseMin(max_val); return vortex::utils::math::clamp_values(values, min_val, max_val); } types::Vector7d anti_windup(const double dt, const types::Eta& error, const types::Vector7d& integral) { - // Eigen::VectorXd integral_eig = integral; - // Eigen::VectorXd updated = vortex::utils::math::anti_windup( - // dt, error.to_vector(), integral_eig, -80.0, 80.0); - - // types::Vector7d integral_anti_windup; - // for (int i = 0; i < 7; ++i) { - // integral_anti_windup(i) = updated(i); - // } - - // return integral_anti_windup; return vortex::utils::math::anti_windup(dt, error.to_vector(), integral, -80.0, 80.0); } From f1df4f7a9d8531a6988e6973e4bdfa0ab27e1990 Mon Sep 17 00:00:00 2001 From: ppakr Date: Sat, 21 Feb 2026 18:11:00 +0100 Subject: [PATCH 131/290] feat: transform joystick inputs from body frame to world frame in movement calculations --- .../joystick_interface_auv_node.py | 19 ++++++++++++++++--- 1 file changed, 16 insertions(+), 3 deletions(-) diff --git a/mission/joystick_interface_auv/joystick_interface_auv/joystick_interface_auv_node.py b/mission/joystick_interface_auv/joystick_interface_auv/joystick_interface_auv_node.py index df664f197..c43f1f31a 100755 --- a/mission/joystick_interface_auv/joystick_interface_auv/joystick_interface_auv_node.py +++ b/mission/joystick_interface_auv/joystick_interface_auv/joystick_interface_auv_node.py @@ -1,5 +1,6 @@ #!/usr/bin/env python3 +import numpy as np import rclpy from geometry_msgs.msg import PoseWithCovarianceStamped, WrenchStamped from rclpy.node import Node, Parameter @@ -376,10 +377,22 @@ def update_reference(self): The position and orientation (roll, pitch, yaw) are updated using the current joystick inputs scaled by their respective parameters. + The linear velocities (surge, sway, heave) are transformed from the + body frame to the world frame using the current orientation. """ - self._desired_state.x += self.surge * self._guidance_surge_gain - self._desired_state.y += self.sway * self._guidance_sway_gain - self._desired_state.z -= self.heave * self._guidance_heave_gain + surge_vector = self.surge * self._guidance_surge_gain + sway_vector = self.sway * self._guidance_sway_gain + heave_vector = -self.heave * self._guidance_heave_gain + + body_frame_vector = np.array([surge_vector, sway_vector, heave_vector]) + + rotation_matrix = self._desired_state.as_rotation_matrix() + world_frame_vector = rotation_matrix @ body_frame_vector + + self._desired_state.x += world_frame_vector[0] + self._desired_state.y += world_frame_vector[1] + self._desired_state.z += world_frame_vector[2] + self._desired_state.roll += self.roll * self._guidance_roll_gain self._desired_state.pitch += self.pitch * self._guidance_pitch_gain self._desired_state.yaw += self.yaw * self._guidance_yaw_gain From 386fd9921f2b3aa134f719b3541ecf8faefa3421 Mon Sep 17 00:00:00 2001 From: ppakr Date: Sat, 21 Feb 2026 19:08:25 +0100 Subject: [PATCH 132/290] fix: update PID parameters and joystick interface --- .../pid_controller_dp/config/pid_params.yaml | 26 +++++++++---------- .../joystick_interface_auv_node.py | 5 ++-- 2 files changed, 16 insertions(+), 15 deletions(-) diff --git a/control/pid_controller_dp/config/pid_params.yaml b/control/pid_controller_dp/config/pid_params.yaml index c6efcb8bd..811e8c63d 100644 --- a/control/pid_controller_dp/config/pid_params.yaml +++ b/control/pid_controller_dp/config/pid_params.yaml @@ -4,20 +4,20 @@ # Ki: [2.0, 2.0, 2.0, 0.12, 0.12, 0.12] # Kd: [10.0, 10.0, 10.0, 4.0, 5.0, 4.0] Kp_x: 20.0 - Kp_y: 0.0 + Kp_y: 26.0 Kp_z: 50.0 - Kp_roll: 0.0 - Kp_pitch: 0.0 - Kp_yaw: 0.0 - Ki_x: 0.08 - Ki_y: 0.0 - Ki_z: 0.0 - Ki_roll: 0.0 - Ki_pitch: 0.0 - Ki_yaw: 0.0 - Kd_x: 0.0 + Kp_roll: 10.0 + Kp_pitch: 41.0 + Kp_yaw: 6.0 + Ki_x: 0.092 + Ki_y: 0.00059 + Ki_z: 0.085 + Ki_roll: 0.002 + Ki_pitch: 0.01 + Ki_yaw: 0.0003 + Kd_x: 0.001 Kd_y: 0.0 Kd_z: 0.0 - Kd_roll: 0.0 + Kd_roll: 0.002 Kd_pitch: 0.0 - Kd_yaw: 0.0 + Kd_yaw: 0.001 diff --git a/mission/joystick_interface_auv/joystick_interface_auv/joystick_interface_auv_node.py b/mission/joystick_interface_auv/joystick_interface_auv/joystick_interface_auv_node.py index c43f1f31a..b690c37d4 100755 --- a/mission/joystick_interface_auv/joystick_interface_auv/joystick_interface_auv_node.py +++ b/mission/joystick_interface_auv/joystick_interface_auv/joystick_interface_auv_node.py @@ -9,11 +9,11 @@ from vortex_msgs.msg import OperationMode, ReferenceFilter from vortex_msgs.srv import GetOperationMode, SetOperationMode, ToggleKillswitch from vortex_utils.python_utils import PoseData +from vortex_utils_ros.ros_converter import pose_from_ros from vortex_utils_ros.qos_profiles import ( reliable_profile, sensor_data_profile, ) -from vortex_utils_ros.ros_converter import pose_from_ros from joystick_interface_auv.joystick_utils import ( Wired, @@ -206,7 +206,8 @@ def create_reference_message(self) -> ReferenceFilter: """Creates a reference message with the desired state values.""" reference_msg = ReferenceFilter() reference_msg.header.stamp = self.get_clock().now().to_msg() - reference_msg.header.frame_id = "odom" + # reference_msg.header.frame_id = "odom" + reference_msg.header.frame_id = "base_link" reference_msg.x = self._desired_state.x reference_msg.y = self._desired_state.y reference_msg.z = self._desired_state.z From 73b460c25e3ab6f5493a37c3003be0b555b117d0 Mon Sep 17 00:00:00 2001 From: Anbit Date: Sun, 22 Feb 2026 18:32:38 +0100 Subject: [PATCH 133/290] 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 134/290] 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 135/290] 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 136/290] 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 574b64ccbef9b5fef986781c19a86252c0b46740 Mon Sep 17 00:00:00 2001 From: Ahmed Date: Wed, 18 Feb 2026 19:10:57 +0100 Subject: [PATCH 137/290] fixes for pre-commit --- .pre-commit-config.yaml | 2 + navigation/eskf/CMakeLists.txt | 4 + navigation/eskf/config/eskf_params.yaml | 12 +- navigation/eskf/include/eskf/eskf.tpp | 2 +- navigation/eskf/include/eskf/eskf_ros.hpp | 34 +++++ navigation/eskf/package.xml | 2 + navigation/eskf/src/eskf.cpp | 6 +- navigation/eskf/src/eskf_ros.cpp | 140 ++++++++++++++++-- navigation/eskf/test/eskf_consistency_test.py | 6 +- 9 files changed, 187 insertions(+), 21 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 6609d6cbc..23e3bcd0f 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -12,6 +12,8 @@ # # See https://github.com/pre-commit/pre-commit +exclude: (^|/)(test|tests)/.* + repos: # Standard hooks - repo: https://github.com/pre-commit/pre-commit-hooks diff --git a/navigation/eskf/CMakeLists.txt b/navigation/eskf/CMakeLists.txt index 0028693bf..b292b256d 100644 --- a/navigation/eskf/CMakeLists.txt +++ b/navigation/eskf/CMakeLists.txt @@ -25,6 +25,8 @@ find_package(vortex_utils REQUIRED) find_package(vortex_utils_ros REQUIRED) find_package(spdlog REQUIRED) find_package(fmt REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_eigen REQUIRED) if(NOT DEFINED EIGEN3_INCLUDE_DIR) set(EIGEN3_INCLUDE_DIR ${EIGEN3_INCLUDE_DIRS}) @@ -53,6 +55,8 @@ ament_target_dependencies(${LIB_NAME} PUBLIC vortex_utils_ros spdlog fmt + tf2_ros + tf2_eigen ) rclcpp_components_register_node( diff --git a/navigation/eskf/config/eskf_params.yaml b/navigation/eskf/config/eskf_params.yaml index 076e1f65b..5a897f0a9 100644 --- a/navigation/eskf/config/eskf_params.yaml +++ b/navigation/eskf/config/eskf_params.yaml @@ -5,9 +5,15 @@ eskf_node: odom_topic: odom_ESKF diag_Q_std: [0.05, 0.05, 0.1, 0.01, 0.01, 0.02, 0.001, 0.001, 0.001, 0.0001, 0.0001, 0.0001] diag_p_init: [1.0, 1.0, 1.0, 0.5, 0.5, 0.5, 0.1, 0.1, 0.1, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001] - imu_frame: [ -1.0, 0.0, 0.0, + imu_frame_r: [ -1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, -1.0 ] - dvl_frame: [ 1.0, 0.0, 0.0, + imu_frame_t: [ 0.0, 0.0, 0.0 ] + + dvl_frame_r: [ 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0 ] \ No newline at end of file + 0.0, 0.0, 1.0 ] + dvl_frame_t: [ 0.0, 0.0, 0.0 ] + + depth_frame_t: [ 0.0, 0.0, 0.0 ] + use_tf_transforms: false diff --git a/navigation/eskf/include/eskf/eskf.tpp b/navigation/eskf/include/eskf/eskf.tpp index eeb579f6c..6189a609a 100644 --- a/navigation/eskf/include/eskf/eskf.tpp +++ b/navigation/eskf/include/eskf/eskf.tpp @@ -13,7 +13,7 @@ void ESKF::measurement_update(const SensorT& meas) { Eigen::MatrixXd PHt = P * H.transpose(); // More stable and faster than P * H.transpose() * S.inverse() - Eigen::MatrixXd K = S.ldlt().solve(PHt.transpose()).transpose(); + Eigen::MatrixXd K = S.ldlt().solve(PHt.transpose()).transpose(); #ifndef NDEBUG nis_ = compute_nis(innovation, S); diff --git a/navigation/eskf/include/eskf/eskf_ros.hpp b/navigation/eskf/include/eskf/eskf_ros.hpp index 2e2630a10..3dbe84c51 100644 --- a/navigation/eskf/include/eskf/eskf_ros.hpp +++ b/navigation/eskf/include/eskf/eskf_ros.hpp @@ -1,8 +1,12 @@ #ifndef ESKF__ESKF_ROS_HPP_ #define ESKF__ESKF_ROS_HPP_ +#include +#include +#include #include #include +#include #include #include #include @@ -12,6 +16,7 @@ #include #include #include +#include #include "eskf/eskf.hpp" #include "eskf/typedefs.hpp" #include "spdlog/spdlog.h" @@ -40,6 +45,14 @@ class ESKFNode : public rclcpp::Node { // @brief Set the parameters for the eskf void set_parameters(); + // @brief lookup transforms + void initialize_static_transforms(); + + // @brief broadcast the State as a TF + void publish_tf(const StateQuat& nom_state); + + // Subscribers and Publishers + rclcpp::Subscription::SharedPtr imu_sub_; rclcpp::Subscription< @@ -49,6 +62,8 @@ class ESKFNode : public rclcpp::Node { rclcpp::Publisher::SharedPtr nis_pub_; + // Member variable for the ESKF instance + std::chrono::milliseconds time_step; rclcpp::TimerBase::SharedPtr odom_pub_timer_; @@ -58,13 +73,32 @@ class ESKFNode : public rclcpp::Node { bool first_imu_msg_received_ = false; Eigen::Matrix3d R_imu_eskf_{}; + Eigen::Vector3d T_imu_eskf_{}; Eigen::Matrix3d R_dvl_eskf_{}; + Eigen::Vector3d T_dvl_eskf_{}; + + Eigen::Vector3d T_depth_eskf_{}; rclcpp::Time last_imu_time_{}; // Latest gyro measurement (used for publishing odom output of eskf) Eigen::Vector3d latest_gyro_measurement_{}; + + // TF2 Handling + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + std::unique_ptr tf_broadcaster_; + rclcpp::TimerBase::SharedPtr tf_timer_; + + // Flags and Storage + bool use_tf_transforms_ = false; + bool tf_sensors_loaded_ = false; + + // hold the transfer from Sensor -> Base Link + Eigen::Isometry3d Tf_base_imu_ = Eigen::Isometry3d::Identity(); + Eigen::Isometry3d Tf_base_dvl_ = Eigen::Isometry3d::Identity(); + Eigen::Isometry3d Tf_base_depth_ = Eigen::Isometry3d::Identity(); }; #endif // ESKF__ESKF_ROS_HPP_ diff --git a/navigation/eskf/package.xml b/navigation/eskf/package.xml index f8472c188..5d3f9fb9d 100644 --- a/navigation/eskf/package.xml +++ b/navigation/eskf/package.xml @@ -17,6 +17,8 @@ vortex_msgs vortex_utils vortex_utils_ros + tf2_ros + tf2_eigen ament_cmake diff --git a/navigation/eskf/src/eskf.cpp b/navigation/eskf/src/eskf.cpp index fc3660522..d904a8bb5 100644 --- a/navigation/eskf/src/eskf.cpp +++ b/navigation/eskf/src/eskf.cpp @@ -12,10 +12,10 @@ double compute_nis(const Eigen::Vector3d& innovation, } ESKF::ESKF(const EskfParams& params) : Q_(params.Q) { - // Initialize Covariance + // Initialize Covariance current_error_state_.covariance = params.P; - // Initialize Nominal Quaternion to Identity + // Initialize Nominal Quaternion to Identity current_nom_state_.quat = Eigen::Quaterniond::Identity(); } @@ -158,7 +158,7 @@ void ESKF::injection_and_reset() { current_nom_state_.gyro_bias + current_error_state_.gyro_bias; current_nom_state_.accel_bias = current_nom_state_.accel_bias + current_error_state_.accel_bias; - + // reset current_error_state_.set_from_vector(Eigen::Vector15d::Zero()); } diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp index 289625ff3..be0046022 100644 --- a/navigation/eskf/src/eskf_ros.cpp +++ b/navigation/eskf/src/eskf_ros.cpp @@ -23,6 +23,31 @@ ESKFNode::ESKFNode(const rclcpp::NodeOptions& options) set_parameters(); + // Initialize TF Buffer & Listener + tf_buffer_ = std::make_shared(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + + // Initialize Broadcaster (for odom -> base_link) + tf_broadcaster_ = std::make_unique(*this); + + // flag to determine whether to use TF-based transforms or parameter-based + // transforms + this->declare_parameter("use_tf_transforms", true); + use_tf_transforms_ = this->get_parameter("use_tf_transforms").as_bool(); + + // if we have parameters, we skip the TF lookup + tf_sensors_loaded_ = !use_tf_transforms_; + + if (use_tf_transforms_) { + // Check for static transforms every 0.5 seconds + tf_timer_ = this->create_wall_timer( + std::chrono::milliseconds(500), + std::bind(&ESKFNode::initialize_static_transforms, this)); + } else { + spdlog::info( + "Using parameter-based sensor transforms. TF lookup disabled."); + } + spdlog::info(start_message); #ifndef NDEBUG @@ -58,19 +83,35 @@ void ESKFNode::set_subscribers_and_publisher() { } void ESKFNode::set_parameters() { - + // Load sensor frame Rotation correction parameters std::vector R_imu_correction; - this->declare_parameter>("imu_frame"); - R_imu_correction = get_parameter("imu_frame").as_double_array(); + this->declare_parameter>("imu_frame_r"); + R_imu_correction = get_parameter("imu_frame_r").as_double_array(); R_imu_eskf_ = Eigen::Map>( R_imu_correction.data()); - + + // Load sensor frame Translation correction parameters + std::vector T_imu_correction; + this->declare_parameter>("imu_frame_t"); + T_imu_correction = get_parameter("imu_frame_t").as_double_array(); + T_imu_eskf_ = Eigen::Map(T_imu_correction.data()); + std::vector R_dvl_correction; - this->declare_parameter>("dvl_frame"); - R_dvl_correction = get_parameter("dvl_frame").as_double_array(); + this->declare_parameter>("dvl_frame_r"); + R_dvl_correction = get_parameter("dvl_frame_r").as_double_array(); R_dvl_eskf_ = Eigen::Map>( R_dvl_correction.data()); + std::vector T_dvl_correction; + this->declare_parameter>("dvl_frame_t"); + T_dvl_correction = get_parameter("dvl_frame_t").as_double_array(); + T_dvl_eskf_ = Eigen::Map(T_dvl_correction.data()); + + std::vector T_depth_correction; + this->declare_parameter>("depth_frame_t"); + T_depth_correction = get_parameter("depth_frame_t").as_double_array(); + T_depth_eskf_ = Eigen::Map(T_depth_correction.data()); + std::vector diag_Q_std; this->declare_parameter>("diag_Q_std"); @@ -120,7 +161,8 @@ void ESKFNode::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { Eigen::Vector3d raw_gyro(msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z); - // currently the gyro and the accelorometer are rotated differently. should be changed with the actual drone. + // currently the gyro and the accelorometer are rotated differently. + // should be changed with the actual drone params. // imu_measurement.gyro = R_imu_eskf_ * raw_gyro; imu_measurement.gyro = raw_gyro; @@ -142,13 +184,13 @@ void ESKFNode::dvl_callback( msg->twist.covariance[8], msg->twist.covariance[12], msg->twist.covariance[13], msg->twist.covariance[14]; - // Apply the rotation correction to the DVL measurement + // Apply the rotation correction to the DVL measurement dvl_sensor.measurement = R_dvl_eskf_ * dvl_sensor.measurement; - dvl_sensor.measurement_noise = R_dvl_eskf_ * dvl_sensor.measurement_noise * R_dvl_eskf_.transpose(); + dvl_sensor.measurement_noise = + R_dvl_eskf_ * dvl_sensor.measurement_noise * R_dvl_eskf_.transpose(); eskf_->dvl_update(dvl_sensor); - #ifndef NDEBUG // Publish NIS in Debug mode std_msgs::msg::Float64 nis_msg; @@ -176,7 +218,8 @@ void ESKFNode::publish_odom() { odom_msg.twist.twist.linear.z = nom_state.vel.z(); // Add bias values to the angular velocity field of twist - Eigen::Vector3d body_angular_vel = latest_gyro_measurement_ - nom_state.gyro_bias; + Eigen::Vector3d body_angular_vel = + latest_gyro_measurement_ - nom_state.gyro_bias; odom_msg.twist.twist.angular.x = body_angular_vel.x(); odom_msg.twist.twist.angular.y = body_angular_vel.y(); odom_msg.twist.twist.angular.z = body_angular_vel.z(); @@ -213,6 +256,81 @@ void ESKFNode::publish_odom() { } } odom_pub_->publish(odom_msg); + + publish_tf(nom_state); +} + +void ESKFNode::initialize_static_transforms() { + // if already loaded, no need to lookup again. + if (tf_sensors_loaded_) { + tf_timer_->cancel(); + return; + } + + try { + // Lookup IMU -> Base Link + geometry_msgs::msg::TransformStamped tf_imu = + tf_buffer_->lookupTransform("base_link", "imu_frame", + tf2::TimePointZero); + + Tf_base_imu_ = tf2::transformToEigen(tf_imu); + + // Overwrite the parameter-based matrix + R_imu_eskf_ = Tf_base_imu_.rotation(); + spdlog::info("TF: Loaded base_link <- imu_frame transform"); + + // Lookup DVL -> Base Link + geometry_msgs::msg::TransformStamped tf_dvl = + tf_buffer_->lookupTransform("base_link", "dvl_frame", + tf2::TimePointZero); + + Tf_base_dvl_ = tf2::transformToEigen(tf_dvl); + + // Overwrite the parameter-based matrix + R_dvl_eskf_ = Tf_base_dvl_.rotation(); + spdlog::info("TF: Loaded base_link <- dvl_frame transform"); + + // Lookup Depth sensor -> Base Link + geometry_msgs::msg::TransformStamped tf_depth = + tf_buffer_->lookupTransform("base_link", "depth_sensor_frame", + tf2::TimePointZero); + + Tf_base_depth_ = tf2::transformToEigen(tf_depth); + + // Overwrite the parameter-based matrix + T_depth_eskf_ = Tf_base_depth_.translation(); + spdlog::info("TF: Loaded base_link <- depth_sensor_frame transform"); + + // If we reach this point, all transforms were loaded successfully + tf_sensors_loaded_ = true; + + spdlog::info("All static transforms loaded successfully."); + + // Turn off the timer so this function never runs again + tf_timer_->cancel(); + } catch (const tf2::TransformException& ex) { + // It is common to fail on startup before static_publisher is ready + spdlog::warn("TF Lookup failed (will retry): {}", ex.what()); + } +} + +void ESKFNode::publish_tf(const StateQuat& nom_state) { + geometry_msgs::msg::TransformStamped tf_msg; + + tf_msg.header.stamp = this->now(); + tf_msg.header.frame_id = "odom"; + tf_msg.child_frame_id = "base_link"; + + tf_msg.transform.translation.x = nom_state.pos.x(); + tf_msg.transform.translation.y = nom_state.pos.y(); + tf_msg.transform.translation.z = nom_state.pos.z(); + + tf_msg.transform.rotation.w = nom_state.quat.w(); + tf_msg.transform.rotation.x = nom_state.quat.x(); + tf_msg.transform.rotation.y = nom_state.quat.y(); + tf_msg.transform.rotation.z = nom_state.quat.z(); + + tf_broadcaster_->sendTransform(tf_msg); } RCLCPP_COMPONENTS_REGISTER_NODE(ESKFNode) diff --git a/navigation/eskf/test/eskf_consistency_test.py b/navigation/eskf/test/eskf_consistency_test.py index a7d1495b0..0f0c8061f 100644 --- a/navigation/eskf/test/eskf_consistency_test.py +++ b/navigation/eskf/test/eskf_consistency_test.py @@ -5,7 +5,7 @@ from std_msgs.msg import Float64MultiArray import message_filters import numpy as np -from scipy.spatial.transform import Rotation as R +from scipy.spatial.transform import Rotation # --- IMPORT YOUR EXISTING QOS FUNCTION --- try: @@ -79,8 +79,8 @@ def callback(self, est_msg, gt_msg): q_gt = [gt_msg.pose.pose.orientation.x, gt_msg.pose.pose.orientation.y, gt_msg.pose.pose.orientation.z, gt_msg.pose.pose.orientation.w] # Convert to Rotation objects - r_est = R.from_quat(q_est) - r_gt = R.from_quat(q_gt) + r_est = Rotation.from_quat(q_est) + r_gt = Rotation.from_quat(q_gt) # --------------------------------------------------------- # Extract Euler Angles (Roll, Pitch, Yaw) From 59f08350be82dfb410885360f34cf21f6e3f0d24 Mon Sep 17 00:00:00 2001 From: Ahmed Date: Wed, 25 Feb 2026 15:51:16 +0100 Subject: [PATCH 138/290] Switching dvl to the actual simulated topic. Removing duplicate lines from previous merge --- navigation/eskf/CMakeLists.txt | 2 ++ navigation/eskf/config/eskf_params.yaml | 6 ++--- navigation/eskf/include/eskf/eskf_ros.hpp | 7 +++--- navigation/eskf/package.xml | 1 + navigation/eskf/src/eskf_ros.cpp | 27 ++++++++--------------- 5 files changed, 18 insertions(+), 25 deletions(-) diff --git a/navigation/eskf/CMakeLists.txt b/navigation/eskf/CMakeLists.txt index b292b256d..5ceb26e19 100644 --- a/navigation/eskf/CMakeLists.txt +++ b/navigation/eskf/CMakeLists.txt @@ -27,6 +27,7 @@ find_package(spdlog REQUIRED) find_package(fmt REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2_eigen REQUIRED) +find_package(stonefish_ros2 REQUIRED) if(NOT DEFINED EIGEN3_INCLUDE_DIR) set(EIGEN3_INCLUDE_DIR ${EIGEN3_INCLUDE_DIRS}) @@ -57,6 +58,7 @@ ament_target_dependencies(${LIB_NAME} PUBLIC fmt tf2_ros tf2_eigen + stonefish_ros2 ) rclcpp_components_register_node( diff --git a/navigation/eskf/config/eskf_params.yaml b/navigation/eskf/config/eskf_params.yaml index 1e21de66f..46d639244 100644 --- a/navigation/eskf/config/eskf_params.yaml +++ b/navigation/eskf/config/eskf_params.yaml @@ -1,7 +1,7 @@ eskf_node: ros__parameters: imu_topic: /imu/data_raw - dvl_topic: /orca/twist + dvl_topic: /dvl/sim odom_topic: odom_ESKF diag_Q_std: [0.05, 0.05, 0.1, 0.01, 0.01, 0.02, 0.001, 0.001, 0.001, 0.0001, 0.0001, 0.0001] diag_p_init: [1.0, 1.0, 1.0, 0.5, 0.5, 0.5, 0.1, 0.1, 0.1, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001] @@ -10,8 +10,8 @@ eskf_node: 0.0, 0.0, -1.0 ] imu_frame_t: [ 0.0, 0.0, 0.0 ] - dvl_frame_r: [ 1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, + dvl_frame_r: [ -1.0, 0.0, 0.0, + 0.0, -1.0, 0.0, 0.0, 0.0, 1.0 ] dvl_frame_t: [ 0.0, 0.0, 0.0 ] diff --git a/navigation/eskf/include/eskf/eskf_ros.hpp b/navigation/eskf/include/eskf/eskf_ros.hpp index 3dbe84c51..bcec346e8 100644 --- a/navigation/eskf/include/eskf/eskf_ros.hpp +++ b/navigation/eskf/include/eskf/eskf_ros.hpp @@ -16,6 +16,7 @@ #include #include #include +#include #include #include "eskf/eskf.hpp" #include "eskf/typedefs.hpp" @@ -33,8 +34,7 @@ class ESKFNode : public rclcpp::Node { // @brief Callback function for the dvl topic // @param msg: TwistWithCovarianceStamped message containing the dvl data - void dvl_callback( - const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); + void dvl_callback(const stonefish_ros2::msg::DVL::SharedPtr msg); // @brief Publish the odometry message void publish_odom(); @@ -55,8 +55,7 @@ class ESKFNode : public rclcpp::Node { rclcpp::Subscription::SharedPtr imu_sub_; - rclcpp::Subscription< - geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr dvl_sub_; + rclcpp::Subscription::SharedPtr dvl_sub_; rclcpp::Publisher::SharedPtr odom_pub_; diff --git a/navigation/eskf/package.xml b/navigation/eskf/package.xml index 5d3f9fb9d..85cd79ff6 100644 --- a/navigation/eskf/package.xml +++ b/navigation/eskf/package.xml @@ -19,6 +19,7 @@ vortex_utils_ros tf2_ros tf2_eigen + stonefish_ros2 ament_cmake diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp index efeae92a8..1897d0bc0 100644 --- a/navigation/eskf/src/eskf_ros.cpp +++ b/navigation/eskf/src/eskf_ros.cpp @@ -66,8 +66,7 @@ void ESKFNode::set_subscribers_and_publisher() { this->declare_parameter("dvl_topic"); std::string dvl_topic = this->get_parameter("dvl_topic").as_string(); - dvl_sub_ = this->create_subscription< - geometry_msgs::msg::TwistWithCovarianceStamped>( + dvl_sub_ = this->create_subscription( dvl_topic, qos_sensor_data, std::bind(&ESKFNode::dvl_callback, this, std::placeholders::_1)); @@ -155,8 +154,6 @@ void ESKFNode::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { Eigen::Vector3d raw_gyro(msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z); - // currently the gyro and the accelorometer are rotated differently. - // should be changed with the actual drone params. // currently the gyro and the accelorometer are rotated differently. // should be changed with the actual drone params. // imu_measurement.gyro = R_imu_eskf_ * raw_gyro; @@ -168,25 +165,21 @@ void ESKFNode::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { eskf_->imu_update(imu_measurement, dt); } -void ESKFNode::dvl_callback( - const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { +void ESKFNode::dvl_callback(const stonefish_ros2::msg::DVL::SharedPtr msg) { SensorDVL dvl_sensor; - dvl_sensor.measurement << msg->twist.twist.linear.x, - msg->twist.twist.linear.y, msg->twist.twist.linear.z; - dvl_sensor.measurement_noise << msg->twist.covariance[0], - msg->twist.covariance[1], msg->twist.covariance[2], - msg->twist.covariance[6], msg->twist.covariance[7], - msg->twist.covariance[8], msg->twist.covariance[12], - msg->twist.covariance[13], msg->twist.covariance[14]; + dvl_sensor.measurement << msg->velocity.x, msg->velocity.y, msg->velocity.z; + + dvl_sensor.measurement_noise << msg->velocity_covariance[0], + msg->velocity_covariance[1], msg->velocity_covariance[2], + msg->velocity_covariance[3], msg->velocity_covariance[4], + msg->velocity_covariance[5], msg->velocity_covariance[6], + msg->velocity_covariance[7], msg->velocity_covariance[8]; - // Apply the rotation correction to the DVL measurement // Apply the rotation correction to the DVL measurement dvl_sensor.measurement = R_dvl_eskf_ * dvl_sensor.measurement; dvl_sensor.measurement_noise = R_dvl_eskf_ * dvl_sensor.measurement_noise * R_dvl_eskf_.transpose(); - dvl_sensor.measurement_noise = - R_dvl_eskf_ * dvl_sensor.measurement_noise * R_dvl_eskf_.transpose(); eskf_->dvl_update(dvl_sensor); @@ -217,8 +210,6 @@ void ESKFNode::publish_odom() { odom_msg.twist.twist.linear.z = nom_state.vel.z(); // Add bias values to the angular velocity field of twist - Eigen::Vector3d body_angular_vel = - latest_gyro_measurement_ - nom_state.gyro_bias; Eigen::Vector3d body_angular_vel = latest_gyro_measurement_ - nom_state.gyro_bias; odom_msg.twist.twist.angular.x = body_angular_vel.x(); From 4c59e21a680ee44b99922534119350cda338aa66 Mon Sep 17 00:00:00 2001 From: Henrik Date: Wed, 25 Feb 2026 18:00:50 +0100 Subject: [PATCH 139/290] 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 140/290] 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 013db77d739033fe01d883d8329c2254fe895f9d Mon Sep 17 00:00:00 2001 From: Ahmed Date: Mon, 2 Mar 2026 16:45:55 +0100 Subject: [PATCH 141/290] adding the effect from translation of dvl and imu frames --- navigation/eskf/config/eskf_params.yaml | 2 +- navigation/eskf/src/eskf.cpp | 8 +++++- navigation/eskf/src/eskf_ros.cpp | 34 ++++++++++++++++++------- 3 files changed, 33 insertions(+), 11 deletions(-) diff --git a/navigation/eskf/config/eskf_params.yaml b/navigation/eskf/config/eskf_params.yaml index 46d639244..48864df1b 100644 --- a/navigation/eskf/config/eskf_params.yaml +++ b/navigation/eskf/config/eskf_params.yaml @@ -14,7 +14,7 @@ eskf_node: 0.0, -1.0, 0.0, 0.0, 0.0, 1.0 ] - dvl_frame_t: [ 0.0, 0.0, 0.0 ] + dvl_frame_t: [ 0.4, 0.0, 0.2 ] depth_frame_t: [ 0.0, 0.0, 0.0 ] use_tf_transforms: false diff --git a/navigation/eskf/src/eskf.cpp b/navigation/eskf/src/eskf.cpp index d904a8bb5..876410714 100644 --- a/navigation/eskf/src/eskf.cpp +++ b/navigation/eskf/src/eskf.cpp @@ -16,7 +16,13 @@ ESKF::ESKF(const EskfParams& params) : Q_(params.Q) { current_error_state_.covariance = params.P; // Initialize Nominal Quaternion to Identity - current_nom_state_.quat = Eigen::Quaterniond::Identity(); + // current_nom_state_.quat = Eigen::Quaterniond::Identity(); + + // Initialize Quaternion: -90 degrees Yaw because of initial + // drone_orientation + Eigen::AngleAxisd init_rotation(-M_PI / 2.0, Eigen::Vector3d::UnitZ()); + current_nom_state_.quat = Eigen::Quaterniond(init_rotation); + current_nom_state_.quat.normalize(); } std::pair ESKF::van_loan_discretization( diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp index 1897d0bc0..ed8e966d8 100644 --- a/navigation/eskf/src/eskf_ros.cpp +++ b/navigation/eskf/src/eskf_ros.cpp @@ -144,22 +144,32 @@ void ESKFNode::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { double dt = (current_time - last_imu_time_).nanoseconds() * 1e-9; last_imu_time_ = current_time; + ImuMeasurement imu_measurement{}; + Eigen::Vector3d raw_accel(msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z); - ImuMeasurement imu_measurement{}; - imu_measurement.accel = R_imu_eskf_ * raw_accel; - Eigen::Vector3d raw_gyro(msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z); - // currently the gyro and the accelorometer are rotated differently. + Eigen::Vector3d accel_aligned = R_imu_eskf_ * raw_accel; + + // currently the gyro and the accelorometer are rotated differently in sim. // should be changed with the actual drone params. - // imu_measurement.gyro = R_imu_eskf_ * raw_gyro; - imu_measurement.gyro = raw_gyro; + // Eigen::Vector3d gyro_aligned = R_imu_eskf_ * raw_gyro; + Eigen::Vector3d gyro_aligned = raw_gyro; + imu_measurement.gyro = gyro_aligned; - // used for publishing odom output of eskf + // lever arm correction for accelerometer + StateQuat nom_state = eskf_->get_nominal_state(); + Eigen::Vector3d omega = gyro_aligned - nom_state.gyro_bias; + + // a_corrected = a_meas - omega x (omega x T) + Eigen::Vector3d centripetal_accel = omega.cross(omega.cross(T_imu_eskf_)); + imu_measurement.accel = accel_aligned - centripetal_accel; + + // save latest gyro readings (used for DVL correction and odom output) latest_gyro_measurement_ = imu_measurement.gyro; eskf_->imu_update(imu_measurement, dt); @@ -176,8 +186,14 @@ void ESKFNode::dvl_callback(const stonefish_ros2::msg::DVL::SharedPtr msg) { msg->velocity_covariance[5], msg->velocity_covariance[6], msg->velocity_covariance[7], msg->velocity_covariance[8]; - // Apply the rotation correction to the DVL measurement - dvl_sensor.measurement = R_dvl_eskf_ * dvl_sensor.measurement; + // Apply the rotation and translation corrections to the DVL measurement + StateQuat nom_state = eskf_->get_nominal_state(); + // get the angular velocity + Eigen::Vector3d omega_corrected = + latest_gyro_measurement_ - nom_state.gyro_bias; + // correct rotation and translation: v_base = v_sensor - omega x T + dvl_sensor.measurement = R_dvl_eskf_ * dvl_sensor.measurement - + omega_corrected.cross(T_dvl_eskf_); dvl_sensor.measurement_noise = R_dvl_eskf_ * dvl_sensor.measurement_noise * R_dvl_eskf_.transpose(); From 1801343ce57902145a2f9206de061eb35ebfff58 Mon Sep 17 00:00:00 2001 From: Ahmed Date: Mon, 2 Mar 2026 18:25:21 +0100 Subject: [PATCH 142/290] fixing dvl rotation matrix, publishing vel in body frame --- navigation/eskf/config/eskf_params.yaml | 6 +++--- navigation/eskf/src/eskf_ros.cpp | 11 ++++++++--- 2 files changed, 11 insertions(+), 6 deletions(-) diff --git a/navigation/eskf/config/eskf_params.yaml b/navigation/eskf/config/eskf_params.yaml index 48864df1b..12571d6b2 100644 --- a/navigation/eskf/config/eskf_params.yaml +++ b/navigation/eskf/config/eskf_params.yaml @@ -10,9 +10,9 @@ eskf_node: 0.0, 0.0, -1.0 ] imu_frame_t: [ 0.0, 0.0, 0.0 ] - dvl_frame_r: [ -1.0, 0.0, 0.0, - 0.0, -1.0, 0.0, - 0.0, 0.0, 1.0 ] + dvl_frame_r: [ 0.0, -1.0, 0.0, + 1.0, 0.0, 0.0, + 0.0, 0.0, 1.0 ] dvl_frame_t: [ 0.4, 0.0, 0.2 ] diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp index ed8e966d8..a6c8c3c88 100644 --- a/navigation/eskf/src/eskf_ros.cpp +++ b/navigation/eskf/src/eskf_ros.cpp @@ -221,9 +221,14 @@ void ESKFNode::publish_odom() { odom_msg.pose.pose.orientation.y = nom_state.quat.y(); odom_msg.pose.pose.orientation.z = nom_state.quat.z(); - odom_msg.twist.twist.linear.x = nom_state.vel.x(); - odom_msg.twist.twist.linear.y = nom_state.vel.y(); - odom_msg.twist.twist.linear.z = nom_state.vel.z(); + // publishing the velocity in the body frame + Eigen::Matrix3d R_body_to_world = nom_state.quat.toRotationMatrix(); + + Eigen::Vector3d v_body = R_body_to_world.transpose() * nom_state.vel; + + odom_msg.twist.twist.linear.x = v_body.x(); + odom_msg.twist.twist.linear.y = v_body.y(); + odom_msg.twist.twist.linear.z = v_body.z(); // Add bias values to the angular velocity field of twist Eigen::Vector3d body_angular_vel = From 7c7292928edb5d0c6dcef6adfd27ceca2c5026d5 Mon Sep 17 00:00:00 2001 From: ppakr Date: Fri, 6 Mar 2026 13:23:16 +0100 Subject: [PATCH 143/290] docs: update docs --- control/pid_controller_dp/README.md | 83 ++++++++++++++++++++++++++++- 1 file changed, 81 insertions(+), 2 deletions(-) diff --git a/control/pid_controller_dp/README.md b/control/pid_controller_dp/README.md index 902fc9fc3..8afc0f381 100644 --- a/control/pid_controller_dp/README.md +++ b/control/pid_controller_dp/README.md @@ -1,7 +1,86 @@ -## PID controller +# PID controller + The PID controller is defined + ```math \tau = -J_{q}^{\dagger}(K_p \tilde{\eta} + K_d \dot{\tilde{\eta}} + K_i \int^t_0 \tilde{\eta}(\tau)d\tau) ``` -where $\tau$ is the control input, $\tilde{\eta} = \eta - \eta_d$ is the pose error, $J_q$ is the quaternion based Jacobian matrix and $K_p$, $K_d$ and $K_i$ are tuning matrices. +where: + +- $\tilde{\eta} = \eta - \eta_d$ is the pose error (7D quaternion representation), +- $J_q$ is the quaternion Jacobian (7×6), and $J_q^{\dagger}$ is its (pseudo-)inverse, +- $K_p$, $K_d$, $K_i$ are 6×6 gain matrices. + +## PID controller (pid_controller_dp) + +This package implements a 6-DOF PID controller that operates on the +6-dimensional control vector +$$\tau = [X, Y, Z, K, M, N]^T$$ +and uses a quaternion-based 7D pose representation for attitude. + +## Build + + +This package is built as part of the workspace. From the workspace root: + +```bash +colcon build --packages-select pid_controller_dp +``` + +To run tests for this package only: + +```bash +colcon test --packages-select pid_controller_dp && colcon test-result --verbose +``` + +## Usage (ROS 2 node) + +The package provides a node `pid_controller_node` that subscribes to pose, +twist and guidance topics and publishes wrench (tau) commands. + +Common topics & parameters (examples) + +- `topics.pose` (type: `geometry_msgs/PoseWithCovarianceStamped`) — vehicle pose input +- `topics.twist` (type: `geometry_msgs/TwistWithCovarianceStamped`) — velocity input +- `topics.guidance.dp` (type: `vortex_msgs/ReferenceFilter`) — desired pose/velocity +- `topics.wrench_input` (type: `geometry_msgs/WrenchStamped`) — output wrench + +Parameters expose PID gains (Kp, Ki, Kd) as per-component values which are +assembled into diagonal gain matrices inside the node. See the node source +(`src/pid_controller_ros.cpp`) for parameter names. + +## Examples + +Start the node (after sourcing workspace): + +1. Run the simulation + + ```bash + ros2 launch stonefish_sim simulation.launch.py scenario:=default + ``` + +2. Run the thrust allocation node: + + ```bash + ros2 launch thrust_allocator_auv thrust_allocator_auv.launch.py + ``` + +3. To move the robot, run the joystick node + + ```bash + ros2 launch stonefish_sim orca_sim.launch.py + ``` + +4. Run the controller + + ```bash + ros2 launch pid_controller_dp pid_controller_dp.launch.py + ``` + +Use the joy stick to move the robot. The key mappings are: +B - kill +Y - autonomous mode (reference model) +A - manual mode + +Note: When plotting, the axis plotted and actual command might not align since the plotting is based on the joy controller frame (`odom`), whereas the controller works on the robot frame (`body_frame`) From 17d89c98ccfca092d9071c756aee3e5fd08ea7ad Mon Sep 17 00:00:00 2001 From: Anbit Date: Sun, 8 Mar 2026 15:30:10 +0100 Subject: [PATCH 144/290] 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 145/290] 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 146/290] 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 147/290] 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 148/290] 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 149/290] 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 150/290] 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 151/290] 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 152/290] 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 153/290] 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 154/290] 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 155/290] 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 156/290] 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 d97c0f23b96aefdf659aa21105a9f4e118715753 Mon Sep 17 00:00:00 2001 From: ppakr Date: Wed, 11 Mar 2026 19:34:21 +0100 Subject: [PATCH 157/290] refactor: remove commented-out PID parameter arrays from pid_params.yaml --- control/pid_controller_dp/config/pid_params.yaml | 3 --- 1 file changed, 3 deletions(-) diff --git a/control/pid_controller_dp/config/pid_params.yaml b/control/pid_controller_dp/config/pid_params.yaml index 811e8c63d..17bcd5026 100644 --- a/control/pid_controller_dp/config/pid_params.yaml +++ b/control/pid_controller_dp/config/pid_params.yaml @@ -1,8 +1,5 @@ /**: ros__parameters: - # Kp: [70.0, 70.0, 70.0, 12.0, 12.0, 12.0] - # Ki: [2.0, 2.0, 2.0, 0.12, 0.12, 0.12] - # Kd: [10.0, 10.0, 10.0, 4.0, 5.0, 4.0] Kp_x: 20.0 Kp_y: 26.0 Kp_z: 50.0 From d69fed65eaf5235451437ba7f25a6eaf9b632b2e Mon Sep 17 00:00:00 2001 From: ppakr Date: Wed, 11 Mar 2026 19:34:58 +0100 Subject: [PATCH 158/290] docs(controller): update readme --- control/pid_controller_dp/README.md | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/control/pid_controller_dp/README.md b/control/pid_controller_dp/README.md index 8afc0f381..40c470e3e 100644 --- a/control/pid_controller_dp/README.md +++ b/control/pid_controller_dp/README.md @@ -39,11 +39,10 @@ colcon test --packages-select pid_controller_dp && colcon test-result --verbose The package provides a node `pid_controller_node` that subscribes to pose, twist and guidance topics and publishes wrench (tau) commands. -Common topics & parameters (examples) - `topics.pose` (type: `geometry_msgs/PoseWithCovarianceStamped`) — vehicle pose input - `topics.twist` (type: `geometry_msgs/TwistWithCovarianceStamped`) — velocity input -- `topics.guidance.dp` (type: `vortex_msgs/ReferenceFilter`) — desired pose/velocity +- `topics.guidance.dp` (type: `vortex_msgs/ReferenceFilter`) — desired states (pose/vecocity) - `topics.wrench_input` (type: `geometry_msgs/WrenchStamped`) — output wrench Parameters expose PID gains (Kp, Ki, Kd) as per-component values which are @@ -79,8 +78,17 @@ Start the node (after sourcing workspace): ``` Use the joy stick to move the robot. The key mappings are: -B - kill -Y - autonomous mode (reference model) -A - manual mode + +- B - kill +- Y - autonomous mode (reference model) +- A - manual mode Note: When plotting, the axis plotted and actual command might not align since the plotting is based on the joy controller frame (`odom`), whereas the controller works on the robot frame (`body_frame`) + +## Tuning + +The `rqt_reconfigure` can be used to change the controller gains. + +```bash +ros2 run rqt_reconfigure rqt_reconfigure +``` From 7a39ece3d4571a45643e6505dbe83a65d64400ec Mon Sep 17 00:00:00 2001 From: Anbit Date: Thu, 12 Mar 2026 16:09:42 +0100 Subject: [PATCH 159/290] 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 d00ffd1f347b24d772c9d42d9e99696480b176d4 Mon Sep 17 00:00:00 2001 From: Ahmed Date: Sun, 15 Mar 2026 19:06:07 +0100 Subject: [PATCH 160/290] changing dvl input and params use based on latest changes in simulation and auv_setup --- navigation/eskf/config/eskf_params.yaml | 32 +++++++++--------- navigation/eskf/include/eskf/eskf_ros.hpp | 6 ++-- navigation/eskf/launch/eskf.launch.py | 33 +++++++++++++++---- navigation/eskf/src/eskf_ros.cpp | 32 ++++++++++-------- navigation/eskf/test/eskf_consistency_test.py | 4 +-- 5 files changed, 66 insertions(+), 41 deletions(-) diff --git a/navigation/eskf/config/eskf_params.yaml b/navigation/eskf/config/eskf_params.yaml index 12571d6b2..15837f13e 100644 --- a/navigation/eskf/config/eskf_params.yaml +++ b/navigation/eskf/config/eskf_params.yaml @@ -1,20 +1,18 @@ -eskf_node: - ros__parameters: - imu_topic: /imu/data_raw - dvl_topic: /dvl/sim - odom_topic: odom_ESKF - diag_Q_std: [0.05, 0.05, 0.1, 0.01, 0.01, 0.02, 0.001, 0.001, 0.001, 0.0001, 0.0001, 0.0001] - diag_p_init: [1.0, 1.0, 1.0, 0.5, 0.5, 0.5, 0.1, 0.1, 0.1, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001] - imu_frame_r: [ -1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, -1.0 ] - imu_frame_t: [ 0.0, 0.0, 0.0 ] +/**: + eskf_node: + ros__parameters: + diag_Q_std: [0.05, 0.05, 0.1, 0.01, 0.01, 0.02, 0.001, 0.001, 0.001, 0.0001, 0.0001, 0.0001] + diag_p_init: [1.0, 1.0, 1.0, 0.5, 0.5, 0.5, 0.1, 0.1, 0.1, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001] + imu_frame_r: [ -1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, -1.0 ] + imu_frame_t: [ 0.0, 0.0, 0.0 ] - dvl_frame_r: [ 0.0, -1.0, 0.0, - 1.0, 0.0, 0.0, - 0.0, 0.0, 1.0 ] + dvl_frame_r: [ 0.0, -1.0, 0.0, + 1.0, 0.0, 0.0, + 0.0, 0.0, 1.0 ] - dvl_frame_t: [ 0.4, 0.0, 0.2 ] + dvl_frame_t: [ 0.4, 0.0, 0.2 ] - depth_frame_t: [ 0.0, 0.0, 0.0 ] - use_tf_transforms: false + depth_frame_t: [ 0.0, 0.0, 0.0 ] + use_tf_transforms: false diff --git a/navigation/eskf/include/eskf/eskf_ros.hpp b/navigation/eskf/include/eskf/eskf_ros.hpp index bcec346e8..1f1b4ad54 100644 --- a/navigation/eskf/include/eskf/eskf_ros.hpp +++ b/navigation/eskf/include/eskf/eskf_ros.hpp @@ -34,7 +34,8 @@ class ESKFNode : public rclcpp::Node { // @brief Callback function for the dvl topic // @param msg: TwistWithCovarianceStamped message containing the dvl data - void dvl_callback(const stonefish_ros2::msg::DVL::SharedPtr msg); + void dvl_callback( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); // @brief Publish the odometry message void publish_odom(); @@ -55,7 +56,8 @@ class ESKFNode : public rclcpp::Node { rclcpp::Subscription::SharedPtr imu_sub_; - rclcpp::Subscription::SharedPtr dvl_sub_; + rclcpp::Subscription< + geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr dvl_sub_; rclcpp::Publisher::SharedPtr odom_pub_; diff --git a/navigation/eskf/launch/eskf.launch.py b/navigation/eskf/launch/eskf.launch.py index d7b3a8a4e..cdb4a2aac 100644 --- a/navigation/eskf/launch/eskf.launch.py +++ b/navigation/eskf/launch/eskf.launch.py @@ -1,23 +1,44 @@ +import os from os import path from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription +from launch.actions import OpaqueFunction from launch_ros.actions import Node +from auv_setup.launch_arg_common import ( + declare_drone_and_namespace_args, + resolve_drone_and_namespace, +) + eskf_params = path.join( get_package_share_directory("eskf"), "config", "eskf_params.yaml" ) -def generate_launch_description(): +def launch_setup(context, *args, **kwargs): + drone, namespace = resolve_drone_and_namespace(context) + + drone_params = os.path.join( + get_package_share_directory("auv_setup"), + "config", + "robots", + f"{drone}.yaml", + ) eskf_node = Node( package="eskf", executable="eskf_node", name="eskf_node", - # namespace="orca", - parameters=[ - eskf_params, - ], + namespace=namespace, + parameters=[eskf_params, drone_params], output="screen", ) - return LaunchDescription([eskf_node]) + + return [eskf_node] + + +def generate_launch_description(): + # This function defines WHAT to do, but doesn't execute the logic yet + return LaunchDescription( + declare_drone_and_namespace_args() + [OpaqueFunction(function=launch_setup)] + ) diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp index a6c8c3c88..21239ff99 100644 --- a/navigation/eskf/src/eskf_ros.cpp +++ b/navigation/eskf/src/eskf_ros.cpp @@ -58,20 +58,22 @@ ESKFNode::ESKFNode(const rclcpp::NodeOptions& options) void ESKFNode::set_subscribers_and_publisher() { auto qos_sensor_data = vortex::utils::qos_profiles::sensor_data_profile(1); - this->declare_parameter("imu_topic"); - std::string imu_topic = this->get_parameter("imu_topic").as_string(); + + this->declare_parameter("topics.imu"); + std::string imu_topic = this->get_parameter("topics.imu").as_string(); imu_sub_ = this->create_subscription( imu_topic, qos_sensor_data, std::bind(&ESKFNode::imu_callback, this, std::placeholders::_1)); - this->declare_parameter("dvl_topic"); - std::string dvl_topic = this->get_parameter("dvl_topic").as_string(); - dvl_sub_ = this->create_subscription( + this->declare_parameter("topics.dvl_twist"); + std::string dvl_topic = this->get_parameter("topics.dvl_twist").as_string(); + dvl_sub_ = this->create_subscription< + geometry_msgs::msg::TwistWithCovarianceStamped>( dvl_topic, qos_sensor_data, std::bind(&ESKFNode::dvl_callback, this, std::placeholders::_1)); - this->declare_parameter("odom_topic"); - std::string odom_topic = this->get_parameter("odom_topic").as_string(); + this->declare_parameter("topics.odom"); + std::string odom_topic = this->get_parameter("topics.odom").as_string(); odom_pub_ = this->create_publisher( odom_topic, qos_sensor_data); @@ -175,16 +177,18 @@ void ESKFNode::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { eskf_->imu_update(imu_measurement, dt); } -void ESKFNode::dvl_callback(const stonefish_ros2::msg::DVL::SharedPtr msg) { +void ESKFNode::dvl_callback( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { SensorDVL dvl_sensor; - dvl_sensor.measurement << msg->velocity.x, msg->velocity.y, msg->velocity.z; + dvl_sensor.measurement << msg->twist.twist.linear.x, + msg->twist.twist.linear.y, msg->twist.twist.linear.z; - dvl_sensor.measurement_noise << msg->velocity_covariance[0], - msg->velocity_covariance[1], msg->velocity_covariance[2], - msg->velocity_covariance[3], msg->velocity_covariance[4], - msg->velocity_covariance[5], msg->velocity_covariance[6], - msg->velocity_covariance[7], msg->velocity_covariance[8]; + dvl_sensor.measurement_noise << msg->twist.covariance[0], + msg->twist.covariance[1], msg->twist.covariance[2], + msg->twist.covariance[6], msg->twist.covariance[7], + msg->twist.covariance[8], msg->twist.covariance[12], + msg->twist.covariance[13], msg->twist.covariance[14]; // Apply the rotation and translation corrections to the DVL measurement StateQuat nom_state = eskf_->get_nominal_state(); diff --git a/navigation/eskf/test/eskf_consistency_test.py b/navigation/eskf/test/eskf_consistency_test.py index 0f0c8061f..f130a1352 100644 --- a/navigation/eskf/test/eskf_consistency_test.py +++ b/navigation/eskf/test/eskf_consistency_test.py @@ -20,8 +20,8 @@ class EskfValidator(Node): def __init__(self): super().__init__('eskf_validator') - self.est_topic = '/odom_ESKF' - self.gt_topic = '/orca/odom' + self.est_topic = 'orca/odom' + self.gt_topic = '/orca/odom/stonefish' # --- Publishers (Foxglove Visualizers) --- # RMSE (Running Root Mean Square Error) From 0431417b0da2021f77e7b2088f60890f2251b63f Mon Sep 17 00:00:00 2001 From: Anbit Date: Mon, 16 Mar 2026 13:13:55 +0100 Subject: [PATCH 161/290] 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 162/290] 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 163/290] 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 164/290] 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 165/290] 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 166/290] 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 9a560f7c19ef20169135fb34c593f8dc8645a075 Mon Sep 17 00:00:00 2001 From: ppakr Date: Sat, 21 Mar 2026 16:25:33 +0100 Subject: [PATCH 167/290] refactor: update message types in PID controller to use Odom as input and change type in utils from stamped -> no stamped --- .../pid_controller_conversions.hpp | 8 ++--- .../pid_controller_dp/pid_controller_ros.hpp | 20 +++--------- .../src/pid_controller_conversions.cpp | 32 +++++++++---------- .../src/pid_controller_ros.cpp | 32 ++++++------------- 4 files changed, 33 insertions(+), 59 deletions(-) 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 a28097e9f..5b2ea6dbc 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 @@ -3,16 +3,16 @@ #include #include -#include -#include +#include +#include #include #include #include "pid_controller_dp/typedefs.hpp" types::Eta eta_convert_from_ros_to_eigen( - const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); + const geometry_msgs::msg::PoseWithCovariance& msg); types::Nu nu_convert_from_ros_to_eigen( - const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); + const geometry_msgs::msg::TwistWithCovariance& msg); #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 0f1cbdbf3..616f1ec76 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 @@ -36,16 +36,6 @@ class PIDControllerNode : public rclcpp::Node { void operation_mode_callback( const vortex_msgs::msg::OperationMode::SharedPtr msg); - // @brief Callback function for the pose topic - // @param msg: PoseWithCovarianceStamped message containing the AUV pose - void pose_callback( - const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); - - // @brief Callback function for the twist topic - // @param msg: TwistWithCovarianceStamped message containing the AUV speed - void twist_callback( - const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); - // @brief Callback function for the tau publisher timer void publish_tau(); @@ -65,6 +55,10 @@ class PIDControllerNode : public rclcpp::Node { void guidance_callback( const vortex_msgs::msg::ReferenceFilter::SharedPtr msg); + // @brief Callback function for the odometry topic + // @param msg: Odometry message containing the AUV pose and speed + void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg); + // @brief Callback function for parameter updates // @param parameters: vector of parameters to be set rcl_interfaces::msg::SetParametersResult parametersCallback( @@ -80,11 +74,7 @@ class PIDControllerNode : public rclcpp::Node { rclcpp::Subscription::SharedPtr operation_mode_sub_; - rclcpp::Subscription< - geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_; - - rclcpp::Subscription< - geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr twist_sub_; + rclcpp::Subscription::SharedPtr odom_sub_; rclcpp::Subscription::SharedPtr guidance_sub_; diff --git a/control/pid_controller_dp/src/pid_controller_conversions.cpp b/control/pid_controller_dp/src/pid_controller_conversions.cpp index d13099cac..1720752c4 100644 --- a/control/pid_controller_dp/src/pid_controller_conversions.cpp +++ b/control/pid_controller_dp/src/pid_controller_conversions.cpp @@ -5,30 +5,28 @@ #include "pid_controller_dp/typedefs.hpp" types::Eta eta_convert_from_ros_to_eigen( - const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { + const geometry_msgs::msg::PoseWithCovariance& msg) { types::Eta eta; - eta.x = msg->pose.pose.position.x; - eta.y = msg->pose.pose.position.y; - eta.z = msg->pose.pose.position.z; - - eta.qw = msg->pose.pose.orientation.w; - eta.qx = msg->pose.pose.orientation.x; - eta.qy = msg->pose.pose.orientation.y; - eta.qz = msg->pose.pose.orientation.z; + eta.x = msg.pose.position.x; + eta.y = msg.pose.position.y; + eta.z = msg.pose.position.z; + eta.qw = msg.pose.orientation.w; + eta.qx = msg.pose.orientation.x; + eta.qy = msg.pose.orientation.y; + eta.qz = msg.pose.orientation.z; return eta; } types::Nu nu_convert_from_ros_to_eigen( - const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { + const geometry_msgs::msg::TwistWithCovariance& msg) { types::Nu nu; - 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; + nu.u = msg.twist.linear.x; + nu.v = msg.twist.linear.y; + nu.w = msg.twist.linear.z; + nu.p = msg.twist.angular.x; + nu.q = msg.twist.angular.y; + nu.r = msg.twist.angular.z; return nu; } diff --git a/control/pid_controller_dp/src/pid_controller_ros.cpp b/control/pid_controller_dp/src/pid_controller_ros.cpp index 24b438c92..c473956c9 100644 --- a/control/pid_controller_dp/src/pid_controller_ros.cpp +++ b/control/pid_controller_dp/src/pid_controller_ros.cpp @@ -32,11 +32,8 @@ void PIDControllerNode::set_subscribers_and_publisher() { std::string dp_reference_topic = this->get_parameter("topics.guidance.dp").as_string(); - this->declare_parameter("topics.pose"); - std::string pose_topic = this->get_parameter("topics.pose").as_string(); - - this->declare_parameter("topics.twist"); - std::string twist_topic = this->get_parameter("topics.twist").as_string(); + this->declare_parameter("topics.odom"); + std::string odom_topic = this->get_parameter("topics.odom").as_string(); this->declare_parameter("topics.killswitch"); std::string software_kill_switch_topic = @@ -60,16 +57,9 @@ void PIDControllerNode::set_subscribers_and_publisher() { std::bind(&PIDControllerNode::operation_mode_callback, this, std::placeholders::_1)); - pose_sub_ = this->create_subscription< - geometry_msgs::msg::PoseWithCovarianceStamped>( - pose_topic, qos_sensor_data, - std::bind(&PIDControllerNode::pose_callback, this, - std::placeholders::_1)); - - twist_sub_ = this->create_subscription< - geometry_msgs::msg::TwistWithCovarianceStamped>( - twist_topic, qos_sensor_data, - std::bind(&PIDControllerNode::twist_callback, this, + odom_sub_ = this->create_subscription( + odom_topic, qos_sensor_data, + std::bind(&PIDControllerNode::odom_callback, this, std::placeholders::_1)); guidance_sub_ = @@ -125,14 +115,10 @@ void PIDControllerNode::operation_mode_callback( operation_mode_ = vortex::utils::ros_conversions::convert_from_ros(*msg); } -void PIDControllerNode::pose_callback( - const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { - eta_ = eta_convert_from_ros_to_eigen(msg); -} - -void PIDControllerNode::twist_callback( - const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { - nu_ = nu_convert_from_ros_to_eigen(msg); +void PIDControllerNode::odom_callback( + const nav_msgs::msg::Odometry::SharedPtr msg) { + eta_ = eta_convert_from_ros_to_eigen(msg->pose); + nu_ = nu_convert_from_ros_to_eigen(msg->twist); } void PIDControllerNode::publish_tau() { From 2d347c25adcc9c2f78afd02717d1dd9b8fb6ef68 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Sat, 21 Mar 2026 17:05:56 +0100 Subject: [PATCH 168/290] init package structure --- .../reference_filter_dp_quat/CMakeLists.txt | 106 ++++++++ .../config/reference_filter_params.yaml | 5 + .../lib/eigen_typedefs.hpp | 21 ++ .../lib/reference_filter.hpp | 50 ++++ .../lib/waypoint_follower.hpp | 83 ++++++ .../lib/waypoint_types.hpp | 34 +++ .../lib/waypoint_utils.hpp | 42 +++ .../ros/reference_filter_ros.hpp | 97 +++++++ .../ros/reference_filter_ros_utils.hpp | 65 +++++ .../launch/reference_filter_dp_quat.launch.py | 45 ++++ guidance/reference_filter_dp_quat/package.xml | 24 ++ .../src/lib/reference_filter.cpp | 37 +++ .../src/lib/waypoint_follower.cpp | 73 ++++++ .../src/lib/waypoint_utils.cpp | 72 ++++++ .../src/ros/reference_filter_ros.cpp | 243 ++++++++++++++++++ .../test/CMakeLists.txt | 57 ++++ .../test/test_reference_filter.cpp | 58 +++++ .../test/test_waypoint_follower.cpp | 111 ++++++++ .../test/test_waypoint_utils.cpp | 129 ++++++++++ 19 files changed, 1352 insertions(+) create mode 100644 guidance/reference_filter_dp_quat/CMakeLists.txt create mode 100644 guidance/reference_filter_dp_quat/config/reference_filter_params.yaml create mode 100644 guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/eigen_typedefs.hpp create mode 100644 guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/reference_filter.hpp create mode 100644 guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_follower.hpp create mode 100644 guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_types.hpp create mode 100644 guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_utils.hpp create mode 100644 guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros.hpp create mode 100644 guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros_utils.hpp create mode 100644 guidance/reference_filter_dp_quat/launch/reference_filter_dp_quat.launch.py create mode 100644 guidance/reference_filter_dp_quat/package.xml create mode 100644 guidance/reference_filter_dp_quat/src/lib/reference_filter.cpp create mode 100644 guidance/reference_filter_dp_quat/src/lib/waypoint_follower.cpp create mode 100644 guidance/reference_filter_dp_quat/src/lib/waypoint_utils.cpp create mode 100644 guidance/reference_filter_dp_quat/src/ros/reference_filter_ros.cpp create mode 100644 guidance/reference_filter_dp_quat/test/CMakeLists.txt create mode 100644 guidance/reference_filter_dp_quat/test/test_reference_filter.cpp create mode 100644 guidance/reference_filter_dp_quat/test/test_waypoint_follower.cpp create mode 100644 guidance/reference_filter_dp_quat/test/test_waypoint_utils.cpp diff --git a/guidance/reference_filter_dp_quat/CMakeLists.txt b/guidance/reference_filter_dp_quat/CMakeLists.txt new file mode 100644 index 000000000..6967c0a88 --- /dev/null +++ b/guidance/reference_filter_dp_quat/CMakeLists.txt @@ -0,0 +1,106 @@ +cmake_minimum_required(VERSION 3.8) +project(reference_filter_dp_quat) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 20) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(vortex_msgs REQUIRED) +find_package(vortex_utils REQUIRED) +find_package(vortex_utils_ros REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(spdlog REQUIRED) +find_package(fmt REQUIRED) + +include_directories(include) + +set(CORE_LIB_NAME "${PROJECT_NAME}") + +add_library(${CORE_LIB_NAME} SHARED + src/lib/reference_filter.cpp + src/lib/waypoint_utils.cpp + src/lib/waypoint_follower.cpp +) + +target_include_directories(${CORE_LIB_NAME} + PUBLIC + $ + $ +) + +ament_target_dependencies(${CORE_LIB_NAME} + Eigen3 + vortex_utils +) + +set(COMPONENT_LIB_NAME "${PROJECT_NAME}_component") + +add_library(${COMPONENT_LIB_NAME} SHARED + src/ros/reference_filter_ros.cpp +) + +ament_target_dependencies(${COMPONENT_LIB_NAME} + rclcpp + rclcpp_components + rclcpp_action + geometry_msgs + nav_msgs + vortex_msgs + vortex_utils + vortex_utils_ros + spdlog + fmt +) + +target_link_libraries(${COMPONENT_LIB_NAME} ${CORE_LIB_NAME}) + +rclcpp_components_register_node( + ${COMPONENT_LIB_NAME} + PLUGIN "ReferenceFilterNode" + EXECUTABLE ${PROJECT_NAME}_node +) + +install( + TARGETS + ${CORE_LIB_NAME} + ${COMPONENT_LIB_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install( + DIRECTORY include/ + DESTINATION include +) + +install(DIRECTORY + launch + config + DESTINATION share/${PROJECT_NAME}/ +) + +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + Eigen3 + vortex_utils +) +ament_export_include_directories(include) +ament_export_libraries(${CORE_LIB_NAME}) + +if(BUILD_TESTING) + add_subdirectory(test) +endif() + +ament_package() diff --git a/guidance/reference_filter_dp_quat/config/reference_filter_params.yaml b/guidance/reference_filter_dp_quat/config/reference_filter_params.yaml new file mode 100644 index 000000000..1aef0ec13 --- /dev/null +++ b/guidance/reference_filter_dp_quat/config/reference_filter_params.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + zeta: [1., 1., 1., 1., 1., 1.] + omega: [0.2, 0.2, 0.2, 0.2, 0.2, 0.2] + time_step_ms: 10 diff --git a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/eigen_typedefs.hpp b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/eigen_typedefs.hpp new file mode 100644 index 000000000..1c9ce6734 --- /dev/null +++ b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/eigen_typedefs.hpp @@ -0,0 +1,21 @@ +/** + * @file eigen_typedefs.hpp + * @brief Contains Eigen typedefs used in this package. + */ + +#ifndef REFERENCE_FILTER_DP_QUAT__LIB__EIGEN_TYPEDEFS_HPP_ +#define REFERENCE_FILTER_DP_QUAT__LIB__EIGEN_TYPEDEFS_HPP_ + +#include + +namespace Eigen { + +typedef Eigen::Matrix Matrix18d; +typedef Eigen::Matrix Matrix18x6d; +typedef Eigen::Matrix Matrix6d; +typedef Eigen::Matrix Vector6d; +typedef Eigen::Matrix Vector18d; + +} // namespace Eigen + +#endif // REFERENCE_FILTER_DP_QUAT__LIB__EIGEN_TYPEDEFS_HPP_ diff --git a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/reference_filter.hpp b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/reference_filter.hpp new file mode 100644 index 000000000..9eefc4a55 --- /dev/null +++ b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/reference_filter.hpp @@ -0,0 +1,50 @@ +#ifndef REFERENCE_FILTER_DP_QUAT__LIB__REFERENCE_FILTER_HPP_ +#define REFERENCE_FILTER_DP_QUAT__LIB__REFERENCE_FILTER_HPP_ + +#include "reference_filter_dp_quat/lib/eigen_typedefs.hpp" + +namespace vortex::guidance { + +struct ReferenceFilterParams { + Eigen::Vector6d omega = Eigen::Vector6d::Zero(); + Eigen::Vector6d zeta = Eigen::Vector6d::Zero(); +}; + +/** + * @brief Stateless third-order reference filter. + * + * Computes the state derivative x_dot = Ad * x + Bd * r, where the state + * x = [eta, eta_dot, eta_ddot] is 18-dimensional and r is the 6D reference. + */ +class ReferenceFilter { + public: + explicit ReferenceFilter(const ReferenceFilterParams& params); + + // @brief Calculate the state derivative + // @param x The state vector 18x1 + // @param r The reference vector 6x1 + // @return The state derivative 18x1 + // REF: Handbook of Marine Craft Hydrodynamics and Motion Control, Fossen + // 2021 p. 337 eq: 12.11 + Eigen::Vector18d calculate_x_dot(const Eigen::Vector18d& x, + const Eigen::Vector6d& r); + + // @brief Calculate the state transition matrix + // REF: Handbook of Marine Craft Hydrodynamics and Motion Control, Fossen + // 2021 p. 337 eq: 12.12 + void calculate_Ad(const Eigen::Vector6d& omega, + const Eigen::Vector6d& zeta); + + // @brief Calculate the input matrix + // REF: Handbook of Marine Craft Hydrodynamics and Motion Control, Fossen + // 2021 p. 337 eq: 12.12 + void calculate_Bd(const Eigen::Vector6d& omega); + + private: + Eigen::Matrix18d Ad_ = Eigen::Matrix18d::Zero(); + Eigen::Matrix18x6d Bd_ = Eigen::Matrix18x6d::Zero(); +}; + +} // namespace vortex::guidance + +#endif // REFERENCE_FILTER_DP_QUAT__LIB__REFERENCE_FILTER_HPP_ diff --git a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_follower.hpp b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_follower.hpp new file mode 100644 index 000000000..8cc3dba6c --- /dev/null +++ b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_follower.hpp @@ -0,0 +1,83 @@ +#ifndef REFERENCE_FILTER_DP_QUAT__LIB__WAYPOINT_FOLLOWER_HPP_ +#define REFERENCE_FILTER_DP_QUAT__LIB__WAYPOINT_FOLLOWER_HPP_ + +#include +#include +#include "reference_filter_dp_quat/lib/eigen_typedefs.hpp" +#include "reference_filter_dp_quat/lib/reference_filter.hpp" +#include "reference_filter_dp_quat/lib/waypoint_types.hpp" + +namespace vortex::guidance { + +using vortex::utils::types::PoseEuler; +using vortex::utils::types::Twist; + +/** + * @brief Manages reference filter state and waypoint following logic. + * + * Thread-safe: all public methods acquire a mutex. + */ +class WaypointFollower { + public: + WaypointFollower(const ReferenceFilterParams& params, double dt_seconds); + + /** + * @brief Initialize the follower with the current vehicle state and target. + * @param pose Current vehicle pose. + * @param twist Current vehicle twist (body frame). + * @param waypoint Target waypoint with mode. + * @param convergence_threshold Max error norm to consider target reached. + */ + void start(const PoseEuler& pose, + const Twist& twist, + const Waypoint& waypoint, + double convergence_threshold); + + /** + * @brief Advance the filter by one time step. + * @return The updated filter state. + */ + Eigen::Vector18d step(); + + /** + * @brief Check if the measured pose has converged to the reference goal. + * @param measured_pose Current measured pose. + * @return True if the error norm is within the convergence threshold. + */ + bool within_convergance(const Eigen::Vector6d& measured_pose) const; + + /** + * @brief Update the reference goal pose mid-sequence. + * @param reference_goal_pose The new reference pose. + */ + void set_reference(const PoseEuler& reference_goal_pose); + + /** + * @brief Snap the position component of the filter state to the reference. + * + * Useful after convergence to eliminate any remaining steady-state offset + */ + void snap_state_to_reference(); + + /// @brief Get the current 18D filter state. + Eigen::Vector18d state() const; + + /// @brief Get the current 6D reference goal pose. + Eigen::Vector6d reference() const; + + private: + Eigen::Vector18d compute_initial_state(const PoseEuler& pose, + const Twist& twist); + + mutable std::mutex mutex_; + ReferenceFilter filter_; + double dt_seconds_{0.01}; + Eigen::Vector18d state_ = Eigen::Vector18d::Zero(); + Eigen::Vector6d reference_goal_ = Eigen::Vector6d::Zero(); + WaypointMode waypoint_mode_{WaypointMode::FULL_POSE}; + double convergence_threshold_{0.1}; +}; + +} // namespace vortex::guidance + +#endif // REFERENCE_FILTER_DP_QUAT__LIB__WAYPOINT_FOLLOWER_HPP_ diff --git a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_types.hpp b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_types.hpp new file mode 100644 index 000000000..9d379d5e3 --- /dev/null +++ b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_types.hpp @@ -0,0 +1,34 @@ +#ifndef REFERENCE_FILTER_DP_QUAT__LIB__WAYPOINT_TYPES_HPP_ +#define REFERENCE_FILTER_DP_QUAT__LIB__WAYPOINT_TYPES_HPP_ + +#include +#include + +namespace vortex::guidance { + +using vortex::utils::types::PoseEuler; + +/** + * @brief Determines which degrees of freedom the reference filter controls. + * + * The mode affects both the reference goal computation (via apply_mode_logic) + * and the convergence check (via has_converged). + */ +enum class WaypointMode : uint8_t { + FULL_POSE = 0, ///< Control all 6 DOF. + ONLY_POSITION = 1, ///< Control x, y, z; hold current orientation. + FORWARD_HEADING = 2, ///< Control x, y, z with yaw toward target. + ONLY_ORIENTATION = 3, ///< Control roll, pitch, yaw; hold current position. +}; + +/** + * @brief A target pose with an associated waypoint mode. + */ +struct Waypoint { + PoseEuler pose{}; + WaypointMode mode = WaypointMode::FULL_POSE; +}; + +} // namespace vortex::guidance + +#endif // REFERENCE_FILTER_DP_QUAT__LIB__WAYPOINT_TYPES_HPP_ diff --git a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_utils.hpp b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_utils.hpp new file mode 100644 index 000000000..0eb309213 --- /dev/null +++ b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_utils.hpp @@ -0,0 +1,42 @@ +#ifndef REFERENCE_FILTER_DP_QUAT__LIB__WAYPOINT_UTILS_HPP_ +#define REFERENCE_FILTER_DP_QUAT__LIB__WAYPOINT_UTILS_HPP_ + +#include "reference_filter_dp_quat/lib/eigen_typedefs.hpp" +#include "reference_filter_dp_quat/lib/waypoint_types.hpp" + +namespace vortex::guidance { + +/** + * @brief Apply waypoint mode logic to a reference pose. + * + * For modes that don't control all DOFs, the uncontrolled components are + * replaced with values from @p current_state so the filter holds them steady. + * + * @param r_in The raw reference pose (6D). + * @param mode The waypoint mode. + * @param current_state The current filter state pose (6D). + * @return The adjusted reference pose. + */ +Eigen::Vector6d apply_mode_logic(const Eigen::Vector6d& r_in, + WaypointMode mode, + const Eigen::Vector6d& current_state); + +/** + * @brief Check whether the measured pose has converged to the reference. + * + * Only the DOFs relevant to the waypoint mode are included in the error norm. + * + * @param measured_pose The current measured pose (6D). + * @param reference The reference goal pose (6D). + * @param mode The waypoint mode. + * @param convergence_threshold The maximum allowed error norm. + * @return True if the error is below the threshold. + */ +bool has_converged(const Eigen::Vector6d& measured_pose, + const Eigen::Vector6d& reference, + WaypointMode mode, + double convergence_threshold); + +} // namespace vortex::guidance + +#endif // REFERENCE_FILTER_DP_QUAT__LIB__WAYPOINT_UTILS_HPP_ diff --git a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros.hpp b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros.hpp new file mode 100644 index 000000000..ccf893040 --- /dev/null +++ b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros.hpp @@ -0,0 +1,97 @@ +#ifndef REFERENCE_FILTER_DP_QUAT__ROS__REFERENCE_FILTER_ROS_HPP_ +#define REFERENCE_FILTER_DP_QUAT__ROS__REFERENCE_FILTER_ROS_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "reference_filter_dp_quat/lib/waypoint_follower.hpp" + +namespace vortex::guidance { + +class ReferenceFilterNode : public rclcpp::Node { + public: + explicit ReferenceFilterNode( + const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); + + ~ReferenceFilterNode(); + + private: + // @brief Set the subscribers and publishers + void set_subscribers_and_publisher(); + + // @brief Set the action server + void set_action_server(); + + // @brief Initializes the reference filter with ROS parameters. + void set_refererence_filter(); + + /// @brief Accept all incoming goals unconditionally. + rclcpp_action::GoalResponse handle_goal( + const rclcpp_action::GoalUUID& uuid, + std::shared_ptr< + const vortex_msgs::action::ReferenceFilterWaypoint::Goal> goal); + + /// @brief Accept all cancel requests. + rclcpp_action::CancelResponse handle_cancel( + const std::shared_ptr> goal_handle); + + /// @brief Join the old execution thread and spawn a new one for the goal. + void handle_accepted( + const std::shared_ptr> goal_handle); + + /** + * @brief Execute the action goal in a loop until convergence or + * preemption. + * @param goal_handle The goal handle. + */ + void execute( + const std::shared_ptr> goal_handle); + + rclcpp_action::Server< + vortex_msgs::action::ReferenceFilterWaypoint>::SharedPtr action_server_; + + ReferenceFilterParams filter_params_; + + std::unique_ptr follower_; + + rclcpp::Publisher::SharedPtr + reference_pub_; + + rclcpp::Subscription::SharedPtr + reference_sub_; + + rclcpp::Subscription< + geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_; + + rclcpp::Subscription< + geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr twist_sub_; + + rclcpp::TimerBase::SharedPtr reference_pub_timer_; + + std::chrono::milliseconds time_step_{}; + + vortex::utils::types::PoseEuler current_pose_; + + vortex::utils::types::Twist current_twist_; + + std::mutex sensor_mutex_; + + std::atomic preempted_{false}; + std::mutex execute_mutex_; + std::thread execute_thread_; +}; + +} // namespace vortex::guidance + +#endif // REFERENCE_FILTER_DP_QUAT__ROS__REFERENCE_FILTER_ROS_HPP_ diff --git a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros_utils.hpp b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros_utils.hpp new file mode 100644 index 000000000..99c583455 --- /dev/null +++ b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros_utils.hpp @@ -0,0 +1,65 @@ +#ifndef REFERENCE_FILTER_DP_QUAT__ROS__REFERENCE_FILTER_ROS_UTILS_HPP_ +#define REFERENCE_FILTER_DP_QUAT__ROS__REFERENCE_FILTER_ROS_UTILS_HPP_ + +#include +#include +#include +#include +#include +#include +#include "reference_filter_dp_quat/lib/eigen_typedefs.hpp" +#include "reference_filter_dp_quat/lib/waypoint_types.hpp" + +namespace vortex::guidance { + +/// @brief Convert a ROS waypoint mode to a WaypointMode enum. +/// @throws std::invalid_argument if the mode value is not recognized. +inline WaypointMode waypoint_mode_from_ros(uint8_t mode) { + switch (mode) { + case vortex_msgs::msg::Waypoint::FULL_POSE: + return WaypointMode::FULL_POSE; + case vortex_msgs::msg::Waypoint::ONLY_POSITION: + return WaypointMode::ONLY_POSITION; + case vortex_msgs::msg::Waypoint::FORWARD_HEADING: + return WaypointMode::FORWARD_HEADING; + case vortex_msgs::msg::Waypoint::ONLY_ORIENTATION: + return WaypointMode::ONLY_ORIENTATION; + default: + throw std::invalid_argument("Invalid ROS waypoint mode: " + + std::to_string(mode)); + } +} + +/// @brief Convert a ROS Waypoint message to an internal Waypoint struct. +inline vortex::guidance::Waypoint waypoint_from_ros( + const vortex_msgs::msg::Waypoint& ros_wp) { + Waypoint wp; + wp.pose = + vortex::utils::ros_conversions::ros_pose_to_pose_euler(ros_wp.pose); + wp.mode = waypoint_mode_from_ros(ros_wp.mode); + return wp; +} + +/// @brief Fill a ReferenceFilter message from an 18D state vector. +inline vortex_msgs::msg::ReferenceFilter fill_reference_msg( + const Eigen::Vector18d& x) { + using vortex::utils::math::ssa; + vortex_msgs::msg::ReferenceFilter msg; + msg.x = x(0); + msg.y = x(1); + msg.z = x(2); + msg.roll = ssa(x(3)); + msg.pitch = ssa(x(4)); + msg.yaw = ssa(x(5)); + msg.x_dot = x(6); + msg.y_dot = x(7); + msg.z_dot = x(8); + msg.roll_dot = x(9); + msg.pitch_dot = x(10); + msg.yaw_dot = x(11); + return msg; +} + +} // namespace vortex::guidance + +#endif // REFERENCE_FILTER_DP_QUAT__ROS__REFERENCE_FILTER_ROS_UTILS_HPP_ diff --git a/guidance/reference_filter_dp_quat/launch/reference_filter_dp_quat.launch.py b/guidance/reference_filter_dp_quat/launch/reference_filter_dp_quat.launch.py new file mode 100644 index 000000000..8a8d4d310 --- /dev/null +++ b/guidance/reference_filter_dp_quat/launch/reference_filter_dp_quat.launch.py @@ -0,0 +1,45 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import OpaqueFunction +from launch_ros.actions import Node + +from auv_setup.launch_arg_common import ( + declare_drone_and_namespace_args, + resolve_drone_and_namespace, +) + + +def launch_setup(context, *args, **kwargs): + drone, namespace = resolve_drone_and_namespace(context) + + config_file_path = os.path.join( + get_package_share_directory("reference_filter_dp_quat"), + "config", + "reference_filter_params.yaml", + ) + + drone_params = os.path.join( + get_package_share_directory("auv_setup"), + "config", + "robots", + f"{drone}.yaml", + ) + + return [ + Node( + package="reference_filter_dp_quat", + executable="reference_filter_dp_quat_node", + name="reference_filter_node", + namespace=namespace, + parameters=[config_file_path, drone_params], + output="screen", + ) + ] + + +def generate_launch_description(): + return LaunchDescription( + declare_drone_and_namespace_args() + [OpaqueFunction(function=launch_setup)] + ) diff --git a/guidance/reference_filter_dp_quat/package.xml b/guidance/reference_filter_dp_quat/package.xml new file mode 100644 index 000000000..ce1ad0087 --- /dev/null +++ b/guidance/reference_filter_dp_quat/package.xml @@ -0,0 +1,24 @@ + + + + reference_filter_dp_quat + 0.0.0 + Reference model for the DP controller + andeshog + MIT + + ament_cmake + + rclcpp + rclcpp_action + geometry_msgs + vortex_msgs + vortex_utils + vortex_utils_ros + nav_msgs + eigen + + + ament_cmake + + diff --git a/guidance/reference_filter_dp_quat/src/lib/reference_filter.cpp b/guidance/reference_filter_dp_quat/src/lib/reference_filter.cpp new file mode 100644 index 000000000..c7d1f0cb5 --- /dev/null +++ b/guidance/reference_filter_dp_quat/src/lib/reference_filter.cpp @@ -0,0 +1,37 @@ +#include "reference_filter_dp_quat/lib/reference_filter.hpp" + +namespace vortex::guidance { + +ReferenceFilter::ReferenceFilter(const ReferenceFilterParams& params) { + calculate_Ad(params.omega, params.zeta); + calculate_Bd(params.omega); +} + +Eigen::Vector18d ReferenceFilter::calculate_x_dot(const Eigen::Vector18d& x, + const Eigen::Vector6d& r) { + Eigen::Vector18d x_dot = Ad_ * x + Bd_ * r; + + return x_dot; +} + +void ReferenceFilter::calculate_Ad(const Eigen::Vector6d& omega, + const Eigen::Vector6d& zeta) { + Eigen::Matrix6d omega_diag = omega.asDiagonal(); + Eigen::Matrix6d delta = zeta.asDiagonal(); + Eigen::Matrix6d omega_diag_squared = omega_diag * omega_diag; + Eigen::Matrix6d omega_diag_cubed = omega_diag * omega_diag * omega_diag; + Ad_.block<6, 6>(0, 6) = Eigen::Matrix6d::Identity(); + Ad_.block<6, 6>(12, 0) = -omega_diag_cubed; + Ad_.block<6, 6>(12, 6) = + -(2 * delta + Eigen::Matrix6d::Identity()) * omega_diag_squared; + Ad_.block<6, 6>(12, 12) = + -(2 * delta + Eigen::Matrix6d::Identity()) * omega_diag; + Ad_.block<6, 6>(6, 12) = Eigen::Matrix6d::Identity(); +} + +void ReferenceFilter::calculate_Bd(const Eigen::Vector6d& omega) { + Eigen::Matrix6d omega_diag = omega.asDiagonal(); + Bd_.block<6, 6>(12, 0) = omega_diag * omega_diag * omega_diag; +} + +} // namespace vortex::guidance diff --git a/guidance/reference_filter_dp_quat/src/lib/waypoint_follower.cpp b/guidance/reference_filter_dp_quat/src/lib/waypoint_follower.cpp new file mode 100644 index 000000000..074d5f722 --- /dev/null +++ b/guidance/reference_filter_dp_quat/src/lib/waypoint_follower.cpp @@ -0,0 +1,73 @@ +#include "reference_filter_dp_quat/lib/waypoint_follower.hpp" +#include +#include "reference_filter_dp_quat/lib/eigen_typedefs.hpp" +#include "reference_filter_dp_quat/lib/waypoint_utils.hpp" + +namespace vortex::guidance { + +WaypointFollower::WaypointFollower(const ReferenceFilterParams& params, + double dt_seconds) + : filter_(params), dt_seconds_(dt_seconds) {} + +void WaypointFollower::start(const PoseEuler& pose, + const Twist& twist, + const Waypoint& waypoint, + double convergence_threshold) { + std::lock_guard lock(mutex_); + state_ = compute_initial_state(pose, twist); + waypoint_mode_ = waypoint.mode; + convergence_threshold_ = convergence_threshold; + reference_goal_ = apply_mode_logic(waypoint.pose.to_vector(), + waypoint_mode_, state_.head<6>()); +} + +Eigen::Vector18d WaypointFollower::compute_initial_state(const PoseEuler& pose, + const Twist& twist) { + Eigen::Vector18d x = Eigen::Vector18d::Zero(); + + x.head<6>() = pose.to_vector(); + + Eigen::Matrix J = pose.as_j_matrix(); + x.segment<6>(6) = J * twist.to_vector(); + + return x; +} + +Eigen::Vector18d WaypointFollower::step() { + std::lock_guard lock(mutex_); + Eigen::Vector18d state_dot_ = + filter_.calculate_x_dot(state_, reference_goal_); + state_ += state_dot_ * dt_seconds_; + + return state_; +} + +bool WaypointFollower::within_convergance( + const Eigen::Vector6d& measured_pose) const { + std::lock_guard lock(mutex_); + return has_converged(measured_pose, reference_goal_, waypoint_mode_, + convergence_threshold_); +} + +void WaypointFollower::set_reference(const PoseEuler& reference_goal_pose) { + std::lock_guard lock(mutex_); + reference_goal_ = apply_mode_logic(reference_goal_pose.to_vector(), + waypoint_mode_, state_.head<6>()); +} + +Eigen::Vector18d WaypointFollower::state() const { + std::lock_guard lock(mutex_); + return state_; +} + +Eigen::Vector6d WaypointFollower::reference() const { + std::lock_guard lock(mutex_); + return reference_goal_; +} + +void WaypointFollower::snap_state_to_reference() { + std::lock_guard lock(mutex_); + state_.head<6>() = reference_goal_; +} + +} // namespace vortex::guidance diff --git a/guidance/reference_filter_dp_quat/src/lib/waypoint_utils.cpp b/guidance/reference_filter_dp_quat/src/lib/waypoint_utils.cpp new file mode 100644 index 000000000..9a03249f4 --- /dev/null +++ b/guidance/reference_filter_dp_quat/src/lib/waypoint_utils.cpp @@ -0,0 +1,72 @@ +#include "reference_filter_dp_quat/lib/waypoint_utils.hpp" +#include +#include + +namespace vortex::guidance { + +Eigen::Vector6d apply_mode_logic(const Eigen::Vector6d& reference_in, + WaypointMode mode, + const Eigen::Vector6d& current_state) { + Eigen::Vector6d reference_out = reference_in; + + switch (mode) { + case WaypointMode::FULL_POSE: + break; + + case WaypointMode::ONLY_POSITION: + reference_out(3) = current_state(3); + reference_out(4) = current_state(4); + reference_out(5) = current_state(5); + break; + + case WaypointMode::FORWARD_HEADING: { + double dx = reference_in(0) - current_state(0); + double dy = reference_in(1) - current_state(1); + double forward_heading = std::atan2(dy, dx); + + reference_out(3) = 0.0; + reference_out(4) = 0.0; + reference_out(5) = vortex::utils::math::ssa(forward_heading); + break; + } + + case WaypointMode::ONLY_ORIENTATION: + reference_out(0) = current_state(0); + reference_out(1) = current_state(1); + reference_out(2) = current_state(2); + break; + } + + return reference_out; +} + +bool has_converged(const Eigen::Vector6d& measured_pose, + const Eigen::Vector6d& reference, + WaypointMode mode, + double convergence_threshold) { + using vortex::utils::math::ssa; + const Eigen::Vector3d ep = measured_pose.head<3>() - reference.head<3>(); + + Eigen::Vector3d ea; + ea(0) = ssa(measured_pose(3) - reference(3)); + ea(1) = ssa(measured_pose(4) - reference(4)); + ea(2) = ssa(measured_pose(5) - reference(5)); + + const double err = [&] { + switch (mode) { + case WaypointMode::ONLY_POSITION: + return ep.norm(); + case WaypointMode::ONLY_ORIENTATION: + return ea.norm(); + case WaypointMode::FORWARD_HEADING: + return std::sqrt(ep.squaredNorm() + ea(2) * ea(2)); + case WaypointMode::FULL_POSE: + default: + return std::sqrt(ep.squaredNorm() + ea.squaredNorm()); + } + }(); + + return err < convergence_threshold; +} + +} // namespace vortex::guidance diff --git a/guidance/reference_filter_dp_quat/src/ros/reference_filter_ros.cpp b/guidance/reference_filter_dp_quat/src/ros/reference_filter_ros.cpp new file mode 100644 index 000000000..b8e686f5c --- /dev/null +++ b/guidance/reference_filter_dp_quat/src/ros/reference_filter_ros.cpp @@ -0,0 +1,243 @@ +#include "reference_filter_dp_quat/ros/reference_filter_ros.hpp" +#include +#include +#include +#include +#include +#include +#include "reference_filter_dp_quat/ros/reference_filter_ros_utils.hpp" + +const auto start_message = R"( + ____ __ _____ _ _ _ + | _ \ ___ / _| ___ _ __ ___ _ __ ___ ___ | ___(_) | |_ ___ _ __ + | |_) / _ \ |_ / _ \ '__/ _ \ '_ \ / __/ _ \ | |_ | | | __/ _ \ '__| + | _ < __/ _| __/ | | __/ | | | (_| __/ | _| | | | || __/ | + |_| \_\___|_| \___|_| \___|_| |_|\___\___| |_| |_|_|\__\___|_| + + )"; + +namespace vortex::guidance { + +ReferenceFilterNode::ReferenceFilterNode(const rclcpp::NodeOptions& options) + : Node("reference_filter_node", options) { + int time_step_ms = this->declare_parameter("time_step_ms"); + time_step_ = std::chrono::milliseconds(time_step_ms); + + set_subscribers_and_publisher(); + + set_action_server(); + + set_refererence_filter(); + + spdlog::info(start_message); +} + +ReferenceFilterNode::~ReferenceFilterNode() { + preempted_ = true; + if (execute_thread_.joinable()) { + execute_thread_.join(); + } +} + +void ReferenceFilterNode::set_subscribers_and_publisher() { + this->declare_parameter("topics.pose"); + this->declare_parameter("topics.twist"); + this->declare_parameter("topics.guidance.dp"); + this->declare_parameter("topics.reference_pose"); + + std::string pose_topic = this->get_parameter("topics.pose").as_string(); + std::string twist_topic = this->get_parameter("topics.twist").as_string(); + std::string guidance_topic = + this->get_parameter("topics.guidance.dp").as_string(); + std::string reference_pose_topic = + this->get_parameter("topics.reference_pose").as_string(); + + auto qos_sensor_data = vortex::utils::qos_profiles::sensor_data_profile(1); + reference_pub_ = this->create_publisher( + guidance_topic, qos_sensor_data); + + reference_sub_ = this->create_subscription( + reference_pose_topic, qos_sensor_data, + [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg) { + follower_->set_reference( + vortex::utils::ros_conversions::ros_pose_to_pose_euler( + msg->pose)); + }); + + pose_sub_ = this->create_subscription< + geometry_msgs::msg::PoseWithCovarianceStamped>( + pose_topic, qos_sensor_data, + [this](const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr + msg) { + std::lock_guard lock(sensor_mutex_); + current_pose_ = + vortex::utils::ros_conversions::ros_pose_to_pose_euler( + msg->pose.pose); + }); + + twist_sub_ = this->create_subscription< + geometry_msgs::msg::TwistWithCovarianceStamped>( + twist_topic, qos_sensor_data, + [this](const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr + msg) { + std::lock_guard lock(sensor_mutex_); + current_twist_ = vortex::utils::ros_conversions::ros_twist_to_twist( + msg->twist.twist); + }); +} + +void ReferenceFilterNode::set_action_server() { + this->declare_parameter("action_servers.reference_filter"); + std::string action_server_name = + this->get_parameter("action_servers.reference_filter").as_string(); + + action_server_ = rclcpp_action::create_server< + vortex_msgs::action::ReferenceFilterWaypoint>( + this, action_server_name, + [this](const auto& uuid, auto goal) { + return handle_goal(uuid, std::move(goal)); + }, + [this](auto goal_handle) { return handle_cancel(goal_handle); }, + [this](auto goal_handle) { handle_accepted(goal_handle); }); +} + +void ReferenceFilterNode::set_refererence_filter() { + this->declare_parameter>("zeta"); + this->declare_parameter>("omega"); + + std::vector zeta = this->get_parameter("zeta").as_double_array(); + std::vector omega = this->get_parameter("omega").as_double_array(); + + Eigen::Vector6d zeta_eigen = Eigen::Map(zeta.data()); + Eigen::Vector6d omega_eigen = Eigen::Map(omega.data()); + + filter_params_ = ReferenceFilterParams{omega_eigen, zeta_eigen}; + + double dt_seconds = time_step_.count() / 1000.0; + follower_ = std::make_unique(filter_params_, dt_seconds); +} + +rclcpp_action::GoalResponse ReferenceFilterNode::handle_goal( + const rclcpp_action::GoalUUID& /*uuid*/, + std::shared_ptr + /*goal*/) { + spdlog::info("Accepted goal request"); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse ReferenceFilterNode::handle_cancel( + const std::shared_ptr> /*goal_handle*/) { + spdlog::info("Received request to cancel goal"); + return rclcpp_action::CancelResponse::ACCEPT; +} + +void ReferenceFilterNode::handle_accepted( + const std::shared_ptr> goal_handle) { + std::lock_guard lock(execute_mutex_); + preempted_ = true; + if (execute_thread_.joinable()) { + execute_thread_.join(); + } + preempted_ = false; + + execute_thread_ = + std::thread([this, goal_handle]() { execute(goal_handle); }); +} + +void ReferenceFilterNode::execute( + const std::shared_ptr> goal_handle) { + spdlog::info("Executing goal"); + + double convergence_threshold = + goal_handle->get_goal()->convergence_threshold; + + if (convergence_threshold <= 0.0) { + convergence_threshold = 0.1; + spdlog::warn( + "ReferenceFilter: Invalid convergence_threshold received (<= 0). " + "Using default 0.1"); + } + + const auto wp = waypoint_from_ros(goal_handle->get_goal()->waypoint); + + const auto [pose, twist] = [this] { + std::lock_guard lock(sensor_mutex_); + return std::pair{current_pose_, current_twist_}; + }(); + + follower_->start(pose, twist, wp, convergence_threshold); + + auto feedback = std::make_shared< + vortex_msgs::action::ReferenceFilterWaypoint::Feedback>(); + auto result = std::make_shared< + vortex_msgs::action::ReferenceFilterWaypoint::Result>(); + + rclcpp::Rate loop_rate(1000.0 / time_step_.count()); + + while (rclcpp::ok()) { + if (preempted_.load()) { + result->success = false; + goal_handle->abort(result); + spdlog::info("Goal preempted by newer goal"); + return; + } + + if (goal_handle->is_canceling()) { + result->success = false; + goal_handle->canceled(result); + spdlog::info("Goal canceled"); + return; + } + + Eigen::Vector18d filter_state = follower_->step(); + + const auto current_pose_vector = [this] { + std::lock_guard lock(sensor_mutex_); + return current_pose_.to_vector(); + }(); + + bool target_reached = + follower_->within_convergance(current_pose_vector); + + if (target_reached) { + follower_->snap_state_to_reference(); + + vortex_msgs::msg::ReferenceFilter final_reference_msg = + fill_reference_msg(follower_->state()); + + feedback->reference = final_reference_msg; + goal_handle->publish_feedback(feedback); + reference_pub_->publish(final_reference_msg); + + result->success = true; + goal_handle->succeed(result); + spdlog::info("Goal reached"); + return; + } + + vortex_msgs::msg::ReferenceFilter reference_msg = + fill_reference_msg(filter_state); + reference_pub_->publish(reference_msg); + feedback->reference = reference_msg; + goal_handle->publish_feedback(feedback); + loop_rate.sleep(); + } + if (!rclcpp::ok() && goal_handle->is_active()) { + auto result = std::make_shared< + vortex_msgs::action::ReferenceFilterWaypoint::Result>(); + result->success = false; + + try { + goal_handle->abort(result); + } catch (...) { + // Ignore exceptions during shutdown + } + } +} + +RCLCPP_COMPONENTS_REGISTER_NODE(ReferenceFilterNode) + +} // namespace vortex::guidance diff --git a/guidance/reference_filter_dp_quat/test/CMakeLists.txt b/guidance/reference_filter_dp_quat/test/CMakeLists.txt new file mode 100644 index 000000000..2b8229c0c --- /dev/null +++ b/guidance/reference_filter_dp_quat/test/CMakeLists.txt @@ -0,0 +1,57 @@ +cmake_minimum_required(VERSION 3.8) + +find_package(GTest REQUIRED) +include(GoogleTest) + +set(TEST_BINARY_NAME ${PROJECT_NAME}_test) +add_executable( + ${TEST_BINARY_NAME} + test_reference_filter.cpp +) + +target_link_libraries( + ${TEST_BINARY_NAME} + PRIVATE + ${CORE_LIB_NAME} + GTest::GTest +) + +ament_target_dependencies(${TEST_BINARY_NAME} PUBLIC Eigen3) + +gtest_discover_tests(${TEST_BINARY_NAME}) + +# Waypoint utils tests +set(TEST_WAYPOINT_UTILS ${PROJECT_NAME}_test_waypoint_utils) +add_executable( + ${TEST_WAYPOINT_UTILS} + test_waypoint_utils.cpp +) + +target_link_libraries( + ${TEST_WAYPOINT_UTILS} + PRIVATE + ${CORE_LIB_NAME} + GTest::GTest +) + +ament_target_dependencies(${TEST_WAYPOINT_UTILS} PUBLIC Eigen3 vortex_utils) + +gtest_discover_tests(${TEST_WAYPOINT_UTILS}) + +# Waypoint follower tests +set(TEST_WAYPOINT_FOLLOWER ${PROJECT_NAME}_test_waypoint_follower) +add_executable( + ${TEST_WAYPOINT_FOLLOWER} + test_waypoint_follower.cpp +) + +target_link_libraries( + ${TEST_WAYPOINT_FOLLOWER} + PRIVATE + ${CORE_LIB_NAME} + GTest::GTest +) + +ament_target_dependencies(${TEST_WAYPOINT_FOLLOWER} PUBLIC Eigen3 vortex_utils) + +gtest_discover_tests(${TEST_WAYPOINT_FOLLOWER}) diff --git a/guidance/reference_filter_dp_quat/test/test_reference_filter.cpp b/guidance/reference_filter_dp_quat/test/test_reference_filter.cpp new file mode 100644 index 000000000..d5fab4c19 --- /dev/null +++ b/guidance/reference_filter_dp_quat/test/test_reference_filter.cpp @@ -0,0 +1,58 @@ +#include + +#include "reference_filter_dp_quat/lib/eigen_typedefs.hpp" +#include "reference_filter_dp_quat/lib/reference_filter.hpp" + +namespace vortex::guidance { + +class ReferenceFilterTests : public ::testing::Test { + protected: + ReferenceFilterTests() : reference_filter_{get_filter_params()} {} + + ReferenceFilterParams get_filter_params() { + ReferenceFilterParams params; + params.omega = Eigen::Vector6d::Ones(); + params.zeta = Eigen::Vector6d::Ones(); + return params; + } + + ReferenceFilter reference_filter_; +}; + +TEST_F(ReferenceFilterTests, T01_positive_command_positive_xdot) { + Eigen::Vector18d x = Eigen::Vector18d::Zero(); + Eigen::Vector6d r = Eigen::Vector6d::Ones(); + Eigen::Vector18d xdot = reference_filter_.calculate_x_dot(x, r); + + for (int i = 0; i < xdot.size(); ++i) { + EXPECT_GE(xdot(i), 0.0); + } +} + +TEST_F(ReferenceFilterTests, T02_negative_command_negative_xdot) { + Eigen::Vector18d x = Eigen::Vector18d::Zero(); + Eigen::Vector6d r = -Eigen::Vector6d::Ones(); + Eigen::Vector18d xdot = reference_filter_.calculate_x_dot(x, r); + + for (int i = 0; i < xdot.size(); ++i) { + EXPECT_LE(xdot(i), 0.0); + } +} + +TEST_F(ReferenceFilterTests, T03_zero_command_zero_xdot) { + Eigen::Vector18d x = Eigen::Vector18d::Zero(); + Eigen::Vector6d r = Eigen::Vector6d::Zero(); + Eigen::Vector18d xdot = reference_filter_.calculate_x_dot(x, r); + + for (int i = 0; i < xdot.size(); ++i) { + EXPECT_DOUBLE_EQ(xdot(i), 0.0); + } +} + +} // namespace vortex::guidance + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} diff --git a/guidance/reference_filter_dp_quat/test/test_waypoint_follower.cpp b/guidance/reference_filter_dp_quat/test/test_waypoint_follower.cpp new file mode 100644 index 000000000..e99dcf7e7 --- /dev/null +++ b/guidance/reference_filter_dp_quat/test/test_waypoint_follower.cpp @@ -0,0 +1,111 @@ +#include +#include "reference_filter_dp_quat/lib/waypoint_follower.hpp" +#include "reference_filter_dp_quat/lib/waypoint_utils.hpp" + +namespace vortex::guidance { + +class WaypointFollowerTests : public ::testing::Test { + protected: + ReferenceFilterParams get_params() { + ReferenceFilterParams params; + params.omega = Eigen::Vector6d::Ones(); + params.zeta = Eigen::Vector6d::Ones(); + return params; + } + + PoseEuler zero_pose() { return PoseEuler{}; } + Twist zero_twist() { return Twist{}; } +}; + +TEST_F(WaypointFollowerTests, StartAndStepConverges) { + WaypointFollower follower(get_params(), 0.01); + + Waypoint wp; + wp.pose = PoseEuler{1.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + wp.mode = WaypointMode::FULL_POSE; + + follower.start(zero_pose(), zero_twist(), wp, 0.1); + + Eigen::Vector18d state = follower.step(); + + // Simulate the measured pose being at the reference + Eigen::Vector6d measured_at_ref; + measured_at_ref << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0; + + EXPECT_TRUE(follower.within_convergance(measured_at_ref)); + EXPECT_EQ(state.size(), 18); +} + +TEST_F(WaypointFollowerTests, StepDoesNotConvergeWhenFar) { + WaypointFollower follower(get_params(), 0.01); + + Waypoint wp; + wp.pose = PoseEuler{10.0, 10.0, 0.0, 0.0, 0.0, 0.0}; + wp.mode = WaypointMode::FULL_POSE; + + follower.start(zero_pose(), zero_twist(), wp, 0.1); + + follower.step(); + + Eigen::Vector6d measured_far = Eigen::Vector6d::Zero(); + EXPECT_FALSE(follower.within_convergance(measured_far)); +} + +TEST_F(WaypointFollowerTests, SetReferenceUpdatesMidSequence) { + WaypointFollower follower(get_params(), 0.01); + + Waypoint wp; + wp.pose = PoseEuler{1.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + wp.mode = WaypointMode::FULL_POSE; + + follower.start(zero_pose(), zero_twist(), wp, 0.1); + + PoseEuler new_ref{5.0, 5.0, 0.0, 0.0, 0.0, 0.0}; + follower.set_reference(new_ref); + + EXPECT_DOUBLE_EQ(follower.reference()(0), 5.0); + EXPECT_DOUBLE_EQ(follower.reference()(1), 5.0); +} + +TEST_F(WaypointFollowerTests, SnapStateToReference) { + WaypointFollower follower(get_params(), 0.01); + + Waypoint wp; + wp.pose = PoseEuler{3.0, 4.0, 5.0, 0.1, 0.2, 0.3}; + wp.mode = WaypointMode::FULL_POSE; + + follower.start(zero_pose(), zero_twist(), wp, 0.1); + follower.snap_state_to_reference(); + + Eigen::Vector18d state = follower.state(); + Eigen::Vector6d ref = follower.reference(); + + for (int i = 0; i < 6; ++i) { + EXPECT_DOUBLE_EQ(state(i), ref(i)); + } +} + +TEST_F(WaypointFollowerTests, StateEvolvesWithStep) { + WaypointFollower follower(get_params(), 0.01); + + Waypoint wp; + wp.pose = PoseEuler{1.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + wp.mode = WaypointMode::FULL_POSE; + + follower.start(zero_pose(), zero_twist(), wp, 0.1); + + // Run several steps — state should move toward reference + for (int i = 0; i < 100; ++i) { + follower.step(); + } + + // x position should have moved toward 1.0 + EXPECT_GT(follower.state()(0), 0.0); +} + +} // namespace vortex::guidance + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/guidance/reference_filter_dp_quat/test/test_waypoint_utils.cpp b/guidance/reference_filter_dp_quat/test/test_waypoint_utils.cpp new file mode 100644 index 000000000..ed9e48aa2 --- /dev/null +++ b/guidance/reference_filter_dp_quat/test/test_waypoint_utils.cpp @@ -0,0 +1,129 @@ +#include +#include +#include "reference_filter_dp_quat/lib/waypoint_utils.hpp" + +namespace vortex::guidance { + +// --- apply_mode_logic tests --- + +TEST(ApplyModeLogic, FullPoseReturnsInputUnchanged) { + Eigen::Vector6d r_in; + r_in << 1.0, 2.0, 3.0, 0.1, 0.2, 0.3; + Eigen::Vector6d state = Eigen::Vector6d::Zero(); + + Eigen::Vector6d result = + apply_mode_logic(r_in, WaypointMode::FULL_POSE, state); + + for (int i = 0; i < 6; ++i) { + EXPECT_DOUBLE_EQ(result(i), r_in(i)); + } +} + +TEST(ApplyModeLogic, OnlyPositionKeepsOrientationFromState) { + Eigen::Vector6d r_in; + r_in << 1.0, 2.0, 3.0, 0.1, 0.2, 0.3; + Eigen::Vector6d state; + state << 10.0, 20.0, 30.0, 0.4, 0.5, 0.6; + + Eigen::Vector6d result = + apply_mode_logic(r_in, WaypointMode::ONLY_POSITION, state); + + EXPECT_DOUBLE_EQ(result(0), 1.0); + EXPECT_DOUBLE_EQ(result(1), 2.0); + EXPECT_DOUBLE_EQ(result(2), 3.0); + EXPECT_DOUBLE_EQ(result(3), 0.4); + EXPECT_DOUBLE_EQ(result(4), 0.5); + EXPECT_DOUBLE_EQ(result(5), 0.6); +} + +TEST(ApplyModeLogic, ForwardHeadingComputesYawFromDelta) { + Eigen::Vector6d r_in; + r_in << 1.0, 1.0, 0.0, 0.0, 0.0, 0.0; + Eigen::Vector6d state = Eigen::Vector6d::Zero(); + + Eigen::Vector6d result = + apply_mode_logic(r_in, WaypointMode::FORWARD_HEADING, state); + + double expected_yaw = std::atan2(1.0, 1.0); // pi/4 + EXPECT_DOUBLE_EQ(result(0), 1.0); + EXPECT_DOUBLE_EQ(result(1), 1.0); + EXPECT_DOUBLE_EQ(result(3), 0.0); + EXPECT_DOUBLE_EQ(result(4), 0.0); + EXPECT_NEAR(result(5), expected_yaw, 1e-12); +} + +TEST(ApplyModeLogic, OnlyOrientationKeepsPositionFromState) { + Eigen::Vector6d r_in; + r_in << 1.0, 2.0, 3.0, 0.1, 0.2, 0.3; + Eigen::Vector6d state; + state << 10.0, 20.0, 30.0, 0.4, 0.5, 0.6; + + Eigen::Vector6d result = + apply_mode_logic(r_in, WaypointMode::ONLY_ORIENTATION, state); + + EXPECT_DOUBLE_EQ(result(0), 10.0); + EXPECT_DOUBLE_EQ(result(1), 20.0); + EXPECT_DOUBLE_EQ(result(2), 30.0); + EXPECT_DOUBLE_EQ(result(3), 0.1); + EXPECT_DOUBLE_EQ(result(4), 0.2); + EXPECT_DOUBLE_EQ(result(5), 0.3); +} + +// --- has_converged tests --- + +TEST(HasConverged, FullPoseBelowThreshold) { + Eigen::Vector6d measured; + measured << 1.0, 2.0, 3.0, 0.1, 0.2, 0.3; + Eigen::Vector6d reference = measured; + reference(0) += 0.001; + + EXPECT_TRUE( + has_converged(measured, reference, WaypointMode::FULL_POSE, 0.1)); +} + +TEST(HasConverged, FullPoseAboveThreshold) { + Eigen::Vector6d measured = Eigen::Vector6d::Zero(); + Eigen::Vector6d reference; + reference << 1.0, 1.0, 1.0, 0.0, 0.0, 0.0; + + EXPECT_FALSE( + has_converged(measured, reference, WaypointMode::FULL_POSE, 0.1)); +} + +TEST(HasConverged, OnlyPositionIgnoresOrientation) { + Eigen::Vector6d measured; + measured << 1.0, 2.0, 3.0, 0.0, 0.0, 0.0; + Eigen::Vector6d reference; + reference << 1.0, 2.0, 3.0, 1.0, 1.0, 1.0; + + EXPECT_TRUE( + has_converged(measured, reference, WaypointMode::ONLY_POSITION, 0.1)); +} + +TEST(HasConverged, OnlyOrientationIgnoresPosition) { + Eigen::Vector6d measured; + measured << 100.0, 200.0, 300.0, 0.1, 0.2, 0.3; + Eigen::Vector6d reference; + reference << 0.0, 0.0, 0.0, 0.1, 0.2, 0.3; + + EXPECT_TRUE(has_converged(measured, reference, + WaypointMode::ONLY_ORIENTATION, 0.1)); +} + +TEST(HasConverged, ForwardHeadingUsesPositionAndYawOnly) { + Eigen::Vector6d measured; + measured << 1.0, 2.0, 3.0, 0.5, 0.5, 0.1; + Eigen::Vector6d reference; + reference << 1.0, 2.0, 3.0, 0.0, 0.0, 0.1; + + // Roll and pitch differ but should be ignored + EXPECT_TRUE( + has_converged(measured, reference, WaypointMode::FORWARD_HEADING, 0.1)); +} + +} // namespace vortex::guidance + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From 03289be0b2f017d4c449805e9e0dcb0763723b42 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Sat, 21 Mar 2026 18:25:37 +0100 Subject: [PATCH 169/290] update waypoint utils to quat --- .../lib/waypoint_types.hpp | 4 +- .../lib/waypoint_utils.hpp | 30 +++++++------- .../src/lib/waypoint_utils.cpp | 40 ++++++++----------- 3 files changed, 34 insertions(+), 40 deletions(-) diff --git a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_types.hpp b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_types.hpp index 9d379d5e3..15445c62c 100644 --- a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_types.hpp +++ b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_types.hpp @@ -6,7 +6,7 @@ namespace vortex::guidance { -using vortex::utils::types::PoseEuler; +using vortex::utils::types::Pose; /** * @brief Determines which degrees of freedom the reference filter controls. @@ -25,7 +25,7 @@ enum class WaypointMode : uint8_t { * @brief A target pose with an associated waypoint mode. */ struct Waypoint { - PoseEuler pose{}; + Pose pose{}; WaypointMode mode = WaypointMode::FULL_POSE; }; diff --git a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_utils.hpp b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_utils.hpp index 0eb309213..cc711233c 100644 --- a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_utils.hpp +++ b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_utils.hpp @@ -1,39 +1,41 @@ #ifndef REFERENCE_FILTER_DP_QUAT__LIB__WAYPOINT_UTILS_HPP_ #define REFERENCE_FILTER_DP_QUAT__LIB__WAYPOINT_UTILS_HPP_ -#include "reference_filter_dp_quat/lib/eigen_typedefs.hpp" #include "reference_filter_dp_quat/lib/waypoint_types.hpp" namespace vortex::guidance { +using vortex::utils::types::Pose; + /** - * @brief Apply waypoint mode logic to a reference pose. + * @brief Compute the waypoint goal by applying the mode logic to the incoming + * waypoint. * * For modes that don't control all DOFs, the uncontrolled components are - * replaced with values from @p current_state so the filter holds them steady. + * replaced with values from @p current_state. * - * @param r_in The raw reference pose (6D). + * @param incoming_waypoint The incoming waypoint to compute goal from. * @param mode The waypoint mode. - * @param current_state The current filter state pose (6D). - * @return The adjusted reference pose. + * @param current_state The current state pose. + * @return The adjusted waypoint goal. */ -Eigen::Vector6d apply_mode_logic(const Eigen::Vector6d& r_in, - WaypointMode mode, - const Eigen::Vector6d& current_state); +Pose compute_waypoint_goal(const Pose& incoming_waypoint, + WaypointMode mode, + const Pose& current_state); /** - * @brief Check whether the measured pose has converged to the reference. + * @brief Check whether the state has converged to the waypoint goal. * * Only the DOFs relevant to the waypoint mode are included in the error norm. * - * @param measured_pose The current measured pose (6D). - * @param reference The reference goal pose (6D). + * @param state The current state pose. + * @param waypoint_goal The waypoint goal pose. * @param mode The waypoint mode. * @param convergence_threshold The maximum allowed error norm. * @return True if the error is below the threshold. */ -bool has_converged(const Eigen::Vector6d& measured_pose, - const Eigen::Vector6d& reference, +bool has_converged(const Pose& state, + const Pose& waypoint_goal, WaypointMode mode, double convergence_threshold); diff --git a/guidance/reference_filter_dp_quat/src/lib/waypoint_utils.cpp b/guidance/reference_filter_dp_quat/src/lib/waypoint_utils.cpp index 9a03249f4..32f1208d4 100644 --- a/guidance/reference_filter_dp_quat/src/lib/waypoint_utils.cpp +++ b/guidance/reference_filter_dp_quat/src/lib/waypoint_utils.cpp @@ -4,53 +4,45 @@ namespace vortex::guidance { -Eigen::Vector6d apply_mode_logic(const Eigen::Vector6d& reference_in, - WaypointMode mode, - const Eigen::Vector6d& current_state) { - Eigen::Vector6d reference_out = reference_in; +Pose compute_waypoint_goal(const Pose& incoming_waypoint, + WaypointMode mode, + const Pose& current_state) { + Pose waypoint_out = incoming_waypoint; switch (mode) { case WaypointMode::FULL_POSE: break; case WaypointMode::ONLY_POSITION: - reference_out(3) = current_state(3); - reference_out(4) = current_state(4); - reference_out(5) = current_state(5); + waypoint_out.set_ori(current_state.ori_quaternion()); break; case WaypointMode::FORWARD_HEADING: { - double dx = reference_in(0) - current_state(0); - double dy = reference_in(1) - current_state(1); + double dx = incoming_waypoint.x - current_state.x; + double dy = incoming_waypoint.y - current_state.y; double forward_heading = std::atan2(dy, dx); - reference_out(3) = 0.0; - reference_out(4) = 0.0; - reference_out(5) = vortex::utils::math::ssa(forward_heading); + waypoint_out.set_ori(Eigen::Quaterniond( + Eigen::AngleAxisd(forward_heading, Eigen::Vector3d::UnitZ()))); break; } case WaypointMode::ONLY_ORIENTATION: - reference_out(0) = current_state(0); - reference_out(1) = current_state(1); - reference_out(2) = current_state(2); + waypoint_out.set_pos(current_state.pos_vector()); break; } - return reference_out; + return waypoint_out; } -bool has_converged(const Eigen::Vector6d& measured_pose, - const Eigen::Vector6d& reference, +bool has_converged(const Pose& state, + const Pose& waypoint_goal, WaypointMode mode, double convergence_threshold) { - using vortex::utils::math::ssa; - const Eigen::Vector3d ep = measured_pose.head<3>() - reference.head<3>(); + const Eigen::Vector3d ep = state.pos_vector() - waypoint_goal.pos_vector(); - Eigen::Vector3d ea; - ea(0) = ssa(measured_pose(3) - reference(3)); - ea(1) = ssa(measured_pose(4) - reference(4)); - ea(2) = ssa(measured_pose(5) - reference(5)); + const Eigen::Vector3d ea = vortex::utils::math::quaternion_error( + state.ori_quaternion(), waypoint_goal.ori_quaternion()); const double err = [&] { switch (mode) { From acb05b4b7c6723d80f431553574087c97dad353d Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Sat, 21 Mar 2026 22:28:07 +0100 Subject: [PATCH 170/290] refernce filter quat implementation --- guidance/reference_filter_dp_quat/README.md | 106 ++++++++++++ .../lib/waypoint_follower.hpp | 43 +++-- .../ros/reference_filter_ros.hpp | 22 ++- .../ros/reference_filter_ros_utils.hpp | 40 ++--- .../src/lib/waypoint_follower.cpp | 75 ++++---- .../src/ros/reference_filter_ros.cpp | 47 +++-- .../test/test_waypoint_follower.cpp | 43 ++--- .../test/test_waypoint_utils.cpp | 160 ++++++++++-------- 8 files changed, 338 insertions(+), 198 deletions(-) create mode 100644 guidance/reference_filter_dp_quat/README.md diff --git a/guidance/reference_filter_dp_quat/README.md b/guidance/reference_filter_dp_quat/README.md new file mode 100644 index 000000000..ada98da02 --- /dev/null +++ b/guidance/reference_filter_dp_quat/README.md @@ -0,0 +1,106 @@ +## Reference filter (quaternion) + +### Third-order reference filter + +The underlying filter is the third-order model (Fossen, 2021): +```math +\dot{x}_d = A_d x_d + B_d r +``` +where +```math +A_d = \begin{bmatrix} +0_{n\times n} & I_n & 0_{n\times n} \\ +0_{n\times n} & 0_{n \times n} & I_n \\ +-\Omega^3 & -(2 \Delta + I_n) \Omega^2 & -(2 \Delta + I_n) \Omega +\end{bmatrix} +``` +and +```math +B_d = \begin{bmatrix} +0_{n \times n} \\ +0_{n \times n} \\ +\Omega^3 +\end{bmatrix}. +``` + +The state is integrated using forward Euler: $x_{i+1} = x_i + \dot{x}_i \cdot dt$. + +### Error-state formulation + +This package avoids the singularity issues of Euler angles by using a quaternion-based error state. + +**Nominal pose** — a `Pose` (position + quaternion) representing the current best estimate of the reference trajectory. This is the actual output. + +**Error state** — the 18D filter state, where the first 6 elements are small errors relative to the nominal: +```math +x = \begin{bmatrix} \delta p \\ \delta \phi \\ \dot{\eta} \\ \ddot{\eta} \end{bmatrix} \in \mathbb{R}^{18} +``` + +| Indices | Meaning | +|---|---| +| `x[0:3]` | Position error $\delta p$ from nominal (meters) | +| `x[3:6]` | Orientation error $\delta \phi$ from nominal (rotation vector, radians) | +| `x[6:9]` | World-frame linear velocity (m/s) | +| `x[9:12]` | World-frame angular velocity (rad/s) | +| `x[12:18]` | Accelerations (linear + angular) | + +### Step cycle + +Each time step performs three operations: + +1. **Compute reference error** — The 6D reference $r$ is the error between the waypoint goal and the nominal pose: + - $r_{0:3} = p_{goal} - p_{nominal}$ + - $r_{3:6} = \text{quaternion\_error}(q_{nominal},\ q_{goal})$ + + The `quaternion_error` function returns $2 \cdot \text{vec}(q_{nominal}^{-1} \otimes q_{goal})$, which for small angles approximates the rotation vector from nominal to goal. + +2. **Integrate** — The standard filter step: + ```math + \dot{x} = A_d x + B_d r, \quad x \leftarrow x + \dot{x} \cdot dt + ``` + +3. **Reset** — Absorb position and orientation errors into the nominal pose, then zero them: + - $p_{nominal} \leftarrow p_{nominal} + \delta p, \quad \delta p \leftarrow 0$ + - $q_{nominal} \leftarrow q_{nominal} \otimes \exp(\delta \phi), \quad \delta \phi \leftarrow 0$ + + where $\exp(\delta \phi)$ converts the rotation vector to a quaternion via `AngleAxis`. + +The reset step keeps $\delta p$ and $\delta \phi$ near zero at all times, ensuring the linearized quaternion error remains accurate. The velocity and acceleration states ($x_{6:18}$) are **not** reset — they carry over to provide smooth, continuous motion. + +### Output message (`ReferenceFilterQuat`) + +| Field | Source | Meaning | +|---|---|---| +| `x`, `y`, `z` | Nominal pose position | Desired position in world frame (m) | +| `qw`, `qx`, `qy`, `qz` | Nominal pose quaternion | Desired orientation (unit quaternion) | +| `x_dot`, `y_dot`, `z_dot` | `x[6:9]` | World-frame linear velocity (m/s) | +| `roll_dot`, `pitch_dot`, `yaw_dot` | `x[9:12]` | World-frame angular velocity (rad/s) | + + + + +### Waypoint modes + +Each waypoint has a mode that determines which degrees of freedom are controlled by the reference filter. The mode also determines how convergence is measured. + +| Mode | Controlled DOFs | Convergence metric | +|---|---|---| +| `FULL_POSE` | All 6 DOF (position + orientation) | Position error norm + quaternion error norm | +| `ONLY_POSITION` | x, y, z (orientation holds current value) | Position error norm | +| `FORWARD_HEADING` | x, y, z + yaw toward target | Position error norm + yaw component of quaternion error | +| `ONLY_ORIENTATION` | Orientation only (position holds current value) | Quaternion error norm | + +For all modes, convergence is reached when the error metric drops below the `convergence_threshold` specified in the action goal. + +### Action Server + +The action server handles goal requests and publishes guidance commands. The server always prioritizes new goal requests, aborting ongoing requests when a new one arrives. + +- Action name: `/reference_filter` +- Goal type: Waypoint (pose + mode + convergence threshold) +- Result type: bool +- Guidance topic: `/dp/reference` + +### Overwriting the reference during an action + +While an action is executing, the reference goal can be updated by publishing a `geometry_msgs/msg/PoseStamped` to the reference pose topic. The convergence check will use the updated reference. diff --git a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_follower.hpp b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_follower.hpp index 8cc3dba6c..ddb89362b 100644 --- a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_follower.hpp +++ b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_follower.hpp @@ -9,12 +9,17 @@ namespace vortex::guidance { -using vortex::utils::types::PoseEuler; +using vortex::utils::types::Pose; using vortex::utils::types::Twist; /** * @brief Manages reference filter state and waypoint following logic. * + * Uses an error-state formulation: the 18D state vector holds + * [δp(3), δφ(3), velocity(6), acceleration(6)], while a separate + * nominal Pose tracks the full quaternion orientation. Each step, + * position/orientation errors are absorbed into the nominal and reset. + * * Thread-safe: all public methods acquire a mutex. */ class WaypointFollower { @@ -28,52 +33,56 @@ class WaypointFollower { * @param waypoint Target waypoint with mode. * @param convergence_threshold Max error norm to consider target reached. */ - void start(const PoseEuler& pose, + void start(const Pose& pose, const Twist& twist, const Waypoint& waypoint, double convergence_threshold); /** * @brief Advance the filter by one time step. - * @return The updated filter state. + * + * Computes the error-state reference, integrates, then absorbs + * position/orientation errors into the nominal pose. */ - Eigen::Vector18d step(); + void step(); /** - * @brief Check if the measured pose has converged to the reference goal. + * @brief Check if the measured pose has converged to the waypoint goal. * @param measured_pose Current measured pose. * @return True if the error norm is within the convergence threshold. */ - bool within_convergance(const Eigen::Vector6d& measured_pose) const; + bool within_convergance(const Pose& measured_pose) const; /** * @brief Update the reference goal pose mid-sequence. * @param reference_goal_pose The new reference pose. */ - void set_reference(const PoseEuler& reference_goal_pose); + void set_reference(const Pose& reference_goal_pose); /** - * @brief Snap the position component of the filter state to the reference. + * @brief Snap the nominal pose to the waypoint goal and zero + * errors/velocity. * - * Useful after convergence to eliminate any remaining steady-state offset + * Useful after convergence to eliminate any remaining steady-state offset. */ void snap_state_to_reference(); - /// @brief Get the current 18D filter state. - Eigen::Vector18d state() const; + /// @brief Get the current nominal pose (position + quaternion). + Pose pose() const; - /// @brief Get the current 6D reference goal pose. - Eigen::Vector6d reference() const; + /// @brief Get the current world-frame velocity (linear + angular). + Eigen::Vector6d velocity() const; - private: - Eigen::Vector18d compute_initial_state(const PoseEuler& pose, - const Twist& twist); + /// @brief Get the current waypoint goal. + Pose waypoint_goal() const; + private: mutable std::mutex mutex_; ReferenceFilter filter_; double dt_seconds_{0.01}; + Pose nominal_pose_; Eigen::Vector18d state_ = Eigen::Vector18d::Zero(); - Eigen::Vector6d reference_goal_ = Eigen::Vector6d::Zero(); + Pose waypoint_goal_; WaypointMode waypoint_mode_{WaypointMode::FULL_POSE}; double convergence_threshold_{0.1}; }; diff --git a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros.hpp b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros.hpp index ccf893040..87df8e7e6 100644 --- a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros.hpp +++ b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros.hpp @@ -9,8 +9,8 @@ #include #include #include -#include -#include +#include +#include #include #include "reference_filter_dp_quat/lib/waypoint_follower.hpp" @@ -37,17 +37,17 @@ class ReferenceFilterNode : public rclcpp::Node { rclcpp_action::GoalResponse handle_goal( const rclcpp_action::GoalUUID& uuid, std::shared_ptr< - const vortex_msgs::action::ReferenceFilterWaypoint::Goal> goal); + const vortex_msgs::action::ReferenceFilterQuatWaypoint::Goal> goal); /// @brief Accept all cancel requests. rclcpp_action::CancelResponse handle_cancel( const std::shared_ptr> goal_handle); + vortex_msgs::action::ReferenceFilterQuatWaypoint>> goal_handle); /// @brief Join the old execution thread and spawn a new one for the goal. void handle_accepted( const std::shared_ptr> goal_handle); + vortex_msgs::action::ReferenceFilterQuatWaypoint>> goal_handle); /** * @brief Execute the action goal in a loop until convergence or @@ -56,16 +56,16 @@ class ReferenceFilterNode : public rclcpp::Node { */ void execute( const std::shared_ptr> goal_handle); + vortex_msgs::action::ReferenceFilterQuatWaypoint>> goal_handle); - rclcpp_action::Server< - vortex_msgs::action::ReferenceFilterWaypoint>::SharedPtr action_server_; + rclcpp_action::Server:: + SharedPtr action_server_; ReferenceFilterParams filter_params_; std::unique_ptr follower_; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr reference_pub_; rclcpp::Subscription::SharedPtr @@ -77,11 +77,9 @@ class ReferenceFilterNode : public rclcpp::Node { rclcpp::Subscription< geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr twist_sub_; - rclcpp::TimerBase::SharedPtr reference_pub_timer_; - std::chrono::milliseconds time_step_{}; - vortex::utils::types::PoseEuler current_pose_; + vortex::utils::types::Pose current_pose_; vortex::utils::types::Twist current_twist_; diff --git a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros_utils.hpp b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros_utils.hpp index 99c583455..d04dc02a9 100644 --- a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros_utils.hpp +++ b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros_utils.hpp @@ -5,7 +5,7 @@ #include #include #include -#include +#include #include #include "reference_filter_dp_quat/lib/eigen_typedefs.hpp" #include "reference_filter_dp_quat/lib/waypoint_types.hpp" @@ -34,29 +34,29 @@ inline WaypointMode waypoint_mode_from_ros(uint8_t mode) { inline vortex::guidance::Waypoint waypoint_from_ros( const vortex_msgs::msg::Waypoint& ros_wp) { Waypoint wp; - wp.pose = - vortex::utils::ros_conversions::ros_pose_to_pose_euler(ros_wp.pose); + wp.pose = vortex::utils::ros_conversions::ros_pose_to_pose(ros_wp.pose); wp.mode = waypoint_mode_from_ros(ros_wp.mode); return wp; } -/// @brief Fill a ReferenceFilter message from an 18D state vector. -inline vortex_msgs::msg::ReferenceFilter fill_reference_msg( - const Eigen::Vector18d& x) { - using vortex::utils::math::ssa; - vortex_msgs::msg::ReferenceFilter msg; - msg.x = x(0); - msg.y = x(1); - msg.z = x(2); - msg.roll = ssa(x(3)); - msg.pitch = ssa(x(4)); - msg.yaw = ssa(x(5)); - msg.x_dot = x(6); - msg.y_dot = x(7); - msg.z_dot = x(8); - msg.roll_dot = x(9); - msg.pitch_dot = x(10); - msg.yaw_dot = x(11); +/// @brief Fill a ReferenceFilterQuat message from a Pose and velocity vector. +inline vortex_msgs::msg::ReferenceFilterQuat fill_reference_msg( + const vortex::utils::types::Pose& pose, + const Eigen::Vector6d& velocity) { + vortex_msgs::msg::ReferenceFilterQuat msg; + msg.x = pose.x; + msg.y = pose.y; + msg.z = pose.z; + msg.qw = pose.qw; + msg.qx = pose.qx; + msg.qy = pose.qy; + msg.qz = pose.qz; + msg.x_dot = velocity(0); + msg.y_dot = velocity(1); + msg.z_dot = velocity(2); + msg.roll_dot = velocity(3); + msg.pitch_dot = velocity(4); + msg.yaw_dot = velocity(5); return msg; } diff --git a/guidance/reference_filter_dp_quat/src/lib/waypoint_follower.cpp b/guidance/reference_filter_dp_quat/src/lib/waypoint_follower.cpp index 074d5f722..a44944c02 100644 --- a/guidance/reference_filter_dp_quat/src/lib/waypoint_follower.cpp +++ b/guidance/reference_filter_dp_quat/src/lib/waypoint_follower.cpp @@ -9,65 +9,80 @@ WaypointFollower::WaypointFollower(const ReferenceFilterParams& params, double dt_seconds) : filter_(params), dt_seconds_(dt_seconds) {} -void WaypointFollower::start(const PoseEuler& pose, +void WaypointFollower::start(const Pose& pose, const Twist& twist, const Waypoint& waypoint, double convergence_threshold) { std::lock_guard lock(mutex_); - state_ = compute_initial_state(pose, twist); + + nominal_pose_ = pose; + state_ = Eigen::Vector18d::Zero(); + + Eigen::Matrix3d R = pose.as_rotation_matrix(); + Eigen::Vector6d twist_vec = twist.to_vector(); + state_.segment<3>(6) = R * twist_vec.head<3>(); + state_.segment<3>(9) = R * twist_vec.tail<3>(); + waypoint_mode_ = waypoint.mode; convergence_threshold_ = convergence_threshold; - reference_goal_ = apply_mode_logic(waypoint.pose.to_vector(), - waypoint_mode_, state_.head<6>()); + waypoint_goal_ = + compute_waypoint_goal(waypoint.pose, waypoint_mode_, nominal_pose_); } -Eigen::Vector18d WaypointFollower::compute_initial_state(const PoseEuler& pose, - const Twist& twist) { - Eigen::Vector18d x = Eigen::Vector18d::Zero(); +void WaypointFollower::step() { + std::lock_guard lock(mutex_); + + Eigen::Vector6d r; + r.head<3>() = waypoint_goal_.pos_vector() - nominal_pose_.pos_vector(); + r.tail<3>() = vortex::utils::math::quaternion_error( + nominal_pose_.ori_quaternion(), waypoint_goal_.ori_quaternion()); - x.head<6>() = pose.to_vector(); + Eigen::Vector18d x_dot = filter_.calculate_x_dot(state_, r); + state_ += x_dot * dt_seconds_; - Eigen::Matrix J = pose.as_j_matrix(); - x.segment<6>(6) = J * twist.to_vector(); + nominal_pose_.set_pos(nominal_pose_.pos_vector() + state_.head<3>()); + state_.head<3>().setZero(); - return x; + Eigen::Vector3d dphi = state_.segment<3>(3); + double angle = dphi.norm(); + if (angle >= 1e-10) { + Eigen::Quaterniond dq(Eigen::AngleAxisd(angle, dphi.normalized())); + nominal_pose_.set_ori(nominal_pose_.ori_quaternion() * dq); + state_.segment<3>(3).setZero(); + } } -Eigen::Vector18d WaypointFollower::step() { +bool WaypointFollower::within_convergance(const Pose& measured_pose) const { std::lock_guard lock(mutex_); - Eigen::Vector18d state_dot_ = - filter_.calculate_x_dot(state_, reference_goal_); - state_ += state_dot_ * dt_seconds_; - - return state_; + return has_converged(measured_pose, waypoint_goal_, waypoint_mode_, + convergence_threshold_); } -bool WaypointFollower::within_convergance( - const Eigen::Vector6d& measured_pose) const { +void WaypointFollower::set_reference(const Pose& reference_goal_pose) { std::lock_guard lock(mutex_); - return has_converged(measured_pose, reference_goal_, waypoint_mode_, - convergence_threshold_); + waypoint_goal_ = compute_waypoint_goal(reference_goal_pose, waypoint_mode_, + nominal_pose_); } -void WaypointFollower::set_reference(const PoseEuler& reference_goal_pose) { +void WaypointFollower::snap_state_to_reference() { std::lock_guard lock(mutex_); - reference_goal_ = apply_mode_logic(reference_goal_pose.to_vector(), - waypoint_mode_, state_.head<6>()); + nominal_pose_ = waypoint_goal_; + state_.head<12>().setZero(); } -Eigen::Vector18d WaypointFollower::state() const { +Pose WaypointFollower::pose() const { std::lock_guard lock(mutex_); - return state_; + return nominal_pose_; } -Eigen::Vector6d WaypointFollower::reference() const { +Eigen::Vector6d WaypointFollower::velocity() const { std::lock_guard lock(mutex_); - return reference_goal_; + return state_.segment<6>(6); } -void WaypointFollower::snap_state_to_reference() { +Pose WaypointFollower::waypoint_goal() const { std::lock_guard lock(mutex_); - state_.head<6>() = reference_goal_; + return waypoint_goal_; } } // namespace vortex::guidance diff --git a/guidance/reference_filter_dp_quat/src/ros/reference_filter_ros.cpp b/guidance/reference_filter_dp_quat/src/ros/reference_filter_ros.cpp index b8e686f5c..cfea83085 100644 --- a/guidance/reference_filter_dp_quat/src/ros/reference_filter_ros.cpp +++ b/guidance/reference_filter_dp_quat/src/ros/reference_filter_ros.cpp @@ -53,15 +53,15 @@ void ReferenceFilterNode::set_subscribers_and_publisher() { this->get_parameter("topics.reference_pose").as_string(); auto qos_sensor_data = vortex::utils::qos_profiles::sensor_data_profile(1); - reference_pub_ = this->create_publisher( - guidance_topic, qos_sensor_data); + reference_pub_ = + this->create_publisher( + guidance_topic, qos_sensor_data); reference_sub_ = this->create_subscription( reference_pose_topic, qos_sensor_data, [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg) { follower_->set_reference( - vortex::utils::ros_conversions::ros_pose_to_pose_euler( - msg->pose)); + vortex::utils::ros_conversions::ros_pose_to_pose(msg->pose)); }); pose_sub_ = this->create_subscription< @@ -70,9 +70,8 @@ void ReferenceFilterNode::set_subscribers_and_publisher() { [this](const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { std::lock_guard lock(sensor_mutex_); - current_pose_ = - vortex::utils::ros_conversions::ros_pose_to_pose_euler( - msg->pose.pose); + current_pose_ = vortex::utils::ros_conversions::ros_pose_to_pose( + msg->pose.pose); }); twist_sub_ = this->create_subscription< @@ -92,7 +91,7 @@ void ReferenceFilterNode::set_action_server() { this->get_parameter("action_servers.reference_filter").as_string(); action_server_ = rclcpp_action::create_server< - vortex_msgs::action::ReferenceFilterWaypoint>( + vortex_msgs::action::ReferenceFilterQuatWaypoint>( this, action_server_name, [this](const auto& uuid, auto goal) { return handle_goal(uuid, std::move(goal)); @@ -119,7 +118,8 @@ void ReferenceFilterNode::set_refererence_filter() { rclcpp_action::GoalResponse ReferenceFilterNode::handle_goal( const rclcpp_action::GoalUUID& /*uuid*/, - std::shared_ptr + std::shared_ptr< + const vortex_msgs::action::ReferenceFilterQuatWaypoint::Goal> /*goal*/) { spdlog::info("Accepted goal request"); return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; @@ -127,14 +127,14 @@ rclcpp_action::GoalResponse ReferenceFilterNode::handle_goal( rclcpp_action::CancelResponse ReferenceFilterNode::handle_cancel( const std::shared_ptr> /*goal_handle*/) { + vortex_msgs::action::ReferenceFilterQuatWaypoint>> /*goal_handle*/) { spdlog::info("Received request to cancel goal"); return rclcpp_action::CancelResponse::ACCEPT; } void ReferenceFilterNode::handle_accepted( const std::shared_ptr> goal_handle) { + vortex_msgs::action::ReferenceFilterQuatWaypoint>> goal_handle) { std::lock_guard lock(execute_mutex_); preempted_ = true; if (execute_thread_.joinable()) { @@ -148,7 +148,7 @@ void ReferenceFilterNode::handle_accepted( void ReferenceFilterNode::execute( const std::shared_ptr> goal_handle) { + vortex_msgs::action::ReferenceFilterQuatWaypoint>> goal_handle) { spdlog::info("Executing goal"); double convergence_threshold = @@ -171,9 +171,9 @@ void ReferenceFilterNode::execute( follower_->start(pose, twist, wp, convergence_threshold); auto feedback = std::make_shared< - vortex_msgs::action::ReferenceFilterWaypoint::Feedback>(); + vortex_msgs::action::ReferenceFilterQuatWaypoint::Feedback>(); auto result = std::make_shared< - vortex_msgs::action::ReferenceFilterWaypoint::Result>(); + vortex_msgs::action::ReferenceFilterQuatWaypoint::Result>(); rclcpp::Rate loop_rate(1000.0 / time_step_.count()); @@ -192,21 +192,20 @@ void ReferenceFilterNode::execute( return; } - Eigen::Vector18d filter_state = follower_->step(); + follower_->step(); - const auto current_pose_vector = [this] { + const auto current_pose = [this] { std::lock_guard lock(sensor_mutex_); - return current_pose_.to_vector(); + return current_pose_; }(); - bool target_reached = - follower_->within_convergance(current_pose_vector); + bool target_reached = follower_->within_convergance(current_pose); if (target_reached) { follower_->snap_state_to_reference(); - vortex_msgs::msg::ReferenceFilter final_reference_msg = - fill_reference_msg(follower_->state()); + auto final_reference_msg = + fill_reference_msg(follower_->pose(), follower_->velocity()); feedback->reference = final_reference_msg; goal_handle->publish_feedback(feedback); @@ -218,8 +217,8 @@ void ReferenceFilterNode::execute( return; } - vortex_msgs::msg::ReferenceFilter reference_msg = - fill_reference_msg(filter_state); + auto reference_msg = + fill_reference_msg(follower_->pose(), follower_->velocity()); reference_pub_->publish(reference_msg); feedback->reference = reference_msg; goal_handle->publish_feedback(feedback); @@ -227,7 +226,7 @@ void ReferenceFilterNode::execute( } if (!rclcpp::ok() && goal_handle->is_active()) { auto result = std::make_shared< - vortex_msgs::action::ReferenceFilterWaypoint::Result>(); + vortex_msgs::action::ReferenceFilterQuatWaypoint::Result>(); result->success = false; try { diff --git a/guidance/reference_filter_dp_quat/test/test_waypoint_follower.cpp b/guidance/reference_filter_dp_quat/test/test_waypoint_follower.cpp index e99dcf7e7..47248b565 100644 --- a/guidance/reference_filter_dp_quat/test/test_waypoint_follower.cpp +++ b/guidance/reference_filter_dp_quat/test/test_waypoint_follower.cpp @@ -13,7 +13,7 @@ class WaypointFollowerTests : public ::testing::Test { return params; } - PoseEuler zero_pose() { return PoseEuler{}; } + Pose zero_pose() { return Pose{}; } Twist zero_twist() { return Twist{}; } }; @@ -21,75 +21,76 @@ TEST_F(WaypointFollowerTests, StartAndStepConverges) { WaypointFollower follower(get_params(), 0.01); Waypoint wp; - wp.pose = PoseEuler{1.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + wp.pose = Pose{1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0}; wp.mode = WaypointMode::FULL_POSE; follower.start(zero_pose(), zero_twist(), wp, 0.1); - Eigen::Vector18d state = follower.step(); + follower.step(); // Simulate the measured pose being at the reference - Eigen::Vector6d measured_at_ref; - measured_at_ref << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0; + Pose measured_at_ref{1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0}; EXPECT_TRUE(follower.within_convergance(measured_at_ref)); - EXPECT_EQ(state.size(), 18); } TEST_F(WaypointFollowerTests, StepDoesNotConvergeWhenFar) { WaypointFollower follower(get_params(), 0.01); Waypoint wp; - wp.pose = PoseEuler{10.0, 10.0, 0.0, 0.0, 0.0, 0.0}; + wp.pose = Pose{10.0, 10.0, 0.0, 1.0, 0.0, 0.0, 0.0}; wp.mode = WaypointMode::FULL_POSE; follower.start(zero_pose(), zero_twist(), wp, 0.1); follower.step(); - Eigen::Vector6d measured_far = Eigen::Vector6d::Zero(); - EXPECT_FALSE(follower.within_convergance(measured_far)); + EXPECT_FALSE(follower.within_convergance(zero_pose())); } TEST_F(WaypointFollowerTests, SetReferenceUpdatesMidSequence) { WaypointFollower follower(get_params(), 0.01); Waypoint wp; - wp.pose = PoseEuler{1.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + wp.pose = Pose{1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0}; wp.mode = WaypointMode::FULL_POSE; follower.start(zero_pose(), zero_twist(), wp, 0.1); - PoseEuler new_ref{5.0, 5.0, 0.0, 0.0, 0.0, 0.0}; + Pose new_ref{5.0, 5.0, 0.0, 1.0, 0.0, 0.0, 0.0}; follower.set_reference(new_ref); - EXPECT_DOUBLE_EQ(follower.reference()(0), 5.0); - EXPECT_DOUBLE_EQ(follower.reference()(1), 5.0); + EXPECT_DOUBLE_EQ(follower.waypoint_goal().x, 5.0); + EXPECT_DOUBLE_EQ(follower.waypoint_goal().y, 5.0); } TEST_F(WaypointFollowerTests, SnapStateToReference) { WaypointFollower follower(get_params(), 0.01); Waypoint wp; - wp.pose = PoseEuler{3.0, 4.0, 5.0, 0.1, 0.2, 0.3}; + wp.pose = Pose{3.0, 4.0, 5.0, 1.0, 0.0, 0.0, 0.0}; wp.mode = WaypointMode::FULL_POSE; follower.start(zero_pose(), zero_twist(), wp, 0.1); follower.snap_state_to_reference(); - Eigen::Vector18d state = follower.state(); - Eigen::Vector6d ref = follower.reference(); + Pose pose = follower.pose(); + Pose goal = follower.waypoint_goal(); - for (int i = 0; i < 6; ++i) { - EXPECT_DOUBLE_EQ(state(i), ref(i)); - } + EXPECT_DOUBLE_EQ(pose.x, goal.x); + EXPECT_DOUBLE_EQ(pose.y, goal.y); + EXPECT_DOUBLE_EQ(pose.z, goal.z); + EXPECT_DOUBLE_EQ(pose.qw, goal.qw); + EXPECT_DOUBLE_EQ(pose.qx, goal.qx); + EXPECT_DOUBLE_EQ(pose.qy, goal.qy); + EXPECT_DOUBLE_EQ(pose.qz, goal.qz); } TEST_F(WaypointFollowerTests, StateEvolvesWithStep) { WaypointFollower follower(get_params(), 0.01); Waypoint wp; - wp.pose = PoseEuler{1.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + wp.pose = Pose{1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0}; wp.mode = WaypointMode::FULL_POSE; follower.start(zero_pose(), zero_twist(), wp, 0.1); @@ -100,7 +101,7 @@ TEST_F(WaypointFollowerTests, StateEvolvesWithStep) { } // x position should have moved toward 1.0 - EXPECT_GT(follower.state()(0), 0.0); + EXPECT_GT(follower.pose().x, 0.0); } } // namespace vortex::guidance diff --git a/guidance/reference_filter_dp_quat/test/test_waypoint_utils.cpp b/guidance/reference_filter_dp_quat/test/test_waypoint_utils.cpp index ed9e48aa2..0636a54ed 100644 --- a/guidance/reference_filter_dp_quat/test/test_waypoint_utils.cpp +++ b/guidance/reference_filter_dp_quat/test/test_waypoint_utils.cpp @@ -4,119 +4,131 @@ namespace vortex::guidance { -// --- apply_mode_logic tests --- +// --- compute_waypoint_goal tests --- + +TEST(ComputeWaypointGoal, FullPoseReturnsInputUnchanged) { + Pose incoming{1.0, 2.0, 3.0, 1.0, 0.0, 0.0, 0.0}; + Pose current{}; + + Pose result = + compute_waypoint_goal(incoming, WaypointMode::FULL_POSE, current); + + EXPECT_DOUBLE_EQ(result.x, 1.0); + EXPECT_DOUBLE_EQ(result.y, 2.0); + EXPECT_DOUBLE_EQ(result.z, 3.0); + EXPECT_DOUBLE_EQ(result.qw, 1.0); + EXPECT_DOUBLE_EQ(result.qx, 0.0); + EXPECT_DOUBLE_EQ(result.qy, 0.0); + EXPECT_DOUBLE_EQ(result.qz, 0.0); +} -TEST(ApplyModeLogic, FullPoseReturnsInputUnchanged) { - Eigen::Vector6d r_in; - r_in << 1.0, 2.0, 3.0, 0.1, 0.2, 0.3; - Eigen::Vector6d state = Eigen::Vector6d::Zero(); +TEST(ComputeWaypointGoal, OnlyPositionKeepsOrientationFromState) { + Pose incoming{1.0, 2.0, 3.0, 1.0, 0.0, 0.0, 0.0}; - Eigen::Vector6d result = - apply_mode_logic(r_in, WaypointMode::FULL_POSE, state); + // Current state with a non-identity orientation (90 deg about Z) + Eigen::Quaterniond q(Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitZ())); + Pose current = Pose::from_eigen(Eigen::Vector3d(10.0, 20.0, 30.0), q); - for (int i = 0; i < 6; ++i) { - EXPECT_DOUBLE_EQ(result(i), r_in(i)); - } -} + Pose result = + compute_waypoint_goal(incoming, WaypointMode::ONLY_POSITION, current); + + EXPECT_DOUBLE_EQ(result.x, 1.0); + EXPECT_DOUBLE_EQ(result.y, 2.0); + EXPECT_DOUBLE_EQ(result.z, 3.0); -TEST(ApplyModeLogic, OnlyPositionKeepsOrientationFromState) { - Eigen::Vector6d r_in; - r_in << 1.0, 2.0, 3.0, 0.1, 0.2, 0.3; - Eigen::Vector6d state; - state << 10.0, 20.0, 30.0, 0.4, 0.5, 0.6; - - Eigen::Vector6d result = - apply_mode_logic(r_in, WaypointMode::ONLY_POSITION, state); - - EXPECT_DOUBLE_EQ(result(0), 1.0); - EXPECT_DOUBLE_EQ(result(1), 2.0); - EXPECT_DOUBLE_EQ(result(2), 3.0); - EXPECT_DOUBLE_EQ(result(3), 0.4); - EXPECT_DOUBLE_EQ(result(4), 0.5); - EXPECT_DOUBLE_EQ(result(5), 0.6); + // Orientation should match current state + Eigen::Quaterniond result_q = result.ori_quaternion(); + EXPECT_TRUE(result_q.isApprox(q.normalized(), 1e-12)); } -TEST(ApplyModeLogic, ForwardHeadingComputesYawFromDelta) { - Eigen::Vector6d r_in; - r_in << 1.0, 1.0, 0.0, 0.0, 0.0, 0.0; - Eigen::Vector6d state = Eigen::Vector6d::Zero(); +TEST(ComputeWaypointGoal, ForwardHeadingComputesYawFromDelta) { + Pose incoming{1.0, 1.0, 0.0, 1.0, 0.0, 0.0, 0.0}; + Pose current{}; + + Pose result = + compute_waypoint_goal(incoming, WaypointMode::FORWARD_HEADING, current); + + EXPECT_DOUBLE_EQ(result.x, 1.0); + EXPECT_DOUBLE_EQ(result.y, 1.0); - Eigen::Vector6d result = - apply_mode_logic(r_in, WaypointMode::FORWARD_HEADING, state); + // Expected yaw = atan2(1, 1) = pi/4 + double expected_yaw = std::atan2(1.0, 1.0); + Eigen::Quaterniond expected_q( + Eigen::AngleAxisd(expected_yaw, Eigen::Vector3d::UnitZ())); - double expected_yaw = std::atan2(1.0, 1.0); // pi/4 - EXPECT_DOUBLE_EQ(result(0), 1.0); - EXPECT_DOUBLE_EQ(result(1), 1.0); - EXPECT_DOUBLE_EQ(result(3), 0.0); - EXPECT_DOUBLE_EQ(result(4), 0.0); - EXPECT_NEAR(result(5), expected_yaw, 1e-12); + Eigen::Quaterniond result_q = result.ori_quaternion(); + EXPECT_TRUE(result_q.isApprox(expected_q.normalized(), 1e-12)); } -TEST(ApplyModeLogic, OnlyOrientationKeepsPositionFromState) { - Eigen::Vector6d r_in; - r_in << 1.0, 2.0, 3.0, 0.1, 0.2, 0.3; - Eigen::Vector6d state; - state << 10.0, 20.0, 30.0, 0.4, 0.5, 0.6; - - Eigen::Vector6d result = - apply_mode_logic(r_in, WaypointMode::ONLY_ORIENTATION, state); - - EXPECT_DOUBLE_EQ(result(0), 10.0); - EXPECT_DOUBLE_EQ(result(1), 20.0); - EXPECT_DOUBLE_EQ(result(2), 30.0); - EXPECT_DOUBLE_EQ(result(3), 0.1); - EXPECT_DOUBLE_EQ(result(4), 0.2); - EXPECT_DOUBLE_EQ(result(5), 0.3); +TEST(ComputeWaypointGoal, OnlyOrientationKeepsPositionFromState) { + Eigen::Quaterniond q(Eigen::AngleAxisd(0.3, Eigen::Vector3d::UnitZ())); + Pose incoming = Pose::from_eigen(Eigen::Vector3d(1.0, 2.0, 3.0), q); + Pose current{10.0, 20.0, 30.0, 1.0, 0.0, 0.0, 0.0}; + + Pose result = compute_waypoint_goal( + incoming, WaypointMode::ONLY_ORIENTATION, current); + + EXPECT_DOUBLE_EQ(result.x, 10.0); + EXPECT_DOUBLE_EQ(result.y, 20.0); + EXPECT_DOUBLE_EQ(result.z, 30.0); + + Eigen::Quaterniond result_q = result.ori_quaternion(); + EXPECT_TRUE(result_q.isApprox(q.normalized(), 1e-12)); } // --- has_converged tests --- TEST(HasConverged, FullPoseBelowThreshold) { - Eigen::Vector6d measured; - measured << 1.0, 2.0, 3.0, 0.1, 0.2, 0.3; - Eigen::Vector6d reference = measured; - reference(0) += 0.001; + Pose measured{1.0, 2.0, 3.0, 1.0, 0.0, 0.0, 0.0}; + Pose reference{1.001, 2.0, 3.0, 1.0, 0.0, 0.0, 0.0}; EXPECT_TRUE( has_converged(measured, reference, WaypointMode::FULL_POSE, 0.1)); } TEST(HasConverged, FullPoseAboveThreshold) { - Eigen::Vector6d measured = Eigen::Vector6d::Zero(); - Eigen::Vector6d reference; - reference << 1.0, 1.0, 1.0, 0.0, 0.0, 0.0; + Pose measured{}; + Pose reference{1.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0}; EXPECT_FALSE( has_converged(measured, reference, WaypointMode::FULL_POSE, 0.1)); } TEST(HasConverged, OnlyPositionIgnoresOrientation) { - Eigen::Vector6d measured; - measured << 1.0, 2.0, 3.0, 0.0, 0.0, 0.0; - Eigen::Vector6d reference; - reference << 1.0, 2.0, 3.0, 1.0, 1.0, 1.0; + Pose measured{1.0, 2.0, 3.0, 1.0, 0.0, 0.0, 0.0}; + + // Same position, very different orientation + Eigen::Quaterniond q(Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitZ())); + Pose reference = Pose::from_eigen(Eigen::Vector3d(1.0, 2.0, 3.0), q); EXPECT_TRUE( has_converged(measured, reference, WaypointMode::ONLY_POSITION, 0.1)); } TEST(HasConverged, OnlyOrientationIgnoresPosition) { - Eigen::Vector6d measured; - measured << 100.0, 200.0, 300.0, 0.1, 0.2, 0.3; - Eigen::Vector6d reference; - reference << 0.0, 0.0, 0.0, 0.1, 0.2, 0.3; + Eigen::Quaterniond q(Eigen::AngleAxisd(0.1, Eigen::Vector3d::UnitZ())); + Pose measured = Pose::from_eigen(Eigen::Vector3d(100.0, 200.0, 300.0), q); + Pose reference = Pose::from_eigen(Eigen::Vector3d(0.0, 0.0, 0.0), q); EXPECT_TRUE(has_converged(measured, reference, WaypointMode::ONLY_ORIENTATION, 0.1)); } TEST(HasConverged, ForwardHeadingUsesPositionAndYawOnly) { - Eigen::Vector6d measured; - measured << 1.0, 2.0, 3.0, 0.5, 0.5, 0.1; - Eigen::Vector6d reference; - reference << 1.0, 2.0, 3.0, 0.0, 0.0, 0.1; - - // Roll and pitch differ but should be ignored + // Same position and yaw, but different roll (single axis keeps error + // purely in x-component, so z-component of quaternion_error is zero) + Eigen::Quaterniond q_measured = + Eigen::AngleAxisd(0.1, Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(0.5, Eigen::Vector3d::UnitX()); + Eigen::Quaterniond q_reference( + Eigen::AngleAxisd(0.1, Eigen::Vector3d::UnitZ())); + + Pose measured = + Pose::from_eigen(Eigen::Vector3d(1.0, 2.0, 3.0), q_measured); + Pose reference = + Pose::from_eigen(Eigen::Vector3d(1.0, 2.0, 3.0), q_reference); + + // Roll differs but should be ignored in FORWARD_HEADING mode EXPECT_TRUE( has_converged(measured, reference, WaypointMode::FORWARD_HEADING, 0.1)); } From c71c094d2859dcef4218056705f1088fa4ad0b8c Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Sat, 21 Mar 2026 22:42:03 +0100 Subject: [PATCH 171/290] RPY reference testing publisher --- .../ros/reference_filter_ros.hpp | 6 +++++ .../ros/reference_filter_ros_utils.hpp | 23 +++++++++++++++++ .../launch/reference_filter_dp_quat.launch.py | 25 ++++++++++++++++--- .../src/ros/reference_filter_ros.cpp | 18 +++++++++++++ 4 files changed, 69 insertions(+), 3 deletions(-) diff --git a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros.hpp b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros.hpp index 87df8e7e6..8617098ca 100644 --- a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros.hpp +++ b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros.hpp @@ -10,6 +10,7 @@ #include #include #include +#include #include #include #include "reference_filter_dp_quat/lib/waypoint_follower.hpp" @@ -68,6 +69,11 @@ class ReferenceFilterNode : public rclcpp::Node { rclcpp::Publisher::SharedPtr reference_pub_; + rclcpp::Publisher::SharedPtr + rpy_debug_pub_; + + bool publish_rpy_debug_{false}; + rclcpp::Subscription::SharedPtr reference_sub_; diff --git a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros_utils.hpp b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros_utils.hpp index d04dc02a9..043a5c526 100644 --- a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros_utils.hpp +++ b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros_utils.hpp @@ -5,6 +5,7 @@ #include #include #include +#include #include #include #include "reference_filter_dp_quat/lib/eigen_typedefs.hpp" @@ -60,6 +61,28 @@ inline vortex_msgs::msg::ReferenceFilterQuat fill_reference_msg( return msg; } +/// @brief Fill an RPY ReferenceFilter message from a Pose and velocity vector. +inline vortex_msgs::msg::ReferenceFilter fill_reference_rpy_msg( + const vortex::utils::types::Pose& pose, + const Eigen::Vector6d& velocity) { + using vortex::utils::math::ssa; + auto euler = pose.as_pose_euler(); + vortex_msgs::msg::ReferenceFilter msg; + msg.x = euler.x; + msg.y = euler.y; + msg.z = euler.z; + msg.roll = ssa(euler.roll); + msg.pitch = ssa(euler.pitch); + msg.yaw = ssa(euler.yaw); + msg.x_dot = velocity(0); + msg.y_dot = velocity(1); + msg.z_dot = velocity(2); + msg.roll_dot = velocity(3); + msg.pitch_dot = velocity(4); + msg.yaw_dot = velocity(5); + return msg; +} + } // namespace vortex::guidance #endif // REFERENCE_FILTER_DP_QUAT__ROS__REFERENCE_FILTER_ROS_UTILS_HPP_ diff --git a/guidance/reference_filter_dp_quat/launch/reference_filter_dp_quat.launch.py b/guidance/reference_filter_dp_quat/launch/reference_filter_dp_quat.launch.py index 8a8d4d310..a610a8ef3 100644 --- a/guidance/reference_filter_dp_quat/launch/reference_filter_dp_quat.launch.py +++ b/guidance/reference_filter_dp_quat/launch/reference_filter_dp_quat.launch.py @@ -2,7 +2,8 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import OpaqueFunction +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node from auv_setup.launch_arg_common import ( @@ -13,6 +14,7 @@ def launch_setup(context, *args, **kwargs): drone, namespace = resolve_drone_and_namespace(context) + rpy_publish = LaunchConfiguration("rpy_publish").perform(context) == "true" config_file_path = os.path.join( get_package_share_directory("reference_filter_dp_quat"), @@ -27,13 +29,21 @@ def launch_setup(context, *args, **kwargs): f"{drone}.yaml", ) + extra_params = {} + if rpy_publish: + extra_params = { + "publish_rpy_debug": True, + "topics.guidance.dp_rpy": "guidance/dp", + "topics.guidance.dp": "guidance/dp_quat", + } + return [ Node( package="reference_filter_dp_quat", executable="reference_filter_dp_quat_node", name="reference_filter_node", namespace=namespace, - parameters=[config_file_path, drone_params], + parameters=[config_file_path, drone_params, extra_params], output="screen", ) ] @@ -41,5 +51,14 @@ def launch_setup(context, *args, **kwargs): def generate_launch_description(): return LaunchDescription( - declare_drone_and_namespace_args() + [OpaqueFunction(function=launch_setup)] + declare_drone_and_namespace_args() + + [ + DeclareLaunchArgument( + "rpy_publish", + default_value="false", + description="When true, publish RPY ReferenceFilter on guidance/dp " + "for compatibility with controllers expecting RPY messages.", + ), + OpaqueFunction(function=launch_setup), + ] ) diff --git a/guidance/reference_filter_dp_quat/src/ros/reference_filter_ros.cpp b/guidance/reference_filter_dp_quat/src/ros/reference_filter_ros.cpp index cfea83085..7e85b1923 100644 --- a/guidance/reference_filter_dp_quat/src/ros/reference_filter_ros.cpp +++ b/guidance/reference_filter_dp_quat/src/ros/reference_filter_ros.cpp @@ -57,6 +57,16 @@ void ReferenceFilterNode::set_subscribers_and_publisher() { this->create_publisher( guidance_topic, qos_sensor_data); + publish_rpy_debug_ = this->declare_parameter("publish_rpy_debug"); + if (publish_rpy_debug_) { + std::string rpy_topic = this->declare_parameter( + "topics.guidance.dp_rpy", guidance_topic + "_rpy"); + rpy_debug_pub_ = + this->create_publisher( + rpy_topic, qos_sensor_data); + spdlog::info("RPY debug publisher enabled on topic: {}", rpy_topic); + } + reference_sub_ = this->create_subscription( reference_pose_topic, qos_sensor_data, [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg) { @@ -210,6 +220,10 @@ void ReferenceFilterNode::execute( feedback->reference = final_reference_msg; goal_handle->publish_feedback(feedback); reference_pub_->publish(final_reference_msg); + if (rpy_debug_pub_) { + rpy_debug_pub_->publish(fill_reference_rpy_msg( + follower_->pose(), follower_->velocity())); + } result->success = true; goal_handle->succeed(result); @@ -220,6 +234,10 @@ void ReferenceFilterNode::execute( auto reference_msg = fill_reference_msg(follower_->pose(), follower_->velocity()); reference_pub_->publish(reference_msg); + if (rpy_debug_pub_) { + rpy_debug_pub_->publish(fill_reference_rpy_msg( + follower_->pose(), follower_->velocity())); + } feedback->reference = reference_msg; goal_handle->publish_feedback(feedback); loop_rate.sleep(); From e0b29a593cf88ec0b0b0842f36d2fa7ecf9d5030 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Sat, 21 Mar 2026 22:48:47 +0100 Subject: [PATCH 172/290] sim test for quat ref filter --- .../config/reference_filter_params.yaml | 1 + .../waypoint_navigation_quat/check_goal.py | 93 ++++++++++++++ .../waypoint_navigation_quat/send_goal.py | 93 ++++++++++++++ .../simulator_test.sh | 113 ++++++++++++++++++ 4 files changed, 300 insertions(+) create mode 100644 tests/simulator_tests/waypoint_navigation_quat/check_goal.py create mode 100644 tests/simulator_tests/waypoint_navigation_quat/send_goal.py create mode 100755 tests/simulator_tests/waypoint_navigation_quat/simulator_test.sh diff --git a/guidance/reference_filter_dp_quat/config/reference_filter_params.yaml b/guidance/reference_filter_dp_quat/config/reference_filter_params.yaml index 1aef0ec13..71aad134a 100644 --- a/guidance/reference_filter_dp_quat/config/reference_filter_params.yaml +++ b/guidance/reference_filter_dp_quat/config/reference_filter_params.yaml @@ -3,3 +3,4 @@ zeta: [1., 1., 1., 1., 1., 1.] omega: [0.2, 0.2, 0.2, 0.2, 0.2, 0.2] time_step_ms: 10 + publish_rpy_debug: false diff --git a/tests/simulator_tests/waypoint_navigation_quat/check_goal.py b/tests/simulator_tests/waypoint_navigation_quat/check_goal.py new file mode 100644 index 000000000..9f29c4709 --- /dev/null +++ b/tests/simulator_tests/waypoint_navigation_quat/check_goal.py @@ -0,0 +1,93 @@ +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.1 # meters +ori_tol = 0.2 # rad + + +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 drone to reach goal: {goal_pos} with orientation {goal_ori}") + + start_time = time.time() + timeout = 20 # seconds + + 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"Drone reached goal: {goal_pos} and orientation: {goal_ori}") + print(f"Final drone pose: ({x, y, z}), {current_ori}") + print(f"Euclidean error: {dist}, {dist_ori}") + rclpy.shutdown() + exit(0) + + print(f"Drone did not reach goal: {goal_pos} and orientation: {goal_ori}") + print(f"Current_drone pose: ({x, y, z}), {current_ori}") + print(f"Euclidean error: {dist}, {dist_ori}") + rclpy.shutdown() + exit(1) + + +if __name__ == '__main__': + main() diff --git a/tests/simulator_tests/waypoint_navigation_quat/send_goal.py b/tests/simulator_tests/waypoint_navigation_quat/send_goal.py new file mode 100644 index 000000000..c5da67abb --- /dev/null +++ b/tests/simulator_tests/waypoint_navigation_quat/send_goal.py @@ -0,0 +1,93 @@ +import random + +import rclpy +import yaml +from rclpy.action import ActionClient +from rclpy.node import Node +from vortex_msgs.action import ReferenceFilterQuatWaypoint +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(-0.25, 0.25) + pose.yaw = random.uniform(-1.57, 1.57) + + return pose + + +class ReferenceFilterQuatWaypointClient(Node): + def __init__(self): + super().__init__('reference_filter_quat_waypoint_client') + + self._action_client = ActionClient( + self, ReferenceFilterQuatWaypoint, '/orca/reference_filter' + ) + self.send_goal() + + def send_goal(self): + goal_pose = randomize_pose() + goal_msg = ReferenceFilterQuatWaypoint.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] + + goal_msg.convergence_threshold = 0.1 + + # 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 = ReferenceFilterQuatWaypointClient() + rclpy.spin(action_client) + + +if __name__ == '__main__': + main() diff --git a/tests/simulator_tests/waypoint_navigation_quat/simulator_test.sh b/tests/simulator_tests/waypoint_navigation_quat/simulator_test.sh new file mode 100755 index 000000000..6f403104f --- /dev/null +++ b/tests/simulator_tests/waypoint_navigation_quat/simulator_test.sh @@ -0,0 +1,113 @@ +#!/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..." + kill -TERM -"$SIM_PID" -"$ORCA_PID" -"$CONTROLLER_PID" -"$FILTER_PID" -"$OP_MODE_PID" || true + 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 drone_sim.launch.py & +ORCA_PID=$! +echo "Launched orca with PID: $ORCA_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 quaternion reference filter with RPY compatibility (publishes RPY on guidance/dp) +setsid ros2 launch reference_filter_dp_quat reference_filter_dp_quat.launch.py rpy_publish:=true & +FILTER_PID=$! +echo "Launched quat reference filter (rpy_publish) with PID: $FILTER_PID" + +# Launch controller separately +setsid ros2 launch dp_adapt_backs_controller dp_adapt_backs_controller.launch.py & +CONTROLLER_PID=$! +echo "Launched controller 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 + +echo "Sleeping for 5 seconds to make sure operation is stable..." +sleep 5 + +# Set operation mode +echo "Turning off killswitch and setting operation mode to 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}}" + +echo "Sleeping for 5 seconds to make sure operation is stable..." +sleep 5 + +# 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: Drone did not reach goal." + exit 1 +else + echo "Test passed: Drone reached goal." +fi + +# Terminate processes +kill -TERM -"$SIM_PID" -"$ORCA_PID" -"$CONTROLLER_PID" -"$FILTER_PID" -"$BAG_PID" -"$OP_MODE_PID" + +echo "Test completed successfully." From 7b3eaf279174ab0cbc9a0e1535d35ad13316e482 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Sat, 21 Mar 2026 22:59:44 +0100 Subject: [PATCH 173/290] update pkg maintainer :) --- guidance/reference_filter_dp_quat/package.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/guidance/reference_filter_dp_quat/package.xml b/guidance/reference_filter_dp_quat/package.xml index ce1ad0087..d7aa188a2 100644 --- a/guidance/reference_filter_dp_quat/package.xml +++ b/guidance/reference_filter_dp_quat/package.xml @@ -3,8 +3,8 @@ reference_filter_dp_quat 0.0.0 - Reference model for the DP controller - andeshog + Quaternion Reference model for the DP controller + jorgenfj MIT ament_cmake From 217831ac2e0abaedf4d275cc0779f7045cbc119d Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Sat, 21 Mar 2026 23:01:30 +0100 Subject: [PATCH 174/290] add reference quat sim test to workflow --- .github/workflows/simulator-test.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/simulator-test.yml b/.github/workflows/simulator-test.yml index 69038666b..194450eca 100644 --- a/.github/workflows/simulator-test.yml +++ b/.github/workflows/simulator-test.yml @@ -19,4 +19,5 @@ jobs: "tests/simulator_tests/waypoint_navigation/simulator_test.sh", "tests/simulator_tests/los_test/simulator_test.sh", "tests/simulator_tests/waypoint_manager_test/simulator_test.sh", + "tests/simulator_tests/waypoint_navigation_quat/simulator_test.sh", ]' From beb0b70413a59ebdb01311d5fdea9dadfb26500e Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Sun, 22 Mar 2026 12:56:51 +0100 Subject: [PATCH 175/290] helpers for step function --- .../lib/waypoint_follower.hpp | 24 +++++++++++++-- .../src/lib/waypoint_follower.cpp | 30 ++++++++++++------- 2 files changed, 41 insertions(+), 13 deletions(-) diff --git a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_follower.hpp b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_follower.hpp index ddb89362b..43cf5f8a1 100644 --- a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_follower.hpp +++ b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_follower.hpp @@ -67,16 +67,34 @@ class WaypointFollower { */ void snap_state_to_reference(); - /// @brief Get the current nominal pose (position + quaternion). + /** + * @brief Get the current nominal pose (position + quaternion). + */ Pose pose() const; - /// @brief Get the current world-frame velocity (linear + angular). + /** + * @brief Get the current world-frame velocity (linear + angular). + */ Eigen::Vector6d velocity() const; - /// @brief Get the current waypoint goal. + /** + * @brief Get the current waypoint goal. + */ Pose waypoint_goal() const; private: + /** + * @brief Compute error-state reference based on the current nominal pose + * and waypoint goal. + */ + Eigen::Vector6d update_reference() const; + + /** + * @brief Absorb position/orientation errors into the nominal pose and + * reset the error states. + */ + void inject_and_reset(); + mutable std::mutex mutex_; ReferenceFilter filter_; double dt_seconds_{0.01}; diff --git a/guidance/reference_filter_dp_quat/src/lib/waypoint_follower.cpp b/guidance/reference_filter_dp_quat/src/lib/waypoint_follower.cpp index a44944c02..7c9d8b49a 100644 --- a/guidance/reference_filter_dp_quat/src/lib/waypoint_follower.cpp +++ b/guidance/reference_filter_dp_quat/src/lib/waypoint_follower.cpp @@ -31,23 +31,33 @@ void WaypointFollower::start(const Pose& pose, void WaypointFollower::step() { std::lock_guard lock(mutex_); + const Eigen::Vector6d filter_reference = update_reference(); - Eigen::Vector6d r; - r.head<3>() = waypoint_goal_.pos_vector() - nominal_pose_.pos_vector(); - r.tail<3>() = vortex::utils::math::quaternion_error( - nominal_pose_.ori_quaternion(), waypoint_goal_.ori_quaternion()); + const Eigen::Vector18d state_derivative = + filter_.calculate_x_dot(state_, filter_reference); + state_ += state_derivative * dt_seconds_; + inject_and_reset(); +} - Eigen::Vector18d x_dot = filter_.calculate_x_dot(state_, r); - state_ += x_dot * dt_seconds_; +Eigen::Vector6d WaypointFollower::update_reference() const { + Eigen::Vector6d filter_reference; + filter_reference.head<3>() = + waypoint_goal_.pos_vector() - nominal_pose_.pos_vector(); + filter_reference.tail<3>() = vortex::utils::math::quaternion_error( + nominal_pose_.ori_quaternion(), waypoint_goal_.ori_quaternion()); + return filter_reference; +} +void WaypointFollower::inject_and_reset() { nominal_pose_.set_pos(nominal_pose_.pos_vector() + state_.head<3>()); state_.head<3>().setZero(); - Eigen::Vector3d dphi = state_.segment<3>(3); - double angle = dphi.norm(); + const Eigen::Vector3d delta_orientation = state_.segment<3>(3); + const double angle = delta_orientation.norm(); if (angle >= 1e-10) { - Eigen::Quaterniond dq(Eigen::AngleAxisd(angle, dphi.normalized())); - nominal_pose_.set_ori(nominal_pose_.ori_quaternion() * dq); + Eigen::Quaterniond delta_quat( + Eigen::AngleAxisd(angle, delta_orientation.normalized())); + nominal_pose_.set_ori(nominal_pose_.ori_quaternion() * delta_quat); state_.segment<3>(3).setZero(); } } From ef4517efd5bbffb2b7d2030843b9cf8e2a360f69 Mon Sep 17 00:00:00 2001 From: Anbit Date: Mon, 23 Mar 2026 12:14:30 +0100 Subject: [PATCH 176/290] 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 177/290] 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 178/290] 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 179/290] 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 180/290] 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 181/290] 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 182/290] 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 183/290] 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 184/290] 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 185/290] 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 186/290] 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 187/290] 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 188/290] 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 189/290] 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 190/290] 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 191/290] 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 b965ebd5ce9637b71c422d1d49402c86f3f2ae1e Mon Sep 17 00:00:00 2001 From: Anbit Date: Tue, 24 Mar 2026 15:51:35 +0100 Subject: [PATCH 192/290] Fix: Issues with slowdown function --- .../los_guidance/config/guidance_params.yaml | 6 ++-- .../include/los_guidance/los_guidance_ros.hpp | 3 ++ .../launch/guidance_test.launch.py | 10 +++---- .../los_guidance/src/los_guidance_ros.cpp | 30 ++++++++++++++++--- 4 files changed, 38 insertions(+), 11 deletions(-) diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 0a546a4a8..7a7b8c091 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -28,9 +28,11 @@ vector_field_los: common: active_los_method: 2 # 0: Adaptive LOS, 1: Proportional LOS, 2: Integral LOS, 3: Vector Field LOS u_desired: 0.3 - goal_reached_tol: 0.15 + goal_reached_tol: 0.20 max_pitch_angle: 0.96 # 55 degrees - slow_down_distance: 0.40 + slow_down_distance: 1.0 + u_slow_min_ : 0.3 + surge_initialization: false # 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 a996c42b9..a2baf24a2 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -113,6 +113,9 @@ class LosGuidanceNode : public rclcpp::Node { double goal_reached_tol_{}; double max_pitch_angle_{}; double slow_down_distance_{}; + bool surge_initialized_{}; + double u_slow_min_{}; + double commanded_surge_{}; 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 7b3470932..79d3a1c94 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( @@ -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, diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 976d73980..b39d141fb 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -261,15 +261,34 @@ vortex_msgs::msg::LOSGuidance LosGuidanceNode::fill_los_reference( 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 + double target_surge = u_desired_; + double surge_rate_limit_ = 0.3; if (distance_to_goal <= slow_down_distance_) { const double alpha = distance_to_goal / slow_down_distance_; - reference_msg.surge = u_slow_min + alpha * (u_desired_ - u_slow_min); - } else { - reference_msg.surge = u_desired_; + reference_msg.surge = u_slow_min_ + alpha * (u_desired_ - u_slow_min_); + } + + if (!surge_initialized_) { + commanded_surge_ = target_surge; + surge_initialized_ = true; } + else { + const double dt = static_cast(time_step_.count()) / 1000.0; + const double max_step = surge_rate_limit_ * dt; + const double delta = target_surge - commanded_surge_; + + if (delta > max_step) { + commanded_surge_ += max_step; + } else if (delta < -max_step) { + commanded_surge_ -= max_step; + } else { + commanded_surge_ = target_surge; + } + } + + reference_msg.surge = commanded_surge_; return reference_msg; } @@ -284,6 +303,9 @@ void LosGuidanceNode::parse_common_config(YAML::Node common_config) { 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(); + u_slow_min_ = common_config["u_slow_min_"].as(); + surge_initialized_ = common_config["surge_initialization"].as(); + method_ = static_cast( common_config["active_los_method"].as()); } From bcd49aa475bf2ad93c41a7ba0ae9d6444c972943 Mon Sep 17 00:00:00 2001 From: Anbit Date: Tue, 24 Mar 2026 16:09:20 +0100 Subject: [PATCH 193/290] Merge with main --- .../los_guidance/config/guidance_params.yaml | 3 +- .../include/los_guidance/los_guidance_ros.hpp | 1 + .../launch/guidance_test.launch.py | 32 +------------------ .../los_guidance/src/los_guidance_ros.cpp | 2 +- 4 files changed, 5 insertions(+), 33 deletions(-) diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 7a7b8c091..7a8c613d7 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -31,7 +31,8 @@ common: goal_reached_tol: 0.20 max_pitch_angle: 0.96 # 55 degrees slow_down_distance: 1.0 - u_slow_min_ : 0.3 + u_slow_min_ : 0.1 + surge_rate_limit: 0.1 surge_initialization: false # Debug Settings 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 a2baf24a2..e6f3d3851 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -116,6 +116,7 @@ class LosGuidanceNode : public rclcpp::Node { bool surge_initialized_{}; double u_slow_min_{}; double commanded_surge_{}; + double surge_rate_limit_ {}; 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 79d3a1c94..7a0470421 100644 --- a/guidance/los_guidance/launch/guidance_test.launch.py +++ b/guidance/los_guidance/launch/guidance_test.launch.py @@ -23,9 +23,7 @@ def launch_setup(context, *args, **kwargs): test_scenario = LaunchConfiguration("test_scenario").perform(context) stonefish_dir = get_package_share_directory("stonefish_sim") - vortex_sim_interface_dir = get_package_share_directory("vortex_sim_interface") los_guidance_dir = get_package_share_directory("los_guidance") - operation_mode_manager_dir = get_package_share_directory("operation_mode_manager") velocity_controller_dir = get_package_share_directory('velocity_controller') stonefish_sim = IncludeLaunchDescription( @@ -34,33 +32,7 @@ def launch_setup(context, *args, **kwargs): ), launch_arguments={ "scenario": "default", - "rendering": "false", - }.items(), - ) - - vortex_sim_interface = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join( - vortex_sim_interface_dir, "launch", "vortex_sim_interface.launch.py" - ) - ), - launch_arguments={ - "drone": drone, - "namespace": namespace, - }.items(), - ) - - operation_mode_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join( - operation_mode_manager_dir, - "launch", - "operation_mode_manager.launch.py", - ) - ), - launch_arguments={ - "drone": drone, - "namespace": namespace, + "rendering": "true", }.items(), ) @@ -139,8 +111,6 @@ def launch_setup(context, *args, **kwargs): return [ stonefish_sim, - #vortex_sim_interface, - #operation_mode_launch, 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 b39d141fb..2598257ae 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -262,7 +262,6 @@ vortex_msgs::msg::LOSGuidance LosGuidanceNode::fill_los_reference( (inputs.current_position - inputs.next_point).as_vector().norm(); double target_surge = u_desired_; - double surge_rate_limit_ = 0.3; if (distance_to_goal <= slow_down_distance_) { const double alpha = distance_to_goal / slow_down_distance_; @@ -305,6 +304,7 @@ void LosGuidanceNode::parse_common_config(YAML::Node common_config) { slow_down_distance_ = common_config["slow_down_distance"].as(); u_slow_min_ = common_config["u_slow_min_"].as(); surge_initialized_ = common_config["surge_initialization"].as(); + surge_rate_limit_ = common_config["surge_rate_limit"].as(); method_ = static_cast( common_config["active_los_method"].as()); From e4db24b02847d7a214c30d2b2fd099bca26b55d0 Mon Sep 17 00:00:00 2001 From: Anbit Date: Tue, 24 Mar 2026 19:05:19 +0100 Subject: [PATCH 194/290] fix slowing issue --- .../velocity_controller_lqr/__init__.py | 0 guidance/los_guidance/config/guidance_params.yaml | 2 +- guidance/los_guidance/src/los_guidance_ros.cpp | 11 ++++++----- 3 files changed, 7 insertions(+), 6 deletions(-) create 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 new file mode 100644 index 000000000..e69de29bb diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 7a8c613d7..ca9e93dcf 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -30,7 +30,7 @@ common: u_desired: 0.3 goal_reached_tol: 0.20 max_pitch_angle: 0.96 # 55 degrees - slow_down_distance: 1.0 + slow_down_distance: 1.5 u_slow_min_ : 0.1 surge_rate_limit: 0.1 surge_initialization: false diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 2598257ae..241b8347f 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -253,6 +253,7 @@ vortex_msgs::msg::LOSGuidance LosGuidanceNode::fill_los_reference( 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; @@ -264,16 +265,16 @@ vortex_msgs::msg::LOSGuidance LosGuidanceNode::fill_los_reference( double target_surge = u_desired_; 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_); + const double alpha = std::clamp( + distance_to_goal / slow_down_distance_, 0.0, 1.0); + target_surge = + u_slow_min_ + alpha * (u_desired_ - u_slow_min_); } if (!surge_initialized_) { commanded_surge_ = target_surge; surge_initialized_ = true; - } - - else { + } else { const double dt = static_cast(time_step_.count()) / 1000.0; const double max_step = surge_rate_limit_ * dt; const double delta = target_surge - commanded_surge_; From 5d766b2f68e0893069e73d3e27c36a52c22dfb25 Mon Sep 17 00:00:00 2001 From: Anbit Date: Tue, 24 Mar 2026 19:17:29 +0100 Subject: [PATCH 195/290] Pre-commit check and changes --- .../los_guidance/config/guidance_params.yaml | 3 +- .../include/los_guidance/los_guidance_ros.hpp | 3 +- .../launch/guidance_test.launch.py | 3 +- .../los_guidance/src/los_guidance_ros.cpp | 50 ++++++++++--------- 4 files changed, 32 insertions(+), 27 deletions(-) diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index ca9e93dcf..7c89a1b35 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -30,8 +30,9 @@ common: u_desired: 0.3 goal_reached_tol: 0.20 max_pitch_angle: 0.96 # 55 degrees + slow_approach: false slow_down_distance: 1.5 - u_slow_min_ : 0.1 + u_slow_min: 0.1 surge_rate_limit: 0.1 surge_initialization: false 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 e6f3d3851..fa0b878e2 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -112,11 +112,12 @@ class LosGuidanceNode : public rclcpp::Node { double u_desired_{}; double goal_reached_tol_{}; double max_pitch_angle_{}; + bool slow_approach_{}; double slow_down_distance_{}; bool surge_initialized_{}; double u_slow_min_{}; double commanded_surge_{}; - double surge_rate_limit_ {}; + double surge_rate_limit_{}; 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 7a0470421..772474673 100644 --- a/guidance/los_guidance/launch/guidance_test.launch.py +++ b/guidance/los_guidance/launch/guidance_test.launch.py @@ -52,11 +52,10 @@ def launch_setup(context, *args, **kwargs): velocity_controller_dir, 'launch', 'velocity_controller.launch.py' ) ), - launch_arguments={ + launch_arguments={ "drone": drone, "namespace": namespace, }.items(), - ) orca_sim = TimerAction( diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 241b8347f..5c0f8dc3a 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -259,33 +259,36 @@ vortex_msgs::msg::LOSGuidance LosGuidanceNode::fill_los_reference( reference_msg.pitch = clamped_pitch; reference_msg.yaw = outputs.psi_d; - const double distance_to_goal = - (inputs.current_position - inputs.next_point).as_vector().norm(); + if (slow_approach_) { + const double distance_to_goal = + (inputs.current_position - inputs.next_point).as_vector().norm(); - double target_surge = u_desired_; + double target_surge = u_desired_; - if (distance_to_goal <= slow_down_distance_) { - const double alpha = std::clamp( - distance_to_goal / slow_down_distance_, 0.0, 1.0); - target_surge = - u_slow_min_ + alpha * (u_desired_ - u_slow_min_); - } + if (distance_to_goal <= slow_down_distance_) { + const double alpha = + std::clamp(distance_to_goal / slow_down_distance_, 0.0, 1.0); + target_surge = u_slow_min_ + alpha * (u_desired_ - u_slow_min_); + } - if (!surge_initialized_) { - commanded_surge_ = target_surge; - surge_initialized_ = true; - } else { - const double dt = static_cast(time_step_.count()) / 1000.0; - const double max_step = surge_rate_limit_ * dt; - const double delta = target_surge - commanded_surge_; - - if (delta > max_step) { - commanded_surge_ += max_step; - } else if (delta < -max_step) { - commanded_surge_ -= max_step; - } else { + if (!surge_initialized_) { commanded_surge_ = target_surge; + surge_initialized_ = true; + } else { + const double dt = static_cast(time_step_.count()) / 1000.0; + const double max_step = surge_rate_limit_ * dt; + const double delta = target_surge - commanded_surge_; + + if (delta > max_step) { + commanded_surge_ += max_step; + } else if (delta < -max_step) { + commanded_surge_ -= max_step; + } else { + commanded_surge_ = target_surge; + } } + } else { + commanded_surge_ = u_desired_; } reference_msg.surge = commanded_surge_; @@ -302,8 +305,9 @@ 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_approach_ = common_config["slow_approach"].as(); slow_down_distance_ = common_config["slow_down_distance"].as(); - u_slow_min_ = common_config["u_slow_min_"].as(); + u_slow_min_ = common_config["u_slow_min"].as(); surge_initialized_ = common_config["surge_initialization"].as(); surge_rate_limit_ = common_config["surge_rate_limit"].as(); From 3e6f6f10e2f1ad2a0bf4a5d3d8a03f89907fa9f2 Mon Sep 17 00:00:00 2001 From: Henrik Date: Tue, 24 Mar 2026 23:17:03 +0100 Subject: [PATCH 196/290] 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 197/290] 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 198/290] changed the file system so that test files is only built when testing --- control/velocity_controller/CMakeLists.txt | 27 ------------------- .../velocity_controller/tests/CMakeLists.txt | 26 +++++++++++++++++- .../{src => tests}/LQR_test.cpp | 0 .../{src => tests}/test_VC.cpp | 5 +--- .../velocity_controller => tests}/test_VC.hpp | 6 ----- 5 files changed, 26 insertions(+), 38 deletions(-) rename control/velocity_controller/{src => tests}/LQR_test.cpp (100%) rename control/velocity_controller/{src => tests}/test_VC.cpp (92%) rename control/velocity_controller/{include/velocity_controller => tests}/test_VC.hpp (84%) diff --git a/control/velocity_controller/CMakeLists.txt b/control/velocity_controller/CMakeLists.txt index 5540b1e01..30fd54084 100644 --- a/control/velocity_controller/CMakeLists.txt +++ b/control/velocity_controller/CMakeLists.txt @@ -107,34 +107,7 @@ if(BUILD_TESTING) add_subdirectory(tests) endif() -add_executable(test_VC_node -src/test_VC.cpp -src/utilities.cpp -src/ct_instantiations.cpp -) - -target_include_directories(test_VC_node PUBLIC -$ -$ -${EIGEN3_INCLUDE_DIR} -) - -target_link_libraries(test_VC_node Eigen3::Eigen ct_optcon ct_core) - -ament_target_dependencies(test_VC_node -rclcpp -rclcpp_lifecycle -std_msgs -vortex_msgs -geometry_msgs -nav_msgs -vortex_utils -) - -install(TARGETS test_VC_node -DESTINATION lib/${PROJECT_NAME} -) ament_package() diff --git a/control/velocity_controller/tests/CMakeLists.txt b/control/velocity_controller/tests/CMakeLists.txt index b76128d1c..0d579c4f3 100644 --- a/control/velocity_controller/tests/CMakeLists.txt +++ b/control/velocity_controller/tests/CMakeLists.txt @@ -78,6 +78,30 @@ target_link_libraries( ct_optcon ct_core ) +add_executable(test_VC_node +test_VC.cpp +../src/utilities.cpp +) + +target_include_directories(test_VC_node PUBLIC +$ +$ +${EIGEN3_INCLUDE_DIR} +) + + +target_link_libraries(test_VC_node Eigen3::Eigen ct_optcon ct_core) +ament_target_dependencies(test_VC_node +rclcpp +rclcpp_lifecycle +std_msgs +vortex_msgs +geometry_msgs +nav_msgs +vortex_utils +) -#gtest_discover_tests(${TEST_BINARY_NAME}) \ No newline at end of file +install(TARGETS test_VC_node +DESTINATION lib/${PROJECT_NAME} +) \ No newline at end of file diff --git a/control/velocity_controller/src/LQR_test.cpp b/control/velocity_controller/tests/LQR_test.cpp similarity index 100% rename from control/velocity_controller/src/LQR_test.cpp rename to control/velocity_controller/tests/LQR_test.cpp diff --git a/control/velocity_controller/src/test_VC.cpp b/control/velocity_controller/tests/test_VC.cpp similarity index 92% rename from control/velocity_controller/src/test_VC.cpp rename to control/velocity_controller/tests/test_VC.cpp index 42a660e20..93dced1f4 100644 --- a/control/velocity_controller/src/test_VC.cpp +++ b/control/velocity_controller/tests/test_VC.cpp @@ -5,7 +5,7 @@ #include #include ///#include "velocity_controller/PID_setup.hpp" -#include "velocity_controller/test_VC.hpp" +#include "test_VC.hpp" #include #include #include @@ -47,13 +47,10 @@ void test_VC::send_guidance() reference_msg.yaw=std::numbers::pi*sin(time1*std::numbers::pi/9); //reference_msg.pitch=0.3*sin(time1*std::numbers::pi/9); reference_msg.surge=0.20;reference_msg.pitch=-0.4;//reference_msg.yaw=0.0; //Surge, pitch, yaw - //RCLCPP_INFO(this->get_logger(), "guidance callback: %f, %f, %f",reference_msg.surge,reference_msg.pitch,reference_msg.yaw); - publisher_guidance->publish(reference_msg); } void test_VC::odometry_callback(const nav_msgs::msg::Odometry::SharedPtr msg_ptr){ - //RCLCPP_INFO(this->get_logger(), "odo callback"); vortex_msgs::msg::LOSGuidance msg; angle temp=quaternion_to_euler_angle(msg_ptr->pose.pose.orientation.w, msg_ptr->pose.pose.orientation.x, msg_ptr->pose.pose.orientation.y, msg_ptr->pose.pose.orientation.z); msg.set__pitch(temp.thetat); diff --git a/control/velocity_controller/include/velocity_controller/test_VC.hpp b/control/velocity_controller/tests/test_VC.hpp similarity index 84% rename from control/velocity_controller/include/velocity_controller/test_VC.hpp rename to control/velocity_controller/tests/test_VC.hpp index 711c9f334..ff82b8cd5 100644 --- a/control/velocity_controller/include/velocity_controller/test_VC.hpp +++ b/control/velocity_controller/tests/test_VC.hpp @@ -15,10 +15,8 @@ class test_VC : public rclcpp::Node{ public: test_VC(); //Callback functions - //void read_thrust(geometry_msgs::msg::WrenchStamped::SharedPtr msg); void send_guidance(); void odometry_callback(const nav_msgs::msg::Odometry::SharedPtr msg_ptr); - //void send_state(); //Variables @@ -30,19 +28,15 @@ class test_VC : public rclcpp::Node{ rclcpp::TimerBase::SharedPtr timer_; rclcpp::Clock::SharedPtr clock_; //Messages - //std::vector thrust_vector; vortex_msgs::msg::LOSGuidance reference_msg; //Topics - //std::string topic_odom; - //std::string topic_thrust; std::string topic_guidance; std::string topic_state="/state"; std::string topic_odometry; //MSGS - //nav_msgs::msg::Odometry odom_msg; double time1=0; }; From 8d2707a3ce7c30b0e623b3ed6d1c103df441c846 Mon Sep 17 00:00:00 2001 From: Anbit Date: Wed, 25 Mar 2026 12:17:15 +0100 Subject: [PATCH 199/290] Edit in read me --- guidance/los_guidance/README.md | 1 - 1 file changed, 1 deletion(-) diff --git a/guidance/los_guidance/README.md b/guidance/los_guidance/README.md index af54fdb90..20dd8f38e 100644 --- a/guidance/los_guidance/README.md +++ b/guidance/los_guidance/README.md @@ -324,7 +324,6 @@ However it performs worse when the path contains **sharp turns or rapidly changi | 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` | From e7a3eaa63188a90f7abd4c5636b97d35d9fb2d48 Mon Sep 17 00:00:00 2001 From: Henrik Date: Wed, 25 Mar 2026 12:34:51 +0100 Subject: [PATCH 200/290] cleaned up the program for push --- control/velocity_controller/CMakeLists.txt | 50 +- .../config/nautilus_params.yaml | 25 +- .../config/orca_params.yaml | 27 +- .../include/velocity_controller/LQR_setup.hpp | 4 +- .../velocity_controller/NMPC_acados.hpp | 90 -- .../velocity_controller/NMPC_setup.hpp | 40 - .../velocity_controller.hpp | 19 +- .../velocity_controller/scripts/auv_ocp.json | 1145 ---------------- .../scripts/build_auv_solver/Makefile | 208 --- .../acados_sim_solver_auv_model.c | 310 ----- .../acados_sim_solver_auv_model.h | 105 -- .../build_auv_solver/acados_solver.pxd | 63 - .../acados_solver_auv_model.c | 1187 ----------------- .../acados_solver_auv_model.h | 174 --- .../acados_solver_auv_model.o | Bin 35504 -> 0 bytes .../auv_model_model/auv_model_impl_dae_fun.c | 394 ------ .../auv_model_model/auv_model_impl_dae_fun.o | Bin 11584 -> 0 bytes .../auv_model_impl_dae_fun_jac_x_xdot_u.c | 847 ------------ .../auv_model_impl_dae_fun_jac_x_xdot_u.o | Bin 21328 -> 0 bytes .../auv_model_impl_dae_fun_jac_x_xdot_u_z.c | 851 ------------ .../auv_model_impl_dae_fun_jac_x_xdot_u_z.o | Bin 21944 -> 0 bytes .../auv_model_impl_dae_fun_jac_x_xdot_z.c | 809 ----------- .../auv_model_impl_dae_fun_jac_x_xdot_z.o | Bin 20472 -> 0 bytes .../auv_model_impl_dae_jac_x_xdot_u_z.c | 708 ---------- .../auv_model_impl_dae_jac_x_xdot_u_z.o | Bin 17736 -> 0 bytes .../auv_model_model/auv_model_model.h | 81 -- .../libacados_ocp_solver_auv_model.so | Bin 86256 -> 0 bytes .../scripts/build_auv_solver/main_auv_model.c | 189 --- .../build_auv_solver/main_sim_auv_model.c | 141 -- .../velocity_controller/scripts/dynamics.py | 163 --- .../velocity_controller/scripts/generator.py | 223 ---- control/velocity_controller/src/LQR_setup.cpp | 95 +- .../velocity_controller/src/NMPC_acados.cpp | 183 --- .../velocity_controller/src/NMPC_setup.cpp | 351 ----- .../src/velocity_controller.cpp | 170 +-- .../velocity_controller/tests/LQR_test.cpp | 55 - .../velocity_controller/tests/test_LQR.cpp | 16 +- control/velocity_controller/tests/test_VC.cpp | 4 +- control/velocity_controller/tests/test_VC.hpp | 4 +- 39 files changed, 80 insertions(+), 8651 deletions(-) delete mode 100644 control/velocity_controller/include/velocity_controller/NMPC_acados.hpp delete mode 100644 control/velocity_controller/include/velocity_controller/NMPC_setup.hpp delete mode 100644 control/velocity_controller/scripts/auv_ocp.json delete mode 100644 control/velocity_controller/scripts/build_auv_solver/Makefile delete mode 100644 control/velocity_controller/scripts/build_auv_solver/acados_sim_solver_auv_model.c delete mode 100644 control/velocity_controller/scripts/build_auv_solver/acados_sim_solver_auv_model.h delete mode 100644 control/velocity_controller/scripts/build_auv_solver/acados_solver.pxd delete mode 100644 control/velocity_controller/scripts/build_auv_solver/acados_solver_auv_model.c delete mode 100644 control/velocity_controller/scripts/build_auv_solver/acados_solver_auv_model.h delete mode 100644 control/velocity_controller/scripts/build_auv_solver/acados_solver_auv_model.o delete mode 100644 control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun.c delete mode 100644 control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun.o delete mode 100644 control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u.c delete mode 100644 control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u.o delete mode 100644 control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u_z.c delete mode 100644 control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u_z.o delete mode 100644 control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_z.c delete mode 100644 control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_z.o delete mode 100644 control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_jac_x_xdot_u_z.c delete mode 100644 control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_jac_x_xdot_u_z.o delete mode 100644 control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_model.h delete mode 100755 control/velocity_controller/scripts/build_auv_solver/libacados_ocp_solver_auv_model.so delete mode 100644 control/velocity_controller/scripts/build_auv_solver/main_auv_model.c delete mode 100644 control/velocity_controller/scripts/build_auv_solver/main_sim_auv_model.c delete mode 100644 control/velocity_controller/scripts/dynamics.py delete mode 100644 control/velocity_controller/scripts/generator.py delete mode 100644 control/velocity_controller/src/NMPC_acados.cpp delete mode 100644 control/velocity_controller/src/NMPC_setup.cpp delete mode 100644 control/velocity_controller/tests/LQR_test.cpp diff --git a/control/velocity_controller/CMakeLists.txt b/control/velocity_controller/CMakeLists.txt index 30fd54084..04e477b4a 100644 --- a/control/velocity_controller/CMakeLists.txt +++ b/control/velocity_controller/CMakeLists.txt @@ -9,7 +9,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() -# find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) @@ -18,43 +17,20 @@ find_package(std_msgs REQUIRED) find_package(vortex_msgs REQUIRED) find_package(vortex_utils REQUIRED) find_package(Eigen3 REQUIRED) -#find_package(CasADi REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(ct_optcon REQUIRED) find_package(ct_core REQUIRED) find_package(casadi REQUIRED) -set(ACADOS_ROOT "$ENV{ACADOS_SOURCE_DIR}" CACHE PATH "acados root") include_directories( - ${ACADOS_ROOT}/include - ${ACADOS_ROOT}/include/acados - ${ACADOS_ROOT}/include/acados/ocp_nlp - ${ACADOS_ROOT}/include/acados_c - ${ACADOS_ROOT}/include/blasfeo/include - ${ACADOS_ROOT}/include/hpipm/include - ${ACADOS_ROOT}/include/qpOASES_e - ${CMAKE_CURRENT_SOURCE_DIR}/scripts/build_auv_solver - include - -) -link_directories(${ACADOS_ROOT}/lib) - - -# After setting ACADOS_ROOT etc. -file(GLOB_RECURSE GEN_SRCS - ${CMAKE_CURRENT_SOURCE_DIR}/scripts/build_auv_solver/*.c - ${CMAKE_CURRENT_SOURCE_DIR}/scripts/build_auv_solver/*/*.c -) - - - -add_library(auv_nmpc STATIC - src/NMPC_acados.cpp - ${GEN_SRCS} ) +#target_include_directories(velocity_controller_node PUBLIC +# $ +# $ +#) add_executable(velocity_controller_node src/velocity_controller.cpp @@ -62,18 +38,7 @@ add_executable(velocity_controller_node src/LQR_setup.cpp src/utilities.cpp src/ct_instantiations.cpp - src/NMPC_setup.cpp - #src/NMPC_acados.cpp ) - - - -#add_executable(test_LQR_node -# src/LQR_test.cpp -# src/LQR_setup.cpp -# src/utilities.cpp -# src/ct_instantiations.cpp -# ) ament_target_dependencies(velocity_controller_node rclcpp rclcpp_lifecycle @@ -81,13 +46,11 @@ ament_target_dependencies(velocity_controller_node std_msgs vortex_msgs geometry_msgs - # CasADi nav_msgs vortex_utils ) #target_include_directories(velocity_controller_node PRIVATE casadi Eigen3) -target_link_libraries(auv_nmpc acados hpipm blasfeo qpOASES_e m) -target_link_libraries(velocity_controller_node Eigen3::Eigen casadi::casadi ct_optcon ct_core auv_nmpc) +target_link_libraries(velocity_controller_node Eigen3::Eigen casadi::casadi ct_optcon ct_core) install(TARGETS velocity_controller_node DESTINATION lib/${PROJECT_NAME} @@ -107,7 +70,4 @@ if(BUILD_TESTING) add_subdirectory(tests) endif() - - - ament_package() diff --git a/control/velocity_controller/config/nautilus_params.yaml b/control/velocity_controller/config/nautilus_params.yaml index 3d183cae5..becd032bb 100644 --- a/control/velocity_controller/config/nautilus_params.yaml +++ b/control/velocity_controller/config/nautilus_params.yaml @@ -9,25 +9,16 @@ LQR_params: Q: [200.0,32.84,32.84,15.0,15.0,100.0,32.84,32.84] R: [0.02,3.1,3.10] - NMPCA_params: - Q: [1.0,1.0,1.0,5.0,5.0] # u,q,r,theta,psi - R: [0.1,0.1,0.1] # u_surge, u_theta, u_psi - N: 20 - NMPC_params: - Q: [100.0,0.0,0.0,0.0,1.0,1.0,0.0,5.0,50.0] # u,q,r,theta,psi - R: [0.1,0.1,0.1] # u_surge, u_theta, u_psi - N: 20 - #inertia_matrix: [ 30.0, 0.0, 0.0, 0.0, 0.0, 0.60, 0.0, 30.0, 0.0, 0.0, -0.60, 0.30, 0.0, 0.0, 30.0, 0.60, 0.30, 0.0, 0.0, 0.0, 0.60, 0.68, 0.0, 0.0, 0.0, -0.60, 0.30, 0.0, 3.32, 0.0, 0.60, 0.30, 0.0, 0.0, 0.0, 3.34] dampening_matrix_low: [23.0,0.0,0.0,0.0,0.0,0.0, 0.0,46.0,0.0,0.0,0.0,0.0, 0.0,0.0,46.0,0.0,0.0,0.0, 0.0,0.0,0.0,46.0,0.0,0.0, 0.0,0.0,0.0,0.0,46.0,0.0, 0.0,0.0,0.0,0.0,0.0,46.0] dampening_matrix_high: [1.0,0.0,0.0,0.0,0.0,0.0, 0.0,1.0,0.0,0.0,0.0,0.0, 0.0,0.0,1.0,0.0,0.0,0.0, 0.0,0.0,0.0,1.0,0.0,0.0, 0.0,0.0,0.0,0.0,1.0,0.0, 0.0,0.0,0.0,0.0,0.0,1.0] - publish_rate: 100 #ms - #Clamp parameter - max_force: 99.5 - controller_type: 2 #1 PID 2 LQR 3 NMPC 4NMPC fast - #Q: [300.0,0.01,0.01,0.01,32.84,32.84,32.84,32.84,32.84] # u,v,w,p,q,r,phi,theta,psi - # R: [0.02,3.1,3.10] # u_surge, u_theta, u_psi - - #Fixes: reduce oscillations, follows angles close to wrap around, model restoring forces in pitch, test different references, try to find out why the fuck it went backwards?, + Settings: #Settings for the controller + controller_type: 2 #1 PID 2 LQR + auto_start: false #0 for no, 1 for yes + anti_overshoot: false + reset_on_new_ref: true + odometry_dropout_guard: true + publish_rate: 100 #ms + max_force: 99.5 diff --git a/control/velocity_controller/config/orca_params.yaml b/control/velocity_controller/config/orca_params.yaml index b19015c95..9a111d51a 100644 --- a/control/velocity_controller/config/orca_params.yaml +++ b/control/velocity_controller/config/orca_params.yaml @@ -7,28 +7,21 @@ yaw: [10.0,1.0,5.0] LQR_params: - #Q: [200.0,62.84,62.84,15.0,15.0,10.0,3.84,3.84] - #R: [8.0,100.0,100.0] Q: [200.0,32.84,32.84,15.0,15.0,100.0,32.84,32.84] R: [0.02,3.1,3.10] - NMPCA_params: - Q: [1.0,1.0,1.0,5.0,5.0] # u,q,r,theta,psi - R: [0.1,0.1,0.1] # u_surge, u_theta, u_psi - N: 20 - NMPC_params: - Q: [100.0,0.0,0.0,0.0,1.0,1.0,0.0,5.0,50.0] # u,q,r,theta,psi - R: [0.1,0.1,0.1] # u_surge, u_theta, u_psi - N: 20 dampening_matrix_low: [23.0,0.0,0.0,0.0,0.0,0.0, 0.0,46.0,0.0,0.0,0.0,0.0, 0.0,0.0,46.0,0.0,0.0,0.0, 0.0,0.0,0.0,46.0,0.0,0.0, 0.0,0.0,0.0,0.0,46.0,0.0, 0.0,0.0,0.0,0.0,0.0,46.0] dampening_matrix_high: [1.0,0.0,0.0,0.0,0.0,0.0, 0.0,1.0,0.0,0.0,0.0,0.0, 0.0,0.0,1.0,0.0,0.0,0.0, 0.0,0.0,0.0,1.0,0.0,0.0, 0.0,0.0,0.0,0.0,1.0,0.0, 0.0,0.0,0.0,0.0,0.0,1.0] - publish_rate: 100 #ms - #Clamp parameter - max_force: 99.5 - controller_type: 2 #1 PID 2 LQR 3 NMPC 4NMPC fast - #Q: [300.0,0.01,0.01,0.01,32.84,32.84,32.84,32.84,32.84] # u,v,w,p,q,r,phi,theta,psi - # R: [0.02,3.1,3.10] # u_surge, u_theta, u_psi + Settings: #Settings for the controller + controller_type: 2 #1 PID 2 LQR + auto_start: false #0 for no, 1 for yes + anti_overshoot: false + reset_on_new_ref: true + odometry_dropout_guard: true + publish_rate: 100 #ms + max_force: 99.5 - #Fixes: reduce oscillations, follows angles close to wrap around, model restoring forces in pitch, test different references, try to find out why the fuck it went backwards?, + + diff --git a/control/velocity_controller/include/velocity_controller/LQR_setup.hpp b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp index 3ac78e980..e38cc68cb 100644 --- a/control/velocity_controller/include/velocity_controller/LQR_setup.hpp +++ b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp @@ -43,7 +43,7 @@ class LQRController{ public: LQRController(); - bool set_matrices(std::vector Q_,std::vector R_,std::vector inertia_matrix, double max_force,std::vector water_r_low,std::vector water_r_high); + bool set_matrices(std::vector Q_,std::vector R_,std::vector inertia_matrix, double max_force,std::vector D_low); void reset_controller(int nr=0); bool calculate_thrust(State states, Guidance_data guidance_values); bool set_interval(double interval); @@ -63,7 +63,7 @@ class LQRController{ double integral_error_surge; double integral_error_pitch; double integral_error_yaw; bool surge_windup; bool pitch_windup; bool yaw_windup; Eigen::Matrix Q; Eigen::Matrix R; Eigen::Matrix B; - Eigen::Matrix D_low; Eigen::Matrix D_high; + Eigen::Matrix D; double max_force, mass, Ixx, Iyy,Izz; Eigen::Matrix inertia_matrix_inv; diff --git a/control/velocity_controller/include/velocity_controller/NMPC_acados.hpp b/control/velocity_controller/include/velocity_controller/NMPC_acados.hpp deleted file mode 100644 index a54c495a6..000000000 --- a/control/velocity_controller/include/velocity_controller/NMPC_acados.hpp +++ /dev/null @@ -1,90 +0,0 @@ - -#pragma once -#include -#include -#include -#include -#include -#include - -// acados C API (generated) -extern "C" { -#include "acados_c/ocp_nlp_interface.h" -#include "acados/ocp_nlp/ocp_nlp_common.h" -#include "acados/utils/print.h" - -// Generated solver headers -#include "acados_solver_auv_model.h" // <-- from build_auv_solver/ -} - -class AuvNMPC { -public: - // Adjust sizes if your model differs - static constexpr int NX = AUV_MODEL_NX; // [u v w p q r phi theta psi] - static constexpr int NU = AUV_MODEL_NU; // [Fx My Mz] - static constexpr int NY = AUV_MODEL_NY; - static constexpr int NY_E = AUV_MODEL_NYN; - - -// Pass N if your generated header does not provide _acados_get_N() - explicit AuvNMPC(int N_horizon_override = -1) - : N_override_(N_horizon_override) {} - - ~AuvNMPC(); - - - // Not copyable - AuvNMPC(const AuvNMPC&) = delete; - AuvNMPC& operator=(const AuvNMPC&) = delete; - - // Lifecycle - bool init(); - - - void set_weights(const std::vector& W_diag, const std::vector& W_e_diag); - void set_max_force(double max_force); // updates bound on con_h: Fx^2+My^2+Mz^2 <= max^2 - - // Inputs - void setState(const std::array& x); - void setReference(const std::array& x_ref); - void setWeights(const std::vector& W_diag, const std::vector& W_e_diag); // sizes: NY, NY_E - void setMaxForce(double max_force); // updates con_h bounds - - - // One-shot solve: provide current state, desired state & input refs, get u0 back. - // Returns solver status (0 == success). - int solve_once(); - bool initialize_guess(std::array x,std::array u_init); - - // Outputs - std::vector getU0(); - - private: - - // generated capsule type (from acados_solver_auv_model.h) - auv_model_solver_capsule* capsule_ = nullptr; - - - // acados solver - ocp_nlp_solver* solver_ = nullptr; - ocp_nlp_config* config_ = nullptr; - ocp_nlp_dims* dims_ = nullptr; - ocp_nlp_in* nlp_in_ = nullptr; - ocp_nlp_out* nlp_out_= nullptr; - - int N_=20; - int N_override_=-1; - - std::array W_; // length NY - std::vector W_e_{NY_E*NY_E,0.0}; // length NY_E - double max_force2_ = 100*100; // squared constraint, default 100^2 - - - static void set_diag(double* M, int n, const std::vector& diag); - //U out - std::vector u0_out={0,0,0}; - //Recorded states states - std::array x0; - std::array xr; - std::array ur={0,0,0}; -}; diff --git a/control/velocity_controller/include/velocity_controller/NMPC_setup.hpp b/control/velocity_controller/include/velocity_controller/NMPC_setup.hpp deleted file mode 100644 index 19208d44c..000000000 --- a/control/velocity_controller/include/velocity_controller/NMPC_setup.hpp +++ /dev/null @@ -1,40 +0,0 @@ -#pragma once -//#include -#include -#include -#include "velocity_controller/utilities.hpp" - -class NMPC_controller{ - public: - Eigen::Matrix get_thrust(); - bool calculate_thrust(Guidance_data guidance_values, State state); - bool set_matrices(std::vector Q_,std::vector R_,std::vector inertia_matrix, double max_force,std::vector water_r_low,std::vector water_r_high); - void reset_controller(); - bool set_interval(double interval); - bool initialize_MPC(); - private: - Eigen::Matrix Q_; - Eigen::MatrixR_; - Eigen::MatrixM_inv; - Eigen::MatrixD_low; - Eigen::MatrixD_high; - //Eigen::MatrixB_; - double interval_; - double mass; - double Iz; - double Ix; - double Iy; - int N=3; - int n=9; - int m=3; - casadi::DM Z0_next; //For warm start - casadi::DM lbx; - casadi::DM ubx; - casadi::DM lbg; - casadi::DM ubg; - casadi::DM Pval; - casadi::Function solver; - Eigen::Matrixthrust; - - -}; \ No newline at end of file diff --git a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp index eff738c7c..a81483f9f 100644 --- a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp +++ b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp @@ -11,8 +11,6 @@ #include "LQR_setup.hpp" #include "nav_msgs/msg/odometry.hpp" #include "vortex_msgs/msg/los_guidance.hpp" -#include "velocity_controller/NMPC_setup.hpp" -#include "velocity_controller/NMPC_acados.hpp" #include #include @@ -84,22 +82,13 @@ class Velocity_node : public rclcpp_lifecycle::LifecycleNode{ std::vector inertia_matrix; std::vector dampening_matrix_low; std::vector dampening_matrix_high; - //NMPC controller - NMPC_controller NMPC; - //NMPC acados - AuvNMPC NMPC_acados; - std::vector Q2; - std::vector R2; - //NMPC parameters - std::vector Q3; - std::vector R3; std::atomic_bool should_exit_{false}; //VC settings - bool reset_on_new_ref=true; - bool anti_overshoot=false; - bool auto_start=false; - bool odometry_dropout_guard=true; + bool reset_on_new_ref; + bool anti_overshoot; + bool auto_start; + bool odometry_dropout_guard; int publish_counter=0; bool first_start=true; diff --git a/control/velocity_controller/scripts/auv_ocp.json b/control/velocity_controller/scripts/auv_ocp.json deleted file mode 100644 index 1becb5b1f..000000000 --- a/control/velocity_controller/scripts/auv_ocp.json +++ /dev/null @@ -1,1145 +0,0 @@ -{ - "code_gen_opts": { - "acados_include_path": "/home/henrik/ros2_ws_v/src/acados/include", - "acados_lib_path": "/home/henrik/ros2_ws_v/src/acados/lib", - "acados_link_libs": { - "clarabel": "", - "daqp": "", - "hpmpc": "", - "ooqp": "", - "openmp": "-fopenmp", - "osqp": "-losqp", - "qpdunes": "", - "qpoases": "-lqpOASES_e" - }, - "acados_version": "508482dac", - "code_export_directory": "/home/henrik/ros2_ws_v/src/vortex-auv/control/velocity_controller/scripts/build_auv_solver", - "cython_include_dirs": [ - "/usr/lib/python3/dist-packages/numpy/core/include", - "/usr/include/python3.10" - ], - "json_file": "auv_ocp.json", - "os": "unix", - "shared_lib_ext": ".so" - }, - "constraints": { - "C": [], - "C_e": [], - "D": [], - "constr_type": "BGH", - "constr_type_0": "BGH", - "constr_type_e": "BGH", - "constr_types": [ - "BGH", - "BGP" - ], - "has_x0": true, - "idxbu": [ - 0, - 1, - 2 - ], - "idxbx": [ - 7 - ], - "idxbx_0": [ - 0, - 1, - 2, - 3, - 4, - 5, - 6, - 7, - 8 - ], - "idxbx_e": [], - "idxbxe_0": [ - 0, - 1, - 2, - 3, - 4, - 5, - 6, - 7, - 8 - ], - "idxs_rev": [], - "idxs_rev_0": [], - "idxs_rev_e": [], - "idxsbu": [], - "idxsbx": [], - "idxsbx_e": [], - "idxsg": [], - "idxsg_e": [], - "idxsh": [], - "idxsh_0": [], - "idxsh_e": [], - "idxsphi": [], - "idxsphi_0": [], - "idxsphi_e": [], - "lbu": [ - -99.5, - -99.5, - -99.5 - ], - "lbx": [ - -1.4 - ], - "lbx_0": [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - "lbx_e": [], - "lg": [], - "lg_e": [], - "lh": [], - "lh_0": [], - "lh_e": [], - "lphi": [], - "lphi_0": [], - "lphi_e": [], - "ls": [], - "ls_0": [], - "ls_e": [], - "lsbu": [], - "lsbx": [], - "lsbx_e": [], - "lsg": [], - "lsg_e": [], - "lsh": [], - "lsh_0": [], - "lsh_e": [], - "lsphi": [], - "lsphi_0": [], - "lsphi_e": [], - "ubu": [ - 99.5, - 99.5, - 99.5 - ], - "ubx": [ - 1.4 - ], - "ubx_0": [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - "ubx_e": [], - "ug": [], - "ug_e": [], - "uh": [], - "uh_0": [], - "uh_e": [], - "uphi": [], - "uphi_0": [], - "uphi_e": [], - "us": [], - "us_0": [], - "us_e": [], - "usbu": [], - "usbx": [], - "usbx_e": [], - "usg": [], - "usg_e": [], - "ush": [], - "ush_0": [], - "ush_e": [], - "usphi": [], - "usphi_0": [], - "usphi_e": [] - }, - "cost": { - "Vu": [ - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 1.0, - 0.0, - 0.0 - ], - [ - 0.0, - 1.0, - 0.0 - ], - [ - 0.0, - 0.0, - 1.0 - ] - ], - "Vu_0": [ - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 1.0, - 0.0, - 0.0 - ], - [ - 0.0, - 1.0, - 0.0 - ], - [ - 0.0, - 0.0, - 1.0 - ] - ], - "Vx": [ - [ - 1.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 1.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 1.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 1.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 1.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ] - ], - "Vx_0": [ - [ - 1.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 1.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 1.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 1.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 1.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ] - ], - "Vx_e": [ - [ - 1.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 1.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 1.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 1.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 1.0 - ] - ], - "Vz": [], - "Vz_0": [], - "W": [ - [ - 100.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 1.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.1, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.1, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.1 - ] - ], - "W_0": [ - [ - 100.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 1.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.1, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.1, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.1 - ] - ], - "W_e": [ - [ - 100.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 1.0 - ] - ], - "Zl": [], - "Zl_0": [], - "Zl_e": [], - "Zu": [], - "Zu_0": [], - "Zu_e": [], - "cost_ext_fun_type": "casadi", - "cost_ext_fun_type_0": "casadi", - "cost_ext_fun_type_e": "casadi", - "cost_ext_fun_types": [ - "casadi", - "generic" - ], - "cost_function_ext_cost": null, - "cost_function_ext_cost_0": null, - "cost_function_ext_cost_e": null, - "cost_source_ext_cost": null, - "cost_source_ext_cost_0": null, - "cost_source_ext_cost_e": null, - "cost_type": "LINEAR_LS", - "cost_type_0": "LINEAR_LS", - "cost_type_e": "LINEAR_LS", - "cost_types": [ - "LINEAR_LS", - "NONLINEAR_LS", - "EXTERNAL", - "CONVEX_OVER_NONLINEAR", - "AUTO" - ], - "yref": [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - "yref_0": [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - "yref_e": [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - "zl": [], - "zl_0": [], - "zl_e": [], - "zu": [], - "zu_0": [], - "zu_e": [] - }, - "dims": { - "N": 20, - "n_global_data": 0, - "nbu": 3, - "nbx": 1, - "nbx_0": 9, - "nbx_e": 0, - "nbxe_0": 9, - "ng": 0, - "ng_e": 0, - "nh": 0, - "nh_0": 0, - "nh_e": 0, - "np": 0, - "np_global": 0, - "nphi": 0, - "nphi_0": 0, - "nphi_e": 0, - "nr": 0, - "nr_0": 0, - "nr_e": 0, - "ns": 0, - "ns_0": 0, - "ns_e": 0, - "nsbu": 0, - "nsbx": 0, - "nsbx_e": 0, - "nsg": 0, - "nsg_e": 0, - "nsh": 0, - "nsh_0": 0, - "nsh_e": 0, - "nsphi": 0, - "nsphi_0": 0, - "nsphi_e": 0, - "nu": 3, - "nx": 9, - "nx_next": 9, - "ny": 8, - "ny_0": 8, - "ny_e": 5, - "nz": 0 - }, - "external_function_files_model": [ - "auv_model_model/auv_model_impl_dae_fun.c", - "auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_z.c", - "auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u_z.c", - "auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u.c", - "auv_model_model/auv_model_impl_dae_jac_x_xdot_u_z.c" - ], - "external_function_files_ocp": [], - "hash": "78dcc9b0fd627d3908081bfa461a62e0", - "model": { - "con_h_expr": [], - "con_h_expr_0": [], - "con_h_expr_e": [], - "con_phi_expr": [], - "con_phi_expr_0": [], - "con_phi_expr_e": [], - "con_r_expr": [], - "con_r_expr_0": [], - "con_r_expr_e": [], - "con_r_in_phi": [], - "con_r_in_phi_0": [], - "con_r_in_phi_e": [], - "cost_conl_custom_outer_hess": [], - "cost_conl_custom_outer_hess_0": [], - "cost_conl_custom_outer_hess_e": [], - "cost_expr_ext_cost": [], - "cost_expr_ext_cost_0": [], - "cost_expr_ext_cost_custom_hess": [], - "cost_expr_ext_cost_custom_hess_0": [], - "cost_expr_ext_cost_custom_hess_e": [], - "cost_expr_ext_cost_e": [], - "cost_psi_expr": [], - "cost_psi_expr_0": [], - "cost_psi_expr_e": [], - "cost_r_in_psi_expr": [], - "cost_r_in_psi_expr_0": [], - "cost_r_in_psi_expr_e": [], - "cost_y_expr": [], - "cost_y_expr_0": [], - "cost_y_expr_e": [], - "disc_dyn_custom_hess_ux_expr": [], - "disc_dyn_custom_jac_ux_expr": [], - "disc_dyn_expr": [], - "dyn_disc_fun": null, - "dyn_disc_fun_jac": null, - "dyn_disc_fun_jac_hess": null, - "dyn_ext_fun_type": "casadi", - "dyn_generic_source": null, - "dyn_impl_dae_fun": null, - "dyn_impl_dae_fun_jac": null, - "dyn_impl_dae_jac": null, - "expression_names": [ - "f_expl_expr", - "f_impl_expr", - "p", - "p_global", - "pi", - "u", - "x", - "xdot", - "z" - ], - "f_expl_expr": "SX(@1=-30, @2=30, @3=((Fx-(((@1*r)*v)+((@2*q)*w)))-((23+fabs(u))*u)), @4=46, @5=((((@2*r)*u)+((@1*p)*w))+((@4+fabs(v))*v)), @6=((((@1*q)*u)+((@2*p)*v))+((@4+fabs(w))*w)), @7=((((3.34*r)*q)+((-3.32*q)*r))+((@4+fabs(p))*p)), @8=((My-(((-3.34*r)*p)+((0.68*p)*r)))-((@4+fabs(q))*q)), @9=((Mz-(((3.32*q)*p)+((-0.68*p)*q)))-((@4+fabs(r))*r)), @10=-0.000546005, [((((((0.0334536*@3)-(6.0369e-05*@5))-(-1.11163e-07*@6))-(9.80847e-08*@7))+(1.09201e-05*@8))+(-0.00601506*@9)), ((((((6.0369e-05*@3)-(0.0334847*@5))-(-6.16582e-05*@6))-(5.44043e-05*@7))+(0.00605702*@8))+(-0.00301845*@9)), ((((((-1.11163e-07*@3)-(-6.16582e-05*@5))-(0.0339635*@6))-(-0.0299678*@7))+(-0.00308013*@8))+(5.55814e-06*@9)), ((((((9.80847e-08*@3)-(5.44043e-05*@5))-(-0.0299678*@6))-(1.49703*@7))+(0.00271776*@8))+(-4.90424e-06*@9)), ((((((1.09201e-05*@3)-(0.00605702*@5))-(-0.00308013*@6))-(0.00271776*@7))+(0.302578*@8))+(@10*@9)), ((((((-0.00601506*@3)-(-0.00301845*@5))-(5.55814e-06*@6))-(-4.90424e-06*@7))+(@10*@8))+(0.300753*@9)), ((p+((sin(phi)*tan(theta))*q))+((cos(phi)*tan(theta))*r)), ((cos(phi)*q)-(sin(phi)*r)), (((sin(phi)/cos(theta))*q)+((cos(phi)/cos(theta))*r))])", - "f_impl_expr": "SX(@1=-30, @2=30, @3=((Fx-(((@1*r)*v)+((@2*q)*w)))-((23+fabs(u))*u)), @4=46, @5=((((@2*r)*u)+((@1*p)*w))+((@4+fabs(v))*v)), @6=((((@1*q)*u)+((@2*p)*v))+((@4+fabs(w))*w)), @7=((((3.34*r)*q)+((-3.32*q)*r))+((@4+fabs(p))*p)), @8=((My-(((-3.34*r)*p)+((0.68*p)*r)))-((@4+fabs(q))*q)), @9=((Mz-(((3.32*q)*p)+((-0.68*p)*q)))-((@4+fabs(r))*r)), @10=-0.000546005, [(xdot_0-((((((0.0334536*@3)-(6.0369e-05*@5))-(-1.11163e-07*@6))-(9.80847e-08*@7))+(1.09201e-05*@8))+(-0.00601506*@9))), (xdot_1-((((((6.0369e-05*@3)-(0.0334847*@5))-(-6.16582e-05*@6))-(5.44043e-05*@7))+(0.00605702*@8))+(-0.00301845*@9))), (xdot_2-((((((-1.11163e-07*@3)-(-6.16582e-05*@5))-(0.0339635*@6))-(-0.0299678*@7))+(-0.00308013*@8))+(5.55814e-06*@9))), (xdot_3-((((((9.80847e-08*@3)-(5.44043e-05*@5))-(-0.0299678*@6))-(1.49703*@7))+(0.00271776*@8))+(-4.90424e-06*@9))), (xdot_4-((((((1.09201e-05*@3)-(0.00605702*@5))-(-0.00308013*@6))-(0.00271776*@7))+(0.302578*@8))+(@10*@9))), (xdot_5-((((((-0.00601506*@3)-(-0.00301845*@5))-(5.55814e-06*@6))-(-4.90424e-06*@7))+(@10*@8))+(0.300753*@9))), (xdot_6-((p+((sin(phi)*tan(theta))*q))+((cos(phi)*tan(theta))*r))), (xdot_7-((cos(phi)*q)-(sin(phi)*r))), (xdot_8-(((sin(phi)/cos(theta))*q)+((cos(phi)/cos(theta))*r)))])", - "gnsf_model": null, - "name": "auv_model", - "nu_original": null, - "p": "SX(0x1)", - "p_global": "SX(0x1)", - "pi": "SX([pi_0, pi_1, pi_2, pi_3, pi_4, pi_5, pi_6, pi_7, pi_8])", - "serialized_expressions": "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", - "t": [], - "t0": null, - "t_label": "t", - "u": "SX([Fx, My, Mz])", - "u_labels": [ - "u0", - "u1", - "u2" - ], - "x": "SX([u, v, w, p, q, r, phi, theta, psi])", - "x_labels": [ - "x0", - "x1", - "x2", - "x3", - "x4", - "x5", - "x6", - "x7", - "x8" - ], - "xdot": "SX([xdot_0, xdot_1, xdot_2, xdot_3, xdot_4, xdot_5, xdot_6, xdot_7, xdot_8])", - "z": "SX(0x1)" - }, - "name": "auv_model", - "p_global_values": [], - "parameter_values": [], - "problem_class": "OCP", - "ros_opts": null, - "simulink_opts": null, - "solver_options": { - "N_horizon": 20, - "Tsim": 0.1, - "adaptive_levenberg_marquardt_lam": 5.0, - "adaptive_levenberg_marquardt_mu0": 0.001, - "adaptive_levenberg_marquardt_mu_min": 1e-16, - "adaptive_levenberg_marquardt_obj_scalar": 2.0, - "allow_direction_mode_switch_to_nominal": true, - "anderson_activation_threshold": 10.0, - "as_rti_iter": 1, - "as_rti_level": 4, - "byrd_omojokon_slack_relaxation_factor": 1.00001, - "collocation_type": "GAUSS_LEGENDRE", - "cost_discretization": "EULER", - "cost_scaling": [ - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 1.0 - ], - "custom_templates": [], - "custom_update_copy": true, - "custom_update_filename": "", - "custom_update_header_filename": "", - "eval_residual_at_max_iter": false, - "exact_hess_constr": 1, - "exact_hess_cost": 1, - "exact_hess_dyn": 1, - "ext_cost_num_hess": 0, - "ext_fun_compile_flags": "-O2", - "ext_fun_expand_constr": false, - "ext_fun_expand_cost": false, - "ext_fun_expand_dyn": false, - "ext_fun_expand_precompute": false, - "fixed_hess": 0, - "globalization": "FIXED_STEP", - "globalization_alpha_min": 0.05, - "globalization_alpha_reduction": 0.7, - "globalization_eps_sufficient_descent": 0.0001, - "globalization_fixed_step_length": 1.0, - "globalization_full_step_dual": 0, - "globalization_funnel_fraction_switching_condition": 0.001, - "globalization_funnel_init_increase_factor": 15.0, - "globalization_funnel_init_upper_bound": 1.0, - "globalization_funnel_initial_penalty_parameter": 1.0, - "globalization_funnel_kappa": 0.9, - "globalization_funnel_sufficient_decrease_factor": 0.9, - "globalization_funnel_use_merit_fun_only": false, - "globalization_line_search_use_sufficient_descent": 0, - "globalization_use_SOC": 0, - "hessian_approx": "GAUSS_NEWTON", - "hpipm_mode": "BALANCE", - "integrator_type": "IRK", - "levenberg_marquardt": 0.01, - "log_dual_step_norm": false, - "log_primal_step_norm": false, - "model_external_shared_lib_dir": null, - "model_external_shared_lib_name": null, - "nlp_qp_tol_min_comp": 1e-11, - "nlp_qp_tol_min_eq": 1e-10, - "nlp_qp_tol_min_ineq": 1e-10, - "nlp_qp_tol_min_stat": 1e-09, - "nlp_qp_tol_reduction_factor": 0.1, - "nlp_qp_tol_safety_factor": 0.1, - "nlp_qp_tol_strategy": "FIXED_QP_TOL", - "nlp_solver_ext_qp_res": 0, - "nlp_solver_max_iter": 100, - "nlp_solver_tol_comp": 1e-06, - "nlp_solver_tol_eq": 1e-06, - "nlp_solver_tol_ineq": 1e-06, - "nlp_solver_tol_min_step_norm": 0.0, - "nlp_solver_tol_stat": 1e-06, - "nlp_solver_type": "SQP", - "nlp_solver_warm_start_first_qp": false, - "nlp_solver_warm_start_first_qp_from_nlp": false, - "print_level": 2, - "qp_solver": "FULL_CONDENSING_HPIPM", - "qp_solver_cond_N": 20, - "qp_solver_cond_block_size": null, - "qp_solver_cond_ric_alg": 1, - "qp_solver_iter_max": 50, - "qp_solver_mu0": 0.0, - "qp_solver_ric_alg": 1, - "qp_solver_t0_init": 2, - "qp_solver_tol_comp": null, - "qp_solver_tol_eq": null, - "qp_solver_tol_ineq": null, - "qp_solver_tol_stat": null, - "qp_solver_warm_start": 1, - "qpscaling_lb_norm_inf_grad_obj": 0.0001, - "qpscaling_scale_constraints": "NO_CONSTRAINT_SCALING", - "qpscaling_scale_objective": "NO_OBJECTIVE_SCALING", - "qpscaling_ub_max_abs_eig": 100000.0, - "reg_adaptive_eps": false, - "reg_epsilon": 0.0001, - "reg_max_cond_block": 10000000.0, - "reg_min_epsilon": 1e-08, - "regularize_method": "NO_REGULARIZE", - "rti_log_only_available_residuals": 0, - "rti_log_residuals": 0, - "search_direction_mode": "NOMINAL_QP", - "sens_forw_p": false, - "shooting_nodes": [ - 0.0, - 0.1, - 0.2, - 0.30000000000000004, - 0.4, - 0.5, - 0.6, - 0.7, - 0.7999999999999999, - 0.8999999999999999, - 0.9999999999999999, - 1.0999999999999999, - 1.2, - 1.3, - 1.4000000000000001, - 1.5000000000000002, - 1.6000000000000003, - 1.7000000000000004, - 1.8000000000000005, - 1.9000000000000006, - 2.0000000000000004 - ], - "sim_method_jac_reuse": [ - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0 - ], - "sim_method_newton_iter": 3, - "sim_method_newton_tol": 0.0, - "sim_method_num_stages": [ - 2, - 2, - 2, - 2, - 2, - 2, - 2, - 2, - 2, - 2, - 2, - 2, - 2, - 2, - 2, - 2, - 2, - 2, - 2, - 2 - ], - "sim_method_num_steps": [ - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4 - ], - "solution_sens_qp_t_lam_min": 1e-09, - "store_iterates": false, - "tau_min": 0.0, - "tf": 2.0, - "time_steps": [ - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1, - 0.1 - ], - "timeout_heuristic": "LAST", - "timeout_max_time": 0.0, - "use_constraint_hessian_in_feas_qp": false, - "with_adaptive_levenberg_marquardt": false, - "with_anderson_acceleration": false, - "with_batch_functionality": false, - "with_solution_sens_wrt_params": false, - "with_value_sens_wrt_params": false - }, - "zoro_description": null -} \ No newline at end of file diff --git a/control/velocity_controller/scripts/build_auv_solver/Makefile b/control/velocity_controller/scripts/build_auv_solver/Makefile deleted file mode 100644 index 7e20d181d..000000000 --- a/control/velocity_controller/scripts/build_auv_solver/Makefile +++ /dev/null @@ -1,208 +0,0 @@ -# -# Copyright (c) The acados authors. -# -# This file is part of acados. -# -# The 2-Clause BSD License -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# 1. Redistributions of source code must retain the above copyright notice, -# this list of conditions and the following disclaimer. -# -# 2. Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE.; -# - - - - - -# define sources and use make's implicit rules to generate object files (*.o) - -# model -MODEL_SRC= - -MODEL_SRC+= auv_model_model/auv_model_impl_dae_fun.c -MODEL_SRC+= auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_z.c -MODEL_SRC+= auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u_z.c -MODEL_SRC+= auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u.c -MODEL_SRC+= auv_model_model/auv_model_impl_dae_jac_x_xdot_u_z.c - -MODEL_OBJ := $(MODEL_SRC:.c=.o) -# optimal control problem - mostly CasADi exports -OCP_SRC= - - - -OCP_SRC+= acados_solver_auv_model.c - -OCP_OBJ := $(OCP_SRC:.c=.o) -# for sim solver -SIM_SRC= acados_sim_solver_auv_model.c -SIM_OBJ := $(SIM_SRC:.c=.o) - -# for target example_sim -EX_SIM_SRC= main_sim_auv_model.c -EX_SIM_OBJ := $(EX_SIM_SRC:.c=.o) -EX_SIM_EXE := $(EX_SIM_SRC:.c=) -# for target example -EX_SRC= main_auv_model.c -EX_OBJ := $(EX_SRC:.c=.o) -EX_EXE := $(EX_SRC:.c=) - - -# combine model, (potentially) sim and ocp object files -OBJ= -OBJ+= $(MODEL_OBJ) -OBJ+= $(SIM_OBJ) -OBJ+= $(OCP_OBJ) - -EXTERNAL_DIR= -EXTERNAL_LIB= - -INCLUDE_PATH = /home/henrik/ros2_ws_v/src/acados/include -LIB_PATH = /home/henrik/ros2_ws_v/src/acados/lib - -# preprocessor flags for make's implicit rules -CPPFLAGS+= -I$(INCLUDE_PATH) -CPPFLAGS+= -I$(INCLUDE_PATH)/acados -CPPFLAGS+= -I$(INCLUDE_PATH)/blasfeo/include -CPPFLAGS+= -I$(INCLUDE_PATH)/hpipm/include - - -# define the c-compiler flags for make's implicit rules -CFLAGS = -fPIC -std=c99 -O2 - -# # Debugging -# CFLAGS += -g3 -fno-diagnostics-show-line-numbers -g - -# linker flags -LDFLAGS+= -L$(LIB_PATH) - - -# link to libraries -LDLIBS+= -lacados -LDLIBS+= -lhpipm -LDLIBS+= -lblasfeo -LDLIBS+= -lm -LDLIBS+= - -# libraries -LIBACADOS_SOLVER=libacados_solver_auv_model.so -LIBACADOS_OCP_SOLVER=libacados_ocp_solver_auv_model.so -LIBACADOS_SIM_SOLVER=lib$(SIM_SRC:.c=.so) - -# virtual targets -.PHONY : all clean - - all: clean example_sim example -shared_lib: bundled_shared_lib ocp_shared_lib sim_shared_lib - -# some linker targets -example: $(EX_OBJ) $(OBJ) - $(CC) $^ -o $(EX_EXE) $(LDFLAGS) $(LDLIBS) - -example_sim: $(EX_SIM_OBJ) $(MODEL_OBJ) $(SIM_OBJ) - $(CC) $^ -o $(EX_SIM_EXE) $(LDFLAGS) $(LDLIBS) - -sim_shared_lib: $(SIM_OBJ) $(MODEL_OBJ) - $(CC) -shared $^ -o $(LIBACADOS_SIM_SOLVER) $(LDFLAGS) $(LDLIBS) -bundled_shared_lib: $(OBJ) - $(CC) -shared $^ -o $(LIBACADOS_SOLVER) $(LDFLAGS) $(LDLIBS) - -ocp_shared_lib: $(OCP_OBJ) $(MODEL_OBJ) - $(CC) -shared $^ -o $(LIBACADOS_OCP_SOLVER) $(LDFLAGS) $(LDLIBS) \ - -L$(EXTERNAL_DIR) -l$(EXTERNAL_LIB) - - -# Cython targets -ocp_cython_c: ocp_shared_lib - cython \ - -o acados_ocp_solver_pyx.c \ - -I $(INCLUDE_PATH)/../interfaces/acados_template/acados_template \ - $(INCLUDE_PATH)/../interfaces/acados_template/acados_template/acados_ocp_solver_pyx.pyx \ - -I /home/henrik/ros2_ws_v/src/vortex-auv/control/velocity_controller/scripts/build_auv_solver \ - -ocp_cython_o: ocp_cython_c - $(CC) $(ACADOS_FLAGS) -c -O2 \ - -fPIC \ - -DNPY_NO_DEPRECATED_API=NPY_1_7_API_VERSION \ - -o acados_ocp_solver_pyx.o \ - -I $(INCLUDE_PATH)/blasfeo/include/ \ - -I $(INCLUDE_PATH)/hpipm/include/ \ - -I $(INCLUDE_PATH) \ - -I /usr/lib/python3/dist-packages/numpy/core/include \ - -I /usr/include/python3.10 \ - acados_ocp_solver_pyx.c \ - -ocp_cython: ocp_cython_o - $(CC) $(ACADOS_FLAGS) -shared \ - -o acados_ocp_solver_pyx.so \ - -Wl,-rpath=$(LIB_PATH) \ - acados_ocp_solver_pyx.o \ - $(abspath .)/libacados_ocp_solver_auv_model.so \ - $(LDFLAGS) $(LDLIBS) - - -# Sim Cython targets -sim_cython_c: sim_shared_lib - cython \ - -o acados_sim_solver_pyx.c \ - -I $(INCLUDE_PATH)/../interfaces/acados_template/acados_template \ - $(INCLUDE_PATH)/../interfaces/acados_template/acados_template/acados_sim_solver_pyx.pyx \ - -I /home/henrik/ros2_ws_v/src/vortex-auv/control/velocity_controller/scripts/build_auv_solver \ - -sim_cython_o: sim_cython_c - $(CC) $(ACADOS_FLAGS) -c -O2 \ - -fPIC \ - -DNPY_NO_DEPRECATED_API=NPY_1_7_API_VERSION \ - -o acados_sim_solver_pyx.o \ - -I $(INCLUDE_PATH)/blasfeo/include/ \ - -I $(INCLUDE_PATH)/hpipm/include/ \ - -I $(INCLUDE_PATH) \ - -I /usr/lib/python3/dist-packages/numpy/core/include \ - -I /usr/include/python3.10 \ - acados_sim_solver_pyx.c \ - -sim_cython: sim_cython_o - $(CC) $(ACADOS_FLAGS) -shared \ - -o acados_sim_solver_pyx.so \ - -Wl,-rpath=$(LIB_PATH) \ - acados_sim_solver_pyx.o \ - $(abspath .)/libacados_sim_solver_auv_model.so \ - $(LDFLAGS) $(LDLIBS) - -clean: - $(RM) $(OBJ) $(EX_OBJ) $(EX_SIM_OBJ) - $(RM) $(LIBACADOS_SOLVER) $(LIBACADOS_OCP_SOLVER) $(LIBACADOS_SIM_SOLVER) - $(RM) $(EX_EXE) $(EX_SIM_EXE) -clean_ocp_shared_lib: - $(RM) $(LIBACADOS_OCP_SOLVER) - $(RM) $(OCP_OBJ) - -clean_ocp_cython: - $(RM) libacados_ocp_solver_auv_model.so - $(RM) acados_solver_auv_model.o - $(RM) acados_ocp_solver_pyx.so - $(RM) acados_ocp_solver_pyx.o - -clean_sim_cython: - $(RM) libacados_sim_solver_auv_model.so - $(RM) acados_sim_solver_auv_model.o - $(RM) acados_sim_solver_pyx.so - $(RM) acados_sim_solver_pyx.o diff --git a/control/velocity_controller/scripts/build_auv_solver/acados_sim_solver_auv_model.c b/control/velocity_controller/scripts/build_auv_solver/acados_sim_solver_auv_model.c deleted file mode 100644 index 3da4f7b13..000000000 --- a/control/velocity_controller/scripts/build_auv_solver/acados_sim_solver_auv_model.c +++ /dev/null @@ -1,310 +0,0 @@ -/* - * Copyright (c) The acados authors. - * - * This file is part of acados. - * - * The 2-Clause BSD License - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE.; - */ -// standard -#include -#include - -// acados -#include "acados_c/external_function_interface.h" -#include "acados_c/sim_interface.h" -#include "acados_c/external_function_interface.h" - -#include "acados/sim/sim_common.h" -#include "acados/utils/external_function_generic.h" -#include "acados/utils/print.h" - - -// example specific -#include "auv_model_model/auv_model_model.h" -#include "acados_sim_solver_auv_model.h" - - -// ** solver data ** - -auv_model_sim_solver_capsule * auv_model_acados_sim_solver_create_capsule() -{ - void* capsule_mem = malloc(sizeof(auv_model_sim_solver_capsule)); - auv_model_sim_solver_capsule *capsule = (auv_model_sim_solver_capsule *) capsule_mem; - - return capsule; -} - - -int auv_model_acados_sim_solver_free_capsule(auv_model_sim_solver_capsule * capsule) -{ - free(capsule); - return 0; -} - - -int auv_model_acados_sim_create(auv_model_sim_solver_capsule * capsule) -{ - // initialize - const int nx = AUV_MODEL_NX; - const int nu = AUV_MODEL_NU; - const int nz = AUV_MODEL_NZ; - const int np = AUV_MODEL_NP; - bool tmp_bool; - - double Tsim = 0.1; - - capsule->acados_sim_mem = NULL; - - external_function_opts ext_fun_opts; - external_function_opts_set_to_default(&ext_fun_opts); - ext_fun_opts.external_workspace = false; - - - capsule->sim_impl_dae_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); - capsule->sim_impl_dae_fun_jac_x_xdot_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); - capsule->sim_impl_dae_jac_x_xdot_u_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); - - - capsule->sim_impl_dae_jac_p = NULL; - - // external functions (implicit model) - capsule->sim_impl_dae_fun->casadi_fun = &auv_model_impl_dae_fun; - capsule->sim_impl_dae_fun->casadi_work = &auv_model_impl_dae_fun_work; - capsule->sim_impl_dae_fun->casadi_sparsity_in = &auv_model_impl_dae_fun_sparsity_in; - capsule->sim_impl_dae_fun->casadi_sparsity_out = &auv_model_impl_dae_fun_sparsity_out; - capsule->sim_impl_dae_fun->casadi_n_in = &auv_model_impl_dae_fun_n_in; - capsule->sim_impl_dae_fun->casadi_n_out = &auv_model_impl_dae_fun_n_out; - external_function_param_casadi_create(capsule->sim_impl_dae_fun, np, &ext_fun_opts); - - capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_fun = &auv_model_impl_dae_fun_jac_x_xdot_z; - capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_work = &auv_model_impl_dae_fun_jac_x_xdot_z_work; - capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_sparsity_in = &auv_model_impl_dae_fun_jac_x_xdot_z_sparsity_in; - capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_sparsity_out = &auv_model_impl_dae_fun_jac_x_xdot_z_sparsity_out; - capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_n_in = &auv_model_impl_dae_fun_jac_x_xdot_z_n_in; - capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_n_out = &auv_model_impl_dae_fun_jac_x_xdot_z_n_out; - external_function_param_casadi_create(capsule->sim_impl_dae_fun_jac_x_xdot_z, np, &ext_fun_opts); - - capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_fun = &auv_model_impl_dae_jac_x_xdot_u_z; - capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_work = &auv_model_impl_dae_jac_x_xdot_u_z_work; - capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_sparsity_in = &auv_model_impl_dae_jac_x_xdot_u_z_sparsity_in; - capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_sparsity_out = &auv_model_impl_dae_jac_x_xdot_u_z_sparsity_out; - capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_n_in = &auv_model_impl_dae_jac_x_xdot_u_z_n_in; - capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_n_out = &auv_model_impl_dae_jac_x_xdot_u_z_n_out; - external_function_param_casadi_create(capsule->sim_impl_dae_jac_x_xdot_u_z, np, &ext_fun_opts); - - - - - - // sim plan & config - sim_solver_plan_t plan; - plan.sim_solver = IRK; - - // create correct config based on plan - sim_config * auv_model_sim_config = sim_config_create(plan); - capsule->acados_sim_config = auv_model_sim_config; - - // sim dims - void *auv_model_sim_dims = sim_dims_create(auv_model_sim_config); - capsule->acados_sim_dims = auv_model_sim_dims; - sim_dims_set(auv_model_sim_config, auv_model_sim_dims, "nx", &nx); - sim_dims_set(auv_model_sim_config, auv_model_sim_dims, "nu", &nu); - sim_dims_set(auv_model_sim_config, auv_model_sim_dims, "nz", &nz); - sim_dims_set(auv_model_sim_config, auv_model_sim_dims, "np", &np); - - - // sim opts - sim_opts *auv_model_sim_opts = sim_opts_create(auv_model_sim_config, auv_model_sim_dims); - capsule->acados_sim_opts = auv_model_sim_opts; - int tmp_int = 3; - sim_opts_set(auv_model_sim_config, auv_model_sim_opts, "newton_iter", &tmp_int); - double tmp_double = 0; - sim_opts_set(auv_model_sim_config, auv_model_sim_opts, "newton_tol", &tmp_double); - sim_collocation_type collocation_type = GAUSS_LEGENDRE; - sim_opts_set(auv_model_sim_config, auv_model_sim_opts, "collocation_type", &collocation_type); - - - tmp_int = 2; - sim_opts_set(auv_model_sim_config, auv_model_sim_opts, "num_stages", &tmp_int); - tmp_int = 4; - sim_opts_set(auv_model_sim_config, auv_model_sim_opts, "num_steps", &tmp_int); - tmp_bool = 0; - sim_opts_set(auv_model_sim_config, auv_model_sim_opts, "jac_reuse", &tmp_bool); - - - // sim in / out - sim_in *auv_model_sim_in = sim_in_create(auv_model_sim_config, auv_model_sim_dims); - capsule->acados_sim_in = auv_model_sim_in; - sim_out *auv_model_sim_out = sim_out_create(auv_model_sim_config, auv_model_sim_dims); - capsule->acados_sim_out = auv_model_sim_out; - - sim_in_set(auv_model_sim_config, auv_model_sim_dims, - auv_model_sim_in, "T", &Tsim); - - // model functions - auv_model_sim_config->model_set(auv_model_sim_in->model, - "impl_ode_fun", capsule->sim_impl_dae_fun); - auv_model_sim_config->model_set(auv_model_sim_in->model, - "impl_ode_fun_jac_x_xdot", capsule->sim_impl_dae_fun_jac_x_xdot_z); - auv_model_sim_config->model_set(auv_model_sim_in->model, - "impl_ode_jac_x_xdot_u", capsule->sim_impl_dae_jac_x_xdot_u_z); - - - // sim solver - sim_solver *auv_model_sim_solver = sim_solver_create(auv_model_sim_config, - auv_model_sim_dims, auv_model_sim_opts, auv_model_sim_in); - capsule->acados_sim_solver = auv_model_sim_solver; - - capsule->acados_sim_mem = auv_model_sim_solver->mem; - - - - /* initialize input */ - // x - double x0[9]; - for (int ii = 0; ii < 9; ii++) - x0[ii] = 0.0; - - sim_in_set(auv_model_sim_config, auv_model_sim_dims, - auv_model_sim_in, "x", x0); - - - // u - double u0[3]; - for (int ii = 0; ii < 3; ii++) - u0[ii] = 0.0; - - sim_in_set(auv_model_sim_config, auv_model_sim_dims, - auv_model_sim_in, "u", u0); - - // S_forw - double S_forw[108]; - for (int ii = 0; ii < 108; ii++) - S_forw[ii] = 0.0; - for (int ii = 0; ii < 9; ii++) - S_forw[ii + ii * 9 ] = 1.0; - - - sim_in_set(auv_model_sim_config, auv_model_sim_dims, - auv_model_sim_in, "S_forw", S_forw); - - int status = sim_precompute(auv_model_sim_solver, auv_model_sim_in, auv_model_sim_out); - - return status; -} - - -int auv_model_acados_sim_solve(auv_model_sim_solver_capsule *capsule) -{ - // integrate dynamics using acados sim_solver - int status = sim_solve(capsule->acados_sim_solver, - capsule->acados_sim_in, capsule->acados_sim_out); - if (status != 0) - printf("error in auv_model_acados_sim_solve()! Exiting.\n"); - - return status; -} - - - - -int auv_model_acados_sim_free(auv_model_sim_solver_capsule *capsule) -{ - // free memory - sim_solver_destroy(capsule->acados_sim_solver); - sim_in_destroy(capsule->acados_sim_in); - sim_out_destroy(capsule->acados_sim_out); - sim_opts_destroy(capsule->acados_sim_opts); - sim_dims_destroy(capsule->acados_sim_dims); - sim_config_destroy(capsule->acados_sim_config); - - // free external function - external_function_param_casadi_free(capsule->sim_impl_dae_fun); - external_function_param_casadi_free(capsule->sim_impl_dae_fun_jac_x_xdot_z); - external_function_param_casadi_free(capsule->sim_impl_dae_jac_x_xdot_u_z); - - free(capsule->sim_impl_dae_fun); - free(capsule->sim_impl_dae_fun_jac_x_xdot_z); - free(capsule->sim_impl_dae_jac_x_xdot_u_z); - - - return 0; -} - - -int auv_model_acados_sim_update_params(auv_model_sim_solver_capsule *capsule, double *p, int np) -{ - int status = 0; - int casadi_np = AUV_MODEL_NP; - - if (casadi_np != np) { - printf("auv_model_acados_sim_update_params: trying to set %i parameters for external functions." - " External function has %i parameters. Exiting.\n", np, casadi_np); - exit(1); - } - capsule->sim_impl_dae_fun[0].set_param(capsule->sim_impl_dae_fun, p); - capsule->sim_impl_dae_fun_jac_x_xdot_z[0].set_param(capsule->sim_impl_dae_fun_jac_x_xdot_z, p); - capsule->sim_impl_dae_jac_x_xdot_u_z[0].set_param(capsule->sim_impl_dae_jac_x_xdot_u_z, p); - - - return status; -} - -/* getters pointers to C objects*/ -sim_config * auv_model_acados_get_sim_config(auv_model_sim_solver_capsule *capsule) -{ - return capsule->acados_sim_config; -}; - -sim_in * auv_model_acados_get_sim_in(auv_model_sim_solver_capsule *capsule) -{ - return capsule->acados_sim_in; -}; - -sim_out * auv_model_acados_get_sim_out(auv_model_sim_solver_capsule *capsule) -{ - return capsule->acados_sim_out; -}; - -void * auv_model_acados_get_sim_dims(auv_model_sim_solver_capsule *capsule) -{ - return capsule->acados_sim_dims; -}; - -sim_opts * auv_model_acados_get_sim_opts(auv_model_sim_solver_capsule *capsule) -{ - return capsule->acados_sim_opts; -}; - -sim_solver * auv_model_acados_get_sim_solver(auv_model_sim_solver_capsule *capsule) -{ - return capsule->acados_sim_solver; -}; - -void * auv_model_acados_get_sim_mem(auv_model_sim_solver_capsule *capsule) -{ - return capsule->acados_sim_mem; -}; - diff --git a/control/velocity_controller/scripts/build_auv_solver/acados_sim_solver_auv_model.h b/control/velocity_controller/scripts/build_auv_solver/acados_sim_solver_auv_model.h deleted file mode 100644 index beed43e59..000000000 --- a/control/velocity_controller/scripts/build_auv_solver/acados_sim_solver_auv_model.h +++ /dev/null @@ -1,105 +0,0 @@ -/* - * Copyright (c) The acados authors. - * - * This file is part of acados. - * - * The 2-Clause BSD License - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE.; - */ - -#ifndef ACADOS_SIM_auv_model_H_ -#define ACADOS_SIM_auv_model_H_ - -#include "acados_c/sim_interface.h" -#include "acados_c/external_function_interface.h" - -#define AUV_MODEL_NX 9 -#define AUV_MODEL_NZ 0 -#define AUV_MODEL_NU 3 -#define AUV_MODEL_NP 0 - -#ifdef __cplusplus -extern "C" { -#endif - - -// ** capsule for solver data ** -typedef struct auv_model_sim_solver_capsule -{ - // acados objects - sim_in *acados_sim_in; - sim_out *acados_sim_out; - sim_solver *acados_sim_solver; - sim_opts *acados_sim_opts; - sim_config *acados_sim_config; - void *acados_sim_dims; - void *acados_sim_mem; - - /* external functions */ - // ERK - external_function_param_casadi * sim_expl_vde_forw; - external_function_param_casadi * sim_vde_adj_casadi; - external_function_param_casadi * sim_expl_ode_fun_casadi; - external_function_param_casadi * sim_expl_ode_hess; - external_function_param_casadi * sim_expl_vde_forw_p; - - // IRK - external_function_param_casadi * sim_impl_dae_fun; - external_function_param_casadi * sim_impl_dae_fun_jac_x_xdot_z; - external_function_param_casadi * sim_impl_dae_jac_x_xdot_u_z; - external_function_param_casadi * sim_impl_dae_hess; - external_function_param_casadi * sim_impl_dae_jac_p; - - // GNSF - external_function_param_casadi * sim_gnsf_phi_fun; - external_function_param_casadi * sim_gnsf_phi_fun_jac_y; - external_function_param_casadi * sim_gnsf_phi_jac_y_uhat; - external_function_param_casadi * sim_gnsf_f_lo_jac_x1_x1dot_u_z; - external_function_param_casadi * sim_gnsf_get_matrices_fun; - -} auv_model_sim_solver_capsule; - - -ACADOS_SYMBOL_EXPORT int auv_model_acados_sim_create(auv_model_sim_solver_capsule *capsule); -ACADOS_SYMBOL_EXPORT int auv_model_acados_sim_solve(auv_model_sim_solver_capsule *capsule); - -ACADOS_SYMBOL_EXPORT int auv_model_acados_sim_free(auv_model_sim_solver_capsule *capsule); -ACADOS_SYMBOL_EXPORT int auv_model_acados_sim_update_params(auv_model_sim_solver_capsule *capsule, double *value, int np); - -ACADOS_SYMBOL_EXPORT sim_config * auv_model_acados_get_sim_config(auv_model_sim_solver_capsule *capsule); -ACADOS_SYMBOL_EXPORT sim_in * auv_model_acados_get_sim_in(auv_model_sim_solver_capsule *capsule); -ACADOS_SYMBOL_EXPORT sim_out * auv_model_acados_get_sim_out(auv_model_sim_solver_capsule *capsule); -ACADOS_SYMBOL_EXPORT void * auv_model_acados_get_sim_dims(auv_model_sim_solver_capsule *capsule); -ACADOS_SYMBOL_EXPORT sim_opts * auv_model_acados_get_sim_opts(auv_model_sim_solver_capsule *capsule); -ACADOS_SYMBOL_EXPORT sim_solver * auv_model_acados_get_sim_solver(auv_model_sim_solver_capsule *capsule); -ACADOS_SYMBOL_EXPORT void * auv_model_acados_get_sim_mem(auv_model_sim_solver_capsule *capsule); - -ACADOS_SYMBOL_EXPORT auv_model_sim_solver_capsule * auv_model_acados_sim_solver_create_capsule(void); -ACADOS_SYMBOL_EXPORT int auv_model_acados_sim_solver_free_capsule(auv_model_sim_solver_capsule *capsule); - -#ifdef __cplusplus -} -#endif - -#endif // ACADOS_SIM_auv_model_H_ diff --git a/control/velocity_controller/scripts/build_auv_solver/acados_solver.pxd b/control/velocity_controller/scripts/build_auv_solver/acados_solver.pxd deleted file mode 100644 index a6a039341..000000000 --- a/control/velocity_controller/scripts/build_auv_solver/acados_solver.pxd +++ /dev/null @@ -1,63 +0,0 @@ -# -# Copyright (c) The acados authors. -# -# This file is part of acados. -# -# The 2-Clause BSD License -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# 1. Redistributions of source code must retain the above copyright notice, -# this list of conditions and the following disclaimer. -# -# 2. Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE.; -# - -cimport acados_solver_common - -cdef extern from "acados_solver_auv_model.h": - ctypedef struct nlp_solver_capsule "auv_model_solver_capsule": - pass - - nlp_solver_capsule * acados_create_capsule "auv_model_acados_create_capsule"() - int acados_free_capsule "auv_model_acados_free_capsule"(nlp_solver_capsule *capsule) - - int acados_create "auv_model_acados_create"(nlp_solver_capsule * capsule) - - int acados_create_with_discretization "auv_model_acados_create_with_discretization"(nlp_solver_capsule * capsule, int n_time_steps, double* new_time_steps) - int acados_update_time_steps "auv_model_acados_update_time_steps"(nlp_solver_capsule * capsule, int N, double* new_time_steps) - int acados_update_qp_solver_cond_N "auv_model_acados_update_qp_solver_cond_N"(nlp_solver_capsule * capsule, int qp_solver_cond_N) - - int acados_update_params "auv_model_acados_update_params"(nlp_solver_capsule * capsule, int stage, double *value, int np_) - int acados_update_params_sparse "auv_model_acados_update_params_sparse"(nlp_solver_capsule * capsule, int stage, int *idx, double *p, int n_update) - int acados_set_p_global_and_precompute_dependencies "auv_model_acados_set_p_global_and_precompute_dependencies"(nlp_solver_capsule * capsule, double *value, int data_len) - int acados_solve "auv_model_acados_solve"(nlp_solver_capsule * capsule) - int acados_reset "auv_model_acados_reset"(nlp_solver_capsule * capsule, int reset_qp_solver_mem) - int acados_free "auv_model_acados_free"(nlp_solver_capsule * capsule) - void acados_print_stats "auv_model_acados_print_stats"(nlp_solver_capsule * capsule) - - int acados_custom_update "auv_model_acados_custom_update"(nlp_solver_capsule* capsule, double * data, int data_len) - - acados_solver_common.ocp_nlp_in *acados_get_nlp_in "auv_model_acados_get_nlp_in"(nlp_solver_capsule * capsule) - acados_solver_common.ocp_nlp_out *acados_get_nlp_out "auv_model_acados_get_nlp_out"(nlp_solver_capsule * capsule) - acados_solver_common.ocp_nlp_out *acados_get_sens_out "auv_model_acados_get_sens_out"(nlp_solver_capsule * capsule) - acados_solver_common.ocp_nlp_solver *acados_get_nlp_solver "auv_model_acados_get_nlp_solver"(nlp_solver_capsule * capsule) - acados_solver_common.ocp_nlp_config *acados_get_nlp_config "auv_model_acados_get_nlp_config"(nlp_solver_capsule * capsule) - void *acados_get_nlp_opts "auv_model_acados_get_nlp_opts"(nlp_solver_capsule * capsule) - acados_solver_common.ocp_nlp_dims *acados_get_nlp_dims "auv_model_acados_get_nlp_dims"(nlp_solver_capsule * capsule) - acados_solver_common.ocp_nlp_plan *acados_get_nlp_plan "auv_model_acados_get_nlp_plan"(nlp_solver_capsule * capsule) diff --git a/control/velocity_controller/scripts/build_auv_solver/acados_solver_auv_model.c b/control/velocity_controller/scripts/build_auv_solver/acados_solver_auv_model.c deleted file mode 100644 index 6fbe2da06..000000000 --- a/control/velocity_controller/scripts/build_auv_solver/acados_solver_auv_model.c +++ /dev/null @@ -1,1187 +0,0 @@ -/* - * Copyright (c) The acados authors. - * - * This file is part of acados. - * - * The 2-Clause BSD License - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE.; - */ - -// standard -#include -#include -#include -// acados -// #include "acados/utils/print.h" -#include "acados_c/ocp_nlp_interface.h" -#include "acados_c/external_function_interface.h" - -// example specific - -#include "auv_model_model/auv_model_model.h" - - - - - -#include "acados_solver_auv_model.h" - -#define NX AUV_MODEL_NX -#define NZ AUV_MODEL_NZ -#define NU AUV_MODEL_NU -#define NP AUV_MODEL_NP -#define NP_GLOBAL AUV_MODEL_NP_GLOBAL -#define NY0 AUV_MODEL_NY0 -#define NY AUV_MODEL_NY -#define NYN AUV_MODEL_NYN - -#define NBX AUV_MODEL_NBX -#define NBX0 AUV_MODEL_NBX0 -#define NBU AUV_MODEL_NBU -#define NG AUV_MODEL_NG -#define NBXN AUV_MODEL_NBXN -#define NGN AUV_MODEL_NGN - -#define NH AUV_MODEL_NH -#define NHN AUV_MODEL_NHN -#define NH0 AUV_MODEL_NH0 -#define NPHI AUV_MODEL_NPHI -#define NPHIN AUV_MODEL_NPHIN -#define NPHI0 AUV_MODEL_NPHI0 -#define NR AUV_MODEL_NR - -#define NS AUV_MODEL_NS -#define NS0 AUV_MODEL_NS0 -#define NSN AUV_MODEL_NSN - -#define NSBX AUV_MODEL_NSBX -#define NSBU AUV_MODEL_NSBU -#define NSH0 AUV_MODEL_NSH0 -#define NSH AUV_MODEL_NSH -#define NSHN AUV_MODEL_NSHN -#define NSG AUV_MODEL_NSG -#define NSPHI0 AUV_MODEL_NSPHI0 -#define NSPHI AUV_MODEL_NSPHI -#define NSPHIN AUV_MODEL_NSPHIN -#define NSGN AUV_MODEL_NSGN -#define NSBXN AUV_MODEL_NSBXN - - - -// ** solver data ** - -auv_model_solver_capsule * auv_model_acados_create_capsule(void) -{ - void* capsule_mem = malloc(sizeof(auv_model_solver_capsule)); - auv_model_solver_capsule *capsule = (auv_model_solver_capsule *) capsule_mem; - - return capsule; -} - - -int auv_model_acados_free_capsule(auv_model_solver_capsule *capsule) -{ - free(capsule); - return 0; -} - - -int auv_model_acados_create(auv_model_solver_capsule* capsule) -{ - int N_shooting_intervals = AUV_MODEL_N; - double* new_time_steps = NULL; // NULL -> don't alter the code generated time-steps - return auv_model_acados_create_with_discretization(capsule, N_shooting_intervals, new_time_steps); -} - - -int auv_model_acados_update_time_steps(auv_model_solver_capsule* capsule, int N, double* new_time_steps) -{ - - if (N != capsule->nlp_solver_plan->N) { - fprintf(stderr, "auv_model_acados_update_time_steps: given number of time steps (= %d) " \ - "differs from the currently allocated number of " \ - "time steps (= %d)!\n" \ - "Please recreate with new discretization and provide a new vector of time_stamps!\n", - N, capsule->nlp_solver_plan->N); - return 1; - } - - ocp_nlp_config * nlp_config = capsule->nlp_config; - ocp_nlp_dims * nlp_dims = capsule->nlp_dims; - ocp_nlp_in * nlp_in = capsule->nlp_in; - - for (int i = 0; i < N; i++) - { - ocp_nlp_in_set(nlp_config, nlp_dims, nlp_in, i, "Ts", &new_time_steps[i]); - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "scaling", &new_time_steps[i]); - } - return 0; - -} - -/** - * Internal function for auv_model_acados_create: step 1 - */ -void auv_model_acados_create_set_plan(ocp_nlp_plan_t* nlp_solver_plan, const int N) -{ - assert(N == nlp_solver_plan->N); - - /************************************************ - * plan - ************************************************/ - - nlp_solver_plan->nlp_solver = SQP; - - nlp_solver_plan->ocp_qp_solver_plan.qp_solver = FULL_CONDENSING_HPIPM; - nlp_solver_plan->relaxed_ocp_qp_solver_plan.qp_solver = FULL_CONDENSING_HPIPM; - nlp_solver_plan->nlp_cost[0] = LINEAR_LS; - for (int i = 1; i < N; i++) - nlp_solver_plan->nlp_cost[i] = LINEAR_LS; - - nlp_solver_plan->nlp_cost[N] = LINEAR_LS; - - for (int i = 0; i < N; i++) - { - nlp_solver_plan->nlp_dynamics[i] = CONTINUOUS_MODEL; - nlp_solver_plan->sim_solver_plan[i].sim_solver = IRK; - } - - nlp_solver_plan->nlp_constraints[0] = BGH; - - for (int i = 1; i < N; i++) - { - nlp_solver_plan->nlp_constraints[i] = BGH; - } - nlp_solver_plan->nlp_constraints[N] = BGH; - - nlp_solver_plan->regularization = NO_REGULARIZE; - - nlp_solver_plan->globalization = FIXED_STEP; -} - - -static ocp_nlp_dims* auv_model_acados_create_setup_dimensions(auv_model_solver_capsule* capsule) -{ - ocp_nlp_plan_t* nlp_solver_plan = capsule->nlp_solver_plan; - const int N = nlp_solver_plan->N; - ocp_nlp_config* nlp_config = capsule->nlp_config; - - /************************************************ - * dimensions - ************************************************/ - #define NINTNP1MEMS 18 - int* intNp1mem = (int*)malloc( (N+1)*sizeof(int)*NINTNP1MEMS ); - - int* nx = intNp1mem + (N+1)*0; - int* nu = intNp1mem + (N+1)*1; - int* nbx = intNp1mem + (N+1)*2; - int* nbu = intNp1mem + (N+1)*3; - int* nsbx = intNp1mem + (N+1)*4; - int* nsbu = intNp1mem + (N+1)*5; - int* nsg = intNp1mem + (N+1)*6; - int* nsh = intNp1mem + (N+1)*7; - int* nsphi = intNp1mem + (N+1)*8; - int* ns = intNp1mem + (N+1)*9; - int* ng = intNp1mem + (N+1)*10; - int* nh = intNp1mem + (N+1)*11; - int* nphi = intNp1mem + (N+1)*12; - int* nz = intNp1mem + (N+1)*13; - int* ny = intNp1mem + (N+1)*14; - int* nr = intNp1mem + (N+1)*15; - int* nbxe = intNp1mem + (N+1)*16; - int* np = intNp1mem + (N+1)*17; - - for (int i = 0; i < N+1; i++) - { - // common - nx[i] = NX; - nu[i] = NU; - nz[i] = NZ; - ns[i] = NS; - // cost - ny[i] = NY; - // constraints - nbx[i] = NBX; - nbu[i] = NBU; - nsbx[i] = NSBX; - nsbu[i] = NSBU; - nsg[i] = NSG; - nsh[i] = NSH; - nsphi[i] = NSPHI; - ng[i] = NG; - nh[i] = NH; - nphi[i] = NPHI; - nr[i] = NR; - nbxe[i] = 0; - np[i] = NP; - } - - // for initial state - nbx[0] = NBX0; - nsbx[0] = 0; - ns[0] = NS0; - - nbxe[0] = 9; - - ny[0] = NY0; - nh[0] = NH0; - nsh[0] = NSH0; - nsphi[0] = NSPHI0; - nphi[0] = NPHI0; - - - // terminal - common - nu[N] = 0; - nz[N] = 0; - ns[N] = NSN; - // cost - ny[N] = NYN; - // constraint - nbx[N] = NBXN; - nbu[N] = 0; - ng[N] = NGN; - nh[N] = NHN; - nphi[N] = NPHIN; - nr[N] = 0; - - nsbx[N] = NSBXN; - nsbu[N] = 0; - nsg[N] = NSGN; - nsh[N] = NSHN; - nsphi[N] = NSPHIN; - - /* create and set ocp_nlp_dims */ - ocp_nlp_dims * nlp_dims = ocp_nlp_dims_create(nlp_config); - - ocp_nlp_dims_set_opt_vars(nlp_config, nlp_dims, "nx", nx); - ocp_nlp_dims_set_opt_vars(nlp_config, nlp_dims, "nu", nu); - ocp_nlp_dims_set_opt_vars(nlp_config, nlp_dims, "nz", nz); - ocp_nlp_dims_set_opt_vars(nlp_config, nlp_dims, "ns", ns); - ocp_nlp_dims_set_opt_vars(nlp_config, nlp_dims, "np", np); - - ocp_nlp_dims_set_global(nlp_config, nlp_dims, "np_global", 0); - ocp_nlp_dims_set_global(nlp_config, nlp_dims, "n_global_data", 0); - - for (int i = 0; i <= N; i++) - { - ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nbx", &nbx[i]); - ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nbu", &nbu[i]); - ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nsbx", &nsbx[i]); - ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nsbu", &nsbu[i]); - ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "ng", &ng[i]); - ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nsg", &nsg[i]); - ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nbxe", &nbxe[i]); - } - ocp_nlp_dims_set_cost(nlp_config, nlp_dims, 0, "ny", &ny[0]); - for (int i = 1; i < N; i++) - ocp_nlp_dims_set_cost(nlp_config, nlp_dims, i, "ny", &ny[i]); - ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, 0, "nh", &nh[0]); - ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, 0, "nsh", &nsh[0]); - - for (int i = 1; i < N; i++) - { - ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nh", &nh[i]); - ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nsh", &nsh[i]); - } - ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, N, "nh", &nh[N]); - ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, N, "nsh", &nsh[N]); - ocp_nlp_dims_set_cost(nlp_config, nlp_dims, N, "ny", &ny[N]); - free(intNp1mem); - - return nlp_dims; -} - - -/** - * Internal function for auv_model_acados_create: step 3 - */ -void auv_model_acados_create_setup_functions(auv_model_solver_capsule* capsule) -{ - const int N = capsule->nlp_solver_plan->N; - - /************************************************ - * external functions - ************************************************/ - -#define MAP_CASADI_FNC(__CAPSULE_FNC__, __MODEL_BASE_FNC__) do{ \ - capsule->__CAPSULE_FNC__.casadi_fun = & __MODEL_BASE_FNC__ ;\ - capsule->__CAPSULE_FNC__.casadi_n_in = & __MODEL_BASE_FNC__ ## _n_in; \ - capsule->__CAPSULE_FNC__.casadi_n_out = & __MODEL_BASE_FNC__ ## _n_out; \ - capsule->__CAPSULE_FNC__.casadi_sparsity_in = & __MODEL_BASE_FNC__ ## _sparsity_in; \ - capsule->__CAPSULE_FNC__.casadi_sparsity_out = & __MODEL_BASE_FNC__ ## _sparsity_out; \ - capsule->__CAPSULE_FNC__.casadi_work = & __MODEL_BASE_FNC__ ## _work; \ - external_function_external_param_casadi_create(&capsule->__CAPSULE_FNC__, &ext_fun_opts); \ - } while(false) - - external_function_opts ext_fun_opts; - external_function_opts_set_to_default(&ext_fun_opts); - - - ext_fun_opts.external_workspace = true; - if (N > 0) - { - - - - - // implicit dae - capsule->impl_dae_fun = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*N); - for (int i = 0; i < N; i++) { - MAP_CASADI_FNC(impl_dae_fun[i], auv_model_impl_dae_fun); - } - - capsule->impl_dae_fun_jac_x_xdot_z = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*N); - for (int i = 0; i < N; i++) { - MAP_CASADI_FNC(impl_dae_fun_jac_x_xdot_z[i], auv_model_impl_dae_fun_jac_x_xdot_z); - } - - capsule->impl_dae_jac_x_xdot_u_z = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*N); - for (int i = 0; i < N; i++) { - MAP_CASADI_FNC(impl_dae_jac_x_xdot_u_z[i], auv_model_impl_dae_jac_x_xdot_u_z); - } - - - - } // N > 0 - -#undef MAP_CASADI_FNC -} - - -/** - * Internal function for auv_model_acados_create: step 5 - */ -void auv_model_acados_create_set_default_parameters(auv_model_solver_capsule* capsule) -{ - - // no parameters defined - - - // no global parameters defined -} - - -/** - * Internal function for auv_model_acados_create: step 5 - */ -void auv_model_acados_setup_nlp_in(auv_model_solver_capsule* capsule, const int N, double* new_time_steps) -{ - assert(N == capsule->nlp_solver_plan->N); - ocp_nlp_config* nlp_config = capsule->nlp_config; - ocp_nlp_dims* nlp_dims = capsule->nlp_dims; - - int tmp_int = 0; - - /************************************************ - * nlp_in - ************************************************/ - ocp_nlp_in * nlp_in = capsule->nlp_in; - /************************************************ - * nlp_out - ************************************************/ - ocp_nlp_out * nlp_out = capsule->nlp_out; - - // set up time_steps and cost_scaling - - if (new_time_steps) - { - // NOTE: this sets scaling and time_steps - auv_model_acados_update_time_steps(capsule, N, new_time_steps); - } - else - { - // set time_steps - - double time_step = 0.1; - for (int i = 0; i < N; i++) - { - ocp_nlp_in_set(nlp_config, nlp_dims, nlp_in, i, "Ts", &time_step); - } - // set cost scaling - double* cost_scaling = malloc((N+1)*sizeof(double)); - cost_scaling[0] = 0.1; - cost_scaling[1] = 0.1; - cost_scaling[2] = 0.1; - cost_scaling[3] = 0.1; - cost_scaling[4] = 0.1; - cost_scaling[5] = 0.1; - cost_scaling[6] = 0.1; - cost_scaling[7] = 0.1; - cost_scaling[8] = 0.1; - cost_scaling[9] = 0.1; - cost_scaling[10] = 0.1; - cost_scaling[11] = 0.1; - cost_scaling[12] = 0.1; - cost_scaling[13] = 0.1; - cost_scaling[14] = 0.1; - cost_scaling[15] = 0.1; - cost_scaling[16] = 0.1; - cost_scaling[17] = 0.1; - cost_scaling[18] = 0.1; - cost_scaling[19] = 0.1; - cost_scaling[20] = 1; - for (int i = 0; i <= N; i++) - { - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "scaling", &cost_scaling[i]); - } - free(cost_scaling); - } - - - - /**** Dynamics ****/ - for (int i = 0; i < N; i++) - { - ocp_nlp_dynamics_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, i, "impl_dae_fun", &capsule->impl_dae_fun[i]); - ocp_nlp_dynamics_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, i, - "impl_dae_fun_jac_x_xdot_z", &capsule->impl_dae_fun_jac_x_xdot_z[i]); - ocp_nlp_dynamics_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, i, - "impl_dae_jac_x_xdot_u", &capsule->impl_dae_jac_x_xdot_u_z[i]); - - } - - /**** Cost ****/ - double* yref_0 = calloc(NY0, sizeof(double)); - // change only the non-zero elements: - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "yref", yref_0); - free(yref_0); - - double* W_0 = calloc(NY0*NY0, sizeof(double)); - // change only the non-zero elements: - W_0[0+(NY0) * 0] = 100; - W_0[4+(NY0) * 4] = 1; - W_0[5+(NY0) * 5] = 0.1; - W_0[6+(NY0) * 6] = 0.1; - W_0[7+(NY0) * 7] = 0.1; - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "W", W_0); - free(W_0); - double* Vx_0 = calloc(NY0*NX, sizeof(double)); - // change only the non-zero elements: - Vx_0[0+(NY0) * 0] = 1; - Vx_0[1+(NY0) * 4] = 1; - Vx_0[2+(NY0) * 5] = 1; - Vx_0[3+(NY0) * 7] = 1; - Vx_0[4+(NY0) * 8] = 1; - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "Vx", Vx_0); - free(Vx_0); - double* Vu_0 = calloc(NY0*NU, sizeof(double)); - // change only the non-zero elements: - Vu_0[5+(NY0) * 0] = 1; - Vu_0[6+(NY0) * 1] = 1; - Vu_0[7+(NY0) * 2] = 1; - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "Vu", Vu_0); - free(Vu_0); - double* yref = calloc(NY, sizeof(double)); - // change only the non-zero elements: - - for (int i = 1; i < N; i++) - { - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "yref", yref); - } - free(yref); - double* W = calloc(NY*NY, sizeof(double)); - // change only the non-zero elements: - W[0+(NY) * 0] = 100; - W[4+(NY) * 4] = 1; - W[5+(NY) * 5] = 0.1; - W[6+(NY) * 6] = 0.1; - W[7+(NY) * 7] = 0.1; - - for (int i = 1; i < N; i++) - { - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "W", W); - } - free(W); - double* Vx = calloc(NY*NX, sizeof(double)); - // change only the non-zero elements: - Vx[0+(NY) * 0] = 1; - Vx[1+(NY) * 4] = 1; - Vx[2+(NY) * 5] = 1; - Vx[3+(NY) * 7] = 1; - Vx[4+(NY) * 8] = 1; - for (int i = 1; i < N; i++) - { - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "Vx", Vx); - } - free(Vx); - - - double* Vu = calloc(NY*NU, sizeof(double)); - // change only the non-zero elements: - Vu[5+(NY) * 0] = 1; - Vu[6+(NY) * 1] = 1; - Vu[7+(NY) * 2] = 1; - - for (int i = 1; i < N; i++) - { - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "Vu", Vu); - } - free(Vu); - double* yref_e = calloc(NYN, sizeof(double)); - // change only the non-zero elements: - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "yref", yref_e); - free(yref_e); - - double* W_e = calloc(NYN*NYN, sizeof(double)); - // change only the non-zero elements: - W_e[0+(NYN) * 0] = 100; - W_e[4+(NYN) * 4] = 1; - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "W", W_e); - free(W_e); - double* Vx_e = calloc(NYN*NX, sizeof(double)); - // change only the non-zero elements: - Vx_e[0+(NYN) * 0] = 1; - Vx_e[1+(NYN) * 4] = 1; - Vx_e[2+(NYN) * 5] = 1; - Vx_e[3+(NYN) * 7] = 1; - Vx_e[4+(NYN) * 8] = 1; - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "Vx", Vx_e); - free(Vx_e); - - - - - - - - /**** Constraints ****/ - - // bounds for initial stage - // x0 - int* idxbx0 = malloc(NBX0 * sizeof(int)); - idxbx0[0] = 0; - idxbx0[1] = 1; - idxbx0[2] = 2; - idxbx0[3] = 3; - idxbx0[4] = 4; - idxbx0[5] = 5; - idxbx0[6] = 6; - idxbx0[7] = 7; - idxbx0[8] = 8; - - double* lubx0 = calloc(2*NBX0, sizeof(double)); - double* lbx0 = lubx0; - double* ubx0 = lubx0 + NBX0; - // change only the non-zero elements: - - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "idxbx", idxbx0); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "lbx", lbx0); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "ubx", ubx0); - free(idxbx0); - free(lubx0); - // idxbxe_0 - int* idxbxe_0 = malloc(9 * sizeof(int)); - idxbxe_0[0] = 0; - idxbxe_0[1] = 1; - idxbxe_0[2] = 2; - idxbxe_0[3] = 3; - idxbxe_0[4] = 4; - idxbxe_0[5] = 5; - idxbxe_0[6] = 6; - idxbxe_0[7] = 7; - idxbxe_0[8] = 8; - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "idxbxe", idxbxe_0); - free(idxbxe_0); - - - - - - - - - - - - - /* constraints that are the same for initial and intermediate */ - // u - int* idxbu = malloc(NBU * sizeof(int)); - idxbu[0] = 0; - idxbu[1] = 1; - idxbu[2] = 2; - double* lubu = calloc(2*NBU, sizeof(double)); - double* lbu = lubu; - double* ubu = lubu + NBU; - lbu[0] = -99.5; - ubu[0] = 99.5; - lbu[1] = -99.5; - ubu[1] = 99.5; - lbu[2] = -99.5; - ubu[2] = 99.5; - - for (int i = 0; i < N; i++) - { - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxbu", idxbu); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lbu", lbu); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "ubu", ubu); - } - free(idxbu); - free(lubu); - - - - - - - /* Path constraints */ - - // x - int* idxbx = malloc(NBX * sizeof(int)); - idxbx[0] = 7; - double* lubx = calloc(2*NBX, sizeof(double)); - double* lbx = lubx; - double* ubx = lubx + NBX; - lbx[0] = -1.4; - ubx[0] = 1.4; - - for (int i = 1; i < N; i++) - { - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxbx", idxbx); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lbx", lbx); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "ubx", ubx); - } - free(idxbx); - free(lubx); - - - - - - - - - - - - - - /* terminal constraints */ - - - - - - - - - - - - - - - - - - - - -} - - -static void auv_model_acados_create_set_opts(auv_model_solver_capsule* capsule) -{ - const int N = capsule->nlp_solver_plan->N; - ocp_nlp_config* nlp_config = capsule->nlp_config; - void *nlp_opts = capsule->nlp_opts; - - /************************************************ - * opts - ************************************************/ - - - - int fixed_hess = 0; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "fixed_hess", &fixed_hess); - - double globalization_fixed_step_length = 1; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "globalization_fixed_step_length", &globalization_fixed_step_length); - - - - - int with_solution_sens_wrt_params = false; - ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "with_solution_sens_wrt_params", &with_solution_sens_wrt_params); - - int with_value_sens_wrt_params = false; - ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "with_value_sens_wrt_params", &with_value_sens_wrt_params); - - double solution_sens_qp_t_lam_min = 0.000000001; - ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "solution_sens_qp_t_lam_min", &solution_sens_qp_t_lam_min); - - int globalization_full_step_dual = 0; - ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "globalization_full_step_dual", &globalization_full_step_dual); - - // set collocation type (relevant for implicit integrators) - sim_collocation_type collocation_type = GAUSS_LEGENDRE; - for (int i = 0; i < N; i++) - ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_collocation_type", &collocation_type); - - // set up sim_method_num_steps - // all sim_method_num_steps are identical - int sim_method_num_steps = 4; - for (int i = 0; i < N; i++) - ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_num_steps", &sim_method_num_steps); - - // set up sim_method_num_stages - // all sim_method_num_stages are identical - int sim_method_num_stages = 2; - for (int i = 0; i < N; i++) - ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_num_stages", &sim_method_num_stages); - - int newton_iter_val = 3; - for (int i = 0; i < N; i++) - ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_newton_iter", &newton_iter_val); - - double newton_tol_val = 0; - for (int i = 0; i < N; i++) - ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_newton_tol", &newton_tol_val); - - // set up sim_method_jac_reuse - bool tmp_bool = (bool) 0; - for (int i = 0; i < N; i++) - ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_jac_reuse", &tmp_bool); - - double levenberg_marquardt = 0.01; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "levenberg_marquardt", &levenberg_marquardt); - - /* options QP solver */ - - int nlp_solver_ext_qp_res = 0; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "ext_qp_res", &nlp_solver_ext_qp_res); - - bool store_iterates = false; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "store_iterates", &store_iterates); - int log_primal_step_norm = false; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "log_primal_step_norm", &log_primal_step_norm); - - int log_dual_step_norm = false; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "log_dual_step_norm", &log_dual_step_norm); - - double nlp_solver_tol_min_step_norm = 0; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "tol_min_step_norm", &nlp_solver_tol_min_step_norm); - // set HPIPM mode: should be done before setting other QP solver options - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_hpipm_mode", "BALANCE"); - - - - int qp_solver_t0_init = 2; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_t0_init", &qp_solver_t0_init); - - - - - // set SQP specific options - double nlp_solver_tol_stat = 0.000001; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "tol_stat", &nlp_solver_tol_stat); - - double nlp_solver_tol_eq = 0.000001; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "tol_eq", &nlp_solver_tol_eq); - - double nlp_solver_tol_ineq = 0.000001; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "tol_ineq", &nlp_solver_tol_ineq); - - double nlp_solver_tol_comp = 0.000001; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "tol_comp", &nlp_solver_tol_comp); - - int nlp_solver_max_iter = 100; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "max_iter", &nlp_solver_max_iter); - - // set options for adaptive Levenberg-Marquardt Update - bool with_adaptive_levenberg_marquardt = false; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "with_adaptive_levenberg_marquardt", &with_adaptive_levenberg_marquardt); - - double adaptive_levenberg_marquardt_lam = 5; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "adaptive_levenberg_marquardt_lam", &adaptive_levenberg_marquardt_lam); - - double adaptive_levenberg_marquardt_mu_min = 0.0000000000000001; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "adaptive_levenberg_marquardt_mu_min", &adaptive_levenberg_marquardt_mu_min); - - double adaptive_levenberg_marquardt_mu0 = 0.001; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "adaptive_levenberg_marquardt_mu0", &adaptive_levenberg_marquardt_mu0); - - double adaptive_levenberg_marquardt_obj_scalar = 2; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "adaptive_levenberg_marquardt_obj_scalar", &adaptive_levenberg_marquardt_obj_scalar); - - bool eval_residual_at_max_iter = false; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "eval_residual_at_max_iter", &eval_residual_at_max_iter); - - // QP scaling - double qpscaling_ub_max_abs_eig = 100000; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qpscaling_ub_max_abs_eig", &qpscaling_ub_max_abs_eig); - - double qpscaling_lb_norm_inf_grad_obj = 0.0001; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qpscaling_lb_norm_inf_grad_obj", &qpscaling_lb_norm_inf_grad_obj); - - qpscaling_scale_objective_type qpscaling_scale_objective = NO_OBJECTIVE_SCALING; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qpscaling_scale_objective", &qpscaling_scale_objective); - - ocp_nlp_qpscaling_constraint_type qpscaling_scale_constraints = NO_CONSTRAINT_SCALING; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qpscaling_scale_constraints", &qpscaling_scale_constraints); - - // NLP QP tol strategy - ocp_nlp_qp_tol_strategy_t nlp_qp_tol_strategy = FIXED_QP_TOL; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_strategy", &nlp_qp_tol_strategy); - - double nlp_qp_tol_reduction_factor = 0.1; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_reduction_factor", &nlp_qp_tol_reduction_factor); - - double nlp_qp_tol_safety_factor = 0.1; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_safety_factor", &nlp_qp_tol_safety_factor); - - double nlp_qp_tol_min_stat = 0.000000001; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_min_stat", &nlp_qp_tol_min_stat); - - double nlp_qp_tol_min_eq = 0.0000000001; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_min_eq", &nlp_qp_tol_min_eq); - - double nlp_qp_tol_min_ineq = 0.0000000001; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_min_ineq", &nlp_qp_tol_min_ineq); - - double nlp_qp_tol_min_comp = 0.00000000001; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_min_comp", &nlp_qp_tol_min_comp); - - bool with_anderson_acceleration = false; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "with_anderson_acceleration", &with_anderson_acceleration); - - double anderson_activation_threshold = 10; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "anderson_activation_threshold", &anderson_activation_threshold); - - int qp_solver_iter_max = 50; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_iter_max", &qp_solver_iter_max); - - - int qp_solver_warm_start = 1; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_warm_start", &qp_solver_warm_start); - - int print_level = 2; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "print_level", &print_level); - - int ext_cost_num_hess = 0; -} - - -/** - * Internal function for auv_model_acados_create: step 7 - */ -void auv_model_acados_set_nlp_out(auv_model_solver_capsule* capsule) -{ - const int N = capsule->nlp_solver_plan->N; - ocp_nlp_config* nlp_config = capsule->nlp_config; - ocp_nlp_dims* nlp_dims = capsule->nlp_dims; - ocp_nlp_out* nlp_out = capsule->nlp_out; - ocp_nlp_in* nlp_in = capsule->nlp_in; - - // initialize primal solution - double* xu0 = calloc(NX+NU, sizeof(double)); - double* x0 = xu0; - - // initialize with x0 - - - double* u0 = xu0 + NX; - - for (int i = 0; i < N; i++) - { - // x0 - ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, i, "x", x0); - // u0 - ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, i, "u", u0); - } - ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, N, "x", x0); - free(xu0); -} - - -/** - * Internal function for auv_model_acados_create: step 9 - */ -int auv_model_acados_create_precompute(auv_model_solver_capsule* capsule) { - int status = ocp_nlp_precompute(capsule->nlp_solver, capsule->nlp_in, capsule->nlp_out); - - if (status != ACADOS_SUCCESS) { - printf("\nocp_nlp_precompute failed!\n\n"); - exit(1); - } - - return status; -} - - -int auv_model_acados_create_with_discretization(auv_model_solver_capsule* capsule, int N, double* new_time_steps) -{ - // If N does not match the number of shooting intervals used for code generation, new_time_steps must be given. - if (N != AUV_MODEL_N && !new_time_steps) { - fprintf(stderr, "auv_model_acados_create_with_discretization: new_time_steps is NULL " \ - "but the number of shooting intervals (= %d) differs from the number of " \ - "shooting intervals (= %d) during code generation! Please provide a new vector of time_stamps!\n", \ - N, AUV_MODEL_N); - return 1; - } - - // number of expected runtime parameters - capsule->nlp_np = NP; - - // 1) create and set nlp_solver_plan; create nlp_config - capsule->nlp_solver_plan = ocp_nlp_plan_create(N); - auv_model_acados_create_set_plan(capsule->nlp_solver_plan, N); - capsule->nlp_config = ocp_nlp_config_create(*capsule->nlp_solver_plan); - - // 2) create and set dimensions - capsule->nlp_dims = auv_model_acados_create_setup_dimensions(capsule); - - // 3) create and set nlp_opts - capsule->nlp_opts = ocp_nlp_solver_opts_create(capsule->nlp_config, capsule->nlp_dims); - auv_model_acados_create_set_opts(capsule); - - // 4) create and set nlp_out - // 4.1) nlp_out - capsule->nlp_out = ocp_nlp_out_create(capsule->nlp_config, capsule->nlp_dims); - // 4.2) sens_out - capsule->sens_out = ocp_nlp_out_create(capsule->nlp_config, capsule->nlp_dims); - auv_model_acados_set_nlp_out(capsule); - - // 5) create nlp_in - capsule->nlp_in = ocp_nlp_in_create(capsule->nlp_config, capsule->nlp_dims); - - // 6) setup functions, nlp_in and default parameters - auv_model_acados_create_setup_functions(capsule); - auv_model_acados_setup_nlp_in(capsule, N, new_time_steps); - auv_model_acados_create_set_default_parameters(capsule); - - // 7) create solver - capsule->nlp_solver = ocp_nlp_solver_create(capsule->nlp_config, capsule->nlp_dims, capsule->nlp_opts, capsule->nlp_in); - - - // 8) do precomputations - int status = auv_model_acados_create_precompute(capsule); - - return status; -} - -/** - * This function is for updating an already initialized solver with a different number of qp_cond_N. It is useful for code reuse after code export. - */ -int auv_model_acados_update_qp_solver_cond_N(auv_model_solver_capsule* capsule, int qp_solver_cond_N) -{ - printf("\nacados_update_qp_solver_cond_N() not implemented, since no partial condensing solver is used!\n\n"); - exit(1); -} - - -int auv_model_acados_reset(auv_model_solver_capsule* capsule, int reset_qp_solver_mem) -{ - - // set initialization to all zeros - - const int N = capsule->nlp_solver_plan->N; - ocp_nlp_config* nlp_config = capsule->nlp_config; - ocp_nlp_dims* nlp_dims = capsule->nlp_dims; - ocp_nlp_out* nlp_out = capsule->nlp_out; - ocp_nlp_in* nlp_in = capsule->nlp_in; - ocp_nlp_solver* nlp_solver = capsule->nlp_solver; - - double* buffer = calloc(NX+NU+NZ+2*NS+2*NSN+2*NS0+NBX+NBU+NG+NH+NPHI+NBX0+NBXN+NHN+NH0+NPHIN+NGN, sizeof(double)); - - for(int i=0; inlp_config, capsule->nlp_dims, capsule->nlp_in, stage, "parameter_values", p); - - return solver_status; -} - - -int auv_model_acados_update_params_sparse(auv_model_solver_capsule * capsule, int stage, int *idx, double *p, int n_update) -{ - ocp_nlp_in_set_params_sparse(capsule->nlp_config, capsule->nlp_dims, capsule->nlp_in, stage, idx, p, n_update); - - return 0; -} - - -int auv_model_acados_set_p_global_and_precompute_dependencies(auv_model_solver_capsule* capsule, double* data, int data_len) -{ - - // printf("No global_data, auv_model_acados_set_p_global_and_precompute_dependencies does nothing.\n"); - return 0; -} - - - - -int auv_model_acados_solve(auv_model_solver_capsule* capsule) -{ - // solve NLP - int solver_status = ocp_nlp_solve(capsule->nlp_solver, capsule->nlp_in, capsule->nlp_out); - - return solver_status; -} - - - -int auv_model_acados_setup_qp_matrices_and_factorize(auv_model_solver_capsule* capsule) -{ - int solver_status = ocp_nlp_setup_qp_matrices_and_factorize(capsule->nlp_solver, capsule->nlp_in, capsule->nlp_out); - - return solver_status; -} - - - - - - -int auv_model_acados_free(auv_model_solver_capsule* capsule) -{ - // before destroying, keep some info - const int N = capsule->nlp_solver_plan->N; - // free memory - ocp_nlp_solver_opts_destroy(capsule->nlp_opts); - ocp_nlp_in_destroy(capsule->nlp_in); - ocp_nlp_out_destroy(capsule->nlp_out); - ocp_nlp_out_destroy(capsule->sens_out); - ocp_nlp_solver_destroy(capsule->nlp_solver); - ocp_nlp_dims_destroy(capsule->nlp_dims); - ocp_nlp_config_destroy(capsule->nlp_config); - ocp_nlp_plan_destroy(capsule->nlp_solver_plan); - - /* free external function */ - // dynamics - for (int i = 0; i < N; i++) - { - external_function_external_param_casadi_free(&capsule->impl_dae_fun[i]); - external_function_external_param_casadi_free(&capsule->impl_dae_fun_jac_x_xdot_z[i]); - external_function_external_param_casadi_free(&capsule->impl_dae_jac_x_xdot_u_z[i]); - - } - free(capsule->impl_dae_fun); - free(capsule->impl_dae_fun_jac_x_xdot_z); - free(capsule->impl_dae_jac_x_xdot_u_z); - - - // cost - - // constraints - - - - return 0; -} - - -void auv_model_acados_print_stats(auv_model_solver_capsule* capsule) -{ - int nlp_iter, stat_m, stat_n, tmp_int; - ocp_nlp_get(capsule->nlp_solver, "nlp_iter", &nlp_iter); - ocp_nlp_get(capsule->nlp_solver, "stat_n", &stat_n); - ocp_nlp_get(capsule->nlp_solver, "stat_m", &stat_m); - - - int stat_n_max = 16; - if (stat_n > stat_n_max) - { - printf("stat_n_max = %d is too small, increase it in the template!\n", stat_n_max); - exit(1); - } - double stat[1616]; - ocp_nlp_get(capsule->nlp_solver, "statistics", stat); - - int nrow = nlp_iter+1 < stat_m ? nlp_iter+1 : stat_m; - - - printf("iter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter\talpha"); - if (stat_n > 8) - printf("\t\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp"); - printf("\n"); - for (int i = 0; i < nrow; i++) - { - for (int j = 0; j < stat_n + 1; j++) - { - if (j == 0 || j == 5 || j == 6) - { - tmp_int = (int) stat[i + j * nrow]; - printf("%d\t", tmp_int); - } - else - { - printf("%e\t", stat[i + j * nrow]); - } - } - printf("\n"); - } -} - -int auv_model_acados_custom_update(auv_model_solver_capsule* capsule, double* data, int data_len) -{ - (void)capsule; - (void)data; - (void)data_len; - printf("\ndummy function that can be called in between solver calls to update parameters or numerical data efficiently in C.\n"); - printf("nothing set yet..\n"); - return 1; - -} - - - -ocp_nlp_in *auv_model_acados_get_nlp_in(auv_model_solver_capsule* capsule) { return capsule->nlp_in; } -ocp_nlp_out *auv_model_acados_get_nlp_out(auv_model_solver_capsule* capsule) { return capsule->nlp_out; } -ocp_nlp_out *auv_model_acados_get_sens_out(auv_model_solver_capsule* capsule) { return capsule->sens_out; } -ocp_nlp_solver *auv_model_acados_get_nlp_solver(auv_model_solver_capsule* capsule) { return capsule->nlp_solver; } -ocp_nlp_config *auv_model_acados_get_nlp_config(auv_model_solver_capsule* capsule) { return capsule->nlp_config; } -void *auv_model_acados_get_nlp_opts(auv_model_solver_capsule* capsule) { return capsule->nlp_opts; } -ocp_nlp_dims *auv_model_acados_get_nlp_dims(auv_model_solver_capsule* capsule) { return capsule->nlp_dims; } -ocp_nlp_plan_t *auv_model_acados_get_nlp_plan(auv_model_solver_capsule* capsule) { return capsule->nlp_solver_plan; } diff --git a/control/velocity_controller/scripts/build_auv_solver/acados_solver_auv_model.h b/control/velocity_controller/scripts/build_auv_solver/acados_solver_auv_model.h deleted file mode 100644 index 8fd75b39a..000000000 --- a/control/velocity_controller/scripts/build_auv_solver/acados_solver_auv_model.h +++ /dev/null @@ -1,174 +0,0 @@ -/* - * Copyright (c) The acados authors. - * - * This file is part of acados. - * - * The 2-Clause BSD License - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE.; - */ - -#ifndef ACADOS_SOLVER_auv_model_H_ -#define ACADOS_SOLVER_auv_model_H_ - -#include "acados/utils/types.h" - -#include "acados_c/ocp_nlp_interface.h" -#include "acados_c/external_function_interface.h" - -#define AUV_MODEL_NX 9 -#define AUV_MODEL_NZ 0 -#define AUV_MODEL_NU 3 -#define AUV_MODEL_NP 0 -#define AUV_MODEL_NP_GLOBAL 0 -#define AUV_MODEL_NBX 1 -#define AUV_MODEL_NBX0 9 -#define AUV_MODEL_NBU 3 -#define AUV_MODEL_NSBX 0 -#define AUV_MODEL_NSBU 0 -#define AUV_MODEL_NSH 0 -#define AUV_MODEL_NSH0 0 -#define AUV_MODEL_NSG 0 -#define AUV_MODEL_NSPHI 0 -#define AUV_MODEL_NSHN 0 -#define AUV_MODEL_NSGN 0 -#define AUV_MODEL_NSPHIN 0 -#define AUV_MODEL_NSPHI0 0 -#define AUV_MODEL_NSBXN 0 -#define AUV_MODEL_NS 0 -#define AUV_MODEL_NS0 0 -#define AUV_MODEL_NSN 0 -#define AUV_MODEL_NG 0 -#define AUV_MODEL_NBXN 0 -#define AUV_MODEL_NGN 0 -#define AUV_MODEL_NY0 8 -#define AUV_MODEL_NY 8 -#define AUV_MODEL_NYN 5 -#define AUV_MODEL_N 20 -#define AUV_MODEL_NH 0 -#define AUV_MODEL_NHN 0 -#define AUV_MODEL_NH0 0 -#define AUV_MODEL_NPHI0 0 -#define AUV_MODEL_NPHI 0 -#define AUV_MODEL_NPHIN 0 -#define AUV_MODEL_NR 0 - -#ifdef __cplusplus -extern "C" { -#endif - - -// ** capsule for solver data ** -typedef struct auv_model_solver_capsule -{ - // acados objects - ocp_nlp_in *nlp_in; - ocp_nlp_out *nlp_out; - ocp_nlp_out *sens_out; - ocp_nlp_solver *nlp_solver; - void *nlp_opts; - ocp_nlp_plan_t *nlp_solver_plan; - ocp_nlp_config *nlp_config; - ocp_nlp_dims *nlp_dims; - - // number of expected runtime parameters - unsigned int nlp_np; - - /* external functions */ - - // dynamics - - external_function_external_param_casadi *impl_dae_fun; - external_function_external_param_casadi *impl_dae_fun_jac_x_xdot_z; - external_function_external_param_casadi *impl_dae_jac_x_xdot_u_z; - external_function_external_param_casadi *impl_dae_jac_p; - - - - - // cost - - - - - - - // constraints - - - - - - - -} auv_model_solver_capsule; - -ACADOS_SYMBOL_EXPORT auv_model_solver_capsule * auv_model_acados_create_capsule(void); -ACADOS_SYMBOL_EXPORT int auv_model_acados_free_capsule(auv_model_solver_capsule *capsule); - -ACADOS_SYMBOL_EXPORT int auv_model_acados_create(auv_model_solver_capsule * capsule); - -ACADOS_SYMBOL_EXPORT int auv_model_acados_reset(auv_model_solver_capsule* capsule, int reset_qp_solver_mem); - -/** - * Generic version of auv_model_acados_create which allows to use a different number of shooting intervals than - * the number used for code generation. If new_time_steps=NULL and n_time_steps matches the number used for code - * generation, the time-steps from code generation is used. - */ -ACADOS_SYMBOL_EXPORT int auv_model_acados_create_with_discretization(auv_model_solver_capsule * capsule, int n_time_steps, double* new_time_steps); -/** - * Update the time step vector. Number N must be identical to the currently set number of shooting nodes in the - * nlp_solver_plan. Returns 0 if no error occurred and a otherwise a value other than 0. - */ -ACADOS_SYMBOL_EXPORT int auv_model_acados_update_time_steps(auv_model_solver_capsule * capsule, int N, double* new_time_steps); -/** - * This function is used for updating an already initialized solver with a different number of qp_cond_N. - */ -ACADOS_SYMBOL_EXPORT int auv_model_acados_update_qp_solver_cond_N(auv_model_solver_capsule * capsule, int qp_solver_cond_N); -ACADOS_SYMBOL_EXPORT int auv_model_acados_update_params(auv_model_solver_capsule * capsule, int stage, double *value, int np); -ACADOS_SYMBOL_EXPORT int auv_model_acados_update_params_sparse(auv_model_solver_capsule * capsule, int stage, int *idx, double *p, int n_update); -ACADOS_SYMBOL_EXPORT int auv_model_acados_set_p_global_and_precompute_dependencies(auv_model_solver_capsule* capsule, double* data, int data_len); - -ACADOS_SYMBOL_EXPORT int auv_model_acados_solve(auv_model_solver_capsule * capsule); -ACADOS_SYMBOL_EXPORT int auv_model_acados_setup_qp_matrices_and_factorize(auv_model_solver_capsule* capsule); - - - -ACADOS_SYMBOL_EXPORT int auv_model_acados_free(auv_model_solver_capsule * capsule); -ACADOS_SYMBOL_EXPORT void auv_model_acados_print_stats(auv_model_solver_capsule * capsule); -ACADOS_SYMBOL_EXPORT int auv_model_acados_custom_update(auv_model_solver_capsule* capsule, double* data, int data_len); - -ACADOS_SYMBOL_EXPORT ocp_nlp_in *auv_model_acados_get_nlp_in(auv_model_solver_capsule * capsule); -ACADOS_SYMBOL_EXPORT ocp_nlp_out *auv_model_acados_get_nlp_out(auv_model_solver_capsule * capsule); -ACADOS_SYMBOL_EXPORT ocp_nlp_out *auv_model_acados_get_sens_out(auv_model_solver_capsule * capsule); -ACADOS_SYMBOL_EXPORT ocp_nlp_solver *auv_model_acados_get_nlp_solver(auv_model_solver_capsule * capsule); -ACADOS_SYMBOL_EXPORT ocp_nlp_config *auv_model_acados_get_nlp_config(auv_model_solver_capsule * capsule); -ACADOS_SYMBOL_EXPORT void *auv_model_acados_get_nlp_opts(auv_model_solver_capsule * capsule); -ACADOS_SYMBOL_EXPORT ocp_nlp_dims *auv_model_acados_get_nlp_dims(auv_model_solver_capsule * capsule); -ACADOS_SYMBOL_EXPORT ocp_nlp_plan_t *auv_model_acados_get_nlp_plan(auv_model_solver_capsule * capsule); - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif // ACADOS_SOLVER_auv_model_H_ diff --git a/control/velocity_controller/scripts/build_auv_solver/acados_solver_auv_model.o b/control/velocity_controller/scripts/build_auv_solver/acados_solver_auv_model.o deleted file mode 100644 index 17dcd34d54974794d4f68bd95dce9a5b47a9c28e..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 35504 zcmcJ23w&Hvwf9Mzv}u7%3bo{7l>-i#LW!9^6TTvuHfc{_(ne{aKy^AzCJ#&|Au~f; zC{RpO8Ha)TKnp73^8*#d2iGDXDQyX;2%;j0Lcs@A9xZU?ExrG>_TDpVcIH&>cYojc zF*)b__kXRu_S$Pd&OZB;%UdE#CzO>r<|uPM>=f<{>NxdV3)fA2+2kDQOmo8Lc)9n` zefaF~s@BZE!Yfyu*XTB!^fI~hm)+A zdC|)}*_>V5@GRdVZMDL@Y=uHN4=>XkT+Ni$Y}zFi)sWK$Kv^|$i_)hTrBBbNYrRIz zTCIcF$YX7hj+!DJHF+J#a3^mx12O6K{qeh^l#;Y6AE&{|AAcX#DZMyyOq=S^>{G22l7r{ z@X&VN+Z>#13O6|~vzas;(l7a1j|2yG9H=0p${A$;QRUArN`q~)Npn8AvM3q0R2I_e zi_#!kUnDxuPM%kkyg?^JZUZIg=+QYJ!4QXZaY7{6VC!w5YE*JM(zV=G!Ocj?hvgQ& zEls?#m`Jy!iQ9^abX%IZy_iV1iU_AKvWu{&QzlO-N`}*?_~db8aUOJA+L)V)iF8|< zcxy3{Zc7t)6cg!I5mCIUw*6Cml#CWDq1)2A-B(Pc+k9?KMY$oInvhyZZnl%rp@?M9 zPWB4Pb~HVNBoR$?tE{kzj}{Z@wlr~XF_CUd6A2Yr(`{+ufnp-vmL|SlOr%>yLt@Q}z5mCl7nshR`SrlFSZ*ip0=7sJw-OG_G$5jO=Gg zWgw;_UgoWE=D)_4RRhM%V{&+`g|3|8cgg}+UyF;}*#}#5mzIZK^>#izL6xLqcX+IM zglUoNC+fZ2Y`*cbXV!YzvtWJRA7kZYl`~d8fejuWE%zGtr1zOj!ozQurKX2-D}#0( z$lw&tz#Ch_^AswgDjfpC@jC9!n_#k!pk`82jq2y_x~m)t_H`dTc+ku7AoR1(1Mt*r z#LJyV>bX230EE9XZy``TF1TJHfaOSBT&hI08yj<`R} z1l_=_mBH%3teHVo`I#l?`o^A{On%8;;&lZE&|2^p^T9~jeuN;drw3cF*hAf1VA`_! zz_b-jfobQovv{QOh4g*M{{+rIvtL=q&IhWdZZkMBFoV&+3C9aDN87S1yynbr zBAKV#aVk5Xq|#ksQfFRqOCQWsyc0 z6BxM!&GF}zFz#;p!x@C`hi)is&Fs`Uv}X3;A%hbmy7yEvO-2T#?+ezlB$4H)jU=>= zw#>el!+~kH^q>vqwYE5n-3p8^Ue_1by0|EjohPz`PMuwml`q2c)i4&PZS*ilXBz(K z{HQ*=81G7GuQ{Z4cE}#6X|Gl4+XqSK4kJ|Tm`VdTXBP!qGxvsv52E`%5fK!=3s&Ia zOO1!knSE`jOEW%^+l{B2Gtai<`pa4x_op9=WL^tr#=<E~emr@@bA- z*_u(FzKy(3=xxU3w~@=yZV)l3As;&qgop1f3pb9D9s9=_Au9zv3bKb^T>Ng-{=ZTC z|3>Zq8_EBHV>iPM|C@Yeb)l`Pd^b?b+dwt5gKB2myDyh;YXv0vmMX7wTfn4(8O62! zA9+E{IdmH`jRDPI4X+0j{A{Y_E2BoPSRM85$_}GWDM7=eO3pl~>#I4lD2Ni*;J5uI z$k6#weXge($NgHIi!8o7Xtb+XIuf_&iFuU6nS9`kHMx~IA$|kXDg)D+bT5u6SW9RW z1q`1Z$!-dwv!tMns6ic;Me@#hL-BNy=4EE9(Frs=xi!~JuD`|$@nwi8LR04aaH8bYr zyqd1S^wGdH_^y^w%EK^JQj;4#FMNJ;c6P93GnW# z;g-y^Sdf8x52i36DzA!uA)j276hREIbx}yw5Jw3@O@4KU)3LGdv)@hgHAa+I$OmH%x-fWRjB!)u;YGnR@>i}2zHJUxqKBCj*g`#A z1j8+tPj!3Sipm#0r>F?5ay86n4;b^w5u)_xkyDflfEEggnrC_Rn&;9g*2f#M-j?1L z!GLYbV^0U)LX)8BAj(~srmN?Y`~wJTl3PT!oCfkpE=94#vo18M2#@oTVExGGWmxRN zv;h6h8|cs5s0Ovw^jx7VYg`meKgn|iXrl-~D>|6lQ08?U^maZokx3?U%#?yNhilQQ zbeyReZgO$Pyr>EDqNaBI*5Wr9n6`N{oQ~5roM7RVH{y)k*x2fk(ZH2|1VDM$aOkyU z-f7G1r_j{#*OqyMCJ)wlME!s3@R*wQv^4(n(nmRH$VljUjmyI3v)TJ9UORYD>w7mb zWughi+}Dn29X(%SmdFZK&|B; zc{`sf_ox@orEYQB${uXXl~3_HeqHuzYwqw2ck{W!ot=9pcpdi}q$cymtbO9X*6Vn% zY(IJdBDCd#w|gD$l)dP6K*gflk<|~WYjG))2Cw5l*|Xku0^4%aulU5CD!N&81u>d3 z`*%K5o_T%Neo)Y*Py)nfBe5^op6v^I7=5mM6Q%MxnuCbimBAh_vnIIKgVP)6p`H4B z>J|nuxMNUgCqcX-=?gaH>&K(VhcH$V@T}zEiNC_31$zKAIMA7NV0Ea%sZ{q9siu)k zG!m``2bYY|iLe?iPS>^K+AXYBC<#&mWi&kFL?aPpY0eCzL1IY*3+925c3Npb*-$_f zl}a>7g~6f$7ONuY1F>!t2{whtCaS`aI?@+Z=gA5!qk`x)m@!I0WG{YujEV(keV!v} zjI)M2-t#uWxHyt8P)B9Bna(#6IDBn*Yy!;^_$dr2N9mL=2RAc{=kmO8Avw3E0yO<61xB0VW>BcstompB1W>x zYc#5@f^(=w+cBqV3H>#Ku&$>_-q4zP!pl4t$?P%xG9>GpX>5K1y`>%@mgmZAb#c&> zt5lr?Qa*J#bpv*4Uas0s$;(l^xra=x@l9QkEB}z47#`k@(RhyLIBlGOuBPbD#alQwI;WW-sD2MbtM))y`q2dI&3GB1+A}3H5cW zQG^IEqh4#ftiCl{)tWuqC0UaO)DDSYIsd1iV6Y5^<0A5U@BQw zFgjnWg2CcSRW?y2?7KwI18II1diduwpAWm)^zlVMvmiT zc(knXp}_KoLZji#BY91gfoooYbMkRj2&tBlR|8l72x+Yy_hNeCsTBb3)V$oxJxIWs zHm?eJSzahuo}0H98P6OU-FVV-=fH1xJWSO^GB2`De)?%GdxCSOr#*Vio4M&j;oP#f zusT~kY54WB^$)xpnEepO2$q1MpcxUSU-}zeQcb{!sMZ;1x>4EMxH~X1z!PBQ=lpB1 zbOhb~4tT!M-R~;a#AfaLCt5M9{uyf6RNuI+mbs!qnD$&kV+n{IK`#>V3ULd0QS}Ge z$=3ZcB5W?CFRQlhYpnZv>)y5Q8?5`A=wNIp)~{19iuI-AFyg?7whZ2IL11=(tUl=WGX%w z?M%n|oX(AjSbuLvGTJfF*Ei6iIH`?;@j^->-OmC^8;W(uB^FdqfFf|}@ zx**mO9g3%uai=eSQ9Q9OKGYrUj}2XzjtzCDocM-R6y^^>buu+D6laC86mI(lx}$?b zz5Ow5e_~*$pFt{!DiTA;jLI#L#oZ84h-jqWk$y7{T z#4qIg-h{sB80a5#`ePeZ(cPq}Ff|#A4}-xZ3*aI-KS1 zX{WhloK73;izQC^@N%cOf3Ppw8H-1|(uty*D8<}{=!VXLRP^FPia^uO#-VtZbB=TN z2IuUw)7!aW-3F%*zchY)7w1zNU>acBL6S3dpkpvfh71nH$vf#(-0h0>_QgAon>y7= zY;Y23CvmZpKy41O<%Acq7LCX#fKq^oY6@msec?j%s_J-F?0l6_7R z*~j{wi=DwNHoU2by4a`&^L5NyN6<()MAmJi9kd>M`n5u z^%sw(di&8Iqeo4i>UP5g2^T#kdNp^T%cV4z)7;wA+!H!yxt+aTU1&RQ*U&(}o9cPXL`NL3ix!#cjrG9}^q2|2XkXl) zktZ+&azA1GS`<}k$UoIh4Q(W;)Brck6M9Kyte+Y=VhELq@iIP?Ko;opcsxkfxh*#4 z_QaBIkt~sV$=~^At~!SV_0KMaDp{BsK(-ig`(~qBsG1RXy(nvfD=!sCnGu0Hf~cdJ zg5I7-Qoe{^IEBwNnD7PlkyFTl=!fXcBC)=~o)~;Eg^;|wzJ{Q_VkP>TN1~~n>HhwW z`EtXgSjz2)CERsrRInNC2!^hUr`E^OCbiosfy(8o#w)!EXCkuVL%ra*)HA#BuCCsW zUe!3EYDvLYRAuAZ0ea4~8V)e)Dv_?+320Fou}DYZIB@#4kDU1IVRh3!e983hFMI0Z zEvM&Rxb4Q*?!EQHiZi|G&dj4<{L#Z-Ui9|OFFoA0uKjc^+r;M!UT->O$;{`TUU1Cm z*I!4+?Wf;y*B>5xa?|6dUE8_wsmuTMtp6RETtnp+pDt3RayLnUrQm(7_| zS3hr-hFJyar};hQv26J&%6F77UTnshxBB5+cJW7@vJKT`?>&5SWM9s_u}Hbo2!~8 z3?b(wC35blYFIiBZh*?yNiU8Qs4}2GNBXat7_M@+mc#zxisq{N38Pc1YJr4JNtzFH zNOm5{;5eQtv6Sqxs=*1(mMpwLJ{Z)pyz+s2+9A0`V=fI6g|f1r$CY(+*~Fz4RacgI zRoluY)|TOFanTp&2nT)J1l|v0wZqoXjNI&ZS zys1@{VFU}s$#$K#sS-B@pOmV9#9rCuD8~4TKI5?7s%;Y{5MkosrGxVfVsT%}c z(?RE%8asi2wyNs2IMr+HGdgarfp*g-e0?(E`|OT0*d3Si^|__u75d39pVxVug=_Ll zul5V2-Kx_*q|$2ItK75EWZ2$c=`_=iP#N}W%(QdD{>#E@fmKqcfa4ze1AkvN0a3qo zVsn){Tt4B4RW(bvDLc+;B+t{__fQ=b>ZVk@F00z%Myhr+;kPzYbs#?Rvt=dw2O z54LID4Y;SW51?>l^GzD3aG-;*J2gh{@#rAzp%U0%OJJ{;zz)}qQtERpjQlvy!YFUQ zgTkcTdFf!O^og|R-#Xrl`w0T)z8Bac{K_ywxH!{soH{ul#hGw(yx&~lJW=2V?%@kl zLW4Sv8Ysu(Db05aRKB{{2j8r5iYa+)%r9ZG2f6K~2iFd{N<0yvXBv zzNv6#Nf6gx<@-v0TP>5m$u|{FvjlN{v&N$q{(X&avhW{k{0kO-yT+BiLgL*TzpDW8 z)gFyMY~jDu_}?x3H+-+!hD`c{=D#69T>n|)M_3{E7me3g_&$w)(8B3s2AWMQvhaUu ze1(O-rSX`Bt4}^FoL&o`%=Z<}dJ9)}S91TzD)pbEH2;$p|35UIweVv#{#gs>c>v1w zB?~`U^S^1~A-Ow(RZ{cEEdEB#x66Au)C7R zQT12hylLS){-VBS=y`%s^+o1YI_(C~=dWn}Ct3V&Xnd)K->mVdh5tb7r*Tjow`qRT z;@_e15ewg`^{kajaIO4U;aq3&AJBSkvGCt&Jw2A5$29*wi~pp?|8C)b)A-94{+!11 z4-7h=mo$F3o=ceQU(tq1D3Q ztNCYJ_;DH^wD1pVe5-|@!gwVXB#V!^nxD1!jT+Bc_~{zoX5lT2PvJR?%;8MU|D47D zh{mt8@N+c&6$}5E#=mCa9U8yM!h1FTeG4DZ_>U|+&G?jY{n90jqrF@8eudWapO&6a zX?%x;=QRE^3;&$fU(M%&=T!Tta2~bzU)6dZxA1Rk{0R$J?XALj%EEu5`G2+WJ2d`` zh2Nv`mn{5#jUTk|hcrG>&r?mw{-E*67XF0B54Z5YX?&W6KdJ;$F*?vJXPU*z`|9(fO(x&-WgiY2QB{lG=7qW2Q_|*g`cePkcH3D_*@Hb)c67m zZ_;?!!k1~h)xwuEKFR6fv#Bk|p^a-aUpmj?uhDpig?DKD0t>%D;}=@^g^W*fYAt+& z=5Mm_%Qb$Pg{%Iq!uh0ytNsh+vT*f`igwNV3g!Khwr9J=|GLh1tA*bTyt*KszuM`8 zKkb9R0{lok32oDlLS~KWDCAd4|1rRcf1~D`=i}?}rAR4$gXWKFzM7ZgCA*J)s=rct zuJFOX=!4(jgMSY=m$y>6OwC^%=K&x7?|ks5wVu%_N)NAT;c_Y-)k@i4?}IP%!7uQ^ zxBB2;_rdS=!T;ieABirel>G~Q@Gc+x8XtUz55CU_KMHYE%AWZ?_!=L4*a!cv5B``B zUWvG&_&J~-q~S(B7HZr*mulw*1LmZc~v&IL1A8_(Rlh)7cjmV?bhyO7jJn4gf$_M|N4}QB3{)i9$ybnGZ zYssbjaEuQ=+Xr6^ocuF-ta7~3d7j1}()bCwUWPU9xC&6uX^wNfkN$6K{*A{eKCgM= za*xq-yux|C6PM5V=&3{;lb!XNkFe)svBsM;Zul0{x4~~+QM(wxNG5u z*dR0R)ued!I53FKMbT*cM_X2|JU6;@)$%1PTURWPM(aW)(R!yYvZP@?{ax6|*9-Y? z1O08}RQ?m9`#JPyF8!HDe^_b(T`3v53Mn?zL(C7cK!_zmED~aw5DSGkqY!63hh^ue ztXOsq%g$lhIV?MeW#_Q$9G0EKvU6E>F3ZkUcCzeTmYvJ8b6IvS%g$xlxhy-6W#_T% zJeHlO3dyqbSau%E&STkmEIW^7=d+{zB$2WX?jCTF6ohnYmCgS+;?5X<(@amTF+B29|1M zypi!n#v7FlY+)noY-F8{th14IHgamHo>hnHSx2azm4xb9OQ@dJgz8yCsGe1X>RCsq zo|S~^l@=2+N{bE}9W^>^blm8`(UGG=N5xJ^1y4vtPiT&^Ma54@g-%GtQAmYQNX1Y{ z1yM-FPe=t&Xs#-Q3Z9UPo{$QwkczjE3Z@XE$!rqS+eWAXV*h5!507Gx7B)z9$I%{f z?)XHsqvsRY%-7pDe#?Pw*Lv3ub}7;pqVcq@p?ExMNaH$(ar>C^meDRsVQ9xd65GeH zr3Skt0Y{fbR$yyobj8x8XSJ-1t_&}Zv_!Fu1bYLAV0#zu>+2%x$6F*iB;E_rAMa_%3mcaO0d${`}uDfzJ)1P6!j~LMSGWa{X3UiT7BH3 zH>!!YrDI=hAm>eE!ULMFS`+Q}LoJ%VD!(s0-`t~C=+Pcq+D(Q2#t4b+%c7eoxV(@d zZ{Qde)c zjdu3-W5+yizsyTB247O&gZ;Nd$?++A`>)ZRmk@p%FGQVg9+g9gl|OmVg3x_ih?du= zkF&=+Pmd7TePwKZh`h<8q)%Fng_| z*aa(gXJarJjMG+3Z0k#o&zwt_&rRKxQjgIGzUYsouvrir9w`sC3%~c`g0V2;ki2n< zX_PL`d`JysopF2>Ft9P71~=Fc)xM4Cy0T#oI~&w-Zo@P-=k?}005kI1U4kBsxE$k3 zr*`?%&d);2=q?o?9cg^{(H~WDGQN%=!@Wn*kfVDRlSVzuI0Gn=zo_v|E$n$NElugl zQU2-tcvUiQk|y8#%c@7CORCKg!9Lu6kPT=b_PJBEn=J~w>r!{Az zC#i8;&sM$1M_}>w?4X0IgiuNFA{lVNlapZ+ld!|&6$&A(FcY3b1L&AtqqKPvdNv}gE# z^3nf>;MWNLbTSZ!tzW$lR`$~u#YTU`hre3mbUH@xFAzBG{W5w|LXWK9t%5&O@YQ?J z@%8vE!GE9N|G-E8PQj3j}!Ru0zc75PlMopK=509_z}Sm3VuxBwB%&$N%-hV3%=CzX@S#{lhN}3tg=RF5a&XUZGY zxXnLV@J|-}CV|fuc$<%&m4bhY;P(o=PT&{$=($wzX)W5=|5<_83;e4-dcH0AA;G^x z;P`eoum3(DJ%1GZxq@%@x>Bj;h8dZr8h0>Pgp@Pz_j z;G@SAd|E3u`L6QecME=_;HP}}^nQ^JD%Yv_8U3I0;a@NK|3~nDAaL4)YxLadqi2`k zOFfSYoIVmUdj964=Xt@G<9OL52;-n~(MKyrPnE{)`g)JxpDy?(3A{<*3w`u_Sn#Eu zvjiR%de-{r=@$IOg1<@NO9Y!T`Zwvev0{_1Pe^}r*3H&jE-z@OG z0{@P{pAqNeKK1!M{h~^vrAYJSg=4 zMBsY`|04o_(T9If@aY>BqyGpp0teYI_16phHl!K;VvSR|&JuW=;LHAJmB3}c*C}wR zf1}X9Qt00<@Kpl8Q|OWT?iRSr_qPI<`93T3pDpwsQ-vEGCee;8MRU@XtWb_-BsBsa|M`VenH0zFOezLXWgFA^7JC{zk$7Pl0Cy|2%8(TD#p!H)`l)ls;?Ve79Kxb$0t558F7(x2xET>5RD#;G3H;%EHub%Dz`zeVVg z^?Rq_%Q%0)hyS$T%Q%0*hhI4z#^E6Ud=5WjXS2r1emO5XTi|C1{-=bVYX$y2flI&r zMCg(FcM6<_dSg%HdvJq;>^T!ZgRjsy+5ZKBe?s80Tw8qbt9d&O-y!hZefWC?U+Vvx4}UtnoWMcl`Wb%44<~D!{L?A$If8$Oz!wSp zPJt&heiAVGJbtOr9~bhku^n4+#D`AAU;k z2L*q#5C5}*f1%*t;KRRF@P`EdP9OdQf}a%pKl|_x3VurPkE54uILM#7@iTrN^ueo@3wukykBeee;1%ko|$a5*k~Rp5JIm+{XI zf!{0e`+e}=3;d^o|Fpoz1pb1+zasE=1pZ@zPdXMiILL25!q3=wjKJ>`_!5m%J<51k zC-~Ao3Bi~3o)dgo?>7j(toQHx@b?P-dST}?f`5m=UlRD80&jA0gM<7a$I}a3LREjV z4rk+^v`eVs%kgx?WtjUPIi7xABVpgTQYV_@e^@{ zz#kL*-2(rkz#kC!;{ty~;C~YM>jM9?z{@{?8yr+#i68BQpCE8qUv)nCLLdApjZZ+n zSKw#-e5>F;DezIj|D?e8`0#%(_``z#M<4z_1%Hd+zvII{A_x)=vVR0WV}DTNj;navE!=zu`V$Lxj#vDLEZltWY5pA!V^5Rj zzi#o@YTW!g8HT@ExKsChUg)D%~*7c56V=kIoEY0Q+beX=-V;;yR!68_&Rm?7k%S(_(w@k z;SaF%eRm>VhkrPD5Z@p--V^Pje~wp`?0@&yg*Y{tNtT)>eLw5O;spKLArHP6 zS5zfUc|=`oI8U7$YSlS_6Xm0uuUQV^Y9d^Rr6S{Rua&P!Ag(-S>zgf$heR+m&%in;6Qt^N5CzXWycZ=}9D8((U zdmH&0m0U7!O1f`>B0TyYbtZr3@U!LV-HjdlBynSe{yNSdC{uCR{+ouhQu3Qel+Z0g JmM(4i{{~5-Bo6=p diff --git a/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun.c b/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun.c deleted file mode 100644 index acd0b61e9..000000000 --- a/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun.c +++ /dev/null @@ -1,394 +0,0 @@ -/* This file was automatically generated by CasADi 3.7.2. - * It consists of: - * 1) content generated by CasADi runtime: not copyrighted - * 2) template code copied from CasADi source: permissively licensed (MIT-0) - * 3) user code: owned by the user - * - */ -#ifdef __cplusplus -extern "C" { -#endif - -/* How to prefix internal symbols */ -#ifdef CASADI_CODEGEN_PREFIX - #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID) - #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID - #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID) -#else - #define CASADI_PREFIX(ID) auv_model_impl_dae_fun_ ## ID -#endif - -#include - -#ifndef casadi_real -#define casadi_real double -#endif - -#ifndef casadi_int -#define casadi_int int -#endif - -/* Add prefix to internal symbols */ -#define casadi_f0 CASADI_PREFIX(f0) -#define casadi_fabs CASADI_PREFIX(fabs) -#define casadi_s0 CASADI_PREFIX(s0) -#define casadi_s1 CASADI_PREFIX(s1) -#define casadi_s2 CASADI_PREFIX(s2) -#define casadi_s3 CASADI_PREFIX(s3) - -/* Symbol visibility in DLLs */ -#ifndef CASADI_SYMBOL_EXPORT - #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) - #if defined(STATIC_LINKED) - #define CASADI_SYMBOL_EXPORT - #else - #define CASADI_SYMBOL_EXPORT __declspec(dllexport) - #endif - #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) - #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default"))) - #else - #define CASADI_SYMBOL_EXPORT - #endif -#endif - -casadi_real casadi_fabs(casadi_real x) { -/* Pre-c99 compatibility */ -#if __STDC_VERSION__ < 199901L - return x>0 ? x : -x; -#else - return fabs(x); -#endif -} - -static const casadi_int casadi_s0[3] = {9, 1, 1}; -static const casadi_int casadi_s1[3] = {3, 1, 1}; -static const casadi_int casadi_s2[3] = {0, 1, 1}; -static const casadi_int casadi_s3[3] = {0, 0, 1}; - -/* auv_model_impl_dae_fun:(i0[9],i1[9],i2[3],i3[0],i4[],i5[0])->(o0[9]) */ -static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { - casadi_real a00, a01, a02, a03, a04, a05, a06, a07, a08, a09, a10, a11; - casadi_real a12, a13, a14; - a00=arg[1]? arg[1][0] : 0; - a01=3.3453634479321918e-02; - a02=arg[2]? arg[2][0] : 0; - a03=-30.; - a04=arg[0]? arg[0][5] : 0; - a05=(a03*a04); - a06=arg[0]? arg[0][1] : 0; - a05=(a05*a06); - a07=30.; - a08=arg[0]? arg[0][4] : 0; - a09=(a07*a08); - a10=arg[0]? arg[0][2] : 0; - a09=(a09*a10); - a05=(a05+a09); - a02=(a02-a05); - a05=23.; - a09=arg[0]? arg[0][0] : 0; - a11=casadi_fabs(a09); - a05=(a05+a11); - a05=(a05*a09); - a02=(a02-a05); - a01=(a01*a02); - a05=6.0368975005218254e-05; - a11=(a07*a04); - a11=(a11*a09); - a12=arg[0]? arg[0][3] : 0; - a13=(a03*a12); - a13=(a13*a10); - a11=(a11+a13); - a13=46.; - a14=casadi_fabs(a06); - a14=(a13+a14); - a14=(a14*a06); - a11=(a11+a14); - a05=(a05*a11); - a01=(a01-a05); - a05=-1.1116270017027866e-07; - a03=(a03*a08); - a03=(a03*a09); - a07=(a07*a12); - a07=(a07*a06); - a03=(a03+a07); - a07=casadi_fabs(a10); - a07=(a13+a07); - a07=(a07*a10); - a03=(a03+a07); - a05=(a05*a03); - a01=(a01-a05); - a05=9.8084735444363873e-08; - a07=3.3399999999999999e+00; - a07=(a07*a04); - a07=(a07*a08); - a10=-3.3199999999999998e+00; - a10=(a10*a08); - a10=(a10*a04); - a07=(a07+a10); - a10=casadi_fabs(a12); - a10=(a13+a10); - a10=(a10*a12); - a07=(a07+a10); - a05=(a05*a07); - a01=(a01-a05); - a05=1.0920100546139184e-05; - a10=arg[2]? arg[2][1] : 0; - a06=-3.3399999999999999e+00; - a06=(a06*a04); - a06=(a06*a12); - a09=6.8000000000000005e-01; - a09=(a09*a12); - a09=(a09*a04); - a06=(a06+a09); - a10=(a10-a06); - a06=casadi_fabs(a08); - a06=(a13+a06); - a06=(a06*a08); - a10=(a10-a06); - a05=(a05*a10); - a01=(a01+a05); - a05=-6.0150572994295574e-03; - a06=arg[2]? arg[2][2] : 0; - a09=3.3199999999999998e+00; - a09=(a09*a08); - a09=(a09*a12); - a14=-6.8000000000000005e-01; - a14=(a14*a12); - a14=(a14*a08); - a09=(a09+a14); - a06=(a06-a09); - a09=casadi_fabs(a04); - a13=(a13+a09); - a13=(a13*a04); - a06=(a06-a13); - a05=(a05*a06); - a01=(a01+a05); - a00=(a00-a01); - if (res[0]!=0) res[0][0]=a00; - a00=arg[1]? arg[1][1] : 0; - a01=6.0368975005218423e-05; - a01=(a01*a02); - a05=3.3484658136227786e-02; - a05=(a05*a11); - a01=(a01-a05); - a05=-6.1658244361114702e-05; - a05=(a05*a03); - a01=(a01-a05); - a05=5.4404333259807184e-05; - a05=(a05*a07); - a01=(a01-a05); - a05=6.0570157695918683e-03; - a05=(a05*a10); - a01=(a01+a05); - a05=-3.0184487502609172e-03; - a05=(a05*a06); - a01=(a01+a05); - a00=(a00-a01); - if (res[0]!=0) res[0][1]=a00; - a00=arg[1]? arg[1][2] : 0; - a01=-1.1116270017027936e-07; - a01=(a01*a02); - a05=-6.1658244361114783e-05; - a05=(a05*a11); - a01=(a01-a05); - a05=3.3963490377380855e-02; - a05=(a05*a03); - a01=(a01-a05); - a05=-2.9967785627100754e-02; - a05=(a05*a07); - a01=(a01-a05); - a05=-3.0801331505514837e-03; - a05=(a05*a10); - a01=(a01+a05); - a05=5.5581350085139543e-06; - a05=(a05*a06); - a01=(a01+a05); - a00=(a00-a01); - if (res[0]!=0) res[0][2]=a00; - a00=arg[1]? arg[1][3] : 0; - a01=9.8084735444364535e-08; - a01=(a01*a02); - a05=5.4404333259807062e-05; - a05=(a05*a11); - a01=(a01-a05); - a05=-2.9967785627100469e-02; - a05=(a05*a03); - a01=(a01-a05); - a05=1.4970303990827358e+00; - a05=(a05*a07); - a01=(a01-a05); - a05=2.7177645446042520e-03; - a05=(a05*a10); - a01=(a01+a05); - a05=-4.9042367722181902e-06; - a05=(a05*a06); - a01=(a01+a05); - a00=(a00-a01); - if (res[0]!=0) res[0][3]=a00; - a00=arg[1]? arg[1][4] : 0; - a01=1.0920100546139210e-05; - a01=(a01*a02); - a05=6.0570157695918657e-03; - a05=(a05*a11); - a01=(a01-a05); - a05=-3.0801331505514781e-03; - a05=(a05*a03); - a01=(a01-a05); - a05=2.7177645446042498e-03; - a05=(a05*a07); - a01=(a01-a05); - a05=3.0257778596593993e-01; - a05=(a05*a10); - a01=(a01+a05); - a05=-5.4600502730695923e-04; - a13=(a05*a06); - a01=(a01+a13); - a00=(a00-a01); - if (res[0]!=0) res[0][4]=a00; - a00=arg[1]? arg[1][5] : 0; - a01=-6.0150572994295635e-03; - a01=(a01*a02); - a02=-3.0184487502609133e-03; - a02=(a02*a11); - a01=(a01-a02); - a02=5.5581350085139340e-06; - a02=(a02*a03); - a01=(a01-a02); - a02=-4.9042367722181935e-06; - a02=(a02*a07); - a01=(a01-a02); - a05=(a05*a10); - a01=(a01+a05); - a05=3.0075286497147785e-01; - a05=(a05*a06); - a01=(a01+a05); - a00=(a00-a01); - if (res[0]!=0) res[0][5]=a00; - a00=arg[1]? arg[1][6] : 0; - a01=arg[0]? arg[0][6] : 0; - a05=sin(a01); - a06=arg[0]? arg[0][7] : 0; - a10=tan(a06); - a02=(a05*a10); - a02=(a02*a08); - a12=(a12+a02); - a01=cos(a01); - a10=(a01*a10); - a10=(a10*a04); - a12=(a12+a10); - a00=(a00-a12); - if (res[0]!=0) res[0][6]=a00; - a00=arg[1]? arg[1][7] : 0; - a12=(a01*a08); - a10=(a05*a04); - a12=(a12-a10); - a00=(a00-a12); - if (res[0]!=0) res[0][7]=a00; - a00=arg[1]? arg[1][8] : 0; - a06=cos(a06); - a05=(a05/a06); - a05=(a05*a08); - a01=(a01/a06); - a01=(a01*a04); - a05=(a05+a01); - a00=(a00-a05); - if (res[0]!=0) res[0][8]=a00; - return 0; -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ - return casadi_f0(arg, res, iw, w, mem); -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_alloc_mem(void) { - return 0; -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_init_mem(int mem) { - return 0; -} - -CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_free_mem(int mem) { -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_checkout(void) { - return 0; -} - -CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_release(int mem) { -} - -CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_incref(void) { -} - -CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_decref(void) { -} - -CASADI_SYMBOL_EXPORT casadi_int auv_model_impl_dae_fun_n_in(void) { return 6;} - -CASADI_SYMBOL_EXPORT casadi_int auv_model_impl_dae_fun_n_out(void) { return 1;} - -CASADI_SYMBOL_EXPORT casadi_real auv_model_impl_dae_fun_default_in(casadi_int i) { - switch (i) { - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const char* auv_model_impl_dae_fun_name_in(casadi_int i) { - switch (i) { - case 0: return "i0"; - case 1: return "i1"; - case 2: return "i2"; - case 3: return "i3"; - case 4: return "i4"; - case 5: return "i5"; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const char* auv_model_impl_dae_fun_name_out(casadi_int i) { - switch (i) { - case 0: return "o0"; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const casadi_int* auv_model_impl_dae_fun_sparsity_in(casadi_int i) { - switch (i) { - case 0: return casadi_s0; - case 1: return casadi_s0; - case 2: return casadi_s1; - case 3: return casadi_s2; - case 4: return casadi_s3; - case 5: return casadi_s2; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const casadi_int* auv_model_impl_dae_fun_sparsity_out(casadi_int i) { - switch (i) { - case 0: return casadi_s0; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { - if (sz_arg) *sz_arg = 6; - if (sz_res) *sz_res = 1; - if (sz_iw) *sz_iw = 0; - if (sz_w) *sz_w = 0; - return 0; -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { - if (sz_arg) *sz_arg = 6*sizeof(const casadi_real*); - if (sz_res) *sz_res = 1*sizeof(casadi_real*); - if (sz_iw) *sz_iw = 0*sizeof(casadi_int); - if (sz_w) *sz_w = 0*sizeof(casadi_real); - return 0; -} - - -#ifdef __cplusplus -} /* extern "C" */ -#endif diff --git a/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun.o b/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun.o deleted file mode 100644 index 0dc1612cdaca9a4f10a4ca3b8fc76bd5eb4d0921..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 11584 zcmb`N4Rq7h6~O<60&QhU>Nf3wL8d_?LQK*YDk4dsg-olb)1t|Yv>{DwN55=RDS}v} zbvQ)pWFB-*@p$yG=@B+Pj>A+kJ_J920v({DFs;~57*M7%26p$p|9#2rqv@RT&dLA1 z_j~u=_jTXL|KHwQR6Ju)TAC(9nl@ES<|Ng$xHXw}i=<&hkER_Ga$`C%uc0tON<~vY8p))_5$l$7nJ|erjB_*SoC9ROIOF z13Lg}K#VPmGBgcD>#gwkAyxtL_b4tm^%(todyLsJO9skhM2yzWkgc#-!E$XUWIqz6 zW-Nt=e}~AT+WYYPYmd>lsmB;Nbkev;G(TdzwH1a1LxWJSkI@t>?O(ZxH#-+>K`hl_K&S4t% zpmsY*iN5*7&?4&SDNGLZLJJOmOtdm!!GU}iiz~n|MAep@9*@y?qIg+2BT@uYfR6(p zbP~pP7bq56>LIris@(`VF`)hqAQd>MzPAN)qX`i9oe&BzM*epo^<#Yu{n*uo7U+%t zV5HD!g{<#gKx0L*$LNRILT#wZ3r+HGf&tf6!RlK`I>5evJ3c<`DKh#tV#7-zeLU4d{>p~wtkHhIFpOc-0rF?Fk026i6 z%J#!l`gNb_qY!u#I83Xbwl8ih01LtoQ|zZLtxp+jPkx;4vx&OE`L+N&0kS_aErhNFa2pChUD>x4 z=y`XdZN*r7HkgE3V^gr%vZ7u`Ae};xEBT9L(6XO;>~zm zIpVk7?g@Bq_ssT`LHouO3im$-JKV82I{!O3s<^^Wfo5`|jOYy63&1ejANqI&dIX^Eq#lc5^ErJjWC3X8p-SZu=M4r$NTb~R{-B$KEo||C(PAm zIPN;Z0eszf-oR|aSq@w9c6`L)k$t@xpVO`A^sN`1?S->ZzdC?lbinTj{J@CyKz9%d zVRBnw!Zi(@t@T+T-KP+@ftFag-mQg}aumRXRVGX_VU~#unXqF*xMpSdmalqyjboaT zh+`6(F6t>i1NF>-N5m%U=k@te0qe`bgs@fL{kpKFLH9op#0-6alLqc6FehT=;9?CG zNY^hFOA<D!uy|^ z-xNvq=bMzP@@jtFQ0P9bKeV?FFQ_b!ZHBvNGf;8=T+%yaUY&Qjn70Rdy`l8=-n90q zy@!lr+_2jTcLj01{pTwWV<7eiY+q~ddTa`r_8N;e_8l*5pX)oePu8cl|C`kBZS9Q> z-8u|M)!X_dB!}XFdRuox61Hkgz6D9~8h~?~oXtoZ+9!SVaMDMwPpMyJ*Qy*^l~b$A z)vEHes(h_ZEDo&c{|@}!1k*AP$Ad`Ae5R6@74_=jUYV!3xo`fsOV3Y@Q|#8`Nfgg? zuN&R^`Ons6xgXy6g*EVSR(umi8$Wd&+?4go-^|ar){NeM)nm^aT~qEheU`QFo;T7L z^mdn)-aYlDzaOx#Ied8PrJ>88pZaXmhHWHwcFWak4qKjaxu-4qb^8^j4Zj;PZuQXn zhr6s#7ruS%&L-Djw{OD`d`;Dc zl?Pi|w32;OCsMu3$lh3z+x_0|v3pN`YEhW50BzY&Li$t_>IKv3a=zy zqVW4fK0}Nl6^ZDVDfolLxh$q15g=2;9h8U1L?-hb`@bbFt^`bB6>&3Tn0!v;GZX$V zi(epam4M>4#0wPuDsivEbzjcZ+zQ`F@^ai{@iyYpk0jqgyj%i`JBU{*{C(n03O_`= zUg3v{H!J)I@udpyA>O9&Q^Z#){A=Q?6`n49KT}(!@GRo`cqa=@#MdQ}NL@rcuJBRB z^>IxWUP63-5{Z*JcN_ZP(V_9l5f@f^j@UBu;nDK!=nH!Je@5!c6CN^9Cu;z^>X zmJ2>4alXm&M~Tb4j2|a%lz`%=iK{!}bHwG|FV$WkF84>t|495o2`K(E@C(_b$TM~i zaTCdja~9J704^Cfbs_~nnu32p_K#BsH-%<@}5%-bP%W&({+-;Y9?3Jfr6lw-fj2 z#o9yCjw1gj;%kY^^LhvI;|kY;jdzvR)K!G5%c^SXtIH~a;j&0$tu2(Ipe$-nS>jAt zlAE%`q1lS3JMl3QA9j4?;Uga(lkic14~HnZS|o*zBUhw_szYcvgo;DhaR^O^E-wr? zg#o8tpU`p&3r?Zs6c(I9*C}+Jx|XnzE3|TjghayTQwInfkDtwpP9Lv>Lt7OYJn zQ5LMOt_zjbglkd>RMl3+1}cvuZX57vbLSMdN9MC*eM(W=<|0Zb%B1~4Jfp~14U zIrC%TXyQH!+m?n~5Pba+VJ__}$Z?C1JjytpC6aGt9QQWKzaWn9bol8~@)5AlV^HnO za}+tZGlk?;J2#}@`aJ>PG5Xf4pV#Md{d_!^?_ll3Gf=kw0OJP6XJf-5V1#G3l)r~@ zJSQc8oN?aY_ZiP-^77(<#du~*JCk5P$AJ9%@RNKdpIcHE3}e=A{}`&&8V+~4Li zK9<@472_7hpJ1H(+aDOeoXM|eocr6`jB|h6%Q*M9&l%@_b%t^7hv^y62naaN+^YQnW(sXyeH^kIOXmlOzQstCsq=Sb?y@w)*Sir_mB567u~Gl!6t`-_Zk1J<9u{=Xucrhjiv z8jVX diff --git a/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u.c b/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u.c deleted file mode 100644 index 0321b6e18..000000000 --- a/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u.c +++ /dev/null @@ -1,847 +0,0 @@ -/* This file was automatically generated by CasADi 3.7.2. - * It consists of: - * 1) content generated by CasADi runtime: not copyrighted - * 2) template code copied from CasADi source: permissively licensed (MIT-0) - * 3) user code: owned by the user - * - */ -#ifdef __cplusplus -extern "C" { -#endif - -/* How to prefix internal symbols */ -#ifdef CASADI_CODEGEN_PREFIX - #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID) - #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID - #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID) -#else - #define CASADI_PREFIX(ID) auv_model_impl_dae_fun_jac_x_xdot_u_ ## ID -#endif - -#include - -#ifndef casadi_real -#define casadi_real double -#endif - -#ifndef casadi_int -#define casadi_int int -#endif - -/* Add prefix to internal symbols */ -#define casadi_f0 CASADI_PREFIX(f0) -#define casadi_fabs CASADI_PREFIX(fabs) -#define casadi_s0 CASADI_PREFIX(s0) -#define casadi_s1 CASADI_PREFIX(s1) -#define casadi_s2 CASADI_PREFIX(s2) -#define casadi_s3 CASADI_PREFIX(s3) -#define casadi_s4 CASADI_PREFIX(s4) -#define casadi_s5 CASADI_PREFIX(s5) -#define casadi_s6 CASADI_PREFIX(s6) -#define casadi_sign CASADI_PREFIX(sign) -#define casadi_sq CASADI_PREFIX(sq) - -/* Symbol visibility in DLLs */ -#ifndef CASADI_SYMBOL_EXPORT - #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) - #if defined(STATIC_LINKED) - #define CASADI_SYMBOL_EXPORT - #else - #define CASADI_SYMBOL_EXPORT __declspec(dllexport) - #endif - #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) - #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default"))) - #else - #define CASADI_SYMBOL_EXPORT - #endif -#endif - -casadi_real casadi_fabs(casadi_real x) { -/* Pre-c99 compatibility */ -#if __STDC_VERSION__ < 199901L - return x>0 ? x : -x; -#else - return fabs(x); -#endif -} - -casadi_real casadi_sign(casadi_real x) { return x<0 ? -1 : x>0 ? 1 : x;} - -casadi_real casadi_sq(casadi_real x) { return x*x;} - -static const casadi_int casadi_s0[3] = {9, 1, 1}; -static const casadi_int casadi_s1[3] = {3, 1, 1}; -static const casadi_int casadi_s2[3] = {0, 1, 1}; -static const casadi_int casadi_s3[3] = {0, 0, 1}; -static const casadi_int casadi_s4[60] = - {9, 9, 0, 6, 12, 18, 25, 34, - 43, 46, 48, 48, 0, 1, 2, 3, - 4, 5, 0, 1, 2, 3, 4, 5, - 0, 1, 2, 3, 4, 5, 0, 1, - 2, 3, 4, 5, 6, 0, 1, 2, - 3, 4, 5, 6, 7, 8, 0, 1, - 2, 3, 4, 5, 6, 7, 8, 6, - 7, 8, 6, 8}; -static const casadi_int casadi_s5[21] = - {9, 9, 0, 1, 2, 3, 4, 5, - 6, 7, 8, 9, 0, 1, 2, 3, - 4, 5, 6, 7, 8}; -static const casadi_int casadi_s6[24] = - {9, 3, 0, 6, 12, 18, 0, 1, - 2, 3, 4, 5, 0, 1, 2, 3, - 4, 5, 0, 1, 2, 3, 4, 5}; - -/* auv_model_impl_dae_fun_jac_x_xdot_u:(i0[9],i1[9],i2[3],i3[0],i4[],i5[0])->(o0[9],o1[9x9,48nz],o2[9x9,9nz],o3[9x3,18nz]) */ -static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { - casadi_real a00, a01, a02, a03, a04, a05, a06, a07, a08, a09, a10, a11; - casadi_real a12, a13, a14, a15, a16, a17, a18, a19, a20, a21, a22, a23; - casadi_real a24, a25, a26, a27, a28, a29, a30, a31, a32, a33, a34, a35; - casadi_real a36, a37, a38, a39, a40, a41, a42, a43, a44, a45, a46, a47; - casadi_real a48, a49, a50, a51, a52, a53, a54, a55, a56, a57, a58, a59; - casadi_real a60, a61, a62, a63, a64, a65, a66, a67, a68, a69, a70, a71; - casadi_real a72, a73, a74, a75, a76, a77, a78; - a00=arg[1]? arg[1][0] : 0; - a01=3.3453634479321918e-02; - a02=arg[2]? arg[2][0] : 0; - a03=-30.; - a04=arg[0]? arg[0][5] : 0; - a05=(a03*a04); - a06=arg[0]? arg[0][1] : 0; - a07=(a05*a06); - a08=30.; - a09=arg[0]? arg[0][4] : 0; - a10=(a08*a09); - a11=arg[0]? arg[0][2] : 0; - a12=(a10*a11); - a07=(a07+a12); - a02=(a02-a07); - a07=23.; - a12=arg[0]? arg[0][0] : 0; - a13=casadi_fabs(a12); - a07=(a07+a13); - a13=(a07*a12); - a02=(a02-a13); - a13=(a01*a02); - a14=6.0368975005218254e-05; - a15=(a08*a04); - a16=(a15*a12); - a17=arg[0]? arg[0][3] : 0; - a18=(a03*a17); - a19=(a18*a11); - a16=(a16+a19); - a19=46.; - a20=casadi_fabs(a06); - a20=(a19+a20); - a21=(a20*a06); - a16=(a16+a21); - a21=(a14*a16); - a13=(a13-a21); - a21=-1.1116270017027866e-07; - a22=(a03*a09); - a23=(a22*a12); - a24=(a08*a17); - a25=(a24*a06); - a23=(a23+a25); - a25=casadi_fabs(a11); - a25=(a19+a25); - a26=(a25*a11); - a23=(a23+a26); - a26=(a21*a23); - a13=(a13-a26); - a26=9.8084735444363873e-08; - a27=3.3399999999999999e+00; - a28=(a27*a04); - a29=(a28*a09); - a30=-3.3199999999999998e+00; - a31=(a30*a09); - a32=(a31*a04); - a29=(a29+a32); - a32=casadi_fabs(a17); - a32=(a19+a32); - a33=(a32*a17); - a29=(a29+a33); - a33=(a26*a29); - a13=(a13-a33); - a33=1.0920100546139184e-05; - a34=arg[2]? arg[2][1] : 0; - a35=-3.3399999999999999e+00; - a36=(a35*a04); - a37=(a36*a17); - a38=6.8000000000000005e-01; - a39=(a38*a17); - a40=(a39*a04); - a37=(a37+a40); - a34=(a34-a37); - a37=casadi_fabs(a09); - a37=(a19+a37); - a40=(a37*a09); - a34=(a34-a40); - a40=(a33*a34); - a13=(a13+a40); - a40=-6.0150572994295574e-03; - a41=arg[2]? arg[2][2] : 0; - a42=3.3199999999999998e+00; - a43=(a42*a09); - a44=(a43*a17); - a45=-6.8000000000000005e-01; - a46=(a45*a17); - a47=(a46*a09); - a44=(a44+a47); - a41=(a41-a44); - a44=casadi_fabs(a04); - a19=(a19+a44); - a44=(a19*a04); - a41=(a41-a44); - a44=(a40*a41); - a13=(a13+a44); - a00=(a00-a13); - if (res[0]!=0) res[0][0]=a00; - a00=arg[1]? arg[1][1] : 0; - a13=6.0368975005218423e-05; - a44=(a13*a02); - a47=3.3484658136227786e-02; - a48=(a47*a16); - a44=(a44-a48); - a48=-6.1658244361114702e-05; - a49=(a48*a23); - a44=(a44-a49); - a49=5.4404333259807184e-05; - a50=(a49*a29); - a44=(a44-a50); - a50=6.0570157695918683e-03; - a51=(a50*a34); - a44=(a44+a51); - a51=-3.0184487502609172e-03; - a52=(a51*a41); - a44=(a44+a52); - a00=(a00-a44); - if (res[0]!=0) res[0][1]=a00; - a00=arg[1]? arg[1][2] : 0; - a44=-1.1116270017027936e-07; - a52=(a44*a02); - a53=-6.1658244361114783e-05; - a54=(a53*a16); - a52=(a52-a54); - a54=3.3963490377380855e-02; - a55=(a54*a23); - a52=(a52-a55); - a55=-2.9967785627100754e-02; - a56=(a55*a29); - a52=(a52-a56); - a56=-3.0801331505514837e-03; - a57=(a56*a34); - a52=(a52+a57); - a57=5.5581350085139543e-06; - a58=(a57*a41); - a52=(a52+a58); - a00=(a00-a52); - if (res[0]!=0) res[0][2]=a00; - a00=arg[1]? arg[1][3] : 0; - a52=9.8084735444364535e-08; - a58=(a52*a02); - a59=5.4404333259807062e-05; - a60=(a59*a16); - a58=(a58-a60); - a60=-2.9967785627100469e-02; - a61=(a60*a23); - a58=(a58-a61); - a61=1.4970303990827358e+00; - a62=(a61*a29); - a58=(a58-a62); - a62=2.7177645446042520e-03; - a63=(a62*a34); - a58=(a58+a63); - a63=-4.9042367722181902e-06; - a64=(a63*a41); - a58=(a58+a64); - a00=(a00-a58); - if (res[0]!=0) res[0][3]=a00; - a00=arg[1]? arg[1][4] : 0; - a58=1.0920100546139210e-05; - a64=(a58*a02); - a65=6.0570157695918657e-03; - a66=(a65*a16); - a64=(a64-a66); - a66=-3.0801331505514781e-03; - a67=(a66*a23); - a64=(a64-a67); - a67=2.7177645446042498e-03; - a68=(a67*a29); - a64=(a64-a68); - a68=3.0257778596593993e-01; - a69=(a68*a34); - a64=(a64+a69); - a69=-5.4600502730695923e-04; - a70=(a69*a41); - a64=(a64+a70); - a00=(a00-a64); - if (res[0]!=0) res[0][4]=a00; - a00=arg[1]? arg[1][5] : 0; - a64=-6.0150572994295635e-03; - a02=(a64*a02); - a70=-3.0184487502609133e-03; - a16=(a70*a16); - a02=(a02-a16); - a16=5.5581350085139340e-06; - a23=(a16*a23); - a02=(a02-a23); - a23=-4.9042367722181935e-06; - a29=(a23*a29); - a02=(a02-a29); - a34=(a69*a34); - a02=(a02+a34); - a34=3.0075286497147785e-01; - a41=(a34*a41); - a02=(a02+a41); - a00=(a00-a02); - if (res[0]!=0) res[0][5]=a00; - a00=arg[1]? arg[1][6] : 0; - a02=arg[0]? arg[0][6] : 0; - a41=sin(a02); - a29=arg[0]? arg[0][7] : 0; - a71=tan(a29); - a72=(a41*a71); - a73=(a72*a09); - a73=(a17+a73); - a02=cos(a02); - a74=(a02*a71); - a75=(a74*a04); - a73=(a73+a75); - a00=(a00-a73); - if (res[0]!=0) res[0][6]=a00; - a00=arg[1]? arg[1][7] : 0; - a73=(a02*a09); - a75=(a41*a04); - a73=(a73-a75); - a00=(a00-a73); - if (res[0]!=0) res[0][7]=a00; - a00=arg[1]? arg[1][8] : 0; - a73=cos(a29); - a75=(a41/a73); - a76=(a75*a09); - a77=(a02/a73); - a78=(a77*a04); - a76=(a76+a78); - a00=(a00-a76); - if (res[0]!=0) res[0][8]=a00; - a00=casadi_sign(a12); - a00=(a12*a00); - a00=(a00+a07); - a07=(a01*a00); - a76=(a14*a15); - a07=(a07+a76); - a76=(a21*a22); - a07=(a07+a76); - if (res[1]!=0) res[1][0]=a07; - a07=(a13*a00); - a76=(a47*a15); - a07=(a07+a76); - a76=(a48*a22); - a07=(a07+a76); - if (res[1]!=0) res[1][1]=a07; - a07=(a44*a00); - a76=(a53*a15); - a07=(a07+a76); - a76=(a54*a22); - a07=(a07+a76); - if (res[1]!=0) res[1][2]=a07; - a07=(a52*a00); - a76=(a59*a15); - a07=(a07+a76); - a76=(a60*a22); - a07=(a07+a76); - if (res[1]!=0) res[1][3]=a07; - a07=(a58*a00); - a76=(a65*a15); - a07=(a07+a76); - a76=(a66*a22); - a07=(a07+a76); - if (res[1]!=0) res[1][4]=a07; - a00=(a64*a00); - a15=(a70*a15); - a00=(a00+a15); - a22=(a16*a22); - a00=(a00+a22); - if (res[1]!=0) res[1][5]=a00; - a00=(a01*a05); - a22=casadi_sign(a06); - a22=(a06*a22); - a22=(a22+a20); - a20=(a14*a22); - a00=(a00+a20); - a20=(a21*a24); - a00=(a00+a20); - if (res[1]!=0) res[1][6]=a00; - a00=(a13*a05); - a20=(a47*a22); - a00=(a00+a20); - a20=(a48*a24); - a00=(a00+a20); - if (res[1]!=0) res[1][7]=a00; - a00=(a44*a05); - a20=(a53*a22); - a00=(a00+a20); - a20=(a54*a24); - a00=(a00+a20); - if (res[1]!=0) res[1][8]=a00; - a00=(a52*a05); - a20=(a59*a22); - a00=(a00+a20); - a20=(a60*a24); - a00=(a00+a20); - if (res[1]!=0) res[1][9]=a00; - a00=(a58*a05); - a20=(a65*a22); - a00=(a00+a20); - a20=(a66*a24); - a00=(a00+a20); - if (res[1]!=0) res[1][10]=a00; - a05=(a64*a05); - a22=(a70*a22); - a05=(a05+a22); - a24=(a16*a24); - a05=(a05+a24); - if (res[1]!=0) res[1][11]=a05; - a05=(a01*a10); - a24=(a14*a18); - a05=(a05+a24); - a24=casadi_sign(a11); - a24=(a11*a24); - a24=(a24+a25); - a25=(a21*a24); - a05=(a05+a25); - if (res[1]!=0) res[1][12]=a05; - a05=(a13*a10); - a25=(a47*a18); - a05=(a05+a25); - a25=(a48*a24); - a05=(a05+a25); - if (res[1]!=0) res[1][13]=a05; - a05=(a44*a10); - a25=(a53*a18); - a05=(a05+a25); - a25=(a54*a24); - a05=(a05+a25); - if (res[1]!=0) res[1][14]=a05; - a05=(a52*a10); - a25=(a59*a18); - a05=(a05+a25); - a25=(a60*a24); - a05=(a05+a25); - if (res[1]!=0) res[1][15]=a05; - a05=(a58*a10); - a25=(a65*a18); - a05=(a05+a25); - a25=(a66*a24); - a05=(a05+a25); - if (res[1]!=0) res[1][16]=a05; - a10=(a64*a10); - a18=(a70*a18); - a10=(a10+a18); - a24=(a16*a24); - a10=(a10+a24); - if (res[1]!=0) res[1][17]=a10; - a10=(a03*a11); - a24=(a14*a10); - a18=(a08*a06); - a05=(a21*a18); - a24=(a24+a05); - a05=casadi_sign(a17); - a05=(a17*a05); - a05=(a05+a32); - a32=(a26*a05); - a24=(a24+a32); - a38=(a38*a04); - a36=(a36+a38); - a38=(a33*a36); - a24=(a24+a38); - a45=(a45*a09); - a43=(a43+a45); - a45=(a40*a43); - a24=(a24+a45); - if (res[1]!=0) res[1][18]=a24; - a24=(a47*a10); - a45=(a48*a18); - a24=(a24+a45); - a45=(a49*a05); - a24=(a24+a45); - a45=(a50*a36); - a24=(a24+a45); - a45=(a51*a43); - a24=(a24+a45); - if (res[1]!=0) res[1][19]=a24; - a24=(a53*a10); - a45=(a54*a18); - a24=(a24+a45); - a45=(a55*a05); - a24=(a24+a45); - a45=(a56*a36); - a24=(a24+a45); - a45=(a57*a43); - a24=(a24+a45); - if (res[1]!=0) res[1][20]=a24; - a24=(a59*a10); - a45=(a60*a18); - a24=(a24+a45); - a45=(a61*a05); - a24=(a24+a45); - a45=(a62*a36); - a24=(a24+a45); - a45=(a63*a43); - a24=(a24+a45); - if (res[1]!=0) res[1][21]=a24; - a24=(a65*a10); - a45=(a66*a18); - a24=(a24+a45); - a45=(a67*a05); - a24=(a24+a45); - a45=(a68*a36); - a24=(a24+a45); - a45=(a69*a43); - a24=(a24+a45); - if (res[1]!=0) res[1][22]=a24; - a10=(a70*a10); - a18=(a16*a18); - a10=(a10+a18); - a05=(a23*a05); - a10=(a10+a05); - a36=(a69*a36); - a10=(a10+a36); - a43=(a34*a43); - a10=(a10+a43); - if (res[1]!=0) res[1][23]=a10; - a10=-1.; - if (res[1]!=0) res[1][24]=a10; - a11=(a08*a11); - a10=(a01*a11); - a43=(a03*a12); - a21=(a21*a43); - a10=(a10+a21); - a30=(a30*a04); - a28=(a28+a30); - a30=(a26*a28); - a10=(a10+a30); - a30=casadi_sign(a09); - a30=(a09*a30); - a30=(a30+a37); - a37=(a33*a30); - a10=(a10+a37); - a42=(a42*a17); - a42=(a42+a46); - a46=(a40*a42); - a10=(a10+a46); - if (res[1]!=0) res[1][25]=a10; - a10=(a13*a11); - a48=(a48*a43); - a10=(a10+a48); - a48=(a49*a28); - a10=(a10+a48); - a48=(a50*a30); - a10=(a10+a48); - a48=(a51*a42); - a10=(a10+a48); - if (res[1]!=0) res[1][26]=a10; - a10=(a44*a11); - a54=(a54*a43); - a10=(a10+a54); - a54=(a55*a28); - a10=(a10+a54); - a54=(a56*a30); - a10=(a10+a54); - a54=(a57*a42); - a10=(a10+a54); - if (res[1]!=0) res[1][27]=a10; - a10=(a52*a11); - a60=(a60*a43); - a10=(a10+a60); - a60=(a61*a28); - a10=(a10+a60); - a60=(a62*a30); - a10=(a10+a60); - a60=(a63*a42); - a10=(a10+a60); - if (res[1]!=0) res[1][28]=a10; - a10=(a58*a11); - a66=(a66*a43); - a10=(a10+a66); - a66=(a67*a28); - a10=(a10+a66); - a66=(a68*a30); - a10=(a10+a66); - a66=(a69*a42); - a10=(a10+a66); - if (res[1]!=0) res[1][29]=a10; - a11=(a64*a11); - a16=(a16*a43); - a11=(a11+a16); - a28=(a23*a28); - a11=(a11+a28); - a30=(a69*a30); - a11=(a11+a30); - a42=(a34*a42); - a11=(a11+a42); - if (res[1]!=0) res[1][30]=a11; - a72=(-a72); - if (res[1]!=0) res[1][31]=a72; - a72=(-a02); - if (res[1]!=0) res[1][32]=a72; - a72=(-a75); - if (res[1]!=0) res[1][33]=a72; - a03=(a03*a06); - a01=(a01*a03); - a08=(a08*a12); - a14=(a14*a08); - a01=(a01+a14); - a27=(a27*a09); - a27=(a27+a31); - a26=(a26*a27); - a01=(a01+a26); - a35=(a35*a17); - a35=(a35+a39); - a33=(a33*a35); - a01=(a01+a33); - a33=casadi_sign(a04); - a33=(a04*a33); - a33=(a33+a19); - a40=(a40*a33); - a01=(a01+a40); - if (res[1]!=0) res[1][34]=a01; - a13=(a13*a03); - a47=(a47*a08); - a13=(a13+a47); - a49=(a49*a27); - a13=(a13+a49); - a50=(a50*a35); - a13=(a13+a50); - a51=(a51*a33); - a13=(a13+a51); - if (res[1]!=0) res[1][35]=a13; - a44=(a44*a03); - a53=(a53*a08); - a44=(a44+a53); - a55=(a55*a27); - a44=(a44+a55); - a56=(a56*a35); - a44=(a44+a56); - a57=(a57*a33); - a44=(a44+a57); - if (res[1]!=0) res[1][36]=a44; - a52=(a52*a03); - a59=(a59*a08); - a52=(a52+a59); - a61=(a61*a27); - a52=(a52+a61); - a62=(a62*a35); - a52=(a52+a62); - a63=(a63*a33); - a52=(a52+a63); - if (res[1]!=0) res[1][37]=a52; - a58=(a58*a03); - a65=(a65*a08); - a58=(a58+a65); - a67=(a67*a27); - a58=(a58+a67); - a68=(a68*a35); - a58=(a58+a68); - a68=(a69*a33); - a58=(a58+a68); - if (res[1]!=0) res[1][38]=a58; - a64=(a64*a03); - a70=(a70*a08); - a64=(a64+a70); - a23=(a23*a27); - a64=(a64+a23); - a69=(a69*a35); - a64=(a64+a69); - a34=(a34*a33); - a64=(a64+a34); - if (res[1]!=0) res[1][39]=a64; - a74=(-a74); - if (res[1]!=0) res[1][40]=a74; - if (res[1]!=0) res[1][41]=a41; - a74=(-a77); - if (res[1]!=0) res[1][42]=a74; - a74=(a71*a02); - a74=(a09*a74); - a71=(a71*a41); - a71=(a04*a71); - a74=(a74-a71); - a74=(-a74); - if (res[1]!=0) res[1][43]=a74; - a74=(a09*a41); - a71=(a04*a02); - a74=(a74+a71); - if (res[1]!=0) res[1][44]=a74; - a74=(a09*a77); - a71=(a04*a75); - a74=(a74-a71); - a74=(-a74); - if (res[1]!=0) res[1][45]=a74; - a74=casadi_sq(a73); - a41=(a41/a74); - a41=(a09*a41); - a02=(a02/a74); - a02=(a04*a02); - a41=(a41+a02); - a41=(-a41); - if (res[1]!=0) res[1][46]=a41; - a75=(a75/a73); - a29=sin(a29); - a75=(a75*a29); - a09=(a09*a75); - a77=(a77/a73); - a77=(a77*a29); - a04=(a04*a77); - a09=(a09+a04); - a09=(-a09); - if (res[1]!=0) res[1][47]=a09; - a09=1.; - if (res[2]!=0) res[2][0]=a09; - if (res[2]!=0) res[2][1]=a09; - if (res[2]!=0) res[2][2]=a09; - if (res[2]!=0) res[2][3]=a09; - if (res[2]!=0) res[2][4]=a09; - if (res[2]!=0) res[2][5]=a09; - if (res[2]!=0) res[2][6]=a09; - if (res[2]!=0) res[2][7]=a09; - if (res[2]!=0) res[2][8]=a09; - a09=-3.3453634479321918e-02; - if (res[3]!=0) res[3][0]=a09; - a09=-6.0368975005218423e-05; - if (res[3]!=0) res[3][1]=a09; - a09=1.1116270017027936e-07; - if (res[3]!=0) res[3][2]=a09; - a09=-9.8084735444364535e-08; - if (res[3]!=0) res[3][3]=a09; - a09=-1.0920100546139210e-05; - if (res[3]!=0) res[3][4]=a09; - a09=6.0150572994295635e-03; - if (res[3]!=0) res[3][5]=a09; - a09=-1.0920100546139184e-05; - if (res[3]!=0) res[3][6]=a09; - a09=-6.0570157695918683e-03; - if (res[3]!=0) res[3][7]=a09; - a09=3.0801331505514837e-03; - if (res[3]!=0) res[3][8]=a09; - a09=-2.7177645446042520e-03; - if (res[3]!=0) res[3][9]=a09; - a09=-3.0257778596593993e-01; - if (res[3]!=0) res[3][10]=a09; - a09=5.4600502730695923e-04; - if (res[3]!=0) res[3][11]=a09; - a04=6.0150572994295574e-03; - if (res[3]!=0) res[3][12]=a04; - a04=3.0184487502609172e-03; - if (res[3]!=0) res[3][13]=a04; - a04=-5.5581350085139543e-06; - if (res[3]!=0) res[3][14]=a04; - a04=4.9042367722181902e-06; - if (res[3]!=0) res[3][15]=a04; - if (res[3]!=0) res[3][16]=a09; - a09=-3.0075286497147785e-01; - if (res[3]!=0) res[3][17]=a09; - return 0; -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_u(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ - return casadi_f0(arg, res, iw, w, mem); -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_u_alloc_mem(void) { - return 0; -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_u_init_mem(int mem) { - return 0; -} - -CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_jac_x_xdot_u_free_mem(int mem) { -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_u_checkout(void) { - return 0; -} - -CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_jac_x_xdot_u_release(int mem) { -} - -CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_jac_x_xdot_u_incref(void) { -} - -CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_jac_x_xdot_u_decref(void) { -} - -CASADI_SYMBOL_EXPORT casadi_int auv_model_impl_dae_fun_jac_x_xdot_u_n_in(void) { return 6;} - -CASADI_SYMBOL_EXPORT casadi_int auv_model_impl_dae_fun_jac_x_xdot_u_n_out(void) { return 4;} - -CASADI_SYMBOL_EXPORT casadi_real auv_model_impl_dae_fun_jac_x_xdot_u_default_in(casadi_int i) { - switch (i) { - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const char* auv_model_impl_dae_fun_jac_x_xdot_u_name_in(casadi_int i) { - switch (i) { - case 0: return "i0"; - case 1: return "i1"; - case 2: return "i2"; - case 3: return "i3"; - case 4: return "i4"; - case 5: return "i5"; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const char* auv_model_impl_dae_fun_jac_x_xdot_u_name_out(casadi_int i) { - switch (i) { - case 0: return "o0"; - case 1: return "o1"; - case 2: return "o2"; - case 3: return "o3"; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const casadi_int* auv_model_impl_dae_fun_jac_x_xdot_u_sparsity_in(casadi_int i) { - switch (i) { - case 0: return casadi_s0; - case 1: return casadi_s0; - case 2: return casadi_s1; - case 3: return casadi_s2; - case 4: return casadi_s3; - case 5: return casadi_s2; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const casadi_int* auv_model_impl_dae_fun_jac_x_xdot_u_sparsity_out(casadi_int i) { - switch (i) { - case 0: return casadi_s0; - case 1: return casadi_s4; - case 2: return casadi_s5; - case 3: return casadi_s6; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_u_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { - if (sz_arg) *sz_arg = 6; - if (sz_res) *sz_res = 4; - if (sz_iw) *sz_iw = 0; - if (sz_w) *sz_w = 0; - return 0; -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_u_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { - if (sz_arg) *sz_arg = 6*sizeof(const casadi_real*); - if (sz_res) *sz_res = 4*sizeof(casadi_real*); - if (sz_iw) *sz_iw = 0*sizeof(casadi_int); - if (sz_w) *sz_w = 0*sizeof(casadi_real); - return 0; -} - - -#ifdef __cplusplus -} /* extern "C" */ -#endif diff --git a/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u.o b/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u.o deleted file mode 100644 index 4f9555c24d94168607aa270aeeee8e89f9bc61ee..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 21328 zcmcJX3w+eomB;_c!${fuCl)PjT^)76U?ol-KxmarAdzW}HfoTW8caeS5``p^L1U#B zOK4TJ#_FeKw{D9IwY1AtyLG#)bxGtIK`{cpz#>|TP+tLjRnhG4oO^EOLFk`DUr-^<0{aM>rbsPWH0AqyFh1Hux38 z32pGtkoUKrXz;Up8vOISZ#?~ozkchGBYuEIAo_TNKXYq?f92y4k9al#FN;YB={w@b zyYzeUtl}$6bKfhTSzIwQdi%#$L~lPDz4eVYm>FF*BO|)BD{&Ik1RK8U-*Oq6lper)Rzzw$8T zpKkDHJ&9{ygFpI^KYmSUAM61?M4G8q?;_!Cyd1+tLLcCIg#^PW!yo@xs23Uh@vA~X zyps$JDG6mmYCq8~q#nctb7R~JbKg)FuFNT_McAkXz7Cd~`=kP1*cLQflOpqvf<+fP`> zu$CUtG91!T%2L8zqAV2QJY~zyK2qWbCPKt)+3<^YJbVn{eJrwKFe-c1plLd z{So=^V?V=(ce1ZQ5}_b!_&n*Uc2|X*hb303pdmvv`|)=r)!P{nT*;^G}3k zr%w=_7mC^p6(Nj@007MF!L}HyC7X#RAoB`vXG8+q!rz8|R z;$M9b*FG+!8uz+T6dKoXW<);5^(^q3P&ur|RD#J8>lRf3DX;Nlb_dFVT3!@+MhX~X zU>&l?QI{CaPJSHoTx>nO){U$i$#Z42#9_LDRq?0EojeL=lK04&@J(em1WLHJn@kRJ zSXl_BQjS(iZ3;vopUw4-E}Kp(zu&%rEsXPWm(AS|7CdH`&{L4=L6=$Ts9LBe zt~l1)(Or1<;Nt$%fb|&Z_wi~8$Tq;qp4|!ffOFUc&_5r1@*$_EB>OXA8i?^ zj;GL*#}I6$X?f%=<(Hb!lAjcc?Vzy@huvA%4_J|C5sr%~`8E23kYghre0p^1p-56~{$q)@m6)PAdd>7KkpBaYnnS z_~Gd79bH_i>k+$Fx!%Tx4c3&{oWk4-3Jz-4N>6 zz|}^9?V%p1r2Q6kpal1AO#9fxy+PTZq8k~vC^Q!;4&pM?L5V+lJ#vl4gcFxZW%Qc> z@h%KttSE8zGjdy(RVG4h7+M>k!OFjfyfhIh7XQ8TzE|E~8c-Oyc`$%F;p=I-7)BiZH6h-3UHpx zleE6fY7Z?2MA`=?4eK2$Fbg}8ti^JIG{=jIPfZ^@W&9jd2DzS3jAr^l0|)6ma6t%~ zvgwR!PBvoktUpuYJRB?V;NDF6w-{Lk;{v9>InI$}vb`0hkQMjaypWg7t_Y)R!a$p19uKSt6|lZ%egD^_kALD|nkyayTEc^il+h!xY(3*ws~ z35gPa)+;cx2jW{nMn8#)R-1}q*C{j6b=aL@x4g>qdb=$}*NtFHiW08w=DjScPHP=L zssvDK1#JeeuAr?e2v%B}?pPt$r5cHKlt@z?O2<6O{hXHNAT+&%i;Oo5GhE5K`7n#K zmgzjGAD#Be$CzzqVS!)T0^6}e3DB2=anWVc?xrI}Z;z4(yZkJS;YfgXYPiA>A4B3z zu!8#((v#6hK*fV99#(OvwOJ$ODqgMPEy*}=VWbaDbkG{`p3Wh9?_Vg3+VnV< z)juu#r83cL`pY!44%gF87rK7O@o-6BUNDw0m*EtIspUnqMRqsd9tXzg!L18Z5ij3( z$gvV1D(UMJjg+OU5BFiUG#{(*u@f!ND^_$1Rt%a@qg%|$%fw4U5sKKOAVx|Z%_X?U z4vjg|*Vo{m(M|&Ro@%~1qMRezg$-C+Q*ZbNLs2LRrKajq)!v?-n+u3nW z=}Y>C-`E_zi`U`X`=YhO9*qv^ncBCHzNbg;;!Y27)BeZn9X{!|PjoKnJ0bCvZ;z9= zrG1HShM>bF$Y@oP(r?%4X zH8nErmLK$t4-{P_&t+dYclC(xjw-s|q_3NFbKjwgif>N+-MxEO{G#@OqOslE zN9}v)Cq-j_TC(-Rr`w7?EQ-w>`{>-F|9R!5@IwauoCl|_Z0jr=w{iciUui5_ z{BnD{SN{A|(cfwHJNY}so^@hRr&FHQ*J<_H`dE3J-_~!K*!?e3pI?c6*<$zEV&~8Z z+utemooefQqp4@-ju&@~eeUS{MR8M~&O_mCk9JINC|WM{`rPuTuYC2MUF)R2ol^gP ze^`6>OTI6Pe16vU$<4EO7L|9Lx3_SAai@K@ZrXnMwxXT3y*i~nu6^so-|K97QonUl zzfQ5YNbK#D@@zRpQvV{Uzpc+Ysn1lgr%3GSG##v0o>RNb`_zbePL}74$lBAFzJA%- zk&(OCznfio_sFgd^jQDLB6)TSw{l(HjOo+A9L%0MXJJ!fVK6s8CpRZNp>QF;=H8N* zmlG}+FPMv>`7MBp=C~P`&ZK#-iX{*8`2gS^2JdnW^`a+QJ&KzjZYSc z6SH&86iFjiB5^y-JBJr%hQB+aB(reI ziC@cnc<9jgP9B+AI6X65oS8i>GYHvfnSp7U8PU%mJC(6o^wFG3f!JdlGxMxvJ;Lmq zPICJPk)zKV6tvtZnQSx*4#EnAjWjCQ7TVKM(5zCJ^$XcThc*f~n+NA@7e;=gKyu54 zSw9k6Gl*Q5FuR~qd+u@=P|84}!@R?Ng9!}f8TQjDLp{41B-748bO?BmHxg#OrPND> z^MZ>wt-}-+;v)H8vDNbXg$Eox8-%aGlR|}gcz?o^^jK4Sg=ax8@rb-v%d?Qn%kT>2 z-99c99=1T9(}V{dJ|hJ$6JGAfU!H=~*@RZWh$DY>3SKAN*3)LYHU&5R4f;9VN_*Z7 zR=>j+2`_i}cT(`%gjYNA-%r8s6K?ypwdkLP^O?~?JXZ*}!CyUz`->*t!&aqwpR_erpQeBfP_re=Y@oN%&%ZCNsU7f*-Vc9Qn6W@OOk8 zzb5s6n1Y*eo8jG-6y?|m9-kRro5M#5Z*lm^!ZRE@#|ZZve!6hezmm#A!k0Vp=L%o# zaQf+r#@EA16&#zCf?ptfg(E*L1&<03IOSfNf>#JP{UTY&H&XCw;rkr@4Jr6M;k}Oh zf)u<>_}Zi?9J@vML5H^s-{MP3myIe^9--r;h%_n#NoquBbng^9lwnf9(H(u`AJ^X(Q~TEcRBK>iF~;u zf0oFb`6-#0E!^%OY)X#sAlJiE&npma=7}W#vhau_9}!;e@QZ|3JG@MIi^DG$zS!Y0 z;ckDJ&3t(OzTH|{E8M=@XQOc60(rIwcXx0%2zPt#BH?y_X4Neb?)J|+gxhZ|R{kF0 z{PE91JeLdqqGA;NDfr21QLtx_ZsFEH_8f9pxa}vl9%FFWAwAWipU)$RUjS}(ND@D$ zr{F~?_>2_%VsI+Aht59;_S|xl@O=({PJ#=s({Q==t{ygD9htCoo zcKCOMM}*t+%fAUPclfiytA$%X4?~9{ds>9s^G#TIuW)Wms z&gv0v&pWRP57UP+1k1geg*Q~qZ?3DKSJgPbWnNWXO?_3v!ltTgYig_7s@m$B6IBay zYSWRbY7M6+mY1GbetKfL>4{BDPplw4u}NM|+4MZRO{N>aPNY~a-zVmB+~6?Bjhx|y zoQLnZQ#j7|JeHiy_dJ%&<0A6%NhYs=@2o%1l+QXR8qV4#8a;fU$ddU+C+o>)os(Hx zzA>Kj<{QHCi6+i@Cz)JaRz8=N&!yxW{Y(m2|3sE4VEvO=LxIt6WSC6myaim!WY$o? zr4(@9Nvx;9)W}GhJ4+TCZLGhLwG|pYtf7$atiVW+qc~r#flPB*M=s~iHFae&iQ|)u z6aY01JR3+fxv60NIGTePVvP5(Ak zrCX`G`i7c?^Ac$*u9;u|SIMSpM{7&Xg4V{wP3bD{mru8V8=DtgmsWd_VAY(P67{WE zAMN>rww?5C?wI^=Lcb?bAWq+0EiWO6K>X{tEZ;;BfjI5CEPs_C0`bdmS#Hk(#PPS( z{rnfQ-^Vt`TcG%_g_Hc{ihrH}f-8TfaC%&U%a%Jah5Us|p1vho`BKHTo+}iespQT1 z`P23+R`OcUD#fo-2_jH(7A{-g>lLT7g5|d={#nKE zQJnVjR{jCSX{%@XuN9{)n&n#+r{5VYe@SsV6IlL+;`9fAmLF66bj3$upGtx1dj>8m zf12V!#Y+@FTXEVqQy@KO;<9?KQGBf8H!4ow#I5}I6d$knj})gdXyt#cIQ`CR`9{Sj zD85Vau;Tj_&r|#z#q$-XeLMxKZvie_Zb0!FijPx#vf@(|pQw1T;?oqrMDb$9w)e z|GIFuzSk;wUEg-ab$$O`ac$3kE3WJNgyOosFDf2VcD|yx_QRWsPgn9I@9z}PRpo9`{L6}urcDEa zTi=Po$v=7>xlnPv9$l>XmB?f3yHN3%;@?wTuSY*p{BuhF=Zfp~=)V-#>&rI9^*Xgj zalP)muDD)zK2kiW?D>p*SR?<8Rs0Oa^?G!^;^!#&BE`om{&mH_qT(3Jz z6xZv>y^8B~WToPI9r>N&dOqKv_+(}0HpTTkyhCxlFMU&SJ^%k%@r#uH;qqaT{IBQ# zGZmkqUTP4QyIOBBCNxH~S^D|x-2Jf?VD>B*!`GXmN94P16yoUQm(il3)=rQ%Z+ zKUML|6{k;ZR{tEuzo7Wdihot{A1JQpjh`rk~H_yvmpR`GKcU$6L7#kVVdp5nU| zk0}0@;$@1TLYrU&@-sDs^}`jy-FCcF$y3zIuTWgiZ@*Do&nJ&5K0)c}R$QM`{-F5R zl>9r2U!wRBT5J%gUV2|WTJhOPv-W2zuJ>;<6pt$TS&Cn*c&*|&iZ?5+=k1#n*Ym&+ z6xZ|ZLyGHuyIyhKZ}%v!=g-#_*Yn#)itB!ee>lN`{HgofIK}n+lcTuqx04i)K$rD@ zRB=6DolKhp1nP$gTvonT@tYK1Bi!xJI~Av;$?DmYf)Ay|8-es(jmyesDqf{H{cjQ! zNWL1El^>^gjpBKV&ry7;;+$;&#r65_mx@nQ<^EQ2y-(Pzc!`qVp|~D*uPUy`5&e%k6v)qd-9LdQBLw1l zzj2x3dc0IBPFoD?|5b|9Qe*kUieIDn9>w*#^N!->N`53Q-UwvpWw@+<`}-zwt-n>t zYyFF8bBsXpLvdL>_bNVD@s)}tTCKhpX^!Sefr%V&Uo-YuN^HU%sHl5klb`?>v{*ZR-O zZxnekvtPbi0PTe+Sou2PL8eK*TzJUgi-gnOl7iLKA)MA|3YNF0&~K#7w=XSk>q&ba z&1plLu=BK^+tpzx5x!@>(4 z9uaQ-o;B&Wa^cmEe6{d|!&`*gzvHv^^a{V7)M2Glu1%9>x39d1CY}G^{JF?WI%%(?gDs!V3_95I?Xwh#6sP{9!#=V9 zJSQQjg%FS68q9y$7aPH#6h!``Hgf%U2^fjU_8T!10x~gnIOW^+v(Kv_K3M)0Qt_Bo zC{J6yea@BqVMp!*$#3URn=IAgGKtYzYMGUxXNW$64H8`0TOo5IE~lDaw&V{wI;`Cm z{}Upl)6E~1{0E%^-Te0 - -#ifndef casadi_real -#define casadi_real double -#endif - -#ifndef casadi_int -#define casadi_int int -#endif - -/* Add prefix to internal symbols */ -#define casadi_f0 CASADI_PREFIX(f0) -#define casadi_fabs CASADI_PREFIX(fabs) -#define casadi_s0 CASADI_PREFIX(s0) -#define casadi_s1 CASADI_PREFIX(s1) -#define casadi_s2 CASADI_PREFIX(s2) -#define casadi_s3 CASADI_PREFIX(s3) -#define casadi_s4 CASADI_PREFIX(s4) -#define casadi_s5 CASADI_PREFIX(s5) -#define casadi_s6 CASADI_PREFIX(s6) -#define casadi_s7 CASADI_PREFIX(s7) -#define casadi_sign CASADI_PREFIX(sign) -#define casadi_sq CASADI_PREFIX(sq) - -/* Symbol visibility in DLLs */ -#ifndef CASADI_SYMBOL_EXPORT - #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) - #if defined(STATIC_LINKED) - #define CASADI_SYMBOL_EXPORT - #else - #define CASADI_SYMBOL_EXPORT __declspec(dllexport) - #endif - #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) - #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default"))) - #else - #define CASADI_SYMBOL_EXPORT - #endif -#endif - -casadi_real casadi_fabs(casadi_real x) { -/* Pre-c99 compatibility */ -#if __STDC_VERSION__ < 199901L - return x>0 ? x : -x; -#else - return fabs(x); -#endif -} - -casadi_real casadi_sign(casadi_real x) { return x<0 ? -1 : x>0 ? 1 : x;} - -casadi_real casadi_sq(casadi_real x) { return x*x;} - -static const casadi_int casadi_s0[3] = {9, 1, 1}; -static const casadi_int casadi_s1[3] = {3, 1, 1}; -static const casadi_int casadi_s2[3] = {0, 1, 1}; -static const casadi_int casadi_s3[3] = {0, 0, 1}; -static const casadi_int casadi_s4[60] = - {9, 9, 0, 6, 12, 18, 25, 34, - 43, 46, 48, 48, 0, 1, 2, 3, - 4, 5, 0, 1, 2, 3, 4, 5, - 0, 1, 2, 3, 4, 5, 0, 1, - 2, 3, 4, 5, 6, 0, 1, 2, - 3, 4, 5, 6, 7, 8, 0, 1, - 2, 3, 4, 5, 6, 7, 8, 6, - 7, 8, 6, 8}; -static const casadi_int casadi_s5[21] = - {9, 9, 0, 1, 2, 3, 4, 5, - 6, 7, 8, 9, 0, 1, 2, 3, - 4, 5, 6, 7, 8}; -static const casadi_int casadi_s6[24] = - {9, 3, 0, 6, 12, 18, 0, 1, - 2, 3, 4, 5, 0, 1, 2, 3, - 4, 5, 0, 1, 2, 3, 4, 5}; -static const casadi_int casadi_s7[3] = {9, 0, 1}; - -/* auv_model_impl_dae_fun_jac_x_xdot_u_z:(i0[9],i1[9],i2[3],i3[0],i4[],i5[0])->(o0[9],o1[9x9,48nz],o2[9x9,9nz],o3[9x3,18nz],o4[9x0]) */ -static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { - casadi_real a00, a01, a02, a03, a04, a05, a06, a07, a08, a09, a10, a11; - casadi_real a12, a13, a14, a15, a16, a17, a18, a19, a20, a21, a22, a23; - casadi_real a24, a25, a26, a27, a28, a29, a30, a31, a32, a33, a34, a35; - casadi_real a36, a37, a38, a39, a40, a41, a42, a43, a44, a45, a46, a47; - casadi_real a48, a49, a50, a51, a52, a53, a54, a55, a56, a57, a58, a59; - casadi_real a60, a61, a62, a63, a64, a65, a66, a67, a68, a69, a70, a71; - casadi_real a72, a73, a74, a75, a76, a77, a78; - a00=arg[1]? arg[1][0] : 0; - a01=3.3453634479321918e-02; - a02=arg[2]? arg[2][0] : 0; - a03=-30.; - a04=arg[0]? arg[0][5] : 0; - a05=(a03*a04); - a06=arg[0]? arg[0][1] : 0; - a07=(a05*a06); - a08=30.; - a09=arg[0]? arg[0][4] : 0; - a10=(a08*a09); - a11=arg[0]? arg[0][2] : 0; - a12=(a10*a11); - a07=(a07+a12); - a02=(a02-a07); - a07=23.; - a12=arg[0]? arg[0][0] : 0; - a13=casadi_fabs(a12); - a07=(a07+a13); - a13=(a07*a12); - a02=(a02-a13); - a13=(a01*a02); - a14=6.0368975005218254e-05; - a15=(a08*a04); - a16=(a15*a12); - a17=arg[0]? arg[0][3] : 0; - a18=(a03*a17); - a19=(a18*a11); - a16=(a16+a19); - a19=46.; - a20=casadi_fabs(a06); - a20=(a19+a20); - a21=(a20*a06); - a16=(a16+a21); - a21=(a14*a16); - a13=(a13-a21); - a21=-1.1116270017027866e-07; - a22=(a03*a09); - a23=(a22*a12); - a24=(a08*a17); - a25=(a24*a06); - a23=(a23+a25); - a25=casadi_fabs(a11); - a25=(a19+a25); - a26=(a25*a11); - a23=(a23+a26); - a26=(a21*a23); - a13=(a13-a26); - a26=9.8084735444363873e-08; - a27=3.3399999999999999e+00; - a28=(a27*a04); - a29=(a28*a09); - a30=-3.3199999999999998e+00; - a31=(a30*a09); - a32=(a31*a04); - a29=(a29+a32); - a32=casadi_fabs(a17); - a32=(a19+a32); - a33=(a32*a17); - a29=(a29+a33); - a33=(a26*a29); - a13=(a13-a33); - a33=1.0920100546139184e-05; - a34=arg[2]? arg[2][1] : 0; - a35=-3.3399999999999999e+00; - a36=(a35*a04); - a37=(a36*a17); - a38=6.8000000000000005e-01; - a39=(a38*a17); - a40=(a39*a04); - a37=(a37+a40); - a34=(a34-a37); - a37=casadi_fabs(a09); - a37=(a19+a37); - a40=(a37*a09); - a34=(a34-a40); - a40=(a33*a34); - a13=(a13+a40); - a40=-6.0150572994295574e-03; - a41=arg[2]? arg[2][2] : 0; - a42=3.3199999999999998e+00; - a43=(a42*a09); - a44=(a43*a17); - a45=-6.8000000000000005e-01; - a46=(a45*a17); - a47=(a46*a09); - a44=(a44+a47); - a41=(a41-a44); - a44=casadi_fabs(a04); - a19=(a19+a44); - a44=(a19*a04); - a41=(a41-a44); - a44=(a40*a41); - a13=(a13+a44); - a00=(a00-a13); - if (res[0]!=0) res[0][0]=a00; - a00=arg[1]? arg[1][1] : 0; - a13=6.0368975005218423e-05; - a44=(a13*a02); - a47=3.3484658136227786e-02; - a48=(a47*a16); - a44=(a44-a48); - a48=-6.1658244361114702e-05; - a49=(a48*a23); - a44=(a44-a49); - a49=5.4404333259807184e-05; - a50=(a49*a29); - a44=(a44-a50); - a50=6.0570157695918683e-03; - a51=(a50*a34); - a44=(a44+a51); - a51=-3.0184487502609172e-03; - a52=(a51*a41); - a44=(a44+a52); - a00=(a00-a44); - if (res[0]!=0) res[0][1]=a00; - a00=arg[1]? arg[1][2] : 0; - a44=-1.1116270017027936e-07; - a52=(a44*a02); - a53=-6.1658244361114783e-05; - a54=(a53*a16); - a52=(a52-a54); - a54=3.3963490377380855e-02; - a55=(a54*a23); - a52=(a52-a55); - a55=-2.9967785627100754e-02; - a56=(a55*a29); - a52=(a52-a56); - a56=-3.0801331505514837e-03; - a57=(a56*a34); - a52=(a52+a57); - a57=5.5581350085139543e-06; - a58=(a57*a41); - a52=(a52+a58); - a00=(a00-a52); - if (res[0]!=0) res[0][2]=a00; - a00=arg[1]? arg[1][3] : 0; - a52=9.8084735444364535e-08; - a58=(a52*a02); - a59=5.4404333259807062e-05; - a60=(a59*a16); - a58=(a58-a60); - a60=-2.9967785627100469e-02; - a61=(a60*a23); - a58=(a58-a61); - a61=1.4970303990827358e+00; - a62=(a61*a29); - a58=(a58-a62); - a62=2.7177645446042520e-03; - a63=(a62*a34); - a58=(a58+a63); - a63=-4.9042367722181902e-06; - a64=(a63*a41); - a58=(a58+a64); - a00=(a00-a58); - if (res[0]!=0) res[0][3]=a00; - a00=arg[1]? arg[1][4] : 0; - a58=1.0920100546139210e-05; - a64=(a58*a02); - a65=6.0570157695918657e-03; - a66=(a65*a16); - a64=(a64-a66); - a66=-3.0801331505514781e-03; - a67=(a66*a23); - a64=(a64-a67); - a67=2.7177645446042498e-03; - a68=(a67*a29); - a64=(a64-a68); - a68=3.0257778596593993e-01; - a69=(a68*a34); - a64=(a64+a69); - a69=-5.4600502730695923e-04; - a70=(a69*a41); - a64=(a64+a70); - a00=(a00-a64); - if (res[0]!=0) res[0][4]=a00; - a00=arg[1]? arg[1][5] : 0; - a64=-6.0150572994295635e-03; - a02=(a64*a02); - a70=-3.0184487502609133e-03; - a16=(a70*a16); - a02=(a02-a16); - a16=5.5581350085139340e-06; - a23=(a16*a23); - a02=(a02-a23); - a23=-4.9042367722181935e-06; - a29=(a23*a29); - a02=(a02-a29); - a34=(a69*a34); - a02=(a02+a34); - a34=3.0075286497147785e-01; - a41=(a34*a41); - a02=(a02+a41); - a00=(a00-a02); - if (res[0]!=0) res[0][5]=a00; - a00=arg[1]? arg[1][6] : 0; - a02=arg[0]? arg[0][6] : 0; - a41=sin(a02); - a29=arg[0]? arg[0][7] : 0; - a71=tan(a29); - a72=(a41*a71); - a73=(a72*a09); - a73=(a17+a73); - a02=cos(a02); - a74=(a02*a71); - a75=(a74*a04); - a73=(a73+a75); - a00=(a00-a73); - if (res[0]!=0) res[0][6]=a00; - a00=arg[1]? arg[1][7] : 0; - a73=(a02*a09); - a75=(a41*a04); - a73=(a73-a75); - a00=(a00-a73); - if (res[0]!=0) res[0][7]=a00; - a00=arg[1]? arg[1][8] : 0; - a73=cos(a29); - a75=(a41/a73); - a76=(a75*a09); - a77=(a02/a73); - a78=(a77*a04); - a76=(a76+a78); - a00=(a00-a76); - if (res[0]!=0) res[0][8]=a00; - a00=casadi_sign(a12); - a00=(a12*a00); - a00=(a00+a07); - a07=(a01*a00); - a76=(a14*a15); - a07=(a07+a76); - a76=(a21*a22); - a07=(a07+a76); - if (res[1]!=0) res[1][0]=a07; - a07=(a13*a00); - a76=(a47*a15); - a07=(a07+a76); - a76=(a48*a22); - a07=(a07+a76); - if (res[1]!=0) res[1][1]=a07; - a07=(a44*a00); - a76=(a53*a15); - a07=(a07+a76); - a76=(a54*a22); - a07=(a07+a76); - if (res[1]!=0) res[1][2]=a07; - a07=(a52*a00); - a76=(a59*a15); - a07=(a07+a76); - a76=(a60*a22); - a07=(a07+a76); - if (res[1]!=0) res[1][3]=a07; - a07=(a58*a00); - a76=(a65*a15); - a07=(a07+a76); - a76=(a66*a22); - a07=(a07+a76); - if (res[1]!=0) res[1][4]=a07; - a00=(a64*a00); - a15=(a70*a15); - a00=(a00+a15); - a22=(a16*a22); - a00=(a00+a22); - if (res[1]!=0) res[1][5]=a00; - a00=(a01*a05); - a22=casadi_sign(a06); - a22=(a06*a22); - a22=(a22+a20); - a20=(a14*a22); - a00=(a00+a20); - a20=(a21*a24); - a00=(a00+a20); - if (res[1]!=0) res[1][6]=a00; - a00=(a13*a05); - a20=(a47*a22); - a00=(a00+a20); - a20=(a48*a24); - a00=(a00+a20); - if (res[1]!=0) res[1][7]=a00; - a00=(a44*a05); - a20=(a53*a22); - a00=(a00+a20); - a20=(a54*a24); - a00=(a00+a20); - if (res[1]!=0) res[1][8]=a00; - a00=(a52*a05); - a20=(a59*a22); - a00=(a00+a20); - a20=(a60*a24); - a00=(a00+a20); - if (res[1]!=0) res[1][9]=a00; - a00=(a58*a05); - a20=(a65*a22); - a00=(a00+a20); - a20=(a66*a24); - a00=(a00+a20); - if (res[1]!=0) res[1][10]=a00; - a05=(a64*a05); - a22=(a70*a22); - a05=(a05+a22); - a24=(a16*a24); - a05=(a05+a24); - if (res[1]!=0) res[1][11]=a05; - a05=(a01*a10); - a24=(a14*a18); - a05=(a05+a24); - a24=casadi_sign(a11); - a24=(a11*a24); - a24=(a24+a25); - a25=(a21*a24); - a05=(a05+a25); - if (res[1]!=0) res[1][12]=a05; - a05=(a13*a10); - a25=(a47*a18); - a05=(a05+a25); - a25=(a48*a24); - a05=(a05+a25); - if (res[1]!=0) res[1][13]=a05; - a05=(a44*a10); - a25=(a53*a18); - a05=(a05+a25); - a25=(a54*a24); - a05=(a05+a25); - if (res[1]!=0) res[1][14]=a05; - a05=(a52*a10); - a25=(a59*a18); - a05=(a05+a25); - a25=(a60*a24); - a05=(a05+a25); - if (res[1]!=0) res[1][15]=a05; - a05=(a58*a10); - a25=(a65*a18); - a05=(a05+a25); - a25=(a66*a24); - a05=(a05+a25); - if (res[1]!=0) res[1][16]=a05; - a10=(a64*a10); - a18=(a70*a18); - a10=(a10+a18); - a24=(a16*a24); - a10=(a10+a24); - if (res[1]!=0) res[1][17]=a10; - a10=(a03*a11); - a24=(a14*a10); - a18=(a08*a06); - a05=(a21*a18); - a24=(a24+a05); - a05=casadi_sign(a17); - a05=(a17*a05); - a05=(a05+a32); - a32=(a26*a05); - a24=(a24+a32); - a38=(a38*a04); - a36=(a36+a38); - a38=(a33*a36); - a24=(a24+a38); - a45=(a45*a09); - a43=(a43+a45); - a45=(a40*a43); - a24=(a24+a45); - if (res[1]!=0) res[1][18]=a24; - a24=(a47*a10); - a45=(a48*a18); - a24=(a24+a45); - a45=(a49*a05); - a24=(a24+a45); - a45=(a50*a36); - a24=(a24+a45); - a45=(a51*a43); - a24=(a24+a45); - if (res[1]!=0) res[1][19]=a24; - a24=(a53*a10); - a45=(a54*a18); - a24=(a24+a45); - a45=(a55*a05); - a24=(a24+a45); - a45=(a56*a36); - a24=(a24+a45); - a45=(a57*a43); - a24=(a24+a45); - if (res[1]!=0) res[1][20]=a24; - a24=(a59*a10); - a45=(a60*a18); - a24=(a24+a45); - a45=(a61*a05); - a24=(a24+a45); - a45=(a62*a36); - a24=(a24+a45); - a45=(a63*a43); - a24=(a24+a45); - if (res[1]!=0) res[1][21]=a24; - a24=(a65*a10); - a45=(a66*a18); - a24=(a24+a45); - a45=(a67*a05); - a24=(a24+a45); - a45=(a68*a36); - a24=(a24+a45); - a45=(a69*a43); - a24=(a24+a45); - if (res[1]!=0) res[1][22]=a24; - a10=(a70*a10); - a18=(a16*a18); - a10=(a10+a18); - a05=(a23*a05); - a10=(a10+a05); - a36=(a69*a36); - a10=(a10+a36); - a43=(a34*a43); - a10=(a10+a43); - if (res[1]!=0) res[1][23]=a10; - a10=-1.; - if (res[1]!=0) res[1][24]=a10; - a11=(a08*a11); - a10=(a01*a11); - a43=(a03*a12); - a21=(a21*a43); - a10=(a10+a21); - a30=(a30*a04); - a28=(a28+a30); - a30=(a26*a28); - a10=(a10+a30); - a30=casadi_sign(a09); - a30=(a09*a30); - a30=(a30+a37); - a37=(a33*a30); - a10=(a10+a37); - a42=(a42*a17); - a42=(a42+a46); - a46=(a40*a42); - a10=(a10+a46); - if (res[1]!=0) res[1][25]=a10; - a10=(a13*a11); - a48=(a48*a43); - a10=(a10+a48); - a48=(a49*a28); - a10=(a10+a48); - a48=(a50*a30); - a10=(a10+a48); - a48=(a51*a42); - a10=(a10+a48); - if (res[1]!=0) res[1][26]=a10; - a10=(a44*a11); - a54=(a54*a43); - a10=(a10+a54); - a54=(a55*a28); - a10=(a10+a54); - a54=(a56*a30); - a10=(a10+a54); - a54=(a57*a42); - a10=(a10+a54); - if (res[1]!=0) res[1][27]=a10; - a10=(a52*a11); - a60=(a60*a43); - a10=(a10+a60); - a60=(a61*a28); - a10=(a10+a60); - a60=(a62*a30); - a10=(a10+a60); - a60=(a63*a42); - a10=(a10+a60); - if (res[1]!=0) res[1][28]=a10; - a10=(a58*a11); - a66=(a66*a43); - a10=(a10+a66); - a66=(a67*a28); - a10=(a10+a66); - a66=(a68*a30); - a10=(a10+a66); - a66=(a69*a42); - a10=(a10+a66); - if (res[1]!=0) res[1][29]=a10; - a11=(a64*a11); - a16=(a16*a43); - a11=(a11+a16); - a28=(a23*a28); - a11=(a11+a28); - a30=(a69*a30); - a11=(a11+a30); - a42=(a34*a42); - a11=(a11+a42); - if (res[1]!=0) res[1][30]=a11; - a72=(-a72); - if (res[1]!=0) res[1][31]=a72; - a72=(-a02); - if (res[1]!=0) res[1][32]=a72; - a72=(-a75); - if (res[1]!=0) res[1][33]=a72; - a03=(a03*a06); - a01=(a01*a03); - a08=(a08*a12); - a14=(a14*a08); - a01=(a01+a14); - a27=(a27*a09); - a27=(a27+a31); - a26=(a26*a27); - a01=(a01+a26); - a35=(a35*a17); - a35=(a35+a39); - a33=(a33*a35); - a01=(a01+a33); - a33=casadi_sign(a04); - a33=(a04*a33); - a33=(a33+a19); - a40=(a40*a33); - a01=(a01+a40); - if (res[1]!=0) res[1][34]=a01; - a13=(a13*a03); - a47=(a47*a08); - a13=(a13+a47); - a49=(a49*a27); - a13=(a13+a49); - a50=(a50*a35); - a13=(a13+a50); - a51=(a51*a33); - a13=(a13+a51); - if (res[1]!=0) res[1][35]=a13; - a44=(a44*a03); - a53=(a53*a08); - a44=(a44+a53); - a55=(a55*a27); - a44=(a44+a55); - a56=(a56*a35); - a44=(a44+a56); - a57=(a57*a33); - a44=(a44+a57); - if (res[1]!=0) res[1][36]=a44; - a52=(a52*a03); - a59=(a59*a08); - a52=(a52+a59); - a61=(a61*a27); - a52=(a52+a61); - a62=(a62*a35); - a52=(a52+a62); - a63=(a63*a33); - a52=(a52+a63); - if (res[1]!=0) res[1][37]=a52; - a58=(a58*a03); - a65=(a65*a08); - a58=(a58+a65); - a67=(a67*a27); - a58=(a58+a67); - a68=(a68*a35); - a58=(a58+a68); - a68=(a69*a33); - a58=(a58+a68); - if (res[1]!=0) res[1][38]=a58; - a64=(a64*a03); - a70=(a70*a08); - a64=(a64+a70); - a23=(a23*a27); - a64=(a64+a23); - a69=(a69*a35); - a64=(a64+a69); - a34=(a34*a33); - a64=(a64+a34); - if (res[1]!=0) res[1][39]=a64; - a74=(-a74); - if (res[1]!=0) res[1][40]=a74; - if (res[1]!=0) res[1][41]=a41; - a74=(-a77); - if (res[1]!=0) res[1][42]=a74; - a74=(a71*a02); - a74=(a09*a74); - a71=(a71*a41); - a71=(a04*a71); - a74=(a74-a71); - a74=(-a74); - if (res[1]!=0) res[1][43]=a74; - a74=(a09*a41); - a71=(a04*a02); - a74=(a74+a71); - if (res[1]!=0) res[1][44]=a74; - a74=(a09*a77); - a71=(a04*a75); - a74=(a74-a71); - a74=(-a74); - if (res[1]!=0) res[1][45]=a74; - a74=casadi_sq(a73); - a41=(a41/a74); - a41=(a09*a41); - a02=(a02/a74); - a02=(a04*a02); - a41=(a41+a02); - a41=(-a41); - if (res[1]!=0) res[1][46]=a41; - a75=(a75/a73); - a29=sin(a29); - a75=(a75*a29); - a09=(a09*a75); - a77=(a77/a73); - a77=(a77*a29); - a04=(a04*a77); - a09=(a09+a04); - a09=(-a09); - if (res[1]!=0) res[1][47]=a09; - a09=1.; - if (res[2]!=0) res[2][0]=a09; - if (res[2]!=0) res[2][1]=a09; - if (res[2]!=0) res[2][2]=a09; - if (res[2]!=0) res[2][3]=a09; - if (res[2]!=0) res[2][4]=a09; - if (res[2]!=0) res[2][5]=a09; - if (res[2]!=0) res[2][6]=a09; - if (res[2]!=0) res[2][7]=a09; - if (res[2]!=0) res[2][8]=a09; - a09=-3.3453634479321918e-02; - if (res[3]!=0) res[3][0]=a09; - a09=-6.0368975005218423e-05; - if (res[3]!=0) res[3][1]=a09; - a09=1.1116270017027936e-07; - if (res[3]!=0) res[3][2]=a09; - a09=-9.8084735444364535e-08; - if (res[3]!=0) res[3][3]=a09; - a09=-1.0920100546139210e-05; - if (res[3]!=0) res[3][4]=a09; - a09=6.0150572994295635e-03; - if (res[3]!=0) res[3][5]=a09; - a09=-1.0920100546139184e-05; - if (res[3]!=0) res[3][6]=a09; - a09=-6.0570157695918683e-03; - if (res[3]!=0) res[3][7]=a09; - a09=3.0801331505514837e-03; - if (res[3]!=0) res[3][8]=a09; - a09=-2.7177645446042520e-03; - if (res[3]!=0) res[3][9]=a09; - a09=-3.0257778596593993e-01; - if (res[3]!=0) res[3][10]=a09; - a09=5.4600502730695923e-04; - if (res[3]!=0) res[3][11]=a09; - a04=6.0150572994295574e-03; - if (res[3]!=0) res[3][12]=a04; - a04=3.0184487502609172e-03; - if (res[3]!=0) res[3][13]=a04; - a04=-5.5581350085139543e-06; - if (res[3]!=0) res[3][14]=a04; - a04=4.9042367722181902e-06; - if (res[3]!=0) res[3][15]=a04; - if (res[3]!=0) res[3][16]=a09; - a09=-3.0075286497147785e-01; - if (res[3]!=0) res[3][17]=a09; - return 0; -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_u_z(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ - return casadi_f0(arg, res, iw, w, mem); -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_u_z_alloc_mem(void) { - return 0; -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_u_z_init_mem(int mem) { - return 0; -} - -CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_jac_x_xdot_u_z_free_mem(int mem) { -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_u_z_checkout(void) { - return 0; -} - -CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_jac_x_xdot_u_z_release(int mem) { -} - -CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_jac_x_xdot_u_z_incref(void) { -} - -CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_jac_x_xdot_u_z_decref(void) { -} - -CASADI_SYMBOL_EXPORT casadi_int auv_model_impl_dae_fun_jac_x_xdot_u_z_n_in(void) { return 6;} - -CASADI_SYMBOL_EXPORT casadi_int auv_model_impl_dae_fun_jac_x_xdot_u_z_n_out(void) { return 5;} - -CASADI_SYMBOL_EXPORT casadi_real auv_model_impl_dae_fun_jac_x_xdot_u_z_default_in(casadi_int i) { - switch (i) { - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const char* auv_model_impl_dae_fun_jac_x_xdot_u_z_name_in(casadi_int i) { - switch (i) { - case 0: return "i0"; - case 1: return "i1"; - case 2: return "i2"; - case 3: return "i3"; - case 4: return "i4"; - case 5: return "i5"; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const char* auv_model_impl_dae_fun_jac_x_xdot_u_z_name_out(casadi_int i) { - switch (i) { - case 0: return "o0"; - case 1: return "o1"; - case 2: return "o2"; - case 3: return "o3"; - case 4: return "o4"; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const casadi_int* auv_model_impl_dae_fun_jac_x_xdot_u_z_sparsity_in(casadi_int i) { - switch (i) { - case 0: return casadi_s0; - case 1: return casadi_s0; - case 2: return casadi_s1; - case 3: return casadi_s2; - case 4: return casadi_s3; - case 5: return casadi_s2; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const casadi_int* auv_model_impl_dae_fun_jac_x_xdot_u_z_sparsity_out(casadi_int i) { - switch (i) { - case 0: return casadi_s0; - case 1: return casadi_s4; - case 2: return casadi_s5; - case 3: return casadi_s6; - case 4: return casadi_s7; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_u_z_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { - if (sz_arg) *sz_arg = 6; - if (sz_res) *sz_res = 5; - if (sz_iw) *sz_iw = 0; - if (sz_w) *sz_w = 0; - return 0; -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_u_z_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { - if (sz_arg) *sz_arg = 6*sizeof(const casadi_real*); - if (sz_res) *sz_res = 5*sizeof(casadi_real*); - if (sz_iw) *sz_iw = 0*sizeof(casadi_int); - if (sz_w) *sz_w = 0*sizeof(casadi_real); - return 0; -} - - -#ifdef __cplusplus -} /* extern "C" */ -#endif diff --git a/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u_z.o b/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u_z.o deleted file mode 100644 index dc3e918bb13381d14374a20d16c91be469187efa..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 21944 zcmchf3wTwt>2LXIlvAO@XW=*m(a(crp z?R?+fd*(N5)~s1Gdmj4)zExCw`62L&?=N@qsy`ShB|Jx3#VyxjsbBa3GwMHY3%PlK93-Ism0?~g1l z@)u>iGYGx`=z;j!-c9wW#+EeP!c?H)eZT_2m)+C zR%vhy?j6A-)Q|j1Kbras7YQI4KUmPGqvh{WE?``7%md}2-mA57(yMs39Ey>DBzpW z5ey(rGBBhtmo8lO1+QOh8HZKg8K+`6eRTvB* z_9izfVk#g5U`*;>s=OUas2V@=xZ*KI8GVE5L2xV$nWd+ZWGdH!Otz zF}~hF`G?rg@Zr7eE0B0FfEK<$ddj)cj|53Ka+U{|8NGgLf9w?!DTU#96bD<17te)@ z%3p)74Y*#d^PRO1&z&e(L&GxmJg&FVnjIjI@UUPdzUZGJ_8eewunB|;bckzX`;mx~ zW1~AD^zxAcU+hmvP$kb%Vh7zYRz)c}naGRC|D>+Km$n;O9~0RF@;pVh6UN9ci({N4 zjhC$aMe+t-hVsK~MHn$-B)OY}P-pp8@9bemZ$z*r*DAfC}PpT{QrL8?)&1N{9 z)K+qh7{{M&W;wqRC$X~FznJckW@1DhuTtQC=i0X&{TbuAX>gJ9jo|3pn7qVVUHJxZ z^mFVoQ|X|*P2hw3ywDeWiXqtGo~ma`Dv5Lkx! zCrq=`UlE-XjMxIDLA(`y0GQc>Ya^tw7-ACKg%)DE{gejmQ#9wc0@z|)0PjB)HWr{v zVK92ccf$c(d%2QI+-rgnXk5;P5&0C?AAy$#OJFso5=@p@x2Orod6Orz@1P#25R)}d$&ZHdzC`y$x>0lkd9Ik2I7~ONDz=T>$#20-@*X)8zA5j9Kq0qw zqbWfSD-Oa`DiQ4_!6KBmU4ba%)41J{#nWl!_qDBO3uC<8rSb5C1*`26dKPj$=rT(k zH4E*;6~lU)+J$EiE*?L1SdWqa5U-YiTvDLKV?DlDbtHubQxbt1nUas zds?88A$CQv_W=wz(3cVpaMQ*NZrT&JX=6}qpe4%~qpN9$fgFB+@$BH$1?|G-E4JIT zkG2fd#?dhAm6w*#l0P66-ANfx5JrnixJ72I z@9(U~jp+{sWglVt%j{9CS?GW463~F{+;g-8KqXjjY_CPPSW&QDs8pw4g!3zrFS>0< zQrUhFXnB;kJsRDDvP~!pS?XD8OlzyLxrUZ(jLtEzj(3L`iZt-=m5m_YE(l=Q4|JlH z$GaKeu^Kvs-=-XB#Gvfo>9C^y84=63KNaf_#w;(jz8Kc37_STE<+Mzj7U8uhrL{at zJJ?A^V+`+m^j)N-hS*Uj|Ibc7G^Z?UA8ZY6AO3Eg5B~#nE;%VWQ6s|jyXY=l z*c+v^oBqfCFAbVX=}3a{MWd`ExWp9a-7HQd{SyaN@&N@S)r`3x(JPAG25rTBu@OPo z_9BG(HE^?0VOy{VDrvt(11Qd88`C~Eac@)cXX!@9Eey_viUYXJbWrF^U58Ssm~dh; zsieLG5bMGVj1?uuenx5QvhsMa1+Uh6Xt44hpe#*9ipBoqq+gcwOM@!I``Jxq`_k5# z39tWE#%nDWVwr+|ZYOkTjs-Txua3i?0ykzGdD4yjw$bfNd6<{u0$&tOj6!!5dZJIk z6zT;yPv%KlU&`8ot$@h;$mC(YLj%gN6G>^66Qns_lzwLV;3?x5m@>%qd}1`y4>~wN z=YdOu(3D1JRCBTsi_88(nR9Tgz=KCKmEUA!fp}h|Jn?=Rwiq(`px1igwnNC}!wD_J z=Dj#l#9Qtdoy}P%M4~us$V`KRH1Ny$a4`-oB^c)bf5adI0K-^9gOHde_5ntW_AF-Q)$+ zGvFJ5qJvQvBVWqha*nli;nWcw@PjY*3DSRsm&Dz%_h2{e1hy&$FGjaQ6|MBvY>XfF zj2%bd3wf`D3e1}W#n0lxLYGQMopRc)MV~>egExTWKD-raJ7H)Uj(t(uq0;vSYz1fs zN2zBqtjwMRyD%CUVJO=G7-mb#pF{a=RQ?!AZzC5Sqa;>tdV|tlfOroIw(&L)QxGer zqZhWmw=BHNkf5Q2g}eV0>h;^t%~Ik$WQK!7g74-r=yHc51jn z5FbP04X}d86!Hh6VZVw8R6L~OP-}~ZOH{m4#hV7=ybX!mv4OsNWprJ4eL;D_^#wN+ zRM_uiXw%DeMZQB_G|Zo-j_Sn@0qcMNzG4@;FeT1k8+ho#J$Q}up@|M!gVIAeM8Etu z>Y_e9iFNf)3xBIljGF#B&8)-iw9|!d-$^`N(w7&!OPI@W3c}Q~8-0=1jnrCTygj&e zVJhO~8xJ{F;zK2UeWEvI(XzvRSS`)RDtzoj&-02E*@P8?Ce+9#bMi9r!eE#p_9%!q zrH1Ab+@lA_9O>(;^PSg50{EV4zB!_vF0C^_nJ6$K$X_Gr`&};xJ@;~sv4koxlNk0K z=8NV)22C_vXNff&sX~5l&6`C!dkFsq%uV9`l%(a(DO`VM~hS2nefnuoU* z6?-reYn}a`h!jtnlx|mcv06Gr9Q+Q{;=42X5yDs`%FPsjnFPBX8uO*WT-aC2b8RV& z|0pEN>9Du<;8J+Kt}fkrtFE2CKWB7dYUuahXVdFdiQ~z;`TtjX)hM)#s)Qd%-zpln zDouT7-*d{pvv1@rjgk9!9lob8Qa$2{$grNgzPj1w_bB=_q8QyY z*Xz+^#;24Iu_TvI<7k5N^&zi5!>iBq>a)E1Y_C4YYYcgf8D3+i*O=usW>Yo)^l+Ng z$c_9PwUgX%$tQ=Mj_V9bQ(5vC<&VI{BOlKb;iO;N;_9P1PsNpV0&)=T4_d#r{|k^f zAJ@gW(s5B|(naA!>_1U?vLR6!-PWPVXnBg0k8IHWN^zT}zS3#(k52cQ@GYyydA3U0 zKAr5|{1xXud&&2I?#uslOMCuX>`N27FAzJ2huQv4X>Xow?=7aCo!fVB zANTyx5A$QDJ)H+bTb^j2UYGx{wCnQ^Z@c!j2X?NJ_I679`~6|<-6!SF7Wu4{ty3DW z-;rO^KJn$;eFdHN*}QS<;k)y9*#7F2_PF+~5r40-^-25INc%d)-h8pQQ|hzzSyGBfRR|MQ>fTYIOMib?>K@-#@x*Jw4XF zkuT3q;a0B8n=yU*mjY=sXU%Vj&ktl|rDvpvCgslOWXA28ndzbI34*yQn&14mXpWol zwV5>UbrG|3BYwZ-FzmLgyVP~IyYEnC1EGl!38Vux3Ky(;*B`t&qwBAy<8W-`f!>Rqm>C-j^E8l@Q z@e+qWD?BW1>!m^nAK)T+>nE${JjfB>OF{^x=HXF45pR`rr95wDy-8lKr0wGy!b29w zbAj-H!&?*ZMZ!xQ`NawN1H!|O{0|cF9}BmBvBj1q;J>tb_-Um*Z$$$BTj3>+{F4dz zTH%$ByxE;5c`koek*^oN(hAD6F#(@1e2F8! zAOXKaxZkPwt_1u(;XRK0g9-Q#h3|Fbf0}^*On9#&|LX+&G2yG6_WoY@0f+xV_+E#v z7vAmgr-gSpe4FrHj-7uL{*lA?2yb%us|omBj@9;B)k8$`Hga;gcp71n>j}snp_{G9=9iG7)^R~mMhBR^B*OC0&@M4k`$Dz8eopZjMZ<3`~DhtCym_b0YQ zvv9kgvHVuycHd(8w}e+Z`WFdra`&;Jzevp}A| z6z=ZqRtR@T?r(+L{gzd?S~!1Xv=Gnr!tFO0o3&Z^xfaOtIpOX&-zEHPNB$M?Gt^>X z&n%-y7;gP$&n-E^?f9|nyFqxR=;!kb;%(qohpu{Y0)AHlet!b~AUN6IL+2#~d(K%e ze6Pcg2=5hc&n#mRVLyC+u$~g(e&JUBcHses|5|v+;k$%~h1>Jaa10!(x5VL>2(J`w z{ah)$Nw_@^eOGv|aC;{DgViGgB5IoF9TMIp+{&L12a-J@`e24&&qr4XZ*sU-HUAqG za~o@F=Ty|sZJJY2Q&n40H@~6c>s8eiEfp;_jq!^46}P2V4^6J3IW$zk%%KWq4OK8> zsDjx;6`V3u!JMHAPWIA^r)Sb_3f(xFL$M6L=VWl);1I`+oZ-1#hVL0uInMV?mYl-( zOqR^#Dl)T3CNrDwtUuG#&pLAqXKgt~58rcGGRx>>Jz1=C3Tw+U#&g*$LpYve;#_vJ zDaCbVaa~zlOP0~kB%Af;uuL}VpUfJvjeaA;WD1wf=31t(hHS1So6AmSJ=vy3M$+6_ zGS_Hh{kg0y*XUslxqN2@MuHs0r@+7l3Ns2H-p8!Cq0yqem&s`+!` zC!oA)ZtV$558ayPrmA_(_3>MWUO|6}p;vNC=Ev55^UL zOwJ)Be~sc_Ab@~vnWyE=cNkMIeS@_6a}vm3CY&BKmAv_W@>xCCDEVuZd`klTtx8_o z)1i2o((^iQ*}WZ&W;_ z_=}2172mJ;1&Y6~_!PyH;uG_m?@o~z|R~6TOct`Oj-L4YFwI3EJPTzX0 zpBF2x?f;SDy1h#k*Y>YaT(|d0#WPjCn-u?d#Zy0r1cKY%9O2|2y^dU_xL%LGs<>Vs z=PMqAF6)Q86xZv~4-_Ax
KUXOmSxL#kjD6ZG3J&Nmf=Pktps@_i&*Xz#bPw}AQmnfd5c)sEnDSoBm6BVyeT(1|+itBafPQ~>)@}S~+9a*ZlUPqo(T+iq0 z70*?6Zc#j}_;$thzVsc%_5A-A#b+ozBjp1o`CrffV-=4m`ALfF^{YT}y&l~y+Ku{o!8;A&mM>8J zD~f+laXoMRMDYnq{#S}$rugp^zgY2gisvi7Rq;uRzo>YD;_oVch2m$?;RJ#FOd__u z*9dp}@m?jb_s>fd*Yn$N6xZ{~YQ@u4z1@n>RQz?tzpD6qihoV_Y~Lj?Fz;9xLv2X9=CfG*YoFF zitG996UFs7#6KwEK>pO@ZM@=o{z+F{kK4(L>+u;;T+df$&|pHKarg~fw!f+sU!eGM z;qG|ep*T&AR?nUUd^l}N5J*o2E-Rm`c%|a>zidz-`6^sie!Swd6wg$=TJb!^YZU*A z;wUsgitBZDyW;x2drfiu9?}0aM1lOQ*ZosylZ`+;jLZ7-D#i8tQm!~H-B!Lsahj4X zU#a+P#rG(#*PZtize>rErb!cl?7SM6)o*`CC9d^1D|xMdA#DN>NPajjtLH(*>lI(B z_}3Nxjp8>czFzS;itkW-uHvsN-k|s~#mP?TKJQH0+#`_vjkv7+7bxDOc$VTfD?VNE zd5T}7c(dYj6eoM=ou_c8!^_Dqga;jdsqm!^pD6q{4lfq|jKjmM-Sb{|_@%>4#!-h` zdxq0ui9l+}{!&UHoaykF4E8Q?_(I|KL3-#~BK<_`69vm35}ssmKkpV!?-T_q|B`T7 ze*3xoz1sG_mER!p(%gRefPiz1`hK1&JmBz%aGLWeSpDU~Z9iKc7EW_G1DZS@|yE8ytE2cO3Tb0G)Zhg$(P@ zt8R{G;Fn~PL<&DVS2xa`TiXCt4UO^Ibo`TwNzL)9>YF5eW5fLPrg@D`we#Y)iq6{E z6?OCQyPkC5-}wljJh^6+qbDrsr83@awjIwlO=k|81H%@`vxGKF2qm%})82sUNg{|K z-NN63h|}Dx!|P7HHcz_ICOw*uNaz2zJnbb(XC^KkZ2e<3MBLVIpR_llIE^120`$=h zVUCj#U^;LMBYHglEs=@J{(T|&kNU{ye1Hz0 z2vPIszuWrlb2f6Qe5hZ-L|UXg^^XoDOTj*AZ(`$|A-4`GuNlQ%**m~a#pN{9TPEcL z$kV~vZP^bIA)RjdZYh7jsn9L|5Xv5}|5kp@)Ua2|Q~%Mv*_OBC|9Ev%$(3X2?s)+! ugh2fj$K{r%&-L!u2l9H->F#;AlXFyXnuGs2_4wu6OH84qsw_p_^8X71$7nqO diff --git a/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_z.c b/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_z.c deleted file mode 100644 index 7cbf6b868..000000000 --- a/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_z.c +++ /dev/null @@ -1,809 +0,0 @@ -/* This file was automatically generated by CasADi 3.7.2. - * It consists of: - * 1) content generated by CasADi runtime: not copyrighted - * 2) template code copied from CasADi source: permissively licensed (MIT-0) - * 3) user code: owned by the user - * - */ -#ifdef __cplusplus -extern "C" { -#endif - -/* How to prefix internal symbols */ -#ifdef CASADI_CODEGEN_PREFIX - #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID) - #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID - #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID) -#else - #define CASADI_PREFIX(ID) auv_model_impl_dae_fun_jac_x_xdot_z_ ## ID -#endif - -#include - -#ifndef casadi_real -#define casadi_real double -#endif - -#ifndef casadi_int -#define casadi_int int -#endif - -/* Add prefix to internal symbols */ -#define casadi_f0 CASADI_PREFIX(f0) -#define casadi_fabs CASADI_PREFIX(fabs) -#define casadi_s0 CASADI_PREFIX(s0) -#define casadi_s1 CASADI_PREFIX(s1) -#define casadi_s2 CASADI_PREFIX(s2) -#define casadi_s3 CASADI_PREFIX(s3) -#define casadi_s4 CASADI_PREFIX(s4) -#define casadi_s5 CASADI_PREFIX(s5) -#define casadi_s6 CASADI_PREFIX(s6) -#define casadi_sign CASADI_PREFIX(sign) -#define casadi_sq CASADI_PREFIX(sq) - -/* Symbol visibility in DLLs */ -#ifndef CASADI_SYMBOL_EXPORT - #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) - #if defined(STATIC_LINKED) - #define CASADI_SYMBOL_EXPORT - #else - #define CASADI_SYMBOL_EXPORT __declspec(dllexport) - #endif - #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) - #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default"))) - #else - #define CASADI_SYMBOL_EXPORT - #endif -#endif - -casadi_real casadi_fabs(casadi_real x) { -/* Pre-c99 compatibility */ -#if __STDC_VERSION__ < 199901L - return x>0 ? x : -x; -#else - return fabs(x); -#endif -} - -casadi_real casadi_sign(casadi_real x) { return x<0 ? -1 : x>0 ? 1 : x;} - -casadi_real casadi_sq(casadi_real x) { return x*x;} - -static const casadi_int casadi_s0[3] = {9, 1, 1}; -static const casadi_int casadi_s1[3] = {3, 1, 1}; -static const casadi_int casadi_s2[3] = {0, 1, 1}; -static const casadi_int casadi_s3[3] = {0, 0, 1}; -static const casadi_int casadi_s4[60] = - {9, 9, 0, 6, 12, 18, 25, 34, - 43, 46, 48, 48, 0, 1, 2, 3, - 4, 5, 0, 1, 2, 3, 4, 5, - 0, 1, 2, 3, 4, 5, 0, 1, - 2, 3, 4, 5, 6, 0, 1, 2, - 3, 4, 5, 6, 7, 8, 0, 1, - 2, 3, 4, 5, 6, 7, 8, 6, - 7, 8, 6, 8}; -static const casadi_int casadi_s5[21] = - {9, 9, 0, 1, 2, 3, 4, 5, - 6, 7, 8, 9, 0, 1, 2, 3, - 4, 5, 6, 7, 8}; -static const casadi_int casadi_s6[3] = {9, 0, 1}; - -/* auv_model_impl_dae_fun_jac_x_xdot_z:(i0[9],i1[9],i2[3],i3[0],i4[],i5[0])->(o0[9],o1[9x9,48nz],o2[9x9,9nz],o3[9x0]) */ -static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { - casadi_real a00, a01, a02, a03, a04, a05, a06, a07, a08, a09, a10, a11; - casadi_real a12, a13, a14, a15, a16, a17, a18, a19, a20, a21, a22, a23; - casadi_real a24, a25, a26, a27, a28, a29, a30, a31, a32, a33, a34, a35; - casadi_real a36, a37, a38, a39, a40, a41, a42, a43, a44, a45, a46, a47; - casadi_real a48, a49, a50, a51, a52, a53, a54, a55, a56, a57, a58, a59; - casadi_real a60, a61, a62, a63, a64, a65, a66, a67, a68, a69, a70, a71; - casadi_real a72, a73, a74, a75, a76, a77, a78; - a00=arg[1]? arg[1][0] : 0; - a01=3.3453634479321918e-02; - a02=arg[2]? arg[2][0] : 0; - a03=-30.; - a04=arg[0]? arg[0][5] : 0; - a05=(a03*a04); - a06=arg[0]? arg[0][1] : 0; - a07=(a05*a06); - a08=30.; - a09=arg[0]? arg[0][4] : 0; - a10=(a08*a09); - a11=arg[0]? arg[0][2] : 0; - a12=(a10*a11); - a07=(a07+a12); - a02=(a02-a07); - a07=23.; - a12=arg[0]? arg[0][0] : 0; - a13=casadi_fabs(a12); - a07=(a07+a13); - a13=(a07*a12); - a02=(a02-a13); - a13=(a01*a02); - a14=6.0368975005218254e-05; - a15=(a08*a04); - a16=(a15*a12); - a17=arg[0]? arg[0][3] : 0; - a18=(a03*a17); - a19=(a18*a11); - a16=(a16+a19); - a19=46.; - a20=casadi_fabs(a06); - a20=(a19+a20); - a21=(a20*a06); - a16=(a16+a21); - a21=(a14*a16); - a13=(a13-a21); - a21=-1.1116270017027866e-07; - a22=(a03*a09); - a23=(a22*a12); - a24=(a08*a17); - a25=(a24*a06); - a23=(a23+a25); - a25=casadi_fabs(a11); - a25=(a19+a25); - a26=(a25*a11); - a23=(a23+a26); - a26=(a21*a23); - a13=(a13-a26); - a26=9.8084735444363873e-08; - a27=3.3399999999999999e+00; - a28=(a27*a04); - a29=(a28*a09); - a30=-3.3199999999999998e+00; - a31=(a30*a09); - a32=(a31*a04); - a29=(a29+a32); - a32=casadi_fabs(a17); - a32=(a19+a32); - a33=(a32*a17); - a29=(a29+a33); - a33=(a26*a29); - a13=(a13-a33); - a33=1.0920100546139184e-05; - a34=arg[2]? arg[2][1] : 0; - a35=-3.3399999999999999e+00; - a36=(a35*a04); - a37=(a36*a17); - a38=6.8000000000000005e-01; - a39=(a38*a17); - a40=(a39*a04); - a37=(a37+a40); - a34=(a34-a37); - a37=casadi_fabs(a09); - a37=(a19+a37); - a40=(a37*a09); - a34=(a34-a40); - a40=(a33*a34); - a13=(a13+a40); - a40=-6.0150572994295574e-03; - a41=arg[2]? arg[2][2] : 0; - a42=3.3199999999999998e+00; - a43=(a42*a09); - a44=(a43*a17); - a45=-6.8000000000000005e-01; - a46=(a45*a17); - a47=(a46*a09); - a44=(a44+a47); - a41=(a41-a44); - a44=casadi_fabs(a04); - a19=(a19+a44); - a44=(a19*a04); - a41=(a41-a44); - a44=(a40*a41); - a13=(a13+a44); - a00=(a00-a13); - if (res[0]!=0) res[0][0]=a00; - a00=arg[1]? arg[1][1] : 0; - a13=6.0368975005218423e-05; - a44=(a13*a02); - a47=3.3484658136227786e-02; - a48=(a47*a16); - a44=(a44-a48); - a48=-6.1658244361114702e-05; - a49=(a48*a23); - a44=(a44-a49); - a49=5.4404333259807184e-05; - a50=(a49*a29); - a44=(a44-a50); - a50=6.0570157695918683e-03; - a51=(a50*a34); - a44=(a44+a51); - a51=-3.0184487502609172e-03; - a52=(a51*a41); - a44=(a44+a52); - a00=(a00-a44); - if (res[0]!=0) res[0][1]=a00; - a00=arg[1]? arg[1][2] : 0; - a44=-1.1116270017027936e-07; - a52=(a44*a02); - a53=-6.1658244361114783e-05; - a54=(a53*a16); - a52=(a52-a54); - a54=3.3963490377380855e-02; - a55=(a54*a23); - a52=(a52-a55); - a55=-2.9967785627100754e-02; - a56=(a55*a29); - a52=(a52-a56); - a56=-3.0801331505514837e-03; - a57=(a56*a34); - a52=(a52+a57); - a57=5.5581350085139543e-06; - a58=(a57*a41); - a52=(a52+a58); - a00=(a00-a52); - if (res[0]!=0) res[0][2]=a00; - a00=arg[1]? arg[1][3] : 0; - a52=9.8084735444364535e-08; - a58=(a52*a02); - a59=5.4404333259807062e-05; - a60=(a59*a16); - a58=(a58-a60); - a60=-2.9967785627100469e-02; - a61=(a60*a23); - a58=(a58-a61); - a61=1.4970303990827358e+00; - a62=(a61*a29); - a58=(a58-a62); - a62=2.7177645446042520e-03; - a63=(a62*a34); - a58=(a58+a63); - a63=-4.9042367722181902e-06; - a64=(a63*a41); - a58=(a58+a64); - a00=(a00-a58); - if (res[0]!=0) res[0][3]=a00; - a00=arg[1]? arg[1][4] : 0; - a58=1.0920100546139210e-05; - a64=(a58*a02); - a65=6.0570157695918657e-03; - a66=(a65*a16); - a64=(a64-a66); - a66=-3.0801331505514781e-03; - a67=(a66*a23); - a64=(a64-a67); - a67=2.7177645446042498e-03; - a68=(a67*a29); - a64=(a64-a68); - a68=3.0257778596593993e-01; - a69=(a68*a34); - a64=(a64+a69); - a69=-5.4600502730695923e-04; - a70=(a69*a41); - a64=(a64+a70); - a00=(a00-a64); - if (res[0]!=0) res[0][4]=a00; - a00=arg[1]? arg[1][5] : 0; - a64=-6.0150572994295635e-03; - a02=(a64*a02); - a70=-3.0184487502609133e-03; - a16=(a70*a16); - a02=(a02-a16); - a16=5.5581350085139340e-06; - a23=(a16*a23); - a02=(a02-a23); - a23=-4.9042367722181935e-06; - a29=(a23*a29); - a02=(a02-a29); - a34=(a69*a34); - a02=(a02+a34); - a34=3.0075286497147785e-01; - a41=(a34*a41); - a02=(a02+a41); - a00=(a00-a02); - if (res[0]!=0) res[0][5]=a00; - a00=arg[1]? arg[1][6] : 0; - a02=arg[0]? arg[0][6] : 0; - a41=sin(a02); - a29=arg[0]? arg[0][7] : 0; - a71=tan(a29); - a72=(a41*a71); - a73=(a72*a09); - a73=(a17+a73); - a02=cos(a02); - a74=(a02*a71); - a75=(a74*a04); - a73=(a73+a75); - a00=(a00-a73); - if (res[0]!=0) res[0][6]=a00; - a00=arg[1]? arg[1][7] : 0; - a73=(a02*a09); - a75=(a41*a04); - a73=(a73-a75); - a00=(a00-a73); - if (res[0]!=0) res[0][7]=a00; - a00=arg[1]? arg[1][8] : 0; - a73=cos(a29); - a75=(a41/a73); - a76=(a75*a09); - a77=(a02/a73); - a78=(a77*a04); - a76=(a76+a78); - a00=(a00-a76); - if (res[0]!=0) res[0][8]=a00; - a00=casadi_sign(a12); - a00=(a12*a00); - a00=(a00+a07); - a07=(a01*a00); - a76=(a14*a15); - a07=(a07+a76); - a76=(a21*a22); - a07=(a07+a76); - if (res[1]!=0) res[1][0]=a07; - a07=(a13*a00); - a76=(a47*a15); - a07=(a07+a76); - a76=(a48*a22); - a07=(a07+a76); - if (res[1]!=0) res[1][1]=a07; - a07=(a44*a00); - a76=(a53*a15); - a07=(a07+a76); - a76=(a54*a22); - a07=(a07+a76); - if (res[1]!=0) res[1][2]=a07; - a07=(a52*a00); - a76=(a59*a15); - a07=(a07+a76); - a76=(a60*a22); - a07=(a07+a76); - if (res[1]!=0) res[1][3]=a07; - a07=(a58*a00); - a76=(a65*a15); - a07=(a07+a76); - a76=(a66*a22); - a07=(a07+a76); - if (res[1]!=0) res[1][4]=a07; - a00=(a64*a00); - a15=(a70*a15); - a00=(a00+a15); - a22=(a16*a22); - a00=(a00+a22); - if (res[1]!=0) res[1][5]=a00; - a00=(a01*a05); - a22=casadi_sign(a06); - a22=(a06*a22); - a22=(a22+a20); - a20=(a14*a22); - a00=(a00+a20); - a20=(a21*a24); - a00=(a00+a20); - if (res[1]!=0) res[1][6]=a00; - a00=(a13*a05); - a20=(a47*a22); - a00=(a00+a20); - a20=(a48*a24); - a00=(a00+a20); - if (res[1]!=0) res[1][7]=a00; - a00=(a44*a05); - a20=(a53*a22); - a00=(a00+a20); - a20=(a54*a24); - a00=(a00+a20); - if (res[1]!=0) res[1][8]=a00; - a00=(a52*a05); - a20=(a59*a22); - a00=(a00+a20); - a20=(a60*a24); - a00=(a00+a20); - if (res[1]!=0) res[1][9]=a00; - a00=(a58*a05); - a20=(a65*a22); - a00=(a00+a20); - a20=(a66*a24); - a00=(a00+a20); - if (res[1]!=0) res[1][10]=a00; - a05=(a64*a05); - a22=(a70*a22); - a05=(a05+a22); - a24=(a16*a24); - a05=(a05+a24); - if (res[1]!=0) res[1][11]=a05; - a05=(a01*a10); - a24=(a14*a18); - a05=(a05+a24); - a24=casadi_sign(a11); - a24=(a11*a24); - a24=(a24+a25); - a25=(a21*a24); - a05=(a05+a25); - if (res[1]!=0) res[1][12]=a05; - a05=(a13*a10); - a25=(a47*a18); - a05=(a05+a25); - a25=(a48*a24); - a05=(a05+a25); - if (res[1]!=0) res[1][13]=a05; - a05=(a44*a10); - a25=(a53*a18); - a05=(a05+a25); - a25=(a54*a24); - a05=(a05+a25); - if (res[1]!=0) res[1][14]=a05; - a05=(a52*a10); - a25=(a59*a18); - a05=(a05+a25); - a25=(a60*a24); - a05=(a05+a25); - if (res[1]!=0) res[1][15]=a05; - a05=(a58*a10); - a25=(a65*a18); - a05=(a05+a25); - a25=(a66*a24); - a05=(a05+a25); - if (res[1]!=0) res[1][16]=a05; - a10=(a64*a10); - a18=(a70*a18); - a10=(a10+a18); - a24=(a16*a24); - a10=(a10+a24); - if (res[1]!=0) res[1][17]=a10; - a10=(a03*a11); - a24=(a14*a10); - a18=(a08*a06); - a05=(a21*a18); - a24=(a24+a05); - a05=casadi_sign(a17); - a05=(a17*a05); - a05=(a05+a32); - a32=(a26*a05); - a24=(a24+a32); - a38=(a38*a04); - a36=(a36+a38); - a38=(a33*a36); - a24=(a24+a38); - a45=(a45*a09); - a43=(a43+a45); - a45=(a40*a43); - a24=(a24+a45); - if (res[1]!=0) res[1][18]=a24; - a24=(a47*a10); - a45=(a48*a18); - a24=(a24+a45); - a45=(a49*a05); - a24=(a24+a45); - a45=(a50*a36); - a24=(a24+a45); - a45=(a51*a43); - a24=(a24+a45); - if (res[1]!=0) res[1][19]=a24; - a24=(a53*a10); - a45=(a54*a18); - a24=(a24+a45); - a45=(a55*a05); - a24=(a24+a45); - a45=(a56*a36); - a24=(a24+a45); - a45=(a57*a43); - a24=(a24+a45); - if (res[1]!=0) res[1][20]=a24; - a24=(a59*a10); - a45=(a60*a18); - a24=(a24+a45); - a45=(a61*a05); - a24=(a24+a45); - a45=(a62*a36); - a24=(a24+a45); - a45=(a63*a43); - a24=(a24+a45); - if (res[1]!=0) res[1][21]=a24; - a24=(a65*a10); - a45=(a66*a18); - a24=(a24+a45); - a45=(a67*a05); - a24=(a24+a45); - a45=(a68*a36); - a24=(a24+a45); - a45=(a69*a43); - a24=(a24+a45); - if (res[1]!=0) res[1][22]=a24; - a10=(a70*a10); - a18=(a16*a18); - a10=(a10+a18); - a05=(a23*a05); - a10=(a10+a05); - a36=(a69*a36); - a10=(a10+a36); - a43=(a34*a43); - a10=(a10+a43); - if (res[1]!=0) res[1][23]=a10; - a10=-1.; - if (res[1]!=0) res[1][24]=a10; - a11=(a08*a11); - a10=(a01*a11); - a43=(a03*a12); - a21=(a21*a43); - a10=(a10+a21); - a30=(a30*a04); - a28=(a28+a30); - a30=(a26*a28); - a10=(a10+a30); - a30=casadi_sign(a09); - a30=(a09*a30); - a30=(a30+a37); - a37=(a33*a30); - a10=(a10+a37); - a42=(a42*a17); - a42=(a42+a46); - a46=(a40*a42); - a10=(a10+a46); - if (res[1]!=0) res[1][25]=a10; - a10=(a13*a11); - a48=(a48*a43); - a10=(a10+a48); - a48=(a49*a28); - a10=(a10+a48); - a48=(a50*a30); - a10=(a10+a48); - a48=(a51*a42); - a10=(a10+a48); - if (res[1]!=0) res[1][26]=a10; - a10=(a44*a11); - a54=(a54*a43); - a10=(a10+a54); - a54=(a55*a28); - a10=(a10+a54); - a54=(a56*a30); - a10=(a10+a54); - a54=(a57*a42); - a10=(a10+a54); - if (res[1]!=0) res[1][27]=a10; - a10=(a52*a11); - a60=(a60*a43); - a10=(a10+a60); - a60=(a61*a28); - a10=(a10+a60); - a60=(a62*a30); - a10=(a10+a60); - a60=(a63*a42); - a10=(a10+a60); - if (res[1]!=0) res[1][28]=a10; - a10=(a58*a11); - a66=(a66*a43); - a10=(a10+a66); - a66=(a67*a28); - a10=(a10+a66); - a66=(a68*a30); - a10=(a10+a66); - a66=(a69*a42); - a10=(a10+a66); - if (res[1]!=0) res[1][29]=a10; - a11=(a64*a11); - a16=(a16*a43); - a11=(a11+a16); - a28=(a23*a28); - a11=(a11+a28); - a30=(a69*a30); - a11=(a11+a30); - a42=(a34*a42); - a11=(a11+a42); - if (res[1]!=0) res[1][30]=a11; - a72=(-a72); - if (res[1]!=0) res[1][31]=a72; - a72=(-a02); - if (res[1]!=0) res[1][32]=a72; - a72=(-a75); - if (res[1]!=0) res[1][33]=a72; - a03=(a03*a06); - a01=(a01*a03); - a08=(a08*a12); - a14=(a14*a08); - a01=(a01+a14); - a27=(a27*a09); - a27=(a27+a31); - a26=(a26*a27); - a01=(a01+a26); - a35=(a35*a17); - a35=(a35+a39); - a33=(a33*a35); - a01=(a01+a33); - a33=casadi_sign(a04); - a33=(a04*a33); - a33=(a33+a19); - a40=(a40*a33); - a01=(a01+a40); - if (res[1]!=0) res[1][34]=a01; - a13=(a13*a03); - a47=(a47*a08); - a13=(a13+a47); - a49=(a49*a27); - a13=(a13+a49); - a50=(a50*a35); - a13=(a13+a50); - a51=(a51*a33); - a13=(a13+a51); - if (res[1]!=0) res[1][35]=a13; - a44=(a44*a03); - a53=(a53*a08); - a44=(a44+a53); - a55=(a55*a27); - a44=(a44+a55); - a56=(a56*a35); - a44=(a44+a56); - a57=(a57*a33); - a44=(a44+a57); - if (res[1]!=0) res[1][36]=a44; - a52=(a52*a03); - a59=(a59*a08); - a52=(a52+a59); - a61=(a61*a27); - a52=(a52+a61); - a62=(a62*a35); - a52=(a52+a62); - a63=(a63*a33); - a52=(a52+a63); - if (res[1]!=0) res[1][37]=a52; - a58=(a58*a03); - a65=(a65*a08); - a58=(a58+a65); - a67=(a67*a27); - a58=(a58+a67); - a68=(a68*a35); - a58=(a58+a68); - a68=(a69*a33); - a58=(a58+a68); - if (res[1]!=0) res[1][38]=a58; - a64=(a64*a03); - a70=(a70*a08); - a64=(a64+a70); - a23=(a23*a27); - a64=(a64+a23); - a69=(a69*a35); - a64=(a64+a69); - a34=(a34*a33); - a64=(a64+a34); - if (res[1]!=0) res[1][39]=a64; - a74=(-a74); - if (res[1]!=0) res[1][40]=a74; - if (res[1]!=0) res[1][41]=a41; - a74=(-a77); - if (res[1]!=0) res[1][42]=a74; - a74=(a71*a02); - a74=(a09*a74); - a71=(a71*a41); - a71=(a04*a71); - a74=(a74-a71); - a74=(-a74); - if (res[1]!=0) res[1][43]=a74; - a74=(a09*a41); - a71=(a04*a02); - a74=(a74+a71); - if (res[1]!=0) res[1][44]=a74; - a74=(a09*a77); - a71=(a04*a75); - a74=(a74-a71); - a74=(-a74); - if (res[1]!=0) res[1][45]=a74; - a74=casadi_sq(a73); - a41=(a41/a74); - a41=(a09*a41); - a02=(a02/a74); - a02=(a04*a02); - a41=(a41+a02); - a41=(-a41); - if (res[1]!=0) res[1][46]=a41; - a75=(a75/a73); - a29=sin(a29); - a75=(a75*a29); - a09=(a09*a75); - a77=(a77/a73); - a77=(a77*a29); - a04=(a04*a77); - a09=(a09+a04); - a09=(-a09); - if (res[1]!=0) res[1][47]=a09; - a09=1.; - if (res[2]!=0) res[2][0]=a09; - if (res[2]!=0) res[2][1]=a09; - if (res[2]!=0) res[2][2]=a09; - if (res[2]!=0) res[2][3]=a09; - if (res[2]!=0) res[2][4]=a09; - if (res[2]!=0) res[2][5]=a09; - if (res[2]!=0) res[2][6]=a09; - if (res[2]!=0) res[2][7]=a09; - if (res[2]!=0) res[2][8]=a09; - return 0; -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_z(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ - return casadi_f0(arg, res, iw, w, mem); -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_z_alloc_mem(void) { - return 0; -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_z_init_mem(int mem) { - return 0; -} - -CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_jac_x_xdot_z_free_mem(int mem) { -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_z_checkout(void) { - return 0; -} - -CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_jac_x_xdot_z_release(int mem) { -} - -CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_jac_x_xdot_z_incref(void) { -} - -CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_jac_x_xdot_z_decref(void) { -} - -CASADI_SYMBOL_EXPORT casadi_int auv_model_impl_dae_fun_jac_x_xdot_z_n_in(void) { return 6;} - -CASADI_SYMBOL_EXPORT casadi_int auv_model_impl_dae_fun_jac_x_xdot_z_n_out(void) { return 4;} - -CASADI_SYMBOL_EXPORT casadi_real auv_model_impl_dae_fun_jac_x_xdot_z_default_in(casadi_int i) { - switch (i) { - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const char* auv_model_impl_dae_fun_jac_x_xdot_z_name_in(casadi_int i) { - switch (i) { - case 0: return "i0"; - case 1: return "i1"; - case 2: return "i2"; - case 3: return "i3"; - case 4: return "i4"; - case 5: return "i5"; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const char* auv_model_impl_dae_fun_jac_x_xdot_z_name_out(casadi_int i) { - switch (i) { - case 0: return "o0"; - case 1: return "o1"; - case 2: return "o2"; - case 3: return "o3"; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const casadi_int* auv_model_impl_dae_fun_jac_x_xdot_z_sparsity_in(casadi_int i) { - switch (i) { - case 0: return casadi_s0; - case 1: return casadi_s0; - case 2: return casadi_s1; - case 3: return casadi_s2; - case 4: return casadi_s3; - case 5: return casadi_s2; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const casadi_int* auv_model_impl_dae_fun_jac_x_xdot_z_sparsity_out(casadi_int i) { - switch (i) { - case 0: return casadi_s0; - case 1: return casadi_s4; - case 2: return casadi_s5; - case 3: return casadi_s6; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_z_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { - if (sz_arg) *sz_arg = 6; - if (sz_res) *sz_res = 4; - if (sz_iw) *sz_iw = 0; - if (sz_w) *sz_w = 0; - return 0; -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_z_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { - if (sz_arg) *sz_arg = 6*sizeof(const casadi_real*); - if (sz_res) *sz_res = 4*sizeof(casadi_real*); - if (sz_iw) *sz_iw = 0*sizeof(casadi_int); - if (sz_w) *sz_w = 0*sizeof(casadi_real); - return 0; -} - - -#ifdef __cplusplus -} /* extern "C" */ -#endif diff --git a/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_z.o b/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_z.o deleted file mode 100644 index 1ffd6fd57b05f22d4aee0d6afbc8d7620d5dfd28..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 20472 zcmcJV4|LSkmB%N75GkAAMA5QUaasoqD(U1OBvr`-5}4L#qXwDLND`8aL?MY}DABSQ zYiPA-jayE&d)SIw+NGrvg$dZ`~Px7=>FrOdyaI#%+Si&nV}W&*jZ5Hi+<5rekioE!dsE| z`yuchKo5x1)iTaQHBO|O@b1XLPtb>-B15v5`7=@bDUbzT{8S;8J<<>KD>Ue3B&z-6 z@!ROHLTxSYAiB_x@{zsBK8n;ve;8&V@{B)|Baxl~C%k>Ysd*C;yU0Wo@}dZIi6)xOO<=M^0oeMt^pN~ugDu5QXki~&Sv{0&vt>a2~{?d6$hQJ@14!#Y8m{G;qm**m5ZoV ztJv3w#QZ*Z^8w>6=&9k+wwF|14%57={ZtD?-Xv5FoAIjjcfcdRi#q2X#4vgvxqBem zNaHZ_8d5JH^`w6tyDD-7t$Bqt(Ga0>095AJfR+9>5GuV^TpKxrM2s98eg#5r43s&M z-y-oAmp}r#L?8K=vJW8Z&(Sg`XFsw|68R0tYZTc{_!ve_%paiiN3iz*Ibts&#wzkJ zWucN9su4$^MM+`ZTmi4MukaeH)R9tku6TnIuTDgdZtoNEgi2AXQ`_Wqw z2tBybzW@_I^x!l8I=rz=Jm9CjlUE8faZxdTj0_@cSqMWOEy6Mzq!sE9h&KA`Y=LS_ z1e)1(FtZoWR!F}OkuLu}cnM4H5gM?+qII_uz!u{IUZ_N2V*$#P`@;jyyc2l(xW167 z3K}*CCZ7si`et< zSPv9#C9hRt{+oI2M6hYrY~~rTpFBs7gkNfUAW-gyRgrC`1Uajcn~oCU9ullTi)=Sg zZYo9UQ=9G<&)Z{$4%?ZUw;vfs3iI+)scF1>krBLK` zS9-sbT@|p^B`s$^NxP}Mw1l?)q)-?qpzvlW2%<$*+*!V%Eqcb7zREA>h-81okz>um zfGDS3a@=n2x%Heyn@)rdL2*>F!-|6KbEOZ!`E|$_-L@mCY@(-K&xgJ=#Lc0*%h`u&%~$T1Nx_0XgLHxf>$iW0|^ z4-g-RnT9dT`>hkfK4s2EHGKXtEyA>@roB9j9X6rqJ*2UqBz8=!nnUujk_^d*=InLd zL#;vmL(2SJIv@Ri(7EQc=*(Ux^T*W-=G+-tDf27>H$;ww?(dFst!|IleQJm|X^*0H z@FR>Mcshd1fbFXxa>vQ*G^$n49X34^rWLgh&mPEhoBqckFbA5dY0zPOVHjqw_OCI; z`813BNMf2nC0|c4Qq7qA9!*i4H0V6$L`WV-Oer1ndnwDz6r8&LKxa*!<%nQdVzHY` z!=cOHi|Wv-cerKPJj?047-1J;N*)0|1x=Bck-$|DbF>0K7@{$M2a<1N>dE30;X$9A zisL@UaWDL*q!|@Xq{oGzco?*krJU>yJ;t^_!w9SqTtAMZi3(+<3)98Pu7Gc&Y~%!4 zY}d48F3jme@e_FdI+HucUSlJlK*st2%hKE~eRt@qG?uO9GD-inFM}2jrAG&kss%kDbV$9q> zQ6_ELc=3Ry^4p9o5O+A`iErlOx-F+M_@y6?I*MHE@c7wNBS{(aM>b7>`Y=b#Jl*ugx)$xS`vqR8ahx<3!bsj34fBP z!3qBwap80sFk=bt9Z<`s>hOC|CnFeWJJq`dhGV-bbH??M7fhdmZv_euMR7(>_Ibjo6MI}T_#EU)FEo;a0a=mv2jMGf??tWEbRL{n zO($0nY__!Gu|jSOoHMVHGHI1V?HG_e&S_rwpy_oyWIS)Fbnax`e4oWt%M2d0k1qV= z<1Lu9*yC5U!FHS|z4Y;5QfQ@&F*A@t_lL-XaVHxyJLsju8lFJto~<1yl^4JHma>|% zTgv8@)!Oe>Q2quMz@u>*Z!eO^`r&u1`iUFQKKL>_#)oP%d(1^yUrw=^=*tXUa)zXb zUTk>Ipw`rNk#kRDU5WYmx9UXqCF(TOpWA8Yt!~e0yyW4N2n}j1D!7qgj_*fb|9#VLff#Qn~1puSs0{lRHirK5=Q=<`tD%=(Lm$nl$48QnHiQiRxNt_3KQ>26Z;Za8h2l3sP{D<~om98;!5L1~>92o1> z{$h-UYQEc5)3^^qqJ}P9n-5woKJD5TDMr%aKsd(Pneed815i}AeE6DAo_LTmE?XmW` z+e`jB-S&pi)=}F=Q?G?qe2nNQ>afs?-H776M9~)!#pt9pTKBdYUs686l3d=$(G=yI z1D@tQPjkMfxxmw0=xHwUv<5t_d7jpMPip}Rp4M=dXk+6Mr?h6&z7cq+#Wd2$38V33 z;z?HyqKTmOldiFN@Dbbd&v-tE=Q2FGc*t4wP$1jfcAPFwHl)&}+nSDymZv!R$Oh|Q zN>kjX$+tR9{?X|{6Yd5cE_4dCZC+Ql{`_}-=#>8Dg_5Ta`5!O+-Ig)Wyy|*VW~z+B;qJZ?gJbF<^OV?^ zBX(abc8&2P4jv)wbJrH@KGKmF*g z8{c|(&vPz+lpYkh^CiC^d*`&)TV5%x>c0Gq;)7+I?7RJioyYDgeWlcNX#Xbjo*tKX z(7dO~dvwMoq9V)p;K&(S zIS-s!mKFH=sPe4hdq;mMYyF53%g-K@RXj5*P?nW5Bg+Tb8Cl*LS(%|vp*YpCPV~_l zMuFHyj+u4MvR+}lyCP0<2Ziafbo-{D<;Ke7*335yD-<@ysNgz6!>}4*)-PlW-6AM_ z)x5ZDmoV}h1(JJInDryECx?-X3$q&x_2(WJ14PV(zre2efP-W00M%kw_otY6ao^3Fzi;sHrp z{d5+k6~0)weO)Eo%3D4|xX%K4&rZQBg;%-cuTR0l!h?6wr#jJHfr zkn^py$MY4d-^G^+uX6FPrQr7ouXD+ND+OOA+>UE&(Z31jyOV`@uMuwNtIhhMmFKsm zc8+9vj9-)dCn8UOiPK?&@J3%!+szbD-I zDJlPl6#PTsc7E9sCsS}fz+t}JX9eXwiVk+*9WFjrc$5r!13x%6;k<`Cf_=Y6nScjE&@#R*(i+2g{aq({okGpuc@O>`(za#vFi?0^m@8bU< zywkvZ#VFbUezqs+G<#NmK)CgfJ+J>%xb>%P$LqrD zL_eR~DUQp6)gf8@8l8fVNWnjqf;-@3f7N+LyFH^{CA`kXZxh}o+@94R5#H&N-zvP@ zCEqW6jf=c~RTK+Q#~(+USy&+S}?I zYCCE>8e3zvcjY#uBURfTNKY(3J+XrH#PZS;D@sqSFg>xUp4`fr`Sdf5emGe~u{{1Q z%Hz1f0gf9v!;85L|K^o&oPYCKavJ~Uvt&M3kzYVE`Gx$;`twcwth2~))>dTn@NW@I z78sqZp@4NxWBCGOypb`v%!^E%`BauE;JOO9t^%&5!02aE$oh*|rjYedWetT!zmZ`w zjms8tErqPVkV{Qv4TYvHM#lVdEz|h7*l6S5V%Ap7dZw|4V%ElL*a9xjyi?-CJ>ED*xbQ{&!6s=#fFqXFB z`bACuC)srEXm6`u+}<3!GhO8g`E)B-+Pe7mwEBYtYvge_U}onpys1#p!Qj zmTy;_zN=gQy5e-UxBQ6W^uIrrpHh5+;$v}sq(JSx2#=M&Kyjbq<%(ad_*}(5tN5*o zPgH!V;`9yJ*82^`CoBGl;xq@X{EroIU_(8?<6{qtx1@cb;9;<&O z&czgn7vizptN1L%Cn-Kn@e;+06faY}Oz~?J4=Nr}obGJao@T{oD!xSVD->U$c!lB* zD^CBiXZ8O;ar)zx<>TZ+M#FBA;uVUwD85YbR>j+7AtpVyDZWxT`Je8>w%*l>)BV%( z&5D0s@z)fetN2@re@^jZiceL1j9k>no=J*dta!2F#fq0JezkCSd!tHTx3^Pq-QFK5 zuI>4$;<~*rD6ZT4s^WfS=Wi9)emJc73?+YtTzttty1f?)cenQnC9m66skrvTQpM?S zS+?K5p}4mH5yf?TA5&c0|AgYYy+2nxPu06c@h>Rur9%UPyS;hB$v=7@DN$VSN3#{z z`{NyohoQ^*;al!ZuJ@6L6xaL6V~Xp2Um6Yy}tcKalM}WLh&g|&o;&NJ>@OMzog`kD}IgQ_M*X!+5itF*YS#do+4=AqJ z&-WGA>)T1i^?1voPDLQU>2WntalQUbQCyGDBE|K1D^pyrSJ`wpK%jAWJ09C#^A&GX z{0ZS?ryl3K6{oGq>N${tUq+iZ0_oBBpy`V1ed=1p^}aSwaqa&W#r6KRTyedBJ)*dt zFaM>u9&gVo9zYvxd$%i|ulNDQ3lx7}alNm7thnAUK1GK%1oE4nM-vp+<9w3hL6os} z<}0rEo#~3}{q$PJ^?r1d;(C1Ernnw&Ur}7|kKa^W@5}oY*W>D-;9std_&mjLR$PzgcEzide3#Ag4T?7^zEtrh#aAdERs2!KZ&my$#TO|4lH%kC=~mC%F1|4X znFz;Se6{cqv{)lpe?B1m92d9$<9CUR)0u!mk&D}ULoUABhs<1E!ecJpFZ@0i zx8ual8*8Wi`}ca6y#4#P`MWj@s2Gv?*%r7zZKQz z#+o`}p4`UzSiL8AetWwocX2BvNIWkWf9zu+!xDK7?Xf)k@lYg@!k-KqS{E&9YJsYj z)>u<6{sntVd#t|Uc1hpbvLv@{acf)C;@F*{vuQzXbTR%?BOUk;zI-T8E*s_26O{DZ zcH&9ej%O=QXJ=Xyf)>bo105a^#>@Ue$8f60t`k=6B;mAn>TofAj2I?J_a<^^ev;0= zw|pMTl1?!fF%xY4bhg&P)^Fc*HlR3-A06t%{+FzvWcsua;z2ya`LCA_zX(1lPyVAm za{KQZFcOhtDqJ<=y`$=`Ozt zWryp({*bAmPYkC1qw|w3Z^!>|byLYWeJn#5Ki0gdznby5%bx}29{W^cA6*U*4!9<^ Zm}4^WOV_I5%J - -#ifndef casadi_real -#define casadi_real double -#endif - -#ifndef casadi_int -#define casadi_int int -#endif - -/* Add prefix to internal symbols */ -#define casadi_f0 CASADI_PREFIX(f0) -#define casadi_fabs CASADI_PREFIX(fabs) -#define casadi_s0 CASADI_PREFIX(s0) -#define casadi_s1 CASADI_PREFIX(s1) -#define casadi_s2 CASADI_PREFIX(s2) -#define casadi_s3 CASADI_PREFIX(s3) -#define casadi_s4 CASADI_PREFIX(s4) -#define casadi_s5 CASADI_PREFIX(s5) -#define casadi_s6 CASADI_PREFIX(s6) -#define casadi_s7 CASADI_PREFIX(s7) -#define casadi_sign CASADI_PREFIX(sign) -#define casadi_sq CASADI_PREFIX(sq) - -/* Symbol visibility in DLLs */ -#ifndef CASADI_SYMBOL_EXPORT - #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) - #if defined(STATIC_LINKED) - #define CASADI_SYMBOL_EXPORT - #else - #define CASADI_SYMBOL_EXPORT __declspec(dllexport) - #endif - #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) - #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default"))) - #else - #define CASADI_SYMBOL_EXPORT - #endif -#endif - -casadi_real casadi_sign(casadi_real x) { return x<0 ? -1 : x>0 ? 1 : x;} - -casadi_real casadi_fabs(casadi_real x) { -/* Pre-c99 compatibility */ -#if __STDC_VERSION__ < 199901L - return x>0 ? x : -x; -#else - return fabs(x); -#endif -} - -casadi_real casadi_sq(casadi_real x) { return x*x;} - -static const casadi_int casadi_s0[3] = {9, 1, 1}; -static const casadi_int casadi_s1[3] = {3, 1, 1}; -static const casadi_int casadi_s2[3] = {0, 1, 1}; -static const casadi_int casadi_s3[3] = {0, 0, 1}; -static const casadi_int casadi_s4[60] = - {9, 9, 0, 6, 12, 18, 25, 34, - 43, 46, 48, 48, 0, 1, 2, 3, - 4, 5, 0, 1, 2, 3, 4, 5, - 0, 1, 2, 3, 4, 5, 0, 1, - 2, 3, 4, 5, 6, 0, 1, 2, - 3, 4, 5, 6, 7, 8, 0, 1, - 2, 3, 4, 5, 6, 7, 8, 6, - 7, 8, 6, 8}; -static const casadi_int casadi_s5[21] = - {9, 9, 0, 1, 2, 3, 4, 5, - 6, 7, 8, 9, 0, 1, 2, 3, - 4, 5, 6, 7, 8}; -static const casadi_int casadi_s6[24] = - {9, 3, 0, 6, 12, 18, 0, 1, - 2, 3, 4, 5, 0, 1, 2, 3, - 4, 5, 0, 1, 2, 3, 4, 5}; -static const casadi_int casadi_s7[3] = {9, 0, 1}; - -/* auv_model_impl_dae_jac_x_xdot_u_z:(i0[9],i1[9],i2[3],i3[0],i4[],i5[0])->(o0[9x9,48nz],o1[9x9,9nz],o2[9x3,18nz],o3[9x0]) */ -static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { - casadi_real a00, a01, a02, a03, a04, a05, a06, a07, a08, a09, a10, a11; - casadi_real a12, a13, a14, a15, a16, a17, a18, a19, a20, a21, a22, a23; - casadi_real a24, a25, a26, a27, a28, a29, a30, a31, a32, a33, a34, a35; - casadi_real a36, a37, a38, a39, a40, a41, a42, a43, a44, a45, a46, a47; - casadi_real a48, a49, a50, a51, a52, a53; - a00=3.3453634479321918e-02; - a01=arg[0]? arg[0][0] : 0; - a02=casadi_sign(a01); - a02=(a01*a02); - a03=23.; - a04=casadi_fabs(a01); - a03=(a03+a04); - a02=(a02+a03); - a03=(a00*a02); - a04=6.0368975005218254e-05; - a05=30.; - a06=arg[0]? arg[0][5] : 0; - a07=(a05*a06); - a08=(a04*a07); - a03=(a03+a08); - a08=-1.1116270017027866e-07; - a09=-30.; - a10=arg[0]? arg[0][4] : 0; - a11=(a09*a10); - a12=(a08*a11); - a03=(a03+a12); - if (res[0]!=0) res[0][0]=a03; - a03=6.0368975005218423e-05; - a12=(a03*a02); - a13=3.3484658136227786e-02; - a14=(a13*a07); - a12=(a12+a14); - a14=-6.1658244361114702e-05; - a15=(a14*a11); - a12=(a12+a15); - if (res[0]!=0) res[0][1]=a12; - a12=-1.1116270017027936e-07; - a15=(a12*a02); - a16=-6.1658244361114783e-05; - a17=(a16*a07); - a15=(a15+a17); - a17=3.3963490377380855e-02; - a18=(a17*a11); - a15=(a15+a18); - if (res[0]!=0) res[0][2]=a15; - a15=9.8084735444364535e-08; - a18=(a15*a02); - a19=5.4404333259807062e-05; - a20=(a19*a07); - a18=(a18+a20); - a20=-2.9967785627100469e-02; - a21=(a20*a11); - a18=(a18+a21); - if (res[0]!=0) res[0][3]=a18; - a18=1.0920100546139210e-05; - a21=(a18*a02); - a22=6.0570157695918657e-03; - a23=(a22*a07); - a21=(a21+a23); - a23=-3.0801331505514781e-03; - a24=(a23*a11); - a21=(a21+a24); - if (res[0]!=0) res[0][4]=a21; - a21=-6.0150572994295635e-03; - a02=(a21*a02); - a24=-3.0184487502609133e-03; - a07=(a24*a07); - a02=(a02+a07); - a07=5.5581350085139340e-06; - a11=(a07*a11); - a02=(a02+a11); - if (res[0]!=0) res[0][5]=a02; - a02=(a09*a06); - a11=(a00*a02); - a25=arg[0]? arg[0][1] : 0; - a26=casadi_sign(a25); - a26=(a25*a26); - a27=46.; - a28=casadi_fabs(a25); - a28=(a27+a28); - a26=(a26+a28); - a28=(a04*a26); - a11=(a11+a28); - a28=arg[0]? arg[0][3] : 0; - a29=(a05*a28); - a30=(a08*a29); - a11=(a11+a30); - if (res[0]!=0) res[0][6]=a11; - a11=(a03*a02); - a30=(a13*a26); - a11=(a11+a30); - a30=(a14*a29); - a11=(a11+a30); - if (res[0]!=0) res[0][7]=a11; - a11=(a12*a02); - a30=(a16*a26); - a11=(a11+a30); - a30=(a17*a29); - a11=(a11+a30); - if (res[0]!=0) res[0][8]=a11; - a11=(a15*a02); - a30=(a19*a26); - a11=(a11+a30); - a30=(a20*a29); - a11=(a11+a30); - if (res[0]!=0) res[0][9]=a11; - a11=(a18*a02); - a30=(a22*a26); - a11=(a11+a30); - a30=(a23*a29); - a11=(a11+a30); - if (res[0]!=0) res[0][10]=a11; - a02=(a21*a02); - a26=(a24*a26); - a02=(a02+a26); - a29=(a07*a29); - a02=(a02+a29); - if (res[0]!=0) res[0][11]=a02; - a02=(a05*a10); - a29=(a00*a02); - a26=(a09*a28); - a11=(a04*a26); - a29=(a29+a11); - a11=arg[0]? arg[0][2] : 0; - a30=casadi_sign(a11); - a30=(a11*a30); - a31=casadi_fabs(a11); - a31=(a27+a31); - a30=(a30+a31); - a31=(a08*a30); - a29=(a29+a31); - if (res[0]!=0) res[0][12]=a29; - a29=(a03*a02); - a31=(a13*a26); - a29=(a29+a31); - a31=(a14*a30); - a29=(a29+a31); - if (res[0]!=0) res[0][13]=a29; - a29=(a12*a02); - a31=(a16*a26); - a29=(a29+a31); - a31=(a17*a30); - a29=(a29+a31); - if (res[0]!=0) res[0][14]=a29; - a29=(a15*a02); - a31=(a19*a26); - a29=(a29+a31); - a31=(a20*a30); - a29=(a29+a31); - if (res[0]!=0) res[0][15]=a29; - a29=(a18*a02); - a31=(a22*a26); - a29=(a29+a31); - a31=(a23*a30); - a29=(a29+a31); - if (res[0]!=0) res[0][16]=a29; - a02=(a21*a02); - a26=(a24*a26); - a02=(a02+a26); - a30=(a07*a30); - a02=(a02+a30); - if (res[0]!=0) res[0][17]=a02; - a02=(a09*a11); - a30=(a04*a02); - a26=(a05*a25); - a29=(a08*a26); - a30=(a30+a29); - a29=9.8084735444363873e-08; - a31=casadi_sign(a28); - a31=(a28*a31); - a32=casadi_fabs(a28); - a32=(a27+a32); - a31=(a31+a32); - a32=(a29*a31); - a30=(a30+a32); - a32=1.0920100546139184e-05; - a33=-3.3399999999999999e+00; - a34=(a33*a06); - a35=6.8000000000000005e-01; - a36=(a35*a06); - a34=(a34+a36); - a36=(a32*a34); - a30=(a30+a36); - a36=-6.0150572994295574e-03; - a37=3.3199999999999998e+00; - a38=(a37*a10); - a39=-6.8000000000000005e-01; - a40=(a39*a10); - a38=(a38+a40); - a40=(a36*a38); - a30=(a30+a40); - if (res[0]!=0) res[0][18]=a30; - a30=(a13*a02); - a40=(a14*a26); - a30=(a30+a40); - a40=5.4404333259807184e-05; - a41=(a40*a31); - a30=(a30+a41); - a41=6.0570157695918683e-03; - a42=(a41*a34); - a30=(a30+a42); - a42=-3.0184487502609172e-03; - a43=(a42*a38); - a30=(a30+a43); - if (res[0]!=0) res[0][19]=a30; - a30=(a16*a02); - a43=(a17*a26); - a30=(a30+a43); - a43=-2.9967785627100754e-02; - a44=(a43*a31); - a30=(a30+a44); - a44=-3.0801331505514837e-03; - a45=(a44*a34); - a30=(a30+a45); - a45=5.5581350085139543e-06; - a46=(a45*a38); - a30=(a30+a46); - if (res[0]!=0) res[0][20]=a30; - a30=(a19*a02); - a46=(a20*a26); - a30=(a30+a46); - a46=1.4970303990827358e+00; - a47=(a46*a31); - a30=(a30+a47); - a47=2.7177645446042520e-03; - a48=(a47*a34); - a30=(a30+a48); - a48=-4.9042367722181902e-06; - a49=(a48*a38); - a30=(a30+a49); - if (res[0]!=0) res[0][21]=a30; - a30=(a22*a02); - a49=(a23*a26); - a30=(a30+a49); - a49=2.7177645446042498e-03; - a50=(a49*a31); - a30=(a30+a50); - a50=3.0257778596593993e-01; - a51=(a50*a34); - a30=(a30+a51); - a51=-5.4600502730695923e-04; - a52=(a51*a38); - a30=(a30+a52); - if (res[0]!=0) res[0][22]=a30; - a02=(a24*a02); - a26=(a07*a26); - a02=(a02+a26); - a26=-4.9042367722181935e-06; - a31=(a26*a31); - a02=(a02+a31); - a34=(a51*a34); - a02=(a02+a34); - a34=3.0075286497147785e-01; - a38=(a34*a38); - a02=(a02+a38); - if (res[0]!=0) res[0][23]=a02; - a02=-1.; - if (res[0]!=0) res[0][24]=a02; - a11=(a05*a11); - a02=(a00*a11); - a38=(a09*a01); - a08=(a08*a38); - a02=(a02+a08); - a08=3.3399999999999999e+00; - a31=(a08*a06); - a30=-3.3199999999999998e+00; - a52=(a30*a06); - a31=(a31+a52); - a52=(a29*a31); - a02=(a02+a52); - a52=casadi_sign(a10); - a52=(a10*a52); - a53=casadi_fabs(a10); - a53=(a27+a53); - a52=(a52+a53); - a53=(a32*a52); - a02=(a02+a53); - a37=(a37*a28); - a39=(a39*a28); - a37=(a37+a39); - a39=(a36*a37); - a02=(a02+a39); - if (res[0]!=0) res[0][25]=a02; - a02=(a03*a11); - a14=(a14*a38); - a02=(a02+a14); - a14=(a40*a31); - a02=(a02+a14); - a14=(a41*a52); - a02=(a02+a14); - a14=(a42*a37); - a02=(a02+a14); - if (res[0]!=0) res[0][26]=a02; - a02=(a12*a11); - a17=(a17*a38); - a02=(a02+a17); - a17=(a43*a31); - a02=(a02+a17); - a17=(a44*a52); - a02=(a02+a17); - a17=(a45*a37); - a02=(a02+a17); - if (res[0]!=0) res[0][27]=a02; - a02=(a15*a11); - a20=(a20*a38); - a02=(a02+a20); - a20=(a46*a31); - a02=(a02+a20); - a20=(a47*a52); - a02=(a02+a20); - a20=(a48*a37); - a02=(a02+a20); - if (res[0]!=0) res[0][28]=a02; - a02=(a18*a11); - a23=(a23*a38); - a02=(a02+a23); - a23=(a49*a31); - a02=(a02+a23); - a23=(a50*a52); - a02=(a02+a23); - a23=(a51*a37); - a02=(a02+a23); - if (res[0]!=0) res[0][29]=a02; - a11=(a21*a11); - a07=(a07*a38); - a11=(a11+a07); - a31=(a26*a31); - a11=(a11+a31); - a52=(a51*a52); - a11=(a11+a52); - a37=(a34*a37); - a11=(a11+a37); - if (res[0]!=0) res[0][30]=a11; - a11=arg[0]? arg[0][6] : 0; - a37=sin(a11); - a52=arg[0]? arg[0][7] : 0; - a31=tan(a52); - a07=(a37*a31); - a07=(-a07); - if (res[0]!=0) res[0][31]=a07; - a11=cos(a11); - a07=(-a11); - if (res[0]!=0) res[0][32]=a07; - a07=cos(a52); - a38=(a37/a07); - a02=(-a38); - if (res[0]!=0) res[0][33]=a02; - a09=(a09*a25); - a00=(a00*a09); - a05=(a05*a01); - a04=(a04*a05); - a00=(a00+a04); - a08=(a08*a10); - a30=(a30*a10); - a08=(a08+a30); - a29=(a29*a08); - a00=(a00+a29); - a33=(a33*a28); - a35=(a35*a28); - a33=(a33+a35); - a32=(a32*a33); - a00=(a00+a32); - a32=casadi_sign(a06); - a32=(a06*a32); - a35=casadi_fabs(a06); - a27=(a27+a35); - a32=(a32+a27); - a36=(a36*a32); - a00=(a00+a36); - if (res[0]!=0) res[0][34]=a00; - a03=(a03*a09); - a13=(a13*a05); - a03=(a03+a13); - a40=(a40*a08); - a03=(a03+a40); - a41=(a41*a33); - a03=(a03+a41); - a42=(a42*a32); - a03=(a03+a42); - if (res[0]!=0) res[0][35]=a03; - a12=(a12*a09); - a16=(a16*a05); - a12=(a12+a16); - a43=(a43*a08); - a12=(a12+a43); - a44=(a44*a33); - a12=(a12+a44); - a45=(a45*a32); - a12=(a12+a45); - if (res[0]!=0) res[0][36]=a12; - a15=(a15*a09); - a19=(a19*a05); - a15=(a15+a19); - a46=(a46*a08); - a15=(a15+a46); - a47=(a47*a33); - a15=(a15+a47); - a48=(a48*a32); - a15=(a15+a48); - if (res[0]!=0) res[0][37]=a15; - a18=(a18*a09); - a22=(a22*a05); - a18=(a18+a22); - a49=(a49*a08); - a18=(a18+a49); - a50=(a50*a33); - a18=(a18+a50); - a50=(a51*a32); - a18=(a18+a50); - if (res[0]!=0) res[0][38]=a18; - a21=(a21*a09); - a24=(a24*a05); - a21=(a21+a24); - a26=(a26*a08); - a21=(a21+a26); - a51=(a51*a33); - a21=(a21+a51); - a34=(a34*a32); - a21=(a21+a34); - if (res[0]!=0) res[0][39]=a21; - a21=(a11*a31); - a21=(-a21); - if (res[0]!=0) res[0][40]=a21; - if (res[0]!=0) res[0][41]=a37; - a21=(a11/a07); - a34=(-a21); - if (res[0]!=0) res[0][42]=a34; - a34=(a31*a11); - a34=(a10*a34); - a31=(a31*a37); - a31=(a06*a31); - a34=(a34-a31); - a34=(-a34); - if (res[0]!=0) res[0][43]=a34; - a34=(a10*a37); - a31=(a06*a11); - a34=(a34+a31); - if (res[0]!=0) res[0][44]=a34; - a34=(a10*a21); - a31=(a06*a38); - a34=(a34-a31); - a34=(-a34); - if (res[0]!=0) res[0][45]=a34; - a34=casadi_sq(a07); - a37=(a37/a34); - a37=(a10*a37); - a11=(a11/a34); - a11=(a06*a11); - a37=(a37+a11); - a37=(-a37); - if (res[0]!=0) res[0][46]=a37; - a38=(a38/a07); - a52=sin(a52); - a38=(a38*a52); - a10=(a10*a38); - a21=(a21/a07); - a21=(a21*a52); - a06=(a06*a21); - a10=(a10+a06); - a10=(-a10); - if (res[0]!=0) res[0][47]=a10; - a10=1.; - if (res[1]!=0) res[1][0]=a10; - if (res[1]!=0) res[1][1]=a10; - if (res[1]!=0) res[1][2]=a10; - if (res[1]!=0) res[1][3]=a10; - if (res[1]!=0) res[1][4]=a10; - if (res[1]!=0) res[1][5]=a10; - if (res[1]!=0) res[1][6]=a10; - if (res[1]!=0) res[1][7]=a10; - if (res[1]!=0) res[1][8]=a10; - a10=-3.3453634479321918e-02; - if (res[2]!=0) res[2][0]=a10; - a10=-6.0368975005218423e-05; - if (res[2]!=0) res[2][1]=a10; - a10=1.1116270017027936e-07; - if (res[2]!=0) res[2][2]=a10; - a10=-9.8084735444364535e-08; - if (res[2]!=0) res[2][3]=a10; - a10=-1.0920100546139210e-05; - if (res[2]!=0) res[2][4]=a10; - a10=6.0150572994295635e-03; - if (res[2]!=0) res[2][5]=a10; - a10=-1.0920100546139184e-05; - if (res[2]!=0) res[2][6]=a10; - a10=-6.0570157695918683e-03; - if (res[2]!=0) res[2][7]=a10; - a10=3.0801331505514837e-03; - if (res[2]!=0) res[2][8]=a10; - a10=-2.7177645446042520e-03; - if (res[2]!=0) res[2][9]=a10; - a10=-3.0257778596593993e-01; - if (res[2]!=0) res[2][10]=a10; - a10=5.4600502730695923e-04; - if (res[2]!=0) res[2][11]=a10; - a06=6.0150572994295574e-03; - if (res[2]!=0) res[2][12]=a06; - a06=3.0184487502609172e-03; - if (res[2]!=0) res[2][13]=a06; - a06=-5.5581350085139543e-06; - if (res[2]!=0) res[2][14]=a06; - a06=4.9042367722181902e-06; - if (res[2]!=0) res[2][15]=a06; - if (res[2]!=0) res[2][16]=a10; - a10=-3.0075286497147785e-01; - if (res[2]!=0) res[2][17]=a10; - return 0; -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_jac_x_xdot_u_z(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ - return casadi_f0(arg, res, iw, w, mem); -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_jac_x_xdot_u_z_alloc_mem(void) { - return 0; -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_jac_x_xdot_u_z_init_mem(int mem) { - return 0; -} - -CASADI_SYMBOL_EXPORT void auv_model_impl_dae_jac_x_xdot_u_z_free_mem(int mem) { -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_jac_x_xdot_u_z_checkout(void) { - return 0; -} - -CASADI_SYMBOL_EXPORT void auv_model_impl_dae_jac_x_xdot_u_z_release(int mem) { -} - -CASADI_SYMBOL_EXPORT void auv_model_impl_dae_jac_x_xdot_u_z_incref(void) { -} - -CASADI_SYMBOL_EXPORT void auv_model_impl_dae_jac_x_xdot_u_z_decref(void) { -} - -CASADI_SYMBOL_EXPORT casadi_int auv_model_impl_dae_jac_x_xdot_u_z_n_in(void) { return 6;} - -CASADI_SYMBOL_EXPORT casadi_int auv_model_impl_dae_jac_x_xdot_u_z_n_out(void) { return 4;} - -CASADI_SYMBOL_EXPORT casadi_real auv_model_impl_dae_jac_x_xdot_u_z_default_in(casadi_int i) { - switch (i) { - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const char* auv_model_impl_dae_jac_x_xdot_u_z_name_in(casadi_int i) { - switch (i) { - case 0: return "i0"; - case 1: return "i1"; - case 2: return "i2"; - case 3: return "i3"; - case 4: return "i4"; - case 5: return "i5"; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const char* auv_model_impl_dae_jac_x_xdot_u_z_name_out(casadi_int i) { - switch (i) { - case 0: return "o0"; - case 1: return "o1"; - case 2: return "o2"; - case 3: return "o3"; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const casadi_int* auv_model_impl_dae_jac_x_xdot_u_z_sparsity_in(casadi_int i) { - switch (i) { - case 0: return casadi_s0; - case 1: return casadi_s0; - case 2: return casadi_s1; - case 3: return casadi_s2; - case 4: return casadi_s3; - case 5: return casadi_s2; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT const casadi_int* auv_model_impl_dae_jac_x_xdot_u_z_sparsity_out(casadi_int i) { - switch (i) { - case 0: return casadi_s4; - case 1: return casadi_s5; - case 2: return casadi_s6; - case 3: return casadi_s7; - default: return 0; - } -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_jac_x_xdot_u_z_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { - if (sz_arg) *sz_arg = 6; - if (sz_res) *sz_res = 4; - if (sz_iw) *sz_iw = 0; - if (sz_w) *sz_w = 0; - return 0; -} - -CASADI_SYMBOL_EXPORT int auv_model_impl_dae_jac_x_xdot_u_z_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { - if (sz_arg) *sz_arg = 6*sizeof(const casadi_real*); - if (sz_res) *sz_res = 4*sizeof(casadi_real*); - if (sz_iw) *sz_iw = 0*sizeof(casadi_int); - if (sz_w) *sz_w = 0*sizeof(casadi_real); - return 0; -} - - -#ifdef __cplusplus -} /* extern "C" */ -#endif diff --git a/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_jac_x_xdot_u_z.o b/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_jac_x_xdot_u_z.o deleted file mode 100644 index 1fde7b8e4e1dfd14abd342bbdf246919a4508b66..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 17736 zcmcJV4Rlr2mB()cG*FuRQe#V7icfvmU@^Xge6=EZfgn#Oc2Wg-Z;T`%$t$TLiR3XN zwHO!B1qH|SV;t?+8LUog*Eq^j*DR`x315=>fe0vQ)zX3#sul|MWBh35zxUnyWphfF z&$e^cy6>LzJNxW&&ffc+d+tqco>_b8X*oHbrW|jYH&jyUc>}Sbe2|TKpYrm(6aIwv zQvT6jr2N92DgUB@8zvn0H$Rzk+z*YS1}T5`rj$SD$&`QfrY-*2F;6Q{=plii{cIKn zCHs=T=``J*Rs1ocQVwuZpie5x?b)4QLu)b4Gl7O(aHY;w$^pW1ze6xS!u1 zi6MuI!&HOpO_3OecNl7q;=UaslOs}>;Es>`iIEWqUc#!@%*3z@_d)zuQ0uR_^OLOQ zLeUmSAE7cS)PI%Q(P^#K5c$s9PsP901!At(6yCS_%7hiK` zeB~~G#VEKBJ)9*Ygy``aKQxhx$OJtjBLc~tR2`TB8O#;UYy9M|>E}iId5(TI(a$gN zllaY$a&DWz=uT3W*mV-{ld3#{FeuzjI{G0YmQQXuUhOBgVkY|)`uXQ#!dq|!gGD_H zy#%2xe*W~33PNHtu3#pR+C-`lh6S4;6{g?_(R3&{K7lp~cqhL;Ld{Wwd~Ku)zmG(E zAcV|KkpzC*BaIq3Es+pLd>CE4f%{gj9LJS$1PNRxb4b)g#z86Cqbp>f0>7#^gl^D$ zC)6M`67Tsd8pg7V8mZCjNF9DP9`>M*mdH4Ww?x8dfGN-YO{&H(+=HKjT{Zrs*)-UnZugU~p*A@{KNx=U7&3{cF{+8s%x8da}BQdUfIHwOGii~%eZFme+F$g*m}p-T7>!cU>@ zbzDI}`ib8|$pB{jJUWV*j!5|)$Ztm7Cb%6d?daR&@lg=!BLBUB>j_#VJ(x|(|MYD3 zeL9jLV`hAfGTCuE35$zIKm%dF5OTZ~Jk1&k168PH1JO!+79-t(k??HL;PsRQZ^}RG zP1Ip_m=y${!7!hu%pqj@aUFzYpawQaOUWnqr%?9uyA zhuA`tBmYBOR}7Cnz6O?qD3+VXLlEG>>fk<-1rZMQI)BANY*=IwM4T?D(Bue_KS50` zFD)p8tp-t+pb8Yy%HZk8*GQZsvAZT(u-_tgQQvXYjEM5e>;p05I>w@1So7 z?2{(jvKLqb^6*NsjHHl%(-@AVg(r`}JGT6mQ%1NC)%*FYXoIF1Jqw+9Rwno5WgWIb-|O{^ifC=Mso zi_hUMJi!{-EuoBqx3PnB)xi+1U2`5*eyCm?1EUtcf5pm!*n5-jqw!6VE`P=Ir*k)x z;biP^6vMPriSIIA=D40)OAoxjvhg(w$?7lS!dgh4IPTZ~j?DHv8sp&#B!ORmMU}wb zFY$%0ccV=|;9lrrA4B+maj92#!M5+1x(}W5yG|LWn#4QR^mG^y-=TE@{Hn2$z|%Jq z5AeTu#6O86PBQD$${z>kHHThar5RFJcSZJ5SFja&g0xPSR(1j#2kn1}Cn30&k1z3+ zZ~80l8p(#98I9k%F>)W8$8UWkvJQ86;$B|*>bZrLFxR~-gD&7!JZNI*df8Ex*vJi0 zc?^{`{0!qDgz?jKrh;p+SJ3(>+>wg=%P{Ez1oYein!t8{(%MK5I#z9@owgrhtS@xj zpSK?fTL!wuwmP?lo^coxtqz{nyCjR;ibyV>VlagsLVjpuaAKqJQbbA-I}Pv4ua1Ba zVlvn^o~QMc7TrO62|2_GLJ6CCC00)1@2Sa66jpQJ0OioaToKq9^j@@&vPbc-XoUaUdGiQ%9s~-kzMWLQ z0*@roKDm>u4}}F=(K5y@>z^Y{^q^(ekn@rskQhDR@$@fz9^%xFV!Qq+Kfkw*+F=Xa zPEy#d>3q=BsKW@hxcHhcq3HmxB??1Kdp5s6VDgy9b12u+gKXZr=$Ov0czsCH%K{xz zB!Fva1D%J*3no^}3uR>&df`=}s{tI1ccSqCRPqCM9v?#a1j}7GYV%hvfs*)|8M$>0r!sBy{B9`-6jcFN6@kQ_Tb8nsGz*i__jgyg%_GomoulmAiv~ z4%DtLPbr)6yiZ+QNs_DEP?o6v$Q36dEw92$lys$HEX_v0a1cjSl12M6g1#lf;W z2$o?s-X7Gd>sx8#y6p#pTDaI1M8`;yzVfc7~>}fpzmA^n1yB}>S*88bm9#7Cd%8j^vn6tyQZ;N_u#a>&9 z*EYp#EA`sSyw0fCS?qO|c%4(w@Nb$%i#8rS@>Is*gcD5|Z~pjh79^6P`BFVufFlq0 zkHbm7?bFpqeV&0U_hZOGG?T1;JO0l?;#^!`!c~NeoIw|*k2C&{D^Ft>u8o~*HZoS8 z@?;}T>OU$|Ud!YwTPFM1GKxsYs)Bn(?E0ml-;UY)t;a8D{^C!j-Ls@G;Im>ICaiq@ zYa7P&-?sU!!us3B#J13F^J`Ud?-SlHaGQQaKW#8L5Dby$9omT$M!6UP0FP(O2z^m4L?t=SH|Mu9b$G-o| zbw6vqyK3UVjN zM2!3IlkxZI_-~ML?|O09#1~GyQ~mXzz?v)g3a<&)o6s)xY`fp2wZ}^c@tr z&rAK3{2djY*Y2*W>z({+<-zJcy?1Zh@rPAayEFDxd#n9*K59?3x7y#eui8)TsrFX; zsJ%pgpY-SY^KtQ)#$%P(?{TqTpV*^H?9nIl(|J{ieXGR2YOlw|Uejd$RWkoRZ`O<% zUkMk^nZLLry*OMvrKq?ldST^aE*38J__7wv_!zB-5Y$9`hEhZZ1D zDQD@GUe1zG&gaLTk=u&}NNvV*jpuz??!8o_sv?A`DBXwfB5*hG%s}9 zY18w@{nLo)dEr|}PR}b`JE}S_`t8$e@+y~~ae3acoDtvq6^ig=dnT<*^Ls(-62fvNbLSo$n^`;2QT?^kHdh5O(Z(f`@`U=1V->q z{O2_zJbh{m)lEb;2Ruvcc6{5v9Fr-E{)J%RMQ{C44SNB%D1haLW7 z;rktapYSN(hbkW!hCd>F!H_8D9u?l`@P6S-9R7sxE{8uYyw~BohT;2!uXE&I9flti z9{6pj(eH%oKA|-sAA)!s{HqT6nL+Zxi0=@P8J* z&f)*cJlBh#G%L@$TjbX}@;?>6)8YRmyx-vu3qR)Yp9|mb@IK-CJknupW1jnG``c3@ zug@cue@?hQ@09Nq9>l3q&wE99jPFDIfbcqp9}(W@@T0=J9R8N@9*4g#yw~A5d;lEn zt#kMp!rgT=mieg6xlw2EDdAey`#9lr{Ih9-aQ6fe5w2$?ZF7NeeNHQn3U}9axp4g^ zpz>3N^G7O$xK9`UdBZ3h2mh2e6g+wc?i3yqc|8Z;F1${-&f_uRjl%gnjQlI$u8m3L z;4pmuF#PZ^{0(p#e@HsfbMrVH{)vYjK0|m^xSoMKgvT8DJB8Od@>_&AI{XdcT@F7B z9npAtgzK4ls_I#`T8;8(fq&xwZY9hB)ms>LU;iJlg6{(k)I(v zDuJcv?QaP0a=6#D_#Yb4wD(ON*L6TBe~p znvGCNHbPUf5h~6`s5~2?(rkpvvJtBAifU&>>4$UWlq;j3FVoMI68bIX`cnQaE@r~K zl$#V!VM+ciF6CNQP|CWBS*9##RmqZpOrW17OIY(1mMme(DJ)sSI?DnXZdJnhDXh69 z_~lkpSX)V;KhPHR$Q{xTR}}|McirQB(0Fa>T^%93TQwv6S=SWj6n zykLUdwv6S=SiYR)%UQmh^_R0uIkzfj{pGBuoHdkls|s#a!SWT{wu0p=xNQZus^C@? z+=_8!(1;aO280zuB!m z_R>uql>O)i4NdLsoy`pkTNh>_+t$&R&QdhBsI@gq(dGrM&DV7QIMXl|vP2H_o zNWu(@T2omFwzOs`*wK)cJv$n*n@US-s%de1I!mQZ3tK;iWOgIy?rK`p-Il&FJH;94 z?3CZox#+qqtWRPM^KVSIc6*+lbLiMtjOzpW0A4E3KjL)OQm*HA;w6Uv2zg3)tb6lu zsXTqBp+vkHm-0;nkw_0cJC*Msh(w$o-O67lh(w$ot;*jeh(w&ebt#`9M>XPf##3Ht z_-72)?{y?kXH=E{p^^Wr;XgC{bA~@;IDIcwJ&ze4HhjSF^9_H_@QC5>7*0M?{lWWs zV225Yf0ibMM7MKssr*F4=`Z@qgZJY=p5ASh>-z{bn`C(K{u;>B->OvpMx%eS;VTWN zziX-d3&P#_IbpbspFyLi+~}cmIwh*M{=L(1>&Lqd52KBa=K;g5fB)NXYo9*Dt>5WC zrzp{QF2beyPZ%CGd=wb~iR7&x^9{H5|AOJ?qK)b=HaueZbi-}DebsR5$N7d^e=RlK z`ggN%y3IHJzF>H>;e$qxwR7+9rgz@_6k$M8zS zCmBxft}0(=xV7^l!@pwWml|&MEH~WR?K_6s=g32bTYElj_{FB*eTIM9@PmfavrOl8 z%mQ8BZkHGyk{4p)^hZ?H6E^${hEFy8i-uorc$MMT7#=gc&G2f&ml{6Z@KuJ- zF#Jx#YYcx;xEohT47YJrl#9klWQUo!)DGo_+c>%0@Jo&S^@h(f{1(G)+}>_@+{pjf z@XHK;$Z+f5&4ye5?lRo^_m76t-==iF`cEHZ2kXbN@_~VP%*c;3-1;$M_;e#5HQf4f zs^Qj;^9=txqvsys$y z@R%9ry@uO3S#P+FlZ}SkI2kb9#>oN0&oljwq|FzJ>^Tva+HIq7@_UQn=g5al;%$bv z3nxAHeIY#zzsvBi8a+QX{O=8a*zh{T`wah@;ZGTUrQv%Gzsm3fhST4G)ec7upJVv@ zhF@)Xgcc8yYlo?ZQ&ZJH%W%pnUuC#`@A$sq_Pypl!{-`3>kX%@>fdI#eQ$c%aQl8V zo(6$Lez*I{G{fzF(r&ojH-0Rf>|@8d!EigyJ%-!3+9=;|NzVveI+m;n0{PtzA1iu(>u}mzDZS70G7j0sGJL#nX_n#o-Cyme`bUa9y(3UkzE1e~0B88U!ov>V zCp_Ztjlv5ZzEil`L;Kx2jDBhEl{$K2QjR;kPRjKTZ!`ANTa4;J!d}`0(0Plx|&;_KI4X(oJ5` z{O)eAXi+B>NW8cR-^^JkV41q+?leB@iy#X0M}+3ig$r9dpsJ%Y-CBfy{kyO`-PC-Y zl)v7wxTtGUXIJZ@^o^pkbwNXF5k7&6WB-VB7L(jPVkfN-w`h^SQ_`Lg z%a9F13gBlAKfQ(J)bX}_D-7)IQ?p!xKWE9bwfJ;()Pt@OFBz%*`)nXun>9e zU+?rTMtO=io5J)F0co;R5oS8{6GQe?_6yUY7%40rk^RU=uKlh6BN5kJb#!<@qGzy8 z+FtMU%so~6_0oR5Gk*2I-WN#wsO<##Mscw2pIR-`;Xa97k4u@#(0#22h=s_zvbTWI zE3-39uUFcKQD>9RTiKn+kWRP#dTD>y>CkQe1GGJr|6=s=g0x@SlmF=4sqJ-or|O&9 z-9aDSkhB!}?D75m|G&SZ zo!3>bURAw%_0H;UZq3fUqEA$mqW$_RBb6|uC{CFX$O&VeO1g49a?Vr|nKoK0+(TM$ znLui(ViPcNv(T-R{@~~2zk2%ULju-PV>y=BCh1LS-me8-OO56FAr>fug|OjTKl z4y`g$&ps$aoXXWwUT^JSwwE+7+md*Y&y2ltq5|_Y zlP-Ow0(2iIVagx%`>c$=9lWjjn}0+%4S0C>Yje(e_6;KS;Y!0ryawPp`?#~CZ3`@x ze&-h`3HE-`cE#o#V2`^*$-Sfh0K5C%sJ|*oOw;XsLbpf9oqx_raVwRq`N>x-UGe2f zw!Y3kMcew>PduSk={q*s1{ITB1-@M8Hm7s&`ASmE>T{G~Co0Y;#TngHdt$Uhv8USm z*dY_T)}DBu(sYM%e;?B2Qe5e{sH=>?H4@ioT-mtjcZEn47t$Pg{~M&^aOFyPJkl$1 zU4`pvToZ6j#5D;Q{oJ@bxUMI_@Vf!`UISc!d$LiX0jJ&iaWfTH39eFHH{mM7MZa>9 zC>2O6aRnq?g>)LO=@OoSbQZ2#aLvJWE3Vsc{SH?huKBpM-|e{hJ+3=&g>c=4s{t4N z?#6WwuKNkV??GID#Pw$a@LP!M5nPWFfFGJsc?{PgTuDYwt40 zH4V?5*7k<`e=?(Mp5AU5_jr8U?`Mr(cjv4})r-CJF3Wp-^{rj^z1@0=_ocXoS+{?9 z;H+gGBUWc74)@;HC;Qc4?8SR04_o%}xR(n5ym`ZiC3`Ra$M0Ww=(|hb`*2_9@GBl` zIzR4<@4x%Q##z>F?bnRD{IvTnTGKEoD(%pDYkV&_Mj_uh?%eG|9nnv8S?ep)rZhk5H^sDc=W}G@acGsqN18-h(!QTe!8f8@FTS~O->9~{yJx*%eQ(30@hg5c`jWN&?OT66 zW@_t{)f*O7T{Pt%$2b1r!2GT`{kqB?tiRBio3P-5nPs1E|J_YL-*`pxE9yNrXTAK` zhmW1|T=uf|q@Yo?Gxnr+={8h)q#-tC> z{5}2(qZH-D9&)Hp^@P9exSsKe7$ke5Z^Yo!6Mk4Pa%cAGnZAE7{J}=C_n`mXmY(@M z(+hq{FZ^%pMbAfj;X~s?PxW2i3;u$bp6U1WV&_YH(evY8_}tZt{U7dy&!2nYliG_u zSM~2%f0}pfxXi!KUie?w3!foiU^g=X{B1AxZ|bGJu0WY{df;a_!tU{C%~)(fANz2JY>3qI6K`_jD8liZVgsh7PMd*0DYy-w|gzN{De>w2M2fj)zJ zXqV4>p)VWQbG@GJMef5`ES;#Fpfs-)LTG(Jzs|k_Z{H%|i$$WWKUv@lHVFjO;9t%u zl76#*b0z*)@dCf3MIbajv~VN2Hd+MWhi2lR3mt>;PYGDVhfWaqwg&`$xJZTPlRmsa0G-BX2K&hgit`#S zuABpT^t+D7ffJNkS`^`Tp-7Y@Y0o^VzsBbg_Wu)<08J?PY4*G-R>-ZD_0sSorT$HF zvG%zrrYzuo3VmqNg&#ovZHJSS{(&_D*7!`Z3Vie5Bk@;CJr${^){gPgPue6OjsH~1 zzl|19_!YuG==bjNf`60A{vXPEwaR+QY^CfZNpI4#Qrf50vc5KH2aQh>2-qK_ z{WX7hPV#A%{Sn=nfBDirc2hsimi;AF+D-F^fQFa+HMtK+{hLhndP}x%?L;BZ4m$ds zF2@1oDFJJGPU3M4#w!!}l_F8zlydV-^}0~nxzW_`MnEyLgI%_x?8Zu8*)N*@E*NY2 z+ofOGO@5Uk$A{FX1-)D9^Qjyk+N9kye6_5XUDiuhSGiBx-zn*}esPlQ7quooyjSYk zYSQy0Y3Ev#zpa=4pqTt2UGsk_P;2*Asi#xwsp<2I>=y+leJ+;zEHUZxH|cN9vRyPk zc}ez{Jd>Wk)%?VSUnKQ+%KoeI3CM9NAm^`KY5(0)pAM;y=09i2d8$d;L95r3+W0K_ zKaTSBJ0$JUEbVZUKr1gvJ(rmLd6V?3W=RP3__smYx#@lZYyJ>_yzs*|$w$+(S?b?r z(toA&hh}Mi&3{gj?b7&+;6Gd>idXt)qx4Tr|Jl<1snX8qR{T4p*|SB!7fbxrQcp$t zr>5t7l23u;qqWO4*C>^~1JVD8m^-7ZX zx1|1dX-~~RN6YcScC{c(l6JTWh9ds81p@CBi85REqej_}K*qm=(w=RyVKskSEBQA| z`+O=JX8%CJzd-UqasK@Ue@U)Q)(gWX|At9Bx0VSw9)3)}ak70=WqWCS9+CW=CjQht zdAybUH9a>;yE&!Z(B1gwJW+M%j*<9Xh2xjZzjyzY0iy zPL=+w(GQdO4wFA*O8*RO7W%oRo^MM20h8bEmi|*K^+)sZ??o7v?eL(0HMxJ3^;%-8 z*I(rLlloL7{UkXac1XX{`g zORIzastM!AlvP&vC-|n6`9)sOIo`qGY{y?>^St%@GP_(eJIw<86x;hBUR6)z) zKvii)u$c7hE?cUI0^UHGuRNbhmj9kF;Lb7yu~#Yg~8IwifY9_1FZnF z>L}fs-e4sfs@PXk7BuKkS{^9#7Ww={-wWDX9q?6Emj-8gOM4}$I85I3$|_^6jS|DG zD{Few%3I;3%yfca_gWg$3Llai8_qrQ-LfNAv zJe*Y`IO^6vjEN!&2z!N{SU0$Vs;1}aM^f}!853D=RWXqrrCvuexru8p=85e7)VHMX zd)Zt3=q1$#2ax2VH_=dpR@6qNqL~%G^3uZUaL4wVT#x!rp%_`TLWSsz!73kmy`fx> z0(s=4w$+N_o(Gk13r1EZP~|VIEDzKK!vm)Q<@FlI8vl$^W4DUrGrcre;w>tz#`qH~ zo#mtPL)U=@BCRWFdZ9|irBlt=qSA70l+{Z!j%<#R+&c|}NB5kmWtCHWW!>=xyY-Y= z9X6A3vJgY4UVtoULSdjbVPtuU?Zw#LTTIRcH|w(%G%oU9*`2V;4-4oj(S0`=k{5eq z58fD&ExJXx$;Hegycx444blGY5~vXZUSVvn4~m981sx*buPE|Y6qfpX5YFZat4}j? zkL)q~z!v4cU{z_MznWzf`wD}VRS^?A&r-e9b+Zi3$C1M2vY^P1ZdEx`&x0AvC~AE( zPByj~&5mkDNe33Vwv-?iOudltfb7L;syBktic+lBde7%Y;bHO%OZ@}du^~t!kejyd$Udi=@A<`g8E3U9zlF07lg68$l_C6OP4 zV-oo>xF(Swy>klH4>weq>~ho&N}P{!Hzi1q&fkbu+X@`j_`=K&KbV@T$UWugwb6Gl zN3D)t;^>9+wnyv1bC0n<@@#zMp}*$n`(pJ`^~9seEK;efT-a$*mah`Bh02;!W_h(JOh550q-{8p`iXNFyLhfkMl|l_=!3- zp8Gc7Pcz_W81VF5w)U$v;1e|@(mM?JMgzXlfT!o=wci2*-mW2$HW~1>2K-_JKEr@t zV!+GL8|N-H;Ij<$%?A8s2K+h$-eq zvFg7(10Eq+{pU8|wU{tv78vl<2ee;_0k6fbDKB8aYki6EGYt4Y6GHxK4R|fKOj&mr z@Yia@NE;1!gqro=0s|f)bp6+4z-uvN%3N%~Bjl_9mKg9)=vYNrYQS3!_+|rMi@j6! zIs+b|Z2i}2z#HEo*<`>Ybg%!~40wdN^`C0MYtI5uW`_Zf5V!u@Z@?qeAO4*k_FFmh za$bx9pAe>2YzF+P27H16|3?GfZor>zz$Y881StI{2d1TCIkL? z1HR3GPch(C1O5U7zQcgO(171>z)vyY6*=$I7?x_l#~APv40xLXe~|&7V8B0Qz}pS@ zu?Bpy0e`UppK8F4<`7YqEq+XCNO&phC&+Zp4kx8&fR64(6ZV4tp5 zSKaEERjhbd+Lu6gb|rm<#Cn!O^d*$j+5SaWS63s$v=r-Xe^-ZT!_e8jLWgM~-`W10 z4$}g@v;9#WriFWF`@K3$3--?T+jN*Fxz6^Rb(j|Do$WpyrVVRn`&Bwj3-ZqPOdX~L zcW3)h9j1kLXZzVYObhJJ_CY#K3+vAIC>^GSb7%YBLt1@l!HoLrFfEi(e;uZUFzT-{yIzxT-0BOX<>`{>o6^7QGXq#jW_DA!?b`!{dJfY zuBg8b)5aV1*I`<~qW(I37Q=fFYW24>{DltF0v7exVOpS~{yIzxQ`BFFX@QCQ>o6@K zQGXq#g(K>(!?a*T{dJfYim1O1(?SsS*I`-!qW(He3p~_chiPGl`s*+)&`^IJriB^m zufwz;L;L@%)t?qzXn!51jXdhF!?eIc{dJfYN~pgM(*g;% z*5P!9eL74J0i*spOa~FDzYbr{@K7Bd!SLBSOa~ySzYf#E3hJ-JqZr=%lU9Ex!(Zrd zCd2RQ@Mwlt=w6Qt%LH#T!JnGo4@~e|CU}hre$52G zWP+bI!H=8ZhfVNl)cz)TmkHixflz= zo^OKZnBW=>b~R+o#J#J*Qboz|{lR!FW&ZG?SjzP8YO|hIi_u3}B~#GnFn!u+N(LWt zg*sd-c8_wc*x%O`wbu3SpJTFa+^qpn9;T|Sj>pN>qaPv&G-L;UnVC`#s}%!xUngP9X1 zUgNrLX9AW~u6l2(tG?1+I>4< zWe2*tD9h;Ln7)e&;JEqqmgV1z+)e1h#E2q)HDN+1i@pk;6ZB_MU6*31kyr8$)3N}dUr0? zeAz>HTQe$A??BP3Wz5)k3ttDkaI|{Dk zf=;EUa;*@mm2=PGUaY12TJfq-5F4&`< z$y&L!S&rj0VaK6N8kebs20fPR_)*pmt#igB)atp8fb@fg_~TKS8YiGG*e`r!h@RIM zdC-jyywsU8J4Vmm^MIm2ml#F8Ugo9hd0!$AMfnU$9VxSi>Dik^_At^}JxgZKkl83V zgEHVft3aoA{XmMvuE)L3U^QiI=ZsuOx{m%Q&?ru?u_|f{W!GC$9}bth%TX&BKg`VL zF{nNw2#<>(yh9Mqk%R=QHG=T7g+`8zg77*?NT8Z42w#dITp$Q9mxKhW=Mo_rWQ1OY zOOTB!j3}b)Xqnw#&mMr|Xc)b}qN@A2dY%XwO+v;%P|>(Rpt@WTrbiGi7KF!3LITx4 z2*Oh$2$u*#l1m&3RLce7zCRo7uv8HCmxKhWSwx8H4bb(!;(peDfTAV~Ca**=X%-xBD82l7Tk3AeL@QY)jUDCmq%6_fn5!EIW`HxFC-y>>aPT0Z3JPP zAbeL65~v>fjt%g)2trj5E|Y`=svii#xeQuqR9>GMRnW!OtmSjSpI#M#p)tSshHrgfkB9d{I%+AxZi$!)G z3M;B2vnS};6Gip}?&V*6OU4mXV3F(BVot2dbltX^X3OiY&&ga4 zDG&8XdFa-MU>;lBH)5z&7sEfTXU&5OVn4FH1UK!^Lz`kg-=OmmwHj?Ww3!b^iifAGA8fPzRZa{dGle)*yOBwXL{!Fj+$wh!c%kWr>5uDUpRl?+^b!o_p*m}VNoIE z<%Bk8g?`L!uuQubA9D+RlN;LZ3hhxRLYmCdaG7;2QLegGF`yj}TJ=O|ojv?e&1Rc0`$_go-GJ?m_F5a7BhsPq)tB!zC@} zFMxtrj<`ZUWrn_M8EXR??sHsbi-^zE9g4Evjd?_AnADZiFeiHGKG%vb`>3aPq9%o_ zGh4FeQvtBJDb>|5gaKFmxMWxTHPBwSM@uwmrL;u%VTJ3Oqg}(-)U<0tGV6Yh3dUtN zOmG-wyu7!o3j$~x50zTR#Bqu$irUfo%T>5mS3whx+N1^tXsma<_F6Q`wNJ(343p4f z7;j$12n`=k+jb8n8T*l_*P%WJ2Qb(x^b;J#Wmv6YdUDl|#VVo5aY=6IYgb`Ox|$2) z!}TtSn!oGTb6xcy(Oua(e*@}_mT~E7J0Ui!oR$FJTIa^Ddz0E{ZhZO%P7@WRu7@){_&z0R&gF4$gJht%D~xY}UbNIMh#IDpzO>hQ5{^$B|v? zZ*dh{E74o<7wf^?s6B8&+{Za`Zd*gcoORIHRO_JMI<12y=c&6oy1H_Q@2Xh`J(7}z z1fgBZokF{`t67Vr>o`f-`35PIHiw|WMWjg?X)?^AUM)1iSkST~R&RgklLzVftWS|a zpCMs=ywGPV^r_5KuQ%wE(F$3pv9wknRv@F06^LC($>7-zI~31&nSp^0>&Y4U5^qyi zf)y--g|kT?H}nazKC=z_M2Gd64t;KgK4bILxAwr>$v*9NoSvyyMmmc;KV4<^9~wWj z!NV0A=IHK2!vpAu8*t7Ehr(GcHU^;|F|c4z`4z3{?5_D6t@Ao?PFg?A;R?-m#Hf#< zI-OQq+BLKYsn2!90La*fk)YE$DCkJWU)07`80Wxfgu-{Dcf)uS90}t?2XaF1Xyenk zhUlxu*ZYb=ih?v_<9VKTTXw}GI-6<_23PrIiVGh z@G@BBgw|mCRlBG>M$Ze8h+N%$+Eg+18mY#3sM{lCH-MmB9AQ|5$(rC%cJ6ZR#|PhZ6T`o7*G7zHj=xajm4C;9^sAZN?%QWiz4}^^MO6$&F-Mdq( zk%i1Ug-FnN#=h`(Fd_5eFd+1!+5uIl)tBl8Po;u`nfH%cIqD3P8p#1|`FtKA?CfV& zqFEkprgD!;cBMKynsC)EK+_V2yQE-(jw!T2dz}S7S)maQ)Hngxg2gC9=_Z+`Qi}sF zK&pX0SrLPH$tYEyQJ$j)iV#?*;Jj%bJa zD3ko4dY~ei`6{GB>FM7XcuO65yrPyN-TR3;#O}VE8XX!!fdP;}RX|Oc{b**tM?C;$ z%LkGz?6?!d8U@HuX|&u^;E>sJMGUQwb3*;pT$Dm1SY8A^C)DCV)1P*JMz#&9IFNyP zyMmYy8)7H_UNfEA02}Dp$QrF@<-!Nl3S@gxDuHMkVv~^zHu~v~dL{C4a+B$%fhE(E zHGGJprh&xAV1lt>kWqKGlWDuutw@*CdQBaGJ7E>FL5qSRjB2iq1~01fs}1^oZhGABdHqTBV_J&Oi#Lw&YDLu?*$gdv3E zsP`cMS~7|-0k9~40aIbV`dqC@=h#`24_iQJ;hQtaiHv zhb@vH`P2erP#ZKN6+ZQk?M9y>Z~6|5QO))|EbC~0iB+On0D9qhJk-An5;O;zAI3n1 z0yNYIXsBO8Lw$QX_L?MaFp9Mg=JTG|M9eC$-cIe@kalYwB^g`bFY5Q(uzIwCA@46H zvlPQ3F@g3K@_ZO;z^O4=^)qOL$>T&88ai4XBNTSkHDkbn%V*E;n(Ib;M)YGGthU35 z7$zHHr;~X02=!8wB|nEBOA6#)tLL-FoCKO{(L*7t=6UKha3zgf7WOqbIVSHv8>Q5* zQDZoAGHP#dJr=u`(#ea2_2G4=-YNK06oSn970TRxa$tZ1%HF(aZNTx%gRLd^@eOge-L&Qe0!G|M6wc6|6<oRJsNp5ECevQ zV^YYYf(Vh6Ih=a?xM=&3iBp9Bpv8{gg7-1#CYmM(N{I(fhb&5rfcsKQBb&&munjc0 zWgI1hOOVCsKG|?@51&?u62t_|`iX>QA`+>kj7EpVkp>RTt@HEfqycq91yNP1QGhCg zLjxRENhMbdR&O%2mW3eyS3x~5C@-5&gDxY;S7iq~l zg;N*SUodHwO)|c9P&T`{l zCiI&?4_&Y0o*>*u;B3GxB-|?C&~7ejGPE12m|HKbMzh*P7>8Ok4{NIIp<8p|)~V#l zGjc+oxk5W~Lu<5g86{J*Xm0)tqb1Iv#)J8l4bjQ!)nAZ)7|BI0vMxauh&Xo`a<8Oe zK+ioxge&I?YZ?uI5VK*usFz?2DLLVklK3cAoFhJJ|Cfc;VzJJuB#Kw)>amD~n(NY+YhgsD&hibEUF z3tfoVMhdBap#o4cIXkpF3k6-pjl2K>_pDH-s5P-yr-6`iAh%7=p-nBbaU&OtnwnLl zuATJ5=`vdc;Y8_~R1q>A+VOz;D^?U;1_0#~XXvR~CB7hxPaync7LPGpZTO6)ql)yL z`njnnsa{VdLO)=Jva46Yk#g#%ah@7Ygc3k*jQZ`T8l_FW6h_uE)zh?$erhEO(OBn3 z6>`B=9r+0tQu{#+iOi`#A*X(loyw{kK`cZk$VoXoAGgm(L2hUG&=t3YeTX`qQ}ZI> zK(mMsnFpk2nn=^NMivdchg1azy@B`6lB3KDZdCPcCV-XOZ#g8^fwcY@y76^dP_#EA zmte7Ap(Rm6nge|ei>p3BjN$ta{}Vv%;Mp2fSgbW`@8$m~;1 z+3!VWAAl8%%wLYo{+LWiyT|QB)DYV?gVg#4=e8_+8L~Y$3`hOu{+3piIw!6JJ|v~p z;eHVEyn%HN#?XfNNyt+hVBHApIKoZ`)_rZ=swm=XZD>10Ds-t$L|nHzAtHEUIGd$) zKD8(5F@Jr{d1!(3oY1?j`cn{AslOE)Ez5V%o+hBj!AdSyLvL_H4_yzqA?+><5$dH| zwAMekIimIT_!71Z8;L!56X(ploY4C5p;pKN^K=wq<_W8Ky*#uzydGu!!fWUDArNJ# zf>>W0@^?lm!1)MUxk3l85qpYT?eH}<9{%d8pQETd*`RY`)N{cY7@K;UxJgiN7BuOq zDln<)J75l)Wc3nY=6^hQw5xE{(2rdWan9TZM`O;4tbmL% zIutxr)7$-0%qM88^y!LXIC`JwSd6G843EGLwk0kEo#NCHtide|oq;m3D5L($fJ9DO z*EAqc1hNiKv)HWnOhnH_V1o#Ls~IM2ybsDGAc3-MVNffyQoodjZ|}JGp4dAH3hl6h{+kg z+B(0oiy&cVJq%Wpi{XALEFT{3SAo+Q48>BE`t^s{L4S{4%!pUf!qj|^qN`)5eFK?{ zR~vDsz6lc>I)v7rL4US^ZIG3;$=UC@{ z=&H}gx!%?YShslZ}6te(PNldTZt8dfx0DoRJnI8FrnwEMu4p#Yes1vSXcJnT66P+QTWpg_!( zamng(4dG-&g8B@(0W8E+E;7;p+Y=Aa@4JL0?!0v9%eh zj|oqQT~rt5=Xp0{A4rx$$FXtU+_AgivV?lSGYf`R0U>WTo!a3TiL}Ii3w7H-*+pQC zqztvqw7U1^*85|Mv(W_f=MxZp3){YnB6#Kt&^r*j5)d2I^FA;|WK{PXjB^qT5WDws z0ocg%Chiiau?8NhDznq8D_=|c>%t)4Z+;3K-*M0YK$%M29w z9CsNzG2ue5L&(bigiQA}IOMDJ=PMZC)8b5PG6nKHI|=nAp-?R4X;DlCyocCG_<#mK zF7kiW;D-hLGb+mT4+{7vzz0bxqIiPQT_WQU+WglfC@5fEkc4$XWg=e}R3u#{x*urBLE0YjE2QRt;rGC`BMN|3NjAZ;oC zBM@m@L<-p+%UZq{Nr^e;!AQFc)aCJI9#!(agmSI!oeuqgIVY`b9gJu&7AQyjm zFcD)c4>a^|+W?K@e*DE|7L`CR@_YuJ^Lea9uXm%m4@x?vxLd$5Gm;0RJ%{Zy#RIq31I0XQUdT5ni~s}jH7F_kg8_ym{hY?*WSpc>!^+^#C4TiWukl zntH)IFve<3cU$2A?sl4Q)&aAMhTym8Pc!}u8fT}!W0J|~=O2HNJvR3Cn`xR)ejnFP zTws#?3BR|cpVyMJ&3_E!?fdbkxDoY?Us{Fr93I#!Kri8&WbKAldW^AaH+1;Pm{hu9 z*DPsY^FuhkL(?QwXE_BwL(UZZiHM)YyQDm~#w$VR6=;8rd7&12K4$KA)B!UM)+lHb z?m5(13)9svH^^R!PJx*bVOJ_aPEXD+Twb`paQyZ@sBj~wnn53%A7D#u0(UHmlGXN$ zpr%~knfH5U*ti9v_3e@J&Gcq?DV-M678B=a#E)0O2AFg!T-CipB9e4+`q5R5wN`pxfpm0PaC(oqR#ux76$ zWy9-r(r*;>qud_!JKMy6FUXAgzDuws5Bh&$%qRPV<$s8*?($vOTlH%>j;!jMzYn+l z=x`n}DcoYHrrY(n%=#Tt&>*322S)So5h8(06;B-a(Q?&Szm{H-ha&CO>cPr zVaK{@?sl>N*hNcb+LSPIo4)_3h@(DiOIlmODLpJ>b5egwTgDiyt79M?W2mi!2H`wF zh_L~+78Sz68nY(4B_`H9TBU8nEl-i2kC2UVZO}4@8*1U^MShb81<^OTKYjsT4t~nf25X>9JFX4D5q=Nu_ge=qa&&NB+>b=( zgh1=yWe$4I3HfbckM#nmw-RMDDAlR!Xy|ZTJ}*bJVlM{$#1s86lXYH{Fv<+7m{^t1 zM)j=8Ezl;5bdohomjb|A#q~7_x3Ta=jz)-K!3nex`k_V0n+|y_Yq6GXS3l-FTT&tC zxh>aliroWDZOfP_Aby-jyduC{(H5|I2`h;AOC{?F`H_7odkeCDqLm9)R_+6Y`52fF z>1HFL_rh!NkO;xwL*FfA2yF23g;r!=%7T!5k_aZKpp#IvQ2drSuojrw^0B|SrQO6G zlX>wLQo@^%{~6`~B=W!0;L9-(p<+Ln;7(}E^fI58sNa%qKng|!G(mWH=L~9%#2A6m zMJ+Ii+9Y6Il!SFr65czW*6Clxl3wm_QB~XZyw$>bNJvgFNM@I?CB7uwRu?T{y_S!g zL`zr~En%)DON1{AVY0t{4=k3}2WCSlD2)`dAu!X&H4;1kv?J(qhE%H{2cJ^e@xTcBvSlrWwL|5<~ z!eOCFE7wlkU~NgQU`Km&;EuK*?ze?O?39z83~nn3fsvH<1cV%)%KNRhSdu08Tl6@o z3;Qj4xDcbzT@D2|>f+VBQsXVxG6z3$N?neK;PR*uhFK>~1@x?L5#DC}XoXzbIl&nS zsM(5(mb2gR;I@f}g}9yMuxn}Dtp!4DUCG6`0Cr@=s z7&l+zLeIpUMbD>-@nz5=t^yBccyvesOB@W~FOA5ydHnVfG($1kySjOwg+)9a!C3Rx zf#GVh8Y0x7rhZMQG%zLifX!q%GGsc@f&kHM5YW~aC4fYU0#Sm`ZEV>nuQda1L7%K{ zK%HInW9Zb;I&T$g=;7%uIX3e~w(lZs=8Sg&0oBA*2i<-~9dXgwRa`UDR$NJ!b164g zB*_o(Yzbui$zf#t`F!tb#dDL$vW#Ppres-$23jg}qAX(_CE_Dr+Jq(z|$>ftj)iIvZjA|xy)1aGpDc^fOK^+(#E8xv|wmX%?{a(PaUXhZWEOmTO^lniTn);Cu z`x}vg6_9%c7&uX*Jg$*lJjPPfU>e> zsWP>-5Rba>DJi@&0ovu`n2}u3hWoqZA`b&88j-ea>X2s9h@fG5D$Bl_Dh^H3#z7h~ zQ|_NCD+@-pB!3Y@J8&O|o(7-g`PS;8L6z4=`Mmz|#Gi|rqgza%Q&=}5$>2O_aigeUO02*^z9@50z&cL}>pUf_^Ncs}Tp)R7nt1Lt z@szO6Q^GpW;Rc>flIMfPnw^B6kDGW(Sm!BW$#c-+#ez@THs)hXDHmzR)8ON(kAGdT zgBd)Cj5aniwUAV;OY&GM`CeG_CV_`U9tXdN2SJ(#n*mS!R0G{Op*c>`2Q4PArj|QJ z;MzsYxM@Uf!}UIB_`M3y@EI32_i#G+mmPl>I+{drUd>`ZqAq<+_c`94!1IL9Jwja+ z=UTKLvwDagOH5fB=XR78`;#9uSat2DwWj3*A2h>EiDfU=apVs39IX(CT5aQ+Ve-tP z^`a;3uTUOcawpI~gOdkm?r{gWk7lEz!bS1)5x}p|#WHRuAy!Yy+agUmS(M!e!R{Yf zjd_%VGx>F4)NO#-{s1bq^529aTjG&EE#?6d# ze}%H1{lwVv7lGm2Lb``KZ79AUm*qY~<|J{C#<5vu6%b3$AzW#!4-ZR|=Q(Pj3&xE7 zNI-|ljRj{~Cu8sE-i~IcSsZVLs7x@ZR=gbo!WgtFp4#LVPEf!3r(qiS8%7PX47UZzRVb2Y@t0*G00(FmDM^><0KyrXzO)V`@zm}+GM&XHZn%79yXd&xO z$YwiSCSaVWy3zK@X%|sJEKX4oEGXP>6OQ?_72R)x6;1ZU>QX_PG)knFv4Ry?dlJv( zdp8B`%4qrHCO<|FCJu5Sii<50%PyGjQwVt{$cK;O| zc11oJCXP1zWMiD+O~xEQKLFja?6T3}0Up7L?WC>%NBRb-E94>zHskdcENk$XwRr6S z;RHH;iU=pj^C~d^Ophn5CySvQDz(?lZn^_ zy)N?g9j1i!jg5pcl|!mg)|?1gWZblC1&TK1ENhEN*0m;C64qr&m=+|lcQdU?=FkY4 zXg*uo2?Av~QY8O`@Xha1E_Mg_6S>u1R8!!q*W$F!OEWE?Q$?GyJAn>=bS(0xcw zjX?!F67=!SS~Ot=&~RgpCSPFmPGkjp47my%C)7{L|cc*R|w$2r*fclV(d^c2E;} zw1eKAP9+8%z);%8IkxygC`LzkG>T9v`mimjKkz8|fIMon#VZ1Vy#ml;Izh!)mpmq< znWnvs-dDo=K;<#C8=IemmsZ=m@QjN%hlg%_{+JxU1>J;dYl9jrR}P3tw|RtqjjSJb zinNe#r&sB44lK`nF-qr)K8UVND-QVj1m4?;2L&e3WakD&K5e*Gw4W9E0<$=eHpdst zu<-=@zY>p#Mh%Z59?=Ga^l;w4Y;xrJ{*xwnu?fD%1pm$i*AT452dKV?5x60`uB%V* z@>|aXifOhsP9xz9F@M$pxCP{)~Ijycn39D@9~Nw$2;tYE#FhTqi7Nv z<#6$iCt=PW;~nnnxNI$i{+Ht&yXDb71_Q)7Xv(D>V^YgR8bL&KgCiaouP{<@+#~6E z&Jo}Xk8@b@dp$&BVH_+{IJd7|1 z;vKl!Ftv6Mb>LhKn<$QT;IJl{h_D`y`+|TL*uyH zvSR{Vqg%{l&JklCIMl+)m_5tuGPB30Pku zN?2dENtl+1k@1exG_jbXMZ6=yBxFDSlrg$+kL5VN7kXYJ?)9bcPux94-VY*AUkdLrl%w$tb&XgGe@!?n zgZ~rp4mY>sVdEW3|ISP1Iq9h9A}!wWlos!RW|%8D-hur*1~ZrX^TrgxS zF1+ELjME4Vf^<@X;g%nTM7(2}qZSCY?MW`i1!!*zaoWxj%U(COhbpc%7E-`cv4c() z!4ljY#PtEtPigTE%numi6*0trZH#&Fq$pytddvew=`lAM^T0UQd&~pp1v2I_k*wD} z=COv>#m1NiO7tG{aR1G4BD4yYfF?5LG5#u^pO*2a0riW|1p>VEmQ_SN`Sw@d}&;*S`k=S^V5gGHaOC)Wh(RP?P)j3ZX zszxh4t{Uy`Cd;~2yD6h{~EH3HUS z9`_44_9l_yc*A44hwpocc}x-+I5z1P^Z5JYh8n%dHNu1_*AcN#|5mErj2zS<@h%{^ z&URE}Be&cV&gvfX7$h{?1fd)Q`GR?cV;;8&IDWH8(c+$@hxtB&9_2ljZ4%#m%tI{i zM9hO%hiEShs^ORi6Tpda{=7(38Eq|J_c}#{hWTyP(N?BQ2m?Yjjjv-9nJ;XeC zUdYGWybu|GA!?2;Wr%r9WRBsOhlFv?B2MciEYJOIX`c$o7_!4Lk5dJXmJhTlPU&yr zDPf(bgms>08+a}d4V^OH#PgtlF)oQ{frNFQ64rTU8+bNJo{x^#>?HJj&csu~I!_5p zp5d6sUgl#o75oMGk2UJ?4@1SKa3#V;+B|5eR3!TFe8T3{RF|UXmO#I&q8x zfikXv^%e!q5Dh_PFzjPnvXhFxMSn0EyJtH}z(U1kY0hQVaVzQA6G2u_;_HO5#_a;a z(~KL;D4w(sYKYPvOR8{Nj)U;?(20)-dYmfpK!h`fN((%flTs6L%`!QiRu>cxJt;<10Fd7)&m}yRL~UgVE$~yh=9ixf;8z$ky<7( zD?Fo8fXZ;#V+dm??D1a@c)SDGo%}O3M)!aRo?7^K10LTzOz!(140x12%FA3nNdC71 z9<5{8$&M88h(hnh5NP=kU81*uNBtPQgwY(Vph2b-?v8*6-B$3ela7QeRpQ=G_tV6E zD&5Z#_fEQ>EAI2?{&(WOfbMS>_W`=UlkaP5F}D0`ah93Xwdw7yaGd2I0)=T_Y^aI_ ztnc_Hu?fNt4N6$w@kyArmyvOn_y}3FtV!$4*1D{aRb`UZDPVmUEn!`jgn3vvkM%Oi zTrUL3`qAbx?FoUh{6m-RN!h~pLWh0gUf*0kV#s@l^$EvW?h`P_i0&5m5u3|9h5r9; zoMo~2Xl7DfmKJBp2;lZeaTc)^#B36dvm{UqK|7zON2)hD$?rMdk}**7w8Nd?6&!Ct zpo7luI)I_HjdN^Ck2(on#_<+VF4E#FNeg5KZEjd{9&Da;C$NCJM_t5GX?sy*yybCx z3$y(PJ>H^6Sm5$fA39G2l;lCTCA}J@Uj2ojMZAeyu+n;RI0$i7_T{>sc<;j%Ff?oyk;cIXz`i@!%?wc;x)(oq6!vXhmY56 zhKGth8D3%O7O#0zYbs=!Zt)tw$cT*B z1SHZBuSqmk?NzO69IshTP!LFxx%{xLHSlMHFlkY{mrbXZ(v3tB`m4WVW zLi31tO|`)NyYZT>^}5d;W4vbcg=EfuC0=8L>Nxy1#A{99-(HrA6O@e`l*Nh{~kHM1G>e?z?HwE5(||G{|8E5GMu?!Om$vibeCat&9_WhcxZ0V=V_N1K8_hNH-g}B!@ zm%Eu>H+(&FKw=nMCelJFg*SrTO74OAlG~@O^5wA)975Tl0*W^o{ zcDNI~;&Abrz30(UWe@QhRc1uSYqkLUUy0W|Ihgba$7?)7(Er8pnrO7mQN(N7&>MP) zmn?Mfar!)JVSb;)1m9tTD^2he6FiaNhGAtto6n|h-oy95Qn9vx0c*+dWDgKoqR(YqukjZC$9TdYBVI~vR(~9gMowZw z4=W77NSllvU&c=gBSWpcm1k=D`U)QD!kj~Ieq*p4gny9j1OfJ@eN#Cd@Ngy%etYRE&dD) ziqMjnxp7Qli$7OXuoXy4B~c@T-T{1%Zkj=1k@D%=JoGupxHdu#fq3hnb5IJOKlLOg zkv7O@B7Te4I%qUas$LgUEP{vy&6}=%%FDl2a1eIM6xs!7ok1}qw4nEyM#rfG* zJj@yZ=0!&>JP0?l`AuZJm91VfM=tSf&{&0tok%eQqa_p6naC6oE4O97ol4>p2{g^4 zJVfD!U+lJNd=Xusg%eoS%`QN(XM^W(O3$Tay& z+AqCN8{3Up^ztZlc55F%pm(6RldIsHccS?Za$Rgmm!m_0ecClhrP*z<+r`=-{&mWv zW^Z6Bd{r5(yN>=qee9FaL>o{zX(VQ9AjlWE@je0&dlghAKjbej!zi|-vp{AYbag83 z<8RV9;?cviM5W$?!gy^yc^mRmc91VnGl3OZ)^ZLPaC7D*;+~Er>8Y_DB<>=C!&94h zgWnqW0R%Zo(1}72F_*8i$E`rKqo?iJM=fpvQGnWyKAMJ?3KlwQVVYv=$^7y;-pF>7 zF$>h++`?@QnCytHdGcEI55T$V`+@_Zt;wJROY6K1%pqB@LT=K7Sg27?#Pj_860#+D zCYyT)He!jbJOIY;LLO>|?-GEJzM8RtZnop585iDf@+95Plx`deCMTW7BY~$4&s^eF zd;Yu)zOMwT*d=gtVEJM2XhFq?g)w~3cs!v0bj2n=k2cjUuFxILXfd)qc;FwcK}|EZfE*ZnBrlG>;eh9cWh=xk z0SDZpRXk~{#Xa5=_rxydd-27G4JZv<2ik$Z03l&K5*-1<-32@Z^!Q*xBh;W8;gt%0 zeF0&1PpqAIp(}K7HC8fb_dBFHsbJX~k^O?mrjJ)FK6pBTk5AR`#2lg)%J-m>KY=80_awe8#s&0v`oukrf9^Lx zco%;+lE1jIFq#c5J_k;b82UV26H>H<*rKTYr*X^i=jj&lYpPHj&5CArqF!_NsAH>{ zVi^}iW(P8}=-FMHW%jAW$`hnD6`B@f1@({Qw~JlkwOx9*m;S|tiWckOyBxI;tPTz` zH{wpOgc6SV+3000plBY&_YzPri(e@gYI3#q6D{uMX&>5JB#YxQ3H8yMutx&y1c;G_ z$K}uZq8#QkF>HK5{fS>e#cKey#1aF`#|g~Z16RVJo{oMeK7l0P0RT2}9gi&d;#UH+ zo9p=3z_v03ghx*yZ~px>9Txikk(6>B|VlhmLba4>byNE zFY>*>&k%UX@FYHEzy|~#vM~N@cqtP{Em+T>xjktfWI-~0mV)Ywmd8Ii0V~i{MnjDy zLFhoAJ;KODuZQ1IM#BQV-{7W~Cz3xxd9gdkSW45M9WP>->(uMv!VGNC*tB`Hbev8&PU{|F%KGpD~teU7_Di2 zP+LGqE7Y*5jaB49zedq@+?QyOM%D!0KcMoc$Gd=YX;n|3R`k&NmCjmdvp`?PlMbBA z?;>J)LB;sZU-6L{?iFgi=v%;}FfDr4v2;}b2Iv5oR#CFhp`sR!bRR-xxHFw0Z zewJFKy&nYfhi&-5?IrMMc@X##--?64SH!&?`dD5Q_ZWtgEi1%5>1kOb?(^t=y|^!+ z`?th>fbQSr`&#N?I0#&dB;o@(;yWX#Y+CC{$l68AaW{ydI@ZTv5NpTCoD)KG+br?% zR5dI=T>6%dL0kLQpZNJ#5Fryr<2QFen9zO+9RO3A<*!IG41DZ_F;HFyF2_}f4&%V z;?bN}1>nFvoLB7}#77bH$X;|_wb=x3GQrDD@bf14ae}q;AbI|C2h1Nn{|SM@bpBHW zg6`)(6M*f0{-d2&EyY0HLw%malEZjjHPZxp2-ftY=ZB5?Qs4Pt9oF|!7|x{cY7lhS zcO7~IE zZ*^r^O|Z1G!dvaHsP;~;3VH*+Dqp$Go#rd6@psFLEOT?f8}yd>%Dv^K70T4I$|=6G z(pf&{SzJ?A=B*C;1Ky$K1gyBk?a9n?OOkYKjzp5IteT9Yo zGC#C~B8mZv+DQ*8fvrj^%ZkX#8lX5aIQ0A*h8F{5rS{^9#7Ww?%;+l#Gz)P++!#ks> zGU%NZ&M{zXl$lljVr8;2X@)YXMky_tF=d8QhN}h_Lw-(bfUE(sMj23981Rx3fhs>) zrY7jO7yC-f{6%LE7@$60iux-_{{BiuWv~PuYp?bP?KAzs3oaNyW14B8;8BT3iZXx2)L@AioP0&T068%? zoVoLgk@0XOG%9K1=V9r}Yk2S?+9S}V9u+_IJ}vPtLF>6%Do*l@=GH zx7drTD$DJ`62HB$rmD(c5iFZ&_lcp_UvxMIXAj6L^ZTm(cKBJ9AM)+g2kjO9>0n(A zR8S7-c62~{psI3OX_4RVV}fZIXexERsK1p5s?X*|hOxLG75W0zHD&&kkv()MY0z$V z5PEodEQr+MG7@VrkiE3pe&xj6T>F%oAXh$YDGUphL2^Fyet#8Qx4Ku09aW(k3?ozR zg&5K8Q~eckP&(T#jdv_|84%gr7_H<$gYmh@du8%qJNk{CMjL-Q8q!}h#9m!mQRoNS zj?Ne?^_4*njFA;UqjT{HMwY-_$D@O0*9fl?bNglXVAV`26|Cf5nN&(FedW}@;X`Ol zOpN}j3W&fc$1_0n1@>$+&R*iHwnvmDqEfQA=;Ne<7#?Uuwv(%phQUgR#mrhZ1kFOt z48JQyT`Rcpf_~H)9w|RKu>9ntoWjWTc>pb1n;Q`s4E(7moP($-NuLPstl4 z%Q`u|albQV%o#hryyUb|5B-ULuZ;TVYaea?eD=o~4;Ib*;?{$YX9#+S6Y(U4F7fj? zwL40qaP`53Ab>(gKNeg>dmJwQwYi&R>MZ8pzU49*+g_4%p+FMeQubkwtF zzIEXrp0JJ@G0XP#fRE<99(_w!`-BOzMn3b!C#lbF-#+r(!e2cx^067q*GamAYlc6& z-SOxs=jgfjwGOr|fB3ZX7x%k;;Hc!kWWAB`&Wurgo$iTAua+#oS@OF=(*5)PkDHz> ze00?Gb#D)Bd-jp#p7U03z3uYS(p^00?Mo}Ke{WP?yFN$}0ii=G%|^7@1N# z($CngMqF=x8G|xMOUDA_A1b4jBOk|;Bnpme6@=*&9H%RI9znsAJ_YyCpw zr`1QJ*UD@4)B3;GueEVN>)+aVp!IW2o>pI3|B+h%)%v^EkF|cU^=GaBoBOr2$0)5o zYyH&RueJWI^%+kzia)M;GX)>vEq&6 zLr?sSeaxt{d3-R^kq+9=$ls{{FXNA}5ZQ<9uK9u1KeYa$^-s;;wei5zU$yyHn^(2@ zQkw@~*UHQO`nv3|+B~bxpUY+cSuXp{DA}L2d3%)XkFU%AI8vjN{cE|_?po^Hi1`s$ z0j>b9T3mU!RCy)5hxrlrF*4Q4X(`#~7ylR5=&65F&avRB57MuvGUh&fBpM?<-TxQn zUuDUcU0qGMT5+}E>cG{E%l(zO?m$|CYlcjXV-(f%7yl9a8?r@D?Rl*D|Bv`jE!9g= zI^=j{-v4O(ppnhm`bJyNY5N^*{S$S1-^(4?B-Vjj?O5#JaNjx(K8LgoX)UFB*!v+3 zxG}CFtws7X(k7&F*jxrYU0p$>P1kmHEkW9vk2L_&Hl#eMPepkcwhifir1lb&!)CN8 zfHeiuB}f+_ZA1DL(%LGlL(tjUkPbm=!y`j3q;{nFNS#OnNDGk8Lz;>Q?3N(S!-IVT z(IFaLYO-H%}X)e-cqy2hxQ|mFeJv)P{5&QajR52!9*&zyX^L zsS~Lk=>(*yNK249kg!Uj9sSOLLB}g4en~|m?ZAF@k zv<+zi(hj6Sq{>66KT;dgg-Gp4mmp0=x*F*cq??c`e@1&EEkLTo0*^EfX$R6|q^S#G z7o<+46Oh&-EkW9dbT(4u1=Js@4JrNe!FHtIAx%XZa{|gCJp*YT(p01cNVAXzkkaS( zYmt^AZA4m&vv^Nz11 z8&qu80!&7Ln_j7z5eXb;aEf3^bFw5`HnKTv5d?8(PnKj$Shfz`DcFL*wRnI`1SrHz zndFa(32jVqK#AN@;6?(_D&&I<3PT4Nx;7sNd&(pJ(;%d%N~JlmD7` zMtXKX`@GNlywCe%-#zWUYFdKpkjo)c&mu06X~=HKnO9MN$P(lXWEt`xWCijNWEJul zr1O2$zX|n#Tnd?hY=KNcUIUqi%s^%!2O!IkyC4&<&CN|h79ppJLp}}J^aJ!W$P8o! zatiW&$Z5z%yaF==xePMtiLUuuxAvZ&&eu92R9C8n23Gxw0N_b67rMiiYPTj7ix(gPc-Z+H` zcR46tYH?cIQFqJndroU}Tkk%7ja#m7NS=O% z+X~j^wm@f<+l)M`+{R7}4Qe*$AbyIQ=H}2PRHCvE;pYx8RlhE`=~}l@`BMeo1O5tW zuBNhK?}3I+DtlG1><%|wKZcUqLrU&spuCgFbF^n}?m~J<3Cp|2&DD2Uc@YJQ!7TC~{^Hymre~G* z;`d>&LtqKaNk&~&oNMY@tZP`k(7ms&%blugXsN@|Dz{j73*>r`F~|t;8(y&Y~g z>-M&}nQIm44J)A;Cf|Vl3hbxJKHC1~Gu*~Dbe0jER&i}XALcLISB#j1wLkh^-Cd|g zZ9}hHH0(~**ONlS;_%M74&_n1WbqKNba;Mj5G#N+fn9`SXT95$g}4vw60omxzj+YW zWlTiIr2a`2j&&*=cd6r>!ul)hQ(TUrjGxdGr(Qu_ib!Y{94orellAEJ_cV04iMton|E1g9 zu7)z^30$6p?yl`~bLXOsjJ65uuWQ@`iEj5m8b8@?w>sGHP+jO8LCvn_C|ciKvjg`B zoKxLLQ8}uIg8?%DPTheLFK~}_MD|R0(QV+MX;wWwik@nb~J5KqB8b@?YG!-VEe$# zb}EBC26ns3sq-EIqqQGijaG^5(Ry_m81*|!#1^9hLzZ3wtNbEcu;&a*??vrF3Z5bRCfA8eCj zVE0*!9)9Kk<*`5nfV98)&a~b$iaI-yAU`N2r_Ur&_ zdT`!vDz6u8F<3v|N{`c?AD*N=_rcyJ_!m5ALY1h_&w;gAtPHjs%xupiU*`5vt%t0`-J(q%2!NhNB&la#_V85>R+y#5| znY!7YlVFV&n+96|X13?kV2-6X3r3&Qn|g=Aj$5n>_P)gyEJl675FV8%R?T4adB4dn z0XqsNaUs1l*by+ZJ$t}1@UL5&Q?VZcn*=l4^A4~>mfjTD(!UF~=VM@LgOOR)|G|gA z&9*Cp?FBR2_z2h(nE2g|{Kvs2!TyY%|K{(X)>?z_pVhjCht3S0Z~gmc7YaP~{a|}; z20Lo89M};svz_*URV=+nzz$n_`@s%btOWLk#ohp;XH>K8j)J{vv5&!K!6Yse(z|zZty$>eilgcQ7CGZR^4CpuMHxW<~$gS0ZNU@~H zAxe+(;!VzPHWrk&z zWsYT$t{uq?4Gv#hYJ zvUFB)`79GGQ!LXgGc2<#b1aK2r&vz2oMBmFS!P*bS!L<8bNMV2EK@AgEHf;#EORW2 zET>pbvz%dBVp(QcVOeGAba44B6D(6K(=6$KyHLuqY`w5fov&$czdF%!!mjBjza^JX&d85VGGC%%b zLH&N_&6fUGz{AS@HtQGP4cd8`dDi0ZG0#}MxnBF9v-lU7J3rI*d$e%2GS4!vGQXdB z^;poKr@_PgKP>it9@Jmhp#7=*Pmm{>mo0uH^Qy&*V&^?=NBn%8`Edr)UavDRvcA;! z7t9mPv)Y_PpJj&CYaKZC2j|0}|F<%)d=TXHcT>_YTm0{tS1tYy^Wyv3j`(>d77%2o z!d&XRl6iu8R%dg*1Rm!9U96uz9`utwJ0$xFi~l3@CX2tzJZ157upS{hS&onR)5<*Y zzq(xU{|}j$SYPV<$ILTUoYBog>-8O_f40-Th%|yJEDp~=-*L`zrrCeVF$-SrEOBHV zDEu{LAFHZF&^~=;Scg@b%zL6=WiIo$@Lwu^u4AtYPQwDN-oYv`Et*==XN_lLeoH;1 z&t)BVHS>&GWzN3?+snNCL{R@;=A{=k@72Qj9`oX`2I}t$IDC!yv}NZL(YMN7h6Ux> zY||8%DegMeotn1t#SP4J>`yoI+av6JgY`@754xX9&oi&=)#vhz|1;)Ohcy2!Eu8al zK=CX-se!Ef+lAktx%yie4*Qr-ar~v+N#==%wLVT&dPX=uN651)y+;rhpO0AIVgDr# zpFvj$)9;Aj^gaUBYntmT&*gVU=+ozBVfJ5&;BT@0Vpi8Pt%dUmbNb)rl$L7YEL()3 z!pElgEI9dD+OO$$zIe0ZrlvmL!TO0e^|{1{-hUuF)h9HUc6ovMl+{1qW}bOL>!Ta1 zbk<^Rr)=4$&#TCOncGGB$#%s#vmxj+s<3!I5W)Ac{jBBBzeMQ2%lZ|ozRTbk`8oAn z?aX%e={n|Fs~^()7^Gj}a;4us!Mx1lLip>78x`flpGWZ1-0(Og6-T+;F_B%R8=23% zq|ar%+!JAk-uoc`r`e9^Kg&FQhqj~sK7zv^ihZu{LVej;xJ27Y@pzH`kW!qL4AI5< zmE9VOoo&K7@Y4SuiLmq2=J5DD6`}up1pn6v{vV3FPI|xgQ|5stELp<(=Q8kwQBOH+ zRr&~?6(4%9h2o#)@%{>DxR?11_fLuEvx=a`pAG*^EU#^FZhnPJT@(!#l2aXvA`q}bv17611$pW=By`rBK~XDt0+ zfF}&Qa(FI+PjM@9JTYym^m*nvtG>g`QxjTG#^cu%=gfw9l=ZXLxcJ)${kK`aWcBBU zbM*IS4AbiH)8J~{as0)fYZSM&9cP&JtCszHnP+Bo9`(684j*Tp-lX}3cz&StPs}S% z>T~HQA22UIq`CBy1O_0gR+BDM9Q~H#DdS>8g#LEccUWKSf0cPDtBX+Yhv4uT=2bR) z6?^dS%o996aP^#DGA}Z}jP-x-T<+)0p{CN+%%{1&(tr9DXC*`IV*T8k8us$FKWCoi z_)9y!!aTwKQ|kL+g#G28(tc*y9~m##Gq1j&3zRs2g?X8|`2QW|P1Zc~y5dHK`0yy} zCtlK^oB4TInp6KQvp-U9C-ah3?oQ?@o+l*^f66?;T>N}R@mha=62U)aJ4G&6;&XAc z_A_mbqt(o(SbrfGHO4%5g9gGMWuDoq`4z1HGV>JM$1+%@cNFInLwpp$m!7BnoMAh1 zzfLi4;&CDI=~mn@%!mD~?{GZD|0(7@5ZkocC0x3mEvsF5HGU+4BHWZs?00gPb5C)-~&nWvuUF? zqTUb1;Y#Myp62SENE}`-`qq4PhvHh-{5{F~GkkxM{`O*o{*PEcXZ0W2HJJQN->$7o zKTk5xTJdrE#|rtvICe(tcd*gS;5O`9g56CDJG}mp(Vf@<$nyq9{e4uh!9^b1!|ZZs zkAM;ElQ57>g==_QvB{6uH#XL{+rxMK$96kgX=ep&fP`%Yki|Gt+W`esqqn1}*%rX~ z+W%+{TlsiiW@Beh&rROy8#c7}ti5i7=OtH!(v-2sfVrCu0x@5TdBMv^W%2TtN9MUA z^2&;+E5=?q6Vcw7S|@NVs21sGP%YBwpjxaNc=^x8si>D<8Bgu^W2lXL@d5|Ec%f5@ zae}852M*oAgcyZ3hi*l@PibCTx zMLsla0rw-iXiz62WVTXh*R+*F!(}Ul2Fq58QMrgf*_J}*23zS$OyH|Gwyp2CTM%orhZD{GKZZsHX!>S&FCOytCfxyi@4g8PQ4ndRXqM zK7ekFb)edja4SZ_cHhPo$(6JvrP_Im_7~K9FZx8)!|U(M_YDktdj02DRXN9OKJwC6 z)s}9?CW1x~SVi@!e!Wc_yxX>eb_AuJ6EW+|+w-esMU0xKrhCutSr}#^EHH5=gb8$0 ztK)DrXey#%OyLMCwZ`wTDZ#I$?%AsvV)s*1To~1hrwY@5il?iNn zFM_&P3wPCwC+$ec-)8UDDlZG`X z`n{;8YDOgd@@f0NsJIyVCy(4jl$AhpL{Gc{HRDDtp8B&O?=>3XQEgWW+#{kRE#+Wu z!>z;HaEHKVwWGWJ#>EsNV@5Oy*=q4`r!pgpHOwH6x3*_JOv;A>SVYlB0zG|OuyhKk zuNAVeDTOOvv$toMxtjh$(;W|R@sf7>^D7#b_y{#^7yl9T< zU~SRXJS6qjmiRzqa4;|mBPwtTi^K`<v2k~*6 z+S1=;JEFJL%)!C>g=~Bq<%JQkngxGwEb67OVJGZz?Mj)o)G#(OM+XsGv+wlhY!wul zp+kaUxUbgvviJ@>?)hce+Hsx={4Ou8>hDBPo3mp(b}SqHPOX>O=sU4$dJ2=&xLzZN zSIh3JUgS(;1;C%y{JP0=O8;m+-b3XnE^K+NAG;ng73p#ClpaFiN7(Z;&z}+wb2)h8 zsWlMTkap!!AX8!7rlfzA$p!P z6RpnUuEuND1l%+{P~_u@h)Nkg({1F$3qng-#%R8PW&c=UyqvtWV8ui9t-?+;o-Eby z0bVnWah#&oTb5BZW<%VxTGaSRCyugzABv-V;+fv^m%D}ZDD~`96N2dO8#!dDzW%K* zaFYu5J6av6b?;rN2*F^KR3dGQlOvHUIV9ft0*GcAg?$^IV4B;hX#YmZ9B)4_&*6b zeAu8(n;blxowmW@`L?OvVf(Ylcc}XLwi*HkCf!#5dUR_dE)R?3&}4~uN#43f_-|61fDv5!cx9`KUm}tXAu8IPvj8t(|3>MT)uxS zGRuXL|D+@SGj?vn0oj)P@_lEK@_lD1U-C=+|A_Oi5`!!GfTeujn)1^-FLwT~fl(Fl zH6NYQK}r3~OyY;IdvIXpPo(t~k!p`UQHGTFp+tTi%*>zb(8?l3PfW0F))sjmUU4&%^dDhAzyYby7^EcMgY7+&tj_m0 zbz+>0owvZ2;79Vy_inQ>^ZzIEQs0&Q@;$X7&R@}P`1=`1xzdggBQL2){;uEA8M>Be zc>veUB>6>Fana20f8VZ2=WmsUVIz`H4^N= zs#wF^13)Y_kJORm|E$F&zbUOMsvJ2k{}H_ZObM?!I)8v4@n5<%y{9O?l5&O7yPRS9y)LcE-9Xe;UHuar{2yICD8>K) diff --git a/control/velocity_controller/scripts/build_auv_solver/main_auv_model.c b/control/velocity_controller/scripts/build_auv_solver/main_auv_model.c deleted file mode 100644 index 800de47b4..000000000 --- a/control/velocity_controller/scripts/build_auv_solver/main_auv_model.c +++ /dev/null @@ -1,189 +0,0 @@ -/* - * Copyright (c) The acados authors. - * - * This file is part of acados. - * - * The 2-Clause BSD License - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE.; - */ - - -// standard -#include -#include -// acados -#include "acados/utils/print.h" -#include "acados/utils/math.h" -#include "acados_c/ocp_nlp_interface.h" -#include "acados_c/external_function_interface.h" -#include "acados_solver_auv_model.h" - -// blasfeo -#include "blasfeo_d_aux_ext_dep.h" - -#define NX AUV_MODEL_NX -#define NP AUV_MODEL_NP -#define NU AUV_MODEL_NU -#define NBX0 AUV_MODEL_NBX0 -#define NP_GLOBAL AUV_MODEL_NP_GLOBAL - - -int main() -{ - - auv_model_solver_capsule *acados_ocp_capsule = auv_model_acados_create_capsule(); - // there is an opportunity to change the number of shooting intervals in C without new code generation - int N = AUV_MODEL_N; - // allocate the array and fill it accordingly - double* new_time_steps = NULL; - int status = auv_model_acados_create_with_discretization(acados_ocp_capsule, N, new_time_steps); - - if (status) - { - printf("auv_model_acados_create() returned status %d. Exiting.\n", status); - exit(1); - } - - ocp_nlp_config *nlp_config = auv_model_acados_get_nlp_config(acados_ocp_capsule); - ocp_nlp_dims *nlp_dims = auv_model_acados_get_nlp_dims(acados_ocp_capsule); - ocp_nlp_in *nlp_in = auv_model_acados_get_nlp_in(acados_ocp_capsule); - ocp_nlp_out *nlp_out = auv_model_acados_get_nlp_out(acados_ocp_capsule); - ocp_nlp_solver *nlp_solver = auv_model_acados_get_nlp_solver(acados_ocp_capsule); - void *nlp_opts = auv_model_acados_get_nlp_opts(acados_ocp_capsule); - // initial condition - double lbx0[NBX0]; - double ubx0[NBX0]; - lbx0[0] = 0; - ubx0[0] = 0; - lbx0[1] = 0; - ubx0[1] = 0; - lbx0[2] = 0; - ubx0[2] = 0; - lbx0[3] = 0; - ubx0[3] = 0; - lbx0[4] = 0; - ubx0[4] = 0; - lbx0[5] = 0; - ubx0[5] = 0; - lbx0[6] = 0; - ubx0[6] = 0; - lbx0[7] = 0; - ubx0[7] = 0; - lbx0[8] = 0; - ubx0[8] = 0; - - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "lbx", lbx0); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "ubx", ubx0); - - // initialization for state values - double x_init[NX]; - x_init[0] = 0.0; - x_init[1] = 0.0; - x_init[2] = 0.0; - x_init[3] = 0.0; - x_init[4] = 0.0; - x_init[5] = 0.0; - x_init[6] = 0.0; - x_init[7] = 0.0; - x_init[8] = 0.0; - - // initial value for control input - double u0[NU]; - u0[0] = 0.0; - u0[1] = 0.0; - u0[2] = 0.0; - - // prepare evaluation - int NTIMINGS = 1; - double min_time = 1e12; - double kkt_norm_inf; - double elapsed_time; - int sqp_iter; - - double xtraj[NX * (N+1)]; - double utraj[NU * N]; - - // solve ocp in loop - for (int ii = 0; ii < NTIMINGS; ii++) - { - // initialize solution - for (int i = 0; i < N; i++) - { - ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, i, "x", x_init); - ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, i, "u", u0); - } - ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, N, "x", x_init); - status = auv_model_acados_solve(acados_ocp_capsule); - ocp_nlp_get(nlp_solver, "time_tot", &elapsed_time); - min_time = MIN(elapsed_time, min_time); - } - - /* print solution and statistics */ - for (int ii = 0; ii <= nlp_dims->N; ii++) - ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "x", &xtraj[ii*NX]); - for (int ii = 0; ii < nlp_dims->N; ii++) - ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "u", &utraj[ii*NU]); - - printf("\n--- xtraj ---\n"); - d_print_exp_tran_mat( NX, N+1, xtraj, NX); - printf("\n--- utraj ---\n"); - d_print_exp_tran_mat( NU, N, utraj, NU ); - // ocp_nlp_out_print(nlp_solver->dims, nlp_out); - - printf("\nsolved ocp %d times, solution printed above\n\n", NTIMINGS); - - if (status == ACADOS_SUCCESS) - { - printf("auv_model_acados_solve(): SUCCESS!\n"); - } - else - { - printf("auv_model_acados_solve() failed with status %d.\n", status); - } - - // get solution - ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "kkt_norm_inf", &kkt_norm_inf); - ocp_nlp_get(nlp_solver, "sqp_iter", &sqp_iter); - - auv_model_acados_print_stats(acados_ocp_capsule); - - printf("\nSolver info:\n"); - printf(" SQP iterations %2d\n minimum time for %d solve %f [ms]\n KKT %e\n", - sqp_iter, NTIMINGS, min_time*1000, kkt_norm_inf); - - - - // free solver - status = auv_model_acados_free(acados_ocp_capsule); - if (status) { - printf("auv_model_acados_free() returned status %d. \n", status); - } - // free solver capsule - status = auv_model_acados_free_capsule(acados_ocp_capsule); - if (status) { - printf("auv_model_acados_free_capsule() returned status %d. \n", status); - } - - return status; -} diff --git a/control/velocity_controller/scripts/build_auv_solver/main_sim_auv_model.c b/control/velocity_controller/scripts/build_auv_solver/main_sim_auv_model.c deleted file mode 100644 index 3b036370b..000000000 --- a/control/velocity_controller/scripts/build_auv_solver/main_sim_auv_model.c +++ /dev/null @@ -1,141 +0,0 @@ -/* - * Copyright (c) The acados authors. - * - * This file is part of acados. - * - * The 2-Clause BSD License - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE.; - */ - - -// standard -#include -#include -// acados -#include "acados/utils/print.h" -#include "acados/utils/math.h" -#include "acados_c/sim_interface.h" -#include "acados_sim_solver_auv_model.h" - -#define NX AUV_MODEL_NX -#define NZ AUV_MODEL_NZ -#define NU AUV_MODEL_NU -#define NP AUV_MODEL_NP - - -int main() -{ - int status = 0; - auv_model_sim_solver_capsule *capsule = auv_model_acados_sim_solver_create_capsule(); - status = auv_model_acados_sim_create(capsule); - - if (status) - { - printf("acados_create() returned status %d. Exiting.\n", status); - exit(1); - } - - sim_config *acados_sim_config = auv_model_acados_get_sim_config(capsule); - sim_in *acados_sim_in = auv_model_acados_get_sim_in(capsule); - sim_out *acados_sim_out = auv_model_acados_get_sim_out(capsule); - void *acados_sim_dims = auv_model_acados_get_sim_dims(capsule); - - // initial condition - double x_current[NX]; - x_current[0] = 0.0; - x_current[1] = 0.0; - x_current[2] = 0.0; - x_current[3] = 0.0; - x_current[4] = 0.0; - x_current[5] = 0.0; - x_current[6] = 0.0; - x_current[7] = 0.0; - x_current[8] = 0.0; - - - x_current[0] = 0; - x_current[1] = 0; - x_current[2] = 0; - x_current[3] = 0; - x_current[4] = 0; - x_current[5] = 0; - x_current[6] = 0; - x_current[7] = 0; - x_current[8] = 0; - - - - - // initial value for control input - double u0[NU]; - u0[0] = 0.0; - u0[1] = 0.0; - u0[2] = 0.0; - - - - - int n_sim_steps = 3; - // solve ocp in loop - for (int ii = 0; ii < n_sim_steps; ii++) - { - // set inputs - sim_in_set(acados_sim_config, acados_sim_dims, - acados_sim_in, "x", x_current); - sim_in_set(acados_sim_config, acados_sim_dims, - acados_sim_in, "u", u0); - - // solve - status = auv_model_acados_sim_solve(capsule); - if (status != ACADOS_SUCCESS) - { - printf("acados_solve() failed with status %d.\n", status); - } - - // get outputs - sim_out_get(acados_sim_config, acados_sim_dims, - acados_sim_out, "x", x_current); - - - - // print solution - printf("\nx_current, %d\n", ii); - for (int jj = 0; jj < NX; jj++) - { - printf("%e\n", x_current[jj]); - } - } - - printf("\nPerformed %d simulation steps with acados integrator successfully.\n\n", n_sim_steps); - - // free solver - status = auv_model_acados_sim_free(capsule); - if (status) { - printf("auv_model_acados_sim_free() returned status %d. \n", status); - } - - auv_model_acados_sim_solver_free_capsule(capsule); - - return status; -} diff --git a/control/velocity_controller/scripts/dynamics.py b/control/velocity_controller/scripts/dynamics.py deleted file mode 100644 index cfd1e220b..000000000 --- a/control/velocity_controller/scripts/dynamics.py +++ /dev/null @@ -1,163 +0,0 @@ - -# auv_model.py -from casadi import SX, vertcat, diag, cos, sin, tan, fabs, inv -import numpy as np -import yaml - -def euler_kinematics_T(phi, theta): - """ - Euler ZYX rate mapping: [phi_dot, theta_dot, psi_dot] = T(phi,theta) * [p q r] - Avoid singularity at cos(theta)=0 with a tiny epsilon if needed (handled later in OCP). - """ - T = SX.zeros(3, 3) - T[0, 0] = 1.0 - T[0, 1] = sin(phi) * tan(theta) - T[0, 2] = cos(phi) * tan(theta) - T[1, 0] = 0.0 - T[1, 1] = cos(phi) - T[1, 2] = -sin(phi) - T[2, 0] = 0.0 - T[2, 1] = sin(phi) / cos(theta) - T[2, 2] = cos(phi) / cos(theta) - return T - -def coriolis_rb_diag(m, Ix, Iy, Iz, u, v, w, p, q, r): - """ - Rigid-body Coriolis/centripetal matrix C_RB for diagonal inertia matrices. - Fossen 2011-style, for 6DOF body velocities [u v w p q r]. - """ - C = SX.zeros(6, 6) - - # Angular velocity block - C[0, 1] = -m * r - C[0, 2] = m * q - C[1, 0] = m * r - C[1, 2] = -m * p - C[2, 0] = -m * q - C[2, 1] = m * p - - # Angular-Angular block (J*omega x omega) - C[3, 4] = Iz * r - C[3, 5] = -Iy * q - C[4, 3] = -Iz * r - C[4, 5] = Ix * p - C[5, 3] = Iy * q - C[5, 4] = -Ix * p - return C - -def dampening(linear_diag, quad_diag, u, v, w, p, q, r): - """ - Linear + quadratic diagonal damping D(v)*v = (Dl + Dq*|v|) v. - Inputs Dl, Dq are 6-vectors (or lists) for [u v w p q r]. - """ - Dl = SX(linear_diag) - Dq_vec = SX(6, 1) - vel = vertcat(u, v, w, p, q, r) - abs_vel = SX(6, 1) - for i in range(6): - abs_vel[i] = fabs(vel[i]) - Dq = SX(quad_diag) # elementwise times abs_vel when applied - # Effective damping operator when multiplying right by vel: - # (Dl + Dq*|vel|) * vel - return Dl, Dq, abs_vel - -def make_auv_model(mass_inertia_matrix,D_lin,D_quad): - """ - Build symbolic CasADi model for a 9x3 AUV suitable for acados codegen. - - params (dict) expected keys: - - m, Ix, Iy, Iz : scalars - - D_lin: length-6 list/tuple : linear damping diag for [u v w p q r] - - D_quad: length-6 list/tuple : quadratic damping diag - - g_vec: length-6 list/tuple : restoring forces/torques in body (optional; use zeros if unknown) - Returns: - dict with keys: x, u, f_expl_expr, name - """ - # Unpack parameters - M=SX(mass_inertia_matrix) - Ix = mass_inertia_matrix[3][3] - Iy = mass_inertia_matrix[4][4] - Iz = mass_inertia_matrix[5][5] - m=M[0][0] - #D_lin = params.get('D_lin') - #D_quad = params.get('D_quad') - - # States: body velocities and Euler angles - u = SX.sym('u') # surge - v = SX.sym('v') # sway - w = SX.sym('w') # heave - p = SX.sym('p') # roll rate NED - q = SX.sym('q') # pitch rate NED - r = SX.sym('r') # yaw rate NED - phi = SX.sym('phi') #Body - theta = SX.sym('theta') #Body - psi = SX.sym('psi') #Body - - x = vertcat(u, v, w, p, q, r, phi, theta, psi) - - # Inputs: surge force, pitch & yaw moments - Fx = SX.sym('Fx') - My = SX.sym('My') - Mz = SX.sym('Mz') - u_in = vertcat(Fx, My, Mz) - - # Inertia (diagonal for now) - #M = diag(SX([m, m, m, Ix, Iy, Iz])) - - # Coriolis matrix for diagonal inertia - C = coriolis_rb_diag(m, Ix, Iy, Iz, u, v, w, p, q, r) - - # Damping (linear + quadratic diagonal) - Dl, Dq, abs_vel = dampening(D_lin, D_quad, u, v, w, p, q, r) - #TODO: blend the dampening functions - # Generalized input vector tau: map [Fx, My, Mz] to 6x1 wrench - # tau = [Fx, 0, 0, 0, My, Mz]^T - tau = vertcat(Fx, 0.0, 0.0, 0.0, My, Mz) - - - # 6DOF body dynamics: nu_dot = M^{-1} (tau - C*nu - (Dl + Dq*|nu|) nu - g) - nu = vertcat(u, v, w, p, q, r) - D_eff_times_nu = (Dl + diag(Dq @ abs_vel)) @ nu # elementwise: (Dq*|nu|) * nu - rhs_nu = SX(6, 1) - rhs_nu = inv(M) @ (tau - C @ nu - D_eff_times_nu) # - print(D_eff_times_nu) - # Kinematics: eta_dot = T(eta) * omega - T = euler_kinematics_T(phi, theta) - eta_dot = T @ vertcat(p, q, r) - - xdot = vertcat(rhs_nu, eta_dot) - xdot_sym = SX.sym("xdot", x.size()[0]) - model = { - "name": "auv_model", - "x": x, - "xdot": xdot_sym, - "u": u_in, - "f_expl_expr": xdot, # explicit ODE f(x,u) - # implicit form (optional in acados): f_impl = xdot - f(x,u) - "f_impl_expr": xdot_sym - xdot - } - return model - -if __name__ == "__main__": - # Quick smoke test - m=[[6,0,0,0,0,0,], - [0,5,0,0,0,0,], - [0,0,4,0,0,0,], - [0,0,0,3,0,0,], - [0,0,0,0,5,0,], - [0,0,0,0,0,9]] - D_lin=[[6,0,0,0,0,0,], - [0,5,0,0,0,0,], - [0,0,4,0,0,0,], - [0,0,0,3,0,0,], - [0,0,0,0,5,0,], - [0,0,0,0,0,9]] - D_quad=[[6,0,0,0,0,0,], - [0,5,0,0,0,0,], - [0,0,4,0,0,0,], - [0,0,0,3,0,0,], - [0,0,0,0,5,0,], - [0,0,0,0,0,9]] - mdl = make_auv_model(m,D_lin,D_quad) - print("Model:", mdl["name"]) - print("x dim:", mdl["x"].numel(), "u dim:", mdl["u"].numel()) diff --git a/control/velocity_controller/scripts/generator.py b/control/velocity_controller/scripts/generator.py deleted file mode 100644 index cbd2ee512..000000000 --- a/control/velocity_controller/scripts/generator.py +++ /dev/null @@ -1,223 +0,0 @@ - -#!/usr/bin/env python3 -""" -AUV NMPC OCP generator for acados -N = 20, Tf = 0.05 (2.5 ms steps) -Nonlinear thrust magnitude constraint. -Diagonal Q/R/Qe loaded from weights.yaml or defaults. - -Generates a solver in ./build_auv_solver/ -""" - -import numpy as np -import yaml -from pathlib import Path -import scipy.linalg - -from acados_template import AcadosOcp, AcadosOcpSolver, AcadosModel -from casadi import SX, vertcat - -# Import the underwater vehicle model from Step 1 -from dynamics import make_auv_model - - -# ----------------------------- -# Load weighting matrices -# ----------------------------- -def load_matrices(path="../config/parameters.yaml"): - if Path(path).exists(): - with open(path, "r") as f: - data = yaml.safe_load(f) - node_key = next(iter(data.keys())) - print("Top-level keys:", list(data.keys())) - print("[INFO] Loading weights from", path) - Q = np.diag(data[node_key]["ros__parameters"]["NMPC_params"]["Q"]) - R = np.diag(data[node_key]["ros__parameters"]["NMPC_params"]["R"]) - Qe = np.diag(data[node_key]["ros__parameters"]["NMPC_params"]["Q"]) - inertia_M=np.reshape(data[node_key]["ros__parameters"]["inertia_matrix"],(6,6)) - D_lin = np.reshape(data[node_key]["ros__parameters"]["dampening_matrix_low"],(6,6)) - D_quad = np.reshape(data[node_key]["ros__parameters"]["dampening_matrix_high"],(6,6)) - N=data[node_key]["ros__parameters"]["NMPC_params"]["N"] - delta_t=data[node_key]["ros__parameters"]["publish_rate"] - max_force=data[node_key]["ros__parameters"]["max_force"] - else: - print("[ERROR], yaml file not found") - - return Q, R, Qe, inertia_M, D_lin, D_quad,N,delta_t,max_force - - -# ----------------------------- -# Build the OCP -# ----------------------------- -def create_auv_ocp(): - # Load weights - Q, R, Qe, inertia_Matrix, D_lin, D_quad, N, delta_t, max_force = load_matrices() - - # Load dynamical model - mdl = make_auv_model(inertia_Matrix, D_lin, D_quad) - - # Wrap into acados model format - acados_model = AcadosModel() - acados_model.name = mdl["name"] - acados_model.x = mdl["x"] - acados_model.xdot=mdl["xdot"] - acados_model.u = mdl["u"] - acados_model.f_expl_expr = mdl["f_expl_expr"] - acados_model.f_impl_expr = mdl["f_impl_expr"] - - # Create OCP - ocp = AcadosOcp() - ocp.model = acados_model - - # Horizon settings - Tf = (delta_t*N)/1000 # total horizon [seconds] - ocp.dims.N = N - ocp.solver_options.tf = Tf - - ocp.solver_options.integrator_type="IRK" - ocp.solver_options.sim_method_num_stages=2 - ocp.solver_options.sim_method_num_steps=4 - - nx = acados_model.x.size()[0] - nu = acados_model.u.size()[0] - - # ---------------------------------- - # Cost: LINEAR_LS (yref-based) - # ---------------------------------- - # ---------------------------------- - ocp.cost.cost_type = "LINEAR_LS" - ocp.cost.cost_type_e = "LINEAR_LS" - - # States you care about: u=0, q=4, r=5 - idx_states = [0, 4, 5, 7, 8] - idx_controls = [0, 1, 2] - - n_y = len(idx_states) + len(idx_controls) # 8 - n_ye = len(idx_states) # 5 - - # Vx: (8, nx) — selects only u, q, r from state vector - Vx = np.zeros((n_y, nx)) - for i, idx in enumerate(idx_states): - Vx[i, idx] = 1.0 - - # Vu: (8, nu) — selects all 3 controls, placed in lower block - Vu = np.zeros((n_y, nu)) - for i, idx in enumerate(idx_controls): - Vu[len(idx_states) + i, idx] = 1.0 - - # W: (8, 8) — only track what you care about, well conditioned - Q_tracked = np.diag([ - Q[0, 0], # u - Q[1, 1], # q - Q[2, 2], # r - Q[3, 3], # theta - Q[4, 4], # psi -]) - R_tracked = np.diag([R[0,0], R[1,1], R[2,2]]) # weights for controls - - W = scipy.linalg.block_diag(Q_tracked, R_tracked) - - ocp.cost.Vx = Vx # (8, nx) - ocp.cost.Vu = Vu # (8, nu) - ocp.cost.W = W # (8, 8) - - # Terminal cost — same state selection, no controls - Vx_e = np.zeros((n_ye, nx)) - for i, idx in enumerate(idx_states): - Vx_e[i, idx] = 1.0 - - Q_e_tracked = np.diag([Qe[0,0], Qe[1,1], Qe[2,2], Qe[3,3],Qe[4,4]]) - - ocp.cost.Vx_e = Vx_e # (5, nx) - ocp.cost.W_e = Q_e_tracked # (5, 5) - - # References must match ny=6 and ny_e=3 - ocp.cost.yref = np.zeros(n_y) # [u, q, r, theta, psi, u1, u2, u3] - ocp.cost.yref_e = np.zeros(n_ye) # [u, q, r, theta, psi] - - # ---------------------------------- - # Nonlinear input constraint: - # Fx^2 + My^2 + Mz^2 <= 10000 - # ---------------------------------- - #Fx, My, Mz = acados_model.u[0], acados_model.u[1], acados_model.u[2] - #h_expr = Fx**2 + My**2 + Mz**2 - - #ocp.model.con_h_expr = vertcat(h_expr) # 1 constraint - #ocp.dims.nh=1 - #ocp.constraints.lh = np.array([0.0]) # lower bound: h >= 0 (redundant) - #ocp.constraints.uh = np.array([max_force**2]) # upper bound: magnitude <= 100 - - # No bounds on slack variables (we don't use slacks) - #ocp.constraints.idxsh = np.array([], dtype=int) - #ocp.constraints.lsh = np.array([]) - #ocp.constraints.ush = np.array([]) - - # No box-constraints on h (handled via lh/uh) - #ocp.constraints.idxbh = np.array([], dtype=int) - #ocp.constraints.lbh = np.array([]) - #ocp.constraints.ubh = np.array([]) - - u_max = max_force # from YAML - - ocp.constraints.lbu = -u_max * np.ones(nu) - ocp.constraints.ubu = u_max * np.ones(nu) - ocp.constraints.idxbu = np.arange(nu, dtype=int) - ocp.constraints.idxbx = np.array([7]) - ocp.constraints.lbx = -1.4 - ocp.constraints.ubx = 1.4 - ocp.dims.nbx = 1 - - # ---------------------------------- - # Initial state constraint (must be updated before solve) - # ---------------------------------- - ocp.constraints.x0 = np.zeros(nx) - - # ---------------------------------- - # Solver options - # ---------------------------------- - print("W shape:", ocp.cost.W.shape) - print("W diagonal:", np.diag(ocp.cost.W)) - print("W_e shape:", ocp.cost.W_e.shape) - print("W_e diagonal:", np.diag(ocp.cost.W_e)) - print("Vx shape:", ocp.cost.Vx.shape) - print("Vx_e shape:", ocp.cost.Vx_e.shape) - print("yref:", ocp.cost.yref) - print("yref_e:", ocp.cost.yref_e) - print("ny:", ocp.dims.ny) - print("ny_e:", ocp.dims.ny_e) - print("VX",ocp.cost.Vx) - print("VU", ocp.cost.Vu) - ocp.solver_options.qp_solver = "FULL_CONDENSING_HPIPM" - ocp.solver_options.qp_solver_warm_start=1 - ocp.solver_options.hessian_approx = "GAUSS_NEWTON" - #ocp.solver_options.integrator_type = "ERK" - ocp.solver_options.nlp_solver_type = "SQP" # fast real-time iteration - #ocp.solver_options.nlp_solver_max_iter = 100 - - #ocp.solver_options.globalization = 'MERIT_BACKTRACKING' - ocp.solver_options.levenberg_marquardt = 1e-2 - ocp.solver_options.print_level = 2 - - # ---------------------------------- - # Output directory - # ---------------------------------- - ocp.code_gen_opts.code_export_directory = "build_auv_solver" - - print("nh:", ocp.dims.nh) - print("lh:", ocp.constraints.lh) - print("uh:", ocp.constraints.uh) - - #print("idxbh:", ocp.constraints.idxbh) - print("idxsh:", ocp.constraints.idxsh) - - return ocp - - -# ----------------------------- -# Main entry: generate solver -# ----------------------------- -if __name__ == "__main__": - ocp = create_auv_ocp() - print("[INFO] Generating AUV NMPC solver...") - AcadosOcpSolver(ocp, json_file="auv_ocp.json") - print("[INFO] Done. Solver code is in ./build_auv_solver/") diff --git a/control/velocity_controller/src/LQR_setup.cpp b/control/velocity_controller/src/LQR_setup.cpp index 0de30a5ff..f6314cac6 100644 --- a/control/velocity_controller/src/LQR_setup.cpp +++ b/control/velocity_controller/src/LQR_setup.cpp @@ -1,4 +1,3 @@ - #include "velocity_controller/LQR_setup.hpp" #include "rclcpp/rclcpp.hpp" #include @@ -17,22 +16,16 @@ //#include #include "vortex/utils/math.hpp" #include "ct/optcon/lqr/LQR.hpp" -#include "velocity_controller/NMPC_setup.hpp" - -//Eigen::IOFormat fmt(Eigen::StreamPrecision, 0, ", ", "\n", "[", "]"); -LQRController::LQRController() -{ +LQRController::LQRController(){ Q.setZero(); R.setZero(); B.setZero(); - D_low.setZero(); - D_high.setZero(); + D.setZero(); inertia_matrix_inv.setZero(); }; -bool LQRController::set_matrices(std::vector Q_,std::vector R_,std::vector inertia_matrix_,double max_force_, std::vector water_r_low,std::vector water_r_high){ - //Possible error handling here to check for size and allowed values. +bool LQRController::set_matrices(std::vector Q_,std::vector R_,std::vector inertia_matrix_,double max_force_, std::vector D_low){ if (Q_.size()!=8){ RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The Q matrix has the wrong amount of elements"); return 0; @@ -45,29 +38,21 @@ bool LQRController::set_matrices(std::vector Q_,std::vector R_,s RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The M matrix has the wrong amount of elements"); return 0; } - if(water_r_low.size()!=36||water_r_high.size()!=36){ - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The D matrix has the wrong amount of elements"); - return 0; - } if (max_force_<0){ RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The max_force need to be >0"); return 0; } max_force=max_force_; - // Ensure full matrices are zeroed before assigning diagonals Q.diagonal() = Eigen::Map(Q_.data(), Q_.size()); R.diagonal() = Eigen::Map(R_.data(), R_.size()); Ixx=inertia_matrix_.at(6*3+3); Iyy=inertia_matrix_.at(4*6+4); Izz=inertia_matrix_.at(5*6+5); mass=inertia_matrix_.at(0); Eigen::Matrix inertia_matrix = Eigen::Map>(inertia_matrix_.data(),6,6); - D_low=Eigen::Map>(water_r_low.data(),6,6); - D_high=Eigen::Map>(water_r_high.data(),6,6); + D=Eigen::Map>(D_low.data(),6,6); inertia_matrix_inv=inertia_matrix.inverse(); Eigen::MatrixB_t=inertia_matrix_inv*(Eigen::Matrix()<<1,0,0, 0,0,0, 0,0,0, 0,0,0, 0,1,0, 0,0,1).finished(); - B.setZero(); - Eigen::Matrix B_m; - B_m.setZero(); + Eigen::Matrix B_m=Eigen::Matrix::Zero(); B_m.block<6,3>(0,0)=B_t; std::vector> swaplines{{1,7},{2,8},{3,4},{4,5}}; for (long unsigned int i=0;i Q_,std::vector R_,s return 1; } -//Can be optimized std::tuple LQRController::saturate (double value, bool windup, double limit){ if (abs(value) > limit){ windup=true; @@ -102,28 +86,17 @@ double LQRController::anti_windup(double error, double integral_sum, bool windup Eigen::Matrix LQRController::linearize(State s){ - //Eigen::Matrix A; - Eigen::Matrix D=Eigen::Matrix::Zero(); + Eigen::Matrix D_=Eigen::Matrix::Zero(); - if (s.surge<100){ //Threshold tbd - D=-inertia_matrix_inv*D_low; - } - else { - D=-inertia_matrix_inv*D_high; - } + D_=-inertia_matrix_inv*D; //Assuming linear dampening for now + Eigen::Matrix C=coriolis(s); - /* //Need to decide if i want to learize around 0? - C(1,5)=-mass*s.surge; - C(2,4)=mass*s.surge; - */ - D-=inertia_matrix_inv*C; //To avoid unneccessary allocation + + D_-=inertia_matrix_inv*C; //To avoid unneccessary allocation Eigen::Matrix T=Eigen::Matrix::Identity(); - /*T<<1,sin(s.yaw)*tan(s.pitch),cos(s.yaw)*tan(s.pitch), - 0,cos(s.yaw),-sin(s.yaw), - 0,sin(s.yaw)/cos(s.pitch),cos(s.yaw)/cos(s.pitch);*/ Eigen::Matrix A; - A.block<6,6>(0,0)=D; + A.block<6,6>(0,0)=D_; A.block<3,3>(0,6)=A.block<3,3>(6,0)=A.block<3,3>(6,6)=Eigen::Matrix3d::Zero(); A.block<3,3>(6,3)=T; std::vector> swaplines{{1,7},{2,8},{3,4},{4,5}}; @@ -161,13 +134,8 @@ Eigen::Vector LQRController::saturate_input(Eigen::Vector u) bool LQRController::calculate_thrust(State state, Guidance_data error){ ct::optcon::LQR<8,3> lqr; Eigen::Matrix K_l; - - //TODO: consider making my own solver using eigen so that it does not need controll_toolbox library bool INFO= lqr.compute(Q,R,linearize(state),B,K_l,true,false); - if(INFO==0){ - return false; - } - + if(INFO==0)return false; Eigen::Matrix state_error = update_error(error, state); u=saturate_input( (K_l*state_error)); return true; @@ -197,54 +165,17 @@ bool LQRController::set_interval(double interval){ Eigen::Vector LQRController::get_thrust(){ return u; } -//Hjelpefunksjoner for å konvertere mellom std::vector og Eigen::Matrix3d -/*/ -Eigen::Matrix3d vector_to_matrix3d(const std::vector &other_matrix){ - Eigen::Matrix3d mat; - for (int i = 0; i < 3; ++i) - for (int j = 0; j < 3; ++j) - mat(i, j) = other_matrix[i * 3 + j]; - return mat; -} -std::vector matrix3d_to_vector(const Eigen::Matrix3d &mat){ - std::vector other_matrix(9); - for (int i = 0; i < 3; ++i) - for (int j = 0; j < 3; ++j) - other_matrix[i * 3 + j] = mat(i, j); - return other_matrix; -} - -std::vector> matrix3d_to_vector2d(const Eigen::Matrix3d &mat){ - std::vector> other_matrix(3, std::vector(3)); - for (int i = 0; i < 3; ++i) - for (int j = 0; j < 3; ++j) - other_matrix[i][j] = mat(i, j); - return other_matrix; -} -Eigen::Matrix3d vector2d_to_matrix3d(const std::vector> &other_matrix){ - Eigen::Matrix3d mat; - for (int i = 0; i < 3; ++i) - for (int j = 0; j < 3; ++j) - mat(i, j) = other_matrix[i][j]; - return mat; -} -*/ //TODO: double check the matrices here -Eigen::Matrix LQRController::coriolis(const State& s) -{ - // Body velocities +Eigen::Matrix LQRController::coriolis(const State& s){ double u = s.surge; double v = s.sway; double w = s.heave; double p = s.roll_rate; double q = s.pitch_rate; double r = s.yaw_rate; - - Eigen::Matrix C = Eigen::Matrix::Zero(); - // Top-right block (translational-rotational coupling) C(0,4) = mass * w; C(0,5) = -mass * v; C(1,3) = -mass * w; C(1,5) = mass * u; diff --git a/control/velocity_controller/src/NMPC_acados.cpp b/control/velocity_controller/src/NMPC_acados.cpp deleted file mode 100644 index 9f7e99a11..000000000 --- a/control/velocity_controller/src/NMPC_acados.cpp +++ /dev/null @@ -1,183 +0,0 @@ -//#include "rclcpp/rclcpp.hpp" -#include "velocity_controller/NMPC_acados.hpp" -#include // memcpy -#include -//#include "acados_solver_auv_model.h" -#include - -void AuvNMPC::set_diag(double* M, int n, const std::vector& diag) { - for (int i = 0; i < n; ++i) { - std::memset(M + i*n, 0, n * sizeof(double)); - if (i < (int)diag.size()) M[i*n + i] = diag[i]; - } -} - -AuvNMPC::~AuvNMPC() { - if (capsule_) { - auv_model_acados_free(capsule_); - auv_model_acados_free_capsule(capsule_); - capsule_ = nullptr; - std::cout << "Destroying AuvNMPC..." << std::endl; - } -} - -bool AuvNMPC::init() { - capsule_ = auv_model_acados_create_capsule(); - if (!capsule_) return false; - - - int status = auv_model_acados_create(capsule_); - if (status) { - std::cerr << "[AuvNMPC] create failed, status: " << status << std::endl; - return false; - } - - solver_ = auv_model_acados_get_nlp_solver(capsule_); - config_ = auv_model_acados_get_nlp_config(capsule_); - dims_ = auv_model_acados_get_nlp_dims(capsule_); - nlp_in_ = auv_model_acados_get_nlp_in(capsule_); - nlp_out_= auv_model_acados_get_nlp_out(capsule_); - N_ = (N_override_ > 0) ? N_override_ : AUV_MODEL_N; // fallback - return true; -} -bool AuvNMPC::initialize_guess(std::array x,std::array u_init){ - for (int i=0;i& Wd, const std::vector& We) { - std::vector W_diag_(NY, 0.0); - std::vector W_e_diag_(NY_E, 0.0); - if ((int)Wd.size() == NY) { - W_diag_ = Wd; - } else { - std::cerr << "[AuvNMPC] set_weights: W size mismatch, got " - << Wd.size() << " expected " << NY << std::endl; - } - if ((int)We.size() == NY_E) { - W_e_diag_ = We; - } else { - std::cerr << "[AuvNMPC] set_weights: W_e size mismatch, got " - << We.size() << " expected " << NY_E << std::endl; - } - // Build W and W_e from current diagonals (could cache) - - set_diag(W_.data(), NY, W_diag_); - - set_diag(W_e_.data(), NY_E, W_e_diag_); -} - -void AuvNMPC::set_max_force(double max_force) { - max_force2_ = max_force; - if (std::isnan(max_force2_)) std::cout<<"Max force is Nan"<(x0.data())); - ocp_nlp_constraints_model_set(config_, dims_, nlp_in_, nlp_out_, 0, "ubx", const_cast(x0.data())); - - - - // Update stages - // Stage yref: [u, q, r, theta, psi, tau1, tau2, tau3] - // Matches Vx selecting states [0,4,5,7,8] and Vu selecting all 3 inputs - double yref[NY] = { - xr[0], // u (surge velocity) - xr[1], // q (pitch rate) - xr[2], // r (yaw rate) - xr[3], // theta (pitch) - xr[4], // psi (yaw) - ur[0], // tau_surge - ur[1], // tau_pitch - ur[2] // tau_yaw - }; - for (int k = 0; k < N_; ++k) { - ocp_nlp_cost_model_set(config_, dims_, nlp_in_, k, "yref", yref); - ocp_nlp_cost_model_set(config_, dims_, nlp_in_, k, "W", W_.data()); - } - // Terminal yref_e: [u, q, r, theta, psi] - double yref_e[NY_E] = { - xr[0], // u - xr[1], // q - xr[2], // r - xr[3], // theta - xr[4] // psi - }; - - ocp_nlp_cost_model_set(config_, dims_, nlp_in_, N_, "yref_e", yref_e); - ocp_nlp_cost_model_set(config_, dims_, nlp_in_, N_, "W_e", W_e_.data()); - - // Solve (blocking) - /* - for (int k = 0; k <= N_; ++k) { - ocp_nlp_out_set(config_, dims_, nlp_out_, nlp_in_,k, "x", const_cast(x0.data())); - } - double u_init[NU] = {0.0, 0.0, 0.0}; - for (int k = 0; k < N_; ++k) { - ocp_nlp_out_set(config_, dims_, nlp_out_,nlp_in_, k, "u", u_init); - }*/ - - int status = auv_model_acados_solve(capsule_); - - if (status == 0) { - std::cout << "--- Predicted Trajectory ---" << std::endl; - for (int k = 0; k <= N_; ++k) { - double x_k[NX]; - ocp_nlp_out_get(config_, dims_, nlp_out_, k, "x", x_k); - std::cout << "Step " << k << ": " < AuvNMPC::getU0(){ - return u0_out; -} - -void AuvNMPC::setState(const std::array& x){ - x0=x; - for (int i=0;i& x_ref){ //surge, pitch_rate, yaw_rate, pitch, yaw - xr=x_ref; - std::cout<<"xr: "; - for (int i=0;i -#include -#include -#include -#include -#include -#include -#include -#include "rclcpp/rclcpp.hpp" -#include "velocity_controller/utilities.hpp" -#include "velocity_controller/NMPC_setup.hpp" - - -bool NMPC_controller::set_matrices(std::vector Q,std::vector R,std::vector inertia_matrix, double max_force,std::vector water_r_low,std::vector water_r_high){ - if (Q.size()!=9){ - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The Q matrix has the wrong amount of elements"); - return 0; - } - if(R.size()!=3){ - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The R matrix has the wrong amount of elements"); - return 0; - } - if(inertia_matrix.size()!=36){ - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The M matrix has the wrong amount of elements"); - return 0; - } - if(water_r_low.size()!=36||water_r_high.size()!=36){ - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The D matrix has the wrong amount of elements"); - return 0; - } - if (max_force<0){ - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The max_force need to be >0"); - return 0; - } - if (inertia_matrix[0]<0){ - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"Negative mass?"); - return 0; - } - Q_.setZero(); - R_.setZero(); - Q_.diagonal() = Eigen::Map(Q.data(), Q.size()); - R_.diagonal() = Eigen::Map(R.data(), R.size()); - Eigen::Matrix inertia_matrix_ = Eigen::Map>(inertia_matrix.data(),6,6); - D_low=Eigen::Map>(water_r_low.data(),6,6); - D_high=Eigen::Map>(water_r_high.data(),6,6); - M_inv=inertia_matrix_.inverse(); - mass=inertia_matrix[0]; - Ix=inertia_matrix_(3,3);std::vector Q2; - std::vector R2; - Iy=inertia_matrix_(4,4); - Iz=inertia_matrix_(5,5); - - //B_=M_inv*(Eigen::Matrix() << 1,0,0,0,1,0,0,0,1,0,0,0,0,0,0,0,0,0).finished(); - - return true; -}; -void NMPC_controller::reset_controller(){ - return; -} -bool NMPC_controller::set_interval(double interval){ - interval_=interval; - return true; -} -bool NMPC_controller::initialize_MPC(){ - using SYM=casadi::MX; - SYM X=SYM::sym("X",n,1); //u,v,w,p,q,r,phi,theta,psi - //SYM A=SYM::zeros(n,n); - casadi::DM M_i=casadi::DM::zeros(6,6); - SYM U=SYM::sym("U",3,1); - casadi::DM Q=casadi::DM::zeros(n,n); - casadi::DM R=casadi::DM::zeros(m,m); - /*U(0,0)=SYM::sym("u_surge"); - U(1,0)=SYM::sym("u_pitch"); - U(2,0)=SYM::sym("u_yaw");*/ - - //Creating M_i matrixstd::vector Q2; - std::vector R2; - for (int i=0;i X_v(N+1), U_v(N); - for (int i=0;i<=N;i++) X_v[i] = SYM::sym("X_"+std::to_string(i),n); - for (int i=0;i z_parts; - z_parts.insert(z_parts.end(), X_v.begin(), X_v.end()); - z_parts.insert(z_parts.end(), U_v.begin(), U_v.end()); - SYM Z = vertcat(z_parts); - - //Initial state - SYM x0=SYM::sym("x0",n); - SYM xr=SYM::sym("xr",n); - SYM ur=SYM::sym("ur",m); - - SYM P=SYM::vertcat({x0,xr,ur}); - - - auto p_x0 = P(casadi::Slice(0, n)); // x0 - auto p_xr = P(casadi::Slice(n, 2*n)); // xr - auto p_ur = P(casadi::Slice(2*n, 2*n + m)); // ur - - //Dynamic constraints - - std::vector g_list; - g_list.push_back(X_v[0]-p_x0); - for (int i=0; i lbx_parts, ubx_parts; - x_min(7)=-1.4; - x_max(7)=1.4; - // X0..XN - for (int k = 0; k <= N; ++k) { - lbx_parts.push_back(x_min); - ubx_parts.push_back(x_max); - } - // U0..U_{N-1} - for (int k = 0; k < N; ++k) { - lbx_parts.push_back(u_min); - ubx_parts.push_back(u_max); - } - lbx = vertcat(lbx_parts); - ubx = vertcat(ubx_parts); - - // Equality constraints: G == 0 - lbg = casadi::DM::zeros(n*(N+1)); - ubg = casadi::DM::zeros(n*(N+1)); - - - //building NLP - casadi::MXDict nlp; - nlp["x"]=Z; - nlp["f"]=J; - nlp["g"]=G; - nlp["p"]=P; - casadi::Dict opts1; - opts1["ipopt.print_level"]=2; - opts1["print_time"]=false; - opts1["ipopt.sb"]="yes"; - opts1["expand"]=true; - opts1["jit"]=false; - opts1["ipopt.tol"]=1e-4; - opts1["ipopt.max_iter"]=100; - opts1["ipopt.linear_solver"]="mumps"; //robust default - - solver=casadi::nlpsol("solver","ipopt",nlp,opts1); - - // -------------------------------------------------------- - // Prepare parameter vector Pval and initial guess Z0 - // -------------------------------------------------------- - // Example numeric values: - casadi::DM x0_val = casadi::DM::zeros(n,1); - std::vector Pval_parts; - Pval_parts.push_back(x0_val); - Pval_parts.push_back(casadi::DM::zeros(n,1)); - Pval_parts.push_back(casadi::DM::zeros(m,1)); - Pval = vertcat(Pval_parts); - - // Initial guess: Z0 (X guesses then U guesses) - std::vector Z0_parts; - for (int k = 0; k <= N; ++k) Z0_parts.push_back(x0_val); // start with x0 everywhere - for (int k = 0; k < N; ++k) Z0_parts.push_back(casadi::DM::zeros(m,1)); - Z0_next = vertcat(Z0_parts); - - - return true; -} - -bool NMPC_controller::calculate_thrust(Guidance_data guidance_values, State state){ - - casadi::DM x0_val={state.surge,state.sway,state.heave,state.roll_rate,state.pitch_rate,state.yaw_rate,state.roll,state.pitch,state.yaw}; - casadi::DM xr_val={guidance_values.surge,0,0,0,0,0,0,guidance_values.pitch,guidance_values.yaw}; - casadi::DM ur_val={0,0,0}; - Pval=casadi::DM::vertcat({x0_val,xr_val,ur_val}); - // Solve - - casadi::DMDict solver_in; - solver_in["x0"] = Z0_next; - solver_in["lbx"] = lbx; - solver_in["ubx"] = ubx; - solver_in["lbg"] = lbg; - solver_in["ubg"] = ubg; - solver_in["p"] = Pval; - auto sol = solver(solver_in); - if (sol.count("x") == 0) { - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "NLP solver failed"); - return 1; - } - casadi::DM Zstar = sol.at("x"); // optimal stacked decision vector - //TODO: check to see NAN or INF values in solution - - // index of U0 start: - int offset_u0 = n*(N+1); - // Extract u0* (first control block after all states) - // Z = [X0; X1; ...; XN; U0; U1; ...; U_{N-1}] - casadi::DM u0_star = Zstar(casadi::Slice(offset_u0, offset_u0 + m)); - - std::cout << "u0* = " << u0_star << std::endl; - - - // Warm-start shift for next iteration - // Build new Z0_next = [X1*; X2*; ...; XN*; XN*; U1*; ...; U_{N-1}*; U_{N-1}*] - // (X0 will be re-anchored by the new measured x0 in constraints) - std::vector Xstar(N+1), Ustar(N); - // Unstack from Zstar: - for (int k = 0; k <= N; ++k) { - int i0 = k*n; - Xstar[k] = Zstar(casadi::Slice(i0, i0+n)); - } - for (int k = 0; k < N; ++k) { - int i0 = n*(N+1) + k*m; - Ustar[k] = Zstar(casadi::Slice(i0, i0+m)); - } - - std::vector Z0_next_parts; - // shifted states: X1..XN, repeat XN at end - for (int k = 1; k <= N; ++k) Z0_next_parts.push_back(Xstar[k]); - Z0_next_parts.push_back(Xstar[N]); - // shifted inputs: U1..U_{N-1}, repeat last - for (int k = 1; k < N; ++k) Z0_next_parts.push_back(Ustar[k]); - Z0_next_parts.push_back(Ustar[N-1]); - - Z0_next = vertcat(Z0_next_parts); - - - thrust(0) = double(u0_star(0)); - thrust(1) = double(u0_star(1)); - thrust(2) = double(u0_star(2)); - return 0; -} - -Eigen::Matrix NMPC_controller::get_thrust(){ - return thrust; -} \ No newline at end of file diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index 037730231..c577a73af 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -1,13 +1,6 @@ -#include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/string.hpp" #include "velocity_controller/velocity_controller.hpp" #include -#include "geometry_msgs/msg/wrench_stamped.hpp" -#include "std_msgs/msg/float64_multi_array.hpp" -#include "std_msgs/msg/bool.hpp" -#include "velocity_controller/NMPC_setup.hpp" #include "velocity_controller/PID_setup.hpp" -#include #include #include #include @@ -16,34 +9,19 @@ #include #include #include -#include "vortex_msgs/msg/los_guidance.hpp" #include "vortex/utils/math.hpp" #include "velocity_controller/utilities.hpp" #include -//Konstruktør + Velocity_node::Velocity_node() : rclcpp_lifecycle::LifecycleNode("velocity_controller_lifecycle"), lqr_controller(), pub_QoS(10), sub_QoS(10) { get_new_parameters(); pub_QoS.keep_last(10).reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT).durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); sub_QoS.keep_last(10).reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT).durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); - //TODO: dont need to save the Q3 R3 and the other matries, just use #define - //NMPC controller - NMPC.set_matrices(Q3,R3, inertia_matrix, max_force, dampening_matrix_low, dampening_matrix_high); - NMPC.set_interval(publish_rate/1000.0); - //NMPC.initialize_MPC(); - //NMPC acados controller - NMPC_acados.init(); - NMPC_acados.set_max_force(max_force); - std::vector W=Q2; - W.insert(W.end(),R2.begin(),R2.end()); - std::vector We=Q2; - NMPC_acados.set_weights(W, We); - - - //Controllers + //PID initialization PID_surge.set_output_limits(-max_force, max_force); PID_pitch.set_output_limits(-max_force, max_force); PID_yaw.set_output_limits(-max_force, max_force); @@ -51,10 +29,12 @@ Velocity_node::Velocity_node() : rclcpp_lifecycle::LifecycleNode("velocity_contr PID_pitch.set_parameters(pitch_params,publish_rate/1000.0); PID_yaw.set_parameters(yaw_params,publish_rate/1000.0); - if(!lqr_controller.set_matrices(Q,R,inertia_matrix,max_force,dampening_matrix_low,dampening_matrix_high)||!lqr_controller.set_interval(static_cast(publish_rate)/1000)){ + //LQR + if(!lqr_controller.set_matrices(Q,R,inertia_matrix,max_force,dampening_matrix_low)||!lqr_controller.set_interval(static_cast(publish_rate)/1000)){ controller_type=1; RCLCPP_INFO(this->get_logger(),"Switching to PID"); }; + //Automaticly start in activate if auto_start is true if(auto_start){ startup_timer_=create_wall_timer(std::chrono::milliseconds(0), [this](){startup_timer_->cancel(); trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);}); } @@ -86,7 +66,6 @@ void Velocity_node::calc_thrust() switch (controller_type) { case 1:{ - //TODO: some logic for removing the change in reference PID_surge.calculate_thrust(error.surge); PID_pitch.calculate_thrust(error.pitch,-current_state.pitch_rate); PID_yaw.calculate_thrust(error.yaw, -current_state.yaw_rate); @@ -111,47 +90,11 @@ void Velocity_node::calc_thrust() } break; } - case 3:{ - //RCLCPP_INFO(this->get_logger(),"Guidance: %f, %f, %f",guidance_values.surge,guidance_values.pitch,guidance_values.yaw); - Eigen::Matrix u; - if (NMPC.calculate_thrust(guidance_values, current_state)){ - controller_type=1; - RCLCPP_ERROR(this->get_logger(),"Switching to PID"); - rclcpp::shutdown(); - } - else{ - u=NMPC.get_thrust(); - thrust_out.wrench.force.x=u[0]; - thrust_out.wrench.torque.y=u[1]; - thrust_out.wrench.torque.z=u[2]; - RCLCPP_INFO(this->get_logger(),"NMPC: surge: %f, pitch %f, yaw %f",u(0),u(1),u(2)); - } - - break; - } - case 4:{ - std::vectoru; - std::array x_ref={guidance_values.surge,0.0,0.0,guidance_values.pitch,guidance_values.yaw}; //surge, pitch_rate, yaw_rate, pitch, yaw - - NMPC_acados.setReference(x_ref); - std::array state_array={current_state.surge,current_state.sway,current_state.heave,current_state.roll_rate,current_state.pitch_rate,current_state.yaw_rate,current_state.roll,current_state.pitch,current_state.yaw}; - NMPC_acados.setState(state_array); - int status=NMPC_acados.solve_once(); - RCLCPP_INFO(this->get_logger(),"Status %i",status); - if(status){ - - rclcpp::shutdown(); - }; - u=NMPC_acados.getU0(); - thrust_out.wrench.force.x=u[0]; - thrust_out.wrench.torque.y=u[1]; - thrust_out.wrench.torque.z=u[2]; - break; - } default:{ - //Some crash handling here - RCLCPP_ERROR(this->get_logger(),"Unknown controller set"); - break; + RCLCPP_ERROR(this->get_logger(),"Unknown controller set, switching to PID"); + controller_type=1; + calc_thrust(); + return; } } publisher_thrust->publish(thrust_out); @@ -180,22 +123,27 @@ void Velocity_node::odometry_callback(const nav_msgs::msg::Odometry::SharedPtr m void Velocity_node::get_new_parameters(){ - //topics //TODO: check what happens when same parameter in global and local file + //topics this->declare_parameter("topics.wrench_input"); - this->topic_thrust = this->get_parameter("topics.wrench_input").as_string(); + topic_thrust = this->get_parameter("topics.wrench_input").as_string(); this->declare_parameter("topics.guidance.los"); - this->topic_guidance = this->get_parameter("topics.guidance.los").as_string(); + topic_guidance = this->get_parameter("topics.guidance.los").as_string(); this->declare_parameter("topics.odom"); - this->topic_odometry = this->get_parameter("topics.odom").as_string(); + topic_odometry = this->get_parameter("topics.odom").as_string(); - //variables - - this->declare_parameter("max_force"); - this->max_force = this->get_parameter("max_force").as_double(); - this->declare_parameter("publish_rate"); - this->publish_rate = this->get_parameter("publish_rate").as_int(); - this->declare_parameter("controller_type"); - this->controller_type=this->get_parameter("controller_type").as_int(); + //Settings + this->declare_parameter("Settings.max_force"); + max_force = this->get_parameter("Settings.max_force").as_double(); + this->declare_parameter("Settings.publish_rate"); + publish_rate = this->get_parameter("Settings.publish_rate").as_int(); + this->declare_parameter("Settings.controller_type"); + controller_type=this->get_parameter("Settings.controller_type").as_int(); + this->declare_parameters("Settings", {{"auto_start", false}, {"reset_on_new_ref", true}, {"anti_overshoot", true},{"odometry_dropout_guard", true}}); + auto settings = this->get_parameters({"Settings.auto_start", "Settings.reset_on_new_ref", "Settings.anti_overshoot", "Settings.odometry_dropout_guard"}); + auto_start = settings[0].as_bool(); + reset_on_new_ref = settings[1].as_bool(); + anti_overshoot = settings[2].as_bool(); + odometry_dropout_guard = settings[3].as_bool(); //PID Params this->declare_parameter>("PID_params.surge"); @@ -220,18 +168,7 @@ void Velocity_node::get_new_parameters(){ this->dampening_matrix_low=this->get_parameter("dampening_matrix_low").as_double_array(); this->dampening_matrix_high=this->get_parameter("dampening_matrix_high").as_double_array(); - //NMPC acados Parameters - this->declare_parameter>("NMPCA_params.Q"); - this->declare_parameter>("NMPCA_params.R"); - Q2=this->get_parameter("NMPCA_params.Q").as_double_array(); - R2=this->get_parameter("NMPCA_params.R").as_double_array(); - //NMPC - - this->declare_parameter>("NMPC_params.Q"); - this->declare_parameter>("NMPC_params.R"); - Q3=this->get_parameter("NMPC_params.Q").as_double_array(); - R3=this->get_parameter("NMPC_params.R").as_double_array(); - + } @@ -307,20 +244,17 @@ void Velocity_node::reset_controllers(int nr){ case 1: PID_surge.reset_controller(); lqr_controller.reset_controller(1); - break; case 2: PID_pitch.reset_controller(); lqr_controller.reset_controller(2); - break; case 3: PID_yaw.reset_controller(); lqr_controller.reset_controller(3); break; - } } @@ -335,57 +269,7 @@ int main(int argc, char * argv[]) while (rclcpp::ok()&&!lc_node->should_exit_){ exec.spin_some(); } - //rclcpp::shutdown(); return 0; } -//---------------------------------------------------------------------------------------------------------------- -//Operator overloading for geometry_msgs::msg::WrenchStamped -/* -geometry_msgs::msg::WrenchStamped operator-(const geometry_msgs::msg::WrenchStamped & a, const geometry_msgs::msg::WrenchStamped & b) -{ - geometry_msgs::msg::WrenchStamped result; - result.wrench.force.x = a.wrench.force.x - b.wrench.force.x; - result.wrench.force.y = a.wrench.force.y - b.wrench.force.y; - result.wrench.force.z = a.wrench.force.z - b.wrench.force.z; - result.wrench.torque.x = a.wrench.torque.x - b.wrench.torque.x; - result.wrench.torque.y = a.wrench.torque.y - b.wrench.torque.y; - result.wrench.torque.z = a.wrench.torque.z - b.wrench.torque.z; - return result; -} -geometry_msgs::msg::WrenchStamped operator+(const geometry_msgs::msg::WrenchStamped & a, const geometry_msgs::msg::WrenchStamped & b) -{ - geometry_msgs::msg::WrenchStamped result; - result.wrench.force.x = a.wrench.force.x + b.wrench.force.x; - result.wrench.force.y = a.wrench.force.y + b.wrench.force.y; - result.wrench.force.z = a.wrench.force.z + b.wrench.force.z; - result.wrench.torque.x = a.wrench.torque.x + b.wrench.torque.x; - result.wrench.torque.y = a.wrench.torque.y + b.wrench.torque.y; - result.wrench.torque.z = a.wrench.torque.z + b.wrench.torque.z; - return result; -} -//operator overloading for guidance_data -Guidance_data Guidance_data::operator-(const Guidance_data & b) const -{ - Guidance_data result; - result.surge = this->surge - b.surge; - result.pitch = this->pitch - b.pitch; - result.yaw = this->yaw - b.yaw; - return result; -} - -Guidance_data& Guidance_data::operator=(const std_msgs::msg::Float64MultiArray& msg) -{ - if (msg.data.size()>=3){ - surge=msg.data[0]; - pitch=msg.data[1]; - yaw=msg.data[2]; - } - else{ - //throw std::runtime_error("Guidance message too short, needs at least 3 values"); - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Guidance message too short, needs at least 3 values"); - } - return *this; -} -*/ diff --git a/control/velocity_controller/tests/LQR_test.cpp b/control/velocity_controller/tests/LQR_test.cpp deleted file mode 100644 index 392103cff..000000000 --- a/control/velocity_controller/tests/LQR_test.cpp +++ /dev/null @@ -1,55 +0,0 @@ -#include "rclcpp/rclcpp.hpp" -#include -#include "velocity_controller/LQR_setup.hpp" -#include "ct/optcon/lqr/LQR.hpp" - -class test_LQR_node : public rclcpp::Node{ - public: - double q_surge =75.0, q_pitch= 175.0, q_yaw= 175.0, r_surge= 0.3, r_pitch= 0.4, r_yaw= 0.4, i_surge= 0.3, - i_pitch= 0.4, i_yaw= 0.3, i_weight= 0.5, dt= 0.1; - LQRparameters param={q_surge, q_pitch, q_yaw, r_surge, r_pitch, r_yaw, i_surge, i_pitch, i_yaw, i_weight}; - LQRController controller; - test_LQR_node():Node("test_LQR_node"), controller(){ - RCLCPP_INFO(this->get_logger(),"LQR test node started"); - Eigen::Matrix Q; - Eigen::VectorXd qdiag(8); - qdiag << 75, 175, 175, 1, 0.3, 0.4, 0.3, 1; - Q.diagonal() = qdiag; - Eigen::Matrix R; - Eigen::VectorXd rdiag(3); - rdiag << 0.3, 0.4, 0.4; - R.diagonal() = rdiag; - Eigen::Matrix A=(Eigen::Matrix()<<5,7,23,0,0,0,1,1,0,45,21,4,3,4,3,2,0,23,1,7,6,5,5,7,5,7,6,3,5,7,0,9,2,2,3,2,1,0,3,7,0,0,8,7,6,5,8,5,4,7,1,5,6,2,4,7,6,2,4,5,2,12,5,6).finished(); - Eigen::Matrix B=(Eigen::Matrix()<<2,0,0,3,0,2,0,3,0,1,2,0,3,4,0,3,5,0,6,3,4,1,2,3).finished(); - /*Eigen::Matrix3d inertia_matrix=(Eigen::Matrix3d()<<30.0, 0.6, 0.0, 0.6, 1.629, 0.0, 0.0, 0.0, 1.729).finished(); - Eigen::Matrix3d inertia_matrix_inv=inertia_matrix.inverse(); - Eigen::Matrix3d coriolis_matrix=(Eigen::Matrix3d()<<0.2,-30*2*0.01,-30*2*0.0,30 * 2*0.01,0,1.629 * 2,30 * 2*0.01,1.769 * 2,0).finished(); - Eigen::Matrix3d system_matrix=inertia_matrix.inverse()*coriolis_matrix; - Eigen::Matrix augmented_system_matrix =(Eigen::Matrix()< augmented_input_matrix=(Eigen::Matrix()<< inertia_matrix_inv(0,0),inertia_matrix_inv(0,1),inertia_matrix_inv(0,2),0,0,0, - inertia_matrix_inv(1,0),inertia_matrix_inv(1,1),inertia_matrix_inv(1,2),0,0,0, - inertia_matrix_inv(2,0),inertia_matrix_inv(2,1),inertia_matrix_inv(2,2),0,0,0).finished();*/ - ct::optcon::LQR<8,3> lqr_solver; - Eigen::Matrix K; - lqr_solver.compute(Q,R,A,B,K,true,false); - RCLCPP_INFO(this->get_logger(),"LQR Gain K matrix:"); - RCLCPP_INFO(this->get_logger(),"\n%f %f %f %f %f %f %f %f\n%f %f %f %f %f %f %f %f\n%f %f %f %f %f %f %f %f", - K(0,0),K(0,1),K(0,2),K(0,3),K(0,4),K(0,5),K(0,6),K(0,7), - K(1,0),K(1,1),K(1,2),K(1,3),K(1,4),K(1,5),K(1,6),K(1,7), - K(2,0),K(2,1),K(2,2),K(2,3),K(2,4),K(2,5),K(2,6),K(2,7)); - - } -}; - -int main(int argc, char const *argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/control/velocity_controller/tests/test_LQR.cpp b/control/velocity_controller/tests/test_LQR.cpp index 9317da845..f2355ab44 100644 --- a/control/velocity_controller/tests/test_LQR.cpp +++ b/control/velocity_controller/tests/test_LQR.cpp @@ -20,7 +20,7 @@ class LQR_test : public ::testing::Test{ }; void SetUp() override{ - controller.set_matrices(cfg["/**"]["ros__parameters"]["LQR_params"]["Q"].as>(),cfg["/**"]["ros__parameters"]["LQR_params"]["R"].as>(),cfg["/**"]["ros__parameters"]["inertia_matrix"].as>(),cfg["/**"]["ros__parameters"]["max_force"].as(),cfg["/**"]["ros__parameters"]["dampening_matrix_low"].as>(),cfg["/**"]["ros__parameters"]["dampening_matrix_high"].as>()); + controller.set_matrices(cfg["/**"]["ros__parameters"]["LQR_params"]["Q"].as>(),cfg["/**"]["ros__parameters"]["LQR_params"]["R"].as>(),cfg["/**"]["ros__parameters"]["inertia_matrix"].as>(),cfg["/**"]["ros__parameters"]["max_force"].as(),cfg["/**"]["ros__parameters"]["dampening_matrix_low"].as>()); controller.reset_controller(); controller.set_interval(0.01); } @@ -43,13 +43,13 @@ TEST_F(LQR_test,wrong_setup){ std::vector six={1,2,3,4,5,6}; std::vector thirty_six={1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24,25,26,27,28,29,30,31,32,33,34,35,36}; std::vector three={1,2,3}; - EXPECT_TRUE(controller.set_matrices(eight,three,thirty_six,100,thirty_six,thirty_six)); - EXPECT_FALSE(controller.set_matrices(eight,eight,thirty_six,100,thirty_six,thirty_six)); - EXPECT_FALSE(controller.set_matrices(three,three,thirty_six,100,thirty_six,thirty_six)); - EXPECT_FALSE(controller.set_matrices(eight,three,eight,100,thirty_six,thirty_six)); - EXPECT_FALSE(controller.set_matrices(eight,three,thirty_six,100,eight,thirty_six)); - EXPECT_FALSE(controller.set_matrices(eight,three,thirty_six,100,thirty_six,eight)); - EXPECT_FALSE(controller.set_matrices(eight,three,thirty_six,-100,thirty_six,thirty_six)); + EXPECT_TRUE(controller.set_matrices(eight,three,thirty_six,100,thirty_six)); + EXPECT_FALSE(controller.set_matrices(eight,eight,thirty_six,100,thirty_six)); + EXPECT_FALSE(controller.set_matrices(three,three,thirty_six,100,thirty_six)); + EXPECT_FALSE(controller.set_matrices(eight,three,eight,100,thirty_six)); + EXPECT_FALSE(controller.set_matrices(eight,three,thirty_six,100,eight)); + EXPECT_FALSE(controller.set_matrices(eight,three,thirty_six,100,eight)); + EXPECT_FALSE(controller.set_matrices(eight,three,thirty_six,-100,thirty_six)); }; /* TEST_F(LQR_test,solve){ diff --git a/control/velocity_controller/tests/test_VC.cpp b/control/velocity_controller/tests/test_VC.cpp index 93dced1f4..9e32436f7 100644 --- a/control/velocity_controller/tests/test_VC.cpp +++ b/control/velocity_controller/tests/test_VC.cpp @@ -4,15 +4,13 @@ #include #include #include -///#include "velocity_controller/PID_setup.hpp" #include "test_VC.hpp" #include #include #include #include "vortex_msgs/msg/los_guidance.hpp" #include "velocity_controller/utilities.hpp" -//#include "velocity_controller/velocity_controller.hpp" -//#include "LQR_setup.hpp" + //Denne noden er kun for å teste velocity_controller noden test_VC::test_VC() : Node("test_VC_node") diff --git a/control/velocity_controller/tests/test_VC.hpp b/control/velocity_controller/tests/test_VC.hpp index ff82b8cd5..d5270b509 100644 --- a/control/velocity_controller/tests/test_VC.hpp +++ b/control/velocity_controller/tests/test_VC.hpp @@ -5,11 +5,11 @@ #include #include #include -#include "velocity_controller/PID_setup.hpp" -#include "velocity_controller/velocity_controller.hpp" #include #include #include "vortex_msgs/msg/los_guidance.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "velocity_controller/utilities.hpp" class test_VC : public rclcpp::Node{ public: From f4d9c574c4380522985297e876959eeed3620017 Mon Sep 17 00:00:00 2001 From: Henrik Date: Wed, 25 Mar 2026 14:32:28 +0100 Subject: [PATCH 201/290] Cleaned up a bit more, removed comments, Guidance_values class and LQRParameters class --- .../include/velocity_controller/LQR_setup.hpp | 38 ------------------- .../include/velocity_controller/utilities.hpp | 12 ------ .../velocity_controller.hpp | 2 +- 3 files changed, 1 insertion(+), 51 deletions(-) diff --git a/control/velocity_controller/include/velocity_controller/LQR_setup.hpp b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp index e38cc68cb..2b4adfadd 100644 --- a/control/velocity_controller/include/velocity_controller/LQR_setup.hpp +++ b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp @@ -3,42 +3,9 @@ #include #include #include -#include "PID_setup.hpp" #include #include "velocity_controller/utilities.hpp" - - -/*class Guidance_values{ - //Dataclass to store guidance values for LQR controller - public: - double surge=0.0; double pitch=0.0; double yaw=0.0; - double integral_surge=0.0; double integral_pitch=0.0; double integral_yaw=0.0; -}; -*/ -class LQRparameters{ - //Dataclass to store LQR parameters - public: - double q_surge=0.0; double q_pitch=0.0; double q_yaw=0.0; - double r_surge=0.0; double r_pitch=0.0; double r_yaw=0.0; - double i_surge=0.0; double i_pitch=0.0; double i_yaw=0.0; - double i_weight=0.0; double max_force=0.0; -}; - -/*class angle{ - public: - double phit=0.0; - double thetat=0.0; - double psit=0.0; - -};*/ -/* -struct LQRsolveResult{ - Eigen::MatrixXd K; - Eigen::MatrixXd P; - int INFO=0; - LQRsolveResult(Eigen::MatrixXd K=Eigen::MatrixXd::Zero(),Eigen::MatrixXd P=Eigen::MatrixXd::Zero(), int INFO=0):K(K),P(P),INFO(INFO) {}; -};*/ class LQRController{ public: @@ -78,8 +45,3 @@ class LQRController{ }; -//Extra operations -//Eigen::Matrix3d vector_to_matrix3d(const std::vector &other_matrix); -//std::vector matrix3d_to_vector(const Eigen::Matrix3d &mat); -//std::vector> matrix3d_to_vector2d(const Eigen::Matrix3d &mat); -//Eigen::Matrix3d vector2d_to_matrix3d(const std::vector> &other_matrix); diff --git a/control/velocity_controller/include/velocity_controller/utilities.hpp b/control/velocity_controller/include/velocity_controller/utilities.hpp index c59efcc5a..70512f5cf 100644 --- a/control/velocity_controller/include/velocity_controller/utilities.hpp +++ b/control/velocity_controller/include/velocity_controller/utilities.hpp @@ -17,36 +17,24 @@ angle quaternion_to_euler_angle(double w, double x, double y, double z); geometry_msgs::msg::Quaternion euler_angle_to_quaternion(double roll, double pitch, double yaw); class State{ - //Dataclass to store state values for LQR controller public: double surge=0.0, sway=0.0, heave=0.0, roll_rate=0.0, pitch_rate=0.0, yaw_rate=0.0; //roll_rate=0.0, pitch_rate=0.0, yaw_rate=0.0; double roll=0.0, pitch=0.0, yaw=0.0; //phi, theta, psi double w=0.0, x=0.0,y=0.0,z=0.0; - //double integral_surge=0.0; double integral_pitch=0.0; double integral_yaw=0.0; State(double surge=0,double pitch=0, double yaw=0):surge{surge}, pitch{pitch},yaw{yaw}{}; - //State(){}; State operator=(int n){if (n){surge=0.0,sway=0.0,heave=0.0,roll_rate=0.0,pitch_rate=0.0,yaw_rate=0.0,roll=0.0,pitch=0.0,yaw=0.0;} return *this;}; State operator=(nav_msgs::msg::Odometry::SharedPtr rhs); angle get_angle(); }; -//TODO: fix these so that changing the quaternions changes the angles, so that the state is always consistent class Guidance_data{ public: double surge=0.0; double pitch=0.0; double yaw=0.0; - //Guidance_data(vortex_msgs::msg::LOSGuidance msg):State{msg.surge,msg.pitch,msg.yaw}{}; Guidance_data(double surge, double pitch, double yaw):surge{surge},pitch{pitch}, yaw{yaw} {}; Guidance_data():surge{0.0}, pitch{0.0}, yaw{0.0} {}; - //Guidance_data():State{0, 0, 0} {}; - - //Guidance_data operator-(const Guidance_data& other) const; Guidance_data& operator=(const vortex_msgs::msg::LOSGuidance::SharedPtr& msg); }; -//angle NED_to_BODY(const angle &a,const State &s); -//Eigen::Vector3d NED_to_BODY(const Eigen::Vector3d &a, const State &s); -//Eigen::Matrix3d R_ned_to_body(double roll, double pitch, double yaw); -//Eigen::Matrix3d T_euler_to_body(double roll, double pitch); Eigen::Vector3d body_rates_to_euler_rates(double roll, double pitch,double p, double q, double r); angle angle_NED_to_body( double roll_des, double pitch_des, double yaw_des, double roll, double pitch, double yaw); angle angle_NED_to_body(angle desired, angle state); diff --git a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp index a81483f9f..9bdd2864f 100644 --- a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp +++ b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp @@ -61,7 +61,7 @@ class Velocity_node : public rclcpp_lifecycle::LifecycleNode{ geometry_msgs::msg::WrenchStamped thrust_out; - int controller_type; //1 PID, 2 LQR, 3 NMPC, 4 NMPC acados + int controller_type; //1 PID, 2 LQR //PID controllers PID_controller PID_surge; From c01882122c47e20ac32a10b2228cf7738233a0a6 Mon Sep 17 00:00:00 2001 From: Henrik Date: Wed, 25 Mar 2026 14:32:36 +0100 Subject: [PATCH 202/290] Added ReadMe file --- control/velocity_controller/README.md | 201 ++++++++++++++++++++++++++ 1 file changed, 201 insertions(+) create mode 100644 control/velocity_controller/README.md diff --git a/control/velocity_controller/README.md b/control/velocity_controller/README.md new file mode 100644 index 000000000..25556cb17 --- /dev/null +++ b/control/velocity_controller/README.md @@ -0,0 +1,201 @@ +# velocity_controller + +ROS2 lifecycle node for velocity control of an AUV (autonomous underwater vehicle). Supports two control strategies — a PID controller and an LQR controller. + +--- + +## Overview + +The package implements a `Velocity_node` that subscribes to odometry and guidance inputs, computes thrust commands, and publishes them as `WrenchStamped` messages. The node is managed as a ROS2 lifecycle node, meaning it can be managed by a lifecycle manager, however if you do not want to use a lifecycle manager you can change the parameter autostart in the parameter file so that it automaticly goes into active state. + +The LQR controller linearizes the vehicle dynamics around the current state at each timestep (gain-scheduled LQR), using a body-frame model that includes linear hydrodynamic damping, Coriolis effects, and integral action for steady-state error rejection. The PID controller serves as a simpler backup. + +--- + +## Dependencies + +| Dependency | Purpose | +|---|---| +| `rclcpp` / `rclcpp_lifecycle` | ROS2 node and lifecycle management | +| `Eigen3` | Matrix math for LQR | +| `control_toolbox` (`ct::optcon`) | Riccati equation solver for LQR gain | +| `CasADi` | Used in utilities (NMPC-related) | +| `vortex_msgs` | Custom guidance message (`LOSGuidance`) | +| `nav_msgs` | Odometry input | +| `geometry_msgs` | Thrust output (`WrenchStamped`) | + +--- + +## Topics + +| Topic | Type | Direction | Description | +|---|---|---|---| +| `topic_thrust` | `geometry_msgs/WrenchStamped` | Published | Force and torque commands to thruster allocator | +| `topic_guidance` | `vortex_msgs/LOSGuidance` | Subscribed | Desired surge, pitch, yaw from guidance system | +| `topic_odometry` | `nav_msgs/Odometry` | Subscribed | Current vehicle state from state estimator | + +Topic names are configurable via ROS2 global parameter file. + +--- + +## Parameters + +All parameters are loaded in the constructor via `get_new_parameters()`. + +### Controller selection + +| Parameter | Type | Description | +|---|---|---| +| `controller_type` | `int` | `1` = PID, `2` = LQR | +| `publish_rate` | `int` | Control loop frequency in Hz | +| `max_force` | `double` | Saturation limit applied to all outputs (N / Nm) | + +### LQR parameters + +| Parameter | Type | Size | Description | +|---|---|---|---| +| `Q` | `double[]` | 8 | Diagonal of state weight matrix. States: `[surge_err, pitch_err, yaw_err, pitch_rate_err, yaw_rate_err, ∫surge, ∫pitch, ∫yaw]` | +| `R` | `double[]` | 3 | Diagonal of input weight matrix. Inputs: `[Fx, Ty, Tz]` | +| `inertia_matrix` | `double[]` | 36 | Row-major 6x6 rigid body inertia matrix | +| `dampening_matrix_low` | `double[]` | 36 | Row-major 6x6 hydrodynamic damping matrix at low speed | + +### PID parameters + +Each axis (surge, pitch, yaw) takes a parameter vector of the form `[Kp, Ki, Kd]`. + +| Parameter | Description | +|---|---| +| `surge_params` | PID gains for surge | +| `pitch_params` | PID gains for pitch | +| `yaw_params` | PID gains for yaw | + +### Behaviour flags + +| Parameter | Type | Description | +|---|---|---| +| `reset_on_new_ref` | `bool` | Reset integrators when a new guidance reference arrives | +| `anti_overshoot` | `bool` | Enable anti-overshoot logic | +| `odometry_dropout_guard` | `bool` | Stop publishing if odometry stops arriving | +| `auto_start` | `bool` | If true, node self-transitions to active on startup | + + +--- + +## Lifecycle states + +The node uses the standard ROS2 managed lifecycle: + +``` +Unconfigured → [configure] → Inactive → [activate] → Active + ← [deactivate] ← +``` + +If `auto_start` is set to `true` in the parameters, the node will automatically call configure and activate itself after startup without needing an external lifecycle manager. + +To manually manage the node: + +```bash +# Configure +ros2 lifecycle set /velocity_controller configure + +# Activate +ros2 lifecycle set /velocity_controller activate + +# Deactivate +ros2 lifecycle set /velocity_controller deactivate + +# Cleanup +ros2 lifecycle set /velocity_controller cleanup + +# Shutdown +ros2 lifecycle set /velocity_controller shutdown + +``` + +--- + +## Controller details + +### LQR + +The LQR controller uses an 8-state augmented model in the body frame: + +``` +x = [surge_err, pitch_err, yaw_err, pitch_rate_err, yaw_rate_err, ∫surge, ∫pitch, ∫yaw] +u = [Fx, Ty, Tz] +``` + +The system matrix `A` is re-linearized around the current state every control timestep. Guidance references in NED are converted to body-frame errors using the rotation matrix method before being passed to the controller — not by angle subtraction. + +The gain `K` is computed by solving the continuous-time algebraic Riccati equation via `ct::optcon::LQR`. The control law is: + +``` +u = K * x_error +``` + +where `ct::optcon` produces `K` such that this is equivalent to `u = -K * (x - x_ref)`. + +If the Riccati solver fails (e.g. due to an unstabilizable operating point), the node automatically falls back to PID and logs an error. + +### PID + +Three independent PID controllers run on surge, pitch, and yaw. Each supports anti-windup via integrator clamping. The derivative term can be computed either from the error signal or from a separately provided error derivative, depending on which `calculate_thrust` overload is called. + +--- + +## Building + +```bash +colcon build --packages-select velocity_controller --symlink-install +source install/setup.bash +``` +or with + +```bash +colcon build --packages-up-to velocity_controller --symlink-install +source install/setup.bash +``` +Done in root of workspace +--- + +## Running + +Via a launch file with a parameter file: + +```bash +ros2 launch velocity_controller velocity_controller.launch.py +``` + +--- + +## Tests +There are system tests and a helper node that generates a reference for the controller to follow. +Tests are build like this: +```bash +colcon build --packages-select velocity_controller --symlink-install --cmake-args -DBUILD_TESTING=ON +source install/setup.bash +``` +System tests are run with the command + +```bash +colcon test +``` + +Helper node is run with + +```bash +ros2 launch velocity_controller VCnTest.launch.py +``` + +## Notes for new team members + +- The guidance input is expected in NED frame (north-east-down). The controller handles the NED-to-body conversion internally. +- All angle errors are wrapped to `[-π, π]` using `ssa()` (smallest signed angle) before being fed to the controller. +- The LQR Q matrix ordering matters — the 8 diagonal values correspond exactly to `[surge_err, pitch_err, yaw_err, pitch_rate_err, yaw_rate_err, ∫surge, ∫pitch, ∫yaw]` in that order. +- If the vehicle behaves oddly, check that `interval_` (the control timestep) is being set correctly — a value of `0` disables integral action silently. + +## Adding new controllers +After adding the hpp file, add the calculation to calc_thrust function in a new switch case, add to the reset_controller function, with options to reset only one integral, lastly update documentation. Remeber to intialize correctly, either in 'on_configure' or in constructor, add the appropriate parameters, and update alle the {drone}_params.yaml files. + +## Adding new drones +Copy a {drone}_params.yaml file and change the name to the new name of the drone. Add the appropriate matrices, and tune to satisfying behaviour. \ No newline at end of file From a74fd950390d8ab9fa3b7cf3afc1fe20f0d440ce Mon Sep 17 00:00:00 2001 From: Anbit Date: Wed, 25 Mar 2026 15:27:44 +0100 Subject: [PATCH 203/290] Final changes before PR --- guidance/los_guidance/README.md | 19 ++++++ .../los_guidance/config/guidance_params.yaml | 8 +-- .../include/los_guidance/los_guidance_ros.hpp | 2 +- .../los_guidance/src/los_guidance_ros.cpp | 32 +++++---- .../los_guidance/test/adaptive_los_test.cpp | 66 ------------------- 5 files changed, 39 insertions(+), 88 deletions(-) diff --git a/guidance/los_guidance/README.md b/guidance/los_guidance/README.md index 20dd8f38e..10a3a6341 100644 --- a/guidance/los_guidance/README.md +++ b/guidance/los_guidance/README.md @@ -319,6 +319,25 @@ However it performs worse when the path contains **sharp turns or rapidly changi --- +## Testing with `guidance_test.launch.py` + +To test the entire los system from just one place a simple, low-effort launch script has been implemented. The file is named `los_guidance/launch/guidance_test.launch.py` It launches the necessary nodes for simulation and autopilot and lets you choose a test scenario through the `test_scenario` argument.The sceraios that have been implemented are: + +| Scenario | Use | +|----------|------| +| Circle | ends the drone in a circular path | +| Square |sends the drone through a square waypoint pattern| +| opposite_point | sends the drone toward a waypoint and then toward a waypoint in the opposite direction (180 degrees) | + +Example of use: + +``` +ros2 launch los_guidance guidance_test.launch.py test_scenario:=circle +``` + +--- + + ## ROS Interfaces | Interface | Name | Type | Message-Type | diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 7c89a1b35..1a5368dbb 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -26,7 +26,7 @@ vector_field_los: # Common Guidance Parameters common: - active_los_method: 2 # 0: Adaptive LOS, 1: Proportional LOS, 2: Integral LOS, 3: Vector Field LOS + active_los_method: 2 # 0: Proportional LOS, 1: Integral LOS, 2: Adaptive LOS, 3: Vector Field LOS u_desired: 0.3 goal_reached_tol: 0.20 max_pitch_angle: 0.96 # 55 degrees @@ -34,9 +34,3 @@ common: slow_down_distance: 1.5 u_slow_min: 0.1 surge_rate_limit: 0.1 - surge_initialization: false - -# Debug Settings -debug: - enable_debug: true - debug_topic_name: "/los_guidance_debug" 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 fa0b878e2..50fc6af2c 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -114,7 +114,7 @@ class LosGuidanceNode : public rclcpp::Node { double max_pitch_angle_{}; bool slow_approach_{}; double slow_down_distance_{}; - bool surge_initialized_{}; + bool surge_initialized_{false}; double u_slow_min_{}; double commanded_surge_{}; double surge_rate_limit_{}; diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 5c0f8dc3a..156466aa3 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -236,7 +236,7 @@ void LosGuidanceNode::handle_accepted( const std::shared_ptr< rclcpp_action::ServerGoalHandle> goal_handle) { - execute(goal_handle); + std::thread{[this, goal_handle]() { execute(goal_handle); }}.detach(); } // Service Callback @@ -308,7 +308,6 @@ void LosGuidanceNode::parse_common_config(YAML::Node common_config) { slow_approach_ = common_config["slow_approach"].as(); slow_down_distance_ = common_config["slow_down_distance"].as(); u_slow_min_ = common_config["u_slow_min"].as(); - surge_initialized_ = common_config["surge_initialization"].as(); surge_rate_limit_ = common_config["surge_rate_limit"].as(); method_ = static_cast( @@ -398,23 +397,28 @@ 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); + if (debug_current_odom_) { + 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( - Eigen::Quaterniond(debug_current_odom_->pose.pose.orientation.w, - debug_current_odom_->pose.pose.orientation.x, - debug_current_odom_->pose.pose.orientation.y, - debug_current_odom_->pose.pose.orientation.z)); + vortex_msgs::msg::LOSGuidance 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_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 ((inputs_copy.current_position - inputs_copy.next_point) .as_vector() .norm() < goal_reached_tol_) { diff --git a/guidance/los_guidance/test/adaptive_los_test.cpp b/guidance/los_guidance/test/adaptive_los_test.cpp index 5fd71027a..dbf63eb04 100644 --- a/guidance/los_guidance/test/adaptive_los_test.cpp +++ b/guidance/los_guidance/test/adaptive_los_test.cpp @@ -20,72 +20,6 @@ class AdaptiveLosTest : public ::testing::Test { 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) { From 86c6f3bf742d1c2f28ac63792096bd943f13a4b3 Mon Sep 17 00:00:00 2001 From: Anbit Date: Wed, 25 Mar 2026 15:53:08 +0100 Subject: [PATCH 204/290] Add: New simulation test --- tests/simulator_tests/los_test/send_goal.py | 118 +++++++++++++++----- 1 file changed, 89 insertions(+), 29 deletions(-) diff --git a/tests/simulator_tests/los_test/send_goal.py b/tests/simulator_tests/los_test/send_goal.py index d9ad9770f..c07f9e53b 100644 --- a/tests/simulator_tests/los_test/send_goal.py +++ b/tests/simulator_tests/los_test/send_goal.py @@ -1,61 +1,121 @@ 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 LOSGuidanceClient(Node): def __init__(self): - super().__init__('los_guidance_client') - # Create the action client - self._action_client = ActionClient(self, LOSGuidance, '/orca/los_guidance') + super().__init__("los_guidance_client") + + self.declare_parameter("drone", "orca") + self.declare_parameter("x", 20.0) + self.declare_parameter("y", 20.0) + self.declare_parameter("z", 2.5) + + self.drone = self.get_parameter("drone").value + self.goal_x = float(self.get_parameter("x").value) + self.goal_y = float(self.get_parameter("y").value) + self.goal_z = float(self.get_parameter("z").value) + + self._action_client = ActionClient( + self, + LOSGuidance, + f"/{self.drone}/los_guidance", + ) + + self.get_logger().info(f"Using drone namespace: {self.drone}") self.send_goal() def send_goal(self): + self.get_logger().info("Waiting for action server...") + if not self._action_client.wait_for_server(timeout_sec=10.0): + self.get_logger().error("Action server not available") + self.shutdown_with_code(1) + return + goal_msg = LOSGuidance.Goal() - # Create a message with the goal - goal_msg.goal.point.x = 20.0 - goal_msg.goal.point.y = 20.0 - goal_msg.goal.point.z = 5.0 + header = Header() + header.frame_id = "world_ned" + header.stamp = self.get_clock().now().to_msg() + goal_msg.goal.header = header + + goal_msg.goal.point.x = self.goal_x + goal_msg.goal.point.y = self.goal_y + goal_msg.goal.point.z = self.goal_z + + self.get_logger().info( + f"Sending goal: x={self.goal_x:.2f}, y={self.goal_y:.2f}, z={self.goal_z:.2f}" + ) - # Send the goal asynchronously - self._action_client.wait_for_server(timeout_sec=10.0) - self.get_logger().info('Sending goal...') - self._send_goal_future = self._action_client.send_goal_async(goal_msg) + 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 goal_handle is None: + self.get_logger().error("Failed to send goal") + self.shutdown_with_code(1) + return + if not goal_handle.accepted: - self.get_logger().info('Goal rejected :(') + self.get_logger().error("Goal rejected") + self.shutdown_with_code(1) return - self.get_logger().info('Goal accepted :)') + 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}') - if result: - self.destroy_node() - if rclpy.ok(): - rclpy.shutdown() - exit(0) + result_msg = future.result() + + if result_msg is None: + self.get_logger().error("Did not receive result") + self.shutdown_with_code(1) + return + + result = result_msg.result + status = result_msg.status + + self.get_logger().info(f"Result status: {status}") + self.get_logger().info(f"Goal success: {result.success}") + + if result.success: + self.get_logger().info("Goal reached successfully") + self.shutdown_with_code(0) else: - self.get_logger().info('Goal failed :(') - self.destroy_node() - if rclpy.ok(): - rclpy.shutdown() - exit(1) + self.get_logger().error("Goal failed") + self.shutdown_with_code(1) + + def feedback_callback(self, feedback_msg): + self.get_logger().debug("Received feedback") + + def shutdown_with_code(self, code): + self.destroy_node() + if rclpy.ok(): + rclpy.shutdown() def main(args=None): rclpy.init(args=args) - action_client = LOSGuidanceClient() - rclpy.spin(action_client) + node = LOSGuidanceClient() + + try: + rclpy.spin(node) + except KeyboardInterrupt: + node.get_logger().info("Interrupted by user") + finally: + if rclpy.ok(): + node.destroy_node() + rclpy.shutdown() -if __name__ == '__main__': - main() +if __name__ == "__main__": + main() \ No newline at end of file From bf60cb3eaaa77edb189f621b32ecf259e8178826 Mon Sep 17 00:00:00 2001 From: Anbit Date: Wed, 25 Mar 2026 16:21:56 +0100 Subject: [PATCH 205/290] Final Final changes for PR --- tests/simulator_tests/los_test/send_goal.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/simulator_tests/los_test/send_goal.py b/tests/simulator_tests/los_test/send_goal.py index c07f9e53b..934ba93ab 100644 --- a/tests/simulator_tests/los_test/send_goal.py +++ b/tests/simulator_tests/los_test/send_goal.py @@ -118,4 +118,4 @@ def main(args=None): if __name__ == "__main__": - main() \ No newline at end of file + main() From 8ac7b2b441abc0bab74f2489c2896c687b66b79c Mon Sep 17 00:00:00 2001 From: Anbit Date: Wed, 25 Mar 2026 16:51:27 +0100 Subject: [PATCH 206/290] FInal FInal Final change before PR --- guidance/los_guidance/src/los_guidance_ros.cpp | 1 - .../los_guidance/test/vector_field_los_test.cpp | 14 +++++++------- 2 files changed, 7 insertions(+), 8 deletions(-) diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 156466aa3..b04d7121d 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -1,5 +1,4 @@ #include "los_guidance/los_guidance_ros.hpp" - #include #include #include diff --git a/guidance/los_guidance/test/vector_field_los_test.cpp b/guidance/los_guidance/test/vector_field_los_test.cpp index 956e9b5f0..34bbf1e6a 100644 --- a/guidance/los_guidance/test/vector_field_los_test.cpp +++ b/guidance/los_guidance/test/vector_field_los_test.cpp @@ -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 = 1.5; // needs tuning + params.k_p_v = 0.9; // needs tuning params.time_step = 0.01; return params; } @@ -69,8 +69,8 @@ TEST_F(VectorFieldLosTest, T08_test_commanded_angles) { 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); + EXPECT_LT(O.theta_d, 0.0); + EXPECT_GT(O.theta_d, -1.57); } // Test commanded angles when drone is over the track @@ -86,8 +86,8 @@ TEST_F(VectorFieldLosTest, T09_test_commanded_angles) { 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); + EXPECT_GT(O.theta_d, 0.0); + EXPECT_LT(O.theta_d, 1.57); } // Test commanded angles when drone is over and to the right of the track @@ -105,7 +105,7 @@ TEST_F(VectorFieldLosTest, T10_test_commanded_angles) { // Pitch cmd should be between -pi/2 and 0 EXPECT_LT(O.theta_d, 0.0); - EXPECT_GT(O.theta_d, -1.57); + EXPECT_GT(O.theta_d, 1.57); } } // namespace vortex::guidance::los From a0cc0754bcab5292b0d7e995fabdd4793b1d8ce9 Mon Sep 17 00:00:00 2001 From: Anbit Date: Wed, 25 Mar 2026 17:19:12 +0100 Subject: [PATCH 207/290] Finally Final changes before PR --- guidance/los_guidance/test/vector_field_los_test.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/guidance/los_guidance/test/vector_field_los_test.cpp b/guidance/los_guidance/test/vector_field_los_test.cpp index 34bbf1e6a..e11f7d3dd 100644 --- a/guidance/los_guidance/test/vector_field_los_test.cpp +++ b/guidance/los_guidance/test/vector_field_los_test.cpp @@ -104,8 +104,8 @@ TEST_F(VectorFieldLosTest, T10_test_commanded_angles) { 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); + EXPECT_GT(O.theta_d, 0.0); + EXPECT_LT(O.theta_d, 1.57); } } // namespace vortex::guidance::los From ffff21f1fbd736be776adf99ad56da709d958367 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Wed, 25 Mar 2026 21:14:38 +0100 Subject: [PATCH 208/290] rebase --- .github/workflows/simulator-test.yml | 5 ++- .../waypoint_navigation/check_goal.py | 5 ++- .../waypoint_navigation/send_goal.py | 5 ++- .../waypoint_navigation/simulator_test.sh | 41 +++++++++---------- 4 files changed, 32 insertions(+), 24 deletions(-) diff --git a/.github/workflows/simulator-test.yml b/.github/workflows/simulator-test.yml index c75075c0d..82a077fcb 100644 --- a/.github/workflows/simulator-test.yml +++ b/.github/workflows/simulator-test.yml @@ -16,6 +16,9 @@ jobs: setup_script: "tests/setup.sh" pre_test_script: "scripts/ci_install_dependencies.sh" test_scripts: '[ - "tests/simulator_tests/waypoint_navigation/simulator_test.sh", + "tests/simulator_tests/waypoint_navigation/simulator_test.sh nautilus qp", + "tests/simulator_tests/waypoint_navigation/simulator_test.sh nautilus pseudoinverse", + "tests/simulator_tests/waypoint_navigation/simulator_test.sh orca qp", + "tests/simulator_tests/waypoint_navigation/simulator_test.sh orca pseudoinverse", "tests/simulator_tests/waypoint_manager_test/simulator_test.sh", ]' diff --git a/tests/simulator_tests/waypoint_navigation/check_goal.py b/tests/simulator_tests/waypoint_navigation/check_goal.py index 69bf0a71a..995c27f6c 100644 --- a/tests/simulator_tests/waypoint_navigation/check_goal.py +++ b/tests/simulator_tests/waypoint_navigation/check_goal.py @@ -1,5 +1,6 @@ import math import os +import sys import time import rclpy @@ -9,6 +10,8 @@ from rclpy.qos import QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy from vortex_utils.python_utils import quat_to_euler +namespace = sys.argv[1] if len(sys.argv) > 1 else "nautilus" + best_effort_qos = QoSProfile( history=QoSHistoryPolicy.KEEP_LAST, depth=1, @@ -35,7 +38,7 @@ def __init__(self): super().__init__('check_goal_node') self.pose_sub_ = self.create_subscription( PoseWithCovarianceStamped, - '/nautilus/pose', + f'/{namespace}/pose', self.pose_callback, best_effort_qos, ) diff --git a/tests/simulator_tests/waypoint_navigation/send_goal.py b/tests/simulator_tests/waypoint_navigation/send_goal.py index c9d7287ad..93c17b57b 100644 --- a/tests/simulator_tests/waypoint_navigation/send_goal.py +++ b/tests/simulator_tests/waypoint_navigation/send_goal.py @@ -1,4 +1,5 @@ import random +import sys import rclpy import yaml @@ -7,6 +8,8 @@ from vortex_msgs.action import ReferenceFilterWaypoint from vortex_utils.python_utils import PoseData, euler_to_quat +namespace = sys.argv[1] if len(sys.argv) > 1 else "nautilus" + def randomize_pose() -> PoseData: pose: PoseData = PoseData() @@ -25,7 +28,7 @@ def __init__(self): super().__init__('reference_filter_waypoint_client') self._action_client = ActionClient( - self, ReferenceFilterWaypoint, '/nautilus/reference_filter' + self, ReferenceFilterWaypoint, f'/{namespace}/reference_filter' ) self.send_goal() diff --git a/tests/simulator_tests/waypoint_navigation/simulator_test.sh b/tests/simulator_tests/waypoint_navigation/simulator_test.sh index 79c2db117..e85c34216 100755 --- a/tests/simulator_tests/waypoint_navigation/simulator_test.sh +++ b/tests/simulator_tests/waypoint_navigation/simulator_test.sh @@ -2,6 +2,10 @@ set -e set -o pipefail +DRONE="${1:-nautilus}" +SOLVER_TYPE="${2:-qp}" + +echo "===== Waypoint Navigation Test: drone=$DRONE, solver_type=$SOLVER_TYPE =====" echo "Setting up ROS 2 environment..." . /opt/ros/humble/setup.sh . "${WORKSPACE:-$HOME/ros2_ws}/install/setup.bash" @@ -13,30 +17,25 @@ SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" # Function to terminate processes safely on error cleanup() { echo "Error detected. Cleaning up..." - kill -TERM -"$SIM_PID" -"$NAUTILUS_PID" -"$CONTROLLER_PID" -"$FILTER_PID" -"$OP_MODE_PID" || true + kill -TERM -"$SIM_PID" -"$CONTROLLER_PID" || true exit 1 } trap cleanup ERR -setsid ros2 bag record -o ${WORKSPACE}/bags/recording -s mcap -a & +setsid ros2 bag record -o ${WORKSPACE}/bags/recording_${DRONE}_${SOLVER_TYPE} -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:=nautilus_no_gpu & +# Launch Stonefish Simulator + Drone (combined launch) +setsid ros2 launch stonefish_sim vortex_sim_launch.py rendering:=false scenario:=${DRONE}_no_gpu drone:=${DRONE} solver_type:=${SOLVER_TYPE} & SIM_PID=$! echo "Launched simulator with PID: $SIM_PID" -# Launch NAUTILUS Simulation -setsid ros2 launch stonefish_sim drone_sim.launch.py & -NAUTILUS_PID=$! -echo "Launched nautilus with PID: $NAUTILUS_PID" - echo "Waiting for simulator to start..." -timeout 30s bash -c ' - while ! ros2 topic list | grep -q "/nautilus/odom"; do +timeout 30s bash -c " + while ! ros2 topic list | grep -q '/${DRONE}/odom'; do sleep 1 - done || true' + done || true" echo "Simulator started" # Check for ROS errors in logs @@ -47,11 +46,11 @@ fi # Wait for odometry data echo "Waiting for odom data..." -timeout 10s ros2 topic echo /nautilus/odom --once +timeout 10s ros2 topic echo /${DRONE}/odom --once echo "Got odom data" echo "Waiting for sim interface to start..." -timeout 30s bash -c 'until ros2 topic list | grep -q "/nautilus/pose"; do sleep 1; done' +timeout 30s bash -c "until ros2 topic list | grep -q '/${DRONE}/pose'; do sleep 1; done" echo "Simulator started" # Check for ROS errors again @@ -62,11 +61,11 @@ fi # Wait for pose data echo "Waiting for pose data..." -timeout 10s ros2 topic echo /nautilus/pose --once +timeout 10s ros2 topic echo /${DRONE}/pose --once echo "Got pose data" # Launch controller and reference filter -setsid ros2 launch auv_setup dp.launch.py & +setsid ros2 launch auv_setup dp.launch.py drone:=${DRONE} & CONTROLLER_PID=$! echo "Launched controller and reference filter with PID: $CONTROLLER_PID" @@ -81,19 +80,19 @@ sleep 5 # Set operation mode echo "Turning off killswitch and setting operation mode to autonomous mode" -ros2 service call /nautilus/set_killswitch vortex_msgs/srv/SetKillswitch "{killswitch_on: false}" -ros2 service call /nautilus/set_operation_mode vortex_msgs/srv/SetOperationMode "{requested_operation_mode: {operation_mode: 1}}" +ros2 service call /${DRONE}/set_killswitch vortex_msgs/srv/SetKillswitch "{killswitch_on: false}" +ros2 service call /${DRONE}/set_operation_mode vortex_msgs/srv/SetOperationMode "{requested_operation_mode: {operation_mode: 1}}" echo "Sleeping for 5 seconds to make sure operation is stable..." sleep 5 # Send waypoint goal echo "Sending goal" -python3 "$SCRIPT_DIR/send_goal.py" +python3 "$SCRIPT_DIR/send_goal.py" "$DRONE" # Check if goal reached echo "Checking if goal reached" -python3 "$SCRIPT_DIR/check_goal.py" +python3 "$SCRIPT_DIR/check_goal.py" "$DRONE" if [ $? -ne 0 ]; then echo "Test failed: Drone did not reach goal." @@ -103,6 +102,6 @@ else fi # Terminate processes -kill -TERM -"$SIM_PID" -"$NAUTILUS_PID" -"$CONTROLLER_PID" -"$BAG_PID" -"$OP_MODE_PID" +kill -TERM -"$SIM_PID" -"$CONTROLLER_PID" -"$BAG_PID" echo "Test completed successfully." From 0eb1234ab2f84cc48f0304b9c2c29f293863160a Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Wed, 25 Mar 2026 21:48:36 +0100 Subject: [PATCH 209/290] waypoint sim test wrapper scripts --- .github/workflows/simulator-test.yml | 8 ++++---- .../waypoint_navigation/nautilus_pseudoinverse.sh | 3 +++ tests/simulator_tests/waypoint_navigation/nautilus_qp.sh | 3 +++ .../waypoint_navigation/orca_pseudoinverse.sh | 3 +++ tests/simulator_tests/waypoint_navigation/orca_qp.sh | 3 +++ 5 files changed, 16 insertions(+), 4 deletions(-) create mode 100755 tests/simulator_tests/waypoint_navigation/nautilus_pseudoinverse.sh create mode 100755 tests/simulator_tests/waypoint_navigation/nautilus_qp.sh create mode 100755 tests/simulator_tests/waypoint_navigation/orca_pseudoinverse.sh create mode 100755 tests/simulator_tests/waypoint_navigation/orca_qp.sh diff --git a/.github/workflows/simulator-test.yml b/.github/workflows/simulator-test.yml index 82a077fcb..213693412 100644 --- a/.github/workflows/simulator-test.yml +++ b/.github/workflows/simulator-test.yml @@ -16,9 +16,9 @@ jobs: setup_script: "tests/setup.sh" pre_test_script: "scripts/ci_install_dependencies.sh" test_scripts: '[ - "tests/simulator_tests/waypoint_navigation/simulator_test.sh nautilus qp", - "tests/simulator_tests/waypoint_navigation/simulator_test.sh nautilus pseudoinverse", - "tests/simulator_tests/waypoint_navigation/simulator_test.sh orca qp", - "tests/simulator_tests/waypoint_navigation/simulator_test.sh orca pseudoinverse", + "tests/simulator_tests/waypoint_navigation/nautilus_qp.sh", + "tests/simulator_tests/waypoint_navigation/nautilus_pseudoinverse.sh", + "tests/simulator_tests/waypoint_navigation/orca_qp.sh", + "tests/simulator_tests/waypoint_navigation/orca_pseudoinverse.sh", "tests/simulator_tests/waypoint_manager_test/simulator_test.sh", ]' diff --git a/tests/simulator_tests/waypoint_navigation/nautilus_pseudoinverse.sh b/tests/simulator_tests/waypoint_navigation/nautilus_pseudoinverse.sh new file mode 100755 index 000000000..0932a998e --- /dev/null +++ b/tests/simulator_tests/waypoint_navigation/nautilus_pseudoinverse.sh @@ -0,0 +1,3 @@ +#!/bin/bash +SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" +exec "$SCRIPT_DIR/simulator_test.sh" nautilus pseudoinverse diff --git a/tests/simulator_tests/waypoint_navigation/nautilus_qp.sh b/tests/simulator_tests/waypoint_navigation/nautilus_qp.sh new file mode 100755 index 000000000..1da7ceaa7 --- /dev/null +++ b/tests/simulator_tests/waypoint_navigation/nautilus_qp.sh @@ -0,0 +1,3 @@ +#!/bin/bash +SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" +exec "$SCRIPT_DIR/simulator_test.sh" nautilus qp diff --git a/tests/simulator_tests/waypoint_navigation/orca_pseudoinverse.sh b/tests/simulator_tests/waypoint_navigation/orca_pseudoinverse.sh new file mode 100755 index 000000000..9e84147b9 --- /dev/null +++ b/tests/simulator_tests/waypoint_navigation/orca_pseudoinverse.sh @@ -0,0 +1,3 @@ +#!/bin/bash +SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" +exec "$SCRIPT_DIR/simulator_test.sh" orca pseudoinverse diff --git a/tests/simulator_tests/waypoint_navigation/orca_qp.sh b/tests/simulator_tests/waypoint_navigation/orca_qp.sh new file mode 100755 index 000000000..9cd5c9cbc --- /dev/null +++ b/tests/simulator_tests/waypoint_navigation/orca_qp.sh @@ -0,0 +1,3 @@ +#!/bin/bash +SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" +exec "$SCRIPT_DIR/simulator_test.sh" orca qp From d23e4c41898ddef2afef169d0e277ac8be97b027 Mon Sep 17 00:00:00 2001 From: forrisdahl Date: Thu, 26 Mar 2026 08:53:57 +0100 Subject: [PATCH 210/290] feat: convert from i2c to uart Since CAN is not working from motor control PCB we have to use uart as a temporary solution --- .../config/thruster_interface_auv_config.yaml | 9 +- .../thruster_interface_auv_driver.hpp | 108 +++++++------ .../thruster_interface_auv_ros.hpp | 50 +++--- .../src/thruster_interface_auv_driver.cpp | 142 +++++++++++----- .../src/thruster_interface_auv_ros.cpp | 152 +++++++++++------- 5 files changed, 296 insertions(+), 165 deletions(-) diff --git a/motion/thruster_interface_auv/config/thruster_interface_auv_config.yaml b/motion/thruster_interface_auv/config/thruster_interface_auv_config.yaml index 295e1ef87..7d31c9fc3 100644 --- a/motion/thruster_interface_auv/config/thruster_interface_auv_config.yaml +++ b/motion/thruster_interface_auv/config/thruster_interface_auv_config.yaml @@ -20,9 +20,10 @@ # LEFT: [2.55792, 27.51178, 147.67001, 1472.23069] # RIGHT: [1.33421, -18.75129, 121.29079, 1528.99886] - i2c: - bus: 7 - address: 0x21 + uart: + device: "/dev/ttyUSB0" + baud_rate: 115200 + packet_id: 1 debug: - flag: False + flag: True 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 8b11f913d..b8ce765a6 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,17 +1,10 @@ #ifndef THRUSTER_INTERFACE_AUV__THRUSTER_INTERFACE_AUV_DRIVER_HPP_ #define THRUSTER_INTERFACE_AUV__THRUSTER_INTERFACE_AUV_DRIVER_HPP_ -#include -#include -#include -#include -#include -#include -#include +#include + #include -#include -#include -#include +#include #include #include @@ -19,10 +12,10 @@ * @brief struct to hold the parameters for a single thruster */ struct ThrusterParameters { - uint8_t mapping; - int8_t direction; - uint16_t pwm_min; - uint16_t pwm_max; + std::uint8_t mapping; + std::int8_t direction; + std::uint16_t pwm_min; + std::uint16_t pwm_max; }; enum PolySide { @@ -33,8 +26,7 @@ enum PolySide { /** * @brief class instantiated by ThrusterInterfaceAUVNode to control the * thrusters, takes the thruster forces and converts them to PWM signals to be - * sent via I2C to the ESCs (PCA9685 Adafruit 16-Channel 12-bit PWM/Servo - * Driver) + * sent via UART to the ESC controller. * * @details Based on the datasheets found in /resources, approximate the map * with a piecewise (>0 and <0) third order polynomial. @@ -44,56 +36,55 @@ enum PolySide { * all the handling of the other voltages to save resources. Could be * re-implemented in the future for more flexibility if we ever need it to * operate at different voltages in different situations. + * + * @note Over UART, the PWM values are packed into a framed packet: + * [magic][id][length][payload][checksum], where the payload is 8 uint16_t. */ class ThrusterInterfaceAUVDriver { - public: +public: ~ThrusterInterfaceAUVDriver(); /** * @brief called from ThrusterInterfaceAUVNode .cpp when instantiating the * object, initializes all the params. * - * @param i2c_bus bus number used to communicate - * @param pico_i2c_address i2c address of the ESC that drive the + * @param serial_device serial device used to communicate + * (for example /dev/ttyUSB0) + * @param baud_rate UART baud rate + * @param packet_id packet ID sent in the UART frame * @param thruster_parameters describe mapping, direction, min and max pwm - * value for each thruster - * @param left_coeffs LEFT(<0) and RIGHT(>0) third order - * @param right_coeffs - * polynomial coefficients + * value for each thruster + * @param right_coeffs RIGHT(>0) third order polynomial coefficients + * @param left_coeffs LEFT(<0) third order polynomial coefficients */ - ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver( - std::int16_t i2c_bus, - int pico_i2c_address, + ThrusterInterfaceAUVDriver( + const std::string& serial_device, + unsigned int baud_rate, + std::uint8_t packet_id, const std::vector& thruster_parameters, const std::vector& right_coeffs, const std::vector& left_coeffs); /** - * @brief initializes i2c + * @brief initializes UART * @return 0 on success, negative number on failure */ - int init_i2c(); + int init_uart(); + /** * @brief calls both 1) interpolate_forces_to_pwm() to * convert the thruster forces to PWM values and 2) send_data_to_escs() to - * send them to the ESCs via I2C + * send them over UART * * @param thruster_forces_array vector of forces for each thruster * - * @return std::vector vector of pwm values sent to each thruster + * @return std::optional> vector of pwm values sent to + * each thruster, or std::nullopt on failure */ - std::vector drive_thrusters( + std::optional> drive_thrusters( const std::vector& thruster_forces_array); - private: - int bus_fd_; - int i2c_bus_; - int pico_i2c_address_; - std::vector thruster_parameters_; - std::vector right_coeffs_; - std::vector left_coeffs_; - uint16_t idle_pwm_value_; - +private: /** * @brief only take the thruster forces and return PWM values * @@ -102,16 +93,14 @@ class ThrusterInterfaceAUVDriver { * @return std::vector vector of pwm values sent to each thruster * if we want to publish them for debug purposes */ - std::vector interpolate_forces_to_pwm( + std::vector interpolate_forces_to_pwm( const std::vector& thruster_forces_array); /** * @brief scalar map from force to pwm x->y. Choose coefficients [LEFT] or * [RIGHT] based on sign(force) * - * @param force scalar force value - * @param coeffs std::vector> coeffs contains the pair - * of coefficients + * @param force scalar force value * * @return std::uint16_t scalar pwm value */ @@ -119,6 +108,7 @@ class ThrusterInterfaceAUVDriver { /** * @brief compute y = a*x^3 + b*x^2 + c*x + d + * * @param force x * @param coeffs a,b,c,d * @@ -128,12 +118,25 @@ class ThrusterInterfaceAUVDriver { /** * @brief only takes the pwm values computed and sends them - * to the ESCs via I2C + * over UART as a framed packet * * @param thruster_pwm_array vector of pwm values to send * @return 0 on success, -1 on failure */ - int send_data_to_escs(const std::vector& thruster_pwm_array); + int send_data_to_escs(const std::vector& thruster_pwm_array); + + /** + * @brief create UART packet with format: + * [magic][id][length][payload][checksum] + * + * @param id packet ID + * @param thruster_pwm_array vector of 8 pwm values to send as payload + * + * @return std::vector serialized packet bytes + */ + std::vector create_packet( + std::uint8_t id, + const std::vector& thruster_pwm_array) const; /** * @brief convert Newtons to Kg @@ -143,6 +146,19 @@ class ThrusterInterfaceAUVDriver { * @return double Kg */ static constexpr double to_kg(double force) { return force / 9.80665; } + +private: + std::string serial_device_; + unsigned int baud_rate_; + std::uint8_t packet_id_; + + asio::io_context io_; + asio::serial_port serial_{io_}; + + std::vector thruster_parameters_; + std::vector right_coeffs_; + std::vector left_coeffs_; + std::uint16_t idle_pwm_value_{1500}; }; #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 9050610f2..b849f9f71 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 @@ -2,21 +2,23 @@ #define THRUSTER_INTERFACE_AUV__THRUSTER_INTERFACE_AUV_ROS_HPP_ #include +#include +#include + #include #include #include #include -#include -#include #include + #include "thruster_interface_auv/thruster_interface_auv_driver.hpp" class ThrusterInterfaceAUVNode : public rclcpp::Node { - public: +public: ThrusterInterfaceAUVNode( const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); - private: +private: /** * @brief periodically receive thruster forces topic * @@ -26,7 +28,7 @@ class ThrusterInterfaceAUVNode : public rclcpp::Node { const vortex_msgs::msg::ThrusterForces::SharedPtr msg); /** - * @brief publish and send pwm commands to thrusters. Sinchronous with + * @brief publish and send PWM commands to thrusters. Synchronous with * thruster_forces_callback */ void pwm_callback(); @@ -43,7 +45,6 @@ class ThrusterInterfaceAUVNode : public rclcpp::Node { /** * @brief Initialize the parameter handler and a parameter event callback. - * */ void initialize_parameter_handler(); @@ -52,28 +53,38 @@ class ThrusterInterfaceAUVNode : public rclcpp::Node { */ void set_publisher(); - int i2c_bus_; - int i2c_address_; + /** + * @brief specific callback for updating debug_flag. + */ + void update_debug_flag(const rclcpp::Parameter& p); + +private: + std::string serial_device_; + unsigned int baud_rate_; + std::uint8_t packet_id_; + std::string subscriber_topic_name_; std::string publisher_topic_name_; + std::vector thruster_parameters_; std::vector left_coeffs_; std::vector right_coeffs_; std::vector thruster_forces_array_; - bool debug_flag_; + bool debug_flag_{false}; std::unique_ptr - thruster_driver_; ///<-- pwm driver - rclcpp::Subscription:: - SharedPtr ///<-- thruster forces subscriber - thruster_forces_subscriber_; - rclcpp::Publisher< - std_msgs::msg::Int16MultiArray>::SharedPtr ///<-- pwm publisher - thruster_pwm_publisher_; + thruster_driver_; ///<-- UART/USART thruster driver + + rclcpp::Subscription::SharedPtr + thruster_forces_subscriber_; ///<-- thruster forces subscriber + + rclcpp::Publisher::SharedPtr + thruster_pwm_publisher_; ///<-- pwm publisher + rclcpp::TimerBase::SharedPtr watchdog_timer_; rclcpp::Time last_msg_time_; - rclcpp::Duration watchdog_timeout_ = std::chrono::seconds(1); + rclcpp::Duration watchdog_timeout_ = rclcpp::Duration::from_seconds(1.0); bool watchdog_triggered_ = false; /** @@ -91,11 +102,6 @@ class ThrusterInterfaceAUVNode : public rclcpp::Node { * made with the parameter event handler (`param_handler_`). */ rclcpp::ParameterCallbackHandle::SharedPtr debug_flag_parameter_cb; - - /** - * specific callback for updating debug_flag. - */ - void update_debug_flag(const rclcpp::Parameter& p); }; #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 b82584a59..6ae0d10cd 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp @@ -1,48 +1,73 @@ #include "thruster_interface_auv/thruster_interface_auv_driver.hpp" -#include + +#include #include -#include +#include ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver( - std::int16_t i2c_bus, - int pico_i2c_address, + const std::string& serial_device, + unsigned int baud_rate, + std::uint8_t packet_id, const std::vector& thruster_parameters, const std::vector& right_coeffs, const std::vector& left_coeffs) - : i2c_bus_(i2c_bus), - pico_i2c_address_(pico_i2c_address), + : serial_device_(serial_device), + baud_rate_(baud_rate), + packet_id_(packet_id), thruster_parameters_(thruster_parameters), right_coeffs_(right_coeffs), left_coeffs_(left_coeffs) { idle_pwm_value_ = - (calc_poly(0, left_coeffs_) + calc_poly(0, right_coeffs_)) / 2; + static_cast( + (calc_poly(0.0, left_coeffs_) + calc_poly(0.0, right_coeffs_)) / 2); } -int ThrusterInterfaceAUVDriver::init_i2c() { - std::string i2c_filename = std::format("/dev/i2c-{}", i2c_bus_); - bus_fd_ = open(i2c_filename.c_str(), O_RDWR); - if (bus_fd_ < 0) { - return bus_fd_; - } +int ThrusterInterfaceAUVDriver::init_uart() { + std::error_code ec; - if (ioctl(bus_fd_, I2C_SLAVE, pico_i2c_address_) < 0) { + serial_.open(serial_device_, ec); + if (ec) { + std::cerr << "Failed to open serial port " << serial_device_ + << ": " << ec.message() << '\n'; return -1; } + + serial_.set_option(asio::serial_port_base::baud_rate(baud_rate_), ec); + if (ec) return -1; + + serial_.set_option(asio::serial_port_base::character_size(8), ec); + if (ec) return -1; + + serial_.set_option( + asio::serial_port_base::parity(asio::serial_port_base::parity::none), ec); + if (ec) return -1; + + serial_.set_option( + asio::serial_port_base::stop_bits(asio::serial_port_base::stop_bits::one), ec); + if (ec) return -1; + + serial_.set_option( + asio::serial_port_base::flow_control( + asio::serial_port_base::flow_control::none), + ec); + if (ec) return -1; + return 0; } ThrusterInterfaceAUVDriver::~ThrusterInterfaceAUVDriver() { - if (bus_fd_ >= 0) { - send_data_to_escs(std::vector(thruster_parameters_.size(), - idle_pwm_value_)); - close(bus_fd_); + if (serial_.is_open()) { + send_data_to_escs( + std::vector(thruster_parameters_.size(), idle_pwm_value_)); + + std::error_code ec; + serial_.close(ec); } } std::vector ThrusterInterfaceAUVDriver::interpolate_forces_to_pwm( const std::vector& thruster_forces_array) { - std::vector pwm; - pwm.resize(thruster_forces_array.size()); + std::vector pwm(thruster_forces_array.size()); for (std::size_t i = 0; i < thruster_forces_array.size(); ++i) { const double force_in_kg = to_kg(thruster_forces_array[i]); @@ -53,32 +78,77 @@ std::vector ThrusterInterfaceAUVDriver::interpolate_forces_to_pwm( } std::uint16_t ThrusterInterfaceAUVDriver::force_to_pwm(double force) { - if (force < 0) { + if (force < 0.0) { return calc_poly(force, left_coeffs_); - } else if (force > 0) { + } + if (force > 0.0) { return calc_poly(force, right_coeffs_); - } else { - return idle_pwm_value_; // 1500 } + return idle_pwm_value_; } std::uint16_t ThrusterInterfaceAUVDriver::calc_poly( double force, const std::vector& coeffs) { - return static_cast(coeffs[0] * std::pow(force, 3) + - coeffs[1] * std::pow(force, 2) + - coeffs[2] * force + coeffs[3]); + return static_cast( + coeffs[0] * std::pow(force, 3) + + coeffs[1] * std::pow(force, 2) + + coeffs[2] * force + + coeffs[3]); +} + +std::vector ThrusterInterfaceAUVDriver::create_packet( + std::uint8_t id, + const std::vector& thruster_pwm_array) const { + constexpr std::uint8_t magic = 0xAA; + constexpr std::size_t expected_thrusters = 8; + + if (thruster_pwm_array.size() != expected_thrusters) { + return {}; + } + + std::vector packet; + packet.reserve(1 + 1 + 1 + expected_thrusters * 2 + 1); + + packet.push_back(magic); + packet.push_back(id); + + const std::uint8_t payload_length = + static_cast(thruster_pwm_array.size() * sizeof(std::uint16_t)); + packet.push_back(payload_length); + + for (std::uint16_t value : thruster_pwm_array) { + // little-endian + packet.push_back(static_cast(value & 0xFF)); + packet.push_back(static_cast((value >> 8) & 0xFF)); + } + + std::uint8_t checksum = 0; + for (std::uint8_t byte : packet) { + checksum = static_cast(checksum + byte); + } + + packet.push_back(checksum); + return packet; } int ThrusterInterfaceAUVDriver::send_data_to_escs( const std::vector& thruster_pwm_array) { - constexpr std::size_t i2c_data_size = 8 * 2; - std::array i2c_data_array; + if (!serial_.is_open()) { + return -1; + } + + const auto packet = create_packet(packet_id_, thruster_pwm_array); + if (packet.empty()) { + return -1; + } - std::memcpy(i2c_data_array.data(), thruster_pwm_array.data(), - i2c_data_size); + std::error_code ec; + const auto bytes_written = asio::write(serial_, asio::buffer(packet), ec); - if (write(bus_fd_, i2c_data_array.data(), i2c_data_size) != i2c_data_size) { + if (ec || bytes_written != packet.size()) { + std::cerr << "UART write failed: " + << (ec ? ec.message() : "short write") << '\n'; return -1; } @@ -87,13 +157,11 @@ int ThrusterInterfaceAUVDriver::send_data_to_escs( std::optional> ThrusterInterfaceAUVDriver::drive_thrusters( const std::vector& thruster_forces_array) { - std::vector mapped_forces(thruster_forces_array.size()); + std::vector mapped_forces(thruster_parameters_.size()); for (std::size_t i = 0; i < thruster_parameters_.size(); ++i) { const auto& param = thruster_parameters_[i]; - const std::size_t idx = param.mapping; - const double raw_force = thruster_forces_array[idx]; mapped_forces[i] = raw_force * param.direction; } @@ -101,8 +169,8 @@ std::optional> ThrusterInterfaceAUVDriver::drive_thrusters std::vector thruster_pwm_array = interpolate_forces_to_pwm(mapped_forces); - if (send_data_to_escs(thruster_pwm_array)) { - return {}; + if (send_data_to_escs(thruster_pwm_array) != 0) { + return std::nullopt; } return thruster_pwm_array; 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 8938e333f..0bda24eb8 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp @@ -1,9 +1,15 @@ #include "thruster_interface_auv/thruster_interface_auv_ros.hpp" -#include + #include -#include +#include #include +#include +#include +#include +#include +#include + const auto start_message = R"( _____ _ _ ___ _ __ |_ _| |__ _ __ _ _ ___| |_ ___ _ __ |_ _|_ __ | |_ ___ _ __ / _| __ _ ___ ___ @@ -32,15 +38,26 @@ ThrusterInterfaceAUVNode::ThrusterInterfaceAUVNode( vortex::utils::qos_profiles::reliable_profile(1)); thruster_driver_ = std::make_unique( - i2c_bus_, i2c_address_, thruster_parameters_, right_coeffs_, + serial_device_, + baud_rate_, + packet_id_, + thruster_parameters_, + right_coeffs_, left_coeffs_); - thruster_driver_.init_i2c(); - thruster_forces_array_ = std::vector(8, 0.00); + if (thruster_driver_->init_uart() != 0) { + spdlog::error("Failed to initialize UART thruster driver"); + } else { + spdlog::info("UART thruster driver initialized on {} @ {} baud, packet id 0x{:02X}", + serial_device_, baud_rate_, packet_id_); + } + + thruster_forces_array_ = std::vector(8, 0.0); watchdog_timer_ = this->create_wall_timer( std::chrono::milliseconds(500), std::bind(&ThrusterInterfaceAUVNode::watchdog_callback, this)); + last_msg_time_ = this->now(); this->initialize_parameter_handler(); @@ -58,29 +75,38 @@ void ThrusterInterfaceAUVNode::thruster_forces_callback( } void ThrusterInterfaceAUVNode::pwm_callback() { - std::vector thruster_pwm_array = - thruster_driver_->drive_thrusters(this->thruster_forces_array_); + auto thruster_pwm_array_opt = + thruster_driver_->drive_thrusters(thruster_forces_array_); - if (thruster_pwm_array.has_value() == false){ - spdlog::warn("Sending PWM values to thrusters failed"); + if (!thruster_pwm_array_opt.has_value()) { + spdlog::warn("Sending PWM values to thrusters failed"); + return; } - if (debug_flag_) { + const auto& thruster_pwm_array = thruster_pwm_array_opt.value(); + std_msgs::msg::Int16MultiArray pwm_message; - pwm_message.data = std::vector(thruster_pwm_array.value().begin(), - thruster_pwm_array.value().end()); + pwm_message.data = std::vector( + thruster_pwm_array.begin(), thruster_pwm_array.end()); + thruster_pwm_publisher_->publish(pwm_message); } } void ThrusterInterfaceAUVNode::watchdog_callback() { - auto now = this->now(); + const auto now = this->now(); + if ((now - last_msg_time_) >= watchdog_timeout_ && !watchdog_triggered_) { - thruster_forces_array_.assign(8, 0.00); - thruster_driver_->drive_thrusters(thruster_forces_array_); + thruster_forces_array_.assign(8, 0.0); + + if (!thruster_driver_->drive_thrusters(thruster_forces_array_).has_value()) { + spdlog::warn("Watchdog triggered, but failed to send zero command to thrusters"); + } else { + spdlog::warn("Watchdog triggered, all thrusters set to 0.0"); + } + watchdog_triggered_ = true; - spdlog::warn("Watchdog triggered, all thrusters set to 0.00"); } } @@ -88,8 +114,9 @@ void ThrusterInterfaceAUVNode::initialize_parameter_handler() { param_handler_ = std::make_shared(this); debug_flag_parameter_cb = param_handler_->add_parameter_callback( - "debug.flag", std::bind(&ThrusterInterfaceAUVNode::update_debug_flag, - this, std::placeholders::_1)); + "debug.flag", + std::bind(&ThrusterInterfaceAUVNode::update_debug_flag, this, + std::placeholders::_1)); } void ThrusterInterfaceAUVNode::update_debug_flag(const rclcpp::Parameter& p) { @@ -100,77 +127,90 @@ void ThrusterInterfaceAUVNode::update_debug_flag(const rclcpp::Parameter& p) { } void ThrusterInterfaceAUVNode::extract_all_parameters() { - this->declare_parameter>( + this->declare_parameter>( "propulsion.thrusters.thruster_to_pin_mapping"); - this->declare_parameter>( + this->declare_parameter>( "propulsion.thrusters.thruster_direction"); - this->declare_parameter>( + this->declare_parameter>( "propulsion.thrusters.thruster_PWM_min"); - this->declare_parameter>( + this->declare_parameter>( "propulsion.thrusters.thruster_PWM_max"); - // approx poly coeffs for 16V from thruster_interface_auv.yaml + // Approx poly coeffs for 16V from thruster_interface_auv.yaml this->declare_parameter>("coeffs.16V.LEFT"); this->declare_parameter>("coeffs.16V.RIGHT"); - this->declare_parameter("i2c.bus"); - this->declare_parameter("i2c.address"); + this->declare_parameter("uart.device"); + this->declare_parameter("uart.baud_rate"); + this->declare_parameter("uart.packet_id"); this->declare_parameter("topics.thruster_forces"); this->declare_parameter("topics.pwm_output"); this->declare_parameter("debug.flag"); - this->declare_parameter("propulsion.thrusters.watchdog_timeout"); - //----------------------------------------------------------------------- - - auto thruster_mapping = + const auto thruster_mapping = this->get_parameter("propulsion.thrusters.thruster_to_pin_mapping") .as_integer_array(); - auto thruster_direction = + const auto thruster_direction = this->get_parameter("propulsion.thrusters.thruster_direction") .as_integer_array(); - auto thruster_PWM_min = + const auto thruster_pwm_min = this->get_parameter("propulsion.thrusters.thruster_PWM_min") .as_integer_array(); - auto thruster_PWM_max = + const auto thruster_pwm_max = this->get_parameter("propulsion.thrusters.thruster_PWM_max") .as_integer_array(); - this->left_coeffs_ = - this->get_parameter("coeffs.16V.LEFT").as_double_array(); - this->right_coeffs_ = - this->get_parameter("coeffs.16V.RIGHT").as_double_array(); + left_coeffs_ = this->get_parameter("coeffs.16V.LEFT").as_double_array(); + right_coeffs_ = this->get_parameter("coeffs.16V.RIGHT").as_double_array(); - this->i2c_bus_ = this->get_parameter("i2c.bus").as_int(); - this->i2c_address_ = this->get_parameter("i2c.address").as_int(); + serial_device_ = this->get_parameter("uart.device").as_string(); + baud_rate_ = static_cast( + this->get_parameter("uart.baud_rate").as_int()); + packet_id_ = static_cast( + this->get_parameter("uart.packet_id").as_int()); - this->subscriber_topic_name_ = + subscriber_topic_name_ = this->get_parameter("topics.thruster_forces").as_string(); - this->publisher_topic_name_ = + publisher_topic_name_ = this->get_parameter("topics.pwm_output").as_string(); - this->debug_flag_ = this->get_parameter("debug.flag").as_bool(); + debug_flag_ = this->get_parameter("debug.flag").as_bool(); + + const auto thruster_count = thruster_mapping.size(); - auto create_thruster_parameters = [&](const int64_t& mapping, - const int64_t& direction) { - size_t index = &mapping - &thruster_mapping[0]; - return ThrusterParameters{ - static_cast(mapping), static_cast(direction), - static_cast(thruster_PWM_min[index]), - static_cast(thruster_PWM_max[index])}; - }; + if (thruster_direction.size() != thruster_count || + thruster_pwm_min.size() != thruster_count || + thruster_pwm_max.size() != thruster_count) { + throw std::runtime_error( + "Thruster parameter arrays must all have the same length"); + } + + if (thruster_count != 8) { + spdlog::warn( + "UART packet format expects 8 thrusters, but config contains {} entries", + thruster_count); + } + + thruster_parameters_.clear(); + thruster_parameters_.reserve(thruster_count); + + for (std::size_t i = 0; i < thruster_count; ++i) { + thruster_parameters_.push_back(ThrusterParameters{ + static_cast(thruster_mapping[i]), + static_cast(thruster_direction[i]), + static_cast(thruster_pwm_min[i]), + static_cast(thruster_pwm_max[i]), + }); + } - std::ranges::transform(thruster_mapping, thruster_direction, - std::back_inserter(this->thruster_parameters_), - create_thruster_parameters); + const double timeout_threshold_param = + this->get_parameter("propulsion.thrusters.watchdog_timeout").as_double(); - double timout_treshold_param = - this->get_parameter("propulsion.thrusters.watchdog_timeout") - .as_double(); watchdog_timeout_ = std::chrono::duration_cast( - std::chrono::duration(timout_treshold_param)); + std::chrono::duration(timeout_threshold_param)); } RCLCPP_COMPONENTS_REGISTER_NODE(ThrusterInterfaceAUVNode) From 26aae1bed22f7dd8fb80a2a1ea85af2ca64c0787 Mon Sep 17 00:00:00 2001 From: forrisdahl Date: Thu, 26 Mar 2026 11:56:54 +0100 Subject: [PATCH 211/290] feat: add correct create packet and correct sending for testing --- .../src/thruster_interface_auv_driver.cpp | 136 ++++++++++++++---- 1 file changed, 106 insertions(+), 30 deletions(-) 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 6ae0d10cd..118c309ff 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp @@ -96,58 +96,134 @@ std::uint16_t ThrusterInterfaceAUVDriver::calc_poly( coeffs[2] * force + coeffs[3]); } - -std::vector ThrusterInterfaceAUVDriver::create_packet( - std::uint8_t id, - const std::vector& thruster_pwm_array) const { - constexpr std::uint8_t magic = 0xAA; - constexpr std::size_t expected_thrusters = 8; - - if (thruster_pwm_array.size() != expected_thrusters) { - return {}; - } - - std::vector packet; - packet.reserve(1 + 1 + 1 + expected_thrusters * 2 + 1); - - packet.push_back(magic); +// +// std::vector ThrusterInterfaceAUVDriver::create_packet( +// std::uint8_t id, +// const std::vector& thruster_pwm_array) const { +// constexpr std::uint8_t magic = 0xAA; +// constexpr std::size_t expected_thrusters = 8; +// +// if (thruster_pwm_array.size() != expected_thrusters) { +// return {}; +// } +// +// std::vector packet; +// packet.reserve(1 + 1 + 1 + expected_thrusters * 2 + 1); +// +// packet.push_back(magic); +// packet.push_back(id); +// +// const std::uint8_t payload_length = +// static_cast(thruster_pwm_array.size() * sizeof(std::uint16_t)); +// packet.push_back(payload_length); +// +// for (std::uint16_t value : thruster_pwm_array) { +// // little-endian +// packet.push_back(static_cast(value & 0xFF)); +// packet.push_back(static_cast((value >> 8) & 0xFF)); +// } +// +// std::uint8_t checksum = 0; +// for (std::uint8_t byte : packet) { +// checksum = static_cast(checksum + byte); +// } +// +// packet.push_back(checksum); +// return packet; +// } + +std::vector create_packet(uint8_t id, const std::vector& thruster_pwm_array) { + std::vector packet; + + // 1. Magic byte + packet.push_back(0xAA); + + // 2. ID packet.push_back(id); - const std::uint8_t payload_length = - static_cast(thruster_pwm_array.size() * sizeof(std::uint16_t)); - packet.push_back(payload_length); + // 3. Length (payload size in bytes) + uint8_t length = static_cast(payload.size() * sizeof(uint16_t)); + packet.push_back(length); - for (std::uint16_t value : thruster_pwm_array) { - // little-endian - packet.push_back(static_cast(value & 0xFF)); - packet.push_back(static_cast((value >> 8) & 0xFF)); + // 4. Payload (8 x uint16_t -> 16 bytes), little-endian + for (uint16_t value : payload) { + packet.push_back(static_cast(value & 0xFF)); // LSB + packet.push_back(static_cast((value >> 8) & 0xFF)); // MSB } - std::uint8_t checksum = 0; - for (std::uint8_t byte : packet) { - checksum = static_cast(checksum + byte); + // 5. Checksum: XOR of ID, LENGTH, PAYLOAD + // This matches your embedded compute_checksum() + uint8_t checksum = id ^ length; + for (uint16_t value : payload) { + checksum ^= static_cast(value & 0xFF); + checksum ^= static_cast((value >> 8) & 0xFF); } packet.push_back(checksum); + return packet; } +// +// int ThrusterInterfaceAUVDriver::send_data_to_escs( +// const std::vector& thruster_pwm_array) { +// if (!serial_.is_open()) { +// return -1; +// } +// +// const auto packet = create_packet(0x04, thruster_pwm_array); +// if (packet.empty()) { +// return -1; +// } +// +// std::error_code ec; +// const auto bytes_written = asio::write(serial_, asio::buffer(packet), ec); +// +// if (ec || bytes_written != packet.size()) { +// std::cerr << "UART write failed: " +// << (ec ? ec.message() : "short write") << '\n'; +// return -1; +// } +// +// return 0; +// } int ThrusterInterfaceAUVDriver::send_data_to_escs( const std::vector& thruster_pwm_array) { if (!serial_.is_open()) { return -1; } - const auto packet = create_packet(packet_id_, thruster_pwm_array); - if (packet.empty()) { + const auto packet = create_packet(0x04, thruster_pwm_array); + constexpr std::size_t header_size = 3; + + if (packet.size() < header_size) { return -1; } std::error_code ec; - const auto bytes_written = asio::write(serial_, asio::buffer(packet), ec); - if (ec || bytes_written != packet.size()) { - std::cerr << "UART write failed: " + // Send header first: [magic][id][length] + const auto header_bytes_written = + asio::write(serial_, asio::buffer(packet.data(), header_size), ec); + + if (ec || header_bytes_written != header_size) { + std::cerr << "UART header write failed: " + << (ec ? ec.message() : "short write") << '\n'; + return -1; + } + + // Small delay so the receiver can switch state + std::this_thread::sleep_for(std::chrono::milliseconds(20)); + + // Send payload + checksum + const auto remaining_size = packet.size() - header_size; + const auto payload_bytes_written = + asio::write(serial_, + asio::buffer(packet.data() + header_size, remaining_size), + ec); + + if (ec || payload_bytes_written != remaining_size) { + std::cerr << "UART payload write failed: " << (ec ? ec.message() : "short write") << '\n'; return -1; } From f2949efc2a6a2b9713032f3e8ec4b6bf99821063 Mon Sep 17 00:00:00 2001 From: forrisdahl Date: Thu, 26 Mar 2026 11:59:54 +0100 Subject: [PATCH 212/290] fix: wrong name --- .../src/thruster_interface_auv_driver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 118c309ff..96cd5270b 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp @@ -132,7 +132,7 @@ std::uint16_t ThrusterInterfaceAUVDriver::calc_poly( // return packet; // } -std::vector create_packet(uint8_t id, const std::vector& thruster_pwm_array) { +std::vector create_packet(uint8_t id, const std::vector& payload) { std::vector packet; // 1. Magic byte From a4f3d6afec3bec18084f342a8155dde622a92e4f Mon Sep 17 00:00:00 2001 From: forrisdahl Date: Thu, 26 Mar 2026 12:17:47 +0100 Subject: [PATCH 213/290] fix: lookup table error on create packet function --- .../src/thruster_interface_auv_driver.cpp | 23 +++++++++++-------- 1 file changed, 13 insertions(+), 10 deletions(-) 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 96cd5270b..eb832d3ae 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp @@ -132,8 +132,11 @@ std::uint16_t ThrusterInterfaceAUVDriver::calc_poly( // return packet; // } -std::vector create_packet(uint8_t id, const std::vector& payload) { - std::vector packet; +std::vector ThrusterInterfaceAUVDriver::create_packet( + std::uint8_t id, + const std::vector& thruster_pwm_array) const { + + std::vector packet; // 1. Magic byte packet.push_back(0xAA); @@ -142,19 +145,19 @@ std::vector create_packet(uint8_t id, const std::vector& payl packet.push_back(id); // 3. Length (payload size in bytes) - uint8_t length = static_cast(payload.size() * sizeof(uint16_t)); + uint8_t length = static_cast( + thruster_pwm_array.size() * sizeof(uint16_t)); packet.push_back(length); - // 4. Payload (8 x uint16_t -> 16 bytes), little-endian - for (uint16_t value : payload) { - packet.push_back(static_cast(value & 0xFF)); // LSB - packet.push_back(static_cast((value >> 8) & 0xFF)); // MSB + // 4. Payload (little-endian) + for (uint16_t value : thruster_pwm_array) { + packet.push_back(static_cast(value & 0xFF)); + packet.push_back(static_cast((value >> 8) & 0xFF)); } - // 5. Checksum: XOR of ID, LENGTH, PAYLOAD - // This matches your embedded compute_checksum() + // 5. XOR checksum (matching your MCU) uint8_t checksum = id ^ length; - for (uint16_t value : payload) { + for (uint16_t value : thruster_pwm_array) { checksum ^= static_cast(value & 0xFF); checksum ^= static_cast((value >> 8) & 0xFF); } From 7c9c4f3c49b01ad3a31487e532f2acdb8cc9955c Mon Sep 17 00:00:00 2001 From: Anbit Date: Thu, 26 Mar 2026 13:58:49 +0100 Subject: [PATCH 214/290] Fix: errror in launch file --- guidance/los_guidance/launch/los_guidance.launch.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/guidance/los_guidance/launch/los_guidance.launch.py b/guidance/los_guidance/launch/los_guidance.launch.py index fb1749a3f..6ffefc6df 100644 --- a/guidance/los_guidance/launch/los_guidance.launch.py +++ b/guidance/los_guidance/launch/los_guidance.launch.py @@ -33,7 +33,13 @@ def launch_setup(context, *args, **kwargs): executable="los_guidance_node", name="los_guidance_node", namespace=namespace, - parameters=[drone_params, los_config], + parameters=[ + drone_params, + { + "los_config_file": los_config, + "time_step": 0.1, + }, + ], output="screen", ) ] From 66cdafa803d3ae8685954137ce246cc1ae216131 Mon Sep 17 00:00:00 2001 From: Cyprian Osinski Date: Thu, 26 Mar 2026 22:39:01 +0100 Subject: [PATCH 215/290] made it so that gripper does not have buoyancy or mass. Collision might be a bit wonky --- auv_setup/config/robots/nautilus.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/auv_setup/config/robots/nautilus.yaml b/auv_setup/config/robots/nautilus.yaml index 60ecae05e..dc5e1080d 100644 --- a/auv_setup/config/robots/nautilus.yaml +++ b/auv_setup/config/robots/nautilus.yaml @@ -16,7 +16,7 @@ /**: ros__parameters: physical: - center_of_mass: [0.0, 0.0, 0.0] # CO is aligned with CM Position (x,y,z) in meters (M) + center_of_mass: [0.0, 0.0, 0.01] # CO is aligned with CM Position (x,y) in meters (M), small cg offset in z to keep drone naturally stable in roll/pitch mass_matrix: [53.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 53.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 53.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 11.0628, 1.086, -3.17502, 0.0, 0.0, 0.0, 1.086, 23.1128, 0.1025, 0.0, 0.0, 0.0, -3.17502, 0.1025, 26.23998] # 6x6 mass_inertia_matrix propulsion: From ab5ee12f40e44ed2acf4207abe6ec33cb17eef5e Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Wed, 25 Mar 2026 21:14:38 +0100 Subject: [PATCH 216/290] rebase --- .github/workflows/simulator-test.yml | 5 ++- .../waypoint_navigation/check_goal.py | 5 ++- .../waypoint_navigation/send_goal.py | 5 ++- .../waypoint_navigation/simulator_test.sh | 41 +++++++++---------- 4 files changed, 32 insertions(+), 24 deletions(-) diff --git a/.github/workflows/simulator-test.yml b/.github/workflows/simulator-test.yml index c75075c0d..82a077fcb 100644 --- a/.github/workflows/simulator-test.yml +++ b/.github/workflows/simulator-test.yml @@ -16,6 +16,9 @@ jobs: setup_script: "tests/setup.sh" pre_test_script: "scripts/ci_install_dependencies.sh" test_scripts: '[ - "tests/simulator_tests/waypoint_navigation/simulator_test.sh", + "tests/simulator_tests/waypoint_navigation/simulator_test.sh nautilus qp", + "tests/simulator_tests/waypoint_navigation/simulator_test.sh nautilus pseudoinverse", + "tests/simulator_tests/waypoint_navigation/simulator_test.sh orca qp", + "tests/simulator_tests/waypoint_navigation/simulator_test.sh orca pseudoinverse", "tests/simulator_tests/waypoint_manager_test/simulator_test.sh", ]' diff --git a/tests/simulator_tests/waypoint_navigation/check_goal.py b/tests/simulator_tests/waypoint_navigation/check_goal.py index 69bf0a71a..995c27f6c 100644 --- a/tests/simulator_tests/waypoint_navigation/check_goal.py +++ b/tests/simulator_tests/waypoint_navigation/check_goal.py @@ -1,5 +1,6 @@ import math import os +import sys import time import rclpy @@ -9,6 +10,8 @@ from rclpy.qos import QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy from vortex_utils.python_utils import quat_to_euler +namespace = sys.argv[1] if len(sys.argv) > 1 else "nautilus" + best_effort_qos = QoSProfile( history=QoSHistoryPolicy.KEEP_LAST, depth=1, @@ -35,7 +38,7 @@ def __init__(self): super().__init__('check_goal_node') self.pose_sub_ = self.create_subscription( PoseWithCovarianceStamped, - '/nautilus/pose', + f'/{namespace}/pose', self.pose_callback, best_effort_qos, ) diff --git a/tests/simulator_tests/waypoint_navigation/send_goal.py b/tests/simulator_tests/waypoint_navigation/send_goal.py index c9d7287ad..93c17b57b 100644 --- a/tests/simulator_tests/waypoint_navigation/send_goal.py +++ b/tests/simulator_tests/waypoint_navigation/send_goal.py @@ -1,4 +1,5 @@ import random +import sys import rclpy import yaml @@ -7,6 +8,8 @@ from vortex_msgs.action import ReferenceFilterWaypoint from vortex_utils.python_utils import PoseData, euler_to_quat +namespace = sys.argv[1] if len(sys.argv) > 1 else "nautilus" + def randomize_pose() -> PoseData: pose: PoseData = PoseData() @@ -25,7 +28,7 @@ def __init__(self): super().__init__('reference_filter_waypoint_client') self._action_client = ActionClient( - self, ReferenceFilterWaypoint, '/nautilus/reference_filter' + self, ReferenceFilterWaypoint, f'/{namespace}/reference_filter' ) self.send_goal() diff --git a/tests/simulator_tests/waypoint_navigation/simulator_test.sh b/tests/simulator_tests/waypoint_navigation/simulator_test.sh index 79c2db117..e85c34216 100755 --- a/tests/simulator_tests/waypoint_navigation/simulator_test.sh +++ b/tests/simulator_tests/waypoint_navigation/simulator_test.sh @@ -2,6 +2,10 @@ set -e set -o pipefail +DRONE="${1:-nautilus}" +SOLVER_TYPE="${2:-qp}" + +echo "===== Waypoint Navigation Test: drone=$DRONE, solver_type=$SOLVER_TYPE =====" echo "Setting up ROS 2 environment..." . /opt/ros/humble/setup.sh . "${WORKSPACE:-$HOME/ros2_ws}/install/setup.bash" @@ -13,30 +17,25 @@ SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" # Function to terminate processes safely on error cleanup() { echo "Error detected. Cleaning up..." - kill -TERM -"$SIM_PID" -"$NAUTILUS_PID" -"$CONTROLLER_PID" -"$FILTER_PID" -"$OP_MODE_PID" || true + kill -TERM -"$SIM_PID" -"$CONTROLLER_PID" || true exit 1 } trap cleanup ERR -setsid ros2 bag record -o ${WORKSPACE}/bags/recording -s mcap -a & +setsid ros2 bag record -o ${WORKSPACE}/bags/recording_${DRONE}_${SOLVER_TYPE} -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:=nautilus_no_gpu & +# Launch Stonefish Simulator + Drone (combined launch) +setsid ros2 launch stonefish_sim vortex_sim_launch.py rendering:=false scenario:=${DRONE}_no_gpu drone:=${DRONE} solver_type:=${SOLVER_TYPE} & SIM_PID=$! echo "Launched simulator with PID: $SIM_PID" -# Launch NAUTILUS Simulation -setsid ros2 launch stonefish_sim drone_sim.launch.py & -NAUTILUS_PID=$! -echo "Launched nautilus with PID: $NAUTILUS_PID" - echo "Waiting for simulator to start..." -timeout 30s bash -c ' - while ! ros2 topic list | grep -q "/nautilus/odom"; do +timeout 30s bash -c " + while ! ros2 topic list | grep -q '/${DRONE}/odom'; do sleep 1 - done || true' + done || true" echo "Simulator started" # Check for ROS errors in logs @@ -47,11 +46,11 @@ fi # Wait for odometry data echo "Waiting for odom data..." -timeout 10s ros2 topic echo /nautilus/odom --once +timeout 10s ros2 topic echo /${DRONE}/odom --once echo "Got odom data" echo "Waiting for sim interface to start..." -timeout 30s bash -c 'until ros2 topic list | grep -q "/nautilus/pose"; do sleep 1; done' +timeout 30s bash -c "until ros2 topic list | grep -q '/${DRONE}/pose'; do sleep 1; done" echo "Simulator started" # Check for ROS errors again @@ -62,11 +61,11 @@ fi # Wait for pose data echo "Waiting for pose data..." -timeout 10s ros2 topic echo /nautilus/pose --once +timeout 10s ros2 topic echo /${DRONE}/pose --once echo "Got pose data" # Launch controller and reference filter -setsid ros2 launch auv_setup dp.launch.py & +setsid ros2 launch auv_setup dp.launch.py drone:=${DRONE} & CONTROLLER_PID=$! echo "Launched controller and reference filter with PID: $CONTROLLER_PID" @@ -81,19 +80,19 @@ sleep 5 # Set operation mode echo "Turning off killswitch and setting operation mode to autonomous mode" -ros2 service call /nautilus/set_killswitch vortex_msgs/srv/SetKillswitch "{killswitch_on: false}" -ros2 service call /nautilus/set_operation_mode vortex_msgs/srv/SetOperationMode "{requested_operation_mode: {operation_mode: 1}}" +ros2 service call /${DRONE}/set_killswitch vortex_msgs/srv/SetKillswitch "{killswitch_on: false}" +ros2 service call /${DRONE}/set_operation_mode vortex_msgs/srv/SetOperationMode "{requested_operation_mode: {operation_mode: 1}}" echo "Sleeping for 5 seconds to make sure operation is stable..." sleep 5 # Send waypoint goal echo "Sending goal" -python3 "$SCRIPT_DIR/send_goal.py" +python3 "$SCRIPT_DIR/send_goal.py" "$DRONE" # Check if goal reached echo "Checking if goal reached" -python3 "$SCRIPT_DIR/check_goal.py" +python3 "$SCRIPT_DIR/check_goal.py" "$DRONE" if [ $? -ne 0 ]; then echo "Test failed: Drone did not reach goal." @@ -103,6 +102,6 @@ else fi # Terminate processes -kill -TERM -"$SIM_PID" -"$NAUTILUS_PID" -"$CONTROLLER_PID" -"$BAG_PID" -"$OP_MODE_PID" +kill -TERM -"$SIM_PID" -"$CONTROLLER_PID" -"$BAG_PID" echo "Test completed successfully." From 463ea7ceeb90f4354e11fb1eebc9879f073e6d7b Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Wed, 25 Mar 2026 21:48:36 +0100 Subject: [PATCH 217/290] waypoint sim test wrapper scripts --- .github/workflows/simulator-test.yml | 8 ++++---- .../waypoint_navigation/nautilus_pseudoinverse.sh | 3 +++ tests/simulator_tests/waypoint_navigation/nautilus_qp.sh | 3 +++ .../waypoint_navigation/orca_pseudoinverse.sh | 3 +++ tests/simulator_tests/waypoint_navigation/orca_qp.sh | 3 +++ 5 files changed, 16 insertions(+), 4 deletions(-) create mode 100755 tests/simulator_tests/waypoint_navigation/nautilus_pseudoinverse.sh create mode 100755 tests/simulator_tests/waypoint_navigation/nautilus_qp.sh create mode 100755 tests/simulator_tests/waypoint_navigation/orca_pseudoinverse.sh create mode 100755 tests/simulator_tests/waypoint_navigation/orca_qp.sh diff --git a/.github/workflows/simulator-test.yml b/.github/workflows/simulator-test.yml index 82a077fcb..213693412 100644 --- a/.github/workflows/simulator-test.yml +++ b/.github/workflows/simulator-test.yml @@ -16,9 +16,9 @@ jobs: setup_script: "tests/setup.sh" pre_test_script: "scripts/ci_install_dependencies.sh" test_scripts: '[ - "tests/simulator_tests/waypoint_navigation/simulator_test.sh nautilus qp", - "tests/simulator_tests/waypoint_navigation/simulator_test.sh nautilus pseudoinverse", - "tests/simulator_tests/waypoint_navigation/simulator_test.sh orca qp", - "tests/simulator_tests/waypoint_navigation/simulator_test.sh orca pseudoinverse", + "tests/simulator_tests/waypoint_navigation/nautilus_qp.sh", + "tests/simulator_tests/waypoint_navigation/nautilus_pseudoinverse.sh", + "tests/simulator_tests/waypoint_navigation/orca_qp.sh", + "tests/simulator_tests/waypoint_navigation/orca_pseudoinverse.sh", "tests/simulator_tests/waypoint_manager_test/simulator_test.sh", ]' diff --git a/tests/simulator_tests/waypoint_navigation/nautilus_pseudoinverse.sh b/tests/simulator_tests/waypoint_navigation/nautilus_pseudoinverse.sh new file mode 100755 index 000000000..0932a998e --- /dev/null +++ b/tests/simulator_tests/waypoint_navigation/nautilus_pseudoinverse.sh @@ -0,0 +1,3 @@ +#!/bin/bash +SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" +exec "$SCRIPT_DIR/simulator_test.sh" nautilus pseudoinverse diff --git a/tests/simulator_tests/waypoint_navigation/nautilus_qp.sh b/tests/simulator_tests/waypoint_navigation/nautilus_qp.sh new file mode 100755 index 000000000..1da7ceaa7 --- /dev/null +++ b/tests/simulator_tests/waypoint_navigation/nautilus_qp.sh @@ -0,0 +1,3 @@ +#!/bin/bash +SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" +exec "$SCRIPT_DIR/simulator_test.sh" nautilus qp diff --git a/tests/simulator_tests/waypoint_navigation/orca_pseudoinverse.sh b/tests/simulator_tests/waypoint_navigation/orca_pseudoinverse.sh new file mode 100755 index 000000000..9e84147b9 --- /dev/null +++ b/tests/simulator_tests/waypoint_navigation/orca_pseudoinverse.sh @@ -0,0 +1,3 @@ +#!/bin/bash +SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" +exec "$SCRIPT_DIR/simulator_test.sh" orca pseudoinverse diff --git a/tests/simulator_tests/waypoint_navigation/orca_qp.sh b/tests/simulator_tests/waypoint_navigation/orca_qp.sh new file mode 100755 index 000000000..9cd5c9cbc --- /dev/null +++ b/tests/simulator_tests/waypoint_navigation/orca_qp.sh @@ -0,0 +1,3 @@ +#!/bin/bash +SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" +exec "$SCRIPT_DIR/simulator_test.sh" orca qp From 5f9c567fdc4a3a6331691df8bdd9bbf3e249ecf2 Mon Sep 17 00:00:00 2001 From: Cyprian Osinski Date: Thu, 26 Mar 2026 22:39:01 +0100 Subject: [PATCH 218/290] made it so that gripper does not have buoyancy or mass. Collision might be a bit wonky --- auv_setup/config/robots/nautilus.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/auv_setup/config/robots/nautilus.yaml b/auv_setup/config/robots/nautilus.yaml index 60ecae05e..dc5e1080d 100644 --- a/auv_setup/config/robots/nautilus.yaml +++ b/auv_setup/config/robots/nautilus.yaml @@ -16,7 +16,7 @@ /**: ros__parameters: physical: - center_of_mass: [0.0, 0.0, 0.0] # CO is aligned with CM Position (x,y,z) in meters (M) + center_of_mass: [0.0, 0.0, 0.01] # CO is aligned with CM Position (x,y) in meters (M), small cg offset in z to keep drone naturally stable in roll/pitch mass_matrix: [53.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 53.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 53.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 11.0628, 1.086, -3.17502, 0.0, 0.0, 0.0, 1.086, 23.1128, 0.1025, 0.0, 0.0, 0.0, -3.17502, 0.1025, 26.23998] # 6x6 mass_inertia_matrix propulsion: From d710cc74d45dfbb81fcf2a2ee517ec5fe430067a Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Fri, 27 Mar 2026 11:05:49 +0100 Subject: [PATCH 219/290] remove control toolbox from package.xml --- control/velocity_controller/package.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/control/velocity_controller/package.xml b/control/velocity_controller/package.xml index f138d6dbe..b97ee6137 100644 --- a/control/velocity_controller/package.xml +++ b/control/velocity_controller/package.xml @@ -14,8 +14,8 @@ vortex_msgs geometry_msgs nav_msgs - ct_optcon - ct_core + + vortex_utils rclcpp_lifecycle lifecycle_msgs From 97c80038494b6ec050dfc9f24806529d7caca82a Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Fri, 27 Mar 2026 11:15:42 +0100 Subject: [PATCH 220/290] added install_ct script --- scripts/ci_install_dependencies.sh | 3 ++ scripts/install_ct.sh | 73 ++++++++++++++++++++++++++++++ 2 files changed, 76 insertions(+) create mode 100755 scripts/install_ct.sh diff --git a/scripts/ci_install_dependencies.sh b/scripts/ci_install_dependencies.sh index 90b877d6c..c35f85488 100755 --- a/scripts/ci_install_dependencies.sh +++ b/scripts/ci_install_dependencies.sh @@ -30,4 +30,7 @@ sudo update-alternatives --install /usr/bin/gcov gcov /usr/bin/gcov-13 100 # Install casadi using CasADi install script "$SCRIPT_DIR/install_casadi.sh" +# Install Control Toolbox using the provided script +"$SCRIPT_DIR/install_ct.sh" + echo "Done installing additional dependencies." diff --git a/scripts/install_ct.sh b/scripts/install_ct.sh new file mode 100755 index 000000000..6758039b5 --- /dev/null +++ b/scripts/install_ct.sh @@ -0,0 +1,73 @@ +#!/usr/bin/env bash +set -euo pipefail + +echo "Installing Control Toolbox..." + +# ---- Config ---- +WORKSPACE_DIR="${HOME}/third_party/ct_ws" +SRC_DIR="${WORKSPACE_DIR}/src" +INSTALL_PREFIX="${HOME}/.local" +BUILD_TYPE="Release" + +KINDR_REPO="https://github.com/ANYbotics/kindr.git" +CT_REPO="https://github.com/ethz-adrl/control-toolbox.git" + +KINDR_DIR="${SRC_DIR}/kindr" +CT_DIR="${SRC_DIR}/control-toolbox" + +mkdir -p "${SRC_DIR}" +mkdir -p "${INSTALL_PREFIX}" + +echo "Workspace: ${WORKSPACE_DIR}" +echo "Install prefix: ${INSTALL_PREFIX}" + +# ---- System deps ---- +sudo apt-get update +sudo apt-get install -y \ + git \ + build-essential \ + cmake \ + libeigen3-dev \ + libboost-all-dev + +# ---- Clone/update kindr ---- +if [ ! -d "${KINDR_DIR}/.git" ]; then + git clone "${KINDR_REPO}" "${KINDR_DIR}" +else + git -C "${KINDR_DIR}" pull --ff-only +fi + +# ---- Build/install kindr ---- +cmake -S "${KINDR_DIR}" -B "${KINDR_DIR}/build" \ + -DUSE_CMAKE=true \ + -DCMAKE_BUILD_TYPE="${BUILD_TYPE}" \ + -DCMAKE_INSTALL_PREFIX="${INSTALL_PREFIX}" + +cmake --build "${KINDR_DIR}/build" -j"$(nproc)" +cmake --install "${KINDR_DIR}/build" + +# ---- Clone/update control-toolbox ---- +if [ ! -d "${CT_DIR}/.git" ]; then + git clone "${CT_REPO}" "${CT_DIR}" +else + git -C "${CT_DIR}" pull --ff-only +fi + +# ---- Build CT using the provided script ---- +chmod +x "${CT_DIR}/ct/build_ct.sh" + +export CMAKE_PREFIX_PATH="${INSTALL_PREFIX}:${CMAKE_PREFIX_PATH:-}" + +pushd "${CT_DIR}/ct" >/dev/null +./build_ct.sh \ + -DCMAKE_BUILD_TYPE="${BUILD_TYPE}" \ + -DCMAKE_PREFIX_PATH="${INSTALL_PREFIX}" +popd >/dev/null + +echo +echo "Control Toolbox build complete." +echo "You may want this in your shell before building your ROS 2 package:" +echo " export CMAKE_PREFIX_PATH=\"${INSTALL_PREFIX}:\$CMAKE_PREFIX_PATH\"" +echo +echo "CT source: ${CT_DIR}" +echo "Kindr install: ${INSTALL_PREFIX}" \ No newline at end of file From e0e9ab12db2e9acd09a447b1a8712bd462f2e8dc Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Fri, 27 Mar 2026 11:16:38 +0100 Subject: [PATCH 221/290] remove ct from dep.repos. Use install script instead --- dependencies.repos | 3 --- 1 file changed, 3 deletions(-) diff --git a/dependencies.repos b/dependencies.repos index b925a14d8..a62354be6 100644 --- a/dependencies.repos +++ b/dependencies.repos @@ -5,9 +5,6 @@ repositories: vortex-vkf: type: git url: https://github.com/vortexntnu/vortex-vkf.git - control-toolbox: - type: git - url: https://github.com/ethz-adrl/control-toolbox.git vortex-utils: type: git url: https://github.com/vortexntnu/vortex-utils.git From 4ac866bf6894f00cd752ce3a7715f7ff1a0783ae Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Fri, 27 Mar 2026 11:59:50 +0100 Subject: [PATCH 222/290] convert los guidance to composable node --- guidance/los_guidance/CMakeLists.txt | 25 ++++++------------- .../include/los_guidance/los_guidance_ros.hpp | 2 +- guidance/los_guidance/package.xml | 1 + .../los_guidance/src/los_guidance_node.cpp | 16 ------------ .../los_guidance/src/los_guidance_ros.cpp | 6 ++++- 5 files changed, 14 insertions(+), 36 deletions(-) delete mode 100644 guidance/los_guidance/src/los_guidance_node.cpp diff --git a/guidance/los_guidance/CMakeLists.txt b/guidance/los_guidance/CMakeLists.txt index 00c00a391..f8e7e7b4c 100644 --- a/guidance/los_guidance/CMakeLists.txt +++ b/guidance/los_guidance/CMakeLists.txt @@ -12,6 +12,7 @@ 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_ros REQUIRED) find_package(geometry_msgs REQUIRED) @@ -36,6 +37,7 @@ add_library(${LIB_NAME} SHARED ament_target_dependencies(${LIB_NAME} rclcpp rclcpp_action + rclcpp_components geometry_msgs nav_msgs vortex_msgs @@ -45,21 +47,13 @@ ament_target_dependencies(${LIB_NAME} fmt ) -add_executable(los_guidance_node - src/los_guidance_node.cpp -) - -ament_target_dependencies(los_guidance_node - rclcpp - rclcpp_action - geometry_msgs - vortex_msgs - vortex_utils_ros +target_link_libraries(${LIB_NAME} + yaml-cpp ) -target_link_libraries(los_guidance_node - ${LIB_NAME} - yaml-cpp +rclcpp_components_register_node(${LIB_NAME} + PLUGIN "vortex::guidance::los::LosGuidanceNode" + EXECUTABLE los_guidance_node ) install(TARGETS @@ -70,11 +64,6 @@ install(TARGETS RUNTIME DESTINATION bin ) -install(TARGETS - los_guidance_node - DESTINATION lib/${PROJECT_NAME} -) - install( DIRECTORY include/ DESTINATION include/ 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 50fc6af2c..55adfa079 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -30,7 +30,7 @@ namespace vortex::guidance::los { class LosGuidanceNode : public rclcpp::Node { public: // Constructor - LosGuidanceNode(); + explicit LosGuidanceNode(const rclcpp::NodeOptions& options); private: // Setup Functions diff --git a/guidance/los_guidance/package.xml b/guidance/los_guidance/package.xml index 5f696a725..450afeae7 100644 --- a/guidance/los_guidance/package.xml +++ b/guidance/los_guidance/package.xml @@ -11,6 +11,7 @@ rclcpp rclcpp_action + rclcpp_components geometry_msgs vortex_msgs nav_msgs diff --git a/guidance/los_guidance/src/los_guidance_node.cpp b/guidance/los_guidance/src/los_guidance_node.cpp deleted file mode 100644 index 31ad2eb74..000000000 --- a/guidance/los_guidance/src/los_guidance_node.cpp +++ /dev/null @@ -1,16 +0,0 @@ -#include - -#include "los_guidance/los_guidance_ros.hpp" - -// Main Entry Point -int main(int argc, char** argv) { - rclcpp::init(argc, argv); - - auto node = std::make_shared(); - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(node); - - executor.spin(); - - return 0; -} diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index b04d7121d..1f10519f4 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -1,5 +1,6 @@ #include "los_guidance/los_guidance_ros.hpp" #include +#include #include #include #include @@ -18,7 +19,8 @@ const auto start_message = R"( namespace vortex::guidance::los { // Constructor -LosGuidanceNode::LosGuidanceNode() : Node("los_guidance_node") { +LosGuidanceNode::LosGuidanceNode(const rclcpp::NodeOptions& options) + : Node("los_guidance_node", options) { double time_step_s = this->declare_parameter("time_step"); time_step_ = std::chrono::milliseconds(static_cast(time_step_s * 1000)); @@ -438,3 +440,5 @@ void LosGuidanceNode::execute( } } // namespace vortex::guidance::los + +RCLCPP_COMPONENTS_REGISTER_NODE(vortex::guidance::los::LosGuidanceNode) From debb3346c37efe5cc5cddbb22603a0ce4389025b Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Fri, 27 Mar 2026 12:24:19 +0100 Subject: [PATCH 223/290] velocity_controller composable node --- control/velocity_controller/CMakeLists.txt | 23 ++++++++++++++----- .../velocity_controller.hpp | 2 +- control/velocity_controller/package.xml | 1 + .../src/velocity_controller.cpp | 17 +++----------- 4 files changed, 22 insertions(+), 21 deletions(-) diff --git a/control/velocity_controller/CMakeLists.txt b/control/velocity_controller/CMakeLists.txt index 04e477b4a..d3c5b5426 100644 --- a/control/velocity_controller/CMakeLists.txt +++ b/control/velocity_controller/CMakeLists.txt @@ -11,6 +11,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(lifecycle_msgs REQUIRED) find_package(std_msgs REQUIRED) @@ -32,15 +33,17 @@ include_directories( # $ #) -add_executable(velocity_controller_node +set(LIB_NAME velocity_controller_component) +add_library(${LIB_NAME} SHARED src/velocity_controller.cpp src/PID_setup.cpp src/LQR_setup.cpp src/utilities.cpp src/ct_instantiations.cpp ) -ament_target_dependencies(velocity_controller_node +ament_target_dependencies(${LIB_NAME} rclcpp + rclcpp_components rclcpp_lifecycle lifecycle_msgs std_msgs @@ -49,11 +52,19 @@ ament_target_dependencies(velocity_controller_node nav_msgs vortex_utils ) -#target_include_directories(velocity_controller_node PRIVATE casadi Eigen3) -target_link_libraries(velocity_controller_node Eigen3::Eigen casadi::casadi ct_optcon ct_core) +#target_include_directories(${LIB_NAME} PRIVATE casadi Eigen3) +target_link_libraries(${LIB_NAME} Eigen3::Eigen casadi::casadi ct_optcon ct_core) + +rclcpp_components_register_node(${LIB_NAME} + PLUGIN "Velocity_node" + EXECUTABLE velocity_controller_node +) + install(TARGETS - velocity_controller_node - DESTINATION lib/${PROJECT_NAME} + ${LIB_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) install( diff --git a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp index 9bdd2864f..58a783b89 100644 --- a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp +++ b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp @@ -18,7 +18,7 @@ class Velocity_node : public rclcpp_lifecycle::LifecycleNode{ public: - explicit Velocity_node(); + explicit Velocity_node(const rclcpp::NodeOptions& options); Velocity_node(const Velocity_node&)=delete; //no copy constructor Velocity_node& operator=(const Velocity_node&) = delete; //no copy assignment //TODO: decide if i one should be allowed to move or transfer ownership if the class diff --git a/control/velocity_controller/package.xml b/control/velocity_controller/package.xml index b97ee6137..dd2cf0912 100644 --- a/control/velocity_controller/package.xml +++ b/control/velocity_controller/package.xml @@ -17,6 +17,7 @@ vortex_utils + rclcpp_components rclcpp_lifecycle lifecycle_msgs auv_setup diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index c577a73af..de3903618 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -7,6 +7,7 @@ #include #include #include +#include #include #include #include "vortex/utils/math.hpp" @@ -15,7 +16,7 @@ -Velocity_node::Velocity_node() : rclcpp_lifecycle::LifecycleNode("velocity_controller_lifecycle"), lqr_controller(), pub_QoS(10), sub_QoS(10) +Velocity_node::Velocity_node(const rclcpp::NodeOptions& options) : rclcpp_lifecycle::LifecycleNode("velocity_controller_lifecycle", options), lqr_controller(), pub_QoS(10), sub_QoS(10) { get_new_parameters(); pub_QoS.keep_last(10).reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT).durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); @@ -258,18 +259,6 @@ void Velocity_node::reset_controllers(int nr){ } } -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - auto lc_node = std::make_shared(); - - rclcpp::executors::SingleThreadedExecutor exec; - exec.add_node(lc_node->get_node_base_interface()); - - while (rclcpp::ok()&&!lc_node->should_exit_){ - exec.spin_some(); - } - return 0; -} +RCLCPP_COMPONENTS_REGISTER_NODE(Velocity_node) From bd74b5f47248245c944bd81b16d4f5cb5f080146 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Fri, 27 Mar 2026 13:02:11 +0100 Subject: [PATCH 224/290] fix autopilot launch --- auv_setup/launch/autopilot.launch.py | 50 ++++++++++++++++------------ 1 file changed, 28 insertions(+), 22 deletions(-) diff --git a/auv_setup/launch/autopilot.launch.py b/auv_setup/launch/autopilot.launch.py index 082c5120d..e282cd55d 100644 --- a/auv_setup/launch/autopilot.launch.py +++ b/auv_setup/launch/autopilot.launch.py @@ -16,47 +16,53 @@ def launch_setup(context, *args, **kwargs): drone, namespace = resolve_drone_and_namespace(context) drone_params = os.path.join( - get_package_share_directory("auv_setup"), - "config", - "robots", - f"{drone}.yaml", + get_package_share_directory('auv_setup'), + 'config', + 'robots', + f'{drone}.yaml', ) - VC_params = os.path.join( - get_package_share_directory("velocity_controller"), - "config", - "parameters.yaml", + velocity_control_params = os.path.join( + get_package_share_directory('velocity_controller'), + 'config', + f'{drone}_params.yaml', ) - adapt_params = os.path.join( + los_config = os.path.join( get_package_share_directory("los_guidance"), "config", "guidance_params.yaml", ) container=ComposableNodeContainer( - name="autopilot_container", + name='autopilot_container', namespace=namespace, - package="rclcpp_components", - executable="component_container_mt", + package='rclcpp_components', + executable='component_container_mt', composable_node_descriptions=[ ComposableNode( - package="velocity_controller", - plugin="velocity_controller_node", - name="velocity_controller_node", + package='velocity_controller', + plugin='Velocity_node', + name='velocity_controller_node', namespace=namespace, - parameters=[VC_params,drone_params], + parameters=[velocity_control_params,drone_params], extra_arguments=[{"use_intra_process_comms":True}], ), ComposableNode( - package="los_guidance", - plugin="los_guidance_node", - name="los_guidance_node", + package='los_guidance', + plugin='vortex::guidance::los::LosGuidanceNode', + name='los_guidance_node', namespace=namespace, - parameters=[adapt_params,drone_params], + parameters=[ + drone_params, + { + "los_config_file": los_config, + "time_step": 0.1, + }, + ], extra_arguments=[{"use_intra_process_comms":True}], ), ], - output="screen", - arguments=["--ros-args","--log-level","error"], + output='screen', + arguments=['--ros-args','--log-level','error'], ) return [container] From da2a485a3e75c44870c37a91af346d8fdb66e832 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Fri, 27 Mar 2026 13:02:49 +0100 Subject: [PATCH 225/290] remove feedback from LOS guidance action --- guidance/los_guidance/src/los_guidance_ros.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 1f10519f4..2c90a868f 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -341,8 +341,6 @@ void LosGuidanceNode::execute( path_inputs_.next_point = new_wp; } - auto feedback = - std::make_shared(); auto result = std::make_shared(); rclcpp::Rate loop_rate(1000.0 / time_step_.count()); @@ -394,7 +392,6 @@ void LosGuidanceNode::execute( vortex_msgs::msg::LOSGuidance reference_msg = fill_los_reference(outputs, inputs_copy); - feedback->feedback = reference_msg; los_debug_pub_->publish(reference_msg); @@ -417,7 +414,6 @@ void LosGuidanceNode::execute( state_debug_pub_->publish(state_debug_msg); } - goal_handle->publish_feedback(feedback); reference_pub_->publish(reference_msg); if ((inputs_copy.current_position - inputs_copy.next_point) From 059a22ecc619d6531ce36097bd55249237e065c4 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Fri, 27 Mar 2026 13:03:16 +0100 Subject: [PATCH 226/290] remove reference feedback from waypoint manager action --- .../waypoint_manager/waypoint_manager_ros.hpp | 3 -- .../src/waypoint_manager_ros.cpp | 40 +++---------------- 2 files changed, 6 insertions(+), 37 deletions(-) diff --git a/mission/waypoint_manager/include/waypoint_manager/waypoint_manager_ros.hpp b/mission/waypoint_manager/include/waypoint_manager/waypoint_manager_ros.hpp index 86ba42d3e..3fb8128a7 100644 --- a/mission/waypoint_manager/include/waypoint_manager/waypoint_manager_ros.hpp +++ b/mission/waypoint_manager/include/waypoint_manager/waypoint_manager_ros.hpp @@ -4,7 +4,6 @@ #include #include #include -#include #include #include @@ -105,8 +104,6 @@ class WaypointManagerNode : public rclcpp::Node { 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; diff --git a/mission/waypoint_manager/src/waypoint_manager_ros.cpp b/mission/waypoint_manager/src/waypoint_manager_ros.cpp index ccbc26919..a2779252e 100644 --- a/mission/waypoint_manager/src/waypoint_manager_ros.cpp +++ b/mission/waypoint_manager/src/waypoint_manager_ros.cpp @@ -2,7 +2,6 @@ #include #include #include -#include namespace vortex::mission { @@ -87,11 +86,6 @@ 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::to_pose_msg( - latest_ref_feedback_.reference); - } return result; } @@ -100,7 +94,6 @@ void WaypointManagerNode::cleanup_mission_state() { 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( @@ -122,6 +115,12 @@ void WaypointManagerNode::send_next_reference_filter_goal() { return; } + if (active_action_goal_ && active_action_goal_->is_active()) { + auto wm_fb = std::make_shared(); + wm_fb->current_waypoint = waypoints_[current_index_]; + active_action_goal_->publish_feedback(wm_fb); + } + ReferenceFilterAction::Goal rf_goal; rf_goal.waypoint = waypoints_[current_index_]; rf_goal.convergence_threshold = convergence_threshold_; @@ -153,7 +152,6 @@ rclcpp_action::GoalResponse WaypointManagerNode::handle_waypoint_goal( 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_) { @@ -212,7 +210,6 @@ void WaypointManagerNode::handle_send_waypoints_service_request( mission_id_++; waypoints_ = request->waypoints; current_index_ = 0; - has_reference_pose_ = false; if (active_reference_filter_goal_) { reference_filter_client_->async_cancel_goal( @@ -269,31 +266,6 @@ void WaypointManagerNode::send_reference_filter_goal( } }; - 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::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& From 2cf9f6f23829f9eacef81e9c04e2dd5ac3b0d81a Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Fri, 27 Mar 2026 13:35:55 +0100 Subject: [PATCH 227/290] ref_quat remove waypoint_utils and ref feedback --- .../reference_filter_dp_quat/CMakeLists.txt | 1 - .../lib/waypoint_follower.hpp | 4 +- .../lib/waypoint_types.hpp | 34 ----- .../lib/waypoint_utils.hpp | 44 ------ .../ros/reference_filter_ros_utils.hpp | 30 ---- .../src/lib/waypoint_follower.cpp | 8 +- .../src/lib/waypoint_utils.cpp | 64 -------- .../src/ros/reference_filter_ros.cpp | 10 +- .../test/CMakeLists.txt | 18 --- .../test/test_waypoint_follower.cpp | 2 +- .../test/test_waypoint_utils.cpp | 141 ------------------ 11 files changed, 10 insertions(+), 346 deletions(-) delete mode 100644 guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_types.hpp delete mode 100644 guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_utils.hpp delete mode 100644 guidance/reference_filter_dp_quat/src/lib/waypoint_utils.cpp delete mode 100644 guidance/reference_filter_dp_quat/test/test_waypoint_utils.cpp diff --git a/guidance/reference_filter_dp_quat/CMakeLists.txt b/guidance/reference_filter_dp_quat/CMakeLists.txt index 6967c0a88..cfe51ca33 100644 --- a/guidance/reference_filter_dp_quat/CMakeLists.txt +++ b/guidance/reference_filter_dp_quat/CMakeLists.txt @@ -28,7 +28,6 @@ set(CORE_LIB_NAME "${PROJECT_NAME}") add_library(${CORE_LIB_NAME} SHARED src/lib/reference_filter.cpp - src/lib/waypoint_utils.cpp src/lib/waypoint_follower.cpp ) diff --git a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_follower.hpp b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_follower.hpp index 43cf5f8a1..6505036cd 100644 --- a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_follower.hpp +++ b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_follower.hpp @@ -3,14 +3,16 @@ #include #include +#include #include "reference_filter_dp_quat/lib/eigen_typedefs.hpp" #include "reference_filter_dp_quat/lib/reference_filter.hpp" -#include "reference_filter_dp_quat/lib/waypoint_types.hpp" namespace vortex::guidance { using vortex::utils::types::Pose; using vortex::utils::types::Twist; +using vortex::utils::types::Waypoint; +using vortex::utils::types::WaypointMode; /** * @brief Manages reference filter state and waypoint following logic. diff --git a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_types.hpp b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_types.hpp deleted file mode 100644 index 15445c62c..000000000 --- a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_types.hpp +++ /dev/null @@ -1,34 +0,0 @@ -#ifndef REFERENCE_FILTER_DP_QUAT__LIB__WAYPOINT_TYPES_HPP_ -#define REFERENCE_FILTER_DP_QUAT__LIB__WAYPOINT_TYPES_HPP_ - -#include -#include - -namespace vortex::guidance { - -using vortex::utils::types::Pose; - -/** - * @brief Determines which degrees of freedom the reference filter controls. - * - * The mode affects both the reference goal computation (via apply_mode_logic) - * and the convergence check (via has_converged). - */ -enum class WaypointMode : uint8_t { - FULL_POSE = 0, ///< Control all 6 DOF. - ONLY_POSITION = 1, ///< Control x, y, z; hold current orientation. - FORWARD_HEADING = 2, ///< Control x, y, z with yaw toward target. - ONLY_ORIENTATION = 3, ///< Control roll, pitch, yaw; hold current position. -}; - -/** - * @brief A target pose with an associated waypoint mode. - */ -struct Waypoint { - Pose pose{}; - WaypointMode mode = WaypointMode::FULL_POSE; -}; - -} // namespace vortex::guidance - -#endif // REFERENCE_FILTER_DP_QUAT__LIB__WAYPOINT_TYPES_HPP_ diff --git a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_utils.hpp b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_utils.hpp deleted file mode 100644 index cc711233c..000000000 --- a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_utils.hpp +++ /dev/null @@ -1,44 +0,0 @@ -#ifndef REFERENCE_FILTER_DP_QUAT__LIB__WAYPOINT_UTILS_HPP_ -#define REFERENCE_FILTER_DP_QUAT__LIB__WAYPOINT_UTILS_HPP_ - -#include "reference_filter_dp_quat/lib/waypoint_types.hpp" - -namespace vortex::guidance { - -using vortex::utils::types::Pose; - -/** - * @brief Compute the waypoint goal by applying the mode logic to the incoming - * waypoint. - * - * For modes that don't control all DOFs, the uncontrolled components are - * replaced with values from @p current_state. - * - * @param incoming_waypoint The incoming waypoint to compute goal from. - * @param mode The waypoint mode. - * @param current_state The current state pose. - * @return The adjusted waypoint goal. - */ -Pose compute_waypoint_goal(const Pose& incoming_waypoint, - WaypointMode mode, - const Pose& current_state); - -/** - * @brief Check whether the state has converged to the waypoint goal. - * - * Only the DOFs relevant to the waypoint mode are included in the error norm. - * - * @param state The current state pose. - * @param waypoint_goal The waypoint goal pose. - * @param mode The waypoint mode. - * @param convergence_threshold The maximum allowed error norm. - * @return True if the error is below the threshold. - */ -bool has_converged(const Pose& state, - const Pose& waypoint_goal, - WaypointMode mode, - double convergence_threshold); - -} // namespace vortex::guidance - -#endif // REFERENCE_FILTER_DP_QUAT__LIB__WAYPOINT_UTILS_HPP_ diff --git a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros_utils.hpp b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros_utils.hpp index 043a5c526..c51fc75f0 100644 --- a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros_utils.hpp +++ b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros_utils.hpp @@ -3,43 +3,13 @@ #include #include -#include #include #include #include -#include #include "reference_filter_dp_quat/lib/eigen_typedefs.hpp" -#include "reference_filter_dp_quat/lib/waypoint_types.hpp" namespace vortex::guidance { -/// @brief Convert a ROS waypoint mode to a WaypointMode enum. -/// @throws std::invalid_argument if the mode value is not recognized. -inline WaypointMode waypoint_mode_from_ros(uint8_t mode) { - switch (mode) { - case vortex_msgs::msg::Waypoint::FULL_POSE: - return WaypointMode::FULL_POSE; - case vortex_msgs::msg::Waypoint::ONLY_POSITION: - return WaypointMode::ONLY_POSITION; - case vortex_msgs::msg::Waypoint::FORWARD_HEADING: - return WaypointMode::FORWARD_HEADING; - case vortex_msgs::msg::Waypoint::ONLY_ORIENTATION: - return WaypointMode::ONLY_ORIENTATION; - default: - throw std::invalid_argument("Invalid ROS waypoint mode: " + - std::to_string(mode)); - } -} - -/// @brief Convert a ROS Waypoint message to an internal Waypoint struct. -inline vortex::guidance::Waypoint waypoint_from_ros( - const vortex_msgs::msg::Waypoint& ros_wp) { - Waypoint wp; - wp.pose = vortex::utils::ros_conversions::ros_pose_to_pose(ros_wp.pose); - wp.mode = waypoint_mode_from_ros(ros_wp.mode); - return wp; -} - /// @brief Fill a ReferenceFilterQuat message from a Pose and velocity vector. inline vortex_msgs::msg::ReferenceFilterQuat fill_reference_msg( const vortex::utils::types::Pose& pose, diff --git a/guidance/reference_filter_dp_quat/src/lib/waypoint_follower.cpp b/guidance/reference_filter_dp_quat/src/lib/waypoint_follower.cpp index 7c9d8b49a..7a9a6a642 100644 --- a/guidance/reference_filter_dp_quat/src/lib/waypoint_follower.cpp +++ b/guidance/reference_filter_dp_quat/src/lib/waypoint_follower.cpp @@ -1,7 +1,7 @@ #include "reference_filter_dp_quat/lib/waypoint_follower.hpp" #include #include "reference_filter_dp_quat/lib/eigen_typedefs.hpp" -#include "reference_filter_dp_quat/lib/waypoint_utils.hpp" +#include namespace vortex::guidance { @@ -26,7 +26,7 @@ void WaypointFollower::start(const Pose& pose, waypoint_mode_ = waypoint.mode; convergence_threshold_ = convergence_threshold; waypoint_goal_ = - compute_waypoint_goal(waypoint.pose, waypoint_mode_, nominal_pose_); + vortex::utils::waypoints::compute_waypoint_goal(waypoint.pose, waypoint_mode_, nominal_pose_); } void WaypointFollower::step() { @@ -64,13 +64,13 @@ void WaypointFollower::inject_and_reset() { bool WaypointFollower::within_convergance(const Pose& measured_pose) const { std::lock_guard lock(mutex_); - return has_converged(measured_pose, waypoint_goal_, waypoint_mode_, + return vortex::utils::waypoints::has_converged(measured_pose, waypoint_goal_, waypoint_mode_, convergence_threshold_); } void WaypointFollower::set_reference(const Pose& reference_goal_pose) { std::lock_guard lock(mutex_); - waypoint_goal_ = compute_waypoint_goal(reference_goal_pose, waypoint_mode_, + waypoint_goal_ = vortex::utils::waypoints::compute_waypoint_goal(reference_goal_pose, waypoint_mode_, nominal_pose_); } diff --git a/guidance/reference_filter_dp_quat/src/lib/waypoint_utils.cpp b/guidance/reference_filter_dp_quat/src/lib/waypoint_utils.cpp deleted file mode 100644 index 32f1208d4..000000000 --- a/guidance/reference_filter_dp_quat/src/lib/waypoint_utils.cpp +++ /dev/null @@ -1,64 +0,0 @@ -#include "reference_filter_dp_quat/lib/waypoint_utils.hpp" -#include -#include - -namespace vortex::guidance { - -Pose compute_waypoint_goal(const Pose& incoming_waypoint, - WaypointMode mode, - const Pose& current_state) { - Pose waypoint_out = incoming_waypoint; - - switch (mode) { - case WaypointMode::FULL_POSE: - break; - - case WaypointMode::ONLY_POSITION: - waypoint_out.set_ori(current_state.ori_quaternion()); - break; - - case WaypointMode::FORWARD_HEADING: { - double dx = incoming_waypoint.x - current_state.x; - double dy = incoming_waypoint.y - current_state.y; - double forward_heading = std::atan2(dy, dx); - - waypoint_out.set_ori(Eigen::Quaterniond( - Eigen::AngleAxisd(forward_heading, Eigen::Vector3d::UnitZ()))); - break; - } - - case WaypointMode::ONLY_ORIENTATION: - waypoint_out.set_pos(current_state.pos_vector()); - break; - } - - return waypoint_out; -} - -bool has_converged(const Pose& state, - const Pose& waypoint_goal, - WaypointMode mode, - double convergence_threshold) { - const Eigen::Vector3d ep = state.pos_vector() - waypoint_goal.pos_vector(); - - const Eigen::Vector3d ea = vortex::utils::math::quaternion_error( - state.ori_quaternion(), waypoint_goal.ori_quaternion()); - - const double err = [&] { - switch (mode) { - case WaypointMode::ONLY_POSITION: - return ep.norm(); - case WaypointMode::ONLY_ORIENTATION: - return ea.norm(); - case WaypointMode::FORWARD_HEADING: - return std::sqrt(ep.squaredNorm() + ea(2) * ea(2)); - case WaypointMode::FULL_POSE: - default: - return std::sqrt(ep.squaredNorm() + ea.squaredNorm()); - } - }(); - - return err < convergence_threshold; -} - -} // namespace vortex::guidance diff --git a/guidance/reference_filter_dp_quat/src/ros/reference_filter_ros.cpp b/guidance/reference_filter_dp_quat/src/ros/reference_filter_ros.cpp index 7e85b1923..1c4ab4158 100644 --- a/guidance/reference_filter_dp_quat/src/ros/reference_filter_ros.cpp +++ b/guidance/reference_filter_dp_quat/src/ros/reference_filter_ros.cpp @@ -4,7 +4,7 @@ #include #include #include -#include +#include #include "reference_filter_dp_quat/ros/reference_filter_ros_utils.hpp" const auto start_message = R"( @@ -171,7 +171,7 @@ void ReferenceFilterNode::execute( "Using default 0.1"); } - const auto wp = waypoint_from_ros(goal_handle->get_goal()->waypoint); + const auto wp = vortex::utils::waypoints::waypoint_from_ros(goal_handle->get_goal()->waypoint); const auto [pose, twist] = [this] { std::lock_guard lock(sensor_mutex_); @@ -180,8 +180,6 @@ void ReferenceFilterNode::execute( follower_->start(pose, twist, wp, convergence_threshold); - auto feedback = std::make_shared< - vortex_msgs::action::ReferenceFilterQuatWaypoint::Feedback>(); auto result = std::make_shared< vortex_msgs::action::ReferenceFilterQuatWaypoint::Result>(); @@ -217,8 +215,6 @@ void ReferenceFilterNode::execute( auto final_reference_msg = fill_reference_msg(follower_->pose(), follower_->velocity()); - feedback->reference = final_reference_msg; - goal_handle->publish_feedback(feedback); reference_pub_->publish(final_reference_msg); if (rpy_debug_pub_) { rpy_debug_pub_->publish(fill_reference_rpy_msg( @@ -238,8 +234,6 @@ void ReferenceFilterNode::execute( rpy_debug_pub_->publish(fill_reference_rpy_msg( follower_->pose(), follower_->velocity())); } - feedback->reference = reference_msg; - goal_handle->publish_feedback(feedback); loop_rate.sleep(); } if (!rclcpp::ok() && goal_handle->is_active()) { diff --git a/guidance/reference_filter_dp_quat/test/CMakeLists.txt b/guidance/reference_filter_dp_quat/test/CMakeLists.txt index 2b8229c0c..9295a4300 100644 --- a/guidance/reference_filter_dp_quat/test/CMakeLists.txt +++ b/guidance/reference_filter_dp_quat/test/CMakeLists.txt @@ -20,24 +20,6 @@ ament_target_dependencies(${TEST_BINARY_NAME} PUBLIC Eigen3) gtest_discover_tests(${TEST_BINARY_NAME}) -# Waypoint utils tests -set(TEST_WAYPOINT_UTILS ${PROJECT_NAME}_test_waypoint_utils) -add_executable( - ${TEST_WAYPOINT_UTILS} - test_waypoint_utils.cpp -) - -target_link_libraries( - ${TEST_WAYPOINT_UTILS} - PRIVATE - ${CORE_LIB_NAME} - GTest::GTest -) - -ament_target_dependencies(${TEST_WAYPOINT_UTILS} PUBLIC Eigen3 vortex_utils) - -gtest_discover_tests(${TEST_WAYPOINT_UTILS}) - # Waypoint follower tests set(TEST_WAYPOINT_FOLLOWER ${PROJECT_NAME}_test_waypoint_follower) add_executable( diff --git a/guidance/reference_filter_dp_quat/test/test_waypoint_follower.cpp b/guidance/reference_filter_dp_quat/test/test_waypoint_follower.cpp index 47248b565..a37e636e6 100644 --- a/guidance/reference_filter_dp_quat/test/test_waypoint_follower.cpp +++ b/guidance/reference_filter_dp_quat/test/test_waypoint_follower.cpp @@ -1,6 +1,6 @@ #include #include "reference_filter_dp_quat/lib/waypoint_follower.hpp" -#include "reference_filter_dp_quat/lib/waypoint_utils.hpp" +#include namespace vortex::guidance { diff --git a/guidance/reference_filter_dp_quat/test/test_waypoint_utils.cpp b/guidance/reference_filter_dp_quat/test/test_waypoint_utils.cpp deleted file mode 100644 index 0636a54ed..000000000 --- a/guidance/reference_filter_dp_quat/test/test_waypoint_utils.cpp +++ /dev/null @@ -1,141 +0,0 @@ -#include -#include -#include "reference_filter_dp_quat/lib/waypoint_utils.hpp" - -namespace vortex::guidance { - -// --- compute_waypoint_goal tests --- - -TEST(ComputeWaypointGoal, FullPoseReturnsInputUnchanged) { - Pose incoming{1.0, 2.0, 3.0, 1.0, 0.0, 0.0, 0.0}; - Pose current{}; - - Pose result = - compute_waypoint_goal(incoming, WaypointMode::FULL_POSE, current); - - EXPECT_DOUBLE_EQ(result.x, 1.0); - EXPECT_DOUBLE_EQ(result.y, 2.0); - EXPECT_DOUBLE_EQ(result.z, 3.0); - EXPECT_DOUBLE_EQ(result.qw, 1.0); - EXPECT_DOUBLE_EQ(result.qx, 0.0); - EXPECT_DOUBLE_EQ(result.qy, 0.0); - EXPECT_DOUBLE_EQ(result.qz, 0.0); -} - -TEST(ComputeWaypointGoal, OnlyPositionKeepsOrientationFromState) { - Pose incoming{1.0, 2.0, 3.0, 1.0, 0.0, 0.0, 0.0}; - - // Current state with a non-identity orientation (90 deg about Z) - Eigen::Quaterniond q(Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitZ())); - Pose current = Pose::from_eigen(Eigen::Vector3d(10.0, 20.0, 30.0), q); - - Pose result = - compute_waypoint_goal(incoming, WaypointMode::ONLY_POSITION, current); - - EXPECT_DOUBLE_EQ(result.x, 1.0); - EXPECT_DOUBLE_EQ(result.y, 2.0); - EXPECT_DOUBLE_EQ(result.z, 3.0); - - // Orientation should match current state - Eigen::Quaterniond result_q = result.ori_quaternion(); - EXPECT_TRUE(result_q.isApprox(q.normalized(), 1e-12)); -} - -TEST(ComputeWaypointGoal, ForwardHeadingComputesYawFromDelta) { - Pose incoming{1.0, 1.0, 0.0, 1.0, 0.0, 0.0, 0.0}; - Pose current{}; - - Pose result = - compute_waypoint_goal(incoming, WaypointMode::FORWARD_HEADING, current); - - EXPECT_DOUBLE_EQ(result.x, 1.0); - EXPECT_DOUBLE_EQ(result.y, 1.0); - - // Expected yaw = atan2(1, 1) = pi/4 - double expected_yaw = std::atan2(1.0, 1.0); - Eigen::Quaterniond expected_q( - Eigen::AngleAxisd(expected_yaw, Eigen::Vector3d::UnitZ())); - - Eigen::Quaterniond result_q = result.ori_quaternion(); - EXPECT_TRUE(result_q.isApprox(expected_q.normalized(), 1e-12)); -} - -TEST(ComputeWaypointGoal, OnlyOrientationKeepsPositionFromState) { - Eigen::Quaterniond q(Eigen::AngleAxisd(0.3, Eigen::Vector3d::UnitZ())); - Pose incoming = Pose::from_eigen(Eigen::Vector3d(1.0, 2.0, 3.0), q); - Pose current{10.0, 20.0, 30.0, 1.0, 0.0, 0.0, 0.0}; - - Pose result = compute_waypoint_goal( - incoming, WaypointMode::ONLY_ORIENTATION, current); - - EXPECT_DOUBLE_EQ(result.x, 10.0); - EXPECT_DOUBLE_EQ(result.y, 20.0); - EXPECT_DOUBLE_EQ(result.z, 30.0); - - Eigen::Quaterniond result_q = result.ori_quaternion(); - EXPECT_TRUE(result_q.isApprox(q.normalized(), 1e-12)); -} - -// --- has_converged tests --- - -TEST(HasConverged, FullPoseBelowThreshold) { - Pose measured{1.0, 2.0, 3.0, 1.0, 0.0, 0.0, 0.0}; - Pose reference{1.001, 2.0, 3.0, 1.0, 0.0, 0.0, 0.0}; - - EXPECT_TRUE( - has_converged(measured, reference, WaypointMode::FULL_POSE, 0.1)); -} - -TEST(HasConverged, FullPoseAboveThreshold) { - Pose measured{}; - Pose reference{1.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0}; - - EXPECT_FALSE( - has_converged(measured, reference, WaypointMode::FULL_POSE, 0.1)); -} - -TEST(HasConverged, OnlyPositionIgnoresOrientation) { - Pose measured{1.0, 2.0, 3.0, 1.0, 0.0, 0.0, 0.0}; - - // Same position, very different orientation - Eigen::Quaterniond q(Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitZ())); - Pose reference = Pose::from_eigen(Eigen::Vector3d(1.0, 2.0, 3.0), q); - - EXPECT_TRUE( - has_converged(measured, reference, WaypointMode::ONLY_POSITION, 0.1)); -} - -TEST(HasConverged, OnlyOrientationIgnoresPosition) { - Eigen::Quaterniond q(Eigen::AngleAxisd(0.1, Eigen::Vector3d::UnitZ())); - Pose measured = Pose::from_eigen(Eigen::Vector3d(100.0, 200.0, 300.0), q); - Pose reference = Pose::from_eigen(Eigen::Vector3d(0.0, 0.0, 0.0), q); - - EXPECT_TRUE(has_converged(measured, reference, - WaypointMode::ONLY_ORIENTATION, 0.1)); -} - -TEST(HasConverged, ForwardHeadingUsesPositionAndYawOnly) { - // Same position and yaw, but different roll (single axis keeps error - // purely in x-component, so z-component of quaternion_error is zero) - Eigen::Quaterniond q_measured = - Eigen::AngleAxisd(0.1, Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(0.5, Eigen::Vector3d::UnitX()); - Eigen::Quaterniond q_reference( - Eigen::AngleAxisd(0.1, Eigen::Vector3d::UnitZ())); - - Pose measured = - Pose::from_eigen(Eigen::Vector3d(1.0, 2.0, 3.0), q_measured); - Pose reference = - Pose::from_eigen(Eigen::Vector3d(1.0, 2.0, 3.0), q_reference); - - // Roll differs but should be ignored in FORWARD_HEADING mode - EXPECT_TRUE( - has_converged(measured, reference, WaypointMode::FORWARD_HEADING, 0.1)); -} - -} // namespace vortex::guidance - -int main(int argc, char** argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} From aa900c9c2f02f35e57eb188dcf9f1227a1beebab Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Fri, 27 Mar 2026 13:36:32 +0100 Subject: [PATCH 228/290] ref_RPY remove feedback --- .../reference_filter_dp/src/ros/reference_filter_ros.cpp | 6 ------ 1 file changed, 6 deletions(-) diff --git a/guidance/reference_filter_dp/src/ros/reference_filter_ros.cpp b/guidance/reference_filter_dp/src/ros/reference_filter_ros.cpp index a63f54a56..1393be495 100644 --- a/guidance/reference_filter_dp/src/ros/reference_filter_ros.cpp +++ b/guidance/reference_filter_dp/src/ros/reference_filter_ros.cpp @@ -170,8 +170,6 @@ void ReferenceFilterNode::execute( follower_->start(pose, twist, wp, convergence_threshold); - auto feedback = std::make_shared< - vortex_msgs::action::ReferenceFilterWaypoint::Feedback>(); auto result = std::make_shared< vortex_msgs::action::ReferenceFilterWaypoint::Result>(); @@ -208,8 +206,6 @@ void ReferenceFilterNode::execute( vortex_msgs::msg::ReferenceFilter final_reference_msg = fill_reference_msg(follower_->state()); - feedback->reference = final_reference_msg; - goal_handle->publish_feedback(feedback); reference_pub_->publish(final_reference_msg); result->success = true; @@ -221,8 +217,6 @@ void ReferenceFilterNode::execute( vortex_msgs::msg::ReferenceFilter reference_msg = fill_reference_msg(filter_state); reference_pub_->publish(reference_msg); - feedback->reference = reference_msg; - goal_handle->publish_feedback(feedback); loop_rate.sleep(); } if (!rclcpp::ok() && goal_handle->is_active()) { From 7458952a26804c25130828bff5b2e8a74855354c Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Fri, 27 Mar 2026 14:05:43 +0100 Subject: [PATCH 229/290] use local WaypointEuler type --- .../lib/waypoint_follower.hpp | 2 +- .../lib/waypoint_types.hpp | 18 +- .../ros/reference_filter_ros_utils.hpp | 4 +- .../src/lib/waypoint_follower.cpp | 2 +- .../src/lib/waypoint_utils.cpp | 70 +++---- .../reference_filter_dp/test/CMakeLists.txt | 18 -- .../test/test_waypoint_follower.cpp | 10 +- .../test/test_waypoint_utils.cpp | 174 ------------------ 8 files changed, 34 insertions(+), 264 deletions(-) delete mode 100644 guidance/reference_filter_dp/test/test_waypoint_utils.cpp diff --git a/guidance/reference_filter_dp/include/reference_filter_dp/lib/waypoint_follower.hpp b/guidance/reference_filter_dp/include/reference_filter_dp/lib/waypoint_follower.hpp index 121a9f431..826abdc25 100644 --- a/guidance/reference_filter_dp/include/reference_filter_dp/lib/waypoint_follower.hpp +++ b/guidance/reference_filter_dp/include/reference_filter_dp/lib/waypoint_follower.hpp @@ -30,7 +30,7 @@ class WaypointFollower { */ void start(const PoseEuler& pose, const Twist& twist, - const Waypoint& waypoint, + const WaypointEuler& waypoint, double convergence_threshold); /** diff --git a/guidance/reference_filter_dp/include/reference_filter_dp/lib/waypoint_types.hpp b/guidance/reference_filter_dp/include/reference_filter_dp/lib/waypoint_types.hpp index ed8e8e2db..556316764 100644 --- a/guidance/reference_filter_dp/include/reference_filter_dp/lib/waypoint_types.hpp +++ b/guidance/reference_filter_dp/include/reference_filter_dp/lib/waypoint_types.hpp @@ -1,30 +1,18 @@ #ifndef REFERENCE_FILTER_DP__LIB__WAYPOINT_TYPES_HPP_ #define REFERENCE_FILTER_DP__LIB__WAYPOINT_TYPES_HPP_ -#include #include namespace vortex::guidance { using vortex::utils::types::PoseEuler; - -/** - * @brief Determines which degrees of freedom the reference filter controls. - * - * The mode affects both the reference goal computation (via apply_mode_logic) - * and the convergence check (via has_converged). - */ -enum class WaypointMode : uint8_t { - FULL_POSE = 0, ///< Control all 6 DOF. - ONLY_POSITION = 1, ///< Control x, y, z; hold current orientation. - FORWARD_HEADING = 2, ///< Control x, y, z with yaw toward target. - ONLY_ORIENTATION = 3, ///< Control roll, pitch, yaw; hold current position. -}; +using vortex::utils::types::WaypointMode; /** * @brief A target pose with an associated waypoint mode. + * */ -struct Waypoint { +struct WaypointEuler { PoseEuler pose{}; WaypointMode mode = WaypointMode::FULL_POSE; }; diff --git a/guidance/reference_filter_dp/include/reference_filter_dp/ros/reference_filter_ros_utils.hpp b/guidance/reference_filter_dp/include/reference_filter_dp/ros/reference_filter_ros_utils.hpp index 35c8a9927..43d66ce56 100644 --- a/guidance/reference_filter_dp/include/reference_filter_dp/ros/reference_filter_ros_utils.hpp +++ b/guidance/reference_filter_dp/include/reference_filter_dp/ros/reference_filter_ros_utils.hpp @@ -35,9 +35,9 @@ inline WaypointMode waypoint_mode_from_ros( } /// @brief Convert a ROS Waypoint message to an internal Waypoint struct. -inline vortex::guidance::Waypoint waypoint_from_ros( +inline vortex::guidance::WaypointEuler waypoint_from_ros( const vortex_msgs::msg::Waypoint& ros_wp) { - Waypoint wp; + WaypointEuler wp; wp.pose = vortex::utils::ros_conversions::ros_pose_to_pose_euler(ros_wp.pose); wp.mode = waypoint_mode_from_ros(ros_wp.waypoint_mode); diff --git a/guidance/reference_filter_dp/src/lib/waypoint_follower.cpp b/guidance/reference_filter_dp/src/lib/waypoint_follower.cpp index 4da1d7c31..cdf8d7e64 100644 --- a/guidance/reference_filter_dp/src/lib/waypoint_follower.cpp +++ b/guidance/reference_filter_dp/src/lib/waypoint_follower.cpp @@ -11,7 +11,7 @@ WaypointFollower::WaypointFollower(const ReferenceFilterParams& params, void WaypointFollower::start(const PoseEuler& pose, const Twist& twist, - const Waypoint& waypoint, + const WaypointEuler& waypoint, double convergence_threshold) { std::lock_guard lock(mutex_); state_ = compute_initial_state(pose, twist); diff --git a/guidance/reference_filter_dp/src/lib/waypoint_utils.cpp b/guidance/reference_filter_dp/src/lib/waypoint_utils.cpp index fe72ebf13..6d72a27b5 100644 --- a/guidance/reference_filter_dp/src/lib/waypoint_utils.cpp +++ b/guidance/reference_filter_dp/src/lib/waypoint_utils.cpp @@ -1,9 +1,22 @@ #include "reference_filter_dp/lib/waypoint_utils.hpp" #include #include +#include +#include namespace vortex::guidance { +namespace { +using vortex::utils::types::PoseEuler; +using vortex::utils::types::Pose; + +Pose vec_to_pose(const Eigen::Vector6d& v) { + return PoseEuler{.x = v(0), .y = v(1), .z = v(2), + .roll = v(3), .pitch = v(4), .yaw = v(5)} + .as_pose(); +} +} // namespace + Eigen::Vector6d apply_mode_logic(const Eigen::Vector6d& reference_in, WaypointMode mode, const Eigen::Vector6d& current_state) { @@ -22,32 +35,13 @@ Eigen::Vector6d apply_mode_logic(const Eigen::Vector6d& reference_in, break; case WaypointMode::ONLY_POSITION: - reference_out(3) = current_state(3); - reference_out(4) = current_state(4); - reference_out(5) = current_state(5); - break; - - case WaypointMode::FORWARD_HEADING: { - double dx = reference_in(0) - current_state(0); - double dy = reference_in(1) - current_state(1); - double forward_heading = std::atan2(dy, dx); - - reference_out(3) = 0.0; - reference_out(4) = 0.0; - reference_out(5) = vortex::utils::math::ssa(forward_heading); - break; - } - + case WaypointMode::FORWARD_HEADING: case WaypointMode::ONLY_ORIENTATION: - reference_out(0) = current_state(0); - reference_out(1) = current_state(1); - reference_out(2) = current_state(2); - reference_out(3) = - current_state(3) + ssa(reference_in(3) - current_state(3)); - reference_out(4) = - current_state(4) + ssa(reference_in(4) - current_state(4)); - reference_out(5) = - current_state(5) + ssa(reference_in(5) - current_state(5)); + reference_out = vortex::utils::waypoints::compute_waypoint_goal( + vec_to_pose(reference_in), mode, + vec_to_pose(current_state)) + .as_pose_euler() + .to_vector(); break; } @@ -58,29 +52,9 @@ bool has_converged(const Eigen::Vector6d& measured_pose, const Eigen::Vector6d& reference, WaypointMode mode, double convergence_threshold) { - using vortex::utils::math::ssa; - const Eigen::Vector3d ep = measured_pose.head<3>() - reference.head<3>(); - - Eigen::Vector3d ea; - ea(0) = ssa(measured_pose(3) - reference(3)); - ea(1) = ssa(measured_pose(4) - reference(4)); - ea(2) = ssa(measured_pose(5) - reference(5)); - - const double err = [&] { - switch (mode) { - case WaypointMode::ONLY_POSITION: - return ep.norm(); - case WaypointMode::ONLY_ORIENTATION: - return ea.norm(); - case WaypointMode::FORWARD_HEADING: - return std::sqrt(ep.squaredNorm() + ea(2) * ea(2)); - case WaypointMode::FULL_POSE: - default: - return std::sqrt(ep.squaredNorm() + ea.squaredNorm()); - } - }(); - - return err < convergence_threshold; + return vortex::utils::waypoints::has_converged( + vec_to_pose(measured_pose), vec_to_pose(reference), mode, + convergence_threshold); } } // namespace vortex::guidance diff --git a/guidance/reference_filter_dp/test/CMakeLists.txt b/guidance/reference_filter_dp/test/CMakeLists.txt index 2b8229c0c..9295a4300 100644 --- a/guidance/reference_filter_dp/test/CMakeLists.txt +++ b/guidance/reference_filter_dp/test/CMakeLists.txt @@ -20,24 +20,6 @@ ament_target_dependencies(${TEST_BINARY_NAME} PUBLIC Eigen3) gtest_discover_tests(${TEST_BINARY_NAME}) -# Waypoint utils tests -set(TEST_WAYPOINT_UTILS ${PROJECT_NAME}_test_waypoint_utils) -add_executable( - ${TEST_WAYPOINT_UTILS} - test_waypoint_utils.cpp -) - -target_link_libraries( - ${TEST_WAYPOINT_UTILS} - PRIVATE - ${CORE_LIB_NAME} - GTest::GTest -) - -ament_target_dependencies(${TEST_WAYPOINT_UTILS} PUBLIC Eigen3 vortex_utils) - -gtest_discover_tests(${TEST_WAYPOINT_UTILS}) - # Waypoint follower tests set(TEST_WAYPOINT_FOLLOWER ${PROJECT_NAME}_test_waypoint_follower) add_executable( diff --git a/guidance/reference_filter_dp/test/test_waypoint_follower.cpp b/guidance/reference_filter_dp/test/test_waypoint_follower.cpp index 06e063d0f..1c0c6b6db 100644 --- a/guidance/reference_filter_dp/test/test_waypoint_follower.cpp +++ b/guidance/reference_filter_dp/test/test_waypoint_follower.cpp @@ -20,7 +20,7 @@ class WaypointFollowerTests : public ::testing::Test { TEST_F(WaypointFollowerTests, StartAndStepConverges) { WaypointFollower follower(get_params(), 0.01); - Waypoint wp; + WaypointEuler wp; wp.pose = PoseEuler{1.0, 0.0, 0.0, 0.0, 0.0, 0.0}; wp.mode = WaypointMode::FULL_POSE; @@ -39,7 +39,7 @@ TEST_F(WaypointFollowerTests, StartAndStepConverges) { TEST_F(WaypointFollowerTests, StepDoesNotConvergeWhenFar) { WaypointFollower follower(get_params(), 0.01); - Waypoint wp; + WaypointEuler wp; wp.pose = PoseEuler{10.0, 10.0, 0.0, 0.0, 0.0, 0.0}; wp.mode = WaypointMode::FULL_POSE; @@ -54,7 +54,7 @@ TEST_F(WaypointFollowerTests, StepDoesNotConvergeWhenFar) { TEST_F(WaypointFollowerTests, SetReferenceUpdatesMidSequence) { WaypointFollower follower(get_params(), 0.01); - Waypoint wp; + WaypointEuler wp; wp.pose = PoseEuler{1.0, 0.0, 0.0, 0.0, 0.0, 0.0}; wp.mode = WaypointMode::FULL_POSE; @@ -70,7 +70,7 @@ TEST_F(WaypointFollowerTests, SetReferenceUpdatesMidSequence) { TEST_F(WaypointFollowerTests, SnapStateToReference) { WaypointFollower follower(get_params(), 0.01); - Waypoint wp; + WaypointEuler wp; wp.pose = PoseEuler{3.0, 4.0, 5.0, 0.1, 0.2, 0.3}; wp.mode = WaypointMode::FULL_POSE; @@ -88,7 +88,7 @@ TEST_F(WaypointFollowerTests, SnapStateToReference) { TEST_F(WaypointFollowerTests, StateEvolvesWithStep) { WaypointFollower follower(get_params(), 0.01); - Waypoint wp; + WaypointEuler wp; wp.pose = PoseEuler{1.0, 0.0, 0.0, 0.0, 0.0, 0.0}; wp.mode = WaypointMode::FULL_POSE; diff --git a/guidance/reference_filter_dp/test/test_waypoint_utils.cpp b/guidance/reference_filter_dp/test/test_waypoint_utils.cpp deleted file mode 100644 index 6e9de1b82..000000000 --- a/guidance/reference_filter_dp/test/test_waypoint_utils.cpp +++ /dev/null @@ -1,174 +0,0 @@ -#include -#include -#include "reference_filter_dp/lib/waypoint_utils.hpp" - -namespace vortex::guidance { - -// --- apply_mode_logic tests --- - -TEST(ApplyModeLogic, FullPoseWrapsAnglesToShortestPath) { - Eigen::Vector6d r_in; - r_in << 1.0, 2.0, 3.0, 0.1, 0.2, 0.3; - Eigen::Vector6d state = Eigen::Vector6d::Zero(); - - Eigen::Vector6d result = - apply_mode_logic(r_in, WaypointMode::FULL_POSE, state); - - // Position unchanged - EXPECT_DOUBLE_EQ(result(0), 1.0); - EXPECT_DOUBLE_EQ(result(1), 2.0); - EXPECT_DOUBLE_EQ(result(2), 3.0); - - // Small angles: wrapping has no effect - EXPECT_NEAR(result(3), 0.1, 1e-12); - EXPECT_NEAR(result(4), 0.2, 1e-12); - EXPECT_NEAR(result(5), 0.3, 1e-12); -} - -TEST(ApplyModeLogic, FullPoseTakesShortestRotation) { - // Current yaw ~170°, reference yaw ~-170° => only ~20° apart - // Without wrapping, the filter would see a ~340° difference - Eigen::Vector6d state; - state << 0.0, 0.0, 0.0, 0.0, 0.0, 2.96; // ~170° - Eigen::Vector6d r_in; - r_in << 0.0, 0.0, 0.0, 0.0, 0.0, -2.96; // ~-170° - - Eigen::Vector6d result = - apply_mode_logic(r_in, WaypointMode::FULL_POSE, state); - - // The wrapped reference should be close to the current state + short - // rotation - double yaw_error = result(5) - state(5); - EXPECT_NEAR(yaw_error, -2.96 - 2.96 + 2.0 * M_PI, 1e-12); - EXPECT_LT(std::abs(yaw_error), M_PI + 1e-9); -} - -TEST(ApplyModeLogic, OnlyPositionKeepsOrientationFromState) { - Eigen::Vector6d r_in; - r_in << 1.0, 2.0, 3.0, 0.1, 0.2, 0.3; - Eigen::Vector6d state; - state << 10.0, 20.0, 30.0, 0.4, 0.5, 0.6; - - Eigen::Vector6d result = - apply_mode_logic(r_in, WaypointMode::ONLY_POSITION, state); - - EXPECT_DOUBLE_EQ(result(0), 1.0); - EXPECT_DOUBLE_EQ(result(1), 2.0); - EXPECT_DOUBLE_EQ(result(2), 3.0); - EXPECT_DOUBLE_EQ(result(3), 0.4); - EXPECT_DOUBLE_EQ(result(4), 0.5); - EXPECT_DOUBLE_EQ(result(5), 0.6); -} - -TEST(ApplyModeLogic, ForwardHeadingComputesYawFromDelta) { - Eigen::Vector6d r_in; - r_in << 1.0, 1.0, 0.0, 0.0, 0.0, 0.0; - Eigen::Vector6d state = Eigen::Vector6d::Zero(); - - Eigen::Vector6d result = - apply_mode_logic(r_in, WaypointMode::FORWARD_HEADING, state); - - double expected_yaw = std::atan2(1.0, 1.0); // pi/4 - EXPECT_DOUBLE_EQ(result(0), 1.0); - EXPECT_DOUBLE_EQ(result(1), 1.0); - EXPECT_DOUBLE_EQ(result(3), 0.0); - EXPECT_DOUBLE_EQ(result(4), 0.0); - EXPECT_NEAR(result(5), expected_yaw, 1e-12); -} - -TEST(ApplyModeLogic, OnlyOrientationKeepsPositionFromState) { - Eigen::Vector6d r_in; - r_in << 1.0, 2.0, 3.0, 0.1, 0.2, 0.3; - Eigen::Vector6d state; - state << 10.0, 20.0, 30.0, 0.4, 0.5, 0.6; - - Eigen::Vector6d result = - apply_mode_logic(r_in, WaypointMode::ONLY_ORIENTATION, state); - - EXPECT_DOUBLE_EQ(result(0), 10.0); - EXPECT_DOUBLE_EQ(result(1), 20.0); - EXPECT_DOUBLE_EQ(result(2), 30.0); - - // Small angle differences: wrapping has no effect, result matches input - EXPECT_NEAR(result(3), 0.1, 1e-12); - EXPECT_NEAR(result(4), 0.2, 1e-12); - EXPECT_NEAR(result(5), 0.3, 1e-12); -} - -TEST(ApplyModeLogic, OnlyOrientationTakesShortestRotation) { - Eigen::Vector6d state; - state << 10.0, 20.0, 30.0, 0.0, 0.0, 2.96; - Eigen::Vector6d r_in; - r_in << 1.0, 2.0, 3.0, 0.0, 0.0, -2.96; - - Eigen::Vector6d result = - apply_mode_logic(r_in, WaypointMode::ONLY_ORIENTATION, state); - - // Position from state - EXPECT_DOUBLE_EQ(result(0), 10.0); - EXPECT_DOUBLE_EQ(result(1), 20.0); - EXPECT_DOUBLE_EQ(result(2), 30.0); - - // Yaw should take the shortest path - double yaw_error = result(5) - state(5); - EXPECT_LT(std::abs(yaw_error), M_PI + 1e-9); -} - -// --- has_converged tests --- - -TEST(HasConverged, FullPoseBelowThreshold) { - Eigen::Vector6d measured; - measured << 1.0, 2.0, 3.0, 0.1, 0.2, 0.3; - Eigen::Vector6d reference = measured; - reference(0) += 0.001; - - EXPECT_TRUE( - has_converged(measured, reference, WaypointMode::FULL_POSE, 0.1)); -} - -TEST(HasConverged, FullPoseAboveThreshold) { - Eigen::Vector6d measured = Eigen::Vector6d::Zero(); - Eigen::Vector6d reference; - reference << 1.0, 1.0, 1.0, 0.0, 0.0, 0.0; - - EXPECT_FALSE( - has_converged(measured, reference, WaypointMode::FULL_POSE, 0.1)); -} - -TEST(HasConverged, OnlyPositionIgnoresOrientation) { - Eigen::Vector6d measured; - measured << 1.0, 2.0, 3.0, 0.0, 0.0, 0.0; - Eigen::Vector6d reference; - reference << 1.0, 2.0, 3.0, 1.0, 1.0, 1.0; - - EXPECT_TRUE( - has_converged(measured, reference, WaypointMode::ONLY_POSITION, 0.1)); -} - -TEST(HasConverged, OnlyOrientationIgnoresPosition) { - Eigen::Vector6d measured; - measured << 100.0, 200.0, 300.0, 0.1, 0.2, 0.3; - Eigen::Vector6d reference; - reference << 0.0, 0.0, 0.0, 0.1, 0.2, 0.3; - - EXPECT_TRUE(has_converged(measured, reference, - WaypointMode::ONLY_ORIENTATION, 0.1)); -} - -TEST(HasConverged, ForwardHeadingUsesPositionAndYawOnly) { - Eigen::Vector6d measured; - measured << 1.0, 2.0, 3.0, 0.5, 0.5, 0.1; - Eigen::Vector6d reference; - reference << 1.0, 2.0, 3.0, 0.0, 0.0, 0.1; - - // Roll and pitch differ but should be ignored - EXPECT_TRUE( - has_converged(measured, reference, WaypointMode::FORWARD_HEADING, 0.1)); -} - -} // namespace vortex::guidance - -int main(int argc, char** argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} From 155a7dbe7dd2f8158ccf08dacd31bfa990ca72d5 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Fri, 27 Mar 2026 14:07:36 +0100 Subject: [PATCH 230/290] eskf remove stonefish dependency --- navigation/eskf/CMakeLists.txt | 1 - navigation/eskf/include/eskf/eskf_ros.hpp | 1 - navigation/eskf/package.xml | 1 - 3 files changed, 3 deletions(-) diff --git a/navigation/eskf/CMakeLists.txt b/navigation/eskf/CMakeLists.txt index 5ceb26e19..b5df00c95 100644 --- a/navigation/eskf/CMakeLists.txt +++ b/navigation/eskf/CMakeLists.txt @@ -27,7 +27,6 @@ find_package(spdlog REQUIRED) find_package(fmt REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2_eigen REQUIRED) -find_package(stonefish_ros2 REQUIRED) if(NOT DEFINED EIGEN3_INCLUDE_DIR) set(EIGEN3_INCLUDE_DIR ${EIGEN3_INCLUDE_DIRS}) diff --git a/navigation/eskf/include/eskf/eskf_ros.hpp b/navigation/eskf/include/eskf/eskf_ros.hpp index 1f1b4ad54..3dbe84c51 100644 --- a/navigation/eskf/include/eskf/eskf_ros.hpp +++ b/navigation/eskf/include/eskf/eskf_ros.hpp @@ -16,7 +16,6 @@ #include #include #include -#include #include #include "eskf/eskf.hpp" #include "eskf/typedefs.hpp" diff --git a/navigation/eskf/package.xml b/navigation/eskf/package.xml index 85cd79ff6..5d3f9fb9d 100644 --- a/navigation/eskf/package.xml +++ b/navigation/eskf/package.xml @@ -19,7 +19,6 @@ vortex_utils_ros tf2_ros tf2_eigen - stonefish_ros2 ament_cmake From ba978f231e90626dc7e5111ef03250f6f00d41b1 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Fri, 27 Mar 2026 14:13:53 +0100 Subject: [PATCH 231/290] eskf actually remove stonefish dep --- navigation/eskf/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/navigation/eskf/CMakeLists.txt b/navigation/eskf/CMakeLists.txt index b5df00c95..b292b256d 100644 --- a/navigation/eskf/CMakeLists.txt +++ b/navigation/eskf/CMakeLists.txt @@ -57,7 +57,6 @@ ament_target_dependencies(${LIB_NAME} PUBLIC fmt tf2_ros tf2_eigen - stonefish_ros2 ) rclcpp_components_register_node( From fe941b20ba0e205545cf5b3a35033d7cf9948faa Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Fri, 27 Mar 2026 19:43:31 +0100 Subject: [PATCH 232/290] update tf2 lookups --- navigation/eskf/config/eskf_params.yaml | 23 +-- navigation/eskf/include/eskf/eskf_ros.hpp | 18 ++- navigation/eskf/launch/eskf.launch.py | 2 +- navigation/eskf/src/eskf.cpp | 6 +- navigation/eskf/src/eskf_ros.cpp | 177 ++++++++++------------ 5 files changed, 110 insertions(+), 116 deletions(-) diff --git a/navigation/eskf/config/eskf_params.yaml b/navigation/eskf/config/eskf_params.yaml index 15837f13e..ad17654d1 100644 --- a/navigation/eskf/config/eskf_params.yaml +++ b/navigation/eskf/config/eskf_params.yaml @@ -3,16 +3,19 @@ ros__parameters: diag_Q_std: [0.05, 0.05, 0.1, 0.01, 0.01, 0.02, 0.001, 0.001, 0.001, 0.0001, 0.0001, 0.0001] diag_p_init: [1.0, 1.0, 1.0, 0.5, 0.5, 0.5, 0.1, 0.1, 0.1, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001] - imu_frame_r: [ -1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, -1.0 ] - imu_frame_t: [ 0.0, 0.0, 0.0 ] + transform: + imu_frame_r: [ -1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, -1.0 ] + imu_frame_t: [ 0.0, 0.0, 0.0 ] - dvl_frame_r: [ 0.0, -1.0, 0.0, - 1.0, 0.0, 0.0, - 0.0, 0.0, 1.0 ] + dvl_frame_r: [ 0.0, -1.0, 0.0, + 1.0, 0.0, 0.0, + 0.0, 0.0, 1.0 ] - dvl_frame_t: [ 0.4, 0.0, 0.2 ] + dvl_frame_t: [ 0.4, 0.0, 0.2 ] - depth_frame_t: [ 0.0, 0.0, 0.0 ] - use_tf_transforms: false + depth_frame_t: [ 0.0, 0.0, 0.0 ] + use_tf_transforms: true + publish_tf: true + publish_rate_ms: 1 diff --git a/navigation/eskf/include/eskf/eskf_ros.hpp b/navigation/eskf/include/eskf/eskf_ros.hpp index 3dbe84c51..5959e500b 100644 --- a/navigation/eskf/include/eskf/eskf_ros.hpp +++ b/navigation/eskf/include/eskf/eskf_ros.hpp @@ -46,10 +46,16 @@ class ESKFNode : public rclcpp::Node { void set_parameters(); // @brief lookup transforms - void initialize_static_transforms(); + void lookup_static_transforms(); + + // @brief Create subs/pubs and start the publish timer. Called once + // transforms are available (or immediately if use_tf_transforms_ is false). + void complete_initialization(); // @brief broadcast the State as a TF - void publish_tf(const StateQuat& nom_state); + void publish_tf(const StateQuat& nom_state, const rclcpp::Time& current_time); + + // Startup message // Subscribers and Publishers @@ -64,7 +70,7 @@ class ESKFNode : public rclcpp::Node { // Member variable for the ESKF instance - std::chrono::milliseconds time_step; + std::chrono::milliseconds time_step_{1}; rclcpp::TimerBase::SharedPtr odom_pub_timer_; @@ -91,9 +97,15 @@ class ESKFNode : public rclcpp::Node { std::unique_ptr tf_broadcaster_; rclcpp::TimerBase::SharedPtr tf_timer_; + std::string frame(const std::string& name) const { + return frame_prefix_.empty() ? name : frame_prefix_ + "/" + name; + } + // Flags and Storage + std::string frame_prefix_{""}; bool use_tf_transforms_ = false; bool tf_sensors_loaded_ = false; + bool publish_tf_{false}; // hold the transfer from Sensor -> Base Link Eigen::Isometry3d Tf_base_imu_ = Eigen::Isometry3d::Identity(); diff --git a/navigation/eskf/launch/eskf.launch.py b/navigation/eskf/launch/eskf.launch.py index cdb4a2aac..94b98584f 100644 --- a/navigation/eskf/launch/eskf.launch.py +++ b/navigation/eskf/launch/eskf.launch.py @@ -30,7 +30,7 @@ def launch_setup(context, *args, **kwargs): executable="eskf_node", name="eskf_node", namespace=namespace, - parameters=[eskf_params, drone_params], + parameters=[eskf_params, drone_params, {"frame_prefix": namespace}], output="screen", ) diff --git a/navigation/eskf/src/eskf.cpp b/navigation/eskf/src/eskf.cpp index 876410714..62497c5bd 100644 --- a/navigation/eskf/src/eskf.cpp +++ b/navigation/eskf/src/eskf.cpp @@ -20,8 +20,10 @@ ESKF::ESKF(const EskfParams& params) : Q_(params.Q) { // Initialize Quaternion: -90 degrees Yaw because of initial // drone_orientation - Eigen::AngleAxisd init_rotation(-M_PI / 2.0, Eigen::Vector3d::UnitZ()); - current_nom_state_.quat = Eigen::Quaterniond(init_rotation); + // Eigen::AngleAxisd init_rotation(-M_PI / 2.0, Eigen::Vector3d::UnitZ()); + + // initialize to identity quat + current_nom_state_.quat = Eigen::Quaterniond::Identity(); current_nom_state_.quat.normalize(); } diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp index 21239ff99..780e2b7f9 100644 --- a/navigation/eskf/src/eskf_ros.cpp +++ b/navigation/eskf/src/eskf_ros.cpp @@ -15,64 +15,54 @@ auto start_message{R"( ESKFNode::ESKFNode(const rclcpp::NodeOptions& options) : Node("eskf_node", options) { - time_step = std::chrono::milliseconds(1); - odom_pub_timer_ = this->create_wall_timer( - time_step, std::bind(&ESKFNode::publish_odom, this)); - - set_subscribers_and_publisher(); - set_parameters(); - - // Initialize TF Buffer & Listener - tf_buffer_ = std::make_shared(this->get_clock()); - tf_listener_ = std::make_shared(*tf_buffer_); + use_tf_transforms_ = this->declare_parameter("use_tf_transforms"); + tf_sensors_loaded_ = !use_tf_transforms_; - // Initialize Broadcaster (for odom -> base_link) - tf_broadcaster_ = std::make_unique(*this); + frame_prefix_ = this->declare_parameter("frame_prefix", ""); + if (!frame_prefix_.empty() && frame_prefix_.back() == '/') { + frame_prefix_.pop_back(); + } - // flag to determine whether to use TF-based transforms or parameter-based - // transforms - this->declare_parameter("use_tf_transforms", true); - use_tf_transforms_ = this->get_parameter("use_tf_transforms").as_bool(); + publish_tf_ = this->declare_parameter("publish_tf"); + if (publish_tf_) { + tf_broadcaster_ = std::make_unique(*this); + } - // if we have parameters, we skip the TF lookup - tf_sensors_loaded_ = !use_tf_transforms_; + // Declare these here so they appear in `ros2 param list` from startup, + // even though they are read in complete_initialization(). + this->declare_parameter("publish_rate_ms"); + this->declare_parameter("topics.imu"); + this->declare_parameter("topics.dvl_twist"); + this->declare_parameter("topics.odom"); if (use_tf_transforms_) { - // Check for static transforms every 0.5 seconds + tf_buffer_ = std::make_shared(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); tf_timer_ = this->create_wall_timer( std::chrono::milliseconds(500), - std::bind(&ESKFNode::initialize_static_transforms, this)); + std::bind(&ESKFNode::lookup_static_transforms, this)); } else { spdlog::info( "Using parameter-based sensor transforms. TF lookup disabled."); + complete_initialization(); } - - spdlog::info(start_message); - -#ifndef NDEBUG - spdlog::info( - "______________________Debug mode is enabled______________________"); -#endif } void ESKFNode::set_subscribers_and_publisher() { auto qos_sensor_data = vortex::utils::qos_profiles::sensor_data_profile(1); - this->declare_parameter("topics.imu"); std::string imu_topic = this->get_parameter("topics.imu").as_string(); imu_sub_ = this->create_subscription( imu_topic, qos_sensor_data, std::bind(&ESKFNode::imu_callback, this, std::placeholders::_1)); - this->declare_parameter("topics.dvl_twist"); std::string dvl_topic = this->get_parameter("topics.dvl_twist").as_string(); dvl_sub_ = this->create_subscription< geometry_msgs::msg::TwistWithCovarianceStamped>( dvl_topic, qos_sensor_data, std::bind(&ESKFNode::dvl_callback, this, std::placeholders::_1)); - this->declare_parameter("topics.odom"); std::string odom_topic = this->get_parameter("topics.odom").as_string(); odom_pub_ = this->create_publisher( odom_topic, qos_sensor_data); @@ -84,28 +74,26 @@ void ESKFNode::set_subscribers_and_publisher() { } void ESKFNode::set_parameters() { - // Load sensor frame Rotation correction parameters - std::vector R_imu_correction; - this->declare_parameter>("imu_frame_r"); - R_imu_correction = get_parameter("imu_frame_r").as_double_array(); - R_imu_eskf_ = Eigen::Map>( - R_imu_correction.data()); - - std::vector R_dvl_correction; - this->declare_parameter>("dvl_frame_r"); - R_dvl_correction = get_parameter("dvl_frame_r").as_double_array(); - R_dvl_eskf_ = Eigen::Map>( - R_dvl_correction.data()); - - std::vector T_dvl_correction; - this->declare_parameter>("dvl_frame_t"); - T_dvl_correction = get_parameter("dvl_frame_t").as_double_array(); - T_dvl_eskf_ = Eigen::Map(T_dvl_correction.data()); - - std::vector T_depth_correction; - this->declare_parameter>("depth_frame_t"); - T_depth_correction = get_parameter("depth_frame_t").as_double_array(); - T_depth_eskf_ = Eigen::Map(T_depth_correction.data()); + + if (!use_tf_transforms_) { + std::vector R_imu_correction = + this->declare_parameter>("transform.imu_frame_r"); + R_imu_eskf_ = Eigen::Map>( + R_imu_correction.data()); + + std::vector R_dvl_correction = + this->declare_parameter>("transform.dvl_frame_r"); + R_dvl_eskf_ = Eigen::Map>( + R_dvl_correction.data()); + + std::vector T_dvl_correction = + this->declare_parameter>("transform.dvl_frame_t"); + T_dvl_eskf_ = Eigen::Map(T_dvl_correction.data()); + + std::vector T_depth_correction = + this->declare_parameter>("transform.depth_frame_t"); + T_depth_eskf_ = Eigen::Map(T_depth_correction.data()); + } std::vector diag_Q_std; this->declare_parameter>("diag_Q_std"); @@ -243,9 +231,9 @@ void ESKFNode::publish_odom() { // If you also want to include gyro bias, you could add it to the covariance // matrix or publish a separate topic for biases - - odom_msg.header.stamp = this->now(); - odom_msg.header.frame_id = "odom"; + rclcpp::Time current_time = this->now(); + odom_msg.header.stamp = current_time; + odom_msg.header.frame_id = frame("odom"); // Some cross terms of the covariance are ignored, and the acc/gyro biases // cov are not published. Pos and orientation cov needs to be mapped from @@ -274,69 +262,58 @@ void ESKFNode::publish_odom() { } odom_pub_->publish(odom_msg); - publish_tf(nom_state); -} - -void ESKFNode::initialize_static_transforms() { - // if already loaded, no need to lookup again. - if (tf_sensors_loaded_) { - tf_timer_->cancel(); - return; + if (publish_tf_) { + publish_tf(nom_state, current_time); } +} +void ESKFNode::lookup_static_transforms() { try { - // Lookup IMU -> Base Link - geometry_msgs::msg::TransformStamped tf_imu = - tf_buffer_->lookupTransform("base_link", "imu_frame", - tf2::TimePointZero); - - Tf_base_imu_ = tf2::transformToEigen(tf_imu); - - // Overwrite the parameter-based matrix + Tf_base_imu_ = tf2::transformToEigen(tf_buffer_->lookupTransform( + frame("base_link"), frame("imu_link"), tf2::TimePointZero)); R_imu_eskf_ = Tf_base_imu_.rotation(); - spdlog::info("TF: Loaded base_link <- imu_frame transform"); - // Lookup DVL -> Base Link - geometry_msgs::msg::TransformStamped tf_dvl = - tf_buffer_->lookupTransform("base_link", "dvl_frame", - tf2::TimePointZero); - - Tf_base_dvl_ = tf2::transformToEigen(tf_dvl); - - // Overwrite the parameter-based matrix + Tf_base_dvl_ = tf2::transformToEigen(tf_buffer_->lookupTransform( + frame("base_link"), frame("dvl_link"), tf2::TimePointZero)); R_dvl_eskf_ = Tf_base_dvl_.rotation(); - spdlog::info("TF: Loaded base_link <- dvl_frame transform"); - - // Lookup Depth sensor -> Base Link - geometry_msgs::msg::TransformStamped tf_depth = - tf_buffer_->lookupTransform("base_link", "depth_sensor_frame", - tf2::TimePointZero); - - Tf_base_depth_ = tf2::transformToEigen(tf_depth); + T_dvl_eskf_ = Tf_base_dvl_.translation(); - // Overwrite the parameter-based matrix + Tf_base_depth_ = tf2::transformToEigen(tf_buffer_->lookupTransform( + frame("base_link"), frame("pressure_sensor_link"), tf2::TimePointZero)); T_depth_eskf_ = Tf_base_depth_.translation(); - spdlog::info("TF: Loaded base_link <- depth_sensor_frame transform"); - // If we reach this point, all transforms were loaded successfully tf_sensors_loaded_ = true; - - spdlog::info("All static transforms loaded successfully."); - - // Turn off the timer so this function never runs again tf_timer_->cancel(); + spdlog::info("All static transforms loaded successfully."); + complete_initialization(); } catch (const tf2::TransformException& ex) { - // It is common to fail on startup before static_publisher is ready spdlog::warn("TF Lookup failed (will retry): {}", ex.what()); } } -void ESKFNode::publish_tf(const StateQuat& nom_state) { +void ESKFNode::complete_initialization() { + set_subscribers_and_publisher(); + set_parameters(); + + time_step_ = std::chrono::milliseconds( + this->get_parameter("publish_rate_ms").as_int()); + odom_pub_timer_ = this->create_wall_timer( + time_step_, std::bind(&ESKFNode::publish_odom, this)); + + spdlog::info(start_message); + +#ifndef NDEBUG + spdlog::info( + "______________________Debug mode is enabled______________________"); +#endif +} + +void ESKFNode::publish_tf(const StateQuat& nom_state, const rclcpp::Time& time) { geometry_msgs::msg::TransformStamped tf_msg; - tf_msg.header.stamp = this->now(); - tf_msg.header.frame_id = "odom"; - tf_msg.child_frame_id = "base_link"; + tf_msg.header.stamp = time; + tf_msg.header.frame_id = frame("odom"); + tf_msg.child_frame_id = frame("base_link"); tf_msg.transform.translation.x = nom_state.pos.x(); tf_msg.transform.translation.y = nom_state.pos.y(); From 82cb900810d087a66e8eaaaee3ed22b7522d0ed1 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Fri, 27 Mar 2026 21:22:33 +0100 Subject: [PATCH 233/290] update default ip launch config --- auv_setup/launch/state_estimation.launch.py | 118 ++++++++++++++++++++ 1 file changed, 118 insertions(+) create mode 100644 auv_setup/launch/state_estimation.launch.py diff --git a/auv_setup/launch/state_estimation.launch.py b/auv_setup/launch/state_estimation.launch.py new file mode 100644 index 000000000..fba7bf4c5 --- /dev/null +++ b/auv_setup/launch/state_estimation.launch.py @@ -0,0 +1,118 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import OpaqueFunction +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + +from auv_setup.launch_arg_common import ( + declare_drone_and_namespace_args, + resolve_drone_and_namespace, +) + + +def launch_setup(context, *args, **kwargs): + drone, namespace = resolve_drone_and_namespace(context) + + drone_params = os.path.join( + get_package_share_directory("auv_setup"), + "config", + "robots", + f"{drone}.yaml", + ) + + eskf_params = os.path.join( + get_package_share_directory("eskf"), "config", "eskf_params.yaml" + ) + + container = ComposableNodeContainer( + name="eskf_container", + namespace=namespace, + package="rclcpp_components", + executable="component_container_mt", + composable_node_descriptions=[ + ComposableNode( + package="eskf", + plugin="ESKFNode", + name="eskf_node", + namespace=namespace, + parameters=[drone_params, eskf_params], + extra_arguments=[{"use_intra_process_comms": True}], + ), + ComposableNode( + package="mru_ros_interface", + plugin="MruRosInterface", + name="mru_ros_interface_node", + namespace=namespace, + parameters=[ + { + "imu_pub_topic": f"/{namespace}/imu/data_raw", + "frame_id": f"/{namespace}/imu_link", + "connection_params.remote_ip": "10.0.1.20", # MRU IP + "connection_params.data_remote_port": 7550, + "connection_params.data_local_port": 7551, + "connection_params.control_local_port": 7552, + "mru_settings.channel": "UDP1", + "mru_settings.port": 7551, + "mru_settings.ip_addr": "10.0.0.69", # Host computer IP + "mru_settings.format": "MRUBIN", + "mru_settings.interval": 5, + "mru_settings.token": 21, + } + ], + extra_arguments=[{"use_intra_process_comms": True}], + ), + ComposableNode( + package="nortek_nucleus_ros_interface", + plugin="NortekNucleusRosInterface", + name="nortek_nucleus_ros_interface_node", + namespace=namespace, + parameters=[ + { + "frame_id": f"/{namespace}/nucleus_frame", + "connection_params.remote_ip": "192.168.1.100", + "connection_params.data_remote_port": 9000, + "connection_params.password": "", + "enable_imu": True, + "enable_dvl": True, + "enable_pressure": True, + "enable_magnetometer": True, + "enable_ins_twist": True, + "enable_ins_position": True, + "imu_data_raw_pub_topic": f"/{namespace}/imu/data_raw", + "imu_data_pub_topic": f"/{namespace}/imu/data", + "dvl_pub_topic": f"/{namespace}/nucleus/dvl", + "pressure_pub_topic": f"/{namespace}/nucleus/pressure", + "magnetometer_pub_topic": f"/{namespace}/imu/mag", + "ins_twist_pub_topic": f"/{namespace}/nucleus/ins/twist", + "ins_position_pub_topic": f"/{namespace}/nucleus/ins/position", + "imu_settings.freq": 125, + "ahrs_settings.freq": 10, + "ahrs_settings.mode": 0, + "bottom_track_settings.mode": 2, + "bottom_track_settings.velocity_range": 5, + "bottom_track_settings.enable_watertrack": False, + "fast_pressure_settings.enable": True, + "fast_pressure_settings.sampling_rate": 16, + "magnetometer_settings.freq": 75, + "magnetometer_settings.mode": 0, + } + ], + extra_arguments=[{"use_intra_process_comms": True}], + ), + ], + output="screen", + arguments=["--ros-args", "--log-level", "error"], + ) + + return [container] + + +def generate_launch_description(): + return LaunchDescription( + declare_drone_and_namespace_args() + + [ + OpaqueFunction(function=launch_setup), + ] + ) \ No newline at end of file From 9df8a2370f4961a8d70a5fd61438d5967a2fa6db Mon Sep 17 00:00:00 2001 From: Cyprian Osinski Date: Fri, 27 Mar 2026 22:16:07 +0100 Subject: [PATCH 234/290] changed so that drone type is resolved at launch for the adaptive dp backstepping controller, tuned the adaptive dp backstepping controller a bit for nautilus not yet finished. Added roll back into the controllable parameters, it looks good for now. --- auv_setup/launch/dp.launch.py | 2 +- .../config/adapt_params.yaml | 10 ---- .../config/adapt_params_nautilus.yaml | 7 +++ .../config/adapt_params_orca.yaml | 7 +++ .../dp_adapt_backs_controller.launch.py | 2 +- control/dp_adapt_backs_controller/package.xml | 1 + .../src/dp_adapt_backs_controller.cpp | 2 +- .../src/dp_adapt_backs_controller_ros.cpp | 21 ++++--- .../test/CMakeLists.txt | 7 +++ .../test/dp_adapt_backs_controller_tests.cpp | 56 ++++++++++++++----- 10 files changed, 77 insertions(+), 38 deletions(-) delete mode 100644 control/dp_adapt_backs_controller/config/adapt_params.yaml create mode 100644 control/dp_adapt_backs_controller/config/adapt_params_nautilus.yaml create mode 100644 control/dp_adapt_backs_controller/config/adapt_params_orca.yaml diff --git a/auv_setup/launch/dp.launch.py b/auv_setup/launch/dp.launch.py index c0901836c..f178cb1a7 100644 --- a/auv_setup/launch/dp.launch.py +++ b/auv_setup/launch/dp.launch.py @@ -31,7 +31,7 @@ def launch_setup(context, *args, **kwargs): adapt_params = os.path.join( get_package_share_directory("dp_adapt_backs_controller"), "config", - "adapt_params.yaml", + f"adapt_params_{drone}.yaml", ) container = ComposableNodeContainer( diff --git a/control/dp_adapt_backs_controller/config/adapt_params.yaml b/control/dp_adapt_backs_controller/config/adapt_params.yaml deleted file mode 100644 index 32fb79dd7..000000000 --- a/control/dp_adapt_backs_controller/config/adapt_params.yaml +++ /dev/null @@ -1,10 +0,0 @@ -/**: - ros__parameters: - K1 : [10.5, 10.5, 13.5, 0.0, 4.0, 4.0] - K2 : [20.5, 20.5, 20.5, 0.0, 5.5, 5.5] - r_b_bg : [0.01, 0.0, 0.02] - adapt_gain : [0.4, 0.1, 0.4, 0.1, 0.4, 0.1, 0.4, 0.1, 0.4, 0.1, 0.4, 0.1] - d_gain : [0.6, 0.6, 0.6, 0.6, 0.6, 0.6] - inertia_matrix : [0.68, 3.32, 3.34] - mass_matrix : [ 30.0, 0.0, 0.0, 0.0, 0.0, 0.6, 0.0, 30.0, 0.0, 0.0, -0.6, 0.3, 0.0, 0.0, 30.0, 0.6, 0.3, 0.0, 0.0, 0.0, 0.6, 0.68, 0.0, 0.0, 0.0, -0.6, 0.3, 0.0, 3.32, 0.0, 0.6, 0.3, 0.0, 0.0, 0.0, 3.34] - m : 30.0 diff --git a/control/dp_adapt_backs_controller/config/adapt_params_nautilus.yaml b/control/dp_adapt_backs_controller/config/adapt_params_nautilus.yaml new file mode 100644 index 000000000..65b639ed7 --- /dev/null +++ b/control/dp_adapt_backs_controller/config/adapt_params_nautilus.yaml @@ -0,0 +1,7 @@ +/**: + ros__parameters: + K1 : [15.0, 15.0, 15.0, 1.5, 6.0, 6.0] # Outer loop tuning parameters + K2 : [30.0, 30.0, 30.0, 2.0, 8.5, 8.5] # Inner loop tuning parameters + r_b_bg : [0.0, 0.0, 0.01] # Vector from body centre to centre of gravity + adapt_gain : [0.6, 0.15, 0.6, 0.15, 0.6, 0.15, 0.6, 0.15, 0.6, 0.15, 0.6, 0.15] # Tuning parameters for linear and nonlinear damping + d_gain : [0.8, 0.8, 0.8, 0.8, 0.8, 0.8] # Tuning parameters for the unmodeled disturbances and uncertanties diff --git a/control/dp_adapt_backs_controller/config/adapt_params_orca.yaml b/control/dp_adapt_backs_controller/config/adapt_params_orca.yaml new file mode 100644 index 000000000..e128131b2 --- /dev/null +++ b/control/dp_adapt_backs_controller/config/adapt_params_orca.yaml @@ -0,0 +1,7 @@ +/**: + ros__parameters: + K1 : [10.5, 10.5, 13.5, 0.0, 4.0, 4.0] # Outer loop tuning parameters + K2 : [20.5, 20.5, 20.5, 0.0, 5.5, 5.5] # Inner loop tuning parameters + r_b_bg : [0.01, 0.0, 0.02] # Vector from body centre to centre of gravity + adapt_gain : [0.4, 0.1, 0.4, 0.1, 0.4, 0.1, 0.4, 0.1, 0.4, 0.1, 0.4, 0.1] # Tuning parameters for linear and nonlinear damping + d_gain : [0.6, 0.6, 0.6, 0.6, 0.6, 0.6] # Tuning parameters for the unmodeled disturbances and uncertanties diff --git a/control/dp_adapt_backs_controller/launch/dp_adapt_backs_controller.launch.py b/control/dp_adapt_backs_controller/launch/dp_adapt_backs_controller.launch.py index 5dba5f5d6..fe3972bf8 100644 --- a/control/dp_adapt_backs_controller/launch/dp_adapt_backs_controller.launch.py +++ b/control/dp_adapt_backs_controller/launch/dp_adapt_backs_controller.launch.py @@ -17,7 +17,7 @@ def launch_setup(context, *args, **kwargs): adapt_params = os.path.join( get_package_share_directory("dp_adapt_backs_controller"), "config", - "adapt_params.yaml", + f"adapt_params_{drone}.yaml", ) drone_params = os.path.join( diff --git a/control/dp_adapt_backs_controller/package.xml b/control/dp_adapt_backs_controller/package.xml index 0b708c8b7..c618f6e06 100644 --- a/control/dp_adapt_backs_controller/package.xml +++ b/control/dp_adapt_backs_controller/package.xml @@ -17,6 +17,7 @@ vortex_msgs vortex_utils vortex_utils_ros + yaml-cpp ament_cmake 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 78102c0bf..e009d7f90 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 @@ -49,7 +49,7 @@ Eigen::Vector6d DPAdaptBacksController::calculate_tau(const PoseEuler& pose, (pose.as_j_matrix().transpose() * z_1) - (K2_ * z_2) - F_est - d_est_; - tau = tau.cwiseMax(-80.0).cwiseMin(80.0); + tau = tau.cwiseMax(-100.0).cwiseMin(100.0); adapt_param_ += adapt_param_dot * dt_; d_est_ += d_est_dot * dt_; adapt_param_ = adapt_param_.cwiseMax(-10.0).cwiseMin(10.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 d0c40d906..8e6c884ce 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 @@ -176,9 +176,7 @@ void DPAdaptBacksControllerNode::set_adap_params() { this->declare_parameter>("K1"); this->declare_parameter>("K2"); this->declare_parameter>("r_b_bg"); - this->declare_parameter>("inertia_matrix"); - this->declare_parameter>("mass_matrix"); - this->declare_parameter("m"); + this->declare_parameter>("physical.mass_matrix"); std::vector adapt_param_vec = this->get_parameter("adapt_gain").as_double_array(); @@ -188,12 +186,8 @@ void DPAdaptBacksControllerNode::set_adap_params() { std::vector K2_vec = this->get_parameter("K2").as_double_array(); std::vector r_b_bg_vec = this->get_parameter("r_b_bg").as_double_array(); - std::vector I_b_vec = - this->get_parameter("inertia_matrix").as_double_array(); std::vector mass_matrix_vec = - this->get_parameter("mass_matrix").as_double_array(); - - double mass{this->get_parameter("m").as_double()}; + this->get_parameter("physical.mass_matrix").as_double_array(); Eigen::Vector12d adapt_param_eigen = Eigen::Map(adapt_param_vec.data()); @@ -203,10 +197,13 @@ void DPAdaptBacksControllerNode::set_adap_params() { Eigen::Vector6d K2_eigen = Eigen::Map(K2_vec.data()); Eigen::Vector3d r_b_bg_eigen = Eigen::Map(r_b_bg_vec.data()); - Eigen::Vector3d I_b_eigen = Eigen::Map(I_b_vec.data()); Eigen::Matrix6d mass_matrix = Eigen::Map(mass_matrix_vec.data()); + double mass = mass_matrix(0, 0); + Eigen::Vector3d I_b_eigen(mass_matrix(3, 3), mass_matrix(4, 4), + mass_matrix(5, 5)); + DPAdaptParams dp_adapt_params; dp_adapt_params.adapt_param = adapt_param_eigen; dp_adapt_params.d_gain = d_gain_eigen; @@ -237,8 +234,10 @@ void DPAdaptBacksControllerNode::publish_tau() { tau_msg.wrench.force.x = tau(0); tau_msg.wrench.force.y = tau(1); tau_msg.wrench.force.z = tau(2); - // tau_msg.wrench.torque.x = tau(3); commented out since roll control is not - // needed and causes minor instability, if needed uncomment + + // comment out if roll control is not needed + tau_msg.wrench.torque.x = tau(3); + tau_msg.wrench.torque.y = tau(4); tau_msg.wrench.torque.z = tau(5); diff --git a/control/dp_adapt_backs_controller/test/CMakeLists.txt b/control/dp_adapt_backs_controller/test/CMakeLists.txt index 2c7994231..5b97abfbf 100644 --- a/control/dp_adapt_backs_controller/test/CMakeLists.txt +++ b/control/dp_adapt_backs_controller/test/CMakeLists.txt @@ -1,6 +1,7 @@ cmake_minimum_required(VERSION 3.8) find_package(GTest REQUIRED) +find_package(yaml-cpp REQUIRED) include(GoogleTest) set(TEST_BINARY_NAME ${PROJECT_NAME}_test) @@ -9,10 +10,16 @@ add_executable( dp_adapt_backs_controller_tests.cpp ) +target_compile_definitions(${TEST_BINARY_NAME} PRIVATE + DRONE_YAML_PATH="${CMAKE_CURRENT_SOURCE_DIR}/../../../auv_setup/config/robots/nautilus.yaml" + CONTROLLER_YAML_PATH="${CMAKE_CURRENT_SOURCE_DIR}/../config/adapt_params_nautilus.yaml" +) + target_link_libraries( ${TEST_BINARY_NAME} PRIVATE ${LIB_NAME} + yaml-cpp GTest::GTest ) 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 b3adf9d17..01232bdd3 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 @@ -1,4 +1,5 @@ #include +#include #include @@ -11,23 +12,44 @@ namespace vortex::control { using vortex::utils::types::PoseEuler; using vortex::utils::types::Twist; +DPAdaptParams load_dp_adapt_params(const std::string& drone_yaml_path, + const std::string& controller_yaml_path) { + YAML::Node drone_params = + YAML::LoadFile(drone_yaml_path)["/**"]["ros__parameters"]; + YAML::Node controller_params = + YAML::LoadFile(controller_yaml_path)["/**"]["ros__parameters"]; + + auto K1_vec = controller_params["K1"].as>(); + auto K2_vec = controller_params["K2"].as>(); + auto adapt_gain_vec = + controller_params["adapt_gain"].as>(); + auto d_gain_vec = controller_params["d_gain"].as>(); + auto r_b_bg_vec = controller_params["r_b_bg"].as>(); + + auto mass_matrix_vec = + drone_params["physical"]["mass_matrix"].as>(); + + Eigen::Matrix6d mass_matrix = + Eigen::Map(mass_matrix_vec.data()); + + DPAdaptParams params; + params.K1 = Eigen::Map(K1_vec.data()); + params.K2 = Eigen::Map(K2_vec.data()); + params.adapt_param = Eigen::Map(adapt_gain_vec.data()); + params.d_gain = Eigen::Map(d_gain_vec.data()); + params.r_b_bg = Eigen::Map(r_b_bg_vec.data()); + params.mass_matrix = mass_matrix; + params.mass = mass_matrix(0, 0); + params.I_b = Eigen::Vector3d(mass_matrix(3, 3), mass_matrix(4, 4), + mass_matrix(5, 5)); + return params; +} + class DPAdaptBacksControllerTests : public ::testing::Test { protected: DPAdaptBacksControllerTests() - : dp_adapt_backs_controller_{get_dp_params()} {} - - DPAdaptParams get_dp_params() { - DPAdaptParams params; - params.adapt_param = Eigen::Vector12d::Ones() * 0.1; - params.d_gain = Eigen::Vector6d::Ones() * 0.6; - params.K1 = Eigen::Vector6d::Ones() * 5.0; - params.K2 = Eigen::Vector6d::Ones() * 5.0; - params.r_b_bg = Eigen::Vector3d(0.01, 0.0, 0.02); - params.I_b = Eigen::Vector3d(0.68, 3.32, 3.34); - params.mass_matrix = Eigen::Matrix6d::Identity() * 30.0; - params.mass = 30.0; - return params; - } + : dp_adapt_backs_controller_{ + load_dp_adapt_params(DRONE_YAML_PATH, CONTROLLER_YAML_PATH)} {} PoseEuler generate_current_pose(const double north_pos, const double east_pos, @@ -395,6 +417,8 @@ TEST_F(DPAdaptBacksControllerTests, EXPECT_NEAR(tau[3], 0.0, 0.01); EXPECT_NEAR(tau[4], 0.0, 0.01); EXPECT_NEAR(tau[5], 0.0, 0.01); + // Torque channels may have cross-coupling from off-diagonal mass matrix + // terms in the Coriolis matrix } /* @@ -415,6 +439,8 @@ TEST_F(DPAdaptBacksControllerTests, EXPECT_NEAR(tau[3], 0.0, 0.01); EXPECT_NEAR(tau[4], 0.0, 0.01); EXPECT_NEAR(tau[5], 0.0, 0.01); + // Torque channels may have cross-coupling from off-diagonal mass matrix + // terms in the Coriolis matrix } /* @@ -435,6 +461,8 @@ TEST_F(DPAdaptBacksControllerTests, EXPECT_NEAR(tau[3], 0.0, 0.01); EXPECT_NEAR(tau[4], 0.0, 0.01); EXPECT_NEAR(tau[5], 0.0, 0.01); + // Torque channels may have cross-coupling from off-diagonal mass matrix + // terms in the Coriolis matrix } } // namespace vortex::control From 64c24b76c882c72c821fe561e2bc99e2f8e84129 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Sat, 28 Mar 2026 14:54:24 +0100 Subject: [PATCH 235/290] ESKF: pose and twist publishers --- navigation/eskf/config/eskf_params.yaml | 2 ++ navigation/eskf/include/eskf/eskf_ros.hpp | 8 ++++++ navigation/eskf/src/eskf_ros.cpp | 34 +++++++++++++++++++++++ 3 files changed, 44 insertions(+) diff --git a/navigation/eskf/config/eskf_params.yaml b/navigation/eskf/config/eskf_params.yaml index ad17654d1..81905d41a 100644 --- a/navigation/eskf/config/eskf_params.yaml +++ b/navigation/eskf/config/eskf_params.yaml @@ -18,4 +18,6 @@ depth_frame_t: [ 0.0, 0.0, 0.0 ] use_tf_transforms: true publish_tf: true + publish_pose: true + publish_twist: true publish_rate_ms: 1 diff --git a/navigation/eskf/include/eskf/eskf_ros.hpp b/navigation/eskf/include/eskf/eskf_ros.hpp index 5959e500b..6d9c5d007 100644 --- a/navigation/eskf/include/eskf/eskf_ros.hpp +++ b/navigation/eskf/include/eskf/eskf_ros.hpp @@ -66,6 +66,12 @@ class ESKFNode : public rclcpp::Node { rclcpp::Publisher::SharedPtr odom_pub_; + rclcpp::Publisher< + geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_pub_; + + rclcpp::Publisher< + geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr twist_pub_; + rclcpp::Publisher::SharedPtr nis_pub_; // Member variable for the ESKF instance @@ -106,6 +112,8 @@ class ESKFNode : public rclcpp::Node { bool use_tf_transforms_ = false; bool tf_sensors_loaded_ = false; bool publish_tf_{false}; + bool publish_pose_{false}; + bool publish_twist_{false}; // hold the transfer from Sensor -> Base Link Eigen::Isometry3d Tf_base_imu_ = Eigen::Isometry3d::Identity(); diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp index 780e2b7f9..f467f7e3e 100644 --- a/navigation/eskf/src/eskf_ros.cpp +++ b/navigation/eskf/src/eskf_ros.cpp @@ -29,12 +29,17 @@ ESKFNode::ESKFNode(const rclcpp::NodeOptions& options) tf_broadcaster_ = std::make_unique(*this); } + publish_pose_ = this->declare_parameter("publish_pose"); + publish_twist_ = this->declare_parameter("publish_twist"); + // Declare these here so they appear in `ros2 param list` from startup, // even though they are read in complete_initialization(). this->declare_parameter("publish_rate_ms"); this->declare_parameter("topics.imu"); this->declare_parameter("topics.dvl_twist"); this->declare_parameter("topics.odom"); + this->declare_parameter("topics.pose"); + this->declare_parameter("topics.twist"); if (use_tf_transforms_) { tf_buffer_ = std::make_shared(this->get_clock()); @@ -67,6 +72,21 @@ void ESKFNode::set_subscribers_and_publisher() { odom_pub_ = this->create_publisher( odom_topic, qos_sensor_data); + if (publish_pose_) { + std::string pose_topic = this->get_parameter("topics.pose").as_string(); + pose_pub_ = + this->create_publisher( + pose_topic, qos_sensor_data); + } + + if (publish_twist_) { + std::string twist_topic = + this->get_parameter("topics.twist").as_string(); + twist_pub_ = + this->create_publisher( + twist_topic, qos_sensor_data); + } + #ifndef NDEBUG nis_pub_ = create_publisher( "eskf/nis", vortex::utils::qos_profiles::reliable_profile()); @@ -262,6 +282,20 @@ void ESKFNode::publish_odom() { } odom_pub_->publish(odom_msg); + if (publish_pose_) { + geometry_msgs::msg::PoseWithCovarianceStamped pose_msg; + pose_msg.header = odom_msg.header; + pose_msg.pose = odom_msg.pose; + pose_pub_->publish(pose_msg); + } + + if (publish_twist_) { + geometry_msgs::msg::TwistWithCovarianceStamped twist_msg; + twist_msg.header = odom_msg.header; + twist_msg.twist = odom_msg.twist; + twist_pub_->publish(twist_msg); + } + if (publish_tf_) { publish_tf(nom_state, current_time); } From 612ae6e5da206d1562d0ce796ad8a1ca1a55afba Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Sat, 28 Mar 2026 23:09:26 +0100 Subject: [PATCH 236/290] simple odom publisher --- navigation/odom_transformer/CMakeLists.txt | 67 +++++++ .../config/odom_transformer_params.yaml | 12 ++ .../odom_transformer/odom_transformer.hpp | 56 ++++++ .../launch/odom_transformer.launch.py | 51 +++++ navigation/odom_transformer/package.xml | 26 +++ .../odom_transformer/src/odom_transformer.cpp | 177 ++++++++++++++++++ 6 files changed, 389 insertions(+) create mode 100644 navigation/odom_transformer/CMakeLists.txt create mode 100644 navigation/odom_transformer/config/odom_transformer_params.yaml create mode 100644 navigation/odom_transformer/include/odom_transformer/odom_transformer.hpp create mode 100644 navigation/odom_transformer/launch/odom_transformer.launch.py create mode 100644 navigation/odom_transformer/package.xml create mode 100644 navigation/odom_transformer/src/odom_transformer.cpp diff --git a/navigation/odom_transformer/CMakeLists.txt b/navigation/odom_transformer/CMakeLists.txt new file mode 100644 index 000000000..6a27c4b62 --- /dev/null +++ b/navigation/odom_transformer/CMakeLists.txt @@ -0,0 +1,67 @@ +cmake_minimum_required(VERSION 3.8) +project(odom_transformer) + +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_components REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_eigen REQUIRED) + +include_directories(include ${EIGEN3_INCLUDE_DIRS}) + +set(LIB_NAME "${PROJECT_NAME}_component") + +add_library(${LIB_NAME} SHARED + src/odom_transformer.cpp +) + +ament_target_dependencies(${LIB_NAME} PUBLIC + rclcpp + rclcpp_components + nav_msgs + geometry_msgs + Eigen3 + tf2 + tf2_ros + tf2_eigen +) + +rclcpp_components_register_node( + ${LIB_NAME} + PLUGIN "OdomTransformer" + 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 + config + DESTINATION share/${PROJECT_NAME}/ +) + +ament_package() diff --git a/navigation/odom_transformer/config/odom_transformer_params.yaml b/navigation/odom_transformer/config/odom_transformer_params.yaml new file mode 100644 index 000000000..d99f65ab1 --- /dev/null +++ b/navigation/odom_transformer/config/odom_transformer_params.yaml @@ -0,0 +1,12 @@ +/**: + odom_transformer_node: + ros__parameters: + sensor_frame: "dvl_link" + publish_tf: true + publish_pose: true + publish_twist: true + topics: + input: "nucleus/odom" + output: "odom" + pose: "pose" + twist: "twist" diff --git a/navigation/odom_transformer/include/odom_transformer/odom_transformer.hpp b/navigation/odom_transformer/include/odom_transformer/odom_transformer.hpp new file mode 100644 index 000000000..194846b35 --- /dev/null +++ b/navigation/odom_transformer/include/odom_transformer/odom_transformer.hpp @@ -0,0 +1,56 @@ +#ifndef ODOM_TRANSFORMER__ODOM_TRANSFORMER_HPP_ +#define ODOM_TRANSFORMER__ODOM_TRANSFORMER_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class OdomTransformer : public rclcpp::Node { + public: + explicit OdomTransformer( + const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); + + private: + void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg); + void lookup_static_transforms(); + void complete_initialization(); + + std::string frame(const std::string& name) const { + return frame_prefix_.empty() ? name : frame_prefix_ + "/" + name; + } + + // TF2 + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + std::unique_ptr tf_broadcaster_; + rclcpp::TimerBase::SharedPtr tf_timer_; + + // Pub / Sub + rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Publisher::SharedPtr odom_pub_; + rclcpp::Publisher::SharedPtr + pose_pub_; + rclcpp::Publisher< + geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr twist_pub_; + + // Transform from base_link to sensor_link + Eigen::Matrix3d R_base_sensor_ = Eigen::Matrix3d::Identity(); + Eigen::Vector3d t_base_sensor_ = Eigen::Vector3d::Zero(); + + std::string frame_prefix_; + std::string sensor_frame_; + bool tf_loaded_{false}; + bool publish_tf_{false}; + bool publish_pose_{false}; + bool publish_twist_{false}; +}; + +#endif // ODOM_TRANSFORMER__ODOM_TRANSFORMER_HPP_ diff --git a/navigation/odom_transformer/launch/odom_transformer.launch.py b/navigation/odom_transformer/launch/odom_transformer.launch.py new file mode 100644 index 000000000..06afbe536 --- /dev/null +++ b/navigation/odom_transformer/launch/odom_transformer.launch.py @@ -0,0 +1,51 @@ +import os +from os import path + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import OpaqueFunction +from launch_ros.actions import Node + +from auv_setup.launch_arg_common import ( + declare_drone_and_namespace_args, + resolve_drone_and_namespace, +) + +odom_transformer_params = path.join( + get_package_share_directory("odom_transformer"), + "config", + "odom_transformer_params.yaml", +) + + +def launch_setup(context, *args, **kwargs): + drone, namespace = resolve_drone_and_namespace(context) + + drone_params = os.path.join( + get_package_share_directory("auv_setup"), + "config", + "robots", + f"{drone}.yaml", + ) + + node = Node( + package="odom_transformer", + executable="odom_transformer_node", + name="odom_transformer_node", + namespace=namespace, + parameters=[ + odom_transformer_params, + drone_params, + {"frame_prefix": namespace}, + ], + output="screen", + ) + + return [node] + + +def generate_launch_description(): + return LaunchDescription( + declare_drone_and_namespace_args() + + [OpaqueFunction(function=launch_setup)] + ) diff --git a/navigation/odom_transformer/package.xml b/navigation/odom_transformer/package.xml new file mode 100644 index 000000000..9658764c8 --- /dev/null +++ b/navigation/odom_transformer/package.xml @@ -0,0 +1,26 @@ + + + + odom_transformer + 0.1.0 + Transforms odometry from a sensor frame to base_link using tf2 static transforms + jorgen fjermedal + MIT + + ament_cmake + + rclcpp + rclcpp_components + nav_msgs + geometry_msgs + eigen + tf2 + tf2_ros + tf2_eigen + + auv_setup + + + ament_cmake + + diff --git a/navigation/odom_transformer/src/odom_transformer.cpp b/navigation/odom_transformer/src/odom_transformer.cpp new file mode 100644 index 000000000..82a73a6f4 --- /dev/null +++ b/navigation/odom_transformer/src/odom_transformer.cpp @@ -0,0 +1,177 @@ +#include "odom_transformer/odom_transformer.hpp" +#include + +OdomTransformer::OdomTransformer(const rclcpp::NodeOptions& options) + : Node("odom_transformer_node", options) { + frame_prefix_ = this->declare_parameter("frame_prefix"); + if (!frame_prefix_.empty() && frame_prefix_.back() == '/') { + frame_prefix_.pop_back(); + } + + sensor_frame_ = this->declare_parameter("sensor_frame"); + publish_tf_ = this->declare_parameter("publish_tf"); + publish_pose_ = this->declare_parameter("publish_pose"); + publish_twist_ = this->declare_parameter("publish_twist"); + + if (publish_tf_) { + tf_broadcaster_ = + std::make_unique(*this); + } + + this->declare_parameter("topics.input"); + this->declare_parameter("topics.output"); + this->declare_parameter("topics.pose"); + this->declare_parameter("topics.twist"); + + tf_buffer_ = std::make_shared(this->get_clock()); + tf_listener_ = + std::make_shared(*tf_buffer_); + tf_timer_ = this->create_wall_timer( + std::chrono::milliseconds(500), + std::bind(&OdomTransformer::lookup_static_transforms, this)); +} + +void OdomTransformer::lookup_static_transforms() { + try { + auto tf = tf2::transformToEigen(tf_buffer_->lookupTransform( + frame("base_link"), frame(sensor_frame_), tf2::TimePointZero)); + R_base_sensor_ = tf.rotation(); + t_base_sensor_ = tf.translation(); + + tf_loaded_ = true; + tf_timer_->cancel(); + RCLCPP_INFO(get_logger(), + "Loaded static transform: %s -> %s t=(%.3f, %.3f, %.3f)", + frame("base_link").c_str(), frame(sensor_frame_).c_str(), + t_base_sensor_.x(), t_base_sensor_.y(), + t_base_sensor_.z()); + complete_initialization(); + } catch (const tf2::TransformException& ex) { + RCLCPP_WARN(get_logger(), "TF lookup failed (will retry): %s", + ex.what()); + } +} + +void OdomTransformer::complete_initialization() { + auto qos = rclcpp::QoS(1).best_effort(); + + auto input_topic = this->get_parameter("topics.input").as_string(); + auto output_topic = this->get_parameter("topics.output").as_string(); + + odom_sub_ = this->create_subscription( + input_topic, qos, + std::bind(&OdomTransformer::odom_callback, this, + std::placeholders::_1)); + + odom_pub_ = + this->create_publisher(output_topic, qos); + + if (publish_pose_) { + pose_pub_ = + this->create_publisher< + geometry_msgs::msg::PoseWithCovarianceStamped>( + this->get_parameter("topics.pose").as_string(), qos); + } + + if (publish_twist_) { + twist_pub_ = + this->create_publisher< + geometry_msgs::msg::TwistWithCovarianceStamped>( + this->get_parameter("topics.twist").as_string(), qos); + } + + RCLCPP_INFO(get_logger(), "Odom transformer: %s -> %s", + input_topic.c_str(), output_topic.c_str()); +} + +void OdomTransformer::odom_callback( + const nav_msgs::msg::Odometry::SharedPtr msg) { + // Current orientation of the sensor in odom frame + Eigen::Quaterniond q_odom_sensor( + msg->pose.pose.orientation.w, msg->pose.pose.orientation.x, + msg->pose.pose.orientation.y, msg->pose.pose.orientation.z); + Eigen::Matrix3d R_odom_sensor = q_odom_sensor.toRotationMatrix(); + + // Orientation: R_odom_base = R_odom_sensor * R_base_sensor^-1 + Eigen::Matrix3d R_odom_base = + R_odom_sensor * R_base_sensor_.transpose(); + Eigen::Quaterniond q_odom_base(R_odom_base); + q_odom_base.normalize(); + + // Position: p_base = p_sensor - R_odom_base * t_base_sensor + Eigen::Vector3d p_sensor(msg->pose.pose.position.x, + msg->pose.pose.position.y, + msg->pose.pose.position.z); + Eigen::Vector3d p_base = p_sensor - R_odom_base * t_base_sensor_; + + // Angular velocity: rotate from sensor frame to base_link frame + Eigen::Vector3d omega_sensor(msg->twist.twist.angular.x, + msg->twist.twist.angular.y, + msg->twist.twist.angular.z); + Eigen::Vector3d omega_base = R_base_sensor_ * omega_sensor; + + // Linear velocity: lever arm correction + // v_base = R_base_sensor * v_sensor - omega_base x t_base_sensor + Eigen::Vector3d v_sensor(msg->twist.twist.linear.x, + msg->twist.twist.linear.y, + msg->twist.twist.linear.z); + Eigen::Vector3d v_base = + R_base_sensor_ * v_sensor - omega_base.cross(t_base_sensor_); + + // Build output odometry + auto out = std::make_unique(); + out->header.stamp = msg->header.stamp; + out->header.frame_id = frame("odom"); + out->child_frame_id = frame("base_link"); + + out->pose.pose.position.x = p_base.x(); + out->pose.pose.position.y = p_base.y(); + out->pose.pose.position.z = p_base.z(); + out->pose.pose.orientation.w = q_odom_base.w(); + out->pose.pose.orientation.x = q_odom_base.x(); + out->pose.pose.orientation.y = q_odom_base.y(); + out->pose.pose.orientation.z = q_odom_base.z(); + + out->twist.twist.linear.x = v_base.x(); + out->twist.twist.linear.y = v_base.y(); + out->twist.twist.linear.z = v_base.z(); + out->twist.twist.angular.x = omega_base.x(); + out->twist.twist.angular.y = omega_base.y(); + out->twist.twist.angular.z = omega_base.z(); + + out->pose.covariance = msg->pose.covariance; + out->twist.covariance = msg->twist.covariance; + + if (pose_pub_) { + auto pose_msg = + std::make_unique(); + pose_msg->header = out->header; + pose_msg->pose = out->pose; + pose_pub_->publish(std::move(pose_msg)); + } + + if (twist_pub_) { + auto twist_msg = + std::make_unique(); + twist_msg->header = out->header; + twist_msg->header.frame_id = out->child_frame_id; + twist_msg->twist = out->twist; + twist_pub_->publish(std::move(twist_msg)); + } + + if (tf_broadcaster_) { + geometry_msgs::msg::TransformStamped tf_msg; + tf_msg.header.stamp = msg->header.stamp; + tf_msg.header.frame_id = frame("odom"); + tf_msg.child_frame_id = frame("base_link"); + tf_msg.transform.translation.x = p_base.x(); + tf_msg.transform.translation.y = p_base.y(); + tf_msg.transform.translation.z = p_base.z(); + tf_msg.transform.rotation = out->pose.pose.orientation; + tf_broadcaster_->sendTransform(tf_msg); + } + + odom_pub_->publish(std::move(out)); +} + +RCLCPP_COMPONENTS_REGISTER_NODE(OdomTransformer) From 1401dfbcfbe6e681f377d892a5251198d6d08882 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Sun, 29 Mar 2026 16:54:50 +0200 Subject: [PATCH 237/290] update config with thruster mapping --- auv_setup/config/robots/nautilus.yaml | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/auv_setup/config/robots/nautilus.yaml b/auv_setup/config/robots/nautilus.yaml index 60ecae05e..8624f1328 100644 --- a/auv_setup/config/robots/nautilus.yaml +++ b/auv_setup/config/robots/nautilus.yaml @@ -20,6 +20,7 @@ mass_matrix: [53.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 53.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 53.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 11.0628, 1.086, -3.17502, 0.0, 0.0, 0.0, 1.086, 23.1128, 0.1025, 0.0, 0.0, 0.0, -3.17502, 0.1025, 26.23998] # 6x6 mass_inertia_matrix propulsion: + solver_type: "qp" dofs: num: 6 dimensions: @@ -90,6 +91,10 @@ thrust_update_rate: 100.0 # [Hz] watchdog_timeout: 1.0 # [s] + thruster_to_pin_mapping: [4, 5, 6, 7, 0, 1, 2, 3] # I.e. if thruster_to_pin = [ 7, 6, 5, 4, 3, 2, 1, 0 ] then thruster 0 is pin 1 etc.. + thruster_direction: [1, -1, -1, 1, 1, -1, -1, 1] # Disclose during thruster mapping (+/- 1) + thruster_PWM_min: [1050, 1050, 1050, 1050, 1050, 1050, 1050, 1050] # Minimum PWM value, Recommended: [1100, 1100, 1100, 1100, 1100, 1100, 1100, 1100] + thruster_PWM_max: [1950, 1950, 1950, 1950, 1950, 1950, 1950, 1950] # Maximum PWM value, Recommended: [1900, 1900, 1900, 1900, 1900, 1900, 1900, 1900] topics: wrench_input: "wrench_input" From d3c6d6b0ae57702ddb66095dd319edd543db28e5 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Sun, 29 Mar 2026 17:00:27 +0200 Subject: [PATCH 238/290] fix utility asio include bug --- .../thruster_interface_auv/thruster_interface_auv_driver.hpp | 1 + 1 file changed, 1 insertion(+) 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 b8ce765a6..ad0ea2c45 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,6 +1,7 @@ #ifndef THRUSTER_INTERFACE_AUV__THRUSTER_INTERFACE_AUV_DRIVER_HPP_ #define THRUSTER_INTERFACE_AUV__THRUSTER_INTERFACE_AUV_DRIVER_HPP_ +#include #include #include From 2432f8d2aec9b0028d42de307f1e103211d3baf8 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Sun, 29 Mar 2026 17:05:53 +0200 Subject: [PATCH 239/290] add solver type to thrusters.launch --- auv_setup/launch/thruster.launch.py | 20 +++++++++++++++++--- 1 file changed, 17 insertions(+), 3 deletions(-) diff --git a/auv_setup/launch/thruster.launch.py b/auv_setup/launch/thruster.launch.py index 3d49274c8..20c87869f 100755 --- a/auv_setup/launch/thruster.launch.py +++ b/auv_setup/launch/thruster.launch.py @@ -2,10 +2,12 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import OpaqueFunction, SetEnvironmentVariable +from launch.actions import DeclareLaunchArgument, OpaqueFunction, SetEnvironmentVariable from launch_ros.actions import ComposableNodeContainer +from launch.substitutions import LaunchConfiguration from launch_ros.descriptions import ComposableNode + from auv_setup.launch_arg_common import ( declare_drone_and_namespace_args, resolve_drone_and_namespace, @@ -14,6 +16,8 @@ def launch_setup(context, *args, **kwargs): drone, namespace = resolve_drone_and_namespace(context) + solver_type = LaunchConfiguration("solver_type").perform(context) + drone_params = os.path.join( get_package_share_directory("auv_setup"), @@ -39,7 +43,10 @@ def launch_setup(context, *args, **kwargs): plugin="ThrustAllocator", name="thrust_allocator_auv_node", namespace=namespace, - parameters=[drone_params], + parameters=[ + drone_params, + {"propulsion.solver_type": solver_type}, + ], extra_arguments=[{"use_intra_process_comms": True}], ), ComposableNode( @@ -78,5 +85,12 @@ def generate_launch_description() -> LaunchDescription: return LaunchDescription( [set_env_var] + declare_drone_and_namespace_args() - + [OpaqueFunction(function=launch_setup)] + + [ + DeclareLaunchArgument( + "solver_type", + default_value="qp", + description="Thrust allocator solver type (available: pseudoinverse, qp)", + ), + OpaqueFunction(function=launch_setup) + ] ) From a380ab45223cded994b63cd0fd29ff7954303610 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Sun, 29 Mar 2026 17:32:09 +0200 Subject: [PATCH 240/290] update state estimation launch with nortek updates --- auv_setup/launch/state_estimation.launch.py | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/auv_setup/launch/state_estimation.launch.py b/auv_setup/launch/state_estimation.launch.py index fba7bf4c5..376451d89 100644 --- a/auv_setup/launch/state_estimation.launch.py +++ b/auv_setup/launch/state_estimation.launch.py @@ -75,18 +75,22 @@ def launch_setup(context, *args, **kwargs): "connection_params.data_remote_port": 9000, "connection_params.password": "", "enable_imu": True, + "enable_ins_odom": True, "enable_dvl": True, "enable_pressure": True, "enable_magnetometer": True, "enable_ins_twist": True, "enable_ins_position": True, + "enable_ins_pose": True, "imu_data_raw_pub_topic": f"/{namespace}/imu/data_raw", "imu_data_pub_topic": f"/{namespace}/imu/data", + "ins_pub_topic": f"/{namespace}/nucleus/odom", "dvl_pub_topic": f"/{namespace}/nucleus/dvl", "pressure_pub_topic": f"/{namespace}/nucleus/pressure", "magnetometer_pub_topic": f"/{namespace}/imu/mag", "ins_twist_pub_topic": f"/{namespace}/nucleus/ins/twist", "ins_position_pub_topic": f"/{namespace}/nucleus/ins/position", + "ins_pose_pub_topic": f"/{namespace}/nucleus/ins/pose", "imu_settings.freq": 125, "ahrs_settings.freq": 10, "ahrs_settings.mode": 0, @@ -97,6 +101,9 @@ def launch_setup(context, *args, **kwargs): "fast_pressure_settings.sampling_rate": 16, "magnetometer_settings.freq": 75, "magnetometer_settings.mode": 0, + "instrument_settings.rotxy": 180.0, + "instrument_settings.rotyz": 0.0, + "instrument_settings.rotxz": 0.0, } ], extra_arguments=[{"use_intra_process_comms": True}], From fb39d7e61065c6012082a2595501664b4967e508 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Sun, 29 Mar 2026 17:46:42 +0200 Subject: [PATCH 241/290] nortek odom transformer launch file --- .../launch/nucleus_odom_transformer.launch.py | 100 ++++++++++++++++++ 1 file changed, 100 insertions(+) create mode 100644 navigation/odom_transformer/launch/nucleus_odom_transformer.launch.py diff --git a/navigation/odom_transformer/launch/nucleus_odom_transformer.launch.py b/navigation/odom_transformer/launch/nucleus_odom_transformer.launch.py new file mode 100644 index 000000000..2ad0d391d --- /dev/null +++ b/navigation/odom_transformer/launch/nucleus_odom_transformer.launch.py @@ -0,0 +1,100 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import OpaqueFunction +from launch_ros.actions import Node + +from auv_setup.launch_arg_common import ( + declare_drone_and_namespace_args, + resolve_drone_and_namespace, +) + + +def launch_setup(context, *args, **kwargs): + drone, namespace = resolve_drone_and_namespace(context) + + drone_params = os.path.join( + get_package_share_directory("auv_setup"), + "config", + "robots", + f"{drone}.yaml", + ) + + nortek_nucleus_ros_interface_node = Node( + package="nortek_nucleus_ros_interface", + executable="nortek_nucleus_ros_interface_node", + name="nortek_nucleus_ros_interface_node", + namespace=namespace, + parameters=[ + { + "frame_id": f"{namespace}/nucleus_frame", + "connection_params.remote_ip": "10.0.0.42", + "connection_params.data_remote_port": 9000, + "connection_params.password": "", + "enable_imu": False, + "enable_ins_odom": True, + "enable_dvl": False, + "enable_pressure": False, + "enable_magnetometer": False, + "enable_ins_twist": False, + "enable_ins_position": False, + "enable_ins_pose": False, + "imu_data_raw_pub_topic": f"/{namespace}/imu/data_raw", + "imu_data_pub_topic": f"/{namespace}/imu/data", + "ins_pub_topic": f"/{namespace}/nucleus/odom", + "dvl_pub_topic": f"/{namespace}/nucleus/dvl", + "pressure_pub_topic": f"/{namespace}/nucleus/pressure", + "magnetometer_pub_topic": f"/{namespace}/imu/mag", + "ins_twist_pub_topic": f"/{namespace}/nucleus/ins/twist", + "ins_position_pub_topic": f"/{namespace}/nucleus/ins/position", + "ins_pose_pub_topic": f"/{namespace}/nucleus/ins/pose", + "imu_settings.freq": 125, + "ahrs_settings.freq": 10, + "ahrs_settings.mode": 0, + "bottom_track_settings.mode": 2, + "bottom_track_settings.velocity_range": 5, + "bottom_track_settings.enable_watertrack": False, + "fast_pressure_settings.enable": True, + "fast_pressure_settings.sampling_rate": 16, + "magnetometer_settings.freq": 75, + "magnetometer_settings.mode": 0, + "instrument_settings.rotxy": 180.0, + "instrument_settings.rotyz": 0.0, + "instrument_settings.rotxz": 0.0, + }, + drone_params, + ], + output="screen", + ) + + odom_transformer_node = Node( + package="odom_transformer", + executable="odom_transformer_node", + name="odom_transformer_node", + namespace=namespace, + parameters=[ + { + "sensor_frame": "dvl_link", + "publish_tf": True, + "publish_pose": True, + "publish_twist": True, + "topics.input": f"/{namespace}/nucleus/odom", + "topics.output": f"/{namespace}/odom", + "topics.pose": f"/{namespace}/pose", + "topics.twist": f"/{namespace}/twist", + }, + drone_params, + {"frame_prefix": namespace}, + ], + output="screen", + ) + + return [nortek_nucleus_ros_interface_node, odom_transformer_node] + + +def generate_launch_description(): + return LaunchDescription( + declare_drone_and_namespace_args() + + [OpaqueFunction(function=launch_setup)] + ) From 05f725f92af2f27d2b464ddd2e08257446d8d7dc Mon Sep 17 00:00:00 2001 From: Cyprian Osinski Date: Sun, 29 Mar 2026 23:58:03 +0200 Subject: [PATCH 242/290] updated thruster layout such that it fits the more spread out thruster layout --- auv_setup/config/robots/nautilus.yaml | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/auv_setup/config/robots/nautilus.yaml b/auv_setup/config/robots/nautilus.yaml index 61fa6b061..fb9df0fc1 100644 --- a/auv_setup/config/robots/nautilus.yaml +++ b/auv_setup/config/robots/nautilus.yaml @@ -16,7 +16,7 @@ /**: ros__parameters: physical: - center_of_mass: [0.0, 0.0, 0.01] # CO is aligned with CM Position (x,y) in meters (M), small cg offset in z to keep drone naturally stable in roll/pitch + center_of_mass: [0.0, 0.0, 0.025] # CO is aligned with CM Position (x,y) in meters (M), small cg offset in z to keep drone naturally stable in roll/pitch mass_matrix: [53.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 53.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 53.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 11.0628, 1.086, -3.17502, 0.0, 0.0, 0.0, 1.086, 23.1128, 0.1025, 0.0, 0.0, 0.0, -3.17502, 0.1025, 26.23998] # 6x6 mass_inertia_matrix propulsion: @@ -53,22 +53,22 @@ 0.00000, # Heave ] thruster_position: [ # Position (x0,x1 ... x7,y1,y2, ...,y7,z1,z2, ... ,z7) in meters (M). i.e thruster0 has position (x0,y0,z0) - 0.413892, - 0.140095, - -0.163904, - -0.413892, - -0.413892, - -0.163904, - 0.140095, - 0.413892, # x-positions of the thrusters + 0.45015, + 0.24060, + -0.22970, + -0.43861, + -0.43861, + -0.22970, + 0.240600, + 0.450150, # x-positions of the thrusters + 0.305680, 0.313022, 0.313022, - 0.313022, - 0.313022, - -0.313022, + 0.305680, + -0.305680, -0.313022, -0.313022, - -0.313022, # y-positions of the thrusters + -0.305680, # y-positions of the thrusters 0.021736, 0.021736, 0.021736, From 5df6781198ec66c414c1cc73216d64dc60e7a6db Mon Sep 17 00:00:00 2001 From: vortexuser-Pi Date: Mon, 30 Mar 2026 15:06:23 +0200 Subject: [PATCH 243/290] updated thruster mapping, 6-7 swapped --- auv_setup/config/robots/nautilus.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/auv_setup/config/robots/nautilus.yaml b/auv_setup/config/robots/nautilus.yaml index 8624f1328..29ab42159 100644 --- a/auv_setup/config/robots/nautilus.yaml +++ b/auv_setup/config/robots/nautilus.yaml @@ -92,7 +92,7 @@ thrust_update_rate: 100.0 # [Hz] watchdog_timeout: 1.0 # [s] thruster_to_pin_mapping: [4, 5, 6, 7, 0, 1, 2, 3] # I.e. if thruster_to_pin = [ 7, 6, 5, 4, 3, 2, 1, 0 ] then thruster 0 is pin 1 etc.. - thruster_direction: [1, -1, -1, 1, 1, -1, -1, 1] # Disclose during thruster mapping (+/- 1) + thruster_direction: [-1, -1, 1, 1, -1, -1, 1, 1] # Disclose during thruster mapping (+/- 1) thruster_PWM_min: [1050, 1050, 1050, 1050, 1050, 1050, 1050, 1050] # Minimum PWM value, Recommended: [1100, 1100, 1100, 1100, 1100, 1100, 1100, 1100] thruster_PWM_max: [1950, 1950, 1950, 1950, 1950, 1950, 1950, 1950] # Maximum PWM value, Recommended: [1900, 1900, 1900, 1900, 1900, 1900, 1900, 1900] From 7d8596622ddc572139caebdbe935c859a2c5bee3 Mon Sep 17 00:00:00 2001 From: vortexuser-Pi Date: Mon, 30 Mar 2026 16:51:30 +0200 Subject: [PATCH 244/290] fix state_est launch, update eskf pub rate to 5ms --- auv_setup/launch/state_estimation.launch.py | 16 ++++++++-------- navigation/eskf/config/eskf_params.yaml | 2 +- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/auv_setup/launch/state_estimation.launch.py b/auv_setup/launch/state_estimation.launch.py index 376451d89..88a840fa6 100644 --- a/auv_setup/launch/state_estimation.launch.py +++ b/auv_setup/launch/state_estimation.launch.py @@ -37,7 +37,7 @@ def launch_setup(context, *args, **kwargs): plugin="ESKFNode", name="eskf_node", namespace=namespace, - parameters=[drone_params, eskf_params], + parameters=[drone_params, eskf_params, {"frame_prefix": namespace}], extra_arguments=[{"use_intra_process_comms": True}], ), ComposableNode( @@ -74,15 +74,15 @@ def launch_setup(context, *args, **kwargs): "connection_params.remote_ip": "192.168.1.100", "connection_params.data_remote_port": 9000, "connection_params.password": "", - "enable_imu": True, - "enable_ins_odom": True, + "enable_imu": False, + "enable_ins_odom": False, "enable_dvl": True, "enable_pressure": True, - "enable_magnetometer": True, - "enable_ins_twist": True, - "enable_ins_position": True, - "enable_ins_pose": True, - "imu_data_raw_pub_topic": f"/{namespace}/imu/data_raw", + "enable_magnetometer": False, + "enable_ins_twist": False, + "enable_ins_position": False, + "enable_ins_pose": False, + "imu_data_raw_pub_topic": f"/{namespace}/nucleus/imu/data_raw", "imu_data_pub_topic": f"/{namespace}/imu/data", "ins_pub_topic": f"/{namespace}/nucleus/odom", "dvl_pub_topic": f"/{namespace}/nucleus/dvl", diff --git a/navigation/eskf/config/eskf_params.yaml b/navigation/eskf/config/eskf_params.yaml index 81905d41a..6d1e91615 100644 --- a/navigation/eskf/config/eskf_params.yaml +++ b/navigation/eskf/config/eskf_params.yaml @@ -20,4 +20,4 @@ publish_tf: true publish_pose: true publish_twist: true - publish_rate_ms: 1 + publish_rate_ms: 5 \ No newline at end of file From c2ac163a6a4f0835c37ed5b7b317edcf1d590531 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Mon, 30 Mar 2026 18:34:26 +0200 Subject: [PATCH 245/290] ESKF: added 'add_gravity_to_imu' arg --- auv_setup/launch/state_estimation.launch.py | 56 +++++++++++++++++++-- navigation/eskf/config/eskf_params.yaml | 3 +- navigation/eskf/include/eskf/eskf.hpp | 2 + navigation/eskf/include/eskf/eskf_ros.hpp | 1 + navigation/eskf/src/eskf_ros.cpp | 14 +++++- 5 files changed, 69 insertions(+), 7 deletions(-) diff --git a/auv_setup/launch/state_estimation.launch.py b/auv_setup/launch/state_estimation.launch.py index 88a840fa6..dac567dcf 100644 --- a/auv_setup/launch/state_estimation.launch.py +++ b/auv_setup/launch/state_estimation.launch.py @@ -22,10 +22,6 @@ def launch_setup(context, *args, **kwargs): f"{drone}.yaml", ) - eskf_params = os.path.join( - get_package_share_directory("eskf"), "config", "eskf_params.yaml" - ) - container = ComposableNodeContainer( name="eskf_container", namespace=namespace, @@ -37,7 +33,57 @@ def launch_setup(context, *args, **kwargs): plugin="ESKFNode", name="eskf_node", namespace=namespace, - parameters=[drone_params, eskf_params, {"frame_prefix": namespace}], + parameters=[ + drone_params, + { + "frame_prefix": namespace, + + "diag_Q_std": [ + 0.05, 0.05, 0.1, + 0.01, 0.01, 0.02, + 0.001, 0.001, 0.001, + 0.0001, 0.0001, 0.0001 + ], + + "diag_p_init": [ + 1.0, 1.0, 1.0, + 0.5, 0.5, 0.5, + 0.1, 0.1, 0.1, + 0.001, 0.001, 0.001, + 0.001, 0.001, 0.001 + ], + + "transform.imu_frame_r": [ + -1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, -1.0 + ], + "transform.imu_frame_t": [ + 0.0, 0.0, 0.0 + ], + + "transform.dvl_frame_r": [ + 0.0, -1.0, 0.0, + 1.0, 0.0, 0.0, + 0.0, 0.0, 1.0 + ], + "transform.dvl_frame_t": [ + 0.4, 0.0, 0.2 + ], + + "transform.depth_frame_t": [ + 0.0, 0.0, 0.0 + ], + + "use_tf_transforms": True, + "publish_tf": True, + "publish_pose": True, + "publish_twist": True, + "publish_rate_ms": 5, + "add_gravity_to_imu": True, + "frame_prefix": namespace, + }, + ], extra_arguments=[{"use_intra_process_comms": True}], ), ComposableNode( diff --git a/navigation/eskf/config/eskf_params.yaml b/navigation/eskf/config/eskf_params.yaml index 6d1e91615..d3a9c9647 100644 --- a/navigation/eskf/config/eskf_params.yaml +++ b/navigation/eskf/config/eskf_params.yaml @@ -20,4 +20,5 @@ publish_tf: true publish_pose: true publish_twist: true - publish_rate_ms: 5 \ No newline at end of file + publish_rate_ms: 5 + add_gravity_to_imu: false \ No newline at end of file diff --git a/navigation/eskf/include/eskf/eskf.hpp b/navigation/eskf/include/eskf/eskf.hpp index cd96ff190..6b578ff7e 100644 --- a/navigation/eskf/include/eskf/eskf.hpp +++ b/navigation/eskf/include/eskf/eskf.hpp @@ -25,6 +25,8 @@ class ESKF { inline double get_nis() const { return nis_; } + inline Eigen::Vector3d get_gravity() const { return g_; } + private: // @brief Predict the nominal state // @param imu_meas: IMU measurement diff --git a/navigation/eskf/include/eskf/eskf_ros.hpp b/navigation/eskf/include/eskf/eskf_ros.hpp index 6d9c5d007..db73c429f 100644 --- a/navigation/eskf/include/eskf/eskf_ros.hpp +++ b/navigation/eskf/include/eskf/eskf_ros.hpp @@ -114,6 +114,7 @@ class ESKFNode : public rclcpp::Node { bool publish_tf_{false}; bool publish_pose_{false}; bool publish_twist_{false}; + bool add_gravity_to_imu_{false}; // hold the transfer from Sensor -> Base Link Eigen::Isometry3d Tf_base_imu_ = Eigen::Isometry3d::Identity(); diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp index f467f7e3e..a7a940dcf 100644 --- a/navigation/eskf/src/eskf_ros.cpp +++ b/navigation/eskf/src/eskf_ros.cpp @@ -23,6 +23,7 @@ ESKFNode::ESKFNode(const rclcpp::NodeOptions& options) if (!frame_prefix_.empty() && frame_prefix_.back() == '/') { frame_prefix_.pop_back(); } + spdlog::info("frame_prefix set to '{}'", frame_prefix_); publish_tf_ = this->declare_parameter("publish_tf"); if (publish_tf_) { @@ -140,6 +141,10 @@ void ESKFNode::set_parameters() { EskfParams eskf_params{.Q = Q, .P = P}; eskf_ = std::make_unique(eskf_params); + + add_gravity_to_imu_ = + this->declare_parameter("add_gravity_to_imu"); + spdlog::info("add_gravity_to_imu: {}", add_gravity_to_imu_); } void ESKFNode::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { @@ -177,7 +182,14 @@ void ESKFNode::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { // a_corrected = a_meas - omega x (omega x T) Eigen::Vector3d centripetal_accel = omega.cross(omega.cross(T_imu_eskf_)); - imu_measurement.accel = accel_aligned - centripetal_accel; + accel_aligned -= centripetal_accel; + + if (add_gravity_to_imu_) { + Eigen::Matrix3d R = nom_state.quat.normalized().toRotationMatrix(); + accel_aligned -= R.transpose() * eskf_->get_gravity(); + } + + imu_measurement.accel = accel_aligned; // save latest gyro readings (used for DVL correction and odom output) latest_gyro_measurement_ = imu_measurement.gyro; From 0377affe24ea00506b8ccd41196d74ad6741fac4 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Mon, 30 Mar 2026 18:42:30 +0200 Subject: [PATCH 246/290] move nucleus_odom_transformer to auv_setup --- .../launch/nucleus_odom_transformer.launch.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename {navigation/odom_transformer => auv_setup}/launch/nucleus_odom_transformer.launch.py (100%) diff --git a/navigation/odom_transformer/launch/nucleus_odom_transformer.launch.py b/auv_setup/launch/nucleus_odom_transformer.launch.py similarity index 100% rename from navigation/odom_transformer/launch/nucleus_odom_transformer.launch.py rename to auv_setup/launch/nucleus_odom_transformer.launch.py From fb28b46ec23718d6e9193fdd5aed13ef867da471 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Mon, 30 Mar 2026 19:15:35 +0200 Subject: [PATCH 247/290] added tf2 launch pub to state estimation launch --- .../launch/nucleus_odom_transformer.launch.py | 25 ++++++++++++++++--- auv_setup/launch/state_estimation.launch.py | 21 +++++++++++++--- 2 files changed, 39 insertions(+), 7 deletions(-) diff --git a/auv_setup/launch/nucleus_odom_transformer.launch.py b/auv_setup/launch/nucleus_odom_transformer.launch.py index 2ad0d391d..9f46081c2 100644 --- a/auv_setup/launch/nucleus_odom_transformer.launch.py +++ b/auv_setup/launch/nucleus_odom_transformer.launch.py @@ -2,7 +2,8 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import OpaqueFunction +from launch.actions import IncludeLaunchDescription, OpaqueFunction +from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node from auv_setup.launch_arg_common import ( @@ -21,6 +22,20 @@ def launch_setup(context, *args, **kwargs): f"{drone}.yaml", ) + drone_description_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("auv_setup"), + "launch", + "drone_description.launch.py", + ) + ), + launch_arguments={ + "drone": drone, + "namespace": namespace, + }.items(), + ) + nortek_nucleus_ros_interface_node = Node( package="nortek_nucleus_ros_interface", executable="nortek_nucleus_ros_interface_node", @@ -90,11 +105,15 @@ def launch_setup(context, *args, **kwargs): output="screen", ) - return [nortek_nucleus_ros_interface_node, odom_transformer_node] + return [ + drone_description_launch, + nortek_nucleus_ros_interface_node, + odom_transformer_node, + ] def generate_launch_description(): return LaunchDescription( declare_drone_and_namespace_args() + [OpaqueFunction(function=launch_setup)] - ) + ) \ No newline at end of file diff --git a/auv_setup/launch/state_estimation.launch.py b/auv_setup/launch/state_estimation.launch.py index dac567dcf..9ca1a5ce5 100644 --- a/auv_setup/launch/state_estimation.launch.py +++ b/auv_setup/launch/state_estimation.launch.py @@ -2,7 +2,8 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import OpaqueFunction +from launch.actions import IncludeLaunchDescription, OpaqueFunction +from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import ComposableNodeContainer from launch_ros.descriptions import ComposableNode @@ -22,6 +23,20 @@ def launch_setup(context, *args, **kwargs): f"{drone}.yaml", ) + drone_description_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("auv_setup"), + "launch", + "drone_description.launch.py", + ) + ), + launch_arguments={ + "drone": drone, + "namespace": namespace, + }.items(), + ) + container = ComposableNodeContainer( name="eskf_container", namespace=namespace, @@ -36,8 +51,6 @@ def launch_setup(context, *args, **kwargs): parameters=[ drone_params, { - "frame_prefix": namespace, - "diag_Q_std": [ 0.05, 0.05, 0.1, 0.01, 0.01, 0.02, @@ -159,7 +172,7 @@ def launch_setup(context, *args, **kwargs): arguments=["--ros-args", "--log-level", "error"], ) - return [container] + return [drone_description_launch, container] def generate_launch_description(): From 1bc4db825120ab08f8429d49abd39886bdab84ed Mon Sep 17 00:00:00 2001 From: vortexuser-Pi Date: Mon, 30 Mar 2026 20:39:34 +0200 Subject: [PATCH 248/290] thruster interface uart config update --- .../config/thruster_interface_auv_config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/motion/thruster_interface_auv/config/thruster_interface_auv_config.yaml b/motion/thruster_interface_auv/config/thruster_interface_auv_config.yaml index 7d31c9fc3..6ed8b5ea2 100644 --- a/motion/thruster_interface_auv/config/thruster_interface_auv_config.yaml +++ b/motion/thruster_interface_auv/config/thruster_interface_auv_config.yaml @@ -21,7 +21,7 @@ # RIGHT: [1.33421, -18.75129, 121.29079, 1528.99886] uart: - device: "/dev/ttyUSB0" + device: "/dev/serial0" baud_rate: 115200 packet_id: 1 From 59c2f5feebc34c8adda08a0d6957e959fdd1a73a Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Mon, 30 Mar 2026 21:36:39 +0200 Subject: [PATCH 249/290] ESKF: initial nominal bias config --- navigation/eskf/config/eskf_params.yaml | 4 +++- navigation/eskf/include/eskf/typedefs.hpp | 2 ++ navigation/eskf/src/eskf.cpp | 4 ++++ navigation/eskf/src/eskf_ros.cpp | 22 +++++++++++++++++++++- 4 files changed, 30 insertions(+), 2 deletions(-) diff --git a/navigation/eskf/config/eskf_params.yaml b/navigation/eskf/config/eskf_params.yaml index d3a9c9647..76dc53137 100644 --- a/navigation/eskf/config/eskf_params.yaml +++ b/navigation/eskf/config/eskf_params.yaml @@ -21,4 +21,6 @@ publish_pose: true publish_twist: true publish_rate_ms: 5 - add_gravity_to_imu: false \ No newline at end of file + add_gravity_to_imu: false + initial_gyro_bias: [0.0, 0.0, 0.0] + initial_accel_bias: [0.0, 0.0, 0.0] \ No newline at end of file diff --git a/navigation/eskf/include/eskf/typedefs.hpp b/navigation/eskf/include/eskf/typedefs.hpp index 56b6a224b..687dd1778 100644 --- a/navigation/eskf/include/eskf/typedefs.hpp +++ b/navigation/eskf/include/eskf/typedefs.hpp @@ -98,6 +98,8 @@ struct StateEuler { struct EskfParams { Eigen::Matrix12d Q = Eigen::Matrix12d::Zero(); Eigen::Matrix15d P = Eigen::Matrix15d::Zero(); + Eigen::Vector3d initial_gyro_bias = Eigen::Vector3d::Zero(); + Eigen::Vector3d initial_accel_bias = Eigen::Vector3d::Zero(); }; struct ImuMeasurement { Eigen::Vector3d accel = Eigen::Vector3d::Zero(); diff --git a/navigation/eskf/src/eskf.cpp b/navigation/eskf/src/eskf.cpp index 62497c5bd..de704c3d0 100644 --- a/navigation/eskf/src/eskf.cpp +++ b/navigation/eskf/src/eskf.cpp @@ -25,6 +25,10 @@ ESKF::ESKF(const EskfParams& params) : Q_(params.Q) { // initialize to identity quat current_nom_state_.quat = Eigen::Quaterniond::Identity(); current_nom_state_.quat.normalize(); + + // Initialize nominal bias values + current_nom_state_.gyro_bias = params.initial_gyro_bias; + current_nom_state_.accel_bias = params.initial_accel_bias; } std::pair ESKF::van_loan_discretization( diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp index a7a940dcf..66fb84221 100644 --- a/navigation/eskf/src/eskf_ros.cpp +++ b/navigation/eskf/src/eskf_ros.cpp @@ -138,7 +138,27 @@ void ESKFNode::set_parameters() { } Eigen::Matrix15d P = createDiagonalMatrix<15>(diag_p_init); - EskfParams eskf_params{.Q = Q, .P = P}; + std::vector initial_gyro_bias = + this->declare_parameter>( + "initial_gyro_bias", std::vector{0.0, 0.0, 0.0}); + if (initial_gyro_bias.size() != 3) { + throw std::runtime_error("initial_gyro_bias must have length 3"); + } + + std::vector initial_accel_bias = + this->declare_parameter>( + "initial_accel_bias", std::vector{0.0, 0.0, 0.0}); + if (initial_accel_bias.size() != 3) { + throw std::runtime_error("initial_accel_bias must have length 3"); + } + + EskfParams eskf_params{ + .Q = Q, + .P = P, + .initial_gyro_bias = + Eigen::Map(initial_gyro_bias.data()), + .initial_accel_bias = + Eigen::Map(initial_accel_bias.data())}; eskf_ = std::make_unique(eskf_params); From 928e9384b9e1148cfac5d08f98b23ac6f98af157 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Mon, 30 Mar 2026 22:00:16 +0200 Subject: [PATCH 250/290] print initial bias values --- auv_setup/launch/state_estimation.launch.py | 2 ++ navigation/eskf/src/eskf_ros.cpp | 4 ++++ 2 files changed, 6 insertions(+) diff --git a/auv_setup/launch/state_estimation.launch.py b/auv_setup/launch/state_estimation.launch.py index 9ca1a5ce5..d76c3fdc0 100644 --- a/auv_setup/launch/state_estimation.launch.py +++ b/auv_setup/launch/state_estimation.launch.py @@ -95,6 +95,8 @@ def launch_setup(context, *args, **kwargs): "publish_rate_ms": 5, "add_gravity_to_imu": True, "frame_prefix": namespace, + "initial_gyro_bias": [0.0, 0.0, 0.0], + "initial_accel_bias": [0.0, 0.0, -0.05] }, ], extra_arguments=[{"use_intra_process_comms": True}], diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp index 66fb84221..de503a55d 100644 --- a/navigation/eskf/src/eskf_ros.cpp +++ b/navigation/eskf/src/eskf_ros.cpp @@ -141,6 +141,8 @@ void ESKFNode::set_parameters() { std::vector initial_gyro_bias = this->declare_parameter>( "initial_gyro_bias", std::vector{0.0, 0.0, 0.0}); + spdlog::info("initial_gyro_bias: [{}, {}, {}]", initial_gyro_bias[0], + initial_gyro_bias[1], initial_gyro_bias[2]); if (initial_gyro_bias.size() != 3) { throw std::runtime_error("initial_gyro_bias must have length 3"); } @@ -148,6 +150,8 @@ void ESKFNode::set_parameters() { std::vector initial_accel_bias = this->declare_parameter>( "initial_accel_bias", std::vector{0.0, 0.0, 0.0}); + spdlog::info("initial_accel_bias: [{}, {}, {}]", initial_accel_bias[0], + initial_accel_bias[1], initial_accel_bias[2]); if (initial_accel_bias.size() != 3) { throw std::runtime_error("initial_accel_bias must have length 3"); } From 17d55cddc51727f837fb0a419c0a43e61a496b08 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Mon, 30 Mar 2026 22:35:17 +0200 Subject: [PATCH 251/290] ESKF: fix set imu translation --- navigation/eskf/src/eskf_ros.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp index de503a55d..0c709708b 100644 --- a/navigation/eskf/src/eskf_ros.cpp +++ b/navigation/eskf/src/eskf_ros.cpp @@ -102,6 +102,10 @@ void ESKFNode::set_parameters() { R_imu_eskf_ = Eigen::Map>( R_imu_correction.data()); + std::vector T_imu_correction = + this->declare_parameter>("transform.imu_frame_t"); + T_imu_eskf_ = Eigen::Map(T_imu_correction.data()); + std::vector R_dvl_correction = this->declare_parameter>("transform.dvl_frame_r"); R_dvl_eskf_ = Eigen::Map>( @@ -342,6 +346,7 @@ void ESKFNode::lookup_static_transforms() { Tf_base_imu_ = tf2::transformToEigen(tf_buffer_->lookupTransform( frame("base_link"), frame("imu_link"), tf2::TimePointZero)); R_imu_eskf_ = Tf_base_imu_.rotation(); + T_imu_eskf_ = Tf_base_imu_.translation(); Tf_base_dvl_ = tf2::transformToEigen(tf_buffer_->lookupTransform( frame("base_link"), frame("dvl_link"), tf2::TimePointZero)); From db646f42b255f97baa30bff061d23c299d7aed2e Mon Sep 17 00:00:00 2001 From: Cyprian Osinski Date: Mon, 30 Mar 2026 22:47:00 +0200 Subject: [PATCH 252/290] Added TODOs, figured out error quaternions and prepared for changes --- .../dp_adapt_backs_controller.hpp | 2 + .../CMakeLists.txt | 2 +- .../README.md | 0 .../config/adapt_params_nautilus.yaml | 0 .../config/adapt_params_orca.yaml | 0 .../dp_adapt_backs_controller.hpp | 65 +++++++++++++++++++ .../dp_adapt_backs_controller_ros.hpp | 4 +- .../dp_adapt_backs_controller_utils.hpp | 4 +- .../typedefs.hpp | 0 .../dp_adapt_backs_controller_quat.launch.py} | 4 +- .../package.xml | 4 +- .../src/dp_adapt_backs_controller.cpp | 7 +- .../src/dp_adapt_backs_controller_ros.cpp | 6 +- .../src/dp_adapt_backs_controller_utils.cpp | 6 +- .../test/CMakeLists.txt | 0 .../test/dp_adapt_backs_controller_tests.cpp | 0 16 files changed, 88 insertions(+), 16 deletions(-) rename control/{dp_adapt_backs_controller => dp_adapt_backs_controller_quat}/CMakeLists.txt (97%) rename control/{dp_adapt_backs_controller => dp_adapt_backs_controller_quat}/README.md (100%) rename control/{dp_adapt_backs_controller => dp_adapt_backs_controller_quat}/config/adapt_params_nautilus.yaml (100%) rename control/{dp_adapt_backs_controller => dp_adapt_backs_controller_quat}/config/adapt_params_orca.yaml (100%) create mode 100644 control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller.hpp rename control/{dp_adapt_backs_controller/include/dp_adapt_backs_controller => dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat}/dp_adapt_backs_controller_ros.hpp (96%) rename control/{dp_adapt_backs_controller/include/dp_adapt_backs_controller => dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat}/dp_adapt_backs_controller_utils.hpp (96%) rename control/{dp_adapt_backs_controller/include/dp_adapt_backs_controller => dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat}/typedefs.hpp (100%) rename control/{dp_adapt_backs_controller/launch/dp_adapt_backs_controller.launch.py => dp_adapt_backs_controller_quat/launch/dp_adapt_backs_controller_quat.launch.py} (89%) rename control/{dp_adapt_backs_controller => dp_adapt_backs_controller_quat}/package.xml (85%) rename control/{dp_adapt_backs_controller => dp_adapt_backs_controller_quat}/src/dp_adapt_backs_controller.cpp (90%) rename control/{dp_adapt_backs_controller => dp_adapt_backs_controller_quat}/src/dp_adapt_backs_controller_ros.cpp (97%) rename control/{dp_adapt_backs_controller => dp_adapt_backs_controller_quat}/src/dp_adapt_backs_controller_utils.cpp (96%) rename control/{dp_adapt_backs_controller => dp_adapt_backs_controller_quat}/test/CMakeLists.txt (100%) rename control/{dp_adapt_backs_controller => dp_adapt_backs_controller_quat}/test/dp_adapt_backs_controller_tests.cpp (100%) 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 a26969cee..d88a584fa 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 @@ -22,6 +22,8 @@ class DPAdaptBacksController { public: explicit DPAdaptBacksController(const DPAdaptParams& dp_adapt_params); + // TODO: change calculate tau, to be based on error state of ori quaternion + // @brief Calculate thecontrol input tau // @param pose: 6D vector containing the vehicle pose [x, y, z, roll, pitch, // yaw] diff --git a/control/dp_adapt_backs_controller/CMakeLists.txt b/control/dp_adapt_backs_controller_quat/CMakeLists.txt similarity index 97% rename from control/dp_adapt_backs_controller/CMakeLists.txt rename to control/dp_adapt_backs_controller_quat/CMakeLists.txt index 1a38d1bde..0fac2659e 100644 --- a/control/dp_adapt_backs_controller/CMakeLists.txt +++ b/control/dp_adapt_backs_controller_quat/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(dp_adapt_backs_controller) +project(dp_adapt_backs_controller_quat) if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 20) diff --git a/control/dp_adapt_backs_controller/README.md b/control/dp_adapt_backs_controller_quat/README.md similarity index 100% rename from control/dp_adapt_backs_controller/README.md rename to control/dp_adapt_backs_controller_quat/README.md diff --git a/control/dp_adapt_backs_controller/config/adapt_params_nautilus.yaml b/control/dp_adapt_backs_controller_quat/config/adapt_params_nautilus.yaml similarity index 100% rename from control/dp_adapt_backs_controller/config/adapt_params_nautilus.yaml rename to control/dp_adapt_backs_controller_quat/config/adapt_params_nautilus.yaml diff --git a/control/dp_adapt_backs_controller/config/adapt_params_orca.yaml b/control/dp_adapt_backs_controller_quat/config/adapt_params_orca.yaml similarity index 100% rename from control/dp_adapt_backs_controller/config/adapt_params_orca.yaml rename to control/dp_adapt_backs_controller_quat/config/adapt_params_orca.yaml diff --git a/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller.hpp b/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller.hpp new file mode 100644 index 000000000..d1b85e559 --- /dev/null +++ b/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller.hpp @@ -0,0 +1,65 @@ +#ifndef DP_ADAPT_BACKS_CONTROLLER__DP_ADAPT_BACKS_CONTROLLER_HPP_ +#define DP_ADAPT_BACKS_CONTROLLER__DP_ADAPT_BACKS_CONTROLLER_HPP_ + +#include +#include +#include "dp_adapt_backs_controller_quat/typedefs.hpp" + +namespace vortex::control { + +struct DPAdaptParams { + Eigen::Vector12d adapt_param = Eigen::Vector12d::Zero(); + Eigen::Vector6d d_gain = Eigen::Vector6d::Zero(); + Eigen::Vector6d K1 = Eigen::Vector6d::Zero(); + Eigen::Vector6d K2 = Eigen::Vector6d::Zero(); + Eigen::Vector3d r_b_bg = Eigen::Vector3d::Zero(); + Eigen::Vector3d I_b = Eigen::Vector3d::Zero(); + Eigen::Matrix6d mass_matrix = Eigen::Matrix6d::Zero(); + double mass{}; +}; + +class DPAdaptBacksController { + public: + explicit DPAdaptBacksController(const DPAdaptParams& dp_adapt_params); + + // TODO: change calculate tau, to be based on error state of quaternion + + // @brief Calculate thecontrol input tau + // @param pose: 6D vector containing the vehicle pose [x, y, z, roll, pitch, + // yaw] + // @param pose_d: 6D vector containing the desired vehicle pose [x, y, z, + // roll, pitch, yaw] + // @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::PoseEuler& pose, + const vortex::utils::types::PoseEuler& pose_d, + const vortex::utils::types::Twist& twist); + + // @brief Reset the adaptive parameters + void reset_adap_param(); + + // @brief Reset the disturbance estimate + void reset_d_est(); + + // @brief Set the time step + // @param dt: Time step + void set_time_step(const double dt); + + private: + Eigen::Matrix6d K1_; + Eigen::Matrix6d K2_; + Eigen::Vector3d r_b_bg_; + Eigen::Matrix12d adapt_gain_; + Eigen::Matrix6d d_gain_; + Eigen::Vector12d adapt_param_; + Eigen::Vector6d d_est_; + Eigen::Matrix3d I_b_; + Eigen::Matrix6d mass_matrix_; + double m_{}; + double dt_{}; +}; + +} // namespace vortex::control + +#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_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller_ros.hpp similarity index 96% rename from control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_ros.hpp rename to control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller_ros.hpp index bf25cd57e..b02e364c5 100644 --- a/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_ros.hpp +++ b/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller_ros.hpp @@ -17,8 +17,8 @@ #include #include #include -#include "dp_adapt_backs_controller/dp_adapt_backs_controller.hpp" -#include "dp_adapt_backs_controller/typedefs.hpp" +#include "dp_adapt_backs_controller_quat/dp_adapt_backs_controller.hpp" +#include "dp_adapt_backs_controller_quat/typedefs.hpp" #include "typedefs.hpp" namespace vortex::control { 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_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller_utils.hpp similarity index 96% rename from control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_utils.hpp rename to control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller_utils.hpp index 7a0e214a9..9848473d1 100644 --- a/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_utils.hpp +++ b/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller_utils.hpp @@ -2,9 +2,11 @@ #define DP_ADAPT_BACKS_CONTROLLER__DP_ADAPT_BACKS_CONTROLLER_UTILS_HPP_ #include -#include "dp_adapt_backs_controller/typedefs.hpp" +#include "dp_adapt_backs_controller_quat/typedefs.hpp" #include "typedefs.hpp" +// TODO: change T_dot, J_dot and J_inv + namespace vortex::control { // @brief Calculate the derivative of the rotation matrix diff --git a/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/typedefs.hpp b/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/typedefs.hpp similarity index 100% rename from control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/typedefs.hpp rename to control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/typedefs.hpp diff --git a/control/dp_adapt_backs_controller/launch/dp_adapt_backs_controller.launch.py b/control/dp_adapt_backs_controller_quat/launch/dp_adapt_backs_controller_quat.launch.py similarity index 89% rename from control/dp_adapt_backs_controller/launch/dp_adapt_backs_controller.launch.py rename to control/dp_adapt_backs_controller_quat/launch/dp_adapt_backs_controller_quat.launch.py index fe3972bf8..8cb6a5f2c 100644 --- a/control/dp_adapt_backs_controller/launch/dp_adapt_backs_controller.launch.py +++ b/control/dp_adapt_backs_controller_quat/launch/dp_adapt_backs_controller_quat.launch.py @@ -15,7 +15,7 @@ def launch_setup(context, *args, **kwargs): drone, namespace = resolve_drone_and_namespace(context) adapt_params = os.path.join( - get_package_share_directory("dp_adapt_backs_controller"), + get_package_share_directory("dp_adapt_backs_controller_quat"), "config", f"adapt_params_{drone}.yaml", ) @@ -29,7 +29,7 @@ def launch_setup(context, *args, **kwargs): return [ Node( - package="dp_adapt_backs_controller", + package="dp_adapt_backs_controller_quat", executable="dp_adapt_backs_controller_node", name="dp_adapt_backs_controller_node", namespace=namespace, diff --git a/control/dp_adapt_backs_controller/package.xml b/control/dp_adapt_backs_controller_quat/package.xml similarity index 85% rename from control/dp_adapt_backs_controller/package.xml rename to control/dp_adapt_backs_controller_quat/package.xml index c618f6e06..a2d7afe43 100644 --- a/control/dp_adapt_backs_controller/package.xml +++ b/control/dp_adapt_backs_controller_quat/package.xml @@ -1,10 +1,10 @@ - dp_adapt_backs_controller + dp_adapt_backs_controller_quat 1.0.0 Adaptive backstepping controller for DP - talhanc + Cyprian Osinski MIT ament_cmake diff --git a/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller.cpp b/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller.cpp similarity index 90% rename from control/dp_adapt_backs_controller/src/dp_adapt_backs_controller.cpp rename to control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller.cpp index e009d7f90..353e1f29f 100644 --- a/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller.cpp +++ b/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller.cpp @@ -1,9 +1,9 @@ -#include "dp_adapt_backs_controller/dp_adapt_backs_controller.hpp" +#include "dp_adapt_backs_controller_quat/dp_adapt_backs_controller.hpp" #include #include #include -#include "dp_adapt_backs_controller/dp_adapt_backs_controller_utils.hpp" -#include "dp_adapt_backs_controller/typedefs.hpp" +#include "dp_adapt_backs_controller_quat/dp_adapt_backs_controller_utils.hpp" +#include "dp_adapt_backs_controller_quat/typedefs.hpp" namespace vortex::control { @@ -49,6 +49,7 @@ Eigen::Vector6d DPAdaptBacksController::calculate_tau(const PoseEuler& pose, (pose.as_j_matrix().transpose() * z_1) - (K2_ * z_2) - F_est - d_est_; + // TODO: look at better ways to clamp tau w.r.t new thrusters and allocator tau = tau.cwiseMax(-100.0).cwiseMin(100.0); adapt_param_ += adapt_param_dot * dt_; d_est_ += d_est_dot * dt_; diff --git a/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_ros.cpp b/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_ros.cpp similarity index 97% rename from control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_ros.cpp rename to control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_ros.cpp index 8e6c884ce..5890acc2f 100644 --- a/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_ros.cpp +++ b/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_ros.cpp @@ -1,4 +1,4 @@ -#include "dp_adapt_backs_controller/dp_adapt_backs_controller_ros.hpp" +#include "dp_adapt_backs_controller_quat/dp_adapt_backs_controller_ros.hpp" #include #include #include @@ -6,8 +6,8 @@ #include #include #include -#include "dp_adapt_backs_controller/dp_adapt_backs_controller_utils.hpp" -#include "dp_adapt_backs_controller/typedefs.hpp" +#include "dp_adapt_backs_controller_quat/dp_adapt_backs_controller_utils.hpp" +#include "dp_adapt_backs_controller_quat/typedefs.hpp" constexpr std::string_view start_message = R"( ____ ____ ____ _ _ _ diff --git a/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_utils.cpp b/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_utils.cpp similarity index 96% rename from control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_utils.cpp rename to control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_utils.cpp index 116326a6b..1922580bf 100644 --- a/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_utils.cpp +++ b/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_utils.cpp @@ -1,8 +1,10 @@ -#include "dp_adapt_backs_controller/dp_adapt_backs_controller_utils.hpp" +#include "dp_adapt_backs_controller_quat/dp_adapt_backs_controller_utils.hpp" #include #include #include -#include "dp_adapt_backs_controller/typedefs.hpp" +#include "dp_adapt_backs_controller_quat/typedefs.hpp" + +// TODO: change j_inv, t_dot, j_dot namespace vortex::control { diff --git a/control/dp_adapt_backs_controller/test/CMakeLists.txt b/control/dp_adapt_backs_controller_quat/test/CMakeLists.txt similarity index 100% rename from control/dp_adapt_backs_controller/test/CMakeLists.txt rename to control/dp_adapt_backs_controller_quat/test/CMakeLists.txt diff --git a/control/dp_adapt_backs_controller/test/dp_adapt_backs_controller_tests.cpp b/control/dp_adapt_backs_controller_quat/test/dp_adapt_backs_controller_tests.cpp similarity index 100% rename from control/dp_adapt_backs_controller/test/dp_adapt_backs_controller_tests.cpp rename to control/dp_adapt_backs_controller_quat/test/dp_adapt_backs_controller_tests.cpp From 71c1b84ed3af80dee78ad4e017a99ee6a924c8bb Mon Sep 17 00:00:00 2001 From: Cyprian Osinski Date: Mon, 30 Mar 2026 22:53:40 +0200 Subject: [PATCH 253/290] Readded old controller, no need to throw it away. --- .../dp_adapt_backs_controller/CMakeLists.txt | 79 +++ control/dp_adapt_backs_controller/README.md | 418 +++++++++++++++ .../config/adapt_params_nautilus.yaml | 7 + .../config/adapt_params_orca.yaml | 7 + .../dp_adapt_backs_controller.hpp | 2 - .../dp_adapt_backs_controller_ros.hpp | 111 ++++ .../dp_adapt_backs_controller_utils.hpp | 57 +++ .../dp_adapt_backs_controller/typedefs.hpp | 22 + .../dp_adapt_backs_controller.launch.py | 45 ++ control/dp_adapt_backs_controller/package.xml | 25 + .../src/dp_adapt_backs_controller.cpp | 73 +++ .../src/dp_adapt_backs_controller_ros.cpp | 259 ++++++++++ .../src/dp_adapt_backs_controller_utils.cpp | 121 +++++ .../test/CMakeLists.txt | 28 ++ .../test/dp_adapt_backs_controller_tests.cpp | 474 ++++++++++++++++++ 15 files changed, 1726 insertions(+), 2 deletions(-) create mode 100644 control/dp_adapt_backs_controller/CMakeLists.txt create mode 100644 control/dp_adapt_backs_controller/README.md create mode 100644 control/dp_adapt_backs_controller/config/adapt_params_nautilus.yaml create mode 100644 control/dp_adapt_backs_controller/config/adapt_params_orca.yaml create mode 100644 control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_ros.hpp create mode 100644 control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_utils.hpp create mode 100644 control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/typedefs.hpp create mode 100644 control/dp_adapt_backs_controller/launch/dp_adapt_backs_controller.launch.py create mode 100644 control/dp_adapt_backs_controller/package.xml create mode 100644 control/dp_adapt_backs_controller/src/dp_adapt_backs_controller.cpp create mode 100644 control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_ros.cpp create mode 100644 control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_utils.cpp create mode 100644 control/dp_adapt_backs_controller/test/CMakeLists.txt create mode 100644 control/dp_adapt_backs_controller/test/dp_adapt_backs_controller_tests.cpp diff --git a/control/dp_adapt_backs_controller/CMakeLists.txt b/control/dp_adapt_backs_controller/CMakeLists.txt new file mode 100644 index 000000000..1a38d1bde --- /dev/null +++ b/control/dp_adapt_backs_controller/CMakeLists.txt @@ -0,0 +1,79 @@ +cmake_minimum_required(VERSION 3.8) +project(dp_adapt_backs_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(vortex_utils REQUIRED) +find_package(vortex_utils_ros REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(tf2 REQUIRED) +find_package(vortex_msgs REQUIRED) +find_package(fmt REQUIRED) +find_package(spdlog REQUIRED) + + +include_directories(include) + +set(LIB_NAME "${PROJECT_NAME}_component") + +add_library(${LIB_NAME} SHARED + src/dp_adapt_backs_controller.cpp + src/dp_adapt_backs_controller_ros.cpp + src/dp_adapt_backs_controller_utils.cpp) + +ament_target_dependencies(${LIB_NAME} PUBLIC + rclcpp + rclcpp_components + geometry_msgs + nav_msgs + Eigen3 + tf2 + fmt + spdlog + vortex_msgs + vortex_utils + vortex_utils_ros +) + +rclcpp_components_register_node( + ${LIB_NAME} + PLUGIN "DPAdaptBacksControllerNode" + 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 + config + DESTINATION share/${PROJECT_NAME}/ +) + +if(BUILD_TESTING) + add_subdirectory(test) +endif() + +ament_package() diff --git a/control/dp_adapt_backs_controller/README.md b/control/dp_adapt_backs_controller/README.md new file mode 100644 index 000000000..3c34a2b6b --- /dev/null +++ b/control/dp_adapt_backs_controller/README.md @@ -0,0 +1,418 @@ +## DP Adaptive Backstepping Controller + +This package implements a dynamic positioning (DP) Adaptive backstepping controller for the orca AUV. It estimates the linear and nonlinear damping using adaptive parameters, and compensates for uncertainties and disturbances in real-time in the same manner as a state estimator. The proof for this is done using Lyapunov functions and stability requirements to ensure convergence and stability. + +### Overview +- Uses the backstepping control method for position and orientation control +- Includes adaptive terms to account for unmodeled dynamics and uncertainties. + +### Model for AUV + +```math +\dot{\eta} = J(\eta)\nu, +``` + +```math +M \dot{\nu} + C(\nu)\,\nu - F(\nu, \Theta^\star) = \tau + d^\star +``` + +- $\nu$: Body-fixed velocity vector +- $\eta$: Inertial position and orientation vector +- M: Constant mass-inertia matrix +- C($\eta$): Coriolis and centripetal terms +- J($\eta$): Transformation from body to inertial coordinates +- F($\nu$, $\Theta^\star$) = Y($\nu$) $\Theta^\star$: The damping assumed damping function (linear and nonlinear), where Y(*) describes the behaviour +- $d^\star$: Disturbance and uncertainties + +### File overview +1. **dp_adapt_backs_controller.cpp/hpp** + - The controller implementation + - Implements the main control input, sets gains, references, and mass parameters. + +2. **dp_adapt_backs_controller_utils.cpp/hpp** + - Provides utility functions: skew-symmetric matrix generation, quaternion-to-Euler, Jacobian and some functions needed for the adaptive functions. + +3. **dp_adapt_backs_controller_ros.cpp/hpp** + - ROS node wrapper for the controller, subscribing to pose, twist, killswitch, and reference topics. + - Publishes thrust commands. + +4. **dp_adapt_backs_controller_node.cpp** + - Entry point for the ROS executable. + +5. **adapt_params.yaml** + - Tunable controller parameters (K1, K2, adapt_gain, d_gain) + - Contains the mass matrix (6×6) and other physical parameters: + ```math + M = + \begin{bmatrix} + 30.0 & 0.0 & 0.0 & \cdots \\ + 0.0 & 30.0 & \cdots & \cdots \\ + 0.0 & \cdots & 30.0 & \cdots \\ + \cdots & \cdots & \cdots & \cdots + \end{bmatrix} + ``` + (partially shown for brevity) + This is also added into the orca.yaml file + +6. **CMakeLists.txt** + - Build configuration, ROS 2 dependencies, executable generation, and installation setup. + +### Tuning Parameters +- **K1** and **K2**: Gains for the backstepping control laws. +- **adapt_gain**: Adaptive gain for the linear and nonlinear damping. +- **d_gain**: Adaptive gain for the disturbances and uncertainties. +- **M** and **I_b**: Mass inertia matrix and rotational inertia. +- **m**: Vehicle mass. + +## Backstepping controller + +Using the Lyapunov proof we get the following functions for the backstepping controller: + +##### Backstepping variables + +```math +z_1 = \eta - \eta_d +``` + +```math +z_2 = \nu - \alpha +``` + +where the $\alpha$ is defined as the function that stabilizes the $z_1$ variable in the Lyapunov function system. + +##### Adaptive parameters + +```math +\tilde{\Theta} = \hat{\Theta} - \Theta^\star +``` + +```math +\tilde{d} = \hat{d} - d^\star +``` + +where: + +- $\Theta^\star$ and $d^\star$ are the actual parameters, +- $\hat{\Theta}$ and $\hat{d}$ are the estimated parameters, +- $\tilde{\Theta}$ and $\tilde{d}$ are the estimation errors. + +#### Proof of control law + +We define the Lyapunov function candidate (LFC) as: + +```math +V_1 = \frac{1}{2} z_1^\top z_1 +``` + +By fulfilling the criteriums of RUB, positive definite and \(V_1(0) = 0\), we need to do a derivation and ensure negative definite. + +Taking the derivative of the Lyapunov function candidate \(V_1\): + +```math +\dot{V}_1 = \frac{d}{dt} \left( \frac{1}{2} z_1^\top z_1 \right) +``` + +Using the chain rule, we get: + +```math +\dot{V}_1 = z_1^\top \dot{z}_1 +``` + +Substitute $\dot{z}_1$ with the dynamics of $z_1$: + +```math +\dot{z}_1 = \dot{\eta} - \dot{\eta}_d +``` + +and then inserting in the relation: + +```math +\dot{\eta} = J(\eta)\nu +``` + +```math +\dot{z}_1 = J(\eta)\nu - \dot{\eta}_d +``` + +and +```math +\nu = z_2 + \alpha +``` +Thus, the derivative of the Lyapunov function candidate is: +```math +\dot{V}_1 = z_1^\top J(\eta)(z_2 + \alpha) +``` + +We also assume that $\dot{\eta_d} = 0$ since we only get a desired position and orientation ($\eta$). + +We choose an $\alpha$ value to make $z_1$ be negative semi-definite. + +```math +\boxed{ +\alpha = -J(\eta)^{-1}(K_1 z_1) +} +``` + +```math +\dot{V}_1 = z_1^\top J(\eta)z_2 - z_1^\top K_1 z_1 < 0 +``` + +We will now define the rest of the Lyapunov function candidates, including the adaptive parameter. + +```math +V_2 = \frac{1}{2} z_2^\top M z_2 +``` + +For the second backstepping variable: + +```math +V_{\theta} = \frac{1}{2} \tilde{\Theta}^\top \Gamma^{-1}_{\theta} \tilde{\Theta} +``` + +```math +V_d = \frac{1}{2} \tilde{d}^\top \Gamma^{-1}_{d} \tilde{d} +``` + +Where the $\Gamma$ matrix is a diagonal gain matrix for tuning the adaptive rate of the parameters. The reasoning behind inserting the adaptive parameters is that we want to ensure that the error, marked by the tilde, converges towards zero. To achieve this, we need to ensure that it actually does this by accounting for it in the Lyapunov function and controller. + +Then we combine them like this: + +```math +V = V_1 + V_2 + V_{\theta} + V_d +``` + +and now we will analyze the derivative of this CLF, and ensure convergence for the whole function. + +```math +\dot{V} = \dot{V}_1 + z_2^\top M \dot{z}_2 + \tilde{\Theta}^\top \Gamma^{-1}_{\theta} \dot{\tilde{\Theta}} + \tilde{d}^\top \Gamma^{-1}_{d} \dot{\tilde{d}} +``` + +Before we write this out, we need to make some assumptions to make this more readable and easier to understand. + +For $\dot{\tilde{\Theta}} = \dot{\hat{\Theta}} - \dot{\Theta}^\star$ we assume that the actual value has no changes, assuming it's static, and therefore the derivative $\dot{\Theta}^\star = 0$ + +2. The same condition holds for $\dot{\tilde{d}}$ + +```math +\dot{V} = \dot{V}_1 + z_2^\top M (\dot{\nu} - \dot{\alpha}) + \tilde{\Theta}^\top \Gamma^{-1}_{\theta} \dot{\hat{\Theta}} + \tilde{d}^\top \Gamma^{-1}_{d} \dot{\hat{d}} +``` + +```math += \dot{V}_1 - z_2^\top M \dot{\alpha} + z_2^\top\tau - z_2^\top C(\nu)\,\nu + z_2^\top F(\nu, \Theta^\star) + z_2^\top d + \tilde{\Theta}^\top \Gamma^{-1}_{\theta} \dot{\hat{\Theta}} + \tilde{d}^\top \Gamma^{-1}_{d} \dot{\hat{d}} +``` + +```math += - z_1^\top K_1z_1 + z_2^\top J(\eta)^\top z_1 - z_2^\top M \dot{\alpha} + z_2^\top \tau - z_2^\top C(\nu)\,\nu + z_2^\top Y(\nu) \Theta\star + z_2^\top d + \tilde{\Theta}^\top \Gamma^{-1}_{\theta} \dot{\hat{\Theta}} + \tilde{d}^\top \Gamma^{-1}_{d} \dot{\hat{d}} +``` + +Since we only know the estimate of the adaptive parameters, we can write the controller in two parts: + +```math +\tau = \tau_{controller} - F(\nu, \hat{\Theta}) - d = \tau_{controller} -Y(\nu) \hat{\Theta} - \hat{d} +``` +Important to notice is that we introduce the estimate (^) for the variables, not the actual value (*). +We insert this into the system and get: + +```math += - z_1^\top K_1 z_1 + z_2^\top J(\eta)^\top z_1 - z_2^\top M \dot{\alpha} + z_2^\top \tau_{controller} - z_2^\top C(\nu)\,\nu - z_2^\top Y(\nu) (\hat{\Theta} - \Theta\star ) - z_2^\top (\hat{d} - d\star) + \tilde{\Theta}^\top \Gamma^{-1}_{\theta} \dot{\hat{\Theta}} + \tilde{d}^\top \Gamma^{-1}_{d} \dot{\hat{d}} +``` + +We look at the adaptive parameters a little more and try to simplify them as much as possible + +```math +\hat{\Theta} - \Theta^\star = \hat{\Theta} - (\hat{\Theta} - \tilde{\Theta}) = \tilde{\Theta} \newline +``` +```math +\hat{d} - d^\star = \hat{d} - (\hat{d} - \tilde{d}) = \tilde{d} +``` + +Now we have: +```math += - z_1^\top K_1 z_1 + z_2^\top J(\eta)^\top z_1 - z_2^\top M \dot{\alpha} + z_2^\top \tau_{controller} - z_2^\top C(\nu)\,\nu + (-z_2^\top Y(\nu) \tilde{\Theta}+ \tilde{\Theta}^\top \Gamma^{-1}_{\theta} \dot{\hat{\Theta}}) + (-z_2^\top \tilde{d} + \tilde{d}^\top \Gamma^{-1}_{d} \dot{\hat{d}}) +``` +From this we can separate out the terms with the adaptive parameters. We can write them up as two separate equations: + +```math +-z_2^\top Y(\nu) \tilde{\Theta}+\tilde{\Theta}^\top \Gamma^{-1}_{\theta} \dot{\hat{\Theta}} = - \tilde{\Theta}^\top Y(\nu)^\top z_2 +\tilde{\Theta}^\top \Gamma^{-1}_{\theta} \dot{\hat{\Theta}} +``` + +```math +-z_2^\top \tilde{d} + \tilde{d}^\top \Gamma^{-1}_{d} \dot{\hat{d}} = - \tilde{d}^\top z_2+ \tilde{d}^\top \Gamma^{-1}_{d} \dot{\hat{d}} +``` + +We choose the \(\dot{\hat{\Theta}}\) and \(\dot{\hat{d}}\) to zero this out +```math +\boxed{ +\dot{\hat{\Theta}} = \Gamma_{\theta} Y(\nu)^\top z_2 +} +``` + +```math +\boxed{ +\dot{\hat{d}} = \Gamma_{d} z_2 +} +``` +Now that we have defined this we can insert and remove this from the equation, which should leave us with the normal system. An observation made during the construction of the controller was that the adaptive part and backstepping part is decoupled. Maybe this is dependent on the method used for the adaptive part, which is more of a MRAC type. Sadly, I don't have enough information about adaptive controllers to comment on this in detail. + +```math += - z_1^\top K_1 z_1 + z_2^\top J(\eta)^\top z_1 - z_2^\top M \dot{\alpha} + z_2^\top \tau_{controller} - z_2^\top C(\nu)\,\nu +``` +```math +\tau_{controller} = -K_2 z_2 + M \dot{\alpha} + C(\nu)\nu - J(\eta)^\top z_1 +``` + +Combined this gives us the control law: + +```math +\boxed{ +\tau = -K_2 z_2 + M \dot{\alpha} + C(\nu)\nu - J(\eta)z_1 - F(\nu, \hat{\Theta}) - \hat{d} +} +``` +### Controller gains + +The gains \(K_1\) and \(K_2\) are crucial for ensuring the stability and performance of the controller. + +**\(K_1\)**: This gain matrix is associated with the outer loop stability, which primarily deals with the position and orientation errors (\(z_1\)). Proper tuning of \(K_1\) ensures that the position and orientation errors converge to zero, thereby stabilizing the outer loop. + +```math +\begin{bmatrix} +k_{1,1} & 0 & 0 & 0 & 0 & 0 \\ +0 & k_{1,2} & 0 & 0 & 0 & 0 \\ +0 & 0 & k_{1,3} & 0 & 0 & 0 \\ +0 & 0 & 0 & k_{1,4} & 0 & 0 \\ +0 & 0 & 0 & 0 & k_{1,5} & 0 \\ +0 & 0 & 0 & 0 & 0 & k_{1,6} +\end{bmatrix} +``` + +**\(K_2\)**: This gain matrix is related to the inner loop stability, which handles the velocity errors (\(z_2\)). Tuning \(K_2\) appropriately ensures that the velocity errors are minimized, stabilizing the inner loop. + +```math +K_2 = +\begin{bmatrix} +k_{2,1} & 0 & 0 & 0 & 0 & 0 \\ +0 & k_{2,2} & 0 & 0 & 0 & 0 \\ +0 & 0 & k_{2,3} & 0 & 0 & 0 \\ +0 & 0 & 0 & k_{2,4} & 0 & 0 \\ +0 & 0 & 0 & 0 & k_{2,5} & 0 \\ +0 & 0 & 0 & 0 & 0 & k_{2,6} +\end{bmatrix} +``` + +Together, \(K_1\) and \(K_2\) work to ensure that both the position/orientation and velocity errors are driven to zero, guaranteeing the overall stability and performance of the adaptive backstepping controller. + +### Adaptive parameters and functions + +Now that we have the proof for the control law, we need to look at the adaptive functions and parameters. + +Since we wanted a damping (linear and nonlinear) \(Y(\nu)\) was chosen to be a 6x12 matrix with one linear and one nonlinear damping element: + +```math +Y(\nu) = +\begin{bmatrix} +\nu[0] & \nu[0] |\nu[0]| & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ +0 & 0 & \nu[1] & \nu[1] |\nu[1]| & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ +0 & 0 & 0 & 0 & \nu[2] & \nu[2] |\nu[2]| & 0 & 0 & 0 & 0 & 0 & 0 \\ +0 & 0 & 0 & 0 & 0 & 0 & \nu[3] & \nu[3] |\nu[3]| & 0 & 0 & 0 & 0 \\ +0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & \nu[4] & \nu[4] |\nu[4]| & 0 & 0 \\ +0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & \nu[5] & \nu[5] |\nu[5]| +\end{bmatrix} +``` + +while the \(\hat{\Theta}\) will be a 12x1 vector: + +```math +\hat{\Theta} = +\begin{bmatrix} +\alpha_1 & \beta_1 & \alpha_2 & \beta_2 & \alpha_3 & \beta_3 & \alpha_4 & \beta_4 & \alpha_5 & \beta_5 & \alpha_6 & \beta_6 +\end{bmatrix}^\top +``` + +The \(\Gamma_{\Theta}\) will therefore be a 12x12 diagonal vector with the gains for the linear and nonlinear damping. + +The disturbance \(d\) will have a 6x1 vector for the disturbance and uncertainty estimates: + +```math +\hat{d} = +\begin{bmatrix} +d_1 & d_2 & d_3 & d_4 & d_5 & d_6 +\end{bmatrix}^\top +``` + +#### Important information + +The implementation of the controller requires \(\dot{\alpha}\) which is not that simple to calculate, given that this requires the derivative of the \(J(\eta)\) matrix. + +```math +\dot{\alpha} +\;=\; +-\;J(\eta)^{-1}\,\dot{J}(\eta)\,J(\eta)^{-1}\bigl[- k_1\,(\eta - \eta_d)\bigr] +\;+\; +-J(\eta)^{-1} k_1\,\dot{\eta} +``` + +This was manually derived, and \(J(\eta)\) was also derived manually but here we can use a trick: + +```math +J(\eta) = +\begin{bmatrix} +R & 0_{3x3} \\ +0_{3x3} & T +\end{bmatrix} +``` + +where: +- \(R\) is the rotation matrix representing the orientation of the AUV. +- \(T\) is the transformation matrix for the translational components. +- \(0_{3x3}\) is a 3x3 zero matrix. + +The derivative of \(J(\eta)\) can be expressed in terms of the derivatives of \(R\) and \(T\): + +```math +\dot{J}(\eta) = +\begin{bmatrix} +\dot{R} & 0_{3x3} \\ +0_{3x3} & \dot{T} +\end{bmatrix} +``` + +The trick is that \(\dot{R}\) can be written as: + +```math +\dot{R} = R \cdot S \cdot r +``` + +where: +- \(R\) is the rotation matrix calculated from \(\eta\). +- \(S\) is the skew-symmetric matrix of the vector \((0, 0, 1)\). +- \(r\) is the z-component of the angular speed from \(\nu\). + +For the \(T\) the expression needs to be manually calculated and this is not nice to look at: + +```math +\dot{T}(\eta, \nu) = +\begin{bmatrix} +0 & \cos(\phi) \tan(\theta) \nu_x + \sin(\phi) \sec^2(\theta) \nu_y & -\sin(\phi) \tan(\theta) \nu_x + \cos(\phi) \sec^2(\theta) \nu_y \\ +0 & -\sin(\phi) \nu_x & -\cos(\phi) \nu_x \\ +0 & \cos(\phi) / \cos(\theta) \nu_x + \sin(\phi) \sin(\theta) / \cos^2(\theta) \nu_y & -\sin(\phi) / \cos(\theta) \nu_x + \cos(\phi) \sin(\theta) / \cos^2(\theta) \nu_y +\end{bmatrix} +``` + +This can be calculated by the chain rule and partial differentiation on \(\phi, \theta, \psi\): + +```math +\dot{T}(\eta, \nu) = \frac{\partial T}{\partial \phi} \dot{\phi} + \frac{\partial T}{\partial \theta} \dot{\theta} + \frac{\partial T}{\partial \psi} \dot{\psi} +``` + +Here we could potentially use methods for estimating the derivative, but one of the benefits of this calculation is that we get good and accurate values for the rotation and translation derivatives. Instead of introducing more uncertainties as we try to remove this in the system with the adaptive part. + +## Launch + +To run the controller, use the ROS 2 launch file: +```bash +ros2 launch dp_adapt_backs_controller dp_adapt_backs_controller.launch.py +``` + +It's important to `colcon build` and `source install/setup.bash` before launching, or else it will not be launchable. + +This will load the parameters from the provided YAML, start the node, and begin control operation. diff --git a/control/dp_adapt_backs_controller/config/adapt_params_nautilus.yaml b/control/dp_adapt_backs_controller/config/adapt_params_nautilus.yaml new file mode 100644 index 000000000..65b639ed7 --- /dev/null +++ b/control/dp_adapt_backs_controller/config/adapt_params_nautilus.yaml @@ -0,0 +1,7 @@ +/**: + ros__parameters: + K1 : [15.0, 15.0, 15.0, 1.5, 6.0, 6.0] # Outer loop tuning parameters + K2 : [30.0, 30.0, 30.0, 2.0, 8.5, 8.5] # Inner loop tuning parameters + r_b_bg : [0.0, 0.0, 0.01] # Vector from body centre to centre of gravity + adapt_gain : [0.6, 0.15, 0.6, 0.15, 0.6, 0.15, 0.6, 0.15, 0.6, 0.15, 0.6, 0.15] # Tuning parameters for linear and nonlinear damping + d_gain : [0.8, 0.8, 0.8, 0.8, 0.8, 0.8] # Tuning parameters for the unmodeled disturbances and uncertanties diff --git a/control/dp_adapt_backs_controller/config/adapt_params_orca.yaml b/control/dp_adapt_backs_controller/config/adapt_params_orca.yaml new file mode 100644 index 000000000..e128131b2 --- /dev/null +++ b/control/dp_adapt_backs_controller/config/adapt_params_orca.yaml @@ -0,0 +1,7 @@ +/**: + ros__parameters: + K1 : [10.5, 10.5, 13.5, 0.0, 4.0, 4.0] # Outer loop tuning parameters + K2 : [20.5, 20.5, 20.5, 0.0, 5.5, 5.5] # Inner loop tuning parameters + r_b_bg : [0.01, 0.0, 0.02] # Vector from body centre to centre of gravity + adapt_gain : [0.4, 0.1, 0.4, 0.1, 0.4, 0.1, 0.4, 0.1, 0.4, 0.1, 0.4, 0.1] # Tuning parameters for linear and nonlinear damping + d_gain : [0.6, 0.6, 0.6, 0.6, 0.6, 0.6] # Tuning parameters for the unmodeled disturbances and uncertanties 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 d88a584fa..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 @@ -22,8 +22,6 @@ class DPAdaptBacksController { public: explicit DPAdaptBacksController(const DPAdaptParams& dp_adapt_params); - // TODO: change calculate tau, to be based on error state of ori quaternion - // @brief Calculate thecontrol input tau // @param pose: 6D vector containing the vehicle pose [x, y, z, roll, pitch, // yaw] 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 new file mode 100644 index 000000000..bf25cd57e --- /dev/null +++ b/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_ros.hpp @@ -0,0 +1,111 @@ +#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 +#include +#include "dp_adapt_backs_controller/dp_adapt_backs_controller.hpp" +#include "dp_adapt_backs_controller/typedefs.hpp" +#include "typedefs.hpp" + +namespace vortex::control { + +// @brief Class for the DP Adaptive Backstepping controller node +class DPAdaptBacksControllerNode : public rclcpp::Node { + public: + explicit DPAdaptBacksControllerNode( + const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); + + private: + // @brief Client for the GetOperationMode service + rclcpp::Client::SharedPtr + get_operation_mode_client_; + + // @brief Callback function for the killswitch topic + // @param msg: Bool message containing the killswitch status + void killswitch_callback(const std_msgs::msg::Bool::SharedPtr msg); + + // @brief Callback function for the software mode topic + // @param msg: String message containing the software mode + void operation_mode_callback( + const vortex_msgs::msg::OperationMode::SharedPtr msg); + + // @brief Callback function for the pose topic + // @param msg: PoseWithCovarianceStamped message containing the AUV pose + void pose_callback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); + + // @brief Callback function for the twist topic + // @param msg: TwistWithCovarianceStamped message containing the AUV speed + void twist_callback( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); + + // @brief Callback function for the control input tau publish + void publish_tau(); + + // @brief set the DP Adaptive Backstepping controller parameters + void set_adap_params(); + + // @brief Set the subscriber and publisher for the node + void set_subscribers_and_publisher(); + + // @brief Initialize the operation mode by calling the GetOperationMode + // service + void initialize_operation_mode(); + + // @brief Callback function for the guidance topic + // @param msg: ReferenceFilter message containing the desired vehicle pose + // and velocity + void guidance_callback( + const vortex_msgs::msg::ReferenceFilter::SharedPtr msg); + + rclcpp::Subscription::SharedPtr killswitch_sub_{}; + + rclcpp::Subscription::SharedPtr + operation_mode_sub_{}; + + rclcpp::Subscription< + geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_{}; + + rclcpp::Subscription< + geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr twist_sub_{}; + + rclcpp::Subscription::SharedPtr + guidance_sub_{}; + + rclcpp::Publisher::SharedPtr tau_pub_{}; + + rclcpp::TimerBase::SharedPtr tau_pub_timer_{}; + + std::chrono::milliseconds time_step_{}; + + vortex::utils::types::PoseEuler pose_; + + vortex::utils::types::PoseEuler pose_d_; + + vortex::utils::types::Twist twist_; + + std::unique_ptr dp_adapt_backs_controller_{}; + + bool killswitch_on_{true}; + + vortex::utils::types::Mode operation_mode_{ + vortex::utils::types::Mode::manual}; +}; + +} // namespace vortex::control + +#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 new file mode 100644 index 000000000..7a0e214a9 --- /dev/null +++ b/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_utils.hpp @@ -0,0 +1,57 @@ +#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" +#include "typedefs.hpp" + +namespace vortex::control { + +// @brief Calculate the derivative of the rotation matrix +// @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::PoseEuler& pose, + const vortex::utils::types::Twist& twist); + +// @brief Calculate the derivative of the transformation matrix +// @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::PoseEuler& pose, + const vortex::utils::types::Twist& twist); + +// @brief Calculate the pseudo-inverse of the Jacobian matrix +// @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::PoseEuler& pose); + +// @brief calculate the derivative of the Jacobian matrix +// @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 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::Twist& twist, + const Eigen::Matrix3d& I_b); + +// @brief Calculate the damping matrix for the adaptive backstepping controller +// @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::Twist& twist); + +} // namespace vortex::control + +#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 new file mode 100644 index 000000000..49f908072 --- /dev/null +++ b/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/typedefs.hpp @@ -0,0 +1,22 @@ +/** + * @file typedefs.hpp + * @brief Contains the Eigen typedefs for the controller. + */ + +#ifndef DP_ADAPT_BACKS_CONTROLLER__TYPEDEFS_HPP_ +#define DP_ADAPT_BACKS_CONTROLLER__TYPEDEFS_HPP_ + +#include + +namespace Eigen { + +typedef Eigen::Matrix Vector6d; +typedef Eigen::Matrix Vector12d; +typedef Eigen::Matrix Matrix6d; +typedef Eigen::Matrix Matrix6x12d; +typedef Eigen::Matrix Matrix12x6d; +typedef Eigen::Matrix Matrix12d; + +} // namespace Eigen + +#endif // DP_ADAPT_BACKS_CONTROLLER__TYPEDEFS_HPP_ diff --git a/control/dp_adapt_backs_controller/launch/dp_adapt_backs_controller.launch.py b/control/dp_adapt_backs_controller/launch/dp_adapt_backs_controller.launch.py new file mode 100644 index 000000000..fe3972bf8 --- /dev/null +++ b/control/dp_adapt_backs_controller/launch/dp_adapt_backs_controller.launch.py @@ -0,0 +1,45 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import OpaqueFunction +from launch_ros.actions import Node + +from auv_setup.launch_arg_common import ( + declare_drone_and_namespace_args, + resolve_drone_and_namespace, +) + + +def launch_setup(context, *args, **kwargs): + drone, namespace = resolve_drone_and_namespace(context) + + adapt_params = os.path.join( + get_package_share_directory("dp_adapt_backs_controller"), + "config", + f"adapt_params_{drone}.yaml", + ) + + drone_params = os.path.join( + get_package_share_directory("auv_setup"), + "config", + "robots", + f"{drone}.yaml", + ) + + return [ + Node( + package="dp_adapt_backs_controller", + executable="dp_adapt_backs_controller_node", + name="dp_adapt_backs_controller_node", + namespace=namespace, + parameters=[adapt_params, drone_params], + output="screen", + ) + ] + + +def generate_launch_description(): + return LaunchDescription( + declare_drone_and_namespace_args() + [OpaqueFunction(function=launch_setup)] + ) diff --git a/control/dp_adapt_backs_controller/package.xml b/control/dp_adapt_backs_controller/package.xml new file mode 100644 index 000000000..c618f6e06 --- /dev/null +++ b/control/dp_adapt_backs_controller/package.xml @@ -0,0 +1,25 @@ + + + + dp_adapt_backs_controller + 1.0.0 + Adaptive backstepping controller for DP + talhanc + MIT + + ament_cmake + + rclcpp + geometry_msgs + nav_msgs + eigen + tf2 + vortex_msgs + vortex_utils + vortex_utils_ros + yaml-cpp + + + ament_cmake + + 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 new file mode 100644 index 000000000..e009d7f90 --- /dev/null +++ b/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller.cpp @@ -0,0 +1,73 @@ +#include "dp_adapt_backs_controller/dp_adapt_backs_controller.hpp" +#include +#include +#include +#include "dp_adapt_backs_controller/dp_adapt_backs_controller_utils.hpp" +#include "dp_adapt_backs_controller/typedefs.hpp" + +namespace vortex::control { + +using vortex::utils::types::PoseEuler; +using vortex::utils::types::Twist; + +DPAdaptBacksController::DPAdaptBacksController( + const DPAdaptParams& dp_adapt_params) + : K1_(dp_adapt_params.K1.asDiagonal().toDenseMatrix()), + K2_(dp_adapt_params.K2.asDiagonal().toDenseMatrix()), + r_b_bg_(dp_adapt_params.r_b_bg), + adapt_gain_(dp_adapt_params.adapt_param.asDiagonal().toDenseMatrix()), + d_gain_(dp_adapt_params.d_gain.asDiagonal().toDenseMatrix()), + adapt_param_(Eigen::Vector12d::Zero()), + d_est_(Eigen::Vector6d::Zero()), + I_b_(dp_adapt_params.I_b.asDiagonal().toDenseMatrix()), + mass_matrix_(dp_adapt_params.mass_matrix), + m_(dp_adapt_params.mass), + dt_(0.01) {} + +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_, 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 = twist.to_vector() - alpha; + Eigen::Vector6d alpha_dot = + ((J_inv * J_dot * J_inv) * K1_ * z_1) - + (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 * twist.to_vector()) - + (pose.as_j_matrix().transpose() * z_1) - (K2_ * z_2) - + F_est - d_est_; + + tau = tau.cwiseMax(-100.0).cwiseMin(100.0); + adapt_param_ += adapt_param_dot * dt_; + d_est_ += d_est_dot * dt_; + adapt_param_ = adapt_param_.cwiseMax(-10.0).cwiseMin(10.0); + d_est_ = d_est_.cwiseMax(-10.0).cwiseMin(10.0); + + return tau; +} + +void DPAdaptBacksController::reset_adap_param() { + adapt_param_.setZero(); +} + +void DPAdaptBacksController::reset_d_est() { + d_est_.setZero(); +} + +void DPAdaptBacksController::set_time_step(const double dt) { + dt_ = dt; +} + +} // namespace vortex::control 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 new file mode 100644 index 000000000..8e6c884ce --- /dev/null +++ b/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_ros.cpp @@ -0,0 +1,259 @@ +#include "dp_adapt_backs_controller/dp_adapt_backs_controller_ros.hpp" +#include +#include +#include +#include +#include +#include +#include +#include "dp_adapt_backs_controller/dp_adapt_backs_controller_utils.hpp" +#include "dp_adapt_backs_controller/typedefs.hpp" + +constexpr std::string_view start_message = R"( + ____ ____ ____ _ _ _ + | _ \| _ \ / ___|___ _ __ | |_ _ __ ___ | | | ___ _ __ + | | | | |_) | | | / _ \| '_ \| __| '__/ _ \| | |/ _ \ '__| + | |_| | __/ | |__| (_) | | | | |_| | | (_) | | | __/ | + |____/|_| \____\___/|_| |_|\__|_| \___/|_|_|\___|_| + +)"; + +namespace vortex::control { + +DPAdaptBacksControllerNode::DPAdaptBacksControllerNode( + const rclcpp::NodeOptions& options) + : Node("dp_adapt_backs_controller_node", options) { + time_step_ = std::chrono::milliseconds(10); + + set_subscribers_and_publisher(); + initialize_operation_mode(); + + tau_pub_timer_ = this->create_wall_timer( + time_step_, std::bind(&DPAdaptBacksControllerNode::publish_tau, this)); + set_adap_params(); + + spdlog::info(start_message); +} + +void DPAdaptBacksControllerNode::set_subscribers_and_publisher() { + const auto qos_sensor_data{ + vortex::utils::qos_profiles::sensor_data_profile(1)}; + const auto qos_reliable{vortex::utils::qos_profiles::reliable_profile(1)}; + + this->declare_parameter("topics.guidance.dp"); + std::string dp_reference_topic = + this->get_parameter("topics.guidance.dp").as_string(); + guidance_sub_ = + this->create_subscription( + dp_reference_topic, qos_sensor_data, + std::bind(&DPAdaptBacksControllerNode::guidance_callback, this, + std::placeholders::_1)); + + this->declare_parameter("topics.pose"); + std::string pose_topic = this->get_parameter("topics.pose").as_string(); + pose_sub_ = this->create_subscription< + geometry_msgs::msg::PoseWithCovarianceStamped>( + pose_topic, qos_sensor_data, + std::bind(&DPAdaptBacksControllerNode::pose_callback, this, + std::placeholders::_1)); + + this->declare_parameter("topics.twist"); + std::string twist_topic = this->get_parameter("topics.twist").as_string(); + twist_sub_ = this->create_subscription< + geometry_msgs::msg::TwistWithCovarianceStamped>( + twist_topic, qos_sensor_data, + std::bind(&DPAdaptBacksControllerNode::twist_callback, this, + std::placeholders::_1)); + + this->declare_parameter("topics.killswitch"); + std::string software_kill_switch_topic = + this->get_parameter("topics.killswitch").as_string(); + killswitch_sub_ = this->create_subscription( + software_kill_switch_topic, qos_reliable, + std::bind(&DPAdaptBacksControllerNode::killswitch_callback, this, + std::placeholders::_1)); + + this->declare_parameter("topics.operation_mode"); + std::string software_operation_mode_topic = + this->get_parameter("topics.operation_mode").as_string(); + operation_mode_sub_ = + this->create_subscription( + software_operation_mode_topic, qos_reliable, + std::bind(&DPAdaptBacksControllerNode::operation_mode_callback, + this, std::placeholders::_1)); + + this->declare_parameter("topics.wrench_input"); + std::string control_topic = + this->get_parameter("topics.wrench_input").as_string(); + tau_pub_ = this->create_publisher( + control_topic, qos_sensor_data); +} + +void DPAdaptBacksControllerNode::initialize_operation_mode() { + this->declare_parameter("services.get_operation_mode"); + std::string get_operation_mode_service = + this->get_parameter("services.get_operation_mode").as_string(); + + get_operation_mode_client_ = + this->create_client( + get_operation_mode_service); + + while (!get_operation_mode_client_->wait_for_service( + std::chrono::seconds(1))) { + spdlog::warn("Waiting for GetOperationMode service to be available..."); + } + + auto request = + std::make_shared(); + get_operation_mode_client_->async_send_request( + request, + [this](rclcpp::Client::SharedFuture + future) { + try { + auto response = future.get(); + operation_mode_ = + vortex::utils::ros_conversions::convert_from_ros( + response->current_operation_mode); + killswitch_on_ = response->killswitch_status; + spdlog::info( + "Initial operation mode: {} | Killswitch status: {}", + vortex::utils::types::mode_to_string(operation_mode_), + killswitch_on_ ? "on" : "off"); + } catch (const std::exception& e) { + spdlog::error("Failed to get initial operation mode: {}", + e.what()); + killswitch_on_ = true; + } + }); +} + +void DPAdaptBacksControllerNode::killswitch_callback( + const std_msgs::msg::Bool::SharedPtr msg) { + killswitch_on_ = msg->data; + dp_adapt_backs_controller_->reset_adap_param(); + dp_adapt_backs_controller_->reset_d_est(); + spdlog::info("Killswitch: {}", killswitch_on_ ? "on" : "off"); +} + +void DPAdaptBacksControllerNode::operation_mode_callback( + const vortex_msgs::msg::OperationMode::SharedPtr msg) { + operation_mode_ = vortex::utils::ros_conversions::convert_from_ros(*msg); + spdlog::info("Operation mode: {}", + vortex::utils::types::mode_to_string(operation_mode_)); + + if (operation_mode_ == vortex::utils::types::Mode::autonomous || + operation_mode_ == vortex::utils::types::Mode::reference) { + pose_d_ = pose_; + } +} + +void DPAdaptBacksControllerNode::pose_callback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { + 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); + 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) { + 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() { + this->declare_parameter>("adapt_gain"); + this->declare_parameter>("d_gain"); + this->declare_parameter>("K1"); + this->declare_parameter>("K2"); + this->declare_parameter>("r_b_bg"); + this->declare_parameter>("physical.mass_matrix"); + + std::vector adapt_param_vec = + this->get_parameter("adapt_gain").as_double_array(); + std::vector d_gain_vec = + this->get_parameter("d_gain").as_double_array(); + std::vector K1_vec = this->get_parameter("K1").as_double_array(); + std::vector K2_vec = this->get_parameter("K2").as_double_array(); + std::vector r_b_bg_vec = + this->get_parameter("r_b_bg").as_double_array(); + std::vector mass_matrix_vec = + this->get_parameter("physical.mass_matrix").as_double_array(); + + Eigen::Vector12d adapt_param_eigen = + Eigen::Map(adapt_param_vec.data()); + Eigen::Vector6d d_gain_eigen = + Eigen::Map(d_gain_vec.data()); + Eigen::Vector6d K1_eigen = Eigen::Map(K1_vec.data()); + Eigen::Vector6d K2_eigen = Eigen::Map(K2_vec.data()); + Eigen::Vector3d r_b_bg_eigen = + Eigen::Map(r_b_bg_vec.data()); + Eigen::Matrix6d mass_matrix = + Eigen::Map(mass_matrix_vec.data()); + + double mass = mass_matrix(0, 0); + Eigen::Vector3d I_b_eigen(mass_matrix(3, 3), mass_matrix(4, 4), + mass_matrix(5, 5)); + + DPAdaptParams dp_adapt_params; + dp_adapt_params.adapt_param = adapt_param_eigen; + dp_adapt_params.d_gain = d_gain_eigen; + dp_adapt_params.K1 = K1_eigen; + dp_adapt_params.K2 = K2_eigen; + dp_adapt_params.r_b_bg = r_b_bg_eigen; + dp_adapt_params.I_b = I_b_eigen; + dp_adapt_params.mass_matrix = mass_matrix; + dp_adapt_params.mass = mass; + + dp_adapt_backs_controller_ = + std::make_unique(dp_adapt_params); + ; +} + +void DPAdaptBacksControllerNode::publish_tau() { + if (killswitch_on_ || + operation_mode_ == vortex::utils::types::Mode::manual) { + return; + } + + Eigen::Vector6d tau = + dp_adapt_backs_controller_->calculate_tau(pose_, pose_d_, twist_); + + geometry_msgs::msg::WrenchStamped tau_msg; + tau_msg.header.stamp = this->now(); + tau_msg.header.frame_id = "base_link"; + tau_msg.wrench.force.x = tau(0); + tau_msg.wrench.force.y = tau(1); + tau_msg.wrench.force.z = tau(2); + + // comment out if roll control is not needed + tau_msg.wrench.torque.x = tau(3); + + tau_msg.wrench.torque.y = tau(4); + tau_msg.wrench.torque.z = tau(5); + + tau_pub_->publish(tau_msg); +} + +void DPAdaptBacksControllerNode::guidance_callback( + const vortex_msgs::msg::ReferenceFilter::SharedPtr msg) { + 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) + +} // namespace vortex::control 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 new file mode 100644 index 000000000..116326a6b --- /dev/null +++ b/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_utils.cpp @@ -0,0 +1,121 @@ +#include "dp_adapt_backs_controller/dp_adapt_backs_controller_utils.hpp" +#include +#include +#include +#include "dp_adapt_backs_controller/typedefs.hpp" + +namespace vortex::control { + +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 is singular"); + + // Moore-Penrose pseudoinverse in case of near singular matrix, better + // result for smaller singular values + return J.completeOrthogonalDecomposition().pseudoInverse(); + } + + return J.inverse(); +} + +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( + twist.to_vector().tail(3)); +} + +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 pose_dot = pose.as_j_matrix() * twist.to_vector(); + + 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, + -sin_phi * tan_theta * phi_dot, 0.0, -sin_phi * phi_dot, + -cos_phi * phi_dot, 0.0, (cos_phi * phi_dot) / cos_theta, + (-sin_phi * phi_dot) / cos_theta; + + Eigen::Matrix3d dt_dtheta; + dt_dtheta << 0.0, sin_phi * inv_cos2 * theta_dot, + cos_phi * inv_cos2 * theta_dot, 0.0, 0.0, 0.0, 0.0, + (sin_phi * sin_theta) * inv_cos2 * theta_dot, + (cos_phi * sin_theta) * inv_cos2 * theta_dot; + + return dt_dphi + dt_dtheta; +} + +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; + J_dot.bottomRightCorner<3, 3>() = T_dot; + + return J_dot; +} + +Eigen::Matrix6d calculate_coriolis(const double mass, + const Eigen::Vector3d& r_b_bg, + const vortex::utils::types::Twist& twist, + const Eigen::Matrix3d& I_b) { + using vortex::utils::math::get_skew_symmetric_matrix; + 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); + C.topRightCorner<3, 3>() = -mass * + get_skew_symmetric_matrix(angular_speed) * + get_skew_symmetric_matrix(r_b_bg); + C.bottomLeftCorner<3, 3>() = mass * + get_skew_symmetric_matrix(angular_speed) * + get_skew_symmetric_matrix(r_b_bg); + ; + C.bottomRightCorner<3, 3>() = + get_skew_symmetric_matrix(I_b * angular_speed); + + return C; +} + +Eigen::Matrix6x12d calculate_Y_v(const vortex::utils::types::Twist& twist) { + Eigen::Matrix6x12d Y_v; + Y_v.setZero(); + + Y_v(0, 0) = twist.u; + Y_v(0, 1) = twist.u * std::abs(twist.u); + + Y_v(1, 2) = twist.v; + Y_v(1, 3) = twist.v * std::abs(twist.v); + + Y_v(2, 4) = twist.w; + Y_v(2, 5) = twist.w * std::abs(twist.w); + + Y_v(3, 6) = twist.p; + Y_v(3, 7) = twist.p * std::abs(twist.p); + + Y_v(4, 8) = twist.q; + Y_v(4, 9) = twist.q * std::abs(twist.q); + + Y_v(5, 10) = twist.r; + Y_v(5, 11) = twist.r * std::abs(twist.r); + + return Y_v; +} + +} // namespace vortex::control diff --git a/control/dp_adapt_backs_controller/test/CMakeLists.txt b/control/dp_adapt_backs_controller/test/CMakeLists.txt new file mode 100644 index 000000000..5b97abfbf --- /dev/null +++ b/control/dp_adapt_backs_controller/test/CMakeLists.txt @@ -0,0 +1,28 @@ +cmake_minimum_required(VERSION 3.8) + +find_package(GTest REQUIRED) +find_package(yaml-cpp REQUIRED) +include(GoogleTest) + +set(TEST_BINARY_NAME ${PROJECT_NAME}_test) +add_executable( + ${TEST_BINARY_NAME} + dp_adapt_backs_controller_tests.cpp +) + +target_compile_definitions(${TEST_BINARY_NAME} PRIVATE + DRONE_YAML_PATH="${CMAKE_CURRENT_SOURCE_DIR}/../../../auv_setup/config/robots/nautilus.yaml" + CONTROLLER_YAML_PATH="${CMAKE_CURRENT_SOURCE_DIR}/../config/adapt_params_nautilus.yaml" +) + +target_link_libraries( + ${TEST_BINARY_NAME} + PRIVATE + ${LIB_NAME} + yaml-cpp + GTest::GTest +) + +ament_target_dependencies(${TEST_BINARY_NAME} PUBLIC Eigen3 tf2) + +gtest_discover_tests(${TEST_BINARY_NAME}) 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 new file mode 100644 index 000000000..01232bdd3 --- /dev/null +++ b/control/dp_adapt_backs_controller/test/dp_adapt_backs_controller_tests.cpp @@ -0,0 +1,474 @@ +#include +#include + +#include + +#include "dp_adapt_backs_controller/dp_adapt_backs_controller.hpp" +#include "dp_adapt_backs_controller/dp_adapt_backs_controller_utils.hpp" +#include "dp_adapt_backs_controller/typedefs.hpp" + +namespace vortex::control { + +using vortex::utils::types::PoseEuler; +using vortex::utils::types::Twist; + +DPAdaptParams load_dp_adapt_params(const std::string& drone_yaml_path, + const std::string& controller_yaml_path) { + YAML::Node drone_params = + YAML::LoadFile(drone_yaml_path)["/**"]["ros__parameters"]; + YAML::Node controller_params = + YAML::LoadFile(controller_yaml_path)["/**"]["ros__parameters"]; + + auto K1_vec = controller_params["K1"].as>(); + auto K2_vec = controller_params["K2"].as>(); + auto adapt_gain_vec = + controller_params["adapt_gain"].as>(); + auto d_gain_vec = controller_params["d_gain"].as>(); + auto r_b_bg_vec = controller_params["r_b_bg"].as>(); + + auto mass_matrix_vec = + drone_params["physical"]["mass_matrix"].as>(); + + Eigen::Matrix6d mass_matrix = + Eigen::Map(mass_matrix_vec.data()); + + DPAdaptParams params; + params.K1 = Eigen::Map(K1_vec.data()); + params.K2 = Eigen::Map(K2_vec.data()); + params.adapt_param = Eigen::Map(adapt_gain_vec.data()); + params.d_gain = Eigen::Map(d_gain_vec.data()); + params.r_b_bg = Eigen::Map(r_b_bg_vec.data()); + params.mass_matrix = mass_matrix; + params.mass = mass_matrix(0, 0); + params.I_b = Eigen::Vector3d(mass_matrix(3, 3), mass_matrix(4, 4), + mass_matrix(5, 5)); + return params; +} + +class DPAdaptBacksControllerTests : public ::testing::Test { + protected: + DPAdaptBacksControllerTests() + : dp_adapt_backs_controller_{ + load_dp_adapt_params(DRONE_YAML_PATH, CONTROLLER_YAML_PATH)} {} + + 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}; + } + + 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}; + } + + 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}; + } + + DPAdaptBacksController dp_adapt_backs_controller_; +}; + +/* +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) { + 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(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); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative north error with positive heading gives a positive surge +command and negative sway command. +*/ + +TEST_F( + DPAdaptBacksControllerTests, + T02_neg_north_error_with_positive_heading_gives_pos_surge_and_neg_sway_command) { + 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(pose, pose_d, twist)}; + EXPECT_GT(tau[0], 0.0); + EXPECT_LT(tau[1], 0.0); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative north error with negative heading gives a positive surge +command and positive sway command. +*/ + +TEST_F( + DPAdaptBacksControllerTests, + T03_neg_north_error_with_negative_heading_gives_pos_surge_and_pos_sway_command) { + 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(pose, pose_d, twist)}; + EXPECT_GT(tau[0], 0.0); + EXPECT_GT(tau[1], 0.0); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative down error with zero roll and pitch gives a positive heave +command. +*/ + +TEST_F( + DPAdaptBacksControllerTests, + T04_neg_down_error_with_zero_roll_and_pitch_gives_positive_heave_command) { + 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(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); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative down error with zero roll and negative pitch gives a positive +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) { + 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(pose, pose_d, twist)}; + EXPECT_GT(tau[0], 0.0); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_GT(tau[2], 0.0); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative down error with zero roll and positive pitch gives a positive +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) { + 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(pose, pose_d, twist)}; + EXPECT_LT(tau[0], 0.0); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_GT(tau[2], 0.0); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +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) { + 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(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); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +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) { + 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(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); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative east error with positive heading gives a positive surge and +sway command. +*/ + +TEST_F( + DPAdaptBacksControllerTests, + T09_neg_east_error_with_positive_heading_gives_pos_sway_and_pos_surge_command) { + 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(pose, pose_d, twist)}; + EXPECT_GT(tau[0], 0.0); + EXPECT_GT(tau[1], 0.0); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative east error with negative heading gives a negative surge and +positive sway command. +*/ + +TEST_F( + DPAdaptBacksControllerTests, + T10_neg_east_error_with_negative_heading_gives_pos_sway_and_neg_surge_command) { + 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(pose, pose_d, twist)}; + EXPECT_LT(tau[0], 0.0); + EXPECT_GT(tau[1], 0.0); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative roll error gives positive roll command. +*/ + +TEST_F(DPAdaptBacksControllerTests, + T11_neg_roll_error_gives_positive_roll_command) { + 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(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); + EXPECT_GT(tau[3], 0.0); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that positive roll error gives negative roll command. +*/ + +TEST_F(DPAdaptBacksControllerTests, T12_pos_roll_error_gives_neg_roll_command) { + 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(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); + EXPECT_LT(tau[3], 0.0); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative pitch error gives positive pitch command. +*/ + +TEST_F(DPAdaptBacksControllerTests, + T13_neg_pitch_error_gives_pos_pitch_command) { + 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(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); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_GT(tau[4], 0.0); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that positive pitch error gives negative pitch command. +*/ + +TEST_F(DPAdaptBacksControllerTests, + T14_pos_pitch_error_gives_neg_pitch_command) { + 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(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); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_LT(tau[4], 0.0); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative yaw error gives positive yaw command. +*/ + +TEST_F(DPAdaptBacksControllerTests, T15_neg_yaw_error_gives_pos_yaw_command) { + 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(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); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_GT(tau[5], 0.0); +} + +/* +Test that positive yaw error gives negative yaw command. +*/ + +TEST_F(DPAdaptBacksControllerTests, T16_pos_yaw_error_gives_neg_yaw_command) { + 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(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); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_LT(tau[5], 0.0); +} + +/* +Test that positive surge velocity only results in negative surge command +(breaking effect). +*/ + +TEST_F(DPAdaptBacksControllerTests, + T17_pos_surge_vel_gives_negative_surge_command) { + 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(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); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); + // Torque channels may have cross-coupling from off-diagonal mass matrix + // terms in the Coriolis matrix +} + +/* +Test that positive sway velocity only results in negative sway command (breaking +effect). +*/ + +TEST_F(DPAdaptBacksControllerTests, + T18_pos_sway_vel_gives_negative_sway_command) { + 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(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); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); + // Torque channels may have cross-coupling from off-diagonal mass matrix + // terms in the Coriolis matrix +} + +/* +Test that positive heave velocity only results in negative heave command +(breaking effect). +*/ + +TEST_F(DPAdaptBacksControllerTests, + T19_pos_heave_vel_gives_negative_heave_command) { + 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(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); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); + // Torque channels may have cross-coupling from off-diagonal mass matrix + // terms in the Coriolis matrix +} + +} // namespace vortex::control + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} From 72cf9210f142cacd0a8ba16a1c14b3f7a35b311d Mon Sep 17 00:00:00 2001 From: Cyprian Osinski Date: Tue, 31 Mar 2026 14:34:18 +0200 Subject: [PATCH 254/290] started implementing quat dp --- .../config/adapt_params_nautilus.yaml | 1 + .../config/adapt_params_orca.yaml | 1 + .../dp_adapt_backs_controller.hpp | 8 +- .../dp_adapt_backs_controller_ros.hpp | 6 +- .../dp_adapt_backs_controller_utils.hpp | 24 +++--- .../src/dp_adapt_backs_controller.cpp | 20 +++-- .../src/dp_adapt_backs_controller_ros.cpp | 84 +++++++++++++------ .../src/dp_adapt_backs_controller_utils.cpp | 4 +- .../scripts/velocity_controller_lqr_node.py | 5 +- 9 files changed, 93 insertions(+), 60 deletions(-) diff --git a/control/dp_adapt_backs_controller_quat/config/adapt_params_nautilus.yaml b/control/dp_adapt_backs_controller_quat/config/adapt_params_nautilus.yaml index 65b639ed7..011a84d5c 100644 --- a/control/dp_adapt_backs_controller_quat/config/adapt_params_nautilus.yaml +++ b/control/dp_adapt_backs_controller_quat/config/adapt_params_nautilus.yaml @@ -5,3 +5,4 @@ r_b_bg : [0.0, 0.0, 0.01] # Vector from body centre to centre of gravity adapt_gain : [0.6, 0.15, 0.6, 0.15, 0.6, 0.15, 0.6, 0.15, 0.6, 0.15, 0.6, 0.15] # Tuning parameters for linear and nonlinear damping d_gain : [0.8, 0.8, 0.8, 0.8, 0.8, 0.8] # Tuning parameters for the unmodeled disturbances and uncertanties + timestep: 10 #Controller timestep in milliseconds (ms) diff --git a/control/dp_adapt_backs_controller_quat/config/adapt_params_orca.yaml b/control/dp_adapt_backs_controller_quat/config/adapt_params_orca.yaml index e128131b2..4f14e19ee 100644 --- a/control/dp_adapt_backs_controller_quat/config/adapt_params_orca.yaml +++ b/control/dp_adapt_backs_controller_quat/config/adapt_params_orca.yaml @@ -5,3 +5,4 @@ r_b_bg : [0.01, 0.0, 0.02] # Vector from body centre to centre of gravity adapt_gain : [0.4, 0.1, 0.4, 0.1, 0.4, 0.1, 0.4, 0.1, 0.4, 0.1, 0.4, 0.1] # Tuning parameters for linear and nonlinear damping d_gain : [0.6, 0.6, 0.6, 0.6, 0.6, 0.6] # Tuning parameters for the unmodeled disturbances and uncertanties + timestep: 10 #Controller timestep in milliseconds (ms) diff --git a/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller.hpp b/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller.hpp index d1b85e559..b4e316b55 100644 --- a/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller.hpp +++ b/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller.hpp @@ -13,8 +13,8 @@ struct DPAdaptParams { Eigen::Vector6d K1 = Eigen::Vector6d::Zero(); Eigen::Vector6d K2 = Eigen::Vector6d::Zero(); Eigen::Vector3d r_b_bg = Eigen::Vector3d::Zero(); - Eigen::Vector3d I_b = Eigen::Vector3d::Zero(); - Eigen::Matrix6d mass_matrix = Eigen::Matrix6d::Zero(); + Eigen::Vector3d inertia_matrix_body = Eigen::Vector3d::Zero(); + Eigen::Matrix6d mass_intertia_matrix = Eigen::Matrix6d::Zero(); double mass{}; }; @@ -54,8 +54,8 @@ class DPAdaptBacksController { Eigen::Matrix6d d_gain_; Eigen::Vector12d adapt_param_; Eigen::Vector6d d_est_; - Eigen::Matrix3d I_b_; - Eigen::Matrix6d mass_matrix_; + Eigen::Matrix3d inertia_matrix_body_; + Eigen::Matrix6d mass_intertia_matrix_; double m_{}; double dt_{}; }; diff --git a/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller_ros.hpp b/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller_ros.hpp index b02e364c5..4f330070e 100644 --- a/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller_ros.hpp +++ b/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller_ros.hpp @@ -83,7 +83,7 @@ class DPAdaptBacksControllerNode : public rclcpp::Node { rclcpp::Subscription< geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr twist_sub_{}; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr guidance_sub_{}; rclcpp::Publisher::SharedPtr tau_pub_{}; @@ -92,9 +92,9 @@ class DPAdaptBacksControllerNode : public rclcpp::Node { std::chrono::milliseconds time_step_{}; - vortex::utils::types::PoseEuler pose_; + vortex::utils::types::Pose pose_; - vortex::utils::types::PoseEuler pose_d_; + vortex::utils::types::Pose pose_d_; vortex::utils::types::Twist twist_; diff --git a/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller_utils.hpp b/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller_utils.hpp index 9848473d1..c753f9d39 100644 --- a/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller_utils.hpp +++ b/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller_utils.hpp @@ -10,44 +10,40 @@ namespace vortex::control { // @brief Calculate the derivative of the rotation matrix -// @param pose: 6D vector containing the vehicle pose [x, y, z, roll, pitch, -// yaw] +// @param pose: 7D vector containing the vehicle pose [x, y, z, qw, qx, qy, qz] // @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::PoseEuler& pose, +Eigen::Matrix3d calculate_R_dot(const vortex::utils::types::Pose& pose, const vortex::utils::types::Twist& twist); // @brief Calculate the derivative of the transformation matrix -// @param pose: 6D vector containing the vehicle pose [x, y, z, roll, pitch, -// yaw] +// @param pose: 6D vector containing the vehicle pose [x, y, z, qw, qx, qy, qz] // @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::PoseEuler& pose, +Eigen::Matrix3d calculate_T_dot(const vortex::utils::types::Pose& pose, const vortex::utils::types::Twist& twist); // @brief Calculate the pseudo-inverse of the Jacobian matrix -// @param pose: 6D vector containing the vehicle pose [x, y, z, roll, pitch, -// yaw] +// @param pose: 7D vector containing the vehicle pose [x, y, z, qw, qx, qy, qz] // @return 6x6 pseudo-inverse Jacobian matrix -Eigen::Matrix6d calculate_J_inv(const vortex::utils::types::PoseEuler& pose); +Eigen::Matrix6d calculate_J_inv(const vortex::utils::types::Pose& pose); // @brief calculate the derivative of the Jacobian matrix -// @param pose: 6D vector containing the vehicle pose [x, y, z, roll, pitch, -// yaw] +// @param pose: 7D vector containing the vehicle pose [x, y, z, qw, qx, qy, qz] // @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, +Eigen::Matrix6d calculate_J_dot(const vortex::utils::types::Pose& 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 twist: 6D vector containing linear and angular velocity of the vehicle -// @param I_b : 3D matrix containing the inertia matrix +// @param inertia_matrix_body : 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::Twist& twist, - const Eigen::Matrix3d& I_b); + const Eigen::Matrix3d& inertia_matrix_body); // @brief Calculate the damping matrix for the adaptive backstepping controller // @param twist: 6D vector containing the vehicle velocity [u, v, w, p, q, r] diff --git a/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller.cpp b/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller.cpp index 353e1f29f..b884eb514 100644 --- a/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller.cpp +++ b/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller.cpp @@ -19,20 +19,24 @@ DPAdaptBacksController::DPAdaptBacksController( d_gain_(dp_adapt_params.d_gain.asDiagonal().toDenseMatrix()), adapt_param_(Eigen::Vector12d::Zero()), d_est_(Eigen::Vector6d::Zero()), - I_b_(dp_adapt_params.I_b.asDiagonal().toDenseMatrix()), - mass_matrix_(dp_adapt_params.mass_matrix), + inertia_matrix_body_( + dp_adapt_params.inertia_matrix_body.asDiagonal().toDenseMatrix()), + mass_intertia_matrix_(dp_adapt_params.mass_intertia_matrix), m_(dp_adapt_params.mass), dt_(0.01) {} -Eigen::Vector6d DPAdaptBacksController::calculate_tau(const PoseEuler& pose, - const PoseEuler& pose_d, +Eigen::Vector6d DPAdaptBacksController::calculate_tau(const Pose& pose, + const Pose& pose_d, const Twist& twist) { + // TODO: implement error state calculation. Maybe look at hybrid + // switching between pure RPY and error state when error is big 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_, twist, I_b_); + Eigen::Matrix6d C = + calculate_coriolis(m_, r_b_bg_, twist, inertia_matrix_body_); 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(); @@ -45,9 +49,9 @@ Eigen::Vector6d DPAdaptBacksController::calculate_tau(const PoseEuler& pose, 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 * twist.to_vector()) - - (pose.as_j_matrix().transpose() * z_1) - (K2_ * z_2) - - F_est - d_est_; + Eigen::Vector6d tau = + (mass_intertia_matrix_ * alpha_dot) + (C * twist.to_vector()) - + (pose.as_j_matrix().transpose() * z_1) - (K2_ * z_2) - F_est - d_est_; // TODO: look at better ways to clamp tau w.r.t new thrusters and allocator tau = tau.cwiseMax(-100.0).cwiseMin(100.0); diff --git a/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_ros.cpp b/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_ros.cpp index 5890acc2f..0df8cf426 100644 --- a/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_ros.cpp +++ b/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_ros.cpp @@ -10,12 +10,14 @@ #include "dp_adapt_backs_controller_quat/typedefs.hpp" constexpr std::string_view start_message = R"( - ____ ____ ____ _ _ _ - | _ \| _ \ / ___|___ _ __ | |_ _ __ ___ | | | ___ _ __ - | | | | |_) | | | / _ \| '_ \| __| '__/ _ \| | |/ _ \ '__| - | |_| | __/ | |__| (_) | | | | |_| | | (_) | | | __/ | - |____/|_| \____\___/|_| |_|\__|_| \___/|_|_|\___|_| - + /$$$$$$$ /$$$$$$$ /$$$$$$ /$$ /$$$$$$ /$$ /$$ /$$ +| $$__ $$| $$__ $$ /$$__ $$ | $$ /$$__ $$ | $$ | $$| $$ +| $$ \ $$| $$ \ $$ | $$ \ $$ /$$ /$$ /$$$$$$ /$$$$$$ | $$ \__/ /$$$$$$ /$$$$$$$ /$$$$$$ /$$$$$$ /$$$$$$ | $$| $$ /$$$$$$ /$$$$$$ +| $$ | $$| $$$$$$$/ | $$ | $$| $$ | $$ |____ $$|_ $$_/ | $$ /$$__ $$| $$__ $$|_ $$_/ /$$__ $$ /$$__ $$| $$| $$ /$$__ $$ /$$__ $$ +| $$ | $$| $$____/ | $$ | $$| $$ | $$ /$$$$$$$ | $$ | $$ | $$ \ $$| $$ \ $$ | $$ | $$ \__/| $$ \ $$| $$| $$| $$$$$$$$| $$ \__/ +| $$ | $$| $$ | $$/$$ $$| $$ | $$ /$$__ $$ | $$ /$$ | $$ $$| $$ | $$| $$ | $$ | $$ /$$| $$ | $$ | $$| $$| $$| $$_____/| $$ +| $$$$$$$/| $$ | $$$$$$/| $$$$$$/| $$$$$$$ | $$$$/ | $$$$$$/| $$$$$$/| $$ | $$ | $$$$/| $$ | $$$$$$/| $$| $$| $$$$$$$| $$ +|_______/ |__/ \____ $$$ \______/ \_______/ \___/ \______/ \______/ |__/ |__/ \___/ |__/ \______/ |__/|__/ \_______/|__/ )"; namespace vortex::control { @@ -23,7 +25,9 @@ namespace vortex::control { DPAdaptBacksControllerNode::DPAdaptBacksControllerNode( const rclcpp::NodeOptions& options) : Node("dp_adapt_backs_controller_node", options) { - time_step_ = std::chrono::milliseconds(10); + this->declare_parameter("time_step"); + int time_step = static_cast(this->get_parameter("time_step")); + time_step_ = std::chrono::milliseconds(time_step); set_subscribers_and_publisher(); initialize_operation_mode(); @@ -152,12 +156,12 @@ void DPAdaptBacksControllerNode::pose_callback( 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); - pose_.roll = euler_angles(0); - pose_.pitch = euler_angles(1); - pose_.yaw = euler_angles(2); + pose_.w = o.w; + pose_.qx = o.x; + pose_.qy = o.y; + pose_.qz = o.z; } void DPAdaptBacksControllerNode::twist_callback( @@ -176,7 +180,8 @@ void DPAdaptBacksControllerNode::set_adap_params() { this->declare_parameter>("K1"); this->declare_parameter>("K2"); this->declare_parameter>("r_b_bg"); - this->declare_parameter>("physical.mass_matrix"); + this->declare_parameter>( + "physical.mass_intertia_matrix"); std::vector adapt_param_vec = this->get_parameter("adapt_gain").as_double_array(); @@ -186,7 +191,7 @@ void DPAdaptBacksControllerNode::set_adap_params() { std::vector K2_vec = this->get_parameter("K2").as_double_array(); std::vector r_b_bg_vec = this->get_parameter("r_b_bg").as_double_array(); - std::vector mass_matrix_vec = + std::vector mass_intertia_matrix_vec = this->get_parameter("physical.mass_matrix").as_double_array(); Eigen::Vector12d adapt_param_eigen = @@ -197,12 +202,13 @@ void DPAdaptBacksControllerNode::set_adap_params() { Eigen::Vector6d K2_eigen = Eigen::Map(K2_vec.data()); Eigen::Vector3d r_b_bg_eigen = Eigen::Map(r_b_bg_vec.data()); - Eigen::Matrix6d mass_matrix = - Eigen::Map(mass_matrix_vec.data()); + Eigen::Matrix6d mass_intertia_matrix = + Eigen::Map(mass_intertia_matrix_vec.data()); - double mass = mass_matrix(0, 0); - Eigen::Vector3d I_b_eigen(mass_matrix(3, 3), mass_matrix(4, 4), - mass_matrix(5, 5)); + double mass = mass_intertia_matrix(0, 0); + Eigen::Vector3d inertia_matrix_body_eigen(mass_intertia_matrix(3, 3), + mass_intertia_matrix(4, 4), + mass_intertia_matrix(5, 5)); DPAdaptParams dp_adapt_params; dp_adapt_params.adapt_param = adapt_param_eigen; @@ -210,8 +216,8 @@ void DPAdaptBacksControllerNode::set_adap_params() { dp_adapt_params.K1 = K1_eigen; dp_adapt_params.K2 = K2_eigen; dp_adapt_params.r_b_bg = r_b_bg_eigen; - dp_adapt_params.I_b = I_b_eigen; - dp_adapt_params.mass_matrix = mass_matrix; + dp_adapt_params.inertia_matrix_body = inertia_matrix_body_eigen; + dp_adapt_params.mass_intertia_matrix = mass_intertia_matrix; dp_adapt_params.mass = mass; dp_adapt_backs_controller_ = @@ -237,7 +243,6 @@ void DPAdaptBacksControllerNode::publish_tau() { // comment out if roll control is not needed tau_msg.wrench.torque.x = tau(3); - tau_msg.wrench.torque.y = tau(4); tau_msg.wrench.torque.z = tau(5); @@ -245,15 +250,42 @@ void DPAdaptBacksControllerNode::publish_tau() { } void DPAdaptBacksControllerNode::guidance_callback( - const vortex_msgs::msg::ReferenceFilter::SharedPtr msg) { + const vortex_msgs::msg::ReferenceFilterQuat::SharedPtr msg) { 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; + + pose_d_.qw = msg->qw; + pose_d_.qx = msg->qx; + pose_d_.qy = msg->qy; + pose_d_.qz = msg->qz; } RCLCPP_COMPONENTS_REGISTER_NODE(DPAdaptBacksControllerNode) } // namespace vortex::control + +// ⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣀⣀⣠⡤⣦⢦⠖⡶⠲⢦⣤⢤⣤⣤⣀⢀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀ +// ⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣠⡴⢶⠛⡍⢎⠱⣈⠒⢌⡘⠰⠉⢆⠰⢉⠔⠢⡉⢝⢫⠳⢦⣄⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀ +// ⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣠⡶⢏⠳⡘⠢⢉⠔⡈⠔⠠⠈⠄⠠⠁⠌⡀⠂⠌⠠⠁⠌⡐⢂⠉⢆⠩⡝⢳⣦⣀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀ +// ⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣴⡞⢫⠔⡉⠆⡁⠢⢁⣂⡐⡈⠄⢁⠈⠄⣁⡤⠄⣁⣠⠁⡈⠄⠐⠠⠉⠠⠁⡌⢡⠊⡝⢷⣄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀ +// ⠀⠀⠀⠀⠀⠀⠀⠀⣠⡾⢣⠎⡡⢊⠴⣅⣶⣽⣷⣾⣿⣶⠌⠀⠄⢺⢾⣿⣿⣿⣷⣾⣿⣤⣧⡰⠈⠠⢁⠐⢂⠑⠌⡸⠸⣷⣤⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀ +// ⠀⠀⠀⠀⠀⠀⠀⣸⢯⣑⠣⣌⠱⢨⣿⣿⣿⣿⡿⠟⠋⠁⡀⠌⠐⠠⢁⠚⡹⢛⡿⢿⣿⣿⣿⠯⢀⠁⠂⠌⠠⢈⠂⣁⠣⡘⠽⣦⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀ +// ⠀⠀⠀⠀⠀⠀⢴⡟⡥⢊⠵⣠⠣⢍⠺⢟⠩⣁⠒⠨⢀⠂⠀⠄⠡⡁⢂⠠⠀⠄⢠⠉⠄⡊⢄⠂⡄⠊⠄⢃⠤⢁⠂⢄⠂⠥⡙⣚⢷⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀ +// ⠀⠀⠀⠀⠀⢀⡿⡜⠴⣉⠖⣡⢚⣬⢳⡉⠖⣀⠃⣁⠢⠐⡈⠠⢁⠒⡄⢂⠡⠊⠄⡘⠤⠑⡌⢒⠤⢃⠜⣀⠂⢆⠘⡠⠘⡀⢆⢡⢻⣧⠀⠀⠀⠀⠀⠀⠀⠀⠀ +// ⠀⠀⠀⠀⠀⢸⣛⣌⠳⡌⢎⡵⣫⣎⠧⡘⠡⠀⠂⢀⠂⠱⢠⢁⠊⡴⣘⠢⠁⠌⠀⠐⠠⢁⠘⡄⠫⡜⠲⣄⠩⢄⠢⠁⠆⠡⠌⡂⢎⡽⣆⠀⠀⠀⠀⠀⠀⠀⠀ +// ⠀⠀⠀⠀⠀⢸⡷⢌⠳⡌⢧⣿⢿⡜⣢⣱⣤⡧⣐⣢⡀⢃⢢⠁⢎⡶⣡⢆⣵⠴⠧⢦⣁⣂⠰⣈⠱⣘⠳⣬⡓⢌⠢⡉⢌⠡⡘⢄⢣⢚⣿⠀⠀⠀⠀⠀⠀⠀⠀ +// ⠀⠀⠀⠀⠀⢹⡞⣌⠳⡌⢧⣿⣿⣽⢷⣫⣔⡠⢀⢀⠈⠉⢲⣭⣰⣟⣯⣹⣔⣢⢡⠀⡀⠌⠙⠞⣳⠿⢷⣷⣝⠦⡑⠨⠄⡃⠔⡈⠦⣙⡾⡆⠀⠀⠀⠀⠀⠀⠀ +// ⠀⠀⠀⠀⠀⢸⡿⢤⠓⣌⢣⢻⣹⣿⣻⣿⣿⣿⣶⠢⣌⢡⠂⡴⢻⡿⣷⣿⣿⣿⣷⣳⠴⡈⡜⡰⣄⢻⣜⣿⢳⡡⢊⡑⢌⠰⠡⡘⡰⣡⢿⡇⠀⠀⠀⠀⠀⠀⠀ +// ⠀⠀⠀⠀⠀⠸⣟⢦⢋⡔⢣⡙⣷⡻⣿⣿⣿⣿⣿⣷⣎⢦⣙⢶⢏⠻⣿⣿⣿⣿⣿⡿⢧⡹⣴⢳⣽⣳⡿⢡⠗⡠⢃⠔⡈⢆⠱⣐⠱⡬⢿⡇⠀⠀⠀⠀⠀⠀⠀ +// ⠀⠀⠀⠀⠀⠀⢻⣎⠖⡬⢑⡜⢲⢻⣮⠻⣿⣿⣻⣷⣾⡻⢞⠧⢎⠛⣮⣛⠿⣽⣻⣽⣯⣿⣾⣿⠿⢋⡴⢋⡒⢡⠊⡔⣁⠊⡔⠤⣛⠼⣿⠀⠀⠀⠀⠀⠀⠀⠀ +// ⠀⠀⠀⠀⠀⠀⠘⢯⣞⠰⡡⠎⢥⢋⡞⡿⢶⡞⣭⡝⣲⡙⢎⡱⢊⡱⠄⣍⠛⢦⢋⠭⣉⡍⣡⠔⣎⠣⡜⡡⠘⡄⢣⠐⡄⢣⠘⢦⢭⣿⡟⠀⠀⠀⠀⠀⠀⠀⠀ +// ⠀⠀⠀⠀⠀⣠⣤⡸⣿⢢⡑⠎⣆⢣⠚⣭⢣⡛⡴⡙⢆⠹⢠⠑⠢⢄⠃⠄⣉⠂⠍⠒⡡⠜⠤⢋⠔⡃⢆⠡⢃⡘⢄⠣⡘⢤⢋⡼⣺⣿⠉⣀⣤⣄⠀⠀⠀⠀⠀ +// ⠀⠀⠀⠀⣤⡗⠀⣉⣻⣧⣙⠲⡐⢎⡱⢂⠇⣎⠱⢡⠊Life is pain,⡈⢂⠡⠌⡐⡈⠤⢁⠰⡈⢆⠱⣊⠼⣼⣿⣿⣛⣉⠐⠾⣄⠀⠀⠀⠀ +// ⠀⠀⠀⡼⠃⠀⠀⠙⣿⡘⢿⣵⠿⡼⣴⣩⣚Existence is meaningless⢢⣑⣎⡱⢬⣻⣿⠇⣸⡿⠁⠀⠀⠙⣆⠀⠀⠀ +// ⠀⠀⣰⡗⠀⠀⠀⠀⠘⣧⡀⠙⠻⣷⣾⣭⣿⣹⢏⡻⢩⢛⠛⡛⢛⠻⠛⠟⡛⠟⢛⠛⡛⠹⢛⠛⣙⡋⣏⣙⣩⣭⣭⣴⣼⣿⠟⠁⠈⣿⠁⠀⠀⠀⠀⢾⡄⠀⠀ +// ⠀⢠⠻⠀⢀⣠⣄⠀⠀⣻⠀⠀⠀⠀⠛⠿⣿⣿⣿⣷⣷⣮⣵⣌⣦⡱⣌⣲⡰⣌⣦⣱⣬⣷⣾⣿⣿⣿⡿⣿⢿⣿⣿⣿⠛⠁⠀⢀⢸⡇⠀⠀⣀⣀⠀⠘⡿⡀⠀ +// ⠀⣧⠇⠀⣴⠟⠈⠀⠀⣿⡄⠀⠀⠀⠀⠀⠈⠙⠻⢿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⢿⣻⢯⣷⣯⣿⣽⡿⠟⠉⠀⠀⠀⠀⠀⣘⣿⠀⢸⠉⠿⣄⠀⠰⣡⠀ +// ⠸⡌⠀⢹⡇⠀⠀⣏⠀⣽⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠉⠙⠛⠛⠿⠿⠿⢽⢿⡾⠷⠿⠿⠿⠟⠛⠋⠉⠀⠀⠀⠀⠀⠀⠀⠀⠀⢸⣇⠀⠨⠀⠀⢹⠇⠀⢏⠂ +// ⠀⡇⠀⣻⡆⠀⠀⠉⠒⠋⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠈⠓⠒⠃⠀⠀⢸⣟⠀⣸⠀ +// ⠀⠣⠴⠞⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠘⠷⠄⠆⠀ diff --git a/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_utils.cpp b/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_utils.cpp index 1922580bf..20b390238 100644 --- a/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_utils.cpp +++ b/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_utils.cpp @@ -75,7 +75,7 @@ Eigen::Matrix6d calculate_J_dot(const vortex::utils::types::PoseEuler& pose, Eigen::Matrix6d calculate_coriolis(const double mass, const Eigen::Vector3d& r_b_bg, const vortex::utils::types::Twist& twist, - const Eigen::Matrix3d& I_b) { + const Eigen::Matrix3d& inertia_matrix_body) { using vortex::utils::math::get_skew_symmetric_matrix; Eigen::Vector3d linear_speed = twist.to_vector().head(3); Eigen::Vector3d angular_speed = twist.to_vector().tail(3); @@ -90,7 +90,7 @@ Eigen::Matrix6d calculate_coriolis(const double mass, get_skew_symmetric_matrix(r_b_bg); ; C.bottomRightCorner<3, 3>() = - get_skew_symmetric_matrix(I_b * angular_speed); + get_skew_symmetric_matrix(inertia_matrix_body * angular_speed); return C; } 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 c2b7722c0..ccd36a0aa 100755 --- a/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py +++ b/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py @@ -305,13 +305,12 @@ def main(args=None): if __name__ == "__main__": main() - -# ⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀ +⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀ # ⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣀⣠⢴⣤⣠⣤⢤⣂⣔⣲⣒⣖⡺⢯⣝⡿⣿⣿⣿⣷⣶⣶⣢⢦⣤⣄⣀⣀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀ # ⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣰⣯⣿⣾⣿⣶⣺⣯⡿⣓⣽⢞⡸⣻⢏⣋⠌⣛⣭⣿⢟⣿⣛⣿⢷⣿⣿⣿⡟⣿⣻⣵⣲⢢⣄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀ # ⠀⠀⠀⠀⠀⠀⢀⣀⣤⡴⠲⠶⢦⠤⢤⡤⠿⠿⠿⠿⣿⣽⣿⣽⣷⣿⢽⣾⣷⣭⡞⣩⡐⠏⡋⣽⡬⣭⠏⢍⣞⢿⣽⣿⣷⢿⣿⣿⡿⠾⣿⢶⡶⣤⣀⠀⠀⠀⠀⠀⠀⠀⠀ # ⠀⠀⠀⠀⣤⣖⠯⡙⢢⡁⠄⢢⡤⢠⢀⣸⠀⣄⡠⣀⣀⣦⡄⣠⢀⡃⠽⣽⠬⠍⣿⣿⣤⣥⣷⣿⣿⣿⣾⡍⠛⢌⠈⢫⣍⢋⣍⡁⠹⢍⠈⣳⢎⠴⠟⠻⢧⣄⠀⠀⠀⠀⠀ -# ⠀⠀⣠⣾⣿⣿⣿⣯⡔⠆⠠⠈⣿⣿⠾⡾⠿⣶⠿⡟⣻⡛⣭⢙⠍⢩ANDERS ER GOATED⣤⣥⣩⣶⣟⣻⠧⣻⠶⢤⢰⡱⣬⣤⣌⣑⠞⠲⠓⠭⡀⠀⠀⠀ +# ⠀⠀⣠⣾⣿⣿⣿⣯⡔⠆⠠⠈⣿⣿⠾⡾⠿⣶⠿⡟⣻⡛⣭⢙⠍⢩ANDERS ER GOATED⣶⣟⣻⠧⣻⠶⢤⢰⡱⣬⣤⣌⣑⠞⠲⠓⠭⡀⠀⠀⠀ # ⠀⠐⣿⣿⣿⢟⡋⢈⢤⣤⣷⢿⣿⠒⢜⣁⡱⡧⢿⣹⣷⣿⡿⣷⠌⣢⣟⢱⢽⡨⢊⠴⡉⢉⡿⣯⢿⣏⢹⠏⣯⣩⣙⠾⢿⣳⣶⢻⣟⣿⠧⢀⠋⠟⣿⡧⠠⠄⡤⠈⢠⠀⠀ # ⠀⠀⠘⠻⠿⠶⠶⠿⠛⣹⣟⡞⠸⣬⠐⠙⢍⢉⣔⠪⠟⡛⠫⢵⣾⣣⣼⣽⢈⠔⡅⣜⡽⢯⢞⡕⡠⠓⣡⢚⣷⣷⣿⣳⡄⠢⠉⠛⢿⣲⢿⣶⢿⣬⣾⣛⠳⣼⡮⠳⡂⠒⠀ # ⠀⠀⠀⠀⠀⠀⠀⠀⢠⠏⡁⢉⣀⣑⣆⡐⠊⣅⡕⢦⣀⣱⡶⢫⣨⢟⠽⠕⣇⢶⣵⣋⢝⣉⣋⠜⠉⠉⡯⠛⣿⣿⣿⣾⣳⠠⠤⠪⠁⠊⠉⠻⣟⣾⣿⣿⣟⣧⣧⢸⠂⠠⢠ From d6eaa218e0ad3b20977cc3e3cda5a75c92b78a01 Mon Sep 17 00:00:00 2001 From: Ahmed Date: Tue, 31 Mar 2026 15:05:26 +0200 Subject: [PATCH 255/290] added the depth sensor, with some modifications related to the imu calib --- navigation/eskf/config/eskf_params.yaml | 5 +- .../eskf/config/eskf_params_real_world.yaml | 31 +++++ navigation/eskf/include/eskf/eskf.hpp | 4 +- navigation/eskf/include/eskf/eskf_ros.hpp | 36 +++-- navigation/eskf/include/eskf/typedefs.hpp | 9 ++ navigation/eskf/launch/eskf.launch.py | 45 ++++-- navigation/eskf/src/eskf.cpp | 45 ++++-- navigation/eskf/src/eskf_ros.cpp | 130 ++++++++++++++---- navigation/eskf/test/eskf_consistency_test.py | 104 +++++++------- .../odom_transformer/odom_transformer.hpp | 4 +- .../launch/odom_transformer.launch.py | 3 +- .../odom_transformer/src/odom_transformer.cpp | 33 ++--- 12 files changed, 321 insertions(+), 128 deletions(-) create mode 100644 navigation/eskf/config/eskf_params_real_world.yaml diff --git a/navigation/eskf/config/eskf_params.yaml b/navigation/eskf/config/eskf_params.yaml index 76dc53137..33bf2bd60 100644 --- a/navigation/eskf/config/eskf_params.yaml +++ b/navigation/eskf/config/eskf_params.yaml @@ -16,11 +16,12 @@ dvl_frame_t: [ 0.4, 0.0, 0.2 ] depth_frame_t: [ 0.0, 0.0, 0.0 ] - use_tf_transforms: true + use_tf_transforms: false publish_tf: true publish_pose: true publish_twist: true publish_rate_ms: 5 add_gravity_to_imu: false initial_gyro_bias: [0.0, 0.0, 0.0] - initial_accel_bias: [0.0, 0.0, 0.0] \ No newline at end of file + initial_accel_bias: [0.0, 0.0, 0.0] + publish_biases: false diff --git a/navigation/eskf/config/eskf_params_real_world.yaml b/navigation/eskf/config/eskf_params_real_world.yaml new file mode 100644 index 000000000..ed2455aa9 --- /dev/null +++ b/navigation/eskf/config/eskf_params_real_world.yaml @@ -0,0 +1,31 @@ +/**: + eskf_node: + ros__parameters: + diag_Q_std: [0.05, 0.05, 0.1, 0.01, 0.01, 0.02, 0.001, 0.001, 0.001, 0.0001, 0.0001, 0.0001] + diag_p_init: [1.0, 1.0, 1.0, 0.5, 0.5, 0.5, 0.1, 0.1, 0.1, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001] + transform: + imu_frame_r: [ 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0 ] + imu_frame_t: [ 0.0, 0.0, 0.0 ] + + dvl_frame_r: [ 0.0, -1.0, 0.0, + 1.0, 0.0, 0.0, + 0.0, 0.0, 1.0 ] + + dvl_frame_t: [ 0.4, 0.0, 0.2 ] + + depth_frame_t: [ 0.0, 0.0, 0.0 ] + use_tf_transforms: false + publish_tf: true + publish_pose: true + publish_twist: true + publish_rate_ms: 5 + add_gravity_to_imu: true + initial_gyro_bias: [0.0, 0.0, 0.0] + initial_accel_bias: [3.53e-2, 0.0, -0.0533] + publish_biases: true + +# USED SENSORS + # IMU: Kongsberg mini MRU + # DVL & Depth: Nucleus 1000 diff --git a/navigation/eskf/include/eskf/eskf.hpp b/navigation/eskf/include/eskf/eskf.hpp index 6b578ff7e..96d354bdc 100644 --- a/navigation/eskf/include/eskf/eskf.hpp +++ b/navigation/eskf/include/eskf/eskf.hpp @@ -19,6 +19,8 @@ class ESKF { // @param dvl_meas: DVL measurement void dvl_update(const SensorDVL& dvl_meas); + void depth_update(const SensorDepth& depth_meas); + inline StateQuat get_nominal_state() const { return current_nom_state_; } inline StateEuler get_error_state() const { return current_error_state_; } @@ -101,7 +103,7 @@ Eigen::Matrix3x15d calculate_h_jacobian(const StateQuat& current_nom_state_); // Jacobian of h(x) with respect to the nominal state --> Hx Eigen::Matrix3x16d calculate_hx(const StateQuat& current_nom_state_); -double compute_nis(const Eigen::Vector3d& innovation, const Eigen::Matrix3d& S); +double compute_nis(const Eigen::VectorXd& innovation, const Eigen::MatrixXd& S); #include "eskf.tpp" // including template implementation diff --git a/navigation/eskf/include/eskf/eskf_ros.hpp b/navigation/eskf/include/eskf/eskf_ros.hpp index db73c429f..03d4c7266 100644 --- a/navigation/eskf/include/eskf/eskf_ros.hpp +++ b/navigation/eskf/include/eskf/eskf_ros.hpp @@ -8,14 +8,17 @@ #include #include #include +#include #include #include #include +#include #include #include #include #include #include +#include #include #include "eskf/eskf.hpp" #include "eskf/typedefs.hpp" @@ -36,6 +39,8 @@ class ESKFNode : public rclcpp::Node { void dvl_callback( const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); + void depth_callback(const sensor_msgs::msg::FluidPressure::SharedPtr msg); + // @brief Publish the odometry message void publish_odom(); @@ -53,9 +58,8 @@ class ESKFNode : public rclcpp::Node { void complete_initialization(); // @brief broadcast the State as a TF - void publish_tf(const StateQuat& nom_state, const rclcpp::Time& current_time); - - // Startup message + void publish_tf(const StateQuat& nom_state, + const rclcpp::Time& current_time); // Subscribers and Publishers @@ -64,15 +68,25 @@ class ESKFNode : public rclcpp::Node { rclcpp::Subscription< geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr dvl_sub_; + rclcpp::Subscription::SharedPtr depth_sub_; + rclcpp::Publisher::SharedPtr odom_pub_; - rclcpp::Publisher< - geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_pub_; + rclcpp::Publisher::SharedPtr + pose_pub_; + + rclcpp::Publisher::SharedPtr + twist_pub_; - rclcpp::Publisher< - geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr twist_pub_; + rclcpp::Publisher::SharedPtr nis_dvl_pub_; - rclcpp::Publisher::SharedPtr nis_pub_; + rclcpp::Publisher::SharedPtr nis_depth_pub_; + + rclcpp::Publisher::SharedPtr + accel_bias_pub_; + + rclcpp::Publisher::SharedPtr + gyro_bias_pub_; // Member variable for the ESKF instance @@ -114,12 +128,18 @@ class ESKFNode : public rclcpp::Node { bool publish_tf_{false}; bool publish_pose_{false}; bool publish_twist_{false}; + bool publish_biases_{false}; bool add_gravity_to_imu_{false}; // hold the transfer from Sensor -> Base Link Eigen::Isometry3d Tf_base_imu_ = Eigen::Isometry3d::Identity(); Eigen::Isometry3d Tf_base_dvl_ = Eigen::Isometry3d::Identity(); Eigen::Isometry3d Tf_base_depth_ = Eigen::Isometry3d::Identity(); + + // gravity, water density and atmospheric pressure parameters + double gravity; + double water_density; + double atmospheric_pressure; }; #endif // ESKF__ESKF_ROS_HPP_ diff --git a/navigation/eskf/include/eskf/typedefs.hpp b/navigation/eskf/include/eskf/typedefs.hpp index 687dd1778..ee04e8911 100644 --- a/navigation/eskf/include/eskf/typedefs.hpp +++ b/navigation/eskf/include/eskf/typedefs.hpp @@ -98,6 +98,7 @@ struct StateEuler { struct EskfParams { Eigen::Matrix12d Q = Eigen::Matrix12d::Zero(); Eigen::Matrix15d P = Eigen::Matrix15d::Zero(); + Eigen::Vector3d g_{0.0, 0.0, -9.81}; Eigen::Vector3d initial_gyro_bias = Eigen::Vector3d::Zero(); Eigen::Vector3d initial_accel_bias = Eigen::Vector3d::Zero(); }; @@ -126,4 +127,12 @@ struct SensorDVL { Eigen::MatrixXd noise_covariance() const; }; +struct SensorDepth { + double measurement; + double measurement_noise; + Eigen::VectorXd innovation(const StateQuat& state) const; + Eigen::MatrixXd jacobian(const StateQuat& state) const; + Eigen::MatrixXd noise_covariance() const; +}; + #endif // ESKF__TYPEDEFS_HPP_ diff --git a/navigation/eskf/launch/eskf.launch.py b/navigation/eskf/launch/eskf.launch.py index 94b98584f..cc3f4bfe8 100644 --- a/navigation/eskf/launch/eskf.launch.py +++ b/navigation/eskf/launch/eskf.launch.py @@ -1,9 +1,9 @@ import os -from os import path from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import OpaqueFunction +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node from auv_setup.launch_arg_common import ( @@ -11,9 +11,9 @@ resolve_drone_and_namespace, ) -eskf_params = path.join( - get_package_share_directory("eskf"), "config", "eskf_params.yaml" -) +# eskf_params = path.join( +# get_package_share_directory("eskf"), "config", "eskf_params_real_world.yaml" +# ) def launch_setup(context, *args, **kwargs): @@ -25,12 +25,35 @@ def launch_setup(context, *args, **kwargs): "robots", f"{drone}.yaml", ) + + use_sim = LaunchConfiguration('use_sim').perform(context).lower() == 'true' + + if use_sim: + param_file_name = "eskf_params.yaml" + else: + param_file_name = "eskf_params_real_world.yaml" + + eskf_params = os.path.join( + get_package_share_directory("eskf"), "config", param_file_name + ) + + drone_env_params = os.path.join( + get_package_share_directory("auv_setup"), + "config", + "environments", + "trondheim_saltwater.yaml", + ) eskf_node = Node( package="eskf", executable="eskf_node", name="eskf_node", namespace=namespace, - parameters=[eskf_params, drone_params, {"frame_prefix": namespace}], + parameters=[ + eskf_params, + drone_params, + drone_env_params, + {"frame_prefix": namespace}, + ], output="screen", ) @@ -38,7 +61,13 @@ def launch_setup(context, *args, **kwargs): def generate_launch_description(): - # This function defines WHAT to do, but doesn't execute the logic yet + sim_arg = DeclareLaunchArgument( + 'use_sim', + default_value='false', + description='Set to "false" to load real-world hardware parameters.', + ) return LaunchDescription( - declare_drone_and_namespace_args() + [OpaqueFunction(function=launch_setup)] + [sim_arg] + + declare_drone_and_namespace_args() + + [OpaqueFunction(function=launch_setup)] ) diff --git a/navigation/eskf/src/eskf.cpp b/navigation/eskf/src/eskf.cpp index de704c3d0..3db940929 100644 --- a/navigation/eskf/src/eskf.cpp +++ b/navigation/eskf/src/eskf.cpp @@ -5,27 +5,20 @@ #include #include "eskf/typedefs.hpp" -double compute_nis(const Eigen::Vector3d& innovation, - const Eigen::Matrix3d& S) { - Eigen::Matrix3d S_inv = S.inverse(); - return innovation.transpose() * S_inv * innovation; +double compute_nis(const Eigen::VectorXd& innovation, + const Eigen::MatrixXd& S) { + Eigen::MatrixXd S_inv = S.inverse(); + return (innovation.transpose() * S_inv * innovation)(0); } ESKF::ESKF(const EskfParams& params) : Q_(params.Q) { // Initialize Covariance current_error_state_.covariance = params.P; - // Initialize Nominal Quaternion to Identity - // current_nom_state_.quat = Eigen::Quaterniond::Identity(); - - // Initialize Quaternion: -90 degrees Yaw because of initial - // drone_orientation - // Eigen::AngleAxisd init_rotation(-M_PI / 2.0, Eigen::Vector3d::UnitZ()); + g_ = params.g_; - // initialize to identity quat + // Initialize Nominal Quaternion to Identity current_nom_state_.quat = Eigen::Quaterniond::Identity(); - current_nom_state_.quat.normalize(); - // Initialize nominal bias values current_nom_state_.gyro_bias = params.initial_gyro_bias; current_nom_state_.accel_bias = params.initial_accel_bias; @@ -185,6 +178,11 @@ void ESKF::dvl_update(const SensorDVL& dvl_meas) { injection_and_reset(); } +void ESKF::depth_update(const SensorDepth& depth_meas) { + measurement_update(depth_meas); + injection_and_reset(); +} + // DVL sensor model implementations Eigen::VectorXd SensorDVL::innovation(const StateQuat& state) const { @@ -200,3 +198,24 @@ Eigen::MatrixXd SensorDVL::jacobian(const StateQuat& state) const { Eigen::MatrixXd SensorDVL::noise_covariance() const { return this->measurement_noise; } + +// Depth sensor model implementations + +Eigen::VectorXd SensorDepth::innovation(const StateQuat& state) const { + double predicted_depth = state.pos[2]; + Eigen::VectorXd innovation(1); + innovation(0) = this->measurement - predicted_depth; + return innovation; +} + +Eigen::MatrixXd SensorDepth::jacobian(const StateQuat& /*state*/) const { + Eigen::MatrixXd H = Eigen::MatrixXd::Zero(1, 15); + H(0, 2) = 1.0; + return H; +} + +Eigen::MatrixXd SensorDepth::noise_covariance() const { + Eigen::MatrixXd R(1, 1); + R(0, 0) = this->measurement_noise; + return R; +} diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp index 0c709708b..61e193fb4 100644 --- a/navigation/eskf/src/eskf_ros.cpp +++ b/navigation/eskf/src/eskf_ros.cpp @@ -15,7 +15,6 @@ auto start_message{R"( ESKFNode::ESKFNode(const rclcpp::NodeOptions& options) : Node("eskf_node", options) { - use_tf_transforms_ = this->declare_parameter("use_tf_transforms"); tf_sensors_loaded_ = !use_tf_transforms_; @@ -27,24 +26,29 @@ ESKFNode::ESKFNode(const rclcpp::NodeOptions& options) publish_tf_ = this->declare_parameter("publish_tf"); if (publish_tf_) { - tf_broadcaster_ = std::make_unique(*this); + tf_broadcaster_ = + std::make_unique(*this); } publish_pose_ = this->declare_parameter("publish_pose"); publish_twist_ = this->declare_parameter("publish_twist"); + publish_biases_ = this->declare_parameter("publish_biases"); + // Declare these here so they appear in `ros2 param list` from startup, // even though they are read in complete_initialization(). this->declare_parameter("publish_rate_ms"); this->declare_parameter("topics.imu"); this->declare_parameter("topics.dvl_twist"); + this->declare_parameter("topics.pressure_sensor"); this->declare_parameter("topics.odom"); this->declare_parameter("topics.pose"); this->declare_parameter("topics.twist"); if (use_tf_transforms_) { tf_buffer_ = std::make_shared(this->get_clock()); - tf_listener_ = std::make_shared(*tf_buffer_); + tf_listener_ = + std::make_shared(*tf_buffer_); tf_timer_ = this->create_wall_timer( std::chrono::milliseconds(500), std::bind(&ESKFNode::lookup_static_transforms, this)); @@ -69,54 +73,75 @@ void ESKFNode::set_subscribers_and_publisher() { dvl_topic, qos_sensor_data, std::bind(&ESKFNode::dvl_callback, this, std::placeholders::_1)); + std::string pressure_topic = + this->get_parameter("topics.pressure_sensor").as_string(); + depth_sub_ = this->create_subscription( + pressure_topic, qos_sensor_data, + std::bind(&ESKFNode::depth_callback, this, std::placeholders::_1)); + std::string odom_topic = this->get_parameter("topics.odom").as_string(); odom_pub_ = this->create_publisher( odom_topic, qos_sensor_data); if (publish_pose_) { std::string pose_topic = this->get_parameter("topics.pose").as_string(); - pose_pub_ = - this->create_publisher( - pose_topic, qos_sensor_data); + pose_pub_ = this->create_publisher< + geometry_msgs::msg::PoseWithCovarianceStamped>(pose_topic, + qos_sensor_data); } if (publish_twist_) { std::string twist_topic = this->get_parameter("topics.twist").as_string(); - twist_pub_ = - this->create_publisher( - twist_topic, qos_sensor_data); + twist_pub_ = this->create_publisher< + geometry_msgs::msg::TwistWithCovarianceStamped>(twist_topic, + qos_sensor_data); + } + + if (publish_biases_) { + accel_bias_pub_ = + this->create_publisher( + "eskf/accel_bias", qos_sensor_data); + gyro_bias_pub_ = + this->create_publisher( + "eskf/gyro_bias", qos_sensor_data); } #ifndef NDEBUG - nis_pub_ = create_publisher( - "eskf/nis", vortex::utils::qos_profiles::reliable_profile()); + nis_dvl_pub_ = create_publisher( + "eskf/nis_dvl", vortex::utils::qos_profiles::reliable_profile()); + nis_depth_pub_ = create_publisher( + "eskf/nis_depth", vortex::utils::qos_profiles::reliable_profile()); #endif } void ESKFNode::set_parameters() { - if (!use_tf_transforms_) { std::vector R_imu_correction = - this->declare_parameter>("transform.imu_frame_r"); + this->declare_parameter>( + "transform.imu_frame_r"); R_imu_eskf_ = Eigen::Map>( R_imu_correction.data()); std::vector T_imu_correction = - this->declare_parameter>("transform.imu_frame_t"); + this->declare_parameter>( + "transform.imu_frame_t"); T_imu_eskf_ = Eigen::Map(T_imu_correction.data()); std::vector R_dvl_correction = - this->declare_parameter>("transform.dvl_frame_r"); + this->declare_parameter>( + "transform.dvl_frame_r"); R_dvl_eskf_ = Eigen::Map>( R_dvl_correction.data()); std::vector T_dvl_correction = - this->declare_parameter>("transform.dvl_frame_t"); + this->declare_parameter>( + "transform.dvl_frame_t"); T_dvl_eskf_ = Eigen::Map(T_dvl_correction.data()); std::vector T_depth_correction = - this->declare_parameter>("transform.depth_frame_t"); + this->declare_parameter>( + "transform.depth_frame_t"); T_depth_eskf_ = Eigen::Map(T_depth_correction.data()); } @@ -142,11 +167,13 @@ void ESKFNode::set_parameters() { } Eigen::Matrix15d P = createDiagonalMatrix<15>(diag_p_init); + Eigen::Vector3d g_vec(0.0, 0.0, this->gravity); + std::vector initial_gyro_bias = this->declare_parameter>( "initial_gyro_bias", std::vector{0.0, 0.0, 0.0}); - spdlog::info("initial_gyro_bias: [{}, {}, {}]", initial_gyro_bias[0], - initial_gyro_bias[1], initial_gyro_bias[2]); + spdlog::info("initial_gyro_bias: [{}, {}, {}]", initial_gyro_bias[0], + initial_gyro_bias[1], initial_gyro_bias[2]); if (initial_gyro_bias.size() != 3) { throw std::runtime_error("initial_gyro_bias must have length 3"); } @@ -154,8 +181,8 @@ void ESKFNode::set_parameters() { std::vector initial_accel_bias = this->declare_parameter>( "initial_accel_bias", std::vector{0.0, 0.0, 0.0}); - spdlog::info("initial_accel_bias: [{}, {}, {}]", initial_accel_bias[0], - initial_accel_bias[1], initial_accel_bias[2]); + spdlog::info("initial_accel_bias: [{}, {}, {}]", initial_accel_bias[0], + initial_accel_bias[1], initial_accel_bias[2]); if (initial_accel_bias.size() != 3) { throw std::runtime_error("initial_accel_bias must have length 3"); } @@ -163,6 +190,7 @@ void ESKFNode::set_parameters() { EskfParams eskf_params{ .Q = Q, .P = P, + .g_ = g_vec, .initial_gyro_bias = Eigen::Map(initial_gyro_bias.data()), .initial_accel_bias = @@ -170,8 +198,7 @@ void ESKFNode::set_parameters() { eskf_ = std::make_unique(eskf_params); - add_gravity_to_imu_ = - this->declare_parameter("add_gravity_to_imu"); + add_gravity_to_imu_ = this->declare_parameter("add_gravity_to_imu"); spdlog::info("add_gravity_to_imu: {}", add_gravity_to_imu_); } @@ -255,7 +282,27 @@ void ESKFNode::dvl_callback( // Publish NIS in Debug mode std_msgs::msg::Float64 nis_msg; nis_msg.data = eskf_->get_nis(); - nis_pub_->publish(nis_msg); + nis_dvl_pub_->publish(nis_msg); +#endif +} + +void ESKFNode::depth_callback( + const sensor_msgs::msg::FluidPressure::SharedPtr msg) { + SensorDepth depth_sensor; + // the simulation is a gauge sensor so we don't subtract atmospheric + // pressure. + depth_sensor.measurement = + -msg->fluid_pressure / (this->water_density * this->gravity); + depth_sensor.measurement_noise = msg->variance; + + // spdlog::info("depth meas is: {}",depth_sensor.measurement); + eskf_->depth_update(depth_sensor); + +#ifndef NDEBUG + // Publish NIS in Debug mode + std_msgs::msg::Float64 nis_msg; + nis_msg.data = eskf_->get_nis(); + nis_depth_pub_->publish(nis_msg); #endif } @@ -339,6 +386,28 @@ void ESKFNode::publish_odom() { if (publish_tf_) { publish_tf(nom_state, current_time); } + + if (publish_biases_) { + geometry_msgs::msg::Vector3Stamped accel_bias_msg; + accel_bias_msg.header.stamp = current_time; + accel_bias_msg.header.frame_id = + frame("base_link"); // Biases are in the body frame + + accel_bias_msg.vector.x = nom_state.accel_bias.x(); + accel_bias_msg.vector.y = nom_state.accel_bias.y(); + accel_bias_msg.vector.z = nom_state.accel_bias.z(); + + accel_bias_pub_->publish(accel_bias_msg); + + geometry_msgs::msg::Vector3Stamped gyro_bias_msg; + gyro_bias_msg.header = accel_bias_msg.header; + + gyro_bias_msg.vector.x = nom_state.gyro_bias.x(); + gyro_bias_msg.vector.y = nom_state.gyro_bias.y(); + gyro_bias_msg.vector.z = nom_state.gyro_bias.z(); + + gyro_bias_pub_->publish(gyro_bias_msg); + } } void ESKFNode::lookup_static_transforms() { @@ -354,7 +423,8 @@ void ESKFNode::lookup_static_transforms() { T_dvl_eskf_ = Tf_base_dvl_.translation(); Tf_base_depth_ = tf2::transformToEigen(tf_buffer_->lookupTransform( - frame("base_link"), frame("pressure_sensor_link"), tf2::TimePointZero)); + frame("base_link"), frame("pressure_sensor_link"), + tf2::TimePointZero)); T_depth_eskf_ = Tf_base_depth_.translation(); tf_sensors_loaded_ = true; @@ -368,6 +438,13 @@ void ESKFNode::lookup_static_transforms() { void ESKFNode::complete_initialization() { set_subscribers_and_publisher(); + // gravity, water density and atmospheric pressure. + this->gravity = + -this->declare_parameter("gravity.acceleration", 9.81); + this->water_density = + this->declare_parameter("water.density", 1000.0); + this->atmospheric_pressure = + this->declare_parameter("atmosphere.pressure", 100000.0); set_parameters(); time_step_ = std::chrono::milliseconds( @@ -383,7 +460,8 @@ void ESKFNode::complete_initialization() { #endif } -void ESKFNode::publish_tf(const StateQuat& nom_state, const rclcpp::Time& time) { +void ESKFNode::publish_tf(const StateQuat& nom_state, + const rclcpp::Time& time) { geometry_msgs::msg::TransformStamped tf_msg; tf_msg.header.stamp = time; diff --git a/navigation/eskf/test/eskf_consistency_test.py b/navigation/eskf/test/eskf_consistency_test.py index f130a1352..662e06421 100644 --- a/navigation/eskf/test/eskf_consistency_test.py +++ b/navigation/eskf/test/eskf_consistency_test.py @@ -20,28 +20,42 @@ class EskfValidator(Node): def __init__(self): super().__init__('eskf_validator') - self.est_topic = 'orca/odom' - self.gt_topic = '/orca/odom/stonefish' + self.est_topic = '/nautilus/odom/eskf' + self.gt_topic = '/nautilus/odom' # --- Publishers (Foxglove Visualizers) --- - # RMSE (Running Root Mean Square Error) - self.pub_rmse_pos = self.create_publisher(Float64, 'eskf_metrics/rmse/position', 10) + # Overall 3D RMSE + self.pub_rmse_pos_total = self.create_publisher(Float64, 'eskf_metrics/rmse/position/total', 10) + self.pub_rmse_vel_total = self.create_publisher(Float64, 'eskf_metrics/rmse/velocity/total', 10) self.pub_rmse_ori = self.create_publisher(Float64, 'eskf_metrics/rmse/orientation_rad', 10) - self.pub_rmse_vel = self.create_publisher(Float64, 'eskf_metrics/rmse/velocity', 10) - # Euler Angles (for visual comparison) + # Axis-Specific Position RMSE + self.pub_rmse_pos_x = self.create_publisher(Float64, 'eskf_metrics/rmse/position/x', 10) + self.pub_rmse_pos_y = self.create_publisher(Float64, 'eskf_metrics/rmse/position/y', 10) + self.pub_rmse_pos_z = self.create_publisher(Float64, 'eskf_metrics/rmse/position/z', 10) + + # Axis-Specific Velocity RMSE + self.pub_rmse_vel_x = self.create_publisher(Float64, 'eskf_metrics/rmse/velocity/x', 10) + self.pub_rmse_vel_y = self.create_publisher(Float64, 'eskf_metrics/rmse/velocity/y', 10) + self.pub_rmse_vel_z = self.create_publisher(Float64, 'eskf_metrics/rmse/velocity/z', 10) + + # Euler Angles & NEES self.pub_euler_est = self.create_publisher(Float64MultiArray, 'debug/euler/est', 10) self.pub_euler_gt = self.create_publisher(Float64MultiArray, 'debug/euler/gt', 10) - - # NEES (Normalized Estimation Error Squared) self.pub_nees_pos = self.create_publisher(Float64, 'eskf_metrics/nees/position', 10) self.pub_nees_ori = self.create_publisher(Float64, 'eskf_metrics/nees/orientation', 10) # --- State for Running RMSE --- self.n_samples = 0 - self.sse_pos = 0.0 # Sum of Squared Errors + + # NumPy arrays make axis-by-axis tracking incredibly easy + self.sse_pos_xyz = np.array([0.0, 0.0, 0.0]) + self.sse_vel_xyz = np.array([0.0, 0.0, 0.0]) + + # Total Euclidean SSE + self.sse_pos_total = 0.0 + self.sse_vel_total = 0.0 self.sse_ori = 0.0 - self.sse_vel = 0.0 # --- Subscribers --- self.get_logger().info(f"Subscribing to {self.est_topic} and {self.gt_topic}...") @@ -59,90 +73,86 @@ def callback(self, est_msg, gt_msg): self.n_samples += 1 # =========================== - # 1. POSITION (Euclidean) + # 1. POSITION (Euclidean & Axis) # =========================== p_est = np.array([est_msg.pose.pose.position.x, est_msg.pose.pose.position.y, est_msg.pose.pose.position.z]) p_gt = np.array([gt_msg.pose.pose.position.x, gt_msg.pose.pose.position.y, gt_msg.pose.pose.position.z]) err_pos_vec = p_est - p_gt - err_pos_norm = np.linalg.norm(err_pos_vec) - # Update RMSE - self.sse_pos += err_pos_norm**2 - self.publish_float(self.pub_rmse_pos, np.sqrt(self.sse_pos / self.n_samples)) + # Update Axis-Specific RMSE (Squares elements individually) + self.sse_pos_xyz += err_pos_vec**2 + rmse_pos_xyz = np.sqrt(self.sse_pos_xyz / self.n_samples) + + self.publish_float(self.pub_rmse_pos_x, rmse_pos_xyz[0]) + self.publish_float(self.pub_rmse_pos_y, rmse_pos_xyz[1]) + self.publish_float(self.pub_rmse_pos_z, rmse_pos_xyz[2]) + + # Update Total 3D RMSE + self.sse_pos_total += np.linalg.norm(err_pos_vec)**2 + self.publish_float(self.pub_rmse_pos_total, np.sqrt(self.sse_pos_total / self.n_samples)) # =========================== # 2. ORIENTATION (Quaternion) # =========================== - # Extract Quaternions (x, y, z, w) q_est = [est_msg.pose.pose.orientation.x, est_msg.pose.pose.orientation.y, est_msg.pose.pose.orientation.z, est_msg.pose.pose.orientation.w] q_gt = [gt_msg.pose.pose.orientation.x, gt_msg.pose.pose.orientation.y, gt_msg.pose.pose.orientation.z, gt_msg.pose.pose.orientation.w] - # Convert to Rotation objects r_est = Rotation.from_quat(q_est) r_gt = Rotation.from_quat(q_gt) - # --------------------------------------------------------- - # Extract Euler Angles (Roll, Pitch, Yaw) - # Using 'xyz' sequence (standard for ROS aerospace/underwater) - # degrees=True makes it easier to read in plots (0-360 or +/-180) - # --------------------------------------------------------- - euler_est = r_est.as_euler('xyz', degrees=True) # [roll, pitch, yaw] - euler_gt = r_gt.as_euler('xyz', degrees=True) # [roll, pitch, yaw] + euler_est = r_est.as_euler('xyz', degrees=True) + euler_gt = r_gt.as_euler('xyz', degrees=True) - # Publish Estimated Euler msg_euler_est = Float64MultiArray() msg_euler_est.data = euler_est.tolist() self.pub_euler_est.publish(msg_euler_est) - # Publish GT Euler msg_euler_gt = Float64MultiArray() msg_euler_gt.data = euler_gt.tolist() self.pub_euler_gt.publish(msg_euler_gt) - # Calculate Error Rotation: R_err = R_gt^T * R_est - # This gives the relative rotation needed to go from GT to Est - r_err = r_gt.inv() * r_est - # Convert to Rotation Vector (magnitude is the angle error in radians) + r_err = r_gt.inv() * r_est err_ori_vec = r_err.as_rotvec() - err_ori_norm = np.linalg.norm(err_ori_vec) - - # Update RMSE - self.sse_ori += err_ori_norm**2 + + self.sse_ori += np.linalg.norm(err_ori_vec)**2 self.publish_float(self.pub_rmse_ori, np.sqrt(self.sse_ori / self.n_samples)) # =========================== - # 3. VELOCITY (Euclidean) + # 3. VELOCITY (Euclidean & Axis) # =========================== v_est = np.array([est_msg.twist.twist.linear.x, est_msg.twist.twist.linear.y, est_msg.twist.twist.linear.z]) v_gt = np.array([gt_msg.twist.twist.linear.x, gt_msg.twist.twist.linear.y, gt_msg.twist.twist.linear.z]) err_vel_vec = v_est - v_gt - self.sse_vel += np.linalg.norm(err_vel_vec)**2 - self.publish_float(self.pub_rmse_vel, np.sqrt(self.sse_vel / self.n_samples)) + + # Update Axis-Specific RMSE + self.sse_vel_xyz += err_vel_vec**2 + rmse_vel_xyz = np.sqrt(self.sse_vel_xyz / self.n_samples) + + self.publish_float(self.pub_rmse_vel_x, rmse_vel_xyz[0]) + self.publish_float(self.pub_rmse_vel_y, rmse_vel_xyz[1]) + self.publish_float(self.pub_rmse_vel_z, rmse_vel_xyz[2]) - # Note: Standard Odometry does NOT contain Linear Acceleration. - # If you need Accel RMSE, you must subscribe to the IMU topic separately. + # Update Total 3D RMSE + self.sse_vel_total += np.linalg.norm(err_vel_vec)**2 + self.publish_float(self.pub_rmse_vel_total, np.sqrt(self.sse_vel_total / self.n_samples)) # =========================== # 4. NEES CALCULATION # =========================== - # NEES = error^T * Covariance^-1 * error - - # Reshape the 36-float array into 6x6 matrix cov_pose = np.array(est_msg.pose.covariance).reshape(6, 6) - # --- Position NEES (Top-Left 3x3) --- + # --- Position NEES --- cov_pos = cov_pose[0:3, 0:3] try: cov_pos_inv = np.linalg.inv(cov_pos) nees_pos = err_pos_vec.T @ cov_pos_inv @ err_pos_vec self.publish_float(self.pub_nees_pos, nees_pos) except np.linalg.LinAlgError: - pass # Singular matrix, skip + pass - # --- Orientation NEES (Bottom-Right 3x3) --- - # Note: We use the rotation vector error (err_ori_vec) calculated earlier + # --- Orientation NEES --- cov_ori = cov_pose[3:6, 3:6] try: cov_ori_inv = np.linalg.inv(cov_ori) @@ -168,4 +178,4 @@ def main(args=None): rclpy.shutdown() if __name__ == '__main__': - main() \ No newline at end of file + main() diff --git a/navigation/odom_transformer/include/odom_transformer/odom_transformer.hpp b/navigation/odom_transformer/include/odom_transformer/odom_transformer.hpp index 194846b35..8a42bdb7b 100644 --- a/navigation/odom_transformer/include/odom_transformer/odom_transformer.hpp +++ b/navigation/odom_transformer/include/odom_transformer/odom_transformer.hpp @@ -38,8 +38,8 @@ class OdomTransformer : public rclcpp::Node { rclcpp::Publisher::SharedPtr odom_pub_; rclcpp::Publisher::SharedPtr pose_pub_; - rclcpp::Publisher< - geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr twist_pub_; + rclcpp::Publisher::SharedPtr + twist_pub_; // Transform from base_link to sensor_link Eigen::Matrix3d R_base_sensor_ = Eigen::Matrix3d::Identity(); diff --git a/navigation/odom_transformer/launch/odom_transformer.launch.py b/navigation/odom_transformer/launch/odom_transformer.launch.py index 06afbe536..7e5bd759f 100644 --- a/navigation/odom_transformer/launch/odom_transformer.launch.py +++ b/navigation/odom_transformer/launch/odom_transformer.launch.py @@ -46,6 +46,5 @@ def launch_setup(context, *args, **kwargs): def generate_launch_description(): return LaunchDescription( - declare_drone_and_namespace_args() - + [OpaqueFunction(function=launch_setup)] + declare_drone_and_namespace_args() + [OpaqueFunction(function=launch_setup)] ) diff --git a/navigation/odom_transformer/src/odom_transformer.cpp b/navigation/odom_transformer/src/odom_transformer.cpp index 82a73a6f4..86ef43f7e 100644 --- a/navigation/odom_transformer/src/odom_transformer.cpp +++ b/navigation/odom_transformer/src/odom_transformer.cpp @@ -24,8 +24,7 @@ OdomTransformer::OdomTransformer(const rclcpp::NodeOptions& options) this->declare_parameter("topics.twist"); tf_buffer_ = std::make_shared(this->get_clock()); - tf_listener_ = - std::make_shared(*tf_buffer_); + tf_listener_ = std::make_shared(*tf_buffer_); tf_timer_ = this->create_wall_timer( std::chrono::milliseconds(500), std::bind(&OdomTransformer::lookup_static_transforms, this)); @@ -41,14 +40,13 @@ void OdomTransformer::lookup_static_transforms() { tf_loaded_ = true; tf_timer_->cancel(); RCLCPP_INFO(get_logger(), - "Loaded static transform: %s -> %s t=(%.3f, %.3f, %.3f)", - frame("base_link").c_str(), frame(sensor_frame_).c_str(), - t_base_sensor_.x(), t_base_sensor_.y(), - t_base_sensor_.z()); + "Loaded static transform: %s -> %s t=(%.3f, %.3f, %.3f)", + frame("base_link").c_str(), frame(sensor_frame_).c_str(), + t_base_sensor_.x(), t_base_sensor_.y(), t_base_sensor_.z()); complete_initialization(); } catch (const tf2::TransformException& ex) { RCLCPP_WARN(get_logger(), "TF lookup failed (will retry): %s", - ex.what()); + ex.what()); } } @@ -67,21 +65,19 @@ void OdomTransformer::complete_initialization() { this->create_publisher(output_topic, qos); if (publish_pose_) { - pose_pub_ = - this->create_publisher< - geometry_msgs::msg::PoseWithCovarianceStamped>( - this->get_parameter("topics.pose").as_string(), qos); + pose_pub_ = this->create_publisher< + geometry_msgs::msg::PoseWithCovarianceStamped>( + this->get_parameter("topics.pose").as_string(), qos); } if (publish_twist_) { - twist_pub_ = - this->create_publisher< - geometry_msgs::msg::TwistWithCovarianceStamped>( - this->get_parameter("topics.twist").as_string(), qos); + twist_pub_ = this->create_publisher< + geometry_msgs::msg::TwistWithCovarianceStamped>( + this->get_parameter("topics.twist").as_string(), qos); } - RCLCPP_INFO(get_logger(), "Odom transformer: %s -> %s", - input_topic.c_str(), output_topic.c_str()); + RCLCPP_INFO(get_logger(), "Odom transformer: %s -> %s", input_topic.c_str(), + output_topic.c_str()); } void OdomTransformer::odom_callback( @@ -93,8 +89,7 @@ void OdomTransformer::odom_callback( Eigen::Matrix3d R_odom_sensor = q_odom_sensor.toRotationMatrix(); // Orientation: R_odom_base = R_odom_sensor * R_base_sensor^-1 - Eigen::Matrix3d R_odom_base = - R_odom_sensor * R_base_sensor_.transpose(); + Eigen::Matrix3d R_odom_base = R_odom_sensor * R_base_sensor_.transpose(); Eigen::Quaterniond q_odom_base(R_odom_base); q_odom_base.normalize(); From 14772e8d7ae40c04ed067c7ba4a7dee0b2266a74 Mon Sep 17 00:00:00 2001 From: Ahmed Date: Tue, 31 Mar 2026 15:28:24 +0200 Subject: [PATCH 256/290] fixing problem with environment params --- navigation/eskf/config/eskf_params.yaml | 5 +++++ navigation/eskf/config/eskf_params_real_world.yaml | 5 +++++ navigation/eskf/launch/eskf.launch.py | 11 ----------- navigation/eskf/src/eskf_ros.cpp | 7 +++---- 4 files changed, 13 insertions(+), 15 deletions(-) diff --git a/navigation/eskf/config/eskf_params.yaml b/navigation/eskf/config/eskf_params.yaml index 33bf2bd60..001ede019 100644 --- a/navigation/eskf/config/eskf_params.yaml +++ b/navigation/eskf/config/eskf_params.yaml @@ -22,6 +22,11 @@ publish_twist: true publish_rate_ms: 5 add_gravity_to_imu: false + # biases in the IMU initial_gyro_bias: [0.0, 0.0, 0.0] initial_accel_bias: [0.0, 0.0, 0.0] publish_biases: false + # environmental parameters + atmospheric_pressure: 101000.0 # [Pa] + water_density: 1026.0 # [kg/m3] + gravity: 9.82841 diff --git a/navigation/eskf/config/eskf_params_real_world.yaml b/navigation/eskf/config/eskf_params_real_world.yaml index ed2455aa9..0f82da7d7 100644 --- a/navigation/eskf/config/eskf_params_real_world.yaml +++ b/navigation/eskf/config/eskf_params_real_world.yaml @@ -21,10 +21,15 @@ publish_pose: true publish_twist: true publish_rate_ms: 5 + # biases in the IMU add_gravity_to_imu: true initial_gyro_bias: [0.0, 0.0, 0.0] initial_accel_bias: [3.53e-2, 0.0, -0.0533] publish_biases: true + # environmental parameters + atmospheric_pressure: 101000.0 # [Pa] + water_density: 1026.0 # [kg/m3] + gravity: 9.82841 # USED SENSORS # IMU: Kongsberg mini MRU diff --git a/navigation/eskf/launch/eskf.launch.py b/navigation/eskf/launch/eskf.launch.py index cc3f4bfe8..0d3acd889 100644 --- a/navigation/eskf/launch/eskf.launch.py +++ b/navigation/eskf/launch/eskf.launch.py @@ -11,10 +11,6 @@ resolve_drone_and_namespace, ) -# eskf_params = path.join( -# get_package_share_directory("eskf"), "config", "eskf_params_real_world.yaml" -# ) - def launch_setup(context, *args, **kwargs): drone, namespace = resolve_drone_and_namespace(context) @@ -37,12 +33,6 @@ def launch_setup(context, *args, **kwargs): get_package_share_directory("eskf"), "config", param_file_name ) - drone_env_params = os.path.join( - get_package_share_directory("auv_setup"), - "config", - "environments", - "trondheim_saltwater.yaml", - ) eskf_node = Node( package="eskf", executable="eskf_node", @@ -51,7 +41,6 @@ def launch_setup(context, *args, **kwargs): parameters=[ eskf_params, drone_params, - drone_env_params, {"frame_prefix": namespace}, ], output="screen", diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp index 61e193fb4..ed70aee78 100644 --- a/navigation/eskf/src/eskf_ros.cpp +++ b/navigation/eskf/src/eskf_ros.cpp @@ -439,12 +439,11 @@ void ESKFNode::lookup_static_transforms() { void ESKFNode::complete_initialization() { set_subscribers_and_publisher(); // gravity, water density and atmospheric pressure. - this->gravity = - -this->declare_parameter("gravity.acceleration", 9.81); + this->gravity = -this->declare_parameter("gravity", 9.81); this->water_density = - this->declare_parameter("water.density", 1000.0); + this->declare_parameter("water_density", 1000.0); this->atmospheric_pressure = - this->declare_parameter("atmosphere.pressure", 100000.0); + this->declare_parameter("atmospheric_pressure", 100000.0); set_parameters(); time_step_ = std::chrono::milliseconds( From 1f6cb36f530d11af94893f3dccb279e47e1f43d8 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Tue, 31 Mar 2026 17:27:06 +0200 Subject: [PATCH 257/290] thrust pwm: clamp to publish neutral pwn --- .../src/thruster_interface_auv_driver.cpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) 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 eb832d3ae..35305749a 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp @@ -78,13 +78,18 @@ std::vector ThrusterInterfaceAUVDriver::interpolate_forces_to_pwm( } std::uint16_t ThrusterInterfaceAUVDriver::force_to_pwm(double force) { + // The LEFT/RIGHT polynomials have a gap at force=0 (~1468 vs ~1534) + // because they were fitted to data outside the ESC deadband. + // Forces below this threshold (in kg) return idle PWM to bridge the gap. + constexpr double deadband_kg = 0.03; + + if (std::abs(force) < deadband_kg) { + return idle_pwm_value_; + } if (force < 0.0) { return calc_poly(force, left_coeffs_); } - if (force > 0.0) { - return calc_poly(force, right_coeffs_); - } - return idle_pwm_value_; + return calc_poly(force, right_coeffs_); } std::uint16_t ThrusterInterfaceAUVDriver::calc_poly( From 8481728f49dbe12cfa13d415a69683364717a013 Mon Sep 17 00:00:00 2001 From: Cyprian Osinski Date: Tue, 31 Mar 2026 18:38:12 +0200 Subject: [PATCH 258/290] Finished the Lyapunov proof for the controller, and will upload it to Vortex wiki and link it in README. Began changing variable names and adding neccesary functionality in vortex-utils --- .../dp_adapt_backs_controller.hpp | 20 +++--- .../dp_adapt_backs_controller_utils.hpp | 8 +-- .../src/dp_adapt_backs_controller.cpp | 23 +++---- .../src/dp_adapt_backs_controller_ros.cpp | 11 +--- .../src/dp_adapt_backs_controller_utils.cpp | 66 +++++++------------ 5 files changed, 54 insertions(+), 74 deletions(-) diff --git a/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller.hpp b/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller.hpp index b4e316b55..37c39a5cd 100644 --- a/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller.hpp +++ b/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller.hpp @@ -22,18 +22,20 @@ class DPAdaptBacksController { public: explicit DPAdaptBacksController(const DPAdaptParams& dp_adapt_params); - // TODO: change calculate tau, to be based on error state of quaternion - - // @brief Calculate thecontrol input tau - // @param pose: 6D vector containing the vehicle pose [x, y, z, roll, pitch, - // yaw] - // @param pose_d: 6D vector containing the desired vehicle pose [x, y, z, - // roll, pitch, yaw] + // @brief Calculates the control input tau found in the backstepping proof. + // Utilizes error state to avoid 7x6 non invertible J matrix. The + // approximation of quaternion error -> euler angle error is used, and + // therefore we explicitly assume small pertrubations + // + // @param pose: 7D vector containing the vehicle pose [x, y, z, qw, qx, qy, + // qz] + // @param pose_d: 7D vector containing the desired vehicle pose [x, y, z, + // qw, qx, qy, qz] // @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::PoseEuler& pose, - const vortex::utils::types::PoseEuler& pose_d, + Eigen::Vector6d calculate_tau(const vortex::utils::types::Pose& pose, + const vortex::utils::types::Pose& pose_d, const vortex::utils::types::Twist& twist); // @brief Reset the adaptive parameters diff --git a/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller_utils.hpp b/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller_utils.hpp index c753f9d39..7ef767045 100644 --- a/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller_utils.hpp +++ b/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller_utils.hpp @@ -5,8 +5,6 @@ #include "dp_adapt_backs_controller_quat/typedefs.hpp" #include "typedefs.hpp" -// TODO: change T_dot, J_dot and J_inv - namespace vortex::control { // @brief Calculate the derivative of the rotation matrix @@ -20,18 +18,18 @@ Eigen::Matrix3d calculate_R_dot(const vortex::utils::types::Pose& pose, // @param pose: 6D vector containing the vehicle pose [x, y, z, qw, qx, qy, qz] // @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::Pose& pose, +Eigen::Matrix3d calculate_Q_dot(const vortex::utils::types::Pose& pose, const vortex::utils::types::Twist& twist); // @brief Calculate the pseudo-inverse of the Jacobian matrix // @param pose: 7D vector containing the vehicle pose [x, y, z, qw, qx, qy, qz] // @return 6x6 pseudo-inverse Jacobian matrix -Eigen::Matrix6d calculate_J_inv(const vortex::utils::types::Pose& pose); +Eigen::Matrix6d calculate_L_inv(const vortex::utils::types::Pose& pose); // @brief calculate the derivative of the Jacobian matrix // @param pose: 7D vector containing the vehicle pose [x, y, z, qw, qx, qy, qz] // @param twist: 6D vector containing the vehicle velocity [u, v, w, p, q, r] -Eigen::Matrix6d calculate_J_dot(const vortex::utils::types::Pose& pose, +Eigen::Matrix6d calculate_L_dot(const vortex::utils::types::Pose& pose, const vortex::utils::types::Twist& twist); // @brief Calculate the coriolis matrix diff --git a/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller.cpp b/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller.cpp index b884eb514..e666b318d 100644 --- a/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller.cpp +++ b/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller.cpp @@ -7,7 +7,7 @@ namespace vortex::control { -using vortex::utils::types::PoseEuler; +using vortex::utils::types::Pose; using vortex::utils::types::Twist; DPAdaptBacksController::DPAdaptBacksController( @@ -30,21 +30,22 @@ Eigen::Vector6d DPAdaptBacksController::calculate_tau(const Pose& pose, const Twist& twist) { // TODO: implement error state calculation. Maybe look at hybrid // switching between pure RPY and error state when error is big - 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::Vector3d pos_error = pose.pos_vector() - pose_d.pos_vector(); + Eigen::Vector3d quat_error = vortex::utils::math::quaternion_error( + pose.ori_quaternion, pose_d.ori_quaternion); - Eigen::Matrix6d C = + Pose error_state = Pose.from_eigen(pos_error, quat_error) + + Eigen::Matrix6d C = calculate_coriolis(m_, r_b_bg_, twist, inertia_matrix_body_); - 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::Matrix6d L_inv = calculate_L_inv(pose); + Eigen::Matrix6d L_dot = calculate_L_dot(pose, twist); + Eigen::Vector6d alpha = -L_inv * K1_ * error.to_vector(); Eigen::Vector6d z_1 = error.to_vector(); Eigen::Vector6d z_2 = twist.to_vector() - alpha; Eigen::Vector6d alpha_dot = - ((J_inv * J_dot * J_inv) * K1_ * z_1) - - (J_inv * K1_ * pose.as_j_matrix() * twist.to_vector()); + ((L_inv * L_dot * L_inv) * K1_ * z_1) - + (L_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; diff --git a/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_ros.cpp b/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_ros.cpp index 0df8cf426..b7a2b052e 100644 --- a/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_ros.cpp +++ b/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_ros.cpp @@ -10,14 +10,9 @@ #include "dp_adapt_backs_controller_quat/typedefs.hpp" constexpr std::string_view start_message = R"( - /$$$$$$$ /$$$$$$$ /$$$$$$ /$$ /$$$$$$ /$$ /$$ /$$ -| $$__ $$| $$__ $$ /$$__ $$ | $$ /$$__ $$ | $$ | $$| $$ -| $$ \ $$| $$ \ $$ | $$ \ $$ /$$ /$$ /$$$$$$ /$$$$$$ | $$ \__/ /$$$$$$ /$$$$$$$ /$$$$$$ /$$$$$$ /$$$$$$ | $$| $$ /$$$$$$ /$$$$$$ -| $$ | $$| $$$$$$$/ | $$ | $$| $$ | $$ |____ $$|_ $$_/ | $$ /$$__ $$| $$__ $$|_ $$_/ /$$__ $$ /$$__ $$| $$| $$ /$$__ $$ /$$__ $$ -| $$ | $$| $$____/ | $$ | $$| $$ | $$ /$$$$$$$ | $$ | $$ | $$ \ $$| $$ \ $$ | $$ | $$ \__/| $$ \ $$| $$| $$| $$$$$$$$| $$ \__/ -| $$ | $$| $$ | $$/$$ $$| $$ | $$ /$$__ $$ | $$ /$$ | $$ $$| $$ | $$| $$ | $$ | $$ /$$| $$ | $$ | $$| $$| $$| $$_____/| $$ -| $$$$$$$/| $$ | $$$$$$/| $$$$$$/| $$$$$$$ | $$$$/ | $$$$$$/| $$$$$$/| $$ | $$ | $$$$/| $$ | $$$$$$/| $$| $$| $$$$$$$| $$ -|_______/ |__/ \____ $$$ \______/ \_______/ \___/ \______/ \______/ |__/ |__/ \___/ |__/ \______/ |__/|__/ \_______/|__/ +████▄ █████▄ ▄█████▄ ▄▄ ▄▄ ▄▄▄ ▄▄▄▄▄▄ ▄█████ ▄▄▄ ▄▄ ▄▄ ▄▄▄▄▄▄ ▄▄▄▄ ▄▄▄ ▄▄ ▄▄ ▄▄▄▄▄ ▄▄▄▄ +██ ██ ██▄▄█▀ ██ ▄ ██ ██ ██ ██▀██ ██ ██ ██▀██ ███▄██ ██ ██▄█▄ ██▀██ ██ ██ ██▄▄ ██▄█▄ +████▀ ██ ▀█████▀ ▀███▀ ██▀██ ██ ▀█████ ▀███▀ ██ ▀██ ██ ██ ██ ▀███▀ ██▄▄▄ ██▄▄▄ ██▄▄▄ ██ ██ )"; namespace vortex::control { diff --git a/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_utils.cpp b/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_utils.cpp index 20b390238..00d5bd497 100644 --- a/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_utils.cpp +++ b/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_utils.cpp @@ -1,75 +1,59 @@ #include "dp_adapt_backs_controller_quat/dp_adapt_backs_controller_utils.hpp" +#include #include +#include #include #include +#include "dp_adapt_backs_controller/typedefs.hpp" #include "dp_adapt_backs_controller_quat/typedefs.hpp" -// TODO: change j_inv, t_dot, j_dot - namespace vortex::control { -Eigen::Matrix6d calculate_J_inv(const vortex::utils::types::PoseEuler& pose) { - Eigen::Matrix6d J = pose.as_j_matrix(); +Eigen::Matrix6d calculate_L_inv(const vortex::utils::types::Pose& pose) { + Eigen::Matrix6d L = pose.as_L_matrix(); constexpr double tolerance = 1e-8; - if (std::abs(J.determinant()) < tolerance) { - spdlog::error("J is singular"); + if (std::abs(L.determinant()) < tolerance) { + spdlog::error("L is singular"); // Moore-Penrose pseudoinverse in case of near singular matrix, better // result for smaller singular values - return J.completeOrthogonalDecomposition().pseudoInverse(); + return L.completeOrthogonalDecomposition().pseudoInverse(); } - return J.inverse(); + return L.inverse(); } -Eigen::Matrix3d calculate_R_dot(const vortex::utils::types::PoseEuler& pose, +Eigen::Matrix3d calculate_R_dot(const vortex::utils::types::Pose& pose, const vortex::utils::types::Twist& twist) { return pose.as_rotation_matrix() * vortex::utils::math::get_skew_symmetric_matrix( twist.to_vector().tail(3)); } -Eigen::Matrix3d calculate_T_dot(const vortex::utils::types::PoseEuler& pose, +Eigen::Matrix3d calculate_Q_dot(const vortex::utils::types::Pose& 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 pose_dot = pose.as_j_matrix() * twist.to_vector(); - - 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, - -sin_phi * tan_theta * phi_dot, 0.0, -sin_phi * phi_dot, - -cos_phi * phi_dot, 0.0, (cos_phi * phi_dot) / cos_theta, - (-sin_phi * phi_dot) / cos_theta; - - Eigen::Matrix3d dt_dtheta; - dt_dtheta << 0.0, sin_phi * inv_cos2 * theta_dot, - cos_phi * inv_cos2 * theta_dot, 0.0, 0.0, 0.0, 0.0, - (sin_phi * sin_theta) * inv_cos2 * theta_dot, - (cos_phi * sin_theta) * inv_cos2 * theta_dot; - - return dt_dphi + dt_dtheta; + Eigen::Vector3d omega = twist.to_vector().tail(3); + Eigen::Matrix3d eta_error_dot = + pose.ori_quaternion_vector_part() * omega * Eigen::Matrix3d::Identity(); + Eigen::Matrix3d epsilon_error_dot = + vortex::utils::math::get_skew_symmetric_matrix(pose.as_Q_matrix() * + omega); + Eigen::Matrix3d Q_dot = (1 / 2) * (epsilon_error_dot - eta_error_dot); + return Q_dot; } -Eigen::Matrix6d calculate_J_dot(const vortex::utils::types::PoseEuler& pose, +Eigen::Matrix6d calculate_L_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::Matrix3d Q_dot = calculate_Q_dot(pose, twist); - Eigen::Matrix6d J_dot = Eigen::Matrix6d::Zero(); - J_dot.topLeftCorner<3, 3>() = R_dot; - J_dot.bottomRightCorner<3, 3>() = T_dot; + Eigen::Matrix6d L_dot = Eigen::Matrix6d::Zero(); + L_dot.topLeftCorner<3, 3>() = R_dot; + L_dot.bottomRightCorner<3, 3>() = Q_dot; - return J_dot; + return L_dot; } Eigen::Matrix6d calculate_coriolis(const double mass, From ecdb27ead920c6df215deaffdb63d98a0b841857 Mon Sep 17 00:00:00 2001 From: Cyprian Osinski Date: Sun, 29 Mar 2026 23:58:03 +0200 Subject: [PATCH 259/290] fixed --- auv_setup/config/robots/nautilus.yaml | 27 +++++++++++++-------------- 1 file changed, 13 insertions(+), 14 deletions(-) diff --git a/auv_setup/config/robots/nautilus.yaml b/auv_setup/config/robots/nautilus.yaml index 29ab42159..6efd6dbda 100644 --- a/auv_setup/config/robots/nautilus.yaml +++ b/auv_setup/config/robots/nautilus.yaml @@ -16,11 +16,10 @@ /**: ros__parameters: physical: - center_of_mass: [0.0, 0.0, 0.0] # CO is aligned with CM Position (x,y,z) in meters (M) + center_of_mass: [0.0, 0.0, 0.025] # CO is aligned with CM Position (x,y) in meters (M), small cg offset in z to keep drone naturally stable in roll/pitch mass_matrix: [53.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 53.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 53.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 11.0628, 1.086, -3.17502, 0.0, 0.0, 0.0, 1.086, 23.1128, 0.1025, 0.0, 0.0, 0.0, -3.17502, 0.1025, 26.23998] # 6x6 mass_inertia_matrix propulsion: - solver_type: "qp" dofs: num: 6 dimensions: @@ -54,22 +53,22 @@ 0.00000, # Heave ] thruster_position: [ # Position (x0,x1 ... x7,y1,y2, ...,y7,z1,z2, ... ,z7) in meters (M). i.e thruster0 has position (x0,y0,z0) - 0.413892, - 0.140095, - -0.163904, - -0.413892, - -0.413892, - -0.163904, - 0.140095, - 0.413892, # x-positions of the thrusters + 0.45015, + 0.24060, + -0.22970, + -0.43861, + -0.43861, + -0.22970, + 0.240600, + 0.450150, # x-positions of the thrusters + 0.305680, 0.313022, 0.313022, - 0.313022, - 0.313022, - -0.313022, + 0.305680, + -0.305680, -0.313022, -0.313022, - -0.313022, # y-positions of the thrusters + -0.305680, # y-positions of the thrusters 0.021736, 0.021736, 0.021736, From d075b6257be6cd1b4a08d01b61c84b34f1a638aa Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Tue, 31 Mar 2026 19:52:25 +0200 Subject: [PATCH 260/290] update imu tf --- auv_setup/description/nautilus.urdf.xacro | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/auv_setup/description/nautilus.urdf.xacro b/auv_setup/description/nautilus.urdf.xacro index 12b7aba27..f330fb6a2 100644 --- a/auv_setup/description/nautilus.urdf.xacro +++ b/auv_setup/description/nautilus.urdf.xacro @@ -18,7 +18,7 @@ - + From af998beb264c76c0d30686e7f4c2f04a1991e4e8 Mon Sep 17 00:00:00 2001 From: jorgenfj <144696109+jorgenfj@users.noreply.github.com> Date: Sat, 28 Mar 2026 19:22:07 +0100 Subject: [PATCH 261/290] Feat/solver and drone sim tests (#699) * rebase * waypoint sim test wrapper scripts * made it so that gripper does not have buoyancy or mass. Collision might be a bit wonky * rebase * waypoint sim test wrapper scripts * made it so that gripper does not have buoyancy or mass. Collision might be a bit wonky * changed so that drone type is resolved at launch for the adaptive dp backstepping controller, tuned the adaptive dp backstepping controller a bit for nautilus not yet finished. Added roll back into the controllable parameters, it looks good for now. --------- Co-authored-by: Cyprian Osinski --- .github/workflows/simulator-test.yml | 5 +- auv_setup/launch/dp.launch.py | 2 +- .../config/adapt_params.yaml | 10 ---- .../config/adapt_params_nautilus.yaml | 7 +++ .../config/adapt_params_orca.yaml | 7 +++ .../dp_adapt_backs_controller.launch.py | 2 +- control/dp_adapt_backs_controller/package.xml | 1 + .../src/dp_adapt_backs_controller.cpp | 2 +- .../src/dp_adapt_backs_controller_ros.cpp | 21 ++++--- .../test/CMakeLists.txt | 7 +++ .../test/dp_adapt_backs_controller_tests.cpp | 56 ++++++++++++++----- .../waypoint_navigation/check_goal.py | 5 +- .../nautilus_pseudoinverse.sh | 3 + .../waypoint_navigation/nautilus_qp.sh | 3 + .../waypoint_navigation/orca_pseudoinverse.sh | 3 + .../waypoint_navigation/orca_qp.sh | 3 + .../waypoint_navigation/send_goal.py | 5 +- .../waypoint_navigation/simulator_test.sh | 41 +++++++------- 18 files changed, 121 insertions(+), 62 deletions(-) delete mode 100644 control/dp_adapt_backs_controller/config/adapt_params.yaml create mode 100644 control/dp_adapt_backs_controller/config/adapt_params_nautilus.yaml create mode 100644 control/dp_adapt_backs_controller/config/adapt_params_orca.yaml create mode 100755 tests/simulator_tests/waypoint_navigation/nautilus_pseudoinverse.sh create mode 100755 tests/simulator_tests/waypoint_navigation/nautilus_qp.sh create mode 100755 tests/simulator_tests/waypoint_navigation/orca_pseudoinverse.sh create mode 100755 tests/simulator_tests/waypoint_navigation/orca_qp.sh diff --git a/.github/workflows/simulator-test.yml b/.github/workflows/simulator-test.yml index 5a310761e..c3a462f54 100644 --- a/.github/workflows/simulator-test.yml +++ b/.github/workflows/simulator-test.yml @@ -16,7 +16,10 @@ jobs: setup_script: "tests/setup.sh" pre_test_script: "scripts/ci_install_dependencies.sh" test_scripts: '[ - "tests/simulator_tests/waypoint_navigation/simulator_test.sh", + "tests/simulator_tests/waypoint_navigation/nautilus_qp.sh", + "tests/simulator_tests/waypoint_navigation/nautilus_pseudoinverse.sh", + "tests/simulator_tests/waypoint_navigation/orca_qp.sh", + "tests/simulator_tests/waypoint_navigation/orca_pseudoinverse.sh", "tests/simulator_tests/waypoint_manager_test/simulator_test.sh", "tests/simulator_tests/waypoint_navigation_quat/simulator_test.sh", ]' diff --git a/auv_setup/launch/dp.launch.py b/auv_setup/launch/dp.launch.py index c0901836c..f178cb1a7 100644 --- a/auv_setup/launch/dp.launch.py +++ b/auv_setup/launch/dp.launch.py @@ -31,7 +31,7 @@ def launch_setup(context, *args, **kwargs): adapt_params = os.path.join( get_package_share_directory("dp_adapt_backs_controller"), "config", - "adapt_params.yaml", + f"adapt_params_{drone}.yaml", ) container = ComposableNodeContainer( diff --git a/control/dp_adapt_backs_controller/config/adapt_params.yaml b/control/dp_adapt_backs_controller/config/adapt_params.yaml deleted file mode 100644 index 32fb79dd7..000000000 --- a/control/dp_adapt_backs_controller/config/adapt_params.yaml +++ /dev/null @@ -1,10 +0,0 @@ -/**: - ros__parameters: - K1 : [10.5, 10.5, 13.5, 0.0, 4.0, 4.0] - K2 : [20.5, 20.5, 20.5, 0.0, 5.5, 5.5] - r_b_bg : [0.01, 0.0, 0.02] - adapt_gain : [0.4, 0.1, 0.4, 0.1, 0.4, 0.1, 0.4, 0.1, 0.4, 0.1, 0.4, 0.1] - d_gain : [0.6, 0.6, 0.6, 0.6, 0.6, 0.6] - inertia_matrix : [0.68, 3.32, 3.34] - mass_matrix : [ 30.0, 0.0, 0.0, 0.0, 0.0, 0.6, 0.0, 30.0, 0.0, 0.0, -0.6, 0.3, 0.0, 0.0, 30.0, 0.6, 0.3, 0.0, 0.0, 0.0, 0.6, 0.68, 0.0, 0.0, 0.0, -0.6, 0.3, 0.0, 3.32, 0.0, 0.6, 0.3, 0.0, 0.0, 0.0, 3.34] - m : 30.0 diff --git a/control/dp_adapt_backs_controller/config/adapt_params_nautilus.yaml b/control/dp_adapt_backs_controller/config/adapt_params_nautilus.yaml new file mode 100644 index 000000000..65b639ed7 --- /dev/null +++ b/control/dp_adapt_backs_controller/config/adapt_params_nautilus.yaml @@ -0,0 +1,7 @@ +/**: + ros__parameters: + K1 : [15.0, 15.0, 15.0, 1.5, 6.0, 6.0] # Outer loop tuning parameters + K2 : [30.0, 30.0, 30.0, 2.0, 8.5, 8.5] # Inner loop tuning parameters + r_b_bg : [0.0, 0.0, 0.01] # Vector from body centre to centre of gravity + adapt_gain : [0.6, 0.15, 0.6, 0.15, 0.6, 0.15, 0.6, 0.15, 0.6, 0.15, 0.6, 0.15] # Tuning parameters for linear and nonlinear damping + d_gain : [0.8, 0.8, 0.8, 0.8, 0.8, 0.8] # Tuning parameters for the unmodeled disturbances and uncertanties diff --git a/control/dp_adapt_backs_controller/config/adapt_params_orca.yaml b/control/dp_adapt_backs_controller/config/adapt_params_orca.yaml new file mode 100644 index 000000000..e128131b2 --- /dev/null +++ b/control/dp_adapt_backs_controller/config/adapt_params_orca.yaml @@ -0,0 +1,7 @@ +/**: + ros__parameters: + K1 : [10.5, 10.5, 13.5, 0.0, 4.0, 4.0] # Outer loop tuning parameters + K2 : [20.5, 20.5, 20.5, 0.0, 5.5, 5.5] # Inner loop tuning parameters + r_b_bg : [0.01, 0.0, 0.02] # Vector from body centre to centre of gravity + adapt_gain : [0.4, 0.1, 0.4, 0.1, 0.4, 0.1, 0.4, 0.1, 0.4, 0.1, 0.4, 0.1] # Tuning parameters for linear and nonlinear damping + d_gain : [0.6, 0.6, 0.6, 0.6, 0.6, 0.6] # Tuning parameters for the unmodeled disturbances and uncertanties diff --git a/control/dp_adapt_backs_controller/launch/dp_adapt_backs_controller.launch.py b/control/dp_adapt_backs_controller/launch/dp_adapt_backs_controller.launch.py index 5dba5f5d6..fe3972bf8 100644 --- a/control/dp_adapt_backs_controller/launch/dp_adapt_backs_controller.launch.py +++ b/control/dp_adapt_backs_controller/launch/dp_adapt_backs_controller.launch.py @@ -17,7 +17,7 @@ def launch_setup(context, *args, **kwargs): adapt_params = os.path.join( get_package_share_directory("dp_adapt_backs_controller"), "config", - "adapt_params.yaml", + f"adapt_params_{drone}.yaml", ) drone_params = os.path.join( diff --git a/control/dp_adapt_backs_controller/package.xml b/control/dp_adapt_backs_controller/package.xml index 0b708c8b7..c618f6e06 100644 --- a/control/dp_adapt_backs_controller/package.xml +++ b/control/dp_adapt_backs_controller/package.xml @@ -17,6 +17,7 @@ vortex_msgs vortex_utils vortex_utils_ros + yaml-cpp ament_cmake 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 78102c0bf..e009d7f90 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 @@ -49,7 +49,7 @@ Eigen::Vector6d DPAdaptBacksController::calculate_tau(const PoseEuler& pose, (pose.as_j_matrix().transpose() * z_1) - (K2_ * z_2) - F_est - d_est_; - tau = tau.cwiseMax(-80.0).cwiseMin(80.0); + tau = tau.cwiseMax(-100.0).cwiseMin(100.0); adapt_param_ += adapt_param_dot * dt_; d_est_ += d_est_dot * dt_; adapt_param_ = adapt_param_.cwiseMax(-10.0).cwiseMin(10.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 d0c40d906..8e6c884ce 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 @@ -176,9 +176,7 @@ void DPAdaptBacksControllerNode::set_adap_params() { this->declare_parameter>("K1"); this->declare_parameter>("K2"); this->declare_parameter>("r_b_bg"); - this->declare_parameter>("inertia_matrix"); - this->declare_parameter>("mass_matrix"); - this->declare_parameter("m"); + this->declare_parameter>("physical.mass_matrix"); std::vector adapt_param_vec = this->get_parameter("adapt_gain").as_double_array(); @@ -188,12 +186,8 @@ void DPAdaptBacksControllerNode::set_adap_params() { std::vector K2_vec = this->get_parameter("K2").as_double_array(); std::vector r_b_bg_vec = this->get_parameter("r_b_bg").as_double_array(); - std::vector I_b_vec = - this->get_parameter("inertia_matrix").as_double_array(); std::vector mass_matrix_vec = - this->get_parameter("mass_matrix").as_double_array(); - - double mass{this->get_parameter("m").as_double()}; + this->get_parameter("physical.mass_matrix").as_double_array(); Eigen::Vector12d adapt_param_eigen = Eigen::Map(adapt_param_vec.data()); @@ -203,10 +197,13 @@ void DPAdaptBacksControllerNode::set_adap_params() { Eigen::Vector6d K2_eigen = Eigen::Map(K2_vec.data()); Eigen::Vector3d r_b_bg_eigen = Eigen::Map(r_b_bg_vec.data()); - Eigen::Vector3d I_b_eigen = Eigen::Map(I_b_vec.data()); Eigen::Matrix6d mass_matrix = Eigen::Map(mass_matrix_vec.data()); + double mass = mass_matrix(0, 0); + Eigen::Vector3d I_b_eigen(mass_matrix(3, 3), mass_matrix(4, 4), + mass_matrix(5, 5)); + DPAdaptParams dp_adapt_params; dp_adapt_params.adapt_param = adapt_param_eigen; dp_adapt_params.d_gain = d_gain_eigen; @@ -237,8 +234,10 @@ void DPAdaptBacksControllerNode::publish_tau() { tau_msg.wrench.force.x = tau(0); tau_msg.wrench.force.y = tau(1); tau_msg.wrench.force.z = tau(2); - // tau_msg.wrench.torque.x = tau(3); commented out since roll control is not - // needed and causes minor instability, if needed uncomment + + // comment out if roll control is not needed + tau_msg.wrench.torque.x = tau(3); + tau_msg.wrench.torque.y = tau(4); tau_msg.wrench.torque.z = tau(5); diff --git a/control/dp_adapt_backs_controller/test/CMakeLists.txt b/control/dp_adapt_backs_controller/test/CMakeLists.txt index 2c7994231..5b97abfbf 100644 --- a/control/dp_adapt_backs_controller/test/CMakeLists.txt +++ b/control/dp_adapt_backs_controller/test/CMakeLists.txt @@ -1,6 +1,7 @@ cmake_minimum_required(VERSION 3.8) find_package(GTest REQUIRED) +find_package(yaml-cpp REQUIRED) include(GoogleTest) set(TEST_BINARY_NAME ${PROJECT_NAME}_test) @@ -9,10 +10,16 @@ add_executable( dp_adapt_backs_controller_tests.cpp ) +target_compile_definitions(${TEST_BINARY_NAME} PRIVATE + DRONE_YAML_PATH="${CMAKE_CURRENT_SOURCE_DIR}/../../../auv_setup/config/robots/nautilus.yaml" + CONTROLLER_YAML_PATH="${CMAKE_CURRENT_SOURCE_DIR}/../config/adapt_params_nautilus.yaml" +) + target_link_libraries( ${TEST_BINARY_NAME} PRIVATE ${LIB_NAME} + yaml-cpp GTest::GTest ) 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 b3adf9d17..01232bdd3 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 @@ -1,4 +1,5 @@ #include +#include #include @@ -11,23 +12,44 @@ namespace vortex::control { using vortex::utils::types::PoseEuler; using vortex::utils::types::Twist; +DPAdaptParams load_dp_adapt_params(const std::string& drone_yaml_path, + const std::string& controller_yaml_path) { + YAML::Node drone_params = + YAML::LoadFile(drone_yaml_path)["/**"]["ros__parameters"]; + YAML::Node controller_params = + YAML::LoadFile(controller_yaml_path)["/**"]["ros__parameters"]; + + auto K1_vec = controller_params["K1"].as>(); + auto K2_vec = controller_params["K2"].as>(); + auto adapt_gain_vec = + controller_params["adapt_gain"].as>(); + auto d_gain_vec = controller_params["d_gain"].as>(); + auto r_b_bg_vec = controller_params["r_b_bg"].as>(); + + auto mass_matrix_vec = + drone_params["physical"]["mass_matrix"].as>(); + + Eigen::Matrix6d mass_matrix = + Eigen::Map(mass_matrix_vec.data()); + + DPAdaptParams params; + params.K1 = Eigen::Map(K1_vec.data()); + params.K2 = Eigen::Map(K2_vec.data()); + params.adapt_param = Eigen::Map(adapt_gain_vec.data()); + params.d_gain = Eigen::Map(d_gain_vec.data()); + params.r_b_bg = Eigen::Map(r_b_bg_vec.data()); + params.mass_matrix = mass_matrix; + params.mass = mass_matrix(0, 0); + params.I_b = Eigen::Vector3d(mass_matrix(3, 3), mass_matrix(4, 4), + mass_matrix(5, 5)); + return params; +} + class DPAdaptBacksControllerTests : public ::testing::Test { protected: DPAdaptBacksControllerTests() - : dp_adapt_backs_controller_{get_dp_params()} {} - - DPAdaptParams get_dp_params() { - DPAdaptParams params; - params.adapt_param = Eigen::Vector12d::Ones() * 0.1; - params.d_gain = Eigen::Vector6d::Ones() * 0.6; - params.K1 = Eigen::Vector6d::Ones() * 5.0; - params.K2 = Eigen::Vector6d::Ones() * 5.0; - params.r_b_bg = Eigen::Vector3d(0.01, 0.0, 0.02); - params.I_b = Eigen::Vector3d(0.68, 3.32, 3.34); - params.mass_matrix = Eigen::Matrix6d::Identity() * 30.0; - params.mass = 30.0; - return params; - } + : dp_adapt_backs_controller_{ + load_dp_adapt_params(DRONE_YAML_PATH, CONTROLLER_YAML_PATH)} {} PoseEuler generate_current_pose(const double north_pos, const double east_pos, @@ -395,6 +417,8 @@ TEST_F(DPAdaptBacksControllerTests, EXPECT_NEAR(tau[3], 0.0, 0.01); EXPECT_NEAR(tau[4], 0.0, 0.01); EXPECT_NEAR(tau[5], 0.0, 0.01); + // Torque channels may have cross-coupling from off-diagonal mass matrix + // terms in the Coriolis matrix } /* @@ -415,6 +439,8 @@ TEST_F(DPAdaptBacksControllerTests, EXPECT_NEAR(tau[3], 0.0, 0.01); EXPECT_NEAR(tau[4], 0.0, 0.01); EXPECT_NEAR(tau[5], 0.0, 0.01); + // Torque channels may have cross-coupling from off-diagonal mass matrix + // terms in the Coriolis matrix } /* @@ -435,6 +461,8 @@ TEST_F(DPAdaptBacksControllerTests, EXPECT_NEAR(tau[3], 0.0, 0.01); EXPECT_NEAR(tau[4], 0.0, 0.01); EXPECT_NEAR(tau[5], 0.0, 0.01); + // Torque channels may have cross-coupling from off-diagonal mass matrix + // terms in the Coriolis matrix } } // namespace vortex::control diff --git a/tests/simulator_tests/waypoint_navigation/check_goal.py b/tests/simulator_tests/waypoint_navigation/check_goal.py index 69bf0a71a..995c27f6c 100644 --- a/tests/simulator_tests/waypoint_navigation/check_goal.py +++ b/tests/simulator_tests/waypoint_navigation/check_goal.py @@ -1,5 +1,6 @@ import math import os +import sys import time import rclpy @@ -9,6 +10,8 @@ from rclpy.qos import QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy from vortex_utils.python_utils import quat_to_euler +namespace = sys.argv[1] if len(sys.argv) > 1 else "nautilus" + best_effort_qos = QoSProfile( history=QoSHistoryPolicy.KEEP_LAST, depth=1, @@ -35,7 +38,7 @@ def __init__(self): super().__init__('check_goal_node') self.pose_sub_ = self.create_subscription( PoseWithCovarianceStamped, - '/nautilus/pose', + f'/{namespace}/pose', self.pose_callback, best_effort_qos, ) diff --git a/tests/simulator_tests/waypoint_navigation/nautilus_pseudoinverse.sh b/tests/simulator_tests/waypoint_navigation/nautilus_pseudoinverse.sh new file mode 100755 index 000000000..0932a998e --- /dev/null +++ b/tests/simulator_tests/waypoint_navigation/nautilus_pseudoinverse.sh @@ -0,0 +1,3 @@ +#!/bin/bash +SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" +exec "$SCRIPT_DIR/simulator_test.sh" nautilus pseudoinverse diff --git a/tests/simulator_tests/waypoint_navigation/nautilus_qp.sh b/tests/simulator_tests/waypoint_navigation/nautilus_qp.sh new file mode 100755 index 000000000..1da7ceaa7 --- /dev/null +++ b/tests/simulator_tests/waypoint_navigation/nautilus_qp.sh @@ -0,0 +1,3 @@ +#!/bin/bash +SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" +exec "$SCRIPT_DIR/simulator_test.sh" nautilus qp diff --git a/tests/simulator_tests/waypoint_navigation/orca_pseudoinverse.sh b/tests/simulator_tests/waypoint_navigation/orca_pseudoinverse.sh new file mode 100755 index 000000000..9e84147b9 --- /dev/null +++ b/tests/simulator_tests/waypoint_navigation/orca_pseudoinverse.sh @@ -0,0 +1,3 @@ +#!/bin/bash +SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" +exec "$SCRIPT_DIR/simulator_test.sh" orca pseudoinverse diff --git a/tests/simulator_tests/waypoint_navigation/orca_qp.sh b/tests/simulator_tests/waypoint_navigation/orca_qp.sh new file mode 100755 index 000000000..9cd5c9cbc --- /dev/null +++ b/tests/simulator_tests/waypoint_navigation/orca_qp.sh @@ -0,0 +1,3 @@ +#!/bin/bash +SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" +exec "$SCRIPT_DIR/simulator_test.sh" orca qp diff --git a/tests/simulator_tests/waypoint_navigation/send_goal.py b/tests/simulator_tests/waypoint_navigation/send_goal.py index c9d7287ad..93c17b57b 100644 --- a/tests/simulator_tests/waypoint_navigation/send_goal.py +++ b/tests/simulator_tests/waypoint_navigation/send_goal.py @@ -1,4 +1,5 @@ import random +import sys import rclpy import yaml @@ -7,6 +8,8 @@ from vortex_msgs.action import ReferenceFilterWaypoint from vortex_utils.python_utils import PoseData, euler_to_quat +namespace = sys.argv[1] if len(sys.argv) > 1 else "nautilus" + def randomize_pose() -> PoseData: pose: PoseData = PoseData() @@ -25,7 +28,7 @@ def __init__(self): super().__init__('reference_filter_waypoint_client') self._action_client = ActionClient( - self, ReferenceFilterWaypoint, '/nautilus/reference_filter' + self, ReferenceFilterWaypoint, f'/{namespace}/reference_filter' ) self.send_goal() diff --git a/tests/simulator_tests/waypoint_navigation/simulator_test.sh b/tests/simulator_tests/waypoint_navigation/simulator_test.sh index 79c2db117..e85c34216 100755 --- a/tests/simulator_tests/waypoint_navigation/simulator_test.sh +++ b/tests/simulator_tests/waypoint_navigation/simulator_test.sh @@ -2,6 +2,10 @@ set -e set -o pipefail +DRONE="${1:-nautilus}" +SOLVER_TYPE="${2:-qp}" + +echo "===== Waypoint Navigation Test: drone=$DRONE, solver_type=$SOLVER_TYPE =====" echo "Setting up ROS 2 environment..." . /opt/ros/humble/setup.sh . "${WORKSPACE:-$HOME/ros2_ws}/install/setup.bash" @@ -13,30 +17,25 @@ SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" # Function to terminate processes safely on error cleanup() { echo "Error detected. Cleaning up..." - kill -TERM -"$SIM_PID" -"$NAUTILUS_PID" -"$CONTROLLER_PID" -"$FILTER_PID" -"$OP_MODE_PID" || true + kill -TERM -"$SIM_PID" -"$CONTROLLER_PID" || true exit 1 } trap cleanup ERR -setsid ros2 bag record -o ${WORKSPACE}/bags/recording -s mcap -a & +setsid ros2 bag record -o ${WORKSPACE}/bags/recording_${DRONE}_${SOLVER_TYPE} -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:=nautilus_no_gpu & +# Launch Stonefish Simulator + Drone (combined launch) +setsid ros2 launch stonefish_sim vortex_sim_launch.py rendering:=false scenario:=${DRONE}_no_gpu drone:=${DRONE} solver_type:=${SOLVER_TYPE} & SIM_PID=$! echo "Launched simulator with PID: $SIM_PID" -# Launch NAUTILUS Simulation -setsid ros2 launch stonefish_sim drone_sim.launch.py & -NAUTILUS_PID=$! -echo "Launched nautilus with PID: $NAUTILUS_PID" - echo "Waiting for simulator to start..." -timeout 30s bash -c ' - while ! ros2 topic list | grep -q "/nautilus/odom"; do +timeout 30s bash -c " + while ! ros2 topic list | grep -q '/${DRONE}/odom'; do sleep 1 - done || true' + done || true" echo "Simulator started" # Check for ROS errors in logs @@ -47,11 +46,11 @@ fi # Wait for odometry data echo "Waiting for odom data..." -timeout 10s ros2 topic echo /nautilus/odom --once +timeout 10s ros2 topic echo /${DRONE}/odom --once echo "Got odom data" echo "Waiting for sim interface to start..." -timeout 30s bash -c 'until ros2 topic list | grep -q "/nautilus/pose"; do sleep 1; done' +timeout 30s bash -c "until ros2 topic list | grep -q '/${DRONE}/pose'; do sleep 1; done" echo "Simulator started" # Check for ROS errors again @@ -62,11 +61,11 @@ fi # Wait for pose data echo "Waiting for pose data..." -timeout 10s ros2 topic echo /nautilus/pose --once +timeout 10s ros2 topic echo /${DRONE}/pose --once echo "Got pose data" # Launch controller and reference filter -setsid ros2 launch auv_setup dp.launch.py & +setsid ros2 launch auv_setup dp.launch.py drone:=${DRONE} & CONTROLLER_PID=$! echo "Launched controller and reference filter with PID: $CONTROLLER_PID" @@ -81,19 +80,19 @@ sleep 5 # Set operation mode echo "Turning off killswitch and setting operation mode to autonomous mode" -ros2 service call /nautilus/set_killswitch vortex_msgs/srv/SetKillswitch "{killswitch_on: false}" -ros2 service call /nautilus/set_operation_mode vortex_msgs/srv/SetOperationMode "{requested_operation_mode: {operation_mode: 1}}" +ros2 service call /${DRONE}/set_killswitch vortex_msgs/srv/SetKillswitch "{killswitch_on: false}" +ros2 service call /${DRONE}/set_operation_mode vortex_msgs/srv/SetOperationMode "{requested_operation_mode: {operation_mode: 1}}" echo "Sleeping for 5 seconds to make sure operation is stable..." sleep 5 # Send waypoint goal echo "Sending goal" -python3 "$SCRIPT_DIR/send_goal.py" +python3 "$SCRIPT_DIR/send_goal.py" "$DRONE" # Check if goal reached echo "Checking if goal reached" -python3 "$SCRIPT_DIR/check_goal.py" +python3 "$SCRIPT_DIR/check_goal.py" "$DRONE" if [ $? -ne 0 ]; then echo "Test failed: Drone did not reach goal." @@ -103,6 +102,6 @@ else fi # Terminate processes -kill -TERM -"$SIM_PID" -"$NAUTILUS_PID" -"$CONTROLLER_PID" -"$BAG_PID" -"$OP_MODE_PID" +kill -TERM -"$SIM_PID" -"$CONTROLLER_PID" -"$BAG_PID" echo "Test completed successfully." From e51e3d86f929d4bc7285df6cde3b43e775031a8b Mon Sep 17 00:00:00 2001 From: Cyprian Osinski Date: Wed, 1 Apr 2026 18:49:50 +0200 Subject: [PATCH 262/290] claude fix dp to be composable with ref filter --- auv_setup/launch/dp.launch.py | 127 ++++++++++++------ control/pid_controller_dp/CMakeLists.txt | 46 ++++--- .../pid_controller_dp/config/pid_params.yaml | 6 +- .../pid_controller_dp/pid_controller_ros.hpp | 2 +- control/pid_controller_dp/package.xml | 1 + .../src/pid_controller_node.cpp | 12 +- .../src/pid_controller_ros.cpp | 17 ++- 7 files changed, 138 insertions(+), 73 deletions(-) diff --git a/auv_setup/launch/dp.launch.py b/auv_setup/launch/dp.launch.py index c0901836c..89469ffab 100644 --- a/auv_setup/launch/dp.launch.py +++ b/auv_setup/launch/dp.launch.py @@ -1,11 +1,10 @@ import os - from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import OpaqueFunction -from launch_ros.actions import ComposableNodeContainer +from launch.actions import OpaqueFunction, DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer, Node from launch_ros.descriptions import ComposableNode - from auv_setup.launch_arg_common import ( declare_drone_and_namespace_args, resolve_drone_and_namespace, @@ -14,12 +13,7 @@ def launch_setup(context, *args, **kwargs): drone, namespace = resolve_drone_and_namespace(context) - - filter_file_path = os.path.join( - get_package_share_directory("reference_filter_dp"), - "config", - "reference_filter_params.yaml", - ) + controller_type = LaunchConfiguration("controller_type").perform(context).lower() drone_params = os.path.join( get_package_share_directory("auv_setup"), @@ -27,47 +21,98 @@ def launch_setup(context, *args, **kwargs): "robots", f"{drone}.yaml", ) - - adapt_params = os.path.join( - get_package_share_directory("dp_adapt_backs_controller"), + filter_file_path = os.path.join( + get_package_share_directory("reference_filter_dp"), "config", - "adapt_params.yaml", + "reference_filter_params.yaml", ) - container = ComposableNodeContainer( - name="dp_container", - namespace=namespace, - package="rclcpp_components", - executable="component_container_mt", - composable_node_descriptions=[ - ComposableNode( - package="reference_filter_dp", - plugin="ReferenceFilterNode", - name="reference_filter_node", - namespace=namespace, - parameters=[filter_file_path, drone_params], - extra_arguments=[{"use_intra_process_comms": True}], - ), - ComposableNode( - package="dp_adapt_backs_controller", - plugin="DPAdaptBacksControllerNode", - name="dp_adapt_backs_controller_node", - namespace=namespace, - parameters=[adapt_params, drone_params], - extra_arguments=[{"use_intra_process_comms": True}], - ), - ], - output="screen", - arguments=["--ros-args", "--log-level", "error"], - ) + nodes = [] + + if controller_type == "adaptive": + adapt_params = os.path.join( + get_package_share_directory("dp_adapt_backs_controller"), + "config", + "adapt_params.yaml", + ) + container = ComposableNodeContainer( + name="dp_container", + namespace=namespace, + package="rclcpp_components", + executable="component_container_mt", + composable_node_descriptions=[ + ComposableNode( + package="reference_filter_dp", + plugin="ReferenceFilterNode", + name="reference_filter_node", + namespace=namespace, + parameters=[filter_file_path, drone_params], + extra_arguments=[{"use_intra_process_comms": True}], + ), + ComposableNode( + package="dp_adapt_backs_controller", + plugin="DPAdaptBacksControllerNode", + name="dp_adapt_backs_controller_node", + namespace=namespace, + parameters=[adapt_params, drone_params], + extra_arguments=[{"use_intra_process_comms": True}], + ), + ], + output="screen", + arguments=["--ros-args", "--log-level", "error"], + ) + nodes.append(container) - return [container] + elif controller_type == "pid": + pid_params = os.path.join( + get_package_share_directory("pid_controller_dp"), + "config", + "pid_params.yaml", + ) + container = ComposableNodeContainer( + name="dp_container", + namespace=namespace, + package="rclcpp_components", + executable="component_container_mt", + composable_node_descriptions=[ + ComposableNode( + package="reference_filter_dp", + plugin="ReferenceFilterNode", + name="reference_filter_node", + namespace=namespace, + parameters=[filter_file_path, drone_params], + extra_arguments=[{"use_intra_process_comms": True}], + ), + ComposableNode( + package="pid_controller_dp", + plugin="PIDControllerNode", + name="pid_controller_node", + namespace=namespace, + parameters=[pid_params, drone_params], + extra_arguments=[{"use_intra_process_comms": True}], + ), + ], + output="screen", + ) + nodes.append(container) + else: + raise ValueError( + f"Unknown controller_type '{controller_type}'. " + f"Expected 'adaptive' or 'pid'." + ) + + return nodes def generate_launch_description(): return LaunchDescription( declare_drone_and_namespace_args() + [ + DeclareLaunchArgument( + "controller_type", + default_value="adaptive", + description="Controller to use: 'adaptive' or 'pid'", + ), OpaqueFunction(function=launch_setup), ] ) diff --git a/control/pid_controller_dp/CMakeLists.txt b/control/pid_controller_dp/CMakeLists.txt index 80f9e132e..481c7ad31 100644 --- a/control/pid_controller_dp/CMakeLists.txt +++ b/control/pid_controller_dp/CMakeLists.txt @@ -11,6 +11,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) find_package(nav_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(Eigen3 REQUIRED) @@ -46,18 +47,13 @@ ament_target_dependencies(${LIB_NAME} PUBLIC ) -install(TARGETS - ${LIB_NAME} - DESTINATION lib/${PROJECT_NAME} -) - -add_executable(pid_controller_node - src/pid_controller_node.cpp +add_library(pid_controller_component SHARED src/pid_controller_ros.cpp ) -ament_target_dependencies(pid_controller_node +ament_target_dependencies(pid_controller_component PUBLIC rclcpp + rclcpp_components geometry_msgs nav_msgs Eigen3 @@ -70,11 +66,23 @@ ament_target_dependencies(pid_controller_node fmt ) -target_link_libraries( - pid_controller_node - ${LIB_NAME} - spdlog::spdlog - # vortex_utilis::vortex_utils +target_link_libraries(pid_controller_component PUBLIC + ${LIB_NAME} + spdlog::spdlog +) + +rclcpp_components_register_node(pid_controller_component + PLUGIN "PIDControllerNode" + EXECUTABLE pid_controller_node_component +) + +add_executable(pid_controller_node + src/pid_controller_node.cpp +) + +target_link_libraries(pid_controller_node PRIVATE + pid_controller_component + spdlog::spdlog ) ament_export_targets(export_${LIB_NAME}) @@ -86,9 +94,15 @@ install(TARGETS ${LIB_NAME} RUNTIME DESTINATION bin ) -install(TARGETS - pid_controller_node - DESTINATION lib/${PROJECT_NAME}) +install(TARGETS pid_controller_component + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(TARGETS pid_controller_node + DESTINATION lib/${PROJECT_NAME} +) install(DIRECTORY config diff --git a/control/pid_controller_dp/config/pid_params.yaml b/control/pid_controller_dp/config/pid_params.yaml index 17bcd5026..0a3c8268a 100644 --- a/control/pid_controller_dp/config/pid_params.yaml +++ b/control/pid_controller_dp/config/pid_params.yaml @@ -1,8 +1,8 @@ /**: ros__parameters: - Kp_x: 20.0 - Kp_y: 26.0 - Kp_z: 50.0 + Kp_x: 10.0 + Kp_y: 5.0 + Kp_z: 30.0 Kp_roll: 10.0 Kp_pitch: 41.0 Kp_yaw: 6.0 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 616f1ec76..4c631e3e9 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 @@ -24,7 +24,7 @@ // @brief Class for the PID controller node class PIDControllerNode : public rclcpp::Node { public: - PIDControllerNode(); + explicit PIDControllerNode(const rclcpp::NodeOptions & options); private: // @brief Callback function for the killswitch topic diff --git a/control/pid_controller_dp/package.xml b/control/pid_controller_dp/package.xml index 3d5abc768..0ec8c9b80 100644 --- a/control/pid_controller_dp/package.xml +++ b/control/pid_controller_dp/package.xml @@ -10,6 +10,7 @@ ament_cmake rclcpp + rclcpp_components geometry_msgs nav_msgs eigen diff --git a/control/pid_controller_dp/src/pid_controller_node.cpp b/control/pid_controller_dp/src/pid_controller_node.cpp index 46d51b1ee..433ddf4bf 100644 --- a/control/pid_controller_dp/src/pid_controller_node.cpp +++ b/control/pid_controller_dp/src/pid_controller_node.cpp @@ -1,18 +1,8 @@ -#include #include "pid_controller_dp/pid_controller_ros.hpp" -auto start_msg = R"( - ____ ___ ____ ____ _ _ _ - | _ \_ _| _ \ / ___|___ _ __ | |_ _ __ ___ | | | ___ _ __ - | |_) | || | | | | | / _ \| '_ \| __| '__/ _ \| | |/ _ \ '__| - | __/| || |_| | | |__| (_) | | | | |_| | | (_) | | | __/ | - |_| |___|____/ \____\___/|_| |_|\__|_| \___/|_|_|\___|_| -)"; - int main(int argc, char** argv) { rclcpp::init(argc, argv); - spdlog::info(start_msg); - rclcpp::spin(std::make_shared()); + rclcpp::spin(std::make_shared(rclcpp::NodeOptions())); rclcpp::shutdown(); return 0; } diff --git a/control/pid_controller_dp/src/pid_controller_ros.cpp b/control/pid_controller_dp/src/pid_controller_ros.cpp index c473956c9..42290f13d 100644 --- a/control/pid_controller_dp/src/pid_controller_ros.cpp +++ b/control/pid_controller_dp/src/pid_controller_ros.cpp @@ -1,4 +1,5 @@ #include +#include #include #include #include @@ -8,7 +9,17 @@ #include "pid_controller_dp/pid_controller_utils.hpp" #include "pid_controller_dp/typedefs.hpp" -PIDControllerNode::PIDControllerNode() : Node("pid_controller_node") { +constexpr std::string_view start_message = R"( + ____ ___ ____ ____ _ _ _ + | _ \_ _| _ \ / ___|___ _ __ | |_ _ __ ___ | | | ___ _ __ + | |_) | || | | | | | / _ \| '_ \| __| '__/ _ \| | |/ _ \ '__| + | __/| || |_| | | |__| (_) | | | | |_| | | (_) | | | __/ | + |_| |___|____/ \____\___/|_| |_|\__|_| \___/|_|_|\___|_| + +)"; + +PIDControllerNode::PIDControllerNode(const rclcpp::NodeOptions & options) + : Node("pid_controller_node", options) { time_step_ = std::chrono::milliseconds(10); set_subscribers_and_publisher(); @@ -20,6 +31,8 @@ PIDControllerNode::PIDControllerNode() : Node("pid_controller_node") { callback_handle_ = this->add_on_set_parameters_callback(std::bind( &PIDControllerNode::parametersCallback, this, std::placeholders::_1)); + + spdlog::info(start_message); } void PIDControllerNode::set_subscribers_and_publisher() { @@ -343,3 +356,5 @@ rcl_interfaces::msg::SetParametersResult PIDControllerNode::parametersCallback( } return result; } + +RCLCPP_COMPONENTS_REGISTER_NODE(PIDControllerNode) From 77f50d1ab3762a11ed51daf0de7cb3b37ebbed99 Mon Sep 17 00:00:00 2001 From: Cyprian Osinski Date: Wed, 1 Apr 2026 19:58:55 +0200 Subject: [PATCH 263/290] changed to eskf topic odom --- auv_setup/config/robots/nautilus.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/auv_setup/config/robots/nautilus.yaml b/auv_setup/config/robots/nautilus.yaml index 6efd6dbda..e736e1c82 100644 --- a/auv_setup/config/robots/nautilus.yaml +++ b/auv_setup/config/robots/nautilus.yaml @@ -106,7 +106,7 @@ joy: "joy" pose: "pose" twist: "twist" - odom: "odom" + odom: "odom_eskf" operation_mode: "operation_mode" killswitch: "killswitch" reference_pose: "reference_pose" From 5977225ed20d0f83c6eaba147882837b89fda24d Mon Sep 17 00:00:00 2001 From: Cyprian Osinski Date: Wed, 1 Apr 2026 22:08:53 +0200 Subject: [PATCH 264/290] some changes to dp_controller_backstepping --- .../dp_adapt_backs_controller.hpp | 2 + .../src/dp_adapt_backs_controller.cpp | 3 +- .../src/dp_adapt_backs_controller_ros.cpp | 61 +++++++++++++++++++ 3 files changed, 65 insertions(+), 1 deletion(-) 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 a26969cee..fd87d998b 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 @@ -15,6 +15,7 @@ struct DPAdaptParams { Eigen::Vector3d r_b_bg = Eigen::Vector3d::Zero(); Eigen::Vector3d I_b = Eigen::Vector3d::Zero(); Eigen::Matrix6d mass_matrix = Eigen::Matrix6d::Zero(); + Eigen::Vector6d tau_max = Eigen::Vector6d::Ones(); double mass{}; }; @@ -54,6 +55,7 @@ class DPAdaptBacksController { Eigen::Vector6d d_est_; Eigen::Matrix3d I_b_; Eigen::Matrix6d mass_matrix_; + Eigen::Vector6d tau_max_; double m_{}; double dt_{}; }; 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 e009d7f90..54ae228f1 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 @@ -21,6 +21,7 @@ DPAdaptBacksController::DPAdaptBacksController( d_est_(Eigen::Vector6d::Zero()), I_b_(dp_adapt_params.I_b.asDiagonal().toDenseMatrix()), mass_matrix_(dp_adapt_params.mass_matrix), + tau_max_(dp_adapt_params.tau_max), m_(dp_adapt_params.mass), dt_(0.01) {} @@ -49,7 +50,7 @@ Eigen::Vector6d DPAdaptBacksController::calculate_tau(const PoseEuler& pose, (pose.as_j_matrix().transpose() * z_1) - (K2_ * z_2) - F_est - d_est_; - tau = tau.cwiseMax(-100.0).cwiseMin(100.0); + tau = tau.cwiseMax(-tau_max_).cwiseMin(tau_max_); adapt_param_ += adapt_param_dot * dt_; d_est_ += d_est_dot * dt_; adapt_param_ = adapt_param_.cwiseMax(-10.0).cwiseMin(10.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 8e6c884ce..508d45750 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 @@ -177,6 +177,17 @@ void DPAdaptBacksControllerNode::set_adap_params() { this->declare_parameter>("K2"); this->declare_parameter>("r_b_bg"); this->declare_parameter>("physical.mass_matrix"); + this->declare_parameter>("physical.center_of_mass"); + this->declare_parameter>( + "propulsion.thrusters.thruster_force_direction"); + this->declare_parameter>( + "propulsion.thrusters.thruster_position"); + this->declare_parameter("propulsion.thrusters.num"); + this->declare_parameter("propulsion.dimensions.num"); + this->declare_parameter( + "propulsion.thrusters.constraints.min_force"); + this->declare_parameter( + "propulsion.thrusters.constraints.max_force"); std::vector adapt_param_vec = this->get_parameter("adapt_gain").as_double_array(); @@ -204,6 +215,55 @@ void DPAdaptBacksControllerNode::set_adap_params() { Eigen::Vector3d I_b_eigen(mass_matrix(3, 3), mass_matrix(4, 4), mass_matrix(5, 5)); + // Compute per-DOF max wrench from the thruster configuration + int num_thrusters = + this->get_parameter("propulsion.thrusters.num").as_int(); + int num_dims = this->get_parameter("propulsion.dimensions.num").as_int(); + double min_force = this->get_parameter( + "propulsion.thrusters.constraints.min_force") + .as_double(); + double max_force = this->get_parameter( + "propulsion.thrusters.constraints.max_force") + .as_double(); + + Eigen::Vector3d center_of_mass = Eigen::Map( + this->get_parameter("physical.center_of_mass") + .as_double_array() + .data()); + + auto dir_vec = this->get_parameter( + "propulsion.thrusters.thruster_force_direction") + .as_double_array(); + auto pos_vec = + this->get_parameter("propulsion.thrusters.thruster_position") + .as_double_array(); + + Eigen::MatrixXd thruster_dir = + Eigen::Map>( + dir_vec.data(), num_dims, num_thrusters); + Eigen::MatrixXd thruster_pos = + Eigen::Map>( + pos_vec.data(), num_dims, num_thrusters); + + Eigen::MatrixXd T = Eigen::MatrixXd::Zero(6, num_thrusters); + for (int i = 0; i < num_thrusters; i++) { + Eigen::Vector3d pos = thruster_pos.col(i) - center_of_mass; + Eigen::Vector3d F = thruster_dir.col(i); + T.block<3, 1>(0, i) = F; + T.block<3, 1>(3, i) = pos.cross(F); + } + + Eigen::Vector6d tau_max; + for (int i = 0; i < 6; i++) { + double w = 0.0; + for (int j = 0; j < num_thrusters; j++) { + w += (T(i, j) > 0) ? T(i, j) * max_force : T(i, j) * min_force; + } + tau_max(i) = w; + } + DPAdaptParams dp_adapt_params; dp_adapt_params.adapt_param = adapt_param_eigen; dp_adapt_params.d_gain = d_gain_eigen; @@ -212,6 +272,7 @@ void DPAdaptBacksControllerNode::set_adap_params() { dp_adapt_params.r_b_bg = r_b_bg_eigen; dp_adapt_params.I_b = I_b_eigen; dp_adapt_params.mass_matrix = mass_matrix; + dp_adapt_params.tau_max = tau_max; dp_adapt_params.mass = mass; dp_adapt_backs_controller_ = From b9714b623ae6cd9a4789ee32c30d42a2d26676d6 Mon Sep 17 00:00:00 2001 From: Cyprian Osinski Date: Wed, 1 Apr 2026 22:26:50 +0200 Subject: [PATCH 265/290] added utility scripts --- utility_scripts/build_ws_drone.sh | 164 ++++++++++++++++++++++ utility_scripts/build_ws_topside.sh | 172 ++++++++++++++++++++++++ utility_scripts/launch_drone_topside.sh | 57 ++++++++ utility_scripts/launch_drone_vehicle.sh | 70 ++++++++++ 4 files changed, 463 insertions(+) create mode 100755 utility_scripts/build_ws_drone.sh create mode 100755 utility_scripts/build_ws_topside.sh create mode 100755 utility_scripts/launch_drone_topside.sh create mode 100755 utility_scripts/launch_drone_vehicle.sh diff --git a/utility_scripts/build_ws_drone.sh b/utility_scripts/build_ws_drone.sh new file mode 100755 index 000000000..ce6a1ea96 --- /dev/null +++ b/utility_scripts/build_ws_drone.sh @@ -0,0 +1,164 @@ +#!/bin/bash +set -e + +WS_DIR="$HOME/vscopium/ros2_ws" +SRC_DIR="$WS_DIR/src" +GITHUB_ORG="https://github.com/vortexntnu" + +# Repos to clone +REPOS=( + "vortex-auv" + "vortex-msgs" + "vortex-utils" +) + +# ---- Install dependencies script ---- + +# python3 -m pip install --upgrade pip +# install numpy, pynput, joy, wheel +# install ros-humble-xacro, ros-humble-joy +# run casadi install script + +# ------------ Clone missing repos ------------ +mkdir -p "$SRC_DIR" + +for repo in "${REPOS[@]}"; do + if [ -d "$SRC_DIR/$repo" ]; then + echo "[SKIP] $SRC_DIR/$repo already exists" + else + echo "[CLONE] $GITHUB_ORG/$repo.git -> $SRC_DIR/$repo" + git clone "$GITHUB_ORG/$repo.git" "$SRC_DIR/$repo" + fi +done + +# ---------- Build ---------- +cd "$WS_DIR" + +# Default packages to build +CONTROL_PKGS=( + vortex_msgs + vortex_utils + vortex_utils_ros + thrust_allocator_auv + thruster_interface_auv + dp_adapt_backs_controller + pid_controller_dp + reference_filter_dp + auv_setup + operation_mode_manager + los_guidance +) + +#TODO: add perception pkgs and a seperate launch arg. +PERCEPTION_PKGS=() + +# Use arguments if provided, otherwise use defaults +if [ $# -gt 0 ]; then + PKGS=("$@") +else + PKGS=("${CONTROL_PKGS[@]}") +fi + +BUILD_FAILED=0 +echo "[BUILD] Building packages: ${PKGS[*]}" +colcon build \ + --packages-up-to "${PKGS[@]}" \ + --symlink-install \ + --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON || + BUILD_FAILED=1 + +# ---------- Handle result ---------- +if [ $BUILD_FAILED -eq 1 ]; then + echo "" + echo " ╔══════════════════════════════════════════════════╗ " + echo " ║ BUILD FAILED ║ " + echo " ╚══════════════════════════════════════════════════╝ " + echo "" + echo "⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⡖⢀⠀⠒⠒⠒⠒⠒⠒⠒⠀⠀⠀⠀⠀⠀⠀⡀⠀⠀⠀⠀⠀⠀⠀" + echo "⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⠿⠛⠙⠉⠉⠉⠉⠉⠉⠉⠙⠃⠀⠀⠠⠄⠂⠐⠀⠀⠀⠀⠀⠀⠀⠉⠁⠀⠀⠀⢸⠀⠀⠀⠀⠀" + echo "⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⡿⠋⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢸⠀⠀⠀⠀⠀" + echo "⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⡟⠀⠀⠀⠀⠀⠀⠀⡀⣀⢀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠈⠀⠀⠀⠀⠀" + echo "⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⡟⠀⠀⠀⠀⠀⢀⣶⢿⣵⣮⣞⣶⣆⡶⣴⣀⠀⠀⠀⠀⠀⠀⠡⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" + echo "⠉⠉⠁⠂⠐⠂⡐⠈⠻⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⠓⢀⡖⠀⢠⣴⣿⣿⣿⣿⣿⣿⣿⣿⣿⣶⣭⢚⠤⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠅" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠉⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⠀⣾⠃⣴⣿⣿⢻⣿⣿⣿⣿⣿⣿⣿⣿⣿⡎⣧⢣⡅⢣⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢠" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠐⠨⣿⣿⣿⣿⣿⣿⣿⠿⠋⠀⠉⢠⢿⣟⣯⣿⣾⣿⣿⣿⣿⣿⣿⣿⣿⡿⣜⠮⡜⣌⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠠⠀⠈" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠚⠿⠿⠿⠿⠿⠏⠀⠂⣴⣲⢠⣛⣾⣿⣿⣿⣿⣿⢿⣿⣿⣿⣿⣿⣿⣯⡟⡴⢂⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠠⠀⠔" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡀⠀⠀⠀⠀⠀⠀⠈⠷⡙⢰⣉⡴⣀⡉⠉⠋⠙⢉⣿⡟⠛⠛⠋⠛⠙⡉⠘⠁⠀⢀⣞⠀⠀⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠡⠈" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠐⡀⠀⠀⠀⠀⠀⠀⠀⠀⢠⠃⠀⠉⠘⠁⠀⢀⣠⣾⣧⡀⠀⠀⠐⠃⠂⠁⠀⠀⠠⠈⠄⠈⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠁⠂" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠐⠠⠀⠀⠀⠀⠀⠀⠀⢀⣋⢶⣠⣤⣤⡶⢮⣿⣿⣿⣷⠄⢦⣄⣀⡀⢄⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠁⠁⠒⣀⠀⠀⠀⠀⠀⠀⠜⣿⡿⣿⡟⣡⡿⢿⣿⣿⢿⡒⢌⠻⣯⠗⡎⠄⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠁⠀⠒⠤⣄⣀⢀⡀⠀⠀⢙⢣⡽⡅⠀⠀⠀⠀⠀⠀⢈⡖⠁⠉⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢰⡶⡲⡒⡒⣴⡶⣒⣖⢶⣶" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠀⠀⠈⠄⡁⠢⠐⠠⢌⠁⠁⠀⠱⣈⠎⢙⡳⣍⠾⠶⡲⠦⣐⠎⠲⠀⡄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠘⠓⠔⠥⠇⠛⠆⠋⠭⠚⠛" + echo "⠀⠀⠐⠀⠀⠀⠀⢀⠀⠀⠂⢌⡐⢠⠒⣤⣁⠎⠅⣈⡀⠀⠀⠀⠑⢪⠀⡈⣈⣁⡀⠁⣠⡁⠀⠀⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" + echo "⢀⣄⣐⣠⡈⠤⡁⠆⡑⣀⠉⠆⠉⢂⡙⢀⣂⣤⠚⣵⢯⡄⠀⠀⠀⠀⠡⠀⠀⠉⠘⠋⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" + echo "⠈⣨⣧⣶⣟⠷⢛⣡⢤⡲⡜⢮⠌⣍⢿⣦⠐⣈⢷⡌⠞⡜⢆⠀⠀⠀⠀⠡⣀⢄⣀⣀⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠠⣀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" + echo "⣾⣿⠟⣴⣞⣯⠻⣜⣣⢟⣹⢂⢯⡘⢎⡞⣦⠀⢂⠙⢦⠉⢄⠊⢀⠀⠀⠀⠀⠊⠐⠁⠈⠀⠀⠀⠀⠀⠀⠀⠀⡀⢀⠠⠀⠀⡣⢤⡉⢄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" + echo "⣿⡧⣏⢳⡬⢷⡙⢆⡳⢎⡳⢜⡢⢝⡲⢸⢥⠣⢌⠢⣤⣉⠢⠄⡄⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡀⢆⡱⠌⢀⠐⢢⢍⠢⢈⠄⡀⠀⠀⠀⠀⠀⠀⠀" + echo "⡟⣠⠻⣄⠻⣜⢻⡘⢣⡛⠼⣀⠻⡘⡤⢛⡄⢿⡀⢧⣛⢧⢇⡄⠀⠠⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠠⠀⠀⢠⡘⠄⡤⢀⠄⡀⠘⡄⠇⡘⠠⣀⠀⠀⠀⠀⠀⠀⠀" + echo "⠐⣡⠛⣌⠳⣌⠳⣌⠣⡜⡱⢌⢣⠱⢌⠣⡜⢢⡑⢎⡝⣊⠞⡼⣉⠖⡠⢐⠀⠀⠀⠀⠀⠀⠀⠀⠀⢐⠀⡘⠤⣌⠳⠀⡌⠎⡄⠀⠘⡀⣁⠒⢠⠂⠀⠀⠀⠀⠀⠀" + echo "⠠⢡⠉⡄⠣⢌⠳⡄⡓⢬⡐⠎⣄⠣⢊⠱⡘⡄⢙⢢⠹⣤⢋⡴⠡⢎⠱⡈⠆⠀⢀⠀⠀⠀⠀⢀⠀⡜⠠⣘⠲⢌⡱⠠⡑⢌⠰⡡⠀⠐⢠⠘⠠⢀⠃⠀⠀⠀⠀⠀" + echo "⢡⠂⠥⠐⡀⢈⠳⣐⡉⠆⡜⠒⡄⢣⠉⢆⠱⡐⠈⢂⠓⡌⢣⠘⡡⢊⠐⡘⠄⠀⠂⡀⠂⠌⣠⡔⢂⠩⠐⢤⢋⠆⡀⠂⠔⡈⠆⠥⠁⠀⠠⠈⡐⠨⢘⠀⠀⠀⠀⠀" + echo "⢀⠣⢈⠱⡐⠀⡇⠢⢅⡍⠰⡉⡔⢡⠊⡄⢣⠘⡀⠀⢂⠐⠠⢁⠐⠀⢂⡉⠆⠀⠡⢐⣹⣾⣿⣿⡆⡣⢘⢢⢉⠂⠠⢁⡘⠤⣉⠒⠀⢂⠀⠀⠤⠑⢂⠂⠀⠀⢀⠀" + echo "⡀⢃⠆⠐⡈⠡⢌⡑⠢⢌⠱⡐⢌⠢⡑⢌⠂⡅⢂⠁⡀⠂⢁⠠⠈⢀⠀⡜⠀⢈⠐⢿⣿⣿⣿⣿⡇⡅⡌⠆⠌⠀⠐⠠⡐⢢⠐⡀⠈⢀⠀⠀⠠⢁⠂⠌⠀⢀⢰⣤" + echo "⡘⠆⠌⠀⠠⢁⠢⠌⡁⠎⢠⠑⡈⠆⡑⢂⠥⢘⠠⢂⠀⠄⠂⠀⡀⠂⠠⢌⠀⣢⡍⢎⡻⣿⣿⣿⡇⣼⣿⣎⠀⠀⢂⠡⠐⢂⠡⠀⢀⠂⠌⠀⠀⠠⠀⠌⠀⠀⠂⠹" + echo "" + echo "" + exit 1 +fi + +# ---------- Merge compile_commands.json ---------- +# Each package gets its own compile_commands.json under build//. +# Merge them into a single file at the workspace root for clangd / IDEs. +echo "[MERGE] Combining compile_commands.json files..." +python3 -c " +import json, glob, pathlib + +ws = pathlib.Path('$WS_DIR') +combined = [] +for f in sorted(ws.glob('build/*/compile_commands.json')): + combined.extend(json.loads(f.read_text())) + +out = ws / 'compile_commands.json' +out.write_text(json.dumps(combined, indent=2)) +print(f' -> Wrote {len(combined)} entries to {out}') +" + +# ---------- Source ---------- +source "$WS_DIR/install/setup.bash" + +echo "" +echo " ╔══════════════════════════════════════════════════╗" +echo " ║ BUILD SUCCESSFUL ║" +echo " ╚══════════════════════════════════════════════════╝" +echo "" +echo "⠀⠀⢀⠀⣠⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" +echo "⢀⠀⣿⡂⢹⡇⠀⠀⣰⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" +echo "⢸⡇⢸⣇⢸⣇⠀⢀⣿⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢾⠀⠀⣯⡀⡆⠀⠀" +echo "⢸⣷⢸⣇⣸⣇⠀⣾⠏⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣀⣀⣀⣠⣀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢳⣂⠀⣿⡄⢸⡀⣤" +echo "⢠⣿⣿⣿⣿⣿⣿⠇⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣾⣿⣿⣊⡝⠛⠙⠂⠄⠠⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠈⢿⣦⣼⣷⣼⣁⠼" +echo "⢸⣿⣿⣿⣿⣿⣿⣀⢀⣀⣀⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣰⣿⣿⣿⣿⡻⣥⢋⡔⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠻⣿⣂⣜⣿⡟⢿⣿⣿⣄" +echo "⠈⣿⣿⣿⣿⣿⣿⣿⠿⠋⠉⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣿⣿⣿⣿⣿⣷⢯⣿⣾⡔⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠙⢪⣷⣿⢿⣿⣿" +echo "⠀⣿⣿⣟⢿⠿⠋⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢻⣿⡟⠛⠉⡉⢸⡉⠁⢀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢢⣽⣗⣿⠇" +echo "⠀⣿⣿⣿⡏⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠺⣿⡇⣤⡤⢔⡿⣇⠀⢦⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣿⣿⣯⠀" +echo "⠘⡟⣛⠋⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠈⡇⣿⣿⠗⡲⠏⠟⠿⠀⠈⠓⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣿⠍⠁⠁⠀" +echo "⠃⡜⡠⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠘⣼⣿⡟⢡⡿⠿⠷⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢸⣟⠒⠂⠂" +echo "⠐⢐⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢻⠸⣡⢶⣿⣟⡃⠀⠘⠀⠀⢀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣼⡇⠀⡀⠀" +echo "⢠⡏⠀⠃⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⡰⢨⠣⠉⠉⠋⠉⠀⠀⠀⠀⢈⠀⡂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠠⡿⠀⠀⠀⠀" +echo "⢺⡇⢸⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣽⡿⢛⢭⠏⣢⠍⠈⠖⠀⠀⠒⣶⢦⡁⠂⠀⠀⠀⠀⠀⠯⠤⣤⣴⢶⣍⠝⣯⣦⡀⠀⠀⠀⠀⠀⠀⠀⠀⢌⣿⠱⠀⠀⠀⠀⠀" +echo "⣯⣯⠸⣀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡠⠄⠀⠈⠀⠁⠀⠀⠀⠀⠀⠀⠀⠂⠀⠀⠏⠈⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠧⠍⠶⠤⠈⣆⠀⠀⠀⠀⠀⠀⠀⣷⡻⠀⣼⠀⠀⠀" +echo "⣯⣨⡀⢀⡠⠤⣐⠤⣀⣰⠔⠊⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠁⠑⠐⠐⠢⠺⠥⡾⠉⡠⠀⠀⠀" +echo "⠋⠙⠈⠉⠉⠁⠈⠈⠀⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" +echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" +echo "⠓⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" +echo "⠀⠀⠇⣣⡁⢶⣠⢀⣀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⢶⠀⡶⣲⠀⣆⡒⣰⠒⢦⢰⠀⢰⡆⣴⠐⣶⠒⣐⣒⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣠⣴⣺⣿⣿⣿⠛" +echo "⠀⠀⠑⢌⠻⣗⣔⠉⡅⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠞⠚⠃⠻⠴⠃⠦⠝⠘⠤⠎⠸⠤⠘⠧⠞⠀⠛⠀⠰⠤⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣼⡟⣾⣿⣿⣿⠃⠀" +echo "⠀⠀⠀⠀⠉⠢⠁⠀⠀⠀⠀⢀⣤⣤⣤⣄⠀⠀⢠⣤⠀⠀⣤⣄⠀⠀⠀⣤⣤⠀⢠⣤⣤⣤⣤⣤⡄⢠⣤⣄⠀⠀⠀⠀⣤⣤⡄⠀⠀⠀⢠⣤⡄⠀⠀⠀⢘⡮⡝⣿⣿⡿⢆⠁⠀" +echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣰⣿⠏⠉⠉⢿⣷⠀⢸⣿⠀⠠⣿⣿⣧⡀⠀⣿⣿⠀⢸⣿⡏⠉⠉⠉⠁⢼⣿⣿⡄⠀⠀⢸⡿⣿⡇⠀⠀⢀⣿⢻⣷⠀⠀⠀⠞⡜⣹⣿⣿⡙⢆⠀⠀" +echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣿⣿⠀⠀⠀⠀⠀⠀⢸⣿⠀⠐⣿⡯⢻⣷⡀⣿⣿⠀⢸⣿⣷⣶⣶⡆⠀⢺⣿⠹⣿⡀⢠⣿⠃⣿⡇⠀⠀⣾⡟⠀⢿⣧⠀⠀⠀⠠⢽⣿⣯⡙⠀⠀⠀" +echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢿⣿⡀⠀⠀⣠⣤⠀⢸⣿⠀⢈⣿⡧⠀⠹⣿⣿⣿⠀⢸⣿⡇⠀⠀⠀⠀⢸⣿⡄⢻⣧⣾⡏⢠⣿⡇⠀⣼⣿⣷⣶⣾⣿⣇⠀⠀⠀⠘⣿⢣⠜⠁⠀⠀" +echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠈⢿⣿⣶⣾⣿⠏⠀⢸⣿⠀⠀⣿⡷⠀⠀⠹⣿⣿⠀⢸⣿⣿⣿⣿⣿⡆⢸⣿⡆⠀⢿⡿⠀⢰⣿⡇⢀⣿⡏⠀⠀⠀⢹⣿⡀⠀⠀⠀⠀⠈⡆⠀⠀⠀" +echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠈⠉⠉⠀⠀⠀⠈⠉⠀⠀⠉⠁⠀⠀⠀⠉⠉⠀⠈⠉⠉⠈⠉⠉⠁⠈⠉⠀⠀⠈⠁⠀⠀⠉⠁⠈⠉⠀⠀⠀⠀⠈⠉⠁⠐⡀⠀⠀⠀⠀⠀⠀⠀" +echo "" +echo "" +echo " Remember to source your new changes bossman! " +echo "" diff --git a/utility_scripts/build_ws_topside.sh b/utility_scripts/build_ws_topside.sh new file mode 100755 index 000000000..5edaebe41 --- /dev/null +++ b/utility_scripts/build_ws_topside.sh @@ -0,0 +1,172 @@ +#!/bin/bash +set -e + +WS_DIR="$HOME/vscopium/ros2_ws" +SRC_DIR="$WS_DIR/src" +GITHUB_ORG="https://github.com/vortexntnu" + +# Repos to clone +REPOS=( + "stonefish_ros2" + "vortex-auv" + "vortex-msgs" + "vortex-stonefish-interface" + "vortex-stonefish-sim" + "vortex-utils" + "vortex-ci" +) + +# ---- Install dependencies script ---- + +# python3 -m pip install --upgrade pip +# install numpy, pynput, joy, wheel +# install ros-humble-xacro, ros-humble-joy +# run casadi install script + +# ------------ Clone missing repos ------------ +mkdir -p "$SRC_DIR" + +for repo in "${REPOS[@]}"; do + if [ -d "$SRC_DIR/$repo" ]; then + echo "[SKIP] $SRC_DIR/$repo already exists" + else + echo "[CLONE] $GITHUB_ORG/$repo.git -> $SRC_DIR/$repo" + git clone "$GITHUB_ORG/$repo.git" "$SRC_DIR/$repo" + fi +done + +# ---------- Build ---------- +cd "$WS_DIR" + +# Default packages to build +DEFAULT_PKGS=( + vortex_msgs + vortex_utility_nodes + vortex_utils + vortex_utils_ros + stonefish_ros2 + vortex_sim_interface + stonefish_sim + stonefish_sim_interface + thrust_allocator_auv + thruster_interface_auv + dp_adapt_backs_controller + pid_controller_dp + reference_filter_dp + keyboard_joy + joystick_interface_auv + auv_setup + operation_mode_manager + los_guidance +) + +# Use arguments if provided, otherwise use defaults +if [ $# -gt 0 ]; then + PKGS=("$@") +else + PKGS=("${DEFAULT_PKGS[@]}") +fi + +BUILD_FAILED=0 +echo "[BUILD] Building packages: ${PKGS[*]}" +colcon build \ + --packages-up-to "${PKGS[@]}" \ + --symlink-install \ + --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON || + BUILD_FAILED=1 + +# ---------- Handle result ---------- +if [ $BUILD_FAILED -eq 1 ]; then + echo "" + echo " ╔══════════════════════════════════════════════════╗ " + echo " ║ BUILD FAILED ║ " + echo " ╚══════════════════════════════════════════════════╝ " + echo "" + echo "⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⡖⢀⠀⠒⠒⠒⠒⠒⠒⠒⠀⠀⠀⠀⠀⠀⠀⡀⠀⠀⠀⠀⠀⠀⠀" + echo "⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⠿⠛⠙⠉⠉⠉⠉⠉⠉⠉⠙⠃⠀⠀⠠⠄⠂⠐⠀⠀⠀⠀⠀⠀⠀⠉⠁⠀⠀⠀⢸⠀⠀⠀⠀⠀" + echo "⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⡿⠋⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢸⠀⠀⠀⠀⠀" + echo "⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⡟⠀⠀⠀⠀⠀⠀⠀⡀⣀⢀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠈⠀⠀⠀⠀⠀" + echo "⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⡟⠀⠀⠀⠀⠀⢀⣶⢿⣵⣮⣞⣶⣆⡶⣴⣀⠀⠀⠀⠀⠀⠀⠡⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" + echo "⠉⠉⠁⠂⠐⠂⡐⠈⠻⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⠓⢀⡖⠀⢠⣴⣿⣿⣿⣿⣿⣿⣿⣿⣿⣶⣭⢚⠤⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠅" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠉⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⠀⣾⠃⣴⣿⣿⢻⣿⣿⣿⣿⣿⣿⣿⣿⣿⡎⣧⢣⡅⢣⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢠" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠐⠨⣿⣿⣿⣿⣿⣿⣿⠿⠋⠀⠉⢠⢿⣟⣯⣿⣾⣿⣿⣿⣿⣿⣿⣿⣿⡿⣜⠮⡜⣌⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠠⠀⠈" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠚⠿⠿⠿⠿⠿⠏⠀⠂⣴⣲⢠⣛⣾⣿⣿⣿⣿⣿⢿⣿⣿⣿⣿⣿⣿⣯⡟⡴⢂⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠠⠀⠔" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡀⠀⠀⠀⠀⠀⠀⠈⠷⡙⢰⣉⡴⣀⡉⠉⠋⠙⢉⣿⡟⠛⠛⠋⠛⠙⡉⠘⠁⠀⢀⣞⠀⠀⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠡⠈" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠐⡀⠀⠀⠀⠀⠀⠀⠀⠀⢠⠃⠀⠉⠘⠁⠀⢀⣠⣾⣧⡀⠀⠀⠐⠃⠂⠁⠀⠀⠠⠈⠄⠈⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠁⠂" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠐⠠⠀⠀⠀⠀⠀⠀⠀⢀⣋⢶⣠⣤⣤⡶⢮⣿⣿⣿⣷⠄⢦⣄⣀⡀⢄⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠁⠁⠒⣀⠀⠀⠀⠀⠀⠀⠜⣿⡿⣿⡟⣡⡿⢿⣿⣿⢿⡒⢌⠻⣯⠗⡎⠄⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠁⠀⠒⠤⣄⣀⢀⡀⠀⠀⢙⢣⡽⡅⠀⠀⠀⠀⠀⠀⢈⡖⠁⠉⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢰⡶⡲⡒⡒⣴⡶⣒⣖⢶⣶" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠀⠀⠈⠄⡁⠢⠐⠠⢌⠁⠁⠀⠱⣈⠎⢙⡳⣍⠾⠶⡲⠦⣐⠎⠲⠀⡄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠘⠓⠔⠥⠇⠛⠆⠋⠭⠚⠛" + echo "⠀⠀⠐⠀⠀⠀⠀⢀⠀⠀⠂⢌⡐⢠⠒⣤⣁⠎⠅⣈⡀⠀⠀⠀⠑⢪⠀⡈⣈⣁⡀⠁⣠⡁⠀⠀⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" + echo "⢀⣄⣐⣠⡈⠤⡁⠆⡑⣀⠉⠆⠉⢂⡙⢀⣂⣤⠚⣵⢯⡄⠀⠀⠀⠀⠡⠀⠀⠉⠘⠋⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" + echo "⠈⣨⣧⣶⣟⠷⢛⣡⢤⡲⡜⢮⠌⣍⢿⣦⠐⣈⢷⡌⠞⡜⢆⠀⠀⠀⠀⠡⣀⢄⣀⣀⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠠⣀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" + echo "⣾⣿⠟⣴⣞⣯⠻⣜⣣⢟⣹⢂⢯⡘⢎⡞⣦⠀⢂⠙⢦⠉⢄⠊⢀⠀⠀⠀⠀⠊⠐⠁⠈⠀⠀⠀⠀⠀⠀⠀⠀⡀⢀⠠⠀⠀⡣⢤⡉⢄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" + echo "⣿⡧⣏⢳⡬⢷⡙⢆⡳⢎⡳⢜⡢⢝⡲⢸⢥⠣⢌⠢⣤⣉⠢⠄⡄⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡀⢆⡱⠌⢀⠐⢢⢍⠢⢈⠄⡀⠀⠀⠀⠀⠀⠀⠀" + echo "⡟⣠⠻⣄⠻⣜⢻⡘⢣⡛⠼⣀⠻⡘⡤⢛⡄⢿⡀⢧⣛⢧⢇⡄⠀⠠⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠠⠀⠀⢠⡘⠄⡤⢀⠄⡀⠘⡄⠇⡘⠠⣀⠀⠀⠀⠀⠀⠀⠀" + echo "⠐⣡⠛⣌⠳⣌⠳⣌⠣⡜⡱⢌⢣⠱⢌⠣⡜⢢⡑⢎⡝⣊⠞⡼⣉⠖⡠⢐⠀⠀⠀⠀⠀⠀⠀⠀⠀⢐⠀⡘⠤⣌⠳⠀⡌⠎⡄⠀⠘⡀⣁⠒⢠⠂⠀⠀⠀⠀⠀⠀" + echo "⠠⢡⠉⡄⠣⢌⠳⡄⡓⢬⡐⠎⣄⠣⢊⠱⡘⡄⢙⢢⠹⣤⢋⡴⠡⢎⠱⡈⠆⠀⢀⠀⠀⠀⠀⢀⠀⡜⠠⣘⠲⢌⡱⠠⡑⢌⠰⡡⠀⠐⢠⠘⠠⢀⠃⠀⠀⠀⠀⠀" + echo "⢡⠂⠥⠐⡀⢈⠳⣐⡉⠆⡜⠒⡄⢣⠉⢆⠱⡐⠈⢂⠓⡌⢣⠘⡡⢊⠐⡘⠄⠀⠂⡀⠂⠌⣠⡔⢂⠩⠐⢤⢋⠆⡀⠂⠔⡈⠆⠥⠁⠀⠠⠈⡐⠨⢘⠀⠀⠀⠀⠀" + echo "⢀⠣⢈⠱⡐⠀⡇⠢⢅⡍⠰⡉⡔⢡⠊⡄⢣⠘⡀⠀⢂⠐⠠⢁⠐⠀⢂⡉⠆⠀⠡⢐⣹⣾⣿⣿⡆⡣⢘⢢⢉⠂⠠⢁⡘⠤⣉⠒⠀⢂⠀⠀⠤⠑⢂⠂⠀⠀⢀⠀" + echo "⡀⢃⠆⠐⡈⠡⢌⡑⠢⢌⠱⡐⢌⠢⡑⢌⠂⡅⢂⠁⡀⠂⢁⠠⠈⢀⠀⡜⠀⢈⠐⢿⣿⣿⣿⣿⡇⡅⡌⠆⠌⠀⠐⠠⡐⢢⠐⡀⠈⢀⠀⠀⠠⢁⠂⠌⠀⢀⢰⣤" + echo "⡘⠆⠌⠀⠠⢁⠢⠌⡁⠎⢠⠑⡈⠆⡑⢂⠥⢘⠠⢂⠀⠄⠂⠀⡀⠂⠠⢌⠀⣢⡍⢎⡻⣿⣿⣿⡇⣼⣿⣎⠀⠀⢂⠡⠐⢂⠡⠀⢀⠂⠌⠀⠀⠠⠀⠌⠀⠀⠂⠹" + echo "" + echo "" + exit 1 +fi + +# ---------- Merge compile_commands.json ---------- +# Each package gets its own compile_commands.json under build//. +# Merge them into a single file at the workspace root for clangd / IDEs. +echo "[MERGE] Combining compile_commands.json files..." +python3 -c " +import json, glob, pathlib + +ws = pathlib.Path('$WS_DIR') +combined = [] +for f in sorted(ws.glob('build/*/compile_commands.json')): + combined.extend(json.loads(f.read_text())) + +out = ws / 'compile_commands.json' +out.write_text(json.dumps(combined, indent=2)) +print(f' -> Wrote {len(combined)} entries to {out}') +" + +# ---------- Source ---------- +source "$WS_DIR/install/setup.bash" + +echo "" +echo " ╔══════════════════════════════════════════════════╗" +echo " ║ BUILD SUCCESSFUL ║" +echo " ╚══════════════════════════════════════════════════╝" +echo "" +echo "⠀⠀⢀⠀⣠⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" +echo "⢀⠀⣿⡂⢹⡇⠀⠀⣰⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" +echo "⢸⡇⢸⣇⢸⣇⠀⢀⣿⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢾⠀⠀⣯⡀⡆⠀⠀" +echo "⢸⣷⢸⣇⣸⣇⠀⣾⠏⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣀⣀⣀⣠⣀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢳⣂⠀⣿⡄⢸⡀⣤" +echo "⢠⣿⣿⣿⣿⣿⣿⠇⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣾⣿⣿⣊⡝⠛⠙⠂⠄⠠⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠈⢿⣦⣼⣷⣼⣁⠼" +echo "⢸⣿⣿⣿⣿⣿⣿⣀⢀⣀⣀⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣰⣿⣿⣿⣿⡻⣥⢋⡔⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠻⣿⣂⣜⣿⡟⢿⣿⣿⣄" +echo "⠈⣿⣿⣿⣿⣿⣿⣿⠿⠋⠉⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣿⣿⣿⣿⣿⣷⢯⣿⣾⡔⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠙⢪⣷⣿⢿⣿⣿" +echo "⠀⣿⣿⣟⢿⠿⠋⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢻⣿⡟⠛⠉⡉⢸⡉⠁⢀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢢⣽⣗⣿⠇" +echo "⠀⣿⣿⣿⡏⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠺⣿⡇⣤⡤⢔⡿⣇⠀⢦⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣿⣿⣯⠀" +echo "⠘⡟⣛⠋⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠈⡇⣿⣿⠗⡲⠏⠟⠿⠀⠈⠓⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣿⠍⠁⠁⠀" +echo "⠃⡜⡠⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠘⣼⣿⡟⢡⡿⠿⠷⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢸⣟⠒⠂⠂" +echo "⠐⢐⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢻⠸⣡⢶⣿⣟⡃⠀⠘⠀⠀⢀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣼⡇⠀⡀⠀" +echo "⢠⡏⠀⠃⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⡰⢨⠣⠉⠉⠋⠉⠀⠀⠀⠀⢈⠀⡂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠠⡿⠀⠀⠀⠀" +echo "⢺⡇⢸⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣽⡿⢛⢭⠏⣢⠍⠈⠖⠀⠀⠒⣶⢦⡁⠂⠀⠀⠀⠀⠀⠯⠤⣤⣴⢶⣍⠝⣯⣦⡀⠀⠀⠀⠀⠀⠀⠀⠀⢌⣿⠱⠀⠀⠀⠀⠀" +echo "⣯⣯⠸⣀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡠⠄⠀⠈⠀⠁⠀⠀⠀⠀⠀⠀⠀⠂⠀⠀⠏⠈⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠧⠍⠶⠤⠈⣆⠀⠀⠀⠀⠀⠀⠀⣷⡻⠀⣼⠀⠀⠀" +echo "⣯⣨⡀⢀⡠⠤⣐⠤⣀⣰⠔⠊⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠁⠑⠐⠐⠢⠺⠥⡾⠉⡠⠀⠀⠀" +echo "⠋⠙⠈⠉⠉⠁⠈⠈⠀⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" +echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" +echo "⠓⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" +echo "⠀⠀⠇⣣⡁⢶⣠⢀⣀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⢶⠀⡶⣲⠀⣆⡒⣰⠒⢦⢰⠀⢰⡆⣴⠐⣶⠒⣐⣒⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣠⣴⣺⣿⣿⣿⠛" +echo "⠀⠀⠑⢌⠻⣗⣔⠉⡅⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠞⠚⠃⠻⠴⠃⠦⠝⠘⠤⠎⠸⠤⠘⠧⠞⠀⠛⠀⠰⠤⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣼⡟⣾⣿⣿⣿⠃⠀" +echo "⠀⠀⠀⠀⠉⠢⠁⠀⠀⠀⠀⢀⣤⣤⣤⣄⠀⠀⢠⣤⠀⠀⣤⣄⠀⠀⠀⣤⣤⠀⢠⣤⣤⣤⣤⣤⡄⢠⣤⣄⠀⠀⠀⠀⣤⣤⡄⠀⠀⠀⢠⣤⡄⠀⠀⠀⢘⡮⡝⣿⣿⡿⢆⠁⠀" +echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣰⣿⠏⠉⠉⢿⣷⠀⢸⣿⠀⠠⣿⣿⣧⡀⠀⣿⣿⠀⢸⣿⡏⠉⠉⠉⠁⢼⣿⣿⡄⠀⠀⢸⡿⣿⡇⠀⠀⢀⣿⢻⣷⠀⠀⠀⠞⡜⣹⣿⣿⡙⢆⠀⠀" +echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣿⣿⠀⠀⠀⠀⠀⠀⢸⣿⠀⠐⣿⡯⢻⣷⡀⣿⣿⠀⢸⣿⣷⣶⣶⡆⠀⢺⣿⠹⣿⡀⢠⣿⠃⣿⡇⠀⠀⣾⡟⠀⢿⣧⠀⠀⠀⠠⢽⣿⣯⡙⠀⠀⠀" +echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢿⣿⡀⠀⠀⣠⣤⠀⢸⣿⠀⢈⣿⡧⠀⠹⣿⣿⣿⠀⢸⣿⡇⠀⠀⠀⠀⢸⣿⡄⢻⣧⣾⡏⢠⣿⡇⠀⣼⣿⣷⣶⣾⣿⣇⠀⠀⠀⠘⣿⢣⠜⠁⠀⠀" +echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠈⢿⣿⣶⣾⣿⠏⠀⢸⣿⠀⠀⣿⡷⠀⠀⠹⣿⣿⠀⢸⣿⣿⣿⣿⣿⡆⢸⣿⡆⠀⢿⡿⠀⢰⣿⡇⢀⣿⡏⠀⠀⠀⢹⣿⡀⠀⠀⠀⠀⠈⡆⠀⠀⠀" +echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠈⠉⠉⠀⠀⠀⠈⠉⠀⠀⠉⠁⠀⠀⠀⠉⠉⠀⠈⠉⠉⠈⠉⠉⠁⠈⠉⠀⠀⠈⠁⠀⠀⠉⠁⠈⠉⠀⠀⠀⠀⠈⠉⠁⠐⡀⠀⠀⠀⠀⠀⠀⠀" +echo "" +echo "" +echo " Remember to source your new changes bossman! " +echo "" diff --git a/utility_scripts/launch_drone_topside.sh b/utility_scripts/launch_drone_topside.sh new file mode 100755 index 000000000..80681499d --- /dev/null +++ b/utility_scripts/launch_drone_topside.sh @@ -0,0 +1,57 @@ +#!/bin/bash +# Launch drone simulation stack in a tmux session + +SESSION="drone_launch" +S="source ~/vscopium/ros2_ws/install/setup.bash" + +# Kill existing session if it exists +tmux kill-session -t "$SESSION" 2>/dev/null + +# Launch Foxglove Studio only if not already running +if ! pgrep -f foxglove-studio &>/dev/null; then + foxglove-studio &>/dev/null & +fi + +# ============================================= +# Window 1: sim (4 equal panes) +# ============================================= +tmux new-session -d -s "$SESSION" -n "sim" + +# Grab the initial pane ID +PANE_SIM=$(tmux list-panes -t "$SESSION:sim" -F '#{pane_id}') + +# Stonefish simulation (top-left) +tmux send-keys -t "$PANE_SIM" "clear && $S && ros2 launch stonefish_sim simulation.launch.py drone:=nautilus" Enter + +# Split right -> Keyboard joy (top-right) +PANE_JOY=$(tmux split-window -h -t "$PANE_SIM" -P -F '#{pane_id}') +tmux send-keys -t "$PANE_JOY" "clear && $S && ros2 launch keyboard_joy keyboard_joy_node.launch.py" Enter + +# Split sim pane down -> AUV setup (bottom-left) +PANE_DP=$(tmux split-window -v -t "$PANE_SIM" -P -F '#{pane_id}') +tmux send-keys -t "$PANE_DP" "clear && $S && ros2 launch auv_setup dp.launch.py" Enter + +# Split joy pane down -> Drone sim (bottom-right) +PANE_DRONE=$(tmux split-window -v -t "$PANE_JOY" -P -F '#{pane_id}') +tmux send-keys -t "$PANE_DRONE" "clear && $S && ros2 launch stonefish_sim drone_sim.launch.py" Enter + +# Force equal pane sizes +tmux select-layout -t "$SESSION:sim" tiled + +# ============================================= +# Window 2: tools (2 panes) +# ============================================= +tmux new-window -t "$SESSION" -n "tools" + +PANE_FOX=$(tmux list-panes -t "$SESSION:tools" -F '#{pane_id}') +tmux send-keys -t "$PANE_FOX" "clear && $S && ros2 launch foxglove_bridge foxglove_bridge_launch.xml" Enter + +# Split down -> Message publisher +PANE_MSG=$(tmux split-window -v -t "$PANE_FOX" -P -F '#{pane_id}') +tmux send-keys -t "$PANE_MSG" "clear && $S && ros2 launch vortex_utility_nodes message_publisher.launch.py" Enter + +# ============================================= +# Focus first window and attach +# ============================================= +tmux select-window -t "$SESSION:sim" +tmux attach-session -t "$SESSION" diff --git a/utility_scripts/launch_drone_vehicle.sh b/utility_scripts/launch_drone_vehicle.sh new file mode 100755 index 000000000..8e0639738 --- /dev/null +++ b/utility_scripts/launch_drone_vehicle.sh @@ -0,0 +1,70 @@ +#!/bin/bash +# Launch drone vehicle stack in a tmux session + +SESSION="drone_vehicle" + +# Kill existing session if it exists +tmux kill-session -t "$SESSION" 2>/dev/null + +# ============================================= +# Window 1: control (4 equal panes) +# ============================================= +tmux new-session -d -s "$SESSION" -n "control" + +PANE_C1=$(tmux list-panes -t "$SESSION:control" -F '#{pane_id}') +tmux send-keys -t "$PANE_C1" "s && ros2 launch auv_setup thruster.launch.py" Enter + +PANE_C2=$(tmux split-window -h -t "$PANE_C1" -P -F '#{pane_id}') +tmux send-keys -t "$PANE_C2" "s && ros2 launch operation_mode_manager operation_mode_manager.launch.py" Enter + +PANE_C3=$(tmux split-window -v -t "$PANE_C1" -P -F '#{pane_id}') +tmux send-keys -t "$PANE_C3" "s" Enter + +PANE_C4=$(tmux split-window -v -t "$PANE_C2" -P -F '#{pane_id}') +tmux send-keys -t "$PANE_C4" "s" Enter + +tmux select-layout -t "$SESSION:control" tiled + +# ============================================= +# Window 2: perception (4 equal panes) +# ============================================= +tmux new-window -t "$SESSION" -n "perception" + +PANE_P1=$(tmux list-panes -t "$SESSION:perception" -F '#{pane_id}') +tmux send-keys -t "$PANE_P1" "s" Enter + +PANE_P2=$(tmux split-window -h -t "$PANE_P1" -P -F '#{pane_id}') +tmux send-keys -t "$PANE_P2" "s" Enter + +PANE_P3=$(tmux split-window -v -t "$PANE_P1" -P -F '#{pane_id}') +tmux send-keys -t "$PANE_P3" "s" Enter + +PANE_P4=$(tmux split-window -v -t "$PANE_P2" -P -F '#{pane_id}') +tmux send-keys -t "$PANE_P4" "s" Enter + +tmux select-layout -t "$SESSION:perception" tiled + +# ============================================= +# Window 3: misc (4 equal panes) +# ============================================= +tmux new-window -t "$SESSION" -n "misc" + +PANE_M1=$(tmux list-panes -t "$SESSION:misc" -F '#{pane_id}') +tmux send-keys -t "$PANE_M1" "s" Enter + +PANE_M2=$(tmux split-window -h -t "$PANE_M1" -P -F '#{pane_id}') +tmux send-keys -t "$PANE_M2" "s" Enter + +PANE_M3=$(tmux split-window -v -t "$PANE_M1" -P -F '#{pane_id}') +tmux send-keys -t "$PANE_M3" "s" Enter + +PANE_M4=$(tmux split-window -v -t "$PANE_M2" -P -F '#{pane_id}') +tmux send-keys -t "$PANE_M4" "s" Enter + +tmux select-layout -t "$SESSION:misc" tiled + +# ============================================= +# Focus control window and attach +# ============================================= +tmux select-window -t "$SESSION:control" +tmux attach-session -t "$SESSION" From a54a569abf518068b6045ea46f9e36575e4c96c0 Mon Sep 17 00:00:00 2001 From: Cyprian Osinski Date: Wed, 1 Apr 2026 23:30:18 +0200 Subject: [PATCH 266/290] tuned controllers --- auv_setup/config/robots/nautilus.yaml | 4 +-- .../config/adapt_params_nautilus.yaml | 10 +++--- .../pid_controller_dp/config/pid_params.yaml | 34 +++++++++---------- 3 files changed, 24 insertions(+), 24 deletions(-) diff --git a/auv_setup/config/robots/nautilus.yaml b/auv_setup/config/robots/nautilus.yaml index e736e1c82..078512fd8 100644 --- a/auv_setup/config/robots/nautilus.yaml +++ b/auv_setup/config/robots/nautilus.yaml @@ -16,7 +16,7 @@ /**: ros__parameters: physical: - center_of_mass: [0.0, 0.0, 0.025] # CO is aligned with CM Position (x,y) in meters (M), small cg offset in z to keep drone naturally stable in roll/pitch + center_of_mass: [0.0, 0.0, 0.015] # CO is aligned with CM Position (x,y) in meters (M), small cg offset in z to keep drone naturally stable in roll/pitch mass_matrix: [53.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 53.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 53.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 11.0628, 1.086, -3.17502, 0.0, 0.0, 0.0, 1.086, 23.1128, 0.1025, 0.0, 0.0, 0.0, -3.17502, 0.1025, 26.23998] # 6x6 mass_inertia_matrix propulsion: @@ -106,7 +106,7 @@ joy: "joy" pose: "pose" twist: "twist" - odom: "odom_eskf" + odom: "odom" operation_mode: "operation_mode" killswitch: "killswitch" reference_pose: "reference_pose" diff --git a/control/dp_adapt_backs_controller/config/adapt_params_nautilus.yaml b/control/dp_adapt_backs_controller/config/adapt_params_nautilus.yaml index 65b639ed7..6d1f0e1e5 100644 --- a/control/dp_adapt_backs_controller/config/adapt_params_nautilus.yaml +++ b/control/dp_adapt_backs_controller/config/adapt_params_nautilus.yaml @@ -1,7 +1,7 @@ /**: ros__parameters: - K1 : [15.0, 15.0, 15.0, 1.5, 6.0, 6.0] # Outer loop tuning parameters - K2 : [30.0, 30.0, 30.0, 2.0, 8.5, 8.5] # Inner loop tuning parameters - r_b_bg : [0.0, 0.0, 0.01] # Vector from body centre to centre of gravity - adapt_gain : [0.6, 0.15, 0.6, 0.15, 0.6, 0.15, 0.6, 0.15, 0.6, 0.15, 0.6, 0.15] # Tuning parameters for linear and nonlinear damping - d_gain : [0.8, 0.8, 0.8, 0.8, 0.8, 0.8] # Tuning parameters for the unmodeled disturbances and uncertanties + K1 : [9.0, 9.0, 11.0, 1.0, 4.5, 4.5] # Outer loop tuning parameters + K2 : [20.0, 20.0, 25.0, 3.0, 7.5, 7.5] # Inner loop tuning parameters + r_b_bg : [0.0, 0.0, 0.015] # Vector from body centre to centre of gravity + adapt_gain : [0.3, 0.05, 0.3, 0.05, 0.3, 0.05, 0.3, 0.05, 0.3, 0.05, 0.3, 0.05] # Tuning parameters for linear and nonlinear damping + d_gain : [0.4, 0.4, 0.4, 0.4, 0.4, 0.4] # Tuning parameters for the unmodeled disturbances and uncertanties diff --git a/control/pid_controller_dp/config/pid_params.yaml b/control/pid_controller_dp/config/pid_params.yaml index 0a3c8268a..e133febef 100644 --- a/control/pid_controller_dp/config/pid_params.yaml +++ b/control/pid_controller_dp/config/pid_params.yaml @@ -1,20 +1,20 @@ /**: ros__parameters: - Kp_x: 10.0 - Kp_y: 5.0 - Kp_z: 30.0 - Kp_roll: 10.0 - Kp_pitch: 41.0 - Kp_yaw: 6.0 - Ki_x: 0.092 - Ki_y: 0.00059 - Ki_z: 0.085 - Ki_roll: 0.002 - Ki_pitch: 0.01 - Ki_yaw: 0.0003 - Kd_x: 0.001 - Kd_y: 0.0 - Kd_z: 0.0 + Kp_x: 30.0 + Kp_y: 30.0 + Kp_z: 40.0 + Kp_roll: 30.0 + Kp_pitch: 60.0 + Kp_yaw: 10.0 + Ki_x: 0.01 + Ki_y: 0.001 + Ki_z: 0.05 + Ki_roll: 0.01 + Ki_pitch: 0.005 + Ki_yaw: 0.01 + Kd_x: 5.0 + Kd_y: 5.0 + Kd_z: 5.0 Kd_roll: 0.002 - Kd_pitch: 0.0 - Kd_yaw: 0.001 + Kd_pitch: 0.005 + Kd_yaw: 0.002 From 5e176c9ee3eba0773ac72c8e204d1946038d54ef Mon Sep 17 00:00:00 2001 From: vortexuser-Pi Date: Thu, 2 Apr 2026 16:11:30 +0200 Subject: [PATCH 267/290] update with local drone changes --- auv_setup/config/robots/nautilus.yaml | 2 +- .../launch/nucleus_odom_transformer.launch.py | 12 ++++--- auv_setup/launch/state_estimation.launch.py | 2 +- .../config/adapt_params_orca.yaml | 7 ----- .../src/dp_adapt_backs_controller_ros.cpp | 3 +- navigation/eskf/src/eskf_ros.cpp | 4 +-- .../config/odom_transformer_params.yaml | 1 + .../odom_transformer/odom_transformer.hpp | 1 + .../odom_transformer/src/odom_transformer.cpp | 31 +++++++++++++++---- scripts/nautilus.sh | 9 ++++++ scripts/nautilus_dp_launch.sh | 0 11 files changed, 48 insertions(+), 24 deletions(-) delete mode 100644 control/dp_adapt_backs_controller/config/adapt_params_orca.yaml create mode 100755 scripts/nautilus.sh create mode 100755 scripts/nautilus_dp_launch.sh diff --git a/auv_setup/config/robots/nautilus.yaml b/auv_setup/config/robots/nautilus.yaml index 078512fd8..b6398615e 100644 --- a/auv_setup/config/robots/nautilus.yaml +++ b/auv_setup/config/robots/nautilus.yaml @@ -90,7 +90,7 @@ thrust_update_rate: 100.0 # [Hz] watchdog_timeout: 1.0 # [s] - thruster_to_pin_mapping: [4, 5, 6, 7, 0, 1, 2, 3] # I.e. if thruster_to_pin = [ 7, 6, 5, 4, 3, 2, 1, 0 ] then thruster 0 is pin 1 etc.. + thruster_to_pin_mapping: [4, 5, 7, 6, 0, 1, 2, 3] # I.e. if thruster_to_pin = [ 7, 6, 5, 4, 3, 2, 1, 0 ] then thruster 0 is pin 1 etc.. thruster_direction: [-1, -1, 1, 1, -1, -1, 1, 1] # Disclose during thruster mapping (+/- 1) thruster_PWM_min: [1050, 1050, 1050, 1050, 1050, 1050, 1050, 1050] # Minimum PWM value, Recommended: [1100, 1100, 1100, 1100, 1100, 1100, 1100, 1100] thruster_PWM_max: [1950, 1950, 1950, 1950, 1950, 1950, 1950, 1950] # Maximum PWM value, Recommended: [1900, 1900, 1900, 1900, 1900, 1900, 1900, 1900] diff --git a/auv_setup/launch/nucleus_odom_transformer.launch.py b/auv_setup/launch/nucleus_odom_transformer.launch.py index 9f46081c2..4c94fee1b 100644 --- a/auv_setup/launch/nucleus_odom_transformer.launch.py +++ b/auv_setup/launch/nucleus_odom_transformer.launch.py @@ -47,10 +47,11 @@ def launch_setup(context, *args, **kwargs): "connection_params.remote_ip": "10.0.0.42", "connection_params.data_remote_port": 9000, "connection_params.password": "", - "enable_imu": False, + "enable_imu": True, "enable_ins_odom": True, - "enable_dvl": False, - "enable_pressure": False, + "enable_dvl": True, + "enable_pressure": True, + "enable_altimeter": True, "enable_magnetometer": False, "enable_ins_twist": False, "enable_ins_position": False, @@ -60,6 +61,7 @@ def launch_setup(context, *args, **kwargs): "ins_pub_topic": f"/{namespace}/nucleus/odom", "dvl_pub_topic": f"/{namespace}/nucleus/dvl", "pressure_pub_topic": f"/{namespace}/nucleus/pressure", + "altimeter_pub_topic": f"/{namespace}/nucleus/altitude", "magnetometer_pub_topic": f"/{namespace}/imu/mag", "ins_twist_pub_topic": f"/{namespace}/nucleus/ins/twist", "ins_position_pub_topic": f"/{namespace}/nucleus/ins/position", @@ -70,8 +72,7 @@ def launch_setup(context, *args, **kwargs): "bottom_track_settings.mode": 2, "bottom_track_settings.velocity_range": 5, "bottom_track_settings.enable_watertrack": False, - "fast_pressure_settings.enable": True, - "fast_pressure_settings.sampling_rate": 16, + "altimeter_settings.power_level": 0, # 0=default "magnetometer_settings.freq": 75, "magnetometer_settings.mode": 0, "instrument_settings.rotxy": 180.0, @@ -94,6 +95,7 @@ def launch_setup(context, *args, **kwargs): "publish_tf": True, "publish_pose": True, "publish_twist": True, + "rotate_yaw_180": True, "topics.input": f"/{namespace}/nucleus/odom", "topics.output": f"/{namespace}/odom", "topics.pose": f"/{namespace}/pose", diff --git a/auv_setup/launch/state_estimation.launch.py b/auv_setup/launch/state_estimation.launch.py index d76c3fdc0..8423709e6 100644 --- a/auv_setup/launch/state_estimation.launch.py +++ b/auv_setup/launch/state_estimation.launch.py @@ -132,7 +132,7 @@ def launch_setup(context, *args, **kwargs): parameters=[ { "frame_id": f"/{namespace}/nucleus_frame", - "connection_params.remote_ip": "192.168.1.100", + "connection_params.remote_ip": "10.0.0.42", "connection_params.data_remote_port": 9000, "connection_params.password": "", "enable_imu": False, diff --git a/control/dp_adapt_backs_controller/config/adapt_params_orca.yaml b/control/dp_adapt_backs_controller/config/adapt_params_orca.yaml deleted file mode 100644 index e128131b2..000000000 --- a/control/dp_adapt_backs_controller/config/adapt_params_orca.yaml +++ /dev/null @@ -1,7 +0,0 @@ -/**: - ros__parameters: - K1 : [10.5, 10.5, 13.5, 0.0, 4.0, 4.0] # Outer loop tuning parameters - K2 : [20.5, 20.5, 20.5, 0.0, 5.5, 5.5] # Inner loop tuning parameters - r_b_bg : [0.01, 0.0, 0.02] # Vector from body centre to centre of gravity - adapt_gain : [0.4, 0.1, 0.4, 0.1, 0.4, 0.1, 0.4, 0.1, 0.4, 0.1, 0.4, 0.1] # Tuning parameters for linear and nonlinear damping - d_gain : [0.6, 0.6, 0.6, 0.6, 0.6, 0.6] # Tuning parameters for the unmodeled disturbances and uncertanties 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 508d45750..3cb708f12 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 @@ -297,8 +297,7 @@ void DPAdaptBacksControllerNode::publish_tau() { tau_msg.wrench.force.z = tau(2); // comment out if roll control is not needed - tau_msg.wrench.torque.x = tau(3); - + //tau_msg.wrench.torque.x = tau(3); tau_msg.wrench.torque.y = tau(4); tau_msg.wrench.torque.z = tau(5); diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp index ed70aee78..6c6efca99 100644 --- a/navigation/eskf/src/eskf_ros.cpp +++ b/navigation/eskf/src/eskf_ros.cpp @@ -79,9 +79,9 @@ void ESKFNode::set_subscribers_and_publisher() { pressure_topic, qos_sensor_data, std::bind(&ESKFNode::depth_callback, this, std::placeholders::_1)); - std::string odom_topic = this->get_parameter("topics.odom").as_string(); + // std::string odom_topic = this->get_parameter("topics.odom").as_string(); odom_pub_ = this->create_publisher( - odom_topic, qos_sensor_data); + "/nautilus/odom_eskf", qos_sensor_data); if (publish_pose_) { std::string pose_topic = this->get_parameter("topics.pose").as_string(); diff --git a/navigation/odom_transformer/config/odom_transformer_params.yaml b/navigation/odom_transformer/config/odom_transformer_params.yaml index d99f65ab1..8927450fe 100644 --- a/navigation/odom_transformer/config/odom_transformer_params.yaml +++ b/navigation/odom_transformer/config/odom_transformer_params.yaml @@ -5,6 +5,7 @@ publish_tf: true publish_pose: true publish_twist: true + rotate_yaw_180: false topics: input: "nucleus/odom" output: "odom" diff --git a/navigation/odom_transformer/include/odom_transformer/odom_transformer.hpp b/navigation/odom_transformer/include/odom_transformer/odom_transformer.hpp index 8a42bdb7b..b58f029dd 100644 --- a/navigation/odom_transformer/include/odom_transformer/odom_transformer.hpp +++ b/navigation/odom_transformer/include/odom_transformer/odom_transformer.hpp @@ -51,6 +51,7 @@ class OdomTransformer : public rclcpp::Node { bool publish_tf_{false}; bool publish_pose_{false}; bool publish_twist_{false}; + bool rotate_yaw_180_{false}; }; #endif // ODOM_TRANSFORMER__ODOM_TRANSFORMER_HPP_ diff --git a/navigation/odom_transformer/src/odom_transformer.cpp b/navigation/odom_transformer/src/odom_transformer.cpp index 86ef43f7e..a210c916c 100644 --- a/navigation/odom_transformer/src/odom_transformer.cpp +++ b/navigation/odom_transformer/src/odom_transformer.cpp @@ -22,6 +22,8 @@ OdomTransformer::OdomTransformer(const rclcpp::NodeOptions& options) this->declare_parameter("topics.output"); this->declare_parameter("topics.pose"); this->declare_parameter("topics.twist"); + rotate_yaw_180_ = this->declare_parameter("rotate_yaw_180"); + tf_buffer_ = std::make_shared(this->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); @@ -86,6 +88,29 @@ void OdomTransformer::odom_callback( Eigen::Quaterniond q_odom_sensor( msg->pose.pose.orientation.w, msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z); + + + // Velocities in sensor frame + Eigen::Vector3d v_sensor(msg->twist.twist.linear.x, + msg->twist.twist.linear.y, + msg->twist.twist.linear.z); + Eigen::Vector3d omega_sensor(msg->twist.twist.angular.x, + msg->twist.twist.angular.y, + msg->twist.twist.angular.z); + + if (rotate_yaw_180_) { + // 180 deg yaw flips X and Y, leaves Z unchanged + q_odom_sensor = + Eigen::Quaterniond(Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitZ())) + * q_odom_sensor; + v_sensor.x() = -v_sensor.x(); + v_sensor.y() = -v_sensor.y(); + omega_sensor.x() = -omega_sensor.x(); + omega_sensor.y() = -omega_sensor.y(); + msg->pose.pose.position.x = -msg->pose.pose.position.x; + msg->pose.pose.position.y = -msg->pose.pose.position.y; + } + Eigen::Matrix3d R_odom_sensor = q_odom_sensor.toRotationMatrix(); // Orientation: R_odom_base = R_odom_sensor * R_base_sensor^-1 @@ -100,16 +125,10 @@ void OdomTransformer::odom_callback( Eigen::Vector3d p_base = p_sensor - R_odom_base * t_base_sensor_; // Angular velocity: rotate from sensor frame to base_link frame - Eigen::Vector3d omega_sensor(msg->twist.twist.angular.x, - msg->twist.twist.angular.y, - msg->twist.twist.angular.z); Eigen::Vector3d omega_base = R_base_sensor_ * omega_sensor; // Linear velocity: lever arm correction // v_base = R_base_sensor * v_sensor - omega_base x t_base_sensor - Eigen::Vector3d v_sensor(msg->twist.twist.linear.x, - msg->twist.twist.linear.y, - msg->twist.twist.linear.z); Eigen::Vector3d v_base = R_base_sensor_ * v_sensor - omega_base.cross(t_base_sensor_); diff --git a/scripts/nautilus.sh b/scripts/nautilus.sh new file mode 100755 index 000000000..4583d4bd1 --- /dev/null +++ b/scripts/nautilus.sh @@ -0,0 +1,9 @@ +#!/bin/bash + +tmux new-session -d -s "nautilus" + +tmux split-window -v -t "nautilus:0" +tmux split-window -h -t "nautilus:0.0" +tmux split-window -h -t "nautilus:0.2" + +tmux attach-session -t "nautilus" diff --git a/scripts/nautilus_dp_launch.sh b/scripts/nautilus_dp_launch.sh new file mode 100755 index 000000000..e69de29bb From 9ca4b552425e61dd5e379427f42cf66fe069aeff Mon Sep 17 00:00:00 2001 From: Cyprian Osinski Date: Thu, 2 Apr 2026 22:18:40 +0200 Subject: [PATCH 268/290] tuned los a bit with vel controller and los guidance --- auv_setup/config/robots/nautilus.yaml | 2 +- .../config/param_velocity_controller_lqr.yaml | 24 +++++++++---------- .../los_guidance/config/guidance_params.yaml | 4 ++-- 3 files changed, 15 insertions(+), 15 deletions(-) diff --git a/auv_setup/config/robots/nautilus.yaml b/auv_setup/config/robots/nautilus.yaml index b6398615e..f38cd9649 100644 --- a/auv_setup/config/robots/nautilus.yaml +++ b/auv_setup/config/robots/nautilus.yaml @@ -16,7 +16,7 @@ /**: ros__parameters: physical: - center_of_mass: [0.0, 0.0, 0.015] # CO is aligned with CM Position (x,y) in meters (M), small cg offset in z to keep drone naturally stable in roll/pitch + center_of_mass: [0.0, 0.0, 0.02] # CO is aligned with CM Position (x,y) in meters (M), small cg offset in z to keep drone naturally stable in roll/pitch mass_matrix: [53.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 53.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 53.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 11.0628, 1.086, -3.17502, 0.0, 0.0, 0.0, 1.086, 23.1128, 0.1025, 0.0, 0.0, 0.0, -3.17502, 0.1025, 26.23998] # 6x6 mass_inertia_matrix propulsion: diff --git a/control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml b/control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml index 727c1c71e..c3afbdabe 100644 --- a/control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml +++ b/control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml @@ -1,23 +1,23 @@ /**: ros__parameters: - dt: 0.1 + dt: 0.05 LQR_params: - q_surge: 75 - q_pitch: 175 - q_yaw: 175 + q_surge: 40 + q_pitch: 80 + q_yaw: 80 - r_surge: 0.3 - r_pitch: 0.4 - r_yaw: 0.4 + r_surge: 0.4 + r_pitch: 0.6 + r_yaw: 0.6 - i_surge: 0.3 - i_pitch: 0.4 - i_yaw: 0.3 + i_surge: 0.2 + i_pitch: 0.2 + i_yaw: 0.2 i_weight: 0.5 - inertia_matrix: [30.0, 0.6, 0.0, 0.6, 1.629, 0.0, 0.0, 0.0, 1.729] + inertia_matrix: [53.7, 0.0, 0.0, 0.0, 23.1128, 0.1025, 0.0, 0.1025, 26.23998] #Clamp parameter - max_force: 99.5 + max_force: 80 diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 1a5368dbb..4921d8d91 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -1,7 +1,7 @@ # Adaptive LOS Parameters adaptive_los: - lookahead_distance_h: 0.9 - lookahead_distance_v: 1.4 + lookahead_distance_h: 0.6 + lookahead_distance_v: 1.0 gamma_h: 0.03 gamma_v: 0.02 From 28138d5e76fab0eb68b742ebac9e8e20c346bc91 Mon Sep 17 00:00:00 2001 From: vortexuser-Pi Date: Fri, 3 Apr 2026 13:56:13 +0200 Subject: [PATCH 269/290] reset nucleus pose on start --- auv_setup/launch/nucleus_odom_transformer.launch.py | 1 + auv_setup/launch/state_estimation.launch.py | 1 + 2 files changed, 2 insertions(+) diff --git a/auv_setup/launch/nucleus_odom_transformer.launch.py b/auv_setup/launch/nucleus_odom_transformer.launch.py index 4c94fee1b..a1aa0bd31 100644 --- a/auv_setup/launch/nucleus_odom_transformer.launch.py +++ b/auv_setup/launch/nucleus_odom_transformer.launch.py @@ -47,6 +47,7 @@ def launch_setup(context, *args, **kwargs): "connection_params.remote_ip": "10.0.0.42", "connection_params.data_remote_port": 9000, "connection_params.password": "", + "reset_pose_on_start": True, "enable_imu": True, "enable_ins_odom": True, "enable_dvl": True, diff --git a/auv_setup/launch/state_estimation.launch.py b/auv_setup/launch/state_estimation.launch.py index 8423709e6..a7520cb90 100644 --- a/auv_setup/launch/state_estimation.launch.py +++ b/auv_setup/launch/state_estimation.launch.py @@ -135,6 +135,7 @@ def launch_setup(context, *args, **kwargs): "connection_params.remote_ip": "10.0.0.42", "connection_params.data_remote_port": 9000, "connection_params.password": "", + "reset_pose_on_start": True, "enable_imu": False, "enable_ins_odom": False, "enable_dvl": True, From 9d502ca83218e8579291d1e043ce7fdac49ff33e Mon Sep 17 00:00:00 2001 From: Cyprian Osinski Date: Fri, 3 Apr 2026 15:00:25 +0200 Subject: [PATCH 270/290] changed joystick and auv_setup to be able to give references in both quat and rpy. Also updated their respective README files --- auv_setup/README.md | 32 +++++++++++- auv_setup/launch/dp.launch.py | 9 +++- mission/joystick_interface_auv/README.md | 44 +++++++++++++++- .../joystick_interface_auv_node.py | 52 ++++++++++++++++--- .../launch/joystick_interface_auv.launch.py | 20 +++++-- 5 files changed, 142 insertions(+), 15 deletions(-) diff --git a/auv_setup/README.md b/auv_setup/README.md index ead2200b4..56a4b2172 100644 --- a/auv_setup/README.md +++ b/auv_setup/README.md @@ -10,8 +10,36 @@ The config folder contains physical parameters related to the AUV and the enviro * thrusters: Thruster configs for different thruster types ### Launch -This package contains a launchfile for each specific AUV. Additionally the topside.launch file is to -be used on the topside computer that the joystick is connected to, for ROV operations. + +This package contains launchfiles for each specific AUV. Additionally `topside.launch.py` is used on the topside computer that the joystick is connected to, for ROV operations. + +#### dp.launch.py + +Launches the DP (dynamic positioning) stack — a reference filter and a controller — as a composable node container. + +```bash +ros2 launch auv_setup dp.launch.py +``` + +| Argument | Default | Description | +|---|---|---| +| `drone` | `nautilus` | Drone model — loads `auv_setup/config/robots/.yaml` | +| `namespace` | `` | ROS namespace. Defaults to the drone name if left empty | +| `controller_type` | `adaptive` | Controller to use: `adaptive` or `pid` | + +**`adaptive`** — launches `dp_adapt_backs_controller` with `reference_filter_dp` (Euler/RPY reference filter). Controller params are loaded from `dp_adapt_backs_controller/config/adapt_params_.yaml`. + +```bash +ros2 launch auv_setup dp.launch.py controller_type:=adaptive drone:=nautilus +``` + +**`pid`** — launches `pid_controller_dp` with `reference_filter_dp_quat` (quaternion reference filter). Controller params are loaded from `pid_controller_dp/config/pid_params.yaml`. + +```bash +ros2 launch auv_setup dp.launch.py controller_type:=pid drone:=nautilus +``` + +> When using the joystick to send references, make sure `orientation_mode` in `joystick_interface_auv` matches the controller: use `euler` with `adaptive` and `quat` with `pid`. ### Description The description folder contains the URDF and xacro files for the AUVs. The main description launch file is drone_description.launch.py, which makes all static transforms available to the ros graph. diff --git a/auv_setup/launch/dp.launch.py b/auv_setup/launch/dp.launch.py index a0d42de7c..12510e6a4 100644 --- a/auv_setup/launch/dp.launch.py +++ b/auv_setup/launch/dp.launch.py @@ -69,6 +69,11 @@ def launch_setup(context, *args, **kwargs): "config", "pid_params.yaml", ) + filter_quat_file_path = os.path.join( + get_package_share_directory("reference_filter_dp_quat"), + "config", + "reference_filter_params.yaml", + ) container = ComposableNodeContainer( name="dp_container", namespace=namespace, @@ -76,11 +81,11 @@ def launch_setup(context, *args, **kwargs): executable="component_container_mt", composable_node_descriptions=[ ComposableNode( - package="reference_filter_dp", + package="reference_filter_dp_quat", plugin="ReferenceFilterNode", name="reference_filter_node", namespace=namespace, - parameters=[filter_file_path, drone_params], + parameters=[filter_quat_file_path, drone_params], extra_arguments=[{"use_intra_process_comms": True}], ), ComposableNode( diff --git a/mission/joystick_interface_auv/README.md b/mission/joystick_interface_auv/README.md index 36ae93c7b..eb7993f28 100644 --- a/mission/joystick_interface_auv/README.md +++ b/mission/joystick_interface_auv/README.md @@ -1,2 +1,44 @@ ## Joystick interface -A joystick interface for manual control of AUV. A ROS2 node that subscribes on inputs from the XBOX controller and publishes the according wrench message to be used in Thruster Allocation. + +A joystick interface for manual control and reference sending of the AUV. Subscribes to Xbox controller inputs and publishes wrench commands (manual mode) or pose references (reference/guidance mode) depending on the active operation mode. + +### Launching + +```bash +ros2 launch joystick_interface_auv joystick_interface_auv.launch.py +``` + +#### Launch arguments + +| Argument | Default | Description | +|---|---|---| +| `drone` | `nautilus` | Drone model — loads the matching config from `auv_setup/config/robots/.yaml` | +| `namespace` | `` | ROS namespace. Defaults to the drone name if left empty | +| `orientation_mode` | `euler` | Reference orientation representation: `euler` (publishes `ReferenceFilter` with RPY angles) or `quat` (publishes `ReferenceFilterQuat` with quaternion) | + +The `orientation_mode` must match the reference filter used by the active DP controller. Use `euler` with the adaptive backstepping controller (`reference_filter_dp`) and `quat` with the PID controller (`reference_filter_dp_quat`). + +Example — launch with quaternion mode for use with the PID controller: + +```bash +ros2 launch joystick_interface_auv joystick_interface_auv.launch.py orientation_mode:=quat +``` + +### Controller button mapping + +| Button | Action | +|---|---| +| **A** | Manual mode (direct wrench from joystick axes) | +| **B** | Toggle software killswitch | +| **X** | Autonomous mode | +| **Y** | Reference mode (joystick incrementally updates the pose reference sent to the DP controller) | + +In **reference mode**, the left stick controls surge/sway, triggers control heave, the right stick controls pitch/yaw, and shoulder buttons control roll. Movement is expressed in the body frame and rotated into the world frame before being added to the desired pose. + +### Config + +Gains for both manual wrench output and reference increments are set in `config/param_joystick_interface_auv.yaml`: + +- `joystick_*_gain` — scales raw axis/button input to force/torque in manual mode +- `guidance_*_gain` — scales input to position/orientation increments in reference mode +- `debounce_duration` — minimum seconds between button state changes (prevents double-triggers) diff --git a/mission/joystick_interface_auv/joystick_interface_auv/joystick_interface_auv_node.py b/mission/joystick_interface_auv/joystick_interface_auv/joystick_interface_auv_node.py index 5d4a51443..934b9bb89 100755 --- a/mission/joystick_interface_auv/joystick_interface_auv/joystick_interface_auv_node.py +++ b/mission/joystick_interface_auv/joystick_interface_auv/joystick_interface_auv_node.py @@ -6,9 +6,9 @@ from rclpy.node import Node, Parameter from sensor_msgs.msg import JointState, Joy from std_msgs.msg import Bool -from vortex_msgs.msg import OperationMode, ReferenceFilter +from vortex_msgs.msg import OperationMode, ReferenceFilter, ReferenceFilterQuat from vortex_msgs.srv import GetOperationMode, SetOperationMode, ToggleKillswitch -from vortex_utils.python_utils import PoseData +from vortex_utils.python_utils import PoseData, euler_to_quat from vortex_utils_ros.ros_converter import pose_from_ros from vortex_utils_ros.qos_profiles import ( reliable_profile, @@ -120,6 +120,14 @@ def get_parameters(self): self.declare_parameter('topics.guidance.dp', Parameter.Type.STRING) self.guidance_topic = self.get_parameter('topics.guidance.dp').value + self.declare_parameter('orientation_mode', 'euler') + self._orientation_mode = self.get_parameter('orientation_mode').value + if self._orientation_mode not in ('euler', 'quat'): + self.get_logger().warn( + f"Unknown orientation_mode '{self._orientation_mode}', defaulting to 'euler'" + ) + self._orientation_mode = 'euler' + def init_movement(self): self.surge = 0.0 self.sway = 0.0 @@ -156,9 +164,14 @@ def set_publishers_and_subscribers(self): self._wrench_publisher = self.create_publisher( WrenchStamped, self.wrench_input_topic, qos_profile=best_effort_qos ) - self._ref_publisher = self.create_publisher( - ReferenceFilter, self.guidance_topic, qos_profile=best_effort_qos - ) + if self._orientation_mode == 'quat': + self._ref_publisher = self.create_publisher( + ReferenceFilterQuat, self.guidance_topic, qos_profile=best_effort_qos + ) + else: + self._ref_publisher = self.create_publisher( + ReferenceFilter, self.guidance_topic, qos_profile=best_effort_qos + ) self._gripper_publisher = self.create_publisher( JointState, self.gripper_servos_topic, qos_profile=best_effort_qos @@ -230,6 +243,25 @@ def create_reference_message(self) -> ReferenceFilter: reference_msg.yaw = self._desired_state.yaw return reference_msg + def create_reference_quat_message(self) -> ReferenceFilterQuat: + """Creates a reference message with quaternion orientation from the desired state.""" + q = euler_to_quat( + self._desired_state.roll, + self._desired_state.pitch, + self._desired_state.yaw, + ) + reference_msg = ReferenceFilterQuat() + reference_msg.header.stamp = self.get_clock().now().to_msg() + reference_msg.header.frame_id = "base_link" + reference_msg.x = self._desired_state.x + reference_msg.y = self._desired_state.y + reference_msg.z = self._desired_state.z + reference_msg.qx = float(q[0]) + reference_msg.qy = float(q[1]) + reference_msg.qz = float(q[2]) + reference_msg.qw = float(q[3]) + return reference_msg + def create_wrench_message(self) -> WrenchStamped: """Creates a 3D wrench message with the given x, y, heave, roll, pitch, and yaw values. @@ -256,6 +288,12 @@ def transition_to_xbox_mode(self): future.add_done_callback(self.operation_mode_response_callback) self.get_logger().info("XBOX mode") + def _create_reference_msg(self): + """Returns the appropriate reference message based on orientation_mode.""" + if self._orientation_mode == 'quat': + return self.create_reference_quat_message() + return self.create_reference_message() + def transition_to_reference_mode(self): """Publishes a pose message and signals that the operational mode has switched to Reference mode.""" self._desired_state = PoseData( @@ -266,7 +304,7 @@ def transition_to_reference_mode(self): pitch=self._current_state.pitch, yaw=self._current_state.yaw, ) - reference_msg = self.create_reference_message() + reference_msg = self._create_reference_msg() # Still autonomous mode, but now the reference is being controlled by the joystick request = SetOperationMode.Request() @@ -472,7 +510,7 @@ def joystick_cb(self, msg: Joy): self._wrench_publisher.publish(wrench_msg) else: self.update_reference() - ref_msg = self.create_reference_message() + ref_msg = self._create_reference_msg() self._ref_publisher.publish(ref_msg) close = float(buttons.get("stick_button_left", 0)) diff --git a/mission/joystick_interface_auv/launch/joystick_interface_auv.launch.py b/mission/joystick_interface_auv/launch/joystick_interface_auv.launch.py index 1359a43fd..e5acfdf9c 100755 --- a/mission/joystick_interface_auv/launch/joystick_interface_auv.launch.py +++ b/mission/joystick_interface_auv/launch/joystick_interface_auv.launch.py @@ -2,7 +2,8 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import OpaqueFunction +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node from auv_setup.launch_arg_common import ( @@ -14,6 +15,7 @@ def launch_setup(context, *args, **kwargs): """Set up the joystick_interface_auv node with drone-specific config.""" drone, namespace = resolve_drone_and_namespace(context) + orientation_mode = LaunchConfiguration("orientation_mode").perform(context) joystick_params = os.path.join( get_package_share_directory("joystick_interface_auv"), @@ -35,7 +37,11 @@ def launch_setup(context, *args, **kwargs): name="joystick_interface_auv", namespace=namespace, output="screen", - parameters=[joystick_params, drone_params, {"drone": drone}], + parameters=[ + joystick_params, + drone_params, + {"drone": drone, "orientation_mode": orientation_mode}, + ], ) ] @@ -53,5 +59,13 @@ def generate_launch_description() -> LaunchDescription: """ return LaunchDescription( - declare_drone_and_namespace_args() + [OpaqueFunction(function=launch_setup)] + declare_drone_and_namespace_args() + + [ + DeclareLaunchArgument( + "orientation_mode", + default_value="euler", + description="Reference orientation representation: 'euler' (ReferenceFilter) or 'quat' (ReferenceFilterQuat)", + ), + OpaqueFunction(function=launch_setup), + ] ) From 7a3192d3d5321986bb472d5889b0a2cff035a90f Mon Sep 17 00:00:00 2001 From: Cyprian Osinski Date: Fri, 3 Apr 2026 16:19:45 +0200 Subject: [PATCH 271/290] changed such that quat dp utilizes quat more efficiently --- .../pid_controller_dp/config/pid_params.yaml | 28 ++++++------ .../pid_controller_dp/pid_controller.hpp | 2 +- .../pid_controller_dp/pid_controller_ros.hpp | 6 +-- .../pid_controller_utils.hpp | 45 +++++++++---------- .../include/pid_controller_dp/typedefs.hpp | 8 ++-- .../pid_controller_dp/src/pid_controller.cpp | 34 +++++++------- .../src/pid_controller_ros.cpp | 27 ++++------- .../src/pid_controller_utils.cpp | 29 ++++++------ utility_scripts/build_ws_topside.sh | 17 ++++--- 9 files changed, 97 insertions(+), 99 deletions(-) diff --git a/control/pid_controller_dp/config/pid_params.yaml b/control/pid_controller_dp/config/pid_params.yaml index e133febef..286807f3c 100644 --- a/control/pid_controller_dp/config/pid_params.yaml +++ b/control/pid_controller_dp/config/pid_params.yaml @@ -1,20 +1,20 @@ /**: ros__parameters: - Kp_x: 30.0 - Kp_y: 30.0 + Kp_x: 20.0 + Kp_y: 20.0 Kp_z: 40.0 - Kp_roll: 30.0 - Kp_pitch: 60.0 - Kp_yaw: 10.0 + Kp_roll: 0.0 + Kp_pitch: 15.0 + Kp_yaw: 7.5 Ki_x: 0.01 - Ki_y: 0.001 - Ki_z: 0.05 - Ki_roll: 0.01 - Ki_pitch: 0.005 - Ki_yaw: 0.01 + Ki_y: 0.01 + Ki_z: 0.025 + Ki_roll: 0.0 + Ki_pitch: 0.05 + Ki_yaw: 0.02 Kd_x: 5.0 Kd_y: 5.0 - Kd_z: 5.0 - Kd_roll: 0.002 - Kd_pitch: 0.005 - Kd_yaw: 0.002 + Kd_z: 10.0 + Kd_roll: 0.0 + Kd_pitch: 5.0 + Kd_yaw: 0.02 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 52a855262..389249311 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 @@ -45,7 +45,7 @@ class PIDController { types::Matrix6d Kp_; types::Matrix6d Ki_; types::Matrix6d Kd_; - types::Vector7d integral_; + types::Vector6d integral_; double dt_; }; 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 4c631e3e9..6b09d4fb6 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 @@ -16,7 +16,7 @@ #include #include #include -#include +#include #include #include "pid_controller_dp/pid_controller.hpp" #include "pid_controller_dp/typedefs.hpp" @@ -53,7 +53,7 @@ class PIDControllerNode : public rclcpp::Node { // @param msg: ReferenceFilter message containing the desired vehicle pose // and velocity void guidance_callback( - const vortex_msgs::msg::ReferenceFilter::SharedPtr msg); + const vortex_msgs::msg::ReferenceFilterQuat::SharedPtr msg); // @brief Callback function for the odometry topic // @param msg: Odometry message containing the AUV pose and speed @@ -76,7 +76,7 @@ class PIDControllerNode : public rclcpp::Node { rclcpp::Subscription::SharedPtr odom_sub_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr guidance_sub_; rclcpp::Subscription::SharedPtr kp_sub_; 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 dcd79b400..28c7f10ae 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 @@ -23,26 +23,26 @@ double ssa(double angle); // p.34 eq: 2.72 types::Matrix3d calculate_R_quat(const types::Eta& eta); -// @brief Calculate the transformation matrix from a quaternion -// @param q: Quaternion represented as a 4D vector [w, x, y, z] -// @return 4x3 transformation matrix +// @brief Calculate the transformation sub-matrix from a quaternion. +// Returns the bottom 3 rows of the full 4x3 T matrix (rows for qx, qy, qz +// derivatives), giving a 3x3 mapping from body angular velocity to d/dt [qx, +// qy, qz]. // REF: Handbook of Marine Craft Hydrodynamics and Motion Control, Fossen 2021 // p.35 eq: 2.78 -types::Matrix4x3d calculate_T_quat(const types::Eta& eta); +types::Matrix3d calculate_T_quat(const types::Eta& eta); -// @brief Calculate the Jacobian matrix -// @param eta: 7D vector containing the vehicle pose [x, y, z, w, x, y, z] -// @return 7x6 Jacobian matrix -// REF: Handbook of Marine Craft Hydrodynamics and Motion Control, Fossen 2021 -// p.36 eq: 2.83 +// @brief Calculate the 6x6 Jacobian matrix using only the vector part of the +// quaternion. +// J = blockdiag(R, T_33) where T_33 maps body angular velocity to d/dt +// [qx,qy,qz]. +// @param eta: vehicle pose [x, y, z, qw, qx, qy, qz] +// @return 6x6 Jacobian matrix types::J_transformation calculate_J(const types::Eta& eta); -// @brief Calculate the pseudo-inverse of the Jacobian matrix -// @param eta: 7D vector containing the vehicle pose [x, y, z, w, x, y, z] -// @return 6x7 pseudo-inverse Jacobian matrix -// REF: Handbook of Marine Craft Hydrodynamics and Motion Control, Fossen 2021 -// p.34 eq: 2.72 -types::Matrix6x7d calculate_J_sudo_inv(const types::Eta& eta); +// @brief Calculate the inverse of the 6x6 Jacobian matrix. +// @param eta: vehicle pose [x, y, z, qw, qx, qy, qz] +// @return 6x6 inverse Jacobian matrix +types::Matrix6d calculate_J_sudo_inv(const types::Eta& eta); // @brief Calculate the error between the desired and actual vehicle pose // @param eta: 7D vector containing the actual vehicle pose [x, y, z, w, x, y, @@ -63,15 +63,14 @@ Eigen::VectorXd clamp_values(const Eigen::VectorXd& values, double min_val, double max_val); -// @brief Calculate the anti-windup term +// @brief Calculate the anti-windup term using the 6D error [x,y,z,qx,qy,qz] +// (qw is excluded since only the vector part of the quaternion is used). // @param dt: Time step -// @param error: 7D vector containing the error between the desired and -// actual vehicle pose [x, y, z, w, x, y, z] -// @param integral: 7D vector containing the integral term of the PID -// controller [x, y, z, w, x, y, z] -// @return 7D vector containing the anti-windup term -types::Vector7d anti_windup(const double dt, +// @param error: Eta error struct (only x,y,z,qx,qy,qz are read) +// @param integral: 6D integral term [x, y, z, qx, qy, qz] +// @return 6D anti-windup clamped integral +types::Vector6d anti_windup(const double dt, const types::Eta& error, - const types::Vector7d& integral); + const types::Vector6d& integral); #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 d2f6a8707..5b8a65373 100644 --- a/control/pid_controller_dp/include/pid_controller_dp/typedefs.hpp +++ b/control/pid_controller_dp/include/pid_controller_dp/typedefs.hpp @@ -28,12 +28,12 @@ using Nu = ::vortex::utils::types::Twist; struct J_transformation { Matrix3d R = Matrix3d::Identity(); - Matrix4x3d T = Matrix4x3d::Zero(); + Matrix3d T = Matrix3d::Zero(); - Matrix7x6d as_matrix() const { - Matrix7x6d mat = Matrix7x6d::Zero(); + Matrix6d as_matrix() const { + Matrix6d mat = Matrix6d::Zero(); mat.block<3, 3>(0, 0) = R; - mat.block<4, 3>(3, 3) = T; + mat.block<3, 3>(3, 3) = T; return mat; } }; diff --git a/control/pid_controller_dp/src/pid_controller.cpp b/control/pid_controller_dp/src/pid_controller.cpp index 8867b160f..90afbb652 100644 --- a/control/pid_controller_dp/src/pid_controller.cpp +++ b/control/pid_controller_dp/src/pid_controller.cpp @@ -2,7 +2,7 @@ #include "pid_controller_dp/pid_controller_utils.hpp" void print_eta(const types::Eta& eta) { - // spdlog::info("Eta values:"); + spdlog::info("Eta values:"); auto pos = eta.pos_vector(); auto ori = eta.ori_quaternion(); spdlog::info("Position - North: {}, East: {}, Down: {}", pos[0], pos[1], @@ -71,7 +71,7 @@ PIDController::PIDController() : Kp_(types::Matrix6d::Identity()), Ki_(types::Matrix6d::Zero()), Kd_(types::Matrix6d::Zero()), - integral_(types::Vector7d::Zero()), + integral_(types::Vector6d::Zero()), dt_(0.01) {} types::Vector6d PIDController::calculate_tau(const types::Eta& eta, @@ -80,26 +80,24 @@ types::Vector6d PIDController::calculate_tau(const types::Eta& eta, const types::Eta& eta_dot_d) { types::Eta error = error_eta(eta, eta_d); // calculate eta error - // set quaternion scalar part w = 0 (only use vector part of quaternion for - // error) - error.qw = 0.0; + // Only use the vector part of the quaternion for orientation error. + types::Vector6d error_6; + error_6 << error.x, error.y, error.z, error.qx, error.qy, error.qz; - auto eta_dot_d_copy = eta_dot_d; - eta_dot_d_copy.qw = 0.0; // set w = 0 for desired eta_dot + // Desired velocity: also drop qw, keeping [x,y,z,qx,qy,qz] derivatives. + types::Vector6d eta_dot_d_6; + eta_dot_d_6 << eta_dot_d.x, eta_dot_d.y, eta_dot_d.z, + eta_dot_d.qx, eta_dot_d.qy, eta_dot_d.qz; - types::Matrix6x7d J_inv = - calculate_J_sudo_inv(eta); // calculate J pseudo inverse + // 6x6 J inverse: blockdiag(R, T_33)^{-1} + types::Matrix6d J_inv = calculate_J_sudo_inv(eta); - types::Vector6d nu_d = - J_inv * eta_dot_d_copy.to_vector(); // calculate velocity - - types::Vector6d error_nu = nu.to_vector() - nu_d; // calculate vel error - - types::Vector6d P = Kp_ * J_inv * error.to_vector(); // P term + types::Vector6d nu_d = J_inv * eta_dot_d_6; // desired body velocity + types::Vector6d error_nu = nu.to_vector() - nu_d; // velocity error + types::Vector6d P = Kp_ * J_inv * error_6; // P term types::Vector6d I = Ki_ * J_inv * integral_; // I term - - types::Vector6d D = Kd_ * error_nu; // D term + types::Vector6d D = Kd_ * error_nu; // D term types::Vector6d tau = -clamp_values((P + I + D), -80.0, 80.0); @@ -132,4 +130,4 @@ types::Matrix6d PIDController::get_ki() { } types::Matrix6d PIDController::get_kd() { return this->Kd_; -} \ No newline at end of file +} diff --git a/control/pid_controller_dp/src/pid_controller_ros.cpp b/control/pid_controller_dp/src/pid_controller_ros.cpp index 42290f13d..3c97ea9a5 100644 --- a/control/pid_controller_dp/src/pid_controller_ros.cpp +++ b/control/pid_controller_dp/src/pid_controller_ros.cpp @@ -1,6 +1,6 @@ #include -#include #include +#include #include #include #include @@ -18,7 +18,7 @@ constexpr std::string_view start_message = R"( )"; -PIDControllerNode::PIDControllerNode(const rclcpp::NodeOptions & options) +PIDControllerNode::PIDControllerNode(const rclcpp::NodeOptions& options) : Node("pid_controller_node", options) { time_step_ = std::chrono::milliseconds(10); @@ -76,7 +76,7 @@ void PIDControllerNode::set_subscribers_and_publisher() { std::placeholders::_1)); guidance_sub_ = - this->create_subscription( + this->create_subscription( dp_reference_topic, qos_sensor_data, std::bind(&PIDControllerNode::guidance_callback, this, std::placeholders::_1)); @@ -215,26 +215,17 @@ void PIDControllerNode::set_pid_params() { } void PIDControllerNode::guidance_callback( - const vortex_msgs::msg::ReferenceFilter::SharedPtr msg) { + const vortex_msgs::msg::ReferenceFilterQuat::SharedPtr msg) { // Set desired position eta_d_.x = msg->x; eta_d_.y = msg->y; eta_d_.z = msg->z; - // Convert desired attitude (roll, pitch, yaw) to quaternion and store - double roll = msg->roll; - double pitch = msg->pitch; - double yaw = msg->yaw; - - Eigen::Quaterniond quat = - Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX()) * - Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()); - - eta_d_.qw = quat.w(); - eta_d_.qx = quat.x(); - eta_d_.qy = quat.y(); - eta_d_.qz = quat.z(); + // set desired ori quaternion + eta_d_.qw = msg->qw; + eta_d_.qx = msg->qx; + eta_d_.qy = msg->qy; + eta_d_.qz = msg->qz; } rcl_interfaces::msg::SetParametersResult PIDControllerNode::parametersCallback( diff --git a/control/pid_controller_dp/src/pid_controller_utils.cpp b/control/pid_controller_dp/src/pid_controller_utils.cpp index 834322e11..7f56666ae 100644 --- a/control/pid_controller_dp/src/pid_controller_utils.cpp +++ b/control/pid_controller_dp/src/pid_controller_utils.cpp @@ -9,18 +9,20 @@ types::Matrix3d calculate_R_quat(const types::Eta& eta) { return eta.as_rotation_matrix(); } -types::Matrix4x3d calculate_T_quat(const types::Eta& eta) { - return eta.as_transformation_matrix(); +types::Matrix3d calculate_T_quat(const types::Eta& eta) { + // Full T is 4x3; rows 1-3 map body angular velocity to d/dt[qx, qy, qz]. + return eta.as_transformation_matrix().bottomRows<3>(); } -types::Matrix6x7d calculate_J_sudo_inv(const types::Eta& eta) { - auto J_matrix = eta.as_j_matrix(); - Eigen::MatrixXd J_pseudo_inv_dynamic = - vortex::utils::math::pseudo_inverse(J_matrix); +types::Matrix6d calculate_J_sudo_inv(const types::Eta& eta) { + types::Matrix3d R = calculate_R_quat(eta); + types::Matrix3d T = calculate_T_quat(eta); - types::Matrix6x7d J_pseudo_inv; - J_pseudo_inv = J_pseudo_inv_dynamic; - return J_pseudo_inv; + types::Matrix6d J = types::Matrix6d::Zero(); + J.topLeftCorner<3, 3>() = R; + J.bottomRightCorner<3, 3>() = T; + + return J.inverse(); } types::Eta error_eta(const types::Eta& eta, const types::Eta& eta_d) { @@ -33,9 +35,10 @@ Eigen::VectorXd clamp_values(const Eigen::VectorXd& values, return vortex::utils::math::clamp_values(values, min_val, max_val); } -types::Vector7d anti_windup(const double dt, +types::Vector6d anti_windup(const double dt, const types::Eta& error, - const types::Vector7d& integral) { - return vortex::utils::math::anti_windup(dt, error.to_vector(), integral, - -80.0, 80.0); + const types::Vector6d& integral) { + types::Vector6d error_6; + error_6 << error.x, error.y, error.z, error.qx, error.qy, error.qz; + return vortex::utils::math::anti_windup(dt, error_6, integral, -80.0, 80.0); } diff --git a/utility_scripts/build_ws_topside.sh b/utility_scripts/build_ws_topside.sh index 5edaebe41..a4b8fe0c1 100755 --- a/utility_scripts/build_ws_topside.sh +++ b/utility_scripts/build_ws_topside.sh @@ -16,12 +16,19 @@ REPOS=( "vortex-ci" ) -# ---- Install dependencies script ---- +# ---- Install dependencies ---- -# python3 -m pip install --upgrade pip -# install numpy, pynput, joy, wheel -# install ros-humble-xacro, ros-humble-joy -# run casadi install script +echo "[DEPS] Upgrading pip..." +python3 -m pip install --upgrade pip + +echo "[DEPS] Installing Python packages..." +python3 -m pip install numpy pynput wheel + +echo "[DEPS] Installing ROS apt packages..." +ROS_DISTRO="${ROS_DISTRO:-humble}" +sudo apt-get install -y --no-install-recommends \ + ros-${ROS_DISTRO}-xacro \ + ros-${ROS_DISTRO}-joy # ------------ Clone missing repos ------------ mkdir -p "$SRC_DIR" From e1edd91b0462205b313463b672baf9f5cfa138fa Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Fri, 3 Apr 2026 17:07:04 +0200 Subject: [PATCH 272/290] urfd: updated gripper position --- auv_setup/description/nautilus.urdf.xacro | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/auv_setup/description/nautilus.urdf.xacro b/auv_setup/description/nautilus.urdf.xacro index f330fb6a2..baa8d9ac1 100644 --- a/auv_setup/description/nautilus.urdf.xacro +++ b/auv_setup/description/nautilus.urdf.xacro @@ -66,7 +66,7 @@ - + From 9a086fc3c5d016341aaa506e80da9469daecccb5 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Fri, 3 Apr 2026 17:24:26 +0200 Subject: [PATCH 273/290] URDF: update down cam link --- auv_setup/description/nautilus.urdf.xacro | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/auv_setup/description/nautilus.urdf.xacro b/auv_setup/description/nautilus.urdf.xacro index baa8d9ac1..f7db7222c 100644 --- a/auv_setup/description/nautilus.urdf.xacro +++ b/auv_setup/description/nautilus.urdf.xacro @@ -46,7 +46,7 @@ - + From 23ba63afa2a81326fa7e5915fa4d2cf12e753a5d Mon Sep 17 00:00:00 2001 From: Cyprian Osinski Date: Fri, 3 Apr 2026 17:40:32 +0200 Subject: [PATCH 274/290] new tuning values --- .../config/adapt_params_nautilus.yaml | 8 ++--- .../src/dp_adapt_backs_controller_ros.cpp | 2 +- .../pid_controller_dp/config/pid_params.yaml | 30 +++++++++---------- 3 files changed, 20 insertions(+), 20 deletions(-) diff --git a/control/dp_adapt_backs_controller/config/adapt_params_nautilus.yaml b/control/dp_adapt_backs_controller/config/adapt_params_nautilus.yaml index 6d1f0e1e5..de23818f7 100644 --- a/control/dp_adapt_backs_controller/config/adapt_params_nautilus.yaml +++ b/control/dp_adapt_backs_controller/config/adapt_params_nautilus.yaml @@ -1,7 +1,7 @@ /**: ros__parameters: - K1 : [9.0, 9.0, 11.0, 1.0, 4.5, 4.5] # Outer loop tuning parameters - K2 : [20.0, 20.0, 25.0, 3.0, 7.5, 7.5] # Inner loop tuning parameters + K1 : [3.5, 3.5, 2.0, 1.0, 2.0, 4.0] # Outer loop tuning parameters + K2 : [14.5, 14.5, 35.0, 10.0, 35.0, 13.5] # Inner loop tuning parameters r_b_bg : [0.0, 0.0, 0.015] # Vector from body centre to centre of gravity - adapt_gain : [0.3, 0.05, 0.3, 0.05, 0.3, 0.05, 0.3, 0.05, 0.3, 0.05, 0.3, 0.05] # Tuning parameters for linear and nonlinear damping - d_gain : [0.4, 0.4, 0.4, 0.4, 0.4, 0.4] # Tuning parameters for the unmodeled disturbances and uncertanties + adapt_gain : [0.2, 0.05, 0.2, 0.05, 0.2, 0.05, 0.2, 0.05, 0.2, 0.05, 0.2, 0.05] # Tuning parameters for linear and nonlinear damping + d_gain : [0.2, 0.2, 0.2, 0.2, 0.2, 0.2] # Tuning parameters for the unmodeled disturbances and uncertanties 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 3cb708f12..cbc3e8514 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 @@ -297,7 +297,7 @@ void DPAdaptBacksControllerNode::publish_tau() { tau_msg.wrench.force.z = tau(2); // comment out if roll control is not needed - //tau_msg.wrench.torque.x = tau(3); + tau_msg.wrench.torque.x = tau(3); tau_msg.wrench.torque.y = tau(4); tau_msg.wrench.torque.z = tau(5); diff --git a/control/pid_controller_dp/config/pid_params.yaml b/control/pid_controller_dp/config/pid_params.yaml index 286807f3c..a3356ece0 100644 --- a/control/pid_controller_dp/config/pid_params.yaml +++ b/control/pid_controller_dp/config/pid_params.yaml @@ -1,20 +1,20 @@ /**: ros__parameters: - Kp_x: 20.0 - Kp_y: 20.0 + Kp_x: 40.0 + Kp_y: 40.0 Kp_z: 40.0 - Kp_roll: 0.0 - Kp_pitch: 15.0 - Kp_yaw: 7.5 - Ki_x: 0.01 - Ki_y: 0.01 - Ki_z: 0.025 - Ki_roll: 0.0 - Ki_pitch: 0.05 + Kp_roll: 7.5 + Kp_pitch: 11.0 + Kp_yaw: 9.0 + Ki_x: 0.04 + Ki_y: 0.04 + Ki_z: 0.02 + Ki_roll: 0.02 + Ki_pitch: 0.04 Ki_yaw: 0.02 - Kd_x: 5.0 - Kd_y: 5.0 - Kd_z: 10.0 - Kd_roll: 0.0 + Kd_x: 7.5 + Kd_y: 7.5 + Kd_z: 12.5 + Kd_roll: 0.1 Kd_pitch: 5.0 - Kd_yaw: 0.02 + Kd_yaw: 0.1 From 6402f5f9d48c1b321ad480be4e30055c333c79c1 Mon Sep 17 00:00:00 2001 From: Anbit Date: Fri, 3 Apr 2026 17:58:14 +0200 Subject: [PATCH 275/290] Guidance changes --- guidance/los_guidance/README.md | 42 +- .../los_guidance/config/guidance_params.yaml | 60 ++- .../include/los_guidance/lib/adaptive_los.hpp | 103 ++++- .../include/los_guidance/lib/integral_los.hpp | 93 ++++- .../los_guidance/lib/proportional_los.hpp | 71 +++- .../include/los_guidance/lib/types.hpp | 51 ++- .../los_guidance/lib/vector_field_los.hpp | 78 +++- .../include/los_guidance/los_guidance_ros.hpp | 163 ++++++-- .../launch/guidance_test.launch.py | 110 ++---- .../los_guidance/scripts/test_scenarios.py | 12 +- .../los_guidance/src/lib/adaptive_los.cpp | 41 +- .../los_guidance/src/lib/integral_los.cpp | 38 +- .../los_guidance/src/lib/proportional_los.cpp | 27 +- .../los_guidance/src/lib/vector_field_los.cpp | 37 +- .../los_guidance/src/los_guidance_ros.cpp | 374 +++++++++++------- guidance/los_guidance/test/CMakeLists.txt | 1 + .../los_guidance/test/adaptive_los_test.cpp | 24 +- .../los_guidance/test/integral_los_test.cpp | 22 +- .../test/los_invalid_params_test.cpp | 56 +++ .../test/proportional_los_test.cpp | 18 +- .../test/vector_field_los_test.cpp | 22 +- 21 files changed, 998 insertions(+), 445 deletions(-) create mode 100644 guidance/los_guidance/test/los_invalid_params_test.cpp diff --git a/guidance/los_guidance/README.md b/guidance/los_guidance/README.md index 10a3a6341..b482a74e6 100644 --- a/guidance/los_guidance/README.md +++ b/guidance/los_guidance/README.md @@ -30,7 +30,7 @@ The library supports four LOS guidance algorithms. | 2 | Adaptive LOS | | 3 | Vector Field LOS | -The guidance method can be **changed during runtime** using a ROS service. +The guidance method can be preconfigured in the package configuration file before startup, and changed during runtime using a ROS service. --- @@ -40,7 +40,7 @@ The guidance method can be **changed during runtime** using a ROS service. A waypoint can be sent to the guidance node using the action interface: ``` -ros2 action send_goal /orca/los_guidance \ +ros2 action send_goal /drone_name/los_guidance \ vortex_msgs/action/LOSGuidance \ "{goal: {header: {frame_id: world_ned}, point: {x: 0.0, y: 0.0, z: 0.0}}}" ``` @@ -51,10 +51,12 @@ This command instructs the guidance node to start following a path toward the wa # Switching LOS Method -The active LOS guidance method can be changed during runtime. +The active LOS guidance method can be configured in advance through the package config file, and it can also be changed during runtime. + +At runtime, the LOS guidance method can be switched with: ``` -ros2 service call /orca/set_los_mode \ +ros2 service call /drone_name/set_los_mode \ vortex_msgs/srv/SetLosMode "{mode: X}" ``` @@ -70,7 +72,7 @@ Where Example: ``` -ros2 service call /orca/set_los_mode vortex_msgs/srv/SetLosMode "{mode: 2}" +ros2 service call /drone_name/set_los_mode vortex_msgs/srv/SetLosMode "{mode: 2}" ``` This switches the guidance system to **Adaptive LOS**. @@ -218,10 +220,10 @@ Proportional LOS is the simplest LOS guidance law. \tan^{-1}\left(\frac{z_e^p}{\Delta_v}\right) ``` -### Parameters +where -- $\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**. @@ -267,12 +269,12 @@ k_{i,v}\int z_e^p dt \tan^{-1}(u_v) ``` -### Parameters +where -- $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. @@ -300,10 +302,10 @@ Vector Field LOS generates a **bounded approach angle** toward the path. \tan^{-1}(k_p y_e^p) ``` -### Parameters +where -- $\psi_{max}$ — maximum allowed approach angle -- $k_p$ — proportional gain controlling path convergence +- $\psi_{max}$ maximum allowed approach angle +- $k_p$ proportional gain controlling path convergence The bounded approach angle prevents excessively aggressive heading changes. @@ -342,10 +344,10 @@ ros2 launch los_guidance guidance_test.launch.py test_scenario:=circle | Interface | Name | Type | Message-Type | |----------|------|------|---------| -| Action Server | `/orca/los_guidance` | Goal input | `vortex_msgs/action/LOSGuidance` | -| 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` | +| Action Server | `/drone_name/los_guidance` | Goal input | `vortex_msgs/action/LOSGuidance` | +| Subscriber | `/drone_name/pose` | Vehicle pose | `geometry_msgs/PoseWithCovarianceStamped` | +| Subscriber | `/drone_name/odom` | Vehicle velocity | `nav_msgs/Odometry` | +| Publisher | `/drone_name/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` | diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 4921d8d91..16c66b470 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -1,36 +1,54 @@ -# Adaptive LOS Parameters +#/** +# * @file guidance_params.yaml +# * @brief Configuration parameters for the LOS guidance node. +# * +# * This file contains the tunable parameters for all supported Line-Of-Sight +# * (LOS) guidance methods, together with common guidance settings shared across +# * the node. +# * +# * Parameter descriptions: +# * - `lookahead_distance_h`: Horizontal lookahead distance used to define the +# * LOS reference point in the horizontal plane. +# * - `lookahead_distance_v`: Vertical lookahead distance used to define the +# * LOS reference point in the vertical plane. +# * - `gamma_h`: Adaptive update gain for horizontal LOS adaptation. +# * - `gamma_v`: Adaptive update gain for vertical LOS adaptation. +# * - `k_p_h`: Proportional gain in the horizontal plane. +# * - `k_p_v`: Proportional gain in the vertical plane. +# * - `k_i_h`: Integral gain in the horizontal plane. +# * - `k_i_v`: Integral gain in the vertical plane. +# * - `max_approach_angle_h`: Maximum allowed horizontal approach angle for +# * vector field LOS guidance. +# * - `max_approach_angle_v`: Maximum allowed vertical approach angle for +# * vector field LOS guidance. +# * - +# */ adaptive_los: - lookahead_distance_h: 0.6 - lookahead_distance_v: 1.0 - gamma_h: 0.03 - gamma_v: 0.02 + lookahead_distance_h: 2.0 + lookahead_distance_v: 1.4 + adaptation_gain_h: 0.005 + adaptation_gain_v: 0.005 -# Proportional LOS Parameters prop_los: lookahead_distance_h: 0.74 lookahead_distance_v: 0.8 -# Integral LOS Parameters integer_los: - k_p_h: 0.5 - k_p_v: 0.5 - k_i_h: 0.1 - k_i_v: 0.1 + proportional_gain_h: 0.5 + proportional_gain_v: 0.5 + integral_gain_h: 0.1 + integral_gain_v: 0.1 -# Vector Field LOS Parameters vector_field_los: max_approach_angle_h: 1.0 max_approach_angle_v: 1.0 - k_p_h: 1.5 - k_p_v: 0.9 + proportional_gain_h: 1.5 + proportional_gain_v: 0.9 -# Common Guidance Parameters common: - active_los_method: 2 # 0: Proportional LOS, 1: Integral LOS, 2: Adaptive LOS, 3: Vector Field LOS + active_los_method: 2 u_desired: 0.3 goal_reached_tol: 0.20 - max_pitch_angle: 0.96 # 55 degrees - slow_approach: false - slow_down_distance: 1.5 - u_slow_min: 0.1 - surge_rate_limit: 0.1 + max_pitch_angle: 0.96 + missed_goal_distance_margin: 1.0 + missed_goal_timeout: 10.0 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 1f9784178..68cffec71 100644 --- a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp @@ -1,3 +1,9 @@ +/** + * @file adaptive_los.hpp + * @brief Defines the AdaptiveLOSGuidance class and its parameter structure for + * the adaptive LOS guidance algorithm. + */ + #ifndef LOS_GUIDANCE__LIB__ADAPTIVE_LOS_HPP_ #define LOS_GUIDANCE__LIB__ADAPTIVE_LOS_HPP_ @@ -9,47 +15,112 @@ namespace vortex::guidance::los { -// Parameter Structure +/** + * @brief Parameters for the Adaptive LOS guidance algorithm. + * + * This structure stores the tuning parameters used by the adaptive LOS + * controller for horizontal and vertical path-following. + */ struct AdaptiveLosParams { double lookahead_distance_h{}; double lookahead_distance_v{}; - double gamma_h{}; - double gamma_v{}; - double time_step{}; + double adaptation_gain_h{}; + double adaptation_gain_v{}; + double time_step{}; // in milliseconds }; -// Adaptive LOS Guidance Class +/** + * @brief Implements the adaptive LOS guidance algorithm. + * + * This class computes desired heading and pitch commands based on the current + * vehicle position and the active path segment. + */ class AdaptiveLOSGuidance { public: - // Constructor / Destructor + /** + * @brief Constructs an AdaptiveLOSGuidance object. + * @param params Parameters for the adaptive LOS guidance algorithm. + */ explicit AdaptiveLOSGuidance(const AdaptiveLosParams& params); + + /** + * @brief Destroys the AdaptiveLOSGuidance object. + */ ~AdaptiveLOSGuidance() = default; - // Main Output Calculation + /** + * @brief Calculates the desired LOS guidance outputs. + * @param inputs Input values required for adaptive LOS computation. + * @return types::Outputs The desired heading and pitch commands. + */ types::Outputs calculate_outputs(const types::Inputs& inputs); + /** + * @brief resets the adaptive values when new-segment starts + */ + void reset(); + private: - // Internal Update Functions + /** + * @brief Updates the path heading and path pitch angles from the active + * path segment. + * @param inputs Input values containing the previous and next path points. + */ void update_angles(const types::Inputs& inputs); + + /** + * @brief Calculates the cross-track error in the path-fixed reference + * frame. + * @param inputs Input values containing the current vehicle position and + * path information. + * @return types::CrossTrackError The calculated horizontal and vertical + * cross-track errors. + */ const types::CrossTrackError calculate_crosstrack_error( const types::Inputs& inputs); + + /** + * @brief Updates the adaptive estimates based on the current cross-track + * error. + * @param cross_track_error The current cross-track error in the path frame. + */ void update_adaptive_estimates( const types::CrossTrackError& cross_track_error); - // Parameters + /** + * @brief Parameters used by the adaptive LOS guidance algorithm. + */ AdaptiveLosParams params_{}; - // Rotation Matrices + /** + * @brief Rotation matrix for the path pitch rotation about the y-axis. + */ Eigen::Matrix3d rotation_y_ = Eigen::Matrix3d::Zero(); + + /** + * @brief Rotation matrix for the path heading rotation about the z-axis. + */ Eigen::Matrix3d rotation_z_ = Eigen::Matrix3d::Zero(); - // Path Angles - double pi_h_{}; - double pi_v_{}; + /** + * @brief Stores the horizontal path angle. + */ + double path_heading_{0.0}; + + /** + * @brief Stores the vertical path angle. + */ + double path_pitch_{0.0}; + + /** + * @brief Stores the horizontal adaptive estimate. + */ + double beta_c_hat_{0.0}; - // Adaptive Estimates - double beta_c_hat_ = 0.0; - double alpha_c_hat_ = 0.0; + /** + * @brief Stores the vertical adaptive estimate. + */ + double alpha_c_hat_{0.0}; }; } // 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 bde7d0d5f..2bf8d951c 100644 --- a/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp @@ -1,3 +1,8 @@ +/** + * @file integral_los.hpp + * @brief Defines the IntegralLOSGuidance class and parameter structure for the + * integral LOS guidance algorithm. + */ #ifndef LOS_GUIDANCE__LIB__INTEGRAL_LOS_HPP_ #define LOS_GUIDANCE__LIB__INTEGRAL_LOS_HPP_ @@ -9,44 +14,96 @@ namespace vortex::guidance::los { -// Parameter Structure +/** + * @brief Parameters for the Integral LOS guidance algorithm. + */ struct IntegralLosParams { - double k_p_h{}; - double k_p_v{}; - double k_i_h{}; - double k_i_v{}; - double time_step{}; + double proportional_gain_h{}; + double proportional_gain_v{}; + double integral_gain_h{}; + double integral_gain_v{}; + double time_step{}; // in milliseconds }; -// Integral LOS Guidance Class +/** + * @brief Implements the integral LOS guidance algorithm. + * + * This class computes desired heading and pitch commands using proportional and + * integral cross-track error feedback. + */ class IntegralLOSGuidance { public: - // Constructor / Destructor + /** + * @brief Constructs an IntegralLOSGuidance object. + * @param params Parameters for the integral LOS guidance algorithm. + */ explicit IntegralLOSGuidance(const IntegralLosParams& params); + + /** + * @brief Destroys the IntegralLOSGuidance object. + */ ~IntegralLOSGuidance() = default; - // Main Output Calculation + /** + * @brief Calculates the desired LOS guidance outputs. + * @param inputs Input values required for integral LOS computation. + * @return types::Outputs The desired heading and pitch commands. + */ types::Outputs calculate_outputs(const types::Inputs& inputs); private: - // Internal Update Functions + /** + * @brief Updates the path heading and path pitch angles from the active + * path segment. + * @param inputs Input values containing the previous and next path points. + */ void update_angles(const types::Inputs& inputs); + + /** + * @brief Calculates the cross-track error in the path-fixed reference + * frame. + * @param inputs Input values containing the current vehicle position and + * path information. + * @return types::CrossTrackError The calculated horizontal and vertical + * cross-track errors. + */ types::CrossTrackError calculate_crosstrack_error( const types::Inputs& inputs); - // Parameters + /** + * @brief Parameters used by the integral LOS guidance algorithm. + */ IntegralLosParams m_params{}; - // Integral States - double int_h{}; - double int_v{}; + /** + * @brief Stores the integral of the horizontal cross-track error. + */ + double integrated_horizontal_error_{}; + + /** + * @brief Stores the integral of the vertical cross-track error. + */ + double integrated_vertical_error_{}; - // Path Angles - double pi_h_{}; - double pi_v_{}; + /** + * @brief Stores the horizontal path angle. + */ + double path_heading_{}; - // Rotation Representation + /** + * @brief Stores the vertical path angle. + */ + double path_pitch_{}; + + /** + * @brief Rotation representation for the path pitch angle about the y-axis. + */ Eigen::AngleAxisd rotation_y_{0.0, Eigen::Vector3d::UnitY()}; + + /** + * @brief Rotation representation for the path heading angle about the + * z-axis. + */ 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 a2e9663e1..cf81bc774 100644 --- a/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp @@ -1,3 +1,8 @@ +/** + * @file proportional_los.hpp + * @brief Defines the ProportionalLOSGuidance class and parameter structure for + * the proportional LOS guidance algorithm. + */ #ifndef LOS_GUIDANCE__LIB__PROPORTIONAL_LOS_HPP_ #define LOS_GUIDANCE__LIB__PROPORTIONAL_LOS_HPP_ @@ -9,37 +14,83 @@ namespace vortex::guidance::los { -// Parameter Structure +/** + * @brief Parameters for the proportional LOS guidance algorithm. + */ struct ProportionalLosParams { double lookahead_distance_h{}; double lookahead_distance_v{}; }; -// Proportional LOS Guidance Class +/** + * @brief Implements the proportional LOS guidance algorithm. + * + * This class computes desired heading and pitch commands using proportional + * cross-track error feedback. + */ class ProportionalLOSGuidance { public: - // Constructor / Destructor + /** + * @brief Constructs a ProportionalLOSGuidance object. + * @param params Parameters for the proportional LOS guidance algorithm. + */ explicit ProportionalLOSGuidance(const ProportionalLosParams& params); + + /** + * @brief Destroys the ProportionalLOSGuidance object. + */ ~ProportionalLOSGuidance() = default; - // Main Output Calculation + /** + * @brief Calculates the desired LOS guidance outputs. + * @param inputs Input values required for proportional LOS computation. + * @return types::Outputs The desired heading and pitch commands. + */ types::Outputs calculate_outputs(const types::Inputs& inputs); private: - // Internal Update Functions + /** + * @brief Updates the path heading and path pitch angles from the active + * path segment. + * @param inputs Input values containing the previous and next path points. + */ void update_angles(const types::Inputs& inputs); + + /** + * @brief Calculates the cross-track error in the path-fixed reference + * frame. + * @param inputs Input values containing the current vehicle position and + * path information. + * @return types::CrossTrackError The calculated horizontal and vertical + * cross-track errors. + */ types::CrossTrackError calculate_crosstrack_error( const types::Inputs& inputs) const; - // Parameters + /** + * @brief Parameters used by the proportional LOS guidance algorithm. + */ ProportionalLosParams m_params{}; - // Path Angles - double pi_h_{0.0}; - double pi_v_{0.0}; + /** + * @brief Stores the horizontal path angle. + */ + double path_heading_{0.0}; - // Rotation Representation + /** + * @brief Stores the vertical path angle. + */ + double path_pitch_{0.0}; + + /** + * @brief Rotation representation for the path pitch angle about the y-axis. + */ Eigen::AngleAxisd rotation_y_{0.0, Eigen::Vector3d::UnitY()}; + + /** + * @brief Rotation representation for the path heading angle about the + * z-axis. + */ Eigen::AngleAxisd rotation_z_{0.0, Eigen::Vector3d::UnitZ()}; }; diff --git a/guidance/los_guidance/include/los_guidance/lib/types.hpp b/guidance/los_guidance/include/los_guidance/lib/types.hpp index 52aab3d48..76f8fa296 100644 --- a/guidance/los_guidance/include/los_guidance/lib/types.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/types.hpp @@ -1,3 +1,7 @@ +/** + * @file types.hpp + * @brief Defines shared data types used by the LOS guidance algorithms. + */ #ifndef LOS_GUIDANCE__LIB__TYPES_HPP_ #define LOS_GUIDANCE__LIB__TYPES_HPP_ @@ -9,50 +13,83 @@ namespace vortex::guidance::los::types { -// Point Representation +/** + * @brief Represents a 3D point. + * + * This struct is used to store waypoint positions and vehicle position data + * in Cartesian coordinates. + */ struct Point { double x{}; double y{}; double z{}; - // Point Operations + /** + * @brief Subtracts another point from this point. + * @param other The point to subtract. + * @return Point The resulting difference vector as a point. + */ Point operator-(const Point& other) const { return Point{x - other.x, y - other.y, z - other.z}; } - // Conversion Functions + /** + * @brief Converts the point to an Eigen 3D vector. + * @return Eigen::Vector3d The point represented as an Eigen vector. + */ Eigen::Vector3d as_vector() const { return Eigen::Vector3d(x, y, z); } + /** + * @brief Creates a Point from a ROS point message. + * @param msg ROS point message. + * @return Point The converted point. + */ static Point point_from_ros(const geometry_msgs::msg::Point& msg) { return Point{msg.x, msg.y, msg.z}; } }; -// Cross Track Error +/** + * @brief Represents cross-track error in the path-fixed reference frame. + * + * The values describe the position error relative to the active path segment + * in the transformed coordinate frame. + */ struct CrossTrackError { double x_e{}; double y_e{}; double z_e{}; + /** + * @brief Creates a CrossTrackError from an Eigen vector. + * @param vector Eigen vector containing cross-track error components. + * @return CrossTrackError The converted cross-track error. + */ inline static CrossTrackError from_vector(const Eigen::Vector3d& vector) { return CrossTrackError{vector.x(), vector.y(), vector.z()}; } }; -// Guidance Outputs +/** + * @brief Stores the LOS guidance outputs. + */ struct Outputs { double psi_d{}; double theta_d{}; }; -// Guidance Inputs +/** + * @brief Stores the inputs required by the LOS guidance algorithms. + */ struct Inputs { Point prev_point{}; Point next_point{}; Point current_position{}; }; -// Active LOS Method +/** + * @brief Enumerates the available LOS guidance methods. + */ enum class ActiveLosMethod { PROPORTIONAL, INTEGRAL, ADAPTIVE, VECTOR_FIELD }; } // 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 60d332929..6f4ec133e 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,3 +1,8 @@ +/** + * @file vector_field_los.hpp + * @brief Defines the VectorFieldLOSGuidance class and parameter structure for + * the vector field LOS guidance algorithm. + */ #ifndef LOS_GUIDANCE__LIB__VECTOR_FIELD_LOS_HPP_ #define LOS_GUIDANCE__LIB__VECTOR_FIELD_LOS_HPP_ @@ -9,40 +14,87 @@ namespace vortex::guidance::los { -// Parameter Structure +/** + * @brief Parameters for the vector field LOS guidance algorithm. + */ struct VectorFieldLosParams { double max_approach_angle_h{}; double max_approach_angle_v{}; - double k_p_h{}; - double k_p_v{}; - double time_step{}; + double proportional_gain_h{}; + double proportional_gain_v{}; + double time_step{}; // in milliseconds }; -// Vector Field LOS Guidance Class +/** + * @brief Implements the vector field LOS guidance algorithm. + * + * This class computes desired heading and pitch commands using vector field + * path-following logic based on the current vehicle position and active path + * segment. + */ class VectorFieldLOSGuidance { public: - // Constructor / Destructor + /** + * @brief Constructs a VectorFieldLOSGuidance object. + * @param params Parameters for the vector field LOS guidance algorithm. + */ explicit VectorFieldLOSGuidance(const VectorFieldLosParams& params); + + /** + * @brief Destroys the VectorFieldLOSGuidance object. + */ ~VectorFieldLOSGuidance() = default; - // Main Output Calculation + /** + * @brief Calculates the desired LOS guidance outputs. + * @param inputs Input values required for vector field LOS computation. + * @return types::Outputs The desired heading and pitch commands. + */ types::Outputs calculate_outputs(const types::Inputs& inputs); private: - // Internal Update Functions + /** + * @brief Updates the path heading and path pitch angles from the active + * path segment. + * @param inputs Input values containing the previous and next path points. + */ void update_angles(const types::Inputs& inputs); + + /** + * @brief Calculates the cross-track error in the path-fixed reference + * frame. + * @param inputs Input values containing the current vehicle position and + * path information. + * @return types::CrossTrackError The calculated horizontal and vertical + * cross-track errors. + */ types::CrossTrackError calculate_crosstrack_error( const types::Inputs& inputs) const; - // Parameters + /** + * @brief Parameters used by the vector field LOS guidance algorithm. + */ VectorFieldLosParams m_params{}; - // Path Angles - double pi_h_{0.0}; - double pi_v_{0.0}; + /** + * @brief Stores the horizontal path angle. + */ + double path_heading_{0.0}; - // Rotation Representation + /** + * @brief Stores the vertical path angle. + */ + double path_pitch_{0.0}; + + /** + * @brief Rotation representation for the path pitch angle about the y-axis. + */ Eigen::AngleAxisd rotation_y_{0.0, Eigen::Vector3d::UnitY()}; + + /** + * @brief Rotation representation for the path heading angle about the + * z-axis. + */ Eigen::AngleAxisd rotation_z_{0.0, Eigen::Vector3d::UnitZ()}; }; 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 55adfa079..698507d3e 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -1,3 +1,8 @@ +/** + * @file los_guidance_ros.hpp + * @brief The LosGuidanceNode class initializes ROS interfaces, loads + * configuration parameters, and runs the LOS guidance node. + */ #ifndef LOS_GUIDANCE__LOS_GUIDANCE_ROS_HPP_ #define LOS_GUIDANCE__LOS_GUIDANCE_ROS_HPP_ @@ -15,6 +20,7 @@ #include #include +#include #include #include @@ -26,62 +32,179 @@ namespace vortex::guidance::los { -// LOS Guidance ROS Node +/** + * @brief The LosGuidanceNode class initializes ROS interfaces, loads LOS + * guidance parameters, and manages path-following execution. + */ class LosGuidanceNode : public rclcpp::Node { public: - // Constructor + /** + * @brief Constructs a LosGuidanceNode object. + * @param options ROS node options used when creating the node. + */ explicit LosGuidanceNode(const rclcpp::NodeOptions& options); private: - // Setup Functions + /** + * @brief Sets up the ROS subscribers and publishers used by the node. + */ void set_subscribers_and_publisher(); + + /** + * @brief Sets up the LOS guidance action server. + */ void set_action_server(); + + /** + * @brief Sets up the service server used for changing LOS guidance mode. + */ void set_service_server(); - // Configuration Functions + /** + * @brief Initializes the adaptive LOS guidance module from configuration. + * @param config YAML configuration node containing adaptive LOS parameters. + */ void set_adaptive_los_guidance(YAML::Node config); + + /** + * @brief Initializes the proportional LOS guidance module from + * configuration. + * @param config YAML configuration node containing proportional LOS + * parameters. + */ void set_proportional_los_guidance(YAML::Node config); + + /** + * @brief Initializes the integral LOS guidance module from configuration. + * @param config YAML configuration node containing integral LOS parameters. + */ void set_integral_los_guidance(YAML::Node config); + + /** + * @brief Initializes the vector field LOS guidance module from + * configuration. + * @param config YAML configuration node containing vector field LOS + * parameters. + */ void set_vector_field_guidance(YAML::Node config); + + /** + * @brief Loads the LOS guidance YAML configuration file. + * @param yaml_file_path Path to the YAML configuration file. + * @return YAML::Node Parsed YAML configuration. + */ YAML::Node get_los_config(std::string yaml_file_path); + + /** + * @brief Parses common guidance parameters shared by all LOS methods. + * @param common_config YAML node containing common guidance parameters. + */ void parse_common_config(YAML::Node common_config); - // Callback Functions + /** + * @brief Callback for receiving waypoint updates. + * @param msg Received waypoint message. + */ void waypoint_callback( const geometry_msgs::msg::PointStamped::SharedPtr msg); + + /** + * @brief Callback for receiving pose updates. + * @param msg Received pose message. + */ void pose_callback( const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); + + /** + * @brief Callback for receiving odometry updates. + * @param msg Received odometry message. + */ void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg); - // Action Server Functions + /** + * @brief Handles an incoming LOS guidance action goal request. + * @param uuid Unique identifier for the received goal. + * @param goal Requested LOS guidance goal. + * @return rclcpp_action::GoalResponse Response indicating whether the goal + * is accepted. + */ rclcpp_action::GoalResponse handle_goal( const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal); + + /** + * @brief Handles cancellation of an active LOS guidance goal. + * @param goal_handle Handle to the goal being cancelled. + * @return rclcpp_action::CancelResponse Response indicating whether + * cancellation is accepted. + */ rclcpp_action::CancelResponse handle_cancel( const std::shared_ptr< rclcpp_action::ServerGoalHandle> goal_handle); + + /** + * @brief Handles an accepted LOS guidance goal. + * @param goal_handle Handle to the accepted goal. + */ void handle_accepted(const std::shared_ptr> goal_handle); + + /** + * @brief Executes the LOS guidance action. + * @param goal_handle Handle to the active LOS guidance goal. + */ void execute(const std::shared_ptr> goal_handle); - // Service Functions + /** + * @brief Service callback for changing the active LOS guidance method. + * @param request Service request containing the desired LOS mode. + * @param response Service response indicating whether the mode change + * succeeded. + */ void set_los_mode( const std::shared_ptr request, std::shared_ptr response); - // Publish Functions + /** + * @brief Publishes debug information about the current vehicle state. + * @param current_pose Current vehicle pose used for debug publishing. + */ void publish_state_debug( const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr current_pose); - vortex_msgs::msg::LOSGuidance fill_los_reference(types::Outputs output, - types::Inputs inputs); - // State Flags + /** + * @brief Fills a LOS guidance reference message from computed outputs and + * inputs. + * @param output Calculated LOS guidance outputs. + * @param inputs Current LOS guidance inputs. + * @return vortex_msgs::msg::LOSGuidance Populated LOS guidance reference + * message. + */ + vortex_msgs::msg::LOSGuidance fill_los_reference(types::Outputs output); + + /** + * @brief Checks if the given LOS guidance goal is feasible based on the + * provided inputs. + * @param inputs Current LOS guidance inputs. + * @return true if the goal is feasible, false otherwise. + */ + bool is_goal_feasible( + const types::Inputs& inputs, + std::shared_ptr goal); + + /** + * @brief Checks if the LOS guidance goal has been missed based on the + * provided inputs. + * @param inputs Current LOS guidance inputs. + * @return true if the goal is missed, false otherwise. + */ + bool is_goal_missed(const types::Inputs& inputs); + bool has_active_segment_{false}; - // ROS Interfaces rclcpp_action::Server::SharedPtr action_server_; rclcpp::Service::SharedPtr los_mode_service_; @@ -97,36 +220,30 @@ class LosGuidanceNode : public rclcpp::Node { rclcpp::TimerBase::SharedPtr reference_pub_timer_; rclcpp::CallbackGroup::SharedPtr cb_group_; - // Timing and Synchronization std::chrono::milliseconds time_step_; std::mutex mutex_; - // Action State rclcpp_action::GoalUUID preempted_goal_id_; std::shared_ptr< rclcpp_action::ServerGoalHandle> goal_handle_; - // Guidance State types::Inputs path_inputs_{}; double u_desired_{}; double goal_reached_tol_{}; double max_pitch_angle_{}; - bool slow_approach_{}; - double slow_down_distance_{}; - bool surge_initialized_{false}; - double u_slow_min_{}; - double commanded_surge_{}; - double surge_rate_limit_{}; types::ActiveLosMethod method_{}; - // Guidance Modules + double nearest_been_to_goal_{std::numeric_limits::max()}; + double time_since_nearest_goal_{}; + double missed_goal_distance_margin_{}; + double missed_goal_timeout_{}; + std::unique_ptr adaptive_los_{}; std::unique_ptr integral_los_{}; std::unique_ptr proportional_los_{}; std::unique_ptr vector_field_los_{}; - // Debug Data nav_msgs::msg::Odometry::SharedPtr debug_current_odom_{}; }; diff --git a/guidance/los_guidance/launch/guidance_test.launch.py b/guidance/los_guidance/launch/guidance_test.launch.py index a70c39f62..9ce95fb64 100644 --- a/guidance/los_guidance/launch/guidance_test.launch.py +++ b/guidance/los_guidance/launch/guidance_test.launch.py @@ -9,6 +9,7 @@ OpaqueFunction, TimerAction, ) +from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration @@ -21,27 +22,28 @@ def launch_setup(context, *args, **kwargs): drone, namespace = resolve_drone_and_namespace(context) test_scenario = LaunchConfiguration("test_scenario").perform(context) + use_keyboard_joy = LaunchConfiguration("use_keyboard_joy") stonefish_dir = get_package_share_directory("stonefish_sim") 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') + keyboard_joy_dir = get_package_share_directory("keyboard_joy") + velocity_controller_dir = get_package_share_directory("velocity_controller") + utility_dir = get_package_share_directory("vortex_utility_nodes") stonefish_sim = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(stonefish_dir, "launch", "simulation.launch.py") ), launch_arguments={ - "scenario": "default", + "drone": drone, + "scenario": "nautilus_no_gpu", "rendering": "true", }.items(), ) - - vortex_sim_interface = IncludeLaunchDescription( + + los_guidance_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join( - vortex_sim_interface_dir, "launch", "vortex_sim_interface.launch.py" - ) + os.path.join(los_guidance_dir, "launch", "los_guidance.launch.py") ), launch_arguments={ "drone": drone, @@ -49,23 +51,18 @@ def launch_setup(context, *args, **kwargs): }.items(), ) - operation_mode_launch = IncludeLaunchDescription( + keyboard_joy = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join( - operation_mode_manager_dir, - "launch", - "operation_mode_manager.launch.py", - ) + os.path.join(keyboard_joy_dir, "launch", "keyboard_joy_node.launch.py") ), - launch_arguments={ - "drone": drone, - "namespace": namespace, - }.items(), + condition=IfCondition(use_keyboard_joy), ) - los_guidance_launch = IncludeLaunchDescription( + velocity_controller_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(los_guidance_dir, "launch", "los_guidance.launch.py") + os.path.join( + velocity_controller_dir, "launch", "velocity_controller.launch.py" + ) ), launch_arguments={ "drone": drone, @@ -73,49 +70,19 @@ def launch_setup(context, *args, **kwargs): }.items(), ) - velocity_controller_launch = IncludeLaunchDescription( + drone_sim = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join( - velocity_controller_dir, 'launch', 'velocity_controller.launch.py' - ) + os.path.join(stonefish_dir, "launch", "drone_sim.launch.py") ), launch_arguments={ "drone": drone, - "namespace": namespace, }.items(), ) - orca_sim = TimerAction( - period=12.0, - actions=[ - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(stonefish_dir, "launch", "drone_sim.launch.py") - ) - ) - ], - ) - - set_autonomy = TimerAction( - period=12.0, - actions=[ - ExecuteProcess( - cmd=[ - "bash", - "-lc", - ( - f"ros2 service call /{namespace}/set_killswitch " - "vortex_msgs/srv/SetKillswitch " - "\"{killswitch_on: false}\" " - "&& " - f"ros2 service call /{namespace}/set_operation_mode " - "vortex_msgs/srv/SetOperationMode " - "\"{requested_operation_mode: {operation_mode: 1}}\"" - ), - ], - output="screen", - ), - ], + utility_node = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(utility_dir, "launch", "message_publisher.launch.py") + ) ) run_test_scenario = TimerAction( @@ -123,12 +90,13 @@ def launch_setup(context, *args, **kwargs): actions=[ ExecuteProcess( cmd=[ - "bash", - "-lc", - ( - f"python3 {os.path.join(los_guidance_dir, 'scripts', 'test_scenarios.py')} " - f"--ros-args -p drone:={drone} -p test_scenario:={test_scenario}" - ), + "python3", + os.path.join(los_guidance_dir, "scripts", "test_scenarios.py"), + "--ros-args", + "-p", + f"drone:={drone}", + "-p", + f"test_scenario:={test_scenario}", ], output="screen", ) @@ -137,24 +105,28 @@ def launch_setup(context, *args, **kwargs): return [ stonefish_sim, - #vortex_sim_interface, - #operation_mode_launch, + keyboard_joy, los_guidance_launch, velocity_controller_launch, - orca_sim, - set_autonomy, + drone_sim, + utility_node, run_test_scenario, ] def generate_launch_description(): return LaunchDescription( - declare_drone_and_namespace_args(default_drone="orca") + declare_drone_and_namespace_args(default_drone="nautilus") + [ DeclareLaunchArgument( "test_scenario", - default_value="square", - description="Scenario to run: square, circle, test_pitch, opposite_point", + default_value="4_corner", + description="Scenario to run: 4_corner, circle, test_pitch, opposite_point", + ), + DeclareLaunchArgument( + "use_keyboard_joy", + default_value="true", + description="Launch keyboard joy node", ), OpaqueFunction(function=launch_setup), ] diff --git a/guidance/los_guidance/scripts/test_scenarios.py b/guidance/los_guidance/scripts/test_scenarios.py index 95edef2d6..9b8a82a8c 100644 --- a/guidance/los_guidance/scripts/test_scenarios.py +++ b/guidance/los_guidance/scripts/test_scenarios.py @@ -11,8 +11,8 @@ class WaypointTest(Node): def __init__(self): super().__init__("waypoint_test_client") - self.declare_parameter("test_scenario", "square") - self.declare_parameter("drone", "orca") + self.declare_parameter("test_scenario", "4_corner") + self.declare_parameter("drone", "nautilus") self.test_scenario = self.get_parameter("test_scenario").value self.drone = self.get_parameter("drone").value @@ -41,7 +41,7 @@ def __init__(self): self.send_next_goal() def generate_waypoints(self, test_scenario): - if test_scenario == "square": + if test_scenario == "4_corner": s = self.square_size d = self.depth return [ @@ -66,7 +66,7 @@ def generate_waypoints(self, test_scenario): elif test_scenario == "test_pitch": # 0 = water surface, do not go above - # 3 = seabed/ground, do not touch + # this test scenario has no seabed, so z can be however we need. # Keep all depths safely between these return [ (3.0, 0.0, 1.0), # slight up @@ -84,9 +84,9 @@ def generate_waypoints(self, test_scenario): else: self.get_logger().warn( - f"Unknown test_scenario '{test_scenario}', defaulting to square" + f"Unknown test_scenario '{test_scenario}', defaulting to 4_corner" ) - return self.generate_waypoints("square") + return self.generate_waypoints("4_corner") def send_next_goal(self): if self.current_index >= len(self.waypoints): diff --git a/guidance/los_guidance/src/lib/adaptive_los.cpp b/guidance/los_guidance/src/lib/adaptive_los.cpp index db6177b1c..c3ad64fc5 100644 --- a/guidance/los_guidance/src/lib/adaptive_los.cpp +++ b/guidance/los_guidance/src/lib/adaptive_los.cpp @@ -4,24 +4,33 @@ namespace vortex::guidance::los { -// Constructor AdaptiveLOSGuidance::AdaptiveLOSGuidance(const AdaptiveLosParams& params) - : params_{params} {} + : params_{params} { + if (params.lookahead_distance_h <= 0.0 || + params.lookahead_distance_v <= 0.0 || params.adaptation_gain_h <= 0.0 || + params.adaptation_gain_v <= 0.0 || params.time_step <= 0.0) { + throw std::invalid_argument( + "AdaptiveLOSGuidance: all params must be > 0"); + } +} + +void AdaptiveLOSGuidance::reset() { + beta_c_hat_ = 0.0; + alpha_c_hat_ = 0.0; +} -// Angle Update void AdaptiveLOSGuidance::update_angles(const types::Inputs& inputs) { const double dx = inputs.next_point.x - inputs.prev_point.x; const double dy = inputs.next_point.y - inputs.prev_point.y; const double dz = inputs.next_point.z - inputs.prev_point.z; - pi_h_ = std::atan2(dy, dx); - pi_v_ = std::atan2(-dz, std::sqrt(dx * dx + dy * dy)); + path_heading_ = std::atan2(dy, dx); + path_pitch_ = 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(path_pitch_, Eigen::Vector3d::UnitY()); + rotation_z_ = Eigen::AngleAxisd(path_heading_, 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; @@ -33,7 +42,6 @@ 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 = @@ -43,10 +51,10 @@ 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 * + const double beta_dot = params_.adaptation_gain_h * (params_.lookahead_distance_h / denom_h) * cross_track_error.y_e; - const double alpha_dot = params_.gamma_v * + const double alpha_dot = params_.adaptation_gain_v * (params_.lookahead_distance_v / denom_v) * cross_track_error.z_e; @@ -54,7 +62,6 @@ void AdaptiveLOSGuidance::update_adaptive_estimates( alpha_c_hat_ += alpha_dot * params_.time_step; } -// Output Calculation types::Outputs AdaptiveLOSGuidance::calculate_outputs( const types::Inputs& inputs) { update_angles(inputs); @@ -64,15 +71,15 @@ types::Outputs AdaptiveLOSGuidance::calculate_outputs( update_adaptive_estimates(cross_track_error); - const double psi_d = - pi_h_ - beta_c_hat_ - + const double desired_yaw = + path_heading_ - beta_c_hat_ - std::atan(cross_track_error.y_e / params_.lookahead_distance_h); - const double theta_d = - pi_v_ + alpha_c_hat_ + + const double desired_pitch = + path_pitch_ + alpha_c_hat_ + std::atan(cross_track_error.z_e / params_.lookahead_distance_v); - return types::Outputs{psi_d, theta_d}; + return types::Outputs{desired_yaw, desired_pitch}; } } // 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 c8f0b4d59..9add664c5 100644 --- a/guidance/los_guidance/src/lib/integral_los.cpp +++ b/guidance/los_guidance/src/lib/integral_los.cpp @@ -4,18 +4,26 @@ namespace vortex::guidance::los { // Constructor IntegralLOSGuidance::IntegralLOSGuidance(const IntegralLosParams& params) - : m_params{params} {} + : m_params(params) { + if (params.proportional_gain_h <= 0.0 || + params.proportional_gain_v <= 0.0 || params.integral_gain_h <= 0.0 || + params.integral_gain_v <= 0.0 || params.time_step <= 0.0) { + throw std::invalid_argument( + "IntegralLOSGuidance: all params must be > 0"); + } +} // Angle Update void IntegralLOSGuidance::update_angles(const types::Inputs& inputs) { const types::Point difference = inputs.next_point - inputs.prev_point; - pi_h_ = std::atan2(difference.y, difference.x); - pi_v_ = std::atan2(-difference.z, std::sqrt(difference.x * difference.x + - difference.y * difference.y)); + path_heading_ = std::atan2(difference.y, difference.x); + path_pitch_ = 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(path_pitch_, Eigen::Vector3d::UnitY()); + rotation_z_ = Eigen::AngleAxisd(path_heading_, Eigen::Vector3d::UnitZ()); } // Cross-Track Error Calculation @@ -39,18 +47,18 @@ types::Outputs IntegralLOSGuidance::calculate_outputs( 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; + integrated_horizontal_error_ += cross_track_error.y_e * m_params.time_step; + integrated_vertical_error_ += 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.proportional_gain_h * cross_track_error.y_e + + m_params.integral_gain_h * integrated_horizontal_error_; + const double u_v = m_params.proportional_gain_v * cross_track_error.z_e + + m_params.integral_gain_v * integrated_vertical_error_; - const double psi_d = pi_h_ - std::atan(u_h); - const double theta_d = pi_v_ + std::atan(u_v); + const double desired_yaw = path_heading_ - std::atan(u_h); + const double desired_pitch = path_pitch_ + std::atan(u_v); - return types::Outputs{psi_d, theta_d}; + return types::Outputs{desired_yaw, desired_pitch}; } } // 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 f76df385c..4cb0a2386 100644 --- a/guidance/los_guidance/src/lib/proportional_los.cpp +++ b/guidance/los_guidance/src/lib/proportional_los.cpp @@ -5,18 +5,25 @@ namespace vortex::guidance::los { // Constructor ProportionalLOSGuidance::ProportionalLOSGuidance( const ProportionalLosParams& params) - : m_params{params} {} + : m_params(params) { + if (params.lookahead_distance_h <= 0.0 || + params.lookahead_distance_v <= 0.0) { + throw std::invalid_argument( + "ProportionalLOSGuidance: all params must be > 0"); + } +} // Angle Update void ProportionalLOSGuidance::update_angles(const types::Inputs& inputs) { const types::Point difference = inputs.next_point - inputs.prev_point; - pi_h_ = std::atan2(difference.y, difference.x); - pi_v_ = std::atan2(-difference.z, std::sqrt(difference.x * difference.x + - difference.y * difference.y)); + path_heading_ = std::atan2(difference.y, difference.x); + path_pitch_ = 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(path_pitch_, Eigen::Vector3d::UnitY()); + rotation_z_ = Eigen::AngleAxisd(path_heading_, Eigen::Vector3d::UnitZ()); } // Cross-Track Error Calculation @@ -43,10 +50,12 @@ types::Outputs ProportionalLOSGuidance::calculate_outputs( const double k_p_h = 1.0 / m_params.lookahead_distance_h; const double k_p_v = 1.0 / m_params.lookahead_distance_v; - const double psi_d = pi_h_ - std::atan(k_p_h * cross_track_error.y_e); - const double theta_d = pi_v_ + std::atan(k_p_v * cross_track_error.z_e); + const double desired_yaw = + path_heading_ - std::atan(k_p_h * cross_track_error.y_e); + const double desired_pitch = + path_pitch_ + std::atan(k_p_v * cross_track_error.z_e); - return types::Outputs{psi_d, theta_d}; + return types::Outputs{desired_yaw, desired_pitch}; } } // 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 edf352a6e..ca1e9d267 100644 --- a/guidance/los_guidance/src/lib/vector_field_los.cpp +++ b/guidance/los_guidance/src/lib/vector_field_los.cpp @@ -5,18 +5,27 @@ namespace vortex::guidance::los { // Constructor VectorFieldLOSGuidance::VectorFieldLOSGuidance( const VectorFieldLosParams& params) - : m_params{params} {} + : m_params(params) { + if (params.max_approach_angle_h <= 0.0 || + params.max_approach_angle_v <= 0.0 || + params.proportional_gain_h <= 0.0 || + params.proportional_gain_v <= 0.0 || params.time_step <= 0.0) { + throw std::invalid_argument( + "VectorFieldLOSGuidance: all params must be > 0"); + } +} // Angle Update void VectorFieldLOSGuidance::update_angles(const types::Inputs& inputs) { const types::Point difference = inputs.next_point - inputs.prev_point; - pi_h_ = std::atan2(difference.y, difference.x); - pi_v_ = std::atan2(-difference.z, std::sqrt(difference.x * difference.x + - difference.y * difference.y)); + path_heading_ = std::atan2(difference.y, difference.x); + path_pitch_ = 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(path_pitch_, Eigen::Vector3d::UnitY()); + rotation_z_ = Eigen::AngleAxisd(path_heading_, Eigen::Vector3d::UnitZ()); } // Cross-Track Error Calculation @@ -40,16 +49,18 @@ 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.proportional_gain_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.proportional_gain_v * cross_track_error.z_e); - const double psi_d = pi_h_ - approach_h; - const double theta_d = pi_v_ - approach_v; + const double desired_yaw = path_heading_ - approach_h; + const double desired_pitch = path_pitch_ - approach_v; - return types::Outputs{psi_d, theta_d}; + return types::Outputs{desired_yaw, desired_pitch}; } } // 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 2c90a868f..9b7007a66 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -1,19 +1,25 @@ #include "los_guidance/los_guidance_ros.hpp" #include -#include #include #include +#include #include #include "los_guidance/lib/types.hpp" -const auto start_message = R"( - _ ___ ____ ____ _ _ - | | / _ \/ ___| / ___|_ _(_) __| | __ _ _ __ ___ ___ - | | | | | \___ \ | | _| | | | |/ _` |/ _` | '_ \ / __/ _ \ - | |__| |_| |___) | | |_| | |_| | | (_| | (_| | | | | (_| __/ - |_____\___/|____/ \____|\__,_|_|\__,_|\__,_|_| |_|\___\___| +#ifdef NDEBUG +constexpr bool debug = false; +#else +constexpr bool debug = true; +#endif +const auto start_message = R"( +██╗ ██████╗ ███████╗ ██████╗ ██╗ ██╗██╗██████╗ █████╗ ███╗ ██╗ ██████╗███████╗ +██║ ██╔═══██╗██╔════╝ ██╔════╝ ██║ ██║██║██╔══██╗██╔══██╗████╗ ██║██╔════╝██╔════╝ +██║ ██║ ██║███████╗ ██║ ███╗██║ ██║██║██║ ██║███████║██╔██╗ ██║██║ █████╗ +██║ ██║ ██║╚════██║ ██║ ██║██║ ██║██║██║ ██║██╔══██║██║╚██╗██║██║ ██╔══╝ +███████╗╚██████╔╝███████║ ╚██████╔╝╚██████╔╝██║██████╔╝██║ ██║██║ ╚████║╚██████╗███████╗ +╚══════╝ ╚═════╝ ╚══════╝ ╚═════╝ ╚═════╝ ╚═╝╚═════╝ ╚═╝ ╚═╝╚═╝ ╚═══╝ ╚═════╝╚══════╝ )"; namespace vortex::guidance::los { @@ -22,8 +28,8 @@ namespace vortex::guidance::los { LosGuidanceNode::LosGuidanceNode(const rclcpp::NodeOptions& options) : Node("los_guidance_node", options) { double time_step_s = this->declare_parameter("time_step"); - time_step_ = - std::chrono::milliseconds(static_cast(time_step_s * 1000)); + time_step_ = std::chrono::milliseconds(static_cast( + time_step_s * 1000)); // Convert seconds to milliseconds const std::string yaml_path = this->declare_parameter("los_config_file"); @@ -42,7 +48,6 @@ LosGuidanceNode::LosGuidanceNode(const rclcpp::NodeOptions& options) spdlog::info(start_message); } -// ROS Setup void LosGuidanceNode::set_subscribers_and_publisher() { this->declare_parameter("topics.pose"); this->declare_parameter("topics.guidance.los"); @@ -61,9 +66,6 @@ void LosGuidanceNode::set_subscribers_and_publisher() { 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); @@ -114,66 +116,94 @@ 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; - - adaptive_los_ = std::make_unique(params); + try { + params.lookahead_distance_h = + adaptive_los_config["lookahead_distance_h"].as(); + params.lookahead_distance_v = + adaptive_los_config["lookahead_distance_v"].as(); + params.adaptation_gain_h = + adaptive_los_config["adaptation_gain_h"].as(); + params.adaptation_gain_v = + adaptive_los_config["adaptation_gain_v"].as(); + params.time_step = static_cast(time_step_.count()) / 1000.0; + + adaptive_los_ = std::make_unique(params); + } catch (const YAML::Exception& e) { + throw std::runtime_error( + std::string("Failed to load adaptive_los parameters: ") + e.what()); + } } void LosGuidanceNode::set_proportional_los_guidance(YAML::Node config) { auto proportional_los_config = config["prop_los"]; auto params = ProportionalLosParams{}; - params.lookahead_distance_h = - proportional_los_config["lookahead_distance_h"].as(); - params.lookahead_distance_v = - proportional_los_config["lookahead_distance_v"].as(); - - proportional_los_ = std::make_unique(params); + try { + params.lookahead_distance_h = + proportional_los_config["lookahead_distance_h"].as(); + params.lookahead_distance_v = + proportional_los_config["lookahead_distance_v"].as(); + + proportional_los_ = std::make_unique(params); + } catch (const YAML::Exception& e) { + throw std::runtime_error( + std::string("Failed to load proportional_los parameters: ") + + e.what()); + } } void LosGuidanceNode::set_integral_los_guidance(YAML::Node config) { auto integral_los_config = config["integer_los"]; auto params = IntegralLosParams{}; - params.k_p_h = integral_los_config["k_p_h"].as(); - params.k_p_v = integral_los_config["k_p_v"].as(); - params.k_i_h = integral_los_config["k_i_h"].as(); - params.k_i_v = integral_los_config["k_i_v"].as(); - params.time_step = static_cast(time_step_.count()) / 1000.0; - - integral_los_ = std::make_unique(params); + try { + params.proportional_gain_h = + integral_los_config["proportional_gain_h"].as(); + params.proportional_gain_v = + integral_los_config["proportional_gain_v"].as(); + params.integral_gain_h = + integral_los_config["integral_gain_h"].as(); + params.integral_gain_v = + integral_los_config["integral_gain_v"].as(); + params.time_step = static_cast(time_step_.count()) / 1000.0; + + integral_los_ = std::make_unique(params); + } catch (const YAML::Exception& e) { + throw std::runtime_error( + std::string("Failed to load integral_los parameters: ") + e.what()); + } } 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); + try { + 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.proportional_gain_h = + vector_field_config["proportional_gain_h"].as(); + params.proportional_gain_v = + vector_field_config["proportional_gain_v"].as(); + params.time_step = static_cast(time_step_.count()) / 1000.0; + + vector_field_los_ = std::make_unique(params); + } catch (const YAML::Exception& e) { + throw std::runtime_error( + std::string("Failed to load vector_field_los parameters: ") + + e.what()); + } } -// Topic Callbacks void LosGuidanceNode::waypoint_callback( const geometry_msgs::msg::PointStamped::SharedPtr wp_msg) { - std::lock_guard lock(mutex_); + std::unique_lock lock(mutex_); const auto new_wp = types::Point::point_from_ros(wp_msg->point); @@ -186,6 +216,8 @@ void LosGuidanceNode::waypoint_callback( path_inputs_.next_point = new_wp; } + lock.unlock(); + spdlog::info("Received waypoint: ({}, {}, {})", new_wp.x, new_wp.y, new_wp.z); } @@ -193,34 +225,48 @@ void LosGuidanceNode::waypoint_callback( void LosGuidanceNode::pose_callback( const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr current_pose) { - std::lock_guard lock(mutex_); + std::unique_lock lock(mutex_); path_inputs_.current_position = types::Point::point_from_ros(current_pose->pose.pose.position); + lock.unlock(); } void LosGuidanceNode::odom_callback( const nav_msgs::msg::Odometry::SharedPtr msg) { - std::lock_guard lock(mutex_); + std::unique_lock lock(mutex_); debug_current_odom_ = msg; + lock.unlock(); } -// Action Server Callbacks rclcpp_action::GoalResponse LosGuidanceNode::handle_goal( const rclcpp_action::GoalUUID&, std::shared_ptr goal) { - (void)goal; + types::Inputs inputs_copy; { - 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(); - } + std::unique_lock lock(mutex_); + inputs_copy = path_inputs_; + lock.unlock(); + } + + if (!is_goal_feasible(inputs_copy, goal)) { + RCLCPP_WARN(this->get_logger(), + "Rejected goal request: waypoint is not reachable with " + "current pitch limit"); + return rclcpp_action::GoalResponse::REJECT; + } + + { + std::unique_lock lock(mutex_); + if (goal_handle_ && goal_handle_->is_active()) { + RCLCPP_INFO(this->get_logger(), + "Aborting current goal and accepting new goal"); + preempted_goal_id_ = goal_handle_->get_goal_id(); } + lock.unlock(); } - spdlog::info("Accepted goal request"); + RCLCPP_INFO(this->get_logger(), "Accepted goal request"); return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } @@ -240,89 +286,109 @@ void LosGuidanceNode::handle_accepted( std::thread{[this, goal_handle]() { execute(goal_handle); }}.detach(); } -// Service Callback void LosGuidanceNode::set_los_mode( const std::shared_ptr request, std::shared_ptr response) { - method_ = static_cast(request->mode); - spdlog::info("LOS mode set to {}", static_cast(method_)); + { + std::unique_lock lock(mutex_); + method_ = static_cast(request->mode); + lock.unlock(); + } + + spdlog::info("LOS mode set to {}", static_cast(request->mode)); response->success = true; } -// Message Helpers vortex_msgs::msg::LOSGuidance LosGuidanceNode::fill_los_reference( - types::Outputs outputs, - types::Inputs inputs) { + types::Outputs outputs) { 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_; + return reference_msg; +} - if (slow_approach_) { - const double distance_to_goal = - (inputs.current_position - inputs.next_point).as_vector().norm(); +bool LosGuidanceNode::is_goal_feasible( + const types::Inputs& inputs, + std::shared_ptr goal) { + const auto& current_position = inputs.current_position; + const auto& goal_point = goal->goal.point; - double target_surge = u_desired_; + const double dx = goal_point.x - current_position.x; + const double dy = goal_point.y - current_position.y; + const double dz = goal_point.z - current_position.z; - if (distance_to_goal <= slow_down_distance_) { - const double alpha = - std::clamp(distance_to_goal / slow_down_distance_, 0.0, 1.0); - target_surge = u_slow_min_ + alpha * (u_desired_ - u_slow_min_); - } + const double horizontal_distance = std::sqrt(dx * dx + dy * dy); + const double required_pitch = std::atan2(-dz, horizontal_distance); - if (!surge_initialized_) { - commanded_surge_ = target_surge; - surge_initialized_ = true; - } else { - const double dt = static_cast(time_step_.count()) / 1000.0; - const double max_step = surge_rate_limit_ * dt; - const double delta = target_surge - commanded_surge_; - - if (delta > max_step) { - commanded_surge_ += max_step; - } else if (delta < -max_step) { - commanded_surge_ -= max_step; - } else { - commanded_surge_ = target_surge; - } - } + return std::abs(required_pitch) <= max_pitch_angle_; +} + +bool LosGuidanceNode::is_goal_missed(const types::Inputs& inputs) { + const double distance_to_goal = + (inputs.current_position - inputs.next_point).as_vector().norm(); + + const double dt = static_cast(time_step_.count()) / 1000.0; + + if (distance_to_goal < nearest_been_to_goal_) { + nearest_been_to_goal_ = distance_to_goal; + time_since_nearest_goal_ = 0.0; + return false; + } + + if (distance_to_goal > + nearest_been_to_goal_ + missed_goal_distance_margin_) { + time_since_nearest_goal_ += dt; } else { - commanded_surge_ = u_desired_; + time_since_nearest_goal_ = 0.0; } - reference_msg.surge = commanded_surge_; - return reference_msg; + return time_since_nearest_goal_ >= missed_goal_timeout_; } YAML::Node LosGuidanceNode::get_los_config(std::string yaml_file_path) { - YAML::Node config = YAML::LoadFile(yaml_file_path); - return config; + try { + YAML::Node config = YAML::LoadFile(yaml_file_path); + return config; + } catch (const YAML::Exception& e) { + throw std::runtime_error( + std::string("Failed to load LOS config file '") + yaml_file_path + + "': " + e.what()); + } } void LosGuidanceNode::parse_common_config(YAML::Node common_config) { - std::lock_guard lock(mutex_); - u_desired_ = common_config["u_desired"].as(); - max_pitch_angle_ = common_config["max_pitch_angle"].as(); - goal_reached_tol_ = common_config["goal_reached_tol"].as(); - slow_approach_ = common_config["slow_approach"].as(); - slow_down_distance_ = common_config["slow_down_distance"].as(); - u_slow_min_ = common_config["u_slow_min"].as(); - surge_rate_limit_ = common_config["surge_rate_limit"].as(); - - method_ = static_cast( - common_config["active_los_method"].as()); + try { + std::unique_lock 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(); + missed_goal_timeout_ = + common_config["missed_goal_timeout"].as(); + missed_goal_distance_margin_ = + common_config["missed_goal_distance_margin"].as(); + + method_ = static_cast( + common_config["active_los_method"].as()); + + lock.unlock(); + } catch (const YAML::Exception& e) { + throw std::runtime_error( + std::string("Failed to load common parameters: ") + e.what()); + } } -// Goal Execution void LosGuidanceNode::execute( const std::shared_ptr< rclcpp_action::ServerGoalHandle> goal_handle) { { - std::lock_guard lock(mutex_); + std::unique_lock lock(mutex_); this->goal_handle_ = goal_handle; + lock.unlock(); } spdlog::info("Executing goal"); @@ -332,27 +398,36 @@ void LosGuidanceNode::execute( 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; + { + std::unique_lock lock(mutex_); + 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; + } + lock.unlock(); } + adaptive_los_->reset(); + auto result = std::make_shared(); + nearest_been_to_goal_ = std::numeric_limits::infinity(); + time_since_nearest_goal_ = 0.0; rclcpp::Rate loop_rate(1000.0 / time_step_.count()); while (rclcpp::ok()) { { - std::lock_guard lock(mutex_); + std::unique_lock lock(mutex_); if (goal_handle->get_goal_id() == preempted_goal_id_) { result->success = false; goal_handle->abort(result); return; } + lock.unlock(); } if (goal_handle->is_canceling()) { @@ -363,14 +438,29 @@ void LosGuidanceNode::execute( } types::Inputs inputs_copy; + types::ActiveLosMethod method_copy; + nav_msgs::msg::Odometry::SharedPtr odom_copy; + double goal_reached_tol_copy; + { - std::lock_guard lock(mutex_); + std::unique_lock lock(mutex_); inputs_copy = path_inputs_; + method_copy = method_; + odom_copy = debug_current_odom_; + goal_reached_tol_copy = goal_reached_tol_; + lock.unlock(); + } + + if (is_goal_missed(inputs_copy)) { + result->success = false; + goal_handle->abort(result); + spdlog::info("Aborting goal: waypoint missed"); + return; } types::Outputs outputs; - switch (method_) { + switch (method_copy) { case types::ActiveLosMethod::ADAPTIVE: outputs = adaptive_los_->calculate_outputs(inputs_copy); break; @@ -390,22 +480,33 @@ void LosGuidanceNode::execute( return; } - vortex_msgs::msg::LOSGuidance reference_msg = - fill_los_reference(outputs, inputs_copy); + auto reference_msg = std::make_unique( + fill_los_reference(outputs)); - los_debug_pub_->publish(reference_msg); + if ((inputs_copy.current_position - inputs_copy.next_point) + .as_vector() + .norm() < goal_reached_tol_copy) { + reference_msg->pitch = 0.0; + reference_msg->surge = 0.0; - if (debug_current_odom_) { - const auto& v = debug_current_odom_->twist.twist.linear; + result->success = true; + goal_handle->succeed(result); + spdlog::info("Goal reached"); + return; + } + + reference_pub_->publish(std::move(reference_msg)); + + if (debug && odom_copy) { + const auto& v = odom_copy->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(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::Vector3d euler = vortex::utils::math::quat_to_euler( + Eigen::Quaterniond(odom_copy->pose.pose.orientation.w, + odom_copy->pose.pose.orientation.x, + odom_copy->pose.pose.orientation.y, + odom_copy->pose.pose.orientation.z)); state_debug_msg.pitch = euler.y(); state_debug_msg.yaw = euler.z(); @@ -414,23 +515,6 @@ void LosGuidanceNode::execute( state_debug_pub_->publish(state_debug_msg); } - reference_pub_->publish(reference_msg); - - if ((inputs_copy.current_position - inputs_copy.next_point) - .as_vector() - .norm() < goal_reached_tol_) { - auto stop_ref = reference_msg; - stop_ref.surge = 0.0; - stop_ref.pitch = 0.0; - stop_ref.yaw = reference_msg.yaw; - - reference_pub_->publish(stop_ref); - result->success = true; - goal_handle->succeed(result); - spdlog::info("Goal reached"); - return; - } - loop_rate.sleep(); } } diff --git a/guidance/los_guidance/test/CMakeLists.txt b/guidance/los_guidance/test/CMakeLists.txt index c0cdc1ae7..c06410f19 100644 --- a/guidance/los_guidance/test/CMakeLists.txt +++ b/guidance/los_guidance/test/CMakeLists.txt @@ -11,6 +11,7 @@ add_executable( proportional_los_test.cpp integral_los_test.cpp vector_field_los_test.cpp + los_invalid_params_test.cpp ) diff --git a/guidance/los_guidance/test/adaptive_los_test.cpp b/guidance/los_guidance/test/adaptive_los_test.cpp index dbf63eb04..afd707604 100644 --- a/guidance/los_guidance/test/adaptive_los_test.cpp +++ b/guidance/los_guidance/test/adaptive_los_test.cpp @@ -1,7 +1,7 @@ #include "los_guidance/lib/adaptive_los.hpp" #include -namespace vortex::guidance::los { // new namespace added los +namespace vortex::guidance::los { class AdaptiveLosTest : public ::testing::Test { protected: @@ -9,10 +9,10 @@ class AdaptiveLosTest : public ::testing::Test { 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.lookahead_distance_h = 0.9; + p.lookahead_distance_v = 1.4; + p.adaptation_gain_h = 0.03; + p.adaptation_gain_v = 0.02; p.time_step = 0.01; return p; } @@ -22,7 +22,7 @@ class AdaptiveLosTest : public ::testing::Test { }; // Test commanded angles when drone is to the right of the track -TEST_F(AdaptiveLosTest, T06_test_commanded_angles) { +TEST_F(AdaptiveLosTest, T01_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}; @@ -39,7 +39,7 @@ TEST_F(AdaptiveLosTest, T06_test_commanded_angles) { } // Test commanded angles when drone is to the left of the track -TEST_F(AdaptiveLosTest, T07_test_commanded_angles) { +TEST_F(AdaptiveLosTest, T02_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}; @@ -55,7 +55,7 @@ TEST_F(AdaptiveLosTest, T07_test_commanded_angles) { } // Test commanded angles when drone is under the track -TEST_F(AdaptiveLosTest, T08_test_commanded_angles) { +TEST_F(AdaptiveLosTest, T03_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}; @@ -70,8 +70,8 @@ TEST_F(AdaptiveLosTest, T08_test_commanded_angles) { EXPECT_LT(O.theta_d, 1.57); } -// Test commanded angles when drone is over the track -TEST_F(AdaptiveLosTest, T09_test_commanded_angles) { +// Test commanded angles when drone is above the track +TEST_F(AdaptiveLosTest, T04_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}; @@ -86,9 +86,9 @@ TEST_F(AdaptiveLosTest, T09_test_commanded_angles) { EXPECT_GT(O.theta_d, -1.57); } -// Test commanded angles when drone is over and to the right of the track +// Test commanded angles when drone is above and to the right of the track -TEST_F(AdaptiveLosTest, T10_test_commanded_angles) { +TEST_F(AdaptiveLosTest, T05_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}; diff --git a/guidance/los_guidance/test/integral_los_test.cpp b/guidance/los_guidance/test/integral_los_test.cpp index 1999abac8..7e06559f4 100644 --- a/guidance/los_guidance/test/integral_los_test.cpp +++ b/guidance/los_guidance/test/integral_los_test.cpp @@ -10,10 +10,10 @@ class IntegralLosTest : public ::testing::Test { IntegralLosParams get_params() { IntegralLosParams params; - params.k_i_h = 0.1; // needs tuning - params.k_i_v = 0.1; // needs tuning - params.k_p_h = 0.667; // needs tuning - params.k_p_v = 0.582; // needs tuning + params.integral_gain_h = 0.5; + params.integral_gain_v = 0.5; + params.proportional_gain_h = 0.1; + params.proportional_gain_v = 0.1; params.time_step = 0.01; return params; } @@ -23,7 +23,7 @@ class IntegralLosTest : public ::testing::Test { }; // Test commanded angles when drone is to the right of the track -TEST_F(IntegralLosTest, T06_test_commanded_angles) { +TEST_F(IntegralLosTest, T01_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}; @@ -40,7 +40,7 @@ TEST_F(IntegralLosTest, T06_test_commanded_angles) { } // Test commanded angles when drone is to the left of the track -TEST_F(IntegralLosTest, T07_test_commanded_angles) { +TEST_F(IntegralLosTest, T02_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}; @@ -56,7 +56,7 @@ TEST_F(IntegralLosTest, T07_test_commanded_angles) { } // Test commanded angles when drone is under the track -TEST_F(IntegralLosTest, T08_test_commanded_angles) { +TEST_F(IntegralLosTest, T03_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}; @@ -71,8 +71,8 @@ TEST_F(IntegralLosTest, T08_test_commanded_angles) { EXPECT_LT(O.theta_d, 1.57); } -// Test commanded angles when drone is over the track -TEST_F(IntegralLosTest, T09_test_commanded_angles) { +// Test commanded angles when drone is above the track +TEST_F(IntegralLosTest, T04_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}; @@ -87,9 +87,9 @@ TEST_F(IntegralLosTest, T09_test_commanded_angles) { EXPECT_GT(O.theta_d, -1.57); } -// Test commanded angles when drone is over and to the right of the track +// Test commanded angles when drone is above and to the right of the track -TEST_F(IntegralLosTest, T10_test_commanded_angles) { +TEST_F(IntegralLosTest, T05_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}; diff --git a/guidance/los_guidance/test/los_invalid_params_test.cpp b/guidance/los_guidance/test/los_invalid_params_test.cpp new file mode 100644 index 000000000..4473c1c66 --- /dev/null +++ b/guidance/los_guidance/test/los_invalid_params_test.cpp @@ -0,0 +1,56 @@ +#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/vector_field_los.hpp" + +namespace vortex::guidance::los { + +TEST(LosInvalidParamsTest, AdaptiveLosRejectsNegativeLookaheadDistance) { + AdaptiveLosParams params; + params.lookahead_distance_h = -0.9; + params.lookahead_distance_v = 1.4; + params.adaptation_gain_h = 0.03; + params.adaptation_gain_v = 0.02; + params.time_step = 0.01; + + EXPECT_THROW( + { AdaptiveLOSGuidance guidance(params); }, std::invalid_argument); +} + +TEST(LosInvalidParamsTest, ProportionalLosRejectsZeroLookaheadDistance) { + ProportionalLosParams params; + params.lookahead_distance_h = 0.0; + params.lookahead_distance_v = 0.8; + + EXPECT_THROW( + { ProportionalLOSGuidance guidance(params); }, std::invalid_argument); +} + +TEST(LosInvalidParamsTest, IntegralLosRejectsZeroTimeStep) { + IntegralLosParams params; + params.proportional_gain_h = 0.5; + params.proportional_gain_v = 0.5; + params.integral_gain_h = 0.1; + params.integral_gain_v = 0.1; + params.time_step = 0.0; + + EXPECT_THROW( + { IntegralLOSGuidance guidance(params); }, std::invalid_argument); +} + +TEST(LosInvalidParamsTest, VectorFieldLosRejectsNegativeApproachAngle) { + VectorFieldLosParams params; + params.max_approach_angle_h = -1.0; + params.max_approach_angle_v = 1.0; + params.proportional_gain_h = 1.5; + params.proportional_gain_v = 0.9; + params.time_step = 0.01; + + EXPECT_THROW( + { VectorFieldLOSGuidance guidance(params); }, std::invalid_argument); +} + +} // namespace vortex::guidance::los diff --git a/guidance/los_guidance/test/proportional_los_test.cpp b/guidance/los_guidance/test/proportional_los_test.cpp index b35a3ba6f..311d7d23a 100644 --- a/guidance/los_guidance/test/proportional_los_test.cpp +++ b/guidance/los_guidance/test/proportional_los_test.cpp @@ -9,8 +9,8 @@ class ProportionalLosTest : public ::testing::Test { ProportionalLosParams get_params() { ProportionalLosParams params; - params.lookahead_distance_h = 10.0; - params.lookahead_distance_v = 10.0; + params.lookahead_distance_h = 0.74; + params.lookahead_distance_v = 0.8; return params; } @@ -19,7 +19,7 @@ class ProportionalLosTest : public ::testing::Test { }; // Test commanded angles when drone is to the right of the track -TEST_F(ProportionalLosTest, T06_test_commanded_angles) { +TEST_F(ProportionalLosTest, T01_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}; @@ -36,7 +36,7 @@ TEST_F(ProportionalLosTest, T06_test_commanded_angles) { } // Test commanded angles when drone is to the left of the track -TEST_F(ProportionalLosTest, T07_test_commanded_angles) { +TEST_F(ProportionalLosTest, T02_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}; @@ -52,7 +52,7 @@ TEST_F(ProportionalLosTest, T07_test_commanded_angles) { } // Test commanded angles when drone is under the track -TEST_F(ProportionalLosTest, T08_test_commanded_angles) { +TEST_F(ProportionalLosTest, T03_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}; @@ -67,8 +67,8 @@ TEST_F(ProportionalLosTest, T08_test_commanded_angles) { EXPECT_LT(O.theta_d, 1.57); } -// Test commanded angles when drone is over the track -TEST_F(ProportionalLosTest, T09_test_commanded_angles) { +// Test commanded angles when drone is above the track +TEST_F(ProportionalLosTest, T04_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}; @@ -83,9 +83,9 @@ TEST_F(ProportionalLosTest, T09_test_commanded_angles) { EXPECT_GT(O.theta_d, -1.57); } -// Test commanded angles when drone is over and to the right of the track +// Test commanded angles when drone is above and to the right of the track -TEST_F(ProportionalLosTest, T10_test_commanded_angles) { +TEST_F(ProportionalLosTest, T05_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}; diff --git a/guidance/los_guidance/test/vector_field_los_test.cpp b/guidance/los_guidance/test/vector_field_los_test.cpp index e11f7d3dd..f5f121920 100644 --- a/guidance/los_guidance/test/vector_field_los_test.cpp +++ b/guidance/los_guidance/test/vector_field_los_test.cpp @@ -10,10 +10,10 @@ class VectorFieldLosTest : public ::testing::Test { 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 = 1.5; // needs tuning - params.k_p_v = 0.9; // needs tuning + params.max_approach_angle_h = 1.0; + params.max_approach_angle_v = 1.0; + params.proportional_gain_h = 1.5; + params.proportional_gain_v = 0.9; params.time_step = 0.01; return params; } @@ -23,7 +23,7 @@ class VectorFieldLosTest : public ::testing::Test { }; // Test commanded angles when drone is to the right of the track -TEST_F(VectorFieldLosTest, T06_test_commanded_angles) { +TEST_F(VectorFieldLosTest, T01_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}; @@ -40,7 +40,7 @@ TEST_F(VectorFieldLosTest, T06_test_commanded_angles) { } // Test commanded angles when drone is to the left of the track -TEST_F(VectorFieldLosTest, T07_test_commanded_angles) { +TEST_F(VectorFieldLosTest, T02_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}; @@ -57,7 +57,7 @@ TEST_F(VectorFieldLosTest, T07_test_commanded_angles) { } // Test commanded angles when drone is under the track -TEST_F(VectorFieldLosTest, T08_test_commanded_angles) { +TEST_F(VectorFieldLosTest, T03_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}; @@ -73,8 +73,8 @@ TEST_F(VectorFieldLosTest, T08_test_commanded_angles) { EXPECT_GT(O.theta_d, -1.57); } -// Test commanded angles when drone is over the track -TEST_F(VectorFieldLosTest, T09_test_commanded_angles) { +// Test commanded angles when drone is above the track +TEST_F(VectorFieldLosTest, T04_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}; @@ -90,8 +90,8 @@ TEST_F(VectorFieldLosTest, T09_test_commanded_angles) { EXPECT_LT(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) { +// Test commanded angles when drone is above and to the right of the track +TEST_F(VectorFieldLosTest, T05_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}; From 926c93c7b04f4641d7daf65b612866100a28a6ea Mon Sep 17 00:00:00 2001 From: Cyprian Osinski Date: Fri, 3 Apr 2026 18:53:15 +0200 Subject: [PATCH 276/290] quat and rpy fix --- auv_setup/config/robots/nautilus.yaml | 3 ++- .../src/dp_adapt_backs_controller_ros.cpp | 4 ++-- control/pid_controller_dp/src/pid_controller_ros.cpp | 4 ++-- .../pid_controller_dp_euler/src/pid_controller_ros.cpp | 4 ++-- .../src/ros/reference_filter_ros.cpp | 4 ++-- .../launch/reference_filter_dp_quat.launch.py | 6 +----- .../src/ros/reference_filter_ros.cpp | 4 ++-- .../joystick_interface_auv_node.py | 10 +++++++--- 8 files changed, 20 insertions(+), 19 deletions(-) diff --git a/auv_setup/config/robots/nautilus.yaml b/auv_setup/config/robots/nautilus.yaml index f38cd9649..316c8f7de 100644 --- a/auv_setup/config/robots/nautilus.yaml +++ b/auv_setup/config/robots/nautilus.yaml @@ -114,7 +114,8 @@ waypoint: "waypoint" guidance: los: "guidance/los" - dp: "guidance/dp" + dp_rpy: "guidance/dp_rpy" + dp_quat: "guidance/dp_quat" dvl_twist: "dvl/twist" dvl_altitude: "dvl/altitude" imu: "imu/data_raw" 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 cbc3e8514..1b9578932 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 @@ -40,9 +40,9 @@ void DPAdaptBacksControllerNode::set_subscribers_and_publisher() { vortex::utils::qos_profiles::sensor_data_profile(1)}; const auto qos_reliable{vortex::utils::qos_profiles::reliable_profile(1)}; - this->declare_parameter("topics.guidance.dp"); + this->declare_parameter("topics.guidance.dp_rpy"); std::string dp_reference_topic = - this->get_parameter("topics.guidance.dp").as_string(); + this->get_parameter("topics.guidance.dp_rpy").as_string(); guidance_sub_ = this->create_subscription( dp_reference_topic, qos_sensor_data, diff --git a/control/pid_controller_dp/src/pid_controller_ros.cpp b/control/pid_controller_dp/src/pid_controller_ros.cpp index 3c97ea9a5..128e8d104 100644 --- a/control/pid_controller_dp/src/pid_controller_ros.cpp +++ b/control/pid_controller_dp/src/pid_controller_ros.cpp @@ -41,9 +41,9 @@ void PIDControllerNode::set_subscribers_and_publisher() { rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); const auto qos_reliable{vortex::utils::qos_profiles::reliable_profile(1)}; - this->declare_parameter("topics.guidance.dp"); + this->declare_parameter("topics.guidance.dp_quat"); std::string dp_reference_topic = - this->get_parameter("topics.guidance.dp").as_string(); + this->get_parameter("topics.guidance.dp_quat").as_string(); this->declare_parameter("topics.odom"); std::string odom_topic = this->get_parameter("topics.odom").as_string(); diff --git a/control/pid_controller_dp_euler/src/pid_controller_ros.cpp b/control/pid_controller_dp_euler/src/pid_controller_ros.cpp index 2b456647f..93a727d11 100644 --- a/control/pid_controller_dp_euler/src/pid_controller_ros.cpp +++ b/control/pid_controller_dp_euler/src/pid_controller_ros.cpp @@ -20,9 +20,9 @@ PIDControllerNode::PIDControllerNode() : Node("pid_controller_euler_node") { } void PIDControllerNode::set_subscribers_and_publisher() { - this->declare_parameter("topics.guidance.dp"); + this->declare_parameter("topics.guidance.dp_rpy"); std::string dp_reference_topic = - this->get_parameter("topics.guidance.dp").as_string(); + this->get_parameter("topics.guidance.dp_rpy").as_string(); this->declare_parameter("topics.pose"); std::string pose_topic = this->get_parameter("topics.pose").as_string(); diff --git a/guidance/reference_filter_dp/src/ros/reference_filter_ros.cpp b/guidance/reference_filter_dp/src/ros/reference_filter_ros.cpp index 1393be495..db0cbd03b 100644 --- a/guidance/reference_filter_dp/src/ros/reference_filter_ros.cpp +++ b/guidance/reference_filter_dp/src/ros/reference_filter_ros.cpp @@ -42,13 +42,13 @@ ReferenceFilterNode::~ReferenceFilterNode() { void ReferenceFilterNode::set_subscribers_and_publisher() { this->declare_parameter("topics.pose"); this->declare_parameter("topics.twist"); - this->declare_parameter("topics.guidance.dp"); + this->declare_parameter("topics.guidance.dp_rpy"); this->declare_parameter("topics.reference_pose"); std::string pose_topic = this->get_parameter("topics.pose").as_string(); std::string twist_topic = this->get_parameter("topics.twist").as_string(); std::string guidance_topic = - this->get_parameter("topics.guidance.dp").as_string(); + this->get_parameter("topics.guidance.dp_rpy").as_string(); std::string reference_pose_topic = this->get_parameter("topics.reference_pose").as_string(); diff --git a/guidance/reference_filter_dp_quat/launch/reference_filter_dp_quat.launch.py b/guidance/reference_filter_dp_quat/launch/reference_filter_dp_quat.launch.py index a610a8ef3..6fdd052da 100644 --- a/guidance/reference_filter_dp_quat/launch/reference_filter_dp_quat.launch.py +++ b/guidance/reference_filter_dp_quat/launch/reference_filter_dp_quat.launch.py @@ -31,11 +31,7 @@ def launch_setup(context, *args, **kwargs): extra_params = {} if rpy_publish: - extra_params = { - "publish_rpy_debug": True, - "topics.guidance.dp_rpy": "guidance/dp", - "topics.guidance.dp": "guidance/dp_quat", - } + extra_params = {"publish_rpy_debug": True} return [ Node( diff --git a/guidance/reference_filter_dp_quat/src/ros/reference_filter_ros.cpp b/guidance/reference_filter_dp_quat/src/ros/reference_filter_ros.cpp index 1c4ab4158..a5207d45e 100644 --- a/guidance/reference_filter_dp_quat/src/ros/reference_filter_ros.cpp +++ b/guidance/reference_filter_dp_quat/src/ros/reference_filter_ros.cpp @@ -42,13 +42,13 @@ ReferenceFilterNode::~ReferenceFilterNode() { void ReferenceFilterNode::set_subscribers_and_publisher() { this->declare_parameter("topics.pose"); this->declare_parameter("topics.twist"); - this->declare_parameter("topics.guidance.dp"); + this->declare_parameter("topics.guidance.dp_quat"); this->declare_parameter("topics.reference_pose"); std::string pose_topic = this->get_parameter("topics.pose").as_string(); std::string twist_topic = this->get_parameter("topics.twist").as_string(); std::string guidance_topic = - this->get_parameter("topics.guidance.dp").as_string(); + this->get_parameter("topics.guidance.dp_quat").as_string(); std::string reference_pose_topic = this->get_parameter("topics.reference_pose").as_string(); diff --git a/mission/joystick_interface_auv/joystick_interface_auv/joystick_interface_auv_node.py b/mission/joystick_interface_auv/joystick_interface_auv/joystick_interface_auv_node.py index 934b9bb89..6031e3bab 100755 --- a/mission/joystick_interface_auv/joystick_interface_auv/joystick_interface_auv_node.py +++ b/mission/joystick_interface_auv/joystick_interface_auv/joystick_interface_auv_node.py @@ -117,9 +117,6 @@ def get_parameters(self): self.get_parameter(f'services.{param}').value, ) - self.declare_parameter('topics.guidance.dp', Parameter.Type.STRING) - self.guidance_topic = self.get_parameter('topics.guidance.dp').value - self.declare_parameter('orientation_mode', 'euler') self._orientation_mode = self.get_parameter('orientation_mode').value if self._orientation_mode not in ('euler', 'quat'): @@ -128,6 +125,13 @@ def get_parameters(self): ) self._orientation_mode = 'euler' + self.declare_parameter('topics.guidance.dp_rpy', Parameter.Type.STRING) + self.declare_parameter('topics.guidance.dp_quat', Parameter.Type.STRING) + if self._orientation_mode == 'quat': + self.guidance_topic = self.get_parameter('topics.guidance.dp_quat').value + else: + self.guidance_topic = self.get_parameter('topics.guidance.dp_rpy').value + def init_movement(self): self.surge = 0.0 self.sway = 0.0 From a361efbe2104b53a843a6672f0f95a252754bafc Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Sat, 4 Apr 2026 11:11:19 +0200 Subject: [PATCH 277/290] add asio to install script --- scripts/ci_install_dependencies.sh | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/scripts/ci_install_dependencies.sh b/scripts/ci_install_dependencies.sh index c35f85488..cbca12a89 100755 --- a/scripts/ci_install_dependencies.sh +++ b/scripts/ci_install_dependencies.sh @@ -27,6 +27,14 @@ sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-13 100 sudo update-alternatives --install /usr/bin/g++ g++ /usr/bin/g++-13 100 sudo update-alternatives --install /usr/bin/gcov gcov /usr/bin/gcov-13 100 +# Install asio +sudo apt-get update -qq +sudo apt-get install -y \ + build-essential \ + cmake \ + git \ + libasio-dev + # Install casadi using CasADi install script "$SCRIPT_DIR/install_casadi.sh" From 348d129b64f3eed070332bbf5a39a594e4deb275 Mon Sep 17 00:00:00 2001 From: vortexuser-topside-laptop Date: Sat, 4 Apr 2026 11:36:45 +0200 Subject: [PATCH 278/290] added dp topcis for orca with new system --- auv_setup/config/robots/orca.yaml | 3 ++- .../config/param_joystick_interface_auv.yaml | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/auv_setup/config/robots/orca.yaml b/auv_setup/config/robots/orca.yaml index b42c57f86..37111d771 100644 --- a/auv_setup/config/robots/orca.yaml +++ b/auv_setup/config/robots/orca.yaml @@ -134,7 +134,8 @@ waypoint: "waypoint" guidance: los: "guidance/los" - dp: "guidance/dp" + dp_rpy: "guidance/dp_rpy" + dp_quat: "guidance/dp_quat" fsm: active_controller: "fsm/active_controller" dvl_twist: "dvl/twist" diff --git a/mission/joystick_interface_auv/config/param_joystick_interface_auv.yaml b/mission/joystick_interface_auv/config/param_joystick_interface_auv.yaml index 5876ce830..e4720276e 100644 --- a/mission/joystick_interface_auv/config/param_joystick_interface_auv.yaml +++ b/mission/joystick_interface_auv/config/param_joystick_interface_auv.yaml @@ -2,7 +2,7 @@ ros__parameters: joystick_surge_gain: 60.0 joystick_sway_gain: 60.0 - joystick_heave_gain: 17.5 + joystick_heave_gain: 60.0 joystick_roll_gain: 30.0 joystick_pitch_gain: 20.0 joystick_yaw_gain: 25.0 From 79414e5c501b9bdc7b4b6d95feca45c77f66b73b Mon Sep 17 00:00:00 2001 From: Cyprian Osinski Date: Sat, 4 Apr 2026 13:19:41 +0200 Subject: [PATCH 279/290] utility --- utility_scripts/launch_drone_vehicle.sh | 59 +++++++++++++++++++++++-- 1 file changed, 56 insertions(+), 3 deletions(-) diff --git a/utility_scripts/launch_drone_vehicle.sh b/utility_scripts/launch_drone_vehicle.sh index 8e0639738..97972363e 100755 --- a/utility_scripts/launch_drone_vehicle.sh +++ b/utility_scripts/launch_drone_vehicle.sh @@ -1,8 +1,61 @@ #!/bin/bash # Launch drone vehicle stack in a tmux session +# Usage: ./launch_drone_vehicle.sh [--ori_type quat|euler] [--controller_type pid|adapt] SESSION="drone_vehicle" +# ============================================= +# Parse arguments +# ============================================= +ORI_TYPE="euler" +CONTROLLER_TYPE="adapt" + +while [[ $# -gt 0 ]]; do + case "$1" in + --ori_type | -o) + ORI_TYPE="$2" + shift 2 + ;; + --controller_type | -c) + CONTROLLER_TYPE="$2" + shift 2 + ;; + *) + echo "Unknown argument: $1" + echo "Usage: $0 [--ori_type quat|euler] [--controller_type pid|adapt]" + exit 1 + ;; + esac +done + +# Validate +if [[ "$ORI_TYPE" != "quat" && "$ORI_TYPE" != "euler" ]]; then + echo "Error: ori_type must be 'quat' or 'euler' (got: $ORI_TYPE)" + exit 1 +fi + +if [[ "$CONTROLLER_TYPE" != "pid" && "$CONTROLLER_TYPE" != "adapt" ]]; then + echo "Error: controller_type must be 'pid' or 'adapt' (got: $CONTROLLER_TYPE)" + exit 1 +fi + +# Cross-validate: pid uses quat reference filter, adaptive uses euler +if [[ "$CONTROLLER_TYPE" == "pid" && "$ORI_TYPE" != "quat" ]]; then + echo "Warning: pid controller uses quaternion representation — consider --ori_type quat" +fi +if [[ "$CONTROLLER_TYPE" == "adapt" && "$ORI_TYPE" != "euler" ]]; then + echo "Warning: adaptive controller uses euler representation — consider --ori_type euler" +fi + +# Map shell arg -> dp.launch.py arg ('adapt' -> 'adaptive') +if [[ "$CONTROLLER_TYPE" == "adapt" ]]; then + DP_CONTROLLER="adaptive" +else + DP_CONTROLLER="pid" +fi + +echo "[LAUNCH] ori_type=$ORI_TYPE controller_type=$CONTROLLER_TYPE" + # Kill existing session if it exists tmux kill-session -t "$SESSION" 2>/dev/null @@ -18,10 +71,10 @@ PANE_C2=$(tmux split-window -h -t "$PANE_C1" -P -F '#{pane_id}') tmux send-keys -t "$PANE_C2" "s && ros2 launch operation_mode_manager operation_mode_manager.launch.py" Enter PANE_C3=$(tmux split-window -v -t "$PANE_C1" -P -F '#{pane_id}') -tmux send-keys -t "$PANE_C3" "s" Enter +tmux send-keys -t "$PANE_C3" "s && ros2 launch auv_setp dp.launch.py controller_type:=$DP_CONTROLLER orientation_mode:=$ORI_TYPE" Enter PANE_C4=$(tmux split-window -v -t "$PANE_C2" -P -F '#{pane_id}') -tmux send-keys -t "$PANE_C4" "s" Enter +tmux send-keys -t "$PANE_C4" "s && ros2 topic echo /nautilus/wrench_input" Enter tmux select-layout -t "$SESSION:control" tiled @@ -31,7 +84,7 @@ tmux select-layout -t "$SESSION:control" tiled tmux new-window -t "$SESSION" -n "perception" PANE_P1=$(tmux list-panes -t "$SESSION:perception" -F '#{pane_id}') -tmux send-keys -t "$PANE_P1" "s" Enter +tmux send-keys -t "$PANE_P1" "s && ros2 launch auv_setup nucleus_odom_transformer.launch.py" Enter PANE_P2=$(tmux split-window -h -t "$PANE_P1" -P -F '#{pane_id}') tmux send-keys -t "$PANE_P2" "s" Enter From 7deec26210401808e1ad859f6e7f05f8a38d0d25 Mon Sep 17 00:00:00 2001 From: Cyprian Osinski Date: Sat, 4 Apr 2026 19:49:35 +0200 Subject: [PATCH 280/290] launched dp adapt quat controller --- auv_setup/launch/dp_quat.launch.py | 60 +++++++++ .../config/adapt_params_nautilus.yaml | 2 +- .../config/adapt_params_orca.yaml | 2 +- .../dp_adapt_backs_controller_ros.hpp | 3 +- .../dp_adapt_backs_controller_quat.launch.py | 2 +- .../src/dp_adapt_backs_controller.cpp | 17 ++- .../src/dp_adapt_backs_controller_ros.cpp | 9 +- .../src/dp_adapt_backs_controller_utils.cpp | 20 +-- .../test/dp_adapt_backs_controller_tests.cpp | 123 +++++++++--------- 9 files changed, 152 insertions(+), 86 deletions(-) create mode 100644 auv_setup/launch/dp_quat.launch.py diff --git a/auv_setup/launch/dp_quat.launch.py b/auv_setup/launch/dp_quat.launch.py new file mode 100644 index 000000000..dd77414ae --- /dev/null +++ b/auv_setup/launch/dp_quat.launch.py @@ -0,0 +1,60 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import OpaqueFunction +from launch_ros.actions import Node + +from auv_setup.launch_arg_common import ( + declare_drone_and_namespace_args, + resolve_drone_and_namespace, +) + + +def launch_setup(context, *args, **kwargs): + drone, namespace = resolve_drone_and_namespace(context) + + filter_config = os.path.join( + get_package_share_directory("reference_filter_dp_quat"), + "config", + "reference_filter_params.yaml", + ) + + drone_params = os.path.join( + get_package_share_directory("auv_setup"), + "config", + "robots", + f"{drone}.yaml", + ) + + adapt_params = os.path.join( + get_package_share_directory("dp_adapt_backs_controller_quat"), + "config", + f"adapt_params_{drone}.yaml", + ) + + return [ + Node( + package="reference_filter_dp_quat", + executable="reference_filter_dp_quat_node", + name="reference_filter_node", + namespace=namespace, + parameters=[filter_config, drone_params], + output="screen", + ), + Node( + package="dp_adapt_backs_controller_quat", + executable="dp_adapt_backs_controller_quat_node", + name="dp_adapt_backs_controller_node", + namespace=namespace, + parameters=[adapt_params, drone_params], + output="screen", + ), + ] + + +def generate_launch_description(): + return LaunchDescription( + declare_drone_and_namespace_args() + + [OpaqueFunction(function=launch_setup)] + ) diff --git a/control/dp_adapt_backs_controller_quat/config/adapt_params_nautilus.yaml b/control/dp_adapt_backs_controller_quat/config/adapt_params_nautilus.yaml index 011a84d5c..5a42f3fe7 100644 --- a/control/dp_adapt_backs_controller_quat/config/adapt_params_nautilus.yaml +++ b/control/dp_adapt_backs_controller_quat/config/adapt_params_nautilus.yaml @@ -5,4 +5,4 @@ r_b_bg : [0.0, 0.0, 0.01] # Vector from body centre to centre of gravity adapt_gain : [0.6, 0.15, 0.6, 0.15, 0.6, 0.15, 0.6, 0.15, 0.6, 0.15, 0.6, 0.15] # Tuning parameters for linear and nonlinear damping d_gain : [0.8, 0.8, 0.8, 0.8, 0.8, 0.8] # Tuning parameters for the unmodeled disturbances and uncertanties - timestep: 10 #Controller timestep in milliseconds (ms) + time_step: 10 #Controller timestep in milliseconds (ms) diff --git a/control/dp_adapt_backs_controller_quat/config/adapt_params_orca.yaml b/control/dp_adapt_backs_controller_quat/config/adapt_params_orca.yaml index 4f14e19ee..0971470f0 100644 --- a/control/dp_adapt_backs_controller_quat/config/adapt_params_orca.yaml +++ b/control/dp_adapt_backs_controller_quat/config/adapt_params_orca.yaml @@ -5,4 +5,4 @@ r_b_bg : [0.01, 0.0, 0.02] # Vector from body centre to centre of gravity adapt_gain : [0.4, 0.1, 0.4, 0.1, 0.4, 0.1, 0.4, 0.1, 0.4, 0.1, 0.4, 0.1] # Tuning parameters for linear and nonlinear damping d_gain : [0.6, 0.6, 0.6, 0.6, 0.6, 0.6] # Tuning parameters for the unmodeled disturbances and uncertanties - timestep: 10 #Controller timestep in milliseconds (ms) + time_step: 10 #Controller timestep in milliseconds (ms) diff --git a/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller_ros.hpp b/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller_ros.hpp index 4f330070e..b3ad6e6e0 100644 --- a/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller_ros.hpp +++ b/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller_ros.hpp @@ -16,6 +16,7 @@ #include #include #include +#include #include #include "dp_adapt_backs_controller_quat/dp_adapt_backs_controller.hpp" #include "dp_adapt_backs_controller_quat/typedefs.hpp" @@ -70,7 +71,7 @@ class DPAdaptBacksControllerNode : public rclcpp::Node { // @param msg: ReferenceFilter message containing the desired vehicle pose // and velocity void guidance_callback( - const vortex_msgs::msg::ReferenceFilter::SharedPtr msg); + const vortex_msgs::msg::ReferenceFilterQuat::SharedPtr msg); rclcpp::Subscription::SharedPtr killswitch_sub_{}; diff --git a/control/dp_adapt_backs_controller_quat/launch/dp_adapt_backs_controller_quat.launch.py b/control/dp_adapt_backs_controller_quat/launch/dp_adapt_backs_controller_quat.launch.py index 8cb6a5f2c..8350bd019 100644 --- a/control/dp_adapt_backs_controller_quat/launch/dp_adapt_backs_controller_quat.launch.py +++ b/control/dp_adapt_backs_controller_quat/launch/dp_adapt_backs_controller_quat.launch.py @@ -30,7 +30,7 @@ def launch_setup(context, *args, **kwargs): return [ Node( package="dp_adapt_backs_controller_quat", - executable="dp_adapt_backs_controller_node", + executable="dp_adapt_backs_controller_quat_node", name="dp_adapt_backs_controller_node", namespace=namespace, parameters=[adapt_params, drone_params], diff --git a/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller.cpp b/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller.cpp index e666b318d..7e564fbbd 100644 --- a/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller.cpp +++ b/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller.cpp @@ -32,27 +32,30 @@ Eigen::Vector6d DPAdaptBacksController::calculate_tau(const Pose& pose, // switching between pure RPY and error state when error is big Eigen::Vector3d pos_error = pose.pos_vector() - pose_d.pos_vector(); Eigen::Vector3d quat_error = vortex::utils::math::quaternion_error( - pose.ori_quaternion, pose_d.ori_quaternion); + pose.ori_quaternion(), pose_d.ori_quaternion()); + Eigen::Vector6d z_1; + z_1 << pos_error, quat_error; - Pose error_state = Pose.from_eigen(pos_error, quat_error) + Eigen::Matrix6d L = Eigen::Matrix6d::Zero(); + L.topLeftCorner<3, 3>() = pose.as_rotation_matrix(); + L.bottomRightCorner<3, 3>() = pose.as_transformation_matrix().bottomRows<3>(); - Eigen::Matrix6d C = + Eigen::Matrix6d C = calculate_coriolis(m_, r_b_bg_, twist, inertia_matrix_body_); Eigen::Matrix6d L_inv = calculate_L_inv(pose); Eigen::Matrix6d L_dot = calculate_L_dot(pose, twist); - Eigen::Vector6d alpha = -L_inv * K1_ * error.to_vector(); - Eigen::Vector6d z_1 = error.to_vector(); + Eigen::Vector6d alpha = -L_inv * K1_ * z_1; Eigen::Vector6d z_2 = twist.to_vector() - alpha; Eigen::Vector6d alpha_dot = ((L_inv * L_dot * L_inv) * K1_ * z_1) - - (L_inv * K1_ * pose.as_j_matrix() * twist.to_vector()); + (L_inv * K1_ * L * 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_intertia_matrix_ * alpha_dot) + (C * twist.to_vector()) - - (pose.as_j_matrix().transpose() * z_1) - (K2_ * z_2) - F_est - d_est_; + (L.transpose() * z_1) - (K2_ * z_2) - F_est - d_est_; // TODO: look at better ways to clamp tau w.r.t new thrusters and allocator tau = tau.cwiseMax(-100.0).cwiseMin(100.0); diff --git a/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_ros.cpp b/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_ros.cpp index b7a2b052e..0f28327a4 100644 --- a/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_ros.cpp +++ b/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_ros.cpp @@ -21,7 +21,7 @@ DPAdaptBacksControllerNode::DPAdaptBacksControllerNode( const rclcpp::NodeOptions& options) : Node("dp_adapt_backs_controller_node", options) { this->declare_parameter("time_step"); - int time_step = static_cast(this->get_parameter("time_step")); + int time_step = static_cast(this->get_parameter("time_step").as_int()); time_step_ = std::chrono::milliseconds(time_step); set_subscribers_and_publisher(); @@ -43,7 +43,7 @@ void DPAdaptBacksControllerNode::set_subscribers_and_publisher() { std::string dp_reference_topic = this->get_parameter("topics.guidance.dp").as_string(); guidance_sub_ = - this->create_subscription( + this->create_subscription( dp_reference_topic, qos_sensor_data, std::bind(&DPAdaptBacksControllerNode::guidance_callback, this, std::placeholders::_1)); @@ -153,7 +153,7 @@ void DPAdaptBacksControllerNode::pose_callback( pose_.z = msg->pose.pose.position.z; const auto& o = msg->pose.pose.orientation; - pose_.w = o.w; + pose_.qw = o.w; pose_.qx = o.x; pose_.qy = o.y; pose_.qz = o.z; @@ -175,8 +175,7 @@ void DPAdaptBacksControllerNode::set_adap_params() { this->declare_parameter>("K1"); this->declare_parameter>("K2"); this->declare_parameter>("r_b_bg"); - this->declare_parameter>( - "physical.mass_intertia_matrix"); + this->declare_parameter>("physical.mass_matrix"); std::vector adapt_param_vec = this->get_parameter("adapt_gain").as_double_array(); diff --git a/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_utils.cpp b/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_utils.cpp index 00d5bd497..22ac8ecdc 100644 --- a/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_utils.cpp +++ b/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_utils.cpp @@ -4,13 +4,14 @@ #include #include #include -#include "dp_adapt_backs_controller/typedefs.hpp" #include "dp_adapt_backs_controller_quat/typedefs.hpp" namespace vortex::control { Eigen::Matrix6d calculate_L_inv(const vortex::utils::types::Pose& pose) { - Eigen::Matrix6d L = pose.as_L_matrix(); + Eigen::Matrix6d L = Eigen::Matrix6d::Zero(); + L.topLeftCorner<3, 3>() = pose.as_rotation_matrix(); + L.bottomRightCorner<3, 3>() = pose.as_transformation_matrix().bottomRows<3>(); constexpr double tolerance = 1e-8; @@ -35,16 +36,15 @@ Eigen::Matrix3d calculate_R_dot(const vortex::utils::types::Pose& pose, Eigen::Matrix3d calculate_Q_dot(const vortex::utils::types::Pose& pose, const vortex::utils::types::Twist& twist) { Eigen::Vector3d omega = twist.to_vector().tail(3); - Eigen::Matrix3d eta_error_dot = - pose.ori_quaternion_vector_part() * omega * Eigen::Matrix3d::Identity(); - Eigen::Matrix3d epsilon_error_dot = - vortex::utils::math::get_skew_symmetric_matrix(pose.as_Q_matrix() * - omega); - Eigen::Matrix3d Q_dot = (1 / 2) * (epsilon_error_dot - eta_error_dot); - return Q_dot; + Eigen::Matrix3d Q_tilde = pose.as_transformation_matrix().bottomRows<3>(); + Eigen::Vector3d eps = pose.ori_quaternion().vec(); + Eigen::Matrix3d eta_dot_term = eps.dot(omega) * Eigen::Matrix3d::Identity(); + Eigen::Matrix3d eps_dot_term = + vortex::utils::math::get_skew_symmetric_matrix(Q_tilde * omega); + return 0.5 * (eps_dot_term - eta_dot_term); } -Eigen::Matrix6d calculate_L_dot(const vortex::utils::types::PoseEuler& pose, +Eigen::Matrix6d calculate_L_dot(const vortex::utils::types::Pose& pose, const vortex::utils::types::Twist& twist) { Eigen::Matrix3d R_dot = calculate_R_dot(pose, twist); Eigen::Matrix3d Q_dot = calculate_Q_dot(pose, twist); diff --git a/control/dp_adapt_backs_controller_quat/test/dp_adapt_backs_controller_tests.cpp b/control/dp_adapt_backs_controller_quat/test/dp_adapt_backs_controller_tests.cpp index 01232bdd3..2e19489ac 100644 --- a/control/dp_adapt_backs_controller_quat/test/dp_adapt_backs_controller_tests.cpp +++ b/control/dp_adapt_backs_controller_quat/test/dp_adapt_backs_controller_tests.cpp @@ -3,12 +3,13 @@ #include -#include "dp_adapt_backs_controller/dp_adapt_backs_controller.hpp" -#include "dp_adapt_backs_controller/dp_adapt_backs_controller_utils.hpp" -#include "dp_adapt_backs_controller/typedefs.hpp" +#include "dp_adapt_backs_controller_quat/dp_adapt_backs_controller.hpp" +#include "dp_adapt_backs_controller_quat/dp_adapt_backs_controller_utils.hpp" +#include "dp_adapt_backs_controller_quat/typedefs.hpp" namespace vortex::control { +using vortex::utils::types::Pose; using vortex::utils::types::PoseEuler; using vortex::utils::types::Twist; @@ -38,10 +39,10 @@ DPAdaptParams load_dp_adapt_params(const std::string& drone_yaml_path, params.adapt_param = Eigen::Map(adapt_gain_vec.data()); params.d_gain = Eigen::Map(d_gain_vec.data()); params.r_b_bg = Eigen::Map(r_b_bg_vec.data()); - params.mass_matrix = mass_matrix; + params.mass_intertia_matrix = mass_matrix; params.mass = mass_matrix(0, 0); - params.I_b = Eigen::Vector3d(mass_matrix(3, 3), mass_matrix(4, 4), - mass_matrix(5, 5)); + params.inertia_matrix_body = Eigen::Vector3d(mass_matrix(3, 3), mass_matrix(4, 4), + mass_matrix(5, 5)); return params; } @@ -51,24 +52,26 @@ class DPAdaptBacksControllerTests : public ::testing::Test { : dp_adapt_backs_controller_{ load_dp_adapt_params(DRONE_YAML_PATH, CONTROLLER_YAML_PATH)} {} - 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}; + Pose 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 PoseEuler{north_pos, east_pos, down_pos, + roll_angle, pitch_angle, yaw_angle} + .as_pose(); } - 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}; + Pose 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 PoseEuler{north_pos, east_pos, down_pos, + roll_angle, pitch_angle, yaw_angle} + .as_pose(); } Twist generate_current_velocity(const double surge_vel, @@ -90,8 +93,8 @@ 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) { - 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)}; + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Pose 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(pose, pose_d, twist)}; @@ -111,8 +114,8 @@ command and negative sway command. TEST_F( DPAdaptBacksControllerTests, T02_neg_north_error_with_positive_heading_gives_pos_surge_and_neg_sway_command) { - 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)}; + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 1.5)}; + Pose 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(pose, pose_d, twist)}; @@ -132,8 +135,8 @@ command and positive sway command. TEST_F( DPAdaptBacksControllerTests, T03_neg_north_error_with_negative_heading_gives_pos_surge_and_pos_sway_command) { - 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)}; + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, -1.5)}; + Pose 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(pose, pose_d, twist)}; @@ -153,8 +156,8 @@ command. TEST_F( DPAdaptBacksControllerTests, T04_neg_down_error_with_zero_roll_and_pitch_gives_positive_heave_command) { - 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)}; + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Pose 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(pose, pose_d, twist)}; @@ -174,8 +177,8 @@ 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) { - 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)}; + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, -0.5, 0.0)}; + Pose 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(pose, pose_d, twist)}; @@ -195,8 +198,8 @@ 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) { - 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)}; + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.5, 0.0)}; + Pose 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(pose, pose_d, twist)}; @@ -214,8 +217,8 @@ 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) { - 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)}; + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Pose 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(pose, pose_d, twist)}; @@ -233,8 +236,8 @@ 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) { - 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)}; + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Pose 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(pose, pose_d, twist)}; @@ -254,8 +257,8 @@ sway command. TEST_F( DPAdaptBacksControllerTests, T09_neg_east_error_with_positive_heading_gives_pos_sway_and_pos_surge_command) { - 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)}; + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 1.5)}; + Pose 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(pose, pose_d, twist)}; @@ -275,8 +278,8 @@ positive sway command. TEST_F( DPAdaptBacksControllerTests, T10_neg_east_error_with_negative_heading_gives_pos_sway_and_neg_surge_command) { - 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)}; + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, -1.5)}; + Pose 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(pose, pose_d, twist)}; @@ -294,8 +297,8 @@ Test that negative roll error gives positive roll command. TEST_F(DPAdaptBacksControllerTests, T11_neg_roll_error_gives_positive_roll_command) { - 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)}; + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Pose 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(pose, pose_d, twist)}; @@ -312,8 +315,8 @@ Test that positive roll error gives negative roll command. */ TEST_F(DPAdaptBacksControllerTests, T12_pos_roll_error_gives_neg_roll_command) { - 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)}; + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Pose 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(pose, pose_d, twist)}; @@ -331,8 +334,8 @@ Test that negative pitch error gives positive pitch command. TEST_F(DPAdaptBacksControllerTests, T13_neg_pitch_error_gives_pos_pitch_command) { - 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)}; + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Pose 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(pose, pose_d, twist)}; @@ -350,8 +353,8 @@ Test that positive pitch error gives negative pitch command. TEST_F(DPAdaptBacksControllerTests, T14_pos_pitch_error_gives_neg_pitch_command) { - 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)}; + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Pose 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(pose, pose_d, twist)}; @@ -368,8 +371,8 @@ Test that negative yaw error gives positive yaw command. */ TEST_F(DPAdaptBacksControllerTests, T15_neg_yaw_error_gives_pos_yaw_command) { - 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)}; + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Pose 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(pose, pose_d, twist)}; @@ -386,8 +389,8 @@ Test that positive yaw error gives negative yaw command. */ TEST_F(DPAdaptBacksControllerTests, T16_pos_yaw_error_gives_neg_yaw_command) { - 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)}; + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Pose 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(pose, pose_d, twist)}; @@ -406,8 +409,8 @@ Test that positive surge velocity only results in negative surge command TEST_F(DPAdaptBacksControllerTests, T17_pos_surge_vel_gives_negative_surge_command) { - 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)}; + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Pose 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(pose, pose_d, twist)}; @@ -428,8 +431,8 @@ effect). TEST_F(DPAdaptBacksControllerTests, T18_pos_sway_vel_gives_negative_sway_command) { - 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)}; + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Pose 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(pose, pose_d, twist)}; @@ -450,8 +453,8 @@ Test that positive heave velocity only results in negative heave command TEST_F(DPAdaptBacksControllerTests, T19_pos_heave_vel_gives_negative_heave_command) { - 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)}; + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Pose 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(pose, pose_d, twist)}; From 358d11dbf4d6bea485ee91c292c5dc575d7279de Mon Sep 17 00:00:00 2001 From: Cyprian Osinski Date: Sun, 5 Apr 2026 06:41:03 +0200 Subject: [PATCH 281/290] changed so gimbal lock no longer gimbals --- auv_setup/config/robots/nautilus.yaml | 2 +- auv_setup/config/robots/nautilus_bak.yaml | 140 ++++++++++++++++++ .../config/adapt_params_nautilus.yaml | 10 +- .../src/dp_adapt_backs_controller_ros.cpp | 4 +- .../pid_controller_dp/config/pid_params.yaml | 36 ++--- .../config/pid_params_bak.yaml | 20 +++ .../pid_controller_utils.hpp | 10 +- .../pid_controller_dp/src/pid_controller.cpp | 18 ++- .../src/pid_controller_ros.cpp | 28 +++- .../src/pid_controller_utils.cpp | 29 ++-- .../src/lib/waypoint_follower.cpp | 10 +- 11 files changed, 249 insertions(+), 58 deletions(-) create mode 100644 auv_setup/config/robots/nautilus_bak.yaml create mode 100644 control/pid_controller_dp/config/pid_params_bak.yaml diff --git a/auv_setup/config/robots/nautilus.yaml b/auv_setup/config/robots/nautilus.yaml index 316c8f7de..eebf373d0 100644 --- a/auv_setup/config/robots/nautilus.yaml +++ b/auv_setup/config/robots/nautilus.yaml @@ -16,7 +16,7 @@ /**: ros__parameters: physical: - center_of_mass: [0.0, 0.0, 0.02] # CO is aligned with CM Position (x,y) in meters (M), small cg offset in z to keep drone naturally stable in roll/pitch + center_of_mass: [0.0, 0.0, 0.15] # CO is aligned with CM Position (x,y) in meters (M), small cg offset in z to keep drone naturally stable in roll/pitch mass_matrix: [53.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 53.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 53.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 11.0628, 1.086, -3.17502, 0.0, 0.0, 0.0, 1.086, 23.1128, 0.1025, 0.0, 0.0, 0.0, -3.17502, 0.1025, 26.23998] # 6x6 mass_inertia_matrix propulsion: diff --git a/auv_setup/config/robots/nautilus_bak.yaml b/auv_setup/config/robots/nautilus_bak.yaml new file mode 100644 index 000000000..a0407a309 --- /dev/null +++ b/auv_setup/config/robots/nautilus_bak.yaml @@ -0,0 +1,140 @@ +# This file defines parameters specific to Nautilus. +# When looking at the AUV from above, the thruster placement is: +# +# front +# |======| +# |=7↗=| |=0↖=| +# | | | | +# | 6• | | 1• | +# | | | | +# | | | | +# | 5• | | 2• | +# | | | | +# |=4↘=|==||==|=3↙=| +# + +/**: + ros__parameters: + physical: + center_of_mass: [0.0, 0.0, 0.1] # CO is aligned with CM Position (x,y) in meters (M), small cg offset in z to keep drone naturally stable in roll/pitch + mass_matrix: [53.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 53.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 53.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 11.0628, 1.086, -3.17502, 0.0, 0.0, 0.0, 1.086, 23.1128, 0.1025, 0.0, 0.0, 0.0, -3.17502, 0.1025, 26.23998] + # 6x6 mass_inertia_matrix + propulsion: + dofs: + num: 6 + dimensions: + num: 3 + thrusters: + num: 8 + thruster_force_direction: [ # Direction of forces X,Y,Z, same logic as thruster_position thruster0 produces thrust in (X0,Y0,Z0) and ||(X0,Y0,Z0)|| = 1 + 0.70711, + 0.00000, + 0.00000, + -0.70711, + -0.70711, + 0.00000, + 0.00000, + 0.70711, # Surge + -0.70711, + 0.00000, + 0.00000, + -0.70711, + 0.70711, + 0.00000, + 0.00000, + 0.70711, # Sway + 0.00000, + 1.00000, + 1.00000, + 0.00000, + 0.00000, + 1.00000, + 1.00000, + 0.00000, # Heave + ] + thruster_position: [ # Position (x0,x1 ... x7,y1,y2, ...,y7,z1,z2, ... ,z7) in meters (M). i.e thruster0 has position (x0,y0,z0) + 0.45015, + 0.24060, + -0.22970, + -0.43861, + -0.43861, + -0.22970, + 0.240600, + 0.450150, # x-positions of the thrusters + 0.305680, + 0.313022, + 0.313022, + 0.305680, + -0.305680, + -0.313022, + -0.313022, + -0.305680, # y-positions of the thrusters + 0.021736, + 0.021736, + 0.021736, + 0.021736, + 0.021736, + 0.021736, + 0.021736, + 0.021736, # z-positions of the thrusters + ] + + constraints: + max_force: 40.0 + min_force: -40.0 + input_matrix_weights: [1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0] + slack_matrix_weights: [2000.0,2000.0,2000.0,2000.0,2000.0,2000.0] + + rate_of_change: + max: 1 # Maximum rate of change in newton per second for a thruster + + thrust_update_rate: 100.0 # [Hz] + watchdog_timeout: 1.0 # [s] + thruster_to_pin_mapping: [4, 5, 7, 6, 0, 1, 2, 3] # I.e. if thruster_to_pin = [ 7, 6, 5, 4, 3, 2, 1, 0 ] then thruster 0 is pin 1 etc.. + thruster_direction: [-1, -1, 1, 1, -1, -1, 1, 1] # Disclose during thruster mapping (+/- 1) + thruster_PWM_min: [1050, 1050, 1050, 1050, 1050, 1050, 1050, 1050] # Minimum PWM value, Recommended: [1100, 1100, 1100, 1100, 1100, 1100, 1100, 1100] + thruster_PWM_max: [1950, 1950, 1950, 1950, 1950, 1950, 1950, 1950] # Maximum PWM value, Recommended: [1900, 1900, 1900, 1900, 1900, 1900, 1900, 1900] + + topics: + wrench_input: "wrench_input" + thruster_forces: "thruster_forces" + pwm_output: "pwm" + current: "power_sense_module/current" + voltage: "power_sense_module/voltage" + pressure: "pressure" + temperature: "temperature" + joy: "joy" + pose: "pose" + twist: "twist" + odom: "odom" + operation_mode: "operation_mode" + killswitch: "killswitch" + reference_pose: "reference_pose" + landmarks: "landmarks" + waypoint: "waypoint" + guidance: + los: "guidance/los" + dp_rpy: "guidance/dp_rpy" + dp_quat: "guidance/dp_quat" + dvl_twist: "dvl/twist" + dvl_altitude: "dvl/altitude" + imu: "imu/data_raw" + sonar_info: "fls/sonar_info" + pressure_sensor: "pressure_sensor" + gripper_servos: "gripper_servos" + + + action_servers: + reference_filter: "reference_filter" + los: "los_guidance" + landmark_polling: "landmark_polling" + landmark_convergence: "landmark_convergence" + waypoint_manager: "waypoint_manager" + + services: + set_operation_mode: "set_operation_mode" + set_killswitch: "set_killswitch" + toggle_killswitch: "toggle_killswitch" + get_operation_mode: "get_operation_mode" + waypoint_addition: "waypoint_addition" + start_mission: "start_mission" diff --git a/control/dp_adapt_backs_controller/config/adapt_params_nautilus.yaml b/control/dp_adapt_backs_controller/config/adapt_params_nautilus.yaml index de23818f7..049309462 100644 --- a/control/dp_adapt_backs_controller/config/adapt_params_nautilus.yaml +++ b/control/dp_adapt_backs_controller/config/adapt_params_nautilus.yaml @@ -1,7 +1,7 @@ /**: ros__parameters: - K1 : [3.5, 3.5, 2.0, 1.0, 2.0, 4.0] # Outer loop tuning parameters - K2 : [14.5, 14.5, 35.0, 10.0, 35.0, 13.5] # Inner loop tuning parameters - r_b_bg : [0.0, 0.0, 0.015] # Vector from body centre to centre of gravity - adapt_gain : [0.2, 0.05, 0.2, 0.05, 0.2, 0.05, 0.2, 0.05, 0.2, 0.05, 0.2, 0.05] # Tuning parameters for linear and nonlinear damping - d_gain : [0.2, 0.2, 0.2, 0.2, 0.2, 0.2] # Tuning parameters for the unmodeled disturbances and uncertanties + K1 : [0.1, 0.1, 10.0, 0.1, 0.1, 10.0] # Outer loop tuning parameters + K2 : [10.0, 10.0, 250.0, 10.0, 10.0, 250.0] # Inner loop tuning parameters + r_b_bg : [0.01, 0.0, 0.015] # Vector from body centre to centre of gravity + adapt_gain : [0.1, 0.05, 0.1, 0.05, 0.1, 0.05, 0.1, 0.05, 0.1, 0.05, 0.1, 0.05] # Tuning parameters for linear and nonlinear damping + d_gain : [0.1, 0.1, 0.1, 0.1, 0.1, 0.1] # Tuning parameters for the unmodeled disturbances and uncertanties 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 1b9578932..a9201b41f 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 @@ -292,8 +292,8 @@ void DPAdaptBacksControllerNode::publish_tau() { geometry_msgs::msg::WrenchStamped tau_msg; tau_msg.header.stamp = this->now(); tau_msg.header.frame_id = "base_link"; - tau_msg.wrench.force.x = tau(0); - tau_msg.wrench.force.y = tau(1); + tau_msg.wrench.force.x = -tau(0); + tau_msg.wrench.force.y = -tau(1); tau_msg.wrench.force.z = tau(2); // comment out if roll control is not needed diff --git a/control/pid_controller_dp/config/pid_params.yaml b/control/pid_controller_dp/config/pid_params.yaml index a3356ece0..487640b84 100644 --- a/control/pid_controller_dp/config/pid_params.yaml +++ b/control/pid_controller_dp/config/pid_params.yaml @@ -1,20 +1,20 @@ /**: ros__parameters: - Kp_x: 40.0 - Kp_y: 40.0 - Kp_z: 40.0 - Kp_roll: 7.5 - Kp_pitch: 11.0 - Kp_yaw: 9.0 - Ki_x: 0.04 - Ki_y: 0.04 - Ki_z: 0.02 - Ki_roll: 0.02 - Ki_pitch: 0.04 - Ki_yaw: 0.02 - Kd_x: 7.5 - Kd_y: 7.5 - Kd_z: 12.5 - Kd_roll: 0.1 - Kd_pitch: 5.0 - Kd_yaw: 0.1 + Kp_x: 50.0 + Kp_y: 50.0 + Kp_z: 25.0 + Kp_roll: 50.0 + Kp_pitch: 80.0 + Kp_yaw: 50.0 + Ki_x: 0.00 + Ki_y: 0.00 + Ki_z: 0.01 + Ki_roll: 0.07 + Ki_pitch: 0.10 + Ki_yaw: 0.0 + Kd_x: 120.0 + Kd_y: 120.0 + Kd_z: 250.0 + Kd_roll: 90.0 + Kd_pitch: 120.0 + Kd_yaw: 90.0 diff --git a/control/pid_controller_dp/config/pid_params_bak.yaml b/control/pid_controller_dp/config/pid_params_bak.yaml new file mode 100644 index 000000000..c72f6cd21 --- /dev/null +++ b/control/pid_controller_dp/config/pid_params_bak.yaml @@ -0,0 +1,20 @@ +/**: + ros__parameters: + Kp_x: 7.0 + Kp_y: 7.0 + Kp_z: 55.0 + Kp_roll: 20.0 + Kp_pitch: 25.0 + Kp_yaw: 20.0 + Ki_x: 0.00 + Ki_y: 0.00 + Ki_z: 0.15 + Ki_roll: 0.05 + Ki_pitch: 0.05 + Ki_yaw: 0.0 + Kd_x: 220.0 + Kd_y: 220.0 + Kd_z: 100.0 + Kd_roll: 100.0 + Kd_pitch: 100.0 + Kd_yaw: 75.0 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 28c7f10ae..442e31813 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 @@ -63,14 +63,14 @@ Eigen::VectorXd clamp_values(const Eigen::VectorXd& values, double min_val, double max_val); -// @brief Calculate the anti-windup term using the 6D error [x,y,z,qx,qy,qz] -// (qw is excluded since only the vector part of the quaternion is used). +// @brief Calculate the anti-windup term. // @param dt: Time step -// @param error: Eta error struct (only x,y,z,qx,qy,qz are read) -// @param integral: 6D integral term [x, y, z, qx, qy, qz] +// @param error_body: 6D error in body frame [surge, sway, heave, roll, pitch, +// yaw] +// @param integral: 6D integral term in body frame // @return 6D anti-windup clamped integral types::Vector6d anti_windup(const double dt, - const types::Eta& error, + const types::Vector6d& error_body, const types::Vector6d& integral); #endif // PID_CONTROLLER_DP__PID_CONTROLLER_UTILS_HPP_ diff --git a/control/pid_controller_dp/src/pid_controller.cpp b/control/pid_controller_dp/src/pid_controller.cpp index 90afbb652..2b523248e 100644 --- a/control/pid_controller_dp/src/pid_controller.cpp +++ b/control/pid_controller_dp/src/pid_controller.cpp @@ -86,22 +86,24 @@ types::Vector6d PIDController::calculate_tau(const types::Eta& eta, // Desired velocity: also drop qw, keeping [x,y,z,qx,qy,qz] derivatives. types::Vector6d eta_dot_d_6; - eta_dot_d_6 << eta_dot_d.x, eta_dot_d.y, eta_dot_d.z, - eta_dot_d.qx, eta_dot_d.qy, eta_dot_d.qz; + eta_dot_d_6 << eta_dot_d.x, eta_dot_d.y, eta_dot_d.z, eta_dot_d.qx, + eta_dot_d.qy, eta_dot_d.qz; // 6x6 J inverse: blockdiag(R, T_33)^{-1} types::Matrix6d J_inv = calculate_J_sudo_inv(eta); - types::Vector6d nu_d = J_inv * eta_dot_d_6; // desired body velocity + types::Vector6d error_body = J_inv * error_6; // error in body frame + + types::Vector6d nu_d = J_inv * eta_dot_d_6; // desired body velocity types::Vector6d error_nu = nu.to_vector() - nu_d; // velocity error - types::Vector6d P = Kp_ * J_inv * error_6; // P term - types::Vector6d I = Ki_ * J_inv * integral_; // I term - types::Vector6d D = Kd_ * error_nu; // D term + types::Vector6d P = Kp_ * error_body; // P term + types::Vector6d I = Ki_ * integral_; // I term + types::Vector6d D = Kd_ * error_nu; // D term - types::Vector6d tau = -clamp_values((P + I + D), -80.0, 80.0); + types::Vector6d tau = -clamp_values((P + I + D), -90.0, 90.0); - integral_ = anti_windup(dt_, error, integral_); + integral_ = anti_windup(dt_, error_body, integral_); return tau; } diff --git a/control/pid_controller_dp/src/pid_controller_ros.cpp b/control/pid_controller_dp/src/pid_controller_ros.cpp index 128e8d104..797d4020d 100644 --- a/control/pid_controller_dp/src/pid_controller_ros.cpp +++ b/control/pid_controller_dp/src/pid_controller_ros.cpp @@ -131,6 +131,12 @@ void PIDControllerNode::operation_mode_callback( void PIDControllerNode::odom_callback( const nav_msgs::msg::Odometry::SharedPtr msg) { eta_ = eta_convert_from_ros_to_eigen(msg->pose); + if (eta_.qw < 0.0) { + eta_.qw = -eta_.qw; + eta_.qx = -eta_.qx; + eta_.qy = -eta_.qy; + eta_.qz = -eta_.qz; + } nu_ = nu_convert_from_ros_to_eigen(msg->twist); } @@ -216,16 +222,26 @@ void PIDControllerNode::set_pid_params() { void PIDControllerNode::guidance_callback( const vortex_msgs::msg::ReferenceFilterQuat::SharedPtr msg) { - // Set desired position eta_d_.x = msg->x; eta_d_.y = msg->y; eta_d_.z = msg->z; - // set desired ori quaternion - eta_d_.qw = msg->qw; - eta_d_.qx = msg->qx; - eta_d_.qy = msg->qy; - eta_d_.qz = msg->qz; + // Enforce positive hemisphere so eta_d_ and eta_ stay in the same half of + // the double cover, reducing sign-flip errors in the PID error computation. + const double sign = msg->qw >= 0.0 ? 1.0 : -1.0; + eta_d_.qw = sign * msg->qw; + eta_d_.qx = sign * msg->qx; + eta_d_.qy = sign * msg->qy; + eta_d_.qz = sign * msg->qz; + + // Desired velocity feedforward (x/y/z in world frame; roll/pitch/yaw are + // the body-frame angular-velocity components from the reference filter). + eta_dot_d_.x = msg->x_dot; + eta_dot_d_.y = msg->y_dot; + eta_dot_d_.z = msg->z_dot; + eta_dot_d_.qx = msg->roll_dot; + eta_dot_d_.qy = msg->pitch_dot; + eta_dot_d_.qz = msg->yaw_dot; } rcl_interfaces::msg::SetParametersResult PIDControllerNode::parametersCallback( diff --git a/control/pid_controller_dp/src/pid_controller_utils.cpp b/control/pid_controller_dp/src/pid_controller_utils.cpp index 7f56666ae..f48aa7796 100644 --- a/control/pid_controller_dp/src/pid_controller_utils.cpp +++ b/control/pid_controller_dp/src/pid_controller_utils.cpp @@ -16,17 +16,23 @@ types::Matrix3d calculate_T_quat(const types::Eta& eta) { types::Matrix6d calculate_J_sudo_inv(const types::Eta& eta) { types::Matrix3d R = calculate_R_quat(eta); - types::Matrix3d T = calculate_T_quat(eta); - - types::Matrix6d J = types::Matrix6d::Zero(); - J.topLeftCorner<3, 3>() = R; - J.bottomRightCorner<3, 3>() = T; - - return J.inverse(); + types::Matrix6d J_inv = types::Matrix6d::Zero(); + J_inv.topLeftCorner<3, 3>() = R.transpose(); + J_inv.bottomRightCorner<3, 3>() = types::Matrix3d::Identity(); + return J_inv; } types::Eta error_eta(const types::Eta& eta, const types::Eta& eta_d) { - return eta - eta_d; + types::Eta error = eta - eta_d; + // Enforce shortest path: q and -q represent the same rotation, but only + // qw >= 0 gives the correct sign for the vector part used as error signal. + if (error.qw < 0.0) { + error.qw = -error.qw; + error.qx = -error.qx; + error.qy = -error.qy; + error.qz = -error.qz; + } + return error; } Eigen::VectorXd clamp_values(const Eigen::VectorXd& values, @@ -36,9 +42,8 @@ Eigen::VectorXd clamp_values(const Eigen::VectorXd& values, } types::Vector6d anti_windup(const double dt, - const types::Eta& error, + const types::Vector6d& error_body, const types::Vector6d& integral) { - types::Vector6d error_6; - error_6 << error.x, error.y, error.z, error.qx, error.qy, error.qz; - return vortex::utils::math::anti_windup(dt, error_6, integral, -80.0, 80.0); + return vortex::utils::math::anti_windup(dt, error_body, integral, -50.0, + 50.0); } diff --git a/guidance/reference_filter_dp_quat/src/lib/waypoint_follower.cpp b/guidance/reference_filter_dp_quat/src/lib/waypoint_follower.cpp index 7a9a6a642..7ad9e9329 100644 --- a/guidance/reference_filter_dp_quat/src/lib/waypoint_follower.cpp +++ b/guidance/reference_filter_dp_quat/src/lib/waypoint_follower.cpp @@ -57,7 +57,15 @@ void WaypointFollower::inject_and_reset() { if (angle >= 1e-10) { Eigen::Quaterniond delta_quat( Eigen::AngleAxisd(angle, delta_orientation.normalized())); - nominal_pose_.set_ori(nominal_pose_.ori_quaternion() * delta_quat); + Eigen::Quaterniond q_new = + nominal_pose_.ori_quaternion() * delta_quat; + // Enforce positive hemisphere to prevent sign flips in the published + // reference quaternion that would cause the downstream controller to + // see large spurious orientation errors. + if (q_new.w() < 0.0) { + q_new.coeffs() = -q_new.coeffs(); + } + nominal_pose_.set_ori(q_new); state_.segment<3>(3).setZero(); } } From e47529e468489363f4671f244465651a5fdde429 Mon Sep 17 00:00:00 2001 From: Cyprian Osinski Date: Sun, 5 Apr 2026 06:49:55 +0200 Subject: [PATCH 282/290] changed to sim and vehicle params --- .../config/adapt_params_nautilus_sim.yaml | 7 +++++++ ...ms_nautilus.yaml => adapt_params_nautilus_vehicle.yaml} | 0 .../config/{pid_params_bak.yaml => pid_params_drone.yaml} | 0 .../config/{pid_params.yaml => pid_params_sim.yaml} | 0 4 files changed, 7 insertions(+) create mode 100644 control/dp_adapt_backs_controller/config/adapt_params_nautilus_sim.yaml rename control/dp_adapt_backs_controller/config/{adapt_params_nautilus.yaml => adapt_params_nautilus_vehicle.yaml} (100%) rename control/pid_controller_dp/config/{pid_params_bak.yaml => pid_params_drone.yaml} (100%) rename control/pid_controller_dp/config/{pid_params.yaml => pid_params_sim.yaml} (100%) diff --git a/control/dp_adapt_backs_controller/config/adapt_params_nautilus_sim.yaml b/control/dp_adapt_backs_controller/config/adapt_params_nautilus_sim.yaml new file mode 100644 index 000000000..907f90868 --- /dev/null +++ b/control/dp_adapt_backs_controller/config/adapt_params_nautilus_sim.yaml @@ -0,0 +1,7 @@ +/**: + ros__parameters: + K1 : [5.0, 5.0, 9.5, 1.5, 5.0, 9.5] # Outer loop tuning parameters + K2 : [25.0, 25.0, 40.0, 25.0, 25.0, 40.0] # Inner loop tuning parameters + r_b_bg : [0.00, 0.0, 0.015] # Vector from body centre to centre of gravity + adapt_gain : [0.4, 0.2, 0.4, 0.2, 0.4, 0.2, 0.4, 0.2, 0.4, 0.2, 0.4, 0.2] # Tuning parameters for linear and nonlinear damping + d_gain : [0.2, 0.2, 0.2, 0.2, 0.2, 0.2] # Tuning parameters for the unmodeled disturbances and uncertanties diff --git a/control/dp_adapt_backs_controller/config/adapt_params_nautilus.yaml b/control/dp_adapt_backs_controller/config/adapt_params_nautilus_vehicle.yaml similarity index 100% rename from control/dp_adapt_backs_controller/config/adapt_params_nautilus.yaml rename to control/dp_adapt_backs_controller/config/adapt_params_nautilus_vehicle.yaml diff --git a/control/pid_controller_dp/config/pid_params_bak.yaml b/control/pid_controller_dp/config/pid_params_drone.yaml similarity index 100% rename from control/pid_controller_dp/config/pid_params_bak.yaml rename to control/pid_controller_dp/config/pid_params_drone.yaml diff --git a/control/pid_controller_dp/config/pid_params.yaml b/control/pid_controller_dp/config/pid_params_sim.yaml similarity index 100% rename from control/pid_controller_dp/config/pid_params.yaml rename to control/pid_controller_dp/config/pid_params_sim.yaml From 81d3f83f9c36af79b3fd25e8b7a4fcfc35048e53 Mon Sep 17 00:00:00 2001 From: Cyprian Osinski Date: Sun, 5 Apr 2026 07:28:30 +0200 Subject: [PATCH 283/290] fixed convention error in quaternion pertrubation that made controller proof unstable --- .../config/adapt_params_nautilus.yaml | 7 ------- .../config/adapt_params_orca.yaml | 7 ------- .../config/adapt_params_nautilus.yaml | 10 +++++----- .../config/adapt_params_orca.yaml | 8 -------- .../src/dp_adapt_backs_controller.cpp | 2 +- 5 files changed, 6 insertions(+), 28 deletions(-) delete mode 100644 control/dp_adapt_backs_controller/config/adapt_params_nautilus.yaml delete mode 100644 control/dp_adapt_backs_controller/config/adapt_params_orca.yaml delete mode 100644 control/dp_adapt_backs_controller_quat/config/adapt_params_orca.yaml diff --git a/control/dp_adapt_backs_controller/config/adapt_params_nautilus.yaml b/control/dp_adapt_backs_controller/config/adapt_params_nautilus.yaml deleted file mode 100644 index 65b639ed7..000000000 --- a/control/dp_adapt_backs_controller/config/adapt_params_nautilus.yaml +++ /dev/null @@ -1,7 +0,0 @@ -/**: - ros__parameters: - K1 : [15.0, 15.0, 15.0, 1.5, 6.0, 6.0] # Outer loop tuning parameters - K2 : [30.0, 30.0, 30.0, 2.0, 8.5, 8.5] # Inner loop tuning parameters - r_b_bg : [0.0, 0.0, 0.01] # Vector from body centre to centre of gravity - adapt_gain : [0.6, 0.15, 0.6, 0.15, 0.6, 0.15, 0.6, 0.15, 0.6, 0.15, 0.6, 0.15] # Tuning parameters for linear and nonlinear damping - d_gain : [0.8, 0.8, 0.8, 0.8, 0.8, 0.8] # Tuning parameters for the unmodeled disturbances and uncertanties diff --git a/control/dp_adapt_backs_controller/config/adapt_params_orca.yaml b/control/dp_adapt_backs_controller/config/adapt_params_orca.yaml deleted file mode 100644 index e128131b2..000000000 --- a/control/dp_adapt_backs_controller/config/adapt_params_orca.yaml +++ /dev/null @@ -1,7 +0,0 @@ -/**: - ros__parameters: - K1 : [10.5, 10.5, 13.5, 0.0, 4.0, 4.0] # Outer loop tuning parameters - K2 : [20.5, 20.5, 20.5, 0.0, 5.5, 5.5] # Inner loop tuning parameters - r_b_bg : [0.01, 0.0, 0.02] # Vector from body centre to centre of gravity - adapt_gain : [0.4, 0.1, 0.4, 0.1, 0.4, 0.1, 0.4, 0.1, 0.4, 0.1, 0.4, 0.1] # Tuning parameters for linear and nonlinear damping - d_gain : [0.6, 0.6, 0.6, 0.6, 0.6, 0.6] # Tuning parameters for the unmodeled disturbances and uncertanties diff --git a/control/dp_adapt_backs_controller_quat/config/adapt_params_nautilus.yaml b/control/dp_adapt_backs_controller_quat/config/adapt_params_nautilus.yaml index 5a42f3fe7..ff7710bfb 100644 --- a/control/dp_adapt_backs_controller_quat/config/adapt_params_nautilus.yaml +++ b/control/dp_adapt_backs_controller_quat/config/adapt_params_nautilus.yaml @@ -1,8 +1,8 @@ /**: ros__parameters: - K1 : [15.0, 15.0, 15.0, 1.5, 6.0, 6.0] # Outer loop tuning parameters - K2 : [30.0, 30.0, 30.0, 2.0, 8.5, 8.5] # Inner loop tuning parameters - r_b_bg : [0.0, 0.0, 0.01] # Vector from body centre to centre of gravity - adapt_gain : [0.6, 0.15, 0.6, 0.15, 0.6, 0.15, 0.6, 0.15, 0.6, 0.15, 0.6, 0.15] # Tuning parameters for linear and nonlinear damping - d_gain : [0.8, 0.8, 0.8, 0.8, 0.8, 0.8] # Tuning parameters for the unmodeled disturbances and uncertanties + K1 : [5.0, 5.0, 9.5, 1.5, 5.0, 9.5] # Outer loop tuning parameters + K2 : [25.0, 25.0, 40.0, 25.0, 25.0, 40.0] # Inner loop tuning parameters + r_b_bg : [0.00, 0.0, 0.015] # Vector from body centre to centre of gravity + adapt_gain : [0.4, 0.2, 0.4, 0.2, 0.4, 0.2, 0.4, 0.2, 0.4, 0.2, 0.4, 0.2] # Tuning parameters for linear and nonlinear damping + d_gain : [0.2, 0.2, 0.2, 0.2, 0.2, 0.2] # Tuning parameters for the unmodeled disturbances and uncertanties time_step: 10 #Controller timestep in milliseconds (ms) diff --git a/control/dp_adapt_backs_controller_quat/config/adapt_params_orca.yaml b/control/dp_adapt_backs_controller_quat/config/adapt_params_orca.yaml deleted file mode 100644 index 0971470f0..000000000 --- a/control/dp_adapt_backs_controller_quat/config/adapt_params_orca.yaml +++ /dev/null @@ -1,8 +0,0 @@ -/**: - ros__parameters: - K1 : [10.5, 10.5, 13.5, 0.0, 4.0, 4.0] # Outer loop tuning parameters - K2 : [20.5, 20.5, 20.5, 0.0, 5.5, 5.5] # Inner loop tuning parameters - r_b_bg : [0.01, 0.0, 0.02] # Vector from body centre to centre of gravity - adapt_gain : [0.4, 0.1, 0.4, 0.1, 0.4, 0.1, 0.4, 0.1, 0.4, 0.1, 0.4, 0.1] # Tuning parameters for linear and nonlinear damping - d_gain : [0.6, 0.6, 0.6, 0.6, 0.6, 0.6] # Tuning parameters for the unmodeled disturbances and uncertanties - time_step: 10 #Controller timestep in milliseconds (ms) diff --git a/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller.cpp b/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller.cpp index 7e564fbbd..5d897dcab 100644 --- a/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller.cpp +++ b/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller.cpp @@ -32,7 +32,7 @@ Eigen::Vector6d DPAdaptBacksController::calculate_tau(const Pose& pose, // switching between pure RPY and error state when error is big Eigen::Vector3d pos_error = pose.pos_vector() - pose_d.pos_vector(); Eigen::Vector3d quat_error = vortex::utils::math::quaternion_error( - pose.ori_quaternion(), pose_d.ori_quaternion()); + pose_d.ori_quaternion(), pose.ori_quaternion()); Eigen::Vector6d z_1; z_1 << pos_error, quat_error; From e819972cac838d4351fab8bf4f3234203d22054e Mon Sep 17 00:00:00 2001 From: vortexuser-Pi Date: Sun, 5 Apr 2026 10:57:33 +0200 Subject: [PATCH 284/290] fixed everything --- auv_setup/config/robots/nautilus.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/auv_setup/config/robots/nautilus.yaml b/auv_setup/config/robots/nautilus.yaml index eebf373d0..d56f8e660 100644 --- a/auv_setup/config/robots/nautilus.yaml +++ b/auv_setup/config/robots/nautilus.yaml @@ -16,7 +16,7 @@ /**: ros__parameters: physical: - center_of_mass: [0.0, 0.0, 0.15] # CO is aligned with CM Position (x,y) in meters (M), small cg offset in z to keep drone naturally stable in roll/pitch + center_of_mass: [0.0, 0.0, 0.05] # CO is aligned with CM Position (x,y) in meters (M), small cg offset in z to keep drone naturally stable in roll/pitch mass_matrix: [53.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 53.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 53.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 11.0628, 1.086, -3.17502, 0.0, 0.0, 0.0, 1.086, 23.1128, 0.1025, 0.0, 0.0, 0.0, -3.17502, 0.1025, 26.23998] # 6x6 mass_inertia_matrix propulsion: @@ -80,8 +80,8 @@ ] constraints: - max_force: 40.0 - min_force: -40.0 + max_force: 39.0 + min_force: -41.0 input_matrix_weights: [1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0] slack_matrix_weights: [2000.0,2000.0,2000.0,2000.0,2000.0,2000.0] From 09b387dea8f34c1f2ca36d3d4c0e5536de4fa15a Mon Sep 17 00:00:00 2001 From: Cyprian Osinski Date: Sun, 5 Apr 2026 12:40:37 +0200 Subject: [PATCH 285/290] added cool ascii art to controller and minor fixups --- .../src/dp_adapt_backs_controller.cpp | 48 +++++++++++++++---- .../src/dp_adapt_backs_controller_ros.cpp | 10 ++-- .../src/dp_adapt_backs_controller_utils.cpp | 2 +- 3 files changed, 48 insertions(+), 12 deletions(-) diff --git a/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller.cpp b/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller.cpp index 5d897dcab..43b23501d 100644 --- a/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller.cpp +++ b/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller.cpp @@ -1,5 +1,6 @@ #include "dp_adapt_backs_controller_quat/dp_adapt_backs_controller.hpp" #include +#include #include #include #include "dp_adapt_backs_controller_quat/dp_adapt_backs_controller_utils.hpp" @@ -28,22 +29,53 @@ DPAdaptBacksController::DPAdaptBacksController( Eigen::Vector6d DPAdaptBacksController::calculate_tau(const Pose& pose, const Pose& pose_d, const Twist& twist) { - // TODO: implement error state calculation. Maybe look at hybrid - // switching between pure RPY and error state when error is big Eigen::Vector3d pos_error = pose.pos_vector() - pose_d.pos_vector(); - Eigen::Vector3d quat_error = vortex::utils::math::quaternion_error( - pose_d.ori_quaternion(), pose.ori_quaternion()); + + // Error quaternion q_e = q_d^{-1} * q (rotation from desired to current). + // z_1_ori = 2*eps_e, so d/dt(z_1_ori) = (qw_e*I + S(eps_e)) * omega. + // This requires building L with the error quaternion, not q_current, + // otherwise the Lyapunov cross-terms don't cancel and orientation diverges. + Eigen::Quaterniond q_e = + pose_d.ori_quaternion().conjugate() * pose.ori_quaternion(); + if (q_e.w() < 0.0) q_e.coeffs() = -q_e.coeffs(); + const Eigen::Vector3d eps_e = q_e.vec(); + const double qw_e = q_e.w(); + const Eigen::Vector3d quat_error = 2.0 * eps_e; + Eigen::Vector6d z_1; z_1 << pos_error, quat_error; + // L: [R(q_current), 0; 0, qw_e*I + S(eps_e)] + const Eigen::Matrix3d R = pose.as_rotation_matrix(); + const Eigen::Matrix3d Q_e = + qw_e * Eigen::Matrix3d::Identity() + + vortex::utils::math::get_skew_symmetric_matrix(eps_e); Eigen::Matrix6d L = Eigen::Matrix6d::Zero(); - L.topLeftCorner<3, 3>() = pose.as_rotation_matrix(); - L.bottomRightCorner<3, 3>() = pose.as_transformation_matrix().bottomRows<3>(); + L.topLeftCorner<3, 3>() = R; + L.bottomRightCorner<3, 3>() = Q_e; + + // L_inv with singularity guard + Eigen::Matrix6d L_inv; + if (std::abs(L.determinant()) < 1e-8) { + spdlog::error("L is singular"); + L_inv = L.completeOrthogonalDecomposition().pseudoInverse(); + } else { + L_inv = L.inverse(); + } + + // L_dot: R_dot = R*S(omega), Q_e_dot via quaternion kinematics on q_e + // qw_e_dot = -0.5 * eps_e^T * omega + // eps_e_dot = 0.5 * Q_e * omega (standard quat kinematics) + const Eigen::Vector3d omega = twist.to_vector().tail<3>(); + const Eigen::Matrix3d Q_e_dot = + (-0.5 * eps_e.dot(omega)) * Eigen::Matrix3d::Identity() + + vortex::utils::math::get_skew_symmetric_matrix(0.5 * Q_e * omega); + Eigen::Matrix6d L_dot = Eigen::Matrix6d::Zero(); + L_dot.topLeftCorner<3, 3>() = calculate_R_dot(pose, twist); + L_dot.bottomRightCorner<3, 3>() = Q_e_dot; Eigen::Matrix6d C = calculate_coriolis(m_, r_b_bg_, twist, inertia_matrix_body_); - Eigen::Matrix6d L_inv = calculate_L_inv(pose); - Eigen::Matrix6d L_dot = calculate_L_dot(pose, twist); Eigen::Vector6d alpha = -L_inv * K1_ * z_1; Eigen::Vector6d z_2 = twist.to_vector() - alpha; Eigen::Vector6d alpha_dot = diff --git a/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_ros.cpp b/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_ros.cpp index 85670e212..baa68d52c 100644 --- a/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_ros.cpp +++ b/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_ros.cpp @@ -10,9 +10,13 @@ #include "dp_adapt_backs_controller_quat/typedefs.hpp" constexpr std::string_view start_message = R"( -████▄ █████▄ ▄█████▄ ▄▄ ▄▄ ▄▄▄ ▄▄▄▄▄▄ ▄█████ ▄▄▄ ▄▄ ▄▄ ▄▄▄▄▄▄ ▄▄▄▄ ▄▄▄ ▄▄ ▄▄ ▄▄▄▄▄ ▄▄▄▄ -██ ██ ██▄▄█▀ ██ ▄ ██ ██ ██ ██▀██ ██ ██ ██▀██ ███▄██ ██ ██▄█▄ ██▀██ ██ ██ ██▄▄ ██▄█▄ -████▀ ██ ▀█████▀ ▀███▀ ██▀██ ██ ▀█████ ▀███▀ ██ ▀██ ██ ██ ██ ▀███▀ ██▄▄▄ ██▄▄▄ ██▄▄▄ ██ ██ + ██████╗ ██╗ ██╗ █████╗ ████████╗███████╗██████╗ ███╗ ██╗██╗ ██████╗ ███╗ ██╗ ██████╗ ██████╗ +██╔═══██╗██║ ██║██╔══██╗╚══██╔══╝██╔════╝██╔══██╗████╗ ██║██║██╔═══██╗████╗ ██║ ██╔══██╗██╔══██╗ +██║ ██║██║ ██║███████║ ██║ █████╗ ██████╔╝██╔██╗ ██║██║██║ ██║██╔██╗ ██║ ██║ ██║██████╔╝ +██║▄▄ ██║██║ ██║██╔══██║ ██║ ██╔══╝ ██╔══██╗██║╚██╗██║██║██║ ██║██║╚██╗██║ ██║ ██║██╔═══╝ +╚██████╔╝╚██████╔╝██║ ██║ ██║ ███████╗██║ ██║██║ ╚████║██║╚██████╔╝██║ ╚████║ ██████╔╝██║ + ╚══▀▀═╝ ╚═════╝ ╚═╝ ╚═╝ ╚═╝ ╚══════╝╚═╝ ╚═╝╚═╝ ╚═══╝╚═╝ ╚═════╝ ╚═╝ ╚═══╝ ╚═════╝ ╚═╝ + )"; namespace vortex::control { diff --git a/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_utils.cpp b/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_utils.cpp index 22ac8ecdc..52cfc81ed 100644 --- a/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_utils.cpp +++ b/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_utils.cpp @@ -38,7 +38,7 @@ Eigen::Matrix3d calculate_Q_dot(const vortex::utils::types::Pose& pose, Eigen::Vector3d omega = twist.to_vector().tail(3); Eigen::Matrix3d Q_tilde = pose.as_transformation_matrix().bottomRows<3>(); Eigen::Vector3d eps = pose.ori_quaternion().vec(); - Eigen::Matrix3d eta_dot_term = eps.dot(omega) * Eigen::Matrix3d::Identity(); + Eigen::Matrix3d eta_dot_term = 0.5 * eps.dot(omega) * Eigen::Matrix3d::Identity(); Eigen::Matrix3d eps_dot_term = vortex::utils::math::get_skew_symmetric_matrix(Q_tilde * omega); return 0.5 * (eps_dot_term - eta_dot_term); From f7dcf8ada55789731f59eb633c14a6017701e795 Mon Sep 17 00:00:00 2001 From: Cyprian Osinski Date: Sun, 5 Apr 2026 15:34:26 +0200 Subject: [PATCH 286/290] fixed everything 2nd DP has been tuned --- auv_setup/config/robots/nautilus.yaml | 2 +- ...ehicle.yaml => adapt_params_nautilus.yaml} | 0 .../config/adapt_params_nautilus.yaml | 10 +-- .../config/adapt_params_nautilus_sim.yaml | 8 +++ .../dp_adapt_backs_controller.hpp | 2 + .../src/dp_adapt_backs_controller.cpp | 24 +++---- .../src/dp_adapt_backs_controller_ros.cpp | 62 ++++++++++++++++++- .../pid_controller_dp/config/pid_params.yaml | 20 ++++++ .../config/pid_params_drone.yaml | 20 ------ 9 files changed, 109 insertions(+), 39 deletions(-) rename control/dp_adapt_backs_controller/config/{adapt_params_nautilus_vehicle.yaml => adapt_params_nautilus.yaml} (100%) create mode 100644 control/dp_adapt_backs_controller_quat/config/adapt_params_nautilus_sim.yaml create mode 100644 control/pid_controller_dp/config/pid_params.yaml delete mode 100644 control/pid_controller_dp/config/pid_params_drone.yaml diff --git a/auv_setup/config/robots/nautilus.yaml b/auv_setup/config/robots/nautilus.yaml index d56f8e660..653c0a2a8 100644 --- a/auv_setup/config/robots/nautilus.yaml +++ b/auv_setup/config/robots/nautilus.yaml @@ -17,7 +17,7 @@ ros__parameters: physical: center_of_mass: [0.0, 0.0, 0.05] # CO is aligned with CM Position (x,y) in meters (M), small cg offset in z to keep drone naturally stable in roll/pitch - mass_matrix: [53.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 53.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 53.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 11.0628, 1.086, -3.17502, 0.0, 0.0, 0.0, 1.086, 23.1128, 0.1025, 0.0, 0.0, 0.0, -3.17502, 0.1025, 26.23998] + mass_matrix: [55.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 55.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 55.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 11.0628, 1.086, -3.17502, 0.0, 0.0, 0.0, 1.086, 23.1128, 0.1025, 0.0, 0.0, 0.0, -3.17502, 0.1025, 26.23998] # 6x6 mass_inertia_matrix propulsion: dofs: diff --git a/control/dp_adapt_backs_controller/config/adapt_params_nautilus_vehicle.yaml b/control/dp_adapt_backs_controller/config/adapt_params_nautilus.yaml similarity index 100% rename from control/dp_adapt_backs_controller/config/adapt_params_nautilus_vehicle.yaml rename to control/dp_adapt_backs_controller/config/adapt_params_nautilus.yaml diff --git a/control/dp_adapt_backs_controller_quat/config/adapt_params_nautilus.yaml b/control/dp_adapt_backs_controller_quat/config/adapt_params_nautilus.yaml index ff7710bfb..a1d0221ac 100644 --- a/control/dp_adapt_backs_controller_quat/config/adapt_params_nautilus.yaml +++ b/control/dp_adapt_backs_controller_quat/config/adapt_params_nautilus.yaml @@ -1,8 +1,8 @@ /**: ros__parameters: - K1 : [5.0, 5.0, 9.5, 1.5, 5.0, 9.5] # Outer loop tuning parameters - K2 : [25.0, 25.0, 40.0, 25.0, 25.0, 40.0] # Inner loop tuning parameters - r_b_bg : [0.00, 0.0, 0.015] # Vector from body centre to centre of gravity - adapt_gain : [0.4, 0.2, 0.4, 0.2, 0.4, 0.2, 0.4, 0.2, 0.4, 0.2, 0.4, 0.2] # Tuning parameters for linear and nonlinear damping - d_gain : [0.2, 0.2, 0.2, 0.2, 0.2, 0.2] # Tuning parameters for the unmodeled disturbances and uncertanties + K1 : [-0.55, -0.7, 2.5, 1.4, 2.0, 0.9] # Outer loop tuning parameters + K2 : [125.0, 120.0, 100.0, 60.0, 60.0, 60.0] # Inner loop tuning parameters + r_b_bg : [0.00, 0.0, 0.010] # Vector from body centre to centre of gravity + adapt_gain : [0.32, 0.11, 0.32, 0.11, 0.32, 0.11, 0.32, 0.11, 0.32, 0.11, 0.32, 0.11] # Tuning parameters for linear and nonlinear damping + d_gain : [0.13, 0.13, 0.13, 0.13, 0.13, 0.13] # Tuning parameters for the unmodeled disturbances and uncertanties time_step: 10 #Controller timestep in milliseconds (ms) diff --git a/control/dp_adapt_backs_controller_quat/config/adapt_params_nautilus_sim.yaml b/control/dp_adapt_backs_controller_quat/config/adapt_params_nautilus_sim.yaml new file mode 100644 index 000000000..ff7710bfb --- /dev/null +++ b/control/dp_adapt_backs_controller_quat/config/adapt_params_nautilus_sim.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + K1 : [5.0, 5.0, 9.5, 1.5, 5.0, 9.5] # Outer loop tuning parameters + K2 : [25.0, 25.0, 40.0, 25.0, 25.0, 40.0] # Inner loop tuning parameters + r_b_bg : [0.00, 0.0, 0.015] # Vector from body centre to centre of gravity + adapt_gain : [0.4, 0.2, 0.4, 0.2, 0.4, 0.2, 0.4, 0.2, 0.4, 0.2, 0.4, 0.2] # Tuning parameters for linear and nonlinear damping + d_gain : [0.2, 0.2, 0.2, 0.2, 0.2, 0.2] # Tuning parameters for the unmodeled disturbances and uncertanties + time_step: 10 #Controller timestep in milliseconds (ms) diff --git a/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller.hpp b/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller.hpp index 37c39a5cd..eb74f17ff 100644 --- a/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller.hpp +++ b/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller.hpp @@ -15,6 +15,7 @@ struct DPAdaptParams { Eigen::Vector3d r_b_bg = Eigen::Vector3d::Zero(); Eigen::Vector3d inertia_matrix_body = Eigen::Vector3d::Zero(); Eigen::Matrix6d mass_intertia_matrix = Eigen::Matrix6d::Zero(); + Eigen::Vector6d tau_max = Eigen::Vector6d::Ones(); double mass{}; }; @@ -58,6 +59,7 @@ class DPAdaptBacksController { Eigen::Vector6d d_est_; Eigen::Matrix3d inertia_matrix_body_; Eigen::Matrix6d mass_intertia_matrix_; + Eigen::Vector6d tau_max_; double m_{}; double dt_{}; }; diff --git a/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller.cpp b/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller.cpp index 43b23501d..46c0c8aa3 100644 --- a/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller.cpp +++ b/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller.cpp @@ -1,6 +1,6 @@ #include "dp_adapt_backs_controller_quat/dp_adapt_backs_controller.hpp" -#include #include +#include #include #include #include "dp_adapt_backs_controller_quat/dp_adapt_backs_controller_utils.hpp" @@ -23,6 +23,7 @@ DPAdaptBacksController::DPAdaptBacksController( inertia_matrix_body_( dp_adapt_params.inertia_matrix_body.asDiagonal().toDenseMatrix()), mass_intertia_matrix_(dp_adapt_params.mass_intertia_matrix), + tau_max_(dp_adapt_params.tau_max), m_(dp_adapt_params.mass), dt_(0.01) {} @@ -37,7 +38,8 @@ Eigen::Vector6d DPAdaptBacksController::calculate_tau(const Pose& pose, // otherwise the Lyapunov cross-terms don't cancel and orientation diverges. Eigen::Quaterniond q_e = pose_d.ori_quaternion().conjugate() * pose.ori_quaternion(); - if (q_e.w() < 0.0) q_e.coeffs() = -q_e.coeffs(); + if (q_e.w() < 0.0) + q_e.coeffs() = -q_e.coeffs(); const Eigen::Vector3d eps_e = q_e.vec(); const double qw_e = q_e.w(); const Eigen::Vector3d quat_error = 2.0 * eps_e; @@ -78,23 +80,21 @@ Eigen::Vector6d DPAdaptBacksController::calculate_tau(const Pose& pose, calculate_coriolis(m_, r_b_bg_, twist, inertia_matrix_body_); Eigen::Vector6d alpha = -L_inv * K1_ * z_1; Eigen::Vector6d z_2 = twist.to_vector() - alpha; - Eigen::Vector6d alpha_dot = - ((L_inv * L_dot * L_inv) * K1_ * z_1) - - (L_inv * K1_ * L * twist.to_vector()); + Eigen::Vector6d alpha_dot = ((L_inv * L_dot * L_inv) * K1_ * z_1) - + (L_inv * K1_ * L * 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_intertia_matrix_ * alpha_dot) + (C * twist.to_vector()) - - (L.transpose() * z_1) - (K2_ * z_2) - F_est - d_est_; + Eigen::Vector6d tau = (mass_intertia_matrix_ * alpha_dot) + + (C * twist.to_vector()) - (L.transpose() * z_1) - + (K2_ * z_2) - F_est - d_est_; - // TODO: look at better ways to clamp tau w.r.t new thrusters and allocator - tau = tau.cwiseMax(-100.0).cwiseMin(100.0); + tau = tau.cwiseMax(-tau_max_).cwiseMin(tau_max_); adapt_param_ += adapt_param_dot * dt_; d_est_ += d_est_dot * dt_; - adapt_param_ = adapt_param_.cwiseMax(-10.0).cwiseMin(10.0); - d_est_ = d_est_.cwiseMax(-10.0).cwiseMin(10.0); + adapt_param_ = adapt_param_.cwiseMax(-15.0).cwiseMin(15.0); + d_est_ = d_est_.cwiseMax(-15.0).cwiseMin(15.0); return tau; } diff --git a/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_ros.cpp b/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_ros.cpp index baa68d52c..226998b92 100644 --- a/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_ros.cpp +++ b/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_ros.cpp @@ -180,6 +180,17 @@ void DPAdaptBacksControllerNode::set_adap_params() { this->declare_parameter>("K2"); this->declare_parameter>("r_b_bg"); this->declare_parameter>("physical.mass_matrix"); + this->declare_parameter>("physical.center_of_mass"); + this->declare_parameter>( + "propulsion.thrusters.thruster_force_direction"); + this->declare_parameter>( + "propulsion.thrusters.thruster_position"); + this->declare_parameter("propulsion.thrusters.num"); + this->declare_parameter("propulsion.dimensions.num"); + this->declare_parameter( + "propulsion.thrusters.constraints.min_force"); + this->declare_parameter( + "propulsion.thrusters.constraints.max_force"); std::vector adapt_param_vec = this->get_parameter("adapt_gain").as_double_array(); @@ -208,6 +219,55 @@ void DPAdaptBacksControllerNode::set_adap_params() { mass_intertia_matrix(4, 4), mass_intertia_matrix(5, 5)); + // Compute per-DOF max wrench from the thruster configuration + int num_thrusters = + this->get_parameter("propulsion.thrusters.num").as_int(); + int num_dims = this->get_parameter("propulsion.dimensions.num").as_int(); + double min_force = + this->get_parameter("propulsion.thrusters.constraints.min_force") + .as_double(); + double max_force = + this->get_parameter("propulsion.thrusters.constraints.max_force") + .as_double(); + + Eigen::Vector3d center_of_mass = Eigen::Map( + this->get_parameter("physical.center_of_mass") + .as_double_array() + .data()); + + auto dir_vec = + this->get_parameter("propulsion.thrusters.thruster_force_direction") + .as_double_array(); + auto pos_vec = + this->get_parameter("propulsion.thrusters.thruster_position") + .as_double_array(); + + Eigen::MatrixXd thruster_dir = + Eigen::Map>( + dir_vec.data(), num_dims, num_thrusters); + Eigen::MatrixXd thruster_pos = + Eigen::Map>( + pos_vec.data(), num_dims, num_thrusters); + + Eigen::MatrixXd T = Eigen::MatrixXd::Zero(6, num_thrusters); + for (int i = 0; i < num_thrusters; i++) { + Eigen::Vector3d pos = thruster_pos.col(i) - center_of_mass; + Eigen::Vector3d F = thruster_dir.col(i); + T.block<3, 1>(0, i) = F; + T.block<3, 1>(3, i) = pos.cross(F); + } + + Eigen::Vector6d tau_max; + for (int i = 0; i < 6; i++) { + double w = 0.0; + for (int j = 0; j < num_thrusters; j++) { + w += (T(i, j) > 0) ? T(i, j) * max_force : T(i, j) * min_force; + } + tau_max(i) = w; + } + DPAdaptParams dp_adapt_params; dp_adapt_params.adapt_param = adapt_param_eigen; dp_adapt_params.d_gain = d_gain_eigen; @@ -216,11 +276,11 @@ void DPAdaptBacksControllerNode::set_adap_params() { dp_adapt_params.r_b_bg = r_b_bg_eigen; dp_adapt_params.inertia_matrix_body = inertia_matrix_body_eigen; dp_adapt_params.mass_intertia_matrix = mass_intertia_matrix; + dp_adapt_params.tau_max = tau_max; dp_adapt_params.mass = mass; dp_adapt_backs_controller_ = std::make_unique(dp_adapt_params); - ; } void DPAdaptBacksControllerNode::publish_tau() { diff --git a/control/pid_controller_dp/config/pid_params.yaml b/control/pid_controller_dp/config/pid_params.yaml new file mode 100644 index 000000000..226d6b219 --- /dev/null +++ b/control/pid_controller_dp/config/pid_params.yaml @@ -0,0 +1,20 @@ +/**: + ros__parameters: + Kp_x: -80.0 + Kp_y: -90.0 + Kp_z: 120.0 + Kp_roll: 35.0 + Kp_pitch: 35.0 + Kp_yaw: 60.0 + Ki_x: 0.00000006 + Ki_y: 0.00000006 + Ki_z: 0.2 + Ki_roll: 0.35 + Ki_pitch: 0.35 + Ki_yaw: 0.005 + Kd_x: 235.0 + Kd_y: 240.0 + Kd_z: 135.0 + Kd_roll: 135.0 + Kd_pitch: 135.0 + Kd_yaw: 125.0 diff --git a/control/pid_controller_dp/config/pid_params_drone.yaml b/control/pid_controller_dp/config/pid_params_drone.yaml deleted file mode 100644 index c72f6cd21..000000000 --- a/control/pid_controller_dp/config/pid_params_drone.yaml +++ /dev/null @@ -1,20 +0,0 @@ -/**: - ros__parameters: - Kp_x: 7.0 - Kp_y: 7.0 - Kp_z: 55.0 - Kp_roll: 20.0 - Kp_pitch: 25.0 - Kp_yaw: 20.0 - Ki_x: 0.00 - Ki_y: 0.00 - Ki_z: 0.15 - Ki_roll: 0.05 - Ki_pitch: 0.05 - Ki_yaw: 0.0 - Kd_x: 220.0 - Kd_y: 220.0 - Kd_z: 100.0 - Kd_roll: 100.0 - Kd_pitch: 100.0 - Kd_yaw: 75.0 From 37edc22dcb33e5e0fd63964179a471515ab73c1e Mon Sep 17 00:00:00 2001 From: Cyprian Osinski Date: Sun, 5 Apr 2026 15:50:28 +0200 Subject: [PATCH 287/290] added utility script to run dp --- utility_scripts/launch_drone_vehicle.sh | 55 ++++++++++++++----------- 1 file changed, 30 insertions(+), 25 deletions(-) diff --git a/utility_scripts/launch_drone_vehicle.sh b/utility_scripts/launch_drone_vehicle.sh index 97972363e..1ddf250e4 100755 --- a/utility_scripts/launch_drone_vehicle.sh +++ b/utility_scripts/launch_drone_vehicle.sh @@ -1,30 +1,30 @@ #!/bin/bash # Launch drone vehicle stack in a tmux session -# Usage: ./launch_drone_vehicle.sh [--ori_type quat|euler] [--controller_type pid|adapt] +# Usage: ./launch_drone_vehicle.sh [--ori_type quat|euler] [--controller_type pid|adapt|adapt_quat] SESSION="drone_vehicle" # ============================================= # Parse arguments # ============================================= -ORI_TYPE="euler" -CONTROLLER_TYPE="adapt" +ORI_TYPE="quat" +CONTROLLER_TYPE="adapt_quat" while [[ $# -gt 0 ]]; do case "$1" in - --ori_type | -o) - ORI_TYPE="$2" - shift 2 - ;; - --controller_type | -c) - CONTROLLER_TYPE="$2" - shift 2 - ;; - *) - echo "Unknown argument: $1" - echo "Usage: $0 [--ori_type quat|euler] [--controller_type pid|adapt]" - exit 1 - ;; + --ori_type | -o) + ORI_TYPE="$2" + shift 2 + ;; + --controller_type | -c) + CONTROLLER_TYPE="$2" + shift 2 + ;; + *) + echo "Unknown argument: $1" + echo "Usage: $0 [--ori_type quat|euler] [--controller_type pid|adapt|adapt_quat]" + exit 1 + ;; esac done @@ -34,8 +34,8 @@ if [[ "$ORI_TYPE" != "quat" && "$ORI_TYPE" != "euler" ]]; then exit 1 fi -if [[ "$CONTROLLER_TYPE" != "pid" && "$CONTROLLER_TYPE" != "adapt" ]]; then - echo "Error: controller_type must be 'pid' or 'adapt' (got: $CONTROLLER_TYPE)" +if [[ "$CONTROLLER_TYPE" != "pid" && "$CONTROLLER_TYPE" != "adapt" && "$CONTROLLER_TYPE" != "adapt_quat" ]]; then + echo "Error: controller_type must be 'pid', 'adapt' or 'adapt_quat' (got: $CONTROLLER_TYPE)" exit 1 fi @@ -46,15 +46,20 @@ fi if [[ "$CONTROLLER_TYPE" == "adapt" && "$ORI_TYPE" != "euler" ]]; then echo "Warning: adaptive controller uses euler representation — consider --ori_type euler" fi +if [[ "$CONTROLLER_TYPE" == "adapt_quat" && "$ORI_TYPE" != "quat" ]]; then + echo "Warning: adapt_quat controller uses quaternion representation — consider --ori_type quat" +fi -# Map shell arg -> dp.launch.py arg ('adapt' -> 'adaptive') -if [[ "$CONTROLLER_TYPE" == "adapt" ]]; then - DP_CONTROLLER="adaptive" +# Select the DP launch file +if [[ "$CONTROLLER_TYPE" == "adapt_quat" ]]; then + DP_LAUNCH="auv_setp dp_quat.launch.py" +elif [[ "$CONTROLLER_TYPE" == "pid" ]]; then + DP_LAUNCH="auv_setp dp.launch.py controller_type:=pid orientation_mode:=$ORI_TYPE" else - DP_CONTROLLER="pid" + DP_LAUNCH="auv_setp dp.launch.py controller_type:=adaptive orientation_mode:=$ORI_TYPE" fi -echo "[LAUNCH] ori_type=$ORI_TYPE controller_type=$CONTROLLER_TYPE" +echo "[LAUNCH] ori_type=$ORI_TYPE controller_type=$CONTROLLER_TYPE dp_launch=$DP_LAUNCH" # Kill existing session if it exists tmux kill-session -t "$SESSION" 2>/dev/null @@ -71,10 +76,10 @@ PANE_C2=$(tmux split-window -h -t "$PANE_C1" -P -F '#{pane_id}') tmux send-keys -t "$PANE_C2" "s && ros2 launch operation_mode_manager operation_mode_manager.launch.py" Enter PANE_C3=$(tmux split-window -v -t "$PANE_C1" -P -F '#{pane_id}') -tmux send-keys -t "$PANE_C3" "s && ros2 launch auv_setp dp.launch.py controller_type:=$DP_CONTROLLER orientation_mode:=$ORI_TYPE" Enter +tmux send-keys -t "$PANE_C3" "s && ros2 launch $DP_LAUNCH" Enter PANE_C4=$(tmux split-window -v -t "$PANE_C2" -P -F '#{pane_id}') -tmux send-keys -t "$PANE_C4" "s && ros2 topic echo /nautilus/wrench_input" Enter +tmux send-keys -t "$PANE_C4" "s" Enter tmux select-layout -t "$SESSION:control" tiled From 132d26917cd79cc8ac710700d15bc63ddc721531 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Sun, 5 Apr 2026 15:52:24 +0200 Subject: [PATCH 288/290] unify reference actions to same action type --- .../ros/reference_filter_ros.hpp | 12 ++++++------ .../src/ros/reference_filter_ros.cpp | 14 +++++++------- 2 files changed, 13 insertions(+), 13 deletions(-) diff --git a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros.hpp b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros.hpp index 8617098ca..458b9a383 100644 --- a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros.hpp +++ b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros.hpp @@ -9,7 +9,7 @@ #include #include #include -#include +#include #include #include #include @@ -38,17 +38,17 @@ class ReferenceFilterNode : public rclcpp::Node { rclcpp_action::GoalResponse handle_goal( const rclcpp_action::GoalUUID& uuid, std::shared_ptr< - const vortex_msgs::action::ReferenceFilterQuatWaypoint::Goal> goal); + const vortex_msgs::action::ReferenceFilterWaypoint::Goal> goal); /// @brief Accept all cancel requests. rclcpp_action::CancelResponse handle_cancel( const std::shared_ptr> goal_handle); + vortex_msgs::action::ReferenceFilterWaypoint>> goal_handle); /// @brief Join the old execution thread and spawn a new one for the goal. void handle_accepted( const std::shared_ptr> goal_handle); + vortex_msgs::action::ReferenceFilterWaypoint>> goal_handle); /** * @brief Execute the action goal in a loop until convergence or @@ -57,9 +57,9 @@ class ReferenceFilterNode : public rclcpp::Node { */ void execute( const std::shared_ptr> goal_handle); + vortex_msgs::action::ReferenceFilterWaypoint>> goal_handle); - rclcpp_action::Server:: + rclcpp_action::Server:: SharedPtr action_server_; ReferenceFilterParams filter_params_; diff --git a/guidance/reference_filter_dp_quat/src/ros/reference_filter_ros.cpp b/guidance/reference_filter_dp_quat/src/ros/reference_filter_ros.cpp index edc1ad0ae..af789f4fd 100644 --- a/guidance/reference_filter_dp_quat/src/ros/reference_filter_ros.cpp +++ b/guidance/reference_filter_dp_quat/src/ros/reference_filter_ros.cpp @@ -101,7 +101,7 @@ void ReferenceFilterNode::set_action_server() { this->get_parameter("action_servers.reference_filter").as_string(); action_server_ = rclcpp_action::create_server< - vortex_msgs::action::ReferenceFilterQuatWaypoint>( + vortex_msgs::action::ReferenceFilterWaypoint>( this, action_server_name, [this](const auto& uuid, auto goal) { return handle_goal(uuid, std::move(goal)); @@ -129,7 +129,7 @@ void ReferenceFilterNode::set_refererence_filter() { rclcpp_action::GoalResponse ReferenceFilterNode::handle_goal( const rclcpp_action::GoalUUID& /*uuid*/, std::shared_ptr< - const vortex_msgs::action::ReferenceFilterQuatWaypoint::Goal> + const vortex_msgs::action::ReferenceFilterWaypoint::Goal> /*goal*/) { spdlog::info("Accepted goal request"); return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; @@ -137,14 +137,14 @@ rclcpp_action::GoalResponse ReferenceFilterNode::handle_goal( rclcpp_action::CancelResponse ReferenceFilterNode::handle_cancel( const std::shared_ptr> /*goal_handle*/) { + vortex_msgs::action::ReferenceFilterWaypoint>> /*goal_handle*/) { spdlog::info("Received request to cancel goal"); return rclcpp_action::CancelResponse::ACCEPT; } void ReferenceFilterNode::handle_accepted( const std::shared_ptr> goal_handle) { + vortex_msgs::action::ReferenceFilterWaypoint>> goal_handle) { std::lock_guard lock(execute_mutex_); preempted_ = true; if (execute_thread_.joinable()) { @@ -158,7 +158,7 @@ void ReferenceFilterNode::handle_accepted( void ReferenceFilterNode::execute( const std::shared_ptr> goal_handle) { + vortex_msgs::action::ReferenceFilterWaypoint>> goal_handle) { spdlog::info("Executing goal"); double convergence_threshold = @@ -182,7 +182,7 @@ void ReferenceFilterNode::execute( follower_->start(pose, twist, wp, convergence_threshold); auto result = std::make_shared< - vortex_msgs::action::ReferenceFilterQuatWaypoint::Result>(); + vortex_msgs::action::ReferenceFilterWaypoint::Result>(); rclcpp::Rate loop_rate(1000.0 / time_step_.count()); @@ -239,7 +239,7 @@ void ReferenceFilterNode::execute( } if (!rclcpp::ok() && goal_handle->is_active()) { auto result = std::make_shared< - vortex_msgs::action::ReferenceFilterQuatWaypoint::Result>(); + vortex_msgs::action::ReferenceFilterWaypoint::Result>(); result->success = false; try { From 0510b55b7950796cd062d5b698c633182c067b3c Mon Sep 17 00:00:00 2001 From: Cyprian Osinski Date: Mon, 6 Apr 2026 20:08:56 +0200 Subject: [PATCH 289/290] added readme to quat AB dp --- .../dp_adapt_backs_controller_quat/README.md | 386 +++++++----------- 1 file changed, 151 insertions(+), 235 deletions(-) diff --git a/control/dp_adapt_backs_controller_quat/README.md b/control/dp_adapt_backs_controller_quat/README.md index 3c34a2b6b..b856f5372 100644 --- a/control/dp_adapt_backs_controller_quat/README.md +++ b/control/dp_adapt_backs_controller_quat/README.md @@ -1,325 +1,281 @@ -## DP Adaptive Backstepping Controller +## DP Adaptive Backstepping Controller — Quaternion Formulation -This package implements a dynamic positioning (DP) Adaptive backstepping controller for the orca AUV. It estimates the linear and nonlinear damping using adaptive parameters, and compensates for uncertainties and disturbances in real-time in the same manner as a state estimator. The proof for this is done using Lyapunov functions and stability requirements to ensure convergence and stability. +This package implements a dynamic positioning (DP) adaptive backstepping controller for the Nautilus AUV. It uses a **unit-quaternion error-state formulation** for the attitude kinematics, which eliminates the gimbal-lock singularity that occurs at ±90° pitch in the Euler-angle version. The adaptive terms estimate the linear and quadratic damping online and compensate for unmodelled disturbances, in the same spirit as a model reference adaptive controller. Stability and convergence are proven via a composite Lyapunov function. ### Overview - Uses the backstepping control method for position and orientation control -- Includes adaptive terms to account for unmodeled dynamics and uncertainties. +- Replaces the Euler-angle Jacobian $J(\eta)$ with the **quaternion error-state Jacobian** $J_e(\eta)$ (called `L` in the code), which is square, smooth, and invertible for all attitude errors smaller than 180° +- Includes adaptive terms to estimate linear and nonlinear (quadratic) damping and external disturbances ### Model for AUV +**Kinematics (error-state form):** + ```math -\dot{\eta} = J(\eta)\nu, +\dot{z}_1 = J_e(\eta)\,\nu ``` +where the error state $z_1 \in \mathbb{R}^6$ is defined below, and the error-state Jacobian is: + ```math -M \dot{\nu} + C(\nu)\,\nu - F(\nu, \Theta^\star) = \tau + d^\star +J_e(\eta) = +\begin{bmatrix} +R(q) & 0_{3\times 3} \\ +0_{3\times 3} & T_e(q_e) +\end{bmatrix}, \quad +T_e(q_e) = \eta_e I_3 + S(\varepsilon_e) ``` -- $\nu$: Body-fixed velocity vector -- $\eta$: Inertial position and orientation vector -- M: Constant mass-inertia matrix -- C($\eta$): Coriolis and centripetal terms -- J($\eta$): Transformation from body to inertial coordinates -- F($\nu$, $\Theta^\star$) = Y($\nu$) $\Theta^\star$: The damping assumed damping function (linear and nonlinear), where Y(*) describes the behaviour -- $d^\star$: Disturbance and uncertainties +Here $R(q) \in SO(3)$ is the rotation matrix from NED to body, $q_e = q_d^* \otimes q = \begin{bmatrix}\eta_e \\ \varepsilon_e\end{bmatrix}$ is the error quaternion, and $S(\cdot)$ is the skew-symmetric (cross-product) matrix. + +**Dynamics (Newton–Euler, body frame):** + +```math +M\dot{\nu} + C(\nu)\,\nu - F(\nu,\Theta^\star) = \tau + d^\star +``` + +- $\nu \in \mathbb{R}^6$: body-fixed velocity (linear and angular) +- $M$: constant, symmetric, positive-definite mass-inertia matrix +- $C(\nu)$: Coriolis and centripetal matrix +- $F(\nu, \Theta^\star) = Y(\nu)\Theta^\star$: damping in regressor form (linear and quadratic per DOF) +- $\tau$: control wrench +- $d^\star$: lumped disturbances and unmodelled dynamics + +**Notation note:** The symbol $\eta_e$ denotes the scalar part of the error quaternion (not the pose vector $\eta$); this follows Fossen's standard overloading. ### File overview + 1. **dp_adapt_backs_controller.cpp/hpp** - - The controller implementation - - Implements the main control input, sets gains, references, and mass parameters. + - Core controller implementation: computes `L`, `L_inv`, `L_dot`, the error state $z_1$, $z_2$, $\alpha$, $\dot\alpha$, and the full control wrench $\tau$. + - Integrates the adaptive parameters online. 2. **dp_adapt_backs_controller_utils.cpp/hpp** - - Provides utility functions: skew-symmetric matrix generation, quaternion-to-Euler, Jacobian and some functions needed for the adaptive functions. + - Utility functions: `calculate_L_inv`, `calculate_R_dot`, `calculate_Q_dot`, `calculate_L_dot`, `calculate_coriolis`, `calculate_Y_v`. 3. **dp_adapt_backs_controller_ros.cpp/hpp** - - ROS node wrapper for the controller, subscribing to pose, twist, killswitch, and reference topics. - - Publishes thrust commands. - -4. **dp_adapt_backs_controller_node.cpp** - - Entry point for the ROS executable. - -5. **adapt_params.yaml** - - Tunable controller parameters (K1, K2, adapt_gain, d_gain) - - Contains the mass matrix (6×6) and other physical parameters: - ```math - M = - \begin{bmatrix} - 30.0 & 0.0 & 0.0 & \cdots \\ - 0.0 & 30.0 & \cdots & \cdots \\ - 0.0 & \cdots & 30.0 & \cdots \\ - \cdots & \cdots & \cdots & \cdots - \end{bmatrix} - ``` - (partially shown for brevity) - This is also added into the orca.yaml file - -6. **CMakeLists.txt** - - Build configuration, ROS 2 dependencies, executable generation, and installation setup. + - ROS 2 node wrapper: subscribes to odometry, killswitch, and reference topics; publishes thrust commands. -### Tuning Parameters -- **K1** and **K2**: Gains for the backstepping control laws. -- **adapt_gain**: Adaptive gain for the linear and nonlinear damping. -- **d_gain**: Adaptive gain for the disturbances and uncertainties. -- **M** and **I_b**: Mass inertia matrix and rotational inertia. -- **m**: Vehicle mass. +4. **adapt_params_nautilus.yaml / adapt_params_nautilus_sim.yaml** + - Tunable controller parameters (`K1`, `K2`, `adapt_gain`, `d_gain`, `r_b_bg`, `time_step`). -## Backstepping controller +5. **CMakeLists.txt** + - Build configuration, ROS 2 dependencies, executable generation, and installation setup. -Using the Lyapunov proof we get the following functions for the backstepping controller: - -##### Backstepping variables - -```math -z_1 = \eta - \eta_d -``` +### Tuning Parameters +- **K1**: Outer loop gain matrix (position and orientation errors $z_1$). +- **K2**: Inner loop gain matrix (velocity error $z_2$). +- **adapt\_gain**: Diagonal adaptation rate for the 12 damping parameters ($\Gamma_\theta$). +- **d\_gain**: Diagonal adaptation rate for the 6 disturbance estimates ($\Gamma_d$). +- **r\_b\_bg**: Vector from body origin to centre of gravity (used in the Coriolis matrix). -```math -z_2 = \nu - \alpha -``` +## Backstepping Controller -where the $\alpha$ is defined as the function that stabilizes the $z_1$ variable in the Lyapunov function system. +### Error state and backstepping variables -##### Adaptive parameters +The orientation error quaternion is formed by left-multiplication with the desired quaternion conjugate: ```math -\tilde{\Theta} = \hat{\Theta} - \Theta^\star +q_e = q_d^* \otimes q = \begin{bmatrix} \eta_e \\ \varepsilon_e \end{bmatrix} ``` +The tracking error in $\mathbb{R}^6$ is then: + ```math -\tilde{d} = \hat{d} - d^\star +z_1 = \begin{bmatrix} p - p_d \\ 2\varepsilon_e \end{bmatrix} ``` -where: - -- $\Theta^\star$ and $d^\star$ are the actual parameters, -- $\hat{\Theta}$ and $\hat{d}$ are the estimated parameters, -- $\tilde{\Theta}$ and $\tilde{d}$ are the estimation errors. - -#### Proof of control law - -We define the Lyapunov function candidate (LFC) as: +The factor of 2 on the vector part of the error quaternion ensures $\dot{z}_{1,\text{ori}} = T_e(q_e)\,\omega$ and makes the LFC derivative tractable. The velocity error is: ```math -V_1 = \frac{1}{2} z_1^\top z_1 +z_2 = \nu - \alpha ``` -By fulfilling the criteriums of RUB, positive definite and \(V_1(0) = 0\), we need to do a derivation and ensure negative definite. +where $\alpha$ is the virtual control law defined below. -Taking the derivative of the Lyapunov function candidate \(V_1\): +### Adaptive parameters ```math -\dot{V}_1 = \frac{d}{dt} \left( \frac{1}{2} z_1^\top z_1 \right) +\tilde{\Theta} = \hat{\Theta} - \Theta^\star, \qquad \tilde{d} = \hat{d} - d^\star ``` -Using the chain rule, we get: +where: +- $\Theta^\star$ and $d^\star$ are the (unknown) true parameters +- $\hat{\Theta}$ and $\hat{d}$ are online estimates +- $\tilde{\Theta}$ and $\tilde{d}$ are the estimation errors -```math -\dot{V}_1 = z_1^\top \dot{z}_1 -``` +### Proof of control law -Substitute $\dot{z}_1$ with the dynamics of $z_1$: +#### Step 1 — Outer loop (position and attitude) + +Define the LFC: ```math -\dot{z}_1 = \dot{\eta} - \dot{\eta}_d +V_1 = \frac{1}{2} z_1^\top z_1 ``` -and then inserting in the relation: +which is positive definite, radially unbounded, and satisfies $V_1(0) = 0$. For a constant setpoint ($\dot{\eta}_d = 0$): ```math -\dot{\eta} = J(\eta)\nu +\dot{V}_1 = z_1^\top \dot{z}_1 = z_1^\top J_e(\eta)\,\nu ``` -```math -\dot{z}_1 = J(\eta)\nu - \dot{\eta}_d -``` +Treating $\nu$ as a virtual input (Khalil §14.3) and splitting $\nu = \alpha + z_2$: -and ```math -\nu = z_2 + \alpha +\dot{V}_1 = z_1^\top J_e\,\alpha + z_1^\top J_e\,z_2 ``` -Thus, the derivative of the Lyapunov function candidate is: -```math -\dot{V}_1 = z_1^\top J(\eta)(z_2 + \alpha) -``` - -We also assume that $\dot{\eta_d} = 0$ since we only get a desired position and orientation ($\eta$). -We choose an $\alpha$ value to make $z_1$ be negative semi-definite. +Choose the virtual control law: ```math \boxed{ -\alpha = -J(\eta)^{-1}(K_1 z_1) +\alpha = -J_e(\eta)^{-1} K_1\, z_1, \quad K_1 = K_1^\top > 0 } ``` -```math -\dot{V}_1 = z_1^\top J(\eta)z_2 - z_1^\top K_1 z_1 < 0 -``` - -We will now define the rest of the Lyapunov function candidates, including the adaptive parameter. +Then $z_1^\top J_e\,\alpha = -z_1^\top K_1 z_1 < 0$ and: ```math -V_2 = \frac{1}{2} z_2^\top M z_2 +\dot{V}_1 = -z_1^\top K_1 z_1 + z_1^\top J_e\, z_2 ``` -For the second backstepping variable: +The cross term $z_1^\top J_e z_2$ will be cancelled in Step 2. -```math -V_{\theta} = \frac{1}{2} \tilde{\Theta}^\top \Gamma^{-1}_{\theta} \tilde{\Theta} -``` +#### Step 2 — Inner loop (velocity) + +Augment the LFC with the inertia-weighted velocity term (Fossen 2021, §12.1): ```math -V_d = \frac{1}{2} \tilde{d}^\top \Gamma^{-1}_{d} \tilde{d} +V_2 = \frac{1}{2} z_2^\top M\, z_2, \quad M = M^\top > 0,\; \dot{M} = 0 ``` -Where the $\Gamma$ matrix is a diagonal gain matrix for tuning the adaptive rate of the parameters. The reasoning behind inserting the adaptive parameters is that we want to ensure that the error, marked by the tilde, converges towards zero. To achieve this, we need to ensure that it actually does this by accounting for it in the Lyapunov function and controller. - -Then we combine them like this: +Differentiating and substituting the dynamics: ```math -V = V_1 + V_2 + V_{\theta} + V_d +\dot{V}_2 = z_2^\top M\,(\dot{\nu} - \dot{\alpha}) = z_2^\top\bigl(\tau - C(\nu)\nu + Y(\nu)\Theta^\star + d^\star - M\dot{\alpha}\bigr) ``` -and now we will analyze the derivative of this CLF, and ensure convergence for the whole function. +**Cross-term cancellation.** The scalar $z_1^\top J_e z_2 = z_2^\top J_e^\top z_1$. If the control law contains $-J_e^\top z_1$, then in $\dot{V}_1 + \dot{V}_2$: ```math -\dot{V} = \dot{V}_1 + z_2^\top M \dot{z}_2 + \tilde{\Theta}^\top \Gamma^{-1}_{\theta} \dot{\tilde{\Theta}} + \tilde{d}^\top \Gamma^{-1}_{d} \dot{\tilde{d}} +z_1^\top J_e\, z_2 + z_2^\top(-J_e^\top z_1) = z_2^\top J_e^\top z_1 - z_2^\top J_e^\top z_1 = 0 ``` -Before we write this out, we need to make some assumptions to make this more readable and easier to understand. +The cross terms cancel exactly, independent of the structure of $J_e$. -For $\dot{\tilde{\Theta}} = \dot{\hat{\Theta}} - \dot{\Theta}^\star$ we assume that the actual value has no changes, assuming it's static, and therefore the derivative $\dot{\Theta}^\star = 0$ +#### Adaptive extension -2. The same condition holds for $\dot{\tilde{d}}$ +Since $\Theta^\star$ and $d^\star$ are unknown, form the composite LFC: ```math -\dot{V} = \dot{V}_1 + z_2^\top M (\dot{\nu} - \dot{\alpha}) + \tilde{\Theta}^\top \Gamma^{-1}_{\theta} \dot{\hat{\Theta}} + \tilde{d}^\top \Gamma^{-1}_{d} \dot{\hat{d}} +V = V_1 + V_2 + \frac{1}{2}\tilde{\Theta}^\top \Gamma^{-1}_{\theta}\,\tilde{\Theta} + \frac{1}{2}\tilde{d}^\top \Gamma^{-1}_{d}\,\tilde{d} ``` -```math -= \dot{V}_1 - z_2^\top M \dot{\alpha} + z_2^\top\tau - z_2^\top C(\nu)\,\nu + z_2^\top F(\nu, \Theta^\star) + z_2^\top d + \tilde{\Theta}^\top \Gamma^{-1}_{\theta} \dot{\hat{\Theta}} + \tilde{d}^\top \Gamma^{-1}_{d} \dot{\hat{d}} -``` +Assuming $\dot{\Theta}^\star = 0$ and $\dot{d}^\star = 0$ (static true parameters): ```math -= - z_1^\top K_1z_1 + z_2^\top J(\eta)^\top z_1 - z_2^\top M \dot{\alpha} + z_2^\top \tau - z_2^\top C(\nu)\,\nu + z_2^\top Y(\nu) \Theta\star + z_2^\top d + \tilde{\Theta}^\top \Gamma^{-1}_{\theta} \dot{\hat{\Theta}} + \tilde{d}^\top \Gamma^{-1}_{d} \dot{\hat{d}} +\dot{V} = \dot{V}_1 + z_2^\top M(\dot{\nu} - \dot{\alpha}) + \tilde{\Theta}^\top \Gamma^{-1}_{\theta}\,\dot{\hat{\Theta}} + \tilde{d}^\top \Gamma^{-1}_{d}\,\dot{\hat{d}} ``` -Since we only know the estimate of the adaptive parameters, we can write the controller in two parts: +Substituting the control law: ```math -\tau = \tau_{controller} - F(\nu, \hat{\Theta}) - d = \tau_{controller} -Y(\nu) \hat{\Theta} - \hat{d} +\tau = -J_e^\top z_1 - K_2\, z_2 + M\dot{\alpha} + C(\nu)\nu - Y(\nu)\hat{\Theta} - \hat{d} ``` -Important to notice is that we introduce the estimate (^) for the variables, not the actual value (*). -We insert this into the system and get: -```math -= - z_1^\top K_1 z_1 + z_2^\top J(\eta)^\top z_1 - z_2^\top M \dot{\alpha} + z_2^\top \tau_{controller} - z_2^\top C(\nu)\,\nu - z_2^\top Y(\nu) (\hat{\Theta} - \Theta\star ) - z_2^\top (\hat{d} - d\star) + \tilde{\Theta}^\top \Gamma^{-1}_{\theta} \dot{\hat{\Theta}} + \tilde{d}^\top \Gamma^{-1}_{d} \dot{\hat{d}} -``` +and collecting terms (cross terms cancel, $C(\nu)\nu$ cancels, $M\dot\alpha$ cancels): -We look at the adaptive parameters a little more and try to simplify them as much as possible - -```math -\hat{\Theta} - \Theta^\star = \hat{\Theta} - (\hat{\Theta} - \tilde{\Theta}) = \tilde{\Theta} \newline -``` ```math -\hat{d} - d^\star = \hat{d} - (\hat{d} - \tilde{d}) = \tilde{d} +\dot{V} = -z_1^\top K_1 z_1 - z_2^\top K_2 z_2 ++ \tilde{\Theta}^\top\!\left(\Gamma_\theta^{-1}\dot{\hat{\Theta}} - Y(\nu)^\top z_2\right) ++ \tilde{d}^\top\!\left(\Gamma_d^{-1}\dot{\hat{d}} - z_2\right) ``` -Now we have: -```math -= - z_1^\top K_1 z_1 + z_2^\top J(\eta)^\top z_1 - z_2^\top M \dot{\alpha} + z_2^\top \tau_{controller} - z_2^\top C(\nu)\,\nu + (-z_2^\top Y(\nu) \tilde{\Theta}+ \tilde{\Theta}^\top \Gamma^{-1}_{\theta} \dot{\hat{\Theta}}) + (-z_2^\top \tilde{d} + \tilde{d}^\top \Gamma^{-1}_{d} \dot{\hat{d}}) -``` -From this we can separate out the terms with the adaptive parameters. We can write them up as two separate equations: +From this we can separate the adaptive terms: ```math --z_2^\top Y(\nu) \tilde{\Theta}+\tilde{\Theta}^\top \Gamma^{-1}_{\theta} \dot{\hat{\Theta}} = - \tilde{\Theta}^\top Y(\nu)^\top z_2 +\tilde{\Theta}^\top \Gamma^{-1}_{\theta} \dot{\hat{\Theta}} +\tilde{\Theta}^\top\!\left(\Gamma_\theta^{-1}\dot{\hat{\Theta}} - Y(\nu)^\top z_2\right) = \tilde{\Theta}^\top \Gamma_\theta^{-1}\!\left(\dot{\hat{\Theta}} - \Gamma_\theta Y(\nu)^\top z_2\right) ``` ```math --z_2^\top \tilde{d} + \tilde{d}^\top \Gamma^{-1}_{d} \dot{\hat{d}} = - \tilde{d}^\top z_2+ \tilde{d}^\top \Gamma^{-1}_{d} \dot{\hat{d}} +\tilde{d}^\top\!\left(\Gamma_d^{-1}\dot{\hat{d}} - z_2\right) = \tilde{d}^\top \Gamma_d^{-1}\!\left(\dot{\hat{d}} - \Gamma_d z_2\right) ``` -We choose the \(\dot{\hat{\Theta}}\) and \(\dot{\hat{d}}\) to zero this out +Choosing the update laws to zero these brackets: + ```math \boxed{ -\dot{\hat{\Theta}} = \Gamma_{\theta} Y(\nu)^\top z_2 +\dot{\hat{\Theta}} = \Gamma_{\theta}\, Y(\nu)^\top z_2 } ``` ```math \boxed{ -\dot{\hat{d}} = \Gamma_{d} z_2 +\dot{\hat{d}} = \Gamma_{d}\, z_2 } ``` -Now that we have defined this we can insert and remove this from the equation, which should leave us with the normal system. An observation made during the construction of the controller was that the adaptive part and backstepping part is decoupled. Maybe this is dependent on the method used for the adaptive part, which is more of a MRAC type. Sadly, I don't have enough information about adaptive controllers to comment on this in detail. + +This gives the final Lyapunov derivative: ```math -= - z_1^\top K_1 z_1 + z_2^\top J(\eta)^\top z_1 - z_2^\top M \dot{\alpha} + z_2^\top \tau_{controller} - z_2^\top C(\nu)\,\nu -``` -```math -\tau_{controller} = -K_2 z_2 + M \dot{\alpha} + C(\nu)\nu - J(\eta)^\top z_1 +\dot{V} = -z_1^\top K_1 z_1 - z_2^\top K_2 z_2 < 0, \quad \forall\,(z_1,z_2) \neq 0 ``` -Combined this gives us the control law: +Global asymptotic stability of $z_1 = 0$, $z_2 = 0$ follows from LaSalle's invariance principle (Khalil §4.2), with parameter estimates remaining bounded by the adaptive law structure. + +### Full control law ```math \boxed{ -\tau = -K_2 z_2 + M \dot{\alpha} + C(\nu)\nu - J(\eta)z_1 - F(\nu, \hat{\Theta}) - \hat{d} +\tau = -J_e^\top z_1 - K_2\, z_2 + M\dot{\alpha} + C(\nu)\nu - Y(\nu)\hat{\Theta} - \hat{d} } ``` -### Controller gains -The gains \(K_1\) and \(K_2\) are crucial for ensuring the stability and performance of the controller. +### Controller gains -**\(K_1\)**: This gain matrix is associated with the outer loop stability, which primarily deals with the position and orientation errors (\(z_1\)). Proper tuning of \(K_1\) ensures that the position and orientation errors converge to zero, thereby stabilizing the outer loop. +**$K_1$** is the outer loop gain (position and orientation errors): ```math +K_1 = \begin{bmatrix} -k_{1,1} & 0 & 0 & 0 & 0 & 0 \\ -0 & k_{1,2} & 0 & 0 & 0 & 0 \\ -0 & 0 & k_{1,3} & 0 & 0 & 0 \\ -0 & 0 & 0 & k_{1,4} & 0 & 0 \\ -0 & 0 & 0 & 0 & k_{1,5} & 0 \\ -0 & 0 & 0 & 0 & 0 & k_{1,6} +k_{1,1} & & & & & \\ +& k_{1,2} & & & & \\ +& & k_{1,3} & & & \\ +& & & k_{1,4} & & \\ +& & & & k_{1,5} & \\ +& & & & & k_{1,6} \end{bmatrix} ``` -**\(K_2\)**: This gain matrix is related to the inner loop stability, which handles the velocity errors (\(z_2\)). Tuning \(K_2\) appropriately ensures that the velocity errors are minimized, stabilizing the inner loop. +**$K_2$** is the inner loop gain (velocity errors): ```math K_2 = \begin{bmatrix} -k_{2,1} & 0 & 0 & 0 & 0 & 0 \\ -0 & k_{2,2} & 0 & 0 & 0 & 0 \\ -0 & 0 & k_{2,3} & 0 & 0 & 0 \\ -0 & 0 & 0 & k_{2,4} & 0 & 0 \\ -0 & 0 & 0 & 0 & k_{2,5} & 0 \\ -0 & 0 & 0 & 0 & 0 & k_{2,6} +k_{2,1} & & & & & \\ +& k_{2,2} & & & & \\ +& & k_{2,3} & & & \\ +& & & k_{2,4} & & \\ +& & & & k_{2,5} & \\ +& & & & & k_{2,6} \end{bmatrix} ``` -Together, \(K_1\) and \(K_2\) work to ensure that both the position/orientation and velocity errors are driven to zero, guaranteeing the overall stability and performance of the adaptive backstepping controller. - ### Adaptive parameters and functions -Now that we have the proof for the control law, we need to look at the adaptive functions and parameters. - -Since we wanted a damping (linear and nonlinear) \(Y(\nu)\) was chosen to be a 6x12 matrix with one linear and one nonlinear damping element: +The damping regressor $Y(\nu) \in \mathbb{R}^{6 \times 12}$ captures one linear and one quadratic term per DOF: ```math Y(\nu) = \begin{bmatrix} -\nu[0] & \nu[0] |\nu[0]| & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ -0 & 0 & \nu[1] & \nu[1] |\nu[1]| & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ -0 & 0 & 0 & 0 & \nu[2] & \nu[2] |\nu[2]| & 0 & 0 & 0 & 0 & 0 & 0 \\ -0 & 0 & 0 & 0 & 0 & 0 & \nu[3] & \nu[3] |\nu[3]| & 0 & 0 & 0 & 0 \\ -0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & \nu[4] & \nu[4] |\nu[4]| & 0 & 0 \\ -0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & \nu[5] & \nu[5] |\nu[5]| +\nu_1 & \nu_1|\nu_1| & 0 & 0 & \cdots & 0 & 0 \\ +0 & 0 & \nu_2 & \nu_2|\nu_2| & \cdots & 0 & 0 \\ +\vdots & & & & \ddots & & \vdots \\ +0 & 0 & 0 & 0 & \cdots & \nu_6 & \nu_6|\nu_6| \end{bmatrix} ``` -while the \(\hat{\Theta}\) will be a 12x1 vector: +The parameter vector $\hat{\Theta} \in \mathbb{R}^{12}$ is: ```math \hat{\Theta} = @@ -328,91 +284,51 @@ while the \(\hat{\Theta}\) will be a 12x1 vector: \end{bmatrix}^\top ``` -The \(\Gamma_{\Theta}\) will therefore be a 12x12 diagonal vector with the gains for the linear and nonlinear damping. +where $\alpha_i$ and $\beta_i$ are the estimated linear and quadratic damping coefficients for DOF $i$. The adaptation gain $\Gamma_\theta$ is a $12 \times 12$ positive-definite diagonal matrix. -The disturbance \(d\) will have a 6x1 vector for the disturbance and uncertainty estimates: +The disturbance estimate $\hat{d} \in \mathbb{R}^6$ has one component per DOF, adapted with the $6 \times 6$ diagonal gain $\Gamma_d$. -```math -\hat{d} = -\begin{bmatrix} -d_1 & d_2 & d_3 & d_4 & d_5 & d_6 -\end{bmatrix}^\top -``` - -#### Important information - -The implementation of the controller requires \(\dot{\alpha}\) which is not that simple to calculate, given that this requires the derivative of the \(J(\eta)\) matrix. +### Important implementation detail: computing $\dot{\alpha}$ -```math -\dot{\alpha} -\;=\; --\;J(\eta)^{-1}\,\dot{J}(\eta)\,J(\eta)^{-1}\bigl[- k_1\,(\eta - \eta_d)\bigr] -\;+\; --J(\eta)^{-1} k_1\,\dot{\eta} -``` - -This was manually derived, and \(J(\eta)\) was also derived manually but here we can use a trick: +The control law requires $\dot{\alpha}$, the time derivative of the virtual control. Since $\alpha = -J_e(\eta)^{-1}K_1 z_1$, applying the matrix identity $\tfrac{d}{dt}(A^{-1}) = -A^{-1}\dot{A}A^{-1}$: ```math -J(\eta) = -\begin{bmatrix} -R & 0_{3x3} \\ -0_{3x3} & T -\end{bmatrix} +\dot{\alpha} = J_e^{-1}\dot{J}_e J_e^{-1} K_1 z_1 - J_e^{-1} K_1 J_e\,\nu ``` -where: -- \(R\) is the rotation matrix representing the orientation of the AUV. -- \(T\) is the transformation matrix for the translational components. -- \(0_{3x3}\) is a 3x3 zero matrix. - -The derivative of \(J(\eta)\) can be expressed in terms of the derivatives of \(R\) and \(T\): +The block structure $J_e = \begin{bmatrix} R & 0 \\ 0 & T_e \end{bmatrix}$ gives: ```math -\dot{J}(\eta) = +\dot{J}_e = \begin{bmatrix} -\dot{R} & 0_{3x3} \\ -0_{3x3} & \dot{T} +\dot{R} & 0_{3\times 3} \\ +0_{3\times 3} & \dot{T}_e \end{bmatrix} ``` -The trick is that \(\dot{R}\) can be written as: - -```math -\dot{R} = R \cdot S \cdot r -``` - -where: -- \(R\) is the rotation matrix calculated from \(\eta\). -- \(S\) is the skew-symmetric matrix of the vector \((0, 0, 1)\). -- \(r\) is the z-component of the angular speed from \(\nu\). - -For the \(T\) the expression needs to be manually calculated and this is not nice to look at: +**$\dot{R}$** uses the standard identity: ```math -\dot{T}(\eta, \nu) = -\begin{bmatrix} -0 & \cos(\phi) \tan(\theta) \nu_x + \sin(\phi) \sec^2(\theta) \nu_y & -\sin(\phi) \tan(\theta) \nu_x + \cos(\phi) \sec^2(\theta) \nu_y \\ -0 & -\sin(\phi) \nu_x & -\cos(\phi) \nu_x \\ -0 & \cos(\phi) / \cos(\theta) \nu_x + \sin(\phi) \sin(\theta) / \cos^2(\theta) \nu_y & -\sin(\phi) / \cos(\theta) \nu_x + \cos(\phi) \sin(\theta) / \cos^2(\theta) \nu_y -\end{bmatrix} +\dot{R} = R\,S(\omega) ``` -This can be calculated by the chain rule and partial differentiation on \(\phi, \theta, \psi\): +**$\dot{T}_e$** follows from differentiating $T_e = \eta_e I + S(\varepsilon_e)$ using the quaternion kinematic equations $\dot{\eta}_e = -\tfrac{1}{2}\varepsilon_e^\top\omega$ and $\dot{\varepsilon}_e = \tfrac{1}{2}T_e\omega$: ```math -\dot{T}(\eta, \nu) = \frac{\partial T}{\partial \phi} \dot{\phi} + \frac{\partial T}{\partial \theta} \dot{\theta} + \frac{\partial T}{\partial \psi} \dot{\psi} +\dot{T}_e = \tfrac{1}{2}\bigl(S(T_e\,\omega) - (\varepsilon_e^\top\omega)\,I_3\bigr) ``` -Here we could potentially use methods for estimating the derivative, but one of the benefits of this calculation is that we get good and accurate values for the rotation and translation derivatives. Instead of introducing more uncertainties as we try to remove this in the system with the adaptive part. +This compact closed form — compared to the lengthy trigonometric expression required for $\dot{T}$ in the Euler-angle version — is a key practical advantage of the quaternion parameterisation. ## Launch To run the controller, use the ROS 2 launch file: ```bash -ros2 launch dp_adapt_backs_controller dp_adapt_backs_controller.launch.py +ros2 launch dp_adapt_backs_controller_quat dp_adapt_backs_controller_quat.launch.py ``` -It's important to `colcon build` and `source install/setup.bash` before launching, or else it will not be launchable. +Remember to `colcon build` and `source install/setup.bash` first. -This will load the parameters from the provided YAML, start the node, and begin control operation. +Two configuration files are provided: +- `adapt_params_nautilus.yaml` — tuned for the physical Nautilus AUV +- `adapt_params_nautilus_sim.yaml` — tuned for simulation From f6908647d7da51707382dc09aba2e38438613519 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Mon, 13 Apr 2026 14:29:16 +0200 Subject: [PATCH 290/290] tf: move down cam down --- auv_setup/description/nautilus.urdf.xacro | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/auv_setup/description/nautilus.urdf.xacro b/auv_setup/description/nautilus.urdf.xacro index f7db7222c..ac43afe3c 100644 --- a/auv_setup/description/nautilus.urdf.xacro +++ b/auv_setup/description/nautilus.urdf.xacro @@ -46,7 +46,7 @@ - +