Update nn arch should be used with corresponding gcop branch

neural_network_model_data/tensorflow_model_vars_16_8_tanh/joint_gains_kd_0

@@ -1,3 +1,3 @@
 2
 1
-1.489354,1.2015601,
+1.2879044,1.1704179,

neural_network_model_data/tensorflow_model_vars_16_8_tanh/joint_gains_kp_0

@@ -1,3 +1,3 @@
 2
 1
-11.780129,11.661143,
+11.422288,11.645887,

neural_network_model_data/tensorflow_model_vars_16_8_tanh/residual_dynamics_dense_0_residual_dynamics_beta_0

@@ -1,3 +1,3 @@
 16
 1
-0.27311215,-0.104236856,-0.025411092,0.29267374,-0.2025853,0.021320792,0.06885028,0.16424008,-0.015703725,-0.14184234,0.074954376,-0.36680064,0.45662504,-0.16305926,0.040393177,-0.04175968,
+-0.0047926186,-0.0075431955,0.11958674,-0.10918364,-0.01612634,0.063661486,0.029086206,0.047851354,0.0053286874,-0.08630042,-0.014087217,-0.11339993,-0.02294452,0.04781778,-0.04164681,-0.037811738,

neural_network_model_data/tensorflow_model_vars_16_8_tanh/residual_dynamics_dense_0_residual_dynamics_gamma_0

@@ -1,3 +1,3 @@
 16
 1
-0.63099766,0.47105283,1.0321938,0.49864632,0.6379682,0.6887875,0.89560276,0.62884676,0.8724765,0.94491845,1.587332,0.8730545,0.8333377,0.6422129,0.8679904,0.779207,
+0.33846265,2.2373595,0.55494624,0.5208485,0.47802243,0.32896978,1.9201336,0.46899405,0.3514824,0.42919007,0.35195422,1.3166497,0.35678586,2.1223361,2.2631538,0.83180225,

neural_network_model_data/tensorflow_model_vars_16_8_tanh/residual_dynamics_dense_0_residual_dynamics_moving_mean_0

@@ -1,3 +1,3 @@
 16
 1
--2.0789804,0.6657096,-6.670102,-2.7619963,0.4236306,12.664186,-9.342,-14.674688,12.748245,-11.165259,3.5782506,-2.296099,-15.195986,9.613851,19.242052,16.455389,
+-0.0021787419,0.15565325,-1.1851991,-0.3887733,-0.781049,0.075614214,0.5228897,0.35720992,0.07733806,0.272907,0.22505729,-0.20781657,0.51566654,0.07451289,-0.46856713,0.021119505,

neural_network_model_data/tensorflow_model_vars_16_8_tanh/residual_dynamics_dense_0_residual_dynamics_moving_variance_0

@@ -1,3 +1,3 @@
 16
 1
-0.7465025,1.4063803,1.0290928,1.0668128,1.3317267,3.0863025,5.22682,3.032115,3.8225298,2.9258606,2.010817,0.87457055,2.6959667,1.3964008,2.967397,2.900481,
+0.25169238,2.331338,2.0717125,0.357323,0.54824793,0.2672733,4.577049,0.27728173,0.05837644,0.28401634,0.0062248204,3.8090374,0.29252848,2.2509565,1.5358577,1.6141387,

neural_network_model_data/tensorflow_model_vars_16_8_tanh/residual_dynamics_dense_1_residual_dynamics_beta_0

@@ -1,3 +1,3 @@
 8
 1
--0.074566334,0.04588292,0.013513993,-0.09232558,0.08453735,-0.08074734,-0.012946017,-0.049817093,
+-0.00592649,-0.037208334,0.02271216,-0.13219856,0.0020418598,0.013361917,0.022096215,-0.045767006,

neural_network_model_data/tensorflow_model_vars_16_8_tanh/residual_dynamics_dense_1_residual_dynamics_gamma_0

@@ -1,3 +1,3 @@
 8
 1
-0.407895,0.46385458,0.59804624,0.6940339,0.65389645,0.57341135,0.60178936,0.6238263,
+0.3233974,0.55816483,1.7782717,0.8032256,0.2996349,0.3085106,0.39490306,0.4113996,

neural_network_model_data/tensorflow_model_vars_16_8_tanh/residual_dynamics_dense_1_residual_dynamics_moving_mean_0

@@ -1,3 +1,3 @@
 8
 1
--0.027100913,-0.08684619,0.07639614,-0.3055278,0.2933677,0.030098654,-0.24982458,-0.102109015,
+-0.062266674,-0.02714833,-0.012357137,-0.0359934,-0.010994579,0.04012002,0.08387415,0.011114856,

neural_network_model_data/tensorflow_model_vars_16_8_tanh/residual_dynamics_dense_1_residual_dynamics_moving_variance_0

@@ -1,3 +1,3 @@
 8
 1
-0.117956616,0.29506895,0.41771913,0.2054379,0.07992752,0.20110367,0.6744197,0.57749856,
+0.510723,0.59528226,1.2511085,0.55236363,0.5134672,0.52601063,0.44630972,0.34227136,

neural_network_model_data/tensorflow_model_vars_16_8_tanh/residual_dynamics_dense_1_weights_0

@@ -1,3 +1,3 @@
 16
 8
--0.3955174982547760,0.3475214540958405,0.1666727811098099,0.0723626837134361,0.2040266841650009,0.2776500284671783,-0.2691484093666077,-0.8262147903442383,-0.5009171366691589,0.3135701119899750,-0.1797807067632675,-0.0054747839458287,0.2813730835914612,0.0369706563651562,-0.2981808483600616,-0.0751455724239349,-0.3041052520275116,-0.2850141823291779,-0.1785960644483566,0.2523778676986694,0.3894436657428741,0.1003281250596046,0.4786004722118378,0.0696862637996674,-0.0341695286333561,-0.1455603092908859,0.0134820388630033,-0.1852132380008698,-0.0570538826286793,0.3001527190208435,-0.4711738228797913,0.3554453551769257,-0.3214689195156097,0.0352475531399250,-0.0016455277800560,0.0972204506397247,-0.3351288437843323,-0.2225704342126846,0.4871674180030823,0.3621132373809814,0.0729761421680450,-0.0597230568528175,0.0621601231396198,0.5811676383018494,0.3744303882122040,-0.6263814568519592,-0.0575744248926640,0.0956700667738914,0.1255384534597397,0.3155196011066437,0.2859912216663361,0.3514328002929688,0.3252155482769012,-0.4174808263778687,-0.3955832719802856,-0.2823237776756287,0.1657994985580444,-0.3833343088626862,0.4771501421928406,0.0193396937102079,-0.2951161265373230,-0.3858037889003754,-0.0931366756558418,0.0565191507339478,0.0573556944727898,0.2362638264894485,-0.6156558394432068,-0.2911368906497955,0.4094492197036743,-0.1326601803302765,-0.4948909878730774,0.0474076606333256,-0.3281344175338745,0.5307967066764832,-0.0575741119682789,0.2978443205356598,0.1407214701175690,-0.4407677352428436,0.1542461067438126,0.3518172204494476,-0.1211540848016739,0.2927716076374054,-0.7705510258674622,-0.2600173056125641,0.6238839626312256,-0.1764952242374420,0.4458127021789551,-0.0878090336918831,0.3454935252666473,0.1552325338125229,0.0573949143290520,0.1756333708763123,-0.3654964566230774,0.1241131350398064,-0.3488118350505829,-0.3709834516048431,-0.3155064284801483,-0.1541214436292648,-0.1467889100313187,-0.4616673588752747,0.3663932979106903,-0.2733428180217743,-0.1969577670097351,-0.1497866809368134,-0.5888675451278687,-0.3959282934665680,-0.0771000683307648,0.6153556108474731,-0.3025265932083130,-0.1340731084346771,0.0317353010177612,-0.4254517853260040,0.2067295014858246,0.0715371966362000,-0.2630290091037750,0.2111856341362000,0.3259117305278778,-0.4022766649723053,-0.1947669535875320,-0.1605077385902405,-0.3210546076297760,-0.0752286165952682,-0.1233168765902519,-0.3483547270298004,-0.2834948003292084,-0.2165223062038422,0.3089371323585510,0.2668958306312561,
+0.2186670452356339,-0.3146375119686127,-0.1833985447883606,0.4130999445915222,0.6552199721336365,0.4321439266204834,0.2873339951038361,-0.3448870480060577,-0.0341655798256397,-0.1716942489147186,0.4724788963794708,-0.2457936555147171,-0.0567096695303917,-0.2010623514652252,0.2816126048564911,0.0553758218884468,0.3291748464107513,-0.4456990063190460,0.1006847992539406,-0.1879170387983322,-0.0683682113885880,0.1859945207834244,-0.2184516042470932,-0.1009938716888428,0.4270479977130890,-0.4277004599571228,-0.0536135509610176,0.3096832036972046,0.3286302983760834,-0.3625821173191071,-0.2897882163524628,0.0497084334492683,0.0764876008033752,0.1890842914581299,0.3433853983879089,0.2066864818334579,0.1272892504930496,-0.3845951855182648,-0.3870612978935242,0.3045388162136078,-0.3582243025302887,-0.1359174698591232,-0.0346539206802845,-0.5058526396751404,-0.0517051853239536,-0.0677018091082573,0.6350773572921753,0.4986394643783569,-0.0570167452096939,0.3232947289943695,0.4980818033218384,0.1771551370620728,-0.0494567342102528,0.1392125636339188,0.1302938312292099,-0.1636015027761459,-0.4278486967086792,-0.2293502390384674,-0.2090692073106766,-0.1454353481531143,0.0594659708440304,0.4697293043136597,-0.3836808204650879,-0.0860973745584488,-0.5190468430519104,0.4766353964805603,-0.3736885786056519,0.4465203583240509,-0.4400622546672821,0.1481685042381287,-0.1684322357177734,0.5852466821670532,0.2135460674762726,0.3841982781887054,0.0980013310909271,-0.0197493564337492,-0.5728057026863098,0.1376891285181046,-0.5003357529640198,0.0207847636193037,0.1884478926658630,-0.4288673102855682,0.0670324340462685,-0.3732773065567017,-0.3260323405265808,0.8347244858741760,-0.5182760357856750,-0.6719217896461487,0.2127904593944550,-0.1736206412315369,-0.1459772586822510,-0.3033678233623505,0.0019878863822669,-0.0624870061874390,-0.1898268759250641,0.0575478821992874,-0.7126914858818054,0.0671223327517509,0.4611491858959198,0.0254538040608168,0.7106738090515137,0.4699913859367371,0.1933408230543137,-0.1028054058551788,0.1129772514104843,0.6264473199844360,-0.1243461668491364,0.5852644443511963,0.4170316159725189,0.0319458432495594,-0.1350708454847336,-0.3061890006065369,0.1822737753391266,0.0200467258691788,0.7738122344017029,-0.2643924355506897,-0.1990559101104736,-0.1024299412965775,-0.0343050882220268,-0.0738689824938774,-0.2006210982799530,0.0601284392178059,-0.1430158019065857,-0.3450992703437805,0.2700622379779816,0.2427021116018295,-0.0486969687044621,0.0083232419565320,

neural_network_model_data/tensorflow_model_vars_16_8_tanh/residual_dynamics_dense_final_biases_0

@@ -1,3 +1,3 @@
 8
 1
--0.0016912151,-0.099235654,0.04752723,0.238435,-0.16314504,-0.00993302,-0.23952977,0.0340867,
+-0.012836409,-0.13071178,0.035183474,0.20557193,-0.18581991,0.0029871017,-0.09271976,-0.11484799,

neural_network_model_data/tensorflow_model_vars_16_8_tanh/residual_dynamics_dense_final_weights_0

@@ -1,3 +1,3 @@
 8
 8
--0.1304899752140045,0.5376957654953003,0.3657346963882446,0.0211216472089291,0.9303691983222961,-0.0230968613177538,-0.5883984565734863,-0.5699948072433472,0.0085330447182059,0.1501954346895218,-0.5975889563560486,0.4543600678443909,0.6764503121376038,-0.0355348400771618,-0.3476428389549255,-0.6163758635520935,-0.2919064462184906,-0.0622706785798073,-0.1573668867349625,-0.5903056859970093,-0.6360251307487488,0.1800285726785660,-0.7477556467056274,-0.6263556480407715,-0.1856515109539032,0.2501020729541779,-0.4835608303546906,-0.3755324184894562,-0.2784877717494965,0.1496413350105286,-1.4380418062210083,-0.2659917771816254,-0.0646716058254242,0.2803609371185303,-0.2114755064249039,-0.1652359962463379,0.4358581602573395,0.1223051920533180,1.4152486324310303,1.2322967052459717,0.4261342287063599,0.3276852071285248,-0.1587564945220947,0.4951451718807220,0.1598387360572815,-0.0454892069101334,0.3677675426006317,1.0028101205825806,-0.3015342056751251,-0.3436273932456970,0.0812027975916862,-0.0243031568825245,-0.1675346195697784,-0.1325416117906570,1.4216343164443970,1.7214468717575073,0.2044980525970459,0.3093000948429108,0.4273132383823395,0.1155300512909889,0.7266339659690857,0.0504091754555702,0.2010100781917572,0.6295794248580933,
+-0.2078329026699066,-0.1944836676120758,0.1967778950929642,-0.3055453300476074,-0.6239414215087891,-0.1868178993463516,0.7155768871307373,0.1347559392452240,0.0779939517378807,0.1233999356627464,0.1227722540497780,0.5918513536453247,0.6187155842781067,-0.2174564450979233,0.9010667204856873,-1.6594997644424438,0.0127861360087991,-0.0134318331256509,-0.0060421619564295,-0.1372151672840118,-0.1408181190490723,0.1007382646203041,2.1808717250823975,0.6747477054595947,-0.0121351694688201,0.0208426266908646,0.0864434316754341,-0.3299374282360077,0.4883072674274445,0.0645743384957314,-0.6753569841384888,-1.4463539123535156,-0.1937327831983566,0.1215608045458794,-0.2580211162567139,-0.5671874880790710,-0.3301273286342621,0.3928345739841461,-0.5559937357902527,-1.7193880081176758,0.1233323141932487,-0.2344708889722824,-0.2872360050678253,0.4583125114440918,-0.1673358380794525,-0.1091032922267914,-0.5000016689300537,-0.9308024048805237,-0.1118915975093842,0.1560886502265930,-0.1692378968000412,-0.1682958751916885,1.2167209386825562,-0.3185026943683624,1.0577803850173950,0.9811016321182251,0.0601774938404560,0.2490518242120743,0.1685770004987717,0.2915897369384766,0.7500129938125610,0.1572253555059433,-0.7012147903442383,1.2724392414093018,

neural_network_model_data/tensorflow_model_vars_16_8_tanh/rpy_gains_kd_0

@@ -1,3 +1,3 @@
 3
 1
-8.323076,6.1193175,3.2318325,
+8.173418,7.0072937,3.3337743,

neural_network_model_data/tensorflow_model_vars_16_8_tanh/rpy_gains_kp_0

@@ -1,3 +1,3 @@
 2
 1
-16.546741,17.375994,
+16.51752,17.266687,

param/mpc_controller_config.pbtxt

@@ -1,17 +1,17 @@
 ddp_config {
   # State cost
   # XYZ
-  Q: 1
-  Q: 1
-  Q: 1
+  Q: 100
+  Q: 100
+  Q: 100
   # RPY
   Q: 4
   Q: 4
   Q: 4
   # Vxyz
-  Q: 4
-  Q: 4
-  Q: 4
+  Q: 100
+  Q: 100
+  Q: 100
   # RPYdot
   Q: 4
   Q: 4
@@ -57,8 +57,8 @@ ddp_config {
   Qf: 100
   Qf: 100
   # Joint_cmd
-  Qf: 0.1
-  Qf: 0.1
+  Qf: 100
+  Qf: 100
   # Control cost
   # Thrust
   R: 6.0
@@ -76,9 +76,9 @@ ddp_config {
   # Min cost decrease
   min_cost_decrease: 1e-4
   # Look ahead time
-  look_ahead_time: 0.02
+  look_ahead_time: 0.04
   # Max cost
-  max_cost: 100.0
+  max_cost: 500.0
 }
 
 weights_folder: "neural_network_model_data/tensorflow_model_vars_16_8_tanh/"

scripts/analysis/compare_mpc_trajectories.py

@@ -0,0 +1,54 @@
+#!/usr/bin/env python3
+import matplotlib.pyplot as plt
+import argparse
+import os
+import pandas as pd
+import seaborn as sns
+import numpy as np
+# Args: -f ../../log_mpc_June_13th_2018/data_18_06_12_23_16_26 ../../log_mpc_June_13th_2018/data_18_06_12_22_59_56 -l RNN FF -s ../../
+# %% Getting data
+parser = argparse.ArgumentParser(
+    prog='plot_quad_data')
+parser.add_argument('-f', '--folders', type=str,nargs='+',
+                    help='Data folders to compare')
+parser.add_argument('-l', '--legends', type=str, nargs='+',
+                    help='Legends for each folder')
+parser.add_argument('-s', '--save_folder', type=str, default='./',
+                    help='Folder to save final plot')
+parser.add_argument('--tStart', type=float, default=0.0, help='Start time')
+parser.add_argument('--tEnd', type=float, default=1e3, help='End time')
+args = parser.parse_args()
+error_df_list = []
+assert(len(args.folders) == len(args.legends))
+
+for iFolder, folder in enumerate(args.folders):
+    state_data = pd.read_csv(os.path.join(folder, 'mpc_state_estimator'))
+    error_data = pd.read_csv(os.path.join(folder, 'ddp_mpc_controller'))
+    ts = state_data['#Time'].values
+    ts1 = (error_data['#Time'].values - ts[0])/1e9
+    ts = (ts - ts[0])/1e9
+    iStart = np.argmin(np.abs(ts - args.tStart))
+    iEnd = np.argmin(np.abs(ts - args.tEnd))
+    interp_error_list = []
+    labels = ['Errorx','Errory','Errorz','Errorja1','Errorja2']
+    for label in labels:
+        interp_error_list.append(np.interp(ts, ts1, error_data[label].values))
+    interp_errors = np.vstack(interp_error_list).T
+    abs_errors = np.abs(interp_errors)
+    folder_label = args.legends[iFolder]
+    readable_labels = ['X (m)', 'Y (m)', 'Z (m)', 'J1 (rad)', 'J2 (rad)']
+    df = pd.DataFrame(abs_errors, columns=readable_labels)
+    df = df.stack().reset_index()
+    df.columns = ['Index', 'Sensor Channels', 'Mean Absolute Error']
+    df['FolderLabel'] = [folder_label]*df.shape[0]
+    error_df_list.append(df)
+
+error_df = pd.concat(error_df_list)
+# %% Plotting
+sns.set_style('whitegrid')
+sns.set(font_scale = 1.2)
+plt.figure(1)
+sns.barplot('Sensor Channels', 'Mean Absolute Error',
+            'FolderLabel', data=error_df, ci=95)
+plt.savefig(os.path.join(args.save_folder, 'mpc_error_plot.eps'),
+            bbox_inches='tight')

scripts/analysis/plot_mpc_trajectories.py

@@ -0,0 +1,64 @@
+#!/usr/bin/env python3
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
+import argparse
+import os
+import pandas as pd
+import seaborn as sns
+import numpy as np
+# %% Getting data
+parser = argparse.ArgumentParser(
+    prog='plot_quad_data')
+parser.add_argument('folder', type=str, help='Data folder')
+parser.add_argument('--tStart', type=float, default=0.0, help='Start time')
+parser.add_argument('--tEnd', type=float, default=1e3, help='End time')
+args = parser.parse_args()
+state_data = pd.read_csv(os.path.join(args.folder, 'mpc_state_estimator'))
+error_data = pd.read_csv(os.path.join(args.folder, 'ddp_mpc_controller'))
+ts = state_data['#Time'].values
+ts1 = (error_data['#Time'].values - ts[0])/1e9
+ts = (ts - ts[0])/1e9
+iStart = np.argmin(np.abs(ts - args.tStart))
+iEnd = np.argmin(np.abs(ts - args.tEnd))
+interp_error_list = []
+for label in ['Errorx','Errory','Errorz','Errorja1','Errorja2']:
+    interp_error_list.append(np.interp(ts, ts1, error_data[label].values))
+interp_errors = np.vstack(interp_error_list).T
+xyz_ja = state_data[['x','y','z','ja1','ja2']].values
+ref_xyz_ja = xyz_ja - interp_errors
+# %% Plotting
+sns.set_style('whitegrid')
+sns.set(font_scale = 1.2)
+plt.figure(1)
+labels = ['X', 'Y', 'Z', 'Ja1', 'Ja2']
+units = ['m','m','m','rad', 'rad']
+legend = ['Tracked', 'Reference']
+ts_sub = ts[iStart:iEnd]
+for i in range(5):
+    plt.figure(i+1)
+    plt.plot(ts_sub, xyz_ja[iStart:iEnd, i])
+    plt.plot(ts_sub, ref_xyz_ja[iStart:iEnd, i])
+    plt.ylabel(labels[i]+' ('+units[i]+')')
+    plt.xlabel('Time (seconds)')
+    plt.legend(legend)
+    plt.savefig(os.path.join(args.folder,labels[i]+'.eps'),
+                             bbox_inches='tight')
+
+fig = plt.figure(6)
+ax = fig.add_subplot(111, projection='3d')
+ax.plot(xyz_ja[iStart:iEnd,0], xyz_ja[iStart:iEnd,1], xyz_ja[iStart:iEnd,2])
+ax.plot(ref_xyz_ja[iStart:iEnd,0], ref_xyz_ja[iStart:iEnd,1], ref_xyz_ja[iStart:iEnd,2])
+ax.legend(legend)
+ax.set_xlabel('X (m)')
+ax.set_ylabel('Y (m)')
+ax.set_zlabel('Z (m)')
+rms_errors = np.sqrt(np.mean(np.square(interp_errors[iStart:iEnd, :]), axis=0))
+np.set_printoptions(precision=2, suppress=True)
+np.savetxt(os.path.join(args.folder, 'rms_errors.csv'),
+           rms_errors[:,np.newaxis].T, fmt='%.2f',
+           delimiter=',',
+           header='RMSX, RMSY, RMSZ, RMSJ1, RMSJ2')
+print("RMS ERRORS: ", rms_errors)
+# %%For 3d plot save it yourself
+plt.savefig(os.path.join(args.folder,'trajectory.eps'),
+            bbox_inches='tight')

src/controller_tuners/mpc_controller_tuner.cpp

@@ -108,9 +108,9 @@ int main(int argc, char **argv) {
   visualizer_config.mutable_trajectory_color()->set_r(0.0);
   visualizer_config.mutable_desired_trajectory_color()->set_a(0.5);
   MPCTrajectoryVisualizer visualizer(controller_connector, visualizer_config);
-  auto reference_ptr =
-      createWayPoint(PositionYaw(0.2, 0.2, 0.2, 0.0), -0.8, 1.4);
-  // auto reference_ptr = createSpiralReference(drone_hardware);
+  // auto reference_ptr =
+  //    createWayPoint(PositionYaw(0.2, 0.2, 0.2, 0.0), -0.8, 1.4);
+  auto reference_ptr = createSpiralReference(drone_hardware);
   controller_connector.usePerfectTimeDiff(
       0.02); ///\todo Remove this flag business
   // Start drone