Commit c9e75580 authored by elokdae's avatar elokdae
Browse files

Implementation of Deepc setup and optimization, still running LQR controller...

Implementation of Deepc setup and optimization, still running LQR controller with optimization in the background
parent ff1be47f
...@@ -135,6 +135,12 @@ struct control_output ...@@ -135,6 +135,12 @@ struct control_output
// V A A R R III A A BBBB LLLLL EEEEE SSSS // V A A R R III A A BBBB LLLLL EEEEE SSSS
// ---------------------------------------------------------------------------------- // ----------------------------------------------------------------------------------
// NOTE ABOUT THREAD MANAGEMENT
// Variables starting with 's_' are shared between main and Deepc threads
// Mutex must be used before read/writing them
// Variables starting with 'd_' are used by Deepc thread only
// They are declared global for inter-function communication and/or for speed
// All other variables are used by main thread only
// The ID of the agent that this node is monitoring // The ID of the agent that this node is monitoring
int m_agentID; int m_agentID;
...@@ -219,15 +225,15 @@ string yaml_dataFolder = "/work/D-FaLL-System/Deepc_data/"; ...@@ -219,15 +225,15 @@ string yaml_dataFolder = "/work/D-FaLL-System/Deepc_data/";
// CSV output data folder location, relative to dataFolder // CSV output data folder location, relative to dataFolder
string yaml_outputFolder = "output/"; string yaml_outputFolder = "output/";
// Log files folder location, relative to dataFolder
string yaml_logFolder = "log/";
// CSV input data files location, relative to dataFolder // CSV input data files location, relative to dataFolder
string yaml_thrustExcSignalFile = "thrust_exc_signal.csv"; string yaml_thrustExcSignalFile = "thrust_exc_signal.csv";
string yaml_rollRateExcSignalFile = "rollRate_exc_signal.csv"; string yaml_rollRateExcSignalFile = "rollRate_exc_signal.csv";
string yaml_pitchRateExcSignalFile = "pitchRate_exc_signal.csv"; string yaml_pitchRateExcSignalFile = "pitchRate_exc_signal.csv";
string yaml_yawRateExcSignalFile = "yawRate_exc_signal.csv"; string yaml_yawRateExcSignalFile = "yawRate_exc_signal.csv";
// Log files folder location, relative to dataFolder
string yaml_logFolder = "log/";
// Thrust excitation parameters // Thrust excitation parameters
float yaml_thrustExcAmp_in_grams = 0.0; float yaml_thrustExcAmp_in_grams = 0.0;
...@@ -249,28 +255,28 @@ bool yaml_Deepc_measure_roll_pitch = true; ...@@ -249,28 +255,28 @@ bool yaml_Deepc_measure_roll_pitch = true;
// Flag that activates yaw control through Deepc // Flag that activates yaw control through Deepc
bool yaml_Deepc_yaw_control = true; bool yaml_Deepc_yaw_control = true;
// Tini in discrete time steps // Tini in discrete time steps
int yaml_Tini = 3; int s_yaml_Tini = 3;
// Prediction horizon in discrete time steps // Prediction horizon in discrete time steps
int yaml_N = 25; int s_yaml_N = 25;
// Output cost matrix diagonal entries (x, y, z, x_dot, y_dot, z_dot, roll, pitch, yaw) // Output cost matrix diagonal entries (x, y, z, x_dot, y_dot, z_dot, roll, pitch, yaw)
vector<float> yaml_Q = {40.0, 40.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10.0}; vector<float> s_yaml_Q = {40.0, 40.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10.0};
// Input cost matrix diagonal entries (thurst, rollRate, pitchRate, yawRate) // Input cost matrix diagonal entries (thurst, rollRate, pitchRate, yawRate)
vector<float> yaml_R = {4.0, 4.0, 4.0, 1.0}; vector<float> s_yaml_R = {4.0, 4.0, 4.0, 1.0};
// Terminal output cost matrix diagonal entries (x, y, z, x_dot, y_dot, z_dot, roll, pitch, yaw) // Terminal output cost matrix diagonal entries (x, y, z, x_dot, y_dot, z_dot, roll, pitch, yaw)
vector<float> yaml_P = {657.21, 657.21, 8.88, 96.92, 96.92, 0.47, 629.60, 629.60, 84.21}; vector<float> s_yaml_P = {657.21, 657.21, 8.88, 96.92, 96.92, 0.47, 629.60, 629.60, 84.21};
// Regularization parameters // Regularization parameters
float yaml_lambda2_g = 0.0; float s_yaml_lambda2_g = 0.0;
float yaml_lambda2_s = 1.0e7; float s_yaml_lambda2_s = 1.0e7;
// Output constraints (x, y, z, x_dot, y_dot, z_dot, roll, pitch, yaw) // Output constraints (x, y, z, x_dot, y_dot, z_dot, roll, pitch, yaw)
vector<float> yaml_output_min = {-4.0, -4.0, -4.0, -100, -100, -100, -PI/6, -PI/6, -PI/6}; vector<float> s_yaml_output_min = {-4.0, -4.0, -4.0, -100, -100, -100, -PI/6, -PI/6, -PI/6};
vector<float> yaml_output_max = {4.0, 4.0, 4.0, 100, 100, 100, PI/6, PI/6, PI/6}; vector<float> s_yaml_output_max = {4.0, 4.0, 4.0, 100, 100, 100, PI/6, PI/6, PI/6};
// Input constraints (thurst, rollRate, pitchRate, yawRate) // Input constraints (thurst, rollRate, pitchRate, yawRate)
vector<float> yaml_input_min = {0.0, -PI, -PI, -PI}; vector<float> s_yaml_input_min = {0.0, -PI, -PI, -PI};
vector<float> yaml_input_max = {0.6388, PI, PI, PI}; vector<float> s_yaml_input_max = {0.6388, PI, PI, PI};
// Gurobi optimization parameters // Gurobi optimization parameters
bool yaml_grb_LogToFile = false; bool s_yaml_grb_LogToFile = false;
bool yaml_grb_LogToConsole = false; bool s_yaml_grb_LogToConsole = false;
// The weight of the Crazyflie in Newtons, i.e., mg // The weight of the Crazyflie in Newtons, i.e., mg
float m_cf_weight_in_newtons = 0.0; float m_cf_weight_in_newtons = 0.0;
...@@ -292,9 +298,6 @@ string m_dataFolder = HOME + yaml_dataFolder; ...@@ -292,9 +298,6 @@ string m_dataFolder = HOME + yaml_dataFolder;
// Absolute CSV output data folder location // Absolute CSV output data folder location
string m_outputFolder = m_dataFolder + yaml_outputFolder; string m_outputFolder = m_dataFolder + yaml_outputFolder;
// Absolute log files folder location
string m_logFolder = m_dataFolder + yaml_logFolder;
// Thrust excitation variables // Thrust excitation variables
float m_thrustExcAmp_in_newtons = 0.0; float m_thrustExcAmp_in_newtons = 0.0;
MatrixXf m_thrustExcSignal; MatrixXf m_thrustExcSignal;
...@@ -313,7 +316,7 @@ MatrixXf m_pitchRateExcSignal; ...@@ -313,7 +316,7 @@ MatrixXf m_pitchRateExcSignal;
bool m_pitchRateExcEnable = false; bool m_pitchRateExcEnable = false;
int m_pitchRateExcIndex = 0; int m_pitchRateExcIndex = 0;
// Yaw rate excitation in variables // Yaw rate excitation variables
float m_yawRateExcAmp_in_rad = 0.0; float m_yawRateExcAmp_in_rad = 0.0;
MatrixXf m_yawRateExcSignal; MatrixXf m_yawRateExcSignal;
bool m_yawRateExcEnable = false; bool m_yawRateExcEnable = false;
...@@ -325,34 +328,60 @@ MatrixXf m_y_data; ...@@ -325,34 +328,60 @@ MatrixXf m_y_data;
int m_dataIndex = 0; int m_dataIndex = 0;
bool m_write_data = false; bool m_write_data = false;
// Deepc variables // Variables shared between main and Deepc thread
int m_num_inputs; bool s_Deepc_measure_roll_pitch = true;
int m_num_outputs; bool s_Deepc_yaw_control = true;
int m_Ng; float s_cf_weight_in_newtons = m_cf_weight_in_newtons;
MatrixXf m_U_f; string s_dataFolder = m_dataFolder;
Map<MatrixXf> m_r(m_setpoint, 4, 1); string s_logFolder = m_dataFolder + yaml_logFolder;
MatrixXf m_uini; int s_num_inputs;
MatrixXf m_yini; int s_num_outputs;
MatrixXf m_g; int s_Nuini;
MatrixXf m_u_f; int s_Nyini;
MatrixXf s_setpoint = MatrixXf::Zero(4, 1);
// Gurobi optimization variables MatrixXf s_uini;
GRBEnv m_grb_env; MatrixXf s_yini;
GRBModel m_grb_model = GRBModel(m_grb_env); MatrixXf s_u_f;
GRBVar* m_grb_vars = 0; bool s_setupDeepc_success = false;
GRBQuadExpr m_q_obj = 0; int s_DeepcOpt_status = 0;
GRBLinExpr m_l_obj_us = 0;
GRBConstr* m_grb_eq_constrs = 0;
bool m_grb_setup_success = false;
int m_opt_status;
// Variables for thread management // Variables for thread management
mutex m_Deepc_mutex; mutex s_Deepc_mutex;
// Flags for communication with Deepc thread // Flags for communication with Deepc thread
bool m_params_changed = false; bool s_params_changed = false;
bool m_setpoint_changed = false; bool s_setpoint_changed = false;
bool m_setupDeepc = false; bool s_setupDeepc = false;
bool m_solveDeepc = false; bool s_solveDeepc = false;
// Global variables used by Deepc thread only
// Declared as global for inter-function communication and/or speed
string d_logFolder = s_logFolder;
bool d_Deepc_measure_roll_pitch = true;
bool d_Deepc_yaw_control = true;
int d_Nuini;
int d_Nyini;
int d_Ng;
MatrixXf d_U_f;
MatrixXf d_Y_f;
MatrixXf d_Q;
MatrixXf d_P;
MatrixXf d_g;
MatrixXf d_uini;
MatrixXf d_yini;
int d_DeepcOpt_status = 0;
int d_i;
MatrixXf d_u_f;
// Gurobi optimization variables
GRBEnv d_grb_env;
GRBModel d_grb_model = GRBModel(d_grb_env);
GRBVar* d_grb_vars = 0;
GRBQuadExpr d_grb_quad_obj = 0;
GRBLinExpr d_grb_lin_obj_us = 0;
GRBConstr* d_grb_eq_constrs = 0;
// Deepc related global variables used by main thread only
// Declared as global for speed
MatrixXf m_uini;
MatrixXf m_yini;
// ROS Publisher for debugging variables // ROS Publisher for debugging variables
ros::Publisher m_debugPublisher; ros::Publisher m_debugPublisher;
...@@ -392,13 +421,15 @@ ros::Publisher m_manoeuvreCompletePublisher; ...@@ -392,13 +421,15 @@ ros::Publisher m_manoeuvreCompletePublisher;
// DEEPC FUNCTIONS // DEEPC FUNCTIONS
void Deepc_thread_main(); void Deepc_thread_main();
void change_Deepc_params(); void change_Deepc_params();
bool setup_Deepc(); void change_Deepc_setpoint();
bool solve_Deepc(); void setup_Deepc();
void solve_Deepc();
// DEEPC HELPER FUNCTIONS // DEEPC HELPER FUNCTIONS
// DATA TO HANKEL // DATA TO HANKEL
MatrixXf data2hankel(MatrixXf data, int num_block_rows); MatrixXf data2hankel(MatrixXf data, int num_block_rows);
// UPDATE UINI YINI
void update_uini_yini(Controller::Request &request, control_output &output);
// READ/WRITE CSV FILES // READ/WRITE CSV FILES
MatrixXf read_csv(const string & path); MatrixXf read_csv(const string & path);
bool write_csv(const string & path, MatrixXf M); bool write_csv(const string & path, MatrixXf M);
......
...@@ -54,6 +54,19 @@ gainMatrixRollRate : [ 0.00,-2.79, 0.00, 0.00, -1.83, 0.00, 5. ...@@ -54,6 +54,19 @@ gainMatrixRollRate : [ 0.00,-2.79, 0.00, 0.00, -1.83, 0.00, 5.
gainMatrixPitchRate : [ 2.79, 0.00, 0.00, 1.83, 0.00, 0.00, 0.00, 5.91, 0.00] gainMatrixPitchRate : [ 2.79, 0.00, 0.00, 1.83, 0.00, 0.00, 0.00, 5.91, 0.00]
gainMatrixYawRate : [ 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.97] gainMatrixYawRate : [ 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.97]
# Data folder location, relative to HOME path
dataFolder : /work/D-FaLL-System/Deepc_data/
# CSV data files location, relative to dataFolder
outputFolder : output/
thrustExcSignalFile : thrust_exc_signal.csv
rollRateExcSignalFile : rollRate_exc_signal.csv
pitchRateExcSignalFile : pitchRate_exc_signal.csv
yawRateExcSignalFile : yawRate_exc_signal.csv
# Log files folder location, relative to dataFolder
logFolder : log/
# Thrust excitation magnitude, in grams # Thrust excitation magnitude, in grams
thrustExcAmp : 10.0 thrustExcAmp : 10.0
...@@ -69,18 +82,40 @@ yawRateExcAmp : 60.0 ...@@ -69,18 +82,40 @@ yawRateExcAmp : 60.0
# Excitation start time, in s. Used to collect steady-state data before excitation # Excitation start time, in s. Used to collect steady-state data before excitation
exc_start_time : 5.0 exc_start_time : 5.0
# Data folder location, relative to HOME path # EVERYTHING DEEPC
dataFolder : /work/D-FaLL-System/Deepc_data/
# CSV data files location, relative to dataFolder # Flag that indicates whether to use roll and pitch angle measurements in Deepc
outputFolder : output/ Deepc_measure_roll_pitch : true
thrustExcSignalFile : thrust_exc_signal.csv
rollRateExcSignalFile : rollRate_exc_signal.csv
pitchRateExcSignalFile : pitchRate_exc_signal.csv
yawRateExcSignalFile : yawRate_exc_signal.csv
# Log files folder location, relative to dataFolder # Flag that activates yaw control through Deepc
logFolder : log/ Deepc_yaw_control : true
# Tini in discrete time steps
Tini : 3
# Prediction horizon in discrete time steps
N : 25
# Output cost matrix diagonal entries (x, y, z, x_dot, y_dot, z_dot, roll, pitch, yaw)
Q : [40.0, 40.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10.0]
# Input cost matrix diagonal entries (thurst, rollRate, pitchRate, yawRate)
R : [4.0, 4.0, 4.0, 1.0]
# Terminal output cost matrix diagonal entries (x, y, z, x_dot, y_dot, z_dot, roll, pitch, yaw)
P : [657.21, 657.21, 8.88, 96.92, 96.92, 0.47, 629.60, 629.60, 84.21]
# Regularization parameters
lambda2_g : 0.0
lambda2_s : 1.0e7
# Output constraints (x, y, z, x_dot, y_dot, z_dot, roll, pitch, yaw)
output_min : [-4.0, -4.0, -4.0, -100, -100, -100, -0.5236, -0.5236, -0.5236]
output_max : [4.0, 4.0, 4.0, 100, 100, 100, 0.5236, 0.5236, 0.5236]
# Input constraints (thurst, rollRate, pitchRate, yawRate)
input_min : [0.0, -3.1416, -3.1416, -3.1416]
input_max : [0.6388, 3.1416, 3.1416, 3.1416]
# Gurobi optimization parameters # Gurobi optimization parameters
grb_LogToFile : false grb_LogToFile : false
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment