Adjust based on reviews

ARISE-Initiative · Nov 22, 2024 · 12074e0 · 12074e0
1 parent 17686f3
commit 12074e0
Show file tree

Hide file tree

Showing 7 changed files with 34 additions and 31 deletions.
diff --git a/robosuite/controllers/config/robots/default_gr1_fixed_lower_body.json b/robosuite/controllers/config/robots/default_gr1_fixed_lower_body.json
@@ -12,8 +12,8 @@
         "ik_input_rotation_repr": "axis_angle",
         "verbose": false,
         "ik_posture_weights": {
-            "robot0_torso_waist_yaw": 30.0,
-            "robot0_torso_waist_pitch": 100.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,

diff --git a/robosuite/devices/device.py b/robosuite/devices/device.py
@@ -163,8 +163,7 @@ def input2action(self, mirror_actions=False) -> Optional[Dict]:
         )
         ac_dict[f"{active_arm}_abs"] = arm_action["abs"]
         ac_dict[f"{active_arm}_delta"] = arm_action["delta"]
-        # ac_dict[f"{active_arm}_gripper"] = np.array([grasp] * gripper_dof)
-        ac_dict[f"{active_arm}_gripper"] = [1, 0.45 * grasp, grasp, grasp, grasp, grasp]
+        ac_dict[f"{active_arm}_gripper"] = np.array([grasp] * gripper_dof)
 
         # clip actions between -1 and 1
         for k, v in ac_dict.items():

diff --git a/robosuite/devices/spacemouse.py b/robosuite/devices/spacemouse.py
@@ -102,7 +102,7 @@ def convert(b1, b2):
     return scale_to_control(to_int16(b1, b2))
 
 
-class SpaceMouseSdkWrapper:
+class SpaceMouseListener:
     """
     A wrapper for interfacing with SpaceMouse devices using the HID library.
 
@@ -329,9 +329,9 @@ def __init__(
                 product_id_path_pairs.add((device["product_id"], device["path"]))
         product_id_path_pairs = sorted(list(product_id_path_pairs))
 
-        self.devices = []
+        self.listeners = []
         for product_id, path in product_id_path_pairs:
-            self.devices.append(SpaceMouseSdkWrapper(product_id, path, pos_sensitivity, rot_sensitivity))
+            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)
@@ -368,17 +368,17 @@ def _reset_internal_state(self):
         """
         super()._reset_internal_state()
 
-        for device in self.devices:
-            device._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 device in self.devices:
-            device.start_control()
+        for listener in self.listeners:
+            listener.start_control()
 
     def get_controller_state(self):
         """
@@ -387,7 +387,7 @@ def get_controller_state(self):
         Returns:
             dict: A dictionary containing dpos, orn, unmodified orn, grasp, and reset
         """
-        controller_state = self.devices[self.active_arm_index % len(self.devices)].get_controller_state()
+        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
 
@@ -404,12 +404,12 @@ def input2action(self, mirror_actions=False) -> Optional[Dict]:
         """
         final_ac = None
         original_arm_index = self.active_arm_index
-        for i in range(len(self.devices)):
+        for i in range(len(self.listeners)):
             ac_dict = super().input2action(mirror_actions)
             if ac_dict is None:  # reset from any space mouse
-                for device in self.devices:
-                    device._reset_state = 1
-                    device._enabled = False
+                for listener in self.listeners:
+                    listener._reset_state = 1
+                    listener._enabled = False
                 self._reset_internal_state()
                 return None
             elif final_ac is None:
@@ -469,7 +469,7 @@ def _postprocess_device_outputs(self, dpos, drotation):
     for device in hid.enumerate():
         if device["vendor_id"] == vendor_id and device["path"] not in device_pathes:
             space_mice.append(
-                SpaceMouseSdkWrapper(device["product_id"], device["path"], pos_sensitivity, rot_sensitivity)
+                SpaceMouseListener(device["product_id"], device["path"], pos_sensitivity, rot_sensitivity)
             )
             device_pathes.add(device["path"])
 

diff --git a/robosuite/examples/third_party_controller/default_mink_ik_gr1.json b/robosuite/examples/third_party_controller/default_mink_ik_gr1.json
@@ -12,8 +12,8 @@
         "ik_input_rotation_repr": "axis_angle",
         "verbose": false,
         "ik_posture_weights": {
-            "robot0_torso_waist_yaw": 30.0,
-            "robot0_torso_waist_pitch": 100.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,

diff --git a/robosuite/models/assets/grippers/inspire_left_hand.xml b/robosuite/models/assets/grippers/inspire_left_hand.xml
@@ -79,7 +79,7 @@
             <site name="ft_frame" pos="0 0 0" size="0.01 0.01 0.01" rgba="1 0 0 0" type="sphere" group="1"/>
             <inertial diaginertia="0 0 0" mass="0" pos="0 0 0"/>
             <body name="eef" pos="0 0 0" quat="0.707 0. -0.707 0.">
-                <site name="grip_site" pos="0 0 0" size="0.01 0.01 0.01" rgba="1 1 0 0" type="sphere" group="2"/>
+                <site name="grip_site" pos="0 0 0" size="0.01 0.01 0.01" rgba="1 1 0 1" type="sphere" group="2"/>
                 <site name="ee_x" pos="0.1 0 0" size="0.005 .1"  quat="0.707105  0 0.707108 0 " rgba="1 0 0 0" type="cylinder" group="1"/>
                 <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"/>
@@ -94,8 +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" 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="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 -->

diff --git a/robosuite/models/assets/grippers/inspire_right_hand.xml b/robosuite/models/assets/grippers/inspire_right_hand.xml
@@ -76,7 +76,7 @@
             <site name="ft_frame" pos="0 0 0" size="0.01 0.01 0.01" rgba="1 0 0 0" type="sphere" group="1"/>
             <inertial diaginertia="0 0 0" mass="0" pos="0 0 0"/>
             <body name="eef" pos="0 0 0" quat="0.707 0. -0.707 0.">
-                <site name="grip_site" pos="0 0 0" size="0.01 0.01 0.01" rgba="1 1 0 0" type="sphere" group="2"/>
+                <site name="grip_site" pos="0 0 0" size="0.01 0.01 0.01" rgba="1 1 0 1" type="sphere" group="2"/>
                 <site name="ee_x" pos="0.1 0 0" size="0.005 .1"  quat="0.707105  0 0.707108 0 " rgba="1 0 0 0" type="cylinder" group="1"/>
                 <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"/>
@@ -91,8 +91,10 @@
                 <geom name="r_palm_vis" type="mesh" mesh="r_palm_vis" material="r_base_material" group="1" contype="0" conaffinity="0"/>
                 <!-- Palm Collision -->
                 <geom name="r_palm_col" type="mesh" mesh="r_palm_col" mass="0.1" />
-                <site name="r_pinch_sphere" 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_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="r_thumb" pos="0. 0. 0.">
                     <!-- Thumb Proximal 1 -->

diff --git a/robosuite/models/grippers/inspire_hands.py b/robosuite/models/grippers/inspire_hands.py
@@ -23,9 +23,9 @@ def format_action(self, action):
         # however the tag makes finger movement laggy, so manually copy the value for finger joints
         # 0 is thumb rot, no copying. Thumb bend has 3 joints, so copy 3 times. Other fingers has 2 joints, so copy 2 times.
         assert len(action) == self.dof
-        action = np.array(action)
-        indices = np.array([0, 1, 1, 1, 2, 2, 3, 3, 4, 4, 5, 5])
-        return action[indices]
+        vel = np.array([1, 0.45 * action[0], action[0]])
+        indices = np.array([0, 1, 1, 1, 2, 2, 2, 2, 2, 2, 2, 2])
+        return vel[indices]
 
     @property
     def init_qpos(self):
@@ -93,9 +93,9 @@ def format_action(self, action):
         # however the tag makes finger movement laggy, so manually copy the value for finger joints
         # 0 is thumb rot, no copying. Thumb bend has 3 joints, so copy 3 times. Other fingers has 2 joints, so copy 2 times.
         assert len(action) == self.dof
-        action = np.array(action)
-        indices = np.array([0, 1, 1, 1, 2, 2, 3, 3, 4, 4, 5, 5])
-        return action[indices]
+        vel = np.array([1, 0.45 * action[0], action[0]])
+        indices = np.array([0, 1, 1, 1, 2, 2, 2, 2, 2, 2, 2, 2])
+        return vel[indices]
 
     @property
     def init_qpos(self):