def onepath(ol, ctxt=1, nopt=1, mu=1, effector=False):
reset()
sc(ctxt)
global path
global states
print "ctxt", ctxt
print "q", len(states[ol + 1].q())
s = max(norm(array(states[ol + 1].q()) - array(states[ol].q())), 1.) * 0.4
print "s", s
if (ol > len(path) - 1):
path += [
go0([states[ol], states[ol + 1]],
num_optim=nopt,
mu=mu,
use_kin=False,
s=s,
effector=effector)
]
else:
path[ol] = go0([states[ol], states[ol + 1]],
num_optim=nopt,
mu=mu,
use_kin=False,
s=s,
effector=effector)
all_paths[ctxt] = path
def sg(mu=1, nopt=2):
ast()
global path
reset()
try:
path += [go0(states[-2:], num_optim=nopt, mu=mu, use_kin=context == 0)]
except:
global states
states = states[:-1]
def one_step(mu=0.6):
reset()
poss = generatePossibilities()
found = False
global states
s = states[-1]
while (not found and len(poss) > 0):
ac, lId = poss.pop(0)
print "selected ", ac, lId, "\n nnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnn \n",
if (ac == 'rm'):
if (states[-1].getLimbsInContact() <= 3):
print "passing"
print erjiejrjr
pass
ok = rm(lId, mu)
print "limbs in contact ", states[-1].getLimbsInContact()
if (not ok):
pass
else:
found = cpa()
print "found PATH ?", found
else: # add
res = computeNext(s, lId, True, 10, mu)
print "CANDIDATES ", len(res)
while (not found and len(res) > 0):
print "GOING IN"
sc = res.pop(0)
if (norm(
array(s.getEffectorPosition(lId)[0]) -
array(sc.getEffectorPosition(lId)[0])) < 0.02):
print "too close"
else:
ol = len(path)
try:
global path
path += [
go0([states[-1], sc],
num_optim=0,
mu=mu,
use_kin=context == 0)
]
if len(path) > 0 and len(path[-1]) == 0:
path = path[:-1]
print "empty path"
else:
print "found PATH !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! \, !!!!!!!!!!!!!!!!!!!!!"
found = True
states += [sc]
except:
e = sys.exc_info()[0]
print("Error in go0 : " + str(e))
if found:
onepath(len(path) - 1, context, mu=mu, nopt=2)
pl()
sac()
def cpa(mu=1):
global path
reset()
try:
s = max(norm(array(states[i + 1].q()) - array(states[i].q())), 1.) * 1
if (context == 0):
s = max(norm(array(states[i + 1].q()) - array(states[i].q())),
1.) * 0.6
path += [go0(states[-2:], num_optim=1, mu=mu, use_kin=context == 0)]
except:
global states
states = states[:-1]
def cpa(mu=1):
global path
reset()
try:
s = max(norm(array(states[i + 1].q()) - array(states[i].q())), 1.) * 1
if (context == 0):
s = max(norm(array(states[i + 1].q()) - array(states[i].q())),
1.) * 0.6
#~ path += [go0(states[-2:], num_optim=1, mu=mu, use_kin = context == 0)]
path += [go0(states[-2:], num_optim=0, mu=mu, use_kin=context == 0)]
return True
except:
e = sys.exc_info()[0]
print("Error in rm : " + str(e))
global states
states = states[:-1]
return False
def cpa(mu=1):
global path
reset()
try:
s = max(norm(array(states[i + 1].q()) - array(states[i].q())), 1.) * 1
if (context == 0):
s = max(norm(array(states[i + 1].q()) - array(states[i].q())),
1.) * 0.6
path += [go0(states[-2:], num_optim=1, mu=mu, use_kin=context == 0)]
print "success "
#~ print tg
return True
except:
global states
states = states[:-1]
print "fail "
#~ print tg
return False
def go():
return go0(states, mu=0.6, num_optim=2, use_kin=context == 0)
def computeAllPath(nopt=1, mu=1, reverse=False, effector=False, start=0):
global states
global path
if (reverse):
one = 0
zero = 1
else:
one = 1
zero = 0
sc(zero)
path = []
sc(one)
path = []
for ol in range(start, len(states) - 1):
sc(zero)
reset()
global path
global states
print 'path ' + str(ol) + 'for' + str(zero)
s = max(norm(array(states[i + 1].q()) - array(states[i].q())),
1.) * 0.4
if (ol > len(path) - 1):
path += [
go0([states[ol], states[ol + 1]],
num_optim=nopt,
mu=mu,
use_kin=context == 0,
s=s,
effector=effector)
]
else:
path[ol] = go0([states[ol], states[ol + 1]],
num_optim=nopt,
mu=mu,
use_kin=ctxt == 0,
s=s,
effector=effector)
all_paths[zero] = path[:]
reset()
pl()
sc(one)
reset()
print 'path ' + str(ol) + 'for' + str(one)
global path
global states
s = max(norm(array(states[i + 1].q()) - array(states[i].q())), 1.) * 1
if (ol > len(path) - 1):
path += [
go0([states[ol], states[ol + 1]],
num_optim=nopt,
mu=mu,
use_kin=context == 0,
s=s,
effector=effector)
]
else:
path[ol] = go0([states[ol], states[ol + 1]],
num_optim=nopt,
mu=mu,
use_kin=ctxt == 0,
s=s,
effector=effector)
all_paths[one] = path[:]
reset()
pl()
save_paths("test_paths")
def go():
return go0(states, mu=0.6,num_optim=2)
