From 1dc81627bcf9bd8c39b0d3fd6879cc1ee0c152df Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tahsincan=20K=C3=B6se?= Date: Wed, 10 Jul 2024 12:32:12 +0300 Subject: [PATCH 1/2] Add unit-test for get_link_transform API. --- tests/curobo_robot_world_model_test.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/tests/curobo_robot_world_model_test.py b/tests/curobo_robot_world_model_test.py index 0778308..4991e7d 100644 --- a/tests/curobo_robot_world_model_test.py +++ b/tests/curobo_robot_world_model_test.py @@ -134,3 +134,9 @@ def test_cu_robot_batch_world_collision(): ) assert d_world.shape[0] == b assert torch.sum(d_world) == 0.0 + +def test_cu_robot_get_link_transform(): + model = load_robot_world() + world_T_panda_hand = model.kinematics.get_link_transform("panda_hand") + # It seems the panda hand is initialized at the origin. + assert torch.sum(world_T_panda_hand.position) == 0.0 \ No newline at end of file From ba4e57ede9874125f5edff06e4c45f12f4fbd3c1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tahsincan=20K=C3=B6se?= Date: Wed, 10 Jul 2024 12:32:47 +0300 Subject: [PATCH 2/2] Fix get_link_transform method in CudaRobotModel. --- src/curobo/cuda_robot_model/cuda_robot_model.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/curobo/cuda_robot_model/cuda_robot_model.py b/src/curobo/cuda_robot_model/cuda_robot_model.py index f52282c..2057ca3 100644 --- a/src/curobo/cuda_robot_model/cuda_robot_model.py +++ b/src/curobo/cuda_robot_model/cuda_robot_model.py @@ -412,7 +412,7 @@ class CudaRobotModel(CudaRobotModelConfig): return mesh def get_link_transform(self, link_name: str) -> Pose: - mat = self._kinematics_config.fixed_transforms[self._name_to_idx_map[link_name]] + mat = self.kinematics_config.fixed_transforms[self.kinematics_config.link_name_to_idx_map[link_name]] pose = Pose(position=mat[:3, 3], rotation=mat[:3, :3]) return pose