StudentControllerService.cpp 75.4 KB
Newer Older
beuchatp's avatar
beuchatp committed
1
//    Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat, Angel Romero, Cyrill Burgener, Marco Mueller, Philipp Friedli
roangel's avatar
roangel committed
2
//
3
4
5
//    This file is part of D-FaLL-System.
//    
//    D-FaLL-System is free software: you can redistribute it and/or modify
roangel's avatar
roangel committed
6
7
8
//    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.
9
10
//    
//    D-FaLL-System is distributed in the hope that it will be useful,
roangel's avatar
roangel committed
11
12
13
//    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.
14
//    
roangel's avatar
roangel committed
15
//    You should have received a copy of the GNU General Public License
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
//    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:
//    Place for students to implement their controller
//
//    ----------------------------------------------------------------------------------
31
32


33
34
35



36
37
// INCLUDE THE HEADER
#include "nodes/StudentControllerService.h"
38

39

40
41
42
43
44
45
46
47
48
49
50
51
52
//    ----------------------------------------------------------------------------------
//    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
//    ----------------------------------------------------------------------------------
roangel's avatar
roangel committed
53

54

roangel's avatar
roangel committed
55
56


roangel's avatar
roangel committed
57

58
59
60
61
62
63
64
65
66
67
68
69
70
//    ------------------------------------------------------------------------------
//     OOO   U   U  TTTTT  EEEEE  RRRR 
//    O   O  U   U    T    E      R   R
//    O   O  U   U    T    EEE    RRRR
//    O   O  U   U    T    E      R  R
//     OOO    UUU     T    EEEEE  R   R
//
//     CCCC   OOO   N   N  TTTTT  RRRR    OOO   L           L       OOO    OOO   PPPP
//    C      O   O  NN  N    T    R   R  O   O  L           L      O   O  O   O  P   P
//    C      O   O  N N N    T    RRRR   O   O  L           L      O   O  O   O  PPPP
//    C      O   O  N  NN    T    R  R   O   O  L           L      O   O  O   O  P
//     CCCC   OOO   N   N    T    R   R   OOO   LLLLL       LLLLL   OOO    OOO   P
//    ----------------------------------------------------------------------------------
roangel's avatar
roangel committed
71

72
// This function is the callback that is linked to the "StudentController" service that
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
// is advertised in the main function. This must have arguments that match the
// "input-output" behaviour defined in the "Controller.srv" file (located in the "srv"
// folder)
//
// The arument "request" is a structure provided to this service with the following two
// properties:
// request.ownCrazyflie
// This property is itself a structure of type "CrazyflieData",  which is defined in the
// file "CrazyflieData.msg", and has the following properties
// string crazyflieName
//     float64 x                         The x position of the Crazyflie [metres]
//     float64 y                         The y position of the Crazyflie [metres]
//     float64 z                         The z position of the Crazyflie [metres]
//     float64 roll                      The roll component of the intrinsic Euler angles [radians]
//     float64 pitch                     The pitch component of the intrinsic Euler angles [radians]
//     float64 yaw                       The yaw component of the intrinsic Euler angles [radians]
//     float64 acquiringTime #delta t    The time elapsed since the previous "CrazyflieData" was received [seconds]
//     bool occluded                     A boolean indicted whether the Crazyflie for visible at the time of this measurement
// The values in these properties are directly the measurement taken by the Vicon
// motion capture system of the Crazyflie that is to be controlled by this service
//
// request.otherCrazyflies
// This property is an array of "CrazyflieData" structures, what allows access to the
// Vicon measurements of other Crazyflies.
//
// The argument "response" is a structure that is expected to be filled in by this
// service by this function, it has only the following property
// response.ControlCommand
// This property is iteself a structure of type "ControlCommand", which is defined in
// the file "ControlCommand.msg", and has the following properties:
//     float32 roll                      The command sent to the Crazyflie for the body frame x-axis
//     float32 pitch                     The command sent to the Crazyflie for the body frame y-axis
//     float32 yaw                       The command sent to the Crazyflie for the body frame z-axis
//     uint16 motorCmd1                  The command sent to the Crazyflie for motor 1
//     uint16 motorCmd2                  The command sent to the Crazyflie for motor 1
//     uint16 motorCmd3                  The command sent to the Crazyflie for motor 1
//     uint16 motorCmd4                  The command sent to the Crazyflie for motor 1
//     uint8 onboardControllerType       The flag sent to the Crazyflie for indicating how to implement the command
// 
// IMPORTANT NOTES FOR "onboardControllerType"  AND AXIS CONVENTIONS
// > The allowed values for "onboardControllerType" are in the "Defines" section at the
//   top of this file, they are MOTOR_MODE, RATE_MODE, OR ANGLE_MODE.
// > In the PPS exercise we will only use the RATE_MODE.
// > In RATE_MODE the ".roll", ".ptich", and ".yaw" properties of "response.ControlCommand"
//   specify the angular rate in [radians/second] that will be requested from the
//   PID controllers running in the Crazyflie 2.0 firmware.
// > In RATE_MODE the ".motorCmd1" to ".motorCmd4" properties of "response.ControlCommand"
//   are the baseline motor commands requested from the Crazyflie, with the adjustment
//   for body rates being added on top of this in the firmware (i.e., as per the code
//   of the "distribute_power" function provided in exercise sheet 2).
// > In RATE_MODE the axis convention for the roll, pitch, and yaw body rates returned
//   in "response.ControlCommand" should use right-hand coordinate axes with x-forward
//   and z-upwards (i.e., the positive z-axis is aligned with the direction of positive
//   thrust). To assist, teh following is an ASCII art of this convention:
//
// ASCII ART OF THE CRAZYFLIE 2.0 LAYOUT
//
//  > This is a top view,
//  > M1 to M4 stand for Motor 1 to Motor 4,
//  > "CW"  indicates that the motor rotates Clockwise,
//  > "CCW" indicates that the motor rotates Counter-Clockwise,
//  > By right-hand axis convention, the positive z-direction points our of the screen,
//  > This being a "top view" means tha the direction of positive thrust produced
//    by the propellers is also out of the screen.
//
//        ____                         ____
//       /    \                       /    \
//  (CW) | M4 |           x           | M1 | (CCW)
//       \____/\          ^          /\____/
//            \ \         |         / /
//             \ \        |        / /
//              \ \______ | ______/ /
//               \        |        /
//                |       |       |
//        y <-------------o       |
//                |               |
//               / _______________ \
//              / /               \ \
//             / /                 \ \
//        ____/ /                   \ \____
//       /    \/                     \/    \
// (CCW) | M3 |                       | M2 | (CW)
//       \____/                       \____/
//
157
//
158
//
159
// The distributed controller 
160
bool calculateControlOutput(Controller::Request &request, Controller::Response &response)
161
162
163
164
165
166
167
168
{	
	switch(controller_state)
	{
		case 0:
		{
			// --------------------------------------
			// Motors Off
			// --------------------------------------
169
170
171
172
173
174
			if (outer_loop_counter == 0) 
			{
				ROS_INFO("[STUDENT CONTROLLER] computed Motors Off control command");
			}
			turn_motors_off(response);
			break;
175
176
177
		}
		case 1:
		{
Tobias's avatar
Tobias committed
178
			
179
180
181
			// --------------------------------------
			// Take-off Copters
			// --------------------------------------
Tobias's avatar
Tobias committed
182
183
184
185
186
			if (outer_loop_counter == 0) 
			{
				// current load position is origin
				load_setpoint[0] 	= request.otherCrazyflies[3].x;
				load_setpoint[1] 	= request.otherCrazyflies[3].y;
187
				load_setpoint[2] 	= 0.0;
188
				float yaw = request.otherCrazyflies[3].yaw;
189
190
				while(yaw > PI) {yaw -= 2 * PI;}
    			while(yaw < -PI) {yaw += 2 * PI;}
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
    			load_setpoint[3] 	= yaw;

    			// convert agent position w.r.t. the load into world frame
    			std::vector<float> distance_to_load_world;
    			if (my_agentID == 1)
    			{
    				distance_to_load_world = transformation_frame_load_to_world(attachmentPoint.P1 , 0.0, 0.0, yaw );
    			}
    			else if (my_agentID == 2)
    			{
    				distance_to_load_world = transformation_frame_load_to_world(attachmentPoint.P2 , 0.0, 0.0, yaw );
    			}
    			else if (my_agentID == 3)
    			{
    				distance_to_load_world = transformation_frame_load_to_world(attachmentPoint.P3 , 0.0, 0.0, yaw );
    			}

    			agent_setpoint[0] 	= load_setpoint[0] + distance_to_load_world[0];
				agent_setpoint[1] 	= load_setpoint[1] + distance_to_load_world[1];
				agent_setpoint[2] 	= load_setpoint[2] + takeOffinitial;
				agent_setpoint[3] 	= 0.0 ;
Tobias's avatar
Tobias committed
212
			}
213
			if (outer_loop_counter <= (int)(control_frequency*durationTakeOff) ) {
214
215
				agent_setpoint[2] += takeOffDistance_increment;
			}
216
217
218
			else
			{
				agent_setpoint[2] = takeOffDistance;
Tobias's avatar
Tobias committed
219
220
				float delta_z_current = request.ownCrazyflie.z - takeOffDistance ;
				gravity_force -= 0.001*delta_z_current;
221
			}
222
223
			// compute agent's lqr command for the setpoint
			agent_lqr_command( request , response );
Tobias's avatar
Tobias committed
224

225
			
226
			break;
227
228
		}
		case 2:
229
230
231
232
233
		{
			// --------------------------------------
			// Distributed controller copter and load
			// --------------------------------------
			if (outer_loop_counter == 0) {
234
235
236
				// distributed controller was enabled
				ROS_INFO("[STUDENT CONTROLLER] Distributed controller enabled");	

237
				// load z-setpoint is current load z-position
238
239
				load_setpoint[0] = request.otherCrazyflies[3].x;	
				load_setpoint[1] = request.otherCrazyflies[3].y;	
240
				load_setpoint[2] = request.otherCrazyflies[3].z;			
241
242
			}

243
244
245
			// Distributed Controller runs at 100Hz --> downsampling by factor 2
			if (outer_loop_counter % 2 == 0)
			{
246
				// update measurement vector using vicon Data
247
				get_y_from_CFData( request );
248

249
			}
250
251


252
			// compute distributed controller commands
253
			distributed_control_command( request, response );
Tobias's avatar
Tobias committed
254
255

			// compute agent's lqr command for the setpoint
256
			//agent_lqr_command( request , response );
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271

			break;
		}	
		case 3:
		{
			float delta_z_load_current = request.otherCrazyflies[3].z - landingDistance_load ;
			// --------------------------------------
			// Prepare to land the load
			// --------------------------------------
			if (outer_loop_counter == 0) {
				ROS_INFO("[STUDENT CONTROLLER] Prepare to land the load");	
				z_load_decrement = delta_z_load_current / ( control_frequency * durationLanding_load);

			}

Tobias's avatar
Tobias committed
272
			if (delta_z_load_current > 0.0)
273
274
			{
				// load has not reached landing distance
275
				if (outer_loop_counter <= (int)(control_frequency*durationLanding_load)) {
276
277
278
279
					load_setpoint[2] -= landingDistance_decrement;
				}
				else {
					// switch to landing copters
Tobias's avatar
Tobias committed
280
					controller_state = 4;
281
				}
282

283
284
			}

285
286
287
			// Distributed Controller runs at 100Hz --> downsampling by factor 2
			if (outer_loop_counter % 2 == 0)
			{
288
				// update measurement vector using vicon Data
289
				get_y_from_CFData( request );
290

291
			}
292
293

			// compute distributed controller commands
294
			distributed_control_command ( request, response );
295

296
			break;
297
		}			
298
		case 4:
299
		{
300

301
			float delta_z_current = request.ownCrazyflie.z - landingDistance;
302
303
304
			// --------------------------------------
			// Landing Copters
			// --------------------------------------
305
306
307
			if ( outer_loop_counter == 0 ) {
				// Landing was initiated
				landingDistance_decrement = delta_z_current / ( control_frequency * durationLanding );
308
309
310
311
				// current load position is origin
				load_setpoint[0] 	= request.otherCrazyflies[3].x;
				load_setpoint[1] 	= request.otherCrazyflies[3].y;
				load_setpoint[2] 	= request.otherCrazyflies[3].z;
312
				float yaw = request.otherCrazyflies[3].yaw;
313
314
315
316
317
318
319
320
321
322
323
324
				while(yaw > PI) {yaw -= 2 * PI;}
    			while(yaw < -PI) {yaw += 2 * PI;}
    			load_setpoint[3] 	= yaw;

    			// convert agent position w.r.t. the load into world frame
    			std::vector<float> distance_to_load_world;
    			std::vector<float> landing_delta; 			// for safely flying the copters next to the laod 
    			if (my_agentID == 1)
    			{
    				landing_delta = {0.1 , 0.0, 0.0};
    				landing_delta[0] += attachmentPoint.P1[0];
    				landing_delta[1] += attachmentPoint.P1[1];
325
    				landing_delta[2] += attachmentPoint.P1[2];				
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
    			}
    			else if (my_agentID == 2)
    			{
    				landing_delta = {-0.1 , 0.1, 0.0};
     				landing_delta[0] += attachmentPoint.P2[0];
    				landing_delta[1] += attachmentPoint.P2[1];
    				landing_delta[2] += attachmentPoint.P2[2];
    			}
    			else if (my_agentID == 3)
    			{
    				landing_delta = {-0.1 , -0.1, 0.0};
     				landing_delta[0] += attachmentPoint.P3[0];
    				landing_delta[1] += attachmentPoint.P3[1];
    				landing_delta[2] += attachmentPoint.P3[2];   				
    			}

342
    			distance_to_load_world = transformation_frame_load_to_world(landing_delta , 0.0, 0.0, yaw );
343
344
345
346
    			agent_setpoint[0] 	= load_setpoint[0] + distance_to_load_world[0];
				agent_setpoint[1] 	= load_setpoint[1] + distance_to_load_world[1];
				agent_setpoint[2] 	= request.ownCrazyflie.z;
				agent_setpoint[3] 	= 0.0 ;		
347

348
349
			}
			// check if copter is within landing distance
Tobias's avatar
Tobias committed
350
			if ( delta_z_current > 0.0 ) 
351
			{
352
				/// decrease z-setpoint
353
354
				if (outer_loop_counter <= (int)( control_frequency*durationLanding) ) 
				{
355
					agent_setpoint[2] -= landingDistance_decrement;
356
357
					// compute agent's lqr command for new setpoint
					agent_lqr_command( request , response );					
358
				}
359
360
				else 
				{
361
362
363
364
365
					// landing duration is over
					turn_motors_off( response );
					// set controller state to zero
					controller_state = 0;					
				}
366
			}
367
368
			else 
			{
369
370
				// copter is within landing distance
				turn_motors_off( response );
371
				// set controller state to zero
372
373
374
				controller_state = 0;
			}
			break;			
375
		}
376
377
		case 5:
		{
378

379
380
381
382
383
384
385
386
387
388
			// --------------------------------------
			// Centralized LQG controller copter and load
			// --------------------------------------
			if (outer_loop_counter == 0) {
				// distributed controller was enabled
				ROS_INFO("[STUDENT CONTROLLER] Centralized LQG enabled");	

				// load z-setpoint is current load z-position
				load_setpoint[0] = request.otherCrazyflies[3].x;	
				load_setpoint[1] = request.otherCrazyflies[3].y;	
389
				load_setpoint[2] = request.otherCrazyflies[3].z;
390
391
				//load_setpoint[3] = request.otherCrazyflies[3].yaw;
				//gravity_force_copterloadsystem = gravity_force + load_mass * 9.81/(1000*4*3);
392
			
393
394
395
			}

			// Distributed Controller runs at 100Hz --> downsampling by factor 2
396
			if (outer_loop_counter % 2 == 0)
397
398
			{
				// update measurement vector using vicon Data
399
400
				//get_y_from_CFData( request );
				get_y_from_CFData_withLoad( request );
401
				/*
402
403
404
405
406
				y_only_agent_1 = y_to_controller;
				for(int i = 6 ; i < 24 ; ++i )
				{
					y_only_agent_1(i,0) = 0;
				}
407
				*/
408
409
410
				// compute centralized controller commands
				centralized_control_command( request, response );
				//display_control_agent1( request, response);
411
				
412
413
			}

414
			//float delta_z_load_current = request.otherCrazyflies[3].z - load_setpoint[2];
Tobias's avatar
Tobias committed
415
			// gravity_force_copterloadsystem -= 0.001*delta_z_load_current;
416
			// compute agent's lqr command for the setpoint
Paul Beuchat's avatar
Paul Beuchat committed
417
			//agent_lqr_command( request , response );
418
419
420

			break;
		}			
421
422
423
424
425
	}
	outer_loop_counter += 1;
	return true;
}

426

427
void distributed_control_command( Controller::Request &request, Controller::Response &response)
428
{
429

430
431
432
	// Control output
	Eigen::Matrix<float,12,1> u = overlappingController.K * overlapping_estimator_state_prev;
	// Estimator update
433
	overlapping_estimator_state_prev = overlappingController.Phi * overlapping_estimator_state_prev + overlappingController.Gamma * y_to_controller;
434

435
	// extract motor command from u:
436
437
438
439
	float thrustSum 	= 0.0;
	float outRoll		= 0.0;
	float outPitch		= 0.0;
	float outYaw		= 0.0;
440

Tobias's avatar
same    
Tobias committed
441
442
	//if ( request.ownCrazyflie.crazyflieName == request.otherCrazyflies[0].crazyflieName )
	if (my_agentID == 1 )
443
444
	{
		// Controller for crazyflie 01
445
446
447
448
		thrustSum 	= u(0,0);
		outRoll		= u(1,0);
		outPitch	= u(2,0);
		outYaw		= u(3,0);
449
	}
Tobias's avatar
same    
Tobias committed
450
451
	//else if (request.ownCrazyflie.crazyflieName == request.otherCrazyflies[1].crazyflieName )
	else if (my_agentID == 2 )
452
453
	{
		// Controller for crazyflie 02
454
455
456
457
		thrustSum 	= u(4,0);
		outRoll		= u(5,0);
		outPitch	= u(6,0);
		outYaw		= u(7,0);
458
	}
Tobias's avatar
same    
Tobias committed
459
460
	//else if (request.ownCrazyflie.crazyflieName == request.otherCrazyflies[2].crazyflieName )
	else if (my_agentID == 3 )
461
462
	{
		// Controller for crazyflie 03
463
464
465
466
		thrustSum 	= u(8,0);
		outRoll		= u(9,0);
		outPitch	= u(10,0);
		outYaw		= u(11,0);	
467
468
	}

469
	if (outer_loop_counter % 100 == 0)
470
471
472
	{
		ROS_INFO_STREAM("[STUDENT CONTROLLER] u thrust sum computed = " << thrustSum );
	}
473
	
474
	// PREPARE AND RETURN THE VARIABLE "response"
475
	
Tobias's avatar
Tobias committed
476
477
478
    response.controlOutput.roll 		= outRoll;
    response.controlOutput.pitch 		= outPitch;
    response.controlOutput.yaw 			= outYaw;
479
480
481
482
    response.controlOutput.motorCmd1 	= computeMotorPolyBackward(thrustSum/4 + gravity_force_copterloadsystem);
    response.controlOutput.motorCmd2 	= computeMotorPolyBackward(thrustSum/4 + gravity_force_copterloadsystem);
    response.controlOutput.motorCmd3 	= computeMotorPolyBackward(thrustSum/4 + gravity_force_copterloadsystem);
    response.controlOutput.motorCmd4 	= computeMotorPolyBackward(thrustSum/4 + gravity_force_copterloadsystem);
483
  
484
485
	// choosing the Crazyflie onBoard controller type.
	// it can either be Motor, Rate or Angle based 
486
487
488
	// response.controlOutput.onboardControllerType = MOTOR_MODE;
	response.controlOutput.onboardControllerType = RATE_MODE;
	// response.controlOutput.onboardControllerType = ANGLE_MODE;
489
490
491
}


492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
void display_control_agent1( Controller::Request &request, Controller::Response &response)
{

	// Control output
	Eigen::Matrix<float,12,1> u = centralized_controller.K * estimator_state_prev_only_agent1;
	// Estimator update
	estimator_state_prev_only_agent1 = centralized_controller.Phi * estimator_state_prev_only_agent1 + centralized_controller.Gamma * y_only_agent_1;

	// extract motor command from u:
	float thrustSum 	= 0.0;
	float outRoll		= 0.0;
	float outPitch		= 0.0;
	float outYaw		= 0.0;

	if (my_agentID == 1 )
	{
		// Controller for crazyflie 01
		thrustSum 	= u(0,0);
		outRoll		= u(1,0);
		outPitch	= u(2,0);
		outYaw		= u(3,0);
	}


	if (outer_loop_counter % 100 == 0)
	{
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
		ROS_INFO_STREAM("--------------------------" );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: roll estimate = " 			<< estimator_state_prev_only_agent1[0] );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: pitch estimate = " 			<< estimator_state_prev_only_agent1[1] );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: yaw estimate = " 			<< estimator_state_prev_only_agent1[2] );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: cable phi estimate = " 		<< estimator_state_prev_only_agent1[3] );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: cable theta estimate = " 	<< estimator_state_prev_only_agent1[4] );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: cable phi_dot estimate = " 	<< estimator_state_prev_only_agent1[5] );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: cable ttheta_dot estimate = " << estimator_state_prev_only_agent1[6] );

	}

	if (outer_loop_counter % 100 == 0)
	{
		ROS_INFO_STREAM("--------------------------" );		
		ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: u thrust sum computed = " 		<< thrustSum );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: desired roll-rate computed = " 	<< outRoll );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: desired pitch-rate computed = " 	<< outPitch );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: desired yaw-rate computed = " 	<< outYaw );
536
537
538
539
540

	}
	// Debugging
	if (outer_loop_counter % 100 == 0)
	{	
541
		ROS_INFO_STREAM("--------------------------" );	
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
		ROS_INFO_STREAM("[STUDENT CONTROLLER] roll-error for copter1 = " << y_only_agent_1(0,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] pitch-error for copter1 = " << y_only_agent_1(1,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] yaw-error for copter1 = " << y_only_agent_1(2,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] x-error for copter1 = " << y_only_agent_1(3,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] y-error for copter1 = " << y_only_agent_1(4,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] z-error for copter1 = " << y_only_agent_1(5,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] roll-error for copter2 = " << y_only_agent_1(6,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] pitch-error for copter2 = " << y_only_agent_1(7,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] yaw-error for copter2 = " << y_only_agent_1(8,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] x-error for copter2 = " << y_only_agent_1(9,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] y-error for copter2 = " << y_only_agent_1(10,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] z-error for copter2 = " << y_only_agent_1(11,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] roll-error for copter3 = " << y_only_agent_1(12,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] pitch-error for copter3 = " << y_only_agent_1(13,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] yaw-error for copter3 = " << y_only_agent_1(14,0) );		
		ROS_INFO_STREAM("[STUDENT CONTROLLER] x-error for copter3 = " << y_only_agent_1(15,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] y-error for copter3 = " << y_only_agent_1(16,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] z-error for copter3 = " << y_only_agent_1(17,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] x-error for load = " 		<< y_only_agent_1(18,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] y-error for load = " 		<< y_only_agent_1(19,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] z-error for load = " 		<< y_only_agent_1(20,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] roll-error for load = " << y_only_agent_1(21,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] pitch-error for load = " << y_only_agent_1(22,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] yaw-error for load = " << y_only_agent_1(23,0) );	
		ROS_INFO_STREAM("[STUDENT CONTROLLER] y_only_agent_1^T = " << y_only_agent_1.transpose() );		
	}

	// PREPARE AND RETURN THE VARIABLE "response"
    response.controlOutput.roll 		= outRoll;
    response.controlOutput.pitch 		= outPitch;
    response.controlOutput.yaw 			= outYaw;
    response.controlOutput.motorCmd1 	= computeMotorPolyBackward(thrustSum/4 + gravity_force_copterloadsystem);
    response.controlOutput.motorCmd2 	= computeMotorPolyBackward(thrustSum/4 + gravity_force_copterloadsystem);
    response.controlOutput.motorCmd3 	= computeMotorPolyBackward(thrustSum/4 + gravity_force_copterloadsystem);
    response.controlOutput.motorCmd4 	= computeMotorPolyBackward(thrustSum/4 + gravity_force_copterloadsystem);
  
	// choosing the Crazyflie onBoard controller type.
	// it can either be Motor, Rate or Angle based 
	// response.controlOutput.onboardControllerType = MOTOR_MODE;
	response.controlOutput.onboardControllerType = RATE_MODE;
	// response.controlOutput.onboardControllerType = ANGLE_MODE;	
}

585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
void centralized_control_command( Controller::Request &request, Controller::Response &response)
{

	// Control output
	Eigen::Matrix<float,12,1> u = centralized_controller.K * estimator_state_prev;
	// Estimator update
	estimator_state_prev = centralized_controller.Phi * estimator_state_prev + centralized_controller.Gamma * y_to_controller;

	// extract motor command from u:
	float thrustSum 	= 0.0;
	float outRoll		= 0.0;
	float outPitch		= 0.0;
	float outYaw		= 0.0;

	//if ( request.ownCrazyflie.crazyflieName == request.otherCrazyflies[0].crazyflieName )
	if (my_agentID == 1 )
	{
		// Controller for crazyflie 01
603
		thrustSum 	= u(0,0);
604
605
606
607
608
609
610
611
		outRoll		= u(1,0);
		outPitch	= u(2,0);
		outYaw		= u(3,0);
	}
	//else if (request.ownCrazyflie.crazyflieName == request.otherCrazyflies[1].crazyflieName )
	else if (my_agentID == 2 )
	{
		// Controller for crazyflie 02
612
		thrustSum 	= u(4,0);
613
614
615
616
617
618
619
620
		outRoll		= u(5,0);
		outPitch	= u(6,0);
		outYaw		= u(7,0);
	}
	//else if (request.ownCrazyflie.crazyflieName == request.otherCrazyflies[2].crazyflieName )
	else if (my_agentID == 3 )
	{
		// Controller for crazyflie 03
621
		thrustSum 	= u(8,0);
622
623
624
625
626
		outRoll		= u(9,0);
		outPitch	= u(10,0);
		outYaw		= u(11,0);	
	}

627
	if (outer_loop_counter % 1 == 0)
628
	{
629
630
631
632
		ROS_INFO_STREAM("[STUDENT CONTROLLER] u thrust sum computed = " 		<< thrustSum );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] desired roll-rate computed = " 	<< outRoll );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] desired pitch-rate computed = " 	<< outPitch );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] desired yaw-rate computed = " 	<< outYaw );
633
	}
634
	if (outer_loop_counter % 1 == 0)
635
	{
636
		
637
638
639
640
641
642
643
		ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: roll estimate = " 			<< estimator_state_prev[0] );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: pitch estimate = " 			<< estimator_state_prev[1] );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: yaw estimate = " 				<< estimator_state_prev[2] );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: cable phi estimate = " 		<< estimator_state_prev[3] );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: cable theta estimate = " 		<< estimator_state_prev[4] );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: cable phi_dot estimate = " 	<< estimator_state_prev[5] );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: cable ttheta_dot estimate = " << estimator_state_prev[6] );
644
645

	}
646
647
648
649
650
651
	
	// PREPARE AND RETURN THE VARIABLE "response"
	
    response.controlOutput.roll 		= outRoll;
    response.controlOutput.pitch 		= outPitch;
    response.controlOutput.yaw 			= outYaw;
652
653
654
655
    response.controlOutput.motorCmd1 	= computeMotorPolyBackward(thrustSum/4 + gravity_force_copterloadsystem);
    response.controlOutput.motorCmd2 	= computeMotorPolyBackward(thrustSum/4 + gravity_force_copterloadsystem);
    response.controlOutput.motorCmd3 	= computeMotorPolyBackward(thrustSum/4 + gravity_force_copterloadsystem);
    response.controlOutput.motorCmd4 	= computeMotorPolyBackward(thrustSum/4 + gravity_force_copterloadsystem);
656
657
658
659
660
661
662
663
  
	// choosing the Crazyflie onBoard controller type.
	// it can either be Motor, Rate or Angle based 
	// response.controlOutput.onboardControllerType = MOTOR_MODE;
	response.controlOutput.onboardControllerType = RATE_MODE;
	// response.controlOutput.onboardControllerType = ANGLE_MODE;
}

664
void get_y_from_CFData( Controller::Request &request  )
665
666
{
	// get load orientation for transformation to world frame
667
	float roll 	= request.otherCrazyflies[3].roll;
668
669
	float pitch = request.otherCrazyflies[3].pitch;
	float yaw 	= request.otherCrazyflies[3].yaw;
670

671

672
	std::vector<float> P1_world;
673
	P1_world = transformation_frame_load_to_world( attachmentPoint.P1, roll , pitch , yaw );
674
	std::vector<float> P2_world;
675
	P2_world = transformation_frame_load_to_world( attachmentPoint.P2, roll , pitch , yaw );	
676
	std::vector<float> P3_world;
677
	P3_world = transformation_frame_load_to_world( attachmentPoint.P3, roll , pitch , yaw );
678

Tobias's avatar
Tobias committed
679
680
681
682
683
684
685
	std::vector<float> P1_world_eq;
	P1_world_eq = transformation_frame_load_to_world( attachmentPoint.P1, 0.0 , 0.0 , load_setpoint[3] );
	std::vector<float> P2_world_eq;
	P2_world_eq = transformation_frame_load_to_world( attachmentPoint.P2, 0.0 , 0.0 , load_setpoint[3] );	
	std::vector<float> P3_world_eq;
	P3_world_eq = transformation_frame_load_to_world( attachmentPoint.P3, 0.0 , 0.0 , load_setpoint[3] );

686
	// y1 = [CF01.x , CF01.y , CF01.z, CF01.roll, CF01.pitch, CF01.yaw , len_cable*phi1 , len_cable*theta1 ]
687
	yaw = request.otherCrazyflies[0].yaw;
Tobias's avatar
Tobias committed
688
689
	while(yaw > PI) {yaw -= 2 * PI;}
    while(yaw < -PI) {yaw += 2 * PI;}
690
691
692

    Eigen::Matrix<float,24,1> y_meas;

693
694
695
696
697
698
	y_meas(0,0) 	= request.otherCrazyflies[0].roll;
	y_meas(1,0) 	= request.otherCrazyflies[0].pitch;
	y_meas(2,0) 	= yaw;
	y_meas(3,0) 	= request.otherCrazyflies[0].x;
	y_meas(4,0) 	= request.otherCrazyflies[0].y;
	y_meas(5,0) 	= request.otherCrazyflies[0].z;
699
700
	y_meas(6,0) 	= request.otherCrazyflies[0].x - request.otherCrazyflies[3].x - P1_world[0];
	y_meas(7,0) 	= request.otherCrazyflies[0].y - request.otherCrazyflies[3].y - P1_world[1];
701
702

	// y2 = [CF02.x , CF02.y , CF02.z, CF02.roll, CF02.pitch, CF02.yaw , len_cable*phi2 , len_cable*theta2 ]
Tobias's avatar
Tobias committed
703
	yaw = request.otherCrazyflies[1].yaw;
Tobias's avatar
Tobias committed
704
705
	while(yaw > PI) {yaw -= 2 * PI;}
    while(yaw < -PI) {yaw += 2 * PI;}
706
707
708
709
710
711
	y_meas(8,0) 	= request.otherCrazyflies[1].roll;
	y_meas(9,0) 	= request.otherCrazyflies[1].pitch;
	y_meas(10,0) 	= yaw;
	y_meas(11,0) 	= request.otherCrazyflies[1].x;
	y_meas(12,0) 	= request.otherCrazyflies[1].y;
	y_meas(13,0) 	= request.otherCrazyflies[1].z;
712
713
	y_meas(14,0) 	= request.otherCrazyflies[1].x - request.otherCrazyflies[3].x - P2_world[0];
	y_meas(15,0) 	= request.otherCrazyflies[1].y - request.otherCrazyflies[3].y - P2_world[1];
714
715

	// y3 = [CF03.x , CF03.y , CF03.z, CF03.roll, CF03.pitch, CF03.yaw , len_cable*phi3 , len_cable*theta3 ]
Tobias's avatar
Tobias committed
716
	yaw = request.otherCrazyflies[2].yaw;
Tobias's avatar
Tobias committed
717
718
	while(yaw > PI) {yaw -= 2 * PI;}
    while(yaw < -PI) {yaw += 2 * PI;}	
719
720
721
722
723
724
	y_meas(16,0) 	= request.otherCrazyflies[2].roll;
	y_meas(17,0) 	= request.otherCrazyflies[2].pitch;
	y_meas(18,0) 	= yaw;
	y_meas(19,0) 	= request.otherCrazyflies[2].x;
	y_meas(20,0) 	= request.otherCrazyflies[2].y;
	y_meas(21,0) 	= request.otherCrazyflies[2].z;
725
726
727
	y_meas(22,0) 	= request.otherCrazyflies[2].x - request.otherCrazyflies[3].x - P3_world[0];
	y_meas(23,0) 	= request.otherCrazyflies[2].y - request.otherCrazyflies[3].y - P3_world[1];

728

729
730
	// compute measurement difference measured vs. set point
	// 								    y_meas - C * xi_equiliburium
731
732
733
	//Eigen::Matrix<float,24,1> y_eq = outputMatrix * augmented_state_eq;
	Eigen::Matrix<float,24,1> y_eq;
	y_eq.setZero();
Tobias's avatar
Tobias committed
734
735
736
737
738
739
740
741
742
	y_eq(3,0) 	= load_setpoint[0] + P1_world_eq[0]; 				// equilibrium x-pose copter 1
	y_eq(4,0) 	= load_setpoint[1] + P1_world_eq[1]; 				// equilibrium y-pose copter 1
	y_eq(5,0) 	= load_setpoint[2] + P1_world_eq[2] + len_cable; 	// equilibrium z-pose copter 1
	y_eq(11,0) 	= load_setpoint[0] + P2_world_eq[0]; 				// equilibrium x-pose copter 2
	y_eq(12,0) 	= load_setpoint[1] + P2_world_eq[1]; 				// equilibrium y-pose copter 2
	y_eq(13,0) 	= load_setpoint[2] + P2_world_eq[2] + len_cable;	// equilibrium z-pose copter 2
	y_eq(19,0) 	= load_setpoint[0] + P3_world_eq[0]; 				// equilibrium x-pose copter 3
	y_eq(20,0) 	= load_setpoint[1] + P3_world_eq[1]; 				// equilibrium y-pose copter 3
	y_eq(21,0) 	= load_setpoint[2] + P3_world_eq[2] + len_cable;	// equilibrium z-pose copter 3
743
744

	// compute y w.r.t. equilibrium position, passed to controller
745
746
747
	y_to_controller = y_meas - y_eq;

	// Debugging
748
	if (outer_loop_counter % 1 == 0)
749
	{
750
		ROS_INFO_STREAM("--------------------------" );
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
		ROS_INFO_STREAM("[STUDENT CONTROLLER] Agent :  " << my_agentID);
		ROS_INFO_STREAM("[STUDENT CONTROLLER] y_measured^T = " << y_meas.transpose() );				
		ROS_INFO_STREAM("[STUDENT CONTROLLER] y_eq^T = " << y_eq.transpose() );		
		ROS_INFO_STREAM("[STUDENT CONTROLLER] x-error for copter1 = " << y_to_controller(3,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] y-error for copter1 = " << y_to_controller(4,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] z-error for copter1 = " << y_to_controller(5,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] x-error for copter2 = " << y_to_controller(11,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] y-error for copter2 = " << y_to_controller(12,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] z-error for copter2 = " << y_to_controller(13,0) );		
		ROS_INFO_STREAM("[STUDENT CONTROLLER] x-error for copter3 = " << y_to_controller(19,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] y-error for copter3 = " << y_to_controller(20,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] z-error for copter3 = " << y_to_controller(21,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] y_to_controller^T = " << y_to_controller.transpose() );		
	}	


767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
}

void get_y_from_CFData_withLoad( Controller::Request &request  )
{

	std::vector<float> P1_world_eq;
	P1_world_eq = transformation_frame_load_to_world( attachmentPoint.P1, 0.0 , 0.0 , load_setpoint[3] );
	std::vector<float> P2_world_eq;
	P2_world_eq = transformation_frame_load_to_world( attachmentPoint.P2, 0.0 , 0.0 , load_setpoint[3] );	
	std::vector<float> P3_world_eq;
	P3_world_eq = transformation_frame_load_to_world( attachmentPoint.P3, 0.0 , 0.0 , load_setpoint[3] );


	// y1 = [CF01.x , CF01.y , CF01.z, CF01.roll, CF01.pitch, CF01.yaw , len_cable*phi1 , len_cable*theta1 ]
	float yaw = request.otherCrazyflies[0].yaw;
	while(yaw > PI) {yaw -= 2 * PI;}
    while(yaw < -PI) {yaw += 2 * PI;}

    Eigen::Matrix<float,24,1> y_meas;

	y_meas(0,0) 	= request.otherCrazyflies[0].roll;
	y_meas(1,0) 	= request.otherCrazyflies[0].pitch;
	y_meas(2,0) 	= yaw;
	y_meas(3,0) 	= request.otherCrazyflies[0].x;
	y_meas(4,0) 	= request.otherCrazyflies[0].y;
	y_meas(5,0) 	= request.otherCrazyflies[0].z;

	// y2 = [CF02.x , CF02.y , CF02.z, CF02.roll, CF02.pitch, CF02.yaw , len_cable*phi2 , len_cable*theta2 ]
	yaw = request.otherCrazyflies[1].yaw;
	while(yaw > PI) {yaw -= 2 * PI;}
    while(yaw < -PI) {yaw += 2 * PI;}
	y_meas(6,0) 	= request.otherCrazyflies[1].roll;
	y_meas(7,0) 	= request.otherCrazyflies[1].pitch;
	y_meas(8,0) 	= yaw;
	y_meas(9,0) 	= request.otherCrazyflies[1].x;
	y_meas(10,0) 	= request.otherCrazyflies[1].y;
	y_meas(11,0) 	= request.otherCrazyflies[1].z;


	// y3 = [CF03.x , CF03.y , CF03.z, CF03.roll, CF03.pitch, CF03.yaw , len_cable*phi3 , len_cable*theta3 ]
	yaw = request.otherCrazyflies[2].yaw;
	while(yaw > PI) {yaw -= 2 * PI;}
    while(yaw < -PI) {yaw += 2 * PI;}	
	y_meas(12,0) 	= request.otherCrazyflies[2].roll;
	y_meas(13,0) 	= request.otherCrazyflies[2].pitch;
	y_meas(14,0) 	= yaw;
	y_meas(15,0) 	= request.otherCrazyflies[2].x;
	y_meas(16,0) 	= request.otherCrazyflies[2].y;
	y_meas(17,0) 	= request.otherCrazyflies[2].z;

817
	// load measurement
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
	y_meas(18,0)	= request.otherCrazyflies[3].x;
	y_meas(19,0)	= request.otherCrazyflies[3].y;
	y_meas(20,0)	= request.otherCrazyflies[3].z;
	y_meas(21,0) 	= request.otherCrazyflies[3].roll;
	y_meas(22,0) 	= request.otherCrazyflies[3].pitch;
	yaw = request.otherCrazyflies[3].yaw;
	while(yaw > PI) {yaw -= 2 * PI;}
    while(yaw < -PI) {yaw += 2 * PI;}		
	y_meas(23,0) 	= yaw;


	// compute measurement difference measured vs. set point
	// 								    y_meas - C * xi_equiliburium
	//Eigen::Matrix<float,24,1> y_eq = outputMatrix * augmented_state_eq;
	Eigen::Matrix<float,24,1> y_eq;
	y_eq.setZero();
	y_eq(3,0) 	= load_setpoint[0] + P1_world_eq[0]; 				// equilibrium x-pose copter 1
	y_eq(4,0) 	= load_setpoint[1] + P1_world_eq[1]; 				// equilibrium y-pose copter 1
	y_eq(5,0) 	= load_setpoint[2] + P1_world_eq[2] + len_cable; 	// equilibrium z-pose copter 1
	y_eq(9,0) 	= load_setpoint[0] + P2_world_eq[0]; 				// equilibrium x-pose copter 2
	y_eq(10,0) 	= load_setpoint[1] + P2_world_eq[1]; 				// equilibrium y-pose copter 2
	y_eq(11,0) 	= load_setpoint[2] + P2_world_eq[2] + len_cable;	// equilibrium z-pose copter 2
	y_eq(15,0) 	= load_setpoint[0] + P3_world_eq[0]; 				// equilibrium x-pose copter 3
	y_eq(16,0) 	= load_setpoint[1] + P3_world_eq[1]; 				// equilibrium y-pose copter 3
	y_eq(17,0) 	= load_setpoint[2] + P3_world_eq[2] + len_cable;	// equilibrium z-pose copter 3
	y_eq(18,0)  = load_setpoint[0];									// equilibrium x-pose load
	y_eq(19,0)  = load_setpoint[1];									// equilibrium y-pose load
	y_eq(20,0)  = load_setpoint[2];									// equilibrium z-pose load
	y_eq(23,0)  = load_setpoint[3];									// equilibrium yaw load
	// compute y w.r.t. equilibrium position, passed to controller
	y_to_controller = y_meas - y_eq;

	// Debugging
851
	if (outer_loop_counter % 1 == 0)
852
	{
853
		ROS_INFO_STREAM("--------------------------" );
854
		ROS_INFO_STREAM("[STUDENT CONTROLLER] Agent :  " << my_agentID);
855
856
857
858
859
		ROS_INFO_STREAM("[STUDENT CONTROLLER] y_measured^T = " 			<< y_meas.transpose() );				
		ROS_INFO_STREAM("[STUDENT CONTROLLER] y_eq^T = " 				<< y_eq.transpose() );		
		ROS_INFO_STREAM("[STUDENT CONTROLLER] roll-error for copter1 = " << y_to_controller(0,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] pitch-error for copter1 = " << y_to_controller(1,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] yaw-error for copter1 = " << y_to_controller(2,0) );
860
861
862
		ROS_INFO_STREAM("[STUDENT CONTROLLER] x-error for copter1 = " << y_to_controller(3,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] y-error for copter1 = " << y_to_controller(4,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] z-error for copter1 = " << y_to_controller(5,0) );
863
864
865
		ROS_INFO_STREAM("[STUDENT CONTROLLER] roll-error for copter2 = " << y_to_controller(6,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] pitch-error for copter2 = " << y_to_controller(7,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] yaw-error for copter2 = " << y_to_controller(8,0) );
866
867
		ROS_INFO_STREAM("[STUDENT CONTROLLER] x-error for copter2 = " << y_to_controller(9,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] y-error for copter2 = " << y_to_controller(10,0) );
868
869
870
871
		ROS_INFO_STREAM("[STUDENT CONTROLLER] z-error for copter2 = " << y_to_controller(11,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] roll-error for copter3 = " << y_to_controller(12,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] pitch-error for copter3 = " << y_to_controller(13,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] yaw-error for copter3 = " << y_to_controller(14,0) );		
872
873
874
875
876
877
		ROS_INFO_STREAM("[STUDENT CONTROLLER] x-error for copter3 = " << y_to_controller(15,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] y-error for copter3 = " << y_to_controller(16,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] z-error for copter3 = " << y_to_controller(17,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] x-error for load = " 		<< y_to_controller(18,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] y-error for load = " 		<< y_to_controller(19,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] z-error for load = " 		<< y_to_controller(20,0) );
878
879
880
		ROS_INFO_STREAM("[STUDENT CONTROLLER] roll-error for load = " << y_to_controller(21,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] pitch-error for load = " << y_to_controller(22,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] yaw-error for load = " << y_to_controller(23,0) );	
881
882
883
884
		ROS_INFO_STREAM("[STUDENT CONTROLLER] y_to_controller^T = " << y_to_controller.transpose() );		
	}	


885
886
}

887
std::vector<float> transformation_frame_load_to_world(std::vector<float> P_load , float roll, float pitch, float yaw )
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
{
	// get load orientation for transformation to world frame
	float cos_roll_load 	= cos(roll);
	float sin_roll_load 	= sin(roll);
	float cos_pitch_load 	= cos(pitch);
	float sin_pitch_load 	= sin(pitch);
	float cos_yaw_load 		= cos(yaw);
	float sin_yaw_load 		= sin(yaw);
	// xP
	float P_x_world = 0.0;
	P_x_world += cos_yaw_load*cos_pitch_load * P_load[0];
	P_x_world += ( -sin_yaw_load*cos_roll_load + cos_yaw_load*sin_pitch_load*sin_roll_load ) * P_load[1];
	P_x_world += ( sin_yaw_load*sin_roll_load + cos_yaw_load*sin_pitch_load*cos_roll_load ) * P_load[2];
	// yP
	float P_y_world = 0.0;
	P_y_world += sin_yaw_load*cos_pitch_load*P_load[0];
	P_y_world += ( cos_yaw_load*cos_roll_load + sin_yaw_load*sin_pitch_load*sin_roll_load ) * P_load[1];
905
	P_y_world += ( -cos_yaw_load*sin_roll_load + sin_yaw_load*sin_pitch_load*cos_roll_load ) * P_load[2];
906
907
908
909
910
911
	// zP
	float P_z_world = 0.0;
	P_z_world += -sin_pitch_load * P_load[0];
	P_z_world +=  cos_pitch_load * sin_roll_load * P_load[1];
	P_z_world +=  cos_pitch_load * cos_roll_load * P_load[2];	

912
	std::vector<float> P_world = {0.0 , 0.0 , 0.0 };
913
914
915
	P_world[0] = P_x_world;
	P_world[1] = P_y_world;
	P_world[2] = P_z_world;
916
917

	return P_world;
918
}
919
920
921


Eigen::MatrixXf readMatrix(std::string filename)
922
{
923
    int cols = 0, rows = 0;
Tobias's avatar
Tobias committed
924
    float buff[10000];
925
926
927
928
929
930
931
932
933
934

    // Read numbers from file into buffer.
    std::ifstream infile;
    infile.open(filename);
    while (! infile.eof())
        {
        std::string line;
        std::getline(infile, line);

        int temp_cols = 0;
935
        float temp_bufferVal = 0;
936
937
        std::stringstream stream(line);
        while(! stream.eof())
938
            stream >> buff[cols*rows+temp_cols++] ;
939
940
941
942
943
944
945
946
947
948
949
950

        if (temp_cols == 0)
            continue;

        if (cols == 0)
            cols = temp_cols;

        rows++;
        }

    infile.close();

951
    //rows--;
952
953
954
955
956
957
958
959

    // Populate matrix with numbers.
    Eigen::MatrixXf result(rows,cols);
    for (int i = 0; i < rows; i++)
        for (int j = 0; j < cols; j++)
            result(i,j) = buff[ cols*i+j ];

    return result;
960
 }
961

962
963
void turn_motors_off( Controller::Response &response ) 
{
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
	// Put the computed roll rate into the "response" variable
	response.controlOutput.roll = 0;
	response.controlOutput.pitch = 0;
	response.controlOutput.yaw = 0;
	response.controlOutput.motorCmd1 = 0;
	response.controlOutput.motorCmd2 = 0;
	response.controlOutput.motorCmd3 = 0;
	response.controlOutput.motorCmd4 = 0;

	
	// PREPARE AND RETURN THE VARIABLE "response"

	/*choosing the Crazyflie onBoard controller type.
	it can either be Motor, Rate or Angle based */
	// response.controlOutput.onboardControllerType = MOTOR_MODE;
	response.controlOutput.onboardControllerType = RATE_MODE;
	// response.controlOutput.onboardControllerType = ANGLE_MODE;
981
}
982

983
984
void agent_lqr_command( Controller::Request &request , Controller::Response &response ) 
{
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
	float yaw_measured = request.ownCrazyflie.yaw;

    //move coordinate system to make setpoint origin
    request.ownCrazyflie.x -= agent_setpoint[0];
    request.ownCrazyflie.y -= agent_setpoint[1];
    request.ownCrazyflie.z -= agent_setpoint[2];
    float yaw = request.ownCrazyflie.yaw - agent_setpoint[3];

    while(yaw > PI) {yaw -= 2 * PI;}
    while(yaw < -PI) {yaw += 2 * PI;}
    request.ownCrazyflie.yaw = yaw;

    float est[9]; //px, py, pz, vx, vy, vz, roll, pitch, yaw
    estimateState(request, est);
    float state[9]; //px, py, pz, vx, vy, vz, roll, pitch, yaw
    convertIntoBodyFrame(est, state, yaw_measured);
For faster browsing, not all history is shown. View entire blame