def writeURDFGeometry(output, element):
"""This functions writes the URDF geometry for a given element at the end of a given String.
:param output: The String to append the URDF output string on.
:type output: str
:param element: A certain element to parse into URDF.
:type element: dict
:return: str -- The extended String
"""
geometry = element['geometry']
try:
output.append(indent * 4 + '<geometry>\n')
if geometry['type'] == 'box':
output.append(xmlline(5, 'box', ['size'], [l2str(geometry['size'])]))
elif geometry['type'] == "cylinder":
output.append(xmlline(5, 'cylinder', ['radius', 'length'], [geometry['radius'], geometry['length']]))
elif geometry['type'] == "sphere":
output.append(xmlline(5, 'sphere', ['radius'], [geometry['radius']]))
elif geometry['type'] == 'mesh':
output.append(xmlline(5, 'mesh', ['filename', 'scale'],
[os.path.join(ioUtils.getRelativeMeshpath(),
geometry['filename'] + '.' + ioUtils.getOutputMeshtype()),
l2str(geometry['scale'])]))
elif geometry['type'] == 'capsule':
output.append(xmlline(5, 'cylinder', ['radius', 'length'], [geometry['radius'], geometry['length']])) # FIXME: real capsules here!
else:
raise TypeError("Unknown geometry type")
output.append(indent * 4 + '</geometry>\n')
except (KeyError, TypeError) as err:
log("Misdefined geometry in element " + element['name'] + " " + str(err), "ERROR", "writeURDFGeometry")
def writeURDFGeometry(output, element, filepath):
"""This functions writes the URDF geometry for a given element at the end of a given String.
:param output: The String to append the URDF output string on.
:type output: str
:param element: A certain element to parse into URDF.
:type element: dict
:return: str -- The extended String
"""
geometry = element['geometry']
try:
output.append(indent * 4 + '<geometry>\n')
if geometry['type'] == 'box':
output.append(
xmlline(5, 'box', ['size'], [l2str(geometry['size'])]))
elif geometry['type'] == "cylinder":
output.append(
xmlline(5, 'cylinder', ['radius', 'length'],
[geometry['radius'], geometry['length']]))
elif geometry['type'] == "sphere":
output.append(
xmlline(5, 'sphere', ['radius'], [geometry['radius']]))
elif geometry['type'] == 'mesh':
# FIXME: the following will crash if unstructured export is used
log(
"writeURDFGeometry: " + filepath + ' ' +
ioUtils.getOutputMeshpath(path.dirname(filepath)), "DEBUG")
meshpath = ioUtils.getOutputMeshpath(path.dirname(filepath))
output.append(
xmlline(5, 'mesh', ['filename', 'scale'], [
path.join(
path.relpath(meshpath,
filepath), geometry['filename'] + '.' +
ioUtils.getOutputMeshtype()),
l2str(geometry['scale'])
]))
elif geometry['type'] == 'capsule':
# FIXME: real capsules here!
output.append(
xmlline(5, 'cylinder', ['radius', 'length'],
[geometry['radius'], geometry['length']]))
else:
raise TypeError("Unknown geometry type")
output.append(indent * 4 + '</geometry>\n')
except (KeyError, TypeError) as err:
log(
"Misdefined geometry in element " + element['name'] + " " +
str(err), "ERROR")
def writeURDFGeometry(output, element, filepath):
"""This functions writes the URDF geometry for a given element at the end of a given String.
Args:
output(str): The String to append the URDF output string on.
element(dict): A certain element to parse into URDF.
filepath:
Returns:
: str -- The extended String
"""
geometry = element['geometry']
try:
output.append(indent * 4 + '<geometry>\n')
if geometry['type'] == 'box':
output.append(
xmlline(5, 'box', ['size'], [l2str(geometry['size'])]))
elif geometry['type'] == "cylinder":
output.append(
xmlline(5, 'cylinder', ['radius', 'length'],
[geometry['radius'], geometry['length']]))
elif geometry['type'] == "sphere":
output.append(
xmlline(5, 'sphere', ['radius'], [geometry['radius']]))
elif geometry['type'] == 'mesh':
meshpath = ioUtils.getOutputMeshpath(
path.dirname(filepath), None, None
) + geometry['filename'] + '.' + ioUtils.getOutputMeshtype()
if not "package://" in meshpath:
meshpath = path.relpath(meshpath, filepath)
output.append(
xmlline(
5,
'mesh',
['filename', 'scale'],
[
meshpath,
l2str(geometry['scale']),
],
))
else:
raise TypeError("Unknown geometry type")
output.append(indent * 4 + '</geometry>\n')
except (KeyError, TypeError) as err:
log(
"Misdefined geometry in element " + element['name'] + " " +
str(err), "ERROR")
def writeURDFGeometry(output, element, filepath):
"""This functions writes the URDF geometry for a given element at the end of a given String.
Args:
output(str): The String to append the URDF output string on.
element(dict): A certain element to parse into URDF.
filepath:
Returns:
: str -- The extended String
"""
geometry = element['geometry']
try:
output.append(indent * 4 + '<geometry>\n')
if geometry['type'] == 'box':
output.append(xmlline(5, 'box', ['size'], [l2str(geometry['size'])]))
elif geometry['type'] == "cylinder":
output.append(
xmlline(
5, 'cylinder', ['radius', 'length'], [geometry['radius'], geometry['length']]
)
)
elif geometry['type'] == "sphere":
output.append(xmlline(5, 'sphere', ['radius'], [geometry['radius']]))
elif geometry['type'] == 'mesh':
meshpath = ioUtils.getOutputMeshpath(path.dirname(filepath))
output.append(
xmlline(
5,
'mesh',
['filename', 'scale'],
[
path.join(
path.relpath(meshpath, filepath),
geometry['filename'] + '.' + ioUtils.getOutputMeshtype(),
),
l2str(geometry['scale']),
],
)
)
else:
raise TypeError("Unknown geometry type")
output.append(indent * 4 + '</geometry>\n')
except (KeyError, TypeError) as err:
log("Misdefined geometry in element " + element['name'] + " " + str(err), "ERROR")
def exportSRDF(model, path, mesh_format=''):
"""This function exports the SRDF-relevant data from the dictionary to a specified path.
Further detail on different elements of SRDF:
<group>
Groups in SRDF can contain *links*, *joints*, *chains* and other *groups* (the latter two of which have to be specified
upstream. As nested groups is just a shortcut for adding links and joints to a group, it is not supported and the
user will have to add all links and joints explicitly to each group.
Originally both links and their associated parent joints were added. SRDF however implicitly assumes this, so the
current implementation only adds the links.
<chain>
Chains are elements to simplify defining groups and are supported. The dictionary also contains a list of all
elements belonging to that chain, which is discarded and not written to SRDF, however. It might be written to SMURF
in the future.
<link_sphere_approximatio>
SRDF defines the convention that if no sphere is defined, one large sphere is
assumed for that link. If one wants to have no sphere at all, it is necessary to define a sphere of radius 0.
As one large sphere can be explicitly added by the user and should be if that is what he intends (WYSIWYG),
we add a sphere of radius 0 by default if no sphere is specified.
<passive_joint>
Marks a joint as passive.
<disable_collisions>
Disables collisions between pairs of links to simplify collision checking and avoid collisions
of parents and children at their joints.
Currently not supported:
- <group_state>
- <virtual_joint>
:param model: a robot model dictionary.
:type model: dict
:param path: the outpath for the file.
:type path: str
"""
output = [xmlHeader, indent + '<robot name="' + model['name'] + '">\n\n']
sorted_group_keys = sorted(model['groups'])
for groupname in sorted_group_keys:
output.append(indent * 2 + '<group name="' + groupname + '">\n')
# TODO: once groups are implemented, this should be sorted aswell:
for member in model['groups'][groupname]:
output.append(indent * 3 + '<' + member['type'] + ' name="' +
member['name'] + '" />\n')
output.append(indent * 2 + '</group>\n\n')
sorted_chain_keys = sorted(model['chains'].keys())
for chainname in sorted_chain_keys:
output.append(indent * 2 + '<group name="' + chainname + '">\n')
chain = model['chains'][chainname]
output.append(indent * 3 + '<chain base_link="' + chain['start'] +
'" tip_link="' + chain['end'] + '" />\n')
output.append(indent * 2 + '</group>\n\n')
#for joint in model['state']['joints']:
# pass
# passive joints
sorted_joint_keys = sorted(model['joints'].keys())
for joint in sorted_joint_keys:
try:
if model['joints'][joint]['passive']:
output.append(indent * 2 + '<passive_joint name="' +
model['links'][joint]['name'] + '"/>\n\n')
except KeyError:
pass
sorted_link_keys = sorted(model['links'].keys())
for link in sorted_link_keys:
if len(model['links'][link]['approxcollision']) > 0:
output.append(indent * 2 + '<link_sphere_approximation link="' +
model['links'][link]['name'] + '">\n')
# TODO: there does not seem to be a way to sort the spheres if there are multiple
for sphere in model['links'][link]['approxcollision']:
output.append(
xmlline(3, 'sphere', ('center', 'radius'),
(l2str(sphere['center']), sphere['radius'])))
output.append(indent * 2 + '</link_sphere_approximation>\n\n')
else:
output.append(indent * 2 + '<link_sphere_approximation link="' +
model['links'][link]['name'] + '">\n')
output.append(
xmlline(3, 'sphere', ('center', 'radius'),
('0.0 0.0 0.0', '0')))
output.append(indent * 2 + '</link_sphere_approximation>\n\n')
# calculate collision-exclusive links
collisionExclusives = []
for combination in itertools.combinations(model['links'], 2):
link1 = model['links'][combination[0]]
link2 = model['links'][combination[1]]
# TODO: we might want to automatically add parent/child link combinations
try:
if link1['collision_bitmask'] & link2['collision_bitmask'] == 0:
#output.append(xmlline(2, 'disable_collisions', ('link1', 'link2'), (link1['name'], link2['name'])))
collisionExclusives.append((link1['name'], link2['name']))
except KeyError:
pass
with open(os.path.join(path, model['name'] + '.srdf'), 'w') as outputfile:
outputfile.write(''.join(output))
def exportUrdf(model, outpath):
"""This functions writes the URDF of a given model into a file at the given filepath.
An existing file with this path will be overwritten.
Args:
model(dict): Dictionary of the model to be exported as URDF.
outpath(str): The path of the exported file.
Returns:
"""
log("Export URDF to " + outpath, "INFO")
filename = path.join(outpath, model['name'] + '.urdf')
stored_element_order = None
# CHECK test Windows path consistency
order_file_name = model['name'] + '_urdf_order'
if order_file_name in bpy.data.texts:
stored_element_order = yaml.load(bpy.data.texts[order_file_name].as_string())
output = [xmlHeader, indent + '<robot name="' + model['name'] + '">\n\n']
# export link information
if stored_element_order is None:
sorted_link_keys = sorted(model['links'])
else:
sorted_link_keys = stored_element_order['links']
new_keys = []
for link_key in model['links']:
if link_key not in sorted_link_keys:
new_keys.append(link_key)
sorted_link_keys += sort_urdf_elements(new_keys)
for l in sorted_link_keys:
if l in model['links']:
link = model['links'][l]
output.append(indent * 2 + '<link name="' + link['name'] + '">\n')
if 'mass' in link['inertial'] and 'inertia' in link['inertial']:
output.append(indent * 3 + '<inertial>\n')
if 'pose' in link['inertial']:
output.append(
xmlline(
4,
'origin',
['xyz', 'rpy'],
[
l2str(link['inertial']['pose']['translation']),
l2str(link['inertial']['pose']['rotation_euler']),
],
)
)
output.append(xmlline(4, 'mass', ['value'], [str(link['inertial']['mass'])]))
output.append(
xmlline(
4,
'inertia',
['ixx', 'ixy', 'ixz', 'iyy', 'iyz', 'izz'],
[str(i) for i in link['inertial']['inertia']],
)
)
output.append(indent * 3 + '</inertial>\n')
# visual object
if link['visual']:
if stored_element_order is None:
sorted_visual_keys = sorted(link['visual'])
else:
sorted_visual_keys = stored_element_order['viscol'][link['name']]['visual']
new_keys = []
for vis_key in link['visual']:
if vis_key not in sorted_visual_keys:
new_keys.append(vis_key)
sorted_visual_keys += sort_urdf_elements(new_keys)
for v in sorted_visual_keys:
if v in link['visual']:
vis = link['visual'][v]
output.append(indent * 3 + '<visual name="' + vis['name'] + '">\n')
output.append(
xmlline(
4,
'origin',
['xyz', 'rpy'],
[
l2str(vis['pose']['translation']),
l2str(vis['pose']['rotation_euler']),
],
)
)
writeURDFGeometry(output, vis, outpath)
if 'material' in vis:
# FIXME: change back to 1 when implemented in urdfloader
if model['materials'][vis['material']]['users'] == 0:
mat = model['materials'][vis['material']]
output.append(
indent * 4 + '<material name="' + mat['name'] + '">\n'
)
color = mat['diffuseColor']
output.append(
indent * 5
+ '<color rgba="'
+ l2str([color[num] for num in ['r', 'g', 'b']])
+ ' '
+ str(mat["transparency"])
+ '"/>\n'
)
if 'diffuseTexture' in mat:
output.append(
indent * 5
+ '<texture filename="'
+ mat['diffuseTexture']
+ '"/>\n'
)
output.append(indent * 4 + '</material>\n')
else:
output.append(
indent * 4 + '<material name="' + vis["material"] + '"/>\n'
)
output.append(indent * 3 + '</visual>\n')
# collision object
if link['collision']:
if stored_element_order is None:
sorted_collision_keys = sorted(link['collision'])
else:
sorted_collision_keys = stored_element_order['viscol'][link['name']][
'collision'
]
new_keys = []
for col_key in link['collision']:
if col_key not in sorted_collision_keys:
new_keys.append(col_key)
sorted_collision_keys += sort_urdf_elements(new_keys)
for c in sorted_collision_keys:
if c in link['collision']:
col = link['collision'][c]
output.append(indent * 3 + '<collision name="' + col['name'] + '">\n')
output.append(
xmlline(
4,
'origin',
['xyz', 'rpy'],
[
l2str(col['pose']['translation']),
l2str(col['pose']['rotation_euler']),
],
)
)
writeURDFGeometry(output, col, outpath)
output.append(indent * 3 + '</collision>\n')
output.append(indent * 2 + '</link>\n\n')
# export joint information
missing_values = False
if stored_element_order is None:
sorted_joint_keys = sorted(model['joints'])
else:
sorted_joint_keys = stored_element_order['joints']
new_keys = []
for joint_key in model['joints']:
if joint_key not in sorted_joint_keys:
new_keys.append(joint_key)
sorted_joint_keys += sort_urdf_elements(new_keys)
for j in sorted_joint_keys:
if j in model['joints']:
joint = model['joints'][j]
output.append(
indent * 2 + '<joint name="' + joint['name'] + '" type="' + joint["type"] + '">\n'
)
child = model['links'][joint["child"]]
output.append(
xmlline(
3,
'origin',
['xyz', 'rpy'],
[l2str(child['pose']['translation']), l2str(child['pose']['rotation_euler'])],
)
)
output.append(indent * 3 + '<parent link="' + joint["parent"] + '"/>\n')
output.append(indent * 3 + '<child link="' + joint["child"] + '"/>\n')
if 'axis' in joint:
output.append(indent * 3 + '<axis xyz="' + l2str(joint['axis']) + '"/>\n')
if 'limits' in joint:
for limit_value in ['effort', 'velocity']:
if limit_value not in joint['limits']:
log(
"joint '"
+ joint['name']
+ "' does not specify a maximum "
+ limit_value
+ "!",
"WARNING",
)
missing_values = True
used_limits = []
for limit in ['lower', 'upper', 'effort', 'velocity']:
if limit in joint['limits']:
used_limits.append(limit)
output.append(
xmlline(3, 'limit', used_limits, [joint['limits'][p] for p in used_limits])
)
elif joint['type'] in ['revolute', 'prismatic']:
log(
"joint '"
+ joint['name']
+ "' does not specify limits, though its type is "
+ joint['type']
+ "!",
"WARNING",
)
missing_values = True
output.append(indent * 2 + '</joint>\n\n')
# export material information
if missing_values:
log("Created URDF is invalid due to missing values!", "WARNING")
if stored_element_order is None:
sorted_material_keys = sorted(model['materials'])
else:
sorted_material_keys = stored_element_order['materials']
new_keys = []
for material_key in model['materials']:
if material_key not in sorted_material_keys:
new_keys.append(material_key)
sorted_material_keys += sort_urdf_elements(new_keys)
for m in sorted_material_keys:
if m in model['materials']:
# FIXME: change back to 1 when implemented in urdfloader
if model['materials'][m]['users'] > 0:
output.append(indent * 2 + '<material name="' + m + '">\n')
color = model['materials'][m]['diffuseColor']
transparency = (
model['materials'][m]['transparency']
if 'transparency' in model['materials'][m]
else 0.0
)
output.append(
indent * 3
+ '<color rgba="'
+ l2str([color[num] for num in ['r', 'g', 'b']])
+ ' '
+ str(1.0 - transparency)
+ '"/>\n'
)
if 'diffuseTexture' in model['materials'][m]:
output.append(
indent * 3
+ '<texture filename="'
+ model['materials'][m]['diffuseTexture']
+ '"/>\n'
)
output.append(indent * 2 + '</material>\n\n')
# finish the export
output.append(indent + '</robot>\n')
with open(filename, 'w') as outputfile:
outputfile.write(''.join(output))
# FIXME: different joint transformations needed for fixed joints
log("Writing model data to " + filename, "INFO")
def exportUrdf(model, outpath):
"""This functions writes the URDF of a given model into a file at the given filepath.
An existing file with this path will be overwritten.
Args:
model(dict): Dictionary of the model to be exported as URDF.
outpath(str): The path of the exported file.
Returns:
"""
log("Export URDF to " + outpath, "INFO")
filename = path.join(outpath, model['name'] + '.urdf')
stored_element_order = None
# CHECK test Windows path consistency
order_file_name = model['name'] + '_urdf_order'
if order_file_name in bpy.data.texts:
stored_element_order = yaml.load(bpy.data.texts[order_file_name].as_string())
output = [xmlHeader, indent + '<robot name="' + model['name'] + '">\n\n']
# export link information
if stored_element_order is None:
sorted_link_keys = sorted(model['links'])
else:
sorted_link_keys = stored_element_order['links']
new_keys = []
for link_key in model['links']:
if link_key not in sorted_link_keys:
new_keys.append(link_key)
sorted_link_keys += sort_urdf_elements(new_keys)
for l in sorted_link_keys:
if l in model['links']:
link = model['links'][l]
output.append(indent * 2 + '<link name="' + link['name'] + '">\n')
if 'mass' in link['inertial'] and 'inertia' in link['inertial']:
output.append(indent * 3 + '<inertial>\n')
if 'pose' in link['inertial']:
output.append(
xmlline(
4,
'origin',
['xyz', 'rpy'],
[
l2str(link['inertial']['pose']['translation']),
l2str(link['inertial']['pose']['rotation_euler']),
],
)
)
output.append(xmlline(4, 'mass', ['value'], [str(link['inertial']['mass'])]))
output.append(
xmlline(
4,
'inertia',
['ixx', 'ixy', 'ixz', 'iyy', 'iyz', 'izz'],
[str(i) for i in link['inertial']['inertia']],
)
)
output.append(indent * 3 + '</inertial>\n')
# visual object
if link['visual']:
if stored_element_order is None:
sorted_visual_keys = sorted(link['visual'])
else:
sorted_visual_keys = stored_element_order['viscol'][link['name']]['visual']
new_keys = []
for vis_key in link['visual']:
if vis_key not in sorted_visual_keys:
new_keys.append(vis_key)
sorted_visual_keys += sort_urdf_elements(new_keys)
for v in sorted_visual_keys:
if v in link['visual']:
vis = link['visual'][v]
output.append(indent * 3 + '<visual name="' + vis['name'] + '">\n')
output.append(
xmlline(
4,
'origin',
['xyz', 'rpy'],
[
l2str(vis['pose']['translation']),
l2str(vis['pose']['rotation_euler']),
],
)
)
writeURDFGeometry(output, vis, outpath)
if 'material' in vis:
# FIXME: change back to 1 when implemented in urdfloader
if model['materials'][vis['material']]['users'] == 0:
mat = model['materials'][vis['material']]
output.append(
indent * 4 + '<material name="' + mat['name'] + '">\n'
)
color = mat['diffuseColor']
output.append(
indent * 5
+ '<color rgba="'
+ l2str([color[num] for num in ['r', 'g', 'b']])
+ ' '
+ str(mat["transparency"])
+ '"/>\n'
)
if 'diffuseTexture' in mat:
output.append(
indent * 5
+ '<texture filename="'
+ mat['diffuseTexture']
+ '"/>\n'
)
output.append(indent * 4 + '</material>\n')
else:
output.append(
indent * 4 + '<material name="' + vis["material"] + '"/>\n'
)
output.append(indent * 3 + '</visual>\n')
# collision object
if link['collision']:
if stored_element_order is None:
sorted_collision_keys = sorted(link['collision'])
else:
sorted_collision_keys = stored_element_order['viscol'][link['name']][
'collision'
]
new_keys = []
for col_key in link['collision']:
if col_key not in sorted_collision_keys:
new_keys.append(col_key)
sorted_collision_keys += sort_urdf_elements(new_keys)
for c in sorted_collision_keys:
if c in link['collision']:
col = link['collision'][c]
output.append(indent * 3 + '<collision name="' + col['name'] + '">\n')
output.append(
xmlline(
4,
'origin',
['xyz', 'rpy'],
[
l2str(col['pose']['translation']),
l2str(col['pose']['rotation_euler']),
],
)
)
writeURDFGeometry(output, col, outpath)
output.append(indent * 3 + '</collision>\n')
output.append(indent * 2 + '</link>\n\n')
# export joint information
missing_values = False
if stored_element_order is None:
sorted_joint_keys = sorted(model['joints'])
else:
sorted_joint_keys = stored_element_order['joints']
new_keys = []
for joint_key in model['joints']:
if joint_key not in sorted_joint_keys:
new_keys.append(joint_key)
sorted_joint_keys += sort_urdf_elements(new_keys)
for j in sorted_joint_keys:
if j in model['joints']:
joint = model['joints'][j]
output.append(
indent * 2 + '<joint name="' + joint['name'] + '" type="' + joint["type"] + '">\n'
)
child = model['links'][joint["child"]]
output.append(
xmlline(
3,
'origin',
['xyz', 'rpy'],
[l2str(child['pose']['translation']), l2str(child['pose']['rotation_euler'])],
)
)
output.append(indent * 3 + '<parent link="' + joint["parent"] + '"/>\n')
output.append(indent * 3 + '<child link="' + joint["child"] + '"/>\n')
if 'axis' in joint:
output.append(indent * 3 + '<axis xyz="' + l2str(joint['axis']) + '"/>\n')
if 'limits' in joint:
for limit_value in ['effort', 'velocity']:
if limit_value not in joint['limits']:
log(
"joint '"
+ joint['name']
+ "' does not specify a maximum "
+ limit_value
+ "!",
"WARNING",
)
missing_values = True
used_limits = []
for limit in ['lower', 'upper', 'effort', 'velocity']:
if limit in joint['limits']:
used_limits.append(limit)
output.append(
xmlline(3, 'limit', used_limits, [joint['limits'][p] for p in used_limits])
)
elif joint['type'] in ['revolute', 'prismatic']:
log(
"joint '"
+ joint['name']
+ "' does not specify limits, though its type is "
+ joint['type']
+ "!",
"WARNING",
)
missing_values = True
output.append(indent * 2 + '</joint>\n\n')
# export material information
if missing_values:
log("Created URDF is invalid due to missing values!", "WARNING")
if stored_element_order is None:
sorted_material_keys = sorted(model['materials'])
else:
sorted_material_keys = stored_element_order['materials']
new_keys = []
for material_key in model['materials']:
if material_key not in sorted_material_keys:
new_keys.append(material_key)
sorted_material_keys += sort_urdf_elements(new_keys)
for m in sorted_material_keys:
if m in model['materials']:
# FIXME: change back to 1 when implemented in urdfloader
if model['materials'][m]['users'] > 0:
output.append(indent * 2 + '<material name="' + m + '">\n')
color = model['materials'][m]['diffuseColor']
transparency = (
model['materials'][m]['transparency']
if 'transparency' in model['materials'][m]
else 0.0
)
output.append(
indent * 3
+ '<color rgba="'
+ l2str([color[num] for num in ['r', 'g', 'b']])
+ ' '
+ str(1.0 - transparency)
+ '"/>\n'
)
if 'diffuseTexture' in model['materials'][m]:
output.append(
indent * 3
+ '<texture filename="'
+ model['materials'][m]['diffuseTexture']
+ '"/>\n'
)
output.append(indent * 2 + '</material>\n\n')
# finish the export
output.append(indent + '</robot>\n')
with open(filename, 'w') as outputfile:
outputfile.write(''.join(output))
# FIXME: different joint transformations needed for fixed joints
log("Writing model data to " + filename, "INFO")
def exportSRDF(model, path, mesh_format=''):
"""This function exports the SRDF-relevant data from the dictionary to a specified path.
Further detail on different elements of SRDF:
<group>
Groups in SRDF can contain *links*, *joints*, *chains* and other *groups* (the latter two of which have to be specified
upstream. As nested groups is just a shortcut for adding links and joints to a group, it is not supported and the
user will have to add all links and joints explicitly to each group.
Originally both links and their associated parent joints were added. SRDF however implicitly assumes this, so the
current implementation only adds the links.
<chain>
Chains are elements to simplify defining groups and are supported. The dictionary also contains a list of all
elements belonging to that chain, which is discarded and not written to SRDF, however. It might be written to SMURF
in the future.
<link_sphere_approximatio>
SRDF defines the convention that if no sphere is defined, one large sphere is
assumed for that link. If one wants to have no sphere at all, it is necessary to define a sphere of radius 0.
As one large sphere can be explicitly added by the user and should be if that is what he intends (WYSIWYG),
we add a sphere of radius 0 by default if no sphere is specified.
<passive_joint>
Marks a joint as passive.
<disable_collisions>
Disables collisions between pairs of links to simplify collision checking and avoid collisions
of parents and children at their joints.
Currently not supported:
- <group_state>
- <virtual_joint>
Args:
model(dict): a robot model dictionary.
path(str): the outpath for the file.
mesh_format: (Default value = '')
Returns:
"""
output = [xmlHeader, indent + '<robot name="' + model['name'] + '">\n\n']
sorted_group_keys = sorted(model['groups'])
for groupname in sorted_group_keys:
output.append(indent * 2 + '<group name="' + groupname + '">\n')
# TODO: once groups are implemented, this should be sorted aswell:
for member in model['groups'][groupname]:
output.append(indent * 3 + '<' + member['type'] + ' name="' + member['name'] + '" />\n')
output.append(indent * 2 + '</group>\n\n')
sorted_chain_keys = sorted(model['chains'].keys())
for chainname in sorted_chain_keys:
output.append(indent * 2 + '<group name="' + chainname + '">\n')
chain = model['chains'][chainname]
output.append(
indent * 3
+ '<chain base_link="'
+ chain['start']
+ '" tip_link="'
+ chain['end']
+ '" />\n'
)
output.append(indent * 2 + '</group>\n\n')
# TODO delete me?
# for joint in model['state']['joints']:
# pass
# passive joints
sorted_joint_keys = sorted(model['joints'].keys())
for joint in sorted_joint_keys:
try:
if model['joints'][joint]['passive']:
output.append(
indent * 2 + '<passive_joint name="' + model['links'][joint]['name'] + '"/>\n\n'
)
except KeyError:
pass
sorted_link_keys = sorted(model['links'].keys())
for link in sorted_link_keys:
if len(model['links'][link]['approxcollision']) > 0:
output.append(
indent * 2
+ '<link_sphere_approximation link="'
+ model['links'][link]['name']
+ '">\n'
)
# TODO: there does not seem to be a way to sort the spheres if there are multiple
for sphere in model['links'][link]['approxcollision']:
output.append(
xmlline(
3,
'sphere',
('center', 'radius'),
(l2str(sphere['center']), sphere['radius']),
)
)
output.append(indent * 2 + '</link_sphere_approximation>\n\n')
else:
output.append(
indent * 2
+ '<link_sphere_approximation link="'
+ model['links'][link]['name']
+ '">\n'
)
output.append(xmlline(3, 'sphere', ('center', 'radius'), ('0.0 0.0 0.0', '0')))
output.append(indent * 2 + '</link_sphere_approximation>\n\n')
# calculate collision-exclusive links
collisionExclusives = []
for combination in itertools.combinations(model['links'], 2):
link1 = model['links'][combination[0]]
link2 = model['links'][combination[1]]
# TODO: we might want to automatically add parent/child link combinations
try:
if link1['collision_bitmask'] & link2['collision_bitmask'] == 0:
# TODO delete me?
# output.append(xmlline(2, 'disable_collisions', ('link1', 'link2'), (link1['name'], link2['name'])))
collisionExclusives.append((link1['name'], link2['name']))
except KeyError:
pass
with open(os.path.join(path, model['name'] + '.srdf'), 'w') as outputfile:
outputfile.write(''.join(output))
