Ejemplo n.º 1
0
def main():
    """
    Main Script
    """

    # Make sure that you've looked at and understand path_planner.py before starting

    ## TF CODE
    tfBuffer = tf2_ros.Buffer()
    tfListener = tf2_ros.TransformListener(tfBuffer)

    ## TF CODE

    planner = PathPlanner("right_arm")

    Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3])  # Stolen from 106B Students
    Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5
                          ])  # Stolen from 106B Students
    Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1])  # Untuned
    Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9])  # Untuned

    limb = intera_interface.Limb("right")
    control = Controller(Kp, Kd, Ki, Kw, limb)

    ##
    ## Add the obstacle to the planning scene here
    ##
    X = 0.40
    Y = 1.20
    Z = 0.10

    Xp = 0.5
    Yp = 0.00
    Zp = -0.15
    Xpa = 0.00
    Ypa = 0.00
    Zpa = 0.00
    Wpa = 1.00

    pose = PoseStamped()
    pose.header.stamp = rospy.Time.now()
    pose.header.frame_id = "base"
    pose.pose.position.x = Xp
    pose.pose.position.y = Yp
    pose.pose.position.z = Zp

    pose.pose.orientation.x = Xpa
    pose.pose.orientation.y = Ypa
    pose.pose.orientation.z = Zpa
    pose.pose.orientation.w = Wpa

    #planner.add_box_obstacle([X,Y,Z], "wall", pose)

    #Wall 2

    X = 0.40
    Y = 0.10
    Z = 0.30

    Xp = 0.5
    Yp = -0.15
    Zp = 0.05
    Xpa = 0.00
    Ypa = 0.00
    Zpa = 0.00
    Wpa = 1.00

    pose = PoseStamped()
    pose.header.stamp = rospy.Time.now()
    pose.header.frame_id = "base"
    pose.pose.position.x = Xp
    pose.pose.position.y = Yp
    pose.pose.position.z = Zp

    pose.pose.orientation.x = Xpa
    pose.pose.orientation.y = Ypa
    pose.pose.orientation.z = Zpa
    pose.pose.orientation.w = Wpa

    #planner.add_box_obstacle([X,Y,Z], "wall2", pose)

    # #Create a path constraint for the arm
    # #UNCOMMENT FOR THE ORIENTATION CONSTRAINTS PART
    orien_const = OrientationConstraint()
    orien_const.link_name = "right_gripper"
    orien_const.header.frame_id = "base"
    #orien_const.orientation.y = -1.0;
    #orien_const.absolute_x_axis_tolerance = 0.1;
    orien_const.absolute_y_axis_tolerance = 0.05
    orien_const.absolute_z_axis_tolerance = 0.05
    orien_const.weight = 1.0

    waypoints = list()

    # Section to add waypoints
    # step_size = size between x position changes
    # final_length = final x position

    #GOAL Points
    # goal_1.pose.position.x = 0.585
    # goal_1.pose.position.y = 0.156
    # goal_1.pose.position.z = -0.138
    # goal_1.pose.orientation.x =  0
    # goal_1.pose.orientation.y =  0.707
    # goal_1.pose.orientation.z =  0
    # goal_1.pose.orientation.w =  0.707
    #GOAL Points

    while not rospy.is_shutdown():

        ## ZERO THE ROBOT ##
        goal_1 = PoseStamped()
        goal_1.header.frame_id = "base"
        goal_1.pose.position.x = 0.585
        goal_1.pose.position.y = 0.156
        goal_1.pose.position.z = -0.138
        goal_1.pose.orientation.x = 0  # -0.026
        goal_1.pose.orientation.y = 0.707  #  0.723
        goal_1.pose.orientation.z = 0  # -0.052
        goal_1.pose.orientation.w = 0.707  #  0.689
        waypoints.append(goal_1.pose)

        while not rospy.is_shutdown():
            try:

                plan = planner.plan_to_pose(waypoints, list())

                if not control.execute_path(plan):
                    raise Exception("Execution failed")
            except Exception as e:
                print e
            else:
                break

        waypoints = list()

        ## ZERO THE ROBOT ##
        print("ROBOT ZEROED")

        #control._velocity_scalar = 0.5

        step_size = 0.0005  #0.0025

        FINAL_X = 0.785  #

        offset = 0.14

        tf_px = 0.585
        tf_py = 0.156
        tf_pz = -0.138

        tf_rx = 0
        tf_ry = 0.707
        tf_rz = 0
        tf_rw = 0.707

        goal_x = 0.585
        goal_y = 0.156
        goal_z = -0.138

        try:
            trans = tfBuffer.lookup_transform('base', 'right_gripper_base',
                                              rospy.Time())
            tf_px = trans.transform.translation.x
            tf_py = trans.transform.translation.y
            tf_pz = trans.transform.translation.z
            tf_rx = trans.transform.rotation.x
            tf_ry = trans.transform.rotation.y
            tf_rz = trans.transform.rotation.z
            tf_rw = trans.transform.rotation.w

            goal_x = tf_px
            goal_y = tf_py
            goal_z = tf_pz

        except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
                tf2_ros.ExtrapolationException):
            continue

        offset = 0.14
        FINAL_X = goal_x + offset + 0.2

        x = tf_px + offset
        print("TF'd zero")

        update = 0
        execute = 0

        i = 0
        STOP = 0

        while ((x < FINAL_X or update == 0) and (i < 5)):

            if (update == 0):
                try:
                    trans = tfBuffer.lookup_transform('base',
                                                      'right_gripper_base',
                                                      rospy.Time())
                    tf_px = trans.transform.translation.x
                    tf_py = trans.transform.translation.y
                    tf_pz = trans.transform.translation.z
                    tf_rx = trans.transform.rotation.x
                    tf_ry = trans.transform.rotation.y
                    tf_rz = trans.transform.rotation.z
                    tf_rw = trans.transform.rotation.w
                    x = tf_px + offset

                except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
                        tf2_ros.ExtrapolationException):
                    continue
                update = 1
                execute = 0

            y = tf_py
            z = tf_pz

            if (y < goal_y - 0.05):
                y = y + step_size

            if (y > goal_y + 0.05):
                y = y - step_size

            if (z < goal_z - 0.05):
                z = z + step_size

            if (z > goal_z + 0.05):
                z = z - step_size

            #print("TF'd while")
            goal_1 = PoseStamped()
            goal_1.header.frame_id = "base"
            goal_1.pose.position.x = x
            goal_1.pose.position.y = y
            goal_1.pose.position.z = z

            #Orientation as a quaternion
            goal_1.pose.orientation.x = 0  # -0.026
            goal_1.pose.orientation.y = 0.707  #  0.723
            goal_1.pose.orientation.z = 0  # -0.052
            goal_1.pose.orientation.w = 0.707  #  0.689

            waypoints.append(goal_1.pose)

            x = x + step_size

            if (x >= FINAL_X):
                execute = 1
            #print("added waypoint")

            if (execute == 1):
                while not rospy.is_shutdown():
                    try:

                        plan = planner.plan_to_pose(waypoints, list())

                        if not control.execute_path(plan):
                            raise Exception("Execution failed")
                    except Exception as e:
                        print e
                    else:
                        break

                #STOP = control._flag
                waypoints = list()

                update = 0
                print("EXEcute")
                print(i)
                i = i + 1
        print("END OF LOOP")

        print("ZERO AGAIN")
        control._zero = 1
        waypoints = list()

        ## ZERO THE ROBOT ##
        control._flag = 0
        control._trigger = 0
        control._velocity_scalar = 1

        goal_1 = PoseStamped()
        goal_1.header.frame_id = "base"
        goal_1.pose.position.x = 0.585
        goal_1.pose.position.y = 0.156
        goal_1.pose.position.z = -0.138
        goal_1.pose.orientation.x = 0  # -0.026
        goal_1.pose.orientation.y = 0.707  #  0.723
        goal_1.pose.orientation.z = 0  # -0.052
        goal_1.pose.orientation.w = 0.707  #  0.689
        waypoints.append(goal_1.pose)

        while not rospy.is_shutdown():
            try:

                plan = planner.plan_to_pose(waypoints, list())

                if not control.execute_path(plan):
                    raise Exception("Execution failed")
            except Exception as e:
                print e
            else:
                break

        waypoints = list()
        control._zero = 0
        ## ZERO THE ROBOT ##
        print("ROBOT ZEROED...AGAIN")

        #plt.plot(control._sensor_data)
        s = list(map(float, control._sensor_data))
        scales = list(map(float, control._scalar_data))
        goal_line = list(map(float, control._goal_list))
        #s = list(map(float, control._debug))

        #control._sensor_data = sorted(control._sensor_data)
        ##print(min(s))
        ##print(max(s))
        plt.subplot(211)
        plt.plot(s)
        plt.plot(goal_line)

        plt.axis([0, len(control._sensor_data), min(s), max(s)])
        plt.ylabel('Sensor Data')
        plt.subplot(212)
        plt.plot(scales)
        plt.show()
        control._sensor_data = list()
        control._scalar_data = list()
        control._goal_list = list()
        waypoints = list()
        control._velocity_scalar = 1
Ejemplo n.º 2
0
def main():
    """
    Main Script
    """

    # Make sure that you've looked at and understand path_planner.py before starting

    ## TF CODE
    tfBuffer = tf2_ros.Buffer()
    tfListener = tf2_ros.TransformListener(tfBuffer)

    ## TF CODE

    planner = PathPlanner("right_arm")

    Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3])  # Stolen from 106B Students
    Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5
                          ])  # Stolen from 106B Students
    Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1])  # Untuned
    Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9])  # Untuned

    limb = intera_interface.Limb("right")
    control = Controller(Kp, Kd, Ki, Kw, limb)

    ##
    ## Add the obstacle to the planning scene here
    ##
    X = 0.40
    Y = 1.20
    Z = 0.10

    Xp = 0.5
    Yp = 0.00
    Zp = -0.15
    Xpa = 0.00
    Ypa = 0.00
    Zpa = 0.00
    Wpa = 1.00

    pose = PoseStamped()
    pose.header.stamp = rospy.Time.now()
    pose.header.frame_id = "base"
    pose.pose.position.x = Xp
    pose.pose.position.y = Yp
    pose.pose.position.z = Zp

    pose.pose.orientation.x = Xpa
    pose.pose.orientation.y = Ypa
    pose.pose.orientation.z = Zpa
    pose.pose.orientation.w = Wpa

    #planner.add_box_obstacle([X,Y,Z], "wall", pose)

    #Wall 2

    X = 0.40
    Y = 0.10
    Z = 0.30

    Xp = 0.5
    Yp = -0.15
    Zp = 0.05
    Xpa = 0.00
    Ypa = 0.00
    Zpa = 0.00
    Wpa = 1.00

    pose = PoseStamped()
    pose.header.stamp = rospy.Time.now()
    pose.header.frame_id = "base"
    pose.pose.position.x = Xp
    pose.pose.position.y = Yp
    pose.pose.position.z = Zp

    pose.pose.orientation.x = Xpa
    pose.pose.orientation.y = Ypa
    pose.pose.orientation.z = Zpa
    pose.pose.orientation.w = Wpa

    #planner.add_box_obstacle([X,Y,Z], "wall2", pose)

    # #Create a path constraint for the arm
    # #UNCOMMENT FOR THE ORIENTATION CONSTRAINTS PART
    orien_const = OrientationConstraint()
    orien_const.link_name = "right_gripper"
    orien_const.header.frame_id = "base"
    #orien_const.orientation.y = -1.0;
    #orien_const.absolute_x_axis_tolerance = 0.1;
    orien_const.absolute_y_axis_tolerance = 0.05
    orien_const.absolute_z_axis_tolerance = 0.05
    orien_const.weight = 1.0

    waypoints = list()

    # Section to add waypoints
    # step_size = size between x position changes
    # final_length = final x position

    #GOAL Points
    # goal_1.pose.position.x = 0.585
    # goal_1.pose.position.y = 0.156
    # goal_1.pose.position.z = -0.138
    # goal_1.pose.orientation.x =  0
    # goal_1.pose.orientation.y =  0.707
    # goal_1.pose.orientation.z =  0
    # goal_1.pose.orientation.w =  0.707
    #GOAL Points

    def moveUp(planner, control, waypoints, value_of_step):

        control._probing = 1
        #STARTOF WORKING CODE
        # Z MUST MOVE UP TWO BLOCK
        # Z = 0.025
        #control._velocity_scalar = 0.5
        jenga = 0
        while (jenga != 1):
            step_size = 0.0005  #0.0025
            FINAL_X = 0.785  #
            offset = 0.14
            tf_px = 0.585
            tf_py = 0.156
            tf_pz = -0.138
            tf_rx = 0
            tf_ry = 0.707
            tf_rz = 0
            tf_rw = 0.707
            goal_x = 0.585
            goal_y = 0.156
            goal_z = -0.138
            try:
                trans = tfBuffer.lookup_transform('base', 'right_gripper_base',
                                                  rospy.Time())
                tf_px = trans.transform.translation.x
                tf_py = trans.transform.translation.y
                tf_pz = trans.transform.translation.z
                tf_rx = trans.transform.rotation.x
                tf_ry = trans.transform.rotation.y
                tf_rz = trans.transform.rotation.z
                tf_rw = trans.transform.rotation.w
                goal_x = tf_px
                goal_y = tf_py
                goal_z = tf_pz
            except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
                    tf2_ros.ExtrapolationException):
                continue
            offset = 0.14
            FINAL_Z = goal_z + value_of_step  # + offsett
            z = tf_pz  #+ offset
            #print("TF'd zero")
            update = 0
            execute = 0
            i = 0
            STOP = 0
            while ((z < FINAL_Z or update == 0) and (i < 3)):
                if (update == 0):
                    try:
                        trans = tfBuffer.lookup_transform(
                            'base', 'right_gripper_base', rospy.Time())
                        tf_px = trans.transform.translation.x
                        tf_py = trans.transform.translation.y
                        tf_pz = trans.transform.translation.z
                        tf_rx = trans.transform.rotation.x
                        tf_ry = trans.transform.rotation.y
                        tf_rz = trans.transform.rotation.z
                        tf_rw = trans.transform.rotation.w
                        z = tf_pz  # + offset
                    except (tf2_ros.LookupException,
                            tf2_ros.ConnectivityException,
                            tf2_ros.ExtrapolationException):
                        continue
                    update = 1
                    execute = 0
                y = tf_py
                x = tf_px + offset
                if (y < goal_y - 0.05):
                    y = y + step_size
                if (y > goal_y + 0.05):
                    y = y - step_size
                #print("TF'd while")
                goal_1 = PoseStamped()
                goal_1.header.frame_id = "base"
                goal_1.pose.position.x = x
                goal_1.pose.position.y = y
                goal_1.pose.position.z = z
                #Orientation as a quaternion
                goal_1.pose.orientation.x = 0  # -0.026
                goal_1.pose.orientation.y = 0.707  #  0.723
                goal_1.pose.orientation.z = 0  # -0.052
                goal_1.pose.orientation.w = 0.707  #  0.689
                waypoints.append(goal_1.pose)
                z = z + step_size
                if (z >= FINAL_Z):
                    execute = 1
                #print("added waypoint")
                if (execute == 1):
                    while not rospy.is_shutdown():
                        try:

                            plan = planner.plan_to_pose(waypoints, list())

                            if not control.execute_path(plan):
                                raise Exception("Execution failed")
                        except Exception as e:
                            print e
                        else:
                            break

                    #STOP = control._flag
                    waypoints = list()
                    update = 0
                    #print("EXEcute")
                    #print(i)
                    i = i + 1
            #print("END OF LOOP")
            jenga = 1
        control._probing = 0

        # END OF WORKING CODE

    def moveDown(planner, control, waypoints):
        control._probing = 1
        #STARTOF WORKING CODE
        # Z MUST MOVE UP TWO BLOCK
        # Z = 0.025
        value_of_step = 0.033
        #control._velocity_scalar = 0.5
        jenga = 0
        while (jenga != 1):
            step_size = 0.0005  #0.0025
            FINAL_X = 0.785  #
            offset = 0.14
            tf_px = 0.585
            tf_py = 0.156
            tf_pz = -0.138
            tf_rx = 0
            tf_ry = 0.707
            tf_rz = 0
            tf_rw = 0.707
            goal_x = 0.585
            goal_y = 0.156
            goal_z = -0.138
            try:
                trans = tfBuffer.lookup_transform('base', 'right_gripper_base',
                                                  rospy.Time())
                tf_px = trans.transform.translation.x
                tf_py = trans.transform.translation.y
                tf_pz = trans.transform.translation.z
                tf_rx = trans.transform.rotation.x
                tf_ry = trans.transform.rotation.y
                tf_rz = trans.transform.rotation.z
                tf_rw = trans.transform.rotation.w
                goal_x = tf_px
                goal_y = tf_py
                goal_z = tf_pz
            except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
                    tf2_ros.ExtrapolationException):
                continue
            offset = 0.14
            FINAL_Z = goal_z - value_of_step  # + offsett
            z = tf_pz  #+ offset
            #print("TF'd zero")
            update = 0
            execute = 0
            i = 0
            STOP = 0
            while ((z > FINAL_Z or update == 0) and (i < 3)):
                if (update == 0):
                    try:
                        trans = tfBuffer.lookup_transform(
                            'base', 'right_gripper_base', rospy.Time())
                        tf_px = trans.transform.translation.x
                        tf_py = trans.transform.translation.y
                        tf_pz = trans.transform.translation.z
                        tf_rx = trans.transform.rotation.x
                        tf_ry = trans.transform.rotation.y
                        tf_rz = trans.transform.rotation.z
                        tf_rw = trans.transform.rotation.w
                        z = tf_pz  # + offset
                    except (tf2_ros.LookupException,
                            tf2_ros.ConnectivityException,
                            tf2_ros.ExtrapolationException):
                        continue
                    update = 1
                    execute = 0
                y = tf_py
                x = tf_px + offset
                if (y < goal_y - 0.05):
                    y = y + step_size
                if (y > goal_y + 0.05):
                    y = y - step_size
                #print("TF'd while")
                goal_1 = PoseStamped()
                goal_1.header.frame_id = "base"
                goal_1.pose.position.x = x
                goal_1.pose.position.y = y
                goal_1.pose.position.z = z
                #Orientation as a quaternion
                goal_1.pose.orientation.x = 0  # -0.026
                goal_1.pose.orientation.y = 0.707  #  0.723
                goal_1.pose.orientation.z = 0  # -0.052
                goal_1.pose.orientation.w = 0.707  #  0.689
                waypoints.append(goal_1.pose)
                z = z - step_size
                if (z <= FINAL_Z):
                    execute = 1
                #print("added waypoint")
                if (execute == 1):
                    while not rospy.is_shutdown():
                        try:

                            plan = planner.plan_to_pose(waypoints, list())

                            if not control.execute_path(plan):
                                raise Exception("Execution failed")
                        except Exception as e:
                            print e
                        else:
                            break

                    #STOP = control._flag
                    waypoints = list()
                    update = 0
                    #print("EXEcute")
                    #print(i)
                    i = i + 1
            #print("END OF LOOP")
            jenga = 1
        control._probing = 0
        # END OF WORKING CODE

    def moveLeft(planner, control, waypoints, value_of_step):
        control._probing = 1
        #STARTOF WORKING CODE
        # Z MUST MOVE UP TWO BLOCK
        # Z = 0.025
        #value_of_step = 0.014
        #control._velocity_scalar = 0.5
        jenga = 0
        while (jenga != 1):
            step_size = 0.0005  #0.0025
            FINAL_X = 0.785  #
            offset = 0.14
            tf_px = 0.585
            tf_py = 0.156
            tf_pz = -0.138
            tf_rx = 0
            tf_ry = 0.707
            tf_rz = 0
            tf_rw = 0.707
            goal_x = 0.585
            goal_y = 0.156
            goal_z = -0.138
            try:
                trans = tfBuffer.lookup_transform('base', 'right_gripper_base',
                                                  rospy.Time())
                tf_px = trans.transform.translation.x
                tf_py = trans.transform.translation.y
                tf_pz = trans.transform.translation.z
                tf_rx = trans.transform.rotation.x
                tf_ry = trans.transform.rotation.y
                tf_rz = trans.transform.rotation.z
                tf_rw = trans.transform.rotation.w
                goal_x = tf_px
                goal_y = tf_py
                goal_z = tf_pz
                #print(goal_z)
            except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
                    tf2_ros.ExtrapolationException):
                continue
            offset = 0.14
            FINAL_Y = goal_y + value_of_step  # + offsett
            y = tf_py  #+ offset

            update = 0
            execute = 0
            i = 0
            STOP = 0
            while ((y < FINAL_Y or update == 0) and (i < 3)):
                if (update == 0):
                    try:
                        trans = tfBuffer.lookup_transform(
                            'base', 'right_gripper_base', rospy.Time())
                        tf_px = trans.transform.translation.x
                        tf_py = trans.transform.translation.y
                        tf_pz = trans.transform.translation.z
                        tf_rx = trans.transform.rotation.x
                        tf_ry = trans.transform.rotation.y
                        tf_rz = trans.transform.rotation.z
                        tf_rw = trans.transform.rotation.w
                        y = tf_py  # + offset
                    except (tf2_ros.LookupException,
                            tf2_ros.ConnectivityException,
                            tf2_ros.ExtrapolationException):
                        continue
                    update = 1
                    execute = 0

                x = tf_px + offset
                z = tf_pz

                #print("TF'd while")
                goal_1 = PoseStamped()
                goal_1.header.frame_id = "base"
                goal_1.pose.position.x = x
                goal_1.pose.position.y = y
                goal_1.pose.position.z = z
                #Orientation as a quaternion
                goal_1.pose.orientation.x = 0  # -0.026
                goal_1.pose.orientation.y = 0.707  #  0.723
                goal_1.pose.orientation.z = 0  # -0.052
                goal_1.pose.orientation.w = 0.707  #  0.689
                waypoints.append(goal_1.pose)
                y = y + step_size
                if (y >= FINAL_Y):
                    execute = 1
                #print("added waypoint")
                if (execute == 1):
                    while not rospy.is_shutdown():
                        try:

                            plan = planner.plan_to_pose(waypoints, list())

                            if not control.execute_path(plan):
                                raise Exception("Execution failed")
                        except Exception as e:
                            print e
                        else:
                            break

                    #STOP = control._flag
                    waypoints = list()
                    update = 0
                    #print("EXEcute")
                    #print(i)
                    i = i + 1
            #print("END OF LOOP")
            jenga = 1
        control._probing = 0
        # END OF WORKING CODE

    def moveRight(planner, control, waypoints):
        control._probing = 1
        #STARTOF WORKING CODE
        # Z MUST MOVE UP TWO BLOCK
        # Z = 0.025
        value_of_step = 0.014
        #control._velocity_scalar = 0.5
        jenga = 0
        while (jenga != 1):
            step_size = 0.0005  #0.0025
            FINAL_X = 0.785  #
            offset = 0.14
            tf_px = 0.585
            tf_py = 0.156
            tf_pz = -0.138
            tf_rx = 0
            tf_ry = 0.707
            tf_rz = 0
            tf_rw = 0.707
            goal_x = 0.585
            goal_y = 0.156
            goal_z = -0.138
            try:
                trans = tfBuffer.lookup_transform('base', 'right_gripper_base',
                                                  rospy.Time())
                tf_px = trans.transform.translation.x
                tf_py = trans.transform.translation.y
                tf_pz = trans.transform.translation.z
                tf_rx = trans.transform.rotation.x
                tf_ry = trans.transform.rotation.y
                tf_rz = trans.transform.rotation.z
                tf_rw = trans.transform.rotation.w
                goal_x = tf_px
                goal_y = tf_py
                goal_z = tf_pz
            except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
                    tf2_ros.ExtrapolationException):
                continue
            offset = 0.14
            FINAL_Y = goal_y - value_of_step
            y = tf_py  #+ offset
            #print("TF'd zero")
            update = 0
            execute = 0
            i = 0
            STOP = 0
            while ((y > FINAL_Y or update == 0) and (i < 3)):
                if (update == 0):
                    try:
                        trans = tfBuffer.lookup_transform(
                            'base', 'right_gripper_base', rospy.Time())
                        tf_px = trans.transform.translation.x
                        tf_py = trans.transform.translation.y
                        tf_pz = trans.transform.translation.z
                        tf_rx = trans.transform.rotation.x
                        tf_ry = trans.transform.rotation.y
                        tf_rz = trans.transform.rotation.z
                        tf_rw = trans.transform.rotation.w
                        y = tf_py  # + offset
                    except (tf2_ros.LookupException,
                            tf2_ros.ConnectivityException,
                            tf2_ros.ExtrapolationException):
                        continue
                    update = 1
                    execute = 0
                z = tf_pz
                x = tf_px + offset

                #print("TF'd while")
                goal_1 = PoseStamped()
                goal_1.header.frame_id = "base"
                goal_1.pose.position.x = x
                goal_1.pose.position.y = y
                goal_1.pose.position.z = z
                #Orientation as a quaternion
                goal_1.pose.orientation.x = 0  # -0.026
                goal_1.pose.orientation.y = 0.707  #  0.723
                goal_1.pose.orientation.z = 0  # -0.052
                goal_1.pose.orientation.w = 0.707  #  0.689
                waypoints.append(goal_1.pose)
                y = y - step_size
                if (y <= FINAL_Y):
                    execute = 1
                #print("added waypoint")
                if (execute == 1):
                    while not rospy.is_shutdown():
                        try:

                            plan = planner.plan_to_pose(waypoints, list())

                            if not control.execute_path(plan):
                                raise Exception("Execution failed")
                        except Exception as e:
                            print e
                        else:
                            break

                    #STOP = control._flag
                    print("LAST WAYPOINT")
                    print(waypoints[-1].position)
                    waypoints = list()
                    update = 0
                    #print("EXEcute")
                    #print(i)
                    i = i + 1
            #print("END OF LOOP")
            jenga = 1
        control._probing = 0
        # END OF WORKING CODE

    def probe(planner, control, waypoints):
        control._probing = 1
        value_of_step = 0.004

        step_size = 0.0005  #0.0025
        goal_x = 0.585
        goal_y = 0.156
        goal_z = -0.138
        jenga = 0
        while (jenga != 1):

            FINAL_X = 0.785  #
            offset = 0.14
            tf_px = 0.585
            tf_py = 0.156
            tf_pz = -0.138
            tf_rx = 0
            tf_ry = 0.707
            tf_rz = 0
            tf_rw = 0.707
            goal_x = 0.585
            goal_y = 0.156
            goal_z = -0.138
            try:
                trans = tfBuffer.lookup_transform('base', 'right_gripper_base',
                                                  rospy.Time())
                tf_px = trans.transform.translation.x
                tf_py = trans.transform.translation.y
                tf_pz = trans.transform.translation.z
                tf_rx = trans.transform.rotation.x
                tf_ry = trans.transform.rotation.y
                tf_rz = trans.transform.rotation.z
                tf_rw = trans.transform.rotation.w
                goal_x = tf_px
                goal_y = tf_py
                goal_z = tf_pz
            except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
                    tf2_ros.ExtrapolationException):
                continue

            offset = 0.14
            FINAL_X = goal_x + offset + value_of_step
            x = tf_px + offset
            update = 0
            execute = 0
            STOP = 0
            while ((x < FINAL_X or update == 0)):
                if (update == 0):
                    try:
                        trans = tfBuffer.lookup_transform(
                            'base', 'right_gripper_base', rospy.Time())
                        tf_px = trans.transform.translation.x
                        tf_py = trans.transform.translation.y
                        tf_pz = trans.transform.translation.z
                        tf_rx = trans.transform.rotation.x
                        tf_ry = trans.transform.rotation.y
                        tf_rz = trans.transform.rotation.z
                        tf_rw = trans.transform.rotation.w
                        x = tf_px + offset
                    except (tf2_ros.LookupException,
                            tf2_ros.ConnectivityException,
                            tf2_ros.ExtrapolationException):
                        continue
                    update = 1
                    execute = 0

                y = tf_py
                z = tf_pz
                if (y < goal_y - 0.05):
                    y = y + step_size
                if (y > goal_y + 0.05):
                    y = y - step_size
                if (z < goal_z - 0.05):
                    z = z + step_size
                if (z > goal_z + 0.05):
                    z = z - step_size

                goal_1 = PoseStamped()
                goal_1.header.frame_id = "base"
                goal_1.pose.position.x = x
                goal_1.pose.position.y = y
                goal_1.pose.position.z = z

                goal_1.pose.orientation.x = 0  # -0.026
                goal_1.pose.orientation.y = 0.707  #  0.723
                goal_1.pose.orientation.z = 0  # -0.052
                goal_1.pose.orientation.w = 0.707  #  0..

                #print("added a point")
                waypoints.append(goal_1.pose)
                x = x + step_size
                if (x >= FINAL_X):
                    execute = 1
                if (execute == 1):
                    while not rospy.is_shutdown():
                        try:
                            #print("Executing Probe")

                            plan = planner.plan_to_pose(waypoints, list())
                            if not control.execute_path(plan):
                                raise Exception("Execution failed")
                        except Exception as e:
                            print e
                        else:
                            break

            jenga = 1

        waypoints = waypoints[::-1]

        goal_1 = PoseStamped()
        goal_1.header.frame_id = "base"
        goal_1.pose.position.x = goal_x + offset
        goal_1.pose.position.y = goal_y
        goal_1.pose.position.z = goal_z

        goal_1.pose.orientation.x = 0  # -0.026
        goal_1.pose.orientation.y = 0.707  #  0.723
        goal_1.pose.orientation.z = 0  # -0.052
        goal_1.pose.orientation.w = 0.707  #  0..

        #waypoints.append(goal_1.pose)

        while not rospy.is_shutdown():
            try:
                plan = planner.plan_to_pose(waypoints, list())
                if not control.execute_path(plan):
                    raise Exception("Execution failed")
            except Exception as e:
                print e
            else:
                break

        print("PROBE TEST")
        print(min(list(map(float, control._sensor_data))))
        control._poke_data.append(min(list(map(float, control._sensor_data))))
        control._sensor_data = list()
        control._scalar_data = list()
        control._probing = 0

    def zero_spot(planner, control, waypoints):
        control._probing = 1
        jenga = 0
        while (jenga != 1):
            step_size = 0.0005  #0.0025
            FINAL_X = 0.585
            FINAL_Y = 0.156
            FINAL_Z = -0.138

            offset = 0.14

            tf_px = 0.585
            tf_py = 0.156
            tf_pz = -0.138

            goal_x = 0.585
            goal_y = 0.156
            goal_z = -0.138

            x_done = 0
            y_done = 0
            z_done = 0

            try:
                trans = tfBuffer.lookup_transform('base', 'right_gripper_base',
                                                  rospy.Time())
                tf_px = trans.transform.translation.x
                tf_py = trans.transform.translation.y
                tf_pz = trans.transform.translation.z

            except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
                    tf2_ros.ExtrapolationException):
                continue

            offset = 0.14

            x = tf_px + offset
            y = tf_py
            z = tf_pz

            update = 0
            execute = 0
            i = 0

            while (((x_done == 0 or y_done == 0 or z_done == 0) or update == 0)
                   and (i < 3)):
                if (update == 0):
                    try:
                        trans = tfBuffer.lookup_transform(
                            'base', 'right_gripper_base', rospy.Time())
                        tf_px = trans.transform.translation.x
                        tf_py = trans.transform.translation.y
                        tf_pz = trans.transform.translation.z
                        tf_rx = trans.transform.rotation.x
                        tf_ry = trans.transform.rotation.y
                        tf_rz = trans.transform.rotation.z
                        tf_rw = trans.transform.rotation.w
                        x = tf_px + offset
                        y = tf_py
                        z = tf_pz

                    except (tf2_ros.LookupException,
                            tf2_ros.ConnectivityException,
                            tf2_ros.ExtrapolationException):
                        continue
                    update = 1
                    execute = 0

                #print("TF'd while")
                goal_1 = PoseStamped()
                goal_1.header.frame_id = "base"
                goal_1.pose.position.x = x
                goal_1.pose.position.y = y
                goal_1.pose.position.z = z
                #Orientation as a quaternion
                goal_1.pose.orientation.x = 0  # -0.026
                goal_1.pose.orientation.y = 0.707  #  0.723
                goal_1.pose.orientation.z = 0  # -0.052
                goal_1.pose.orientation.w = 0.707  #  0.689
                waypoints.append(goal_1.pose)

                print(x)
                if (x < goal_x + offset - 0.05):
                    x = x + step_size
                    print("add")
                elif (x > goal_x + offset + 0.05):
                    x = x - step_size
                    print("sub")

                else:
                    x_done = 1
                    print("xdone")

                if (y < goal_y - 0.05):
                    y = y + step_size
                elif (y > goal_y + 0.05):
                    y = y - step_size
                else:
                    y_done = 1
                    print("ydone")

                if (z < goal_z - 0.05):
                    z = z + step_size
                elif (z > goal_z + 0.05):
                    z = z - step_size
                else:
                    z_done = 1
                    print("zdone")

                if (x_done == 1 and y_done == 1 and z_done == 1):
                    execute = 1
                    print("execute done")

                if (execute == 1):
                    while not rospy.is_shutdown():
                        try:

                            plan = planner.plan_to_pose(waypoints, list())
                            print("EXECUTED ZERO")
                            if not control.execute_path(plan):
                                raise Exception("Execution failed")
                        except Exception as e:
                            print e
                        else:
                            break

                    #STOP = control._flag
                    waypoints = list()
                    update = 0
                    #print("EXEcute")
                    #print(i)
                    i = i + 1
            #print("END OF LOOP")
            jenga = 1
        control._probing = 0
        # END OF WORKING CODE

    while not rospy.is_shutdown():

        value_of_step_up = 0.023
        value_of_step_left = 0.0125

        story = 0

        while (story < 1):
            control._probing = 1

            ## ZERO THE ROBOT ##
            goal_1 = PoseStamped()
            goal_1.header.frame_id = "base"
            goal_1.pose.position.x = 0.585
            goal_1.pose.position.y = 0.156
            goal_1.pose.position.z = -0.138
            goal_1.pose.orientation.x = 0  # -0.026
            goal_1.pose.orientation.y = 0.707  #  0.723
            goal_1.pose.orientation.z = 0  # -0.052
            goal_1.pose.orientation.w = 0.707  #  0.689
            waypoints.append(goal_1.pose)

            while not rospy.is_shutdown():
                try:

                    plan = planner.plan_to_pose(waypoints, list())

                    if not control.execute_path(plan):
                        raise Exception("Execution failed")
                except Exception as e:
                    print e
                else:
                    break

            waypoints = list()
            ## ZERO THE ROBOT ##
            print("ROBOT ZEROED")
            #ospy.sleep(2)

            bump = 0
            while (story - bump > 0):
                if (bump > 2):
                    value_of_step_up = 0.033
                else:
                    value_of_step_up = 0.025

                moveUp(planner, control, waypoints, value_of_step_up)
                waypoints = list()
                rospy.sleep(2)

                bump += 1

            probe(planner, control, waypoints)
            waypoints = list()
            rospy.sleep(2)

            moveLeft(planner, control, waypoints, value_of_step_left)
            waypoints = list()
            rospy.sleep(2)

            probe(planner, control, waypoints)
            waypoints = list()
            rospy.sleep(2)

            moveLeft(planner, control, waypoints, value_of_step_left)
            waypoints = list()
            rospy.sleep(2)

            probe(planner, control, waypoints)
            waypoints = list()
            rospy.sleep(2)

            story += 1

        control._probing = 1

        ## ZERO THE ROBOT ##
        goal_1 = PoseStamped()
        goal_1.header.frame_id = "base"
        goal_1.pose.position.x = 0.585
        goal_1.pose.position.y = 0.156
        goal_1.pose.position.z = -0.138
        goal_1.pose.orientation.x = 0  # -0.026
        goal_1.pose.orientation.y = 0.707  #  0.723
        goal_1.pose.orientation.z = 0  # -0.052
        goal_1.pose.orientation.w = 0.707  #  0.689
        waypoints.append(goal_1.pose)
        while not rospy.is_shutdown():
            try:
                plan = planner.plan_to_pose(waypoints, list())
                if not control.execute_path(plan):
                    raise Exception("Execution failed")
            except Exception as e:
                print e
            else:
                break
        waypoints = list()
        ## ZERO THE ROBOT ##
        print("ROBOT ZEROED")
        #rospy.sleep(2)
        control._probing = 0
        control._trigger = 0
        control._flag = 0
        control._zero = 0

        #control._velocity_scalar = 0.5

        step_size = 0.0005  #0.0025

        FINAL_X = 0.785  #

        offset = 0.14

        tf_px = 0.585
        tf_py = 0.156
        tf_pz = -0.138

        tf_rx = 0
        tf_ry = 0.707
        tf_rz = 0
        tf_rw = 0.707

        goal_x = 0.585
        goal_y = 0.156
        goal_z = -0.138

        try:
            trans = tfBuffer.lookup_transform('base', 'right_gripper_base',
                                              rospy.Time())
            tf_px = trans.transform.translation.x
            tf_py = trans.transform.translation.y
            tf_pz = trans.transform.translation.z
            tf_rx = trans.transform.rotation.x
            tf_ry = trans.transform.rotation.y
            tf_rz = trans.transform.rotation.z
            tf_rw = trans.transform.rotation.w

            goal_x = tf_px
            goal_y = tf_py
            goal_z = tf_pz

        except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
                tf2_ros.ExtrapolationException):
            continue

        offset = 0.14
        FINAL_X = goal_x + offset + 0.2

        x = tf_px + offset
        print("TF'd zero")

        update = 0
        execute = 0

        i = 0
        STOP = 0

        while ((x < FINAL_X or update == 0) and (i < 5)):

            if (update == 0):
                try:
                    trans = tfBuffer.lookup_transform('base',
                                                      'right_gripper_base',
                                                      rospy.Time())
                    tf_px = trans.transform.translation.x
                    tf_py = trans.transform.translation.y
                    tf_pz = trans.transform.translation.z
                    tf_rx = trans.transform.rotation.x
                    tf_ry = trans.transform.rotation.y
                    tf_rz = trans.transform.rotation.z
                    tf_rw = trans.transform.rotation.w
                    x = tf_px + offset

                except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
                        tf2_ros.ExtrapolationException):
                    continue
                update = 1
                execute = 0

            y = tf_py
            z = tf_pz

            if (y < goal_y - 0.05):
                y = y + step_size

            if (y > goal_y + 0.05):
                y = y - step_size

            if (z < goal_z - 0.05):
                z = z + step_size

            if (z > goal_z + 0.05):
                z = z - step_size

            #print("TF'd while")
            goal_1 = PoseStamped()
            goal_1.header.frame_id = "base"
            goal_1.pose.position.x = x
            goal_1.pose.position.y = y
            goal_1.pose.position.z = z

            #Orientation as a quaternion
            goal_1.pose.orientation.x = 0  # -0.026
            goal_1.pose.orientation.y = 0.707  #  0.723
            goal_1.pose.orientation.z = 0  # -0.052
            goal_1.pose.orientation.w = 0.707  #  0.689

            waypoints.append(goal_1.pose)

            x = x + step_size

            if (x >= FINAL_X):
                execute = 1
            #print("added waypoint")

            if (execute == 1):
                while not rospy.is_shutdown():
                    try:
                        control._probing = 0

                        plan = planner.plan_to_pose(waypoints, list())
                        print("TAKING BLOCK OUT EXECUTE")
                        if not control.execute_path(plan):
                            raise Exception("Execution failed")
                    except Exception as e:
                        print e
                    else:
                        break

                #STOP = control._flag
                waypoints = list()

                update = 0
                #print("EXEcute")
                #print(i)
                i = i + 1
        print("END OF LOOP")

        control._probing = 0
        control._trigger = 0
        control._flag = 0
        control._zero = 1

        ## ZERO THE ROBOT ##
        goal_1 = PoseStamped()
        goal_1.header.frame_id = "base"
        goal_1.pose.position.x = 0.585
        goal_1.pose.position.y = 0.156
        goal_1.pose.position.z = -0.138
        goal_1.pose.orientation.x = 0  # -0.026
        goal_1.pose.orientation.y = 0.707  #  0.723
        goal_1.pose.orientation.z = 0  # -0.052
        goal_1.pose.orientation.w = 0.707  #  0.689
        waypoints.append(goal_1.pose)
        while not rospy.is_shutdown():
            try:
                plan = planner.plan_to_pose(waypoints, list())
                if not control.execute_path(plan):
                    raise Exception("Execution failed")
            except Exception as e:
                print e
            else:
                break
        waypoints = list()
        ## ZERO THE ROBOT ##
        print("ROBOT ZEROED")
        #ospy.sleep(2)

        control._probing = 0
        control._trigger = 0
        control._flag = 0
        control._zero = 0

        moveLeft(planner, control, waypoints, value_of_step_left)
        waypoints = list()
        rospy.sleep(2)

        moveLeft(planner, control, waypoints, value_of_step_left)
        waypoints = list()
        rospy.sleep(2)

        #control._velocity_scalar = 0.5

        step_size = 0.0005  #0.0025

        FINAL_X = 0.785  #

        offset = 0.14

        tf_px = 0.585
        tf_py = 0.156
        tf_pz = -0.138

        tf_rx = 0
        tf_ry = 0.707
        tf_rz = 0
        tf_rw = 0.707

        goal_x = 0.585
        goal_y = 0.156
        goal_z = -0.138

        try:
            trans = tfBuffer.lookup_transform('base', 'right_gripper_base',
                                              rospy.Time())
            tf_px = trans.transform.translation.x
            tf_py = trans.transform.translation.y
            tf_pz = trans.transform.translation.z
            tf_rx = trans.transform.rotation.x
            tf_ry = trans.transform.rotation.y
            tf_rz = trans.transform.rotation.z
            tf_rw = trans.transform.rotation.w

            goal_x = tf_px
            goal_y = tf_py
            goal_z = tf_pz

        except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
                tf2_ros.ExtrapolationException):
            continue

        offset = 0.14
        FINAL_X = goal_x + offset + 0.2

        x = tf_px + offset
        print("TF'd zero")

        update = 0
        execute = 0

        i = 0
        STOP = 0

        while ((x < FINAL_X or update == 0) and (i < 5)):

            if (update == 0):
                try:
                    trans = tfBuffer.lookup_transform('base',
                                                      'right_gripper_base',
                                                      rospy.Time())
                    tf_px = trans.transform.translation.x
                    tf_py = trans.transform.translation.y
                    tf_pz = trans.transform.translation.z
                    tf_rx = trans.transform.rotation.x
                    tf_ry = trans.transform.rotation.y
                    tf_rz = trans.transform.rotation.z
                    tf_rw = trans.transform.rotation.w
                    x = tf_px + offset

                except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
                        tf2_ros.ExtrapolationException):
                    continue
                update = 1
                execute = 0

            y = tf_py
            z = tf_pz

            if (y < goal_y - 0.05):
                y = y + step_size

            if (y > goal_y + 0.05):
                y = y - step_size

            if (z < goal_z - 0.05):
                z = z + step_size

            if (z > goal_z + 0.05):
                z = z - step_size

            #print("TF'd while")
            goal_1 = PoseStamped()
            goal_1.header.frame_id = "base"
            goal_1.pose.position.x = x
            goal_1.pose.position.y = y
            goal_1.pose.position.z = z

            #Orientation as a quaternion
            goal_1.pose.orientation.x = 0  # -0.026
            goal_1.pose.orientation.y = 0.707  #  0.723
            goal_1.pose.orientation.z = 0  # -0.052
            goal_1.pose.orientation.w = 0.707  #  0.689

            waypoints.append(goal_1.pose)

            x = x + step_size

            if (x >= FINAL_X):
                execute = 1
            #print("added waypoint")

            if (execute == 1):
                while not rospy.is_shutdown():
                    try:
                        control._probing = 0

                        plan = planner.plan_to_pose(waypoints, list())
                        print("TAKING BLOCK OUT EXECUTE")
                        if not control.execute_path(plan):
                            raise Exception("Execution failed")
                    except Exception as e:
                        print e
                    else:
                        break

                #STOP = control._flag
                waypoints = list()

                update = 0
                #print("EXEcute")
                #print(i)
                i = i + 1
        print("END OF LOOP")

        control._probing = 0
        control._trigger = 0
        control._flag = 0
        control._zero = 1

        ## ZERO THE ROBOT ##
        goal_1 = PoseStamped()
        goal_1.header.frame_id = "base"
        goal_1.pose.position.x = 0.585
        goal_1.pose.position.y = 0.156
        goal_1.pose.position.z = -0.138
        goal_1.pose.orientation.x = 0  # -0.026
        goal_1.pose.orientation.y = 0.707  #  0.723
        goal_1.pose.orientation.z = 0  # -0.052
        goal_1.pose.orientation.w = 0.707  #  0.689
        waypoints.append(goal_1.pose)
        while not rospy.is_shutdown():
            try:
                plan = planner.plan_to_pose(waypoints, list())
                if not control.execute_path(plan):
                    raise Exception("Execution failed")
            except Exception as e:
                print e
            else:
                break
        waypoints = list()
        ## ZERO THE ROBOT ##
        print("ROBOT ZEROED")
        #ospy.sleep(2)

        control._probing = 0
        control._trigger = 0
        control._flag = 0
        control._zero = 0

        #waypoints = list()
        #zero_spot(planner, control, waypoints)
        #waypoints = list()

        #rospy.sleep(2)

        #waypoints = list()

        #rospy.sleep(2)

        #zero_spot(planner, control, waypoints)
        #waypoints = list()

        #rospy.sleep(2)

        # #probe(planner, control, waypoints)
        # waypoints = list()

        # moveUp(planner, control, waypoints)
        # waypoints = list()

        # #probe(planner, control, waypoints)
        # waypoints = list()

        # moveLeft(planner, control, waypoints)
        # waypoints = list()

        # #probe(planner, control, waypoints)
        # waypoints = list()

        # moveLeft(planner, control, waypoints)
        # waypoints = list()

        # #probe(planner, control, waypoints)
        # waypoints = list()

        #plt.plot(control._sensor_data)
        s = list(map(float, control._sensor_data))
        scales = list(map(float, control._scalar_data))
        goal_line = list(map(float, control._goal_list))
        #s = list(map(float, control._debug))

        #control._sensor_data = sorted(control._sensor_data)
        #print(min(s))
        #print(max(s))
        plt.subplot(211)
        plt.plot(s)
        plt.plot(goal_line)

        plt.axis([0, len(control._sensor_data), min(s), max(s)])
        plt.ylabel('Sensor Data')
        plt.subplot(212)
        plt.plot(scales)
        plt.show()

        # PRINT GRID

        control._sensor_data = list()
        control._scalar_data = list()
        control._goal_list = list()
        waypoints = list()
        control._velocity_scalar = 1