示例#1
0
def runRRT(dynamics, robotSize, data, start=[0, 50], end=[1950, 1650]):
    data = data[::-1, :]
    plt.imshow(data.T,
               interpolation='nearest')  # show 2D representation of map
    img = obstaclefinder.imgToObs()  # create img from imported picture
    if dynamics == 'HOUND':
        scaleFactor = 3
    elif dynamics == 'HIPPO':
        scaleFactor = 6
    obs = img.obsSpaceGen(robotSize, data, scaleFactor, debug=False)
    plt.imshow(data, interpolation='nearest')  #show 2D representation of map

    #initialize RRT
    #print(obs[0].shape)
    r = RRT.rrt(N=7000,
                obstacles=obs.T,
                obstacletype='array',
                maxcoords=obs[0].shape,
                origin=start + [0, '', 0],
                goal=end,
                live=False,
                divis=10)

    #Perform RRT
    trajectory = r.rrt(dynamics, plotting=True)

    return trajectory


#(verbose = True, plotting=True, dynamics)
示例#2
0
def runRRT(dynamics, robotSize, data, start=[0, 50], end=[1950, 1650]):
    data = data[::-1, :]
    plt.imshow(data.T,
               interpolation='nearest')  # show 2D representation of map
    img = obstaclefinder.imgToObs()  # create img from imported picture
    if dynamics == 'HOUND':
        scaleFactor = 3
    elif dynamics == 'HIPPO':
        scaleFactor = 6
    obs = img.obsSpaceGen(robotSize, data, scaleFactor, debug=False)
    plt.imshow(data, interpolation='nearest')  #show 2D representation of map

    #############################################################################################
    """
        #SET THE CAR THAT IS BEING TESTED
        Contestant = "Hound" or "Hippo" whoever is running the maze
    """
    Contestant = "Hound"

    #############################################################################################

    if dynamics == 'HOUND' and Contestant == "Hippo":
        return [[0, 0, 0, [[0, 0, 100]]]]
    if dynamics == 'HIPPO' and Contestant == "Hound":
        return [[0, 0, 0, [[0, 0, 0, 0, 100]]]]

    #initialize RRT
    #print(obs[0].shape)
    r = RRT.rrt(N=5000,
                obstacles=obs.T,
                obstacletype='array',
                maxcoords=obs[0].shape,
                origin=start + [0, '', 0],
                goal=end,
                live=False,
                divis=10)

    #Perform RRT
    trajectory = r.rrt(dynamics, plotting=True)

    return trajectory


#(verbose = True, plotting=True, dynamics)
示例#3
0
import sys

import matplotlib.pyplot as plt
import RRT
import obstaclefinder
import math
import numpy as np
robotradius = 85
scale = 5      #set scale-up to increase obstacle accuracy
img = obstaclefinder.imgToObs(imagepath = "/home/sokchetraeung/EE183DB/ECE183DA/lab3/maze.bmp") #create img from imported picture
[image,obsimg] = img.obsfind(scale,robotradius/scale) #takes scale up factor and obstacle expansion factor and produces display array and obstacle array

plt.imshow(image.T,interpolation='nearest') #show 2D representation of map

#initialize RRT
r = RRT.rrt(N = 3000,obstacles = obsimg, obstacletype = 'array', maxcoords = image.shape,
            origin = [35*scale,215*scale,0,0,'',0],goal = [125*scale,30*scale,math.pi/2],live = True, divis = 5,scale = scale,arb = False)
#Perform RRT
trajectory = r.rrt("hippo" ,verbose = True,plotting=True)
#print trajectory
print(trajectory)
#print input list
for x in trajectory:
    print(x[3])
#print(r.outputList)
示例#4
0
def runRRT(dynamics, robotsize, data ,start = [0,50],end = [1950,1650]):
    data = data[::-1, :]
    plt.imshow(data.T, interpolation='nearest')  # show 2D representation of map
    img = obstaclefinder.imgToObs()  # create img from imported picture
        
        
    ##########################################################################################
    """
        THIS SECTION DETERMINES PARAMETER OF CONFIG SPACE:
        
        img.obsSpaceGen(robotSize, data, scaleFactor, debug=False)
        
        robotSize = [length in pixels, width in pixels]
            For scale, the area covered by the box is roughly 1200 by 1200 pixels
            As this increases so does compuation time
        data = camera view
            (DO NOT ALTER)
        scaleFactor = Downsizing of resolution i.e 4 means 16 pixels become 1 pixel
            As scale factor goes up, computation speed increases but accuracy decreases 
        debug = True spits out config slices, False supresses slices
            Will show 0,30,45,60,90,120,135,150,180 degree slices
    """
    robotSize = [100,200]
    scaleFactor = 6
    debug_val = True

    """
        THIS SECTION DETERMINES PARAMETER OF PLANNER
        r = RRT.rrt(N = 10000,obstacles = obs.T, obstacletype = 'array', maxcoords = obs[0].shape,
            origin = start+[0,'',0],goal = end, live = False, divis = 10)
            
        N = Upper limit on nodes generated in RRT
            As N goes up, higher chance of success but longer run time
        obstacles = obstacles as determined by config 
            (DO NOT ALTER)
        obstacletype = format of obstacle data
            (DO NOT ALTER)
        maxcoords = size of space
            (DO NOT ALTER)
        origin = Start position of car
            (DO NOT ALTER)
        goal = Goal, may alter first three values for x,y,theta (DO NOT ALTER REMAINING VALUES)
            This is set by green cone
        live = Live Plotting (Turned Off, takes too long)
            (DO NOT ALTER)
        divis = Time division of live plotter
            (DO NOT ALTER)
    """
    N = 10000
    
    
    ##########################################################################################
    

    #initialize RRT
    if dynamics == "HIPPO":
        debug_val = False
    obs = img.obsSpaceGen(robotSize, data, scaleFactor, debug=debug_val)
    plt.imshow(data,interpolation='nearest') #show 2D representation of map
    r = RRT.rrt(N = N,obstacles = obs.T, obstacletype = 'array', maxcoords = obs[0].shape,
            origin = start+[0,'',0],goal = end, live = False, divis = 10)
            
    #Perform RRT
    trajectory = r.rrt(dynamics,plotting=True)

    return trajectory

#(verbose = True, plotting=True, dynamics)