Improve spacemouse

robosuite/controllers/config/robots/default_gr1_fixed_lower_body.json

@@ -12,9 +12,9 @@
         "ik_input_rotation_repr": "axis_angle",
         "verbose": false,
         "ik_posture_weights": {
-            "robot0_torso_waist_yaw": 10.0,
-            "robot0_torso_waist_pitch": 10.0,
-            "robot0_torso_waist_roll": 200.0,
+            "robot0_torso_waist_yaw": 20.0,
+            "robot0_torso_waist_pitch": 20.0,
+            "robot0_torso_waist_roll": 100.0,
             "robot0_l_shoulder_pitch": 4.0,
             "robot0_r_shoulder_pitch": 4.0,
             "robot0_l_shoulder_roll": 3.0,

robosuite/devices/spacemouse.py

@@ -19,6 +19,7 @@
 import threading
 import time
 from collections import namedtuple
+from typing import Dict, List, Optional  # for abstract base class definitions
 
 import numpy as np
 from pynput.keyboard import Controller, Key, Listener
@@ -101,37 +102,26 @@ def convert(b1, b2):
     return scale_to_control(to_int16(b1, b2))
 
 
-class SpaceMouse(Device):
+class SpaceMouseListener:
     """
-    A minimalistic driver class for SpaceMouse with HID library.
+    A wrapper for interfacing with SpaceMouse devices using the HID library.
 
-    Note: Use hid.enumerate() to view all USB human interface devices (HID).
-    Make sure SpaceMouse is detected before running the script.
-    You can look up its vendor/product id from this method.
+    This class provides methods to initialize the device, configure sensitivity settings,
+    handle 6-DOF input data, and run a listener thread to continuously receive input
+    from the SpaceMouse hardware.
 
     Args:
-        env (RobotEnv): The environment which contains the robot(s) to control
-                        using this device.
-        pos_sensitivity (float): Magnitude of input position command scaling
-        rot_sensitivity (float): Magnitude of scale input rotation commands scaling
+        product_id (int): The unique identifier for the SpaceMouse model.
+        device_path (str): Path to the device as detected by the HID library.
+        pos_sensitivity (float): Scaling factor for position commands.
+        rot_sensitivity (float): Scaling factor for rotational commands.
     """
 
-    def __init__(
-        self,
-        env,
-        vendor_id=macros.SPACEMOUSE_VENDOR_ID,
-        product_id=macros.SPACEMOUSE_PRODUCT_ID,
-        pos_sensitivity=1.0,
-        rot_sensitivity=1.0,
-    ):
-        super().__init__(env)
-
-        print("Opening SpaceMouse device")
-        self.vendor_id = vendor_id
+    def __init__(self, product_id, device_path, pos_sensitivity, rot_sensitivity):
         self.product_id = product_id
         self.device = hid.device()
         try:
-            self.device.open(self.vendor_id, self.product_id)  # SpaceMouse
+            self.device.open_path(device_path)  # SpaceMouse
         except OSError as e:
             ROBOSUITE_DEFAULT_LOGGER.warning(
                 "Failed to open SpaceMouse device"
@@ -149,8 +139,6 @@ def __init__(
         self.x, self.y, self.z = 0, 0, 0
         self.roll, self.pitch, self.yaw = 0, 0, 0
 
-        self._display_controls()
-
         self.single_click_and_hold = False
 
         self._control = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
@@ -163,41 +151,10 @@ def __init__(
         self.thread.daemon = True
         self.thread.start()
 
-        # also add a keyboard for aux controls
-        self.listener = Listener(on_press=self.on_press, on_release=self.on_release)
-
-        # start listening
-        self.listener.start()
-
-    @staticmethod
-    def _display_controls():
-        """
-        Method to pretty print controls.
-        """
-
-        def print_command(char, info):
-            char += " " * (30 - len(char))
-            print("{}\t{}".format(char, info))
-
-        print("")
-        print_command("Control", "Command")
-        print_command("Right button", "reset simulation")
-        print_command("Left button (hold)", "close gripper")
-        print_command("Move mouse laterally", "move arm horizontally in x-y plane")
-        print_command("Move mouse vertically", "move arm vertically")
-        print_command("Twist mouse about an axis", "rotate arm about a corresponding axis")
-        print_command("Control+C", "quit")
-        print_command("b", "toggle arm/base mode (if applicable)")
-        print_command("s", "switch active arm (if multi-armed robot)")
-        print_command("=", "switch active robot (if multi-robot environment)")
-        print("")
-
     def _reset_internal_state(self):
         """
         Resets internal state of controller, except for the reset signal.
         """
-        super()._reset_internal_state()
-
         self.rotation = np.array([[-1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, -1.0]])
         # Reset 6-DOF variables
         self.x, self.y, self.z = 0, 0, 0
@@ -239,7 +196,6 @@ def get_controller_state(self):
             raw_drotation=np.array([roll, pitch, yaw]),
             grasp=self.control_gripper,
             reset=self._reset_state,
-            base_mode=int(self.base_mode),
         )
 
     def run(self):
@@ -310,8 +266,6 @@ def run(self):
                     # right button is for reset
                     if d[1] == 2:
                         self._reset_state = 1
-                        self._enabled = False
-                        self._reset_internal_state()
 
     @property
     def control(self):
@@ -335,6 +289,140 @@ def control_gripper(self):
             return 1.0
         return 0
 
+
+class SpaceMouse(Device):
+    """
+    A minimalistic driver class for SpaceMouse with HID library.
+
+    Note: Use hid.enumerate() to view all USB human interface devices (HID).
+    Make sure SpaceMouse is detected before running the script.
+    You can look up its vendor/product id from this method.
+
+    Args:
+        env (RobotEnv): The environment which contains the robot(s) to control
+                        using this device.
+        vendor_id (int): The vendor ID used to locate the device.
+        product_id (int): Currently unused. The product ID is retrieved from hid.enumerate().
+        pos_sensitivity (float): Magnitude of input position command scaling
+        rot_sensitivity (float): Magnitude of scale input rotation commands scaling
+    """
+
+    def __init__(
+        self,
+        env,
+        vendor_id=macros.SPACEMOUSE_VENDOR_ID,
+        product_id=macros.SPACEMOUSE_PRODUCT_ID,
+        pos_sensitivity=1.0,
+        rot_sensitivity=1.0,
+    ):
+        super().__init__(env)
+
+        print("Opening SpaceMouse device")
+        self.vendor_id = vendor_id
+        self.product_id = product_id
+
+        self._display_controls()
+
+        product_id_path_pairs = set()
+        for device in hid.enumerate():
+            if device["vendor_id"] == vendor_id:
+                product_id_path_pairs.add((device["product_id"], device["path"]))
+        product_id_path_pairs = sorted(list(product_id_path_pairs))
+
+        self.listeners = []
+        for product_id, path in product_id_path_pairs:
+            self.listeners.append(SpaceMouseListener(product_id, path, pos_sensitivity, rot_sensitivity))
+
+        # also add a keyboard for aux controls
+        self.listener = Listener(on_press=self.on_press, on_release=self.on_release)
+
+        # start listening
+        self.listener.start()
+
+    @staticmethod
+    def _display_controls():
+        """
+        Method to pretty print controls.
+        """
+
+        def print_command(char, info):
+            char += " " * (30 - len(char))
+            print("{}\t{}".format(char, info))
+
+        print("")
+        print_command("Control", "Command")
+        print_command("Right button", "reset simulation")
+        print_command("Left button (hold)", "close gripper")
+        print_command("Move mouse laterally", "move arm horizontally in x-y plane")
+        print_command("Move mouse vertically", "move arm vertically")
+        print_command("Twist mouse about an axis", "rotate arm about a corresponding axis")
+        print_command("Control+C", "quit")
+        print_command("b", "toggle arm/base mode (if applicable)")
+        print_command("s", "switch active arm (if multi-armed robot)")
+        print_command("=", "switch active robot (if multi-robot environment)")
+        print("")
+
+    def _reset_internal_state(self):
+        """
+        Resets internal state of controller, except for the reset signal.
+        """
+        super()._reset_internal_state()
+
+        for listener in self.listeners:
+            listener._reset_internal_state()
+
+    def start_control(self):
+        """
+        Method that should be called externally before controller can
+        start receiving commands.
+        """
+        self._reset_internal_state()
+        for listener in self.listeners:
+            listener.start_control()
+
+    def get_controller_state(self):
+        """
+        Grabs the current state of the 3D mouse.
+
+        Returns:
+            dict: A dictionary containing dpos, orn, unmodified orn, grasp, and reset
+        """
+        controller_state = self.listeners[self.active_arm_index % len(self.listeners)].get_controller_state()
+        controller_state.update({"base_mode": int(self.base_mode)})
+        return controller_state
+
+    def input2action(self, mirror_actions=False) -> Optional[Dict]:
+        """
+        Processes input from all space mice and computes the final action for the robot.
+
+        Args:
+            mirror_actions (bool): Whether to mirror the actions for a mirrored setup.
+
+        Returns:
+            Optional[Dict]: A dictionary containing the computed actions for the active arm,
+            or None if a reset is triggered.
+        """
+        final_ac = None
+        original_arm_index = self.active_arm_index
+        for i in range(len(self.listeners)):
+            ac_dict = super().input2action(mirror_actions)
+            if ac_dict is None:  # reset from any space mouse
+                for listener in self.listeners:
+                    listener._reset_state = 1
+                    listener._enabled = False
+                self._reset_internal_state()
+                return None
+            elif final_ac is None:
+                final_ac = ac_dict
+            else:
+                active_arm = self.active_arm
+                final_ac[f"{active_arm}_abs"] = ac_dict[f"{active_arm}_abs"]
+                final_ac[f"{active_arm}_delta"] = ac_dict[f"{active_arm}_delta"]
+                final_ac[f"{active_arm}_gripper"] = ac_dict[f"{active_arm}_gripper"]
+            self.active_arm_index = (self.active_arm_index + 1) % len(self.all_robot_arms[self.active_robot])
+        self.active_arm_index = original_arm_index
+        return final_ac
+
     def on_press(self, key):
         """
         Key handler for key presses.
@@ -372,8 +460,23 @@ def _postprocess_device_outputs(self, dpos, drotation):
 
 
 if __name__ == "__main__":
+    vendor_id = macros.SPACEMOUSE_VENDOR_ID
+    pos_sensitivity = 1.0
+    rot_sensitivity = 1.0
+
+    space_mice = []
+    device_pathes = set()
+    for device in hid.enumerate():
+        if device["vendor_id"] == vendor_id and device["path"] not in device_pathes:
+            space_mice.append(
+                SpaceMouseListener(device["product_id"], device["path"], pos_sensitivity, rot_sensitivity)
+            )
+            device_pathes.add(device["path"])
+
+    for space_mouse in space_mice:
+        space_mouse.start_control()
 
-    space_mouse = SpaceMouse()
     for i in range(100):
-        print(space_mouse.control, space_mouse.control_gripper)
+        for i, space_mouse in enumerate(space_mice):
+            print(f"[Device {i}] {space_mouse.control} {space_mouse.control_gripper}")
         time.sleep(0.02)

robosuite/examples/third_party_controller/default_mink_ik_gr1.json

@@ -12,9 +12,9 @@
         "ik_input_rotation_repr": "axis_angle",
         "verbose": false,
         "ik_posture_weights": {
-            "robot0_torso_waist_yaw": 10.0,
-            "robot0_torso_waist_pitch": 10.0,
-            "robot0_torso_waist_roll": 200.0,
+            "robot0_torso_waist_yaw": 20.0,
+            "robot0_torso_waist_pitch": 20.0,
+            "robot0_torso_waist_roll": 100.0,
             "robot0_l_shoulder_pitch": 4.0,
             "robot0_r_shoulder_pitch": 4.0,
             "robot0_l_shoulder_roll": 3.0,

robosuite/models/assets/grippers/inspire_left_hand.xml

@@ -84,7 +84,7 @@
                 <site name="ee_y" pos="0 0.1 0" size="0.005 .1" quat="0.707105 0.707108 0 0" rgba="0 1 0 0" type="cylinder" group="1"/>
                 <site name="ee_z" pos="0 0 0.1" size="0.005 .1" quat="1 0 0 0" rgba="0 0 1 0" type="cylinder" group="1"/>
                 <!-- This site was added for visualization. -->
-                <site name="grip_site_cylinder" pos="0 0 0" quat="-0.5 -0.5 -0.5 0.5" size="0.005 0.5" rgba="0 1 0 0.3" type="cylinder" group="1"/>
+                <site name="grip_site_cylinder" pos="0 0 0" quat="-0.5 -0.5 -0.5 0.5" size="0.005 0.5" rgba="0 1 0 0" type="cylinder" group="1"/>
             </body>
 
 
@@ -94,7 +94,10 @@
                 <geom name="l_palm_vis" type="mesh" mesh="l_palm_vis" material="l_base_material" group="1" contype="0" conaffinity="0"/>
                 <!-- Palm Collision -->
                 <geom name="l_palm_col" type="mesh" mesh="l_palm_col" mass="0.1" />
-
+                <site name="l_pinch_sphere_0" pos="-0.06 -0.12 0.03" size="0.01 0.01 0.01" rgba="1 0 0 1" type="sphere" group="1"/>
+                <site name="r_pinch_sphere_1" pos="-0.06 -0.12 0.01" size="0.01 0.01 0.01" rgba="1 0.5 0.5 0.5" type="sphere" group="1"/>
+                <site name="r_pinch_sphere_2" pos="-0.06 -0.12 -0.01" size="0.01 0.01 0.01" rgba="1 0.5 0.5 0.5" type="sphere" group="1"/>
+                <site name="r_pinch_sphere_3" pos="-0.06 -0.12 -0.03" size="0.01 0.01 0.01" rgba="1 0.5 0.5 0.5" type="sphere" group="1"/>
                 <!-- Thumb -->
                 <body name="l_thumb" pos="0. 0. 0.">
                     <!-- Thumb Proximal 1 -->
@@ -124,6 +127,7 @@
                                     <geom name="l_thumb_distal_vis" type="mesh" mesh="l_thumb_distal_vis" material="l_thumb_distal_material" group="1" contype="0" conaffinity="0"> </geom>
                                     <geom name="l_thumb_distal_col" type="mesh" mesh="l_thumb_distal_col" mass="0.1" />
                                     <site name="site_l_thumb_distal" size="0.01" material="site_left_hand"/>
+                                    <site name="l_thumb_distal_site_cylinder" pos="0.04242 -0.04242 -0.006" quat="0.7071 0.5 0.5 0" size="0.002 0.06" rgba="0 1 0 0.3" type="cylinder" group="1"/>
                                 </body>
                                 <!-- End of Thumb Distal -->
                             </body>
@@ -149,6 +153,7 @@
                             <site name="site_l_index_distal" size="0.01" material="site_left_hand"/>
                             <geom name="l_index_distal_vis" type="mesh" mesh="l_index_distal_vis" material="l_index_material" group="1" contype="0" conaffinity="0"/>
                             <geom name="l_index_distal_col" type="mesh" mesh="l_index_distal_col" mass="0.1" />
+                            <site name="l_index_distal_site_cylinder" pos="0 -0.06 -0.005" quat="0.7071 0.7071 0 0" size="0.002 0.06" rgba="0 1 0 0.3" type="cylinder" group="1"/>
                         </body>
                         <!-- End of Index Distal -->
                     </body>
@@ -169,6 +174,7 @@
                             <site name="site_l_middle_distal" size="0.01" material="site_left_hand"/>
                             <geom name="l_middle_distal_vis" type="mesh" mesh="l_middle_distal_vis" material="l_middle_material" group="1" contype="0" conaffinity="0"/>
                             <geom name="l_middle_distal_col" type="mesh" mesh="l_middle_distal_col" mass="0.1" />
+                            <site name="l_middle_distal_site_cylinder" pos="0 -0.06 -0.005" quat="0.7071 0.7071 0 0" size="0.002 0.06" rgba="0 1 0 0.3" type="cylinder" group="1"/>
                         </body>
                         <!-- End of Middle Distal -->
                     </body>
@@ -190,6 +196,7 @@
                             <site name="site_l_ring_distal" size="0.01" material="site_left_hand"/>
                             <geom name="l_ring_distal_vis" type="mesh" mesh="l_ring_distal_vis" material="l_ring_material" group="1" contype="0" conaffinity="0"/>
                             <geom name="l_ring_distal_col" type="mesh" mesh="l_ring_distal_col" mass="0.1" />
+                            <site name="l_ring_distal_site_cylinder" pos="0 -0.06 -0.005" quat="0.7071 0.7071 0 0" size="0.002 0.06" rgba="0 1 0 0.3" type="cylinder" group="1"/>
                         </body>
                         <!-- End of Ring Distal -->
                     </body>
@@ -211,6 +218,7 @@
                             <site name="site_l_pinky_distal" size="0.01" material="site_left_hand"/>
                             <geom name="l_pinky_distal_vis" type="mesh" mesh="l_pinky_distal_vis" material="l_pinky_material" group="1" contype="0" conaffinity="0"/>
                             <geom name="l_pinky_distal_col" type="mesh" mesh="l_pinky_distal_col" mass="0.1" />
+                            <site name="l_pinky_distal_site_cylinder_5" pos="0 -0.06 -0.005" quat="0.7071 0.7071 0 0" size="0.002 0.06" rgba="0 1 0 0.3" type="cylinder" group="1"/>
                         </body>
                         <!-- End of Pinky Distal -->
                     </body>