add gripper grasp tol

StanfordVL · Jun 4, 2021 · 340bbfc · 340bbfc
1 parent 81897db
commit 340bbfc
Show file tree

Hide file tree

Showing 4 changed files with 31 additions and 5 deletions.
diff --git a/demos/demo_control_cfg.yaml b/demos/demo_control_cfg.yaml
@@ -1,6 +1,6 @@
 # Cfg file for  Demo control environment
 world:
-  type: 'Bullet' #'Bullet' or 'Real'
+  type: 'Real' #'Bullet' or 'Real'
   robot: 'panda'
   controlType: 'EEImpedance'
 data_dir: 'data'

diff --git a/demos/osc_demo.py b/demos/osc_demo.py
@@ -19,7 +19,7 @@
 
 AXIS_DIM_NUM = {'x': 0, 'y': 1, 'z': 2}
 
-DEFAULT_ROTATION_DELTA_VAL = np.pi / 600.0
+DEFAULT_ROTATION_DELTA_VAL = np.pi / 100.0
 
 
 def compute_error(goal_state, new_state):

diff --git a/perls2/redis_interfaces/panda_redis_keys.py b/perls2/redis_interfaces/panda_redis_keys.py
@@ -18,7 +18,11 @@
 GRIPPER_WIDTH_CMD_KEY = DRIVER_PREFIX + "gripper::control::width"
 GRIPPER_SPEED_CMD_KEY = DRIVER_PREFIX + "gripper::control::speed"
 GRIPPER_FORCE_CMD_KEY = DRIVER_PREFIX + "gripper::control::force"
+<<<<<<< Updated upstream
 GRIPPER_GRASP_TOL_KEY = DRIVER_PREFIX + "gripper::control::grasp_tol"
+=======
+GRIPPER_STATUS_KEY = DRIVER_PREFIX + "gripper::status"
+>>>>>>> Stashed changes
 
 # Torque cmd key: 
 TORQUE_CMD_KEY = DRIVER_PREFIX + "control::tau"

diff --git a/perls2/robots/real_panda_interface.py b/perls2/robots/real_panda_interface.py
@@ -63,7 +63,7 @@ def reset(self):
 
         self.redisClient.mset(reset_cmd)
         # Wait for reset to be read by contrl interface.
-        # time.sleep(self.RESET_TIMEOUT)
+        time.sleep(3)
         start = time.time()
 
         while (self.redisClient.get(ROBOT_RESET_COMPL_KEY) != b'True' and (time.time() - start < self.RESET_TIMEOUT)):
@@ -214,21 +214,43 @@ def open_gripper(self):
         """
         self.set_gripper_to_value(0.99, mode='move')
 
+
     def close_gripper(self):
         """ Close robot gripper
         """
         self.set_gripper_to_value(0.1)
-
+        
     def set_gripper_to_value(self, value, mode='grasp'):
         """Set gripper to desired value
         """
         if (value > 1.0 or value < 0):
             raise ValueError("Invalid gripper value must be fraction between 0 and 1")
 
-        self.redisClient.set(ROBOT_SET_GRIPPER_CMD_KEY, value*self.GRIPPER_MAX_VALUE)
+        self.redisClient.set(ROBOT_SET_GRIPPER_CMD_KEY, value*self.GRIPPER_MAX_VALUE*0.95)
         self.redisClient.set(ROBOT_GRIPPER_CMD_TYPE_KEY, mode)
         self.redisClient.set(ROBOT_SET_GRIPPER_CMD_TSTAMP_KEY, time.time())
 
     @property
     def gripper_position(self):
         return self.redisClient.get(P.GRIPPER_WIDTH_KEY)
+
+    @property
+    def gripper_status(self):
+        return self.redisClient.get(P.GRIPPER_STATUS_KEY).decode()
+
+    def stop_gripper(self):
+        """Stop current grasp
+        """
+        self.redisClient.set(ROBOT_SET_GRIPPER_CMD_TSTAMP_KEY, time.time())
+        self.redisClient.set(ROBOT_GRIPPER_CMD_TYPE_KEY, "stop")
+        # start = time.time()
+        # while (self.gripper_status != 'not_grasped') and (time.time() - start < 3):
+        #     time.sleep(0.1)
+        # if self.gripper_status != 'not_grasped':
+        #     raise ValueError("Gripper not stopped.")
+
+        # self.open_gripper()
+        # start = time.time()
+        # while (self.redisClient.get("franka_panda::gripper::control::mode").decode() != 'idle') and (time.time() - start < 15):
+        #     time.sleep(0.1)
+        # print(self.redisClient.get("franka_panda::gripper::control::mode").decode())