Skip to content
Snippets Groups Projects
Commit 6cd9be1c authored by Petr Listov's avatar Petr Listov
Browse files

+ path reset bug fixed;

parent 4f484d79
Branches master
No related tags found
No related merge requests found
......@@ -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;
}
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment