TrajectoryUtils.h

Go to the documentation of this file.

#ifndef TRAJECTORY_UTILS_H
#define TRAJECTORY_UTILS_H
 
#include <mav_trajectory_generation/polynomial_optimization_nonlinear.h>
#include <mav_trajectory_generation/segment.h>
#include <mav_trajectory_generation/trajectory.h>
#include <mav_trajectory_generation/vertex.h>
 
#include <Eigen/Core>
#include <optional>
#include <vector>
 
#include "utils/NodeUtils.h"
 
namespace mav_trajectory_generation {
 
template <int DerivativeToOptimize>
std::optional<Segment::Vector> optimize_path(const std::vector<Eigen::VectorXd>& waypoints,
                                             const Eigen::VectorXd& start_vel, double v_max, double a_max) {
    const int dimension = start_vel.size();
    constexpr int N     = 2 * (DerivativeToOptimize + 1);
 
    Vertex::Vector vertices;
    for (size_t i = 0; i < waypoints.size(); ++i) {
        Vertex vertex(dimension);
        if (i == 0) {
            vertex.makeStartOrEnd(waypoints[i], DerivativeToOptimize);
            vertex.addConstraint(derivative_order::VELOCITY, start_vel);
        } else if (i == waypoints.size() - 1) {
            vertex.makeStartOrEnd(waypoints[i], DerivativeToOptimize);
        } else {
            vertex.addConstraint(derivative_order::POSITION, waypoints[i]);
        }
        vertices.push_back(vertex);
    }
 
    std::vector<double> segment_times = estimateSegmentTimes(vertices, v_max, a_max);
 
    NonlinearOptimizationParameters parameters;
    PolynomialOptimizationNonLinear<N> opt(dimension, parameters);
    if (!opt.setupFromVertices(vertices, segment_times, DerivativeToOptimize)) {
        RCLCPP_WARN(Controller::NodeUtils::get_logger(), "[TRAJ PLANNER] setupFromVertices failed");
        return std::nullopt;
    }
 
    opt.addMaximumMagnitudeConstraint(derivative_order::VELOCITY, v_max);
    opt.addMaximumMagnitudeConstraint(derivative_order::ACCELERATION, a_max);
 
    int result = opt.optimize();
    if (result < 0 && result != nlopt::ROUNDOFF_LIMITED) {
        RCLCPP_WARN(Controller::NodeUtils::get_logger(), "[TRAJ PLANNER] nlopt optimization failed: %s",
                    nlopt::returnValueToString(result).c_str());
        // TODO: need to fix error handling in mav_trajectory_generation itself
        // currently it just try catches and reports -1 (nlopt::FAILURE) even if the error is something recoverable like
        // nlopt::ROUNDOFF_LIMITED (which is the error usually)
 
        // do not block return for now
        // return std::nullopt;
    }
 
    Trajectory trajectory;
    opt.getTrajectory(&trajectory);
 
    if (!trajectory.scaleSegmentTimesToMeetConstraints(v_max, a_max)) {
        RCLCPP_WARN(Controller::NodeUtils::get_logger(), "[TRAJ PLANNER] scaleSegmentTimesToMeetConstraints failed");
        return std::nullopt;
    }
 
    Segment::Vector segments;
    trajectory.getSegments(&segments);
    return segments;
}
 
template <int DerivativeToOptimize>
Segment get_constant_segment(const Eigen::VectorXd& value, double duration) {
    constexpr int N     = 2 * (DerivativeToOptimize + 1);
    const int dimension = value.size();
 
    Segment seg(N, dimension);
    seg.setTime(duration);
 
    for (int d = 0; d < dimension; ++d) {
        Eigen::VectorXd coeffs = Eigen::VectorXd::Zero(N);
        coeffs(0)              = value(d);
        seg[d].setCoefficients(coeffs);
    }
 
    return seg;
}
 
template <int DerivativeToOptimize>
std::optional<Trajectory> calculate_trajectory(const std::vector<Eigen::VectorXd>& waypoints,
                                               const Eigen::VectorXd& start_vel, double v_max, double a_max,
                                               double min_segment_time) {
    if (waypoints.size() < 2) {
        return std::nullopt;
    }
 
    const int dimension = start_vel.size();
    Segment::Vector all_segments;
 
    Eigen::VectorXd current_vel = start_vel;
 
    size_t i = 0;
    while (i < waypoints.size() - 1) {
        if (waypoints[i] == waypoints[i + 1]) {
            all_segments.push_back(get_constant_segment<DerivativeToOptimize>(waypoints[i], min_segment_time));
            current_vel = Eigen::VectorXd::Zero(dimension);
            ++i;
        } else {
            std::vector<Eigen::VectorXd> path;
            path.push_back(waypoints[i]);
            while (i < waypoints.size() - 1 && waypoints[i] != waypoints[i + 1]) {
                path.push_back(waypoints[i + 1]);
                ++i;
            }
 
            auto segments = optimize_path<DerivativeToOptimize>(path, current_vel, v_max, a_max);
            if (!segments) {
                return std::nullopt;
            }
 
            all_segments.insert(all_segments.end(), segments->begin(), segments->end());
            current_vel = Eigen::VectorXd::Zero(dimension);
        }
    }
 
    if (all_segments.empty()) {
        return std::nullopt;
    }
 
    Trajectory trajectory;
    trajectory.setSegments(all_segments);
    return trajectory;
}
 
struct DimensionGroup {
    int start;
 
    int size;
 
    double v_max;
 
    double a_max;
};
 
template <int DerivativeToOptimize>
std::optional<Trajectory> calculate_trajectory_multidim(const std::vector<Eigen::VectorXd>& waypoints,
                                                        const Eigen::VectorXd& start_vel,
                                                        const std::vector<DimensionGroup>& groups,
                                                        double min_segment_time) {
    if (waypoints.size() < 2 || groups.empty()) {
        return std::nullopt;
    }
 
    std::optional<Trajectory> combined;
 
    for (const auto& group : groups) {
        std::vector<Eigen::VectorXd> sub_waypoints;
        sub_waypoints.reserve(waypoints.size());
        for (const auto& wp : waypoints) {
            sub_waypoints.push_back(wp.segment(group.start, group.size));
        }
 
        Eigen::VectorXd sub_vel = start_vel.segment(group.start, group.size);
 
        auto traj = calculate_trajectory<DerivativeToOptimize>(sub_waypoints, sub_vel, group.v_max, group.a_max,
                                                               min_segment_time);
        if (!traj) {
            return std::nullopt;
        }
 
        if (!combined) {
            combined = *traj;
        } else {
            Trajectory appended;
            if (!combined->getTrajectoryWithAppendedDimension(*traj, &appended)) {
                return std::nullopt;
            }
            combined = appended;
        }
    }
 
    return combined;
}
 
}  // namespace mav_trajectory_generation
 
#endif  // TRAJECTORY_UTILS_H