def run(self): ''' Start the update process. Values are calculated from torso joint state. Therefore the actual torso jointstate copied to the output file ''' attributes2update = {} torso, arm = self.ts.calc_references(self.debug) if not torso is None: (X, Y, Z) = torso attributes2update["def_torso_lower_neck_tilt_ref"] = X attributes2update["def_torso_pan_ref"] = Y attributes2update["def_torso_upper_neck_tilt_ref"] = Z if not arm is None: (a1, a2, a3, a4, a5, a6, a7) = arm attributes2update["arm_1_ref"] = a1 attributes2update["arm_2_ref"] = a2 attributes2update["arm_3_ref"] = a3 attributes2update["arm_4_ref"] = a4 attributes2update["arm_5_ref"] = a5 attributes2update["arm_6_ref"] = a6 attributes2update["arm_7_ref"] = a7 if self.debug: print attributes2update # update calibration xml based on attributes2update dict urdf_updater = calibration_urdf_updater.CalibrationUrdfUpdater( self.file_urdf_in, self.file_urdf_out, True) urdf_updater.update_references(attributes2update)
def test_update_one_param_string(self): # define params to update attributes2update = {'a': "1.0"} # do update updater = calibration_urdf_updater.CalibrationUrdfUpdater( FILE_IN, FILE_OUT1) updater.update(attributes2update) # compare with correct results self.assertTrue(self._cmp_files(FILE_OUT1, FILE_OUT_RES1))
def test_update_all_params(self): # define params to update attributes2update = {'a': 1.0} attributes2update = {'b': 2.0} attributes2update = {'c': 3.0} # do update updater = calibration_urdf_updater.CalibrationUrdfUpdater( FILE_IN, FILE_OUT2) updater.update(attributes2update) # compare with correct results self.assertTrue(self._cmp_files(FILE_OUT2, FILE_OUT_RES2))
def run(self): ''' Start the update process. Values are read from yaml files and are preprocessed for writing to xml ''' # load yaml files print "--> loading calibrated system from '%s'" % self.file_yaml_calib_system calib_system = yaml.load(file(self.file_yaml_calib_system)) print "--> loading initial system from '%s'" % self.file_yaml_init_system initial_system = yaml.load(file(self.file_yaml_init_system)) attributes2update = {} # process transforms for tf_name in self.tfs2update.keys(): prefix = self.tfs2update[tf_name] (x, y, z, roll, pitch, yaw) = self._convert_transform( calib_system['transforms'][tf_name]) # add to attributes2update dict as "attribute name -> new_value" entries attributes2update[prefix + "x"] = round(x, 10) attributes2update[prefix + "y"] = round(y, 10) attributes2update[prefix + "z"] = round(z, 10) attributes2update[prefix + "roll"] = round(roll, 10) attributes2update[prefix + "pitch"] = round(pitch, 10) attributes2update[prefix + "yaw"] = round(yaw, 10) # process dh chains for chain in self.chains2update: segments = self.chains2update[chain] for id in range(len(segments)): # process segment with id segment = segments[id] # process segment of chain initial_value = initial_system["dh_chains"][chain]["dh"][id][ 0] # get initial theta value of segment for current chain calib_value = calib_system["dh_chains"][chain]["dh"][id][ 0] # get calibr. theta value of segment for current chain new_value = float(calib_value) - float(eval( str(initial_value))) new_value = round(new_value, 10) # add to attributes2update dict as "attribute name -> new_value" entries attributes2update[segment] = new_value # update calibration xml based on attributes2update dict urdf_updater = calibration_urdf_updater.CalibrationUrdfUpdater( self.file_urdf_in, self.file_urdf_out, self.debug) urdf_updater.update(attributes2update)