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(planner, guide_step_size, guide_max_yaw, max_surface_area, ref_root_height):
from sl1m.fix_sparsity import solveL1
#surfaces_dict = getAllSurfacesDict(planner.afftool)
success = False
maxIt = 50
it = 0
defaultStep = guide_step_size
step = defaultStep
variation = 0.4 # FIXME : put it in config file, +- bounds on the step size
if hasattr(planner, "pathId"):
pathId = planner.pathId
elif hasattr(planner, "pId"):
pathId = planner.pId
else:
pathId = planner.ps.numberPaths() - 1
while not success and it < maxIt:
if it > 0:
step = defaultStep + random.uniform(-variation, variation)
#configs = getConfigsFromPath (planner.ps, planner.pathId, step)
#getSurfacesFromPath(planner.rbprmBuilder, configs, surfaces_dict, planner.v, True, False)
viewer = planner.v
if not hasattr(viewer, "client"):
viewer = None
R, surfaces = getSurfacesFromGuideContinuous(planner.rbprmBuilder,
planner.ps,
planner.afftool,
pathId,
viewer,
step,
useIntersection=True,
max_yaw=guide_max_yaw,
max_surface_area=max_surface_area)
pb = gen_pb(planner.q_init, R, surfaces, ref_root_height)
try:
pb, coms, footpos, allfeetpos, res = solveL1(pb, surfaces, None)
success = True
except:
logger.warning("## Planner failed at iter : %d with step length = %f", it, step)
it += 1
if not success:
raise RuntimeError("planner always fail.")
return pathId, pb, coms, footpos, allfeetpos, res
def draw_rectangle(l, ax):
#~ plotPoints(ax,l)
lr = l + [l[0]]
cx = [c[0] for c in lr]
cy = [c[1] for c in lr]
cz = [c[2] for c in lr]
ax.plot(cx, cy, cz)
def draw_scene(surfaces, ax = None, color = "p"):
if ax is None:
fig = plt.figure()
ax = fig.add_subplot(111, projection="3d")
[draw_rectangle(l,ax) for l in all_surfaces]
return ax
############# main ###################
if __name__ == '__main__':
from sl1m.fix_sparsity import solveL1
pb = gen_stair_pb()
pb, coms, footpos, allfeetpos, res = solveL1(pb, surfaces, draw_scene, plot = True)
def solve(planner, cfg, display_surfaces = False, initial_contacts = None, final_contacts = None):
"""
Automatically formulate and solve a SL1M problem.
First call the method to extract the surfaces candidates from the guide path,
Then generate the problem from this surfaces and finally solve it with SL1M.
If the problem cannot be solved, try again with a small variation on the discretization step size
used to extract surfaces candidates from the guide path.
:param planner: A rbprm.AbstractPlanner instance which store the result of the planning problem
:param cfg:
:param display_surfaces: if True, display the candidate surfaces with the viewer instanciated in the planner
:param initial_contacts: a list of the initial contact position for each limb.
If None, it will be computed from the initial root position stored in the planner
:param final_contacts: a list of the final contact position for each limb.
If None, it will be computed from the final root position stored in the planner
:return: [pathId, pb, coms, footpos, allfeetpos, res]:
pathId: the Id of the guide path used
pb: a dictionary defining the SL1M problem
coms: a list of CoM position at each phase, computed by SL1M
footpos: a list of effector position at each phase, computed by SL1M
allfeetpos: a list of effector position at each phase, computed by SL1M
res: A dictionary containing the otput of SL1M
"""
if cfg.SL1M_USE_MIP:
num_eff = len(cfg.Robot.limbs_names) # number of effectors used to create contacts
global __ineq_com
global __ineq_relative
__ineq_com = [None for _ in range(num_eff)]
__ineq_relative = [[None for _ in range(num_eff - 1)] for __ in range(num_eff)]
if initial_contacts is None:
initial_contacts = foot_pose_from_guide(cfg.Robot, planner.q_init)
if final_contacts is None:
final_contacts = foot_pose_from_guide(cfg.Robot, planner.q_goal)
logger.info("Initial contacts : %s", initial_contacts)
logger.info("Final contacts : %s", final_contacts)
# Extract candidate surfaces and root rotation from the guide planning
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
if hasattr(planner, "pathId"):
pathId = planner.pathId
elif hasattr(planner, "pId"):
pathId = planner.pId
else:
pathId = planner.ps.numberPaths() - 1
while not success and it < maxIt:
if it > 0:
step = defaultStep + random.uniform(-variation, variation)
viewer = planner.v
if not hasattr(viewer, "client"):
viewer = None
R, surfaces = getSurfacesFromGuideContinuous(planner.rbprmBuilder,
planner.ps,
planner.afftool,
pathId,
viewer if display_surfaces else None,
step,
useIntersection=cfg.SL1M_USE_INTERSECTION,
max_yaw=cfg.GUIDE_MAX_YAW,
max_surface_area=cfg.MAX_SURFACE_AREA,
use_all_limbs = cfg.SL1M_USE_MIP)
if cfg.SL1M_USE_MIP:
from sl1m.planner_l1_generic_equalities_as_ineq import solveMIPGurobi, initGlobals, posturalCost, targetCom, \
retrieve_points_from_res, targetLegCenter, targetEndPos
initGlobals(nEffectors=num_eff)
pb = gen_pb_mip(cfg.Robot, R, surfaces)
initCom = np.array(planner.q_init[0:3])
endCom = np.array(planner.q_goal[0:3])
pb, res, time = solveMIPGurobi(pb, surfaces, MIP=True, draw_scene=None, plot=True, l1Contact=False,
initPos=initial_contacts, endPos=final_contacts,
initCom=initCom, endCom=endCom,
costs=[(5., posturalCost), (1., targetEndPos)],
constraint_init_pos_surface = False,
refPos = foot_pose_from_guide(cfg.Robot, [0, 0 ,planner.q_init[2]]))
coms, footpos, allfeetpos = retrieve_points_from_res(pb, res)
success = True # FIXME check this from mip outputs ?
else:
from sl1m.fix_sparsity import solveL1
pb = gen_pb(cfg.Robot, initial_contacts, R, surfaces)
try:
pb, coms, footpos, allfeetpos, res = solveL1(pb, surfaces, None)
success = True
except:
logger.warning("## Planner failed at iter : %d with step length = %f", it, step)
it += 1
if not success:
raise RuntimeError("planner always fail.")
return pathId, pb, coms, footpos, allfeetpos, res
lr = l + [l[0]]
cx = [c[0] for c in lr]
cy = [c[1] for c in lr]
cz = [c[2] for c in lr]
ax.plot(cx, cy, cz)
def draw_scene(surfaces, ax = None, color = "p"):
if ax is None:
fig = plt.figure()
ax = fig.add_subplot(111, projection="3d")
[draw_rectangle(l,ax) for l in all_surfaces]
return ax
############# main ###################
if __name__ == '__main__':
from sl1m.fix_sparsity import solveL1, solveMIP
pb = gen_stair_pb()
solveMIP(pb, surfaces, MIP = True, draw_scene = draw_scene, plot = True)
pb = gen_stair_pb()
solveL1(pb, surfaces, draw_scene, plot = True)
def solve():
from sl1m.fix_sparsity import solveL1
pb = gen_pb(surfaces)
return solveL1(pb, surfaces, draw_scene)