python turtle 手撸RRT算法

Python RRT源代码

# -*- coding: utf-8 -*-
"""
Spyder EditorThis is a temporary script file.
"""import random
import math
import turtle as tt.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=0Min=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:breakif  distance<=Min and distance!=0:Min=distancei=j     return tree[i]px=start_x
py=start_ynew_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_xrandom_y=goal_yelse: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_lengthnew_y=node_y+(random_y-node_y)/foot_lengthif 1:if new_x>-250 and new_x<-100 and new_y<75 and new_y>25:flag_1=1else:flag_1=0if new_x>-150 and new_x<-50 and new_y<-50 and new_y>-250:flag_11=1else:flag_11=0if new_x>0 and new_x<250 and new_y<50 and new_y>0:flag_111=1else:flag_111=0if 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 EditorThis 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=0Min=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:breakif  distance<=Min and distance!=0:Min=distancei=j     return tree[i]px=start_x
py=start_ynew_x=start_x
new_y=start_ynew2_x = goal_x
new2_y = goal_yTree = []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_xrandom_y=new2_yelse: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_lengthnew_y=node_y+(random_y-node_y)/foot_lengthif 1:if new_x>-250 and new_x<-100 and new_y<75 and new_y>25:flag_1=1else:flag_1=0if new_x>-150 and new_x<-50 and new_y<-50 and new_y>-250:flag_11=1else:flag_11=0if new_x>0 and new_x<250 and new_y<50 and new_y>0:flag_111=1else:flag_111=0if 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_xrandom2_y=new_yelse: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_lengthnew2_y=node2_y+(random2_y-node2_y)/foot_lengthif 1:if new2_x>-250 and new2_x<-100 and new2_y<75 and new2_y>25:flag_1=1else:flag_1=0if new2_x>-150 and new2_x<-50 and new2_y<-50 and new2_y>-250:flag_11=1else:flag_11=0if new2_x>0 and new2_x<250 and new2_y<50 and new2_y>0:flag_111=1else:flag_111=0if 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 + tree2node_x,node_y = new2_x,new2_y
node2_x,node2_y = new_x,new_ywhile 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

结果如下：

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

python turtle 手撸RRT算法相关推荐

清华大一Python作业太难上热榜！只上3节课，手撸AI算法，网友：离本科毕设只差一篇万字论文...
点击上方"视学算法",选择加"星标"或"置顶" 重磅干货,第一时间送达金磊发自凹非寺量子位报道 | 公众号 QbitAI 太难了! ...
Python学习--手撸LBP实现过程
因为课题的原因,最近在分析自己的想法与思路的时候,准备先从头开始复现一下LBP特征值的提取过程,平常提取LBP特征往往都是调用Scikit-Image库,直接一句话就能完成,如下: from skim ...
【附源代码】手把手教你用Python+uiautomator2手撸一款自动抢菜应用
包菜 -- 包你有菜包菜是我开发的一款自动抢菜软件,解决yiqing期间大家吃菜难的问题. 需要完整源代码的朋友可以私信我背景事情的起因是这样的:yiqing导致物资紧张.配送困难,抢菜成为风控 ...
php 辗转相除法,手撸golang 基本数据结构与算法最大公约数欧几里得算法/辗转相除法...
手撸golang 基本数据结构与算法最大公约数欧几里得算法/辗转相除法缘起最近阅读<>([日]石田保辉:宫崎修一) 本系列笔记拟采用golang练习之欧几里得算法欧几里得算法(又 ...
python手撕分水岭算法
python手撕分水岭算法 1 分水岭算法实现主要思路就是: 利用一个优先队列与有序队列(有序队列其实可以不用).优先队列是按像素的灰度值排列的,灰度值低的先被淹. 通过统计像素的附近的点的标记种类 ...
Selenium4.0+Python手撸自动化框架系列之 Web元素等待方式介绍与封装
目录前言三种等待一.线程等待二.隐性等待三.显性等待封装一.参数设计二.函数名设计三.封装代码设计前言 web自动化测试,常常因为硬件配制,浏览器,网速等因素导致网页加载速度过慢, ...
python机器学习手写算法系列——逻辑回归
从机器学习到逻辑回归今天,我们只关注机器学习到线性回归这条线上的概念.别的以后再说.为了让大家听懂,我这次也不查维基百科了,直接按照自己的理解用大白话说,可能不是很严谨. 机器学习就是机器可以自己学 ...
手撸web框架即引入框架思想，wsgierf模块，动静态网页，模板语法jinja2，python三大主流web框架，django安装，三板斧...
手撸web框架 web框架什么是web框架? 暂时可理解为服务端. 软件开发架构 C/S架构 B/S架构 # 后端 import socketserver = socket.socket() # 不 ...
python机器学习手写算法系列——线性回归
本系列另一篇文章<决策树> https://blog.csdn.net/juwikuang/article/details/89333344 本文源代码: https://github.c ...

python turtle 手撸RRT算法

Python RRT源代码

python turtle 手撸RRT算法相关推荐

最新文章

热门文章