Commit c2e9eeb5 authored by Jayant Khatkar's avatar Jayant Khatkar
Browse files

use different vert_or for r2 (#19)

parent 1c62dace
...@@ -4,28 +4,21 @@ calibration: ...@@ -4,28 +4,21 @@ calibration:
quat_ET: [-0.2705980500730985, -0.6532814824381882, 0.27059805007309845, 0.6532814824381883] quat_ET: [-0.2705980500730985, -0.6532814824381882, 0.27059805007309845, 0.6532814824381883]
quat_WR: [0.005077893908742152, 0.007918175551724624, 0.013359410788764806, 0.9998665129069784] quat_WR: [0.005077893908742152, 0.007918175551724624, 0.013359410788764806, 0.9998665129069784]
measurements: 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: pos_RE:
- [0.0584231472040356, -0.7888108610213361, 0.29224624613259653] - [-0.2516333141744929, -0.4116135285217244, 0.2838269936302523]
- [-0.1453238446293856, -0.35482213917107674, 0.28432730816842033] - [-0.03569841229825402, -0.4168126158425754, 0.28734125017362583]
- [0.2834969700403753, -0.36612935013429077, 0.291672094257169]
- [-0.3807992876767562, -0.566308084352216, 0.28314301995943336]
- [0.4842187683428001, -0.2379454794306125, 0.19894353741958098]
quat_RE: quat_RE:
- [0.27030443249517855, 0.6529994452370946, -0.2706893166892845, 0.6536471006007708] - [-0.7125705366105385, 0.012338676350220358, 0.7014665647172886, 0.005970427473748586]
- [0.2702398705984472, 0.6530912744201057, -0.27078819741106336, 0.6535410865111108] - [-0.712339087472529, 0.012479682698013866, 0.7017001944059005, 0.005841159926140043]
- [0.27031418268873514, 0.653199293004063, -0.2706655963918572, 0.6534531820919948]
- [0.3529762575660405, 0.353731950105599, -0.1464218844920094, 0.8537224963716084]
- [0.0744084309460823, 0.7683808434582252, -0.5132226899090738, 0.3750423114437397]
targets: targets:
pos_W: pos_W:
- [0, -0.215, 0]
- [-0.215, 0.215, 0] - [-0.215, 0.215, 0]
- [0, 0.215, 0]
- [0.215, 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.322, 0]
- [-0.322, 0, 0]
...@@ -28,7 +28,7 @@ class Calibrator(object): ...@@ -28,7 +28,7 @@ class Calibrator(object):
self.controller = None 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 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.
...@@ -46,7 +46,7 @@ class Calibrator(object): ...@@ -46,7 +46,7 @@ class Calibrator(object):
# gather configurations # gather configurations
# [ ( [joint angles] , ROS pose message ) , ... ] # [ ( [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] pos_RE = [[c[1].pose.position.x, c[1].pose.position.y, c[1].pose.position.z]
for c in configurations] for c in configurations]
...@@ -296,10 +296,16 @@ def main(calibFile_in, calibFile_out, calibData): ...@@ -296,10 +296,16 @@ def main(calibFile_in, calibFile_out, calibData):
calibData['calibration'] = 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
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: if len(pos_RE) != nTarget or len(quat_RE) != nTarget:
# Take measurements of the targets # Take measurements of the targets
for i in range(len(pos_RE), nTarget): 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()) pos_RE.extend(pos_RE_Mx3.tolist())
quat_RE.extend(quat_RE_Mx4.tolist()) quat_RE.extend(quat_RE_Mx4.tolist())
...@@ -370,8 +376,7 @@ def main(calibFile_in, calibFile_out, calibData): ...@@ -370,8 +376,7 @@ def main(calibFile_in, calibFile_out, calibData):
# Save them to file # Save them to file
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__": if __name__=="__main__":
...@@ -395,8 +400,7 @@ if __name__=="__main__": ...@@ -395,8 +400,7 @@ if __name__=="__main__":
calibFile_out = args.calibFile_in calibFile_out = args.calibFile_in
# Read calibration file # Read calibration file
with open(calibFile_in,'r') as file: with open(calibFile_in, 'r') as file:
calibData = yaml.load(file, calibData = yaml.load(file, Loader=yaml.FullLoader)
Loader=yaml.FullLoader)
main(calibFile_in, calibFile_out, calibData) main(calibFile_in, calibFile_out, calibData)
...@@ -37,10 +37,14 @@ def getTransformMat(quat, pos): ...@@ -37,10 +37,14 @@ def getTransformMat(quat, pos):
return np.linalg.inv(T) return np.linalg.inv(T)
# also used for fast-calibration-ik # 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], [ 8.33732192e-04, -9.99999285e-01, -8.33877944e-04, -4.81068309e-05],
[-9.99999404e-01, -8.34294187e-04, 6.73821778e-04, 2.74792069e-06], [-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): class KeyboardController(Controller):
...@@ -542,7 +546,7 @@ class KeyboardController(Controller): ...@@ -542,7 +546,7 @@ class KeyboardController(Controller):
pass 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. Prompts user to control the robot to a series of configurations.
Relative to robot base, the robot can be translated in the: Relative to robot base, the robot can be translated in the:
...@@ -586,7 +590,6 @@ class KeyboardController(Controller): ...@@ -586,7 +590,6 @@ class KeyboardController(Controller):
if isinstance(prompt, list): if isinstance(prompt, list):
for p in prompt: for p in prompt:
print(p)
print('Move robot tip to {}'.format(str(p))) print('Move robot tip to {}'.format(str(p)))
if priors: if priors:
...@@ -594,7 +597,7 @@ class KeyboardController(Controller): ...@@ -594,7 +597,7 @@ class KeyboardController(Controller):
mat = np.identity(4) mat = np.identity(4)
p[2] += 0.01 # go 1cm above bed for safety p[2] += 0.01 # go 1cm above bed for safety
mat[:3,3] = p 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() q = Rotation.from_dcm(mat[:3,:3]).as_quat()
p_r = mat[0:3,3] p_r = mat[0:3,3]
gpose = Pose() gpose = Pose()
......
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