From 254f52a942ad776b328ec2239fe25c9819062f33 Mon Sep 17 00:00:00 2001 From: Yuzhe Qin Date: Mon, 15 Jul 2024 16:30:36 +0800 Subject: [PATCH] [update] update all robot configs to support all modes for both hands --- assets | 2 +- .../configs/offline/ability_hand_left.yml | 16 ++++++++++++++++ .../configs/offline/allegro_hand_left.yml | 14 ++++++++++++++ .../configs/offline/inspire_hand_left.yml | 17 +++++++++++++++++ .../configs/offline/inspire_hand_right.yml | 17 +++++++++++++++++ .../configs/offline/leap_hand_left.yml | 13 +++++++++++++ .../configs/offline/schunk_svh_hand_left.yml | 16 ++++++++++++++++ .../configs/offline/shadow_hand_left.yml | 14 ++++++++++++++ .../configs/teleop/inspire_hand_left.yml | 18 ++++++++++++++++++ .../teleop/inspire_hand_left_dexpilot.yml | 13 +++++++++++++ .../configs/teleop/inspire_hand_right.yml | 18 ++++++++++++++++++ .../teleop/inspire_hand_right_dexpilot.yml | 13 +++++++++++++ .../configs/teleop/leap_hand_left.yml | 17 +++++++++++++++++ .../configs/teleop/leap_hand_left_dexpilot.yml | 12 ++++++++++++ .../configs/teleop/shadow_hand_left.yml | 17 +++++++++++++++++ .../teleop/shadow_hand_left_dexpilot.yml | 12 ++++++++++++ dex_retargeting/constants.py | 2 ++ tests/test_optimizer.py | 6 +++--- 18 files changed, 233 insertions(+), 4 deletions(-) create mode 100644 dex_retargeting/configs/offline/ability_hand_left.yml create mode 100644 dex_retargeting/configs/offline/allegro_hand_left.yml create mode 100644 dex_retargeting/configs/offline/inspire_hand_left.yml create mode 100644 dex_retargeting/configs/offline/inspire_hand_right.yml create mode 100644 dex_retargeting/configs/offline/leap_hand_left.yml create mode 100644 dex_retargeting/configs/offline/schunk_svh_hand_left.yml create mode 100644 dex_retargeting/configs/offline/shadow_hand_left.yml create mode 100644 dex_retargeting/configs/teleop/inspire_hand_left.yml create mode 100644 dex_retargeting/configs/teleop/inspire_hand_left_dexpilot.yml create mode 100644 dex_retargeting/configs/teleop/inspire_hand_right.yml create mode 100644 dex_retargeting/configs/teleop/inspire_hand_right_dexpilot.yml create mode 100644 dex_retargeting/configs/teleop/leap_hand_left.yml create mode 100644 dex_retargeting/configs/teleop/leap_hand_left_dexpilot.yml create mode 100644 dex_retargeting/configs/teleop/shadow_hand_left.yml create mode 100644 dex_retargeting/configs/teleop/shadow_hand_left_dexpilot.yml diff --git a/assets b/assets index 80fad41..f6fb7db 160000 --- a/assets +++ b/assets @@ -1 +1 @@ -Subproject commit 80fad41cad75fb3bfed5504a14efc29fa6f83688 +Subproject commit f6fb7db8498a9ea9dada663c1d35bc957dfeede8 diff --git a/dex_retargeting/configs/offline/ability_hand_left.yml b/dex_retargeting/configs/offline/ability_hand_left.yml new file mode 100644 index 0000000..c246c74 --- /dev/null +++ b/dex_retargeting/configs/offline/ability_hand_left.yml @@ -0,0 +1,16 @@ +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 ] + + # 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 diff --git a/dex_retargeting/configs/offline/allegro_hand_left.yml b/dex_retargeting/configs/offline/allegro_hand_left.yml new file mode 100644 index 0000000..ef407e1 --- /dev/null +++ b/dex_retargeting/configs/offline/allegro_hand_left.yml @@ -0,0 +1,14 @@ +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 ] + + # 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 diff --git a/dex_retargeting/configs/offline/inspire_hand_left.yml b/dex_retargeting/configs/offline/inspire_hand_left.yml new file mode 100644 index 0000000..db709b4 --- /dev/null +++ b/dex_retargeting/configs/offline/inspire_hand_left.yml @@ -0,0 +1,17 @@ +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 ] + + # 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 diff --git a/dex_retargeting/configs/offline/inspire_hand_right.yml b/dex_retargeting/configs/offline/inspire_hand_right.yml new file mode 100644 index 0000000..c2fc2fe --- /dev/null +++ b/dex_retargeting/configs/offline/inspire_hand_right.yml @@ -0,0 +1,17 @@ +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 ] + + # 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 diff --git a/dex_retargeting/configs/offline/leap_hand_left.yml b/dex_retargeting/configs/offline/leap_hand_left.yml new file mode 100644 index 0000000..0cc8853 --- /dev/null +++ b/dex_retargeting/configs/offline/leap_hand_left.yml @@ -0,0 +1,13 @@ +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 ] + + # 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 diff --git a/dex_retargeting/configs/offline/schunk_svh_hand_left.yml b/dex_retargeting/configs/offline/schunk_svh_hand_left.yml new file mode 100644 index 0000000..341d1e7 --- /dev/null +++ b/dex_retargeting/configs/offline/schunk_svh_hand_left.yml @@ -0,0 +1,16 @@ +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', + 'left_hand_Ring_Finger', 'left_hand_Middle_Finger_Proximal', 'left_hand_Middle_Finger_Distal' ] + target_link_names: [ "left_hand_c", "left_hand_t", "left_hand_s", "left_hand_r", + "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 ] + + # 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 diff --git a/dex_retargeting/configs/offline/shadow_hand_left.yml b/dex_retargeting/configs/offline/shadow_hand_left.yml new file mode 100644 index 0000000..be0b055 --- /dev/null +++ b/dex_retargeting/configs/offline/shadow_hand_left.yml @@ -0,0 +1,14 @@ +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 ] + + # 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 diff --git a/dex_retargeting/configs/teleop/inspire_hand_left.yml b/dex_retargeting/configs/teleop/inspire_hand_left.yml new file mode 100644 index 0000000..8e0ed82 --- /dev/null +++ b/dex_retargeting/configs/teleop/inspire_hand_left.yml @@ -0,0 +1,18 @@ +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', + 'thumb_proximal_pitch_joint', 'thumb_proximal_yaw_joint' ] + target_origin_link_names: [ "base", "base", "base", "base", "base" ] + target_task_link_names: [ "thumb_tip", "index_tip", "middle_tip", "ring_tip", "pinky_tip" ] + scaling_factor: 1.15 + + # Source refers to the retargeting input, which usually corresponds to the human hand + # The joint indices of human hand joint which corresponds to each link in the target_link_names + target_link_human_indices: [ [ 0, 0, 0, 0, 0 ], [ 4, 8, 12, 16, 20 ] ] + + # A smaller alpha means stronger filtering, i.e. more smooth but also larger latency + low_pass_alpha: 0.2 diff --git a/dex_retargeting/configs/teleop/inspire_hand_left_dexpilot.yml b/dex_retargeting/configs/teleop/inspire_hand_left_dexpilot.yml new file mode 100644 index 0000000..66a42ec --- /dev/null +++ b/dex_retargeting/configs/teleop/inspire_hand_left_dexpilot.yml @@ -0,0 +1,13 @@ +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' ] + finger_tip_link_names: [ "thumb_tip", "index_tip", "middle_tip", "ring_tip", "pinky_tip" ] + scaling_factor: 1.15 + + # A smaller alpha means stronger filtering, i.e. more smooth but also larger latency + low_pass_alpha: 0.2 diff --git a/dex_retargeting/configs/teleop/inspire_hand_right.yml b/dex_retargeting/configs/teleop/inspire_hand_right.yml new file mode 100644 index 0000000..47a80f7 --- /dev/null +++ b/dex_retargeting/configs/teleop/inspire_hand_right.yml @@ -0,0 +1,18 @@ +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', + 'thumb_proximal_pitch_joint', 'thumb_proximal_yaw_joint' ] + target_origin_link_names: [ "base", "base", "base", "base", "base" ] + target_task_link_names: [ "thumb_tip", "index_tip", "middle_tip", "ring_tip", "pinky_tip" ] + scaling_factor: 1.15 + + # Source refers to the retargeting input, which usually corresponds to the human hand + # The joint indices of human hand joint which corresponds to each link in the target_link_names + target_link_human_indices: [ [ 0, 0, 0, 0, 0 ], [ 4, 8, 12, 16, 20 ] ] + + # A smaller alpha means stronger filtering, i.e. more smooth but also larger latency + low_pass_alpha: 0.2 diff --git a/dex_retargeting/configs/teleop/inspire_hand_right_dexpilot.yml b/dex_retargeting/configs/teleop/inspire_hand_right_dexpilot.yml new file mode 100644 index 0000000..8eacafd --- /dev/null +++ b/dex_retargeting/configs/teleop/inspire_hand_right_dexpilot.yml @@ -0,0 +1,13 @@ +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' ] + finger_tip_link_names: [ "thumb_tip", "index_tip", "middle_tip", "ring_tip", "pinky_tip" ] + scaling_factor: 1.15 + + # A smaller alpha means stronger filtering, i.e. more smooth but also larger latency + low_pass_alpha: 0.2 diff --git a/dex_retargeting/configs/teleop/leap_hand_left.yml b/dex_retargeting/configs/teleop/leap_hand_left.yml new file mode 100644 index 0000000..342d555 --- /dev/null +++ b/dex_retargeting/configs/teleop/leap_hand_left.yml @@ -0,0 +1,17 @@ +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 + + # Source refers to the retargeting input, which usually corresponds to the human hand + # The joint indices of human hand joint which corresponds to each link in the target_link_names + target_link_human_indices: [ [ 0, 0, 0, 0 ], [ 4, 8, 12, 16 ] ] + + # A smaller alpha means stronger filtering, i.e. more smooth but also larger latency + low_pass_alpha: 0.2 diff --git a/dex_retargeting/configs/teleop/leap_hand_left_dexpilot.yml b/dex_retargeting/configs/teleop/leap_hand_left_dexpilot.yml new file mode 100644 index 0000000..509554f --- /dev/null +++ b/dex_retargeting/configs/teleop/leap_hand_left_dexpilot.yml @@ -0,0 +1,12 @@ +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 + finger_tip_link_names: [ "thumb_tip_head", "index_tip_head", "middle_tip_head", "ring_tip_head" ] + scaling_factor: 1.6 + + # A smaller alpha means stronger filtering, i.e. more smooth but also larger latency + low_pass_alpha: 0.2 diff --git a/dex_retargeting/configs/teleop/shadow_hand_left.yml b/dex_retargeting/configs/teleop/shadow_hand_left.yml new file mode 100644 index 0000000..21de464 --- /dev/null +++ b/dex_retargeting/configs/teleop/shadow_hand_left.yml @@ -0,0 +1,17 @@ +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 + + # Source refers to the retargeting input, which usually corresponds to the human hand + # The joint indices of human hand joint which corresponds to each link in the target_link_names + target_link_human_indices: [ [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ], [ 4, 8, 12, 16, 20, 2, 6, 10, 14, 18 ] ] + + # A smaller alpha means stronger filtering, i.e. more smooth but also larger latency + low_pass_alpha: 0.2 diff --git a/dex_retargeting/configs/teleop/shadow_hand_left_dexpilot.yml b/dex_retargeting/configs/teleop/shadow_hand_left_dexpilot.yml new file mode 100644 index 0000000..730418e --- /dev/null +++ b/dex_retargeting/configs/teleop/shadow_hand_left_dexpilot.yml @@ -0,0 +1,12 @@ +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 + finger_tip_link_names: [ "thtip", "fftip", "mftip", "rftip", "lftip" ] + scaling_factor: 1.2 + + # A smaller alpha means stronger filtering, i.e. more smooth but also larger latency + low_pass_alpha: 0.2 diff --git a/dex_retargeting/constants.py b/dex_retargeting/constants.py index d93cdee..27f7e87 100644 --- a/dex_retargeting/constants.py +++ b/dex_retargeting/constants.py @@ -8,6 +8,7 @@ class RobotName(enum.Enum): svh = enum.auto() leap = enum.auto() ability = enum.auto() + inspire = enum.auto() class RetargetingType(enum.Enum): @@ -27,6 +28,7 @@ class HandType(enum.Enum): RobotName.svh: "schunk_svh_hand", RobotName.leap: "leap_hand", RobotName.ability: "ability_hand", + RobotName.inspire: "inspire_hand", } ROBOT_NAMES = list(ROBOT_NAME_MAP.keys()) diff --git a/tests/test_optimizer.py b/tests/test_optimizer.py index d12d84f..a3013a1 100644 --- a/tests/test_optimizer.py +++ b/tests/test_optimizer.py @@ -63,7 +63,7 @@ def generate_position_retargeting_data_gt(robot: RobotWrapper, optimizer: Positi return random_pin_qpos, init_qpos, random_target_pos @pytest.mark.parametrize("robot_name", ROBOT_NAMES) - @pytest.mark.parametrize("hand_type", [name for name in HandType][:1]) + @pytest.mark.parametrize("hand_type", [name for name in HandType]) def test_position_optimizer(self, robot_name, hand_type): config_path = get_default_config_path(robot_name, RetargetingType.position, hand_type) @@ -105,7 +105,7 @@ def test_position_optimizer(self, robot_name, hand_type): assert np.mean(errors["pos"]) < 1e-2 @pytest.mark.parametrize("robot_name", ROBOT_NAMES) - @pytest.mark.parametrize("hand_type", [name for name in HandType][:1]) + @pytest.mark.parametrize("hand_type", [name for name in HandType]) def test_vector_optimizer(self, robot_name, hand_type): config_path = get_default_config_path(robot_name, RetargetingType.vector, hand_type) @@ -153,7 +153,7 @@ def test_vector_optimizer(self, robot_name, hand_type): assert np.mean(errors["pos"]) < 1e-2 @pytest.mark.parametrize("robot_name", DEXPILOT_ROBOT_NAMES) - @pytest.mark.parametrize("hand_type", [name for name in HandType][:1]) + @pytest.mark.parametrize("hand_type", [name for name in HandType]) def test_dexpilot_optimizer(self, robot_name, hand_type): config_path = get_default_config_path(robot_name, RetargetingType.dexpilot, hand_type)