Skip to content

Commit 6308ec6

Browse files
onlytaileionlytailei
committed
update indent
1 parent 0e4ced2 commit 6308ec6

File tree

2 files changed

+76
-75
lines changed

2 files changed

+76
-75
lines changed

‎CMakeLists.txt‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -16,8 +16,8 @@ include_directories(${EIGEN3_INCLUDE_DIR})
1616
find_package(OpenCV REQUIRED)
1717
include_directories(${EIGEN3_INCLUDE_DIR})
1818

19-
# include_directories(/home/tai/Software/Ipopt-3.12.13/build/include)
20-
# link_directories(/home/tai/Software/Ipopt-3.12.13/build/lib)
19+
#include_directories(/home/tai/Software/Ipopt-3.12.13/build/include)
20+
#link_directories(/home/tai/Software/Ipopt-3.12.13/build/lib)
2121

2222
include_directories(./include)
2323

@@ -61,5 +61,5 @@ target_link_libraries(lqr ${OpenCV_LIBS})
6161
add_executable(lqr_full src/lqr_speed_steer_control.cpp)
6262
target_link_libraries(lqr_full ${OpenCV_LIBS})
6363

64-
# add_executable(mpc src/model_predictive_control.cpp)
65-
# target_link_libraries(mpc ${OpenCV_LIBS} ipopt)
64+
#add_executable(mpc src/model_predictive_control.cpp)
65+
#target_link_libraries(mpc ${OpenCV_LIBS} ipopt)

‎src/model_predictive_control.cpp‎

Lines changed: 72 additions & 71 deletions
Original file line numberDiff line numberDiff line change
@@ -189,73 +189,74 @@ void smooth_yaw(Vec_f& cyaw){
189189

190190

191191
class FG_EVAL{
192-
public:
193-
// Eigen::VectorXd coeeffs;
194-
M_XREF traj_ref;
192+
public:
193+
// Eigen::VectorXd coeeffs;
194+
M_XREF traj_ref;
195195

196-
FG_EVAL(M_XREF traj_ref){
197-
this->traj_ref = traj_ref;
196+
FG_EVAL(M_XREF traj_ref){
197+
this->traj_ref = traj_ref;
198+
}
199+
200+
typedef CPPAD_TESTVECTOR(AD<double>) ADvector;
201+
202+
void operator()(ADvector& fg, const ADvector& vars){
203+
fg[0] = 0;
204+
205+
for(int i=0; i<T-1; i++){
206+
fg[0] += 0.01 * CppAD::pow(vars[a_start+i], 2);
207+
fg[0] += 0.01 * CppAD::pow(vars[delta_start+i], 2);
198208
}
199209

200-
typedef CPPAD_TESTVECTOR(AD<double>) ADvector;
201-
202-
void operator()(ADvector& fg, const ADvector& vars){
203-
fg[0] = 0;
204-
205-
for(int i=0; i<T-1; i++){
206-
fg[0] += 0.01 * CppAD::pow(vars[a_start+i], 2);
207-
fg[0] += 0.01 * CppAD::pow(vars[delta_start+i], 2);
208-
}
209-
210-
for(int i=0; i<T-2; i++){
211-
fg[0] += 0.01 * CppAD::pow(vars[a_start+i+1] - vars[a_start+i], 2);
212-
fg[0] += 1 * CppAD::pow(vars[delta_start+i+1] - vars[delta_start+i], 2);
213-
}
214-
215-
// fix the initial state as a constraint
216-
fg[1 + x_start] = vars[x_start];
217-
fg[1 + y_start] = vars[y_start];
218-
fg[1 + yaw_start] = vars[yaw_start];
219-
fg[1 + v_start] = vars[v_start];
220-
221-
// fg[0] += CppAD::pow(traj_ref(0, 0) - vars[x_start], 2);
222-
// fg[0] += CppAD::pow(traj_ref(1, 0) - vars[y_start], 2);
223-
// fg[0] += 0.5 * CppAD::pow(traj_ref(2, 0) - vars[yaw_start], 2);
224-
// fg[0] += 0.5 * CppAD::pow(traj_ref(3, 0) - vars[v_start], 2);
225-
// The rest of the constraints
226-
for (int i = 0; i < T - 1; i++) {
227-
// The state at time t+1 .
228-
AD<double> x1 = vars[x_start + i + 1];
229-
AD<double> y1 = vars[y_start + i + 1];
230-
AD<double> yaw1 = vars[yaw_start + i + 1];
231-
AD<double> v1 = vars[v_start + i + 1];
232-
233-
// The state at time t.
234-
AD<double> x0 = vars[x_start + i];
235-
AD<double> y0 = vars[y_start + i];
236-
AD<double> yaw0 = vars[yaw_start + i];
237-
AD<double> v0 = vars[v_start + i];
238-
239-
// Only consider the actuation at time t.
240-
AD<double> delta0 = vars[delta_start + i];
241-
AD<double> a0 = vars[a_start + i];
242-
243-
// constraint with the dynamic model
244-
fg[2 + x_start + i] = x1 - (x0 + v0 * CppAD::cos(yaw0) * DT);
245-
fg[2 + y_start + i] = y1 - (y0 + v0 * CppAD::sin(yaw0) * DT);
246-
fg[2 + yaw_start + i] = yaw1 - (yaw0 + v0 * CppAD::tan(delta0) / WB * DT);
247-
fg[2 + v_start + i] = v1 - (v0 + a0 * DT);
248-
// cost with the ref traj
249-
fg[0] += CppAD::pow(traj_ref(0, i+1) - (x0 + v0 * CppAD::cos(yaw0) * DT), 2);
250-
fg[0] += CppAD::pow(traj_ref(1, i+1) - (y0 + v0 * CppAD::sin(yaw0) * DT), 2);
251-
fg[0] += 0.5 * CppAD::pow(traj_ref(2, i+1) - (yaw0 + v0 * CppAD::tan(delta0) / WB * DT), 2);
252-
fg[0] += 0.5 * CppAD::pow(traj_ref(3, i+1) - (v0 + a0 * DT), 2);
253-
}
210+
for(int i=0; i<T-2; i++){
211+
fg[0] += 0.01 * CppAD::pow(vars[a_start+i+1] - vars[a_start+i], 2);
212+
fg[0] += 1 * CppAD::pow(vars[delta_start+i+1] - vars[delta_start+i], 2);
254213
}
214+
215+
// fix the initial state as a constraint
216+
fg[1 + x_start] = vars[x_start];
217+
fg[1 + y_start] = vars[y_start];
218+
fg[1 + yaw_start] = vars[yaw_start];
219+
fg[1 + v_start] = vars[v_start];
220+
221+
// fg[0] += CppAD::pow(traj_ref(0, 0) - vars[x_start], 2);
222+
// fg[0] += CppAD::pow(traj_ref(1, 0) - vars[y_start], 2);
223+
// fg[0] += 0.5 * CppAD::pow(traj_ref(2, 0) - vars[yaw_start], 2);
224+
// fg[0] += 0.5 * CppAD::pow(traj_ref(3, 0) - vars[v_start], 2);
225+
226+
// The rest of the constraints
227+
for (int i = 0; i < T - 1; i++) {
228+
// The state at time t+1 .
229+
AD<double> x1 = vars[x_start + i + 1];
230+
AD<double> y1 = vars[y_start + i + 1];
231+
AD<double> yaw1 = vars[yaw_start + i + 1];
232+
AD<double> v1 = vars[v_start + i + 1];
233+
234+
// The state at time t.
235+
AD<double> x0 = vars[x_start + i];
236+
AD<double> y0 = vars[y_start + i];
237+
AD<double> yaw0 = vars[yaw_start + i];
238+
AD<double> v0 = vars[v_start + i];
239+
240+
// Only consider the actuation at time t.
241+
AD<double> delta0 = vars[delta_start + i];
242+
AD<double> a0 = vars[a_start + i];
243+
244+
// constraint with the dynamic model
245+
fg[2 + x_start + i] = x1 - (x0 + v0 * CppAD::cos(yaw0) * DT);
246+
fg[2 + y_start + i] = y1 - (y0 + v0 * CppAD::sin(yaw0) * DT);
247+
fg[2 + yaw_start + i] = yaw1 - (yaw0 + v0 * CppAD::tan(delta0) / WB * DT);
248+
fg[2 + v_start + i] = v1 - (v0 + a0 * DT);
249+
// cost with the ref traj
250+
fg[0] += CppAD::pow(traj_ref(0, i+1) - (x0 + v0 * CppAD::cos(yaw0) * DT), 2);
251+
fg[0] += CppAD::pow(traj_ref(1, i+1) - (y0 + v0 * CppAD::sin(yaw0) * DT), 2);
252+
fg[0] += 0.5 * CppAD::pow(traj_ref(2, i+1) - (yaw0 + v0 * CppAD::tan(delta0) / WB * DT), 2);
253+
fg[0] += 0.5 * CppAD::pow(traj_ref(3, i+1) - (v0 + a0 * DT), 2);
254+
}
255+
}
255256
};
256257

257258
Vec_f mpc_solve(State x0, M_XREF traj_ref){
258-
259+
259260
typedef CPPAD_TESTVECTOR(double) Dvector;
260261
double x = x0.x;
261262
double y = x0.y;
@@ -406,44 +407,44 @@ void mpc_simulation(Vec_f cx, Vec_f cy, Vec_f cyaw, Vec_f ck, Vec_f speed_profil
406407
// cv_offset(output[x_start+j], output[y_start+j], bg.cols, bg.rows),
407408
// 10, cv::Scalar(0, 0, 255), -1);
408409
// }
409-
410+
410411
for(unsigned int k=0; k<x_h.size(); k++){
411412
cv::circle(
412413
bg,
413414
cv_offset(x_h[k], y_h[k], bg.cols, bg.rows),
414415
8, cv::Scalar(255, 0, 0), -1);
415416
}
416-
417+
417418

418419
cv::line(
419420
bg,
420421
cv_offset(state.x, state.y, bg.cols, bg.rows),
421422
cv_offset(state.x + std::cos(state.yaw)*WB*2, state.y + std::sin(state.yaw)*WB*2, bg.cols, bg.rows),
422423
cv::Scalar(255,0,255),
423424
15);
424-
425+
425426
cv::line(
426427
bg,
427-
cv_offset(state.x + std::cos(state.yaw)*0.5,
428+
cv_offset(state.x + std::cos(state.yaw)*0.5,
428429
state.y + std::sin(state.yaw)*0.5,
429430
bg.cols, bg.rows),
430-
cv_offset(state.x - std::cos(state.yaw)*0.5,
431+
cv_offset(state.x - std::cos(state.yaw)*0.5,
431432
state.y - std::sin(state.yaw)*0.5,
432433
bg.cols, bg.rows),
433434
cv::Scalar(255,0,127),
434435
30);
435-
436+
436437
cv::line(
437438
bg,
438-
cv_offset(state.x + std::cos(state.yaw)*WB*2 + std::cos(state.yaw+steer)*0.5,
439+
cv_offset(state.x + std::cos(state.yaw)*WB*2 + std::cos(state.yaw+steer)*0.5,
439440
state.y + std::sin(state.yaw)*WB*2 + std::sin(state.yaw+steer)*0.5,
440441
bg.cols, bg.rows),
441-
cv_offset(state.x + std::cos(state.yaw)*WB*2 - std::cos(state.yaw+steer)*0.5,
442-
state.y + std::sin(state.yaw)*WB*2 - std::sin(state.yaw+steer)*0.5,
442+
cv_offset(state.x + std::cos(state.yaw)*WB*2 - std::cos(state.yaw+steer)*0.5,
443+
state.y + std::sin(state.yaw)*WB*2 - std::sin(state.yaw+steer)*0.5,
443444
bg.cols, bg.rows),
444445
cv::Scalar(255,0,127),
445446
30);
446-
447+
447448
for(unsigned int k=0; k<xref.cols(); k++){
448449
cv::drawMarker(
449450
bg,
@@ -452,7 +453,7 @@ void mpc_simulation(Vec_f cx, Vec_f cy, Vec_f cyaw, Vec_f ck, Vec_f speed_profil
452453
cv::MARKER_CROSS,
453454
20, 3);
454455
}
455-
456+
456457
// save image in build/bin/pngs
457458
// struct timeval tp;
458459
// gettimeofday(&tp, NULL);

0 commit comments

Comments
 (0)