Refactor/type rename

control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller.hpp

@@ -23,15 +23,16 @@ class DPAdaptBacksController {
     explicit DPAdaptBacksController(const DPAdaptParams& dp_adapt_params);
 
     // @brief Calculate thecontrol input tau
-    // @param eta: 6D vector containing the vehicle pose [x, y, z, roll, pitch,
+    // @param pose: 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,
+    // @param pose_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]
+    // @param twist: 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]
-    Eigen::Vector6d calculate_tau(const vortex::utils::types::Eta& eta,
-                                  const vortex::utils::types::Eta& eta_d,
-                                  const vortex::utils::types::Nu& nu);
+    Eigen::Vector6d calculate_tau(const vortex::utils::types::PoseEuler& pose,
+                                  const vortex::utils::types::PoseEuler& pose_d,
+                                  const vortex::utils::types::Twist& twist);
 
     // @brief Reset the adaptive parameters
     void reset_adap_param();

control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_ros.hpp

@@ -80,11 +80,11 @@ class DPAdaptBacksControllerNode : public rclcpp::Node {
 
     std::chrono::milliseconds time_step_{};
 
-    vortex::utils::types::Eta eta_;
+    vortex::utils::types::PoseEuler pose_;
 
-    vortex::utils::types::Eta eta_d_;
+    vortex::utils::types::PoseEuler pose_d_;
 
-    vortex::utils::types::Nu nu_;
+    vortex::utils::types::Twist twist_;
 
     std::unique_ptr<DPAdaptBacksController> dp_adapt_backs_controller_{};
 

control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_utils.hpp

@@ -8,45 +8,49 @@
 namespace vortex::control {
 
 // @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]
+// @param pose: 6D vector containing the vehicle pose [x, y, z, roll, pitch,
+// yaw]
+// @param twist: 6D vector containing the vehicle velocity [u, v, w, p, q, r]
 // @return 3x3 derivative of the rotation matrix
-Eigen::Matrix3d calculate_R_dot(const vortex::utils::types::Eta& eta,
-                                const vortex::utils::types::Nu& nu);
+Eigen::Matrix3d calculate_R_dot(const vortex::utils::types::PoseEuler& pose,
+                                const vortex::utils::types::Twist& twist);
 
 // @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]
+// @param pose: 6D vector containing the vehicle pose [x, y, z, roll, pitch,
+// yaw]
+// @param twist: 6D vector containing the vehicle velocity [u, v, w, p, q, r]
 // @return 3x3 derivative of the transformation matrix
-Eigen::Matrix3d calculate_T_dot(const vortex::utils::types::Eta& eta,
-                                const vortex::utils::types::Nu& nu);
+Eigen::Matrix3d calculate_T_dot(const vortex::utils::types::PoseEuler& pose,
+                                const vortex::utils::types::Twist& twist);
 
 // @brief Calculate the pseudo-inverse of the Jacobian matrix
-// @param eta: 6D vector containing the vehicle pose [x, y, z, roll, pitch, yaw]
+// @param pose: 6D vector containing the vehicle pose [x, y, z, roll, pitch,
+// yaw]
 // @return 6x6 pseudo-inverse Jacobian matrix
-Eigen::Matrix6d calculate_J_inv(const vortex::utils::types::Eta& eta);
+Eigen::Matrix6d calculate_J_inv(const vortex::utils::types::PoseEuler& pose);
 
 // @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]
-Eigen::Matrix6d calculate_J_dot(const vortex::utils::types::Eta& eta,
-                                const vortex::utils::types::Nu& nu);
+// @param pose: 6D vector containing the vehicle pose [x, y, z, roll, pitch,
+// yaw]
+// @param twist: 6D vector containing the vehicle velocity [u, v, w, p, q, r]
+Eigen::Matrix6d calculate_J_dot(const vortex::utils::types::PoseEuler& pose,
+                                const vortex::utils::types::Twist& twist);
 
 // @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 twist: 6D vector containing linear and angular velocity of the vehicle
 // @param I_b : 3D matrix containing the inertia matrix
 // @return 6x6 coriolis matrix
 Eigen::Matrix6d calculate_coriolis(const double mass,
                                    const Eigen::Vector3d& r_b_bg,
-                                   const vortex::utils::types::Nu& nu,
+                                   const vortex::utils::types::Twist& twist,
                                    const Eigen::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]
+// @param twist: 6D vector containing the vehicle velocity [u, v, w, p, q, r]
 // @return 6x6 damping matrix
-Eigen::Matrix6x12d calculate_Y_v(const vortex::utils::types::Nu& nu);
+Eigen::Matrix6x12d calculate_Y_v(const vortex::utils::types::Twist& twist);
 
 }  // namespace vortex::control
 

control/dp_adapt_backs_controller/src/dp_adapt_backs_controller.cpp

@@ -7,8 +7,8 @@
 
 namespace vortex::control {
 
-using vortex::utils::types::Eta;
-using vortex::utils::types::Nu;
+using vortex::utils::types::PoseEuler;
+using vortex::utils::types::Twist;
 
 DPAdaptBacksController::DPAdaptBacksController(
     const DPAdaptParams& dp_adapt_params)
@@ -24,29 +24,29 @@ DPAdaptBacksController::DPAdaptBacksController(
       m_(dp_adapt_params.mass),
       dt_(0.01) {}
 
-Eigen::Vector6d DPAdaptBacksController::calculate_tau(const Eta& eta,
-                                                      const Eta& eta_d,
-                                                      const Nu& nu) {
-    Eta error = eta - eta_d;
+Eigen::Vector6d DPAdaptBacksController::calculate_tau(const PoseEuler& pose,
+                                                      const PoseEuler& pose_d,
+                                                      const Twist& twist) {
+    PoseEuler error = pose - pose_d;
     error.roll = vortex::utils::math::ssa(error.roll);
     error.pitch = vortex::utils::math::ssa(error.pitch);
     error.yaw = vortex::utils::math::ssa(error.yaw);
 
-    Eigen::Matrix6d C = calculate_coriolis(m_, r_b_bg_, nu, I_b_);
-    Eigen::Matrix6d J_inv = calculate_J_inv(eta);
-    Eigen::Matrix6d J_dot = calculate_J_dot(eta, nu);
+    Eigen::Matrix6d C = calculate_coriolis(m_, r_b_bg_, twist, I_b_);
+    Eigen::Matrix6d J_inv = calculate_J_inv(pose);
+    Eigen::Matrix6d J_dot = calculate_J_dot(pose, twist);
     Eigen::Vector6d alpha = -J_inv * K1_ * error.to_vector();
     Eigen::Vector6d z_1 = error.to_vector();
-    Eigen::Vector6d z_2 = nu.to_vector() - alpha;
+    Eigen::Vector6d z_2 = twist.to_vector() - alpha;
     Eigen::Vector6d alpha_dot =
         ((J_inv * J_dot * J_inv) * K1_ * z_1) -
-        (J_inv * K1_ * eta.as_j_matrix() * nu.to_vector());
-    Eigen::Matrix6x12d Y_v = calculate_Y_v(nu);
+        (J_inv * K1_ * pose.as_j_matrix() * twist.to_vector());
+    Eigen::Matrix6x12d Y_v = calculate_Y_v(twist);
     Eigen::Vector12d adapt_param_dot = adapt_gain_ * Y_v.transpose() * z_2;
     Eigen::Vector6d d_est_dot = d_gain_ * z_2;
     Eigen::Vector6d F_est = Y_v * adapt_param_;
-    Eigen::Vector6d tau = (mass_matrix_ * alpha_dot) + (C * nu.to_vector()) -
-                          (eta.as_j_matrix().transpose() * z_1) - (K2_ * z_2) -
+    Eigen::Vector6d tau = (mass_matrix_ * alpha_dot) + (C * twist.to_vector()) -
+                          (pose.as_j_matrix().transpose() * z_1) - (K2_ * z_2) -
                           F_est - d_est_;
 
     tau = tau.cwiseMax(-80.0).cwiseMin(80.0);

control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_ros.cpp

@@ -100,31 +100,31 @@ void DPAdaptBacksControllerNode::software_mode_callback(
     spdlog::info("Software mode: {}", software_mode_);
 
     if (software_mode_ == "autonomous mode") {
-        eta_d_ = eta_;
+        pose_d_ = pose_;
     }
 }
 
 void DPAdaptBacksControllerNode::pose_callback(
     const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) {
-    eta_.x = msg->pose.pose.position.x;
-    eta_.y = msg->pose.pose.position.y;
-    eta_.z = msg->pose.pose.position.z;
+    pose_.x = msg->pose.pose.position.x;
+    pose_.y = msg->pose.pose.position.y;
+    pose_.z = msg->pose.pose.position.z;
     const auto& o = msg->pose.pose.orientation;
     Eigen::Quaterniond q(o.w, o.x, o.y, o.z);
     Eigen::Vector3d euler_angles = vortex::utils::math::quat_to_euler(q);
-    eta_.roll = euler_angles(0);
-    eta_.pitch = euler_angles(1);
-    eta_.yaw = euler_angles(2);
+    pose_.roll = euler_angles(0);
+    pose_.pitch = euler_angles(1);
+    pose_.yaw = euler_angles(2);
 }
 
 void DPAdaptBacksControllerNode::twist_callback(
     const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) {
-    nu_.u = msg->twist.twist.linear.x;
-    nu_.v = msg->twist.twist.linear.y;
-    nu_.w = msg->twist.twist.linear.z;
-    nu_.p = msg->twist.twist.angular.x;
-    nu_.q = msg->twist.twist.angular.y;
-    nu_.r = msg->twist.twist.angular.z;
+    twist_.u = msg->twist.twist.linear.x;
+    twist_.v = msg->twist.twist.linear.y;
+    twist_.w = msg->twist.twist.linear.z;
+    twist_.p = msg->twist.twist.angular.x;
+    twist_.q = msg->twist.twist.angular.y;
+    twist_.r = msg->twist.twist.angular.z;
 }
 
 void DPAdaptBacksControllerNode::set_adap_params() {
@@ -185,7 +185,7 @@ void DPAdaptBacksControllerNode::publish_tau() {
     }
 
     Eigen::Vector6d tau =
-        dp_adapt_backs_controller_->calculate_tau(eta_, eta_d_, nu_);
+        dp_adapt_backs_controller_->calculate_tau(pose_, pose_d_, twist_);
 
     geometry_msgs::msg::WrenchStamped tau_msg;
     tau_msg.header.stamp = this->now();
@@ -203,12 +203,12 @@ void DPAdaptBacksControllerNode::publish_tau() {
 
 void DPAdaptBacksControllerNode::guidance_callback(
     const vortex_msgs::msg::ReferenceFilter::SharedPtr msg) {
-    eta_d_.x = msg->x;
-    eta_d_.y = msg->y;
-    eta_d_.z = msg->z;
-    eta_d_.roll = msg->roll;
-    eta_d_.pitch = msg->pitch;
-    eta_d_.yaw = msg->yaw;
+    pose_d_.x = msg->x;
+    pose_d_.y = msg->y;
+    pose_d_.z = msg->z;
+    pose_d_.roll = msg->roll;
+    pose_d_.pitch = msg->pitch;
+    pose_d_.yaw = msg->yaw;
 }
 
 RCLCPP_COMPONENTS_REGISTER_NODE(DPAdaptBacksControllerNode)

control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_utils.cpp

@@ -6,13 +6,13 @@
 
 namespace vortex::control {
 
-Eigen::Matrix6d calculate_J_inv(const vortex::utils::types::Eta& eta) {
-    Eigen::Matrix6d J = eta.as_j_matrix();
+Eigen::Matrix6d calculate_J_inv(const vortex::utils::types::PoseEuler& pose) {
+    Eigen::Matrix6d J = pose.as_j_matrix();
 
     constexpr double tolerance = 1e-8;
 
     if (std::abs(J.determinant()) < tolerance) {
-        spdlog::error("J(eta) is singular");
+        spdlog::error("J is singular");
 
         // Moore-Penrose pseudoinverse in case of near singular matrix, better
         // result for smaller singular values
@@ -22,26 +22,26 @@ Eigen::Matrix6d calculate_J_inv(const vortex::utils::types::Eta& eta) {
     return J.inverse();
 }
 
-Eigen::Matrix3d calculate_R_dot(const vortex::utils::types::Eta& eta,
-                                const vortex::utils::types::Nu& nu) {
-    return eta.as_rotation_matrix() *
+Eigen::Matrix3d calculate_R_dot(const vortex::utils::types::PoseEuler& pose,
+                                const vortex::utils::types::Twist& twist) {
+    return pose.as_rotation_matrix() *
            vortex::utils::math::get_skew_symmetric_matrix(
-               nu.to_vector().tail(3));
+               twist.to_vector().tail(3));
 }
 
-Eigen::Matrix3d calculate_T_dot(const vortex::utils::types::Eta& eta,
-                                const vortex::utils::types::Nu& nu) {
-    double cos_phi{std::cos(eta.roll)};
-    double sin_phi{std::sin(eta.roll)};
-    double cos_theta{std::cos(eta.pitch)};
-    double sin_theta{std::sin(eta.pitch)};
+Eigen::Matrix3d calculate_T_dot(const vortex::utils::types::PoseEuler& pose,
+                                const vortex::utils::types::Twist& twist) {
+    double cos_phi{std::cos(pose.roll)};
+    double sin_phi{std::sin(pose.roll)};
+    double cos_theta{std::cos(pose.pitch)};
+    double sin_theta{std::sin(pose.pitch)};
     double tan_theta{sin_theta / cos_theta};
     double inv_cos2{1.0 / (cos_theta * cos_theta)};
 
-    Eigen::Vector6d eta_dot = eta.as_j_matrix() * nu.to_vector();
+    Eigen::Vector6d pose_dot = pose.as_j_matrix() * twist.to_vector();
 
-    double phi_dot{eta_dot(3)};
-    double theta_dot{eta_dot(4)};
+    double phi_dot{pose_dot(3)};
+    double theta_dot{pose_dot(4)};
 
     Eigen::Matrix3d dt_dphi;
     dt_dphi << 0.0, cos_phi * tan_theta * phi_dot,
@@ -58,10 +58,10 @@ Eigen::Matrix3d calculate_T_dot(const vortex::utils::types::Eta& eta,
     return dt_dphi + dt_dtheta;
 }
 
-Eigen::Matrix6d calculate_J_dot(const vortex::utils::types::Eta& eta,
-                                const vortex::utils::types::Nu& nu) {
-    Eigen::Matrix3d R_dot = calculate_R_dot(eta, nu);
-    Eigen::Matrix3d T_dot = calculate_T_dot(eta, nu);
+Eigen::Matrix6d calculate_J_dot(const vortex::utils::types::PoseEuler& pose,
+                                const vortex::utils::types::Twist& twist) {
+    Eigen::Matrix3d R_dot = calculate_R_dot(pose, twist);
+    Eigen::Matrix3d T_dot = calculate_T_dot(pose, twist);
 
     Eigen::Matrix6d J_dot = Eigen::Matrix6d::Zero();
     J_dot.topLeftCorner<3, 3>() = R_dot;
@@ -72,11 +72,11 @@ Eigen::Matrix6d calculate_J_dot(const vortex::utils::types::Eta& eta,
 
 Eigen::Matrix6d calculate_coriolis(const double mass,
                                    const Eigen::Vector3d& r_b_bg,
-                                   const vortex::utils::types::Nu& nu,
+                                   const vortex::utils::types::Twist& twist,
                                    const Eigen::Matrix3d& I_b) {
     using vortex::utils::math::get_skew_symmetric_matrix;
-    Eigen::Vector3d linear_speed = nu.to_vector().head(3);
-    Eigen::Vector3d angular_speed = nu.to_vector().tail(3);
+    Eigen::Vector3d linear_speed = twist.to_vector().head(3);
+    Eigen::Vector3d angular_speed = twist.to_vector().tail(3);
     Eigen::Matrix6d C;
     C.topLeftCorner<3, 3>() =
         mass * vortex::utils::math::get_skew_symmetric_matrix(linear_speed);
@@ -93,27 +93,27 @@ Eigen::Matrix6d calculate_coriolis(const double mass,
     return C;
 }
 
-Eigen::Matrix6x12d calculate_Y_v(const vortex::utils::types::Nu& nu) {
+Eigen::Matrix6x12d calculate_Y_v(const vortex::utils::types::Twist& twist) {
     Eigen::Matrix6x12d Y_v;
     Y_v.setZero();
 
-    Y_v(0, 0) = nu.u;
-    Y_v(0, 1) = nu.u * std::abs(nu.u);
+    Y_v(0, 0) = twist.u;
+    Y_v(0, 1) = twist.u * std::abs(twist.u);
 
-    Y_v(1, 2) = nu.v;
-    Y_v(1, 3) = nu.v * std::abs(nu.v);
+    Y_v(1, 2) = twist.v;
+    Y_v(1, 3) = twist.v * std::abs(twist.v);
 
-    Y_v(2, 4) = nu.w;
-    Y_v(2, 5) = nu.w * std::abs(nu.w);
+    Y_v(2, 4) = twist.w;
+    Y_v(2, 5) = twist.w * std::abs(twist.w);
 
-    Y_v(3, 6) = nu.p;
-    Y_v(3, 7) = nu.p * std::abs(nu.p);
+    Y_v(3, 6) = twist.p;
+    Y_v(3, 7) = twist.p * std::abs(twist.p);
 
-    Y_v(4, 8) = nu.q;
-    Y_v(4, 9) = nu.q * std::abs(nu.q);
+    Y_v(4, 8) = twist.q;
+    Y_v(4, 9) = twist.q * std::abs(twist.q);
 
-    Y_v(5, 10) = nu.r;
-    Y_v(5, 11) = nu.r * std::abs(nu.r);
+    Y_v(5, 10) = twist.r;
+    Y_v(5, 11) = twist.r * std::abs(twist.r);
 
     return Y_v;
 }