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 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