def parseJoint(joint): """ Args: joint: Returns: """ jointdict = {a: joint.attrib[a] for a in joint.attrib} pose = parsePose(joint.find('origin')) jointdict['parent'] = joint.find('parent').attrib['link'] jointdict['child'] = joint.find('child').attrib['link'] axis = joint.find('axis') if axis is not None: jointdict['axis'] = gUtils.parse_text(axis.attrib['xyz']) limit = joint.find('limit') if limit is not None: jointdict['limits'] = {a: gUtils.parse_text(limit.attrib[a]) for a in limit.attrib} for category in ('dynamics', 'calibration', 'safety_controller', 'mimic'): data = joint.find(category) try: jointdict[category] = {a: gUtils.parse_text(data.attrib[a]) for a in data.attrib} except AttributeError: pass # no such category return jointdict, pose
def parseLink(link, sourcefilepath=None): """This function parses the link from the given link dict object. :param link: The link you want to :return: """ #print(link.attrib['name'] + ', ', end='') newlink = {a: link.attrib[a] for a in link.attrib} link_name = link.attrib['name'] parseInertial(newlink, link) #no_visual_geo = parseVisual(newlink, link) #no_collision_geo = parseCollision(newlink, link) #handle_missing_geometry(no_visual_geo, no_collision_geo, newlink) #parse visual and collision objects for objtype in ['visual', 'collision']: newlink[objtype] = {} i = 0 for xmlelement in link.iter(objtype): try: elementname = xmlelement.attrib['name'] except KeyError: elementname = objtype + '_' + str(i) + '_' + newlink['name'] i += 1 newlink[objtype][elementname] = {a: xmlelement.attrib[a] for a in xmlelement.attrib} dictelement = newlink[objtype][elementname] #viscol_order[objtype].append(elementname) dictelement['name'] = elementname dictelement['pose'] = parsePose(xmlelement.find('origin')) geometry = xmlelement.find('geometry') if geometry is not None: dictelement['geometry'] = {a: gUtils.parse_text(geometry[0].attrib[a]) for a in geometry[0].attrib} dictelement['geometry']['type'] = geometry[0].tag if geometry[0].tag == 'mesh': dictelement['geometry']['filename'] = geometry[0].attrib['filename'] if sourcefilepath: dictelement['geometry']['sourcefilepath'] = sourcefilepath try: dictelement['geometry']['scale'] = gUtils.parse_text(geometry[0].attrib['scale']) except KeyError: dictelement['geometry']['scale'] = [1.0, 1.0, 1.0] material = xmlelement.find('material') if material is not None: dictelement['material'] = {'name': material.attrib['name']} # We don't need to do the following, as any material with color or texture # will be parsed in the parsing of materials in parseModel # This might be necessary if there are name conflicts etc. #color = material.find('color') #if color is not None: # dictelement['material']['color'] = parse_text(color.attrib['rgba']) #element_order['viscol'][link_name] = viscol_order if newlink == {}: print("\n### WARNING:", newlink['name'], "is empty.") return newlink
def parseJoint(joint): newjoint = {a: joint.attrib[a] for a in joint.attrib} pose = parsePose(joint.find('origin')) newjoint['parent'] = joint.find('parent').attrib['link'] newjoint['child'] = joint.find('child').attrib['link'] axis = joint.find('axis') if axis is not None: newjoint['axis'] = gUtils.parse_text(axis.attrib['xyz']) limit = joint.find('limit') if limit is not None: newjoint['limits'] = {a: gUtils.parse_text(limit.attrib[a]) for a in limit.attrib} #calibration #dynamics #limit #mimic #safety_controller return newjoint, pose
def parsePose(origin): """This function parses the robot models pose and returns it as a dictionary. :param origin: The origin blender object to parse the pose from. :type orign: blender object. :return: dict -- The origins pose. """ pose = {} if origin is not None: try: pose['translation'] = gUtils.parse_text(origin.attrib['xyz']) except KeyError: pose['translation'] = [0.0, 0.0, 0.0] try: pose['rotation_euler'] = gUtils.parse_text(origin.attrib['rpy']) except KeyError: pose['rotation_euler'] = [0.0, 0.0, 0.0] else: pose['translation'] = [0.0, 0.0, 0.0] pose['rotation_euler'] = [0.0, 0.0, 0.0] return pose
def parsePose(origin): """This function parses the robot models pose and returns it as a dictionary. Args: origin: The origin blender object to parse the pose from. Returns: : dict -- The origins pose. """ pose = {} if origin is not None: try: pose['translation'] = gUtils.parse_text(origin.attrib['xyz']) except KeyError: pose['translation'] = [0.0, 0.0, 0.0] try: pose['rotation_euler'] = gUtils.parse_text(origin.attrib['rpy']) except KeyError: pose['rotation_euler'] = [0.0, 0.0, 0.0] else: pose['translation'] = [0.0, 0.0, 0.0] pose['rotation_euler'] = [0.0, 0.0, 0.0] return pose
def parseLink(link, urdffilepath): """Parses a URDF link xml definition. Args: link(xml.etree.ElementTree.ElementTree): xml representation of the link urdffilepath(str): path of originating urdf file (for filename handling) Returns: : dict -- model representation of the link """ newlink = {a: link.attrib[a] for a in link.attrib} newlink['children'] = [] inertial = parseInertial(link) if inertial: newlink['inertial'] = inertial for objtype in ['visual', 'collision']: log(' Parsing ' + objtype + ' elements...', 'DEBUG') newlink[objtype] = {} for xmlelement in link.iter(objtype): # generate name for visual/collision representation if 'name' not in xmlelement.attrib: elementname = objtype + '_' + str(len(newlink[objtype])) + '_' + newlink['name'] else: elementname = xmlelement.attrib['name'] # assign values to element dictionary elementdict = {a: xmlelement.attrib[a] for a in xmlelement.attrib} elementdict['name'] = elementname elementdict['pose'] = parsePose(xmlelement.find('origin')) # gather material material = xmlelement.find('material') if material is not None: elementdict['material'] = material.attrib['name'] # objects without geometry skip the last part geometry = xmlelement.find('geometry') if geometry is None: newlink[objtype][elementname] = elementdict continue # gather geometry information for visual/collision elementdict['geometry'] = { a: gUtils.parse_text(geometry[0].attrib[a]) for a in geometry[0].attrib } elementdict['geometry']['type'] = geometry[0].tag # gather mesh information if geometry[0].tag == 'mesh': # interpret filename filename = geometry[0].attrib['filename'] filepath = path.normpath(path.join(path.dirname(urdffilepath), filename)) log( " Filepath for element " + elementname + ': ' + path.relpath(filepath, start=urdffilepath), 'DEBUG', ) # Remove 'urdf/package://{package_name}' to workaround the lack of rospack here, # assuming the urdf file is in the 'urdf' folder and meshes are in the 'meshes' # folder at the same level. if 'package://' in filepath: filepath = re.sub(r'(.*)urdf/package://([^/]+)/(.*)', '\\1\\3', filepath) elementdict['geometry']['filename'] = filepath # read scale if 'scale' in geometry[0].attrib: elementdict['geometry']['scale'] = gUtils.parse_text( geometry[0].attrib['scale'] ) else: elementdict['geometry']['scale'] = [1.0, 1.0, 1.0] newlink[objtype][elementname] = elementdict if newlink == {}: log("Link information for " + newlink['name'] + " is empty.", 'WARNING') return newlink
def importUrdf(filepath): """Parses the URDF representation of the model and builds a model dictionary from it. The URDF file is opened from the filepath. If it does not exist, an empty dictionary is returned. Args: filepath: str Returns: dict -- model representation of the URDF file """ model = {} log("Parsing URDF model from " + filepath, 'INFO') if not path.exists(filepath): log("Could not open URDF file. File not found: " + filepath, 'ERROR') return {} # load element tree from file tree = ET.parse(filepath) root = tree.getroot() model['name'] = root.attrib['name'] if 'version' in root.attrib: model['version'] = root.attrib['version'] links = {} log("Parsing links...", 'INFO') for link in root.iter('link'): log(" Adding link {}.".format(link.attrib['name']), 'DEBUG') links[link.attrib['name']] = parseLink(link, filepath) model['links'] = links joints = {} log("Parsing joints...", 'INFO') for joint in root.iter('joint'): # this is needed as there are "joint" tags e.g. in transmission if joint.find('parent') is not None: # parse joint from elementtree log(" Adding joint {} ...".format(joint.attrib['name']), 'DEBUG') newjoint, pose = parseJoint(joint) model['links'][newjoint['child']]['pose'] = pose joints[newjoint['name']] = newjoint # add parent-child hierarchy to link information parentlink = model['links'][newjoint['parent']] childlink = model['links'][newjoint['child']] childlink['parent'] = newjoint['parent'] parentlink['children'].append(newjoint['child']) log( " ... and connected parent link {} to {}.".format( parentlink['name'], childlink['name'] ), 'DEBUG', ) model['joints'] = joints # find any links that still have no pose (most likely because they had no parent) for link in links: if 'pose' not in links[link]: links[link]['pose'] = parsePose(None) log("Parsing materials...", 'INFO') materials = {} for material in root.iter('material'): color = material.find('color') # add only materials with specified color if color is not None: log(" Adding material {}.".format(material.attrib['name']), 'DEBUG') newmat = {a: material.attrib[a] for a in material.attrib} newmat['diffuse'] = gUtils.parse_text(color.attrib['rgba']) newmat['specular'] = (1., 1., 1.) # duplicates are overwritten, but not silent if newmat['name'] in materials: log(" Overwriting duplicate material {}!".format(newmat['name']), 'WARNING') materials[newmat['name']] = newmat model['materials'] = materials return model
def parseLink(link, urdffilepath=None): """Parses a URDF link xml definition. Args: link: link to be parsed urdffilepath: path of originating urdf file (for filename handling) (Default value = None) Returns: """ newlink = {a: link.attrib[a] for a in link.attrib} newlink['children'] = [] log('Parsing link ' + newlink['name'] + '...', 'INFO') inertial = parseInertial(link) if inertial: newlink['inertial'] = inertial # TODO delete me? #no_visual_geo = parseVisual(newlink, link) #no_collision_geo = parseCollision(newlink, link) #handle_missing_geometry(no_visual_geo, no_collision_geo, newlink) #parse visual and collision objects for objtype in ['visual', 'collision']: log('Parsing ' + objtype + ' elements...', 'INFO') newlink[objtype] = {} for xmlelement in link.iter(objtype): try: elementname = xmlelement.attrib['name'] except KeyError: elementname = objtype + '_' + str(len( newlink[objtype])) + '_' + newlink['name'] newlink[objtype][elementname] = { a: xmlelement.attrib[a] for a in xmlelement.attrib } elementdict = newlink[objtype][elementname] # TODO delete me? #viscol_order[objtype].append(elementname) elementdict['name'] = elementname elementdict['pose'] = parsePose(xmlelement.find('origin')) geometry = xmlelement.find('geometry') if geometry is not None: elementdict['geometry'] = { a: gUtils.parse_text(geometry[0].attrib[a]) for a in geometry[0].attrib } elementdict['geometry']['type'] = geometry[0].tag if geometry[0].tag == 'mesh': # interpret filename filename = geometry[0].attrib['filename'] filepath = path.normpath( path.join(path.dirname(urdffilepath), filename)) log( 'filepath for element ' + elementname + ': ' + filepath, 'DEBUG') # Remove 'urdf/package://{package_name}' to workaround the lack of rospack here, assuming the # urdf file is in the 'urdf' folder and meshes are in the 'meshes' folder at the same level. if 'package://' in filepath: filepath = re.sub(r'(.*)urdf/package://([^/]+)/(.*)', '\\1\\3', filepath) elementdict['geometry']['filename'] = filepath # read scale try: elementdict['geometry']['scale'] = gUtils.parse_text( geometry[0].attrib['scale']) except KeyError: elementdict['geometry']['scale'] = [1.0, 1.0, 1.0] material = xmlelement.find('material') if material is not None: elementdict['material'] = material.attrib['name'] #element_order['viscol'][link_name] = viscol_order if newlink == {}: # TODO convert to log? print("\n### WARNING:", newlink['name'], "is empty.") return newlink
def importUrdf(filepath): """This function parses the whole URDF representation of the model and builds the model dictionary from it. The created model is stored in the model value of the parser and the URDF file is specified by the filepath given to the Parser. :return: Nothing. Args: filepath: Returns: """ model = {} # TODO delete me? #element_order = {'links': [], 'joints': [], 'viscol': {}, 'materials': []} log("Parsing URDF model from " + filepath, "INFO") # TODO filepath consistency? tree = ET.parse(filepath) # TODO comment needed? root = tree.getroot() #[0] model["name"] = root.attrib["name"] if 'version' in root.attrib: model["version"] = root.attrib['version'] # parse links links = {} log("Parsing links...", "INFO") for link in root.iter('link'): links[link.attrib['name']] = parseLink(link, filepath) # TODO delete me? #element_order['links'].append(links.attrib['name']) #viscol_order = {'visual': [], 'collision': []} model['links'] = links # parse joints joints = {} log("Parsing joints...", "INFO") for joint in root.iter('joint'): # this is needed as there are "joint" tags e.g. in transmission if joint.find('parent') is not None: newjoint, pose = parseJoint(joint) # TODO delete me? #element_order['joints'].append(joint.attrib['name']) model['links'][newjoint['child']]['pose'] = pose joints[newjoint['name']] = newjoint model['joints'] = joints # find any links that still have no pose (most likely because they had no parent) for link in links: if 'pose' not in links[link]: links[link]['pose'] = parsePose(None) # write parent-child information to links log("Writing parent-child information to links...", "INFO") for j in model['joints']: joint = model['joints'][j] parentlink = model['links'][joint['parent']] childlink = model['links'][joint['child']] childlink['parent'] = joint['parent'] parentlink['children'].append(joint['child']) # parse materials log("Parsing materials..", 'INFO') materiallist = [] for material in root.iter('material'): newmaterial = {a: material.attrib[a] for a in material.attrib} color = material.find('color') if color is not None: newmaterial['color'] = gUtils.parse_text(color.attrib['rgba']) materiallist.append(newmaterial) # simply overwrite duplicates for m in materiallist: materials.createMaterial(m['name'], tuple(m['color'][0:3]), (1, 1, 1), m['color'][-1]) model['materials'] = {m['name']: m for m in materiallist} # TODO delete me? #element_order['materials'].append(m['name']) return model
def parseLink(link, urdffilepath): """Parses a URDF link xml definition. Args: link(xml.etree.ElementTree.ElementTree): xml representation of the link urdffilepath(str): path of originating urdf file (for filename handling) Returns: : dict -- model representation of the link """ newlink = {a: link.attrib[a] for a in link.attrib} newlink['children'] = [] inertial = parseInertial(link) if inertial: newlink['inertial'] = inertial for objtype in ['visual', 'collision']: log(' Parsing ' + objtype + ' elements...', 'DEBUG') newlink[objtype] = {} for xmlelement in link.iter(objtype): # generate name for visual/collision representation if 'name' not in xmlelement.attrib: elementname = objtype + '_' + str(len( newlink[objtype])) + '_' + newlink['name'] else: elementname = xmlelement.attrib['name'] # assign values to element dictionary elementdict = {a: xmlelement.attrib[a] for a in xmlelement.attrib} elementdict['name'] = elementname elementdict['pose'] = parsePose(xmlelement.find('origin')) # gather material material = xmlelement.find('material') if material is not None: elementdict['material'] = material.attrib['name'] # objects without geometry skip the last part geometry = xmlelement.find('geometry') if geometry is None: newlink[objtype][elementname] = elementdict continue # gather geometry information for visual/collision elementdict['geometry'] = { a: gUtils.parse_text(geometry[0].attrib[a]) for a in geometry[0].attrib } elementdict['geometry']['type'] = geometry[0].tag # gather mesh information if geometry[0].tag == 'mesh': # interpret filename filename = geometry[0].attrib['filename'] # Some URDFs use rospack "package://" or "urdf/package://" # We'll assume the package root is in Phobos models dir filename = filename.split('package://')[-1] phobos_models = bUtils.getPhobosPreferences().modelsfolder filepath = path.join(phobos_models, filename) elementdict['geometry']['filename'] = filepath log( "Filepath for {element} mesh: {path}".format( element=elementname, path=filepath), 'DEBUG') # read scale if 'scale' in geometry[0].attrib: elementdict['geometry']['scale'] = gUtils.parse_text( geometry[0].attrib['scale']) else: elementdict['geometry']['scale'] = [1.0, 1.0, 1.0] newlink[objtype][elementname] = elementdict if newlink == {}: log("Link information for " + newlink['name'] + " is empty.", 'WARNING') return newlink