class gripper(): # Hardcoded values for the open and the close message. closeGrip = outputMsg.Robotiq2FGripper_robot_output(rACT=1, rGTO=1, rATR=0, rPR=255, rSP=255, rFR=0) openGrip = outputMsg.Robotiq2FGripper_robot_output(rACT=1, rGTO=1, rATR=0, rPR=0, rSP=255, rFR=0) def __init__(self): print "Gripper initialized" # Open gripper: rACT = 1, rGTO = 1, rATR = 0, rPR = 0, rSP = 255, rFR = 25 # Close gripper: rACT = 1, rGTO = 1, rATR = 0, rPR = 255, rSP = 255, rFR = 25 # Publish msg to open to Gripper node. # Return: msg (Robotiq) def open(self): return self.openGrip # Publish msg to close to Gripper node. # Return: msg (Robotiq) def close(self): return self.closeGrip
def __init__(self): # run the robotiq controller process os.system("sudo chmod 777 /dev/ttyUSB1") self.proc = subprocess.Popen(['exec rosrun robotiq_2f_gripper_control Robotiq2FGripperRtuNode.py /dev/ttyUSB1'], stdout=subprocess.PIPE, \ shell=True) time.sleep(2) print("Robotiq Gripper Started") self.gripper_pub = rospy.Publisher('Robotiq2FGripperRobotOutput', outputMsg.Robotiq2FGripper_robot_output, queue_size = 3) # perform a gripper reset print("============ Gripper about to reset Press `Enter` to continue ...") raw_input() self.command = outputMsg.Robotiq2FGripper_robot_output(); self.command.rACT = 0 self.gripper_pub.publish(self.command) time.sleep(1) self.command = outputMsg.Robotiq2FGripper_robot_output(); self.command.rACT = 1 self.command.rGTO = 1 self.command.rSP = 180 self.command.rFR = 25 self.gripper_pub.publish(self.command)
def grijper(action): global pub global command if action == "reset": command = outputMsg.Robotiq2FGripper_robot_output(); command.rACT = 0 pub.publish(command) rospy.sleep(0.1) if action == "activate": # Activate command = outputMsg.Robotiq2FGripper_robot_output(); command.rACT = 1 command.rGTO = 1 command.rSP = 255 command.rFR = 150 pub.publish(command) rospy.sleep(0.1) if action == "open": command.rPR = 0 pub.publish(command) rospy.sleep(1) if action == "close": command.rPR = 80 pub.publish(command) rospy.sleep(1)
def execute(): command = outputMsg.Robotiq2FGripper_robot_output() command.rACT = 1 command.rGTO = 1 command.rSP = 50 command.rFR = 150 pub.publish(command) rospy.sleep(3) print(1) command = outputMsg.Robotiq2FGripper_robot_output() command.rACT = 0 pub.publish(command) rospy.sleep(3) print(0) command = outputMsg.Robotiq2FGripper_robot_output() command.rACT = 1 command.rGTO = 1 command.rSP = 50 command.rFR = 150 pub.publish(command) rospy.sleep(3) print(1) command.rPR = 100 pub.publish(command) rospy.sleep(2) print(2) command.rSP = 250 command.rPR = 255 pub.publish(command) rospy.sleep(1) print(3)
def control_gripper(self, action): '''opens or closes the gripper. action must be an int in the range 0 - 100, where 0 is fully closed and 100 is fully open ''' global MAINLOOPRUNNING, pub try: action = int(action) except Exception: raise TypeError('Must take an int as arg') if action > 100 or action < 0: raise ValueError('input too big or too small') action_to_255 = 255 - int(action * 2.55) if real and ur5: if not MAINLOOPRUNNING: MAINLOOPRUNNING = True activate_gripper_thread() pub = rospy.Publisher('Robotiq2FGripperRobotOutput', outputMsg.Robotiq2FGripper_robot_output, queue_size=20) command = outputMsg.Robotiq2FGripper_robot_output() command.rACT = 1 command.rGTO = 1 command.rSP = 255 command.rFR = 150 pub.publish(command) time.sleep(1) command = outputMsg.Robotiq2FGripper_robot_output() command.rACT = 1 command.rGTO = 1 command.rATR = 0 command.rPR = action_to_255 command.rSP = 255 command.rFR = 25 pub.publish(command) if panda: value = action / 100 * 0.04 print(value) jointstate = sensor_msgs.msg.JointState() jointstate.name += [ 'panda_finger_joint1', 'panda_joint1', 'panda_joint2', 'panda_joint3', 'panda_joint4', 'panda_joint5', 'panda_joint6', 'panda_joint7' ] jointstate.position += [ value ] + self.robot.group.get_current_joint_values() self.finger_pub.publish(jointstate)
def genCommand(char, command): """Update the command according to the character entered by the user.""" if char == 'a': command = outputMsg.Robotiq2FGripper_robot_output(); command.rACT = 1 command.rGTO = 1 command.rSP = 255 command.rFR = 150 if char == 'r': command = outputMsg.Robotiq2FGripper_robot_output(); command.rACT = 0 if char == 'c': command.rPR = 255 if char == 'o': command.rPR = 0 #If the command entered is a int, assign this value to rPRA try: command.rPR = int(char) if command.rPR > 255: command.rPR = 255 if command.rPR < 0: command.rPR = 0 except ValueError: pass if char == 'f': command.rSP += 25 if command.rSP > 255: command.rSP = 255 if char == 'l': command.rSP -= 25 if command.rSP < 0: command.rSP = 0 if char == 'i': command.rFR += 25 if command.rFR > 255: command.rFR = 255 if char == 'd': command.rFR -= 25 if command.rFR < 0: command.rFR = 0 return command
def init_2f_gripper(self): # rospy.init_node('Robotiq2FGripper') self.gripper_pub = rospy.Publisher('Robotiq2FGripperRobotOutput', outputMsg.Robotiq2FGripper_robot_output) self.gripper_command = outputMsg.Robotiq2FGripper_robot_output(); # gripper reset self.gripper_command.rACT = 0 # gripper activate self.gripper_command = outputMsg.Robotiq2FGripper_robot_output(); self.gripper_command.rACT = 1 self.gripper_command.rGTO = 1 self.gripper_command.rSP = 255 self.gripper_command.rFR = 150 rospy.sleep(0.1)
def __init__(self): rospy.init_node('robotiq_gripper_controller', anonymous=True) self.pub = rospy.Publisher('Robotiq2FGripperRobotOutput', outputMsg.Robotiq2FGripper_robot_output, queue_size=10) self.command = outputMsg.Robotiq2FGripper_robot_output() # activate the gripper self.command = outputMsg.Robotiq2FGripper_robot_output() self.command.rACT = 1 self.command.rGTO = 1 self.command.rSP = 255 self.command.rFR = 150 self.pub.publish(self.command) time.sleep(1)
def reset_gripper(): pub = rospy.Publisher('/gripper/output', outputMsg.Robotiq2FGripper_robot_output, queue_size=1000) command = outputMsg.Robotiq2FGripper_robot_output() command.rACT = 0 pub.publish(command) rospy.sleep(0.1) return True
def __init__(self): self.GOAL = 80 # in terms of desired pressure 230, 80 self.TOLERANCE = 10 self.TOLERANCE_QTY = TOLERANCE_QTY self.input_topic = rospy.get_param("~input", "input") self.output_topic = rospy.get_param("~output", "output") # self.input_topic = rospy.get_param("~input", "Robotiq2FGripperRobotInput") # self.output_topic = rospy.get_param("~output", "Robotiq2FGripperRobotOutput") self.state=0 self.current_pos=0 rospy.init_node('pid_helper') self.pub = rospy.Publisher('state', Float64, queue_size=100) self.pub_goal = rospy.Publisher('setpoint', Float64, queue_size=100) self.pub_plant = rospy.Publisher(self.output_topic, outputMsg.Robotiq2FGripper_robot_output, queue_size=100) self.pub_pid_start = rospy.Publisher('pid_enable', Bool, queue_size=100) self.pub_pid_done = rospy.Publisher('pid_done', Empty, queue_size=100) rospy.Subscriber(self.input_topic, inputMsg.Robotiq2FGripper_robot_input, self.getStatus) # command to be sent self.command = outputMsg.Robotiq2FGripper_robot_output(); self.command.rACT = 0 # 1: activate the gripper, 0: reset the gripper -> try to activate the gripper from outside self.command.rGTO = 0 # Go To action: 0 or 1, 1 is action is taken self.command.rATR = 0 # Automatic Realease -> no need for now self.command.rPR = 0 # Desired target self.command.rSP = 0 # Desired speed: keep 0 self.command.rFR = 0 # Desired force: keep 0 self.init_gripper() self.pub_pid_start.publish(Bool(data=0))
def publisher(): """Main loop which requests new commands and publish them on the Robotiq2FGripperRobotOutput topic.""" rospy.init_node('Robotiq2FGripperSimpleController') pub = rospy.Publisher('Robotiq2FGripperRobotOutput', outputMsg.Robotiq2FGripper_robot_output) command = outputMsg.Robotiq2FGripper_robot_output(); while not rospy.is_shutdown(): print('aaa') ''' command = genCommand(askForCommand(command), command) pub.publish(command) rospy.sleep(0.1) ''' raw_input() command = res() pub.publish(command) rospy.sleep(0.1) print('sss') raw_input() command = test() pub.publish(command) rospy.sleep(0.1)
def publisher(): """Main loop which requests new commands and publish them on the Robotiq2FGripperRobotOutput topic.""" rospy.init_node('Robotiq2FGripperSimpleController') pub = rospy.Publisher('Robotiq2FGripperRobotOutput', outputMsg.Robotiq2FGripper_robot_output,queue_size=10) command = outputMsg.Robotiq2FGripper_robot_output() command.rACT = 0 command.rACT = 1 command.rGTO = 1 command.rPR = 130 command.rSP = 255 command.rFR = 150 pub.publish(command) while not rospy.is_shutdown(): while True: data, addr = s1.recvfrom(4096) conv = pickle.loads(data) print(conv) break command = genCommand(conv, command) #askForCommand(command) pub.publish(command) rospy.sleep(0.1)
def gripper_to_pos(position, force, speed, hp): hp_g = True rospy.loginfo("I'm in the function") pub = rospy.Publisher('/gripper/output', outputMsg.Robotiq2FGripper_robot_output, queue_size=1000) command = outputMsg.Robotiq2FGripper_robot_output() hp_g = hp while not hp_g: command.rACT = 1 command.rGTO = 1 command.rSP = speed command.rFR = force command.rPR = position pub.publish(command) rospy.loginfo("I'm in the while + hp: %d", hp) rospy.loginfo("Position: %d", position) rospy.sleep(1.0) hp_g = True
def publisher(): hoi = True rospy.init_node('Robotiq2FGripperSimpleController2') pub = rospy.Publisher('Robotiq2FGripperRobotOutput', outputMsg.Robotiq2FGripper_robot_output, queue_size=10) command = outputMsg.Robotiq2FGripper_robot_output() command.rACT = 1 command.rGTO = 1 command.rSP = 255 command.rFR = 150 pub.publish(command) rospy.sleep(3.0) while not rospy.is_shutdown(): if (hoi == True): command.rPR = 0 pub.publish(command) rospy.sleep(2.0) hoi = False if (hoi == False): command.rPR = 255 pub.publish(command) rospy.sleep(2.0) hoi = True
def __init__(self): ## Initializing instances of subclasses self.r = robot.robot() self.g = gripper.gripper() ## Initializing node and setting up topics rospy.init_node('main', anonymous=True) self.o = optoForce.optoForce(tf, rospy) ## test self.m = mode.mode(self.r, self.g, self.o, self) self.rate = rospy.Rate(125) self.urPublisher = rospy.Publisher('/ur_driver/URScript', String, queue_size=10) self.ledPublisher = rospy.Publisher('/led', LED, queue_size=10) self.gripperPub = rospy.Publisher( '/Robotiq2FGripperRobotOutput', outputMsg.Robotiq2FGripper_robot_output, queue_size=10) self.optoZeroPub = rospy.Publisher('/ethdaq_zero', Bool, queue_size=1) rospy.Subscriber("/joint_states", JointState, self.robotCallback) rospy.Subscriber("/ethdaq_data", WrenchStamped, self.wrenchSensorCallback) rospy.Subscriber("/buttons", Buttons, self.buttonsCallback) rospy.Subscriber("/Robotiq2FGripperRobotInput", inputMsg.Robotiq2FGripper_robot_input, self.gripperCallback) time.sleep(1) self.ledPublisher.publish(led1=True, led2=True, led3=True) ## Activating gripper # Sending first a reset to the gripper so if it have been wrongly activated before it will be a fresh start when our init runs. # Sleep 0.1s and then start the activating sequence. msgReset = outputMsg.Robotiq2FGripper_robot_output() msgReset.rACT = 0 self.gripperPub.publish(msgReset) time.sleep(0.3) msgActivate = outputMsg.Robotiq2FGripper_robot_output() msgActivate.rACT = 1 msgActivate.rGTO = 1 msgActivate.rSP = 255 msgActivate.rFR = 10 self.gripperPub.publish(msgActivate) time.sleep(2) ## Initialization complete, ready for the work in workspace to be done. self.workspace()
def init_params(self): # Initial position self.go_to_init_possition = True self.go_to_init_possition2 = False self.custom_command = Float64MultiArray() self.custom_command.data = [0, 0, 0, 0, 0, 0] self.salad_offset = np.zeros(3) # Target self.target_reached = False self.target_reached_init = False self.attack_reached = False self.target_position_initialized = False self.target_read = False self.trans_target_position_logged = False # End effector transform self.ee_svr_logged = False self.trans_target_logged = False self.trans_world_logged = False # Robot parameters self.max_vel = 0.6 self.robot_gain = 7 # Grasping self.grasped = 0 self.allegroMsg = String() self.trans_target_orig = np.array( [0.1, -0.62, 0.248]) #0.236 for one plate only, 0.245 for 5 plates self.trans_target_test = self.trans_target_orig self.trans_target_position = self.trans_target_orig # self.drop_position = np.array([0.50,-0.2,0.25]) # Without burger self.drop_position = np.array([0.50, -0.2, 0.27]) # With burger self.pick_up_1 = False self.pick_up_2 = False self.pick_up_3 = False self.pick_up_4 = False self.pick_up_5 = False self.counter0 = 0 self.counter1 = 0 self.counter2 = 0 self.gripper_closed = 219 # ORiginal # self.gripper_closed=200 # Bun top self.gripper_open = 150 # Original # self.gripper_open=120 # Bun top self.gripper_open2 = 140 self.gripperMsg = outputMsg.Robotiq2FGripper_robot_output() self.gripperMsg.rACT = 1 self.gripperMsg.rGTO = 1 self.gripperMsg.rATR = 0 self.gripperMsg.rPR = self.gripper_open self.gripperMsg.rSP = 255 self.gripperMsg.rFR = 150 # Cheese communication self.cheesemsg = Int32() self.start_motion = False self.burger_offset = 0
def test(): print('bbb') command = outputMsg.Robotiq2FGripper_robot_output() command.rACT = 1 command.rGTO = 1 command.rSP = 255 command.rFR = 150 return command
def gripper_move_command(degree): command = outputMsg.Robotiq2FGripper_robot_output() command.rACT = 1 command.rGTO = 1 command.rPR = degree command.rSP = 0 command.rFR = 150 # gripper_set_vel_pub.publish(command) return command
def __init__(self): rospy.init_node("movement", anonymous=True, disable_signals=False) # two ways of controlling the arm with coordinates / joint behaviors self.coordinates_pub_pollux = rospy.Publisher( "/coordinates_cmd_pollux", String, queue_size=10) self.joints_pub_pollux = rospy.Publisher("/behaviors_cmd_pollux", String, queue_size=10) # controller_status determines when Perception start looking for a new goal self.status_pub = rospy.Publisher("/controller_status", Bool, queue_size=10) # setting the origin (left top) of the chess board x_init = 0.196 y_init = -0.694 z_init = 0.350 global origin origin = [x_init, y_init, z_init] # setting the units for the chess board, according to the right bottom global x_unit x_unit = (0.196 + 0.234) / 7 global y_unit y_unit = (0.694 - 0.247) / 7 #Gripper Code #rospy.init_node('robotiq_gripper_controller') self.pub = rospy.Publisher('Robotiq2FGripperRobotOutput', outputMsg.Robotiq2FGripper_robot_output) self.command = outputMsg.Robotiq2FGripper_robot_output() # activate the gripper self.command = outputMsg.Robotiq2FGripper_robot_output() self.command.rACT = 1 self.command.rGTO = 1 self.command.rSP = 255 self.command.rFR = 150 self.pub.publish(self.command) time.sleep(1)
def activate_gripper(): pub = rospy.Publisher('/gripper/output', outputMsg.Robotiq2FGripper_robot_output, queue_size=1000) command = outputMsg.Robotiq2FGripper_robot_output() command.rACT = 1 command.rGTO = 1 command.rSP = 255 command.rFR = 150 pub.publish(command) rospy.sleep(0.1) return True
def publisher(): """Publishes once to output topic to reset after power loss""" rospy.init_node('robotiq2FGripperReset') pub = rospy.Publisher('Robotiq2FGripperRobotOutput', outputMsg.Robotiq2FGripper_robot_output) rospy.sleep(1.0) command = outputMsg.Robotiq2FGripper_robot_output(); command.rACT = 0 pub.publish(command) rospy.sleep(1.0) command = outputMsg.Robotiq2FGripper_robot_output(); command.rACT = 1 command.rGTO = 1 command.rSP = 255 command.rFR = 150 pub.publish(command)
def run(self): while not rospy.is_shutdown(): try: self.close() time.sleep(5) except KeyboardInterrupt: self.command = outputMsg.Robotiq2FGripper_robot_output() self.command.rACT = 0 self.pub.publish(self.command) time.sleep(1) break
def publisher(): """Main loop which requests new commands and publish them on the Robotiq2FGripperRobotOutput topic.""" rospy.init_node('Robotiq2FGripperSimpleController') pub = rospy.Publisher('Robotiq2FGripperRobotOutput', outputMsg.Robotiq2FGripper_robot_output, queue_size=10) command = outputMsg.Robotiq2FGripper_robot_output(); while not rospy.is_shutdown(): command = genCommand(askForCommand(command), command) pub.publish(command) rospy.sleep(0.1)
def run(self): while not rospy.is_shutdown(): try: # perform gripper functions multiple times self.do_stuff() time.sleep(5) except KeyboardInterrupt: # reset the gripper self.command = outputMsg.Robotiq2FGripper_robot_output() self.command.rACT = 0 self.pub.publish(self.command) time.sleep(1) break
def init_params(self): # Initial position self.go_to_init_possition = True self.go_to_init_possition2 = False self.custom_command = Float64MultiArray() self.custom_command.data = [0, 0, 0, 0, 0, 0] self.salad_offset = np.zeros(3) # Target self.target_reached = False self.target_reached_init = False self.attack_reached = False self.target_position_initialized = False self.target_read = False # End effector transform self.ee_svr_logged = False self.trans_target_logged = False self.trans_world_logged = False # Robot parameters self.max_vel = 0.4 self.robot_gain = 6 # Grasping self.grasped = 0 self.allegroMsg = String() self.drop_position = np.array([0.70, 0.1, 0.25]) self.pick_up_1 = False self.pick_up_2 = False self.pick_up_3 = False self.pick_up_4 = False self.pick_up_5 = False self.counter0 = 0 self.counter1 = 0 self.counter2 = 0 self.gripper_closed = 217 self.gripper_open = 150 self.gripperMsg = outputMsg.Robotiq2FGripper_robot_output() self.gripperMsg.rACT = 1 self.gripperMsg.rGTO = 1 self.gripperMsg.rATR = 0 self.gripperMsg.rPR = self.gripper_open self.gripperMsg.rSP = 255 self.gripperMsg.rFR = 150
def ungrasp(self): """Main loop which requests new commands and publish them on the Robotiq2FGripperRobotOutput topic.""" #rospy.init_node('Robotiq2FGripperSimpleController') pub = rospy.Publisher('Robotiq2FGripperRobotOutput', outputMsg.Robotiq2FGripper_robot_output) command = outputMsg.Robotiq2FGripper_robot_output(); timeout = 2.5 timeout_start = time.time() while time.time() < timeout_start + timeout: #rospy.sleep(0.5) #command = genCommand(askForCommand(command), command) command = self.genCommand('r', command) pub.publish(command) rospy.sleep(1) command = self.genCommand('a', command) pub.publish(command) rospy.sleep(1)
def __init__(self): # define ros node rospy.init_node('gripper_controller', anonymous=True) # define ros publisher for sending command to the gripper interfaces self.grip_pub = rospy.Publisher( '/gripper/Robotiq2FGripperRobotOutput', outputMsg.Robotiq2FGripper_robot_output, queue_size=1) # define ros publisher for sending joint states self.jointState_pub = rospy.Publisher('/gripper/joint_states', JointState, queue_size=1) # define a subscriber for listening to the gripper position self.gripper_listener = rospy.Subscriber( "/gripper/Robotiq2FGripperRobotInput", inputMsg.Robotiq2FGripper_robot_input, self.gripperListener) # define ros subscriber for the robot pose self.grip_sub = rospy.Subscriber('/gripper/command', Int8, self.command_receiver) # define a variable to hold the position of the gripper self.gripper_position = 0.0 # define a JointState message for the gripper joint states self.js_msg = JointState() self.js_msg.name = [ "finger_joint", "left_inner_knuckle_joint", "left_inner_finger_joint", "right_outer_knuckle_joint", "right_inner_knuckle_joint", "right_inner_finger_joint" ] # self.js_msg.position.resize(6) self.command = outputMsg.Robotiq2FGripper_robot_output() self.current_command = 0 self.gripper_initialized = False
def reset_bool(self): self.trans_target = np.array([0.65, -0.1, 0.175]) self.attack_reached = False self.target_reached = False self.allegroMsg = String() self.pick_up_1 = False self.pick_up_2 = False self.pick_up_3 = False self.pick_up_4 = False self.pick_up_5 = False self.counter0 = 0 self.counter1 = 0 self.counter2 = 0 self.gripper_closed = 217 self.gripper_open = 190 self.gripperMsg = outputMsg.Robotiq2FGripper_robot_output() self.gripperMsg.rACT = 1 self.gripperMsg.rGTO = 1 self.gripperMsg.rATR = 0 self.gripperMsg.rPR = self.gripper_open self.gripperMsg.rSP = 255 self.gripperMsg.rFR = 150
def publishCommand(self): """ function for publishing the command """ if not self.gripper_initialized: self.command = outputMsg.Robotiq2FGripper_robot_output() self.command.rACT = 1 self.command.rGTO = 1 self.command.rSP = 255 self.command.rFR = 150 self.gripper_initialized = True else: if self.current_command == 0: # reinitialize gripper self.gripper_initialized = True if self.current_command == 1: # close the gripper self.command.rPR = 255 if self.current_command == 2: # open the gripper self.command.rPR = 0 self.grip_pub.publish(self.command)
def reset_bool(self): self.trans_target = self.trans_target_orig self.attack_reached = False self.target_reached = False self.target_position_initialized = False self.allegroMsg = String() self.pick_up_1 = False self.pick_up_2 = False self.pick_up_3 = False self.pick_up_4 = False self.pick_up_5 = False self.counter0 = 0 self.counter1 = 0 self.counter2 = 0 self.gripper_closed = 219 self.gripper_open = 150 self.gripperMsg = outputMsg.Robotiq2FGripper_robot_output() self.gripperMsg.rACT = 1 self.gripperMsg.rGTO = 1 self.gripperMsg.rATR = 0 self.gripperMsg.rPR = self.gripper_open self.gripperMsg.rSP = 255 self.gripperMsg.rFR = 150 self.desired_orientation_cheese = [0.2081947, 2.0178752, -0.4424049]