Merge pull request #25 from neurobionics/feature/inertia-matrix-wrt

Added routine to modify inertial matrix w/r/t a new transformation matrix.

Added a few other minor changes that are out of the scope of this PR:

Switching from "-" to "_" as the default str joiner for sanitized names
Added an argument to remove onshape's tags to link & joint names
Made changes to parse and urdf modules to use _ instead of - in visual and collision names.
Made name tags in visual and collision elements to be optional

Author: senthurayyappan

.gitignore

@@ -189,6 +189,7 @@ benchmark/**/*.png
 benchmark/**/*.prof
 benchmark/**/*.json
 examples/**/*.xml
+examples/simulation/scenes
 
 tests/**/*.urdf
 tests/**/*.stl

examples/export/main.py

@@ -18,5 +18,5 @@
 
     save_model_as_json(robot.assembly, "quadruped.json")
 
-    robot.show_graph(file_name="quadruped.png")
+    # robot.show_graph(file_name="quadruped.png")
     robot.save()

onshape_robotics_toolkit/models/link.py

@@ -434,6 +434,15 @@ def zero_inertia(cls) -> "Inertia":
         """
         return cls(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
 
+    @property
+    def to_matrix(self) -> np.array:
+        return np.array([
+            [self.ixx, self.ixy, self.ixz],
+            [self.ixy, self.iyy, self.iyz],
+            [self.ixz, self.iyz, self.izz]
+        ])
+
+
 
 @dataclass
 class Material:
@@ -602,6 +611,77 @@ def to_mjcf(self, root: ET.Element) -> None:
         self.origin.to_mjcf(inertial)
         self.inertia.to_mjcf(inertial)
 
+    # Adding a method to transform (rotate and translate) an Inertia Matrix
+    def transform(self, tf_matrix: np.matrix, inplace: bool = False) -> Union["InertialLink", None]:
+        """
+        Apply a transformation matrix to the Inertial Properties of the a link.
+
+        Args:
+            matrix: The transformation matrix to apply to the origin.
+            inplace: Whether to apply the transformation in place.
+
+        Returns:
+            An updated Inertial Link with the transformation applied to both:
+            * the inertia tensor (giving a transformed "inertia tensor prime" = [ixx', iyy', izz', ixy', ixz', iyz'])
+            * AND to the origin too (via the Origin class's transform logic [~line 100])
+
+        Examples {@}:
+            >>> origin = Origin(xyz=(1.0, 2.0, 3.0), rpy=(0.0, 0.0, 0.0))
+            >>> matrix = np.eye(4)
+            >>> inertial.transform(matrix)
+
+        Analysis and References:
+            The essence is to convert the Inertia tensor to a matrix and then transform the matrix via the equation
+            I_prime = R·I·Transpose[R] + m(||d||^2·I - d·Transpose[d]) 
+            Then we put the components into the resultant Inertial Link
+            An analysis (on 100k runs) suggests that this is 3× faster than a direct approach on the tensor elements likely because numpy's libraries are optimized for matrix operations.
+            Ref: https://chatgpt.com/share/6781b6ac-772c-8006-b1a9-7f2dc3e3ef4d
+        """
+        # Extract the rotation matrix R and translation vector d from T
+        R = tf_matrix[:3, :3]  # Top-left 3x3 block is the rotation matrix
+        p = tf_matrix[:3, 3]   # Top-right 3x1 block is the translation vector
+
+        # Unpack the inertia tensor components
+        # Example is ixx=1.0, iyy=2.0, izz=3.0, ixy=0.0, ixz=0.0, iyz=0.
+
+        # Construct the original inertia matrix
+        inertia_matrix = self.inertia.to_matrix
+
+        # Rotate the inertia matrix
+        I_rot = R @ inertia_matrix @ R.T
+
+        # Compute the parallel axis theorem adjustment
+        parallel_axis_adjustment = self.mass * (np.dot(p, p) * np.eye(3) - np.outer(p, p))
+
+        # Final transformed inertia matrix
+        I_transformed = I_rot + parallel_axis_adjustment
+
+        # Extract the components of the transformed inertia tensor
+        ixx_prime = I_transformed[0, 0]
+        iyy_prime = I_transformed[1, 1]
+        izz_prime = I_transformed[2, 2]
+        ixy_prime = I_transformed[0, 1]
+        ixz_prime = I_transformed[0, 2]
+        iyz_prime = I_transformed[1, 2]
+
+        # Transform the Origin (Don't replace the original in case the user keeps the inplace flag false)
+        Origin_prime = self.origin.transform(tf_matrix)
+
+        # Update values and (if requested) put the extracted values into a new_InertialLink
+        if inplace:
+            # mass stays the same :-) ==> self.mass = new_InertialLink.mass
+            self.inertia.ixx = ixx_prime
+            self.inertia.iyy = iyy_prime
+            self.inertia.izz = izz_prime
+            self.inertia.ixy = ixy_prime
+            self.inertia.ixz = ixz_prime
+            self.inertia.iyz = iyz_prime
+            self.origin = Origin_prime
+            return None
+        else:
+            new_InertialLink = InertialLink(mass=self.mass, inertia=Inertia(ixx_prime, iyy_prime, izz_prime, ixy_prime, ixz_prime, iyz_prime), origin=Origin_prime)
+            return new_InertialLink
+
     @classmethod
     def from_xml(cls, xml: ET.Element) -> "InertialLink":
         """
@@ -661,7 +741,7 @@ class VisualLink:
         <Element 'visual' at 0x7f8b3c0b4c70>
     """
 
-    name: str
+    name: Union[str, None]
     origin: Origin
     geometry: BaseGeometry
     material: Material
@@ -700,7 +780,8 @@ def to_xml(self, root: Optional[ET.Element] = None) -> ET.Element:
             <Element 'visual' at 0x7f8b3c0b4c70>
         """
         visual = ET.Element("visual") if root is None else ET.SubElement(root, "visual")
-        visual.set("name", self.name)
+        if self.name:
+            visual.set("name", self.name)
         self.origin.to_xml(visual)
         self.geometry.to_xml(visual)
         self.material.to_xml(visual)
@@ -722,7 +803,8 @@ def to_mjcf(self, root: ET.Element) -> None:
             <Element 'visual' at 0x7f8b3c0b4c70>
         """
         visual = root if root.tag == "geom" else ET.SubElement(root, "geom")
-        visual.set("name", self.name)
+        if self.name:
+            visual.set("name", self.name)
         # TODO: Parent body uses visual origin, these share the same?
         self.origin.to_mjcf(visual)
 
@@ -754,7 +836,6 @@ def from_xml(cls, xml: ET.Element) -> "VisualLink":
             VisualLink(name='visual', origin=None, geometry=None, material=None)
         """
         name = xml.get("name")
-
         origin_element = xml.find("origin")
         origin = Origin.from_xml(origin_element) if origin_element is not None else None
 
@@ -784,7 +865,7 @@ class CollisionLink:
         <Element 'collision' at 0x7f8b3c0b4c70>
     """
 
-    name: str
+    name: Union[str, None]
     origin: Origin
     geometry: BaseGeometry
 
@@ -824,7 +905,8 @@ def to_xml(self, root: Optional[ET.Element] = None) -> ET.Element:
             <Element 'collision' at 0x7f8b3c0b4c70>
         """
         collision = ET.Element("collision") if root is None else ET.SubElement(root, "collision")
-        collision.set("name", self.name)
+        if self.name:
+            collision.set("name", self.name)
         self.origin.to_xml(collision)
         self.geometry.to_xml(collision)
         return collision
@@ -858,7 +940,8 @@ def to_mjcf(self, root: ET.Element) -> None:
             <Element 'collision' at 0x7f8b3c0b4c70>
         """
         collision = root if root.tag == "geom" else ET.SubElement(root, "geom")
-        collision.set("name", self.name)
+        if self.name:
+            collision.set("name", self.name)
         collision.set("contype", "1")
         collision.set("conaffinity", "1")
         self.origin.to_mjcf(collision)

onshape_robotics_toolkit/parse.py

@@ -31,10 +31,7 @@
 from onshape_robotics_toolkit.models.document import WorkspaceType
 from onshape_robotics_toolkit.utilities.helpers import get_sanitized_name
 
-os.environ["TCL_LIBRARY"] = "C:\\Users\\imsen\\AppData\\Local\\Programs\\Python\\Python313\\tcl\\tcl8.6"
-os.environ["TK_LIBRARY"] = "C:\\Users\\imsen\\AppData\\Local\\Programs\\Python\\Python313\\tcl\\tk8.6"
-
-SUBASSEMBLY_JOINER = "-SUB-"
+SUBASSEMBLY_JOINER = "_SUB_"
 MATE_JOINER = "_to_"
 
 CHILD = 0

onshape_robotics_toolkit/urdf.py

@@ -140,7 +140,7 @@ def get_robot_link(
     _link = Link(
         name=name,
         visual=VisualLink(
-            name=f"{name}-visual",
+            name=f"{name}_visual",
             origin=_origin,
             geometry=MeshGeometry(_mesh_path),
             material=Material.from_color(name=f"{name}-material", color=random.SystemRandom().choice(list(Colors))),
@@ -161,7 +161,7 @@ def get_robot_link(
             ),
         ),
         collision=CollisionLink(
-            name=f"{name}-collision",
+            name=f"{name}_collision",
             origin=_origin,
             geometry=MeshGeometry(_mesh_path),
         ),
@@ -269,15 +269,15 @@ def get_robot_joint(
 
     elif mate.mateType == MateType.BALL:
         dummy_x = Link(
-            name=f"{parent}-{get_sanitized_name(mate.name)}-x",
+            name=f"{parent}_{get_sanitized_name(mate.name)}_x",
             inertial=InertialLink(
                 mass=0.0,
                 inertia=Inertia.zero_inertia(),
                 origin=Origin.zero_origin(),
             ),
         )
         dummy_y = Link(
-            name=f"{parent}-{get_sanitized_name(mate.name)}-y",
+            name=f"{parent}_{get_sanitized_name(mate.name)}_y",
             inertial=InertialLink(
                 mass=0.0,
                 inertia=Inertia.zero_inertia(),
@@ -289,7 +289,7 @@ def get_robot_joint(
 
         return [
             RevoluteJoint(
-                name=sanitized_name + "-x",
+                name=sanitized_name + "_x",
                 parent=parent,
                 child=dummy_x.name,
                 origin=origin,
@@ -304,7 +304,7 @@ def get_robot_joint(
                 mimic=mimic,
             ),
             RevoluteJoint(
-                name=sanitized_name + "-y",
+                name=sanitized_name + "_y",
                 parent=dummy_x.name,
                 child=dummy_y.name,
                 origin=Origin.zero_origin(),
@@ -319,7 +319,7 @@ def get_robot_joint(
                 mimic=mimic,
             ),
             RevoluteJoint(
-                name=sanitized_name + "-z",
+                name=sanitized_name + "_z",
                 parent=dummy_y.name,
                 child=child,
                 origin=Origin.zero_origin(),

onshape_robotics_toolkit/utilities/helpers.py

@@ -278,33 +278,46 @@ def make_unique_name(name: str, existing_names: set[str]) -> str:
     return f"{name}-{count}"
 
 
-def get_sanitized_name(name: str, replace_with: str = "-") -> str:
+def get_sanitized_name(name: str, replace_with: str = "_", remove_onshape_tags: bool = False) -> str:
     """
-    Sanitize a name by removing special characters, preserving "-" and "_", and
-    replacing spaces with a specified character. Ensures no consecutive replacement
-    characters in the result.
+    Sanitize a name by removing special characters, preserving only the specified
+    replacement character, and replacing spaces with it. Ensures no consecutive
+    replacement characters in the result.
+
+    Optionally preserves a trailing " <n>" tag where n is a number.
 
     Args:
-        name: Name to sanitize
-        replace_with: Character to replace spaces with (default is '-')
+        name (str): Name to sanitize.
+        replace_with (str): Character to replace spaces and other special characters with (default is '_').
+        remove_onshape_tags (bool): If True, removes a trailing " <n>" tag where n is a number. Default is False.
 
     Returns:
-        Sanitized name
+        str: Sanitized name.
 
     Examples:
-        >>> get_sanitized_name("wheel1 <3>", '-')
+        >>> get_sanitized_name("wheel1 <3>")
+        "wheel1"
+
+        >>> get_sanitized_name("wheel1 <3>", remove_onshape_tags=False)
+        "wheel1_3"
+
+        >>> get_sanitized_name("wheel1 <3>", replace_with='-', remove_onshape_tags=False)
         "wheel1-3"
-        >>> get_sanitized_name("Hello  World!", '_')
-        "Hello_World"
-        >>> get_sanitized_name("my--robot!!", '-')
-        "my-robot"
-        >>> get_sanitized_name("bad__name__", '_')
-        "bad_name"
     """
 
     if replace_with not in "-_":
         raise ValueError("replace_with must be either '-' or '_'")
 
+    tag = ""
+    if remove_onshape_tags:
+        # Regular expression to detect a trailing " <n>" where n is one or more digits
+        tag_pattern = re.compile(r"\s<\d+>$")
+        match = tag_pattern.search(name)
+        if match:
+            tag = match.group()  # e.g., " <3>"
+            if tag:
+                name = name[: match.start()]
+
     sanitized_name = "".join(char if char.isalnum() or char in "-_ " else "" for char in name)
     sanitized_name = sanitized_name.replace(" ", replace_with)
     sanitized_name = re.sub(f"{re.escape(replace_with)}{{2,}}", replace_with, sanitized_name)
@@ -333,4 +346,4 @@ def save_gif(frames, filename="sim.gif", framerate=60):
 
 
 if __name__ == "__main__":
-    LOGGER.info(get_sanitized_name(input("Enter a name: ")))
+    LOGGER.info(get_sanitized_name("Part 3 <1>", remove_onshape_tags=True))