Removed dissolve_fixed_joints method and switched to euler angles ins…

…tead of quaternions for MJCF.

onshape_api/models/link.py

@@ -163,7 +163,7 @@ def to_mjcf(self, root: ET.Element) -> None:
             <Element 'origin' at 0x7f8b3c0b4c70>
         """
         root.set("pos", " ".join(format_number(v) for v in self.xyz))
-        root.set("quat", " ".join(format_number(v) for v in self.quat()))
+        root.set("euler", " ".join(format_number(v) for v in self.rpy))
 
     @classmethod
     def from_xml(cls, xml: ET.Element) -> "Origin":
@@ -186,7 +186,7 @@ def from_xml(cls, xml: ET.Element) -> "Origin":
         rpy = tuple(map(float, xml.get("rpy").split()))
         return cls(xyz, rpy)
 
-    def quat(self, sequence: str = "zyx") -> str:
+    def quat(self, sequence: str = "xyz") -> str:
         """
         Convert the origin to a quaternion.
 
@@ -585,7 +585,7 @@ def to_mjcf(self, root: ET.Element) -> None:
 
         Example XML:
         ```xml
-        <inertial pos="0 0 -0.0075" quat="0.5 0.5 -0.5 0.5" mass="0.624"
+        <inertial pos="0 0 -0.0075" euler="0.5 0.5 -0.5" mass="0.624"
                   diaginertia="0.073541512 0.07356916 0.073543931" />
         ```
         Args:
@@ -718,7 +718,7 @@ def to_mjcf(self, root: ET.Element) -> None:
         visual = root if root.tag == "geom" else ET.SubElement(root, "geom")
         visual.set("name", self.name)
         # TODO: Parent body uses visual origin, these share the same?
-        # self.origin.to_mjcf(visual)
+        self.origin.to_mjcf(visual)
 
         if self.geometry:
             self.geometry.to_mjcf(visual)
@@ -976,8 +976,8 @@ def to_mjcf(self, root: Optional[ET.Element] = None) -> ET.Element:
         """
         link = ET.Element("body") if root is None else ET.SubElement(root, "body")
         link.set("name", self.name)
-        link.set("pos", f"{self.visual.origin.xyz[0]} {self.visual.origin.xyz[1]} {self.visual.origin.xyz[2]}")
-        link.set("quat", " ".join(map(str, self.visual.origin.quat())))
+        link.set("pos", " ".join(map(str, self.visual.origin.xyz)))
+        link.set("euler", " ".join(map(str, self.visual.origin.rpy)))
 
         if self.collision:
             self.collision.to_mjcf(link)
@@ -1038,3 +1038,8 @@ def from_xml(cls, xml: ET.Element) -> "Link":
     #     """
     #     _cls = cls(name=part.partId)
     #     return _cls
+
+
+if __name__ == "__main__":
+    origin = Origin(xyz=(0.0, 0.0, 0.0), rpy=(0.0, 0.0, 0.0))
+    print(origin.quat())

onshape_api/robot.py

@@ -11,9 +11,7 @@
 from typing import Any, Optional
 
 import networkx as nx
-import numpy as np
 from lxml import etree as ET
-from scipy.spatial.transform import Rotation as R
 
 from onshape_api.connect import Asset, Client
 from onshape_api.graph import create_graph, plot_graph
@@ -48,6 +46,7 @@
     get_subassemblies,
 )
 from onshape_api.urdf import get_joint_name, get_robot_joint, get_robot_link, get_topological_mates
+from onshape_api.utilities.helpers import format_number
 
 DEFAULT_COMPILER_ATTRIBUTES = {
     "angle": "radian",
@@ -180,7 +179,7 @@ def add_sensor(self, sensor: dict[str, Any]) -> None:
         pass
 
     def add_ground_plane(
-        self, root: ET.Element, size: int = 2, orientation: tuple[float, float, float, float] = (1, 0, 0, 0)
+        self, root: ET.Element, size: int = 2, orientation: tuple[float, float, float] = (0, 0, 0)
     ) -> None:
         """
         Add a ground plane to the root element.
@@ -191,7 +190,7 @@ def add_ground_plane(
         geom = ET.Element("geom", name="ground")
         geom.set("type", "plane")
         geom.set("pos", " ".join(map(str, self.ground_position)))
-        geom.set("quat", " ".join(map(str, orientation)))
+        geom.set("euler", " ".join(map(str, orientation)))
         geom.set("size", f"{size} {size} 0.001")
         geom.set("condim", "3")
         geom.set("conaffinity", "15")
@@ -218,39 +217,6 @@ def to_urdf(self) -> str:
 
         return ET.tostring(robot, pretty_print=True, encoding="unicode")
 
-    def dissolve_fixed_joints(self) -> None:
-        """Dissolve all fixed joints by merging child links into parent links."""
-        fixed_joints = [
-            (parent, child, data["data"])
-            for parent, child, data in self.graph.edges(data=True)
-            if isinstance(data.get("data"), FixedJoint)
-        ]
-
-        for parent, child, joint in fixed_joints:
-            # Get link data
-            parent_link = self.graph.nodes[parent]["data"]
-            child_link = self.graph.nodes[child]["data"]
-
-            if not parent_link or not child_link:
-                continue  # Skip if either link is missing data
-
-            # Apply transformation from child to parent
-            transform = joint.origin  # Assuming joint.origin provides translation/rotation
-            translation = transform.xyz
-            rotation = transform.rpy
-
-            # Convert rotation to transformation matrix
-            rotation_matrix = R.from_euler("xyz", rotation).as_matrix()
-            transformation_matrix = np.eye(4)
-            transformation_matrix[:3, :3] = rotation_matrix
-            transformation_matrix[:3, 3] = translation
-
-            # Transform visuals and collisions of child link
-            child_link.visual.transform(transformation_matrix)
-            child_link.collision.transform(transformation_matrix)
-
-            print(f"Dissolved fixed joint between {parent} and {child}.")
-
     def to_mjcf(self) -> str:  # noqa: C901
         """Generate MJCF XML from the graph."""
         model = ET.Element("mujoco", model=self.name)
@@ -298,6 +264,9 @@ def to_mjcf(self) -> str:  # noqa: C901
                         for element in list(child_body):
                             if element.tag == "inertial":
                                 continue
+                            elif element.tag == "geom":
+                                element.set("pos", " ".join(format_number(v) for v in joint_data.origin.xyz))
+                                element.set("euler", " ".join(format_number(v) for v in joint_data.origin.rpy))
 
                             parent_body.append(element)
 
@@ -320,6 +289,8 @@ def save(self, file_path: Optional[str] = None, download_assets: bool = True) ->
             asyncio.run(self._download_assets())
 
         if not file_path:
+            LOGGER.warning("No file path provided. Saving to current directory.")
+            LOGGER.warning("Please keep in mind that the path to the assets will not be updated")
             file_path = f"{self.name}.{self.type}"
 
         xml_declaration = '<?xml version="1.0" ?>\n'
@@ -330,7 +301,6 @@ def save(self, file_path: Optional[str] = None, download_assets: bool = True) ->
                 f.write(urdf_str)
 
         elif self.type == RobotType.MJCF:
-            self.dissolve_fixed_joints()
             mjcf_str = xml_declaration + self.to_mjcf()
             with open(file_path, "w", encoding="utf-8") as f:
                 f.write(mjcf_str)
@@ -575,9 +545,13 @@ def get_robot(
 if __name__ == "__main__":
     LOGGER.set_file_name("test.log")
 
-    robot = Robot.from_urdf(filename="E:/onshape-api/onshape_api/ballbot.urdf", robot_type=RobotType.MJCF)
+    robot = Robot.from_urdf(filename="E:/onshape-api/playground/ballbot.urdf", robot_type=RobotType.MJCF)
     robot.set_robot_position((0, 0, 0.6))
-    robot.save()
+    robot.save(file_path="E:/onshape-api/playground/ballbot.xml")
+
+    # import mujoco
+    # model = mujoco.MjModel.from_xml_path(filename="E:/onshape-api/playground/ballbot.urdf")
+    # mujoco.mj_saveLastXML("ballbot-raw-ref.xml", model)
 
     # simulate_robot("test.xml")