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