`, ``, `` form
47 | - ``: defines the dynamics information about the link
48 | - ``: the mass of the link
49 | - ``: defines the link's center-of-mass-frame *C* w.r.t the link-frame *L*
50 | - ``: defines the inertia tensor in center-of-mass-frame *C*
51 | - ``: the link's moment of inertia about the CoM frame *C*'s x-axis
52 | - ``: the link's product of inertia about CoM frame's x-axis and y-axis
53 | - ``: the link's product of inertia about CoM frame's x-axis and z-axis
54 | - ``: the link's moment of inertia about the CoM frame *C*'s y-axis
55 | - ``: the link's product of inertia about CoM frame's y-axis and z-axis
56 | - ``: the link's moment of inertia about the CoM frame *C*'s z-axis
57 | - ``: defines collision geometry of the link with required `name` attribute
58 | - ``: defines visual properties of the link with required `name` attribute
59 | - ``: defines a joint with required `name` and `type` attributes
60 | - ``: defines the name of the parent frame (conventionally the parent link frame) or "world"
61 | - ``: defines the name of the child frame (conventionally the child link frame)
62 | - ``: defines the axis for joint motion
63 | - ``: expressed the axis with a unit vector in `` `` `` form w.r.t the joint frame *J*
64 | - ``: defines the limits of the joint with this axis
65 | - ``: lower joint limit (radians for revolute joints, meters for prismatic joints)
66 | - ``: upper joint limit (radians for revolute joints, meters for prismatic joints)
67 | - ``: the second axis for some special joint type
68 | - ``: expressed the axis with a unit vector in `` `` `` form w.r.t the joint frame *J*
69 | - ``: defines the limits of the joint with this axis
70 | - ``: lower joint limit (radians for revolute joints, meters for prismatic joints)
71 | - ``: upper joint limit (radians for revolute joints, meters for prismatic joints)
72 | - ``: defines the joint frame *J*. By default, the joint frame is expressed in the child link frame
73 |
74 | ## Fusion 360 to URDF
75 | | URDF Element | Fusion 360 Elements | Transformation |
76 | | ------------ | ------------------- | -------------- |
77 | | Joint *i* origin $^{L_{i-1}}T_{J_{i}}$| $^{W}T_{L_{i-1}}$, $^{W}T_{J_{i}}$ | $^{L_{i-1}}T_{J_{i}} = {(^{W}T_{L_{i-1}})}^{T} \cdot {^{W}T_{J_{i}}}$ |
78 | | Joint *i* axis $^{J_{i}}\vec{a}$| $^{W}\vec{a}$, $^{W}T_{J_{i}}$| $^{J_{i}}\vec{a} = {(^{W}T_{J_{i}})}^{T} \cdot ^{W}\vec{a}$ |
79 | | Link *i*'s inertial origin $^{J_{i}}{T_{L_{i}CoM}}$ | $^{W}T_{J_{i}}$, $^{W}{T_{L_{i}CoM}}$ | $^{J_{i}}{T_{L_{i}CoM}} = {(^{W}T_{J_{i}})^{T}} \cdot {^{W}{T_{L_{i}CoM}}}$ |
80 | |Link *i*'s visual origin $^{J_{i}}T_{L_{i}}$ | $^{W}T_{J_{i}}$, $^{W}T_{L_{i}}$ | $^{J_{i}}T_{L_{i}} = {(^{W}T_{J_{i}})^{T}} \cdot {^{W}T_{L_{i}}}$ |
81 | |Link *i*'s collision origin $^{J_{i}}T_{L_{i}}$ | $^{W}T_{J_{i}}$, $^{W}T_{L_{i}}$ | $^{J_{i}}T_{L_{i}} = {(^{W}T_{J_{i}})^{T}} \cdot {^{W}T_{L_{i}}}$ |
82 |
83 | ## Fusion 360 to SDFormat
84 | | SDF Element | Fusion 360 Elements | Transformation |
85 | | ----------- | ------------------- | -------------- |
86 | | Joint *i* pose $^{L_{i}}T_{J_{iC}}$ | $^{W}T_{L_{i}}$, $^{W}T_{J_{iC}}$ | $^{L_{i}}T_{J_{iC}} = {(^{W}T_{L_{i}})^{T}} \cdot {^{W}T_{J_{iC}}}$|
87 | | Joint *i* axis1 $^{J_{iC}}\vec{a_1}$| $^{W}\vec{a_1}$, $^{W}T_{J_{iC}}$ | $^{J_{iC}}\vec{a_1} = {(^{W}T_{J_{iC}})^{T}} \cdot {^{W}\vec{a_1}}$ |
88 | | Joint *i* axis1 $^{J_{iC}}\vec{a_2}$| $^{W}\vec{a_2}$, $^{W}T_{J_{iC}}$ | $^{J_{iC}}\vec{a_2} = {(^{W}T_{J_{iC}})^{T}} \cdot {^{W}\vec{a_2}}$ |
89 | | Link *i*'s pose $^{W}T_{L_{i}}$ | $^{W}T_{L_{i}}$ | $^{W}T_{L_{i}} = {^{W}T_{L_{i}}}$ |
90 | | Link *i*'s inertial pose $^{L_{i}}T_{L_{i}COM}$ | $^{W}T_{L_{i}}$, $^{W}{T_{L_{i}CoM}}$ | $^{L_{i}}T_{L_{i}COM} = {(^{W}T_{L_{i}})^{T}} \cdot {^{W}{T_{L_{i}CoM}}}$ |
91 |
--------------------------------------------------------------------------------
/Add-IN/ACDC4Robot/core/math_operation.py:
--------------------------------------------------------------------------------
1 | # -*- coding: utf-8 -*-
2 | # Author: Nuofan - 12233200@mail.sustech.edu.cn
3 | """
4 | contains functions for math operations,
5 | including matrix operations, spacial mathematics, etc
6 | """
7 | import adsk, adsk.core, adsk.fusion
8 | import math
9 |
10 | def matrix3d_2_pose(matrix: adsk.core.Matrix3D):
11 | """
12 | Convert configuration matrix (Matrix3D) into pose(translation, rotation) representation
13 |
14 | Parameter:
15 | ---------
16 | matrix: adsk.core.Matric3D
17 | Homogeneous matrix
18 |
19 | Return:
20 | ---------
21 | pose: List
22 | [x, y, z, roll, pitch, yaw]
23 | This euler angle convertion is "XYZ"
24 | """
25 | x = matrix.translation.x * 0.01 # cm to m
26 | y = matrix.translation.y * 0.01 # cm to m
27 | z = matrix.translation.z * 0.01 # cm to m
28 |
29 | threshold = 10e-10
30 | check = lambda v: v if abs(v) > threshold else 0.0
31 |
32 | r11 = matrix.getCell(0, 0)
33 | r21 = matrix.getCell(1, 0)
34 | r31 = matrix.getCell(2, 0)
35 | r32 = matrix.getCell(2, 1)
36 | r33 = matrix.getCell(2, 2)
37 | r23 = matrix.getCell(1, 2)
38 | r22 = matrix.getCell(1, 1)
39 | sy = math.sqrt(r11**2 + r21**2)
40 | singular = sy < 1e-6
41 |
42 | if not singular:
43 | roll = math.atan2(r32, r33)
44 | pitch = math.atan2(-r31, sy)
45 | yaw = math.atan2(r21, r11)
46 | else:
47 | roll = math.atan2(-r23, r22)
48 | pitch = math.atan2(-r31, sy)
49 | yaw = 0
50 |
51 | pose = [x, y, z, roll, pitch, yaw]
52 | return pose
53 |
54 | def matrix3d_2_euler_xyz(matrix: adsk.core.Matrix3D):
55 | """
56 | Convert configuration matrix (Matrix3D) into pose(translation, rotation) representation
57 |
58 | Parameter:
59 | ---------
60 | matrix: adsk.core.Matric3D
61 | Homogeneous matrix
62 |
63 | Return:
64 | ---------
65 | pose: List
66 | [x, y, z, roll, pitch, yaw]
67 | This euler angle convertion is "xyz"
68 | """
69 |
70 | # TODO: Still needs to check if it is right
71 | # Some problem seems related with this function
72 | x = matrix.translation.x * 0.01 # cm to m
73 | y = matrix.translation.y * 0.01 # cm to m
74 | z = matrix.translation.z * 0.01 # cm to m
75 |
76 | threshold = 10e-10
77 | check = lambda v: v if abs(v) > threshold else 0.0
78 |
79 | r11 = matrix.getCell(0, 0)
80 | r12 = matrix.getCell(0, 1)
81 | r13 = matrix.getCell(0, 2)
82 | r22 = matrix.getCell(1, 1)
83 | r23 = matrix.getCell(1, 2)
84 | r32 = matrix.getCell(2, 1)
85 | r33 = matrix.getCell(2, 2)
86 |
87 | sy = math.sqrt(r33**2 + r23**2)
88 | singular = sy < 1e-6
89 |
90 | if not singular:
91 | roll = math.atan2(-r23, r33)
92 | pitch = math.atan2(r13, sy)
93 | yaw = math.atan2(-r12, r11)
94 | else:
95 | roll = math.atan2(-r32, r22)
96 | pitch = math.atan2(r13, sy)
97 | yaw = 0
98 |
99 | pose = [x, y, z, roll, pitch, yaw]
100 | return pose
101 |
102 | def coordinate_transform(w_T_from: adsk.core.Matrix3D, w_T_to: adsk.core.Matrix3D) -> adsk.core.Matrix3D:
103 | """
104 | Returns a transform matrix to transform a from_frame to to_frame
105 | Parameters
106 | ---------
107 | w_T_from: the from_frame
108 | w_T_to: the to_frame
109 | Return
110 | ---------
111 | from_T_to: adsk.core.Matrix3D
112 | represent to_frame w.r.t from_frame
113 | w_T_from * from_T_to = w_T_to ->
114 | from_T_to = inv(w_T_from) * w_T_to
115 | """
116 | from_T_to = adsk.core.Matrix3D.create()
117 | w_T_from.invert() # from_T_w
118 |
119 | for i in range(4):
120 | for j in range(4):
121 | value = 0
122 | for k in range(4):
123 | value += w_T_from.getCell(i, k) * w_T_to.getCell(k, j)
124 | from_T_to.setCell(i, j, value)
125 |
126 | return from_T_to
127 |
128 | def change_orientation(a_R_b: list, b_v: list):
129 | """
130 | Rotate vector v to a different orientation or
131 | Represent vector v in a different frame
132 |
133 | Parameters
134 | ---------
135 | a_R_b: 3*3 array
136 | represent b-frame's orientation w.r.t a-frame
137 | b_v: 3*1 array
138 | v's coordinates w.r.t b-frame
139 | Return:
140 | ---------
141 | a_v: 3*1 array
142 | v's coordinates w.r.t a-frame
143 | """
144 | a_v = [[a_R_b[0][0]*b_v[0][0]+a_R_b[0][1]*b_v[1][0]+a_R_b[0][2]*b_v[2][0]],
145 | [a_R_b[1][0]*b_v[0][0]+a_R_b[1][1]*b_v[1][0]+a_R_b[1][2]*b_v[2][0]],
146 | [a_R_b[2][0]*b_v[0][0]+a_R_b[2][1]*b_v[1][0]+a_R_b[2][2]*b_v[2][0]]]
147 |
148 | return a_v
149 |
150 | def matrix3D_to_str(M: adsk.core.Matrix3D) -> str:
151 | """
152 | Return a str for printing Matrix3D object
153 | """
154 | s = "[{}, {}, {}, {}\n \
155 | {}, {}, {}, {}\n \
156 | {}, {}, {}, {}\n \
157 | {}, {}, {}, {}\n]".format(M.getCell(0,0), M.getCell(0,1), M.getCell(0,2), M.getCell(0,3),
158 | M.getCell(1,0), M.getCell(1,1), M.getCell(1,2), M.getCell(1,3),
159 | M.getCell(2,0), M.getCell(2,1), M.getCell(2,2), M.getCell(2,3),
160 | M.getCell(3,0), M.getCell(3,1), M.getCell(3,2), M.getCell(3,3))
161 |
162 | return s
163 |
164 | def matrix_multi(M1: list, M2: list):
165 | """
166 | Return M = M1 * M2
167 |
168 | Parameters:
169 | ---------
170 | M1: 3*3 list
171 | M2: 3*3 list
172 |
173 | Return:
174 | ---------
175 | M: 3*3 list
176 | """
177 | M = [[0 for i in range(3)] for j in range(3)] # generate a 3*3 list
178 |
179 | for i in range(3):
180 | for j in range(3):
181 | for k in range(3):
182 | M[i][j] += M1[i][k]*M2[k][j]
183 |
184 | return M
185 |
186 | def get_rotation_matrix(H: adsk.core.Matrix3D):
187 | """
188 | Get rotation matrix R from Matrix3D object
189 |
190 | Parameters:
191 | ---------
192 | H: adsk.core.Matrix3D
193 |
194 | Return:
195 | ---------
196 | R: 3*3 list
197 | Rotation matrix
198 | """
199 | R = [[H.getCell(0, 0), H.getCell(0, 1), H.getCell(0, 2)],
200 | [H.getCell(1, 0), H.getCell(1, 1), H.getCell(1, 2)],
201 | [H.getCell(2, 0), H.getCell(2, 1), H.getCell(2, 2)]]
202 |
203 | return R
204 |
205 | def matrix_transpose(M: list):
206 | """
207 | Return transpose of a matrix
208 |
209 | Parameter:
210 | ---------
211 | M: 3*3 list
212 |
213 | Return:
214 | ---------
215 | M_T: 3*3 list
216 | Transpose of M
217 | """
218 | M_T = [[0 for i in range(3)] for j in range(3)] # generate a 3*3 list
219 |
220 | for i in range(3):
221 | for j in range(3):
222 | M_T[i][j] = M[j][i]
223 |
224 | return M_T
225 |
226 | def matrix_multiply(A, B):
227 | # Matrix multiplication of A and B
228 | result = [[sum(A[i][k] * B[k][j] for k in range(len(B))) for j in range(len(B[0]))] for i in range(len(A))]
229 | return result
230 |
231 | def matrix_subtract(A, B):
232 | # Matrix subtraction of A and B
233 | result = [[A[i][j] - B[i][j] for j in range(len(A[0]))] for i in range(len(A))]
234 | return result
235 |
236 | def determinant_2x2(matrix):
237 | # Calculate determinant of a 2x2 matrix
238 | return matrix[0][0] * matrix[1][1] - matrix[0][1] * matrix[1][0]
239 |
240 | def determinant_3x3(matrix):
241 | # Calculate determinant of a 3x3 matrix
242 | det = matrix[0][0] * determinant_2x2([[matrix[1][1], matrix[1][2]], [matrix[2][1], matrix[2][2]]])
243 | det -= matrix[0][1] * determinant_2x2([[matrix[1][0], matrix[1][2]], [matrix[2][0], matrix[2][2]]])
244 | det += matrix[0][2] * determinant_2x2([[matrix[1][0], matrix[1][1]], [matrix[2][0], matrix[2][1]]])
245 | return det
246 |
247 | def eigenvalues_and_eigenvectors(matrix):
248 | # Finding eigenvalues and eigenvectors of a 3x3 matrix
249 | detA = determinant_3x3(matrix)
250 | A_minus_lambdaI = matrix_subtract(matrix, [[detA, 0, 0], [0, detA, 0], [0, 0, detA]])
251 |
252 | eigenvalues = []
253 | eigenvectors = []
254 |
255 | # Finding the eigenvalues
256 | eigenvalues.append(detA - matrix[0][0])
257 | eigenvalues.append(detA - matrix[1][1])
258 | eigenvalues.append(detA - matrix[2][2])
259 |
260 | # Finding the eigenvectors for each eigenvalue
261 | for eig_val in eigenvalues:
262 | A_minus_lambdaI[0][0] = matrix[0][0] - eig_val
263 | A_minus_lambdaI[1][1] = matrix[1][1] - eig_val
264 | A_minus_lambdaI[2][2] = matrix[2][2] - eig_val
265 |
266 | # Solve the linear system (A-lambdaI)v = 0
267 | v = [0, 0, 0]
268 | if A_minus_lambdaI[0][0] == 0 and A_minus_lambdaI[0][1] == 0 and A_minus_lambdaI[0][2] == 0:
269 | v[0] = 1
270 | elif A_minus_lambdaI[1][0] == 0 and A_minus_lambdaI[1][1] == 0 and A_minus_lambdaI[1][2] == 0:
271 | v[1] = 1
272 | elif A_minus_lambdaI[2][0] == 0 and A_minus_lambdaI[2][1] == 0 and A_minus_lambdaI[2][2] == 0:
273 | v[2] = 1
274 | else:
275 | v[0] = -(A_minus_lambdaI[0][1]*A_minus_lambdaI[1][2] - A_minus_lambdaI[0][2]*A_minus_lambdaI[1][1])
276 | v[1] = A_minus_lambdaI[0][0]*A_minus_lambdaI[1][2] - A_minus_lambdaI[0][2]*A_minus_lambdaI[1][0]
277 | v[2] = -(A_minus_lambdaI[0][0]*A_minus_lambdaI[1][1] - A_minus_lambdaI[0][1]*A_minus_lambdaI[1][0])
278 |
279 | # Normalize the eigenvector
280 | magnitude = (v[0]**2 + v[1]**2 + v[2]**2)**0.5
281 | eigenvectors.append([v[0] / magnitude, v[1] / magnitude, v[2] / magnitude])
282 |
283 | return eigenvalues, eigenvectors
--------------------------------------------------------------------------------
/Add-IN/ACDC4Robot/core/mjcf.py:
--------------------------------------------------------------------------------
1 | # -*- coding: utf-8 -*-
2 | # Author: Nuofan - 12233200@mail.sustech.edu.cn
3 | # Date: 20231124
4 | # Functions for generating mjcf file
5 |
6 | import adsk, adsk.core, adsk.fusion
7 | from .link import Link
8 | from .joint import Joint
9 | import xml.etree.ElementTree as ET
10 | from xml.etree.ElementTree import Element, SubElement
11 | from . import math_operation as math_op
12 | from . import utils
13 | from ..commands.ACDC4Robot import constants
14 |
15 | def get_mjcf_mesh(link: Link) -> Element:
16 | """
17 | Get mesh element for mjcf
18 |
19 | Return
20 | ---------
21 | mesh_ele: Element
22 | mesh element in mjcf
23 | """
24 | mesh_ele = ET.Element("mesh")
25 | name: str = link.get_name()
26 | file_name: str = "meshes/" + link.get_name() + ".stl"
27 | mesh_ele.attrib = {"name":name, "file": file_name, "scale": "0.001 0.001 0.001"}
28 | return mesh_ele
29 |
30 | def get_mjcf_body(link: Link, parent_link: Link = None) -> Element:
31 | """
32 | Get body element for mjcf
33 |
34 | Return
35 | ---------
36 | body_ele: Element
37 | body element represent a link in mjcf
38 | """
39 | body_ele = Element("body")
40 | body_name: str = link.get_name()
41 | if parent_link is None:
42 | # a root of a body tree
43 | # pose = math_op.matrix3d_2_euler_xyz(link.pose)
44 | pose = math_op.matrix3d_2_pose(link.pose)
45 | pos_attr = "{} {} {}".format(pose[0], pose[1], pose[2])
46 | euler_attr = "{} {} {}".format(pose[3], pose[4], pose[5])
47 | else:
48 | parent_frame: adsk.core.Matrix3D = parent_link.pose # parent_frame w.r.t world_frame
49 | child_frame: adsk.core.Matrix3D = link.pose # child_frame w.r.t world_frame
50 | parent_T_child: adsk.core.Matrix3D = math_op.coordinate_transform(parent_frame, child_frame)
51 | # pose = math_op.matrix3d_2_euler_xyz(parent_T_child)
52 | pose = math_op.matrix3d_2_pose(parent_T_child)
53 | pos_attr = "{} {} {}".format(pose[0], pose[1], pose[2])
54 | euler_attr = "{} {} {}".format(pose[3], pose[4], pose[5])
55 |
56 | body_ele.attrib = {"name": body_name, "pos": pos_attr, "euler": euler_attr}
57 |
58 | # insert parent joint if it exists
59 | parent_joint = link.get_parent_joint()
60 | if parent_joint is not None:
61 | joint_ele: Element = get_mjcf_joint(Joint(parent_joint))
62 | # undifined joint will welded two body
63 | if joint_ele is not None:
64 | body_ele.append(joint_ele)
65 |
66 | geom_ele: Element = get_mjcf_geom(link)
67 | inertial_ele: Element = get_mjcf_inertial(link)
68 |
69 | body_ele.append(geom_ele)
70 | body_ele.append(inertial_ele)
71 |
72 | return body_ele
73 |
74 | def get_mjcf_geom(link: Link) -> Element:
75 | """
76 | Get geom element for mjcf
77 |
78 | Return
79 | ---------
80 | geom_ele: Element
81 | geom element in mjcf
82 | """
83 | geom_ele = Element("geom")
84 | geom_name = link.get_name() + "_geom"
85 | # geom_pos = "{} {} {}".format(link.get_pose_sdf()[0], link.get_pose_sdf()[1], link.get_pose_sdf()[2])
86 | # geom_euler = "{} {} {}".format(link.get_pose_sdf()[3], link.get_pose_sdf()[4], link.get_pose_sdf()[5])
87 |
88 | # geom should coincide with the body frame? It works
89 | geom_pos = "0 0 0"
90 | geom_euler = "0 0 0"
91 |
92 | visual_body = link.get_visual_body()
93 | col_body = link.get_collision_body()
94 | if (visual_body is None) and (col_body is None):
95 | geom_ele.attrib = {"name": geom_name, "type": "mesh",
96 | "mesh": link.get_name(), "pos": geom_pos, "euler": geom_euler}
97 | elif (visual_body is not None) and (col_body is not None):
98 | error_message = "mjcf will automatically generate geometry for collision. \n"
99 | error_message = error_message + link.get_name() + " does not need to set geometry for visual and collision seperately."
100 | utils.error_box(error_message)
101 | utils.terminate_box()
102 | elif (visual_body is None) and (col_body is not None):
103 | error_message = "mjcf will automatically generate geometry for collision. \n"
104 | error_message = error_message + link.get_name() + " does not need to set geometry for visual and collision seperately."
105 | utils.error_box(error_message)
106 | utils.terminate_box()
107 | elif (visual_body is not None) and (col_body is None):
108 | error_message = "mjcf will automatically generate geometry for collision. \n"
109 | error_message = error_message + link.get_name() + " does not need to set geometry for visual and collision seperately."
110 | utils.error_box(error_message)
111 | utils.terminate_box()
112 |
113 | return geom_ele
114 |
115 |
116 | def get_mjcf_inertial(link: Link) -> Element:
117 | """
118 | Get inertial element for mjcf
119 |
120 | Return
121 | ---------
122 | inertial_ele: Element
123 | inertial element in mjcf
124 | """
125 | inertial_ele = Element("inertial")
126 | mass: str = str(link.get_mass())
127 | CoM: list = link.get_CoM_wrt_link()
128 | pos_att: str = "{} {} {}".format(CoM[0], CoM[1], CoM[2])
129 | # get error for euler attribute in inertial, so remove it
130 | # euler_att: str = "{} {} {}".format(CoM[3], CoM[4], CoM[5])
131 | inertial: list = link.get_inertia_mjcf()
132 | # 6 numbers in the following order: M(1,1), M(2,2), M(3,3), M(1,2), M(1,3), M(2,3).
133 | # which is ixx, iyy, izz, ixy, ixz, iyz
134 | fullinertia_att = "{} {} {} {} {} {}".format(inertial[0], inertial[1], inertial[2],
135 | inertial[3], inertial[4], inertial[5])
136 | # inertial_ele.attrib = {"mass": mass, "pos": pos_att,
137 | # "euler": euler_att, "fullinertia": fullinertia_att}
138 | inertial_ele.attrib = {"mass": mass, "pos": pos_att,
139 | "fullinertia": fullinertia_att}
140 |
141 | return inertial_ele
142 |
143 | def get_mjcf_joint(joint: Joint) -> Element:
144 | """
145 | Get joint element for mjcf
146 | A joint creates motion degrees of freedom
147 | between the body where it is defined and the body's parent.
148 |
149 | Return
150 | ---------
151 | joint_ele: Element
152 | joint element in mjcf
153 | """
154 | # If no joints are defined, the body is welded to its parent.
155 | joint_type = joint.get_mjcf_joint_type()
156 | if joint_type is not None:
157 | joint_ele: Element = ET.Element("joint")
158 | name_att = joint.get_name()
159 | pose = joint.get_sdf_origin()
160 | axis = joint.get_axis_mjcf()
161 | joint_ele.attrib = {"name": name_att, "type": joint_type,
162 | "axis": "{} {} {}".format(axis[0], axis[1], axis[2]),
163 | "pos": "{} {} {}".format(pose[0], pose[1], pose[2])}
164 | return joint_ele
165 | else:
166 | return None
167 |
168 |
169 | def get_mjcf(root_comp: adsk.fusion.Component, robot_name: str, dir: str) -> Element:
170 | """
171 | Get a mjcf format xml file contains robot model
172 |
173 | Parameters:
174 | root_comp: root_comp of the design
175 | robotName: robot name
176 | dir: directory of the mjcf file
177 |
178 | Return:
179 | mujoco_ele: Element
180 | root element of a mujoco xml file
181 | """
182 | root = ET.Element("mujoco", {"model": robot_name})
183 |
184 | # add compiler subelement of mujoco
185 | # the default eulerseq conventioin used in URDF corresponds to the default “xyz” in MJCF
186 | # Here we use an extrinsic X-Y-Z rotation
187 | # compiler_ele = ET.SubElement(root, "compiler",
188 | # {"angle": "radian", "meshdir": (dir + "/meshes"), "eulerseq":"XYZ"})
189 | # Rotation matrix to euler in "xyz" seems have problem, use "XYZ" at temporary
190 | compiler_ele = ET.SubElement(root, "compiler",
191 | {"angle": "radian", "eulerseq":"XYZ"})
192 |
193 | # add asset subelement of mujoco
194 | asset_ele = ET.SubElement(root, "asset")
195 |
196 | # add worldbody subelement of mujoco
197 | worldbody_ele = ET.SubElement(root, "worldbody")
198 |
199 | # add light subelement of worldbody
200 | light_ele = ET.SubElement(worldbody_ele, "light")
201 | light_ele.attrib = {"directional":"true", "pos":"-0.5 0.5 3", "dir":"0 0 -1"}
202 |
203 | # add floor
204 | floor = ET.SubElement(worldbody_ele, "geom", )
205 | floor.attrib = {"pos": "0 0 0", "size": "1 1 1", "type": "plane", "rgba": "1 0.83 0.61 0.5"}
206 |
207 | # add body elements to construct a robot
208 | parent_child_dict = {}
209 | joints = root_comp.allJoints
210 | all_occs = root_comp.allOccurrences
211 |
212 | for joint in joints:
213 | parent = joint.occurrenceTwo
214 | child = joint.occurrenceOne
215 |
216 | if parent.fullPathName not in parent_child_dict:
217 | parent_child_dict[parent.fullPathName] = []
218 |
219 | parent_child_dict[parent.fullPathName].append(child)
220 |
221 | # Function to recursively add body elements to mjcf
222 | def add_body_element(parent: adsk.fusion.Occurrence, parent_body_ele: Element) -> Element:
223 | children = parent_child_dict.get(parent.fullPathName, [])
224 | for child_occ in children:
225 | # Create mjcf element for child
226 | child_link = Link(child_occ)
227 | parent_link = Link(parent)
228 | child_ele = get_mjcf_body(child_link, parent_link)
229 | parent_body_ele.append(child_ele)
230 | # child_ele = ET.SubElement(parent_body_ele, "body")
231 | # child_ele_name: str = child_link.get_name()
232 | # child_ele.attrib = {"name": child_ele_name}
233 |
234 | # Recursively add children of this child_ele
235 | add_body_element(child_occ, child_ele)
236 |
237 | # traverse all the occs for body elements
238 | for occ in all_occs:
239 | asset_ele.append(get_mjcf_mesh(Link(occ)))
240 | if occ.fullPathName not in [item.fullPathName for sublist in parent_child_dict.values() for item in sublist]:
241 | # parent_ele = ET.SubElement(worldbody_ele, "body", name=Link(occ).get_name())
242 | parent_ele = get_mjcf_body(Link(occ), None)
243 | worldbody_ele.append(parent_ele)
244 | add_body_element(occ, parent_ele)
245 |
246 | return root
247 |
248 |
249 |
--------------------------------------------------------------------------------
/Add-IN/ACDC4Robot/commands/ACDC4Robot/entry.py:
--------------------------------------------------------------------------------
1 | import adsk.core
2 | import os
3 | from ...lib import fusion360utils as futil
4 | from ... import config
5 | from . import acdc4robot
6 | from . import constants
7 | app = adsk.core.Application.get()
8 | ui = app.userInterface
9 |
10 |
11 | # TODO *** Specify the command identity information. ***
12 | CMD_ID = f'{config.COMPANY_NAME}_{config.ADDIN_NAME}_ACDC4Robot'
13 | CMD_NAME = 'ACDC4Robot'
14 | CMD_Description = 'Export Autodesk Fusion design model to robot description format'
15 |
16 | # Specify that the command will be promoted to the panel.
17 | IS_PROMOTED = True
18 |
19 | # TODO *** Define the location where the command button will be created. ***
20 | # This is done by specifying the workspace, the tab, and the panel, and the
21 | # command it will be inserted beside. Not providing the command to position it
22 | # will insert it at the end.
23 | WORKSPACE_ID = 'FusionSolidEnvironment'
24 | PANEL_ID = 'SolidScriptsAddinsPanel'
25 | COMMAND_BESIDE_ID = 'ScriptsManagerCommand'
26 |
27 | # Resource location for command icons, here we assume a sub folder in this directory named "resources".
28 | ICON_FOLDER = os.path.join(os.path.dirname(os.path.abspath(__file__)), 'resources', '')
29 |
30 | # Local list of event handlers used to maintain a reference so
31 | # they are not released and garbage collected.
32 | local_handlers = []
33 |
34 |
35 | # Executed when add-in is run.
36 | def start():
37 | # Create a command Definition.
38 | cmd_def = ui.commandDefinitions.addButtonDefinition(CMD_ID, CMD_NAME, CMD_Description, ICON_FOLDER)
39 |
40 | # Define an event handler for the command created event. It will be called when the button is clicked.
41 | futil.add_handler(cmd_def.commandCreated, command_created)
42 |
43 | # ******** Add a button into the UI so the user can run the command. ********
44 | # Get the target workspace the button will be created in.
45 | workspace = ui.workspaces.itemById(WORKSPACE_ID)
46 |
47 | # Get the panel the button will be created in.
48 | panel = workspace.toolbarPanels.itemById(PANEL_ID)
49 |
50 | # Create the button command control in the UI after the specified existing command.
51 | control = panel.controls.addCommand(cmd_def, COMMAND_BESIDE_ID, False)
52 |
53 | # Specify if the command is promoted to the main toolbar.
54 | control.isPromoted = IS_PROMOTED
55 |
56 |
57 | # Executed when add-in is stopped.
58 | def stop():
59 | # Get the various UI elements for this command
60 | workspace = ui.workspaces.itemById(WORKSPACE_ID)
61 | panel = workspace.toolbarPanels.itemById(PANEL_ID)
62 | command_control = panel.controls.itemById(CMD_ID)
63 | command_definition = ui.commandDefinitions.itemById(CMD_ID)
64 |
65 | # Delete the button command control
66 | if command_control:
67 | command_control.deleteMe()
68 |
69 | # Delete the command definition
70 | if command_definition:
71 | command_definition.deleteMe()
72 |
73 |
74 | # Function that is called when a user clicks the corresponding button in the UI.
75 | # This defines the contents of the command dialog and connects to the command related events.
76 | def command_created(args: adsk.core.CommandCreatedEventArgs):
77 | # General logging for debug.
78 | futil.log(f'{CMD_NAME} Command Created Event')
79 |
80 | # create command inputs for model.config
81 | inputs = args.command.commandInputs
82 |
83 | # create a drop down command input to choose robot description format
84 | rdf_input = inputs.addDropDownCommandInput("robot_description_format", "Robot Description Format", adsk.core.DropDownStyles.LabeledIconDropDownStyle)
85 | rdf_items = rdf_input.listItems
86 | rdf_items.add("None", True)
87 | rdf_items.add("URDF", False)
88 | rdf_items.add("SDFormat", False)
89 | rdf_items.add("MJCF", False)
90 | rdf_items.add("URDF+", False)
91 |
92 | # create a drop down command input to choose simulation environment
93 | sim_env_input = inputs.addDropDownCommandInput("simulation_env", "Simulation Environment", adsk.core.DropDownStyles.LabeledIconDropDownStyle)
94 | sim_env_items = sim_env_input.listItems
95 | sim_env_items.add("None", True)
96 | sim_env_items.add("Gazebo", False)
97 | sim_env_items.add("PyBullet", False)
98 | sim_env_items.add("MuJoCo", False)
99 | sim_env_input.isVisible = False
100 |
101 | # create string value input for sdf info
102 | sdf_author_input = inputs.addStringValueInput("SDF_Author_name", "Author Name", "ACDC4Robot")
103 | sdf_author_input.isVisible = False
104 | sdf_description_input = inputs.addTextBoxCommandInput("SDF_Description", "Description", "Description about the robot model", 3, False)
105 | sdf_description_input.isVisible = False
106 |
107 | # # https://help.autodesk.com/view/fusion360/ENU/?contextId=CommandInputs
108 | # inputs = args.command.commandInputs
109 |
110 | # # TODO Define the dialog for your command by adding different inputs to the command.
111 |
112 | # # Create a simple text box input.
113 | # inputs.addTextBoxCommandInput('text_box', 'Some Text', 'Enter some text.', 1, False)
114 |
115 | # # Create a value input field and set the default using 1 unit of the default length unit.
116 | # defaultLengthUnits = app.activeProduct.unitsManager.defaultLengthUnits
117 | # default_value = adsk.core.ValueInput.createByString('1')
118 | # inputs.addValueInput('value_input', 'Some Value', defaultLengthUnits, default_value)
119 |
120 | # TODO Connect to the events that are needed by this command.
121 | futil.add_handler(args.command.execute, command_execute, local_handlers=local_handlers)
122 | futil.add_handler(args.command.inputChanged, command_input_changed, local_handlers=local_handlers)
123 | # futil.add_handler(args.command.executePreview, command_preview, local_handlers=local_handlers)
124 | # futil.add_handler(args.command.validateInputs, command_validate_input, local_handlers=local_handlers)
125 | futil.add_handler(args.command.destroy, command_destroy, local_handlers=local_handlers)
126 |
127 |
128 | # This event handler is called when the user clicks the OK button in the command dialog or
129 | # is immediately called after the created event not command inputs were created for the dialog.
130 | def command_execute(args: adsk.core.CommandEventArgs):
131 | # General logging for debug.
132 | futil.log(f'{CMD_NAME} Command Execute Event')
133 |
134 | # get the command inputs value
135 | inputs = args.command.commandInputs
136 | sdf_input: adsk.core.DropDownCommandInput = inputs.itemById("robot_description_format")
137 | sim_env_input: adsk.core.DropDownCommandInput = inputs.itemById("simulation_env")
138 | name_input: adsk.core.StringValueCommandInput = inputs.itemById("SDF_Author_name")
139 | text_input: adsk.core.TextBoxCommandInput = inputs.itemById("SDF_Description")
140 | constants.set_rdf(sdf_input.selectedItem.name)
141 | constants.set_sim_env(sim_env_input.selectedItem.name)
142 | constants.set_author_name(name_input.value)
143 | constants.set_model_description(text_input.text)
144 |
145 | acdc4robot.run()
146 | # TODO ******************************** Your code here ********************************
147 |
148 | # Get a reference to your command's inputs.
149 | # inputs = args.command.commandInputs
150 | # text_box: adsk.core.TextBoxCommandInput = inputs.itemById('text_box')
151 | # value_input: adsk.core.ValueCommandInput = inputs.itemById('value_input')
152 |
153 | # # Do something interesting
154 | # text = text_box.text
155 | # expression = value_input.expression
156 | # msg = f'Your text: {text}
Your value: {expression}'
157 | # ui.messageBox(msg)
158 |
159 |
160 | # This event handler is called when the command needs to compute a new preview in the graphics window.
161 | def command_preview(args: adsk.core.CommandEventArgs):
162 | # General logging for debug.
163 | futil.log(f'{CMD_NAME} Command Preview Event')
164 | inputs = args.command.commandInputs
165 |
166 |
167 | # This event handler is called when the user changes anything in the command dialog
168 | # allowing you to modify values of other inputs based on that change.
169 | def command_input_changed(args: adsk.core.InputChangedEventArgs):
170 | changed_input = args.input
171 | command = changed_input.parentCommand
172 | inputs = command.commandInputs
173 | rdf: adsk.core.DropDownCommandInput = inputs.itemById("robot_description_format")
174 | sim_env: adsk.core.DropDownCommandInput = inputs.itemById("simulation_env")
175 | sim_env_items = sim_env.listItems
176 | sdf_author = inputs.itemById("SDF_Author_name")
177 | sdf_description = inputs.itemById("SDF_Description")
178 | # inputs = args.inputs
179 |
180 | if changed_input.id == "robot_description_format":
181 | if changed_input.selectedItem.name == "None":
182 | sim_env.isVisible = False
183 |
184 | elif changed_input.selectedItem.name == "URDF":
185 | sim_env.isVisible = True
186 |
187 | elif changed_input.selectedItem.name == "SDFormat":
188 | sim_env.isVisible = True
189 |
190 | elif changed_input.selectedItem.name == "MJCF":
191 | sim_env.isVisible = True
192 |
193 |
194 | if (rdf.selectedItem.name == "SDFormat") and (sim_env.selectedItem.name == "Gazebo"):
195 | sdf_author.isVisible = True
196 | sdf_description.isVisible = True
197 | else:
198 | sdf_author.isVisible = False
199 | sdf_description.isVisible = False
200 |
201 | # General logging for debug.
202 | futil.log(f'{CMD_NAME} Input Changed Event fired from a change to {changed_input.id}')
203 |
204 |
205 | # This event handler is called when the user interacts with any of the inputs in the dialog
206 | # which allows you to verify that all of the inputs are valid and enables the OK button.
207 | def command_validate_input(args: adsk.core.ValidateInputsEventArgs):
208 | # General logging for debug.
209 | futil.log(f'{CMD_NAME} Validate Input Event')
210 |
211 | inputs = args.inputs
212 |
213 | # Verify the validity of the input values. This controls if the OK button is enabled or not.
214 | valueInput = inputs.itemById('value_input')
215 | if valueInput.value >= 0:
216 | args.areInputsValid = True
217 | else:
218 | args.areInputsValid = False
219 |
220 |
221 | # This event handler is called when the command terminates.
222 | def command_destroy(args: adsk.core.CommandEventArgs):
223 | # General logging for debug.
224 | futil.log(f'{CMD_NAME} Command Destroy Event')
225 |
226 | global local_handlers
227 | local_handlers = []
228 |
--------------------------------------------------------------------------------
/Add-IN/ACDC4Robot/commands/ACDC4Robot/acdc4robot.py:
--------------------------------------------------------------------------------
1 | # -*- coding: utf-8 -*-
2 | # Author: nuofan
3 | """
4 | Export robot description format files from Fusion360 design
5 | """
6 |
7 | import adsk, adsk.core, adsk.fusion, traceback
8 | import os, sys
9 | from ...core.link import Link
10 | from ...core.joint import Joint
11 | from . import constants
12 | from ...core import write
13 | from ...core import utils
14 | from ...core.robot import Robot
15 | from ...core.urdf_plus import URDF_PLUS
16 | import time
17 |
18 | def get_link_joint_list(design: adsk.fusion.Design):
19 | """
20 | Get the link list and joint list to export
21 |
22 | Return:
23 | link_list: [Link]
24 | a list contains all the links that will be exported
25 | joint_list: [Joint]
26 | a list contains all the joint that will be exported
27 | """
28 | root = design.rootComponent
29 | link_list = []
30 | joint_list = []
31 | occs: adsk.fusion.OccurrenceList = root.allOccurrences
32 |
33 | # try to solve the nested components problem
34 | # but still not fully tested
35 | for occ in occs:
36 | # TODO: it seems use occ.joints.count will make it usable with occurrences? Test it
37 | if occ.component.joints.count > 0:
38 | # textPalette.writeText(str(occ.fullPathName))
39 | continue
40 | else:
41 | # Only occurrence contains zero joint and has zero childOccurrences
42 | # can be seen as a link
43 | if occ.childOccurrences.count > 0:
44 | # textPalette.writeText(str(occ.fullPathName))
45 | # textPalette.writeText(str(occ.childOccurrences.count))
46 | continue
47 | else:
48 | # textPalette.writeText(str(occ.fullPathName))
49 | # textPalette.writeText(str(occ.childOccurrences.count))
50 | if occ.isLightBulbOn:
51 | # only the occurrence light bulb on that the occurrence will be exported
52 | link_list.append(Link(occ)) # add link objects into link_list
53 |
54 | for joint in root.allJoints:
55 | joint_list.append(Joint(joint)) # add joint objects into joint_list
56 |
57 | return link_list, joint_list
58 |
59 | def export_stl(design: adsk.fusion.Design, save_dir: str, links: list[Link]):
60 | """
61 | export each component's stl file into "save_dir/mesh"
62 |
63 | Parameters
64 | ---------
65 | design: adsk.fusion.Design
66 | current active design
67 | save_dir: str
68 | the directory to store the export stl file
69 | """
70 | # create a single exportManager instance
71 | export_manager = design.exportManager
72 | # set the directory for the mesh file
73 | try: os.mkdir(save_dir + "/meshes")
74 | except: pass
75 | mesh_dir = save_dir + "/meshes"
76 |
77 | for link in links:
78 | visual_body: adsk.fusion.BRepBody = link.get_visual_body()
79 | col_body: adsk.fusion.BRepBody = link.get_collision_body()
80 | if (visual_body is None) and (col_body is None):
81 | # export the whole occurrence
82 | mesh_name = mesh_dir + "/" + link.get_name()
83 | occ = link.get_link_occ()
84 | # obj_export_options = export_manager.createOBJExportOptions(occ, mesh_name)
85 | # obj_export_options.unitType = adsk.fusion.DistanceUnits.MillimeterDistanceUnits # set unit to mm
86 | # obj_export_options.meshRefinement = adsk.fusion.MeshRefinementSettings.MeshRefinementLow
87 | # export_manager.execute(obj_export_options)
88 | stl_export_options = export_manager.createSTLExportOptions(occ, mesh_name)
89 | stl_export_options.sendToPrintUtility = False
90 | stl_export_options.isBinaryFormat = True
91 | stl_export_options.meshRefinement = adsk.fusion.MeshRefinementSettings.MeshRefinementLow
92 | export_manager.execute(stl_export_options)
93 | elif (visual_body is not None) and (col_body is not None):
94 | # export visual and collision geometry seperately
95 | visual_mesh_name = mesh_dir + "/" + link.get_name() + "_visual"
96 | visual_exp_options = export_manager.createSTLExportOptions(visual_body, visual_mesh_name)
97 | visual_exp_options.sendToPrintUtility = False
98 | visual_exp_options.isBinaryFormat = True
99 | visual_exp_options.meshRefinement = adsk.fusion.MeshRefinementSettings.MeshRefinementLow
100 | export_manager.execute(visual_exp_options)
101 |
102 | col_mesh_name = mesh_dir + "/" + link.get_name() + "_collision"
103 | col_exp_options = export_manager.createSTLExportOptions(col_body, col_mesh_name)
104 | col_exp_options.sendToPrintUtility = False
105 | col_exp_options.isBinaryFormat = True
106 | col_exp_options.meshRefinement = adsk.fusion.MeshRefinementSettings.MeshRefinementLow
107 | export_manager.execute(col_exp_options)
108 |
109 | elif (visual_body is None) and (col_body is not None):
110 | error_message = "Please set two bodies, one for visual and one for collision. \n"
111 | error_message = error_message + "Body for visual missing."
112 | utils.error_box(error_message)
113 | utils.terminate_box()
114 | elif (visual_body is not None) and (col_body is None):
115 | error_message = "Please set two bodies, one for visual and one for collision. \n"
116 | error_message = error_message + "Body for collision missing."
117 | utils.error_box(error_message)
118 | utils.terminate_box()
119 |
120 |
121 | def run():
122 | # Initialization
123 | app = adsk.core.Application.get()
124 | ui = app.userInterface
125 | product = app.activeProduct
126 | design = adsk.fusion.Design.cast(product)
127 |
128 | msg_box_title = "ACDC4Robot Message"
129 |
130 | # open a text palette for debuging
131 | textPalette = ui.palettes.itemById("TextCommands")
132 | if not textPalette.isVisible:
133 | textPalette.isVisible = True
134 | constants.set_text_palette(textPalette)
135 |
136 | try:
137 | # Set design type into do not capture design history
138 | design.designType = adsk.fusion.DesignTypes.DirectDesignType
139 |
140 | # # Check the length unit of Fusion360
141 | # if design.unitsManager.defaultLengthUnits != "m":
142 | # ui.messageBox("Please set length unit to 'm'!", msg_box_title)
143 | # return 0 # exit run() function
144 |
145 | root = design.rootComponent # get root component
146 | allComp = design.allComponents
147 | robot_name = root.name.split()[0]
148 | constants.set_robot_name(robot_name)
149 |
150 | # Set the folder to store exported files
151 | folder_dialog = ui.createFolderDialog()
152 | folder_dialog.title = "Chose your folder to export"
153 | dialog_result = folder_dialog.showDialog() # show folder dialog
154 | save_folder = ""
155 | if dialog_result == adsk.core.DialogResults.DialogOK:
156 | save_folder = folder_dialog.folder
157 | else:
158 | ui.messageBox("ACDC4Robot was canceled", msg_box_title)
159 | return 0 # exit run() function
160 |
161 | save_folder = save_folder + "/" + robot_name
162 | try: os.mkdir(save_folder)
163 | except: pass
164 |
165 | ui.messageBox("Start ACDC4Robot Add-IN", msg_box_title)
166 |
167 | # get all the link & joint elements to export
168 | link_list, joint_list = get_link_joint_list(design)
169 |
170 | rdf = constants.get_rdf()
171 | simulator = constants.get_sim_env()
172 |
173 | if rdf == None:
174 | ui.messageBox("Robot description format is None.\n" +
175 | "Please choose one robot description format", msg_box_title)
176 | elif rdf == "URDF":
177 | if simulator == "None":
178 | ui.messageBox("Simulation environment is None.\n" +
179 | "Please select a simulation environment.", msg_box_title)
180 | elif simulator == "Gazebo":
181 | # write to .urdf file
182 | write.write_urdf(link_list, joint_list, save_folder, robot_name)
183 | # export mesh files
184 | export_stl(design, save_folder, link_list)
185 | ui.messageBox("Finished exporting URDF for Gazebo.", msg_box_title)
186 |
187 | elif simulator == "PyBullet":
188 | # write to .urdf file
189 | write.write_urdf(link_list, joint_list, save_folder, robot_name)
190 | # export mesh files
191 | export_stl(design, save_folder, link_list)
192 | # generate pybullet script
193 | write.write_hello_pybullet(rdf, robot_name, save_folder)
194 | ui.messageBox("Finished exporting URDF for PyBullet.", msg_box_title)
195 |
196 | elif simulator == "MuJoCo":
197 | # write to .urdf file
198 | write.write_urdf(link_list, joint_list, save_folder, robot_name)
199 | # export mesh files
200 | export_stl(design, save_folder, link_list)
201 |
202 | ui.messageBox("Finished exporting URDF for MuJoCo.", msg_box_title)
203 |
204 | elif rdf == "SDFormat":
205 | if simulator == "None":
206 | ui.messageBox("Simulation environment is None.\n" +
207 | "Please select a simulation environment.", msg_box_title)
208 | elif simulator == "Gazebo":
209 | # write to .sdf file
210 | write.write_sdf(link_list, joint_list, save_folder, robot_name)
211 | # write a model cofig file
212 | author = constants.get_author_name()
213 | des = constants.get_model_description()
214 | write.write_sdf_config(save_folder, robot_name, author, des)
215 | # export stl files
216 | export_stl(design, save_folder, link_list)
217 | ui.messageBox("Finished exporting SDFormat for Gazebo.", msg_box_title)
218 | elif simulator == "PyBullet":
219 | # write to .sdf file
220 | write.write_sdf(link_list, joint_list, save_folder, robot_name)
221 | # export stl files
222 | export_stl(design, save_folder, link_list)
223 | # generate pybullet script
224 | write.write_hello_pybullet(rdf,robot_name, save_folder)
225 | ui.messageBox("Finished exporting SDFormat for PyBullet.", msg_box_title)
226 |
227 | elif simulator == "MuJoCo":
228 | ui.messageBox("MuJoCo does not support SDFormat. \n" +
229 | "Please select PyBullet or Gazebo as simulation environment.", msg_box_title)
230 |
231 | elif rdf == "MJCF":
232 | if simulator == "None":
233 | ui.messageBox("Simulation environment is None. \n" +
234 | "Please select a simulation environment.", msg_box_title)
235 | elif simulator == "Gazebo":
236 | ui.messageBox("Gazebo does not support MJCF. \n"+
237 | "Please select MuJoCo for simulation.", msg_box_title)
238 | elif simulator == "PyBullet":
239 | ui.messageBox("PyBullet does not support MJCF. \n" +
240 | "Please select MuJoCo for simulation.", msg_box_title)
241 | elif simulator == "MuJoCo":
242 | # write to .xml file
243 | write.write_mjcf(root, robot_name, save_folder)
244 | # export stl files
245 | export_stl(design, save_folder, link_list)
246 | time.sleep(0.1)
247 | ui.messageBox("Finished exporting MJCF for MuJoCo.", msg_box_title)
248 |
249 | elif rdf == "URDF+":
250 | robot = Robot(design)
251 | urdf_plus = URDF_PLUS(robot)
252 | urdf_plus_path = save_folder + "/{}.urdf".format(robot.get_robot_name())
253 | urdf_plus.write_file(urdf_plus_path)
254 | stl_list: list[Link] = robot.get_links()
255 | export_stl(design, save_folder, stl_list)
256 | time.sleep(0.1)
257 | ui.messageBox("Finished exporting URDF+.", msg_box_title)
258 |
259 | except:
260 | if ui:
261 | ui.messageBox('Failed:\n{}'.format(traceback.format_exc()))
262 |
--------------------------------------------------------------------------------
/Add-IN/ACDC4Robot/core/joint.py:
--------------------------------------------------------------------------------
1 | # -*- coding: utf-8 -*-
2 | """
3 | Get informations about joints from Fusion360 API
4 |
5 | """
6 |
7 | import adsk, adsk.fusion, adsk.core
8 | from xml.etree.ElementTree import ElementTree, Element, SubElement
9 | from . import utils
10 | from . import math_operation as math_op
11 | # from .link import Link
12 |
13 | class Joint():
14 | """
15 | Joint class for joint
16 | """
17 |
18 | def __init__(self, joint: adsk.fusion.Joint) -> None:
19 | self.joint = joint
20 | self.name = joint.name
21 | self.parent = joint.occurrenceTwo # parent link of joint
22 | self.child = joint.occurrenceOne
23 | # self.parent = Link(joint.occurrenceTwo) # parent link of joint
24 | # self.child = Link(joint.occurrenceOne)
25 |
26 | def get_name(self):
27 | """
28 | Return:
29 | name: str
30 | joint's full path name
31 | """
32 | # joint names inside each occurrence are identical
33 | # but joint names in different occurrences can be the same, in which makes conflict
34 | # parent_name = utils.get_valid_filename(self.parent.fullPathName) # inorder to fix joint's name confliction
35 | # name = parent_name + "_" + utils.get_valid_filename(self.name)
36 | # output name is consistent with the name in Fusion
37 | name = utils.get_valid_filename(self.name)
38 | return name
39 |
40 | def get_parent(self):
41 | """
42 | Return name of parent link
43 | """
44 | if self.parent.component.name == "base_link":
45 | parent_name = "base_link"
46 | else:
47 | parent_name = utils.get_valid_filename(self.parent.fullPathName)
48 |
49 | return parent_name
50 |
51 | def get_child(self):
52 | """
53 | Return name of child link
54 | """
55 | if self.child.component.name == "base_link":
56 | child_name = "base_link"
57 | else:
58 | child_name = utils.get_valid_filename(self.child.fullPathName)
59 |
60 | return child_name
61 |
62 | def get_sdf_joint_type(self) -> str:
63 | """
64 | Currently support following joint type:
65 | fixed, revolute, prismatic, continuous
66 |
67 | Return:
68 | joint_type: str
69 | fixed, revolute, prismatic
70 | """
71 | # currently, only support these three sdf joint type
72 | sdf_joint_type_list = ["fixed", "revolute", "prismatic"]
73 | if self.joint.jointMotion.jointType <= 2:
74 | sdf_joint_type = sdf_joint_type_list[self.joint.jointMotion.jointType]
75 | # # TODO:It seems continuous joint type has some problem with gazebo
76 | # if sdf_joint_type == "revolute" and (self.get_limits() is None):
77 | # sdf_joint_type = "continuous"
78 | else:
79 | pass
80 | return sdf_joint_type
81 |
82 | def get_mjcf_joint_type(self) -> str:
83 | """
84 | Return joint type for mjcf
85 | Return
86 | ---------
87 | joint_type: str
88 | """
89 | mjcf_joint_type_list = [None, "hinge", "slide"]
90 | if self.joint.jointMotion.jointType <= 2:
91 | mjcf_joint_type = mjcf_joint_type_list[self.joint.jointMotion.jointType]
92 | else:
93 | pass
94 | return mjcf_joint_type
95 |
96 | def get_urdf_joint_type(self) -> str:
97 | urdf_joint_type_list = ["fixed", "revolute", "prismatic"]
98 | if self.joint.jointMotion.jointType <= 2:
99 | urdf_joint_type = urdf_joint_type_list[self.joint.jointMotion.jointType]
100 | if urdf_joint_type == "revolute" and (self.get_limits() is None):
101 | urdf_joint_type = "continuous"
102 | else:
103 | # other joint types are not supported yet
104 | pass
105 | return urdf_joint_type
106 |
107 | def get_limits(self):
108 | """
109 | Return joint limits list: [lower_limit, upper_limit]
110 | or None
111 | """
112 | if self.joint.jointMotion.jointType == 0: # RigidJointType
113 | return None
114 | elif self.joint.jointMotion.jointType == 1: # RevoluteJointType
115 | max_enabled = self.joint.jointMotion.rotationLimits.isMaximumValueEnabled
116 | min_enabled = self.joint.jointMotion.rotationLimits.isMinimumValueEnabled
117 | if max_enabled and min_enabled:
118 | # unit: radians
119 | upper_limit = self.joint.jointMotion.rotationLimits.maximumValue
120 | lower_limit = self.joint.jointMotion.rotationLimits.minimumValue
121 | return [round(lower_limit, 6), round(upper_limit, 6)]
122 | else:
123 | return None
124 | elif self.joint.jointMotion.jointType == 2: # SliderJointType
125 | max_enabled = self.joint.jointMotion.slideLimits.isMaximumValueEnabled
126 | min_enabled = self.joint.jointMotion.slideLimits.isMinimumValueEnabled
127 | if max_enabled and min_enabled:
128 | upper_limit = self.joint.jointMotion.slideLimits.maximumValue * 0.01 # cm -> m
129 | lower_limit = self.joint.jointMotion.slideLimits.minimumValue * 0.01 # cm -> m
130 | return [round(lower_limit, 6), round(upper_limit, 6)]
131 | else:
132 | return None
133 |
134 | def get_sdf_origin(self):
135 | """
136 | From: http://sdformat.org/tutorials?tut=spec_model_kinematics&cat=specification&#jointpose
137 | For a joint with parent link frame `P` and child link frame `C`,
138 | the joint `` tag specifies the pose `X_CJc` of a joint frame `Jc` rigidly attached to the child link.
139 | """
140 | # I think the joint frame is defined by the parent joint origin in Fusion360
141 | # Because in Fusion360, a joint motion is defined by the axis in parent joint orign
142 | # and use offset to define the location of child joint origin w.r.t parent joint
143 | # I guess using parent joint origin would solve the offset problem?
144 | # I guess using child joint origin works just because they coincide together for all the text examples
145 |
146 | # get parent joint origin as the child joint
147 | if self.joint.geometryOrOriginTwo == adsk.fusion.JointOrigin:
148 | w_P_Jc = self.joint.geometryOrOriginTwo.geometry.origin.asArray()
149 | else:
150 | w_P_Jc = self.joint.geometryOrOriginTwo.origin.asArray()
151 |
152 | # convert from cm to m
153 | w_P_Jc = [round(i*0.01, 6) for i in w_P_Jc]
154 | # get child link frame's origin point w.r.t world frame
155 | w_P_Lc = [self.child.transform2.translation.x * 0.01,
156 | self.child.transform2.translation.y * 0.01,
157 | self.child.transform2.translation.z * 0.01,]
158 | # vector from child link frame's origin point to child joint origin point w.r.t world frame
159 | w_V_LcJc = [[w_P_Jc[0]-w_P_Lc[0]],
160 | [w_P_Jc[1]-w_P_Lc[1]],
161 | [w_P_Jc[2]-w_P_Lc[2]]] # 3*1 vector
162 |
163 | w_T_Lc = self.child.transform2 # configuration of child-link-frame w.r.t world-frame w
164 | w_R_Lc = math_op.get_rotation_matrix(w_T_Lc) # rotation matrix of child-link-frame w.r.t world-frame w
165 | Lc_R_w = math_op.matrix_transpose(w_R_Lc) # rotation matrix of world-frame w.r.t child-link-frame Lc
166 | # vector from child link frame's origin point to child joint origin point w.r.t child-link-frame Lc
167 | Lc_V_LcJc = math_op.change_orientation(Lc_R_w, w_V_LcJc) # 3*1 array
168 | # assume the joint frame has the same oritation as child link frame
169 | # it seems that rpy of joint doesn't matter, so set them as 0
170 | sdf_origin = [Lc_V_LcJc[0][0], Lc_V_LcJc[1][0], Lc_V_LcJc[2][0], 0.0, 0.0, 0.0]
171 |
172 | return sdf_origin
173 |
174 | def get_joint_frame(self) -> adsk.core.Matrix3D:
175 | """
176 | Get the joint frame whose origin coincides with parent joint origin,
177 | and has same orientation with parent joint origin frame
178 |
179 | Return:
180 | joint_frame: adsk.core.Matrix3D
181 | a homogeneous matrix represents joint frame J in world frame W
182 | translation unit: cm
183 | """
184 | # get parent joint origin's coordinate w.r.t world frame
185 | if self.joint.geometryOrOriginTwo == adsk.fusion.JointOrigin:
186 | w_P_J = self.joint.geometryOrOriginTwo.geometry.origin.asArray()
187 | else:
188 | w_P_J = self.joint.geometryOrOriginTwo.origin.asArray()
189 |
190 | w_P_J = [round(i, 6) for i in w_P_J]
191 |
192 | # no matter jointGeometry or jointOrigin object, both have these properties
193 | zAxis: adsk.core.Vector3D = self.joint.geometryOrOriginTwo.primaryAxisVector
194 | xAxis: adsk.core.Vector3D = self.joint.geometryOrOriginTwo.secondaryAxisVector
195 | yAxis: adsk.core.Vector3D = self.joint.geometryOrOriginTwo.thirdAxisVector
196 |
197 | origin = adsk.core.Point3D.create(w_P_J[0], w_P_J[1], w_P_J[2])
198 |
199 | joint_frame = adsk.core.Matrix3D.create()
200 | joint_frame.setWithCoordinateSystem(origin, xAxis, yAxis, zAxis)
201 |
202 | return joint_frame
203 |
204 | def get_urdf_origin(self):
205 | """
206 | Get joint origin, which is the transform from the parent frame to this joint
207 | """
208 | parent_link = self.parent
209 |
210 | # get parent_frame w.r.t world frame
211 | def get_parent_joint(link: adsk.fusion.Occurrence) -> adsk.fusion.Joint:
212 | joint_list: adsk.fusion.JointList = link.joints
213 | for j in joint_list:
214 | if j.occurrenceOne == link:
215 | return j
216 | else:
217 | continue
218 | return None
219 |
220 | parent_joint: adsk.fusion.Joint = get_parent_joint(parent_link)
221 | if parent_joint is None:
222 | # if the parent link does not have parent joint(which means the root link),
223 | # then the parent link frame is the parent frame
224 | parent_frame: adsk.core.Matrix3D = parent_link.transform2
225 | else:
226 | if parent_joint.geometryOrOriginTwo == adsk.fusion.JointOrigin:
227 | w_P_J = parent_joint.geometryOrOriginTwo.geometry.origin.asArray()
228 | else:
229 | w_P_J = parent_joint.geometryOrOriginTwo.origin.asArray()
230 |
231 | w_P_J = [round(i, 6) for i in w_P_J]
232 |
233 | # no matter jointGeometry or jointOrigin object, both have these properties
234 | zAxis: adsk.core.Vector3D = parent_joint.geometryOrOriginTwo.primaryAxisVector
235 | xAxis: adsk.core.Vector3D = parent_joint.geometryOrOriginTwo.secondaryAxisVector
236 | yAxis: adsk.core.Vector3D = parent_joint.geometryOrOriginTwo.thirdAxisVector
237 |
238 | origin = adsk.core.Point3D.create(w_P_J[0], w_P_J[1], w_P_J[2])
239 |
240 | parent_frame = adsk.core.Matrix3D.create()
241 | parent_frame.setWithCoordinateSystem(origin, xAxis, yAxis, zAxis)
242 |
243 |
244 | # get joint frame w.r.t world frame
245 | joint_frame = self.get_joint_frame()
246 |
247 | # from_origin, from_xAxis, from_yAxis, from_zAxis = parent_frame.getAsCoordinateSystem()
248 | # to_origin, to_xAsix, to_yAxis, to_zAxis = joint_frame.getAsCoordinateSystem()
249 |
250 | transform = adsk.core.Matrix3D.create()
251 | # transform.setToAlignCoordinateSystems(from_origin, from_xAxis, from_yAxis, from_zAxis,
252 | # to_origin, to_xAsix, to_yAxis, to_zAxis)
253 | transform = math_op.coordinate_transform(parent_frame, joint_frame)
254 |
255 | joint_origin = math_op.matrix3d_2_pose(transform)
256 |
257 | return joint_origin
258 |
259 | def get_axes(self):
260 | """
261 | Return:
262 | ---------
263 | axis1: list or None
264 | list: [x1, y1, z1]
265 | axis2: list or None
266 | list: [x2, y2, z2]
267 | """
268 | # Fusion360 api returns joint axis w.r.t world frame
269 | if self.joint.jointMotion.jointType == 0: # RigidJointType
270 | axis1 = None
271 | axis2 = None
272 | return axis1, axis2
273 | elif self.joint.jointMotion.jointType == 1: # RevoluteJointType
274 | axis1 = [round(i, 6) for i in self.joint.jointMotion.rotationAxisVector.asArray()] # In Fusion360, returned axis is normalized
275 | axis2 = None
276 | return axis1, axis2
277 | elif self.joint.jointMotion.jointType == 2: # SliderJointType
278 | axis1 = [round(i, 6) for i in self.joint.jointMotion.slideDirectionVector.asArray()] # In Fusion360, returned axis is normalized
279 | axis2 = None
280 | return axis1, axis2
281 | elif self.joint.jointMotion.jointType == 3: # CylindricalJointType
282 | axis1 = [round(i, 6) for i in self.joint.jointMotion.rotationAxisVector.asArray()] # In Fusion360, returned axis is normalized
283 | axis2 = None
284 | return axis1, axis2
285 | elif self.joint.jointMotion.jointType == 4: # PinSlotJointType
286 | axis1 = [round(i, 6) for i in self.joint.jointMotion.rotationAxisVector.asArray()] # rotation axis
287 | axis2 = [round(i, 6) for i in self.joint.jointMotion.slideDirectionVector.asArray()] # slide axis
288 | return axis1, axis2
289 | elif self.joint.jointMotion.jointType == 5: # PlanarJointType
290 | axis1 = [round(i, 6) for i in self.joint.jointMotion.primarySlideDirectionVector.asArray()] # rotation axis
291 | axis2 = [round(i, 6) for i in self.joint.jointMotion.secondarySlideDirectionVector.asArray()] # slide axis
292 | elif self.joint.jointMotion.jointType == 6: # BallJointType
293 | pass
294 |
295 | def get_axes_urdf(self):
296 | """
297 | Return
298 | ---------
299 | axis1: list or None
300 | list: [x1, y1, z1], w.r.t joint-frame J
301 | axis2: list or None
302 | list: [x2, y2, z2], w.r.t joint-frame J
303 | """
304 | # both axes are expressed w.r.t world-frame w
305 | w_axis1, w_axis2 = self.get_axes()
306 | J_axis1, J_axis2 = None, None
307 | joint_frame: adsk.core.Matrix3D = self.get_joint_frame()
308 | w_R_J = math_op.get_rotation_matrix(joint_frame) # represent joint-frame J's orientation w.r.t world-frame w
309 | J_R_w = math_op.matrix_transpose(w_R_J)
310 | if w_axis1 is not None:
311 | w_axis1 = [[w_axis1[0]], [w_axis1[1]], [w_axis1[2]]] # from 1*3 to 3*1 list
312 | J_axis1 = math_op.change_orientation(J_R_w, w_axis1)
313 | J_axis1 = [J_axis1[0][0], J_axis1[1][0], J_axis1[2][0]]
314 | if w_axis2 is not None:
315 | w_axis2 = [[w_axis2[0]], [w_axis2[1]], [w_axis2[2]]]
316 | J_axis2 = math_op.change_orientation(J_R_w, w_axis2)
317 | J_axis2 = [J_axis2[0][0], J_axis2[1][0], J_axis2[2][0]]
318 |
319 | return J_axis1, J_axis2
320 |
321 | def get_axis_mjcf(self):
322 | """
323 | I guess the reference frame of the axis is the body contains the joint element,
324 | which is the child link of the joint
325 |
326 | Return:
327 | axis: list or None
328 | """
329 | w_axis1, _ = self.get_axes()
330 | C_axis = None
331 | child_link_frame: adsk.core.Matrix3D = self.child.transform2
332 | w_R_C = math_op.get_rotation_matrix(child_link_frame)
333 | C_R_w = math_op.matrix_transpose(w_R_C)
334 | if w_axis1 is not None:
335 | w_axis1 = [[w_axis1[0]], [w_axis1[1]], [w_axis1[2]]] # from 1*3 to 3*1 list
336 | C_axis = math_op.change_orientation(C_R_w, w_axis1)
337 | C_axis = [C_axis[0][0], C_axis[1][0], C_axis[2][0]]
338 |
339 | return C_axis
340 |
--------------------------------------------------------------------------------
/Add-IN/ACDC4Robot/core/sdf.py:
--------------------------------------------------------------------------------
1 | # -*- coding: utf-8 -*-
2 | # Author: Nuofan
3 | # 20230811
4 | # Contains functions for generating urdf
5 | import adsk, adsk.core, adsk.fusion
6 | from .link import Link
7 | from .joint import Joint
8 | from xml.etree.ElementTree import Element, SubElement
9 | from . import math_operation as math_op
10 | from . import utils
11 | from ..commands.ACDC4Robot import constants
12 |
13 | def get_link_name(link: Link) -> str:
14 | """
15 | Return
16 | ---------
17 | link_name: str
18 | link's full path name
19 | """
20 | name = link.get_name()
21 | return name
22 |
23 | def get_link_pose(link: Link) -> list[float]:
24 | """
25 | Return
26 | ---------
27 | link_pose: [x, y, z, roll, pitch, yaw]
28 | Without @relative_to setting, link pose is expressed in parent element frame,
29 | which is the model frame (world frame in Fusion360) by default
30 | """
31 | link_pose = math_op.matrix3d_2_pose(link.pose)
32 | return link_pose
33 |
34 | def get_link_mass(link: Link) -> float:
35 | """
36 | Return
37 | ---------
38 | mass: float
39 | unit: kg
40 | """
41 | mass = link.phyPro.mass
42 | return mass
43 |
44 | def get_link_CoM(link: Link) -> list[float]:
45 | """
46 | Return
47 | ---------
48 | CoM: [x, y, z, roll, pitch, yaw]
49 | Center of mass frame C relative to the link frame L
50 | unit: m, radian
51 | """
52 | # Let the orientation of center-of-mass frame C is same as link-frame L
53 | roll = 0.0
54 | pitch = 0.0
55 | yaw = 0.0
56 | w_CoM_x = link.phyPro.centerOfMass.x # point CoM's x coordinate w.r.t world-frame
57 | w_CoM_y = link.phyPro.centerOfMass.y
58 | w_CoM_z = link.phyPro.centerOfMass.z
59 | w_Lo_x = link.pose.translation.x # link-frame's origin point's x coordinat w.r.t world-frame
60 | w_Lo_y = link.pose.translation.y
61 | w_Lo_z = link.pose.translation.z
62 | # represent vector Lo-CoM which start from link-fram's origin point to CoM point w.r.t world-frame
63 | w_Lo_CoM = [[(w_CoM_x-w_Lo_x)*0.01], [(w_CoM_y-w_Lo_y)*0.01], [(w_CoM_z-w_Lo_z)*0.01]] # cm -> m
64 | # represent world-frame's orientation w.r.t link-frame
65 | L_R_w = [[link.pose.getCell(0, 0), link.pose.getCell(1, 0), link.pose.getCell(2, 0)],
66 | [link.pose.getCell(0, 1), link.pose.getCell(1, 1), link.pose.getCell(2, 1)],
67 | [link.pose.getCell(0, 2), link.pose.getCell(1, 2), link.pose.getCell(2, 2)]]
68 |
69 | L_Lo_CoM = math_op.change_orientation(L_R_w, w_Lo_CoM)
70 | CoM = [L_Lo_CoM[0][0], L_Lo_CoM[1][0], L_Lo_CoM[2][0], roll, pitch, yaw]
71 |
72 | return CoM
73 |
74 | def get_link_inertia(link: Link) -> list[float]:
75 | """
76 | Return
77 | ---------
78 | inertia_list: [ixx, iyy, izz, ixy, iyz, ixz]
79 | Inertia tensor elements w.r.t CoM frame, which has the same orientation with the link frame L
80 | unit: kg*m^2
81 | """
82 | (_, w_ixx, w_iyy, w_izz, w_ixy, w_iyz, w_ixz) = link.phyPro.getXYZMomentsOfInertia() # unit: kg*cm^2
83 | # Use parallel axis theorem to change inertia from w.r.t world-frame's origin to w.r.t CoM
84 | x = link.phyPro.centerOfMass.x * 0.01 # get the x coordinate w.r.t world-frame, unit: m
85 | y = link.phyPro.centerOfMass.y * 0.01
86 | z = link.phyPro.centerOfMass.z * 0.01
87 | mass = link.phyPro.mass # unit: kg
88 | com_ixx = w_ixx*0.0001 - mass*(y**2+z**2) # kg*cm^2 -> kg*m^2
89 | com_iyy = w_iyy*0.0001 - mass*(x**2+z**2)
90 | com_izz = w_izz*0.0001 - mass*(x**2+y**2)
91 | com_ixy = w_ixy*0.0001 + mass*(x*y)
92 | com_iyz = w_iyz*0.0001 + mass*(y*z)
93 | com_ixz = w_ixz*0.0001 + mass*(x*z)
94 |
95 | # Change inertia tensor to the orientation of link-frame L
96 | # reference: https://robot.sia.cn/CN/abstract/abstract374.shtml
97 | R = math_op.get_rotation_matrix(link.pose)
98 | R_T = math_op.matrix_transpose(R)
99 | inertia_tensor = [[com_ixx, com_ixy, com_ixz],
100 | [com_ixy, com_iyy, com_iyz],
101 | [com_ixz, com_iyz, com_izz]]
102 |
103 | # moments of inertia has the same orientation of L-frame
104 | I = math_op.matrix_multi(math_op.matrix_multi(R_T, inertia_tensor), R)
105 | # I = math_op.matrix_multi(math_op.matrix_multi(R, inertia_tensor), R_T)
106 | inertia_list = [I[0][0], I[1][1], I[2][2], I[0][1], I[1][2], I[0][2]]
107 |
108 | return inertia_list # unit: kg*m^2
109 |
110 | def get_link_visual_name(link: Link) -> str:
111 | """
112 | Return
113 | ---------
114 | visual_name: str
115 | """
116 | visual_name = link.get_name() + "_visual"
117 | return visual_name
118 |
119 | def get_link_collision_name(link: Link) -> str:
120 | """
121 | Return:
122 | collision_name: str
123 | """
124 | collision_name = link.get_name() + "_collision"
125 | return collision_name
126 |
127 | def get_link_mesh_origin(link: Link) -> list[float]:
128 | """
129 | Return:
130 | mesh_origin: [x, y, z, roll, pitch, yaw]
131 | unit: m, radian
132 | """
133 | collision_name = link.get_name() + "_collision"
134 | return collision_name
135 |
136 | def get_link_visual_geo(link: Link) -> str:
137 | """
138 | A trimesh file for visual geometry
139 | Return:
140 | ---------
141 | mesh_loc: str
142 | the path to find mesh file
143 | """
144 | robot_name = constants.get_robot_name()
145 | visual_body = link.get_visual_body()
146 | col_body = link.get_collision_body()
147 | if (visual_body is None) and (col_body is None):
148 | # visual and collision geometry is same
149 | mesh_loc = "model://" + robot_name + "/meshes/" + link.get_name() + ".stl"
150 | return mesh_loc
151 | elif (visual_body is not None) and (col_body is not None):
152 | mesh_loc = "model://" + robot_name + "/meshes/" + link.get_name() + "_visual.stl"
153 | return mesh_loc
154 | elif (visual_body is None) and (col_body is not None):
155 | error_message = "Please set two bodies, one for visual and one for collision. \n"
156 | error_message = error_message + link.get_name() + " body for visual missing."
157 | utils.error_box(error_message)
158 | utils.terminate_box()
159 | elif (visual_body is not None) and (col_body is None):
160 | error_message = "Please set two bodies, one for visual and one for collision. \n"
161 | error_message = error_message + link.get_name() + " body for collision missing."
162 | utils.error_box(error_message)
163 | utils.terminate_box()
164 |
165 | def get_link_collision_geo(link: Link) -> str:
166 | """
167 | A trimesh file for collision geometry
168 | Return:
169 | ---------
170 | mesh_loc: str
171 | the path to find mesh file
172 | """
173 | robot_name = constants.get_robot_name()
174 | visual_body = link.get_visual_body()
175 | col_body = link.get_collision_body()
176 | if (visual_body is None) and (col_body is None):
177 | # visual and collision geometry is same
178 | mesh_loc = "model://" + robot_name + "/meshes/" + link.get_name() + ".stl"
179 | return mesh_loc
180 | elif (visual_body is not None) and (col_body is not None):
181 | mesh_loc = "model://" + robot_name + "/meshes/" + link.get_name() + "_collision.stl"
182 | return mesh_loc
183 | elif (visual_body is None) and (col_body is not None):
184 | error_message = "Please set two bodies, one for visual and one for collision. \n"
185 | error_message = error_message + link.get_name() + " body for visual missing."
186 | utils.error_box(error_message)
187 | utils.terminate_box()
188 | elif (visual_body is not None) and (col_body is None):
189 | error_message = "Please set two bodies, one for visual and one for collision. \n"
190 | error_message = error_message + link.get_name() + " body for collision missing."
191 | utils.error_box(error_message)
192 | utils.terminate_box()
193 |
194 | def get_link_element(link: Link) -> Element:
195 | """
196 | Return
197 | ---------
198 | link_ele: Element
199 | xml elements contains informations for link
200 | """
201 | link_ele = Element("link")
202 | link_ele.attrib = {"name": get_link_name(link)}
203 |
204 | # pose
205 | pose = SubElement(link_ele, "pose")
206 | pose.text = "{} {} {} {} {} {}".format(get_link_pose(link)[0], get_link_pose(link)[1],
207 | get_link_pose(link)[2], get_link_pose(link)[3],
208 | get_link_pose(link)[4], get_link_pose(link)[5])
209 |
210 | # inertial
211 | inertial = SubElement(link_ele, "inertial")
212 | mass = SubElement(inertial, "mass")
213 | mass.text = str(get_link_mass(link))
214 | CoM = SubElement(inertial, "pose")
215 | CoM.text = "{} {} {} {} {} {}".format(get_link_CoM(link)[0], get_link_CoM(link)[1],
216 | get_link_CoM(link)[2], get_link_CoM(link)[3],
217 | get_link_CoM(link)[4], get_link_CoM(link)[5])
218 | inertia = SubElement(inertial, "inertia")
219 | ixx = SubElement(inertia, "ixx")
220 | iyy = SubElement(inertia, "iyy")
221 | izz = SubElement(inertia, "izz")
222 | ixy = SubElement(inertia, "ixy")
223 | iyz = SubElement(inertia, "iyz")
224 | ixz = SubElement(inertia, "ixz")
225 | ixx.text = "{}".format(get_link_inertia(link)[0])
226 | iyy.text = "{}".format(get_link_inertia(link)[1])
227 | izz.text = "{}".format(get_link_inertia(link)[2])
228 | ixy.text = "{}".format(get_link_inertia(link)[3])
229 | iyz.text = "{}".format(get_link_inertia(link)[4])
230 | ixz.text = "{}".format(get_link_inertia(link)[5])
231 |
232 | # visual
233 | visual = SubElement(link_ele, "visual")
234 | visual_name = get_link_visual_name(link)
235 | visual_uri = get_link_visual_geo(link)
236 | visual.attrib = {"name": visual_name}
237 | vis_geo = SubElement(visual, "geometry")
238 | vis_geo_mesh = SubElement(vis_geo, "mesh")
239 | vis_geo_mesh_uri = SubElement(vis_geo_mesh, "uri")
240 | vis_geo_mesh_uri.text = visual_uri
241 | vis_geo_mesh_scale = SubElement(vis_geo_mesh, "scale")
242 | vis_geo_mesh_scale.text = "0.001 0.001 0.001"
243 |
244 | # collision
245 | collision = SubElement(link_ele, "collision")
246 | collision_name = get_link_collision_name(link)
247 | collision_uri = get_link_collision_geo(link)
248 | collision.attrib = {"name": collision_name}
249 | col_geo = SubElement(collision, "geometry")
250 | col_geo_mesh = SubElement(col_geo, "mesh")
251 | col_geo_mesh_uri = SubElement(col_geo_mesh, "uri")
252 | col_geo_mesh_uri.text = collision_uri
253 | col_geo_mesh_scale = SubElement(col_geo_mesh, "scale")
254 | col_geo_mesh_scale.text = "0.001 0.001 0.001"
255 |
256 | return link_ele
257 |
258 | def get_joint_name(joint: Joint) -> str:
259 | """
260 | Return
261 | ---------
262 | joint_name: str
263 | """
264 | joint_name = joint.get_name()
265 | return joint_name
266 |
267 | def get_joint_type(joint: Joint) -> str:
268 | """
269 | Return
270 | ---------
271 | joint_type: str
272 | fixed, revolute, prismatic, continuous
273 | currently, only support urdf joint types above
274 | """
275 | # currently, only support these three sdf joint type
276 | sdf_joint_type_list = ["fixed", "revolute", "prismatic"]
277 | if joint.joint.jointMotion.jointType <= 2:
278 | sdf_joint_type = sdf_joint_type_list[joint.joint.jointMotion.jointType]
279 | # # TODO:It seems continuous joint type has some problem with gazebo
280 | # if sdf_joint_type == "revolute" and (self.get_limits() is None):
281 | # sdf_joint_type = "continuous"
282 | else:
283 | pass
284 | return sdf_joint_type
285 |
286 | def get_joint_pose(joint: Joint) -> list[float]:
287 | """
288 | Return
289 | ---------
290 | joint_pose: [x, y, z, roll, pitch, yaw]
291 | From: http://sdformat.org/tutorials?tut=spec_model_kinematics&cat=specification&#jointpose
292 | For a joint with parent link frame `P` and child link frame `C`,
293 | the joint `` tag specifies the pose `X_CJc` of a joint frame `Jc` rigidly attached to the child link.
294 | """
295 | # get parent joint origin's coordinate w.r.t world frame
296 | # get parent joint origin as the child joint frame
297 | # NOTE: joint origin is a concept in Fusion360
298 | if joint.joint.geometryOrOriginTwo == adsk.fusion.JointOrigin:
299 | w_P_Jc = joint.joint.geometryOrOriginTwo.geometry.origin.asArray()
300 | else:
301 | w_P_Jc = joint.joint.geometryOrOriginTwo.origin.asArray()
302 |
303 | # convert from cm to m
304 | w_P_Jc = [round(i*0.01, 6) for i in w_P_Jc]
305 | # get child link frame's origin point w.r.t world frame
306 | w_P_Lc = [joint.child.transform2.translation.x * 0.01,
307 | joint.child.transform2.translation.y * 0.01,
308 | joint.child.transform2.translation.z * 0.01,]
309 | # vector from child link frame's origin point to child joint origin point w.r.t world frame
310 | w_V_LcJc = [[w_P_Jc[0]-w_P_Lc[0]],
311 | [w_P_Jc[1]-w_P_Lc[1]],
312 | [w_P_Jc[2]-w_P_Lc[2]]] # 3*1 vector
313 |
314 | w_T_Lc = joint.child.transform2 # configuration of child-link-frame w.r.t world-frame w
315 | w_R_Lc = math_op.get_rotation_matrix(w_T_Lc) # rotation matrix of child-link-frame w.r.t world-frame w
316 | Lc_R_w = math_op.matrix_transpose(w_R_Lc) # rotation matrix of world-frame w.r.t child-link-frame Lc
317 | # vector from child link frame's origin point to child joint origin point w.r.t child-link-frame Lc
318 | Lc_V_LcJc = math_op.change_orientation(Lc_R_w, w_V_LcJc) # 3*1 array
319 | # assume the joint frame has the same oritation as child link frame
320 | # it seems that rpy of joint doesn't matter, so set them as 0
321 | sdf_origin = [Lc_V_LcJc[0][0], Lc_V_LcJc[1][0], Lc_V_LcJc[2][0], 0.0, 0.0, 0.0]
322 |
323 | return sdf_origin
324 |
325 | def get_joint_parent(joint: Joint) -> Link:
326 | """
327 | Return
328 | ---------
329 | parent_link: Link
330 | parent link of the joint
331 | """
332 | joint_parent: adsk.fusion.Occurrence = joint.parent
333 | parent_link: Link = Link(joint_parent)
334 | return parent_link
335 |
336 | def get_joint_child(joint: Joint) -> Link:
337 | """
338 | Return
339 | ---------
340 | child_link: Link
341 | child link of the joint
342 | """
343 | joint_child: adsk.fusion.Occurrence = joint.child
344 | child_link: Link = Link(joint_child)
345 | return child_link
346 |
347 | def get_joint_axis(joint: Joint) -> list[float]:
348 | """
349 | Return
350 | ---------
351 | joint_axis: [x, y, z]
352 | joint axis w.r.t world frame
353 | if joint type is fixed, return None
354 | """
355 | # Fusion360 api returns joint axis w.r.t world frame
356 | if joint.joint.jointMotion.jointType == 0: # RigidJointType
357 | axis = None
358 | return axis
359 | elif joint.joint.jointMotion.jointType == 1: # RevoluteJointType
360 | axis = [round(i, 6) for i in joint.joint.jointMotion.rotationAxisVector.asArray()] # In Fusion360, returned axis is normalized
361 | return axis
362 | elif joint.joint.jointMotion.jointType == 2: # SliderJointType
363 | axis = [round(i, 6) for i in joint.joint.jointMotion.slideDirectionVector.asArray()] # In Fusion360, returned axis is normalized
364 | return axis
365 |
366 | def get_joint_axis2(joint: Joint) -> list[float]:
367 | """
368 | Return
369 | ---------
370 | joint_axis:2 [x, y, z]
371 | """
372 | pass
373 |
374 | def get_joint_limit(joint: Joint) -> list[float]:
375 | """
376 | Return
377 | ---------
378 | joint_limit: [lower, upper]
379 | """
380 | if joint.joint.jointMotion.jointType == 0: # RigidJointType
381 | return None
382 | elif joint.joint.jointMotion.jointType == 1: # RevoluteJointType
383 | max_enabled = joint.joint.jointMotion.rotationLimits.isMaximumValueEnabled
384 | min_enabled = joint.joint.jointMotion.rotationLimits.isMinimumValueEnabled
385 | if max_enabled and min_enabled:
386 | # unit: radians
387 | lower_limit = joint.joint.jointMotion.rotationLimits.minimumValue
388 | upper_limit = joint.joint.jointMotion.rotationLimits.maximumValue
389 | return [round(lower_limit, 6), round(upper_limit, 6)]
390 | else:
391 | return None
392 | elif joint.joint.jointMotion.jointType == 2: # SliderJointType
393 | max_enabled = joint.joint.jointMotion.slideLimits.isMaximumValueEnabled
394 | min_enabled = joint.joint.jointMotion.slideLimits.isMinimumValueEnabled
395 | if max_enabled and min_enabled:
396 | lower_limit = joint.joint.jointMotion.slideLimits.minimumValue * 0.01 # cm -> m
397 | upper_limit = joint.joint.jointMotion.slideLimits.maximumValue * 0.01 # cm -> m
398 | return [round(lower_limit, 6), round(upper_limit, 6)]
399 | else:
400 | return None
401 |
402 | def get_joint_limit2(joint: Joint) -> list[float]:
403 | """
404 | Return
405 | ---------
406 | joint_limit2: [lower, upper]
407 | """
408 | pass
409 |
410 | def get_joint_element(joint: Joint) -> list[float]:
411 | """
412 | Return
413 | ---------
414 | joint_element: Element
415 | xml elements contains informations for joint
416 | """
417 | joint_ele = Element("joint")
418 | joint_ele.attrib = {"name": get_joint_name(joint), "type": get_joint_type(joint)}
419 | pose = SubElement(joint_ele, "pose")
420 | pose.text = "{} {} {} {} {} {}".format(get_joint_pose(joint)[0], get_joint_pose(joint)[1],
421 | get_joint_pose(joint)[2], get_joint_pose(joint)[3],
422 | get_joint_pose(joint)[4], get_joint_pose(joint)[5])
423 | parent = SubElement(joint_ele, "parent")
424 | parent.text = "{}".format(get_joint_parent(joint).get_name())
425 | child = SubElement(joint_ele, "child")
426 | child.text = "{}".format(get_joint_child(joint).get_name())
427 | axis1 = get_joint_axis(joint)
428 | axis2 = get_joint_axis2(joint)
429 | if axis1 is not None:
430 | axis1_ele = SubElement(joint_ele, "axis")
431 | axis1_xyz = SubElement(axis1_ele, "xyz")
432 | # Fusion360 api returns joint axis w.r.t world frame,
433 | # which is the model frame here
434 | axis1_xyz.attrib = {"expressed_in": "__model__"}
435 | axis1_xyz.text = "{} {} {}".format(axis1[0], axis1[1], axis1[2])
436 | joint_limit1 = get_joint_limit(joint)
437 | if joint_limit1 is not None:
438 | limit = SubElement(axis1_ele, "limit")
439 | lower = SubElement(limit, "lower")
440 | lower.text = str(joint_limit1[0])
441 | upper = SubElement(limit, "upper")
442 | upper.text = str(joint_limit1[1])
443 | if axis2 is not None:
444 | pass
445 |
446 | return joint_ele
--------------------------------------------------------------------------------
/Add-IN/ACDC4Robot/core/link.py:
--------------------------------------------------------------------------------
1 | # -*- coding: utf-8 -*-
2 | """
3 | Get informations about links which are components from Fusion360 API
4 |
5 | """
6 | import adsk, adsk.fusion, adsk.core
7 | from xml.etree.ElementTree import ElementTree, Element, SubElement
8 | from ..commands.ACDC4Robot import constants
9 | from . import utils
10 | from .joint import Joint
11 | from . import math_operation as math_op
12 |
13 | # TODO: support for dependent component
14 | # TODO: support for external component
15 |
16 | class Link():
17 | """
18 | Data abstruction for link element
19 | with some methods to access commonly used information
20 |
21 | Attributes:
22 | link: adsk.fusion.Occurrence
23 | occurrence in Fusion360 refers to linkage
24 | dir: str
25 | location of the exported files
26 | name: str
27 | identified name of the link
28 | pose: Matrix3D
29 | get by adsk.fusion.Occurrence.transform2
30 | Gets the 3d matrix data that defines this occurrences orientation and position in its assembly context
31 | the assembly context here is the root component's frame
32 | phyPro: adsk.fusion.PhysicalProperties
33 | object for accessing link's physical properties
34 | """
35 |
36 | dir = "" # location of the exported file
37 | def __init__(self, occurrence: adsk.fusion.Occurrence) -> None:
38 | """
39 | Initialize attributes for an object
40 | """
41 | # TODO: maybe this is a better and accurate way for this, not test
42 | # self.link = occurrence.createForAssemblyContext(occurrence)
43 |
44 | self.link: adsk.fusion.Occurrence = occurrence
45 | self.name = None
46 | # pose can have two meanings:
47 | # 1. the homogeneous matrix of link-frame L w.r.t world-frame w
48 | # 2. the coordinates of link-frame L
49 | self.pose: adsk.core.Matrix3D = occurrence.transform2
50 | self.phyPro = occurrence.getPhysicalProperties(adsk.fusion.CalculationAccuracy.VeryHighCalculationAccuracy)
51 |
52 | def get_link_occ(self) -> adsk.fusion.Occurrence:
53 | """
54 | Return:
55 | occ: adsk.fusion.Occurrence
56 | occurrence the link instance referenced
57 | """
58 | return self.link
59 |
60 | def get_parent_joint(self) -> adsk.fusion.Joint:
61 | """
62 | Get the parent joint of a robot link
63 | Every robot link can only have one parent joint,
64 | but can have multiple child joints to connect child links
65 |
66 | Return:
67 | j: parent_joints
68 | """
69 | # TODO: in closed loop mechanism, there exits one assembly method that
70 | # make one link have two parent joint, write a instruction or fix this bug
71 | # text_palette = constants.get_text_palette()
72 | joint_list: adsk.fusion.JointList = self.link.joints
73 | # text_palette.writeText("Link: {}".format(self.get_name()))
74 | for j in joint_list:
75 | # text_palette.writeText("Joint list item: {}".format(j.name))
76 | # text_palette.writeText("Joint's child link: {}".format(j.occurrenceOne.fullPathName))
77 | if j.occurrenceOne == self.link:
78 | # text_palette.writeText("Return j type: {}".format(j.objectType))
79 | return j
80 | else:
81 | continue
82 |
83 | # text_palette.writeText("Return a None")
84 | return None
85 |
86 | def get_name(self) -> str:
87 | """
88 | Get the link valid name for link fullPathName
89 | Return:
90 | ---------
91 | name: str
92 | """
93 | if self.link.component.name == "base_link":
94 | name = "base_link"
95 | else:
96 | name = utils.get_valid_filename(self.link.fullPathName)
97 |
98 | return name
99 |
100 | def get_pose_sdf(self) -> list:
101 | """
102 | Return a pose(translation, rotation) expressed in the frame named by @relative_to.
103 | The first three components (x, y, z) represent the position of the element's origin(in the @relative_to frame)
104 | The rotation component represents the orientation of the element as either a sequence of Euler rotations(r, p, y)
105 |
106 | Return
107 | ---------
108 | pose: list
109 | [x, y, z, roll, pitch, yaw]
110 | """
111 | pose = math_op.matrix3d_2_pose(self.pose)
112 | return pose
113 |
114 | def get_inertia_sdf(self) -> list:
115 | """
116 | Return link's moments of inertia about Co(the link's center of mass)
117 | for the unit vectors Cx, Cy, Cz fixed in the center-of-mass-frame C.
118 | Note: the orientation of Cx, Cy, Cz relative to Lx, Ly, Lz which is specified
119 | by the `pose` tag.
120 | We set center-of-mass frame's orientation to be the same as link-frame L
121 |
122 | Return:
123 | ---------
124 | list: [ixx, iyy, izz, ixy, iyz, ixz]
125 | a inertia tensor w.r.t CoM frame which has the same orientation with parent frame
126 | """
127 | (_, w_ixx, w_iyy, w_izz, w_ixy, w_iyz, w_ixz) = self.phyPro.getXYZMomentsOfInertia() # unit: kg*cm^2
128 | # Use parallel axis theorem to change inertia from w.r.t world-frame's origin to w.r.t CoM
129 | x = self.phyPro.centerOfMass.x * 0.01 # get the x coordinate w.r.t world-frame, unit: m
130 | y = self.phyPro.centerOfMass.y * 0.01
131 | z = self.phyPro.centerOfMass.z * 0.01
132 | mass = self.phyPro.mass # unit: kg
133 | com_ixx = w_ixx*0.0001 - mass*(y**2+z**2) # kg*cm^2 -> kg*m^2
134 | com_iyy = w_iyy*0.0001 - mass*(x**2+z**2)
135 | com_izz = w_izz*0.0001 - mass*(x**2+y**2)
136 | com_ixy = w_ixy*0.0001 + mass*(x*y)
137 | com_iyz = w_iyz*0.0001 + mass*(y*z)
138 | com_ixz = w_ixz*0.0001 + mass*(x*z)
139 |
140 | # Change inertia tensor to the orientation of link-frame L
141 | # reference: https://robot.sia.cn/CN/abstract/abstract374.shtml
142 | R = math_op.get_rotation_matrix(self.pose)
143 | R_T = math_op.matrix_transpose(R)
144 | inertia_tensor = [[com_ixx, com_ixy, com_ixz],
145 | [com_ixy, com_iyy, com_iyz],
146 | [com_ixz, com_iyz, com_izz]]
147 |
148 | # moments of inertia has the same orientation of L-frame
149 | I = math_op.matrix_multi(math_op.matrix_multi(R_T, inertia_tensor), R)
150 | # I = math_op.matrix_multi(math_op.matrix_multi(R, inertia_tensor), R_T)
151 |
152 | ixx = I[0][0]
153 | iyy = I[1][1]
154 | izz = I[2][2]
155 | ixy = I[0][1]
156 | iyz = I[1][2]
157 | ixz = I[0][2]
158 |
159 | return [ixx, iyy, izz, ixy, iyz, ixz] # unit: kg*m^2
160 |
161 | def get_initia_urdf(self):
162 | """
163 | Return [ixx, iyy, izz, ixy, iyz, ixz]
164 | the inertia tensor elements of the link w.r.t the CoM frame C
165 | CoM frame C has the same orientation with the parent frame
166 |
167 | Return:
168 | ---------
169 | list: [ixx, iyy, izz, ixy, iyz, ixz]
170 | a inertia tensor w.r.t CoM frame which has the same orientation with parent frame
171 | """
172 | # First, get the inertia tensor and CoM w.r.t the world frame
173 | (_, w_ixx, w_iyy, w_izz, w_ixy, w_iyz, w_ixz) = self.phyPro.getXYZMomentsOfInertia() # unit: kg*cm^2
174 | x = self.phyPro.centerOfMass.x * 0.01 # get the x coordinate w.r.t world-frame, unit: m
175 | y = self.phyPro.centerOfMass.y * 0.01
176 | z = self.phyPro.centerOfMass.z * 0.01
177 |
178 | # Use parallel axis theorem to change inertia from w.r.t world-frame's origin to w.r.t CoM
179 | mass = self.phyPro.mass # unit: kg
180 | com_ixx = w_ixx*0.0001 - mass*(y**2+z**2) # kg*cm^2 -> kg*m^2
181 | com_iyy = w_iyy*0.0001 - mass*(x**2+z**2)
182 | com_izz = w_izz*0.0001 - mass*(x**2+y**2)
183 | com_ixy = w_ixy*0.0001 + mass*(x*y)
184 | com_iyz = w_iyz*0.0001 + mass*(y*z)
185 | com_ixz = w_ixz*0.0001 + mass*(x*z)
186 | inertia_tensor = [[com_ixx, com_ixy, com_ixz],
187 | [com_ixy, com_iyy, com_iyz],
188 | [com_ixz, com_iyz, com_izz]]
189 |
190 | # get the parent frame
191 | parent_joint = self.get_parent_joint()
192 | # parent frame for a link has parent joint, is the parent joint frame
193 | # else, is the link frame itselt
194 | if parent_joint is None:
195 | parent_frame: adsk.core.Matrix3D = self.pose
196 | else:
197 | joint = Joint(parent_joint)
198 | parent_frame: adsk.core.Matrix3D = joint.get_joint_frame()
199 |
200 | # Change inertia tensor to the orientation of parent frame
201 |
202 |
203 | # # Get eigenvectors, which are the principal axes of the inertia tensor
204 | # _, eigenvectors = math_op.eigenvalues_and_eigenvectors(inertia_tensor)
205 |
206 | # # Get the rotation matrix represents the orientation of the principal axes
207 | # R = math_op.matrix_transpose(eigenvectors)
208 | # R_T = eigenvectors
209 |
210 | R = math_op.get_rotation_matrix(parent_frame)
211 | R_T = math_op.matrix_transpose(R)
212 |
213 | # Express inertia tersor w.r.t CoM frame C with axes Cx, Cy, Cz aligned with principal inertia directions
214 | I = math_op.matrix_multi(math_op.matrix_multi(R_T, inertia_tensor), R)
215 | ixx = I[0][0]
216 | iyy = I[1][1]
217 | izz = I[2][2]
218 | ixy = I[0][1]
219 | iyz = I[1][2]
220 | ixz = I[0][2]
221 |
222 | return [ixx, iyy, izz, ixy, iyz, ixz] # unit: kg*m^2
223 |
224 | def get_inertia_mjcf(self) -> list:
225 | """
226 | Get inertia tensor as mjcf's body subelement
227 | 6 numbers in the following order: M(1,1), M(2,2), M(3,3), M(1,2), M(1,3), M(2,3).
228 | which is ixx, iyy, izz, ixy, ixz, iyz
229 |
230 | Return
231 | ---------
232 | inertia: list
233 | [ixx, iyy, izz, ixy, ixz, iyz]
234 | """
235 | inertia = self.get_inertia_sdf()
236 | inertia = [inertia[0], inertia[1], inertia[2], inertia[3], inertia[5], inertia[4]]
237 | return inertia
238 |
239 | def get_mass(self) -> float:
240 | """
241 | Get mass of the link, which is same in urdf, sdf, mjcf
242 |
243 | Return:
244 | ---------
245 | mass: float
246 | """
247 | mass = self.phyPro.mass # kg
248 | return mass
249 |
250 | def get_CoM_wrt_link(self):
251 | """
252 | Get CoM frame w.r.t link frame in [x, y, z roll, pitch, yaw] representation
253 |
254 | Return:
255 | pose_CoM: [x, y, z, roll, pitch, yaw]
256 | Center-of-mass frame C w.r.t to the link-frame L which contains the CoM
257 | """
258 | # Let the orientation of center-of-mass frame C is same as link-frame L
259 | roll = 0.0
260 | pitch = 0.0
261 | yaw = 0.0
262 | w_CoM_x = self.phyPro.centerOfMass.x # point CoM's x coordinate w.r.t world-frame
263 | w_CoM_y = self.phyPro.centerOfMass.y
264 | w_CoM_z = self.phyPro.centerOfMass.z
265 | w_Lo_x = self.pose.translation.x # link-frame's origin point's x coordinat w.r.t world-frame
266 | w_Lo_y = self.pose.translation.y
267 | w_Lo_z = self.pose.translation.z
268 | # represent vector Lo-CoM which start from link-fram's origin point to CoM point w.r.t world-frame
269 | w_Lo_CoM = [[(w_CoM_x-w_Lo_x)*0.01], [(w_CoM_y-w_Lo_y)*0.01], [(w_CoM_z-w_Lo_z)*0.01]] # cm -> m
270 | # represent world-frame's orientation w.r.t link-frame
271 | L_R_w = [[self.pose.getCell(0, 0), self.pose.getCell(1, 0), self.pose.getCell(2, 0)],
272 | [self.pose.getCell(0, 1), self.pose.getCell(1, 1), self.pose.getCell(2, 1)],
273 | [self.pose.getCell(0, 2), self.pose.getCell(1, 2), self.pose.getCell(2, 2)]]
274 |
275 | L_Lo_CoM = math_op.change_orientation(L_R_w, w_Lo_CoM)
276 | pose_CoM = [L_Lo_CoM[0][0], L_Lo_CoM[1][0], L_Lo_CoM[2][0], roll, pitch, yaw]
277 |
278 | return pose_CoM
279 |
280 | def get_CoM_sdf(self):
281 | """
282 | Get CoM frame w.r.t link frame in [x, y, z, roll, pitch, yaw] representation
283 |
284 | Return:
285 | pose_CoM: [x, y, z, roll, pitch, yaw]
286 | Center-of-mass frame C relative to the link-frame L
287 | """
288 | # TODO: to avoid compatibility issues associated with the negative sign convention for product
289 | # of inertia, align CoM frame C's axes to the principal inertia directions, so that all the
290 | # products of inertia are zero
291 |
292 | # Let the orientation of center-of-mass frame C is same as link-frame L
293 | roll = 0.0
294 | pitch = 0.0
295 | yaw = 0.0
296 | w_CoM_x = self.phyPro.centerOfMass.x # point CoM's x coordinate w.r.t world-frame
297 | w_CoM_y = self.phyPro.centerOfMass.y
298 | w_CoM_z = self.phyPro.centerOfMass.z
299 | w_Lo_x = self.pose.translation.x # link-frame's origin point's x coordinat w.r.t world-frame
300 | w_Lo_y = self.pose.translation.y
301 | w_Lo_z = self.pose.translation.z
302 | # represent vector Lo-CoM which start from link-fram's origin point to CoM point w.r.t world-frame
303 | w_Lo_CoM = [[(w_CoM_x-w_Lo_x)*0.01], [(w_CoM_y-w_Lo_y)*0.01], [(w_CoM_z-w_Lo_z)*0.01]] # cm -> m
304 | # represent world-frame's orientation w.r.t link-frame
305 | L_R_w = [[self.pose.getCell(0, 0), self.pose.getCell(1, 0), self.pose.getCell(2, 0)],
306 | [self.pose.getCell(0, 1), self.pose.getCell(1, 1), self.pose.getCell(2, 1)],
307 | [self.pose.getCell(0, 2), self.pose.getCell(1, 2), self.pose.getCell(2, 2)]]
308 |
309 | L_Lo_CoM = math_op.change_orientation(L_R_w, w_Lo_CoM)
310 | pose_CoM = [L_Lo_CoM[0][0], L_Lo_CoM[1][0], L_Lo_CoM[2][0], roll, pitch, yaw]
311 |
312 | return pose_CoM
313 |
314 | def get_CoM_urdf(self):
315 | """
316 | Get CoM frame w.r.t link frame in [x, y, z, roll, pitch, yaw] representation
317 | In URDF, parent link frame L is the parent joint frame Jp
318 | Return:
319 | ---------
320 | pose_CoM: [x, y, z, roll, pitch, yaw]
321 | """
322 | if self.get_parent_joint() is None:
323 | # for the first link which does not have parent joint
324 | # do not know the CoM is w.r.t world frame or the link frame
325 | # here let it be w.r.t the link frame
326 | pose_CoM = self.get_CoM_sdf()
327 | else:
328 | textPalette: adsk.core.Palette = constants.get_text_palette()
329 | joint = Joint(self.get_parent_joint())
330 | textPalette.writeText("Parent joint: {}".format(joint.get_name()))
331 | parent_joint_frame: adsk.core.Matrix3D = joint.get_joint_frame()
332 | from_origin, from_xAxis, from_yAxis, from_zAxis = parent_joint_frame.getAsCoordinateSystem()
333 | textPalette.writeText("Parent joint frame: \n"+math_op.matrix3D_to_str(parent_joint_frame))
334 |
335 | w_CoM_x = self.phyPro.centerOfMass.x # point CoM's x coordinate w.r.t world-frame
336 | w_CoM_y = self.phyPro.centerOfMass.y
337 | w_CoM_z = self.phyPro.centerOfMass.z
338 |
339 | # Let the CoM-frame C has the same orientation with parent link frame
340 | CoM_frame_O: adsk.core.Point3D = adsk.core.Point3D.create()
341 | CoM_frame_O.set(w_CoM_x, w_CoM_y, w_CoM_z)
342 | CoM_frame: adsk.core.Matrix3D = adsk.core.Matrix3D.create()
343 | CoM_frame_x: adsk.core.Vector3D = from_xAxis
344 | CoM_frame_y: adsk.core.Vector3D = from_yAxis
345 | CoM_frame_z: adsk.core.Vector3D = from_zAxis
346 | CoM_frame.setWithCoordinateSystem(CoM_frame_O, CoM_frame_x, CoM_frame_y, CoM_frame_z)
347 | to_origin, to_xAsix, to_yAxis, to_zAxis = CoM_frame.getAsCoordinateSystem()
348 | textPalette.writeText("CoM frame: \n" + math_op.matrix3D_to_str(CoM_frame))
349 |
350 | transform = adsk.core.Matrix3D.create()
351 | # transform.setToAlignCoordinateSystems(from_origin, from_xAxis, from_yAxis, from_zAxis,
352 | # to_origin, to_xAsix, to_yAxis, to_zAxis)
353 | transform = math_op.coordinate_transform(parent_joint_frame, CoM_frame)
354 | textPalette.writeText("Transform matrix: \n" + math_op.matrix3D_to_str(transform))
355 |
356 | pose_CoM = math_op.matrix3d_2_pose(transform)
357 |
358 | return pose_CoM
359 |
360 | def get_mesh_origin(self):
361 | """
362 | Fusion360 returns the mesh file with the same coordinate frame as the reference occurrence, which is the link fram L
363 | So the mesh origin transforms the parent-joint frame J to the link-frame L
364 | this is used for the mesh element of visual and collision in URDF
365 | Return:
366 | ---------
367 | mesh_orign: list
368 | [x, y, z, roll, pitch, yaw]
369 | """
370 | if self.get_parent_joint() is None:
371 | # for the first link which does not have parent joint
372 | # haven't found the reference about how to define the mesh origin
373 | # it might be w.r.t world frame or link frame
374 | # here let it be w.r.t link frame, which coincide with the link frame
375 | mesh_origin = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
376 | # transform = self.pose
377 | # mesh_origin = math_op.matrix3d_2_pose(transform)
378 | else:
379 | joint = Joint(self.get_parent_joint())
380 | parent_joint_frame: adsk.core.Matrix3D = joint.get_joint_frame()
381 | textPalette: adsk.core.Palette = constants.get_text_palette()
382 | textPalette.writeText("Parent joint frame: \n"+math_op.matrix3D_to_str(parent_joint_frame))
383 | # link frame coincides with the mesh's frame
384 | link_frame: adsk.core.Matrix3D = self.pose
385 | textPalette.writeText("Link frame: \n" + math_op.matrix3D_to_str(link_frame))
386 | from_origin, from_xAxis, from_yAxis, from_zAxis = parent_joint_frame.getAsCoordinateSystem()
387 | to_origin, to_xAsix, to_yAxis, to_zAxis = link_frame.getAsCoordinateSystem()
388 |
389 | transform = adsk.core.Matrix3D.create()
390 | # transform.setToAlignCoordinateSystems(from_origin, from_xAxis, from_yAxis, from_zAxis,
391 | # to_origin, to_xAsix, to_yAxis, to_zAxis)
392 | transform = math_op.coordinate_transform(parent_joint_frame, link_frame)
393 | # textPalette.writeText("Transform matrix: \n" + math_op.matrix3D_to_str(transform))
394 | mesh_origin = math_op.matrix3d_2_pose(transform)
395 |
396 | return mesh_origin
397 |
398 | def get_collision_sdf(self):
399 | """
400 | Get colision info for sdf collision element
401 | Since mesh frame coincide with the parent link frame
402 | Use the default position for colision mesh
403 | Return:
404 | ---------
405 | col_name: a string for the element's name attribute
406 | geometry_uri: a uri to locate the collisioin's mesh geometry
407 | """
408 | col_name = str(self.get_name()) + "_collision"
409 | robot_name = constants.get_robot_name()
410 | geometry_uri = "model://" + robot_name + "/meshes/" + str(self.get_name()) + ".stl"
411 | return col_name, geometry_uri
412 |
413 | def get_collision_urdf(self):
414 | """
415 | Get colision info for urdf collision element
416 | Return:
417 | ---------
418 | col_name: a string for the element's name attribute
419 | mesh_loc: the path to the mesh file
420 | """
421 | col_name = self.get_name() + "_collision"
422 | # robot_name = constants.get_robot_name()s
423 | # mesh_loc = "package://" + robot_name + "/meshes/" + self.get_name() + ".stl"
424 | mesh_loc = "meshes/" + self.get_name() + ".stl"
425 | return col_name, mesh_loc
426 |
427 | def get_visual_sdf(self):
428 | """
429 | Get colision info for sdf collision element
430 | Since mesh frame coincide with the parent link frame
431 | Use the default position for visualization mesh
432 | Return:
433 | ---------
434 | visual_name: a string for the element's name attribute
435 | geometry_uri: a uri to locate the visual's mesh geometry
436 | """
437 | visual_name = self.get_name() + "_visual"
438 | robot_name = constants.get_robot_name()
439 | geometry_uri = "model://" + robot_name + "/meshes/" + str(self.get_name()) + ".stl"
440 | return visual_name, geometry_uri
441 |
442 | def get_visual_urdf(self):
443 | """
444 | Return:
445 | visual_name: a string for the element's name attribute
446 | mesh_loc: str
447 | the path to the mesh file
448 | """
449 | visual_name = self.get_name() + "_visual"
450 | robot_name = constants.get_robot_name()
451 | # mesh_loc = "package://" + robot_name + "/meshes/" + self.get_name() + ".stl"
452 | mesh_loc = "meshes/" + self.get_name() + ".stl"
453 | return visual_name, mesh_loc
454 |
455 | def get_visual_body(self) -> adsk.fusion.BRepBody:
456 | """
457 | get body that contains 'visual' in its name
458 | which means they are used as visual geometry
459 |
460 | Return:
461 | visual_geo: BRepBody
462 | the body represents visual geometry
463 | None
464 | """
465 | for body in self.link.bRepBodies:
466 | b_name: str = body.name
467 | if b_name.find('visual') != -1:
468 | visual_geo: adsk.fusion.BRepBody = body
469 | return visual_geo
470 |
471 | return None
472 |
473 | def get_collision_body(self) -> adsk.fusion.BRepBody:
474 | """
475 | get body that contains 'collision' in its name
476 | which means they are used as collision geometry
477 |
478 | Return:
479 | col_geo: BRepBody
480 | the body represents collision geometry
481 | None
482 | """
483 | for body in self.link.bRepBodies:
484 | b_name: str = body.name
485 | if b_name.find('collision') != -1:
486 | col_geo: adsk.fusion.BRepBody = body
487 | return col_geo
488 |
489 | return None
490 |
--------------------------------------------------------------------------------
/Add-IN/ACDC4Robot/core/urdf.py:
--------------------------------------------------------------------------------
1 | # -*- coding: utf-8 -*-
2 | # Author: Nuofan
3 | # 20230811
4 | # Contains functions for generating urdf
5 | import adsk, adsk.core, adsk.fusion
6 | from .link import Link
7 | from .joint import Joint
8 | from typing import List
9 | from xml.etree.ElementTree import Element, SubElement
10 | import xml.etree.ElementTree as ET
11 | from . import math_operation as math_op
12 | from . import utils
13 | from ..commands.ACDC4Robot import constants
14 | from .robot import Robot
15 |
16 | class URDF():
17 |
18 | def __init__(self, robot: Robot):
19 | self.robot = robot
20 | self.links: list[Link] = robot.get_links()
21 | self.tree_joints: list[Joint] = robot.get_tree_joints()
22 |
23 | def write_file(self, file_path):
24 | robot_ele = self.get_robot_ele()
25 | urdf_tree = ET.ElementTree(robot_ele)
26 | acdc4robot_info = self.get_acdc4robot_info()
27 | comment = ET.Comment(acdc4robot_info)
28 | robot_ele.append(comment)
29 |
30 | for link in self.links:
31 | link_ele = self.get_link_element(link)
32 | robot_ele.append(link_ele)
33 |
34 | for joint in self.tree_joints:
35 | joint_ele = self.get_joint_element(joint)
36 | robot_ele.append(joint_ele)
37 |
38 | # set indent to pretty the xml output
39 | ET.indent(urdf_tree, space=" ", level=0)
40 |
41 | urdf_tree.write(file_path, encoding="utf-8", xml_declaration=True)
42 |
43 | def get_acdc4robot_info(self,) -> str:
44 | info = "This URDF file is generated by ACDC4Robot: https://github.com/bionicdl-sustech/ACDC4Robot"
45 | return info
46 |
47 | def get_link_element(self, link: Link) -> Element:
48 | """
49 | Return
50 | ---------
51 | link_ele: Element
52 | xml elements contains informations for link
53 | """
54 | # create a link element
55 | link_ele = Element("link")
56 | link_ele.attrib = {"name": self.get_link_name(link)}
57 |
58 | # add inertia sub-element
59 | inertial = SubElement(link_ele, "inertial")
60 | origin = SubElement(inertial, "origin")
61 | origin.attrib = {"xyz": "{} {} {}".format(self.get_link_inertial_origin(link)[0], self.get_link_inertial_origin(link)[1], self.get_link_inertial_origin(link)[2]),
62 | "rpy": "{} {} {}".format(self.get_link_inertial_origin(link)[3], self.get_link_inertial_origin(link)[4], self.get_link_inertial_origin(link)[5])}
63 | mass = SubElement(inertial, "mass")
64 | mass.attrib = {"value": "{}".format(self.get_link_mass(link))}
65 | inertia = SubElement(inertial, "inertia")
66 | inertia.attrib = {"ixx": "{}".format(self.get_link_inertia(link)[0]),
67 | "iyy": "{}".format(self.get_link_inertia(link)[1]),
68 | "izz": "{}".format(self.get_link_inertia(link)[2]),
69 | "ixy": "{}".format(self.get_link_inertia(link)[3]),
70 | "iyz": "{}".format(self.get_link_inertia(link)[4]),
71 | "ixz": "{}".format(self.get_link_inertia(link)[5])}
72 |
73 | # add visual sub-element
74 | visual = SubElement(link_ele, "visual")
75 | visual.attrib = {"name": "{}".format(self.get_link_visual_name(link))}
76 | origin_v = SubElement(visual, "origin")
77 | origin_v.attrib = {"xyz": "{} {} {}".format(self.get_mesh_origin(link)[0], self.get_mesh_origin(link)[1], self.get_mesh_origin(link)[2]),
78 | "rpy": "{} {} {}".format(self.get_mesh_origin(link)[3], self.get_mesh_origin(link)[4], self.get_mesh_origin(link)[5])}
79 | geometry_v = SubElement(visual, "geometry")
80 | mesh_v = SubElement(geometry_v, "mesh")
81 | mesh_v.attrib = {"filename": self.get_link_visual_geo(link), "scale": "0.001 0.001 0.001"}
82 |
83 | # add collision sub-element
84 | collision = SubElement(link_ele, "collision")
85 | collision.attrib = {"name": "{}".format(self.get_link_collision_name(link))}
86 | origin_c = SubElement(collision, "origin")
87 | origin_c.attrib = {"xyz": "{} {} {}".format(self.get_mesh_origin(link)[0], self.get_mesh_origin(link)[1], self.get_mesh_origin(link)[2]),
88 | "rpy": "{} {} {}".format(self.get_mesh_origin(link)[3], self.get_mesh_origin(link)[4], self.get_mesh_origin(link)[5])}
89 | geometry_c = SubElement(collision, "geometry")
90 | mesh_c = SubElement(geometry_c, "mesh")
91 | mesh_c.attrib = {"filename": self.get_link_collision_geo(link), "scale": "0.001 0.001 0.001"}
92 |
93 | return link_ele
94 |
95 | def get_joint_element(self, joint: Joint) -> Element:
96 | """
97 | Return
98 | ---------
99 | joint_ele: Element
100 | xml elements contains informations for joint
101 | """
102 | joint_ele = Element("joint")
103 | joint_ele.attrib = {"name": self.get_joint_name(joint),
104 | "type": self.get_joint_type(joint)}
105 |
106 | # add joint origin element
107 | origin = SubElement(joint_ele, "origin")
108 | origin.attrib = {"xyz": "{} {} {}".format(self.get_joint_origin(joint)[0], self.get_joint_origin(joint)[1], self.get_joint_origin(joint)[2]),
109 | "rpy": "{} {} {}".format(self.get_joint_origin(joint)[3], self.get_joint_origin(joint)[4], self.get_joint_origin(joint)[5])}
110 |
111 | # add parent and child element
112 | parent = SubElement(joint_ele, "parent")
113 | parent.attrib = {"link": self.get_link_name(self.get_joint_parent(joint))}
114 | child = SubElement(joint_ele, "child")
115 | child.attrib = {"link": self.get_link_name(self.get_joint_child(joint))}
116 |
117 | # add axis1, urdf only has one axis for joint
118 | axis = self.get_joint_axis(joint)
119 | if axis is not None:
120 | axis_ele = SubElement(joint_ele, "axis")
121 | axis_ele.attrib = {"xyz": "{} {} {}".format(axis[0], axis[1], axis[2])}
122 |
123 | # add limits
124 | limit = self.get_joint_limit(joint)
125 | if limit is not None:
126 | limit_ele = SubElement(joint_ele, "limit")
127 | limit_ele.attrib = {"lower": "{}".format(limit[0]),
128 | "upper": "{}".format(limit[1]),
129 | "effort": "{}".format(limit[2]),
130 | "velocity": "{}".format(limit[3])}
131 |
132 | return joint_ele
133 |
134 | def get_robot_ele(self, ):
135 | """
136 | Root element of URDF
137 | """
138 | robot_ele = Element("robot")
139 | robot_ele.attrib = {"name": self.robot.get_robot_name()}
140 |
141 | return robot_ele
142 |
143 |
144 | def get_link_name(self, link: Link) -> str:
145 | """
146 | Return
147 | ---------
148 | name: str
149 | link's full path name
150 | """
151 | name = link.get_name()
152 | return name
153 |
154 | def get_link_inertial_origin(self, link: Link) -> list[float]:
155 | """
156 | Return
157 | ---------
158 | inertial_origin: [x, y, z, roll, pitch, yaw]
159 | Pose (translation, rotation) of link's CoM frame C w.r.t link-frame L(parent-joint frame J)
160 | unit: m, radian
161 | """
162 | # the link is the first link so called base-link
163 | if link.get_parent_joint() is None:
164 | # for the first link which does not have parent joint
165 | # do not know the CoM is w.r.t world frame or the link frame
166 | # do not found the details from the description, but according to the figure from:
167 | # http://wiki.ros.org/urdf/XML/model
168 | # it seems to be the link frame
169 |
170 | # Let the orientation of center-of-mass frame C is same as link-frame L
171 | roll = 0.0
172 | pitch = 0.0
173 | yaw = 0.0
174 | w_CoM_x = link.phyPro.centerOfMass.x # point CoM's x coordinate w.r.t world-frame
175 | w_CoM_y = link.phyPro.centerOfMass.y
176 | w_CoM_z = link.phyPro.centerOfMass.z
177 | w_Lo_x = link.pose.translation.x # link-frame's origin point's x coordinat w.r.t world-frame
178 | w_Lo_y = link.pose.translation.y
179 | w_Lo_z = link.pose.translation.z
180 | # represent vector Lo-CoM which start from link-fram's origin point to CoM point w.r.t world-frame
181 | w_Lo_CoM = [[(w_CoM_x-w_Lo_x)*0.01], [(w_CoM_y-w_Lo_y)*0.01], [(w_CoM_z-w_Lo_z)*0.01]] # cm -> m
182 | # represent world-frame's orientation w.r.t link-frame
183 | L_R_w = [[link.pose.getCell(0, 0), link.pose.getCell(1, 0), link.pose.getCell(2, 0)],
184 | [link.pose.getCell(0, 1), link.pose.getCell(1, 1), link.pose.getCell(2, 1)],
185 | [link.pose.getCell(0, 2), link.pose.getCell(1, 2), link.pose.getCell(2, 2)]]
186 |
187 | L_Lo_CoM = math_op.change_orientation(L_R_w, w_Lo_CoM)
188 | inertial_origin = [L_Lo_CoM[0][0], L_Lo_CoM[1][0], L_Lo_CoM[2][0], roll, pitch, yaw]
189 | # the link is not the first link
190 | else:
191 | # for the link is not the first link, CoM frame is w.r.t parent-joint frame J
192 | joint = Joint(link.get_parent_joint())
193 | parent_joint_frame: adsk.core.Matrix3D = joint.get_joint_frame()
194 | from_origin, from_xAxis, from_yAxis, from_zAxis = parent_joint_frame.getAsCoordinateSystem()
195 |
196 | w_CoM_x = link.phyPro.centerOfMass.x # point CoM's x coordinate w.r.t world-frame
197 | w_CoM_y = link.phyPro.centerOfMass.y
198 | w_CoM_z = link.phyPro.centerOfMass.z
199 |
200 | # Let the CoM-frame C has the same orientation with parent link frame
201 | CoM_frame_O: adsk.core.Point3D = adsk.core.Point3D.create()
202 | CoM_frame_O.set(w_CoM_x, w_CoM_y, w_CoM_z)
203 | CoM_frame: adsk.core.Matrix3D = adsk.core.Matrix3D.create()
204 | CoM_frame_x: adsk.core.Vector3D = from_xAxis
205 | CoM_frame_y: adsk.core.Vector3D = from_yAxis
206 | CoM_frame_z: adsk.core.Vector3D = from_zAxis
207 | CoM_frame.setWithCoordinateSystem(CoM_frame_O, CoM_frame_x, CoM_frame_y, CoM_frame_z)
208 | to_origin, to_xAsix, to_yAxis, to_zAxis = CoM_frame.getAsCoordinateSystem()
209 | transform = adsk.core.Matrix3D.create()
210 | # transform.setToAlignCoordinateSystems(from_origin, from_xAxis, from_yAxis, from_zAxis,
211 | # to_origin, to_xAsix, to_yAxis, to_zAxis)
212 | transform = math_op.coordinate_transform(parent_joint_frame, CoM_frame)
213 |
214 | inertial_origin = math_op.matrix3d_2_pose(transform)
215 |
216 | return inertial_origin
217 |
218 | def get_link_mass(self, link: Link) -> float:
219 | """
220 | Return
221 | ---------
222 | mass: float
223 | unit: kg
224 | """
225 | mass = link.get_mass()
226 | return mass
227 |
228 | def get_link_inertia(self, link: Link) -> list[float]:
229 | """
230 | Get the inertia tensor elements of the link w.r.t the CoM frame C,
231 | CoM frame C has the same orientation with the parent frame:
232 | link-frame L for the first link, else the parent joint frame J
233 |
234 | Return
235 | ---------
236 | inertia_list: [ixx, iyy, izz, ixy, iyz, ixz]
237 | unit: kg*m^2
238 | """
239 | # First, get the inertia tensor and CoM w.r.t the world frame
240 | (_, w_ixx, w_iyy, w_izz, w_ixy, w_iyz, w_ixz) = link.phyPro.getXYZMomentsOfInertia() # unit: kg*cm^2
241 | x = link.phyPro.centerOfMass.x * 0.01 # get the x coordinate w.r.t world-frame, unit: m
242 | y = link.phyPro.centerOfMass.y * 0.01
243 | z = link.phyPro.centerOfMass.z * 0.01
244 |
245 | # Use parallel axis theorem to change inertia from w.r.t world-frame's origin to w.r.t CoM
246 | mass = link.phyPro.mass # unit: kg
247 | com_ixx = w_ixx*0.0001 - mass*(y**2+z**2) # kg*cm^2 -> kg*m^2
248 | com_iyy = w_iyy*0.0001 - mass*(x**2+z**2)
249 | com_izz = w_izz*0.0001 - mass*(x**2+y**2)
250 | com_ixy = w_ixy*0.0001 + mass*(x*y)
251 | com_iyz = w_iyz*0.0001 + mass*(y*z)
252 | com_ixz = w_ixz*0.0001 + mass*(x*z)
253 | inertia_tensor = [[com_ixx, com_ixy, com_ixz],
254 | [com_ixy, com_iyy, com_iyz],
255 | [com_ixz, com_iyz, com_izz]]
256 |
257 | # get the parent frame
258 | parent_joint = link.get_parent_joint()
259 | # parent frame for a link has parent joint, is the parent joint frame
260 | # else, is the link frame itselt
261 | if parent_joint is None:
262 | parent_frame: adsk.core.Matrix3D = link.pose
263 | else:
264 | joint = Joint(parent_joint)
265 | parent_frame: adsk.core.Matrix3D = joint.get_joint_frame()
266 |
267 | # Change inertia tensor to the orientation of parent frame
268 | R = math_op.get_rotation_matrix(parent_frame)
269 | R_T = math_op.matrix_transpose(R)
270 |
271 | I = math_op.matrix_multi(math_op.matrix_multi(R_T, inertia_tensor), R)
272 | inertia_list = [I[0][0], I[1][1], I[2][2], I[0][1], I[1][2], I[0][2]]
273 |
274 | return inertia_list # unit: kg*m^2
275 |
276 | def get_link_visual_name(self, link: Link) -> str:
277 | """
278 | Return
279 | ---------
280 | visual_name: str
281 | """
282 | visual_name = link.get_name() + "_visual"
283 | return visual_name
284 |
285 | def get_link_collision_name(self, link: Link) -> str:
286 | """
287 | Return
288 | --------
289 | collision_name: str
290 | """
291 | collision_name = link.get_name() + "_collision"
292 | return collision_name
293 |
294 | def get_mesh_origin(self, link: Link) -> list[float]:
295 | """
296 | The reference frame for mesh element of visual and collision w.r.t link frame L
297 | Return
298 | ---------
299 | mesh_origin: [x, y, z, roll, pitch, yaw]
300 | unit: m, radian
301 | """
302 | if link.get_parent_joint() is None:
303 | # for the first link which does not have parent joint
304 | # the mesh frame coincides with the link frame L
305 | mesh_origin = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
306 | else:
307 | joint = Joint(link.get_parent_joint())
308 | parent_joint_frame: adsk.core.Matrix3D = joint.get_joint_frame()
309 | # link frame coincides with the mesh's frame
310 | link_frame: adsk.core.Matrix3D = link.pose
311 | from_origin, from_xAxis, from_yAxis, from_zAxis = parent_joint_frame.getAsCoordinateSystem()
312 | to_origin, to_xAsix, to_yAxis, to_zAxis = link_frame.getAsCoordinateSystem()
313 |
314 | transform = adsk.core.Matrix3D.create()
315 | # transform.setToAlignCoordinateSystems(from_origin, from_xAxis, from_yAxis, from_zAxis,
316 | # to_origin, to_xAsix, to_yAxis, to_zAxis)
317 | transform = math_op.coordinate_transform(parent_joint_frame, link_frame)
318 | mesh_origin = math_op.matrix3d_2_pose(transform)
319 |
320 | return mesh_origin
321 |
322 | def get_link_visual_geo(self, link: Link) -> str:
323 | """
324 | A dir to locate link's visual mesh file
325 |
326 | Return:
327 | mesh_loc: str
328 | the path to find mesh file
329 | """
330 | visual_body = link.get_visual_body()
331 | col_body = link.get_collision_body()
332 | if (visual_body is None) and (col_body is None):
333 | # visual and collision geometry is same
334 | mesh_loc = "meshes/" + link.get_name() + ".stl"
335 | return mesh_loc
336 | elif (visual_body is not None) and (col_body is not None):
337 | mesh_loc = "meshes/" + link.get_name() + "_visual.stl"
338 | return mesh_loc
339 | elif (visual_body is None) and (col_body is not None):
340 | error_message = "Please set two bodies, one for visual and one for collision. \n"
341 | error_message = error_message + link.get_name() + " body for visual missing."
342 | utils.error_box(error_message)
343 | utils.terminate_box()
344 | elif (visual_body is not None) and (col_body is None):
345 | error_message = "Please set two bodies, one for visual and one for collision. \n"
346 | error_message = error_message + link.get_name() + " body for collision missing."
347 | utils.error_box(error_message)
348 | utils.terminate_box()
349 |
350 | def get_link_collision_geo(self, link: Link) -> str:
351 | """
352 | A dir to locate link's collision mesh file
353 |
354 | Return:
355 | mesh_loc: str
356 | the path to find mesh file
357 | """
358 | visual_body = link.get_visual_body()
359 | col_body = link.get_collision_body()
360 | if (visual_body is None) and (col_body is None):
361 | mesh_loc = "meshes/" + link.get_name() + ".stl"
362 | return mesh_loc
363 | elif (visual_body is not None) and (col_body is not None):
364 | mesh_loc = "meshes/" + link.get_name() + "_collision.stl"
365 | return mesh_loc
366 | elif (visual_body is None) and (col_body is not None):
367 | error_message = "Please set two bodies, one for visual and one for collision. \n"
368 | error_message = error_message + link.get_name() + " body for visual missing."
369 | utils.error_box(error_message)
370 | utils.terminate_box()
371 | elif (visual_body is not None) and (col_body is None):
372 | error_message = "Please set two bodies, one for visual and one for collision. \n"
373 | error_message = error_message + link.get_name() + " body for collision missing."
374 | utils.error_box(error_message)
375 | utils.terminate_box()
376 |
377 |
378 | def get_joint_name(self, joint: Joint) -> str:
379 | """
380 | Return
381 | ---------
382 | joint_name: str
383 | """
384 | joint_name = joint.get_name()
385 | return joint_name
386 |
387 | def get_joint_type(self, joint: Joint) -> str:
388 | """
389 | Return
390 | ---------
391 | joint_type: str
392 | fixed, revolute, prismatic, continuous
393 | currently, only support urdf joint types above
394 | """
395 | urdf_joint_type_list = ["fixed", "revolute", "prismatic"]
396 | if joint.joint.jointMotion.jointType <= 2:
397 | urdf_joint_type = urdf_joint_type_list[joint.joint.jointMotion.jointType]
398 | if urdf_joint_type == "revolute" and (self.get_joint_limit(joint) is None):
399 | urdf_joint_type = "continuous"
400 | else:
401 | # other joint types are not supported yet
402 | pass
403 | return urdf_joint_type
404 |
405 | def get_joint_origin(self, joint: Joint) -> list[float]:
406 | """
407 | Return
408 | ---------
409 | joint_origin: [x, y, z, roll, pitch, yaw]
410 | describe the pose of the joint frame J w.r.t parent frame(parent link frame L or parent link's parent jont frame J)
411 | unit: m, radian
412 | """
413 | parent_link: adsk.fusion.Occurrence = joint.parent
414 | parent_link: Link = Link(parent_link)
415 | parent_joint: adsk.fusion.Joint = parent_link.get_parent_joint()
416 |
417 | # get the parent frame
418 | if parent_joint is None:
419 | # if the parent link does not have parent joint(which means the root link),
420 | # then the parent link frame is the parent frame
421 | parent_frame: adsk.core.Matrix3D = parent_link.pose
422 | else:
423 | parent_joint: Joint = Joint(parent_joint)
424 | parent_frame = parent_joint.get_joint_frame()
425 |
426 | # get joint frame w.r.t world frame
427 | joint_frame = joint.get_joint_frame()
428 |
429 | transform = adsk.core.Matrix3D.create()
430 | transform = math_op.coordinate_transform(parent_frame, joint_frame)
431 | joint_origin = math_op.matrix3d_2_pose(transform)
432 |
433 | return joint_origin
434 |
435 | def get_joint_parent(self, joint: Joint) -> Link:
436 | """
437 | Return
438 | ---------
439 | parent_link: Link
440 | parent link of the joint
441 | """
442 | joint_parent: adsk.fusion.Occurrence = joint.parent
443 | parent_link: Link = Link(joint_parent)
444 | return parent_link
445 |
446 | def get_joint_child(self, joint: Joint) -> Link:
447 | """
448 | Return
449 | ---------
450 | child_link: Link
451 | child link of the joint
452 | """
453 | joint_child: adsk.fusion.Occurrence = joint.child
454 | child_link: Link = Link(joint_child)
455 | return child_link
456 |
457 | def get_joint_axis(self, joint: Joint) -> list[float]:
458 | """
459 | Return
460 | ---------
461 | axis: [x, y, z]
462 | joint axis specified in the joint frame
463 | if joint type is rigid, return None
464 | """
465 | # urdf joint axis is expressed w.r.t parent frame, which is the link frame or the parent joint frame
466 | if joint.joint.jointMotion.jointType == 0: # RigidJointType
467 | w_axis = None
468 | axis = None
469 | return axis
470 | elif joint.joint.jointMotion.jointType == 1: # RevoluteJointType
471 | w_axis = [round(i, 6) for i in joint.joint.jointMotion.rotationAxisVector.asArray()] # In Fusion360, returned axis is normalized
472 | elif joint.joint.jointMotion.jointType == 2: # SliderJointType
473 | w_axis = [round(i, 6) for i in joint.joint.jointMotion.slideDirectionVector.asArray()] # In Fusion360, returned axis is normalized
474 |
475 | J_axis = None
476 | joint_frame: adsk.core.Matrix3D = joint.get_joint_frame()
477 | w_R_J = math_op.get_rotation_matrix(joint_frame) # represent joint-frame J's orientation w.r.t world-frame w
478 | J_R_w = math_op.matrix_transpose(w_R_J) # for a rotation matrix, its inverse is its transpose
479 |
480 | w_axis = [[w_axis[0]], [w_axis[1]], [w_axis[2]]] # transform from 1*3 to 3*1 list
481 | J_axis = math_op.change_orientation(J_R_w, w_axis)
482 | axis = [J_axis[0][0], J_axis[1][0], J_axis[2][0]]
483 |
484 | return axis
485 |
486 | def get_joint_limit(self, joint: Joint) -> list[float]:
487 | """
488 | required only for revolute and prismatic joint
489 | Return
490 | ---------
491 | limit: [lower, upper, effort, velocity]
492 | lower: optional, lower joint limit; unit: radian or m
493 | upper: optional, upper joint limit; unit: radian or m
494 | effort: required, the maximum joint effort, default: 1,000,000, unit: N or Nm
495 | velocity: required, the maximum joint velocity, default: 1,000,000, unit: m/s or rad/s
496 | if joint type is rigid, return None
497 | """
498 | if joint.joint.jointMotion.jointType == 0: # RigidJointType
499 | return None
500 | elif joint.joint.jointMotion.jointType == 1: # RevoluteJointType
501 | max_enabled = joint.joint.jointMotion.rotationLimits.isMaximumValueEnabled
502 | min_enabled = joint.joint.jointMotion.rotationLimits.isMinimumValueEnabled
503 | if max_enabled and min_enabled:
504 | # unit: radians
505 | lower_limit = joint.joint.jointMotion.rotationLimits.minimumValue
506 | upper_limit = joint.joint.jointMotion.rotationLimits.maximumValue
507 | return [round(lower_limit, 6), round(upper_limit, 6), 1_000_000, 1_000_000]
508 | else:
509 | return None
510 | elif joint.joint.jointMotion.jointType == 2: # SliderJointType
511 | max_enabled = joint.joint.jointMotion.slideLimits.isMaximumValueEnabled
512 | min_enabled = joint.joint.jointMotion.slideLimits.isMinimumValueEnabled
513 | if max_enabled and min_enabled:
514 | lower_limit = joint.joint.jointMotion.slideLimits.minimumValue * 0.01 # cm -> m
515 | upper_limit = joint.joint.jointMotion.slideLimits.maximumValue * 0.01 # cm -> m
516 | return [round(lower_limit, 6), round(upper_limit, 6), 1_000_000, 1_000_000]
517 | else:
518 | return None
519 |
520 |
521 |
522 |
523 | def get_link_name(link: Link) -> str:
524 | """
525 | Return
526 | ---------
527 | name: str
528 | link's full path name
529 | """
530 | name = link.get_name()
531 | return name
532 |
533 | def get_link_inertial_origin(link: Link) -> list[float]:
534 | """
535 | Return
536 | ---------
537 | inertial_origin: [x, y, z, roll, pitch, yaw]
538 | Pose (translation, rotation) of link's CoM frame C w.r.t link-frame L(parent-joint frame J)
539 | unit: m, radian
540 | """
541 | # the link is the first link so called base-link
542 | if link.get_parent_joint() is None:
543 | # for the first link which does not have parent joint
544 | # do not know the CoM is w.r.t world frame or the link frame
545 | # do not found the details from the description, but according to the figure from:
546 | # http://wiki.ros.org/urdf/XML/model
547 | # it seems to be the link frame
548 |
549 | # Let the orientation of center-of-mass frame C is same as link-frame L
550 | roll = 0.0
551 | pitch = 0.0
552 | yaw = 0.0
553 | w_CoM_x = link.phyPro.centerOfMass.x # point CoM's x coordinate w.r.t world-frame
554 | w_CoM_y = link.phyPro.centerOfMass.y
555 | w_CoM_z = link.phyPro.centerOfMass.z
556 | w_Lo_x = link.pose.translation.x # link-frame's origin point's x coordinat w.r.t world-frame
557 | w_Lo_y = link.pose.translation.y
558 | w_Lo_z = link.pose.translation.z
559 | # represent vector Lo-CoM which start from link-fram's origin point to CoM point w.r.t world-frame
560 | w_Lo_CoM = [[(w_CoM_x-w_Lo_x)*0.01], [(w_CoM_y-w_Lo_y)*0.01], [(w_CoM_z-w_Lo_z)*0.01]] # cm -> m
561 | # represent world-frame's orientation w.r.t link-frame
562 | L_R_w = [[link.pose.getCell(0, 0), link.pose.getCell(1, 0), link.pose.getCell(2, 0)],
563 | [link.pose.getCell(0, 1), link.pose.getCell(1, 1), link.pose.getCell(2, 1)],
564 | [link.pose.getCell(0, 2), link.pose.getCell(1, 2), link.pose.getCell(2, 2)]]
565 |
566 | L_Lo_CoM = math_op.change_orientation(L_R_w, w_Lo_CoM)
567 | inertial_origin = [L_Lo_CoM[0][0], L_Lo_CoM[1][0], L_Lo_CoM[2][0], roll, pitch, yaw]
568 | # the link is not the first link
569 | else:
570 | # for the link is not the first link, CoM frame is w.r.t parent-joint frame J
571 | joint = Joint(link.get_parent_joint())
572 | parent_joint_frame: adsk.core.Matrix3D = joint.get_joint_frame()
573 | from_origin, from_xAxis, from_yAxis, from_zAxis = parent_joint_frame.getAsCoordinateSystem()
574 |
575 | w_CoM_x = link.phyPro.centerOfMass.x # point CoM's x coordinate w.r.t world-frame
576 | w_CoM_y = link.phyPro.centerOfMass.y
577 | w_CoM_z = link.phyPro.centerOfMass.z
578 |
579 | # Let the CoM-frame C has the same orientation with parent link frame
580 | CoM_frame_O: adsk.core.Point3D = adsk.core.Point3D.create()
581 | CoM_frame_O.set(w_CoM_x, w_CoM_y, w_CoM_z)
582 | CoM_frame: adsk.core.Matrix3D = adsk.core.Matrix3D.create()
583 | CoM_frame_x: adsk.core.Vector3D = from_xAxis
584 | CoM_frame_y: adsk.core.Vector3D = from_yAxis
585 | CoM_frame_z: adsk.core.Vector3D = from_zAxis
586 | CoM_frame.setWithCoordinateSystem(CoM_frame_O, CoM_frame_x, CoM_frame_y, CoM_frame_z)
587 | to_origin, to_xAsix, to_yAxis, to_zAxis = CoM_frame.getAsCoordinateSystem()
588 | transform = adsk.core.Matrix3D.create()
589 | # transform.setToAlignCoordinateSystems(from_origin, from_xAxis, from_yAxis, from_zAxis,
590 | # to_origin, to_xAsix, to_yAxis, to_zAxis)
591 | transform = math_op.coordinate_transform(parent_joint_frame, CoM_frame)
592 |
593 | inertial_origin = math_op.matrix3d_2_pose(transform)
594 |
595 | return inertial_origin
596 |
597 | def get_link_mass(link: Link) -> float:
598 | """
599 | Return
600 | ---------
601 | mass: float
602 | unit: kg
603 | """
604 | mass = link.get_mass()
605 | return mass
606 |
607 | def get_link_inertia(link: Link) -> list[float]:
608 | """
609 | Get the inertia tensor elements of the link w.r.t the CoM frame C,
610 | CoM frame C has the same orientation with the parent frame:
611 | link-frame L for the first link, else the parent joint frame J
612 |
613 | Return
614 | ---------
615 | inertia_list: [ixx, iyy, izz, ixy, iyz, ixz]
616 | unit: kg*m^2
617 | """
618 | # First, get the inertia tensor and CoM w.r.t the world frame
619 | (_, w_ixx, w_iyy, w_izz, w_ixy, w_iyz, w_ixz) = link.phyPro.getXYZMomentsOfInertia() # unit: kg*cm^2
620 | x = link.phyPro.centerOfMass.x * 0.01 # get the x coordinate w.r.t world-frame, unit: m
621 | y = link.phyPro.centerOfMass.y * 0.01
622 | z = link.phyPro.centerOfMass.z * 0.01
623 |
624 | # Use parallel axis theorem to change inertia from w.r.t world-frame's origin to w.r.t CoM
625 | mass = link.phyPro.mass # unit: kg
626 | com_ixx = w_ixx*0.0001 - mass*(y**2+z**2) # kg*cm^2 -> kg*m^2
627 | com_iyy = w_iyy*0.0001 - mass*(x**2+z**2)
628 | com_izz = w_izz*0.0001 - mass*(x**2+y**2)
629 | com_ixy = w_ixy*0.0001 + mass*(x*y)
630 | com_iyz = w_iyz*0.0001 + mass*(y*z)
631 | com_ixz = w_ixz*0.0001 + mass*(x*z)
632 | inertia_tensor = [[com_ixx, com_ixy, com_ixz],
633 | [com_ixy, com_iyy, com_iyz],
634 | [com_ixz, com_iyz, com_izz]]
635 |
636 | # get the parent frame
637 | parent_joint = link.get_parent_joint()
638 | # parent frame for a link has parent joint, is the parent joint frame
639 | # else, is the link frame itselt
640 | if parent_joint is None:
641 | parent_frame: adsk.core.Matrix3D = link.pose
642 | else:
643 | joint = Joint(parent_joint)
644 | parent_frame: adsk.core.Matrix3D = joint.get_joint_frame()
645 |
646 | # Change inertia tensor to the orientation of parent frame
647 | R = math_op.get_rotation_matrix(parent_frame)
648 | R_T = math_op.matrix_transpose(R)
649 |
650 | I = math_op.matrix_multi(math_op.matrix_multi(R_T, inertia_tensor), R)
651 | inertia_list = [I[0][0], I[1][1], I[2][2], I[0][1], I[1][2], I[0][2]]
652 |
653 | return inertia_list # unit: kg*m^2
654 |
655 | def get_link_visual_name(link: Link) -> str:
656 | """
657 | Return
658 | ---------
659 | visual_name: str
660 | """
661 | visual_name = link.get_name() + "_visual"
662 | return visual_name
663 |
664 | def get_link_collision_name(link: Link) -> str:
665 | """
666 | Return
667 | --------
668 | collision_name: str
669 | """
670 | collision_name = link.get_name() + "_collision"
671 | return collision_name
672 |
673 | def get_mesh_origin(link: Link) -> list[float]:
674 | """
675 | The reference frame for mesh element of visual and collision w.r.t link frame L
676 | Return
677 | ---------
678 | mesh_origin: [x, y, z, roll, pitch, yaw]
679 | unit: m, radian
680 | """
681 | if link.get_parent_joint() is None:
682 | # for the first link which does not have parent joint
683 | # the mesh frame coincides with the link frame L
684 | mesh_origin = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
685 | else:
686 | joint = Joint(link.get_parent_joint())
687 | parent_joint_frame: adsk.core.Matrix3D = joint.get_joint_frame()
688 | # link frame coincides with the mesh's frame
689 | link_frame: adsk.core.Matrix3D = link.pose
690 | from_origin, from_xAxis, from_yAxis, from_zAxis = parent_joint_frame.getAsCoordinateSystem()
691 | to_origin, to_xAsix, to_yAxis, to_zAxis = link_frame.getAsCoordinateSystem()
692 |
693 | transform = adsk.core.Matrix3D.create()
694 | # transform.setToAlignCoordinateSystems(from_origin, from_xAxis, from_yAxis, from_zAxis,
695 | # to_origin, to_xAsix, to_yAxis, to_zAxis)
696 | transform = math_op.coordinate_transform(parent_joint_frame, link_frame)
697 | mesh_origin = math_op.matrix3d_2_pose(transform)
698 |
699 | return mesh_origin
700 |
701 | def get_link_visual_geo(link: Link) -> str:
702 | """
703 | A dir to locate link's visual mesh file
704 |
705 | Return:
706 | mesh_loc: str
707 | the path to find mesh file
708 | """
709 | visual_body = link.get_visual_body()
710 | col_body = link.get_collision_body()
711 | if (visual_body is None) and (col_body is None):
712 | # visual and collision geometry is same
713 | mesh_loc = "meshes/" + link.get_name() + ".stl"
714 | return mesh_loc
715 | elif (visual_body is not None) and (col_body is not None):
716 | mesh_loc = "meshes/" + link.get_name() + "_visual.stl"
717 | return mesh_loc
718 | elif (visual_body is None) and (col_body is not None):
719 | error_message = "Please set two bodies, one for visual and one for collision. \n"
720 | error_message = error_message + link.get_name() + " body for visual missing."
721 | utils.error_box(error_message)
722 | utils.terminate_box()
723 | elif (visual_body is not None) and (col_body is None):
724 | error_message = "Please set two bodies, one for visual and one for collision. \n"
725 | error_message = error_message + link.get_name() + " body for collision missing."
726 | utils.error_box(error_message)
727 | utils.terminate_box()
728 |
729 | def get_link_collision_geo(link: Link) -> str:
730 | """
731 | A dir to locate link's collision mesh file
732 |
733 | Return:
734 | mesh_loc: str
735 | the path to find mesh file
736 | """
737 | visual_body = link.get_visual_body()
738 | col_body = link.get_collision_body()
739 | if (visual_body is None) and (col_body is None):
740 | mesh_loc = "meshes/" + link.get_name() + ".stl"
741 | return mesh_loc
742 | elif (visual_body is not None) and (col_body is not None):
743 | mesh_loc = "meshes/" + link.get_name() + "_collision.stl"
744 | return mesh_loc
745 | elif (visual_body is None) and (col_body is not None):
746 | error_message = "Please set two bodies, one for visual and one for collision. \n"
747 | error_message = error_message + link.get_name() + " body for visual missing."
748 | utils.error_box(error_message)
749 | utils.terminate_box()
750 | elif (visual_body is not None) and (col_body is None):
751 | error_message = "Please set two bodies, one for visual and one for collision. \n"
752 | error_message = error_message + link.get_name() + " body for collision missing."
753 | utils.error_box(error_message)
754 | utils.terminate_box()
755 |
756 | def get_link_element(link: Link) -> Element:
757 | """
758 | Return
759 | ---------
760 | link_ele: Element
761 | xml elements contains informations for link
762 | """
763 | # create a link element
764 | link_ele = Element("link")
765 | link_ele.attrib = {"name": get_link_name(link)}
766 |
767 | # add inertia sub-element
768 | inertial = SubElement(link_ele, "inertial")
769 | origin = SubElement(inertial, "origin")
770 | origin.attrib = {"xyz": "{} {} {}".format(get_link_inertial_origin(link)[0], get_link_inertial_origin(link)[1], get_link_inertial_origin(link)[2]),
771 | "rpy": "{} {} {}".format(get_link_inertial_origin(link)[3], get_link_inertial_origin(link)[4], get_link_inertial_origin(link)[5])}
772 | mass = SubElement(inertial, "mass")
773 | mass.attrib = {"value": "{}".format(get_link_mass(link))}
774 | inertia = SubElement(inertial, "inertia")
775 | inertia.attrib = {"ixx": "{}".format(get_link_inertia(link)[0]),
776 | "iyy": "{}".format(get_link_inertia(link)[1]),
777 | "izz": "{}".format(get_link_inertia(link)[2]),
778 | "ixy": "{}".format(get_link_inertia(link)[3]),
779 | "iyz": "{}".format(get_link_inertia(link)[4]),
780 | "ixz": "{}".format(get_link_inertia(link)[5])}
781 |
782 | # add visual sub-element
783 | visual = SubElement(link_ele, "visual")
784 | visual.attrib = {"name": "{}".format(get_link_visual_name(link))}
785 | origin_v = SubElement(visual, "origin")
786 | origin_v.attrib = {"xyz": "{} {} {}".format(get_mesh_origin(link)[0], get_mesh_origin(link)[1], get_mesh_origin(link)[2]),
787 | "rpy": "{} {} {}".format(get_mesh_origin(link)[3], get_mesh_origin(link)[4], get_mesh_origin(link)[5])}
788 | geometry_v = SubElement(visual, "geometry")
789 | mesh_v = SubElement(geometry_v, "mesh")
790 | mesh_v.attrib = {"filename": get_link_visual_geo(link), "scale": "0.001 0.001 0.001"}
791 |
792 | # add collision sub-element
793 | collision = SubElement(link_ele, "collision")
794 | collision.attrib = {"name": "{}".format(get_link_collision_name(link))}
795 | origin_c = SubElement(collision, "origin")
796 | origin_c.attrib = {"xyz": "{} {} {}".format(get_mesh_origin(link)[0], get_mesh_origin(link)[1], get_mesh_origin(link)[2]),
797 | "rpy": "{} {} {}".format(get_mesh_origin(link)[3], get_mesh_origin(link)[4], get_mesh_origin(link)[5])}
798 | geometry_c = SubElement(collision, "geometry")
799 | mesh_c = SubElement(geometry_c, "mesh")
800 | mesh_c.attrib = {"filename": get_link_collision_geo(link), "scale": "0.001 0.001 0.001"}
801 |
802 | return link_ele
803 |
804 |
805 | def get_joint_name(joint: Joint) -> str:
806 | """
807 | Return
808 | ---------
809 | joint_name: str
810 | """
811 | joint_name = joint.get_name()
812 | return joint_name
813 |
814 | def get_joint_type(joint: Joint) -> str:
815 | """
816 | Return
817 | ---------
818 | joint_type: str
819 | fixed, revolute, prismatic, continuous
820 | currently, only support urdf joint types above
821 | """
822 | urdf_joint_type_list = ["fixed", "revolute", "prismatic"]
823 | if joint.joint.jointMotion.jointType <= 2:
824 | urdf_joint_type = urdf_joint_type_list[joint.joint.jointMotion.jointType]
825 | if urdf_joint_type == "revolute" and (get_joint_limit(joint) is None):
826 | urdf_joint_type = "continuous"
827 | else:
828 | # other joint types are not supported yet
829 | pass
830 | return urdf_joint_type
831 |
832 | def get_joint_origin(joint: Joint) -> list[float]:
833 | """
834 | Return
835 | ---------
836 | joint_origin: [x, y, z, roll, pitch, yaw]
837 | describe the pose of the joint frame J w.r.t parent frame(parent link frame L or parent link's parent jont frame J)
838 | unit: m, radian
839 | """
840 | parent_link: adsk.fusion.Occurrence = joint.parent
841 | parent_link: Link = Link(parent_link)
842 | parent_joint: adsk.fusion.Joint = parent_link.get_parent_joint()
843 |
844 | # get the parent frame
845 | if parent_joint is None:
846 | # if the parent link does not have parent joint(which means the root link),
847 | # then the parent link frame is the parent frame
848 | parent_frame: adsk.core.Matrix3D = parent_link.pose
849 | else:
850 | parent_joint: Joint = Joint(parent_joint)
851 | parent_frame = parent_joint.get_joint_frame()
852 |
853 | # get joint frame w.r.t world frame
854 | joint_frame = joint.get_joint_frame()
855 |
856 | transform = adsk.core.Matrix3D.create()
857 | transform = math_op.coordinate_transform(parent_frame, joint_frame)
858 | joint_origin = math_op.matrix3d_2_pose(transform)
859 |
860 | return joint_origin
861 |
862 | def get_joint_parent(joint: Joint) -> Link:
863 | """
864 | Return
865 | ---------
866 | parent_link: Link
867 | parent link of the joint
868 | """
869 | joint_parent: adsk.fusion.Occurrence = joint.parent
870 | parent_link: Link = Link(joint_parent)
871 | return parent_link
872 |
873 | def get_joint_child(joint: Joint) -> Link:
874 | """
875 | Return
876 | ---------
877 | child_link: Link
878 | child link of the joint
879 | """
880 | joint_child: adsk.fusion.Occurrence = joint.child
881 | child_link: Link = Link(joint_child)
882 | return child_link
883 |
884 | def get_joint_axis(joint: Joint) -> list[float]:
885 | """
886 | Return
887 | ---------
888 | axis: [x, y, z]
889 | joint axis specified in the joint frame
890 | if joint type is rigid, return None
891 | """
892 | # urdf joint axis is expressed w.r.t parent frame, which is the link frame or the parent joint frame
893 | if joint.joint.jointMotion.jointType == 0: # RigidJointType
894 | w_axis = None
895 | axis = None
896 | return axis
897 | elif joint.joint.jointMotion.jointType == 1: # RevoluteJointType
898 | w_axis = [round(i, 6) for i in joint.joint.jointMotion.rotationAxisVector.asArray()] # In Fusion360, returned axis is normalized
899 | elif joint.joint.jointMotion.jointType == 2: # SliderJointType
900 | w_axis = [round(i, 6) for i in joint.joint.jointMotion.slideDirectionVector.asArray()] # In Fusion360, returned axis is normalized
901 |
902 | J_axis = None
903 | joint_frame: adsk.core.Matrix3D = joint.get_joint_frame()
904 | w_R_J = math_op.get_rotation_matrix(joint_frame) # represent joint-frame J's orientation w.r.t world-frame w
905 | J_R_w = math_op.matrix_transpose(w_R_J) # for a rotation matrix, its inverse is its transpose
906 |
907 | w_axis = [[w_axis[0]], [w_axis[1]], [w_axis[2]]] # transform from 1*3 to 3*1 list
908 | J_axis = math_op.change_orientation(J_R_w, w_axis)
909 | axis = [J_axis[0][0], J_axis[1][0], J_axis[2][0]]
910 |
911 | return axis
912 |
913 | def get_joint_limit(joint: Joint) -> list[float]:
914 | """
915 | required only for revolute and prismatic joint
916 | Return
917 | ---------
918 | limit: [lower, upper, effort, velocity]
919 | lower: optional, lower joint limit; unit: radian or m
920 | upper: optional, upper joint limit; unit: radian or m
921 | effort: required, the maximum joint effort, default: 1,000,000, unit: N or Nm
922 | velocity: required, the maximum joint velocity, default: 1,000,000, unit: m/s or rad/s
923 | if joint type is rigid, return None
924 | """
925 | if joint.joint.jointMotion.jointType == 0: # RigidJointType
926 | return None
927 | elif joint.joint.jointMotion.jointType == 1: # RevoluteJointType
928 | max_enabled = joint.joint.jointMotion.rotationLimits.isMaximumValueEnabled
929 | min_enabled = joint.joint.jointMotion.rotationLimits.isMinimumValueEnabled
930 | if max_enabled and min_enabled:
931 | # unit: radians
932 | lower_limit = joint.joint.jointMotion.rotationLimits.minimumValue
933 | upper_limit = joint.joint.jointMotion.rotationLimits.maximumValue
934 | return [round(lower_limit, 6), round(upper_limit, 6), 1_000_000, 1_000_000]
935 | else:
936 | return None
937 | elif joint.joint.jointMotion.jointType == 2: # SliderJointType
938 | max_enabled = joint.joint.jointMotion.slideLimits.isMaximumValueEnabled
939 | min_enabled = joint.joint.jointMotion.slideLimits.isMinimumValueEnabled
940 | if max_enabled and min_enabled:
941 | lower_limit = joint.joint.jointMotion.slideLimits.minimumValue * 0.01 # cm -> m
942 | upper_limit = joint.joint.jointMotion.slideLimits.maximumValue * 0.01 # cm -> m
943 | return [round(lower_limit, 6), round(upper_limit, 6), 1_000_000, 1_000_000]
944 | else:
945 | return None
946 |
947 | def get_joint_element(joint: Joint) -> Element:
948 | """
949 | Return
950 | ---------
951 | joint_ele: Element
952 | xml elements contains informations for joint
953 | """
954 | joint_ele = Element("joint")
955 | joint_ele.attrib = {"name": get_joint_name(joint),
956 | "type": get_joint_type(joint)}
957 |
958 | # add joint origin element
959 | origin = SubElement(joint_ele, "origin")
960 | origin.attrib = {"xyz": "{} {} {}".format(get_joint_origin(joint)[0], get_joint_origin(joint)[1], get_joint_origin(joint)[2]),
961 | "rpy": "{} {} {}".format(get_joint_origin(joint)[3], get_joint_origin(joint)[4], get_joint_origin(joint)[5])}
962 |
963 | # add parent and child element
964 | parent = SubElement(joint_ele, "parent")
965 | parent.attrib = {"link": get_link_name(get_joint_parent(joint))}
966 | child = SubElement(joint_ele, "child")
967 | child.attrib = {"link": get_link_name(get_joint_child(joint))}
968 |
969 | # add axis1, urdf only has one axis for joint
970 | axis = get_joint_axis(joint)
971 | if axis is not None:
972 | axis_ele = SubElement(joint_ele, "axis")
973 | axis_ele.attrib = {"xyz": "{} {} {}".format(axis[0], axis[1], axis[2])}
974 |
975 | # add limits
976 | limit = get_joint_limit(joint)
977 | if limit is not None:
978 | limit_ele = SubElement(joint_ele, "limit")
979 | limit_ele.attrib = {"lower": "{}".format(limit[0]),
980 | "upper": "{}".format(limit[1]),
981 | "effort": "{}".format(limit[2]),
982 | "velocity": "{}".format(limit[3])}
983 |
984 | return joint_ele
985 |
986 | def get_urdf(joint: Joint, link: Link) -> Element:
987 | """
988 |
989 | """
990 | pass
--------------------------------------------------------------------------------