DeepcControllerService.cpp 111 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
//    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:
28
//    A Deepc Controller for students build from
29
30
31
32
33
34
35
36
//
//    ----------------------------------------------------------------------------------





// INCLUDE THE HEADER
37
#include "nodes/DeepcControllerService.h"
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58






//    ----------------------------------------------------------------------------------
//    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
//    ----------------------------------------------------------------------------------


59
// DEEPC FUNCTIONS
60

61
62
// DEEPC THREAD MAIN
// Deepc operations run in seperate thread as they are time consuming
63
void Deepc_thread_main()
64
{
65
66
67
68
	bool params_changed;
	bool setpoint_changed;
	bool setupDeepc;
	bool solveDeepc;
69
70
71

	while (ros::ok())
	{
72
		s_Deepc_mutex.lock();
73
		//ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Lock 72");
74
75
76
77
78
        params_changed = s_params_changed;
        setpoint_changed = s_setpoint_changed;
        setupDeepc = s_setupDeepc;
        solveDeepc = s_solveDeepc;
        s_Deepc_mutex.unlock();
79
        //ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Unlock 72");
80
81
82
83
84
85

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

92
93
94
95
96
        if (setpoint_changed)
        {
        	change_Deepc_setpoint();
        	
        	s_Deepc_mutex.lock();
97
        	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Lock 96");
98
99
        	s_setpoint_changed = false;
        	s_Deepc_mutex.unlock();
100
        	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Unlock 96");
101
        }
102

103
        if (setupDeepc)
104
        {
105
        	setup_Deepc();
106

107
        	s_Deepc_mutex.lock();
108
        	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Lock 107");
109
110
        	s_setupDeepc = false;
        	s_Deepc_mutex.unlock();
111
        	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Unlock 107");
112
113
        }

114
        if (solveDeepc)
115
116
117
        {
        	solve_Deepc();

118
		  	s_Deepc_mutex.lock();
119
		  	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Lock 118");
120
121
        	s_solveDeepc = false;
        	s_Deepc_mutex.unlock();
122
        	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Unlock 118");
123
124
        }
	}
125
126
127

	// Clear any heap allocated variables
	delete[] d_grb_vars;
128
	delete[] d_grb_ini_constrs;
129
130
131
132
}

void change_Deepc_params()
{
133
	s_Deepc_mutex.lock();
134
	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Lock 133");
135
136
137
	d_logFolder = s_logFolder;
	d_Deepc_measure_roll_pitch = s_Deepc_measure_roll_pitch;
	d_Deepc_yaw_control = s_Deepc_yaw_control;
elokdae's avatar
elokdae committed
138
139
140
	d_Tini = s_yaml_Tini;
	d_N = s_yaml_N;
	d_lambda2_g = s_yaml_lambda2_g;
141
142
	d_opt_sparse = s_yaml_opt_sparse;
	bool opt_verbose = s_yaml_opt_verbose;
143
	bool grb_LogToFile = s_yaml_grb_LogToFile;
144
	d_grb_presolve_at_setup = s_yaml_grb_presolve_at_setup;
145
146
	// Deepc setup must be re-run after changes
	s_setupDeepc_success = false;
147
	s_Deepc_mutex.unlock();
148
	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Unlock 133");
149
150
151
152
153
154
155

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

		// Inform the user
	    ROS_INFO("[DEEPC CONTROLLER] Deepc parameters change successful");
160
	    ROS_INFO("[DEEPC CONTROLLER] (Re-)setup Deepc to apply changes");
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
	}

	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();
184
	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Lock 193");
185
186
187
	bool setupDeepc_success = s_setupDeepc_success;
	MatrixXf setpoint = s_setpoint;
	s_Deepc_mutex.unlock();
188
189
190
191
	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Unlock 193");

	if (!setupDeepc_success)
		return;
192
193
194
195

	try
	{
		// UPDATE GUROBI LINEAR COST VECTOR FOR REFERENCE TRACKING
elokdae's avatar
elokdae committed
196
		d_r.topRows(3) = setpoint.topRows(3);
197
		if (d_Deepc_yaw_control)
elokdae's avatar
elokdae committed
198
			d_r.bottomRows(1) = setpoint.bottomRows(1);
199

200
		if (d_opt_sparse)
201
		{
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
			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;
			}
221
222
		}

elokdae's avatar
elokdae committed
223
224
225
226
227
		// 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);

228
		d_grb_c_gs = -2.0 * d_lambda2_g * d_gs;
elokdae's avatar
elokdae committed
229
230
231
232

		// Update linear objective terms
	    d_grb_lin_obj_r = 0;
	    d_grb_lin_obj_gs = 0;
233
234
235
236
237
238
239
240
	    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
elokdae's avatar
elokdae committed
241
	    {
242
243
244
245
246
	    	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];
		    }
elokdae's avatar
elokdae committed
247
	    }
248
249

	    // Update objective
elokdae's avatar
elokdae committed
250
	    d_grb_model.setObjective(d_grb_quad_obj + d_grb_lin_obj_us + d_grb_lin_obj_r + d_grb_lin_obj_gs);
251
252
253
254
255
256
257
258

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

	catch(GRBException e)
    {
    	s_Deepc_mutex.lock();
259
    	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Lock 241");
260
261
    	s_setupDeepc_success = false;
    	s_Deepc_mutex.unlock();
262
    	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Unlock 241");
263
264
265
266
267
268
269
270

	    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();
271
    	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Lock 253");
272
273
    	s_setupDeepc_success = false;
    	s_Deepc_mutex.unlock();
274
    	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Unlock 253");
275
276
277
278
279
280
281

	    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();
282
  		// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Lock 264");
283
284
    	s_setupDeepc_success = false;
    	s_Deepc_mutex.unlock();
285
    	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Unlock 264");
286
287
288
289

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

292
void setup_Deepc()
293
{
294
	s_Deepc_mutex.lock();
295
	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Lock 277");
296
297
298
299
300
301
302
303
304
	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;
305
306
	vector<float> output_min_vec = s_yaml_output_min;
	vector<float> output_max_vec = s_yaml_output_max;
307
	s_Deepc_mutex.unlock();
308
	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Unlock 277");
309

310
311
	try
    {
312
313
314
		MatrixXf u_data_in = read_csv(dataFolder + "u_data.csv");
		if (u_data_in.size() <= 0)
		{
315
			s_Deepc_mutex.lock();
316
			// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Lock 301");
317
318
        	s_setupDeepc_success = false;
        	s_Deepc_mutex.unlock();
319
        	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Unlock 301");
320
321

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

			return;
324
		}
325
326
327
		MatrixXf y_data_in = read_csv(dataFolder + "y_data.csv");
		if (y_data_in.size() <= 0)
		{	
328
			s_Deepc_mutex.lock();
329
			// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Lock 314");
330
331
        	s_setupDeepc_success = false;
        	s_Deepc_mutex.unlock();
332
        	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Unlock 314");
333
334
			
			ROS_INFO("[DEEPC CONTROLLER] Failed to read y data file");
335
336

			return;
337
		}
338

339
340
341
342
343
344
		MatrixXf u_data;
		if (d_Deepc_yaw_control)
			u_data = u_data_in;
		else
			u_data = u_data_in.topRows(3);

345
		MatrixXf y_data;
346
		if (d_Deepc_measure_roll_pitch && d_Deepc_yaw_control)
347
			y_data = y_data_in;
348
		else if (d_Deepc_measure_roll_pitch)
349
			y_data = y_data_in.topRows(5);
350
		else if (d_Deepc_yaw_control)
351
		{
352
			y_data = MatrixXf::Zero(4, y_data_in.cols());
353
354
355
356
357
358
			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);

359
360
		// VARIABLE LENGTHS
		// Number of inputs m
361
		int num_inputs = u_data.rows();
362
		// Number of outputs p
363
		d_num_outputs = y_data.rows();
364
		// Previous inputs vector (uini) length
elokdae's avatar
elokdae committed
365
		d_Nuini = num_inputs * d_Tini;
366
		// Previous outputs vector (yini) length
367
		d_Nyini = d_num_outputs * d_Tini;
368
369
370
371
372
373
374
375
		// 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
elokdae's avatar
elokdae committed
376
377
		MatrixXf H_u = data2hankel(u_data, d_Tini + d_N + 1);
		MatrixXf H_y = data2hankel(y_data, d_Tini + d_N + 1);
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
		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;
		}
398
399
400

		// COST MATRICES
		// Output cost and terminal output cost matrix
401
402
		d_Q = MatrixXf::Zero(d_num_outputs, d_num_outputs);
		d_P = MatrixXf::Zero(d_num_outputs, d_num_outputs);
403
404
		for (int i = 0; i < 3; i++)
		{
405
406
			d_Q(i,i) = Q_vec[i];
			d_P(i,i) = P_vec[i];
407
		}
408
		if (d_Deepc_measure_roll_pitch)
409
410
			for (int i = 3; i < 5; i++)
			{
411
412
				d_Q(i,i) = Q_vec[i+3];
				d_P(i,i) = P_vec[i+3];
413
			}
414
		if (d_Deepc_yaw_control)
415
		{
416
417
			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];
418
419
420
		}

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

		// GUROBI QUADRATIC COST MATRIX
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
		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)
452
		{
453
454
455
			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;
456
457
458
		}

		// GUROBI LINEAR COST VECTOR
elokdae's avatar
elokdae committed
459
		// Steady state input
460
461
		MatrixXf us = MatrixXf::Zero(num_inputs, 1);
		us(0) = cf_weight_in_newtons;
462

elokdae's avatar
elokdae committed
463
		// Reference
464
		d_r = MatrixXf::Zero(d_num_outputs, 1);
elokdae's avatar
elokdae committed
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
		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);

482
483
484
		d_grb_c_gs = -2.0 * d_lambda2_g * d_gs;
		MatrixXf grb_c_us;
		if (d_opt_sparse)
485
		{
486
487
488
489
			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++)
490
			{
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
				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;
514
515
516
			}
		}

517
518
519
520
521
522
523
524
525
		//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];
		}

526
		// OUTPUT CONSTRAINTS
527
528
		MatrixXf output_min = MatrixXf::Zero(d_num_outputs, 1);
		MatrixXf output_max = MatrixXf::Zero(d_num_outputs, 1);
529
530
		for (int i = 0; i < 3; i++)
		{
531
532
			output_min(i) = output_min_vec[i];
			output_max(i) = output_max_vec[i];
533
		}
534
		if (d_Deepc_measure_roll_pitch)
535
536
			for (int i = 3; i < 5; i++)
			{
537
538
				output_min(i) = output_min_vec[i+3];
				output_max(i) = output_max_vec[i+3];
539
			}
540
		if (d_Deepc_yaw_control)
541
		{
542
543
			output_min(d_num_outputs-1) = output_min_vec[8];
			output_max(d_num_outputs-1) = output_max_vec[8];
544
545
546
		}

		// GUROBI LINEAR INEQUALITY CONSTRAINT MATRIX
547
548
549
550
551
552
553
554
555
556
557
558
		// 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;
		}
559

560
		// GUROBI LINEAR INEQUALITY CONSTRAINT VECTOR
561
562
563
564
565
		// Only used in 'dense' formulation
		MatrixXf grb_b;
		if (d_opt_sparse)
			grb_b = MatrixXf::Zero(0, 0);
		else
566
		{
567
568
			grb_b = MatrixXf::Zero(grb_Ag.rows(), 1);
			for (int i = 0; i < d_N + 1; i++)
569
			{
570
571
572
573
574
575
576
				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;
577
578
			}
		}
579

580
		// GUROBI BOUNDS VECTOR
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
		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;
624
625
626
627

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

	    // Clear variables if previously created
632
	    int num_vars = d_grb_model.get(GRB_IntAttr_NumVars);
633
634
635
636
637
	    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;
638
639

	    // Create variables
640
641
642
	    double grb_lb[num_opt_vars];
	    double grb_ub[num_opt_vars];
	    for (int i = 0; i < num_opt_vars; i++)
643
644
645
646
	    {
	    	grb_lb[i] = lb(i);
	    	grb_ub[i] = ub(i);
	    }
647
	    d_grb_vars = d_grb_model.addVars(grb_lb, grb_ub, NULL, NULL, NULL, num_opt_vars);
648
649

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

	    // Set linear objective term
656
	    d_grb_lin_obj_us = 0;
elokdae's avatar
elokdae committed
657
658
	    d_grb_lin_obj_r = 0;
	    d_grb_lin_obj_gs = 0;
659
	    if (d_opt_sparse)
660
	    {
661
662
663
664
665
666
	    	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];
667
	    }
668
669
670
671
672
673
674
675
676
677
	    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];
		    }
	    }
	    
678
	    // Set objective
elokdae's avatar
elokdae committed
679
	    d_grb_model.setObjective(d_grb_quad_obj + d_grb_lin_obj_us + d_grb_lin_obj_r + d_grb_lin_obj_gs);
680
681

	    // Clear constraints if previously created
682
	    int num_constrs = d_grb_model.get(GRB_IntAttr_NumConstrs);
683
684
685
686
687
    	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;
688
    	delete[] d_grb_ini_constrs;
689
690

	    // Add inequality constraints
691
	    // Note that grb_Ag is empty when using sparse formulation and for loop below does not run
692
693
694
	    for (int i = 0; i < grb_Ag.rows(); i++)
	    {
	    	GRBLinExpr lhs = 0;
695
696
697
	    	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));
698
699
	    }

700
701
702
	    // 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++)
703
704
	    {
	    	GRBLinExpr lhs = 0;
705
	    	for (int j = 0; j < num_opt_vars; j++)
706
	    		lhs += grb_A_eq(i,j) * d_grb_vars[j];
707
	    	d_grb_model.addConstr(lhs, '=', grb_b_eq(i));
708
709
	    }

710
711
712
713
	    // 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++)
714
	    {
715
716
717
718
	    	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));
719
	    }
720

721
722
723
724
725
726
727
728
729
730
	    // 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)
731
732
733
734
735
736
	    {
		    static GRBModel grb_model_presolved = d_grb_model.presolve();
		    
		    // Update variables to presolved model variables
		    d_grb_vars = grb_model_presolved.getVars();

737
		    // Update dynamic equality constraints to presolved model constraints. They are last set of constraints in presolved model
738
739
740
		    num_constrs = grb_model_presolved.get(GRB_IntAttr_NumConstrs);
		    GRBConstr* grb_constrs_presolved = grb_model_presolved.getConstrs();
		    int j = 0;
741
	    	for (int i = num_constrs - num_ini_constrs; i < num_constrs; i++)
742
	    	{
743
	    		d_grb_ini_constrs[j] = grb_constrs_presolved[i];
744
745
746
747
748
749
750
751
752
753
	    		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;
		}
754

755
	    // Setup output variables
756
	    d_g = MatrixXf::Zero(d_Ng, 1);
757
758
759
760
761
	    if (d_opt_sparse)
	    {
	    	d_uf_start_i = d_Ng + d_Ns;
	    	d_u_f = MatrixXf::Zero(d_Nuf, 1);
	    }
762
763

	    s_Deepc_mutex.lock();
764
	    // ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Lock 568");
765
		s_num_inputs = num_inputs;
766
		s_num_outputs = d_num_outputs;
767
768
769
770
771
772
		s_Nuini = d_Nuini;
		s_Nyini = d_Nyini;
		s_uini = d_uini;
		s_yini = d_yini;
    	s_setupDeepc_success = true;
		s_Deepc_mutex.unlock();
773
		// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Unlock 568");
774
775
776
777
778
779
780

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

787
	    ROS_INFO_STREAM("[DEEPC CONTROLLER] Deepc optimization setup exception with Gurobi error code = " << e.getErrorCode());
788
	    ROS_INFO_STREAM("[DEEPC CONTROLLER] Error message: " << e.getMessage());
789
	    ROS_INFO("[DEEPC CONTROLLER] Deepc must be (re-)setup");
790
  	}
791
792
  	catch(exception& e)
    {
793
    	s_Deepc_mutex.lock();
794
    	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Lock 598");
795
796
    	s_setupDeepc_success = false;
    	s_Deepc_mutex.unlock();
797
    	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Unlock 598");
798

799
	    ROS_INFO_STREAM("[DEEPC CONTROLLER] Deepc optimization setup exception with standard error message: " << e.what());
800
	    ROS_INFO("[DEEPC CONTROLLER] Deepc must be (re-)setup");
801
  	}
802
803
  	catch(...)
  	{
804
  		s_Deepc_mutex.lock();
805
  		// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Lock 609");
806
807
    	s_setupDeepc_success = false;
    	s_Deepc_mutex.unlock();
808
    	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Unlock 609");
809

810
811
    	ROS_INFO("[DEEPC CONTROLLER] Deepc optimization setup exception");
    	ROS_INFO("[DEEPC CONTROLLER] Deepc must be (re-)setup");
812
813
814
  	}
}

815
void solve_Deepc()
816
{
817
	s_Deepc_mutex.lock();
818
	//ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Lock 622");
819
820
821
	d_uini = s_uini;
	d_yini = s_yini;
	s_Deepc_mutex.unlock();
822
	//ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Unlock 622");
823

824
825
	try
	{
826
827
		// Update equality constraints RHS
		for (d_i = 0; d_i < d_Nuini; d_i++)
828
			d_grb_ini_constrs[d_i].set(GRB_DoubleAttr_RHS, d_uini(d_i));
829
		for (d_i = 0; d_i < d_Nyini; d_i++)
830
			d_grb_ini_constrs[d_Nuini + d_i].set(GRB_DoubleAttr_RHS, d_yini(d_i));
831

832
833
		// Solve optimization - presolve only applies if using dense formulation
		if (d_opt_sparse || !d_grb_presolve_at_setup)
834
835
836
837
838
839
840
841
842
		{
			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);
		}
843
844
845
		

		if (d_DeepcOpt_status == GRB_OPTIMAL)
846
847
848
849
850
851
852
853
854
855
856
857
		{	
			// 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);
858

859
860
				d_u_f = d_U_f * d_g;
			}
861

862
			s_Deepc_mutex.lock();
863
			// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Lock 649");
864
865
			s_u_f = d_u_f;
			s_Deepc_mutex.unlock();
866
			//ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Unlock 649");
867

868
			ROS_INFO("[DEEPC CONTROLLER] Deepc optimal solution found:");
869
870
871
872
873
			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));
874

875
			if (d_opt_sparse || !d_grb_presolve_at_setup)
876
877
878
879
880
881
882
883
884
			{
				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));
			}
885
886
887
		}
		else
		{
888
			ROS_INFO_STREAM("[DEEPC CONTROLLER] Deepc failed to find optimal solution with status code = " << d_DeepcOpt_status);
889
		}
890
891
892
	}
	catch(GRBException e)
    {
893
    	s_Deepc_mutex.lock();
894
    	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Lock 678");
895
896
    	s_setupDeepc_success = false;
		s_Deepc_mutex.unlock();
897
		// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Unlock 678");
898

899
	    ROS_INFO_STREAM("[DEEPC CONTROLLER] Deepc optimization exception with Gurobi error code = " << e.getErrorCode());
900
	    ROS_INFO_STREAM("[DEEPC CONTROLLER] Error message: " << e.getMessage());
901
	    ROS_INFO("[DEEPC CONTROLLER] Deepc must be (re-)setup");
902
  	}
903
904
  	catch(exception& e)
    {
905
    	s_Deepc_mutex.lock();
906
    	// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Lock 691");
907
908
    	s_setupDeepc_success = false;
		s_Deepc_mutex.unlock();
909
		// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Unlock 691");
910

911
912
	    ROS_INFO_STREAM("[DEEPC CONTROLLER] Deepc optimization exception with standard error message: " << e.what());
	    ROS_INFO("[DEEPC CONTROLLER] Deepc must be (re-)setup");
913
  	}
914
915
  	catch(...)
  	{
916
  		s_Deepc_mutex.lock();
917
  		// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Lock 703");
918
919
    	s_setupDeepc_success = false;
		s_Deepc_mutex.unlock();
920
		// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Unlock 703");
921

922
923
    	ROS_INFO("[DEEPC CONTROLLER] Deepc optimization exception");
    	ROS_INFO("[DEEPC CONTROLLER] Deepc must be (re-)setup");
924
925
926
927
928
  	}
}

// DEEPC HELPER FUNCTIONS

929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
// 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;
}

948
949
950
951
952
953
// 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();
954
	//ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Lock 741");
955
956
957
958
959
960
961
962
	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();
963
964
965
966
	//ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Unlock 750");

	if (!setupDeepc_success)
		return;
967

968
	try
969
	{
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
		// 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();
994
		//ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Lock 786");
995
996
997
		s_uini = m_uini;
		s_yini = m_yini;
		s_Deepc_mutex.unlock();
998
		//ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Unlock 786");
999
1000
	}

1001
1002
1003
1004
1005
1006
1007
1008
	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");
  	}
1009
1010
}

1011

1012
1013
1014
1015
1016
1017
1018
1019
1020
1021
1022
1023
1024
1025
1026
// ---------- 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;
1027
	try
1028
	{
1029
		while (getline(fin, line))
1030
		{
1031
1032
1033
1034
1035
1036
1037
1038
1039
1040
			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++;
1041
		}
1042
		return Map<Matrix<float, Dynamic, Dynamic, RowMajor>>(values.data(), rows, values.size()/rows);
1043
	}
1044
1045
1046
1047
1048
1049
1050
1051
1052
1053
1054
1055
1056
1057
1058

	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;
  	}
1059
1060
1061
1062
1063
1064
1065
1066
1067
1068
1069
1070
1071
1072
1073
1074
1075
1076
1077
}

// 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;
}
1078

1079
1080
1081
1082
1083
1084
1085
1086
1087
1088
1089
1090
1091
1092
1093
1094
1095
1096
1097
1098
1099
1100
1101
//    ------------------------------------------------------------------------------
//    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)
	{
1102
		case DEEPC_CONTROLLER_REQUEST_TAKEOFF:
1103
1104
		{
			// Inform the user
1105
			ROS_INFO("[DEEPC CONTROLLER] Received request to take off. Switch to state: LQR");
1106
			// Update the state accordingly
1107
			m_current_state = DEEPC_CONTROLLER_STATE_LQR;
1108
1109
1110
1111
1112
1113
			m_current_state_changed = true;
			// Provide dummy response
			response.data = 0;
			break;
		}

1114
		case DEEPC_CONTROLLER_REQUEST_LANDING:
elokdae's avatar