docs: added documentation

vortexntnu · Talhanc · Sep 27, 2024 · Oct 11, 2024 · Oct 11, 2024 · Oct 13, 2024
commit 614bc3e13196afe320e918a802aa5e0cbae2d4ac
diff --git a/control/pid_controller_dp/CMakeLists.txt b/control/pid_controller_dp/CMakeLists.txt
@@ -1,7 +1,6 @@
 cmake_minimum_required(VERSION 3.8)
 project(pid_controller_dp)
 
-# Default to C++14
 if(NOT CMAKE_CXX_STANDARD)
   set(CMAKE_CXX_STANDARD 14)
 endif()
@@ -10,7 +9,6 @@ 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(nav_msgs REQUIRED)
@@ -21,15 +19,15 @@ find_package(vortex_msgs REQUIRED)
 
 include_directories(include)
 
-add_executable(pid_controller_node 
+add_executable(pid_controller_node
   src/pid_controller_node.cpp
   src/pid_controller_ros.cpp
   src/pid_controller.cpp
   src/pid_controller_utils.cpp
 )
 
-ament_target_dependencies(pid_controller_node 
-  rclcpp 
+ament_target_dependencies(pid_controller_node
+  rclcpp
   geometry_msgs
   nav_msgs
   Eigen3
@@ -52,4 +50,4 @@ install(DIRECTORY
   DESTINATION share/${PROJECT_NAME}/
 )
 
-ament_package()
+ament_package()
diff --git a/control/pid_controller_dp/config/pid_params.yaml b/control/pid_controller_dp/config/pid_params.yaml
@@ -1,9 +1,5 @@
 pid_controller_node:
   ros__parameters:
-    # Kp: [100.0, 100.0, 100.0, 40.0, 40.0, 40.0]
-    # Ki: [5.0, 5.0, 5.0, 1.0, 1.0, 1.0]
-    # Kd: [28.0, 22.0, 28.0, 3.0, 3.0, 3.0]
-
     Kp: [110.0, 110.0, 110.0, 15.0, 15.0, 15.0]
     Ki: [2.0, 2.0, 2.0, 0.12, 0.12, 0.12]
     Kd: [15.0, 15.0, 15.0, 6.0, 7.0, 6.0]
diff --git a/control/pid_controller_dp/include/pid_controller_dp/eigen_typedefs.hpp b/control/pid_controller_dp/include/pid_controller_dp/eigen_typedefs.hpp
@@ -9,15 +9,15 @@
 #include <eigen3/Eigen/Dense>
 
 namespace Eigen {
-  typedef Eigen::Matrix<double, 6, 6> Matrix6d;
-  typedef Eigen::Matrix<double, 3, 3> Matrix3d;
   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::Matrix<double, 7, 7> Matrix7d;
 } // namespace Eigen
 
 #endif
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
@@ -7,14 +7,28 @@ class PIDController {
 public:
   explicit PIDController();
 
+  // @brief Calculate the control input tau
+  // @param eta: 7D vector containing the vehicle pose [x, y, z, w, x, y, z]
+  // @param eta_d: 7D vector containing the desired vehicle pose [x, y, z, w, x, y, z]
+  // @param nu: 6D vector containing the vehicle velocity [u, v, w, p, q, r]
+  // @param eta_dot_d: 7D vector containing the desired vehicle velocity [u, v, w, p, q, r]
+  // @return 6D vector containing the control input tau [u, v, w, p, q, r]
   Eigen::Vector6d calculate_tau(const Eigen::Vector7d &eta, const Eigen::Vector7d &eta_d, const Eigen::Vector6d &nu, const Eigen::Vector7d &eta_dot_d);
 
+  // @brief Set the proportional gain matrix
+  // @param Kp: 6x6 matrix containing the proportional gain matrix
   void setKp(const Eigen::Matrix6d &Kp);
 
+  // @brief Set the integral gain matrix
+  // @param Ki: 6x6 matrix containing the integral gain matrix
   void setKi(const Eigen::Matrix6d &Ki);
 
+  // @brief Set the derivative gain matrix
+  // @param Kd: 6x6 matrix containing the derivative gain matrix
   void setKd(const Eigen::Matrix6d &Kd);
 
+  // @brief Set the time step
+  // @param dt: Time step
   void setTimeStep(double dt);
 
 private:

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
@@ -9,24 +9,37 @@
 #include <std_msgs/msg/float64_multi_array.hpp>
 #include <vortex_msgs/msg/reference_filter.hpp>
 
+// @brief Class for the PID controller node
 class PIDControllerNode : public rclcpp::Node {
 public:
   explicit PIDControllerNode();
 
 private:
+  // @brief Callback function for the odometry topic
+  // @param msg: Odometry message containing the vehicle pose and velocity
   void odometry_callback(const nav_msgs::msg::Odometry::SharedPtr msg);
 
-  void guidance_callback(const vortex_msgs::msg::ReferenceFilter::SharedPtr msg);
-
+  // @brief Callback function for the proportional gain matrix
   void publish_tau();
 
+  // @brief Set the PID controller parameters
+  void set_pid_params();
+
+  // @brief Callback function for the proportional gain matrix
+  // @param msg: Float64MultiArray message containing the proportional gain matrix
   void kp_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg);
 
+  // @brief Callback function for the integral gain matrix
+  // @param msg: Float64MultiArray message containing the integral gain matrix
   void ki_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg);
 
+  // @brief Callback function for the derivative gain matrix
+  // @param msg: Float64MultiArray message containing the derivative gain matrix
   void kd_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg);
 
-  void set_pid_params();
+  // @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);
 
   PIDController pid_controller_;
 

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
@@ -8,26 +8,52 @@
 #include <tf2/LinearMath/Matrix3x3.h>
 #include <tf2/LinearMath/Quaternion.h>
 
-Eigen::Matrix6d float64multiarray_to_diagonal_matrix6d(const std_msgs::msg::Float64MultiArray &msg);
-
+// @brief Calculate the sine of an angle in degrees
+// @param angle: Angle in degrees
+// @return Smallest sine angle of the input
 double ssa(double angle);
 
-// Eigen::Vector6d apply_ssa(const Eigen::Vector7d &eta);
+// @brief Convert a Float64MultiArray message to a 6x6 diagonal matrix
+// @param msg: Float64MultiArray message containing 6 elements
+// @return 6x6 diagonal matrix with the elements of the message on the diagonal
+Eigen::Matrix6d float64multiarray_to_diagonal_matrix6d(const std_msgs::msg::Float64MultiArray &msg);
 
+// @brief Calculate the rotation matrix from a quaternion
+// @param q: Quaternion represented as a 4D vector [w, x, y, z]
+// @return 3x3 rotation matrix
 Eigen::Matrix3d calculate_R_quat(const Eigen::Vector4d &q);
 
-// Eigen::Matrix3d calculate_T(const Eigen::Vector7d &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
 Eigen::Matrix4x3d calculate_T_quat(const Eigen::Vector4d &q);
 
+// @brief Calculate the Jacobian matrix
+// @param eta: 7D vector containing the vehicle pose [x, y, z, w, x, y, z]
+// @return 7x6 Jacobian matrix
+Eigen::Matrix7x6d calculate_J(const Eigen::Vector7d &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
 Eigen::Matrix6x7d calculate_J_sudo_inv(const Eigen::Vector7d &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, z]
+// @param eta_d: 7D vector containing the desired vehicle pose [x, y, z, w, x, y, z]
+// @return 7D vector containing the error between the desired and actual vehicle pose
 Eigen::Vector7d error_eta(const Eigen::Vector7d &eta, const Eigen::Vector7d &eta_d);
 
-Eigen::Matrix7x6d calculate_J(const Eigen::Vector7d &eta);
-
+// @brief Calculate the anti-windup term
+// @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
 Eigen::Vector7d anti_windup(const double dt, const Eigen::Vector7d &error, const Eigen::Vector7d &integral);
 
+// @brief Limit the input to the PID controller
+// @param input: 6D vector containing the input to the PID controller [u, v, w, p, q, r]
+// @return 6D vector containing the limited input to the PID controller [u, v, w, p, q, r]
 Eigen::Vector6d limit_input(const Eigen::Vector6d &input);
 
 #endif
diff --git a/control/pid_controller_dp/src/pid_controller.cpp b/control/pid_controller_dp/src/pid_controller.cpp
@@ -10,11 +10,6 @@ PIDController::PIDController()
       dt(0.01) {}
 
 Eigen::Vector6d PIDController::calculate_tau(const Eigen::Vector7d &eta, const Eigen::Vector7d &eta_d, const Eigen::Vector6d &nu, const Eigen::Vector7d &eta_dot_d) {
-  // Eigen::Vector6d error = apply_ssa(eta - eta_d);
-
-  // Eigen::Matrix6d J = calculate_J(eta);
-
-  // Eigen::Matrix6d J_inv = J.inverse();
 
   Eigen::Vector7d error = error_eta(eta, eta_d);
 
@@ -26,13 +21,11 @@ Eigen::Vector6d PIDController::calculate_tau(const Eigen::Vector7d &eta, const E
 
   Eigen::Vector6d P = Kp_ * J_inv * error;
 
-  Eigen::Vector6d I = Ki_ * J_inv * integral_ ;
+  Eigen::Vector6d I = Ki_ * J_inv * integral_;
 
   Eigen::Vector6d D = Kd_ * error_nu;
-
-  Eigen::Vector6d tau = -(P + I + D);
 
-  tau = limit_input(tau);
+  Eigen::Vector6d tau = -limit_input((P + I + D));
 
   integral_ = anti_windup(dt, error, integral_);
 
@@ -41,20 +34,14 @@ Eigen::Vector6d PIDController::calculate_tau(const Eigen::Vector7d &eta, const E
 
 void PIDController::setKp(const Eigen::Matrix6d &Kp) {
   this->Kp_ = Kp;
-  std::cout << "Kp: " << std::endl
-            << Kp_ << std::endl;
 }
 
 void PIDController::setKi(const Eigen::Matrix6d &Ki) {
   this->Ki_ = Ki;
-  std::cout << "Ki: " << std::endl
-            << Ki_ << std::endl;
 }
 
 void PIDController::setKd(const Eigen::Matrix6d &Kd) {
   this->Kd_ = Kd;
-  std::cout << "Kd: " << std::endl
-            << Kd_ << std::endl;
 }
 
 void PIDController::setTimeStep(double dt) {

diff --git a/control/pid_controller_dp/src/pid_controller_ros.cpp b/control/pid_controller_dp/src/pid_controller_ros.cpp
@@ -27,16 +27,6 @@ void PIDControllerNode::odometry_callback(const nav_msgs::msg::Odometry::SharedP
   double y_ori = msg->pose.pose.orientation.y;
   double z_ori = msg->pose.pose.orientation.z;
 
-  // tf2::Quaternion quat;
-  //   quat.setX(msg->pose.pose.orientation.x);
-  //   quat.setY(msg->pose.pose.orientation.y);
-  //   quat.setZ(msg->pose.pose.orientation.z);
-  //   quat.setW(msg->pose.pose.orientation.w);
-
-  //   tf2::Matrix3x3 m(quat);
-  //   double roll, pitch, yaw;
-    //   m.getRPY(roll, pitch, yaw);
-
   eta_ << x, y, z, w_ori, x_ori, y_ori, z_ori;
 
   double u = msg->twist.twist.linear.x;