Page MenuHomec4science
Files F86660641

chebyshev.hpp
No OneTemporary
Actions

File Metadata

Created
Mon, Oct 7, 20:46

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

c4science · Help