Python RRT源代碼
# -*- coding: utf-8 -*-
"""
Spyder Editor
This is a temporary script file.
"""
import random
import math
import turtle as t
t.speed(10000)
#initial the map 500*500 map +-250
t.pu()
t.goto(-250,250)
t.pd()
t.fd(500)
for i in range(3):
t.right(90)
t.fd(500)
#initial the obstacle x1 -250<x<-100 25<y<75
t.begin_fill()
t.color("black")
t.pu()
t.goto(-250,75)
t.pd()
t.right(90)
t.fd(150)
t.right(90)
t.fd(50)
t.right(90)
t.fd(150)
t.right(90)
t.fd(50)
t.end_fill()
#initial the obstacle x2 -150<x<-50 -250<y<-50
t.begin_fill()
t.color("black")
t.pu()
t.goto(-150,-50)
t.pd()
t.right(90)
t.fd(100)
t.right(90)
t.fd(200)
t.right(90)
t.fd(100)
t.right(90)
t.fd(200)
t.end_fill()
#initial the obstacle x3 0<x<250 0<y<50
t.begin_fill()
t.color("black")
t.pu()
t.goto(0,50)
t.pd()
t.right(90)
t.fd(250)
t.right(90)
t.fd(50)
t.right(90)
t.fd(250)
t.right(90)
t.fd(50)
t.end_fill()
#initial the start and the goal
start_x = -220
start_y = 220
goal_x = -220
goal_y = -220
t.pu()
t.goto(start_x,start_y)
t.pd()
t.circle(2)
t.pu()
t.goto(goal_x,goal_y)
t.pd()
t.circle(3)
tree=[]
foot_length=10
tree.append((start_x,start_y))
def nearest(new_x,new_y):
i=0
Min=math.sqrt((tree[0][0]-new_x)**2+(tree[0][1]-new_y)**2)
for j in range(len(tree)):
distance=math.sqrt((tree[j][0]-new_x)**2+(tree[j][1]-new_y)**2)
if tree[j][0]==new_x and tree[j][1]==new_y:
break
if distance<=Min and distance!=0:
Min=distance
i=j
return tree[i]
px=start_x
py=start_y
new_x=start_x
new_y=start_y
while math.sqrt((new_x-goal_x)**2+(new_y-goal_y)**2)>=10:
if random.random()<0.2:
random_x=goal_x
random_y=goal_y
else:
num1=random.random()
num2=random.random()
random_x=500 * (-0.5 + num1)
random_y=500 * (-0.5 + num2)
node_x,node_y = nearest(random_x,random_y)
new_x=node_x+(random_x-node_x)/foot_length
new_y=node_y+(random_y-node_y)/foot_length
if 1:
if new_x>-250 and new_x<-100 and new_y<75 and new_y>25:
flag_1=1
else:
flag_1=0
if new_x>-150 and new_x<-50 and new_y<-50 and new_y>-250:
flag_11=1
else:
flag_11=0
if new_x>0 and new_x<250 and new_y<50 and new_y>0:
flag_111=1
else:
flag_111=0
if flag_1==0 and flag_11==0 and flag_111==0:
tree.append((new_x,new_y))
t.pu()
t.goto(node_x,node_y)
t.pd()
t.goto(new_x,new_y)
t.circle(1)
final_path = []
node_x,node_y = goal_x,goal_y
while True:
node_x,node_y=nearest(node_x,node_y)
t.begin_fill()
t.goto(node_x,node_y)
final_path.append((node_x,node_y))
t.color("red")
t.end_fill()
if node_x == start_x and node_y == start_y:
break;
print(final_path)
最終輸出結果如下圖
RRT connect 算法源代碼
# -*- coding: utf-8 -*-
"""
Created on Sun Jan 19 20:27:02 2020
@author: 1918358
"""
# -*- coding: utf-8 -*-
"""
Spyder Editor
This is a temporary script file.
"""
import random
import math
import turtle as t
import datetime
starttime = datetime.datetime.now()
t.speed(10000)
#initial the map 500*500 map +-250
t.pu()
t.goto(-250,250)
t.pd()
t.fd(500)
for i in range(3):
t.right(90)
t.fd(500)
#initial the obstacle x1 -250<x<-100 25<y<75
t.begin_fill()
t.color("black")
t.pu()
t.goto(-250,75)
t.pd()
t.right(90)
t.fd(150)
t.right(90)
t.fd(50)
t.right(90)
t.fd(150)
t.right(90)
t.fd(50)
t.end_fill()
#initial the obstacle x2 -150<x<-50 -250<y<-50
t.begin_fill()
t.color("black")
t.pu()
t.goto(-150,-50)
t.pd()
t.right(90)
t.fd(100)
t.right(90)
t.fd(200)
t.right(90)
t.fd(100)
t.right(90)
t.fd(200)
t.end_fill()
#initial the obstacle x3 0<x<250 0<y<50
t.begin_fill()
t.color("black")
t.pu()
t.goto(0,50)
t.pd()
t.right(90)
t.fd(250)
t.right(90)
t.fd(50)
t.right(90)
t.fd(250)
t.right(90)
t.fd(50)
t.end_fill()
#initial the start and the goal
start_x = -220
start_y = 220
goal_x = 220
goal_y = -220
t.pu()
t.goto(start_x,start_y)
t.pd()
t.circle(2)
t.pu()
t.goto(goal_x,goal_y)
t.pd()
t.circle(3)
tree1=[]
tree2=[]
foot_length=15
tree1.append((start_x,start_y))
tree2.append((goal_x,goal_y))
def nearest(new_x,new_y,tree):
i=0
Min=math.sqrt((tree[0][0]-new_x)**2+(tree[0][1]-new_y)**2)
for j in range(len(tree)):
distance=math.sqrt((tree[j][0]-new_x)**2+(tree[j][1]-new_y)**2)
if tree[j][0]==new_x and tree[j][1]==new_y:
break
if distance<=Min and distance!=0:
Min=distance
i=j
return tree[i]
px=start_x
py=start_y
new_x=start_x
new_y=start_y
new2_x = goal_x
new2_y = goal_y
Tree = []
x2,y2 = nearest(new_x,new_y,tree2)
x1,y1 = nearest(new2_x,new2_y,tree1)
while math.sqrt((new_x-x2)**2+(new_y-y2)**2)>=25 or math.sqrt((new2_x-x1)**2+(new2_y-y1)**2)>=25:
if random.random()<0.1:
random_x=new2_x
random_y=new2_y
else:
num1=random.random()
num2=random.random()
random_x=500 * (-0.5 + num1)
random_y=500 * (-0.5 + num2)
node_x,node_y = nearest(random_x,random_y,tree1)
new_x=node_x+(random_x-node_x)/foot_length
new_y=node_y+(random_y-node_y)/foot_length
if 1:
if new_x>-250 and new_x<-100 and new_y<75 and new_y>25:
flag_1=1
else:
flag_1=0
if new_x>-150 and new_x<-50 and new_y<-50 and new_y>-250:
flag_11=1
else:
flag_11=0
if new_x>0 and new_x<250 and new_y<50 and new_y>0:
flag_111=1
else:
flag_111=0
if flag_1==0 and flag_11==0 and flag_111==0:
tree1.append((new_x,new_y))
t.pu()
t.goto(node_x,node_y)
t.pd()
t.goto(new_x,new_y)
t.circle(1)
if random.random()<0.1:
random2_x=new_x
random2_y=new_y
else:
num11=random.random()
num22=random.random()
random2_x=500 * (-0.5 + num11)
random2_y=500 * (-0.5 + num22)
node2_x,node2_y = nearest(random2_x,random2_y,tree2)
new2_x=node2_x+(random2_x-node2_x)/foot_length
new2_y=node2_y+(random2_y-node2_y)/foot_length
if 1:
if new2_x>-250 and new2_x<-100 and new2_y<75 and new2_y>25:
flag_1=1
else:
flag_1=0
if new2_x>-150 and new2_x<-50 and new2_y<-50 and new2_y>-250:
flag_11=1
else:
flag_11=0
if new2_x>0 and new2_x<250 and new2_y<50 and new2_y>0:
flag_111=1
else:
flag_111=0
if flag_1==0 and flag_11==0 and flag_111==0:
tree2.append((new2_x,new2_y))
t.pu()
t.goto(node2_x,node2_y)
t.pd()
t.goto(new2_x,new2_y)
t.circle(1)
x2,y2 = nearest(new_x,new_y,tree2)
x1,y1 = nearest(new2_x,new2_y,tree1)
#tree2 = tree2[::-1]
Tree = tree1 + tree2
node_x,node_y = new2_x,new2_y
node2_x,node2_y = new_x,new_y
while True:
node_x,node_y=nearest(node_x,node_y,tree1)
t.begin_fill()
t.goto(node_x,node_y)
t.color("red")
t.end_fill()
if node_x == start_x and node_y == start_y:
break
t.pu()
t.goto(new_x,new_y)
t.pd()
t.begin_fill()
t.goto(new2_x,new2_y)
t.color("red")
t.end_fill()
while True:
node2_x,node2_y=nearest(node2_x,node2_y,tree2)
t.begin_fill()
t.goto(node2_x,node2_y)
t.color("red")
t.end_fill()
if node2_x == goal_x and node2_y == goal_y:
break
endtime = datetime.datetime.now()
print (endtime - starttime).seconds
結果如下：
這個階段的學習存檔，請大家互相批評指正，共同學習！