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

Fixed setLastControlRef and graph consistency (fd_grid)

master
Christoph Rösmann 2020-03-06 16:42:38 +01:00
parent 8a7d14049f
commit e3658ac44d
1 changed files with 5 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -71,7 +71,7 @@ corbo::GridUpdateResult FullDiscretizationGridBaseSE2::update(const Eigen::Vecto
setPreviousControl(Eigen::VectorXd::Zero(dynamics->getInputDimension()), prev_u_dt); setPreviousControl(Eigen::VectorXd::Zero(dynamics->getInputDimension()), prev_u_dt);
// set last control to uref // set last control to uref
setLastControlRef(uref.getReferenceCached(n)); setLastControlRef(uref.getReferenceCached(n - 1));
// TODO(roesmann): we do not check if bounds in nlp_fun are updated // TODO(roesmann): we do not check if bounds in nlp_fun are updated
// updateBounds(); // calling this everytime is not efficient // updateBounds(); // calling this everytime is not efficient
@ -554,8 +554,10 @@ void FullDiscretizationGridBaseSE2::getVertices(std::vector<VertexInterface*>& v
for (VectorVertexSE2& vtx : _x_seq) vertices.push_back(&vtx); for (VectorVertexSE2& vtx : _x_seq) vertices.push_back(&vtx);
for (VectorVertex& vtx : _u_seq) vertices.push_back(&vtx); for (VectorVertex& vtx : _u_seq) vertices.push_back(&vtx);
vertices.push_back(&_xf); vertices.push_back(&_xf);
vertices.push_back(&_dt); // make sure to make it fixed if desired in any subclass vertices.push_back(&_dt); // make sure to make it fixed if desired in any subclass
vertices.push_back(&_u_prev); // always fixed... vertices.push_back(&_u_prev); // always fixed...
vertices.push_back(&_u_prev_dt); // always fixed...
vertices.push_back(&_u_ref); // always fixed...
} }
void FullDiscretizationGridBaseSE2::computeActiveVertices() void FullDiscretizationGridBaseSE2::computeActiveVertices()