feat/fix: PID quaternion-based DP controller

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")
@@ -16,24 +16,71 @@ find_package(geometry_msgs REQUIRED)
 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
   nav_msgs
   Eigen3
   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
@@ -46,4 +93,9 @@ install(DIRECTORY
   DESTINATION share/${PROJECT_NAME}/
 )
 
+
+if(BUILD_TESTING)
+  add_subdirectory(test)
+endif()
+
 ament_package()

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

control/pid_controller_dp/include/pid_controller_dp/pid_controller.hpp

@@ -1,6 +1,7 @@
 #ifndef PID_CONTROLLER_HPP
 #define PID_CONTROLLER_HPP
 
+#include <spdlog/spdlog.h>
 #include "pid_controller_dp/typedefs.hpp"
 
 class PIDController {
@@ -36,6 +37,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_;

control/pid_controller_dp/include/pid_controller_dp/pid_controller_ros.hpp

@@ -7,6 +7,7 @@
 #include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
 #include <geometry_msgs/msg/wrench_stamped.hpp>
 #include <nav_msgs/msg/odometry.hpp>
+#include <rcl_interfaces/msg/set_parameters_result.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <std_msgs/msg/bool.hpp>
 #include <std_msgs/msg/float64_multi_array.hpp>
@@ -55,6 +56,12 @@ 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
+    // @param parameters: vector of parameters to be set
+    rcl_interfaces::msg::SetParametersResult parametersCallback(
+        const std::vector<rclcpp::Parameter>& parameters);
+
     PIDController pid_controller_;
 
     rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr killswitch_sub_;
@@ -93,6 +100,8 @@ class PIDControllerNode : public rclcpp::Node {
     bool killswitch_on_;
 
     std::string software_mode_;
+
+    OnSetParametersCallbackHandle::SharedPtr callback_handle_;
 };
 
 #endif

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 <spdlog/spdlog.h>
 #include <tf2/LinearMath/Matrix3x3.h>
 #include <tf2/LinearMath/Quaternion.h>
 #include <cmath>
 #include <eigen3/Eigen/Geometry>
 #include <std_msgs/msg/float64_multi_array.hpp>
+#include <vortex/utils/types.hpp>
 #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

control/pid_controller_dp/package.xml

@@ -15,6 +15,8 @@
   <depend>eigen</depend>
   <depend>tf2</depend>
   <depend>vortex_msgs</depend>
+  <depend>rcl_interfaces</depend>
+  <depend>vortex_utils</depend>
 
   <export>
     <build_type>ament_cmake</build_type>

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()),
@@ -12,21 +77,52 @@ 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
+
+    // set w = 0
+    error.ori.w() = 0.0;  // only use vector part of quaternion for error
 
-    types::Matrix6x7d J_inv = calculate_J_sudo_inv(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;
+    spdlog::info("Eta: ");
+    print_eta(eta);
+    spdlog::info("Eta desired: ");
+    print_eta(eta_d);
+    spdlog::info("Eta error:");
+    print_eta(error);
 
-    types::Vector6d nu_d = J_inv * eta_dot_d.as_vector();
+    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 error_nu = nu.as_vector() - 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 P = Kp_ * J_inv * error.as_vector();
+    types::Vector6d error_nu = nu.as_vector() - nu_d;  // calculate vel error
+    // error_nu_debug = error_nu;
+    // print_vect_6d(error_nu);
 
-    types::Vector6d I = Ki_ * J_inv * integral_;
+    types::Vector6d P = Kp_ * J_inv * error.as_vector();  // P term
+    // P_debug = P;
+    Kp_debug = Kp_;
 
-    types::Vector6d D = Kd_ * error_nu;
+    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();
 
     integral_ = anti_windup(dt_, error, integral_);
 

control/pid_controller_dp/src/pid_controller_node.cpp

@@ -1,8 +1,18 @@
+#include <spdlog/spdlog.h>
 #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<PIDControllerNode>());
     rclcpp::shutdown();
     return 0;