Update Reach task and xarm6 init qpos

Author: dwaitbhatt

robosuite/environments/manipulation/reach.py

@@ -187,8 +187,8 @@ def __init__(
 
         self.target_pos = target_pos
         if self.target_pos is None:
-            self.target_pos_min = [-0.05, -0.25, 0.8]
-            self.target_pos_max = [0.15, 0.25, 1.2]
+            self.target_pos_min = [-0.05, -0.25, 0.73]
+            self.target_pos_max = [0.15, 0.25, 0.95]
 
         self.base_types = base_types
 
@@ -255,6 +255,12 @@ def reward(self, action=None):
             # reaching_reward = -((2 * dist) ** 2)
             reward += reaching_reward
 
+            # penalize for non-zero joint velocities once close to target
+            if target_to_gripper_dist < 0.03:
+                qvels = np.linalg.norm(self.robots[0]._joint_velocities)
+                static_reward = 1 - np.tanh(10.0 * qvels)
+                reward += static_reward
+
         return reward
 
     def _load_model(self):
@@ -286,9 +292,9 @@ def _load_model(self):
 
         # Set target location without physical objects
         if self.target_pos is None:
-            target_pos = np.random.uniform(low=self.target_pos_min, high=self.target_pos_max)
+            self.target_pos = np.random.uniform(low=self.target_pos_min, high=self.target_pos_max)
         else:
-            target_pos = self.target_pos
+            self.target_pos = self.target_pos
 
         self.target = BallObject(
             name="target",
@@ -297,7 +303,7 @@ def _load_model(self):
             obj_type='visual',
             joints=None
         )
-        self.target.get_obj().set("pos", " ".join([str(num) for num in target_pos]))
+        self.target.get_obj().set("pos", " ".join([str(num) for num in self.target_pos]))
 
         # task includes arena, robot, and objects of interest
         self.model = ManipulationTask(
@@ -410,6 +416,9 @@ def _reset_internal(self):
         """
         Resets simulation internal configurations.
         """
+        self.target_pos = np.random.uniform(low=self.target_pos_min, high=self.target_pos_max)
+        self.target.get_obj().set("pos", " ".join([str(num) for num in self.target_pos]))
+
         super()._reset_internal()
 
 
@@ -430,15 +439,16 @@ def _check_success(self):
         Check if gripper has reached target
 
         Returns:
-            bool: True if target has been reached
+            bool: True if target has been reached, and robot is static
         """
 
         target_to_gripper_dist = self._gripper_to_target(
             gripper=self.robots[0].gripper, target=self.target.root_body, target_type="body", return_distance=True
         )
+        is_robot_static = np.linalg.norm(self.robots[0]._joint_velocities) < 0.01
 
-        # gripper is within a small distance to the target
-        return target_to_gripper_dist < 0.03
+        # gripper is within a small distance to the target and robot is static
+        return target_to_gripper_dist < 0.03 and is_robot_static
 
     def reset_target(self):
 

robosuite/models/robots/manipulators/xarm6_robot.py

@@ -48,7 +48,10 @@ def init_qpos(self):
         # return np.array([3.74565364e-03, -9.37793861e-01, -2.82236445e-03, 6.77735371e-04, 9.08002899e-01, -8.60226130e-03])
 
         # For EEF z 0.05 above table when xarm6 is placed on table directly
-        return np.array([8.84830716e-03, -6.66687447e-01, 4.89064751e-02, 1.94762656e-04, 5.84571386e-01, -3.86422130e-03])
+        # np.array([8.84830716e-03, -6.66687447e-01, 4.89064751e-02, 1.94762656e-04, 5.84571386e-01, -3.86422130e-03])
+
+        # From maniskill experiments
+        return np.array([6.672368e-4, 2.2206e-1, -1.2311444, 1.6927806e-4, 1.0088931, 7.304605e-4])
 
     @property
     def base_xpos_offset(self):