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.

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

Read frames from the original model description #150

Merged
merged 3 commits into from
May 20, 2024
Merged

Conversation

diegoferigo
Copy link
Member

@diegoferigo diegoferigo commented May 9, 2024

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/

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
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

3 participants