Expose quantities related to generic frames #148
Conversation
xela-95
changed the title
|
The implementation needed by issue #147 have been completed. Now the unit test for the jacobian function of @diegoferigo @flferretti could you please review the math in the frame functions and the unit tests implementation? |
|
Thanks a lot @xela-95 for the PR! Could you please explain how did you get the errors using the |
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 J_WL_frames = jax.vmap(
lambda idx: js.frame.jacobian(model=model, data=data, frame_index=idx)
)(jnp.array(frame_indexes))I get: |
There was a problem hiding this comment.
Great, looks good!
For the records, JIT compilation is slow because it has to compile the frame.transform and frame.jacobian functions for each frame. After the first compilation, the compiled functions should work also on different instances of model and data.
Users, generally, don't need to calculate these quantities for all frames, contrarily to links. For the time being, JIT speed is not a major concern.
|
Thanks for providing an example. I'll take a look at it, in the meanwhile for me it is good to go |
|
Perfect, thanks for the clear explanations!! |
|
I just want to point out that from |
Nice catch! I'll try to iterate on that point to find a solution exploiting Jax capabilities |
|
The CI is still failing with error 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
force-pushed
the
feature/expose-frame-quantities
branch
from
fcaaa94
to
52331b0
Compare
Brief recap of yesterdayThis 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 In this case the unit test passed. Then, I wanted to use frames for more complex models, like the @diegoferigo added support to frames super quickly in #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. |
|
Right now on
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 |
|
Here's the log containg the homogeneous transforms from world to frame that are failing: Logs |
|
The math compute by the
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? |
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? |
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. |
Sorry, I read later #148 (comment), this is already happening. |
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 |
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 |
diegoferigo
force-pushed
the
feature/expose-frame-quantities
branch
from
561923a
to
8498c1b
Compare
diegoferigo
force-pushed
the
feature/expose-frame-quantities
branch
from
8498c1b
to
61d71a4
Compare
diegoferigo
force-pushed
the
feature/expose-frame-quantities
branch
from
61d71a4
to
a0ed4a8
Compare
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. |
Are you using GPU or CPU? |
Now CPU, but yesterday I tested with GPU and locally was passing either. |
|
Can you please try to run this and see if you get the same error? Test Script
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) |
|
That is instead failing as reported below. Failure |
|
If it is blocking for this PR, feel free to merge, we can address this in the future |
|
Yeah I guess your script can be a good start to look for a permanent fix (or better, a pattern that would no longer produce this problem). Since your problem was already there, but for some reason the pytree test was not affected, I propose to proceed merging. Let me know if we have applications that are affected (I don't think so). |
diegoferigo
force-pushed
the
feature/expose-frame-quantities
branch
from
c498d6e
to
8b5ac05
Compare
|
I believe I found a possible reason, check #103 (comment) |
* Create `jaxsim.api.frame` module with `transform` function * Add `frame` module to `jaxsim.api` package * Add unit test for `jaxsim.api.frame` module * Add index-related functions to `frame` module * Add `test_frame_index` to `frame` unit tests * Add `jacobian` method to `frame` module * Add `test_frame_jacobians` to `frame` unit tests * Add `.vscode` to gitignore * Add `frame` module to sphynx documentation * Apply suggestions from code review Co-authored-by: Filippo Luca Ferretti <102977828+flferretti@users.noreply.github.com> Co-authored-by: Diego Ferigo <dgferigo@gmail.com> * Update frame.py * Add additional frame attached to link in box model * Refactor `test_frame_jacobians` to better debug jacobians not matching with iDynTree * Exclude from `test_frame_jacobians` the frames that are not loaded in KynDynComputations * Add `frame_parent_link_name` method to `KinDynComputations class` * Update code style of `frame.transform` function * WIP Update `test_frame_transforms` to print parent link frames and not fail at the first failed assertion * Update `test_frame_transforms` * Add single pendulum fixture in `conftest.py` * Clean `test_api_frames` * Fix retrieval of the frame's parent link index * Add function to get the frame's parent link index * Update frames test * Align link and joint tests * Removed unused tested model * Update tests to use new ROD URDF exporter function * Use plain integers for frame indices * Add JaxSimModel.frame_names * Fix regression raising TracerBoolConversionError when comparing pytrees * Temporarily disable test_pytree --------- Co-authored-by: Filippo Luca Ferretti <102977828+flferretti@users.noreply.github.com> Co-authored-by: Diego Ferigo <dgferigo@gmail.com> Co-authored-by: diegoferigo <diego.ferigo@iit.it>
* Create `jaxsim.api.frame` module with `transform` function * Add `frame` module to `jaxsim.api` package * Add unit test for `jaxsim.api.frame` module * Add index-related functions to `frame` module * Add `test_frame_index` to `frame` unit tests * Add `jacobian` method to `frame` module * Add `test_frame_jacobians` to `frame` unit tests * Add `.vscode` to gitignore * Add `frame` module to sphynx documentation * Apply suggestions from code review Co-authored-by: Filippo Luca Ferretti <102977828+flferretti@users.noreply.github.com> Co-authored-by: Diego Ferigo <dgferigo@gmail.com> * Update frame.py * Add additional frame attached to link in box model * Refactor `test_frame_jacobians` to better debug jacobians not matching with iDynTree * Exclude from `test_frame_jacobians` the frames that are not loaded in KynDynComputations * Add `frame_parent_link_name` method to `KinDynComputations class` * Update code style of `frame.transform` function * WIP Update `test_frame_transforms` to print parent link frames and not fail at the first failed assertion * Update `test_frame_transforms` * Add single pendulum fixture in `conftest.py` * Clean `test_api_frames` * Fix retrieval of the frame's parent link index * Add function to get the frame's parent link index * Update frames test * Align link and joint tests * Removed unused tested model * Update tests to use new ROD URDF exporter function * Use plain integers for frame indices * Add JaxSimModel.frame_names * Fix regression raising TracerBoolConversionError when comparing pytrees * Temporarily disable test_pytree --------- Co-authored-by: Filippo Luca Ferretti <102977828+flferretti@users.noreply.github.com> Co-authored-by: Diego Ferigo <dgferigo@gmail.com> Co-authored-by: diegoferigo <diego.ferigo@iit.it>
* Create `jaxsim.api.frame` module with `transform` function * Add `frame` module to `jaxsim.api` package * Add unit test for `jaxsim.api.frame` module * Add index-related functions to `frame` module * Add `test_frame_index` to `frame` unit tests * Add `jacobian` method to `frame` module * Add `test_frame_jacobians` to `frame` unit tests * Add `.vscode` to gitignore * Add `frame` module to sphynx documentation * Apply suggestions from code review Co-authored-by: Filippo Luca Ferretti <102977828+flferretti@users.noreply.github.com> Co-authored-by: Diego Ferigo <dgferigo@gmail.com> * Update frame.py * Add additional frame attached to link in box model * Refactor `test_frame_jacobians` to better debug jacobians not matching with iDynTree * Exclude from `test_frame_jacobians` the frames that are not loaded in KynDynComputations * Add `frame_parent_link_name` method to `KinDynComputations class` * Update code style of `frame.transform` function * WIP Update `test_frame_transforms` to print parent link frames and not fail at the first failed assertion * Update `test_frame_transforms` * Add single pendulum fixture in `conftest.py` * Clean `test_api_frames` * Fix retrieval of the frame's parent link index * Add function to get the frame's parent link index * Update frames test * Align link and joint tests * Removed unused tested model * Update tests to use new ROD URDF exporter function * Use plain integers for frame indices * Add JaxSimModel.frame_names * Fix regression raising TracerBoolConversionError when comparing pytrees * Temporarily disable test_pytree --------- Co-authored-by: Filippo Luca Ferretti <102977828+flferretti@users.noreply.github.com> Co-authored-by: Diego Ferigo <dgferigo@gmail.com> Co-authored-by: diegoferigo <diego.ferigo@iit.it>
Closes #147
📚 Documentation preview 📚: https://jaxsim--148.org.readthedocs.build//148/