executing plans one arm at a time using main1

calibrations/r1_tforms.yaml

 calibration:
-  pos_ET: [0.05631447282455423, -0.00029333822933579745, -0.2859753520679244]
-  pos_WR: [-0.38795741071515005, -0.6889547059768463, -0.02615409022679342]
-  quat_ET: [-0.2705980500730985, -0.6532814824381882, 0.27059805007309845, 0.6532814824381883]
-  quat_WR: [-0.004364280042214779, -0.0012936521576746582, 0.0011527958856873358,
-    0.9999889752319543]
+  pos_ET: [0.05631447204048056, -0.00029333984739081343, -0.2859753506297314]
+  pos_WR: [-0.38795740866781425, -0.6889547056851266, -0.026154087507405808]
+  quat_ET: [0.0, 0.0, 0.0, 1.0]
+  quat_WR: [-0.004364283639840516, -0.0012936485490402405, 0.0011527989443047648,
+    0.9999889752173954]
 measurements:
   errors:
-  - [0.0011793652064979732, -9.341060544723915e-05, 1.5709868600992383e-05]
-  - [-0.00043544949631380003, 0.00012267238950997283, -1.5704794591002436e-05]
-  - [-0.0007773292271404664, -0.00015854010997116844, -5.627958682419276e-09]
-  - [-6.945114304579558e-05, -0.00036755607549499025, -0.00011776139941228891]
-  - [0.00010286479304155716, 0.000496834384032141, 0.00011776199913214255]
+  - [0.0011793655526978197, -9.340838611010012e-05, 1.5708567648563265e-05]
+  - [-0.00043544770615600026, 0.000122674060645267, -1.5706962818697046e-05]
+  - [-0.000777328885787465, -0.00015854220117562847, -2.030257399698865e-09]
+  - [-6.945261663759972e-05, -0.0003675572476772304, -0.00011776224000879104]
+  - [0.00010286365941136744, 0.0004968337792151667, 0.00011776266681620684]
   pos_RE:
   - [-0.28951506556414164, 0.6362687797801195, 0.3194075212624789]
   - [-0.49387306825507354, 0.6376846438455942, 0.23963798142001758]

src/Controller.py

@@ -386,8 +386,9 @@ class Controller(object):
 
 
 def getR_ET():
-    return Rotation.from_euler('XYZ',[-45,-90,0],
-        degrees=True)
+    q = Rotation.from_quat([0,0,0,1])
+    return q#Rotation.from_euler('XYZ',[-45,-90,0],
+        #degrees=True)
 
 
 if __name__=='__main__':

src/main1.py

@@ -15,7 +15,7 @@ from trajectory_msgs.msg import (
 ## Our libraries
 from utils import load_plan, JTraj2ROS, speed_multiplier
 from Controller import Controller
-import gcode2contour as gc
+#import gcode2contour as gc
 
 
 def execute_plan(plan,
@@ -27,6 +27,7 @@ def execute_plan(plan,
         move_speed=1,
         n_trajs=100,
         retry_time=0.1,
+        arm=0, 
         max_traj_retries=20):
     """
     Executes a plan on MoveIt
@@ -35,10 +36,10 @@ def execute_plan(plan,
     skips the first n trajectories in the plan
     """
     # plan to the start position of the plan
-    controller.group.go(plan.trajs[0][skip_first].positions[0])
+    controller.group.go(plan.trajs[arm][skip_first].positions[0])
 
     # execute trajectories in plan
-    for i, t in enumerate(plan.trajs[0]):
+    for i, t in enumerate(plan.trajs[arm]):
         print("---")
 
         if i == n_trajs + skip_first:
@@ -82,9 +83,8 @@ def execute_plan(plan,
 
 
 if __name__ == '__main__':
-    plan1 = load_plan('origin')
-    calibration_test = load_plan('r2_cal_test')
-    plan2, flexirex_contours = load_plan('flexirex', contours=True)
+    calibration_test = load_plan('test')
+    #plan2, flexirex_contours = load_plan('flexirex', contours=True)
 
     # Printing
     #con = Controller()
@@ -94,7 +94,7 @@ if __name__ == '__main__':
     #con = Controller(robot=1, disable_extruder=True) # r1 when both arms running
     #con = Controller(robot=2, disable_extruder=True) # r2 when both arms running
     con = Controller(disable_extruder=True)         # any when one arm  running
-    execute_plan(calibration_test, con, confirm=True)
+    execute_plan(calibration_test, con, confirm=True,arm=1)
 
     # go to origin and stay (to check calibration)
     #plan05 = copy.deepcopy(plan1)