Page Menu
Home
c4science
Search
Configure Global Search
Log In
Files
F66934697
kite.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, Jun 18, 23:24
Size
17 KB
Mime Type
text/x-c
Expires
Thu, Jun 20, 23:24 (1 d, 23 h)
Engine
blob
Format
Raw Data
Handle
18318643
Attached To
R1517 test_package
kite.cpp
View Options
#include "kite.h"
using
namespace
casadi
;
namespace
kite_utils
{
KiteProperties
LoadProperties
(
const
std
::
string
&
filename
)
{
//read YAML config file
YAML
::
Node
config
=
YAML
::
LoadFile
(
filename
);
//create properties object and fill in with data
KiteProperties
props
;
props
.
Name
=
config
[
"name"
].
as
<
std
::
string
>
();
props
.
Geometry
.
WingSpan
=
config
[
"geometry"
][
"b"
].
as
<
double
>
();
props
.
Geometry
.
MAC
=
config
[
"geometry"
][
"c"
].
as
<
double
>
();
props
.
Geometry
.
AspectRatio
=
config
[
"geometry"
][
"AR"
].
as
<
double
>
();
props
.
Geometry
.
WingSurfaceArea
=
config
[
"geometry"
][
"S"
].
as
<
double
>
();
props
.
Geometry
.
TaperRatio
=
config
[
"geometry"
][
"lam"
].
as
<
double
>
();
props
.
Geometry
.
HTailsurface
=
config
[
"geometry"
][
"St"
].
as
<
double
>
();
props
.
Geometry
.
TailLeverArm
=
config
[
"geometry"
][
"lt"
].
as
<
double
>
();
props
.
Geometry
.
FinSurfaceArea
=
config
[
"geometry"
][
"Sf"
].
as
<
double
>
();
props
.
Geometry
.
FinLeverArm
=
config
[
"geometry"
][
"lf"
].
as
<
double
>
();
props
.
Geometry
.
AerodynamicCenter
=
config
[
"geometry"
][
"Xac"
].
as
<
double
>
();
props
.
Inertia
.
Mass
=
config
[
"inertia"
][
"mass"
].
as
<
double
>
();
props
.
Inertia
.
Ixx
=
config
[
"inertia"
][
"Ixx"
].
as
<
double
>
();
props
.
Inertia
.
Iyy
=
config
[
"inertia"
][
"Iyy"
].
as
<
double
>
();
props
.
Inertia
.
Izz
=
config
[
"inertia"
][
"Izz"
].
as
<
double
>
();
props
.
Inertia
.
Ixz
=
config
[
"inertia"
][
"Ixz"
].
as
<
double
>
();
props
.
Aerodynamics
.
CL0
=
config
[
"aerodynamic"
][
"CL0"
].
as
<
double
>
();
props
.
Aerodynamics
.
CL0_tail
=
config
[
"aerodynamic"
][
"CL0_tail"
].
as
<
double
>
();
props
.
Aerodynamics
.
CLa_total
=
config
[
"aerodynamic"
][
"CLa_total"
].
as
<
double
>
();
props
.
Aerodynamics
.
CLa_wing
=
config
[
"aerodynamic"
][
"CLa_wing"
].
as
<
double
>
();
props
.
Aerodynamics
.
CLa_tail
=
config
[
"aerodynamic"
][
"CLa_tail"
].
as
<
double
>
();
props
.
Aerodynamics
.
e_oswald
=
config
[
"aerodynamic"
][
"e_oswald"
].
as
<
double
>
();
props
.
Aerodynamics
.
CD0_total
=
config
[
"aerodynamic"
][
"CD0_total"
].
as
<
double
>
();
props
.
Aerodynamics
.
CD0_wing
=
config
[
"aerodynamic"
][
"CD0_wing"
].
as
<
double
>
();
props
.
Aerodynamics
.
CD0_tail
=
config
[
"aerodynamic"
][
"CD0_tail"
].
as
<
double
>
();
props
.
Aerodynamics
.
CYb
=
config
[
"aerodynamic"
][
"CYb"
].
as
<
double
>
();
props
.
Aerodynamics
.
CYb_vtail
=
config
[
"aerodynamic"
][
"CYb_vtail"
].
as
<
double
>
();
props
.
Aerodynamics
.
Cm0
=
config
[
"aerodynamic"
][
"Cm0"
].
as
<
double
>
();
props
.
Aerodynamics
.
Cma
=
config
[
"aerodynamic"
][
"Cma"
].
as
<
double
>
();
props
.
Aerodynamics
.
Cn0
=
config
[
"aerodynamic"
][
"Cn0"
].
as
<
double
>
();
props
.
Aerodynamics
.
Cnb
=
config
[
"aerodynamic"
][
"Cnb"
].
as
<
double
>
();
props
.
Aerodynamics
.
Cl0
=
config
[
"aerodynamic"
][
"Cl0"
].
as
<
double
>
();
props
.
Aerodynamics
.
Clb
=
config
[
"aerodynamic"
][
"Clb"
].
as
<
double
>
();
props
.
Aerodynamics
.
CLq
=
config
[
"aerodynamic"
][
"CLq"
].
as
<
double
>
();
props
.
Aerodynamics
.
Cmq
=
config
[
"aerodynamic"
][
"Cmq"
].
as
<
double
>
();
props
.
Aerodynamics
.
CYr
=
config
[
"aerodynamic"
][
"CYr"
].
as
<
double
>
();
props
.
Aerodynamics
.
Cnr
=
config
[
"aerodynamic"
][
"Cnr"
].
as
<
double
>
();
props
.
Aerodynamics
.
Clr
=
config
[
"aerodynamic"
][
"Clr"
].
as
<
double
>
();
props
.
Aerodynamics
.
CYp
=
config
[
"aerodynamic"
][
"CYp"
].
as
<
double
>
();
props
.
Aerodynamics
.
Clp
=
config
[
"aerodynamic"
][
"Clp"
].
as
<
double
>
();
props
.
Aerodynamics
.
Cnp
=
config
[
"aerodynamic"
][
"Cnp"
].
as
<
double
>
();
props
.
Aerodynamics
.
CLde
=
config
[
"aerodynamic"
][
"CLde"
].
as
<
double
>
();
props
.
Aerodynamics
.
CYdr
=
config
[
"aerodynamic"
][
"CYdr"
].
as
<
double
>
();
props
.
Aerodynamics
.
Cmde
=
config
[
"aerodynamic"
][
"Cmde"
].
as
<
double
>
();
props
.
Aerodynamics
.
Cndr
=
config
[
"aerodynamic"
][
"Cndr"
].
as
<
double
>
();
props
.
Aerodynamics
.
Cldr
=
config
[
"aerodynamic"
][
"Cldr"
].
as
<
double
>
();
props
.
Aerodynamics
.
CDde
=
config
[
"aerodynamic"
][
"CDde"
].
as
<
double
>
();
if
(
config
[
"tether_length"
])
props
.
Tether_length
=
config
[
"tether_length"
].
as
<
double
>
();
else
{
std
::
cout
<<
"WARNING: KITE DYNAMICS: TETHER LENGTH NOT SPECIFIED- USING DEFAULT VALUE
\n
"
;
props
.
Tether_length
=
50
;
}
return
props
;
}
SX
quat_multiply
(
const
SX
&
q1
,
const
SX
&
q2
)
{
SX
s1
=
q1
[
0
];
SX
v1
=
q1
(
Slice
(
1
,
4
),
0
);
SX
s2
=
q2
[
0
];
SX
v2
=
q2
(
Slice
(
1
,
4
),
0
);
SX
s
=
(
s1
*
s2
)
-
SX
::
dot
(
v1
,
v2
);
SX
v
=
SX
::
cross
(
v1
,
v2
)
+
(
s1
*
v2
)
+
(
s2
*
v1
);
SXVector
tmp
{
s
,
v
};
return
SX
::
vertcat
(
tmp
);
}
SX
quat_inverse
(
const
SX
&
q
)
{
SXVector
tmp
{
q
[
0
],
-
q
[
1
],
-
q
[
2
],
-
q
[
3
]};
return
SX
::
vertcat
(
tmp
);
}
SX
heaviside
(
const
SX
&
x
,
const
double
K
)
{
return
K
/
(
1
+
exp
(
-
4
*
x
));
}
SX
rk4_symbolic
(
const
SX
&
x
,
const
SX
&
u
,
Function
&
func
,
const
SX
&
h
)
{
SXVector
res
=
func
(
SXVector
{
x
,
u
});
SX
k1
=
res
[
0
];
res
=
func
(
SXVector
{
x
+
0.5
*
h
*
k1
,
u
});
SX
k2
=
res
[
0
];
res
=
func
(
SXVector
{
x
+
0.5
*
h
*
k2
,
u
});
SX
k3
=
res
[
0
];
res
=
func
(
SXVector
{
x
+
h
*
k3
,
u
});
SX
k4
=
res
[
0
];
return
x
+
(
h
/
6
)
*
(
k1
+
2
*
k2
+
2
*
k3
+
k4
);
}
std
::
chrono
::
time_point
<
std
::
chrono
::
system_clock
>
get_time
()
{
/** OS dependent */
#ifdef __APPLE__
return
std
::
chrono
::
system_clock
::
now
();
#elif
return
std
::
chrono
::
high_resolution_clock
::
now
();
#endif
}
}
KiteDynamics
::
KiteDynamics
(
const
KiteProperties
&
KiteProps
,
const
AlgorithmProperties
&
AlgoProps
)
{
/** enviromental constants */
const
double
g
=
9.80665
;
/** gravitational acceleration [m/s2] [WGS84] */
const
double
ro
=
1.2985
;
/** standart atmospheric density [kg/m3] [Standart Atmosphere 1976] */
/** --------------------- **/
/** Geometric parameters **/
/** -------------------- **/
double
b
=
KiteProps
.
Geometry
.
WingSpan
;
double
c
=
KiteProps
.
Geometry
.
MAC
;
double
AR
=
KiteProps
.
Geometry
.
AspectRatio
;
double
S
=
KiteProps
.
Geometry
.
WingSurfaceArea
;
//double lam = KiteProps.Geometry.TaperRatio;
//double St = KiteProps.Geometry.HTailsurface;
//double lt = KiteProps.Geometry.TailLeverArm;
//double Sf = KiteProps.Geometry.FinSurfaceArea;
//double lf = KiteProps.Geometry.FinLeverArm;
//double Xac = KiteProps.Geometry.AerodynamicCenter;
/** @todo: get rid of all magic numbers **/
//double Xcg = 0.031/c; /** Center of Gravity (CoG) wrt leading edge [1/c] **/
//double Vf = (Sf * lf) / (S * b); /** fin volume coefficient [] **/
//double Vh = (lt * St) / (S * c); /** horizontal tail volume coefficient [] **/
/** --------------------------- **/
/** Mass and inertia parameters **/
/** --------------------------- **/
double
Mass
=
KiteProps
.
Inertia
.
Mass
;
double
Ixx
=
KiteProps
.
Inertia
.
Ixx
;
double
Iyy
=
KiteProps
.
Inertia
.
Iyy
;
double
Izz
=
KiteProps
.
Inertia
.
Izz
;
double
Ixz
=
KiteProps
.
Inertia
.
Ixz
;
/** ------------------------------- **/
/** Static aerodynamic coefficients **/
/** ------------------------------- **/
double
CL0
=
KiteProps
.
Aerodynamics
.
CL0
;
//double CL0_t = KiteProps.Aerodynamics.CL0_tail;
double
CLa_tot
=
KiteProps
.
Aerodynamics
.
CLa_total
;
//double CLa_w = KiteProps.Aerodynamics.CLa_wing;
//double CLa_t = KiteProps.Aerodynamics.CLa_tail;
double
e_o
=
KiteProps
.
Aerodynamics
.
e_oswald
;
//double dw = CLa_tot / (pi * e_o * AR); /** downwash acting at the tail [] **/
double
CD0_tot
=
KiteProps
.
Aerodynamics
.
CD0_total
;
//double CD0_w = KiteProps.Aerodynamics.CD0_wing;
//double CD0_t = KiteProps.Aerodynamics.CD0_tail;
double
CYb
=
KiteProps
.
Aerodynamics
.
CYb
;
//double CYb_vt = KiteProps.Aerodynamics.CYb_vtail;
double
Cm0
=
KiteProps
.
Aerodynamics
.
Cm0
;
double
Cma
=
KiteProps
.
Aerodynamics
.
Cma
;
double
Cn0
=
KiteProps
.
Aerodynamics
.
Cn0
;
double
Cnb
=
KiteProps
.
Aerodynamics
.
Cnb
;
double
Cl0
=
KiteProps
.
Aerodynamics
.
Cl0
;
double
Clb
=
KiteProps
.
Aerodynamics
.
Clb
;
double
CLq
=
KiteProps
.
Aerodynamics
.
CLq
;
double
Cmq
=
KiteProps
.
Aerodynamics
.
Cmq
;
double
CYr
=
KiteProps
.
Aerodynamics
.
CYr
;
double
Cnr
=
KiteProps
.
Aerodynamics
.
Cnr
;
double
Clr
=
KiteProps
.
Aerodynamics
.
Clr
;
double
CYp
=
KiteProps
.
Aerodynamics
.
CYp
;
double
Clp
=
KiteProps
.
Aerodynamics
.
Clp
;
double
Cnp
=
KiteProps
.
Aerodynamics
.
Cnp
;
/** ------------------------------ **/
/** Aerodynamic effects of control **/
/** ------------------------------ **/
double
CLde
=
KiteProps
.
Aerodynamics
.
CLde
;
double
CYdr
=
KiteProps
.
Aerodynamics
.
CYdr
;
double
Cmde
=
KiteProps
.
Aerodynamics
.
Cmde
;
double
Cndr
=
KiteProps
.
Aerodynamics
.
Cndr
;
double
Cldr
=
KiteProps
.
Aerodynamics
.
Cldr
;
//double CDde = KiteProps.Aerodynamics.CDde;
//double CL_daoa = -2 * CLa_t * Vh * dw;
//double Cm_daoa = -2 * CLa_t * Vh * (lt/c) * dw;
/** -------------------------- **/
/** State variables definition **/
/** -------------------------- **/
SX
r
=
SX
::
sym
(
"r"
,
3
);
/** position of the CoG in the Inertial Reference Frame (IRF) [m] **/
SX
q
=
SX
::
sym
(
"q"
,
4
);
/** body attitude relative to IRF [unit quaternion] **/
SX
v
=
SX
::
sym
(
"v"
,
3
);
/** linear velocity of the CoG in the Body Reference Frame (BRF) [m/s] **/
SX
w
=
SX
::
sym
(
"w"
,
3
);
/** glider angular velocity in BRF [rad/s] **/
/** ---------------------------- **/
/** Control variables definition **/
/** ---------------------------- **/
/** @todo: consider more detailed propeller model **/
SX
T
=
SX
::
sym
(
"T"
);
/** propeller propulsion : applies along X-axis in BRF [N] **/
SX
dE
=
SX
::
sym
(
"dE"
);
/** elevator deflection [positive down] [rad] **/
SX
dR
=
SX
::
sym
(
"dR"
);
/** rudder deflection [rad] **/
//SX dA = SX::sym("dA"); /** aileron deflection [reserved, but not used] [rad] **/
//SX dF = SX::sym("dF"); /** flaps deflection [reserved, but not used] **/
/** ---------------------------- **/
/** Tether **/
/** ---------------------------- **/
double
Lt
=
KiteProps
.
Tether_length
;
/** @todo: read algorithm params here **/
/** @todo: add wind to the model **/
SX
V
=
SX
::
norm_2
(
v
);
SX
V2
=
SX
::
dot
(
v
,
v
);
SX
ss
=
asin
(
v
[
1
]
/
V
);
/** side slip angle [rad] (v(3)/v(1)) // small angle assumption **/
SX
aoa
=
atan2
(
v
[
2
]
,
v
[
0
]);
/** angle of attack definition [rad] (v(2)/L2(v)) **/
SX
dyn_press
=
0.5
*
ro
*
V2
;
/** dynamic pressure **/
SX
CD
=
CD0_tot
+
pow
(
CL0
+
CLa_tot
*
aoa
,
2
)
/
(
pi
*
e_o
*
AR
);
/** total drag coefficient **/
/** ------------------------- **/
/** Dynamic Equations: Forces */
/** ------------------------- **/
SX
LIFT
=
(
CL0
+
CLa_tot
*
aoa
)
*
dyn_press
*
S
+
(
0.25
*
CLq
*
c
*
S
*
ro
)
*
V
*
w
[
1
];
SX
DRAG
=
CD
*
dyn_press
*
S
;
SX
SF
=
(
CYb
*
ss
+
CYdr
*
dR
)
*
dyn_press
*
S
+
0.25
*
(
CYr
*
w
[
2
]
+
CYp
*
w
[
0
])
*
(
b
*
ro
*
S
)
*
V
;
/** Compute transformation betweeen WRF and BRF: qw_b **/
/** qw_b = q(aoa) * q(-ss); **/
SX
q_aoa
=
SX
::
vertcat
({
cos
(
aoa
/
2
),
0
,
sin
(
aoa
/
2
),
0
});
SX
q_ss
=
SX
::
vertcat
({
cos
(
-
ss
/
2
),
0
,
0
,
sin
(
-
ss
/
2
)});
SX
qw_b
=
kite_utils
::
quat_multiply
(
q_aoa
,
q_ss
);
SX
qw_b_inv
=
kite_utils
::
quat_inverse
(
qw_b
);
/** Aerodynamic forces in BRF: Faer0_b = qw_b * [0; -DRAG; SF; -LIFT] * qw_b_inv */
SX
qF_tmp
=
kite_utils
::
quat_multiply
(
qw_b_inv
,
SX
::
vertcat
({
0
,
-
DRAG
,
0
,
-
LIFT
}));
SX
qF_q
=
kite_utils
::
quat_multiply
(
qF_tmp
,
qw_b
);
SX
Faero_b
=
qF_q
(
Slice
(
1
,
4
),
0
);
SX
Zde
=
(
-
CLde
)
*
dE
*
dyn_press
*
S
;
SX
FdE_tmp
=
kite_utils
::
quat_multiply
(
kite_utils
::
quat_inverse
(
q_aoa
),
SX
::
vertcat
({
0
,
0
,
0
,
Zde
}));
SX
qFdE
=
kite_utils
::
quat_multiply
(
FdE_tmp
,
q_aoa
);
SX
FdE
=
qFdE
(
Slice
(
1
,
4
),
0
);
Faero_b
=
Faero_b
+
FdE
+
SX
::
vertcat
({
0
,
SF
,
0
});
/** Gravity force in BRF */
SX
qG
=
kite_utils
::
quat_multiply
(
kite_utils
::
quat_inverse
(
q
),
SX
::
vertcat
({
0
,
0
,
0
,
g
}));
SX
qG_q
=
kite_utils
::
quat_multiply
(
qG
,
q
);
SX
G_b
=
qG_q
(
Slice
(
1
,
4
),
0
);
/** Propulsion force in BRF */
SX
T_b
=
SX
::
vertcat
({
T
,
0
,
0
});
/** Tether force */
/** value: using smooth ramp approximation */
SX
d_
=
SX
::
norm_2
(
r
);
/** spring term */
/** @todo: put all coefficients in the config */
const
double
Ks
=
15
*
Mass
;
const
double
Kd
=
10
*
Mass
;
SX
Rv
=
((
d_
-
Lt
));
SX
Rs
=
-
Rv
*
(
r
/
d_
);
/** damping term */
SX
qvi
=
kite_utils
::
quat_multiply
(
q
,
SX
::
vertcat
({
0
,
v
}));
SX
qvi_q
=
kite_utils
::
quat_multiply
(
qvi
,
kite_utils
::
quat_inverse
(
q
));
SX
vi
=
qvi_q
(
Slice
(
1
,
4
),
0
);
SX
Rd
=
(
-
r
/
d_
)
*
SX
::
dot
(
r
,
vi
)
/
d_
;
SX
R
=
(
Ks
*
Rs
+
Kd
*
Rd
)
*
kite_utils
::
heaviside
(
d_
-
Lt
,
1
);
/** BRF */
SX
qR
=
kite_utils
::
quat_multiply
(
kite_utils
::
quat_inverse
(
q
),
SX
::
vertcat
({
0
,
R
}));
SX
qR_q
=
kite_utils
::
quat_multiply
(
qR
,
q
);
SX
R_b
=
qR_q
(
Slice
(
1
,
4
),
0
);
/** Total external forces devided by glider's mass (linear acceleration) */
/** @todo : ADD TETHER FORCE */
auto
v_dot
=
(
Faero_b
+
T_b
+
R_b
)
/
Mass
+
G_b
-
SX
::
cross
(
w
,
v
);
/** ------------------------- */
/** Dynamic Equation: Moments */
/** ------------------------- */
/** Rolling Aerodynamic Moment */
SX
L
=
(
Cl0
+
Clb
*
ss
+
Cldr
*
dR
)
*
dyn_press
*
S
*
b
+
(
Clr
*
w
[
2
]
+
Clp
*
w
[
0
])
*
(
0.25
*
ro
*
std
::
pow
(
b
,
2
)
*
S
)
*
V
;
/** Pitching Aerodynamic Moment */
SX
M
=
(
Cm0
+
Cma
*
aoa
+
Cmde
*
dE
)
*
dyn_press
*
S
*
c
+
Cmq
*
(
0.25
*
S
*
std
::
pow
(
c
,
2
)
*
ro
)
*
w
[
1
]
*
V
;
/** Yawing Aerodynamic Moment */
SX
N
=
(
Cn0
+
Cnb
*
ss
+
Cndr
*
dR
)
*
dyn_press
*
S
*
b
+
(
Cnp
*
w
[
0
]
+
Cnr
*
w
[
2
])
*
(
0.25
*
S
*
std
::
pow
(
b
,
2
)
*
ro
)
*
V
;
/** Aircraft Inertia Matrix */
SXVector
j_vec
{
Ixx
,
Iyy
,
Izz
};
auto
J
=
SX
::
diag
(
SX
::
vertcat
(
j_vec
));
J
(
0
,
2
)
=
Ixz
;
J
(
2
,
0
)
=
Ixz
;
/** Angular motion equationin BRF */
/** Moments transformation SRF -> BRF */
SX
T_tmp
=
kite_utils
::
quat_multiply
(
kite_utils
::
quat_inverse
(
q_aoa
),
SX
::
vertcat
({
0
,
L
,
M
,
N
}));
SX
Trot
=
kite_utils
::
quat_multiply
(
T_tmp
,
q_aoa
);
auto
Maero
=
Trot
(
Slice
(
1
,
4
),
0
);
auto
w_dot
=
SX
::
mtimes
(
SX
::
inv
(
J
),
(
Maero
-
SX
::
cross
(
w
,
SX
::
mtimes
(
J
,
w
)))
);
/** ----------------------------- */
/** Kinematic Equations: Position */
/** ----------------------------- */
/** Aircraft position in the IRF */
SX
qv
=
kite_utils
::
quat_multiply
(
q
,
SX
::
vertcat
({
0
,
v
}));
SX
qv_q
=
kite_utils
::
quat_multiply
(
qv
,
kite_utils
::
quat_inverse
(
q
));
auto
r_dot
=
qv_q
(
Slice
(
1
,
4
),
0
);
/** ----------------------------- */
/** Kinematic Equations: Attitude */
/** ----------------------------- */
/** Aircraft attitude wrt to IRF */
auto
q_dot
=
0.5
*
kite_utils
::
quat_multiply
(
q
,
SX
::
vertcat
({
0
,
w
}));
/** Complete dynamics of the Kite */
auto
state
=
SX
::
vertcat
({
v
,
w
,
r
,
q
});
auto
control
=
SX
::
vertcat
({
T
,
dE
,
dR
});
auto
dynamics
=
SX
::
vertcat
({
v_dot
,
w_dot
,
r_dot
,
q_dot
});
Function
dyn_func
=
Function
(
"dynamics"
,
{
state
,
control
},
{
dynamics
});
/** compute dynamics state Jacobian */
SX
d_jacobian
=
SX
::
jacobian
(
dynamics
,
state
);
Function
dyn_jac
=
Function
(
"dyn_jacobian"
,{
state
,
control
},
{
d_jacobian
});
/** define RK4 integrator scheme */
SX
X
=
SX
::
sym
(
"X"
,
13
);
SX
U
=
SX
::
sym
(
"U"
,
3
);
SX
dT
=
SX
::
sym
(
"dT"
);
/** get symbolic expression for RK4 integrator */
SX
sym_integrator
=
kite_utils
::
rk4_symbolic
(
X
,
U
,
dyn_func
,
dT
);
Function
RK4_INT
=
Function
(
"RK4"
,
{
X
,
U
,
dT
},{
sym_integrator
});
/** CVODES integrator */
/** @todo: make smarter initialisation of integrator */
double
h
=
AlgoProps
.
sampling_time
;
SXDict
ode
=
{{
"x"
,
state
},
{
"p"
,
control
},
{
"ode"
,
dynamics
}};
Dict
opts
=
{{
"tf"
,
h
}};
Function
CVODES_INT
=
integrator
(
"CVODES_INT"
,
"cvodes"
,
ode
,
opts
);
/** assign class atributes */
this
->
State
=
state
;
this
->
Control
=
control
;
this
->
SymDynamics
=
dynamics
;
this
->
SymIntegartor
=
sym_integrator
;
this
->
SymJacobian
=
d_jacobian
;
this
->
NumDynamics
=
dyn_func
;
this
->
NumJacobian
=
dyn_jac
;
/** return integrator function */
if
(
AlgoProps
.
Integrator
==
IntType
::
CVODES
)
this
->
NumIntegrator
=
CVODES_INT
;
else
this
->
NumIntegrator
=
RK4_INT
;
}
/** --------------------- */
/** Rigid Body Kinematics */
/** --------------------- */
RigidBodyKinematics
::
RigidBodyKinematics
(
const
AlgorithmProperties
&
AlgoProps
)
{
algo_props
=
AlgoProps
;
/** define system dynamics */
SX
r
=
SX
::
sym
(
"r"
,
3
);
SX
q
=
SX
::
sym
(
"q"
,
4
);
SX
vb
=
SX
::
sym
(
"vb"
,
3
);
SX
wb
=
SX
::
sym
(
"wb"
,
3
);
SX
u
=
SX
::
sym
(
"u"
,
3
);
/** translation */
SX
qv
=
kite_utils
::
quat_multiply
(
q
,
SX
::
vertcat
({
0
,
vb
}));
SX
qv_q
=
kite_utils
::
quat_multiply
(
qv
,
kite_utils
::
quat_inverse
(
q
));
SX
rdot
=
qv_q
(
Slice
(
1
,
4
),
0
);
/** rotation / with norm correction*/
double
lambda
=
-
10
;
SX
qdot
=
0.5
*
kite_utils
::
quat_multiply
(
q
,
SX
::
vertcat
({
0
,
wb
}))
+
0.5
*
lambda
*
q
*
(
SX
::
dot
(
q
,
q
)
-
1
);
/** velocities */
SX
vb_dot
=
SX
::
zeros
(
3
);
SX
wb_dot
=
SX
::
zeros
(
3
);
state
=
SX
::
vertcat
({
vb
,
wb
,
r
,
q
});
SX
Dynamics
=
SX
::
vertcat
({
vb_dot
,
wb_dot
,
rdot
,
qdot
});
NumDynamics
=
Function
(
"RB_Dynamics"
,
{
state
},
{
Dynamics
});
/** Jacobian */
SX
Jacobian
=
SX
::
jacobian
(
Dynamics
,
state
);
NumJacobian
=
Function
(
"RB_Jacobian"
,
{
state
,
u
},
{
Jacobian
});
/** Integrators */
/** CVODES */
double
h
=
algo_props
.
sampling_time
;
SXDict
ode
=
{{
"x"
,
state
},
{
"ode"
,
Dynamics
}};
Dict
opts
=
{{
"tf"
,
h
}};
Function
CVODES_INT
=
integrator
(
"CVODES_INT"
,
"cvodes"
,
ode
,
opts
);
NumIntegartor
=
CVODES_INT
;
}
Event Timeline
Log In to Comment