From c1822ca5f8348eca842964ab94f7ec05ddff1807 Mon Sep 17 00:00:00 2001 From: Sai Raghava Reddy Ganapaa Date: Sun, 18 Aug 2019 19:50:21 +0200 Subject: [PATCH] optimizing the class modules --- .../knowledge_transfer.cpython-36.pyc | Bin 0 -> 4925 bytes .../__pycache__/leg_dynamics.cpython-36.pyc | Bin 0 -> 12265 bytes .../__pycache__/my_network.cpython-36.pyc | Bin 0 -> 4502 bytes spider_control/control1.py | 27 ++-- spider_control/knowledge_transfer.py | 5 +- spider_control/launch/spider_control.launch | 2 +- spider_control/leg_dynamics.py | 136 +++++++++--------- spider_control/my_network.py | 6 +- 8 files changed, 87 insertions(+), 89 deletions(-) create mode 100644 spider_control/__pycache__/knowledge_transfer.cpython-36.pyc create mode 100644 spider_control/__pycache__/leg_dynamics.cpython-36.pyc create mode 100644 spider_control/__pycache__/my_network.cpython-36.pyc mode change 100644 => 100755 spider_control/control1.py mode change 100644 => 100755 spider_control/knowledge_transfer.py mode change 100644 => 100755 spider_control/leg_dynamics.py mode change 100644 => 100755 spider_control/my_network.py diff --git a/spider_control/__pycache__/knowledge_transfer.cpython-36.pyc b/spider_control/__pycache__/knowledge_transfer.cpython-36.pyc new file mode 100644 index 0000000000000000000000000000000000000000..90fc436f5673227f820a46330373198e92a546a7 GIT binary patch literal 4925 zcmaJ_TXW<_6;|s$SI>IAUhjoPy9t4TWzA*-yd(s&2?-Q9gis*}f{>>*Ga758NlUY~ zr{pRMZ}DdG94LMOKY<^?3vcu*FYG@^@x*sp^31H`Fs^PLopZX?=YHCcI-TYp-}>;Q z&u(hkzqPfW0r>kE`42I1jk84anR@EJfv29BQ_HtlMYDZd(H!4-TjK^dpK9DZ)qR&+ z++?Ja^ zohL>7I0^IoC)p^@i+C`g)6vht@Gu@@DU1Yn42JZ0_>LnE@HL3BOuP@+x|>re7yI?U@1R%t2TF8F9{ zeIf6?6T7q(?>6|ZW9*EKQ}!Dp+zzjYJF9bW4Sun)_YCIN;JsEp4rh^U97geITm*6Y zOb0-TZYpPB23Y+l6wg5&gcY4PG1OU*orc>Irsrp5mREB0&*feCm#DEhhpUK6y% zbMP(cq|sbtX)f&~JdQ*z^gvCLe28a1>E(W{#EQW&WM=;3J zLS)JAL7E*V5g$cCfrID6NIaMwOEbv4AL}#I7`;rZ0w`PoC2H049 zov5&Ip8HN(xur5&HGP$+UFEF7aGnlm$u4j?IiaSvS(~{~@#5|!s(pUdfZ~#V76t{? za|(u98C0lOO8um#GLi|UvlDX_`=v<=-aD6BPgCjS*|3;~M{5#WN@8&X-;j1?13_n& z21O6K`WOF7!+hOG)l4w(^3pMC_p_GMJ`2=Dmi*payY5XSbR{aEDa#rx!Q1ud6?AF zDuty1B4mg}0bSI(C4_pfl(N!tahQn%Q~;#z{V1Unj@F{c_xg=0m|ECmK8S*86sF=K z~-*&y*p^pfJ>qic7>QB}CM681@o_{pfHUCy^I=`w>K{JUg7K6nMeQ z_t#jKX|foHMI8!#COY`~9=w>c+***E7u;Ifl){4aqDGojVXL^>!wxysSWR!T+m}1{ zihJqYu(qbR`TZ8*ETmd9Mm1$#pEFm@$cA3Mut z?RfcosuJ^>z>Nz6i*I6qpaLZsub3&xiC1X4kmTA`DxOBgIOD$nL*B+jS&sgcXMFjR zNMB#IOm&pL$vu>7yaAxliYj&M>S-L^FO0$@_>{e=eRLaDla=NTwBC!-W2uf3c!Xs&+?vEmABvFcduWFdB4wOqP48W@%U}W({aXVF$6E9Tm z{57_$v$~67waFIVC9*!ZWGO6FIT}jX&j6q=MQ*1w1qindRksiWfY62G4R*({P7TyD zd*YPp)=4B-Ti$8`znSM5;~PlI9-96@10PIX01E-O`<0EuA{W)?Gb#=mcB9;{@~-?95K+ z9&nBD3U0te&R*3CQ>s_(@H2y;x(heMqd4zxi+2e}$`aRT`W8(Tal)fXH7XAJ7;4!C zg_3G+#a)7SX!;IKxc+J|t`OldHPk5@71i^KdqjPKChBZdffwHf)NfY)(H(^79g~!2#9rLL4mr z8YT?^)#>&Try08`xn_?@a6BEgUl=+eoL97kF8S43FlF9lH- zDoA|F!}%5_S9f3Rxoy{Vx~^;Y9M^8&RvZHcD*cGv#(0d8{|OVNZROl>c&;Dl;w509 zI|P+Zfpuo0RpX#ppuJJk5P(bCJD}O1{kEoAYqa~IG0+w@&0eGR3WGbP_7{!2f5nyd z53m?4)u;oCELw+=c#mKeVLt=3us={YhsAAbXwt0kLb^-Dyy^hGbUxjWMBl6f1}#R0 zRwhpKNZ|m^8f$< literal 0 HcmV?d00001 diff --git a/spider_control/__pycache__/leg_dynamics.cpython-36.pyc b/spider_control/__pycache__/leg_dynamics.cpython-36.pyc new file mode 100644 index 0000000000000000000000000000000000000000..f29e93f23fa120e1a5be7850fbbdc0ec1ba77e6d GIT binary patch literal 12265 zcmeHNTaP1074D1M_INzE-PxVnUcC1>K$hLv8xYtfgal9+gz^9wqL#cc_ zcUEnLGT{v&Ug00`&I9}lBwnDEhe&xEA@Rf$LW1v{uCA%}*dFhKf{@adt52Ugb-Jp~ zSEs5^)%wg#X`dFE`3>8lLSHJ~X_-fz_Du%(soYa|m6=M%_%D-Tk1s*`mknQZG!v@%d#9%QNvZ zt5NXmhJ$|*bjd3;%ANz2#YV*|HKw>nDZ*$H16};#+K<-y_pZq z#sbr`-W=#tOwW4@pieV>$~z5uk?BS64CpgVpY@hNpY@i#73f>?R=qXcm%Ve|dE8gF z4MC4^~(C56X-Ze;{_pYN8ZX`2r`P*(YibQ*JFY*sfwD+)p zL@-sPM~f2bk!}2Oef+THq?AE-%Kira;W;F+85^zPsQ9*DSaXhGT+onmUFUGxu;52 zQfZ#f<=eTZYVK(!eL67HR-iLyHv~hoghmQ#wxGT~{c(V7gMWHMq+LzW-Bqhq`K21gS%xBAa zGaU&Hx6(%_`g~SWHe)+ckNh86N|1fT8A{GlvP21uHnBnpjUcf~$r_TnCCH&9cG&Lf zh7vSD#6?PKlw6{OoPai-D*MVxFCk}ym5zjRRw6j@So%AN<5uztF%YSbCya}1L`mzY zXC6mnEydxnl;iODVNN!>cnsw@JdSc49!EJ2j~?PkWE6&tCYd*z%bQD|sO%#7>G|Q~ zjGX-;dELVWO4xeLBkyxaj_i=kdPx5IkSuaYrj~XT=ZV$^+$46Ke(H+$_G2DypF{b~ zkON*(llsh(H(YcU&I(0NwrS$1sety*R46i)GVovf*?&)mqX({)#oIWX3 ztw{gCQ!r1)<@PMP*?q0VYKJNJ4X#3*N z|N8ykUzmDy_&ZdE4O}m!-%snrxQnDw&U|d6oFO<;3>;p%=JMMIiQ z3~Sy5CQj!aGRw56H|U}Ts9FwufZ>3TTZCq?Bu7AI1xueen~Hhzqp6t|>(Y-&-cn8q z{fKFE2_L{T#p&g<9Ne*#$RzNN?2Rawe`8jHbj8_c0Z+=4>P8JR?Re*K?RtIEX}Mn- z*AKZ`Y5kM&b$zsaR5kb%+IM7`QMKup2irEjOuqKxWe0ovBlDp2WnWuTUBzCjaO6-msB(`vGC3fK6N&`bx6%cD9JXVZ+ z(Q$9d`)zq&PYT2z97xqx6EUrJ=u+ml@Zyj{9)|fZW6J!<{3VU5fn|*S zF6TQSJOr}vx1YWofsU~9J}?ig*Nu0U`}huy0T0i5w;+}gRkH767J6^_2N*zJ0le2@ zyKi+1q=7?W&Ib3*HnigU_+c8$p_Ld%=!Scni4(SZ!cUy2 z7d3;VFBRYsec1^`c&5 zHS6-qwTx(ct~L|Ad;FW(Wv0&QKWNkOKG$1B&JFHFYtMN3qZ*Hw(|h$KNp>Mx4HO!( z=#h>l74Orf&>}>Zj-d&%Sq|=qG-VP%>~WiG(z%{jF{@#39Mr3~`y_dZri$MC!^&`N zGBjzm9DMhu(I#ojF}3JV>+KmYe^le~a(b_xBndQC^%|%im2t(KZ&I;FT?+k*h+~G8 zVL6aM1TvEtiY92*^(fz@VwscZ)!TiNyhPK?sJ7>7lc7ng<^Q5h(w1Xt(Vx}ZGhY4} znnvoKToP!S(`%rtAmhzBG^rT0E`@$Y+$+P%upCHWZzq!&iY92*^(Zu{822Q4hoVW- ztk*-LX?|4ObG6CPG(s0IppW}oO*-dqU${*Y9hzRfMLg3j=vG${UyCUT`X{6rCC4r`8y#~q(yh}2GCdt##1P)q+J(LV9 zdig8|ckHWV5<}56phuyJcV}{F(j*7<4n@-ld5NZ#QEkuaoN$1e?!flf1 z(DV-WKz~(l&v^M`7#XQ|a!H_RO|OAM1Mj2_ph@yHG=YN_VV^C-ie5g;!F}aOVknvh z^e8m(4qgsTn&hC~p=cT*FVS>vRNJ$<=g>4l*9bIe(m6D}aGNALG`)j8&_A!YXT1C| zjEvMfxg^kZK?^coj4SaptWrh5u@=w7v!LfdUyA4B1<v?D%3_i!a5O<16vi_*#5@yL@2wZ^Snc(p()G(&R%| z>2U!*N5H$lyr;{1zr1zJySuy#%$vBp&&&J4yq(KCzBdqHOZU`x@0a(8>1059@_@o` z*khs-K;jCL#Om%QPE&~He*L9%*Wm%NO_7*^pwnLB%k*57XA8?|@D$FsWMyO-YXr?&^8V)`4O1|=-QC@behiWD zbtH!QPxFuF2hWjG$W{NXn$OPV_jdBHYSHFDT@<$^TyY%wV-ScU*+bojS1kGoZ_HIj(@71wBP0HlB{F2L*gv8#|1)Id%FRhnV`*1Nnp1(<46JLNJ*Q`9M>=A|Kh3hUEvS^gGfke~Jt%jUB) zd8A1l8}$ox(ig{meb-GYuG{H(djZi^*L`oV8KgPlD|kx-n>3pZ`za;+?xy%E^8$BW z&|Yo>yEp5oGB!5%!nEtZ2DYeBLMP+|pFd^8Olp%yRE!O?LDbun9F0zljwSr2J~yuV15Lycbo5O~@|zS^oxe Cy9Js6 literal 0 HcmV?d00001 diff --git a/spider_control/__pycache__/my_network.cpython-36.pyc b/spider_control/__pycache__/my_network.cpython-36.pyc new file mode 100644 index 0000000000000000000000000000000000000000..32a928f6c5f7d2830aefab3bcb2233486822aac8 GIT binary patch literal 4502 zcma)9Nt4^g74F7Pkh6~#OJj#ooP}95TC!HDY&ml5i;IcnO5{Stg~Fi0Ax0oTH9(DK z3OtQ`R5>{&$5bx4y^1zmQAvy#~l3DajSG(B0^_bie)K?%JCB`(J$e z*@J70{fjO9bgb{=$@>@t6FgxaSF`5nn6<zU`^$@)EBnd6frI0)%=_M0DEU?) zo6*f~IvE{Ak}wFGrFl0_yHOAhyGd{YK#+8`u`XifvHhv@@>co6$QB&JN*-HvFmP>o)tlg24+q{d1_A zk(1`bU>u#386<7R8{fvhQCXuneHtW~gJ(3GqBt2xm61o0h#wh1{Hw#*IUW|9Yar2N zn|J`idY3AzHgjb+E>UU4z_Y?^G!6<+dwSbzkCPybhFKyaS=oam3yQmUDyK&G**wKGLRjT3_`NMFnJx*f9~%Ovr)7^ibI(lWW|0M6p!Q7KhF1a8Sdxf7*aKp zA7*JGvt)mC;x}P)b9_=czK=*Ne18M{@;wZUTijZ^#;@_W@Sn|le2>4)PcJP7?adi& zs1Y50qRT})B{*LU{{A<<)?2g+4d!}d8(w)B10=96Ejl&8T;buR4;WhHU3%*gTK%? zdxYBcF?+~9#SFD-C`evwfZ+8-92PxYX-Rg$MSdLvv`pHlcu?^J&mtlHbXBCy!>E7{ zQ;zqM6DHAJkmRI)#$EX7>6=TEtYU{G0R706M5P4zAr>SEU0&ew$neIL{7QqL=x5rL z{6}y6hdhT*?b3!wdjEqSif)0>&c|6E`N87LLC;hoQUWVRlcdrEAr|Cy7OZbK*7ppG ze9r`W5qky*ZhJaZ;dN$dDIHIWRjqVl+Vw|KG0enIAWcpV%Cv3Xfqh+m`o@x07P05? zOPZl)$+UI!QpKBUl8Y{nK&sPF_j$YPI|rOcvE5VEbKE`Ts*TdYwDZ z|BGLvBzX($DeU>m zDJj_@$srDqxu4xH3>0dPZwmc=_Svmz3ox{VfVc47!9Ldp+8Ye-^i_hlvPJJ_aFeoi zk3Bhf#GXA>9JGeR03{abLztzz%Vy)1hR?KJwvX)6DqZkEw5hY`HcshYoCfZC>E0tK z!CI7<6|PHfB(Wtb)S0<6CAR%tiqY=iR_&T)RnpX@zirGpe|_s>H z%(@3roy8{hdOJv-W6unv3{K>?2#dBW6Se0sl0Dj>mY}kN@iWq_s7Py#i>fyJwzpna74O?`;>Yg|94oWe90h4A9P z&bRq3JbQc{t?oMC#QGZM4!?{&+TG@-J4+#1bDT$_4yN>|NBxBAya-4XfmInCoTw806X55nb>XOW%Z>O9r{7zrj-G0- z?^mwxk1{byXx;JsCzBv)Ahz#|EcAW(CYZ`^(4f3Yd606IAJHD&R2g5sN2{OHaFNi| ziqfVe4Ny+XsS9Lk&jFHRsv9Ojjl2*vkRm#Un5wear{C)Mvd L$gtE4slV|*0Yg6+ literal 0 HcmV?d00001 diff --git a/spider_control/control1.py b/spider_control/control1.py old mode 100644 new mode 100755 index e2a62e8..5dab368 --- a/spider_control/control1.py +++ b/spider_control/control1.py @@ -1,20 +1,19 @@ -#!/usr/bin/python3 +#!/usr/bin/python3 #from my_network import Architecture import my_network as nn -from leg_dynamics import Leg_attribute -import Leg_attribute as ld +import leg_dynamics as ld import knowledge_transfer as kt import tensorflow as tf import numpy as np import roslib roslib.load_manifest('spider_control') -from matplotlib as pyplot -import pyplot as plt +from matplotlib import pyplot +#import pyplot as plt from geometry_msgs import * import rospy, yaml, sys #from osrf_msgs.msg import JointCommands from sensor_msgs.msg import JointState -from joint_states_listener.srv import * +#from joint_states_listener.srv import * #from joint_States_listener.srv import ReturnJointStates import threading import time @@ -46,23 +45,27 @@ def joint_callback(data, args): model2 = nn.Architecture(3, 24) model3 = nn.Architecture(3, 24) model4 = nn.Architecture(3, 24) + velocity = 10*len(data.name) + effort = 10*len(data.name) + leg = ld.Leg_attribute(g_positions, velocity, effort, tactile_output) knowledge = kt.MultiClassLogistic(8, 3) - carollis_inp = ld.carollis_input(g_positions) + carollis_inp = leg.carollis_input(g_positions) knowledge_out = knowledge.run(carollis_inp) model_num = knowledge_out.index(max(knowledge_out)) + reward = leg.leg_run() if(model_num == 0): pub_msg.position = model1.nn_run(g_positions) - model1.nn_learn(g_positions, tactile_output) + model1.nn_learn(reward) elif(model_num == 1): pub_msg.position = model2.nn_run(g_positions) - model2.nn_learn(g_positions, tactile_output) + model2.nn_learn(reward) elif(model_num == 2): pub_msg.position = model3.nn_run(g_positions) - model3.nn_learn(g_positions, tactile_output) + model3.nn_learn(reward) elif(model_num == 3): pub_msg.position = model4.nn_run(g_positions) - model4.nn_learn(g_positions, tactile_output) - if(simulation_mode = 'train'): + model4.nn_learn(reward) + if(simulation_mode == 'train'): knowledge.learn(out, knowledge_out) joint_pub.publish(pub_msg) diff --git a/spider_control/knowledge_transfer.py b/spider_control/knowledge_transfer.py old mode 100644 new mode 100755 index dc7e7be..fb7c928 --- a/spider_control/knowledge_transfer.py +++ b/spider_control/knowledge_transfer.py @@ -1,7 +1,8 @@ +#!/usr/bin/python3 import tensorflow as tf import numpy as np -from matplotlib import pylot -import pyplot as plt +from matplotlib import pyplot +#import pyplot as plt import math import time diff --git a/spider_control/launch/spider_control.launch b/spider_control/launch/spider_control.launch index d882596..92dcf6c 100644 --- a/spider_control/launch/spider_control.launch +++ b/spider_control/launch/spider_control.launch @@ -43,7 +43,7 @@ - + diff --git a/spider_control/leg_dynamics.py b/spider_control/leg_dynamics.py old mode 100644 new mode 100755 index d8ab1fc..8fa66f8 --- a/spider_control/leg_dynamics.py +++ b/spider_control/leg_dynamics.py @@ -1,4 +1,4 @@ -#!/usr/bin/python3 +#!/usr/bin/python3 import tensorflow as tf import numpy as np from matplotlib import pylab @@ -21,17 +21,11 @@ class Leg_attribute: j_velocities = [] #np.ones((1,3)) j_efforts = [] #np.ones((1, 3)) #touch = False #variable to detect whether tactile sensor touch the ground or not - def __init__(self, position1, position2, position3, velocity1, velocity2, velocity3, effort1, effort2, effort3, acceleration): - self.j_angles[0] = position1 # vector containing the joint angles - self.j_angles[1] = position2 - self.j_angles[2] = position3 - self.j_velocities[0] = velocity1 # vector containing joint velocities - self.j_velocities[1] = velocity2 - self.j_velocities[2] = velocity3 - self.j_efforts[0] = effort1 # vector containing efforts of each joint in the leg - self.j_efforts[1] = effort2 - self.j_efforts[2] = effort3 - self.acc = acceleration + def __init__(self, j_angles, velocity, effort, tactile): + self.j_angles = j_angles # vector containing the joint angles + self.velocity = velocity # vector containing joint velocities + self.j_efforts = effort # vector containing efforts of each joint in the leg + self.tactile = tactile def give_angles(self, j_angles): a = j_angles @@ -81,70 +75,70 @@ class Leg_attribute: term_3 = acc*(m_l*9.8*j_velocities[0]*math.sin(j_angles[0]+j_angles[1]+j_angles[2])+m_l*9.8*j_velocities[1]*math.sin(j_angles[0]+j_angles[1]+j_angles[2])+m_l*9.8*j_velocities[2]*math.sin(j_angles[0]+j_angles[1]+j_angles[2])) term = term_1 +term_2 + term_3 return term - -def tactile_run(tactile_sub = []): - score = 0 - total = 0 - for element in range(0, len(tactile_sub)): - if(tactile_sub[element]>0.5): - total +=1 - if(total>3): - score = total - else: - score = 0 - return score - -def carollis_input(m, r, l, velocity, acc, angles = []): - term_1_1 = acc*(m*9.8*velocity*(r*math.sin(angles[0])+2*l*math.sin(angles[0])+r*math.sin(angles[0]+angles[1])+r*math.sin(angles[0]+angles[1]+angles[2]))+m*9.8*velocity*(r*math.sin(angles[0]+angles[1])+l*math.sin(angles[1])+r*math.sin(angles[0]+angles[1]+angles[2]))+m*9.8*velocity*(r*math.sin(angles[0]+angles[1]+angles[2]))) - term_1_2 = acc*(m*9.8*velocity*(r*math.sin(angles[0]+angles[1])+r*math.sin(angles[0]+angles[1]+angles[2]))+m*9.8*velocity*(r*math.sin(angles[0]+angles[1])+l*math.sin(angles[1])+r*math.sin(angles[0]+angles[1]+angles[2]))+m*9.8*velocity*math.sin(angles[0]+angles[1]+angles[2])) - term_1_3 = acc*(m*9.8*velocity*math.sin(angles[0]+angles[1]+angles[2])+m*9.8*velocity*math.sin(angles[0]+angles[1]+angles[2])+m*9.8*velocity*math.sin(angles[0]+angles[1]+angles[2])) - term_2_1 = acc*(m*9.8*velocity*(r*math.sin(angles[3])+2*l*math.sin(angles[3])+r*math.sin(angles[3]+angles[4])+r*math.sin(angles[3]+angles[4]+angles[5]))+m*9.8*velocity*(r*math.sin(angles[3]+angles[4])+l*math.sin(angles[4])+r*math.sin(angles[3]+angles[4]+angles[5]))+m*9.8*velocity*(r*math.sin(angles[3]+angles[4]+angles[5]))) - term_2_2 = acc*(m*9.8*velocity*(r*math.sin(angles[3]+angles[4])+r*math.sin(angles[3]+angles[4]+angles[5]))+m*9.8*velocity*(r*math.sin(angles[3]+angles[4])+l*math.sin(angles[4])+r*math.sin(angles[3]+angles[4]+angles[5]))+m*9.8*velocity*math.sin(angles[3]+angles[4]+angles[5])) - term_2_3 = acc*(m*9.8*velocity*math.sin(angles[3]+angles[4]+angles[5])+m*9.8*velocity*math.sin(angles[3]+angles[4]+angles[5])+m*9.8*velocity*math.sin(angles[3]+angles[4]+angles[5])) - term_3_1 = acc*(m*9.8*velocity*(r*math.sin(angles[6])+2*l*math.sin(angles[6])+r*math.sin(angles[6]+angles[7])+r*math.sin(angles[6]+angles[7]+angles[8]))+m*9.8*velocity*(r*math.sin(angles[6]+angles[7])+l*math.sin(angles[7])+r*math.sin(angles[6]+angles[7]+angles[8]))+m*9.8*velocity*(r*math.sin(angles[6]+angles[7]+angles[8]))) - term_3_2 = acc*(m*9.8*velocity*(r*math.sin(angles[6]+angles[7])+r*math.sin(angles[6]+angles[7]+angles[8]))+m*9.8*velocity*(r*math.sin(angles[6]+angles[7])+l*math.sin(angles[7])+r*math.sin(angles[6]+angles[7]+angles[8]))+m*9.8*velocity*math.sin(angles[6]+angles[7]+angles[8])) - term_3_3 = acc*(m*9.8*velocity*math.sin(angles[6]+angles[7]+angles[8])+m*9.8*velocity*math.sin(angles[6]+angles[7]+angles[8])+m*9.8*velocity*math.sin(angles[6]+angles[7]+angles[8])) - term_4_1 = acc*(m*9.8*velocity*(r*math.sin(angles[9])+2*l*math.sin(angles[9])+r*math.sin(angles[9]+angles[10])+r*math.sin(angles[9]+angles[10]+angles[11]))+m*9.8*velocity*(r*math.sin(angles[9]+angles[10])+l*math.sin(angles[10])+r*math.sin(angles[9]+angles[10]+angles[11]))+m*9.8*velocity*(r*math.sin(angles[9]+angles[10]+angles[11]))) - term_4_2 = acc*(m*9.8*velocity*(r*math.sin(angles[9]+angles[10])+r*math.sin(angles[9]+angles[10]+angles[11]))+m*9.8*velocity*(r*math.sin(angles[9]+angles[10])+l*math.sin(angles[10])+r*math.sin(angles[9]+angles[10]+angles[11]))+m*9.8*velocity*math.sin(angles[9]+angles[10]+angles[11])) - term_4_3 = acc*(m*9.8*velocity*math.sin(angles[9]+angles[10]+angles[11])+m*9.8*velocity*math.sin(angles[9]+angles[10]+angles[11])+m*9.8*velocity*math.sin(angles[9]+angles[10]+angles[11])) - term_5_1 = acc*(m*9.8*velocity*(r*math.sin(angles[12])+2*l*math.sin(angles[12])+r*math.sin(angles[12]+angles[13])+r*math.sin(angles[12]+angles[13]+angles[14]))+m*9.8*velocity*(r*math.sin(angles[12]+angles[13])+l*math.sin(angles[13])+r*math.sin(angles[12]+angles[13]+angles[14]))+m*9.8*velocity*(r*math.sin(angles[12]+angles[13]+angles[14]))) - term_5_2 = acc*(m*9.8*velocity*(r*math.sin(angles[12]+angles[13])+r*math.sin(angles[12]+angles[13]+angles[14]))+m*9.8*velocity*(r*math.sin(angles[12]+angles[13])+l*math.sin(angles[13])+r*math.sin(angles[12]+angles[13]+angles[14]))+m*9.8*velocity*math.sin(angles[12]+angles[13]+angles[14])) - term_5_3 = acc*(m*9.8*velocity*math.sin(angles[12]+angles[13]+angles[14])+m*9.8*velocity*math.sin(angles[12]+angles[13]+angles[14])+m*9.8*velocity*math.sin(angles[12]+angles[13]+angles[14])) - term_6_1 = acc*(m*9.8*velocity*(r*math.sin(angles[15])+2*l*math.sin(angles[15])+r*math.sin(angles[15]+angles[16])+r*math.sin(angles[16]+angles[16]+angles[17]))+m*9.8*velocity*(r*math.sin(angles[15]+angles[16])+l*math.sin(angles[16])+r*math.sin(angles[15]+angles[16]+angles[17]))+m*9.8*velocity*(r*math.sin(angles[15]+angles[16]+angles[17]))) - term_6_2 = acc*(m*9.8*velocity*(r*math.sin(angles[15]+angles[16])+r*math.sin(angles[15]+angles[16]+angles[17]))+m*9.8*velocity*(r*math.sin(angles[15]+angles[16])+l*math.sin(angles[16])+r*math.sin(angles[15]+angles[16]+angles[17]))+m*9.8*velocity*math.sin(angles[15]+angles[16]+angles[17])) - term_6_3 = acc*(m*9.8*velocity*math.sin(angles[15]+angles[16]+angles[17])+m*9.8*velocity*math.sin(angles[15]+angles[16]+angles[17])+m*9.8*velocity*math.sin(angles[15]+angles[16]+angles[17])) - term_7_1 = acc*(m*9.8*velocity*(r*math.sin(angles[18])+2*l*math.sin(angles[18])+r*math.sin(angles[18]+angles[19])+r*math.sin(angles[18]+angles[19]+angles[20]))+m*9.8*velocity*(r*math.sin(angles[18]+angles[19])+l*math.sin(angles[19])+r*math.sin(angles[18]+angles[19]+angles[20]))+m*9.8*velocity*(r*math.sin(angles[18]+angles[19]+angles[20]))) - term_7_2 = acc*(m*9.8*velocity*(r*math.sin(angles[18]+angles[19])+r*math.sin(angles[18]+angles[19]+angles[20]))+m*9.8*velocity*(r*math.sin(angles[18]+angles[19])+l*math.sin(angles[19])+r*math.sin(angles[18]+angles[19]+angles[20]))+m*9.8*velocity*math.sin(angles[18]+angles[19]+angles[20])) - term_7_3 = acc*(m*9.8*velocity*math.sin(angles[18]+angles[19]+angles[20])+m*9.8*velocity*math.sin(angles[18]+angles[19]+angles[20])+m*9.8*velocity*math.sin(angles[18]+angles[19]+angles[20])) - term_8_1 = acc*(m*9.8*velocity*(r*math.sin(angles[21])+2*l*math.sin(angles[21])+r*math.sin(angles[21]+angles[22])+r*math.sin(angles[21]+angles[22]+angles[23]))+m*9.8*velocity*(r*math.sin(angles[21]+angles[22])+l*math.sin(angles[22])+r*math.sin(angles[21]+angles[22]+angles[23]))+m*9.8*velocity*(r*math.sin(angles[21]+angles[22]+angles[23]))) - term_8_2 = acc*(m*9.8*velocity*(r*math.sin(angles[21]+angles[22])+r*math.sin(angles[21]+angles[22]+angles[23]))+m*9.8*velocity*(r*math.sin(angles[21]+angles[22])+l*math.sin(angles[22])+r*math.sin(angles[21]+angles[22]+angles[23]))+m*9.8*velocity*math.sin(angles[21]+angles[22]+angles[23])) - term_8_3 = acc*(m*9.8*velocity*math.sin(angles[21]+angles[22]+angles[23])+m*9.8*velocity*math.sin(angles[21]+angles[22]+angles[23])+m*9.8*velocity*math.sin(angles[21]+angles[22]+angles[23])) - term1 = term_1_1 +term_1_2 + term_1_3 - term2 = term_2_1+term_2_2+term_2_3 - term3 = term_3_1+term_3_2+term_3_3 - term4 = term_4_1+term_4_2+term_4_3 - term5 = term_5_1+term_5_2+term_5_3 - term6 = term_6_1+term_6_2+term_6_3 - term7 = term_7_1+term_7_2+term_7_3 - term8 = term_8_1+term_8_2+term_8_3 - term = np.array([term1, term2, term3, term4, term5, term6, term7, term8]) - return term -def leg_run(angles_joints = [], tactile_scores): + + def tactile_run(self, tactile_sub = []): + score = 0 + total = 0 + for element in range(0, len(tactile_sub)): + if(tactile_sub[element]>0.5): + total +=1 + if(total>3): + score = total + else: + score = 0 + return score + + def carollis_input(self): + term_1_1 = (self.m_l*9.8*self.velocity*(self.r_1*math.sin(self.j_angles[0])+2*self.l_1*math.sin(self.j_angles[0])+self.r_1*math.sin(self.j_angles[0]+self.j_angles[1])+self.r_1*math.sin(self.j_angles[0]+self.j_angles[1]+self.j_angles[2]))+self.m_l*9.8*self.velocity*(self.r_1*math.sin(self.j_angles[0]+self.j_angles[1])+self.l_1*math.sin(self.j_angles[1])+self.r_1*math.sin(self.j_angles[0]+self.j_angles[1]+self.j_angles[2]))+self.m_l*9.8*self.velocity*(self.r_1*math.sin(self.j_angles[0]+self.j_angles[1]+self.j_angles[2]))) + term_1_2 = (self.m_l*9.8*self.velocity*(self.r_1*math.sin(self.j_angles[0]+self.j_angles[1])+self.r_1*math.sin(self.j_angles[0]+self.j_angles[1]+self.j_angles[2]))+self.m_l*9.8*self.velocity*(self.r_1*math.sin(self.j_angles[0]+self.j_angles[1])+self.l_1*math.sin(self.j_angles[1])+self.r_1*math.sin(self.j_angles[0]+self.j_angles[1]+self.j_angles[2]))+self.m_l*9.8*self.velocity*math.sin(self.j_angles[0]+self.j_angles[1]+self.j_angles[2])) + term_1_3 = (self.m_l*9.8*self.velocity*math.sin(self.j_angles[0]+self.j_angles[1]+self.j_angles[2])+self.m_l*9.8*self.velocity*math.sin(self.j_angles[0]+self.j_angles[1]+self.j_angles[2])+self.m_l*9.8*self.velocity*math.sin(self.j_angles[0]+self.j_angles[1]+self.j_angles[2])) + term_2_1 = (self.m_l*9.8*self.velocity*(self.r_1*math.sin(self.j_angles[3])+2*self.l_1*math.sin(self.j_angles[3])+self.r_1*math.sin(self.j_angles[3]+self.j_angles[4])+self.r_1*math.sin(self.j_angles[3]+self.j_angles[4]+self.j_angles[5]))+self.m_l*9.8*self.velocity*(self.r_1*math.sin(self.j_angles[3]+self.j_angles[4])+self.l_1*math.sin(self.j_angles[4])+self.r_1*math.sin(self.j_angles[3]+self.j_angles[4]+self.j_angles[5]))+self.m_l*9.8*self.velocity*(self.r_1*math.sin(self.j_angles[3]+self.j_angles[4]+self.j_angles[5]))) + term_2_2 = (self.m_l*9.8*self.velocity*(self.r_1*math.sin(self.j_angles[3]+self.j_angles[4])+self.r_1*math.sin(self.j_angles[3]+self.j_angles[4]+self.j_angles[5]))+self.m_l*9.8*self.velocity*(self.r_1*math.sin(self.j_angles[3]+self.j_angles[4])+self.l_1*math.sin(self.j_angles[4])+self.r_1*math.sin(self.j_angles[3]+self.j_angles[4]+self.j_angles[5]))+self.m_l*9.8*self.velocity*math.sin(self.j_angles[3]+self.j_angles[4]+self.j_angles[5])) + term_2_3 = (self.m_l*9.8*self.velocity*math.sin(self.j_angles[3]+self.j_angles[4]+self.j_angles[5])+self.m_l*9.8*self.velocity*math.sin(self.j_angles[3]+self.j_angles[4]+self.j_angles[5])+self.m_l*9.8*self.velocity*math.sin(self.j_angles[3]+self.j_angles[4]+self.j_angles[5])) + term_3_1 = (self.m_l*9.8*self.velocity*(self.r_1*math.sin(self.j_angles[6])+2*self.l_1*math.sin(self.j_angles[6])+self.r_1*math.sin(self.j_angles[6]+self.j_angles[7])+self.r_1*math.sin(self.j_angles[6]+self.j_angles[7]+self.j_angles[8]))+self.m_l*9.8*self.velocity*(self.r_1*math.sin(self.j_angles[6]+self.j_angles[7])+self.l_1*math.sin(self.j_angles[7])+self.r_1*math.sin(self.j_angles[6]+self.j_angles[7]+self.j_angles[8]))+self.m_l*9.8*self.velocity*(self.r_1*math.sin(self.j_angles[6]+self.j_angles[7]+self.j_angles[8]))) + term_3_2 = (self.m_l*9.8*self.velocity*(self.r_1*math.sin(self.j_angles[6]+self.j_angles[7])+self.r_1*math.sin(self.j_angles[6]+self.j_angles[7]+self.j_angles[8]))+self.m_l*9.8*self.velocity*(self.r_1*math.sin(self.j_angles[6]+self.j_angles[7])+self.l_1*math.sin(self.j_angles[7])+self.r_1*math.sin(self.j_angles[6]+self.j_angles[7]+self.j_angles[8]))+self.m_l*9.8*self.velocity*math.sin(self.j_angles[6]+self.j_angles[7]+self.j_angles[8])) + term_3_3 = (self.m_l*9.8*self.velocity*math.sin(self.j_angles[6]+self.j_angles[7]+self.j_angles[8])+self.m_l*9.8*self.velocity*math.sin(self.j_angles[6]+self.j_angles[7]+self.j_angles[8])+self.m_l*9.8*self.velocity*math.sin(self.j_angles[6]+self.j_angles[7]+self.j_angles[8])) + term_4_1 = (self.m_l*9.8*self.velocity*(self.r_1*math.sin(self.j_angles[9])+2*self.l_1*math.sin(self.j_angles[9])+self.r_1*math.sin(self.j_angles[9]+self.j_angles[10])+self.r_1*math.sin(self.j_angles[9]+self.j_angles[10]+self.j_angles[11]))+self.m_l*9.8*self.velocity*(self.r_1*math.sin(self.j_angles[9]+self.j_angles[10])+self.l_1*math.sin(self.j_angles[10])+self.r_1*math.sin(self.j_angles[9]+self.j_angles[10]+self.j_angles[11]))+self.m_l*9.8*self.velocity*(self.r_1*math.sin(self.j_angles[9]+self.j_angles[10]+self.j_angles[11]))) + term_4_2 = (self.m_l*9.8*self.velocity*(self.r_1*math.sin(self.j_angles[9]+self.j_angles[10])+self.r_1*math.sin(self.j_angles[9]+self.j_angles[10]+self.j_angles[11]))+self.m_l*9.8*self.velocity*(self.r_1*math.sin(self.j_angles[9]+self.j_angles[10])+self.l_1*math.sin(self.j_angles[10])+self.r_1*math.sin(self.j_angles[9]+self.j_angles[10]+self.j_angles[11]))+self.m_l*9.8*self.velocity*math.sin(self.j_angles[9]+self.j_angles[10]+self.j_angles[11])) + term_4_3 = (self.m_l*9.8*self.velocity*math.sin(self.j_angles[9]+self.j_angles[10]+self.j_angles[11])+self.m_l*9.8*self.velocity*math.sin(self.j_angles[9]+self.j_angles[10]+self.j_angles[11])+self.m_l*9.8*self.velocity*math.sin(self.j_angles[9]+self.j_angles[10]+self.j_angles[11])) + term_5_1 = (self.m_l*9.8*self.velocity*(self.r_1*math.sin(self.j_angles[12])+2*self.l_1*math.sin(self.j_angles[12])+self.r_1*math.sin(self.j_angles[12]+self.j_angles[13])+self.r_1*math.sin(self.j_angles[12]+self.j_angles[13]+self.j_angles[14]))+self.m_l*9.8*self.velocity*(self.r_1*math.sin(self.j_angles[12]+self.j_angles[13])+self.l_1*math.sin(self.j_angles[13])+self.r_1*math.sin(self.j_angles[12]+self.j_angles[13]+self.j_angles[14]))+self.m_l*9.8*self.velocity*(self.r_1*math.sin(self.j_angles[12]+self.j_angles[13]+self.j_angles[14]))) + term_5_2 = (self.m_l*9.8*self.velocity*(self.r_1*math.sin(self.j_angles[12]+self.j_angles[13])+self.r_1*math.sin(self.j_angles[12]+self.j_angles[13]+self.j_angles[14]))+self.m_l*9.8*self.velocity*(self.r_1*math.sin(self.j_angles[12]+self.j_angles[13])+self.l_1*math.sin(self.j_angles[13])+self.r_1*math.sin(self.j_angles[12]+self.j_angles[13]+self.j_angles[14]))+self.m_l*9.8*self.velocity*math.sin(self.j_angles[12]+self.j_angles[13]+self.j_angles[14])) + term_5_3 = (self.m_l*9.8*self.velocity*math.sin(self.j_angles[12]+self.j_angles[13]+self.j_angles[14])+self.m_l*9.8*self.velocity*math.sin(self.j_angles[12]+self.j_angles[13]+self.j_angles[14])+self.m_l*9.8*self.velocity*math.sin(self.j_angles[12]+self.j_angles[13]+self.j_angles[14])) + term_6_1 = (self.m_l*9.8*self.velocity*(self.r_1*math.sin(self.j_angles[15])+2*self.l_1*math.sin(self.j_angles[15])+self.r_1*math.sin(self.j_angles[15]+self.j_angles[16])+self.r_1*math.sin(self.j_angles[15]+self.j_angles[16]+self.j_angles[17]))+self.m_l*9.8*self.velocity*(self.r_1*math.sin(self.j_angles[15]+self.j_angles[16])+self.l_1*math.sin(self.j_angles[16])+self.r_1*math.sin(self.j_angles[15]+self.j_angles[16]+self.j_angles[17]))+self.m_l*9.8*self.velocity*(self.r_1*math.sin(self.j_angles[15]+self.j_angles[16]+self.j_angles[17]))) + term_6_2 = (self.m_l*9.8*self.velocity*(self.r_1*math.sin(self.j_angles[15]+self.j_angles[16])+self.r_1*math.sin(self.j_angles[15]+self.j_angles[16]+self.j_angles[17]))+self.m_l*9.8*self.velocity*(self.r_1*math.sin(self.j_angles[15]+self.j_angles[16])+self.l_1*math.sin(self.j_angles[16])+self.r_1*math.sin(self.j_angles[15]+self.j_angles[16]+self.j_angles[17]))+self.m_l*9.8*self.velocity*math.sin(self.j_angles[15]+self.j_angles[16]+self.j_angles[17])) + term_6_3 = (self.m_l*9.8*self.velocity*math.sin(self.j_angles[15]+self.j_angles[16]+self.j_angles[17])+self.m_l*9.8*self.velocity*math.sin(self.j_angles[15]+self.j_angles[16]+self.j_angles[17])+self.m_l*9.8*self.velocity*math.sin(self.j_angles[15]+self.j_angles[16]+self.j_angles[17])) + term_7_1 = (self.m_l*9.8*self.velocity*(self.r_1*math.sin(self.j_angles[18])+2*self.l_1*math.sin(self.j_angles[18])+self.r_1*math.sin(self.j_angles[18]+self.j_angles[19])+self.r_1*math.sin(self.j_angles[18]+self.j_angles[19]+self.j_angles[20]))+self.m_l*9.8*self.velocity*(self.r_1*math.sin(self.j_angles[18]+self.j_angles[19])+self.l_1*math.sin(self.j_angles[19])+self.r_1*math.sin(self.j_angles[18]+self.j_angles[19]+self.j_angles[20]))+self.m_l*9.8*self.velocity*(self.r_1*math.sin(self.j_angles[18]+self.j_angles[19]+self.j_angles[20]))) + term_7_2 = (self.m_l*9.8*self.velocity*(self.r_1*math.sin(self.j_angles[18]+self.j_angles[19])+self.r_1*math.sin(self.j_angles[18]+self.j_angles[19]+self.j_angles[20]))+self.m_l*9.8*self.velocity*(self.r_1*math.sin(self.j_angles[18]+self.j_angles[19])+self.l_1*math.sin(self.j_angles[19])+self.r_1*math.sin(self.j_angles[18]+self.j_angles[19]+self.j_angles[20]))+self.m_l*9.8*self.velocity*math.sin(self.j_angles[18]+self.j_angles[19]+self.j_angles[20])) + term_7_3 = (self.m_l*9.8*self.velocity*math.sin(self.j_angles[18]+self.j_angles[19]+self.j_angles[20])+self.m_l*9.8*self.velocity*math.sin(self.j_angles[18]+self.j_angles[19]+self.j_angles[20])+self.m_l*9.8*self.velocity*math.sin(self.j_angles[18]+self.j_angles[19]+self.j_angles[20])) + term_8_1 = (self.m_l*9.8*self.velocity*(self.r_1*math.sin(self.j_angles[21])+2*self.l_1*math.sin(self.j_angles[21])+self.r_1*math.sin(self.j_angles[21]+self.j_angles[22])+self.r_1*math.sin(self.j_angles[21]+self.j_angles[22]+self.j_angles[23]))+self.m_l*9.8*self.velocity*(self.r_1*math.sin(self.j_angles[21]+self.j_angles[22])+self.l_1*math.sin(self.j_angles[22])+self.r_1*math.sin(self.j_angles[21]+self.j_angles[22]+self.j_angles[23]))+self.m_l*9.8*self.velocity*(self.r_1*math.sin(self.j_angles[21]+self.j_angles[22]+self.j_angles[23]))) + term_8_2 = (self.m_l*9.8*self.velocity*(self.r_1*math.sin(self.j_angles[21]+self.j_angles[22])+self.r_1*math.sin(self.j_angles[21]+self.j_angles[22]+self.j_angles[23]))+self.m_l*9.8*self.velocity*(self.r_1*math.sin(self.j_angles[21]+self.j_angles[22])+self.l_1*math.sin(self.j_angles[22])+self.r_1*math.sin(self.j_angles[21]+self.j_angles[22]+self.j_angles[23]))+self.m_l*9.8*self.velocity*math.sin(self.j_angles[21]+self.j_angles[22]+self.j_angles[23])) + term_8_3 = (self.m_l*9.8*self.velocity*math.sin(self.j_angles[21]+self.j_angles[22]+self.j_angles[23])+self.m_l*9.8*self.velocity*math.sin(self.j_angles[21]+self.j_angles[22]+self.j_angles[23])+self.m_l*9.8*self.velocity*math.sin(self.j_angles[21]+self.j_angles[22]+self.j_angles[23])) + term1 = term_1_1 +term_1_2 + term_1_3 + term2 = term_2_1+term_2_2+term_2_3 + term3 = term_3_1+term_3_2+term_3_3 + term4 = term_4_1+term_4_2+term_4_3 + term5 = term_5_1+term_5_2+term_5_3 + term6 = term_6_1+term_6_2+term_6_3 + term7 = term_7_1+term_7_2+term_7_3 + term8 = term_8_1+term_8_2+term_8_3 + term = np.array([term1, term2, term3, term4, term5, term6, term7, term8]) + return term + def leg_run(self): m = 0.3 g = 9.8 r = 0.1 l = 0.4 L = 0.6 M = 2.8 - x_l_com = Leg_attribute.x_left_com(M/2, g, r, l, angles_joints) - y_l_com = Leg_attribute.y_left_com(M/2, g, r, l, angles_joints) - x_r_com = Leg_attribute.x_right_com(M/2, g, r, l, angles_joints) - y_r_com = Leg_attribute.y_right_com(M/2, g, r, l, angles_joints) - x_s_com = Leg_attribute.x_system_com(M, g, r, l, angles_joints) - y_s_com = Leg_attribute.y_system_com(M, g, r, l, angles_joints) - x_m_com = Leg_attribute.mid_point_x(x_l_com, x_r_com) - y_m_com = Leg_attribute.mid_point_y(y_l_com, y_r_com) - reward_stability = Leg_attribute.slope(x_m_com, y_m_com, x_s_com, y_s_com) - reward_tactile = tactile_run(tactile_scores) + x_l_com = self.x_left_com(M/2, g, r, l, self.j_angles) + y_l_com = self.y_left_com(M/2, g, r, l, self.j_angles) + x_r_com = self.x_right_com(M/2, g, r, l, self.j_angles) + y_r_com = self.y_right_com(M/2, g, r, l, self.j_angles) + x_s_com = self.x_system_com(M, g, r, l, self.j_angles) + y_s_com = self.y_system_com(M, g, r, l, self.j_angles) + x_m_com = self.mid_point_x(x_l_com, x_r_com) + y_m_com = self.mid_point_y(y_l_com, y_r_com) + reward_stability = self.slope(x_m_com, y_m_com, x_s_com, y_s_com) + reward_tactile = self.tactile_run(self.tactile) reward = reward_stability+reward_tactile return reward \ No newline at end of file diff --git a/spider_control/my_network.py b/spider_control/my_network.py old mode 100644 new mode 100755 index f6465f7..d79aeb5 --- a/spider_control/my_network.py +++ b/spider_control/my_network.py @@ -1,4 +1,4 @@ -#!/usr/bin/python3 +#!/usr/bin/python3 import tensorflow as tf import numpy as np from matplotlib import pylab @@ -110,9 +110,9 @@ class Architecture: self.get_neuron_value(self.neuron_o, input_layerout) output_layerout = self.output_function(self.neuron_o) # output of the neural network return output_layerout - def nn_learn(self, angles = []): + def nn_learn(self, rew): Learningrate = 0.1 - self.NN_learningReward(Learningrate, ld.leg_run(angles)) + self.NN_learningReward(Learningrate, rew) -- GitLab