Henrik/velocity control

control/velocity_controller/CMakeLists.txt

@@ -0,0 +1,129 @@
+cmake_minimum_required(VERSION 3.8)
+project(velocity_controller)
+
+set(CMAKE_CXX_STANDARD 20)
+
+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)
+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(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
+#  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+#  $<INSTALL_INTERFACE:include>
+#)
+include_directories(
+  ${EIGEN3_INCLUDE_DIR}
+  include
+)
+
+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
+)
+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
+  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)
+
+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
+  velocity_controller_node
+  test_VC_node
+  test_LQR_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()

control/velocity_controller/config/parameters.yaml

@@ -0,0 +1,41 @@
+/**:
+  ros__parameters:
+
+    topics:
+      odom_topic: /orca/odom #Odometry
+      twist_topic: /dvl/twist #Twist
+      pose_topic: /dvl/pose #Pose
+      guidance_topic: /guidance/los #Guidance
+      thrust_topic: /orca/wrench_input #Thrust
+      softwareoperation_topic: /softwareOperationMode #Software Operation
+      killswitch_topic: /softwareKillSwitch #Kill Switch
+
+    LQR_params:
+      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
+
+    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: 1 #1 PID 2 LQR
+

control/velocity_controller/include/velocity_controller/LQR_setup.hpp

@@ -0,0 +1,90 @@
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/string.hpp>
+#include <geometry_msgs/msg/wrench_stamped.hpp>
+#include "PID_setup.hpp"
+#include <Eigen/Dense>
+#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::Matrix<double,3,6> K;
+    Eigen::Matrix<double,6,6> P;
+    int INFO=0;
+    LQRsolveResult(Eigen::Matrix<double,3,6> K,Eigen::Matrix<double,6,6> P, 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());
+
+
+    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);
+
+    //angle quaternion_to_euler_angle(double w, double x, double y, double z);
+    //double ssa(double angle);
+
+    std::tuple<double,double> saturate (double value, bool windup, double limit);
+    double anti_windup(double ki, double error, double integral_sum, bool windup);  
+    Eigen::Vector<double,3> saturate_input(Eigen::Vector<double,3> u);
+
+    Eigen::Vector<double,6> update_error(Guidance_data guidance_values, State states);
+    Eigen::Vector<double,3> calculate_lqr_u(State states, Guidance_data guidance_values);
+
+    LQRsolveResult solve_k_p(const Eigen::Matrix<double,6,6> &A,const Eigen::Matrix<double,6,3> &B,const Eigen::Matrix<double,6,6> &Q, const Eigen::Matrix<double,3,3> &R);
+
+    //Resets controller
+    void reset_controller();
+
+    // VariablesEigen::Matrix3d vector_to_matrix3d(const std::vector<double> &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;
+    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::Matrix3d inertia_matrix_inv;
+    Eigen::Matrix<double,6,6> state_weight_matrix;
+    Eigen::Matrix3d input_weight_matrix;
+    Eigen::Matrix<double,6,6> augmented_system_matrix;
+    Eigen::Matrix<double,6,3> augmented_input_matrix;
+
+
+};
+
+//Extra operations
+Eigen::Matrix3d vector_to_matrix3d(const std::vector<double> &other_matrix);
+std::vector<double> matrix3d_to_vector(const Eigen::Matrix3d &mat);
+std::vector<std::vector<double>> matrix3d_to_vector2d(const Eigen::Matrix3d &mat);
+Eigen::Matrix3d vector2d_to_matrix3d(const std::vector<std::vector<double>> &other_matrix);

control/velocity_controller/include/velocity_controller/PID_setup.hpp

@@ -0,0 +1,28 @@
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/string.hpp>
+#include <std_msgs/msg/float64_multi_array.hpp>
+#include <geometry_msgs/msg/wrench_stamped.hpp>
+#include "utilities.hpp"
+
+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 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 integral;
+    double previous_error;
+    double previous_position;
+    double last_output;
+    double max_output;
+    double min_output;
+};
+

control/velocity_controller/include/velocity_controller/test_VC.hpp

@@ -0,0 +1,48 @@
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/string.hpp>
+#include <geometry_msgs/msg/wrench_stamped.hpp>
+#include <cmath>
+#include <std_msgs/msg/float64_multi_array.hpp>
+#include "velocity_controller/PID_setup.hpp"
+#include "velocity_controller/velocity_controller.hpp"
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
+#include "vortex_msgs/msg/los_guidance.hpp" 
+
+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
+
+    //Subscribers and publishers
+    rclcpp::Publisher<vortex_msgs::msg::LOSGuidance>::SharedPtr publisher_guidance;
+    rclcpp::Publisher<vortex_msgs::msg::LOSGuidance>::SharedPtr publisher_state;
+    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr subscription_state;
+    //Timers
+    rclcpp::TimerBase::SharedPtr timer_;
+    rclcpp::Clock::SharedPtr clock_;
+    //Messages
+    //std::vector<double> 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;
+};
+
+geometry_msgs::msg::Quaternion euler_angle_to_quaternion(double roll, double pitch, double yaw);

control/velocity_controller/include/velocity_controller/utilities.hpp

@@ -0,0 +1,32 @@
+#pragma once
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/string.hpp>
+#include "std_msgs/msg/float64_multi_array.hpp"
+#include "vortex_msgs/msg/los_guidance.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, 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(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} {};
+
+    Guidance_data operator-(const Guidance_data& other) const;
+    Guidance_data& operator=(const std_msgs::msg::Float64MultiArray& msg);
+};