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)