From d4ce47af2b75ee3703f3501e3c7ad7f50ff7621c Mon Sep 17 00:00:00 2001 From: syuntoku14 Date: Tue, 25 Sep 2018 01:26:57 -0700 Subject: [PATCH 1/4] Added revolute type --- URDF_Exporter/URDF_Exporter.py | 70 +++++++++++++++++++++++++++------- 1 file changed, 56 insertions(+), 14 deletions(-) diff --git a/URDF_Exporter/URDF_Exporter.py b/URDF_Exporter/URDF_Exporter.py index 9982ecc..a73ea1e 100644 --- a/URDF_Exporter/URDF_Exporter.py +++ b/URDF_Exporter/URDF_Exporter.py @@ -12,6 +12,10 @@ # If there is no 'body' in the root component, maybe the corrdinates are wrong. """ +# should be added limits +# joint effort: 100 +# joint velocity: 100 + # -------------------- # utilities @@ -318,16 +322,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 @@ -341,9 +347,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': + 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:]) @@ -362,8 +373,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} @@ -400,11 +411,38 @@ def set_joints_dict(root, joints_dict, msg): msg: str Tell the status """ + joint_type_list = [ + 'fixed', 'revolute', 'Slider', '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['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) + print(joint_dict['upper_limit'], joint_dict['lower_limit']) + 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 == 'fixed': + pass + if joint.occurrenceTwo.component.name == 'base_link': joint_dict['parent'] = 'base_link' else: @@ -500,10 +538,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) From 2b83982e3d9ccfb0670aed43bf7c89cd1c1c6670 Mon Sep 17 00:00:00 2001 From: syuntoku14 Date: Tue, 25 Sep 2018 01:54:47 -0700 Subject: [PATCH 2/4] Added prismatic joint --- URDF_Exporter/URDF_Exporter.py | 21 +++++++++++++++++++-- 1 file changed, 19 insertions(+), 2 deletions(-) diff --git a/URDF_Exporter/URDF_Exporter.py b/URDF_Exporter/URDF_Exporter.py index a73ea1e..d622cf6 100644 --- a/URDF_Exporter/URDF_Exporter.py +++ b/URDF_Exporter/URDF_Exporter.py @@ -15,6 +15,9 @@ # should be added limits # joint effort: 100 # joint velocity: 100 +# added prismatic joint + +# I don't know how prismatic joint acts if there is no limit # -------------------- # utilities @@ -412,7 +415,7 @@ def set_joints_dict(root, joints_dict, msg): Tell the status """ joint_type_list = [ - 'fixed', 'revolute', 'Slider', 'Cylinderical', + 'fixed', 'revolute', 'prismatic', 'Cylinderical', 'PinSlot', 'Planner', 'Ball'] # these are the names in urdf for joint in root.joints: @@ -421,6 +424,7 @@ def set_joints_dict(root, joints_dict, msg): 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': @@ -431,7 +435,6 @@ def set_joints_dict(root, joints_dict, msg): 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) - print(joint_dict['upper_limit'], joint_dict['lower_limit']) elif max_enabled and not min_enabled: msg = joint.name + 'is not set its lower limit. Please set it and try again.' break @@ -440,6 +443,20 @@ def set_joints_dict(root, joints_dict, msg): 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 From 9f25b0004b318c321da74c3d9467a8d50e8d3fa6 Mon Sep 17 00:00:00 2001 From: syuntoku14 Date: Tue, 25 Sep 2018 21:36:58 -0700 Subject: [PATCH 3/4] Fixed bugs around slider joint axis --- URDF_Exporter/URDF_Exporter.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/URDF_Exporter/URDF_Exporter.py b/URDF_Exporter/URDF_Exporter.py index d622cf6..f659210 100644 --- a/URDF_Exporter/URDF_Exporter.py +++ b/URDF_Exporter/URDF_Exporter.py @@ -350,7 +350,7 @@ def gen_joint_xml(self): parent.attrib = {'link':self.parent} child = SubElement(joint, 'child') child.attrib = {'link':self.child} - if self.type == 'revolute' or self.type == 'continuous': + 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': From f6bb58de9d6ff07eaf7da0f10483487387ee4562 Mon Sep 17 00:00:00 2001 From: Toshinori Kitamura Date: Tue, 25 Sep 2018 15:58:43 -0700 Subject: [PATCH 4/4] Fixed some comments --- URDF_Exporter/URDF_Exporter.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/URDF_Exporter/URDF_Exporter.py b/URDF_Exporter/URDF_Exporter.py index f659210..f585240 100644 --- a/URDF_Exporter/URDF_Exporter.py +++ b/URDF_Exporter/URDF_Exporter.py @@ -12,12 +12,12 @@ # If there is no 'body' in the root component, maybe the corrdinates are wrong. """ -# should be added limits # joint effort: 100 # joint velocity: 100 -# added prismatic joint -# I don't know how prismatic joint acts if there is no limit +# supports "Revolute", "Rigid" and "Slider" joint types + +# I'm not sure how prismatic joint acts if there is no limit in fusion model # -------------------- # utilities @@ -733,4 +733,4 @@ def run(context): except: if ui: - ui.messageBox('Failed:\n{}'.format(traceback.format_exc())) \ No newline at end of file + ui.messageBox('Failed:\n{}'.format(traceback.format_exc()))