world.regionOperating.size[0] = 600
world.regionOperating.size[1] = 400
world.regionGoal.center[0] = 100
world.regionGoal.center[1] = 100
world.regionGoal.size[0] = 40
world.regionGoal.size[0] = 40
obs = Region(2)
obs.center[0] = 0
obs.center[1] = 200
obs.size[0] = 10
obs.size[1] = 10
world.obstacles.append(obs)
planner = RRTstarPlanner(2)
planner.world = world
root = planner.getRootVertex()
rootState = root.state
rootState[0] = 0
rootState[1] = 0
planner.initialize()
# This parameter should be larger than 1.5 for asymptotic
# optimality. Larger values will weigh on optimization
# rather than exploration in the RRT* algorithm. Lower
# values, such as 0.1, should recover the RRT.
planner.setGamma(1.5)
'''
Created on Jan 4, 2015
@author: daqing_yi
'''
from RRTstarPlanner import *
import time
if __name__ == '__main__':
def calcDist(currentPos, referencePos):
dist = 0.0
if referencePos == None:
return dist
dist = np.sqrt((currentPos[0] - referencePos[0])**2 +
(currentPos[1] - referencePos[1])**2)
return dist
MAP_FILE = './map.png'
planner = RRTstarPlanner([600, 400], 10, calcDist, MAP_FILE)
path = planner.findPath([40, 40], [500, 40], 6000)
print path
import pygame.image
pygame.image.save(planner.rrts_viz.screen, 'RRTstar02.png')
while True:
planner.rrts_viz.update()
readInput = True
start_pos = mapMgrViz.convertedTrackingPosList[0]
end_pos = mapMgrViz.convertedTrackingPosList[
len(mapMgrViz.convertedTrackingPosList) - 1]
def calcDist(currentPos, referencePos):
dist = 0.0
if referencePos == None:
return dist
dist = np.sqrt((currentPos[0] - referencePos[0])**2 +
(currentPos[1] - referencePos[1])**2)
return dist
homoMgr = HomotopyMgr(mapMgrViz.world_map, mapMgrViz.reader)
homoMgr.init(mapMgrViz.refString)
planner = RRTstarPlanner([mapMgr.width, mapMgr.height], 10, calcDist,
MAP_FILE)
for subseg in mapMgr.subsegments:
planner.rrts_viz.refLines.append(
[subseg.line_seg.coords[0], subseg.line_seg.coords[1]])
path = planner.findPath(start_pos, end_pos, 1000, homoMgr)
print path
import pygame.image
pygame.image.save(planner.rrts_viz.screen, 'RRTstar02.png')
while True:
planner.rrts_viz.update()
Created on Jan 4, 2015
@author: daqing_yi
'''
from RRTstarPlanner import *
import time
if __name__ == '__main__':
def calcDist(currentNode, referenceNode):
dist = 0.0
if referenceNode == None:
return dist
dist = np.sqrt((currentNode.pos[0] - referenceNode.pos[0])**2 +
(currentNode.pos[1] - referenceNode.pos[1])**2)
return dist
MAP_FILE = './lab-map-inferior.png'
MAP_FILE = './lab-map-scaled.png'
planner = RRTstarPlanner([444, 989], 10, calcDist, MAP_FILE)
path = planner.findPath([40, 40], [320, 300], 1000)
print path
import pygame.image
pygame.image.save(planner.rrts_viz.screen, 'RRTstar01.png')
while True:
planner.rrts_viz.update()
error = int(dx / 2.0)
ystep = 1 if y1 < y2 else -1
y = y1
points = []
for x in range(x1, x2):
coord = (y, x) if is_steep else (x, y)
points.append(coord)
if coord[1] >= objVals.shape[0] or coord[0] >= objVals.shape[1]:
continue
cost += objVals[int(coord[1]), int(coord[0])] / 255.0
error -= abs(dy)
if error < 0:
y += ystep
error += dx
return cost
planner = RRTstarPlanner([600, 400], 10, calcCost)
planner.rrts_viz.loadObj(FIT_FILE)
path = planner.findPath([40, 40], [500, 40], 2000)
print path
import pygame.image
pygame.image.save(planner.rrts_viz.screen, 'RRTstar00-1.png')
while True:
planner.rrts_viz.update()