From e3658ac44db050c2ef72cd58029bdf000b546dfb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20R=C3=B6smann?= Date: Fri, 6 Mar 2020 16:42:38 +0100 Subject: [PATCH] Fixed setLastControlRef and graph consistency (fd_grid) --- .../optimal_control/full_discretization_grid_base_se2.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/mpc_local_planner/src/optimal_control/full_discretization_grid_base_se2.cpp b/mpc_local_planner/src/optimal_control/full_discretization_grid_base_se2.cpp index c477c1a..9f89342 100644 --- a/mpc_local_planner/src/optimal_control/full_discretization_grid_base_se2.cpp +++ b/mpc_local_planner/src/optimal_control/full_discretization_grid_base_se2.cpp @@ -71,7 +71,7 @@ corbo::GridUpdateResult FullDiscretizationGridBaseSE2::update(const Eigen::Vecto setPreviousControl(Eigen::VectorXd::Zero(dynamics->getInputDimension()), prev_u_dt); // 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 // updateBounds(); // calling this everytime is not efficient @@ -554,8 +554,10 @@ void FullDiscretizationGridBaseSE2::getVertices(std::vector& v for (VectorVertexSE2& vtx : _x_seq) vertices.push_back(&vtx); for (VectorVertex& vtx : _u_seq) vertices.push_back(&vtx); vertices.push_back(&_xf); - 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(&_dt); // make sure to make it fixed if desired in any subclass + 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()