diff --git a/.gitignore b/.gitignore index 41eed4018..31e22429e 100644 --- a/.gitignore +++ b/.gitignore @@ -59,3 +59,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 diff --git a/control/pid_controller_dp/CMakeLists.txt b/control/pid_controller_dp/CMakeLists.txt index 17db75976..80f9e132e 100644 --- a/control/pid_controller_dp/CMakeLists.txt +++ b/control/pid_controller_dp/CMakeLists.txt @@ -16,19 +16,46 @@ 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) find_package(vortex_utils_ros 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 + vortex_utils_ros + 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 @@ -36,8 +63,27 @@ ament_target_dependencies(pid_controller_node Eigen3 tf2 vortex_msgs + rcl_interfaces vortex_utils vortex_utils_ros + 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 @@ -50,4 +96,9 @@ install(DIRECTORY DESTINATION share/${PROJECT_NAME}/ ) + +if(BUILD_TESTING) + add_subdirectory(test) +endif() + ament_package() diff --git a/control/pid_controller_dp/README.md b/control/pid_controller_dp/README.md index 902fc9fc3..40c470e3e 100644 --- a/control/pid_controller_dp/README.md +++ b/control/pid_controller_dp/README.md @@ -1,7 +1,94 @@ -## PID controller +# PID controller + The PID controller is defined + ```math \tau = -J_{q}^{\dagger}(K_p \tilde{\eta} + K_d \dot{\tilde{\eta}} + K_i \int^t_0 \tilde{\eta}(\tau)d\tau) ``` -where $\tau$ is the control input, $\tilde{\eta} = \eta - \eta_d$ is the pose error, $J_q$ is the quaternion based Jacobian matrix and $K_p$, $K_d$ and $K_i$ are tuning matrices. +where: + +- $\tilde{\eta} = \eta - \eta_d$ is the pose error (7D quaternion representation), +- $J_q$ is the quaternion Jacobian (7×6), and $J_q^{\dagger}$ is its (pseudo-)inverse, +- $K_p$, $K_d$, $K_i$ are 6×6 gain matrices. + +## PID controller (pid_controller_dp) + +This package implements a 6-DOF PID controller that operates on the +6-dimensional control vector +$$\tau = [X, Y, Z, K, M, N]^T$$ +and uses a quaternion-based 7D pose representation for attitude. + +## Build + + +This package is built as part of the workspace. From the workspace root: + +```bash +colcon build --packages-select pid_controller_dp +``` + +To run tests for this package only: + +```bash +colcon test --packages-select pid_controller_dp && colcon test-result --verbose +``` + +## Usage (ROS 2 node) + +The package provides a node `pid_controller_node` that subscribes to pose, +twist and guidance topics and publishes wrench (tau) commands. + + +- `topics.pose` (type: `geometry_msgs/PoseWithCovarianceStamped`) — vehicle pose input +- `topics.twist` (type: `geometry_msgs/TwistWithCovarianceStamped`) — velocity input +- `topics.guidance.dp` (type: `vortex_msgs/ReferenceFilter`) — desired states (pose/vecocity) +- `topics.wrench_input` (type: `geometry_msgs/WrenchStamped`) — output wrench + +Parameters expose PID gains (Kp, Ki, Kd) as per-component values which are +assembled into diagonal gain matrices inside the node. See the node source +(`src/pid_controller_ros.cpp`) for parameter names. + +## Examples + +Start the node (after sourcing workspace): + +1. Run the simulation + + ```bash + ros2 launch stonefish_sim simulation.launch.py scenario:=default + ``` + +2. Run the thrust allocation node: + + ```bash + ros2 launch thrust_allocator_auv thrust_allocator_auv.launch.py + ``` + +3. To move the robot, run the joystick node + + ```bash + ros2 launch stonefish_sim orca_sim.launch.py + ``` + +4. Run the controller + + ```bash + ros2 launch pid_controller_dp pid_controller_dp.launch.py + ``` + +Use the joy stick to move the robot. The key mappings are: + +- B - kill +- Y - autonomous mode (reference model) +- A - manual mode + +Note: When plotting, the axis plotted and actual command might not align since the plotting is based on the joy controller frame (`odom`), whereas the controller works on the robot frame (`body_frame`) + +## Tuning + +The `rqt_reconfigure` can be used to change the controller gains. + +```bash +ros2 run rqt_reconfigure rqt_reconfigure +``` diff --git a/control/pid_controller_dp/config/pid_params.yaml b/control/pid_controller_dp/config/pid_params.yaml index 2a77ffd59..17bcd5026 100644 --- a/control/pid_controller_dp/config/pid_params.yaml +++ b/control/pid_controller_dp/config/pid_params.yaml @@ -1,5 +1,20 @@ /**: 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_x: 20.0 + Kp_y: 26.0 + Kp_z: 50.0 + Kp_roll: 10.0 + Kp_pitch: 41.0 + Kp_yaw: 6.0 + Ki_x: 0.092 + Ki_y: 0.00059 + Ki_z: 0.085 + Ki_roll: 0.002 + Ki_pitch: 0.01 + Ki_yaw: 0.0003 + Kd_x: 0.001 + Kd_y: 0.0 + Kd_z: 0.0 + Kd_roll: 0.002 + Kd_pitch: 0.0 + Kd_yaw: 0.001 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 index 1017b5bd6..52a855262 100644 --- a/control/pid_controller_dp/include/pid_controller_dp/pid_controller.hpp +++ b/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 #include "pid_controller_dp/typedefs.hpp" class PIDController { @@ -36,6 +37,10 @@ class PIDController { // @param dt: Time step void set_time_step(double dt); + types::Matrix6d get_kp(); + types::Matrix6d get_ki(); + types::Matrix6d get_kd(); + private: types::Matrix6d Kp_; types::Matrix6d Ki_; diff --git a/control/pid_controller_dp/include/pid_controller_dp/pid_controller_conversions.hpp b/control/pid_controller_dp/include/pid_controller_dp/pid_controller_conversions.hpp index a28097e9f..5b2ea6dbc 100644 --- a/control/pid_controller_dp/include/pid_controller_dp/pid_controller_conversions.hpp +++ b/control/pid_controller_dp/include/pid_controller_dp/pid_controller_conversions.hpp @@ -3,16 +3,16 @@ #include #include -#include -#include +#include +#include #include #include #include "pid_controller_dp/typedefs.hpp" types::Eta eta_convert_from_ros_to_eigen( - const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); + const geometry_msgs::msg::PoseWithCovariance& msg); types::Nu nu_convert_from_ros_to_eigen( - const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); + const geometry_msgs::msg::TwistWithCovariance& msg); #endif // PID_CONTROLLER_DP__PID_CONTROLLER_CONVERSIONS_HPP_ 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 index 9b302d1af..616f1ec76 100644 --- 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 @@ -7,6 +7,7 @@ #include #include #include +#include #include #include #include @@ -35,16 +36,6 @@ class PIDControllerNode : public rclcpp::Node { void operation_mode_callback( const vortex_msgs::msg::OperationMode::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 tau publisher timer void publish_tau(); @@ -64,6 +55,15 @@ class PIDControllerNode : public rclcpp::Node { void guidance_callback( const vortex_msgs::msg::ReferenceFilter::SharedPtr msg); + // @brief Callback function for the odometry topic + // @param msg: Odometry message containing the AUV pose and speed + void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg); + + // @brief Callback function for parameter updates + // @param parameters: vector of parameters to be set + rcl_interfaces::msg::SetParametersResult parametersCallback( + const std::vector& parameters); + rclcpp::Client::SharedPtr get_operation_mode_client_; @@ -74,11 +74,7 @@ class PIDControllerNode : public rclcpp::Node { rclcpp::Subscription::SharedPtr operation_mode_sub_; - rclcpp::Subscription< - geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_; - - rclcpp::Subscription< - geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr twist_sub_; + rclcpp::Subscription::SharedPtr odom_sub_; rclcpp::Subscription::SharedPtr guidance_sub_; @@ -107,6 +103,8 @@ class PIDControllerNode : public rclcpp::Node { vortex::utils::types::Mode operation_mode_{ vortex::utils::types::Mode::manual}; + + OnSetParametersCallbackHandle::SharedPtr callback_handle_; }; #endif // PID_CONTROLLER_DP__PID_CONTROLLER_ROS_HPP_ 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 index 1bdce214b..dcd79b400 100644 --- 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 @@ -1,12 +1,15 @@ #ifndef PID_CONTROLLER_DP__PID_CONTROLLER_UTILS_HPP_ #define PID_CONTROLLER_DP__PID_CONTROLLER_UTILS_HPP_ +#include #include #include #include #include #include +#include #include "pid_controller_dp/typedefs.hpp" +#include "typedefs.hpp" // @brief Calculate the sine of an angle in degrees // @param angle: Angle in degrees diff --git a/control/pid_controller_dp/include/pid_controller_dp/typedefs.hpp b/control/pid_controller_dp/include/pid_controller_dp/typedefs.hpp index beeea3174..d2f6a8707 100644 --- a/control/pid_controller_dp/include/pid_controller_dp/typedefs.hpp +++ b/control/pid_controller_dp/include/pid_controller_dp/typedefs.hpp @@ -7,48 +7,31 @@ #define PID_CONTROLLER_DP__TYPEDEFS_HPP_ #include +#include namespace types { -typedef Eigen::Matrix Vector3d; -typedef Eigen::Matrix Vector6d; -typedef Eigen::Matrix Vector7d; -typedef Eigen::Matrix Vector4d; -typedef Eigen::Matrix Matrix6d; -typedef Eigen::Matrix Matrix3d; -typedef Eigen::Matrix Matrix4x3d; -typedef Eigen::Matrix Matrix7x6d; -typedef Eigen::Matrix Matrix6x7d; -typedef Eigen::Matrix 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; +using Vector4d = Eigen::Matrix; +using Vector6d = Eigen::Matrix; +using Vector7d = Eigen::Matrix; +using Matrix3d = Eigen::Matrix; +using Matrix4x3d = Eigen::Matrix; +using Matrix6d = Eigen::Matrix; +using Matrix7x6d = Eigen::Matrix; +using Matrix6x7d = Eigen::Matrix; +using Matrix7d = Eigen::Matrix; +using Quaterniond = Eigen::Quaterniond; + +// Alias canonical types from vortex utils +using Eta = ::vortex::utils::types::Pose; +using Nu = ::vortex::utils::types::Twist; 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; diff --git a/control/pid_controller_dp/package.xml b/control/pid_controller_dp/package.xml index dd13d899e..3d5abc768 100644 --- a/control/pid_controller_dp/package.xml +++ b/control/pid_controller_dp/package.xml @@ -15,6 +15,7 @@ eigen tf2 vortex_msgs + rcl_interfaces vortex_utils vortex_utils_ros diff --git a/control/pid_controller_dp/src/pid_controller.cpp b/control/pid_controller_dp/src/pid_controller.cpp index 427c7dcdf..8867b160f 100644 --- a/control/pid_controller_dp/src/pid_controller.cpp +++ b/control/pid_controller_dp/src/pid_controller.cpp @@ -1,6 +1,72 @@ #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:"); + auto pos = eta.pos_vector(); + auto ori = eta.ori_quaternion(); + spdlog::info("Position - North: {}, East: {}, Down: {}", pos[0], pos[1], + pos[2]); + spdlog::info("Orientation - w: {}, x: {}, y: {}, z: {}", ori.w(), ori.x(), + ori.y(), ori.z()); +} + +void print_nu(const types::Nu& nu) { + spdlog::info("Nu values:"); + auto v = nu.to_vector(); + spdlog::info("Linear Speed - u: {}, v: {}, w: {}", v(0), v(1), v(2)); + spdlog::info("Angular Speed - p: {}, q: {}, r: {}", v(3), v(4), v(5)); +} + +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,19 +78,28 @@ 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 quaternion scalar part w = 0 (only use vector part of quaternion for + // error) + error.qw = 0.0; - types::Matrix6x7d J_inv = calculate_J_sudo_inv(error); + auto eta_dot_d_copy = eta_dot_d; + eta_dot_d_copy.qw = 0.0; // set w = 0 for desired eta_dot - types::Vector6d nu_d = J_inv * eta_dot_d.as_vector(); + types::Matrix6x7d J_inv = + calculate_J_sudo_inv(eta); // calculate J pseudo inverse - types::Vector6d error_nu = nu.as_vector() - nu_d; + types::Vector6d nu_d = + J_inv * eta_dot_d_copy.to_vector(); // calculate velocity - types::Vector6d P = Kp_ * J_inv * error.as_vector(); + types::Vector6d error_nu = nu.to_vector() - nu_d; // calculate vel error - types::Vector6d I = Ki_ * J_inv * integral_; + types::Vector6d P = Kp_ * J_inv * error.to_vector(); // P term - types::Vector6d D = Kd_ * error_nu; + types::Vector6d I = Ki_ * J_inv * integral_; // I term + + types::Vector6d D = Kd_ * error_nu; // D term types::Vector6d tau = -clamp_values((P + I + D), -80.0, 80.0); @@ -48,3 +123,13 @@ void PIDController::set_kd(const types::Matrix6d& Kd) { void PIDController::set_time_step(double dt) { this->dt_ = dt; } + +types::Matrix6d PIDController::get_kp() { + return this->Kp_; +} +types::Matrix6d PIDController::get_ki() { + return this->Ki_; +} +types::Matrix6d PIDController::get_kd() { + return this->Kd_; +} \ No newline at end of file diff --git a/control/pid_controller_dp/src/pid_controller_conversions.cpp b/control/pid_controller_dp/src/pid_controller_conversions.cpp index 8f6ff1970..1720752c4 100644 --- a/control/pid_controller_dp/src/pid_controller_conversions.cpp +++ b/control/pid_controller_dp/src/pid_controller_conversions.cpp @@ -5,24 +5,28 @@ #include "pid_controller_dp/typedefs.hpp" types::Eta eta_convert_from_ros_to_eigen( - const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { + const geometry_msgs::msg::PoseWithCovariance& msg) { types::Eta eta; - eta.pos << msg->pose.pose.position.x, msg->pose.pose.position.y, - msg->pose.pose.position.z; - eta.ori.w() = msg->pose.pose.orientation.w; - eta.ori.x() = msg->pose.pose.orientation.x; - eta.ori.y() = msg->pose.pose.orientation.y; - eta.ori.z() = msg->pose.pose.orientation.z; + eta.x = msg.pose.position.x; + eta.y = msg.pose.position.y; + eta.z = msg.pose.position.z; + eta.qw = msg.pose.orientation.w; + eta.qx = msg.pose.orientation.x; + eta.qy = msg.pose.orientation.y; + eta.qz = msg.pose.orientation.z; return eta; } types::Nu nu_convert_from_ros_to_eigen( - const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { + const geometry_msgs::msg::TwistWithCovariance& msg) { types::Nu nu; - 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; + nu.u = msg.twist.linear.x; + nu.v = msg.twist.linear.y; + nu.w = msg.twist.linear.z; + nu.p = msg.twist.angular.x; + nu.q = msg.twist.angular.y; + nu.r = msg.twist.angular.z; + return nu; } diff --git a/control/pid_controller_dp/src/pid_controller_node.cpp b/control/pid_controller_dp/src/pid_controller_node.cpp index f9bf44663..46d51b1ee 100644 --- a/control/pid_controller_dp/src/pid_controller_node.cpp +++ b/control/pid_controller_dp/src/pid_controller_node.cpp @@ -1,8 +1,17 @@ +#include #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"); + spdlog::info(start_msg); rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; diff --git a/control/pid_controller_dp/src/pid_controller_ros.cpp b/control/pid_controller_dp/src/pid_controller_ros.cpp index 44a63cc63..c473956c9 100644 --- a/control/pid_controller_dp/src/pid_controller_ros.cpp +++ b/control/pid_controller_dp/src/pid_controller_ros.cpp @@ -1,4 +1,5 @@ #include +#include #include #include #include @@ -16,6 +17,9 @@ PIDControllerNode::PIDControllerNode() : Node("pid_controller_node") { tau_pub_timer_ = this->create_wall_timer( time_step_, std::bind(&PIDControllerNode::publish_tau, this)); set_pid_params(); + + callback_handle_ = this->add_on_set_parameters_callback(std::bind( + &PIDControllerNode::parametersCallback, this, std::placeholders::_1)); } void PIDControllerNode::set_subscribers_and_publisher() { @@ -28,11 +32,8 @@ void PIDControllerNode::set_subscribers_and_publisher() { std::string dp_reference_topic = this->get_parameter("topics.guidance.dp").as_string(); - this->declare_parameter("topics.pose"); - std::string pose_topic = this->get_parameter("topics.pose").as_string(); - - this->declare_parameter("topics.twist"); - std::string twist_topic = this->get_parameter("topics.twist").as_string(); + this->declare_parameter("topics.odom"); + std::string odom_topic = this->get_parameter("topics.odom").as_string(); this->declare_parameter("topics.killswitch"); std::string software_kill_switch_topic = @@ -56,16 +57,9 @@ void PIDControllerNode::set_subscribers_and_publisher() { std::bind(&PIDControllerNode::operation_mode_callback, this, std::placeholders::_1)); - pose_sub_ = this->create_subscription< - geometry_msgs::msg::PoseWithCovarianceStamped>( - pose_topic, qos_sensor_data, - std::bind(&PIDControllerNode::pose_callback, this, - std::placeholders::_1)); - - twist_sub_ = this->create_subscription< - geometry_msgs::msg::TwistWithCovarianceStamped>( - twist_topic, qos_sensor_data, - std::bind(&PIDControllerNode::twist_callback, this, + odom_sub_ = this->create_subscription( + odom_topic, qos_sensor_data, + std::bind(&PIDControllerNode::odom_callback, this, std::placeholders::_1)); guidance_sub_ = @@ -121,14 +115,10 @@ void PIDControllerNode::operation_mode_callback( operation_mode_ = vortex::utils::ros_conversions::convert_from_ros(*msg); } -void PIDControllerNode::pose_callback( - const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { - eta_ = eta_convert_from_ros_to_eigen(msg); -} - -void PIDControllerNode::twist_callback( - const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { - nu_ = nu_convert_from_ros_to_eigen(msg); +void PIDControllerNode::odom_callback( + const nav_msgs::msg::Odometry::SharedPtr msg) { + eta_ = eta_convert_from_ros_to_eigen(msg->pose); + nu_ = nu_convert_from_ros_to_eigen(msg->twist); } void PIDControllerNode::publish_tau() { @@ -154,20 +144,57 @@ void PIDControllerNode::publish_tau() { } void PIDControllerNode::set_pid_params() { - this->declare_parameter>( - "Kp", {1.0, 1.0, 1.0, 1.0, 1.0, 1.0}); - this->declare_parameter>( - "Ki", {0.1, 0.1, 0.1, 0.1, 0.1, 0.1}); - this->declare_parameter>( - "Kd", {0.1, 0.1, 0.1, 0.1, 0.1, 0.1}); - - std::vector Kp_vec = this->get_parameter("Kp").as_double_array(); - std::vector Ki_vec = this->get_parameter("Ki").as_double_array(); - std::vector Kd_vec = this->get_parameter("Kd").as_double_array(); - - types::Matrix6d Kp_eigen = Eigen::Map(Kp_vec.data()); - types::Matrix6d Ki_eigen = Eigen::Map(Ki_vec.data()); - types::Matrix6d Kd_eigen = Eigen::Map(Kd_vec.data()); + this->declare_parameter("Kp_x", 1.0); + this->declare_parameter("Kp_y", 1.0); + this->declare_parameter("Kp_z", 1.0); + this->declare_parameter("Kp_roll", 1.0); + this->declare_parameter("Kp_pitch", 1.0); + this->declare_parameter("Kp_yaw", 1.0); + this->declare_parameter("Ki_x", 0.1); + this->declare_parameter("Ki_y", 0.1); + this->declare_parameter("Ki_z", 0.1); + this->declare_parameter("Ki_roll", 0.1); + this->declare_parameter("Ki_pitch", 0.1); + this->declare_parameter("Ki_yaw", 0.1); + this->declare_parameter("Kd_x", 0.1); + this->declare_parameter("Kd_y", 0.1); + this->declare_parameter("Kd_z", 0.1); + this->declare_parameter("Kd_roll", 0.1); + this->declare_parameter("Kd_pitch", 0.1); + this->declare_parameter("Kd_yaw", 0.1); + + std::vector Kp_vec = { + this->get_parameter("Kp_x").as_double(), + this->get_parameter("Kp_y").as_double(), + this->get_parameter("Kp_z").as_double(), + this->get_parameter("Kp_roll").as_double(), + this->get_parameter("Kp_pitch").as_double(), + this->get_parameter("Kp_yaw").as_double(), + }; + std::vector Ki_vec = { + this->get_parameter("Ki_x").as_double(), + this->get_parameter("Ki_y").as_double(), + this->get_parameter("Ki_z").as_double(), + this->get_parameter("Ki_roll").as_double(), + this->get_parameter("Ki_pitch").as_double(), + this->get_parameter("Ki_yaw").as_double(), + }; + std::vector Kd_vec = { + this->get_parameter("Kd_x").as_double(), + this->get_parameter("Kd_y").as_double(), + this->get_parameter("Kd_z").as_double(), + this->get_parameter("Kd_roll").as_double(), + this->get_parameter("Kd_pitch").as_double(), + this->get_parameter("Kd_yaw").as_double(), + }; + + types::Vector6d Kp_vec_eigen(Kp_vec.data()); + types::Vector6d Ki_vec_eigen(Ki_vec.data()); + types::Vector6d Kd_vec_eigen(Kd_vec.data()); + + types::Matrix6d Kp_eigen = Kp_vec_eigen.asDiagonal().toDenseMatrix(); + types::Matrix6d Ki_eigen = Ki_vec_eigen.asDiagonal().toDenseMatrix(); + types::Matrix6d Kd_eigen = Kd_vec_eigen.asDiagonal().toDenseMatrix(); pid_controller_.set_kp(Kp_eigen); pid_controller_.set_ki(Ki_eigen); @@ -176,13 +203,143 @@ void PIDControllerNode::set_pid_params() { void PIDControllerNode::guidance_callback( const vortex_msgs::msg::ReferenceFilter::SharedPtr msg) { - eta_d_.pos << msg->x, msg->y, msg->z; + // Set desired position + eta_d_.x = msg->x; + eta_d_.y = msg->y; + eta_d_.z = msg->z; + // Convert desired attitude (roll, pitch, yaw) to quaternion and store double roll = msg->roll; double pitch = msg->pitch; double yaw = msg->yaw; - eta_d_.ori = Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX()) * - Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()); + Eigen::Quaterniond quat = + Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX()) * + Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()); + + eta_d_.qw = quat.w(); + eta_d_.qx = quat.x(); + eta_d_.qy = quat.y(); + eta_d_.qz = quat.z(); +} + +rcl_interfaces::msg::SetParametersResult PIDControllerNode::parametersCallback( + const std::vector& parameters) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + + bool kp_x_updated = false; + bool kp_y_updated = false; + bool kp_z_updated = false; + bool kp_roll_updated = false; + bool kp_pitch_updated = false; + bool kp_yaw_updated = false; + + bool ki_x_updated = false; + bool ki_y_updated = false; + bool ki_z_updated = false; + bool ki_roll_updated = false; + bool ki_pitch_updated = false; + bool ki_yaw_updated = false; + + bool kd_x_updated = false; + bool kd_y_updated = false; + bool kd_z_updated = false; + bool kd_roll_updated = false; + bool kd_pitch_updated = false; + bool kd_yaw_updated = false; + + types::Vector6d Kp_vec_eigen = pid_controller_.get_kp().diagonal(); + types::Vector6d Ki_vec_eigen = pid_controller_.get_ki().diagonal(); + types::Vector6d Kd_vec_eigen = pid_controller_.get_kd().diagonal(); + + for (const auto& param : parameters) { + if (param.get_name() == "Kp_x") { + Kp_vec_eigen(0) = param.as_double(); + kp_x_updated = true; + } else if (param.get_name() == "Kp_y") { + Kp_vec_eigen(1) = param.as_double(); + kp_y_updated = true; + } else if (param.get_name() == "Kp_z") { + Kp_vec_eigen(2) = param.as_double(); + kp_z_updated = true; + } else if (param.get_name() == "Kp_roll") { + Kp_vec_eigen(3) = param.as_double(); + kp_roll_updated = true; + } else if (param.get_name() == "Kp_pitch") { + Kp_vec_eigen(4) = param.as_double(); + kp_pitch_updated = true; + } else if (param.get_name() == "Kp_yaw") { + Kp_vec_eigen(5) = param.as_double(); + kp_yaw_updated = true; + } else if (param.get_name() == "Ki_x") { + Ki_vec_eigen(0) = param.as_double(); + ki_x_updated = true; + } else if (param.get_name() == "Ki_y") { + Ki_vec_eigen(1) = param.as_double(); + ki_y_updated = true; + } else if (param.get_name() == "Ki_z") { + Ki_vec_eigen(2) = param.as_double(); + ki_z_updated = true; + } else if (param.get_name() == "Ki_roll") { + Ki_vec_eigen(3) = param.as_double(); + ki_roll_updated = true; + } else if (param.get_name() == "Ki_pitch") { + Ki_vec_eigen(4) = param.as_double(); + ki_pitch_updated = true; + } else if (param.get_name() == "Ki_yaw") { + Ki_vec_eigen(5) = param.as_double(); + ki_yaw_updated = true; + } else if (param.get_name() == "Kd_x") { + Kd_vec_eigen(0) = param.as_double(); + kd_x_updated = true; + } else if (param.get_name() == "Kd_y") { + Kd_vec_eigen(1) = param.as_double(); + kd_y_updated = true; + } else if (param.get_name() == "Kd_z") { + Kd_vec_eigen(2) = param.as_double(); + kd_z_updated = true; + } else if (param.get_name() == "Kd_roll") { + Kd_vec_eigen(3) = param.as_double(); + kd_roll_updated = true; + } else if (param.get_name() == "Kd_pitch") { + Kd_vec_eigen(4) = param.as_double(); + kd_pitch_updated = true; + } else if (param.get_name() == "Kd_yaw") { + Kd_vec_eigen(5) = param.as_double(); + kd_yaw_updated = true; + } + } + + // Only set the gains if the parameter update was successful + if (result.successful) { + if (kp_x_updated || kp_y_updated || kp_z_updated || kp_roll_updated || + kp_pitch_updated || kp_yaw_updated) { + types::Matrix6d Kp_eigen = + Kp_vec_eigen.asDiagonal().toDenseMatrix(); + pid_controller_.set_kp(Kp_eigen); + } + if (ki_x_updated || ki_y_updated || ki_z_updated || ki_roll_updated || + ki_pitch_updated || ki_yaw_updated) { + types::Matrix6d Ki_eigen = + Ki_vec_eigen.asDiagonal().toDenseMatrix(); + pid_controller_.set_ki(Ki_eigen); + } + if (kd_x_updated || kd_y_updated || kd_z_updated || kd_roll_updated || + kd_pitch_updated || kd_yaw_updated) { + types::Matrix6d Kd_eigen = + Kd_vec_eigen.asDiagonal().toDenseMatrix(); + pid_controller_.set_kd(Kd_eigen); + } + } + + // print + for (const auto& param : parameters) { + RCLCPP_INFO(this->get_logger(), "%s", param.get_name().c_str()); + RCLCPP_INFO(this->get_logger(), "%s", param.get_type_name().c_str()); + RCLCPP_INFO(this->get_logger(), "%s", param.value_to_string().c_str()); + } + return result; } diff --git a/control/pid_controller_dp/src/pid_controller_utils.cpp b/control/pid_controller_dp/src/pid_controller_utils.cpp index 246e15500..834322e11 100644 --- a/control/pid_controller_dp/src/pid_controller_utils.cpp +++ b/control/pid_controller_dp/src/pid_controller_utils.cpp @@ -1,75 +1,41 @@ #include "pid_controller_dp/pid_controller_utils.hpp" #include +#include +#include #include "pid_controller_dp/pid_controller_conversions.hpp" #include "pid_controller_dp/typedefs.hpp" types::Matrix3d calculate_R_quat(const types::Eta& eta) { - return eta.ori.normalized().toRotationMatrix(); + return eta.as_rotation_matrix(); } types::Matrix4x3d calculate_T_quat(const types::Eta& eta) { - types::Quaterniond quaternion_norm = eta.ori.normalized(); - - double w = quaternion_norm.w(); - double x = quaternion_norm.x(); - double y = quaternion_norm.y(); - double z = quaternion_norm.z(); - - types::Matrix4x3d transformation_matrix; - - transformation_matrix << -x, -y, -z, w, -z, y, z, w, -x, -y, x, w; - - return transformation_matrix * 0.5; + return eta.as_transformation_matrix(); } types::Matrix6x7d calculate_J_sudo_inv(const types::Eta& eta) { - types::Eta eta_norm; - - eta_norm.pos = eta.pos; - eta_norm.ori = eta.ori; - - types::Matrix3d R = calculate_R_quat(eta_norm); - types::Matrix4x3d T = calculate_T_quat(eta_norm); - - types::J_transformation J; - J.R = R; - J.T = T; - - types::Matrix6x7d J_transpose = J.as_matrix().transpose(); - types::Matrix6x7d J_pseudo_inv = - (J_transpose * J.as_matrix()).inverse() * J_transpose; + auto J_matrix = eta.as_j_matrix(); + Eigen::MatrixXd J_pseudo_inv_dynamic = + vortex::utils::math::pseudo_inverse(J_matrix); + types::Matrix6x7d J_pseudo_inv; + J_pseudo_inv = J_pseudo_inv_dynamic; return J_pseudo_inv; } types::Eta error_eta(const types::Eta& eta, const types::Eta& eta_d) { - types::Eta eta_error; - - eta_error.pos = eta.pos - eta_d.pos; - eta_error.ori = eta_d.ori.conjugate() * eta.ori; - - eta_error.ori = eta_error.ori.normalized(); - - return eta_error; + return eta - eta_d; } Eigen::VectorXd clamp_values(const Eigen::VectorXd& values, double min_val, double max_val) { - return values.cwiseMax(min_val).cwiseMin(max_val); + return vortex::utils::math::clamp_values(values, min_val, max_val); } types::Vector7d anti_windup(const double dt, const types::Eta& error, const types::Vector7d& integral) { - types::Eta error_norm; - - error_norm.pos = error.pos; - error_norm.ori = error.ori; - - types::Vector7d integral_anti_windup = - integral + (error_norm.as_vector() * dt); - - integral_anti_windup = clamp_values(integral_anti_windup, -80.0, 80.0); - return integral_anti_windup; + return vortex::utils::math::anti_windup(dt, error.to_vector(), integral, + -80.0, 80.0); } diff --git a/control/pid_controller_dp/test/CMakeLists.txt b/control/pid_controller_dp/test/CMakeLists.txt new file mode 100644 index 000000000..d47998914 --- /dev/null +++ b/control/pid_controller_dp/test/CMakeLists.txt @@ -0,0 +1,22 @@ +cmake_minimum_required(VERSION 3.8) + +find_package(GTest REQUIRED) +include(GoogleTest) + +set(TEST_BINARY_NAME ${PROJECT_NAME}_test) +add_executable( + ${TEST_BINARY_NAME} + pid_controller_tests.cpp +) + +target_link_libraries( + ${TEST_BINARY_NAME} + PRIVATE + ${LIB_NAME} + GTest::GTest + spdlog::spdlog +) + +ament_target_dependencies(${TEST_BINARY_NAME} PUBLIC Eigen3 tf2 vortex_utils) + +gtest_discover_tests(${TEST_BINARY_NAME}) diff --git a/control/pid_controller_dp/test/pid_controller_tests.cpp b/control/pid_controller_dp/test/pid_controller_tests.cpp new file mode 100644 index 000000000..18b2a7e6f --- /dev/null +++ b/control/pid_controller_dp/test/pid_controller_tests.cpp @@ -0,0 +1,522 @@ +#include +#include + +#include +#include +#include "pid_controller_dp/pid_controller.hpp" +#include "pid_controller_dp/pid_controller_utils.hpp" +#include "pid_controller_dp/typedefs.hpp" + +void print_tau(const types::Vector6d& tau) { + spdlog::info("Tau values:"); + spdlog::info("Surge: {}", tau[0]); + spdlog::info("Sway: {}", tau[1]); + spdlog::info("Heave: {}", tau[2]); + spdlog::info("Roll: {}", tau[3]); + spdlog::info("Pitch: {}", tau[4]); + spdlog::info("Yaw: {}", tau[5]); +} + +class PIDControllerTests : public ::testing::Test { + protected: + PIDControllerTests() : pid_controller_() { + // Set PID gains for testing + types::Matrix6d Kp = types::Matrix6d::Identity() * 10.0; + types::Matrix6d Ki = types::Matrix6d::Identity() * 0.5; + types::Matrix6d Kd = types::Matrix6d::Identity() * 2.0; + + pid_controller_.set_kp(Kp); + pid_controller_.set_ki(Ki); + pid_controller_.set_kd(Kd); + } + + types::Eta generate_current_pose(const double north_pos, + const double east_pos, + const double down_pos, + const double roll_angle, + const double pitch_angle, + const double yaw_angle) { + types::Eta current_pose; + current_pose.x = north_pos; + current_pose.y = east_pos; + current_pose.z = down_pos; + Eigen::Quaterniond q = vortex::utils::math::euler_to_quat( + roll_angle, pitch_angle, yaw_angle); + current_pose.qw = q.w(); + current_pose.qx = q.x(); + current_pose.qy = q.y(); + current_pose.qz = q.z(); + return current_pose; + } + + types::Eta generate_reference_pose(const double north_pos, + const double east_pos, + const double down_pos, + const double roll_angle, + const double pitch_angle, + const double yaw_angle) { + types::Eta reference_pose; + reference_pose.x = north_pos; + reference_pose.y = east_pos; + reference_pose.z = down_pos; + Eigen::Quaterniond q = vortex::utils::math::euler_to_quat( + roll_angle, pitch_angle, yaw_angle); + reference_pose.qw = q.w(); + reference_pose.qx = q.x(); + reference_pose.qy = q.y(); + reference_pose.qz = q.z(); + return reference_pose; + } + + types::Nu generate_current_velocity(const double surge_vel, + const double sway_vel, + const double heave_vel, + const double roll_rate, + const double pitch_rate, + const double yaw_rate) { + types::Nu current_velocity; + current_velocity.u = surge_vel; + current_velocity.v = sway_vel; + current_velocity.w = heave_vel; + current_velocity.p = roll_rate; + current_velocity.q = pitch_rate; + current_velocity.r = yaw_rate; + return current_velocity; + } + + PIDController pid_controller_; +}; + +/* +Test that negative north error only (in body) gives positive surge command only. +*/ + +TEST_F(PIDControllerTests, + T01_neg_north_error_with_zero_heading_gives_surge_only_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(10.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + print_tau(tau); + + EXPECT_GT(tau[0], 0.0); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative north error with positive heading gives a positive surge +command and negative sway command. +*/ + +TEST_F( + PIDControllerTests, + T02_neg_north_error_with_positive_heading_gives_pos_surge_and_neg_sway_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 1.5)}; + types::Eta eta_d{generate_reference_pose(10.0, 0.0, 0.0, 0.0, 0.0, 1.5)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + print_tau(tau); + EXPECT_GT(tau[0], 0.0); + EXPECT_LT(tau[1], 0.0); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative north error with negative heading gives a positive surge +command and positive sway command. +*/ + +TEST_F( + PIDControllerTests, + T03_neg_north_error_with_negative_heading_gives_pos_surge_and_pos_sway_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, -1.5)}; + types::Eta eta_d{generate_reference_pose(10.0, 0.0, 0.0, 0.0, 0.0, -1.5)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + print_tau(tau); + + EXPECT_GT(tau[0], 0.0); + EXPECT_GT(tau[1], 0.0); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative down error with zero roll and pitch gives a positive heave +command. +*/ + +TEST_F( + PIDControllerTests, + T04_neg_down_error_with_zero_roll_and_pitch_gives_positive_heave_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 0.0, 2.0, 0.0, 0.0, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + print_tau(tau); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_GT(tau[2], 0.0); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative down error with zero roll and negative pitch gives a positive +heave and positive surge command. +*/ + +TEST_F( + PIDControllerTests, + T05_neg_down_error_with_zero_roll_and_neg_pitch_gives_positive_heave_and_positive_surge_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, -0.5, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 0.0, 2.0, 0.0, -0.5, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + print_tau(tau); + + EXPECT_GT(tau[0], 0.0); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_GT(tau[2], 0.0); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative down error with zero roll and positive pitch gives a positive +heave and negative surge command. +*/ + +TEST_F( + PIDControllerTests, + T06_neg_down_error_with_zero_roll_and_pos_pitch_gives_positive_heave_and_negative_surge_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.5, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 0.0, 2.0, 0.0, 0.5, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + print_tau(tau); + + EXPECT_LT(tau[0], 0.0); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_GT(tau[2], 0.0); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative east error with zero heading gives a positive sway command. +*/ + +TEST_F(PIDControllerTests, + T07_neg_east_error_with_zero_heading_gives_positive_sway_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 10.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + print_tau(tau); + + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_GT(tau[1], 0.0); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that positive east error with zero heading gives a negative sway command. +*/ + +TEST_F(PIDControllerTests, + T08_pos_east_error_with_zero_heading_gives_pos_sway_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, -10.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + print_tau(tau); + + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_LT(tau[1], 0.0); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative east error with positive heading gives a positive surge and +sway command. +*/ + +TEST_F( + PIDControllerTests, + T09_neg_east_error_with_positive_heading_gives_pos_sway_and_pos_surge_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 1.5)}; + types::Eta eta_d{generate_reference_pose(0.0, 10.0, 0.0, 0.0, 0.0, 1.5)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + EXPECT_GT(tau[0], 0.0); + EXPECT_GT(tau[1], 0.0); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative east error with negative heading gives a negative surge and +positive sway command. +*/ + +TEST_F( + PIDControllerTests, + T10_neg_east_error_with_negative_heading_gives_pos_sway_and_neg_surge_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, -1.5)}; + types::Eta eta_d{generate_reference_pose(0.0, 10.0, 0.0, 0.0, 0.0, -1.5)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + print_tau(tau); + + EXPECT_LT(tau[0], 0.0); + EXPECT_GT(tau[1], 0.0); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative roll error gives positive roll command. +*/ + +TEST_F(PIDControllerTests, T11_neg_roll_error_gives_positive_roll_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 1.0, 0.0, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + print_tau(tau); + + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_GT(tau[3], 0.0); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that positive roll error gives negative roll command. +*/ + +TEST_F(PIDControllerTests, T12_pos_roll_error_gives_neg_roll_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, -1.0, 0.0, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_LT(tau[3], 0.0); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative pitch error gives positive pitch command. +*/ + +TEST_F(PIDControllerTests, T13_neg_pitch_error_gives_pos_pitch_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 1.0, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_GT(tau[4], 0.0); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that positive pitch error gives negative pitch command. +*/ + +TEST_F(PIDControllerTests, T14_pos_pitch_error_gives_neg_pitch_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, -1.0, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_LT(tau[4], 0.0); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative yaw error gives positive yaw command. +*/ + +TEST_F(PIDControllerTests, T15_neg_yaw_error_gives_pos_yaw_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, 1.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + print_tau(tau); + + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_GT(tau[5], 0.0); +} + +/* +Test that positive yaw error gives negative yaw command. +*/ + +TEST_F(PIDControllerTests, T16_pos_yaw_error_gives_neg_yaw_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, -1.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + print_tau(tau); + + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_LT(tau[5], 0.0); +} + +/* +Test that positive surge velocity only results in negative surge command +(breaking effect). +*/ + +TEST_F(PIDControllerTests, T17_pos_surge_vel_gives_negative_surge_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(1.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + EXPECT_LT(tau[0], 0.0); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that positive sway velocity only results in negative sway command (breaking +effect). +*/ + +TEST_F(PIDControllerTests, T18_pos_sway_vel_gives_negative_sway_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 1.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_LT(tau[1], 0.0); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that positive heave velocity only results in negative heave command +(breaking effect). +*/ + +TEST_F(PIDControllerTests, T19_pos_heave_vel_gives_negative_heave_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 1.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_LT(tau[2], 0.0); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} diff --git a/mission/joystick_interface_auv/joystick_interface_auv/joystick_interface_auv_node.py b/mission/joystick_interface_auv/joystick_interface_auv/joystick_interface_auv_node.py index d50f83355..5d4a51443 100755 --- a/mission/joystick_interface_auv/joystick_interface_auv/joystick_interface_auv_node.py +++ b/mission/joystick_interface_auv/joystick_interface_auv/joystick_interface_auv_node.py @@ -1,5 +1,6 @@ #!/usr/bin/env python3 +import numpy as np import rclpy from geometry_msgs.msg import PoseWithCovarianceStamped, WrenchStamped from rclpy.node import Node, Parameter @@ -8,11 +9,11 @@ from vortex_msgs.msg import OperationMode, ReferenceFilter from vortex_msgs.srv import GetOperationMode, SetOperationMode, ToggleKillswitch from vortex_utils.python_utils import PoseData +from vortex_utils_ros.ros_converter import pose_from_ros from vortex_utils_ros.qos_profiles import ( reliable_profile, sensor_data_profile, ) -from vortex_utils_ros.ros_converter import pose_from_ros from joystick_interface_auv.joystick_utils import ( Wired, @@ -219,7 +220,8 @@ def create_reference_message(self) -> ReferenceFilter: """Creates a reference message with the desired state values.""" reference_msg = ReferenceFilter() reference_msg.header.stamp = self.get_clock().now().to_msg() - reference_msg.header.frame_id = "odom" + # reference_msg.header.frame_id = "odom" + reference_msg.header.frame_id = "base_link" reference_msg.x = self._desired_state.x reference_msg.y = self._desired_state.y reference_msg.z = self._desired_state.z @@ -390,10 +392,22 @@ def update_reference(self): The position and orientation (roll, pitch, yaw) are updated using the current joystick inputs scaled by their respective parameters. + The linear velocities (surge, sway, heave) are transformed from the + body frame to the world frame using the current orientation. """ - self._desired_state.x += self.surge * self._guidance_surge_gain - self._desired_state.y += self.sway * self._guidance_sway_gain - self._desired_state.z -= self.heave * self._guidance_heave_gain + surge_vector = self.surge * self._guidance_surge_gain + sway_vector = self.sway * self._guidance_sway_gain + heave_vector = -self.heave * self._guidance_heave_gain + + body_frame_vector = np.array([surge_vector, sway_vector, heave_vector]) + + rotation_matrix = self._desired_state.as_rotation_matrix() + world_frame_vector = rotation_matrix @ body_frame_vector + + self._desired_state.x += world_frame_vector[0] + self._desired_state.y += world_frame_vector[1] + self._desired_state.z += world_frame_vector[2] + self._desired_state.roll += self.roll * self._guidance_roll_gain self._desired_state.pitch += self.pitch * self._guidance_pitch_gain self._desired_state.yaw += self.yaw * self._guidance_yaw_gain