def update_urdf(urdf, calibrated_params):
''' Given urdf and calibrated robot_params, updates the URDF. '''
joints = list()
axis = list()
# update each transmission
for chain in calibrated_params.chains.values():
joints += chain._active
axis += numpy.array(chain._axis)[0,:].tolist()
for joint, gearing in zip(chain._active, chain._gearing):
if gearing != 1.0:
update_transmission(urdf, joint, gearing)
for laser in calibrated_params.tilting_lasers.values():
joints.append(laser._config['joint'])
axis.append(5) # TODO: remove this assumption
if laser._gearing != 1.0:
update_transmission(urdf, laser._config['joint'], laser._gearing)
unchanged_joints = [];
# update each transform (or joint calibration)
for joint_name in calibrated_params.transforms.keys():
link_updated = 0
try:
updated_link_params = calibrated_params.transforms[joint_name]._config.T.tolist()[0]
if diff(updated_link_params[0:3], urdf.joint_map[joint_name].origin.position):
print 'Updating xyz for', joint_name, '\n old:', urdf.joint_map[joint_name].origin.position, '\n new:', updated_link_params[0:3]
urdf.joint_map[joint_name].origin.position = updated_link_params[0:3]
link_updated = 1
r1 = RPY_to_angle_axis(urdf.joint_map[joint_name].origin.rotation)
if diff(r1, updated_link_params[3:6]):
# TODO: remove assumption that joints are revolute
if joint_name in joints and urdf.joint_map[joint_name].calibration != None:
cal = urdf.joint_map[joint_name].calibration
a = axis[joints.index(joint_name)]
a = int(a) - 1
print 'Updating calibration for', joint_name, 'by', updated_link_params[a]
if cal.rising != None:
urdf.joint_map[joint_name].calibration.rising += updated_link_params[a]
if cal.falling != None:
urdf.joint_map[joint_name].calibration.falling += updated_link_params[a]
link_updated = 1
else:
rot = angle_axis_to_RPY(updated_link_params[3:6])
print 'Updating rpy for', joint_name, '\n old:', urdf.joint_map[joint_name].origin.rotation, '\n new:', rot
urdf.joint_map[joint_name].origin.rotation = rot
link_updated = 1
except KeyError:
print "Joint removed:", joint_name
print ' xyz:', updated_link_params[0:3]
print ' rpy:', angle_axis_to_RPY(updated_link_params[3:6])
link_updated = 1
if not link_updated:
unchanged_joints.append( joint_name );
print "The following joints weren't updated: \n", ', '.join(unchanged_joints)
return urdf
def params_to_config(self, param_vec):
config_dict = dict()
config_dict["base_link"] = self.base_link
config_dict["transforms"] = dict()
for key, elem in self.transforms.items():
config_dict["transforms"][key] = elem.params_to_config(param_vec[elem.start : elem.end, 0])
config_dict["transforms"][key][3:6] = angle_axis_to_RPY(config_dict["transforms"][key][3:6])
config_dict["sensors"] = {}
config_dict["sensors"]["chains"] = primitive_params_to_config(param_vec, self.chains)
config_dict["sensors"]["tilting_lasers"] = primitive_params_to_config(param_vec, self.tilting_lasers)
config_dict["sensors"]["rectified_cams"] = primitive_params_to_config(param_vec, self.rectified_cams)
config_dict["checkerboards"] = primitive_params_to_config(param_vec, self.checkerboards)
return config_dict
def params_to_config(self, param_vec):
config_dict = dict()
config_dict["base_link"] = self.base_link
config_dict["transforms"] = dict()
for key, elem in self.transforms.items():
config_dict["transforms"][key] = elem.params_to_config(param_vec[elem.start:elem.end,0])
config_dict["transforms"][key][3:6] = angle_axis_to_RPY(config_dict["transforms"][key][3:6])
config_dict["sensors"] = {}
config_dict["sensors"]["chains"] = primitive_params_to_config(param_vec, self.chains)
config_dict["sensors"]["tilting_lasers"] = primitive_params_to_config(param_vec, self.tilting_lasers)
config_dict["sensors"]["rectified_cams"] = primitive_params_to_config(param_vec, self.rectified_cams)
config_dict["checkerboards"] = primitive_params_to_config(param_vec, self.checkerboards)
return config_dict
