Skip to content
GitLab
Commit 5289bce5 authored by roangel's avatar roangel
Browse files

Fixed some bugs: U_tray was not correctly updated. Added saturation. Change of... 

Fixed some bugs: U_tray was not correctly updated. Added saturation. Change of setpoint correct sign now.
parent 94651076
Hide whitespace changes
Inline Side-by-side
pps_ws/src/d_fall_pps/param/MpcController.yaml
View file @ 5289bce5
# Mass of the crazyflie
mass : 32
mass : 30
# Frequency of the controller, in hertz
control_frequency : 200
......@@ -7,9 +7,9 @@ control_frequency : 200
# Quadratic motor regression equation (a0, a1, a2)
motorPoly : [5.484560e-4, 1.032633e-6, 2.130295e-11]
gainMatrixRollRate_Nested : [ 4.00, 0.00, 0.00]
gainMatrixPitchRate_Nested : [ 0.00, 4.00, 0.00]
gainMatrixYawRate_Nested : [ 0.00, 0.00, 2.30]
gainMatrixRollRate_Nested : [ 7.00, 0.00, 0.00]
gainMatrixPitchRate_Nested : [ 0.00, 7.00, 0.00]
gainMatrixYawRate_Nested : [ 0.00, 0.00, 4.30]
# THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
# > For the (x,y,z) position
......
pps_ws/src/d_fall_pps/src/nodes/MpcControllerService.cpp
View file @ 5289bce5
......@@ -106,7 +106,7 @@ using namespace std;
#define ESTIMATOR_METHOD_POINT_MASS_PER_DIMENSION 2 // (DEFAULT)
#define ESTIMATOR_METHOD_QUADROTOR_MODEL_BASED 3
int estimator_method = ESTIMATOR_METHOD_POINT_MASS_PER_DIMENSION;
int estimator_method = ESTIMATOR_METHOD_FINITE_DIFFERENCE;
// Boolean indiciating whether the debugging ROS_INFO_STREAM should be displayed or not
bool shouldDisplayDebugInfo = false;
......@@ -269,7 +269,7 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle);
void processFetchedParameters();
//void customYAMLasMessageCallback(const CustomControllerYAML& newCustomControllerParameters);
void angleControlledCrazyflie(float stateInertial[12], float input_angle[4], Controller::Response &response);
void angleControlledCrazyflie(float stateInertial[12], float raw_input[4], Controller::Response &response);
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]);
......@@ -398,12 +398,22 @@ void initialize_MPC_variables(int N, VectorXtype x0);
//
// This function WILL NEED TO BE edited for successful completion of the PPS exercise
void angleControlledCrazyflie(float stateInertial[12], float input_angle[4], Controller::Response &response)
float saturateThrust(float input)
{
float roll_sp = input_angle[0];
float pitch_sp = input_angle[1];
float yaw_sp = input_angle[2];
float ft_sp = input_angle[3];
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];
// initialize variables that will contain w_x, w_y and w_z
float w_x_sp = 0;
......@@ -416,6 +426,11 @@ void angleControlledCrazyflie(float stateInertial[12], float input_angle[4], Con
stateInertial[7] - pitch_sp,
stateInertial[8] - yaw_sp
};
// ROS_INFO_STREAM("stateInertial[6]" << stateInertial[6]);
// ROS_INFO_STREAM("stateInertial[7]" << stateInertial[7]);
// ROS_INFO_STREAM("stateInertial[8]" << stateInertial[8]);
// Perform the "-Kx" LQR computation for the rates and thrust:
for(int i = 0; i < 3; ++i)
{
......@@ -441,10 +456,10 @@ void angleControlledCrazyflie(float stateInertial[12], float input_angle[4], Con
// 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.
response.controlOutput.motorCmd1 = computeMotorPolyBackward(ft_sp/4.0);
response.controlOutput.motorCmd2 = computeMotorPolyBackward(ft_sp/4.0);
response.controlOutput.motorCmd3 = computeMotorPolyBackward(ft_sp/4.0);
response.controlOutput.motorCmd4 = computeMotorPolyBackward(ft_sp/4.0);
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));
// Specify that this controller is a rate controller
// response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
......@@ -462,11 +477,31 @@ void angleControlledCrazyflie(float stateInertial[12], float input_angle[4], Con
}
int outer_loop_counter = 0;
double raw_input[4];
float raw_input[4];
clock_t begin_time = 0;
clock_t end_time = 0;
ros::Time begin_t;
bool calculateControlOutput(Controller::Request &request, Controller::Response &response)
{
// end_time = clock();
// double elapsed_secs = double(end_time - begin_time) / CLOCKS_PER_SEC;
// std::cout << "elapsed secs: " << elapsed_secs << std::endl;
// begin_time = clock();
double elapsed_secs = ros::Time::now().toSec() - begin_t.toSec();
std::cout << "elapsed secs: " << elapsed_secs << std::endl;
begin_t = ros::Time::now();
// This is the "start" of the outer loop controller, add all your control
// computation here, or you may find it convienient to create functions
......@@ -482,6 +517,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// NO, we dont have to convert into body frame, that was only when the linearization was done around hover with yaw = 0.
// we relinearize in every point
if(outer_loop_counter == 0)
{
......@@ -492,9 +528,9 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// first, we have full state measurement stored in stateInertialEstimate.
// Apply setpoint change first, and store everything in x0 vector
x0 << -stateInertialEstimate[0] + setpoint[0],
-stateInertialEstimate[1] + setpoint[1],
-stateInertialEstimate[2] + setpoint[2],
x0 << stateInertialEstimate[0] - setpoint[0],
stateInertialEstimate[1] - setpoint[1],
stateInertialEstimate[2] - setpoint[2],
stateInertialEstimate[3],
stateInertialEstimate[4],
stateInertialEstimate[5];
......@@ -516,9 +552,9 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// Now update U_tray using the same loop
if(j < N-1)
U_tray[j] = U_0.segment(j+1, N_u);
U_tray[j] = U_0.segment(N_u*(j+1), N_u);
else
U_tray[j] = U_0.segment(j, N_u); // last element of the new U_tray is assumed to be the same as second to last
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
}
// X_tray and U_tray have been completely updated with the new states coming from the new sequence of inputs
......@@ -526,10 +562,28 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
get_matrices_linearization_affine(X_tray, U_tray, params, A_tray, B_tray, g_tray);
// 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);
raw_input[0] = (float)u(0);
raw_input[1] = (float)u(1);
raw_input[2] = (float)u(2);
raw_input[3] = (float)u(3);
// std::cout << "roll_MPC_input: " << raw_input[0] << std::endl;
// std::cout << "pitch_MPC_input: " << raw_input[1] << std::endl;
// std::cout << "yaw_MPC_input: " << raw_input[2] << std::endl;
// std::cout << "ft_MPC_input: " << raw_input[3] << std::endl;
// Test zone
// float correction_z = 0.82*(0.4 - stateInertialEstimate[2]) + 0.22 * (0 - stateInertialEstimate[5]) + params.m*params.g;
// float correction_roll = -0.31*(0-stateInertialEstimate[1]) - 0.25 * (0 - stateInertialEstimate[4]);
// raw_input[0] = correction_roll;
// raw_input[1] = setpoint[0];
// raw_input[2] = 0;
// raw_input[3] = correction_z;
}
outer_loop_counter++;
......@@ -541,17 +595,12 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// ============= END MPC PART ===============
// test:
float x0_full_state[12] = {-setpoint[0], -setpoint[1], -setpoint[2], 0, 0, 0, 0, 0, 0, 0, 0, 0};
// just for debugging purposes: control z axis with some PD gain, check how the angle setpoint works
float correction_z = -0.82*(0.4 - stateInertialEstimate[2]) - 0.22 * (0 - stateInertialEstimate[5]) + cf_mass/1000*9.8;
float input_angle[4] = {-setpoint[0], 0, 0, correction_z};
// create a function that takes as input angle references, like a crazyflie entity with input angles
// This function has to run at 200Hz
angleControlledCrazyflie(stateInertialEstimate, input_angle, response);
angleControlledCrazyflie(stateInertialEstimate, raw_input, response);
......@@ -734,9 +783,9 @@ void setpointCallback(const Setpoint& newSetpoint)
setpoint[2] = newSetpoint.z;
setpoint[3] = newSetpoint.yaw;
x0 << -stateInertialEstimate[0] + setpoint[0],
-stateInertialEstimate[1] + setpoint[1],
-stateInertialEstimate[2] + setpoint[2],
x0 << stateInertialEstimate[0] - setpoint[0],
stateInertialEstimate[1] - setpoint[1],
stateInertialEstimate[2] - setpoint[2],
stateInertialEstimate[3],
stateInertialEstimate[4],
stateInertialEstimate[5];
......@@ -747,9 +796,20 @@ void setpointCallback(const Setpoint& newSetpoint)
// clock_t begin_time = clock();
// // Do the MPC step:
// x0 << - setpoint[0],
// - setpoint[1],
// 0,
// 0,
// 0,
// 0;
// U_0 = mympc_varying_another(A_tray, B_tray, g_tray, Q, R, P, N, x0, X_ref, U_ref, 0, 0, 0);
// VectorUtype u = U_0.tail(4); // this is the input to apply next
// std::cout << "U_0 opt for this setpoint, 1 step MPC" << std::endl;
// std::cout << U_0.format(CleanFmt) << std::endl << std::endl;
// // prediction of next N states:
// VectorXtype x_approx_next;
// std::vector<VectorXtype> x_approx_temp;
......@@ -763,15 +823,35 @@ void setpointCallback(const Setpoint& newSetpoint)
// // Now update U_tray using the same loop
// if(j < N-1)
// U_tray[j] = U_0.segment(j+1, N_u);
// U_tray[j] = U_0.segment(N_u*(j+1), N_u);
// else
// U_tray[j] = U_0.segment(j, N_u); // last element of the new U_tray is assumed to be the same as second to last
// 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
// }
// // X_tray and U_tray have been completely updated with the new states coming from the new sequence of inputs
// 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;
// // Relinearize around new trayectory:
// get_matrices_linearization_affine(X_tray, U_tray, params, A_tray, B_tray, g_tray);
// 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;
// // update the current input to the middle loop
// raw_input[0] = u(0);
// raw_input[1] = u(1);
......@@ -1079,7 +1159,7 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
// Add the "CustomController" namespace to the "nodeHandle"
ros::NodeHandle nodeHandle_for_MpcController(nodeHandle, "MpcController");
// > The mass of the crazyflie
// > The mass of the crazyflieflieflie
cf_mass = getParameterFloat(nodeHandle_for_MpcController , "mass");
// Display one of the YAML parameters to debug if it is working correctly
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment