Skip to content

Commit

Permalink
[fix] small fix for hand type and improve verbose
Browse files Browse the repository at this point in the history
  • Loading branch information
yzqin committed Jul 19, 2024
1 parent ce1821a commit ee18b92
Show file tree
Hide file tree
Showing 6 changed files with 33 additions and 17 deletions.
2 changes: 1 addition & 1 deletion assets
16 changes: 16 additions & 0 deletions dex_retargeting/configs/offline/panda_gripper.yml
Original file line number Diff line number Diff line change
@@ -0,0 +1,16 @@
retargeting:
type: position
urdf_path: panda_gripper/panda_gripper_glb.urdf

target_joint_names: [ "panda_finger_joint1" ]
target_link_names: [ "panda_leftfinger", "panda_rightfinger"]

target_link_human_indices: [ 4, 8 ]
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
18 changes: 13 additions & 5 deletions dex_retargeting/constants.py
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
import enum
from pathlib import Path
from typing import Optional


class RobotName(enum.Enum):
Expand All @@ -21,7 +22,6 @@ class RetargetingType(enum.Enum):
class HandType(enum.Enum):
right = enum.auto()
left = enum.auto()
both = enum.auto()


ROBOT_NAME_MAP = {
Expand All @@ -37,7 +37,9 @@ class HandType(enum.Enum):
ROBOT_NAMES = list(ROBOT_NAME_MAP.keys())


def get_default_config_path(robot_name: RobotName, retargeting_type: RetargetingType, hand_type: HandType) -> Path:
def get_default_config_path(
robot_name: RobotName, retargeting_type: RetargetingType, hand_type: HandType
) -> Optional[Path]:
config_path = Path(__file__).parent / "configs"
if retargeting_type is RetargetingType.position:
config_path = config_path / "offline"
Expand All @@ -46,8 +48,14 @@ def get_default_config_path(robot_name: RobotName, retargeting_type: Retargeting

robot_name_str = ROBOT_NAME_MAP[robot_name]
hand_type_str = hand_type.name
if retargeting_type == RetargetingType.dexpilot:
config_name = f"{robot_name_str}_{hand_type_str}_dexpilot.yml"
if "gripper" in robot_name_str:
config_name = f"{robot_name_str}.yml" # For gripper robots, only use gripper config file.
if retargeting_type == RetargetingType.dexpilot:
print(f"DexPilot retargeting is not supported for gripper.")
return None
else:
config_name = f"{robot_name_str}_{hand_type_str}.yml"
if retargeting_type == RetargetingType.dexpilot:
config_name = f"{robot_name_str}_{hand_type_str}_dexpilot.yml"
else:
config_name = f"{robot_name_str}_{hand_type_str}.yml"
return config_path / config_name
2 changes: 1 addition & 1 deletion example/profiling/profile_online_retargeting.py
Original file line number Diff line number Diff line change
Expand Up @@ -51,7 +51,7 @@ def main():
config_path = get_default_config_path(
robot_name,
RetargetingType.vector,
HandType.right if "gripper" not in ROBOT_NAME_MAP[robot_name] else HandType.both,
HandType.right,
)
retargeting = RetargetingConfig.load_from_file(config_path).build()
total_time = profile_retargeting(retargeting, joint_data)
Expand Down
12 changes: 2 additions & 10 deletions tests/test_optimizer.py
Original file line number Diff line number Diff line change
Expand Up @@ -67,10 +67,6 @@ def generate_position_retargeting_data_gt(robot: RobotWrapper, optimizer: Positi
def test_position_optimizer(self, robot_name, hand_type):
config_path = get_default_config_path(robot_name, RetargetingType.position, hand_type)

if not config_path.exists():
print(f"Skip the test for {robot_name.name} as the config file does not exist.")
return

# Note: The parameters below are adjusted solely for this test
# The smoothness penalty is deactivated here, meaning no low pass filter and no continuous joint value
# This is because the test is focused solely on the efficiency of single step optimization
Expand Down Expand Up @@ -113,9 +109,7 @@ def test_position_optimizer(self, robot_name, hand_type):
@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)

if not config_path.exists():
print(f"Skip the test for {robot_name.name} as the config file does not exist.")
if config_path is None:
return

# Note: The parameters below are adjusted solely for this test
Expand Down Expand Up @@ -166,9 +160,7 @@ def test_vector_optimizer(self, robot_name, hand_type):
@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)

if not config_path.exists():
print(f"Skip the test for {robot_name.name} as the config file does not exist.")
if config_path is None:
return

# Note: The parameters below are adjusted solely for this test
Expand Down

0 comments on commit ee18b92

Please sign in to comment.