From 6ef0df7eed467098b036147901c53d1b3088e61b Mon Sep 17 00:00:00 2001 From: Yuzhe Qin Date: Mon, 15 Jul 2024 16:50:57 +0800 Subject: [PATCH] [update] major update to clean up retargeting config structure and remove unnecessary entries --- dex_retargeting/__init__.py | 2 +- .../configs/offline/ability_hand_left.yml | 2 +- .../configs/offline/ability_hand_right.yml | 4 ++-- .../configs/offline/allegro_hand_left.yml | 2 +- .../configs/offline/allegro_hand_right.yml | 2 +- .../configs/offline/inspire_hand_left.yml | 2 +- .../configs/offline/inspire_hand_right.yml | 2 +- dex_retargeting/configs/offline/leap_hand_left.yml | 2 +- dex_retargeting/configs/offline/leap_hand_right.yml | 2 +- .../configs/offline/schunk_svh_hand_left.yml | 2 +- .../configs/offline/schunk_svh_hand_right.yml | 2 +- dex_retargeting/configs/offline/shadow_hand_left.yml | 2 +- .../configs/offline/shadow_hand_right.yml | 2 +- dex_retargeting/configs/teleop/ability_hand_left.yml | 1 - .../configs/teleop/ability_hand_left_dexpilot.yml | 2 +- .../configs/teleop/ability_hand_right.yml | 1 - .../configs/teleop/ability_hand_right_dexpilot.yml | 2 +- dex_retargeting/configs/teleop/allegro_hand_left.yml | 2 -- .../configs/teleop/allegro_hand_left_dexpilot.yml | 3 +-- .../configs/teleop/allegro_hand_right.yml | 2 -- .../configs/teleop/allegro_hand_right_dexpilot.yml | 3 +-- dex_retargeting/configs/teleop/inspire_hand_left.yml | 1 - .../configs/teleop/inspire_hand_left_dexpilot.yml | 2 +- .../configs/teleop/inspire_hand_right.yml | 1 - .../configs/teleop/inspire_hand_right_dexpilot.yml | 2 +- dex_retargeting/configs/teleop/leap_hand_left.yml | 2 -- .../configs/teleop/leap_hand_left_dexpilot.yml | 3 +-- dex_retargeting/configs/teleop/leap_hand_right.yml | 2 -- .../configs/teleop/leap_hand_right_dexpilot.yml | 3 +-- .../configs/teleop/schunk_svh_hand_left.yml | 1 - .../configs/teleop/schunk_svh_hand_left_dexpilot.yml | 2 +- .../configs/teleop/schunk_svh_hand_right.yml | 1 - .../teleop/schunk_svh_hand_right_dexpilot.yml | 2 +- dex_retargeting/configs/teleop/shadow_hand_left.yml | 2 -- .../configs/teleop/shadow_hand_left_dexpilot.yml | 3 +-- dex_retargeting/configs/teleop/shadow_hand_right.yml | 2 -- .../configs/teleop/shadow_hand_right_dexpilot.yml | 3 +-- dex_retargeting/optimizer.py | 12 ++++-------- dex_retargeting/retargeting_config.py | 8 +++----- example/vector_retargeting/render_robot_hand.py | 2 ++ tests/test_optimizer.py | 3 +++ 41 files changed, 38 insertions(+), 63 deletions(-) diff --git a/dex_retargeting/__init__.py b/dex_retargeting/__init__.py index 334b899..6a9beea 100644 --- a/dex_retargeting/__init__.py +++ b/dex_retargeting/__init__.py @@ -1 +1 @@ -__version__ = "0.3.4" +__version__ = "0.4.0" diff --git a/dex_retargeting/configs/offline/ability_hand_left.yml b/dex_retargeting/configs/offline/ability_hand_left.yml index c246c74..09873d6 100644 --- a/dex_retargeting/configs/offline/ability_hand_left.yml +++ b/dex_retargeting/configs/offline/ability_hand_left.yml @@ -1,12 +1,12 @@ retargeting: type: position urdf_path: ability_hand/ability_hand_left.urdf - wrist_link_name: "base_link" target_joint_names: [ 'thumb_q1', 'thumb_q2', 'index_q1', 'middle_q1', 'pinky_q1', 'ring_q1' ] target_link_names: [ "thumb_tip", "index_tip", "middle_tip", "ring_tip", "pinky_tip" ] target_link_human_indices: [ 4, 8, 12, 16, 20 ] + add_dummy_free_joint: True # A smaller alpha means stronger filtering, i.e. more smooth but also larger latency # 1 means no filter while 0 means not moving diff --git a/dex_retargeting/configs/offline/ability_hand_right.yml b/dex_retargeting/configs/offline/ability_hand_right.yml index 8ac8201..644f2b5 100644 --- a/dex_retargeting/configs/offline/ability_hand_right.yml +++ b/dex_retargeting/configs/offline/ability_hand_right.yml @@ -1,16 +1,16 @@ retargeting: type: position urdf_path: ability_hand/ability_hand_right.urdf - wrist_link_name: "base_link" target_joint_names: [ 'thumb_q1', 'thumb_q2', 'index_q1', 'middle_q1', 'pinky_q1', 'ring_q1' ] target_link_names: [ "thumb_tip", "index_tip", "middle_tip", "ring_tip", "pinky_tip" ] target_link_human_indices: [ 4, 8, 12, 16, 20 ] + add_dummy_free_joint: True # A smaller alpha means stronger filtering, i.e. more smooth but also larger latency # 1 means no filter while 0 means not moving low_pass_alpha: 1 # To ignore the mimic joint tags in the URDF, set it to True - ignore_mimic_joint: False \ No newline at end of file + ignore_mimic_joint: False diff --git a/dex_retargeting/configs/offline/allegro_hand_left.yml b/dex_retargeting/configs/offline/allegro_hand_left.yml index ef407e1..8742bcb 100644 --- a/dex_retargeting/configs/offline/allegro_hand_left.yml +++ b/dex_retargeting/configs/offline/allegro_hand_left.yml @@ -1,13 +1,13 @@ retargeting: type: position urdf_path: allegro_hand/allegro_hand_left.urdf - wrist_link_name: "wrist" target_joint_names: null target_link_names: [ "link_15.0_tip", "link_11.0_tip", "link_7.0_tip", "link_3.0_tip", "link_14.0", "link_10.0", "link_6.0", "link_2.0" ] target_link_human_indices: [ 4, 8, 12, 16, 2, 6, 10, 14 ] + add_dummy_free_joint: True # A smaller alpha means stronger filtering, i.e. more smooth but also larger latency # 1 means no filter while 0 means not moving diff --git a/dex_retargeting/configs/offline/allegro_hand_right.yml b/dex_retargeting/configs/offline/allegro_hand_right.yml index 01c1a55..7620909 100644 --- a/dex_retargeting/configs/offline/allegro_hand_right.yml +++ b/dex_retargeting/configs/offline/allegro_hand_right.yml @@ -1,13 +1,13 @@ retargeting: type: position urdf_path: allegro_hand/allegro_hand_right.urdf - wrist_link_name: "wrist" target_joint_names: null target_link_names: [ "link_15.0_tip", "link_3.0_tip", "link_7.0_tip", "link_11.0_tip", "link_14.0", "link_2.0", "link_6.0", "link_10.0" ] target_link_human_indices: [ 4, 8, 12, 16, 2, 6, 10, 14 ] + add_dummy_free_joint: True # A smaller alpha means stronger filtering, i.e. more smooth but also larger latency # 1 means no filter while 0 means not moving diff --git a/dex_retargeting/configs/offline/inspire_hand_left.yml b/dex_retargeting/configs/offline/inspire_hand_left.yml index db709b4..06473b2 100644 --- a/dex_retargeting/configs/offline/inspire_hand_left.yml +++ b/dex_retargeting/configs/offline/inspire_hand_left.yml @@ -1,13 +1,13 @@ retargeting: type: position urdf_path: inspire_hand/inspire_hand_left.urdf - wrist_link_name: "base" target_joint_names: [ 'pinky_proximal_joint', 'ring_proximal_joint', 'middle_proximal_joint', 'index_proximal_joint', 'thumb_proximal_pitch_joint', 'thumb_proximal_yaw_joint' ] target_link_names: [ "thumb_tip", "index_tip", "middle_tip", "ring_tip", "pinky_tip" ] target_link_human_indices: [ 4, 8, 12, 16, 20 ] + add_dummy_free_joint: True # A smaller alpha means stronger filtering, i.e. more smooth but also larger latency # 1 means no filter while 0 means not moving diff --git a/dex_retargeting/configs/offline/inspire_hand_right.yml b/dex_retargeting/configs/offline/inspire_hand_right.yml index c2fc2fe..3fb2cbf 100644 --- a/dex_retargeting/configs/offline/inspire_hand_right.yml +++ b/dex_retargeting/configs/offline/inspire_hand_right.yml @@ -1,13 +1,13 @@ retargeting: type: position urdf_path: inspire_hand/inspire_hand_right.urdf - wrist_link_name: "base" target_joint_names: [ 'pinky_proximal_joint', 'ring_proximal_joint', 'middle_proximal_joint', 'index_proximal_joint', 'thumb_proximal_pitch_joint', 'thumb_proximal_yaw_joint' ] target_link_names: [ "thumb_tip", "index_tip", "middle_tip", "ring_tip", "pinky_tip" ] target_link_human_indices: [ 4, 8, 12, 16, 20 ] + add_dummy_free_joint: True # A smaller alpha means stronger filtering, i.e. more smooth but also larger latency # 1 means no filter while 0 means not moving diff --git a/dex_retargeting/configs/offline/leap_hand_left.yml b/dex_retargeting/configs/offline/leap_hand_left.yml index 0cc8853..cb585f1 100644 --- a/dex_retargeting/configs/offline/leap_hand_left.yml +++ b/dex_retargeting/configs/offline/leap_hand_left.yml @@ -1,12 +1,12 @@ retargeting: type: position urdf_path: leap_hand/leap_hand_left.urdf - wrist_link_name: "base" target_joint_names: null target_link_names: [ "thumb_tip_head", "index_tip_head", "middle_tip_head", "ring_tip_head", "thumb_dip", "dip", "dip_2", "dip_3" ] target_link_human_indices: [ 4, 8, 12, 16, 2, 6, 10, 14 ] + add_dummy_free_joint: True # A smaller alpha means stronger filtering, i.e. more smooth but also larger latency # 1 means no filter while 0 means not moving diff --git a/dex_retargeting/configs/offline/leap_hand_right.yml b/dex_retargeting/configs/offline/leap_hand_right.yml index d0cded8..a8701f0 100644 --- a/dex_retargeting/configs/offline/leap_hand_right.yml +++ b/dex_retargeting/configs/offline/leap_hand_right.yml @@ -1,12 +1,12 @@ retargeting: type: position urdf_path: leap_hand/leap_hand_right.urdf - wrist_link_name: "base" target_joint_names: null target_link_names: [ "thumb_tip_head", "index_tip_head", "middle_tip_head", "ring_tip_head", "thumb_dip", "dip", "dip_2", "dip_3" ] target_link_human_indices: [ 4, 8, 12, 16, 2, 6, 10, 14 ] + add_dummy_free_joint: True # A smaller alpha means stronger filtering, i.e. more smooth but also larger latency # 1 means no filter while 0 means not moving diff --git a/dex_retargeting/configs/offline/schunk_svh_hand_left.yml b/dex_retargeting/configs/offline/schunk_svh_hand_left.yml index 341d1e7..61a1cc5 100644 --- a/dex_retargeting/configs/offline/schunk_svh_hand_left.yml +++ b/dex_retargeting/configs/offline/schunk_svh_hand_left.yml @@ -1,7 +1,6 @@ retargeting: type: position urdf_path: schunk_hand/schunk_svh_hand_left.urdf - wrist_link_name: "left_hand_base_link" target_joint_names: [ 'left_hand_Thumb_Opposition', 'left_hand_Thumb_Flexion', 'left_hand_Index_Finger_Proximal', 'left_hand_Index_Finger_Distal', 'left_hand_Finger_Spread', 'left_hand_Pinky', @@ -10,6 +9,7 @@ retargeting: "left_hand_q", "left_hand_b", "left_hand_p", "left_hand_o", "left_hand_n", "left_hand_i"] target_link_human_indices: [ 4, 8, 12, 16, 20, 2, 6, 10, 14, 18 ] + add_dummy_free_joint: True # A smaller alpha means stronger filtering, i.e. more smooth but also larger latency # 1 means no filter while 0 means not moving diff --git a/dex_retargeting/configs/offline/schunk_svh_hand_right.yml b/dex_retargeting/configs/offline/schunk_svh_hand_right.yml index ede7306..6c1a3bc 100644 --- a/dex_retargeting/configs/offline/schunk_svh_hand_right.yml +++ b/dex_retargeting/configs/offline/schunk_svh_hand_right.yml @@ -1,7 +1,6 @@ retargeting: type: position urdf_path: schunk_hand/schunk_svh_hand_right.urdf - wrist_link_name: "right_hand_base_link" target_joint_names: [ 'right_hand_Thumb_Opposition', 'right_hand_Thumb_Flexion', 'right_hand_Index_Finger_Proximal', 'right_hand_Index_Finger_Distal', 'right_hand_Finger_Spread', 'right_hand_Pinky', @@ -10,6 +9,7 @@ retargeting: "right_hand_q", "right_hand_b", "right_hand_p", "right_hand_o", "right_hand_n", "right_hand_i"] target_link_human_indices: [ 4, 8, 12, 16, 20, 2, 6, 10, 14, 18 ] + add_dummy_free_joint: True # A smaller alpha means stronger filtering, i.e. more smooth but also larger latency # 1 means no filter while 0 means not moving diff --git a/dex_retargeting/configs/offline/shadow_hand_left.yml b/dex_retargeting/configs/offline/shadow_hand_left.yml index be0b055..7511912 100644 --- a/dex_retargeting/configs/offline/shadow_hand_left.yml +++ b/dex_retargeting/configs/offline/shadow_hand_left.yml @@ -1,13 +1,13 @@ retargeting: type: position urdf_path: shadow_hand/shadow_hand_left.urdf - wrist_link_name: "ee_link" target_joint_names: null target_link_names: [ "thtip", "fftip", "mftip", "rftip", "lftip", "thmiddle", "ffmiddle", "mfmiddle", "rfmiddle", "lfmiddle" ] target_link_human_indices: [ 4, 8, 12, 16, 20, 2, 6, 10, 14, 18 ] + add_dummy_free_joint: True # A smaller alpha means stronger filtering, i.e. more smooth but also larger latency # 1 means no filter while 0 means not moving diff --git a/dex_retargeting/configs/offline/shadow_hand_right.yml b/dex_retargeting/configs/offline/shadow_hand_right.yml index e790b93..7b0ab6c 100644 --- a/dex_retargeting/configs/offline/shadow_hand_right.yml +++ b/dex_retargeting/configs/offline/shadow_hand_right.yml @@ -1,13 +1,13 @@ retargeting: type: position urdf_path: shadow_hand/shadow_hand_right.urdf - wrist_link_name: "ee_link" target_joint_names: null target_link_names: [ "thtip", "fftip", "mftip", "rftip", "lftip", "thmiddle", "ffmiddle", "mfmiddle", "rfmiddle", "lfmiddle" ] target_link_human_indices: [ 4, 8, 12, 16, 20, 2, 6, 10, 14, 18 ] + add_dummy_free_joint: True # A smaller alpha means stronger filtering, i.e. more smooth but also larger latency # 1 means no filter while 0 means not moving diff --git a/dex_retargeting/configs/teleop/ability_hand_left.yml b/dex_retargeting/configs/teleop/ability_hand_left.yml index 134f338..fcd8d6b 100644 --- a/dex_retargeting/configs/teleop/ability_hand_left.yml +++ b/dex_retargeting/configs/teleop/ability_hand_left.yml @@ -1,7 +1,6 @@ retargeting: type: vector urdf_path: ability_hand/ability_hand_left.urdf - wrist_link_name: "base_link" # Target refers to the retargeting target, which is the robot hand target_joint_names: [ 'thumb_q1', 'thumb_q2', 'index_q1', 'middle_q1', 'pinky_q1', 'ring_q1' ] diff --git a/dex_retargeting/configs/teleop/ability_hand_left_dexpilot.yml b/dex_retargeting/configs/teleop/ability_hand_left_dexpilot.yml index bf63943..7bf41aa 100644 --- a/dex_retargeting/configs/teleop/ability_hand_left_dexpilot.yml +++ b/dex_retargeting/configs/teleop/ability_hand_left_dexpilot.yml @@ -1,10 +1,10 @@ retargeting: type: DexPilot urdf_path: ability_hand/ability_hand_left.urdf - wrist_link_name: "base_link" # Target refers to the retargeting target, which is the robot hand target_joint_names: [ 'thumb_q1', 'thumb_q2', 'index_q1', 'middle_q1', 'pinky_q1', 'ring_q1' ] + wrist_link_name: "base_link" finger_tip_link_names: [ "thumb_tip", "index_tip", "middle_tip", "ring_tip", "pinky_tip" ] scaling_factor: 1.0 diff --git a/dex_retargeting/configs/teleop/ability_hand_right.yml b/dex_retargeting/configs/teleop/ability_hand_right.yml index 20be7ac..d8a2324 100644 --- a/dex_retargeting/configs/teleop/ability_hand_right.yml +++ b/dex_retargeting/configs/teleop/ability_hand_right.yml @@ -1,7 +1,6 @@ retargeting: type: vector urdf_path: ability_hand/ability_hand_right.urdf - wrist_link_name: "base_link" # Target refers to the retargeting target, which is the robot hand target_joint_names: [ 'thumb_q1', 'thumb_q2', 'index_q1', 'middle_q1', 'pinky_q1', 'ring_q1' ] diff --git a/dex_retargeting/configs/teleop/ability_hand_right_dexpilot.yml b/dex_retargeting/configs/teleop/ability_hand_right_dexpilot.yml index fb61ec1..6227966 100644 --- a/dex_retargeting/configs/teleop/ability_hand_right_dexpilot.yml +++ b/dex_retargeting/configs/teleop/ability_hand_right_dexpilot.yml @@ -1,10 +1,10 @@ retargeting: type: DexPilot urdf_path: ability_hand/ability_hand_right.urdf - wrist_link_name: "base_link" # Target refers to the retargeting target, which is the robot hand target_joint_names: [ 'thumb_q1', 'thumb_q2', 'index_q1', 'middle_q1', 'pinky_q1', 'ring_q1' ] + wrist_link_name: "base_link" finger_tip_link_names: [ "thumb_tip", "index_tip", "middle_tip", "ring_tip", "pinky_tip" ] scaling_factor: 1.0 diff --git a/dex_retargeting/configs/teleop/allegro_hand_left.yml b/dex_retargeting/configs/teleop/allegro_hand_left.yml index 15691e7..84db599 100644 --- a/dex_retargeting/configs/teleop/allegro_hand_left.yml +++ b/dex_retargeting/configs/teleop/allegro_hand_left.yml @@ -1,10 +1,8 @@ retargeting: type: vector urdf_path: allegro_hand/allegro_hand_left.urdf - wrist_link_name: "wrist" # Target refers to the retargeting target, which is the robot hand - target_joint_names: null target_origin_link_names: [ "wrist", "wrist", "wrist", "wrist" ] target_task_link_names: [ "link_15.0_tip", "link_11.0_tip", "link_7.0_tip", "link_3.0_tip" ] scaling_factor: 1.6 diff --git a/dex_retargeting/configs/teleop/allegro_hand_left_dexpilot.yml b/dex_retargeting/configs/teleop/allegro_hand_left_dexpilot.yml index ea818fe..800b9b3 100644 --- a/dex_retargeting/configs/teleop/allegro_hand_left_dexpilot.yml +++ b/dex_retargeting/configs/teleop/allegro_hand_left_dexpilot.yml @@ -1,10 +1,9 @@ retargeting: type: DexPilot urdf_path: allegro_hand/allegro_hand_left.urdf - wrist_link_name: "wrist" # Target refers to the retargeting target, which is the robot hand - target_joint_names: null + wrist_link_name: "wrist" finger_tip_link_names: [ "link_15.0_tip", "link_11.0_tip", "link_7.0_tip", "link_3.0_tip" ] scaling_factor: 1.6 diff --git a/dex_retargeting/configs/teleop/allegro_hand_right.yml b/dex_retargeting/configs/teleop/allegro_hand_right.yml index 4d2ec73..111c67a 100644 --- a/dex_retargeting/configs/teleop/allegro_hand_right.yml +++ b/dex_retargeting/configs/teleop/allegro_hand_right.yml @@ -1,10 +1,8 @@ retargeting: type: vector urdf_path: allegro_hand/allegro_hand_right.urdf - wrist_link_name: "wrist" # Target refers to the retargeting target, which is the robot hand - target_joint_names: null target_origin_link_names: [ "wrist", "wrist", "wrist", "wrist" ] target_task_link_names: [ "link_15.0_tip", "link_3.0_tip", "link_7.0_tip", "link_11.0_tip" ] scaling_factor: 1.6 diff --git a/dex_retargeting/configs/teleop/allegro_hand_right_dexpilot.yml b/dex_retargeting/configs/teleop/allegro_hand_right_dexpilot.yml index c29688b..75aa6a1 100644 --- a/dex_retargeting/configs/teleop/allegro_hand_right_dexpilot.yml +++ b/dex_retargeting/configs/teleop/allegro_hand_right_dexpilot.yml @@ -1,10 +1,9 @@ retargeting: type: DexPilot urdf_path: allegro_hand/allegro_hand_right.urdf - wrist_link_name: "wrist" # Target refers to the retargeting target, which is the robot hand - target_joint_names: null + wrist_link_name: "wrist" finger_tip_link_names: [ "link_15.0_tip", "link_3.0_tip", "link_7.0_tip", "link_11.0_tip" ] scaling_factor: 1.6 diff --git a/dex_retargeting/configs/teleop/inspire_hand_left.yml b/dex_retargeting/configs/teleop/inspire_hand_left.yml index 8e0ed82..e9c2ccf 100644 --- a/dex_retargeting/configs/teleop/inspire_hand_left.yml +++ b/dex_retargeting/configs/teleop/inspire_hand_left.yml @@ -1,7 +1,6 @@ retargeting: type: vector urdf_path: inspire_hand/inspire_hand_left.urdf - wrist_link_name: "base" # Target refers to the retargeting target, which is the robot hand target_joint_names: [ 'pinky_proximal_joint', 'ring_proximal_joint', 'middle_proximal_joint', 'index_proximal_joint', diff --git a/dex_retargeting/configs/teleop/inspire_hand_left_dexpilot.yml b/dex_retargeting/configs/teleop/inspire_hand_left_dexpilot.yml index 66a42ec..217e3ff 100644 --- a/dex_retargeting/configs/teleop/inspire_hand_left_dexpilot.yml +++ b/dex_retargeting/configs/teleop/inspire_hand_left_dexpilot.yml @@ -1,11 +1,11 @@ retargeting: type: DexPilot urdf_path: inspire_hand/inspire_hand_left.urdf - wrist_link_name: "base" # Target refers to the retargeting target, which is the robot hand target_joint_names: [ 'pinky_proximal_joint', 'ring_proximal_joint', 'middle_proximal_joint', 'index_proximal_joint', 'thumb_proximal_pitch_joint', 'thumb_proximal_yaw_joint' ] + wrist_link_name: "base" finger_tip_link_names: [ "thumb_tip", "index_tip", "middle_tip", "ring_tip", "pinky_tip" ] scaling_factor: 1.15 diff --git a/dex_retargeting/configs/teleop/inspire_hand_right.yml b/dex_retargeting/configs/teleop/inspire_hand_right.yml index 47a80f7..908cf66 100644 --- a/dex_retargeting/configs/teleop/inspire_hand_right.yml +++ b/dex_retargeting/configs/teleop/inspire_hand_right.yml @@ -1,7 +1,6 @@ retargeting: type: vector urdf_path: inspire_hand/inspire_hand_right.urdf - wrist_link_name: "base" # Target refers to the retargeting target, which is the robot hand target_joint_names: [ 'pinky_proximal_joint', 'ring_proximal_joint', 'middle_proximal_joint', 'index_proximal_joint', diff --git a/dex_retargeting/configs/teleop/inspire_hand_right_dexpilot.yml b/dex_retargeting/configs/teleop/inspire_hand_right_dexpilot.yml index 8eacafd..4d9be9e 100644 --- a/dex_retargeting/configs/teleop/inspire_hand_right_dexpilot.yml +++ b/dex_retargeting/configs/teleop/inspire_hand_right_dexpilot.yml @@ -1,11 +1,11 @@ retargeting: type: DexPilot urdf_path: inspire_hand/inspire_hand_right.urdf - wrist_link_name: "base" # Target refers to the retargeting target, which is the robot hand target_joint_names: [ 'pinky_proximal_joint', 'ring_proximal_joint', 'middle_proximal_joint', 'index_proximal_joint', 'thumb_proximal_pitch_joint', 'thumb_proximal_yaw_joint' ] + wrist_link_name: "base" finger_tip_link_names: [ "thumb_tip", "index_tip", "middle_tip", "ring_tip", "pinky_tip" ] scaling_factor: 1.15 diff --git a/dex_retargeting/configs/teleop/leap_hand_left.yml b/dex_retargeting/configs/teleop/leap_hand_left.yml index 342d555..8c937cd 100644 --- a/dex_retargeting/configs/teleop/leap_hand_left.yml +++ b/dex_retargeting/configs/teleop/leap_hand_left.yml @@ -1,10 +1,8 @@ retargeting: type: vector urdf_path: leap_hand/leap_hand_left.urdf - wrist_link_name: "base" # Target refers to the retargeting target, which is the robot hand - target_joint_names: null target_origin_link_names: [ "base", "base", "base", "base" ] target_task_link_names: [ "thumb_tip_head", "index_tip_head", "middle_tip_head", "ring_tip_head" ] scaling_factor: 1.6 diff --git a/dex_retargeting/configs/teleop/leap_hand_left_dexpilot.yml b/dex_retargeting/configs/teleop/leap_hand_left_dexpilot.yml index 509554f..8764637 100644 --- a/dex_retargeting/configs/teleop/leap_hand_left_dexpilot.yml +++ b/dex_retargeting/configs/teleop/leap_hand_left_dexpilot.yml @@ -1,10 +1,9 @@ retargeting: type: DexPilot urdf_path: leap_hand/leap_hand_left.urdf - wrist_link_name: "base" # Target refers to the retargeting target, which is the robot hand - target_joint_names: null + wrist_link_name: "base" finger_tip_link_names: [ "thumb_tip_head", "index_tip_head", "middle_tip_head", "ring_tip_head" ] scaling_factor: 1.6 diff --git a/dex_retargeting/configs/teleop/leap_hand_right.yml b/dex_retargeting/configs/teleop/leap_hand_right.yml index 08838ae..ced0215 100644 --- a/dex_retargeting/configs/teleop/leap_hand_right.yml +++ b/dex_retargeting/configs/teleop/leap_hand_right.yml @@ -1,10 +1,8 @@ retargeting: type: vector urdf_path: leap_hand/leap_hand_right.urdf - wrist_link_name: "base" # Target refers to the retargeting target, which is the robot hand - target_joint_names: null target_origin_link_names: [ "base", "base", "base", "base" ] target_task_link_names: [ "thumb_tip_head", "index_tip_head", "middle_tip_head", "ring_tip_head" ] scaling_factor: 1.6 diff --git a/dex_retargeting/configs/teleop/leap_hand_right_dexpilot.yml b/dex_retargeting/configs/teleop/leap_hand_right_dexpilot.yml index cb70d41..67dbe0b 100644 --- a/dex_retargeting/configs/teleop/leap_hand_right_dexpilot.yml +++ b/dex_retargeting/configs/teleop/leap_hand_right_dexpilot.yml @@ -1,10 +1,9 @@ retargeting: type: DexPilot urdf_path: leap_hand/leap_hand_right.urdf - wrist_link_name: "base" # Target refers to the retargeting target, which is the robot hand - target_joint_names: null + wrist_link_name: "base" finger_tip_link_names: [ "thumb_tip_head", "index_tip_head", "middle_tip_head", "ring_tip_head" ] scaling_factor: 1.6 diff --git a/dex_retargeting/configs/teleop/schunk_svh_hand_left.yml b/dex_retargeting/configs/teleop/schunk_svh_hand_left.yml index 14fe149..5086670 100644 --- a/dex_retargeting/configs/teleop/schunk_svh_hand_left.yml +++ b/dex_retargeting/configs/teleop/schunk_svh_hand_left.yml @@ -1,7 +1,6 @@ retargeting: type: vector urdf_path: schunk_hand/schunk_svh_hand_left.urdf - wrist_link_name: "left_hand_base_link" # Target refers to the retargeting target, which is the robot hand target_joint_names: [ 'left_hand_Thumb_Opposition', 'left_hand_Thumb_Flexion', 'left_hand_Index_Finger_Proximal', diff --git a/dex_retargeting/configs/teleop/schunk_svh_hand_left_dexpilot.yml b/dex_retargeting/configs/teleop/schunk_svh_hand_left_dexpilot.yml index ca70d88..3ea3774 100644 --- a/dex_retargeting/configs/teleop/schunk_svh_hand_left_dexpilot.yml +++ b/dex_retargeting/configs/teleop/schunk_svh_hand_left_dexpilot.yml @@ -1,12 +1,12 @@ retargeting: type: DexPilot urdf_path: schunk_hand/schunk_svh_hand_left.urdf - wrist_link_name: "left_hand_base_link" # Target refers to the retargeting target, which is the robot hand target_joint_names: [ 'left_hand_Thumb_Opposition', 'left_hand_Thumb_Flexion', 'left_hand_Index_Finger_Proximal', 'left_hand_Index_Finger_Distal', 'left_hand_Finger_Spread', 'left_hand_Pinky', 'left_hand_Ring_Finger', 'left_hand_Middle_Finger_Proximal', 'left_hand_Middle_Finger_Distal' ] + wrist_link_name: "left_hand_base_link" finger_tip_link_names: [ "thtip", "fftip", "mftip", "rftip", "lftip" ] scaling_factor: 1.2 diff --git a/dex_retargeting/configs/teleop/schunk_svh_hand_right.yml b/dex_retargeting/configs/teleop/schunk_svh_hand_right.yml index fb2691e..579f81c 100644 --- a/dex_retargeting/configs/teleop/schunk_svh_hand_right.yml +++ b/dex_retargeting/configs/teleop/schunk_svh_hand_right.yml @@ -1,7 +1,6 @@ retargeting: type: vector urdf_path: schunk_hand/schunk_svh_hand_right.urdf - wrist_link_name: "right_hand_base_link" # Target refers to the retargeting target, which is the robot hand target_joint_names: [ 'right_hand_Thumb_Opposition', 'right_hand_Thumb_Flexion', 'right_hand_Index_Finger_Proximal', diff --git a/dex_retargeting/configs/teleop/schunk_svh_hand_right_dexpilot.yml b/dex_retargeting/configs/teleop/schunk_svh_hand_right_dexpilot.yml index 8bb47ca..e0ce6e7 100644 --- a/dex_retargeting/configs/teleop/schunk_svh_hand_right_dexpilot.yml +++ b/dex_retargeting/configs/teleop/schunk_svh_hand_right_dexpilot.yml @@ -1,12 +1,12 @@ retargeting: type: DexPilot urdf_path: schunk_hand/schunk_svh_hand_right.urdf - wrist_link_name: "right_hand_base_link" # Target refers to the retargeting target, which is the robot hand target_joint_names: [ 'right_hand_Thumb_Opposition', 'right_hand_Thumb_Flexion', 'right_hand_Index_Finger_Proximal', 'right_hand_Index_Finger_Distal', 'right_hand_Finger_Spread', 'right_hand_Pinky', 'right_hand_Ring_Finger', 'right_hand_Middle_Finger_Proximal', 'right_hand_Middle_Finger_Distal' ] + wrist_link_name: "right_hand_base_link" finger_tip_link_names: [ "thtip", "fftip", "mftip", "rftip", "lftip" ] scaling_factor: 1.2 diff --git a/dex_retargeting/configs/teleop/shadow_hand_left.yml b/dex_retargeting/configs/teleop/shadow_hand_left.yml index 21de464..9f15fce 100644 --- a/dex_retargeting/configs/teleop/shadow_hand_left.yml +++ b/dex_retargeting/configs/teleop/shadow_hand_left.yml @@ -1,10 +1,8 @@ retargeting: type: vector urdf_path: shadow_hand/shadow_hand_left.urdf - wrist_link_name: "ee_link" # Target refers to the retargeting target, which is the robot hand - target_joint_names: null target_origin_link_names: [ "palm", "palm", "palm", "palm", "palm", "palm", "palm", "palm", "palm", "palm" ] target_task_link_names: [ "thtip", "fftip", "mftip", "rftip", "lftip", "thmiddle", "ffmiddle", "mfmiddle", "rfmiddle", "lfmiddle" ] scaling_factor: 1.2 diff --git a/dex_retargeting/configs/teleop/shadow_hand_left_dexpilot.yml b/dex_retargeting/configs/teleop/shadow_hand_left_dexpilot.yml index 730418e..8476b93 100644 --- a/dex_retargeting/configs/teleop/shadow_hand_left_dexpilot.yml +++ b/dex_retargeting/configs/teleop/shadow_hand_left_dexpilot.yml @@ -1,10 +1,9 @@ retargeting: type: DexPilot urdf_path: shadow_hand/shadow_hand_left.urdf - wrist_link_name: "ee_link" # Target refers to the retargeting target, which is the robot hand - target_joint_names: null + wrist_link_name: "ee_link" finger_tip_link_names: [ "thtip", "fftip", "mftip", "rftip", "lftip" ] scaling_factor: 1.2 diff --git a/dex_retargeting/configs/teleop/shadow_hand_right.yml b/dex_retargeting/configs/teleop/shadow_hand_right.yml index bbdb41f..af49dd4 100644 --- a/dex_retargeting/configs/teleop/shadow_hand_right.yml +++ b/dex_retargeting/configs/teleop/shadow_hand_right.yml @@ -1,10 +1,8 @@ retargeting: type: vector urdf_path: shadow_hand/shadow_hand_right.urdf - wrist_link_name: "ee_link" # Target refers to the retargeting target, which is the robot hand - target_joint_names: null target_origin_link_names: [ "palm", "palm", "palm", "palm", "palm", "palm", "palm", "palm", "palm", "palm" ] target_task_link_names: [ "thtip", "fftip", "mftip", "rftip", "lftip", "thmiddle", "ffmiddle", "mfmiddle", "rfmiddle", "lfmiddle" ] scaling_factor: 1.2 diff --git a/dex_retargeting/configs/teleop/shadow_hand_right_dexpilot.yml b/dex_retargeting/configs/teleop/shadow_hand_right_dexpilot.yml index e881b38..0c5dff6 100644 --- a/dex_retargeting/configs/teleop/shadow_hand_right_dexpilot.yml +++ b/dex_retargeting/configs/teleop/shadow_hand_right_dexpilot.yml @@ -1,10 +1,9 @@ retargeting: type: DexPilot urdf_path: shadow_hand/shadow_hand_right.urdf - wrist_link_name: "ee_link" # Target refers to the retargeting target, which is the robot hand - target_joint_names: null + wrist_link_name: "ee_link" finger_tip_link_names: [ "thtip", "fftip", "mftip", "rftip", "lftip" ] scaling_factor: 1.2 diff --git a/dex_retargeting/optimizer.py b/dex_retargeting/optimizer.py index 50ca06f..eba648b 100644 --- a/dex_retargeting/optimizer.py +++ b/dex_retargeting/optimizer.py @@ -15,13 +15,11 @@ class Optimizer: def __init__( self, robot: RobotWrapper, - wrist_link_name: str, target_joint_names: List[str], target_link_human_indices: np.ndarray, ): self.robot = robot self.num_joints = robot.dof - self.wrist_link_name = wrist_link_name joint_names = robot.dof_joint_names idx_pin2target = [] @@ -101,19 +99,18 @@ def fixed_joint_names(self): class PositionOptimizer(Optimizer): - retargeting_type = "position" + retargeting_type = "POSITION" def __init__( self, robot: RobotWrapper, - wrist_link_name: str, target_joint_names: List[str], target_link_names: List[str], target_link_human_indices: np.ndarray, huber_delta=0.02, norm_delta=4e-3, ): - super().__init__(robot, wrist_link_name, target_joint_names, target_link_human_indices) + super().__init__(robot, target_joint_names, target_link_human_indices) self.body_names = target_link_names self.huber_loss = torch.nn.SmoothL1Loss(beta=huber_delta) self.norm_delta = norm_delta @@ -186,7 +183,6 @@ class VectorOptimizer(Optimizer): def __init__( self, robot: RobotWrapper, - wrist_link_name: str, target_joint_names: List[str], target_origin_link_names: List[str], target_task_link_names: List[str], @@ -195,7 +191,7 @@ def __init__( norm_delta=4e-3, scaling=1.0, ): - super().__init__(robot, wrist_link_name, target_joint_names, target_link_human_indices) + super().__init__(robot, target_joint_names, target_link_human_indices) self.origin_link_names = target_origin_link_names self.task_link_names = target_task_link_names self.huber_loss = torch.nn.SmoothL1Loss(beta=huber_delta, reduction="mean") @@ -336,7 +332,7 @@ def __init__( target_origin_link_names = [link_names[index] for index in origin_link_index] target_task_link_names = [link_names[index] for index in task_link_index] - super().__init__(robot, wrist_link_name, target_joint_names, target_link_human_indices) + super().__init__(robot, target_joint_names, target_link_human_indices) self.origin_link_names = target_origin_link_names self.task_link_names = target_task_link_names self.scaling = scaling diff --git a/dex_retargeting/retargeting_config.py b/dex_retargeting/retargeting_config.py index 8e1b900..a970238 100644 --- a/dex_retargeting/retargeting_config.py +++ b/dex_retargeting/retargeting_config.py @@ -20,9 +20,6 @@ class RetargetingConfig: type: str urdf_path: str - # The link on the robot hand which corresponding to the wrist of human hand - wrist_link_name: str - # Whether to add free joint to the root of the robot. Free joint enable the robot hand move freely in the space add_dummy_free_joint: bool = False @@ -30,6 +27,9 @@ class RetargetingConfig: # The joint indices of human hand joint which corresponds to each link in the target_link_names target_link_human_indices: Optional[np.ndarray] = None + # The link on the robot hand which corresponding to the wrist of human hand + wrist_link_name: Optional[str] = None + # Position retargeting link names target_link_names: Optional[List[str]] = None @@ -168,7 +168,6 @@ def build(self) -> SeqRetargeting: if self.type == "position": optimizer = PositionOptimizer( robot, - self.wrist_link_name, joint_names, target_link_names=self.target_link_names, target_link_human_indices=self.target_link_human_indices, @@ -178,7 +177,6 @@ def build(self) -> SeqRetargeting: elif self.type == "vector": optimizer = VectorOptimizer( robot, - self.wrist_link_name, joint_names, target_origin_link_names=self.target_origin_link_names, target_task_link_names=self.target_task_link_names, diff --git a/example/vector_retargeting/render_robot_hand.py b/example/vector_retargeting/render_robot_hand.py index b885f9e..f1453f1 100644 --- a/example/vector_retargeting/render_robot_hand.py +++ b/example/vector_retargeting/render_robot_hand.py @@ -111,6 +111,8 @@ def render_by_sapien( robot.set_pose(sapien.Pose([0, 0, -0.15])) elif "svh" in robot_name: robot.set_pose(sapien.Pose([0, 0, -0.13])) + elif "inspire" in robot_name: + robot.set_pose(sapien.Pose([0, 0, -0.15])) # Video recorder if record_video: diff --git a/tests/test_optimizer.py b/tests/test_optimizer.py index a3013a1..005c239 100644 --- a/tests/test_optimizer.py +++ b/tests/test_optimizer.py @@ -75,6 +75,7 @@ def test_position_optimizer(self, robot_name, hand_type): config = RetargetingConfig.load_from_file(config_path, override) retargeting = config.build() + assert isinstance(retargeting.optimizer, PositionOptimizer) robot: RobotWrapper = retargeting.optimizer.robot optimizer = retargeting.optimizer @@ -120,6 +121,7 @@ def test_vector_optimizer(self, robot_name, hand_type): config = RetargetingConfig.load_from_file(config_path, override) retargeting = config.build() + assert retargeting.optimizer.retargeting_type == "VECTOR" robot: RobotWrapper = retargeting.optimizer.robot optimizer = retargeting.optimizer @@ -168,6 +170,7 @@ def test_dexpilot_optimizer(self, robot_name, hand_type): config = RetargetingConfig.load_from_file(config_path, override) retargeting = config.build() + assert retargeting.optimizer.retargeting_type == "DEXPILOT" robot: RobotWrapper = retargeting.optimizer.robot optimizer = retargeting.optimizer