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): 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 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 _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 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 __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 __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 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 __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