Commit 550a5e72 authored by Steve T's avatar Steve T
Browse files

flat ground scenario working

parent a796b1e9
......@@ -381,8 +381,10 @@ def convertProblemToLp(pb, convertSurfaces = True):
startRow = SurfaceConstraint(phaseDataT, A, b, startCol, endCol, startRow)
startRow = SlackPositivityConstraint(phaseDataT, A, b, startCol, endCol, startRow)
#equalities
startRowEq = CoMWeightedEqualityConstraint(phaseDataT, E, e, startCol, endCol, startRowEq)
#equalities
#no weighted com on first phase
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
......@@ -533,7 +535,7 @@ def plotConstraints(ax, pb, allfeetpos, coms):
print("qhullfailed")
from scipy.spatial import ConvexHull
from tools.plot_plytopes import plot_hull
from sl1m.tools.plot_plytopes import plot_hull
def plotQPRes(pb, res, linewidth=2, ax = None, plot_constraints = False, show = True, plotSupport = False):
coms, footpos, allfeetpos = retrieve_points_from_res(pb, res)
......
from hpp.corbaserver.rbprm.anymal import Robot
from hpp.gepetto import Viewer
from hpp.corbaserver import Client
from tools.display_tools import *
import time
from hpp.corbaserver.rbprm import rbprmstate
from hpp.corbaserver.rbprm import state_alg
from numpy import array, zeros, ones
#~ rootJointType = 'freeflyer'
packageName = 'hpp-rbprm-corba'
meshPackageName = 'hpp-rbprm-corba'
#~ urdfName = 'hrp2_trunk_flexible'
#~ urdfNameRoms = ['hrp2_larm_rom','hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom']
#~ urdfSuffix = ""
#~ srdfSuffix = ""
fullBody = Robot ()
fullBody.setJointBounds ("root_joint", [-20,20, -10, 10, 0, 2.2])
fullBody.client.robot.setDimensionExtraConfigSpace(0)
from hpp.corbaserver.problem_solver import ProblemSolver
ps = ProblemSolver( fullBody )
r = Viewer (ps)
v = Viewer (ps,viewerClient=r.client, displayCoM = True)
# ~ fullBody.setConstrainedJointsBounds()
fullBody.setJointBounds("LF_KFE",[-1.4,0.])
fullBody.setJointBounds("RF_KFE",[-1.4,0.])
fullBody.setJointBounds("LH_KFE",[0.,1.4])
fullBody.setJointBounds("RH_KFE",[0.,1.4])
fullBody.setJointBounds ("root_joint", [-20,20, -20, 20, -20, 20])
dict_heuristic = {fullBody.rLegId:"static", fullBody.lLegId:"static", fullBody.rArmId:"fixedStep04", fullBody.lArmId:"fixedStep04"}
fullBody.loadAllLimbs(dict_heuristic,"ReferenceConfiguration",nbSamples=12)
q_ref = fullBody.referenceConfig[::]
q_init = q_ref[::]
fullBody.setReferenceConfig(q_ref)
fullBody.setCurrentConfig (q_init)
fullBody.setPostureWeights(fullBody.postureWeights[::])
from sl1m.stand_alone_scenarios.anymal.flat_ground import solve
def getContactsFromConfig(q, limbs = Robot.limbs_names):
s = rbprmstate.State(fullBody, q = q, limbsIncontact = limbs)
res = {}
for limb in limbs:
rLegPn = s.getContactPosAndNormalsForLimb(limb)
res[limb] = (rLegPn[0][0], rLegPn[1][0])
return res
# ~ rLegPn = s.getContactPosAndNormalsForLimb(Robot.rLegId)
# ~ lLegPn = s.getContactPosAndNormalsForLimb(Robot.lLegId)
# ~ return { Robot.rLegId : (rLegPn[0][0], rLegPn[1][0]),Robot.lLegId : (lLegPn[0][0], lLegPn[1][0]) }
#given by limb constraints
from sl1m.stand_alone_scenarios.constraints_anymal import limbNames, effectorNames
from hpp.corbaserver.rbprm.rbprmstate import StateHelper
initContacts = getContactsFromConfig(q_init)
initPos = [array(initContacts[el][0][0]) for el in limbNames]
initCom = array(fullBody.getCenterOfMass())
# ~ pb, coms, footpos, allfeetpos, res = solve(initCom = None, initPos = None)
pb, coms, footpos, allfeetpos, res = solve(initCom = initCom, initPos = initPos)
z = array([0.,0.,1.])
from scipy.optimize import linprog
#static eq is com is convex combination of pos (projected)
def staticEq(positions, com):
sizeX = len(positions)
E = zeros((3,sizeX))
for i, pos in enumerate(positions):
E[:2,i] = pos[:2]
e = array([com[0], com[1], 1.])
E[2,:] = ones(sizeX)
try:
res = linprog(ones(sizeX), A_ub=None, b_ub=None, A_eq=E, b_eq=e, bounds=[(0.,1.) for _ in range(sizeX)], method='interior-point', callback=None, options={'presolve': True})
return res['success']
except:
return False
def gen_state(s, pId, num_max_sample = 1, first = False, normal = z, newContact = True , useCom = False ):
#~ pId = 6
phase = pb["phaseData"][pId]
feetPos = allfeetpos[pId]
sres, succ = StateHelper.cloneState(s)
for limbId, pos in zip(limbNames, feetPos):
sres, succ = state_alg.addNewContact(sres, limbId, pos.tolist(), normal.tolist(), num_max_sample= 200)
# ~ v(sres.q())
if not succ:
print("succes ?", succ)
succCom = False
ite = 0
# ~ #~ if first:
# ~ #~ print "FIRST "
# ~ #~ com[2] -= 0.25
# ~ #~ while(not succCom and ite < 11):
com = coms[pId*2].tolist()
# ~ print("com ", com)
#check whether com is feasible
if staticEq(feetPos, com):
print ("Config equilibrium")
succCom = True
if useCom:
while(not succCom and ite < 30):
succCom = fullBody.projectStateToCOM(sres.sId ,com, num_max_sample)
com[2] -= 0.05
ite += 1
if succCom:
sres2, succ = StateHelper.cloneState(sres)
q = sres2.q()
q[3:7] = [0.,0.,0.,1.]
q[3:] = fullBody.referenceConfig[3:]
sres2.setQ(q)
succCom2 = fullBody.projectStateToCOM(sres2.sId ,com, num_max_sample)
if succCom2:
print ("SUCCESS COM2 ", succCom)
sres = sres2
print ("SUCCESS COM ", succCom)
v(sres.q())
return sres
v(q_init)
s = rbprmstate.State(fullBody, q = q_init, limbsIncontact = limbNames[:])
states = [s]
def run():
for i, _ in enumerate(allfeetpos[1:]):
states.append(gen_state(states[-1],i+1, useCom = True))
def play():
for s in states:
v(s.q())
time.sleep(0.5)
run()
play()
extraDof = [0 for _ in range(6)]
configs = [ st.q() + extraDof for st in states[:]]; i = 0
......@@ -2,7 +2,7 @@
gepetto-gui &
hpp-rbprm-server &
ipython -i --no-confirm-exit ./$1
ipython3 -i --no-confirm-exit ./$1
pkill -f 'gepetto-gui'
pkill -f 'hpp-rbprm-server'
......@@ -7,7 +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 sl1m.stand_alone_scenarios.constraints_anymal import *
......@@ -18,8 +19,8 @@ floor = [ [-10, -10., 0.], [10, -10., 0.], [10, 10., 0.], [-10, 10., 0.] ]
afloor = array(floor).T
all_surfaces = [floor]
# ~ surfaces = [[afloor] for _ in range (10)]
surfaces = [[afloor] for _ in range (10)]
# ~ surfaces = [[afloor] for _ in range (5)]
def gen_flat_pb():
kinematicConstraints = genCOMConstraints()
......@@ -52,6 +53,17 @@ def draw_scene(surfaces, ax = None, color = "p"):
return ax
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])
initGlobals(nEffectors = 4)
pb = gen_flat_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)
coms, footpos, allfeetpos = retrieve_points_from_res(pb, res)
return pb, coms, footpos, allfeetpos, res
############# main ###################
......@@ -69,9 +81,9 @@ if __name__ == '__main__':
endPos = None
initCom = None
endCom = None
# ~ initCom = [0.2, 0., 0.5]
initCom = [0.2, 0., 0.5]
# ~ endCom = [2, 0.7, 0.5]
endCom = [1.2, 0.2, 0.5]
# ~ endCom = [1.2, 0.2, 0.5]
# ~ initPos = [array([0.5, 0.0, 0.0]), array([-0.1, 0.0, -0.0]), array([0.3, 0.5, 0.0]), array([-0.0, 0.4, 0.0])]
# ~ endPos = [array()]
# ~ initPos = [[0.2, 0.6, 0.0] ]; initPos = [array(el) for el in initPos]
......@@ -89,17 +101,17 @@ if __name__ == '__main__':
bcom = b - A[:,4:].dot(res[4:]) - A[:,2].dot(res[2])
Acom = np.hstack([A[:,:2], A[:,3:4]])
for i in range(5):
print ("WTTTTTTTTTTFFFFFFFFFFFFFFf!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!", i)
coms, footpos, allfeetpos = retrieve_points_from_res(pb, res)
pb = gen_flat_pb()
initPos = None
endPos = None
initCom = coms[-1]
endCom = coms[-1] + array([0.6, 0.0, 0.0])
initPos = allfeetpos[-1]
pb, res, time = solveMIPGurobi(pb, surfaces, MIP = True, draw_scene = None, plot = True, l1Contact = False, initPos = initPos, endPos = endPos, initCom = initCom, endCom= endCom)
plotQPRes(pb, res, ax=ax, plot_constraints = False, show = False, plotSupport = True)
# ~ for i in range(5):
# ~ print ("WTTTTTTTTTTFFFFFFFFFFFFFFf!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!", i)
# ~ coms, footpos, allfeetpos = retrieve_points_from_res(pb, res)
# ~ pb = gen_flat_pb()
# ~ initPos = None
# ~ endPos = None
# ~ initCom = coms[-1]
# ~ endCom = coms[-1] + array([0.6, 0.0, 0.0])
# ~ initPos = allfeetpos[-1]
# ~ pb, res, time = solveMIPGurobi(pb, surfaces, MIP = True, draw_scene = None, plot = True, l1Contact = False, initPos = initPos, endPos = endPos, initCom = initCom, endCom= endCom)
# ~ plotQPRes(pb, res, ax=ax, plot_constraints = False, show = False, plotSupport = True)
# ~ plt.show(block = False)
# ~ pb, res, time = solveMIPGurobi(pb, surfaces, MIP = True, draw_scene = None, plot = True, l1Contact = False, initPos = None)
......
# Blender v2.82 (sub 7) OBJ File: ''
# www.blender.org
o anymal_COM_constraints_in_LFleg_effector_frame_quasi_static_red_anymal_COM_constraints_in_LFleg_effector_frame_quasi_static
v 0.009012 -0.479382 0.300503
v 0.073600 -0.337258 0.373977
v -0.586896 -0.048687 0.321994
v -0.596280 -0.307600 0.305818
v -0.152688 -0.491093 0.437979
v -0.369281 -0.442404 0.477921
v -0.345318 -0.550967 0.387976
v -0.424891 0.072019 0.507819
v -0.126346 0.084849 0.498631
v -0.520179 -0.488680 0.307144
v -0.535686 -0.217019 0.466160
v -0.171698 -0.112015 0.569923
v 0.070800 -0.060916 0.419624
v -0.334143 -0.230251 0.570256
vn 0.5421 -0.1316 0.8299
vn 0.4313 -0.2772 0.8586
vn 0.1283 0.6011 -0.7888
vn -0.5007 -0.6151 0.6091
vn -0.4560 0.0467 0.8888
vn -0.0526 0.8578 -0.5113
vn -0.8024 0.2281 0.5514
vn -0.6193 -0.4227 0.6617
vn 0.2420 -0.9486 0.2038
vn -0.1191 0.1664 0.9788
vn -0.0015 -0.6382 0.7699
vn 0.2275 -0.3099 0.9232
vn 0.0060 -0.7857 -0.6185
vn 0.0145 0.3375 0.9412
vn 0.5570 -0.5669 0.6069
vn -0.4540 -0.2913 0.8420
vn -0.9376 0.0123 0.3474
vn -0.8036 -0.3341 0.4925
vn 0.5980 0.1365 -0.7898
vn 0.1467 0.2507 -0.9569
vn 0.0088 0.0620 -0.9980
vn 0.0759 -0.4085 0.9096
vn -0.0123 -0.0125 -0.9998
vn 0.4864 0.1963 0.8514
v -0.015055 -0.485060 0.301539
v -0.468358 0.056761 0.480433
v -0.162166 0.071835 0.513634
v -0.307255 -0.536888 0.387818
v -0.501579 -0.476755 0.310979
v 0.077928 -0.327392 0.361252
v -0.169121 -0.455034 0.457588
v -0.611680 -0.102618 0.309403
v -0.320386 -0.437897 0.488681
v -0.332482 -0.246689 0.558111
v -0.508515 -0.182030 0.486247
v -0.037637 0.000057 0.459908
v -0.249047 -0.092408 0.582926
v -0.062533 -0.181160 0.508927
vn 0.4258 0.1511 0.8921
vn 0.1807 0.3416 -0.9223
vn -0.8805 -0.2574 0.3981
vn 0.4711 0.1694 0.8656
vn 0.2815 -0.8518 0.4419
vn -0.3736 0.0853 0.9236
vn -0.4431 -0.6680 0.5979
vn 0.0499 0.6525 -0.7562
vn -0.8226 0.1517 0.5480
vn -0.1182 0.4384 0.8910
vn 0.0592 0.7051 -0.7066
vn 0.2419 -0.2689 0.9323
vn -0.5860 -0.4243 0.6903
vn -0.0196 -0.0100 -0.9998
vn 0.4892 -0.3409 0.8028
vn -0.0251 -0.8171 -0.5759
vn 0.2269 -0.2731 0.9348
vn 0.1543 -0.3286 0.9318
vn -0.3628 0.0465 0.9307
vn 0.1584 0.2666 -0.9507
vn 0.5353 -0.5564 0.6355
vn 0.7572 0.0716 0.6493
vn -0.4580 -0.3289 0.8259
vn 0.0643 -0.7080 0.7033
s off
f 12//1 2//1 13//1
f 2//2 12//2 5//2
f 3//3 9//3 13//3
f 7//4 6//4 10//4
f 8//5 11//5 14//5
f 3//6 8//6 9//6
f 11//7 8//7 3//7
f 10//8 6//8 11//8
f 7//9 1//9 5//9
f 14//10 12//10 8//10
f 6//11 7//11 5//11
f 12//12 14//12 5//12
f 7//13 10//13 1//13
f 8//14 12//14 9//14
f 2//15 5//15 1//15
f 14//16 11//16 6//16
f 4//17 11//17 3//17
f 10//18 11//18 4//18
f 2//19 1//19 13//19
f 3//20 13//20 1//20
f 4//21 3//21 1//21
f 14//22 6//22 5//22
f 10//23 4//23 1//23
f 12//24 13//24 9//24
f 3//1 13//1 14//1
f 8//2 12//2 6//2
f 5//3 11//3 8//3
f 3//4 14//4 12//4
f 7//5 4//5 1//5
f 11//6 13//6 2//6
f 4//7 9//7 5//7
f 3//8 12//8 2//8
f 8//9 11//9 2//9
f 2//10 13//10 3//10
f 8//11 2//11 12//11
f 7//12 14//12 13//12
f 11//13 5//13 9//13
f 1//14 5//14 8//14
f 6//15 14//15 7//15
f 1//16 4//16 5//16
f 7//17 13//17 10//17
f 9//18 7//18 10//18
f 11//19 10//19 13//19
f 1//20 8//20 6//20
f 7//21 1//21 6//21
f 6//22 12//22 14//22
f 9//23 10//23 11//23
f 4//24 7//24 9//24
# Blender v2.82 (sub 7) OBJ File: ''
# www.blender.org
o anymal_COM_constraints_in_LHleg_effector_frame_quasi_static_red_anymal_COM_constraints_in_LHleg_effector_frame_quasi_static
v -0.051855 -0.000537 0.419507
v 0.541555 -0.306004 0.303705
v 0.348859 -0.121692 0.544058
v -0.003034 -0.489892 0.314639
v 0.100071 -0.298825 0.513467
v 0.424173 -0.518471 0.316434
v -0.091265 -0.388342 0.298486
v 0.187167 -0.536224 0.397235
v 0.249874 -0.248405 0.561721
v 0.530183 -0.019036 0.354205
v 0.130148 0.061118 0.520666
v -0.068492 -0.224388 0.416904
v 0.392231 0.058253 0.508094
v 0.467106 -0.360915 0.428784
vn -0.9933 0.0728 0.0903
vn 0.0491 0.1844 0.9816
vn 0.4280 -0.2118 0.8786
vn -0.4436 -0.5201 0.7299
vn 0.2968 -0.6067 0.7374
vn 0.0172 -0.0693 -0.9974
vn 0.0057 0.1337 0.9910
vn -0.0266 0.8742 -0.4849
vn -0.0313 0.3006 -0.9532
vn -0.5998 -0.4121 0.6859
vn -0.0067 -0.1628 -0.9866
vn -0.3087 0.0068 0.9511
vn 0.8413 -0.4465 0.3047
vn -0.5831 -0.4132 0.6994
vn 0.6798 -0.0173 0.7332
vn -0.0602 -0.9233 -0.3794
vn 0.7464 0.0074 0.6654
vn -0.1222 -0.4722 0.8729
vn -0.0122 0.8891 -0.4575
vn -0.4918 0.0237 0.8704
vn 0.2333 -0.5203 0.8215
vn 0.8681 -0.0525 0.4937
vn -0.1046 0.1683 -0.9802
vn -0.4882 0.0261 0.8723
v -0.016798 -0.472487 0.301533
v -0.082214 -0.096625 0.398295
v 0.532530 -0.058062 0.369943
v 0.398955 -0.511551 0.339610
v -0.056603 -0.313775 0.389855
v 0.352999 0.050747 0.497412
v 0.175979 -0.075108 0.552406
v 0.198711 -0.519174 0.420027
v 0.532482 -0.359493 0.313836
v 0.057530 -0.218154 0.503788
v 0.450527 -0.050102 0.481492
v 0.090371 0.058974 0.492339
v 0.288767 -0.198191 0.546486
v 0.429831 -0.325967 0.465325
vn -0.9633 -0.1040 -0.2473
vn -0.0156 0.1830 -0.9830
vn -0.0049 0.4062 0.9138
vn -0.4713 0.0921 0.8772
vn 0.0365 0.7849 -0.6185
vn -0.0669 -0.9638 -0.2580
vn 0.4377 -0.0853 0.8951
vn -0.0597 0.2391 -0.9692
vn 0.3255 -0.5668 0.7568
vn 0.8119 -0.1069 0.5739
vn -0.5434 0.0988 0.8337
vn -0.4961 -0.5139 0.6998
vn 0.2764 0.1167 0.9539
vn 0.7989 -0.0947 0.5940
vn -0.6564 -0.1064 0.7469
vn -0.1442 -0.3275 0.9338
vn 0.2024 0.1389 0.9694
vn 0.1573 -0.3999 0.9030
vn -0.4358 -0.4251 0.7933
vn 0.6750 0.5810 0.4548
vn -0.0736 0.5744 -0.8153
vn -0.1618 -0.1948 0.9674
vn 0.6670 -0.4907 0.5606
vn 0.0679 -0.2244 -0.9721
s off
f 12//1 1//1 7//1
f 11//2 3//2 13//2
f 9//3 14//3 3//3
f 4//4 8//4 5//4
f 6//5 14//5 8//5
f 2//6 6//6 7//6
f 9//7 3//7 11//7
f 1//8 11//8 10//8
f 1//9 2//9 7//9
f 4//10 12//10 7//10
f 6//11 4//11 7//11
f 5//12 9//12 11//12
f 14//13 6//13 2//13
f 12//14 4//14 5//14
f 14//15 13//15 3//15
f 8//16 4//16 6//16
f 14//17 10//17 13//17
f 8//18 9//18 5//18
f 10//19 11//19 13//19
f 1//20 5//20 11//20
f 9//21 8//21 14//21
f 14//22 2//22 10//22
f 1//23 10//23 2//23
f 1//24 12//24 5//24
f 1//1 5//1 2//1
f 9//2 1//2 3//2
f 7//3 6//3 12//3
f 12//4 10//4 7//4
f 3//5 12//5 6//5
f 4//6 8//6 1//6
f 14//7 11//7 13//7
f 3//8 1//8 2//8
f 14//9 8//9 4//9
f 9//10 3//10 14//10
f 10//11 12//11 2//11
f 1//12 8//12 5//12
f 13//13 11//13 6//13
f 14//14 3//14 11//14
f 10//15 2//15 5//15
f 8//16 13//16 10//16
f 13//17 6//17 7//17
f 14//18 13//18 8//18
f 5//19 8//19 10//19
f 11//20 3//20 6//20
f 2//21 12//21 3//21
f 10//22 13//22 7//22
f 4//23 9//23 14//23
f 9//24 4//24 1//24
# Blender v2.82 (sub 7) OBJ File: ''
# www.blender.org
o anymal_COM_constraints_in_RFleg_effector_frame_quasi_static_red_anymal_COM_constraints_in_RFleg_effector_frame_quasi_static
v 0.057018 -0.009083 0.425123
v -0.550712 0.141725 0.450049
v -0.424882 0.276843 0.530327
v -0.284767 -0.097113 0.541674
v -0.410317 0.536399 0.375664
v -0.544743 0.443559 0.303190
v -0.463563 -0.061081 0.489385
v -0.284857 0.103390 0.585708
v -0.611436 0.077270 0.301546
v -0.070050 0.103717 0.526322
v -0.266923 0.380085 0.533398
v 0.047738 0.292647 0.417180
v 0.019692 0.470538 0.295226
v -0.156040 0.535813 0.384692
vn 0.0218 0.8276 -0.5609
vn -0.0238 0.6992 0.7145
vn 0.9817 0.0252 -0.1890
vn -0.8433 -0.2726 0.4631
vn 0.6439 0.0399 0.7640
vn -0.0144 0.0071 -0.9999
vn 0.0084 -0.8097 -0.5868
vn 0.3810 -0.3416 0.8592
vn 0.5326 0.5341 0.6566
vn -0.9358 0.1690 0.3093
vn -0.7262 0.3124 0.6125
vn 0.1450 -0.2481 -0.9578
vn -0.6381 0.4203 0.6452
vn -0.3394 0.4955 0.7996
vn 0.3974 0.2606 0.8799
vn -0.5005 -0.0474 0.8644
vn 0.2627 0.1628 0.9511
vn -0.1449 0.1929 0.9705
vn 0.4170 0.4546 0.7870
vn 0.2610 -0.2070 0.9429
vn -0.4304 -0.0599 0.9007
vn -0.0418 0.6517 -0.7573
vn -0.3124 -0.2039 0.9278
vn 0.0066 -0.8027 -0.5964
v 0.029214 0.036582 0.438548
v -0.584992 0.052184 0.327828
v 0.068618 0.384956 0.319293
v -0.220395 -0.081406 0.536513
v -0.081610 0.524502 0.350688
v 0.001440 0.280117 0.453927
v -0.446495 -0.039639 0.498840