saberbud/PathPlanning
1
0
Fork 0
mirror of https://github.com/zhm-real/PathPlanning.git synced 2026-08-29 08:34:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

changed path

Browse Source
This commit is contained in:
391311qy
2020-06-24 13:58:07 -07:00
parent 9da3e40db8
commit 71a7da3aac
4 changed files with 18 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Sampling-based Planning/rrt_3D/__pycache__/env3D.cpython-37.pyc
BIN
View File
Binary file not shown.
Sampling-based Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc
BIN
View File
Binary file not shown.
Sampling-based Planning/rrt_3D/rrt3D.py
+8 -3
View File
@@ -5,12 +5,17 @@ This is rrt star code for 3D
"""
import numpy as np
from numpy.matlib import repmat
from rrt_3D.env3D import env
from collections import defaultdict
import pyrr as pyrr
from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path, edgeset
import time
import os
import sys
sys.path.append(os.path.dirname(os.path.abspath(__file__))+"/../../Sampling-based Planning/")
from rrt_3D.env3D import env
from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path, edgeset
class rrtstar():
def __init__(self):
Sampling-based Planning/rrt_3D/rrtstar3D.py
+10 -5
View File
@@ -5,12 +5,16 @@ This is rrt star code for 3D
"""
import numpy as np
from numpy.matlib import repmat
from rrt_3D.env3D import env
from collections import defaultdict
import pyrr as pyrr
from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path, edgeset
import time
import os
import sys
sys.path.append(os.path.dirname(os.path.abspath(__file__))+"/../../Sampling-based Planning/")
from rrt_3D.env3D import env
from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path, edgeset
class rrtstar():
def __init__(self):
@@ -37,6 +41,7 @@ class rrtstar():
ind = 0
xnew = self.env.start
while ind < self.maxiter and getDist(xnew,self.env.goal) > 1:
#while ind < self.maxiter:
xrand = sampleFree(self)
xnearest = nearest(self,xrand)
xnew = steer(self,xnearest,xrand)
@@ -69,9 +74,9 @@ class rrtstar():
# when the goal is reached
if getDist(xnew,self.env.goal) <= 1:
self.wireup(self.env.goal,xnew)
self.Path,D = path(self)
print('Total distance = '+str(D))
self.Path,self.D = path(self)
visualization(self)
print('Total distance = '+str(self.D))
if __name__ == '__main__':
p = rrtstar()