Commit c2e9eeb5 authored by Jayant Khatkar's avatar Jayant Khatkar

use different vert_or for r2 (#19)

parent 1c62dace
......@@ -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]
......@@ -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)
......@@ -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()
......
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