From 381e99543659f4c7c402016994da2bc2621c9fa2 Mon Sep 17 00:00:00 2001
From: liqi <lq@mpig.com.cn>
Date: Sat, 2 Dec 2017 20:52:15 +0800
Subject: [PATCH] Code.python.p34

This is p34 Code.python for Mobile Robot lesson from Liqi
---
 .gitignore                   |   2 +
 Codes.python/p34/compound.py |  25 ++++++++++++
 Codes.python/p34/p34.py      |  72 +++++++++++++++++++++++++++++++++++
 Codes.python/p34/tim.mat     | Bin 0 -> 2672 bytes
 4 files changed, 99 insertions(+)
 create mode 100644 .gitignore
 create mode 100644 Codes.python/p34/compound.py
 create mode 100644 Codes.python/p34/p34.py
 create mode 100644 Codes.python/p34/tim.mat

diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..fd20fdd
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,2 @@
+
+*.pyc
diff --git a/Codes.python/p34/compound.py b/Codes.python/p34/compound.py
new file mode 100644
index 0000000..1d383e2
--- /dev/null
+++ b/Codes.python/p34/compound.py
@@ -0,0 +1,25 @@
+import numpy as np
+from math import*
+def compound(Xwa,Xab):
+    Xwb = np.array(np.zeros([3,3]))
+    rot = np.array([[cos(Xwa[2,0]),-sin(Xwa[2,0] )],
+                  [sin(Xwa[2,0]), cos(Xwa[2,0])]])
+    Xwb[0:2] = rot.dot(np.array([Xab[0],Xab[1]])) + np.tile([Xwa[0:2]],(1, len(Xab[0])))
+    row = np.shape(Xab)
+    if row[0] == 3:
+        Xwb[2] = piTopi(Xwa[2] + Xab[2])
+    return Xwb
+
+def piTopi(angle):
+    twopi = 2*pi
+    angle = angle - twopi * int(angle/twopi)
+    i1 = np.where(angle >= pi)
+    angle[i1] = angle[i1] - twopi
+
+    i2 = np.where(angle < -pi)
+    angle[i2] = angle[i2] + twopi
+    return angle
+
+# a =np.array([[5],[3],[5*pi/18]])
+# b =np.array([[4],[2],[pi/9]])
+# print compound(a,b)
\ No newline at end of file
diff --git a/Codes.python/p34/p34.py b/Codes.python/p34/p34.py
new file mode 100644
index 0000000..5c086ed
--- /dev/null
+++ b/Codes.python/p34/p34.py
@@ -0,0 +1,72 @@
+import compound
+from math import *
+from numpy import *
+import scipy.io as sio
+import matplotlib.pyplot as plt
+
+def compSteer(x,wp,iwp,G,dt):
+    maxG = 30*pi/180
+    rateG = 20*pi/180
+    minD = 0.5
+    cwp = wp[:,iwp-1]
+    d2 = (cwp[0]-x[0])**2 + (cwp[1]-x[1])**2
+    if d2 < minD**2:
+        iwp = iwp + 1
+        if iwp > len(wp[0]):
+            iwp = 0
+        cwp = wp[:,iwp-1]
+
+    deltaG = compound.piTopi(atan2(cwp[1]-x[1],cwp[0]-x[0])-x[2]-G)
+    maxDelta = rateG * dt
+
+    if abs(deltaG) > maxDelta:
+        deltaG = sign(deltaG)*maxDelta
+
+    G = G + deltaG
+    if abs(G) > maxG:
+        G = sign(G)*maxG
+
+    return G,iwp
+
+if __name__=="__main__":
+    #load tim.mat for getting wp
+    load_fn = 'tim.mat'
+    load_data = sio.loadmat(load_fn)
+    wp = load_data['wp']
+
+    steering = 0 # initial steering
+    velocity = 4.0 # the velocity of the vehicle
+    wheelbase = 8  # the length of wheelbase
+    dt = 0.05 # time interval
+    iWp = 1 # index to first waypoint
+    iPos = 0
+    rob = array([[0,-8,-8],[0,-4,4]])
+    rob_plot_array = array([[0,-wheelbase,-wheelbase,0],[0,-4,4,0]])
+
+    #Initialize figure
+    plt.figure()
+    plt.xlabel('X(m)')
+    plt.ylabel('Y(m)')
+    axes = plt.axes(xlim=(-110,100),ylim=(-110,100))
+    axes.plot(wp[0],wp[1],'*g--' ,markersize = 10 ,linewidth = 2)
+    axes.plot(wp[0],wp[1],'c:',linewidth = 2)
+    robplot ,= axes.plot(rob_plot_array[0],rob_plot_array[1],'b')
+    pos = zeros([3, 1])
+    path = zeros([3,3390])
+
+    while iWp!= 0:
+        steering, iWp = compSteer(pos, wp, iWp, steering, dt)
+        pos[0] = pos[0] + velocity * dt * cos(steering + pos[2, :])
+        pos[1] = pos[1] + velocity * dt * sin(steering + pos[2, :])
+        pos[2] = pos[2] + velocity * dt * sin(steering) / wheelbase
+        pos[2] = compound.piTopi(pos[2])
+        robPos = compound.compound(pos, rob)
+        path[:, iPos + 1] = pos[:, 0]
+        iPos += 1
+        axes.plot(path[0, 0:iPos], path[1, 0:iPos], 'r-',linewidth = 2)
+        axes.plot(robPos[0,0],robPos[1,0])
+        b = robPos[0:2, 0:1]
+        a = robPos[0:2]
+        c = hstack((a, b))
+        robplot.set_data(c[0], c[1])
+        plt.pause(0.00001)
diff --git a/Codes.python/p34/tim.mat b/Codes.python/p34/tim.mat
new file mode 100644
index 0000000000000000000000000000000000000000..e406a8f393510cd5a4bc202c99a836e9e913cabd
GIT binary patch
literal 2672
zcmb7GX;f2L5)QW5fXXf)2u5X<Erdma0u=}WLWo%&BqF<j1ICCf?$R#EqO9GFD=JD0
zxTB&&17eNhG$^g;K}E!FaY4WZq)|i==S59^&N=hWd8cljs`pjhs$1`V!Aye7^r5;s
z(5S&o!agBU8gEDCOJmX!R;8@;pz?jifjm2^Z%TYjT6`RJmCS=GCS`EUda4thN~d`^
zxq7&|kqep&_5Xe-DuKb86bhwGjY641N>Y54q@bvN*C&xOn|xI$lPJ=a6pA{zPL?}n
z@3}f{@JB$=gOub849qk&iPT61u-5B6ShpK6;FNWKVXHgnzU~+l8VN95Q}$u%9U%~V
zRWhE&@v*_^$fcz#g;26fC#n7(6FW$hs459Wlc;)DT0lHHU9qYr0L)t{-m8Z^pgD_-
zr%gb0_c7WJ<H6uQwnHKa3j%gL(}8Ur3TQ4|QnYJ6nC*jsc2@#wA38Se6w<I=j+;Z`
z$n#pm$IjP=v3`97H0UR6Icy}sX7B1{*MfMEdEm3pDK!yR^_d*pXyF6v`olD3_d_ti
zfnD_`)D=6<k%6Br!ErLI@od1MV)bF3rx?4)G}B}vEMd)`n({6PCucl;e&l@+>d$Lm
zbMiP7bL6<z&iHHUS##k`F6MB{*7k?dV9VEinqrMu_qbiNGwl73zhzrJ1Mo@06>VD$
zkoL6qmlissfqvo~FLeSkmd2RqJz?Rdo!z<DO#Pw5P@F%Mz<}ClpOFil9x!*ROI%Y&
zC_MRMNt@=m2$Hj77q44wi;XvZedb>kVa4rmnX8n8cs;hIeySKXc2_#{aze1$G1My0
z-T~@|EONydh3?x+xF;{Tfn85P!KX0>meqEb|GQlm7CC&*=&|EMbf&;Ul<b7Vztw*p
zx*rH;z5~+NHwl#FnXky<yMmdg=J4hj5^OknBYcxV06cd%y|L~K7dW)(<|)Pk^hnrX
zIO#19IGrCQFV#bFt>jIw@o526Eu~HyE#=}4vz&KcToKfUTDDDB^~Z|J#WDj7hOQ5$
z`KOuy2cjYZL}o&ujfjpvad*HaN9c!_5AuOhaQ4NmI4#UFF^;&kl>^Y~lzZdMY<N56
zh0ioG4?1iucl90hgd|31=UFxvK%*_%qM8ALJMt}75`@yuC|UXZv0DhV&d&A>QsF@P
zJ)ea`YJ^f>*uOTU#+eUkKaYGkBlQJ^e)oz$+^6zE)7#NarO+RyEK!T@GZ4XmONUnP
zVX}YcX0{~$%?I0CcOU(>M+BTbb?dU?LSXkLNBiufWIev?itiP86C;x|52OVHQTj@@
zqm&18vzGKG9B0B)O$*t*<%Ckt$SPcBzCr{wqowMFX713`l9GahJkTx+FY3Qd2ZM=G
zNvcc;MuJ*;%Q`FIyt(D8lg<apn)eIUYpgJ-`(V<3Jp%a5rbmJ=`bzvk7wU`QZfi8C
zdXe87AqJ!K=?hvtZLv*n_6hv51g6<nFc`-c;N3*$^dRjJVEO5Ux6JWU`ghi)wx#9!
zp(5WKsexu%sX;hi>mLz0!i7WJiA9?FES&X&?c$V*AfR6~OP}Y*QQGNI`jdkL7GrDr
z0k!)&exShnn6AG0vtSXVtUt6qx}J@Se$H*0a4xY5LYmHx6V>Z!kaD}K->sI3l+uer
zf%P2NAIs8mDP;jib$g@T&m!e{tIjuSd0arCS3_n_M}Hs`yrE9>z8!{<hnh4-EXZ?g
z`LK5|Q;eIsonB5Haf0FWO9q2Cm!hKH(ow;Si!?D(n`F5g%5}m0uc4B=N0uUO^l+b(
z!~!>&9y`5iAObt`+#T{J*@AVC4Lx#&5R)&@ek;2*2hZ0Yb(m);L`8p$nC^^rJrTa<
zB>c-biGiHV=lS8IVK^(T?bV?x<ot2$&Ykd$#5CE)qTsb|SYUK=b*5N??C}7r+$a{d
zKNzcf=P5!<WA$T6=4>q57kHEy<fEegihOxYB5TF=VB8$wsCRMgQan+tUOKoS7(aX<
z{4tI|`)g;1Dnw)+13d%Ou|UlA>W4j9e9YAGG=A|W7=tz>Eeg{nFs@~H?&@T~oo8ES
z-W})RV3)S)NTD|x*z_%XG{qk&HI;-!WQjdrG91O{c$jmd@OoQ00~4JrvZnt*AnmW1
z%7@c!Ad;T;`=>H47T9hLKXX+GU1#|(V**|A;~cu4UMd~RDb1-7*#c1DEBZs+D!Lx7
z#zvF#_OVySLcsv{sZHJ(hKhMn^t;3`B0KpR1E{%`S}yOn%KqkDj<5ct$x`MUm%6R7
zC&~{i-~YOk<14^%uO?Hf4-XyRy4dR8<s(%(L)B`o6ZD7Vmf4>PL?e7H(@|LnE+@uz
zX{&f(Gd;pOUFxEoFU9@_*)tcE`)fnd<*A!zm{`DGk91(qH%F-P&c3!?13D&#S=-B~
zxYsww^Fh2DY`JnZpl))vw_^X+d!Igc_+<_#{L7kaZ5HP_0Cu(V^gvbVub2mldI=oA
zW2nR%FD^>llIe`esn)wxq?VxHT(3TKeW6lky|eIIW8#9D3;G7!n|z>zU#}}Y?}E|9
z1tBBKT{*9Igx{Y!vzg#F)uv~Z>4$a`G`(z1HvFlb!3^37sQ7=z=480vIq0vnE9%!^
z+1Ppi20-05o5w$ea8di885gfd&WGgUUjpYfYU7sd)A52zRm`eWEmiSgp`53H%Kzpm
zI^TK9+T?FMMVs{J%XtXb21>~5I^re^b7nrV;zhEdYfwlsksxf?z5T%F*buPRI=y^i
zD;uSs*e@d!#n3CihB;2(?t5=Opz{|jm~IF~a=!9j0JykxQOzN5$PCZpKl(!ejimRp
zasiZWv?#uD6`)aWHxt2@e|Y|__1XsD)xMvie-DIGx!nhF11Yw3@!_kvR*17m`s54(
z;LCXrA;D7j^`wafa&{8a6NY79`OQptK93|a`$JH{yUvhVn~o89{-}!=U4@TLa-LM}
Xiw@1#)h<R5NR`u~VkZpv79al$9Z7VT

literal 0
HcmV?d00001