From 89a04e5cbd502c95066cc1b2f02c54cbd462b8b6 Mon Sep 17 00:00:00 2001
From: yue qi <391311qy@gmail.com>
Date: Sun, 26 Jul 2020 00:42:09 -0700
Subject: [PATCH] 'rrt'

---
 .../__pycache__/plot_util3D.cpython-37.pyc    | Bin 5797 -> 5797 bytes
 .../rrt_3D/__pycache__/utils3D.cpython-37.pyc | Bin 7020 -> 7138 bytes
 Sampling-based Planning/rrt_3D/rrt3D.py       |  19 +++++++++++-------
 Sampling-based Planning/rrt_3D/utils3D.py     |  16 +++++++++------
 4 files changed, 22 insertions(+), 13 deletions(-)

diff --git a/Sampling-based Planning/rrt_3D/__pycache__/plot_util3D.cpython-37.pyc b/Sampling-based Planning/rrt_3D/__pycache__/plot_util3D.cpython-37.pyc
index c82ed3ea42894ad6e3d8c553d95253901678cfe1..7cb6a21e0bd83dd91ad15b197fa593d0fee8c351 100644
GIT binary patch
delta 19
ZcmZ3gyHuCUiI<m)0SH`-H*(Du0{|)O1NZ;{

delta 19
ZcmZ3gyHuCUiI<m)0SM$2H*(Du0{|$k1Hb?P

diff --git a/Sampling-based Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc b/Sampling-based Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc
index 9d70ca8ab5f3d59a1e4b87f44e2c2620f074f1ac..7b6c76bb2a7fb8b4cd116e28dd2c3718198a5984 100644
GIT binary patch
delta 2155
zcmZ`)U1%It6rMYCXLhp5Zqj6%&2Lj<Z9A2Q+SI1fpO7@h3X%q5Q@d1_$<8F1bar+*
zGpV6&j1Vf~Ls6~|(!LZ5g7iu1gRey&eO2fc6#5`2#Rq*8p`LR$b~jBs%Y2+U|M#As
zeRTZcSavd#No)AzM+b{v41Jw_z*-BY7l2>-N`tici``~zLg-%}A}vtRij+((`N_n@
z<m4{=h;0Ooah7H$8019B_<Y|p)Te?t=^=GpsY71$^Io<fUgej}8k^H<;uha)Wl#uN
zai4cFTl~l;-f#?=4x%8by5N>$pjgmFuss9}BJUu+aR-8?@5gM?%+_tzrgH{^!ZW{P
z8FA1Wc)1VdK#4mB3=#cYp97*|?Pdewx^;6*CB{j8FM&z$0>azVDicSqXQg~Kr6%T6
zd)V;3)zn9No?QI?7#~o6$OT!8&h%(&v&n4wG&_9nM*6yL?V~U+iQddm#{nXn)y`$c
z*_ik^BZ}nMvEVpqSO~JMR0&thQ87;3(iKP%e}rHR!AU{gD=}U{kVa);K^ox41zrig
z*@EFl@wQ0UvU}M{aVz`pa-NKa3Dhn2ler0QwB$=8^dsmb5k-;af*Xc0946X0KRF2}
zk#fw&jZ3q%9E6@qVMj&9<Mv~0LLBS3Ij4$EJp-r7mlXz)8!ky6RiG@*WVIUl(sE&`
z97N6T;VfzEYe4-RC)*D+v}<_dO_Y}m1bXO-s2*e#e}a%`HND0*^_sTAbZy;O=QZ79
zy;_YC_GmSuU#syPYI(ZiB5vavii_CbE*k6D>B@`gZJ?Vxxg&FvP<@e^=^^GdRJUr@
zoaXQuV3<;)Yp~g<5YmnlzZwVEe5`4slUqnv6_%4+wggcL{IVY<lCMSnd-c-3s9H%4
zvx#ax+(omkQSM~ZY{Y;W@%OI#nMve38Fl$^R^0DAw9%-@6oq=76w-|Z3TDX#ZywH*
zaF#%|E?q60Ly%Sw#ewG+(vj~%wGu?zt3xWBMBgA4Q=e%#$`<~5@a0UbbY<8DvDWpO
zHH|Xp6YaU)4Z4u#fi9%haAn}3n>(4*FCl!7Z~d%#PAx9QebNoo2{cEl8wyBZH((#E
z=$mZaSevRDTi?beyJEmB%DA|bJG_y-hy|_bi#isK%{VPk&kr5I9)gPm?+~;TP>A|)
zs^sglA^EJEEZ3)kmI=1-(Wj0ec@<TDKw+Fk&{!Mm#2d<`SeDrgE{ply<$8G=HMW-B
zczJZ{Co<Yvz(w)f?%)`yI82(0VZ}`xUFsu8c1$Cx{F6Qv;uw`G<?5NA#b);b_O7_w
zy=O-l&!<4LDU2r`b$_t`NIeXX{c!Q=<JiR_ZTMpGSMh27D4P*Cdb-7fd|zfD(KqSl
zamr3<;!e--oB`wrXva$<_7jK0BGKIUmi+o6SM9G3-Z;JOdR~!K9-?wgd`(?hr{2bd
z-?K1IUZsUoQn~7H`Gf#@&v)5cS5{vC5LWN|V=6N$?Egub6fu_Us$|iQGvgnqvi(zh
z+xyn|62)2}2nnhLjWaws2>2-Y1z**?HnR=ewmJCXY~Pr5nT*=ShkdVYv=Nyjpw3eN
zjW9seFahmkwWQTfh6(zlZ3r_27YJxbph(~$NUO52;+GN?{#kiR`&7B&t%m*?T2!$(
R(eJdj*d4Z=P21e&{{eyipc4Q9

delta 2119
zcmZ`)Piz!b7@s#YJ3F1S+y8Xi(o!iF2a`hDYH0%0mIBd~g3^K(!#drWZl|4{UB4Ny
z;^H2Np1hdng`VJIVoXd3ClW8jlZl=@s816U4@Qg!<AH-Q(eHax>@LOGyx)HBz3+YB
z_r8Dc=ZW8+&(35rmV)2-f&TpMfjimnS+hvFK6r&s6lmsuvRf>}x%%<JW@G{hyO0v;
zq8Cq3&&;&pBr*{crZbBjVbH}3+DH4Jz&`6oagNwk$qqT*r}wZm{+#~W>Da8Y8|=LD
z=-)NYD|Y%?%d+o8o;w&hrApw3MPPUovIH#%#m$@a-J1d(XY+Ow|5~4(7peG4CGc!b
zSbpfo6$hMB1Y{$05;U3y1Gx4Kj;IwuQHSvs7Bf|o?ZN>Ad@=O}%kmy$&$E5F5A6uH
z22T<FzcTyz8%7uF=U0u*3280a2m1(81Vac5r=%senph=e)tSamrw7>nyDRDU)ov2;
z;2-?tJ*5ab%{6PRx&Fwsb&MUlTeIF%jbT!AfLocptu~SMY?GNOHp$=4@H`2&4LCv^
z27=gCC<j+cVLpnT!aAgB{V>5Kf}Ms$DlLp52(7%lEHv<<CA}QD=aw`lj2bkpX9w9a
z{z3NdRSG=pCy-v;L&sC#ghfwiffqstZIKp{T6TgUf<r`WXJ=;M7*cj>+v6ftSoH%}
zny@3H{EwD#c7i|My7{sUHsu;jkt_of$O(!<56e)3LAm5WvE+yK!r>%ss2i|^RS?CE
z*J*FtS9=Nf3mO7_6igZ?a6}^rRkg};O02B0TWUhzW_`+at;(v(npV|1m2Ty_9_v+|
zvfXC$sGA%QrwC3X2s4VkO5|Vjq>T->H7UxN0#vU;-w3_8>U~X%DLm@Ir$|O5RAD)m
zWpIWsw%^ap;@r+iSHMX=)N!!3?VtaY+t#+5mr?}>+zT*|2jMJ%OeFFTyoewSKa70W
zTe3pWflApA8@)v=I&IxSD(Xd0u&@mL<#6bXf7Ov;3tV)3Xbj>W^zk=4duo&?_2spc
z-oOI(!uYYf!mG%5A4k%B)K1xc<Z457$PY3)7m&ahU}Uaq8?&)m)#OZDS8uRr4r#rr
z^(Z{kW!J2W3dT!aQIiNYB`LcX1;8Q!iGqs+)H0Iz$x@NVCz&erb56XP<S3Okrqt+T
zR}gvyS-!;0i0V*bCTm9zb*3?B*m*AT&$~)Pb#+Tpj8!LE9h?1z4sEs3%Y1IPKTdo$
z6RDM;?8LSz)RDtGY6DsRvW^Cr)`fDZa{N2~VD|y`I&bS9*rCStNYS@-Y6|?7?stbr
z6E%9|1uKtV$IR!c-{$i_@`v4{>;-P;4%f2e=^K>&SQ|$bS3#d@^GP44riig7nLzJ|
z&>}Ckb?}JRJ-6s39TwBRp@Gr1>9}s5SboCU)&oS|Ygeze(lSaKK^FdNZgGkf2m`!Q
z`Kq@i9#%=F?72^p-kdN?1;6lW&pQ*+NDB8O`%+SiU{?hTcX$FHM|t))KGHikwL-dl
zf^~v2!FF6FMQ<v&2tVME*D;x?nQ2o8m*4B1FfI{8D}UHKS<4XFMIf6hy<gBzAlobT
zP1#6g-vr8ipq>S12o?yabl@^U0YMn$<u$JmOZ88(OPEJXW%o+p9jDhf;-B@|&4$@(
Knpw-#P5oax{+*uy

diff --git a/Sampling-based Planning/rrt_3D/rrt3D.py b/Sampling-based Planning/rrt_3D/rrt3D.py
index e467893..2edad1f 100644
--- a/Sampling-based Planning/rrt_3D/rrt3D.py	
+++ b/Sampling-based Planning/rrt_3D/rrt3D.py	
@@ -21,12 +21,13 @@ from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near,
 class rrtstar():
     def __init__(self):
         self.env = env()
-        self.Parent = defaultdict(lambda: defaultdict(dict))
+        # self.Parent = defaultdict(lambda: defaultdict(dict))
+        self.Parent = {}
         self.V = []
         self.E = edgeset()
         self.i = 0
         self.maxiter = 10000
-        self.stepsize = 1.0
+        self.stepsize = 0.5
         self.Path = []
         self.done = False
 
@@ -39,7 +40,7 @@ class rrtstar():
         self.ind = 0
         self.fig = plt.figure(figsize=(10, 8))
         xnew = self.env.start
-        while self.ind < self.maxiter and getDist(xnew, self.env.goal) > 1:
+        while self.ind < self.maxiter and getDist(xnew, self.env.goal) > self.stepsize:
             xrand = sampleFree(self)
             xnearest = nearest(self, xrand)
             xnew = steer(self, xnearest, xrand)
@@ -47,13 +48,17 @@ class rrtstar():
             if not collide:
                 self.V.append(xnew)  # add point
                 self.wireup(xnew, xnearest)
+
+                if getDist(xnew, self.env.goal) <= self.stepsize:
+                    goal = tuple(self.env.goal)
+                    self.wireup(goal, xnew)
+                    self.Path, D = path(self)
+                    print('Total distance = ' + str(D))
                 # visualization(self)
                 self.i += 1
             self.ind += 1
-            if getDist(xnew, self.env.goal) <= 1:
-                self.wireup(self.env.goal, xnew)
-                self.Path, D = path(self)
-                print('Total distance = ' + str(D))
+            # if the goal is really reached
+            
         self.done = True
         visualization(self)
         plt.show()
diff --git a/Sampling-based Planning/rrt_3D/utils3D.py b/Sampling-based Planning/rrt_3D/utils3D.py
index 2c0d7d9..b256aa0 100644
--- a/Sampling-based Planning/rrt_3D/utils3D.py	
+++ b/Sampling-based Planning/rrt_3D/utils3D.py	
@@ -169,9 +169,12 @@ def nearest(initparams, x):
 
 
 def steer(initparams, x, y):
-    direc = (y - x) / np.linalg.norm(y - x)
-    xnew = x + initparams.stepsize * direc
-    return tuple(xnew)
+    dist, step = getDist(y, x), initparams.stepsize
+    increment = ((y[0] - x[0]) / dist * step, (y[1] - x[1]) / dist * step, (y[2] - x[2]) / dist * step)
+    xnew = (x[0] + increment[0], x[1] + increment[1], x[2] + increment[2])
+    # direc = (y - x) / np.linalg.norm(y - x)
+    # xnew = x + initparams.stepsize * direc
+    return xnew
 
 
 def near(initparams, x):
@@ -199,8 +202,8 @@ def cost(initparams, x):
 
 
 def path(initparams, Path=[], dist=0):
-    x = initparams.env.goal
-    while not all(x == initparams.env.start):
+    x = tuple(initparams.env.goal)
+    while x != tuple(initparams.env.start):
         x2 = initparams.Parent[x]
         Path.append(np.array([x, x2]))
         dist += getDist(x, x2)
@@ -225,7 +228,8 @@ class edgeset(object):
         if x in self.E:
             self.E[x].add(y)
         else:
-            self.E[x] = set(y)
+            self.E[x] = set()
+            self.E[x].add(y)
 
     def remove_edge(self, edge):
         x, y = edge[0], edge[1]