python turtle 手撸RRT算法

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

结果如下：

这个阶段的学习存档，请大家互相批评指正，共同学习！