Skip to content

Commit

Permalink
Merge branch 'main' into main
Browse files Browse the repository at this point in the history
  • Loading branch information
gursi26 authored Jan 15, 2025
2 parents 3d7d097 + e956ddb commit 1894583
Show file tree
Hide file tree
Showing 11 changed files with 71 additions and 26 deletions.
1 change: 1 addition & 0 deletions .pre-commit-config.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,7 @@ repos:
- id: codespell
additional_dependencies:
- tomli
exclude: "CONTRIBUTORS.md"
# FIXME: Figure out why this is getting stuck under VPN.
# - repo: https://github.com/RobertCraigie/pyright-python
# rev: v1.1.315
Expand Down
1 change: 1 addition & 0 deletions CONTRIBUTORS.md
Original file line number Diff line number Diff line change
Expand Up @@ -62,6 +62,7 @@ Guidelines for modifications:
* Lionel Gulich
* Louis Le Lay
* Lorenz Wellhausen
* Manuel Schweiger
* Masoud Moghani
* Michael Gussert
* Michael Noseworthy
Expand Down
2 changes: 1 addition & 1 deletion docs/source/overview/core-concepts/actuators.rst
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,7 @@ maximum effort:

.. math::
\tau_{j, computed} & = k_p * (q - q_{des}) + k_d * (\dot{q} - \dot{q}_{des}) + \tau_{ff} \\
\tau_{j, computed} & = k_p * (q_{des} - q) + k_d * (\dot{q}_{des} - \dot{q}) + \tau_{ff} \\
\tau_{j, applied} & = clip(\tau_{computed}, -\tau_{j, max}, \tau_{j, max})
Expand Down
2 changes: 1 addition & 1 deletion source/extensions/omni.isaac.lab/config/extension.toml
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
[package]

# Note: Semantic Versioning is used: https://semver.org/
version = "0.30.3"
version = "0.30.4"

# Description
title = "Isaac Lab framework for Robot Learning"
Expand Down
10 changes: 10 additions & 0 deletions source/extensions/omni.isaac.lab/docs/CHANGELOG.rst
Original file line number Diff line number Diff line change
@@ -1,6 +1,16 @@
Changelog
---------

0.30.4 (2025-01-08)
~~~~~~~~~~~~~~~~~~~

Fixed
^^^^^

* fixed docstring in articulation data :class:`omni.isaac.lab.assets.ArticulationData`.
In body properties sections, the second dimension should be num_bodies but was documented as 1.


0.30.3 (2025-01-02)
~~~~~~~~~~~~~~~~~~~

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -385,11 +385,13 @@ def write_root_com_pose_to_sim(self, root_pose: torch.Tensor, env_ids: Sequence[
env_ids: Environment indices. If None, then all indices are used.
"""
# resolve all indices
physx_env_ids = env_ids
if env_ids is None:
local_env_ids = slice(None)
env_ids = slice(None)
physx_env_ids = self._ALL_INDICES

com_pos = self.data.com_pos_b[local_env_ids, 0, :]
com_quat = self.data.com_quat_b[local_env_ids, 0, :]
com_pos = self.data.com_pos_b[env_ids, 0, :]
com_quat = self.data.com_quat_b[env_ids, 0, :]

root_link_pos, root_link_quat = math_utils.combine_frame_transforms(
root_pose[..., :3],
Expand All @@ -399,7 +401,7 @@ def write_root_com_pose_to_sim(self, root_pose: torch.Tensor, env_ids: Sequence[
)

root_link_pose = torch.cat((root_link_pos, root_link_quat), dim=-1)
self.write_root_link_pose_to_sim(root_pose=root_link_pose, env_ids=env_ids)
self.write_root_link_pose_to_sim(root_pose=root_link_pose, env_ids=physx_env_ids)

def write_root_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.
Expand Down Expand Up @@ -458,18 +460,20 @@ def write_root_link_velocity_to_sim(self, root_velocity: torch.Tensor, env_ids:
env_ids: Environment indices. If None, then all indices are used.
"""
# resolve all indices
physx_env_ids = env_ids
if env_ids is None:
local_env_ids = slice(None)
env_ids = slice(None)
physx_env_ids = self._ALL_INDICES

root_com_velocity = root_velocity.clone()
quat = self.data.root_link_state_w[local_env_ids, 3:7]
com_pos_b = self.data.com_pos_b[local_env_ids, 0, :]
quat = self.data.root_link_state_w[env_ids, 3:7]
com_pos_b = self.data.com_pos_b[env_ids, 0, :]
# transform given velocity to center of mass
root_com_velocity[:, :3] += torch.linalg.cross(
root_com_velocity[:, 3:], math_utils.quat_rotate(quat, com_pos_b), dim=-1
)
# write center of mass velocity to sim
self.write_root_com_velocity_to_sim(root_velocity=root_com_velocity, env_ids=env_ids)
self.write_root_com_velocity_to_sim(root_velocity=root_com_velocity, env_ids=physx_env_ids)

def write_joint_state_to_sim(
self,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -747,7 +747,7 @@ def body_ang_acc_w(self) -> torch.Tensor:

@property
def body_link_pos_w(self) -> torch.Tensor:
"""Positions of all bodies in simulation world frame. Shape is (num_instances, 1, 3).
"""Positions of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 3).
This quantity is the position of the rigid bodies' actor frame relative to the world.
"""
Expand All @@ -760,7 +760,7 @@ def body_link_pos_w(self) -> torch.Tensor:

@property
def body_link_quat_w(self) -> torch.Tensor:
"""Orientation (w, x, y, z) of all bodies in simulation world frame. Shape is (num_instances, 1, 4).
"""Orientation (w, x, y, z) of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 4).
This quantity is the orientation of the rigid bodies' actor frame relative to the world.
"""
Expand All @@ -774,7 +774,7 @@ def body_link_quat_w(self) -> torch.Tensor:

@property
def body_link_vel_w(self) -> torch.Tensor:
"""Velocity of all bodies in simulation world frame. Shape is (num_instances, 1, 6).
"""Velocity of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 6).
This quantity contains the linear and angular velocities of the rigid bodies' center of mass frame
relative to the world.
Expand All @@ -783,15 +783,15 @@ def body_link_vel_w(self) -> torch.Tensor:

@property
def body_link_lin_vel_w(self) -> torch.Tensor:
"""Linear velocity of all bodies in simulation world frame. Shape is (num_instances, 1, 3).
"""Linear velocity of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 3).
This quantity is the linear velocity of the rigid bodies' center of mass frame relative to the world.
"""
return self.body_link_state_w[..., 7:10]

@property
def body_link_ang_vel_w(self) -> torch.Tensor:
"""Angular velocity of all bodies in simulation world frame. Shape is (num_instances, 1, 3).
"""Angular velocity of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 3).
This quantity is the angular velocity of the rigid bodies' center of mass frame relative to the world.
"""
Expand All @@ -803,7 +803,7 @@ def body_link_ang_vel_w(self) -> torch.Tensor:

@property
def body_com_pos_w(self) -> torch.Tensor:
"""Positions of all bodies in simulation world frame. Shape is (num_instances, 1, 3).
"""Positions of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 3).
This quantity is the position of the rigid bodies' actor frame.
"""
Expand All @@ -813,13 +813,13 @@ def body_com_pos_w(self) -> torch.Tensor:
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.
Shape is (num_instances, 1, 4). This quantity is the orientation of the rigid bodies' actor frame.
Shape is (num_instances, num_bodies, 4). This quantity is the orientation of the rigid bodies' actor frame.
"""
return self.body_com_state_w[..., 3:7]

@property
def body_com_vel_w(self) -> torch.Tensor:
"""Velocity of all bodies in simulation world frame. Shape is (num_instances, 1, 6).
"""Velocity of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 6).
This quantity contains the linear and angular velocities of the rigid bodies' center of mass frame.
"""
Expand All @@ -832,7 +832,7 @@ def body_com_vel_w(self) -> torch.Tensor:

@property
def body_com_lin_vel_w(self) -> torch.Tensor:
"""Linear velocity of all bodies in simulation world frame. Shape is (num_instances, 1, 3).
"""Linear velocity of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 3).
This quantity is the linear velocity of the rigid bodies' center of mass frame.
"""
Expand All @@ -845,7 +845,7 @@ def body_com_lin_vel_w(self) -> torch.Tensor:

@property
def body_com_ang_vel_w(self) -> torch.Tensor:
"""Angular velocity of all bodies in simulation world frame. Shape is (num_instances, 1, 3).
"""Angular velocity of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 3).
This quantity is the angular velocity of the rigid bodies' center of mass frame.
"""
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -1088,6 +1088,7 @@ def test_write_root_state(self):
# 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
Expand All @@ -1096,9 +1097,15 @@ def test_write_root_state(self):
articulation.update(sim.cfg.dt)

if state_location == "com":
articulation.write_root_com_state_to_sim(rand_state)
if i % 2 == 0:
articulation.write_root_com_state_to_sim(rand_state)
else:
articulation.write_root_com_state_to_sim(rand_state, env_ids=env_idx)
elif state_location == "link":
articulation.write_root_link_state_to_sim(rand_state)
if i % 2 == 0:
articulation.write_root_link_state_to_sim(rand_state)
else:
articulation.write_root_link_state_to_sim(rand_state, env_ids=env_idx)

if state_location == "com":
torch.testing.assert_close(rand_state, articulation.data.root_com_state_w)
Expand Down
22 changes: 22 additions & 0 deletions source/extensions/omni.isaac.lab/test/utils/test_configclass.py
Original file line number Diff line number Diff line change
Expand Up @@ -143,6 +143,16 @@ class BasicDemoTorchCfg:
some_tensor: torch.Tensor = torch.Tensor([1, 2, 3])


@configclass
class BasicActuatorCfg:
"""Dummy configuration class for ActuatorBase config."""

joint_names_expr: list[str] = ["some_string"]
joint_parameter_lookup: list[list[float]] = [[1, 2, 3], [4, 5, 6]]
stiffness: float = 1.0
damping: float = 2.0


"""
Dummy configuration to check type annotations ordering.
"""
Expand Down Expand Up @@ -530,6 +540,18 @@ def test_dict_conversion(self):
self.assertEqual(torch_cfg_dict["some_number"], 0)
self.assertTrue(torch.all(torch_cfg_dict["some_tensor"] == torch.tensor([1, 2, 3])))

def test_actuator_cfg_dict_conversion(self):
"""Test dict conversion of ActuatorConfig."""
# create a basic RemotizedPDActuator config
actuator_cfg = BasicActuatorCfg()
# return writable attributes of config object
actuator_cfg_dict_attr = actuator_cfg.__dict__
# check if __dict__ attribute of config is not empty
self.assertTrue(len(actuator_cfg_dict_attr) > 0)
# class_to_dict utility function should return a primitive dictionary
actuator_cfg_dict = class_to_dict(actuator_cfg)
self.assertTrue(isinstance(actuator_cfg_dict, dict))

def test_dict_conversion_order(self):
"""Tests that order is conserved when converting to dictionary."""
true_outer_order = ["device_id", "env", "robot_default_state", "list_config"]
Expand Down
6 changes: 3 additions & 3 deletions source/standalone/tools/check_instanceable.py
Original file line number Diff line number Diff line change
Expand Up @@ -82,12 +82,12 @@ def main():
sim = SimulationContext(
stage_units_in_meters=1.0, physics_dt=0.01, rendering_dt=0.01, backend="torch", device="cuda:0"
)
# enable flatcache which avoids passing data over to USD structure
# enable fabric which avoids passing data over to USD structure
# this speeds up the read-write operation of GPU buffers
if sim.get_physics_context().use_gpu_pipeline:
sim.get_physics_context().enable_flatcache(True)
sim.get_physics_context().enable_fabric(True)
# enable hydra scene-graph instancing
# this is needed to visualize the scene when flatcache is enabled
# this is needed to visualize the scene when fabric is enabled
set_carb_setting(sim._settings, "/persistent/omnihydra/useSceneGraphInstancing", True)

# Create interface to clone the scene
Expand Down
2 changes: 1 addition & 1 deletion source/standalone/tutorials/04_sensors/run_usd_camera.py
Original file line number Diff line number Diff line change
Expand Up @@ -252,7 +252,7 @@ def run_simulator(sim: sim_utils.SimulationContext, scene_entities: dict):
# Derive pointcloud from camera at camera_index
pointcloud = create_pointcloud_from_depth(
intrinsic_matrix=camera.data.intrinsic_matrices[camera_index],
depth=camera.data.output[camera_index]["distance_to_image_plane"],
depth=camera.data.output["distance_to_image_plane"][camera_index],
position=camera.data.pos_w[camera_index],
orientation=camera.data.quat_w_ros[camera_index],
device=sim.device,
Expand Down

0 comments on commit 1894583

Please sign in to comment.