diff --git a/calibrations/r1_tforms_alt.yaml b/calibrations/r1_tforms_alt.yaml index 5ab6b14e5d68842cd4f6842ee47d1d53250c57fe..8c7876986c1d422cb4dfc5fe6599f1a8d34d4eed 100644 --- a/calibrations/r1_tforms_alt.yaml +++ b/calibrations/r1_tforms_alt.yaml @@ -1,8 +1,42 @@ 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] diff --git a/src/Calibrator.py b/src/Calibrator.py index 336635a1bfab4cdb329acbb8b3e75a946d755459..b0b67596956f85ffec5a68c7a05763ef6b3a00b9 100644 --- a/src/Calibrator.py +++ b/src/Calibrator.py @@ -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)