MpcControllerService.cpp 60.8 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
38
39
40
41
42
43
44
45
46



//    ----------------------------------------------------------------------------------
//    III  N   N   CCCC  L      U   U  DDDD   EEEEE   SSSS
//     I   NN  N  C      L      U   U  D   D  E      S
//     I   N N N  C      L      U   U  D   D  EEE     SSS
//     I   N  NN  C      L      U   U  D   D  E          S
//    III  N   N   CCCC  LLLLL   UUU   DDDD   EEEEE  SSSS
//    ----------------------------------------------------------------------------------

// These various headers need to be included so that this controller service can be
// connected with the D-FaLL system.

47
48
49
50
//some useful libraries
#include <math.h>
#include <stdlib.h>
#include "ros/ros.h"
roangel's avatar
roangel committed
51
#include <ros/package.h>
52

53
#include <ctime>
54
55
#include <atomic>
#include <mutex>
56

57
58
59
60
61
//the generated structs from the msg-files have to be included
#include "d_fall_pps/ViconData.h"
#include "d_fall_pps/Setpoint.h"
#include "d_fall_pps/ControlCommand.h"
#include "d_fall_pps/Controller.h"
roangel's avatar
roangel committed
62
#include "d_fall_pps/DebugMsg.h"
63
#include "d_fall_pps/CustomControllerYAML.h"
64

65
66
67
// Include the Parameter Service shared definitions
#include "nodes/ParameterServiceDefinitions.h"

68
69
#include <std_msgs/Int32.h>

70
#include "MPC_functions.h"
71

72
73
#include <boost/thread/thread.hpp>

74
75
using namespace std;

76
77
78
79
80
81
82
83
84
85
86

//    ----------------------------------------------------------------------------------
//    DDDD   EEEEE  FFFFF  III  N   N  EEEEE   SSSS
//    D   D  E      F       I   NN  N  E      S
//    D   D  EEE    FFF     I   N N N  EEE     SSS
//    D   D  E      F       I   N  NN  E          S
//    DDDD   EEEEE  F      III  N   N  EEEEE  SSSS
//    ----------------------------------------------------------------------------------

// These constants are defined to make the code more readable and adaptable.

87
// Universal constants
88
#define PI 3.1415926535
roangel's avatar
roangel committed
89

90
// These constants define the modes that can be used for controller the Crazyflie 2.0,
91
92
93
94
95
96
97
98
99
100
101
102
103
// the constants defined here need to be in agreement with those defined in the
// firmware running on the Crazyflie 2.0.
// The following is a short description about each mode:
// MOTOR_MODE    In this mode the Crazyflie will apply the requested 16-bit per motor
//               command directly to each of the motors
// RATE_MODE     In this mode the Crazyflie will apply the requested 16-bit per motor
//               command directly to each of the motors, and additionally request the
//               body frame roll, pitch, and yaw angular rates from the PID rate
//               controllers implemented in the Crazyflie 2.0 firmware.
// ANGE_MODE     In this mode the Crazyflie will apply the requested 16-bit per motor
//               command directly to each of the motors, and additionally request the
//               body frame roll, pitch, and yaw angles from the PID attitude
//               controllers implemented in the Crazyflie 2.0 firmware.
104
105
106
107
108
109
110
111
112

#define CF_COMMAND_TYPE_MOTOR   6
#define CF_COMMAND_TYPE_RATE    7
#define CF_COMMAND_TYPE_ANGLE   8

#define ESTIMATOR_METHOD_FINITE_DIFFERENCE          1
#define ESTIMATOR_METHOD_POINT_MASS_PER_DIMENSION   2   // (DEFAULT)
#define ESTIMATOR_METHOD_QUADROTOR_MODEL_BASED      3

113
int estimator_method = ESTIMATOR_METHOD_FINITE_DIFFERENCE;
114
115
116
117
118

// Boolean indiciating whether the debugging ROS_INFO_STREAM should be displayed or not
bool shouldDisplayDebugInfo = false;

float estimator_frequency = 200;
119

120
121
122
123
124
125
126
127
128
129
130
131
// These constants define the controller used for computing the response in the
// "calculateControlOutput" function
// The following is a short description about each mode:
// LQR_RATE_MODE      LQR controller based on the state vector:
//                    [position,velocity,angles]
//
// LQR_ANGLE_MODE     LQR controller based on the state vector:
//                    [position,velocity]
//
#define LQR_RATE_MODE   1   // (DEFAULT)
#define LQR_ANGLE_MODE  2

132
// Namespacing the package
133
134
using namespace d_fall_pps;

135

136
137
138
139
140
141
142
143
//    ----------------------------------------------------------------------------------
//    V   V    A    RRRR   III    A    BBBB   L      EEEEE   SSSS
//    V   V   A A   R   R   I    A A   B   B  L      E      S
//    V   V  A   A  RRRR    I   A   A  BBBB   L      EEE     SSS
//     V V   AAAAA  R  R    I   AAAAA  B   B  L      E          S
//      V    A   A  R   R  III  A   A  BBBB   LLLLL  EEEEE  SSSS
//    ----------------------------------------------------------------------------------

144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161

// THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
// > For the (x,y,z) position
std::vector<float> PMKF_Ahat_row1_for_positions (2,0.0);
std::vector<float> PMKF_Ahat_row2_for_positions (2,0.0);
std::vector<float> PMKF_Kinf_for_positions      (2,0.0);
// > For the (roll,pitch,yaw) angles
std::vector<float> PMKF_Ahat_row1_for_angles    (2,0.0);
std::vector<float> PMKF_Ahat_row2_for_angles    (2,0.0);
std::vector<float> PMKF_Kinf_for_angles         (2,0.0);

std::vector<float> gainMatrixRollRate_Nested            (3,0.0);
std::vector<float> gainMatrixPitchRate_Nested           (3,0.0);
std::vector<float> gainMatrixYawRate_Nested             (3,0.0);

float current_xzy_rpy_measurement[6];
float previous_xzy_rpy_measurement[6];

162
float stateInertialEstimate[12] = {0}; //this variable will contain the estimation of the current state
163

164

165
166
// Variables for controller
float cf_mass;                       // Crazyflie mass in grams
167
std::vector<float> motorPoly(3);     // Coefficients of the 16-bit command to thrust conversion
168
169
170
float control_frequency;             // Frequency at which the controller is running
float gravity_force;                 // The weight of the Crazyflie in Newtons, i.e., mg

171
float previous_stateErrorInertial[9];     // The location error of the Crazyflie at the "previous" time step
172

173
std::vector<float>  setpoint{0.0,0.0,0.4,0.0};     // The setpoints for (x,y,z) position and yaw angle, in that order
174

roangel's avatar
roangel committed
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
// Variables for MPC:

Eigen::MatrixXd P(N_x,N_x);
Eigen::MatrixXd Q(N_x,N_x);
Eigen::MatrixXd R(N_u,N_u);

Eigen::VectorXd X_ref;
Eigen::VectorXd U_ref;

std::vector<MatrixAtype> A_tray;
std::vector<MatrixBtype> B_tray;
std::vector<VectorXtype> g_tray;

std::vector<VectorXtype> X_tray;
std::vector<VectorUtype> U_tray;

191
192
193
194
Eigen::VectorXd U_0;



roangel's avatar
roangel committed
195
196
197
198
params_t params;

VectorXtype x0;

199
200
std::mutex MPC_mutex;

201
int N = 8;
roangel's avatar
roangel committed
202

203

204
// ROS Publisher for debugging variables
roangel's avatar
roangel committed
205
206
ros::Publisher debugPublisher;

207

208
// Variable for the namespaces for the paramter services
209
// > For the paramter service of this agent
210
std::string namespace_to_own_agent_parameter_service;
211
// > For the parameter service of the coordinator
212
std::string namespace_to_coordinator_parameter_service;
213

214
215
216
// The ID of this agent, i.e., the ID of this compute
int my_agentID = 0;

217

218
// RELEVANT NOTES ABOUT THE VARIABLES DECLARE HERE:
219
// The "CrazyflieData" type used for the "request" variable is a
220
221
222
223
224
225
226
227
228
229
230
// structure as defined in the file "CrazyflieData.msg" which has the following
// properties:
//     string crazyflieName              The name given to the Crazyflie in the Vicon software
//     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
231

232
233
234
235




236
237
238
239
240
241
242
243
244
245
246
247
248
//    ----------------------------------------------------------------------------------
//    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
//
//    PPPP   RRRR    OOO   TTTTT   OOO   TTTTT  Y   Y  PPPP   EEEEE   SSSS
//    P   P  R   R  O   O    T    O   O    T     Y Y   P   P  E      S
//    PPPP   RRRR   O   O    T    O   O    T      Y    PPPP   EEE     SSS
//    P      R  R   O   O    T    O   O    T      Y    P      E          S
//    P      R   R   OOO     T     OOO     T      Y    P      EEEEE  SSSS
//    ----------------------------------------------------------------------------------
249

250
251
252
253
254
// These function prototypes are not strictly required for this code to complile, but
// adding the function prototypes here means the the functions can be written below in
// any order. If the function prototypes are not included then the function need to
// written below in an order that ensure each function is implemented before it is
// called from another function, hence why the "main" function is at the bottom.
255

256
257
258
// CONTROLLER COMPUTATIONS
bool calculateControlOutput(Controller::Request &request, Controller::Response &response);

259
260
// CONVERSION FROM THRUST IN NEWTONS TO 16-BIT COMMAND
float computeMotorPolyBackward(float thrust);
261

262
263
// SETPOINT CHANGE CALLBACK
void setpointCallback(const Setpoint& newSetpoint);
264

265
// LOAD PARAMETERS
266
267
268
float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name);
void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length);
int getParameterInt(ros::NodeHandle& nodeHandle, std::string name);
269
270
271
272
void getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length);
int getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val);
bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name);

273
274
275
276
void yamlReadyForFetchCallback(const std_msgs::Int32& msg);
void fetchYamlParameters(ros::NodeHandle& nodeHandle);
void processFetchedParameters();
//void customYAMLasMessageCallback(const CustomControllerYAML& newCustomControllerParameters);
277

278
void angleControlledCrazyflie(float stateInertial[12], float raw_input[4], Controller::Response &response);
279
280
281
void perform_estimator_update_state_interial(Controller::Request &request, float (&stateInertialEstimate)[12]);
void performEstimatorUpdate_forStateInterial_viaFiniteDifference(float (&stateInertialEstimate)[12]);
void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter(float (&stateInertialEstimate)[12]);
282

283
void initialize_MPC_variables(int N, VectorXtype x0);
284
285


286
287
288
289
290
291
292
293
294
295
296
297
298
//    ----------------------------------------------------------------------------------
//    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
299

300

roangel's avatar
roangel committed
301
302


roangel's avatar
roangel committed
303

304
//    ------------------------------------------------------------------------------
305
//     OOO   U   U  TTTTT  EEEEE  RRRR
306
307
308
309
310
311
312
313
314
315
316
//    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
317

318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
// This function is the callback that is linked to the "CustomController" service that
// 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)
//       \____/                       \____/
//
403
//
404
405
//
// This function WILL NEED TO BE edited for successful completion of the PPS exercise
406

407
double saturateThrust(double input)
beuchatp's avatar
beuchatp committed
408
{
409
410
411
412
413
414
415
416
417
418
419
420
421
422
    float out = input;
    if(input > 60000)
        out = 60000;
    if(input < 0)
        out = 0;
    return out;
}

void angleControlledCrazyflie(float stateInertial[12], float raw_input[4], Controller::Response &response)
{
    float roll_sp = raw_input[0];
    float pitch_sp = raw_input[1];
    float yaw_sp = raw_input[2];
    float ft_sp = raw_input[3];
beuchatp's avatar
beuchatp committed
423

424
425
426
427
    // initialize variables that will contain w_x, w_y and w_z
	float w_x_sp  = 0;
	float w_y_sp = 0;
	float w_z_sp   = 0;
beuchatp's avatar
beuchatp committed
428
429

	// Create the angle error to use for the inner controller
430
431
432
433
	float angle_error[3] = {
		stateInertial[6] - roll_sp,
		stateInertial[7] - pitch_sp,
        stateInertial[8] - yaw_sp
beuchatp's avatar
beuchatp committed
434
	};
435
436
437
438
439

    // ROS_INFO_STREAM("stateInertial[6]" << stateInertial[6]);
    // ROS_INFO_STREAM("stateInertial[7]" << stateInertial[7]);
    // ROS_INFO_STREAM("stateInertial[8]" << stateInertial[8]);

beuchatp's avatar
beuchatp committed
440
	// Perform the "-Kx" LQR computation for the rates and thrust:
441
	for(int i = 0; i < 3; ++i)
442
	{
beuchatp's avatar
beuchatp committed
443
		// BODY FRAME Y CONTROLLER
444
		w_x_sp  -= gainMatrixRollRate_Nested[i]  * angle_error[i];
beuchatp's avatar
beuchatp committed
445
		// BODY FRAME X CONTROLLER
446
		w_y_sp -= gainMatrixPitchRate_Nested[i] * angle_error[i];
beuchatp's avatar
beuchatp committed
447
		// BODY FRAME Z CONTROLLER
448
		w_z_sp -= gainMatrixYawRate_Nested[i]   * angle_error[i];
449
450
	}

451

beuchatp's avatar
beuchatp committed
452
	// UPDATE THE "RETURN" THE VARIABLE NAMED "response"
453

beuchatp's avatar
beuchatp committed
454
455
	// Put the computed rates and thrust into the "response" variable
	// > For roll, pitch, and yaw:
456
457
458
	response.controlOutput.roll  = w_x_sp;
	response.controlOutput.pitch = w_y_sp;
	response.controlOutput.yaw   = w_z_sp;
beuchatp's avatar
beuchatp committed
459
460
461
462
463
464
	// > For the thrust adjustment we must add the feed-forward thrust to counter-act gravity.
	// > NOTE: remember that the thrust is commanded per motor, so you sohuld
	//         consider whether the "thrustAdjustment" computed by your
	//         controller needed to be divided by 4 or not.
	// > NOTE: the "gravity_force_quarter" value was already divided by 4 when
	//         it was loaded/processes from the .yaml file.
465
466
467
468
	response.controlOutput.motorCmd1 = saturateThrust(computeMotorPolyBackward(ft_sp/4.0));
	response.controlOutput.motorCmd2 = saturateThrust(computeMotorPolyBackward(ft_sp/4.0));
	response.controlOutput.motorCmd3 = saturateThrust(computeMotorPolyBackward(ft_sp/4.0));
	response.controlOutput.motorCmd4 = saturateThrust(computeMotorPolyBackward(ft_sp/4.0));
beuchatp's avatar
beuchatp committed
469
470
471
472
473
474
475
476

	// Specify that this controller is a rate controller
	// response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
	response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
	// response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;

	// Display some details (if requested)
	if (shouldDisplayDebugInfo)
477
	{
478
479
480
481
		ROS_INFO_STREAM("thrust    =" << ft_sp);
		ROS_INFO_STREAM("rollrate  =" << w_x_sp);
		ROS_INFO_STREAM("pitchrate =" << w_y_sp);
		ROS_INFO_STREAM("yawrate   =" << w_z_sp);
482
	}
beuchatp's avatar
beuchatp committed
483
}
484

485
int outer_loop_counter = 0;
486

487
488
489
490
491
492
float raw_input[4];

clock_t begin_time = 0;
clock_t end_time = 0;

ros::Time begin_t;
493
DebugMsg debugMsg;
494

495
496
497
498
499
500
501
502
503
// float stateInertialEstimate[12];

void do_MPC(int* publish_rate)
{
    ros::Rate loop_rate(*publish_rate);

    while(ros::ok())
    {
        // Put the MPC loop here:
504
505
506
507
        // load x0 protected
        MPC_mutex.lock();
        VectorXtype x0_local = x0;
        MPC_mutex.unlock();
508
509
510
511
512
513
514
515
516
517
518
519

        // Do the MPC step:
        U_0 = mympc_varying_another(A_tray, B_tray, g_tray, Q, R, P, N, x0_local, X_ref, U_ref, 0, 0.1391*4, 0.7887);
        VectorUtype u = U_0.tail(4); // this is the input to apply next

        // prediction of next N states:
        VectorXtype x_approx_next;
        std::vector<VectorXtype> x_approx_temp;
        x_approx_next << x0_local;

        for(int j = 0; j < N; j++)
        {
520
            euler_method_forward(F_crazyflie_6_states, 0, params.Ts/4, params.Ts, x_approx_next, U_0.segment(N_u*j,N_u), params, x_approx_temp);
521
            x_approx_next = x_approx_temp.back();
522
523

            MPC_mutex.lock();
524
525
526
527
528
529
530
            X_tray[j] = x_approx_next;

            // Now update U_tray using the same loop
            if(j < N-1)
                U_tray[j] = U_0.segment(N_u*(j+1), N_u);
            else
                U_tray[j] = U_0.segment(N_u*j, N_u); // last element of the new U_tray is assumed to be the same as second to last
531
            MPC_mutex.unlock();
532
533
534
535
        }
        // X_tray and U_tray have been completely updated with the new states coming from the new sequence of inputs

        // Relinearize around new trayectory:
536
        MPC_mutex.lock();
537
        get_matrices_linearization_affine(X_tray, U_tray, params, A_tray, B_tray, g_tray);
538
        MPC_mutex.unlock();
539
540
541

        // update the current input to the middle loop

542
543
544
545
546
547
        MPC_mutex.lock();
        raw_input[0] = (float)u(0);
        raw_input[1] = (float)u(1);
        raw_input[2] = (float)u(2);
        raw_input[3] = (float)u(3);
        MPC_mutex.unlock();
548
549
550
551
552
553
554
555
556
557
558

        // end MPC loop

        loop_rate.sleep();

        double elapsed_secs = ros::Time::now().toSec() - begin_t.toSec();
        std::cout << "elapsed secs: " << elapsed_secs << std::endl;
        begin_t = ros::Time::now();
    }
}

559

beuchatp's avatar
beuchatp committed
560
561
bool calculateControlOutput(Controller::Request &request, Controller::Response &response)
{
562

beuchatp's avatar
beuchatp committed
563
564
565
566
	// This is the "start" of the outer loop controller, add all your control
	// computation here, or you may find it convienient to create functions
	// to keep you code cleaner
    // Define a local array to fill in with the state error
567

568
	perform_estimator_update_state_interial(request, stateInertialEstimate);
569

570
571
    // read new x0

572
    MPC_mutex.lock();
573
574
575
576
577
578
    x0 << stateInertialEstimate[0] - setpoint[0],
        stateInertialEstimate[1] - setpoint[1],
        stateInertialEstimate[2] - setpoint[2],
        stateInertialEstimate[3],
        stateInertialEstimate[4],
        stateInertialEstimate[5];
579
    MPC_mutex.unlock();
580

581
    // read raw_input from shared_raw_input atomic variable
582
    float local_input[4];
583

584
585
586
587
588
589
    MPC_mutex.lock();
    local_input[0] = raw_input[0];
    local_input[1] = raw_input[1];
    local_input[2] = raw_input[2];
    local_input[3] = raw_input[3];
    MPC_mutex.unlock();
590

591
    // create a function that takes as input angle references, like a crazyflie entity with input angles
592
    // This function has to run at 200Hz
593
    angleControlledCrazyflie(stateInertialEstimate, local_input, response);
594
595


596
597
    // Return "true" to indicate that the control computation was performed successfully
    return true;
598
599
600
}


beuchatp's avatar
beuchatp committed
601
602
603
604
605
606
607
//    ------------------------------------------------------------------------------
//    EEEEE   SSSS  TTTTT  III  M   M    A    TTTTT  III   OOO   N   N
//    E      S        T     I   MM MM   A A     T     I   O   O  NN  N
//    EEE     SSS     T     I   M M M  A   A    T     I   O   O  N N N
//    E          S    T     I   M   M  AAAAA    T     I   O   O  N  NN
//    EEEEE  SSSS     T    III  M   M  A   A    T    III   OOO   N   N
//    ------------------------------------------------------------------------------
608
609
610
611



void perform_estimator_update_state_interial(Controller::Request &request, float (&stateInertialEstimate)[12])
beuchatp's avatar
beuchatp committed
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
{

	// PUT THE CURRENT MEASURED DATA INTO THE CLASS VARIABLE
	// > for (x,y,z) position
	current_xzy_rpy_measurement[0] = request.ownCrazyflie.x;
	current_xzy_rpy_measurement[1] = request.ownCrazyflie.y;
	current_xzy_rpy_measurement[2] = request.ownCrazyflie.z;
	// > for (roll,pitch,yaw) angles
	current_xzy_rpy_measurement[3] = request.ownCrazyflie.roll;
	current_xzy_rpy_measurement[4] = request.ownCrazyflie.pitch;
	current_xzy_rpy_measurement[5] = request.ownCrazyflie.yaw;


	// FILLE IN THE STATE INERTIAL ESTIMATE TO BE USED FOR CONTROL
	switch (estimator_method)
	{
		// Estimator based on finte differences
		case ESTIMATOR_METHOD_FINITE_DIFFERENCE:
		{
631
632
            // RUN THE FINITE DIFFERENCE ESTIMATOR and fill stateEstimate with result
            performEstimatorUpdate_forStateInterial_viaFiniteDifference(stateInertialEstimate);
beuchatp's avatar
beuchatp committed
633
634
635
636
637
			break;
		}
		// Estimator based on Point Mass Kalman Filter
		case ESTIMATOR_METHOD_POINT_MASS_PER_DIMENSION:
		{
638
639
            // RUN THE POINT MASS KALMAN FILTER ESTIMATOR and fill stateEstimate with result
            performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter(stateInertialEstimate);
beuchatp's avatar
beuchatp committed
640
641
642
643
644
645
646
			break;
		}
		// Handle the exception
		default:
		{
			// Display that the "estimator_method" was not recognised
			ROS_INFO_STREAM("[DEMO CONTROLLER] ERROR: in the 'calculateControlOutput' function of the 'DemoControllerService': the 'estimator_method' is not recognised.");
647
648
			// Transfer the finite difference estimate by default and fill stateEstimate with result
            performEstimatorUpdate_forStateInterial_viaFiniteDifference(stateInertialEstimate);
beuchatp's avatar
beuchatp committed
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
			break;
		}
	}


	// NOW THAT THE ESTIMATORS HAVE ALL BEEN RUN, PUT THE CURRENT
	// MEASURED DATA INTO THE CLASS VARIABLE FOR THE PREVIOUS
	// > for (x,y,z) position
	previous_xzy_rpy_measurement[0] = current_xzy_rpy_measurement[0];
	previous_xzy_rpy_measurement[1] = current_xzy_rpy_measurement[1];
	previous_xzy_rpy_measurement[2] = current_xzy_rpy_measurement[2];
	// > for (roll,pitch,yaw) angles
	previous_xzy_rpy_measurement[3] = current_xzy_rpy_measurement[3];
	previous_xzy_rpy_measurement[4] = current_xzy_rpy_measurement[4];
	previous_xzy_rpy_measurement[5] = current_xzy_rpy_measurement[5];
}

666
void performEstimatorUpdate_forStateInterial_viaFiniteDifference(float (&stateInertialEstimate)[12])
beuchatp's avatar
beuchatp committed
667
668
669
{
	// PUT IN THE CURRENT MEASUREMENT DIRECTLY
	// > for (x,y,z) position
670
671
672
	stateInertialEstimate[0]  = current_xzy_rpy_measurement[0];
	stateInertialEstimate[1]  = current_xzy_rpy_measurement[1];
	stateInertialEstimate[2]  = current_xzy_rpy_measurement[2];
beuchatp's avatar
beuchatp committed
673
	// > for (roll,pitch,yaw) angles
674
675
676
	stateInertialEstimate[6]  = current_xzy_rpy_measurement[3];
	stateInertialEstimate[7]  = current_xzy_rpy_measurement[4];
	stateInertialEstimate[8]  = current_xzy_rpy_measurement[5];
beuchatp's avatar
beuchatp committed
677
678
679

	// COMPUTE THE VELOCITIES VIA FINITE DIFFERENCE
	// > for (x,y,z) velocities
680
681
682
	stateInertialEstimate[3]  = (current_xzy_rpy_measurement[0] - previous_xzy_rpy_measurement[0]) * estimator_frequency;
	stateInertialEstimate[4]  = (current_xzy_rpy_measurement[1] - previous_xzy_rpy_measurement[1]) * estimator_frequency;
	stateInertialEstimate[5]  = (current_xzy_rpy_measurement[2] - previous_xzy_rpy_measurement[2]) * estimator_frequency;
beuchatp's avatar
beuchatp committed
683
	// > for (roll,pitch,yaw) velocities
684
685
686
	stateInertialEstimate[9]  = (current_xzy_rpy_measurement[3] - previous_xzy_rpy_measurement[3]) * estimator_frequency;
	stateInertialEstimate[10] = (current_xzy_rpy_measurement[4] - previous_xzy_rpy_measurement[4]) * estimator_frequency;
	stateInertialEstimate[11] = (current_xzy_rpy_measurement[5] - previous_xzy_rpy_measurement[5]) * estimator_frequency;
beuchatp's avatar
beuchatp committed
687
688
689
690
}



691
void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter(float (&stateInertialEstimate)[12])
beuchatp's avatar
beuchatp committed
692
693
694
695
696
697
{
	// PERFORM THE KALMAN FILTER UPDATE STEP
	// > First take a copy of the estimator state
	float temp_PMKFstate[12];
	for(int i = 0; i < 12; ++i)
	{
698
		temp_PMKFstate[i]  = stateInertialEstimate[i];
beuchatp's avatar
beuchatp committed
699
700
701
	}
	// > Now perform update for:
	// > x position and velocity:
702
703
	stateInertialEstimate[0] = PMKF_Ahat_row1_for_positions[0]*temp_PMKFstate[0] + PMKF_Ahat_row1_for_positions[1]*temp_PMKFstate[3] + PMKF_Kinf_for_positions[0]*current_xzy_rpy_measurement[0];
	stateInertialEstimate[3] = PMKF_Ahat_row2_for_positions[0]*temp_PMKFstate[0] + PMKF_Ahat_row2_for_positions[1]*temp_PMKFstate[3] + PMKF_Kinf_for_positions[1]*current_xzy_rpy_measurement[0];
beuchatp's avatar
beuchatp committed
704
	// > y position and velocity:
705
706
	stateInertialEstimate[1] = PMKF_Ahat_row1_for_positions[0]*temp_PMKFstate[1] + PMKF_Ahat_row1_for_positions[1]*temp_PMKFstate[4] + PMKF_Kinf_for_positions[0]*current_xzy_rpy_measurement[1];
	stateInertialEstimate[4] = PMKF_Ahat_row2_for_positions[0]*temp_PMKFstate[1] + PMKF_Ahat_row2_for_positions[1]*temp_PMKFstate[4] + PMKF_Kinf_for_positions[1]*current_xzy_rpy_measurement[1];
beuchatp's avatar
beuchatp committed
707
	// > z position and velocity:
708
709
	stateInertialEstimate[2] = PMKF_Ahat_row1_for_positions[0]*temp_PMKFstate[2] + PMKF_Ahat_row1_for_positions[1]*temp_PMKFstate[5] + PMKF_Kinf_for_positions[0]*current_xzy_rpy_measurement[2];
	stateInertialEstimate[5] = PMKF_Ahat_row2_for_positions[0]*temp_PMKFstate[2] + PMKF_Ahat_row2_for_positions[1]*temp_PMKFstate[5] + PMKF_Kinf_for_positions[1]*current_xzy_rpy_measurement[2];
beuchatp's avatar
beuchatp committed
710
711

	// > roll  position and velocity:
712
713
	stateInertialEstimate[6]  = PMKF_Ahat_row1_for_angles[0]*temp_PMKFstate[6] + PMKF_Ahat_row1_for_angles[1]*temp_PMKFstate[9]  + PMKF_Kinf_for_angles[0]*current_xzy_rpy_measurement[3];
	stateInertialEstimate[9]  = PMKF_Ahat_row2_for_angles[0]*temp_PMKFstate[6] + PMKF_Ahat_row2_for_angles[1]*temp_PMKFstate[9]  + PMKF_Kinf_for_angles[1]*current_xzy_rpy_measurement[3];
beuchatp's avatar
beuchatp committed
714
	// > pitch position and velocity:
715
716
	stateInertialEstimate[7]  = PMKF_Ahat_row1_for_angles[0]*temp_PMKFstate[7] + PMKF_Ahat_row1_for_angles[1]*temp_PMKFstate[10] + PMKF_Kinf_for_angles[0]*current_xzy_rpy_measurement[4];
	stateInertialEstimate[10] = PMKF_Ahat_row2_for_angles[0]*temp_PMKFstate[7] + PMKF_Ahat_row2_for_angles[1]*temp_PMKFstate[10] + PMKF_Kinf_for_angles[1]*current_xzy_rpy_measurement[4];
beuchatp's avatar
beuchatp committed
717
	// > yaw   position and velocity:
718
719
	stateInertialEstimate[8]  = PMKF_Ahat_row1_for_angles[0]*temp_PMKFstate[8] + PMKF_Ahat_row1_for_angles[1]*temp_PMKFstate[11] + PMKF_Kinf_for_angles[0]*current_xzy_rpy_measurement[5];
	stateInertialEstimate[11] = PMKF_Ahat_row2_for_angles[0]*temp_PMKFstate[8] + PMKF_Ahat_row2_for_angles[1]*temp_PMKFstate[11] + PMKF_Kinf_for_angles[1]*current_xzy_rpy_measurement[5];
beuchatp's avatar
beuchatp committed
720
721
722
723
724

}



725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745



//    ------------------------------------------------------------------------------
//    N   N  EEEEE  W     W   TTTTT   OOO   N   N        CCCC  M   M  DDDD
//    NN  N  E      W     W     T    O   O  NN  N       C      MM MM  D   D
//    N N N  EEE    W     W     T    O   O  N N N  -->  C      M M M  D   D
//    N  NN  E       W W W      T    O   O  N  NN       C      M   M  D   D
//    N   N  EEEEE    W W       T     OOO   N   N        CCCC  M   M  DDDD
//
//     CCCC   OOO   N   N  V   V  EEEEE  RRRR    SSSS  III   OOO   N   N
//    C      O   O  NN  N  V   V  E      R   R  S       I   O   O  NN  N
//    C      O   O  N N N  V   V  EEE    RRRR    SSS    I   O   O  N N N
//    C      O   O  N  NN   V V   E      R  R       S   I   O   O  N  NN
//     CCCC   OOO   N   N    V    EEEEE  R   R  SSSS   III   OOO   N   N
//    ----------------------------------------------------------------------------------

// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
float computeMotorPolyBackward(float thrust)
{
    return (-motorPoly[1] + sqrt(motorPoly[1] * motorPoly[1] - 4 * motorPoly[2] * (motorPoly[0] - thrust))) / (2 * motorPoly[2]);
746
747
}

748

749
Eigen::IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768


//    ----------------------------------------------------------------------------------
//    N   N  EEEEE  W     W        SSSS  EEEEE  TTTTT  PPPP    OOO   III  N   N  TTTTT
//    NN  N  E      W     W       S      E        T    P   P  O   O   I   NN  N    T
//    N N N  EEE    W     W        SSS   EEE      T    PPPP   O   O   I   N N N    T
//    N  NN  E       W W W            S  E        T    P      O   O   I   N  NN    T
//    N   N  EEEEE    W W         SSSS   EEEEE    T    P       OOO   III  N   N    T
//
//     GGG   U   U  III        CCCC    A    L      L      BBBB     A     CCCC  K   K
//    G   G  U   U   I        C       A A   L      L      B   B   A A   C      K  K
//    G      U   U   I        C      A   A  L      L      BBBB   A   A  C      KKK
//    G   G  U   U   I        C      AAAAA  L      L      B   B  AAAAA  C      K  K
//     GGGG   UUU   III        CCCC  A   A  LLLLL  LLLLL  BBBB   A   A   CCCC  K   K
//    ----------------------------------------------------------------------------------

// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
void setpointCallback(const Setpoint& newSetpoint)
{
769
    std::cout << "testing cout" << std::endl;
roangel's avatar
roangel committed
770
771
772
773
    setpoint[0] = newSetpoint.x;
    setpoint[1] = newSetpoint.y;
    setpoint[2] = newSetpoint.z;
    setpoint[3] = newSetpoint.yaw;
774

775
776
777
    x0 << stateInertialEstimate[0] - setpoint[0],
        stateInertialEstimate[1] - setpoint[1],
        stateInertialEstimate[2] - setpoint[2],
778
779
780
781
        stateInertialEstimate[3],
        stateInertialEstimate[4],
        stateInertialEstimate[5];

782

783
    initialize_MPC_variables(N, x0);
784

785

786
    // clock_t begin_time = clock();
787

788
    // Do the MPC step:
789
790
791

    // x0 <<  - setpoint[0],
    //     - setpoint[1],
roangel's avatar
roangel committed
792
    //     - setpoint[2],
793
794
795
796
    //     0,
    //     0,
    //     0;

797
    // U_0 = mympc_varying_another(A_tray, B_tray, g_tray, Q, R, P, N, x0, X_ref, U_ref, 0, 0.1391*4, 0.7887);
798
    // VectorUtype u = U_0.tail(4); // this is the input to apply next
799

800
801
802
    // std::cout << "U_0 opt for this setpoint, 1 step MPC" << std::endl;
    // std::cout << U_0.format(CleanFmt) << std::endl << std::endl;

803
804
805
806
    // // prediction of next N states:
    // VectorXtype x_approx_next;
    // std::vector<VectorXtype> x_approx_temp;
    // x_approx_next << x0;
807

808
809
    // for(int j = 0; j < N; j++)
    // {
810
811
812
813
814
815
816
817
818
819
820
821
    //     euler_method_forward(F_crazyflie_6_states, 0, params.Ts/4, params.Ts, x_approx_next, U_0.segment(N_u*j,N_u), params, x_approx_temp);
    //     // std::cout << "for x_0 as input :" << std::endl;
    //     // std::cout << x_approx_next.format(CleanFmt);

    //     // std::cout << "and U as input :" << std::endl;
    //     // std::cout << U_0.segment(N_u*j,N_u).format(CleanFmt) << std::endl;

    //     // for(int i = 0; i < x_approx_temp.size(); i++)
    //     // {
    //     //     std::cout << "x_approx_temp i = " << i << std::endl;
    //     //     std::cout << x_approx_temp[i].format(CleanFmt) << std::endl << std::endl;
    //     // }
822
823
824
825
826
    //     x_approx_next = x_approx_temp.back();
    //     X_tray[j] = x_approx_next;

    //     // Now update U_tray using the same loop
    //     if(j < N-1)
827
    //         U_tray[j] = U_0.segment(N_u*(j+1), N_u);
828
    //     else
829
    //         U_tray[j] = U_0.segment(N_u*j, N_u); // last element of the new U_tray is assumed to be the same as second to last
830
831
    // }
    // // X_tray and U_tray have been completely updated with the new states coming from the new sequence of inputs
832

833
834
835
836
837
838
839
840
    // std::cout << "X_tray for this setpoint, 1 step MPC" << std::endl;
    // for(int i = 0; i < X_tray.size(); i++)
    //     std::cout << X_tray[i].format(CleanFmt) << std::endl << std::endl;

    // std::cout << "U_tray for this setpoint, 1 step MPC" << std::endl;
    // for(int i = 0; i < U_tray.size(); i++)
    //     std::cout << U_tray[i].format(CleanFmt) << std::endl << std::endl;

841
842
    // // Relinearize around new trayectory:
    // get_matrices_linearization_affine(X_tray, U_tray, params, A_tray, B_tray, g_tray);
843

844
845
846
847
848
849
850
851
852
853
854
855
    // std::cout << "A_tray for this setpoint, 1 step MPC" << std::endl;
    // for(int i = 0; i < A_tray.size(); i++)
    //     std::cout << A_tray[i].format(CleanFmt) << std::endl << std::endl;

    // std::cout << "B_tray for this setpoint, 1 step MPC" << std::endl;
    // for(int i = 0; i < B_tray.size(); i++)
    //     std::cout << B_tray[i].format(CleanFmt) << std::endl << std::endl;

    // std::cout << "g_tray for this setpoint, 1 step MPC" << std::endl;
    // for(int i = 0; i < g_tray.size(); i++)
    //     std::cout << g_tray[i].format(CleanFmt) << std::endl << std::endl;

856
857
858
859
860
    // // update the current input to the middle loop
    // raw_input[0] = u(0);
    // raw_input[1] = u(1);
    // raw_input[2] = u(2);
    // raw_input[3] = u(3);
861

862
863
    // clock_t end_time = clock();
    // double elapsed_secs = double(end_time - begin_time) / CLOCKS_PER_SEC;
864
865


866
867
    // // std::cout << "U_0 for this setpoint, 1 step MPC" << std::endl;
    // // std::cout << U_0.format(CleanFmt) << std::endl;
868

869
    // std::cout << "elapsed secs: " << elapsed_secs << std::endl;
870

871
    // testzone:
872

873
874
    // VectorXtype x_vec;
    // x_vec.setZero();
875

876
877
    // VectorUtype u_vec;
    // u_vec << 0, 0, 0, cf_mass/1000*9.81;
878

879
880
    // MatrixAtype A;
    // MatrixBtype B;
881

882
883
884
    // MatrixAtype A_d;
    // MatrixBtype B_d;
    // VectorXtype g_d;
885

886
887
888
    // params.m = cf_mass/1000.0;
    // params.g = 9.81;
    // params.Ts = 1/50.0;
889

890
891
    // // linearization_fast_euler_6_states(x_vec, u_vec, params, A, B);
    // // discretization_affine(A, B, x_vec, u_vec, params, A_d, B_d, g_d);
892

893
894
    // std::vector<VectorXtype> X_tray;
    // std::vector<VectorUtype> U_tray;
895

896
897
898
899
    // for(int i = 0; i < 5; i++)
    // {
    //     VectorXtype x_temp;
    //     VectorUtype u_temp;
900

901
902
    //     x_temp << i, 0, 0.4, 0, 0, 0;
    //     u_temp << i, 0, 0, params.m*params.g;
903

904
905
906
    //     X_tray.push_back(x_temp);
    //     U_tray.push_back(u_temp);
    // }
907

908
909
910
    // std::vector<MatrixAtype> A_tray;
    // std::vector<MatrixBtype> B_tray;
    // std::vector<VectorXtype> g_tray;
roangel's avatar
roangel committed
911

912
    // get_matrices_linearization_affine(X_tray, U_tray, params, A_tray, B_tray, g_tray);
roangel's avatar
roangel committed
913

914
915
916
917
918
    // for(int i = 0; i < A_tray.size(); i++)
    // {
    //     std::cout << "index matrix" << i << std::endl;
    //     std::cout << A_tray[i] << std::endl;
    // }
roangel's avatar
roangel committed
919

920
921
922
    // // debug
    // Eigen::IOFormat OctaveFmt(Eigen::StreamPrecision, 0, ", ", ";\n", "", "", "[", "]");
    // Eigen::IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");
roangel's avatar
roangel committed
923
924
925



926
    // // std::cout << A.format(OctaveFmt) << std::endl;
roangel's avatar
roangel committed
927

928
    // // std::cout << B.format(OctaveFmt) << std::endl;
roangel's avatar
roangel committed
929

930
    // // debug of euler method function:
roangel's avatar
roangel committed
931

932
933
934
935
936
    // std::vector<VectorXtype> x_out;
    // VectorXtype x0;
    // VectorUtype u;
    // x0 << 0, 0, 0, 0, 0, 0;
    // u << 0, 0, 0, params.m*params.g*2;
roangel's avatar
roangel committed
937

938
    // euler_method_forward(F_crazyflie_6_states, 0, params.Ts/4, params.Ts, x0, u, params, x_out);
roangel's avatar
roangel committed
939

940
941
942
943
944
    // for(int i = 0; i < x_out.size(); i++)
    // {
    //     std::cout << "xout index:" << i << std::endl;
    //     std::cout << x_out[i] << std::endl;
    // }
roangel's avatar
roangel committed
945

946
947
948
949
    // Eigen::MatrixXd Sx_test;
    // Eigen::MatrixXd Su_test;
    // Eigen::MatrixXd Sg_test;
    // int N = 5;
950

951
    // funSxSuSg_varying_affine(A_tray, B_tray, N, Sx_test, Su_test, Sg_test);
952

953
954
    // std::cout << "Sx test:" << std::endl << std::endl;
    // std::cout << Sx_test<< std::endl;
955

956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
    // std::cout << "Su test:" << std::endl << std::endl;
    // // std::cout << Su_test<< std::endl;
    // std::cout << Su_test.format(CleanFmt) << std::endl;

    // std::cout << "Sg test:" << std::endl << std::endl;
    // // std::cout << Su_test<< std::endl;
    // std::cout << Sg_test.format(CleanFmt) << std::endl;

    // Eigen::MatrixXd Qbar;
    // Eigen::MatrixXd Rbar;

    // VectorXtype p_vec;
    // p_vec << 3, 3, 3, 2, 2, 2;
    // VectorXtype q_vec;
    // q_vec << 1, 1, 1, 4, 4, 4;

    // VectorUtype r_vec;
    // r_vec << 1, 1, 1, 4;

    // Eigen::MatrixXd P = p_vec.asDiagonal();
    // Eigen::MatrixXd Q = q_vec.asDiagonal();
    // Eigen::MatrixXd R = r_vec.asDiagonal();

    // funQbar(Q, P, N, Qbar);

    // std::cout << "Qbar test:" << std::endl << std::endl;
    // // std::cout << Su_test<< std::endl;
    // std::cout << Qbar.format(CleanFmt) << std::endl;

    // funRbar(R,N, Rbar);

    // std::cout << "Rbar test:" << std::endl << std::endl;
    // // std::cout << Su_test<< std::endl;
    // std::cout << Rbar.format(CleanFmt) << std::endl;
990

991
992
993
994
995
996
997
998
999
1000
1001
1002
1003
1004
1005
1006
    // Eigen::VectorXd X_ref(N_x*(N+1));
    // Eigen::VectorXd U_ref(N_u*N);

    // VectorXtype x_ref;
    // VectorUtype u_ref;
    // x_ref << 0,0,0,0,0,0;
    // u_ref << 0,0,0,params.m*params.g;

    // for(int i = 0; i < N+1; i++)
    // {
    //     X_ref.segment(i*N_x, N_x) = x_ref;
    //     if(i < N)
    //         U_ref.segment(i*N_u, N_u) = u_ref;
    // }

    // Eigen::VectorXd U_0 = mympc_varying_another(A_tray, B_tray, g_tray, Q, R, P, N, x0, X_ref, U_ref, 0, 0, 0);
1007
1008
}

1009
void initialize_MPC_variables(int N, VectorXtype x0)
roangel's avatar
roangel committed
1010
{
1011

roangel's avatar
roangel committed
1012
1013
1014
1015
1016
1017

    // Initialize initial state, 6 states
    VectorUtype u_temp;
    u_temp << 0, 0, 0, params.m*params.g;

    // X_tray and U_tray
1018
1019

    MPC_mutex.lock();
1020
1021
    X_tray.clear();
    U_tray.clear();
roangel's avatar
roangel committed
1022
1023
1024
1025
1026
    for(int i = 0; i < N; i++)
    {
        X_tray.push_back(x0);
        U_tray.push_back(u_temp);
    }
1027
    MPC_mutex.unlock();
roangel's avatar
roangel committed
1028

1029
    // cout << "filled X_tray and U_tray:" << endl;
1030

1031
1032
1033
    // cout << "X_tray:" << endl;
    // for(int i=0; i < X_tray.size(); i++)
    //     cout << X_tray[i].format(CleanFmt) << endl;
1034

1035
1036
1037
    // cout << "U_tray:" << endl;
    // for(int i=0; i < U_tray.size(); i++)
    //     cout << U_tray[i].format(CleanFmt) << endl;
1038

roangel's avatar
roangel committed
1039
    // fill A_tray, B_tray, g_tray with initial values
1040
    MPC_mutex.lock();
roangel's avatar
roangel committed
1041
    get_matrices_linearization_affine(X_tray, U_tray, params, A_tray, B_tray, g_tray);
1042
    MPC_mutex.unlock();
roangel's avatar
roangel committed
1043

1044
    // cout << "filled A_tray  B_tray and g_tray:" << endl;
1045

1046
1047
1048
    // cout << "A_tray:" << endl;
    // for(int i=0; i < A_tray.size(); i++)
    //     cout << A_tray[i].format(CleanFmt) << endl;
1049

1050
1051
1052
    // cout << "B_tray:" << endl;
    // for(int i=0; i < B_tray.size(); i++)
    //     cout << B_tray[i].format(CleanFmt) << endl;
1053

1054
1055
1056
    // cout << "g_tray:" << endl;
    // for(int i=0; i < g_tray.size(); i++)
    //     cout << g_tray[i].format(CleanFmt) << endl;
1057

roangel's avatar
roangel committed
1058
1059
}

1060

1061
1062
1063
1064
1065
1066
1067
1068
1069
1070
1071
1072
1073
1074
1075
1076


//    ----------------------------------------------------------------------------------
//    L       OOO     A    DDDD
//    L      O   O   A A   D   D
//    L      O   O  A   A  D   D
//    L      O   O  AAAAA  D   D
//    LLLLL   OOO   A   A  DDDD
//
//    PPPP     A    RRRR     A    M   M  EEEEE  TTTTT  EEEEE  RRRR    SSSS
//    P   P   A A   R   R   A A   MM MM  E        T    E      R   R  S
//    PPPP   A   A  RRRR   A   A  M M M  EEE      T    EEE    RRRR    SSS
//    P      AAAAA  R  R   AAAAA  M   M  E        T    E      R  R       S
//    P      A   A  R   R  A   A  M   M  EEEEE    T    EEEEE  R   R  SSSS
//    ----------------------------------------------------------------------------------

1077
1078
1079
1080
1081
1082
1083
1084
1085
1086
1087
1088

// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
void yamlReadyForFetchCallback(const std_msgs::Int32& msg)
{
	// Extract from the "msg" for which controller the and from where to fetch the YAML
	// parameters
	int controller_to_fetch_yaml = msg.data;

	// Switch between fetching for the different controllers and from different locations
	switch(controller_to_fetch_yaml)
	{

1089
		// > FOR FETCHING FROM THE AGENT'S OWN PARAMETER SERVICE
1090
		case FETCH_YAML_MPC_CONTROLLER_FROM_OWN_AGENT:
1091
		{
1092
			// Let the user know that this message was received
1093
			ROS_INFO("[MPC CONTROLLER] Received the message that YAML parameters were (re-)loaded. > Now fetching the parameter values from this agent.");
1094
			// Create a node handle to the parameter service running on this agent's machine
1095
			ros::NodeHandle nodeHandle_to_own_agent_parameter_service(namespace_to_own_agent_parameter_service);
1096
			// Call the function that fetches the parameters
1097
1098
			fetchYamlParameters(nodeHandle_to_own_agent_parameter_service);
			break;
1099
		}
1100

1101
		// > FOR FETCHING FROM THE COORDINATOR'S PARAMETER SERVICE
1102
		case FETCH_YAML_MPC_CONTROLLER_FROM_COORDINATOR:
1103
1104
		{
			// Let the user know that this message was received
1105
			ROS_INFO("[MPC CONTROLLER] Received the message that YAML parameters were (re-)loaded. > Now fetching the parameter values from the coordinator.");
1106
			// Create a node handle to the parameter service running on the coordinator machine
1107
			ros::NodeHandle nodeHandle_to_coordinator_parameter_service(namespace_to_coordinator_parameter_service);
1108
1109
1110
1111
1112
			// Call the function that fetches the parameters
			fetchYamlParameters(nodeHandle_to_coordinator_parameter_service);
			break;
		}

1113
		default:
1114
		{
1115
			// Let the user know that the command was not relevant
1116
1117
			//ROS_INFO("The CustomControllerService received the message that YAML parameters were (re-)loaded");
			//ROS_INFO("> However the parameters do not relate to this controller, hence nothing will be fetched.");
1118
			break;
1119
		}
1120
1121
1122
1123
	}
}


1124
// This function CAN BE edited for successful completion of the PPS exercise, and the
1125
// use of parameters fetched from the YAML file is highly recommended to make tuning of
1126
// your controller easier and quicker.
1127
void fetchYamlParameters(ros::NodeHandle& nodeHandle)
1128
{
1129
	// Here we load the parameters that are specified in the CustomController.yaml file
1130

1131
	// Add the "CustomController" namespace to the "nodeHandle"
1132
	ros::NodeHandle nodeHandle_for_MpcController(nodeHandle, "MpcController");
1133

1134
	// > The mass of the crazyflieflieflie
1135
	cf_mass = getParameterFloat(nodeHandle_for_MpcController , "mass");
1136

1137
1138
	// Display one of the YAML parameters to debug if it is working correctly
	//ROS_INFO_STREAM("DEBUGGING: mass leaded from loacl file = " << cf_mass );
1139

1140
1141
	// > The frequency at which the "computeControlOutput" is being called, as determined
	//   by the frequency at which the Vicon system provides position and attitude data
1142
	control_frequency = getParameterFloat(nodeHandle_for_MpcController, "control_frequency");
1143

1144
1145
	// > The co-efficients of the quadratic conversation from 16-bit motor command to
	//   thrust force in Newtons
1146
	getParameterFloatVector(nodeHandle_for_MpcController, "motorPoly", motorPoly, 3);
1147

1148
1149

	// DEBUGGING: Print out one of the parameters that was loaded
1150
	ROS_INFO_STREAM("[MPC CONTROLLER] DEBUGGING: the fetched CustomController/mass = " << cf_mass);
1151