[Navigation][Trajectory] solve the discontinued problem of yaw rotation

jsk-ros-pkg · Nov 14, 2024 · 461f317 · 461f317
1 parent 71d746f
commit 461f317
Show file tree

Hide file tree

Showing 3 changed files with 20 additions and 6 deletions.
diff --git a/...ol/include/aerial_robot_control/trajectory/trajectory_reference/polynomial_trajectory.hpp b/...ol/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<QuadState> states_;
 

diff --git 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
@@ -75,12 +75,10 @@ PolynomialTrajectory<PolyType>::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<PolyType>::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;
 }