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 aaa8c08d authored by elokdae's avatar elokdae

Implemented MPC controller which uses same interface as DeePC and can be selected to run instead

parent 4b3d11bd
......@@ -7054,3 +7054,6 @@ Gurobi 8.1.1 (linux64) logging started Sat Jun 15 00:34:14 2019
Gurobi 8.1.1 (linux64) logging started Sat Jun 15 01:18:40 2019
Gurobi 8.1.1 (linux64) logging started Mon Oct 14 22:51:27 2019
This diff is collapsed.
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
This diff is collapsed.
0.295719,0.295053,0.294607,0.294638,0.294354,0.29495,0.294384,0.294288,0.294391,0.294926,0.294808,0.294346,0.294588,0.294281,0.294971,0.295266,0.295212,0.294917,0.294858,0.294918,0.295208,0.295269,0.295455,0.294611,0.294542,0.294548,0.294388,0.294408,0.294901,0.295281,0.295187,0.294892,0.29559,0.29629,0.29584,0.296067,0.296512,0.29717,0.296415,0.296539,0.29638,0.297176,0.297164,0.29665,0.296602,0.296251,0.296507,0.296322,0.296058,0.295738,0.295597,0.295458,0.295922,0.295577,0.294863,0.294789,0.295125,0.295696,0.295103,0.294694,0.295803,0.294661,0.294506,0.295447,0.295103,0.295185,0.295317,0.29584,0.295156,0.295235,0.296298,0.295266,0.295733,0.296003,0.295932,0.29577,0.295696,0.296497,0.296688,0.296477,0.29672,0.296714,0.296196,0.29591,0.295869,0.295839,0.294925,0.295328,0.295345,0.294455,0.294747,0.295606,0.294647,0.294719,0.294564,0.294486,0.295274,0.29497,0.294706,0.294534,0.294642,0.294957,0.295817,0.295511,0.29595,0.296324,0.297381,0.296752,0.297676,0.297858,0.298515,0.298204,0.297564,0.297301,0.296831,0.297337,0.296447,0.296331,0.295793,0.295437,0.295523,0.295599,0.295053,0.295373,0.295109,0.294857,0.294785
-0.00320654,0.00617304,0.00732829,0.0158676,0.0169295,0.0369859,-0.00163085,-0.0332692,-0.0408721,-0.0175585,-0.00498144,-0.0114825,-0.00456382,-0.0191679,0.0125758,0.0274231,-0.000841193,-0.0325028,-0.0461544,-0.0320753,-0.0203547,0.029319,0.0661862,0.0276001,0.00885172,0.0354055,0.0327189,0.0117661,0.00719341,-0.00772744,-0.0274892,-0.0287053,-0.00416456,-0.0206566,-0.0558439,-0.0691027,-0.0264713,0.0519347,0.0217712,-0.00354078,-0.0121779,0.0279782,0.0242875,-0.0073011,-0.0045078,-0.0172869,-0.00961496,0.0098616,-0.0214613,-0.0287502,-0.023504,-0.0148787,0.0222154,0.0222669,0.00561806,-0.0215273,0.00811075,0.031343,0.025387,0.00404561,0.047585,0.0162275,-0.0175172,7.61971e-05,-0.0216396,-0.0410865,-0.0198322,0.00268099,-0.00732196,-0.00994278,0.0205363,0.016869,-0.00778444,0.0226562,0.0224805,-0.019235,-0.0422161,-0.0421247,-0.049905,-0.0650911,-0.0633748,-0.0306739,-0.0103716,-0.00859654,0.0269326,0.0550237,0.0150956,-0.00350649,0.0380459,0.0240962,0.0234814,0.0597157,0.00384513,0.00236057,0.00077752,0.0117269,0.0228927,-0.0174957,-0.0545957,-0.0716368,-0.0444132,-0.0185813,0.0263449,0.000996463,-0.020611,0.00550349,0.018436,-0.0231896,-0.0282253,-0.0220855,0.0105994,0.0141679,-0.0286391,-0.0431397,-0.0523204,-0.00594502,-0.0101331,-0.036236,-0.0104878,0.00779524,0.0418798,0.0689576,0.0466923,0.0339086,0.0197817,0.00943818,-0.0323263
0.000358153,-0.00289555,-0.00784643,-0.017671,-0.0286318,-0.0149518,-0.0116997,-0.00988712,-0.023563,-0.0179053,-0.00541136,-0.018181,-0.0277817,-0.0317583,-0.00503248,0.00553561,-0.0088065,-0.0207595,-0.0216256,-0.0195687,-0.0136818,-0.0069424,0.00627165,0.00413951,-0.00211207,-0.0106428,-0.0102803,-0.00141922,0.0075268,0.00398449,-0.00518514,-0.00915559,0.00480308,0.0105196,0.00718779,0.0157587,0.0247608,0.0334617,0.0165299,0.00703793,-0.000870444,0.0116723,0.019928,0.00837347,-0.00086838,-0.00999752,-0.00199798,-0.000468452,-0.00742368,-0.0064707,0.000937361,0.000325955,0.00121183,-0.002153,0.00102783,-0.0119854,-0.0151322,0.000826754,0.00461875,-0.00978499,0.00117809,-0.00444913,-0.00820177,0.00310361,-0.00145612,0.00434905,0.0113376,0.0160925,0.00997044,0.00632391,0.0201324,0.0104286,0.00805251,0.0182461,0.0193125,0.00353203,-0.00845092,0.00679793,-0.00326284,-0.0146205,-0.0148011,0.000424661,0.00106436,-0.0102707,-0.0018586,0.00554042,0.00188087,-0.0105818,-0.0138589,-0.0252224,-0.029102,-0.00851708,-0.0119827,-0.0171691,-0.0338528,-0.0411909,-0.0196862,-0.00407406,2.27615e-06,-0.00925288,-0.00663285,-0.00147426,0.0133941,-0.00435255,-0.0132025,-0.0130783,0.00839273,-0.00477837,-0.0109114,-0.0255978,-0.0184318,-0.00571747,-0.017768,-0.0173319,-0.0207155,-0.0133787,-0.0208809,-0.0214589,-0.0196199,-0.0203459,-0.0154766,-0.00222419,-0.00105938,0.00376848,-0.000715684,-0.00345728,-0.00162495
-0.0988697,-0.0896142,-0.0850647,-0.0861951,-0.0824996,-0.0927261,-0.0769945,-0.0751613,-0.0673753,-0.0820234,-0.0787114,-0.0721208,-0.0693623,-0.0616381,-0.0762853,-0.0807141,-0.0672033,-0.0525539,-0.0480151,-0.0473203,-0.0490434,-0.0669186,-0.0780122,-0.0613007,-0.0516715,-0.0537153,-0.052666,-0.062535,-0.0752209,-0.0765333,-0.0708743,-0.0645602,-0.07982,-0.0829959,-0.0693092,-0.0690864,-0.0814642,-0.104536,-0.0920734,-0.0897828,-0.0854736,-0.100377,-0.0893618,-0.0761483,-0.0737423,-0.0677318,-0.0746059,-0.0797352,-0.0681587,-0.063375,-0.0633808,-0.0653263,-0.0827996,-0.0781208,-0.0755978,-0.0717001,-0.0806746,-0.0835283,-0.0799971,-0.0692315,-0.091345,-0.0704084,-0.0637302,-0.0861301,-0.0770045,-0.0725311,-0.0870416,-0.0968477,-0.0813064,-0.0861205,-0.0974534,-0.0827231,-0.0821033,-0.101977,-0.0945286,-0.0860476,-0.073217,-0.078461,-0.068826,-0.0522706,-0.0501781,-0.0483172,-0.0495371,-0.0479877,-0.0607724,-0.0676948,-0.0445785,-0.0462057,-0.0550885,-0.0531298,-0.0571355,-0.060151,-0.042576,-0.0459242,-0.042865,-0.0411972,-0.063037,-0.0660163,-0.0584746,-0.0507599,-0.04996,-0.0565193,-0.0775045,-0.062799,-0.0624092,-0.0704474,-0.0891724,-0.0674795,-0.0598828,-0.0541679,-0.0789878,-0.082212,-0.0711915,-0.0685522,-0.0660923,-0.0859057,-0.0767009,-0.0714168,-0.0694482,-0.0569192,-0.0604909,-0.0551838,-0.0412961,-0.0324812,-0.0344016,-0.0341992,-0.0298787
0.554964,0.30341,0.29625,0.284711,0.277624,0.274901,0.274074,0.275705,0.278795,0.282382,0.285621,0.289214,0.293741,0.29655,0.299884,0.303349,0.305103,0.306545,0.308323,0.308311,0.308182,0.309233,0.308175,0.307371,0.306881,0.306589,0.304607,0.304183,0.304085,0.303009,0.302486,0.302181,0.302533,0.301443,0.301757,0.302096,0.301404,0.30197,0.302367,0.301837,0.301796,0.302397,0.302565,0.302706,0.303286
-1.58488,-0.113686,0.229705,0.276704,0.12217,0.0617981,0.28141,0.188924,0.0951116,0.0575235,0.154365,0.167161,0.0959161,0.0585522,0.0947094,0.0547597,0.0169,0.0454339,0.0743717,0.0512857,0.0447749,0.0208388,0.0319748,-0.00422177,0.0415562,-0.0196388,-0.0360889,0.0230009,0.0143925,-0.0186337,-0.0159241,0.00397944,-0.0269096,-0.00279455,0.0179273,0.00521443,0.0102629,0.00695287,-0.0367622,-0.0539738,-0.0198104,-0.0152108,-0.0108445,0.0185058,0.0215284
-0.433865,-0.0117145,0.0370122,0.0686375,0.0397981,0.00137681,-0.0157544,0.016349,0.0466593,0.0173894,-0.011303,-0.00754612,0.0205642,0.0181241,-0.00912905,-0.00425692,0.010791,0.00433273,-0.00316856,-0.00929583,-0.00899237,-0.0108354,0.00936186,-0.0102826,-0.00388245,0.00720446,-0.00743359,-0.00738731,-0.000631544,-0.00913039,-0.0222792,-0.0228987,-0.00114237,-0.00667276,-0.0148001,-0.0154229,-0.026546,-0.0168069,-0.00788943,-0.0239564,-0.0367143,-0.0166018,-0.00459529,-0.00484787,-0.00319256
-0.00324835,-0.0159512,-0.0342972,-0.0442791,-0.0239896,-0.0212589,-0.0623721,-0.0371048,-0.0360356,-0.0204696,-0.048055,-0.0478022,-0.0229786,-0.0322031,-0.0492384,-0.034315,-0.0208277,-0.0323436,-0.037719,-0.0467556,-0.0363535,-0.0440507,-0.0427918,-0.0441648,-0.0481631,-0.0384424,-0.0459875,-0.0502865,-0.0523132,-0.0459438,-0.0497201,-0.0538958,-0.0372077,-0.0570564,-0.0591684,-0.0528987,-0.0610371,-0.0517623,-0.0437456,-0.0488873,-0.0616013,-0.0527418,-0.0501508,-0.05983,-0.0540912
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
......@@ -60,4 +60,5 @@
// OPTIMIZATION SOLVERS
#define DEEPC_CONTROLLER_SOLVER_GUROBI 0
#define DEEPC_CONTROLLER_SOLVER_OSQP 1
\ No newline at end of file
#define DEEPC_CONTROLLER_SOLVER_OSQP 1
#define DEEPC_CONTROLLER_SOLVER_MPC 2
\ No newline at end of file
......@@ -569,6 +569,22 @@ ros::Publisher m_manoeuvreCompletePublisher;
// each function is implemented before it is called from another function,
// hence why the "main" function is at the bottom.
// MPC VARIABLES
MatrixXf d_mpc_q;
c_float* d_mpc_q_new;
c_float* d_mpc_l_new;
c_float* d_mpc_u_new;
MatrixXf m_previous_xyz = MatrixXf::Zero(3, 1);
MatrixXf m_current_state_estimate = MatrixXf::Zero(8, 1);
MatrixXf d_current_state_estimate = MatrixXf::Zero(8, 1);
MatrixXf s_current_state_estimate = MatrixXf::Zero(8, 1);
// MPC FUNCTIONS
void change_Deepc_setpoint_mpc();
void change_Deepc_setpoint_mpc_changing_ref();
void setup_Deepc_mpc();
void solve_Deepc_mpc();
// DEEPC FUNCTIONS
void Deepc_thread_main();
void change_Deepc_params();
......
......@@ -117,7 +117,7 @@ R : [160.0, 4.0, 4.0, 2.0]
P : [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
# Regularization parameters
#lambda2_g : 500.0
lambda2_g : 500.0
#lambda2_g : 75.0
#lambda2_g : 0.001
lambda2_s : 750000000.0
......@@ -131,7 +131,7 @@ input_min : [0.1597, -1.5708, -1.5708, -1.5708]
input_max : [0.4791, 1.5708, 1.5708, 1.5708]
# Optimization parameters
solver : osqp
solver : mpc
opt_sparse : true
opt_verbose : false
opt_steady_state : false # Available in sparse formulation only, ignored otherwise
......
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