DeepcControllerService.cpp

//    Copyright (C) 2019, ETH Zurich, D-ITET, Paul Beuchat
//
//    This file is part of D-FaLL-System.
//    
//    D-FaLL-System is free software: you can redistribute it and/or modify
//    it under the terms of the GNU General Public License as published by
//    the Free Software Foundation, either version 3 of the License, or
//    (at your option) any later version.
//    
//    D-FaLL-System is distributed in the hope that it will be useful,
//    but WITHOUT ANY WARRANTY; without even the implied warranty of
//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//    GNU General Public License for more details.
//    
//    You should have received a copy of the GNU General Public License
//    along with D-FaLL-System.  If not, see <http://www.gnu.org/licenses/>.
//    
//
//    ----------------------------------------------------------------------------------
//    DDDD        FFFFF        L     L           SSSS  Y   Y   SSSS  TTTTT  EEEEE  M   M
//    D   D       F      aaa   L     L          S       Y Y   S        T    E      MM MM
//    D   D  ---  FFFF  a   a  L     L     ---   SSS     Y     SSS     T    EEE    M M M
//    D   D       F     a  aa  L     L              S    Y        S    T    E      M   M
//    DDDD        F      aa a  LLLL  LLLL       SSSS     Y    SSSS     T    EEEEE  M   M
//
//
//    DESCRIPTION:
//    A Deepc Controller for students build from
//
//    ----------------------------------------------------------------------------------





// INCLUDE THE HEADER
#include "nodes/DeepcControllerService.h"






//    ----------------------------------------------------------------------------------
//    FFFFF  U   U  N   N   CCCC  TTTTT  III   OOO   N   N
//    F      U   U  NN  N  C        T     I   O   O  NN  N
//    FFF    U   U  N N N  C        T     I   O   O  N N N
//    F      U   U  N  NN  C        T     I   O   O  N  NN
//    F       UUU   N   N   CCCC    T    III   OOO   N   N
//
//    III M   M PPPP  L     EEEEE M   M EEEEE N   N TTTTT   A   TTTTT III  OOO  N   N
//     I  MM MM P   P L     E     MM MM E     NN  N   T    A A    T    I  O   O NN  N
//     I  M M M PPPP  L     EEE   M M M EEE   N N N   T   A   A   T    I  O   O N N N
//     I  M   M P     L     E     M   M E     N  NN   T   AAAAA   T    I  O   O N  NN
//    III M   M P     LLLLL EEEEE M   M EEEEE N   N   T   A   A   T   III  OOO  N   N
//    ----------------------------------------------------------------------------------


// DEEPC FUNCTIONS

// DEEPC THREAD MAIN
// Deepc operations run in seperate thread as they are time consuming
void Deepc_thread_main()
{
	bool params_changed;
	bool setpoint_changed;
	bool setupDeepc;
	bool solveDeepc;

	while (ros::ok())
	{
		s_Deepc_mutex.lock();
		//ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Lock 72");
        params_changed = s_params_changed;
        setpoint_changed = s_setpoint_changed;
        setupDeepc = s_setupDeepc;
        solveDeepc = s_solveDeepc;
        s_Deepc_mutex.unlock();
        //ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Unlock 72");

        if (params_changed)
        {
        	change_Deepc_params();
        	
        	s_Deepc_mutex.lock();
        	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Lock 85");
        	s_params_changed = false;
        	s_Deepc_mutex.unlock();
        	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Unlock 85");
        }

        if (setpoint_changed)
        {
        	change_Deepc_setpoint();
        	
        	s_Deepc_mutex.lock();
        	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Lock 96");
        	s_setpoint_changed = false;
        	s_Deepc_mutex.unlock();
        	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Unlock 96");
        }

        if (setupDeepc)
        {
        	setup_Deepc();

        	s_Deepc_mutex.lock();
        	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Lock 107");
        	s_setupDeepc = false;
        	s_Deepc_mutex.unlock();
        	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Unlock 107");
        }

        if (solveDeepc)
        {
        	solve_Deepc();

		  	s_Deepc_mutex.lock();
		  	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Lock 118");
        	s_solveDeepc = false;
        	s_Deepc_mutex.unlock();
        	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Unlock 118");
        }
	}

	// Clear any heap allocated variables
	delete[] d_grb_vars;
	delete[] d_grb_ini_constrs;
}

void change_Deepc_params()
{
	s_Deepc_mutex.lock();
	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Lock 133");
	d_logFolder = s_logFolder;
	d_Deepc_measure_roll_pitch = s_Deepc_measure_roll_pitch;
	d_Deepc_yaw_control = s_Deepc_yaw_control;
	d_Tini = s_yaml_Tini;
	d_N = s_yaml_N;
	d_lambda2_g = s_yaml_lambda2_g;
	d_opt_sparse = s_yaml_opt_sparse;
	bool opt_verbose = s_yaml_opt_verbose;
	bool grb_LogToFile = s_yaml_grb_LogToFile;
	d_grb_presolve_at_setup = s_yaml_grb_presolve_at_setup;
	// Deepc setup must be re-run after changes
	s_setupDeepc_success = false;
	s_Deepc_mutex.unlock();
	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Unlock 133");

	try
	{
		if (grb_LogToFile)
			d_grb_model.set(GRB_StringParam_LogFile, d_logFolder + "gurobi.log");
		else
			d_grb_model.set(GRB_StringParam_LogFile, "");
		d_grb_model.set(GRB_IntParam_LogToConsole, opt_verbose);

		// Inform the user
	    ROS_INFO("[DEEPC CONTROLLER] Deepc parameters change successful");
	    ROS_INFO("[DEEPC CONTROLLER] (Re-)setup Deepc to apply changes");
	}

	catch(GRBException e)
    {
	    ROS_INFO_STREAM("[DEEPC CONTROLLER] Deepc parameter change exception with Gurobi error code = " << e.getErrorCode());
	    ROS_INFO_STREAM("[DEEPC CONTROLLER] Error message: " << e.getMessage());
	    ROS_INFO("[DEEPC CONTROLLER] Deepc must be (re-)setup");
  	}
  	catch(exception& e)
  	{
	    ROS_INFO_STREAM("[DEEPC CONTROLLER] Deepc parameter change exception with standard error message: " << e.what());
	    ROS_INFO("[DEEPC CONTROLLER] Deepc must be (re-)setup");
  	}
  	catch(...)
  	{
  		ROS_INFO("[DEEPC CONTROLLER] Deepc parameter change exception");
  		ROS_INFO("[DEEPC CONTROLLER] Deepc must be (re-)setup");
  	}
}

void change_Deepc_setpoint()
{
	s_Deepc_mutex.lock();
	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Lock 193");
	bool setupDeepc_success = s_setupDeepc_success;
	MatrixXf setpoint = s_setpoint;
	s_Deepc_mutex.unlock();
	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Unlock 193");

	if (!setupDeepc_success)
		return;

	try
	{
		// UPDATE GUROBI LINEAR COST VECTOR FOR REFERENCE TRACKING
		d_r.topRows(3) = setpoint.topRows(3);
		if (d_Deepc_yaw_control)
			d_r.bottomRows(1) = setpoint.bottomRows(1);

		if (d_opt_sparse)
		{
			d_grb_c_r = MatrixXf::Zero(d_Nyf + d_num_outputs, 1);
			for (int i = 0; i < d_N + 1; i++)
			{
				if (i < d_N)
					d_grb_c_r.middleRows(i * d_num_outputs, d_num_outputs) = -2.0 * d_Q * d_r;
				else
					d_grb_c_r.middleRows(i * d_num_outputs, d_num_outputs) = -2.0 * d_P * d_r;
			}
		}
		else
		{
			d_grb_c_r = MatrixXf::Zero(d_Ng, 1);
			for (int i = 0; i < d_N + 1; i++)
			{
				if (i < d_N)
					d_grb_c_r -= 2.0 * d_Y_f.middleRows(i * d_num_outputs, d_num_outputs).transpose() * d_Q * d_r;
				else
					d_grb_c_r -= 2.0 * d_Y_f.middleRows(i * d_num_outputs, d_num_outputs).transpose() * d_P * d_r;
			}
		}

		// UPDATE GUROBI LINEAR COST VECTOR FOR STEADY STATE TRAJECTORY MAPPER
		d_r_gs = d_r.replicate(d_Tini + d_N + 1, 1);
		d_b_gs.bottomRows(d_r_gs.rows()) = d_r_gs;
		d_gs = d_A_gs.bdcSvd(ComputeThinU | ComputeThinV).solve(d_b_gs);

		d_grb_c_gs = -2.0 * d_lambda2_g * d_gs;

		// Update linear objective terms
	    d_grb_lin_obj_r = 0;
	    d_grb_lin_obj_gs = 0;
	    if (d_opt_sparse)
	    {
	    	for (int i = 0; i < d_Ng; i++)
		    	d_grb_lin_obj_gs += d_grb_c_gs(i) * d_grb_vars[i];
		    for (int i = 0; i < d_Nyf + d_num_outputs; i++)
		    	d_grb_lin_obj_r += d_grb_c_r(i) * d_grb_vars[d_Ng + d_Ns + d_Nuf + i];
	    }
	    else
	    {
	    	for (int i = 0; i < d_Ng; i++)
		    {
		    	d_grb_lin_obj_r += d_grb_c_r(i) * d_grb_vars[i];
		    	d_grb_lin_obj_gs += d_grb_c_gs(i) * d_grb_vars[i];
		    }
	    }

	    // Update objective
	    d_grb_model.setObjective(d_grb_quad_obj + d_grb_lin_obj_us + d_grb_lin_obj_r + d_grb_lin_obj_gs);

	    // Inform the user
	    ROS_INFO("[DEEPC CONTROLLER] Deepc setpoint update successful");
	}

	catch(GRBException e)
    {
    	s_Deepc_mutex.lock();
    	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Lock 241");
    	s_setupDeepc_success = false;
    	s_Deepc_mutex.unlock();
    	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Unlock 241");

	    ROS_INFO_STREAM("[DEEPC CONTROLLER] Deepc setpoint update exception with Gurobi error code = " << e.getErrorCode());
	    ROS_INFO_STREAM("[DEEPC CONTROLLER] Error message: " << e.getMessage());
	    ROS_INFO("[DEEPC CONTROLLER] Deepc must be (re-)setup");
  	}
  	catch(exception& e)
    {
    	s_Deepc_mutex.lock();
    	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Lock 253");
    	s_setupDeepc_success = false;
    	s_Deepc_mutex.unlock();
    	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Unlock 253");

	    ROS_INFO_STREAM("[DEEPC CONTROLLER] Deepc setpoint update exception with standard error message: " << e.what());
	    ROS_INFO("[DEEPC CONTROLLER] Deepc must be (re-)setup");
  	}
  	catch(...)
  	{
  		s_Deepc_mutex.lock();
  		// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Lock 264");
    	s_setupDeepc_success = false;
    	s_Deepc_mutex.unlock();
    	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Unlock 264");

  		ROS_INFO("[DEEPC CONTROLLER] Deepc setpoint update exception");
  		ROS_INFO("[DEEPC CONTROLLER] Deepc must be (re-)setup");
  	}
}

void setup_Deepc()
{
	s_Deepc_mutex.lock();
	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Lock 277");
	string dataFolder = s_dataFolder;
	vector<float> Q_vec = s_yaml_Q;
	vector<float> R_vec = s_yaml_R;
	vector<float> P_vec = s_yaml_P;
	float lambda2_s = s_yaml_lambda2_s;
	float cf_weight_in_newtons = s_cf_weight_in_newtons;
	MatrixXf setpoint = s_setpoint;
	vector<float> input_min_vec = s_yaml_input_min;
	vector<float> input_max_vec = s_yaml_input_max;
	vector<float> output_min_vec = s_yaml_output_min;
	vector<float> output_max_vec = s_yaml_output_max;
	s_Deepc_mutex.unlock();
	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Unlock 277");

	try
    {
		MatrixXf u_data_in = read_csv(dataFolder + "u_data.csv");
		if (u_data_in.size() <= 0)
		{
			s_Deepc_mutex.lock();
			// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Lock 301");
        	s_setupDeepc_success = false;
        	s_Deepc_mutex.unlock();
        	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Unlock 301");

        	ROS_INFO("[DEEPC CONTROLLER] Failed to read u data file");

			return;
		}
		MatrixXf y_data_in = read_csv(dataFolder + "y_data.csv");
		if (y_data_in.size() <= 0)
		{	
			s_Deepc_mutex.lock();
			// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Lock 314");
        	s_setupDeepc_success = false;
        	s_Deepc_mutex.unlock();
        	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Unlock 314");
			
			ROS_INFO("[DEEPC CONTROLLER] Failed to read y data file");

			return;
		}

		MatrixXf u_data;
		if (d_Deepc_yaw_control)
			u_data = u_data_in;
		else
			u_data = u_data_in.topRows(3);

		MatrixXf y_data;
		if (d_Deepc_measure_roll_pitch && d_Deepc_yaw_control)
			y_data = y_data_in;
		else if (d_Deepc_measure_roll_pitch)
			y_data = y_data_in.topRows(5);
		else if (d_Deepc_yaw_control)
		{
			y_data = MatrixXf::Zero(4, y_data_in.cols());
			y_data.topRows(3) = y_data_in.topRows(3);
			y_data.bottomRows(1) = y_data_in.bottomRows(1);
		}
		else
			y_data = y_data_in.topRows(3);

		// VARIABLE LENGTHS
		// Number of inputs m
		int num_inputs = u_data.rows();
		// Number of outputs p
		d_num_outputs = y_data.rows();
		// Previous inputs vector (uini) length
		d_Nuini = num_inputs * d_Tini;
		// Previous outputs vector (yini) length
		d_Nyini = d_num_outputs * d_Tini;
		// Slack variable length
		d_Ns = d_Nyini;
		// Future inputs vector (uf) length
		d_Nuf = num_inputs * d_N;
		// Future output vector (yf) length (!!not including terminal output!!)
		d_Nyf = d_num_outputs * d_N;

		// HANKEL MATRICES
		MatrixXf H_u = data2hankel(u_data, d_Tini + d_N + 1);
		MatrixXf H_y = data2hankel(y_data, d_Tini + d_N + 1);
		MatrixXf U_p = H_u.topRows(d_Nuini);
		d_U_f = H_u.middleRows(d_Nuini, d_Nuf);
		MatrixXf Y_p = H_y.topRows(d_Nyini);
		d_Y_f = H_y.bottomRows(d_Nyf + d_num_outputs);

		// MORE VARIABLE SIZES
		// g vector size
		d_Ng = U_p.cols();
		// Optimization decision vector size
		int num_opt_vars;
		if (d_opt_sparse)
		{
			// Sparse optimization variables: [g; slack; uf; yf; yt], where yt is terminal output
			num_opt_vars = d_Ng + d_Ns + d_Nuf + d_Nyf + d_num_outputs;
		}
		else
		{
			// Dense optimization variables: [g; slack]
			num_opt_vars = d_Ng + d_Ns;
		}

		// COST MATRICES
		// Output cost and terminal output cost matrix
		d_Q = MatrixXf::Zero(d_num_outputs, d_num_outputs);
		d_P = MatrixXf::Zero(d_num_outputs, d_num_outputs);
		for (int i = 0; i < 3; i++)
		{
			d_Q(i,i) = Q_vec[i];
			d_P(i,i) = P_vec[i];
		}
		if (d_Deepc_measure_roll_pitch)
			for (int i = 3; i < 5; i++)
			{
				d_Q(i,i) = Q_vec[i+3];
				d_P(i,i) = P_vec[i+3];
			}
		if (d_Deepc_yaw_control)
		{
			d_Q(d_num_outputs-1,d_num_outputs-1) = Q_vec[8];
			d_P(d_num_outputs-1,d_num_outputs-1) = P_vec[8];
		}

		// Input cost matrix
		MatrixXf R = MatrixXf::Zero(num_inputs, num_inputs);
		for (int i = 0; i < num_inputs; i++)
			R(i,i) = R_vec[i];

		// GUROBI QUADRATIC COST MATRIX
		MatrixXf Q_g = d_lambda2_g * MatrixXf::Identity(d_Ng, d_Ng);
		MatrixXf Q_s = lambda2_s * MatrixXf::Identity(d_Ns, d_Ns);
		MatrixXf Q_uf = MatrixXf::Zero(d_Nuf, d_Nuf);
		MatrixXf Q_yf = MatrixXf::Zero(d_Nyf, d_Nyf);
		MatrixXf Q_yt = d_P;
		MatrixXf grb_Q = MatrixXf::Zero(num_opt_vars, num_opt_vars);
		if (d_opt_sparse)
		{
			for (int i = 0; i < d_N; i++)
			{
				Q_uf.block(i * num_inputs, i * num_inputs, num_inputs, num_inputs) = R;
				Q_yf.block(i * d_num_outputs, i * d_num_outputs, d_num_outputs, d_num_outputs) = d_Q;
			}
		}
		else
		{
			for (int i = 0; i < d_N; i++)
			{
				Q_g += d_U_f.middleRows(i * num_inputs, num_inputs).transpose() * R * d_U_f.middleRows(i * num_inputs, num_inputs);
				Q_g += d_Y_f.middleRows(i * d_num_outputs, d_num_outputs).transpose() * d_Q * d_Y_f.middleRows(i * d_num_outputs, d_num_outputs);
			}
			Q_g += d_Y_f.bottomRows(d_num_outputs).transpose() * d_P * d_Y_f.bottomRows(d_num_outputs);
		}
		grb_Q.topLeftCorner(d_Ng, d_Ng) = Q_g;
		grb_Q.block(d_Ng, d_Ng, d_Ns, d_Ns) = Q_s;
		if (d_opt_sparse)
		{
			grb_Q.block(d_Ng + d_Ns, d_Ng + d_Ns, d_Nuf, d_Nuf) = Q_uf;
			grb_Q.block(d_Ng + d_Ns + d_Nuf, d_Ng + d_Ns + d_Nuf, d_Nyf, d_Nyf) = Q_yf;
			grb_Q.bottomRightCorner(d_num_outputs, d_num_outputs) = Q_yt;
		}

		// GUROBI LINEAR COST VECTOR
		// Steady state input
		MatrixXf us = MatrixXf::Zero(num_inputs, 1);
		us(0) = cf_weight_in_newtons;

		// Reference
		d_r = MatrixXf::Zero(d_num_outputs, 1);
		d_r.topRows(3) = setpoint.topRows(3);
		if (d_Deepc_yaw_control)
			d_r.bottomRows(1) = setpoint.bottomRows(1);

		// Steady state trajectory mapper
		MatrixXf u_gs = us.replicate(d_Tini + d_N, 1);
		d_r_gs = d_r.replicate(d_Tini + d_N + 1, 1);
		d_A_gs = MatrixXf::Zero(U_p.rows() + d_U_f.rows() + Y_p.rows() + d_Y_f.rows(), d_Ng);
		d_b_gs = MatrixXf::Zero(d_A_gs.rows(), 1);
		d_A_gs.topRows(U_p.rows()) = U_p;
		d_A_gs.middleRows(U_p.rows(), d_U_f.rows()) = d_U_f;
		d_A_gs.middleRows(U_p.rows() + d_U_f.rows(), Y_p.rows()) = Y_p;
		d_A_gs.bottomRows(d_Y_f.rows()) = d_Y_f;
		d_b_gs.topRows(u_gs.rows()) = u_gs;
		d_b_gs.bottomRows(d_r_gs.rows()) = d_r_gs;
		d_gs = d_A_gs.bdcSvd(ComputeThinU | ComputeThinV).solve(d_b_gs);

		d_grb_c_gs = -2.0 * d_lambda2_g * d_gs;
		MatrixXf grb_c_us;
		if (d_opt_sparse)
		{
			grb_c_us = MatrixXf::Zero(d_Nuf, 1);
			d_grb_c_r = MatrixXf::Zero(d_Nyf + d_num_outputs, 1);

			for (int i = 0; i < d_N + 1; i++)
			{
				if (i < d_N)
				{
					grb_c_us.middleRows(i * num_inputs, num_inputs) = -2.0 * R * us;
					d_grb_c_r.middleRows(i * d_num_outputs, d_num_outputs) = -2.0 * d_Q * d_r;
				}
				else
					d_grb_c_r.middleRows(i * d_num_outputs, d_num_outputs) = -2.0 * d_P * d_r;
			}
		}
		else
		{
			grb_c_us = MatrixXf::Zero(d_Ng, 1);
			d_grb_c_r = MatrixXf::Zero(d_Ng, 1);

			for (int i = 0; i < d_N + 1; i++)
			{
				if (i < d_N)
				{
					grb_c_us -= 2.0 * d_U_f.middleRows(i * num_inputs, num_inputs).transpose() * R * us;
					d_grb_c_r -= 2.0 * d_Y_f.middleRows(i * d_num_outputs, d_num_outputs).transpose() * d_Q * d_r;
				}
				else
					d_grb_c_r -= 2.0 * d_Y_f.middleRows(i * d_num_outputs, d_num_outputs).transpose() * d_P * d_r;
			}
		}

		//INPUT CONSTRAINTS
		MatrixXf input_min = MatrixXf::Zero(num_inputs, 1);
		MatrixXf input_max = MatrixXf::Zero(num_inputs, 1);
		for (int i = 0; i < num_inputs; i++)
		{
			input_min(i) = input_min_vec[i];
			input_max(i) = input_max_vec[i];
		}

		// OUTPUT CONSTRAINTS
		MatrixXf output_min = MatrixXf::Zero(d_num_outputs, 1);
		MatrixXf output_max = MatrixXf::Zero(d_num_outputs, 1);
		for (int i = 0; i < 3; i++)
		{
			output_min(i) = output_min_vec[i];
			output_max(i) = output_max_vec[i];
		}
		if (d_Deepc_measure_roll_pitch)
			for (int i = 3; i < 5; i++)
			{
				output_min(i) = output_min_vec[i+3];
				output_max(i) = output_max_vec[i+3];
			}
		if (d_Deepc_yaw_control)
		{
			output_min(d_num_outputs-1) = output_min_vec[8];
			output_max(d_num_outputs-1) = output_max_vec[8];
		}

		// GUROBI LINEAR INEQUALITY CONSTRAINT MATRIX
		// Only used in 'dense' formulation
		MatrixXf grb_Ag;
		if (d_opt_sparse)
			grb_Ag = MatrixXf::Zero(0, 0);
		else
		{
			grb_Ag = MatrixXf::Zero(2 * d_U_f.rows() + 2 * d_Y_f.rows(), d_Ng);
			grb_Ag.topRows(d_U_f.rows()) = -d_U_f;
			grb_Ag.middleRows(d_U_f.rows(), d_U_f.rows()) = d_U_f;
			grb_Ag.middleRows(2 * d_U_f.rows(), d_Y_f.rows()) = -d_Y_f;
			grb_Ag.bottomRows(d_Y_f.rows()) = d_Y_f;
		}

		// GUROBI LINEAR INEQUALITY CONSTRAINT VECTOR
		// Only used in 'dense' formulation
		MatrixXf grb_b;
		if (d_opt_sparse)
			grb_b = MatrixXf::Zero(0, 0);
		else
		{
			grb_b = MatrixXf::Zero(grb_Ag.rows(), 1);
			for (int i = 0; i < d_N + 1; i++)
			{
				if (i < d_N)
				{
					grb_b.middleRows(i * num_inputs, num_inputs) = -input_min;
					grb_b.middleRows(d_U_f.rows() + i * num_inputs, num_inputs) = input_max;
				}
				grb_b.middleRows(2 * d_U_f.rows() + i * d_num_outputs, d_num_outputs) = -output_min;
				grb_b.middleRows(2 * d_U_f.rows() + d_Y_f.rows() + i * d_num_outputs, d_num_outputs) = output_max;
			}
		}

		// GUROBI BOUNDS VECTOR
		MatrixXf lb = -GRB_INFINITY * MatrixXf::Ones(num_opt_vars, 1);
		MatrixXf ub = GRB_INFINITY * MatrixXf::Ones(num_opt_vars, 1);
		if (d_opt_sparse)
		{
			lb.middleRows(d_Ng + d_Ns, d_Nuf) = input_min.replicate(d_N, 1);
			lb.bottomRows(d_Nyf + d_num_outputs) = output_min.replicate(d_N + 1, 1);

			ub.middleRows(d_Ng + d_Ns, d_Nuf) = input_max.replicate(d_N, 1);
			ub.bottomRows(d_Nyf + d_num_outputs) = output_max.replicate(d_N + 1, 1);
		}

		// GUROBI LINEAR EQUALITY CONSTRAINTS
		// Static equality constraints ([uf; yf; yt])
		MatrixXf grb_A_eq;
		MatrixXf grb_b_eq;
		if (d_opt_sparse)
		{
			grb_A_eq = MatrixXf::Zero(d_Nuf + d_Nyf + d_num_outputs, num_opt_vars);
			grb_A_eq.topLeftCorner(d_Nuf, d_Ng) = d_U_f;
			grb_A_eq.block(0, d_Ng + d_Ns, d_Nuf, d_Nuf) = -MatrixXf::Identity(d_Nuf, d_Nuf);
			grb_A_eq.bottomLeftCorner(d_Nyf + d_num_outputs, d_Ng) = d_Y_f;
			grb_A_eq.bottomRightCorner(d_Nyf + d_num_outputs, d_Nyf + d_num_outputs) = -MatrixXf::Identity(d_Nyf + d_num_outputs, d_Nyf + d_num_outputs);

			grb_b_eq = MatrixXf::Zero(d_Nuf + d_Nyf + d_num_outputs, 1);
		}
		else
		{
			// There are no static equality constraints in dense formulation (they are subtituted)
			grb_A_eq = MatrixXf::Zero(0, 0);
			grb_b_eq = MatrixXf::Zero(0, 0);
		}
		
		// Dynamic equality constraints that change every time ([uini; uini])
		MatrixXf grb_A_eq_ini = MatrixXf::Zero(d_Nuini + d_Nyini, num_opt_vars);
		grb_A_eq_ini.topLeftCorner(d_Nuini, d_Ng) = U_p;
		grb_A_eq_ini.bottomLeftCorner(d_Nyini, d_Ng) = Y_p;
		grb_A_eq_ini.block(d_Nuini, d_Ng, d_Ns, d_Ns) = -MatrixXf::Identity(d_Ns, d_Ns);

		MatrixXf grb_b_eq_ini = MatrixXf::Zero(d_Nuini + d_Nyini, 1);
		d_uini = MatrixXf::Zero(d_Nuini, 1);
	    d_yini = MatrixXf::Zero(d_Nyini, 1);
		grb_b_eq_ini.topRows(d_Nuini) = d_uini;
		grb_b_eq_ini.bottomRows(d_Nyini) = d_yini;

		// GUROBI MODEL SETUP
		// Follows 'dense_c++.cpp' example
    	// (Re-)Configure environment
	    d_grb_env.set(GRB_StringParam_LogFile, d_logFolder + "gurobi.log");
	    d_grb_env.start();

	    // Clear variables if previously created
	    int num_vars = d_grb_model.get(GRB_IntAttr_NumVars);
	    d_grb_vars = d_grb_model.getVars();
	    for (int i = 0; i < num_vars; i++)
	    	d_grb_model.remove(d_grb_vars[i]);
	    d_grb_model.update();
	    delete[] d_grb_vars;

	    // Create variables
	    double grb_lb[num_opt_vars];
	    double grb_ub[num_opt_vars];
	    for (int i = 0; i < num_opt_vars; i++)
	    {
	    	grb_lb[i] = lb(i);
	    	grb_ub[i] = ub(i);
	    }
	    d_grb_vars = d_grb_model.addVars(grb_lb, grb_ub, NULL, NULL, NULL, num_opt_vars);

	    // Set quadratic objective term
	    d_grb_quad_obj = 0;
	    for (int i = 0; i < num_opt_vars; i++)
	    	for (int j = 0; j < num_opt_vars; j++)
	    		d_grb_quad_obj += grb_Q(i,j) * d_grb_vars[i] * d_grb_vars[j];

	    // Set linear objective term
	    d_grb_lin_obj_us = 0;
	    d_grb_lin_obj_r = 0;
	    d_grb_lin_obj_gs = 0;
	    if (d_opt_sparse)
	    {
	    	for (int i = 0; i < d_Ng; i++)
		    	d_grb_lin_obj_gs += d_grb_c_gs(i) * d_grb_vars[i];
		    for (int i = 0; i < d_Nuf; i++)
		    	d_grb_lin_obj_us += grb_c_us(i) * d_grb_vars[d_Ng + d_Ns + i];
		    for (int i = 0; i < d_Nyf + d_num_outputs; i++)
		    	d_grb_lin_obj_r += d_grb_c_r(i) * d_grb_vars[d_Ng + d_Ns + d_Nuf + i];
	    }
	    else
	    {
	    	for (int i = 0; i < d_Ng; i++)
		    {
		    	d_grb_lin_obj_us += grb_c_us(i) * d_grb_vars[i];
		    	d_grb_lin_obj_r += d_grb_c_r(i) * d_grb_vars[i];
		    	d_grb_lin_obj_gs += d_grb_c_gs(i) * d_grb_vars[i];
		    }
	    }
	    
	    // Set objective
	    d_grb_model.setObjective(d_grb_quad_obj + d_grb_lin_obj_us + d_grb_lin_obj_r + d_grb_lin_obj_gs);

	    // Clear constraints if previously created
	    int num_constrs = d_grb_model.get(GRB_IntAttr_NumConstrs);
    	GRBConstr* grb_constrs = d_grb_model.getConstrs();
    	for (int i = 0; i < num_constrs; i++)
    		d_grb_model.remove(grb_constrs[i]);
    	d_grb_model.update();
    	delete[] grb_constrs;
    	delete[] d_grb_ini_constrs;

	    // Add inequality constraints
	    // Note that grb_Ag is empty when using sparse formulation and for loop below does not run
	    for (int i = 0; i < grb_Ag.rows(); i++)
	    {
	    	GRBLinExpr lhs = 0;
	    	for (int j = 0; j < d_Ng; j++)
	    		lhs += grb_Ag(i,j) * d_grb_vars[j];
	    	d_grb_model.addConstr(lhs, '<', grb_b(i));
	    }

	    // Add static equality constraints
	    // Note that grb_A_eq is empty when using dense formulation and for loop below does not run
	    for (int i = 0; i < grb_A_eq.rows(); i++)
	    {
	    	GRBLinExpr lhs = 0;
	    	for (int j = 0; j < num_opt_vars; j++)
	    		lhs += grb_A_eq(i,j) * d_grb_vars[j];
	    	d_grb_model.addConstr(lhs, '=', grb_b_eq(i));
	    }

	    // Add dynamic equality constraints and store in memory for quick change
	    int num_ini_constrs = grb_A_eq_ini.rows();
	    d_grb_ini_constrs = new GRBConstr[num_ini_constrs];
	    for (int i = 0; i < num_ini_constrs; i++)
	    {
	    	GRBLinExpr lhs = 0;
	    	for (int j = 0; j < num_opt_vars; j++)
	    		lhs += grb_A_eq_ini(i,j) * d_grb_vars[j];
	    	d_grb_ini_constrs[i] = d_grb_model.addConstr(lhs, '=', grb_b_eq_ini(i));
	    }

	    // Set model parameters
	    // Skip Presolve when using sparse model, as it returns same model (presolving sparsifies)
	    if (d_opt_sparse)
	    	d_grb_model.set(GRB_IntParam_Presolve, 0);
	    // Setting Aggregate to 0 was found to speed up optimization using Gurobi auto-tune
	    // This is only relevant when Presolve is on (dense formulation)
	    d_grb_model.set(GRB_IntParam_Aggregate, 0);

	    // Presolve - only applicable for dense formulation
	    if (!d_opt_sparse && d_grb_presolve_at_setup)
	    {
		    static GRBModel grb_model_presolved = d_grb_model.presolve();
		    
		    // Update variables to presolved model variables
		    d_grb_vars = grb_model_presolved.getVars();

		    // Update dynamic equality constraints to presolved model constraints. They are last set of constraints in presolved model
		    num_constrs = grb_model_presolved.get(GRB_IntAttr_NumConstrs);
		    GRBConstr* grb_constrs_presolved = grb_model_presolved.getConstrs();
		    int j = 0;
	    	for (int i = num_constrs - num_ini_constrs; i < num_constrs; i++)
	    	{
	    		d_grb_ini_constrs[j] = grb_constrs_presolved[i];
	    		j++;
	    	}
	    	delete[] grb_constrs_presolved;

	    	// Set Presolve parameter of presolved model to 0 to avoid re-presolving
		    grb_model_presolved.set(GRB_IntParam_Presolve, 0);

		    // Global variable is pointer to model
		    d_grb_model_presolved = &grb_model_presolved;
		}

	    // Setup output variables
	    d_g = MatrixXf::Zero(d_Ng, 1);
	    if (d_opt_sparse)
	    {
	    	d_uf_start_i = d_Ng + d_Ns;
	    	d_u_f = MatrixXf::Zero(d_Nuf, 1);
	    }

	    s_Deepc_mutex.lock();
	    // ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Lock 568");
		s_num_inputs = num_inputs;
		s_num_outputs = d_num_outputs;
		s_Nuini = d_Nuini;
		s_Nyini = d_Nyini;
		s_uini = d_uini;
		s_yini = d_yini;
    	s_setupDeepc_success = true;
		s_Deepc_mutex.unlock();
		// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Unlock 568");

	    // Inform the user
	    ROS_INFO("[DEEPC CONTROLLER] Deepc optimization setup successful");
    }
    
    catch(GRBException e)
    {
    	s_Deepc_mutex.lock();
    	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Lock 586");
    	s_setupDeepc_success = false;
    	s_Deepc_mutex.unlock();
    	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Unlock 586");

	    ROS_INFO_STREAM("[DEEPC CONTROLLER] Deepc optimization setup exception with Gurobi error code = " << e.getErrorCode());
	    ROS_INFO_STREAM("[DEEPC CONTROLLER] Error message: " << e.getMessage());
	    ROS_INFO("[DEEPC CONTROLLER] Deepc must be (re-)setup");
  	}
  	catch(exception& e)
    {
    	s_Deepc_mutex.lock();
    	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Lock 598");
    	s_setupDeepc_success = false;
    	s_Deepc_mutex.unlock();
    	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Unlock 598");

	    ROS_INFO_STREAM("[DEEPC CONTROLLER] Deepc optimization setup exception with standard error message: " << e.what());
	    ROS_INFO("[DEEPC CONTROLLER] Deepc must be (re-)setup");
  	}
  	catch(...)
  	{
  		s_Deepc_mutex.lock();
  		// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Lock 609");
    	s_setupDeepc_success = false;
    	s_Deepc_mutex.unlock();
    	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Unlock 609");

    	ROS_INFO("[DEEPC CONTROLLER] Deepc optimization setup exception");
    	ROS_INFO("[DEEPC CONTROLLER] Deepc must be (re-)setup");
  	}
}

void solve_Deepc()
{
	s_Deepc_mutex.lock();
	//ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Lock 622");
	d_uini = s_uini;
	d_yini = s_yini;
	s_Deepc_mutex.unlock();
	//ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Unlock 622");

	try
	{
		// Update equality constraints RHS
		for (d_i = 0; d_i < d_Nuini; d_i++)
			d_grb_ini_constrs[d_i].set(GRB_DoubleAttr_RHS, d_uini(d_i));
		for (d_i = 0; d_i < d_Nyini; d_i++)
			d_grb_ini_constrs[d_Nuini + d_i].set(GRB_DoubleAttr_RHS, d_yini(d_i));

		// Solve optimization - presolve only applies if using dense formulation
		if (d_opt_sparse || !d_grb_presolve_at_setup)
		{
			d_grb_model.optimize();
			d_DeepcOpt_status = d_grb_model.get(GRB_IntAttr_Status);
		}
		else
		{
			d_grb_model_presolved->optimize();
			d_DeepcOpt_status = d_grb_model_presolved->get(GRB_IntAttr_Status);
		}
		

		if (d_DeepcOpt_status == GRB_OPTIMAL)
		{	
			// With sparse formulation can get uf directly
			if (d_opt_sparse)
			{
				for (d_i = 0; d_i < d_Nuf; d_i++)
					d_u_f(d_i) = d_grb_vars[d_uf_start_i + d_i].get(GRB_DoubleAttr_X);
			}
			// With dense formulation get uf through g
			else
			{
				for (d_i = 0; d_i < d_Ng; d_i++)
					d_g(d_i) = d_grb_vars[d_i].get(GRB_DoubleAttr_X);

				d_u_f = d_U_f * d_g;
			}

			s_Deepc_mutex.lock();
			// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Lock 649");
			s_u_f = d_u_f;
			s_Deepc_mutex.unlock();
			//ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Unlock 649");

			ROS_INFO("[DEEPC CONTROLLER] Deepc optimal solution found:");
			ROS_INFO_STREAM("Thrust: " << d_u_f(0));
			ROS_INFO_STREAM("Roll Rate: " << d_u_f(1));
			ROS_INFO_STREAM("Pitch Rate: " << d_u_f(2));
			if (d_Deepc_yaw_control)
				ROS_INFO_STREAM("Yaw Rate: " << d_u_f(3));

			if (d_opt_sparse || !d_grb_presolve_at_setup)
			{
				ROS_INFO_STREAM("Objective: " << d_grb_model.get(GRB_DoubleAttr_ObjVal));
	    		ROS_INFO_STREAM("Runtime: " << d_grb_model.get(GRB_DoubleAttr_Runtime));
			}
			else
			{
				ROS_INFO_STREAM("Objective: " << d_grb_model_presolved->get(GRB_DoubleAttr_ObjVal));
	    		ROS_INFO_STREAM("Runtime: " << d_grb_model_presolved->get(GRB_DoubleAttr_Runtime));
			}
		}
		else
		{
			ROS_INFO_STREAM("[DEEPC CONTROLLER] Deepc failed to find optimal solution with status code = " << d_DeepcOpt_status);
		}
	}
	catch(GRBException e)
    {
    	s_Deepc_mutex.lock();
    	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Lock 678");
    	s_setupDeepc_success = false;
		s_Deepc_mutex.unlock();
		// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Unlock 678");

	    ROS_INFO_STREAM("[DEEPC CONTROLLER] Deepc optimization exception with Gurobi error code = " << e.getErrorCode());
	    ROS_INFO_STREAM("[DEEPC CONTROLLER] Error message: " << e.getMessage());
	    ROS_INFO("[DEEPC CONTROLLER] Deepc must be (re-)setup");
  	}
  	catch(exception& e)
    {
    	s_Deepc_mutex.lock();
    	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Lock 691");
    	s_setupDeepc_success = false;
		s_Deepc_mutex.unlock();
		// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Unlock 691");

	    ROS_INFO_STREAM("[DEEPC CONTROLLER] Deepc optimization exception with standard error message: " << e.what());
	    ROS_INFO("[DEEPC CONTROLLER] Deepc must be (re-)setup");
  	}
  	catch(...)
  	{
  		s_Deepc_mutex.lock();
  		// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Lock 703");
    	s_setupDeepc_success = false;
		s_Deepc_mutex.unlock();
		// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Unlock 703");

    	ROS_INFO("[DEEPC CONTROLLER] Deepc optimization exception");
    	ROS_INFO("[DEEPC CONTROLLER] Deepc must be (re-)setup");
  	}
}

// DEEPC HELPER FUNCTIONS

// Data to Hankel function
MatrixXf data2hankel(MatrixXf data, int num_block_rows)
{
	// data =  Data matrix of size [dimension of data point x number of data points]
	// num_block_rows = number of block rows wanted in Hankel matrix

	int dim = data.rows();
	int num_data_pts = data.cols();
	
	MatrixXf H;
	H.setZero(dim * num_block_rows, num_data_pts - num_block_rows + 1);

	for (int i = 0; i < num_block_rows; i++)
	    for (int j = 0; j < num_data_pts - num_block_rows + 1; j++)
	        H.block(dim*i,j,dim,1) = data.col(i+j);

    return H;
}

// Update uini yini
// This function is called by main thread
void update_uini_yini(Controller::Request &request, control_output &output)
{
	// If Deepc was not setup yet don't do anything as uini and yini matrices are not setup yet
	s_Deepc_mutex.lock();
	//ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Lock 741");
	bool setupDeepc_success = s_setupDeepc_success;
	m_uini = s_uini;
	m_yini = s_yini;
	int num_inputs = s_num_inputs;
	int num_outputs = s_num_outputs;
	int Nuini = s_Nuini;
	int Nyini = s_Nyini;
	s_Deepc_mutex.unlock();
	//ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Unlock 750");

	if (!setupDeepc_success)
		return;

	try
	{
		// Update uini
		int u_shift = Nuini - num_inputs;
		m_uini.topRows(u_shift) = m_uini.bottomRows(u_shift);
		m_uini(u_shift) = output.thrust;
		m_uini(u_shift + 1) = output.rollRate;
		m_uini(u_shift + 2) = output.pitchRate;
		if (yaml_Deepc_yaw_control)
			m_uini(u_shift + 3) = output.yawRate;

		// Update uini
		int y_shift = Nyini - num_outputs;
		m_yini.topRows(y_shift) = m_yini.bottomRows(y_shift);
		m_yini(y_shift) = request.ownCrazyflie.x;
		m_yini(y_shift + 1) = request.ownCrazyflie.y;
		m_yini(y_shift + 2) = request.ownCrazyflie.z;
		if (yaml_Deepc_measure_roll_pitch)
		{
			m_yini(y_shift + 3) = request.ownCrazyflie.roll;
			m_yini(y_shift + 4) = request.ownCrazyflie.pitch;
		}
		if (yaml_Deepc_yaw_control)
			m_yini(Nyini - 1) = request.ownCrazyflie.yaw;

		s_Deepc_mutex.lock();
		//ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Lock 786");
		s_uini = m_uini;
		s_yini = m_yini;
		s_Deepc_mutex.unlock();
		//ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Unlock 786");
	}

	catch(exception& e)
    {
	    ROS_INFO_STREAM("[DEEPC CONTROLLER] Update uini yini exception with standard error message: " << e.what());
  	}
  	catch(...)
  	{
    	ROS_INFO("[DEEPC CONTROLLER] Update uini yini exception");
  	}
}


// ---------- READ/WRITE CSV FILES ----------

// Read csv file into matrix
MatrixXf read_csv(const string & path)
{
	ifstream fin;
	fin.open(path);
	if(!fin)
	{
		MatrixXf M;
		return M;
	}
	string line;
	vector<float> values;
	int rows = 0;
	try
	{
		while (getline(fin, line))
		{
			stringstream lineStream(line);
			string cell;
			bool empty_row = true;
			while (getline(lineStream, cell, ','))
			{
				values.push_back(stof(cell));
				empty_row = false;
			}
			if(!empty_row)
				rows++;
		}
		return Map<Matrix<float, Dynamic, Dynamic, RowMajor>>(values.data(), rows, values.size()/rows);
	}

	catch(exception& e)
    {
	    ROS_INFO_STREAM("[DEEPC CONTROLLER] CSV read exception with standard error message: " << e.what());

	    MatrixXf M;
		return M;
  	}
  	catch(...)
  	{
    	ROS_INFO("[DEEPC CONTROLLER] CSV read exception");

    	MatrixXf M;
		return M;
  	}
}

// Write matrix into csv file
bool write_csv(const string & path, MatrixXf M)
{
	ofstream fout;
	fout.open(path);
	if(!fout)
		return false;
	int rows = M.rows();
	int cols = M.cols();
	for(int i = 0; i < rows; i++)
	{
		for(int j = 0; j < cols - 1; j++)
			fout << M(i,j) << ',';
		fout << M(i,cols-1) << endl;
	}
	return true;
}

//    ------------------------------------------------------------------------------
//    RRRR   EEEEE   QQQ   U   U  EEEEE   SSSS  TTTTT
//    R   R  E      Q   Q  U   U  E      S        T
//    RRRR   EEE    Q   Q  U   U  EEE     SSS     T
//    R   R  E      Q  Q   U   U  E          S    T
//    R   R  EEEEE   QQ Q   UUU   EEEEE  SSSS     T
//    
//    M   M    A    N   N   OOO   EEEEE  U   U  V   V  RRRR   EEEEE
//    MM MM   A A   NN  N  O   O  E      U   U  V   V  R   R  E
//    M M M  A   A  N N N  O   O  EEE    U   U  V   V  RRRR   EEE
//    M   M  AAAAA  N  NN  O   O  E      U   U   V V   R   R  E
//    M   M  A   A  N   N   OOO   EEEEE   UUU     V    R   R  EEEEE
//    ------------------------------------------------------------------------------

// CALLBACK FOR THE REQUEST MANOEUVRE SERVICE
bool requestManoeuvreCallback(IntIntService::Request &request, IntIntService::Response &response)
{
	// Extract the requested manoeuvre
	int requestedManoeuvre = request.data;

	// Switch between the possible manoeuvres
	switch (requestedManoeuvre)
	{
		case DEEPC_CONTROLLER_REQUEST_TAKEOFF:
		{
			// Inform the user
			ROS_INFO("[DEEPC CONTROLLER] Received request to take off. Switch to state: LQR");
			// Update the state accordingly
			m_current_state = DEEPC_CONTROLLER_STATE_LQR;
			m_current_state_changed = true;
			// Provide dummy response
			response.data = 0;
			break;
		}

		case DEEPC_CONTROLLER_REQUEST_LANDING: