def grasp_client(): client = actionlib.SimpleActionClient('/franka_gripper/grasp', GraspAction) print("waiting for server") client.wait_for_server() eps = GraspEpsilon() eps.inner = 0.001 eps.outer = 0.001 goal = GraspGoal(width=0.001, epsilon=eps, speed=0.08, force=70.0) client.send_goal(goal) client.wait_for_result() return client.get_result()
def grasp(self, width, force, speed=None, epsilon_inner=0.005, epsilon_outer=0.005, wait_for_result=True, cb=None): """ Grasps an object. An object is considered grasped if the distance :math:`d` between the gripper fingers satisfies :math:`(width - epsilon\_inner) < d < (width + epsilon\_outer)`. :param width: Size of the object to grasp. [m] :param speed: Closing speed. [m/s] :param force: Grasping force. [N] :param epsilon_inner: Maximum tolerated deviation when the actual grasped width is smaller than the commanded grasp width. :param epsilon_outer: Maximum tolerated deviation when the actual grasped width is wider than the commanded grasp width. :param cb: Optional callback function to use when the service call is done :type width: float :type speed: float :type force: float :type epsilon_inner: float :type epsilon_outer: float :return: True if an object has been grasped, false otherwise. :rtype: bool """ self._caller = "grasp_action" if not speed: speed = self._gripper_speed goal = GraspGoal() goal.width = width goal.speed = speed goal.force = force goal.epsilon = GraspEpsilon(inner=epsilon_inner, outer=epsilon_outer) if not cb: cb = self._done_cb self._grasp_action_client.send_goal(goal, done_cb=cb, active_cb=self._active_cb, feedback_cb=self._feedback_cb) if wait_for_result: result = self._grasp_action_client.wait_for_result( rospy.Duration(15.)) return result return True
def btn_left_gripper_close_on_click(self): force = self._widget.spinbox_left_gripper_force.value() length = self._widget.spinbox_left_gripper_width.value() epsilon = GraspEpsilon(inner=0.027, outer=0.027) req = GraspGoal(width=length, epsilon=epsilon, speed=0.1, force=force) if self.panda_gripper_grasp_client.wait_for_server( rospy.Duration(1.0)): self.panda_gripper_grasp_client.send_goal(req) self._widget.textBrowser.append("LEFT: gripper_close | force: " + str(force) + " | length: " + str(length)) else: self._widget.textBrowser.append( "SERVER NOT WORKING -- LEFT: gripper_close")
def graspGripper(self, width, speed=0.5, force=10, epsilon_inner=0.002, epsilon_outer=0.002): width_normalized = self._normalize(width, self._MIN_WIDTH, self._MAX_WIDTH) force_normalized = self._normalize(force, self._MIN_FORCE, self._MAX_FORCE) if self._real_robot: # Create goal goal = GraspGoal() goal.width = width_normalized goal.epsilon = GraspEpsilon(epsilon_inner, epsilon_outer) goal.speed = speed goal.force = force_normalized # Send goal self._client_grasp.send_goal_and_wait(goal, rospy.Duration.from_sec(self._timeout)) else: self._moveHand(width_normalized) print("GRASPING...") self._current_width = width_normalized
def gripper_grasp(self, width, epsilon_inner=0.05, epsilon_outer=0.05, speed=0.1, force=60): epsilon = GraspEpsilon(inner=epsilon_inner, outer=epsilon_outer) goal = GraspGoal(width=width, epsilon=epsilon, speed=speed, force=force) rospy.loginfo('Grasping:\n{}'.format(goal)) self._gripper_grasp.send_goal(goal) self._gripper_grasp.wait_for_result() while not self._gripper_grasp.get_result().success: rospy.logerr('Failed to grasp, retry') self.gripper_move(width=0.025) self._gripper_grasp.send_goal(goal) self._gripper_grasp.wait_for_result() if rospy.is_shutdown(): sys.exit(0)
def grasp( self, width: float, force: float, speed: float = 0.05, epsilon_inner: float = 0.005, epsilon_outer: float = 0.005, wait_for_result: bool = False, ) -> bool: """Grasps an object. First, the gripper moves with speed `speed` in the direction of `width`: - open if param `width` if higher than current gripper width - close if param `width` if lower than current gripper width Opening/closing stops when a collision is detected (if no collision is detected, the action is aborted). Then, the force `force` is applied. The force is released as soon as the gripper width goes outside the interval $[(\text{width} - \text{epsilon_inner}) ; (\text{width} + \text{epsilon_outer})]$. Args: width (float): Size [m] of the object to grasp. force (float): Grasping force [N]. speed (float, optionnal): Closing speed [m/s]. Default to 0.05. epsilon_inner (float, optionnal): Maximum inner tolerated deviation [m]. Defaults to 0.005. epsilon_outer (float, optionnal): Maximum outter tolerated deviation [m]. Defaults to 0.005. Returns: bool: Whether command was successful. """ width = self._clip_width(width) force = self._clip_force(force) goal = GraspGoal() goal.width = width goal.speed = speed goal.force = force goal.epsilon = GraspEpsilon(inner=epsilon_inner, outer=epsilon_outer) self._grasp_action_client.send_goal(goal) if wait_for_result: return self._grasp_action_client.wait_for_result( timeout=rospy.Duration(15.0)) else: return True
def close_gripper(): """Closes robot gripper If using ``real_panda`` then call move gripper action server. If using gazebo then publish desired grip width to */franka/gripper_width* topic. Returns: bool: returns True when done. """ if real_panda: client = actionlib.SimpleActionClient('/franka_gripper/grasp', GraspAction) rospy.loginfo("CONNECTING") client.wait_for_server() action = GraspGoal(width=0.5,speed=0.08,force=1) rospy.loginfo("SENDING ACTION") client.send_goal(action) client.wait_for_result(rospy.Duration.from_sec(5.0)) rospy.loginfo("DONE") else: pub_gripper.publish(0.05) rospy.sleep(2) return True
char = screen.getch() if char == ord('q'): break elif char == curses.KEY_RIGHT: # print doesn't work with curses, use addstr instead screen.addstr(0, 0, 'open gripper (right)') if (set_width <= max_open): client = actionlib.SimpleActionClient('/franka_gripper/move', MoveAction) client.wait_for_server() #set_width += step_size set_width = release goal = MoveGoal(width=set_width, speed=0.04) client.send_goal(goal) elif char == curses.KEY_LEFT: screen.addstr(0, 0, 'close gripper (left)') if (set_width >= min_open): client = actionlib.SimpleActionClient('/franka_gripper/grasp', GraspAction) client.wait_for_server() #set_width -= step_size set_width = grasp action = GraspGoal(width=set_width, speed=0.04, force=1) client.send_goal(action) elif char == curses.KEY_UP: curses.nocbreak() screen.keypad(0) curses.echo() curses.endwin() loop = False
file_dir = os.path.dirname(os.path.abspath(__file__)) + '/data/expert_demo/demo.pkl' logger = {'inputs': [], 'outputs' : []} rospy.init_node('joy_interface') tf_listener = tf.TransformListener() joy_listener = JoyListener() pub = rospy.Publisher('/pose_cmd', Pose, queue_size=1) client = actionlib.SimpleActionClient('/franka_gripper/grasp', GraspAction) client.wait_for_server() grasp_goal = GraspGoal() grasp_eps = GraspEpsilon() grasp_eps.inner = 0.005 grasp_eps.outer = 0.005 grasp_goal.width = 0.1 grasp_goal.epsilon = grasp_eps grasp_goal.speed = 0.1 grasp_goal.force = 0.001 hz = 10.0 dt = 1.0/hz rate = rospy.Rate(hz) pose_cmd = Pose() expert_demo = ExpertDemo()