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 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']
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):
"""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
