Isaac Gym utils

Utilities for ease of programming of Isaac Gym environments.



Control of robotic manipulators


Inverse kinematics using damped least squares method

This implementation attempts to unify under a single and reusable function the whole set of procedures used to calculate the inverse kinematics of a robotic manipulator shown in Isaac Gym’s example: Franka IK Picking (franka_cube_ik_osc.py)

\(\Delta\theta = J^T (JJ^T + \lambda^2 I)^{-1} \, \vec{e}\)

where

\(\qquad \Delta\theta \;\) is the change in joint angles
\(\qquad J \;\) is the Jacobian
\(\qquad \lambda \;\) is a non-zero damping constant
\(\qquad \vec{e} \;\) is the Cartesian pose error (position and orientation)

API

skrl.utils.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, damping_factor: float = 0.05, squeeze_output: bool = True) torch.Tensor

Inverse kinematics using damped least squares method

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 or None) – End effector’s goal orientation (default: None)

  • damping_factor (float) – Damping factor (default: 0.05)

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

Returns:

Change in joint angles

Return type:

torch.Tensor


Web viewer for development without X server

This library provides an API for instantiating a lightweight web viewer useful, mostly, for designing Isaac Gym environments in remote workstations or docker containers without X server


Gestures and actions

Gestures / actions

Key

Mouse

Orbit (rotate view around a point)

Alt

Left click

Pan (rotate view around itself)

Right click

Walk mode (move view linearly to the current alignment)

Middle click

Zoom in/out

Scroll wheel

Freeze view

V

Toggle image type (color, depth)

T

Watch an animation of the gestures and actions in the following video



Requirements

The web viewer is build on top of Flask and requires the following package to be installed

pip install Flask

Also, to be launched in Visual Studio Code (Preview in Editor), the Live Preview - VS Code Extension must be installed


Usage

import math
from isaacgym import gymapi

from skrl.utils import isaacgym_utils


# create a web viewer instance
web_viewer = isaacgym_utils.WebViewer()

# configure and create simulation
sim_params = gymapi.SimParams()
sim_params.up_axis = gymapi.UP_AXIS_Z
sim_params.gravity = gymapi.Vec3(0.0, 0.0, -9.8)
sim_params.physx.solver_type = 1
sim_params.physx.num_position_iterations = 4
sim_params.physx.num_velocity_iterations = 1
sim_params.physx.use_gpu = True
sim_params.use_gpu_pipeline = True

gym = gymapi.acquire_gym()
sim = gym.create_sim(compute_device=0, graphics_device=0, type=gymapi.SIM_PHYSX, params=sim_params)

# setup num_envs and env's grid
num_envs = 1
spacing = 2.0
env_lower = gymapi.Vec3(-spacing, -spacing, 0.0)
env_upper = gymapi.Vec3(spacing, 0.0, spacing)

# add ground plane
plane_params = gymapi.PlaneParams()
plane_params.normal = gymapi.Vec3(0.0, 0.0, 1.0)
gym.add_ground(sim, plane_params)

envs = []
cameras = []

for i in range(num_envs):
    # create env
    env = gym.create_env(sim, env_lower, env_upper, int(math.sqrt(num_envs)))

    # add sphere
    pose = gymapi.Transform()
    pose.p, pose.r = gymapi.Vec3(0.0, 0.0, 1.0), gymapi.Quat(0.0, 0.0, 0.0, 1.0)
    gym.create_actor(env, gym.create_sphere(sim, 0.2, None), pose, "sphere", i, 0)

    # add camera
    cam_props = gymapi.CameraProperties()
    cam_props.width, cam_props.height = 300, 300
    cam_handle = gym.create_camera_sensor(env, cam_props)
    gym.set_camera_location(cam_handle, env, gymapi.Vec3(1, 1, 1), gymapi.Vec3(0, 0, 0))

    envs.append(env)
    cameras.append(cam_handle)

# setup web viewer
web_viewer.setup(gym, sim, envs, cameras)

gym.prepare_sim(sim)


for i in range(100000):
    gym.simulate(sim)

    # render the scene
    web_viewer.render(fetch_results=True,
                      step_graphics=True,
                      render_all_camera_sensors=True,
                      wait_for_page_load=True)

API

class skrl.utils.isaacgym_utils.WebViewer(host: str = '127.0.0.1', port: int = 5000)

Bases: object

__init__(host: str = '127.0.0.1', port: int = 5000) None

Web viewer for Isaac Gym

Parameters:
  • host (str) – Host address (default: “127.0.0.1”)

  • port (int) – Port number (default: 5000)

_route_index() flask.Response

Render the web page

Returns:

Flask response

Return type:

flask.Response

_route_input_event() flask.Response

Handle keyboard and mouse input

Returns:

Flask response

Return type:

flask.Response

_route_stream() flask.Response

Stream the image to the web page

Returns:

Flask response

Return type:

flask.Response

_stream() bytes

Format the image to be streamed

Returns:

Image encoded as Content-Type

Return type:

bytes

render(fetch_results: bool = True, step_graphics: bool = True, render_all_camera_sensors: bool = True, wait_for_page_load: bool = True) None

Render and get the image from the current camera

This function must be called after the simulation is stepped (post_physics_step). The following Isaac Gym functions are called before get the image. Their calling can be skipped by setting the corresponding argument to False

  • fetch_results

  • step_graphics

  • render_all_camera_sensors

Parameters:
  • fetch_results (bool) – Call Gym.fetch_results method (default: True)

  • step_graphics (bool) – Call Gym.step_graphics method (default: True)

  • render_all_camera_sensors (bool) – Call Gym.render_all_camera_sensors method (default: True)

  • wait_for_page_load (bool) – Wait for the page to load (default: True)

setup(gym: isaacgym.gymapi.Gym, sim: isaacgym.gymapi.Sim, envs: List[int], cameras: List[int]) None

Setup the web viewer

Parameters:
  • gym (isaacgym.gymapi.Gym) – The gym

  • sim (isaacgym.gymapi.Sim) – Simulation handle

  • envs (list of ints) – Environment handles

  • cameras (list of ints) – Camera handles