Skip to content
GitLab
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
Hide whitespace changes
Inline Side-by-side
dfall_ws/src/dfall_pkg/include/nodes/DeepcControllerService.h
View file @ c9e75580
......@@ -135,6 +135,12 @@ struct control_output
// 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
int m_agentID;
......@@ -219,15 +225,15 @@ string yaml_dataFolder = "/work/D-FaLL-System/Deepc_data/";
// CSV output data folder location, relative to dataFolder
string yaml_outputFolder = "output/";
// Log files folder location, relative to dataFolder
string yaml_logFolder = "log/";
// CSV input data files location, relative to dataFolder
string yaml_thrustExcSignalFile = "thrust_exc_signal.csv";
string yaml_rollRateExcSignalFile = "rollRate_exc_signal.csv";
string yaml_pitchRateExcSignalFile = "pitchRate_exc_signal.csv";
string yaml_yawRateExcSignalFile = "yawRate_exc_signal.csv";
// Log files folder location, relative to dataFolder
string yaml_logFolder = "log/";
// Thrust excitation parameters
float yaml_thrustExcAmp_in_grams = 0.0;
......@@ -249,28 +255,28 @@ bool yaml_Deepc_measure_roll_pitch = true;
// Flag that activates yaw control through Deepc
bool yaml_Deepc_yaw_control = true;
// Tini in discrete time steps
int yaml_Tini = 3;
int s_yaml_Tini = 3;
// 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)
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)
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)
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
float yaml_lambda2_g = 0.0;
float yaml_lambda2_s = 1.0e7;
float s_yaml_lambda2_g = 0.0;
float s_yaml_lambda2_s = 1.0e7;
// 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> yaml_output_max = {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> 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)
vector<float> yaml_input_min = {0.0, -PI, -PI, -PI};
vector<float> yaml_input_max = {0.6388, PI, PI, PI};
vector<float> s_yaml_input_min = {0.0, -PI, -PI, -PI};
vector<float> s_yaml_input_max = {0.6388, PI, PI, PI};
// Gurobi optimization parameters
bool yaml_grb_LogToFile = false;
bool yaml_grb_LogToConsole = false;
bool s_yaml_grb_LogToFile = false;
bool s_yaml_grb_LogToConsole = false;
// The weight of the Crazyflie in Newtons, i.e., mg
float m_cf_weight_in_newtons = 0.0;
......@@ -292,9 +298,6 @@ string m_dataFolder = HOME + yaml_dataFolder;
// Absolute CSV output data folder location
string m_outputFolder = m_dataFolder + yaml_outputFolder;
// Absolute log files folder location
string m_logFolder = m_dataFolder + yaml_logFolder;
// Thrust excitation variables
float m_thrustExcAmp_in_newtons = 0.0;
MatrixXf m_thrustExcSignal;
......@@ -313,7 +316,7 @@ MatrixXf m_pitchRateExcSignal;
bool m_pitchRateExcEnable = false;
int m_pitchRateExcIndex = 0;
// Yaw rate excitation in variables
// Yaw rate excitation variables
float m_yawRateExcAmp_in_rad = 0.0;
MatrixXf m_yawRateExcSignal;
bool m_yawRateExcEnable = false;
......@@ -325,34 +328,60 @@ MatrixXf m_y_data;
int m_dataIndex = 0;
bool m_write_data = false;
// Deepc variables
int m_num_inputs;
int m_num_outputs;
int m_Ng;
MatrixXf m_U_f;
Map<MatrixXf> m_r(m_setpoint, 4, 1);
MatrixXf m_uini;
MatrixXf m_yini;
MatrixXf m_g;
MatrixXf m_u_f;
// Gurobi optimization variables
GRBEnv m_grb_env;
GRBModel m_grb_model = GRBModel(m_grb_env);
GRBVar* m_grb_vars = 0;
GRBQuadExpr m_q_obj = 0;
GRBLinExpr m_l_obj_us = 0;
GRBConstr* m_grb_eq_constrs = 0;
bool m_grb_setup_success = false;
int m_opt_status;
// Variables shared between main and Deepc thread
bool s_Deepc_measure_roll_pitch = true;
bool s_Deepc_yaw_control = true;
float s_cf_weight_in_newtons = m_cf_weight_in_newtons;
string s_dataFolder = m_dataFolder;
string s_logFolder = m_dataFolder + yaml_logFolder;
int s_num_inputs;
int s_num_outputs;
int s_Nuini;
int s_Nyini;
MatrixXf s_setpoint = MatrixXf::Zero(4, 1);
MatrixXf s_uini;
MatrixXf s_yini;
MatrixXf s_u_f;
bool s_setupDeepc_success = false;
int s_DeepcOpt_status = 0;
// Variables for thread management
mutex m_Deepc_mutex;
mutex s_Deepc_mutex;
// Flags for communication with Deepc thread
bool m_params_changed = false;
bool m_setpoint_changed = false;
bool m_setupDeepc = false;
bool m_solveDeepc = false;
bool s_params_changed = false;
bool s_setpoint_changed = false;
bool s_setupDeepc = 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 m_debugPublisher;
......@@ -392,13 +421,15 @@ ros::Publisher m_manoeuvreCompletePublisher;
// DEEPC FUNCTIONS
void Deepc_thread_main();
void change_Deepc_params();
bool setup_Deepc();
bool solve_Deepc();
void change_Deepc_setpoint();
void setup_Deepc();
void solve_Deepc();
// DEEPC HELPER FUNCTIONS
// DATA TO HANKEL
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
MatrixXf read_csv(const string & path);
bool write_csv(const string & path, MatrixXf M);
......
dfall_ws/src/dfall_pkg/param/DeepcController.yaml
View file @ c9e75580
......@@ -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]
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
thrustExcAmp : 10.0
......@@ -69,18 +82,40 @@ yawRateExcAmp : 60.0
# Excitation start time, in s. Used to collect steady-state data before excitation
exc_start_time : 5.0
# Data folder location, relative to HOME path
dataFolder : /work/D-FaLL-System/Deepc_data/
# EVERYTHING DEEPC
# 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
# Flag that indicates whether to use roll and pitch angle measurements in Deepc
Deepc_measure_roll_pitch : true
# Log files folder location, relative to dataFolder
logFolder : log/
# Flag that activates yaw control through Deepc
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
grb_LogToFile : false
......
dfall_ws/src/dfall_pkg/src/nodes/DeepcControllerService.cpp
View file @ c9e75580
......@@ -62,103 +62,234 @@
// Deepc operations run in seperate thread as they are time consuming
void Deepc_thread_main()
{
bool params_changed_local;
bool setupDeepc_local;
bool grb_setup_success_local;
bool solveDeepc_local;
bool params_changed;
bool setpoint_changed;
bool setupDeepc;
bool solveDeepc;
while (ros::ok())
{
m_Deepc_mutex.lock();
params_changed_local = m_params_changed;
setupDeepc_local = m_setupDeepc;
solveDeepc_local = m_solveDeepc;
m_Deepc_mutex.unlock();
s_Deepc_mutex.lock();
params_changed = s_params_changed;
setpoint_changed = s_setpoint_changed;
setupDeepc = s_setupDeepc;
solveDeepc = s_solveDeepc;
s_Deepc_mutex.unlock();
if (params_changed)
{
change_Deepc_params();
s_Deepc_mutex.lock();
s_params_changed = false;
s_Deepc_mutex.unlock();
}
if (params_changed_local)
change_Deepc_params;
if (setpoint_changed)
{
change_Deepc_setpoint();
s_Deepc_mutex.lock();
s_setpoint_changed = false;
s_Deepc_mutex.unlock();
}
if (setupDeepc_local)
if (setupDeepc)
{
grb_setup_success_local = setup_Deepc();
setup_Deepc();
m_Deepc_mutex.lock();
m_setupDeepc = false;
m_grb_setup_success = grb_setup_success_local;
m_Deepc_mutex.unlock();
s_Deepc_mutex.lock();
s_setupDeepc = false;
s_Deepc_mutex.unlock();
}
if (solveDeepc_local)
if (solveDeepc)
{
solve_Deepc();
m_Deepc_mutex.lock();
m_solveDeepc = false;
m_Deepc_mutex.unlock();
s_Deepc_mutex.lock();
s_solveDeepc = false;
s_Deepc_mutex.unlock();
}
}
}
void change_Deepc_params()
{
m_Deepc_mutex.lock();
// > Change Gurobi optimization parameters
if (yaml_grb_LogToFile)
m_grb_model.set(GRB_StringParam_LogFile, m_logFolder + "gurobi.log");
else
m_grb_model.set(GRB_StringParam_LogFile, "");
m_grb_model.set(GRB_IntParam_LogToConsole, yaml_grb_LogToConsole);
m_params_changed = false;
s_Deepc_mutex.lock();
d_logFolder = s_logFolder;
d_Deepc_measure_roll_pitch = s_Deepc_measure_roll_pitch;
d_Deepc_yaw_control = s_Deepc_yaw_control;
bool grb_LogToFile = s_yaml_grb_LogToFile;
bool grb_LogToConsole = s_yaml_grb_LogToConsole;
s_Deepc_mutex.unlock();
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, grb_LogToConsole);
// Inform the user
ROS_INFO("[DEEPC CONTROLLER] Deepc parameters change successful");
}
catch(GRBException e)
{
s_Deepc_mutex.lock();
s_setupDeepc_success = false;
s_Deepc_mutex.unlock();
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)
{
s_Deepc_mutex.lock();
s_setupDeepc_success = false;
s_Deepc_mutex.unlock();
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(...)
{
s_Deepc_mutex.lock();
s_setupDeepc_success = false;
s_Deepc_mutex.unlock();
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();
bool setupDeepc_success = s_setupDeepc_success;
s_Deepc_mutex.unlock();
if (!setupDeepc_success)
return
m_Deepc_mutex.unlock();
s_Deepc_mutex.lock();
int N = s_yaml_N;
MatrixXf setpoint = s_setpoint;
int num_outputs = s_num_outputs;
s_Deepc_mutex.unlock();
try
{
// UPDATE GUROBI LINEAR COST VECTOR FOR REFERENCE TRACKING
MatrixXf r = MatrixXf::Zero(num_outputs, 1);
r.topRows(3) = setpoint.topRows(3);
if (d_Deepc_yaw_control)
r.bottomRows(1) = setpoint.bottomRows(1);
MatrixXf grb_cg_r = MatrixXf::Zero(d_Ng, 1);
for (int i = 0; i < N + 1; i++)
{
if (i < N)
grb_cg_r -= 2.0 * d_Y_f.middleRows(i * num_outputs, num_outputs).transpose() * d_Q * r;
else
grb_cg_r -= 2.0 * d_Y_f.middleRows(i * num_outputs, num_outputs).transpose() * d_P * r;
}
// Update linear objective term for reference tracking
GRBLinExpr grb_lin_obj_r = 0;
for (int i = 0; i < d_Ng; i++)
grb_lin_obj_r += grb_cg_r(i) * d_grb_vars[i];
// Update objective
d_grb_model.setObjective(d_grb_quad_obj + d_grb_lin_obj_us + grb_lin_obj_r);
// Inform the user
ROS_INFO("[DEEPC CONTROLLER] Deepc setpoint update successful");
}
catch(GRBException e)
{
s_Deepc_mutex.lock();
s_setupDeepc_success = false;
s_Deepc_mutex.unlock();
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();
s_setupDeepc_success = false;
s_Deepc_mutex.unlock();
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();
s_setupDeepc_success = false;
s_Deepc_mutex.unlock();
ROS_INFO("[DEEPC CONTROLLER] Deepc setpoint update exception");
ROS_INFO("[DEEPC CONTROLLER] Deepc must be (re-)setup");
}
}
bool setup_Deepc()
void setup_Deepc()
{
m_Deepc_mutex.lock();
string dataFolder_local = m_dataFolder;
bool Deepc_measure_roll_pitch_local = yaml_Deepc_measure_roll_pitch;
bool Deepc_yaw_control_local = yaml_Deepc_yaw_control;
int Tini_local = yaml_Tini;
int N_local = yaml_N;
vector<float> Q_local = yaml_Q;
vector<float> R_local = yaml_R;
vector<float> P_local = yaml_P;
float lambda2_g_local = yaml_lambda2_g;
float lambda2_s_local = yaml_lambda2_s;
float cf_weight_in_newtons_local = m_cf_weight_in_newtons;
MatrixXf r_local = m_r;
vector<float> output_min_local = yaml_output_min;
vector<float> output_max_local = yaml_output_max;
vector<float> input_min_local = yaml_input_min;
vector<float> input_max_local = yaml_input_max;
MatrixXf uini_local = m_uini;
MatrixXf yini_local = m_yini;
m_Deepc_mutex.unlock();
s_Deepc_mutex.lock();
string dataFolder = s_dataFolder;
int Tini = s_yaml_Tini;
int N = s_yaml_N;
vector<float> Q_vec = s_yaml_Q;
vector<float> R_vec = s_yaml_R;
vector<float> P_vec = s_yaml_P;
float lambda2_g = s_yaml_lambda2_g;
float lambda2_s = s_yaml_lambda2_s;
float cf_weight_in_newtons = s_cf_weight_in_newtons;
MatrixXf setpoint = s_setpoint;
vector<float> output_min_vec = s_yaml_output_min;
vector<float> output_max_vec = s_yaml_output_max;
vector<float> input_min_vec = s_yaml_input_min;
vector<float> input_max_vec = s_yaml_input_max;
s_Deepc_mutex.unlock();
try
{
MatrixXf y_data_in = read_csv(dataFolder_local + "u_data.csv");
MatrixXf y_data_in = read_csv(dataFolder + "u_data.csv");
if (y_data_in.size() <= 0)
{
{
s_Deepc_mutex.lock();
s_setupDeepc_success = false;
s_Deepc_mutex.unlock();
ROS_INFO("[DEEPC CONTROLLER] Failed to read y data file");
return false;
return;
}
MatrixXf u_data_in = read_csv(dataFolder_local + "u_data.csv");
MatrixXf u_data_in = read_csv(dataFolder + "u_data.csv");
if (u_data_in.size() <= 0)
{
ROS_INFO("[DEEPC CONTROLLER] Failed to read u data file");
return false;
s_Deepc_mutex.lock();
s_setupDeepc_success = false;
s_Deepc_mutex.unlock();
ROS_INFO("[DEEPC CONTROLLER] Failed to read u data file");
return;
}
MatrixXf y_data;
if (Deepc_measure_roll_pitch_local && Deepc_yaw_control_local)
if (d_Deepc_measure_roll_pitch && d_Deepc_yaw_control)
y_data = y_data_in;
else if (Deepc_measure_roll_pitch_local)
else if (d_Deepc_measure_roll_pitch)
y_data = y_data_in.topRows(5);
else if (Deepc_yaw_control_local)
else if (d_Deepc_yaw_control)
{
y_data = MatrixXf(4, y_data_in.cols());
y_data.topRows(3) = y_data_in.topRows(3);
......@@ -168,315 +299,354 @@ bool setup_Deepc()
y_data = y_data_in.topRows(3);
MatrixXf u_data;
if (Deepc_yaw_control_local)
if (d_Deepc_yaw_control)
u_data = u_data_in;
else
u_data = u_data_in.topRows(3);
// HANKEL MATRICES
int num_inputs_local = u_data.rows();
int num_outputs_local = y_data.rows();
MatrixXf H_u = data2hankel(u_data, Tini_local + N_local + 1);
MatrixXf H_y = data2hankel(y_data, Tini_local + N_local + 1);
MatrixXf U_p = H_u.topRows(num_inputs_local * Tini_local);
m_U_f = H_u.middleRows(num_inputs_local * Tini_local, num_inputs_local * N_local);
MatrixXf Y_p = H_y.topRows(num_outputs_local * Tini_local);
MatrixXf Y_f = H_y.bottomRows(num_outputs_local * (N_local + 1));
int num_inputs = u_data.rows();
int num_outputs = y_data.rows();
d_Nuini = num_inputs * Tini;
d_Nyini = num_outputs * Tini;
MatrixXf H_u = data2hankel(u_data, Tini + N + 1);
MatrixXf H_y = data2hankel(y_data, Tini + N + 1);
MatrixXf U_p = H_u.topRows(num_inputs * Tini);
d_U_f = H_u.middleRows(num_inputs * Tini, num_inputs * N);