Henrik/velocity control

control/velocity_controller/CMakeLists.txt

@@ -0,0 +1,117 @@
+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(Eigen3 REQUIRED)
+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
+)
+
+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
+
+)
+
+ament_target_dependencies(test_VC_node
+  rclcpp
+  std_msgs
+  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)
+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})
+
+
+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,37 @@
+/**:
+  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.6, 0.0, 0.6, 1.629, 0.0, 0.0, 0.0, 1.729]
+
+    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
+

control/velocity_controller/include/velocity_controller/LQR_setup.hpp

@@ -0,0 +1,123 @@
+#pragma once
+
+#include <Eigen/Dense>
+#include <geometry_msgs/msg/wrench_stamped.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/string.hpp>
+#include "PID_setup.hpp"
+#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,33 @@
+#pragma once
+
+#include <geometry_msgs/msg/wrench_stamped.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/float64_multi_array.hpp>
+#include <std_msgs/msg/string.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,49 @@
+#pragma once
+
+#include <cmath>
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
+#include <geometry_msgs/msg/wrench_stamped.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/float64_multi_array.hpp>
+#include <std_msgs/msg/string.hpp>
+#include "velocity_controller/PID_setup.hpp"
+#include "velocity_controller/velocity_controller.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 send_state();
+
+    // Variables
+    // guidance_data reference;
+    Guidance_data current_state;
+    // Subscribers and publishers
+    rclcpp::Publisher<vortex_msgs::msg::LOSGuidance>::SharedPtr
+        publisher_guidance;
+    rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr publisher_odom;
+    rclcpp::Subscription<geometry_msgs::msg::WrenchStamped>::SharedPtr
+        subscription_thrust;
+    // 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;
+
+    // MSGS
+    nav_msgs::msg::Odometry odom_msg;
+};
+
+geometry_msgs::msg::Quaternion euler_angle_to_quaternion(double roll,
+                                                         double pitch,
+                                                         double yaw);