Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include "MPC.h"
- #include <cppad/cppad.hpp>
- #include <cppad/ipopt/solve.hpp>
- #include "Eigen-3.3/Eigen/Core"
- using CppAD::AD;
- #define CARWIDTH 3.5
- #define CARLENGTH 8.5
- #define GRANNYRADIUS 0.54
- #define BABYRADIUS 0.43
- // TODO: Set the timestep length and duration
- size_t N = 15;
- //double dt = 0.05;
- // This value assumes the model presented in the classroom is used.
- //
- // It was obtained by measuring the radius formed by running the vehicle in the
- // simulator around in a circle with a constant steering angle and velocity on a
- // flat terrain.
- //
- // Lf was tuned until the the radius formed by the simulating the model
- // presented in the classroom matched the previous radius.
- //
- // This is the length from front to CoG that has a similar radius.
- const double Lf = 2.184738;
- //double ref_v = 50;
- size_t x_start = 0;
- size_t y_start = x_start + N;
- size_t psi_start = y_start + N;
- size_t v_start = psi_start + N;
- size_t cte_start = v_start + N;
- size_t epsi_start = cte_start + N;
- size_t delta_start = epsi_start + N;
- size_t a_start = delta_start + N - 1;
- class FG_eval {
- public:
- // Fitted polynomial coefficients
- 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;
- 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;}
- typedef CPPAD_TESTVECTOR(AD<double>) ADvector;
- void operator()(ADvector& fg, const ADvector& vars) {
- double ref_v = constants[2];
- double dt = constants[0];
- // TODO: implement MPC
- // `fg` a vector of the cost constraints, `vars` is a vector of variable values (state & actuators)
- // NOTE: You'll probably go back and forth between this function and
- // the Solver function below.
- // The cost is stored is the first element of `fg`.
- // Any additions to the cost should be added to `fg[0]`.
- fg[0] = 0;
- // Reference State Cost
- // TODO: Define the cost related the reference state and
- // any anything you think may be beneficial.
- for (int i = 0; i < N; i++) {
- fg[0] += constants[3]*CppAD::pow(vars[cte_start + i], 2); // Don't stray off the track
- fg[0] += constants[4]*CppAD::pow(vars[epsi_start + i], 2); // Stay in same direction as track
- fg[0] += constants[5]*CppAD::pow(vars[v_start + i] - ref_v, 2); // Don't drive away from the speed limit
- }
- for (int i = 0; i < N - 1; i++) {
- fg[0] += constants[6]*CppAD::pow(vars[delta_start + i], 2); // Don't steer too sharply
- fg[0] += constants[7]*CppAD::pow(vars[a_start + i], 2); // Don't accelerate too much
- // try adding penalty for speed + steer
- fg[0] += constants[8]*CppAD::pow(vars[delta_start + i] * vars[v_start+i], 2); // Don't move quickly while steering
- }
- for (int i = 0; i < N - 2; i++) {
- fg[0] += constants[9]*CppAD::pow(vars[delta_start + i + 1] - vars[delta_start + i], 2); // Don't change steering direction between steps
- fg[0] += constants[10]*CppAD::pow(vars[a_start + i + 1] - vars[a_start + i], 2); // Don't change acceleration between steps
- }
- int numPoints = 4;
- for (int i = 0; i<N -2; i++) {
- CppAD::AD<double> Ax = vars[x_start + i] + 0.5*CARLENGTH;
- CppAD::AD<double> Ay = vars[y_start + i] + 0.5*CARWIDTH;
- CppAD::AD<double> Cx = vars[x_start + i] - 0.5*CARLENGTH;
- CppAD::AD<double> Cy = vars[y_start + i] - 0.5*CARWIDTH;
- vector<CppAD::AD<double>> border_x;
- vector<CppAD::AD<double>> border_y;
- for (int point = 0; point <= numPoints; point++){
- double distanceFraction = (double)point/(double)numPoints;
- border_x.push_back(Ax);
- border_y.push_back(Ay - distanceFraction * CARWIDTH);
- border_x.push_back(Ax - distanceFraction * CARLENGTH);
- border_y.push_back(Ay);
- border_x.push_back(Cx);
- border_y.push_back(Cy + distanceFraction * CARWIDTH);
- border_x.push_back(Cx + distanceFraction * CARLENGTH);
- border_y.push_back(Cy);
- }
- vector<CppAD::AD<double>> transformed_border_x;
- vector<CppAD::AD<double>> transformed_border_y;
- // loop through all points and transform
- for (int j = 0; j < border_x.size(); j++){
- CppAD::AD<double> dx = border_x[j] - vars[x_start + i];
- CppAD::AD<double> dy = border_y[j] - vars[y_start + i];
- transformed_border_x.push_back(vars[x_start + i] + dx * cos(vars[psi_start + i]) - dy * sin(vars[psi_start + i]));
- transformed_border_y.push_back(vars[y_start + i] + dx * sin(vars[psi_start + i]) + dy * cos(vars[psi_start + i]));
- }
- for (int j = 0; j < grannies_x.size(); j++){
- CppAD::AD<double> min_granny_distance = 1000000000;
- for (int k = 0; k<border_x.size(); k++) {
- 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));
- if (current_distance < min_granny_distance)
- min_granny_distance = current_distance;
- }
- min_granny_distance -= GRANNYRADIUS;
- if (min_granny_distance<=0.0)
- {
- min_granny_distance = 0.0000000001;
- cout << "I'M GONNA RUN OVER A GRANNY!" << endl;
- }
- fg[0] += constants[11]/min_granny_distance;
- fg[0] += constants[13]*vars[v_start+i]/min_granny_distance;
- }
- for (int j = 0; j < babies_x.size(); j++){
- CppAD::AD<double> min_baby_distance = 1000000000;
- for (int k = 0; k<border_x.size(); k++) {
- 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));
- if (current_distance < min_baby_distance)
- min_baby_distance = current_distance;
- }
- min_baby_distance -= GRANNYRADIUS;
- if (min_baby_distance<=0.0)
- {
- min_baby_distance = 0.0000000001;
- cout << "I'M GONNA RUN OVER A BABY!" << endl;
- }
- fg[0] += constants[12]/min_baby_distance;
- fg[0] += constants[14]*vars[v_start+i]/min_baby_distance;
- }
- }
- // for (int i = 0; i < N - 2; i++) {
- // for (int j = 0; j < grannies_x.size(); j++){
- // 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);
- // }
- // }
- // for (int i = 0; i < N - 2; i++) {
- // for (int j = 0; j < babies_x.size(); j++){
- // 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);
- // }
- // }
- //
- // Setup Constraints
- //
- // NOTE: In this section you'll setup the model constraints.
- // Initial constraints
- //
- // We add 1 to each of the starting indices due to cost being located at
- // index 0 of `fg`.
- // This bumps up the position of all the other values.
- fg[1 + x_start] = vars[x_start];
- fg[1 + y_start] = vars[y_start];
- fg[1 + psi_start] = vars[psi_start];
- fg[1 + v_start] = vars[v_start];
- fg[1 + cte_start] = vars[cte_start];
- fg[1 + epsi_start] = vars[epsi_start];
- // The rest of the constraints
- for (int t = 1; t < N; t++) {
- AD<double> x1 = vars[x_start + t];
- AD<double> x0 = vars[x_start + t - 1];
- AD<double> y1 = vars[y_start + t];
- AD<double> y0 = vars[y_start + t - 1];
- AD<double> psi1 = vars[psi_start + t];
- AD<double> psi0 = vars[psi_start + t - 1];
- AD<double> v1 = vars[v_start + t];
- AD<double> v0 = vars[v_start + t - 1];
- AD<double> cte1 = vars[cte_start + t];
- AD<double> cte0 = vars[cte_start + t - 1];
- AD<double> epsi1 = vars[epsi_start + t];
- AD<double> epsi0 = vars[epsi_start + t - 1];
- AD<double> a = vars[a_start + t - 1];
- AD<double> delta = vars[delta_start + t - 1];
- if (t > 1) { // use previous actuations (to account for latency)
- a = vars[a_start + t - 2];
- delta = vars[delta_start + t - 2];
- }
- // 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);
- // 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));
- AD<double> f0 = coeffs[0] + coeffs[1] * x0 + coeffs[2] * CppAD::pow(x0, 2) + coeffs[3] * CppAD::pow(x0, 3);
- AD<double> psides0 = CppAD::atan(coeffs[1] + 2 * coeffs[2] * x0 + 3 * coeffs[3] * CppAD::pow(x0, 2));
- // Here's `x` to get you started.
- // The idea here is to constraint this value to be 0.
- //
- // NOTE: The use of `AD<double>` and use of `CppAD`!
- // This is also CppAD can compute derivatives and pass
- // these to the solver.
- // TODO: Setup the rest of the model constraints
- fg[1 + x_start + t] = x1 - (x0 + v0 * CppAD::cos(psi0) * dt);
- fg[1 + y_start + t] = y1 - (y0 + v0 * CppAD::sin(psi0) * dt);
- fg[1 + psi_start + t] = psi1 - (psi0 - v0/Lf * delta * dt);
- fg[1 + v_start + t] = v1 - (v0 + a * dt);
- fg[1 + cte_start + t] = cte1 - ((f0 - y0) + (v0 * CppAD::sin(epsi0) * dt));
- fg[1 + epsi_start + t] = epsi1 - ((psi0 - psides0) - v0/Lf * delta * dt);
- }
- }
- };
- //
- // MPC class definition implementation.
- //
- MPC::MPC() {}
- MPC::~MPC() {}
- 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) {
- bool ok = true;
- size_t i;
- typedef CPPAD_TESTVECTOR(double) Dvector;
- double x = state[0];
- double y = state[1];
- double psi = state[2];
- double v = state[3];
- double cte = state[4];
- double epsi = state[5];
- // TODO: Set the number of model variables (includes both states and inputs).
- // For example: If the state is a 4 element vector, the actuators is a 2
- // element vector and there are 10 timesteps. The number of variables is:
- //
- // 4 * 10 + 2 * 9
- size_t n_vars = N * 6 + (N - 1) * 2;
- // TODO: Set the number of constraints
- size_t n_constraints = N * 6;
- // 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;
- }
- Dvector vars_lowerbound(n_vars);
- Dvector vars_upperbound(n_vars);
- // TODO: Set lower and upper limits for variables.
- // Set the initial variable values
- vars[x_start] = x;
- vars[y_start] = y;
- vars[psi_start] = psi;
- vars[v_start] = v;
- vars[cte_start] = cte;
- vars[epsi_start] = epsi;
- // Set all non-actuators upper and lowerlimits
- // to the max negative and positive values.
- for (int i = 0; i < delta_start; i++) {
- vars_lowerbound[i] = -1.0e19;
- vars_upperbound[i] = 1.0e19;
- }
- // The upper and lower limits of delta are set to -25 and 25
- // degrees (values in radians).
- // NOTE: Feel free to change this to something else.
- for (int i = delta_start; i < a_start; i++) {
- vars_lowerbound[i] = -0.436332;
- vars_upperbound[i] = 0.436332;
- }
- // Acceleration/decceleration upper and lower limits.
- // NOTE: Feel free to change this to something else.
- for (int i = a_start; i < n_vars; i++) {
- vars_lowerbound[i] = -4.0;
- vars_upperbound[i] = 3.0;
- }
- // 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;
- }
- constraints_lowerbound[x_start] = x;
- constraints_lowerbound[y_start] = y;
- constraints_lowerbound[psi_start] = psi;
- constraints_lowerbound[v_start] = v;
- constraints_lowerbound[cte_start] = cte;
- constraints_lowerbound[epsi_start] = epsi;
- constraints_upperbound[x_start] = x;
- constraints_upperbound[y_start] = y;
- constraints_upperbound[psi_start] = psi;
- constraints_upperbound[v_start] = v;
- constraints_upperbound[cte_start] = cte;
- constraints_upperbound[epsi_start] = epsi;
- // object that computes objective and constraints
- FG_eval fg_eval(coeffs, constants, grannies_x, grannies_y, babies_x, babies_y);
- //
- // 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 " + std::to_string(constants[0]*0.8f) + "\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;
- // TODO: Return the first actuator values. The variables can be accessed with
- // `solution.x[i]`.
- //
- // {...} is shorthand for creating a vector, so auto x1 = {1.0,2.0}
- // creates a 2 element double vector.
- vector<double> result;
- result.push_back(solution.x[delta_start]);
- result.push_back(solution.x[a_start]);
- // for (int i = 0; i < N-1; i++) {
- // result.push_back(solution.x[x_start + i + 1]);
- // result.push_back(solution.x[y_start + i + 1]);
- // }
- for (int i = 0; i < N-1; i++) {
- int numPoints = 4;
- CppAD::AD<double> Ax = solution.x[x_start + i] + 0.5*CARLENGTH;
- CppAD::AD<double> Ay = solution.x[y_start + i] + 0.5*CARWIDTH;
- CppAD::AD<double> Cx = solution.x[x_start + i] - 0.5*CARLENGTH;
- CppAD::AD<double> Cy = solution.x[y_start + i] - 0.5*CARWIDTH;
- vector<CppAD::AD<double>> border_x;
- vector<CppAD::AD<double>> border_y;
- for (int point = 0; point <= numPoints; point++){
- double distanceFraction = (double)point/(double)numPoints;
- border_x.push_back(Ax);
- border_y.push_back(Ay - distanceFraction * CARWIDTH);
- border_x.push_back(Ax - distanceFraction * CARLENGTH);
- border_y.push_back(Ay);
- border_x.push_back(Cx);
- border_y.push_back(Cy + distanceFraction * CARWIDTH);
- border_x.push_back(Cx + distanceFraction * CARLENGTH);
- border_y.push_back(Cy);
- }
- vector<CppAD::AD<double>> transformed_border_x;
- vector<CppAD::AD<double>> transformed_border_y;
- // loop through all points and transform
- for (int j = 0; j < border_x.size(); j++){
- CppAD::AD<double> dx = border_x[j] - solution.x[x_start + i];
- CppAD::AD<double> dy = border_y[j] - solution.x[y_start + i];
- result.push_back(CppAD::Value(solution.x[x_start + i] + dx * cos(solution.x[psi_start + i]) - dy * sin(solution.x[psi_start + i])));
- result.push_back(CppAD::Value(solution.x[y_start + i] + dx * sin(solution.x[psi_start + i]) + dy * cos(solution.x[psi_start + i])));
- }
- }
- return result;
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement