[add] add grasp qpos for inspire and fourier hand (#670)

* [add] add grasp qpos for inspire and fourier hand

* [fix] fix gripper abs control

Author: Dingry

robosuite/devices/device.py

@@ -112,6 +112,7 @@ def input2action(self, mirror_actions=False) -> Optional[Dict]:
 
         # Get controller reference
         controller = robot.part_controllers[active_arm]
+        gripper = robot.gripper[active_arm]
         gripper_dof = robot.gripper[active_arm].dof
 
         assert controller.name in ["OSC_POSE", "JOINT_POSITION"], "only supporting OSC_POSE and JOINT_POSITION for now"
@@ -163,11 +164,15 @@ 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)
+
+        if hasattr(gripper, "grasp_qpos"):
+            ac_dict[f"{active_arm}_gripper"] = getattr(gripper, "grasp_qpos")[grasp]
+        else:
+            ac_dict[f"{active_arm}_gripper"] = np.array([grasp] * gripper_dof)
 
         # clip actions between -1 and 1
         for (k, v) in ac_dict.items():
-            if "abs" not in k:
+            if "abs" not in k and "gripper" not in k:
                 ac_dict[k] = np.clip(v, -1, 1)
 
         return ac_dict

robosuite/models/grippers/fourier_hands.py

@@ -31,6 +31,13 @@ def format_action(self, action):
     def init_qpos(self):
         return np.array([0.0] * 11)
 
+    @property
+    def grasp_qpos(self):
+        return {
+            -1: np.array([-1.5, -1.5, -1.5, -1.5, -3, 3]),  # open
+            1: np.array([1.5, 1.5, 1.5, 1.5, 3, 3]),  # close
+        }
+
     @property
     def speed(self):
         return 0.15
@@ -99,6 +106,13 @@ def format_action(self, action):
     def init_qpos(self):
         return np.array([0.0] * 11)
 
+    @property
+    def grasp_qpos(self):
+        return {
+            -1: np.array([-1.5, -1.5, -1.5, -1.5, -3, 3]),  # open
+            1: np.array([1.5, 1.5, 1.5, 1.5, 3, 3]),  # close
+        }
+
     @property
     def speed(self):
         return 0.15

robosuite/models/grippers/inspire_hands.py

@@ -31,6 +31,13 @@ def format_action(self, action):
     def init_qpos(self):
         return np.array([0.0] * 12)
 
+    @property
+    def grasp_qpos(self):
+        return {
+            -1: np.array([-1.5, -1.5, -1.5, -1.5, -3, 3]),  # open
+            1: np.array([1.5, 1.5, 1.5, 1.5, 3, 3]),  # close
+        }
+
     @property
     def speed(self):
         return 0.15
@@ -101,6 +108,13 @@ def format_action(self, action):
     def init_qpos(self):
         return np.array([0.0] * 12)
 
+    @property
+    def grasp_qpos(self):
+        return {
+            -1: np.array([-1.5, -1.5, -1.5, -1.5, -3, 3]),  # open
+            1: np.array([1.5, 1.5, 1.5, 1.5, 3, 3]),  # close
+        }
+
     @property
     def speed(self):
         return 0.15