From e0c6ad863fa206c828d1455b03e2e4da7eaf974d Mon Sep 17 00:00:00 2001 From: James Tigue Date: Tue, 1 Apr 2025 13:49:23 -0400 Subject: [PATCH 1/7] add link_pose and link_projected_grav --- .../isaaclab/envs/mdp/observations.py | 58 +++++++++++++++++++ 1 file changed, 58 insertions(+) diff --git a/source/isaaclab/isaaclab/envs/mdp/observations.py b/source/isaaclab/isaaclab/envs/mdp/observations.py index 12e93b69d28..d9a39a73518 100644 --- a/source/isaaclab/isaaclab/envs/mdp/observations.py +++ b/source/isaaclab/isaaclab/envs/mdp/observations.py @@ -96,6 +96,64 @@ def root_ang_vel_w(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntity return asset.data.root_ang_vel_w +""" +Link state +""" + + +def link_pose( + env: ManagerBasedEnv, + asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), + link_name: str | None = None, +) -> torch.Tensor: + """The link pose of the asset w.r.t the env.scene.origin. + + Args: + env: The environment. + asset_cfg: The SceneEntity associated with this observation. + link_name: The specific name of the link in the asset to extract, defaults to base link of Articulation. + + Returns: + The pose of link_name with shape [num_env, 7]. Output order is [x,y,z,qw,qx,qy,qz]. + """ + # extract the used quantities (to enable type-hinting) + asset: Articulation = env.scene[asset_cfg.name] + + if link_name is None: + link_name = asset.body_names[0] # set body name as the base link + link_id = asset.body_names.index(link_name) + pose = asset.data.body_state_w[:, link_id, :7] + pose[:, :3] = pose[:, :3] - env.scene.env_origins + return pose + + +def link_projected_gravity( + env: ManagerBasedEnv, + asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), + link_name: str | None = None, +) -> torch.Tensor: + """The direction of gravity projected on to link_name of an Articulation defined in asset_cfg. + + Args: + env: The environment. + asset_cfg: The Articulation associated with this observation. + link_name: The specific name of the link in the asset to extract, defaults to base link of Articulation. + + Returns: + The unit vector direction of gravity projected onto link_name's frame. + """ + # extract the used quantities (to enable type-hinting) + asset: Articulation = env.scene[asset_cfg.name] + if link_name is not None: + body_id = asset.body_names.index(link_name) + else: + # default to 0th link, which is the base link + body_id = 0 + body_quat = asset.data.body_quat_w[:, body_id] + gravity_dir = asset.data.GRAVITY_VEC_W + return math_utils.quat_rotate_inverse(body_quat, gravity_dir).view(-1) + + """ Joint state. """ From 6e7cbb8ad61f6cf5ea94c74df8338132f55a1e78 Mon Sep 17 00:00:00 2001 From: James Tigue Date: Tue, 1 Apr 2025 13:51:32 -0400 Subject: [PATCH 2/7] changelog --- source/isaaclab/config/extension.toml | 2 +- source/isaaclab/docs/CHANGELOG.rst | 10 ++++++++++ 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/source/isaaclab/config/extension.toml b/source/isaaclab/config/extension.toml index b47bd022ef4..b4711d2624d 100644 --- a/source/isaaclab/config/extension.toml +++ b/source/isaaclab/config/extension.toml @@ -1,7 +1,7 @@ [package] # Note: Semantic Versioning is used: https://semver.org/ -version = "0.36.3" +version = "0.37.0" # Description title = "Isaac Lab framework for Robot Learning" diff --git a/source/isaaclab/docs/CHANGELOG.rst b/source/isaaclab/docs/CHANGELOG.rst index 7ab5e86c40f..fb289d2defe 100644 --- a/source/isaaclab/docs/CHANGELOG.rst +++ b/source/isaaclab/docs/CHANGELOG.rst @@ -1,6 +1,16 @@ Changelog --------- +0.37.0 (2025-04-01) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added :meth:`~isaaclab.envs.mdp.observations.link_pose` +* Added :meth:`~isaaclab.envs.mdp.observations.link_projected_gravity` + + 0.36.4 (2025-03-24) ~~~~~~~~~~~~~~~~~~~ From 19b85b902122d0733aadcb72f879025b9dbaf8be Mon Sep 17 00:00:00 2001 From: James Tigue Date: Tue, 1 Apr 2025 14:24:31 -0400 Subject: [PATCH 3/7] change link to body --- .../isaaclab/envs/mdp/observations.py | 35 +++++++++---------- 1 file changed, 17 insertions(+), 18 deletions(-) diff --git a/source/isaaclab/isaaclab/envs/mdp/observations.py b/source/isaaclab/isaaclab/envs/mdp/observations.py index d9a39a73518..1225a5a9bee 100644 --- a/source/isaaclab/isaaclab/envs/mdp/observations.py +++ b/source/isaaclab/isaaclab/envs/mdp/observations.py @@ -97,57 +97,56 @@ def root_ang_vel_w(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntity """ -Link state +Body state """ -def link_pose( +def body_pose_w( env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), - link_name: str | None = None, + body_name: str | None = None, ) -> torch.Tensor: - """The link pose of the asset w.r.t the env.scene.origin. + """The body pose of the asset w.r.t the env.scene.origin. Args: env: The environment. asset_cfg: The SceneEntity associated with this observation. - link_name: The specific name of the link in the asset to extract, defaults to base link of Articulation. + body_name: The specific name of the body in the asset to extract, defaults to base body of Articulation. Returns: - The pose of link_name with shape [num_env, 7]. Output order is [x,y,z,qw,qx,qy,qz]. + The pose of body_name with shape [num_env, 7]. Output order is [x,y,z,qw,qx,qy,qz]. """ # extract the used quantities (to enable type-hinting) asset: Articulation = env.scene[asset_cfg.name] - if link_name is None: - link_name = asset.body_names[0] # set body name as the base link - link_id = asset.body_names.index(link_name) - pose = asset.data.body_state_w[:, link_id, :7] + if body_name is None: + body_name = asset.body_names[0] + body_id = asset.body_names.index(body_name) + pose = asset.data.body_state_w[:, body_id, :7] pose[:, :3] = pose[:, :3] - env.scene.env_origins return pose -def link_projected_gravity( +def body_projected_gravity_b( env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), - link_name: str | None = None, + body_name: str | None = None, ) -> torch.Tensor: - """The direction of gravity projected on to link_name of an Articulation defined in asset_cfg. + """The direction of gravity projected on to body_name of an Articulation defined in asset_cfg. Args: env: The environment. asset_cfg: The Articulation associated with this observation. - link_name: The specific name of the link in the asset to extract, defaults to base link of Articulation. + body_name: The specific name of the body in the asset to extract, defaults to base body of Articulation. Returns: - The unit vector direction of gravity projected onto link_name's frame. + The unit vector direction of gravity projected onto body_name's frame. """ # extract the used quantities (to enable type-hinting) asset: Articulation = env.scene[asset_cfg.name] - if link_name is not None: - body_id = asset.body_names.index(link_name) + if body_name is not None: + body_id = asset.body_names.index(body_name) else: - # default to 0th link, which is the base link body_id = 0 body_quat = asset.data.body_quat_w[:, body_id] gravity_dir = asset.data.GRAVITY_VEC_W From 9163dba8acc1ea39efa36ae1d885b403b869ff2a Mon Sep 17 00:00:00 2001 From: James Tigue Date: Wed, 2 Apr 2025 13:25:38 -0400 Subject: [PATCH 4/7] generalize to flattened bodies in SceneEntityCfg --- .../isaaclab/isaaclab/envs/mdp/observations.py | 16 +++++----------- 1 file changed, 5 insertions(+), 11 deletions(-) diff --git a/source/isaaclab/isaaclab/envs/mdp/observations.py b/source/isaaclab/isaaclab/envs/mdp/observations.py index 1225a5a9bee..33338e0593d 100644 --- a/source/isaaclab/isaaclab/envs/mdp/observations.py +++ b/source/isaaclab/isaaclab/envs/mdp/observations.py @@ -104,27 +104,21 @@ def root_ang_vel_w(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntity def body_pose_w( env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), - body_name: str | None = None, ) -> torch.Tensor: - """The body pose of the asset w.r.t the env.scene.origin. + """The flattened body poses of the asset w.r.t the env.scene.origin. Args: env: The environment. asset_cfg: The SceneEntity associated with this observation. - body_name: The specific name of the body in the asset to extract, defaults to base body of Articulation. Returns: - The pose of body_name with shape [num_env, 7]. Output order is [x,y,z,qw,qx,qy,qz]. + The poses of bodies in articualtion [num_env, 7*num_bodies]. Output order is [x,y,z,qw,qx,qy,qz] per body. """ # extract the used quantities (to enable type-hinting) asset: Articulation = env.scene[asset_cfg.name] - - if body_name is None: - body_name = asset.body_names[0] - body_id = asset.body_names.index(body_name) - pose = asset.data.body_state_w[:, body_id, :7] - pose[:, :3] = pose[:, :3] - env.scene.env_origins - return pose + pose = asset.data.body_state_w[:, asset_cfg.body_ids, :7] + pose[..., :3] = pose[..., :3] - env.scene.env_origins.unsqueeze(1) + return pose.reshape(env.num_envs, -1) def body_projected_gravity_b( From fc06c7abbae0b1000dd5168856d4de4c2d25f98d Mon Sep 17 00:00:00 2001 From: James Tigue Date: Wed, 2 Apr 2025 13:39:35 -0400 Subject: [PATCH 5/7] update docstrings --- .../isaaclab/envs/mdp/observations.py | 25 ++++++++++--------- 1 file changed, 13 insertions(+), 12 deletions(-) diff --git a/source/isaaclab/isaaclab/envs/mdp/observations.py b/source/isaaclab/isaaclab/envs/mdp/observations.py index 33338e0593d..74f621a57ff 100644 --- a/source/isaaclab/isaaclab/envs/mdp/observations.py +++ b/source/isaaclab/isaaclab/envs/mdp/observations.py @@ -107,12 +107,15 @@ def body_pose_w( ) -> torch.Tensor: """The flattened body poses of the asset w.r.t the env.scene.origin. + Note: Only the bodies configured in :attr:`asset_cfg.body_ids` will have their poses returned. + Args: env: The environment. asset_cfg: The SceneEntity associated with this observation. Returns: - The poses of bodies in articualtion [num_env, 7*num_bodies]. Output order is [x,y,z,qw,qx,qy,qz] per body. + The poses of bodies in articulation [num_env, 7*num_bodies]. Pose order is [x,y,z,qw,qx,qy,qz]. Output is + stacked horizontally per body. """ # extract the used quantities (to enable type-hinting) asset: Articulation = env.scene[asset_cfg.name] @@ -124,27 +127,25 @@ def body_pose_w( def body_projected_gravity_b( env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), - body_name: str | None = None, ) -> torch.Tensor: - """The direction of gravity projected on to body_name of an Articulation defined in asset_cfg. + """The direction of gravity projected on to bodies of an Articulation. + + Note: Only the bodies configured in :attr:`asset_cfg.body_ids` will have their poses returned. Args: env: The environment. asset_cfg: The Articulation associated with this observation. - body_name: The specific name of the body in the asset to extract, defaults to base body of Articulation. Returns: - The unit vector direction of gravity projected onto body_name's frame. + The unit vector direction of gravity projected onto body_name's frame. Gravity projection vector order is + [x,y,z]. Output is stacked horizontally per body. """ # extract the used quantities (to enable type-hinting) asset: Articulation = env.scene[asset_cfg.name] - if body_name is not None: - body_id = asset.body_names.index(body_name) - else: - body_id = 0 - body_quat = asset.data.body_quat_w[:, body_id] - gravity_dir = asset.data.GRAVITY_VEC_W - return math_utils.quat_rotate_inverse(body_quat, gravity_dir).view(-1) + + body_quat = asset.data.body_quat_w[:, asset_cfg.body_ids] + gravity_dir = asset.data.GRAVITY_VEC_W.unsqueeze(1) + return math_utils.quat_rotate_inverse(body_quat, gravity_dir).view(env.num_envs, -1) """ From 6fd8541c8e458abf6b0df0e7a6f91b98177753ea Mon Sep 17 00:00:00 2001 From: James Tigue Date: Wed, 2 Apr 2025 13:40:24 -0400 Subject: [PATCH 6/7] update changelog --- source/isaaclab/docs/CHANGELOG.rst | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/source/isaaclab/docs/CHANGELOG.rst b/source/isaaclab/docs/CHANGELOG.rst index fb289d2defe..8306d092dcf 100644 --- a/source/isaaclab/docs/CHANGELOG.rst +++ b/source/isaaclab/docs/CHANGELOG.rst @@ -7,8 +7,8 @@ Changelog Added ^^^^^ -* Added :meth:`~isaaclab.envs.mdp.observations.link_pose` -* Added :meth:`~isaaclab.envs.mdp.observations.link_projected_gravity` +* Added :meth:`~isaaclab.envs.mdp.observations.body_pose_w` +* Added :meth:`~isaaclab.envs.mdp.observations.body_projected_gravity_b` 0.36.4 (2025-03-24) From d0a0b9e3430591281797bdf107ce9410d3820570 Mon Sep 17 00:00:00 2001 From: Kelly Guo Date: Fri, 2 May 2025 15:36:25 -0700 Subject: [PATCH 7/7] Update source/isaaclab/docs/CHANGELOG.rst Signed-off-by: Kelly Guo --- source/isaaclab/docs/CHANGELOG.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/source/isaaclab/docs/CHANGELOG.rst b/source/isaaclab/docs/CHANGELOG.rst index 9a27a9c2f28..88b088fd26b 100644 --- a/source/isaaclab/docs/CHANGELOG.rst +++ b/source/isaaclab/docs/CHANGELOG.rst @@ -88,7 +88,7 @@ Changed * Modified rendering mode default behavior when the launcher arg :attr:`enable_cameras` is not set. -0.36.15 (2025-03-24) +0.36.15 (2025-03-25) ~~~~~~~~~~~~~~~~~~~~ Added