jaxsim 0.2.1.dev47__py3-none-any.whl → 0.2.1.dev51__py3-none-any.whl

jaxsim/_version.py

@@ -12,5 +12,5 @@ __version__: str
 __version_tuple__: VERSION_TUPLE
 version_tuple: VERSION_TUPLE
 
-__version__ = version = '0.2.1.dev47'
-__version_tuple__ = version_tuple = (0, 2, 1, 'dev47')
+__version__ = version = '0.2.1.dev51'
+__version_tuple__ = version_tuple = (0, 2, 1, 'dev51')

jaxsim/parsers/kinematic_graph.py

@@ -121,7 +121,8 @@ class KinematicGraph(Sequence[descriptions.LinkDescription]):
         # Also here, we assume the model is fixed-base, therefore the first frame will
         # have last_link_idx + 1. These frames are not part of the physics model.
         for index, frame in enumerate(self.frames):
-            frame.index = index + len(self.link_names())
+            with frame.mutable_context(mutability=Mutability.MUTABLE_NO_VALIDATION):
+                frame.index = int(index + len(self.link_names()))
 
         # Number joints so that their index matches their child link index
         links_dict = {l.name: l for l in iter(self)}

jaxsim/parsers/rod/parser.py

@@ -6,6 +6,7 @@ import jax.numpy as jnp
 import numpy as np
 import rod
 
+import jaxsim.utils
 from jaxsim import logging
 from jaxsim.math.quaternion import Quaternion
 from jaxsim.parsers import descriptions, kinematic_graph
@@ -25,6 +26,7 @@ class SDFData(NamedTuple):
 
     link_descriptions: List[descriptions.LinkDescription]
     joint_descriptions: List[descriptions.JointDescription]
+    frame_descriptions: List[descriptions.LinkDescription]
     collision_shapes: List[descriptions.CollisionShape]
 
     sdf_model: rod.Model | None = None
@@ -70,6 +72,8 @@ def extract_model_data(
 
     # Jaxsim supports only models compatible with URDF, i.e. those having all links
     # directly attached to their parent joint without additional roto-translations.
+    # Furthermore, the following switch also post-processes frames such that their
+    # pose is expressed wrt the parent link they are rigidly attached to.
     sdf_model.switch_frame_convention(frame_convention=rod.FrameConvention.Urdf)
 
     # Log type of base link
@@ -113,6 +117,23 @@ def extract_model_data(
     # Create a dictionary to find easily links
     links_dict: Dict[str, descriptions.LinkDescription] = {l.name: l for l in links}
 
+    # ============
+    # Parse frames
+    # ============
+
+    # Parse the frames (unconnected)
+    frames = [
+        descriptions.LinkDescription(
+            name=f.name,
+            mass=jnp.array(0.0, dtype=float),
+            inertia=jnp.zeros(shape=(3, 3)),
+            parent=links_dict[f.attached_to],
+            pose=f.pose.transform() if f.pose is not None else jnp.eye(4),
+        )
+        for f in sdf_model.frames()
+        if f.attached_to in links_dict
+    ]
+
     # =========================
     # Process fixed-base models
     # =========================
@@ -309,6 +330,7 @@ def extract_model_data(
         model_name=sdf_model.name,
         link_descriptions=links,
         joint_descriptions=joints,
+        frame_descriptions=frames,
         collision_shapes=collisions,
         fixed_base=sdf_model.is_fixed_base(),
         base_link_name=sdf_model.get_canonical_link(),
@@ -338,10 +360,14 @@ def build_model_description(
         model_description=model_description, model_name=None, is_urdf=is_urdf
     )
 
-    # Build the model description.
+    # Build the intermediate representation used for building a JaxSim model.
+    # This process, beyond other operations, removes the fixed joints.
     # Note: if the model is fixed-base, the fixed joint between world and the first
     #       link is removed and the pose of the first link is updated.
-    model = descriptions.ModelDescription.build_model_from(
+    #
+    # The whole process is:
+    # URDF/SDF ⟶ rod.Model ⟶ ModelDescription ⟶ JaxSimModel.
+    graph = descriptions.ModelDescription.build_model_from(
         name=sdf_data.model_name,
         links=sdf_data.link_descriptions,
         joints=sdf_data.joint_descriptions,
@@ -356,7 +382,47 @@ def build_model_description(
         ],
     )
 
+    # Depending on how the model is reduced due to the removal of fixed joints,
+    # there might be frames that are no longer attached to existing links.
+    # We need to change the link to which they are attached to, and update their pose.
+    frames_with_no_parent_link = (
+        f for f in sdf_data.frame_descriptions if f.parent.name not in graph
+    )
+
+    # Build the object to compute forward kinematics.
+    fk = kinematic_graph.KinematicGraphTransforms(graph=graph)
+
+    for frame in frames_with_no_parent_link:
+        # Get the original data of the frame.
+        original_pose = frame.pose
+        original_parent_link = frame.parent.name
+
+        # The parent link, that has been removed, became a frame.
+        assert original_parent_link in graph.frames_dict, (frame, original_parent_link)
+
+        # Get the new parent of the frame corresponding to the removed parent link.
+        new_parent_link = graph.frames_dict[original_parent_link].parent.name
+        logging.debug(f"Frame '{frame.name}' is now attached to '{new_parent_link}'")
+
+        # Get the transform from the new parent link to the original parent link.
+        # The original pose is expressed wrt the original parent link.
+        F_H_P = fk.relative_transform(
+            relative_to=new_parent_link, name=original_parent_link
+        )
+
+        # Update the frame with the updated data.
+        with frame.mutable_context(
+            mutability=jaxsim.utils.Mutability.MUTABLE_NO_VALIDATION
+        ):
+            frame.parent = graph.links_dict[new_parent_link]
+            frame.pose = np.array(F_H_P @ original_pose)
+
+    # Include the SDF frames originally stored in the SDF.
+    graph = dataclasses.replace(
+        graph, frames=sdf_data.frame_descriptions + graph.frames
+    )
+
     # Store the parsed SDF tree as extra info
-    model = dataclasses.replace(model, extra_info={"sdf_model": sdf_data.sdf_model})
+    graph = dataclasses.replace(graph, extra_info={"sdf_model": sdf_data.sdf_model})
 
-    return model
+    return graph

{jaxsim-0.2.1.dev47.dist-info → jaxsim-0.2.1.dev51.dist-info}/METADATA

@@ -1,6 +1,6 @@
 Metadata-Version: 2.1
 Name: jaxsim
-Version: 0.2.1.dev47
+Version: 0.2.1.dev51
 Home-page: https://github.com/ami-iit/jaxsim
 Author: Diego Ferigo
 Author-email: diego.ferigo@iit.it
@@ -37,7 +37,7 @@ Requires-Dist: jaxlib >=0.4.13
 Requires-Dist: jaxlie >=1.3.0
 Requires-Dist: jax-dataclasses >=1.4.0
 Requires-Dist: pptree
-Requires-Dist: rod >=0.2.0
+Requires-Dist: rod >=0.3.0
 Requires-Dist: typing-extensions ; python_version < "3.12"
 Provides-Extra: all
 Requires-Dist: black[jupyter] ~=24.0 ; extra == 'all'

{jaxsim-0.2.1.dev47.dist-info → jaxsim-0.2.1.dev51.dist-info}/RECORD

@@ -1,5 +1,5 @@
 jaxsim/__init__.py,sha256=OcrfoYS1DGcmAGqu2AqlCTiUVxcpi-IsVwcr_16x74Q,1789
-jaxsim/_version.py,sha256=i3ttW4e2hf2Ai7KA7TQS2c6qo_d-7-Ul74IrlNLPeWc,426
+jaxsim/_version.py,sha256=gSig5h2ZFD-r1oHIhHQTOvL-PnxvMjAYMiDivQOzr48,426
 jaxsim/logging.py,sha256=c4zhwBKf9eAYAHVp62kTEllqdsZgh0K-kPKVy8L3elU,1584
 jaxsim/typing.py,sha256=MeuOCQtLAr-sPkvB_sU8FtwGNRirz1auCwIgRC-QZl8,646
 jaxsim/api/__init__.py,sha256=fNTCPUeDfOAizRd4RsW3Epv0sLTu0KJGoFRSEsi75VM,162
@@ -33,14 +33,14 @@ jaxsim/mujoco/loaders.py,sha256=-z6TIsems7DDogueDBwYLS2JuoWalNfv6sV1F45H63k,2102
 jaxsim/mujoco/model.py,sha256=5-4KTbEbU19zjrSuvUVdLo3noWxTvlCNsFIs3rQTNDY,13506
 jaxsim/mujoco/visualizer.py,sha256=YlteqcCbeB1B6saAHKBz1IJad3N5Rp163reZrzKLAys,5065
 jaxsim/parsers/__init__.py,sha256=sonYi-bBWAoB04kp1mxT4uIORxjb7SdZ0ukGPmVx98Y,44
-jaxsim/parsers/kinematic_graph.py,sha256=ZOHlCwXSC9EhLACYmb8liLDwpQLT8_fRHgG0w9Swbas,28980
+jaxsim/parsers/kinematic_graph.py,sha256=rAqh1XBJU0qDkDoA_BSdt-PwBBTFO8SXVpr81Xumbp4,29074
 jaxsim/parsers/descriptions/__init__.py,sha256=PbIlunVfb59pB5jSX97YVpMAANRZPRkJ0X-hS14rzv4,221
 jaxsim/parsers/descriptions/collision.py,sha256=HUWwuRgI9KznY29FFw1_zU3bGigDEezrcPOJSxSJGNU,3382
 jaxsim/parsers/descriptions/joint.py,sha256=r7teUWJyw62WS7qNCB_9FGQD1hvGHBepk-2LCSlmcSc,3052
 jaxsim/parsers/descriptions/link.py,sha256=hqLLitrAXnouia6ULk1BPOIEfRxrXwHmoPsi306IZW8,2859
 jaxsim/parsers/descriptions/model.py,sha256=hapbpO1VpC7f5ELkGH1ZtM2I3dpmYLz8Em5cb3DdiaA,9264
 jaxsim/parsers/rod/__init__.py,sha256=G2vqlLajBLUc4gyzXwsEI2Wsi4TMOIF9bLDFeT6KrGU,92
-jaxsim/parsers/rod/parser.py,sha256=hACXVNLUZ8mXMaaq4CeEBCSHI0DjHIp4p4zmpiPKvaU,12537
+jaxsim/parsers/rod/parser.py,sha256=DmYYHAovzn1WnY_8qb6Sihr8QX--RMj32QF1RXPdScM,15278
 jaxsim/parsers/rod/utils.py,sha256=9oO4YsQRaR2v700IkNOXRnPpn5i4N8HFfgjPkMLK2mc,5732
 jaxsim/rbda/__init__.py,sha256=HLwxeU-IxaRpFGUCSQv-LDv20JHTt3Xj7ELiRbRieS8,319
 jaxsim/rbda/aba.py,sha256=0OoCzHhf1v-qqr1y5PIrD7_mPwAlid0fjXxUrIa5E_s,9118
@@ -57,8 +57,8 @@ jaxsim/utils/__init__.py,sha256=tnQq1_CavdfeKaLYt3pmO7Jk4MU2RwwQU_qICkjyoTY,197
 jaxsim/utils/hashless.py,sha256=bFIwKeo9KiWwsY8QM55duEGGQOyyJ4jQyPcuqTLEp5k,297
 jaxsim/utils/jaxsim_dataclass.py,sha256=h26timZ_XrBL_Q_oymv-DkQd-EcUiHn8QexAaZXBY9c,11396
 jaxsim/utils/tracing.py,sha256=KDMoyVPlu2NJvFkhtZwq5AkqMMgajt3munvJom-vEjQ,650
-jaxsim-0.2.1.dev47.dist-info/LICENSE,sha256=eaYdFmdeMbiIoIiPzEK0MjP1S9wtFXjXNR5er49uLR0,1546
-jaxsim-0.2.1.dev47.dist-info/METADATA,sha256=3T8mKCJWUIKK2LWeubpn5rtB8fzAsEeQhairL3Y6TX0,9744
-jaxsim-0.2.1.dev47.dist-info/WHEEL,sha256=GJ7t_kWBFywbagK5eo9IoUwLW6oyOeTKmQ-9iHFVNxQ,92
-jaxsim-0.2.1.dev47.dist-info/top_level.txt,sha256=LxGMA8FLtXjQ6oI7N5gd_R_oSUHxpXxUEOfT1xS_ni0,7
-jaxsim-0.2.1.dev47.dist-info/RECORD,,
+jaxsim-0.2.1.dev51.dist-info/LICENSE,sha256=eaYdFmdeMbiIoIiPzEK0MjP1S9wtFXjXNR5er49uLR0,1546
+jaxsim-0.2.1.dev51.dist-info/METADATA,sha256=UMhmMrS5kyph8xEtRZwiJqDGBqMPgfa4RDw-2NCApHc,9744
+jaxsim-0.2.1.dev51.dist-info/WHEEL,sha256=GJ7t_kWBFywbagK5eo9IoUwLW6oyOeTKmQ-9iHFVNxQ,92
+jaxsim-0.2.1.dev51.dist-info/top_level.txt,sha256=LxGMA8FLtXjQ6oI7N5gd_R_oSUHxpXxUEOfT1xS_ni0,7
+jaxsim-0.2.1.dev51.dist-info/RECORD,,