Skip to content

Commit

Permalink
Update Joint.py
Browse files Browse the repository at this point in the history
The former script seams to be brocken due to an API change.
It seams that the origin of the components is not set correct in the URDF File. The origin needs to be set to the joint values, because the STL files are in the context of the main construction.
I found the calculation for the xyz values in the Autodesk Forum:
https://forums.autodesk.com/t5/fusion-360-api-and-scripts/difference-of-geometryororiginone-and-geometryororiginonetwo/m-p/9837767

Hope this fix the issue
  • Loading branch information
SpaceMaster85 authored Jan 9, 2021
1 parent 9d3cef3 commit 8786e63
Showing 1 changed file with 41 additions and 3 deletions.
44 changes: 41 additions & 3 deletions URDF_Exporter/core/Joint.py
Original file line number Diff line number Diff line change
Expand Up @@ -175,9 +175,47 @@ def make_joints_dict(root, msg):
joint_dict['parent'] = re.sub('[ :()]', '_', joint.occurrenceTwo.name)
joint_dict['child'] = re.sub('[ :()]', '_', joint.occurrenceOne.name)


#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:
joint_dict['xyz'] = [round(i / 100.0, 6) for i in \
joint.geometryOrOriginOne.origin.asArray()] # converted to meter
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:
Expand All @@ -190,4 +228,4 @@ def make_joints_dict(root, msg):
break

joints_dict[joint.name] = joint_dict
return joints_dict, msg
return joints_dict, msg

0 comments on commit 8786e63

Please sign in to comment.