Commit b5bafa3e authored by Steve T's avatar Steve T
Browse files

scenarios modifs

parent 9e8fb808
from sl1m.planner_scenarios.state_methods import *
from sl1m.planner_scenarios.anymal_constants import *
from sl1m.stand_alone_scenarios.anymal.flat_ground import solve
# ~ from sl1m.stand_alone_scenarios.anymal.flat_ground import solve
from sl1m.stand_alone_scenarios.anymal.palet import solve
#load scene
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool ()
afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.05])
afftool.loadObstacleModel ("hpp_environments", "ori/modular_palet_flat", "planning", v,reduceSizes=[0.1,0,0])
# ~ afftool.loadObstacleModel ("hpp_environments", "ori/modular_palet_flat", "planning", v,reduceSizes=[0.1,0,0])
afftool.loadObstacleModel ("package://hpp_environments/urdf/ori/modular_palet_flat.urdf", "planning", v,reduceSizes=[0.1,0,0])
afftool.visualiseAffordances('Support', v, v.color.lightBrown)
v.addLandmark(v.sceneName,1)
......@@ -24,7 +26,8 @@ initPos = [array([-0.8, 0.2, 0.13]),
array([-0.8, -0.2, 0.13]),
array([-1.6, 0.2, 0.13]),
array([-1.6, -0.2, 0.13])]
endCom = [10., 0., 0.4]
# ~ endCom = [6, 0., 0.4]
endCom = [0., 0., 0.5]
from sl1m.stand_alone_scenarios.anymal.palet import solve, overrideSurfaces
......
......@@ -7,8 +7,8 @@ from numpy.linalg import norm
from sl1m.constants_and_tools import *
from sl1m.planner_l1_generic_equalities_as_ineq import *
from hpp.corbaserver.rbprm import rbprmstate
from hpp.corbaserver.rbprm import state_alg
# ~ from hpp.corbaserver.rbprm import rbprmstate
# ~ from hpp.corbaserver.rbprm import state_alg
from sl1m.stand_alone_scenarios.constraints_anymal import *
......@@ -63,9 +63,50 @@ def solve(initCom = None, initPos = None, endCom = None):
print ("initCom ", initCom)
endPos = None
# ~ print ("initPos", initPos)
#MIP
pb, res, time = solveMIPGurobi(pb, surfaces, MIP = True, draw_scene = None, plot = True, l1Contact = False, initPos = initPos, endPos = endPos, initCom = initCom, endCom= endCom, costs = [(0.01, posturalCost),(10, targetCom)])
coms, footpos, allfeetpos = retrieve_points_from_res(pb, res)
return pb, coms, footpos, allfeetpos, res
#SL1M
pb = gen_flat_pb()
endCom = coms[-1]
pb, res1, time = solveMIPGurobi(pb, surfaces, MIP = False, draw_scene = None, plot = True, l1Contact = True, initPos = initPos, endPos = None, initCom = None, endCom= endCom, costs = [(0.01, posturalCost),(10, targetCom)])
coms1, footpos1, allfeetpos1 = retrieve_points_from_res(pb, res1)
Coms = coms.copy(); Footpos = footpos.copy(); Allfeetpos = allfeetpos.copy()
Coms1 = coms1[:]; Footpos1 = footpos1[:]; Allfeetpos1 = allfeetpos1[:]
print ("LEN COMS", len(Coms) )
ax = draw_scene(None)
plotQPRes(pb, res, ax=ax, plot_constraints = False, show = True)
for i in range(0):
pb = gen_flat_pb()
initPos = None
initCom = Coms[-1].copy()
initPos = Allfeetpos[-1].copy()
endCom = initCom + array([6, 0.0, 0.0])
pb, res, time = solveMIPGurobi(pb, surfaces, MIP = True, draw_scene = None, plot = True, l1Contact = False, initPos = initPos, endPos = endPos, initCom = initCom, endCom= endCom,costs = [(0.01, posturalCost),(10, targetCom)])
coms, footpos, allfeetpos = retrieve_points_from_res(pb, res)
Coms += coms[1:].copy(); Footpos += footpos.copy(); Allfeetpos += allfeetpos[1:].copy()
print ("LEN COMS", len(Coms) )
pb = gen_flat_pb()
endCom = coms[-1]
pb, res1, time = solveMIPGurobi(pb, surfaces, MIP = False, draw_scene = None, plot = True, l1Contact = True, initPos = initPos, endPos = None, initCom = None, endCom= endCom, costs = [])
coms1, footpos1, allfeetpos1 = retrieve_points_from_res(pb, res1)
Coms1 += coms1[1:].copy(); Footpos1 += footpos1.copy(); Allfeetpos1 += allfeetpos1[1:].copy()
print ("LEN COMS", len(Coms) )
# ~ return pb, Coms, Footpos, Allfeetpos, res
return pb, Coms1, Footpos1, Allfeetpos1, res1
# ~ return pb, coms1, footpos1, allfeetpos1, res1
# ~ return pb, coms, footpos, allfeetpos, res
############# main ###################
......@@ -104,7 +145,8 @@ if __name__ == '__main__':
ax = draw_scene(None)
# ~ plotQPRes(pb, res, ax=ax, plot_constraints = False, show = False)
plotQPRes(pb, res, ax=ax, plot_constraints = False, show = True)
# ~ plotQPRes(pb, res, ax=ax, plot_constraints = False, show = True)
plotConcatenatedData(pb, coms, footpos, allfeetpos, ax=ax, plot_constraints = False, show = True, plotSupport = False)
# ~ pb = gen_flat_pb()
# ~ A, b, E, e = convertProblemToLp(pb)
# ~ bcom = b - A[:,4:].dot(res[4:]) - A[:,2].dot(res[2])
......
......@@ -162,16 +162,18 @@ def solve(initCom = None, initPos = None, endCom = None, surfaces = surfaces, ax
print ("coms ", Coms)
print ("Allfeetpos ", Allfeetpos)
for i in range(3):
for i in range(10):
# ~ for i in range(1):
# ~ print("round !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!", i )
pb = gen_pb()
initPos = None
endPos = None
initCom = Coms[-1].copy()
endCom = Coms[-1].copy() + array([.2, 0.0, 0.0])
# ~ endCom = Coms[-1].copy() + array([.2, 0.0, 0.0])
# ~ endCom = Coms[-1].copy() + array([10., 0.0, 0.0])
initPos = Allfeetpos[-1].copy()
pb, res, time = solveMIPGurobi(pb, surfaces, MIP = True, draw_scene = None, plot = True, l1Contact = False, initPos = initPos, endPos = endPos, initCom = initCom, endCom= endCom)
# ~ pb, res, time = solveMIPGurobi(pb, surfaces, MIP = True, draw_scene = None, plot = True, l1Contact = False, initPos = initPos, endPos = endPos, initCom = initCom, endCom= endCom, costs = [(2, posturalCost),(10, targetCom)])
pb, res, time = solveMIPGurobi(pb, surfaces, MIP = True, draw_scene = None, plot = True, l1Contact = False, initPos = initPos, endPos = endPos, initCom = initCom, endCom= endCom, costs = [(10, targetCom)])
coms, footpos, allfeetpos = retrieve_points_from_res(pb, res)
Coms += coms[1:]; Footpos += footpos[1:]; Allfeetpos += allfeetpos[1:]
# ~ if ax is not None:
......
......@@ -7,8 +7,8 @@ from numpy.linalg import norm
from sl1m.constants_and_tools import *
from sl1m.planner_l1_generic_equalities_as_ineq import *
from hpp.corbaserver.rbprm import rbprmstate
from hpp.corbaserver.rbprm import state_alg
# ~ from hpp.corbaserver.rbprm import rbprmstate
# ~ from hpp.corbaserver.rbprm import state_alg
from sl1m.stand_alone_scenarios.constraints_anymal import *
all_surfaces = None
......@@ -66,7 +66,7 @@ default_surfaces = [all_surfaces_array for _ in range (6)]
surfaces = None
def genSurf(nsurfs = 6):
def genSurf(nsurfs = 4):
global surfaces
surfaces = [all_surfaces_array for _ in range (nsurfs)]
......@@ -139,19 +139,7 @@ def solve(initCom = None, initPos = None, endCom = None, endPos = None, surfaces
print ("coms ", Coms)
print ("Allfeetpos ", Allfeetpos)
for i in range(2):
# ~ for i in range(1):
# ~ print("round !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!", i )
pb = gen_pb()
initPos = None
# ~ endPos = None
initCom = Coms[-1].copy()
endCom = endCom
initPos = Allfeetpos[-1].copy()
pb, res, time = solveMIPGurobi(pb, surfaces, MIP = True, draw_scene = None, plot = True, l1Contact = False, initPos = initPos, endPos = endPos, initCom = initCom, endCom= endCom,)
coms, footpos, allfeetpos = retrieve_points_from_res(pb, res)
Coms += coms[1:]; Footpos += footpos[1:]; Allfeetpos += allfeetpos[1:]
# ~ if ax is not None:
for i in range(3):
......@@ -264,9 +252,9 @@ if __name__ == '__main__':
endComFeas = array([1.05012165, 0.83104905, 0.62293721])
# ~ plt.show()
# ~ pb, coms, footpos, allfeetpos, res = solve(initPos = initPos, endCom=endCom, ax = None)
pb, coms, footpos, allfeetpos, res = solve(initPos = initPos, endCom=endCom, ax = None)
# ~ pb, coms, footpos, allfeetpos, res = solveFeas(initPos = initPos, endCom=endComFeas, ax = None)
# ~ pb, coms, footpos, allfeetpos, res = solveFeas(initPos = initPos, endCom=endComFeas, ax = None)
pb, coms, footpos, allfeetpos, res = solveFeas(initPos = initPos, endCom=endComFeas, ax = None)
# ~ for i in range(15):
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment