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
Binary files /dev/null and b/spider_control/__pycache__/knowledge_transfer.cpython-36.pyc differ
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
Binary files /dev/null and b/spider_control/__pycache__/leg_dynamics.cpython-36.pyc differ
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
Binary files /dev/null and b/spider_control/__pycache__/my_network.cpython-36.pyc differ
diff --git a/spider_control/control1.py b/spider_control/control1.py
old mode 100644
new mode 100755
index e2a62e821abcf7cfe1e2de1f2df37afb66fbabeb..5dab368895a7ca7cf33416b12d9076f8e5eff2d1
--- 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 dc7e7be4e41a8cd74c00cbde07c0ce72b2aadd0d..fb7c928b3a0b1991a19b9e1b6c60a4cd9f3b19cf
--- 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 d882596cbe7acc9632659aba82f2027fd907ffb7..92dcf6c6fe2fe6a22d906cae8d3e0b6d875336ec 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 d8ab1fc645dba7df7dfcb2df72ce0f654f27e689..8fa66f8a92d8bd6274bc0fd674fa31e4e7a70157
--- 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 f6465f723dd4821aa5268aec02695349b38ffe60..d79aeb5fecad5ea9ef1eb037695ef738bb81018f
--- 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)