From 461f317dfac5fb7df8c9020f37246b9f56063267 Mon Sep 17 00:00:00 2001 From: Yunong_LI Date: Thu, 14 Nov 2024 20:19:11 +0900 Subject: [PATCH] [Navigation][Trajectory] solve the discontinued problem of yaw rotation --- .../polynomial_trajectory.hpp | 1 + .../src/flight_navigation.cpp | 5 ++++- .../polynomial_trajectory.cpp | 20 ++++++++++++++----- 3 files changed, 20 insertions(+), 6 deletions(-) diff --git a/aerial_robot_control/include/aerial_robot_control/trajectory/trajectory_reference/polynomial_trajectory.hpp b/aerial_robot_control/include/aerial_robot_control/trajectory/trajectory_reference/polynomial_trajectory.hpp index 4f198071d..187f4cb4d 100644 --- a/aerial_robot_control/include/aerial_robot_control/trajectory/trajectory_reference/polynomial_trajectory.hpp +++ b/aerial_robot_control/include/aerial_robot_control/trajectory/trajectory_reference/polynomial_trajectory.hpp @@ -57,6 +57,7 @@ class PolynomialTrajectory : public ReferenceBase { PolyType y_; PolyType z_; PolyType yaw_; + QuadState prev_constraint_; std::vector states_; diff --git a/aerial_robot_control/src/flight_navigation.cpp b/aerial_robot_control/src/flight_navigation.cpp index c6cc14f1f..f08395219 100644 --- a/aerial_robot_control/src/flight_navigation.cpp +++ b/aerial_robot_control/src/flight_navigation.cpp @@ -1002,7 +1002,10 @@ void BaseNavigator::generateNewTrajectory(geometry_msgs::PoseStamped pose) } double du_tran = (end_state.p - start_state.p).norm() / trajectory_mean_vel_; - double du_rot = fabs(end_state.getYaw() - start_state.getYaw()) / trajectory_mean_yaw_rate_; + double delta_yaw = end_state.getYaw() - start_state.getYaw(); + if (delta_yaw > M_PI) delta_yaw -= 2 * M_PI; + if (delta_yaw < -M_PI) delta_yaw += 2 * M_PI; + double du_rot = fabs(delta_yaw) / trajectory_mean_yaw_rate_; double du = std::max(du_tran, trajectory_min_du_); if (!enable_latch_yaw_trajectory_) { diff --git a/aerial_robot_control/src/trajectory/trajectory_reference/polynomial_trajectory.cpp b/aerial_robot_control/src/trajectory/trajectory_reference/polynomial_trajectory.cpp index 2b1112ef3..92af636bd 100644 --- a/aerial_robot_control/src/trajectory/trajectory_reference/polynomial_trajectory.cpp +++ b/aerial_robot_control/src/trajectory/trajectory_reference/polynomial_trajectory.cpp @@ -75,12 +75,10 @@ PolynomialTrajectory::PolynomialTrajectory( yaw_.scale(start_state_.t, duration_); addStateConstraint(start_state_); - addStateConstraint(end_state_); - for (size_t i = 1; i < states_.size() - 1; i++) { addStateConstraint(states_.at(i), 0); // only consider the position constraints for the intermediate points } - + addStateConstraint(end_state_); if (!x_.solve()) std::cout << "Could not solve x-axis!" << std::endl; if (!y_.solve()) std::cout << "Could not solve y-axis!" << std::endl; @@ -116,9 +114,21 @@ bool PolynomialTrajectory::addStateConstraint(const QuadState& state, z_.addConstraint(state.t, constraints.row(2).transpose()); // ToDo: Yaw relative to initial yaw. - const Scalar yaw_angle = state.getYaw(); + Scalar yaw_angle = state.getYaw(); if (std::isfinite(yaw_angle)) - yaw_.addConstraint(state.t, Vector<3>(yaw_angle, state.w.z(), 0)); + { + if (std::isfinite(prev_constraint_.t)) + { + Scalar prev_yaw_angle = prev_constraint_.getYaw(); + Scalar diff = yaw_angle - prev_yaw_angle; + if (diff > M_PI) yaw_angle -= 2 * M_PI; + if (diff < -M_PI) yaw_angle += 2 * M_PI; + } + yaw_.addConstraint(state.t, Vector<3>(yaw_angle, state.w.z(), 0)); + } + + + prev_constraint_ = state; return true; }