Skip to content

Commit d72affd

Browse files
jorgenfjjorgenfj
authored
Refactor/type rename (#647)
* waypoint mode handling * update tests with new message * initial package setup * ros interface function declaration * node setup * working prototype * reentrant cb, multithreaded * single threaded impl * conv threshold action goal * default thresholdref conv value * removed switching logic * removed timer execution * sim test utils * waypoint_manager_test setup * no rendering test arg * waypoint tests, timeout error * test refactor * format * rename utils package * test suite and description * first waypoint test * removed unused function * renamed service field to priority. Added simple tests * waypoint manager readme * uniform attitude naming convention * fix pr requests * update tests with new service fields * four corner test * update util func name * update with new action def * removed failing build type * test dependencies * ignore failing yasmin package * remove __init__ files * quat_to_euler in make_pose helper * added __init__ file * removed sim deps for test packages * added action shutdown handling * removed waypoint manager set setup * added waypoint manager node tests * waypoint manager 4 corner sim test * added missing launch testing test dependency * add sleep for topic discovery * fix action member field name * updated to new utils type names * renamed variables to match types * update function arg to reflect vortex type * update variable name in tests * renamed function arg names
1 parent 007bb81 commit d72affd

9 files changed

Lines changed: 222 additions & 219 deletions

File tree

‎control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller.hpp‎

Lines changed: 7 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -23,15 +23,16 @@ class DPAdaptBacksController {
2323
explicit DPAdaptBacksController(const DPAdaptParams& dp_adapt_params);
2424

2525
// @brief Calculate thecontrol input tau
26-
// @param eta: 6D vector containing the vehicle pose [x, y, z, roll, pitch,
26+
// @param pose: 6D vector containing the vehicle pose [x, y, z, roll, pitch,
2727
// yaw]
28-
// @param eta_d: 6D vector containing the desired vehicle pose [x, y, z,
28+
// @param pose_d: 6D vector containing the desired vehicle pose [x, y, z,
2929
// roll, pitch, yaw]
30-
// @param nu: 6D vector containing the vehicle velocity [u, v, w, p, q, r]
30+
// @param twist: 6D vector containing the vehicle velocity [u, v, w, p, q,
31+
// r]
3132
// @return 6D vector containing the control input tau [X, Y, Z, K, M, N]
32-
Eigen::Vector6d calculate_tau(const vortex::utils::types::Eta& eta,
33-
const vortex::utils::types::Eta& eta_d,
34-
const vortex::utils::types::Nu& nu);
33+
Eigen::Vector6d calculate_tau(const vortex::utils::types::PoseEuler& pose,
34+
const vortex::utils::types::PoseEuler& pose_d,
35+
const vortex::utils::types::Twist& twist);
3536

3637
// @brief Reset the adaptive parameters
3738
void reset_adap_param();

‎control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_ros.hpp‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -80,11 +80,11 @@ class DPAdaptBacksControllerNode : public rclcpp::Node {
8080

8181
std::chrono::milliseconds time_step_{};
8282

83-
vortex::utils::types::Eta eta_;
83+
vortex::utils::types::PoseEuler pose_;
8484

85-
vortex::utils::types::Eta eta_d_;
85+
vortex::utils::types::PoseEuler pose_d_;
8686

87-
vortex::utils::types::Nu nu_;
87+
vortex::utils::types::Twist twist_;
8888

8989
std::unique_ptr<DPAdaptBacksController> dp_adapt_backs_controller_{};
9090

‎control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_utils.hpp‎

Lines changed: 22 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -8,45 +8,49 @@
88
namespace vortex::control {
99

1010
// @brief Calculate the derivative of the rotation matrix
11-
// @param eta: 6D vector containing the vehicle pose [x, y, z, roll, pitch, yaw]
12-
// @param nu: 6D vector containing the vehicle velocity [u, v, w, p, q, r]
11+
// @param pose: 6D vector containing the vehicle pose [x, y, z, roll, pitch,
12+
// yaw]
13+
// @param twist: 6D vector containing the vehicle velocity [u, v, w, p, q, r]
1314
// @return 3x3 derivative of the rotation matrix
14-
Eigen::Matrix3d calculate_R_dot(const vortex::utils::types::Eta& eta,
15-
const vortex::utils::types::Nu& nu);
15+
Eigen::Matrix3d calculate_R_dot(const vortex::utils::types::PoseEuler& pose,
16+
const vortex::utils::types::Twist& twist);
1617

1718
// @brief Calculate the derivative of the transformation matrix
18-
// @param eta: 6D vector containing the vehicle pose [x, y, z, roll, pitch, yaw]
19-
// @param nu: 6D vector containing the vehicle velocity [u, v, w, p, q, r]
19+
// @param pose: 6D vector containing the vehicle pose [x, y, z, roll, pitch,
20+
// yaw]
21+
// @param twist: 6D vector containing the vehicle velocity [u, v, w, p, q, r]
2022
// @return 3x3 derivative of the transformation matrix
21-
Eigen::Matrix3d calculate_T_dot(const vortex::utils::types::Eta& eta,
22-
const vortex::utils::types::Nu& nu);
23+
Eigen::Matrix3d calculate_T_dot(const vortex::utils::types::PoseEuler& pose,
24+
const vortex::utils::types::Twist& twist);
2325

2426
// @brief Calculate the pseudo-inverse of the Jacobian matrix
25-
// @param eta: 6D vector containing the vehicle pose [x, y, z, roll, pitch, yaw]
27+
// @param pose: 6D vector containing the vehicle pose [x, y, z, roll, pitch,
28+
// yaw]
2629
// @return 6x6 pseudo-inverse Jacobian matrix
27-
Eigen::Matrix6d calculate_J_inv(const vortex::utils::types::Eta& eta);
30+
Eigen::Matrix6d calculate_J_inv(const vortex::utils::types::PoseEuler& pose);
2831

2932
// @brief calculate the derivative of the Jacobian matrix
30-
// @param eta: 6D vector containing the vehicle pose [x, y, z, roll, pitch, yaw]
31-
// @param nu: 6D vector containing the vehicle velocity [u, v, w, p, q, r]
32-
Eigen::Matrix6d calculate_J_dot(const vortex::utils::types::Eta& eta,
33-
const vortex::utils::types::Nu& nu);
33+
// @param pose: 6D vector containing the vehicle pose [x, y, z, roll, pitch,
34+
// yaw]
35+
// @param twist: 6D vector containing the vehicle velocity [u, v, w, p, q, r]
36+
Eigen::Matrix6d calculate_J_dot(const vortex::utils::types::PoseEuler& pose,
37+
const vortex::utils::types::Twist& twist);
3438

3539
// @brief Calculate the coriolis matrix
3640
// @param m: mass of the vehicle
3741
// @param r_b_bg: 3D vector of the body frame to the center of gravity
38-
// @param nu_2: 3D vector containing angular velocity of the vehicle
42+
// @param twist: 6D vector containing linear and angular velocity of the vehicle
3943
// @param I_b : 3D matrix containing the inertia matrix
4044
// @return 6x6 coriolis matrix
4145
Eigen::Matrix6d calculate_coriolis(const double mass,
4246
const Eigen::Vector3d& r_b_bg,
43-
const vortex::utils::types::Nu& nu,
47+
const vortex::utils::types::Twist& twist,
4448
const Eigen::Matrix3d& I_b);
4549

4650
// @brief Calculate the damping matrix for the adaptive backstepping controller
47-
// @param nu: 6D vector containing the vehicle velocity [u, v, w, p, q, r]
51+
// @param twist: 6D vector containing the vehicle velocity [u, v, w, p, q, r]
4852
// @return 6x6 damping matrix
49-
Eigen::Matrix6x12d calculate_Y_v(const vortex::utils::types::Nu& nu);
53+
Eigen::Matrix6x12d calculate_Y_v(const vortex::utils::types::Twist& twist);
5054

5155
} // namespace vortex::control
5256

‎control/dp_adapt_backs_controller/src/dp_adapt_backs_controller.cpp‎

Lines changed: 14 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -7,8 +7,8 @@
77

88
namespace vortex::control {
99

10-
using vortex::utils::types::Eta;
11-
using vortex::utils::types::Nu;
10+
using vortex::utils::types::PoseEuler;
11+
using vortex::utils::types::Twist;
1212

1313
DPAdaptBacksController::DPAdaptBacksController(
1414
const DPAdaptParams& dp_adapt_params)
@@ -24,29 +24,29 @@ DPAdaptBacksController::DPAdaptBacksController(
2424
m_(dp_adapt_params.mass),
2525
dt_(0.01) {}
2626

27-
Eigen::Vector6d DPAdaptBacksController::calculate_tau(const Eta& eta,
28-
const Eta& eta_d,
29-
const Nu& nu) {
30-
Eta error = eta - eta_d;
27+
Eigen::Vector6d DPAdaptBacksController::calculate_tau(const PoseEuler& pose,
28+
const PoseEuler& pose_d,
29+
const Twist& twist) {
30+
PoseEuler error = pose - pose_d;
3131
error.roll = vortex::utils::math::ssa(error.roll);
3232
error.pitch = vortex::utils::math::ssa(error.pitch);
3333
error.yaw = vortex::utils::math::ssa(error.yaw);
3434

35-
Eigen::Matrix6d C = calculate_coriolis(m_, r_b_bg_, nu, I_b_);
36-
Eigen::Matrix6d J_inv = calculate_J_inv(eta);
37-
Eigen::Matrix6d J_dot = calculate_J_dot(eta, nu);
35+
Eigen::Matrix6d C = calculate_coriolis(m_, r_b_bg_, twist, I_b_);
36+
Eigen::Matrix6d J_inv = calculate_J_inv(pose);
37+
Eigen::Matrix6d J_dot = calculate_J_dot(pose, twist);
3838
Eigen::Vector6d alpha = -J_inv * K1_ * error.to_vector();
3939
Eigen::Vector6d z_1 = error.to_vector();
40-
Eigen::Vector6d z_2 = nu.to_vector() - alpha;
40+
Eigen::Vector6d z_2 = twist.to_vector() - alpha;
4141
Eigen::Vector6d alpha_dot =
4242
((J_inv * J_dot * J_inv) * K1_ * z_1) -
43-
(J_inv * K1_ * eta.as_j_matrix() * nu.to_vector());
44-
Eigen::Matrix6x12d Y_v = calculate_Y_v(nu);
43+
(J_inv * K1_ * pose.as_j_matrix() * twist.to_vector());
44+
Eigen::Matrix6x12d Y_v = calculate_Y_v(twist);
4545
Eigen::Vector12d adapt_param_dot = adapt_gain_ * Y_v.transpose() * z_2;
4646
Eigen::Vector6d d_est_dot = d_gain_ * z_2;
4747
Eigen::Vector6d F_est = Y_v * adapt_param_;
48-
Eigen::Vector6d tau = (mass_matrix_ * alpha_dot) + (C * nu.to_vector()) -
49-
(eta.as_j_matrix().transpose() * z_1) - (K2_ * z_2) -
48+
Eigen::Vector6d tau = (mass_matrix_ * alpha_dot) + (C * twist.to_vector()) -
49+
(pose.as_j_matrix().transpose() * z_1) - (K2_ * z_2) -
5050
F_est - d_est_;
5151

5252
tau = tau.cwiseMax(-80.0).cwiseMin(80.0);

‎control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_ros.cpp‎

Lines changed: 20 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -100,31 +100,31 @@ void DPAdaptBacksControllerNode::software_mode_callback(
100100
spdlog::info("Software mode: {}", software_mode_);
101101

102102
if (software_mode_ == "autonomous mode") {
103-
eta_d_ = eta_;
103+
pose_d_ = pose_;
104104
}
105105
}
106106

107107
void DPAdaptBacksControllerNode::pose_callback(
108108
const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) {
109-
eta_.x = msg->pose.pose.position.x;
110-
eta_.y = msg->pose.pose.position.y;
111-
eta_.z = msg->pose.pose.position.z;
109+
pose_.x = msg->pose.pose.position.x;
110+
pose_.y = msg->pose.pose.position.y;
111+
pose_.z = msg->pose.pose.position.z;
112112
const auto& o = msg->pose.pose.orientation;
113113
Eigen::Quaterniond q(o.w, o.x, o.y, o.z);
114114
Eigen::Vector3d euler_angles = vortex::utils::math::quat_to_euler(q);
115-
eta_.roll = euler_angles(0);
116-
eta_.pitch = euler_angles(1);
117-
eta_.yaw = euler_angles(2);
115+
pose_.roll = euler_angles(0);
116+
pose_.pitch = euler_angles(1);
117+
pose_.yaw = euler_angles(2);
118118
}
119119

120120
void DPAdaptBacksControllerNode::twist_callback(
121121
const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) {
122-
nu_.u = msg->twist.twist.linear.x;
123-
nu_.v = msg->twist.twist.linear.y;
124-
nu_.w = msg->twist.twist.linear.z;
125-
nu_.p = msg->twist.twist.angular.x;
126-
nu_.q = msg->twist.twist.angular.y;
127-
nu_.r = msg->twist.twist.angular.z;
122+
twist_.u = msg->twist.twist.linear.x;
123+
twist_.v = msg->twist.twist.linear.y;
124+
twist_.w = msg->twist.twist.linear.z;
125+
twist_.p = msg->twist.twist.angular.x;
126+
twist_.q = msg->twist.twist.angular.y;
127+
twist_.r = msg->twist.twist.angular.z;
128128
}
129129

130130
void DPAdaptBacksControllerNode::set_adap_params() {
@@ -185,7 +185,7 @@ void DPAdaptBacksControllerNode::publish_tau() {
185185
}
186186

187187
Eigen::Vector6d tau =
188-
dp_adapt_backs_controller_->calculate_tau(eta_, eta_d_, nu_);
188+
dp_adapt_backs_controller_->calculate_tau(pose_, pose_d_, twist_);
189189

190190
geometry_msgs::msg::WrenchStamped tau_msg;
191191
tau_msg.header.stamp = this->now();
@@ -203,12 +203,12 @@ void DPAdaptBacksControllerNode::publish_tau() {
203203

204204
void DPAdaptBacksControllerNode::guidance_callback(
205205
const vortex_msgs::msg::ReferenceFilter::SharedPtr msg) {
206-
eta_d_.x = msg->x;
207-
eta_d_.y = msg->y;
208-
eta_d_.z = msg->z;
209-
eta_d_.roll = msg->roll;
210-
eta_d_.pitch = msg->pitch;
211-
eta_d_.yaw = msg->yaw;
206+
pose_d_.x = msg->x;
207+
pose_d_.y = msg->y;
208+
pose_d_.z = msg->z;
209+
pose_d_.roll = msg->roll;
210+
pose_d_.pitch = msg->pitch;
211+
pose_d_.yaw = msg->yaw;
212212
}
213213

214214
RCLCPP_COMPONENTS_REGISTER_NODE(DPAdaptBacksControllerNode)

‎control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_utils.cpp‎

Lines changed: 36 additions & 36 deletions
Original file line numberDiff line numberDiff line change
@@ -6,13 +6,13 @@
66

77
namespace vortex::control {
88

9-
Eigen::Matrix6d calculate_J_inv(const vortex::utils::types::Eta& eta) {
10-
Eigen::Matrix6d J = eta.as_j_matrix();
9+
Eigen::Matrix6d calculate_J_inv(const vortex::utils::types::PoseEuler& pose) {
10+
Eigen::Matrix6d J = pose.as_j_matrix();
1111

1212
constexpr double tolerance = 1e-8;
1313

1414
if (std::abs(J.determinant()) < tolerance) {
15-
spdlog::error("J(eta) is singular");
15+
spdlog::error("J is singular");
1616

1717
// Moore-Penrose pseudoinverse in case of near singular matrix, better
1818
// result for smaller singular values
@@ -22,26 +22,26 @@ Eigen::Matrix6d calculate_J_inv(const vortex::utils::types::Eta& eta) {
2222
return J.inverse();
2323
}
2424

25-
Eigen::Matrix3d calculate_R_dot(const vortex::utils::types::Eta& eta,
26-
const vortex::utils::types::Nu& nu) {
27-
return eta.as_rotation_matrix() *
25+
Eigen::Matrix3d calculate_R_dot(const vortex::utils::types::PoseEuler& pose,
26+
const vortex::utils::types::Twist& twist) {
27+
return pose.as_rotation_matrix() *
2828
vortex::utils::math::get_skew_symmetric_matrix(
29-
nu.to_vector().tail(3));
29+
twist.to_vector().tail(3));
3030
}
3131

32-
Eigen::Matrix3d calculate_T_dot(const vortex::utils::types::Eta& eta,
33-
const vortex::utils::types::Nu& nu) {
34-
double cos_phi{std::cos(eta.roll)};
35-
double sin_phi{std::sin(eta.roll)};
36-
double cos_theta{std::cos(eta.pitch)};
37-
double sin_theta{std::sin(eta.pitch)};
32+
Eigen::Matrix3d calculate_T_dot(const vortex::utils::types::PoseEuler& pose,
33+
const vortex::utils::types::Twist& twist) {
34+
double cos_phi{std::cos(pose.roll)};
35+
double sin_phi{std::sin(pose.roll)};
36+
double cos_theta{std::cos(pose.pitch)};
37+
double sin_theta{std::sin(pose.pitch)};
3838
double tan_theta{sin_theta / cos_theta};
3939
double inv_cos2{1.0 / (cos_theta * cos_theta)};
4040

41-
Eigen::Vector6d eta_dot = eta.as_j_matrix() * nu.to_vector();
41+
Eigen::Vector6d pose_dot = pose.as_j_matrix() * twist.to_vector();
4242

43-
double phi_dot{eta_dot(3)};
44-
double theta_dot{eta_dot(4)};
43+
double phi_dot{pose_dot(3)};
44+
double theta_dot{pose_dot(4)};
4545

4646
Eigen::Matrix3d dt_dphi;
4747
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,
5858
return dt_dphi + dt_dtheta;
5959
}
6060

61-
Eigen::Matrix6d calculate_J_dot(const vortex::utils::types::Eta& eta,
62-
const vortex::utils::types::Nu& nu) {
63-
Eigen::Matrix3d R_dot = calculate_R_dot(eta, nu);
64-
Eigen::Matrix3d T_dot = calculate_T_dot(eta, nu);
61+
Eigen::Matrix6d calculate_J_dot(const vortex::utils::types::PoseEuler& pose,
62+
const vortex::utils::types::Twist& twist) {
63+
Eigen::Matrix3d R_dot = calculate_R_dot(pose, twist);
64+
Eigen::Matrix3d T_dot = calculate_T_dot(pose, twist);
6565

6666
Eigen::Matrix6d J_dot = Eigen::Matrix6d::Zero();
6767
J_dot.topLeftCorner<3, 3>() = R_dot;
@@ -72,11 +72,11 @@ Eigen::Matrix6d calculate_J_dot(const vortex::utils::types::Eta& eta,
7272

7373
Eigen::Matrix6d calculate_coriolis(const double mass,
7474
const Eigen::Vector3d& r_b_bg,
75-
const vortex::utils::types::Nu& nu,
75+
const vortex::utils::types::Twist& twist,
7676
const Eigen::Matrix3d& I_b) {
7777
using vortex::utils::math::get_skew_symmetric_matrix;
78-
Eigen::Vector3d linear_speed = nu.to_vector().head(3);
79-
Eigen::Vector3d angular_speed = nu.to_vector().tail(3);
78+
Eigen::Vector3d linear_speed = twist.to_vector().head(3);
79+
Eigen::Vector3d angular_speed = twist.to_vector().tail(3);
8080
Eigen::Matrix6d C;
8181
C.topLeftCorner<3, 3>() =
8282
mass * vortex::utils::math::get_skew_symmetric_matrix(linear_speed);
@@ -93,27 +93,27 @@ Eigen::Matrix6d calculate_coriolis(const double mass,
9393
return C;
9494
}
9595

96-
Eigen::Matrix6x12d calculate_Y_v(const vortex::utils::types::Nu& nu) {
96+
Eigen::Matrix6x12d calculate_Y_v(const vortex::utils::types::Twist& twist) {
9797
Eigen::Matrix6x12d Y_v;
9898
Y_v.setZero();
9999

100-
Y_v(0, 0) = nu.u;
101-
Y_v(0, 1) = nu.u * std::abs(nu.u);
100+
Y_v(0, 0) = twist.u;
101+
Y_v(0, 1) = twist.u * std::abs(twist.u);
102102

103-
Y_v(1, 2) = nu.v;
104-
Y_v(1, 3) = nu.v * std::abs(nu.v);
103+
Y_v(1, 2) = twist.v;
104+
Y_v(1, 3) = twist.v * std::abs(twist.v);
105105

106-
Y_v(2, 4) = nu.w;
107-
Y_v(2, 5) = nu.w * std::abs(nu.w);
106+
Y_v(2, 4) = twist.w;
107+
Y_v(2, 5) = twist.w * std::abs(twist.w);
108108

109-
Y_v(3, 6) = nu.p;
110-
Y_v(3, 7) = nu.p * std::abs(nu.p);
109+
Y_v(3, 6) = twist.p;
110+
Y_v(3, 7) = twist.p * std::abs(twist.p);
111111

112-
Y_v(4, 8) = nu.q;
113-
Y_v(4, 9) = nu.q * std::abs(nu.q);
112+
Y_v(4, 8) = twist.q;
113+
Y_v(4, 9) = twist.q * std::abs(twist.q);
114114

115-
Y_v(5, 10) = nu.r;
116-
Y_v(5, 11) = nu.r * std::abs(nu.r);
115+
Y_v(5, 10) = twist.r;
116+
Y_v(5, 11) = twist.r * std::abs(twist.r);
117117

118118
return Y_v;
119119
}

0 commit comments

Comments
 (0)