diff --git a/demos/demo_control_cfg.yaml b/demos/demo_control_cfg.yaml index f9601c0f..947447a2 100644 --- 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 index 0305ebc3..f1815dc2 100644 --- 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 index f4793102..ad937553 100644 --- 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 index 4aebe072..5cb388c2 100644 --- 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())