jaxsim icon indicating copy to clipboard operation
jaxsim copied to clipboard
verified publisher icon ami-iit

Expose quantities related to generic frames

Open xela-95 opened this issue 1 year ago • 43 comments

Closes #147

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

xela-95 avatar May 07 '24 14:05 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?

xela-95 avatar May 08 '24 15:05 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

flferretti avatar May 08 '24 15:05 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

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

xela-95 avatar May 08 '24 15:05 xela-95

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

flferretti avatar May 08 '24 15:05 flferretti

Perfect, thanks for the clear explanations!!

xela-95 avatar May 08 '24 15:05 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.

flferretti avatar May 08 '24 16:05 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.

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

xela-95 avatar May 08 '24 16:05 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 avatar May 08 '24 17:05 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 avatar May 10 '24 07:05 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 avatar May 10 '24 08:05 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 avatar May 10 '24 08:05 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 avatar May 10 '24 08:05 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?

xela-95 avatar May 10 '24 09:05 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.

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 avatar May 10 '24 09:05 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 avatar May 10 '24 09:05 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.

traversaro avatar May 10 '24 09:05 traversaro

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.

xela-95 avatar May 10 '24 09:05 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.

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 avatar May 10 '24 09:05 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 avatar May 10 '24 09:05 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.

diegoferigo avatar May 10 '24 09:05 diegoferigo

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.     ]]

xela-95 avatar May 10 '24 09:05 xela-95

@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.

diegoferigo avatar May 10 '24 09:05 diegoferigo

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 avatar May 10 '24 09:05 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!

xela-95 avatar May 10 '24 09:05 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.

diegoferigo avatar May 10 '24 09:05 diegoferigo

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 avatar May 10 '24 09:05 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"))).

traversaro avatar May 10 '24 09:05 traversaro

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 avatar May 10 '24 10:05 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.

xela-95 avatar May 10 '24 10:05 xela-95

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?

diegoferigo avatar May 10 '24 10:05 diegoferigo