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))
