Commit f9cb4bf9 authored by Jayant Khatkar's avatar Jayant Khatkar

r1 calibration done, living saving of calibration in case it crashes (#19)

parent b60fcca2
calibration:
pos_ET: [0.14982225501989826, -0.048118796065005576, -0.03482924194973972]
pos_WR: [0.033410320051684865, -0.5779185680332759, -0.129]
pos_ET: [0.15007592984204163, -0.04830698520623148, -0.031668261140098686]
pos_WR: [0.03655708997718542, -0.575405839173846, -0.13224784915908203]
quat_ET: [-0.2705980500730985, -0.6532814824381882, 0.27059805007309845, 0.6532814824381883]
quat_WR: [-0.0035406624036226648, 0.00401943913741378, -0.9999807593358486, 0.003128702105328147]
quat_WR: [-0.004907671722772035, 0.0026006325187357495, -0.9999828094804524, 0.0018794180869107977]
measurements:
errors:
- [-0.00035093605232278113, 0.0015154904646795808, 0.0005004698912217387]
- [0.0008203797060748544, 0.0008922491648686859, 0.0002722545866968018]
- [0.001289894680197884, 0.0009346809299097258, -0.0011471590539748888]
- [-0.00026782915015455266, -0.00019411428454588426, -0.00017438502446384674]
- [-0.000711953482581712, -0.00017432903503347696, 0.0007754958989695804]
- [-0.0009272510092923555, -0.001473229551918892, 0.0009148858434974705]
- [-0.0007779454099607619, -0.0013991536560543083, -9.067649884858531e-05]
- [-0.0006907081723367681, -0.00013629415861853315, 0.0005679499481896777]
- [0.0014934971392888685, -0.000434524141417425, -0.001318136805991993]
- [0.00012285175106990787, 0.0004692242681248698, -0.0003006987853121923]
pos_RE:
- [0.28238526739189035, -0.743445281602937, 0.2753943928759848]
- [0.06856808024088912, -0.7433152737798132, 0.27773399071687555]
- [-0.14592886427691276, -0.7424717736062426, 0.28125724809030916]
- [-0.14669659051873055, -0.528554844189385, 0.2813981571448582]
- [-0.1463260383308909, -0.3135266064730779, 0.28156287698542204]
- [0.06844080035242153, -0.3156302723662834, 0.27930760468097077]
- [0.283587823473315, -0.31634541818249157, 0.2781937485538753]
- [0.28285396776950816, -0.530129589269635, 0.2764449159253464]
- [0.5066506093478649, -0.20096898804832836, 0.16677352259446088]
- [-0.369184926735048, -0.5275417892542257, 0.2618225946251175]
quat_RE:
- [0.004515506751761712, 0.7093312647790816, 0.0005263393718474751, 0.7048606174078313]
- [0.004551772876819091, 0.7093577385565347, 0.000732948249208785, 0.7048335568774383]
- [0.004513807736845369, 0.7093838031536429, 0.0007461847780781305, 0.7048075542807879]
- [0.00442392266478542, 0.709359777178923, 0.0006503084317854549, 0.7048324003107893]
- [0.004471491373419877, 0.7093586207919866, 0.0005390274408393596, 0.7048333578388627]
- [0.004466760991158438, 0.7093646956760709, 0.0005683729336144619, 0.7048272508401273]
- [0.004391686236732675, 0.7093756214129284, 0.0005693173620683987, 0.704816725620754]
- [0.00448742433570489, 0.7093749968660662, 0.000748052655762252, 0.7048165841276184]
- [-0.00045970685855608695, 0.6540762178342059, -0.653219981430286, 0.38142855922639246]
- [0.003642363656463826, 0.38561399879651764, 0.002617478574255283, 0.922649297363341]
targets:
pos_W:
- [-0.215, 0.215, 0]
......
......@@ -28,12 +28,12 @@ class Calibrator(object):
self.controller = None
def measureConfigurations(self, pos_W, prior_calibs):
def measureConfigurations(self, prompts, prior_calibs):
"""
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
prompts: list of positions to visit for calibration
"""
if self.controller is None:
if DEBUG:
......@@ -44,9 +44,6 @@ class Calibrator(object):
disableExtruder=True
)
# form prompts to the user to satisfy
prompts = [pos_W[iPos,] for iPos in range(pos_W.shape[0])]
# gather configurations
# [ ( [joint angles] , ROS pose message ) , ... ]
configurations = self.controller.prompt_configurations(prompts, prior_calibs)
......@@ -301,14 +298,15 @@ def main(calibFile_in, calibFile_out, calibData):
# If the number of measurements does not match the number of targets
if len(pos_RE) != nTarget or len(quat_RE) != nTarget:
# Take measurements of the targets
(pos_RE_Mx3,quat_RE_Mx4) = cal.measureConfigurations(pos_W_Mx3, calibration)
pos_RE.extend(pos_RE_Mx3.tolist())
quat_RE.extend(quat_RE_Mx4.tolist())
# And save them to file
with open(calibFile_out,'w') as file:
yaml.dump(calibData,file,
default_flow_style=None)
for i in range(len(pos_RE), nTarget):
(pos_RE_Mx3,quat_RE_Mx4) = cal.measureConfigurations([pos_W_Mx3[i,]], calibration)
pos_RE.extend(pos_RE_Mx3.tolist())
quat_RE.extend(quat_RE_Mx4.tolist())
# And save them to file
with open(calibFile_out,'w') as file:
yaml.dump(calibData,file,
default_flow_style=None)
else:
pos_RE_Mx3 = np.array(pos_RE)
quat_RE_Mx4 = np.array(quat_RE)
......
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