Line data    Source code

       1             : #ifndef TRAJECTORY_UTILS_H
       2             : #define TRAJECTORY_UTILS_H
       3             : 
       4             : #include <mav_trajectory_generation/polynomial_optimization_nonlinear.h>
       5             : #include <mav_trajectory_generation/segment.h>
       6             : #include <mav_trajectory_generation/trajectory.h>
       7             : #include <mav_trajectory_generation/vertex.h>
       8             : 
       9             : #include <Eigen/Core>
      10             : #include <optional>
      11             : #include <vector>
      12             : 
      13             : #include "utils/NodeUtils.h"
      14             : 
      15             : namespace mav_trajectory_generation {
      16             : 
      17             : /**
      18             :  * @brief Plans a minimum-derivative polynomial path through a sequence of distinct waypoints
      19             :  *
      20             :  * Constructs a nonlinear polynomial optimisation problem with velocity and
      21             :  * acceleration magnitude constraints, solves it, and scales segment times to
      22             :  * guarantee the constraints are met.  The starting velocity is constrained to
      23             :  * start_vel; the endpoint has all derivatives up to DerivativeToOptimize
      24             :  * constrained to zero by makeStartOrEnd().
      25             :  *
      26             :  * @tparam DerivativeToOptimize Derivative order to minimise (e.g. ACCELERATION)
      27             :  *
      28             :  * @param waypoints  [in] Ordered waypoints; must contain at least two distinct points
      29             :  * @param start_vel  [in] Initial velocity vector of the same dimension as the waypoints
      30             :  * @param v_max      [in] Maximum velocity magnitude constraint
      31             :  * @param a_max      [in] Maximum acceleration magnitude constraint
      32             :  *
      33             :  * @return Planned trajectory segments, or nullopt if optimisation fails
      34             :  */
      35             : template <int DerivativeToOptimize>
      36           1 : std::optional<Segment::Vector> optimize_path(const std::vector<Eigen::VectorXd>& waypoints,
      37             :                                              const Eigen::VectorXd& start_vel, double v_max, double a_max) {
      38             :     const int dimension = start_vel.size();
      39             :     constexpr int N     = 2 * (DerivativeToOptimize + 1);
      40             : 
      41             :     Vertex::Vector vertices;
      42             :     for (size_t i = 0; i < waypoints.size(); ++i) {
      43             :         Vertex vertex(dimension);
      44             :         if (i == 0) {
      45             :             vertex.makeStartOrEnd(waypoints[i], DerivativeToOptimize);
      46             :             vertex.addConstraint(derivative_order::VELOCITY, start_vel);
      47             :         } else if (i == waypoints.size() - 1) {
      48             :             vertex.makeStartOrEnd(waypoints[i], DerivativeToOptimize);
      49             :         } else {
      50             :             vertex.addConstraint(derivative_order::POSITION, waypoints[i]);
      51             :         }
      52             :         vertices.push_back(vertex);
      53             :     }
      54             : 
      55             :     std::vector<double> segment_times = estimateSegmentTimes(vertices, v_max, a_max);
      56             : 
      57             :     NonlinearOptimizationParameters parameters;
      58             :     PolynomialOptimizationNonLinear<N> opt(dimension, parameters);
      59             :     if (!opt.setupFromVertices(vertices, segment_times, DerivativeToOptimize)) {
      60             :         RCLCPP_WARN(Controller::NodeUtils::get_logger(), "[TRAJ PLANNER] setupFromVertices failed");
      61             :         return std::nullopt;
      62             :     }
      63             : 
      64             :     opt.addMaximumMagnitudeConstraint(derivative_order::VELOCITY, v_max);
      65             :     opt.addMaximumMagnitudeConstraint(derivative_order::ACCELERATION, a_max);
      66             : 
      67             :     int result = opt.optimize();
      68             :     if (result < 0 && result != nlopt::ROUNDOFF_LIMITED) {
      69             :         RCLCPP_WARN(Controller::NodeUtils::get_logger(), "[TRAJ PLANNER] nlopt optimization failed: %s",
      70             :                     nlopt::returnValueToString(result).c_str());
      71             :         // TODO: need to fix error handling in mav_trajectory_generation itself
      72             :         // currently it just try catches and reports -1 (nlopt::FAILURE) even if the error is something recoverable like
      73             :         // nlopt::ROUNDOFF_LIMITED (which is the error usually)
      74             : 
      75             :         // do not block return for now
      76             :         // return std::nullopt;
      77             :     }
      78             : 
      79             :     Trajectory trajectory;
      80             :     opt.getTrajectory(&trajectory);
      81             : 
      82             :     if (!trajectory.scaleSegmentTimesToMeetConstraints(v_max, a_max)) {
      83             :         RCLCPP_WARN(Controller::NodeUtils::get_logger(), "[TRAJ PLANNER] scaleSegmentTimesToMeetConstraints failed");
      84             :         return std::nullopt;
      85             :     }
      86             : 
      87             :     Segment::Vector segments;
      88             :     trajectory.getSegments(&segments);
      89             :     return segments;
      90             : }
      91             : 
      92             : /**
      93             :  * @brief Creates a zero-velocity constant-position polynomial segment
      94             :  *
      95             :  * @tparam DerivativeToOptimize Derivative order used to size the polynomial (must match the rest of the trajectory)
      96             :  *
      97             :  * @param value    [in] Position value to hold for the duration of the segment
      98             :  * @param duration [in] Segment duration in seconds
      99             :  *
     100             :  * @return Constant polynomial segment of the requested duration
     101             :  */
     102             : template <int DerivativeToOptimize>
     103           1 : Segment get_constant_segment(const Eigen::VectorXd& value, double duration) {
     104             :     constexpr int N     = 2 * (DerivativeToOptimize + 1);
     105             :     const int dimension = value.size();
     106             : 
     107             :     Segment seg(N, dimension);
     108             :     seg.setTime(duration);
     109             : 
     110             :     for (int d = 0; d < dimension; ++d) {
     111             :         Eigen::VectorXd coeffs = Eigen::VectorXd::Zero(N);
     112             :         coeffs(0)              = value(d);
     113             :         seg[d].setCoefficients(coeffs);
     114             :     }
     115             : 
     116             :     return seg;
     117             : }
     118             : 
     119             : /**
     120             :  * @brief Plans a trajectory through waypoints, inserting constant segments at duplicate points
     121             :  *
     122             :  * Consecutive identical waypoints are replaced by a constant-position segment
     123             :  * of duration min_segment_time.  Runs of distinct waypoints are planned with
     124             :  * optimize_path().  Resulting segments are concatenated into a single Trajectory.
     125             :  *
     126             :  * @tparam DerivativeToOptimize Derivative order to minimise
     127             :  *
     128             :  * @param waypoints         [in] Ordered waypoints; must contain at least two entries
     129             :  * @param start_vel         [in] Initial velocity vector
     130             :  * @param v_max             [in] Maximum velocity magnitude constraint
     131             :  * @param a_max             [in] Maximum acceleration magnitude constraint
     132             :  * @param min_segment_time  [in] Duration (s) of constant segments inserted at duplicate waypoints
     133             :  *
     134             :  * @return Combined trajectory, or nullopt if planning fails for any sub-path
     135             :  */
     136             : template <int DerivativeToOptimize>
     137           1 : std::optional<Trajectory> calculate_trajectory(const std::vector<Eigen::VectorXd>& waypoints,
     138             :                                                const Eigen::VectorXd& start_vel, double v_max, double a_max,
     139             :                                                double min_segment_time) {
     140             :     if (waypoints.size() < 2) {
     141             :         return std::nullopt;
     142             :     }
     143             : 
     144             :     const int dimension = start_vel.size();
     145             :     Segment::Vector all_segments;
     146             : 
     147             :     Eigen::VectorXd current_vel = start_vel;
     148             : 
     149             :     size_t i = 0;
     150             :     while (i < waypoints.size() - 1) {
     151             :         if (waypoints[i] == waypoints[i + 1]) {
     152             :             all_segments.push_back(get_constant_segment<DerivativeToOptimize>(waypoints[i], min_segment_time));
     153             :             current_vel = Eigen::VectorXd::Zero(dimension);
     154             :             ++i;
     155             :         } else {
     156             :             std::vector<Eigen::VectorXd> path;
     157             :             path.push_back(waypoints[i]);
     158             :             while (i < waypoints.size() - 1 && waypoints[i] != waypoints[i + 1]) {
     159             :                 path.push_back(waypoints[i + 1]);
     160             :                 ++i;
     161             :             }
     162             : 
     163             :             auto segments = optimize_path<DerivativeToOptimize>(path, current_vel, v_max, a_max);
     164             :             if (!segments) {
     165             :                 return std::nullopt;
     166             :             }
     167             : 
     168             :             all_segments.insert(all_segments.end(), segments->begin(), segments->end());
     169             :             current_vel = Eigen::VectorXd::Zero(dimension);
     170             :         }
     171             :     }
     172             : 
     173             :     if (all_segments.empty()) {
     174             :         return std::nullopt;
     175             :     }
     176             : 
     177             :     Trajectory trajectory;
     178             :     trajectory.setSegments(all_segments);
     179             :     return trajectory;
     180             : }
     181             : 
     182             : /**
     183             :  * @brief Describes a contiguous slice of trajectory dimensions sharing a common velocity/acceleration budget
     184             :  */
     185           1 : struct DimensionGroup {
     186             :     /** @brief Index of the first dimension in this group */
     187             :     int start;
     188             : 
     189             :     /** @brief Number of dimensions in this group */
     190             :     int size;
     191             : 
     192             :     /** @brief Maximum velocity magnitude for this group */
     193             :     double v_max;
     194             : 
     195             :     /** @brief Maximum acceleration magnitude for this group */
     196             :     double a_max;
     197             : };
     198             : 
     199             : /**
     200             :  * @brief Plans a multi-DOF trajectory by solving each dimension group independently and concatenating the results
     201             :  *
     202             :  * Splits the M-dimensional waypoints into sub-problems defined by groups,
     203             :  * calls calculate_trajectory on each sub-problem, then appends all resulting
     204             :  * trajectories along the dimension axis.  This allows different axes (e.g. XY
     205             :  * vs Z vs Yaw) to have independent kinematic budgets.
     206             :  *
     207             :  * @tparam DerivativeToOptimize Derivative order to minimise
     208             :  *
     209             :  * @param waypoints         [in] M-dimensional waypoints; must contain at least two entries
     210             :  * @param start_vel         [in] M-dimensional initial velocity
     211             :  * @param groups            [in] Dimension groups that together cover all M dimensions
     212             :  * @param min_segment_time  [in] Duration (s) inserted at duplicate waypoints
     213             :  *
     214             :  * @return Combined multi-DOF trajectory, or nullopt if any group fails
     215             :  */
     216             : template <int DerivativeToOptimize>
     217           1 : std::optional<Trajectory> calculate_trajectory_multidim(const std::vector<Eigen::VectorXd>& waypoints,
     218             :                                                         const Eigen::VectorXd& start_vel,
     219             :                                                         const std::vector<DimensionGroup>& groups,
     220             :                                                         double min_segment_time) {
     221             :     if (waypoints.size() < 2 || groups.empty()) {
     222             :         return std::nullopt;
     223             :     }
     224             : 
     225             :     std::optional<Trajectory> combined;
     226             : 
     227             :     for (const auto& group : groups) {
     228             :         std::vector<Eigen::VectorXd> sub_waypoints;
     229             :         sub_waypoints.reserve(waypoints.size());
     230             :         for (const auto& wp : waypoints) {
     231             :             sub_waypoints.push_back(wp.segment(group.start, group.size));
     232             :         }
     233             : 
     234             :         Eigen::VectorXd sub_vel = start_vel.segment(group.start, group.size);
     235             : 
     236             :         auto traj = calculate_trajectory<DerivativeToOptimize>(sub_waypoints, sub_vel, group.v_max, group.a_max,
     237             :                                                                min_segment_time);
     238             :         if (!traj) {
     239             :             return std::nullopt;
     240             :         }
     241             : 
     242             :         if (!combined) {
     243             :             combined = *traj;
     244             :         } else {
     245             :             Trajectory appended;
     246             :             if (!combined->getTrajectoryWithAppendedDimension(*traj, &appended)) {
     247             :                 return std::nullopt;
     248             :             }
     249             :             combined = appended;
     250             :         }
     251             :     }
     252             : 
     253             :     return combined;
     254             : }
     255             : 
     256             : }  // namespace mav_trajectory_generation
     257             : 
     258             : #endif  // TRAJECTORY_UTILS_H