material.parseFromMaterialNode(child)
linkList = []
jointList = []
parentList = []
childList = []
rootLink = parserURDF.Link()
for joint in jointElementList:
jointList.append(parserURDF.getJoint(joint))
parentList.append(jointList[-1].parent.encode('ascii'))
childList.append(jointList[-1].child.encode('ascii'))
parentList.sort()
childList.sort()
for link in linkElementList:
linkList.append(parserURDF.getLink(link))
for link in linkList:
if parserURDF.isRootLink(link.name, childList):
rootLink = link
# if root link has only one joint which type is fixed,
# it should not be part of the model (link between robot and static environment)
while True:
directJoint = []
found = False # To avoid endless loop
for joint in jointList:
if joint.parent == rootLink.name:
directJoint.append(joint)
if len(directJoint
) == 1 and directJoint[0].type == 'fixed':
for childLink in linkList:
if childLink.name == directJoint[0].child:
if child.localName == 'link':
links.append(child)
elif child.localName == 'joint':
joints.append(child)
elif child.localName == 'material':
material = parserURDF.Material()
material.parseFromMaterialNode(child)
# create Joint and Link instances
rootLink = parserURDF.Link()
joints = [parserURDF.getJoint(joint) for joint in joints]
parents = [joint.parent.encode('ascii') for joint in joints]
children = [joint.child.encode('ascii') for joint in joints]
parents.sort()
children.sort()
links = [parserURDF.getLink(link) for link in links]
# check if root link has only one joint which type is fixed. If so, it should not be part of the model
# (link between robot and static environment)
# so if you have: world --fixed joint--> frame1 --fixed joint--> base
# the root link will be set to be the base
for link in links:
if parserURDF.isRootLink(link.name, children):
rootLink = link
while True:
found = False # To avoid endless loop
# get joints that are the child of the root link
directJoint = [
joint for joint in joints
if joint.parent == rootLink.name