Page Menu
Home
c4science
Search
Configure Global Search
Log In
Files
F85319147
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
Sat, Sep 28, 07:20
Size
12 KB
Mime Type
text/x-c
Expires
Mon, Sep 30, 07:20 (2 d)
Engine
blob
Format
Raw Data
Handle
21157916
Attached To
R1517 test_package
kiteNMPF.cpp
View Options
#include "kiteNMPF.h"
#include "utility"
#include "pseudospectral/chebyshev.hpp"
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
);
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
=
10
;
/** @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
;
/** ----------------------------------------------------------------------------------*/
const
int
num_segments
=
1
;
const
int
poly_order
=
10
;
const
int
dimx
=
15
;
const
int
dimu
=
4
;
const
int
dimp
=
1
;
Chebyshev
<
SX
,
poly_order
,
num_segments
,
dimx
,
dimu
,
dimp
>
spectral
;
SX
diff_constr
=
spectral
.
CollocateDynamics
(
DynamicsFunc
,
0
,
1
);
//diff_constr = diff_constr(Slice(0, diff_constr.size1() - dimx));
/** define an integral cost */
SX
x
=
SX
::
sym
(
"x"
,
dimx
);
SX
u
=
SX
::
sym
(
"u"
,
dimu
);
SXVector
tmp
=
PathFunc
(
SXVector
{
x
[
13
]});
SX
sym_path
=
tmp
[
0
];
SX
residual
=
sym_path
-
x
(
Slice
(
6
,
9
));
SX
lagrange
=
SX
::
sumRows
(
SX
::
mtimes
(
Q
,
pow
(
residual
,
2
))
)
+
SX
::
sumRows
(
SX
::
mtimes
(
W
,
pow
(
reference_velocity
-
x
[
14
],
2
))
);
Function
LagrangeTerm
=
Function
(
"Lagrange"
,
{
x
,
u
},
{
lagrange
});
/** trace functions */
PathError
=
Function
(
"PathError"
,
{
x
},
{
residual
});
VelError
=
Function
(
"VelError"
,
{
x
},
{
reference_velocity
-
x
[
14
]});
SX
mayer
=
SX
::
sumRows
(
SX
::
mtimes
(
2
*
Q
,
pow
(
residual
,
2
))
);
Function
MayerTerm
=
Function
(
"Mayer"
,{
x
},
{
mayer
});
SX
performance_idx
=
spectral
.
CollocateCost
(
MayerTerm
,
LagrangeTerm
,
0
,
1
);
SX
varx
=
spectral
.
VarX
();
SX
varu
=
spectral
.
VarU
();
SX
opt_var
=
SX
::
vertcat
(
SXVector
{
varx
,
varu
});
SX
lbg
=
SX
::
zeros
(
diff_constr
.
size
());
SX
ubg
=
SX
::
zeros
(
diff_constr
.
size
());
/** set inequality (box) constraints */
/** state */
SX
lbx
=
SX
::
repmat
(
SX
::
mtimes
(
Scale_X
,
LBX
),
poly_order
+
1
,
1
);
SX
ubx
=
SX
::
repmat
(
SX
::
mtimes
(
Scale_X
,
UBX
),
poly_order
+
1
,
1
);
/** control */
//lbx = SX::vertcat( {lbx, SX::repmat(SX::mtimes(Scale_U, LBU), poly_order, 1)} );
//ubx = SX::vertcat( {ubx, SX::repmat(SX::mtimes(Scale_U, UBU), poly_order, 1)} );
lbx
=
SX
::
vertcat
(
{
lbx
,
SX
::
repmat
(
SX
::
mtimes
(
Scale_U
,
LBU
),
poly_order
+
1
,
1
)}
);
ubx
=
SX
::
vertcat
(
{
ubx
,
SX
::
repmat
(
SX
::
mtimes
(
Scale_U
,
UBU
),
poly_order
+
1
,
1
)}
);
/** formulate NLP */
NLP
[
"x"
]
=
opt_var
;
NLP
[
"f"
]
=
performance_idx
;
NLP
[
"g"
]
=
diff_constr
;
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"] = 15;
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
,
poly_order
+
1
,
1
),
DM
::
repmat
(
feasible_control
,
poly_order
+
1
,
1
)});
//DM::repmat(feasible_control, poly_order, 1)});
/** ----------------------------------------------------------------------------------*/
/** 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 = kmath::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);
kmath::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 = kmath::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"] = 15;
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
;
/** rectify virtual state */
DM
X0
=
_X0
;
bool
rectify
=
false
;
if
(
X0
[
13
].
nonzeros
()[
0
]
>
2
*
M_PI
)
{
X0
[
13
]
-=
2
*
M_PI
;
rectify
=
true
;
}
else
if
(
X0
[
13
].
nonzeros
()[
0
]
<
-
2
*
M_PI
)
{
X0
[
13
]
+=
2
*
M_PI
;
rectify
=
true
;
}
/** scale input */
X0
=
DM
::
mtimes
(
Scale_X
,
X0
);
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
;
/** rectify initial guess */
if
(
rectify
)
{
for
(
int
i
=
0
;
i
<
(
N
+
1
)
*
nx
;
i
+=
nx
)
{
int
idx
=
i
+
13
;
if
(
NLP_X
[
idx
].
nonzeros
()[
0
]
>
2
*
M_PI
)
NLP_X
[
idx
]
-=
2
*
M_PI
;
else
if
(
NLP_X
[
idx
].
nonzeros
()[
0
]
<
-
2
*
M_PI
)
NLP_X
[
idx
]
+=
2
*
M_PI
;
}
}
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
+
1
));
stats
=
NLP_Solver
.
stats
();
std
::
cout
<<
stats
<<
"
\n
"
;
enableWarmStart
();
}
/** get path error */
double
KiteNMPF
::
getPathError
()
{
double
error
=
0
;
if
(
!
OptimalTrajectory
.
is_empty
())
{
DM
state
=
OptimalTrajectory
(
Slice
(
0
,
OptimalTrajectory
.
size1
()),
OptimalTrajectory
.
size2
()
-
1
);
DMVector
tmp
=
PathError
(
DMVector
{
state
});
error
=
DM
::
norm_2
(
tmp
[
0
]
).
nonzeros
()[
0
];
}
return
error
;
}
/** get velocity error */
double
KiteNMPF
::
getVelocityError
()
{
double
error
=
0
;
if
(
!
OptimalTrajectory
.
is_empty
())
{
DM
state
=
OptimalTrajectory
(
Slice
(
0
,
OptimalTrajectory
.
size1
()),
OptimalTrajectory
.
size2
()
-
1
);
DMVector
tmp
=
VelError
(
DMVector
{
state
});
error
=
DM
::
norm_2
(
tmp
[
0
]
).
nonzeros
()[
0
];
}
return
error
;
}
/** get virtual state */
double
KiteNMPF
::
getVirtState
()
{
double
virt_state
=
0
;
if
(
!
OptimalTrajectory
.
is_empty
())
{
virt_state
=
OptimalTrajectory
(
13
,
OptimalTrajectory
.
size2
()
-
1
).
nonzeros
()[
0
];
}
return
virt_state
;
}
/** compute intial guess for virtual state */
DM
KiteNMPF
::
findClosestPointOnPath
(
const
DM
&
position
,
const
DM
&
init_guess
)
{
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
=
init_guess
;
DMVector
result
=
grad
(
DMVector
{
theta_i
});
double
step
=
0.25
;
/** check for local minimum */
if
(
fabs
(
result
[
0
].
nonzeros
()[
0
])
<
tol
)
{
theta_i
=
{
M_PI_2
+
0.1
};
result
=
grad
(
DMVector
{
theta_i
});
}
/** @badcode : shitty code */
while
(
fabs
(
result
[
0
].
nonzeros
()[
0
])
>=
tol
)
{
counter
++
;
theta_i
-=
step
*
grad
(
theta_i
);
/** gradient update */
result
=
grad
(
DMVector
{
theta_i
});
if
(
counter
>
10
)
break
;
}
std
::
cout
<<
theta_i
<<
"
\n
"
;
return
theta_i
;
}
Event Timeline
Log In to Comment