Expose quantities related to generic frames

[xela-95]

Closes #147

---

📚 Documentation preview 📚: https://jaxsim--148.org.readthedocs.build//148/

[xela-95]

The implementation needed by issue #147 have been completed. Now the unit test for the jacobian function of frame module is taking a lot to complete, but unfortunately I was not able to speed up the computations from the test by using jax.vmap since that would raise Jax errors like TracerArrayConversionError or TracerIntegerConversionError.

@diegoferigo @flferretti could you please review the math in the frame functions and the unit tests implementation?

[flferretti]

Thanks a lot @xela-95 for the PR! Could you please explain how did you get the errors using the vmap? It would be nice to be able to reproduce and eventually find a solution for it

[xela-95]

Thanks a lot @xela-95 for the PR! Could you please explain how did you get the errors using the vmap? It would be nice to be able to reproduce and eventually find a solution for it

For example, if I try to mimick what is done to test the link jacobians in https://github.com/xela-95/jaxsim/blob/96c600dd2e7fc9a0136800924fde8bbe40d9f5cd/tests/test_api_link.py#L139-L141

but instead calling js.frame.jacobian like:

    J_WL_frames = jax.vmap(
        lambda idx: js.frame.jacobian(model=model, data=data, frame_index=idx)
    )(jnp.array(frame_indexes))

I get:

tests/test_api_frame.py:122: in <lambda>
    lambda idx: js.frame.jacobian(model=model, data=data, frame_index=idx)
_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ 

model = JaxSimModel(model_name='ergoCub')
data = JaxSimModelData(velocity_representation=<VelRepr.Inertial: 3>, state=ODEState(physics_model=PhysicsModelState(joint_po...loat64[3])>with<DynamicJaxprTrace(level=2/0)>, time_ns=Traced<ShapedArray(uint64[])>with<DynamicJaxprTrace(level=2/0)>)

    @functools.partial(jax.jit, static_argnames=["frame_index", "output_vel_repr"])
    def jacobian(
        model: js.model.JaxSimModel,
        data: js.data.JaxSimModelData,
        *,
        frame_index: jtp.IntLike,
        output_vel_repr: VelRepr | None = None,
    ) -> jtp.Matrix:
        """
        Compute the free-floating jacobian of the frame.

        Args:
            model: The model to consider.
            data: The data of the considered model.
            frame_index: The index of the frame.
            output_vel_repr:
                The output velocity representation of the free-floating jacobian.

        Returns:
            The 6×(6+n) free-floating jacobian of the frame.

        Note:
            The input representation of the free-floating jacobian is the active
            velocity representation.
        """

        output_vel_repr = (
            output_vel_repr if output_vel_repr is not None else data.velocity_representation
        )

        # Get the free-floating jacobian of the parent link in body-fixed output representation
        L = (
>           model.description.get()
            .frames[frame_index - model.number_of_links()]
            .parent.index
        )
E       jax.errors.TracerIntegerConversionError: The __index__() method was called on traced array with shape int64[].
E       The error occurred while tracing the function jacobian at /home/acroci/repos/jaxsim/src/jaxsim/api/frame.py:127 for jit. This value became a tracer due to JAX operations on these lines:
E       
E         operation a:i64[] = convert_element_type[new_dtype=int64 weak_type=False] b
E           from line /home/acroci/repos/jaxsim/src/jaxsim/api/frame.py:160:16 (jacobian)
E       
E         operation a:i64[] = sub b c
E           from line /home/acroci/repos/jaxsim/src/jaxsim/api/frame.py:160:16 (jacobian)
E       See https://jax.readthedocs.io/en/latest/errors.html#jax.errors.TracerIntegerConversionError
E       --------------------
E       For simplicity, JAX has removed its internal frames from the traceback of the following exception. Set JAX_TRACEBACK_FILTERING=off to include these.

src/jaxsim/api/frame.py:159: TracerIntegerConversionError

[flferretti]

Thanks for providing an example. I'll take a look at it, in the meanwhile for me it is good to go

[xela-95]

Perfect, thanks for the clear explanations!!

[flferretti]

I just want to point out that from E operation a:i64[] = sub b c I get that the error is raised from the subtraction operation frame_index - model.number_of_links(), therefore we must focus on the type of frame_index when it gets passed to the function.

[xela-95]

I just want to point out that from E operation a:i64[] = sub b c I get that the error is raised from the subtraction operation frame_index - model.number_of_links(), therefore we must focus on the type of frame_index when it gets passed to the function.

Nice catch! I'll try to iterate on that point to find a solution exploiting Jax capabilities

[xela-95]

The CI is still failing with error Error: The operation was canceled.. Do you think it's due to some maximum time allowed for action runner?

Could you re-run one of the failing actions, enabling the debug logging to have more details? (I do not have the permissions) https://docs.github.com/en/actions/managing-workflow-runs/re-running-workflows-and-jobs

[xela-95]

Brief recap of yesterday

This PR is not yet merged since modifying the unit test checking the match between the jacobian of frames computed in Jaxsim and iDynTree the test started to fail. Since the only model used in tests with frames (that are not links) is ergoCub, I needed easier examples to work on to debug the function computing the jacobian.

I started updating the simple box model (no joints) by adding a frame attached to the only link of the model using rod APIs: https://github.com/xela-95/jaxsim/blob/3d4261586efe4b09b1e0b30ff668a0aa2f99c5e7/tests/conftest.py#L130-L136

In this case the unit test passed.

Then, I wanted to use frames for more complex models, like the UR10 manipulator. These models are loaded using the robot-descriptions package and they already have in their URDF descriptions dummy frames, but they were not parsed by rod.

@diegoferigo added support to frames super quickly in https://github.com/ami-iit/jaxsim/pull/150. Unfortunately, the issue is that these frames are not loaded in KynDynComputations so it's not possible to use them for the unit test.

[xela-95]

Right now on ErgoCubReduced the jacobians that are not matching are the ones for the frames:

• l_foot_front
• l_foot_rear
• l_hip_3
• l_shoulder_3
• r_hip_3
• r_shoulder3

These test cases are failing in all 3 velocity representations (inertial, body and fixed). All the other frames are passing the test.

[xela-95]

Right now on ErgoCubReduced the jacobians that are not matching are the ones for the frames:

• l_foot_front
• l_foot_rear
• l_hip_3
• l_shoulder_3
• r_hip_3
• r_shoulder3

These test cases are failing in all 3 velocity representations (inertial, body and fixed). All the other frames are passing the test.

Ok I found out the the culprit is actually not the jacobian computation but the transform function of the frame, since after refactoring the test I saw it fails on the same frames. 🕵🏻

[xela-95]

Here's the log containg the homogeneous transforms from world to frame that are failing:

Logs

``` jaxsim[100706] ERROR Assertion failed for frame: l_foot_front jaxsim[100706] DEBUG W_H_F_js: jaxsim[100706] DEBUG [[-0.35031 -0.67322 -0.6512 0.41084] [ 0.928 -0.34364 -0.14396 0.4807 ] [-0.12686 -0.65474 0.74513 0.39986] [ 0. 0. 0. 1. ]] jaxsim[100706] DEBUG W_H_F_iDynTree: jaxsim[100706] DEBUG [[-0.54756 -0.36036 -0.75519 -0.36317] [-0.15713 -0.84218 0.51579 -0.03562] [-0.82188 0.40109 0.40453 0.33124] [ 0. 0. 0. 1. ]] jaxsim[100706] ERROR Assertion failed for frame: l_foot_rear jaxsim[100706] DEBUG W_H_F_js: jaxsim[100706] DEBUG [[-0.35031 -0.67322 -0.6512 0.45261] [ 0.928 -0.34364 -0.14396 0.37004] [-0.12686 -0.65474 0.74513 0.41499] [ 0. 0. 0. 1. ]] jaxsim[100706] DEBUG W_H_F_iDynTree: jaxsim[100706] DEBUG [[-0.54756 -0.36036 -0.75519 -0.29787] [-0.15713 -0.84218 0.51579 -0.01688] [-0.82188 0.40109 0.40453 0.42925] [ 0. 0. 0. 1. ]] jaxsim[100706] ERROR Assertion failed for frame: l_hip_3 jaxsim[100706] DEBUG W_H_F_js: jaxsim[100706] DEBUG [[-0.80645 -0.10969 -0.58104 -0.02238] [ 0.46848 -0.71808 -0.51467 0.27463] [-0.36078 -0.68726 0.63049 0.81591] [ 0. 0. 0. 1. ]] jaxsim[100706] DEBUG W_H_F_iDynTree: jaxsim[100706] DEBUG [[-0.37434 -0.05964 0.92537 -0.24618] [ 0.00864 -0.99811 -0.06083 0.09552] [ 0.92725 -0.01478 0.37415 0.80239] [ 0. 0. 0. 1. ]] jaxsim[100706] ERROR Assertion failed for frame: l_shoulder_3 jaxsim[100706] DEBUG W_H_F_js: jaxsim[100706] DEBUG [[-0.35031 -0.67322 -0.6512 0.46391] [ 0.928 -0.34364 -0.14396 0.35207] [-0.12686 -0.65474 0.74513 0.4128 ] [ 0. 0. 0. 1. ]] jaxsim[100706] DEBUG W_H_F_iDynTree: jaxsim[100706] DEBUG [[-0.6807 -0.7098 -0.18119 -0.32363] [ 0.69106 -0.70426 0.16267 0.01637] [-0.24307 -0.01448 0.9699 1.14715] [ 0. 0. 0. 1. ]] jaxsim[100706] ERROR Assertion failed for frame: r_hip_3 jaxsim[100706] DEBUG W_H_F_js: jaxsim[100706] DEBUG [[-0.33184 -0.18322 0.92537 -0.40775] [ 0.34707 -0.93586 -0.06083 0.13311] [ 0.87717 0.30098 0.37415 0.77404] [ 0. 0. 0. 1. ]] jaxsim[100706] DEBUG W_H_F_iDynTree: jaxsim[100706] DEBUG [[-0.80645 -0.10969 -0.58104 -0.02238] [ 0.46848 -0.71808 -0.51467 0.27463] [-0.36078 -0.68726 0.63049 0.81591] [ 0. 0. 0. 1. ]] jaxsim[100706] ERROR Assertion failed for frame: r_shoulder_3 jaxsim[100706] DEBUG W_H_F_js: jaxsim[100706] DEBUG [[-0.35031 -0.67322 -0.6512 0.46478] [ 0.928 -0.34364 -0.14396 0.34975] [-0.12686 -0.65474 0.74513 0.41312] [ 0. 0. 0. 1. ]] jaxsim[100706] DEBUG W_H_F_iDynTree: jaxsim[100706] DEBUG [[-0.91833 -0.10879 -0.38058 -0.17789] [ 0.21158 -0.94752 -0.23966 0.31045] [-0.33454 -0.30061 0.89315 1.20324] [ 0. 0. 0. 1. ]] ```

[xela-95]

The math compute by the jacobian function is pretty simple:

• Get the parent link transform ${}^WH_L$
• Get the pose of the frame with respect to its parent link ${}^FH_L$ by accessing the pose attribute of the frame https://github.com/ami-iit/jaxsim/blob/d7cfcc6b7c8aab3528a5c81e3a67f157dddade27/src/jaxsim/parsers/descriptions/link.py#L33
• Compute ${}^WH_F = {}^WH_L \cdot {}^LH_F$

Now, since this test is passing for the majority of the frames and failing only for 6 of them, what I'm suspecting is that maybe is not always true that the pose of the frame is relative to its parent link. What do you think @diegoferigo?

[traversaro]

Right now on ErgoCubReduced the jacobians that are not matching are the ones for the frames:

* l_foot_front

* l_foot_rear

* l_hip_3

* l_shoulder_3

* r_hip_3

* r_shoulder3

These test cases are failing in all 3 velocity representations (inertial, body and fixed). All the other frames are passing the test.

The specificity of all this frames is that they are not leaf "fake link frames" but rather proper links (with an inertia) that are lumped to their parents. Perhaps the lumping is not working as expected either in iDynTree or rod? Do we have a check that simply checks forward kinematics for those frames, instead of checking the jacobian?

[traversaro]

Here's the log containg the homogeneous trasnforms from world to frame that are failing: Logs

jaxsim[100706] ERROR Assertion failed for frame: l_foot_front
jaxsim[100706] DEBUG W_H_F_js:
jaxsim[100706] DEBUG [[-0.35031 -0.67322 -0.6512   0.41084]
 [ 0.928   -0.34364 -0.14396  0.4807 ]
 [-0.12686 -0.65474  0.74513  0.39986]
 [ 0.       0.       0.       1.     ]]
jaxsim[100706] DEBUG W_H_F_iDynTree:
jaxsim[100706] DEBUG [[-0.54756 -0.36036 -0.75519 -0.36317]
 [-0.15713 -0.84218  0.51579 -0.03562]
 [-0.82188  0.40109  0.40453  0.33124]
 [ 0.       0.       0.       1.     ]]
jaxsim[100706] ERROR Assertion failed for frame: l_foot_rear
jaxsim[100706] DEBUG W_H_F_js:
jaxsim[100706] DEBUG [[-0.35031 -0.67322 -0.6512   0.45261]
 [ 0.928   -0.34364 -0.14396  0.37004]
 [-0.12686 -0.65474  0.74513  0.41499]
 [ 0.       0.       0.       1.     ]]
jaxsim[100706] DEBUG W_H_F_iDynTree:
jaxsim[100706] DEBUG [[-0.54756 -0.36036 -0.75519 -0.29787]
 [-0.15713 -0.84218  0.51579 -0.01688]
 [-0.82188  0.40109  0.40453  0.42925]
 [ 0.       0.       0.       1.     ]]
jaxsim[100706] ERROR Assertion failed for frame: l_hip_3
jaxsim[100706] DEBUG W_H_F_js:
jaxsim[100706] DEBUG [[-0.80645 -0.10969 -0.58104 -0.02238]
 [ 0.46848 -0.71808 -0.51467  0.27463]
 [-0.36078 -0.68726  0.63049  0.81591]
 [ 0.       0.       0.       1.     ]]
jaxsim[100706] DEBUG W_H_F_iDynTree:
jaxsim[100706] DEBUG [[-0.37434 -0.05964  0.92537 -0.24618]
 [ 0.00864 -0.99811 -0.06083  0.09552]
 [ 0.92725 -0.01478  0.37415  0.80239]
 [ 0.       0.       0.       1.     ]]
jaxsim[100706] ERROR Assertion failed for frame: l_shoulder_3
jaxsim[100706] DEBUG W_H_F_js:
jaxsim[100706] DEBUG [[-0.35031 -0.67322 -0.6512   0.46391]
 [ 0.928   -0.34364 -0.14396  0.35207]
 [-0.12686 -0.65474  0.74513  0.4128 ]
 [ 0.       0.       0.       1.     ]]
jaxsim[100706] DEBUG W_H_F_iDynTree:
jaxsim[100706] DEBUG [[-0.6807  -0.7098  -0.18119 -0.32363]
 [ 0.69106 -0.70426  0.16267  0.01637]
 [-0.24307 -0.01448  0.9699   1.14715]
 [ 0.       0.       0.       1.     ]]
jaxsim[100706] ERROR Assertion failed for frame: r_hip_3
jaxsim[100706] DEBUG W_H_F_js:
jaxsim[100706] DEBUG [[-0.33184 -0.18322  0.92537 -0.40775]
 [ 0.34707 -0.93586 -0.06083  0.13311]
 [ 0.87717  0.30098  0.37415  0.77404]
 [ 0.       0.       0.       1.     ]]
jaxsim[100706] DEBUG W_H_F_iDynTree:
jaxsim[100706] DEBUG [[-0.80645 -0.10969 -0.58104 -0.02238]
 [ 0.46848 -0.71808 -0.51467  0.27463]
 [-0.36078 -0.68726  0.63049  0.81591]
 [ 0.       0.       0.       1.     ]]
jaxsim[100706] ERROR Assertion failed for frame: r_shoulder_3
jaxsim[100706] DEBUG W_H_F_js:
jaxsim[100706] DEBUG [[-0.35031 -0.67322 -0.6512   0.46478]
 [ 0.928   -0.34364 -0.14396  0.34975]
 [-0.12686 -0.65474  0.74513  0.41312]
 [ 0.       0.       0.       1.     ]]
jaxsim[100706] DEBUG W_H_F_iDynTree:
jaxsim[100706] DEBUG [[-0.91833 -0.10879 -0.38058 -0.17789]
 [ 0.21158 -0.94752 -0.23966  0.31045]
 [-0.33454 -0.30061  0.89315  1.20324]
 [ 0.       0.       0.       1.     ]]

These transform are complex. Can you try to set the w_H_B to identity and the joint position to zero, and try again? In that condition the W_H_F rotation of all the leg frames should be the identity, making it more easy to debug.

[traversaro]

Right now on ErgoCubReduced the jacobians that are not matching are the ones for the frames:

* l_foot_front

* l_foot_rear

* l_hip_3

* l_shoulder_3

* r_hip_3

* r_shoulder3

These test cases are failing in all 3 velocity representations (inertial, body and fixed). All the other frames are passing the test.

The specificity of all this frames is that they are not leaf "fake link frames" but rather proper links (with an inertia) that are lumped to their parents. Perhaps the lumping is not working as expected either in iDynTree or rod? Do we have a check that simply checks forward kinematics for those frames, instead of checking the jacobian?

Sorry, I read later https://github.com/ami-iit/jaxsim/pull/148#issuecomment-2104215974, this is already happening.

[xela-95]

These transform are complex. Can you try to set the w_H_B to identity and the joint position to zero, and try again? In that condition the W_H_F rotation of all the leg frames should be the identity, making it more easy to debug.

This is a good idea, but I do not know how to change base and joint position programmatically. I'll try to understand how to do this.

[diegoferigo]

These transform are complex. Can you try to set the w_H_B to identity and the joint position to zero, and try again? In that condition the W_H_F rotation of all the leg frames should be the identity, making it more easy to debug.

This is a good idea, but I do not know how to change base and joint position programmatically. I'll try to understand how to do this.

If you comment out these lines, by default JaxSimModelData is populated with a trivial orientation and zero data (check zero). Then, you can use the reset* methods to set only parts of the configuration.

[diegoferigo]

Now, since this test is passing for the majority of the frames and failing only for 6 of them, what I'm suspecting is that maybe is not always true that the pose of the frame is relative to its parent link. What do you think @diegoferigo?

Maybe, I never tested thoroughly the frame-related logic since this is the first time we are using it. I suspect there might be a bug when the pose of the frames is resolved. This is called when the URDF exported of rod switches to FrameConvention.Urdf here.

[diegoferigo]

Now, since this test is passing for the majority of the frames and failing only for 6 of them, what I'm suspecting is that maybe is not always true that the pose of the frame is relative to its parent link. What do you think @diegoferigo?

Maybe, I never tested thoroughly the frame-related logic since this is the first time we are using it. I suspect there might be a bug when the pose of the frames is resolved. This is called when the URDF exported of rod switches to FrameConvention.Urdf here.

Mmh, I see something smelly here and here. By visual inspection it seems that the transform stored in frame.pose is not ${}^L \mathbf{H}_F$ but rather ${}^M \mathbf{H}_F$, where $M$ is the model frame that in most cases is the base link.

[xela-95]

Here's the result of the transform unit test using default pose, setted using

    data = js.data.JaxSimModelData.zero(model=model)

Details

``` jaxsim[139400] ERROR Assertion failed for frame: l_foot_front jaxsim[139400] DEBUG W_H_F_js: jaxsim[139400] DEBUG [[ 1. -0. 0. 0.10365] [ 0. 1. -0. -0.0744 ] [-0. 0. 1. -0.755 ] [ 0. 0. 0. 1. ]] jaxsim[139400] DEBUG W_H_F_iDynTree: jaxsim[139400] DEBUG [[ 1. -0. 0. 0.10365] [ 0. 1. -0. 0.0744 ] [-0. 0. 1. -0.755 ] [ 0. 0. 0. 1. ]] jaxsim[139400] ERROR Assertion failed for frame: l_foot_rear jaxsim[139400] DEBUG W_H_F_js: jaxsim[139400] DEBUG [[ 1. -0. 0. -0.0156] [ 0. 1. -0. -0.0744] [-0. 0. 1. -0.755 ] [ 0. 0. 0. 1. ]] jaxsim[139400] DEBUG W_H_F_iDynTree: jaxsim[139400] DEBUG [[ 1. -0. 0. -0.0156] [ 0. 1. -0. 0.0744] [-0. 0. 1. -0.755 ] [ 0. 0. 0. 1. ]] jaxsim[139400] ERROR Assertion failed for frame: l_hip_3 jaxsim[139400] DEBUG W_H_F_js: jaxsim[139400] DEBUG [[ 1. 0. 0. 0.01055] [-0. 1. -0. -0.0836 ] [-0. 0. 1. -0.0987 ] [ 0. 0. 0. 1. ]] jaxsim[139400] DEBUG W_H_F_iDynTree: jaxsim[139400] DEBUG [[ 1. -0. 0. 0.01055] [ 0. 1. -0. 0.0836 ] [-0. 0. 1. -0.0987 ] [ 0. 0. 0. 1. ]] jaxsim[139400] ERROR Assertion failed for frame: l_shoulder_3 jaxsim[139400] DEBUG W_H_F_js: jaxsim[139400] DEBUG [[ 1. -0. 0. -0.03595] [ 0. 1. -0. -0.0744 ] [-0. 0. 1. -0.7614 ] [ 0. 0. 0. 1. ]] jaxsim[139400] DEBUG W_H_F_iDynTree: jaxsim[139400] DEBUG [[ 0.96105 0.25056 -0.11665 0.01682] [-0.27145 0.93511 -0.22777 0.16649] [ 0.05201 0.25056 0.9667 0.33741] [ 0. 0. 0. 1. ]] jaxsim[139400] ERROR Assertion failed for frame: r_hip_3 jaxsim[139400] DEBUG W_H_F_js: jaxsim[139400] DEBUG [[ 1. -0. 0. 0.04725] [ 0. 1. -0. 0.0686 ] [-0. 0. 1. -0.2611 ] [ 0. 0. 0. 1. ]] jaxsim[139400] DEBUG W_H_F_iDynTree: jaxsim[139400] DEBUG [[ 1. 0. 0. 0.01055] [-0. 1. -0. -0.0836 ] [-0. 0. 1. -0.0987 ] [ 0. 0. 0. 1. ]] jaxsim[139400] ERROR Assertion failed for frame: r_shoulder_3 jaxsim[139400] DEBUG W_H_F_js: jaxsim[139400] DEBUG [[ 1. -0. 0. -0.03845] [ 0. 1. -0. -0.0744 ] [-0. 0. 1. -0.7614 ] [ 0. 0. 0. 1. ]] jaxsim[139400] DEBUG W_H_F_iDynTree: jaxsim[139400] DEBUG [[ 0.96105 -0.25056 -0.11665 0.01682] [ 0.27145 0.93511 0.22777 -0.16649] [ 0.05201 -0.25056 0.9667 0.33741] [ 0. 0. 0. 1. ]] ```

[diegoferigo]

@xela-95 can you temporarily change your code assuming what I wrote in https://github.com/ami-iit/jaxsim/pull/148#issuecomment-2104283044 is true? It should be faster to update api.frame.transform rather than modifying rod. If this turns out to be correct, then we need to fix rod.

[xela-95]

Mmh, I see something smelly here. By visual inspection it seems that the transform stored in frame.pose is not 𝐿 𝐻 𝐹 but rather 𝑀 𝐻 𝐹 , where 𝑀 is the model frame that in most cases is the base link.

Ok I see! Thanks for the help!

[xela-95]

@xela-95 can you temporarily change your code assuming what I wrote in #148 (comment) is true? It should be faster to update api.frame.transform rather than modifying rod. If this turns out to be correct, then we need to fix rod.

sure!

[diegoferigo]

Mmh I investigated a bit further. Parsed ROD models that are created after exporting to URDF should already have the pose of frames relative to their parent link. You can inspect the logic here.

[traversaro]

Here's the result of the transform unit test using default pose, setted using

    data = js.data.JaxSimModelData.zero(model=model)

Details

jaxsim[139400] ERROR Assertion failed for frame: l_foot_front
jaxsim[139400] DEBUG W_H_F_js:
jaxsim[139400] DEBUG [[ 1.      -0.       0.       0.10365]
 [ 0.       1.      -0.      -0.0744 ]
 [-0.       0.       1.      -0.755  ]
 [ 0.       0.       0.       1.     ]]
jaxsim[139400] DEBUG W_H_F_iDynTree:
jaxsim[139400] DEBUG [[ 1.      -0.       0.       0.10365]
 [ 0.       1.      -0.       0.0744 ]
 [-0.       0.       1.      -0.755  ]
 [ 0.       0.       0.       1.     ]]
jaxsim[139400] ERROR Assertion failed for frame: l_foot_rear
jaxsim[139400] DEBUG W_H_F_js:
jaxsim[139400] DEBUG [[ 1.     -0.      0.     -0.0156]
 [ 0.      1.     -0.     -0.0744]
 [-0.      0.      1.     -0.755 ]
 [ 0.      0.      0.      1.    ]]
jaxsim[139400] DEBUG W_H_F_iDynTree:
jaxsim[139400] DEBUG [[ 1.     -0.      0.     -0.0156]
 [ 0.      1.     -0.      0.0744]
 [-0.      0.      1.     -0.755 ]
 [ 0.      0.      0.      1.    ]]
jaxsim[139400] ERROR Assertion failed for frame: l_hip_3
jaxsim[139400] DEBUG W_H_F_js:
jaxsim[139400] DEBUG [[ 1.       0.       0.       0.01055]
 [-0.       1.      -0.      -0.0836 ]
 [-0.       0.       1.      -0.0987 ]
 [ 0.       0.       0.       1.     ]]
jaxsim[139400] DEBUG W_H_F_iDynTree:
jaxsim[139400] DEBUG [[ 1.      -0.       0.       0.01055]
 [ 0.       1.      -0.       0.0836 ]
 [-0.       0.       1.      -0.0987 ]
 [ 0.       0.       0.       1.     ]]
jaxsim[139400] ERROR Assertion failed for frame: l_shoulder_3
jaxsim[139400] DEBUG W_H_F_js:
jaxsim[139400] DEBUG [[ 1.      -0.       0.      -0.03595]
 [ 0.       1.      -0.      -0.0744 ]
 [-0.       0.       1.      -0.7614 ]
 [ 0.       0.       0.       1.     ]]
jaxsim[139400] DEBUG W_H_F_iDynTree:
jaxsim[139400] DEBUG [[ 0.96105  0.25056 -0.11665  0.01682]
 [-0.27145  0.93511 -0.22777  0.16649]
 [ 0.05201  0.25056  0.9667   0.33741]
 [ 0.       0.       0.       1.     ]]
jaxsim[139400] ERROR Assertion failed for frame: r_hip_3
jaxsim[139400] DEBUG W_H_F_js:
jaxsim[139400] DEBUG [[ 1.      -0.       0.       0.04725]
 [ 0.       1.      -0.       0.0686 ]
 [-0.       0.       1.      -0.2611 ]
 [ 0.       0.       0.       1.     ]]
jaxsim[139400] DEBUG W_H_F_iDynTree:
jaxsim[139400] DEBUG [[ 1.       0.       0.       0.01055]
 [-0.       1.      -0.      -0.0836 ]
 [-0.       0.       1.      -0.0987 ]
 [ 0.       0.       0.       1.     ]]
jaxsim[139400] ERROR Assertion failed for frame: r_shoulder_3
jaxsim[139400] DEBUG W_H_F_js:
jaxsim[139400] DEBUG [[ 1.      -0.       0.      -0.03845]
 [ 0.       1.      -0.      -0.0744 ]
 [-0.       0.       1.      -0.7614 ]
 [ 0.       0.       0.       1.     ]]
jaxsim[139400] DEBUG W_H_F_iDynTree:
jaxsim[139400] DEBUG [[ 0.96105 -0.25056 -0.11665  0.01682]
 [ 0.27145  0.93511  0.22777 -0.16649]
 [ 0.05201 -0.25056  0.9667   0.33741]
 [ 0.       0.       0.       1.     ]]

There is a clear sign error on the y of l_foot_front and similar frames, maybe that is easier to debug. Can you print the transform between l_foot_front and its parent link? In iDynTree this can be computed with model.getFrameTransform(model.getFrameLinkIndex("l_foot_front")), not sure how to do the same in jaxsim/rod.

[traversaro]

There is a clear sign error on the y of l_foot_front and similar frames, maybe that is easier to debug. Can you print the transform between l_foot_front and its parent link? In iDynTree this can be computed with model.getFrameTransform(model.getFrameLinkIndex("l_foot_front")), not sure how to do the same in jaxsim/rod.

If that is correct, I would also double check to which link those frames are connected. Again, in iDynTree you can do that with model.getLinkName(model.getFrameLink(model.getFrameLinkIndex("l_foot_front"))).

[xela-95]

By updating the transform function as suggested in https://github.com/ami-iit/jaxsim/pull/148#issuecomment-2104292526:

@functools.partial(jax.jit, static_argnames=("frame_index",))
def transform(
    model: js.model.JaxSimModel,
    data: js.data.JaxSimModelData,
    *,
    frame_index: jtp.IntLike,
) -> jtp.Matrix:
    """
    Compute the SE(3) transform from the world frame to the specified frame.

    Args:
        model: The model to consider.
        data: The data of the considered model.
        frame_index: The index of the frame for which the transform is requested.

    Returns:
        The 4x4 matrix representing the transform.
    """

    F = model.description.get().frames[frame_index - model.number_of_links()]
    B_H_F = F.pose
    # get the base link transform
    B = model.link_names().index(model.base_link())
    W_H_B = js.link.transform(model=model, data=data, link_index=B)

    return W_H_B @ B_H_F

I get the following results. The unit test is still failing for a larger subset of frames:

Logs

``` jaxsim[159414] ERROR Assertion failed for frame: l_foot_front jaxsim[159414] DEBUG W_H_F_js: jaxsim[159414] DEBUG [[ 1. 0. 0. 0.13525] [-0. 1. 0. -0. ] [-0. -0. 1. -0.0479 ] [ 0. 0. 0. 1. ]] jaxsim[159414] DEBUG W_H_F_iDynTree: jaxsim[159414] DEBUG [[ 1. -0. 0. 0.10365] [ 0. 1. -0. 0.0744 ] [-0. 0. 1. -0.755 ] [ 0. 0. 0. 1. ]] jaxsim[159414] ERROR Assertion failed for frame: l_foot_rear jaxsim[159414] DEBUG W_H_F_js: jaxsim[159414] DEBUG [[ 1. -0. 0. 0.016 ] [ 0. 1. 0. -0. ] [-0. -0. 1. -0.0479] [ 0. 0. 0. 1. ]] jaxsim[159414] DEBUG W_H_F_iDynTree: jaxsim[159414] DEBUG [[ 1. -0. 0. -0.0156] [ 0. 1. -0. 0.0744] [-0. 0. 1. -0.755 ] [ 0. 0. 0. 1. ]] jaxsim[159414] ERROR Assertion failed for frame: l_hip_3 jaxsim[159414] DEBUG W_H_F_js: jaxsim[159414] DEBUG [[ 1. -0. -0. 0.0367] [ 0. 1. 0. -0. ] [ 0. -0. 1. -0.0897] [ 0. 0. 0. 1. ]] jaxsim[159414] DEBUG W_H_F_iDynTree: jaxsim[159414] DEBUG [[ 1. -0. 0. 0.01055] [ 0. 1. -0. 0.0836 ] [-0. 0. 1. -0.0987 ] [ 0. 0. 0. 1. ]] jaxsim[159414] ERROR Assertion failed for frame: l_shoulder_3 jaxsim[159414] DEBUG W_H_F_js: jaxsim[159414] DEBUG [[ 1. -0. -0. -0.00435] [ 0. 1. -0. -0. ] [-0. 0. 1. -0.0543 ] [ 0. 0. 0. 1. ]] jaxsim[159414] DEBUG W_H_F_iDynTree: jaxsim[159414] DEBUG [[ 0.96105 0.25056 -0.11665 0.01682] [-0.27145 0.93511 -0.22777 0.16649] [ 0.05201 0.25056 0.9667 0.33741] [ 0. 0. 0. 1. ]] jaxsim[159414] ERROR Assertion failed for frame: r_foot_front jaxsim[159414] DEBUG W_H_F_js: jaxsim[159414] DEBUG [[ 1. 0. 0. 0.13525] [-0. 1. -0. 0. ] [-0. 0. 1. -0.0479 ] [ 0. 0. 0. 1. ]] jaxsim[159414] DEBUG W_H_F_iDynTree: jaxsim[159414] DEBUG [[ 1. -0. 0. 0.10365] [ 0. 1. -0. -0.0744 ] [-0. 0. 1. -0.755 ] [ 0. 0. 0. 1. ]] jaxsim[159414] ERROR Assertion failed for frame: r_foot_rear jaxsim[159414] DEBUG W_H_F_js: jaxsim[159414] DEBUG [[ 1. 0. 0. 0.016 ] [-0. 1. -0. 0. ] [-0. 0. 1. -0.0479] [ 0. 0. 0. 1. ]] jaxsim[159414] DEBUG W_H_F_iDynTree: jaxsim[159414] DEBUG [[ 1. -0. 0. -0.0156] [ 0. 1. -0. -0.0744] [-0. 0. 1. -0.755 ] [ 0. 0. 0. 1. ]] jaxsim[159414] ERROR Assertion failed for frame: r_hip_3 jaxsim[159414] DEBUG W_H_F_js: jaxsim[159414] DEBUG [[ 1. -0. 0. 0.0367] [ 0. 1. 0. -0. ] [-0. -0. 1. -0.0897] [ 0. 0. 0. 1. ]] jaxsim[159414] DEBUG W_H_F_iDynTree: jaxsim[159414] DEBUG [[ 1. 0. 0. 0.01055] [-0. 1. -0. -0.0836 ] [-0. 0. 1. -0.0987 ] [ 0. 0. 0. 1. ]] jaxsim[159414] ERROR Assertion failed for frame: r_shoulder_3 jaxsim[159414] DEBUG W_H_F_js: jaxsim[159414] DEBUG [[ 1. -0. -0. -0.00685] [ 0. 1. 0. -0. ] [ 0. -0. 1. -0.0543 ] [ 0. 0. 0. 1. ]] jaxsim[159414] DEBUG W_H_F_iDynTree: jaxsim[159414] DEBUG [[ 0.96105 -0.25056 -0.11665 0.01682] [ 0.27145 0.93511 0.22777 -0.16649] [ 0.05201 -0.25056 0.9667 0.33741] [ 0. 0. 0. 1. ]] FAILED ```

[xela-95]

Mmh I investigated a bit further. Parsed ROD models that are created after exporting to URDF should already have the pose of frames relative to their parent link. You can inspect the logic here.

I've read this now. Thanks for the investigation. Ok so I can revert the previous code of the transform function.

[diegoferigo]

Let's make a zoom out because now I struggle to follow:

• iDynTree and JaxSim, in the test, build their model from the URDF exported by ROD. Therefore, if there is a mismatch, the problem is not related to how ROD generates the URDF file.
• However, iDynTree parses directly the URDF, instead JaxSim converts to SDF using sdformat and then reads the resulting data.
• The failing test is using the full model of ErgoCub, therefore the complicated model reduction logic of JaxSim (triggered by jaxsim.api.model.reduce cannot be the culprit.

At this point, my main suspicion is that we do some mistake in JaxSim during the construction of KinematicGraph when all the fixed joints of the URDF model are removed through an initial reduction logic.

@xela-95 do you confirm that the frames that fail are those populated in that piece of code?

[xela-95]

• The failing test is using the full model of ErgoCub, therefore the complicated model reduction logic of JaxSim (triggered by jaxsim.api.model.reduce cannot be the culprit.

Since in the test I'm using the jaxsim_models_types fixture as test argument, I think this uses the reduced model of ergocub.

@xela-95 do you confirm that the frames that fail are those populated in that piece of code?

I'm not sure to have understood you're question

[xela-95]

I think we found a part of the strange behaviour!

By adding a function to utils_idyntree in https://github.com/ami-iit/jaxsim/pull/148/files#diff-36980de6eb353f3a240b97fa6b37e735c4c0abca7acb9d33aca0d3d7e04c71cbR351-R357 we are able to extract the name of the parent link used in iDynTree and compare that with the one used by Jaxsim.

They are different just for r_shoulder_3 and l_shoulder_3. For the others cases (passing and failing) the parent link is the same.

Check the logs below:

Details

``` jaxsim[216979] DEBUG In iDynTree the frame "chest" is connected to link root_link jaxsim[216979] DEBUG In Jaxsim the frame "chest" is connected to link root_link jaxsim[216979] DEBUG In iDynTree the frame "head" is connected to link root_link jaxsim[216979] DEBUG In Jaxsim the frame "head" is connected to link root_link jaxsim[216979] DEBUG In iDynTree the frame "l_foot_front" is connected to link l_ankle_2 jaxsim[216979] DEBUG In Jaxsim the frame "l_foot_front" is connected to link l_ankle_2 jaxsim[216979] ERROR Assertion failed for frame: l_foot_front jaxsim[216979] DEBUG W_H_F_js: jaxsim[216979] DEBUG [[ 1. -0. 0. 0.10365] [ 0. 1. -0. -0.0744 ] [-0. 0. 1. -0.755 ] [ 0. 0. 0. 1. ]] jaxsim[216979] DEBUG W_H_F_iDynTree: jaxsim[216979] DEBUG [[ 1. -0. 0. 0.10365] [ 0. 1. -0. 0.0744 ] [-0. 0. 1. -0.755 ] [ 0. 0. 0. 1. ]] jaxsim[216979] DEBUG In iDynTree the frame "l_foot_rear" is connected to link l_ankle_2 jaxsim[216979] DEBUG In Jaxsim the frame "l_foot_rear" is connected to link l_ankle_2 jaxsim[216979] ERROR Assertion failed for frame: l_foot_rear jaxsim[216979] DEBUG W_H_F_js: jaxsim[216979] DEBUG [[ 1. -0. 0. -0.0156] [ 0. 1. -0. -0.0744] [-0. 0. 1. -0.755 ] [ 0. 0. 0. 1. ]] jaxsim[216979] DEBUG W_H_F_iDynTree: jaxsim[216979] DEBUG [[ 1. -0. 0. -0.0156] [ 0. 1. -0. 0.0744] [-0. 0. 1. -0.755 ] [ 0. 0. 0. 1. ]] jaxsim[216979] DEBUG In iDynTree the frame "l_forearm" is connected to link root_link jaxsim[216979] DEBUG In Jaxsim the frame "l_forearm" is connected to link root_link jaxsim[216979] DEBUG In iDynTree the frame "l_hand_index_1" is connected to link root_link jaxsim[216979] DEBUG In Jaxsim the frame "l_hand_index_1" is connected to link root_link jaxsim[216979] DEBUG In iDynTree the frame "l_hand_index_2" is connected to link root_link jaxsim[216979] DEBUG In Jaxsim the frame "l_hand_index_2" is connected to link root_link jaxsim[216979] DEBUG In iDynTree the frame "l_hand_index_3" is connected to link root_link jaxsim[216979] DEBUG In Jaxsim the frame "l_hand_index_3" is connected to link root_link jaxsim[216979] DEBUG In iDynTree the frame "l_hand_middle_1" is connected to link root_link jaxsim[216979] DEBUG In Jaxsim the frame "l_hand_middle_1" is connected to link root_link jaxsim[216979] DEBUG In iDynTree the frame "l_hand_middle_2" is connected to link root_link jaxsim[216979] DEBUG In Jaxsim the frame "l_hand_middle_2" is connected to link root_link jaxsim[216979] DEBUG In iDynTree the frame "l_hand_palm" is connected to link root_link jaxsim[216979] DEBUG In Jaxsim the frame "l_hand_palm" is connected to link root_link jaxsim[216979] DEBUG In iDynTree the frame "l_hand_pinkie_1" is connected to link root_link jaxsim[216979] DEBUG In Jaxsim the frame "l_hand_pinkie_1" is connected to link root_link jaxsim[216979] DEBUG In iDynTree the frame "l_hand_pinkie_2" is connected to link root_link jaxsim[216979] DEBUG In Jaxsim the frame "l_hand_pinkie_2" is connected to link root_link jaxsim[216979] DEBUG In iDynTree the frame "l_hand_ring_1" is connected to link root_link jaxsim[216979] DEBUG In Jaxsim the frame "l_hand_ring_1" is connected to link root_link jaxsim[216979] DEBUG In iDynTree the frame "l_hand_ring_2" is connected to link root_link jaxsim[216979] DEBUG In Jaxsim the frame "l_hand_ring_2" is connected to link root_link jaxsim[216979] DEBUG In iDynTree the frame "l_hand_thumb_1" is connected to link root_link jaxsim[216979] DEBUG In Jaxsim the frame "l_hand_thumb_1" is connected to link root_link jaxsim[216979] DEBUG In iDynTree the frame "l_hand_thumb_2" is connected to link root_link jaxsim[216979] DEBUG In Jaxsim the frame "l_hand_thumb_2" is connected to link root_link jaxsim[216979] DEBUG In iDynTree the frame "l_hand_thumb_3" is connected to link root_link jaxsim[216979] DEBUG In Jaxsim the frame "l_hand_thumb_3" is connected to link root_link jaxsim[216979] DEBUG In iDynTree the frame "l_hip_3" is connected to link l_hip_2 jaxsim[216979] DEBUG In Jaxsim the frame "l_hip_3" is connected to link l_hip_2 jaxsim[216979] ERROR Assertion failed for frame: l_hip_3 jaxsim[216979] DEBUG W_H_F_js: jaxsim[216979] DEBUG [[ 1. 0. 0. 0.01055] [-0. 1. -0. -0.0836 ] [-0. 0. 1. -0.0987 ] [ 0. 0. 0. 1. ]] jaxsim[216979] DEBUG W_H_F_iDynTree: jaxsim[216979] DEBUG [[ 1. -0. 0. 0.01055] [ 0. 1. -0. 0.0836 ] [-0. 0. 1. -0.0987 ] [ 0. 0. 0. 1. ]] jaxsim[216979] DEBUG In iDynTree the frame "l_shoulder_1" is connected to link root_link jaxsim[216979] DEBUG In Jaxsim the frame "l_shoulder_1" is connected to link root_link jaxsim[216979] DEBUG In iDynTree the frame "l_shoulder_2" is connected to link root_link jaxsim[216979] DEBUG In Jaxsim the frame "l_shoulder_2" is connected to link root_link jaxsim[216979] DEBUG In iDynTree the frame "l_shoulder_3" is connected to link root_link jaxsim[216979] DEBUG In Jaxsim the frame "l_shoulder_3" is connected to link l_shoulder_2 jaxsim[216979] ERROR Assertion failed for frame: l_shoulder_3 jaxsim[216979] DEBUG W_H_F_js: jaxsim[216979] DEBUG [[ 1. -0. 0. -0.03595] [ 0. 1. -0. -0.0744 ] [-0. 0. 1. -0.7614 ] [ 0. 0. 0. 1. ]] jaxsim[216979] DEBUG W_H_F_iDynTree: jaxsim[216979] DEBUG [[ 0.96105 0.25056 -0.11665 0.01682] [-0.27145 0.93511 -0.22777 0.16649] [ 0.05201 0.25056 0.9667 0.33741] [ 0. 0. 0. 1. ]] jaxsim[216979] DEBUG In iDynTree the frame "l_upper_arm" is connected to link root_link jaxsim[216979] DEBUG In Jaxsim the frame "l_upper_arm" is connected to link root_link jaxsim[216979] DEBUG In iDynTree the frame "l_wrist_1" is connected to link root_link jaxsim[216979] DEBUG In Jaxsim the frame "l_wrist_1" is connected to link root_link jaxsim[216979] DEBUG In iDynTree the frame "l_wrist_2" is connected to link root_link jaxsim[216979] DEBUG In Jaxsim the frame "l_wrist_2" is connected to link root_link jaxsim[216979] DEBUG In iDynTree the frame "neck_2" is connected to link root_link jaxsim[216979] DEBUG In Jaxsim the frame "neck_2" is connected to link root_link jaxsim[216979] DEBUG In iDynTree the frame "neck_3" is connected to link root_link jaxsim[216979] DEBUG In Jaxsim the frame "neck_3" is connected to link root_link jaxsim[216979] DEBUG In iDynTree the frame "r_foot_front" is connected to link r_ankle_2 jaxsim[216979] DEBUG In Jaxsim the frame "r_foot_front" is connected to link r_ankle_2 jaxsim[216979] DEBUG In iDynTree the frame "r_foot_rear" is connected to link r_ankle_2 jaxsim[216979] DEBUG In Jaxsim the frame "r_foot_rear" is connected to link r_ankle_2 jaxsim[216979] DEBUG In iDynTree the frame "r_forearm" is connected to link root_link jaxsim[216979] DEBUG In Jaxsim the frame "r_forearm" is connected to link root_link jaxsim[216979] DEBUG In iDynTree the frame "r_hand_index_1" is connected to link root_link jaxsim[216979] DEBUG In Jaxsim the frame "r_hand_index_1" is connected to link root_link jaxsim[216979] DEBUG In iDynTree the frame "r_hand_index_2" is connected to link root_link jaxsim[216979] DEBUG In Jaxsim the frame "r_hand_index_2" is connected to link root_link jaxsim[216979] DEBUG In iDynTree the frame "r_hand_index_3" is connected to link root_link jaxsim[216979] DEBUG In Jaxsim the frame "r_hand_index_3" is connected to link root_link jaxsim[216979] DEBUG In iDynTree the frame "r_hand_middle_1" is connected to link root_link jaxsim[216979] DEBUG In Jaxsim the frame "r_hand_middle_1" is connected to link root_link jaxsim[216979] DEBUG In iDynTree the frame "r_hand_middle_2" is connected to link root_link jaxsim[216979] DEBUG In Jaxsim the frame "r_hand_middle_2" is connected to link root_link jaxsim[216979] DEBUG In iDynTree the frame "r_hand_palm" is connected to link root_link jaxsim[216979] DEBUG In Jaxsim the frame "r_hand_palm" is connected to link root_link jaxsim[216979] DEBUG In iDynTree the frame "r_hand_pinkie_1" is connected to link root_link jaxsim[216979] DEBUG In Jaxsim the frame "r_hand_pinkie_1" is connected to link root_link jaxsim[216979] DEBUG In iDynTree the frame "r_hand_pinkie_2" is connected to link root_link jaxsim[216979] DEBUG In Jaxsim the frame "r_hand_pinkie_2" is connected to link root_link jaxsim[216979] DEBUG In iDynTree the frame "r_hand_ring_1" is connected to link root_link jaxsim[216979] DEBUG In Jaxsim the frame "r_hand_ring_1" is connected to link root_link jaxsim[216979] DEBUG In iDynTree the frame "r_hand_ring_2" is connected to link root_link jaxsim[216979] DEBUG In Jaxsim the frame "r_hand_ring_2" is connected to link root_link jaxsim[216979] DEBUG In iDynTree the frame "r_hand_thumb_1" is connected to link root_link jaxsim[216979] DEBUG In Jaxsim the frame "r_hand_thumb_1" is connected to link root_link jaxsim[216979] DEBUG In iDynTree the frame "r_hand_thumb_2" is connected to link root_link jaxsim[216979] DEBUG In Jaxsim the frame "r_hand_thumb_2" is connected to link root_link jaxsim[216979] DEBUG In iDynTree the frame "r_hand_thumb_3" is connected to link root_link jaxsim[216979] DEBUG In Jaxsim the frame "r_hand_thumb_3" is connected to link root_link jaxsim[216979] DEBUG In iDynTree the frame "r_hip_3" is connected to link r_hip_2 jaxsim[216979] DEBUG In Jaxsim the frame "r_hip_3" is connected to link r_hip_2 jaxsim[216979] ERROR Assertion failed for frame: r_hip_3 jaxsim[216979] DEBUG W_H_F_js: jaxsim[216979] DEBUG [[ 1. -0. 0. 0.04725] [ 0. 1. -0. 0.0686 ] [-0. 0. 1. -0.2611 ] [ 0. 0. 0. 1. ]] jaxsim[216979] DEBUG W_H_F_iDynTree: jaxsim[216979] DEBUG [[ 1. 0. 0. 0.01055] [-0. 1. -0. -0.0836 ] [-0. 0. 1. -0.0987 ] [ 0. 0. 0. 1. ]] jaxsim[216979] DEBUG In iDynTree the frame "r_shoulder_1" is connected to link root_link jaxsim[216979] DEBUG In Jaxsim the frame "r_shoulder_1" is connected to link root_link jaxsim[216979] DEBUG In iDynTree the frame "r_shoulder_2" is connected to link root_link jaxsim[216979] DEBUG In Jaxsim the frame "r_shoulder_2" is connected to link root_link jaxsim[216979] DEBUG In iDynTree the frame "r_shoulder_3" is connected to link root_link jaxsim[216979] DEBUG In Jaxsim the frame "r_shoulder_3" is connected to link r_shoulder_2 jaxsim[216979] ERROR Assertion failed for frame: r_shoulder_3 jaxsim[216979] DEBUG W_H_F_js: jaxsim[216979] DEBUG [[ 1. -0. 0. -0.03845] [ 0. 1. -0. -0.0744 ] [-0. 0. 1. -0.7614 ] [ 0. 0. 0. 1. ]] jaxsim[216979] DEBUG W_H_F_iDynTree: jaxsim[216979] DEBUG [[ 0.96105 -0.25056 -0.11665 0.01682] [ 0.27145 0.93511 0.22777 -0.16649] [ 0.05201 -0.25056 0.9667 0.33741] [ 0. 0. 0. 1. ]] jaxsim[216979] DEBUG In iDynTree the frame "r_upper_arm" is connected to link root_link jaxsim[216979] DEBUG In Jaxsim the frame "r_upper_arm" is connected to link root_link jaxsim[216979] DEBUG In iDynTree the frame "r_wrist_1" is connected to link root_link jaxsim[216979] DEBUG In Jaxsim the frame "r_wrist_1" is connected to link root_link jaxsim[216979] DEBUG In iDynTree the frame "r_wrist_2" is connected to link root_link jaxsim[216979] DEBUG In Jaxsim the frame "r_wrist_2" is connected to link root_link jaxsim[216979] DEBUG In iDynTree the frame "realsense" is connected to link root_link jaxsim[216979] DEBUG In Jaxsim the frame "realsense" is connected to link root_link jaxsim[216979] DEBUG In iDynTree the frame "torso_1" is connected to link root_link jaxsim[216979] DEBUG In Jaxsim the frame "torso_1" is connected to link root_link jaxsim[216979] DEBUG In iDynTree the frame "torso_2" is connected to link root_link jaxsim[216979] DEBUG In Jaxsim the frame "torso_2" is connected to link root_link ```

[traversaro]

They are different just for r_shoulder_3 and l_shoulder_3. For the others cases (passing and failing) the parent link is the same.

The parent link is the same, but it is wrong. It seems that almost all the frames are connected to root_link, and that is wrong. Can you post the URDF that is being parsed by iDynTree?

[xela-95]

@diegoferigo is there a way to print the URDF from the reduced model? https://github.com/ami-iit/jaxsim/blob/d7cfcc6b7c8aab3528a5c81e3a67f157dddade27/tests/conftest.py#L222

[xela-95]

@xela-95 do you confirm that the frames that fail are those populated in that piece of code?

I'm not sure to have understood you're question

@diegoferigo Ok I think to have understood.

Here's the log of the reduction of ergocub model. I don't know if it is a coincidence (probably not) that all the frames that are not passing the test are the ones in which the new parent link resulting from the reuction is not root_link:

jaxsim[216979] DEBUG Model 'ergoCub' is floating-base
jaxsim[216979] DEBUG Considering 'root_link' as base link
jaxsim[216979] INFO Lumping chain: r_foot_rear->(r_foot_rear_ft_sensor)->r_ankle_2
jaxsim[216979] INFO Lumping chain: r_foot_front->(r_foot_front_ft_sensor)->r_ankle_2
jaxsim[216979] INFO Lumping chain: l_foot_rear->(l_foot_rear_ft_sensor)->l_ankle_2
jaxsim[216979] INFO Lumping chain: l_foot_front->(l_foot_front_ft_sensor)->l_ankle_2
jaxsim[216979] INFO Lumping chain: r_shoulder_3->(r_arm_ft_sensor)->r_shoulder_2
jaxsim[216979] INFO Lumping chain: l_shoulder_3->(l_arm_ft_sensor)->l_shoulder_2
jaxsim[216979] INFO Lumping chain: r_hip_3->(r_leg_ft_sensor)->r_hip_2
jaxsim[216979] INFO Lumping chain: l_hip_3->(l_leg_ft_sensor)->l_hip_2
jaxsim[216979] INFO Link 'r_hip_3' became a frame
jaxsim[216979] INFO Link 'l_foot_front' became a frame
jaxsim[216979] INFO Link 'l_shoulder_3' became a frame
jaxsim[216979] INFO Link 'l_hip_3' became a frame
jaxsim[216979] INFO Link 'r_shoulder_3' became a frame
jaxsim[216979] INFO Link 'r_foot_front' became a frame
jaxsim[216979] INFO Link 'r_foot_rear' became a frame
jaxsim[216979] INFO Link 'l_foot_rear' became a frame
jaxsim[216979] INFO New parent of frame 'l_foot_front' is 'l_ankle_2'
jaxsim[216979] INFO New parent of frame 'l_foot_rear' is 'l_ankle_2'
jaxsim[216979] INFO New parent of frame 'l_hip_3' is 'l_hip_2'
jaxsim[216979] INFO New parent of frame 'l_shoulder_3' is 'l_shoulder_2'
jaxsim[216979] INFO New parent of frame 'r_foot_front' is 'r_ankle_2'
jaxsim[216979] INFO New parent of frame 'r_foot_rear' is 'r_ankle_2'
jaxsim[216979] INFO New parent of frame 'r_hip_3' is 'r_hip_2'
jaxsim[216979] INFO New parent of frame 'r_shoulder_3' is 'r_shoulder_2'
jaxsim[216979] DEBUG Moving collidable point: l_foot_front -> l_ankle_2
jaxsim[216979] DEBUG Moving collidable point: l_foot_front -> l_ankle_2
jaxsim[216979] DEBUG Moving collidable point: l_foot_front -> l_ankle_2
jaxsim[216979] DEBUG Moving collidable point: l_foot_front -> l_ankle_2
jaxsim[216979] DEBUG Moving collidable point: l_foot_front -> l_ankle_2
jaxsim[216979] DEBUG Moving collidable point: l_foot_front -> l_ankle_2
jaxsim[216979] DEBUG Moving collidable point: l_foot_front -> l_ankle_2
jaxsim[216979] DEBUG Moving collidable point: l_foot_front -> l_ankle_2
jaxsim[216979] DEBUG Moving collidable point: l_foot_rear -> l_ankle_2
jaxsim[216979] DEBUG Moving collidable point: l_foot_rear -> l_ankle_2
jaxsim[216979] DEBUG Moving collidable point: l_foot_rear -> l_ankle_2
jaxsim[216979] DEBUG Moving collidable point: l_foot_rear -> l_ankle_2
jaxsim[216979] DEBUG Moving collidable point: l_foot_rear -> l_ankle_2
jaxsim[216979] DEBUG Moving collidable point: l_foot_rear -> l_ankle_2
jaxsim[216979] DEBUG Moving collidable point: l_foot_rear -> l_ankle_2
jaxsim[216979] DEBUG Moving collidable point: l_foot_rear -> l_ankle_2
jaxsim[216979] DEBUG Moving collidable point: r_foot_front -> r_ankle_2
jaxsim[216979] DEBUG Moving collidable point: r_foot_front -> r_ankle_2
jaxsim[216979] DEBUG Moving collidable point: r_foot_front -> r_ankle_2
jaxsim[216979] DEBUG Moving collidable point: r_foot_front -> r_ankle_2
jaxsim[216979] DEBUG Moving collidable point: r_foot_front -> r_ankle_2
jaxsim[216979] DEBUG Moving collidable point: r_foot_front -> r_ankle_2
jaxsim[216979] DEBUG Moving collidable point: r_foot_front -> r_ankle_2
jaxsim[216979] DEBUG Moving collidable point: r_foot_front -> r_ankle_2
jaxsim[216979] DEBUG Moving collidable point: r_foot_rear -> r_ankle_2
jaxsim[216979] DEBUG Moving collidable point: r_foot_rear -> r_ankle_2
jaxsim[216979] DEBUG Moving collidable point: r_foot_rear -> r_ankle_2
jaxsim[216979] DEBUG Moving collidable point: r_foot_rear -> r_ankle_2
jaxsim[216979] DEBUG Moving collidable point: r_foot_rear -> r_ankle_2
jaxsim[216979] DEBUG Moving collidable point: r_foot_rear -> r_ankle_2
jaxsim[216979] DEBUG Moving collidable point: r_foot_rear -> r_ankle_2
jaxsim[216979] DEBUG Moving collidable point: r_foot_rear -> r_ankle_2
jaxsim[216979] INFO Lumping chain: r_hand_thumb_3->(r_thumb_dist)->r_hand_thumb_2
jaxsim[216979] INFO Lumping chain: r_hand_index_3->(r_index_dist)->r_hand_index_2
jaxsim[216979] INFO Lumping chain: l_hand_thumb_3->(l_thumb_dist)->l_hand_thumb_2
jaxsim[216979] INFO Lumping chain: l_hand_index_3->(l_index_dist)->l_hand_index_2
jaxsim[216979] INFO Lumping chain: r_hand_thumb_2->(r_thumb_prox)->r_hand_thumb_1
jaxsim[216979] INFO Lumping chain: r_hand_ring_2->(r_ring_dist)->r_hand_ring_1
jaxsim[216979] INFO Lumping chain: r_hand_pinkie_2->(r_pinkie_dist)->r_hand_pinkie_1
jaxsim[216979] INFO Lumping chain: r_hand_middle_2->(r_middle_dist)->r_hand_middle_1
jaxsim[216979] INFO Lumping chain: r_hand_index_2->(r_index_prox)->r_hand_index_1
jaxsim[216979] INFO Lumping chain: l_hand_thumb_2->(l_thumb_prox)->l_hand_thumb_1
jaxsim[216979] INFO Lumping chain: l_hand_ring_2->(l_ring_dist)->l_hand_ring_1
jaxsim[216979] INFO Lumping chain: l_hand_pinkie_2->(l_pinkie_dist)->l_hand_pinkie_1
jaxsim[216979] INFO Lumping chain: l_hand_middle_2->(l_middle_dist)->l_hand_middle_1
jaxsim[216979] INFO Lumping chain: l_hand_index_2->(l_index_prox)->l_hand_index_1
jaxsim[216979] INFO Lumping chain: r_hand_thumb_1->(r_thumb_add)->r_hand_palm
jaxsim[216979] INFO Lumping chain: r_hand_ring_1->(r_ring_prox)->r_hand_palm
jaxsim[216979] INFO Lumping chain: r_hand_pinkie_1->(r_pinkie_prox)->r_hand_palm
jaxsim[216979] INFO Lumping chain: r_hand_middle_1->(r_middle_prox)->r_hand_palm
jaxsim[216979] INFO Lumping chain: r_hand_index_1->(r_index_add)->r_hand_palm
jaxsim[216979] INFO Lumping chain: l_hand_thumb_1->(l_thumb_add)->l_hand_palm
jaxsim[216979] INFO Lumping chain: l_hand_ring_1->(l_ring_prox)->l_hand_palm
jaxsim[216979] INFO Lumping chain: l_hand_pinkie_1->(l_pinkie_prox)->l_hand_palm
jaxsim[216979] INFO Lumping chain: l_hand_middle_1->(l_middle_prox)->l_hand_palm
jaxsim[216979] INFO Lumping chain: l_hand_index_1->(l_index_add)->l_hand_palm
jaxsim[216979] INFO Lumping chain: r_hand_palm->(r_wrist_pitch)->r_wrist_2
jaxsim[216979] INFO Lumping chain: l_hand_palm->(l_wrist_pitch)->l_wrist_2
jaxsim[216979] INFO Lumping chain: r_wrist_2->(r_wrist_roll)->r_wrist_1
jaxsim[216979] INFO Lumping chain: l_wrist_2->(l_wrist_roll)->l_wrist_1
jaxsim[216979] INFO Lumping chain: r_wrist_1->(r_wrist_yaw)->r_forearm
jaxsim[216979] INFO Lumping chain: l_wrist_1->(l_wrist_yaw)->l_forearm
jaxsim[216979] INFO Lumping chain: r_forearm->(r_elbow)->r_upper_arm
jaxsim[216979] INFO Lumping chain: realsense->(camera_tilt)->head
jaxsim[216979] INFO Lumping chain: l_forearm->(l_elbow)->l_upper_arm
jaxsim[216979] INFO Lumping chain: r_upper_arm->(r_shoulder_yaw)->r_shoulder_2
jaxsim[216979] INFO Lumping chain: head->(neck_yaw)->neck_3
jaxsim[216979] INFO Lumping chain: l_upper_arm->(l_shoulder_yaw)->l_shoulder_2
jaxsim[216979] INFO Lumping chain: r_shoulder_2->(r_shoulder_roll)->r_shoulder_1
jaxsim[216979] INFO Lumping chain: neck_3->(neck_roll)->neck_2
jaxsim[216979] INFO Lumping chain: l_shoulder_2->(l_shoulder_roll)->l_shoulder_1
jaxsim[216979] INFO Lumping chain: r_shoulder_1->(r_shoulder_pitch)->chest
jaxsim[216979] INFO Lumping chain: neck_2->(neck_pitch)->chest
jaxsim[216979] INFO Lumping chain: l_shoulder_1->(l_shoulder_pitch)->chest
jaxsim[216979] INFO Lumping chain: chest->(torso_yaw)->torso_2
jaxsim[216979] INFO Lumping chain: torso_2->(torso_pitch)->torso_1
jaxsim[216979] INFO Lumping chain: torso_1->(torso_roll)->root_link
jaxsim[216979] INFO Link 'r_hand_index_1' became a frame
jaxsim[216979] INFO Link 'r_hand_middle_2' became a frame
jaxsim[216979] INFO Link 'chest' became a frame
jaxsim[216979] INFO Link 'l_upper_arm' became a frame
jaxsim[216979] INFO Link 'r_wrist_1' became a frame
jaxsim[216979] INFO Link 'r_hand_thumb_2' became a frame
jaxsim[216979] INFO Link 'l_hand_palm' became a frame
jaxsim[216979] INFO Link 'l_shoulder_1' became a frame
jaxsim[216979] INFO Link 'r_hand_thumb_1' became a frame
jaxsim[216979] INFO Link 'l_hand_thumb_2' became a frame
jaxsim[216979] INFO Link 'l_wrist_2' became a frame
jaxsim[216979] INFO Link 'r_shoulder_2' became a frame
jaxsim[216979] INFO Link 'l_hand_middle_2' became a frame
jaxsim[216979] INFO Link 'l_shoulder_2' became a frame
jaxsim[216979] INFO Link 'r_upper_arm' became a frame
jaxsim[216979] INFO Link 'head' became a frame
jaxsim[216979] INFO Link 'l_hand_index_2' became a frame
jaxsim[216979] INFO Link 'l_hand_index_3' became a frame
jaxsim[216979] INFO Link 'torso_2' became a frame
jaxsim[216979] INFO Link 'neck_2' became a frame
jaxsim[216979] INFO Link 'l_hand_thumb_3' became a frame
jaxsim[216979] INFO Link 'l_hand_pinkie_1' became a frame
jaxsim[216979] INFO Link 'r_hand_palm' became a frame
jaxsim[216979] INFO Link 'l_wrist_1' became a frame
jaxsim[216979] INFO Link 'r_hand_index_3' became a frame
jaxsim[216979] INFO Link 'l_hand_pinkie_2' became a frame
jaxsim[216979] INFO Link 'r_hand_index_2' became a frame
jaxsim[216979] INFO Link 'r_hand_ring_1' became a frame
jaxsim[216979] INFO Link 'l_hand_ring_1' became a frame
jaxsim[216979] INFO Link 'r_shoulder_1' became a frame
jaxsim[216979] INFO Link 'neck_3' became a frame
jaxsim[216979] INFO Link 'r_hand_middle_1' became a frame
jaxsim[216979] INFO Link 'r_wrist_2' became a frame
jaxsim[216979] INFO Link 'l_hand_middle_1' became a frame
jaxsim[216979] INFO Link 'l_hand_index_1' became a frame
jaxsim[216979] INFO Link 'l_forearm' became a frame
jaxsim[216979] INFO Link 'r_hand_ring_2' became a frame
jaxsim[216979] INFO Link 'l_hand_thumb_1' became a frame
jaxsim[216979] INFO Link 'realsense' became a frame
jaxsim[216979] INFO Link 'r_hand_pinkie_2' became a frame
jaxsim[216979] INFO Link 'r_hand_pinkie_1' became a frame
jaxsim[216979] INFO Link 'l_hand_ring_2' became a frame
jaxsim[216979] INFO Link 'r_hand_thumb_3' became a frame
jaxsim[216979] INFO Link 'torso_1' became a frame
jaxsim[216979] INFO Link 'r_forearm' became a frame
jaxsim[216979] INFO New parent of frame 'chest' is 'root_link'
jaxsim[216979] INFO New parent of frame 'head' is 'root_link'
jaxsim[216979] INFO New parent of frame 'l_forearm' is 'root_link'
jaxsim[216979] INFO New parent of frame 'l_hand_index_1' is 'root_link'
jaxsim[216979] INFO New parent of frame 'l_hand_index_2' is 'root_link'
jaxsim[216979] INFO New parent of frame 'l_hand_index_3' is 'root_link'
jaxsim[216979] INFO New parent of frame 'l_hand_middle_1' is 'root_link'
jaxsim[216979] INFO New parent of frame 'l_hand_middle_2' is 'root_link'
jaxsim[216979] INFO New parent of frame 'l_hand_palm' is 'root_link'
jaxsim[216979] INFO New parent of frame 'l_hand_pinkie_1' is 'root_link'
jaxsim[216979] INFO New parent of frame 'l_hand_pinkie_2' is 'root_link'
jaxsim[216979] INFO New parent of frame 'l_hand_ring_1' is 'root_link'
jaxsim[216979] INFO New parent of frame 'l_hand_ring_2' is 'root_link'
jaxsim[216979] INFO New parent of frame 'l_hand_thumb_1' is 'root_link'
jaxsim[216979] INFO New parent of frame 'l_hand_thumb_2' is 'root_link'
jaxsim[216979] INFO New parent of frame 'l_hand_thumb_3' is 'root_link'
jaxsim[216979] INFO New parent of frame 'l_shoulder_1' is 'root_link'
jaxsim[216979] INFO New parent of frame 'l_shoulder_2' is 'root_link'
jaxsim[216979] INFO New parent of frame 'l_upper_arm' is 'root_link'
jaxsim[216979] INFO New parent of frame 'l_wrist_1' is 'root_link'
jaxsim[216979] INFO New parent of frame 'l_wrist_2' is 'root_link'
jaxsim[216979] INFO New parent of frame 'neck_2' is 'root_link'
jaxsim[216979] INFO New parent of frame 'neck_3' is 'root_link'
jaxsim[216979] INFO New parent of frame 'r_forearm' is 'root_link'
jaxsim[216979] INFO New parent of frame 'r_hand_index_1' is 'root_link'
jaxsim[216979] INFO New parent of frame 'r_hand_index_2' is 'root_link'
jaxsim[216979] INFO New parent of frame 'r_hand_index_3' is 'root_link'
jaxsim[216979] INFO New parent of frame 'r_hand_middle_1' is 'root_link'
jaxsim[216979] INFO New parent of frame 'r_hand_middle_2' is 'root_link'
jaxsim[216979] INFO New parent of frame 'r_hand_palm' is 'root_link'
jaxsim[216979] INFO New parent of frame 'r_hand_pinkie_1' is 'root_link'
jaxsim[216979] INFO New parent of frame 'r_hand_pinkie_2' is 'root_link'
jaxsim[216979] INFO New parent of frame 'r_hand_ring_1' is 'root_link'
jaxsim[216979] INFO New parent of frame 'r_hand_ring_2' is 'root_link'
jaxsim[216979] INFO New parent of frame 'r_hand_thumb_1' is 'root_link'
jaxsim[216979] INFO New parent of frame 'r_hand_thumb_2' is 'root_link'
jaxsim[216979] INFO New parent of frame 'r_hand_thumb_3' is 'root_link'
jaxsim[216979] INFO New parent of frame 'r_shoulder_1' is 'root_link'
jaxsim[216979] INFO New parent of frame 'r_shoulder_2' is 'root_link'
jaxsim[216979] INFO New parent of frame 'r_upper_arm' is 'root_link'
jaxsim[216979] INFO New parent of frame 'r_wrist_1' is 'root_link'
jaxsim[216979] INFO New parent of frame 'r_wrist_2' is 'root_link'
jaxsim[216979] INFO New parent of frame 'realsense' is 'root_link'
jaxsim[216979] INFO New parent of frame 'torso_1' is 'root_link'
jaxsim[216979] INFO New parent of frame 'torso_2' is 'root_link'

[traversaro]

For reference, the code that is removing fake links in iDynTree is https://github.com/robotology/idyntree/blob/42e779bbecb98ab934450afee04d100c6274cee7/src/model/src/ModelTransformers.cpp#L56 .

[traversaro]

They are different just for r_shoulder_3 and l_shoulder_3. For the others cases (passing and failing) the parent link is the same.

The parent link is the same, but it is wrong. It seems that almost all the frames are connected to root_link, and that is wrong. Can you post the URDF that is being parsed by iDynTree?

Sorry, I think I misunderstood. Which joints are you considering for this tests? If you are not considering any joint, then it make sense that they are all connected to root_link.

[xela-95]

Sorry, I think I misunderstood. Which joints are you considering for this tests? If you are not considering any joint, then it make sense that they are all connected to root_link.

The joints retained in the reduced model in use are the following:

'l_hip_pitch', 'r_hip_pitch', 'l_hip_roll', 'r_hip_roll', 'l_hip_yaw', 'r_hip_yaw', 'l_knee', 'r_knee', 'l_ankle_pitch', 'r_ankle_pitch', 'l_ankle_roll', 'r_ankle_roll'

[xela-95]

If instead I use the ergocub model not reduced, I obtain the following logs, in which the test fails only for two frames, l_foot_rear and r_foot_rear

test_api_frame.py::test_frame_transforms jaxsim[287607] DEBUG Found model 'ergoCub' in SDF resource
jaxsim[287607] DEBUG Model 'ergoCub' is floating-base
jaxsim[287607] DEBUG Considering 'root_link' as base link
jaxsim[287607] INFO Lumping chain: r_foot_rear->(r_foot_rear_ft_sensor)->r_ankle_2
jaxsim[287607] INFO Lumping chain: r_foot_front->(r_foot_front_ft_sensor)->r_ankle_2
jaxsim[287607] INFO Lumping chain: l_foot_rear->(l_foot_rear_ft_sensor)->l_ankle_2
jaxsim[287607] INFO Lumping chain: l_foot_front->(l_foot_front_ft_sensor)->l_ankle_2
jaxsim[287607] INFO Lumping chain: r_shoulder_3->(r_arm_ft_sensor)->r_shoulder_2
jaxsim[287607] INFO Lumping chain: l_shoulder_3->(l_arm_ft_sensor)->l_shoulder_2
jaxsim[287607] INFO Lumping chain: r_hip_3->(r_leg_ft_sensor)->r_hip_2
jaxsim[287607] INFO Lumping chain: l_hip_3->(l_leg_ft_sensor)->l_hip_2
jaxsim[287607] INFO Link 'l_foot_front' became a frame
jaxsim[287607] INFO Link 'r_foot_front' became a frame
jaxsim[287607] INFO Link 'r_shoulder_3' became a frame
jaxsim[287607] INFO Link 'r_foot_rear' became a frame
jaxsim[287607] INFO Link 'l_shoulder_3' became a frame
jaxsim[287607] INFO Link 'r_hip_3' became a frame
jaxsim[287607] INFO Link 'l_foot_rear' became a frame
jaxsim[287607] INFO Link 'l_hip_3' became a frame
jaxsim[287607] INFO New parent of frame 'l_foot_front' is 'l_ankle_2'
jaxsim[287607] INFO New parent of frame 'l_foot_rear' is 'l_ankle_2'
jaxsim[287607] INFO New parent of frame 'l_hip_3' is 'l_hip_2'
jaxsim[287607] INFO New parent of frame 'l_shoulder_3' is 'l_shoulder_2'
jaxsim[287607] INFO New parent of frame 'r_foot_front' is 'r_ankle_2'
jaxsim[287607] INFO New parent of frame 'r_foot_rear' is 'r_ankle_2'
jaxsim[287607] INFO New parent of frame 'r_hip_3' is 'r_hip_2'
jaxsim[287607] INFO New parent of frame 'r_shoulder_3' is 'r_shoulder_2'
jaxsim[287607] DEBUG Moving collidable point: l_foot_front -> l_ankle_2
jaxsim[287607] DEBUG Moving collidable point: l_foot_front -> l_ankle_2
jaxsim[287607] DEBUG Moving collidable point: l_foot_front -> l_ankle_2
jaxsim[287607] DEBUG Moving collidable point: l_foot_front -> l_ankle_2
jaxsim[287607] DEBUG Moving collidable point: l_foot_front -> l_ankle_2
jaxsim[287607] DEBUG Moving collidable point: l_foot_front -> l_ankle_2
jaxsim[287607] DEBUG Moving collidable point: l_foot_front -> l_ankle_2
jaxsim[287607] DEBUG Moving collidable point: l_foot_front -> l_ankle_2
jaxsim[287607] DEBUG Moving collidable point: l_foot_rear -> l_ankle_2
jaxsim[287607] DEBUG Moving collidable point: l_foot_rear -> l_ankle_2
jaxsim[287607] DEBUG Moving collidable point: l_foot_rear -> l_ankle_2
jaxsim[287607] DEBUG Moving collidable point: l_foot_rear -> l_ankle_2
jaxsim[287607] DEBUG Moving collidable point: l_foot_rear -> l_ankle_2
jaxsim[287607] DEBUG Moving collidable point: l_foot_rear -> l_ankle_2
jaxsim[287607] DEBUG Moving collidable point: l_foot_rear -> l_ankle_2
jaxsim[287607] DEBUG Moving collidable point: l_foot_rear -> l_ankle_2
jaxsim[287607] DEBUG Moving collidable point: r_foot_front -> r_ankle_2
jaxsim[287607] DEBUG Moving collidable point: r_foot_front -> r_ankle_2
jaxsim[287607] DEBUG Moving collidable point: r_foot_front -> r_ankle_2
jaxsim[287607] DEBUG Moving collidable point: r_foot_front -> r_ankle_2
jaxsim[287607] DEBUG Moving collidable point: r_foot_front -> r_ankle_2
jaxsim[287607] DEBUG Moving collidable point: r_foot_front -> r_ankle_2
jaxsim[287607] DEBUG Moving collidable point: r_foot_front -> r_ankle_2
jaxsim[287607] DEBUG Moving collidable point: r_foot_front -> r_ankle_2
jaxsim[287607] DEBUG Moving collidable point: r_foot_rear -> r_ankle_2
jaxsim[287607] DEBUG Moving collidable point: r_foot_rear -> r_ankle_2
jaxsim[287607] DEBUG Moving collidable point: r_foot_rear -> r_ankle_2
jaxsim[287607] DEBUG Moving collidable point: r_foot_rear -> r_ankle_2
jaxsim[287607] DEBUG Moving collidable point: r_foot_rear -> r_ankle_2
jaxsim[287607] DEBUG Moving collidable point: r_foot_rear -> r_ankle_2
jaxsim[287607] DEBUG Moving collidable point: r_foot_rear -> r_ankle_2
jaxsim[287607] DEBUG Moving collidable point: r_foot_rear -> r_ankle_2
jaxsim[287607] DEBUG Joints considered: ('l_hip_pitch', 'r_hip_pitch', 'torso_roll', 'l_hip_roll', 'r_hip_roll', 'torso_pitch', 'l_hip_yaw', 'r_hip_yaw', 'torso_yaw', 'l_knee', 'r_knee', 'l_shoulder_pitch', 'neck_pitch', 'r_shoulder_pitch', 'l_ankle_pitch', 'r_ankle_pitch', 'l_shoulder_roll', 'neck_roll', 'r_shoulder_roll', 'l_ankle_roll', 'r_ankle_roll', 'l_shoulder_yaw', 'neck_yaw', 'r_shoulder_yaw', 'l_elbow', 'camera_tilt', 'r_elbow', 'l_wrist_yaw', 'r_wrist_yaw', 'l_wrist_roll', 'r_wrist_roll', 'l_wrist_pitch', 'r_wrist_pitch', 'l_index_add', 'l_middle_prox', 'l_pinkie_prox', 'l_ring_prox', 'l_thumb_add', 'r_index_add', 'r_middle_prox', 'r_pinkie_prox', 'r_ring_prox', 'r_thumb_add', 'l_index_prox', 'l_middle_dist', 'l_pinkie_dist', 'l_ring_dist', 'l_thumb_prox', 'r_index_prox', 'r_middle_dist', 'r_pinkie_dist', 'r_ring_dist', 'r_thumb_prox', 'l_index_dist', 'l_thumb_dist', 'r_index_dist', 'r_thumb_dist')
jaxsim[287607] DEBUG In iDynTree the frame "l_foot_front" is connected to link l_ankle_2
jaxsim[287607] DEBUG In Jaxsim the frame "l_foot_front" is connected to link l_ankle_2
jaxsim[287607] DEBUG In iDynTree the frame "l_foot_rear" is connected to link l_ankle_2
jaxsim[287607] DEBUG In Jaxsim the frame "l_foot_rear" is connected to link l_ankle_2
jaxsim[287607] ERROR Assertion failed for frame: l_foot_rear
jaxsim[287607] DEBUG W_H_F_js:
jaxsim[287607] DEBUG [[ 0.96105  0.25056 -0.11665  0.06407]
 [-0.27145  0.93511 -0.22777  0.20224]
 [ 0.05201  0.25056  0.9667   0.15075]
 [ 0.       0.       0.       1.     ]]
jaxsim[287607] DEBUG W_H_F_iDynTree:
jaxsim[287607] DEBUG [[ 1.     -0.      0.     -0.0156]
 [ 0.      1.     -0.      0.0744]
 [-0.      0.      1.     -0.755 ]
 [ 0.      0.      0.      1.    ]]
jaxsim[287607] DEBUG In iDynTree the frame "l_hip_3" is connected to link l_hip_2
jaxsim[287607] DEBUG In Jaxsim the frame "l_hip_3" is connected to link l_hip_2
jaxsim[287607] DEBUG In iDynTree the frame "l_shoulder_3" is connected to link l_shoulder_2
jaxsim[287607] DEBUG In Jaxsim the frame "l_shoulder_3" is connected to link l_shoulder_2
jaxsim[287607] DEBUG In iDynTree the frame "r_foot_front" is connected to link r_ankle_2
jaxsim[287607] DEBUG In Jaxsim the frame "r_foot_front" is connected to link r_ankle_2
jaxsim[287607] DEBUG In iDynTree the frame "r_foot_rear" is connected to link r_ankle_2
jaxsim[287607] DEBUG In Jaxsim the frame "r_foot_rear" is connected to link r_ankle_2
jaxsim[287607] ERROR Assertion failed for frame: r_foot_rear
jaxsim[287607] DEBUG W_H_F_js:
jaxsim[287607] DEBUG [[ 1.      -0.       0.00026  0.08336]
 [ 0.       1.      -0.      -0.00003]
 [-0.00026  0.       1.       0.57541]
 [ 0.       0.       0.       1.     ]]
jaxsim[287607] DEBUG W_H_F_iDynTree:
jaxsim[287607] DEBUG [[ 1.     -0.      0.     -0.0156]
 [ 0.      1.     -0.     -0.0744]
 [-0.      0.      1.     -0.755 ]
 [ 0.      0.      0.      1.    ]]
jaxsim[287607] DEBUG In iDynTree the frame "r_hip_3" is connected to link r_hip_2
jaxsim[287607] DEBUG In Jaxsim the frame "r_hip_3" is connected to link r_hip_2
jaxsim[287607] DEBUG In iDynTree the frame "r_shoulder_3" is connected to link r_shoulder_2
jaxsim[287607] DEBUG In Jaxsim the frame "r_shoulder_3" is connected to link r_shoulder_2
FAILED

[diegoferigo]

Since in the test I'm using the jaxsim_models_types fixture as test argument, I think this uses the reduced model of ergocub.

Ow ok. When we talked yesterday I suggested to use in the test only the full model of ergocub. But probably then you went back to all jaxsim_models_types for the new box with frames, I forgot that.

@diegoferigo is there a way to print the URDF from the reduced model?

https://github.com/ami-iit/jaxsim/blob/d7cfcc6b7c8aab3528a5c81e3a67f157dddade27/tests/conftest.py#L222

No this model reduction is done inside JaxSim, there is no way back to URDF like it's done in ROD.

I don't know if it is a coincidence (probably not) that all the frames that are not passing the test are the ones in which the new parent link resulting from the reuction is not root_link:

If instead I use the ergocub model not reduced, I obtain the following logs, in which the test fails only for two frames, l_foot_rear and r_foot_rear

This is a great starting point from now on.

[xela-95]

Ow ok. When we talked yesterday I suggested to use in the test only the full model of ergocub. But probably then you went back to all jaxsim_models_types for the new box with frames, I forgot that.

Ah sorry, I probably forgot that and reverted to the previous version of the test using the reduced model. From now on I will use the complete model to solve this issues.

[xela-95]

Recap

What we figured out is that

• There is something wrong in the chain in the parsing of model descriptions like ergoCub, highlighted by the fact that the parent link of some of its frames is different between iDynTree and Jaxsim (see https://github.com/ami-iit/jaxsim/pull/148#issuecomment-2104408069);
• Up to now with #150 we are able to read into Jaxsim models the frames already available in model descriptions, like the ones in UR10 model, but due to some limitations iDynTree does not consider them, so this is not usable in the unit tests to check that frame quantities are matching between Jaxsim and iDynTree;
• I've added a new fixture of a single pendulum model in https://github.com/ami-iit/jaxsim/pull/148/commits/4a157030200cce802be7eb282c93795e48450dcd: in this way a model containing a joint and a custom frame can be used in the unit test to check that the frame quantities correspond. If the unit test pass also in the CI I think this could conclude the scope of this PR and related issue #147, while new ones can be opened for the problems in the previous points.

CC @diegoferigo @traversaro @flferretti

[xela-95]

A single unit test is failing: tests/test_pytree.py::test_call_jit_compiled_function_passing_different_objects https://github.com/ami-iit/jaxsim/actions/runs/9034447177/job/24827000313?pr=148#step:11:140

I think it's related to https://github.com/ami-iit/jaxsim/issues/103

[diegoferigo]

@flferretti do you mind a final check? I'll squash-merge this time since history has many additions and deletions.

[diegoferigo]

In the meantime, I found another bug by running the frame test on a reduced model.

[diegoferigo]

Trying to fix once again in https://github.com/ami-iit/jaxsim/pull/148/commits/61d71a496724c3f578905d7d6138cf2990ea8a51 a regression introduced in this PR related to #103.

[diegoferigo]

Trying to fix once again in 61d71a4 a regression introduced in this PR related to #103.

Mmh nope it didn't work. We need more time to solve this for good. I propose to proceed with that test disabled so we can start using frames, and fix the problem in another PR. @flferretti

It's worth noting that the test succeeds if I run it locally.

[flferretti]

It's worth noting that the test succeeds if I run it locally.

Are you using GPU or CPU?

[diegoferigo]

It's worth noting that the test succeeds if I run it locally.

Are you using GPU or CPU?

Now CPU, but yesterday I tested with GPU and locally was passing either.

[flferretti]

Can you please try to run this and see if you get the same error?

Test Script

```python import jax.numpy as jnp import jaxsim.api as js import rod.builder.primitives import rod.urdf.exporter from jaxsim import integrators # Create on-the-fly a ROD model of a box. rod_model = ( rod.builder.primitives.BoxBuilder(x=0.3, y=0.2, z=0.1, mass=1.0, name="box") .build_model() .add_link() .add_inertial() .add_visual() .add_collision() .build() ) # Export the URDF string. urdf_string = rod.urdf.exporter.UrdfExporter.sdf_to_urdf_string( sdf=rod_model, pretty=True ) model1 = js.model.JaxSimModel.build_from_model_description( model_description=urdf_string, is_urdf=True, ) model2 = js.model.JaxSimModel.build_from_model_description( model_description=urdf_string, is_urdf=True, ) # Build the data data1 = js.data.JaxSimModelData.build(model=model1) data2 = js.data.JaxSimModelData.build(model=model2) # Create the integrators integrator1 = integrators.fixed_step.Heun2SO3.build( dynamics=js.ode.wrap_system_dynamics_for_integration( model=model1, data=data1, system_dynamics=js.ode.system_dynamics, ), ) integrator2 = integrators.fixed_step.Heun2SO3.build( dynamics=js.ode.wrap_system_dynamics_for_integration( model=model2, data=data2, system_dynamics=js.ode.system_dynamics, ), ) # ! Try to initialize the integrator integrator_state1 = integrator1.init(x0=data1.state, t0=0, dt=1e-3) integrator_state2 = integrator2.init(x0=data2.state, t0=0, dt=1e-3) ```