Homec4science
Diffusion test_package (simple_kite) Browse

R1517/src/kite_control/kite_control_test.cpp
97fc1e2c3e71simple_kite

Last Change
Raw File

kite_control_test.cpp
View Options

#include "kiteEKF.h"
#include "kiteNMPF.h"
#define BOOST_TEST_MODULE kite_control_test
#include <boost/test/included/unit_test.hpp>
#include <fstream>
#include "pseudospectral/chebyshev.hpp"
#include <unordered_set>
using namespace casadi;
BOOST_AUTO_TEST_SUITE( kite_control_suite_test )
BOOST_AUTO_TEST_CASE( integrator_test )
{
AlgorithmProperties algo_props;
algo_props.Integrator = CVODES;
algo_props.sampling_time = 0.02;
/** create a rigid body object */
RigidBodyKinematics rigid_body = RigidBodyKinematics(algo_props);
Function fIntegrator = rigid_body.getNumericIntegrator();
Function fJacobian = rigid_body.getNumericJacobian();
Function fDynamics = rigid_body.getNumericDynamcis();
/** perform integration step */
DM init_state = DM::vertcat({-0.318732, 0.182552, 0.254833, 1.85435, -0.142882, -0.168359,
-0.229383, -0.0500282, -0.746832, 0.189409, -0.836349, -0.48178, 0.180367});
DM dynamics = fDynamics(DMVector{init_state});
std::cout << "Dynamics: " << dynamics[0] << "\n";
DM control = DM::zeros(3);
//DMDict args = {{"x0", init_state}, {"p", control}};
DMDict args = {{"x0", init_state}};
DMDict out = fIntegrator(args);
DM new_state = out["xf"];
std::cout << "Integration: init_state: " << init_state << "\n";
std::cout << "Integration: new_state: " << new_state << "\n" << "\n";
BOOST_CHECK(true);
}
BOOST_AUTO_TEST_CASE( ekf_test )
{
/** make test data */
double dt = 0.0084;
DM control = DM::vertcat({0,0,0});
DM measurement = DM::vertcat({1.4522, -3.1274, -1.7034, -0.5455, -0.2382, -0.2922, -0.7485});
DM x_est = DM::vertcat({6.0026, -0.3965, 0.1705,
0.4414, -0.2068, 0.9293,
1.4634, -3.1765, -1.7037,
-0.5486, -0.2354, -0.2922, -0.7471});
/** reference estimation from matlab */
DM reference_est = DM::vertcat({5.9982, -0.3819, 0.1637,
0.3578, -0.1900, 0.8774,
1.4522, -3.1274, -1.7034,
-0.5455, -0.2382, -0.2922, -0.7485});
/** create a filter and perform estimation */
std::string kite_config_file = "umx_radian.yaml";
KiteProperties kite_props = kite_utils::LoadProperties(kite_config_file);
AlgorithmProperties algo_props;
algo_props.Integrator = RK4;
KiteEKF estimator = KiteEKF(kite_props, algo_props);
estimator.setControl(control);
estimator.setEstimation(x_est);
std::chrono::time_point<std::chrono::system_clock> start = kite_utils::get_time();
estimator._estimate(measurement, dt);
x_est = estimator.getEstimation();
//estimator.propagate(dt);
std::chrono::time_point<std::chrono::system_clock> stop = kite_utils::get_time();
auto duration = std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
auto latency = static_cast<double>(duration.count());
std::cout << "EKF_TEST COMPUTATION TIME: " << latency * 1e-6 << " [seconds]" << "\n";
//BOOST_CHECK(DM::norm_inf(x_est - reference_est).nonzeros()[0] < 0.01);
BOOST_CHECK(true);
}
BOOST_AUTO_TEST_CASE( lqr_test )
{
/** lyapunov test */
Eigen::MatrixXd A(2,2), Q(2,2);
A << 1, 2, -3, -4;
Q << 3, 1, 1, 1;
kite_utils::time_point start = kite_utils::get_time();
Eigen::MatrixXd X = kmath::oc::lyapunov(A, Q);
kite_utils::time_point finish = kite_utils::get_time();
auto duration = std::chrono::duration_cast<std::chrono::microseconds>(finish - start);
auto latency = static_cast<double>(duration.count());
std::cout << "Lyapunov equation solution: \n" << X << "\n";
std::cout << (A * X + X * A.transpose()) << "\n";
std::cout << "Time elapsed : " << latency * 1e-6 << " [seconds]" << "\n";
/** PINV test */
start = kite_utils::get_time();
Eigen::MatrixXd pinvA = kmath::oc::pinv(A);
finish = kite_utils::get_time();
duration = std::chrono::duration_cast<std::chrono::microseconds>(finish - start);
latency = static_cast<double>(duration.count());
std::cout << "pinv: " << pinvA << "\n";
std::cout << "Time elapsed : " << latency * 1e-6 << " [seconds]" << "\n";
/** INIT Newton test */
X = kmath::oc::init_newton_care(A, Q);
std::cout << "Init: \n" << X << "\n";
Eigen::MatrixXd A1(2,2), B1(2,2), C1(2,2);
A1 << -3, 2, 1, 1;
B1 << 0, 0, 0, 1;
C1 << 1, -1, -1, 1;
start = kite_utils::get_time();
Eigen::MatrixXd S = kmath::oc::care(A1, B1, C1);
finish = kite_utils::get_time();
duration = std::chrono::duration_cast<std::chrono::microseconds>(finish - start);
latency = static_cast<double>(duration.count());
std::cout << "CARE solved: \n" << S << "\n";
std::cout << "Time elapsed : " << latency * 1e-6 << " [seconds]" << "\n";
std::cout <<"-----------------------Controllability test------------------------\n";
Eigen::MatrixXd Ac(2,2), Bc(2,2), Qc(2,2), R(2,2), M(2,2);
Ac << 1,1,4,-2;
Bc << Eigen::MatrixXd::Identity(2,2);
Qc = Eigen::MatrixXd::Identity(2,2);
R = Eigen::MatrixXd::Identity(2,2);
M = Eigen::MatrixXd::Zero(2,2); //0.25 * Eigen::MatrixXd::Identity(2,2);
kmath::LinearSystem sys(Ac, Bc, Eigen::MatrixXd());
std::cout << "Controllable: " << sys.is_controllable() << "\n";
std::cout <<"-----------------------LQR test------------------------\n";
start = kite_utils::get_time();
Eigen::MatrixXd K = kmath::oc::lqr(sys, Qc, R, M);
finish = kite_utils::get_time();
duration = std::chrono::duration_cast<std::chrono::microseconds>(finish - start);
latency = static_cast<double>(duration.count());
std::cout << "Feedback matrix: \n" << K << "\n";
std::cout << "Time elapsed : " << latency * 1e-6 << " [seconds]" << "\n";
BOOST_CHECK(true);
}
SX ode(const SX &x, const SX &u, const SX &p)
{
SX f = SX::zeros(2,1);
f[0] = x[1] + u[0];
f[1] = u[1];
return f;
}
BOOST_AUTO_TEST_CASE( pseudo_test )
{
const int poly_order = 2;
const int num_segments = 3;
const int dimx = 2;
const int dimu = 2;
const int dimp = 1;
Chebyshev<SX, poly_order, num_segments, dimx, dimu, dimp> cheb;
std::cout << "----------------------------------------------- \n";
std::cout << "Collocation Matrix \n" << cheb.CPoints() << " \n";
std::cout << "Differentiation Matrix \n" << cheb.D() << "\n";
std::cout << "Quadrature weights \n" << cheb.QWeights() << "\n";
std::cout << "Composite Diff matrix \n" << cheb.CompD() << "\n";
std::cout << "----------------------------------------------- \n";
SX x = SX::sym("x", 2);
SX u = SX::sym("u", 2);
SX f = SX::vertcat({x[1] + u[0], u[1]});
std::cout << "F : " << f << "\n";
Function dynamics = Function("rhs", {x, u}, {f});
std::cout << "X var : " << cheb.VarX() << "\n";
std::cout << "U var : " << cheb.VarU() << "\n";
SX g = cheb.CollocateDynamics(dynamics, 0, 1);
auto G = cheb.CollocateDynamics2(ode, 0, 1);
SX g_fun = G(cheb.VarX(), cheb.VarU(), SX({0}));
std::cout << "Collocated Dynamics \n" << g << "\n";
std::cout << "Collocated Dunamcis2 \n" << g_fun << "\n";
/** Performance index dicretization */
SX sym_M = SX::norm_2(x);
SX sym_L = SX::norm_2(x) + SX::norm_2(u);
Function M = Function(); //Function("Mayer", {x}, {sym_M});
Function L = Function("Lagrange", {x, u}, {sym_L});
SX J = cheb.CollocateCost(M, L, 0, 1);
std::cout << "Collocated Cost : \n" << J << "\n";
std::cout << "Cost size: " << J.size() << "\n";
std::cout << "Chebyshev Expansion : \n";
std::vector<double> coeff = {1, 2, 3, 4};
double value = -1;
double expansion = kmath::chebyshev_expansion<double>(coeff, value);
double expansion2 = kmath::chebyshev_expansion2<double>(coeff, value);
std::cout << expansion << "\n";
std::cout << expansion2 << "\n";
DM cp, D;
kmath::cheb(cp, D, poly_order, std::make_pair<double>(-1,1));
std::cout << "ref collocation points: \n" << cp << "\n";
std::cout << "ref diff matrix: \n" << D << "\n";
BOOST_CHECK(true);
}
BOOST_AUTO_TEST_CASE( collocation_test )
{
/** load data from the log file */
/** std::ifstream log_file;
int nrows = 100;
int ncols = 14;
std::vector<std::vector<double>> data_matrix(nrows, std::vector<double>(ncols));
log_file.open("kite_state_cut.log", std::ios::in);
if(!log_file)
std::cerr << "Unable to open lof file: " << "kite_state_cut.log" << "\n";
for (int i = 0; i < nrows; ++i)
for(int j = 0; j < ncols; ++j)
log_file >> data_matrix[i][j];
DM kite_states(data_matrix);
/** create a controller and solve an OCP */
std::string kite_config_file = "umx_radian.yaml";
KiteProperties kite_props = kite_utils::LoadProperties(kite_config_file);
AlgorithmProperties algo_props;
algo_props.Integrator = RK4;
/** set up the path */
SX x = SX::sym("x");
double radius = 2.31;
double altitude = 0.00;
SX Path = SX::vertcat(SXVector{radius * cos(x), radius * sin(x), altitude});
Function path_fun = Function("path", {x}, {Path});
std::shared_ptr<KiteDynamics> kite = std::make_shared<KiteDynamics>(kite_props, algo_props);
/** Kite Dynamics debugging */
Function DYNAMO = kite->getNumericDynamics();
DM x0 = DM({1.5, 0, 0, 0, 0, 0, 0, 1.0, 0, 1, 0, 0.0, 0.0});
DM u0 = DM({0.1, 0, 0});
DMVector eval = DYNAMO(DMVector{x0,u0});
std::cout << "Kite Dynamics: " << eval[0] << "\n";
KiteNMPF controller = KiteNMPF(kite, path_fun);
/** set control constraints */
double angle_sat = kmath::deg2rad(8.0);
DM lbu = DM::vertcat({0, -angle_sat, -angle_sat, -10});
DM ubu = DM::vertcat({0.3, angle_sat, angle_sat, 10});
controller.setLBU(lbu);
controller.setUBU(ubu);
/** set variable constraints */
DM lbx = DM::vertcat({0.5, -0.5, -DM::inf(1), -2 * M_PI, -2 * M_PI, -2 * M_PI, -DM::inf(1), -DM::inf(1), -DM::inf(1),
-1, -1, -1, -1, -DM::inf(1), -DM::inf(1)});
DM ubx = DM::vertcat({12, 5, DM::inf(1), 2 * M_PI, 2 * M_PI, 2 * M_PI, DM::inf(1), DM::inf(1), DM::inf(1),
1, 1, 1, 1, DM::inf(1), DM::inf(1)});
controller.setLBX(lbx);
controller.setUBX(ubx);
/** set reference velocity */
DM vel_ref = 0.05;
controller.setReferenceVelocity(vel_ref);
/** set scaling of system ODEs */
double vx_max = ubx.nonzeros()[0];
double vy_max = ubx.nonzeros()[1];
double wx_max = ubx.nonzeros()[3];
double wy_max = ubx.nonzeros()[4];
double wz_max = ubx.nonzeros()[5];
double rsphere = 5;
//DM Sx = DM::diag(DM::vertcat(DMVector{1/vx_max, 1/vy_max, 1, 1/wx_max, 1/wy_max, 1/wz_max,
// 1/rsphere, 1/rsphere, 1/rsphere, 1, 1, 1, 1, 1/(2*M_PI), 1/vx_max}));
DM Sx = DM::eye(15);
/** control */
DM Su = DM::eye(4);
controller.setStateScaling(Sx);
controller.setControlScaling(Su);
/** create path following NLP */
controller.createNLP();
/** solve for initial conditions */
DM X0 = DM::vertcat({1.5, 0, 0, 0, 0, 0,
0, -1.0, 0, 1, 0, 0.0, 0.0, M_PI_2, 0});
std::chrono::time_point<std::chrono::system_clock> start = kite_utils::get_time();
controller.computeControl(X0);
std::chrono::time_point<std::chrono::system_clock> stop = kite_utils::get_time();
auto duration = std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
auto latency = static_cast<double>(duration.count());
DM opt_x = controller.getOptimalTrajetory();
std::cout << "Optimal Solution: \n" << opt_x << "\n";
DM opt_u = controller.getOptimalControl();
std::cout << "Optimal Control : \n" << opt_u << "\n";
std::cout << "Actual Control : \n" << opt_u(Slice(0, 4), opt_u.size2() - 1) << "\n";
DM U0 = DM::zeros(4,1);
for(int i = 0; i < opt_u.size2()-1; ++i)
U0 += pow(-1, i) * opt_u(Slice(0,4), i);
std::cout << "Extrapolated control : \n" << U0 << "\n";
std::cout << "COLLOCATION_TEST COMPUTATION TIME: " << latency * 1e-6 << " [seconds]" << "\n";
/** initialization */
DM theta0 = controller.findClosestPointOnPath(X0(Slice(6,9)));
std::cout << "INIT: " << theta0 << "\n";
/** process real trajectory */
bool success = true;
//for(int i = 0; i < kite_states.size1(); ++i)
/** for(int i = 0; i < 1; ++i)
{
start = kite_utils::get_time();
X0 = kite_states(i, Slice(1, kite_states.size2()));
X0 = DM::horzcat({X0, 0, 0}).T();
controller.computeControl(X0);
stop = kite_utils::get_time();
duration = std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
latency = static_cast<double>(duration.count());
std::cout << "COLLOCATION_TEST COMPUTATION TIME: " << latency * 1e-6 << " [seconds]" << "\n";
} */
BOOST_CHECK(success);
}
BOOST_AUTO_TEST_CASE( simple_ocp_test )
{
const int poly_order = 3;
const int num_segments = 1;
const int dimx = 1;
const int dimu = 1;
const int dimp = 1;
Chebyshev<SX, poly_order, num_segments, dimx, dimu, dimp> cheb;
SX x = SX::sym("x");
SX u = SX::sym("u");
SX f = -x + u;
Function dynamics = Function("rhs", {x, u}, {f});
SX g = cheb.CollocateDynamics(dynamics, 0, 1);
/** Performance index dicretization */
SX sym_M = x;
SX sym_L = pow(u, 4);
Function M = Function("Mayer", {x}, {sym_M});
Function L = Function("Lagrange", {x, u}, {sym_L});
SX J = cheb.CollocateCost(M, L, 0, 1);
SX varx = cheb.VarX();
SX varu = cheb.VarU();
SX opt_var = SX::vertcat(SXVector{varx, varu});
SX lbg = SX::zeros(g.size());
SX ubg = SX::zeros(g.size());
/** set inequality (box) constraints */
/** state */
SX lbx = SX::repmat(-SX::inf(), poly_order + 1, 1);
SX ubx = SX::repmat(SX::inf(), poly_order + 1, 1);
lbx = SX::vertcat( {lbx, SX::repmat(-SX::inf(), poly_order + 1, 1)} );
ubx = SX::vertcat( {ubx, SX::repmat(SX::inf(), poly_order + 1, 1)} );
/** formulate NLP */
SXDict NLP;
NLP["x"] = opt_var;
NLP["f"] = J;
NLP["g"] = g;
Dict OPTS;
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;
Function NLP_Solver;
NLP_Solver = nlpsol("solver", "ipopt", NLP, OPTS);
/** set default args */
DMDict ARG;
ARG["lbx"] = lbx;
ARG["ubx"] = ubx;
ARG["lbg"] = lbg;
ARG["ubg"] = ubg;
/** solve */
DMDict res = NLP_Solver(ARG);
std::cout << NLP_Solver.stats() << "\n";
BOOST_CHECK(true);
}
SX add(const SX &a, const SX &b, const SX &c)
{
return a + b;
}
BOOST_AUTO_TEST_CASE( simple_generics_test )
{
const int poly_order = 3;
const int num_segments = 1;
const int dimx = 1;
const int dimu = 1;
const int dimp = 1;
Chebyshev<SX, poly_order, num_segments, dimx, dimu, dimp> cheb;
SX x = SX::sym("x");
SX u = SX::sym("u");
SX p = SX::sym("p");
auto f3 = cheb.CollocateDynamics2(add, 0, 1);
std::cout << "F2 : " << cheb._ode(x, u, p) << "\n";
//std::cout << "F3 : " << f3(x, u, p) << "\n";
BOOST_CHECK(true);
}
BOOST_AUTO_TEST_SUITE_END()
c4science · Help