def run_gait(gait_name, time_scale = 1.0, run_time = 10.0):
robot = RobotPi()
gait_function = get_gait(gait_name)
#scaled_gait_function = scaleTime(gait_function, 1.0 / gait_speed)
robot.run(gait_function,
runSeconds = run_time,
resetFirst = False,
#interpBegin = 2.0,
#interpEnd = 2.0,
interpBegin = 0.0,
interpEnd = 0.0,
timeScale = time_scale)
def main(argv):
#parse arguments
#python UsingANN.py neuralnetworkfile [commandRate(default =40)]
#check for the right number of arguments
if (len(argv) < 1):
print "\nMust provide neural network file name"
sys.exit(2)
nnfile = argv[0] + '.txt'
commandRate = -1
if (len(argv) > 1):
commandRate = float(argv[1])
#open log file for writing
logfile = 'log'
if (len(argv) > 2):
logfile = argv[2] + '.txt'
f = open(logfile, 'w')
f.write("Start logging...")
#Creating the Neural Network using a text file
testann = ANN(1)
testann.create_network(nnfile)
#Loading the ANN with 0s initially
print "\nLoading the ANN [0, 0, 0, 0]"
inputvals = [0.0, 0.0, 0.0, 0.0]
testann.load_NN(inputvals)
#initilize the neural network using CTRNN_Controller()
print "\nUsing CTRNN_Controller: dt = .02"
nnoutput = testann.CTRNN_Controller(.02)
for node in nnoutput:
print node.get_output()
robot = RobotPi() #Query Aracna on current sensors
if (commandRate > 0):
robot = RobotPi(commandRate)
val = "\nneural network file: {0}\ncommandRate: {1}".format(
nnfile, commandRate)
line = str(val)
f.write(line)
commanded_pos = []
while (True):
#Query Aracna on current sensors
#old_pos = current_pos
current_pos = robot.readCurrentPosition(
) #returns a list of 8 servo positions
val = "\ncurrent_pos: {0}".format(current_pos)
line = str(val)
f.write(line)
#if(commanded_pos is empty) load and activate NN
#pos = [right hip, right knee, back hip, back knee, front hip, front knee, left hip, left knee]
#sensors = [right knee, back knee, front knee, left knee]
print "\nafter reading current pos\t", current_pos
for ii in range(0, 1):
sensors = []
for i in [0, 2, 4, 6]:
current_pos[i] = max(min(MAX_HIP, current_pos[i]),
MIN_HIP) #restrict servo pos [0, 1024]
current_pos[i +
1] = max(min(MIN_KNEE, current_pos[i + 1]),
MAX_KNEE) #restrict servo pos [1024, 0]
value = knee_to_NN(
current_pos[i + 1]
) #convert to neural network sensor bounds [-20, 20] degrees
#sensors.append(value* (M_PI/180.0)) #convert to radian
sensors.append(knee_to_NN(current_pos[3]) *
(M_PI / 180.0)) #back knee
sensors.append(knee_to_NN(current_pos[5]) *
(M_PI / 180.0)) #front knee
sensors.append(knee_to_NN(current_pos[1]) *
(M_PI / 180.0)) #right knee
sensors.append(knee_to_NN(current_pos[7]) *
(M_PI / 180.0)) #left knee
#Load sensors into ANN
testann.load_NN(sensors)
#Get ANN nnoutput [0, 1]
''' [0] back knee
[1] back outhip
[2] back hip =0.0
[3] front hip =0.0
[4] front outhip
[5] front knee
[6] right knee
[7] right outhip
[8] right hip = 0.0
[9] left hip = 0.0
[10] left outhip
[11] left '''
#print "\nPropagating the ANN"
nnoutput = testann.output_NN(.01)
'''current_pos[3] = knee_to_POS(nnoutput[0].get_output()) #back knee
current_pos[5] = knee_to_POS(nnoutput[5].get_output()) #front knee
current_pos[1] = knee_to_POS(nnoutput[6].get_output()) #right knee
current_pos[7]= knee_to_POS(nnoutput[11].get_output()) #left knee
'''
#Map nnoutput from [0, 1] to actual servo pos
#print "nnoutput"
#for node in nnoutput:
#print node.get_output()
desired_pos = []
#desired_pos[0] = MIN_NNKNEE + (MAX_NNKNEE-MIN_NNKNEE)*nnoutput[0] #right knee [-20, 20]
#output [back knee, back hip, 0.0, fro
desired_pos.append(hip_to_POS(nnoutput[7].get_output()) *
2) #right hip convert from [0, 1] to [0, 1024]
desired_pos.append(knee_to_POS(nnoutput[6].get_output()) *
2) #right knee convert from [0, 1] to [1024, 0]
desired_pos.append(hip_to_POS(nnoutput[1].get_output()) * 2) #back hip
desired_pos.append(knee_to_POS(nnoutput[0].get_output()) *
2) #back knee
desired_pos.append(hip_to_POS(nnoutput[4].get_output()) *
2) #front hip
desired_pos.append(knee_to_POS(nnoutput[5].get_output()) *
2) #front knee
desired_pos.append(hip_to_POS(nnoutput[10].get_output()) *
2) #left hip
desired_pos.append(knee_to_POS(nnoutput[11].get_output()) *
2) #left knee
#current_pos = desired_pos
#for node in nnoutput:
# print node.get_output()
#Move Aracna using nn output
#if (is_reached(current_pos, commanded_pos)
print "desired_pos", desired_pos
val = "\tdesired_pos: {0}".format(desired_pos)
line = str(val)
f.write(line)
robot.commandPosition(desired_pos, False)
import sys
sys.path.insert(0, '/home/eric/Documents/Aracna/WindowsClone')
from RobotPi import *
from PiConstants import POS_READY
from math import *
import copy
'''
Created on Jan 31, 2013
@author: Eric Gold
'''
def sinWave():
r = 300
p = .5
f = lambda t: (sin(t / p) * r + r, 40, 770, 40, 770, 40, 770, 40)
return f
if __name__ == '__main__':
robot = RobotPi()
f = sinWave()
print "Beginning run"
robot.run(f, runSeconds=50)
