git_ws/mpc_control/src/mpc_two_vehicle/MonitorMPC.cpp

#include "MonitorMPC.h"
#include <cppad/cppad.hpp>
#include <cppad/ipopt/solve.hpp>
#include "Eigen/Core"
#include "Eigen/QR"

using CppAD::AD;

// Set the timestep length and duration
// Currently tuned to predict 1 second worth
size_t N = 10;


// The solver takes all the state variables and actuator
// variables in a singular vector. Thus, we should to establish
// when one variable starts and another ends to make our lifes easier.
size_t nx = 0;
size_t ny = nx + N;
size_t nth = ny + N;
size_t nv = nth + N;
size_t nvth = nv + N;


class FG_eval {
public:
    // Fitted polynomial coefficients
    Eigen::VectorXd coeffs;
    double m_ref_v,m_dt;
    double m_v,m_vth;
    FG_eval(Eigen::VectorXd coeffs,double v,double vth,double ref_v,double dt) {
        this->coeffs = coeffs;
        m_v=v;
        m_vth=vth;
        m_ref_v=ref_v;
        m_dt=dt;
    }

    typedef CPPAD_TESTVECTOR(AD<double>) ADvector;
    void operator()(ADvector& fg, const ADvector& vars) {
        // Implementing MPC below
        // `fg` a vector of the cost constraints, `vars` is a vector of variable values (state & actuators)
        // The cost is stored is the first element of `fg`.
        // Any additions to the cost should be added to `fg[0]`.
        fg[0] = 0;
        double dt=m_dt;
        // Reference State Cost
        // Below defines the cost related the reference state and
        // any anything you think may be beneficial.

        // Weights for how "important" each cost is - can be tuned
        const int y_cost_weight = 1500;
        const int th_cost_weight = 1000;
        const int v_cost_weight = 20;
        const int vth_cost_weight = 100;
        const int a_cost_weight = 1;
        const int ath_cost_weight=100;

        // Cost for CTE, psi error and velocity
        for (int t = 0; t < N; t++) {
            AD<double> xt=vars[nx+t];
            AD<double> fx = coeffs[0] + coeffs[1] * xt + coeffs[2] * pow(xt, 2) + coeffs[3] * pow(xt, 3);
            fg[0] += y_cost_weight * CppAD::pow(vars[ny + t]-fx, 2);

            AD<double> fth = CppAD::atan(coeffs[1] + 2*coeffs[2]*xt + 3*coeffs[3]*pow(xt,2));
            fg[0] += th_cost_weight * CppAD::pow(vars[nth + t]-fth, 2);

            fg[0]+=v_cost_weight*CppAD::pow(vars[nv+t]-m_ref_v,2);

        }

        // Costs for steering (delta) and acceleration (a)
        for (int t = 0; t < N-1; t++) {
            fg[0] += vth_cost_weight * CppAD::pow(vars[nvth + t], 2);
            fg[0] += a_cost_weight * CppAD::pow(vars[nv + t+1]-vars[nv+t], 2);
            fg[0] += ath_cost_weight * CppAD::pow(vars[nvth + t+1]-vars[nvth+t], 2);
        }

        /////////////////////
        fg[1 + nx] = vars[nx]-vars[nv]*dt;
        fg[1 + ny] = vars[ny];
        fg[1 + nth] = vars[nth]-vars[nvth]*dt;

        // The rest of the constraints
        for (int t = 1; t < N; t++) {
            // State at time t + 1
            AD<double> x1 = vars[nx + t];
            AD<double> y1 = vars[ny + t];
            AD<double> th1 = vars[nth + t];
            AD<double> v1 = vars[nv + t];
            AD<double> vth1 = vars[nvth + t];

            // State at time t
            AD<double> x0 = vars[nx + t -1];
            AD<double> y0 = vars[ny + t -1];
            AD<double> th0 = vars[nth + t-1];
            AD<double> v0 = vars[nv + t-1];
            AD<double> vth0 = vars[nvth + t-1];

            // Actuator constraints at time t only

            // Setting up the rest of the model constraints
            fg[1 + nx + t] = x1 - (x0 + v0 * CppAD::cos(th0) * dt);
            fg[1 + ny + t] = y1 - (y0 + v0 * CppAD::sin(th0) * dt);
            fg[1 + nth + t] = th1 - (th0 + vth0 * dt);

        }

        fg[1 + nv]=(vars[nv]-m_v)/dt;
        fg[1+nvth]=(vars[nvth]-m_vth)/dt;
        for(int t=1;t<N;++t)
        {
            fg[1+nv+t]=(vars[nv+t]-vars[nv+t-1])/dt;
            fg[1+nvth+t]=(vars[nvth+t]-vars[nvth+t-1])/dt;
        }

        ////  add

    }
};

//
// MPC class definition implementation.
//
MonitorMPC::MonitorMPC() {}
MonitorMPC::~MonitorMPC() {}

bool MonitorMPC::Solve(Eigen::VectorXd state, Eigen::VectorXd coeffs, std::vector<double>& out) {
    bool ok = true;
    typedef CPPAD_TESTVECTOR(double) Dvector;

    // State vector holds all current values neede for vars below
    double v = state[0];
    double vth = state[1];
    double ref_v = state[2];
    double max_v=state[3];
    double dt=state[4];


    // Setting the number of model variables (includes both states and inputs).
    // N * state vector size + (N - 1) * 2 actuators (For steering & acceleration)
    size_t n_vars = N * 5;
    // Setting the number of constraints
    size_t n_constraints = N * 5;

    // Initial value of the independent variables.
    // SHOULD BE 0 besides initial state.
    Dvector vars(n_vars);
    for (int i = 0; i < n_vars; i++) {
        vars[i] = 0.0;
    }

    Dvector vars_lowerbound(n_vars);
    Dvector vars_upperbound(n_vars);
    // Sets lower and upper limits for variables.
    // Set all non-actuators upper and lowerlimits
    // to the max negative and positive values.
    for (int i = 0; i < n_vars; i++) {
        vars_lowerbound[i] = -1.0e19;
        vars_upperbound[i] = 1.0e19;
    }

    //// limit  v
    for (int i = nv; i < nv+N; i++) {
        vars_lowerbound[i] = -max_v;
        vars_upperbound[i] = max_v;
    }

    ////limint vth
    for (int i = nvth; i < nvth+N; i++) {
        vars_lowerbound[i] = -0.5;
        vars_upperbound[i] = 0.5;
    }

    // Lower and upper limits for the constraints
    // Should be 0 besides initial state.
    Dvector constraints_lowerbound(n_constraints);
    Dvector constraints_upperbound(n_constraints);
    for (int i = 0; i < n_constraints; i++) {
        constraints_lowerbound[i] = 0;
        constraints_upperbound[i] = 0;
    }
    //// acc v
    for (int i = nv; i < nv+N; i++) {
        constraints_lowerbound[i] = -5.0;
        constraints_upperbound[i] = 5.0;
    }
    //// acc vth
    for (int i = nvth; i < nvth+N; i++) {
        constraints_lowerbound[i] = -2.5;
        constraints_upperbound[i] = 2.5;
    }


    // object that computes objective and constraints
    FG_eval fg_eval(coeffs,v,vth,ref_v,dt);

    //
    // NOTE: You don't have to worry about these options
    //
    // options for IPOPT solver
    std::string options;
    // Uncomment this if you'd like more print information
    options += "Integer print_level  0\n";
    // NOTE: Setting sparse to true allows the solver to take advantage
    // of sparse routines, this makes the computation MUCH FASTER. If you
    // can uncomment 1 of these and see if it makes a difference or not but
    // if you uncomment both the computation time should go up in orders of
    // magnitude.
    options += "Sparse  true        forward\n";
    options += "Sparse  true        reverse\n";
    // NOTE: Currently the solver has a maximum time limit of 0.5 seconds.
    // Change this as you see fit.
    options += "Numeric max_cpu_time          0.5\n";

    // place to return solution
    CppAD::ipopt::solve_result<Dvector> solution;

    // solve the problem
    CppAD::ipopt::solve<Dvector, FG_eval>(
        options, vars, vars_lowerbound, vars_upperbound, constraints_lowerbound,
        constraints_upperbound, fg_eval, solution);

    // Check some of the solution values
    ok &= solution.status == CppAD::ipopt::solve_result<Dvector>::success;

    // Cost
    auto cost = solution.obj_value;
    //std::cout << "Cost " << cost << std::endl;

    // Return the first actuator values, along with predicted x and y values to plot in the simulator.
    out.clear();
    m_trajectory.clear();
    out.push_back(solution.x[nv]);
    out.push_back(solution.x[nvth]);
    for (int i = 0; i < N; ++i) {
        out.push_back(solution.x[nx + i]);
        out.push_back(solution.x[ny + i]);
        tf::Pose pose;
        pose.setOrigin(tf::Vector3(solution.x[nx+i],solution.x[ny+i],0));
        m_trajectory.push_back(pose);
    }
    printf(" cost : %.3f\n",cost);
    return ok;

}