def set_gripper_state(self, jval, gripper, wait=False): """ Sets goal for indicated gripper (r_gripper/l_gripper) using provided joint angle""" # Build trajectory message if not (type(jval) is list): jval = [jval] if len(jval) == 0: rospy.logwarn("No %s_gripper goal given" % gripper[0]) if gripper[0] == "l": self.l_gripper_done = True else: self.r_gripper_done = True return if gripper[0] == "l": client = self.l_gripper_client self.l_gripper_done = False elif gripper[0] == "r": client = self.r_gripper_client self.r_gripper_done = False goal = Pr2GripperCommandGoal() goal.command.max_effort = -1 goal.command.position = jval[0] if gripper[0] == "l": client.send_goal(goal, done_cb=self.__l_gripper_done_cb) elif gripper[0] == "r": client.send_goal(goal, done_cb=self.__r_gripper_done_cb) if wait: client.wait_for_result()
def _send_gripper_command(self, pos=DEFAULT_GRIPPER_POSITION, eff=DEFAULT_GRIPPER_EFFORT, wait=DEFAULT_GRIPPER_WAIT): '''Sets the position of the gripper. Args: pos (float, optional): The position ofthe gripper to move to. Close is GRIPPER_CLOSE_POSITION, open is GRIPPER_OPEN_POSITION. Defaults to DEFAULT_GRIPPER_POSITION. eff (float, optional): How much effort to exert when moving the gripper to its position. Units are in PSI (I think). Defaults to DEFAULT_GRIPPER_EFFORT. wait (bool, optional): Whether to wait for the gripper to complete its goal before returning. Defaults to DEFAULT_GRIPPER_WAIT. ''' goal = Pr2GripperCommandGoal() goal.command.position = pos goal.command.max_effort = eff self.gripper_client.send_goal(goal) if wait: self.gripper_client.wait_for_result( rospy.Duration(MAX_GRIPPER_WAIT_TIME))
def grip(self, position, max_effort): goal = Pr2GripperCommandGoal() command = Pr2GripperCommand() command.position = position command.max_effort = max_effort goal.command = command self.GCA.send_goal(goal)
def move_hand(positions, blocking=True): client = actionlib.SimpleActionClient( rospy.get_param('gripper_controller_name', 'l_gripper_controller') + '/gripper_action', Pr2GripperCommandAction) if len(positions) != 1: return False, "Wrong joint number", positions if not client.wait_for_server(): success = False reason = 'failed to connect to action server' return success, reason, positions rospy.loginfo("Connected to Finger server") client.send_goal( Pr2GripperCommandGoal( Pr2GripperCommand(position=positions[0], max_effort=-1))) try: client.wait_for_result() except KeyboardInterrupt: rospy.loginfo("Program interrupted, pre-empting goal") client.cancel_all_goals() success = False reason = 'Program interrupted from keyboard' return success, reason, positions result = client.get_result() positions = [result.position] success = True reason = None return success, reason, positions
def moveGrip(p = 0.08, arm_side = 'r'): """ Open/close the gripper """ open = Pr2GripperCommandGoal() open.command.position = p open.command.max_effort = -1.0 r_g_controller.send_goal(open)
def gripper_controll(self, position, max_effort=-1): goal = Pr2GripperCommandGoal() goal.command.position = position goal.command.max_effort = max_effort return self.gripper_client.send_goal_and_wait( goal, execute_timeout=rospy.Duration(2.0), preempt_timeout=rospy.Duration(0.1))
def _send_gripper_command(self, pos=0.08, eff=30.0, wait=True): '''Sets the position of the gripper''' command = Pr2GripperCommandGoal() command.command.position = pos command.command.max_effort = eff self.gripper_client.send_goal(command) if wait: self.gripper_client.wait_for_result(rospy.Duration(5.0))
def open_close_gripper(arm): if arm is "right": service = "r_gripper_controller/gripper_action" else: service = "l_gripper_controller/gripper_action" client = actionlib.SimpleActionClient(service, Pr2GripperCommandAction) client.wait_for_server() goal = Pr2GripperCommandGoal() goal.command.position = 0.08 goal.command.max_effort = -1 # open fast. client.send_goal(goal) client.wait_for_result() time.sleep(5) goal = Pr2GripperCommandGoal() goal.command.position = 0.0 goal.command.max_effort = 50 # close slowly client.send_goal(goal) client.wait_for_result()
def gripper_action(self, position, max_effort=-1.0, block=False, timeout=20.0): """Send goal to gripper action server""" goal = Pr2GripperCommandGoal(Pr2GripperCommand(position, max_effort)) self.def_gripper_ac.send_goal(goal) if block: return self.def_gripper_ac.wait_for_result(rospy.Duration(timeout))
def __init__(self, side=None): rospy.init_node('gripper_client') self._client = actionlib.SimpleActionClient( '/' + side + '_gripper_controller/gripper_action', Pr2GripperCommandAction) self._goal = Pr2GripperCommandGoal() if not self._client.wait_for_server(rospy.Duration(10.0)): rospy.logerr('The gripper server is not connected') rospy.signal_shutdown("Action Server not found") import sys sys.exit(1)
def set_gripper(gripper, pos, wait=True, max_effort=25): msg = Pr2GripperCommandGoal() msg.command.position = pos msg.command.max_effort = max_effort if gripper.lower() == 'l': l_gripper_client.send_goal(msg) if wait: l_gripper_client.wait_for_result() elif gripper.lower() == 'r': r_gripper_client.send_goal(msg) if wait: r_gripper_client.wait_for_result()
def moveGripper(whichgripper, position, effort): gripperGoal = Pr2GripperCommandGoal() gripperGoal.command.position = position gripperGoal.command.max_effort = effort print 'sending gripper command goal' if whichgripper == 'l': l_gripper_action_client.send_goal(gripperGoal) l_gripper_action_client.wait_for_result() elif whichgripper == 'r': r_gripper_action_client.send_goal(gripperGoal) r_gripper_action_client.wait_for_result() else: print whichgripper, 'needs to be either l or r'
def main(): rospy.init_node("load_tool") from optparse import OptionParser p = OptionParser() p.add_option('-w', '--wait', dest="wait", default=6, help="Set wait time.") p.add_option('-r', '--relax', dest="relax", default=False, action="store_true", help="Set the gripper torque to 0.") p.add_option('-t', '--tighten', dest="tighten", default=False, action="store_true", help="Set the gripper torque to 30.") (opts, args) = p.parse_args() g_client = actionlib.SimpleActionClient( 'l_gripper_controller/gripper_action', Pr2GripperCommandAction) g_client.wait_for_server() g_goal = Pr2GripperCommandGoal() if opts.relax: g_goal.command.position = 0 g_goal.command.max_effort = 0 g_client.send_goal(g_goal) #g_client.wait_for_result() return if opts.tighten: g_goal.command.position = 0 g_goal.command.max_effort = 30 g_client.send_goal(g_goal) g_client.wait_for_result() return g_goal.command.position = 1 g_goal.command.max_effort = -1 g_client.send_goal(g_goal) g_client.wait_for_result() rospy.sleep(float(opts.wait)) g_goal.command.position = 0 g_goal.command.max_effort = 30 g_client.send_goal(g_goal) g_client.wait_for_result()
def command_gripper(self, arm, position, max_effort=MAX_EFFORT, timeout=2.0, blocking=True): goal = Pr2GripperCommandGoal() goal.command.position = position goal.command.max_effort = max_effort client = "%s_gripper" % arm return self._send_command(client, goal, blocking=blocking, timeout=timeout)
def command_gripper(self, position, max_effort, blocking=0): if not self.gripper_controller_state == 'running': self.check_controller_states() if not self.gripper_controller_state == 'running': rospy.logwarn( "gripper controller not running! Attempting to start") self.start_gripper_controller() goal = Pr2GripperCommandGoal() goal.command.position = position goal.command.max_effort = max_effort self.gripper_action_client.send_goal(goal) # if blocking, wait for the gripper to finish if blocking: self.gripper_action_client.wait_for_result(rospy.Duration(4.0)) time.sleep(.5)
def open_right_gripper(self): right_gripper_client = actionlib.SimpleActionClient( "l_gripper_controller/gripper_action", Pr2GripperCommandAction) print("waiting for server") right_gripper_client.wait_for_server() print("server active") goal = Pr2GripperCommandGoal() # goal.command.position = 0.0 # goal.command.max_effort = 50.0 goal.command.position = 0.08 goal.command.max_effort = -1.0 return right_gripper_client.send_goal_and_wait(goal, rospy.Duration(30.0), rospy.Duration(5.0))
def openGripper(self, a): ''' Opens the gripper using the given simpleActionClient(Either /l_gripper_controller/gripper_action or /r_gripper_controller/gripper_action). Specified the size of the operation to be, which is the distance between the two fingertips to be 0.08(max is 0.99 and min 0.00). The max_effort to be -1.0. This field places a limit on the amount of effort (force in N) to apply while moving to that position. If 'max_effort' is negative, it is ignored. @param a: simpleActionClient which must take Pr2GripperCommandGoal as a goal @todo : make it take the max_effort and position as parameter ''' rospy.loginfo("Waiting for open gripper action") a.wait_for_server() goalGripper = Pr2GripperCommandGoal() goalGripper.command.position = 0.08 goalGripper.command.max_effort = -1.0 rospy.loginfo("Opening gripper") a.send_goal_and_wait(goalGripper) return
def spin(self): l_client = actionlib.SimpleActionClient('l_gripper_controller/gripper_action', Pr2GripperCommandAction) r_client = actionlib.SimpleActionClient('r_gripper_controller/gripper_action', Pr2GripperCommandAction) l_client.wait_for_server() r_client.wait_for_server() goal = Pr2GripperCommandGoal() goal.command.position = 0.0 goal.command.max_effort = 100.0 while not rospy.is_shutdown(): if self.goal_received: l_client.send_goal(goal) r_client.send_goal(goal) l_client.wait_for_result() r_client.wait_for_result() rospy.sleep(1)
def gripper_action(self, position, max_effort=-1, blocking=False, block_timeout=20.0): print "Performing Gripper Action" if self.gripper_action_ac: command = Pr2GripperCommand(position, max_effort) goal = Pr2GripperCommandGoal(command) self.gripper_action_ac.send_goal(goal) if blocking: return self.gripper_action_ac.wait_for_result( rospy.Duration(block_timeout)) else: self.def_gripper_ac.send_goal( Pr2GripperCommand(position, max_effort)) if blocking: return self.def_gripper_ac.wait_for_result( rospy.Duration(block_timeout))
def close(self, a): ''' Close the gripper using the given simpleActionClient(Either /l_gripper_controller/gripper_action or /r_gripper_controller/gripper_action). Specified the size of the operation to be, which is the distance between the two fingertips to be 0.00(max is 0.99 and min 0.00). The max_effort set to be 50.0. This field places a limit on the amount of effort (force in N) to apply while moving to that position. @param a: simpleActionClient which must take Pr2GripperCommandGoal as a goal @todo : make it take the max_effort and position as parameter ''' rospy.loginfo("Waiting for close gripper action") a.wait_for_server() squeeze = Pr2GripperCommandGoal() squeeze.command.position = 0.0 squeeze.command.max_effort = 50.0 rospy.loginfo("Closing gripper") a.send_goal_and_wait(squeeze) return
def grip(self, openGripper=True, maxEffort=200.0, rightArm=True, miniOpen=False, stopMovement=False): msg = Pr2GripperCommandGoal() if stopMovement: pos = rospy.wait_for_message('/r_gripper_controller/state', JointControllerState).process_value msg.command.position = pos else: msg.command.position = 0.1 if openGripper else ( 0.005 if miniOpen else 0.0) msg.command.max_effort = maxEffort if stopMovement or not openGripper else -1.0 if rightArm: self.rightGripperClient.send_goal(msg) else: self.leftGripperClient.send_goal(msg)
def run_demo(self): try: if not self.published: point = PointStamped() pos = 0.8 pos2 = 0.4 pos3 = 0.0 goal = Pr2GripperCommandGoal() goal.command.max_effort = 30 goal.command.position = pos goal2 = Pr2GripperCommandGoal() goal2.command.max_effort = 30 goal2.command.position = pos self.ph.send_goal(goal) self.ph2.send_goal_and_wait(goal2) goal = Pr2GripperCommandGoal() goal.command.max_effort = 30 goal.command.position = pos2 goal2 = Pr2GripperCommandGoal() goal2.command.max_effort = 30 goal2.command.position = pos2 self.ph.send_goal(goal) self.ph2.send_goal_and_wait(goal2) goal = Pr2GripperCommandGoal() goal.command.max_effort = 30 goal.command.position = pos3 goal2 = Pr2GripperCommandGoal() goal2.command.max_effort = 30 goal2.command.position = pos3 self.ph.send_goal(goal) self.ph2.send_goal_and_wait(goal2) print goal rospy.sleep(1.) self.published = True rospy.sleep(0.1) except KeyboardInterrupt: print('interrupted!') sys.exit(0)
def gripper(self, position, effort=30): pos = np.clip(position, 0.0, 0.09) goal = Pr2GripperCommandGoal() goal.command.position = pos goal.command.max_effort = effort self.gripper_client.send_goal(goal) finished_within_time = self.gripper_client.wait_for_result( rospy.Duration(15)) if not (finished_within_time): self.gripper_client.cancel_goal() rospy.loginfo("Timed out moving " + self.arm + " gripper") return False else: state = self.gripper_client.get_state() result = self.gripper_client.get_result() if (state == 3): #3 == SUCCEEDED rospy.loginfo("Gripper Action Succeeded") return True else: rospy.loginfo("Gripper Action Failed") rospy.loginfo("Failure Result: %s" % result) return False
def release(self, event=0, acc_thresh=3, slip_thresh=0.005, blocking=False, block_timeout=20): print "Performing Gripper Release" if self.release_ac: release_event = PR2GripperEventDetectorCommand() release_event.trigger_conditions = event release_event.acceleration_trigger_magnitude = acc_thresh release_event.slip_trigger_magnitude = slip_thresh release_command = PR2GripperReleaseCommand(release_event) self.release_ac.send_goal(PR2GripperReleaseGoal(release_command)) if blocking: return self.release_ac.wait_for_result( rospy.Duration(block_timeout)) else: self.def_gripper_ac.send_goal( Pr2GripperCommandGoal(Pr2GripperCommand(0.09, -1.0))) if blocking: return self.def_gripper_ac.wait_for_result( rospy.Duration(block_timeout))
def moveGripperPR2(self, dist=0.08, effort = 15): self.gripper_action_client.send_goal(Pr2GripperCommandGoal(Pr2GripperCommand(position=dist, max_effort = effort)))
def close_gripper(self, arm, effort=15): self.gripper_action_client.send_goal( Pr2GripperCommandGoal( Pr2GripperCommand(position=0.0, max_effort=effort)))
def open_gripper(self, arm): self.gripper_action_client.send_goal( Pr2GripperCommandGoal( Pr2GripperCommand(position=0.08, max_effort=-1)))
def command_gripper(self, arm, position, max_effort, blocking, timeout): goal = Pr2GripperCommandGoal() goal.command.position = position goal.command.max_effort = max_effort client = "%s_gripper" % arm return self.send_command("%s_gripper" % arm, goal, blocking, timeout)
def move_gripper(self, arm, amount=0.08, effort=15): self.gripper_action_client[arm].send_goal( Pr2GripperCommandGoal( Pr2GripperCommand(position=amount, max_effort=effort)))
def l_action_gripper(self, value=0): goal = Pr2GripperCommandGoal() #GripperCommandGoal() goal.command.position = value goal.command.max_effort = 40 self.lgripper.send_goal_and_wait(goal)