Page Menu
Home
c4science
Search
Configure Global Search
Log In
Files
F115946175
kite_control_test.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
Wed, Jun 4, 01:50
Size
15 KB
Mime Type
text/x-c
Expires
Fri, Jun 6, 01:50 (2 d)
Engine
blob
Format
Raw Data
Handle
26583054
Attached To
R1517 test_package
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()
Event Timeline
Log In to Comment