diff --git a/calibrations/r2_tforms_alt.yaml b/calibrations/r2_tforms_alt.yaml
index 11fc58411a490c748086555cf2187f180bfea029..d68f4ba495114d6360d4b7f02f4bb3cf80885e9c 100644
--- a/calibrations/r2_tforms_alt.yaml
+++ b/calibrations/r2_tforms_alt.yaml
@@ -4,28 +4,21 @@ calibration:
quat_ET: [-0.2705980500730985, -0.6532814824381882, 0.27059805007309845, 0.6532814824381883]
quat_WR: [0.005077893908742152, 0.007918175551724624, 0.013359410788764806, 0.9998665129069784]
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:
- - [0.0584231472040356, -0.7888108610213361, 0.29224624613259653]
- - [-0.1453238446293856, -0.35482213917107674, 0.28432730816842033]
- - [0.2834969700403753, -0.36612935013429077, 0.291672094257169]
- - [-0.3807992876767562, -0.566308084352216, 0.28314301995943336]
- - [0.4842187683428001, -0.2379454794306125, 0.19894353741958098]
+ - [-0.2516333141744929, -0.4116135285217244, 0.2838269936302523]
+ - [-0.03569841229825402, -0.4168126158425754, 0.28734125017362583]
quat_RE:
- - [0.27030443249517855, 0.6529994452370946, -0.2706893166892845, 0.6536471006007708]
- - [0.2702398705984472, 0.6530912744201057, -0.27078819741106336, 0.6535410865111108]
- - [0.27031418268873514, 0.653199293004063, -0.2706655963918572, 0.6534531820919948]
- - [0.3529762575660405, 0.353731950105599, -0.1464218844920094, 0.8537224963716084]
- - [0.0744084309460823, 0.7683808434582252, -0.5132226899090738, 0.3750423114437397]
+ - [-0.7125705366105385, 0.012338676350220358, 0.7014665647172886, 0.005970427473748586]
+ - [-0.712339087472529, 0.012479682698013866, 0.7017001944059005, 0.005841159926140043]
targets:
pos_W:
- - [0, -0.215, 0]
- [-0.215, 0.215, 0]
+ - [0, 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, 0]
diff --git a/src/Calibrator.py b/src/Calibrator.py
index b0b67596956f85ffec5a68c7a05763ef6b3a00b9..a3dda9cb300cc91a594e10cd11bb27a3961d647c 100644
--- a/src/Calibrator.py
+++ b/src/Calibrator.py
@@ -28,7 +28,7 @@ class Calibrator(object):
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
and the position of the tool tip relative to the end effector.
@@ -46,7 +46,7 @@ class Calibrator(object):
# gather configurations
# [ ( [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]
for c in configurations]
@@ -296,10 +296,16 @@ def main(calibFile_in, calibFile_out, calibData):
calibData['calibration'] = calibration
# 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:
# Take measurements of the targets
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())
quat_RE.extend(quat_RE_Mx4.tolist())
@@ -370,8 +376,7 @@ def main(calibFile_in, calibFile_out, calibData):
# Save them to file
with open(calibFile_out,'w') as file:
- yaml.dump(calibData,file,
- default_flow_style=None)
+ yaml.dump(calibData,file, default_flow_style=None)
if __name__=="__main__":
@@ -395,8 +400,7 @@ if __name__=="__main__":
calibFile_out = args.calibFile_in
# Read calibration file
- with open(calibFile_in,'r') as file:
- calibData = yaml.load(file,
- Loader=yaml.FullLoader)
+ with open(calibFile_in, 'r') as file:
+ calibData = yaml.load(file, Loader=yaml.FullLoader)
main(calibFile_in, calibFile_out, calibData)
diff --git a/src/KeyboardController.py b/src/KeyboardController.py
index 1867ce38142b7f206b3ddb5527fec0da00376407..31208d276207ffc34ef7e48bbf1a0778f6b7e097 100644
--- a/src/KeyboardController.py
+++ b/src/KeyboardController.py
@@ -37,10 +37,14 @@ def getTransformMat(quat, pos):
return np.linalg.inv(T)
# 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],
[-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):
@@ -542,7 +546,7 @@ class KeyboardController(Controller):
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.
Relative to robot base, the robot can be translated in the:
@@ -586,7 +590,6 @@ class KeyboardController(Controller):
if isinstance(prompt, list):
for p in prompt:
- print(p)
print('Move robot tip to {}'.format(str(p)))
if priors:
@@ -594,7 +597,7 @@ class KeyboardController(Controller):
mat = np.identity(4)
p[2] += 0.01 # go 1cm above bed for safety
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()
p_r = mat[0:3,3]
gpose = Pose()