Page Menu
Home
c4science
Search
Configure Global Search
Log In
Files
F111109030
kiteNMPF.cpp
No One
Temporary
Actions
Download File
Edit File
Delete File
View Transforms
Subscribe
Mute Notifications
Award Token
Subscribers
None
File Metadata
Details
File Info
Storage
Attached
Created
Tue, Apr 29, 15:54
Size
9 KB
Mime Type
text/x-c
Expires
Thu, May 1, 15:54 (1 d, 14 h)
Engine
blob
Format
Raw Data
Handle
25869005
Attached To
R1517 test_package
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(const KiteProperties &KiteProps, const AlgorithmProperties &AlgoProps)
{
Kite = std::make_shared<KiteDynamics>(KiteProps, AlgoProps);
LBX = DEFAULT_LBX;
UBX = DEFAULT_UBX;
LBU = DEFAULT_LBU;
UBU = DEFAULT_UBU;
LBG = DEFAULT_LBG;
UBG = DEFAULT_UBG;
NUM_SHOOTING_INTERVALS = 12;
WARM_START = false;
}
KiteNMPF::KiteNMPF(const std::shared_ptr<KiteDynamics> &_Kite)
{
Kite = _Kite;
LBX = DEFAULT_LBX;
UBX = DEFAULT_UBX;
LBU = DEFAULT_LBU;
UBU = DEFAULT_UBU;
LBG = DEFAULT_LBG;
UBG = DEFAULT_UBG;
NUM_SHOOTING_INTERVALS = 12;
WARM_START = 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);
/** 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});
/** NLP formulation */
/** weight matrices for the cost function */
/** @todo: parametrize */
SX Q = SX::diag(SX::vertcat(SXVector{1e4, 1e4, 5e3}));
SX R = 1e-4 * SX::diag(SX::vertcat(SXVector{1, 1, 1, 1}));
SX W = 1e-3;
SX Wq = SX::diag(SX::vertcat(SXVector{1000, 1000}));
/** reference velocity for the parameter (scaled) */
/** @todo: parametrize profile velocity*/
double v_max = UBX.nonzeros()[0];
SX vel_ref = 0.05 / v_max;
/** 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 */
/** @todo: make parametrized */
SX theta = SX::sym("theta");
Path = SX::vertcat(SXVector{-1, cos(theta), sin(theta)});
Function path_fun = Function("path", {theta}, {Path});
/** Define scaling matrices */
/** state */
/** @badcode: hard coded path and scaling */
double wx_max = UBX.nonzeros()[3];
double wy_max = UBX.nonzeros()[4];
double wz_max = UBX.nonzeros()[5];
double altitude = -45.83;
double line = 50;
//double radius = 20;
SX Sx = SX::diag(SX::vertcat(SXVector{1/v_max, 1, 0.5, 1/wx_max, 1/wy_max, 1/wz_max,
fabs(1/altitude), 1/line, 1/line, 1,1,1,1, 1/(2*M_PI), 1/v_max}));
/** control */
SX Su = SX::diag(1 / UBU);
Scale_X = Sx;
Scale_U = Su;
/** NLP formulation */
/** Lagrangian term */
SX pos = z(Slice(6,9), Slice(1, z.size2()-1));
pos = SX::vec(pos);
SX path = math_utils::mat_func( (2 * M_PI) * z(13, Slice(1, z.size2()-1)), path_fun);
/** path following error */
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(vel_ref, 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(path_fun(SXVector{2 * M_PI * z(13,0)}));
SX err_n = z(Slice(6, 9), 0) - path[0];
err_n = SX::vertcat( SXVector{err_n, z(14, 0) - vel_ref});
SX T = SX::mtimes((10 * SX::diagcat( {Q, W}) ), pow(err_n, 2));
/** objective function */
objective = L + SX::sumRows(T);
/** 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));
/** scaling matrices */
DM invSX = DM::solve(Scale_X, DM::eye(n));
DM invSU = DM::solve(Scale_U, DM::eye(4));
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);
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.ma97_solve_blas3"] = "no";
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 */
int n = 15;
/** scale input */
DM X0 = DM::mtimes(Scale_X, _X0);
if(WARM_START)
{
int idx_in = N * n - 1;
int idx_out = idx_in + n;
ARG["lbx"](Slice(idx_in, idx_out), 0) = X0;
ARG["ubx"](Slice(idx_in, idx_out), 0) = X0;
ARG["x0"] = NLP_X;
ARG["x0"](Slice(idx_in, idx_out), 0) = X0;
}
else
{
ARG["x0"](Slice(0, (N + 1) * n), 0) = DM::repmat(X0, (N + 1), 1);
int idx_in = N * n;
int idx_out = idx_in + n;
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) * n ));
DM invSX = DM::solve(Scale_X, DM::eye(15));
DM opt_x_mat = DM::mtimes(invSX, DM::reshape(opt_x, n, N+1));
std::cout << opt_x_mat << "\n";
enableWarmStart();
}
Event Timeline
Log In to Comment