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 637a206e authored by elokdae's avatar elokdae
Browse files

Commit to use lab laptop

parent 5919af45
0.345449,0.199819,0.195071,0.197819,0.210045,0.223543,0.237892,0.25401,0.467312,0.28671,0.291074,0.487695,0.293733,0.286822,0.47684,0.471939,0.267774,0.24979,0.23738,0.232264,0.430183,0.434981,0.432372,0.223639,0.203363,0.386746,0.18692,0.379041,0.18261,0.180805,0.180065,0.387695,0.400095,0.208632,0.400214,0.396547,0.390202,0.37764,0.163525,0.145583,0.14089,0.147111,0.357642,0.182388,0.19573,0.209419,0.423892,0.244219,0.2534,0.260035,0.266897,0.47345,0.287981,0.484729,0.480524,0.470345,0.450209,0.425426,0.20135,0.369921,0.354305,0.341087,0.323547,0.308629,0.290952,0.0823844,0.267861,0.0740804,0.0787595,0.290532,0.119154,0.142506,0.164369,0.191946,0.222425,0.450198,0.477935,0.295568,0.29452,0.486589,0.292326,0.28547,0.280481,0.477009,0.48053,0.473633,0.259482,0.432682,0.216656,0.198385,0.189254,0.386961,0.198607,0.396149,0.398506,0.200041,0.18954,0.38328,0.387585,0.190935,0.379285,0.180466,0.374984,0.179466,0.374352,0.181541,0.378757,0.382986,0.187158,0.376314,0.374568,0.176956,0.36636,0.365336,0.362165,0.158086,0.342867,0.14401,0.341076,0.347329,0.351941,0.349648,0.145668,0.333713,0.333922,0.139158,0.138107,0.341238,0.160707,0.369209,0.381765,0.390445,0.193611,0.186043,0.183482,0.192891,0.20956,0.425348,0.250638,0.456783,0.26823,0.269108,0.465597,0.272859,0.466926,0.462489,0.258258,0.437888,0.229429,0.415434,0.408,0.202821,0.187325,0.179227,0.184872,0.195708,0.211198,0.4256,0.442936,0.450223,0.445198,0.429465,0.406211,0.383617,0.163538,0.142171,0.328639,0.135954,0.138517,0.343523,0.163834,0.37218,0.188089,0.391678,0.39974,0.403232,0.200896,0.384481,0.379235,0.1788,0.36693,0.167784,0.167205,0.168781,0.181189,0.39561,0.416086,0.231298,0.427606,0.232478,0.230942,0.427585,0.432204,0.433155,0.226124,0.406815,0.39616,0.387538,0.177087,0.160411,0.350818,0.35619,0.161998,0.159756,0.359001,0.372008,0.376089,0.176321,0.170578,0.169448,0.376409,0.392618,0.405176,0.405555,0.199971,0.187282,0.378315,0.384611,0.386237,0.380253,0.369696,0.158349,0.143517,0.139903,0.34501,0.166516,0.179251,0.391058,0.409604,0.419959,0.416663,0.210686,0.39329,0.189746,0.379643,0.18304,0.183407,0.384122,0.395429,0.205031,0.398033,0.399305,0.200788,0.192402,0.190743,0.395279,0.407618,0.217833,0.214036,0.213984,0.417262,0.231539,0.433139,0.240964,0.436156
1.23978,0.0997128,-0.54545,-0.327632,-0.0783052,0.0304135,-0.411562,-0.452035,0.0835512,0.219676,-0.280392,-0.330644,0.218726,0.343522,0.186154,-0.265674,-0.318777,-0.18541,0.258905,0.27855,0.191056,0.0853326,-0.308334,-0.239343,0.208536,0.1988,0.206777,0.114734,0.150544,-0.271464,-0.278739,-0.223605,0.206274,-0.108339,-0.143351,0.220098,0.289994,0.150401,0.0123834,-0.341997,0.139701,-0.0832663,0.263078,-0.152255,-0.211113,0.190319,0.241715,-0.287936,-0.0140444,0.130831,-0.216439,-0.290862,-0.152072,0.281509,0.238828,-0.136571,-0.191699,-0.0890941,0.338866,0.00119281,0.254118,-0.129314,0.133239,-0.13477,-0.139681,-0.137465,-0.121776,-0.030309,0.00600454,-0.102084,-0.0292421,0.450085,0.0968157,0.0447267,0.269626,-0.0219371,-0.108565,0.235955,0.269643,-0.203923,-0.301467,-0.228395,-0.186537,0.193803,0.288132,0.125821,-0.330282,-0.284505,0.131671,-0.175323,0.138277,-0.153241,-0.134889,-0.138739,0.202704,0.241858,-0.218652,0.11575,0.173226,0.137788,0.0158992,-0.322501,-0.258104,-0.205071,-0.164162,0.212907,-0.0494097,-0.0998922,-0.102784,0.306863,-0.0241975,-0.069519,-0.0467257,0.0331251,0.39269,0.0050762,0.232667,0.222079,0.210046,0.129405,0.0294445,-0.367336,0.00998963,0.13711,0.0852144,0.0124649,-0.0967703,-0.0200424,-0.306777,0.0778551,-0.3339,-0.32199,0.0772113,-0.152322,-0.227698,-0.167904,-0.0121506,0.0246437,0.303799,0.232041,-0.162634,-0.221497,0.175989,-0.050828,-0.108045,-0.119428,0.361543,0.394396,0.287981,-0.16293,0.208088,-0.112471,-0.203902,-0.155825,0.30829,0.0476892,0.269992,0.221757,-0.199437,-0.208968,0.199624,0.257001,-0.235353,-0.0231823,-0.281578,0.0297958,-0.309007,0.108578,-0.148047,0.16595,0.236941,-0.131614,0.0595135,0.109433,-0.227002,0.0820879,0.0943245,0.0581338,-0.332133,0.00842623,-0.25669,0.134449,0.221099,0.187031,0.0499628,-0.328901,0.0694521,0.147394,-0.314175,-0.36922,0.172133,-0.109809,0.22486,0.148913,0.0875362,-0.209463,-0.170245,-0.0887409,-0.0573002,-0.0373659,0.314739,0.0568664,0.307114,-0.0716656,-0.0429935,0.344378,-0.0303926,0.16296,0.221501,-0.110735,0.182871,-0.140349,0.160415,0.188508,-0.256614,-0.362014,-0.260969,-0.0872344,-0.146751,-0.151026,0.279124,0.323694,0.202855,0.0575121,0.0848906,0.187498,0.027968,-0.347088,-0.282599,0.143922,-0.196621,-0.238227,0.0892511,-0.0983992,0.186842,-0.175697,0.137924,0.215716,0.230038,-0.295386,0.0112542,0.191313,-0.222439,0.0508455,-0.240806,-0.177424,-0.0950212,-0.088388,0.275426,0.271087,-0.133397,0.130778,-0.236934,-0.118405,0.180787
-1.28315,0.22787,0.475528,0.645928,0.210416,-0.0317598,-0.143407,-0.0635836,0.0634917,0.453558,0.0439555,0.254494,-0.173259,-0.188962,0.212021,-0.104572,0.187857,0.108608,-0.30279,0.00453834,-0.286489,0.0991748,0.142573,-0.274089,-0.350595,-0.326746,-0.193715,-0.069958,-0.0250322,0.335819,0.341093,0.292575,0.215476,0.135352,0.0864312,0.0620323,-0.314638,-0.322834,0.0706726,-0.206371,-0.180026,0.182666,-0.106739,0.249032,-0.113675,0.219004,0.210168,0.19388,-0.215667,0.0644701,0.112914,-0.21879,0.0782868,-0.282661,-0.274255,-0.262779,-0.20302,0.176901,0.23027,-0.15635,0.109039,-0.238739,-0.223976,0.179434,0.239965,0.191785,-0.23067,0.0617094,0.136384,0.218573,-0.106115,-0.161959,0.168963,0.217293,-0.104325,-0.14201,0.205256,0.244861,0.219414,-0.230775,-0.295884,-0.257155,0.201924,0.272218,0.193292,0.0814291,-0.352334,-0.422186,0.0126684,0.135864,0.153903,0.0827524,0.0120413,-0.359946,-0.323899,-0.20721,0.250955,-0.0481031,-0.0827307,0.291426,0.33258,0.295855,0.187164,-0.250147,0.0725863,-0.213406,0.179607,-0.150616,-0.182654,0.186239,0.258009,-0.123765,0.108752,0.0739762,-0.274472,-0.268893,-0.232986,0.216867,0.296985,-0.0819807,-0.174702,-0.164003,0.260694,-0.0448948,0.282288,-0.108762,0.186165,-0.159443,-0.144789,-0.104823,-0.0508382,-0.0101643,-0.0182133,-0.0090265,-0.00496423,0.36977,0.0236138,-0.0277155,0.270869,-0.073757,-0.116132,0.193171,0.225151,-0.160574,-0.245267,-0.23221,-0.164449,0.262009,0.261782,0.174525,-0.293338,-0.347493,0.0770946,-0.133496,0.242644,-0.122257,-0.189366,-0.20088,0.245364,0.317874,-0.095009,0.126294,0.109506,0.0999953,0.0556412,-0.335887,-0.335498,-0.268141,-0.145523,0.297595,0.00111,-0.0325341,-0.099662,0.285052,-0.00236714,-0.0169588,-0.0569141,-0.0169128,0.399876,0.0553183,0.313829,0.248937,0.166629,0.100763,0.0611794,-0.317049,0.0380545,0.0864083,0.0925696,0.0275153,-0.0636412,-0.100958,-0.414082,-0.0672962,-0.375341,-0.359313,0.0503329,-0.192195,-0.17172,-0.161163,-0.11813,-0.0328135,0.385794,0.400961,-0.0401141,-0.157582,0.21396,-0.0599829,-0.0794318,-0.127379,0.230989,0.301498,0.298681,-0.161562,0.0749924,-0.251198,-0.207169,-0.15464,0.260683,-0.0676445,0.197552,0.159845,-0.21688,-0.249665,0.145206,0.217606,-0.151231,0.111365,-0.275139,0.0795915,-0.204151,0.155403,-0.209631,0.105846,0.136966,-0.190853,0.133322,0.147624,-0.193529,0.137354,0.126369,0.133018,-0.255239,0.0505801,-0.269292,0.123723,0.175089,0.153937,0.0649486,-0.35053,-0.00503395,0.0702762,-0.25242,-0.303256,0.0463361
-0.0625743,-0.0630699,-0.0765512,-0.0865402,-0.10532,-0.116144,-0.127826,-0.125256,-0.125201,-0.124387,-0.101832,-0.129502,-0.136428,-0.149276,-0.147876,-0.163371,-0.175808,-0.202831,-0.204868,-0.207602,-0.212734,-0.204349,-0.223018,-0.231925,-0.229018,-0.231768,-0.228106,-0.228907,-0.222106,-0.216912,-0.210716,-0.179534,-0.172489,-0.179148,-0.176037,-0.170175,-0.167735,-0.170343,-0.1662,-0.164716,-0.172714,-0.182158,-0.176024,-0.177594,-0.15507,-0.165418,-0.115377,-0.113013,-0.109408,-0.10623,-0.107736,-0.100117,-0.101055,-0.106612,-0.103671,-0.118517,-0.135299,-0.164766,-0.187557,-0.211631,-0.225141,-0.240462,-0.230075,-0.237416,-0.243663,-0.223069,-0.222535,-0.21435,-0.210065,-0.17943,-0.17697,-0.147931,-0.147843,-0.131134,-0.100097,-0.0887225,-0.0736745,-0.076011,-0.0767099,-0.0828597,-0.073758,-0.111756,-0.0818761,-0.102361,-0.135642,-0.111899,-0.157667,-0.185067,-0.192731,-0.211023,-0.208939,-0.213297,-0.219936,-0.208186,-0.201206,-0.205627,-0.200438,-0.197025,-0.185524,-0.176232,-0.18742,-0.17944,-0.182979,-0.162985,-0.157838,-0.148038,-0.148863,-0.143686,-0.138128,-0.132484,-0.146721,-0.141149,-0.143288,-0.154493,-0.138015,-0.14771,-0.145401,-0.136365,-0.136875,-0.142095,-0.12931,-0.133444,-0.139535,-0.130811,-0.132023,-0.125682,-0.105988,-0.112984,-0.115293,-0.113809,-0.108661,-0.103012,-0.0999926,-0.122349,-0.106783,-0.140426,-0.127842,-0.138953,-0.0964443,-0.105653,-0.122478,-0.0803802,-0.0778963,-0.0851942,-0.0875214,-0.0963495,-0.109744,-0.125829,-0.134155,-0.15692,-0.165622,-0.185842,-0.197564,-0.208428,-0.216714,-0.212262,-0.20434,-0.194391,-0.185864,-0.174654,-0.173334,-0.179261,-0.194273,-0.199669,-0.216341,-0.22275,-0.234577,-0.231092,-0.233098,-0.222552,-0.214783,-0.211459,-0.206315,-0.201457,-0.196483,-0.19196,-0.191999,-0.175977,-0.187858,-0.188682,-0.176296,-0.193685,-0.196048,-0.19525,-0.199637,-0.181674,-0.175088,-0.169502,-0.143434,-0.147341,-0.162552,-0.148178,-0.160028,-0.147083,-0.164622,-0.178214,-0.188992,-0.194301,-0.191545,-0.210413,-0.200353,-0.208615,-0.206459,-0.182532,-0.186194,-0.171178,-0.165685,-0.157483,-0.142596,-0.14933,-0.147261,-0.121774,-0.117873,-0.119307,-0.120736,-0.120213,-0.131942,-0.132087,-0.115105,-0.12567,-0.136575,-0.145225,-0.153384,-0.126767,-0.154809,-0.146314,-0.127413,-0.138432,-0.12327,-0.107829,-0.115363,-0.109187,-0.0995549,-0.128966,-0.134357,-0.130542,-0.139119,-0.135642,-0.134773,-0.115097,-0.113607,-0.115312,-0.117424,-0.11452,-0.118291,-0.111208,-0.104209,-0.0909288,-0.0879899,-0.0756166,-0.0761754,-0.0634553,-0.0657739,-0.0628125,-0.0596508
0.17658,0.19991,0.22019,0.23753,0.25213,0.2642,0.27397,0.47788,0.24091,0.44755,0.2135,0.42313,0.19206,0.40458,0.37245,0.14846,0.17148,0.38842,0.36017,0.13956,0.16546,0.18862,0.40525,0.18012,0.20144,0.22008,0.43235,0.20311,0.4168,0.38527,0.16144,0.38045,0.15784,0.18169,0.39903,0.37084,0.34619,0.12877,0.15746,0.37922,0.1588,0.18436,0.20678,0.22616,0.43885,0.20978,0.22723,0.43838,0.20811,0.42086,0.19229,0.21058,0.42281,0.19379,0.21168,0.42357,0.39046,0.16525,0.38307,0.35565,0.13581,0.35861,0.33573,0.31617,0.10351,0.13658,0.16613,0.38835,0.36426,0.34302,0.32457,0.11257,0.34213,0.1288,0.1607,0.18871,0.21295,0.2336,0.44711,0.41467,0.18938,0.40652,0.18178,0.39955,0.17548,0.19774,0.21722,0.43022,0.39781,0.17297,0.19468,0.41001,0.18375,0.40028,0.3714,0.34622,0.12837,0.35292,0.33157,0.11712,0.14843,0.17629,0.20073,0.41805,0.38935,0.36384,0.14529,0.36887,0.15014,0.37346,0.35061,0.33069,0.31359,0.10293,0.1376,0.36457,0.34483,0.32751,0.31253,0.29977,0.092889,0.32698,0.3138,0.30243,0.29277,0.2847,0.2781
-1.0472,-0.78434,1.5389,1.2103,-1.1739,-0.90168,1.4308,-0.983,1.3562,1.0434,0.76897,-1.564,0.85108,0.60894,-1.6944,-1.3467,-1.0388,-0.76821,1.5618,1.239,0.95388,-1.3902,1.0132,-1.3352,1.064,0.8057,0.58059,0.38632,-1.8738,-1.4875,0.94928,0.72448,0.52885,0.36026,0.21662,-1.9985,0.5218,0.37292,0.24582,0.13867,0.049695,-0.022839,-2.175,-1.6938,-1.269,-0.89721,-0.57522,-0.29962,-0.067021,2.2204,-0.24301,2.0432,-0.41936,1.8694,-0.58908,1.7052,1.3479,-1.0644,-0.81923,1.4877,1.1444,-1.2529,-0.99188,-0.76308,1.5302,-0.91863,-0.70476,-0.51889,1.7355,-0.74846,1.5283,1.1604,-1.2565,1.0836,-1.3191,-1.0602,1.2636,0.93962,0.65786,-1.679,-1.3594,1.02,-1.3477,-1.0581,-0.80215,-0.5778,1.7113,-0.74179,-0.53236,1.7431,-0.7222,1.5707,-0.8779,-0.66339,1.6178,-0.84141,-0.63629,1.6366,1.2646,-1.1567,1.1789,0.86584,-1.5003,0.88639,0.62053,0.39208,-1.8962,-1.5325,-1.2079,1.1742,0.90118,0.66341,0.45828,-1.8111,0.66169,-1.6228,-1.2596,-0.93903,-0.65873,-0.41614,1.8857,1.5348,-0.87241,1.4708,-0.93486,1.4103,-0.99297
-1.0472,1.3101,-1.0812,-0.81601,1.5096,-0.91106,1.4215,-0.99221,-0.74722,1.5603,-0.87637,-0.65223,1.6364,1.2773,-1.1337,1.2097,0.90233,-1.46,0.92896,0.66393,0.43514,-1.8545,-1.493,-1.1711,1.2077,0.93109,0.68948,0.4804,-1.793,0.67591,-1.6124,-1.2528,-0.93561,-0.65844,-0.41868,1.8806,1.5275,-0.8816,1.46,-0.94691,1.3973,-1.0067,-0.75371,-0.53415,1.7488,1.3831,-1.0353,-0.79408,1.5104,-0.92835,1.3888,1.0567,0.76565,-1.5814,0.82198,0.57036,-1.7404,-1.3983,-1.0942,-0.82594,1.503,1.1804,0.89656,-1.4455,0.96074,-1.3843,1.0186,0.76442,0.5436,0.35374,-1.9019,-1.5112,0.92979,0.70908,0.51732,0.35235,0.21205,-2,0.523,0.37652,0.25151,0.14614,0.05862,-0.012742,-2.164,-1.6822,-1.2569,-0.88501,-0.56304,-0.28762,-0.055367,2.2315,-0.2324,2.0532,-0.41014,1.8779,-0.58144,1.7121,1.3539,-1.0592,-0.81484,1.4913,1.1473,-1.2507,-0.99031,-0.76208,1.5307,-0.91863,-0.70518,-0.51966,1.7344,-0.74979,1.5268,1.1587,-1.2583,1.0818,-1.321,-1.0622,1.2617,0.93778,0.65609,-1.6807,-1.361,1.0186,-1.3491,-1.0593,-0.80322
This diff is collapsed.
......@@ -357,6 +357,9 @@ bool s_solveDeepc = false;
string d_logFolder = s_logFolder;
bool d_Deepc_measure_roll_pitch = true;
bool d_Deepc_yaw_control = true;
int d_Tini;
int d_N;
float d_lambda2_g;
int d_Nuini;
int d_Nyini;
int d_Ng;
......@@ -364,6 +367,13 @@ MatrixXf d_U_f;
MatrixXf d_Y_f;
MatrixXf d_Q;
MatrixXf d_P;
MatrixXf d_r;
MatrixXf d_r_gs;
MatrixXf d_A_gs;
MatrixXf d_b_gs;
MatrixXf d_gs;
MatrixXf d_grb_cg_r;
MatrixXf d_grb_cg_gs;
MatrixXf d_g;
MatrixXf d_uini;
MatrixXf d_yini;
......@@ -376,6 +386,8 @@ GRBModel d_grb_model = GRBModel(d_grb_env);
GRBVar* d_grb_vars = 0;
GRBQuadExpr d_grb_quad_obj = 0;
GRBLinExpr d_grb_lin_obj_us = 0;
GRBLinExpr d_grb_lin_obj_r = 0;
GRBLinExpr d_grb_lin_obj_gs = 0;
GRBConstr* d_grb_eq_constrs = 0;
// Deepc related global variables used by main thread only
......
......@@ -24,7 +24,6 @@ landing_spin_motors_time: 0
mass : 29
# Frequency of the controller, in hertz
#control_frequency : 200
control_frequency : 25.0
# Quadratic motor regression equation (a0, a1, a2)
......@@ -45,10 +44,6 @@ shouldPublishDebugMessage : false
shouldDisplayDebugInfo : false
# The LQR Controller parameters for rate mode
#gainMatrixThrust_NineStateVector : [ 0.00, 0.00, 0.98, 0.00, 0.00, 0.25, 0.00, 0.00, 0.00]
#gainMatrixRollRate : [ 0.00,-6.20, 0.00, 0.00,-3.00, 0.00, 5.20, 0.00, 0.00]
#gainMatrixPitchRate : [ 6.20, 0.00, 0.00, 3.00, 0.00, 0.00, 0.00, 5.20, 0.00]
#gainMatrixYawRate : [ 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.30]
gainMatrixThrust_NineStateVector : [ 0.00, 0.00, 0.45, 0.00, 0.00, 0.17, 0.00, 0.00, 0.00]
gainMatrixRollRate : [ 0.00,-2.79, 0.00, 0.00, -1.83, 0.00, 5.91, 0.00, 0.00]
gainMatrixPitchRate : [ 2.79, 0.00, 0.00, 1.83, 0.00, 0.00, 0.00, 5.91, 0.00]
......@@ -71,10 +66,10 @@ logFolder : log/
thrustExcAmp : 10.0
# Roll rate excitation magnitude, in deg/s
rollRateExcAmp : 10.0
rollRateExcAmp : 60.0
# Pitch rate excitation magnitude, in deg/s
pitchRateExcAmp : 10.0
pitchRateExcAmp : 60.0
# Yaw rate excitation magnitude, in deg/s
yawRateExcAmp : 60.0
......@@ -91,22 +86,22 @@ Deepc_measure_roll_pitch : false
Deepc_yaw_control : false
# Tini in discrete time steps
Tini : 3
Tini : 6
# Prediction horizon in discrete time steps
N : 20
N : 15
# Output cost matrix diagonal entries (x, y, z, x_dot, y_dot, z_dot, roll, pitch, yaw)
Q : [40.0, 40.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10.0]
# Input cost matrix diagonal entries (thrust, rollRate, pitchRate, yawRate)
R : [16.0, 4.0, 4.0, 1.0]
R : [4.0, 4.0, 4.0, 1.0]
# Terminal output cost matrix diagonal entries (x, y, z, x_dot, y_dot, z_dot, roll, pitch, yaw)
P : [657.21, 657.21, 8.88, 96.92, 96.92, 0.47, 629.60, 629.60, 84.21]
# Regularization parameters
lambda2_g : 10.0
lambda2_g : 0.0
lambda2_s : 100000000.0
# Output constraints (x, y, z, x_dot, y_dot, z_dot, roll, pitch, yaw)
......
......@@ -135,6 +135,9 @@ void change_Deepc_params()
d_logFolder = s_logFolder;
d_Deepc_measure_roll_pitch = s_Deepc_measure_roll_pitch;
d_Deepc_yaw_control = s_Deepc_yaw_control;
d_Tini = s_yaml_Tini;
d_N = s_yaml_N;
d_lambda2_g = s_yaml_lambda2_g;
bool grb_LogToFile = s_yaml_grb_LogToFile;
bool grb_LogToConsole = s_yaml_grb_LogToConsole;
// Deepc setup must be re-run after changes
......@@ -178,7 +181,6 @@ void change_Deepc_setpoint()
s_Deepc_mutex.lock();
// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Lock 193");
bool setupDeepc_success = s_setupDeepc_success;
int N = s_yaml_N;
MatrixXf setpoint = s_setpoint;
int num_outputs = s_num_outputs;
s_Deepc_mutex.unlock();
......@@ -190,27 +192,36 @@ void change_Deepc_setpoint()
try
{
// UPDATE GUROBI LINEAR COST VECTOR FOR REFERENCE TRACKING
MatrixXf r = MatrixXf::Zero(num_outputs, 1);
r.topRows(3) = setpoint.topRows(3);
d_r.topRows(3) = setpoint.topRows(3);
if (d_Deepc_yaw_control)
r.bottomRows(1) = setpoint.bottomRows(1);
d_r.bottomRows(1) = setpoint.bottomRows(1);
MatrixXf grb_cg_r = MatrixXf::Zero(d_Ng, 1);
for (int i = 0; i < N + 1; i++)
for (int i = 0; i < d_N + 1; i++)
{
if (i < N)
grb_cg_r -= 2.0 * d_Y_f.middleRows(i * num_outputs, num_outputs).transpose() * d_Q * r;
if (i < d_N)
d_grb_cg_r -= 2.0 * d_Y_f.middleRows(i * num_outputs, num_outputs).transpose() * d_Q * d_r;
else
grb_cg_r -= 2.0 * d_Y_f.middleRows(i * num_outputs, num_outputs).transpose() * d_P * r;
d_grb_cg_r -= 2.0 * d_Y_f.middleRows(i * num_outputs, num_outputs).transpose() * d_P * d_r;
}
// Update linear objective term for reference tracking
GRBLinExpr grb_lin_obj_r = 0;
// UPDATE GUROBI LINEAR COST VECTOR FOR STEADY STATE TRAJECTORY MAPPER
d_r_gs = d_r.replicate(d_Tini + d_N + 1, 1);
d_b_gs.bottomRows(d_r_gs.rows()) = d_r_gs;
d_gs = d_A_gs.bdcSvd(ComputeThinU | ComputeThinV).solve(d_b_gs);
d_grb_cg_gs = -2.0 * d_lambda2_g * MatrixXf::Identity(d_Ng, d_Ng) * d_gs;
// Update linear objective terms
d_grb_lin_obj_r = 0;
d_grb_lin_obj_gs = 0;
for (int i = 0; i < d_Ng; i++)
grb_lin_obj_r += grb_cg_r(i) * d_grb_vars[i];
{
d_grb_lin_obj_r += d_grb_cg_r(i) * d_grb_vars[i];
d_grb_lin_obj_gs += d_grb_cg_gs(i) * d_grb_vars[i];
}
// Update objective
d_grb_model.setObjective(d_grb_quad_obj + d_grb_lin_obj_us + grb_lin_obj_r);
d_grb_model.setObjective(d_grb_quad_obj + d_grb_lin_obj_us + d_grb_lin_obj_r + d_grb_lin_obj_gs);
// Inform the user
ROS_INFO("[DEEPC CONTROLLER] Deepc setpoint update successful");
......@@ -257,12 +268,9 @@ void setup_Deepc()
s_Deepc_mutex.lock();
// ROS_INFO("[DEEPC CONTROLLER] DEBUG Mutex Lock 277");
string dataFolder = s_dataFolder;
int Tini = s_yaml_Tini;
int N = s_yaml_N;
vector<float> Q_vec = s_yaml_Q;
vector<float> R_vec = s_yaml_R;
vector<float> P_vec = s_yaml_P;
float lambda2_g = s_yaml_lambda2_g;
float lambda2_s = s_yaml_lambda2_s;
float cf_weight_in_newtons = s_cf_weight_in_newtons;
MatrixXf setpoint = s_setpoint;
......@@ -325,14 +333,14 @@ void setup_Deepc()
// HANKEL MATRICES
int num_inputs = u_data.rows();
int num_outputs = y_data.rows();
d_Nuini = num_inputs * Tini;
d_Nyini = num_outputs * Tini;
MatrixXf H_u = data2hankel(u_data, Tini + N + 1);
MatrixXf H_y = data2hankel(y_data, Tini + N + 1);
MatrixXf U_p = H_u.topRows(num_inputs * Tini);
d_U_f = H_u.middleRows(num_inputs * Tini, num_inputs * N);
MatrixXf Y_p = H_y.topRows(num_outputs * Tini);
d_Y_f = H_y.bottomRows(num_outputs * (N + 1));
d_Nuini = num_inputs * d_Tini;
d_Nyini = num_outputs * d_Tini;
MatrixXf H_u = data2hankel(u_data, d_Tini + d_N + 1);
MatrixXf H_y = data2hankel(y_data, d_Tini + d_N + 1);
MatrixXf U_p = H_u.topRows(num_inputs * d_Tini);
d_U_f = H_u.middleRows(num_inputs * d_Tini, num_inputs * d_N);
MatrixXf Y_p = H_y.topRows(num_outputs * d_Tini);
d_Y_f = H_y.bottomRows(num_outputs * (d_N + 1));
// COST MATRICES
// Output cost and terminal output cost matrix
......@@ -362,9 +370,9 @@ void setup_Deepc()
// GUROBI QUADRATIC COST MATRIX
d_Ng = U_p.cols();
int Ns = num_outputs * Tini;
MatrixXf Qg = lambda2_g * MatrixXf::Identity(d_Ng, d_Ng);
for (int i = 0; i < N; i++)
int Ns = num_outputs * d_Tini;
MatrixXf Qg = d_lambda2_g * MatrixXf::Identity(d_Ng, d_Ng);
for (int i = 0; i < d_N; i++)
{
Qg += d_U_f.middleRows(i * num_inputs, num_inputs).transpose() * R * d_U_f.middleRows(i * num_inputs, num_inputs);
Qg += d_Y_f.middleRows(i * num_outputs, num_outputs).transpose() * d_Q * d_Y_f.middleRows(i * num_outputs, num_outputs);
......@@ -376,26 +384,42 @@ void setup_Deepc()
grb_Q.bottomRightCorner(Ns, Ns) = Qs;
// GUROBI LINEAR COST VECTOR
MatrixXf r = MatrixXf::Zero(num_outputs, 1);
r.topRows(3) = setpoint.topRows(3);
if (d_Deepc_yaw_control)
r.bottomRows(1) = setpoint.bottomRows(1);
// Steady state input
MatrixXf us = MatrixXf::Zero(num_inputs, 1);
us(0) = cf_weight_in_newtons;
// Reference
d_r = MatrixXf::Zero(num_outputs, 1);
d_r.topRows(3) = setpoint.topRows(3);
if (d_Deepc_yaw_control)
d_r.bottomRows(1) = setpoint.bottomRows(1);
// Steady state trajectory mapper
MatrixXf u_gs = us.replicate(d_Tini + d_N, 1);
d_r_gs = d_r.replicate(d_Tini + d_N + 1, 1);
d_A_gs = MatrixXf::Zero(U_p.rows() + d_U_f.rows() + Y_p.rows() + d_Y_f.rows(), d_Ng);
d_b_gs = MatrixXf::Zero(d_A_gs.rows(), 1);
d_A_gs.topRows(U_p.rows()) = U_p;
d_A_gs.middleRows(U_p.rows(), d_U_f.rows()) = d_U_f;
d_A_gs.middleRows(U_p.rows() + d_U_f.rows(), Y_p.rows()) = Y_p;
d_A_gs.bottomRows(d_Y_f.rows()) = d_Y_f;
d_b_gs.topRows(u_gs.rows()) = u_gs;
d_b_gs.bottomRows(d_r_gs.rows()) = d_r_gs;
d_gs = d_A_gs.bdcSvd(ComputeThinU | ComputeThinV).solve(d_b_gs);
MatrixXf grb_cg_us = MatrixXf::Zero(d_Ng, 1);
MatrixXf grb_cg_r = MatrixXf::Zero(d_Ng, 1);
for (int i = 0; i < N + 1; i++)
d_grb_cg_r = MatrixXf::Zero(d_Ng, 1);
for (int i = 0; i < d_N + 1; i++)
{
if (i < N)
if (i < d_N)
{
grb_cg_us -= 2.0 * d_U_f.middleRows(i * num_inputs, num_inputs).transpose() * R * us;
grb_cg_r -= 2.0 * d_Y_f.middleRows(i * num_outputs, num_outputs).transpose() * d_Q * r;
d_grb_cg_r -= 2.0 * d_Y_f.middleRows(i * num_outputs, num_outputs).transpose() * d_Q * d_r;
}
else
grb_cg_r -= 2.0 * d_Y_f.middleRows(i * num_outputs, num_outputs).transpose() * d_P * r;
d_grb_cg_r -= 2.0 * d_Y_f.middleRows(i * num_outputs, num_outputs).transpose() * d_P * d_r;
}
d_grb_cg_gs = -2.0 * d_lambda2_g * MatrixXf::Identity(d_Ng, d_Ng) * d_gs;
//INPUT CONSTRAINTS
MatrixXf input_min = MatrixXf::Zero(num_inputs, 1);
......@@ -435,9 +459,9 @@ void setup_Deepc()
// GUROBI LINEAR INEQUALITY CONSTRAINT VECTOR
MatrixXf grb_b = MatrixXf::Zero(grb_Ag.rows(), 1);
for (int i = 0; i < N + 1; i++)
for (int i = 0; i < d_N + 1; i++)
{
if (i < N)
if (i < d_N)
{
grb_b.middleRows(i * num_inputs, num_inputs) = -input_min;
grb_b.middleRows(d_U_f.rows() + i * num_inputs, num_inputs) = input_max;
......@@ -457,8 +481,8 @@ void setup_Deepc()
grb_A_eq.bottomRightCorner(Ns, Ns) = -MatrixXf::Identity(Ns, Ns);
// GUROBI LINEAR EQUALITY CONSTRAINT VECTOR
d_uini = MatrixXf::Zero(num_inputs * Tini, 1);
d_yini = MatrixXf::Zero(num_outputs * Tini, 1);
d_uini = MatrixXf::Zero(num_inputs * d_Tini, 1);
d_yini = MatrixXf::Zero(num_outputs * d_Tini, 1);
MatrixXf grb_b_eq = MatrixXf::Zero(grb_A_eq.rows(), 1);
grb_b_eq.topRows(d_uini.rows()) = d_uini;
grb_b_eq.bottomRows(d_yini.rows()) = d_yini;
......@@ -497,15 +521,17 @@ void setup_Deepc()
// Set linear objective term
d_grb_lin_obj_us = 0;
GRBLinExpr grb_lin_obj_r = 0;
d_grb_lin_obj_r = 0;
d_grb_lin_obj_gs = 0;
for (int i = 0; i < d_Ng; i++)
{
d_grb_lin_obj_us += grb_cg_us(i) * d_grb_vars[i];
grb_lin_obj_r += grb_cg_r(i) * d_grb_vars[i];
d_grb_lin_obj_r += d_grb_cg_r(i) * d_grb_vars[i];
d_grb_lin_obj_gs = d_grb_cg_gs(i) * d_grb_vars[i];
}
// Set objective
d_grb_model.setObjective(d_grb_quad_obj + d_grb_lin_obj_us + grb_lin_obj_r);
d_grb_model.setObjective(d_grb_quad_obj + d_grb_lin_obj_us + d_grb_lin_obj_r + d_grb_lin_obj_gs);
// Clear constraints if previously created
int num_constrs = d_grb_model.get(GRB_IntAttr_NumConstrs);
......
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