StudentControllerService.cpp

//    Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat, Angel Romero, Cyrill Burgener, Marco Mueller, Philipp Friedli
//
//    This file is part of D-FaLL-System.
//    
//    D-FaLL-System is free software: you can redistribute it and/or modify
//    it under the terms of the GNU General Public License as published by
//    the Free Software Foundation, either version 3 of the License, or
//    (at your option) any later version.
//    
//    D-FaLL-System is distributed in the hope that it will be useful,
//    but WITHOUT ANY WARRANTY; without even the implied warranty of
//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//    GNU General Public License for more details.
//    
//    You should have received a copy of the GNU General Public License
//    along with D-FaLL-System.  If not, see <http://www.gnu.org/licenses/>.
//    
//
//    ----------------------------------------------------------------------------------
//    DDDD        FFFFF        L     L           SSSS  Y   Y   SSSS  TTTTT  EEEEE  M   M
//    D   D       F      aaa   L     L          S       Y Y   S        T    E      MM MM
//    D   D  ---  FFFF  a   a  L     L     ---   SSS     Y     SSS     T    EEE    M M M
//    D   D       F     a  aa  L     L              S    Y        S    T    E      M   M
//    DDDD        F      aa a  LLLL  LLLL       SSSS     Y    SSSS     T    EEEEE  M   M
//
//
//    DESCRIPTION:
//    Place for students to implement their controller
//
//    ----------------------------------------------------------------------------------





// INCLUDE THE HEADER
#include "nodes/StudentControllerService.h"


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





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

// This function is the callback that is linked to the "StudentController" 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)
//       \____/                       \____/
//
//
//
// The distributed controller 
bool calculateControlOutput(Controller::Request &request, Controller::Response &response)
{	
	switch(controller_state)
	{
		case 0:
		{
			// --------------------------------------
			// Motors Off
			// --------------------------------------
			if (outer_loop_counter == 0) 
			{
				ROS_INFO("[STUDENT CONTROLLER] computed Motors Off control command");
			}
			turn_motors_off(response);
			break;
		}
		case 1:
		{
			// --------------------------------------
			// Take-off Copters
			// --------------------------------------
			if (outer_loop_counter == 0) 
			{
				// current load position is origin
				load_setpoint[0] 	= request.otherCrazyflies[3].x;
				load_setpoint[1] 	= request.otherCrazyflies[3].y;
				load_setpoint[2] 	= 0.0;
				float yaw = request.otherCrazyflies[0].yaw;
				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;
    			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 ;
			}
			if (outer_loop_counter <= (int)(control_frequency*durationTakeOff) ) {
				agent_setpoint[2] += takeOffDistance_increment;
			}
			else
			{
				agent_setpoint[2] = takeOffDistance;
			}
			// compute agent's lqr command for the setpoint
			agent_lqr_command( request , response );
			
			break;
		}
		case 2:
		{
			// --------------------------------------
			// Distributed controller copter and load
			// --------------------------------------
			if (outer_loop_counter == 0) {
				// distributed controller was enabled
				ROS_INFO("[STUDENT CONTROLLER] Distributed controller enabled");	

				// load z-setpoint is current load z-position
				load_setpoint[0] = request.otherCrazyflies[3].x;	
				load_setpoint[1] = request.otherCrazyflies[3].y;	
				load_setpoint[2] = request.otherCrazyflies[3].z;			
			}

			// Distributed Controller runs at 100Hz --> downsampling by factor 2
			if (outer_loop_counter % 2 == 0)
			{
				// set equilibrium load position to current load set point
				augmented_state_eq(7,0) 	= load_setpoint[0];
				augmented_state_eq(8,0) 	= load_setpoint[1];
				augmented_state_eq(9,0) 	= load_setpoint[2];
				augmented_state_eq(26,0) 	= load_setpoint[0];
				augmented_state_eq(27,0) 	= load_setpoint[1];
				augmented_state_eq(28,0) 	= load_setpoint[2];			
				augmented_state_eq(45,0) 	= load_setpoint[0];
				augmented_state_eq(46,0) 	= load_setpoint[1];
				augmented_state_eq(47,0) 	= load_setpoint[2];

				// update measurement vector using vicon Data
				get_y_from_CFData( request );

			}


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

			// compute agent's lqr command for the setpoint
			//agent_lqr_command( request , response );

			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);

			}

			if (delta_z_load_current > 0.0)
			{
				// load has not reached landing distance
				if (outer_loop_counter <= (int)(control_frequency*durationLanding_load)) {
					load_setpoint[2] -= landingDistance_decrement;
				}
				else {
					// switch to landing copters
					controller_state = 4;
				}

			}

			// Distributed Controller runs at 100Hz --> downsampling by factor 2
			if (outer_loop_counter % 2 == 0)
			{
				// set equilibrium load position to current load set point
				augmented_state_eq(7,0) 	= load_setpoint[0];
				augmented_state_eq(8,0) 	= load_setpoint[1];
				augmented_state_eq(9,0) 	= load_setpoint[2];
				augmented_state_eq(26,0) 	= load_setpoint[0];
				augmented_state_eq(27,0) 	= load_setpoint[1];
				augmented_state_eq(28,0) 	= load_setpoint[2];			
				augmented_state_eq(45,0) 	= load_setpoint[0];
				augmented_state_eq(46,0) 	= load_setpoint[1];
				augmented_state_eq(47,0) 	= load_setpoint[2];

				// update measurement vector using vicon Data
				get_y_from_CFData( request );

			}

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

			break;
		}			
		case 4:
		{

			float delta_z_current = request.ownCrazyflie.z - landingDistance;
			// --------------------------------------
			// Landing Copters
			// --------------------------------------
			if ( outer_loop_counter == 0 ) {
				// Landing was initiated
				landingDistance_decrement = delta_z_current / ( control_frequency * durationLanding );
				// 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;
				float yaw = request.otherCrazyflies[0].yaw;
				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];
    				landing_delta[2] += attachmentPoint.P1[2];
    				distance_to_load_world = transformation_frame_load_to_world(landing_delta , 0.0, 0.0, yaw );
				
    			}
    			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];
    				distance_to_load_world = transformation_frame_load_to_world(landing_delta, 0.0, 0.0, yaw );
    			}
    			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];   				
    				distance_to_load_world = transformation_frame_load_to_world(landing_delta, 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] 	= request.ownCrazyflie.z;
				agent_setpoint[3] 	= 0.0 ;		

			}
			// check if copter is within landing distance
			if ( delta_z_current > 0.0 ) 
			{
				/// decrease z-setpoint
				if (outer_loop_counter <= (int)( control_frequency*durationLanding) ) 
				{
					agent_setpoint[2] -= landingDistance_decrement;
					// compute agent's lqr command for new setpoint
					agent_lqr_command( request , response );					
				}
				else 
				{
					// landing duration is over
					turn_motors_off( response );
					// set controller state to zero
					controller_state = 0;					
				}
			}
			else 
			{
				// copter is within landing distance
				turn_motors_off( response );
				// set controller state to zero
				controller_state = 0;
			}
			break;			
		}
		case 5:
		{
			// --------------------------------------
			// 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;	
				load_setpoint[2] = request.otherCrazyflies[3].z;			
			}

			// Distributed Controller runs at 100Hz --> downsampling by factor 2
			if (outer_loop_counter % 2 == 0)
			{
				// set equilibrium load position to current load set point
				state_eq(21,0) 	= load_setpoint[0];
				state_eq(22,0) 	= load_setpoint[1];
				state_eq(23,0) 	= load_setpoint[2];

				// update measurement vector using vicon Data
				get_y_from_CFData( request );

			}


			// compute centralized controller commands
			//centralized_control_command( request, response );

			// compute agent's lqr command for the setpoint
			agent_lqr_command( request , response );

			break;
		}			
	}
	outer_loop_counter += 1;
	return true;
}


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

	// Control output
	Eigen::Matrix<float,12,1> u = overlappingController.K * overlapping_estimator_state_prev;
	// Estimator update
	overlapping_estimator_state_prev = overlappingController.Phi * overlapping_estimator_state_prev + overlappingController.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
		thrustSum 	= u(0,0);
		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
		thrustSum 	= u(4,0);
		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
		thrustSum 	= u(8,0);
		outRoll		= u(9,0);
		outPitch	= u(10,0);
		outYaw		= u(11,0);	
	}

	if (outer_loop_counter % 50 == 0)
	{
		ROS_INFO_STREAM("[STUDENT CONTROLLER] u thrust sum computed = " << thrustSum );
	}
	
	// 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;
}


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
		thrustSum 	= u(0,0);
		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
		thrustSum 	= u(4,0);
		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
		thrustSum 	= u(8,0);
		outRoll		= u(9,0);
		outPitch	= u(10,0);
		outYaw		= u(11,0);	
	}

	if (outer_loop_counter % 50 == 0)
	{
		ROS_INFO_STREAM("[STUDENT CONTROLLER] u thrust sum computed = " << thrustSum );
	}
	
	// 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;
}

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


	std::vector<float> P1_world;
	P1_world = transformation_frame_load_to_world( attachmentPoint.P1, roll , pitch , yaw );
	std::vector<float> P2_world;
	P2_world = transformation_frame_load_to_world( attachmentPoint.P2, roll , pitch , yaw );	
	std::vector<float> P3_world;
	P3_world = transformation_frame_load_to_world( attachmentPoint.P3, roll , pitch , yaw );

	// y1 = [CF01.x , CF01.y , CF01.z, CF01.roll, CF01.pitch, CF01.yaw , len_cable*phi1 , len_cable*theta1 ]
	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;
	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];

	// 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(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;
	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];

	// 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(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;
	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];


	// 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[0]; 				// equilibrium x-pose copter 1
	y_eq(4,0) 	= load_setpoint[1] + P1_world[1]; 				// equilibrium y-pose copter 1
	y_eq(5,0) 	= load_setpoint[2] + P1_world[2] + len_cable; 	// equilibrium z-pose copter 1
	y_eq(11,0) 	= load_setpoint[0] + P2_world[0]; 				// equilibrium x-pose copter 2
	y_eq(12,0) 	= load_setpoint[1] + P2_world[1]; 				// equilibrium y-pose copter 2
	y_eq(13,0) 	= load_setpoint[2] + P2_world[2] + len_cable;	// equilibrium z-pose copter 2
	y_eq(19,0) 	= load_setpoint[0] + P3_world[0]; 				// equilibrium x-pose copter 3
	y_eq(20,0) 	= load_setpoint[1] + P3_world[1]; 				// equilibrium y-pose copter 3
	y_eq(21,0) 	= load_setpoint[2] + P3_world[2] + len_cable;	// equilibrium z-pose copter 3

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

	// Debugging
	if (outer_loop_counter % 50 == 0)
	{
		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() );		
	}	


}

std::vector<float> transformation_frame_load_to_world(std::vector<float> P_load , float roll, float pitch, float yaw )
{
	// 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];
	P_y_world += ( -cos_yaw_load*sin_yaw_load + sin_yaw_load*sin_pitch_load*cos_roll_load ) * P_load[2];
	// 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];	

	std::vector<float> P_world = {0 ,0 ,0};
	P_world[0] = P_x_world;
	P_world[1] = P_y_world;
	P_world[2] = P_z_world;

	return P_world;
}


Eigen::MatrixXf readMatrix(std::string filename)
{
    int cols = 0, rows = 0;
    float buff[10000];

    // 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;
        float temp_bufferVal = 0;
        std::stringstream stream(line);
        while(! stream.eof())
            stream >> buff[cols*rows+temp_cols++] ;

        if (temp_cols == 0)
            continue;

        if (cols == 0)
            cols = temp_cols;

        rows++;
        }

    infile.close();

    //rows--;

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

void turn_motors_off( Controller::Response &response ) 
{
	// 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;
}

void agent_lqr_command( Controller::Request &request , Controller::Response &response ) 
{
	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);

    //calculate feedback
    float outRoll = 0;
    float outPitch = 0;
    float outYaw = 0;
    float thrustIntermediate = 0;
    for(int i = 0; i < 9; ++i) {
        outRoll -= gainMatrixRollRate[i] * state[i];
        outPitch -= gainMatrixPitchRate[i] * state[i];
        outYaw -= gainMatrixYawRate[i] * state[i];
        thrustIntermediate -= gainMatrixThrust[i] * state[i];
    }

	// PREPARE AND RETURN THE VARIABLE "response"
 	
    response.controlOutput.roll = outRoll;
    response.controlOutput.pitch = outPitch;
    response.controlOutput.yaw = outYaw;

    response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustIntermediate + gravity_force);
    response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustIntermediate + gravity_force);
    response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustIntermediate + gravity_force);
    response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustIntermediate + gravity_force);
    

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



//    ------------------------------------------------------------------------------
//    RRRR    OOO   TTTTT    A    TTTTT  EEEEE       III  N   N  TTTTT   OOO
//    R   R  O   O    T     A A     T    E            I   NN  N    T    O   O
//    RRRR   O   O    T    A   A    T    EEE          I   N N N    T    O   O
//    R  R   O   O    T    AAAAA    T    E            I   N  NN    T    O   O
//    R   R   OOO     T    A   A    T    EEEEE       III  N   N    T     OOO
//
//    BBBB    OOO   DDDD   Y   Y       FFFFF  RRRR     A    M   M  EEEEE
//    B   B  O   O  D   D   Y Y        F      R   R   A A   MM MM  E
//    BBBB   O   O  D   D    Y         FFF    RRRR   A   A  M M M  EEE
//    B   B  O   O  D   D    Y         F      R  R   AAAAA  M   M  E
//    BBBB    OOO   DDDD     Y         F      R   R  A   A  M   M  EEEEE
//    ----------------------------------------------------------------------------------

// The arguments for this function are as follows:
// stateInertial
// This is an array of length 9 with the estimates the error of of the following values
// relative to the sepcifed setpoint:
//     stateInertial[0]    x position of the Crazyflie relative to the inertial frame origin [meters]
//     stateInertial[1]    y position of the Crazyflie relative to the inertial frame origin [meters]
//     stateInertial[2]    z position of the Crazyflie relative to the inertial frame origin [meters]
//     stateInertial[3]    x-axis component of the velocity of the Crazyflie in the inertial frame [meters/second]
//     stateInertial[4]    y-axis component of the velocity of the Crazyflie in the inertial frame [meters/second]
//     stateInertial[5]    z-axis component of the velocity of the Crazyflie in the inertial frame [meters/second]
//     stateInertial[6]    The roll  component of the intrinsic Euler angles [radians]
//     stateInertial[7]    The pitch component of the intrinsic Euler angles [radians]
//     stateInertial[8]    The yaw   component of the intrinsic Euler angles [radians]
// 
// stateBody
// This is an empty array of length 9, this function should fill in all elements of this
// array with the same ordering as for the "stateInertial" argument, expect that the (x,y)
// position and (x,y) velocities are rotated into the body frame.
//
// yaw_measured
// This is the yaw component of the intrinsic Euler angles in [radians] as measured by
// the Vicon motion capture system
//

void convertIntoBodyFrame(float est[9], float (&state)[9], float yaw_measured)
{
    float sinYaw = sin(yaw_measured);
    float cosYaw = cos(yaw_measured);

    state[0] = est[0] * cosYaw + est[1] * sinYaw;
    state[1] = -est[0] * sinYaw + est[1] * cosYaw;
    state[2] = est[2];

    state[3] = est[3] * cosYaw + est[4] * sinYaw;
    state[4] = -est[3] * sinYaw + est[4] * cosYaw;
    state[5] = est[5];

    state[6] = est[6];
    state[7] = est[7];
    state[8] = est[8];
}


//    ------------------------------------------------------------------------------
//    KALMAN FILTER
//    ----------------------------------------------------------------------------------
void estimateState(Controller::Request &request, float (&est)[9])
{
    // attitude
    est[6] = request.ownCrazyflie.roll;
    est[7] = request.ownCrazyflie.pitch;
    est[8] = request.ownCrazyflie.yaw;

    //velocity & filtering
    float ahat_x[6]; //estimator matrix times state (x, y, z, vx, vy, vz)
    ahat_x[0] = 0; ahat_x[1]=0; ahat_x[2]=0;
    ahat_x[3] = estimatorMatrix[0] * prevEstimate[0] + estimatorMatrix[1] * prevEstimate[3];
    ahat_x[4] = estimatorMatrix[0] * prevEstimate[1] + estimatorMatrix[1] * prevEstimate[4];
    ahat_x[5] = estimatorMatrix[0] * prevEstimate[2] + estimatorMatrix[1] * prevEstimate[5];

    
    float k_x[6]; //filterGain times state
    k_x[0] = request.ownCrazyflie.x * filterGain[0];
    k_x[1] = request.ownCrazyflie.y * filterGain[1];
    k_x[2] = request.ownCrazyflie.z * filterGain[2];
    k_x[3] = request.ownCrazyflie.x * filterGain[3];
    k_x[4] = request.ownCrazyflie.y * filterGain[4];
    k_x[5] = request.ownCrazyflie.z * filterGain[5];
   
    est[0] = ahat_x[0] + k_x[0];
    est[1] = ahat_x[1] + k_x[1];
    est[2] = ahat_x[2] + k_x[2];
    est[3] = ahat_x[3] + k_x[3];
    est[4] = ahat_x[4] + k_x[4];
    est[5] = ahat_x[5] + k_x[5];

    memcpy(prevEstimate, est, 9 * sizeof(float));
}

std::string getcwd_string( void ) {
   char buff[PATH_MAX];
   getcwd( buff, PATH_MAX );
   std::string cwd( buff );
   return cwd;
}
//    ------------------------------------------------------------------------------
//    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]);
}





//    ----------------------------------------------------------------------------------
//    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)
{
    load_setpoint[0] = newSetpoint.x;
    load_setpoint[1] = newSetpoint.y;
    load_setpoint[2] = newSetpoint.z;
    load_setpoint[3] = newSetpoint.yaw;
	ROS_INFO_STREAM("[STUDENT CONTROLLER] new load setpoint, x: "<< load_setpoint[0]  );
	ROS_INFO_STREAM("[STUDENT CONTROLLER] new load setpoint, y: "<< load_setpoint[1]  );
	ROS_INFO_STREAM("[STUDENT CONTROLLER] new load setpoint, z: "<< load_setpoint[2]  );    
	ROS_INFO_STREAM("[STUDENT CONTROLLER] new load setpoint, yaw: "<< load_setpoint[3]  ); 
}


//    ----------------------------------------------------------------------------------
//     CCCC  U   U   SSSS  TTTTT   OOO   M   M
//    C      U   U  S        T    O   O  MM MM
//    C      U   U   SSS     T    O   O  M M M
//    C      U   U      S    T    O   O  M   M
//     CCCC   UUU   SSSS     T     OOO   M   M
//
//     CCCC   OOO   M   M  M   M    A    N   N  DDDD
//    C      O   O  MM MM  MM MM   A A   NN  N  D   D
//    C      O   O  M M M  M M M  A   A  N N N  D   D
//    C      O   O  M   M  M   M  AAAAA  N  NN  D   D
//     CCCC   OOO   M   M  M   M  A   A  N   N  DDDD
//    ----------------------------------------------------------------------------------

// CUSTOM COMMAND RECEIVED CALLBACK
void customCommandReceivedCallback(const CustomButton& commandReceived)
{
	// Extract the data from the message
	int custom_button_index   = commandReceived.button_index;
	float custom_command_code = commandReceived.command_code;

	// Switch between the button pressed
	switch(custom_button_index)
	{

		// > FOR CUSTOM BUTTON 1
		case 1:
		{
			// Let the user know that this part of the code was triggered
			ROS_INFO("[STUDENT CONTROLLER] Button 1 received in controller.");
			// Code here to respond to custom button 1
			break;
		}

		// > FOR CUSTOM BUTTON 2
		case 2: