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 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):
super(UR5, self).__init__()
# Instantiate a `RobotCommander`_ object. Provides information such as the robot's
# kinematic model and the robot's current joint states
robot = moveit_commander.RobotCommander()
# Instantiate a `MoveGroupCommander`_ object. This object is an interface
# to a planning group (group of joints). In this tutorial the group is the primary
# arm joints in the Panda robot, so we set the group's name to "panda_arm".
# If you are using a different robot, change this value to the name of your robot
# arm planning group.
# This interface can be used to plan and execute motions:
arm_name = "manipulator"
arm = moveit_commander.MoveGroupCommander(arm_name)
gripper_name = "gripper"
gripper = moveit_commander.MoveGroupCommander(gripper_name)
# Create a `DisplayTrajectory`_ ROS publisher which is used to display
# trajectories in Rviz:
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
# Settings about the gripper
rospy.Subscriber("/gripper/stat", GripperStat, self._update_gripper_stat, queue_size=10)
self._gripper_pub = rospy.Publisher('/gripper/cmd', GripperCmd, queue_size=10)
self._gripper_stat = GripperStat()
self._gripper_cmd = GripperCmd()
# We can also print the name of the end-effector link for this group:
eef_link = arm.get_end_effector_link()
print("============ End effector link for arm: %s" % eef_link)
eef_link = gripper.get_end_effector_link()
print("============ End effector link for gripper: %s" % eef_link)
# We can get a list of all the groups in the robot:
group_names = robot.get_group_names()
print("============ Available Planning Groups:", robot.get_group_names())
# Sometimes for debugging it is useful to print the entire state of the
# robot:
print("============ Printing robot state")
print(robot.get_current_state())
print("")
self.robot = robot
self.arm = arm
self.gripper = gripper
self.display_trajectory_publisher = display_trajectory_publisher
self.group_names = group_names
self.init_pose = self.arm.get_current_pose().pose
self.waiting = True
self.object_poses = {}
self.tfListener = tf.TransformListener()
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 _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 __init__(self):
self._num_grippers = rospy.get_param('~num_grippers', 1)
self.open = rospy.get_param('~open', True)
self.close = rospy.get_param('~close', False)
self._prefix = rospy.get_param('~prefix', None)
if (self._num_grippers == 1 and self._prefix is None):
rospy.Subscriber("/gripper/stat",
GripperStat,
self._update_gripper_stat,
queue_size=10)
self._gripper_pub = rospy.Publisher('/gripper/cmd',
GripperCmd,
queue_size=10)
elif (self._num_grippers == 1 and self._prefix is not None):
rospy.logwarn('gripper prefix = {}'.format(self._prefix))
rospy.Subscriber("/" + self._prefix + "gripper/stat",
GripperStat,
self._update_gripper_stat,
queue_size=10)
self._gripper_pub = rospy.Publisher('/' + self._prefix +
'gripper/cmd',
GripperCmd,
queue_size=10)
elif (self._num_grippers == 2):
rospy.Subscriber("/left_gripper/stat",
GripperStat,
self._update_gripper_stat,
queue_size=10)
self._left_gripper_pub = rospy.Publisher('/left_gripper/stat',
GripperCmd,
queue_size=10)
rospy.Subscriber("/right_gripper/stat",
GripperStat,
self._update_right_gripper_stat,
queue_size=10)
self._right_gripper_pub = rospy.Publisher('/right_gripper/cmd',
GripperCmd,
queue_size=10)
else:
rospy.logerr(
"Number of grippers not supported (needs to be 1 or 2)")
return
self._gripper_stat = [GripperStat()] * self._num_grippers
self._gripper_cmd = [GripperCmd()] * self._num_grippers
self._run_test()
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 __init__(self):
self._num_grippers = rospy.get_param('~num_grippers',1)
if (self._num_grippers == 1):
rospy.Subscriber("/gripper/stat", GripperStat, self._update_gripper_stat, queue_size=10)
self._gripper_pub = rospy.Publisher('/gripper/cmd', GripperCmd, queue_size=10)
elif (self._num_grippers == 2):
rospy.Subscriber("/left_gripper/stat", GripperStat, self._update_gripper_stat, queue_size=10)
self._left_gripper_pub = rospy.Publisher('/left_gripper/stat', GripperCmd, queue_size=10)
rospy.Subscriber("/right_gripper/stat", GripperStat, self._update_right_gripper_stat, queue_size=10)
self._right_gripper_pub = rospy.Publisher('/right_gripper/cmd', GripperCmd, queue_size=10)
else:
rospy.logerr("Number of grippers not supported (needs to be 1 or 2)")
return
self._gripper_stat = [GripperStat()] * self._num_grippers
self._gripper_cmd = [GripperCmd()] * self._num_grippers
self._run_test()
def __init__(self):
self.num_of_grippers = rospy.get_param("~num_grippers", 1)
# make sure 1 gripper is detected
if self.num_of_grippers != 1:
LOG.ERROR("Incorrect number of grippers detected: {}".format(
self.num_of_grippers))
return
else:
LOG.INFO("Correct number of gripper detected.")
rospy.Subscriber("/gripper/stat",
GripperStat,
self.update_gripper_stat,
queue_size=10)
self.gripper_publisher = rospy.Publisher('/gripper/cmd',
GripperCmd,
queue_size=10)
self.gripper_stat = GripperStat()
self.gripper_cmd = GripperCmd()
# set the gripper force and speed to default value
self.set_gripper_force(100.0)
self.set_gripper_speed(0.02)
self.r = rospy.Rate(1)
gripper_cmd = [float(i) for i in row[1:]]
data_table.append([stamp_tmp, gripper_cmd])
times_np_gripper = [item[0] for item in data_table]
gripper_cmd = [item[1] for item in data_table]
return times_np_gripper, gripper_cmd
if __name__ == "__main__":
# set socket to send command to robot
a = rospy.init_node("play_back_test")
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
sock.connect(("192.168.1.108", 30002))
gripper_publisher = rospy.Publisher('/gripper/cmd',
GripperCmd,
queue_size=10)
gripper_cmd_val = GripperCmd()
file_name_basis = get_prefix()
# load ur5 data from csv
times_np_ur, joint_states = load_ur5_motion(prefix=file_name_basis)
times_np_gripper, gripper_cmd = load_gripper_motion(prefix=file_name_basis)
# load gripper data from csv
init_state = joint_states[0]
# move to the initial joint state
moveJ_ur5(init_state, sock=sock)
rospy.sleep(12)
# convert time from nsec to sec, start from 0
csv_times_ur = process_time_table(times_np_ur)
csv_times_gripper = process_time_table(times_np_gripper)
# stupid things here, should be revised
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
def __init__(self,
solutionFile,
arm=UR5(),
ip='192.168.1.106',
urip='192.168.1.108',
sampleRate=1000,
move_robot=False,
dataOffline=False):
# NETWORKING OPTIONS#################################################################
self.ip = ip
self.dataOffline = dataOffline
self.UDP_PORT = 11000
if not dataOffline:
self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
self.sock.bind((self.ip, self.UDP_PORT))
self.sock.setsockopt(socket.SOL_SOCKET, socket.SO_RCVBUF, 512)
self.urip = urip
if move_robot:
self.urip = urip
self.UR_PORT = 30002
print self.urip
self.urscript_sock = socket.socket(socket.AF_INET,
socket.SOCK_STREAM)
self.urscript_sock.connect((self.urip, self.UR_PORT))
self.ur5_dataReader = UR5_dataReader()
#####################################################################################
# ARM OPTIONS########################################################################
self.arm = arm
self.numDOFs = len(self.arm.axes)
if self.arm.__name__ == 'Robot:UR5':
self.initState = [
M.pi, -0.38, -1.2, -M.pi / 2, -M.pi / 2, -M.pi / 2
] # VREP, alternate start with wrist rotated down
self.rosDisp = [-M.pi, -M.pi / 2, 0, -M.pi / 2, 0, M.pi / 2]
self.opBounds = [(-10, 10), (-1.5, 1.5), (-2.5, 2.5), (-2.3, 2.3),
(-1.8, 1.8), (-10, 10)]
elif self.arm.__name__ == 'Robot:IIWA7':
self.initState = [-0.6, 1, 1.2, -.8, 0, 0, .7]
self.opBounds = [
tuple((-2 * M.pi, 2 * M.pi)) for i in range(0, self.numDOFs)
]
else:
raise Exception(
'ERROR: Trying to initialize playback with invalid robot.')
#####################################################################################
# SOLVER VARIABLES ##################################################################
self.xopt = self.initState # current Solution
self.xoptPub = []
self.allConfigs = [
] # list of all configurations calculated by the solver
self.allConfigsNP = []
for i in range(10):
self.allConfigsNP.append(
N.array(self.initState)
) # jumpstart list by having 10 initial configs for finite differencing
self.allEEPos = []
self.timer = Timer()
self.prevS = self.initState # previous configuration state
self.prevV = self.numDOFs * [0] # previous velocity
self.prevA = self.numDOFs * [0] # previous acceleration
self.ee_origPos = self.arm.getFrames(
self.initState)[0][-1] # original end effector point
for i in range(10):
self.allEEPos.append(
self.ee_origPos
) # jumpstart list by having 10 initial configs for finite differencing
self.prev_eePt = self.ee_origPos # previous end effector point for cartesian space smoothing
self.ee_origQuat = T.quaternion_from_matrix(
self.arm.getFrames(
self.initState)[1][-1]) # original orientation quaternion
self.prevHandPos = N.array(
[0, 0, 0]) # previous hand position for calculating hand velocity
self.maxVel = 0.0 # maximum velocity detected
self.eeGoalPos = self.ee_origPos
self.eeGoalOr = self.ee_origQuat
self.handState = 0
self.handPos = N.array([0, 0, 0])
self.handVelNorm = 0.0
self.handVelMax = 0.04
self.maxRotCon = 0.2 # maximum rotation allowed as constraint (at each joint)
self.prevGoalOr = self.ee_origQuat
self.move_robot = move_robot
self.sampleRate = sampleRate
self.encoderValue = 6.8
self.initialIteration = True
#####################################################################################
# OPTIMIZATION WEIGHTS ##############################################################
self.velW = 3.0 # previous state smoothness weight
self.accW = 10.0 # accleration smoothing weight
self.jrkW = 1.0 # jerk smoothing weight
self.eepW = 15.0 # end effector positional weight
self.uoW = 4.0 # underconstrained orientation weight
self.eeoW = 8.0 # end effector orientation weight
self.elbW = 0.0 # elbow goal weight
self.jtPtW = 1.0 # end effector joint point velocity weight
#####################################################################################
# FILES #############################################################################
self.solutionFile = solutionFile
#####################################################################################
# TEST INFORMATION ##################################################################
self.posDeviation = 0.0 # meters
self.rotDeviation = 0.0 # radians
self.numSolutions = 0.0 # number of total solutions solved for
# OBJECTS ###########################################################################
self.TongsTransform = [] # tongs transform object
self.Parser = []
self.Utils = []
# GRIPPER ############################################################################
self.gripper_pub = rospy.Publisher('/gripper/cmd',
GripperCmd,
queue_size=10)
self.gripper_cmd = GripperCmd()
#! /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