rod 0.4.2.dev3__tar.gz → 0.4.2.dev7__tar.gz

{rod-0.4.2.dev3 → rod-0.4.2.dev7}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: rod
-Version: 0.4.2.dev3
+Version: 0.4.2.dev7
 Summary: The ultimate Python tool for RObot Descriptions processing.
 Author-email: Diego Ferigo <dgferigo@gmail.com>
 Maintainer-email: Filippo Luca Ferretti <filippo.ferretti@iit.it>, Carlotta Sartore <carlotta.sartore@iit.it>

{rod-0.4.2.dev3 → rod-0.4.2.dev7}/src/rod/urdf/exporter.py

@@ -56,6 +56,125 @@ class UrdfExporter(abc.ABC):
             gazebo_preserve_fixed_joints=gazebo_preserve_fixed_joints,
         ).to_urdf_string(sdf=sdf)
 
+    @staticmethod
+    def _get_urdf_joint_type(joint: rod.Joint) -> str:
+        """
+        Get the URDF joint type, converting revolute joints with infinite limits to continuous.
+
+        sdformat converts URDF continuous joints to SDF revolute joints with infinite limits,
+        so we need to convert them back to continuous when exporting to URDF.
+        """
+        if (
+            joint.type == "revolute"
+            and joint.axis is not None
+            and joint.axis.limit is not None
+            and (joint.axis.limit.lower is None or np.isinf(joint.axis.limit.lower))
+            and (joint.axis.limit.upper is None or np.isinf(joint.axis.limit.upper))
+        ):
+            return "continuous"
+        return joint.type
+
+    @staticmethod
+    def _joint_to_urdf_dict(joint: rod.Joint) -> dict[str, Any]:
+        """
+        Convert a ROD joint to a URDF joint dictionary.
+
+        Args:
+            joint: The ROD joint to convert.
+
+        Returns:
+            A dictionary representing the joint in URDF format.
+        """
+        # Compute the corrected joint type once
+        urdf_joint_type = UrdfExporter._get_urdf_joint_type(joint)
+
+        return {
+            "@name": joint.name,
+            "@type": urdf_joint_type,
+            "origin": {
+                "@xyz": " ".join(map(str, joint.pose.xyz)),
+                "@rpy": " ".join(map(str, joint.pose.rpy)),
+            },
+            "parent": {"@link": joint.parent},
+            "child": {"@link": joint.child},
+            **(
+                {"axis": {"@xyz": " ".join(map(str, joint.axis.xyz.xyz))}}
+                if joint.axis is not None
+                and joint.axis.xyz is not None
+                and urdf_joint_type != "fixed"
+                else {}
+            ),
+            # calibration: does not have any SDF corresponding element
+            **(
+                {
+                    "dynamics": {
+                        **(
+                            {"@damping": joint.axis.dynamics.damping}
+                            if joint.axis.dynamics.damping is not None
+                            else {}
+                        ),
+                        **(
+                            {"@friction": joint.axis.dynamics.friction}
+                            if joint.axis.dynamics.friction is not None
+                            else {}
+                        ),
+                    }
+                }
+                if joint.axis is not None
+                and joint.axis.dynamics is not None
+                and {joint.axis.dynamics.damping, joint.axis.dynamics.friction}
+                != {None}
+                and urdf_joint_type != "fixed"
+                else {}
+            ),
+            **(
+                {
+                    "limit": {
+                        **(
+                            {"@effort": joint.axis.limit.effort}
+                            if joint.axis.limit.effort is not None
+                            else (
+                                {"@effort": np.finfo(np.float32).max}
+                                if urdf_joint_type
+                                in {"revolute", "prismatic", "continuous"}
+                                else {}
+                            )
+                        ),
+                        **(
+                            {"@velocity": joint.axis.limit.velocity}
+                            if joint.axis.limit.velocity is not None
+                            else (
+                                {"@velocity": np.finfo(np.float32).max}
+                                if urdf_joint_type
+                                in {"revolute", "prismatic", "continuous"}
+                                else {}
+                            )
+                        ),
+                        **(
+                            {"@lower": joint.axis.limit.lower}
+                            if joint.axis.limit.lower is not None
+                            and not np.isinf(joint.axis.limit.lower)
+                            and urdf_joint_type in {"revolute", "prismatic"}
+                            else {}
+                        ),
+                        **(
+                            {"@upper": joint.axis.limit.upper}
+                            if joint.axis.limit.upper is not None
+                            and not np.isinf(joint.axis.limit.upper)
+                            and urdf_joint_type in {"revolute", "prismatic"}
+                            else {}
+                        ),
+                    },
+                }
+                if joint.axis is not None
+                and joint.axis.limit is not None
+                and urdf_joint_type != "fixed"
+                else {}
+            ),
+            # mimic: does not have any SDF corresponding element
+            # safety_controller: does not have any SDF corresponding element
+        }
+
     def to_urdf_string(self, sdf: rod.Sdf | rod.Model) -> str:
         """
         Convert an in-memory SDF model to a URDF string.
@@ -298,88 +417,7 @@ class UrdfExporter(abc.ABC):
                 + extra_links_from_frames,
                 # http://wiki.ros.org/urdf/XML/joint
                 "joint": [
-                    {
-                        "@name": j.name,
-                        "@type": j.type,
-                        "origin": {
-                            "@xyz": " ".join(map(str, j.pose.xyz)),
-                            "@rpy": " ".join(map(str, j.pose.rpy)),
-                        },
-                        "parent": {"@link": j.parent},
-                        "child": {"@link": j.child},
-                        **(
-                            {"axis": {"@xyz": " ".join(map(str, j.axis.xyz.xyz))}}
-                            if j.axis is not None
-                            and j.axis.xyz is not None
-                            and j.type != "fixed"
-                            else {}
-                        ),
-                        # calibration: does not have any SDF corresponding element
-                        **(
-                            {
-                                "dynamics": {
-                                    **(
-                                        {"@damping": j.axis.dynamics.damping}
-                                        if j.axis.dynamics.damping is not None
-                                        else {}
-                                    ),
-                                    **(
-                                        {"@friction": j.axis.dynamics.friction}
-                                        if j.axis.dynamics.friction is not None
-                                        else {}
-                                    ),
-                                }
-                            }
-                            if j.axis is not None
-                            and j.axis.dynamics is not None
-                            and {j.axis.dynamics.damping, j.axis.dynamics.friction}
-                            != {None}
-                            and j.type != "fixed"
-                            else {}
-                        ),
-                        **(
-                            {
-                                "limit": {
-                                    **(
-                                        {"@effort": j.axis.limit.effort}
-                                        if j.axis.limit.effort is not None
-                                        else (
-                                            {"@effort": np.finfo(np.float32).max}
-                                            if j.type in {"revolute", "prismatic"}
-                                            else {}
-                                        )
-                                    ),
-                                    **(
-                                        {"@velocity": j.axis.limit.velocity}
-                                        if j.axis.limit.velocity is not None
-                                        else (
-                                            {"@velocity": np.finfo(np.float32).max}
-                                            if j.type in {"revolute", "prismatic"}
-                                            else {}
-                                        )
-                                    ),
-                                    **(
-                                        {"@lower": j.axis.limit.lower}
-                                        if j.axis.limit.lower is not None
-                                        and j.type in {"revolute", "prismatic"}
-                                        else {}
-                                    ),
-                                    **(
-                                        {"@upper": j.axis.limit.upper}
-                                        if j.axis.limit.upper is not None
-                                        and j.type in {"revolute", "prismatic"}
-                                        else {}
-                                    ),
-                                },
-                            }
-                            if j.axis is not None
-                            and j.axis.limit is not None
-                            and j.type != "fixed"
-                            else {}
-                        ),
-                        # mimic: does not have any SDF corresponding element
-                        # safety_controller: does not have any SDF corresponding element
-                    }
+                    UrdfExporter._joint_to_urdf_dict(j)
                     for j in model.joints()
                     if j.type in UrdfExporter.SupportedSdfJointTypes
                 ]

{rod-0.4.2.dev3 → rod-0.4.2.dev7}/src/rod.egg-info/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: rod
-Version: 0.4.2.dev3
+Version: 0.4.2.dev7
 Summary: The ultimate Python tool for RObot Descriptions processing.
 Author-email: Diego Ferigo <dgferigo@gmail.com>
 Maintainer-email: Filippo Luca Ferretti <filippo.ferretti@iit.it>, Carlotta Sartore <carlotta.sartore@iit.it>

{rod-0.4.2.dev3 → rod-0.4.2.dev7}/tests/test_urdf_exporter.py

@@ -124,3 +124,67 @@ def test_urdf_exporter(robot: Robot) -> None:
     )
 
     assert locked_inertia_exported == pytest.approx(locked_inertia_original, abs=1e-6)
+
+
+def test_continuous_joint_urdf_roundtrip() -> None:
+    """Test that continuous joints in URDF are preserved through SDF conversion."""
+    import xmltodict
+
+    # Create a URDF with a continuous joint
+    urdf_string = """<?xml version="1.0" encoding="utf-8"?>
+<robot name="test_continuous">
+  <link name="base_link">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="1.0"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
+  </link>
+  <link name="rotating_link">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="0.5"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
+  </link>
+  <joint name="continuous_joint" type="continuous">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <parent link="base_link"/>
+    <child link="rotating_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="100.0" velocity="10.0"/>
+  </joint>
+</robot>
+"""
+
+    # Load the URDF (it gets converted to SDF internally by sdformat)
+    sdf = rod.Sdf.load(sdf=urdf_string, is_urdf=True)
+
+    # Export back to URDF
+    exporter = rod.urdf.exporter.UrdfExporter(pretty=True)
+    exported_urdf_string = exporter.to_urdf_string(sdf=sdf)
+
+    # Parse the exported URDF to check the joint properties
+    urdf_dict = xmltodict.parse(exported_urdf_string)
+    joint = urdf_dict["robot"]["joint"]
+
+    # Verify the joint type is continuous (not revolute)
+    assert (
+        joint["@type"] == "continuous"
+    ), f"Expected joint type 'continuous' after roundtrip, got '{joint['@type']}'"
+
+    # Verify NO upper/lower limits are present
+    assert (
+        "@upper" not in joint["limit"]
+    ), "Continuous joint should NOT have an upper position limit after roundtrip"
+    assert (
+        "@lower" not in joint["limit"]
+    ), "Continuous joint should NOT have a lower position limit after roundtrip"
+
+    # Verify effort and velocity limits are still present
+    assert (
+        "@effort" in joint["limit"]
+    ), "Continuous joint should still have an effort limit after roundtrip"
+    assert (
+        "@velocity" in joint["limit"]
+    ), "Continuous joint should still have a velocity limit after roundtrip"