wam 2 examples

matlab_helpers/GVIMP-examples/WAM/plot_wam_paper.m

@@ -11,16 +11,30 @@
 import gtsam.*
 import gpmp2.*
 
-% =====================================  
+% =================================  
 % full grid GH quadrature results 
-% ===================================== 
+% ================================= 
 % prefix = "case1";
 
-% =================================== 
-% sparse GH quadrature results 0.5
-% ===================================
-prefix = "sparse_gh/case1";
+% =============================
+% sparse GH quadrature results 
+% =============================
+i_exp = 2;
 
+switch i_exp
+    case 1
+        prefix = "sparse_gh/case1";
+    case 2
+        prefix = "sparse_gh/case2";
+end
+
+%% ******************* Start and goal configurations ******************
+start_confs = [-0.8,   -1.70,   1.64,  1.29,   1.1, -0.106,    2.2;
+               -0.9,  -1.70,   1.34,  1.19,   0.8,  -0.126,    2.5];
+end_confs = [-0.0,    0.94,     0,     1.6,     0,   -0.919,   1.55;
+              -0.7,   1.35,    1.2,    1.0,   -0.7,  -0.1,   1.2];
+
+%% read experiment results
 means = csvread([prefix+"/mean.csv"]);
 covs = csvread([prefix+"/cov.csv"]);
 precisions = csvread([prefix+"/joint_precision.csv"]);
@@ -43,7 +57,6 @@
         break
     end
 end
-
 step_size = floor(niters / nsteps);
 n_states = floor(ttl_dim / dim_theta);
 
@@ -60,11 +73,13 @@
 field = signedDistanceField3D(dataset.map, dataset.cell_size);
 disp('calculating signed distance field done');
 
-%% ******************* WAM Arm and start and end conf ******************
+%% ******************* WAM Arm ******************
 arm = generateArm('WAMArm');
 
-start_conf = [-0.8,-1.70,1.64,1.29,1.1,-0.106,2.2]';
-end_conf = [-0.0,0.94,0,1.6,0,-0.919,1.55]';
+% start and goal 
+start_conf = start_confs(i_exp, 1:end)';
+end_conf = end_confs(i_exp, 1:end)';
+
 start_vel = zeros(7,1);
 end_vel = zeros(7,1);
 

matlab_helpers/GVIMP-examples/WAM/sparse_gh/case1/cost.csv

@@ -1,11 +1,30 @@
-625836.336622798
-16302.010358966
-14988.2745573915
-14835.2688939404
-4942.65147559676
-4931.15225123505
-4739.0695871258
-4584.40775719927
-4578.61201045667
-4467.61597855968
-4452.80758445837
+3545.81917623334
+2364.10210594623
+2272.95358445829
+2222.32743061328
+2178.80518895198
+2138.75674983327
+1975.99955553513
+1928.36073529408
+1842.14627560471
+1769.45284451049
+0
+0
+0
+0
+0
+0
+0
+0
+0
+0
+0
+0
+0
+0
+0
+0
+0
+0
+0
+0