trajectory.cpp - master - Logrus/CopterControl

trajectory.cpp
510 строк18 KB
Nikolay Nosorev
Initial commit
04 окт 2025, 16:17
04 окт 2025, 16:1799ef28c
#include "pch.h"
#include "trajectory.h"
//#include <nlopt.hpp>
//#include <cmath>
//#include <iostream>

// Структура для передачи данных в целевую функцию
struct ObjectiveData {
    std::vector<double> Q;
    int n;
};

// Структура для передачи данных в ограничения
struct EqualityConstraintData {
    std::vector<double> A_row;
    double b_val;
};
#ifdef __cplusplus
extern "C" {
#endif
// Целевая функция: 0.5 * x^T Q x
double objective_func(unsigned n, const double* x, double* grad, void* f_data) {
    ObjectiveData* data = static_cast<ObjectiveData*>(f_data);
    double f = 0.0;
    
    // Вычисляем 0.5 * x^T Q x
    for (unsigned i = 0; i < n; ++i) {
        for (unsigned j = 0; j < n; ++j) {
            f += x[i] * data->Q[i * n + j] * x[j];
        }
    }
    f *= 0.5;

    // Вычисляем градиент: grad = Q * x
    if (grad) {
        for (unsigned i = 0; i < n; ++i) {
            grad[i] = 0.0;
            for (unsigned j = 0; j < n; ++j) {
                grad[i] += data->Q[i * n + j] * x[j];
            }
        }
    }
    return f;
}
#ifdef __cplusplus
}
#endif

// Функция для ограничений: A_row * x - b_val = 0
double equality_constraint(unsigned n, const double* x, double* grad, void* f_data) {
    EqualityConstraintData* data = static_cast<EqualityConstraintData*>(f_data);
    double val = -data->b_val;
    
    for (unsigned j = 0; j < n; ++j) {
        val += data->A_row[j] * x[j];
    }
    
    if (grad) {
        for (unsigned j = 0; j < n; ++j) {
            grad[j] = data->A_row[j];
        }
    }
    return val;
}

MinimumSnapTrajectory::MinimumSnapTrajectory(
    const std::vector<Eigen::Vector3d>& waypoints_,
    bool auto_time_estimation,
    bool simple
) : waypoints(waypoints_) 
{
    if (auto_time_estimation) {
        t_points = auto_estimate_t_points();
    }
    
    n_seg = waypoints.size() - 1;
    order = 5;  // 5-й степень полинома
    coeffs_per_seg = order + 1;  // 6 коэффициентов

    if (simple) {
        _compute_coefficients();
    } else {
        _solve_optimization();
    }
}
void MinimumSnapTrajectory::_solve_optimization_test(){
std::cout << "Test case: minimize x^2\n";
for (size_t i = 1; i < t_points.size(); ++i) {
    double T = t_points[i] - t_points[i-1];
    std::cout << "Segment " << i-1 << ": T = " << T << std::endl;
    if (T <= 1e-6) {
        std::cerr << "WARNING: Very small time segment!" << std::endl;
    }
}
try {
    //nlopt::opt opt(nlopt::LD_SLSQP, 1);
    nlopt::opt opt(nlopt::LD_SLSQP, 1);
    opt.set_min_objective([](const std::vector<double>& x, std::vector<double>& grad, void*) {
        if (!grad.empty()) {
            std::cout << "🔧 Evaluating gradient at x=" << x[0] << "\n";
            grad[0] = 2*x[0];
        }
        double f = x[0]*x[0];
        std::cout << "🔧 f(" << x[0] << ") = " << f << "\n";
        return f;
    }, nullptr);

    std::vector<double> x = {5.0};
    double minf;

    opt.set_maxeval(1000);
    opt.set_ftol_rel(1e-4);
    opt.set_xtol_rel(1e-4);
    // opt.set_maxtime(5.0);  // может не работать в старых версиях

    std::cout << "🔍 Before optimize...\n";
    nlopt::result result = opt.optimize(x, minf);
    std::cout << "✅ After optimize\n";  // ← этого мы не видим

    std::cout << "Result: " << nlopt_result_to_string(static_cast<nlopt_result>(result))
              << ", x=" << x[0] << ", f=" << minf << "\n";

} catch (const std::exception& e) {
    std::cerr << "❌ C++ exception: " << e.what() << "\n";
} catch (...) {
    std::cerr << "❌ Unknown exception (possibly SEH)\n";
}
}
void MinimumSnapTrajectory::_solve_optimization() {
    int n_seg = this->n_seg;
    int n_coeffs = this->coeffs_per_seg;
    int total_coeffs = n_seg * n_coeffs;

    // === Формируем МАТРИЦУ ОГРАНИЧЕНИЙ A_eq (одинаковая для всех осей) ===
    std::vector<std::vector<double>> A_eq_base;
    
    // Ограничения позиций (без b_eq)
    for (int i = 0; i < static_cast<int>(waypoints.size()); ++i) {
        if (i < n_seg) {  // Начало сегмента
            double T_seg = t_points[i+1] - t_points[i];
            std::vector<double> row(total_coeffs, 0.0);
            int segment_start = i * n_coeffs;
            auto poly_vec = _poly_vector(0, 0, n_coeffs);
            for (int j = 0; j < n_coeffs; ++j) {
                row[segment_start + j] = poly_vec[j];
            }
            A_eq_base.push_back(row);
        }

        if (i > 0) {  // Конец предыдущего сегмента
            double T_seg_prev = t_points[i] - t_points[i-1];
            std::vector<double> row(total_coeffs, 0.0);
            int segment_start = (i-1) * n_coeffs;
            auto poly_vec = _poly_vector(1, 0, n_coeffs);
            for (int j = 0; j < n_coeffs; ++j) {
                row[segment_start + j] = poly_vec[j];
            }
            A_eq_base.push_back(row);
        }
    }

    // Непрерывность производных
    for (int i = 1; i < n_seg; ++i) {
        double T_prev = t_points[i] - t_points[i-1];
        double T_next = t_points[i+1] - t_points[i];
        
        for (int deriv = 1; deriv <= 3; ++deriv) {
            std::vector<double> row(total_coeffs, 0.0);
            
            int seg_prev_start = (i-1) * n_coeffs;
            auto poly_vec_prev = _poly_vector(1, deriv, n_coeffs);
            for (int j = 0; j < n_coeffs; ++j) {
                row[seg_prev_start + j] = poly_vec_prev[j] / std::pow(T_prev, deriv);
            }
            
            int seg_next_start = i * n_coeffs;
            auto poly_vec_next = _poly_vector(0, deriv, n_coeffs);
            for (int j = 0; j < n_coeffs; ++j) {
                row[seg_next_start + j] -= poly_vec_next[j] / std::pow(T_next, deriv);
            }
            
            A_eq_base.push_back(row);
        }
    }

    // Граничные условия — начальные
    double T0 = t_points[1] - t_points[0];
    for (int deriv = 1; deriv <= 3; ++deriv) {
        std::vector<double> row(total_coeffs, 0.0);
        auto poly_vec = _poly_vector(0, deriv, n_coeffs);
        for (int j = 0; j < n_coeffs; ++j) {
            row[j] = poly_vec[j] / std::pow(T0, deriv);
        }
        A_eq_base.push_back(row);
    }

    // Граничные условия — конечные
    double Tn = t_points.back() - t_points[t_points.size()-2];
    for (int deriv = 1; deriv <= 3; ++deriv) {
        std::vector<double> row(total_coeffs, 0.0);
        auto poly_vec = _poly_vector(1, deriv, n_coeffs);
        for (int j = 0; j < n_coeffs; ++j) {
            row[total_coeffs - n_coeffs + j] = poly_vec[j] / std::pow(Tn, deriv);
        }
        A_eq_base.push_back(row);
    }

    // === Формируем целевую функцию (одинаковая для всех осей) ===
    std::vector<double> Q(total_coeffs * total_coeffs, 0.0);
    _build_snap_hessian(Q, n_seg, n_coeffs);

    // === Решаем для каждой оси отдельно ===
    coeffs.resize(3);
    for (int axis = 0; axis < 3; ++axis) {
        // Формируем b_eq для текущей оси
        std::vector<double> b_eq;
        for (int i = 0; i < static_cast<int>(waypoints.size()); ++i) {
            double val = (axis == 0) ? waypoints[i].x() :
                         (axis == 1) ? waypoints[i].y() :
                                       waypoints[i].z();
            if (i < n_seg) {
                b_eq.push_back(val);
            }
            if (i > 0) {
                b_eq.push_back(val);
            }
        }

        // Непрерывность производных → 0
        int n_cont = (n_seg - 1) * 3;
        for (int i = 0; i < n_cont; ++i) {
            b_eq.push_back(0.0);
        }

        // Граничные условия → 0
        for (int i = 0; i < 6; ++i) { // 3 начальных + 3 конечных
            b_eq.push_back(0.0);
        }

        // Проверка: A_eq_base.size() == b_eq.size()
        if (A_eq_base.size() != b_eq.size()) {
            std::cerr << "ERROR: A_eq and b_eq size mismatch!" << std::endl;
            return;
        }

        // === Решаем оптимизацию для этой оси ===
        nlopt::opt opt(nlopt::LD_SLSQP, total_coeffs);
        ObjectiveData obj_data;
        obj_data.Q = Q;
        obj_data.n = total_coeffs;
        opt.set_min_objective(objective_func, &obj_data);

        std::vector<EqualityConstraintData*> constraint_datas;
        for (size_t i = 0; i < A_eq_base.size(); ++i) {
            EqualityConstraintData* data = new EqualityConstraintData;
            data->A_row = A_eq_base[i];
            data->b_val = b_eq[i];
            constraint_datas.push_back(data);
            opt.add_equality_constraint(equality_constraint, data, 1e-4);
        }

        std::vector<double> x0(total_coeffs, 0.0);
        opt.set_maxeval(500);
        opt.set_ftol_rel(2e-4);
        opt.set_xtol_rel(2e-4);
        double minf;
        try {
            opt.optimize(x0, minf);
        } catch (const std::exception& e) {
            std::cerr << "NLopt failed for axis " << axis << ": " << e.what() << std::endl;
        }

        // Сохраняем результат для этой оси
        coeffs[axis].resize(n_seg);
        for (int seg = 0; seg < n_seg; ++seg) {
            coeffs[axis][seg] = std::vector<double>(
                x0.begin() + seg * n_coeffs,
                x0.begin() + (seg + 1) * n_coeffs
            );
        }

        // Освобождаем память
        for (auto* data : constraint_datas) {
            delete data;
        }
    }
}
void MinimumSnapTrajectory::_build_snap_hessian(
    std::vector<double>& Q, 
    int n_seg, 
    int n_coeffs
) const 
{
    int total_coeffs = n_seg * n_coeffs;
    
    for (int seg = 0; seg < n_seg; ++seg) {
        double T = t_points[seg+1] - t_points[seg];
        int start_idx = seg * n_coeffs;
        
        for (int i = 0; i < n_coeffs; ++i) {
            for (int j = 0; j < n_coeffs; ++j) {
                if (i >= 4 && j >= 4) {
                    // Коэффициенты для 4-й производной
                    double coeff_i = i * (i-1) * (i-2) * (i-3);
                    double coeff_j = j * (j-1) * (j-2) * (j-3);
                    
                    // Интеграл от tau^(i-4) * tau^(j-4) = tau^(i+j-8)
                    int power = i + j - 7;
                    double integral = 1.0 / (power + 1);  // Так как power >= 1
                    
                    Q[(start_idx + i) * total_coeffs + (start_idx + j)] = 
                        coeff_i * coeff_j * integral / std::pow(T,i + j - 7);
                }
            }
        }
    }
}

std::vector<double> MinimumSnapTrajectory::_poly_vector(
    double tau, 
    int derivative_order, 
    int n_coeffs
) const 
{
    std::vector<double> vec(n_coeffs, 0.0);
    for (int i = derivative_order; i < n_coeffs; ++i) {
        double coef = 1.0;
        for (int j = i - derivative_order + 1; j <= i; ++j) {
            coef *= j;
        }
        vec[i] = coef * std::pow(tau, i - derivative_order);
    }
    return vec;
}

TrajectoryPoint MinimumSnapTrajectory::evaluate(double t) const {
    // Найти сегмент
    int i;
    double tau;
    if (t <= t_points[0]) {
        i = 0;
        tau = 0;
    } else if (t >= t_points.back()) {
        i = n_seg - 1;
        tau = 1;
    } else {
        auto it = std::upper_bound(t_points.begin(), t_points.end(), t);
        i = std::distance(t_points.begin(), it) - 1;
        double T = t_points[i+1] - t_points[i];
        tau = (t - t_points[i]) / T;
    }

    double T_seg = t_points[i+1] - t_points[i];

    Eigen::Vector3d pos = Eigen::Vector3d::Zero();
    Eigen::Vector3d vel = Eigen::Vector3d::Zero();
    Eigen::Vector3d acc = Eigen::Vector3d::Zero();

    for (int d = 0; d < 3; ++d) {
        const auto& c = coeffs[d][i]; // std::vector<double>

        for (int j = 0; j < coeffs_per_seg; ++j) {
            pos[d] += c[j] * std::pow(tau, j);
            if (j >= 1)
                vel[d] += j * c[j] * std::pow(tau, j-1);
            if (j >= 2)
                acc[d] += j * (j-1) * c[j] * std::pow(tau, j-2);
        }

        vel[d] /= T_seg;
        acc[d] /= (T_seg * T_seg);
    }

    double speed_xy = std::sqrt(vel.x()*vel.x() + vel.y()*vel.y());
    double psi = (speed_xy > 1e-3) ? std::atan2(vel.y(), vel.x()) : 0.0;

    return {pos, vel, acc, psi};
}

std::vector<double> MinimumSnapTrajectory::auto_estimate_t_points() const {
    std::vector<double> t_points;
    t_points.push_back(0.0);
    double cumulative_time = 0.0;

    constexpr double v_max = 4.0;     // максимальная скорость (м/с)
    constexpr double a_max = 6.0;     // максимальное ускорение (м/с²)
    constexpr double safety_margin = 1.6;

    for (size_t i = 0; i < waypoints.size() - 1; ++i) {
        Eigen::Vector3d start = waypoints[i];
        Eigen::Vector3d end = waypoints[i + 1];
        double distance = (end - start).norm();

        double t_segment;
        if (distance > (v_max * v_max) / a_max) {
            // Траектория "ограничена скоростью": разгон -> крейсерская -> торможение
            t_segment = (distance / v_max + v_max / a_max) * safety_margin;
        } else {
            // Траектория "ограничена ускорением": треугольный профиль скорости
            t_segment = (2.0 * std::sqrt(distance / a_max)) * safety_margin;
        }

        cumulative_time += t_segment;
        t_points.push_back(cumulative_time);
    }

    return t_points;
}

void MinimumSnapTrajectory::_compute_coefficients() {
    int n = n_seg;
    int coeffs_per_seg = this->coeffs_per_seg;
    int total_coeffs = n * coeffs_per_seg;

    // Создаём матрицу A и вектор b
    Eigen::MatrixXd A = Eigen::MatrixXd::Zero(total_coeffs, total_coeffs);
    Eigen::MatrixXd b = Eigen::MatrixXd::Zero(total_coeffs, 3);  // X, Y, Z

    int row = 0;

    // Условия на позиции в контрольных точках
    for (int i = 0; i <= n; ++i) {
        double tau = (i == 0) ? 0.0 : 1.0;
        int seg_idx = (i == 0) ? 0 : i - 1;

        auto poly_vec = _poly_vector(tau, 0, coeffs_per_seg);

        if (i < n) {  // Начало сегмента
            for (int j = 0; j < coeffs_per_seg; ++j) {
                A(row, seg_idx * coeffs_per_seg + j) = poly_vec[j];
            }
            b.row(row) = waypoints[i].transpose();
            row++;
        }

        if (i > 0) {  // Конец предыдущего сегмента
            for (int j = 0; j < coeffs_per_seg; ++j) {
                A(row, (i-1) * coeffs_per_seg + j) = poly_vec[j];
            }
            b.row(row) = waypoints[i].transpose();
            row++;
        }
    }

    // Непрерывность скорости и ускорения между сегментами
    for (int i = 1; i < n; ++i) {
        double T_prev = t_points[i] - t_points[i-1];
        double T_next = t_points[i+1] - t_points[i];

        for (int deriv = 1; deriv <= 2; ++deriv) {  // скорость и ускорение
            auto vec_end_prev = _poly_vector(1.0, deriv, coeffs_per_seg);
            auto vec_start_next = _poly_vector(0.0, deriv, coeffs_per_seg);

            double scale_prev = 1.0 / std::pow(T_prev, deriv);
            double scale_next = 1.0 / std::pow(T_next, deriv);

            for (int j = 0; j < coeffs_per_seg; ++j) {
                A(row, (i-1)*coeffs_per_seg + j) = vec_end_prev[j] * scale_prev;
                A(row, i*coeffs_per_seg + j) = -vec_start_next[j] * scale_next;
            }
            row++;
        }
    }

    // Граничные условия: нулевая начальная и конечная скорость и ускорение
    double T0 = t_points[1] - t_points[0];
    for (int deriv = 1; deriv <= 2; ++deriv) {
        auto vec = _poly_vector(0.0, deriv, coeffs_per_seg);
        double scale = 1.0 / std::pow(T0, deriv);
        for (int j = 0; j < coeffs_per_seg; ++j) {
            A(row, j) = vec[j] * scale;
        }
        row++;
    }

    double Tn = t_points[n] - t_points[n-1];
    for (int deriv = 1; deriv <= 2; ++deriv) {
        auto vec = _poly_vector(1.0, deriv, coeffs_per_seg);
        double scale = 1.0 / std::pow(Tn, deriv);
        for (int j = 0; j < coeffs_per_seg; ++j) {
            A(row, (n-1)*coeffs_per_seg + j) = vec[j] * scale;
        }
        row++;
    }

    // Решаем систему для каждой координаты
    coeffs.resize(3);  // x, y, z
    for (int d = 0; d < 3; ++d) {
        coeffs[d].resize(n);
        Eigen::VectorXd solution = A.colPivHouseholderQr().solve(b.col(d));

        for (int seg = 0; seg < n; ++seg) {
            coeffs[d][seg].resize(coeffs_per_seg);
            for (int j = 0; j < coeffs_per_seg; ++j) {
                coeffs[d][seg][j] = solution[seg * coeffs_per_seg + j];
            }
        }
    }
}
std::string nlopt_result_to_string(nlopt::result result) {
    switch (result) {
        case nlopt::SUCCESS: return "SUCCESS";
        case nlopt::STOPVAL_REACHED: return "STOPVAL_REACHED";
        case nlopt::FTOL_REACHED: return "FTOL_REACHED";
        case nlopt::XTOL_REACHED: return "XTOL_REACHED";
        case nlopt::MAXEVAL_REACHED: return "MAXEVAL_REACHED";
        case nlopt::MAXTIME_REACHED: return "MAXTIME_REACHED";
        default: return "UNKNOWN (" + std::to_string(result) + ")";
    }
}