def solve():
from sl1m.fix_sparsity import solveL1
success = False
maxIt = 50
it = 0
defaultStep = 0.5
step = defaultStep
variation = 0.4
while not success and it < maxIt:
if it > 0:
step = defaultStep + random.uniform(-variation, variation)
R, surfaces = getSurfacesFromGuideContinuous(tp.rbprmBuilder, tp.ps,
tp.afftool, tp.pathId,
tp.v, step, True, False)
pb = gen_pb(tp.q_init, R, surfaces)
try:
pb, coms, footpos, allfeetpos, res = solveL1(pb, surfaces, None)
success = True
except:
print("## Planner failed at iter : " + str(it) +
" with step length = " + str(step))
it += 1
if not success:
raise RuntimeError("planner always fail.")
return pb, coms, footpos, allfeetpos, res
def solve(tp):
from sl1m.fix_sparsity import solveL1
#surfaces_dict = getAllSurfacesDict(tp.afftool)
success = False
maxIt = 50
it = 0
defaultStep = cfg.GUIDE_STEP_SIZE
step = defaultStep
variation = 0.4 # FIXME : put it in config file, +- bounds on the step size
pathId = 0
if hasattr(tp,"pathId"):
pathId = tp.pathId
elif hasattr(tp,"pId"):
pathId = tp.pId
else:
pathId = tp.ps.numberPaths()-1
while not success and it < maxIt:
if it > 0 :
step = defaultStep + random.uniform(-variation,variation)
#configs = getConfigsFromPath (tp.ps, tp.pathId, step)
#getSurfacesFromPath(tp.rbprmBuilder, configs, surfaces_dict, tp.v, True, False)
viewer = tp.v
if not hasattr(viewer,"client"):
viewer = None
R,surfaces = getSurfacesFromGuideContinuous(tp.rbprmBuilder,tp.ps,tp.afftool,pathId,viewer,step,useIntersection=True,max_yaw=cfg.GUIDE_MAX_YAW)
pb = gen_pb(tp.q_init,R,surfaces)
try:
pb, coms, footpos, allfeetpos, res = solveL1(pb, surfaces, None)
success = True
except :
print "## Planner failed at iter : "+str(it)+" with step length = "+str(step)
it += 1
if not success :
raise RuntimeError("planner always fail.")
return pathId,pb, coms, footpos, allfeetpos, res
def solve():
from sl1m.fix_sparsity import solveL1
R, surfaces = getSurfacesFromGuideContinuous(tp.rbprmBuilder, tp.ps,
tp.afftool, tp.pathId, tp.v,
0.2, True)
pb = gen_pb(tp.q_init, R, surfaces)
return solveL1(pb, surfaces, draw_scene)
def solve():
from sl1m.fix_sparsity import solveL1
R, surfaces = getSurfacesFromGuideContinuous(tp.rbprmBuilder, tp.ps,
tp.afftool, tp.pathId, tp.v,
0.2, True)
pb = gen_pb(tp.q_init, R, surfaces)
return solveL1(pb, surfaces, draw_scene)
if __name__ == '__main__':
from sl1m.fix_sparsity import solveL1
R, surfaces = getSurfacesFromGuideContinuous(tp.rbprmBuilder, tp.ps,
tp.afftool, tp.pathId, tp.v,
0.2, True)
pb = gen_pb(tp.q_init, R, surfaces)
pb, coms, footpos, allfeetpos, res = solveL1(pb, surfaces, draw_scene)
"""
import matplotlib.pyplot as plt
import mpl_toolkits.mplot3d as a3
import matplotlib.colors as colors
import scipy as sp
def draw_rectangle(l, ax):
#~ plotPoints(ax,l)
lr = l + [l[0]]
cx = [c[0] for c in lr]