search

ami-iit / jaxsim

A differentiable physics engine and multibody dynamics library for control and robot learning.
https://jaxsim.readthedocs.io/
BSD 3-Clause "New" or "Revised" License
69 stars 10 forks source link

Read frames from the original model description #150

Closed diegoferigo closed 4 months ago

diegoferigo commented 4 months ago

This PR exploits the recent enhancement in our parser ROD (https://github.com/ami-iit/rod/pull/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 commented 4 months ago

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