Skip to content

Commit

Permalink
Merge pull request #7 from syuntoku14/add_joint_type
Browse files Browse the repository at this point in the history 
Added joint type "Rigid", "Slider" and added joint's limit
  • Loading branch information
@syuntoku14
syuntoku14 authored Sep 25, 2018
2 parents 1910514 + f6bb58d commit 418d76a
Showing 1 changed file with 74 additions and 15 deletions.
89 changes: 74 additions & 15 deletions URDF_Exporter/URDF_Exporter.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,13 @@
# If there is no 'body' in the root component, maybe the corrdinates are wrong.
"""

# joint effort: 100
# joint velocity: 100

# supports "Revolute", "Rigid" and "Slider" joint types

# I'm not sure how prismatic joint acts if there is no limit in fusion model

# --------------------
# utilities

Expand Down Expand Up @@ -318,16 +325,18 @@ class Joint:
tran_xml: str
generated xml describing about the transmission
"""
def __init__(self, name, xyz, axis, parent, child, type_='continuous'):
def __init__(self, name, xyz, axis, parent, child, joint_type, upper_limit, lower_limit):
self.name = name
self.type = type_
self.type = joint_type
self.xyz = xyz
self.axis = axis
self.parent = parent
self.child = child
self.joint_xml = None
self.tran_xml = None

self.axis = axis # for 'revolute' and 'continuous'
self.upper_limit = upper_limit # for 'revolute' and 'prismatic'
self.lower_limit = lower_limit # for 'revolute' and 'prismatic'

def gen_joint_xml(self):
"""
Generate the joint_xml and hold it by self.joint_xml
Expand All @@ -341,9 +350,14 @@ def gen_joint_xml(self):
parent.attrib = {'link':self.parent}
child = SubElement(joint, 'child')
child.attrib = {'link':self.child}
axis = SubElement(joint, 'axis')
axis.attrib = {'xyz':' '.join([str(_) for _ in self.axis])}

if self.type == 'revolute' or self.type == 'continuous' or self.type == 'prismatic':
axis = SubElement(joint, 'axis')
axis.attrib = {'xyz':' '.join([str(_) for _ in self.axis])}
if self.type == 'revolute' or self.type == 'prismatic':
limit = SubElement(joint, 'limit')
limit.attrib = {'upper': str(self.upper_limit), 'lower': str(self.lower_limit),
'effort': '100', 'velocity': '100'}

# print("\n".join(prettify(joint).split("\n")[1:]))
self.joint_xml = "\n".join(prettify(joint).split("\n")[1:])

Expand All @@ -362,8 +376,8 @@ def gen_transmission_xml(self):
tran = Element('transmission')
tran.attrib = {'name':self.name + '_tran'}

type_ = SubElement(tran, 'type')
type_.text = 'transmission_interface/SimpleTransmission'
joint_type = SubElement(tran, 'type')
joint_type.text = 'transmission_interface/SimpleTransmission'

joint = SubElement(tran, 'joint')
joint.attrib = {'name':self.name}
Expand Down Expand Up @@ -400,11 +414,52 @@ def set_joints_dict(root, joints_dict, msg):
msg: str
Tell the status
"""
joint_type_list = [
'fixed', 'revolute', 'prismatic', 'Cylinderical',
'PinSlot', 'Planner', 'Ball'] # these are the names in urdf

for joint in root.joints:
joint_dict = {}

joint_dict['axis'] = [round(i / 100.0, 6) for i in \
joint.jointMotion.rotationAxisVector.asArray()] # converted to meter
joint_type = joint_type_list[joint.jointMotion.jointType]
joint_dict['type'] = joint_type

# swhich by the type of the joint
joint_dict['axis'] = [0, 0, 0]
joint_dict['upper_limit'] = 0.0
joint_dict['lower_limit'] = 0.0
if joint_type == 'revolute':
joint_dict['axis'] = [round(i / 100.0, 6) for i in \
joint.jointMotion.rotationAxisVector.asArray()] # converted to meter
max_enabled = joint.jointMotion.rotationLimits.isMaximumValueEnabled
min_enabled = joint.jointMotion.rotationLimits.isMinimumValueEnabled
if max_enabled and min_enabled:
joint_dict['upper_limit'] = round(joint.jointMotion.rotationLimits.maximumValue, 6)
joint_dict['lower_limit'] = round(joint.jointMotion.rotationLimits.minimumValue, 6)
elif max_enabled and not min_enabled:
msg = joint.name + 'is not set its lower limit. Please set it and try again.'
break
elif not max_enabled and min_enabled:
msg = joint.name + 'is not set its upper limit. Please set it and try again.'
break
else: # if there is no angle limit
joint_dict['type'] = 'continuous'
elif joint_type == 'prismatic':
joint_dict['axis'] = [round(i / 100.0, 6) for i in \
joint.jointMotion.slideDirectionVector.asArray()] # converted to meter
max_enabled = joint.jointMotion.slideLimits.isMaximumValueEnabled
min_enabled = joint.jointMotion.slideLimits.isMinimumValueEnabled
if max_enabled and min_enabled:
joint_dict['upper_limit'] = round(joint.jointMotion.slideLimits.maximumValue/100, 6)
joint_dict['lower_limit'] = round(joint.jointMotion.slideLimits.minimumValue/100, 6)
elif max_enabled and not min_enabled:
msg = joint.name + 'is not set its lower limit. Please set it and try again.'
break
elif not max_enabled and min_enabled:
msg = joint.name + 'is not set its upper limit. Please set it and try again.'
break
elif joint_type == 'fixed':
pass

if joint.occurrenceTwo.component.name == 'base_link':
joint_dict['parent'] = 'base_link'
else:
Expand Down Expand Up @@ -500,10 +555,14 @@ def write_joint_tran_urdf(joints_dict, repo, links_xyz_dict, file_name):
for j in joints_dict:
parent = joints_dict[j]['parent']
child = joints_dict[j]['child']
joint_type = joints_dict[j]['type']
upper_limit = joints_dict[j]['upper_limit']
lower_limit = joints_dict[j]['lower_limit']
xyz = [round(p-c, 6) for p, c in \
zip(links_xyz_dict[parent], links_xyz_dict[child])] # xyz = parent - child
joint = Joint(name=j, xyz=xyz, axis=joints_dict[j]['axis'],\
parent=parent, child=child)
joint = Joint(name=j, joint_type = joint_type, xyz=xyz, \
axis=joints_dict[j]['axis'], parent=parent, child=child, \
upper_limit=upper_limit, lower_limit=lower_limit)
joint.gen_joint_xml()
joint.gen_transmission_xml()
f.write(joint.joint_xml)
Expand Down Expand Up @@ -674,4 +733,4 @@ def run(context):

except:
if ui:
ui.messageBox('Failed:\n{}'.format(traceback.format_exc()))
ui.messageBox('Failed:\n{}'.format(traceback.format_exc()))

0 comments on commit 418d76a

Please sign in to comment.