Exemple #1
0
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)
Exemple #3
0
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)
Exemple #5
0
    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)
Exemple #8
0
    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
Exemple #10
0
    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))
Exemple #11
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)
Exemple #12
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,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
Exemple #14
0
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
Exemple #15
0
    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()
Exemple #16
0
    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
Exemple #17
0
def test():
    print('bbb')
    command = outputMsg.Robotiq2FGripper_robot_output()
    command.rACT = 1
    command.rGTO = 1
    command.rSP = 255
    command.rFR = 150
    return command
Exemple #18
0
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
Exemple #19
0
    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)
Exemple #22
0
    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
Exemple #23
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, 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)
Exemple #24
0
    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
Exemple #25
0
    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
Exemple #26
0
	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
Exemple #28
0
 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)
Exemple #30
0
 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]