mirror of
https://github.com/PotentiaRobotics/Fusion2PyBullet-Redux.git
synced 2025-04-09 23:00:18 -04:00
461 lines
20 KiB
Python
461 lines
20 KiB
Python
# -*- coding: utf-8 -*-
|
|
"""
|
|
Export joint infos to XML from Fusion 360
|
|
|
|
@syuntoku
|
|
@yanshil
|
|
"""
|
|
|
|
import adsk, re, traceback
|
|
from xml.etree.ElementTree import Element, SubElement
|
|
from ..utils import utils
|
|
|
|
class Joint:
|
|
def __init__(self, name, xyz, axis, parent, child, joint_type, upper_limit, lower_limit):
|
|
"""
|
|
Attributes
|
|
----------
|
|
name: str
|
|
name of the joint
|
|
type: str
|
|
type of the joint(ex: rev)
|
|
xyz: [x, y, z]
|
|
coordinate of the joint
|
|
axis: [x, y, z]
|
|
coordinate of axis of the joint
|
|
parent: str
|
|
parent link
|
|
child: str
|
|
child link
|
|
joint_xml: str
|
|
generated xml describing about the joint
|
|
tran_xml: str
|
|
generated xml describing about the transmission
|
|
"""
|
|
self.name = name
|
|
self.type = joint_type
|
|
self.xyz = xyz
|
|
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 make_joint_xml(self):
|
|
"""
|
|
Generate the joint_xml and hold it by self.joint_xml
|
|
"""
|
|
joint = Element('joint')
|
|
joint.attrib = {'name':self.name, 'type':self.type}
|
|
|
|
origin = SubElement(joint, 'origin')
|
|
origin.attrib = {'xyz':' '.join([str(_) for _ in self.xyz]), 'rpy':'0 0 0'}
|
|
parent = SubElement(joint, 'parent')
|
|
parent.attrib = {'link':self.parent}
|
|
child = SubElement(joint, 'child')
|
|
child.attrib = {'link':self.child}
|
|
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'}
|
|
|
|
self.joint_xml = "\n".join(utils.prettify(joint).split("\n")[1:])
|
|
|
|
def make_transmission_xml(self):
|
|
"""
|
|
Generate the tran_xml and hold it by self.tran_xml
|
|
|
|
|
|
Notes
|
|
-----------
|
|
mechanicalTransmission: 1
|
|
type: transmission interface/SimpleTransmission
|
|
hardwareInterface: PositionJointInterface
|
|
"""
|
|
|
|
tran = Element('transmission')
|
|
tran.attrib = {'name':self.name + '_tran'}
|
|
|
|
joint_type = SubElement(tran, 'type')
|
|
joint_type.text = 'transmission_interface/SimpleTransmission'
|
|
|
|
joint = SubElement(tran, 'joint')
|
|
joint.attrib = {'name':self.name}
|
|
hardwareInterface_joint = SubElement(joint, 'hardwareInterface')
|
|
hardwareInterface_joint.text = 'PositionJointInterface'
|
|
|
|
actuator = SubElement(tran, 'actuator')
|
|
actuator.attrib = {'name':self.name + '_actr'}
|
|
hardwareInterface_actr = SubElement(actuator, 'hardwareInterface')
|
|
hardwareInterface_actr.text = 'PositionJointInterface'
|
|
mechanicalReduction = SubElement(actuator, 'mechanicalReduction')
|
|
mechanicalReduction.text = '1'
|
|
|
|
self.tran_xml = "\n".join(utils.prettify(tran).split("\n")[1:])
|
|
|
|
########## Nested-component support ##########
|
|
|
|
def make_joints_dict(root, msg):
|
|
"""
|
|
joints_dict holds parent, axis and xyz informatino of the joints
|
|
|
|
|
|
Parameters
|
|
----------
|
|
root: adsk.fusion.Design.cast(product)
|
|
Root component
|
|
msg: str
|
|
Tell the status
|
|
|
|
Returns
|
|
----------
|
|
joints_dict:
|
|
{name: {type, axis, upper_limit, lower_limit, parent, child, xyz}}
|
|
msg: str
|
|
Tell the status
|
|
"""
|
|
|
|
joint_type_list = [
|
|
'fixed', 'revolute', 'prismatic', 'Cylinderical',
|
|
'PinSlot', 'Planner', 'Ball'] # these are the names in urdf
|
|
|
|
joints_dict = {}
|
|
|
|
for joint in root.joints:
|
|
if joint.isLightBulbOn :
|
|
joint_dict = {}
|
|
joint_type = joint_type_list[joint.jointMotion.jointType]
|
|
joint_dict['type'] = joint_type
|
|
|
|
# switch by the type of the joint
|
|
joint_dict['axis'] = [0, 0, 0]
|
|
joint_dict['upper_limit'] = 0.0
|
|
joint_dict['lower_limit'] = 0.0
|
|
|
|
# support "Revolute", "Rigid" and "Slider"
|
|
if joint_type == 'revolute':
|
|
joint_dict['axis'] = [round(i, 6) for i in \
|
|
joint.jointMotion.rotationAxisVector.asArray()] ## In Fusion, exported axis is normalized.
|
|
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, 6) for i in \
|
|
joint.jointMotion.slideDirectionVector.asArray()] # Also normalized
|
|
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
|
|
|
|
|
|
def get_parent(occ):
|
|
# function to find the root component of the joint. This is necessary for the correct component name in the urdf file
|
|
if occ.assemblyContext != None:
|
|
#print(occ.name)
|
|
occ = get_parent(occ.assemblyContext)
|
|
return occ
|
|
|
|
if joint.occurrenceTwo != None and joint.occurrenceOne != None and joint.occurrenceOne.isLightBulbOn:
|
|
parent_occ = get_parent(joint.occurrenceTwo)
|
|
# print("Joint 1 Parent: " +parent_occ.name)
|
|
if "base_link" in parent_occ.name:
|
|
joint_dict['parent'] = 'base_link'
|
|
base_link = parent_occ
|
|
else:
|
|
joint_dict['parent'] = re.sub('[ :()]', '_', parent_occ.name)
|
|
# print("Joint 2: " +joint.occurrenceOne.name)
|
|
parent_occ = get_parent(joint.occurrenceOne)
|
|
# print("Joint 2 Parent: " +parent_occ.name)
|
|
joint_dict['child'] = re.sub('[ :()]', '_', parent_occ.name)
|
|
else:
|
|
break
|
|
|
|
def getJointOriginWorldCoordinates(joint :adsk.fusion.Joint):
|
|
# Function to transform the joint origin coordinates which are in the component context into world coordinates
|
|
# Thanky you for the help in the fusion forum
|
|
# https://forums.autodesk.com/t5/fusion-360-api-and-scripts/how-to-get-the-joint-origin-in-world-context/m-p/10011971/highlight/false#M12401
|
|
def getMatrixFromRoot(root_occ) -> adsk.core.Matrix3D:
|
|
mat = adsk.core.Matrix3D.create()
|
|
|
|
occ = adsk.fusion.Occurrence.cast(root_occ)
|
|
if not occ:
|
|
return mat # root
|
|
|
|
def getParentOccs(joint):
|
|
occs_list = []
|
|
|
|
if joint != None:
|
|
occs_list.append(joint)
|
|
|
|
if joint.assemblyContext != None:
|
|
occs_list = occs_list + getParentOccs(joint.assemblyContext)
|
|
|
|
return occs_list
|
|
|
|
occs = getParentOccs(root_occ)
|
|
mat3ds = [occ.transform for occ in occs if occ!= None]
|
|
#mat3ds.reverse()
|
|
for mat3d in mat3ds:
|
|
mat.transformBy(mat3d)
|
|
return mat
|
|
|
|
# mat :adsk.core.Matrix3D = getMatrixFromRoot(joint.occurrenceOne)
|
|
# ori1 :adsk.core.Point3D = joint.geometryOrOriginOne.origin.copy()
|
|
# ori1.transformBy(mat)
|
|
|
|
mat = getMatrixFromRoot(joint.occurrenceTwo)
|
|
ori2 :adsk.core.Point3D = joint.geometryOrOriginTwo.origin.copy()
|
|
ori2.transformBy(mat)
|
|
return ori2 #ori1,
|
|
|
|
try:
|
|
#xyz_of_joint = getJointOriginWorldCoordinates(joint)
|
|
xyz_of_joint = joint.geometryOrOriginTwo.origin
|
|
joint_dict['xyz'] = [round(i / 100.0, 6) for i in xyz_of_joint.asArray()] # converted to meter
|
|
#print(f"xyz : {joint_dict['xyz']}")
|
|
|
|
except:
|
|
print('Failed:\n{}'.format(traceback.format_exc()))
|
|
try:
|
|
if type(joint.geometryOrOriginTwo)==adsk.fusion.JointOrigin:
|
|
data = joint.geometryOrOriginTwo.geometry.origin.asArray()
|
|
else:
|
|
data = joint.geometryOrOriginTwo.origin.asArray()
|
|
joint_dict['xyz'] = [round(i / 100.0, 6) for i in data] # converted to meter
|
|
except:
|
|
msg = joint.name + " doesn't have joint origin. Please set it and run again."
|
|
break
|
|
|
|
joints_dict[joint.name] = joint_dict
|
|
# msg += str(joints_dict)
|
|
return joints_dict, msg
|
|
# def make_joints_dict(root, msg):
|
|
# """
|
|
# joints_dict holds parent, axis and xyz informatino of the joints
|
|
|
|
|
|
# Parameters
|
|
# ----------
|
|
# root: adsk.fusion.Design.cast(product)
|
|
# Root component
|
|
# msg: str
|
|
# Tell the status
|
|
|
|
# Returns
|
|
# ----------
|
|
# joints_dict:
|
|
# {name: {type, axis, upper_limit, lower_limit, parent, child, xyz}}
|
|
# msg: str
|
|
# Tell the status
|
|
# """
|
|
|
|
# joints_dict = {}
|
|
|
|
# ## Root joints
|
|
# for joint in root.joints:
|
|
# joint_dict = get_joint_dict(joint)
|
|
# if type(joint_dict) is dict:
|
|
# key = utils.get_valid_filename(joint.name)
|
|
# joints_dict[key] = joint_dict
|
|
# else: ## Error happens and throw an msg
|
|
# msg = joint_dict
|
|
|
|
# ## Traverse non-root nested components
|
|
# nonroot_joints_dict = traverseAssembly(root.occurrences.asList, 1)
|
|
|
|
# ## Combine
|
|
# joints_dict.update(nonroot_joints_dict)
|
|
|
|
# return joints_dict, msg
|
|
|
|
|
|
# ## TODO: Make msg more accurate and elegent
|
|
# def traverseAssembly(occurrences, currentLevel, joints_dict={}, msg='Successfully create URDF file'):
|
|
|
|
# for i in range(0, occurrences.count):
|
|
# occ = occurrences.item(i)
|
|
|
|
# if occ.component.joints.count > 0:
|
|
# for joint in occ.component.joints:
|
|
# ass_joint = joint.createForAssemblyContext(occ)
|
|
# joint_dict = get_joint_dict(ass_joint)
|
|
# if type(joint_dict) is dict:
|
|
# key = utils.get_valid_filename(occ.fullPathName) + '_' + joint.name
|
|
# joints_dict[key] = joint_dict
|
|
# else: ## Error happens and throw an msg
|
|
# msg = joint_dict
|
|
# # tmp_joints_dict, msg = make_joints_dict(ass_joint, msg)
|
|
# if msg != 'Successfully create URDF file':
|
|
# print('Check Component: ' + comp.name + '\t Joint: ' + joint.name)
|
|
# return 0
|
|
|
|
# # print('Level {} {}: Joint {}.'.format(currentLevel, occ.name, ass_joint.name))
|
|
# else:
|
|
# pass
|
|
# # print('Level {} {} has no joints.'.format(currentLevel, occ.name))
|
|
|
|
# if occ.childOccurrences:
|
|
# joints_dict = traverseAssembly(occ.childOccurrences, currentLevel + 1, joints_dict, msg)
|
|
|
|
# return joints_dict
|
|
|
|
|
|
|
|
# def get_joint_dict(joint):
|
|
# joint_type_list = [
|
|
# 'fixed', 'revolute', 'prismatic', 'Cylinderical',
|
|
# 'PinSlot', 'Planner', 'Ball'] # these are the names in urdf
|
|
|
|
# joint_dict = {}
|
|
# 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
|
|
|
|
# # support "Revolute", "Rigid" and "Slider"
|
|
# if joint_type == 'revolute':
|
|
# joint_dict['axis'] = [round(i, 6) for i in \
|
|
# joint.jointMotion.rotationAxisVector.asArray()] ## In Fusion, exported axis is normalized.
|
|
# 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.'
|
|
# return msg
|
|
# elif not max_enabled and min_enabled:
|
|
# msg = joint.name + 'is not set its upper limit. Please set it and try again.'
|
|
# return msg
|
|
# else: # if there is no angle limit
|
|
# joint_dict['type'] = 'continuous'
|
|
|
|
# elif joint_type == 'prismatic':
|
|
# joint_dict['axis'] = [round(i, 6) for i in \
|
|
# joint.jointMotion.slideDirectionVector.asArray()] # Also normalized
|
|
# 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.'
|
|
# return msg
|
|
# elif not max_enabled and min_enabled:
|
|
# msg = joint.name + 'is not set its upper limit. Please set it and try again.'
|
|
# return msg
|
|
# elif joint_type == 'fixed':
|
|
# pass
|
|
|
|
# def get_parent(occ):
|
|
# # function to find the root component of the joint. This is necessary for the correct component name in the urdf file
|
|
# if occ.assemblyContext != None:
|
|
# #print(occ.name)
|
|
# occ = get_parent(occ.assemblyContext)
|
|
# return occ
|
|
|
|
# if joint.occurrenceTwo != None and joint.occurrenceOne != None and joint.occurrenceOne.isLightBulbOn:
|
|
# parent_occ = get_parent(joint.occurrenceTwo)
|
|
# # print("Joint 1 Parent: " +parent_occ.name)
|
|
# if "base_link" in parent_occ.name:
|
|
# joint_dict['parent'] = 'base_link'
|
|
# base_link = parent_occ
|
|
# else:
|
|
# joint_dict['parent'] = re.sub('[ :()]', '_', parent_occ.name)
|
|
# # print("Joint 2: " +joint.occurrenceOne.name)
|
|
# parent_occ = get_parent(joint.occurrenceOne)
|
|
# # print("Joint 2 Parent: " +parent_occ.name)
|
|
# joint_dict['child'] = re.sub('[ :()]', '_', parent_occ.name) <-- part of original
|
|
# # joint_dict['child'] = utils.get_valid_filename(joint.occurrenceOne.fullPathName)
|
|
|
|
|
|
# # if joint.occurrenceTwo.component.name == 'base_link':
|
|
# # joint_dict['parent'] = 'base_link'
|
|
# # else:
|
|
# # joint_dict['parent'] = utils.get_valid_filename(joint.occurrenceTwo.fullPathName)
|
|
# # joint_dict['child'] = utils.get_valid_filename(joint.occurrenceOne.fullPathName)
|
|
|
|
# ############### Auxiliary Function for the geometryOrOriginTwo issue
|
|
# # The fix is from @syuntoku14/fusion2urdf, commited by @SpaceMaster85, https://github.com/syuntoku14/fusion2urdf/commit/8786e6318cdcaaf32070148451a27ab6e4f6697d
|
|
|
|
# #There seem to be a problem with geometryOrOriginTwo. To calcualte the correct orogin of the generated stl files following approach was used.
|
|
# #https://forums.autodesk.com/t5/fusion-360-api-and-scripts/difference-of-geometryororiginone-and-geometryororiginonetwo/m-p/9837767
|
|
# #Thanks to Masaki Yamamoto!
|
|
|
|
# # Coordinate transformation by matrix
|
|
# # M: 4x4 transformation matrix
|
|
# # a: 3D vector
|
|
# def trans(M, a):
|
|
# ex = [M[0],M[4],M[8]]
|
|
# ey = [M[1],M[5],M[9]]
|
|
# ez = [M[2],M[6],M[10]]
|
|
# oo = [M[3],M[7],M[11]]
|
|
# b = [0, 0, 0]
|
|
# for i in range(3):
|
|
# b[i] = a[0]*ex[i]+a[1]*ey[i]+a[2]*ez[i]+oo[i]
|
|
# return(b)
|
|
# # Returns True if two arrays are element-wise equal within a tolerance
|
|
# def allclose(v1, v2, tol=1e-6):
|
|
# return( max([abs(a-b) for a,b in zip(v1, v2)]) < tol )
|
|
# ####################################
|
|
|
|
|
|
# try:
|
|
# ## A fix from @SpaceMaster85
|
|
# xyz_from_one_to_joint = joint.geometryOrOriginOne.origin.asArray() # Relative Joint pos
|
|
# xyz_from_two_to_joint = joint.geometryOrOriginTwo.origin.asArray() # Relative Joint pos
|
|
# xyz_of_one = joint.occurrenceOne.transform.translation.asArray() # Link origin
|
|
# xyz_of_two = joint.occurrenceTwo.transform.translation.asArray() # Link origin
|
|
# M_two = joint.occurrenceTwo.transform.asArray() # Matrix as a 16 element array.
|
|
|
|
# # Compose joint position
|
|
# case1 = allclose(xyz_from_two_to_joint, xyz_from_one_to_joint)
|
|
# case2 = allclose(xyz_from_two_to_joint, xyz_of_one)
|
|
# if case1 or case2:
|
|
# xyz_of_joint = xyz_from_two_to_joint
|
|
# else:
|
|
# xyz_of_joint = trans(M_two, xyz_from_two_to_joint)
|
|
|
|
|
|
# joint_dict['xyz'] = [round(i / 100.0, 6) for i in xyz_of_joint] # converted to meter
|
|
# except:
|
|
# try:
|
|
# if type(joint.geometryOrOriginTwo)==adsk.fusion.JointOrigin:
|
|
# data = joint.geometryOrOriginTwo.geometry.origin.asArray()
|
|
# else:
|
|
# data = joint.geometryOrOriginTwo.origin.asArray()
|
|
# joint_dict['xyz'] = [round(i / 100.0, 6) for i in data] # converted to meter
|
|
# except:
|
|
# msg = joint.name + " doesn't have joint origin. Please set it and run again."
|
|
# return msg
|
|
|
|
# # print("Processed joint {}, parent {}, child {}".format(joint.name, joint_dict['parent'], joint_dict['child']))
|
|
# return joint_dict |