From 0b28d4559074d9bed86a621b145a985bd4daf0b6 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Christoph=20R=C3=B6smann?=
 <christoph.roesmann@tu-dortmund.de>
Date: Wed, 20 May 2020 14:35:00 +0200
Subject: [PATCH] Fixed wrong start orientation in point-to-point grid
 initialization

---
 .../src/optimal_control/full_discretization_grid_base_se2.cpp   | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

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 9f89342..b322b83 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
@@ -168,7 +168,7 @@ void FullDiscretizationGridBaseSE2::initializeSequences(const Eigen::VectorXd& x
     {
         // add new state by linear interpolation
         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);
         // add new constant control (assume u0 to hold
         _u_seq.emplace_back(uref.getReferenceCached(k), nlp_fun.u_lb, nlp_fun.u_ub);