diff --git a/docs/source/migration/migrating_from_isaacgymenvs.rst b/docs/source/migration/migrating_from_isaacgymenvs.rst index 22bc33c9a5..2632321917 100644 --- a/docs/source/migration/migrating_from_isaacgymenvs.rst +++ b/docs/source/migration/migrating_from_isaacgymenvs.rst @@ -814,9 +814,9 @@ The ``progress_buf`` variable has also been renamed to ``episode_length_buf``. | | | | | self.joint_pos[env_ids] = joint_pos | | | | -| | self.cartpole.write_root_link_pose_to_sim( | +| | self.cartpole.write_root_pose_to_sim( | | | default_root_state[:, :7], env_ids) | -| | self.cartpole.write_root_com_velocity_to_sim( | +| | self.cartpole.write_root_velocity_to_sim( | | | default_root_state[:, 7:], env_ids) | | | self.cartpole.write_joint_state_to_sim( | | | joint_pos, joint_vel, None, env_ids) | diff --git a/docs/source/migration/migrating_from_omniisaacgymenvs.rst b/docs/source/migration/migrating_from_omniisaacgymenvs.rst index 629be58bd7..7945ccb0b1 100644 --- a/docs/source/migration/migrating_from_omniisaacgymenvs.rst +++ b/docs/source/migration/migrating_from_omniisaacgymenvs.rst @@ -364,8 +364,8 @@ In Isaac Lab, ``root_pose`` and ``root_velocity`` have been combined into single .. code-block::python - self.cartpole.write_root_link_pose_to_sim(default_root_state[:, :7], env_ids) - self.cartpole.write_root_com_velocity_to_sim(default_root_state[:, 7:], env_ids) + self.cartpole.write_root_pose_to_sim(default_root_state[:, :7], env_ids) + self.cartpole.write_root_velocity_to_sim(default_root_state[:, 7:], env_ids) Creating a New Environment @@ -738,9 +738,9 @@ reset the ``episode_length_buf`` buffer. | 1.0 - 2.0 * torch.rand(num_resets, device=self._device)) | self.joint_pos[env_ids] = joint_pos | | | self.joint_vel[env_ids] = joint_vel | | # apply resets | | -| indices = env_ids.to(dtype=torch.int32) | self.cartpole.write_root_link_pose_to_sim( | +| indices = env_ids.to(dtype=torch.int32) | self.cartpole.write_root_pose_to_sim( | | self._cartpoles.set_joint_positions(dof_pos, indices=indices) | default_root_state[:, :7], env_ids) | -| self._cartpoles.set_joint_velocities(dof_vel, indices=indices) | self.cartpole.write_root_com_velocity_to_sim( | +| self._cartpoles.set_joint_velocities(dof_vel, indices=indices) | self.cartpole.write_root_velocity_to_sim( | | | default_root_state[:, 7:], env_ids) | | # bookkeeping | self.cartpole.write_joint_state_to_sim( | | self.reset_buf[env_ids] = 0 | joint_pos, joint_vel, None, env_ids) | diff --git a/docs/source/tutorials/01_assets/run_articulation.rst b/docs/source/tutorials/01_assets/run_articulation.rst index dd456d6c6d..47c0d5b9ec 100644 --- a/docs/source/tutorials/01_assets/run_articulation.rst +++ b/docs/source/tutorials/01_assets/run_articulation.rst @@ -66,7 +66,7 @@ Similar to a rigid object, an articulation also has a root state. This state cor articulation tree. On top of the root state, an articulation also has joint states. These states correspond to the joint positions and velocities. -To reset the articulation, we first set the root state by calling the :meth:`Articulation.write_root_link_pose_to_sim` and :meth:`Articulation.write_root_com_velocity_to_sim` +To reset the articulation, we first set the root state by calling the :meth:`Articulation.write_root_pose_to_sim` and :meth:`Articulation.write_root_velocity_to_sim` methods. Similarly, we set the joint states by calling the :meth:`Articulation.write_joint_state_to_sim` method. Finally, we call the :meth:`Articulation.reset` method to reset any internal buffers and caches. diff --git a/docs/source/tutorials/01_assets/run_deformable_object.rst b/docs/source/tutorials/01_assets/run_deformable_object.rst index 8edb2fdf4a..4041d7e7ed 100644 --- a/docs/source/tutorials/01_assets/run_deformable_object.rst +++ b/docs/source/tutorials/01_assets/run_deformable_object.rst @@ -149,7 +149,7 @@ the average position of all the nodes in the mesh. .. literalinclude:: ../../../../source/standalone/tutorials/01_assets/run_deformable_object.py :language: python :start-at: # update buffers - :end-at: print(f"Root position (in world): {cube_object.data.root_link_pos_w[:, :3]}") + :end-at: print(f"Root position (in world): {cube_object.data.root_pos_w[:, :3]}") The Code Execution diff --git a/docs/source/tutorials/01_assets/run_rigid_object.rst b/docs/source/tutorials/01_assets/run_rigid_object.rst index e4249c45e0..c4c521c395 100644 --- a/docs/source/tutorials/01_assets/run_rigid_object.rst +++ b/docs/source/tutorials/01_assets/run_rigid_object.rst @@ -96,7 +96,7 @@ desired state of the rigid object prim into the world frame before setting it. We use the :attr:`assets.RigidObject.data.default_root_state` attribute to get the default root state of the spawned rigid object prims. This default state can be configured from the :attr:`assets.RigidObjectCfg.init_state` attribute, which we left as identity in this tutorial. We then randomize the translation of the root state and -set the desired state of the rigid object prim using the :meth:`assets.RigidObject.write_root_link_pose_to_sim` and :meth:`assets.RigidObject.write_root_com_velocity_to_sim` methods. +set the desired state of the rigid object prim using the :meth:`assets.RigidObject.write_root_pose_to_sim` and :meth:`assets.RigidObject.write_root_velocity_to_sim` methods. As the name suggests, this method writes the root state of the rigid object prim into the simulation buffer. .. literalinclude:: ../../../../source/standalone/tutorials/01_assets/run_rigid_object.py diff --git a/docs/source/tutorials/05_controllers/run_diff_ik.rst b/docs/source/tutorials/05_controllers/run_diff_ik.rst index ab451ab8ca..d9ef18e8e8 100644 --- a/docs/source/tutorials/05_controllers/run_diff_ik.rst +++ b/docs/source/tutorials/05_controllers/run_diff_ik.rst @@ -79,7 +79,7 @@ joint positions, current end-effector pose, and the Jacobian matrix. While the attribute :attr:`assets.ArticulationData.joint_pos` provides the joint positions, we only want the joint positions of the robot's arm, and not the gripper. Similarly, while -the attribute :attr:`assets.ArticulationData.body_link_state_w` provides the state of all the +the attribute :attr:`assets.Articulationdata.body_state_w` provides the state of all the robot's bodies, we only want the state of the robot's end-effector. Thus, we need to index into these arrays to obtain the desired quantities. diff --git a/source/extensions/omni.isaac.lab/config/extension.toml b/source/extensions/omni.isaac.lab/config/extension.toml index 21b6f09285..45dabe3745 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.5" +version = "0.30.6" # 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 0e91c4eaf2..f8986245b0 100644 --- a/source/extensions/omni.isaac.lab/docs/CHANGELOG.rst +++ b/source/extensions/omni.isaac.lab/docs/CHANGELOG.rst @@ -1,6 +1,35 @@ Changelog --------- +0.30.6 (2025-01-17) +~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* removed deprecation of :attr:`omni.isaac.lab.assets.ArticulationData.root_state_w` and + :attr:`omni.isaac.lab.assets.ArticulationData.body_state_w` derived properties. +* removed deprecation of :meth:`omni.isaac.lab.assets.Articulation.write_root_state_to_sim`. +* replaced calls to :attr:`omni.isaac.lab.assets.ArticulationData.root_com_state_w` and + :attr:`omni.isaac.lab.assets.ArticulationData.root_link_state_w` with corresponding calls to + :attr:`omni.isaac.lab.assets.ArticulationData.root_state_w`. +* replaced calls to :attr:`omni.isaac.lab.assets.ArticulationData.body_com_state_w` and + :attr:`omni.isaac.lab.assets.ArticulationData.body_link_state_w` properties with corresponding calls to + :attr:`omni.isaac.lab.assets.ArticulationData.body_state_w` properties. +* removed deprecation of :attr:`omni.isaac.lab.assets.RigidObjectData.root_state_w` derived properties . +* removed deprecation of :meth:`omni.isaac.lab.assets.RigidObject.write_root_state_to_sim`. +* replaced calls to :attr:`omni.isaac.lab.assets.RigidObjectData.root_com_state_w` and + :attr:`omni.isaac.lab.assets.RigidObjectData.root_link_state_w` properties with corresponding calls to + :attr:`omni.isaac.lab.assets.RigidObjectData.root_state_w` properties. +* removed deprecation of :attr:`omni.isaac.lab.assets.RigidObjectCollectionData.root_state_w` derived properties. +* removed deprecation of :meth:`omni.isaac.lab.assets.RigidObjectCollection.write_root_state_to_sim`. +* replaced calls to :attr:`omni.isaac.lab.assets.RigidObjectCollectionData.root_com_state_w` and + :attr:`omni.isaac.lab.assets.RigidObjectData.root_link_state_w` properties with corresponding calls to + :attr:`omni.isaac.lab.assets.RigidObjectData.root_state_w` properties. +* fixed indexing issue in ``write_root_link_velocity_to_sim`` in :class:`omni.isaac.lab.assets.RigidObject` +* fixed index broadcasting in ``write_object_link_velocity_to_sim`` and ``write_object_com_pose_to_sim`` in :class:`omni.isaac.lab.assets.RigidObjectCollection` + + 0.30.5 (2025-01-14) ~~~~~~~~~~~~~~~~~~~ diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation.py index 593a8c88a6..f8264bf270 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation.py @@ -98,10 +98,6 @@ def __init__(self, cfg: ArticulationCfg): """ super().__init__(cfg) - self._root_state_dep_warn = False - self._root_pose_dep_warn = False - self._root_vel_dep_warn = False - """ Properties """ @@ -285,14 +281,6 @@ def write_root_state_to_sim(self, root_state: torch.Tensor, env_ids: Sequence[in env_ids: Environment indices. If None, then all indices are used. """ - # deprecation warning - if not self._root_state_dep_warn: - omni.log.warn( - "DeprecationWarning: Articluation.write_root_state_to_sim will be removed in a future release. Please" - " use write_root_link_state_to_sim or write_root_com_state_to_sim instead." - ) - self._root_state_dep_warn = True - # set into simulation self.write_root_pose_to_sim(root_state[:, :7], env_ids=env_ids) self.write_root_velocity_to_sim(root_state[:, 7:], env_ids=env_ids) @@ -334,15 +322,21 @@ def write_root_pose_to_sim(self, root_pose: torch.Tensor, env_ids: Sequence[int] root_pose: Root poses in simulation frame. Shape is (len(env_ids), 7). env_ids: Environment indices. If None, then all indices are used. """ - # deprecation warning - if not self._root_pose_dep_warn: - omni.log.warn( - "DeprecationWarning: Articluation.write_root_pos_to_sim will be removed in a future release. Please use" - " write_root_link_pose_to_sim or write_root_com_pose_to_sim instead." - ) - self._root_pose_dep_warn = True - - self.write_root_link_pose_to_sim(root_pose, env_ids) + # resolve all indices + physx_env_ids = env_ids + if env_ids is None: + env_ids = slice(None) + physx_env_ids = self._ALL_INDICES + # note: we need to do this here since tensors are not set into simulation until step. + # set into internal buffers + self._data.root_state_w[env_ids, :7] = root_pose.clone() + # convert root quaternion from wxyz to xyzw + root_poses_xyzw = self._data.root_state_w[:, :7].clone() + root_poses_xyzw[:, 3:] = math_utils.convert_quat(root_poses_xyzw[:, 3:], to="xyzw") + # Need to invalidate the buffer to trigger the update with the new root pose. + self._data._body_state_w.timestamp = -1.0 + # set into simulation + self.root_physx_view.set_root_transforms(root_poses_xyzw, indices=physx_env_ids) def write_root_link_pose_to_sim(self, root_pose: torch.Tensor, env_ids: Sequence[int] | None = None): """Set the root link pose over selected environment indices into the simulation. @@ -361,9 +355,7 @@ def write_root_link_pose_to_sim(self, root_pose: torch.Tensor, env_ids: Sequence # note: we need to do this here since tensors are not set into simulation until step. # set into internal buffers self._data.root_link_state_w[env_ids, :7] = root_pose.clone() - self._data._ignore_dep_warn = True self._data.root_state_w[env_ids, :7] = self._data.root_link_state_w[env_ids, :7] - self._data._ignore_dep_warn = False # convert root quaternion from wxyz to xyzw root_poses_xyzw = self._data.root_link_state_w[:, :7].clone() root_poses_xyzw[:, 3:] = math_utils.convert_quat(root_poses_xyzw[:, 3:], to="xyzw") @@ -413,15 +405,17 @@ def write_root_velocity_to_sim(self, root_velocity: torch.Tensor, env_ids: Seque root_velocity: Root center of mass velocities in simulation world frame. Shape is (len(env_ids), 6). env_ids: Environment indices. If None, then all indices are used. """ - # deprecation warning - if not self._root_vel_dep_warn: - omni.log.warn( - "DeprecationWarning: Articluation.write_root_velocity_to_sim will be removed in a future release." - " Please use write_root_link_velocity_to_sim or write_root_com_velocity_to_sim instead." - ) - self._root_vel_dep_warn = True - - self.write_root_com_velocity_to_sim(root_velocity=root_velocity, env_ids=env_ids) + # resolve all indices + physx_env_ids = env_ids + if env_ids is None: + env_ids = slice(None) + physx_env_ids = self._ALL_INDICES + # note: we need to do this here since tensors are not set into simulation until step. + # set into internal buffers + self._data.root_state_w[env_ids, 7:] = root_velocity.clone() + self._data.body_acc_w[env_ids] = 0.0 + # set into simulation + self.root_physx_view.set_root_velocities(self._data.root_state_w[:, 7:], indices=physx_env_ids) def write_root_com_velocity_to_sim(self, root_velocity: torch.Tensor, env_ids: Sequence[int] | None = None): """Set the root center of mass velocity over selected environment indices into the simulation. @@ -442,9 +436,7 @@ def write_root_com_velocity_to_sim(self, root_velocity: torch.Tensor, env_ids: S # note: we need to do this here since tensors are not set into simulation until step. # set into internal buffers self._data.root_com_state_w[env_ids, 7:] = root_velocity.clone() - self._data._ignore_dep_warn = True self._data.root_state_w[env_ids, 7:] = self._data.root_com_state_w[env_ids, 7:] - self._data._ignore_dep_warn = False self._data.body_acc_w[env_ids] = 0.0 # set into simulation self.root_physx_view.set_root_velocities(self._data.root_com_state_w[:, 7:], indices=physx_env_ids) @@ -507,8 +499,8 @@ def write_joint_state_to_sim( self._data.joint_acc[env_ids, joint_ids] = 0.0 # Need to invalidate the buffer to trigger the update with the new root pose. self._data._body_state_w.timestamp = -1.0 - self._data._body_link_state_w.timestamp = -1.0 - self._data._body_com_state_w.timestamp = -1.0 + # self._data._body_link_state_w.timestamp = -1.0 + # self._data._body_com_state_w.timestamp = -1.0 # set into simulation self.root_physx_view.set_dof_positions(self._data.joint_pos, indices=physx_env_ids) self.root_physx_view.set_dof_velocities(self._data.joint_vel, indices=physx_env_ids) diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation_data.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation_data.py index a66732c14f..97f66b24bb 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation_data.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation_data.py @@ -6,7 +6,6 @@ import torch import weakref -import omni.log import omni.physics.tensors.impl.api as physx import omni.isaac.lab.utils.math as math_utils @@ -75,11 +74,6 @@ def __init__(self, root_physx_view: physx.ArticulationView, device: str): self._joint_acc = TimestampedBuffer() self._joint_vel = TimestampedBuffer() - # deprecation warning check - self._root_state_dep_warn = False - self._body_state_dep_warn = False - self._ignore_dep_warn = False - def update(self, dt: float): # update the simulation timestamp self._sim_timestamp += dt @@ -274,13 +268,6 @@ def root_state_w(self): the linear and angular velocities are of the articulation root's center of mass frame. """ - if not self._root_state_dep_warn and not self._ignore_dep_warn: - omni.log.warn( - "DeprecationWarning: root_state_w and it's derived properties will be deprecated in a future release." - " Please use root_link_state_w or root_com_state_w." - ) - self._root_state_dep_warn = True - if self._root_state_w.timestamp < self._sim_timestamp: # read data from simulation pose = self._root_physx_view.get_root_transforms().clone() @@ -347,13 +334,6 @@ def body_state_w(self): velocities are of the articulation links's center of mass frame. """ - if not self._body_state_dep_warn and not self._ignore_dep_warn: - omni.log.warn( - "DeprecationWarning: body_state_w and it's derived properties will be deprecated in a future release." - " Please use body_link_state_w or body_com_state_w." - ) - self._body_state_dep_warn = True - if self._body_state_w.timestamp < self._sim_timestamp: self._physics_sim_view.update_articulations_kinematic() # read data from simulation diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/rigid_object/rigid_object.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/rigid_object/rigid_object.py index d66f2d95da..5b2a0aed51 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/rigid_object/rigid_object.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/rigid_object/rigid_object.py @@ -55,9 +55,6 @@ def __init__(self, cfg: RigidObjectCfg): cfg: A configuration instance. """ super().__init__(cfg) - self._root_state_dep_warn = False - self._root_pose_dep_warn = False - self._root_vel_dep_warn = False """ Properties @@ -159,13 +156,6 @@ def write_root_state_to_sim(self, root_state: torch.Tensor, env_ids: Sequence[in root_state: Root state in simulation frame. Shape is (len(env_ids), 13). env_ids: Environment indices. If None, then all indices are used. """ - # deprecation warning - if not self._root_state_dep_warn: - omni.log.warn( - "DeprecationWarning: RigidObject.write_root_state_to_sim will be removed in a future release. Please" - " use write_root_link_state_to_sim or write_root_com_state_to_sim instead." - ) - self._root_state_dep_warn = True # set into simulation self.write_root_pose_to_sim(root_state[:, :7], env_ids=env_ids) @@ -208,15 +198,19 @@ def write_root_pose_to_sim(self, root_pose: torch.Tensor, env_ids: Sequence[int] root_pose: Root poses in simulation frame. Shape is (len(env_ids), 7). env_ids: Environment indices. If None, then all indices are used. """ - # deprecation warning - if not self._root_pose_dep_warn: - omni.log.warn( - "DeprecationWarning: RigidObject.write_root_pose_to_sim will be removed in a future release. Please use" - " write_root_link_pose_to_sim or write_root_com_pose_to_sim instead." - ) - self._root_pose_dep_warn = True - - self.write_root_link_pose_to_sim(root_pose, env_ids) + # resolve all indices + physx_env_ids = env_ids + if env_ids is None: + env_ids = slice(None) + physx_env_ids = self._ALL_INDICES + # note: we need to do this here since tensors are not set into simulation until step. + # set into internal buffers + self._data.root_state_w[env_ids, :7] = root_pose.clone() + # convert root quaternion from wxyz to xyzw + root_poses_xyzw = self._data.root_state_w[:, :7].clone() + root_poses_xyzw[:, 3:] = math_utils.convert_quat(root_poses_xyzw[:, 3:], to="xyzw") + # set into simulation + self.root_physx_view.set_transforms(root_poses_xyzw, indices=physx_env_ids) def write_root_link_pose_to_sim(self, root_pose: torch.Tensor, env_ids: Sequence[int] | None = None): """Set the root link pose over selected environment indices into the simulation. @@ -235,9 +229,7 @@ def write_root_link_pose_to_sim(self, root_pose: torch.Tensor, env_ids: Sequence # note: we need to do this here since tensors are not set into simulation until step. # set into internal buffers self._data.root_link_state_w[env_ids, :7] = root_pose.clone() - self._data._ignore_dep_warn = True self._data.root_state_w[env_ids, :7] = self._data.root_link_state_w[env_ids, :7] - self._data._ignore_dep_warn = False # convert root quaternion from wxyz to xyzw root_poses_xyzw = self._data.root_link_state_w[:, :7].clone() root_poses_xyzw[:, 3:] = math_utils.convert_quat(root_poses_xyzw[:, 3:], to="xyzw") @@ -254,10 +246,11 @@ def write_root_com_pose_to_sim(self, root_pose: torch.Tensor, env_ids: Sequence[ root_pose: Root center of mass poses in simulation frame. Shape is (len(env_ids), 7). env_ids: Environment indices. If None, then all indices are used. """ - # resolve all indices if env_ids is None: - local_env_ids = slice(None) + local_env_ids = slice(env_ids) + else: + local_env_ids = env_ids com_pos = self.data.com_pos_b[local_env_ids, 0, :] com_quat = self.data.com_quat_b[local_env_ids, 0, :] @@ -282,15 +275,17 @@ def write_root_velocity_to_sim(self, root_velocity: torch.Tensor, env_ids: Seque root_velocity: Root center of mass velocities in simulation world frame. Shape is (len(env_ids), 6). env_ids: Environment indices. If None, then all indices are used. """ - # deprecation warning - if not self._root_vel_dep_warn: - omni.log.warn( - "DeprecationWarning: RigidObject.write_root_velocity_to_sim will be removed in a future release. Please" - " use write_root_link_velocity_to_sim or write_root_com_velocity_to_sim instead." - ) - self._root_vel_dep_warn = True - - self.write_root_com_velocity_to_sim(root_velocity=root_velocity, env_ids=env_ids) + # resolve all indices + physx_env_ids = env_ids + if env_ids is None: + env_ids = slice(None) + physx_env_ids = self._ALL_INDICES + # note: we need to do this here since tensors are not set into simulation until step. + # set into internal buffers + self._data.root_state_w[env_ids, 7:] = root_velocity.clone() + self._data.body_acc_w[env_ids] = 0.0 + # set into simulation + self.root_physx_view.set_velocities(self._data.root_state_w[:, 7:], indices=physx_env_ids) def write_root_com_velocity_to_sim(self, root_velocity: torch.Tensor, env_ids: Sequence[int] | None = None): """Set the root center of mass velocity over selected environment indices into the simulation. @@ -311,9 +306,7 @@ def write_root_com_velocity_to_sim(self, root_velocity: torch.Tensor, env_ids: S # note: we need to do this here since tensors are not set into simulation until step. # set into internal buffers self._data.root_com_state_w[env_ids, 7:] = root_velocity.clone() - self._data._ignore_dep_warn = True self._data.root_state_w[env_ids, 7:] = self._data.root_com_state_w[env_ids, 7:] - self._data._ignore_dep_warn = False self._data.body_acc_w[env_ids] = 0.0 # set into simulation self.root_physx_view.set_velocities(self._data.root_com_state_w[:, 7:], indices=physx_env_ids) @@ -330,7 +323,9 @@ def write_root_link_velocity_to_sim(self, root_velocity: torch.Tensor, env_ids: """ # resolve all indices if env_ids is None: - local_env_ids = slice(None) + local_env_ids = slice(env_ids) + else: + local_env_ids = env_ids root_com_velocity = root_velocity.clone() quat = self.data.root_link_state_w[local_env_ids, 3:7] diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/rigid_object/rigid_object_data.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/rigid_object/rigid_object_data.py index 0940672e49..47442a7507 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/rigid_object/rigid_object_data.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/rigid_object/rigid_object_data.py @@ -69,10 +69,6 @@ def __init__(self, root_physx_view: physx.RigidBodyView, device: str): self._root_com_state_w = TimestampedBuffer() self._body_acc_w = TimestampedBuffer() - # deprecation warning check - self._root_state_dep_warn = False - self._ignore_dep_warn = False - def update(self, dt: float): """Updates the data for the rigid object. @@ -121,12 +117,6 @@ def root_state_w(self): The position and orientation are of the rigid body's actor frame. Meanwhile, the linear and angular velocities are of the rigid body's center of mass frame. """ - if not self._root_state_dep_warn and not self._ignore_dep_warn: - omni.log.warn( - "DeprecationWarning: root_state_w and it's derived properties will be deprecated in a future release." - " Please use root_link_state_w or root_com_state_w." - ) - self._root_state_dep_warn = True if self._root_state_w.timestamp < self._sim_timestamp: # read data from simulation diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/rigid_object_collection/rigid_object_collection.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/rigid_object_collection/rigid_object_collection.py index 3d1f30e57c..9bb10d7534 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/rigid_object_collection/rigid_object_collection.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/rigid_object_collection/rigid_object_collection.py @@ -105,10 +105,6 @@ def __init__(self, cfg: RigidObjectCollectionCfg): self._debug_vis_handle = None - self._root_state_dep_warn = False - self._root_pose_dep_warn = False - self._root_vel_dep_warn = False - """ Properties """ @@ -226,13 +222,6 @@ def write_object_state_to_sim( env_ids: Environment indices. If None, then all indices are used. object_ids: Object indices. If None, then all indices are used. """ - # deprecation warning - if not self._root_state_dep_warn: - omni.log.warn( - "DeprecationWarning: RigidObjectCollection.write_object_state_to_sim will be removed in a future" - " release. Please use write_object_link_state_to_sim or write_object_com_state_to_sim instead." - ) - self._root_state_dep_warn = True # set into simulation self.write_object_pose_to_sim(object_state[..., :7], env_ids=env_ids, object_ids=object_ids) @@ -291,15 +280,22 @@ def write_object_pose_to_sim( env_ids: Environment indices. If None, then all indices are used. object_ids: Object indices. If None, then all indices are used. """ - # deprecation warning - if not self._root_pose_dep_warn: - omni.log.warn( - "DeprecationWarning: RigidObjectCollection.write_object_pose_to_sim will be removed in a future" - " release. Please use write_object_link_pose_to_sim or write_object_com_pose_to_sim instead." - ) - self._root_pose_dep_warn = True - - self.write_object_link_pose_to_sim(object_pose, env_ids, object_ids) + # resolve all indices + # -- env_ids + if env_ids is None: + env_ids = self._ALL_ENV_INDICES + # -- object_ids + if object_ids is None: + object_ids = self._ALL_OBJ_INDICES + # note: we need to do this here since tensors are not set into simulation until step. + # set into internal buffers + self._data.object_state_w[env_ids[:, None], object_ids, :7] = object_pose.clone() + # convert the quaternion from wxyz to xyzw + poses_xyzw = self._data.object_state_w[..., :7].clone() + poses_xyzw[..., 3:] = math_utils.convert_quat(poses_xyzw[..., 3:], to="xyzw") + # set into simulation + view_ids = self._env_obj_ids_to_view_ids(env_ids, object_ids) + self.root_physx_view.set_transforms(self.reshape_data_to_view(poses_xyzw), indices=view_ids) def write_object_link_pose_to_sim( self, @@ -327,9 +323,7 @@ def write_object_link_pose_to_sim( # note: we need to do this here since tensors are not set into simulation until step. # set into internal buffers self._data.object_link_state_w[env_ids[:, None], object_ids, :7] = object_pose.clone() - self._data._ignore_dep_warn = True self._data.object_state_w[env_ids[:, None], object_ids, :7] = object_pose.clone() - self._data._ignore_dep_warn = False # convert the quaternion from wxyz to xyzw poses_xyzw = self._data.object_link_state_w[..., :7].clone() poses_xyzw[..., 3:] = math_utils.convert_quat(poses_xyzw[..., 3:], to="xyzw") @@ -362,8 +356,8 @@ def write_object_com_pose_to_sim( else: local_object_ids = object_ids - com_pos = self.data.com_pos_b[local_env_ids, local_object_ids, :] - com_quat = self.data.com_quat_b[local_env_ids, local_object_ids, :] + com_pos = self.data.com_pos_b[local_env_ids][:, local_object_ids, :] + com_quat = self.data.com_quat_b[local_env_ids][:, local_object_ids, :] object_link_pos, object_link_quat = math_utils.combine_frame_transforms( object_pose[..., :3], @@ -388,15 +382,22 @@ def write_object_velocity_to_sim( env_ids: Environment indices. If None, then all indices are used. object_ids: Object indices. If None, then all indices are used. """ - # deprecation warning - if not self._root_vel_dep_warn: - omni.log.warn( - "DeprecationWarning: RigidObjectCollection.write_object_velocity_to_sim will be removed in a future" - " release. Please use write_object_link_velocity_to_sim or write_object_com_velocity_to_sim instead." - ) - self._root_vel_dep_warn = True + # resolve all indices + # -- env_ids + if env_ids is None: + env_ids = self._ALL_ENV_INDICES + # -- object_ids + if object_ids is None: + object_ids = self._ALL_OBJ_INDICES + + self._data.object_state_w[env_ids[:, None], object_ids, 7:] = object_velocity.clone() + self._data.object_acc_w[env_ids[:, None], object_ids] = 0.0 - self.write_object_com_velocity_to_sim(object_velocity=object_velocity, env_ids=env_ids, object_ids=object_ids) + # set into simulation + view_ids = self._env_obj_ids_to_view_ids(env_ids, object_ids) + self.root_physx_view.set_velocities( + self.reshape_data_to_view(self._data.object_state_w[..., 7:]), indices=view_ids + ) def write_object_com_velocity_to_sim( self, @@ -420,9 +421,7 @@ def write_object_com_velocity_to_sim( object_ids = self._ALL_OBJ_INDICES self._data.object_com_state_w[env_ids[:, None], object_ids, 7:] = object_velocity.clone() - self._data._ignore_dep_warn = True self._data.object_state_w[env_ids[:, None], object_ids, 7:] = object_velocity.clone() - self._data._ignore_dep_warn = False self._data.object_acc_w[env_ids[:, None], object_ids] = 0.0 # set into simulation @@ -456,8 +455,8 @@ def write_object_link_velocity_to_sim( local_object_ids = object_ids object_com_velocity = object_velocity.clone() - quat = self.data.object_link_state_w[local_env_ids, local_object_ids, 3:7] - com_pos_b = self.data.com_pos_b[local_env_ids, local_object_ids, :] + quat = self.data.object_link_state_w[local_env_ids][:, local_object_ids, 3:7] + com_pos_b = self.data.com_pos_b[local_env_ids][:, local_object_ids, :] # transform given velocity to center of mass object_com_velocity[..., :3] += torch.linalg.cross( object_com_velocity[..., 3:], math_utils.quat_rotate(quat, com_pos_b), dim=-1 diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/rigid_object_collection/rigid_object_collection_data.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/rigid_object_collection/rigid_object_collection_data.py index b266b70751..fd70452c46 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/rigid_object_collection/rigid_object_collection_data.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/assets/rigid_object_collection/rigid_object_collection_data.py @@ -6,7 +6,6 @@ import torch import weakref -import omni.log import omni.physics.tensors.impl.api as physx import omni.isaac.lab.utils.math as math_utils @@ -74,10 +73,6 @@ def __init__(self, root_physx_view: physx.RigidBodyView, num_objects: int, devic self._object_com_state_w = TimestampedBuffer() self._object_acc_w = TimestampedBuffer() - # deprecation warning check - self._root_state_dep_warn = False - self._ignore_dep_warn = False - def update(self, dt: float): """Updates the data for the rigid object collection. @@ -127,13 +122,6 @@ def object_state_w(self): velocities are of the rigid body's center of mass frame. """ - if not self._root_state_dep_warn and not self._ignore_dep_warn: - omni.log.warn( - "DeprecationWarning: object_state_w and it's derived properties will be deprecated in a future release." - " Please use object_link_state_w or object_com_state_w." - ) - self._root_state_dep_warn = True - if self._object_state_w.timestamp < self._sim_timestamp: # read data from simulation pose = self._reshape_view_to_data(self._root_physx_view.get_transforms().clone()) diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/actions/non_holonomic_actions.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/actions/non_holonomic_actions.py index 52c0bbd047..62eaa43435 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/actions/non_holonomic_actions.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/actions/non_holonomic_actions.py @@ -150,7 +150,7 @@ def process_actions(self, actions): def apply_actions(self): # obtain current heading - quat_w = self._asset.data.body_link_quat_w[:, self._body_idx].view(self.num_envs, 4) + quat_w = self._asset.data.body_quat_w[:, self._body_idx].view(self.num_envs, 4) yaw_w = euler_xyz_from_quat(quat_w)[2] # compute joint velocities targets self._joint_vel_command[:, 0] = torch.cos(yaw_w) * self.processed_actions[:, 0] # x diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/actions/task_space_actions.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/actions/task_space_actions.py index 9a255e7706..282f563d4f 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/actions/task_space_actions.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/actions/task_space_actions.py @@ -142,7 +142,7 @@ def jacobian_w(self) -> torch.Tensor: @property def jacobian_b(self) -> torch.Tensor: jacobian = self.jacobian_w - base_rot = self._asset.data.root_link_quat_w + base_rot = self._asset.data.root_quat_w base_rot_matrix = math_utils.matrix_from_quat(math_utils.quat_inv(base_rot)) jacobian[:, :3, :] = torch.bmm(base_rot_matrix, jacobian[:, :3, :]) jacobian[:, 3:, :] = torch.bmm(base_rot_matrix, jacobian[:, 3:, :]) @@ -192,10 +192,10 @@ def _compute_frame_pose(self) -> tuple[torch.Tensor, torch.Tensor]: A tuple of the body's position and orientation in the root frame. """ # obtain quantities from simulation - ee_pos_w = self._asset.data.body_link_pos_w[:, self._body_idx] - ee_quat_w = self._asset.data.body_link_quat_w[:, self._body_idx] - root_pos_w = self._asset.data.root_link_pos_w - root_quat_w = self._asset.data.root_link_quat_w + ee_pos_w = self._asset.data.body_pos_w[:, self._body_idx] + ee_quat_w = self._asset.data.body_quat_w[:, self._body_idx] + root_pos_w = self._asset.data.root_pos_w + root_quat_w = self._asset.data.root_quat_w # compute the pose of the body in the root frame ee_pose_b, ee_quat_b = math_utils.subtract_frame_transforms(root_pos_w, root_quat_w, ee_pos_w, ee_quat_w) # account for the offset @@ -403,7 +403,7 @@ def jacobian_w(self) -> torch.Tensor: @property def jacobian_b(self) -> torch.Tensor: jacobian = self.jacobian_w - base_rot = self._asset.data.root_link_quat_w + base_rot = self._asset.data.root_quat_w base_rot_matrix = math_utils.matrix_from_quat(math_utils.quat_inv(base_rot)) jacobian[:, :3, :] = torch.bmm(base_rot_matrix, jacobian[:, :3, :]) jacobian[:, 3:, :] = torch.bmm(base_rot_matrix, jacobian[:, 3:, :]) @@ -597,12 +597,12 @@ def _compute_ee_jacobian(self): def _compute_ee_pose(self): """Computes the pose of the ee frame in root frame.""" # Obtain quantities from simulation - self._ee_pose_w[:, 0:3] = self._asset.data.body_link_pos_w[:, self._ee_body_idx] - self._ee_pose_w[:, 3:7] = self._asset.data.body_link_quat_w[:, self._ee_body_idx] + self._ee_pose_w[:, 0:3] = self._asset.data.body_pos_w[:, self._ee_body_idx] + self._ee_pose_w[:, 3:7] = self._asset.data.body_quat_w[:, self._ee_body_idx] # Compute the pose of the ee body in the root frame self._ee_pose_b_no_offset[:, 0:3], self._ee_pose_b_no_offset[:, 3:7] = math_utils.subtract_frame_transforms( - self._asset.data.root_link_pos_w, - self._asset.data.root_link_quat_w, + self._asset.data.root_pos_w, + self._asset.data.root_quat_w, self._ee_pose_w[:, 0:3], self._ee_pose_w[:, 3:7], ) @@ -617,17 +617,13 @@ def _compute_ee_pose(self): def _compute_ee_velocity(self): """Computes the velocity of the ee frame in root frame.""" # Extract end-effector velocity in the world frame - self._ee_vel_w[:] = self._asset.data.body_com_vel_w[:, self._ee_body_idx, :] + self._ee_vel_w[:] = self._asset.data.body_vel_w[:, self._ee_body_idx, :] # Compute the relative velocity in the world frame - relative_vel_w = self._ee_vel_w - self._asset.data.root_com_vel_w + relative_vel_w = self._ee_vel_w - self._asset.data.root_vel_w # Convert ee velocities from world to root frame - self._ee_vel_b[:, 0:3] = math_utils.quat_rotate_inverse( - self._asset.data.root_link_quat_w, relative_vel_w[:, 0:3] - ) - self._ee_vel_b[:, 3:6] = math_utils.quat_rotate_inverse( - self._asset.data.root_link_quat_w, relative_vel_w[:, 3:6] - ) + self._ee_vel_b[:, 0:3] = math_utils.quat_rotate_inverse(self._asset.data.root_quat_w, relative_vel_w[:, 0:3]) + self._ee_vel_b[:, 3:6] = math_utils.quat_rotate_inverse(self._asset.data.root_quat_w, relative_vel_w[:, 3:6]) # Account for the offset if self.cfg.body_offset is not None: @@ -644,7 +640,7 @@ def _compute_ee_force(self): self._contact_sensor.update(self._sim_dt) self._ee_force_w[:] = self._contact_sensor.data.net_forces_w[:, 0, :] # type: ignore # Rotate forces and torques into root frame - self._ee_force_b[:] = math_utils.quat_rotate_inverse(self._asset.data.root_link_quat_w, self._ee_force_w) + self._ee_force_b[:] = math_utils.quat_rotate_inverse(self._asset.data.root_quat_w, self._ee_force_w) def _compute_joint_states(self): """Computes the joint states for operational space control.""" @@ -659,8 +655,8 @@ def _compute_task_frame_pose(self): self._task_frame_transformer.update(self._sim_dt) # Calculate the pose of the task frame in the root frame self._task_frame_pose_b[:, :3], self._task_frame_pose_b[:, 3:] = math_utils.subtract_frame_transforms( - self._asset.data.root_link_pos_w, - self._asset.data.root_link_quat_w, + self._asset.data.root_pos_w, + self._asset.data.root_quat_w, self._task_frame_transformer.data.target_pos_w[:, 0, :], self._task_frame_transformer.data.target_quat_w[:, 0, :], ) diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/commands/pose_2d_command.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/commands/pose_2d_command.py index 7ad927ab3d..5dd90a275e 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/commands/pose_2d_command.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/commands/pose_2d_command.py @@ -81,9 +81,7 @@ def command(self) -> torch.Tensor: def _update_metrics(self): # logs data - self.metrics["error_pos_2d"] = torch.norm( - self.pos_command_w[:, :2] - self.robot.data.root_link_pos_w[:, :2], dim=1 - ) + self.metrics["error_pos_2d"] = torch.norm(self.pos_command_w[:, :2] - self.robot.data.root_pos_w[:, :2], dim=1) self.metrics["error_heading"] = torch.abs(wrap_to_pi(self.heading_command_w - self.robot.data.heading_w)) def _resample_command(self, env_ids: Sequence[int]): @@ -97,7 +95,7 @@ def _resample_command(self, env_ids: Sequence[int]): if self.cfg.simple_heading: # set heading command to point towards target - target_vec = self.pos_command_w[env_ids] - self.robot.data.root_link_pos_w[env_ids] + target_vec = self.pos_command_w[env_ids] - self.robot.data.root_pos_w[env_ids] target_direction = torch.atan2(target_vec[:, 1], target_vec[:, 0]) flipped_target_direction = wrap_to_pi(target_direction + torch.pi) @@ -118,8 +116,8 @@ def _resample_command(self, env_ids: Sequence[int]): def _update_command(self): """Re-target the position command to the current root state.""" - target_vec = self.pos_command_w - self.robot.data.root_link_pos_w[:, :3] - self.pos_command_b[:] = quat_rotate_inverse(yaw_quat(self.robot.data.root_link_quat_w), target_vec) + target_vec = self.pos_command_w - self.robot.data.root_pos_w[:, :3] + self.pos_command_b[:] = quat_rotate_inverse(yaw_quat(self.robot.data.root_quat_w), target_vec) self.heading_command_b[:] = wrap_to_pi(self.heading_command_w - self.robot.data.heading_w) def _set_debug_vis_impl(self, debug_vis: bool): @@ -184,7 +182,7 @@ def _resample_command(self, env_ids: Sequence[int]): if self.cfg.simple_heading: # set heading command to point towards target - target_vec = self.pos_command_w[env_ids] - self.robot.data.root_link_pos_w[env_ids] + target_vec = self.pos_command_w[env_ids] - self.robot.data.root_pos_w[env_ids] target_direction = torch.atan2(target_vec[:, 1], target_vec[:, 0]) flipped_target_direction = wrap_to_pi(target_direction + torch.pi) diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/commands/pose_command.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/commands/pose_command.py index 2df02e3829..ae534c8e1a 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/commands/pose_command.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/commands/pose_command.py @@ -92,8 +92,8 @@ def command(self) -> torch.Tensor: def _update_metrics(self): # transform command from base frame to simulation world frame self.pose_command_w[:, :3], self.pose_command_w[:, 3:] = combine_frame_transforms( - self.robot.data.root_link_pos_w, - self.robot.data.root_link_quat_w, + self.robot.data.root_pos_w, + self.robot.data.root_quat_w, self.pose_command_b[:, :3], self.pose_command_b[:, 3:], ) @@ -101,8 +101,8 @@ def _update_metrics(self): pos_error, rot_error = compute_pose_error( self.pose_command_w[:, :3], self.pose_command_w[:, 3:], - self.robot.data.body_link_state_w[:, self.body_idx, :3], - self.robot.data.body_link_state_w[:, self.body_idx, 3:7], + self.robot.data.body_state_w[:, self.body_idx, :3], + self.robot.data.body_state_w[:, self.body_idx, 3:7], ) self.metrics["position_error"] = torch.norm(pos_error, dim=-1) self.metrics["orientation_error"] = torch.norm(rot_error, dim=-1) @@ -151,5 +151,5 @@ def _debug_vis_callback(self, event): # -- goal pose self.goal_pose_visualizer.visualize(self.pose_command_w[:, :3], self.pose_command_w[:, 3:]) # -- current body pose - body_link_state_w = self.robot.data.body_link_state_w[:, self.body_idx] + body_link_state_w = self.robot.data.body_state_w[:, self.body_idx] self.current_pose_visualizer.visualize(body_link_state_w[:, :3], body_link_state_w[:, 3:7]) diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/commands/velocity_command.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/commands/velocity_command.py index 3d89730d56..b560b15cbd 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/commands/velocity_command.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/commands/velocity_command.py @@ -114,10 +114,10 @@ def _update_metrics(self): max_command_step = max_command_time / self._env.step_dt # logs data self.metrics["error_vel_xy"] += ( - torch.norm(self.vel_command_b[:, :2] - self.robot.data.root_com_lin_vel_b[:, :2], dim=-1) / max_command_step + torch.norm(self.vel_command_b[:, :2] - self.robot.data.root_lin_vel_b[:, :2], dim=-1) / max_command_step ) self.metrics["error_vel_yaw"] += ( - torch.abs(self.vel_command_b[:, 2] - self.robot.data.root_com_ang_vel_b[:, 2]) / max_command_step + torch.abs(self.vel_command_b[:, 2] - self.robot.data.root_ang_vel_b[:, 2]) / max_command_step ) def _resample_command(self, env_ids: Sequence[int]): @@ -184,11 +184,11 @@ def _debug_vis_callback(self, event): return # get marker location # -- base state - base_pos_w = self.robot.data.root_link_pos_w.clone() + base_pos_w = self.robot.data.root_pos_w.clone() base_pos_w[:, 2] += 0.5 # -- resolve the scales and quaternions vel_des_arrow_scale, vel_des_arrow_quat = self._resolve_xy_velocity_to_arrow(self.command[:, :2]) - vel_arrow_scale, vel_arrow_quat = self._resolve_xy_velocity_to_arrow(self.robot.data.root_com_lin_vel_b[:, :2]) + vel_arrow_scale, vel_arrow_quat = self._resolve_xy_velocity_to_arrow(self.robot.data.root_lin_vel_b[:, :2]) # display markers self.goal_vel_visualizer.visualize(base_pos_w, vel_des_arrow_quat, vel_des_arrow_scale) self.current_vel_visualizer.visualize(base_pos_w, vel_arrow_quat, vel_arrow_scale) @@ -209,7 +209,7 @@ def _resolve_xy_velocity_to_arrow(self, xy_velocity: torch.Tensor) -> tuple[torc zeros = torch.zeros_like(heading_angle) arrow_quat = math_utils.quat_from_euler_xyz(zeros, zeros, heading_angle) # convert everything back from base to world frame - base_quat_w = self.robot.data.root_link_quat_w + base_quat_w = self.robot.data.root_quat_w arrow_quat = math_utils.quat_mul(base_quat_w, arrow_quat) return arrow_scale, arrow_quat diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/events.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/events.py index 10146093ce..77af38e323 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/events.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/events.py @@ -624,13 +624,13 @@ def push_by_setting_velocity( asset: RigidObject | Articulation = env.scene[asset_cfg.name] # velocities - vel_w = asset.data.root_com_vel_w[env_ids] + vel_w = asset.data.root_vel_w[env_ids] # sample random velocities range_list = [velocity_range.get(key, (0.0, 0.0)) for key in ["x", "y", "z", "roll", "pitch", "yaw"]] ranges = torch.tensor(range_list, device=asset.device) vel_w += math_utils.sample_uniform(ranges[:, 0], ranges[:, 1], vel_w.shape, device=asset.device) # set the velocities into the physics simulation - asset.write_root_com_velocity_to_sim(vel_w, env_ids=env_ids) + asset.write_root_velocity_to_sim(vel_w, env_ids=env_ids) def reset_root_state_uniform( @@ -674,8 +674,8 @@ def reset_root_state_uniform( velocities = root_states[:, 7:13] + rand_samples # set into the physics simulation - asset.write_root_link_pose_to_sim(torch.cat([positions, orientations], dim=-1), env_ids=env_ids) - asset.write_root_com_velocity_to_sim(velocities, env_ids=env_ids) + asset.write_root_pose_to_sim(torch.cat([positions, orientations], dim=-1), env_ids=env_ids) + asset.write_root_velocity_to_sim(velocities, env_ids=env_ids) def reset_root_state_with_random_orientation( @@ -726,8 +726,8 @@ def reset_root_state_with_random_orientation( velocities = root_states[:, 7:13] + rand_samples # set into the physics simulation - asset.write_root_link_pose_to_sim(torch.cat([positions, orientations], dim=-1), env_ids=env_ids) - asset.write_root_com_velocity_to_sim(velocities, env_ids=env_ids) + asset.write_root_pose_to_sim(torch.cat([positions, orientations], dim=-1), env_ids=env_ids) + asset.write_root_velocity_to_sim(velocities, env_ids=env_ids) def reset_root_state_from_terrain( @@ -793,8 +793,8 @@ def reset_root_state_from_terrain( velocities = asset.data.default_root_state[:, 7:13] + rand_samples # set into the physics simulation - asset.write_root_link_pose_to_sim(torch.cat([positions, orientations], dim=-1), env_ids=env_ids) - asset.write_root_com_velocity_to_sim(velocities, env_ids=env_ids) + asset.write_root_pose_to_sim(torch.cat([positions, orientations], dim=-1), env_ids=env_ids) + asset.write_root_velocity_to_sim(velocities, env_ids=env_ids) def reset_joints_by_scale( @@ -914,16 +914,16 @@ def reset_scene_to_default(env: ManagerBasedEnv, env_ids: torch.Tensor): default_root_state = rigid_object.data.default_root_state[env_ids].clone() default_root_state[:, 0:3] += env.scene.env_origins[env_ids] # set into the physics simulation - rigid_object.write_root_link_pose_to_sim(default_root_state[:, :7], env_ids=env_ids) - rigid_object.write_root_com_velocity_to_sim(default_root_state[:, 7:], env_ids=env_ids) + rigid_object.write_root_pose_to_sim(default_root_state[:, :7], env_ids=env_ids) + rigid_object.write_root_velocity_to_sim(default_root_state[:, 7:], env_ids=env_ids) # articulations for articulation_asset in env.scene.articulations.values(): # obtain default and deal with the offset for env origins default_root_state = articulation_asset.data.default_root_state[env_ids].clone() default_root_state[:, 0:3] += env.scene.env_origins[env_ids] # set into the physics simulation - articulation_asset.write_root_link_pose_to_sim(default_root_state[:, :7], env_ids=env_ids) - articulation_asset.write_root_com_velocity_to_sim(default_root_state[:, 7:], env_ids=env_ids) + articulation_asset.write_root_pose_to_sim(default_root_state[:, :7], env_ids=env_ids) + articulation_asset.write_root_velocity_to_sim(default_root_state[:, 7:], env_ids=env_ids) # obtain default joint positions default_joint_pos = articulation_asset.data.default_joint_pos[env_ids].clone() default_joint_vel = articulation_asset.data.default_joint_vel[env_ids].clone() diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/observations.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/observations.py index 9284eae3c9..db4d6a5bca 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/observations.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/observations.py @@ -34,21 +34,21 @@ def base_pos_z(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg( """Root height in the simulation world frame.""" # extract the used quantities (to enable type-hinting) asset: Articulation = env.scene[asset_cfg.name] - return asset.data.root_link_pos_w[:, 2].unsqueeze(-1) + return asset.data.root_pos_w[:, 2].unsqueeze(-1) def base_lin_vel(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: """Root linear velocity in the asset's root frame.""" # extract the used quantities (to enable type-hinting) asset: RigidObject = env.scene[asset_cfg.name] - return asset.data.root_com_lin_vel_b + return asset.data.root_lin_vel_b def base_ang_vel(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: """Root angular velocity in the asset's root frame.""" # extract the used quantities (to enable type-hinting) asset: RigidObject = env.scene[asset_cfg.name] - return asset.data.root_com_ang_vel_b + return asset.data.root_ang_vel_b def projected_gravity(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: @@ -62,7 +62,7 @@ def root_pos_w(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg( """Asset root position in the environment frame.""" # extract the used quantities (to enable type-hinting) asset: RigidObject = env.scene[asset_cfg.name] - return asset.data.root_link_pos_w - env.scene.env_origins + return asset.data.root_pos_w - env.scene.env_origins def root_quat_w( @@ -77,7 +77,7 @@ def root_quat_w( # extract the used quantities (to enable type-hinting) asset: RigidObject = env.scene[asset_cfg.name] - quat = asset.data.root_link_quat_w + quat = asset.data.root_quat_w # make the quaternion real-part positive if configured return math_utils.quat_unique(quat) if make_quat_unique else quat @@ -86,14 +86,14 @@ def root_lin_vel_w(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntity """Asset root linear velocity in the environment frame.""" # extract the used quantities (to enable type-hinting) asset: RigidObject = env.scene[asset_cfg.name] - return asset.data.root_com_lin_vel_w + return asset.data.root_lin_vel_w def root_ang_vel_w(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: """Asset root angular velocity in the environment frame.""" # extract the used quantities (to enable type-hinting) asset: RigidObject = env.scene[asset_cfg.name] - return asset.data.root_com_ang_vel_w + return asset.data.root_ang_vel_w """ diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/rewards.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/rewards.py index 5930f63404..dab116c7e8 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/rewards.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/rewards.py @@ -77,14 +77,14 @@ def lin_vel_z_l2(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntity """Penalize z-axis base linear velocity using L2 squared kernel.""" # extract the used quantities (to enable type-hinting) asset: RigidObject = env.scene[asset_cfg.name] - return torch.square(asset.data.root_com_lin_vel_b[:, 2]) + return torch.square(asset.data.root_lin_vel_b[:, 2]) def ang_vel_xy_l2(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: """Penalize xy-axis base angular velocity using L2 squared kernel.""" # extract the used quantities (to enable type-hinting) asset: RigidObject = env.scene[asset_cfg.name] - return torch.sum(torch.square(asset.data.root_com_ang_vel_b[:, :2]), dim=1) + return torch.sum(torch.square(asset.data.root_ang_vel_b[:, :2]), dim=1) def flat_orientation_l2(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: @@ -119,7 +119,7 @@ def base_height_l2( # Use the provided target height directly for flat terrain adjusted_target_height = target_height # Compute the L2 squared penalty - return torch.square(asset.data.root_link_pos_w[:, 2] - adjusted_target_height) + return torch.square(asset.data.root_pos_w[:, 2] - adjusted_target_height) def body_lin_acc_l2(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: @@ -292,7 +292,7 @@ def track_lin_vel_xy_exp( asset: RigidObject = env.scene[asset_cfg.name] # compute the error lin_vel_error = torch.sum( - torch.square(env.command_manager.get_command(command_name)[:, :2] - asset.data.root_com_lin_vel_b[:, :2]), + torch.square(env.command_manager.get_command(command_name)[:, :2] - asset.data.root_lin_vel_b[:, :2]), dim=1, ) return torch.exp(-lin_vel_error / std**2) @@ -305,7 +305,5 @@ def track_ang_vel_z_exp( # extract the used quantities (to enable type-hinting) asset: RigidObject = env.scene[asset_cfg.name] # compute the error - ang_vel_error = torch.square( - env.command_manager.get_command(command_name)[:, 2] - asset.data.root_com_ang_vel_b[:, 2] - ) + ang_vel_error = torch.square(env.command_manager.get_command(command_name)[:, 2] - asset.data.root_ang_vel_b[:, 2]) return torch.exp(-ang_vel_error / std**2) diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/terminations.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/terminations.py index 86bc0d0ef4..7b97787344 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/terminations.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/terminations.py @@ -69,7 +69,7 @@ def root_height_below_minimum( """ # extract the used quantities (to enable type-hinting) asset: RigidObject = env.scene[asset_cfg.name] - return asset.data.root_link_pos_w[:, 2] < minimum_height + return asset.data.root_pos_w[:, 2] < minimum_height """ diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/ui/viewport_camera_controller.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/ui/viewport_camera_controller.py index cd97db231d..71d2f67230 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/ui/viewport_camera_controller.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/ui/viewport_camera_controller.py @@ -161,7 +161,7 @@ def update_view_to_asset_root(self, asset_name: str): # set origin type to asset_root self.cfg.origin_type = "asset_root" # update the camera origins - self.viewer_origin = self._env.scene[self.cfg.asset_name].data.root_link_pos_w[self.cfg.env_index] + self.viewer_origin = self._env.scene[self.cfg.asset_name].data.root_pos_w[self.cfg.env_index] # update the camera view self.update_view_location() @@ -194,9 +194,7 @@ def update_view_to_asset_body(self, asset_name: str, body_name: str): # set origin type to asset_body self.cfg.origin_type = "asset_body" # update the camera origins - self.viewer_origin = ( - self._env.scene[self.cfg.asset_name].data.body_link_pos_w[self.cfg.env_index, body_id].view(3) - ) + self.viewer_origin = self._env.scene[self.cfg.asset_name].data.body_pos_w[self.cfg.env_index, body_id].view(3) # update the camera view self.update_view_location() diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/scene/interactive_scene.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/scene/interactive_scene.py index 672dfbc7b0..fe93d8db30 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/scene/interactive_scene.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/scene/interactive_scene.py @@ -364,10 +364,10 @@ def get_state(self, is_relative: bool = False) -> dict[str, dict[str, dict[str, state["articulation"] = dict() for asset_name, articulation in self._articulations.items(): asset_state = dict() - asset_state["root_pose"] = articulation.data.root_link_state_w[:, :7].clone() + asset_state["root_pose"] = articulation.data.root_state_w[:, :7].clone() if is_relative: asset_state["root_pose"][:, :3] -= self.env_origins - asset_state["root_velocity"] = articulation.data.root_com_vel_w.clone() + asset_state["root_velocity"] = articulation.data.root_vel_w.clone() asset_state["joint_position"] = articulation.data.joint_pos.clone() asset_state["joint_velocity"] = articulation.data.joint_vel.clone() state["articulation"][asset_name] = asset_state @@ -384,10 +384,10 @@ def get_state(self, is_relative: bool = False) -> dict[str, dict[str, dict[str, state["rigid_object"] = dict() for asset_name, rigid_object in self._rigid_objects.items(): asset_state = dict() - asset_state["root_pose"] = rigid_object.data.root_link_state_w[:, :7].clone() + asset_state["root_pose"] = rigid_object.data.root_state_w[:, :7].clone() if is_relative: asset_state["root_pose"][:, :3] -= self.env_origins - asset_state["root_velocity"] = rigid_object.data.root_com_vel_w.clone() + asset_state["root_velocity"] = rigid_object.data.root_vel_w.clone() state["rigid_object"][asset_name] = asset_state return state @@ -439,8 +439,8 @@ def reset_to( if is_relative: root_pose[:, :3] += self.env_origins[env_ids] root_velocity = asset_state["root_velocity"].clone() - articulation.write_root_link_pose_to_sim(root_pose, env_ids=env_ids) - articulation.write_root_com_velocity_to_sim(root_velocity, env_ids=env_ids) + articulation.write_root_pose_to_sim(root_pose, env_ids=env_ids) + articulation.write_root_velocity_to_sim(root_velocity, env_ids=env_ids) # joint state joint_position = asset_state["joint_position"].clone() joint_velocity = asset_state["joint_velocity"].clone() @@ -463,8 +463,8 @@ def reset_to( if is_relative: root_pose[:, :3] += self.env_origins[env_ids] root_velocity = asset_state["root_velocity"].clone() - rigid_object.write_root_link_pose_to_sim(root_pose, env_ids=env_ids) - rigid_object.write_root_com_velocity_to_sim(root_velocity, env_ids=env_ids) + rigid_object.write_root_pose_to_sim(root_pose, env_ids=env_ids) + rigid_object.write_root_velocity_to_sim(root_velocity, env_ids=env_ids) self.write_data_to_sim() def write_data_to_sim(self): diff --git a/source/extensions/omni.isaac.lab/test/assets/check_external_force.py b/source/extensions/omni.isaac.lab/test/assets/check_external_force.py index 8c90b907a4..da2e5fae58 100644 --- a/source/extensions/omni.isaac.lab/test/assets/check_external_force.py +++ b/source/extensions/omni.isaac.lab/test/assets/check_external_force.py @@ -99,8 +99,8 @@ def main(): root_state = robot.data.default_root_state.clone() root_state[0, :2] = torch.tensor([0.0, -0.5], device=sim.device) root_state[1, :2] = torch.tensor([0.0, 0.5], device=sim.device) - robot.write_root_link_pose_to_sim(root_state[:, :7]) - robot.write_root_com_velocity_to_sim(root_state[:, 7:]) + robot.write_root_pose_to_sim(root_state[:, :7]) + robot.write_root_velocity_to_sim(root_state[:, 7:]) # reset dof state joint_pos, joint_vel = robot.data.default_joint_pos, robot.data.default_joint_vel robot.write_joint_state_to_sim(joint_pos, joint_vel) diff --git a/source/extensions/omni.isaac.lab/test/assets/check_fixed_base_assets.py b/source/extensions/omni.isaac.lab/test/assets/check_fixed_base_assets.py index afb512c777..5cc3e3172a 100644 --- a/source/extensions/omni.isaac.lab/test/assets/check_fixed_base_assets.py +++ b/source/extensions/omni.isaac.lab/test/assets/check_fixed_base_assets.py @@ -113,8 +113,8 @@ def run_simulator(sim: sim_utils.SimulationContext, entities: dict[str, Articula root_state = robot.data.default_root_state.clone() root_state[:, :3] += origins[index] root_state[:, :2] += torch.randn_like(root_state[:, :2]) * 0.25 - robot.write_root_link_pose_to_sim(root_state[:, :7]) - robot.write_root_com_velocity_to_sim(root_state[:, 7:]) + robot.write_root_pose_to_sim(root_state[:, :7]) + robot.write_root_velocity_to_sim(root_state[:, 7:]) # joint state joint_pos, joint_vel = robot.data.default_joint_pos.clone(), robot.data.default_joint_vel.clone() robot.write_joint_state_to_sim(joint_pos, joint_vel) diff --git a/source/extensions/omni.isaac.lab/test/assets/test_articulation.py b/source/extensions/omni.isaac.lab/test/assets/test_articulation.py index c845a6eb74..84e5cdb538 100644 --- a/source/extensions/omni.isaac.lab/test/assets/test_articulation.py +++ b/source/extensions/omni.isaac.lab/test/assets/test_articulation.py @@ -150,8 +150,8 @@ def test_initialization_floating_base_non_root(self): # Check that is fixed base self.assertFalse(articulation.is_fixed_base) # Check buffers that exists and have correct shapes - self.assertEqual(articulation.data.root_link_pos_w.shape, (num_articulations, 3)) - self.assertEqual(articulation.data.root_link_quat_w.shape, (num_articulations, 4)) + self.assertEqual(articulation.data.root_pos_w.shape, (num_articulations, 3)) + self.assertEqual(articulation.data.root_quat_w.shape, (num_articulations, 4)) self.assertEqual(articulation.data.joint_pos.shape, (num_articulations, 21)) # Check some internal physx data for debugging @@ -199,8 +199,8 @@ def test_initialization_floating_base(self): # Check that floating base self.assertFalse(articulation.is_fixed_base) # Check buffers that exists and have correct shapes - self.assertEqual(articulation.data.root_link_pos_w.shape, (num_articulations, 3)) - self.assertEqual(articulation.data.root_link_quat_w.shape, (num_articulations, 4)) + self.assertEqual(articulation.data.root_pos_w.shape, (num_articulations, 3)) + self.assertEqual(articulation.data.root_quat_w.shape, (num_articulations, 4)) self.assertEqual(articulation.data.joint_pos.shape, (num_articulations, 12)) self.assertEqual( articulation.data.default_mass.shape, (num_articulations, articulation.num_bodies) @@ -252,8 +252,8 @@ def test_initialization_fixed_base(self): # Check that fixed base self.assertTrue(articulation.is_fixed_base) # Check buffers that exists and have correct shapes - self.assertEqual(articulation.data.root_link_pos_w.shape, (num_articulations, 3)) - self.assertEqual(articulation.data.root_link_quat_w.shape, (num_articulations, 4)) + self.assertEqual(articulation.data.root_pos_w.shape, (num_articulations, 3)) + self.assertEqual(articulation.data.root_quat_w.shape, (num_articulations, 4)) self.assertEqual(articulation.data.joint_pos.shape, (num_articulations, 9)) self.assertEqual( articulation.data.default_mass.shape, (num_articulations, articulation.num_bodies) @@ -311,8 +311,8 @@ def test_initialization_fixed_base_single_joint(self): # Check that fixed base self.assertTrue(articulation.is_fixed_base) # Check buffers that exists and have correct shapes - self.assertEqual(articulation.data.root_link_pos_w.shape, (num_articulations, 3)) - self.assertEqual(articulation.data.root_link_quat_w.shape, (num_articulations, 4)) + self.assertEqual(articulation.data.root_pos_w.shape, (num_articulations, 3)) + self.assertEqual(articulation.data.root_quat_w.shape, (num_articulations, 4)) self.assertEqual(articulation.data.joint_pos.shape, (num_articulations, 1)) self.assertEqual( articulation.data.default_mass.shape, (num_articulations, articulation.num_bodies) @@ -370,8 +370,8 @@ def test_initialization_hand_with_tendons(self): # Check that fixed base self.assertTrue(articulation.is_fixed_base) # Check buffers that exists and have correct shapes - self.assertTrue(articulation.data.root_link_pos_w.shape == (num_articulations, 3)) - self.assertTrue(articulation.data.root_link_quat_w.shape == (num_articulations, 4)) + self.assertTrue(articulation.data.root_pos_w.shape == (num_articulations, 3)) + self.assertTrue(articulation.data.root_quat_w.shape == (num_articulations, 4)) self.assertTrue(articulation.data.joint_pos.shape == (num_articulations, 24)) self.assertEqual( articulation.data.default_mass.shape, (num_articulations, articulation.num_bodies) @@ -420,8 +420,8 @@ def test_initialization_floating_base_made_fixed_base(self): # Check that is fixed base self.assertTrue(articulation.is_fixed_base) # Check buffers that exists and have correct shapes - self.assertEqual(articulation.data.root_link_pos_w.shape, (num_articulations, 3)) - self.assertEqual(articulation.data.root_link_quat_w.shape, (num_articulations, 4)) + self.assertEqual(articulation.data.root_pos_w.shape, (num_articulations, 3)) + self.assertEqual(articulation.data.root_quat_w.shape, (num_articulations, 4)) self.assertEqual(articulation.data.joint_pos.shape, (num_articulations, 12)) # Check some internal physx data for debugging @@ -475,8 +475,8 @@ def test_initialization_fixed_base_made_floating_base(self): # Check that is floating base self.assertFalse(articulation.is_fixed_base) # Check buffers that exists and have correct shapes - self.assertEqual(articulation.data.root_link_pos_w.shape, (num_articulations, 3)) - self.assertEqual(articulation.data.root_link_quat_w.shape, (num_articulations, 4)) + self.assertEqual(articulation.data.root_pos_w.shape, (num_articulations, 3)) + self.assertEqual(articulation.data.root_quat_w.shape, (num_articulations, 4)) self.assertEqual(articulation.data.joint_pos.shape, (num_articulations, 9)) # Check some internal physx data for debugging @@ -633,8 +633,8 @@ def test_external_force_on_single_body(self): # reset root state root_state = articulation.data.default_root_state.clone() - articulation.write_root_link_pose_to_sim(root_state[:, :7]) - articulation.write_root_com_velocity_to_sim(root_state[:, 7:]) + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_root_velocity_to_sim(root_state[:, 7:]) # reset dof state joint_pos, joint_vel = ( articulation.data.default_joint_pos, @@ -658,7 +658,7 @@ def test_external_force_on_single_body(self): articulation.update(sim.cfg.dt) # check condition that the articulations have fallen down for i in range(num_articulations): - self.assertLess(articulation.data.root_link_pos_w[i, 2].item(), 0.2) + self.assertLess(articulation.data.root_pos_w[i, 2].item(), 0.2) def test_external_force_on_multiple_bodies(self): """Test application of external force on the legs of the articulation.""" @@ -681,12 +681,8 @@ def test_external_force_on_multiple_bodies(self): # Now we are ready! for _ in range(5): # reset root state - articulation.write_root_link_pose_to_sim( - articulation.data.default_root_state.clone()[:, :7] - ) - articulation.write_root_com_velocity_to_sim( - articulation.data.default_root_state.clone()[:, 7:] - ) + articulation.write_root_pose_to_sim(articulation.data.default_root_state.clone()[:, :7]) + articulation.write_root_velocity_to_sim(articulation.data.default_root_state.clone()[:, 7:]) # reset dof state joint_pos, joint_vel = ( articulation.data.default_joint_pos, @@ -711,7 +707,7 @@ def test_external_force_on_multiple_bodies(self): # check condition for i in range(num_articulations): # since there is a moment applied on the articulation, the articulation should rotate - self.assertTrue(articulation.data.root_com_ang_vel_w[i, 2].item() > 0.1) + self.assertTrue(articulation.data.root_ang_vel_w[i, 2].item() > 0.1) def test_loading_gains_from_usd(self): """Test that gains are loaded from USD file if actuator model has them as None.""" @@ -929,8 +925,8 @@ def test_apply_joint_command(self): # are not properly tuned assert not torch.allclose(articulation.data.joint_pos, joint_pos) - def test_body_root_link_state(self): - """Test for the root_link_state_w property""" + def test_body_root_state(self): + """Test for the root_state_w property""" for num_articulations in (1, 2): # for num_articulations in ( 2,): for device in ("cuda:0", "cpu"): @@ -1082,7 +1078,7 @@ def test_write_root_state(self): # check they are set torch.testing.assert_close(articulation.root_physx_view.get_coms(), com) - rand_state = torch.zeros_like(articulation.data.root_link_state_w) + rand_state = torch.zeros_like(articulation.data.root_state_w) rand_state[..., :7] = articulation.data.default_root_state[..., :7] rand_state[..., :3] += env_pos # make quaternion a unit vector diff --git a/source/extensions/omni.isaac.lab/test/assets/test_rigid_object.py b/source/extensions/omni.isaac.lab/test/assets/test_rigid_object.py index e35b1ca12b..f6b3cedf96 100644 --- a/source/extensions/omni.isaac.lab/test/assets/test_rigid_object.py +++ b/source/extensions/omni.isaac.lab/test/assets/test_rigid_object.py @@ -119,8 +119,8 @@ def test_initialization(self): self.assertEqual(len(cube_object.body_names), 1) # Check buffers that exists and have correct shapes - self.assertEqual(cube_object.data.root_link_pos_w.shape, (num_cubes, 3)) - self.assertEqual(cube_object.data.root_link_quat_w.shape, (num_cubes, 4)) + self.assertEqual(cube_object.data.root_pos_w.shape, (num_cubes, 3)) + self.assertEqual(cube_object.data.root_quat_w.shape, (num_cubes, 4)) self.assertEqual(cube_object.data.default_mass.shape, (num_cubes, 1)) self.assertEqual(cube_object.data.default_inertia.shape, (num_cubes, 9)) @@ -153,8 +153,8 @@ def test_initialization_with_kinematic_enabled(self): self.assertEqual(len(cube_object.body_names), 1) # Check buffers that exists and have correct shapes - self.assertEqual(cube_object.data.root_link_pos_w.shape, (num_cubes, 3)) - self.assertEqual(cube_object.data.root_link_quat_w.shape, (num_cubes, 4)) + self.assertEqual(cube_object.data.root_pos_w.shape, (num_cubes, 3)) + self.assertEqual(cube_object.data.root_quat_w.shape, (num_cubes, 4)) # Simulate physics for _ in range(2): @@ -237,8 +237,8 @@ def test_external_force_on_single_body(self): # need to shift the position of the cubes otherwise they will be on top of each other root_state[:, :3] = origins - cube_object.write_root_link_pose_to_sim(root_state[:, :7]) - cube_object.write_root_com_velocity_to_sim(root_state[:, 7:]) + cube_object.write_root_pose_to_sim(root_state[:, :7]) + cube_object.write_root_velocity_to_sim(root_state[:, 7:]) # reset object cube_object.reset() @@ -260,10 +260,10 @@ def test_external_force_on_single_body(self): # First object should still be at the same Z position (1.0) torch.testing.assert_close( - cube_object.data.root_link_pos_w[0::2, 2], torch.ones(num_cubes // 2, device=sim.device) + cube_object.data.root_pos_w[0::2, 2], torch.ones(num_cubes // 2, device=sim.device) ) # Second object should have fallen, so it's Z height should be less than initial height of 1.0 - self.assertTrue(torch.all(cube_object.data.root_link_pos_w[1::2, 2] < 1.0)) + self.assertTrue(torch.all(cube_object.data.root_pos_w[1::2, 2] < 1.0)) def test_set_rigid_object_state(self): """Test setting the state of the rigid object. @@ -289,14 +289,10 @@ def test_set_rigid_object_state(self): # Set each state type individually as they are dependent on each other for state_type_to_randomize in state_types: state_dict = { - "root_pos_w": torch.zeros_like(cube_object.data.root_link_pos_w, device=sim.device), + "root_pos_w": torch.zeros_like(cube_object.data.root_pos_w, device=sim.device), "root_quat_w": default_orientation(num=num_cubes, device=sim.device), - "root_lin_vel_w": torch.zeros_like( - cube_object.data.root_com_lin_vel_w, device=sim.device - ), - "root_ang_vel_w": torch.zeros_like( - cube_object.data.root_com_ang_vel_w, device=sim.device - ), + "root_lin_vel_w": torch.zeros_like(cube_object.data.root_lin_vel_w, device=sim.device), + "root_ang_vel_w": torch.zeros_like(cube_object.data.root_ang_vel_w, device=sim.device), } # Now we are ready! @@ -324,8 +320,8 @@ def test_set_rigid_object_state(self): dim=-1, ) # reset root state - cube_object.write_root_link_pose_to_sim(root_state[:, :7]) - cube_object.write_root_com_velocity_to_sim(root_state[:, 7:]) + cube_object.write_root_pose_to_sim(root_state[:, :7]) + cube_object.write_root_velocity_to_sim(root_state[:, 7:]) sim.step() @@ -361,8 +357,8 @@ def test_reset_rigid_object(self): # Random orientation root_state[:, 3:7] = random_orientation(num=num_cubes, device=sim.device) - cube_object.write_root_link_pose_to_sim(root_state[:, :7]) - cube_object.write_root_com_velocity_to_sim(root_state[:, 7:]) + cube_object.write_root_pose_to_sim(root_state[:, :7]) + cube_object.write_root_velocity_to_sim(root_state[:, 7:]) if i % 2 == 0: # reset object @@ -447,7 +443,7 @@ def test_rigid_body_no_friction(self): initial_velocity = torch.zeros((num_cubes, 6), device=sim.cfg.device) initial_velocity[:, 0] = 0.1 - cube_object.write_root_com_velocity_to_sim(initial_velocity) + cube_object.write_root_velocity_to_sim(initial_velocity) # Simulate physics for _ in range(5): @@ -512,7 +508,7 @@ def test_rigid_body_with_static_friction(self): for _ in range(100): sim.step() cube_object.update(sim.cfg.dt) - cube_object.write_root_com_velocity_to_sim(torch.zeros((num_cubes, 6), device=sim.device)) + cube_object.write_root_velocity_to_sim(torch.zeros((num_cubes, 6), device=sim.device)) cube_mass = cube_object.root_physx_view.get_masses() gravity_magnitude = abs(sim.cfg.gravity[2]) # 2 cases: force applied is below and above mu @@ -521,9 +517,7 @@ def test_rigid_body_with_static_friction(self): for force in "below_mu", "above_mu": with self.subTest(force=force): # set initial velocity to zero - cube_object.write_root_com_velocity_to_sim( - torch.zeros((num_cubes, 6), device=sim.device) - ) + cube_object.write_root_velocity_to_sim(torch.zeros((num_cubes, 6), device=sim.device)) external_wrench_b = torch.zeros((num_cubes, 1, 6), device=sim.device) if force == "below_mu": @@ -541,7 +535,7 @@ def test_rigid_body_with_static_friction(self): ) # Get root state - initial_root_pos = cube_object.data.root_link_pos_w.clone() + initial_root_pos = cube_object.data.root_pos_w.clone() # Simulate physics for _ in range(200): # apply the wrench @@ -552,7 +546,7 @@ def test_rigid_body_with_static_friction(self): if force == "below_mu": # Assert that the block has not moved torch.testing.assert_close( - cube_object.data.root_link_pos_w, initial_root_pos, rtol=1e-3, atol=1e-3 + cube_object.data.root_pos_w, initial_root_pos, rtol=1e-3, atol=1e-3 ) if force == "above_mu": self.assertTrue( @@ -604,8 +598,8 @@ def test_rigid_body_with_restitution(self): root_state[:, 2] = 1.0 # Set an initial drop height root_state[:, 9] = -1.0 # Set an initial downward velocity - cube_object.write_root_link_pose_to_sim(root_state[:, :7]) - cube_object.write_root_com_velocity_to_sim(root_state[:, 7:]) + cube_object.write_root_pose_to_sim(root_state[:, :7]) + cube_object.write_root_velocity_to_sim(root_state[:, 7:]) static_friction = torch.zeros(num_cubes, 1) dynamic_friction = torch.zeros(num_cubes, 1) @@ -616,14 +610,14 @@ def test_rigid_body_with_restitution(self): # Add restitution to cube cube_object.root_physx_view.set_material_properties(cube_object_materials, indices) - curr_z_velocity = cube_object.data.root_com_lin_vel_w[:, 2].clone() + curr_z_velocity = cube_object.data.root_lin_vel_w[:, 2].clone() for _ in range(100): sim.step() # update object cube_object.update(sim.cfg.dt) - curr_z_velocity = cube_object.data.root_com_lin_vel_w[:, 2].clone() + curr_z_velocity = cube_object.data.root_lin_vel_w[:, 2].clone() if expected_collision_type == "inelastic": # assert that the block has not bounced by checking that the z velocity is less than or equal to 0 @@ -758,7 +752,7 @@ def test_body_root_state_properties(self): # Simulate physics for _ in range(100): # spin the object around Z axis (com) - cube_object.write_root_com_velocity_to_sim(spin_twist.repeat(num_cubes, 1)) + cube_object.write_root_velocity_to_sim(spin_twist.repeat(num_cubes, 1)) # perform rendering sim.step() # update object @@ -872,12 +866,13 @@ def test_write_root_state(self): # check ceter of mass has been set torch.testing.assert_close(cube_object.root_physx_view.get_coms(), com) - rand_state = torch.zeros_like(cube_object.data.root_link_state_w) + rand_state = torch.zeros_like(cube_object.data.root_state_w) rand_state[..., :7] = cube_object.data.default_root_state[..., :7] rand_state[..., :3] += env_pos # make quaternion a unit vector rand_state[..., 3:7] = torch.nn.functional.normalize(rand_state[..., 3:7], dim=-1) + env_idx = env_idx.to(device) for i in range(10): # perform step @@ -886,9 +881,15 @@ def test_write_root_state(self): cube_object.update(sim.cfg.dt) if state_location == "com": - cube_object.write_root_com_state_to_sim(rand_state) + if i % 2 == 0: + cube_object.write_root_com_state_to_sim(rand_state) + else: + cube_object.write_root_com_state_to_sim(rand_state, env_ids=env_idx) elif state_location == "link": - cube_object.write_root_link_state_to_sim(rand_state) + if i % 2 == 0: + cube_object.write_root_link_state_to_sim(rand_state) + else: + cube_object.write_root_link_state_to_sim(rand_state, env_ids=env_idx) if state_location == "com": torch.testing.assert_close(rand_state, cube_object.data.root_com_state_w) diff --git a/source/extensions/omni.isaac.lab/test/assets/test_rigid_object_collection.py b/source/extensions/omni.isaac.lab/test/assets/test_rigid_object_collection.py index ee5d61dcdf..25298e42c6 100644 --- a/source/extensions/omni.isaac.lab/test/assets/test_rigid_object_collection.py +++ b/source/extensions/omni.isaac.lab/test/assets/test_rigid_object_collection.py @@ -513,6 +513,8 @@ def test_write_object_state(self): num_envs=num_envs, num_cubes=num_cubes, height=0.0, device=device ) view_ids = torch.tensor([x for x in range(num_cubes * num_cubes)]) + env_ids = torch.tensor([x for x in range(num_envs)]) + object_ids = torch.tensor([x for x in range(num_cubes)]) # Play sim sim.reset() @@ -547,6 +549,8 @@ def test_write_object_state(self): # make quaternion a unit vector rand_state[..., 3:7] = torch.nn.functional.normalize(rand_state[..., 3:7], dim=-1) + env_ids = env_ids.to(device) + object_ids = object_ids.to(device) for i in range(10): # perform step @@ -555,9 +559,19 @@ def test_write_object_state(self): cube_object.update(sim.cfg.dt) if state_location == "com": - cube_object.write_object_com_state_to_sim(rand_state) + if i % 2 == 0: + cube_object.write_object_com_state_to_sim(rand_state) + else: + cube_object.write_object_com_state_to_sim( + rand_state, env_ids=env_ids, object_ids=object_ids + ) elif state_location == "link": - cube_object.write_object_link_state_to_sim(rand_state) + if i % 2 == 0: + cube_object.write_object_link_state_to_sim(rand_state) + else: + cube_object.write_object_link_state_to_sim( + rand_state, env_ids=env_ids, object_ids=object_ids + ) if state_location == "com": torch.testing.assert_close(rand_state, cube_object.data.object_com_state_w) diff --git a/source/extensions/omni.isaac.lab/test/controllers/test_differential_ik.py b/source/extensions/omni.isaac.lab/test/controllers/test_differential_ik.py index 089216a6a0..27e4c49747 100644 --- a/source/extensions/omni.isaac.lab/test/controllers/test_differential_ik.py +++ b/source/extensions/omni.isaac.lab/test/controllers/test_differential_ik.py @@ -147,8 +147,8 @@ def _run_ik_controller( ee_pose_b_des = torch.zeros(self.num_envs, diff_ik_controller.action_dim, device=self.sim.device) ee_pose_b_des[:] = self.ee_pose_b_des_set[current_goal_idx] # Compute current pose of the end-effector - ee_pose_w = robot.data.body_link_state_w[:, ee_frame_idx, 0:7] - root_pose_w = robot.data.root_link_state_w[:, 0:7] + ee_pose_w = robot.data.body_state_w[:, ee_frame_idx, 0:7] + root_pose_w = robot.data.root_state_w[:, 0:7] ee_pos_b, ee_quat_b = subtract_frame_transforms( root_pose_w[:, 0:3], root_pose_w[:, 3:7], ee_pose_w[:, 0:3], ee_pose_w[:, 3:7] ) @@ -177,10 +177,10 @@ def _run_ik_controller( robot.set_joint_position_target(joint_pos) robot.write_data_to_sim() # randomize root state yaw, ik should work regardless base rotation - root_state = robot.data.root_link_state_w.clone() + root_state = robot.data.root_state_w.clone() root_state[:, 3:7] = random_yaw_orientation(self.num_envs, self.sim.device) - robot.write_root_link_pose_to_sim(root_state[:, :7]) - robot.write_root_com_velocity_to_sim(root_state[:, 7:]) + robot.write_root_pose_to_sim(root_state[:, :7]) + robot.write_root_velocity_to_sim(root_state[:, 7:]) robot.reset() # reset actions ee_pose_b_des[:] = self.ee_pose_b_des_set[current_goal_idx] @@ -195,8 +195,8 @@ def _run_ik_controller( # so we MUST skip the first step # obtain quantities from simulation jacobian = robot.root_physx_view.get_jacobians()[:, ee_jacobi_idx, :, arm_joint_ids] - ee_pose_w = robot.data.body_link_state_w[:, ee_frame_idx, 0:7] - root_pose_w = robot.data.root_link_state_w[:, 0:7] + ee_pose_w = robot.data.body_state_w[:, ee_frame_idx, 0:7] + root_pose_w = robot.data.root_state_w[:, 0:7] base_rot = root_pose_w[:, 3:7] base_rot_matrix = matrix_from_quat(quat_inv(base_rot)) jacobian[:, :3, :] = torch.bmm(base_rot_matrix, jacobian[:, :3, :]) diff --git a/source/extensions/omni.isaac.lab/test/controllers/test_operational_space.py b/source/extensions/omni.isaac.lab/test/controllers/test_operational_space.py index 78d42244f5..c582a5ac5a 100644 --- a/source/extensions/omni.isaac.lab/test/controllers/test_operational_space.py +++ b/source/extensions/omni.isaac.lab/test/controllers/test_operational_space.py @@ -843,26 +843,24 @@ def _update_states( gravity = robot.root_physx_view.get_generalized_gravity_forces()[:, arm_joint_ids] # Convert the Jacobian from world to root frame jacobian_b = jacobian_w.clone() - root_rot_matrix = matrix_from_quat(quat_inv(robot.data.root_link_quat_w)) + root_rot_matrix = matrix_from_quat(quat_inv(robot.data.root_quat_w)) jacobian_b[:, :3, :] = torch.bmm(root_rot_matrix, jacobian_b[:, :3, :]) jacobian_b[:, 3:, :] = torch.bmm(root_rot_matrix, jacobian_b[:, 3:, :]) # Compute current pose of the end-effector - root_pose_w = robot.data.root_link_state_w[:, 0:7] - ee_pose_w = robot.data.body_link_state_w[:, ee_frame_idx, 0:7] + root_pose_w = robot.data.root_state_w[:, 0:7] + ee_pose_w = robot.data.body_state_w[:, ee_frame_idx, 0:7] ee_pos_b, ee_quat_b = subtract_frame_transforms( root_pose_w[:, 0:3], root_pose_w[:, 3:7], ee_pose_w[:, 0:3], ee_pose_w[:, 3:7] ) ee_pose_b = torch.cat([ee_pos_b, ee_quat_b], dim=-1) # Compute the current velocity of the end-effector - ee_vel_w = robot.data.body_com_vel_w[:, ee_frame_idx, :] # Extract end-effector velocity in the world frame - root_vel_w = robot.data.root_com_vel_w # Extract root velocity in the world frame + ee_vel_w = robot.data.body_vel_w[:, ee_frame_idx, :] # Extract end-effector velocity in the world frame + root_vel_w = robot.data.root_vel_w # Extract root velocity in the world frame relative_vel_w = ee_vel_w - root_vel_w # Compute the relative velocity in the world frame - ee_lin_vel_b = quat_rotate_inverse( - robot.data.root_link_quat_w, relative_vel_w[:, 0:3] - ) # From world to root frame - ee_ang_vel_b = quat_rotate_inverse(robot.data.root_link_quat_w, relative_vel_w[:, 3:6]) + ee_lin_vel_b = quat_rotate_inverse(robot.data.root_quat_w, relative_vel_w[:, 0:3]) # From world to root frame + ee_ang_vel_b = quat_rotate_inverse(robot.data.root_quat_w, relative_vel_w[:, 3:6]) ee_vel_b = torch.cat([ee_lin_vel_b, ee_ang_vel_b], dim=-1) # Calculate the contact force diff --git a/source/extensions/omni.isaac.lab/test/envs/check_manager_based_env_floating_cube.py b/source/extensions/omni.isaac.lab/test/envs/check_manager_based_env_floating_cube.py index f3071e4c1c..4834d1f8f3 100644 --- a/source/extensions/omni.isaac.lab/test/envs/check_manager_based_env_floating_cube.py +++ b/source/extensions/omni.isaac.lab/test/envs/check_manager_based_env_floating_cube.py @@ -128,11 +128,11 @@ def process_actions(self, actions: torch.Tensor): def apply_actions(self): # implement a PD controller to track the target position - pos_error = self._processed_actions - (self._asset.data.root_link_pos_w - self._env.scene.env_origins) - vel_error = -self._asset.data.root_com_lin_vel_w + pos_error = self._processed_actions - (self._asset.data.root_pos_w - self._env.scene.env_origins) + vel_error = -self._asset.data.root_lin_vel_w # set velocity targets self._vel_command[:, :3] = self.p_gain * pos_error + self.d_gain * vel_error - self._asset.write_root_com_velocity_to_sim(self._vel_command) + self._asset.write_root_velocity_to_sim(self._vel_command) @configclass @@ -151,7 +151,7 @@ def base_position(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg) -> torch.Tens """Root linear velocity in the asset's root frame.""" # extract the used quantities (to enable type-hinting) asset: RigidObject = env.scene[asset_cfg.name] - return asset.data.root_link_pos_w - env.scene.env_origins + return asset.data.root_pos_w - env.scene.env_origins ## diff --git a/source/extensions/omni.isaac.lab/test/markers/check_markers_visibility.py b/source/extensions/omni.isaac.lab/test/markers/check_markers_visibility.py index 2a28934818..aa326e08e2 100644 --- a/source/extensions/omni.isaac.lab/test/markers/check_markers_visibility.py +++ b/source/extensions/omni.isaac.lab/test/markers/check_markers_visibility.py @@ -98,8 +98,8 @@ def run_simulator( # root state root_state = scene["robot"].data.default_root_state.clone() root_state[:, :3] += scene.env_origins - scene["robot"].write_root_link_pose_to_sim(root_state[:, :7]) - scene["robot"].write_root_com_velocity_to_sim(root_state[:, 7:]) + scene["robot"].write_root_pose_to_sim(root_state[:, :7]) + scene["robot"].write_root_velocity_to_sim(root_state[:, 7:]) # set joint positions with some noise joint_pos, joint_vel = ( scene["robot"].data.default_joint_pos.clone(), diff --git a/source/extensions/omni.isaac.lab/test/scene/check_interactive_scene.py b/source/extensions/omni.isaac.lab/test/scene/check_interactive_scene.py index 0772f7bdf3..847a7ef741 100644 --- a/source/extensions/omni.isaac.lab/test/scene/check_interactive_scene.py +++ b/source/extensions/omni.isaac.lab/test/scene/check_interactive_scene.py @@ -130,13 +130,13 @@ def main(): joint_vel = scene.articulations["robot_1"].data.default_joint_vel # -- set root state # -- robot 1 - scene.articulations["robot_1"].write_root_link_pose_to_sim(root_state[:, :7]) - scene.articulations["robot_1"].write_root_com_velocity_to_sim(root_state[:, 7:]) + scene.articulations["robot_1"].write_root_pose_to_sim(root_state[:, :7]) + scene.articulations["robot_1"].write_root_velocity_to_sim(root_state[:, 7:]) scene.articulations["robot_1"].write_joint_state_to_sim(joint_pos, joint_vel) # -- robot 2 root_state[:, 1] += 1.0 - scene.articulations["robot_2"].write_root_link_pose_to_sim(root_state[:, :7]) - scene.articulations["robot_2"].write_root_com_velocity_to_sim(root_state[:, 7:]) + scene.articulations["robot_2"].write_root_pose_to_sim(root_state[:, :7]) + scene.articulations["robot_2"].write_root_velocity_to_sim(root_state[:, 7:]) scene.articulations["robot_2"].write_joint_state_to_sim(joint_pos, joint_vel) # reset buffers scene.reset() diff --git a/source/extensions/omni.isaac.lab/test/sensors/check_imu_sensor.py b/source/extensions/omni.isaac.lab/test/sensors/check_imu_sensor.py index ca34ae9331..f165919c82 100644 --- a/source/extensions/omni.isaac.lab/test/sensors/check_imu_sensor.py +++ b/source/extensions/omni.isaac.lab/test/sensors/check_imu_sensor.py @@ -151,8 +151,8 @@ def main(): # Get the ball initial positions sim.step(render=not args_cli.headless) balls.update(sim.get_physics_dt()) - ball_initial_positions = balls.data.root_link_pos_w.clone() - ball_initial_orientations = balls.data.root_link_quat_w.clone() + ball_initial_positions = balls.data.root_pos_w.clone() + ball_initial_orientations = balls.data.root_quat_w.clone() # Create a counter for resetting the scene step_count = 0 @@ -168,7 +168,7 @@ def main(): # Reset the scene if step_count % 500 == 0: # reset ball positions - balls.write_root_link_pose_to_sim(torch.cat([ball_initial_positions, ball_initial_orientations], dim=-1)) + balls.write_root_pose_to_sim(torch.cat([ball_initial_positions, ball_initial_orientations], dim=-1)) balls.reset() # reset the sensor imu.reset() diff --git a/source/extensions/omni.isaac.lab/test/sensors/test_contact_sensor.py b/source/extensions/omni.isaac.lab/test/sensors/test_contact_sensor.py index 1dda3117d9..029e6313e4 100644 --- a/source/extensions/omni.isaac.lab/test/sensors/test_contact_sensor.py +++ b/source/extensions/omni.isaac.lab/test/sensors/test_contact_sensor.py @@ -405,7 +405,7 @@ def _test_sensor_contact(self, shape: RigidObject, sensor: ContactSensor, mode: duration = self.durations[idx] while current_test_time < duration: # set object states to contact the ground plane - shape.write_root_link_pose_to_sim(root_pose=test_pose) + shape.write_root_pose_to_sim(root_pose=test_pose) # perform simulation step self._perform_sim_step() # increment contact time @@ -432,7 +432,7 @@ def _test_sensor_contact(self, shape: RigidObject, sensor: ContactSensor, mode: dt=duration + self.sim_dt, ) # switch the contact mode for 1 dt step before the next contact test begins. - shape.write_root_link_pose_to_sim(root_pose=reset_pose) + shape.write_root_pose_to_sim(root_pose=reset_pose) # perform simulation step self._perform_sim_step() # set the last air time to 2 sim_dt steps, because last_air_time and last_contact_time diff --git a/source/extensions/omni.isaac.lab/test/sensors/test_frame_transformer.py b/source/extensions/omni.isaac.lab/test/sensors/test_frame_transformer.py index 77433edbf9..81f2aad99f 100644 --- a/source/extensions/omni.isaac.lab/test/sensors/test_frame_transformer.py +++ b/source/extensions/omni.isaac.lab/test/sensors/test_frame_transformer.py @@ -175,8 +175,8 @@ def test_frame_transformer_feet_wrt_base(self): joint_vel = scene.articulations["robot"].data.default_joint_vel # -- set root state # -- robot - scene.articulations["robot"].write_root_link_pose_to_sim(root_state[:, :7]) - scene.articulations["robot"].write_root_com_velocity_to_sim(root_state[:, 7:]) + scene.articulations["robot"].write_root_pose_to_sim(root_state[:, :7]) + scene.articulations["robot"].write_root_velocity_to_sim(root_state[:, 7:]) scene.articulations["robot"].write_joint_state_to_sim(joint_pos, joint_vel) # reset buffers scene.reset() @@ -193,9 +193,9 @@ def test_frame_transformer_feet_wrt_base(self): # check absolute frame transforms in world frame # -- ground-truth - root_pose_w = scene.articulations["robot"].data.root_link_state_w[:, :7] - feet_pos_w_gt = scene.articulations["robot"].data.body_link_pos_w[:, feet_indices] - feet_quat_w_gt = scene.articulations["robot"].data.body_link_quat_w[:, feet_indices] + root_pose_w = scene.articulations["robot"].data.root_state_w[:, :7] + feet_pos_w_gt = scene.articulations["robot"].data.body_pos_w[:, feet_indices] + feet_quat_w_gt = scene.articulations["robot"].data.body_quat_w[:, feet_indices] # -- frame transformer source_pos_w_tf = scene.sensors["frame_transformer"].data.source_pos_w source_quat_w_tf = scene.sensors["frame_transformer"].data.source_quat_w @@ -276,8 +276,8 @@ def test_frame_transformer_feet_wrt_thigh(self): joint_vel = scene.articulations["robot"].data.default_joint_vel # -- set root state # -- robot - scene.articulations["robot"].write_root_link_pose_to_sim(root_state[:, :7]) - scene.articulations["robot"].write_root_com_velocity_to_sim(root_state[:, 7:]) + scene.articulations["robot"].write_root_pose_to_sim(root_state[:, :7]) + scene.articulations["robot"].write_root_velocity_to_sim(root_state[:, 7:]) scene.articulations["robot"].write_joint_state_to_sim(joint_pos, joint_vel) # reset buffers scene.reset() @@ -294,9 +294,9 @@ def test_frame_transformer_feet_wrt_thigh(self): # check absolute frame transforms in world frame # -- ground-truth - source_pose_w_gt = scene.articulations["robot"].data.body_link_state_w[:, source_frame_index, :7] - feet_pos_w_gt = scene.articulations["robot"].data.body_link_pos_w[:, feet_indices] - feet_quat_w_gt = scene.articulations["robot"].data.body_link_quat_w[:, feet_indices] + source_pose_w_gt = scene.articulations["robot"].data.body_state_w[:, source_frame_index, :7] + feet_pos_w_gt = scene.articulations["robot"].data.body_pos_w[:, feet_indices] + feet_quat_w_gt = scene.articulations["robot"].data.body_quat_w[:, feet_indices] # -- frame transformer source_pos_w_tf = scene.sensors["frame_transformer"].data.source_pos_w source_quat_w_tf = scene.sensors["frame_transformer"].data.source_quat_w @@ -358,8 +358,8 @@ def test_frame_transformer_robot_body_to_external_cube(self): joint_vel = scene.articulations["robot"].data.default_joint_vel # -- set root state # -- robot - scene.articulations["robot"].write_root_link_pose_to_sim(root_state[:, :7]) - scene.articulations["robot"].write_root_com_velocity_to_sim(root_state[:, 7:]) + scene.articulations["robot"].write_root_pose_to_sim(root_state[:, :7]) + scene.articulations["robot"].write_root_velocity_to_sim(root_state[:, 7:]) scene.articulations["robot"].write_joint_state_to_sim(joint_pos, joint_vel) # reset buffers scene.reset() @@ -376,9 +376,9 @@ def test_frame_transformer_robot_body_to_external_cube(self): # check absolute frame transforms in world frame # -- ground-truth - root_pose_w = scene.articulations["robot"].data.root_link_state_w[:, :7] - cube_pos_w_gt = scene.rigid_objects["cube"].data.root_link_state_w[:, :3] - cube_quat_w_gt = scene.rigid_objects["cube"].data.root_link_state_w[:, 3:7] + root_pose_w = scene.articulations["robot"].data.root_state_w[:, :7] + cube_pos_w_gt = scene.rigid_objects["cube"].data.root_state_w[:, :3] + cube_quat_w_gt = scene.rigid_objects["cube"].data.root_state_w[:, 3:7] # -- frame transformer source_pos_w_tf = scene.sensors["frame_transformer"].data.source_pos_w source_quat_w_tf = scene.sensors["frame_transformer"].data.source_quat_w @@ -450,8 +450,8 @@ def test_frame_transformer_offset_frames(self): root_state[:, :3] += scene.env_origins # -- set root state # -- cube - scene["cube"].write_root_link_pose_to_sim(root_state[:, :7]) - scene["cube"].write_root_com_velocity_to_sim(root_state[:, 7:]) + scene["cube"].write_root_pose_to_sim(root_state[:, :7]) + scene["cube"].write_root_velocity_to_sim(root_state[:, 7:]) # reset buffers scene.reset() @@ -464,8 +464,8 @@ def test_frame_transformer_offset_frames(self): # check absolute frame transforms in world frame # -- ground-truth - cube_pos_w_gt = scene["cube"].data.root_link_state_w[:, :3] - cube_quat_w_gt = scene["cube"].data.root_link_state_w[:, 3:7] + cube_pos_w_gt = scene["cube"].data.root_state_w[:, :3] + cube_quat_w_gt = scene["cube"].data.root_state_w[:, 3:7] # -- frame transformer source_pos_w_tf = scene.sensors["frame_transformer"].data.source_pos_w source_quat_w_tf = scene.sensors["frame_transformer"].data.source_quat_w @@ -538,8 +538,8 @@ def test_frame_transformer_all_bodies(self): joint_vel = scene.articulations["robot"].data.default_joint_vel # -- set root state # -- robot - scene.articulations["robot"].write_root_link_pose_to_sim(root_state[:, :7]) - scene.articulations["robot"].write_root_com_velocity_to_sim(root_state[:, 7:]) + scene.articulations["robot"].write_root_pose_to_sim(root_state[:, :7]) + scene.articulations["robot"].write_root_velocity_to_sim(root_state[:, 7:]) scene.articulations["robot"].write_joint_state_to_sim(joint_pos, joint_vel) # reset buffers scene.reset() @@ -556,9 +556,9 @@ def test_frame_transformer_all_bodies(self): # check absolute frame transforms in world frame # -- ground-truth - root_pose_w = scene.articulations["robot"].data.root_link_state_w[:, :7] - bodies_pos_w_gt = scene.articulations["robot"].data.body_link_pos_w - bodies_quat_w_gt = scene.articulations["robot"].data.body_link_quat_w + root_pose_w = scene.articulations["robot"].data.root_state_w[:, :7] + bodies_pos_w_gt = scene.articulations["robot"].data.body_pos_w + bodies_quat_w_gt = scene.articulations["robot"].data.body_quat_w # -- frame transformer source_pos_w_tf = scene.sensors["frame_transformer"].data.source_pos_w diff --git a/source/extensions/omni.isaac.lab/test/sensors/test_imu.py b/source/extensions/omni.isaac.lab/test/sensors/test_imu.py index 95ffd5a99c..597aa2273c 100644 --- a/source/extensions/omni.isaac.lab/test/sensors/test_imu.py +++ b/source/extensions/omni.isaac.lab/test/sensors/test_imu.py @@ -189,12 +189,12 @@ def test_constant_velocity(self): for idx in range(200): # set velocity - self.scene.rigid_objects["balls"].write_root_com_velocity_to_sim( + self.scene.rigid_objects["balls"].write_root_velocity_to_sim( torch.tensor([[1.0, 0.0, 0.0, 0.0, 0.0, 0.0]], dtype=torch.float32, device=self.scene.device).repeat( self.scene.num_envs, 1 ) ) - self.scene.rigid_objects["cube"].write_root_com_velocity_to_sim( + self.scene.rigid_objects["cube"].write_root_velocity_to_sim( torch.tensor([[1.0, 0.0, 0.0, 0.0, 0.0, 0.0]], dtype=torch.float32, device=self.scene.device).repeat( self.scene.num_envs, 1 ) @@ -236,7 +236,7 @@ def test_constant_velocity(self): ) # check the imu velocities - # NOTE: the expected lin_vel_b is the same as the set velocity, as write_root_com_velocity_to_sim is + # NOTE: the expected lin_vel_b is the same as the set velocity, as write_root_velocity_to_sim is # setting v_0 (initial velocity) and then a calculation step of v_i = v_0 + a*dt. Consequently, # the data.lin_vel_b is returning approx. v_i. torch.testing.assert_close( @@ -266,7 +266,7 @@ def test_constant_acceleration(self): """Test the Imu sensor with a constant acceleration.""" for idx in range(100): # set acceleration - self.scene.rigid_objects["balls"].write_root_com_velocity_to_sim( + self.scene.rigid_objects["balls"].write_root_velocity_to_sim( torch.tensor([[0.1, 0.0, 0.0, 0.0, 0.0, 0.0]], dtype=torch.float32, device=self.scene.device).repeat( self.scene.num_envs, 1 ) @@ -287,7 +287,7 @@ def test_constant_acceleration(self): torch.testing.assert_close( self.scene.sensors["imu_ball"].data.lin_acc_b, math_utils.quat_rotate_inverse( - self.scene.rigid_objects["balls"].data.root_link_quat_w, + self.scene.rigid_objects["balls"].data.root_quat_w, torch.tensor([[0.1, 0.0, 0.0]], dtype=torch.float32, device=self.scene.device).repeat( self.scene.num_envs, 1 ) @@ -300,7 +300,7 @@ def test_constant_acceleration(self): # check the angular velocity torch.testing.assert_close( self.scene.sensors["imu_ball"].data.ang_vel_b, - self.scene.rigid_objects["balls"].data.root_com_ang_vel_b, + self.scene.rigid_objects["balls"].data.root_ang_vel_b, rtol=1e-4, atol=1e-4, ) @@ -438,7 +438,7 @@ def test_offset_calculation(self): # should achieve same results between the two imu sensors on the robot for idx in range(500): # set acceleration - self.scene.articulations["robot"].write_root_com_velocity_to_sim( + self.scene.articulations["robot"].write_root_velocity_to_sim( torch.tensor([[0.05, 0.0, 0.0, 0.0, 0.0, 0.0]], dtype=torch.float32, device=self.scene.device).repeat( self.scene.num_envs, 1 ) @@ -504,7 +504,7 @@ def test_env_ids_propogation(self): for idx in range(10): # set acceleration - self.scene.articulations["robot"].write_root_com_velocity_to_sim( + self.scene.articulations["robot"].write_root_velocity_to_sim( torch.tensor([[0.5, 0.0, 0.0, 0.0, 0.0, 0.0]], dtype=torch.float32, device=self.scene.device).repeat( self.scene.num_envs, 1 ) diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/anymal_c/anymal_c_env.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/anymal_c/anymal_c_env.py index 006e859659..71fbc4ddc0 100644 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/anymal_c/anymal_c_env.py +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/anymal_c/anymal_c_env.py @@ -89,8 +89,8 @@ def _get_observations(self) -> dict: [ tensor for tensor in ( - self._robot.data.root_com_lin_vel_b, - self._robot.data.root_com_ang_vel_b, + self._robot.data.root_lin_vel_b, + self._robot.data.root_ang_vel_b, self._robot.data.projected_gravity_b, self._commands, self._robot.data.joint_pos - self._robot.data.default_joint_pos, @@ -107,17 +107,15 @@ def _get_observations(self) -> dict: def _get_rewards(self) -> torch.Tensor: # linear velocity tracking - lin_vel_error = torch.sum( - torch.square(self._commands[:, :2] - self._robot.data.root_com_lin_vel_b[:, :2]), dim=1 - ) + lin_vel_error = torch.sum(torch.square(self._commands[:, :2] - self._robot.data.root_lin_vel_b[:, :2]), dim=1) lin_vel_error_mapped = torch.exp(-lin_vel_error / 0.25) # yaw rate tracking - yaw_rate_error = torch.square(self._commands[:, 2] - self._robot.data.root_com_ang_vel_b[:, 2]) + yaw_rate_error = torch.square(self._commands[:, 2] - self._robot.data.root_ang_vel_b[:, 2]) yaw_rate_error_mapped = torch.exp(-yaw_rate_error / 0.25) # z velocity tracking - z_vel_error = torch.square(self._robot.data.root_com_lin_vel_b[:, 2]) + z_vel_error = torch.square(self._robot.data.root_lin_vel_b[:, 2]) # angular velocity x/y - ang_vel_error = torch.sum(torch.square(self._robot.data.root_com_ang_vel_b[:, :2]), dim=1) + ang_vel_error = torch.sum(torch.square(self._robot.data.root_ang_vel_b[:, :2]), dim=1) # joint torques joint_torques = torch.sum(torch.square(self._robot.data.applied_torque), dim=1) # joint acceleration @@ -180,8 +178,8 @@ def _reset_idx(self, env_ids: torch.Tensor | None): joint_vel = self._robot.data.default_joint_vel[env_ids] default_root_state = self._robot.data.default_root_state[env_ids] default_root_state[:, :3] += self._terrain.env_origins[env_ids] - self._robot.write_root_link_pose_to_sim(default_root_state[:, :7], env_ids) - self._robot.write_root_com_velocity_to_sim(default_root_state[:, 7:], env_ids) + self._robot.write_root_pose_to_sim(default_root_state[:, :7], env_ids) + self._robot.write_root_velocity_to_sim(default_root_state[:, 7:], env_ids) self._robot.write_joint_state_to_sim(joint_pos, joint_vel, None, env_ids) # Logging extras = dict() diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/cart_double_pendulum/cart_double_pendulum_env.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/cart_double_pendulum/cart_double_pendulum_env.py index 5ac285fc53..74c29c07be 100644 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/cart_double_pendulum/cart_double_pendulum_env.py +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/cart_double_pendulum/cart_double_pendulum_env.py @@ -182,8 +182,8 @@ def _reset_idx(self, env_ids: Sequence[int] | None): self.joint_pos[env_ids] = joint_pos self.joint_vel[env_ids] = joint_vel - self.robot.write_root_link_pose_to_sim(default_root_state[:, :7], env_ids) - self.robot.write_root_com_velocity_to_sim(default_root_state[:, 7:], env_ids) + self.robot.write_root_pose_to_sim(default_root_state[:, :7], env_ids) + self.robot.write_root_velocity_to_sim(default_root_state[:, 7:], env_ids) self.robot.write_joint_state_to_sim(joint_pos, joint_vel, None, env_ids) diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/cartpole/cartpole_camera_env.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/cartpole/cartpole_camera_env.py index 1f092a7eca..87cbdd3386 100644 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/cartpole/cartpole_camera_env.py +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/cartpole/cartpole_camera_env.py @@ -199,8 +199,8 @@ def _reset_idx(self, env_ids: Sequence[int] | None): self.joint_pos[env_ids] = joint_pos self.joint_vel[env_ids] = joint_vel - self._cartpole.write_root_link_pose_to_sim(default_root_state[:, :7], env_ids) - self._cartpole.write_root_com_velocity_to_sim(default_root_state[:, 7:], env_ids) + self._cartpole.write_root_pose_to_sim(default_root_state[:, :7], env_ids) + self._cartpole.write_root_velocity_to_sim(default_root_state[:, 7:], env_ids) self._cartpole.write_joint_state_to_sim(joint_pos, joint_vel, None, env_ids) diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/cartpole/cartpole_env.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/cartpole/cartpole_env.py index 1884b644c9..3ac5b98d07 100644 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/cartpole/cartpole_env.py +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/cartpole/cartpole_env.py @@ -143,8 +143,8 @@ def _reset_idx(self, env_ids: Sequence[int] | None): self.joint_pos[env_ids] = joint_pos self.joint_vel[env_ids] = joint_vel - self.cartpole.write_root_link_pose_to_sim(default_root_state[:, :7], env_ids) - self.cartpole.write_root_com_velocity_to_sim(default_root_state[:, 7:], env_ids) + self.cartpole.write_root_pose_to_sim(default_root_state[:, :7], env_ids) + self.cartpole.write_root_velocity_to_sim(default_root_state[:, 7:], env_ids) self.cartpole.write_joint_state_to_sim(joint_pos, joint_vel, None, env_ids) diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/factory/factory_env.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/factory/factory_env.py index 6eea50494a..75db2f9eb1 100644 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/factory/factory_env.py +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/factory/factory_env.py @@ -189,18 +189,16 @@ def _setup_scene(self): def _compute_intermediate_values(self, dt): """Get values computed from raw tensors. This includes adding noise.""" # TODO: A lot of these can probably only be set once? - self.fixed_pos = self._fixed_asset.data.root_link_pos_w - self.scene.env_origins - self.fixed_quat = self._fixed_asset.data.root_link_quat_w + self.fixed_pos = self._fixed_asset.data.root_pos_w - self.scene.env_origins + self.fixed_quat = self._fixed_asset.data.root_quat_w - self.held_pos = self._held_asset.data.root_link_pos_w - self.scene.env_origins - self.held_quat = self._held_asset.data.root_link_quat_w + self.held_pos = self._held_asset.data.root_pos_w - self.scene.env_origins + self.held_quat = self._held_asset.data.root_quat_w - self.fingertip_midpoint_pos = ( - self._robot.data.body_link_pos_w[:, self.fingertip_body_idx] - self.scene.env_origins - ) - self.fingertip_midpoint_quat = self._robot.data.body_link_quat_w[:, self.fingertip_body_idx] - self.fingertip_midpoint_linvel = self._robot.data.body_com_lin_vel_w[:, self.fingertip_body_idx] - self.fingertip_midpoint_angvel = self._robot.data.body_com_ang_vel_w[:, self.fingertip_body_idx] + self.fingertip_midpoint_pos = self._robot.data.body_pos_w[:, self.fingertip_body_idx] - self.scene.env_origins + self.fingertip_midpoint_quat = self._robot.data.body_quat_w[:, self.fingertip_body_idx] + self.fingertip_midpoint_linvel = self._robot.data.body_lin_vel_w[:, self.fingertip_body_idx] + self.fingertip_midpoint_angvel = self._robot.data.body_ang_vel_w[:, self.fingertip_body_idx] jacobians = self._robot.root_physx_view.get_jacobians() @@ -554,15 +552,15 @@ def _set_assets_to_default_pose(self, env_ids): held_state = self._held_asset.data.default_root_state.clone()[env_ids] held_state[:, 0:3] += self.scene.env_origins[env_ids] held_state[:, 7:] = 0.0 - self._held_asset.write_root_link_pose_to_sim(held_state[:, 0:7], env_ids=env_ids) - self._held_asset.write_root_com_velocity_to_sim(held_state[:, 7:], env_ids=env_ids) + self._held_asset.write_root_pose_to_sim(held_state[:, 0:7], env_ids=env_ids) + self._held_asset.write_root_velocity_to_sim(held_state[:, 7:], env_ids=env_ids) self._held_asset.reset() fixed_state = self._fixed_asset.data.default_root_state.clone()[env_ids] fixed_state[:, 0:3] += self.scene.env_origins[env_ids] fixed_state[:, 7:] = 0.0 - self._fixed_asset.write_root_link_pose_to_sim(fixed_state[:, 0:7], env_ids=env_ids) - self._fixed_asset.write_root_com_velocity_to_sim(fixed_state[:, 7:], env_ids=env_ids) + self._fixed_asset.write_root_pose_to_sim(fixed_state[:, 0:7], env_ids=env_ids) + self._fixed_asset.write_root_velocity_to_sim(fixed_state[:, 7:], env_ids=env_ids) self._fixed_asset.reset() def set_pos_inverse_kinematics(self, env_ids): @@ -685,8 +683,8 @@ def randomize_initial_state(self, env_ids): # (1.c.) Velocity fixed_state[:, 7:] = 0.0 # vel # (1.d.) Update values. - self._fixed_asset.write_root_link_pose_to_sim(fixed_state[:, 0:7], env_ids=env_ids) - self._fixed_asset.write_root_com_velocity_to_sim(fixed_state[:, 7:], env_ids=env_ids) + self._fixed_asset.write_root_pose_to_sim(fixed_state[:, 0:7], env_ids=env_ids) + self._fixed_asset.write_root_velocity_to_sim(fixed_state[:, 7:], env_ids=env_ids) self._fixed_asset.reset() # (1.e.) Noisy position observation. @@ -772,15 +770,15 @@ def randomize_initial_state(self, env_ids): small_gear_state = self._small_gear_asset.data.default_root_state.clone()[env_ids] small_gear_state[:, 0:7] = fixed_state[:, 0:7] small_gear_state[:, 7:] = 0.0 # vel - self._small_gear_asset.write_root_link_pose_to_sim(small_gear_state[:, 0:7], env_ids=env_ids) - self._small_gear_asset.write_root_com_velocity_to_sim(small_gear_state[:, 7:], env_ids=env_ids) + self._small_gear_asset.write_root_pose_to_sim(small_gear_state[:, 0:7], env_ids=env_ids) + self._small_gear_asset.write_root_velocity_to_sim(small_gear_state[:, 7:], env_ids=env_ids) self._small_gear_asset.reset() large_gear_state = self._large_gear_asset.data.default_root_state.clone()[env_ids] large_gear_state[:, 0:7] = fixed_state[:, 0:7] large_gear_state[:, 7:] = 0.0 # vel - self._large_gear_asset.write_root_link_pose_to_sim(large_gear_state[:, 0:7], env_ids=env_ids) - self._large_gear_asset.write_root_com_velocity_to_sim(large_gear_state[:, 7:], env_ids=env_ids) + self._large_gear_asset.write_root_pose_to_sim(large_gear_state[:, 0:7], env_ids=env_ids) + self._large_gear_asset.write_root_velocity_to_sim(large_gear_state[:, 7:], env_ids=env_ids) self._large_gear_asset.reset() # (3) Randomize asset-in-gripper location. @@ -822,8 +820,8 @@ def randomize_initial_state(self, env_ids): held_state[:, 0:3] = translated_held_asset_pos + self.scene.env_origins held_state[:, 3:7] = translated_held_asset_quat held_state[:, 7:] = 0.0 - self._held_asset.write_root_link_pose_to_sim(held_state[:, 0:7]) - self._held_asset.write_root_com_velocity_to_sim(held_state[:, 7:]) + self._held_asset.write_root_pose_to_sim(held_state[:, 0:7]) + self._held_asset.write_root_velocity_to_sim(held_state[:, 7:]) self._held_asset.reset() # Close hand diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/franka_cabinet/franka_cabinet_env.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/franka_cabinet/franka_cabinet_env.py index 7219374e06..6c8dd967d5 100644 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/franka_cabinet/franka_cabinet_env.py +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/franka_cabinet/franka_cabinet_env.py @@ -299,8 +299,8 @@ def _get_dones(self) -> tuple[torch.Tensor, torch.Tensor]: def _get_rewards(self) -> torch.Tensor: # Refresh the intermediate values after the physics steps self._compute_intermediate_values() - robot_left_finger_pos = self._robot.data.body_link_pos_w[:, self.left_finger_link_idx] - robot_right_finger_pos = self._robot.data.body_link_pos_w[:, self.right_finger_link_idx] + robot_left_finger_pos = self._robot.data.body_pos_w[:, self.left_finger_link_idx] + robot_right_finger_pos = self._robot.data.body_pos_w[:, self.right_finger_link_idx] return self._compute_rewards( self.actions, @@ -372,10 +372,10 @@ def _compute_intermediate_values(self, env_ids: torch.Tensor | None = None): if env_ids is None: env_ids = self._robot._ALL_INDICES - hand_pos = self._robot.data.body_link_pos_w[env_ids, self.hand_link_idx] - hand_rot = self._robot.data.body_link_quat_w[env_ids, self.hand_link_idx] - drawer_pos = self._cabinet.data.body_link_pos_w[env_ids, self.drawer_link_idx] - drawer_rot = self._cabinet.data.body_link_quat_w[env_ids, self.drawer_link_idx] + hand_pos = self._robot.data.body_pos_w[env_ids, self.hand_link_idx] + hand_rot = self._robot.data.body_quat_w[env_ids, self.hand_link_idx] + drawer_pos = self._cabinet.data.body_pos_w[env_ids, self.drawer_link_idx] + drawer_rot = self._cabinet.data.body_quat_w[env_ids, self.drawer_link_idx] ( self.robot_grasp_rot[env_ids], self.robot_grasp_pos[env_ids], diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/inhand_manipulation/inhand_manipulation_env.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/inhand_manipulation/inhand_manipulation_env.py index 2d8cf31a91..cbf29e5a2d 100644 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/inhand_manipulation/inhand_manipulation_env.py +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/inhand_manipulation/inhand_manipulation_env.py @@ -221,8 +221,8 @@ def _reset_idx(self, env_ids: Sequence[int] | None): ) object_default_state[:, 7:] = torch.zeros_like(self.object.data.default_root_state[env_ids, 7:]) - self.object.write_root_link_pose_to_sim(object_default_state[:, :7], env_ids) - self.object.write_root_com_velocity_to_sim(object_default_state[:, 7:], env_ids) + self.object.write_root_pose_to_sim(object_default_state[:, :7], env_ids) + self.object.write_root_velocity_to_sim(object_default_state[:, 7:], env_ids) # reset hand delta_max = self.hand_dof_upper_limits[env_ids] - self.hand.data.default_joint_pos[env_ids] @@ -261,22 +261,22 @@ def _reset_target_pose(self, env_ids): def _compute_intermediate_values(self): # data for hand - self.fingertip_pos = self.hand.data.body_link_pos_w[:, self.finger_bodies] - self.fingertip_rot = self.hand.data.body_link_quat_w[:, self.finger_bodies] + self.fingertip_pos = self.hand.data.body_pos_w[:, self.finger_bodies] + self.fingertip_rot = self.hand.data.body_quat_w[:, self.finger_bodies] self.fingertip_pos -= self.scene.env_origins.repeat((1, self.num_fingertips)).reshape( self.num_envs, self.num_fingertips, 3 ) - self.fingertip_velocities = self.hand.data.body_com_vel_w[:, self.finger_bodies] + self.fingertip_velocities = self.hand.data.body_vel_w[:, self.finger_bodies] self.hand_dof_pos = self.hand.data.joint_pos self.hand_dof_vel = self.hand.data.joint_vel # data for object - self.object_pos = self.object.data.root_link_pos_w - self.scene.env_origins - self.object_rot = self.object.data.root_link_quat_w - self.object_velocities = self.object.data.root_com_vel_w - self.object_linvel = self.object.data.root_com_lin_vel_w - self.object_angvel = self.object.data.root_com_ang_vel_w + self.object_pos = self.object.data.root_pos_w - self.scene.env_origins + self.object_rot = self.object.data.root_quat_w + self.object_velocities = self.object.data.root_vel_w + self.object_linvel = self.object.data.root_lin_vel_w + self.object_angvel = self.object.data.root_ang_vel_w def compute_reduced_observations(self): # Per https://arxiv.org/pdf/1808.00177.pdf Table 2 diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/locomotion/locomotion_env.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/locomotion/locomotion_env.py index accc60d983..b800e134c2 100644 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/locomotion/locomotion_env.py +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/locomotion/locomotion_env.py @@ -68,8 +68,8 @@ def _apply_action(self): self.robot.set_joint_effort_target(forces, joint_ids=self._joint_dof_idx) def _compute_intermediate_values(self): - self.torso_position, self.torso_rotation = self.robot.data.root_link_pos_w, self.robot.data.root_link_quat_w - self.velocity, self.ang_velocity = self.robot.data.root_com_lin_vel_w, self.robot.data.root_com_ang_vel_w + self.torso_position, self.torso_rotation = self.robot.data.root_pos_w, self.robot.data.root_quat_w + self.velocity, self.ang_velocity = self.robot.data.root_lin_vel_w, self.robot.data.root_ang_vel_w self.dof_pos, self.dof_vel = self.robot.data.joint_pos, self.robot.data.joint_vel ( @@ -161,8 +161,8 @@ def _reset_idx(self, env_ids: torch.Tensor | None): default_root_state = self.robot.data.default_root_state[env_ids] default_root_state[:, :3] += self.scene.env_origins[env_ids] - self.robot.write_root_link_pose_to_sim(default_root_state[:, :7], env_ids) - self.robot.write_root_com_velocity_to_sim(default_root_state[:, 7:], env_ids) + self.robot.write_root_pose_to_sim(default_root_state[:, :7], env_ids) + self.robot.write_root_velocity_to_sim(default_root_state[:, 7:], env_ids) self.robot.write_joint_state_to_sim(joint_pos, joint_vel, None, env_ids) to_target = self.targets[env_ids] - default_root_state[:, :3] diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/quadcopter/quadcopter_env.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/quadcopter/quadcopter_env.py index 67ac143c79..fc5f08bcc6 100644 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/quadcopter/quadcopter_env.py +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/quadcopter/quadcopter_env.py @@ -154,12 +154,12 @@ def _apply_action(self): def _get_observations(self) -> dict: desired_pos_b, _ = subtract_frame_transforms( - self._robot.data.root_link_state_w[:, :3], self._robot.data.root_link_state_w[:, 3:7], self._desired_pos_w + self._robot.data.root_state_w[:, :3], self._robot.data.root_state_w[:, 3:7], self._desired_pos_w ) obs = torch.cat( [ - self._robot.data.root_com_lin_vel_b, - self._robot.data.root_com_ang_vel_b, + self._robot.data.root_lin_vel_b, + self._robot.data.root_ang_vel_b, self._robot.data.projected_gravity_b, desired_pos_b, ], @@ -169,9 +169,9 @@ def _get_observations(self) -> dict: return observations def _get_rewards(self) -> torch.Tensor: - lin_vel = torch.sum(torch.square(self._robot.data.root_com_lin_vel_b), dim=1) - ang_vel = torch.sum(torch.square(self._robot.data.root_com_ang_vel_b), dim=1) - distance_to_goal = torch.linalg.norm(self._desired_pos_w - self._robot.data.root_link_pos_w, dim=1) + lin_vel = torch.sum(torch.square(self._robot.data.root_lin_vel_b), dim=1) + ang_vel = torch.sum(torch.square(self._robot.data.root_ang_vel_b), dim=1) + distance_to_goal = torch.linalg.norm(self._desired_pos_w - self._robot.data.root_pos_w, dim=1) distance_to_goal_mapped = 1 - torch.tanh(distance_to_goal / 0.8) rewards = { "lin_vel": lin_vel * self.cfg.lin_vel_reward_scale * self.step_dt, @@ -186,9 +186,7 @@ def _get_rewards(self) -> torch.Tensor: def _get_dones(self) -> tuple[torch.Tensor, torch.Tensor]: time_out = self.episode_length_buf >= self.max_episode_length - 1 - died = torch.logical_or( - self._robot.data.root_link_pos_w[:, 2] < 0.1, self._robot.data.root_link_pos_w[:, 2] > 2.0 - ) + died = torch.logical_or(self._robot.data.root_pos_w[:, 2] < 0.1, self._robot.data.root_pos_w[:, 2] > 2.0) return died, time_out def _reset_idx(self, env_ids: torch.Tensor | None): @@ -197,7 +195,7 @@ def _reset_idx(self, env_ids: torch.Tensor | None): # Logging final_distance_to_goal = torch.linalg.norm( - self._desired_pos_w[env_ids] - self._robot.data.root_link_pos_w[env_ids], dim=1 + self._desired_pos_w[env_ids] - self._robot.data.root_pos_w[env_ids], dim=1 ).mean() extras = dict() for key in self._episode_sums.keys(): @@ -228,8 +226,8 @@ def _reset_idx(self, env_ids: torch.Tensor | None): joint_vel = self._robot.data.default_joint_vel[env_ids] default_root_state = self._robot.data.default_root_state[env_ids] default_root_state[:, :3] += self._terrain.env_origins[env_ids] - self._robot.write_root_link_pose_to_sim(default_root_state[:, :7], env_ids) - self._robot.write_root_com_velocity_to_sim(default_root_state[:, 7:], env_ids) + self._robot.write_root_pose_to_sim(default_root_state[:, :7], env_ids) + self._robot.write_root_velocity_to_sim(default_root_state[:, 7:], env_ids) self._robot.write_joint_state_to_sim(joint_pos, joint_vel, None, env_ids) def _set_debug_vis_impl(self, debug_vis: bool): diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/shadow_hand_over/shadow_hand_over_env.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/shadow_hand_over/shadow_hand_over_env.py index 5f0e01be92..07e40771a0 100644 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/shadow_hand_over/shadow_hand_over_env.py +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/shadow_hand_over/shadow_hand_over_env.py @@ -322,8 +322,8 @@ def _reset_idx(self, env_ids: Sequence[int] | torch.Tensor | None): ) object_default_state[:, 7:] = torch.zeros_like(self.object.data.default_root_state[env_ids, 7:]) - self.object.write_root_link_pose_to_sim(object_default_state[:, :7], env_ids) - self.object.write_root_com_velocity_to_sim(object_default_state[:, 7:], env_ids) + self.object.write_root_pose_to_sim(object_default_state[:, :7], env_ids) + self.object.write_root_velocity_to_sim(object_default_state[:, 7:], env_ids) # reset right hand delta_max = self.hand_dof_upper_limits[env_ids] - self.right_hand.data.default_joint_pos[env_ids] @@ -377,33 +377,33 @@ def _reset_target_pose(self, env_ids): def _compute_intermediate_values(self): # data for right hand - self.right_fingertip_pos = self.right_hand.data.body_link_pos_w[:, self.finger_bodies] - self.right_fingertip_rot = self.right_hand.data.body_link_quat_w[:, self.finger_bodies] + self.right_fingertip_pos = self.right_hand.data.body_pos_w[:, self.finger_bodies] + self.right_fingertip_rot = self.right_hand.data.body_quat_w[:, self.finger_bodies] self.right_fingertip_pos -= self.scene.env_origins.repeat((1, self.num_fingertips)).reshape( self.num_envs, self.num_fingertips, 3 ) - self.right_fingertip_velocities = self.right_hand.data.body_com_vel_w[:, self.finger_bodies] + self.right_fingertip_velocities = self.right_hand.data.body_vel_w[:, self.finger_bodies] self.right_hand_dof_pos = self.right_hand.data.joint_pos self.right_hand_dof_vel = self.right_hand.data.joint_vel # data for left hand - self.left_fingertip_pos = self.left_hand.data.body_link_pos_w[:, self.finger_bodies] - self.left_fingertip_rot = self.left_hand.data.body_link_quat_w[:, self.finger_bodies] + self.left_fingertip_pos = self.left_hand.data.body_pos_w[:, self.finger_bodies] + self.left_fingertip_rot = self.left_hand.data.body_quat_w[:, self.finger_bodies] self.left_fingertip_pos -= self.scene.env_origins.repeat((1, self.num_fingertips)).reshape( self.num_envs, self.num_fingertips, 3 ) - self.left_fingertip_velocities = self.left_hand.data.body_com_vel_w[:, self.finger_bodies] + self.left_fingertip_velocities = self.left_hand.data.body_vel_w[:, self.finger_bodies] self.left_hand_dof_pos = self.left_hand.data.joint_pos self.left_hand_dof_vel = self.left_hand.data.joint_vel # data for object - self.object_pos = self.object.data.root_link_pos_w - self.scene.env_origins - self.object_rot = self.object.data.root_link_quat_w - self.object_velocities = self.object.data.root_com_vel_w - self.object_linvel = self.object.data.root_com_lin_vel_w - self.object_angvel = self.object.data.root_com_ang_vel_w + self.object_pos = self.object.data.root_pos_w - self.scene.env_origins + self.object_rot = self.object.data.root_quat_w + self.object_velocities = self.object.data.root_vel_w + self.object_linvel = self.object.data.root_lin_vel_w + self.object_angvel = self.object.data.root_ang_vel_w @torch.jit.script diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/classic/humanoid/mdp/observations.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/classic/humanoid/mdp/observations.py index f979d67666..a896ae56d7 100644 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/classic/humanoid/mdp/observations.py +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/classic/humanoid/mdp/observations.py @@ -21,7 +21,7 @@ def base_yaw_roll(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntityC # extract the used quantities (to enable type-hinting) asset: Articulation = env.scene[asset_cfg.name] # extract euler angles (in world frame) - roll, _, yaw = math_utils.euler_xyz_from_quat(asset.data.root_link_quat_w) + roll, _, yaw = math_utils.euler_xyz_from_quat(asset.data.root_quat_w) # normalize angle to [-pi, pi] roll = torch.atan2(torch.sin(roll), torch.cos(roll)) yaw = torch.atan2(torch.sin(yaw), torch.cos(yaw)) @@ -46,11 +46,11 @@ def base_heading_proj( # extract the used quantities (to enable type-hinting) asset: Articulation = env.scene[asset_cfg.name] # compute desired heading direction - to_target_pos = torch.tensor(target_pos, device=env.device) - asset.data.root_link_pos_w[:, :3] + to_target_pos = torch.tensor(target_pos, device=env.device) - asset.data.root_pos_w[:, :3] to_target_pos[:, 2] = 0.0 to_target_dir = math_utils.normalize(to_target_pos) # compute base forward vector - heading_vec = math_utils.quat_rotate(asset.data.root_link_quat_w, asset.data.FORWARD_VEC_B) + heading_vec = math_utils.quat_rotate(asset.data.root_quat_w, asset.data.FORWARD_VEC_B) # compute dot product between heading and target direction heading_proj = torch.bmm(heading_vec.view(env.num_envs, 1, 3), to_target_dir.view(env.num_envs, 3, 1)) @@ -64,10 +64,10 @@ def base_angle_to_target( # extract the used quantities (to enable type-hinting) asset: Articulation = env.scene[asset_cfg.name] # compute desired heading direction - to_target_pos = torch.tensor(target_pos, device=env.device) - asset.data.root_link_pos_w[:, :3] + to_target_pos = torch.tensor(target_pos, device=env.device) - asset.data.root_pos_w[:, :3] walk_target_angle = torch.atan2(to_target_pos[:, 1], to_target_pos[:, 0]) # compute base forward vector - _, _, yaw = math_utils.euler_xyz_from_quat(asset.data.root_link_quat_w) + _, _, yaw = math_utils.euler_xyz_from_quat(asset.data.root_quat_w) # normalize angle to target to [-pi, pi] angle_to_target = walk_target_angle - yaw angle_to_target = torch.atan2(torch.sin(angle_to_target), torch.cos(angle_to_target)) diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/classic/humanoid/mdp/rewards.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/classic/humanoid/mdp/rewards.py index fe4bff11db..10361748c5 100644 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/classic/humanoid/mdp/rewards.py +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/classic/humanoid/mdp/rewards.py @@ -53,7 +53,7 @@ def reset(self, env_ids: torch.Tensor): asset: Articulation = self._env.scene["robot"] # compute projection of current heading to desired heading vector target_pos = torch.tensor(self.cfg.params["target_pos"], device=self.device) - to_target_pos = target_pos - asset.data.root_link_pos_w[env_ids, :3] + to_target_pos = target_pos - asset.data.root_pos_w[env_ids, :3] # reward terms self.potentials[env_ids] = -torch.norm(to_target_pos, p=2, dim=-1) / self._env.step_dt self.prev_potentials[env_ids] = self.potentials[env_ids] @@ -68,7 +68,7 @@ def __call__( asset: Articulation = env.scene[asset_cfg.name] # compute vector to target target_pos = torch.tensor(target_pos, device=env.device) - to_target_pos = target_pos - asset.data.root_link_pos_w[:, :3] + to_target_pos = target_pos - asset.data.root_pos_w[:, :3] to_target_pos[:, 2] = 0.0 # update history buffer and compute new potential self.prev_potentials[:] = self.potentials[:] diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/locomotion/velocity/config/spot/mdp/rewards.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/locomotion/velocity/config/spot/mdp/rewards.py index eb09174f95..c79eb8e99d 100644 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/locomotion/velocity/config/spot/mdp/rewards.py +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/locomotion/velocity/config/spot/mdp/rewards.py @@ -49,7 +49,7 @@ def air_time_reward( t_min = torch.clip(t_max, max=mode_time) stance_cmd_reward = torch.clip(current_contact_time - current_air_time, -mode_time, mode_time) cmd = torch.norm(env.command_manager.get_command("base_velocity"), dim=1).unsqueeze(dim=1).expand(-1, 4) - body_vel = torch.linalg.norm(asset.data.root_com_lin_vel_b[:, :2], dim=1).unsqueeze(dim=1).expand(-1, 4) + body_vel = torch.linalg.norm(asset.data.root_lin_vel_b[:, :2], dim=1).unsqueeze(dim=1).expand(-1, 4) reward = torch.where( torch.logical_or(cmd > 0.0, body_vel > velocity_threshold), torch.where(t_max < mode_time, t_min, 0), @@ -64,7 +64,7 @@ def base_angular_velocity_reward(env: ManagerBasedRLEnv, asset_cfg: SceneEntityC asset: RigidObject = env.scene[asset_cfg.name] # compute the error target = env.command_manager.get_command("base_velocity")[:, 2] - ang_vel_error = torch.linalg.norm((target - asset.data.root_com_ang_vel_b[:, 2]).unsqueeze(1), dim=1) + ang_vel_error = torch.linalg.norm((target - asset.data.root_ang_vel_b[:, 2]).unsqueeze(1), dim=1) return torch.exp(-ang_vel_error / std) @@ -76,7 +76,7 @@ def base_linear_velocity_reward( asset: RigidObject = env.scene[asset_cfg.name] # compute the error target = env.command_manager.get_command("base_velocity")[:, :2] - lin_vel_error = torch.linalg.norm((target - asset.data.root_com_lin_vel_b[:, :2]), dim=1) + lin_vel_error = torch.linalg.norm((target - asset.data.root_lin_vel_b[:, :2]), dim=1) # fixed 1.0 multiple for tracking below the ramp_at_vel value, then scale by the rate above vel_cmd_magnitude = torch.linalg.norm(target, dim=1) velocity_scaling_multiple = torch.clamp(1.0 + ramp_rate * (vel_cmd_magnitude - ramp_at_vel), min=1.0) @@ -148,7 +148,7 @@ def __call__( async_reward = async_reward_0 * async_reward_1 * async_reward_2 * async_reward_3 # only enforce gait if cmd > 0 cmd = torch.norm(env.command_manager.get_command("base_velocity"), dim=1) - body_vel = torch.linalg.norm(self.asset.data.root_com_lin_vel_b[:, :2], dim=1) + body_vel = torch.linalg.norm(self.asset.data.root_lin_vel_b[:, :2], dim=1) return torch.where( torch.logical_or(cmd > 0.0, body_vel > self.velocity_threshold), sync_reward * async_reward, 0.0 ) @@ -182,10 +182,8 @@ def foot_clearance_reward( ) -> torch.Tensor: """Reward the swinging feet for clearing a specified height off the ground""" asset: RigidObject = env.scene[asset_cfg.name] - foot_z_target_error = torch.square(asset.data.body_link_pos_w[:, asset_cfg.body_ids, 2] - target_height) - foot_velocity_tanh = torch.tanh( - tanh_mult * torch.norm(asset.data.body_com_lin_vel_w[:, asset_cfg.body_ids, :2], dim=2) - ) + foot_z_target_error = torch.square(asset.data.body_pos_w[:, asset_cfg.body_ids, 2] - target_height) + foot_velocity_tanh = torch.tanh(tanh_mult * torch.norm(asset.data.body_lin_vel_w[:, asset_cfg.body_ids, :2], dim=2)) reward = foot_z_target_error * foot_velocity_tanh return torch.exp(-torch.sum(reward, dim=1) / std) @@ -219,8 +217,8 @@ def base_motion_penalty(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg) -> to """Penalize base vertical and roll/pitch velocity""" # extract the used quantities (to enable type-hinting) asset: RigidObject = env.scene[asset_cfg.name] - return 0.8 * torch.square(asset.data.root_com_lin_vel_b[:, 2]) + 0.2 * torch.sum( - torch.abs(asset.data.root_com_ang_vel_b[:, :2]), dim=1 + return 0.8 * torch.square(asset.data.root_lin_vel_b[:, 2]) + 0.2 * torch.sum( + torch.abs(asset.data.root_ang_vel_b[:, :2]), dim=1 ) @@ -245,7 +243,7 @@ def foot_slip_penalty( # check if contact force is above threshold net_contact_forces = contact_sensor.data.net_forces_w_history is_contact = torch.max(torch.norm(net_contact_forces[:, :, sensor_cfg.body_ids], dim=-1), dim=1)[0] > threshold - foot_planar_velocity = torch.linalg.norm(asset.data.body_com_lin_vel_w[:, asset_cfg.body_ids, :2], dim=2) + foot_planar_velocity = torch.linalg.norm(asset.data.body_lin_vel_w[:, asset_cfg.body_ids, :2], dim=2) reward = is_contact * foot_planar_velocity return torch.sum(reward, dim=1) @@ -265,7 +263,7 @@ def joint_position_penalty( # extract the used quantities (to enable type-hinting) asset: Articulation = env.scene[asset_cfg.name] cmd = torch.linalg.norm(env.command_manager.get_command("base_velocity"), dim=1) - body_vel = torch.linalg.norm(asset.data.root_com_lin_vel_b[:, :2], dim=1) + body_vel = torch.linalg.norm(asset.data.root_lin_vel_b[:, :2], dim=1) reward = torch.linalg.norm((asset.data.joint_pos - asset.data.default_joint_pos), dim=1) return torch.where(torch.logical_or(cmd > 0.0, body_vel > velocity_threshold), reward, stand_still_scale * reward) diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/locomotion/velocity/mdp/curriculums.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/locomotion/velocity/mdp/curriculums.py index eac2ae58d8..0797f03844 100644 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/locomotion/velocity/mdp/curriculums.py +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/locomotion/velocity/mdp/curriculums.py @@ -43,7 +43,7 @@ def terrain_levels_vel( terrain: TerrainImporter = env.scene.terrain command = env.command_manager.get_command("base_velocity") # compute the distance the robot walked - distance = torch.norm(asset.data.root_link_pos_w[env_ids, :2] - env.scene.env_origins[env_ids, :2], dim=1) + distance = torch.norm(asset.data.root_pos_w[env_ids, :2] - env.scene.env_origins[env_ids, :2], dim=1) # robots that walked far enough progress to harder terrains move_up = distance > terrain.cfg.terrain_generator.size[0] / 2 # robots that walked less than half of their required distance go to simpler terrains diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/locomotion/velocity/mdp/rewards.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/locomotion/velocity/mdp/rewards.py index 08570eee80..15db609be2 100644 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/locomotion/velocity/mdp/rewards.py +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/locomotion/velocity/mdp/rewards.py @@ -78,7 +78,7 @@ def feet_slide(env, sensor_cfg: SceneEntityCfg, asset_cfg: SceneEntityCfg = Scen contacts = contact_sensor.data.net_forces_w_history[:, :, sensor_cfg.body_ids, :].norm(dim=-1).max(dim=1)[0] > 1.0 asset = env.scene[asset_cfg.name] - body_vel = asset.data.body_com_lin_vel_w[:, asset_cfg.body_ids, :2] + body_vel = asset.data.body_lin_vel_w[:, asset_cfg.body_ids, :2] reward = torch.sum(body_vel.norm(dim=-1) * contacts, dim=1) return reward @@ -89,7 +89,7 @@ def track_lin_vel_xy_yaw_frame_exp( """Reward tracking of linear velocity commands (xy axes) in the gravity aligned robot frame using exponential kernel.""" # extract the used quantities (to enable type-hinting) asset = env.scene[asset_cfg.name] - vel_yaw = quat_rotate_inverse(yaw_quat(asset.data.root_link_quat_w), asset.data.root_com_lin_vel_w[:, :3]) + vel_yaw = quat_rotate_inverse(yaw_quat(asset.data.root_quat_w), asset.data.root_lin_vel_w[:, :3]) lin_vel_error = torch.sum( torch.square(env.command_manager.get_command(command_name)[:, :2] - vel_yaw[:, :2]), dim=1 ) @@ -102,7 +102,5 @@ def track_ang_vel_z_world_exp( """Reward tracking of angular velocity commands (yaw) in world frame using exponential kernel.""" # extract the used quantities (to enable type-hinting) asset = env.scene[asset_cfg.name] - ang_vel_error = torch.square( - env.command_manager.get_command(command_name)[:, 2] - asset.data.root_com_ang_vel_w[:, 2] - ) + ang_vel_error = torch.square(env.command_manager.get_command(command_name)[:, 2] - asset.data.root_ang_vel_w[:, 2]) return torch.exp(-ang_vel_error / std**2) diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/locomotion/velocity/mdp/terminations.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/locomotion/velocity/mdp/terminations.py index 48970434fa..223814026d 100644 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/locomotion/velocity/mdp/terminations.py +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/locomotion/velocity/mdp/terminations.py @@ -45,8 +45,8 @@ def terrain_out_of_bounds( asset: RigidObject = env.scene[asset_cfg.name] # check if the agent is out of bounds - x_out_of_bounds = torch.abs(asset.data.root_link_pos_w[:, 0]) > 0.5 * map_width - distance_buffer - y_out_of_bounds = torch.abs(asset.data.root_link_pos_w[:, 1]) > 0.5 * map_height - distance_buffer + x_out_of_bounds = torch.abs(asset.data.root_pos_w[:, 0]) > 0.5 * map_width - distance_buffer + y_out_of_bounds = torch.abs(asset.data.root_pos_w[:, 1]) > 0.5 * map_height - distance_buffer return torch.logical_or(x_out_of_bounds, y_out_of_bounds) else: raise ValueError("Received unsupported terrain type, must be either 'plane' or 'generator'.") diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/cabinet/mdp/observations.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/cabinet/mdp/observations.py index 3eb98000d6..1b7838d3de 100644 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/cabinet/mdp/observations.py +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/cabinet/mdp/observations.py @@ -21,7 +21,7 @@ def rel_ee_object_distance(env: ManagerBasedRLEnv) -> torch.Tensor: ee_tf_data: FrameTransformerData = env.scene["ee_frame"].data object_data: ArticulationData = env.scene["object"].data - return object_data.root_link_pos_w - ee_tf_data.target_pos_w[..., 0, :] + return object_data.root_pos_w - ee_tf_data.target_pos_w[..., 0, :] def rel_ee_drawer_distance(env: ManagerBasedRLEnv) -> torch.Tensor: diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/inhand/mdp/commands/orientation_command.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/inhand/mdp/commands/orientation_command.py index 1e1d5b7f8e..2ec47f8e8a 100644 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/inhand/mdp/commands/orientation_command.py +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/inhand/mdp/commands/orientation_command.py @@ -95,10 +95,10 @@ def _update_metrics(self): # logs data # -- compute the orientation error self.metrics["orientation_error"] = math_utils.quat_error_magnitude( - self.object.data.root_link_quat_w, self.quat_command_w + self.object.data.root_quat_w, self.quat_command_w ) # -- compute the position error - self.metrics["position_error"] = torch.norm(self.object.data.root_link_pos_w - self.pos_command_w, dim=1) + self.metrics["position_error"] = torch.norm(self.object.data.root_pos_w - self.pos_command_w, dim=1) # -- compute the number of consecutive successes successes = self.metrics["orientation_error"] < self.cfg.orientation_success_threshold self.metrics["consecutive_success"] += successes.float() diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/inhand/mdp/observations.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/inhand/mdp/observations.py index 7edb0fd5be..f770c57aff 100644 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/inhand/mdp/observations.py +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/inhand/mdp/observations.py @@ -30,7 +30,7 @@ def goal_quat_diff( # obtain the orientations goal_quat_w = command_term.command[:, 3:7] - asset_quat_w = asset.data.root_link_quat_w + asset_quat_w = asset.data.root_quat_w # compute quaternion difference quat = math_utils.quat_mul(asset_quat_w, math_utils.quat_conjugate(goal_quat_w)) diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/inhand/mdp/rewards.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/inhand/mdp/rewards.py index 93283c8b81..3da25bf8e9 100644 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/inhand/mdp/rewards.py +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/inhand/mdp/rewards.py @@ -39,7 +39,7 @@ def success_bonus( # obtain the threshold for the orientation error threshold = command_term.cfg.orientation_success_threshold # calculate the orientation error - dtheta = math_utils.quat_error_magnitude(asset.data.root_link_quat_w, goal_quat_w) + dtheta = math_utils.quat_error_magnitude(asset.data.root_quat_w, goal_quat_w) return dtheta <= threshold @@ -63,7 +63,7 @@ def track_pos_l2( # obtain the goal position goal_pos_e = command_term.command[:, 0:3] # obtain the object position in the environment frame - object_pos_e = asset.data.root_link_pos_w - env.scene.env_origins + object_pos_e = asset.data.root_pos_w - env.scene.env_origins return torch.norm(goal_pos_e - object_pos_e, p=2, dim=-1) @@ -91,6 +91,6 @@ def track_orientation_inv_l2( # obtain the goal orientation goal_quat_w = command_term.command[:, 3:7] # calculate the orientation error - dtheta = math_utils.quat_error_magnitude(asset.data.root_link_quat_w, goal_quat_w) + dtheta = math_utils.quat_error_magnitude(asset.data.root_quat_w, goal_quat_w) return 1.0 / (dtheta + rot_eps) diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/inhand/mdp/terminations.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/inhand/mdp/terminations.py index ed16c3814a..2f951e287d 100644 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/inhand/mdp/terminations.py +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/inhand/mdp/terminations.py @@ -50,7 +50,7 @@ def object_away_from_goal( asset = env.scene[object_cfg.name] # object pos - asset_pos_e = asset.data.root_link_pos_w - env.scene.env_origins + asset_pos_e = asset.data.root_pos_w - env.scene.env_origins goal_pos_e = command_term.command[:, :3] return torch.norm(asset_pos_e - goal_pos_e, p=2, dim=1) > threshold @@ -78,6 +78,6 @@ def object_away_from_robot( object = env.scene[object_cfg.name] # compute distance - dist = torch.norm(robot.data.root_link_pos_w - object.data.root_link_pos_w, dim=1) + dist = torch.norm(robot.data.root_pos_w - object.data.root_pos_w, dim=1) return dist > threshold diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/lift/mdp/observations.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/lift/mdp/observations.py index f7335db24a..11c9295872 100644 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/lift/mdp/observations.py +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/lift/mdp/observations.py @@ -24,8 +24,8 @@ def object_position_in_robot_root_frame( """The position of the object in the robot's root frame.""" robot: RigidObject = env.scene[robot_cfg.name] object: RigidObject = env.scene[object_cfg.name] - object_pos_w = object.data.root_link_pos_w[:, :3] + object_pos_w = object.data.root_pos_w[:, :3] object_pos_b, _ = subtract_frame_transforms( - robot.data.root_link_state_w[:, :3], robot.data.root_link_state_w[:, 3:7], object_pos_w + robot.data.root_state_w[:, :3], robot.data.root_state_w[:, 3:7], object_pos_w ) return object_pos_b diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/lift/mdp/rewards.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/lift/mdp/rewards.py index 3587fc396d..e06449c1ab 100644 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/lift/mdp/rewards.py +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/lift/mdp/rewards.py @@ -22,7 +22,7 @@ def object_is_lifted( ) -> torch.Tensor: """Reward the agent for lifting the object above the minimal height.""" object: RigidObject = env.scene[object_cfg.name] - return torch.where(object.data.root_link_pos_w[:, 2] > minimal_height, 1.0, 0.0) + return torch.where(object.data.root_pos_w[:, 2] > minimal_height, 1.0, 0.0) def object_ee_distance( @@ -36,7 +36,7 @@ def object_ee_distance( object: RigidObject = env.scene[object_cfg.name] ee_frame: FrameTransformer = env.scene[ee_frame_cfg.name] # Target object position: (num_envs, 3) - cube_pos_w = object.data.root_link_pos_w + cube_pos_w = object.data.root_pos_w # End-effector position: (num_envs, 3) ee_w = ee_frame.data.target_pos_w[..., 0, :] # Distance of the end-effector to the object: (num_envs,) @@ -60,10 +60,8 @@ def object_goal_distance( command = env.command_manager.get_command(command_name) # compute the desired position in the world frame des_pos_b = command[:, :3] - des_pos_w, _ = combine_frame_transforms( - robot.data.root_link_state_w[:, :3], robot.data.root_link_state_w[:, 3:7], des_pos_b - ) + des_pos_w, _ = combine_frame_transforms(robot.data.root_state_w[:, :3], robot.data.root_state_w[:, 3:7], des_pos_b) # distance of the end-effector to the object: (num_envs,) - distance = torch.norm(des_pos_w - object.data.root_link_pos_w[:, :3], dim=1) + distance = torch.norm(des_pos_w - object.data.root_pos_w[:, :3], dim=1) # rewarded if the object is lifted above the threshold - return (object.data.root_link_pos_w[:, 2] > minimal_height) * (1 - torch.tanh(distance / std)) + return (object.data.root_pos_w[:, 2] > minimal_height) * (1 - torch.tanh(distance / std)) diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/lift/mdp/terminations.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/lift/mdp/terminations.py index 2b7559ef72..83b4d051df 100644 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/lift/mdp/terminations.py +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/lift/mdp/terminations.py @@ -45,11 +45,9 @@ def object_reached_goal( command = env.command_manager.get_command(command_name) # compute the desired position in the world frame des_pos_b = command[:, :3] - des_pos_w, _ = combine_frame_transforms( - robot.data.root_link_state_w[:, :3], robot.data.root_link_state_w[:, 3:7], des_pos_b - ) + des_pos_w, _ = combine_frame_transforms(robot.data.root_state_w[:, :3], robot.data.root_state_w[:, 3:7], des_pos_b) # distance of the end-effector to the object: (num_envs,) - distance = torch.norm(des_pos_w - object.data.root_link_pos_w[:, :3], dim=1) + distance = torch.norm(des_pos_w - object.data.root_pos_w[:, :3], dim=1) # rewarded if the object is lifted above the threshold return distance < threshold diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/reach/mdp/rewards.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/reach/mdp/rewards.py index 806acd9227..1e8bc1daaf 100644 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/reach/mdp/rewards.py +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/reach/mdp/rewards.py @@ -28,10 +28,8 @@ def position_command_error(env: ManagerBasedRLEnv, command_name: str, asset_cfg: command = env.command_manager.get_command(command_name) # obtain the desired and current positions des_pos_b = command[:, :3] - des_pos_w, _ = combine_frame_transforms( - asset.data.root_link_state_w[:, :3], asset.data.root_link_state_w[:, 3:7], des_pos_b - ) - curr_pos_w = asset.data.body_link_state_w[:, asset_cfg.body_ids[0], :3] # type: ignore + des_pos_w, _ = combine_frame_transforms(asset.data.root_state_w[:, :3], asset.data.root_state_w[:, 3:7], des_pos_b) + curr_pos_w = asset.data.body_state_w[:, asset_cfg.body_ids[0], :3] # type: ignore return torch.norm(curr_pos_w - des_pos_w, dim=1) @@ -48,10 +46,8 @@ def position_command_error_tanh( command = env.command_manager.get_command(command_name) # obtain the desired and current positions des_pos_b = command[:, :3] - des_pos_w, _ = combine_frame_transforms( - asset.data.root_link_state_w[:, :3], asset.data.root_link_state_w[:, 3:7], des_pos_b - ) - curr_pos_w = asset.data.body_link_state_w[:, asset_cfg.body_ids[0], :3] # type: ignore + des_pos_w, _ = combine_frame_transforms(asset.data.root_state_w[:, :3], asset.data.root_state_w[:, 3:7], des_pos_b) + curr_pos_w = asset.data.body_state_w[:, asset_cfg.body_ids[0], :3] # type: ignore distance = torch.norm(curr_pos_w - des_pos_w, dim=1) return 1 - torch.tanh(distance / std) @@ -68,6 +64,6 @@ def orientation_command_error(env: ManagerBasedRLEnv, command_name: str, asset_c command = env.command_manager.get_command(command_name) # obtain the desired and current orientations des_quat_b = command[:, 3:7] - des_quat_w = quat_mul(asset.data.root_link_state_w[:, 3:7], des_quat_b) - curr_quat_w = asset.data.body_link_state_w[:, asset_cfg.body_ids[0], 3:7] # type: ignore + des_quat_w = quat_mul(asset.data.root_state_w[:, 3:7], des_quat_b) + curr_quat_w = asset.data.body_state_w[:, asset_cfg.body_ids[0], 3:7] # type: ignore return quat_error_magnitude(curr_quat_w, des_quat_w) diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/stack/mdp/franka_stack_events.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/stack/mdp/franka_stack_events.py index 5f4b1b9dac..3445d86d98 100644 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/stack/mdp/franka_stack_events.py +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/stack/mdp/franka_stack_events.py @@ -130,10 +130,10 @@ def randomize_object_pose( pose_tensor = torch.tensor([pose_list[i]], device=env.device) positions = pose_tensor[:, 0:3] + env.scene.env_origins[cur_env, 0:3] orientations = math_utils.quat_from_euler_xyz(pose_tensor[:, 3], pose_tensor[:, 4], pose_tensor[:, 5]) - asset.write_root_link_pose_to_sim( + asset.write_root_pose_to_sim( torch.cat([positions, orientations], dim=-1), env_ids=torch.tensor([cur_env], device=env.device) ) - asset.write_root_com_velocity_to_sim( + asset.write_root_velocity_to_sim( torch.zeros(1, 6, device=env.device), env_ids=torch.tensor([cur_env], device=env.device) ) diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/stack/mdp/observations.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/stack/mdp/observations.py index de4c9e0541..ff9a6adfd5 100644 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/stack/mdp/observations.py +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/stack/mdp/observations.py @@ -27,7 +27,7 @@ def cube_positions_in_world_frame( cube_2: RigidObject = env.scene[cube_2_cfg.name] cube_3: RigidObject = env.scene[cube_3_cfg.name] - return torch.cat((cube_1.data.root_link_pos_w, cube_2.data.root_link_pos_w, cube_3.data.root_link_pos_w), dim=1) + return torch.cat((cube_1.data.root_pos_w, cube_2.data.root_pos_w, cube_3.data.root_pos_w), dim=1) def instance_randomize_cube_positions_in_world_frame( @@ -69,7 +69,7 @@ def cube_orientations_in_world_frame( cube_2: RigidObject = env.scene[cube_2_cfg.name] cube_3: RigidObject = env.scene[cube_3_cfg.name] - return torch.cat((cube_1.data.root_link_quat_w, cube_2.data.root_link_quat_w, cube_3.data.root_link_quat_w), dim=1) + return torch.cat((cube_1.data.root_quat_w, cube_2.data.root_quat_w, cube_3.data.root_quat_w), dim=1) def instance_randomize_cube_orientations_in_world_frame( @@ -127,14 +127,14 @@ def object_obs( cube_3: RigidObject = env.scene[cube_3_cfg.name] ee_frame: FrameTransformer = env.scene[ee_frame_cfg.name] - cube_1_pos_w = cube_1.data.root_link_pos_w - cube_1_quat_w = cube_1.data.root_link_quat_w + cube_1_pos_w = cube_1.data.root_pos_w + cube_1_quat_w = cube_1.data.root_quat_w - cube_2_pos_w = cube_2.data.root_link_pos_w - cube_2_quat_w = cube_2.data.root_link_quat_w + cube_2_pos_w = cube_2.data.root_pos_w + cube_2_quat_w = cube_2.data.root_quat_w - cube_3_pos_w = cube_3.data.root_link_pos_w - cube_3_quat_w = cube_3.data.root_link_quat_w + cube_3_pos_w = cube_3.data.root_pos_w + cube_3_quat_w = cube_3.data.root_quat_w ee_pos_w = ee_frame.data.target_pos_w[:, 0, :] gripper_to_cube_1 = cube_1_pos_w - ee_pos_w @@ -279,7 +279,7 @@ def object_grasped( ee_frame: FrameTransformer = env.scene[ee_frame_cfg.name] object: RigidObject = env.scene[object_cfg.name] - object_pos = object.data.root_link_pos_w + object_pos = object.data.root_pos_w end_effector_pos = ee_frame.data.target_pos_w[:, 0, :] pose_diff = torch.linalg.vector_norm(object_pos - end_effector_pos, dim=1) @@ -310,7 +310,7 @@ def object_stacked( upper_object: RigidObject = env.scene[upper_object_cfg.name] lower_object: RigidObject = env.scene[lower_object_cfg.name] - pos_diff = upper_object.data.root_link_pos_w - lower_object.data.root_link_pos_w + pos_diff = upper_object.data.root_pos_w - lower_object.data.root_pos_w height_dist = torch.linalg.vector_norm(pos_diff[:, 2:], dim=1) xy_dist = torch.linalg.vector_norm(pos_diff[:, :2], dim=1) diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/stack/mdp/terminations.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/stack/mdp/terminations.py index 0239b8420d..abdefd0fd8 100644 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/stack/mdp/terminations.py +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/stack/mdp/terminations.py @@ -39,8 +39,8 @@ def cubes_stacked( cube_2: RigidObject = env.scene[cube_2_cfg.name] cube_3: RigidObject = env.scene[cube_3_cfg.name] - pos_diff_c12 = cube_1.data.root_link_pos_w - cube_2.data.root_link_pos_w - pos_diff_c23 = cube_2.data.root_link_pos_w - cube_3.data.root_link_pos_w + pos_diff_c12 = cube_1.data.root_pos_w - cube_2.data.root_pos_w + pos_diff_c23 = cube_2.data.root_pos_w - cube_3.data.root_pos_w # Compute cube position difference in x-y plane xy_dist_c12 = torch.norm(pos_diff_c12[:, :2], dim=1) diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/navigation/mdp/pre_trained_policy_action.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/navigation/mdp/pre_trained_policy_action.py index bf6dea3fb5..419a6eacb3 100644 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/navigation/mdp/pre_trained_policy_action.py +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/navigation/mdp/pre_trained_policy_action.py @@ -134,11 +134,11 @@ def _debug_vis_callback(self, event): return # get marker location # -- base state - base_pos_w = self.robot.data.root_link_pos_w.clone() + base_pos_w = self.robot.data.root_pos_w.clone() base_pos_w[:, 2] += 0.5 # -- resolve the scales and quaternions vel_des_arrow_scale, vel_des_arrow_quat = self._resolve_xy_velocity_to_arrow(self.raw_actions[:, :2]) - vel_arrow_scale, vel_arrow_quat = self._resolve_xy_velocity_to_arrow(self.robot.data.root_com_lin_vel_b[:, :2]) + vel_arrow_scale, vel_arrow_quat = self._resolve_xy_velocity_to_arrow(self.robot.data.root_lin_vel_b[:, :2]) # display markers self.base_vel_goal_visualizer.visualize(base_pos_w, vel_des_arrow_quat, vel_des_arrow_scale) self.base_vel_visualizer.visualize(base_pos_w, vel_arrow_quat, vel_arrow_scale) @@ -159,7 +159,7 @@ def _resolve_xy_velocity_to_arrow(self, xy_velocity: torch.Tensor) -> tuple[torc zeros = torch.zeros_like(heading_angle) arrow_quat = math_utils.quat_from_euler_xyz(zeros, zeros, heading_angle) # convert everything back from base to world frame - base_quat_w = self.robot.data.root_link_quat_w + base_quat_w = self.robot.data.root_quat_w arrow_quat = math_utils.quat_mul(base_quat_w, arrow_quat) return arrow_scale, arrow_quat diff --git a/source/standalone/benchmarks/benchmark_load_robot.py b/source/standalone/benchmarks/benchmark_load_robot.py index 33d9bb95d2..1f6ea3ef10 100644 --- a/source/standalone/benchmarks/benchmark_load_robot.py +++ b/source/standalone/benchmarks/benchmark_load_robot.py @@ -115,8 +115,8 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): # if this is not done, then the robots will be spawned at the (0, 0, 0) of the simulation world root_state = robot.data.default_root_state.clone() root_state[:, :3] += scene.env_origins - robot.write_root_link_pose_to_sim(root_state[:, :7]) - robot.write_root_com_velocity_to_sim(root_state[:, 7:]) + robot.write_root_pose_to_sim(root_state[:, :7]) + robot.write_root_velocity_to_sim(root_state[:, 7:]) # set joint positions with some noise joint_pos, joint_vel = robot.data.default_joint_pos.clone(), robot.data.default_joint_vel.clone() joint_pos += torch.rand_like(joint_pos) * 0.1 diff --git a/source/standalone/demos/arms.py b/source/standalone/demos/arms.py index 8b0a3121a1..9feefcdc0d 100644 --- a/source/standalone/demos/arms.py +++ b/source/standalone/demos/arms.py @@ -179,8 +179,8 @@ def run_simulator(sim: sim_utils.SimulationContext, entities: dict[str, Articula # root state root_state = robot.data.default_root_state.clone() root_state[:, :3] += origins[index] - robot.write_root_link_pose_to_sim(root_state[:, :7]) - robot.write_root_com_velocity_to_sim(root_state[:, 7:]) + robot.write_root_pose_to_sim(root_state[:, :7]) + robot.write_root_velocity_to_sim(root_state[:, 7:]) # set joint positions joint_pos, joint_vel = robot.data.default_joint_pos.clone(), robot.data.default_joint_vel.clone() robot.write_joint_state_to_sim(joint_pos, joint_vel) diff --git a/source/standalone/demos/bipeds.py b/source/standalone/demos/bipeds.py index e7378e7046..56c566e744 100644 --- a/source/standalone/demos/bipeds.py +++ b/source/standalone/demos/bipeds.py @@ -97,8 +97,8 @@ def main(): robot.write_joint_state_to_sim(joint_pos, joint_vel) root_state = robot.data.default_root_state.clone() root_state[:, :3] += origins[index] - robot.write_root_link_pose_to_sim(root_state[:, :7]) - robot.write_root_com_velocity_to_sim(root_state[:, 7:]) + robot.write_root_pose_to_sim(root_state[:, :7]) + robot.write_root_velocity_to_sim(root_state[:, 7:]) robot.reset() # reset command print(">>>>>>>> Reset!") diff --git a/source/standalone/demos/cameras.py b/source/standalone/demos/cameras.py index 01c9fee276..b98e4abefd 100644 --- a/source/standalone/demos/cameras.py +++ b/source/standalone/demos/cameras.py @@ -187,8 +187,8 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): # if this is not done, then the robots will be spawned at the (0, 0, 0) of the simulation world root_state = scene["robot"].data.default_root_state.clone() root_state[:, :3] += scene.env_origins - scene["robot"].write_root_link_pose_to_sim(root_state[:, :7]) - scene["robot"].write_root_com_velocity_to_sim(root_state[:, 7:]) + scene["robot"].write_root_pose_to_sim(root_state[:, :7]) + scene["robot"].write_root_velocity_to_sim(root_state[:, 7:]) # set joint positions with some noise joint_pos, joint_vel = ( scene["robot"].data.default_joint_pos.clone(), diff --git a/source/standalone/demos/hands.py b/source/standalone/demos/hands.py index bad16869b6..0e121de735 100644 --- a/source/standalone/demos/hands.py +++ b/source/standalone/demos/hands.py @@ -113,8 +113,8 @@ def run_simulator(sim: sim_utils.SimulationContext, entities: dict[str, Articula # root state root_state = robot.data.default_root_state.clone() root_state[:, :3] += origins[index] - robot.write_root_link_pose_to_sim(root_state[:, :7]) - robot.write_root_com_velocity_to_sim(root_state[:, 7:]) + robot.write_root_pose_to_sim(root_state[:, :7]) + robot.write_root_velocity_to_sim(root_state[:, 7:]) # joint state joint_pos, joint_vel = robot.data.default_joint_pos.clone(), robot.data.default_joint_vel.clone() robot.write_joint_state_to_sim(joint_pos, joint_vel) diff --git a/source/standalone/demos/multi_asset.py b/source/standalone/demos/multi_asset.py index 7426e8b9fe..d237e2657b 100644 --- a/source/standalone/demos/multi_asset.py +++ b/source/standalone/demos/multi_asset.py @@ -240,8 +240,8 @@ def run_simulator(sim: SimulationContext, scene: InteractiveScene): # object root_state = rigid_object.data.default_root_state.clone() root_state[:, :3] += scene.env_origins - rigid_object.write_root_link_pose_to_sim(root_state[:, :7]) - rigid_object.write_root_com_velocity_to_sim(root_state[:, 7:]) + rigid_object.write_root_pose_to_sim(root_state[:, :7]) + rigid_object.write_root_velocity_to_sim(root_state[:, 7:]) # object collection object_state = rigid_object_collection.data.default_object_state.clone() object_state[..., :3] += scene.env_origins.unsqueeze(1) @@ -251,8 +251,8 @@ def run_simulator(sim: SimulationContext, scene: InteractiveScene): # -- root state root_state = robot.data.default_root_state.clone() root_state[:, :3] += scene.env_origins - robot.write_root_link_pose_to_sim(root_state[:, :7]) - robot.write_root_com_velocity_to_sim(root_state[:, 7:]) + robot.write_root_pose_to_sim(root_state[:, :7]) + robot.write_root_velocity_to_sim(root_state[:, 7:]) # -- joint state joint_pos, joint_vel = robot.data.default_joint_pos.clone(), robot.data.default_joint_vel.clone() robot.write_joint_state_to_sim(joint_pos, joint_vel) diff --git a/source/standalone/demos/quadcopter.py b/source/standalone/demos/quadcopter.py index 221493a708..2f5eba6044 100644 --- a/source/standalone/demos/quadcopter.py +++ b/source/standalone/demos/quadcopter.py @@ -91,8 +91,8 @@ def main(): # reset dof state joint_pos, joint_vel = robot.data.default_joint_pos, robot.data.default_joint_vel robot.write_joint_state_to_sim(joint_pos, joint_vel) - robot.write_root_link_pose_to_sim(robot.data.default_root_state[:, :7]) - robot.write_root_com_velocity_to_sim(robot.data.default_root_state[:, 7:]) + robot.write_root_pose_to_sim(robot.data.default_root_state[:, :7]) + robot.write_root_velocity_to_sim(robot.data.default_root_state[:, 7:]) robot.reset() # reset command print(">>>>>>>> Reset!") diff --git a/source/standalone/demos/quadrupeds.py b/source/standalone/demos/quadrupeds.py index 41ac4e1d10..db9d261d8f 100644 --- a/source/standalone/demos/quadrupeds.py +++ b/source/standalone/demos/quadrupeds.py @@ -142,8 +142,8 @@ def run_simulator(sim: sim_utils.SimulationContext, entities: dict[str, Articula # root state root_state = robot.data.default_root_state.clone() root_state[:, :3] += origins[index] - robot.write_root_link_pose_to_sim(root_state[:, :7]) - robot.write_root_com_velocity_to_sim(root_state[:, 7:]) + robot.write_root_pose_to_sim(root_state[:, :7]) + robot.write_root_velocity_to_sim(root_state[:, 7:]) # joint state joint_pos, joint_vel = robot.data.default_joint_pos.clone(), robot.data.default_joint_vel.clone() robot.write_joint_state_to_sim(joint_pos, joint_vel) diff --git a/source/standalone/demos/sensors/contact_sensor.py b/source/standalone/demos/sensors/contact_sensor.py index 1270e3eae9..85c87549a4 100644 --- a/source/standalone/demos/sensors/contact_sensor.py +++ b/source/standalone/demos/sensors/contact_sensor.py @@ -109,8 +109,8 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): # if this is not done, then the robots will be spawned at the (0, 0, 0) of the simulation world root_state = scene["robot"].data.default_root_state.clone() root_state[:, :3] += scene.env_origins - scene["robot"].write_root_link_pose_to_sim(root_state[:, :7]) - scene["robot"].write_root_com_velocity_to_sim(root_state[:, 7:]) + scene["robot"].write_root_pose_to_sim(root_state[:, :7]) + scene["robot"].write_root_velocity_to_sim(root_state[:, 7:]) # set joint positions with some noise joint_pos, joint_vel = ( scene["robot"].data.default_joint_pos.clone(), diff --git a/source/standalone/demos/sensors/frame_transformer_sensor.py b/source/standalone/demos/sensors/frame_transformer_sensor.py index 67a7898e91..ab5b41f6cc 100644 --- a/source/standalone/demos/sensors/frame_transformer_sensor.py +++ b/source/standalone/demos/sensors/frame_transformer_sensor.py @@ -105,8 +105,8 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): # if this is not done, then the robots will be spawned at the (0, 0, 0) of the simulation world root_state = scene["robot"].data.default_root_state.clone() root_state[:, :3] += scene.env_origins - scene["robot"].write_root_link_pose_to_sim(root_state[:, :7]) - scene["robot"].write_root_com_velocity_to_sim(root_state[:, 7:]) + scene["robot"].write_root_pose_to_sim(root_state[:, :7]) + scene["robot"].write_root_velocity_to_sim(root_state[:, 7:]) # set joint positions with some noise joint_pos, joint_vel = ( scene["robot"].data.default_joint_pos.clone(), diff --git a/source/standalone/demos/sensors/raycaster_sensor.py b/source/standalone/demos/sensors/raycaster_sensor.py index 7a0e3dcd8d..9eca25c5bc 100644 --- a/source/standalone/demos/sensors/raycaster_sensor.py +++ b/source/standalone/demos/sensors/raycaster_sensor.py @@ -93,8 +93,8 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): # if this is not done, then the robots will be spawned at the (0, 0, 0) of the simulation world root_state = scene["robot"].data.default_root_state.clone() root_state[:, :3] += scene.env_origins - scene["robot"].write_root_link_pose_to_sim(root_state[:, :7]) - scene["robot"].write_root_com_velocity_to_sim(root_state[:, 7:]) + scene["robot"].write_root_pose_to_sim(root_state[:, :7]) + scene["robot"].write_root_velocity_to_sim(root_state[:, 7:]) # set joint positions with some noise joint_pos, joint_vel = ( scene["robot"].data.default_joint_pos.clone(), diff --git a/source/standalone/environments/state_machine/lift_cube_sm.py b/source/standalone/environments/state_machine/lift_cube_sm.py index f146f8f863..82b565fc10 100644 --- a/source/standalone/environments/state_machine/lift_cube_sm.py +++ b/source/standalone/environments/state_machine/lift_cube_sm.py @@ -293,7 +293,7 @@ def main(): tcp_rest_orientation = ee_frame_sensor.data.target_quat_w[..., 0, :].clone() # -- object frame object_data: RigidObjectData = env.unwrapped.scene["object"].data - object_position = object_data.root_link_pos_w - env.unwrapped.scene.env_origins + object_position = object_data.root_pos_w - env.unwrapped.scene.env_origins # -- target object frame desired_position = env.unwrapped.command_manager.get_command("object_pose")[..., :3] diff --git a/source/standalone/tutorials/01_assets/run_articulation.py b/source/standalone/tutorials/01_assets/run_articulation.py index 8a1026bdfd..1c52b15511 100644 --- a/source/standalone/tutorials/01_assets/run_articulation.py +++ b/source/standalone/tutorials/01_assets/run_articulation.py @@ -94,8 +94,8 @@ def run_simulator(sim: sim_utils.SimulationContext, entities: dict[str, Articula # if this is not done, then the robots will be spawned at the (0, 0, 0) of the simulation world root_state = robot.data.default_root_state.clone() root_state[:, :3] += origins - robot.write_root_link_pose_to_sim(root_state[:, :7]) - robot.write_root_com_velocity_to_sim(root_state[:, 7:]) + robot.write_root_pose_to_sim(root_state[:, :7]) + robot.write_root_velocity_to_sim(root_state[:, 7:]) # set joint positions with some noise joint_pos, joint_vel = robot.data.default_joint_pos.clone(), robot.data.default_joint_vel.clone() joint_pos += torch.rand_like(joint_pos) * 0.1 diff --git a/source/standalone/tutorials/01_assets/run_rigid_object.py b/source/standalone/tutorials/01_assets/run_rigid_object.py index 32a6dd9cad..6f5263948b 100644 --- a/source/standalone/tutorials/01_assets/run_rigid_object.py +++ b/source/standalone/tutorials/01_assets/run_rigid_object.py @@ -103,8 +103,8 @@ def run_simulator(sim: sim_utils.SimulationContext, entities: dict[str, RigidObj radius=0.1, h_range=(0.25, 0.5), size=cone_object.num_instances, device=cone_object.device ) # write root state to simulation - cone_object.write_root_link_pose_to_sim(root_state[:, :7]) - cone_object.write_root_com_velocity_to_sim(root_state[:, 7:]) + cone_object.write_root_pose_to_sim(root_state[:, :7]) + cone_object.write_root_velocity_to_sim(root_state[:, 7:]) # reset buffers cone_object.reset() print("----------------------------------------") @@ -120,7 +120,7 @@ def run_simulator(sim: sim_utils.SimulationContext, entities: dict[str, RigidObj cone_object.update(sim_dt) # print the root position if count % 50 == 0: - print(f"Root position (in world): {cone_object.data.root_link_state_w[:, :3]}") + print(f"Root position (in world): {cone_object.data.root_state_w[:, :3]}") def main(): diff --git a/source/standalone/tutorials/02_scene/create_scene.py b/source/standalone/tutorials/02_scene/create_scene.py index d14281cea8..759a6b7166 100644 --- a/source/standalone/tutorials/02_scene/create_scene.py +++ b/source/standalone/tutorials/02_scene/create_scene.py @@ -83,8 +83,8 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): # if this is not done, then the robots will be spawned at the (0, 0, 0) of the simulation world root_state = robot.data.default_root_state.clone() root_state[:, :3] += scene.env_origins - robot.write_root_link_pose_to_sim(root_state[:, :7]) - robot.write_root_com_velocity_to_sim(root_state[:, 7:]) + robot.write_root_pose_to_sim(root_state[:, :7]) + robot.write_root_velocity_to_sim(root_state[:, 7:]) # set joint positions with some noise joint_pos, joint_vel = robot.data.default_joint_pos.clone(), robot.data.default_joint_vel.clone() joint_pos += torch.rand_like(joint_pos) * 0.1 diff --git a/source/standalone/tutorials/03_envs/create_cube_base_env.py b/source/standalone/tutorials/03_envs/create_cube_base_env.py index b6d30d8b4d..3f2161cc26 100644 --- a/source/standalone/tutorials/03_envs/create_cube_base_env.py +++ b/source/standalone/tutorials/03_envs/create_cube_base_env.py @@ -120,11 +120,11 @@ def process_actions(self, actions: torch.Tensor): def apply_actions(self): # implement a PD controller to track the target position - pos_error = self._processed_actions - (self._asset.data.root_link_pos_w - self._env.scene.env_origins) - vel_error = -self._asset.data.root_com_lin_vel_w + pos_error = self._processed_actions - (self._asset.data.root_pos_w - self._env.scene.env_origins) + vel_error = -self._asset.data.root_lin_vel_w # set velocity targets self._vel_command[:, :3] = self.p_gain * pos_error + self.d_gain * vel_error - self._asset.write_root_com_velocity_to_sim(self._vel_command) + self._asset.write_root_velocity_to_sim(self._vel_command) @configclass @@ -149,7 +149,7 @@ def base_position(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg) -> torch.Tens """Root linear velocity in the asset's root frame.""" # extract the used quantities (to enable type-hinting) asset: RigidObject = env.scene[asset_cfg.name] - return asset.data.root_link_pos_w - env.scene.env_origins + return asset.data.root_pos_w - env.scene.env_origins ## diff --git a/source/standalone/tutorials/04_sensors/add_sensors_on_robot.py b/source/standalone/tutorials/04_sensors/add_sensors_on_robot.py index 43356465b8..2ee5593920 100644 --- a/source/standalone/tutorials/04_sensors/add_sensors_on_robot.py +++ b/source/standalone/tutorials/04_sensors/add_sensors_on_robot.py @@ -113,8 +113,8 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): # if this is not done, then the robots will be spawned at the (0, 0, 0) of the simulation world root_state = scene["robot"].data.default_root_state.clone() root_state[:, :3] += scene.env_origins - scene["robot"].write_root_link_pose_to_sim(root_state[:, :7]) - scene["robot"].write_root_com_velocity_to_sim(root_state[:, 7:]) + scene["robot"].write_root_pose_to_sim(root_state[:, :7]) + scene["robot"].write_root_velocity_to_sim(root_state[:, 7:]) # set joint positions with some noise joint_pos, joint_vel = ( scene["robot"].data.default_joint_pos.clone(), diff --git a/source/standalone/tutorials/04_sensors/run_ray_caster.py b/source/standalone/tutorials/04_sensors/run_ray_caster.py index 10b593e468..2c31c55a0a 100644 --- a/source/standalone/tutorials/04_sensors/run_ray_caster.py +++ b/source/standalone/tutorials/04_sensors/run_ray_caster.py @@ -109,8 +109,8 @@ def run_simulator(sim: sim_utils.SimulationContext, scene_entities: dict): # Reset the scene if step_count % 250 == 0: # reset the balls - balls.write_root_link_pose_to_sim(ball_default_state[:, :7]) - balls.write_root_com_velocity_to_sim(ball_default_state[:, 7:]) + balls.write_root_pose_to_sim(ball_default_state[:, :7]) + balls.write_root_velocity_to_sim(ball_default_state[:, 7:]) # reset the sensor ray_caster.reset() # reset the counter diff --git a/source/standalone/tutorials/05_controllers/run_diff_ik.py b/source/standalone/tutorials/05_controllers/run_diff_ik.py index 4c7b0dea6c..a6d788e58a 100644 --- a/source/standalone/tutorials/05_controllers/run_diff_ik.py +++ b/source/standalone/tutorials/05_controllers/run_diff_ik.py @@ -160,8 +160,8 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): else: # obtain quantities from simulation jacobian = robot.root_physx_view.get_jacobians()[:, ee_jacobi_idx, :, robot_entity_cfg.joint_ids] - ee_pose_w = robot.data.body_link_state_w[:, robot_entity_cfg.body_ids[0], 0:7] - root_pose_w = robot.data.root_link_state_w[:, 0:7] + ee_pose_w = robot.data.body_state_w[:, robot_entity_cfg.body_ids[0], 0:7] + root_pose_w = robot.data.root_state_w[:, 0:7] joint_pos = robot.data.joint_pos[:, robot_entity_cfg.joint_ids] # compute frame in root frame ee_pos_b, ee_quat_b = subtract_frame_transforms( @@ -181,7 +181,7 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): scene.update(sim_dt) # obtain quantities from simulation - ee_pose_w = robot.data.body_link_state_w[:, robot_entity_cfg.body_ids[0], 0:7] + ee_pose_w = robot.data.body_state_w[:, robot_entity_cfg.body_ids[0], 0:7] # update marker positions ee_marker.visualize(ee_pose_w[:, 0:3], ee_pose_w[:, 3:7]) goal_marker.visualize(ik_commands[:, 0:3] + scene.env_origins, ik_commands[:, 3:7]) diff --git a/source/standalone/tutorials/05_controllers/run_osc.py b/source/standalone/tutorials/05_controllers/run_osc.py index 79f3221ce3..e9a31343bb 100644 --- a/source/standalone/tutorials/05_controllers/run_osc.py +++ b/source/standalone/tutorials/05_controllers/run_osc.py @@ -318,26 +318,26 @@ def update_states( gravity = robot.root_physx_view.get_generalized_gravity_forces()[:, arm_joint_ids] # Convert the Jacobian from world to root frame jacobian_b = jacobian_w.clone() - root_rot_matrix = matrix_from_quat(quat_inv(robot.data.root_link_quat_w)) + root_rot_matrix = matrix_from_quat(quat_inv(robot.data.root_quat_w)) jacobian_b[:, :3, :] = torch.bmm(root_rot_matrix, jacobian_b[:, :3, :]) jacobian_b[:, 3:, :] = torch.bmm(root_rot_matrix, jacobian_b[:, 3:, :]) # Compute current pose of the end-effector - root_pos_w = robot.data.root_link_pos_w - root_quat_w = robot.data.root_link_quat_w - ee_pos_w = robot.data.body_link_pos_w[:, ee_frame_idx] - ee_quat_w = robot.data.body_link_quat_w[:, ee_frame_idx] + root_pos_w = robot.data.root_pos_w + root_quat_w = robot.data.root_quat_w + ee_pos_w = robot.data.body_pos_w[:, ee_frame_idx] + ee_quat_w = robot.data.body_quat_w[:, ee_frame_idx] ee_pos_b, ee_quat_b = subtract_frame_transforms(root_pos_w, root_quat_w, ee_pos_w, ee_quat_w) root_pose_w = torch.cat([root_pos_w, root_quat_w], dim=-1) ee_pose_w = torch.cat([ee_pos_w, ee_quat_w], dim=-1) ee_pose_b = torch.cat([ee_pos_b, ee_quat_b], dim=-1) # Compute the current velocity of the end-effector - ee_vel_w = robot.data.body_com_vel_w[:, ee_frame_idx, :] # Extract end-effector velocity in the world frame - root_vel_w = robot.data.root_com_vel_w # Extract root velocity in the world frame + ee_vel_w = robot.data.body_vel_w[:, ee_frame_idx, :] # Extract end-effector velocity in the world frame + root_vel_w = robot.data.root_vel_w # Extract root velocity in the world frame relative_vel_w = ee_vel_w - root_vel_w # Compute the relative velocity in the world frame - ee_lin_vel_b = quat_rotate_inverse(robot.data.root_link_quat_w, relative_vel_w[:, 0:3]) # From world to root frame - ee_ang_vel_b = quat_rotate_inverse(robot.data.root_link_quat_w, relative_vel_w[:, 3:6]) + ee_lin_vel_b = quat_rotate_inverse(robot.data.root_quat_w, relative_vel_w[:, 0:3]) # From world to root frame + ee_ang_vel_b = quat_rotate_inverse(robot.data.root_quat_w, relative_vel_w[:, 3:6]) ee_vel_b = torch.cat([ee_lin_vel_b, ee_ang_vel_b], dim=-1) # Calculate the contact force