Page Menu
Home
c4science
Search
Configure Global Search
Log In
Files
F86660641
chebyshev.hpp
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
Mon, Oct 7, 20:46
Size
10 KB
Mime Type
text/x-c++
Expires
Wed, Oct 9, 20:46 (1 d, 22 h)
Engine
blob
Format
Raw Data
Handle
21465204
Attached To
R1517 test_package
chebyshev.hpp
View Options
#ifndef CHEBYSHEV_HPP
#define CHEBYSHEV_HPP
#include "kitemath.h"
template
<
class
BaseClass
,
int
PolyOrder
,
int
NumSegments
,
int
NX
,
int
NU
,
int
NP
>
class
Chebyshev
{
public
:
/** constructor */
Chebyshev
();
virtual
~
Chebyshev
(){}
BaseClass
D
(){
return
_D
;}
BaseClass
CompD
(){
return
_ComD
;}
BaseClass
CPoints
(){
return
_Points
;}
BaseClass
QWeights
(){
return
_QuadWeights
;}
BaseClass
VarX
(){
return
_X
;}
BaseClass
VarU
(){
return
_U
;}
BaseClass
CollocateDynamics
(
casadi
::
Function
&
dynamics
,
const
double
&
t0
,
const
double
&
tf
);
BaseClass
CollocateCost
(
casadi
::
Function
&
MayerTerm
,
casadi
::
Function
&
LagrangeTerm
,
const
double
&
t0
,
const
double
&
tf
);
private
:
/** generate Differentiation matrix */
BaseClass
DiffMatrix
();
/** generate Chebyshev collocation points */
BaseClass
CollocPoints
();
/** generate Clenshaw-Curtis quadrature weights */
BaseClass
QuadWeights
();
/** generate Composite Differentiation matrix */
BaseClass
CompDiffMatrix
();
/** Diff matrix */
BaseClass
_D
;
/** Composite diff matrix */
BaseClass
_ComD
;
/** Collocation points */
BaseClass
_Points
;
/** Quadrature weights */
BaseClass
_QuadWeights
;
/** helper functions */
BaseClass
range
(
const
uint
&
first
,
const
uint
&
last
,
const
uint
&
step
);
/** state in terms of Chebyshev coefficients */
BaseClass
_X
;
/** control in terms of Chebyshev coefficients */
BaseClass
_U
;
/** vector of parameters */
BaseClass
_P
;
};
/** @brief constructor */
template
<
class
BaseClass
,
int
PolyOrder
,
int
NumSegments
,
int
NX
,
int
NU
,
int
NP
>
Chebyshev
<
BaseClass
,
PolyOrder
,
NumSegments
,
NX
,
NU
,
NP
>::
Chebyshev
()
{
/** initialize pseudopsectral scheme */
_Points
=
CollocPoints
();
_D
=
DiffMatrix
();
_QuadWeights
=
QuadWeights
();
_ComD
=
CompDiffMatrix
();
/** create discretized states and controls */
_X
=
casadi
::
SX
::
sym
(
"X"
,
NumSegments
*
(
PolyOrder
+
1
)
*
NX
-
(
NumSegments
-
1
));
_U
=
casadi
::
SX
::
sym
(
"U"
,
NumSegments
*
(
PolyOrder
+
1
)
*
NU
-
(
NumSegments
-
1
)
-
NU
);
_P
=
casadi
::
SX
::
sym
(
"P"
,
NP
);
}
/** @brief range */
template
<
class
BaseClass
,
int
PolyOrder
,
int
NumSegments
,
int
NX
,
int
NU
,
int
NP
>
BaseClass
Chebyshev
<
BaseClass
,
PolyOrder
,
NumSegments
,
NX
,
NU
,
NP
>::
range
(
const
uint
&
first
,
const
uint
&
last
,
const
uint
&
step
)
{
int
numel
=
std
::
floor
((
last
-
first
)
/
step
);
BaseClass
_range
;
_range
.
reserve
(
numel
);
int
idx
=
0
;
for
(
uint
value
=
first
;
value
<=
last
;
++
value
)
{
_range
(
idx
)
=
0
;
}
return
_range
;
}
/** @brief compute collocation points */
template
<
class
BaseClass
,
int
PolyOrder
,
int
NumSegments
,
int
NX
,
int
NU
,
int
NP
>
BaseClass
Chebyshev
<
BaseClass
,
PolyOrder
,
NumSegments
,
NX
,
NU
,
NP
>::
CollocPoints
()
{
/** Chebyshev collocation points for the interval [-1, 1]*/
auto
grid_int
=
kmath
::
range
<
double
>
(
0
,
PolyOrder
);
/** cast grid to Casadi type */
BaseClass
grid
(
grid_int
);
BaseClass
X
=
cos
(
grid
*
(
M_PI
/
PolyOrder
));
return
X
;
}
/** @brief compute differentiation matrix / ref {L. Trefethen "Spectral Methods in Matlab"}*/
template
<
class
BaseClass
,
int
PolyOrder
,
int
NumSegments
,
int
NX
,
int
NU
,
int
NP
>
BaseClass
Chebyshev
<
BaseClass
,
PolyOrder
,
NumSegments
,
NX
,
NU
,
NP
>::
DiffMatrix
()
{
/** Chebyshev collocation points for the interval [-1, 1]*/
auto
grid_int
=
kmath
::
range
<
double
>
(
0
,
PolyOrder
);
/** cast grid to Casadi type */
BaseClass
grid
(
grid_int
);
BaseClass
cpoints
=
cos
(
grid
*
(
M_PI
/
PolyOrder
));
/** Diff Matrix */
BaseClass
c
=
BaseClass
::
vertcat
({
2
,
BaseClass
::
ones
(
PolyOrder
-
1
,
1
),
2
});
c
=
BaseClass
::
mtimes
(
BaseClass
::
diag
(
pow
(
-
1
,
grid
)),
c
);
BaseClass
XM
=
BaseClass
::
repmat
(
cpoints
,
1
,
PolyOrder
+
1
);
BaseClass
dX
=
XM
-
XM
.
T
();
BaseClass
Dn
=
BaseClass
::
mtimes
(
c
,
(
1
/
c
).
T
()
)
/
(
dX
+
(
BaseClass
::
eye
(
PolyOrder
+
1
)));
/** off-diagonal entries */
return
Dn
-
BaseClass
::
diag
(
BaseClass
::
sumRows
(
Dn
.
T
()
));
/** diagonal entries */
}
/** @brief compute weights for Clenshaw-Curtis quadrature / ref {L. Trefethen "Spectral Methods in Matlab"}*/
template
<
class
BaseClass
,
int
PolyOrder
,
int
NumSegments
,
int
NX
,
int
NU
,
int
NP
>
BaseClass
Chebyshev
<
BaseClass
,
PolyOrder
,
NumSegments
,
NX
,
NU
,
NP
>::
QuadWeights
()
{
/** Chebyshev collocation points for the interval [-1, 1]*/
auto
grid_int
=
kmath
::
range
<
double
>
(
0
,
PolyOrder
);
/** cast grid to Casadi type */
BaseClass
theta
(
grid_int
);
theta
=
theta
*
(
M_PI
/
PolyOrder
);
BaseClass
w
=
BaseClass
::
zeros
(
1
,
PolyOrder
+
1
);
BaseClass
v
=
BaseClass
::
ones
(
PolyOrder
-
1
,
1
);
if
(
PolyOrder
%
2
==
0
)
{
w
[
0
]
=
1
/
(
pow
(
PolyOrder
,
2
)
-
1
);
w
[
PolyOrder
]
=
w
[
0
];
for
(
int
k
=
1
;
k
<=
PolyOrder
/
2
-
1
;
++
k
)
{
v
=
v
-
2
*
cos
(
2
*
k
*
theta
(
casadi
::
Slice
(
1
,
PolyOrder
)))
/
(
4
*
pow
(
k
,
2
)
-
1
);
}
v
=
v
-
cos
(
PolyOrder
*
theta
(
casadi
::
Slice
(
1
,
PolyOrder
)))
/
(
pow
(
PolyOrder
,
2
)
-
1
);
}
else
{
w
[
0
]
=
1
/
std
::
pow
(
PolyOrder
,
2
);
w
[
PolyOrder
]
=
w
[
0
];
for
(
int
k
=
1
;
k
<=
(
PolyOrder
-
1
)
/
2
;
++
k
)
{
v
=
v
-
2
*
cos
(
2
*
k
*
theta
(
casadi
::
Slice
(
1
,
PolyOrder
)))
/
(
4
*
pow
(
k
,
2
)
-
1
);
}
}
w
(
casadi
::
Slice
(
1
,
PolyOrder
))
=
2
*
v
/
PolyOrder
;
return
w
;
}
/** @brief compute composite differentiation matrix */
template
<
class
BaseClass
,
int
PolyOrder
,
int
NumSegments
,
int
NX
,
int
NU
,
int
NP
>
BaseClass
Chebyshev
<
BaseClass
,
PolyOrder
,
NumSegments
,
NX
,
NU
,
NP
>::
CompDiffMatrix
()
{
int
comp_rows
=
NumSegments
*
(
PolyOrder
+
1
)
-
(
NumSegments
-
1
);;
int
comp_cols
=
NumSegments
*
(
PolyOrder
+
1
)
-
(
NumSegments
-
1
);
BaseClass
CompDiff
=
BaseClass
::
zeros
(
comp_rows
,
comp_cols
);
BaseClass
D
=
DiffMatrix
();
BaseClass
D0
=
D
;
D0
(
D0
.
size1
()
-
1
,
casadi
::
Slice
(
0
,
D0
.
size2
()))
=
BaseClass
::
zeros
(
PolyOrder
+
1
);
D0
(
D0
.
size1
()
-
1
,
D0
.
size2
()
-
1
)
=
1
;
BaseClass
E
=
BaseClass
::
eye
(
NX
);
int
dn_size
=
D
.
size1
();
if
(
NumSegments
<
2
)
{
CompDiff
=
D0
;
}
else
{
/** insert first matrix */
CompDiff
(
casadi
::
Slice
(
CompDiff
.
size1
()
-
D0
.
size1
(),
CompDiff
.
size1
()),
casadi
::
Slice
(
CompDiff
.
size2
()
-
D0
.
size2
(),
CompDiff
.
size2
()))
=
D0
;
/** fill in diagonal terms */
for
(
int
k
=
0
;
k
<
NumSegments
-
1
;
++
k
)
{
int
shift
=
k
>
0
?
-
1
:
0
;
int
i
=
k
*
dn_size
;
int
j
=
k
*
dn_size
+
shift
;
CompDiff
(
casadi
::
Slice
(
i
,
i
+
dn_size
-
1
),
casadi
::
Slice
(
j
,
j
+
dn_size
))
=
D
(
casadi
::
Slice
(
0
,
PolyOrder
),
casadi
::
Slice
(
0
,
D
.
size2
()));
}
}
return
BaseClass
::
kron
(
CompDiff
,
E
);
}
/** @brief collocate differential constraints */
template
<
class
BaseClass
,
int
PolyOrder
,
int
NumSegments
,
int
NX
,
int
NU
,
int
NP
>
BaseClass
Chebyshev
<
BaseClass
,
PolyOrder
,
NumSegments
,
NX
,
NU
,
NP
>::
CollocateDynamics
(
casadi
::
Function
&
dynamics
,
const
double
&
t0
,
const
double
&
tf
)
{
/** evaluate RHS at the collocation points */
int
DIMX
=
_X
.
size1
();
BaseClass
F_XU
=
BaseClass
::
zeros
(
DIMX
);
casadi
::
SXVector
tmp
;
int
j
=
0
;
double
t_scale
=
(
tf
-
t0
)
/
(
2
*
NumSegments
);
for
(
int
i
=
0
;
i
<
DIMX
-
NX
;
i
+=
NX
)
{
//std::cout << "X in : " << _X(casadi::Slice(i, i + NX)) << "\n";
//std::cout << "U in : " << _U(casadi::Slice(j, j + NU)) << "\n";
tmp
=
dynamics
(
casadi
::
SXVector
{
_X
(
casadi
::
Slice
(
i
,
i
+
NX
)),
_U
(
casadi
::
Slice
(
j
,
j
+
NU
))
});
F_XU
(
casadi
::
Slice
(
i
,
i
+
NX
))
=
t_scale
*
tmp
[
0
];
j
+=
NU
;
}
BaseClass
G_XU
=
BaseClass
::
mtimes
(
_ComD
,
_X
)
-
F_XU
;
return
G_XU
;
}
/** @brief collocate differential constraints */
template
<
class
BaseClass
,
int
PolyOrder
,
int
NumSegments
,
int
NX
,
int
NU
,
int
NP
>
BaseClass
Chebyshev
<
BaseClass
,
PolyOrder
,
NumSegments
,
NX
,
NU
,
NP
>::
CollocateCost
(
casadi
::
Function
&
MayerTerm
,
casadi
::
Function
&
LagrangeTerm
,
const
double
&
t0
,
const
double
&
tf
)
{
casadi
::
SXVector
value
;
BaseClass
Mayer
,
Lagrange
;
/** collocate Mayer term */
if
(
!
MayerTerm
.
is_null
())
{
value
=
MayerTerm
(
casadi
::
SXVector
{
_X
(
casadi
::
Slice
(
0
,
NX
))});
Mayer
=
value
[
0
];
}
/** collocate Lagrange term */
Lagrange
=
{
0
};
if
(
!
LagrangeTerm
.
is_null
())
{
/** for each segment */
double
t_scale
=
(
tf
-
t0
)
/
(
2
*
NumSegments
);
for
(
int
k
=
0
;
k
<
NumSegments
;
++
k
)
{
BaseClass
local_int
=
{
0
};
int
j
=
k
*
NU
*
PolyOrder
;
int
m
=
0
;
for
(
int
i
=
k
*
NX
*
PolyOrder
;
i
<=
(
k
+
1
)
*
NX
*
PolyOrder
;
i
+=
NX
)
{
/** extrapolate the first control points : u[-1] */
if
(
j
>
_U
.
size1
()
-
NX
)
{
BaseClass
U0
=
{
0
};
BaseClass
U
=
_U
(
casadi
::
Slice
(
_U
.
size1
()
-
NX
*
(
PolyOrder
),
_U
.
size1
()));
for
(
int
n
=
0
;
n
<
PolyOrder
*
NU
;
n
+=
NU
)
U0
+=
pow
(
-
1
,
n
)
*
U
(
casadi
::
Slice
(
n
,
n
+
NU
));
value
=
LagrangeTerm
(
casadi
::
SXVector
{
_X
(
casadi
::
Slice
(
i
,
i
+
NX
)),
U0
});
}
else
value
=
LagrangeTerm
(
casadi
::
SXVector
{
_X
(
casadi
::
Slice
(
i
,
i
+
NX
)),
_U
(
casadi
::
Slice
(
j
,
j
+
NU
))});
local_int
+=
_QuadWeights
[
m
]
*
value
[
0
];
j
+=
NU
;
++
m
;
}
std
::
cout
<<
"Local Integral : [ "
<<
k
<<
" ] : "
<<
local_int
<<
"
\n
"
;
Lagrange
+=
t_scale
*
local_int
;
}
}
return
Mayer
+
Lagrange
;
}
#endif
// CHEBYSHEV_HPP
Event Timeline
Log In to Comment