From 0a4093c7ae95810037f372aadfbba0011c05e227 Mon Sep 17 00:00:00 2001 From: yzqin Date: Sun, 27 Aug 2023 15:24:59 -0700 Subject: [PATCH] [update] update dexpilot retargeting test --- dex_retargeting/constants.py | 7 +++- tests/test_sapien_optimizer.py | 60 ++++++++++++++++++++++++++++++---- 2 files changed, 60 insertions(+), 7 deletions(-) diff --git a/dex_retargeting/constants.py b/dex_retargeting/constants.py index 331b053..4df60cd 100644 --- a/dex_retargeting/constants.py +++ b/dex_retargeting/constants.py @@ -25,6 +25,8 @@ class HandType(enum.Enum): RobotName.svh: "schunk_svh_hand", } +ROBOT_NAMES = list(ROBOT_NAME_MAP.keys()) + def get_config_path(robot_name: RobotName, retargeting_type: RetargetingType, hand_type: HandType) -> Path: config_path = Path(__file__).parent / "configs" @@ -35,5 +37,8 @@ def get_config_path(robot_name: RobotName, retargeting_type: RetargetingType, ha robot_name_str = ROBOT_NAME_MAP[robot_name] hand_type_str = hand_type.name - 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 diff --git a/tests/test_sapien_optimizer.py b/tests/test_sapien_optimizer.py index dea42aa..dd3ebc2 100644 --- a/tests/test_sapien_optimizer.py +++ b/tests/test_sapien_optimizer.py @@ -7,7 +7,9 @@ from dex_retargeting.optimizer import VectorOptimizer, PositionOptimizer from dex_retargeting.retargeting_config import RetargetingConfig -from utils import ROBOT_NAMES, VECTOR_CONFIG_DICT, POSITION_CONFIG_DICT + +# from utils import ROBOT_NAMES, VECTOR_CONFIG_DICT, POSITION_CONFIG_DICT +from dex_retargeting.constants import ROBOT_NAMES, get_config_path, RetargetingType, HandType class TestVectorOptimizer: @@ -42,8 +44,9 @@ def generate_position_retargeting_data_gt(robot: sapien.Articulation, optimizer: return random_qpos, init_qpos, random_target_pos @pytest.mark.parametrize("robot_name", ROBOT_NAMES[:1]) - def test_position_optimizer(self, robot_name): - config_path = self.config_dir / POSITION_CONFIG_DICT[robot_name] + @pytest.mark.parametrize("hand_type", [name for name in HandType][:1]) + def test_position_optimizer(self, robot_name, hand_type): + config_path = get_config_path(robot_name, RetargetingType.position, hand_type) # 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 @@ -80,15 +83,15 @@ def test_position_optimizer(self, robot_name): assert np.mean(errors["pos"]) < 1e-2 @pytest.mark.parametrize("robot_name", ROBOT_NAMES) - def test_vector_optimizer(self, robot_name): - config_path = self.config_dir / VECTOR_CONFIG_DICT[robot_name] + @pytest.mark.parametrize("hand_type", [name for name in HandType][:1]) + def test_vector_optimizer(self, robot_name, hand_type): + config_path = get_config_path(robot_name, RetargetingType.vector, hand_type) # Note: The parameters below are adjusted solely for this test # For retargeting from human to robot, their values should remain the default in the retargeting config # 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 override = dict() - override["type"] = "vector" # cast it to vector retargeting even if it is DexPilot retargeting override["low_pass_alpha"] = 0 override["scaling_factor"] = 1.0 override["normal_delta"] = 0 @@ -123,3 +126,48 @@ def test_vector_optimizer(self, robot_name): print(f"Mean optimization vector error: {np.mean(errors['pos'])}") print(f"Retargeting computation takes {tac - tic}s for {num_optimization} times") assert np.mean(errors["pos"]) < 1e-2 + + @pytest.mark.parametrize("robot_name", ROBOT_NAMES[:1]) + @pytest.mark.parametrize("hand_type", [name for name in HandType][:1]) + def test_dexpilot_optimizer(self, robot_name, hand_type): + config_path = get_config_path(robot_name, RetargetingType.dexpilot, hand_type) + + # Note: The parameters below are adjusted solely for this test + # For retargeting from human to robot, their values should remain the default in the retargeting config + # 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 + override = dict() + override["low_pass_alpha"] = 0 + override["scaling_factor"] = 1.0 + override["normal_delta"] = 0 + config = RetargetingConfig.load_from_file(config_path, override) + + retargeting = config.build() + + robot = retargeting.optimizer.robot + optimizer = retargeting.optimizer + + num_optimization = 100 + tic = time() + errors = dict(pos=[], joint=[]) + np.random.seed(1) + for i in range(num_optimization): + # Sampled random vector + random_qpos, init_qpos, random_target_vector = self.generate_vector_retargeting_data_gt(robot, optimizer) + + # Optimized vector + computed_qpos = optimizer.retarget(random_target_vector, fixed_qpos=[], last_qpos=init_qpos[:]) + robot.set_qpos(np.array(computed_qpos)) + computed_pos = np.array([robot.get_links()[i].get_pose().p for i in optimizer.robot_link_indices]) + computed_origin_pos = computed_pos[optimizer.origin_link_indices] + computed_task_pos = computed_pos[optimizer.task_link_indices] + computed_target_vector = computed_task_pos - computed_origin_pos + + # Vector difference + error = np.mean(np.linalg.norm(computed_target_vector - random_target_vector, axis=1)) + errors["pos"].append(error) + + tac = time() + print(f"Mean optimization vector error for DexPilot retargeting: {np.mean(errors['pos'])}") + print(f"Retargeting computation takes {tac - tic}s for {num_optimization} times") + assert np.mean(errors["pos"]) < 1e-2