def gripper(self,cmd): if(cmd == "open"): print("Opening Gripper") data = Pr2GripperCommand() data.position = 0.1 data.max_effort= 100.0 self.gripper_pub.publish(data) rospy.sleep(1) elif(cmd == "close"): print("Closing Gripper") data = Pr2GripperCommand() data.position = 0.0 data.max_effort= 100.0 self.gripper_pub.publish(data) rospy.sleep(1)
def go_right_gripper(self, position, max_effort): """Move right gripper to position with max_effort """ ope = Pr2GripperCommand() ope.position = position ope.max_effort = max_effort self.r_gripper_pub.publish(ope)
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 joy_cb(self, data): """ :type data: Joy """ new_t = time.time() last_t = self.last_joy_stamp if (new_t - last_t) < (1.0 / self.node_rate_hz): return self.gripper_amount = self.get_current_gripper_position() if data.buttons[self.deadman_button]: self.gripper_amount += data.buttons[ self.open_button] * self.gripper_step self.gripper_amount += data.buttons[ self.close_button] * -self.gripper_step if self.gripper_amount < self.min_gripper_pos: self.gripper_amount = self.min_gripper_pos elif self.gripper_amount > self.max_gripper_pos: self.gripper_amount = self.max_gripper_pos if self.gripper_amount != self.last_gripper_amount: cmd = Pr2GripperCommand() cmd.max_effort = 100.0 cmd.position = self.gripper_amount self.gripper_pub.publish(cmd) rospy.loginfo("Gripper cmd: " + self.gripper_command_topic + " receiving: " + str(cmd)) self.last_gripper_amount = self.gripper_amount
def open_l_gripper(self): gripper_command_msg = Pr2GripperCommand() gripper_command_msg.max_effort = 40.0 gripper_command_msg.position = 10.0 r = rospy.Rate(10.0) t_init = rospy.Time.now() while (rospy.Time.now() - t_init).to_sec() < 3.0 and not rospy.is_shutdown(): if self.execute_exit(): return False self._l_gripper_pub.publish(gripper_command_msg) # check that preempt has not been requested by the client if self._as.is_preempt_requested(): #HERE THE CODE TO EXECUTE WHEN THE BEHAVIOR TREE DOES HALT THE ACTION rospy.loginfo('action halted while opening gripper') self._as.set_preempted() self._exit = True if self.execute_exit(): return False r.sleep() return True
def __init__(self): self.pub = rospy.Publisher('r_gripper_controller/command', Pr2GripperCommand, queue_size=10) self.grip_msg = Pr2GripperCommand() self.grip_msg.max_effort = -1 self.grip_msg.position = 0.045
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 execute(self): super(Pr2GripperAction, self).execute() command = Pr2GripperCommand(0.04, 0) command.max_effort = 20.0 command.position = self._values[0] #print('Pr2GripperAction.execute() %s: %s' % (self.__class__.__name__, str(self._values[0]))) self._pub.publish(command) self._timer = rospy.Timer(rospy.Duration.from_sec(self.get_duration()), self._timer_finished, oneshot=True)
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_grippers(self): rospy.loginfo("Closing both grippers.") msg = Pr2GripperCommand() msg.position = 0.0 msg.max_effort = 10000.0 for pub in self.gripper_pubs: pub.publish(msg) pub.publish(msg) pub.publish(msg)
def grab(self, gain=0.03, blocking=False, block_timeout=20): print "Performing Gripper Grab" if self.grab_ac: grab_goal = PR2GripperGrabGoal(PR2GripperGrabCommand(gain)) self.grab_ac.send_goal(grab_goal) if blocking: return self.grab_ac.wait_for_result( rospy.Duration(block_timeout)) else: self.def_gripper_ac.send_goal(Pr2GripperCommand(0.0, -1.0)) if blocking: return self.def_gripper_ac.wait_for_result( rospy.Duration(block_timeout))
def open_gripper(ag_name, wait=1): global agents global rate gripper = agents[ag_name]['gripper'] opened = Pr2GripperCommand() opened.position = 1.0 opened.max_effort = -10.0 i = 0 while i < 30: gripper.publish(opened) rate.sleep() i += 1 rate.sleep() rospy.sleep(wait)
def close_gripper(ag_name, wait=1): global agents global rate gripper = agents[ag_name]['gripper'] closed = Pr2GripperCommand() closed.position = 0.0 closed.max_effort = -10.0 i = 0 while i < 30: gripper.publish(closed) rate.sleep() i += 1 rospy.sleep(wait)
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 move_gripper(self, arm, amount=0.08, effort=15): self.gripper_action_client[arm].send_goal( Pr2GripperCommandGoal( Pr2GripperCommand(position=amount, max_effort=effort)))
def left_close(): pub_gripper.publish(Pr2GripperCommand(0.0, 32))
def left_open(): pub_gripper.publish(Pr2GripperCommand(0.025, 32))
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 pose_gripper_r(axes): control = Pr2GripperCommand(0.04, 0) control.max_effort = 10.0 control.position = 0.08 * ((axes[0] + 1.0) / 2.0) gripper_r.publish(control)
def grip(self, amount=0.060, times=10): for i in range(times): self.grip_msg = Pr2GripperCommand() self.grip_msg.max_effort = -1 self.grip_msg.position = amount self.pub.publish(self.grip_msg)
def open_gripper(side): pub = get_gripper(side) pub.publish(Pr2GripperCommand(GRIPPER_OPEN, 32))
def close_gripper(side): pub = get_gripper(side) pub.publish(Pr2GripperCommand(-0.01, 32))