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 onepath2(states_subset, 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_subset[1].q()) - array(states_subset[0].q())), 1.) * 0.4 #~ print "s",s #~ if(ol > len(path) -1): path = all_paths[ctxt][:] path += [ go2(states_subset, num_optim=nopt, mu=mu, use_kin=False, s=None, effector=effector) ] #~ else: #~ path[ol]=go2(states_subset, num_optim=nopt, mu=mu, use_kin = False, s=s, effector = effector) all_paths[ctxt] = path
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")