Page MenuHomec4science
Files F65703593

kiteNMPF.cpp
No OneTemporary
Actions

File Metadata

Created
Wed, Jun 5, 15:48

kiteNMPF.cpp
View Options

#include "kiteNMPF.h"
#include "utility"
using namespace casadi;
const DM KiteNMPF::DEFAULT_LBG = -DM::inf(1);
const DM KiteNMPF::DEFAULT_UBG = DM::inf(1);
const DM KiteNMPF::DEFAULT_LBX = -DM::inf(15);
const DM KiteNMPF::DEFAULT_UBX = DM::inf(15);
const DM KiteNMPF::DEFAULT_LBU = -DM::inf(4);
const DM KiteNMPF::DEFAULT_UBU = DM::inf(4);
namespace math_utils
{
void cheb(DM &CollocPoints, DM &DiffMatrix, const unsigned &N,
const std::pair<double, double> interval = std::make_pair(0,1))
{
/** Chebyshev collocation points for the interval [-1, 1]*/
auto grid_int = range(0, N+1);
/** cast grid to Casadi type */
DMVector DMgrid(grid_int.begin(), grid_int.end());
DM grid = DM::vertcat(DMgrid);
DM X = cos(grid * (M_PI / N));
/** shift and scale points */
CollocPoints = (X + interval.first + 1) * ((interval.second - interval.first) / 2);
/** Compute Differentiation matrix */
DM c = DM::vertcat({2, DM::ones(N-1,1), 2});
c = SX::mtimes(SX::diag( pow(-1, grid)), c);
DM XM = DM::repmat(CollocPoints, 1, N+1);
DM dX = XM - XM.T();
DM Dn = DM::mtimes(c, (1 / c).T() ) / (dX + (DM::eye(N+1))); /** off-diagonal entries */
DiffMatrix = Dn - DM::diag( DM::sumRows(Dn.T() )); /** diagonal entries */
}
SX mat_func(const SX &matrix_in, Function &func)
{
SXVector columns = SX::horzsplit(matrix_in, 1);
/** evaluate fnuction for each column and stack in a single vector*/
SX output_vec;
for(auto it = columns.begin(); it != columns.end(); ++it)
{
SX res = SX::vertcat(func(*it));
/** append to resulting vector*/
output_vec = SX::vertcat(SXVector{output_vec, res});
}
return output_vec;
}
SX mat_dynamics(const SX &arg_x, const SX &arg_u, Function &func)
{
SXVector xdot;
SXVector x = SX::horzsplit(arg_x, 1);
SXVector u = SX::horzsplit(arg_u, 1);
for(uint i = 0; i < u.size(); ++i)
{
SXVector eval = func(SXVector{x[i], u[i]});
xdot.push_back(eval[0]);
}
/** discard the initial state */
xdot.push_back(x.back());
return SX::vertcat(xdot);
}
}
KiteNMPF::KiteNMPF(std::shared_ptr<KiteDynamics> _Kite, const Function &_Path) : Kite(std::move(_Kite))
{
PathFunc = _Path;
/** set up default values */
LBX = DEFAULT_LBX;
UBX = DEFAULT_UBX;
LBU = DEFAULT_LBU;
UBU = DEFAULT_UBU;
LBG = DEFAULT_LBG;
UBG = DEFAULT_UBG;
Q = 1e-2 * SX::diag(SX({1e4, 1e4, 5e3}));
R = 1e-4 * SX::diag(SX({1, 1, 1, 1}));
W = 1e-3;
Scale_X = DM::eye(15); invSX = DM::eye(15);
Scale_U = DM::eye(4); invSU = DM::eye(4);
NUM_SHOOTING_INTERVALS = 13;
/** @attention : need sensible reference velocity */
DM vel_ref = 0.05;
this->setReferenceVelocity(vel_ref);
WARM_START = false;
_initialized = false;
}
void KiteNMPF::createNLP()
{
/** get dynamics function and state Jacobian */
SX dynamics = Kite->getSymbolicDynamics();
SX X = Kite->getSymbolicState();
SX U = Kite->getSymbolicControl();
/** state and control dimensionality */
int n = 15;
int m = 4;
/** Order of polynomial interpolation */
int N = NUM_SHOOTING_INTERVALS;
/** define augmented dynamics of path parameter */
SX V = SX::sym("V", 2);
SX Uv = SX::sym("Uv");
SX Av = SX::zeros(2,2); Av(0,1) = 1;
SX Bv = SX::zeros(2,1); Bv(1,0) = 1;
/** parameter dynamics */
SX p_dynamics = SX::mtimes(Av, V) + SX::mtimes(Bv, Uv);
/** augmented system */
SX aug_state = SX::vertcat({X, V});
SX aug_control = SX::vertcat({U, Uv});
SX aug_dynamics = SX::vertcat({dynamics, p_dynamics});
/** evaluate augmented dynamics */
Function aug_dynamo = Function("AUG_DYNAMO", {aug_state, aug_control}, {aug_dynamics});
DynamicsFunc = aug_dynamo;
/** NLP formulation */
/** define optimisation variables */
SX z = SX::sym("z", n, N+1);
SX z_u = SX::sym("z_u", m, N);
/** allocate vectors to store constraints */
SX vecx = {};
SX g = {};
/** state and control constraints */
SX lbx = {};
SX ubx = {};
/** nonlinear state constraints */
SX lbg = {};
SX ubg = {};
/** objective function */
SX objective = 0;
/** Geometric path definition */
SX theta = SX::sym("theta");
/** NLP formulation */
/** Lagrangian term */
SX pos = z(Slice(6,9), Slice(1, z.size2()-1));
pos = SX::vec(pos);
/** path following error */
DM ScaleXYZ = Scale_X(Slice(6,9), Slice(6,9));
SX path_scaled = SX::mtimes(ScaleXYZ, SX::vertcat(PathFunc(SXVector{theta})));
Function PathFuncScaled = Function("Scaled_Path",{theta},{path_scaled});
/** @badcode: better DM to double conversion should exist */
double scale_th = Scale_X(13,13).nonzeros()[0];
SX path = math_utils::mat_func( scale_th * z(13, Slice(1, z.size2()-1)), PathFuncScaled);
SX err = pos - path;
SX Qn = SX::kron(SX::eye(N-1), Q);
/** reference velocity following */
SX err_v = z(14, Slice(1, z.size2()-1)).T() - SX::repmat(reference_velocity, N-1, 1);
SX Qw = W * SX::eye(N-1);
SX L = SX::sumRows( SX::mtimes(Qn, pow(err, 2)) ) + SX::sumRows( SX::mtimes(Qw, pow(err_v, 2)) );
/** Mayer term */
path = SX::vertcat(PathFuncScaled(SXVector{scale_th * z(13,0)}));
SX err_n = z(Slice(6, 9), 0) - path;
err_n = SX::vertcat( SXVector{err_n, z(14, 0) - reference_velocity});
SX T = SX::mtimes((10 * SX::diagcat( {Q, W}) ), pow(err_n, 2));
/** objective function */
objective = L + SX::sumRows(T);
CostFunction = Function("Cost", {z, z_u}, {objective});
/** set equality constraints */
/** differentiation matrix */
DM _x, _D;
std::pair<double, double> interval = std::make_pair<double, double>(0,1);
math_utils::cheb(_x, _D, N, interval);
SX D = _D;
D(D.size1()-1, Slice(0, D.size2())) = SX::zeros(N+1);
D(D.size1() - 1, D.size2() - 1) = 1;
SX Dn = SX::kron(D, SX::eye(n));
SX iSx = SX::kron(SX::eye(N+1), invSX);
SX iSu = SX::kron(SX::eye(N), invSU);
SX F = math_utils::mat_dynamics(SX::mtimes(invSX, z), SX::mtimes(invSU, z_u), aug_dynamo);
SX G = SX::mtimes(SX::mtimes(Dn, iSx), SX::vec(z)) - F;
G = G(Slice(0, N * n), 0);
DynamicConstraints = Function("lox",{z, z_u},{G});
lbg = SX::zeros(( N ) * n, 1);
ubg = SX::zeros(( N ) * n, 1);
/** set inequality (box) constraints */
/** state */
/** @todo: SCALE CONSTRAINTS */
lbx = SX::repmat(SX::mtimes(Scale_X, LBX), N + 1, 1);
ubx = SX::repmat(SX::mtimes(Scale_X, UBX), N + 1, 1);
/** control */
lbx = SX::vertcat( {lbx, SX::repmat(SX::mtimes(Scale_U, LBU), N, 1)} );
ubx = SX::vertcat( {ubx, SX::repmat(SX::mtimes(Scale_U, UBU), N, 1)} );
/** set decision variable */
vecx = SX::vertcat({SX::vec(z), SX::vec(z_u)});
/** formulate NLP */
NLP["x"] = vecx;
NLP["f"] = objective;
NLP["g"] = G;
OPTS["ipopt.linear_solver"] = "ma97";
OPTS["ipopt.print_level"] = 0;
OPTS["ipopt.tol"] = 1e-5;
OPTS["ipopt.acceptable_tol"] = 1e-4;
OPTS["ipopt.max_iter"] = 11;
NLP_Solver = nlpsol("solver", "ipopt", NLP, OPTS);
/** set default args */
ARG["lbx"] = lbx;
ARG["ubx"] = ubx;
ARG["lbg"] = lbg;
ARG["ubg"] = ubg;
DM feasible_state = DM::mtimes(Scale_X, (UBX + LBX) / 2);
DM feasible_control = DM::mtimes(Scale_U, (UBU + LBU) / 2);
ARG["x0"] = DM::vertcat(DMVector{DM::repmat(feasible_state, N + 1, 1),
DM::repmat(feasible_control, N, 1)});
}
void KiteNMPF::computeControl(const DM &_X0)
{
int N = NUM_SHOOTING_INTERVALS;
/** @badcode : remove magic constants */
/** number of states */
int nx = 15;
/** number of control inputs */
int nu = 4;
/** scale input */
DM X0 = DM::mtimes(Scale_X, _X0);
DM U0 = DM({0.15, 0, 0, 0});
//DM ZX = DM::repmat(X0, 1, N+1);
//DM ZU = DM::repmat(U0, 1, N);
if(WARM_START)
{
int idx_in = N * nx;
int idx_out = idx_in + nx;
ARG["lbx"](Slice(idx_in, idx_out), 0) = X0;
ARG["ubx"](Slice(idx_in, idx_out), 0) = X0;
ARG["x0"] = NLP_X;
}
else
{
ARG["x0"](Slice(0, (N + 1) * nx), 0) = DM::repmat(X0, (N + 1), 1);
int idx_in = N * nx;
int idx_out = idx_in + nx;
ARG["lbx"](Slice(idx_in, idx_out), 0) = X0;
ARG["ubx"](Slice(idx_in, idx_out), 0) = X0;
}
/** store optimal solution */
DMDict res = NLP_Solver(ARG);
NLP_X = res.at("x");
DM opt_x = NLP_X(Slice(0, (N + 1) * nx ));
DM invSX = DM::solve(Scale_X, DM::eye(15));
OptimalTrajectory = DM::mtimes(invSX, DM::reshape(opt_x, nx, N+1));
DM opt_u = NLP_X( Slice((N + 1) * nx, NLP_X.size1()) );
DM invSU = DM::solve(Scale_U, DM::eye(4));
OptimalControl = DM::mtimes(invSU, DM::reshape(opt_u, nu, N));
Dict stats = NLP_Solver.stats();
std::cout << stats << "\n";
//std::cout << stats["return_status"] << " in " << stats["counts"] << " iteration \n";
enableWarmStart();
}
DM KiteNMPF::findClosestPointOnPath(const DM &position)
{
SX theta = SX::sym("theta");
SX sym_residual = 0.5 * SX::norm_2(PathFunc(SXVector{theta})[0] - SX(position));
SX sym_gradient = SX::gradient(sym_residual,theta);
Function grad = Function("gradient", {theta}, {sym_gradient});
double tol = 1e-2;
int counter = 0;
DM theta_i = {0};
DMVector result = grad(DMVector{theta_i});
/** @badcode : shitty code */
while (fabs(result[0].nonzeros()[0]) >= tol)
{
counter++;
theta_i -= grad(theta_i);
/** gradient update */
result = grad(DMVector{theta_i});
if(counter > 10)
break;
}
return theta_i;
}

Event Timeline

c4science · Help