Gripper penalty and blocking environment

README.md

@@ -21,7 +21,15 @@ SERL provides a set of libraries, env wrappers, and examples to train RL policie
   - [Contribution](#contribution)
   - [Citation](#citation)
 
-## Major bug fix
+## Major updates
+#### June 24, 2024
+For people who use SERL for tasks involving controlling the gripper (e.g.,pick up objects), we strong recommend adding a small penalty to the gripper action change, as it will greatly improves the training speed.
+For detail, please refer to: [PR #65](https://github.com/rail-berkeley/serl/pull/65).
+
+
+Further, we also recommend  providing interventions online during training in addition to loading the offline demos. If you have a Franka robot and SpaceMouse, this can be as easy as just touching the SpaceMouse during training.
+
+#### April 25, 2024
 We fixed a major issue in the intervention action frame. See release [v0.1.1](https://github.com/rail-berkeley/serl/releases/tag/v0.1.1) Please update your code with the main branch.
 
 ## Installation

serl_robot_infra/franka_env/envs/bin_relocation_env/config.py

@@ -21,7 +21,7 @@ class BinEnvConfig(DefaultEnvConfig):
         ]
     )
     RESET_POSE = TARGET_POSE + np.array([0.0, 0.0, 0.1, 0.0, 0.0, 0.0])
-    REWARD_THRESHOLD: np.ndarray = np.array([0.01, 0.01, 0.01, 0.2, 0.2, 0.2])
+    REWARD_THRESHOLD: np.ndarray = np.zeros(6)
     ACTION_SCALE = np.array([0.05, 0.1, 1])
     RANDOM_RESET = False
     RANDOM_XY_RANGE = 0.1

serl_robot_infra/franka_env/envs/cable_env/config.py

@@ -21,7 +21,7 @@ class CableEnvConfig(DefaultEnvConfig):
         ]
     )
     RESET_POSE = TARGET_POSE + np.array([0.0, 0.0, 0.1, 0.0, 0.0, 0.0])
-    REWARD_THRESHOLD: np.ndarray = np.array([0.01, 0.01, 0.01, 0.2, 0.2, 0.2])
+    REWARD_THRESHOLD: np.ndarray = np.zeros(6)
     ACTION_SCALE = np.array([0.05, 0.3, 1])
     RANDOM_RESET = True
     RANDOM_XY_RANGE = 0.1

serl_robot_infra/franka_env/envs/cable_env/franka_cable_route.py

@@ -19,6 +19,7 @@ def go_to_rest(self, joint_reset=False):
         Move to the rest position defined in base class.
         Add a small z offset before going to rest to avoid collision with object.
         """
+        self._send_gripper_command(-1)
         self._update_currpos()
         self._send_pos_command(self.currpos)
         time.sleep(0.5)

serl_robot_infra/franka_env/envs/franka_env.py

@@ -59,6 +59,8 @@ class DefaultEnvConfig:
     ABS_POSE_LIMIT_LOW = np.zeros((6,))
     COMPLIANCE_PARAM: Dict[str, float] = {}
     PRECISION_PARAM: Dict[str, float] = {}
+    BINARY_GRIPPER_THREASHOLD: float = 0.5
+    GRIPPER_PENALTY: float = 0.1
 
 
 ##############################################################################
@@ -94,6 +96,7 @@ def __init__(
         self.currjacobian = np.zeros((6, 7))
 
         self.curr_gripper_pos = 0
+        self.gripper_binary_state = 0  # 0 for open, 1 for closed
         self.lastsent = time.time()
         self.randomreset = config.RANDOM_RESET
         self.random_xy_range = config.RANDOM_XY_RANGE
@@ -201,7 +204,7 @@ def step(self, action: np.ndarray) -> tuple:
 
         gripper_action = action[6] * self.action_scale[2]
 
-        self._send_gripper_command(gripper_action)
+        gripper_action_effective = self._send_gripper_command(gripper_action)
         self._send_pos_command(self.clip_safety_box(self.nextpos))
 
         self.curr_path_length += 1
@@ -210,11 +213,11 @@ def step(self, action: np.ndarray) -> tuple:
 
         self._update_currpos()
         ob = self._get_obs()
-        reward = self.compute_reward(ob)
-        done = self.curr_path_length >= self.max_episode_length or reward
-        return ob, int(reward), done, False, {}
+        reward = self.compute_reward(ob, gripper_action_effective)
+        done = self.curr_path_length >= self.max_episode_length or reward == 1
+        return ob, reward, done, False, {}
 
-    def compute_reward(self, obs) -> bool:
+    def compute_reward(self, obs, gripper_action_effective) -> bool:
         """We are using a sparse reward function."""
         current_pose = obs["state"]["tcp_pose"]
         # convert from quat to euler first
@@ -223,10 +226,15 @@ def compute_reward(self, obs) -> bool:
         current_pose = np.hstack([current_pose[:3], euler_angles])
         delta = np.abs(current_pose - self._TARGET_POSE)
         if np.all(delta < self._REWARD_THRESHOLD):
-            return True
+            reward = 1
         else:
             # print(f'Goal not reached, the difference is {delta}, the desired threshold is {_REWARD_THRESHOLD}')
-            return False
+            reward = 0
+
+        if gripper_action_effective:
+            reward -= self.config.GRIPPER_PENALTY
+
+        return reward
 
     def crop_image(self, name, image) -> np.ndarray:
         """Crop realsense images to be a square."""
@@ -379,12 +387,24 @@ def _send_pos_command(self, pos: np.ndarray):
     def _send_gripper_command(self, pos: float, mode="binary"):
         """Internal function to send gripper command to the robot."""
         if mode == "binary":
-            if (pos >= -1) and (pos <= -0.9):  # close gripper
+            if (
+                pos <= -self.config.BINARY_GRIPPER_THREASHOLD
+                and self.gripper_binary_state == 0
+            ):  # close gripper
                 requests.post(self.url + "close_gripper")
-            elif (pos >= 0.9) and (pos <= 1):  # open gripper
+                time.sleep(0.6)
+                self.gripper_binary_state = 1
+                return True
+            elif (
+                pos >= self.config.BINARY_GRIPPER_THREASHOLD
+                and self.gripper_binary_state == 1
+            ):  # open gripper
                 requests.post(self.url + "open_gripper")
+                time.sleep(0.6)
+                self.gripper_binary_state = 0
+                return True
             else:  # do nothing to the gripper
-                return
+                return False
         elif mode == "continuous":
             raise NotImplementedError("Continuous gripper control is optional")
 

serl_robot_infra/franka_env/envs/pcb_env/franka_pcb_insert.py

@@ -27,6 +27,7 @@ def go_to_rest(self, joint_reset=False):
         Move to the rest position defined in base class.
         Add a small z offset before going to rest to avoid collision with object.
         """
+        self._send_gripper_command(-1)
         self._update_currpos()
         self._send_pos_command(self.currpos)
         time.sleep(0.5)

serl_robot_infra/franka_env/envs/peg_env/franka_peg_insert.py

@@ -18,6 +18,7 @@ def go_to_rest(self, joint_reset=False):
         Move to the rest position defined in base class.
         Add a small z offset before going to rest to avoid collision with object.
         """
+        self._send_gripper_command(-1)
         self._update_currpos()
         self._send_pos_command(self.currpos)
         time.sleep(0.5)

serl_robot_infra/franka_env/envs/wrappers.py

@@ -49,8 +49,9 @@ def compute_reward(self, obs):
 
     def step(self, action):
         obs, rew, done, truncated, info = self.env.step(action)
-        rew = self.compute_reward(self.env.get_front_cam_obs())
-        done = done or rew
+        success = self.compute_reward(self.env.get_front_cam_obs())
+        rew += success
+        done = done or success
         return obs, rew, done, truncated, info
 
 
@@ -72,8 +73,9 @@ def compute_reward(self, obs):
 
     def step(self, action):
         obs, rew, done, truncated, info = self.env.step(action)
-        rew = self.compute_reward(self.env.get_front_cam_obs())
-        done = done or rew
+        success = self.compute_reward(self.env.get_front_cam_obs())
+        rew += success
+        done = done or success
         return obs, rew, done, truncated, info
 
 
@@ -94,8 +96,9 @@ def compute_reward(self, obs):
 
     def step(self, action):
         obs, rew, done, truncated, info = self.env.step(action)
-        rew = self.compute_reward(obs)
-        done = done or rew
+        success = self.compute_reward(obs)
+        rew += success
+        done = done or success
         return obs, rew, done, truncated, info