Skip to content

Commit

Permalink
change calls to deprecated properties to the appropriate link or com …
Browse files Browse the repository at this point in the history
…properties
  • Loading branch information
jtigue-bdai committed Oct 11, 2024
1 parent ac0bbdd commit 490ce5b
Show file tree
Hide file tree
Showing 46 changed files with 215 additions and 223 deletions.
2 changes: 1 addition & 1 deletion docs/source/tutorials/05_controllers/run_diff_ik.rst
Original file line number Diff line number Diff line change
Expand Up @@ -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_state_w` provides the state of all the
the attribute :attr:`assets.ArticulationData.body_link_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.

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -351,9 +351,9 @@ def write_root_link_pose_to_sim(self, root_pose: torch.Tensor, env_ids: Sequence
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()
self._data.root_link_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 = self._data.root_link_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_root_transforms(root_poses_xyzw, indices=physx_env_ids)
Expand Down Expand Up @@ -421,10 +421,10 @@ def write_root_com_velocity_to_sim(self, root_velocity: torch.Tensor, env_ids: S
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.root_com_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)
self.root_physx_view.set_root_velocities(self._data.root_com_state_w[:, 7:], indices=physx_env_ids)

def write_root_link_velocity_to_sim(self, root_velocity: torch.Tensor, env_ids: Sequence[int] | None = None):
"""Set the root link velocity over selected environment indices into the simulation.
Expand All @@ -441,7 +441,7 @@ def write_root_link_velocity_to_sim(self, root_velocity: torch.Tensor, env_ids:
local_env_ids = slice(None)

root_com_velocity = root_velocity.clone()
quat = self.data.root_state_w[local_env_ids, 3:7]
quat = self.data.root_link_state_w[local_env_ids, 3:7]
com_pos_b = self.data.com_pos_b[local_env_ids, 0, :]
# transform given velocity to center of mass
root_com_velocity[:, :3] += torch.linalg.cross(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -3,21 +3,16 @@
#
# SPDX-License-Identifier: BSD-3-Clause

import carb
import torch
import weakref

import carb
import omni.physics.tensors.impl.api as physx

import omni.isaac.lab.utils.math as math_utils
from omni.isaac.lab.utils.buffers import TimestampedBuffer


def single_deprecation_warning(dep_msg: str):
"""Single use deprecation warning"""
carb.log_warn("DeprecationWarning: " + dep_msg)


class ArticulationData:
"""Data container for an articulation.
Expand Down Expand Up @@ -212,7 +207,7 @@ def update(self, dt: float):
"""Joint stiffness provided to simulation. Shape is (num_instances, num_joints)."""

joint_damping: torch.Tensor = None
"""Joint damping provided to simulation. Shape is (num_instanbody_state_wnstances, num_joints)."""
"""Joint damping provided to simulation. Shape is (num_instances, num_joints)."""

joint_limits: torch.Tensor = None
"""Joint limits provided to simulation. Shape is (num_instances, num_joints, 2)."""
Expand Down Expand Up @@ -264,10 +259,7 @@ def root_state_w(self):
the linear and angular velocities are of the articulation root's center of mass frame.
"""

carb.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."
)
carb.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.")

if self._root_state_w.timestamp < self._sim_timestamp:
# read data from simulation
Expand Down Expand Up @@ -300,7 +292,7 @@ def root_link_state_w(self):
self._root_link_state_w.data = torch.cat((pose, velocity), dim=-1)
self._root_link_state_w.timestamp = self._sim_timestamp

return self._root_link_state_w.data
return self._root_link_state_w.data

@property
def root_com_state_w(self):
Expand All @@ -317,14 +309,15 @@ def root_com_state_w(self):
velocity = self._root_physx_view.get_root_velocities()

# adjust pose to center of mass
pos, quat = math_utils.combine_frame_transforms(
pose[:, :3], pose[:, 3:7], self.com_pos_b[:, 0, :], self.com_quat_b[:, 0, :]
)
pose = torch.cat((pos, quat), dim=-1)
pos, quat = math_utils.combine_frame_transforms(pose[:,:3],
pose[:,3:7],
self.com_pos_b[:,0,:],
self.com_quat_b[:,0,:])
pose = torch.cat((pos,quat),dim=-1)
# set the buffer data and timestamp
self._root_com_state_w.data = torch.cat((pose, velocity), dim=-1)
self._root_com_state_w.timestamp = self._sim_timestamp
return self._root_com_state_w.data
return self._root_com_state_w.data

@property
def body_state_w(self):
Expand All @@ -335,11 +328,8 @@ def body_state_w(self):
velocities are of the articulation links's center of mass frame.
"""

carb.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 bodt_com_state_w."
)

carb.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 bodt_com_state_w.")

if self._body_state_w.timestamp < self._sim_timestamp:
# read data from simulation
poses = self._root_physx_view.get_link_transforms().clone()
Expand Down Expand Up @@ -370,7 +360,7 @@ def body_link_state_w(self):
# set the buffer data and timestamp
self._body_link_state_w.data = torch.cat((pose, velocity), dim=-1)
self._body_link_state_w.timestamp = self._sim_timestamp

return self._body_link_state_w.data

@property
Expand All @@ -389,14 +379,15 @@ def body_com_state_w(self):
velocity = self._root_physx_view.get_link_velocities()

# adjust pose to center of mass
pos, quat = math_utils.combine_frame_transforms(
pose[..., :3], pose[..., 3:7], self.com_pos_b, self.com_quat_b
)
pose = torch.cat((pos, quat), dim=-1)
pos, quat = math_utils.combine_frame_transforms(pose[...,:3],
pose[...,3:7],
self.com_pos_b,
self.com_quat_b)
pose = torch.cat((pos,quat),dim=-1)
# set the buffer data and timestamp
self._body_com_state_w.data = torch.cat((pose, velocity), dim=-1)
self._body_com_state_w.timestamp = self._sim_timestamp
return self._body_com_state_w.data
return self._body_com_state_w.data

@property
def body_acc_w(self):
Expand All @@ -414,7 +405,7 @@ def body_acc_w(self):
@property
def projected_gravity_b(self):
"""Projection of the gravity direction on base frame. Shape is (num_instances, 3)."""
return math_utils.quat_rotate_inverse(self.root_quat_w, self.GRAVITY_VEC_W)
return math_utils.quat_rotate_inverse(self.root_link_quat_w, self.GRAVITY_VEC_W)

@property
def heading_w(self):
Expand All @@ -424,7 +415,7 @@ def heading_w(self):
This quantity is computed by assuming that the forward-direction of the base
frame is along x-direction, i.e. :math:`(1, 0, 0)`.
"""
forward_w = math_utils.quat_apply(self.root_quat_w, self.FORWARD_VEC_B)
forward_w = math_utils.quat_apply(self.root_link_quat_w, self.FORWARD_VEC_B)
return torch.atan2(forward_w[:, 1], forward_w[:, 0])

@property
Expand Down Expand Up @@ -544,7 +535,7 @@ def root_link_quat_w(self) -> torch.Tensor:
def root_link_vel_w(self) -> torch.Tensor:
"""Root link velocity in simulation world frame. Shape is (num_instances, 6).
This quantity contains the linear and angular velocities of the actor frame of the root
This quantity contains the linear and angular velocities of the actor frame of the root
rigid body relative to the world.
"""
return self.root_link_state_w[:, 7:13]
Expand Down Expand Up @@ -582,10 +573,10 @@ def root_link_ang_vel_b(self) -> torch.Tensor:
rigid body's actor frame.
"""
return math_utils.quat_rotate_inverse(self.root_link_quat_w, self.root_link_ang_vel_w)

#
# Root Center of Mass state properties
#
#
# Root Center of Mass state properties
#

@property
def root_com_pos_w(self) -> torch.Tensor:
Expand Down Expand Up @@ -702,9 +693,10 @@ def body_ang_acc_w(self) -> torch.Tensor:
"""
return self.body_acc_w[..., 3:6]

#
# Link body properties
#

#
# Link body properties
#

@property
def body_link_pos_w(self) -> torch.Tensor:
Expand All @@ -726,7 +718,7 @@ def body_link_quat_w(self) -> torch.Tensor:
def body_link_vel_w(self) -> torch.Tensor:
"""Velocity of all bodies in simulation world frame. Shape is (num_instances, 1, 6).
This quantity contains the linear and angular velocities of the rigid bodies' center of mass frame
This quantity contains the linear and angular velocities of the rigid bodies' center of mass frame
relative to the world.
"""
return self.body_link_state_w[..., 7:13]
Expand All @@ -746,10 +738,10 @@ def body_link_ang_vel_w(self) -> torch.Tensor:
This quantity is the angular velocity of the rigid bodies' center of mass frame relative to the world.
"""
return self.body_link_state_w[..., 10:13]

#
# Center of mass body properties
#
#
# Center of mass body properties
#

@property
def body_com_pos_w(self) -> torch.Tensor:
Expand All @@ -761,8 +753,8 @@ def body_com_pos_w(self) -> torch.Tensor:

@property
def body_com_quat_w(self) -> torch.Tensor:
"""Orientation (w, x, y, z) of the prinicple axies of inertia of all bodies in simulation world frame.
"""Orientation (w, x, y, z) of the prinicple axies of inertia of all bodies in simulation world frame.
Shape is (num_instances, 1, 4). This quantity is the orientation of the rigid bodies' actor frame.
"""
return self.body_com_state_w[..., 3:7]
Expand Down Expand Up @@ -794,16 +786,16 @@ def body_com_ang_vel_w(self) -> torch.Tensor:
@property
def com_pos_b(self) -> torch.Tensor:
"""Center of mass of all of the bodies in simulation world frame. Shape is (num_instances, num_bodies, 3).
This quantity is the center of mass location relative to its body frame.
"""
return self._root_physx_view.get_coms().to(self.device)[..., :3]

return self._root_physx_view.get_coms().to(self.device)[...,:3]
@property
def com_quat_b(self) -> torch.Tensor:
"""Orientation (w,x,y,z) of the prinicple axies of inertia of all of the bodies in simulation world frame. Shape is (num_instances, num_bodies, 4).
This quantity is the orientation of the principles axes of inertia relative to its body frame.
"""
quat = self._root_physx_view.get_coms().to(self.device)[..., 3:7]
return math_utils.convert_quat(quat, to="wxyz")
quat = self._root_physx_view.get_coms().to(self.device)[...,3:7]
return math_utils.convert_quat(quat, to="wxyz")
Original file line number Diff line number Diff line change
Expand Up @@ -227,9 +227,9 @@ def write_root_link_pose_to_sim(self, root_pose: torch.Tensor, env_ids: Sequence
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()
self._data.root_link_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 = self._data.root_link_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)
Expand Down Expand Up @@ -298,10 +298,10 @@ def write_root_com_velocity_to_sim(self, root_velocity: torch.Tensor, env_ids: S
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.root_com_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)
self.root_physx_view.set_velocities(self._data.root_com_state_w[:, 7:], indices=physx_env_ids)

def write_root_link_velocity_to_sim(self, root_velocity: torch.Tensor, env_ids: Sequence[int] | None = None):
"""Set the root link velocity over selected environment indices into the simulation.
Expand All @@ -318,7 +318,7 @@ def write_root_link_velocity_to_sim(self, root_velocity: torch.Tensor, env_ids:
local_env_ids = slice(None)

root_com_velocity = root_velocity.clone()
quat = self.data.root_state_w[local_env_ids, 3:7]
quat = self.data.root_link_state_w[local_env_ids, 3:7]
com_pos_b = self.data.com_pos_b[local_env_ids, 0, :]
# transform given velocity to center of mass
root_com_velocity[:, :3] += torch.linalg.cross(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -221,7 +221,7 @@ def body_acc_w(self):
@property
def projected_gravity_b(self):
"""Projection of the gravity direction on base frame. Shape is (num_instances, 3)."""
return math_utils.quat_rotate_inverse(self.root_quat_w, self.GRAVITY_VEC_W)
return math_utils.quat_rotate_inverse(self.root_link_quat_w, self.GRAVITY_VEC_W)

@property
def heading_w(self):
Expand All @@ -231,7 +231,7 @@ def heading_w(self):
This quantity is computed by assuming that the forward-direction of the base
frame is along x-direction, i.e. :math:`(1, 0, 0)`.
"""
forward_w = math_utils.quat_apply(self.root_quat_w, self.FORWARD_VEC_B)
forward_w = math_utils.quat_apply(self.root_link_quat_w, self.FORWARD_VEC_B)
return torch.atan2(forward_w[:, 1], forward_w[:, 0])

##
Expand Down Expand Up @@ -285,7 +285,7 @@ def root_lin_vel_b(self) -> torch.Tensor:
This quantity is the linear velocity of the root rigid body's center of mass frame with respect to the
rigid body's actor frame.
"""
return math_utils.quat_rotate_inverse(self.root_quat_w, self.root_lin_vel_w)
return math_utils.quat_rotate_inverse(self.root_link_quat_w, self.root_lin_vel_w)

@property
def root_ang_vel_b(self) -> torch.Tensor:
Expand All @@ -294,7 +294,7 @@ def root_ang_vel_b(self) -> torch.Tensor:
This quantity is the angular velocity of the root rigid body's center of mass frame with respect to the
rigid body's actor frame.
"""
return math_utils.quat_rotate_inverse(self.root_quat_w, self.root_ang_vel_w)
return math_utils.quat_rotate_inverse(self.root_link_quat_w, self.root_ang_vel_w)

@property
def root_link_pos_w(self) -> torch.Tensor:
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -132,7 +132,7 @@ def process_actions(self, actions):

def apply_actions(self):
# obtain current heading
quat_w = self._asset.data.body_quat_w[:, self._body_idx]
quat_w = self._asset.data.body_link_quat_w[:, self._body_idx]
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
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -155,8 +155,8 @@ 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_pose_w = self._asset.data.body_state_w[:, self._body_idx, :7]
root_pose_w = self._asset.data.root_state_w[:, :7]
ee_pose_w = self._asset.data.body_link_state_w[:, self._body_idx, :7]
root_pose_w = self._asset.data.root_link_state_w[:, :7]
# compute the pose of the body in the root frame
ee_pose_b, ee_quat_b = math_utils.subtract_frame_transforms(
root_pose_w[:, 0:3], root_pose_w[:, 3:7], ee_pose_w[:, 0:3], ee_pose_w[:, 3:7]
Expand Down
Loading

0 comments on commit 490ce5b

Please sign in to comment.