Commit 70b47707 authored by Jayant Khatkar's avatar Jayant Khatkar
Browse files

create structure for doing shear calibration (#16)

parent e4c29953
shear:
targets:
- [0, 0.215, 0]
- [0.215, -0.215, 0]
- [-0.215, -0.215, 0]
- [0.322, 0, 0]
- [-0.322, -0.322, 0]
#!/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")
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment