diff --git a/control/dp_adapt_backs_controller/CMakeLists.txt b/control/dp_adapt_backs_controller/CMakeLists.txt new file mode 100644 index 00000000..1ffc0d71 --- /dev/null +++ b/control/dp_adapt_backs_controller/CMakeLists.txt @@ -0,0 +1,50 @@ +cmake_minimum_required(VERSION 3.8) +project(dp_adapt_backs_controller) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(tf2 REQUIRED) +find_package(vortex_msgs REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) + +include_directories(include) + +add_executable(dp_adapt_backs_controller_node + src/dp_adapt_backs_controller.cpp + src/dp_adapt_backs_controller_node.cpp + src/dp_adapt_backs_controller_ros.cpp + src/dp_adapt_backs_controller_utils.cpp +) + +ament_target_dependencies(dp_adapt_backs_controller_node + rclcpp + rclcpp_lifecycle + geometry_msgs + nav_msgs + Eigen3 + tf2 + vortex_msgs +) + +install(TARGETS + dp_adapt_backs_controller_node + DESTINATION lib/${PROJECT_NAME}) + +install(DIRECTORY + config + launch + DESTINATION share/${PROJECT_NAME}/ +) + +ament_package() diff --git a/control/dp_adapt_backs_controller/README.md b/control/dp_adapt_backs_controller/README.md new file mode 100644 index 00000000..9378e2e1 --- /dev/null +++ b/control/dp_adapt_backs_controller/README.md @@ -0,0 +1,413 @@ +## DP Adaptive Backstepping Controller + +This package implements a dynamic positioning (DP) Adaptive backstepping controller for the orca AUV. It estimates the linear and nonlinear damping using adaptive parameters, and compensates for uncertainties and disturbances in real-time in the same manner as a state estimator. The proof for this is done using Lyapunov functions and stability requirements to ensure convergence and stability. + +### Overview +- Uses the backstepping control method for position and orientation control +- Includes adaptive terms to account for unmodeled dynamics and uncertainties. + +### Model for AUV + +```math +\dot{\eta} = J(\eta)\nu, +``` + +```math +M \dot{\nu} + C(\nu)\,\nu - F(\nu, \Theta^\star) = \tau + d^\star +``` + +- $\nu$: Body-fixed velocity vector +- $\eta$: Inertial position and orientation vector +- M: Constant mass-inertia matrix +- C($\eta$): Coriolis and centripetal terms +- J($\eta$): Transformation from body to inertial coordinates +- F($\nu$, $\Theta^\star$) = Y($\nu$) $\Theta^\star$: The damping assumed damping function (linear and nonlinear), where Y(*) describes the behaviour +- $d^\star$: Disturbance and uncertainties + +### File overview +1. **dp_adapt_backs_controller.cpp/hpp** + - The controller implementation + - Implements the main control input, sets gains, references, and mass parameters. + +2. **dp_adapt_backs_controller_utils.cpp/hpp** + - Provides utility functions: skew-symmetric matrix generation, quaternion-to-Euler, Jacobian and some functions needed for the adaptive functions. + +3. **dp_adapt_backs_controller_ros.cpp/hpp** + - ROS node wrapper for the controller, subscribing to pose, twist, killswitch, and reference topics. + - Publishes thrust commands. + +4. **dp_adapt_backs_controller_node.cpp** + - Entry point for the ROS executable. + +5. **adapt_params.yaml** + - Tunable controller parameters (K1, K2, adapt_gain, d_gain) + - Contains the mass matrix (6×6) and other physical parameters: + ```math + M = + \begin{bmatrix} + 30.0 & 0.0 & 0.0 & \cdots \\ + 0.0 & 30.0 & \cdots & \cdots \\ + 0.0 & \cdots & 30.0 & \cdots \\ + \cdots & \cdots & \cdots & \cdots + \end{bmatrix} + ``` + (partially shown for brevity) + This is also added into the orca.yaml file + +6. **CMakeLists.txt** + - Build configuration, ROS 2 dependencies, executable generation, and installation setup. + +### Tuning Parameters +- **K1** and **K2**: Gains for the backstepping control laws. +- **adapt_gain**: Adaptive gain for the linear and nonlinear damping. +- **d_gain**: Adaptive gain for the disturbances and uncertainties. +- **M** and **I_b**: Mass inertia matrix and rotational inertia. +- **m**: Vehicle mass. + +## Backstepping controller + +Using the Lyapunov proof we get the following functions for the backstepping controller: + +##### Backstepping variables + +```math +z_1 = \eta - \eta_d +``` + +```math +z_2 = \nu - \alpha +``` + +where the $\alpha$ is defined as the function that stabilizes the $z_1$ variable in the Lyapunov function system. + +##### Adaptive parameters + +```math +\tilde{\Theta} = \hat{\Theta} - \Theta^\star +``` + +```math +\tilde{d} = \hat{d} - d^\star +``` + +where: + +- $\Theta^\star$ and $d^\star$ are the actual parameters, +- $\hat{\Theta}$ and $\hat{d}$ are the estimated parameters, +- $\tilde{\Theta}$ and $\tilde{d}$ are the estimation errors. + +#### Proof of control law + +We define the Lyapunov function candidate (LFC) as: + +```math +V_1 = \frac{1}{2} z_1^\top z_1 +``` + +By fulfilling the criteriums of RUB, positive definite and \(V_1(0) = 0\), we need to do a derivation and ensure negative definite. + +Taking the derivative of the Lyapunov function candidate \(V_1\): + +```math +\dot{V}_1 = \frac{d}{dt} \left( \frac{1}{2} z_1^\top z_1 \right) +``` + +Using the chain rule, we get: + +```math +\dot{V}_1 = z_1^\top \dot{z}_1 +``` + +Substitute $\dot{z}_1$ with the dynamics of $z_1$: + +```math +\dot{z}_1 = \dot{\eta} - \dot{\eta}_d +``` + +and then inserting in the relation: + +```math +\dot{\eta} = J(\eta)\nu +``` + +```math +\dot{z}_1 = J(\eta)\nu - \dot{\eta}_d +``` + +and +```math +\nu = z_2 + \alpha +``` +Thus, the derivative of the Lyapunov function candidate is: +```math +\dot{V}_1 = z_1^\top J(\eta)(z_2 + \alpha) +``` + +We also assume that $\dot{\eta_d} = 0$ since we only get a desired position and orientation ($\eta$). + +We choose an $\alpha$ value to make $z_1$ be negative semi-definite. + +```math +\boxed{ +\alpha = -J(\eta)^{-1}(K_1 z_1) +} +``` + +```math +\dot{V}_1 = z_1^\top J(\eta)z_2 - z_1^\top K_1 z_1 < 0 +``` + +We will now define the rest of the Lyapunov function candidates, including the adaptive parameter. + +```math +V_2 = \frac{1}{2} z_2^\top M z_2 +``` + +For the second backstepping variable: + +```math +V_{\theta} = \frac{1}{2} \tilde{\Theta}^\top \Gamma^{-1}_{\theta} \tilde{\Theta} +``` + +```math +V_d = \frac{1}{2} \tilde{d}^\top \Gamma^{-1}_{d} \tilde{d} +``` + +Where the $\Gamma$ matrix is a diagonal gain matrix for tuning the adaptive rate of the parameters. The reasoning behind inserting the adaptive parameters is that we want to ensure that the error, marked by the tilde, converges towards zero. To achieve this, we need to ensure that it actually does this by accounting for it in the Lyapunov function and controller. + +Then we combine them like this: + +```math +V = V_1 + V_2 + V_{\theta} + V_d +``` + +and now we will analyze the derivative of this CLF, and ensure convergence for the whole function. + +```math +\dot{V} = \dot{V}_1 + z_2^\top M \dot{z}_2 + \tilde{\Theta}^\top \Gamma^{-1}_{\theta} \dot{\tilde{\Theta}} + \tilde{d}^\top \Gamma^{-1}_{d} \dot{\tilde{d}} +``` + +Before we write this out, we need to make some assumptions to make this more readable and easier to understand. + +For $\dot{\tilde{\Theta}} = \dot{\hat{\Theta}} - \dot{\Theta}^\star$ we assume that the actual value has no changes, assuming it's static, and therefore the derivative $\dot{\Theta}^\star = 0$ + +2. The same condition holds for $\dot{\tilde{d}}$ + +```math +\dot{V} = \dot{V}_1 + z_2^\top M (\dot{\nu} - \dot{\alpha}) + \tilde{\Theta}^\top \Gamma^{-1}_{\theta} \dot{\hat{\Theta}} + \tilde{d}^\top \Gamma^{-1}_{d} \dot{\hat{d}} +``` + +```math += \dot{V}_1 - z_2^\top M \dot{\alpha} + z_2^\top\tau - z_2^\top C(\nu)\,\nu + z_2^\top F(\nu, \Theta^\star) + z_2^\top d + \tilde{\Theta}^\top \Gamma^{-1}_{\theta} \dot{\hat{\Theta}} + \tilde{d}^\top \Gamma^{-1}_{d} \dot{\hat{d}} +``` + +```math += - z_1^\top K_1z_1 + z_2^\top J(\eta)z_1 - z_2^\top M \dot{\alpha} + z_2^\top \tau - z_2^\top C(\nu)\,\nu + z_2^\top Y(\nu) \Theta\star + z_2^\top d + \tilde{\Theta}^\top \Gamma^{-1}_{\theta} \dot{\hat{\Theta}} + \tilde{d}^\top \Gamma^{-1}_{d} \dot{\hat{d}} +``` + +Since we only know the estimate of the adaptive parameters, we can write the controller in two parts: + +```math +\tau = \tau_{controller} - F(\nu, \hat{\Theta}) - d = \tau_{controller} -Y(\nu) \hat{\Theta} - \hat{d} +``` +Important to notice is that we introduce the estimate (^) for the variables, not the actual value (*). +We insert this into the system and get: + +```math += - z_1^\top K_1 z_1 + z_2^\top J(\eta)z_1 - z_2^\top M \dot{\alpha} + z_2^\top \tau_{controller} - z_2^\top C(\nu)\,\nu - z_2^\top Y(\nu) (\hat{\Theta} - \Theta\star ) - z_2^\top (\hat{d} - d\star) + \tilde{\Theta}^\top \Gamma^{-1}_{\theta} \dot{\hat{\Theta}} + \tilde{d}^\top \Gamma^{-1}_{d} \dot{\hat{d}} +``` + +We look at the adaptive parameters a little more and try to simplify them as much as possible + +```math +\hat{\Theta} - \Theta^\star = \hat{\Theta} - (\hat{\Theta} - \tilde{\Theta}) = \tilde{\Theta} \newline +\hat{d} - d^\star = \hat{d} - (\hat{d} - \tilde{d}) = \tilde{d} +``` + +Now we have: +```math += - z_1^\top K_1 z_1 + z_2^\top J(\eta)z_1 - z_2^\top M \dot{\alpha} + z_2^\top \tau_{controller} - z_2^\top C(\nu)\,\nu + (-z_2^\top Y(\nu) \tilde{\Theta}+ \tilde{\Theta}^\top \Gamma^{-1}_{\theta} \dot{\hat{\Theta}}) + (-z_2^\top \tilde{d} + \tilde{d}^\top \Gamma^{-1}_{d} \dot{\hat{d}}) +``` +From this we can separate out the terms with the adaptive parameters. We can write them up as two separate equations: + +```math +-z_2^\top Y(\nu) \tilde{\Theta}+\tilde{\Theta}^\top \Gamma^{-1}_{\theta} \dot{\hat{\Theta}} = - \tilde{\Theta}^\top Y(\nu)^\top z_2 +\tilde{\Theta}^\top \Gamma^{-1}_{\theta} \dot{\hat{\Theta}}\newline +-z_2^\top \tilde{d} + \tilde{d}^\top \Gamma^{-1}_{d} \dot{\hat{d}} = - \tilde{d}^\top z_2+ \tilde{d}^\top \Gamma^{-1}_{d} \dot{\hat{d}} +``` + +We choose the \(\dot{\hat{\Theta}}\) and \(\dot{\hat{d}}\) to zero this out +```math +\boxed{ +\dot{\hat{\Theta}} = \Gamma_{\theta} Y(\nu)^\top z_2 +} +``` + +```math +\boxed{ +\dot{\hat{d}} = \Gamma_{d} z_2 +} +``` +Now that we have defined this we can insert and remove this from the equation, which should leave us with the normal system. An observation made during the construction of the controller was that the adaptive part and backstepping part is decoupled. Maybe this is dependent on the method used for the adaptive part, which is more of a MRAC type. Sadly, I don't have enough information about adaptive controllers to comment on this in detail. + +```math += - z_1^\top K_1 z_1 + z_2^\top J(\eta)z_1 - z_2^\top M \dot{\alpha} + z_2^\top \tau_{controller} - z_2^\top C(\nu)\,\nu +``` +```math +\tau_{controller} = -K_2 z_2 + M \dot{\alpha} + C(\nu)\nu - J(\eta)z_1 +``` + +Combined this gives us the control law: + +```math +\boxed{ +\tau = -K_2 z_2 + M \dot{\alpha} + C(\nu)\nu - J(\eta)z_1 - F(\nu, \hat{\Theta}) - \hat{d} +} +``` +### Controller gains + +The gains \(K_1\) and \(K_2\) are crucial for ensuring the stability and performance of the controller. + +**\(K_1\)**: This gain matrix is associated with the outer loop stability, which primarily deals with the position and orientation errors (\(z_1\)). Proper tuning of \(K_1\) ensures that the position and orientation errors converge to zero, thereby stabilizing the outer loop. + +```math +\begin{bmatrix} +k_{1,1} & 0 & 0 & 0 & 0 & 0 \\ +0 & k_{1,2} & 0 & 0 & 0 & 0 \\ +0 & 0 & k_{1,3} & 0 & 0 & 0 \\ +0 & 0 & 0 & k_{1,4} & 0 & 0 \\ +0 & 0 & 0 & 0 & k_{1,5} & 0 \\ +0 & 0 & 0 & 0 & 0 & k_{1,6} +\end{bmatrix} +``` + +**\(K_2\)**: This gain matrix is related to the inner loop stability, which handles the velocity errors (\(z_2\)). Tuning \(K_2\) appropriately ensures that the velocity errors are minimized, stabilizing the inner loop. + +```math +K_2 = +\begin{bmatrix} +k_{2,1} & 0 & 0 & 0 & 0 & 0 \\ +0 & k_{2,2} & 0 & 0 & 0 & 0 \\ +0 & 0 & k_{2,3} & 0 & 0 & 0 \\ +0 & 0 & 0 & k_{2,4} & 0 & 0 \\ +0 & 0 & 0 & 0 & k_{2,5} & 0 \\ +0 & 0 & 0 & 0 & 0 & k_{2,6} +\end{bmatrix} +``` + +Together, \(K_1\) and \(K_2\) work to ensure that both the position/orientation and velocity errors are driven to zero, guaranteeing the overall stability and performance of the adaptive backstepping controller. + +### Adaptive parameters and functions + +Now that we have the proof for the control law, we need to look at the adaptive functions and parameters. + +Since we wanted a damping (linear and nonlinear) \(Y(\nu)\) was chosen to be a 6x12 matrix with one linear and one nonlinear damping element: + +```math +Y(\nu) = +\begin{bmatrix} +\nu[0] & \nu[0] |\nu[0]| & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ +0 & 0 & \nu[1] & \nu[1] |\nu[1]| & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ +0 & 0 & 0 & 0 & \nu[2] & \nu[2] |\nu[2]| & 0 & 0 & 0 & 0 & 0 & 0 \\ +0 & 0 & 0 & 0 & 0 & 0 & \nu[3] & \nu[3] |\nu[3]| & 0 & 0 & 0 & 0 \\ +0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & \nu[4] & \nu[4] |\nu[4]| & 0 & 0 \\ +0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & \nu[5] & \nu[5] |\nu[5]| +\end{bmatrix} +``` + +while the \(\hat{\Theta}\) will be a 12x1 vector: + +```math +\hat{\Theta} = +\begin{bmatrix} +\alpha_1 & \beta_1 & \alpha_2 & \beta_2 & \alpha_3 & \beta_3 & \alpha_4 & \beta_4 & \alpha_5 & \beta_5 & \alpha_6 & \beta_6 +\end{bmatrix}^\top +``` + +The \(\Gamma_{\Theta}\) will therefore be a 12x12 diagonal vector with the gains for the linear and nonlinear damping. + +The disturbance \(d\) will have a 6x1 vector for the disturbance and uncertainty estimates: + +```math +\hat{d} = +\begin{bmatrix} +d_1 & d_2 & d_3 & d_4 & d_5 & d_6 +\end{bmatrix}^\top +``` + +#### Important information + +The implementation of the controller requires \(\dot{\alpha}\) which is not that simple to calculate, given that this requires the derivative of the \(J(\eta)\) matrix. + +```math +\dot{\alpha} +\;=\; +-\;J(\eta)^{-1}\,\dot{J}(\eta)\,J(\eta)^{-1}\bigl[- k_1\,(\eta - \eta_d)\bigr] +\;+\; +-J(\eta)^{-1} k_1\,\dot{\eta} +``` + +This was manually derived, and \(J(\eta)\) was also derived manually but here we can use a trick: + +```math +J(\eta) = +\begin{bmatrix} +R & 0_{3x3} \\ +0_{3x3} & T +\end{bmatrix} +``` + +where: +- \(R\) is the rotation matrix representing the orientation of the AUV. +- \(T\) is the transformation matrix for the translational components. +- \(0_{3x3}\) is a 3x3 zero matrix. + +The derivative of \(J(\eta)\) can be expressed in terms of the derivatives of \(R\) and \(T\): + +```math +\dot{J}(\eta) = +\begin{bmatrix} +\dot{R} & 0_{3x3} \\ +0_{3x3} & \dot{T} +\end{bmatrix} +``` + +The trick is that \(\dot{R}\) can be written as: + +```math +\dot{R} = R \cdot S \cdot r +``` + +where: +- \(R\) is the rotation matrix calculated from \(\eta\). +- \(S\) is the skew-symmetric matrix of the vector \((0, 0, 1)\). +- \(r\) is the z-component of the angular speed from \(\nu\). + +For the \(T\) the expression needs to be manually calculated and this is not nice to look at: + +```math +\dot{T}(\eta, \nu) = +\begin{bmatrix} +0 & \cos(\phi) \tan(\theta) \nu_x + \sin(\phi) \sec^2(\theta) \nu_y & -\sin(\phi) \tan(\theta) \nu_x + \cos(\phi) \sec^2(\theta) \nu_y \\ +0 & -\sin(\phi) \nu_x & -\cos(\phi) \nu_x \\ +0 & \cos(\phi) / \cos(\theta) \nu_x + \sin(\phi) \sin(\theta) / \cos^2(\theta) \nu_y & -\sin(\phi) / \cos(\theta) \nu_x + \cos(\phi) \sin(\theta) / \cos^2(\theta) \nu_y +\end{bmatrix} +``` + +This can be calculated by the chain rule and partial differentiation on \(\phi, \theta, \psi\): + +```math +\dot{T}(\eta, \nu) = \frac{\partial T}{\partial \phi} \dot{\phi} + \frac{\partial T}{\partial \theta} \dot{\theta} + \frac{\partial T}{\partial \psi} \dot{\psi} +``` + +Here we could potentially use methods for estimating the derivative, but one of the benefits of this calculation is that we get good and accurate values for the rotation and translation derivatives. Instead of introducing more uncertainties as we try to remove this in the system with the adaptive part. + +## Launch + +To run the controller, use the ROS 2 launch file: +```bash +ros2 launch dp_adapt_backs_controller dp_adapt_backs_controller.launch.py +``` + +It's important to `colcon build` and `source install/setup.bash` before launching, or else it will not be launchable. + +This will load the parameters from the provided YAML, start the node, and begin control operation. diff --git a/control/dp_adapt_backs_controller/config/adapt_params.yaml b/control/dp_adapt_backs_controller/config/adapt_params.yaml new file mode 100644 index 00000000..2555bfd5 --- /dev/null +++ b/control/dp_adapt_backs_controller/config/adapt_params.yaml @@ -0,0 +1,17 @@ +dp_adapt_backs_controller_node: + ros__parameters: + software_kill_switch_topic: /softwareKillSwitch + software_operation_mode_topic: /softwareOperationMode + control_topic: /thrust/wrench_input + dp_reference_topic: /dp/reference + odom_topic: /orca/odom + pose_topic: /dvl/pose + twist_topic: /dvl/twist + K1 : [20.5, 15.5, 20.5, 1.2, 6.0, 2.5] + K2 : [30.5, 25.5, 30.5, 2.6, 10.0, 6.5] + r_b_bg : [0.01, 0.0, 0.02] + adapt_gain : [0.8, 0.8, 0.8, 0.8, 0.8, 0.8, 0.8, 0.8, 0.8, 0.8, 0.8, 0.8] + d_gain : [0.3, 0.3, 0.45, 0.2, 0.7, 0.6] + inertia_matrix : [0.68, 3.32, 3.34] + mass_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] + m : 30.0 diff --git a/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller.hpp b/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller.hpp new file mode 100644 index 00000000..9d0ba506 --- /dev/null +++ b/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller.hpp @@ -0,0 +1,74 @@ +#ifndef DP_ADAPT_BACKS_CONTROLLER_HPP +#define DP_ADAPT_BACKS_CONTROLLER_HPP + +#include "dp_adapt_backs_controller/typedefs.hpp" +#include "typedefs.hpp" + +class DPAdaptBacksController { + public: + explicit DPAdaptBacksController(); + + // @brief Calculate thecontrol input tau + // @param eta: 6D vector containing the vehicle pose [x, y, z, roll, pitch, + // yaw] + // @param eta_d: 6D vector containing the desired vehicle pose [x, y, z, + // roll, pitch, yaw] + // @param nu: 6D vector containing the vehicle velocity [u, v, w, p, q, r] + // @return 6D vector containing the control input tau [X, Y, Z, K, M, N] + dp_types::Vector6d calculate_tau(const dp_types::Eta& eta, + const dp_types::Eta& eta_d, + const dp_types::Nu& nu); + + // @brief Set the K1 matrix + // @param K1: 6D vector containing the K1 matrix + void set_k1(const dp_types::Vector6d& K1); + + // @brief Set the K2 matrix + // @param K2: 6D vector containing the K2 matrix + void set_k2(const dp_types::Vector6d& K2); + + // @brief Set the r_b_bg vector + // @param r_b_bg: 3D vector containing the r_b_bg vector + void set_rbg(const dp_types::Vector3d& r_b_bg); + + // @brief Set the adaptive parameter matrix + // @param adap_param: 12D vector containing the adaptive parameter matrix + void set_adap_param(const dp_types::Vector12d& adap_param); + + // @brief Set the d gain matrix + // @param d_gain: 6D vector containing the d gain matrix + void set_d_gain(const dp_types::Vector6d& d_gain); + + // @brief Set the inertia matrix + // @param I_b: 3D vector containing the inertia matrix + void set_inertia_matrix(const dp_types::Vector3d& I_b); + + // @brief Set the mass inertia matrix + // @param m: Mass inertia matrix + void set_mass_inertia_matrix(const dp_types::Matrix6d& M); + + // @brief Set the mass + // @param m: Mass + void set_m(double m); + + // @brief Set the time step + // @param dt: Time step + void set_timeStep(double dt); + + // @brief Reset the adaptive parameter, when inactive + void reset_adap_param(); + + private: + dp_types::Matrix6d K1_; + dp_types::Matrix6d K2_; + dp_types::Vector3d r_b_bg_; + dp_types::Matrix12d adapt_gain_; + dp_types::Matrix6d d_gain_; + dp_types::Vector12d adap_param_; + dp_types::Vector6d d_est_; + dp_types::Matrix3d I_b_; + dp_types::Matrix6d M_; + double m_; + double dt_; +}; +#endif diff --git a/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_ros.hpp b/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_ros.hpp new file mode 100644 index 00000000..8b92fa98 --- /dev/null +++ b/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_ros.hpp @@ -0,0 +1,131 @@ +#ifndef DP_ADAPT_BACKS_CONTROLLER_ROS_HPP +#define DP_ADAPT_BACKS_CONTROLLER_ROS_HPP + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "dp_adapt_backs_controller/dp_adapt_backs_controller.hpp" +#include "dp_adapt_backs_controller/typedefs.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +using LifecycleCallbackReturn = + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +// @brief struct for the topics loaded from the parameter server +struct Topics { + std::string dp_reference_topic; + std::string pose_topic; + std::string twist_topic; + std::string software_kill_switch_topic; + std::string software_operation_mode_topic; + std::string control_topic; +}; + +// @brief Class for the DP Adaptive Backstepping controller node +class DPAdaptBacksControllerNode : public rclcpp_lifecycle::LifecycleNode { + public: + explicit DPAdaptBacksControllerNode(); + + // @brief function for configuring the node + // @param previous_state: lifecycle::State previous state of the node + LifecycleCallbackReturn on_configure( + const rclcpp_lifecycle::State& previous_state); + + // @brief function for activating the node + // @param previous_state: lifecycle::State previous state of the node + LifecycleCallbackReturn on_activate( + const rclcpp_lifecycle::State& previous_state); + + // @brief function for cleaning up after the node + // @param previous_state: lifecycle::State previous state of the node + LifecycleCallbackReturn on_cleanup( + const rclcpp_lifecycle::State& previous_state); + + // @brief function for deactivating the node + // @param previous_state: lifecycle::State previous state of the node + LifecycleCallbackReturn on_deactivate( + const rclcpp_lifecycle::State& previous_state); + + // @brief function for shutting down the node + // @param previous_state: lifecycle::State previous state of the node + LifecycleCallbackReturn on_shutdown( + const rclcpp_lifecycle::State& previous_state); + + private: + // @brief Callback function for the killswitch topic + // @param msg: Bool message containing the killswitch status + void killswitch_callback(const std_msgs::msg::Bool::SharedPtr msg); + + // @brief Callback function for the software mode topic + // @param msg: String message containing the software mode + void software_mode_callback(const std_msgs::msg::String::SharedPtr msg); + + // @brief Callback function for the pose topic + // @param msg: PoseWithCovarianceStamped message containing the AUV pose + void pose_callback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); + + // @brief Callback function for the twist topic + // @param msg: TwistWithCovarianceStamped message containing the AUV speed + void twist_callback( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); + + // @brief Callback function for the control input tau publish + void publish_tau(); + + // @brief set the DP Adaptive Backstepping controller parameters + void set_adap_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); + + // @brief get the topics from the parameter server + void get_topics(Topics& topics); + + DPAdaptBacksController dp_adapt_backs_controller_; + + rclcpp::Subscription::SharedPtr killswitch_sub_; + + rclcpp::Subscription::SharedPtr software_mode_sub_; + + rclcpp::Subscription< + geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_; + + rclcpp::Subscription< + geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr twist_sub_; + + rclcpp::Subscription::SharedPtr + guidance_sub_; + + rclcpp_lifecycle::LifecyclePublisher::SharedPtr + tau_pub_; + + rclcpp::TimerBase::SharedPtr tau_pub_timer_; + + std::chrono::milliseconds time_step_; + + dp_types::Eta eta_; + + dp_types::Eta eta_d_; + + dp_types::Nu nu_; + + bool killswitch_on_; + + std::string software_mode_; +}; + +#endif diff --git a/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_utils.hpp b/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_utils.hpp new file mode 100644 index 00000000..147bafaa --- /dev/null +++ b/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_utils.hpp @@ -0,0 +1,95 @@ +#ifndef DP_ADAPT_BACKS_CONTROLLER_UTILS_HPP +#define DP_ADAPT_BACKS_CONTROLLER_UTILS_HPP + +#include +#include +#include +#include +#include +#include "dp_adapt_backs_controller/typedefs.hpp" +#include "typedefs.hpp" + +// @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); + +// @brief construct a skew symmetric matrix from a 3D vector +// @param vec: 3D vector +// @return 3x3 skew symmetric matrix +dp_types::Matrix3d skew_symmetric(const dp_types::Vector3d& vec); + +// @brief Calculate the error in eta +// @param eta: 6D vector containing the vehicle pose [x, y, z, roll, pitch, yaw] +// @param eta_d: 6D vector containing the desired vehicle pose [x, y, z, roll, +// pitch, yaw] +// @return 6D vector containing the error between the desired and actual vehicle +// pose +dp_types::Eta error_eta(const dp_types::Eta& eta, const dp_types::Eta& eta_d); + +// @brief Quaternion to Euler angle conversion +// @param q: Quaternion represented as a 4D vector [w, x, y, z] +// @return 3D vector containing the Euler angles [roll, pitch, yaw] +dp_types::Vector3d quat_to_euler(const double w, + const double x, + const double y, + const double z); + +// @brief Calculate the rotation matrix from a Euler angle +// @param eta: 6D vector containing the vehicle pose [x, y, z, roll, pitch, yaw] +// @return 3x3 rotation matrix +dp_types::Matrix3d calculate_R(const dp_types::Eta& eta); + +// @brief Calculate the transformation matrix from a Euler angle +// @param eta: 6D vector containing the vehicle pose [x, y, z, roll, pitch, yaw] +// @return 3x3 transformation matrix +dp_types::Matrix3d calculate_T(const dp_types::Eta& eta); + +// @brief Calculate the derivative of the rotation matrix +// @param eta: 6D vector containing the vehicle pose [x, y, z, roll, pitch, yaw] +// @param nu: 6D vector containing the vehicle velocity [u, v, w, p, q, r] +// @return 3x3 derivative of the rotation matrix +dp_types::Matrix3d calculate_R_dot(const dp_types::Eta& eta, + const dp_types::Nu& nu); + +// @brief Calculate the derivative of the transformation matrix +// @param eta: 6D vector containing the vehicle pose [x, y, z, roll, pitch, yaw] +// @param nu: 6D vector containing the vehicle velocity [u, v, w, p, q, r] +// @return 3x3 derivative of the transformation matrix +dp_types::Matrix3d calculate_T_dot(const dp_types::Eta& eta, + const dp_types::Nu& nu); + +// @brief Calculate the Jacobian matrix +// @param eta: 6D vector containing the vehicle pose [x, y, z, roll, pitch, yaw] +// @return 6x6 Jacobian matrix +dp_types::Matrix6d calculate_J(const dp_types::Eta& eta); + +// @brief Calculate the pseudo-inverse of the Jacobian matrix +// @param eta: 6D vector containing the vehicle pose [x, y, z, roll, pitch, yaw] +// @return 6x6 pseudo-inverse Jacobian matrix +dp_types::Matrix6d calculate_J_sudo_inv(const dp_types::Eta& eta); + +// @brief calculate the derivative of the Jacobian matrix +// @param eta: 6D vector containing the vehicle pose [x, y, z, roll, pitch, yaw] +// @param nu: 6D vector containing the vehicle velocity [u, v, w, p, q, r] +// @return 6x6 derivative of the Jacobian matrix +dp_types::Matrix6d calculate_J_dot(const dp_types::Eta& eta, + const dp_types::Nu& nu); + +// @brief Calculate the coriolis matrix +// @param m: mass of the vehicle +// @param r_b_bg: 3D vector of the body frame to the center of gravity +// @param nu_2: 3D vector containing angular velocity of the vehicle +// @param I_b : 3D matrix containing the inertia matrix +// @return 6x6 coriolis matrix +dp_types::Matrix6d calculate_C(double m, + const dp_types::Vector3d& r_b_bg, + const dp_types::Nu& nu_2, + const dp_types::Matrix3d& I_b); + +// @brief Calculate the damping matrix for the adaptive backstepping controller +// @param nu: 6D vector containing the vehicle velocity [u, v, w, p, q, r] +// @return 6x6 damping matrix +dp_types::Matrix6x12d calculate_Y_v(const dp_types::Nu& nu); + +#endif diff --git a/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/typedefs.hpp b/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/typedefs.hpp new file mode 100644 index 00000000..b58bfc53 --- /dev/null +++ b/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/typedefs.hpp @@ -0,0 +1,57 @@ +/** + * @file typedefs.hpp + * @brief Contains the typedef and structs for the controller. + */ + +#ifndef VORTEX_DP_ADAPT_BACKSTEPPING_CONTROLLER_TYPEDEFS_H +#define VORTEX_DP_ADAPT_BACKSTEPPING_CONTROLLER_TYPEDEFS_H + +#include + +namespace dp_types { +typedef Eigen::Matrix Vector3d; +typedef Eigen::Matrix Vector6d; +typedef Eigen::Matrix Vector12d; +typedef Eigen::Matrix Matrix6d; +typedef Eigen::Matrix Matrix3d; +typedef Eigen::Matrix Matrix6x12d; +typedef Eigen::Matrix Matrix12x6d; +typedef Eigen::Matrix Matrix12d; + +struct Eta { + dp_types::Vector3d pos = dp_types::Vector3d::Zero(); + dp_types::Vector3d ori = dp_types::Vector3d::Zero(); + + dp_types::Vector6d as_vector() const { + dp_types::Vector6d vec; + vec << pos, ori; + return vec; + } +}; + +struct Nu { + dp_types::Vector3d linear_speed = dp_types::Vector3d::Zero(); + dp_types::Vector3d angular_speed = dp_types::Vector3d::Zero(); + + dp_types::Vector6d as_vector() const { + dp_types::Vector6d vec; + vec << linear_speed, angular_speed; + return vec; + } +}; + +struct J_matrix { + dp_types::Matrix3d R = dp_types::Matrix3d::Identity(); + dp_types::Matrix3d T = dp_types::Matrix3d::Identity(); + + dp_types::Matrix6d as_matrix() const { + dp_types::Matrix6d mat = dp_types::Matrix6d::Zero(); + mat.block<3, 3>(0, 0) = R; + mat.block<3, 3>(3, 3) = T; + return mat; + } +}; + +} // namespace dp_types + +#endif diff --git a/control/dp_adapt_backs_controller/launch/dp_adapt_backs_controller.launch.py b/control/dp_adapt_backs_controller/launch/dp_adapt_backs_controller.launch.py new file mode 100644 index 00000000..dc4c16b6 --- /dev/null +++ b/control/dp_adapt_backs_controller/launch/dp_adapt_backs_controller.launch.py @@ -0,0 +1,32 @@ +from os import path + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode + +adapt_params = path.join( + get_package_share_directory("dp_adapt_backs_controller"), + "config", + "adapt_params.yaml", +) +orca_params = path.join( + get_package_share_directory("auv_setup"), + "config", + "robots", + "orca.yaml", +) + + +def generate_launch_description(): + dp_adapt_backs_controller_node = LifecycleNode( + package="dp_adapt_backs_controller", + executable="dp_adapt_backs_controller_node", + name="dp_adapt_backs_controller_node", + namespace="", + parameters=[ + adapt_params, + orca_params, + ], + output="screen", + ) + return LaunchDescription([dp_adapt_backs_controller_node]) diff --git a/control/dp_adapt_backs_controller/package.xml b/control/dp_adapt_backs_controller/package.xml new file mode 100644 index 00000000..0223f956 --- /dev/null +++ b/control/dp_adapt_backs_controller/package.xml @@ -0,0 +1,23 @@ + + + + dp_adapt_backs_controller + 1.0.0 + Adaptive backstepping controller for DP + talhanc + MIT + + ament_cmake + + rclcpp + rclcpp_lifecycle + geometry_msgs + nav_msgs + eigen + tf2 + vortex_msgs + + + ament_cmake + + diff --git a/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller.cpp b/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller.cpp new file mode 100644 index 00000000..d9eff934 --- /dev/null +++ b/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller.cpp @@ -0,0 +1,109 @@ +#include "dp_adapt_backs_controller/dp_adapt_backs_controller.hpp" +#include +#include +#include "dp_adapt_backs_controller/dp_adapt_backs_controller_utils.hpp" +#include "dp_adapt_backs_controller/typedefs.hpp" + +DPAdaptBacksController::DPAdaptBacksController() + : K1_(dp_types::Matrix6d::Identity()), + K2_(dp_types::Matrix6d::Identity()), + r_b_bg_(dp_types::Vector3d::Zero()), + adapt_gain_(dp_types::Matrix12d::Identity()), + d_gain_(dp_types::Matrix6d::Zero()), + adap_param_(dp_types::Vector12d::Zero()), + d_est_(dp_types::Vector6d::Zero()), + I_b_(dp_types::Matrix3d::Identity()), + M_(dp_types::Matrix6d::Identity()), + m_(0), + dt_(0.04) {} + +dp_types::Vector6d DPAdaptBacksController::calculate_tau( + const dp_types::Eta& eta, + const dp_types::Eta& eta_d, + const dp_types::Nu& nu) { + dp_types::Eta error = error_eta(eta, eta_d); + + dp_types::Matrix6d C = calculate_C(m_, r_b_bg_, nu, I_b_); + + dp_types::Matrix6d J = calculate_J(error); + + dp_types::Matrix6d J_inv = calculate_J_sudo_inv(error); + + dp_types::Matrix6d J_dot = calculate_J_dot(error, nu); + + dp_types::Vector6d alpha = -J_inv * K1_ * error.as_vector(); + + dp_types::Vector6d z_1 = error.as_vector(); + + dp_types::Vector6d z_2 = nu.as_vector() - alpha; + + dp_types::Vector6d alpha_dot = + ((J_inv * J_dot * J_inv) * K1_ * z_1) - (J_inv * K1_ * nu.as_vector()); + + dp_types::Matrix6x12d Y_v = calculate_Y_v(nu); + + dp_types::Vector12d adapt_param_dot = adapt_gain_ * Y_v.transpose() * z_2; + + dp_types::Vector6d d_est_dot = d_gain_ * z_2; + + dp_types::Vector6d F_est = Y_v * adap_param_; + + dp_types::Vector6d tau = (M_ * alpha_dot) + (C * nu.as_vector()) - + (J.transpose() * z_1) - (K2_ * z_2) - F_est - + d_est_; + + tau = tau.cwiseMax(-80.0).cwiseMin(80.0); + + adap_param_ = adap_param_ + adapt_param_dot * dt_; + + d_est_ = d_est_ + d_est_dot * dt_; + + adap_param_ = adap_param_.cwiseMax(-80.0).cwiseMin(80.0); + + d_est_ = d_est_.cwiseMax(-80.0).cwiseMin(80.0); + + return tau; +} + +void DPAdaptBacksController::set_k1(const dp_types::Vector6d& K1) { + this->K1_ = K1.asDiagonal().toDenseMatrix(); +} + +void DPAdaptBacksController::set_k2(const dp_types::Vector6d& K2) { + this->K2_ = K2.asDiagonal().toDenseMatrix(); +} + +void DPAdaptBacksController::set_rbg(const dp_types::Vector3d& r_b_bg) { + this->r_b_bg_ = r_b_bg; +} + +void DPAdaptBacksController::set_adap_param( + const dp_types::Vector12d& adapt_gain) { + this->adapt_gain_ = adapt_gain.asDiagonal().toDenseMatrix(); +} + +void DPAdaptBacksController::set_d_gain(const dp_types::Vector6d& d_gain) { + this->d_gain_ = d_gain.asDiagonal().toDenseMatrix(); +} + +void DPAdaptBacksController::set_inertia_matrix(const dp_types::Vector3d& I_b) { + this->I_b_ = I_b.asDiagonal().toDenseMatrix(); +} + +void DPAdaptBacksController::set_mass_inertia_matrix( + const dp_types::Matrix6d& M) { + this->M_ = M; +} + +void DPAdaptBacksController::set_m(double m) { + this->m_ = m; +} + +void DPAdaptBacksController::set_timeStep(double dt) { + this->dt_ = dt; +} + +void DPAdaptBacksController::reset_adap_param() { + this->adap_param_ = dp_types::Vector12d::Zero(); + this->d_est_ = dp_types::Vector6d::Zero(); +} diff --git a/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_node.cpp b/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_node.cpp new file mode 100644 index 00000000..49afc289 --- /dev/null +++ b/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_node.cpp @@ -0,0 +1,110 @@ +#include "dp_adapt_backs_controller/dp_adapt_backs_controller_ros.hpp" + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), + R"( + ,ooo888888888888888oooo, + o8888YYYYYY77iiiiooo8888888o + 8888YYYY77iiYY8888888888888888 + [88YYY77iiY88888888888888888888] + 88YY7iYY888888888888888888888888 + [88YYi 88888888888888888888888888] + i88Yo8888888888888888888888888888i + i] ^^^88888888^^^ o [i + oi8 i o8o i 8io + ,77788o ^^ ,oooo8888888ooo, ^ o88777, + 7777788888888888888888888888888888877777 + 77777888888888888888888888888888877777 + 77777788888888^7777777^8888888777777 + ,oooo888 ooo 88888778888^7777ooooo7777^8887788888 ,o88^^^^888oo + o8888777788[];78 88888888888888888888888888888888888887 7;8^ 888888888oo^88 + o888888iii788 ]; o 78888887788788888^;;^888878877888887 o7;[]88888888888888o + 88888877 ii78[]8;7o 7888878^ ^8788^;;;;;;^878^ ^878877 o7;8 ]878888888888888 + [88888888887888 87;7oo 777888o8888^;ii;;ii;^888o87777 oo7;7[]8778888888888888 + 88888888888888[]87;777oooooooooooooo888888oooooooooooo77;78]88877i78888888888 + o88888888888888 877;7877788777iiiiiii;;;;;iiiiiiiii77877i;78] 88877i;788888888 + 88^;iiii^88888 o87;78888888888888888888888888888888888887;778] 88877ii;7788888 +;;;iiiii7iiii^ 87;;888888888888888888888888888888888888887;778] 888777ii;78888 +;iiiii7iiiii7iiii77;i88888888888888888888i7888888888888888877;77i 888877777ii78 +iiiiiiiiiii7iiii7iii;;;i7778888888888888ii7788888888888777i;;;;iiii 88888888888 +i;iiiiiiiiiiii7iiiiiiiiiiiiiiiiiiiiiiiiii8877iiiiiiiiiiiiiiiiiii877 88888 +ii;;iiiiiiiiiiiiii;;;ii^^^;;;ii77777788888888888887777iii;; 77777 78 +77iii;;iiiiiiiiii;;;ii;;;;;;;;;^^^^8888888888888888888777ii;; ii7 ;i78 +^ii;8iiiiiiii ';;;;ii;;;;;;;;;;;;;;;;;;^^oo ooooo^^^88888888;;i7 7;788 +o ^;;^^88888^ 'i;;;;;;;;;;;;;;;;;;;;;;;;;;;^^^88oo^^^^888ii7 7;i788 +88ooooooooo ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 788oo^;; 7;i888 +887ii8788888 ;;;;;;;ii;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;^87 7;788 +887i8788888^ ;;;;;;;ii;;;;;;;oo;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;,,, ;;888 +87787888888 ;;;;;;;ii;;;;;;;888888oo;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;,,;i788 +87i8788888^ ';;;ii;;;;;;;8888878777ii8ooo;;;;;;;;;;;;;;;;;;;;;;;;;;i788 7 +77i8788888 ioo;;;;;;oo^^ooooo ^7i88^ooooo;;;;;;;;;;;;;;;;;;;;i7888 78 +7i87788888o 7;ii788887i7;7;788888ooooo7888888ooo;;;;;;;;;;;;;;oo ^^^ 78 +i; 7888888^ 8888^o;ii778877;7;7888887;;7;7788878;878;; ;;;;;;;i78888o ^ +i8 788888 [88888^^ ooo ^^^^^;;77888^^^^;;7787^^^^ ^^;;;; iiii;i78888888 +^8 7888^ [87888 87 ^877i;i8ooooooo8778oooooo888877ii; iiiiiiii788888888 + ^^^ [7i888 87;; ^8i;;i7888888888888888887888888 i7iiiiiii88888^^ + 87;88 o87;;;;o 87i;;;78888788888888888888^^ o 8ii7iiiiii;; + 87;i8 877;77888o ^877;;;i7888888888888^^ 7888 78iii7iii7iiii + ^87; 877;778888887o 877;;88888888888^ 7ii7888 788oiiiiiiiii + ^ 877;7 7888888887 877i;;8888887ii 87i78888 7888888888 + [87;;7 78888888887 87i;;888887i 87ii78888 7888888888] + 877;7 7788888888887 887i;887i^ 87ii788888 78888888888 + 87;i8 788888888888887 887ii;;^ 87ii7888888 78888888888 + [87;i8 7888888888888887 ^^^^ 87ii77888888 78888888888 + 87;;78 7888888888888887ii 87i78888888 778888888888 + 87;788 7888888888888887i] 87i78888888 788888888888 + [87;88 778888888888888887 7ii78888888 788888888888 + 87;;88 78888888888888887] ii778888888 78888888888] + 7;;788 7888888888888888] i7888888888 78888888888' + 7;;788 7888888888888888 'i788888888 78888888888 + 7;i788 788888888888888] 788888888 77888888888] + '7;788 778888888888888] [788888888 78888888888' + ';77888 78888888888888 8888888888 7888888888] + 778888 78888888888888 8888888888 7888888888] + 78888 7888888888888] [8888888888 7888888888 + 7888 788888888888] 88888888888 788888888] + 778 78888888888] ]888888888 778888888] + oooooo ^88888^ ^88888^^^^^^^^8888] + 87;78888ooooooo8o ,oooooo oo888oooooo + [877;i77888888888] [;78887i8888878i7888; + ^877;;ii7888ii788 ;i777;7788887787;778; + ^87777;;;iiii777 ;77^^^^^^^^^^^^^^^^;; + ^^^^^^^^^ii7] ^ o88888888877iiioo + 77777o [88777777iiiiii;;778 + 77777iii 8877iiiii;;;77888888] + 77iiii;8 [77ii;778 788888888888 + 7iii;;88 iii;78888 778888888888 + 77i;78888] ;;;;i88888 78888888888 + ,7;78888888 [;;i788888 7888888888] + i;788888888 ;i7888888 7888888888 + ;788888888] i77888888 788888888] + ';88888888' [77888888 788888888] + [[8ooo88] 78888888 788888888 + [88888] 78888888 788888888 + ^^^ [7888888 77888888] + 88888888 7888887 + 77888888 7888887 + ;i88888 788888i + ,;;78888 788877i7 + ,7;;i;777777i7i;;7 + 87778^^^ ^^^^87778 + ^^^^ o777777o ^^^ + o77777iiiiii7777o + 7777iiii88888iii777 + ;;;i7778888888877ii;; + Imperial Stormtrooper [i77888888^^^^8888877i] + (Standard Shock Trooper) 77888^oooo8888oooo^8887] + [788888888888888888888888] + 88888888888888888888888888 + ]8888888^iiiiiiiii^888888] + Talha and Cyprian iiiiiiiiiiiiiiiiiiiiii + ^^^^^^^^^^^^^ + )"); + + auto node = std::make_shared(); + rclcpp::spin(node->get_node_base_interface()); + + rclcpp::shutdown(); + return 0; +} diff --git a/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_ros.cpp b/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_ros.cpp new file mode 100644 index 00000000..a0b55959 --- /dev/null +++ b/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_ros.cpp @@ -0,0 +1,243 @@ +#include "dp_adapt_backs_controller/dp_adapt_backs_controller_ros.hpp" +#include +#include +#include "dp_adapt_backs_controller/dp_adapt_backs_controller_utils.hpp" +#include "dp_adapt_backs_controller/typedefs.hpp" + +DPAdaptBacksControllerNode::DPAdaptBacksControllerNode() + : LifecycleNode("dp_adapt_backs_controller_node") { + set_adap_params(); +} + +LifecycleCallbackReturn DPAdaptBacksControllerNode::on_configure( + const rclcpp_lifecycle::State& previous_state) { + (void)previous_state; + RCLCPP_INFO(this->get_logger(), "configuration step"); + + rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; + auto qos_sensor_data = rclcpp::QoS( + rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); + + auto best_effort = + rclcpp::QoS(rclcpp::KeepLast(1)).best_effort().durability_volatile(); + + auto reliable = + rclcpp::QoS(rclcpp::KeepLast(5)).reliable().durability_volatile(); + + Topics topics; + get_topics(topics); + + guidance_sub_ = + this->create_subscription( + topics.dp_reference_topic, qos_sensor_data, + std::bind(&DPAdaptBacksControllerNode::guidance_callback, this, + std::placeholders::_1)); + + pose_sub_ = this->create_subscription< + geometry_msgs::msg::PoseWithCovarianceStamped>( + topics.pose_topic, best_effort, + std::bind(&DPAdaptBacksControllerNode::pose_callback, this, + std::placeholders::_1)); + + twist_sub_ = this->create_subscription< + geometry_msgs::msg::TwistWithCovarianceStamped>( + topics.twist_topic, best_effort, + std::bind(&DPAdaptBacksControllerNode::twist_callback, this, + std::placeholders::_1)); + + killswitch_sub_ = this->create_subscription( + topics.software_kill_switch_topic, best_effort, + std::bind(&DPAdaptBacksControllerNode::killswitch_callback, this, + std::placeholders::_1)); + + software_mode_sub_ = this->create_subscription( + topics.software_operation_mode_topic, best_effort, + std::bind(&DPAdaptBacksControllerNode::software_mode_callback, this, + std::placeholders::_1)); + + time_step_ = std::chrono::milliseconds(10); + tau_pub_ = this->create_publisher( + topics.control_topic, reliable); + tau_pub_timer_ = this->create_wall_timer( + time_step_, std::bind(&DPAdaptBacksControllerNode::publish_tau, this)); + tau_pub_timer_->cancel(); + + return LifecycleCallbackReturn::SUCCESS; +} + +LifecycleCallbackReturn DPAdaptBacksControllerNode::on_activate( + const rclcpp_lifecycle::State& previous_state) { + (void)previous_state; + RCLCPP_INFO(this->get_logger(), "activation step"); + tau_pub_timer_->reset(); + rclcpp_lifecycle::LifecycleNode::on_activate(previous_state); + + return LifecycleCallbackReturn::SUCCESS; +} + +LifecycleCallbackReturn DPAdaptBacksControllerNode::on_deactivate( + const rclcpp_lifecycle::State& previous_state) { + (void)previous_state; + RCLCPP_INFO(this->get_logger(), "deactivation step"); + + tau_pub_timer_->reset(); + tau_pub_timer_->cancel(); + dp_adapt_backs_controller_.reset_adap_param(); + return LifecycleCallbackReturn::SUCCESS; +} + +LifecycleCallbackReturn DPAdaptBacksControllerNode::on_cleanup( + const rclcpp_lifecycle::State& previous_state) { + (void)previous_state; + RCLCPP_INFO(this->get_logger(), "cleanup step"); + + dp_adapt_backs_controller_.reset_adap_param(); + tau_pub_timer_->reset(); + tau_pub_timer_->cancel(); + + return LifecycleCallbackReturn::SUCCESS; +} + +LifecycleCallbackReturn DPAdaptBacksControllerNode::on_shutdown( + const rclcpp_lifecycle::State& previous_state) { + (void)previous_state; + RCLCPP_INFO(this->get_logger(), "shutdown step"); + + dp_adapt_backs_controller_.reset_adap_param(); + tau_pub_timer_->cancel(); + tau_pub_.reset(); + + return LifecycleCallbackReturn::SUCCESS; +} + +void DPAdaptBacksControllerNode::killswitch_callback( + const std_msgs::msg::Bool::SharedPtr msg) { + killswitch_on_ = msg->data; +} + +void DPAdaptBacksControllerNode::software_mode_callback( + const std_msgs::msg::String::SharedPtr msg) { + software_mode_ = msg->data; +} + +void DPAdaptBacksControllerNode::pose_callback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { + eta_.pos << msg->pose.pose.position.x, msg->pose.pose.position.y, + msg->pose.pose.position.z; + + eta_.ori = quat_to_euler( + msg->pose.pose.orientation.w, msg->pose.pose.orientation.x, + msg->pose.pose.orientation.y, msg->pose.pose.orientation.z); +} + +void DPAdaptBacksControllerNode::twist_callback( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { + nu_.linear_speed << msg->twist.twist.linear.x, msg->twist.twist.linear.y, + msg->twist.twist.linear.z; + + nu_.angular_speed << msg->twist.twist.angular.x, msg->twist.twist.angular.y, + msg->twist.twist.angular.z; +} + +void DPAdaptBacksControllerNode::publish_tau() { + if (killswitch_on_ || software_mode_ != "autonomous mode") { + return; + } + + dp_types::Vector6d tau = + dp_adapt_backs_controller_.calculate_tau(eta_, eta_d_, nu_); + + geometry_msgs::msg::Wrench tau_msg; + tau_msg.force.x = tau(0); + tau_msg.force.y = tau(1); + tau_msg.force.z = tau(2); + tau_msg.torque.x = tau(3); + tau_msg.torque.y = tau(4); + tau_msg.torque.z = tau(5); + + tau_pub_->publish(tau_msg); +} + +void DPAdaptBacksControllerNode::set_adap_params() { + this->declare_parameter>( + "adap_param", + {0.8, 0.8, 0.8, 0.8, 0.8, 0.8, 0.8, 0.8, 0.8, 0.8, 0.8, 0.8}); + this->declare_parameter>( + "d_gain", {0.3, 0.3, 0.45, 0.2, 0.7, 0.6}); + this->declare_parameter>( + "K1", {20.5, 15.5, 20.5, 1.2, 6.0, 2.5}); + this->declare_parameter>( + "K2", {30.5, 25.5, 30.5, 2.6, 10.0, 6.5}); + this->declare_parameter>("r_b_bg", {0.01, 0.0, 0.02}); + this->declare_parameter>("I_b", {0.68, 3.32, 3.34}); + this->declare_parameter>("mass_matrix", + std::vector(36, 1.0)); + this->declare_parameter("m", {30}); + + std::vector adap_param_vec = + this->get_parameter("adap_param").as_double_array(); + std::vector d_gain_vec = + this->get_parameter("d_gain").as_double_array(); + std::vector K1_vec = this->get_parameter("K1").as_double_array(); + std::vector K2_vec = this->get_parameter("K2").as_double_array(); + std::vector r_b_bg_vec = + this->get_parameter("r_b_bg").as_double_array(); + std::vector I_b_vec = this->get_parameter("I_b").as_double_array(); + std::vector mass_matrix_vec = + this->get_parameter("mass_matrix").as_double_array(); + + double m = this->get_parameter("m").as_double(); + + dp_types::Vector12d adap_param_eigen = + Eigen::Map(adap_param_vec.data()); + dp_types::Vector6d d_gain_eigen = + Eigen::Map(d_gain_vec.data()); + dp_types::Vector6d K1_eigen = Eigen::Map(K1_vec.data()); + dp_types::Vector6d K2_eigen = Eigen::Map(K2_vec.data()); + dp_types::Vector3d r_b_bg_eigen = + Eigen::Map(r_b_bg_vec.data()); + dp_types::Vector3d I_b_eigen = + Eigen::Map(I_b_vec.data()); + dp_types::Matrix6d mass_matrix = + Eigen::Map(mass_matrix_vec.data()); + + dp_adapt_backs_controller_.set_k1(K1_eigen); + dp_adapt_backs_controller_.set_k2(K2_eigen); + dp_adapt_backs_controller_.set_rbg(r_b_bg_eigen); + dp_adapt_backs_controller_.set_adap_param(adap_param_eigen); + dp_adapt_backs_controller_.set_d_gain(d_gain_eigen); + dp_adapt_backs_controller_.set_inertia_matrix(I_b_eigen); + dp_adapt_backs_controller_.set_mass_inertia_matrix(mass_matrix); + dp_adapt_backs_controller_.set_m(m); +} + +void DPAdaptBacksControllerNode::guidance_callback( + const vortex_msgs::msg::ReferenceFilter::SharedPtr msg) { + eta_d_.pos << msg->x, msg->y, msg->z; + eta_d_.ori << msg->roll, msg->pitch, msg->yaw; +} + +void DPAdaptBacksControllerNode::get_topics(Topics& topics) { + this->declare_parameter("dp_reference_topic", "/dp/reference"); + topics.dp_reference_topic = + this->get_parameter("dp_reference_topic").as_string(); + + this->declare_parameter("pose_topic", "/dvl/pose"); + topics.pose_topic = this->get_parameter("pose_topic").as_string(); + + this->declare_parameter("twist_topic", "/dvl/twist"); + topics.twist_topic = this->get_parameter("twist_topic").as_string(); + + this->declare_parameter("software_kill_switch_topic", + "/softwareKillSwitch"); + topics.software_kill_switch_topic = + this->get_parameter("software_kill_switch_topic").as_string(); + + this->declare_parameter("software_operation_mode_topic", + "/softwareOperationMode"); + topics.software_operation_mode_topic = + this->get_parameter("software_operation_mode_topic").as_string(); + + this->declare_parameter("control_topic", "/thrust/wrench_input"); + topics.control_topic = this->get_parameter("control_topic").as_string(); +} diff --git a/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_utils.cpp b/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_utils.cpp new file mode 100644 index 00000000..3a09f6a5 --- /dev/null +++ b/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_utils.cpp @@ -0,0 +1,181 @@ +#include "dp_adapt_backs_controller/dp_adapt_backs_controller_utils.hpp" +#include +#include +#include "dp_adapt_backs_controller/typedefs.hpp" + +double ssa(double angle) { + return std::atan2(std::sin(angle), std::cos(angle)); +} + +dp_types::Eta error_eta(const dp_types::Eta& eta, const dp_types::Eta& eta_d) { + dp_types::Eta eta_error; + eta_error.pos = eta.pos - eta_d.pos; + eta_error.ori.x() = ssa(eta.ori.x() - eta_d.ori.x()); + eta_error.ori.y() = ssa(eta.ori.y() - eta_d.ori.y()); + eta_error.ori.z() = ssa(eta.ori.z() - eta_d.ori.z()); + return eta_error; +} + +dp_types::Matrix3d skew_symmetric(const dp_types::Vector3d& vec) { + dp_types::Matrix3d skew; + + skew << 0, -vec.z(), vec.y(), vec.z(), 0, -vec.x(), -vec.y(), vec.x(), 0; + + return skew; +} + +dp_types::Vector3d quat_to_euler(double w, double x, double y, double z) { + dp_types::Vector3d euler; + euler.x() = std::atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y)); + euler.y() = std::asin(2 * (w * y - z * x)); + euler.z() = std::atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z)); + + return euler; +} + +dp_types::Matrix3d calculate_R(const dp_types::Eta& eta) { + const double roll = eta.ori.x(); + const double pitch = eta.ori.y(); + const double yaw = eta.ori.z(); + + const Eigen::AngleAxisd roll_angle(roll, dp_types::Vector3d::UnitX()); + const Eigen::AngleAxisd pitch_angle(pitch, dp_types::Vector3d::UnitY()); + const Eigen::AngleAxisd yaw_angle(yaw, dp_types::Vector3d::UnitZ()); + + const Eigen::Quaterniond q = yaw_angle * pitch_angle * roll_angle; + + return q.toRotationMatrix(); +} + +dp_types::Matrix3d calculate_T(const dp_types::Eta& eta) { + double sin_phi = std::sin(eta.ori.x()); + double cos_phi = std::cos(eta.ori.x()); + double cos_theta = std::cos(eta.ori.y()); + double tan_theta = std::tan(eta.ori.y()); + + dp_types::Matrix3d T; + T(0, 0) = 1; + T(0, 1) = sin_phi * tan_theta; + T(0, 2) = cos_phi * tan_theta; + T(1, 0) = 0; + T(1, 1) = cos_phi; + T(1, 2) = -sin_phi; + T(2, 0) = 0; + T(2, 1) = sin_phi / cos_theta; + T(2, 2) = cos_phi / cos_theta; + + return T; +} + +dp_types::Matrix6d calculate_J(const dp_types::Eta& eta) { + dp_types::Matrix3d R = calculate_R(eta); + dp_types::Matrix3d T = calculate_T(eta); + + dp_types::J_matrix J; + J.R = R; + J.T = T; + + return J.as_matrix(); +} + +dp_types::Matrix6d calculate_J_sudo_inv(const dp_types::Eta& eta) { + dp_types::Matrix6d J = calculate_J(eta); + dp_types::Matrix6d J_inv = dp_types::Matrix6d::Zero(); + + if (J.determinant() == 0) { + std::cerr << "Jacobian matrix is singular" << std::endl; + return J; + } + + J_inv = J.inverse(); + + return J_inv; +} + +dp_types::Matrix3d calculate_R_dot(const dp_types::Eta& eta, + const dp_types::Nu& nu) { + dp_types::Matrix3d R = calculate_R(eta); + dp_types::Vector3d vec(0, 0, 1); + + dp_types::Matrix3d S = skew_symmetric(vec); + + dp_types::Matrix3d R_dot = R * S * nu.angular_speed.z(); + + return R_dot; +} + +dp_types::Matrix3d calculate_T_dot(const dp_types::Eta& eta, + const dp_types::Nu& nu) { + double cos_phi = std::cos(eta.ori.x()); + double sin_phi = std::sin(eta.ori.x()); + double cos_theta = std::cos(eta.ori.y()); + double sin_theta = std::sin(eta.ori.y()); + double tan_theta = sin_phi / cos_phi; + double inv_cos2 = 1 / (cos_theta * cos_theta); + + dp_types::Matrix3d dt_dphi; + dt_dphi << 0.0, cos_phi * tan_theta, -sin_phi * tan_theta, 0.0, -sin_phi, + -cos_phi, 0.0, cos_phi / cos_theta, -sin_phi / cos_theta; + + dp_types::Matrix3d dt_dtheta; + dt_dtheta << 0.0, sin_phi * inv_cos2, cos_phi * inv_cos2, 0.0, 0.0, 0.0, + 0.0, (sin_phi * sin_theta) * inv_cos2, (cos_phi * sin_theta) * inv_cos2; + + dp_types::Matrix3d T_dot = + dt_dphi * nu.angular_speed.x() + dt_dtheta * nu.angular_speed.y(); + + return T_dot; +} +dp_types::Matrix6d calculate_J_dot(const dp_types::Eta& eta, + const dp_types::Nu& nu) { + dp_types::Matrix3d R_dot = calculate_R_dot(eta, nu); + dp_types::Matrix3d T_dot = calculate_T_dot(eta, nu); + + dp_types::J_matrix J_dot; + J_dot.R = R_dot; + J_dot.T = T_dot; + + return J_dot.as_matrix(); +} + +dp_types::Matrix6d calculate_C(double m, + const dp_types::Vector3d& r_b_bg, + const dp_types::Nu& nu_2, + const dp_types::Matrix3d& I_b) { + dp_types::Matrix6d C; + dp_types::Matrix3d C1 = m * skew_symmetric(nu_2.linear_speed); + dp_types::Matrix3d C2 = + -m * skew_symmetric(nu_2.angular_speed) * skew_symmetric(r_b_bg); + dp_types::Matrix3d C3 = + m * skew_symmetric(nu_2.angular_speed) * skew_symmetric(r_b_bg); + dp_types::Matrix3d C4 = skew_symmetric(I_b * nu_2.angular_speed); + + C << C1, C2, C3, C4; + + return C; +} + +dp_types::Matrix6x12d calculate_Y_v(const dp_types::Nu& nu) { + dp_types::Matrix6x12d Y_v; + Y_v.setZero(); + + Y_v(0, 0) = nu.linear_speed.x(); + Y_v(0, 1) = nu.linear_speed.x() * std::abs(nu.linear_speed.x()); + + Y_v(1, 2) = nu.linear_speed.y(); + Y_v(1, 3) = nu.linear_speed.y() * std::abs(nu.linear_speed.y()); + + Y_v(2, 4) = nu.linear_speed.z(); + Y_v(2, 5) = nu.linear_speed.z() * std::abs(nu.linear_speed.z()); + + Y_v(3, 6) = nu.angular_speed.x(); + Y_v(3, 7) = nu.angular_speed.x() * std::abs(nu.angular_speed.x()); + + Y_v(4, 8) = nu.angular_speed.y(); + Y_v(4, 9) = nu.angular_speed.y() * std::abs(nu.angular_speed.y()); + + Y_v(5, 10) = nu.angular_speed.z(); + Y_v(5, 11) = nu.angular_speed.z() * std::abs(nu.angular_speed.z()); + + return Y_v; +} diff --git a/control/velocity_controller_lqr/README.txt b/control/velocity_controller_lqr/README.txt deleted file mode 100644 index c49e405e..00000000 --- a/control/velocity_controller_lqr/README.txt +++ /dev/null @@ -1 +0,0 @@ -This package contains the velocity controller for the AUV Orca. The controller utilizes an LQR optimal controller (imported from the python control library), and controls pitch, yaw and surge. The controller is meant to traverse larger distances. diff --git a/control/velocity_controller_lqr/Readme.md b/control/velocity_controller_lqr/Readme.md new file mode 100644 index 00000000..06a1660c --- /dev/null +++ b/control/velocity_controller_lqr/Readme.md @@ -0,0 +1,40 @@ +## Overview: +--- +Contains the LQR controller package for the AUV Orca. The controller utilizes an LQR optimal controller (imported from the python control library), and controls pitch, yaw and surge. The controller is meant to traverse larger distances. + +#### Tuning of Parameters: + To tune parameters look at the config/param_velocity_controller_lqr.yaml file: + + +#### Launching the package: +```bash +1. inside ros2ws/colcon build --packages-select velocity_controller_lqr +``` + +```bash +2. Inisde ros2ws/: source install/setup.bash +``` + +```bash +3. ros2 launch velocity_controller_lqr velocity_controller_lqr.launch.py +``` + +#### Transitioning between states manually: +The ROS2 node is implemented using lifecycle nodes, which are managed externally by a lifecycle manager i.e a finite state machine. If you want to manually test the node do the following: + +**From Unconfigured ---> Configured (Inactive)** +```bash +ros2 lifecycle set /velocity_controller_lqr_node configure +``` + +**From Configured ---> Active** +```bash +ros2 lifecycle set /velocity_controller_lqr_node activate +``` + +**From Active ---> Inactive** +```bash +ros2 lifecycle set /velocity_controller_lqr_node deactivate +``` + +More info about the