SHARE
TWEET

Untitled

a guest Jan 28th, 2020 85 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include "MPC.h"
  2. #include <cppad/cppad.hpp>
  3. #include <cppad/ipopt/solve.hpp>
  4. #include "Eigen-3.3/Eigen/Core"
  5.  
  6. using CppAD::AD;
  7.  
  8. #define CARWIDTH 3.5
  9. #define CARLENGTH 8.5
  10. #define GRANNYRADIUS 0.54
  11. #define BABYRADIUS 0.43
  12.  
  13. // TODO: Set the timestep length and duration
  14. size_t N = 15;
  15. //double dt = 0.05;
  16.  
  17. // This value assumes the model presented in the classroom is used.
  18. //
  19. // It was obtained by measuring the radius formed by running the vehicle in the
  20. // simulator around in a circle with a constant steering angle and velocity on a
  21. // flat terrain.
  22. //
  23. // Lf was tuned until the the radius formed by the simulating the model
  24. // presented in the classroom matched the previous radius.
  25. //
  26. // This is the length from front to CoG that has a similar radius.
  27. const double Lf = 2.184738;
  28.  
  29. //double ref_v = 50;
  30. size_t x_start = 0;
  31. size_t y_start = x_start + N;
  32. size_t psi_start = y_start + N;
  33. size_t v_start = psi_start + N;
  34. size_t cte_start = v_start + N;
  35. size_t epsi_start = cte_start + N;
  36. size_t delta_start = epsi_start + N;
  37. size_t a_start = delta_start + N - 1;
  38.  
  39. class FG_eval {
  40.  public:
  41.   // Fitted polynomial coefficients
  42.   Eigen::VectorXd coeffs;
  43.   std::vector<double > constants;
  44.   std::vector<double > grannies_x;
  45.   std::vector<double > grannies_y;
  46.   std::vector<double > babies_x;
  47.   std::vector<double > babies_y;
  48.   FG_eval(Eigen::VectorXd coeffs, std::vector<double> constants, std::vector<double> grannies_x, std::vector<double> grannies_y, std::vector<double> babies_x, std::vector<double> babies_y) { this->coeffs = coeffs; this->constants = constants; this->grannies_x = grannies_x; this->grannies_y = grannies_y; this->babies_x = babies_x; this->babies_y = babies_y;}
  49.  
  50.   typedef CPPAD_TESTVECTOR(AD<double>) ADvector;
  51.   void operator()(ADvector& fg, const ADvector& vars) {
  52.  
  53.     double ref_v = constants[2];
  54.     double dt = constants[0];
  55.  
  56.     // TODO: implement MPC
  57.     // `fg` a vector of the cost constraints, `vars` is a vector of variable values (state & actuators)
  58.     // NOTE: You'll probably go back and forth between this function and
  59.     // the Solver function below.
  60.     // The cost is stored is the first element of `fg`.
  61.     // Any additions to the cost should be added to `fg[0]`.
  62.     fg[0] = 0;
  63.  
  64.     // Reference State Cost
  65.     // TODO: Define the cost related the reference state and
  66.     // any anything you think may be beneficial.
  67.     for (int i = 0; i < N; i++) {
  68.       fg[0] += constants[3]*CppAD::pow(vars[cte_start + i], 2);  // Don't stray off the track
  69.       fg[0] += constants[4]*CppAD::pow(vars[epsi_start + i], 2); // Stay in same direction as track
  70.       fg[0] += constants[5]*CppAD::pow(vars[v_start + i] - ref_v, 2); // Don't drive away from the speed limit
  71.     }
  72.  
  73.     for (int i = 0; i < N - 1; i++) {
  74.       fg[0] += constants[6]*CppAD::pow(vars[delta_start + i], 2); // Don't steer too sharply
  75.       fg[0] += constants[7]*CppAD::pow(vars[a_start + i], 2);     // Don't accelerate too much
  76.       // try adding penalty for speed + steer
  77.       fg[0] += constants[8]*CppAD::pow(vars[delta_start + i] * vars[v_start+i], 2); // Don't move quickly while steering
  78.     }
  79.  
  80.     for (int i = 0; i < N - 2; i++) {
  81.       fg[0] += constants[9]*CppAD::pow(vars[delta_start + i + 1] - vars[delta_start + i], 2); // Don't change steering direction between steps
  82.       fg[0] += constants[10]*CppAD::pow(vars[a_start + i + 1] - vars[a_start + i], 2); // Don't change acceleration between steps
  83.     }
  84.  
  85.     int numPoints = 4;
  86.     for (int i = 0; i<N -2; i++) {
  87.       CppAD::AD<double> Ax = vars[x_start + i] + 0.5*CARLENGTH;
  88.       CppAD::AD<double> Ay = vars[y_start + i] + 0.5*CARWIDTH;
  89.       CppAD::AD<double> Cx = vars[x_start + i] - 0.5*CARLENGTH;
  90.       CppAD::AD<double> Cy = vars[y_start + i] - 0.5*CARWIDTH;
  91.       vector<CppAD::AD<double>> border_x;
  92.       vector<CppAD::AD<double>> border_y;
  93.  
  94.       for (int point = 0; point <= numPoints; point++){
  95.         double distanceFraction = (double)point/(double)numPoints;
  96.         border_x.push_back(Ax);
  97.         border_y.push_back(Ay - distanceFraction * CARWIDTH);
  98.         border_x.push_back(Ax - distanceFraction * CARLENGTH);
  99.         border_y.push_back(Ay);
  100.  
  101.         border_x.push_back(Cx);
  102.         border_y.push_back(Cy + distanceFraction * CARWIDTH);
  103.         border_x.push_back(Cx + distanceFraction * CARLENGTH);
  104.         border_y.push_back(Cy);
  105.  
  106.       }
  107.  
  108.       vector<CppAD::AD<double>> transformed_border_x;
  109.       vector<CppAD::AD<double>> transformed_border_y;
  110.  
  111.       // loop through all points and transform
  112.       for (int j = 0; j < border_x.size(); j++){
  113.         CppAD::AD<double> dx = border_x[j] - vars[x_start + i];
  114.         CppAD::AD<double> dy = border_y[j] - vars[y_start + i];
  115.         transformed_border_x.push_back(vars[x_start + i] + dx * cos(vars[psi_start + i]) - dy * sin(vars[psi_start + i]));
  116.         transformed_border_y.push_back(vars[y_start + i] + dx * sin(vars[psi_start + i]) + dy * cos(vars[psi_start + i]));
  117.       }
  118.  
  119.  
  120.      
  121.       for (int j = 0; j < grannies_x.size(); j++){
  122.         CppAD::AD<double> min_granny_distance = 1000000000;
  123.         for (int k = 0; k<border_x.size(); k++) {
  124.           CppAD::AD<double> current_distance = CppAD::sqrt(CppAD::pow(transformed_border_x[k] - grannies_x[j], 2) + CppAD::pow(transformed_border_y[k] - grannies_y[j], 2));
  125.           if (current_distance < min_granny_distance)
  126.             min_granny_distance = current_distance;
  127.         }
  128.         min_granny_distance -= GRANNYRADIUS;
  129.         if (min_granny_distance<=0.0)
  130.         {
  131.           min_granny_distance = 0.0000000001;
  132.           cout << "I'M GONNA RUN OVER A GRANNY!" << endl;
  133.         }
  134.         fg[0] += constants[11]/min_granny_distance;
  135.         fg[0] += constants[13]*vars[v_start+i]/min_granny_distance;
  136.       }
  137.  
  138.       for (int j = 0; j < babies_x.size(); j++){
  139.         CppAD::AD<double> min_baby_distance = 1000000000;
  140.         for (int k = 0; k<border_x.size(); k++) {
  141.           CppAD::AD<double> current_distance = CppAD::sqrt(CppAD::pow(transformed_border_x[k] - babies_x[j], 2) + CppAD::pow(transformed_border_y[k] - babies_y[j], 2));
  142.           if (current_distance < min_baby_distance)
  143.             min_baby_distance = current_distance;
  144.         }
  145.         min_baby_distance -= GRANNYRADIUS;
  146.         if (min_baby_distance<=0.0)
  147.         {
  148.           min_baby_distance = 0.0000000001;
  149.           cout << "I'M GONNA RUN OVER A BABY!" << endl;
  150.         }
  151.         fg[0] += constants[12]/min_baby_distance;
  152.         fg[0] += constants[14]*vars[v_start+i]/min_baby_distance;
  153.       }
  154.  
  155.     }
  156.  
  157.  
  158.     // for (int i = 0; i < N - 2; i++) {
  159.     //   for (int j = 0; j < grannies_x.size(); j++){
  160.     //     fg[0] += constants[11]/CppAD::pow(CppAD::sqrt(CppAD::pow(vars[x_start + i] - grannies_x[j], 2) + CppAD::pow(vars[y_start + i] - grannies_y[j], 2)), 2);
  161.     //   }
  162.     // }
  163.  
  164.     // for (int i = 0; i < N - 2; i++) {
  165.     //   for (int j = 0; j < babies_x.size(); j++){
  166.     //     fg[0] += constants[12]/CppAD::pow(CppAD::sqrt(CppAD::pow(vars[x_start + i] - babies_x[j], 2) + CppAD::pow(vars[y_start + i] - babies_y[j], 2)), 2);
  167.     //   }
  168.     // }
  169.  
  170.     //
  171.     // Setup Constraints
  172.     //
  173.     // NOTE: In this section you'll setup the model constraints.
  174.  
  175.     // Initial constraints
  176.     //
  177.     // We add 1 to each of the starting indices due to cost being located at
  178.     // index 0 of `fg`.
  179.     // This bumps up the position of all the other values.
  180.     fg[1 + x_start] = vars[x_start];
  181.     fg[1 + y_start] = vars[y_start];
  182.     fg[1 + psi_start] = vars[psi_start];
  183.     fg[1 + v_start] = vars[v_start];
  184.     fg[1 + cte_start] = vars[cte_start];
  185.     fg[1 + epsi_start] = vars[epsi_start];
  186.  
  187.     // The rest of the constraints
  188.     for (int t = 1; t < N; t++) {
  189.       AD<double> x1 = vars[x_start + t];
  190.       AD<double> x0 = vars[x_start + t - 1];
  191.       AD<double> y1 = vars[y_start + t];
  192.       AD<double> y0 = vars[y_start + t - 1];
  193.       AD<double> psi1 = vars[psi_start + t];
  194.       AD<double> psi0 = vars[psi_start + t - 1];
  195.       AD<double> v1 = vars[v_start + t];
  196.       AD<double> v0 = vars[v_start + t - 1];
  197.       AD<double> cte1 = vars[cte_start + t];
  198.       AD<double> cte0 = vars[cte_start + t - 1];
  199.       AD<double> epsi1 = vars[epsi_start + t];
  200.       AD<double> epsi0 = vars[epsi_start + t - 1];
  201.       AD<double> a = vars[a_start + t - 1];
  202.       AD<double> delta = vars[delta_start + t - 1];
  203.       if (t > 1) {   // use previous actuations (to account for latency)
  204.         a = vars[a_start + t - 2];
  205.         delta = vars[delta_start + t - 2];
  206.       }
  207.       // AD<double> f0 = coeffs[0] + coeffs[1] * x0 + coeffs[2] * CppAD::pow(x0, 2) + coeffs[3] * CppAD::pow(x0, 3)+ coeffs[4] * CppAD::pow(x0, 4);
  208.       // AD<double> psides0 = CppAD::atan(coeffs[1] + 2 * coeffs[2] * x0 + 3 * coeffs[3] * CppAD::pow(x0, 2)+ 4 * coeffs[4] * CppAD::pow(x0, 3));
  209.  
  210.       AD<double> f0 = coeffs[0] + coeffs[1] * x0 + coeffs[2] * CppAD::pow(x0, 2) + coeffs[3] * CppAD::pow(x0, 3);
  211.       AD<double> psides0 = CppAD::atan(coeffs[1] + 2 * coeffs[2] * x0 + 3 * coeffs[3] * CppAD::pow(x0, 2));
  212.  
  213.       // Here's `x` to get you started.
  214.       // The idea here is to constraint this value to be 0.
  215.       //
  216.       // NOTE: The use of `AD<double>` and use of `CppAD`!
  217.       // This is also CppAD can compute derivatives and pass
  218.       // these to the solver.
  219.  
  220.       // TODO: Setup the rest of the model constraints
  221.       fg[1 + x_start + t] = x1 - (x0 + v0 * CppAD::cos(psi0) * dt);
  222.       fg[1 + y_start + t] = y1 - (y0 + v0 * CppAD::sin(psi0) * dt);
  223.       fg[1 + psi_start + t] = psi1 - (psi0 - v0/Lf * delta * dt);
  224.       fg[1 + v_start + t] = v1 - (v0 + a * dt);
  225.       fg[1 + cte_start + t] = cte1 - ((f0 - y0) + (v0 * CppAD::sin(epsi0) * dt));
  226.       fg[1 + epsi_start + t] = epsi1 - ((psi0 - psides0) - v0/Lf * delta * dt);
  227.     }
  228.   }
  229. };
  230.  
  231. //
  232. // MPC class definition implementation.
  233. //
  234. MPC::MPC() {}
  235. MPC::~MPC() {}
  236.  
  237. vector<double> MPC::Solve(Eigen::VectorXd state, Eigen::VectorXd coeffs, std::vector< double > constants, std::vector< double > grannies_x, std::vector< double > grannies_y, std::vector< double > babies_x, std::vector< double > babies_y) {
  238.   bool ok = true;
  239.   size_t i;
  240.   typedef CPPAD_TESTVECTOR(double) Dvector;
  241.  
  242.   double x = state[0];
  243.   double y = state[1];
  244.   double psi = state[2];
  245.   double v = state[3];
  246.   double cte = state[4];
  247.   double epsi = state[5];
  248.  
  249.   // TODO: Set the number of model variables (includes both states and inputs).
  250.   // For example: If the state is a 4 element vector, the actuators is a 2
  251.   // element vector and there are 10 timesteps. The number of variables is:
  252.   //
  253.   // 4 * 10 + 2 * 9
  254.   size_t n_vars = N * 6 + (N - 1) * 2;
  255.   // TODO: Set the number of constraints
  256.   size_t n_constraints = N * 6;
  257.  
  258.   // Initial value of the independent variables.
  259.   // SHOULD BE 0 besides initial state.
  260.   Dvector vars(n_vars);
  261.   for (int i = 0; i < n_vars; i++) {
  262.     vars[i] = 0;
  263.   }
  264.  
  265.   Dvector vars_lowerbound(n_vars);
  266.   Dvector vars_upperbound(n_vars);
  267.   // TODO: Set lower and upper limits for variables.
  268.  
  269.   // Set the initial variable values
  270.   vars[x_start] = x;
  271.   vars[y_start] = y;
  272.   vars[psi_start] = psi;
  273.   vars[v_start] = v;
  274.   vars[cte_start] = cte;
  275.   vars[epsi_start] = epsi;
  276.  
  277.   // Set all non-actuators upper and lowerlimits
  278.   // to the max negative and positive values.
  279.   for (int i = 0; i < delta_start; i++) {
  280.     vars_lowerbound[i] = -1.0e19;
  281.     vars_upperbound[i] = 1.0e19;
  282.   }
  283.  
  284.   // The upper and lower limits of delta are set to -25 and 25
  285.   // degrees (values in radians).
  286.   // NOTE: Feel free to change this to something else.
  287.   for (int i = delta_start; i < a_start; i++) {
  288.     vars_lowerbound[i] = -0.436332;
  289.     vars_upperbound[i] = 0.436332;
  290.   }
  291.  
  292.   // Acceleration/decceleration upper and lower limits.
  293.   // NOTE: Feel free to change this to something else.
  294.   for (int i = a_start; i < n_vars; i++) {
  295.     vars_lowerbound[i] = -4.0;
  296.     vars_upperbound[i] = 3.0;
  297.   }
  298.  
  299.   // Lower and upper limits for the constraints
  300.   // Should be 0 besides initial state.
  301.   Dvector constraints_lowerbound(n_constraints);
  302.   Dvector constraints_upperbound(n_constraints);
  303.   for (int i = 0; i < n_constraints; i++) {
  304.     constraints_lowerbound[i] = 0;
  305.     constraints_upperbound[i] = 0;
  306.   }
  307.   constraints_lowerbound[x_start] = x;
  308.   constraints_lowerbound[y_start] = y;
  309.   constraints_lowerbound[psi_start] = psi;
  310.   constraints_lowerbound[v_start] = v;
  311.   constraints_lowerbound[cte_start] = cte;
  312.   constraints_lowerbound[epsi_start] = epsi;
  313.  
  314.   constraints_upperbound[x_start] = x;
  315.   constraints_upperbound[y_start] = y;
  316.   constraints_upperbound[psi_start] = psi;
  317.   constraints_upperbound[v_start] = v;
  318.   constraints_upperbound[cte_start] = cte;
  319.   constraints_upperbound[epsi_start] = epsi;
  320.  
  321.   // object that computes objective and constraints
  322.   FG_eval fg_eval(coeffs, constants, grannies_x, grannies_y, babies_x, babies_y);
  323.  
  324.   //
  325.   // NOTE: You don't have to worry about these options
  326.   //
  327.   // options for IPOPT solver
  328.   std::string options;
  329.   // Uncomment this if you'd like more print information
  330.   options += "Integer print_level  0\n";
  331.   // NOTE: Setting sparse to true allows the solver to take advantage
  332.   // of sparse routines, this makes the computation MUCH FASTER. If you
  333.   // can uncomment 1 of these and see if it makes a difference or not but
  334.   // if you uncomment both the computation time should go up in orders of
  335.   // magnitude.
  336.   options += "Sparse  true        forward\n";
  337.   options += "Sparse  true        reverse\n";
  338.   // NOTE: Currently the solver has a maximum time limit of 0.5 seconds.
  339.   // Change this as you see fit.
  340.   options += "Numeric max_cpu_time         " + std::to_string(constants[0]*0.8f) + "\n";
  341.  
  342.   // place to return solution
  343.   CppAD::ipopt::solve_result<Dvector> solution;
  344.  
  345.   // solve the problem
  346.   CppAD::ipopt::solve<Dvector, FG_eval>(
  347.       options, vars, vars_lowerbound, vars_upperbound, constraints_lowerbound,
  348.       constraints_upperbound, fg_eval, solution);
  349.  
  350.   // Check some of the solution values
  351.   ok &= solution.status == CppAD::ipopt::solve_result<Dvector>::success;
  352.  
  353.   // Cost
  354.   auto cost = solution.obj_value;
  355.   //std::cout << "Cost " << cost << std::endl;
  356.  
  357.   // TODO: Return the first actuator values. The variables can be accessed with
  358.   // `solution.x[i]`.
  359.   //
  360.   // {...} is shorthand for creating a vector, so auto x1 = {1.0,2.0}
  361.   // creates a 2 element double vector.
  362.   vector<double> result;
  363.  
  364.   result.push_back(solution.x[delta_start]);
  365.   result.push_back(solution.x[a_start]);
  366.  
  367.   // for (int i = 0; i < N-1; i++) {
  368.   //   result.push_back(solution.x[x_start + i + 1]);
  369.   //   result.push_back(solution.x[y_start + i + 1]);
  370.   // }
  371.  
  372.   for (int i = 0; i < N-1; i++) {
  373.       int numPoints = 4;
  374.       CppAD::AD<double> Ax = solution.x[x_start + i] + 0.5*CARLENGTH;
  375.       CppAD::AD<double> Ay = solution.x[y_start + i] + 0.5*CARWIDTH;
  376.       CppAD::AD<double> Cx = solution.x[x_start + i] - 0.5*CARLENGTH;
  377.       CppAD::AD<double> Cy = solution.x[y_start + i] - 0.5*CARWIDTH;
  378.       vector<CppAD::AD<double>> border_x;
  379.       vector<CppAD::AD<double>> border_y;
  380.  
  381.       for (int point = 0; point <= numPoints; point++){
  382.         double distanceFraction = (double)point/(double)numPoints;
  383.         border_x.push_back(Ax);
  384.         border_y.push_back(Ay - distanceFraction * CARWIDTH);
  385.         border_x.push_back(Ax - distanceFraction * CARLENGTH);
  386.         border_y.push_back(Ay);
  387.  
  388.         border_x.push_back(Cx);
  389.         border_y.push_back(Cy + distanceFraction * CARWIDTH);
  390.         border_x.push_back(Cx + distanceFraction * CARLENGTH);
  391.         border_y.push_back(Cy);
  392.  
  393.       }
  394.  
  395.       vector<CppAD::AD<double>> transformed_border_x;
  396.       vector<CppAD::AD<double>> transformed_border_y;
  397.  
  398.       // loop through all points and transform
  399.       for (int j = 0; j < border_x.size(); j++){
  400.         CppAD::AD<double> dx = border_x[j] - solution.x[x_start + i];
  401.         CppAD::AD<double> dy = border_y[j] - solution.x[y_start + i];
  402.         result.push_back(CppAD::Value(solution.x[x_start + i] + dx * cos(solution.x[psi_start + i]) - dy * sin(solution.x[psi_start + i])));
  403.         result.push_back(CppAD::Value(solution.x[y_start + i] + dx * sin(solution.x[psi_start + i]) + dy * cos(solution.x[psi_start + i])));
  404.       }
  405.   }
  406.   return result;
  407. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
Top