shear issue fixed by smaller waypoints in planning (#16)

src/ShearCalibrator.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
-
-
-class Calibrator(object):
-
-    def __init__(self,
-        timeScale=0.25):
-
-        self.timeScale = timeScale
-
-        self.controller = None
-
-    def measureConfigurations(self,pos_W):
-        """
-        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
-        """
-        if self.controller is None:
-            if DEBUG:
-                print("Creating KeyboardController")
-            self.controller = KeyboardController(waitForExtruder=False,timeScale=self.timeScale,disableExtruder=True)
-
-        nPos = pos_W.shape[0]
-
-        ## Form prompts to the user to satisfy
-        prompts = ['Move robot tip to {}'.format(np.array2string(pos_W[iPos,])) for iPos in range(nPos)]
-
-        ## Gather configurations
-        # List of (list of joint angle, ROS pose message)
-        configurations = self.controller.prompt_configurations(prompts)
-
-        pos_RE = [[c[1].pose.position.x,c[1].pose.position.y,c[1].pose.position.z] for c in configurations]
-        quat_RE = [[c[1].pose.orientation.x,c[1].pose.orientation.y,c[1].pose.orientation.z,c[1].pose.orientation.w] for c in configurations]
-
-        pos_RE_Mx3 = np.array(pos_RE)
-        quat_RE_Mx4 = np.array(quat_RE)
-
-        return (pos_RE_Mx3,quat_RE_Mx4)
-
-
-    def calibrate(self,pos_W,pos_RE,quat_RE,quat_ET,
-        initPos_WR=np.zeros(3),
-        initQuat_WR=np.array([0,0,0,1]),
-        initPos_ET=np.zeros(3)):
-        """
-        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: Mx3 set of positions visited for calibration
-        pos_RE: Mx3 set of positions of the end effector relative to the robot's base
-        quat_RE: Mx4 set of orientations of the end effector relative to the robot's base
-        quat_ET: Mx4 set of orientations of the tool tip relative to the end effector
-        """
-        if DEBUG:
-            print('calibrate(self,{},{},{},{},{},{},{})'.format(pos_W,pos_RE,quat_RE,quat_ET,initPos_WR,initQuat_WR,initPos_ET))
-        nMeasure = pos_W.shape[0] # =M
-        if nMeasure < 3:
-            warnings.warn('Should use at least 3 configurations for well-defined calibration parameters')
-
-        # Solve equations for configuration parameters
-        # 3x,4x,3x,M3x
-        (pos_WR,quat_WR,pos_ET,err) = calibrate_robotPose_toolPos(pos_W,pos_RE,quat_RE,quat_ET,initPos_WR,initQuat_WR,initPos_ET)
-
-        return (pos_WR,quat_WR,pos_ET,err)
-
-
-def calibrate_robotPose_toolPos(pos_W,pos_RE,quat_RE,quat_ET,
-    initPos_WR=np.zeros(3),
-    initQuat_WR=np.array([0,0,0,1]),
-    initPos_ET=np.zeros(3)):
-    """
-    Calibration for:
-    - the robot's pose from the world
-    - the tool's position from the end effector
-
-    pos_W: Nx3 [x,y,z] positions
-    pos_RE: Nx3 [x,y,z] position of end effector relative to robot base
-    quat_RE: Nx4 [x,y,z,w] orientation of end effector relative to robot base
-    quat_ET: Nx4 [x,y,z,w] orientation of tool tip relative to end effector
-    initPos_WR: 3x (default: [0,0,0])
-    initQuat_WR: 4x (default: [0,0,0,1])
-    initPos_ET: 3x (default: [0,0,0])
-
-    Returns: (pos_WR,quat_WR,pos_ET)
-    pos_WR: 3x
-    quat_WR: 4x
-    pos_ET: 3x
-    """
-
-    nPose = pos_W.shape[0]
-
-    # pos_T: is just zeros because we touch the tip at pos_W points
-    # Nx3
-    pos_T = np.zeros((nPose,3))
-
-    initR_WR = Rotation.from_quat(initQuat_WR)
-    initRotVec_WR = initR_WR.as_rotvec()
-
-    # 9x
-    x0 = np.concatenate((initPos_WR,initRotVec_WR,initPos_ET))
-
-    sol = optimize.least_squares(lambda x:err_robotPose_toolTrans(x,pos_W,pos_RE,quat_RE,quat_ET,pos_T),x0.flatten())
-
-    if not sol.success:
-        warnings.warn('Optimisation failed')
-
-    # 9x
-    xOpt = sol.x
-
-    # 3x
-    pos_WR = xOpt[0:3]
-    rotVec_WR = xOpt[3:6]
-    pos_ET = xOpt[6:9]
-
-    R_WR = Rotation.from_rotvec(rotVec_WR)
-    # 4x
-    quat_WR = R_WR.as_quat()
-
-    return (pos_WR,quat_WR,pos_ET,err_robotPose_toolTrans(xOpt,pos_W,pos_RE,quat_RE,quat_ET,pos_T))
-
-
-def err_robotPose_toolTrans(x,pos_W,pos_RE,quat_RE,quat_ET,pos_T):
-    """
-    x: 1x9 [pos_WR,rotVec_WR,pos_ET]
-    pos_W: Nx3 [x,y,z] positions
-    pos_RE: Nx3 [x,y,z]
-    quat_RE: Nx4 [x,y,z,w]
-    pos_ET: Nx3 [x,y,z]
-    pos_T: Nx3 [x,y,z]
-
-    Returns N3x dimension array
-    """
-    nPose = pos_RE.shape[0]
-
-    # Nx3
-    pos_WR = np.broadcast_to(x[0:3],(nPose,3))
-    rotVec_WR = np.broadcast_to(x[3:6],(nPose,3))
-    pos_ET = np.broadcast_to(x[6:9],(nPose,3))
-
-    R_WR = Rotation.from_rotvec(rotVec_WR)
-    # Nx4
-    quat_WR = R_WR.as_quat()
-
-    # Nx3
-    transPos = getPos_WT(pos_WR,quat_WR,pos_RE,quat_RE,pos_ET,quat_ET,pos_T)
-
-    # Nx3
-    err = pos_W-transPos
-
-    return err.flatten()
-
-
-def getPos_WT(pos_WR,quat_WR,pos_RE,quat_RE,pos_ET,quat_ET,pos_T):
-    """
-    Transforms a position in tooltip frame to world
-    pos_WR: Nx3 [x,y,z]
-    quat_WR: Nx4 [x,y,z,w]
-    pos_RE: Nx3 [x,y,z]
-    quat_RE: Nx4 [x,y,z,w]
-    pos_ET: Nx3 [x,y,z]
-    quat_ET: Nx4 [x,y,z,w]
-    pos_T: Nx3 [x,y,z]
-
-    Returns: Nx3
-    """
-    nPose = pos_WR.shape[0]
-
-    # Nx4x4
-    T_WR = pose2transform(pos_WR,quat_WR)
-    T_RE = pose2transform(pos_RE,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]
-
-
-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)
-
-
-# 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 update_shear(shear_data, n):
-    return shear_data
-
-
-if __name__=="__main__":
-    parser = argparse.ArgumentParser(description='Calibration for UR5e arm')
-    parser.add_argument('--calib',
-        metavar='calibFile_read',
-        dest='calibfile',
-        help='file storing shear calibration targets and parameters')
-    parser.add_argument('--shear',
-        metavar='Shear file',
-        dest='shearfile',
-        help='file storing shear calibration targets and parameters')
-
-    args = parser.parse_args()
-
-    # read input files
-    with open(args.calibfile, 'r') as f:
-        calib_data = yaml.load(f, Loader=yaml.FullLoader)
-    with open(args.shearfile, 'r') as f:
-        shear_data = yaml.load(f, Loader=yaml.FullLoader)
-
-    shear_targets = np.array()
-
-    print(shear_data)
-    #cal = Calibrator()
-
-    if shear_data['shear'] is None:
-
-        print("Doing Shear Calibration from scratch")
-
-        for i in range(len(shear_data['targets'])):
-            update_shear(shear_data, i)
-
-            # save updated shear
-            with open(args.shearfile, 'w') as f:
-                yaml.dump(shear_data, f, default_flow_style=None)
-
-    elif len(shear_data['shear']) < len(shear_data['targets']):
-
-        print("Completing previously started Shear Calibration")
-
-        for i in range(len(shear_data['targets'])):
-            if i < len(shear_data['shear']):
-                continue
-            update_shear(shear_data, i)
-
-            # save updated shear
-            with open(args.shearfile, 'w') as f:
-                yaml.dump(shear_data, f, default_flow_style=None)
-    else:
-
-        print("Updating Shear Claibration")
-
-        user_in = input("Enter index of target to update")
-
-        while len(user_in) > 0:
-
-            try:
-                i = int(user_in)
-            except:
-                print("Must be a number")
-                user_in = input("Enter index of target to update")
-                continue
-            update_shear(shear_data, i)
-
-            # save updated shear
-            with open(args.shearfile, 'w') as f:
-                yaml.dump(shear_data, f, default_flow_style=None)
-
-            # Get next input
-            user_in = input("Enter index of target to update")

src/utils/plan.py

@@ -82,7 +82,7 @@ def load_plan(model, contours=False):
 
     if contours:
         # Read gcode file
-        contours = gc.decode_gcode("plans/" + model + ".gcode")
+        contours = gc.decode_gcode("plans/" + model + ".gcode", max_wp_dist=20)
         return plan, contours
     else:
         return plan