Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Read frames from the original model description #150

Merged
merged 3 commits into from
May 20, 2024
Merged

Read frames from the original model description #150

merged 3 commits into from
May 20, 2024

Conversation

diegoferigo
Copy link
Member

@diegoferigo diegoferigo commented May 9, 2024 •
edited

This PR exploits the recent enhancement in our parser ROD (ami-iit/rod#34) to populate the intermediate description (obtained from SDF/URDF models and used to build the JaxSimModel) with frame-related data.

cc @xela-95

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

@diegoferigo diegoferigo self-assigned this May 9, 2024
@xela-95 xela-95 mentioned this pull request May 10, 2024
Merged
@diegoferigo diegoferigo marked this pull request as ready for review May 20, 2024 09:12
@diegoferigo diegoferigo requested a review from xela-95 May 20, 2024 09:16
flferretti
flferretti approved these changes May 20, 2024
View reviewed changes
Copy link
Collaborator

@flferretti flferretti left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

LGTM, thanks

@diegoferigo
Copy link
Member Author

@xela-95 I tried also to rebase your #148 over this PR and run the test with ErgoCub, it succeeded for all frames included r_sole and l_sole.

pytest on #148 
jaxsim on  feature/expose-frame-quantities via 🐍 v3.12.2 🅒 /jaxsim  took 12s 
✦ ❯ pytest -vv -s tests/test_api_frame.py -k "test_frame_transforms"
rod[24593] INFO Calling sdformat through '/jaxsim/bin/gz sdf'
jaxsim[24593] INFO Enabling JAX to use 64bit precision
jaxsim[24593] DEBUG Using JAX compilation cache located at '/tmp/jaxsim_compilation_cache'
=================================================================================== test session starts ====================================================================================
platform linux -- Python 3.12.2, pytest-8.2.0, pluggy-1.5.0 -- /jaxsim/bin/python3.12
cachedir: .pytest_cache
rootdir: /home/dferigo/git/jaxsim
configfile: pyproject.toml
plugins: typeguard-2.13.3, icdiff-0.9, jaxtyping-0.2.28, anyio-4.3.0
collected 7 items / 6 deselected / 1 selected                                                                                                                                              

tests/test_api_frame.py::test_frame_transforms jaxsim[24593] DEBUG Found model 'ergoCub' in SDF resource
rod[24593] DEBUG Building kinematic tree of model 'ergoCub'
rod[24593] DEBUG Selecting 'root_link' as canonical link
rod[24593] DEBUG Node 'world' became a frame attached to 'root_link'
jaxsim[24593] DEBUG Model 'ergoCub' is floating-base
jaxsim[24593] DEBUG Considering 'root_link' as base link
jaxsim[24593] INFO Lumping chain: r_foot_rear->(r_foot_rear_ft_sensor)->r_ankle_2
jaxsim[24593] INFO Lumping chain: r_foot_front->(r_foot_front_ft_sensor)->r_ankle_2
jaxsim[24593] INFO Lumping chain: l_foot_rear->(l_foot_rear_ft_sensor)->l_ankle_2
jaxsim[24593] INFO Lumping chain: l_foot_front->(l_foot_front_ft_sensor)->l_ankle_2
jaxsim[24593] INFO Lumping chain: r_shoulder_3->(r_arm_ft_sensor)->r_shoulder_2
jaxsim[24593] INFO Lumping chain: l_shoulder_3->(l_arm_ft_sensor)->l_shoulder_2
jaxsim[24593] INFO Lumping chain: r_hip_3->(r_leg_ft_sensor)->r_hip_2
jaxsim[24593] INFO Lumping chain: l_hip_3->(l_leg_ft_sensor)->l_hip_2
jaxsim[24593] INFO Link 'r_foot_rear' became a frame
jaxsim[24593] INFO Link 'l_foot_rear' became a frame
jaxsim[24593] INFO Link 'l_foot_front' became a frame
jaxsim[24593] INFO Link 'r_hip_3' became a frame
jaxsim[24593] INFO Link 'l_hip_3' became a frame
jaxsim[24593] INFO Link 'r_shoulder_3' became a frame
jaxsim[24593] INFO Link 'l_shoulder_3' became a frame
jaxsim[24593] INFO Link 'r_foot_front' became a frame
jaxsim[24593] INFO New parent of frame 'l_foot_front' is 'l_ankle_2'
jaxsim[24593] INFO New parent of frame 'l_foot_rear' is 'l_ankle_2'
jaxsim[24593] INFO New parent of frame 'l_hip_3' is 'l_hip_2'
jaxsim[24593] INFO New parent of frame 'l_shoulder_3' is 'l_shoulder_2'
jaxsim[24593] INFO New parent of frame 'r_foot_front' is 'r_ankle_2'
jaxsim[24593] INFO New parent of frame 'r_foot_rear' is 'r_ankle_2'
jaxsim[24593] INFO New parent of frame 'r_hip_3' is 'r_hip_2'
jaxsim[24593] INFO New parent of frame 'r_shoulder_3' is 'r_shoulder_2'
jaxsim[24593] DEBUG Moving collidable point: l_foot_front -> l_ankle_2
jaxsim[24593] DEBUG Moving collidable point: l_foot_front -> l_ankle_2
jaxsim[24593] DEBUG Moving collidable point: l_foot_front -> l_ankle_2
jaxsim[24593] DEBUG Moving collidable point: l_foot_front -> l_ankle_2
jaxsim[24593] DEBUG Moving collidable point: l_foot_front -> l_ankle_2
jaxsim[24593] DEBUG Moving collidable point: l_foot_front -> l_ankle_2
jaxsim[24593] DEBUG Moving collidable point: l_foot_front -> l_ankle_2
jaxsim[24593] DEBUG Moving collidable point: l_foot_front -> l_ankle_2
jaxsim[24593] DEBUG Moving collidable point: l_foot_rear -> l_ankle_2
jaxsim[24593] DEBUG Moving collidable point: l_foot_rear -> l_ankle_2
jaxsim[24593] DEBUG Moving collidable point: l_foot_rear -> l_ankle_2
jaxsim[24593] DEBUG Moving collidable point: l_foot_rear -> l_ankle_2
jaxsim[24593] DEBUG Moving collidable point: l_foot_rear -> l_ankle_2
jaxsim[24593] DEBUG Moving collidable point: l_foot_rear -> l_ankle_2
jaxsim[24593] DEBUG Moving collidable point: l_foot_rear -> l_ankle_2
jaxsim[24593] DEBUG Moving collidable point: l_foot_rear -> l_ankle_2
jaxsim[24593] DEBUG Moving collidable point: r_foot_front -> r_ankle_2
jaxsim[24593] DEBUG Moving collidable point: r_foot_front -> r_ankle_2
jaxsim[24593] DEBUG Moving collidable point: r_foot_front -> r_ankle_2
jaxsim[24593] DEBUG Moving collidable point: r_foot_front -> r_ankle_2
jaxsim[24593] DEBUG Moving collidable point: r_foot_front -> r_ankle_2
jaxsim[24593] DEBUG Moving collidable point: r_foot_front -> r_ankle_2
jaxsim[24593] DEBUG Moving collidable point: r_foot_front -> r_ankle_2
jaxsim[24593] DEBUG Moving collidable point: r_foot_front -> r_ankle_2
jaxsim[24593] DEBUG Moving collidable point: r_foot_rear -> r_ankle_2
jaxsim[24593] DEBUG Moving collidable point: r_foot_rear -> r_ankle_2
jaxsim[24593] DEBUG Moving collidable point: r_foot_rear -> r_ankle_2
jaxsim[24593] DEBUG Moving collidable point: r_foot_rear -> r_ankle_2
jaxsim[24593] DEBUG Moving collidable point: r_foot_rear -> r_ankle_2
jaxsim[24593] DEBUG Moving collidable point: r_foot_rear -> r_ankle_2
jaxsim[24593] DEBUG Moving collidable point: r_foot_rear -> r_ankle_2
jaxsim[24593] DEBUG Moving collidable point: r_foot_rear -> r_ankle_2
jaxsim[24593] DEBUG Frame 'l_sole_fixed_joint' is now attached to 'l_ankle_2'
jaxsim[24593] DEBUG Frame 'l_sole' is now attached to 'l_ankle_2'
jaxsim[24593] DEBUG Frame 'r_sole_fixed_joint' is now attached to 'r_ankle_2'
jaxsim[24593] DEBUG Frame 'r_sole' is now attached to 'r_ankle_2'
jaxsim[24593] DEBUG In iDynTree the frame "base_link" is connected to link root_link
jaxsim[24593] DEBUG In Jaxsim the frame "base_link" is connected to link root_link
jaxsim[24593] DEBUG In iDynTree the frame "head_imu_0" is connected to link head
jaxsim[24593] DEBUG In Jaxsim the frame "head_imu_0" is connected to link head
jaxsim[24593] DEBUG In iDynTree the frame "head_laser_frame" is connected to link head
jaxsim[24593] DEBUG In Jaxsim the frame "head_laser_frame" is connected to link head
jaxsim[24593] DEBUG In iDynTree the frame "imu_frame" is connected to link head
jaxsim[24593] DEBUG In Jaxsim the frame "imu_frame" is connected to link head
jaxsim[24593] DEBUG In iDynTree the frame "l_arm_ft" is connected to link l_shoulder_2
jaxsim[24593] DEBUG In Jaxsim the frame "l_arm_ft" is connected to link l_shoulder_2
jaxsim[24593] DEBUG In iDynTree the frame "l_foot_front" is connected to link l_ankle_2
jaxsim[24593] DEBUG In Jaxsim the frame "l_foot_front" is connected to link l_ankle_2
jaxsim[24593] DEBUG In iDynTree the frame "l_foot_front_ft" is connected to link l_ankle_2
jaxsim[24593] DEBUG In Jaxsim the frame "l_foot_front_ft" is connected to link l_ankle_2
jaxsim[24593] DEBUG In iDynTree the frame "l_foot_rear" is connected to link l_ankle_2
jaxsim[24593] DEBUG In Jaxsim the frame "l_foot_rear" is connected to link l_ankle_2
jaxsim[24593] DEBUG In iDynTree the frame "l_foot_rear_ft" is connected to link l_ankle_2
jaxsim[24593] DEBUG In Jaxsim the frame "l_foot_rear_ft" is connected to link l_ankle_2
jaxsim[24593] DEBUG In iDynTree the frame "l_hip_3" is connected to link l_hip_2
jaxsim[24593] DEBUG In Jaxsim the frame "l_hip_3" is connected to link l_hip_2
jaxsim[24593] DEBUG In iDynTree the frame "l_leg_ft" is connected to link l_hip_2
jaxsim[24593] DEBUG In Jaxsim the frame "l_leg_ft" is connected to link l_hip_2
jaxsim[24593] DEBUG In iDynTree the frame "l_shoulder_3" is connected to link l_shoulder_2
jaxsim[24593] DEBUG In Jaxsim the frame "l_shoulder_3" is connected to link l_shoulder_2
jaxsim[24593] DEBUG In iDynTree the frame "l_sole" is connected to link l_ankle_2
jaxsim[24593] DEBUG In Jaxsim the frame "l_sole" is connected to link l_ankle_2
jaxsim[24593] DEBUG In iDynTree the frame "r_arm_ft" is connected to link r_shoulder_2
jaxsim[24593] DEBUG In Jaxsim the frame "r_arm_ft" is connected to link r_shoulder_2
jaxsim[24593] DEBUG In iDynTree the frame "r_foot_front" is connected to link r_ankle_2
jaxsim[24593] DEBUG In Jaxsim the frame "r_foot_front" is connected to link r_ankle_2
jaxsim[24593] DEBUG In iDynTree the frame "r_foot_front_ft" is connected to link r_ankle_2
jaxsim[24593] DEBUG In Jaxsim the frame "r_foot_front_ft" is connected to link r_ankle_2
jaxsim[24593] DEBUG In iDynTree the frame "r_foot_rear" is connected to link r_ankle_2
jaxsim[24593] DEBUG In Jaxsim the frame "r_foot_rear" is connected to link r_ankle_2
jaxsim[24593] DEBUG In iDynTree the frame "r_foot_rear_ft" is connected to link r_ankle_2
jaxsim[24593] DEBUG In Jaxsim the frame "r_foot_rear_ft" is connected to link r_ankle_2
jaxsim[24593] DEBUG In iDynTree the frame "r_hip_3" is connected to link r_hip_2
jaxsim[24593] DEBUG In Jaxsim the frame "r_hip_3" is connected to link r_hip_2
jaxsim[24593] DEBUG In iDynTree the frame "r_leg_ft" is connected to link r_hip_2
jaxsim[24593] DEBUG In Jaxsim the frame "r_leg_ft" is connected to link r_hip_2
jaxsim[24593] DEBUG In iDynTree the frame "r_shoulder_3" is connected to link r_shoulder_2
jaxsim[24593] DEBUG In Jaxsim the frame "r_shoulder_3" is connected to link r_shoulder_2
jaxsim[24593] DEBUG In iDynTree the frame "r_sole" is connected to link r_ankle_2
jaxsim[24593] DEBUG In Jaxsim the frame "r_sole" is connected to link r_ankle_2
jaxsim[24593] DEBUG In iDynTree the frame "realsense_depth_frame" is connected to link realsense
jaxsim[24593] DEBUG In Jaxsim the frame "realsense_depth_frame" is connected to link realsense
jaxsim[24593] DEBUG In iDynTree the frame "realsense_imu_0" is connected to link realsense
jaxsim[24593] DEBUG In Jaxsim the frame "realsense_imu_0" is connected to link realsense
jaxsim[24593] DEBUG In iDynTree the frame "realsense_rgb_frame" is connected to link realsense
jaxsim[24593] DEBUG In Jaxsim the frame "realsense_rgb_frame" is connected to link realsense
jaxsim[24593] DEBUG In iDynTree the frame "waist_imu_0" is connected to link torso_1
jaxsim[24593] DEBUG In Jaxsim the frame "waist_imu_0" is connected to link torso_1
PASSED

@diegoferigo diegoferigo merged commit d6748eb into main May 20, 2024
29 checks passed
@diegoferigo diegoferigo deleted the read_sdf_frames branch May 20, 2024 09:30
@diegoferigo diegoferigo mentioned this pull request May 21, 2024
Merged
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Reviewers

@traversaro traversaro traversaro left review comments

@flferretti flferretti flferretti approved these changes

@xela-95 xela-95 Awaiting requested review from xela-95

Assignees

@diegoferigo diegoferigo

Labels
None yet
Projects
None yet
Milestone
No milestone
Development

Successfully merging this pull request may close these issues.

None yet
3 participants
@diegoferigo @traversaro @flferretti