diff --git a/spider_control/control.py b/spider_control/control.py index 93701561afd6373af72ccc6067261cb2420ae374..b758dcebafa00f4dd66b86ce4f0cc641d7940823 100644 --- a/spider_control/control.py +++ b/spider_control/control.py @@ -1,10 +1,10 @@ -#!/usr/bin/env python +#!/usr/bin/env python3.6.7 import tensorflow as tf import numpy as np import roslib roslib.load_manifest('joint_states_listener') roslib.load_manifest('spider_control') -import pylab as plt +#import pylab as plt import math import threading from geometry_msgs.msgs import * @@ -16,8 +16,8 @@ import threading import time from std_msgs.msg import Float64 from std_msgs.msg import Header -from sklearn.multiclass import OneVsRestClassifier -from sklearn.svm import LinearSVC +#from sklearn.multiclass import OneVsRestClassifier +#from sklearn.svm import LinearSVC tf.enable_eager_execution() @@ -120,12 +120,12 @@ def call_return_joint_states(joint_names): try: s = rospy.ServiceProxy("return_joint_States", ReturnJointStates) resp = s(joint_names) - except rospy.ServiceException, e: - print"error when calling return_joint_states: %s"%e + except rospy.ServiceException as e: + print("error when calling return_joint_states: %s"%e) sys.exit(1) for (ind, joint_name) in enumerate(joint_names): if(not resp.found[ind]): - print"joint %s not found"%joint_name + print("joint %s not found"%joint_name) return resp.position, resp.velocity, resp.effort def talker(jointname , position): @@ -192,8 +192,17 @@ class Leg_attribute: def y_right_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)))*(3*a*d+2*b*c*(3*math.sin(two_three)+3*math.sin(four_three)+3*math.sin(six_three)+3*math.sin(eight_three)+2*math.sin(two_two)+2*math.sin(four_two)+2*math.sin(six_two)+2*math.sin(eight_two)+math.sin(two_one)+math.sin(four_one)+math.sin(six_one)+math.sin(eight_one))) - def x_system_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/(a+6*b*c))*(2*a*d+b*c*(3*(math.cos(one_three)+math.cos(two_three)+math.cos(three_three)+math.cos(four_three)+math.cos(five_three)+math.cos(six_three)+math.cos(seven_three)+math.cos(eight_three))+2*(math.cos(one_two)+math.cos(two_two)+math.cos(three_two)+math.cos(four_two)+math.cos(five_two)+math.cos(six_two)+math.cos(seven_two)+math.cos(eight_two))+math.cos(one_one)+math.cos(two_one)+math.cos(three_one)+math.cos(four_one)+math.cos(five_one)+math.cos(six_one)+math.cos(seven_one)+math.cos(eight_one))) + def x_system_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/(a+6*b*c))*(2*a*d+b*c*(3*(math.cos(one_three) + math.cos(two_three) + math.cos(three_three) + + math.cos(four_three) + math.cos(five_three) + math.cos(six_three) + + math.cos(seven_three) + math.cos(eight_three)) + 2*(math.cos(one_two) + + math.cos(two_two) + math.cos(three_two) + math.cos(four_two) + + math.cos(five_two) + math.cos(six_two) + math.cos(seven_two) + + math.cos(eight_two)) + math.cos(one_one) + math.cos(two_one) + + math.cos(three_one) + math.cos(four_one) + math.cos(five_one) + + math.cos(six_one) + math.cos(seven_one) + math.cos(eight_one))) def y_system_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_onr, 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/(a+6*b*c))*(2*a*d+b*c*(3*(math.sin(one_three)+math.sin(two_three)+math.sin(three_three)+math.sin(four_three)+math.sin(five_three)+math.sin(six_three)+math.sin(seven_three)+math.sin(eight_three))+2*(math.sin(one_two)+math.sin(two_two)+math.sin(three_two)+math.sin(four_two)+math.sin(five_two)+math.sin(six_two)+math.sin(seven_two)+math.sin(eight_two))+math.sin(one_one)+math.sin(two_one)+math.sin(three_one)+math.sin(four_one)+math.sin(five_one)+math.sin(six_one)+math.sin(seven_one)+math.sin(eight_one))) @@ -272,11 +281,11 @@ class Architecture: def weight_initialize (self, neurons): weights1 = tf.Variable(tf.random_uniform((neurons, 1), minval=0, maxval=1, dtype=tf.float32, seed=7273)) weights2 = tf.Variable(tf.random_uniform((neurons, 1), minval=0, maxval=1, dtype=tf.float32, seed=3000)) - weights3h=tf.Variable(tf.random_uniform((neurons/3, neurons/6), minval=0, maxval=1, dtype=tf.float32, seed=0119)) + weights3h=tf.Variable(tf.random_uniform((neurons/3, neurons/6), minval=0, maxval=1, dtype=tf.float32, seed= 119)) weights3c=tf.Variable(tf.random_uniform((neurons/3, 1), minval=0, maxval=1, dtype=tf.float32, seed=1508)) sess=tf.Session() weights_i, weights_o, weights_h, weights_h_c = sess.run(weights1, weights2, weights3h, weights3c) - return (weights_i, weights_o, weights_h, weights_h_c) + return weights_i, weights_o, weights_h, weights_h_c # get neuron value method helps us to write the joint angles to the individual neurons which we can later use for learning def get_neuron_value(self, a, b): @@ -347,7 +356,7 @@ class MultiClassLogistic: self.neuron_out = tf.placeholder(tf.float32, shape=(4, 1))#four neurons in output layer because of the four quadrants weights1 = tf.Variable(tf.random_normal((neurons, 1), dtype=tf.float32, seed=1606))#weights of input layer weights2 = tf.Variable(tf.random_normal((neurons/2, neurons/2), dtype=tf.float32, seed=1003))#weights for hidden layer, number of neurons in hidden layer is identified by the quadrant concept - weights3 = tf.Variable(tf.random_normal((4, 2), dtype=tf.float32, seed=0207))#weights for output layer + weights3 = tf.Variable(tf.random_normal((4, 2), dtype=tf.float32, seed= 207))#weights for output layer bias_in =tf.random_normal((neurons, 1), dtype = tf.float32, seed = 2009) bias_hid = tf.random_normal((neurons/2, 1), dtype = tf.float32, seed = 2509) bias_out = tf.random_normal((4, 1), dtype=tf.float32, seed = 1234) @@ -370,7 +379,7 @@ class MultiClassLogistic: out = sess.run(output) return out - def output_func(self, ): + #def output_func(self, ): def activation(self, a): 'where a is the input of the neuron and the activation function is modelled as rectified linear output' @@ -449,7 +458,7 @@ if __name__=="__main__": model_2 = Architecture(3, 24) #for model two of the terrain model_3 = Architecture(3, 24) #for model three of the terrain 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") + run_method = 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]) @@ -459,7 +468,7 @@ if __name__=="__main__": 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") + run_time_time = 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 @@ -488,9 +497,9 @@ if __name__=="__main__": 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") + training_time = input("please enter how long you intend to train the algorithm") run_time = training_time - model_trained = raw_input("which model do you want to train ?, please enter model_number for straight terrain select 1, for accute angled terrain select 2, for obtuse angled terrain select 3, for up/down terrain select 4") + model_trained = input("which model do you want to train ?, please enter model_number for straight terrain select 1, for accute angled terrain select 2, for obtuse angled terrain select 3, for up/down terrain select 4") output = one_hot_encoding(run_time, model_trained) pref_out = tf.placeholder(tf.float32, shape=(4, 1)) with tf.Session as sess1: diff --git a/spider_description/urdf/spider.xacro b/spider_description/urdf/spider.xacro index 92e8a7533da4fc2711c4719e2db979b9f320cba5..0fb3ab486bf49c4e7f192a63c235a1a4680a4909 100644 --- a/spider_description/urdf/spider.xacro +++ b/spider_description/urdf/spider.xacro @@ -13,7 +13,7 @@ - + @@ -953,7 +953,7 @@ - + @@ -984,7 +984,7 @@ - + @@ -1015,7 +1015,7 @@ - + @@ -1046,7 +1046,7 @@ - + @@ -1077,7 +1077,7 @@ - +