omni.isaac.lab.devices.spacemouse.se3_spacemouse 源代码

# Copyright (c) 2022-2024, The Isaac Lab Project Developers.
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause

"""Spacemouse controller for SE(3) control."""

import hid
import numpy as np
import threading
import time
from collections.abc import Callable
from scipy.spatial.transform import Rotation

from ..device_base import DeviceBase
from .utils import convert_buffer


[文档]class Se3SpaceMouse(DeviceBase): """A space-mouse controller for sending SE(3) commands as delta poses. This class implements a space-mouse controller to provide commands to a robotic arm with a gripper. It uses the `HID-API`_ which interfaces with USD and Bluetooth HID-class devices across multiple platforms [1]. The command comprises of two parts: * delta pose: a 6D vector of (x, y, z, roll, pitch, yaw) in meters and radians. * gripper: a binary command to open or close the gripper. Note: The interface finds and uses the first supported device connected to the computer. Currently tested for following devices: - SpaceMouse Compact: https://3dconnexion.com/de/product/spacemouse-compact/ .. _HID-API: https://github.com/libusb/hidapi """
[文档] def __init__(self, pos_sensitivity: float = 0.4, rot_sensitivity: float = 0.8): """Initialize the space-mouse layer. Args: pos_sensitivity: Magnitude of input position command scaling. Defaults to 0.4. rot_sensitivity: Magnitude of scale input rotation commands scaling. Defaults to 0.8. """ # store inputs self.pos_sensitivity = pos_sensitivity self.rot_sensitivity = rot_sensitivity # acquire device interface self._device = hid.device() self._find_device() # read rotations self._read_rotation = False # command buffers self._close_gripper = False self._delta_pos = np.zeros(3) # (x, y, z) self._delta_rot = np.zeros(3) # (roll, pitch, yaw) # dictionary for additional callbacks self._additional_callbacks = dict() # run a thread for listening to device updates self._thread = threading.Thread(target=self._run_device) self._thread.daemon = True self._thread.start()
def __del__(self): """Destructor for the class.""" self._thread.join() def __str__(self) -> str: """Returns: A string containing the information of joystick.""" 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 += "\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" msg += "\tMove mouse vertically: move arm vertically\n" msg += "\tTwist mouse about an axis: rotate arm about a corresponding axis" return msg """ Operations """
[文档] def reset(self): # default flags self._close_gripper = False self._delta_pos = np.zeros(3) # (x, y, z) self._delta_rot = np.zeros(3) # (roll, pitch, yaw)
[文档] def add_callback(self, key: str, func: Callable): # check keys supported by callback if key not in ["L", "R"]: raise ValueError(f"Only left (L) and right (R) buttons supported. Provided: {key}.") # TODO: Improve this to allow multiple buttons on same key. self._additional_callbacks[key] = func
[文档] def advance(self) -> tuple[np.ndarray, bool]: """Provides the result from spacemouse event state. Returns: 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. return np.concatenate([self._delta_pos, rot_vec]), self._close_gripper
""" Internal helpers. """ def _find_device(self): """Find the device connected to computer.""" found = False # implement a timeout for device search for _ in range(5): for device in hid.enumerate(): if device["product_string"] == "SpaceMouse Compact": # set found flag found = True vendor_id = device["vendor_id"] product_id = device["product_id"] # connect to the device self._device.open(vendor_id, product_id) # check if device found if not found: time.sleep(1.0) else: break # no device found: return false if not found: raise OSError("No device found by SpaceMouse. Is the device connected?") def _run_device(self): """Listener thread that keeps pulling new messages.""" # keep running while True: # read the device data data = self._device.read(7) if data is not None: # readings from 6-DoF sensor if data[0] == 1: self._delta_pos[1] = self.pos_sensitivity * convert_buffer(data[1], data[2]) self._delta_pos[0] = self.pos_sensitivity * convert_buffer(data[3], data[4]) self._delta_pos[2] = self.pos_sensitivity * convert_buffer(data[5], data[6]) * -1.0 elif data[0] == 2 and not self._read_rotation: self._delta_rot[1] = self.rot_sensitivity * convert_buffer(data[1], data[2]) self._delta_rot[0] = self.rot_sensitivity * convert_buffer(data[3], data[4]) self._delta_rot[2] = self.rot_sensitivity * convert_buffer(data[5], data[6]) # readings from the side buttons elif data[0] == 3: # press left button if data[1] == 1: # close gripper self._close_gripper = not self._close_gripper # additional callbacks if "L" in self._additional_callbacks: self._additional_callbacks["L"] # right button is for reset if data[1] == 2: # reset layer self.reset() # additional callbacks if "R" in self._additional_callbacks: self._additional_callbacks["R"] if data[1] == 3: self._read_rotation = not self._read_rotation