Adds gamepad device support for teleoperation (#72)

* Adds `se3_gamepad` and `se2_gamepad`
* Update changelog and version
* Adds unsubscribing when destroying

---------
Co-authored-by: Mayank Mittal <mittalma@leggedrobotics.com>

.vscode/tools/settings.template.json

@@ -39,7 +39,9 @@
         "robomimic",
         "teleoperation",
         "xform",
-        "numpy"
+        "numpy",
+        "dpad",
+        "gamepad"
     ],
     // This enables python language server. Seems to work slightly better than jedi:
     "python.languageServer": "Pylance",

source/extensions/omni.isaac.orbit/config/extension.toml

 [package]
 
 # Note: Semantic Versioning is used: https://semver.org/
-version = "0.2.8"
+version = "0.3.0"
 
 # Description
 title = "ORBIT framework for Robot Learning"

source/extensions/omni.isaac.orbit/docs/CHANGELOG.rst

 Changelog
 ---------
 
+0.3.0 (2023-04-20)
+~~~~~~~~~~~~~~~~~~
+
+Fixed
+^^^^^
+
+* Added the destructor to the keyboard devices to unsubscribe from carb.
+
+Added
+^^^^^
+
+* Added the :class:`Se2Gamepad` and :class:`Se3Gamepad` for gamepad teleoperation support.
+
+
 0.2.8 (2023-04-10)
 ~~~~~~~~~~~~~~~~~~
 

source/extensions/omni.isaac.orbit/omni/isaac/orbit/devices/__init__.py

@@ -6,10 +6,11 @@
 """
 Module providing interfaces to different teleoperation devices.
 
-Currently, the module supports two categories of devices:
+Currently, the module supports three categories of devices:
 
 * Keyboard: Standard keyboard with WASD and arrow keys.
 * Spacemouse: 3D mouse with 6 degrees of freedom.
+* Gamepad: Gamepad with 2D two joysticks and buttons. Example: Xbox controller.
 
 All device interfaces inherit from the :class:`DeviceBase` class, which provides a
 common interface for all devices. The device interface reads the input data when
@@ -70,7 +71,8 @@ Example usage showing the keyboard interface:
 
 """
 
+from .gamepad import Se2Gamepad, Se3Gamepad
 from .keyboard import Se2Keyboard, Se3Keyboard
 from .spacemouse import Se2SpaceMouse, Se3SpaceMouse
 
-__all__ = ["Se2Keyboard", "Se3Keyboard", "Se2SpaceMouse", "Se3SpaceMouse"]
+__all__ = ["Se2Keyboard", "Se3Keyboard", "Se2SpaceMouse", "Se3SpaceMouse", "Se2Gamepad", "Se3Gamepad"]

source/extensions/omni.isaac.orbit/omni/isaac/orbit/devices/device_base.py

@@ -31,12 +31,13 @@ class DeviceBase(ABC):
         raise NotImplementedError
 
     @abstractmethod
-    def add_callback(self, key: str, func: Callable):
+    def add_callback(self, key: Any, func: Callable):
         """Add additional functions to bind keyboard.
 
         Args:
-            key (str): The keyboard button to check against.
-            func (Callable): The function to call when key is pressed.
+            key (Any): The button to check against.
+            func (Callable): The function to call when key is pressed. The callback function should not
+                take any arguments.
         """
         raise NotImplementedError
 

source/extensions/omni.isaac.orbit/omni/isaac/orbit/devices/gamepad/__init__.py

+# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto
+# All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+
+"""Gamepad device for SE(2) and SE(3) control."""
+
+from .se2_gamepad import Se2Gamepad
+from .se3_gamepad import Se3Gamepad
+
+__all__ = ["Se2Gamepad", "Se3Gamepad"]

source/extensions/omni.isaac.orbit/omni/isaac/orbit/devices/gamepad/se2_gamepad.py

+# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto
+# All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+
+"""Gamepad controller for SE(2) control."""
+
+
+import numpy as np
+from typing import Callable
+
+import carb
+import omni
+
+from ..device_base import DeviceBase
+
+
+class Se2Gamepad(DeviceBase):
+    r"""A gamepad controller for sending SE(2) commands as velocity commands.
+
+    This class is designed to provide a gamepad controller for mobile base (such as quadrupeds).
+    It uses the Omniverse gamepad interface to listen to gamepad events and map them to robot's
+    task-space commands.
+
+    The command comprises of the base linear and angular velocity: :math:`(v_x, v_y, \omega_z)`.
+
+    Key bindings:
+        ====================== ========================= ========================
+        Command                Key (+ve axis)            Key (-ve axis)
+        ====================== ========================= ========================
+        Move along x-axis      left stick up             left stick down
+        Move along y-axis      left stick right          left stick left
+        Rotate along z-axis    right stick right         right stick left
+        ====================== ========================= ========================
+
+    .. seealso::
+
+        The official documentation for the gamepad interface: `Carb Gamepad Interface <https://docs.omniverse.nvidia.com/kit/docs/carbonite/latest/docs/python/carb.html#carb.input.Gamepad>`__.
+
+    """
+
+    def __init__(
+        self,
+        v_x_sensitivity: float = 1.0,
+        v_y_sensitivity: float = 1.0,
+        omega_z_sensitivity: float = 1.0,
+        dead_zone: float = 0.01,
+    ):
+        """Initialize the gamepad layer.
+
+        Args:
+            v_x_sensitivity (float): Magnitude of linear velocity along x-direction scaling. Defaults to 1.0.
+            v_y_sensitivity (float): Magnitude of linear velocity along y-direction scaling. Defaults to 1.0.
+            omega_z_sensitivity (float): Magnitude of angular velocity along z-direction scaling. Defaults to 1.0.
+            dead_zone (float): Magnitude of dead zone for gamepad. An event value from the gamepad less than
+                this value will be ignored. Defaults to 0.01.
+        """
+        # store inputs
+        self.v_x_sensitivity = v_x_sensitivity
+        self.v_y_sensitivity = v_y_sensitivity
+        self.omega_z_sensitivity = omega_z_sensitivity
+        self.dead_zone = dead_zone
+        # acquire omniverse interfaces
+        self._appwindow = omni.appwindow.get_default_app_window()
+        self._input = carb.input.acquire_input_interface()
+        self._gamepad = self._appwindow.get_gamepad(0)
+        self._gamepad_sub = self._input.subscribe_to_gamepad_events(self._gamepad, self._on_gamepad_event)
+        # bindings for gamepad to command
+        self._create_key_bindings()
+        # command buffers
+        # When using the gamepad, two values are provided for each axis.
+        # For example: when the left stick is moved down, there are two evens: `left_stick_down = 0.8`
+        #   and `left_stick_up = 0.0`. If only the value of left_stick_up is used, the value will be 0.0,
+        #   which is not the desired behavior. Therefore, we save both the values into the buffer and use
+        #   the maximum value.
+        # (positive, negative), (x, y, yaw)
+        self._base_command_raw = np.zeros([2, 3])
+        # dictionary for additional callbacks
+        self._additional_callbacks = dict()
+
+    def __del__(self):
+        """Unsubscribe from gamepad events."""
+        self._input.unsubscribe_from_gamepad_events(self._gamepad, self._gamepad_sub)
+        self._gamepad_sub = None
+
+    def __str__(self) -> str:
+        """Returns: A string containing the information of joystick."""
+        msg = f"Gamepad Controller for SE(2): {self.__class__.__name__}\n"
+        msg += f"\tDevice name: {self._input.get_gamepad_name(self._gamepad)}\n"
+        msg += "\t----------------------------------------------\n"
+        msg += "\tMove in X-Y plane: left stick\n"
+        msg += "\tRotate in Z-axis: right stick\n"
+        return msg
+
+    """
+    Operations
+    """
+
+    def reset(self):
+        # default flags
+        self._base_command_raw.fill(0.0)
+
+    def add_callback(self, key: carb.input.GamepadInput, func: Callable):
+        """Add additional functions to bind gamepad.
+
+        A list of available gamepad keys are present in the
+        `carb documentation <https://docs.omniverse.nvidia.com/kit/docs/carbonite/latest/docs/python/carb.html#carb.input.GamepadInput>`__.
+
+        Args:
+            key (carb.input.GamepadInput): The gamepad button to check against.
+            func (Callable): The function to call when key is pressed. The callback function should not
+                take any arguments.
+        """
+        self._additional_callbacks[key] = func
+
+    def advance(self) -> np.ndarray:
+        """Provides the result from gamepad event state.
+
+        Returns:
+            np.ndarray: A 3D array containing the linear (x,y) and angular velocity (z).
+        """
+        return self._resolve_command_buffer(self._base_command_raw)
+
+    """
+    Internal helpers.
+    """
+
+    def _on_gamepad_event(self, event: carb.input.GamepadEvent, *args, **kwargs):
+        """Subscriber callback to when kit is updated.
+
+        Reference:
+            https://docs.omniverse.nvidia.com/kit/docs/carbonite/latest/docs/python/carb.html?highlight=gamepadeventtype#carb.input.GamepadInput
+        """
+
+        # check if the event is a button press
+        cur_val = event.value
+        if abs(cur_val) < self.dead_zone:
+            cur_val = 0
+
+        # -- left and right stick
+        if event.input in self._INPUT_STICK_VALUE_MAPPING:
+            direction, axis, value = self._INPUT_STICK_VALUE_MAPPING[event.input]
+            # change the value only if the stick is moved (soft press)
+            self._base_command_raw[direction, axis] = value * cur_val
+
+        # additional callbacks
+        if event.input in self._additional_callbacks:
+            self._additional_callbacks[event.input]()
+
+        # since no error, we are fine :)
+        return True
+
+    def _create_key_bindings(self):
+        """Creates default key binding."""
+        self._INPUT_STICK_VALUE_MAPPING = {
+            # forward command
+            carb.input.GamepadInput.LEFT_STICK_UP: (0, 0, self.v_x_sensitivity),
+            # backward command
+            carb.input.GamepadInput.LEFT_STICK_DOWN: (1, 0, self.v_x_sensitivity),
+            # right command
+            carb.input.GamepadInput.LEFT_STICK_RIGHT: (0, 1, self.v_y_sensitivity),
+            # left command
+            carb.input.GamepadInput.LEFT_STICK_LEFT: (1, 1, self.v_y_sensitivity),
+            # yaw command (positive)
+            carb.input.GamepadInput.RIGHT_STICK_RIGHT: (0, 2, self.omega_z_sensitivity),
+            # yaw command (negative)
+            carb.input.GamepadInput.RIGHT_STICK_LEFT: (1, 2, self.omega_z_sensitivity),
+        }
+
+    def _resolve_command_buffer(self, raw_command: np.ndarray) -> np.ndarray:
+        """Resolves the command buffer.
+
+        Args:
+            raw_command (np.ndarray): The raw command from the gamepad. Shape: (2, 3)
+                This is a 2D array since gamepad dpad/stick returns two values corresponding to
+                the positive and negative direction. The first index is the direction (0: positive, 1: negative)
+                and the second index is value (absolute) of the command.
+
+        Returns:
+            np.ndarray: resolved command. Shape: (3,)
+        """
+        # compare the positive and negative value decide the sign of the value
+        #   if the positive value is larger, the sign is positive (i.e. False, 0)
+        #   if the negative value is larger, the sign is positive (i.e. True, 1)
+        command_sign = raw_command[1, :] > raw_command[0, :]
+        # extract the command value
+        command = raw_command.max(axis=0)
+        # apply the sign
+        #  if the sign is positive, the value is already positive.
+        #  if the sign is negative, the value is negative after applying the sign.
+        command[command_sign] *= -1
+
+        return command

source/extensions/omni.isaac.orbit/omni/isaac/orbit/devices/keyboard/se2_keyboard.py

@@ -33,8 +33,10 @@ class Se2Keyboard(DeviceBase):
         Rotate along z-axis    Numpad 7 / X              Numpad 9 / Y
         ====================== ========================= ========================
 
-    Reference:
-        https://docs.omniverse.nvidia.com/kit/docs/carbonite/latest/docs/python/carb.html?highlight=keyboardeventtype#carb.input.Keyboard
+    .. seealso::
+
+        The official documentation for the keyboard interface: `Carb Keyboard Interface <https://docs.omniverse.nvidia.com/kit/docs/carbonite/latest/docs/python/carb.html#carb.input.Keyboard>`__.
+
     """
 
     def __init__(self, v_x_sensitivity: float = 0.8, v_y_sensitivity: float = 0.4, omega_z_sensitivity: float = 1.0):
@@ -61,9 +63,16 @@ class Se2Keyboard(DeviceBase):
         # dictionary for additional callbacks
         self._additional_callbacks = dict()
 
+    def __del__(self):
+        """Release the keyboard interface."""
+        self._input.unsubscribe_from_keyboard_events(self._keyboard, self._keyboard_sub)
+        self._keyboard_sub = None
+
     def __str__(self) -> str:
         """Returns: A string containing the information of joystick."""
         msg = f"Keyboard Controller for SE(2): {self.__class__.__name__}\n"
+        msg += f"\tKeyboard name: {self._input.get_keyboard_name(self._keyboard)}\n"
+        msg += "\t----------------------------------------------\n"
         msg += "\tReset all commands: L\n"
         msg += "\tMove forward   (along x-axis): Numpad 8 / Arrow Up\n"
         msg += "\tMove backward  (along x-axis): Numpad 2 / Arrow Down\n"
@@ -85,13 +94,12 @@ class Se2Keyboard(DeviceBase):
         """Add additional functions to bind keyboard.
 
         A list of available keys are present in the
-        `carb documentation <https://docs.omniverse.nvidia.com/kit/docs/carbonite/latest/docs/python/carb.html?highlight=keyboardeventtype#carb.input.KeyboardInput>`_.
-
-        The callback function should not take any arguments.
+        `carb documentation <https://docs.omniverse.nvidia.com/kit/docs/carbonite/latest/docs/python/carb.html?highlight=keyboardeventtype#carb.input.KeyboardInput>`__.
 
         Args:
             key (str): The keyboard button to check against.
-            func (Callable): The function to call when key is pressed.
+            func (Callable): The function to call when key is pressed. The callback function should not
+                take any arguments.
         """
         self._additional_callbacks[key] = func
 
@@ -99,7 +107,7 @@ class Se2Keyboard(DeviceBase):
         """Provides the result from keyboard event state.
 
         Returns:
-            np.ndarray -- A 3D array containing the linear (x,y) and angular velocity (z).
+            np.ndarray: A 3D array containing the linear (x,y) and angular velocity (z).
         """
         return self._base_command
 

source/extensions/omni.isaac.orbit/omni/isaac/orbit/devices/keyboard/se3_keyboard.py

@@ -41,8 +41,10 @@ class Se3Keyboard(DeviceBase):
         Rotate along z-axis            C                 V
         ============================== ================= =================
 
-    Reference:
-        https://docs.omniverse.nvidia.com/kit/docs/carbonite/latest/docs/python/carb.html?highlight=keyboardeventtype#carb.input.Keyboard
+    .. seealso::
+
+        The official documentation for the keyboard interface: `Carb Keyboard Interface <https://docs.omniverse.nvidia.com/kit/docs/carbonite/latest/docs/python/carb.html#carb.input.Keyboard>`__.
+
     """
 
     def __init__(self, pos_sensitivity: float = 0.4, rot_sensitivity: float = 0.8):
@@ -69,9 +71,16 @@ class Se3Keyboard(DeviceBase):
         # dictionary for additional callbacks
         self._additional_callbacks = dict()
 
+    def __del__(self):
+        """Release the keyboard interface."""
+        self._input.unsubscribe_from_keyboard_events(self._keyboard, self._keyboard_sub)
+        self._keyboard_sub = None
+
     def __str__(self) -> str:
         """Returns: A string containing the information of joystick."""
         msg = f"Keyboard Controller for SE(3): {self.__class__.__name__}\n"
+        msg += f"\tKeyboard name: {self._input.get_keyboard_name(self._keyboard)}\n"
+        msg += "\t----------------------------------------------\n"
         msg += "\tToggle gripper (open/close): K\n"
         msg += "\tMove arm along x-axis: W/S\n"
         msg += "\tMove arm along y-axis: A/D\n"
@@ -95,13 +104,12 @@ class Se3Keyboard(DeviceBase):
         """Add additional functions to bind keyboard.
 
         A list of available keys are present in the
-        `carb documentation <https://docs.omniverse.nvidia.com/kit/docs/carbonite/latest/docs/python/carb.html?highlight=keyboardeventtype#carb.input.KeyboardInput>`_.
-
-        The callback function should not take any arguments.
+        `carb documentation <https://docs.omniverse.nvidia.com/kit/docs/carbonite/latest/docs/python/carb.html?highlight=keyboardeventtype#carb.input.KeyboardInput>`__.
 
         Args:
             key (str): The keyboard button to check against.
-            func (Callable): The function to call when key is pressed.
+            func (Callable): The function to call when key is pressed. The callback function should not
+                take any arguments.
         """
         self._additional_callbacks[key] = func
 
@@ -109,10 +117,11 @@ class Se3Keyboard(DeviceBase):
         """Provides the result from keyboard event state.
 
         Returns:
-            Tuple[np.ndarray, bool] -- A tuple containing the delta pose command and gripper commands.
+            Tuple[np.ndarray, bool]: A tuple containing the delta pose command and gripper commands.
         """
+        # convert to rotation vector
         rot_vec = Rotation.from_euler("XYZ", self._delta_rot).as_rotvec()
-        # if new command received, reset event flag to False until keyboard updated.
+        # return the command and gripper state
         return np.concatenate([self._delta_pos, rot_vec]), self._close_gripper
 
     """

source/extensions/omni.isaac.orbit/omni/isaac/orbit/devices/spacemouse/se2_spacemouse.py

@@ -67,7 +67,7 @@ class Se2SpaceMouse(DeviceBase):
         msg = f"Spacemouse Controller for SE(2): {self.__class__.__name__}\n"
         msg += f"\tManufacturer: {self._device.get_manufacturer_string()}\n"
         msg += f"\tProduct: {self._device.get_product_string()}\n"
-        msg += "----------------------------------------------\n"
+        msg += "\t----------------------------------------------\n"
         msg += "\tRight button: reset command\n"
         msg += "\tMove mouse laterally: move base horizontally in x-y plane\n"
         msg += "\tTwist mouse about z-axis: yaw base about a corresponding axis"
@@ -92,7 +92,7 @@ class Se2SpaceMouse(DeviceBase):
         """Provides the result from spacemouse event state.
 
         Returns:
-            np.ndarray -- A 3D array containing the linear (x,y) and angular velocity (z).
+            np.ndarray: A 3D array containing the linear (x,y) and angular velocity (z).
         """
         return self._base_command
 

source/extensions/omni.isaac.orbit/omni/isaac/orbit/devices/spacemouse/se3_spacemouse.py

@@ -74,7 +74,7 @@ class Se3SpaceMouse(DeviceBase):
         msg = f"Spacemouse Controller for SE(3): {self.__class__.__name__}\n"
         msg += f"\tManufacturer: {self._device.get_manufacturer_string()}\n"
         msg += f"\tProduct: {self._device.get_product_string()}\n"
-        msg += "----------------------------------------------\n"
+        msg += "\t----------------------------------------------\n"
         msg += "\tRight button: reset command\n"
         msg += "\tLeft button: toggle gripper command (open/close)\n"
         msg += "\tMove mouse laterally: move arm horizontally in x-y plane\n"
@@ -103,7 +103,7 @@ class Se3SpaceMouse(DeviceBase):
         """Provides the result from spacemouse event state.
 
         Returns:
-            Tuple[np.ndarray, bool] -- A tuple containing the delta pose command and gripper commands.
+            Tuple[np.ndarray, bool]: A tuple containing the delta pose command and gripper commands.
         """
         rot_vec = Rotation.from_euler("XYZ", self._delta_rot).as_rotvec()
         # if new command received, reset event flag to False until keyboard updated.

source/standalone/environments/teleoperation/teleop_se3_agent.py

@@ -34,7 +34,7 @@ import torch
 
 import carb
 
-from omni.isaac.orbit.devices import Se3Keyboard, Se3SpaceMouse
+from omni.isaac.orbit.devices import Se3Gamepad, Se3Keyboard, Se3SpaceMouse
 
 import omni.isaac.contrib_envs  # noqa: F401
 import omni.isaac.orbit_envs  # noqa: F401
@@ -81,6 +81,10 @@ def main():
         teleop_interface = Se3SpaceMouse(
             pos_sensitivity=0.05 * args_cli.sensitivity, rot_sensitivity=0.005 * args_cli.sensitivity
         )
+    elif args_cli.device.lower() == "gamepad":
+        teleop_interface = Se3Gamepad(
+            pos_sensitivity=0.1 * args_cli.sensitivity, rot_sensitivity=0.1 * args_cli.sensitivity
+        )
     else:
         raise ValueError(f"Invalid device interface '{args_cli.device}'. Supported: 'keyboard', 'spacemouse'.")
     # add teleoperation key for env reset