• API
• FAQ
• Tools
• Archive
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"
4. #include "Eigen-3.3/Eigen/Core"
5.
7.
8. #define CARWIDTH 3.5
9. #define CARLENGTH 8.5
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.
52.
53.     double ref_v = constants;
54.     double dt = constants;
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`.
62.     fg = 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 += constants*CppAD::pow(vars[cte_start + i], 2);  // Don't stray off the track
69.       fg += constants*CppAD::pow(vars[epsi_start + i], 2); // Stay in same direction as track
70.       fg += constants*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 += constants*CppAD::pow(vars[delta_start + i], 2); // Don't steer too sharply
75.       fg += constants*CppAD::pow(vars[a_start + i], 2);     // Don't accelerate too much
76.       // try adding penalty for speed + steer
77.       fg += constants*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 += constants*CppAD::pow(vars[delta_start + i + 1] - vars[delta_start + i], 2); // Don't change steering direction between steps
82.       fg += constants*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++) {
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.
110.
111.       // loop through all points and transform
112.       for (int j = 0; j < border_x.size(); j++){
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++){
123.         for (int k = 0; k<border_x.size(); k++) {
125.           if (current_distance < min_granny_distance)
126.             min_granny_distance = current_distance;
127.         }
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 += constants/min_granny_distance;
135.         fg += constants*vars[v_start+i]/min_granny_distance;
136.       }
137.
138.       for (int j = 0; j < babies_x.size(); j++){
140.         for (int k = 0; k<border_x.size(); k++) {
142.           if (current_distance < min_baby_distance)
143.             min_baby_distance = current_distance;
144.         }
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 += constants/min_baby_distance;
152.         fg += constants*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++){
161.     //   }
162.     // }
163.
164.     // for (int i = 0; i < N - 2; i++) {
165.     //   for (int j = 0; j < babies_x.size(); j++){
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 + coeffs * x0 + coeffs * CppAD::pow(x0, 2) + coeffs * CppAD::pow(x0, 3)+ coeffs * CppAD::pow(x0, 4);
208.       // AD<double> psides0 = CppAD::atan(coeffs + 2 * coeffs * x0 + 3 * coeffs * CppAD::pow(x0, 2)+ 4 * coeffs * CppAD::pow(x0, 3));
209.
210.       AD<double> f0 = coeffs + coeffs * x0 + coeffs * CppAD::pow(x0, 2) + coeffs * CppAD::pow(x0, 3);
211.       AD<double> psides0 = CppAD::atan(coeffs + 2 * coeffs * x0 + 3 * coeffs * 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.       //
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;
241.
242.   double x = state;
243.   double y = state;
244.   double psi = state;
245.   double v = state;
246.   double cte = state;
247.   double epsi = state;
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.8f) + "\n";
341.
342.   // place to return solution
344.
345.   // solve the problem
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;
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.
397.
398.       // loop through all points and transform
399.       for (int j = 0; j < border_x.size(); j++){