Page MenuHomec4science
Files F65137228

kite_control_test.cpp
No OneTemporary
Actions

File Metadata

Created
Sat, Jun 1, 03:22

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>
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( 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";
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_SUITE_END()

Event Timeline

c4science · Help