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 94651076 authored by roangel's avatar roangel
Browse files

fixed setpoint problem, everything prepared for first unbounded MPC trial

parent 0e9a0cd2
......@@ -155,6 +155,8 @@ std::vector<float> gainMatrixYawRate_Nested (3,0.0);
float current_xzy_rpy_measurement[6];
float previous_xzy_rpy_measurement[6];
double stateInertialEstimate[12] = {0}; //this variable will contain the estimation of the current state
// Variables for controller
float cf_mass; // Crazyflie mass in grams
......@@ -490,9 +492,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];
......@@ -732,16 +734,6 @@ void setpointCallback(const Setpoint& newSetpoint)
setpoint[2] = newSetpoint.z;
setpoint[3] = newSetpoint.yaw;
// ================= 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
double stateInertialEstimate[12] = {0};
x0 << -stateInertialEstimate[0] + setpoint[0],
-stateInertialEstimate[1] + setpoint[1],
-stateInertialEstimate[2] + setpoint[2],
......@@ -751,50 +743,49 @@ void setpointCallback(const Setpoint& newSetpoint)
initialize_MPC_variables(N, x0);
clock_t begin_time = clock();
// clock_t begin_time = clock();
// // 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
// 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;
// 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
// 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);
// // 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[0] = u(0);
raw_input[1] = u(1);
raw_input[2] = u(2);
raw_input[3] = u(3);
// // 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);
clock_t end_time = clock();
double elapsed_secs = double(end_time - begin_time) / CLOCKS_PER_SEC;
// clock_t end_time = clock();
// double elapsed_secs = double(end_time - begin_time) / CLOCKS_PER_SEC;
// std::cout << "U_0 for this setpoint, 1 step MPC" << std::endl;
// std::cout << U_0.format(CleanFmt) << std::endl;
// // std::cout << "U_0 for this setpoint, 1 step MPC" << std::endl;
// // std::cout << U_0.format(CleanFmt) << std::endl;
std::cout << "elapsed secs: " << elapsed_secs << std::endl;
// std::cout << "elapsed secs: " << elapsed_secs << std::endl;
// testzone:
......@@ -1343,6 +1334,15 @@ int main(int argc, char* argv[]) {
// in the "DEFINES" section at the top of this file.
ros::NodeHandle namespace_nodeHandle(ros::this_node::getNamespace());
x0 << -stateInertialEstimate[0] + setpoint[0],
-stateInertialEstimate[1] + setpoint[1],
-stateInertialEstimate[2] + setpoint[2],
stateInertialEstimate[3],
stateInertialEstimate[4],
stateInertialEstimate[5];
initialize_MPC_variables(N, x0);
// 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