Commit 32a41a49 authored by Jayant Khatkar's avatar Jayant Khatkar
Browse files

cleanup calibrator existing code (#18)

parent fe331bfd
...@@ -23,41 +23,43 @@ from Controller import getR_ET ...@@ -23,41 +23,43 @@ from Controller import getR_ET
class Calibrator(object): class Calibrator(object):
def __init__(self, def __init__(self, timeScale=0.25):
timeScale=0.25):
self.timeScale = timeScale self.timeScale = timeScale
self.controller = None self.controller = None
def measureConfigurations(self,pos_W):
def measureConfigurations(self, pos_W, prior_calibs):
""" """
Calibrate the arm for the robot's pose relative to the world Calibrate the arm for the robot's pose relative to the world
and the position of the tool tip relative to the end effector. and the position of the tool tip relative to the end effector.
pos_W: Nx3 sequence of positions to visit for calibration pos_W: Nx3 sequence of positions to visit for calibration
""" """
if self.controller is None: if self.controller is None:
if DEBUG: if DEBUG:
print("Creating KeyboardController") print("Creating KeyboardController")
self.controller = KeyboardController(waitForExtruder=False,timeScale=self.timeScale,disableExtruder=True) self.controller = KeyboardController(
waitForExtruder=False,
nPos = pos_W.shape[0] timeScale=self.timeScale,
disableExtruder=True
## Form prompts to the user to satisfy )
prompts = ['Move robot tip to {}'.format(np.array2string(pos_W[iPos,])) for iPos in range(nPos)]
# form prompts to the user to satisfy
## Gather configurations prompts = ['Move robot tip to {}'.format(np.array2string(pos_W[iPos,]))
# List of (list of joint angle, ROS pose message) for iPos in range(pos_W.shape[0])]
# gather configurations
# [ ( [joint angles] , ROS pose message ) , ... ]
configurations = self.controller.prompt_configurations(prompts) 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) pos_RE = [[c[1].pose.position.x, c[1].pose.position.y, c[1].pose.position.z]
quat_RE_Mx4 = np.array(quat_RE) for c in configurations]
return (pos_RE_Mx3,quat_RE_Mx4) 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]
return (np.array(pos_RE), np.array(quat_RE))
def calibrate(self,pos_W,pos_RE,quat_RE,quat_ET, def calibrate(self,pos_W,pos_RE,quat_RE,quat_ET,
...@@ -67,7 +69,7 @@ class Calibrator(object): ...@@ -67,7 +69,7 @@ class Calibrator(object):
""" """
Calibrate the arm for the robot's pose relative to the world Calibrate the arm for the robot's pose relative to the world
and the position of the tool tip relative to the end effector. and the position of the tool tip relative to the end effector.
pos_W: Mx3 set of positions visited for calibration 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 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_RE: Mx4 set of orientations of the end effector relative to the robot's base
...@@ -94,7 +96,7 @@ def calibrate_robotPose_toolPos(pos_W,pos_RE,quat_RE,quat_ET, ...@@ -94,7 +96,7 @@ def calibrate_robotPose_toolPos(pos_W,pos_RE,quat_RE,quat_ET,
Calibration for: Calibration for:
- the robot's pose from the world - the robot's pose from the world
- the tool's position from the end effector - the tool's position from the end effector
pos_W: Nx3 [x,y,z] positions pos_W: Nx3 [x,y,z] positions
pos_RE: Nx3 [x,y,z] position of end effector relative to robot base 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_RE: Nx4 [x,y,z,w] orientation of end effector relative to robot base
...@@ -102,7 +104,7 @@ def calibrate_robotPose_toolPos(pos_W,pos_RE,quat_RE,quat_ET, ...@@ -102,7 +104,7 @@ def calibrate_robotPose_toolPos(pos_W,pos_RE,quat_RE,quat_ET,
initPos_WR: 3x (default: [0,0,0]) initPos_WR: 3x (default: [0,0,0])
initQuat_WR: 4x (default: [0,0,0,1]) initQuat_WR: 4x (default: [0,0,0,1])
initPos_ET: 3x (default: [0,0,0]) initPos_ET: 3x (default: [0,0,0])
Returns: (pos_WR,quat_WR,pos_ET) Returns: (pos_WR,quat_WR,pos_ET)
pos_WR: 3x pos_WR: 3x
quat_WR: 4x quat_WR: 4x
...@@ -149,7 +151,7 @@ def err_robotPose_toolTrans(x,pos_W,pos_RE,quat_RE,quat_ET,pos_T): ...@@ -149,7 +151,7 @@ def err_robotPose_toolTrans(x,pos_W,pos_RE,quat_RE,quat_ET,pos_T):
quat_RE: Nx4 [x,y,z,w] quat_RE: Nx4 [x,y,z,w]
pos_ET: Nx3 [x,y,z] pos_ET: Nx3 [x,y,z]
pos_T: Nx3 [x,y,z] pos_T: Nx3 [x,y,z]
Returns N3x dimension array Returns N3x dimension array
""" """
nPose = pos_RE.shape[0] nPose = pos_RE.shape[0]
...@@ -182,7 +184,7 @@ def getPos_WT(pos_WR,quat_WR,pos_RE,quat_RE,pos_ET,quat_ET,pos_T): ...@@ -182,7 +184,7 @@ def getPos_WT(pos_WR,quat_WR,pos_RE,quat_RE,pos_ET,quat_ET,pos_T):
pos_ET: Nx3 [x,y,z] pos_ET: Nx3 [x,y,z]
quat_ET: Nx4 [x,y,z,w] quat_ET: Nx4 [x,y,z,w]
pos_T: Nx3 [x,y,z] pos_T: Nx3 [x,y,z]
Returns: Nx3 Returns: Nx3
""" """
nPose = pos_WR.shape[0] nPose = pos_WR.shape[0]
...@@ -206,7 +208,7 @@ def pose2transform(pos,quat): ...@@ -206,7 +208,7 @@ def pose2transform(pos,quat):
""" """
pos: Nx3 [x,y,z] pos: Nx3 [x,y,z]
quat: Nx4 [x,y,z,w] quat: Nx4 [x,y,z,w]
Returns: Nx4x4 Returns: Nx4x4
""" """
nPose = pos.shape[0] nPose = pos.shape[0]
...@@ -245,30 +247,10 @@ def printTransforms(T): ...@@ -245,30 +247,10 @@ def printTransforms(T):
print(','.join(strings)) print(','.join(strings))
if __name__=="__main__": def main(calibFile_in, calibFile_out, calibData):
parser = argparse.ArgumentParser(description='Calibration for UR5e arm') """
parser.add_argument('calibFile_in', main Calibration process
metavar='calibFile_read', """
help='file storing calibration targets and calibration parameters')
parser.add_argument('-o','--out',
metavar='calibFile_write',
dest='calibFile_out',
default='',
help='file storing calibration targets and calibration parameters')
args = parser.parse_args()
calibFile_in = args.calibFile_in
if len(args.calibFile_out) > 0:
calibFile_out = args.calibFile_out
else:
# Overwrite the file to read from
calibFile_out = args.calibFile_in
# Read calibration file
with open(calibFile_in,'r') as file:
calibData = yaml.load(file,
Loader=yaml.FullLoader)
if DEBUG: if DEBUG:
print("Creating Calibrator") print("Creating Calibrator")
...@@ -308,10 +290,19 @@ if __name__=="__main__": ...@@ -308,10 +290,19 @@ if __name__=="__main__":
measurements['pos_RE'] = pos_RE measurements['pos_RE'] = pos_RE
measurements['quat_RE'] = quat_RE measurements['quat_RE'] = quat_RE
## Solve nonlinear equations to obtain calibration parameters
# If there is any calibration parameters from before
# also used for faster calibration
if 'calibration' in calibData:
calibration = calibData['calibration']
else:
calibration = {}
calibData['calibration'] = calibration
# If the number of measurements does not match the number of targets # If the number of measurements does not match the number of targets
if len(pos_RE) != nTarget or len(quat_RE) != nTarget: if len(pos_RE) != nTarget or len(quat_RE) != nTarget:
# Take measurements of the targets # Take measurements of the targets
(pos_RE_Mx3,quat_RE_Mx4) = cal.measureConfigurations(pos_W_Mx3) (pos_RE_Mx3,quat_RE_Mx4) = cal.measureConfigurations(pos_W_Mx3, calibration)
pos_RE.extend(pos_RE_Mx3.tolist()) pos_RE.extend(pos_RE_Mx3.tolist())
quat_RE.extend(quat_RE_Mx4.tolist()) quat_RE.extend(quat_RE_Mx4.tolist())
...@@ -323,13 +314,6 @@ if __name__=="__main__": ...@@ -323,13 +314,6 @@ if __name__=="__main__":
pos_RE_Mx3 = np.array(pos_RE) pos_RE_Mx3 = np.array(pos_RE)
quat_RE_Mx4 = np.array(quat_RE) quat_RE_Mx4 = np.array(quat_RE)
## Solve nonlinear equations to obtain calibration parameters
# If there is any calibration parameters from before
if 'calibration' in calibData:
calibration = calibData['calibration']
else:
calibration = {}
calibData['calibration'] = calibration
if 'pos_WR' in calibration and 'quat_WR' in calibration and 'pos_ET' in calibration: if 'pos_WR' in calibration and 'quat_WR' in calibration and 'pos_ET' in calibration:
# Use as initial estimate for solver # Use as initial estimate for solver
...@@ -391,3 +375,31 @@ if __name__=="__main__": ...@@ -391,3 +375,31 @@ if __name__=="__main__":
with open(calibFile_out,'w') as file: with open(calibFile_out,'w') as file:
yaml.dump(calibData,file, yaml.dump(calibData,file,
default_flow_style=None) default_flow_style=None)
if __name__=="__main__":
parser = argparse.ArgumentParser(description='Calibration for UR5e arm')
parser.add_argument('calibFile_in',
metavar='calibFile_read',
help='file storing calibration targets and calibration parameters')
parser.add_argument('-o','--out',
metavar='calibFile_write',
dest='calibFile_out',
default='',
help='file storing calibration targets and calibration parameters')
args = parser.parse_args()
calibFile_in = args.calibFile_in
if len(args.calibFile_out) > 0:
calibFile_out = args.calibFile_out
else:
# Overwrite the file to read from
calibFile_out = args.calibFile_in
# Read calibration file
with open(calibFile_in,'r') as file:
calibData = yaml.load(file,
Loader=yaml.FullLoader)
main(calibFile_in, calibFile_out, calibData)
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