def robotCode(self):

        self.path = np.array([[0.2, 0.2, -0.2, -0.2], [0.4, -0.4, -0.4, 0.4],
                              [15, 15, 15, 15]])
        #        dash = IDash(framerate=0.1)
        #        plt.plot(self.path[0,:], self.path[1,:])
        #        dash.add(plt)

        #k=0.0345    # ball model: d(t) = vmot*k*(1-exp(-t/T))

        goal = (0.0, 0.0)
        self.path, status = passPath(self.getRobotConf(self.bot),
                                     self.ballEngine.getBallPose(),
                                     goal,
                                     kick=True)
        print self.path

        dash = IDash(framerate=0.1)
        dash.add(lambda: plt.plot(-self.path[1, :], self.path[0, :], 'b-*') and
                 plt.xlim([-0.7, 0.7]) and plt.ylim([-0.7, 0.7]))
        dash.plotframe()
        print 'estimated time path'
        print calculatePathTime(self.path)
        t = self.ballEngine.getSimTime()  # time in seconds
        #        while (self.ballEngine.getSimTime()-t)<10: #End program after 30sec
        cc = 1
        while cc:
            robotConf = self.getRobotConf(self.bot)
            cc = self.followPath(robotConf, status, rb=0.05)
            self.ballEngine.update()
            nextBallPos = self.ballEngine.getNextRestPos()


#            robotConf = self.getRobotConf(self.bot)
#            ballPos = self.ballEngine.getBallPose() # (x, y)
#            vRobot = v2Pos(robotConf, ballPos)

        self.setMotorVelocities(0, 0)
        print 'real time path'
        print(self.ballEngine.getSimTime() - t)
    def robotCode(self):
        
        self.path = np.array([[0.2, 0.2, -0.2, -0.2],
                              [0.4, -0.4, -0.4, 0.4],
                              [15, 15, 15, 15]])                          
#        dash = IDash(framerate=0.1)            
#        plt.plot(self.path[0,:], self.path[1,:])  
#        dash.add(plt)
        
        #k=0.0345    # ball model: d(t) = vmot*k*(1-exp(-t/T))

        goal = (0.0, 0.0)
        self.path, status = passPath(self.getRobotConf(self.bot), self.ballEngine.getBallPose(), goal, kick=True)
        print self.path
        
        dash = IDash(framerate=0.1)
        dash.add(lambda: plt.plot(-self.path[1,:], self.path[0,:], 'b-*') and
            plt.xlim([-0.7, 0.7]) and plt.ylim([-0.7, 0.7]))
        dash.plotframe()       
        print 'estimated time path'
        print calculatePathTime(self.path)
        t = self.ballEngine.getSimTime()     # time in seconds
#        while (self.ballEngine.getSimTime()-t)<10: #End program after 30sec
        cc=1
        while cc:
            robotConf = self.getRobotConf(self.bot)            
            cc = self.followPath(robotConf, status, rb=0.05) 
            self.ballEngine.update()
            nextBallPos = self.ballEngine.getNextRestPos()
#            robotConf = self.getRobotConf(self.bot)
#            ballPos = self.ballEngine.getBallPose() # (x, y)
#            vRobot = v2Pos(robotConf, ballPos)
        
        self.setMotorVelocities(0,0)
        print 'real time path'
        print (self.ballEngine.getSimTime()-t)
Exemplo n.º 3
0
#SOM visualization of MNIST digits
image_template = np.zeros((28,28))
map_image = np.tile(image_template, map_shape)
map_image_w = map_image.copy()
lattice_predictions = np.zeros(map_size)
for node_count in range(map_size):
    m_row, n_col = idx1D_to_idx2D(node_count, map_shape)
    closest_x_idx = np.argmin(cdist(w[:,node_count].reshape(1, -1), trX))
    image_of_closest_x = trX[closest_x_idx].reshape((28,28))
    lattice_predictions[node_count] = trY[closest_x_idx] # prediction mapping
    image_of_w = w[:,node_count].reshape((28,28))
    map_image[m_row*28:(m_row+1)*28, n_col*28:(n_col+1)*28] = image_of_closest_x
    map_image_w[m_row*28:(m_row+1)*28, n_col*28:(n_col+1)*28] = image_of_w

dash.add(lambda: plt.imshow(map_image, cmap='gray'))
dash.add(lambda: plt.imshow(map_image_w, cmap='gray'))

# predictions
def predict(X, w, lattice_predictions):
    n_examples = X.shape[0]
    Ypred = np.zeros(n_examples)
    for row in range(n_examples):
        closest_w_idx = np.argmin(cdist(X[row].reshape(1,-1), w.T))
        Ypred[row] = lattice_predictions[closest_w_idx]
    return Ypred
trYpred = predict(trX, w, lattice_predictions)
teYpred = predict(teX, w, lattice_predictions)

# evaluate
train_acc = accuracy_score(trY, trYpred)
class MyRobotRunner(base_robot.BaseRobotRunner):

    def __init__(self, *args, **kwargs):
        super(MyRobotRunner, self).__init__(*args, **kwargs)
        self.idash = IDash(0.005)

    def robotCode(self):
        #waypoint = [(0.2, 0.4), (0, 0.4), (-0.2, 0.4),(-0.2, 0.2), (-0.2, 0),(0, 0), (0.2, 0),(0.2, -0.2), (0.2, -0.4),(0, -0.4), (-0.2, -0.4)]
        waypoint = [(0, 0.4), (-0.2, 0.4),(-0.2, 0.2), (-0.2, 0),(0, 0), (0.2, 0),(0.2, -0.2), (0.2, -0.4),
                        (0, -0.4),(-0.05, -0.4), (-0.1, -0.4), (-0.15, -0.4), (-0.2, -0.4), (-0.2, -0.4)]
        # waypoint = []
        # r = 0.2
        # increments = 5
        # xCenter = 0
        # yCenter = 0.2
        # for i in range(increments-1):
        #     # circle center: (0, 0.2)
        #     x = r*math.sin(i*math.pi/increments) + xCenter
        #     y = r*math.cos(i*math.pi/increments) + yCenter
        #     waypoint.append((x,y))
        # xCenter = 0
        # yCenter = -0.2
        # for i in range(increments):
        #     # circle center: (0, -0.2)
        #     x = -r*math.sin(i*math.pi/increments) + xCenter
        #     y = r*math.cos(i*math.pi/increments) + yCenter
        #     waypoint.append((x,y))
        # for i in range(3):
        #     waypoint.append((-0.2/4*i,-0.4))
        
        #k=0.0345    # ball model: d(t) = vmot*k*(1-exp(-t/T))

        for i in range(len(waypoint)):
            p0 = self.ballEngine.getBallPose()
            curr_bot_pose = list(self.getRobotConf(self.bot))
            curr_bot_pose[1] -= 0.05
            self.path, status = passPath(curr_bot_pose, p0, waypoint[i],vmax=10,vr=7, kq= 0.0035)
            
            print self.path
            
#            dash = IDash(framerate=0.1)
#            dash.add(lambda: plt.plot(-self.path[1,:], self.path[0,:], 'b-*') and
#                plt.xlim([-0.7, 0.7]) and plt.ylim([-0.7, 0.7]))
#            dash.plotframe()       
            print 'estimated time path'
            # print calculatePathTime(self.path)
            t = self.ballEngine.getSimTime()
            t = self.ballEngine.getSimTime()     # time in seconds
            while (self.ballEngine.getSimTime()-t)<10: #End program after 30sec
    #        cc=1
    #        while cc:
                def vizBots():
                    actx, acty = self.getRobotConf()[:2]
                    ballx, bally = self.ballEngine.getBallPose()[:2]
                    plt.hold('on')
                    plt.plot(-waypoint[i][1], waypoint[i][0], 'k*')
                    plt.plot(-bally, ballx, 'b.')
                    plt.plot(-acty, actx, 'g+')
                    plt.plot(-self.path[1,:], self.path[0,:], 'r.')
                    plt.xlim([-0.9, 0.9]) # y axis in the field
                    plt.ylim([-0.65, 0.65]) # x axis in the field
                    plt.xlabel('active path length: {}'.format(self.path.shape[1]))
                self.idash.add(vizBots)
                
                robotConf = self.getRobotConf(self.bot)            
                cc = self.followPath(robotConf, rb=0.05) 
                self.ballEngine.update()
                
                
                p1 = self.ballEngine.getBallPose()
                p3 = self.ballEngine.getNextRestPos()
                dist_from_start = np.sqrt((p1[0] - p0[0])**2 + (p1[1] - p0[1])**2)
                velocity_measure = np.sqrt((p1[0] - p3[0])**2 + (p1[1] - p3[1])**2)
                #self.idash.plotframe()
                if dist_from_start > 0.01: # the ball has been touched
                    if velocity_measure < 0.003:
                        break# wait til velocity reaches zero
    #            robotConf = self.getRobotConf(self.bot)
    #            ballPos = self.ballEngine.getBallPose() # (x, y)
    #            vRobot = v2Pos(robotConf, ballPos)
                
        self.setMotorVelocities(0,0)
        print 'real time path'
        print (self.ballEngine.getSimTime()-t)
Exemplo n.º 5
0
class Lab2Program:

    def __init__(self):
        self.initialize_vrep_client()
        self.initilize_vrep_api()
        self.define_constants()
        self.killer = GracefulKiller()
        self.idash = IDash(framerate=0.05)

    def initialize_vrep_client(self):
        #Initialisation for Python to connect to VREP
        print 'Python program started'
        count = 0
        num_tries = 10
        while count < num_tries:
            vrep.simxFinish(-1) # just in case, close all opened connections
            self.clientID=vrep.simxStart('127.0.0.1',19999,True,True,5000,5) #Timeout=5000ms, Threadcycle=5ms
            if self.clientID!=-1:
                print 'Connected to V-REP'
                break
            else:
                "Trying again in a few moments..."
                time.sleep(3)
                count += 1
        if count >= num_tries:
            print 'Failed connecting to V-REP'
            vrep.simxFinish(self.clientID)

    def initilize_vrep_api(self):
        # initialize ePuck handles and variables
        _, self.bodyelements=vrep.simxGetObjectHandle(
            self.clientID, 'ePuck_bodyElements', vrep.simx_opmode_oneshot_wait)
        _, self.leftMotor=vrep.simxGetObjectHandle(
            self.clientID, 'ePuck_leftJoint', vrep.simx_opmode_oneshot_wait)
        _, self.rightMotor=vrep.simxGetObjectHandle(
            self.clientID, 'ePuck_rightJoint', vrep.simx_opmode_oneshot_wait)
        _, self.ePuck=vrep.simxGetObjectHandle(
            self.clientID, 'ePuck', vrep.simx_opmode_oneshot_wait)
        _, self.ePuckBase=vrep.simxGetObjectHandle(
            self.clientID, 'ePuck_base', vrep.simx_opmode_oneshot_wait)
        # proxSens = prox_sens_initialize(self.clientID)
        # initialize odom of ePuck
        _, self.xyz = vrep.simxGetObjectPosition(
            self.clientID, self.ePuck, -1, vrep.simx_opmode_streaming)
        _, self.eulerAngles = vrep.simxGetObjectOrientation(
            self.clientID, self.ePuck, -1, vrep.simx_opmode_streaming)

        # initialize overhead cam
        _, self.overheadCam=vrep.simxGetObjectHandle(
            self.clientID, 'Global_Vision', vrep.simx_opmode_oneshot_wait)
        _, self.resolution, self.image = vrep.simxGetVisionSensorImage(
            self.clientID,self.overheadCam,0,vrep.simx_opmode_oneshot_wait)

        # initialize goal handle + odom
        _, self.goalHandle=vrep.simxGetObjectHandle(
            self.clientID, 'Goal_Position', vrep.simx_opmode_oneshot_wait)
        _, self.goalPose = vrep.simxGetObjectPosition(
            self.clientID, self.goalHandle, -1, vrep.simx_opmode_streaming)

        # STOP Motor Velocities.  Not sure if this is necessary
        _ = vrep.simxSetJointTargetVelocity(
            self.clientID,self.leftMotor,0,vrep.simx_opmode_oneshot_wait)
        _ = vrep.simxSetJointTargetVelocity(
            self.clientID,self.rightMotor,0,vrep.simx_opmode_oneshot_wait)

    def define_constants(self):
        self.map_side_length = 2.55
        # FIMXE: hard coded goals
        # self.GOALS = [(40+2,6), (40, 6+2), (40,21), (35, 19), (30,22),  (29,10), (27,5), (20,8), (20,33), (20, 48), (5,55)]
        self.GOALS = None
        self.worldNorthTheta = None
        self.maxVelocity = 2.0
        self.history_length = 5
        self.theta_history = RingBuffer(self.history_length)
        self.e_theta_h = RingBuffer(self.history_length)
        self.blurred_paths = None
        self.path_skip = 8

    def global_map_preprocess(self, resolution, image):
        im = format_vrep_image(resolution, image) # original image
        im = image_vert_flip(im)
        resize_length = int(im.shape[0]/2)
        im = skimage.transform.resize(im, (resize_length, resize_length, 3))
        return im

    def global_map_process(self, im):
        walls = im[:,:,0] > 0.25
        paths = walls < 0.15
        if self.blurred_paths is None:
            print "Computed"
            blurred_map = skimage.filters.gaussian_filter(walls, sigma=2)
            blurred_paths = blurred_map < 0.15
            # np.save('binary_grid.npy', blurred_paths)
            return paths, blurred_paths
        else:
            return paths, self.blurred_paths

    def robot_pose_get(self):
        _, xyz = vrep.simxGetObjectPosition(
            self.clientID, self.ePuck, -1, vrep.simx_opmode_buffer)
        _, eulerAngles = vrep.simxGetObjectOrientation(
            self.clientID, self.ePuck, -1, vrep.simx_opmode_buffer)
        x, y, z = xyz
        theta = eulerAngles[2]
        self.theta_history.append(theta)

        return (x, y, theta)

    def lidar_scan_get(self, window_size=21):
        """ gets lidar scan range values """
        fr = (window_size - 1) / 2 # fire range
        lidarValues = pseudoLidarSensor(self.paths, self.pose[0], self.pose[1], fr, original_four=False)
        return lidarValues

    def repulsion_vectors_compute(self, lidarValues, k_repulse=10.0):
        numLidarValues = len(lidarValues)
        lidarAngles = [np.pi / numLidarValues * index for index in range(numLidarValues)]

        repulsionVectors = [np.array(pol2cart(force_repulsion(k_repulse, np.sqrt(val), 2), angle)) for val, angle in zip(lidarValues, lidarAngles)]

        return repulsionVectors

    def attraction_vector_compute(self, k_attract=100.0):
        self.attractionVal, self.attractionAngle = cart2pol(
            self.goal_pose_pixel[1] - self.pose_pixel[1], # cols counts same     to normal horz axes
            self.pose_pixel[0] - self.goal_pose_pixel[0]  # rows counts opposite to normal vert axes
        )
        attractionVector = np.array(pol2cart(k_attract*self.attractionVal, self.attractionAngle))
        return attractionVector

    def robot_code(self):
        t = time.time()

        self.curr_goal = 0
        while (time.time() - t) < 200:

            # global map
            _,resolution,image = vrep.simxGetVisionSensorImage(self.clientID,self.overheadCam,0,vrep.simx_opmode_oneshot_wait) # Get image
            im = self.global_map_preprocess(resolution, image)

            self.pose = self.robot_pose_get()
            x, y, theta = self.pose
            # theta = self.theta_history.weighted_average(scheme='last', mathfunc=lambda x: np.exp(x))

            # initialize worldNorthTheta for the first time
            if self.worldNorthTheta is None:
                self.worldNorthTheta = self.pose[2]
                _ = [self.theta_history.append(self.pose[2]) for _ in range(self.history_length)]

            self.pose_pixel = np.array(odom2pixelmap(x, y, self.map_side_length, im.shape[0]))
            m, n = self.pose_pixel


            self.paths, self.blurred_paths = self.global_map_process(im)

            # calculate intermediate goals once
            if self.GOALS is None:
                raw_input("")
                # acquire pixel location of goal
                _, finishPose = vrep.simxGetObjectPosition(
                    self.clientID, self.goalHandle, -1, vrep.simx_opmode_buffer)
                self.finish_pixel = odom2pixelmap(finishPose[0], finishPose[1], self.map_side_length, im.shape[0])
                contiguousPath = AStarBinaryGrid(self.blurred_paths).calculate_path(self.pose_pixel, self.finish_pixel)
                self.GOALS = contiguousPath[::self.path_skip]
                # SKIP THIS FIRST LOOP AND CONTINUE
                continue
            else:
                self.goal_pose_pixel = np.array(self.GOALS[self.curr_goal])
                goal_m, goal_n = self.goal_pose_pixel
            lidarValues = self.lidar_scan_get(window_size=21)


            #############################
            # Potential Field Algorithm #
            #############################
            repulsionVectors = self.repulsion_vectors_compute(lidarValues, k_repulse=10.0)
            attractionVector = self.attraction_vector_compute(k_attract=100.0)

            finalVector = np.sum(np.vstack((repulsionVectors, attractionVector)), axis=0)
            finalUnitVector = finalVector / np.linalg.norm(finalVector)
            print "finalUnitVector: ", finalUnitVector

            # TODO: do we use the unit vector (for direction) only, or the finalVector?
            finalValue, finalAngle = cart2pol(finalUnitVector[0], finalUnitVector[1])

            error_theta = angle_diff(mapTheta2worldTheta(finalAngle, self.worldNorthTheta), theta)
            self.e_theta_h.append(error_theta)

            k_angular_p = 2.0 * self.maxVelocity
            k_angular_D = 0.25
            k_angular_S = 1.0
            k_angular_I = 2.5
            omega = k_angular_p * (
                  error_theta
                + k_angular_D / k_angular_S * (self.e_theta_h[-1] - self.e_theta_h[-2])
                + k_angular_S / k_angular_I * sum(self.e_theta_h)
            )
            print "Omega: ", round(omega,1)

            #############################
            # StateController Algorithm #
            #############################
            # if desired heading is not directly in front
            if np.abs(error_theta) > np.pi / 3:
                # turn in place
                forward_vel = self.maxVelocity
                # omega *= 0.25
            else:
                # Direct yourself to the goal
                goal_distance = np.linalg.norm(self.goal_pose_pixel - self.pose_pixel)
                print "distance_from_goal: ", goal_distance
                if goal_distance <= 4:
                    # Achieved Goal!
                    forward_vel = self.maxVelocity * 0.25 # Slow down to prepare for the next one
                    self.curr_goal += 1
                else:
                    forward_vel = self.maxVelocity

            # control the motors
            ctrl_sig_left, ctrl_sig_right = vomega2bytecodes(forward_vel, omega, g=1)
            _ = vrep.simxSetJointTargetVelocity(
                self.clientID,self.leftMotor,ctrl_sig_left,vrep.simx_opmode_oneshot_wait) # set left wheel velocity
            _ = vrep.simxSetJointTargetVelocity(
                self.clientID,self.rightMotor,ctrl_sig_right,vrep.simx_opmode_oneshot_wait) # set right wheel velocity

            self.idash.add(lambda: self.plot_maze(self.blurred_paths*1.0, m, n, goal_m, goal_n))
            self.idash.add(lambda: self.plot_maze(im, m, n, goal_m, goal_n))
            def plot_current_and_desired_heading():
                self.plot_unit_quiver(finalUnitVector, 'r')
                self.plot_unit_quiver(pol2cart(1, worldTheta2mapTheta(theta, self.worldNorthTheta)), 'k')
                plt.title("Error Theta: %f" % error_theta)
            self.idash.add(plot_current_and_desired_heading)
            self.idash.add(self.plot_theta_history)

            self.idash.plotframe()

            if self.killer.kill_now:
                self.clean_exit()

    def plot_maze(self, im, m, n, goal_m, goal_n):
        """ plots the maze, with robot pose and goal pose visualized """
        if len(im.shape) == 3:
            goal_pixel_values = np.array((1.0, 1.0, 1.0))
            robot_pixel_values = np.array((255.0/255.0,192/255.0,203/255.0))
        elif len(im.shape) == 2:
            goal_pixel_values = 0.5
            robot_pixel_values = 0.5
        im[goal_m,goal_n] = goal_pixel_values
        im[m,n] = robot_pixel_values
        plt.imshow(im)

    def plot_unit_quiver(self, vector, color):
        X, Y = (0, 0)
        U, V = (vector[0], vector[1])
        plt.quiver(X,Y,U,V,angles='xy',scale_units='xy',scale=1,color=color)
        plt.xlim([-1.1,1.1])
        plt.ylim([-1.1,1.1])

    def plot_theta_history(self, expansion=5):
        plt.plot([theta for theta in self.theta_history])
        if len(self.theta_history) < expansion:
            plt.xlim([0, expansion])
        else:
            plt.xlim([0, len(self.theta_history)])
        ylim = np.pi + 0.5
        plt.ylim([-ylim, ylim])

    def plot_all_goals(self, im):
        # display all goals
        for goal_idx in range(len(self.GOALS)):
            im[self.GOALS[goal_idx][0], self.GOALS[goal_idx][1]] = np.array((1.0, 1.0, 1.0))

    def clean_exit(self):
        _ = vrep.simxStopSimulation(self.clientID,vrep.simx_opmode_oneshot_wait)
        vrep.simxFinish(self.clientID)
        print 'Program ended'
        sys.exit(0)

    def run(self):
        if self.clientID!=-1:
            _ = vrep.simxStartSimulation(self.clientID,vrep.simx_opmode_oneshot_wait)
            self.robot_code()

        self.clean_exit()
class FinalProjectProgram():
    def __init__(self, color, number):
        # parameter init
        self.bot_name = '%s%d' % (color, number)
        self.bot_nameStriker = 'Red1'

        # run startup methods
        self.initializeVrepClient()
        self.initializeVrepApi()
        # self.killer = GracefulKiller()
        self.idash = IDash(framerate=0.05)

    def initializeVrepClient(self):
        #Initialisation for Python to connect to VREP
        print 'Python program started'
        count = 0
        num_tries = 10
        while count < num_tries:
            vrep.simxFinish(-1)  # just in case, close all opened connections
            self.clientID = vrep.simxStart('127.0.0.1', 19999, True, True,
                                           5000,
                                           5)  #Timeout=5000ms, Threadcycle=5ms
            if self.clientID != -1:
                print 'Connected to V-REP'
                break
            else:
                "Trying again in a few moments..."
                time.sleep(3)
                count += 1
        if count >= num_tries:
            print 'Failed connecting to V-REP'
            vrep.simxFinish(self.clientID)

    def initializeVrepApi(self):
        # initialize bot handles and variables
        _, self.leftMotor = vrep.simxGetObjectHandle(
            self.clientID, '%s_leftJoint' % self.bot_name,
            vrep.simx_opmode_oneshot_wait)
        _, self.rightMotor = vrep.simxGetObjectHandle(
            self.clientID, '%s_rightJoint' % self.bot_name,
            vrep.simx_opmode_oneshot_wait)
        _, self.bot = vrep.simxGetObjectHandle(self.clientID, self.bot_name,
                                               vrep.simx_opmode_oneshot_wait)
        # proxSens = prox_sens_initialize(self.clientID)
        # initialize odom of bot
        _, self.xyz = vrep.simxGetObjectPosition(self.clientID, self.bot, -1,
                                                 vrep.simx_opmode_streaming)
        _, self.eulerAngles = vrep.simxGetObjectOrientation(
            self.clientID, self.bot, -1, vrep.simx_opmode_streaming)

        # # initialize overhead cam
        # _, self.overheadCam=vrep.simxGetObjectHandle(
        #     self.clientID, 'Global_Vision', vrep.simx_opmode_oneshot_wait)
        # _, self.resolution, self.image = vrep.simxGetVisionSensorImage(
        #     self.clientID,self.overheadCam,0,vrep.simx_opmode_oneshot_wait)

        # # initialize goal handle + odom
        # _, self.goalHandle=vrep.simxGetObjectHandle(
        #     self.clientID, 'Goal_Position', vrep.simx_opmode_oneshot_wait)
        # _, self.goalPose = vrep.simxGetObjectPosition(
        #     self.clientID, self.goalHandle, -1, vrep.simx_opmode_streaming)

        # initialize bot handles and variables for Red1
        _, self.leftMotorStriker = vrep.simxGetObjectHandle(
            self.clientID, '%s_leftJoint' % self.bot_name,
            vrep.simx_opmode_oneshot_wait)
        _, self.rightMotorStriker = vrep.simxGetObjectHandle(
            self.clientID, '%s_rightJoint' % self.bot_name,
            vrep.simx_opmode_oneshot_wait)
        _, self.botStriker = vrep.simxGetObjectHandle(
            self.clientID, self.bot_nameStriker, vrep.simx_opmode_oneshot_wait)
        _, xyzStriker = vrep.simxGetObjectPosition(self.clientID,
                                                   self.botStriker, -1,
                                                   vrep.simx_opmode_streaming)
        _, eulerAnglesStriker = vrep.simxGetObjectOrientation(
            self.clientID, self.botStriker, -1, vrep.simx_opmode_streaming)

    def robotCode(self):
        """ OUR ROBOT CODE GOES HERE """
        #        count = 0
        #        while True:
        #            self.unittestTurnSideways(count)
        #            count += 1

        xGoalie = self.getRobotPose(self.bot)[0]
        yGoalie = self.getRobotPose(self.bot)[1]
        thetaGoalie = self.getRobotPose(self.bot)[2]
        thetaStriker = self.getRobotPose(self.botStriker)[2]

        # ball pose
        ballPose = (0, 0)  # (x, y)

        goalie2Ball = abs(yGoalie -
                          ballPose[1])  # distance between ball and goalkeeper
        goalieDistance = abs(goalie2Ball * math.tan(thetaStriker))

        if thetaStriker < 0:
            xSaving = xGoalie + goalieDistance
        else:
            xSaving = xGoalie - goalieDistance

        savingPosition = [xSaving, yGoalie,
                          z]  # position in which the the goalie saves the ball

        #        desiredHeading = -math.pi/2
        #        tolerance = 5*math.pi/360
        #        error = []
        #        Kp = 2
        #        Tdiff = 0.01
        #        Tint = 0.1
        #        Tsample = 0.02
        #        i = 0
        #
        #        while ((thetaGoalie < (desiredHeading - tolerance) or thetaGoalie > (desiredHeading + tolerance))) and i <= 20:
        #                error.append(abs(thetaGoalie - desiredHeading))
        #                if i == 0:
        #                    vDiff = Kp*error[i]
        #                else:
        #                    errorSum = 0
        #                    for j in range(i):
        #                        errorSum += error[j]
        #                    vDiff = Kp*(error[i] + Tdiff/Tsample*(error[i] - error[i-1]) + Tsample*Tint*errorSum)
        #                _ = vrep.simxSetJointTargetVelocity(
        #                    self.clientID,self.leftMotor,-vDiff,vrep.simx_opmode_oneshot_wait) # set left wheel velocity
        #                _ = vrep.simxSetJointTargetVelocity(
        #                    self.clientID,self.rightMotor,vDiff,vrep.simx_opmode_oneshot_wait) # set right wheel velocity
        #                time.sleep(Tsample)
        #                thetaGoalie = self.getRobotPose(self.bot)[2]
        #                #print thetaGoalie
        #                i += 1
        #
        #        xTolerance = 0.0001
        #        error = []
        #        i = 0
        #        Kp = 100
        #        Tint = 1
        #        Tsample = 0.01
        #
        #        while ((xGoalie < (xSaving - xTolerance) or xGoalie > (xSaving + xTolerance))) and i <= 25:
        #            error.append(xGoalie - xSaving)
        #            if i == 0:
        #                vComm = Kp*error[i]
        #            else:
        #                errorSum = 0
        #                for j in range(i):
        #                    errorSum += error[j]
        #                vComm = Kp*(error[i] + Tdiff/Tsample*(error[i] - error[i-1]) + Tsample*Tint*errorSum)
        #            _ = vrep.simxSetJointTargetVelocity(
        #                self.clientID,self.leftMotor,-vComm,vrep.simx_opmode_oneshot_wait) # set left wheel velocity
        #            _ = vrep.simxSetJointTargetVelocity(
        #                self.clientID,self.rightMotor,-vComm,vrep.simx_opmode_oneshot_wait) # set right wheel velocity
        #            time.sleep(Tsample)
        #            xGoalie = self.getRobotPose(self.bot)[0]
        #            #print xGoalie
        #            i += 1

        while 1:
            robotConf = self.getRobotConf(self.bot)
            print('robotConf.x=%f' % robotConf[0])
            print('robotConf.y=%f' % robotConf[1])
            print('robotConf.theta=%f' % robotConf[2])
            print('savingPosition.x=%f' % savingPosition[0])
            print('savingPosition.y=%f' % savingPosition[1])
            print('savingPosition.theta=%f' % savingPosition[2])
            vRobot = v2Pos(robotConf, savingPosition)
            print('vRobot.x=%f' % vRobot[0])
            print('vRobot.y=%f' % vRobot[1])
            self.setMotorVelocities(vRobot[0], vRobot[1])
            #self.setDriveVelocities(0, 0)

        time.sleep(3)

    def getRobotConf(self, robot_handle):
        _, xyz = vrep.simxGetObjectPosition(self.clientID, robot_handle, -1,
                                            vrep.simx_opmode_buffer)
        _, eulerAngles = vrep.simxGetObjectOrientation(self.clientID,
                                                       robot_handle, -1,
                                                       vrep.simx_opmode_buffer)
        x, y, z = xyz
        theta = eulerAngles[2]

        return (x, y, theta)

    def getRobotPose(self, robot_handle):
        _, xyz = vrep.simxGetObjectPosition(self.clientID, robot_handle, -1,
                                            vrep.simx_opmode_buffer)
        _, eulerAngles = vrep.simxGetObjectOrientation(self.clientID,
                                                       robot_handle, -1,
                                                       vrep.simx_opmode_buffer)
        x, y, z = xyz
        theta = eulerAngles[2]

        return (x, y, theta)

    def setMotorVelocities(self, forward_vel, omega):
        ctrl_sig_left, ctrl_sig_right = vomega2bytecodes(forward_vel,
                                                         omega,
                                                         g=1)
        _ = vrep.simxSetJointTargetVelocity(
            self.clientID, self.leftMotor, ctrl_sig_left,
            vrep.simx_opmode_oneshot_wait)  # set left wheel velocity
        _ = vrep.simxSetJointTargetVelocity(
            self.clientID, self.rightMotor, ctrl_sig_right,
            vrep.simx_opmode_oneshot_wait)  # set right wheel velocity

    def unittestMoveForward(self):
        self.setMotorVelocities(forward_vel=1, omega=0)

    def unittestTurnSideways(self, not_first_time):
        x, y, theta = self.getRobotPose()
        if not_first_time:
            goal_theta = self.first_theta + np.pi / 2
            print goal_theta
            error_theta = ThetaRange.angleDiff(theta, goal_theta)

            # control
            omega = 10 * error_theta
            print omega
            self.setMotorVelocities(0, omega)

            # visualization
            def plotCurrentDesiredHeadings():
                plotVector(ThetaRange.pol2cart(1, theta), 'k')
                plotVector(ThetaRange.pol2cart(1, goal_theta), 'r')

            self.idash.add(plotCurrentDesiredHeadings)
            self.idash.plotframe()
        else:
            self.first_theta = theta

    def clean_exit(self):
        _ = vrep.simxStopSimulation(self.clientID,
                                    vrep.simx_opmode_oneshot_wait)
        vrep.simxFinish(self.clientID)
        print 'Program ended'

    def run(self):
        if self.clientID != -1:
            _ = vrep.simxStartSimulation(self.clientID,
                                         vrep.simx_opmode_oneshot_wait)
            self.robotCode()

        self.clean_exit()
Exemplo n.º 7
0
class Lab2Program:
    def __init__(self):
        self.initialize_vrep_client()
        self.initilize_vrep_api()
        self.define_constants()
        self.killer = GracefulKiller()
        self.idash = IDash(framerate=0.05)

    def initialize_vrep_client(self):
        #Initialisation for Python to connect to VREP
        print 'Python program started'
        count = 0
        num_tries = 10
        while count < num_tries:
            vrep.simxFinish(-1)  # just in case, close all opened connections
            self.clientID = vrep.simxStart('127.0.0.1', 19999, True, True,
                                           5000,
                                           5)  #Timeout=5000ms, Threadcycle=5ms
            if self.clientID != -1:
                print 'Connected to V-REP'
                break
            else:
                "Trying again in a few moments..."
                time.sleep(3)
                count += 1
        if count >= num_tries:
            print 'Failed connecting to V-REP'
            vrep.simxFinish(self.clientID)

    def initilize_vrep_api(self):
        # initialize ePuck handles and variables
        _, self.bodyelements = vrep.simxGetObjectHandle(
            self.clientID, 'ePuck_bodyElements', vrep.simx_opmode_oneshot_wait)
        _, self.leftMotor = vrep.simxGetObjectHandle(
            self.clientID, 'ePuck_leftJoint', vrep.simx_opmode_oneshot_wait)
        _, self.rightMotor = vrep.simxGetObjectHandle(
            self.clientID, 'ePuck_rightJoint', vrep.simx_opmode_oneshot_wait)
        _, self.ePuck = vrep.simxGetObjectHandle(self.clientID, 'ePuck',
                                                 vrep.simx_opmode_oneshot_wait)
        _, self.ePuckBase = vrep.simxGetObjectHandle(
            self.clientID, 'ePuck_base', vrep.simx_opmode_oneshot_wait)
        # proxSens = prox_sens_initialize(self.clientID)
        # initialize odom of ePuck
        _, self.xyz = vrep.simxGetObjectPosition(self.clientID, self.ePuck, -1,
                                                 vrep.simx_opmode_streaming)
        _, self.eulerAngles = vrep.simxGetObjectOrientation(
            self.clientID, self.ePuck, -1, vrep.simx_opmode_streaming)

        # initialize overhead cam
        _, self.overheadCam = vrep.simxGetObjectHandle(
            self.clientID, 'Global_Vision', vrep.simx_opmode_oneshot_wait)
        _, self.resolution, self.image = vrep.simxGetVisionSensorImage(
            self.clientID, self.overheadCam, 0, vrep.simx_opmode_oneshot_wait)

        # initialize goal handle + odom
        _, self.goalHandle = vrep.simxGetObjectHandle(
            self.clientID, 'Goal_Position', vrep.simx_opmode_oneshot_wait)
        _, self.goalPose = vrep.simxGetObjectPosition(
            self.clientID, self.goalHandle, -1, vrep.simx_opmode_streaming)

        # STOP Motor Velocities.  Not sure if this is necessary
        _ = vrep.simxSetJointTargetVelocity(self.clientID, self.leftMotor, 0,
                                            vrep.simx_opmode_oneshot_wait)
        _ = vrep.simxSetJointTargetVelocity(self.clientID, self.rightMotor, 0,
                                            vrep.simx_opmode_oneshot_wait)

    def define_constants(self):
        self.map_side_length = 2.55
        # FIMXE: hard coded goals
        # self.GOALS = [(40+2,6), (40, 6+2), (40,21), (35, 19), (30,22),  (29,10), (27,5), (20,8), (20,33), (20, 48), (5,55)]
        self.GOALS = None
        self.worldNorthTheta = None
        self.maxVelocity = 2.0
        self.history_length = 5
        self.theta_history = RingBuffer(self.history_length)
        self.e_theta_h = RingBuffer(self.history_length)
        self.blurred_paths = None
        self.path_skip = 8

    def global_map_preprocess(self, resolution, image):
        im = format_vrep_image(resolution, image)  # original image
        im = image_vert_flip(im)
        resize_length = int(im.shape[0] / 2)
        im = skimage.transform.resize(im, (resize_length, resize_length, 3))
        return im

    def global_map_process(self, im):
        walls = im[:, :, 0] > 0.25
        paths = walls < 0.15
        if self.blurred_paths is None:
            print "Computed"
            blurred_map = skimage.filters.gaussian_filter(walls, sigma=2)
            blurred_paths = blurred_map < 0.15
            # np.save('binary_grid.npy', blurred_paths)
            return paths, blurred_paths
        else:
            return paths, self.blurred_paths

    def robot_pose_get(self):
        _, xyz = vrep.simxGetObjectPosition(self.clientID, self.ePuck, -1,
                                            vrep.simx_opmode_buffer)
        _, eulerAngles = vrep.simxGetObjectOrientation(self.clientID,
                                                       self.ePuck, -1,
                                                       vrep.simx_opmode_buffer)
        x, y, z = xyz
        theta = eulerAngles[2]
        self.theta_history.append(theta)

        return (x, y, theta)

    def lidar_scan_get(self, window_size=21):
        """ gets lidar scan range values """
        fr = (window_size - 1) / 2  # fire range
        lidarValues = pseudoLidarSensor(self.paths,
                                        self.pose[0],
                                        self.pose[1],
                                        fr,
                                        original_four=False)
        return lidarValues

    def repulsion_vectors_compute(self, lidarValues, k_repulse=10.0):
        numLidarValues = len(lidarValues)
        lidarAngles = [
            np.pi / numLidarValues * index for index in range(numLidarValues)
        ]

        repulsionVectors = [
            np.array(
                pol2cart(force_repulsion(k_repulse, np.sqrt(val), 2), angle))
            for val, angle in zip(lidarValues, lidarAngles)
        ]

        return repulsionVectors

    def attraction_vector_compute(self, k_attract=100.0):
        self.attractionVal, self.attractionAngle = cart2pol(
            self.goal_pose_pixel[1] -
            self.pose_pixel[1],  # cols counts same     to normal horz axes
            self.pose_pixel[0] -
            self.goal_pose_pixel[0]  # rows counts opposite to normal vert axes
        )
        attractionVector = np.array(
            pol2cart(k_attract * self.attractionVal, self.attractionAngle))
        return attractionVector

    def robot_code(self):
        t = time.time()

        self.curr_goal = 0
        while (time.time() - t) < 200:

            # global map
            _, resolution, image = vrep.simxGetVisionSensorImage(
                self.clientID, self.overheadCam, 0,
                vrep.simx_opmode_oneshot_wait)  # Get image
            im = self.global_map_preprocess(resolution, image)

            self.pose = self.robot_pose_get()
            x, y, theta = self.pose
            # theta = self.theta_history.weighted_average(scheme='last', mathfunc=lambda x: np.exp(x))

            # initialize worldNorthTheta for the first time
            if self.worldNorthTheta is None:
                self.worldNorthTheta = self.pose[2]
                _ = [
                    self.theta_history.append(self.pose[2])
                    for _ in range(self.history_length)
                ]

            self.pose_pixel = np.array(
                odom2pixelmap(x, y, self.map_side_length, im.shape[0]))
            m, n = self.pose_pixel

            self.paths, self.blurred_paths = self.global_map_process(im)

            # calculate intermediate goals once
            if self.GOALS is None:
                raw_input("")
                # acquire pixel location of goal
                _, finishPose = vrep.simxGetObjectPosition(
                    self.clientID, self.goalHandle, -1,
                    vrep.simx_opmode_buffer)
                self.finish_pixel = odom2pixelmap(finishPose[0], finishPose[1],
                                                  self.map_side_length,
                                                  im.shape[0])
                contiguousPath = AStarBinaryGrid(
                    self.blurred_paths).calculate_path(self.pose_pixel,
                                                       self.finish_pixel)
                self.GOALS = contiguousPath[::self.path_skip]
                # SKIP THIS FIRST LOOP AND CONTINUE
                continue
            else:
                self.goal_pose_pixel = np.array(self.GOALS[self.curr_goal])
                goal_m, goal_n = self.goal_pose_pixel
            lidarValues = self.lidar_scan_get(window_size=21)

            #############################
            # Potential Field Algorithm #
            #############################
            repulsionVectors = self.repulsion_vectors_compute(lidarValues,
                                                              k_repulse=10.0)
            attractionVector = self.attraction_vector_compute(k_attract=100.0)

            finalVector = np.sum(np.vstack(
                (repulsionVectors, attractionVector)),
                                 axis=0)
            finalUnitVector = finalVector / np.linalg.norm(finalVector)
            print "finalUnitVector: ", finalUnitVector

            # TODO: do we use the unit vector (for direction) only, or the finalVector?
            finalValue, finalAngle = cart2pol(finalUnitVector[0],
                                              finalUnitVector[1])

            error_theta = angle_diff(
                mapTheta2worldTheta(finalAngle, self.worldNorthTheta), theta)
            self.e_theta_h.append(error_theta)

            k_angular_p = 2.0 * self.maxVelocity
            k_angular_D = 0.25
            k_angular_S = 1.0
            k_angular_I = 2.5
            omega = k_angular_p * (
                error_theta + k_angular_D / k_angular_S *
                (self.e_theta_h[-1] - self.e_theta_h[-2]) +
                k_angular_S / k_angular_I * sum(self.e_theta_h))
            print "Omega: ", round(omega, 1)

            #############################
            # StateController Algorithm #
            #############################
            # if desired heading is not directly in front
            if np.abs(error_theta) > np.pi / 3:
                # turn in place
                forward_vel = self.maxVelocity
                # omega *= 0.25
            else:
                # Direct yourself to the goal
                goal_distance = np.linalg.norm(self.goal_pose_pixel -
                                               self.pose_pixel)
                print "distance_from_goal: ", goal_distance
                if goal_distance <= 4:
                    # Achieved Goal!
                    forward_vel = self.maxVelocity * 0.25  # Slow down to prepare for the next one
                    self.curr_goal += 1
                else:
                    forward_vel = self.maxVelocity

            # control the motors
            ctrl_sig_left, ctrl_sig_right = vomega2bytecodes(forward_vel,
                                                             omega,
                                                             g=1)
            _ = vrep.simxSetJointTargetVelocity(
                self.clientID, self.leftMotor, ctrl_sig_left,
                vrep.simx_opmode_oneshot_wait)  # set left wheel velocity
            _ = vrep.simxSetJointTargetVelocity(
                self.clientID, self.rightMotor, ctrl_sig_right,
                vrep.simx_opmode_oneshot_wait)  # set right wheel velocity

            self.idash.add(lambda: self.plot_maze(self.blurred_paths * 1.0, m,
                                                  n, goal_m, goal_n))
            self.idash.add(lambda: self.plot_maze(im, m, n, goal_m, goal_n))

            def plot_current_and_desired_heading():
                self.plot_unit_quiver(finalUnitVector, 'r')
                self.plot_unit_quiver(
                    pol2cart(1, worldTheta2mapTheta(theta,
                                                    self.worldNorthTheta)),
                    'k')
                plt.title("Error Theta: %f" % error_theta)

            self.idash.add(plot_current_and_desired_heading)
            self.idash.add(self.plot_theta_history)

            self.idash.plotframe()

            if self.killer.kill_now:
                self.clean_exit()

    def plot_maze(self, im, m, n, goal_m, goal_n):
        """ plots the maze, with robot pose and goal pose visualized """
        if len(im.shape) == 3:
            goal_pixel_values = np.array((1.0, 1.0, 1.0))
            robot_pixel_values = np.array(
                (255.0 / 255.0, 192 / 255.0, 203 / 255.0))
        elif len(im.shape) == 2:
            goal_pixel_values = 0.5
            robot_pixel_values = 0.5
        im[goal_m, goal_n] = goal_pixel_values
        im[m, n] = robot_pixel_values
        plt.imshow(im)

    def plot_unit_quiver(self, vector, color):
        X, Y = (0, 0)
        U, V = (vector[0], vector[1])
        plt.quiver(X,
                   Y,
                   U,
                   V,
                   angles='xy',
                   scale_units='xy',
                   scale=1,
                   color=color)
        plt.xlim([-1.1, 1.1])
        plt.ylim([-1.1, 1.1])

    def plot_theta_history(self, expansion=5):
        plt.plot([theta for theta in self.theta_history])
        if len(self.theta_history) < expansion:
            plt.xlim([0, expansion])
        else:
            plt.xlim([0, len(self.theta_history)])
        ylim = np.pi + 0.5
        plt.ylim([-ylim, ylim])

    def plot_all_goals(self, im):
        # display all goals
        for goal_idx in range(len(self.GOALS)):
            im[self.GOALS[goal_idx][0], self.GOALS[goal_idx][1]] = np.array(
                (1.0, 1.0, 1.0))

    def clean_exit(self):
        _ = vrep.simxStopSimulation(self.clientID,
                                    vrep.simx_opmode_oneshot_wait)
        vrep.simxFinish(self.clientID)
        print 'Program ended'
        sys.exit(0)

    def run(self):
        if self.clientID != -1:
            _ = vrep.simxStartSimulation(self.clientID,
                                         vrep.simx_opmode_oneshot_wait)
            self.robot_code()

        self.clean_exit()
Exemplo n.º 8
0
image_template = np.zeros((28, 28))
map_image = np.tile(image_template, map_shape)
map_image_w = map_image.copy()
lattice_predictions = np.zeros(map_size)
for node_count in range(map_size):
    m_row, n_col = idx1D_to_idx2D(node_count, map_shape)
    closest_x_idx = np.argmin(cdist(w[:, node_count].reshape(1, -1), trX))
    image_of_closest_x = trX[closest_x_idx].reshape((28, 28))
    lattice_predictions[node_count] = trY[closest_x_idx]  # prediction mapping
    image_of_w = w[:, node_count].reshape((28, 28))
    map_image[m_row * 28:(m_row + 1) * 28,
              n_col * 28:(n_col + 1) * 28] = image_of_closest_x
    map_image_w[m_row * 28:(m_row + 1) * 28,
                n_col * 28:(n_col + 1) * 28] = image_of_w

dash.add(lambda: plt.imshow(map_image, cmap='gray'))
dash.add(lambda: plt.imshow(map_image_w, cmap='gray'))


# predictions
def predict(X, w, lattice_predictions):
    n_examples = X.shape[0]
    Ypred = np.zeros(n_examples)
    for row in range(n_examples):
        closest_w_idx = np.argmin(cdist(X[row].reshape(1, -1), w.T))
        Ypred[row] = lattice_predictions[closest_w_idx]
    return Ypred


trYpred = predict(trX, w, lattice_predictions)
teYpred = predict(teX, w, lattice_predictions)
Exemplo n.º 9
0
class FinalProjectProgram():
    def __init__(self, color, number):
        # parameter init
        self.bot_name = '%s%d' % (color, number)
        self.bot_player2 = 'Blue2'
        self.bot_nameStriker = 'Red1'
        self.bot_nameGoalie = 'Red3'
        self.bot_nameBall = 'Ball'

        # run startup methods
        self.initializeVrepClient()
        self.initializeVrepApi()
        self.killer = GracefulKiller()
        self.idash = IDash(framerate=0.05)

    def initializeVrepClient(self):
        #Initialisation for Python to connect to VREP
        print 'Python program started'
        count = 0
        num_tries = 10
        while count < num_tries:
            vrep.simxFinish(-1)  # just in case, close all opened connections
            self.clientID = vrep.simxStart('127.0.0.1', 19999, True, True,
                                           5000,
                                           5)  #Timeout=5000ms, Threadcycle=5ms
            if self.clientID != -1:
                print 'Connected to V-REP'
                break
            else:
                "Trying again in a few moments..."
                time.sleep(3)
                count += 1
        if count >= num_tries:
            print 'Failed connecting to V-REP'
            vrep.simxFinish(self.clientID)

    def initializeVrepApi(self):
        # initialize bot handles and variables
        _, self.leftMotor = vrep.simxGetObjectHandle(
            self.clientID, '%s_leftJoint' % self.bot_name,
            vrep.simx_opmode_oneshot_wait)
        _, self.rightMotor = vrep.simxGetObjectHandle(
            self.clientID, '%s_rightJoint' % self.bot_name,
            vrep.simx_opmode_oneshot_wait)
        _, self.bot = vrep.simxGetObjectHandle(self.clientID, self.bot_name,
                                               vrep.simx_opmode_oneshot_wait)
        # proxSens = prox_sens_initialize(self.clientID)
        # initialize odom of bot
        _, self.xyz = vrep.simxGetObjectPosition(self.clientID, self.bot, -1,
                                                 vrep.simx_opmode_streaming)
        _, self.eulerAngles = vrep.simxGetObjectOrientation(
            self.clientID, self.bot, -1, vrep.simx_opmode_streaming)

        _, self.player2 = vrep.simxGetObjectHandle(
            self.clientID, self.bot_player2, vrep.simx_opmode_oneshot_wait)
        # proxSens = prox_sens_initialize(self.clientID)
        # initialize odom of bot
        _, self.xyzPlayer2 = vrep.simxGetObjectPosition(
            self.clientID, self.player2, -1, vrep.simx_opmode_streaming)
        _, self.eulerAnglesPlayer2 = vrep.simxGetObjectOrientation(
            self.clientID, self.player2, -1, vrep.simx_opmode_streaming)

        # # initialize overhead cam
        # _, self.overheadCam=vrep.simxGetObjectHandle(
        #     self.clientID, 'Global_Vision', vrep.simx_opmode_oneshot_wait)
        # _, self.resolution, self.image = vrep.simxGetVisionSensorImage(
        #     self.clientID,self.overheadCam,0,vrep.simx_opmode_oneshot_wait)

        # # initialize goal handle + odom
        # _, self.goalHandle=vrep.simxGetObjectHandle(
        #     self.clientID, 'Goal_Position', vrep.simx_opmode_oneshot_wait)
        # _, self.goalPose = vrep.simxGetObjectPosition(
        #     self.clientID, self.goalHandle, -1, vrep.simx_opmode_streaming)

        # initialize bot handles and variables for Red1
        _, self.leftMotorStriker = vrep.simxGetObjectHandle(
            self.clientID, '%s_leftJoint' % self.bot_name,
            vrep.simx_opmode_oneshot_wait)
        _, self.rightMotorStriker = vrep.simxGetObjectHandle(
            self.clientID, '%s_rightJoint' % self.bot_name,
            vrep.simx_opmode_oneshot_wait)
        _, self.botStriker = vrep.simxGetObjectHandle(
            self.clientID, self.bot_nameStriker, vrep.simx_opmode_oneshot_wait)
        _, xyzStriker = vrep.simxGetObjectPosition(self.clientID,
                                                   self.botStriker, -1,
                                                   vrep.simx_opmode_streaming)
        _, eulerAnglesStriker = vrep.simxGetObjectOrientation(
            self.clientID, self.botStriker, -1, vrep.simx_opmode_streaming)

        _, self.leftMotorGoalie = vrep.simxGetObjectHandle(
            self.clientID, '%s_leftJoint' % self.bot_name,
            vrep.simx_opmode_oneshot_wait)
        _, self.rightMotorGoalie = vrep.simxGetObjectHandle(
            self.clientID, '%s_rightJoint' % self.bot_name,
            vrep.simx_opmode_oneshot_wait)
        _, self.botGoalie = vrep.simxGetObjectHandle(
            self.clientID, self.bot_nameGoalie, vrep.simx_opmode_oneshot_wait)
        _, xyzGoalie = vrep.simxGetObjectPosition(self.clientID,
                                                  self.botGoalie, -1,
                                                  vrep.simx_opmode_streaming)
        _, eulerAnglesGoalie = vrep.simxGetObjectOrientation(
            self.clientID, self.botGoalie, -1, vrep.simx_opmode_streaming)

        _, self.ball = vrep.simxGetObjectHandle(self.clientID,
                                                self.bot_nameBall,
                                                vrep.simx_opmode_oneshot_wait)
        _, xyzBall = vrep.simxGetObjectPosition(self.clientID, self.ball, -1,
                                                vrep.simx_opmode_streaming)
        _, eulerAnglesBall = vrep.simxGetObjectOrientation(
            self.clientID, self.ball, -1, vrep.simx_opmode_streaming)

    def robotCode(self):
        """ OUR ROBOT CODE GOES HERE """
        # Shooting Test

        vrep.simxSetObjectPosition(self.clientID, self.ball, -1, [0, -0.35],
                                   vrep.simx_opmode_streaming)

        target = self.aim()

        vrep.simxSetObjectPosition(self.clientID, self.player2, -1, target,
                                   vrep.simx_opmode_streaming)

        position = []
        position = self.kickingPose(target)
        kickingPosition = [position[0], position[1], z]
        vrep.simxSetObjectPosition(self.clientID, self.bot, -1,
                                   kickingPosition, vrep.simx_opmode_streaming)
        vrep.simxSetObjectOrientation(
            self.clientID, self.bot, -1,
            [self.eulerAngles[0], self.eulerAngles[1], position[2]],
            vrep.simx_opmode_streaming)

        time.sleep(2)

        # Testing kicking Positions


#        vrep.simxSetObjectPosition(
#            self.clientID, self.ball,-1,[0, 0], vrep.simx_opmode_streaming)
#
#        maximum = 36
#        for i in range (0,maximum):
#            print i
#            radius = 0.3
#            angle = 2*math.pi/maximum*i
#            target = [radius*math.sin(angle), radius*math.cos(angle)]
#
#            vrep.simxSetObjectPosition(
#            self.clientID, self.player2, -1, target, vrep.simx_opmode_streaming)
#
#            position = []
#            position = self.kickingPose(target)
#            kickingPosition = [position[0], position[1], z]
#            vrep.simxSetObjectPosition(
#                self.clientID, self.bot, -1, kickingPosition, vrep.simx_opmode_streaming)
#            vrep.simxSetObjectOrientation(
#                self.clientID, self.bot, -1, [self.eulerAngles[0], self.eulerAngles[1], position[2]], vrep.simx_opmode_streaming)
#
#            time.sleep(1)

    def getRobotConf(self, robot_handle):
        _, xyz = vrep.simxGetObjectPosition(self.clientID, robot_handle, -1,
                                            vrep.simx_opmode_buffer)
        _, eulerAngles = vrep.simxGetObjectOrientation(self.clientID,
                                                       robot_handle, -1,
                                                       vrep.simx_opmode_buffer)
        x, y, z = xyz
        theta = eulerAngles[2]

        return (x, y, theta)

    def getRobotPose(self, robot_handle):
        _, xyz = vrep.simxGetObjectPosition(self.clientID, robot_handle, -1,
                                            vrep.simx_opmode_buffer)
        _, eulerAngles = vrep.simxGetObjectOrientation(self.clientID,
                                                       robot_handle, -1,
                                                       vrep.simx_opmode_buffer)
        x, y, z = xyz
        theta = eulerAngles[2]

        return (x, y, theta)

    def setMotorVelocities(self, forward_vel, omega):
        ctrl_sig_left, ctrl_sig_right = vomega2bytecodes(forward_vel,
                                                         omega,
                                                         g=1)
        _ = vrep.simxSetJointTargetVelocity(
            self.clientID, self.leftMotor, ctrl_sig_left,
            vrep.simx_opmode_oneshot_wait)  # set left wheel velocity
        _ = vrep.simxSetJointTargetVelocity(
            self.clientID, self.rightMotor, ctrl_sig_right,
            vrep.simx_opmode_oneshot_wait)  # set right wheel velocity

    def getBallPose(self):
        _, xyz = vrep.simxGetObjectPosition(self.clientID, self.ball, -1,
                                            vrep.simx_opmode_streaming)
        x, y, z = xyz
        return (x, y)

    def aim(self, goaliePosition):
        # assuming: robot is already in perfect position
        # assuming: ball has velocity of zero
        ballRadius = 0.05
        leftGoalPost = [0.2, -0.75]  # position of left goal post
        rightGoalPost = [-0.2, 0.75]  # position of right goal post
        tolerance = 0.01

        #        if goalieOrientation <= .. :
        #            goalieHeading =  # different variable name
        #        else:
        #            goalieHeading = # different variable name
        #
        #        predicGoaliePos = goaliePosition + goalieHeading

        gapRight = abs(goaliePosition[0] - rightGoalPost[0])
        gapLeft = abs(goaliePosition[0] - leftGoalPost[0])

        if gapRight >= gapLeft:
            xAim = rightGoalPost[0] + ballRadius + tolerance
        else:
            xAim = leftGoalPost[0] - ballRadius - tolerance

        return [xAim, goaliePosition[1], z]

    def kickingPose(self, target):
        # CHANGE TO ACTUAL POSITION IF NOT TESTING
        ballPositionX, ballPositionY = (0, -0.35)  #self.getBallPose()
        distance2ball = 0.08
        deltaX = target[0] - ballPositionX
        deltaY = target[1] - ballPositionY
        if deltaY != 0:
            gamma = math.atan(deltaX / deltaY)
        else:
            gamma = math.pi / 2

        # transform gamma into theta orientation
        # CHECK FOR POINTS IN BETWEEN
        if ballPositionX <= target[0]:
            if ballPositionY <= target[1]:
                print '1'
                theta = -gamma
                xOffset = -distance2ball * math.sin(gamma)
                yOffset = -distance2ball * math.cos(gamma)
            else:
                # CHECK THIS ONE!
                print '2'
                theta = -gamma + math.pi
                xOffset = distance2ball * math.sin(gamma)
                yOffset = distance2ball * math.cos(gamma)
        else:
            if ballPositionY <= target[1]:
                print '3'
                theta = -gamma
                xOffset = -distance2ball * math.sin(gamma)
                yOffset = -distance2ball * math.cos(gamma)
                print xOffset
                print yOffset
            else:
                print '4'
                print gamma
                theta = -gamma + math.pi
                xOffset = distance2ball * math.sin(gamma)
                yOffset = distance2ball * math.cos(gamma)
        xPosition = ballPositionX + xOffset
        yPosition = ballPositionY + yOffset
        pose = [xPosition, yPosition, theta]
        return pose

    def kick(self, speed):
        self.setMotorVelocities(speed, speed)
        time.sleep(0.5)
        self.setMotorVelocities(0, 0)

    def unittestMoveForward(self):
        self.setMotorVelocities(forward_vel=1, omega=0)

    def unittestTurnSideways(self, not_first_time):
        x, y, theta = self.getRobotPose()
        if not_first_time:
            goal_theta = self.first_theta + np.pi / 2
            print goal_theta
            error_theta = ThetaRange.angleDiff(theta, goal_theta)

            # control
            omega = 10 * error_theta
            print omega
            self.setMotorVelocities(0, omega)

            # visualization
            def plotCurrentDesiredHeadings():
                plotVector(ThetaRange.pol2cart(1, theta), 'k')
                plotVector(ThetaRange.pol2cart(1, goal_theta), 'r')

            self.idash.add(plotCurrentDesiredHeadings)
            self.idash.plotframe()
        else:
            self.first_theta = theta

    def clean_exit(self):
        _ = vrep.simxStopSimulation(self.clientID,
                                    vrep.simx_opmode_oneshot_wait)
        vrep.simxFinish(self.clientID)
        print 'Program ended'

    def run(self):
        if self.clientID != -1:
            _ = vrep.simxStartSimulation(self.clientID,
                                         vrep.simx_opmode_oneshot_wait)
            self.robotCode()

        self.clean_exit()
class MyRobotRunner(base_robot.BaseRobotRunner):
    def __init__(self, *args, **kwargs):
        super(MyRobotRunner, self).__init__(*args, **kwargs)
        self.idash = IDash(0.005)

    def robotCode(self):
        #waypoint = [(0.2, 0.4), (0, 0.4), (-0.2, 0.4),(-0.2, 0.2), (-0.2, 0),(0, 0), (0.2, 0),(0.2, -0.2), (0.2, -0.4),(0, -0.4), (-0.2, -0.4)]
        waypoint = [(0, 0.4), (-0.2, 0.4), (-0.2, 0.2), (-0.2, 0), (0, 0),
                    (0.2, 0), (0.2, -0.2), (0.2, -0.4), (0, -0.4),
                    (-0.05, -0.4), (-0.1, -0.4), (-0.15, -0.4), (-0.2, -0.4),
                    (-0.2, -0.4)]
        # waypoint = []
        # r = 0.2
        # increments = 5
        # xCenter = 0
        # yCenter = 0.2
        # for i in range(increments-1):
        #     # circle center: (0, 0.2)
        #     x = r*math.sin(i*math.pi/increments) + xCenter
        #     y = r*math.cos(i*math.pi/increments) + yCenter
        #     waypoint.append((x,y))
        # xCenter = 0
        # yCenter = -0.2
        # for i in range(increments):
        #     # circle center: (0, -0.2)
        #     x = -r*math.sin(i*math.pi/increments) + xCenter
        #     y = r*math.cos(i*math.pi/increments) + yCenter
        #     waypoint.append((x,y))
        # for i in range(3):
        #     waypoint.append((-0.2/4*i,-0.4))

        #k=0.0345    # ball model: d(t) = vmot*k*(1-exp(-t/T))

        for i in range(len(waypoint)):
            p0 = self.ballEngine.getBallPose()
            curr_bot_pose = list(self.getRobotConf(self.bot))
            curr_bot_pose[1] -= 0.05
            self.path, status = passPath(curr_bot_pose,
                                         p0,
                                         waypoint[i],
                                         vmax=10,
                                         vr=7,
                                         kq=0.0035)

            print self.path

            #            dash = IDash(framerate=0.1)
            #            dash.add(lambda: plt.plot(-self.path[1,:], self.path[0,:], 'b-*') and
            #                plt.xlim([-0.7, 0.7]) and plt.ylim([-0.7, 0.7]))
            #            dash.plotframe()
            print 'estimated time path'
            # print calculatePathTime(self.path)
            t = self.ballEngine.getSimTime()
            t = self.ballEngine.getSimTime()  # time in seconds
            while (self.ballEngine.getSimTime() -
                   t) < 10:  #End program after 30sec
                #        cc=1
                #        while cc:
                def vizBots():
                    actx, acty = self.getRobotConf()[:2]
                    ballx, bally = self.ballEngine.getBallPose()[:2]
                    plt.hold('on')
                    plt.plot(-waypoint[i][1], waypoint[i][0], 'k*')
                    plt.plot(-bally, ballx, 'b.')
                    plt.plot(-acty, actx, 'g+')
                    plt.plot(-self.path[1, :], self.path[0, :], 'r.')
                    plt.xlim([-0.9, 0.9])  # y axis in the field
                    plt.ylim([-0.65, 0.65])  # x axis in the field
                    plt.xlabel('active path length: {}'.format(
                        self.path.shape[1]))

                self.idash.add(vizBots)

                robotConf = self.getRobotConf(self.bot)
                cc = self.followPath(robotConf, rb=0.05)
                self.ballEngine.update()

                p1 = self.ballEngine.getBallPose()
                p3 = self.ballEngine.getNextRestPos()
                dist_from_start = np.sqrt((p1[0] - p0[0])**2 +
                                          (p1[1] - p0[1])**2)
                velocity_measure = np.sqrt((p1[0] - p3[0])**2 +
                                           (p1[1] - p3[1])**2)
                #self.idash.plotframe()
                if dist_from_start > 0.01:  # the ball has been touched
                    if velocity_measure < 0.003:
                        break  # wait til velocity reaches zero

    #            robotConf = self.getRobotConf(self.bot)
    #            ballPos = self.ballEngine.getBallPose() # (x, y)
    #            vRobot = v2Pos(robotConf, ballPos)

        self.setMotorVelocities(0, 0)
        print 'real time path'
        print(self.ballEngine.getSimTime() - t)
class FinalProjectProgram():

    def __init__(self, color, number):
        # parameter init
        self.bot_name = '%s%d' % (color, number)
        self.bot_player2 = 'Blue2'
        self.bot_nameStriker = 'Red1'
        self.bot_nameGoalie = 'Red3'
        self.bot_nameBall = 'Ball'

        # run startup methods
        self.initializeVrepClient()
        self.initializeVrepApi()
        self.killer = GracefulKiller()
        self.idash = IDash(framerate=0.05)

    def initializeVrepClient(self):
        #Initialisation for Python to connect to VREP
        print 'Python program started'
        count = 0
        num_tries = 10
        while count < num_tries:
            vrep.simxFinish(-1) # just in case, close all opened connections
            self.clientID=vrep.simxStart('127.0.0.1',19999,True,True,5000,5) #Timeout=5000ms, Threadcycle=5ms
            if self.clientID!=-1:
                print 'Connected to V-REP'
                break
            else:
                "Trying again in a few moments..."
                time.sleep(3)
                count += 1
        if count >= num_tries:
            print 'Failed connecting to V-REP'
            vrep.simxFinish(self.clientID)

    def initializeVrepApi(self):
        # initialize bot handles and variables
        _, self.leftMotor=vrep.simxGetObjectHandle(
            self.clientID, '%s_leftJoint' % self.bot_name, vrep.simx_opmode_oneshot_wait)
        _, self.rightMotor=vrep.simxGetObjectHandle(
            self.clientID, '%s_rightJoint' % self.bot_name, vrep.simx_opmode_oneshot_wait)
        _, self.bot=vrep.simxGetObjectHandle(
            self.clientID, self.bot_name, vrep.simx_opmode_oneshot_wait)
        # proxSens = prox_sens_initialize(self.clientID)
        # initialize odom of bot
        _, self.xyz = vrep.simxGetObjectPosition(
            self.clientID, self.bot, -1, vrep.simx_opmode_streaming)
        _, self.eulerAngles = vrep.simxGetObjectOrientation(
            self.clientID, self.bot, -1, vrep.simx_opmode_streaming)
            
        _, self.player2=vrep.simxGetObjectHandle(
            self.clientID, self.bot_player2, vrep.simx_opmode_oneshot_wait)
        # proxSens = prox_sens_initialize(self.clientID)
        # initialize odom of bot
        _, self.xyzPlayer2 = vrep.simxGetObjectPosition(
            self.clientID, self.player2, -1, vrep.simx_opmode_streaming)
        _, self.eulerAnglesPlayer2 = vrep.simxGetObjectOrientation(
            self.clientID, self.player2, -1, vrep.simx_opmode_streaming)

        # # initialize overhead cam
        # _, self.overheadCam=vrep.simxGetObjectHandle(
        #     self.clientID, 'Global_Vision', vrep.simx_opmode_oneshot_wait)
        # _, self.resolution, self.image = vrep.simxGetVisionSensorImage(
        #     self.clientID,self.overheadCam,0,vrep.simx_opmode_oneshot_wait)

        # # initialize goal handle + odom
        # _, self.goalHandle=vrep.simxGetObjectHandle(
        #     self.clientID, 'Goal_Position', vrep.simx_opmode_oneshot_wait)
        # _, self.goalPose = vrep.simxGetObjectPosition(
        #     self.clientID, self.goalHandle, -1, vrep.simx_opmode_streaming)

        # initialize bot handles and variables for Red1
        _, self.leftMotorStriker=vrep.simxGetObjectHandle(
            self.clientID, '%s_leftJoint' % self.bot_name, vrep.simx_opmode_oneshot_wait)
        _, self.rightMotorStriker=vrep.simxGetObjectHandle(
            self.clientID, '%s_rightJoint' % self.bot_name, vrep.simx_opmode_oneshot_wait)
        _, self.botStriker = vrep.simxGetObjectHandle(
            self.clientID, self.bot_nameStriker, vrep.simx_opmode_oneshot_wait)
        _, xyzStriker = vrep.simxGetObjectPosition(
            self.clientID, self.botStriker, -1, vrep.simx_opmode_streaming)
        _, eulerAnglesStriker = vrep.simxGetObjectOrientation(
            self.clientID, self.botStriker, -1, vrep.simx_opmode_streaming)
            
        _, self.leftMotorGoalie =vrep.simxGetObjectHandle(
            self.clientID, '%s_leftJoint' % self.bot_name, vrep.simx_opmode_oneshot_wait)
        _, self.rightMotorGoalie =vrep.simxGetObjectHandle(
            self.clientID, '%s_rightJoint' % self.bot_name, vrep.simx_opmode_oneshot_wait)
        _, self.botGoalie = vrep.simxGetObjectHandle(
            self.clientID, self.bot_nameGoalie, vrep.simx_opmode_oneshot_wait)
        _, xyzGoalie = vrep.simxGetObjectPosition(
            self.clientID, self.botGoalie, -1, vrep.simx_opmode_streaming)
        _, eulerAnglesGoalie = vrep.simxGetObjectOrientation(
            self.clientID, self.botGoalie, -1, vrep.simx_opmode_streaming)
            
        _, self.ball = vrep.simxGetObjectHandle(
            self.clientID, self.bot_nameBall, vrep.simx_opmode_oneshot_wait)
        _, xyzBall = vrep.simxGetObjectPosition(
            self.clientID, self.ball, -1, vrep.simx_opmode_streaming)
        _, eulerAnglesBall = vrep.simxGetObjectOrientation(
            self.clientID, self.ball, -1, vrep.simx_opmode_streaming)

    def robotCode(self):
        """ OUR ROBOT CODE GOES HERE """
        # Shooting Test
        
        vrep.simxSetObjectPosition(
            self.clientID, self.ball,-1,[0, -0.35], vrep.simx_opmode_streaming)  
            
        target = self.aim()
        
        vrep.simxSetObjectPosition(
        self.clientID, self.player2, -1, target, vrep.simx_opmode_streaming)
        
        position = []
        position = self.kickingPose(target)
        kickingPosition = [position[0], position[1], z]
        vrep.simxSetObjectPosition(
            self.clientID, self.bot, -1, kickingPosition, vrep.simx_opmode_streaming)
        vrep.simxSetObjectOrientation(
            self.clientID, self.bot, -1, [self.eulerAngles[0], self.eulerAngles[1], position[2]], vrep.simx_opmode_streaming)
        
        time.sleep(2)
        
        # Testing kicking Positions
#        vrep.simxSetObjectPosition(
#            self.clientID, self.ball,-1,[0, 0], vrep.simx_opmode_streaming)
#        
#        maximum = 36
#        for i in range (0,maximum):
#            print i
#            radius = 0.3
#            angle = 2*math.pi/maximum*i
#            target = [radius*math.sin(angle), radius*math.cos(angle)]
#            
#            vrep.simxSetObjectPosition(
#            self.clientID, self.player2, -1, target, vrep.simx_opmode_streaming)
#            
#            position = []
#            position = self.kickingPose(target)
#            kickingPosition = [position[0], position[1], z]
#            vrep.simxSetObjectPosition(
#                self.clientID, self.bot, -1, kickingPosition, vrep.simx_opmode_streaming)
#            vrep.simxSetObjectOrientation(
#                self.clientID, self.bot, -1, [self.eulerAngles[0], self.eulerAngles[1], position[2]], vrep.simx_opmode_streaming)
#            
#            time.sleep(1)
        
        
    def getRobotConf(self, robot_handle):
        _, xyz = vrep.simxGetObjectPosition(
            self.clientID, robot_handle, -1, vrep.simx_opmode_buffer)
        _, eulerAngles = vrep.simxGetObjectOrientation(
            self.clientID, robot_handle, -1, vrep.simx_opmode_buffer)
        x, y, z = xyz
        theta = eulerAngles[2]

        return (x, y, theta)

    def getRobotPose(self, robot_handle):
        _, xyz = vrep.simxGetObjectPosition(
            self.clientID, robot_handle, -1, vrep.simx_opmode_buffer)
        _, eulerAngles = vrep.simxGetObjectOrientation(
            self.clientID, robot_handle, -1, vrep.simx_opmode_buffer)
        x, y, z = xyz
        theta = eulerAngles[2]

        return (x, y, theta)

    def setMotorVelocities(self, forward_vel, omega):
        ctrl_sig_left, ctrl_sig_right = vomega2bytecodes(forward_vel, omega, g=1)
        _ = vrep.simxSetJointTargetVelocity(
            self.clientID,self.leftMotor,ctrl_sig_left,vrep.simx_opmode_oneshot_wait) # set left wheel velocity
        _ = vrep.simxSetJointTargetVelocity(
            self.clientID,self.rightMotor,ctrl_sig_right,vrep.simx_opmode_oneshot_wait) # set right wheel velocity
            
    def getBallPose(self):
        _, xyz = vrep.simxGetObjectPosition(
            self.clientID, self.ball, -1, vrep.simx_opmode_streaming)
        x, y, z = xyz
        return (x, y)
        
    def aim(self, goaliePosition):
        # assuming: robot is already in perfect position
        # assuming: ball has velocity of zero
        ballRadius = 0.05
        leftGoalPost = [0.2, -0.75] # position of left goal post
        rightGoalPost = [-0.2, 0.75]# position of right goal post       
        tolerance = 0.01
        
#        if goalieOrientation <= .. :
#            goalieHeading =  # different variable name
#        else:
#            goalieHeading = # different variable name
#        
#        predicGoaliePos = goaliePosition + goalieHeading
        
        gapRight = abs(goaliePosition[0] - rightGoalPost[0])
        gapLeft = abs(goaliePosition[0] - leftGoalPost[0])
        
        if gapRight >= gapLeft:
            xAim = rightGoalPost[0] + ballRadius + tolerance
        else:
            xAim = leftGoalPost[0] - ballRadius - tolerance
        
        return [xAim, goaliePosition[1], z]
        
    def kickingPose(self, target):
        # CHANGE TO ACTUAL POSITION IF NOT TESTING
        ballPositionX, ballPositionY = (0,-0.35)#self.getBallPose()
        distance2ball = 0.08
        deltaX = target[0] - ballPositionX
        deltaY = target[1] - ballPositionY
        if deltaY != 0:
            gamma = math.atan(deltaX/deltaY)
        else: 
            gamma = math.pi/2
        
        # transform gamma into theta orientation
        # CHECK FOR POINTS IN BETWEEN
        if ballPositionX <= target[0]:
            if ballPositionY <= target[1]:
                print '1'
                theta = - gamma
                xOffset = - distance2ball*math.sin(gamma)
                yOffset = - distance2ball*math.cos(gamma)
            else:
                # CHECK THIS ONE!
                print '2'
                theta = - gamma + math.pi
                xOffset = distance2ball*math.sin(gamma)
                yOffset = distance2ball*math.cos(gamma)
        else:
            if ballPositionY <= target[1]:
                print '3'
                theta = - gamma
                xOffset = - distance2ball*math.sin(gamma)
                yOffset = - distance2ball*math.cos(gamma)
                print xOffset
                print yOffset
            else:
                print '4'
                print gamma
                theta = - gamma + math.pi
                xOffset = distance2ball*math.sin(gamma)
                yOffset = distance2ball*math.cos(gamma)
        xPosition = ballPositionX + xOffset
        yPosition = ballPositionY + yOffset
        pose = [xPosition, yPosition, theta]
        return pose
        
    def kick(self, speed):
        self.setMotorVelocities(speed, speed)
        time.sleep(0.5)
        self.setMotorVelocities(0, 0)

    def unittestMoveForward(self):
        self.setMotorVelocities(forward_vel=1, omega=0)

    def unittestTurnSideways(self, not_first_time):
        x, y, theta = self.getRobotPose()
        if not_first_time:
            goal_theta = self.first_theta + np.pi / 2
            print goal_theta
            error_theta = ThetaRange.angleDiff(theta, goal_theta)

            # control
            omega = 10 * error_theta
            print omega
            self.setMotorVelocities(0, omega)

            # visualization
            def plotCurrentDesiredHeadings():
                plotVector(ThetaRange.pol2cart(1, theta), 'k')
                plotVector(ThetaRange.pol2cart(1, goal_theta), 'r')
            self.idash.add(plotCurrentDesiredHeadings)
            self.idash.plotframe()
        else:
            self.first_theta = theta

    def clean_exit(self):
        _ = vrep.simxStopSimulation(self.clientID,vrep.simx_opmode_oneshot_wait)
        vrep.simxFinish(self.clientID)
        print 'Program ended'

    def run(self):
        if self.clientID!=-1:
            _ = vrep.simxStartSimulation(self.clientID,vrep.simx_opmode_oneshot_wait)
            self.robotCode()

        self.clean_exit()
class Master(base_robot.MultiRobotCyclicExecutor):
    def __init__(self, *args, **kwargs):
        super(Master, self).__init__(*args, **kwargs)
        self.color = None
        self.idash = IDash(framerate=0.005)

    def getClosestZone(self, pose):
        """ Zones will be from downfield (0) to close to own goal (1) """
        pose = np.array(pose[:2]) # x-y only
        assert pose.size == 2 # pose should be 2-D

        # 2 ZONES: 0 and 1
        two_zone_boundary_y = 0.01
        zone = pose[1] > two_zone_boundary_y
        if self.color == 'Red':
            zone = not zone
        return zone

    def run(self):
        if self.clientID!=-1:
            _ = vrep.simxStartSimulation(self.clientID,vrep.simx_opmode_oneshot_wait)

            # robots have been added
            self.color = self.bots[0].color
            self.originalRoles = ['attacker', 'midfielder', 'goalie']
            self.roles = ['attacker', 'midfielder', 'goalie']
            goalie = self.bots[2]

            activeidx = 0 # striker starts first

            t0 = time.time()
            while time.time() - t0 < 600:
                self.ballEngine.update()

                assert activeidx != 2 # now only have active 0,1
                activebot = self.bots[activeidx]

                offense = self.originalRoles[activeidx] == 'attacker'
                if offense:
                    if self.roles[1] != 'dumb':
                        self.roles[1] = 'attacker'
                else:
                    if self.roles[1] != 'dumb':
                        self.roles[1] = 'midfielder'

                secondaryidx = not activeidx

                ballx, bally = self.ballEngine.getBallPose()
                robotX = np.zeros(2)
                robotY = np.zeros(2)
                robotX[0], robotY[0], _ = self.bots[0].getRobotConf()
                robotX[1], robotY[1], _ = self.bots[1].getRobotConf()
                distance2ball = 9999
                for i in xrange(len(self.bots[:-1])):
                    dist = np.sqrt((ballx - robotX[i])**2 + (bally- robotY[i])**2)
                    if dist < distance2ball:
                        distance2ball = dist
                        closestRobot = i
                ballPosX, ballPosY = self.ballEngine.getBallPose()
                for idx in range(len(self.bots[:-1])):
                    if offense:
                        oppGoalieConf, oppGoalieIdx = self.findOppGoalieConf(return_index=True)
                        oppGoalieIdx = self.convertOppBotsIdx2BotsIdx(oppGoalieIdx)
                        if idx == closestRobot:
                            self.bots[idx].robotCode(
                                role=self.roles[idx],
                                obstacleConfs=self.getObstacleConfs([idx, oppGoalieIdx]), # dont ignore yourself, or the oppGoalie
                                goaliePosition = self.findOppGoalieConf())
                        else: # robot is not closest robot
                            if True: # be a bitch
                                passivePos = oppGoalieConf
                            else:
                                if ballPosX >= 0:
                                    passivePos = [-0.2, ballPosY, 0]
                                else: # ballPosX < 0
                                    passivePos = [0.2, ballPosY, 0]
                            self.bots[idx].secondaryCode(
                                    role=self.roles[idx],
                                    obstacleConfs=self.getObstacleConfs([idx]), # attack the goalie
                                    passivePos=passivePos)
                    # defense
                    else:
                        # secondary defender (original attacker being passive)
                        if idx == secondaryidx:
                            if self.bots[idx].color == 'Red':
                                passivePos = [ballPosX, 0.2, 0]
                            else:
                                passivePos = [ballPosX, -0.2, 0]
                            self.bots[idx].secondaryCode(
                                role=self.roles[idx],
                                obstacleConfs=self.getObstacleConfs(secondaryidx),
                                passivePos=passivePos)
                        # primary defender (original defender being active)
                        else:
                            self.bots[idx].robotCode(
                                role=self.roles[idx],
                                obstacleConfs=self.getObstacleConfs(activeidx),
                                goaliePosition = self.findOppGoalieConf())

                goalie.robotCode(
                    role=self.roles[2],
                    obstacleConfs=self.getObstacleConfs(activeidx),
                    goaliePosition = self.findOppGoalieConf())

                def vizBots():
                    actx, acty = activebot.getRobotConf()[:2]
                    ballx, bally = self.ballEngine.getBallPose()
                    plt.hold('on')
                    try:
                        plt.plot(-self.activebot.passPos[1], self.activebot.passPos, 'ro')
                    except:
                        pass
                    plt.plot(-acty, actx, 'g+')
                    try:
                        plt.plot(-activebot.target[1], activebot.target[0], 'm*')
                    except:
                        pass
                    plt.plot(-oppGoalieConf[1], oppGoalieConf[0], 'yo')
                    plt.plot(-bally,ballx, 'ro')
                    plt.plot(-self.bots[secondaryidx].path[1,:], self.bots[secondaryidx].path[0,:], 'b.')
                    plt.plot(-activebot.path[1,:], activebot.path[0,:], 'g.')
                    plt.xlim([-0.8, 0.8]) # y axis in the field
                    plt.ylim([-0.7, 0.7]) # x axis in the field
                    plt.title('Red = RCV, Green = Active')
                    plt.xlabel('active path length: {}'.format(activebot.path.shape[1]))
                self.idash.add(vizBots)

                # if time.time() - activebot.time_started_2b_dumb > 2:
                #     self.roles = self.originalRoles[:]

                # if activebot.path.shape[1] < 5:
                #     self.roles[activeidx] = 'dumb'
                #     activebot.time_started_2b_dumb = time.time()

                p0 = activebot.p0
                p1 = self.ballEngine.getBallPose()
                p3 = self.ballEngine.getNextRestPos()
                dist_from_start = np.sqrt((p1[0] - p0[0])**2 + (p1[1] - p0[1])**2)
                velocity_measure = np.sqrt((p1[0] - p3[0])**2 + (p1[1] - p3[1])**2)
                closest_zone = self.getClosestZone(p1)
                if dist_from_start > 0.01: # the ball has been touched
                #     if velocity_measure < 0.003: # wait til velocity reaches zero
                # if True:
                    # if velocity_measure < 1.0: # start kicking while ball is moving...
                    for bot in self.bots:
                        bot.executing = False
                    if closest_zone != activeidx: # success
                        # increment what is the new active zone
                        # self.setMotorVelocities(0, 0)
                        activeidx = not activeidx
class ZonePasserMasterCyclic(base_robot.MultiRobotCyclicExecutor):
    """After doing part A of Question 2, the ball will already be
    placed in Zone 4; so let's transport it there now"""


    def __init__(self, *args, **kwargs):
        super(ZonePasserMasterCyclic, self).__init__(*args, **kwargs)
        self.idash = IDash(0.005)
        self.activezones = []
        self.receivingzones = []
        self.activeplayer = None
        self.receivingplayer = None
        self.zones = {( 1,  1): 1,
                      ( 1, -1): 2,
                      (-1,  1): 3,
                      (-1, -1): 4}
        self.zonesigns = [( 1,  1), ( 1, -1), (-1,  1), (-1, -1)]

        self.zone_centers = np.array([[CENTER_X, CENTER_X, -CENTER_X, -CENTER_X],
                                      [CENTER_Y, -CENTER_Y, CENTER_Y, -CENTER_Y]])

        self.zone_corners = np.array([[CORNER_X, CORNER_X, -CORNER_X, -CORNER_X],
                                      [CORNER_Y, -CORNER_Y, CORNER_Y, -CORNER_Y]])

        # TODO: remove me since we're not allowed to set the ball pose
        # start ball at zone 4 - 1 (0 indexed)
        ball_start = self.zone_centers[:,3]
        ball_start[1] -= 0.05 # move closer to players center, but further distance q from player
        # self.ballEngine.setBallPose(ball_start)
        self.ballEngine.update()

        # FIXME: Hardcoded is good for drill, not good for game!
        self.zone_pass_plan = [4, 2, 1, 3, 4, 2] # the locations between the 5 passes
        self.activebot_idx_plan = [0, 1, 2, 0, 1, 2] # which robot should take the active zone during the zone pass plan?

    def getClosestZone(self, pose):
        """ get zone which the current pose is closest to. This pose could
        be a ball, an opposing player, etc. """
        pose = np.array(pose[:2]) # x-y only
        assert pose.size == 2 # pose should be 2-D
        dist_from_zones = cdist(np.expand_dims(pose, axis=0), self.zone_corners.T)
        return np.argmin(dist_from_zones) + 1 # zones are 1-indexed

    def getBotInZone(self, zone):
        """
        Not using now
        Returns the index of the bot which is inside the zone, otherwise None
        if there is no bot inside this zone"""
        bot_zones = np.zeros(len(self.bots))
        for bot_idx, bot in enumerate(self.bots):
            bot_zones[bot_idx] = self.getClosestZone(bot.getRobotConf(bot.bot))
        # we use [0][0] since return of np.where looks like (array([2]),)
        zone_as_array = np.where(bot_zones == zone)[0]
        if zone_as_array.size == 0:
            return None
        else:
            return zone_as_array[0] # the first, if there are multiple in that zone

    def getNearestBotToZone(self, zone, bot_to_ignore=None):
        """ Returns the index of the bot closest to the zone, even if the bot
        may not be directly inside the zone """
        bot_poses = np.vstack([bot.getRobotConf() for bot in self.bots])
        bot_poses = bot_poses[:, :2]
        assert bot_poses.shape == (len(self.bots), 2)
        zone_pose = self.zone_corners[:, zone - 1].reshape(1, 2)
        dist_from_bots = cdist(zone_pose, bot_poses, 'cityblock')
        if bot_to_ignore is None:
            return np.argmin(dist_from_bots)
        else: # the closest bot might be active, in which case we should return next closest
            sortedargs = np.argsort(dist_from_bots.flatten())
            if sortedargs[0] == bot_to_ignore: # first closest is active, not valid to be receive
                return sortedargs[1] # return second closest
            else:
                return sortedargs[0]

    def planToMoveIntoReceivingPosition(self, idx, rcvzone=None, startSmoothPathConf=None, vel=15, r=0.01):
        """ Move into the corner, facing center
        Parameters
        ----------
        idx: integer
            index of the robot (in self.bots)

        startSmoothPathConf: array-like, shape (3,) or None
            the start configuration of the robot to calculate the smooth path.
            Pass this if you are calculating a path from a future point.
            If None, then use current robot configuration.
        """
        if startSmoothPathConf is None:
            startSmoothPathConf = self.bots[idx].getRobotConf()
        if rcvzone is None:
            rcvzone = self.zone_pass_plan[idx]
        final_x = self.bots[idx].zone_corners[0, rcvzone - 1]
        final_y = self.bots[idx].zone_corners[1, rcvzone - 1]
        # final theta will be facing towards center field
        _ , final_theta = ThetaRange.cart2pol(final_x, final_y)
        final_theta = ThetaRange.normalize_angle(final_theta + np.pi) # flip it towards center
        smooth_path, status = smoothPath(
            startSmoothPathConf,             # robotConf
            [final_x, final_y, final_theta], # finalConf
            r=r
        )
        v = vel*np.ones((1, np.size(smooth_path,1)))
        smooth_path = np.concatenate((smooth_path, v), axis=0)
        if self.bots[idx].path is None:
            self.bots[idx].add_to_path(smooth_path)
        elif self.bots[idx].path.shape[1] == 1:
            self.bots[idx].add_to_path(smooth_path)
        else:
            self.bots[idx].path = smooth_path

    def calculateReceivingDestination(self, xy1, xy2, k=0.2):
        """ receiving BallPos should be between rcvbot and next passing zone
        Parameters
        ----------
        xy1: array-like, shape (2,)
            x-y position of receiving bot
        xy2: array-like, shape (2,)
            x-y position of next receiving zone
        k: float, between 0-1
            multiplier to determine how much in between the rcv and nextrcv
        """
        x1, y1 = xy1
        x2, y2 = xy2
        theta = np.arctan2(y1 - y2, x1 - x2)
        hyp = np.sqrt((x1 - x2)**2 + (y1 - y2)**2)
        rcvBallPos = [x1 - k*hyp*np.cos(theta), y1 - k*hyp*np.sin(theta)]
        return rcvBallPos

    def calculateShootingDestination(self):
        # TODO: Intelligently use position of moving opponenet goalie
        posToAim = [0, -1.0] # in the center of the goal
        return posToAim

    def run(self):
        if self.clientID!=-1:
            _ = vrep.simxStartSimulation(self.clientID,vrep.simx_opmode_oneshot_wait)

            # set them up in the first 3 zones
            for idx in range(len(self.bots)):
                if self.zone_pass_plan[idx] == 2:
                # if False:
                    # zone 2 has some robots blocking, so navigate to zone 2
                    # in a couple of steps
                    # TOOD: may want to do this less hardcoded, more like planned path with obstacle avoidance
                    # step 1. get to midfield passing the opposing team
                    x_coord_zone = 0.3
                    self.bots[idx].add_to_path([x_coord_zone, 0, 20])
                    startSmoothPathConf = [x_coord_zone, 0, -np.pi/2]
                else:
                    startSmoothPathConf = self.bots[idx].getRobotConf(self.bots[idx].bot)

                self.planToMoveIntoReceivingPosition(idx, startSmoothPathConf=startSmoothPathConf, vel=10)
                self.bots[idx].add_delay(1*idx) # delay each by one second

            # TODO: replace system time with vrep simxTime
            t0 = time.time()
            # get the bots into position
            while time.time() - t0 < 15:
                for bot in self.bots:
                    if time.time() - t0 >= bot.delay:
                        bot.robotCode()

            # follow the zone passing plan
            first_loop = True
            activezone_idx = 0
            shoot_flag = False
            plan_length = len(self.zone_pass_plan)
            executing = [False] * len(self.bots)
            # TODO: maybe something like below with states for all bots?
            bot_states = [STATE_READY_POS] * len(self.bots)
            t1 = time.time()
            while time.time() - t1 < 1000:
                self.ballEngine.update()

                activezone = self.zone_pass_plan[activezone_idx]
                activebot_idx = self.activebot_idx_plan[activezone_idx]
                activebot = self.bots[activebot_idx]
                if plan_length > activezone_idx + 1:
                    rcvzone = self.zone_pass_plan[activezone_idx + 1]
                    rcvbot_idx = self.activebot_idx_plan[activezone_idx + 1]
                    rcvbot = self.bots[rcvbot_idx]
                else:
                    rcvzone = None
                    rcvbot_idx = None
                    rcvbot = None
                    shoot_flag = True
                if plan_length > activezone_idx + 2:
                    next_rcvzone = self.zone_pass_plan[activezone_idx + 2]
                else:
                    next_rcvzone = None


                # def vizZones():
                #     zones = np.zeros(4)
                #     zones[activezone-1] = 0.5
                #     if rcvzone:
                #         zones[rcvzone-1] = 1.0
                #     plt.imshow(zones.reshape(2,2), interpolation='nearest')
                #     plt.title('Red = RCV, Green = Active')
                # self.idash.add(vizZones)

                # -- STATE MACHINE UPDATE PARAMETERS
                if shoot_flag:
                    bot_states[activebot_idx] = STATE_SHOOT
                else:
                    bot_states[activebot_idx] = STATE_PASS
                if rcvbot_idx:
                    bot_states[rcvbot_idx] = STATE_READY_POS

                # -- STATE MACHINE EXECUTE
                    # _, proximitySensors = rcvbot.senseObstacles()
                    # if bot_states[rcvbot_idx] == STATE_READY_POS:
                    #     rcvbot.receiveBall(proximitySensors)
                    #     rcvp1 = np.array(rcvbot.getRobotConf()[:2])
                    #     rcvp2 = self.zone_corners[:, rcvzone - 1]
                    #     # not yet in position
                    #     if cdist(rcvp1.reshape(1,2), rcvp2.reshape(1,2))[0] > 0.001: # radius buffer
                    #         # FIXME: if receiving bot is moving at the same time as active bot, processing time increases by ~100ms
                    #         if not executing[rcvbot_idx]:
                    #             if next_rcvzone:
                    #                 xy2 = self.zone_centers[:,next_rcvzone-1]
                    #             else:
                    #                 xy2 = [0, -0.75]
                    #             self.planToMoveIntoReceivingPosition(rcvbot_idx, rcvzone, vel=10)
                    #             rcvbot.prunePath()
                    #             rcvbot.pause_before_kick = rcvbot.path.shape[1] > 2
                    #             executing[rcvbot_idx] = True
                    #         rcvbot.robotCode(rb=0.05, pause_before_kick=rcvbot.pause_before_kick)
                    #     else:
                    #         executing[rcvbot_idx] = False

                if bot_states[activebot_idx] == STATE_PASS:
                    if not executing[activebot_idx]:
                        activeRobotConf = activebot.getRobotConf()
                        p0 = self.ballEngine.getBallPose()

                        # -- fancy calculation of finalBallPos, using info about where next
                        xy1 = self.zone_centers[:,rcvzone-1]
                        if next_rcvzone:
                            xy2 = self.zone_centers[:,next_rcvzone-1]
                        else:
                            xy2 = [0, -0.75]
                        finalBallPos = self.calculateReceivingDestination(xy1, xy2, k=0.15)

                        # -- simple calculation of finalBallPos
                        # mult_x, mult_y = self.zonesigns[rcvzone-1]
                        # finalBallPos = [ np.abs(p0[0])*mult_x , np.abs(p0[1])*mult_y ]

                        # slow mode (tested)
                        # activebot.path, status = passPath(activeRobotConf, p0, finalBallPos, vmax=10, vr=7, kq=0.0035, hold=True)

                        # fast mode (good luck!)
                        activebot.path, status = passPath(activeRobotConf, p0, finalBallPos, vmax=20, vr=15, kq=0.0010, hold=False)

                        # avoid all friendly and opposing robots
                        obstacleConf = [self.oppBots[i].getRobotConf() for i in range(len(self.oppBots))]
                        obstacleConf.extend([self.bots[i].getRobotConf() for i in range(len(self.bots)) if i != activebot_idx])
                        for i in xrange(len(obstacleConf)):
                            activebot.obstacleAwarePath(obstacleConf[i],0.05)

                        # avoid the wall boundaries
                        activebot.prunePath()

                        activebot.pause_before_kick = activebot.path.shape[1] > 2
                        p2 = self.ballEngine.getNextRestPos()
                        # FIXME: if the path produced is invalid, i.e some part of it is off the field and invalid
                        #        prune that part of the path to make it valid
                        # FIXME: if path is not long enough
                        #        backup to give more room. or bump the ball and get more room.

                        def vizBots():
                            actx, acty = activebot.getRobotConf()[:2]
                            rcvx, rcvy = rcvbot.getRobotConf()[:2]
                            plt.hold('on')
                            plt.plot(-acty, actx, 'g+')
                            plt.plot(-activebot.path[1,:], activebot.path[0,:], 'g.')
                            plt.plot(-rcvy, rcvx, 'r+')
                            plt.plot(-rcvbot.path[1,:], rcvbot.path[0,:], 'r.')
                            plt.plot(-p0[1], p0[0], 'bo')
                            #plt.plot(-rcvp1[1], rcvp1[0], 'mx')
                            #plt.plot(-rcvp2[1], rcvp2[0], 'kx')
                            plt.xlim([-0.75, 0.75]) # y axis in the field
                            plt.ylim([-0.5, 0.5]) # x axis in the field
                            plt.title('Red = RCV, Green = Active')
                            plt.xlabel('active path length: {}'.format(activebot.path.shape[1]))
                        self.idash.add(vizBots)
                        executing[activebot_idx] = True

                    # Edit the path based on the ball movement
                    # self.ballEngine.update()
                    # deltaxy = np.array(self.ballEngine.getVelocity())
                    # print "deltaxy: ", deltaxy
                    # for idx in range(activebot.path.shape[1]):
                    #     gamma = 1 # no decay
                    #     # gamma = 1 / idx # decay factor
                    #     activebot.path[:2,-(idx+1)] += gamma*deltaxy*10

                    # Follow the path
                    activebot.robotCode(rb=0.05, pause_before_kick=activebot.pause_before_kick)
                    # activebot.robotCode(rb=0.05, pause_before_kick=False)

                if bot_states[activebot_idx] == STATE_SHOOT:
                    if not executing[activebot_idx]:
                        activeRobotConf = activebot.getRobotConf(activebot.bot)
                        ballRestPos = self.ballEngine.getBallPose()
                        finalBallPos = self.calculateShootingDestination()
                        activebot.path, status = passPath(activeRobotConf, ballRestPos, finalBallPos, vmax=25, vr=15, kq=0.0010, hold=True)
                        activebot.prunePath()
                        obstacleConfs = self.getObstacleConfs([activebot_idx])
                        activebot.multiObstacleAwarePath(obstacleConfs, 0.05)
                        activebot.prunePath()
                        executing[activebot_idx] = True
                    activebot.robotCode()
                    def vizShooting():
                        actx, acty = activebot.getRobotConf()[:2]
                        plt.hold('on')
                        plt.plot(-acty, actx, 'g+')
                        plt.plot(-activebot.path[1,:], activebot.path[0,:], 'g.')
                        plt.xlim([-0.75, 0.75]) # y axis in the field
                        plt.ylim([-0.5, 0.5]) # x axis in the field
                        plt.title('SHOOT!!!!')
                    self.idash.add(vizShooting)

                if first_loop:
                    pass
                    first_loop = False
                else:
                    pass
                    print "Elapsed (MS): ", (time.time() - time_start) * 1e3

                # Do measurements, whether pass was successful and state changes
                time_start = time.time()
                p1 = self.ballEngine.getBallPose()
                p3 = self.ballEngine.getNextRestPos()
                dist_from_start = np.sqrt((p1[0] - p0[0])**2 + (p1[1] - p0[1])**2)
                # dist_from_goal = np.sqrt((p1[0] - p2[0])**2 + (p1[1] - p2[1])**2)
                # velocity_measure = np.sqrt((p1[0] - p3[0])**2 + (p1[1] - p3[1])**2)
                vx, vy = self.ballEngine.getVelocity()
                velocity_measure = np.sqrt(vx**2 + vy**2)
                print velocity_measure
                # self.idash.add(lambda: plt.plot(velocity_measure))
                closest_zone = self.getClosestZone(p1)
                if dist_from_start > 0.01: # the ball has been touched
                     if velocity_measure < 0.003: # wait til velocity reaches zero
                         # if dist_from_goal < 0.1: # wait til the ball has entered the predicted zone
                        executing = [False] * len(self.bots) # everyone has new roles, plan next pass
                        if closest_zone == rcvzone: # success
                            # increment what is the new active zone
                            activebot.setMotorVelocities(0,0)
                            activezone_idx += 1

                self.idash.plotframe() # ~100 ms shaved by removing this line
                # time.sleep(50*1e-3)

        self.clean_exit()
class BaseRobotRunner(object):
    __metaclass__ = abc.ABCMeta

    def __init__(self, color, number, clientID=None, ip='127.0.0.1', port=19998): # ip='127.0.0.1', '172.29.34.63'
        """
        color: i.e. 'Blue'
        number: i.e. 1
        clientID: Default None; if provided as parameter,
            `vrep.simxStart` was called outside this class
        """
        # parameter init
        self.ip = ip
        self.port=port
        self.color = color
        self.bot_name = '%s%d' % (color, number)
        self.bot_nameStriker = 'Red1'
        self.vm=self.v=0
        self.tv=self.tvm=0

        # run startup methods
        if clientID is not None:
            # Use existing clientID (multi robot case)
            self.clientID = clientID
            self.multirobots = True
            print("ClientID obtained as parameter! (Probably doing Multirobots...)")
        else:
            # Make connection to VREP client (single robot case)
            self.initializeVrepClient()
            self.multirobots = False
        self.initializeVrepApi()
        #self.killer = GracefulKiller()
        self.idash = IDash(framerate=0.05)
        self.ballEngine = BallEngine(self.clientID)

    def exploreClassAttributes(self):
        for variable_name, variable_value in self.__dict__.iteritems():
            locals()[self.bot_name + "_" + variable_name] = variable_value
        # delete so there are no duplicate variables in the variable explorer
        del(variable_name)
        del(variable_value)
        return # Place Spyder Breakpoint on this Line!

    def initializeVrepClient(self):
        """ Initialization for Python to connect to VREP.
        We obtain a clientID after connecting to VREP, which is saved to
            `self.clientID`
        This method is only called if we controlling one robot with the remote API
        """
        print 'Python program started'
        count = 0
        num_tries = 10
        while count < num_tries:
            vrep.simxFinish(-1) # just in case, close all opened connections
            self.clientID=vrep.simxStart(self.ip,self.port,True,True,5000,5) #Timeout=5000ms, Threadcycle=5ms
            if self.clientID!=-1:
                print 'Connected to V-REP'
                break
            else:
                "Trying again in a few moments..."
                time.sleep(3)
                count += 1
        if count >= num_tries:
            print 'Failed connecting to V-REP'
            vrep.simxFinish(self.clientID)

    def initializeVrepApi(self):
        # initialize bot handles and variables
        _, self.leftMotor=vrep.simxGetObjectHandle(
            self.clientID, '%s_leftJoint' % self.bot_name, vrep.simx_opmode_oneshot_wait)
        _, self.rightMotor=vrep.simxGetObjectHandle(
            self.clientID, '%s_rightJoint' % self.bot_name, vrep.simx_opmode_oneshot_wait)
        _, self.bot=vrep.simxGetObjectHandle(
            self.clientID, self.bot_name, vrep.simx_opmode_oneshot_wait)

        # initialize proximity sensors
        self.proxSensors = prox_sens_initialize(self.clientID, self.bot_name)

        # initialize odom of bot
        _, self.xyz = vrep.simxGetObjectPosition(
            self.clientID, self.bot, -1, vrep.simx_opmode_streaming )
        _, self.eulerAngles = vrep.simxGetObjectOrientation(
            self.clientID, self.bot, -1, vrep.simx_opmode_streaming )

        # FIXME: striker handles shouldn't be part of the default base class
        # FIXME: should be better way to have information regarding all the bots (Central Control?) (Publishers/Subscribers...)?
        # initialize bot handles and variables for Red1
        _, self.leftMotorStriker=vrep.simxGetObjectHandle(
            self.clientID, '%s_leftJoint' % self.bot_name, vrep.simx_opmode_oneshot_wait)
        _, self.rightMotorStriker=vrep.simxGetObjectHandle(
            self.clientID, '%s_rightJoint' % self.bot_name, vrep.simx_opmode_oneshot_wait)
        _, self.botStriker = vrep.simxGetObjectHandle(
            self.clientID, self.bot_nameStriker, vrep.simx_opmode_oneshot_wait)
        _, xyzStriker = vrep.simxGetObjectPosition(
            self.clientID, self.botStriker, -1, vrep.simx_opmode_streaming )
        _, eulerAnglesStriker = vrep.simxGetObjectOrientation(
            self.clientID, self.botStriker, -1, vrep.simx_opmode_streaming )

    @abc.abstractmethod
    def robotCode(self):
        """ OUR ROBOT CODE GOES HERE """
        return

    def secondaryCode(self, role, obstacleConfs=None, passivePos=None):
        """inner while loop for when attacker is not active"""
        # FIXME: role does nothing, but maybe there should be different
        # modes of operation for passive roles
        if passivePos is None:
            passivePos = self.passivePos
        if not self.executing:
            self.status = 0 # for moving freely, without theta adjust

            self.path, self.status = smoothPath(
                self.getRobotConf(self.bot),
                passivePos,
                r=0.08)
            vf = 10
            v = vf*np.ones((1, np.size(self.path,1)))
            self.path = np.concatenate((self.path, v), axis=0)
            self.path[-1, -1]=vf

            self.prunePath()
            self.multiObstacleAwarePath(obstacleConfs, 0.04)
            self.prunePath()

        self.followPath(self.getRobotConf(self.bot), self.status, rb=0.05)

    def getRobotConf(self, robot_handle=None):
        if robot_handle is None:
            robot_handle = self.bot
        _, xyz = vrep.simxGetObjectPosition(
            self.clientID, robot_handle, -1, vrep.simx_opmode_buffer )
        _, eulerAngles = vrep.simxGetObjectOrientation(
            self.clientID, robot_handle, -1, vrep.simx_opmode_buffer )
        x, y, z = xyz
        theta = eulerAngles[2]

        return (x, y, theta)

    def senseObstacles(self):
        """
        Returns
        -------
        objectDetected: array-like, shape 4
            whether an obstacle was detected for the 4 sensors

        objectDistances: array-like, shape 4
            the distances detected
        """
        out = prox_sens_read(self.clientID, self.proxSensors)
        objectDetected = np.zeros(4)
        objectDistances = 100*np.ones(4)
        for i, sens in enumerate(out):
            if sens['detectionState'] == 1:
                objectDetected[i] = 1
                x, y, z = sens['detectedPoint']
                a = np.sqrt(x**2 + y**2)
                distance = np.sqrt(a**2 + z**2)
                objectDistances[i] = distance
        return objectDetected, objectDistances

    def setMotorVelocities(self, forward_vel, omega):
        ctrl_sig_left, ctrl_sig_right = vomega2bytecodes(forward_vel, omega, g=1)
        self.driveMotor(ctrl_sig_left, ctrl_sig_right)

    def driveMotor(self, vl, vr):
        _ = vrep.simxSetJointTargetVelocity(
            self.clientID,self.leftMotor,vl,vrep.simx_opmode_streaming ) # set left wheel velocity
        _ = vrep.simxSetJointTargetVelocity(
            self.clientID,self.rightMotor,vr,vrep.simx_opmode_streaming ) # set right wheel velocity

    def repulsion_vectors_compute(self, lidarValues, k_repulse=10.0):
        numLidarValues = len(lidarValues)
        lidarAngles = [np.pi / numLidarValues * index for index in range(numLidarValues)]

        repulsionVectors = [np.array(pol2cart(force_repulsion(k_repulse, np.sqrt(val), 2), angle)) for val, angle in zip(lidarValues, lidarAngles)]

        return repulsionVectors

    def followPath(self, robotConf, status=0, rb=0.05, dtheta=0.001, k=3.5):
        """ radius of the buffer zone
        """
        robotpos = np.array(robotConf)[0:2]

        if status==2 and self.path.shape[1]==2 and np.linalg.norm(robotpos - self.path[0:2,0])<rb:
            # print 'path'
            # print self.path
            theta = math.atan2(self.path[1,-1]-self.path[1,0], self.path[0,-1]-self.path[0,0])
            finalConf = (self.path[0,0], self.path[1,0],theta)
            # print 'finalConf'
            # print finalConf
            vRobot = v2orientation(robotConf, finalConf)
            self.setMotorVelocities(vRobot[0], vRobot[1])
            if vRobot[1]==0:
                self.path = self.path[:, 1:]
            return 2
        while self.path.shape[1]>1 and np.linalg.norm(robotpos - self.path[0:2,0])<rb :
            self.path = self.path[:, 1:]  # remove first node
        if self.path.shape[1]==1 and np.linalg.norm(robotpos - self.path[0:2,0])<rb :
            self.setMotorVelocities(0,0)
            return 0
        else:
            vRobot = v2Pos(robotConf, self.path[0:2,0], self.path[2,0], k)
            self.setMotorVelocities(vRobot[0], vRobot[1])
            return 1

    def keepGoal(self,robotConf, y=0.7,vmax=30, goalLim=0.2): # 0.7 for left goal, -0.7 for right goal
        """ the robot keep the goal by staying on a line and focusing on ball displacement """
        tol=0.0001
        self.ballEngine.update()
        pm = self.ballEngine.posm2              # previous position
        p = self.ballEngine.getBallPose()       # position
        if math.fabs(p[1]-pm[1])<tol:
            finalPos=[0,y]
            vRobot = v2PosB(robotConf, finalPos)
            self.setMotorVelocities(vRobot[0], vRobot[1])
            return 1
        if y*(p[1]-pm[1])<0:
            self.setMotorVelocities(0, 0)
            return 0
        a = (y-pm[1])/(p[1]-pm[1])  # intersection: (x,y)^t = pm+a(p-pm)
        x = pm[0]+a*(p[0]-pm[0])
        if (x>goalLim):
            x = goalLim
        if (x<-goalLim):
            x = -goalLim
        finalPos=[x,y]
        vRobot = v2PosB(robotConf, finalPos, vmax)
        self.setMotorVelocities(vRobot[0], vRobot[1])
        return 0

    def keepGoal2(self, robotConf, Gy=0.72, vmax=25, Ex=0.18, Ey=0.07): # 0.72 for left goal, -0.72 for right goal
        """ the robot keep the goal by staying on an ellipse and focusing on ball position
            configuration of the robot, princial axis of the ellipse, center of the goal """
#        self.ballEngine.update()
        bp = self.ballEngine.getBallPose()  # ball position
        a=math.atan2(bp[0],Gy-bp[1])
        rp=(Ex*math.sin(a), Gy-Ey*math.cos(a))   # desired robot position
        vRobot = v2PosB(robotConf, rp, vmax)
        self.setMotorVelocities(vRobot[0], vRobot[1])

    def keepGoalP(self, robotConf, Gy=0.72, vmax=40, Ex=0.18, Ey=0.07): # 0.72 for left goal, -0.72 for right goal
        """ the robot keep the goal with a P controller by staying on an ellipse and focusing on ball position
            configuration of the robot, princial axis of the ellipse, center of the goal """
#        self.ballEngine.update()
        bp = self.ballEngine.getBallPose()  # ball position
        a=math.atan2(bp[0],Gy-bp[1])
        rp=(Ex*math.sin(a), Gy-Ey*math.cos(a))   # desired robot position
        vRobot = v2PosP(robotConf, rp, vmax)
        self.setMotorVelocities(vRobot[0], vRobot[1])
        time.sleep(0.001)

    def v2PosA(self, robotConf, finalPos, vmax=40, k=2, kp=200, amax=100):
        """v2pos with acceleration bounded """
        x = finalPos[0] - robotConf[0]
        y = finalPos[1] - robotConf[1]
        norm = (x**2 + y**2)**.5    # euclidian norm
        tv=self.ballEngine.getSimTime()
        v=kp*norm
        if v>vmax:
            v=vmax
        va=math.fabs(self.vm)+amax*(tv-self.tvm)
        print 'va'
        print va
        if v>va:
            v=va
        self.tvm=tv
        self.vm=v
        print 'v'
        print v
        return v2PosB(robotConf, finalPos, v, k, rb=0.01)

    def add_to_path(self, path_objective):
        """ path objective is array-like, shape (3,-1) """
        path_objective = np.asarray(path_objective).reshape(3, -1)
        if self.path is not None:
            self.path = np.column_stack((
                self.path,
                path_objective
            ))
        else:
            self.path = path_objective

    def prunePath(self, xlim=0.455, ylim=0.705):
        """ Removes elements in the path if they are not within the bounds.
        Changes the self.path attribute according to this pruning.

        Parameters
        ----------
        xlim: number
            the magnitude of the bounds of the field in the x direction

        ylim: number
            the magnitude of the bounds of the field in the y direction
        """
        xGoal = 0.18
        yGoal = 0.07
        tol = 0.0125
        tol2 =  0.025
        Ex = 0.18 + tol2
        Ey = 0.07 + tol2
        Gy = 0.72
        if self.color == 'Red':
            Gy *= -1
        for idx in range(self.path.shape[1]):
            exceededX = np.abs(self.path[0,idx]) > xlim
            exceededY = np.abs(self.path[1,idx]) > ylim
            # within the boundary
            if exceededX:
                if self.path[0,idx] >= 0:
                    self.path[0,idx] = xlim - tol
                else:
                    self.path[0,idx] = - (xlim - tol)
            if exceededY:
                if self.path[1,idx] >= 0:
                    self.path[1,idx] = ylim - tol
                else:
                    self.path[1,idx] = - (ylim - tol)
            insideEllipsis = (self.path[0,idx])**2/Ex**2 + (self.path[1,idx] - Gy)**2/Ey**2 <= 1
            if insideEllipsis:
                theta = math.atan2(-self.path[1,idx] + Gy, self.path[0,idx])
                xNew = Ex*math.cos(theta)
                yNew = -Ey*math.sin(theta) + Gy
                self.path[0,idx] = xNew
                self.path[1,idx] = yNew

    def obstacleAwarePath(self, obstacleConf, rb = 0.025):
        robotPosition = self.getRobotConf()
        obstaclePath = interpolatePath(self.path, robotPosition)
        index, distance = obstacleDetector(obstacleConf, obstaclePath, rb)
        self.path = avoidObstacle(obstaclePath, obstacleConf, index, distance, rb)

    def multiObstacleAwarePath(self, obstacleConfs, rb):
        if obstacleConfs:
            for conf in obstacleConfs:
                self.obstacleAwarePath(conf, 0.07)

    def receiveBall(self, proximitySensors, interceptVel = 17.5):
        robotPosition = self.getRobotConf()
        ballPosition = self.ballEngine.getBallPose()
        pathNew = np.zeros((3,1))
        pathNew[0,0] = ballPosition[0]
        pathNew[1,0] = ballPosition[1]
        pathNew[2,0] = interceptVel
        self.path = pathNew
        self.followPath(robotPosition)

        proximity = min(proximitySensors)

        if proximity < 0.001:
            self.path = None

    def unittestMoveForward(self):
        self.setMotorVelocities(forward_vel=1, omega=0)

    def unittestTurnSideways(self, not_first_time):
        x, y, theta = self.getRobotPose()
        if not_first_time:
            goal_theta = self.first_theta + np.pi / 2
            print goal_theta
            error_theta = ThetaRange.angleDiff(theta, goal_theta)

            # control
            omega = 10 * error_theta
            print omega
            self.setMotorVelocities(0, omega)

            # visualization
            def plotCurrentDesiredHeadings():
                plotVector(ThetaRange.pol2cart(1, theta), 'k')
                plotVector(ThetaRange.pol2cart(1, goal_theta), 'r')
            self.idash.add(plotCurrentDesiredHeadings)
            self.idash.plotframe()
        else:
            self.first_theta = theta

    def clean_exit(self):
        vrep.simxFinish(self.clientID)
        print 'Program ended'

    def run(self):
        if self.clientID!=-1:
            if not self.multirobots:
                # Start simulation if MultiRobotRunner not already starting it
                _ = vrep.simxStartSimulation(self.clientID,vrep.simx_opmode_oneshot)
            self.robotCode()

        if not self.multirobots:
            # Stop VREP simulation cleanly if MultiRobotRunner not already stopping it
            self.clean_exit()

    def displayPath(self):
        print('path=')
        for i in range(0, self.path.shape[1]):
            print('%f, %f' % (self.path[0, i], self.path[1, i]))
class Master(base_robot.MultiRobotCyclicExecutor):
    def __init__(self, *args, **kwargs):
        super(Master, self).__init__(*args, **kwargs)
        self.color = None
        self.idash = IDash(framerate=0.005)

    def getClosestZone(self, pose):
        """ Zones will be from downfield (0) to close to own goal (1) """
        pose = np.array(pose[:2])  # x-y only
        assert pose.size == 2  # pose should be 2-D

        # 2 ZONES: 0 and 1
        two_zone_boundary_y = 0.01
        zone = pose[1] > two_zone_boundary_y
        if self.color == 'Red':
            zone = not zone
        return zone

    def run(self):
        if self.clientID != -1:
            _ = vrep.simxStartSimulation(self.clientID,
                                         vrep.simx_opmode_oneshot_wait)

            # robots have been added
            self.color = self.bots[0].color
            self.originalRoles = ['attacker', 'midfielder', 'goalie']
            self.roles = ['attacker', 'midfielder', 'goalie']
            goalie = self.bots[2]

            activeidx = 0  # striker starts first

            t0 = time.time()
            while time.time() - t0 < 600:
                self.ballEngine.update()

                assert activeidx != 2  # now only have active 0,1
                activebot = self.bots[activeidx]

                offense = self.originalRoles[activeidx] == 'attacker'
                if offense:
                    if self.roles[1] != 'dumb':
                        self.roles[1] = 'attacker'
                else:
                    if self.roles[1] != 'dumb':
                        self.roles[1] = 'midfielder'

                secondaryidx = not activeidx

                ballx, bally = self.ballEngine.getBallPose()
                robotX = np.zeros(2)
                robotY = np.zeros(2)
                robotX[0], robotY[0], _ = self.bots[0].getRobotConf()
                robotX[1], robotY[1], _ = self.bots[1].getRobotConf()
                distance2ball = 9999
                for i in xrange(len(self.bots[:-1])):
                    dist = np.sqrt((ballx - robotX[i])**2 +
                                   (bally - robotY[i])**2)
                    if dist < distance2ball:
                        distance2ball = dist
                        closestRobot = i
                ballPosX, ballPosY = self.ballEngine.getBallPose()
                for idx in range(len(self.bots[:-1])):
                    if offense:
                        oppGoalieConf, oppGoalieIdx = self.findOppGoalieConf(
                            return_index=True)
                        oppGoalieIdx = self.convertOppBotsIdx2BotsIdx(
                            oppGoalieIdx)
                        if idx == closestRobot:
                            self.bots[idx].robotCode(
                                role=self.roles[idx],
                                obstacleConfs=self.getObstacleConfs([
                                    idx, oppGoalieIdx
                                ]),  # dont ignore yourself, or the oppGoalie
                                goaliePosition=self.findOppGoalieConf())
                        else:  # robot is not closest robot
                            if True:  # be a bitch
                                passivePos = oppGoalieConf
                            else:
                                if ballPosX >= 0:
                                    passivePos = [-0.2, ballPosY, 0]
                                else:  # ballPosX < 0
                                    passivePos = [0.2, ballPosY, 0]
                            self.bots[idx].secondaryCode(
                                role=self.roles[idx],
                                obstacleConfs=self.getObstacleConfs(
                                    [idx]),  # attack the goalie
                                passivePos=passivePos)
                    # defense
                    else:
                        # secondary defender (original attacker being passive)
                        if idx == secondaryidx:
                            if self.bots[idx].color == 'Red':
                                passivePos = [ballPosX, 0.2, 0]
                            else:
                                passivePos = [ballPosX, -0.2, 0]
                            self.bots[idx].secondaryCode(
                                role=self.roles[idx],
                                obstacleConfs=self.getObstacleConfs(
                                    secondaryidx),
                                passivePos=passivePos)
                        # primary defender (original defender being active)
                        else:
                            self.bots[idx].robotCode(
                                role=self.roles[idx],
                                obstacleConfs=self.getObstacleConfs(activeidx),
                                goaliePosition=self.findOppGoalieConf())

                goalie.robotCode(
                    role=self.roles[2],
                    obstacleConfs=self.getObstacleConfs(activeidx),
                    goaliePosition=self.findOppGoalieConf())

                def vizBots():
                    actx, acty = activebot.getRobotConf()[:2]
                    ballx, bally = self.ballEngine.getBallPose()
                    plt.hold('on')
                    try:
                        plt.plot(-self.activebot.passPos[1],
                                 self.activebot.passPos, 'ro')
                    except:
                        pass
                    plt.plot(-acty, actx, 'g+')
                    try:
                        plt.plot(-activebot.target[1], activebot.target[0],
                                 'm*')
                    except:
                        pass
                    plt.plot(-oppGoalieConf[1], oppGoalieConf[0], 'yo')
                    plt.plot(-bally, ballx, 'ro')
                    plt.plot(-self.bots[secondaryidx].path[1, :],
                             self.bots[secondaryidx].path[0, :], 'b.')
                    plt.plot(-activebot.path[1, :], activebot.path[0, :], 'g.')
                    plt.xlim([-0.8, 0.8])  # y axis in the field
                    plt.ylim([-0.7, 0.7])  # x axis in the field
                    plt.title('Red = RCV, Green = Active')
                    plt.xlabel('active path length: {}'.format(
                        activebot.path.shape[1]))

                self.idash.add(vizBots)

                # if time.time() - activebot.time_started_2b_dumb > 2:
                #     self.roles = self.originalRoles[:]

                # if activebot.path.shape[1] < 5:
                #     self.roles[activeidx] = 'dumb'
                #     activebot.time_started_2b_dumb = time.time()

                p0 = activebot.p0
                p1 = self.ballEngine.getBallPose()
                p3 = self.ballEngine.getNextRestPos()
                dist_from_start = np.sqrt((p1[0] - p0[0])**2 +
                                          (p1[1] - p0[1])**2)
                velocity_measure = np.sqrt((p1[0] - p3[0])**2 +
                                           (p1[1] - p3[1])**2)
                closest_zone = self.getClosestZone(p1)
                if dist_from_start > 0.01:  # the ball has been touched
                    #     if velocity_measure < 0.003: # wait til velocity reaches zero
                    # if True:
                    # if velocity_measure < 1.0: # start kicking while ball is moving...
                    for bot in self.bots:
                        bot.executing = False
                    if closest_zone != activeidx:  # success
                        # increment what is the new active zone
                        # self.setMotorVelocities(0, 0)
                        activeidx = not activeidx
Exemplo n.º 16
0
class ZonePasserMasterCyclic(base_robot.MultiRobotCyclicExecutor):
    """After doing part A of Question 2, the ball will already be
    placed in Zone 4; so let's transport it there now"""
    def __init__(self, *args, **kwargs):
        super(ZonePasserMasterCyclic, self).__init__(*args, **kwargs)
        self.idash = IDash(0.005)
        self.activezones = []
        self.receivingzones = []
        self.activeplayer = None
        self.receivingplayer = None
        self.zones = {(1, 1): 1, (1, -1): 2, (-1, 1): 3, (-1, -1): 4}
        self.zonesigns = [(1, 1), (1, -1), (-1, 1), (-1, -1)]

        self.zone_centers = np.array(
            [[CENTER_X, CENTER_X, -CENTER_X, -CENTER_X],
             [CENTER_Y, -CENTER_Y, CENTER_Y, -CENTER_Y]])

        self.zone_corners = np.array(
            [[CORNER_X, CORNER_X, -CORNER_X, -CORNER_X],
             [CORNER_Y, -CORNER_Y, CORNER_Y, -CORNER_Y]])

        # TODO: remove me since we're not allowed to set the ball pose
        # start ball at zone 4 - 1 (0 indexed)
        ball_start = self.zone_centers[:, 3]
        ball_start[
            1] -= 0.05  # move closer to players center, but further distance q from player
        # self.ballEngine.setBallPose(ball_start)
        self.ballEngine.update()

        # FIXME: Hardcoded is good for drill, not good for game!
        self.zone_pass_plan = [4, 2, 1, 3, 4,
                               2]  # the locations between the 5 passes
        self.activebot_idx_plan = [
            0, 1, 2, 0, 1, 2
        ]  # which robot should take the active zone during the zone pass plan?

    def getClosestZone(self, pose):
        """ get zone which the current pose is closest to. This pose could
        be a ball, an opposing player, etc. """
        pose = np.array(pose[:2])  # x-y only
        assert pose.size == 2  # pose should be 2-D
        dist_from_zones = cdist(np.expand_dims(pose, axis=0),
                                self.zone_corners.T)
        return np.argmin(dist_from_zones) + 1  # zones are 1-indexed

    def getBotInZone(self, zone):
        """
        Not using now
        Returns the index of the bot which is inside the zone, otherwise None
        if there is no bot inside this zone"""
        bot_zones = np.zeros(len(self.bots))
        for bot_idx, bot in enumerate(self.bots):
            bot_zones[bot_idx] = self.getClosestZone(bot.getRobotConf(bot.bot))
        # we use [0][0] since return of np.where looks like (array([2]),)
        zone_as_array = np.where(bot_zones == zone)[0]
        if zone_as_array.size == 0:
            return None
        else:
            return zone_as_array[
                0]  # the first, if there are multiple in that zone

    def getNearestBotToZone(self, zone, bot_to_ignore=None):
        """ Returns the index of the bot closest to the zone, even if the bot
        may not be directly inside the zone """
        bot_poses = np.vstack([bot.getRobotConf() for bot in self.bots])
        bot_poses = bot_poses[:, :2]
        assert bot_poses.shape == (len(self.bots), 2)
        zone_pose = self.zone_corners[:, zone - 1].reshape(1, 2)
        dist_from_bots = cdist(zone_pose, bot_poses, 'cityblock')
        if bot_to_ignore is None:
            return np.argmin(dist_from_bots)
        else:  # the closest bot might be active, in which case we should return next closest
            sortedargs = np.argsort(dist_from_bots.flatten())
            if sortedargs[
                    0] == bot_to_ignore:  # first closest is active, not valid to be receive
                return sortedargs[1]  # return second closest
            else:
                return sortedargs[0]

    def planToMoveIntoReceivingPosition(self,
                                        idx,
                                        rcvzone=None,
                                        startSmoothPathConf=None,
                                        vel=15,
                                        r=0.01):
        """ Move into the corner, facing center
        Parameters
        ----------
        idx: integer
            index of the robot (in self.bots)

        startSmoothPathConf: array-like, shape (3,) or None
            the start configuration of the robot to calculate the smooth path.
            Pass this if you are calculating a path from a future point.
            If None, then use current robot configuration.
        """
        if startSmoothPathConf is None:
            startSmoothPathConf = self.bots[idx].getRobotConf()
        if rcvzone is None:
            rcvzone = self.zone_pass_plan[idx]
        final_x = self.bots[idx].zone_corners[0, rcvzone - 1]
        final_y = self.bots[idx].zone_corners[1, rcvzone - 1]
        # final theta will be facing towards center field
        _, final_theta = ThetaRange.cart2pol(final_x, final_y)
        final_theta = ThetaRange.normalize_angle(
            final_theta + np.pi)  # flip it towards center
        smooth_path, status = smoothPath(
            startSmoothPathConf,  # robotConf
            [final_x, final_y, final_theta],  # finalConf
            r=r)
        v = vel * np.ones((1, np.size(smooth_path, 1)))
        smooth_path = np.concatenate((smooth_path, v), axis=0)
        if self.bots[idx].path is None:
            self.bots[idx].add_to_path(smooth_path)
        elif self.bots[idx].path.shape[1] == 1:
            self.bots[idx].add_to_path(smooth_path)
        else:
            self.bots[idx].path = smooth_path

    def calculateReceivingDestination(self, xy1, xy2, k=0.2):
        """ receiving BallPos should be between rcvbot and next passing zone
        Parameters
        ----------
        xy1: array-like, shape (2,)
            x-y position of receiving bot
        xy2: array-like, shape (2,)
            x-y position of next receiving zone
        k: float, between 0-1
            multiplier to determine how much in between the rcv and nextrcv
        """
        x1, y1 = xy1
        x2, y2 = xy2
        theta = np.arctan2(y1 - y2, x1 - x2)
        hyp = np.sqrt((x1 - x2)**2 + (y1 - y2)**2)
        rcvBallPos = [
            x1 - k * hyp * np.cos(theta), y1 - k * hyp * np.sin(theta)
        ]
        return rcvBallPos

    def calculateShootingDestination(self):
        # TODO: Intelligently use position of moving opponenet goalie
        posToAim = [0, -1.0]  # in the center of the goal
        return posToAim

    def run(self):
        if self.clientID != -1:
            _ = vrep.simxStartSimulation(self.clientID,
                                         vrep.simx_opmode_oneshot_wait)

            # set them up in the first 3 zones
            for idx in range(len(self.bots)):
                if self.zone_pass_plan[idx] == 2:
                    # if False:
                    # zone 2 has some robots blocking, so navigate to zone 2
                    # in a couple of steps
                    # TOOD: may want to do this less hardcoded, more like planned path with obstacle avoidance
                    # step 1. get to midfield passing the opposing team
                    x_coord_zone = 0.3
                    self.bots[idx].add_to_path([x_coord_zone, 0, 20])
                    startSmoothPathConf = [x_coord_zone, 0, -np.pi / 2]
                else:
                    startSmoothPathConf = self.bots[idx].getRobotConf(
                        self.bots[idx].bot)

                self.planToMoveIntoReceivingPosition(
                    idx, startSmoothPathConf=startSmoothPathConf, vel=10)
                self.bots[idx].add_delay(1 * idx)  # delay each by one second

            # TODO: replace system time with vrep simxTime
            t0 = time.time()
            # get the bots into position
            while time.time() - t0 < 15:
                for bot in self.bots:
                    if time.time() - t0 >= bot.delay:
                        bot.robotCode()

            # follow the zone passing plan
            first_loop = True
            activezone_idx = 0
            shoot_flag = False
            plan_length = len(self.zone_pass_plan)
            executing = [False] * len(self.bots)
            # TODO: maybe something like below with states for all bots?
            bot_states = [STATE_READY_POS] * len(self.bots)
            t1 = time.time()
            while time.time() - t1 < 1000:
                self.ballEngine.update()

                activezone = self.zone_pass_plan[activezone_idx]
                activebot_idx = self.activebot_idx_plan[activezone_idx]
                activebot = self.bots[activebot_idx]
                if plan_length > activezone_idx + 1:
                    rcvzone = self.zone_pass_plan[activezone_idx + 1]
                    rcvbot_idx = self.activebot_idx_plan[activezone_idx + 1]
                    rcvbot = self.bots[rcvbot_idx]
                else:
                    rcvzone = None
                    rcvbot_idx = None
                    rcvbot = None
                    shoot_flag = True
                if plan_length > activezone_idx + 2:
                    next_rcvzone = self.zone_pass_plan[activezone_idx + 2]
                else:
                    next_rcvzone = None

                # def vizZones():
                #     zones = np.zeros(4)
                #     zones[activezone-1] = 0.5
                #     if rcvzone:
                #         zones[rcvzone-1] = 1.0
                #     plt.imshow(zones.reshape(2,2), interpolation='nearest')
                #     plt.title('Red = RCV, Green = Active')
                # self.idash.add(vizZones)

                # -- STATE MACHINE UPDATE PARAMETERS
                if shoot_flag:
                    bot_states[activebot_idx] = STATE_SHOOT
                else:
                    bot_states[activebot_idx] = STATE_PASS
                if rcvbot_idx:
                    bot_states[rcvbot_idx] = STATE_READY_POS

                # -- STATE MACHINE EXECUTE
                # _, proximitySensors = rcvbot.senseObstacles()
                # if bot_states[rcvbot_idx] == STATE_READY_POS:
                #     rcvbot.receiveBall(proximitySensors)
                #     rcvp1 = np.array(rcvbot.getRobotConf()[:2])
                #     rcvp2 = self.zone_corners[:, rcvzone - 1]
                #     # not yet in position
                #     if cdist(rcvp1.reshape(1,2), rcvp2.reshape(1,2))[0] > 0.001: # radius buffer
                #         # FIXME: if receiving bot is moving at the same time as active bot, processing time increases by ~100ms
                #         if not executing[rcvbot_idx]:
                #             if next_rcvzone:
                #                 xy2 = self.zone_centers[:,next_rcvzone-1]
                #             else:
                #                 xy2 = [0, -0.75]
                #             self.planToMoveIntoReceivingPosition(rcvbot_idx, rcvzone, vel=10)
                #             rcvbot.prunePath()
                #             rcvbot.pause_before_kick = rcvbot.path.shape[1] > 2
                #             executing[rcvbot_idx] = True
                #         rcvbot.robotCode(rb=0.05, pause_before_kick=rcvbot.pause_before_kick)
                #     else:
                #         executing[rcvbot_idx] = False

                if bot_states[activebot_idx] == STATE_PASS:
                    if not executing[activebot_idx]:
                        activeRobotConf = activebot.getRobotConf()
                        p0 = self.ballEngine.getBallPose()

                        # -- fancy calculation of finalBallPos, using info about where next
                        xy1 = self.zone_centers[:, rcvzone - 1]
                        if next_rcvzone:
                            xy2 = self.zone_centers[:, next_rcvzone - 1]
                        else:
                            xy2 = [0, -0.75]
                        finalBallPos = self.calculateReceivingDestination(
                            xy1, xy2, k=0.15)

                        # -- simple calculation of finalBallPos
                        # mult_x, mult_y = self.zonesigns[rcvzone-1]
                        # finalBallPos = [ np.abs(p0[0])*mult_x , np.abs(p0[1])*mult_y ]

                        # slow mode (tested)
                        # activebot.path, status = passPath(activeRobotConf, p0, finalBallPos, vmax=10, vr=7, kq=0.0035, hold=True)

                        # fast mode (good luck!)
                        activebot.path, status = passPath(activeRobotConf,
                                                          p0,
                                                          finalBallPos,
                                                          vmax=20,
                                                          vr=15,
                                                          kq=0.0010,
                                                          hold=False)

                        # avoid all friendly and opposing robots
                        obstacleConf = [
                            self.oppBots[i].getRobotConf()
                            for i in range(len(self.oppBots))
                        ]
                        obstacleConf.extend([
                            self.bots[i].getRobotConf()
                            for i in range(len(self.bots))
                            if i != activebot_idx
                        ])
                        for i in xrange(len(obstacleConf)):
                            activebot.obstacleAwarePath(obstacleConf[i], 0.05)

                        # avoid the wall boundaries
                        activebot.prunePath()

                        activebot.pause_before_kick = activebot.path.shape[
                            1] > 2
                        p2 = self.ballEngine.getNextRestPos()

                        # FIXME: if the path produced is invalid, i.e some part of it is off the field and invalid
                        #        prune that part of the path to make it valid
                        # FIXME: if path is not long enough
                        #        backup to give more room. or bump the ball and get more room.

                        def vizBots():
                            actx, acty = activebot.getRobotConf()[:2]
                            rcvx, rcvy = rcvbot.getRobotConf()[:2]
                            plt.hold('on')
                            plt.plot(-acty, actx, 'g+')
                            plt.plot(-activebot.path[1, :],
                                     activebot.path[0, :], 'g.')
                            plt.plot(-rcvy, rcvx, 'r+')
                            plt.plot(-rcvbot.path[1, :], rcvbot.path[0, :],
                                     'r.')
                            plt.plot(-p0[1], p0[0], 'bo')
                            #plt.plot(-rcvp1[1], rcvp1[0], 'mx')
                            #plt.plot(-rcvp2[1], rcvp2[0], 'kx')
                            plt.xlim([-0.75, 0.75])  # y axis in the field
                            plt.ylim([-0.5, 0.5])  # x axis in the field
                            plt.title('Red = RCV, Green = Active')
                            plt.xlabel('active path length: {}'.format(
                                activebot.path.shape[1]))

                        self.idash.add(vizBots)
                        executing[activebot_idx] = True

                    # Edit the path based on the ball movement
                    # self.ballEngine.update()
                    # deltaxy = np.array(self.ballEngine.getVelocity())
                    # print "deltaxy: ", deltaxy
                    # for idx in range(activebot.path.shape[1]):
                    #     gamma = 1 # no decay
                    #     # gamma = 1 / idx # decay factor
                    #     activebot.path[:2,-(idx+1)] += gamma*deltaxy*10

                    # Follow the path
                    activebot.robotCode(
                        rb=0.05, pause_before_kick=activebot.pause_before_kick)
                    # activebot.robotCode(rb=0.05, pause_before_kick=False)

                if bot_states[activebot_idx] == STATE_SHOOT:
                    if not executing[activebot_idx]:
                        activeRobotConf = activebot.getRobotConf(activebot.bot)
                        ballRestPos = self.ballEngine.getBallPose()
                        finalBallPos = self.calculateShootingDestination()
                        activebot.path, status = passPath(activeRobotConf,
                                                          ballRestPos,
                                                          finalBallPos,
                                                          vmax=25,
                                                          vr=15,
                                                          kq=0.0010,
                                                          hold=True)
                        activebot.prunePath()
                        obstacleConfs = self.getObstacleConfs([activebot_idx])
                        activebot.multiObstacleAwarePath(obstacleConfs, 0.05)
                        activebot.prunePath()
                        executing[activebot_idx] = True
                    activebot.robotCode()

                    def vizShooting():
                        actx, acty = activebot.getRobotConf()[:2]
                        plt.hold('on')
                        plt.plot(-acty, actx, 'g+')
                        plt.plot(-activebot.path[1, :], activebot.path[0, :],
                                 'g.')
                        plt.xlim([-0.75, 0.75])  # y axis in the field
                        plt.ylim([-0.5, 0.5])  # x axis in the field
                        plt.title('SHOOT!!!!')

                    self.idash.add(vizShooting)

                if first_loop:
                    pass
                    first_loop = False
                else:
                    pass
                    print "Elapsed (MS): ", (time.time() - time_start) * 1e3

                # Do measurements, whether pass was successful and state changes
                time_start = time.time()
                p1 = self.ballEngine.getBallPose()
                p3 = self.ballEngine.getNextRestPos()
                dist_from_start = np.sqrt((p1[0] - p0[0])**2 +
                                          (p1[1] - p0[1])**2)
                # dist_from_goal = np.sqrt((p1[0] - p2[0])**2 + (p1[1] - p2[1])**2)
                # velocity_measure = np.sqrt((p1[0] - p3[0])**2 + (p1[1] - p3[1])**2)
                vx, vy = self.ballEngine.getVelocity()
                velocity_measure = np.sqrt(vx**2 + vy**2)
                print velocity_measure
                # self.idash.add(lambda: plt.plot(velocity_measure))
                closest_zone = self.getClosestZone(p1)
                if dist_from_start > 0.01:  # the ball has been touched
                    if velocity_measure < 0.003:  # wait til velocity reaches zero
                        # if dist_from_goal < 0.1: # wait til the ball has entered the predicted zone
                        executing = [False] * len(
                            self.bots
                        )  # everyone has new roles, plan next pass
                        if closest_zone == rcvzone:  # success
                            # increment what is the new active zone
                            activebot.setMotorVelocities(0, 0)
                            activezone_idx += 1

                self.idash.plotframe()  # ~100 ms shaved by removing this line
                # time.sleep(50*1e-3)

        self.clean_exit()
Exemplo n.º 17
0
class BaseRobotRunner(object):
    __metaclass__ = abc.ABCMeta

    def __init__(self,
                 color,
                 number,
                 clientID=None,
                 ip='127.0.0.1',
                 port=19998):  # ip='127.0.0.1', '172.29.34.63'
        """
        color: i.e. 'Blue'
        number: i.e. 1
        clientID: Default None; if provided as parameter,
            `vrep.simxStart` was called outside this class
        """
        # parameter init
        self.ip = ip
        self.port = port
        self.color = color
        self.bot_name = '%s%d' % (color, number)
        self.bot_nameStriker = 'Red1'
        self.vm = self.v = 0
        self.tv = self.tvm = 0

        # run startup methods
        if clientID is not None:
            # Use existing clientID (multi robot case)
            self.clientID = clientID
            self.multirobots = True
            print(
                "ClientID obtained as parameter! (Probably doing Multirobots...)"
            )
        else:
            # Make connection to VREP client (single robot case)
            self.initializeVrepClient()
            self.multirobots = False
        self.initializeVrepApi()
        #self.killer = GracefulKiller()
        self.idash = IDash(framerate=0.05)
        self.ballEngine = BallEngine(self.clientID)

    def exploreClassAttributes(self):
        for variable_name, variable_value in self.__dict__.iteritems():
            locals()[self.bot_name + "_" + variable_name] = variable_value
        # delete so there are no duplicate variables in the variable explorer
        del (variable_name)
        del (variable_value)
        return  # Place Spyder Breakpoint on this Line!

    def initializeVrepClient(self):
        """ Initialization for Python to connect to VREP.
        We obtain a clientID after connecting to VREP, which is saved to
            `self.clientID`
        This method is only called if we controlling one robot with the remote API
        """
        print 'Python program started'
        count = 0
        num_tries = 10
        while count < num_tries:
            vrep.simxFinish(-1)  # just in case, close all opened connections
            self.clientID = vrep.simxStart(self.ip, self.port, True, True,
                                           5000,
                                           5)  #Timeout=5000ms, Threadcycle=5ms
            if self.clientID != -1:
                print 'Connected to V-REP'
                break
            else:
                "Trying again in a few moments..."
                time.sleep(3)
                count += 1
        if count >= num_tries:
            print 'Failed connecting to V-REP'
            vrep.simxFinish(self.clientID)

    def initializeVrepApi(self):
        # initialize bot handles and variables
        _, self.leftMotor = vrep.simxGetObjectHandle(
            self.clientID, '%s_leftJoint' % self.bot_name,
            vrep.simx_opmode_oneshot_wait)
        _, self.rightMotor = vrep.simxGetObjectHandle(
            self.clientID, '%s_rightJoint' % self.bot_name,
            vrep.simx_opmode_oneshot_wait)
        _, self.bot = vrep.simxGetObjectHandle(self.clientID, self.bot_name,
                                               vrep.simx_opmode_oneshot_wait)

        # initialize proximity sensors
        self.proxSensors = prox_sens_initialize(self.clientID, self.bot_name)

        # initialize odom of bot
        _, self.xyz = vrep.simxGetObjectPosition(self.clientID, self.bot, -1,
                                                 vrep.simx_opmode_streaming)
        _, self.eulerAngles = vrep.simxGetObjectOrientation(
            self.clientID, self.bot, -1, vrep.simx_opmode_streaming)

        # FIXME: striker handles shouldn't be part of the default base class
        # FIXME: should be better way to have information regarding all the bots (Central Control?) (Publishers/Subscribers...)?
        # initialize bot handles and variables for Red1
        _, self.leftMotorStriker = vrep.simxGetObjectHandle(
            self.clientID, '%s_leftJoint' % self.bot_name,
            vrep.simx_opmode_oneshot_wait)
        _, self.rightMotorStriker = vrep.simxGetObjectHandle(
            self.clientID, '%s_rightJoint' % self.bot_name,
            vrep.simx_opmode_oneshot_wait)
        _, self.botStriker = vrep.simxGetObjectHandle(
            self.clientID, self.bot_nameStriker, vrep.simx_opmode_oneshot_wait)
        _, xyzStriker = vrep.simxGetObjectPosition(self.clientID,
                                                   self.botStriker, -1,
                                                   vrep.simx_opmode_streaming)
        _, eulerAnglesStriker = vrep.simxGetObjectOrientation(
            self.clientID, self.botStriker, -1, vrep.simx_opmode_streaming)

    @abc.abstractmethod
    def robotCode(self):
        """ OUR ROBOT CODE GOES HERE """
        return

    def secondaryCode(self, role, obstacleConfs=None, passivePos=None):
        """inner while loop for when attacker is not active"""
        # FIXME: role does nothing, but maybe there should be different
        # modes of operation for passive roles
        if passivePos is None:
            passivePos = self.passivePos
        if not self.executing:
            self.status = 0  # for moving freely, without theta adjust

            self.path, self.status = smoothPath(self.getRobotConf(self.bot),
                                                passivePos,
                                                r=0.08)
            vf = 10
            v = vf * np.ones((1, np.size(self.path, 1)))
            self.path = np.concatenate((self.path, v), axis=0)
            self.path[-1, -1] = vf

            self.prunePath()
            self.multiObstacleAwarePath(obstacleConfs, 0.04)
            self.prunePath()

        self.followPath(self.getRobotConf(self.bot), self.status, rb=0.05)

    def getRobotConf(self, robot_handle=None):
        if robot_handle is None:
            robot_handle = self.bot
        _, xyz = vrep.simxGetObjectPosition(self.clientID, robot_handle, -1,
                                            vrep.simx_opmode_buffer)
        _, eulerAngles = vrep.simxGetObjectOrientation(self.clientID,
                                                       robot_handle, -1,
                                                       vrep.simx_opmode_buffer)
        x, y, z = xyz
        theta = eulerAngles[2]

        return (x, y, theta)

    def senseObstacles(self):
        """
        Returns
        -------
        objectDetected: array-like, shape 4
            whether an obstacle was detected for the 4 sensors

        objectDistances: array-like, shape 4
            the distances detected
        """
        out = prox_sens_read(self.clientID, self.proxSensors)
        objectDetected = np.zeros(4)
        objectDistances = 100 * np.ones(4)
        for i, sens in enumerate(out):
            if sens['detectionState'] == 1:
                objectDetected[i] = 1
                x, y, z = sens['detectedPoint']
                a = np.sqrt(x**2 + y**2)
                distance = np.sqrt(a**2 + z**2)
                objectDistances[i] = distance
        return objectDetected, objectDistances

    def setMotorVelocities(self, forward_vel, omega):
        ctrl_sig_left, ctrl_sig_right = vomega2bytecodes(forward_vel,
                                                         omega,
                                                         g=1)
        self.driveMotor(ctrl_sig_left, ctrl_sig_right)

    def driveMotor(self, vl, vr):
        _ = vrep.simxSetJointTargetVelocity(
            self.clientID, self.leftMotor, vl,
            vrep.simx_opmode_streaming)  # set left wheel velocity
        _ = vrep.simxSetJointTargetVelocity(
            self.clientID, self.rightMotor, vr,
            vrep.simx_opmode_streaming)  # set right wheel velocity

    def repulsion_vectors_compute(self, lidarValues, k_repulse=10.0):
        numLidarValues = len(lidarValues)
        lidarAngles = [
            np.pi / numLidarValues * index for index in range(numLidarValues)
        ]

        repulsionVectors = [
            np.array(
                pol2cart(force_repulsion(k_repulse, np.sqrt(val), 2), angle))
            for val, angle in zip(lidarValues, lidarAngles)
        ]

        return repulsionVectors

    def followPath(self, robotConf, status=0, rb=0.05, dtheta=0.001, k=3.5):
        """ radius of the buffer zone
        """
        robotpos = np.array(robotConf)[0:2]

        if status == 2 and self.path.shape[1] == 2 and np.linalg.norm(
                robotpos - self.path[0:2, 0]) < rb:
            # print 'path'
            # print self.path
            theta = math.atan2(self.path[1, -1] - self.path[1, 0],
                               self.path[0, -1] - self.path[0, 0])
            finalConf = (self.path[0, 0], self.path[1, 0], theta)
            # print 'finalConf'
            # print finalConf
            vRobot = v2orientation(robotConf, finalConf)
            self.setMotorVelocities(vRobot[0], vRobot[1])
            if vRobot[1] == 0:
                self.path = self.path[:, 1:]
            return 2
        while self.path.shape[1] > 1 and np.linalg.norm(
                robotpos - self.path[0:2, 0]) < rb:
            self.path = self.path[:, 1:]  # remove first node
        if self.path.shape[1] == 1 and np.linalg.norm(robotpos -
                                                      self.path[0:2, 0]) < rb:
            self.setMotorVelocities(0, 0)
            return 0
        else:
            vRobot = v2Pos(robotConf, self.path[0:2, 0], self.path[2, 0], k)
            self.setMotorVelocities(vRobot[0], vRobot[1])
            return 1

    def keepGoal(self,
                 robotConf,
                 y=0.7,
                 vmax=30,
                 goalLim=0.2):  # 0.7 for left goal, -0.7 for right goal
        """ the robot keep the goal by staying on a line and focusing on ball displacement """
        tol = 0.0001
        self.ballEngine.update()
        pm = self.ballEngine.posm2  # previous position
        p = self.ballEngine.getBallPose()  # position
        if math.fabs(p[1] - pm[1]) < tol:
            finalPos = [0, y]
            vRobot = v2PosB(robotConf, finalPos)
            self.setMotorVelocities(vRobot[0], vRobot[1])
            return 1
        if y * (p[1] - pm[1]) < 0:
            self.setMotorVelocities(0, 0)
            return 0
        a = (y - pm[1]) / (p[1] - pm[1])  # intersection: (x,y)^t = pm+a(p-pm)
        x = pm[0] + a * (p[0] - pm[0])
        if (x > goalLim):
            x = goalLim
        if (x < -goalLim):
            x = -goalLim
        finalPos = [x, y]
        vRobot = v2PosB(robotConf, finalPos, vmax)
        self.setMotorVelocities(vRobot[0], vRobot[1])
        return 0

    def keepGoal2(self,
                  robotConf,
                  Gy=0.72,
                  vmax=25,
                  Ex=0.18,
                  Ey=0.07):  # 0.72 for left goal, -0.72 for right goal
        """ the robot keep the goal by staying on an ellipse and focusing on ball position
            configuration of the robot, princial axis of the ellipse, center of the goal """
        #        self.ballEngine.update()
        bp = self.ballEngine.getBallPose()  # ball position
        a = math.atan2(bp[0], Gy - bp[1])
        rp = (Ex * math.sin(a), Gy - Ey * math.cos(a)
              )  # desired robot position
        vRobot = v2PosB(robotConf, rp, vmax)
        self.setMotorVelocities(vRobot[0], vRobot[1])

    def keepGoalP(self,
                  robotConf,
                  Gy=0.72,
                  vmax=40,
                  Ex=0.18,
                  Ey=0.07):  # 0.72 for left goal, -0.72 for right goal
        """ the robot keep the goal with a P controller by staying on an ellipse and focusing on ball position
            configuration of the robot, princial axis of the ellipse, center of the goal """
        #        self.ballEngine.update()
        bp = self.ballEngine.getBallPose()  # ball position
        a = math.atan2(bp[0], Gy - bp[1])
        rp = (Ex * math.sin(a), Gy - Ey * math.cos(a)
              )  # desired robot position
        vRobot = v2PosP(robotConf, rp, vmax)
        self.setMotorVelocities(vRobot[0], vRobot[1])
        time.sleep(0.001)

    def v2PosA(self, robotConf, finalPos, vmax=40, k=2, kp=200, amax=100):
        """v2pos with acceleration bounded """
        x = finalPos[0] - robotConf[0]
        y = finalPos[1] - robotConf[1]
        norm = (x**2 + y**2)**.5  # euclidian norm
        tv = self.ballEngine.getSimTime()
        v = kp * norm
        if v > vmax:
            v = vmax
        va = math.fabs(self.vm) + amax * (tv - self.tvm)
        print 'va'
        print va
        if v > va:
            v = va
        self.tvm = tv
        self.vm = v
        print 'v'
        print v
        return v2PosB(robotConf, finalPos, v, k, rb=0.01)

    def add_to_path(self, path_objective):
        """ path objective is array-like, shape (3,-1) """
        path_objective = np.asarray(path_objective).reshape(3, -1)
        if self.path is not None:
            self.path = np.column_stack((self.path, path_objective))
        else:
            self.path = path_objective

    def prunePath(self, xlim=0.455, ylim=0.705):
        """ Removes elements in the path if they are not within the bounds.
        Changes the self.path attribute according to this pruning.

        Parameters
        ----------
        xlim: number
            the magnitude of the bounds of the field in the x direction

        ylim: number
            the magnitude of the bounds of the field in the y direction
        """
        xGoal = 0.18
        yGoal = 0.07
        tol = 0.0125
        tol2 = 0.025
        Ex = 0.18 + tol2
        Ey = 0.07 + tol2
        Gy = 0.72
        if self.color == 'Red':
            Gy *= -1
        for idx in range(self.path.shape[1]):
            exceededX = np.abs(self.path[0, idx]) > xlim
            exceededY = np.abs(self.path[1, idx]) > ylim
            # within the boundary
            if exceededX:
                if self.path[0, idx] >= 0:
                    self.path[0, idx] = xlim - tol
                else:
                    self.path[0, idx] = -(xlim - tol)
            if exceededY:
                if self.path[1, idx] >= 0:
                    self.path[1, idx] = ylim - tol
                else:
                    self.path[1, idx] = -(ylim - tol)
            insideEllipsis = (self.path[0, idx])**2 / Ex**2 + (
                self.path[1, idx] - Gy)**2 / Ey**2 <= 1
            if insideEllipsis:
                theta = math.atan2(-self.path[1, idx] + Gy, self.path[0, idx])
                xNew = Ex * math.cos(theta)
                yNew = -Ey * math.sin(theta) + Gy
                self.path[0, idx] = xNew
                self.path[1, idx] = yNew

    def obstacleAwarePath(self, obstacleConf, rb=0.025):
        robotPosition = self.getRobotConf()
        obstaclePath = interpolatePath(self.path, robotPosition)
        index, distance = obstacleDetector(obstacleConf, obstaclePath, rb)
        self.path = avoidObstacle(obstaclePath, obstacleConf, index, distance,
                                  rb)

    def multiObstacleAwarePath(self, obstacleConfs, rb):
        if obstacleConfs:
            for conf in obstacleConfs:
                self.obstacleAwarePath(conf, 0.07)

    def receiveBall(self, proximitySensors, interceptVel=17.5):
        robotPosition = self.getRobotConf()
        ballPosition = self.ballEngine.getBallPose()
        pathNew = np.zeros((3, 1))
        pathNew[0, 0] = ballPosition[0]
        pathNew[1, 0] = ballPosition[1]
        pathNew[2, 0] = interceptVel
        self.path = pathNew
        self.followPath(robotPosition)

        proximity = min(proximitySensors)

        if proximity < 0.001:
            self.path = None

    def unittestMoveForward(self):
        self.setMotorVelocities(forward_vel=1, omega=0)

    def unittestTurnSideways(self, not_first_time):
        x, y, theta = self.getRobotPose()
        if not_first_time:
            goal_theta = self.first_theta + np.pi / 2
            print goal_theta
            error_theta = ThetaRange.angleDiff(theta, goal_theta)

            # control
            omega = 10 * error_theta
            print omega
            self.setMotorVelocities(0, omega)

            # visualization
            def plotCurrentDesiredHeadings():
                plotVector(ThetaRange.pol2cart(1, theta), 'k')
                plotVector(ThetaRange.pol2cart(1, goal_theta), 'r')

            self.idash.add(plotCurrentDesiredHeadings)
            self.idash.plotframe()
        else:
            self.first_theta = theta

    def clean_exit(self):
        vrep.simxFinish(self.clientID)
        print 'Program ended'

    def run(self):
        if self.clientID != -1:
            if not self.multirobots:
                # Start simulation if MultiRobotRunner not already starting it
                _ = vrep.simxStartSimulation(self.clientID,
                                             vrep.simx_opmode_oneshot)
            self.robotCode()

        if not self.multirobots:
            # Stop VREP simulation cleanly if MultiRobotRunner not already stopping it
            self.clean_exit()

    def displayPath(self):
        print('path=')
        for i in range(0, self.path.shape[1]):
            print('%f, %f' % (self.path[0, i], self.path[1, i]))
Exemplo n.º 18
0
class FinalProjectProgram():

    def __init__(self, color, number):
        # parameter init
        self.bot_name = '%s%d' % (color, number)
        self.bot_nameStriker = 'Red1'

        # run startup methods
        self.initializeVrepClient()
        self.initializeVrepApi()
        # self.killer = GracefulKiller()
        self.idash = IDash(framerate=0.05)

    def initializeVrepClient(self):
        #Initialisation for Python to connect to VREP
        print 'Python program started'
        count = 0
        num_tries = 10
        while count < num_tries:
            vrep.simxFinish(-1) # just in case, close all opened connections
            self.clientID=vrep.simxStart('127.0.0.1',19999,True,True,5000,5) #Timeout=5000ms, Threadcycle=5ms
            if self.clientID!=-1:
                print 'Connected to V-REP'
                break
            else:
                "Trying again in a few moments..."
                time.sleep(3)
                count += 1
        if count >= num_tries:
            print 'Failed connecting to V-REP'
            vrep.simxFinish(self.clientID)

    def initializeVrepApi(self):
        # initialize bot handles and variables
        _, self.leftMotor=vrep.simxGetObjectHandle(
            self.clientID, '%s_leftJoint' % self.bot_name, vrep.simx_opmode_oneshot_wait)
        _, self.rightMotor=vrep.simxGetObjectHandle(
            self.clientID, '%s_rightJoint' % self.bot_name, vrep.simx_opmode_oneshot_wait)
        _, self.bot=vrep.simxGetObjectHandle(
            self.clientID, self.bot_name, vrep.simx_opmode_oneshot_wait)
        # proxSens = prox_sens_initialize(self.clientID)
        # initialize odom of bot
        _, self.xyz = vrep.simxGetObjectPosition(
            self.clientID, self.bot, -1, vrep.simx_opmode_streaming)
        _, self.eulerAngles = vrep.simxGetObjectOrientation(
            self.clientID, self.bot, -1, vrep.simx_opmode_streaming)

        # # initialize overhead cam
        # _, self.overheadCam=vrep.simxGetObjectHandle(
        #     self.clientID, 'Global_Vision', vrep.simx_opmode_oneshot_wait)
        # _, self.resolution, self.image = vrep.simxGetVisionSensorImage(
        #     self.clientID,self.overheadCam,0,vrep.simx_opmode_oneshot_wait)

        # # initialize goal handle + odom
        # _, self.goalHandle=vrep.simxGetObjectHandle(
        #     self.clientID, 'Goal_Position', vrep.simx_opmode_oneshot_wait)
        # _, self.goalPose = vrep.simxGetObjectPosition(
        #     self.clientID, self.goalHandle, -1, vrep.simx_opmode_streaming)

        # initialize bot handles and variables for Red1
        _, self.leftMotorStriker=vrep.simxGetObjectHandle(
            self.clientID, '%s_leftJoint' % self.bot_name, vrep.simx_opmode_oneshot_wait)
        _, self.rightMotorStriker=vrep.simxGetObjectHandle(
            self.clientID, '%s_rightJoint' % self.bot_name, vrep.simx_opmode_oneshot_wait)
        _, self.botStriker = vrep.simxGetObjectHandle(
            self.clientID, self.bot_nameStriker, vrep.simx_opmode_oneshot_wait)
        _, xyzStriker = vrep.simxGetObjectPosition(
            self.clientID, self.botStriker, -1, vrep.simx_opmode_streaming)
        _, eulerAnglesStriker = vrep.simxGetObjectOrientation(
            self.clientID, self.botStriker, -1, vrep.simx_opmode_streaming)

    def robotCode(self):
        """ OUR ROBOT CODE GOES HERE """
#        count = 0
#        while True:
#            self.unittestTurnSideways(count)
#            count += 1
        
        xGoalie = self.getRobotPose(self.bot)[0]
        yGoalie = self.getRobotPose(self.bot)[1]
        thetaGoalie = self.getRobotPose(self.bot)[2]
        thetaStriker = self.getRobotPose(self.botStriker)[2]
        
        # ball pose
        ballPose = (0, 0) # (x, y)
        
        goalie2Ball = abs(yGoalie - ballPose[1]) # distance between ball and goalkeeper
        goalieDistance = abs(goalie2Ball * math.tan(thetaStriker))

        if thetaStriker < 0:
            xSaving = xGoalie + goalieDistance
        else:
            xSaving = xGoalie - goalieDistance
            
        savingPosition = [xSaving, yGoalie, z] # position in which the the goalie saves the ball
        
#        desiredHeading = -math.pi/2
#        tolerance = 5*math.pi/360
#        error = []
#        Kp = 2
#        Tdiff = 0.01
#        Tint = 0.1
#        Tsample = 0.02
#        i = 0
#        
#        while ((thetaGoalie < (desiredHeading - tolerance) or thetaGoalie > (desiredHeading + tolerance))) and i <= 20:
#                error.append(abs(thetaGoalie - desiredHeading))
#                if i == 0:
#                    vDiff = Kp*error[i]
#                else:
#                    errorSum = 0
#                    for j in range(i):
#                        errorSum += error[j]
#                    vDiff = Kp*(error[i] + Tdiff/Tsample*(error[i] - error[i-1]) + Tsample*Tint*errorSum)
#                _ = vrep.simxSetJointTargetVelocity(
#                    self.clientID,self.leftMotor,-vDiff,vrep.simx_opmode_oneshot_wait) # set left wheel velocity
#                _ = vrep.simxSetJointTargetVelocity(
#                    self.clientID,self.rightMotor,vDiff,vrep.simx_opmode_oneshot_wait) # set right wheel velocity
#                time.sleep(Tsample)
#                thetaGoalie = self.getRobotPose(self.bot)[2]
#                #print thetaGoalie
#                i += 1
#        
#        xTolerance = 0.0001
#        error = []
#        i = 0
#        Kp = 100
#        Tint = 1
#        Tsample = 0.01
#    
#        while ((xGoalie < (xSaving - xTolerance) or xGoalie > (xSaving + xTolerance))) and i <= 25:
#            error.append(xGoalie - xSaving)
#            if i == 0:
#                vComm = Kp*error[i]
#            else:
#                errorSum = 0
#                for j in range(i):
#                    errorSum += error[j]
#                vComm = Kp*(error[i] + Tdiff/Tsample*(error[i] - error[i-1]) + Tsample*Tint*errorSum)
#            _ = vrep.simxSetJointTargetVelocity(
#                self.clientID,self.leftMotor,-vComm,vrep.simx_opmode_oneshot_wait) # set left wheel velocity
#            _ = vrep.simxSetJointTargetVelocity(
#                self.clientID,self.rightMotor,-vComm,vrep.simx_opmode_oneshot_wait) # set right wheel velocity
#            time.sleep(Tsample)
#            xGoalie = self.getRobotPose(self.bot)[0]
#            #print xGoalie
#            i += 1
            
        while 1:      
            robotConf = self.getRobotConf(self.bot)
            print('robotConf.x=%f' % robotConf[0])
            print('robotConf.y=%f' % robotConf[1])
            print('robotConf.theta=%f' % robotConf[2])
            print('savingPosition.x=%f' % savingPosition[0])
            print('savingPosition.y=%f' % savingPosition[1]) 
            print('savingPosition.theta=%f' % savingPosition[2])
            vRobot = v2Pos(robotConf, savingPosition)
            print('vRobot.x=%f' % vRobot[0])
            print('vRobot.y=%f' % vRobot[1]) 
            self.setMotorVelocities(vRobot[0], vRobot[1])
            #self.setDriveVelocities(0, 0)
            
        time.sleep(3)
        
    def getRobotConf(self, robot_handle):
        _, xyz = vrep.simxGetObjectPosition(
            self.clientID, robot_handle, -1, vrep.simx_opmode_buffer)
        _, eulerAngles = vrep.simxGetObjectOrientation(
            self.clientID, robot_handle, -1, vrep.simx_opmode_buffer)
        x, y, z = xyz
        theta = eulerAngles[2]

        return (x, y, theta)

    def getRobotPose(self, robot_handle):
        _, xyz = vrep.simxGetObjectPosition(
            self.clientID, robot_handle, -1, vrep.simx_opmode_buffer)
        _, eulerAngles = vrep.simxGetObjectOrientation(
            self.clientID, robot_handle, -1, vrep.simx_opmode_buffer)
        x, y, z = xyz
        theta = eulerAngles[2]

        return (x, y, theta)

    def setMotorVelocities(self, forward_vel, omega):
        ctrl_sig_left, ctrl_sig_right = vomega2bytecodes(forward_vel, omega, g=1)
        _ = vrep.simxSetJointTargetVelocity(
            self.clientID,self.leftMotor,ctrl_sig_left,vrep.simx_opmode_oneshot_wait) # set left wheel velocity
        _ = vrep.simxSetJointTargetVelocity(
            self.clientID,self.rightMotor,ctrl_sig_right,vrep.simx_opmode_oneshot_wait) # set right wheel velocity

    def unittestMoveForward(self):
        self.setMotorVelocities(forward_vel=1, omega=0)

    def unittestTurnSideways(self, not_first_time):
        x, y, theta = self.getRobotPose()
        if not_first_time:
            goal_theta = self.first_theta + np.pi / 2
            print goal_theta
            error_theta = ThetaRange.angleDiff(theta, goal_theta)

            # control
            omega = 10 * error_theta
            print omega
            self.setMotorVelocities(0, omega)

            # visualization
            def plotCurrentDesiredHeadings():
                plotVector(ThetaRange.pol2cart(1, theta), 'k')
                plotVector(ThetaRange.pol2cart(1, goal_theta), 'r')
            self.idash.add(plotCurrentDesiredHeadings)
            self.idash.plotframe()
        else:
            self.first_theta = theta

    def clean_exit(self):
        _ = vrep.simxStopSimulation(self.clientID,vrep.simx_opmode_oneshot_wait)
        vrep.simxFinish(self.clientID)
        print 'Program ended'

    def run(self):
        if self.clientID!=-1:
            _ = vrep.simxStartSimulation(self.clientID,vrep.simx_opmode_oneshot_wait)
            self.robotCode()

        self.clean_exit()