Commit 3d6ce1f0 authored by Steve T's avatar Steve T
Browse files

added cost selection + race example

parent 60e95c5c
......@@ -383,8 +383,8 @@ def convertProblemToLp(pb, convertSurfaces = True):
#equalities
#no weighted com on first phase
# ~ if phaseId != 0:
# ~ startRowEq = CoMWeightedEqualityConstraint(phaseDataT, E, e, startCol, endCol, startRowEq)
if phaseId != 0:
startRowEq = CoMWeightedEqualityConstraint(phaseDataT, E, e, startCol, endCol, startRowEq)
startRowEq = FootContinuityEqualityConstraint(pb, phaseDataT, E, e, previousStartCol, startCol, endCol, startRowEq, phaseId)
previousStartCol = startCol
startCol = endCol
......@@ -576,7 +576,7 @@ def plotQPRes(pb, res, linewidth=2, ax = None, plot_constraints = False, show =
pond += 0.33 * positions[0]
pond[-1] = 0.4
weighted += [pond]
plot_polytope_H_rep(Af,bf, color = "r", just_pts = False, ax = ax)
# ~ plot_polytope_H_rep(Af,bf, color = "r", just_pts = False, ax = ax)
# ~ plot(Ineq(Afoot,k), ax = ax, show = False, color = colors[footId])
# ~ [print("wtf", (Af[:,:2].dot(el[:2])-bf).max() ) for el in weighted]
......@@ -790,7 +790,7 @@ def solveMIP(pb, surfaces, MIP = True, draw_scene = None, plot = True):
# gurobi cost functions
def targetCom(pb, cVars, endCom):
def targetCom(pb, cVars, initPos, endPos, initCom, endCom):
nVarEnd = numVariablesForPhase(pb["phaseData"][-1])
cx_end_diff = cVars[-nVarEnd] - endCom[0]
cy_end_diff = cVars[-nVarEnd+1] - endCom[1]
......@@ -800,7 +800,21 @@ def targetCom(pb, cVars, endCom):
return cx_end_diff * cx_end_diff + cy_end_diff * cy_end_diff
# gurobi cost functions
def targetLegCenter(pb, cVars, endCom):
def targetEndPos(pb, cVars, initPos, endPos, initCom, endCom):
obj = 0
nVarEnd = numVariablesForPhase(pb["phaseData"][-1])
if endPos is not None:
print ("None !!!!!!!!!!!!!!!!!!!!!!!!!!")
for idFoot, pos in enumerate(endPos):
# ~ print("adding end", idRow )
cx_end_diff = cVars[-nVarEnd + idFoot *3 + 4] - endPos[idFoot][0]
cy_end_diff = cVars[-nVarEnd + idFoot *3 + 5] - endPos[idFoot][1]
obj += cx_end_diff * cx_end_diff + cy_end_diff * cy_end_diff
print ("OBJ ", obj)
return obj
# gurobi cost functions
def targetLegCenter(pb, cVars, initPos, endPos, initCom, endCom):
startCol = 0;
previousStartCol = 0;
endCol = 0;
......@@ -825,7 +839,7 @@ def targetLegCenter(pb, cVars, endCom):
print ("cost ", cost)
return cost
def posturalCost(pb, cVars, initPos, initCom):
def posturalCost(pb, cVars, initPos, endPos, initCom, endCom):
startCol = 0;
previousStartCol = 0;
endCol = 0;
......@@ -845,7 +859,7 @@ def posturalCost(pb, cVars, initPos, initCom):
startCol = endCol
return cost
def stepSizeCost(pb, cVars, initPos, initCom):
def stepSizeCost(pb, cVars, initPos, endPos, initCom, endCom):
startCol = 0;
previousStartCol = 0;
endCol = 0;
......@@ -864,7 +878,7 @@ def stepSizeCost(pb, cVars, initPos, initCom):
startCol = endCol
return cost
def maxStepSizeCost(pb, cVars, initPos, initCom):
def maxStepSizeCost(pb, cVars, initPos, endPos, initCom, endCom):
startCol = 0;
previousStartCol = 0;
endCol = 0;
......@@ -884,7 +898,8 @@ def maxStepSizeCost(pb, cVars, initPos, initCom):
return cost
def solveMIPGurobi(pb, surfaces, MIP = True, draw_scene = None, plot = True, initGuess = None, initGuessMip = None, l1Contact = False, initPos = None, endPos = None, initCom = None, endCom = None):
def solveMIPGurobi(pb, surfaces, MIP = True, draw_scene = None, plot = True, initGuess = None, initGuessMip = None, l1Contact = False, initPos = None, endPos = None, initCom = None, endCom = None,
costs = [(1, posturalCost),(2, targetCom)]):
if not MIP_OK:
print ("Mixed integer formulation requires gurobi packaged in cvxpy")
raise ImportError
......@@ -899,6 +914,7 @@ def solveMIPGurobi(pb, surfaces, MIP = True, draw_scene = None, plot = True, ini
# ~ E,e = addInitEndConstraint(pb, E, e, initPos, endPos, initCom, endCom)
#todo is end constraint desirable ?
E,e = addInitEndConstraint(pb, E, e, initPos, endPos, initCom, None)
# ~ E,e = addInitEndConstraint(pb, E, e, None, None, None, None)
slackMatrix = wSelectionMatrix(pb)
slackIndices = [i for i,el in enumerate (slackMatrix) if el > 0]
numSlackVariables = len([el for el in slackMatrix if el > 0])
......@@ -1042,11 +1058,14 @@ def solveMIPGurobi(pb, surfaces, MIP = True, draw_scene = None, plot = True, ini
if initPos is not None:
obj = 0
print (" initPos is not None !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!")
obj += 0.01 * posturalCost(pb, cVars, initPos, initCom)
# ~ obj += 0.01 * posturalCost(pb, cVars, initPos, initCom)
# ~ obj += 1 * posturalCost(pb, cVars, initPos, initCom)
# ~ obj += stepSizeCost(pb, cVars, initPos, initCom)
# ~ obj += 10. * targetCom(pb, cVars, endCom)
obj += 10. * targetCom(pb, cVars, endCom)
# ~ obj += 2 * targetCom(pb, cVars, endCom)
for (weight, cost) in costs:
obj += weight * cost(pb, cVars, initPos, endPos, initCom, endCom)
# ~ obj += targetEndPos(pb, cVars, endPos)
# ~ obj = targetLegCenter(pb, cVars, endCom)
model.setObjective(obj)
......
......@@ -7,7 +7,7 @@ from sl1m.stand_alone_scenarios.anymal.flat_ground import solve
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool ()
afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.05])
afftool.loadObstacleModel ("hpp_environments", "ori/race_slopes_container", "planning", v,reduceSizes=[0.1,0,0])
afftool.loadObstacleModel ("hpp_environments", "ori/race_slopes_container", "planning", v)
afftool.visualiseAffordances('Support', v, v.color.lightBrown)
v.addLandmark(v.sceneName,1)
......@@ -15,44 +15,58 @@ v.addLandmark(v.sceneName,1)
#retrieve surfaces from scene for sl1m
from hpp.corbaserver.rbprm.tools import surfaces_from_path
#starting position
q_init [0:3] = [0, 0, 0.61]
# ~ 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]
initContacts, initPos, initCom = initConstraintsFrom_q_init(fullBody, q_init, limbNames)
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]
from sl1m.stand_alone_scenarios.anymal.race_slopes_container import solve, overrideSurfaces
#starting position
q_init [0:3] = [0, 0, 0.58]
v(q_init)
initContacts, initPos, initCom = initConstraintsFrom_q_init(fullBody, q_init, limbNames)
initPos = [array([0.38437629, 0.18974121, 0.13850503]),
array([ 0.38437629, -0.18974121, 0.1326605 ]),
array([-0.38437629, 0.18974121, 0.11856727]),
array([-0.38437629, -0.18974121, 0.05008679])]
#in case reduceSize changed
overrideSurfaces(surfaces_from_path.getAllSurfaces(afftool))
# ~ overrideSurfaces(surfaces_from_path.getAllSurfaces(afftool))
endCom = [0, 1.25, 0.4]
# ~ #compute contact sequence
pb, coms, footpos, allfeetpos, res = solve(initPos = initPos, endCom = endCom)
# ~ q_init[2] += footOffset
# ~ v(q_init)
# ~ #create init state
# ~ s = rbprmstate.State(fullBody, q = q_init, limbsIncontact = limbNames[:])
# ~ states = [s]
s = gen_init_state(initPos, fullBody, q_init, limbNames, footOffset, normal = z)
states = [s]
q_init = s.q()[:]
v(q_init)
# ~ #compute whole-body states from contact sequences
# ~ run(fullBody, states, allfeetpos, limbNames, coms, pb, footOffset)
run(fullBody, states, allfeetpos, limbNames, coms, pb, footOffset)
# ~ #play motion
# ~ #export contact sequence
# ~ cs = exportCS(fullBody, q_init, states, pb, allfeetpos, limbNames, effectorNames)
cs = exportCS(fullBody, q_init, states, pb, allfeetpos, limbNames, effectorNames, squeeze = True)
# ~ #save file
# ~ cs.saveAsBinary("anymal_palet.cs")
# ~ play(states,v)
cs.saveAsBinary("anymal_race_slopes_container.cs")
play(states,v)
# ~ #display footsteps
# ~ displaySteppingStones(cs, v.client.gui, v.sceneName, fullBody)
......
......@@ -42,6 +42,14 @@ def staticEq(positions, com):
except:
return False
def gen_init_state(feetPos, fullBody, q_init, limbNames, offsetz, normal = z):
s = rbprmstate.State(fullBody, q = q_init, limbsIncontact = limbNames[:])
sres, succ = StateHelper.cloneState(s)
for limbId, pos in zip(limbNames, feetPos):
l = pos.tolist(); l[2] += offsetz
sres, succ = state_alg.addNewContact(sres, limbId, l, normal.tolist(), num_max_sample= 200)
return sres
def gen_state(pb, coms, allfeetpos,fullBody, limbNames, s, pId, num_max_sample = 1, first = False, normal = z, newContact = True , useCom = False ):
#~ pId = 6
feetPos = allfeetpos[pId]
......@@ -121,11 +129,13 @@ from numpy.linalg import norm
extraDof = [0 for _ in range(6)]
def exportCS(fullBody, q_init, states, pb, allfeetpos, limbNames, effectorNames):
def exportCS(fullBody, q_init, states, pb, allfeetpos, limbNames, effectorNames, squeeze = False):
configs = [ st.q() + extraDof for st in states[:]]; i = 0
cs = ContactSequence(0)
addPhaseFromConfig(fullBody, cs, q_init, limbNames[:])
rot = Quaternion.Identity() #todo update
preveffId = None
prevPlacement = None
for pId in range(1, len(states)):
print ('phase ', pId)
# ~ rot = Quaternion.Identity()
......@@ -140,11 +150,15 @@ def exportCS(fullBody, q_init, states, pb, allfeetpos, limbNames, effectorNames)
placement = SE3()
placement.translation = np.array(pos).T
placement.rotation = rot.matrix()
cs.moveEffectorToPlacement(effId, placement)
if preveffId is not None and (not squeeze or preveffId != effId ):
cs.moveEffectorToPlacement(preveffId, prevPlacement)
preveffId = effId
prevPlacement = placement
if not switch:
print ("no switch at phase")
q_end = configs[-1][:]
fullBody.setCurrentConfig(q_end[:-6])
com = fullBody.getCenterOfMass()
setFinalState(cs, com, q=q_end)
print ("no switch at phase")
cs.moveEffectorToPlacement(preveffId, prevPlacement)
q_end = configs[-1][:]
fullBody.setCurrentConfig(q_end[:-6])
com = fullBody.getCenterOfMass()
setFinalState(cs, com, q=q_end)
return cs
......@@ -2,7 +2,7 @@ from sl1m.constants_and_tools import *
from . import qp
np.set_printoptions(formatter={'float': lambda x: "{0:0.1f}".format(x)})
# ~ np.set_printoptions(formatter={'float': lambda x: "{0:0.1f}".format(x)})
#LP contact planner using inequality formulation
......
......@@ -55,7 +55,7 @@ def draw_scene(surfaces, ax = None, color = "p"):
def solve(initCom = None, initPos = None, endCom = None):
if endCom is None and initCom is not None:
endCom = initCom + array([0.6, 0.0, 0.0])
endCom = initCom + array([100, 0.0, 0.0])
# ~ endCom = initCom + array([6, 0.0, 0.0])
initGlobals(nEffectors = 4)
pb = gen_flat_pb()
......@@ -63,7 +63,7 @@ def solve(initCom = None, initPos = None, endCom = None):
print ("initCom ", initCom)
endPos = None
# ~ print ("initPos", initPos)
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 = [(0.01, posturalCost),(100, targetCom)])
coms, footpos, allfeetpos = retrieve_points_from_res(pb, res)
return pb, coms, footpos, allfeetpos, res
......
......@@ -16,86 +16,45 @@ all_surfaces_array = None
default_surfaces = None
surfaces_and_normals = [([[0.39550296660292134, -0.27720715031876186, 0.16499999165534973],
[0.39550296660292134, -0.44479297175155064, 0.16499999165534973],
[0.5644970119394066, -0.44479297175155064, 0.16499999165534973],
[0.5644970119394066, -0.27720715031876186, 0.16499999165534973]],
[0.0, 0.0, 1.0]),
([[0.3955029688071581, 0.20279284117516114, 0.16499999165534973],
[0.3955029688071581, 0.035207045099176625, 0.16499999165534973],
[0.5644970097351698, 0.035207045099176625, 0.16499999165534973],
[0.5644970097351698, 0.20279284117516114, 0.16499999165534973]],
[0.0, 0.0, 1.0]),
([[-0.5644969567065002, -0.27720715472723556, 0.16499999165534973],
[-0.5644969567065002, -0.44479296734307694, 0.16499999165534973],
[-0.39550296223118286, -0.44479296734307694, 0.16499999165534973],
[-0.39550296223118286, -0.27720715472723556, 0.16499999165534973]],
[0.0, 0.0, 1.0]),
([[-0.5644969545022632, 0.20279283676668725, 0.16499999165534973],
[-0.5644969545022632, 0.03520704950765053, 0.16499999165534973],
[-0.3955029644354199, 0.03520704950765053, 0.16499999165534973],
[-0.3955029644354199, 0.20279283676668725, 0.16499999165534973]],
[0.0, 0.0, 1.0]),
([[0.39550296660292134, -0.03720714078201872, 0.2449999898672104],
[0.39550296660292134, -0.20479296221480744, 0.2449999898672104],
[0.5644970119394066, -0.20479296221480744, 0.2449999898672104],
[0.5644970119394066, -0.03720714078201872, 0.2449999898672104]],
[0.0, 0.0, 1.0]),
([[0.3955029688071581, 0.44279285071190433, 0.2449999898672104],
[0.3955029688071581, 0.27520705463591977, 0.2449999898672104],
[0.5644970097351698, 0.27520705463591977, 0.2449999898672104],
[0.5644970097351698, 0.44279285071190433, 0.2449999898672104]],
[0.0, 0.0, 1.0]),
([[-0.5644969567065002, -0.03720714519049237, 0.2449999898672104],
[-0.5644969567065002, -0.2047929578063338, 0.2449999898672104],
[-0.39550296223118286, -0.2047929578063338, 0.2449999898672104],
[-0.39550296223118286, -0.03720714519049237, 0.2449999898672104]],
[0.0, 0.0, 1.0]),
([[-0.5644969545022632, 0.4427928463034304, 0.2449999898672104],
[-0.5644969545022632, 0.2752070590443937, 0.2449999898672104],
[-0.3955029644354199, 0.2752070590443937, 0.2449999898672104],
[-0.3955029644354199, 0.4427928463034304, 0.2449999898672104]],
[0.0, 0.0, 1.0]),
([[-0.08449704809882819, -0.037207136373545634, 0.2449999898672104],
[-0.08449704809882819, -0.20479296662328053, 0.2449999898672104],
[0.08449704809882819, -0.20479296662328053, 0.2449999898672104],
[0.08449704809882819, -0.037207136373545634, 0.2449999898672104]],
[0.0, 0.0, 1.0]),
([[-0.08449704589459173, 0.4427928551203777, 0.2449999898672104],
[-0.08449704589459173, 0.2752070502274464, 0.2449999898672104],
[0.08449704589459173, 0.2752070502274464, 0.2449999898672104],
[0.08449704589459173, 0.4427928551203777, 0.2449999898672104]],
[0.0, 0.0, 1.0]),
([[-0.32449705763557135, -0.2772071161079664, 0.2449999898672104],
[-0.32449705763557135, -0.44479294635770134, 0.2449999898672104],
[-0.15550296143791498, -0.44479294635770134, 0.2449999898672104],
[-0.15550296143791498, -0.2772071161079664, 0.2449999898672104]],
[0.0, 0.0, 1.0]),
([[-0.3244970554313349, 0.2027928753859569, 0.2449999898672104],
[-0.3244970554313349, 0.03520707049302565, 0.2449999898672104],
[-0.15550296364215144, 0.03520707049302565, 0.2449999898672104],
[-0.15550296364215144, 0.2027928753859569, 0.2449999898672104]],
[0.0, 0.0, 1.0]),
([[0.15550296143791498, -0.2772071161079664, 0.2449999898672104],
[0.15550296143791498, -0.44479294635770134, 0.2449999898672104],
[0.32449705763557135, -0.44479294635770134, 0.2449999898672104],
[0.32449705763557135, -0.2772071161079664, 0.2449999898672104]],
[0.0, 0.0, 1.0]),
([[0.15550296364215144, 0.2027928753859569, 0.2449999898672104],
[0.15550296364215144, 0.03520707049302565, 0.2449999898672104],
[0.3244970554313349, 0.03520707049302565, 0.2449999898672104],
[0.3244970554313349, 0.2027928753859569, 0.2449999898672104]],
[0.0, 0.0, 1.0]),
([[-0.6401920455917902, 0.28740094947165623, 0.12999999523162842],
[-1.5621051651015692, 0.28740094947165623, 0.12999999523162842],
[-1.5621051651015692, -0.45012955712622776, 0.12999999523162842],
[-0.6401920455917902, -0.45012955712622776, 0.12999999523162842]],
[0.0, -0.0, 1.0]),
([[1.5659601592927235, 0.46464466094067264, 0.15000000596046448],
[0.6366708374113782, 0.46464466094067264, 0.15000000596046448],
[0.6366708374113782, -0.46464466094067264, 0.15000000596046448],
[1.5659601592927235, -0.46464466094067264, 0.15000000596046448]],
[0.0, -0.0, 1.0])]
surfaces_and_normals = [
# first platform
([[-0.58, -1.2, 0.39],
[-0.6, 0., 0.07],
[0.61, -1.21, 0.39]],
[-0.007255147224555951, -0.277401422155636, 0.9607267113101313]),
([[-0.6, 0., 0.07],
[0.61,-0.004,0.09],
[0.61, -1.21, 0.39]],
[-0.007255147224555951, -0.277401422155636, 0.9607267113101313]),
# second platform
([[-.59,0.,0.06],
[-0.57,1.21,0.4],
[0.61, -0.004, 0.09]],
[-0.007255147224555951, -0.277401422155636, 0.9607267113101313]),
([[-0.57,1.21,0.4],
[0.6, 1.18, 0.43],
[0.61, -0.004, 0.09]],
[-0.007255147224555951, -0.277401422155636, 0.9607267113101313]),
# third platform
([[0.6,-1.21,0.07],
[0.62,-0.009,0.435],
[1.84, -1.22, 0.1]],
[-0.007255147224555951, -0.277401422155636, 0.9607267113101313]),
([[0.62,-0.009,0.435],
[1.82, -0.009, 0.48],
[1.84, -1.22, 0.1]],
[-0.007255147224555951, -0.277401422155636, 0.9607267113101313]),
# fourth platform
([[0.62,-0.009,0.435],
[0.6,1.22,0.1],
[1.82, -0.009, 0.48]],
[-0.007255147224555951, -0.277401422155636, 0.9607267113101313]),
([[0.6,1.22,0.1],
[1.84, 1.22, 0.13],
[1.82, -0.009, 0.48]],
[-0.007255147224555951, -0.277401422155636, 0.9607267113101313]),
]
all_surfaces = [el[0] for el in surfaces_and_normals]
......@@ -109,10 +68,26 @@ surfaces = None
def genSurf():
global surfaces
surfaces = [all_surfaces_array for _ in range (5)]
surfaces = [all_surfaces_array for _ in range (6)]
genSurf()
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 = all_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 surfaces]
return ax
def overrideSurfaces(surfs):
global all_surfaces
global all_surfaces_array
......@@ -123,6 +98,9 @@ def overrideSurfaces(surfs):
default_surfaces = [all_surfaces_array for _ in range (5)]
genSurf()
print (' HELLLOOO ')
draw_scene(all_surfaces)
draw_scene()
plt.show()
......@@ -139,8 +117,8 @@ def gen_pb():
# ~ def solve(initCom = default_init_com, initPos = default_init_pos, endCom = None):
def solve(initCom = None, initPos = None, endCom = None, surfaces = surfaces, ax = None):
print ("initPos ", initPos)
def solve(initCom = None, initPos = None, endCom = None, endPos = None, surfaces = surfaces, ax = None):
print ("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!initPos ", initPos)
# ~ print ("default_init_pos ", default_init_pos)
# ~ if endCom is None:
# ~ endCom = default_init_com + array([0.2, 0.0, 0.0])
......@@ -148,9 +126,9 @@ def solve(initCom = None, initPos = None, endCom = None, surfaces = surfaces, ax
pb = gen_pb()
endPos = None
# ~ print ("initPos", initPos)
pb, res, time = solveMIPGurobi(pb, surfaces, MIP = True, draw_scene = None, plot = True, l1Contact = False, initPos = initPos, endPos = endPos, initCom = initCom, endCom= endCom)
# ~ endPos = None
print ("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 = [(1, posturalCost),(1.5, targetCom)])
coms, footpos, allfeetpos = retrieve_points_from_res(pb, res)
if ax is not None:
plotQPRes(pb, res, ax=ax, plot_constraints = False, show = False, plotSupport = True)
......@@ -162,16 +140,31 @@ def solve(initCom = None, initPos = None, endCom = None, surfaces = surfaces, ax
print ("coms ", Coms)
print ("Allfeetpos ", Allfeetpos)
for i in range(11):
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):
# ~ for i in range(1):
# ~ print("round !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!", i )
pb = gen_pb()
initPos = None
endPos = None
# ~ endPos = None
initCom = Coms[-1].copy()
endCom = Coms[-1].copy() + array([.2, 0.0, 0.0])
endCom = Coms[-1].copy() + array([20, 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 = [(1., targetCom)])
coms, footpos, allfeetpos = retrieve_points_from_res(pb, res)
Coms += coms[1:]; Footpos += footpos[1:]; Allfeetpos += allfeetpos[1:]
# ~ if ax is not None:
......@@ -209,17 +202,35 @@ if __name__ == '__main__':
[draw_rectangle(l,ax) for l in surfaces]
return ax
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])]
# ~ 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])]
initPos = [array([0.60700772, 0.35028565, 0.19173743]),
array([0.38222138, 0.23795264, 0.15202783]),
array([-0.36776191, 0.34361369, 0.23769671]),
array([-0.25487645, -0.23401013, 0.25597898])]
initPos = [array([0.38437629, 0.18974121, 0.13850503]),
array([ 0.38437629, -0.18974121, 0.1326605 ]),
array([-0.38437629, 0.18974121, 0.11856727]),
array([-0.38437629, -0.18974121, 0.05008679])]
# ~ initPos = [array([0.6, 0.3, 0.2]),
# ~ array([0.6, -0.5, 0.2]),
# ~ array([0.2, 0.3, 0.2]),
# ~ array([0.5, -0.5, 0.2])]
endCom = [10., 0., 0.4]
endCom = [0, 1, 0.4]
ax = draw_scene()
plotPoints(ax, initPos)
# ~ plt.show()
pb, coms, footpos, allfeetpos, res = solve(initPos = initPos, endCom=endCom, ax = ax)
pb, coms, footpos, allfeetpos, res = solve(initPos = initPos, endCom=endCom, endPos = None, ax = ax)
# ~ 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