diff --git a/Add-IN/ACDC4Robot/commands/ACDC4Robot/acdc4robot.py b/Add-IN/ACDC4Robot/commands/ACDC4Robot/acdc4robot.py
index aaefb29..74f39a6 100644
--- a/Add-IN/ACDC4Robot/commands/ACDC4Robot/acdc4robot.py
+++ b/Add-IN/ACDC4Robot/commands/ACDC4Robot/acdc4robot.py
@@ -33,12 +33,14 @@ def get_link_joint_list(design: adsk.fusion.Design):
     # try to solve the nested components problem
     # but still not fully tested
     for occ in occs:
+        if not utils.component_has_bodies(occ.component):
+            continue
         # TODO: it seems use occ.joints.count will make it usable with occurrences? Test it
         if occ.component.joints.count > 0:
             # textPalette.writeText(str(occ.fullPathName))
             continue
         else:
-            # Only occurrence contains zero joint and has zero childOccurrences 
+            # Only occurrence contains zero joint and has zero childOccurrences
             # can be seen as a link
             if occ.childOccurrences.count > 0:
                 # textPalette.writeText(str(occ.fullPathName))
@@ -53,6 +55,8 @@ def get_link_joint_list(design: adsk.fusion.Design):
 
     for joint in root.allJoints:
         joint_list.append(Joint(joint)) # add joint objects into joint_list
+    for joint in root.allAsBuiltJoints:
+        joint_list.append(Joint(joint)) # add joint objects into joint_list
 
     return link_list, joint_list
 
@@ -134,9 +138,6 @@ def run():
     constants.set_text_palette(textPalette)
 
     try:
-        # Set design type into do not capture design history
-        design.designType = adsk.fusion.DesignTypes.DirectDesignType
-
         # # Check the length unit of Fusion360
         # if design.unitsManager.defaultLengthUnits != "m":
         #     ui.messageBox("Please set length unit to 'm'!", msg_box_title)
@@ -177,29 +178,15 @@ def run():
             if simulator  == "None":
                 ui.messageBox("Simulation environment is None.\n" +
                               "Please select a simulation environment.", msg_box_title)
-            elif simulator == "Gazebo":
-                # write to .urdf file
-                write.write_urdf(link_list, joint_list, save_folder, robot_name)
-                # export mesh files
-                export_stl(design, save_folder, link_list)
-                ui.messageBox("Finished exporting URDF for Gazebo.", msg_box_title)
-                
-            elif simulator == "PyBullet":
+            elif simulator in ["Gazebo", "PyBullet", "MuJoCo"]:
                 # write to .urdf file
                 write.write_urdf(link_list, joint_list, save_folder, robot_name)
                 # export mesh files
                 export_stl(design, save_folder, link_list)
                 # generate pybullet script
-                write.write_hello_pybullet(rdf, robot_name, save_folder)
-                ui.messageBox("Finished exporting URDF for PyBullet.", msg_box_title)
-            
-            elif simulator == "MuJoCo":
-                # write to .urdf file
-                write.write_urdf(link_list, joint_list, save_folder, robot_name)
-                # export mesh files
-                export_stl(design, save_folder, link_list)
-                
-                ui.messageBox("Finished exporting URDF for MuJoCo.", msg_box_title)
+                if simulator == 'PyBullet':
+                    write.write_hello_pybullet(rdf, robot_name, save_folder)
+                ui.messageBox(f"Finished exporting URDF for {simulator}.", msg_box_title)
 
         elif rdf == "SDFormat":
             if simulator == "None":
diff --git a/Add-IN/ACDC4Robot/core/joint.py b/Add-IN/ACDC4Robot/core/joint.py
index 98d7577..d60700a 100644
--- a/Add-IN/ACDC4Robot/core/joint.py
+++ b/Add-IN/ACDC4Robot/core/joint.py
@@ -4,22 +4,28 @@
 
 """
 
+from typing import Union
 import adsk, adsk.fusion, adsk.core
 from xml.etree.ElementTree import ElementTree, Element, SubElement
+from ..commands.ACDC4Robot import constants
 from . import utils
 from . import math_operation as math_op
-# from .link import Link
 
 class Joint():
     """
     Joint class for joint
     """
 
-    def __init__(self, joint: adsk.fusion.Joint) -> None:
+    def __init__(self, joint: Union[adsk.fusion.Joint, adsk.fusion.AsBuiltJoint]) -> None:
         self.joint = joint
         self.name = joint.name
-        self.parent = joint.occurrenceTwo # parent link of joint
-        self.child = joint.occurrenceOne
+        try:
+            self.parent = joint.occurrenceTwo # parent link of joint
+            self.child = joint.occurrenceOne
+        except Exception as e:
+            textPalette: adsk.core.Palette = constants.get_text_palette()
+            textPalette.writeText(f"Invalid joint: {joint.name}: {str(e)}")
+            pass
         # self.parent = Link(joint.occurrenceTwo) # parent link of joint
         # self.child = Link(joint.occurrenceOne)
 
@@ -37,6 +43,19 @@ def get_name(self):
         name = utils.get_valid_filename(self.name)
         return name
 
+    def get_full_path_name(self):
+        """
+        Return:
+        path: str
+            The joint name and parent component path
+        """
+        # joint names inside each occurrence are identical
+        # but joint names in different occurrences can be the same, in which makes conflict
+        if self.child:
+            child_name = utils.get_valid_filename(self.child.fullPathName)
+            return f"{child_name}_{self.get_name()}"
+        return self.get_name()
+
     def get_parent(self):
         """
         Return name of parent link
@@ -59,6 +78,18 @@ def get_child(self):
 
         return child_name
 
+    def has_origin(self) -> bool:
+        """
+        Checks if the join has a parent origin geometry
+        Return: bool
+        """
+        if hasattr(self.joint, 'geometry'):
+            return self.joint.geometry is not None
+        elif self.joint.geometryOrOriginTwo == adsk.fusion.JointOrigin:
+            return self.joint.geometryOrOriginTwo.geometry is not None
+        else:
+            return self.joint.geometryOrOriginTwo.origin is not None
+
     def get_sdf_joint_type(self) -> str:
         """
         Currently support following joint type:
@@ -144,7 +175,9 @@ def get_sdf_origin(self):
         # I guess using child joint origin works just because they coincide together for all the text examples
 
         # get parent joint origin as the child joint
-        if self.joint.geometryOrOriginTwo == adsk.fusion.JointOrigin:
+        if hasattr(self.joint, 'geometry'):
+            w_P_Jc = self.joint.geometry.origin.asArray()
+        elif self.joint.geometryOrOriginTwo == adsk.fusion.JointOrigin:
             w_P_Jc = self.joint.geometryOrOriginTwo.geometry.origin.asArray()
         else:
             w_P_Jc = self.joint.geometryOrOriginTwo.origin.asArray()
@@ -182,17 +215,24 @@ def get_joint_frame(self) -> adsk.core.Matrix3D:
             translation unit: cm
         """
         # get parent joint origin's coordinate w.r.t world frame
-        if self.joint.geometryOrOriginTwo == adsk.fusion.JointOrigin:
-            w_P_J = self.joint.geometryOrOriginTwo.geometry.origin.asArray()
+        if hasattr(self.joint, 'geometry'):
+            geometry: adsk.fusion.JointGeometry = self.joint.geometry
+            if geometry is None:
+                return None
+            w_P_J = geometry.origin.asArray()
         else:
-            w_P_J = self.joint.geometryOrOriginTwo.origin.asArray()
+            geometry: adsk.fusion.JointOrigin = self.joint.geometryOrOriginTwo
+            if self.joint.geometryOrOriginTwo == adsk.fusion.JointOrigin:
+                w_P_J = geometry.geometry.origin.asArray()
+            else:
+                w_P_J = geometry.origin.asArray()
 
         w_P_J = [round(i, 6) for i in w_P_J]
         
         # no matter jointGeometry or jointOrigin object, both have these properties
-        zAxis: adsk.core.Vector3D = self.joint.geometryOrOriginTwo.primaryAxisVector
-        xAxis: adsk.core.Vector3D = self.joint.geometryOrOriginTwo.secondaryAxisVector
-        yAxis: adsk.core.Vector3D = self.joint.geometryOrOriginTwo.thirdAxisVector
+        zAxis: adsk.core.Vector3D = geometry.primaryAxisVector
+        xAxis: adsk.core.Vector3D = geometry.secondaryAxisVector
+        yAxis: adsk.core.Vector3D = geometry.thirdAxisVector
 
         origin = adsk.core.Point3D.create(w_P_J[0], w_P_J[1], w_P_J[2])
 
diff --git a/Add-IN/ACDC4Robot/core/link.py b/Add-IN/ACDC4Robot/core/link.py
index ffd88c2..0e17b37 100644
--- a/Add-IN/ACDC4Robot/core/link.py
+++ b/Add-IN/ACDC4Robot/core/link.py
@@ -14,10 +14,10 @@
 # TODO: support for external component
 
 class Link():
-    """ 
+    """
     Data abstruction for link element
     with some methods to access commonly used information
-    
+
     Attributes:
         link: adsk.fusion.Occurrence
             occurrence in Fusion360 refers to linkage
@@ -40,13 +40,13 @@ def __init__(self, occurrence: adsk.fusion.Occurrence) -> None:
         """
         # TODO: maybe this is a better and accurate way for this, not test
         # self.link = occurrence.createForAssemblyContext(occurrence)
-        
+
         self.link: adsk.fusion.Occurrence = occurrence
         self.name = None
         # pose can have two meanings:
         # 1. the homogeneous matrix of link-frame L w.r.t world-frame w
         # 2. the coordinates of link-frame L
-        self.pose: adsk.core.Matrix3D = occurrence.transform2 
+        self.pose: adsk.core.Matrix3D = occurrence.transform2
         self.phyPro = occurrence.getPhysicalProperties(adsk.fusion.CalculationAccuracy.VeryHighCalculationAccuracy)
 
     def get_link_occ(self) -> adsk.fusion.Occurrence:
@@ -57,6 +57,14 @@ def get_link_occ(self) -> adsk.fusion.Occurrence:
         """
         return self.link
 
+    def is_visible(self) -> bool:
+        """
+        Check if the occurrence has the light bulb on
+        Return:
+        bool
+        """
+        return self.link.isLightBulbOn
+
     def get_parent_joint(self) -> adsk.fusion.Joint:
         """
         Get the parent joint of a robot link
@@ -69,18 +77,14 @@ def get_parent_joint(self) -> adsk.fusion.Joint:
         # TODO: in closed loop mechanism, there exits one assembly method that
         # make one link have two parent joint, write a instruction or fix this bug
         # text_palette = constants.get_text_palette()
-        joint_list: adsk.fusion.JointList = self.link.joints
-        # text_palette.writeText("Link: {}".format(self.get_name()))
-        for j in joint_list:
-            # text_palette.writeText("Joint list item: {}".format(j.name))
-            # text_palette.writeText("Joint's child link: {}".format(j.occurrenceOne.fullPathName))
-            if j.occurrenceOne == self.link:
-                # text_palette.writeText("Return j type: {}".format(j.objectType))
-                return j
-            else:
-                continue
-
-        # text_palette.writeText("Return a None")
+        app = adsk.core.Application.get()
+        root = adsk.fusion.Design.cast(app.activeProduct).rootComponent
+        for joint in root.allJoints:
+            if joint.occurrenceOne == self.link:
+                return joint
+        for join in root.allAsBuiltJoints:
+            if join.occurrenceOne == self.link:
+                return join
         return None
 
     def get_name(self) -> str:
@@ -144,7 +148,7 @@ def get_inertia_sdf(self) -> list:
         inertia_tensor = [[com_ixx, com_ixy, com_ixz],
                           [com_ixy, com_iyy, com_iyz],
                           [com_ixz, com_iyz, com_izz]]
-        
+
         # moments of inertia has the same orientation of L-frame
         I = math_op.matrix_multi(math_op.matrix_multi(R_T, inertia_tensor), R)
         # I = math_op.matrix_multi(math_op.matrix_multi(R, inertia_tensor), R_T)
@@ -157,13 +161,13 @@ def get_inertia_sdf(self) -> list:
         ixz = I[0][2]
 
         return [ixx, iyy, izz, ixy, iyz, ixz] # unit: kg*m^2
-    
+
     def get_initia_urdf(self):
         """
         Return [ixx, iyy, izz, ixy, iyz, ixz]
         the inertia tensor elements of the link w.r.t the CoM frame C
         CoM frame C has the same orientation with the parent frame
-        
+
         Return:
         ---------
         list: [ixx, iyy, izz, ixy, iyz, ixz]
@@ -186,7 +190,7 @@ def get_initia_urdf(self):
         inertia_tensor = [[com_ixx, com_ixy, com_ixz],
                           [com_ixy, com_iyy, com_iyz],
                           [com_ixz, com_iyz, com_izz]]
-        
+
         # get the parent frame
         parent_joint = self.get_parent_joint()
         # parent frame for a link has parent joint, is the parent joint frame
@@ -220,7 +224,7 @@ def get_initia_urdf(self):
         ixz = I[0][2]
 
         return [ixx, iyy, izz, ixy, iyz, ixz] # unit: kg*m^2
-    
+
     def get_inertia_mjcf(self) -> list:
         """
         Get inertia tensor as mjcf's body subelement
@@ -261,7 +265,7 @@ def get_CoM_wrt_link(self):
         yaw = 0.0
         w_CoM_x = self.phyPro.centerOfMass.x # point CoM's x coordinate w.r.t world-frame
         w_CoM_y = self.phyPro.centerOfMass.y
-        w_CoM_z = self.phyPro.centerOfMass.z 
+        w_CoM_z = self.phyPro.centerOfMass.z
         w_Lo_x = self.pose.translation.x    # link-frame's origin point's x coordinat w.r.t world-frame
         w_Lo_y = self.pose.translation.y
         w_Lo_z = self.pose.translation.z
@@ -274,7 +278,7 @@ def get_CoM_wrt_link(self):
 
         L_Lo_CoM = math_op.change_orientation(L_R_w, w_Lo_CoM)
         pose_CoM = [L_Lo_CoM[0][0], L_Lo_CoM[1][0], L_Lo_CoM[2][0], roll, pitch, yaw]
-        
+
         return pose_CoM
 
     def get_CoM_sdf(self):
@@ -286,16 +290,16 @@ def get_CoM_sdf(self):
             Center-of-mass frame C relative to the link-frame L
         """
         # TODO: to avoid compatibility issues associated with the negative sign convention for product
-        # of inertia, align CoM frame C's axes to the principal inertia directions, so that all the 
+        # of inertia, align CoM frame C's axes to the principal inertia directions, so that all the
         # products of inertia are zero
-        
+
         # Let the orientation of center-of-mass frame C is same as link-frame L
         roll = 0.0
         pitch = 0.0
         yaw = 0.0
         w_CoM_x = self.phyPro.centerOfMass.x # point CoM's x coordinate w.r.t world-frame
         w_CoM_y = self.phyPro.centerOfMass.y
-        w_CoM_z = self.phyPro.centerOfMass.z 
+        w_CoM_z = self.phyPro.centerOfMass.z
         w_Lo_x = self.pose.translation.x    # link-frame's origin point's x coordinat w.r.t world-frame
         w_Lo_y = self.pose.translation.y
         w_Lo_z = self.pose.translation.z
@@ -308,9 +312,9 @@ def get_CoM_sdf(self):
 
         L_Lo_CoM = math_op.change_orientation(L_R_w, w_Lo_CoM)
         pose_CoM = [L_Lo_CoM[0][0], L_Lo_CoM[1][0], L_Lo_CoM[2][0], roll, pitch, yaw]
-        
+
         return pose_CoM
-    
+
     def get_CoM_urdf(self):
         """
         Get CoM frame w.r.t link frame in [x, y, z, roll, pitch, yaw] representation
@@ -331,10 +335,10 @@ def get_CoM_urdf(self):
             parent_joint_frame: adsk.core.Matrix3D = joint.get_joint_frame()
             from_origin, from_xAxis, from_yAxis, from_zAxis = parent_joint_frame.getAsCoordinateSystem()
             textPalette.writeText("Parent joint frame: \n"+math_op.matrix3D_to_str(parent_joint_frame))
-            
+
             w_CoM_x = self.phyPro.centerOfMass.x # point CoM's x coordinate w.r.t world-frame
             w_CoM_y = self.phyPro.centerOfMass.y
-            w_CoM_z = self.phyPro.centerOfMass.z 
+            w_CoM_z = self.phyPro.centerOfMass.z
 
             # Let the CoM-frame C has the same orientation with parent link frame
             CoM_frame_O: adsk.core.Point3D = adsk.core.Point3D.create()
@@ -348,7 +352,7 @@ def get_CoM_urdf(self):
             textPalette.writeText("CoM frame: \n" + math_op.matrix3D_to_str(CoM_frame))
 
             transform = adsk.core.Matrix3D.create()
-            # transform.setToAlignCoordinateSystems(from_origin, from_xAxis, from_yAxis, from_zAxis, 
+            # transform.setToAlignCoordinateSystems(from_origin, from_xAxis, from_yAxis, from_zAxis,
             #                                     to_origin, to_xAsix, to_yAxis, to_zAxis)
             transform = math_op.coordinate_transform(parent_joint_frame, CoM_frame)
             textPalette.writeText("Transform matrix: \n" + math_op.matrix3D_to_str(transform))
@@ -356,44 +360,6 @@ def get_CoM_urdf(self):
             pose_CoM = math_op.matrix3d_2_pose(transform)
 
         return pose_CoM
-    
-    def get_mesh_origin(self):
-        """
-        Fusion360 returns the mesh file with the same coordinate frame as the reference occurrence, which is the link fram L
-        So the mesh origin transforms the parent-joint frame J to the link-frame L
-        this is used for the mesh element of visual and collision in URDF
-        Return:
-        ---------
-        mesh_orign: list
-            [x, y, z, roll, pitch, yaw]
-        """
-        if self.get_parent_joint() is None:
-            # for the first link which does not have parent joint
-            # haven't found the reference about how to define the mesh origin
-            # it might be w.r.t world frame or link frame
-            # here let it be w.r.t link frame, which coincide with the link frame
-            mesh_origin = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
-            # transform = self.pose
-            # mesh_origin = math_op.matrix3d_2_pose(transform)
-        else:
-            joint = Joint(self.get_parent_joint())
-            parent_joint_frame: adsk.core.Matrix3D = joint.get_joint_frame()
-            textPalette: adsk.core.Palette = constants.get_text_palette()
-            textPalette.writeText("Parent joint frame: \n"+math_op.matrix3D_to_str(parent_joint_frame))
-            # link frame coincides with the mesh's frame
-            link_frame: adsk.core.Matrix3D = self.pose
-            textPalette.writeText("Link frame: \n" + math_op.matrix3D_to_str(link_frame))
-            from_origin, from_xAxis, from_yAxis, from_zAxis = parent_joint_frame.getAsCoordinateSystem()
-            to_origin, to_xAsix, to_yAxis, to_zAxis = link_frame.getAsCoordinateSystem()
-
-            transform = adsk.core.Matrix3D.create()
-            # transform.setToAlignCoordinateSystems(from_origin, from_xAxis, from_yAxis, from_zAxis, 
-            #                                     to_origin, to_xAsix, to_yAxis, to_zAxis)
-            transform = math_op.coordinate_transform(parent_joint_frame, link_frame)
-            # textPalette.writeText("Transform matrix: \n" + math_op.matrix3D_to_str(transform))
-            mesh_origin = math_op.matrix3d_2_pose(transform)
-
-        return mesh_origin
 
     def get_collision_sdf(self):
         """
@@ -409,7 +375,7 @@ def get_collision_sdf(self):
         robot_name = constants.get_robot_name()
         geometry_uri = "model://" + robot_name + "/meshes/" + str(self.get_name()) + ".stl"
         return col_name, geometry_uri
-    
+
     def get_collision_urdf(self):
         """
         Get colision info for urdf collision element
@@ -438,7 +404,7 @@ def get_visual_sdf(self):
         robot_name = constants.get_robot_name()
         geometry_uri = "model://" + robot_name + "/meshes/" + str(self.get_name()) + ".stl"
         return visual_name, geometry_uri
-    
+
     def get_visual_urdf(self):
         """
         Return:
@@ -451,7 +417,7 @@ def get_visual_urdf(self):
         # mesh_loc = "package://" + robot_name + "/meshes/" + self.get_name() + ".stl"
         mesh_loc = "meshes/" + self.get_name() + ".stl"
         return visual_name, mesh_loc
-    
+
     def get_visual_body(self) -> adsk.fusion.BRepBody:
         """
         get body that contains 'visual' in its name
@@ -467,15 +433,15 @@ def get_visual_body(self) -> adsk.fusion.BRepBody:
             if b_name.find('visual') != -1:
                 visual_geo: adsk.fusion.BRepBody = body
                 return visual_geo
-        
+
         return None
-    
+
     def get_collision_body(self) -> adsk.fusion.BRepBody:
         """
         get body that contains 'collision' in its name
         which means they are used as collision geometry
 
-        Return: 
+        Return:
         col_geo: BRepBody
             the body represents collision geometry
         None
@@ -485,5 +451,5 @@ def get_collision_body(self) -> adsk.fusion.BRepBody:
             if b_name.find('collision') != -1:
                 col_geo: adsk.fusion.BRepBody = body
                 return col_geo
-        
+
         return None
diff --git a/Add-IN/ACDC4Robot/core/mjcf.py b/Add-IN/ACDC4Robot/core/mjcf.py
index 2bfc6ea..eb9de83 100644
--- a/Add-IN/ACDC4Robot/core/mjcf.py
+++ b/Add-IN/ACDC4Robot/core/mjcf.py
@@ -161,6 +161,13 @@ def get_mjcf_joint(joint: Joint) -> Element:
         joint_ele.attrib = {"name": name_att, "type": joint_type, 
                             "axis": "{} {} {}".format(axis[0], axis[1], axis[2]),
                             "pos": "{} {} {}".format(pose[0], pose[1], pose[2])}
+
+
+        limits = joint.get_limits()
+        if limits is not None:
+            lower, upper = limits
+            joint_ele.attrib["range"] = "{} {}".format(lower, upper)
+            joint_ele.attrib["limited"] = "true"
         return joint_ele
     else:
         return None
@@ -206,13 +213,14 @@ def get_mjcf(root_comp: adsk.fusion.Component, robot_name: str, dir: str) -> Ele
 
     # add body elements to construct a robot
     parent_child_dict = {}
-    joints = root_comp.allJoints
     all_occs = root_comp.allOccurrences
 
+    joints = [j for j in root_comp.allJoints] + [j for j in root_comp.allAsBuiltJoints]
     for joint in joints:
         parent = joint.occurrenceTwo
         child = joint.occurrenceOne
-
+        if parent is None:
+            continue
         if parent.fullPathName not in parent_child_dict:
             parent_child_dict[parent.fullPathName] = []
 
@@ -236,6 +244,8 @@ def add_body_element(parent: adsk.fusion.Occurrence, parent_body_ele: Element) -
     
     # traverse all the occs for body elements
     for occ in all_occs:
+        if not occ.isLightBulbOn or not utils.component_has_bodies(occ.component):
+            continue
         asset_ele.append(get_mjcf_mesh(Link(occ)))
         if occ.fullPathName not in [item.fullPathName for sublist in parent_child_dict.values() for item in sublist]:
             # parent_ele = ET.SubElement(worldbody_ele, "body", name=Link(occ).get_name())
diff --git a/Add-IN/ACDC4Robot/core/robot.py b/Add-IN/ACDC4Robot/core/robot.py
index 4f99387..a49fce4 100644
--- a/Add-IN/ACDC4Robot/core/robot.py
+++ b/Add-IN/ACDC4Robot/core/robot.py
@@ -3,6 +3,7 @@
 import adsk, adsk.fusion, adsk.core
 from .link import Link
 from .joint import Joint
+from . import utils
 
 class Robot():
     """
@@ -31,6 +32,8 @@ def get_links(self, ) -> List[Link]:
         # try to solve the nested components problem
         # but still not fully tested
         for occ in occs:
+            if not utils.component_has_bodies(occ.component):
+                continue
             # TODO: it seems use occ.joints.count will make it usable with occurrences? Test it
             if occ.component.joints.count > 0:
                 # textPalette.writeText(str(occ.fullPathName))
@@ -56,6 +59,8 @@ def get_joints(self, ) -> List[Joint]:
 
         for joint in self.rootComp.allJoints:
             joint_list.append(Joint(joint)) # add joint objects into joint_list
+        for joint in self.rootComp.allAsBuiltJoints:
+            joint_list.append(Joint(joint)) # add joint objects into joint_list
         
         return joint_list
 
diff --git a/Add-IN/ACDC4Robot/core/sdf.py b/Add-IN/ACDC4Robot/core/sdf.py
index d483712..88dfbcb 100644
--- a/Add-IN/ACDC4Robot/core/sdf.py
+++ b/Add-IN/ACDC4Robot/core/sdf.py
@@ -283,44 +283,6 @@ def get_joint_type(joint: Joint) -> str:
         pass
     return sdf_joint_type
 
-def get_joint_pose(joint: Joint) -> list[float]:
-    """
-    Return
-    ---------
-    joint_pose: [x, y, z, roll, pitch, yaw]
-        From: http://sdformat.org/tutorials?tut=spec_model_kinematics&cat=specification&#jointpose
-        For a joint with parent link frame `P` and child link frame `C`, 
-        the joint `<pose>` tag specifies the pose `X_CJc` of a joint frame `Jc` rigidly attached to the child link.
-    """
-    # get parent joint origin's coordinate w.r.t world frame
-    # get parent joint origin as the child joint frame
-    # NOTE: joint origin is a concept in Fusion360
-    if joint.joint.geometryOrOriginTwo == adsk.fusion.JointOrigin:
-        w_P_Jc = joint.joint.geometryOrOriginTwo.geometry.origin.asArray()
-    else:
-        w_P_Jc = joint.joint.geometryOrOriginTwo.origin.asArray()
-
-    # convert from cm to m
-    w_P_Jc = [round(i*0.01, 6) for i in w_P_Jc] 
-    # get child link frame's origin point w.r.t world frame
-    w_P_Lc = [joint.child.transform2.translation.x * 0.01, 
-                joint.child.transform2.translation.y * 0.01,
-                joint.child.transform2.translation.z * 0.01,]
-    # vector from child link frame's origin point to child joint origin point w.r.t world frame
-    w_V_LcJc = [[w_P_Jc[0]-w_P_Lc[0]],
-                [w_P_Jc[1]-w_P_Lc[1]],
-                [w_P_Jc[2]-w_P_Lc[2]]] # 3*1 vector
-
-    w_T_Lc = joint.child.transform2 # configuration of child-link-frame w.r.t world-frame w
-    w_R_Lc = math_op.get_rotation_matrix(w_T_Lc) # rotation matrix of child-link-frame w.r.t world-frame w
-    Lc_R_w = math_op.matrix_transpose(w_R_Lc) # rotation matrix of world-frame w.r.t child-link-frame Lc
-    # vector from child link frame's origin point to child joint origin point w.r.t child-link-frame Lc
-    Lc_V_LcJc = math_op.change_orientation(Lc_R_w, w_V_LcJc) # 3*1 array
-    # assume the joint frame has the same oritation as child link frame
-    # it seems that rpy of joint doesn't matter, so set them as 0
-    sdf_origin = [Lc_V_LcJc[0][0], Lc_V_LcJc[1][0], Lc_V_LcJc[2][0], 0.0, 0.0, 0.0]
-
-    return sdf_origin
 
 def get_joint_parent(joint: Joint) -> Link:
     """
@@ -417,9 +379,10 @@ def get_joint_element(joint: Joint) -> list[float]:
     joint_ele = Element("joint")
     joint_ele.attrib = {"name": get_joint_name(joint), "type": get_joint_type(joint)}
     pose = SubElement(joint_ele, "pose")
-    pose.text = "{} {} {} {} {} {}".format(get_joint_pose(joint)[0], get_joint_pose(joint)[1],
-                                           get_joint_pose(joint)[2], get_joint_pose(joint)[3],
-                                           get_joint_pose(joint)[4], get_joint_pose(joint)[5])
+    poseValues = joint.get_sdf_origin()
+    pose.text = "{} {} {} {} {} {}".format(poseValues[0], poseValues[1],
+                                           poseValues[2], poseValues[3],
+                                           poseValues[4], poseValues[5])
     parent = SubElement(joint_ele, "parent")
     parent.text = "{}".format(get_joint_parent(joint).get_name())
     child = SubElement(joint_ele, "child")
@@ -443,4 +406,4 @@ def get_joint_element(joint: Joint) -> list[float]:
     if axis2 is not None:
         pass
 
-    return joint_ele
\ No newline at end of file
+    return joint_ele
diff --git a/Add-IN/ACDC4Robot/core/urdf.py b/Add-IN/ACDC4Robot/core/urdf.py
index 607052b..6f19586 100644
--- a/Add-IN/ACDC4Robot/core/urdf.py
+++ b/Add-IN/ACDC4Robot/core/urdf.py
@@ -10,7 +10,6 @@
 import xml.etree.ElementTree as ET
 from . import math_operation as math_op
 from . import utils
-from ..commands.ACDC4Robot import constants
 from .robot import Robot
 
 class URDF():
@@ -30,11 +29,12 @@ def write_file(self, file_path):
         for link in self.links:
             link_ele = self.get_link_element(link)
             robot_ele.append(link_ele)
-        
+
         for joint in self.tree_joints:
             joint_ele = self.get_joint_element(joint)
-            robot_ele.append(joint_ele)
-        
+            if joint_ele is not None:
+                robot_ele.append(joint_ele)
+
         # set indent to pretty the xml output
         ET.indent(urdf_tree, space="    ", level=0)
 
@@ -54,6 +54,7 @@ def get_link_element(self, link: Link) -> Element:
         # create a link element
         link_ele = Element("link")
         link_ele.attrib = {"name": self.get_link_name(link)}
+        mesh_origin = self.get_mesh_origin(link)
 
         # add inertia sub-element
         inertial = SubElement(link_ele, "inertial")
@@ -69,13 +70,13 @@ def get_link_element(self, link: Link) -> Element:
                         "ixy": "{}".format(self.get_link_inertia(link)[3]),
                         "iyz": "{}".format(self.get_link_inertia(link)[4]),
                         "ixz": "{}".format(self.get_link_inertia(link)[5])}
-        
+
         # add visual sub-element
         visual = SubElement(link_ele, "visual")
         visual.attrib = {"name": "{}".format(self.get_link_visual_name(link))}
         origin_v = SubElement(visual, "origin")
-        origin_v.attrib = {"xyz": "{} {} {}".format(self.get_mesh_origin(link)[0], self.get_mesh_origin(link)[1], self.get_mesh_origin(link)[2]),
-                        "rpy": "{} {} {}".format(self.get_mesh_origin(link)[3], self.get_mesh_origin(link)[4], self.get_mesh_origin(link)[5])}
+        origin_v.attrib = {"xyz": "{} {} {}".format(mesh_origin[0], mesh_origin[1], mesh_origin[2]),
+                        "rpy": "{} {} {}".format(mesh_origin[3], mesh_origin[4], mesh_origin[5])}
         geometry_v = SubElement(visual, "geometry")
         mesh_v = SubElement(geometry_v, "mesh")
         mesh_v.attrib = {"filename": self.get_link_visual_geo(link), "scale": "0.001 0.001 0.001"}
@@ -84,8 +85,8 @@ def get_link_element(self, link: Link) -> Element:
         collision = SubElement(link_ele, "collision")
         collision.attrib = {"name": "{}".format(self.get_link_collision_name(link))}
         origin_c = SubElement(collision, "origin")
-        origin_c.attrib = {"xyz": "{} {} {}".format(self.get_mesh_origin(link)[0], self.get_mesh_origin(link)[1], self.get_mesh_origin(link)[2]),
-                        "rpy": "{} {} {}".format(self.get_mesh_origin(link)[3], self.get_mesh_origin(link)[4], self.get_mesh_origin(link)[5])}
+        origin_c.attrib = {"xyz": "{} {} {}".format(mesh_origin[0], mesh_origin[1], mesh_origin[2]),
+                        "rpy": "{} {} {}".format(mesh_origin[3], mesh_origin[4], mesh_origin[5])}
         geometry_c = SubElement(collision, "geometry")
         mesh_c = SubElement(geometry_c, "mesh")
         mesh_c.attrib = {"filename": self.get_link_collision_geo(link), "scale": "0.001 0.001 0.001"}
@@ -100,19 +101,25 @@ def get_joint_element(self, joint: Joint) -> Element:
             xml elements contains informations for joint
         """
         joint_ele = Element("joint")
-        joint_ele.attrib = {"name": self.get_joint_name(joint),
+        joint_ele.attrib = {"name": joint.get_full_path_name(),
                             "type": self.get_joint_type(joint)}
-        
+        parentLink = self.get_joint_parent(joint)
+        childLink = self.get_joint_child(joint)
+        if not parentLink.is_visible() or not childLink.is_visible():
+            return None
+
         # add joint origin element
-        origin = SubElement(joint_ele, "origin")
-        origin.attrib = {"xyz": "{} {} {}".format(self.get_joint_origin(joint)[0], self.get_joint_origin(joint)[1], self.get_joint_origin(joint)[2]),
-                        "rpy": "{} {} {}".format(self.get_joint_origin(joint)[3], self.get_joint_origin(joint)[4], self.get_joint_origin(joint)[5])}
-        
+        if joint.has_origin():
+            origin = SubElement(joint_ele, "origin")
+            originCoords = self.get_joint_origin(joint)
+            origin.attrib = {"xyz": "{} {} {}".format(originCoords[0], originCoords[1], originCoords[2]),
+                            "rpy": "{} {} {}".format(originCoords[3], originCoords[4], originCoords[5])}
+
         # add parent and child element
         parent = SubElement(joint_ele, "parent")
-        parent.attrib = {"link": self.get_link_name(self.get_joint_parent(joint))}
+        parent.attrib = {"link": self.get_link_name(parentLink)}
         child = SubElement(joint_ele, "child")
-        child.attrib = {"link": self.get_link_name(self.get_joint_child(joint))}
+        child.attrib = {"link": self.get_link_name(childLink)}
 
         # add axis1, urdf only has one axis for joint
         axis = self.get_joint_axis(joint)
@@ -128,7 +135,7 @@ def get_joint_element(self, joint: Joint) -> Element:
                                 "upper": "{}".format(limit[1]),
                                 "effort": "{}".format(limit[2]),
                                 "velocity": "{}".format(limit[3])}
-        
+
         return joint_ele
 
     def get_robot_ele(self, ):
@@ -140,10 +147,10 @@ def get_robot_ele(self, ):
 
         return robot_ele
 
-    
+
     def get_link_name(self, link: Link) -> str:
         """
-        Return 
+        Return
         ---------
         name: str
             link's full path name
@@ -160,7 +167,7 @@ def get_link_inertial_origin(self, link: Link) -> list[float]:
             unit: m, radian
         """
         # the link is the first link so called base-link
-        if link.get_parent_joint() is None:
+        if not joint_has_geometry(link.get_parent_joint()):
             # for the first link which does not have parent joint
             # do not know the CoM is w.r.t world frame or the link frame
             # do not found the details from the description, but according to the figure from:
@@ -173,7 +180,7 @@ def get_link_inertial_origin(self, link: Link) -> list[float]:
             yaw = 0.0
             w_CoM_x = link.phyPro.centerOfMass.x # point CoM's x coordinate w.r.t world-frame
             w_CoM_y = link.phyPro.centerOfMass.y
-            w_CoM_z = link.phyPro.centerOfMass.z 
+            w_CoM_z = link.phyPro.centerOfMass.z
             w_Lo_x = link.pose.translation.x    # link-frame's origin point's x coordinat w.r.t world-frame
             w_Lo_y = link.pose.translation.y
             w_Lo_z = link.pose.translation.z
@@ -192,10 +199,10 @@ def get_link_inertial_origin(self, link: Link) -> list[float]:
             joint = Joint(link.get_parent_joint())
             parent_joint_frame: adsk.core.Matrix3D = joint.get_joint_frame()
             from_origin, from_xAxis, from_yAxis, from_zAxis = parent_joint_frame.getAsCoordinateSystem()
-            
+
             w_CoM_x = link.phyPro.centerOfMass.x # point CoM's x coordinate w.r.t world-frame
             w_CoM_y = link.phyPro.centerOfMass.y
-            w_CoM_z = link.phyPro.centerOfMass.z 
+            w_CoM_z = link.phyPro.centerOfMass.z
 
             # Let the CoM-frame C has the same orientation with parent link frame
             CoM_frame_O: adsk.core.Point3D = adsk.core.Point3D.create()
@@ -205,10 +212,6 @@ def get_link_inertial_origin(self, link: Link) -> list[float]:
             CoM_frame_y: adsk.core.Vector3D = from_yAxis
             CoM_frame_z: adsk.core.Vector3D = from_zAxis
             CoM_frame.setWithCoordinateSystem(CoM_frame_O, CoM_frame_x, CoM_frame_y, CoM_frame_z)
-            to_origin, to_xAsix, to_yAxis, to_zAxis = CoM_frame.getAsCoordinateSystem()
-            transform = adsk.core.Matrix3D.create()
-            # transform.setToAlignCoordinateSystems(from_origin, from_xAxis, from_yAxis, from_zAxis, 
-            #                                     to_origin, to_xAsix, to_yAxis, to_zAxis)
             transform = math_op.coordinate_transform(parent_joint_frame, CoM_frame)
 
             inertial_origin = math_op.matrix3d_2_pose(transform)
@@ -253,12 +256,12 @@ def get_link_inertia(self, link: Link) -> list[float]:
         inertia_tensor = [[com_ixx, com_ixy, com_ixz],
                             [com_ixy, com_iyy, com_iyz],
                             [com_ixz, com_iyz, com_izz]]
-        
+
         # get the parent frame
         parent_joint = link.get_parent_joint()
         # parent frame for a link has parent joint, is the parent joint frame
         # else, is the link frame itselt
-        if parent_joint is None:
+        if not joint_has_geometry(parent_joint):
             parent_frame: adsk.core.Matrix3D = link.pose
         else:
             joint = Joint(parent_joint)
@@ -291,6 +294,27 @@ def get_link_collision_name(self, link: Link) -> str:
         collision_name = link.get_name() + "_collision"
         return collision_name
 
+    def get_relative_link_frame(self, link: Link) -> adsk.core.Matrix3D:
+        """
+        Get the coordinate frame for a link, relative to it's parent joint.
+        NOTE: Rigid as-built joints don't have an origin, so we need to traverse up the tree
+        until we find a joint origin, and then relatively transform the frame from there
+        """
+        link_frame: adsk.core.Matrix3D = link.pose
+        if link.get_parent_joint() is None:
+            return link.pose
+        else:
+            joint = Joint(link.get_parent_joint())
+            link_frame: adsk.core.Matrix3D = link.pose
+            if joint.has_origin():
+                parent_joint_frame: adsk.core.Matrix3D = joint.get_joint_frame()
+                return math_op.coordinate_transform(parent_joint_frame, link_frame)
+            else:
+                # Traverse up to the next parent link
+                parent_link = Link(joint.parent)
+                parent_frame = self.get_relative_link_frame(parent_link)
+                return math_op.coordinate_transform(parent_frame, link_frame)
+
     def get_mesh_origin(self, link: Link) -> list[float]:
         """
         The reference frame for mesh element of visual and collision w.r.t link frame L
@@ -299,24 +323,8 @@ def get_mesh_origin(self, link: Link) -> list[float]:
         mesh_origin: [x, y, z, roll, pitch, yaw]
             unit: m, radian
         """
-        if link.get_parent_joint() is None:
-            # for the first link which does not have parent joint
-            # the mesh frame coincides with the link frame L
-            mesh_origin = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
-        else:
-            joint = Joint(link.get_parent_joint())
-            parent_joint_frame: adsk.core.Matrix3D = joint.get_joint_frame()
-            # link frame coincides with the mesh's frame
-            link_frame: adsk.core.Matrix3D = link.pose
-            from_origin, from_xAxis, from_yAxis, from_zAxis = parent_joint_frame.getAsCoordinateSystem()
-            to_origin, to_xAsix, to_yAxis, to_zAxis = link_frame.getAsCoordinateSystem()
-
-            transform = adsk.core.Matrix3D.create()
-            # transform.setToAlignCoordinateSystems(from_origin, from_xAxis, from_yAxis, from_zAxis, 
-            #                                     to_origin, to_xAsix, to_yAxis, to_zAxis)
-            transform = math_op.coordinate_transform(parent_joint_frame, link_frame)
-            mesh_origin = math_op.matrix3d_2_pose(transform)
-
+        frame = self.get_relative_link_frame(link)
+        mesh_origin = math_op.matrix3d_2_pose(frame)
         return mesh_origin
 
     def get_link_visual_geo(self, link: Link) -> str:
@@ -374,7 +382,7 @@ def get_link_collision_geo(self, link: Link) -> str:
             utils.error_box(error_message)
             utils.terminate_box()
 
-    
+
     def get_joint_name(self, joint: Joint) -> str:
         """
         Return
@@ -410,25 +418,25 @@ def get_joint_origin(self, joint: Joint) -> list[float]:
             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)
             unit: m, radian
         """
-        parent_link: adsk.fusion.Occurrence = joint.parent
-        parent_link: Link = Link(parent_link)
+        parent_link: Link = Link(joint.parent)
         parent_joint: adsk.fusion.Joint = parent_link.get_parent_joint()
 
         # get the parent frame
-        if parent_joint is None:
-            # if the parent link does not have parent joint(which means the root link), 
+        if not joint_has_geometry(parent_joint):
+            # if the parent link does not have parent joint(which means the root link),
             # then the parent link frame is the parent frame
             parent_frame: adsk.core.Matrix3D = parent_link.pose
         else:
             parent_joint: Joint = Joint(parent_joint)
             parent_frame = parent_joint.get_joint_frame()
-        
-        # get joint frame w.r.t world frame
-        joint_frame = joint.get_joint_frame()
 
-        transform = adsk.core.Matrix3D.create()
-        transform = math_op.coordinate_transform(parent_frame, joint_frame)
-        joint_origin = math_op.matrix3d_2_pose(transform)
+        # get joint frame w.r.t world frame
+        if joint.has_origin():
+            joint_frame = joint.get_joint_frame()
+            transform = math_op.coordinate_transform(parent_frame, joint_frame)
+            joint_origin = math_op.matrix3d_2_pose(transform)
+        else:
+            joint_origin = math_op.matrix3d_2_pose(parent_frame)
 
         return joint_origin
 
@@ -464,14 +472,14 @@ def get_joint_axis(self, joint: Joint) -> list[float]:
         """
         # urdf joint axis is expressed w.r.t parent frame, which is the link frame or the parent joint frame
         if joint.joint.jointMotion.jointType == 0: # RigidJointType
-            w_axis = None 
+            w_axis = None
             axis = None
             return axis
         elif joint.joint.jointMotion.jointType == 1: # RevoluteJointType
             w_axis = [round(i, 6) for i in joint.joint.jointMotion.rotationAxisVector.asArray()] # In Fusion360, returned axis is normalized
         elif joint.joint.jointMotion.jointType == 2: # SliderJointType
             w_axis = [round(i, 6) for i in joint.joint.jointMotion.slideDirectionVector.asArray()] # In Fusion360, returned axis is normalized
-        
+
         J_axis = None
         joint_frame: adsk.core.Matrix3D = joint.get_joint_frame()
         w_R_J = math_op.get_rotation_matrix(joint_frame) # represent joint-frame J's orientation w.r.t world-frame w
@@ -517,12 +525,12 @@ def get_joint_limit(self, joint: Joint) -> list[float]:
             else:
                 return None
 
-    
+
 
 
 def get_link_name(link: Link) -> str:
     """
-    Return 
+    Return
     ---------
     name: str
         link's full path name
@@ -530,6 +538,15 @@ def get_link_name(link: Link) -> str:
     name = link.get_name()
     return name
 
+def joint_has_geometry(joint: adsk.fusion.Joint) -> bool:
+    """
+    Does this joint have origin geometry?
+    If it's an as-built rigig joint, it does not have origin geometry.
+    """
+    if joint is None:
+        return False
+    return Joint(joint).has_origin()
+
 def get_link_inertial_origin(link: Link) -> list[float]:
     """
     Return
@@ -539,7 +556,7 @@ def get_link_inertial_origin(link: Link) -> list[float]:
         unit: m, radian
     """
     # the link is the first link so called base-link
-    if link.get_parent_joint() is None:
+    if not joint_has_geometry(link.get_parent_joint()):
         # for the first link which does not have parent joint
         # do not know the CoM is w.r.t world frame or the link frame
         # do not found the details from the description, but according to the figure from:
@@ -552,7 +569,7 @@ def get_link_inertial_origin(link: Link) -> list[float]:
         yaw = 0.0
         w_CoM_x = link.phyPro.centerOfMass.x # point CoM's x coordinate w.r.t world-frame
         w_CoM_y = link.phyPro.centerOfMass.y
-        w_CoM_z = link.phyPro.centerOfMass.z 
+        w_CoM_z = link.phyPro.centerOfMass.z
         w_Lo_x = link.pose.translation.x    # link-frame's origin point's x coordinat w.r.t world-frame
         w_Lo_y = link.pose.translation.y
         w_Lo_z = link.pose.translation.z
@@ -571,10 +588,10 @@ def get_link_inertial_origin(link: Link) -> list[float]:
         joint = Joint(link.get_parent_joint())
         parent_joint_frame: adsk.core.Matrix3D = joint.get_joint_frame()
         from_origin, from_xAxis, from_yAxis, from_zAxis = parent_joint_frame.getAsCoordinateSystem()
-        
+
         w_CoM_x = link.phyPro.centerOfMass.x # point CoM's x coordinate w.r.t world-frame
         w_CoM_y = link.phyPro.centerOfMass.y
-        w_CoM_z = link.phyPro.centerOfMass.z 
+        w_CoM_z = link.phyPro.centerOfMass.z
 
         # Let the CoM-frame C has the same orientation with parent link frame
         CoM_frame_O: adsk.core.Point3D = adsk.core.Point3D.create()
@@ -586,7 +603,7 @@ def get_link_inertial_origin(link: Link) -> list[float]:
         CoM_frame.setWithCoordinateSystem(CoM_frame_O, CoM_frame_x, CoM_frame_y, CoM_frame_z)
         to_origin, to_xAsix, to_yAxis, to_zAxis = CoM_frame.getAsCoordinateSystem()
         transform = adsk.core.Matrix3D.create()
-        # transform.setToAlignCoordinateSystems(from_origin, from_xAxis, from_yAxis, from_zAxis, 
+        # transform.setToAlignCoordinateSystems(from_origin, from_xAxis, from_yAxis, from_zAxis,
         #                                     to_origin, to_xAsix, to_yAxis, to_zAxis)
         transform = math_op.coordinate_transform(parent_joint_frame, CoM_frame)
 
@@ -632,12 +649,12 @@ def get_link_inertia(link: Link) -> list[float]:
     inertia_tensor = [[com_ixx, com_ixy, com_ixz],
                         [com_ixy, com_iyy, com_iyz],
                         [com_ixz, com_iyz, com_izz]]
-    
+
     # get the parent frame
     parent_joint = link.get_parent_joint()
     # parent frame for a link has parent joint, is the parent joint frame
     # else, is the link frame itselt
-    if parent_joint is None:
+    if not joint_has_geometry(parent_joint):
         parent_frame: adsk.core.Matrix3D = link.pose
     else:
         joint = Joint(parent_joint)
@@ -679,22 +696,16 @@ def get_mesh_origin(link: Link) -> list[float]:
         unit: m, radian
     """
     if link.get_parent_joint() is None:
-        # for the first link which does not have parent joint
-        # the mesh frame coincides with the link frame L
-        mesh_origin = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+        mesh_origin = math_op.matrix3d_2_pose(link.pose)
     else:
         joint = Joint(link.get_parent_joint())
-        parent_joint_frame: adsk.core.Matrix3D = joint.get_joint_frame()
-        # link frame coincides with the mesh's frame
-        link_frame: adsk.core.Matrix3D = link.pose
-        from_origin, from_xAxis, from_yAxis, from_zAxis = parent_joint_frame.getAsCoordinateSystem()
-        to_origin, to_xAsix, to_yAxis, to_zAxis = link_frame.getAsCoordinateSystem()
+        if joint.has_origin():
+            parent_joint_frame: adsk.core.Matrix3D = joint.get_joint_frame()
+            transform = math_op.coordinate_transform(parent_joint_frame, link.pose)
+            mesh_origin = math_op.matrix3d_2_pose(transform)
+        else:
+            mesh_origin = math_op.matrix3d_2_pose(link.pose)
 
-        transform = adsk.core.Matrix3D.create()
-        # transform.setToAlignCoordinateSystems(from_origin, from_xAxis, from_yAxis, from_zAxis, 
-        #                                     to_origin, to_xAsix, to_yAxis, to_zAxis)
-        transform = math_op.coordinate_transform(parent_joint_frame, link_frame)
-        mesh_origin = math_op.matrix3d_2_pose(transform)
 
     return mesh_origin
 
@@ -767,18 +778,21 @@ def get_link_element(link: Link) -> Element:
     # add inertia sub-element
     inertial = SubElement(link_ele, "inertial")
     origin = SubElement(inertial, "origin")
-    origin.attrib = {"xyz": "{} {} {}".format(get_link_inertial_origin(link)[0], get_link_inertial_origin(link)[1], get_link_inertial_origin(link)[2]),
-                     "rpy": "{} {} {}".format(get_link_inertial_origin(link)[3], get_link_inertial_origin(link)[4], get_link_inertial_origin(link)[5])}
+    originCoords = get_link_inertial_origin(link)
+    origin.attrib = {"xyz": "{} {} {}".format(originCoords[0], originCoords[1], originCoords[2]),
+                     "rpy": "{} {} {}".format(originCoords[3], originCoords[4], originCoords[5])}
+
     mass = SubElement(inertial, "mass")
     mass.attrib = {"value": "{}".format(get_link_mass(link))}
+    inertiaCoords = get_link_inertia(link)
     inertia = SubElement(inertial, "inertia")
-    inertia.attrib = {"ixx": "{}".format(get_link_inertia(link)[0]),
-                      "iyy": "{}".format(get_link_inertia(link)[1]),
-                      "izz": "{}".format(get_link_inertia(link)[2]),
-                      "ixy": "{}".format(get_link_inertia(link)[3]),
-                      "iyz": "{}".format(get_link_inertia(link)[4]),
-                      "ixz": "{}".format(get_link_inertia(link)[5])}
-    
+    inertia.attrib = {"ixx": "{}".format(inertiaCoords[0]),
+                      "iyy": "{}".format(inertiaCoords[1]),
+                      "izz": "{}".format(inertiaCoords[2]),
+                      "ixy": "{}".format(inertiaCoords[3]),
+                      "iyz": "{}".format(inertiaCoords[4]),
+                      "ixz": "{}".format(inertiaCoords[5])}
+
     # add visual sub-element
     visual = SubElement(link_ele, "visual")
     visual.attrib = {"name": "{}".format(get_link_visual_name(link))}
@@ -842,20 +856,21 @@ def get_joint_origin(joint: Joint) -> list[float]:
     parent_joint: adsk.fusion.Joint = parent_link.get_parent_joint()
 
     # get the parent frame
-    if parent_joint is None:
-        # if the parent link does not have parent joint(which means the root link), 
+    if not joint_has_geometry(parent_joint):
+        # if the parent link does not have parent joint(which means the root link),
         # then the parent link frame is the parent frame
         parent_frame: adsk.core.Matrix3D = parent_link.pose
     else:
         parent_joint: Joint = Joint(parent_joint)
         parent_frame = parent_joint.get_joint_frame()
-    
-    # get joint frame w.r.t world frame
-    joint_frame = joint.get_joint_frame()
 
-    transform = adsk.core.Matrix3D.create()
-    transform = math_op.coordinate_transform(parent_frame, joint_frame)
-    joint_origin = math_op.matrix3d_2_pose(transform)
+    # get joint frame w.r.t world frame
+    if joint.has_origin():
+        joint_frame = joint.get_joint_frame()
+        transform = math_op.coordinate_transform(parent_frame, joint_frame)
+        joint_origin = math_op.matrix3d_2_pose(transform)
+    else:
+        joint_origin = math_op.matrix3d_2_pose(parent_frame)
 
     return joint_origin
 
@@ -891,14 +906,14 @@ def get_joint_axis(joint: Joint) -> list[float]:
     """
     # urdf joint axis is expressed w.r.t parent frame, which is the link frame or the parent joint frame
     if joint.joint.jointMotion.jointType == 0: # RigidJointType
-        w_axis = None 
+        w_axis = None
         axis = None
         return axis
     elif joint.joint.jointMotion.jointType == 1: # RevoluteJointType
         w_axis = [round(i, 6) for i in joint.joint.jointMotion.rotationAxisVector.asArray()] # In Fusion360, returned axis is normalized
     elif joint.joint.jointMotion.jointType == 2: # SliderJointType
         w_axis = [round(i, 6) for i in joint.joint.jointMotion.slideDirectionVector.asArray()] # In Fusion360, returned axis is normalized
-    
+
     J_axis = None
     joint_frame: adsk.core.Matrix3D = joint.get_joint_frame()
     w_R_J = math_op.get_rotation_matrix(joint_frame) # represent joint-frame J's orientation w.r.t world-frame w
@@ -954,17 +969,23 @@ def get_joint_element(joint: Joint) -> Element:
     joint_ele = Element("joint")
     joint_ele.attrib = {"name": get_joint_name(joint),
                         "type": get_joint_type(joint)}
-    
+    parentLink = get_joint_parent(joint)
+    childLink = get_joint_child(joint)
+    if not parentLink.is_visible() or not childLink.is_visible():
+        return None
+
     # add joint origin element
-    origin = SubElement(joint_ele, "origin")
-    origin.attrib = {"xyz": "{} {} {}".format(get_joint_origin(joint)[0], get_joint_origin(joint)[1], get_joint_origin(joint)[2]),
-                     "rpy": "{} {} {}".format(get_joint_origin(joint)[3], get_joint_origin(joint)[4], get_joint_origin(joint)[5])}
-    
+    if joint.has_origin():
+        origin = SubElement(joint_ele, "origin")
+        originCoords = get_joint_origin(joint)
+        origin.attrib = {"xyz": "{} {} {}".format(originCoords[0], originCoords[1], originCoords[2]),
+                        "rpy": "{} {} {}".format(originCoords[3], originCoords[4], originCoords[5])}
+
     # add parent and child element
     parent = SubElement(joint_ele, "parent")
-    parent.attrib = {"link": get_link_name(get_joint_parent(joint))}
+    parent.attrib = {"link": get_link_name(parentLink)}
     child = SubElement(joint_ele, "child")
-    child.attrib = {"link": get_link_name(get_joint_child(joint))}
+    child.attrib = {"link": get_link_name(childLink)}
 
     # add axis1, urdf only has one axis for joint
     axis = get_joint_axis(joint)
@@ -980,11 +1001,5 @@ def get_joint_element(joint: Joint) -> Element:
                             "upper": "{}".format(limit[1]),
                             "effort": "{}".format(limit[2]),
                             "velocity": "{}".format(limit[3])}
-    
-    return joint_ele
 
-def get_urdf(joint: Joint, link: Link) -> Element:
-    """
-    
-    """
-    pass
\ No newline at end of file
+    return joint_ele
diff --git a/Add-IN/ACDC4Robot/core/urdf_plus.py b/Add-IN/ACDC4Robot/core/urdf_plus.py
index e213404..6f3f907 100644
--- a/Add-IN/ACDC4Robot/core/urdf_plus.py
+++ b/Add-IN/ACDC4Robot/core/urdf_plus.py
@@ -37,7 +37,8 @@ def write_file(self, file_path):
         
         for joint in self.tree_joints:
             joint_ele = self.get_tree_joint_element(joint)
-            robot_ele.append(joint_ele)
+            if joint_ele is not None:
+                robot_ele.append(joint_ele)
 
         for loop in self.loop_joints:
             loop_ele = self.get_loop_joint_element(loop)
diff --git a/Add-IN/ACDC4Robot/core/utils.py b/Add-IN/ACDC4Robot/core/utils.py
index 85d438d..f5225fc 100644
--- a/Add-IN/ACDC4Robot/core/utils.py
+++ b/Add-IN/ACDC4Robot/core/utils.py
@@ -46,4 +46,31 @@ def terminate_box():
     app = adsk.core.Application.get()
     ui = app.userInterface
 
-    _ = ui.terminateActiveCommand()
\ No newline at end of file
+    _ = ui.terminateActiveCommand()
+
+def component_has_bodies(component: adsk.fusion.Component):
+    """
+    Check if the component has visible bodies
+
+    Args:
+    component: adsk.fusion.Component
+        the component to check
+
+    Returns:
+    bool
+        True if the component has bodies, False otherwise
+    """
+    if component.bRepBodies.count == 0 and component.meshBodies.count == 0:
+        return False
+    if not component.isBodiesFolderLightBulbOn:
+        return False
+
+    # Check if the component has visible bodies
+    for body in component.bRepBodies:
+        if body.isLightBulbOn:
+            return True
+    for body in component.meshBodies:
+        if body.isLightBulbOn:
+            return True
+
+    return False
diff --git a/Add-IN/ACDC4Robot/core/write.py b/Add-IN/ACDC4Robot/core/write.py
index 1923cf2..2897e5b 100644
--- a/Add-IN/ACDC4Robot/core/write.py
+++ b/Add-IN/ACDC4Robot/core/write.py
@@ -74,7 +74,8 @@ def write_urdf(link_list: list[Link], joint_list: list[Joint], dir: str, robot_n
     for joint in joint_list:
         # joint_ele = joint.get_joint_urdf_element()
         joint_ele = URDF.get_joint_element(joint)
-        robot_ele.append(joint_ele)
+        if joint_ele is not None:
+            robot_ele.append(joint_ele)
 
     # set indent to pretty the xml output
     ET.indent(urdf_tree, space="    ", level=0)