def actionCb(self, goal):
""" Take an input command of width to open gripper. """
rospy.loginfo('Gripper controller action goal recieved:%f' % goal.command.position)
command = goal.command.position
# publish msgs
cmsg = GripperCmd();
pos = command;
if pos>0.08:
cmsg.position = 0.08 - pos/10;
else:
cmsg.position = 0.08 - pos;
#cmsg.position = 0.8 - command
cmsg.speed = 0.06
cmsg.force = 100.0
cmsg.emergency_release = bool(0)
#cmsg.emergency_release_dir = 0
#cmsg.stop = 0
self.r_pub.publish(cmsg)
rospy.sleep(5.0)
self.server.set_succeeded()
rospy.loginfo('Gripper Controller: Done.')
def __init__(self):
rospy.init_node('ur_control')
self.target_flag = False
self.target_pose = geometry_msgs.msg.Pose()
rospy.Subscriber('/ar_pose_marker', AlvarMarkers, self.ar_pose_cb)
pub = rospy.Publisher('/gripper/cmd', GripperCmd, queue_size=10)
moveit_commander.roscpp_initialize(sys.argv)
arm_group = moveit_commander.MoveGroupCommander("ur5_arm")
print 'waiting for marker...'
while self.target_flag is False:
rospy.sleep(0.05)
print 'got marker pose, planning...'
arm_group.set_pose_target(self.target_pose)
plan1 = arm_group.plan()
print plan1
arm_group.execute(plan1)
rospy.sleep(5.0)
while not rospy.is_shutdown():
gripper_cmd = GripperCmd()
gripper_cmd.position = 0
gripper_cmd.speed = 0.2
gripper_cmd.force = 2.0
pub.publish(gripper_cmd)
moveit_commander.roscpp_shutdown()
def talker():
pub = rospy.Publisher("/gripper/cmd", GripperCmd, queue_size=10)
rospy.init_node("taker", anonymous=True)
if not rospy.is_shutdown():
gripper_cmd = GripperCmd()
gripper_cmd.position = 0.0
gripper_cmd.speed = 0.2
gripper_cmd.force = 2.0
pub.publish(gripper_cmd)
def _execute(self, goal):
print '\n\n\n\n', goal, '\n\n\n\n'
goalPosition = UPPER_LIMIT - goal.command.position / 10.0
# ignore effort for now
# check gripper is ready to accept commands
if not self._stat.is_ready:
print 'Gripper reporting not ready'
self._action_server.set_aborted(
self._generate_result_msg(goalPosition))
return
# verify position is valid
if goalPosition < LOWER_LIMIT or goalPosition > UPPER_LIMIT:
print 'Gripper position out of range'
self._action_server.set_aborted(
self._generate_result_msg(goalPosition))
return
# send gripper command
cmd = GripperCmd()
cmd.position = goalPosition
cmd.speed = 0.1
cmd.force = 100.0
self._cmdPub.publish(cmd)
rospy.sleep(0.05)
# move gripper
preempted = False
while self._stat.is_moving:
if self._action_server.is_preempt_requested():
cmd = GripperCmd()
cmd.stop = True
self._cmdPub.publish(cmd)
preempted = True
break
rospy.sleep(0.05)
self._action_server.publish_feedback(
self._generate_feedback_msg(goalPosition))
# final result
result = self._generate_result_msg(goalPosition)
if preempted:
self._action_server.set_preempted(result)
elif result.reached_goal:
self._action_server.set_succeeded(result)
else:
print goalPosition, '!=', self._stat.position
self._action_server.set_aborted(result)
def _execute(self, goal):
goalPosition = UPPER_LIMIT - goal.command.position / 10.0
# check gripper is ready to accept commands
if not self._stat.is_ready:
rospy.logerr('Gripper reporting not ready')
self._action_server.set_aborted(self._generate_result_msg(goalPosition))
return
# verify position is valid
if goalPosition < LOWER_LIMIT or goalPosition > UPPER_LIMIT:
rospy.logerr('Gripper position out of range')
self._action_server.set_aborted(self._generate_result_msg(goalPosition))
return
# send gripper move command
cmd = GripperCmd()
cmd.position = goalPosition
cmd.speed = self._speed
cmd.force = 100.0
self._cmdPub.publish(cmd)
rospy.sleep(0.5)
# move gripper
preempted = False
while self._stat.is_moving:
if self._action_server.is_preempt_requested():
# send gripper halt command
cmd = GripperCmd()
cmd.stop = True
self._cmdPub.publish(cmd)
preempted = True
break
rospy.sleep(self._loop_delay)
self._action_server.publish_feedback(self._generate_feedback_msg(goalPosition))
# final result
result = self._generate_result_msg(goalPosition)
if preempted:
self._action_server.set_preempted(result)
elif result.reached_goal:
self._action_server.set_succeeded(result)
else:
rospy.logwarn('Gripper did not reach goal position')
self._action_server.set_succeeded(result)
def set_gripper(id, cmd):
gripper_cmd = GripperCmd()
gripper_cmd.force = 50.0
gripper_cmd.speed = 0.03
gripper_cmd.stop = False
gripper_cmd.emergency_release = False
gripper_cmd.emergency_release_dir = 0
if cmd == 'open':
gripper_cmd.position = 0.75
elif cmd == 'close':
gripper_cmd.position = 0.0
if id == 'left':
left_gripper_pub.publish(gripper_cmd)
elif id == "right":
right_gripper_pub.publish(gripper_cmd)
def set_gripper_property(self, pos=0, speed=0, force=0):
control_value = GripperCmd()
control_value.position = pos
control_value.speed = speed
control_value.force = force
return control_value
#! /usr/bin/env python
import rospy
from robotiq_85_msgs.msg import GripperCmd
rospy.init_node('gripper_motion_publisher')
pub = rospy.Publisher('/gripper/cmd', GripperCmd)
rate = rospy.Rate(0.5)
var = GripperCmd()
var.speed = 0.1
var.force = 0.1
while not rospy.is_shutdown():
print "Publishing motion commands for gripper"
var.position = 1.0
pub.publish(var)
rate.sleep()
var.position = 0.06
pub.publish(var)
rate.sleep()
var.position = 0.03
pub.publish(var)
rate.sleep()
var.position = 0.0