From 6cd9be1cb1e60c9c375a84106162cd786f942998 Mon Sep 17 00:00:00 2001
From: Peter Listov <plistov@gmail.com>
Date: Fri, 20 Sep 2019 16:16:18 +0200
Subject: [PATCH] + path reset bug fixed;
---
src/nmpf.hpp | 18 +++++++++++++-----
1 file changed, 13 insertions(+), 5 deletions(-)
diff --git a/src/nmpf.hpp b/src/nmpf.hpp
index 351d1a8..81f2d32 100644
--- a/src/nmpf.hpp
+++ b/src/nmpf.hpp
@@ -130,6 +130,7 @@ private:
bool WARM_START;
bool _initialized;
bool scale;
+ double reset_path_after;
/** TRACE FUNCTIONS */
casadi::Function DynamicsFunc;
@@ -209,6 +210,13 @@ nmpf<System, Path, NX, NU, NumSegments, PolyOrder>::nmpf(const double &tf, const
assert(1 == W.size2());
}
+ reset_path_after = 2 * M_PI;
+ if(mpc_options.find("mpc.reset_path_after") != mpc_options.end())
+ {
+ casadi::DM tmp = mpc_options.find("mpc.reset_path_after")->second;
+ reset_path_after = tmp.nonzeros()[0];
+ }
+
/** assume unconstrained problem */
LBX = -casadi::DM::inf(nx + 2);
UBX = casadi::DM::inf(nx + 2);
@@ -391,17 +399,17 @@ void nmpf<System, Path, NX, NU, NumSegments, PolyOrder>::computeControl(const ca
{
int N = NUM_COLLOCATION_POINTS;
- /** rectify virtual state */
+ /** rectify / reset virtual state */
casadi::DM X0 = _X0;
bool rectify = false;
- if(X0(nx).nonzeros()[0] > 2 * M_PI)
+ if(X0(nx).nonzeros()[0] > reset_path_after)
{
- X0(nx) -= 2 * M_PI;
+ X0(nx) -= reset_path_after;
rectify = true;
}
- else if (X0(nx).nonzeros()[0] < -2 * M_PI)
+ else if (X0(nx).nonzeros()[0] < -reset_path_after)
{
- X0(nx) += 2 * M_PI;
+ X0(nx) += reset_path_after;
rectify = true;
}
--
GitLab