def rm(lId, nu=1):
sF = states[-1]
ns, res = removeContact(sF, lId, True, friction=nu)
print "success ?", res
#~ ns = removeContact(sF,lId,True)[0]
global states
if res:
states += [ns]
r(ns.q())
play()
#~ print "init valid ?", fullBody.isConfigValid(s1.q())
#~ print "end valid ?", fullBody.isConfigValid(s2.q())
r(q_init)
#~ path = go0([s2,s1], mu=0.3,num_optim=1)
res = computeNext(s1, rarmId, True, 10)
s2 = res[0]
r(s2.q())
#~ s3 = computeNext(s2,rLegId,True,10)[0]; r(s3.q())
#~ res2 = computeNext(s2,larmId,True,100)
#~ s3 = res2[0]; r(s3.q())
s4 = removeContact(s2, rLegId, True, 0.1)[0]
r(s4.q())
s5 = removeContact(s4, lLegId, True, 0.1)[0]
r(s5.q())
#~ path = go0([s1,s2,s3,s4,s5], mu=0.6,num_optim=1)
#~ path = go0([s1,s2,s4,s5], mu=0.6,num_optim=1)
states = [s1_hp, s2, s4, s5]
def add(lId):
sF = states[-1]
ns = computeNext(sF, lId, True, 10)[0]
global states
states += [ns]
r(ns.q())
def rm(lId):
sF = states[-1]
ns = removeContact(sF, lId, True)[0]
global states
states += [ns]
r(ns.q())
s1_hp = setupHrp2()
states+=[s1_hp]
r(s1_hp.q())
#~
s1_sp = setupSpidey()
states+=[s1_sp]
r(s1_sp.q())
switch_context(0)
res = computeNext(s1_hp,rarmId,True,10)
s2 = res[0]; r(s2.q())
#~ s3 = computeNext(s2,rLegId,True,10)[0]; r(s3.q())
#~ res2 = computeNext(s2,larmId,True,100)
#~ s3 = res2[0]; r(s3.q())
s4 = removeContact(s2,rLegId,True,0.1)[0]; r(s4.q())
s5 = removeContact(s4,lLegId,True,0.1)[0]; r(s5.q())
#~ path = go0([s1,s2,s3,s4,s5], mu=0.6,num_optim=1)
#~ path = go0([s1,s2,s4,s5], mu=0.6,num_optim=1)
#~ states = [s1,s2,s4,s5]
def add(lId):
sF = states[-1]
ns = computeNext(sF,lId,True,10)[0]
global states
states +=[ns]
r(ns.q())
def rm(lId):
sF = states[-1]