12345qiupeng
/
mpc_local_planner
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fixed wrong start orientation in point-to-point grid initialization

master
Christoph Rösmann 2020-05-20 14:35:00 +02:00 committed by GitHub
parent 1dbc6e122d
commit 0b28d45590
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 1 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
mpc_local_planner/src/optimal_control/full_discretization_grid_base_se2.cpp
View File

@ -168,7 +168,7 @@ void FullDiscretizationGridBaseSE2::initializeSequences(const Eigen::VectorXd& x
{ {
// add new state by linear interpolation // add new state by linear interpolation
Eigen::VectorXd new_x = x0 + (double)k * step * dir; Eigen::VectorXd new_x = x0 + (double)k * step * dir;
new_x[2] = orient_init; if (k > 0) new_x[2] = orient_init; // but do not overwrite start orientation
_x_seq.emplace_back(new_x, nlp_fun.x_lb, nlp_fun.x_ub); _x_seq.emplace_back(new_x, nlp_fun.x_lb, nlp_fun.x_ub);
// add new constant control (assume u0 to hold // add new constant control (assume u0 to hold
_u_seq.emplace_back(uref.getReferenceCached(k), nlp_fun.u_lb, nlp_fun.u_ub); _u_seq.emplace_back(uref.getReferenceCached(k), nlp_fun.u_lb, nlp_fun.u_ub);