Merge pull request NVlabs#332 from tahsinkose/tahsin/fix-get-link-tra…

…nsform

Fix get_link_transform API method

src/curobo/cuda_robot_model/cuda_robot_model.py

@@ -412,7 +412,7 @@ def get_link_mesh(self, link_name: str) -> Mesh:
         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
 

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