Omniverse Isaac Gym utils#

Utilities for ease of programming of Omniverse Isaac Gym environments.



Control of robotic manipulators#


Differential inverse kinematics#

This implementation attempts to unify under a single and reusable function the whole set of procedures used to compute the inverse kinematics of a robotic manipulator, originally shown in the Isaac Orbit framework’s task space controllers section, but this time for Omniverse Isaac Gym.

\(\Delta\theta =\) scale \(J^\dagger \, \vec{e}\)

where

\(\qquad \Delta\theta \;\) is the change in joint angles
\(\qquad \vec{e} \;\) is the Cartesian pose error (position and orientation)
\(\qquad J^\dagger \;\) is the pseudoinverse of the Jacobian estimated as follows:

The pseudoinverse of the Jacobian (\(J^\dagger\)) is estimated as follows:

  • Tanspose: \(\; J^\dagger = J^T\)

  • Pseduoinverse: \(\; J^\dagger = J^T(JJ^T)^{-1}\)

  • Damped least-squares: \(\; J^\dagger = J^T(JJ^T \, +\) damping\({}^2 I)^{-1}\)

  • Singular-vale decomposition: See buss2004introduction (section 6)


API#

skrl.utils.omniverse_isaacgym_utils.ik(jacobian_end_effector: torch.Tensor, current_position: torch.Tensor, current_orientation: torch.Tensor, goal_position: torch.Tensor, goal_orientation: torch.Tensor | None = None, method: str = 'damped least-squares', method_cfg: Mapping[str, float] = {'damping': 0.05, 'min_singular_value': 1e-05, 'scale': 1}, squeeze_output: bool = True) torch.Tensor#

Differential inverse kinematics

Parameters:
  • jacobian_end_effector (torch.Tensor) – End effector’s jacobian

  • current_position (torch.Tensor) – End effector’s current position

  • current_orientation (torch.Tensor) – End effector’s current orientation

  • goal_position (torch.Tensor) – End effector’s goal position

  • goal_orientation (torch.Tensor, optional) – End effector’s goal orientation (default: None). If not provided, the current orientation will be used instead.

  • method (str, optional) –

    Differential inverse kinematics formulation (default: "damped least-squares"). The supported methods are described in the following table:

    IK Method

    Method tag

    Damped least-squares

    "damped least-squares"

    Tanspose

    "transpose"

    Pseduoinverse

    "pseudoinverse"

    Singular-vale decomposition (SVD)

    "singular-vale decomposition"

  • method_cfg (dict, optional) – Method configurations (default: {"scale": 1, "damping": 0.05, "min_singular_value": 1e-5})

  • squeeze_output (bool, optional) – Squeeze output (default: True)

Returns:

Change in joint angles

Return type:

torch.Tensor


OmniIsaacGymEnvs-like environment instance#

Instantiate a VecEnvBase-based object compatible with OmniIsaacGymEnvs for use outside of the OmniIsaacGymEnvs implementation.


API#

skrl.utils.omniverse_isaacgym_utils.get_env_instance(headless: bool = True, enable_livestream: bool = False, enable_viewport: bool = False, multi_threaded: bool = False) omni.isaac.gym.vec_env.VecEnvBase#

Instantiate a VecEnvBase-based object compatible with OmniIsaacGymEnvs

Parameters:
  • headless (bool, optional) – Disable UI when running (default: True)

  • enable_livestream (bool, optional) – Whether to enable live streaming (default: False)

  • enable_viewport (bool, optional) – Whether to enable viewport (default: False)

  • multi_threaded (bool, optional) – Whether to return a multi-threaded environment instance (default: False)

Returns:

Environment instance

Return type:

omni.isaac.gym.vec_env.VecEnvBase

Example:

from skrl.envs.wrappers.torch import wrap_env
from skrl.utils.omniverse_isaacgym_utils import get_env_instance

# get environment instance
env = get_env_instance(headless=True)

# parse sim configuration
from omniisaacgymenvs.utils.config_utils.sim_config import SimConfig
sim_config = SimConfig({"test": False,
                        "device_id": 0,
                        "headless": True,
                        "multi_gpu": False,
                        "sim_device": "gpu",
                        "enable_livestream": False,
                        "task": {"name": "CustomTask",
                                 "physics_engine": "physx",
                                 "env": {"numEnvs": 512,
                                         "envSpacing": 1.5,
                                         "enableDebugVis": False,
                                         "clipObservations": 1000.0,
                                         "clipActions": 1.0,
                                         "controlFrequencyInv": 4},
                                 "sim": {"dt": 0.0083,  # 1 / 120
                                         "use_gpu_pipeline": True,
                                         "gravity": [0.0, 0.0, -9.81],
                                         "add_ground_plane": True,
                                         "use_flatcache": True,
                                         "enable_scene_query_support": False,
                                         "enable_cameras": False,
                                         "default_physics_material": {"static_friction": 1.0,
                                                                      "dynamic_friction": 1.0,
                                                                      "restitution": 0.0},
                                         "physx": {"worker_thread_count": 4,
                                                   "solver_type": 1,
                                                   "use_gpu": True,
                                                   "solver_position_iteration_count": 4,
                                                   "solver_velocity_iteration_count": 1,
                                                   "contact_offset": 0.005,
                                                   "rest_offset": 0.0,
                                                   "bounce_threshold_velocity": 0.2,
                                                   "friction_offset_threshold": 0.04,
                                                   "friction_correlation_distance": 0.025,
                                                   "enable_sleeping": True,
                                                   "enable_stabilization": True,
                                                   "max_depenetration_velocity": 1000.0,
                                                   "gpu_max_rigid_contact_count": 524288,
                                                   "gpu_max_rigid_patch_count": 33554432,
                                                   "gpu_found_lost_pairs_capacity": 524288,
                                                   "gpu_found_lost_aggregate_pairs_capacity": 262144,
                                                   "gpu_total_aggregate_pairs_capacity": 1048576,
                                                   "gpu_max_soft_body_contacts": 1048576,
                                                   "gpu_max_particle_contacts": 1048576,
                                                   "gpu_heap_capacity": 33554432,
                                                   "gpu_temp_buffer_capacity": 16777216,
                                                   "gpu_max_num_partitions": 8}}}})

# import and setup custom task
from custom_task import CustomTask
task = CustomTask(name="CustomTask", sim_config=sim_config, env=env)
env.set_task(task=task, sim_params=sim_config.get_physics_params(), backend="torch", init_sim=True)

# wrap the environment
env = wrap_env(env, "omniverse-isaacgym")