script to collect data to find bed level offset

calibrations/r1_bed.yaml

+0:
+- -0.6912005113271205
+- -0.019345272230380333
+- 0.1340178894628729
+1:
+- -0.34196600134074334
+- -0.011512320290928062
+- 0.16668383277393775
+2:
+- -0.3392953838871119
+- -0.03264003837955299
+- 0.2356014848093443
+3:
+- -0.3445049501838696
+- 0.18725095640828202
+- -0.0027886575681420125
+4:
+- 0.013789011036191956
+- 0.1493006856047863
+- -0.0009622828767152092
+5:
+- 0.3020666225339658
+- -0.12281450937315454
+- 0.0005617955441925074
+6:
+- -0.25690743477523886
+- -0.2987923171207832
+- -0.002272793634302099

src/bed_levelling.py

+#!/usr/bin/env python
+DEBUG = True
+
+## Python libraries
+import warnings
+import sys
+import argparse
+
+## 3rd party libraries
+import yaml
+
+# Scipy (1.2.3)
+from scipy.spatial.transform import Rotation
+from scipy import optimize
+
+# Numpy (1.16.6)
+import numpy as np
+
+## Our libraries
+from KeyboardController import KeyboardController
+from Controller import getR_ET
+
+
+def pose2transform(pos,quat):
+    """
+    pos: Nx3 [x,y,z]
+    quat: Nx4 [x,y,z,w]
+
+    Returns: Nx4x4
+    """
+    nPose = pos.shape[0]
+
+    zeros_1x1x3 = np.zeros((1,1,3))
+    ones_1x1x1 = np.ones((1,1,1))
+    bottom_1x1x4 = np.concatenate((zeros_1x1x3,ones_1x1x1),
+        axis=2)
+    bottom_Nx1x4 = np.broadcast_to(bottom_1x1x4,(nPose,1,4))
+
+    R = Rotation.from_quat(quat)
+
+    R_Nx3x3 = R.as_dcm()
+
+    pos_Nx3x1 = np.expand_dims(pos,2)
+    top_Nx3x4 = np.concatenate((R_Nx3x3,pos_Nx3x1),
+        axis=2)
+
+    return np.concatenate((top_Nx3x4,bottom_Nx1x4),
+        axis=1)
+
+
+def getPos_WT(calibData, pos_RE, quat_RE):
+    """
+    Transforms a position in tooltip frame to world
+    calibData: dictionary of calibration data from yaml file
+    pos_RE: Nx3 [x,y,z]
+    quat_RE: Nx4 [x,y,z,w]
+
+    Returns: Nx3
+    """
+    pos_WR = np.array([calibData['calibration']['pos_WR']])
+    quat_WR = np.array([calibData['calibration']['quat_WR']])
+    pos_ET = np.array([calibData['calibration']['pos_ET']])
+    quat_ET = np.array([calibData['calibration']['quat_ET']])
+
+    nPose = pos_WR.shape[0]
+    pos_T = np.zeros((nPose,3))
+
+    # Nx4x4
+    T_WR = pose2transform(pos_WR,quat_WR)
+    T_RE = pose2transform(np.array(pos_RE),np.array(quat_RE))
+    T_ET = pose2transform(pos_ET,quat_ET)
+
+    # Nx4x1
+    augPos_T = np.ones((nPose,4,1))
+    augPos_T[:,0:3,0] = pos_T
+    augPos_E = np.matmul(T_ET,augPos_T)
+    augPos_R = np.matmul(T_RE,augPos_E)
+    augPos_W = np.matmul(T_WR,augPos_R)
+
+    return augPos_W[:,0:3,0]
+
+
+# T: Nx4x4
+def printTransforms(T):
+    strings = []
+    nTransform = T.shape[0]
+    for iTransform in range(nTransform):
+        string = '[\n'
+        for iRow in range(4):
+            strList = []
+            for iCol in range(4):
+                strList.append('{: .3f}'.format(T[iTransform,iRow,iCol].item()))
+            string += '\t' + ','.join(strList) + '\n'
+        string += ']'
+        strings.append(string)
+    print(','.join(strings))
+
+
+def main(bedFile, calibData, arm):
+    """
+    bedFile: path to yaml file to output bed data to
+    calibData: dictionary of calibration data from yaml file
+    arm: arm unmber for KeyboardController
+    """
+
+    # Get the keyboard controller
+    keyboardController = KeyboardController(robot=arm)
+
+    # get current poses
+    poses = keyboardController.prompt_configurations()
+
+    # extraction poses
+    pos_RE = [[p[1].pose.position.x,
+               p[1].pose.position.y,
+               p[1].pose.position.z]
+               for p in poses]
+    quat_RE = [[p[1].pose.orientation.x,
+                p[1].pose.orientation.y,
+                p[1].pose.orientation.z,
+                p[1].pose.orientation.z] 
+                for p in poses]    
+
+    # get the world positions
+    pos_WT = getPos_WT(calibData, pos_RE, quat_RE)
+    pos_WT2 = {i:p.tolist() for i,p in enumerate(pos_WT)}
+    # Save them to file
+    with open(bedFile, 'w') as f:
+        print("bed poses saved to {}".format(bedFile))
+        yaml.dump(pos_WT2, f)
+    return
+
+
+if __name__=="__main__":
+    parser = argparse.ArgumentParser(description='Bed leveling for an Arm')
+    parser.add_argument('calibFile',
+        metavar='calibFile_read',
+        help='file storing calibration targets for arm')
+    parser.add_argument('bedFile',
+        metavar='bed_level_out',
+        help='file storing bed_levelling info on')
+
+    args = parser.parse_args()
+
+    calibFile_in = args.calibFile
+    if len(args.bedFile) > 0:
+        bedFile = args.bedFile
+    else:
+        raise Exception("Must provide an output file")
+
+    # Read calibration file
+    with open(calibFile_in, 'r') as file:
+        calibData = yaml.load(file, Loader=yaml.FullLoader)
+
+    main(bedFile, calibData, 1) # robot 1 or 2