Merge branch 'bctest' of https://github.com/StanfordVL/perls2 into bc…

…test

local/perls2_local_control_pc/franka-panda.yaml

@@ -188,5 +188,5 @@ control:
 
     # Collision force thresholds [N, Nm]
     # Either a single double or an array of size 6
-    f_collision: 100.
+    f_collision: 200.
 

perls2/controllers/ee_imp.py

@@ -224,6 +224,7 @@ def set_goal(self, delta, set_pos=None, set_ori=None, **kwargs):
                 raise ValueError("incorrect delta dimension")
 
             scaled_delta = self.scale_action(delta)
+            # print("scaled_delta {}".format(delta))
             self.goal_ori = C.set_goal_orientation(
                 scaled_delta[3:],
                 self.model.ee_ori_mat,
@@ -301,6 +302,7 @@ def run_controller(self):
 
         # Calculate desired force, torque at ee using control law and error.
         position_error = desired_pos - self.model.ee_pos
+        # print("pos_error {}".format(position_error))
         vel_pos_error = desired_vel_pos - self.model.ee_pos_vel
         desired_force = (
             np.multiply(np.array(position_error),

perls2/controllers/utils/control_utils.py

@@ -105,6 +105,7 @@ def set_goal_position(delta,
     n = len(current_position)
     if set_pos is not None:
         goal_position = set_pos
+        # print("goal_position {}".format(goal_position))
     else:
         goal_position = current_position + delta
 

perls2/ctrl_interfaces/panda_ctrl_interface.py

@@ -20,8 +20,8 @@
 
 ENV_CHECK_COUNT = 100   # how often we check if workstation is connected.
 LOOP_TIME_REPORT_COUNT = 1000
-MAX_TORQUE = 2.5        # Clip torque commands before sending to driver.
-MIN_TORQUE = -2.5
+MAX_TORQUE = [10, 10, 10, 10, 5, 5, 5]        # Clip torque commands before sending to driver.
+MIN_TORQUE = [-10, -10, -10, -10, -5, -5, -5]
 
 def keyboardInterruptHandler(signal, frame):
     print("KeyboardInterrupt: Exiting Panda Ctrl Interface".format(signal))
@@ -136,7 +136,7 @@ def reset_to_neutral(self):
 
         # Wait for reset to complete.
         while (self.redisClient.get(P.CONTROL_MODE_KEY).decode() != P.IDLE_CTRL_MODE):
-            time.sleep(1)
+            time.sleep(0.1)
         self.redisClient.set(R.ROBOT_RESET_COMPL_KEY, 'True')
         self.action_set = False
 
@@ -169,15 +169,16 @@ def open_gripper(self):
         """
         raise NotImplementedError
 
-    def set_gripper_to_value(self, value, cmd_type):
+    def set_gripper_to_value(self, value, cmd_type, force=10.0):
         """
         Set the gripper grasping opening state
-        :param openning: a float between 0 (closed) and 1 (open).
+        :param value: a gripper width (m) to set grasp
+        :param force: (optional) float force (N/m) with which to close gripper.
         :return: None
         """
-
         self.redisClient.set(P.GRIPPER_WIDTH_CMD_KEY, value)
-
+        self.redisClient.set(P.GRIPPER_GRASP_TOL_KEY, "1.0 1.0")
+        self.redisClient.set(P.GRIPPER_FORCE_CMD_KEY, force)
         self.redisClient.set(P.GRIPPER_MODE_KEY, cmd_type)
 
     def step(self, start):
@@ -202,6 +203,8 @@ def get_gripper_cmd_tstamp(self):
     def des_gripper_state(self):
         return float(self.redisClient.get(R.ROBOT_SET_GRIPPER_CMD_KEY))
 
+
+
     def check_for_new_gripper_cmd(self):
         gripper_cmd_tstamp = self.get_gripper_cmd_tstamp()
         if self.last_gripper_cmd_tstamp is None or (self.last_gripper_cmd_tstamp != gripper_cmd_tstamp):
@@ -216,11 +219,12 @@ def process_gripper_cmd(self, cmd_data, cmd_type):
         Only send gripper command if current gripper open fraction is not
         equal to desired gripper open fraction.
         """
-        if self.prev_gripper_state != cmd_data:
-            self.set_gripper_to_value(cmd_data, cmd_type)
-            self.prev_gripper_state = cmd_data
-        else:
-            pass
+        if cmd_type is not None:
+            if cmd_type.decode() == "stop":
+                self.stop_gripper()
+        self.set_gripper_to_value(cmd_data, cmd_type)
+        self.prev_gripper_state = cmd_data
+
 
     def wait_for_env_connect(self):
         """Blocking code that waits for perls2.RobotInterface to connect to redis.
@@ -389,6 +393,10 @@ def run_dummy(self):
         except KeyboardInterrupt:
             pass
 
+    def stop_gripper(self):
+        """Send command to terminate current grasp (libfranka)
+        """
+        self.redisClient.set(P.GRIPPER_MODE_KEY, "stop")
 
 
 if __name__ == '__main__':

perls2/redis_interfaces/panda_redis_keys.py

@@ -18,6 +18,7 @@
 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"
+GRIPPER_GRASP_TOL_KEY = DRIVER_PREFIX + "gripper::control::grasp_tol"
 
 # Torque cmd key: 
 TORQUE_CMD_KEY = DRIVER_PREFIX + "control::tau"