diff --git a/spider_control/control.py b/spider_control/control.py index 5513c3259322b8ed9650078abddf5719784ebb2e..e842377d45917f88db3caeb168440d09719b6eae 100644 --- a/spider_control/control.py +++ b/spider_control/control.py @@ -6,6 +6,7 @@ roslib.load_manifest('joint_states_listener') roslib.load_manifest('spider_control') import pylab as plt import math +import threading from geometry_msgs.msgs import Pose, Point import rospy, yaml, sys from osrf_msgs.msg import JointCommands @@ -25,7 +26,7 @@ class LatestJointStates: def __init__(self): rospy.init_node('Joint state Listener') - self.lock = threading.lock() + self.lock = threading.Rlock() self.name = [] self.position = [] self.velocity = [] @@ -58,9 +59,9 @@ class LatestJointStates: self.lock.acquire() if joint_name in self.name: index = self.name.index(joint_name) - position = self.position(index) - velocity = self.velocity(index) - effort = self.effort(index) + position = self.position[index] + velocity = self.velocity[index] + effort = self.effort[index] #joint name not found else: @@ -110,7 +111,7 @@ def listener(): tactile_7 = tactile_sub[6] tactile_8 = tactile_sub[7] rospy.spin() - return 0 + return (tactile_1, tactile_2, tactile_3, tactile_4, tactile_5, tactile_6, tactile_7, tactile_8) @@ -131,14 +132,15 @@ def talker(jointname , position): pub1 = rospy.Publisher('joint_states', JointState, queue_size = 10) rospy.init_node('joint_state_publisher') rate = rospy.Rate(10) - robot_configuration = JointState() - robot_configuration.header = Header() - robot_configuration.header.stamp = rospy.Time.now() - robot_configuration.name = [jointname] - robot_configuration.position = [position] - robot_configuration.velocity = [] - robot_configuration.effort = [] - pub1.publish(robot_configuration) + while not rospy.is_shutdown(): + robot_configuration = JointState() + robot_configuration.header = Header() + robot_configuration.header.stamp = rospy.Time.now() + robot_configuration.name = [jointname] + robot_configuration.position = [position] + robot_configuration.velocity = [] + robot_configuration.effort = [] + pub1.publish(robot_configuration) rate.sleep() @@ -158,7 +160,7 @@ class Leg_attribute: j_angles = np.ones((1, 3)) # in radians j_velocities = np.ones((1,3)) j_efforts = np.ones((1, 3)) - bool touch = False #variable to detect whether tactile sensor touch the ground or not + #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 @@ -170,20 +172,12 @@ class Leg_attribute: self.j_efforts[1] = effort2 self.j_efforts[2] = effort3 self.acc = acceleration - self.touch = bool - def give_angles(self): + def give_angles(self, j_angles): a = j_angles return a - def set_touch(a): - if a >= 1: - touch = True - else: - touch = False - return - def x_left_com(self, a, b, c, d, one_one, one_two, one_three, two_one, two_two, two_three, three_one, three_two, three_three, four_one, four_two, four_three, five_one, five_two, five_three, six_one, six_two, six_three, seven_one, seven_two, seven_three, eight_one, eight_two, eight_three): return (1/(2*(a+6*b)))*(a*d+2*b*c(3*(math.cos(one_three)+math.cos(three_three)+math.cos(five_three)+math.cos(seven_three))+2*(math.cos(one_two)+math.cos(three_two)+math.cos(five_two)+math.cos(seven_two))+math.cos(one_one)+math.cos(three_one)+math.cos(five_one)+math.cos(seven_one))) @@ -206,12 +200,12 @@ class Leg_attribute: def mid_point_x(self, x_left, x_right): 'in order to calculate whether the syste com is normal to the line between left and right com' - x_mid = (x_right - x_left)/2 + x_mid = (x_right + x_left)/2 return x_mid def mid_point_y(self, y_left, y_right): 'in order to calculate whether the system com is normal to the line between left and right com' - y_mid = (y_right - y_left)/2 + y_mid = (y_right + y_left)/2 return y_mid def slope(self, x_mid, y_mid, x_system, y_system): @@ -221,7 +215,7 @@ class Leg_attribute: return m_radian #calculates carolis term for each leg - def carolis_term(self, m_l, r_1, l_1, j_angles[], j_velocities[], acc): + def carolis_term(self, acc, m_l, r_1, l_1, j_angles=[], j_velocities=[]): term_1 = acc*(m_l*9.8*j_velocities[0]*(r_1*math.sin(j_angles[0])+2*l_1*math.sin(j_angles[0])+r_1*math.sin(j_angles[0]+j_angles[1])+r_1*math.sin(j_angles[0]+j_angles[1]+j_angles[2]))+m_l*9.8*j_velocities[1]*(r_1*math.sin(j_angles[0]+j_angles[1])+l_1*math.sin(j_angles[1])+r_1*math.sin(j_angles[0]+j_angles[1]+j_angles[2]))+m_l*9.8*j_velocities[2]*(r_1*math.sin(j_angles[0]+j_angles[1]+j_angles[2]))) term_2 = acc*(m_l*9.8*j_velocities[0]*(r_1*math.sin(j_angles[0]+j_angles[1])+r_1*math.sin(j_angles[0]+j_angles[1]+j_angles[2]))+m_l*9.8*j_velocities[1]*(r_1*math.sin(j_angles[0]+j_angles[1])+l_1*math.sin(j_angles[1])+r_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_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])) @@ -420,7 +414,7 @@ class MultiClassLogistic: #function to normalize the input data -def normalization(a = []): +def normalization(a): a_min = min(a) a_max = max(a) for i in range(len(a)): @@ -457,9 +451,42 @@ if __name__=="__main__": model_4 = Architecture(3, 24) #for model four of the terrain run_method = raw_input("How do you want to run the algorithm ? for training, type train. for testing, type test") knowledge_transfer = MultiClassLogistic(8, 3) + g = 9.8 + reward_history = np.array([0]) + prev_reward = 0 + m = Leg_attribute.m_l + r = Leg_attribute.r_1 + l = Leg_attribute.l_1 global run_time global model_trained run_time_time = raw_input("please enter how long you intend to run the robot") + 'giving initial angles ' + initial_angles = np.random.randint(0, 3014, size=24) + number = 0 + for spiderJoint1Name in spiderJoint1Names: + talker(spiderJoint1Name, initial_angles[number]) + number+=1 + for spiderJoint2Name in spiderJoint2Names: + talker(spiderJoint2Name, initial_angles[number]) + number+=1 + for spiderJoint3Name in spiderJoint3Names: + talker(spiderJoint3Name, initial_angles[number]) + number+=1 + for spiderJoint4Name in spiderJoint4Names: + talker(spiderJoint4Name, initial_angles[number]) + number+=1 + for spiderJoint5Name in spiderJoint5Names: + talker(spiderJoint5Name, initial_angles[number]) + number+=1 + for spiderJoint6Name in spiderJoint6Names: + talker(spiderJoint6Name, initial_angles[number]) + number+=1 + for spiderJoint7Name in spiderJoint7Names: + talker(spiderJoint7Name, initial_angles[number]) + number+=1 + for spiderJoint8Name in spiderJoint8Names: + talker(spiderJoint8Name, initial_angles[number]) + number+=1 if ( run_method == 'train'): training_time = raw_input("please enter how long you intend to train the algorithm") run_time = training_time @@ -493,15 +520,29 @@ if __name__=="__main__": for spiderJoint8Name in spiderJoint8Names: (position, velocity, effort) = call_return_joint_states(spiderJoint8Name) leg_8 = Leg_attribute(position, velocity, effort) - listener() - leg_1.set_touch(tactile_1) - leg_2.set_touch(tactile_2) - leg_3.set_touch(tactile_3) - leg_4.set_touch(tactile_4) - leg_5.set_touch(tactile_5) - leg_6.set_touch(tactile_6) - leg_7.set_touch(tactile_7) - leg_8.set_touch(tactile_8) + (tactile_1, tactile_2, tactile_3, tactile_4, tactie_5, tactile_6, tactile_7, tactile_8) = listener() + not_touch = 0 + balance_punishment = 0 + if(tactile_1==0.0): + not_touch +=1 + elif(tactile_2==0.0): + not_touch+=1 + elif(tactile_3==0.0): + not_touch+=1 + elif(tactile_3==0.0): + not_touch+=1 + elif(tactile_4==0.0): + not_touch+=1 + elif(tactile_5==0.0): + not_touch+=1 + elif(tactile_6==0.0): + not_touch+=1 + elif(tactile_7==0.0): + not_touch+=1 + elif(tactile_8==0.0): + not_touch+=1 + if(not_touch>5): + balance_punishment = 5 joint_angle_leg_1 = leg_1.give_angles() joint_angle_leg_2 = leg_2.give_angles() joint_angle_leg_3 = leg_3.give_angles() @@ -539,32 +580,33 @@ if __name__=="__main__": model_1.get_neuron_value(model_1.neuron_o, input_layerout) output_layerout = model_1.output_function(model_1.neuron_o) # output of the neural network for model 1 that is to try: - talker('joint_1_1', output_layerout[0]) - talker('joint_1_2', output_layerout[1]) - talker('joint_1_3', output_layerout[2]) - talker('joint_4_1', output_layerout[9]) - talker('joint_4_2', output_layerout[10]) - talker('joint_4_3', output_layerout[11]) - talker('joint_5_1', output_layerout[12]) - talker('joint_5_2', output_layerout[13]) - talker('joint_5_3', output_layerout[14]) - talker('joint_8_1', output_layerout[21]) - talker('joint_8_2', output_layerout[22]) - talker('joint_8_3', output_layerout[23]) - talker('joint_2_1', output_layerout[3]) - talker('joint_2_2', output_layerout[4]) - talker('joint_2_3', output_layerout[5]) - talker('joint_3_1', output_layerout[6]) - talker('joint_3_2', output_layerout[7]) - talker('joint_3_3', output_layerout[8]) - talker('joint_6_1', output_layerout[15]) - talker('joint_6_2', output_layerout[16]) - talker('joint_6_3', output_layerout[17]) - talker('joint_7_1', output_layerout[18]) - talker('joint_7_2', output_layerout[19]) - talker('joint_7_3', output_layerout[20]) + talker('spider::joint_1_1', output_layerout[0]) + talker('spider::joint_1_2', output_layerout[1]) + talker('spider::joint_1_3', output_layerout[2]) + talker('spider::joint_4_1', output_layerout[9]) + talker('spider::joint_4_2', output_layerout[10]) + talker('spider::joint_4_3', output_layerout[11]) + talker('spider::joint_5_1', output_layerout[12]) + talker('spider::joint_5_2', output_layerout[13]) + talker('spider::joint_5_3', output_layerout[14]) + talker('spider::joint_8_1', output_layerout[21]) + talker('spider::joint_8_2', output_layerout[22]) + talker('spider::joint_8_3', output_layerout[23]) + talker('spider::joint_2_1', output_layerout[3]) + talker('spider::joint_2_2', output_layerout[4]) + talker('spider::joint_2_3', output_layerout[5]) + talker('spider::joint_3_1', output_layerout[6]) + talker('spider::joint_3_2', output_layerout[7]) + talker('spider::joint_3_3', output_layerout[8]) + talker('spider::joint_6_1', output_layerout[15]) + talker('spider::joint_6_2', output_layerout[16]) + talker('spider::joint_6_3', output_layerout[17]) + talker('spider::joint_7_1', output_layerout[18]) + talker('spider::joint_7_2', output_layerout[19]) + talker('spider::joint_7_3', output_layerout[20]) except rospy.ROSInterruptException: pass + rospy.spin() elif(model_trained == 2): model_2.get_neuron_value(model_2.neuron_i, joint_angle_input) for i in range(0, 24, 1): @@ -585,32 +627,33 @@ if __name__=="__main__": model_2.get_neuron_value(model_2.neuron_o, input_layerout) output_layerout = model_2.output_function(model_2.neuron_o) # output of the neural network for model 1 try: - talker('joint_7_1', output_layerout[18]) - talker('joint_7_2', output_layerout[19]) - talker('joint_7_3', output_layerout[20]) - talker('joint_2_1', output_layerout[3]) - talker('joint_2_2', output_layerout[4]) - talker('joint_2_3', output_layerout[5]) - talker('joint_1_1', output_layerout[0]) - talker('joint_1_2', output_layerout[1]) - talker('joint_1_3', output_layerout[2]) - talker('joint_4_1', output_layerout[9]) - talker('joint_4_2', output_layerout[10]) - talker('joint_4_3', output_layerout[11]) - talker('joint_3_1', output_layerout[6]) - talker('joint_3_2', output_layerout[7]) - talker('joint_3_3', output_layerout[8]) - talker('joint_6_1', output_layerout[15]) - talker('joint_6_2', output_layerout[16]) - talker('joint_6_3', output_layerout[17]) - talker('joint_5_1', output_layerout[12]) - talker('joint_5_2', output_layerout[13]) - talker('joint_5_3', output_layerout[14]) - talker('joint_8_1', output_layerout[21]) - talker('joint_8_2', output_layerout[22]) - talker('joint_8_3', output_layerout[23]) + talker('spider::joint_7_1', output_layerout[18]) + talker('spider::joint_7_2', output_layerout[19]) + talker('spider::joint_7_3', output_layerout[20]) + talker('spider::joint_2_1', output_layerout[3]) + talker('spider::joint_2_2', output_layerout[4]) + talker('spider::joint_2_3', output_layerout[5]) + talker('spider::joint_1_1', output_layerout[0]) + talker('spider::joint_1_2', output_layerout[1]) + talker('spider::joint_1_3', output_layerout[2]) + talker('spider::joint_4_1', output_layerout[9]) + talker('spider::joint_4_2', output_layerout[10]) + talker('spider::joint_4_3', output_layerout[11]) + talker('spider::joint_3_1', output_layerout[6]) + talker('spider::joint_3_2', output_layerout[7]) + talker('spider::joint_3_3', output_layerout[8]) + talker('spider::joint_6_1', output_layerout[15]) + talker('spider::joint_6_2', output_layerout[16]) + talker('spider::joint_6_3', output_layerout[17]) + talker('spider::joint_5_1', output_layerout[12]) + talker('spider::joint_5_2', output_layerout[13]) + talker('spider::joint_5_3', output_layerout[14]) + talker('spider::joint_8_1', output_layerout[21]) + talker('spider::joint_8_2', output_layerout[22]) + talker('spider::joint_8_3', output_layerout[23]) except rospy.ROSInterruptException: pass + rospy.spin() elif(model_trained == 3): model_3.get_neuron_value(model_3.neuron_i, joint_angle_input) for i in range(0, 24, 1): @@ -631,32 +674,33 @@ if __name__=="__main__": model_3.get_neuron_value(model_3.neuron_o, input_layerout) output_layerout = model_3.output_function(model_3.neuron_o) # output of the neural network for model 1 try: - talker('joint_1_1', output_layerout[0]) - talker('joint_1_2', output_layerout[1]) - talker('joint_1_3', output_layerout[2]) - talker('joint_8_1', output_layerout[21]) - talker('joint_8_2', output_layerout[22]) - talker('joint_8_3', output_layerout[23]) - talker('joint_3_1', output_layerout[6]) - talker('joint_3_2', output_layerout[7]) - talker('joint_3_3', output_layerout[8]) - talker('joint_2_1', output_layerout[3]) - talker('joint_2_2', output_layerout[4]) - talker('joint_2_3', output_layerout[5]) - talker('joint_5_1', output_layerout[12]) - talker('joint_5_2', output_layerout[13]) - talker('joint_5_3', output_layerout[14]) - talker('joint_4_1', output_layerout[9]) - talker('joint_4_2', output_layerout[10]) - talker('joint_4_3', output_layerout[11]) - talker('joint_7_1', output_layerout[18]) - talker('joint_7_2', output_layerout[19]) - talker('joint_7_3', output_layerout[20]) - talker('joint_6_1', output_layerout[15]) - talker('joint_6_2', output_layerout[16]) - talker('joint_6_3', output_layerout[17]) + talker('spider::joint_1_1', output_layerout[0]) + talker('spider::joint_1_2', output_layerout[1]) + talker('spider::joint_1_3', output_layerout[2]) + talker('spider::joint_8_1', output_layerout[21]) + talker('spider::joint_8_2', output_layerout[22]) + talker('spider::joint_8_3', output_layerout[23]) + talker('spider::joint_3_1', output_layerout[6]) + talker('spider::joint_3_2', output_layerout[7]) + talker('spider::joint_3_3', output_layerout[8]) + talker('spider::joint_2_1', output_layerout[3]) + talker('spider::joint_2_2', output_layerout[4]) + talker('spider::joint_2_3', output_layerout[5]) + talker('spider::joint_5_1', output_layerout[12]) + talker('spider::joint_5_2', output_layerout[13]) + talker('spider::joint_5_3', output_layerout[14]) + talker('spider::joint_4_1', output_layerout[9]) + talker('spider::joint_4_2', output_layerout[10]) + talker('spider::joint_4_3', output_layerout[11]) + talker('spider::joint_7_1', output_layerout[18]) + talker('spider::joint_7_2', output_layerout[19]) + talker('spider::joint_7_3', output_layerout[20]) + talker('spider::joint_6_1', output_layerout[15]) + talker('spider::joint_6_2', output_layerout[16]) + talker('spider::joint_6_3', output_layerout[17]) except rospy.ROSInterruptException: pass + rospy.spin() elif(model_trained == 4): model_4.get_neuron_value(model_4.neuron_i, joint_angle_input) for i in range(0, 24, 1): @@ -677,33 +721,34 @@ if __name__=="__main__": model_4.get_neuron_value(model_4.neuron_o, input_layerout) output_layerout = model_4.output_function(model_4.neuron_o) # output of the neural network for model 1 try: - talker('joint_1_1', output_layerout[0]) - talker('joint_1_2', output_layerout[1]) - talker('joint_1_3', output_layerout[2]) - talker('joint_3_1', output_layerout[6]) - talker('joint_3_2', output_layerout[7]) - talker('joint_3_3', output_layerout[8]) - talker('joint_5_1', output_layerout[12]) - talker('joint_5_2', output_layerout[13]) - talker('joint_5_3', output_layerout[14]) - talker('joint_7_1', output_layerout[18]) - talker('joint_7_2', output_layerout[19]) - talker('joint_7_3', output_layerout[20]) - talker('joint_2_1', output_layerout[3]) - talker('joint_2_2', output_layerout[4]) - talker('joint_2_3', output_layerout[5]) - talker('joint_4_1', output_layerout[9]) - talker('joint_4_2', output_layerout[10]) - talker('joint_4_3', output_layerout[11]) - talker('joint_6_1', output_layerout[15]) - talker('joint_6_2', output_layerout[16]) - talker('joint_6_3', output_layerout[17]) - talker('joint_8_1', output_layerout[21]) - talker('joint_8_2', output_layerout[22]) - talker('joint_8_3', output_layerout[23]) + talker('spider::joint_1_1', output_layerout[0]) + talker('spider::joint_1_2', output_layerout[1]) + talker('spider::joint_1_3', output_layerout[2]) + talker('spider::joint_3_1', output_layerout[6]) + talker('spider::joint_3_2', output_layerout[7]) + talker('spider::joint_3_3', output_layerout[8]) + talker('spider::joint_5_1', output_layerout[12]) + talker('spider::joint_5_2', output_layerout[13]) + talker('spider::joint_5_3', output_layerout[14]) + talker('spider::joint_7_1', output_layerout[18]) + talker('spider::joint_7_2', output_layerout[19]) + talker('spider::joint_7_3', output_layerout[20]) + talker('spider::joint_2_1', output_layerout[3]) + talker('spider::joint_2_2', output_layerout[4]) + talker('spider::joint_2_3', output_layerout[5]) + talker('spider::joint_4_1', output_layerout[9]) + talker('spider::joint_4_2', output_layerout[10]) + talker('spider::joint_4_3', output_layerout[11]) + talker('spider::joint_6_1', output_layerout[15]) + talker('spider::joint_6_2', output_layerout[16]) + talker('spider::joint_6_3', output_layerout[17]) + talker('spider::joint_8_1', output_layerout[21]) + talker('spider::joint_8_2', output_layerout[22]) + talker('spider::joint_8_3', output_layerout[23]) except rospy.ROSInterruptException: pass - x_left_com = Leg_attribute.x_left_com(Leg_attribute.m_l, g, leg_1.r_1, leg_1.l_1, joint_angle_leg_1[0], + rospy.spin() + x_left_com = Leg_attribute.x_left_com(m, g, r, l, joint_angle_leg_1[0], joint_angle_leg_1[1], joint_angle_leg_1[2], joint_angle_leg_2[0], joint_angle_leg_2[1], joint_angle_leg_2[2], joint_angle_leg_3[0], joint_angle_leg_3[1], joint_angle_leg_3[2], joint_angle_leg_4[0], @@ -712,7 +757,7 @@ if __name__=="__main__": joint_angle_leg_6[1], joint_angle_leg_6[2], joint_angle_leg_7[0], joint_angle_leg_7[1], joint_angle_leg_7[2], joint_angle_leg_8[0], joint_angle_leg_8[1], joint_angle_leg_8[2]) - y_left_com = Leg_attribute.y_left_com(Leg_attribute.m_l, g, leg_1.r_1, leg_1.l_1, joint_angle_leg_1[0], + y_left_com = Leg_attribute.y_left_com(m, g, r, l, joint_angle_leg_1[0], joint_angle_leg_1[1], joint_angle_leg_1[2], joint_angle_leg_2[0], joint_angle_leg_2[1], joint_angle_leg_2[2], joint_angle_leg_3[0], joint_angle_leg_3[1], joint_angle_leg_3[2], joint_angle_leg_4[0], @@ -721,7 +766,7 @@ if __name__=="__main__": joint_angle_leg_6[1], joint_angle_leg_6[2], joint_angle_leg_7[0], joint_angle_leg_7[1], joint_angle_leg_7[2], joint_angle_leg_8[0], joint_angle_leg_8[1], joint_angle_leg_8[2]) - x_right_com = Leg_attribute.x_right_com(Leg_attribute.m_l, g, leg_1.r_1, leg_1.l_1, joint_angle_leg_1[0], + x_right_com = Leg_attribute.x_right_com(m, g, r, l, joint_angle_leg_1[0], joint_angle_leg_1[1], joint_angle_leg_1[2], joint_angle_leg_2[0], joint_angle_leg_2[1], joint_angle_leg_2[2], joint_angle_leg_3[0], joint_angle_leg_3[1], joint_angle_leg_3[2], joint_angle_leg_4[0], @@ -730,7 +775,7 @@ if __name__=="__main__": joint_angle_leg_6[1], joint_angle_leg_6[2], joint_angle_leg_7[0], joint_angle_leg_7[1], joint_angle_leg_7[2], joint_angle_leg_8[0], joint_angle_leg_8[1], joint_angle_leg_8[2]) - y_right_com = Leg_attribute.y_right_com(Leg_attribute.m_l, g, leg_1.r_1, leg_1.l_1, joint_angle_leg_1[0], + y_right_com = Leg_attribute.y_right_com(m, g, r, l, joint_angle_leg_1[0], joint_angle_leg_1[1], joint_angle_leg_1[2], joint_angle_leg_2[0], joint_angle_leg_2[1], joint_angle_leg_2[2], joint_angle_leg_3[0], joint_angle_leg_3[1], joint_angle_leg_3[2], joint_angle_leg_4[0], @@ -739,7 +784,7 @@ if __name__=="__main__": joint_angle_leg_6[1], joint_angle_leg_6[2], joint_angle_leg_7[0], joint_angle_leg_7[1], joint_angle_leg_7[2], joint_angle_leg_8[0], joint_angle_leg_8[1], joint_angle_leg_8[2]) - x_system_com = Leg_attribute.x_system_com(Leg_attribute.m_l, g, leg_1.r_1, leg_1.l_1, joint_angle_leg_1[0], + x_system_com = Leg_attribute.x_system_com(m, g, r, l, joint_angle_leg_1[0], joint_angle_leg_1[1], joint_angle_leg_1[2], joint_angle_leg_2[0], joint_angle_leg_2[1], joint_angle_leg_2[2], joint_angle_leg_3[0], joint_angle_leg_3[1], joint_angle_leg_3[2], joint_angle_leg_4[0], @@ -748,7 +793,7 @@ if __name__=="__main__": joint_angle_leg_6[1], joint_angle_leg_6[2], joint_angle_leg_7[0], joint_angle_leg_7[1], joint_angle_leg_7[2], joint_angle_leg_8[0], joint_angle_leg_8[1], joint_angle_leg_8[2]) - y_system_com = Leg_attribute.y_system_com(Leg_attribute.m_l, g, leg_1.r_1, leg_1.l_1, joint_angle_leg_1[0], + y_system_com = Leg_attribute.y_system_com(m, g, r, l, joint_angle_leg_1[0], joint_angle_leg_1[1], joint_angle_leg_1[2], joint_angle_leg_2[0], joint_angle_leg_2[1], joint_angle_leg_2[2], joint_angle_leg_3[0], joint_angle_leg_3[1], joint_angle_leg_3[2], joint_angle_leg_4[0], @@ -759,19 +804,20 @@ if __name__=="__main__": joint_angle_leg_8[1], joint_angle_leg_8[2]) x_mid_com = Leg_attribute.mid_point_x(x_left_com, x_right_com) y_mid_com = Leg_attribute.mid_point_y(y_left_com, y_right_com) - reward = Leg_attribute.slope(x_mid_com, y_mid_com, x_system_com, y_system_com) + reward = prev_reward+ Leg_attribute.slope(x_mid_com, y_mid_com, x_system_com, y_system_com) Learningrate = 0.1 model_1.NN_learningReward(Learningrate, reward) - leg1_carollis = leg_1.carolis_term(leg_1.m_l, leg_1.r_1, leg_1.l_1, joint_angle_leg_1) - leg2_carollis = leg_2.carolis_term(leg_2.m_1, leg_2.r_1, leg_2.l_1, joint_angle_leg_2) - leg3_carollis = leg_3.carolis_term(leg_3.m_1, leg_3.r_1, leg_3.l_1, joint_angle_leg_3) - leg4_carollis = leg_4.carolis_term(leg_4.m_1, leg_4.r_1, leg_4.l_1, joint_angle_leg_4) - leg5_carollis = leg_5.carolis_term(leg_5.m_1, leg_5.r_1, leg_5.l_1, joint_angle_leg_5) - leg6_carollis = leg_6.carolis_term(leg_6.m_1, leg_6.r_1, leg_6.l_1, joint_angle_leg_6) - leg7_carollis = leg_7.carolis_term(leg_7.m_1, leg_7.r_1, leg_7.l_1, joint_angle_leg_7) - leg8_carollis = leg_8.carolis_term(leg_8.m_1, leg_8.r_1, leg_8.l_1, joint_angle_leg_8) - carollis_input = np.array(leg1_carollis, leg2_carollis, leg3_carollis, leg4_carollis, leg5_carollis, - leg6_carollis, leg7_carollis, leg8_carollis) + prev_reward = reward-balance_punishment#considering the punishment if robot happens to fall down + leg1_carollis = leg_1.carolis_term(m, r, l, joint_angle_leg_1) + leg2_carollis = leg_2.carolis_term(m, r, l, joint_angle_leg_2) + leg3_carollis = leg_3.carolis_term(m, r, l, joint_angle_leg_3) + leg4_carollis = leg_4.carolis_term(m, r, l, joint_angle_leg_4) + leg5_carollis = leg_5.carolis_term(m, r, l, joint_angle_leg_5) + leg6_carollis = leg_6.carolis_term(m, r, l, joint_angle_leg_6) + leg7_carollis = leg_7.carolis_term(m, r, l, joint_angle_leg_7) + leg8_carollis = leg_8.carolis_term(m, r, l, joint_angle_leg_8) + carollis_input = np.array([leg1_carollis, leg2_carollis, leg3_carollis, leg4_carollis, leg5_carollis, + leg6_carollis, leg7_carollis, leg8_carollis]) normalization(carollis_input) knowledge_transfer.get_neuron(knowledge_transfer.neuron_input, carollis_input) 'finding the output of the input layer' @@ -840,15 +886,29 @@ if __name__=="__main__": (position, velocity, effort) = call_return_joint_states(spiderJoint8Name) leg8 = Leg_attribute(position, velocity, effort) - listener() - leg1.set_touch(tactile_1) - leg2.set_touch(tactile_2) - leg3.set_touch(tactile_3) - leg4.set_touch(tactile_4) - leg5.set_touch(tactile_5) - leg6.set_touch(tactile_6) - leg7.set_touch(tactile_7) - leg8.set_touch(tactile_8) + (tactile_1, tactile_2, tactile_3, tactile_4, tactile_5, tactile_6, tactile_7, tactile_8) = listener() + not_touch = 0 + balance_punishment = 0 + if (tactile_1 == 0.0): + not_touch += 1 + elif (tactile_2 == 0.0): + not_touch += 1 + elif (tactile_3 == 0.0): + not_touch += 1 + elif (tactile_3 == 0.0): + not_touch += 1 + elif (tactile_4 == 0.0): + not_touch += 1 + elif (tactile_5 == 0.0): + not_touch += 1 + elif (tactile_6 == 0.0): + not_touch += 1 + elif (tactile_7 == 0.0): + not_touch += 1 + elif (tactile_8 == 0.0): + not_touch += 1 + if (not_touch > 5): + balance_punishment = 5 joint_angle_leg_1 = leg1.give_angles() #writing the angles from each leg to a saperate array variable joint_angle_leg_2 = leg2.give_angles() joint_angle_leg_3 = leg3.give_angles() @@ -883,7 +943,7 @@ if __name__=="__main__": input_layerout.append(tf.multiply(output_layerhid, model_1.weights_o[8:16])) input_layerout.append(tf.multiply(output_layerhid, model_1.weights_o[16:24]))#element wise multiplication model_1.get_neuron_value(model_1.neuron_o, input_layerout) - output_layerout = model_1.output_function(model_1.neuron_o)#output of the neural network for model 1 that is to be feeded to the robot + output_layerout1 = model_1.output_function(model_1.neuron_o)#output of the neural network for model 1 that is to be feeded to the robot 'NN for model_2' for i in range(0, 24, 1): output_layer1[i] = model_2.input_method(model_2.neuron_i[i, :], model_2.weight_in[:, i])#finding the ouput of the input layer @@ -901,7 +961,7 @@ if __name__=="__main__": input_layerout.append(tf.multiply(output_layerhid, model_2.weights_o[8:16])) input_layerout.append(tf.multiply(output_layerhid, model_2.weights_o[16:24]))#element wise multiplication model_2.get_neuron_value(model_2.neuron_o, input_layerout) - output_layerout = model_2.output_function(model_2.neuron_o)#output of the neural network for model 1 that is to + output_layerout2 = model_2.output_function(model_2.neuron_o)#output of the neural network for model 1 that is to 'NN for model 3' for i in range(0, 24, 1): output_layer1[i] = model_3.input_method(model_3.neuron_i[i, :], weight_in[:, i])#finding the ouput of the input layer @@ -919,7 +979,7 @@ if __name__=="__main__": input_layerout.append(tf.multiply(output_layerhid, model_3.weights_o[8:16])) input_layerout.append(tf.multiply(output_layerhid, model_3.weights_o[16:24]))#element wise multiplication model_3.get_neuron_value(model_3.neuron_o, input_layerout) - output_layerout = model_3.output_function(model_3.neuron_o)#output of the neural network for model 1 that is to + output_layerout3 = model_3.output_function(model_3.neuron_o)#output of the neural network for model 1 that is to 'NN for model 4' for i in range(0, 24, 1): output_layer1[i] = model_4.input_method(model_4.neuron_i[i, :], model_4.weight_in[:, i])#finding the ouput of the input layer @@ -937,28 +997,17 @@ if __name__=="__main__": input_layerout.append(tf.multiply(output_layerhid, model_4.weights_o[8:16])) input_layerout.append(tf.multiply(output_layerhid, model_4.weights_o[16:24]))#element wise multiplication model_4.get_neuron_value(model_4.neuron_o, input_layerout) - output_layerout = model_4.output_function(model_4.neuron_o)#output of the neural network for model 1 that is to + output_layerout4 = model_4.output_function(model_4.neuron_o)#output of the neural network for model 1 that is to 'knowledge transfer algorithm starts here' - x_left_com = Leg_attribute.x_left_com(Leg_attribute.m_l, g, leg1.r_1, leg1.l_1, joint_angle_leg_1[0], joint_angle_leg_1[1], joint_angle_leg_1[2], joint_angle_leg_2[0], joint_angle_leg_2[1], joint_angle_leg_2[2], joint_angle_leg_3[0], joint_angle_leg_3[1], joint_angle_leg_3[2], joint_angle_leg_4[0], joint_angle_leg_4[1], joint_angle_leg_4[2], joint_angle_leg_5[0], joint_angle_leg_5[1], joint_angle_leg_5[2], joint_angle_leg_6[0], joint_angle_leg_6[1], joint_angle_leg_6[2], joint_angle_leg_7[0], joint_angle_leg_7[1], joint_angle_leg_7[2], joint_angle_leg_8[0], joint_angle_leg_8[1], joint_angle_leg_8[2]) - y_left_com = Leg_attribute.y_left_com(Leg_attribute.m_l, g, leg1.r_1, leg1.l_1, joint_angle_leg_1[0], joint_angle_leg_1[1], joint_angle_leg_1[2], joint_angle_leg_2[0], joint_angle_leg_2[1], joint_angle_leg_2[2], joint_angle_leg_3[0], joint_angle_leg_3[1], joint_angle_leg_3[2], joint_angle_leg_4[0], joint_angle_leg_4[1], joint_angle_leg_4[2], joint_angle_leg_5[0], joint_angle_leg_5[1], joint_angle_leg_5[2], joint_angle_leg_6[0], joint_angle_leg_6[1], joint_angle_leg_6[2], joint_angle_leg_7[0], joint_angle_leg_7[1], joint_angle_leg_7[2], joint_angle_leg_8[0], joint_angle_leg_8[1], joint_angle_leg_8[2]) - x_right_com = Leg_attribute.x_right_com(Leg_attribute.m_l, g, leg1.r_1, leg1.l_1, joint_angle_leg_1[0], joint_angle_leg_1[1], joint_angle_leg_1[2], joint_angle_leg_2[0], joint_angle_leg_2[1], joint_angle_leg_2[2], joint_angle_leg_3[0], joint_angle_leg_3[1], joint_angle_leg_3[2], joint_angle_leg_4[0], joint_angle_leg_4[1], joint_angle_leg_4[2], joint_angle_leg_5[0], joint_angle_leg_5[1], joint_angle_leg_5[2], joint_angle_leg_6[0], joint_angle_leg_6[1], joint_angle_leg_6[2], joint_angle_leg_7[0], joint_angle_leg_7[1], joint_angle_leg_7[2], joint_angle_leg_8[0], joint_angle_leg_8[1], joint_angle_leg_8[2]) - y_right_com = Leg_attribute.y_right_com(Leg_attribute.m_l, g, leg1.r_1, leg1.l_1, joint_angle_leg_1[0], joint_angle_leg_1[1], joint_angle_leg_1[2], joint_angle_leg_2[0], joint_angle_leg_2[1], joint_angle_leg_2[2], joint_angle_leg_3[0], joint_angle_leg_3[1], joint_angle_leg_3[2], joint_angle_leg_4[0], joint_angle_leg_4[1], joint_angle_leg_4[2], joint_angle_leg_5[0], joint_angle_leg_5[1], joint_angle_leg_5[2], joint_angle_leg_6[0], joint_angle_leg_6[1], joint_angle_leg_6[2], joint_angle_leg_7[0], joint_angle_leg_7[1], joint_angle_leg_7[2], joint_angle_leg_8[0], joint_angle_leg_8[1], joint_angle_leg_8[2]) - x_system_com = Leg_attribute.x_system_com(Leg_attribute.m_l, g, leg1.r_1, leg1.l_1, joint_angle_leg_1[0], joint_angle_leg_1[1], joint_angle_leg_1[2], joint_angle_leg_2[0], joint_angle_leg_2[1], joint_angle_leg_2[2], joint_angle_leg_3[0], joint_angle_leg_3[1], joint_angle_leg_3[2], joint_angle_leg_4[0], joint_angle_leg_4[1], joint_angle_leg_4[2], joint_angle_leg_5[0], joint_angle_leg_5[1], joint_angle_leg_5[2], joint_angle_leg_6[0], joint_angle_leg_6[1], joint_angle_leg_6[2], joint_angle_leg_7[0], joint_angle_leg_7[1], joint_angle_leg_7[2], joint_angle_leg_8[0], joint_angle_leg_8[1], joint_angle_leg_8[2]) - y_system_com = Leg_attribute.y_system_com(Leg_attribute.m_l, g, leg1.r_1, leg1.l_1, joint_angle_leg_1[0], joint_angle_leg_1[1], joint_angle_leg_1[2], joint_angle_leg_2[0], joint_angle_leg_2[1], joint_angle_leg_2[2], joint_angle_leg_3[0], joint_angle_leg_3[1], joint_angle_leg_3[2], joint_angle_leg_4[0], joint_angle_leg_4[1], joint_angle_leg_4[2], joint_angle_leg_5[0], joint_angle_leg_5[1], joint_angle_leg_5[2], joint_angle_leg_6[0], joint_angle_leg_6[1], joint_angle_leg_6[2], joint_angle_leg_7[0], joint_angle_leg_7[1], joint_angle_leg_7[2], joint_angle_leg_8[0], joint_angle_leg_8[1], joint_angle_leg_8[2]) - x_mid_com = Leg_attribute.mid_point_x(x_left_com, x_right_com) - y_mid_com = Leg_attribute.mid_point_y(y_left_com, y_right_com) - reward = Leg_attribute.slope(x_mid_com, y_mid_com, x_system_com, y_system_com) - Learningrate = 0.1 - model_1.NN_learningReward(Learningrate, reward) leg1_carollis = leg1.carolis_term(leg1.m_l, leg1.r_1, leg1.l_1, joint_angle_leg_1) - leg2_carollis = leg2.carolis_term(leg2.m_1, leg2.r_1, leg2.l_1, joint_angle_leg_2) - leg3_carollis = leg3.carolis_term(leg3.m_1, leg3.r_1, leg3.l_1, joint_angle_leg_3) - leg4_carollis = leg4.carolis_term(leg4.m_1, leg4.r_1, leg4.l_1, joint_angle_leg_4) - leg5_carollis = leg5.carolis_term(leg5.m_1, leg5.r_1, leg5.l_1, joint_angle_leg_5) - leg6_carollis = leg6.carolis_term(leg6.m_1, leg6.r_1, leg6.l_1, joint_angle_leg_6) - leg7_carollis = leg7.carolis_term(leg7.m_1, leg7.r_1, leg7.l_1, joint_angle_leg_7) - leg8_carollis = leg8.carolis_term(leg8.m_1, leg8.r_1, leg8.l_1, joint_angle_leg_8) - carollis_input = np.array(leg1_carollis, leg2_carollis, leg3_carollis, leg4_carollis, leg5_carollis, leg6_carollis, leg7_carollis, leg8_carollis) + leg2_carollis = leg2.carolis_term(leg2.m_l, leg2.r_1, leg2.l_1, joint_angle_leg_2) + leg3_carollis = leg3.carolis_term(leg3.m_l, leg3.r_1, leg3.l_1, joint_angle_leg_3) + leg4_carollis = leg4.carolis_term(leg4.m_l, leg4.r_1, leg4.l_1, joint_angle_leg_4) + leg5_carollis = leg5.carolis_term(leg5.m_l, leg5.r_1, leg5.l_1, joint_angle_leg_5) + leg6_carollis = leg6.carolis_term(leg6.m_l, leg6.r_1, leg6.l_1, joint_angle_leg_6) + leg7_carollis = leg7.carolis_term(leg7.m_l, leg7.r_1, leg7.l_1, joint_angle_leg_7) + leg8_carollis = leg8.carolis_term(leg8.m_l, leg8.r_1, leg8.l_1, joint_angle_leg_8) + carollis_input = np.array([leg1_carollis, leg2_carollis, leg3_carollis, leg4_carollis, leg5_carollis, leg6_carollis, leg7_carollis, leg8_carollis]) normalization(carollis_input) knowledge_transfer.get_neuron(knowledge_transfer.neuron_input, carollis_input) 'finding the output of the input layer' @@ -980,176 +1029,409 @@ if __name__=="__main__": 'feeding the input value of output neurons' knowledge_transfer.get_neuron(knowledge_transfer.neuron_out, in_out) 'finding the softmax output of the neurons' - softmax_output = np.array(4) + #softmax_output = np.array(4) softmax_output = knowledge_transfer.out_softmax(knowledge_transfer.neuron_out)#this gives the softmax output and stores it in the newly created array model = max(softmax_output) if (softmax_output[0] == model): try: - talker('joint_1_1', output_layerout[0]) - talker('joint_1_2', output_layerout[1]) - talker('joint_1_3', output_layerout[2]) - talker('joint_4_1', output_layerout[9]) - talker('joint_4_2', output_layerout[10]) - talker('joint_4_3', output_layerout[11]) - talker('joint_5_1', output_layerout[12]) - talker('joint_5_2', output_layerout[13]) - talker('joint_5_3', output_layerout[14]) - talker('joint_8_1', output_layerout[21]) - talker('joint_8_2', output_layerout[22]) - talker('joint_8_3', output_layerout[23]) - talker('joint_2_1', output_layerout[3]) - talker('joint_2_2', output_layerout[4]) - talker('joint_2_3', output_layerout[5]) - talker('joint_3_1', output_layerout[6]) - talker('joint_3_2', output_layerout[7]) - talker('joint_3_3', output_layerout[8]) - talker('joint_6_1', output_layerout[15]) - talker('joint_6_2', output_layerout[16]) - talker('joint_6_3', output_layerout[17]) - talker('joint_7_1', output_layerout[18]) - talker('joint_7_2', output_layerout[19]) - talker('joint_7_3', output_layerout[20]) + talker('spider::joint_1_1', output_layerout1[0]) + talker('spider::joint_1_2', output_layerout1[1]) + talker('spider::joint_1_3', output_layerout1[2]) + talker('spider::joint_4_1', output_layerout1[9]) + talker('spider::joint_4_2', output_layerout1[10]) + talker('spider::joint_4_3', output_layerout1[11]) + talker('spider::joint_5_1', output_layerout1[12]) + talker('spider::joint_5_2', output_layerout1[13]) + talker('spider::joint_5_3', output_layerout1[14]) + talker('spider::joint_8_1', output_layerout1[21]) + talker('spider::joint_8_2', output_layerout1[22]) + talker('spider::joint_8_3', output_layerout1[23]) + talker('spider::joint_2_1', output_layerout1[3]) + talker('spider::joint_2_2', output_layerout1[4]) + talker('spider::joint_2_3', output_layerout1[5]) + talker('spider::joint_3_1', output_layerout1[6]) + talker('spider::joint_3_2', output_layerout1[7]) + talker('spider::joint_3_3', output_layerout1[8]) + talker('spider::joint_6_1', output_layerout1[15]) + talker('spider::joint_6_2', output_layerout1[16]) + talker('spider::joint_6_3', output_layerout1[17]) + talker('spider::joint_7_1', output_layerout1[18]) + talker('spider::joint_7_2', output_layerout1[19]) + talker('spider::joint_7_3', output_layerout1[20]) + x_left_com = Leg_attribute.x_left_com(leg1.m_l, g, leg1.r_1, leg1.l_1, output_layerout1[0], + output_layerout1[1], output_layerout1[2], output_layerout1[3], + output_layerout1[4], output_layerout1[5], output_layerout1[6], + output_layerout1[7], output_layerout1[8], output_layerout1[9], + output_layerout1[10], output_layerout1[11], + output_layerout1[12], output_layerout1[13], + output_layerout1[14], output_layerout1[15], + output_layerout1[16], output_layerout1[17], + output_layerout1[18], output_layerout1[19], + output_layerout1[20], output_layerout1[21], + output_layerout1[22], output_layerout1[23]) + y_left_com = Leg_attribute.y_left_com(leg1.m_l, g, leg1.r_1, leg1.l_1, output_layerout1[0], + output_layerout1[1], output_layerout1[2], output_layerout1[3], + output_layerout1[4], output_layerout1[5], output_layerout1[6], + output_layerout1[7], output_layerout1[8], output_layerout1[9], + output_layerout1[10], output_layerout1[11], + output_layerout1[12], output_layerout1[13], + output_layerout1[14], output_layerout1[15], + output_layerout1[16], output_layerout1[17], + output_layerout1[18], output_layerout1[19], + output_layerout1[20], output_layerout1[21], + output_layerout1[22], output_layerout1[23]) + x_right_com = Leg_attribute.x_right_com(leg1.m_l, g, leg1.r_1, leg1.l_1, output_layerout1[0], + output_layerout1[1], output_layerout1[2], output_layerout1[3], + output_layerout1[4], output_layerout1[5], output_layerout1[6], + output_layerout1[7], output_layerout1[8], output_layerout1[9], + output_layerout1[10], output_layerout1[11], + output_layerout1[12], output_layerout1[13], + output_layerout1[14], output_layerout1[15], + output_layerout1[16], output_layerout1[17], + output_layerout1[18], output_layerout1[19], + output_layerout1[20], output_layerout1[21], + output_layerout1[22], output_layerout1[23]) + y_right_com = Leg_attribute.y_right_com(leg1.m_l, g, leg1.r_1, leg1.l_1, output_layerout1[0], + output_layerout1[1], output_layerout1[2], output_layerout1[3], + output_layerout1[4], output_layerout1[5], output_layerout1[6], + output_layerout1[7], output_layerout1[8], output_layerout1[9], + output_layerout1[10], output_layerout1[11], + output_layerout1[12], output_layerout1[13], + output_layerout1[14], output_layerout1[15], + output_layerout1[16], output_layerout1[17], + output_layerout1[18], output_layerout1[19], + output_layerout1[20], output_layerout1[21], + output_layerout1[22], output_layerout1[23]) + x_system_com = Leg_attribute.x_system_com(leg1.m_l, g, leg1.r_1, leg1.l_1, output_layerout1[0], + output_layerout1[1], output_layerout1[2], output_layerout1[3], + output_layerout1[4], output_layerout1[5], output_layerout1[6], + output_layerout1[7], output_layerout1[8], output_layerout1[9], + output_layerout1[10], output_layerout1[11], + output_layerout1[12], output_layerout1[13], + output_layerout1[14], output_layerout1[15], + output_layerout1[16], output_layerout1[17], + output_layerout1[18], output_layerout1[19], + output_layerout1[20], output_layerout1[21], + output_layerout1[22], output_layerout1[23]) + y_system_com = Leg_attribute.y_system_com(leg1.m_l, g, leg1.r_1, leg1.l_1, output_layerout1[0], + output_layerout1[1], output_layerout1[2], output_layerout1[3], + output_layerout1[4], output_layerout1[5], output_layerout1[6], + output_layerout1[7], output_layerout1[8], output_layerout1[9], + output_layerout1[10], output_layerout1[11], + output_layerout1[12], output_layerout1[13], + output_layerout1[14], output_layerout1[15], + output_layerout1[16], output_layerout1[17], + output_layerout1[18], output_layerout1[19], + output_layerout1[20], output_layerout1[21], + output_layerout1[22], output_layerout1[23]) + x_mid_com = Leg_attribute.mid_point_x(x_left_com, x_right_com) + y_mid_com = Leg_attribute.mid_point_y(y_left_com, y_right_com) + reward = prev_reward+ Leg_attribute.slope(x_mid_com, y_mid_com, x_system_com, y_system_com) + np.append(reward_history, [reward], axis=0) + Learningrate = 0.1 + model_1.NN_learningReward(Learningrate, reward) + prev_reward = reward+ balance_punishment except rospy.ROSInterruptException: pass + rospy.spin() elif (softmax_output[1] == model): try: - talker('joint_7_1', output_layerout[18]) - talker('joint_7_2', output_layerout[19]) - talker('joint_7_3', output_layerout[20]) - talker('joint_2_1', output_layerout[3]) - talker('joint_2_2', output_layerout[4]) - talker('joint_2_3', output_layerout[5]) - talker('joint_1_1', output_layerout[0]) - talker('joint_1_2', output_layerout[1]) - talker('joint_1_3', output_layerout[2]) - talker('joint_4_1', output_layerout[9]) - talker('joint_4_2', output_layerout[10]) - talker('joint_4_3', output_layerout[11]) - talker('joint_3_1', output_layerout[6]) - talker('joint_3_2', output_layerout[7]) - talker('joint_3_3', output_layerout[8]) - talker('joint_6_1', output_layerout[15]) - talker('joint_6_2', output_layerout[16]) - talker('joint_6_3', output_layerout[17]) - talker('joint_5_1', output_layerout[12]) - talker('joint_5_2', output_layerout[13]) - talker('joint_5_3', output_layerout[14]) - talker('joint_8_1', output_layerout[21]) - talker('joint_8_2', output_layerout[22]) - talker('joint_8_3', output_layerout[23]) + talker('spider::joint_7_1', output_layerout2[18]) + talker('spider::joint_7_2', output_layerout2[19]) + talker('spider::joint_7_3', output_layerout2[20]) + talker('spider::joint_2_1', output_layerout2[3]) + talker('spider::joint_2_2', output_layerout2[4]) + talker('spider::joint_2_3', output_layerout2[5]) + talker('spider::joint_1_1', output_layerout2[0]) + talker('spider::joint_1_2', output_layerout2[1]) + talker('spider::joint_1_3', output_layerout2[2]) + talker('spider::joint_4_1', output_layerout2[9]) + talker('spider::joint_4_2', output_layerout2[10]) + talker('spider::joint_4_3', output_layerout2[11]) + talker('spider::joint_3_1', output_layerout2[6]) + talker('spider::joint_3_2', output_layerout2[7]) + talker('spider::joint_3_3', output_layerout2[8]) + talker('spider::joint_6_1', output_layerout2[15]) + talker('spider::joint_6_2', output_layerout2[16]) + talker('spider::joint_6_3', output_layerout2[17]) + talker('spider::joint_5_1', output_layerout2[12]) + talker('spider::joint_5_2', output_layerout2[13]) + talker('spider::joint_5_3', output_layerout2[14]) + talker('spider::joint_8_1', output_layerout2[21]) + talker('spider::joint_8_2', output_layerout2[22]) + talker('spider::joint_8_3', output_layerout2[23]) + x_left_com = Leg_attribute.x_left_com(leg1.m_l, g, leg1.r_1, leg1.l_1, output_layerout2[0], + output_layerout2[1], output_layerout2[2], output_layerout2[3], + output_layerout2[4], output_layerout2[5], output_layerout2[6], + output_layerout2[7], output_layerout2[8], output_layerout2[9], + output_layerout2[10], output_layerout2[11], + output_layerout2[12], output_layerout2[13], + output_layerout2[14], output_layerout2[15], + output_layerout2[16], output_layerout2[17], + output_layerout2[18], output_layerout2[19], + output_layerout2[20], output_layerout2[21], + output_layerout2[22], output_layerout2[23]) + y_left_com = Leg_attribute.y_left_com(leg1.m_l, g, leg1.r_1, leg1.l_1, output_layerout2[0], + output_layerout2[1], output_layerout2[2], output_layerout2[3], + output_layerout2[4], output_layerout2[5], output_layerout2[6], + output_layerout2[7], output_layerout2[8], output_layerout2[9], + output_layerout2[10], output_layerout2[11], + output_layerout2[12], output_layerout2[13], + output_layerout2[14], output_layerout2[15], + output_layerout2[16], output_layerout2[17], + output_layerout2[18], output_layerout2[19], + output_layerout2[20], output_layerout2[21], + output_layerout2[22], output_layerout2[23]) + x_right_com = Leg_attribute.x_right_com(leg1.m_l, g, leg1.r_1, leg1.l_1, output_layerout2[0], + output_layerout2[1], output_layerout2[2], output_layerout2[3], + output_layerout2[4], output_layerout2[5], output_layerout2[6], + output_layerout2[7], output_layerout2[8], output_layerout2[9], + output_layerout2[10], output_layerout2[11], + output_layerout2[12], output_layerout2[13], + output_layerout2[14], output_layerout2[15], + output_layerout2[16], output_layerout2[17], + output_layerout2[18], output_layerout2[19], + output_layerout2[20], output_layerout2[21], + output_layerout2[22], output_layerout2[23]) + y_right_com = Leg_attribute.y_right_com(leg1.m_l, g, leg1.r_1, leg1.l_1, output_layerout2[0], + output_layerout2[1], output_layerout2[2], output_layerout2[3], + output_layerout2[4], output_layerout2[5], output_layerout2[6], + output_layerout2[7], output_layerout2[8], output_layerout2[9], + output_layerout2[10], output_layerout2[11], + output_layerout2[12], output_layerout2[13], + output_layerout2[14], output_layerout2[15], + output_layerout2[16], output_layerout2[17], + output_layerout2[18], output_layerout2[19], + output_layerout2[20], output_layerout2[21], + output_layerout2[22], output_layerout2[23]) + x_system_com = Leg_attribute.x_system_com(leg1.m_l, g, leg1.r_1, leg1.l_1, output_layerout2[0], + output_layerout2[1], output_layerout2[2], output_layerout2[3], + output_layerout2[4], output_layerout2[5], output_layerout2[6], + output_layerout2[7], output_layerout2[8], output_layerout2[9], + output_layerout2[10], output_layerout2[11], + output_layerout2[12], output_layerout2[13], + output_layerout2[14], output_layerout2[15], + output_layerout2[16], output_layerout2[17], + output_layerout2[18], output_layerout2[19], + output_layerout2[20], output_layerout2[21], + output_layerout2[22], output_layerout2[23]) + y_system_com = Leg_attribute.y_system_com(leg1.m_l, g, leg1.r_1, leg1.l_1, output_layerout2[0], + output_layerout2[1], output_layerout2[2], output_layerout2[3], + output_layerout2[4], output_layerout2[5], output_layerout2[6], + output_layerout2[7], output_layerout2[8], output_layerout2[9], + output_layerout2[10], output_layerout2[11], + output_layerout2[12], output_layerout2[13], + output_layerout2[14], output_layerout2[15], + output_layerout2[16], output_layerout2[17], + output_layerout2[18], output_layerout2[19], + output_layerout2[20], output_layerout2[21], + output_layerout2[22], output_layerout2[23]) + x_mid_com = Leg_attribute.mid_point_x(x_left_com, x_right_com) + y_mid_com = Leg_attribute.mid_point_y(y_left_com, y_right_com) + reward = prev_reward+Leg_attribute.slope(x_mid_com, y_mid_com, x_system_com, y_system_com) + np.append(reward_history, [reward], axis=0) + Learningrate = 0.1 + model_1.NN_learningReward(Learningrate, reward) + prev_reward = reward+balance_punishment except rospy.ROSInterruptException: pass + rospy.spin() elif (softmax_output[2] == model): try: - talker('joint_1_1', output_layerout[0]) - talker('joint_1_2', output_layerout[1]) - talker('joint_1_3', output_layerout[2]) - talker('joint_8_1', output_layerout[21]) - talker('joint_8_2', output_layerout[22]) - talker('joint_8_3', output_layerout[23]) - talker('joint_3_1', output_layerout[6]) - talker('joint_3_2', output_layerout[7]) - talker('joint_3_3', output_layerout[8]) - talker('joint_2_1', output_layerout[3]) - talker('joint_2_2', output_layerout[4]) - talker('joint_2_3', output_layerout[5]) - talker('joint_5_1', output_layerout[12]) - talker('joint_5_2', output_layerout[13]) - talker('joint_5_3', output_layerout[14]) - talker('joint_4_1', output_layerout[9]) - talker('joint_4_2', output_layerout[10]) - talker('joint_4_3', output_layerout[11]) - talker('joint_7_1', output_layerout[18]) - talker('joint_7_2', output_layerout[19]) - talker('joint_7_3', output_layerout[20]) - talker('joint_6_1', output_layerout[15]) - talker('joint_6_2', output_layerout[16]) - talker('joint_6_3', output_layerout[17]) + talker('spider::joint_1_1', output_layerout3[0]) + talker('spider::joint_1_2', output_layerout3[1]) + talker('spider::joint_1_3', output_layerout3[2]) + talker('spider::joint_8_1', output_layerout3[21]) + talker('spider::joint_8_2', output_layerout3[22]) + talker('spider::joint_8_3', output_layerout3[23]) + talker('spider::joint_3_1', output_layerout3[6]) + talker('spider::joint_3_2', output_layerout3[7]) + talker('spider::joint_3_3', output_layerout3[8]) + talker('spider::joint_2_1', output_layerout3[3]) + talker('spider::joint_2_2', output_layerout3[4]) + talker('spider::joint_2_3', output_layerout3[5]) + talker('spider::joint_5_1', output_layerout3[12]) + talker('spider::joint_5_2', output_layerout3[13]) + talker('spider::joint_5_3', output_layerout3[14]) + talker('spider::joint_4_1', output_layerout3[9]) + talker('spider::joint_4_2', output_layerout3[10]) + talker('spider::joint_4_3', output_layerout3[11]) + talker('spider::joint_7_1', output_layerout3[18]) + talker('spider::joint_7_2', output_layerout3[19]) + talker('spider::joint_7_3', output_layerout3[20]) + talker('spider::joint_6_1', output_layerout3[15]) + talker('spider::joint_6_2', output_layerout3[16]) + talker('spider::joint_6_3', output_layerout3[17]) + x_left_com = Leg_attribute.x_left_com(leg1.m_l, g, leg1.r_1, leg1.l_1, output_layerout3[0], + output_layerout3[1], output_layerout3[2], output_layerout3[3], + output_layerout3[4], output_layerout3[5], output_layerout3[6], + output_layerout3[7], output_layerout3[8], output_layerout3[9], + output_layerout3[10], output_layerout3[11], output_layerout3[12], + output_layerout3[13], output_layerout3[14], output_layerout3[15], + output_layerout3[16], output_layerout3[17], output_layerout3[18], + output_layerout3[19], output_layerout3[20], output_layerout3[21], + output_layerout3[22], output_layerout3[23]) + y_left_com = Leg_attribute.y_left_com(leg1.m_l, g, leg1.r_1, leg1.l_1, output_layerout3[0], + output_layerout3[1], output_layerout3[2], output_layerout3[3], + output_layerout3[4], output_layerout3[5], output_layerout3[6], + output_layerout3[7], output_layerout3[8], output_layerout3[9], + output_layerout3[10], output_layerout3[11], output_layerout3[12], + output_layerout3[13], output_layerout3[14], output_layerout3[15], + output_layerout3[16], output_layerout3[17], output_layerout3[18], + output_layerout3[19], output_layerout3[20], output_layerout3[21], + output_layerout3[22], output_layerout3[23]) + x_right_com = Leg_attribute.x_right_com(leg1.m_l, g, leg1.r_1, leg1.l_1, output_layerout3[0], + output_layerout3[1], output_layerout3[2], output_layerout3[3], + output_layerout3[4], output_layerout3[5], output_layerout3[6], + output_layerout3[7], output_layerout3[8], output_layerout3[9], + output_layerout3[10], output_layerout3[11], + output_layerout3[12], output_layerout3[13], + output_layerout3[14], output_layerout3[15], + output_layerout3[16], output_layerout3[17], + output_layerout3[18], output_layerout3[19], + output_layerout3[20], output_layerout3[21], + output_layerout3[22], output_layerout3[23]) + y_right_com = Leg_attribute.y_right_com(leg1.m_l, g, leg1.r_1, leg1.l_1, output_layerout3[0], + output_layerout3[1], output_layerout3[2], output_layerout3[3], + output_layerout3[4], output_layerout3[5], output_layerout3[6], + output_layerout3[7], output_layerout3[8], output_layerout3[9], + output_layerout3[10], output_layerout3[11], + output_layerout3[12], output_layerout3[13], + output_layerout3[14], output_layerout3[15], + output_layerout3[16], output_layerout3[17], + output_layerout3[18], output_layerout3[19], + output_layerout3[20], output_layerout3[21], + output_layerout3[22], output_layerout3[23]) + x_system_com = Leg_attribute.x_system_com(leg1.m_l, g, leg1.r_1, leg1.l_1, output_layerout3[0], + output_layerout3[1], output_layerout3[2], output_layerout3[3], + output_layerout3[4], output_layerout3[5], output_layerout3[6], + output_layerout3[7], output_layerout3[8], output_layerout3[9], + output_layerout3[10], output_layerout3[11], + output_layerout3[12], output_layerout3[13], + output_layerout3[14], output_layerout3[15], + output_layerout3[16], output_layerout3[17], + output_layerout3[18], output_layerout3[19], + output_layerout3[20], output_layerout3[21], + output_layerout3[22], output_layerout3[23]) + y_system_com = Leg_attribute.y_system_com(leg1.m_l, g, leg1.r_1, leg1.l_1, output_layerout3[0], + output_layerout3[1], output_layerout3[2], output_layerout3[3], + output_layerout3[4], output_layerout3[5], output_layerout3[6], + output_layerout3[7], output_layerout3[8], output_layerout3[9], + output_layerout3[10], output_layerout3[11], + output_layerout3[12], output_layerout3[13], + output_layerout3[14], output_layerout3[15], + output_layerout3[16], output_layerout3[17], + output_layerout3[18], output_layerout3[19], + output_layerout3[20], output_layerout3[21], + output_layerout3[22], output_layerout3[23]) + x_mid_com = Leg_attribute.mid_point_x(x_left_com, x_right_com) + y_mid_com = Leg_attribute.mid_point_y(y_left_com, y_right_com) + reward = prev_reward + Leg_attribute.slope(x_mid_com, y_mid_com, x_system_com, y_system_com) + np.append(reward_history, [reward], axis=0) + Learningrate = 0.1 + model_1.NN_learningReward(Learningrate, reward) + prev_reward =reward+balance_punishment except rospy.ROSInterruptException: pass + rospy.spin() elif (softmax_output[3] == model): try: - talker('joint_1_1', output_layerout[0]) - talker('joint_1_2', output_layerout[1]) - talker('joint_1_3', output_layerout[2]) - talker('joint_3_1', output_layerout[6]) - talker('joint_3_2', output_layerout[7]) - talker('joint_3_3', output_layerout[8]) - talker('joint_5_1', output_layerout[12]) - talker('joint_5_2', output_layerout[13]) - talker('joint_5_3', output_layerout[14]) - talker('joint_7_1', output_layerout[18]) - talker('joint_7_2', output_layerout[19]) - talker('joint_7_3', output_layerout[20]) - talker('joint_2_1', output_layerout[3]) - talker('joint_2_2', output_layerout[4]) - talker('joint_2_3', output_layerout[5]) - talker('joint_4_1', output_layerout[9]) - talker('joint_4_2', output_layerout[10]) - talker('joint_4_3', output_layerout[11]) - talker('joint_6_1', output_layerout[15]) - talker('joint_6_2', output_layerout[16]) - talker('joint_6_3', output_layerout[17]) - talker('joint_8_1', output_layerout[21]) - talker('joint_8_2', output_layerout[22]) - talker('joint_8_3', output_layerout[23]) + talker('spider::joint_1_1', output_layerout4[0]) + talker('spider::joint_1_2', output_layerout4[1]) + talker('spider::joint_1_3', output_layerout4[2]) + talker('spider::joint_3_1', output_layerout4[6]) + talker('spider::joint_3_2', output_layerout4[7]) + talker('spider::joint_3_3', output_layerout4[8]) + talker('spider::joint_5_1', output_layerout4[12]) + talker('spider::joint_5_2', output_layerout4[13]) + talker('spider::joint_5_3', output_layerout4[14]) + talker('spider::joint_7_1', output_layerout4[18]) + talker('spider::joint_7_2', output_layerout4[19]) + talker('spider::joint_7_3', output_layerout4[20]) + talker('spider::joint_2_1', output_layerout4[3]) + talker('spider::joint_2_2', output_layerout4[4]) + talker('spider::joint_2_3', output_layerout4[5]) + talker('spider::joint_4_1', output_layerout4[9]) + talker('spider::joint_4_2', output_layerout4[10]) + talker('spider::joint_4_3', output_layerout4[11]) + talker('spider::joint_6_1', output_layerout4[15]) + talker('spider::joint_6_2', output_layerout4[16]) + talker('spider::joint_6_3', output_layerout4[17]) + talker('spider::joint_8_1', output_layerout4[21]) + talker('spider::joint_8_2', output_layerout4[22]) + talker('spider::joint_8_3', output_layerout4[23]) + x_left_com = Leg_attribute.x_left_com(leg1.m_l, g, leg1.r_1, leg1.l_1, output_layerout4[0], + output_layerout4[1], output_layerout4[2], output_layerout4[3], + output_layerout4[4], output_layerout4[5], output_layerout4[6], + output_layerout4[7], output_layerout4[8], output_layerout4[9], + output_layerout4[10], output_layerout4[11], output_layerout4[12], + output_layerout4[13], output_layerout4[14], output_layerout4[15], + output_layerout4[16], output_layerout4[17], output_layerout4[18], + output_layerout4[19], output_layerout4[20], output_layerout4[21], + output_layerout4[22], output_layerout4[23]) + y_left_com = Leg_attribute.y_left_com(leg1.m_l, g, leg1.r_1, leg1.l_1, output_layerout4[0], + output_layerout4[1], output_layerout4[2], output_layerout4[3], + output_layerout4[4], output_layerout4[5], output_layerout4[6], + output_layerout4[7], output_layerout4[8], output_layerout4[9], + output_layerout4[10], output_layerout4[11], output_layerout4[12], + output_layerout4[13], output_layerout4[14], output_layerout4[15], + output_layerout4[16], output_layerout4[17], output_layerout4[18], + output_layerout4[19], output_layerout4[20], output_layerout4[21], + output_layerout4[22], output_layerout4[23]) + x_right_com = Leg_attribute.x_right_com(leg1.m_l, g, leg1.r_1, leg1.l_1, output_layerout4[0], + output_layerout4[1], output_layerout4[2], output_layerout4[3], + output_layerout4[4], output_layerout4[5], output_layerout4[6], + output_layerout4[7], output_layerout4[8], output_layerout4[9], + output_layerout4[10], output_layerout4[11], + output_layerout4[12], output_layerout4[13], + output_layerout4[14], output_layerout4[15], + output_layerout4[16], output_layerout4[17], + output_layerout4[18], output_layerout4[19], + output_layerout4[20], output_layerout4[21], + output_layerout4[22], output_layerout4[23]) + y_right_com = Leg_attribute.y_right_com(leg1.m_l, g, leg1.r_1, leg1.l_1, output_layerout4[0], + output_layerout4[1], output_layerout4[2], output_layerout4[3], + output_layerout4[4], output_layerout4[5], output_layerout4[6], + output_layerout4[7], output_layerout4[8], output_layerout4[9], + output_layerout4[10], output_layerout4[11], + output_layerout4[12], output_layerout4[13], + output_layerout4[14], output_layerout4[15], + output_layerout4[16], output_layerout4[17], + output_layerout4[18], output_layerout4[19], + output_layerout4[20], output_layerout4[21], + output_layerout4[22], output_layerout4[23]) + x_system_com = Leg_attribute.x_system_com(leg1.m_l, g, leg1.r_1, leg1.l_1, output_layerout4[0], + output_layerout4[1], output_layerout4[2], output_layerout4[3], + output_layerout4[4], output_layerout4[5], output_layerout4[6], + output_layerout4[7], output_layerout4[8], output_layerout4[9], + output_layerout4[10], output_layerout4[11], + output_layerout4[12], output_layerout4[13], + output_layerout4[14], output_layerout4[15], + output_layerout4[16], output_layerout4[17], + output_layerout4[18], output_layerout4[19], + output_layerout4[20], output_layerout4[21], + output_layerout4[22], output_layerout4[23]) + y_system_com = Leg_attribute.y_system_com(leg1.m_l, g, leg1.r_1, leg1.l_1, output_layerout4[0], + output_layerout4[1], output_layerout4[2], output_layerout4[3], + output_layerout4[4], output_layerout4[5], output_layerout4[6], + output_layerout4[7], output_layerout4[8], output_layerout4[9], + output_layerout4[10], output_layerout4[11], + output_layerout4[12], output_layerout4[13], + output_layerout4[14], output_layerout4[15], + output_layerout4[16], output_layerout4[17], + output_layerout4[18], output_layerout4[19], + output_layerout4[20], output_layerout4[21], + output_layerout4[22], output_layerout4[23]) + x_mid_com = Leg_attribute.mid_point_x(x_left_com, x_right_com) + y_mid_com = Leg_attribute.mid_point_y(y_left_com, y_right_com) + reward = prev_reward+Leg_attribute.slope(x_mid_com, y_mid_com, x_system_com, y_system_com) + np.append(reward_history, [reward], axis=0) + Learningrate = 0.1 + model_1.NN_learningReward(Learningrate, reward) + prev_reward = reward+balance_punishment except rospy.ROSInterruptException: pass - #calculating carolis term to find classifier for KT - carolis_term_leg1 = Leg_attribute.carolis_term(leg1.m_l, 9.8, leg1.r_1, leg1.l_1, joint_angle_leg_1) - - carolis_term_leg2 = Leg_attribute.carolis_term(leg2.m_l, 9.8, leg2.r_1, leg2.l_1, joint_angle_leg_2) - carolis_term_leg3 = Leg_attribute.carolis_term(leg3.m_l, 9.8, leg3.r_1, leg3.l_1, joint_angle_leg_3) - carolis_term_leg4 = Leg_attribute.carolis_term(leg4.m_l, 9.8, leg4.r_1, leg4.l_1, joint_angle_leg_4) - carolis_term_leg5 = Leg_attribute.carolis_term(leg5.m_l, 9.8, leg5.r_1, leg5.l_1, joint_angle_leg_5) - carolis_term_leg6 = Leg_attribute.carolis_term(leg6.m_l, 9.8, leg5.r_1, leg5.l_1, joint_angle_leg_6) - carolis_term_leg7 = Leg_attribute.carolis_term(leg7.m_l, 9.8, leg7.r_1, leg7.l_1, joint_angle_leg_7) - carolis_term_leg8 = Leg_attribute.carolis_term(leg8.m_l, 9.8, leg8.r_1, leg8.l_1, joint_angle_leg_8) - net_carolis = carolis_term_leg1+carolis_term_leg2+carolis_term_leg3+carolis_term_leg4+carolis_term_leg5+carolis_term_leg6+carolis_term_leg7+carolis_term_leg8 - #making a y value to plot hyperplane - y_model = np.ones((1, run_time)) - joint_angles_input = np.arange(24) - j1 = 0 #variable used to write joint angles of leg into input array - for i in range (0, 3, 1): - if(j1<3): - joint_angles_input[i] = joint_angle_leg_1[j1] - j1 = j1+1 - j2 = 0 #variable used to write joint angles of leg into input array - for i in range (3, 6, 1): - if(j2<3): - joint_angles_input[i] = joint_angle_leg_2[j2] - j2 = j2+1 - j3 = 0 #variable used to write joint angles of leg into input array - for i in range (6, 9, 1): - if(j3<3): - joint_angles_input[i] = joint_angle_leg_3[j3] - j3 = j3+1 - j4 = 0 #variable used to write joint angles of leg into input array - for i in range (9, 12, 1): - if(j4<3): - joint_angles_input[i] = joint_angle_leg_4[j4] - j4 = j4+1 - j5 = 0 #variable used to write joint angle of leg into input array - for i in range (12, 15, 1): - if(j5<3): - joint_angles_input[i] = joint_angle_leg_5[j5] - j5 = j5+1 - j6 = 0 #variable used to write joint angles of leg into input array - for i in range (15, 18, 1): - if(j6<3): - joint_angles_input[i] = joint_angle_leg_6[j6] - j6 = j6+1 - j7 = 0 #variable usd to write joint angles of leg into input array - for i in range (18, 21, 1): - if(j7<3): - joint_angles_input[i] = joint_angle_leg_7[j7] - j7 = j7+1 - - j8 = 0 #variable used to write joint angles of leg into input array - for i in range(21, 24, 1): - if(j8<3): - joint_angles_input[i] = joint_angle_leg_8[j8] - j8 = j8+1 + rospy.spin()