feat/fix: PID quaternion-based DP controller

.gitignore

@@ -58,3 +58,8 @@ qtcreator-*
 .#*
 
 # 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

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: 20.0
+    Kp_y: 0.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
+    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_DP__PID_CONTROLLER_HPP_
 #define PID_CONTROLLER_DP__PID_CONTROLLER_HPP_
 
+#include <spdlog/spdlog.h>
 #include "pid_controller_dp/typedefs.hpp"
 
 class PIDController {
@@ -36,6 +37,25 @@ 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;
+    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>
@@ -56,6 +57,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_;
@@ -94,6 +101,8 @@ class PIDControllerNode : public rclcpp::Node {
     bool killswitch_on_;
 
     std::string software_mode_;
+
+    OnSetParametersCallbackHandle::SharedPtr callback_handle_;
 };
 
 #endif  // PID_CONTROLLER_DP__PID_CONTROLLER_ROS_HPP_

control/pid_controller_dp/include/pid_controller_dp/pid_controller_utils.hpp

@@ -1,12 +1,15 @@
 #ifndef PID_CONTROLLER_DP__PID_CONTROLLER_UTILS_HPP_
 #define PID_CONTROLLER_DP__PID_CONTROLLER_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"
+#include "typedefs.hpp"
 
 // @brief Calculate the sine of an angle in degrees
 // @param angle: Angle in degrees
@@ -71,4 +74,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  // PID_CONTROLLER_DP__PID_CONTROLLER_UTILS_HPP_

control/pid_controller_dp/include/pid_controller_dp/typedefs.hpp

@@ -7,48 +7,31 @@
 #define PID_CONTROLLER_DP__TYPEDEFS_HPP_
 
 #include <eigen3/Eigen/Dense>
+#include <vortex/utils/types.hpp>
 
 namespace types {
-typedef Eigen::Matrix<double, 3, 1> Vector3d;
-typedef Eigen::Matrix<double, 6, 1> Vector6d;
-typedef Eigen::Matrix<double, 7, 1> Vector7d;
-typedef Eigen::Matrix<double, 4, 1> Vector4d;
-typedef Eigen::Matrix<double, 6, 6> Matrix6d;
-typedef Eigen::Matrix<double, 3, 3> Matrix3d;
-typedef Eigen::Matrix<double, 4, 3> Matrix4x3d;
-typedef Eigen::Matrix<double, 7, 6> Matrix7x6d;
-typedef Eigen::Matrix<double, 6, 7> Matrix6x7d;
-typedef Eigen::Matrix<double, 7, 7> 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<double, 3, 1>;
+using Vector4d = Eigen::Matrix<double, 4, 1>;
+using Vector6d = Eigen::Matrix<double, 6, 1>;
+using Vector7d = Eigen::Matrix<double, 7, 1>;
+using Matrix3d = Eigen::Matrix<double, 3, 3>;
+using Matrix4x3d = Eigen::Matrix<double, 4, 3>;
+using Matrix6d = Eigen::Matrix<double, 6, 6>;
+using Matrix7x6d = Eigen::Matrix<double, 7, 6>;
+using Matrix6x7d = Eigen::Matrix<double, 6, 7>;
+using Matrix7d = Eigen::Matrix<double, 7, 7>;
+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;

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>