update r1 calibration (#19)

calibrations/r1_tforms_alt.yaml

 calibration:
-  pos_ET: [0.15007592984204163, -0.04830698520623148, -0.031668261140098686]
-  pos_WR: [0.03655708997718542, -0.575405839173846, -0.13224784915908203]
+  pos_ET: [0.1502296461658024, -0.04698558961114961, -0.031116243011024135]
+  pos_WR: [0.03844669361396861, -0.572941079182837, -0.13103114911618197]
   quat_ET: [-0.2705980500730985, -0.6532814824381882, 0.27059805007309845, 0.6532814824381883]
-  quat_WR: [-0.004907671722772035, 0.0026006325187357495, -0.9999828094804524, 0.0018794180869107977]
+  quat_WR: [-0.005519573998493153, 0.0023551564258786424, -0.999981578967214, 0.0009106411601250241]
 measurements:
   errors:
-  - [-0.00035093605232278113, 0.0015154904646795808, 0.0005004698912217387]
-  - [0.0008203797060748544, 0.0008922491648686859, 0.0002722545866968018]
-  - [0.001289894680197884, 0.0009346809299097258, -0.0011471590539748888]
-  - [-0.00026782915015455266, -0.00019411428454588426, -0.00017438502446384674]
-  - [-0.000711953482581712, -0.00017432903503347696, 0.0007754958989695804]
-  - [-0.0009272510092923555, -0.001473229551918892, 0.0009148858434974705]
-  - [-0.0007779454099607619, -0.0013991536560543083, -9.067649884858531e-05]
-  - [-0.0006907081723367681, -0.00013629415861853315, 0.0005679499481896777]
-  - [0.0014934971392888685, -0.000434524141417425, -0.001318136805991993]
-  - [0.00012285175106990787, 0.0004692242681248698, -0.0003006987853121923]
+  - [0.00015395547326951808, 0.002142423630859619, 0.0003355815224046488]
+  - [-1.721708528695104e-05, 5.051743681194765e-05, 0.00041670383526773436]
+  - [0.00014560316238965143, -1.839592857558192e-05, -0.0009316653575424105]
+  - [3.161792856745671e-05, -2.53463553584113e-05, -9.16948482303781e-05]
+  - [-0.0010221720460194261, -1.0771535927212028e-05, 0.0012171946733552097]
+  - [-0.0010681151514920634, -0.0009799175199797783, 2.801905010083039e-05]
+  - [-6.682373151054022e-05, -0.0008270223124680898, -0.0008691193706457379]
+  - [2.541162594885127e-05, 0.0001367769605153022, 0.0006730588199809484]
+  - [0.001417878916551707, -0.0008426181698256152, -0.00037090151330573806]
+  - [0.0003998609358597527, 0.00037435379811547335, -0.0004071767733215548]
   pos_RE:
-  - [0.28238526739189035, -0.743445281602937, 0.2753943928759848]
-  - [0.06856808024088912, -0.7433152737798132, 0.27773399071687555]
-  - [-0.14592886427691276, -0.7424717736062426, 0.28125724809030916]
-  - [-0.14669659051873055, -0.528554844189385, 0.2813981571448582]
-  - [-0.1463260383308909, -0.3135266064730779, 0.28156287698542204]
-  - [0.06844080035242153, -0.3156302723662834, 0.27930760468097077]
-  - [0.283587823473315, -0.31634541818249157, 0.2781937485538753]
-  - [0.28285396776950816, -0.530129589269635, 0.2764449159253464]
-  - [0.5066506093478649, -0.20096898804832836, 0.16677352259446088]
-  - [-0.369184926735048, -0.5275417892542257, 0.2618225946251175]
+  - [0.28619790934684075, -0.7402544719962719, 0.2742202952337487]
+  - [0.07105893398903566, -0.7419692559154405, 0.2765033963380906]
+  - [-0.1437662590736294, -0.7416598402185833, 0.2802276770777924]
+  - [-0.1434836763099208, -0.5266801743305487, 0.2804022408548177]
+  - [-0.14417271043384325, -0.3116608075668326, 0.2801197811161653]
+  - [0.0708014310233577, -0.31297986484632256, 0.2789127806741849]
+  - [0.2867904938256065, -0.31325436323976485, 0.2774381453273922]
+  - [0.28647132057396424, -0.5272750808751241, 0.2748870851772862]
+  - [0.49198953051379024, -0.1614223801212086, 0.15202211073521693]
+  - [-0.36583396130433044, -0.5260828760890328, 0.26097777977314823]
   quat_RE:
-  - [0.004515506751761712, 0.7093312647790816, 0.0005263393718474751, 0.7048606174078313]
-  - [0.004551772876819091, 0.7093577385565347, 0.000732948249208785, 0.7048335568774383]
-  - [0.004513807736845369, 0.7093838031536429, 0.0007461847780781305, 0.7048075542807879]
-  - [0.00442392266478542, 0.709359777178923, 0.0006503084317854549, 0.7048324003107893]
-  - [0.004471491373419877, 0.7093586207919866, 0.0005390274408393596, 0.7048333578388627]
-  - [0.004466760991158438, 0.7093646956760709, 0.0005683729336144619, 0.7048272508401273]
-  - [0.004391686236732675, 0.7093756214129284, 0.0005693173620683987, 0.704816725620754]
-  - [0.00448742433570489, 0.7093749968660662, 0.000748052655762252, 0.7048165841276184]
-  - [-0.00045970685855608695, 0.6540762178342059, -0.653219981430286, 0.38142855922639246]
-  - [0.003642363656463826, 0.38561399879651764, 0.002617478574255283, 0.922649297363341]
+  - [0.0010413200769373307, 0.7105862039337016, 0.0006649326483706677, 0.7036090678041056]
+  - [0.001084891693411131, 0.7106329642035853, 0.0006550901734732243, 0.7035617841057772]
+  - [0.0010554592940128175, 0.7106074401362242, 0.0007063304987061758, 0.7035875589618877]
+  - [0.0011041161037969283, 0.7106222933336699, 0.0007199736181426762, 0.7035724687498894]
+  - [0.0011314856150625339, 0.7105696678299577, 0.0006810590537584474, 0.7036256128502517]
+  - [0.00101736222811157, 0.7106283885489839, 0.0006081311970953219, 0.7035665487633834]
+  - [0.0010867007796504266, 0.7106128191421314, 0.0007128943418593383, 0.7035820720668952]
+  - [0.0010999875504787095, 0.7106047226468447, 0.0006587354591232912, 0.7035902815182887]
+  - [-0.14338537701650084, 0.8552601557202018, -0.3542228559665395, 0.34999552569454434]
+  - [0.00045600965998949725, 0.3870842766393148, 0.0012555108399978506, 0.922043371282685]
 targets:
   pos_W:
   - [-0.215, 0.215, 0]

src/Calibrator.py

@@ -300,7 +300,7 @@ def main(calibFile_in, calibFile_out, calibData):
     print(calibFile_in)
     if calibFile_in.split('/')[-1].startswith('r2'):
         arm = 1
-    else:
+    elif not calibFile_in.split('/')[-1].startswith('r'):
         sys.exit(0)
     if len(pos_RE) != nTarget or len(quat_RE) != nTarget:
         # Take measurements of the targets

src/KeyboardController.py

@@ -662,14 +662,14 @@ class KeyboardController(Controller):
 
 
 if __name__=="__main__":
-    #con = KeyboardController(disableExtruder=True)
+    con = KeyboardController(disableExtruder=True)
     #con = KeyboardController()
     #con1 = KeyboardController(robot=1)
-    con2 = KeyboardController(robot=2, disableExtruder=True)
+    #con2 = KeyboardController(robot=1, disableExtruder=True)
 
     print('Collecting configrations...')
-    #configuations = con.prompt_configurations()
+    configuations = con.prompt_configurations()
     #configuations = con1.prompt_configurations()
-    configuations = con2.prompt_configurations()
+    #configuations = con2.prompt_configurations()
     print('Your configurations:')
     print(configuations)

src/main1.py

@@ -83,7 +83,7 @@ def execute_plan(plan,
 
 if __name__ == '__main__':
     plan1 = load_plan('origin')
-    calibration_test = load_plan('r2_cal_test')
+    calibration_test = load_plan('r1_cal_test')
     plan2, flexirex_contours = load_plan('flexirex', contours=True)
 
     # Printing
@@ -91,7 +91,9 @@ if __name__ == '__main__':
     #execute_plan(plan2, con, contours=flexirex_contours, confirm=True, n_trajs=10)
 
     # Test calibration
-    con = Controller(disable_extruder=True)
+    #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)
 
     # go to origin and stay (to check calibration)