Commit b3447f1b authored by Kelly Guo's avatar Kelly Guo Committed by Kelly Guo

Updates Isaac Sim imports for Isaac Sim 4.5 (#191)

# Description

Updates Isaac Sim imports for Isaac Sim 4.5
parent 5cf3d618
...@@ -568,10 +568,10 @@ class OperationalSpaceControllerAction(ActionTerm): ...@@ -568,10 +568,10 @@ class OperationalSpaceControllerAction(ActionTerm):
def _compute_dynamic_quantities(self): def _compute_dynamic_quantities(self):
"""Computes the dynamic quantities for operational space control.""" """Computes the dynamic quantities for operational space control."""
self._mass_matrix[:] = self._asset.root_physx_view.get_mass_matrices()[:, self._joint_ids, :][ self._mass_matrix[:] = self._asset.root_physx_view.get_generalized_mass_matrices()[:, self._joint_ids, :][
:, :, self._joint_ids :, :, self._joint_ids
] ]
self._gravity[:] = self._asset.root_physx_view.get_generalized_gravity_forces()[:, self._joint_ids] self._gravity[:] = self._asset.root_physx_view.get_gravity_compensation_forces()[:, self._joint_ids]
def _compute_ee_jacobian(self): def _compute_ee_jacobian(self):
"""Computes the geometric Jacobian of the ee body frame in root frame. """Computes the geometric Jacobian of the ee body frame in root frame.
......
...@@ -10,7 +10,7 @@ import numpy as np ...@@ -10,7 +10,7 @@ import numpy as np
from typing import TYPE_CHECKING from typing import TYPE_CHECKING
import omni import omni
from omni.isaac.core.simulation_context import SimulationContext from isaacsim.core.api.simulation_context import SimulationContext
from .ui_widget_wrapper import UIWidgetWrapper from .ui_widget_wrapper import UIWidgetWrapper
......
...@@ -12,7 +12,7 @@ from typing import TYPE_CHECKING ...@@ -12,7 +12,7 @@ from typing import TYPE_CHECKING
import carb import carb
import omni.kit.app import omni.kit.app
from omni.isaac.core.simulation_context import SimulationContext from isaacsim.core.api.simulation_context import SimulationContext
from omni.isaac.lab.managers import ManagerBase from omni.isaac.lab.managers import ManagerBase
from omni.isaac.lab.utils import configclass from omni.isaac.lab.utils import configclass
......
...@@ -909,6 +909,7 @@ class TestArticulation(unittest.TestCase): ...@@ -909,6 +909,7 @@ class TestArticulation(unittest.TestCase):
with build_simulation_context( with build_simulation_context(
device=device, add_ground_plane=False, auto_add_lighting=True device=device, add_ground_plane=False, auto_add_lighting=True
) as sim: ) as sim:
sim._app_control_on_stop_handle = None
articulation_cfg = generate_articulation_cfg( articulation_cfg = generate_articulation_cfg(
articulation_type="single_joint", vel_limit=limit, effort_limit=limit articulation_type="single_joint", vel_limit=limit, effort_limit=limit
) )
...@@ -1021,6 +1022,7 @@ class TestArticulation(unittest.TestCase): ...@@ -1021,6 +1022,7 @@ class TestArticulation(unittest.TestCase):
with build_simulation_context( with build_simulation_context(
device=device, add_ground_plane=False, auto_add_lighting=True device=device, add_ground_plane=False, auto_add_lighting=True
) as sim: ) as sim:
sim._app_control_on_stop_handle = None
articulation_cfg = generate_articulation_cfg(articulation_type="single_joint") articulation_cfg = generate_articulation_cfg(articulation_type="single_joint")
articulation, env_pos = generate_articulation(articulation_cfg, num_articulations, device) articulation, env_pos = generate_articulation(articulation_cfg, num_articulations, device)
env_idx = torch.tensor([x for x in range(num_articulations)]) env_idx = torch.tensor([x for x in range(num_articulations)])
...@@ -1138,6 +1140,7 @@ class TestArticulation(unittest.TestCase): ...@@ -1138,6 +1140,7 @@ class TestArticulation(unittest.TestCase):
with build_simulation_context( with build_simulation_context(
device=device, add_ground_plane=False, auto_add_lighting=True, gravity_enabled=False device=device, add_ground_plane=False, auto_add_lighting=True, gravity_enabled=False
) as sim: ) as sim:
sim._app_control_on_stop_handle = None
articulation_cfg = generate_articulation_cfg(articulation_type="anymal") articulation_cfg = generate_articulation_cfg(articulation_type="anymal")
articulation, env_pos = generate_articulation( articulation, env_pos = generate_articulation(
articulation_cfg, num_articulations, device articulation_cfg, num_articulations, device
......
...@@ -194,6 +194,7 @@ class TestRigidObject(unittest.TestCase): ...@@ -194,6 +194,7 @@ class TestRigidObject(unittest.TestCase):
for device in ("cuda:0", "cpu"): for device in ("cuda:0", "cpu"):
with self.subTest(num_cubes=num_cubes, device=device): with self.subTest(num_cubes=num_cubes, device=device):
with build_simulation_context(device=device, auto_add_lighting=True) as sim: with build_simulation_context(device=device, auto_add_lighting=True) as sim:
sim._app_control_on_stop_handle = None
# Generate cubes scene # Generate cubes scene
cube_object, _ = generate_cubes_scene( cube_object, _ = generate_cubes_scene(
num_cubes=num_cubes, api="articulation_root", device=device num_cubes=num_cubes, api="articulation_root", device=device
...@@ -792,6 +793,7 @@ class TestRigidObject(unittest.TestCase): ...@@ -792,6 +793,7 @@ class TestRigidObject(unittest.TestCase):
with build_simulation_context( with build_simulation_context(
device=device, gravity_enabled=False, auto_add_lighting=True device=device, gravity_enabled=False, auto_add_lighting=True
) as sim: ) as sim:
sim._app_control_on_stop_handle = None
# Create a scene with random cubes # Create a scene with random cubes
cube_object, env_pos = generate_cubes_scene(num_cubes=num_cubes, height=0.0, device=device) cube_object, env_pos = generate_cubes_scene(num_cubes=num_cubes, height=0.0, device=device)
env_idx = torch.tensor([x for x in range(num_cubes)]) env_idx = torch.tensor([x for x in range(num_cubes)])
...@@ -911,6 +913,7 @@ class TestRigidObject(unittest.TestCase): ...@@ -911,6 +913,7 @@ class TestRigidObject(unittest.TestCase):
with build_simulation_context( with build_simulation_context(
device=device, gravity_enabled=False, auto_add_lighting=True device=device, gravity_enabled=False, auto_add_lighting=True
) as sim: ) as sim:
sim._app_control_on_stop_handle = None
# Create a scene with random cubes # Create a scene with random cubes
cube_object, env_pos = generate_cubes_scene( cube_object, env_pos = generate_cubes_scene(
num_cubes=num_cubes, height=0.0, device=device num_cubes=num_cubes, height=0.0, device=device
......
...@@ -459,6 +459,7 @@ class TestRigidObjectCollection(unittest.TestCase): ...@@ -459,6 +459,7 @@ class TestRigidObjectCollection(unittest.TestCase):
with build_simulation_context( with build_simulation_context(
device=device, gravity_enabled=False, auto_add_lighting=True device=device, gravity_enabled=False, auto_add_lighting=True
) as sim: ) as sim:
sim._app_control_on_stop_handle = None
# Create a scene with random cubes # Create a scene with random cubes
cube_object, env_pos = generate_cubes_scene( cube_object, env_pos = generate_cubes_scene(
num_envs=num_envs, num_cubes=num_cubes, height=0.0, device=device num_envs=num_envs, num_cubes=num_cubes, height=0.0, device=device
...@@ -578,6 +579,7 @@ class TestRigidObjectCollection(unittest.TestCase): ...@@ -578,6 +579,7 @@ class TestRigidObjectCollection(unittest.TestCase):
with build_simulation_context( with build_simulation_context(
device=device, gravity_enabled=False, auto_add_lighting=True device=device, gravity_enabled=False, auto_add_lighting=True
) as sim: ) as sim:
sim._app_control_on_stop_handle = None
# Create a scene with random cubes # Create a scene with random cubes
cube_object, env_pos = generate_cubes_scene( cube_object, env_pos = generate_cubes_scene(
num_envs=num_envs, num_cubes=num_cubes, height=0.0, device=device num_envs=num_envs, num_cubes=num_cubes, height=0.0, device=device
......
...@@ -15,9 +15,9 @@ simulation_app = AppLauncher(headless=True).app ...@@ -15,9 +15,9 @@ simulation_app = AppLauncher(headless=True).app
import torch import torch
import unittest import unittest
import omni.isaac.core.utils.prims as prim_utils import isaacsim.core.utils.prims as prim_utils
import omni.isaac.core.utils.stage as stage_utils import isaacsim.core.utils.stage as stage_utils
from omni.isaac.cloner import GridCloner from isaacsim.core.cloner import GridCloner
import omni.isaac.lab.sim as sim_utils import omni.isaac.lab.sim as sim_utils
from omni.isaac.lab.assets import Articulation from omni.isaac.lab.assets import Articulation
...@@ -839,8 +839,8 @@ class TestOperationalSpaceController(unittest.TestCase): ...@@ -839,8 +839,8 @@ class TestOperationalSpaceController(unittest.TestCase):
# obtain dynamics related quantities from simulation # obtain dynamics related quantities from simulation
ee_jacobi_idx = ee_frame_idx - 1 ee_jacobi_idx = ee_frame_idx - 1
jacobian_w = robot.root_physx_view.get_jacobians()[:, ee_jacobi_idx, :, arm_joint_ids] jacobian_w = robot.root_physx_view.get_jacobians()[:, ee_jacobi_idx, :, arm_joint_ids]
mass_matrix = robot.root_physx_view.get_mass_matrices()[:, arm_joint_ids, :][:, :, arm_joint_ids] mass_matrix = robot.root_physx_view.get_generalized_mass_matrices()[:, arm_joint_ids, :][:, :, arm_joint_ids]
gravity = robot.root_physx_view.get_generalized_gravity_forces()[:, arm_joint_ids] gravity = robot.root_physx_view.get_gravity_compensation_forces()[:, arm_joint_ids]
# Convert the Jacobian from world to root frame # Convert the Jacobian from world to root frame
jacobian_b = jacobian_w.clone() jacobian_b = jacobian_w.clone()
root_rot_matrix = matrix_from_quat(quat_inv(robot.data.root_quat_w)) root_rot_matrix = matrix_from_quat(quat_inv(robot.data.root_quat_w))
......
...@@ -11,7 +11,7 @@ Imported by base, environment, and task classes. Not directly executed. ...@@ -11,7 +11,7 @@ Imported by base, environment, and task classes. Not directly executed.
import math import math
import torch import torch
import omni.isaac.core.utils.torch as torch_utils import isaacsim.core.utils.torch as torch_utils
from omni.isaac.lab.utils.math import axis_angle_from_quat from omni.isaac.lab.utils.math import axis_angle_from_quat
......
...@@ -7,7 +7,7 @@ import numpy as np ...@@ -7,7 +7,7 @@ import numpy as np
import torch import torch
import carb import carb
import omni.isaac.core.utils.torch as torch_utils import isaacsim.core.utils.torch as torch_utils
import omni.isaac.lab.sim as sim_utils import omni.isaac.lab.sim as sim_utils
from omni.isaac.lab.assets import Articulation from omni.isaac.lab.assets import Articulation
...@@ -205,7 +205,7 @@ class FactoryEnv(DirectRLEnv): ...@@ -205,7 +205,7 @@ class FactoryEnv(DirectRLEnv):
self.left_finger_jacobian = jacobians[:, self.left_finger_body_idx - 1, 0:6, 0:7] self.left_finger_jacobian = jacobians[:, self.left_finger_body_idx - 1, 0:6, 0:7]
self.right_finger_jacobian = jacobians[:, self.right_finger_body_idx - 1, 0:6, 0:7] self.right_finger_jacobian = jacobians[:, self.right_finger_body_idx - 1, 0:6, 0:7]
self.fingertip_midpoint_jacobian = (self.left_finger_jacobian + self.right_finger_jacobian) * 0.5 self.fingertip_midpoint_jacobian = (self.left_finger_jacobian + self.right_finger_jacobian) * 0.5
self.arm_mass_matrix = self._robot.root_physx_view.get_mass_matrices()[:, 0:7, 0:7] self.arm_mass_matrix = self._robot.root_physx_view.get_generalized_mass_matrices()[:, 0:7, 0:7]
self.joint_pos = self._robot.data.joint_pos.clone() self.joint_pos = self._robot.data.joint_pos.clone()
self.joint_vel = self._robot.data.joint_vel.clone() self.joint_vel = self._robot.data.joint_vel.clone()
......
...@@ -314,8 +314,8 @@ def update_states( ...@@ -314,8 +314,8 @@ def update_states(
# obtain dynamics related quantities from simulation # obtain dynamics related quantities from simulation
ee_jacobi_idx = ee_frame_idx - 1 ee_jacobi_idx = ee_frame_idx - 1
jacobian_w = robot.root_physx_view.get_jacobians()[:, ee_jacobi_idx, :, arm_joint_ids] jacobian_w = robot.root_physx_view.get_jacobians()[:, ee_jacobi_idx, :, arm_joint_ids]
mass_matrix = robot.root_physx_view.get_mass_matrices()[:, arm_joint_ids, :][:, :, arm_joint_ids] mass_matrix = robot.root_physx_view.get_generalized_mass_matrices()[:, arm_joint_ids, :][:, :, arm_joint_ids]
gravity = robot.root_physx_view.get_generalized_gravity_forces()[:, arm_joint_ids] gravity = robot.root_physx_view.get_gravity_compensation_forces()[:, arm_joint_ids]
# Convert the Jacobian from world to root frame # Convert the Jacobian from world to root frame
jacobian_b = jacobian_w.clone() jacobian_b = jacobian_w.clone()
root_rot_matrix = matrix_from_quat(quat_inv(robot.data.root_quat_w)) root_rot_matrix = matrix_from_quat(quat_inv(robot.data.root_quat_w))
......
...@@ -10,7 +10,7 @@ Any tests not listed here will use the default timeout. ...@@ -10,7 +10,7 @@ Any tests not listed here will use the default timeout.
PER_TEST_TIMEOUTS = { PER_TEST_TIMEOUTS = {
"test_articulation.py": 200, "test_articulation.py": 200,
"test_deformable_object.py": 200, "test_deformable_object.py": 200,
"test_environments.py": 1500, # This test runs through all the environments for 100 steps each "test_environments.py": 1650, # This test runs through all the environments for 100 steps each
"test_environment_determinism.py": 200, # This test runs through many the environments for 100 steps each "test_environment_determinism.py": 200, # This test runs through many the environments for 100 steps each
"test_env_rendering_logic.py": 300, "test_env_rendering_logic.py": 300,
"test_camera.py": 500, "test_camera.py": 500,
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment