From 73b4f137e8976f1cd7cec7e3103b7e7572a00edb Mon Sep 17 00:00:00 2001 From: Hongyu Li Date: Tue, 24 Sep 2024 18:53:32 -0400 Subject: [PATCH 01/10] add camera setter --- .../omni/isaac/lab/sensors/camera/camera.py | 74 ++++++++++++++++++- 1 file changed, 72 insertions(+), 2 deletions(-) diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/camera/camera.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/camera/camera.py index e5692887d3..137a47a986 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/camera/camera.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/camera/camera.py @@ -280,7 +280,7 @@ def set_world_poses( env_ids: Sequence[int] | None = None, convention: Literal["opengl", "ros", "world"] = "ros", ): - r"""Set the pose of the camera w.r.t. the world frame using specified convention. + r"""Set the world pose of the camera w.r.t. the world frame using specified convention. Since different fields use different conventions for camera orientations, the method allows users to set the camera poses in the specified convention. Possible conventions are: @@ -325,7 +325,7 @@ def set_world_poses( def set_world_poses_from_view( self, eyes: torch.Tensor, targets: torch.Tensor, env_ids: Sequence[int] | None = None ): - """Set the poses of the camera from the eye position and look-at target position. + """Set the world pose of the camera from the eye position and look-at target position. Args: eyes: The positions of the camera's eye. Shape is (N, 3). @@ -343,6 +343,76 @@ def set_world_poses_from_view( orientations = quat_from_matrix(create_rotation_matrix_from_view(eyes, targets, device=self._device)) self._view.set_world_poses(eyes, orientations, env_ids) + def set_local_poses( + self, + positions: torch.Tensor | None = None, + orientations: torch.Tensor | None = None, + env_ids: Sequence[int] | None = None, + convention: Literal["opengl", "ros", "world"] = "ros", + ): + r"""Set the local pose of the camera w.r.t. the local frame using specified convention. + + Since different fields use different conventions for camera orientations, the method allows users to + set the camera poses in the specified convention. Possible conventions are: + + - :obj:`"opengl"` - forward axis: -Z - up axis +Y - Offset is applied in the OpenGL (Usd.Camera) convention + - :obj:`"ros"` - forward axis: +Z - up axis -Y - Offset is applied in the ROS convention + - :obj:`"world"` - forward axis: +X - up axis +Z - Offset is applied in the World Frame convention + + See :meth:`omni.isaac.lab.sensors.camera.utils.convert_orientation_convention` for more details + on the conventions. + + Args: + positions: The cartesian coordinates (in meters). Shape is (N, 3). + Defaults to None, in which case the camera position in not changed. + orientations: The quaternion orientation in (w, x, y, z). Shape is (N, 4). + Defaults to None, in which case the camera orientation in not changed. + env_ids: A sensor ids to manipulate. Defaults to None, which means all sensor indices. + convention: The convention in which the poses are fed. Defaults to "ros". + + Raises: + RuntimeError: If the camera prim is not set. Need to call :meth:`initialize` method first. + """ + # resolve env_ids + if env_ids is None: + env_ids = self._ALL_INDICES + # convert to backend tensor + if positions is not None: + if isinstance(positions, np.ndarray): + positions = torch.from_numpy(positions).to(device=self._device) + elif not isinstance(positions, torch.Tensor): + positions = torch.tensor(positions, device=self._device) + # convert rotation matrix from input convention to OpenGL + if orientations is not None: + if isinstance(orientations, np.ndarray): + orientations = torch.from_numpy(orientations).to(device=self._device) + elif not isinstance(orientations, torch.Tensor): + orientations = torch.tensor(orientations, device=self._device) + orientations = convert_orientation_convention(orientations, origin=convention, target="opengl") + # set the pose + self._view.set_local_poses(positions, orientations, env_ids) + + def set_local_poses_from_view( + self, eyes: torch.Tensor, targets: torch.Tensor, env_ids: Sequence[int] | None = None + ): + """Set the local pose of the camera from the eye position and look-at target position. + + Args: + eyes: The positions of the camera's eye. Shape is (N, 3). + targets: The target locations to look at. Shape is (N, 3). + env_ids: A sensor ids to manipulate. Defaults to None, which means all sensor indices. + + Raises: + RuntimeError: If the camera prim is not set. Need to call :meth:`initialize` method first. + NotImplementedError: If the stage up-axis is not "Y" or "Z". + """ + # resolve env_ids + if env_ids is None: + env_ids = self._ALL_INDICES + # set camera poses using the view + orientations = quat_from_matrix(create_rotation_matrix_from_view(eyes, targets, device=self._device)) + self._view.set_local_poses(eyes, orientations, env_ids) + """ Operations """ From b6dc0f93560a24b3e05086981e427a7bf1877cd1 Mon Sep 17 00:00:00 2001 From: Hongyu Li Date: Tue, 24 Sep 2024 20:11:17 -0400 Subject: [PATCH 02/10] add get local pose --- .../omni/isaac/lab/sensors/camera/camera.py | 21 +++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/camera/camera.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/camera/camera.py index 137a47a986..2941a4a83d 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/camera/camera.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/camera/camera.py @@ -413,6 +413,27 @@ def set_local_poses_from_view( orientations = quat_from_matrix(create_rotation_matrix_from_view(eyes, targets, device=self._device)) self._view.set_local_poses(eyes, orientations, env_ids) + """ + Operations - Get pose. + """ + def get_local_poses(self, env_ids: Sequence[int] | None = None): + """Get the local pose of the camera. + + Args: + env_ids: A sensor ids to manipulate. Defaults to None, which means all sensor indices. + + Raises: + RuntimeError: If the camera prim is not set. Need to call :meth:`initialize` method first. + + Returns: the position and orientation of the local camera pose. quaternion is scalar-first (w, x, y, z) + """ + # resolve env_ids + if env_ids is None: + env_ids = self._ALL_INDICES + # set camera poses using the view + positions, orientations = self._view.get_world_poses(indices=env_ids) + return positions, orientations + """ Operations """ From bc5ff1c386ac597e2af6c16797784e64375d85cf Mon Sep 17 00:00:00 2001 From: Hongyu Li Date: Mon, 7 Oct 2024 10:20:03 -0400 Subject: [PATCH 03/10] fix typo --- .../omni/isaac/lab/sensors/camera/camera.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/camera/camera.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/camera/camera.py index 2941a4a83d..6a655915a9 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/camera/camera.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/camera/camera.py @@ -416,16 +416,17 @@ def set_local_poses_from_view( """ Operations - Get pose. """ - def get_local_poses(self, env_ids: Sequence[int] | None = None): - """Get the local pose of the camera. + + def get_world_poses(self, env_ids: Sequence[int] | None = None): + """Get the world pose of the camera. Args: - env_ids: A sensor ids to manipulate. Defaults to None, which means all sensor indices. + env_ids: indices to specify which prims to query. Defaults to None, which means all sensor indices. Raises: RuntimeError: If the camera prim is not set. Need to call :meth:`initialize` method first. - Returns: the position and orientation of the local camera pose. quaternion is scalar-first (w, x, y, z) + Returns: the position and orientation of the world camera pose. quaternion is scalar-first (w, x, y, z) """ # resolve env_ids if env_ids is None: From 8a2f36541417f72f83d433c7cfb975f57bbff98c Mon Sep 17 00:00:00 2001 From: Hongyu Li Date: Mon, 7 Oct 2024 11:37:42 -0400 Subject: [PATCH 04/10] add test case --- .../test/sensors/test_camera.py | 25 +++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/source/extensions/omni.isaac.lab/test/sensors/test_camera.py b/source/extensions/omni.isaac.lab/test/sensors/test_camera.py index fc6953e166..b995c6df0b 100644 --- a/source/extensions/omni.isaac.lab/test/sensors/test_camera.py +++ b/source/extensions/omni.isaac.lab/test/sensors/test_camera.py @@ -360,6 +360,31 @@ def test_camera_set_world_poses_from_view(self): torch.testing.assert_close(camera.data.pos_w, eyes) torch.testing.assert_close(camera.data.quat_w_ros, quat_ros_gt) + def test_camera_get_world_poses(self): + """Test camera function to get world pose.""" + camera = Camera(self.camera_cfg) + # play sim + self.sim.reset() + + # convert to torch tensors + position = torch.tensor([POSITION], dtype=torch.float32, device=camera.device) + orientation = torch.tensor([QUAT_OPENGL], dtype=torch.float32, device=camera.device) + # set new pose + camera.set_world_poses(position.clone(), orientation.clone(), convention="opengl") + + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + self.sim.step() + + pos_w = camera.get_world_poses()[0] + quat_w_world = camera.get_world_poses()[1] + + # check if transform correctly set in output + torch.testing.assert_close(pos_w, position) + torch.testing.assert_close(quat_w_world, orientation) + def test_intrinsic_matrix(self): """Checks that the camera's set and retrieve methods work for intrinsic matrix.""" camera_cfg = copy.deepcopy(self.camera_cfg) From 3154d408971e4487da97c75795c4f6f47d28f39a Mon Sep 17 00:00:00 2001 From: Hongyu Li Date: Mon, 7 Oct 2024 11:38:18 -0400 Subject: [PATCH 05/10] update name --- CONTRIBUTORS.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CONTRIBUTORS.md b/CONTRIBUTORS.md index 8a96a25c18..58e14f2509 100644 --- a/CONTRIBUTORS.md +++ b/CONTRIBUTORS.md @@ -42,6 +42,7 @@ Guidelines for modifications: * Chenyu Yang * David Yang * HoJin Jeon +* Hongyu Li * Jean Tampon * Jia Lin Yuan * Jingzhou Liu From 6f27f3a7eb416e57b5010661ce96888297494c27 Mon Sep 17 00:00:00 2001 From: Hongyu Li Date: Tue, 8 Oct 2024 17:35:40 -0400 Subject: [PATCH 06/10] add get local pose --- .../omni/isaac/lab/sensors/camera/camera.py | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/camera/camera.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/camera/camera.py index 6a655915a9..7e61022e4f 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/camera/camera.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/camera/camera.py @@ -435,6 +435,24 @@ def get_world_poses(self, env_ids: Sequence[int] | None = None): positions, orientations = self._view.get_world_poses(indices=env_ids) return positions, orientations + def get_local_poses(self, env_ids: Sequence[int] | None = None): + """Get the local pose of the camera. + + Args: + env_ids: indices to specify which prims to query. Defaults to None, which means all sensor indices. + + Raises: + RuntimeError: If the camera prim is not set. Need to call :meth:`initialize` method first. + + Returns: the position and orientation of the local camera pose. quaternion is scalar-first (w, x, y, z) + """ + # resolve env_ids + if env_ids is None: + env_ids = self._ALL_INDICES + # set camera poses using the view + positions, orientations = self._view.get_local_poses(indices=env_ids) + return positions, orientations + """ Operations """ From 4e39dd474b1bc0fe7a410c52d070e0e9e493f11a Mon Sep 17 00:00:00 2001 From: Hongyu Li Date: Tue, 17 Dec 2024 23:19:40 -0500 Subject: [PATCH 07/10] add unit tests for local APIs --- .../test/sensors/test_camera.py | 76 +++++++++++++++++++ 1 file changed, 76 insertions(+) diff --git a/source/extensions/omni.isaac.lab/test/sensors/test_camera.py b/source/extensions/omni.isaac.lab/test/sensors/test_camera.py index b995c6df0b..80ebd91f5b 100644 --- a/source/extensions/omni.isaac.lab/test/sensors/test_camera.py +++ b/source/extensions/omni.isaac.lab/test/sensors/test_camera.py @@ -385,6 +385,82 @@ def test_camera_get_world_poses(self): torch.testing.assert_close(pos_w, position) torch.testing.assert_close(quat_w_world, orientation) + def test_camera_set_local_poses(self): + """Test camera function to set specific local pose.""" + camera = Camera(self.camera_cfg) + # play sim + self.sim.reset() + + # convert to torch tensors + position = torch.tensor([POSITION], dtype=torch.float32, device=camera.device) + orientation = torch.tensor([QUAT_WORLD], dtype=torch.float32, device=camera.device) + # set new local pose + camera.set_local_poses(position.clone(), orientation.clone(), convention="opengl") + + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + self.sim.step() + + # get local poses + local_pos, local_quat = camera._view.get_local_poses(indices=camera._ALL_INDICES) + + # check if transform correctly set in output + torch.testing.assert_close(local_pos, position) + torch.testing.assert_close(local_quat, orientation) + + def test_camera_set_local_poses_from_view(self): + """Test camera function to set specific local pose from view.""" + camera = Camera(self.camera_cfg) + # play sim + self.sim.reset() + + # convert to torch tensors + eyes = torch.tensor([POSITION], dtype=torch.float32, device=camera.device) + targets = torch.tensor([[0.0, 0.0, 0.0]], dtype=torch.float32, device=camera.device) + quat_ros_gt = torch.tensor([QUAT_OPENGL], dtype=torch.float32, device=camera.device) + # set new pose + camera.set_local_poses_from_view(eyes.clone(), targets.clone()) + + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + self.sim.step() + + # get local poses + local_pos, local_quat = camera._view.get_local_poses(indices=camera._ALL_INDICES) + + # check if transform correctly set in output + torch.testing.assert_close(local_pos, eyes) + torch.testing.assert_close(local_quat, quat_ros_gt) + + def test_camera_get_local_poses(self): + """Test camera function to get local pose.""" + camera = Camera(self.camera_cfg) + # play sim + self.sim.reset() + + # convert to torch tensors + position = torch.tensor([POSITION], dtype=torch.float32, device=camera.device) + orientation = torch.tensor([QUAT_OPENGL], dtype=torch.float32, device=camera.device) + # set new pose + camera.set_local_poses(position.clone(), orientation.clone(), convention="opengl") + + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + self.sim.step() + + pos_local = camera.get_local_poses()[0] + quat_local = camera.get_local_poses()[1] + + # check if transform correctly set in output + torch.testing.assert_close(pos_local, position) + torch.testing.assert_close(quat_local, orientation) + def test_intrinsic_matrix(self): """Checks that the camera's set and retrieve methods work for intrinsic matrix.""" camera_cfg = copy.deepcopy(self.camera_cfg) From 2641d0d3a00bc551b775db64471d1a71b1ce3a34 Mon Sep 17 00:00:00 2001 From: Hongyu Li Date: Wed, 18 Dec 2024 00:17:30 -0500 Subject: [PATCH 08/10] run format --- source/extensions/omni.isaac.lab/test/sensors/test_camera.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/source/extensions/omni.isaac.lab/test/sensors/test_camera.py b/source/extensions/omni.isaac.lab/test/sensors/test_camera.py index 2a05b15970..3b1afc346e 100644 --- a/source/extensions/omni.isaac.lab/test/sensors/test_camera.py +++ b/source/extensions/omni.isaac.lab/test/sensors/test_camera.py @@ -405,7 +405,7 @@ def test_camera_set_local_poses(self): # get local poses local_pos, local_quat = camera._view.get_local_poses(indices=camera._ALL_INDICES) - + # check if transform correctly set in output torch.testing.assert_close(local_pos, position) torch.testing.assert_close(local_quat, orientation) From 65fcca07e27a60d084ed3a68bc996294bbeb8df6 Mon Sep 17 00:00:00 2001 From: Hongyu Li Date: Wed, 18 Dec 2024 00:24:21 -0500 Subject: [PATCH 09/10] update changelog and extention.toml version number --- source/extensions/omni.isaac.lab/config/extension.toml | 2 +- source/extensions/omni.isaac.lab/docs/CHANGELOG.rst | 10 ++++++++++ 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/source/extensions/omni.isaac.lab/config/extension.toml b/source/extensions/omni.isaac.lab/config/extension.toml index 05f5be440a..d0ce0dae38 100644 --- a/source/extensions/omni.isaac.lab/config/extension.toml +++ b/source/extensions/omni.isaac.lab/config/extension.toml @@ -1,7 +1,7 @@ [package] # Note: Semantic Versioning is used: https://semver.org/ -version = "0.30.0" +version = "0.30.1" # Description title = "Isaac Lab framework for Robot Learning" diff --git a/source/extensions/omni.isaac.lab/docs/CHANGELOG.rst b/source/extensions/omni.isaac.lab/docs/CHANGELOG.rst index fe823cb371..c9be72977a 100644 --- a/source/extensions/omni.isaac.lab/docs/CHANGELOG.rst +++ b/source/extensions/omni.isaac.lab/docs/CHANGELOG.rst @@ -1,6 +1,16 @@ Changelog --------- +0.30.1 (2024-12-18) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added world pose getter +* Added local pose getter and setter + + 0.30.0 (2024-12-16) ~~~~~~~~~~~~~~~~~~~ From e04c919b676c6a464fa94da3230461e69301613a Mon Sep 17 00:00:00 2001 From: Hongyu Li Date: Wed, 18 Dec 2024 00:25:06 -0500 Subject: [PATCH 10/10] run format on CHANGELOG --- source/extensions/omni.isaac.lab/docs/CHANGELOG.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/source/extensions/omni.isaac.lab/docs/CHANGELOG.rst b/source/extensions/omni.isaac.lab/docs/CHANGELOG.rst index c9be72977a..00f25733c2 100644 --- a/source/extensions/omni.isaac.lab/docs/CHANGELOG.rst +++ b/source/extensions/omni.isaac.lab/docs/CHANGELOG.rst @@ -7,7 +7,7 @@ Changelog Added ^^^^^ -* Added world pose getter +* Added world pose getter * Added local pose getter and setter