add third vert_or for 3 arm simulation, fix greedy n arm (#43)

src/decmcts.py

@@ -513,10 +513,16 @@ if __name__ == "__main__":
     model = "flexirex_big"
     tracker = tu.get_tracker(model)
 
+    # 3arms simulation
+    env_desc3 = tu.PlanningEnv([[0,0,1,0],[0,0,1,0],[0,0,0,1]],
+                       [[0.9, -0.35, 0],[-0.8, -0.35, 0.001],[0, 0.5, 0]],
+                       [[0,0,0,1]]*3,
+                       [[0.15,0,0]]*3) # WARNING - also edit urdf
+
     # 2arms real calibration
     env_desc2 = tu.read_env('calibrations/r1_tforms.yaml',
                             'calibrations/r2_tforms.yaml')
-    env = tu.SimEnv(env_desc=env_desc2, gui=False, home=home)
+    env = tu.SimEnv(env_desc=env_desc3, gui=False, home=home)
 
     # decmcts plan first n contours
     c_traj_file = "gcode/" + model + ".c_traj"

src/greedy.py

@@ -155,8 +155,8 @@ if __name__ == "__main__":
     tracker = tu.get_tracker(model)
 
     # 3arms simulation
-    env_desc3 = tu.PlanningEnv([[0,0,1,0],[0,0,0,1],[0,0,0,1]],
-                       [[0.9, -0.25, 0],[-0.5, -0.25, 0.001],[0, 0.5, 0]],
+    env_desc3 = tu.PlanningEnv([[0,0,1,0],[0,0,1,0],[0,0,0,1]],
+                       [[0.9, -0.35, 0],[-0.8, -0.35, 0.001],[0, 0.5, 0]],
                        [[0,0,0,1]]*3,
                        [[0.15,0,0]]*3) # WARNING - also edit urdf
 
@@ -165,4 +165,4 @@ if __name__ == "__main__":
                             'calibrations/r2_tforms.yaml')
 
     # Greedily plan first n contours
-    plan = greedy(tracker, env_desc2, n_contours=10, gui=True)
+    plan = greedy(tracker, env_desc3, n_contours=10, gui=True)

src/test_decmcts.py

@@ -46,6 +46,12 @@ if __name__ == "__main__":
     model = "flexirex_big"
     tracker = tu.get_tracker(model)
 
+    # 3arms simulation
+    env_desc3 = tu.PlanningEnv([[0,0,1,0],[0,0,1,0],[0,0,0,1]],
+                       [[0.9, -0.35, 0],[-0.8, -0.35, 0.001],[0, 0.5, 0]],
+                       [[0,0,0,1]]*3,
+                       [[0.15,0,0]]*3) # WARNING - also edit urdf
+
     # 2arms real calibration
     env_desc2 = tu.read_env('calibrations/r1_tforms.yaml',
                             'calibrations/r2_tforms.yaml')

src/utils/simEnv.py

@@ -61,7 +61,11 @@ _vert_or = [np.array([[-6.74517010e-04,  8.33315658e-04, -9.99999404e-01, -2.905
                      [ 8.33732192e-04, -9.99999285e-01, -8.33877944e-04, -4.81068309e-05],
                      [-9.99999404e-01, -8.34294187e-04,  6.73821778e-04,  2.74792069e-06],
                      [ 0.00000000e+00,  0.00000000e+00,  0.00000000e+00,  1.00000000e+00]]),
-            np.array([[-6.74517010e-04, -8.33315658e-04,  9.99999404e-01, -2.90597873e-05],
+            np.array([[-6.74517010e-04, -8.33315658e-04,  9.99999404e-01, -2.90597873e-05], # 2nd arm
+                     [ 8.33732192e-04,  9.99999285e-01,  8.33877944e-04, -4.81068309e-05],
+                     [-9.99999404e-01,  8.34294187e-04, -6.73821778e-04,  2.74792069e-06],
+                     [ 0.00000000e+00,  0.00000000e+00,  0.00000000e+00,  1.00000000e+00]]),
+            np.array([[-6.74517010e-04, -8.33315658e-04,  9.99999404e-01, -2.90597873e-05], # 3rd arm
                      [ 8.33732192e-04,  9.99999285e-01,  8.33877944e-04, -4.81068309e-05],
                      [-9.99999404e-01,  8.34294187e-04, -6.73821778e-04,  2.74792069e-06],
                      [ 0.00000000e+00,  0.00000000e+00,  0.00000000e+00,  1.00000000e+00]])]