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

first try of multithreading using atomic library. Kind of works but still have...

first try of multithreading using atomic library. Kind of works but still have problems with protection. Next: mutex
parent de9e567a
......@@ -227,6 +227,10 @@ include_directories(
${GUROBI_HOME}/include
)
find_library(ATOMIC_LIBRARY
NAMES atomic
libatomic
PATHS "/home/ppsteacher/Downloads/libatomic")
find_library( GUROBI_LIBRARY
NAMES gurobi
......@@ -359,7 +363,7 @@ target_link_libraries(PPSClient ${catkin_LIBRARIES})
target_link_libraries(SafeControllerService ${catkin_LIBRARIES})
target_link_libraries(DemoControllerService ${catkin_LIBRARIES})
target_link_libraries(StudentControllerService ${catkin_LIBRARIES})
target_link_libraries(MpcControllerService ${catkin_LIBRARIES} Eigen3::Eigen ${GUROBI_LIBRARIES})
target_link_libraries(MpcControllerService ${catkin_LIBRARIES} Eigen3::Eigen ${GUROBI_LIBRARIES} ${ATOMIC_LIBRARY})
target_link_libraries(CentralManagerService ${catkin_LIBRARIES})
target_link_libraries(ParameterService ${catkin_LIBRARIES})
......
......@@ -342,8 +342,9 @@ VectorXd mympc_varying_another(std::vector<MatrixAtype> A_tray, std::vector<Matr
// W(1,1) = 1/(af^2);
// W(2,2) = 1/(af^2);
// Eigen::VectorXd U_0 = solve_QP(H, F, Au, bu, af);
Eigen::VectorXd U_0 = solve_QP(H, F, Au, bu);
Eigen::VectorXd U_0 = solve_QP(H, F, Au, bu, af);
// Eigen::VectorXd U_0 = solve_QP(H, F, Au, bu);
// Eigen::VectorXd U_0 = solve_QP(H, F);
return U_0;
}
......@@ -595,8 +596,6 @@ VectorXd solve_QP(Eigen::MatrixXd H, Eigen::MatrixXd F, Eigen::MatrixXd A, Eigen
std::vector<double> l_coeff;
/* Create environment */
error = GRBloadenv(&env, "qp.log");
if (error) goto QUIT;
......@@ -681,12 +680,14 @@ QUIT:
// END C INTERFACE
Map<VectorXd> U_0(sol,N_vars);
// Eigen::IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");
Map<VectorXd> U_0(sol,N_vars);
// cout << "The mapped vector U_0 is: \n" << U_0.format(CleanFmt) << "\n";
return U_0;
}
VectorXd solve_QP(Eigen::MatrixXd H, Eigen::MatrixXd F)
{
// H is assumed to be symmetric. There is NO 0.5 factor anywhere, with this we solve:
......@@ -699,11 +700,7 @@ VectorXd solve_QP(Eigen::MatrixXd H, Eigen::MatrixXd F)
std::vector<double> q_val;
std::vector<double> l_obj_coeff;
int N_vars = size_H;
int N_q_coeffs = q_val.size();
for(int i = 0; i < size_H; i++)
for(int i = 0; i < size_H; i++)
{
for(int j = i; j < size_H; j++)
{
......@@ -729,6 +726,10 @@ VectorXd solve_QP(Eigen::MatrixXd H, Eigen::MatrixXd F)
}
int N_vars = size_H;
int N_q_coeffs = q_val.size();
// BEGINNING C INTERFACE:
GRBenv *env = NULL;
......@@ -746,9 +747,6 @@ VectorXd solve_QP(Eigen::MatrixXd H, Eigen::MatrixXd F)
std::vector<double> ub(N_vars, GRB_INFINITY);
std::vector<double> lb(N_vars, -GRB_INFINITY);
/* Create environment */
error = GRBloadenv(&env, "qp.log");
if (error) goto QUIT;
......@@ -817,10 +815,10 @@ QUIT:
// END C INTERFACE
Map<VectorXd> U_0(sol,N_vars);
// Eigen::IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");
Map<VectorXd> U_0(sol,N_vars);
// cout << "The mapped vector U_0 is: \n" << U_0.format(CleanFmt) << "\n";
return U_0;
}
......@@ -51,6 +51,8 @@
#include <ros/package.h>
#include <ctime>
#include <atomic>
#include <mutex>
//the generated structs from the msg-files have to be included
#include "d_fall_pps/ViconData.h"
......@@ -67,6 +69,8 @@
#include "MPC_functions.h"
#include <boost/thread/thread.hpp>
using namespace std;
......@@ -155,7 +159,7 @@ 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
float stateInertialEstimate[12] = {0}; //this variable will contain the estimation of the current state
// Variables for controller
......@@ -192,7 +196,12 @@ params_t params;
VectorXtype x0;
int N = 5;
std::atomic<std::array<float,N_u>> shared_raw_input;
std::atomic<array<float,N_x>> shared_x0;
std::mutex MPC_mutex;
int N = 2;
// ROS Publisher for debugging variables
......@@ -477,6 +486,7 @@ void angleControlledCrazyflie(float stateInertial[12], float raw_input[4], Contr
}
int outer_loop_counter = 0;
float raw_input[4];
clock_t begin_time = 0;
......@@ -485,6 +495,73 @@ clock_t end_time = 0;
ros::Time begin_t;
DebugMsg debugMsg;
// float stateInertialEstimate[12];
void do_MPC(int* publish_rate)
{
ros::Rate loop_rate(*publish_rate);
while(ros::ok())
{
// Put the MPC loop here:
// load x0 from shared one
std::array<float, N_x> x0_loaded = shared_x0.load(std::memory_order_acquire);
VectorXtype x0_local;
x0_local << x0_loaded[0],
x0_loaded[1],
x0_loaded[2],
x0_loaded[3],
x0_loaded[4],
x0_loaded[5];
// 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++)
{
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(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
}
// 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
std::array<float,4> temp_input;
temp_input[0] = (float)u(0);
temp_input[1] = (float)u(1);
temp_input[2] = (float)u(2);
temp_input[3] = (float)u(3);
shared_raw_input.store(temp_input,std::memory_order_release);
// 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();
}
}
bool calculateControlOutput(Controller::Request &request, Controller::Response &response)
{
......@@ -498,11 +575,11 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// std::cout << "elapsed secs: " << elapsed_secs << std::endl;
// begin_time = clock();
double elapsed_secs = ros::Time::now().toSec() - begin_t.toSec();
// double elapsed_secs = ros::Time::now().toSec() - begin_t.toSec();
std::cout << "elapsed secs: " << elapsed_secs << std::endl;
// std::cout << "elapsed secs: " << elapsed_secs << std::endl;
begin_t = ros::Time::now();
// begin_t = ros::Time::now();
......@@ -512,7 +589,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// to keep you code cleaner
// Define a local array to fill in with the state error
float stateInertialEstimate[12];
perform_estimator_update_state_interial(request, stateInertialEstimate);
......@@ -521,6 +598,25 @@ 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
// read new x0
x0 << stateInertialEstimate[0] - setpoint[0],
stateInertialEstimate[1] - setpoint[1],
stateInertialEstimate[2] - setpoint[2],
stateInertialEstimate[3],
stateInertialEstimate[4],
stateInertialEstimate[5];
std::array<float, N_x> x0_temp;
x0_temp[0] = x0(0);
x0_temp[1] = x0(1);
x0_temp[2] = x0(2);
x0_temp[3] = x0(3);
x0_temp[4] = x0(4);
x0_temp[5] = x0(5);
shared_x0.store(x0_temp, std::memory_order_release);
if(outer_loop_counter == 0)
{
......@@ -532,44 +628,8 @@ 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],
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.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;
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(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
}
// 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[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;
......@@ -596,6 +656,15 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
if(outer_loop_counter >= 4) //200 Hz/4 = 50 Hz
outer_loop_counter = 0;
// read raw_input from shared_raw_input atomic variable
std::array<float,4> temp_input = shared_raw_input.load(std::memory_order_acquire);
raw_input[0] = temp_input[0];
raw_input[1] = temp_input[1];
raw_input[2] = temp_input[2];
raw_input[3] = temp_input[3];
// ============= END MPC PART ===============
......@@ -794,6 +863,17 @@ void setpointCallback(const Setpoint& newSetpoint)
stateInertialEstimate[4],
stateInertialEstimate[5];
std::array<float, N_x> x0_temp;
x0_temp[0] = x0(0);
x0_temp[1] = x0(1);
x0_temp[2] = x0(2);
x0_temp[3] = x0(3);
x0_temp[4] = x0(4);
x0_temp[5] = x0(5);
shared_x0.store(x0_temp, std::memory_order_release);
initialize_MPC_variables(N, x0);
......@@ -1427,6 +1507,11 @@ int main(int argc, char* argv[]) {
initialize_MPC_variables(N, x0);
int rate_MPC = 50; //50 Hz
// create a 50Hz thread with what we need:
boost::thread MPC_thread(do_MPC, &rate_MPC);
// Print out some information to the user.
ROS_INFO("[MPC CONTROLLER] Service ready :-)");
......@@ -1434,5 +1519,7 @@ int main(int argc, char* argv[]) {
ros::spin();
// Return zero if the "ross::spin" is cancelled.
MPC_thread.join();
return 0;
}
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