use different vert_or for r2 (#19)

calibrations/r2_tforms_alt.yaml

@@ -4,28 +4,21 @@ calibration:
   quat_ET: [-0.2705980500730985, -0.6532814824381882, 0.27059805007309845, 0.6532814824381883]
   quat_WR: [0.005077893908742152, 0.007918175551724624, 0.013359410788764806, 0.9998665129069784]
 measurements:
-  errors:
-  - [-0.0002168960447584468, -0.0010668195200736752, 0.00018006898437752938]
-  - [0.00016928366139346007, 0.0005170638802264593, 0.00037850513702630817]
-  - [0.0011599950953162796, 0.0003526010651510314, -0.00012294180296171242]
-  - [-0.0003939341283262232, -0.00011329636962762249, -0.00038522469969973216]
-  - [-0.0007184485836231325, 0.00031045094431508824, -5.0407618752634775e-05]
   pos_RE:
-  - [0.0584231472040356, -0.7888108610213361, 0.29224624613259653]
-  - [-0.1453238446293856, -0.35482213917107674, 0.28432730816842033]
-  - [0.2834969700403753, -0.36612935013429077, 0.291672094257169]
-  - [-0.3807992876767562, -0.566308084352216, 0.28314301995943336]
-  - [0.4842187683428001, -0.2379454794306125, 0.19894353741958098]
+  - [-0.2516333141744929, -0.4116135285217244, 0.2838269936302523]
+  - [-0.03569841229825402, -0.4168126158425754, 0.28734125017362583]
   quat_RE:
-  - [0.27030443249517855, 0.6529994452370946, -0.2706893166892845, 0.6536471006007708]
-  - [0.2702398705984472, 0.6530912744201057, -0.27078819741106336, 0.6535410865111108]
-  - [0.27031418268873514, 0.653199293004063, -0.2706655963918572, 0.6534531820919948]
-  - [0.3529762575660405, 0.353731950105599, -0.1464218844920094, 0.8537224963716084]
-  - [0.0744084309460823, 0.7683808434582252, -0.5132226899090738, 0.3750423114437397]
+  - [-0.7125705366105385, 0.012338676350220358, 0.7014665647172886, 0.005970427473748586]
+  - [-0.712339087472529, 0.012479682698013866, 0.7017001944059005, 0.005841159926140043]
 targets:
   pos_W:
-  - [0, -0.215, 0]
   - [-0.215, 0.215, 0]
+  - [0, 0.215, 0]
   - [0.215, 0.215, 0]
-  - [-0.322, 0, 0]
+  - [0.215, 0, 0]
+  - [0.215, -0.215, 0]
+  - [0, -0.215, 0]
+  - [-0.215, -0.215, 0]
+  - [-0.215, 0, 0]
   - [0.322, 0.322, 0]
+  - [-0.322, 0, 0]

src/Calibrator.py

@@ -28,7 +28,7 @@ class Calibrator(object):
         self.controller = None
 
 
-    def measureConfigurations(self, prompts, prior_calibs):
+    def measureConfigurations(self, prompts, prior_calibs, arm):
         """
         Calibrate the arm for the robot's pose relative to the world
         and the position of the tool tip relative to the end effector.
@@ -46,7 +46,7 @@ class Calibrator(object):
 
         # gather configurations
         # [ ( [joint angles] , ROS pose message ) , ... ]
-        configurations = self.controller.prompt_configurations(prompts, prior_calibs)
+        configurations = self.controller.prompt_configurations(prompts, prior_calibs, arm=arm)
 
         pos_RE = [[c[1].pose.position.x, c[1].pose.position.y, c[1].pose.position.z]
                 for c in configurations]
@@ -296,10 +296,16 @@ def main(calibFile_in, calibFile_out, calibData):
         calibData['calibration'] = calibration
 
     # If the number of measurements does not match the number of targets
+    arm = 0
+    print(calibFile_in)
+    if calibFile_in.split('/')[-1].startswith('r2'):
+        arm = 1
+    else:
+        sys.exit(0)
     if len(pos_RE) != nTarget or len(quat_RE) != nTarget:
         # Take measurements of the targets
         for i in range(len(pos_RE), nTarget):
-            (pos_RE_Mx3,quat_RE_Mx4) = cal.measureConfigurations([pos_W_Mx3[i,]], calibration)
+            (pos_RE_Mx3,quat_RE_Mx4) = cal.measureConfigurations([pos_W_Mx3[i,]], calibration, arm)
             pos_RE.extend(pos_RE_Mx3.tolist())
             quat_RE.extend(quat_RE_Mx4.tolist())
 
@@ -370,8 +376,7 @@ def main(calibFile_in, calibFile_out, calibData):
 
     # Save them to file
     with open(calibFile_out,'w') as file:
-        yaml.dump(calibData,file,
-            default_flow_style=None)
+        yaml.dump(calibData,file, default_flow_style=None)
 
 
 if __name__=="__main__":
@@ -395,8 +400,7 @@ if __name__=="__main__":
         calibFile_out = args.calibFile_in
 
     # Read calibration file
-    with open(calibFile_in,'r') as file:
-        calibData = yaml.load(file,
-            Loader=yaml.FullLoader)
+    with open(calibFile_in, 'r') as file:
+        calibData = yaml.load(file, Loader=yaml.FullLoader)
 
     main(calibFile_in, calibFile_out, calibData)

src/KeyboardController.py

@@ -37,10 +37,14 @@ def getTransformMat(quat, pos):
     return np.linalg.inv(T)
 
 # also used for fast-calibration-ik
-_vert_or = np.array([[-6.74517010e-04,  8.33315658e-04, -9.99999404e-01, -2.90597873e-05],
+_vert_or = [np.array([[-6.74517010e-04,  8.33315658e-04, -9.99999404e-01, -2.90597873e-05],
                      [ 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]])
+                     [ 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], # 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]])]
 
 
 class KeyboardController(Controller):
@@ -542,7 +546,7 @@ class KeyboardController(Controller):
             pass
 
 
-    def prompt_configurations(self, prompt=None, prior_calibs=None):
+    def prompt_configurations(self, prompt=None, prior_calibs=None, arm=0):
         """
         Prompts user to control the robot to a series of configurations.
         Relative to robot base, the robot can be translated in the:
@@ -586,7 +590,6 @@ class KeyboardController(Controller):
         if isinstance(prompt, list):
 
             for p in prompt:
-                print(p)
                 print('Move robot tip to {}'.format(str(p)))
 
                 if priors:
@@ -594,7 +597,7 @@ class KeyboardController(Controller):
                     mat = np.identity(4)
                     p[2] += 0.01 # go 1cm above bed for safety
                     mat[:3,3] = p
-                    mat = world2armT.dot(mat).dot(_vert_or).dot(tip2armT)
+                    mat = world2armT.dot(mat).dot(_vert_or[arm]).dot(tip2armT)
                     q = Rotation.from_dcm(mat[:3,:3]).as_quat()
                     p_r = mat[0:3,3]
                     gpose = Pose()