def findOptimumMechWalkFromSimParams(m,p,showProgress=True,debugPrint=False,deepCheck=True):
#VALUES TO VARY: All the walk parameters, calf and thigh length
#GOALS: Optimize stride per actuator displacement while...
# Maintain min height of 6' off the ground - CHECKABLE - simulation1
# Keep center of balance over feet - NOT POSSIBLE YET
# Avoid leg collisions at any point - CHECKABLE - simulation2
# Do not exceed max/min of any joint - IMPLICIT
# Both feet must be at about the same level (+/-3" for now let's say) - CHECKABLE - simulation 1
m = mech(part='all')
resultsList = []
#PART 1: Find all the valid combinations
for rtinit in frange(p['taMin'],p['taMax'],p['rtinit_grainularity']):
#print "Completed a round out of %f"%grainularity
if showProgress:
print "Trying configuration rtinit:%f within taMin:%f, taMax:%f"%(rtinit,p['taMin'],p['taMax'])
for rcinit in frange(p['caMin'],p['caMax'],p['rcinit_grainularity']):
for ltinit in frange(p['taMin'],p['taMax'],p['ltinit_grainularity']):
for lcinit in frange(p['caMin'],p['caMax'],p['lcinit_grainularity']):
if debugPrint:
print "Trying configuration rtinit:%f, rcinit:%f, ltinit:%f, lcinit:%f"%(rtinit,rcinit,ltinit,lcinit)
results = quickCheckStepEfficiency(m,p,rtinit,rcinit,ltinit,lcinit)
if results != 'NULL':
resultsList.append(results)
#PART 2: For the most efficient walk, begin testing the combinations untill you have a match
resultsList.sort(reverse=True)
if not deepCheck:
return resultsList[0]
if showProgress:
print "Found %f possible solutions"%len(resultsList)
if debugPrint:
print "We now have %f possible solutions"%len(resultsList)
s = "results efficency list: "
for i in resultsList:
s = "%s,(%f,%f)"%(s,i[0],i[1]['moveDistance'])
print s
finalResultsList = []
bestResult = 'NULL'
for i in range(len(resultsList)):
if showProgress:
print "Checking solution #%f of %f. So far %f found."%(i,len(resultsList),len(finalResultsList))
if findNoCollisionStateForStep(m,p,resultsList[i]):
print "Found a no-collision result"
finalResultsList.append(resultsList[i])#TODO: Actually check more, given this may not really be optimal
if bestResult != 'NULL' and resultsList[i][0] > bestResult[0]:
bestResult = resultsList[i]
if bestResult == 'NULL':
bestResult = resultsList[i]
if bestResult != 'NULL':
print "Showing the best result so far"
s = displayResults(bestResult,stringOnly=True)
print s
print "collision distance is"
print m.struct.collisionMargin
# break
finalResultsList.sort(reverse=True)
if showProgress:
print "Final results list has %f possible solutions"%len(finalResultsList)
return finalResultsList[0]
def displayMech_basic(result):
m = mech()
rtinit = result[1]["vars"][0]
rcinit = result[1]["vars"][1]
ltinit = result[1]["vars"][2]
lcinit = result[1]["vars"][3]
m.moveAllActuators(rtinit, rcinit, ltinit, lcinit)
m.computeLocations()
def displayMech_basic(result): m = mech() rtinit = result[1]['vars'][0] rcinit = result[1]['vars'][1] ltinit = result[1]['vars'][2] lcinit = result[1]['vars'][3] m.moveAllActuators(rtinit,rcinit,ltinit,lcinit) m.computeLocations() disp=structureDisplay() disp.displayLines_animate(m.struct)
def displayMech_walking(result):
m = mech()
rtinit = result[1]['vars'][0]
rcinit = result[1]['vars'][1]
ltinit = result[1]['vars'][2]
lcinit = result[1]['vars'][3]
lc1 = result[1]['vars'][4]
rt2 = result[1]['vars'][5]
rc3 = result[1]['vars'][6]
lt4 = result[1]['vars'][7]
lc6 = result[1]['vars'][8]
lta = m.leftThighActuator
rta = m.rightThighActuator
lca = m.leftCalfActuator
rca = m.rightCalfActuator
#m.moveAllActuators(result[1]['vars'][0],result[1]['vars'][1],result[1]['vars'][2],result[1]['vars'][3])
#m.computeLocations()
disp=structureDisplay()
commands = []
commands.append([lta,ltinit,0])
commands.append([rta,rtinit,0])
commands.append([lca,lcinit,0])
commands.append([rca,rcinit,0])
commands.append([lca,lc1,10])
commands.append([rta,rt2,10])
commands.append([rca,rc3,10])
commands.append([lta,lt4,10])
commands.append([lta,rtinit,10])
commands.append([lca,lc6,10])
commands.append([lca,rcinit,10])
commands.append([rca,lcinit,10])
commands.append([rta,ltinit,10])
#New step
commands.append([rca,lc1,10])
commands.append([lta,rt2,10])
commands.append([lca,rc3,10])
commands.append([rta,lt4,10])
commands.append([rta,rtinit,10])
commands.append([rca,lc6,10])
commands.append([rca,rcinit,10])
commands.append([lca,lcinit,10])
commands.append([lta,ltinit,10])
disp.displayLines_sequence(m.struct,commands,save=True)
#disp.displayLines_animate(m.struct.getLinesList())
foo = raw_input("Press enter to exit")
def findOptimumMechWalk():
p = {}
p["rtinit_grainularity"] = 50
p["rcinit_grainularity"] = 30.0 # 15
p["ltinit_grainularity"] = 50
p["lcinit_grainularity"] = 30.0 # 15
p["taMin"] = 0.5
p["taMax"] = 1.9
p["caMin"] = 0.5
p["caMax"] = 1.9
p["lc1_grainularity"] = 30.0
p["requiredHeight"] = 4.0 # 7
p["footToFootProximity"] = 1.0 / 12.0
p["minStrideLength"] = 1.0
p["maxStrideLength"] = 7.0
p["moveGrainularity"] = 15.0
m = mech(part="all")
res = findOptimumMechWalkFromSimParams(m, p, showProgress=True)
displayResults(res)
def displayMech_walking(result):
m = mech()
rtinit = result[1]["vars"][0]
rcinit = result[1]["vars"][1]
ltinit = result[1]["vars"][2]
lcinit = result[1]["vars"][3]
lc1 = result[1]["vars"][4]
rt2 = result[1]["vars"][5]
rc3 = result[1]["vars"][6]
lt4 = result[1]["vars"][7]
lc6 = result[1]["vars"][8]
lta = m.leftThighActuator
rta = m.rightThighActuator
lca = m.leftCalfActuator
rca = m.rightCalfActuator
# m.moveAllActuators(result[1]['vars'][0],result[1]['vars'][1],result[1]['vars'][2],result[1]['vars'][3])
# m.computeLocations()
# disp.displayLines_animate(m.struct.getLinesList())
foo = raw_input("Press enter to exit")
def findOptimumMechWalk():
p = {}
p['rtinit_grainularity'] = 15.0#30
p['rcinit_grainularity'] = 10.0#15
p['ltinit_grainularity'] = 30.0#40
p['lcinit_grainularity'] = 10.0#15
p['taMin'] = 0.5
p['taMax'] = 1.9
p['caMin'] = 0.5
p['caMax'] = 1.9
p['lc1_grainularity'] = 15.0#20
p['requiredHeight'] = 5.0#7
p['footToFootProximity'] = 3.0/12.0
p['minStrideLength'] = 3.0
p['maxStrideLength'] = 7.0
p['moveGrainularity'] = 10.0
m = mech()
res = findOptimumMechWalkFromSimParams(m,p,showProgress=True)
displayResults(res)
import sys if(len(sys.argv) > 1): ar = sys.argv[1] if(ar == '-t'): test_software() sys.exit() elif(ar == '-ts'): test_software(short=True) sys.exit() elif(ar == '-td'): structure = test_structureSeed_pyramid() structure.computeLocations() elif(ar == '-rs'): #RUN SIMULATION findOptimumMechWalk() elif(ar == '-rf'): m = mech(part='foot') m.computeLocations() ma = m.struct.getCenterOfMass() print ma.toString() elif(ar == '-rl'): m = mech(part='leg') m.computeLocations() ma = m.struct.getCenterOfMass() print ma.toString() elif(ar == '-ra'): m = mech(part='all') m.computeLocations() ma = m.struct.getCenterOfMass() print ma.toString() elif(ar == '-rh'): m = mech(part='hipandlegs')
