r1 calibration done, living saving of calibration in case it crashes (#19)

calibrations/r1_tforms_alt.yaml

 calibration:
-  pos_ET: [0.14982225501989826, -0.048118796065005576, -0.03482924194973972]
-  pos_WR: [0.033410320051684865, -0.5779185680332759, -0.129]
+  pos_ET: [0.15007592984204163, -0.04830698520623148, -0.031668261140098686]
+  pos_WR: [0.03655708997718542, -0.575405839173846, -0.13224784915908203]
   quat_ET: [-0.2705980500730985, -0.6532814824381882, 0.27059805007309845, 0.6532814824381883]
-  quat_WR: [-0.0035406624036226648, 0.00401943913741378, -0.9999807593358486, 0.003128702105328147]
+  quat_WR: [-0.004907671722772035, 0.0026006325187357495, -0.9999828094804524, 0.0018794180869107977]
+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]
+  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]
+  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]
 targets:
   pos_W:
   - [-0.215, 0.215, 0]

src/Calibrator.py

@@ -28,12 +28,12 @@ class Calibrator(object):
         self.controller = None
 
 
-    def measureConfigurations(self, pos_W, prior_calibs):
+    def measureConfigurations(self, prompts, prior_calibs):
         """
         Calibrate the arm for the robot's pose relative to the world
         and the position of the tool tip relative to the end effector.
 
-        pos_W: Nx3 sequence of positions to visit for calibration
+        prompts: list of positions to visit for calibration
         """
         if self.controller is None:
             if DEBUG:
@@ -44,9 +44,6 @@ class Calibrator(object):
                     disableExtruder=True
                     )
 
-        # form prompts to the user to satisfy
-        prompts = [pos_W[iPos,] for iPos in range(pos_W.shape[0])]
-
         # gather configurations
         # [ ( [joint angles] , ROS pose message ) , ... ]
         configurations = self.controller.prompt_configurations(prompts, prior_calibs)
@@ -301,14 +298,15 @@ def main(calibFile_in, calibFile_out, calibData):
     # If the number of measurements does not match the number of targets
     if len(pos_RE) != nTarget or len(quat_RE) != nTarget:
         # Take measurements of the targets
-        (pos_RE_Mx3,quat_RE_Mx4) = cal.measureConfigurations(pos_W_Mx3, calibration)
-        pos_RE.extend(pos_RE_Mx3.tolist())
-        quat_RE.extend(quat_RE_Mx4.tolist())
-
-        # And save them to file
-        with open(calibFile_out,'w') as file:
-            yaml.dump(calibData,file,
-                default_flow_style=None)
+        for i in range(len(pos_RE), nTarget):
+            (pos_RE_Mx3,quat_RE_Mx4) = cal.measureConfigurations([pos_W_Mx3[i,]], calibration)
+            pos_RE.extend(pos_RE_Mx3.tolist())
+            quat_RE.extend(quat_RE_Mx4.tolist())
+
+            # And save them to file
+            with open(calibFile_out,'w') as file:
+                yaml.dump(calibData,file,
+                    default_flow_style=None)
     else:
         pos_RE_Mx3 = np.array(pos_RE)
         quat_RE_Mx4 = np.array(quat_RE)