To receive notifications about scheduled maintenance, please subscribe to the mailing-list gitlab-operations@sympa.ethz.ch. You can subscribe to the mailing-list at https://sympa.ethz.ch

Commit 2e84fe3c authored by roangel's avatar roangel
Browse files

Loop is closed now. Need to test it and properly update x0 with the setpoint

parent 6352d279
......@@ -39,4 +39,3 @@ void funRbar(Eigen::MatrixXd R, int N, Eigen::MatrixXd &Rbar);
Eigen::VectorXd mympc_varying_another(std::vector<MatrixAtype> A_tray, std::vector<MatrixBtype> B_tray, std::vector<VectorXtype> g_tray, Eigen::MatrixXd Q, Eigen::MatrixXd R, Eigen::MatrixXd P, int N, VectorXtype x0, Eigen::VectorXd X_ref, Eigen::VectorXd U_ref, float ft_min, float ft_max, float angle_constraint);
Eigen::VectorXd solve_QP(Eigen::MatrixXd H, Eigen::MatrixXd F);
......@@ -178,6 +178,10 @@ std::vector<VectorXtype> g_tray;
std::vector<VectorXtype> X_tray;
std::vector<VectorUtype> U_tray;
Eigen::VectorXd U_0;
params_t params;
VectorXtype x0;
......@@ -259,7 +263,7 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle);
void processFetchedParameters();
//void customYAMLasMessageCallback(const CustomControllerYAML& newCustomControllerParameters);
void angleControlledCrazyflie(float stateInertial[12], float input_angle[4], int Ts_div, Controller::Response &response);
void angleControlledCrazyflie(float stateInertial[12], float input_angle[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]);
......@@ -388,7 +392,7 @@ void initialize_MPC_variables(int N);
//
// This function WILL NEED TO BE edited for successful completion of the PPS exercise
void angleControlledCrazyflie(float stateInertial[12], float input_angle[4], int Ts_div, Controller::Response &response)
void angleControlledCrazyflie(float stateInertial[12], float input_angle[4], Controller::Response &response)
{
float roll_sp = input_angle[0];
float pitch_sp = input_angle[1];
......@@ -451,6 +455,8 @@ void angleControlledCrazyflie(float stateInertial[12], float input_angle[4], int
}
}
int outer_loop_counter = 0;
double raw_input[4];
bool calculateControlOutput(Controller::Request &request, Controller::Response &response)
{
......@@ -465,9 +471,68 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
perform_estimator_update_state_interial(request, stateInertialEstimate);
// The estimate of the state is inside stateInertialEstimate now. Next, compute the error
// The estimate of the state is inside stateInertialEstimate now. Next, MPC
// Should we convert into body frame for our MPC controller? Let's skip it for now, YAW is going to be zero in our case
// 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)
{
// ================= MPC PART ==================
// Make sure we run it at 50Hz
// 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],
stateInertialEstimate[3],
stateInertialEstimate[4],
stateInertialEstimate[5];
// Do the MPC step:
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
// prediction of next N states:
VectorXtype x_approx_next;
std::vector<VectorXtype> x_approx_temp;
x_approx_next << x0;
for(int j = 0; j < N; j++)
{
euler_method_forward(F_crazyflie_6_states, 0, params.Ts/4, params.Ts, x_approx_next, U_0.segment(j,N_u), params, x_approx_temp);
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)
U_tray[j] = U_0.segment(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
}
// X_tray and U_tray have been completely updated with the new states coming from the new sequence of inputs
// Relinearize around new trayectory:
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[4] = {u(0), u(1), u(2), u(3)};
}
outer_loop_counter++;
// wrap the counter:
if(outer_loop_counter >= 4) //200 Hz/4 = 50 Hz
outer_loop_counter = 0;
// ============= 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
......@@ -476,7 +541,8 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
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
angleControlledCrazyflie(stateInertialEstimate, input_angle, 1, response);
// This function has to run at 200Hz
angleControlledCrazyflie(stateInertialEstimate, input_angle, response);
......@@ -1180,6 +1246,8 @@ int main(int argc, char* argv[]) {
// in the "DEFINES" section at the top of this file.
ros::NodeHandle namespace_nodeHandle(ros::this_node::getNamespace());
initialize_MPC_variables(N);
// Print out some information to the user.
ROS_INFO("[MPC CONTROLLER] Service ready :-)");
......
Markdown is supported
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