Commit 076b4f0f authored by Steve Tonneau's avatar Steve Tonneau
Browse files

contact generators for posture generator for arbitrary num contacts now functionnal

parent 3cd06448
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
from hpp.gepetto import Viewer
import sys
packageName = "hrp2_14_description"
meshPackageName = "hrp2_14_description"
rootJointType = "freeflyer"
##
# Information to retrieve urdf and srdf files.
urdfName = "hrp2_14"
urdfSuffix = "_reduced"
srdfSuffix = ""
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
#~ fullBody.setJointBounds ("base_joint_xyz", [-1,2, -2, 1, 0.5, 2.5])
#~ AFTER loading obstacles
rLegId = 'hrp2_rleg_rom'
rLeg = 'RLEG_JOINT0'
rLegOffset = [0,-0.105,0,]
rLegNormal = [0,1,0]
rLegx = 0.03; rLegy = 0.03
fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 10000, "manipulability", 0.03)
lLegId = 'hrp2_lleg_rom'
lLeg = 'LLEG_JOINT0'
lLegOffset = [0,-0.105,0]
lLegNormal = [0,1,0]
lLegx = 0.03; lLegy = 0.03
fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 10000, "manipulability", 0.03)
rarmId = 'hrp2_rarm_rom'
rarm = 'RARM_JOINT0'
rHand = 'RARM_JOINT5'
rArmOffset = [-0.05,-0.050,-0.050]
rArmNormal = [1,0,0]
rArmx = 0.024; rArmy = 0.024
fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 10000, "EFORT", 0.05)
larmId = 'hrp2_larm_rom'
larm = 'LARM_JOINT0'
lHand = 'LARM_JOINT5'
lArmOffset = [-0.05,-0.050,-0.050]
lArmNormal = [1,0,0]
lArmx = 0.024; lArmy = 0.024
fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 10000, "EFORT", 0.05)
scene = sys.argv[len(sys.argv)-1]
configFile = "creach_2DGrid_"+scene+'.pkl'
import pickle
pkl_file = open(configFile, 'rb')
ok_configs = pickle.load(pkl_file)
pkl_file.close()
ps = ProblemSolver( fullBody )
r = Viewer (ps)
r.loadObstacleModel ('hpp-rbprm-corba', scene, "planning")
limbs = [larmId, rarmId, lLegId, rLegId]
q_init = [
0, 0, 0, 1.0, 0.0 , 0.0, 0.0, # Free flyer 0-6
0.0, 0.0, 0.0, 0.0, # CHEST HEAD 7-10
0.261799388, 0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17, # LARM 11-17
0.261799388, -0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17, # RARM 18-24
0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0, # LLEG 25-30
0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0, # RLEG 31-36
]; r (q_init)
fullBody.setCurrentConfig (q_init)
negative = {}
positive = {}
nbnegative = 0
totalconfigs = 0
res = {}
x_start = -1.5
y_start = 0
x_max = 2.5
y_max = 2.65
iter_step = 0.1
res = {}
import numpy as np
nbStepsX = int((x_max - x_start) / iter_step)
nbStepsY = int((y_max - y_start) / iter_step)
x_start = 0
y_start = 0
x_max = 2
y_max = 1.64
iter_step = 0.01
x_t = []
y_t = []
#~ res = []
for x in ok_configs.keys():
for y in ok_configs[x].keys():
#~
#~ for x in np.linspace(x_start,x_max, num=nbStepsX):
#~ for y in np.linspace(y_start,y_max, num=nbStepsY):
q = q_init
q[0] = x
q[1] = -0.82
q[2] = y
#~ print "test"
if (fullBody.canGenerateBalancedContact(q, [0,0,1])):
#~ print "ok"
x_t.append(x)
y_t.append(y)
#~ res.append(fullBody.generateContacts(q, [0,0,1]))
x_t.append(0)
y_t.append(0) #for scale
x_t.append(2)
y_t.append(1.64) #for scale
import pickle
sFile = "cgeom_feasible_2DGrid_"+scene+'.pkl'
output = open(sFile, 'wb')
pickle.dump(res, output)
output.close()
import numpy as np
import matplotlib.pyplot as plt
#~ plt.scatter(x_t, y_t, s=200, marker='s', edgecolors='none')
#~ plt.scatter(x_t, y_t, s=170, marker='s' ,color ='c', edgecolors='none')
plt.scatter(x_t, y_t, edgecolors='none')
plt.show()
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
from hpp.gepetto import Viewer
import sys
packageName = "hrp2_14_description"
meshPackageName = "hrp2_14_description"
rootJointType = "freeflyer"
##
# Information to retrieve urdf and srdf files.
urdfName = "hrp2_14"
urdfSuffix = "_reduced"
srdfSuffix = ""
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
#~ fullBody.setJointBounds ("base_joint_xyz", [-1,2, -2, 1, 0.5, 2.5])
#~ AFTER loading obstacles
rLegId = 'hrp2_rleg_rom'
rLeg = 'RLEG_JOINT0'
rLegOffset = [0,-0.105,0,]
rLegNormal = [0,1,0]
rLegx = 0.09; rLegy = 0.05
fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 10000, "manipulability", 0.03)
lLegId = 'hrp2_lleg_rom'
lLeg = 'LLEG_JOINT0'
lLegOffset = [0,-0.105,0]
lLegNormal = [0,1,0]
lLegx = 0.09; lLegy = 0.05
fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 10000, "manipulability", 0.03)
rarmId = 'hrp2_rarm_rom'
rarm = 'RARM_JOINT0'
rHand = 'RARM_JOINT5'
rArmOffset = [-0.05,-0.050,-0.050]
rArmNormal = [1,0,0]
rArmx = 0.024; rArmy = 0.024
fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 10000, "EFORT", 0.05)
larmId = 'hrp2_larm_rom'
larm = 'LARM_JOINT0'
lHand = 'LARM_JOINT5'
lArmOffset = [-0.05,-0.050,-0.050]
lArmNormal = [1,0,0]
lArmx = 0.024; lArmy = 0.024
fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 10000, "EFORT", 0.05)
scene = sys.argv[len(sys.argv)-1]
configFile = "cgeom_feasible_2DGrid_"+scene+'.pkl'
import pickle
pkl_file = open(configFile, 'rb')
ok_configs = pickle.load(pkl_file)
pkl_file.close()
ps = ProblemSolver( fullBody )
r = Viewer (ps)
r.loadObstacleModel ('hpp-rbprm-corba', scene, "planning")
limbs = [larmId, rarmId, lLegId, rLegId]
q_init = [
0, 0, 0, 1.0, 0.0 , 0.0, 0.0, # Free flyer 0-6
0.0, 0.0, 0.0, 0.0, # CHEST HEAD 7-10
0.261799388, 0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17, # LARM 11-17
0.261799388, -0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17, # RARM 18-24
0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0, # LLEG 25-30
0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0, # RLEG 31-36
]; r (q_init)
fullBody.setCurrentConfig (q_init)
negative = {}
positive = {}
nbnegative = 0
totalconfigs = 0
res = {}
x_start = -1.5
y_start = 0
x_max = 2.5
y_max = 2.65
iter_step = 0.1
res = {}
import numpy as np
nbStepsX = int((x_max - x_start) / iter_step)
nbStepsY = int((y_max - y_start) / iter_step)
x_t = []
y_t = []
#~ res = []
for x in ok_configs.keys():
for y in ok_configs[x].keys():
q = q_init
q[0] = x
q[1] = 0
q[2] = y
#~ print "test"
if (fullBody.canGenerateBalancedContact(q, [0,0,1])):
#~ print "ok"
x_t.append(x)
y_t.append(y)
#~ res.append(fullBody.generateContacts(q, [0,0,1]))
import numpy as np
import matplotlib.pyplot as plt
import pickle
sFile = "cbalanced_feasible_2DGrid_"+scene+'.pkl'
output = open(sFile, 'wb')
pickle.dump(res, output)
output.close()
#~ plt.scatter(x_t, y_t, s=200, marker='s', edgecolors='none')
plt.scatter(x_t, y_t ,color ='c', edgecolors='none')
plt.show()
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.gepetto import Viewer
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
import sys
#~ print sys.args
rootJointType = 'freeflyer'
packageName = 'hpp-rbprm-corba'
meshPackageName = 'hpp-rbprm-corba'
urdfNameTested = 'hrp2_trunk_flexible'
urdfNameRoms = ['hrp2_larm_rom','hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom']
urdfSuffix = ""
srdfSuffix = ""
scene = sys.argv[len(sys.argv)-1]
tested = Builder ()
tested.loadModel(urdfNameTested, urdfNameRoms, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
#~ tested.setNormalFilter('hrp2_lleg_rom', [0,0,1], 0.9)
#~ tested.setFilter(['hrp2_lleg_rom','hrp2_rleg_rom'])
ps = ProblemSolver( tested )
r = Viewer (ps)
r.loadObstacleModel (packageName, scene, "planning")
tested.setJointBounds ("base_joint_xyz", [-10.,10,-10,10,0,20])
ps.client.problem.selectConFigurationShooter("RbprmShooter")
q_init = tested.getCurrentConfig ();
q_init [0:3] = [-10, -0.82, 1.25]; tested.setCurrentConfig (q_init); r (q_init)
q_goal = q_init [::]
q_goal [0:3] = [-9, -0.65, 1.25]; r (q_goal)
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
t = ps.solve ()
res = {}
x_start = -1.5
y_start = 0
x_max = 2.84
y_max = 2.65
iter_step = 0.01
res = {}
import numpy as np
nbStepsX = int((x_max - x_start) / iter_step)
nbStepsY = int((y_max - y_start) / iter_step)
x_t = []
y_t = []
q = q_init
for x in np.linspace(x_start,x_max, num=nbStepsX):
ys = {}
for y in np.linspace(y_start,y_max, num=nbStepsY):
q = q_init
q[0] = x
q[1] = 0
q[2] = y
if (tested.isReachable(q)):
#~ print "ok"
#~ print tested.isConfigValid(q)
x_t.append(x)
y_t.append(y)
ys[y] = True
res[x] = ys
#~ x_t.append(2.5)
#~ y_t.append(2.65) #for scale
#~
#~ x_t.append(-1.5)
#~ y_t.append(0) #for scale
#~ x_t.append(-0.91)
#~ y_t.append(0.2) #for scale
#~ x_t.append(-0.71)
#~ y_t.append(0.2) #for scale
#~
#~ x_t.append(-0.07)
#~ y_t.append(0.3) #for scale
#~ x_t.append(0.12)
#~ y_t.append(0.3) #for scale
#~
#~ x_t.append(0.78)
#~ y_t.append(0.4) #for scale
#~ x_t.append(0.98)
#~ y_t.append(0.4) #for scale
#~
#~ x_t.append(2.17)
#~ y_t.append(0.6) #for scale
#~ x_t.append(2.37)
#~ y_t.append(0.6) #for scale
#~
#~ x_t.append(0.58)
#~ y_t.append(2.35) #for scale
#~ x_t.append(1.08)
#~ y_t.append(2.35) #for scale
#~ x_t.append(0.58)
#~ y_t.append(2.65) #for scale
#~ x_t.append(1.08)
#~ y_t.append(2.65) #for scale
#~
#~ x_t.append(-1.5)
#~ y_t.append(0.) #for scale
#~ x_t.append(3.5)
#~ y_t.append(0.5) #for scale
#~
#~
#~ import pickle
#~ sFile = "creach_2DGrid_"+scene+'.pkl'
#~ output = open(sFile, 'wb')
#~ pickle.dump(res, output)
#~ output.close()
q[2] = y_t[0]
q[0] = x_t[0]
r(q)
import numpy as np
import matplotlib.pyplot as plt
#~ plt.scatter(x_t, y_t, s=170, marker='s', edgecolors='none')
plt.scatter(x_t, y_t, edgecolors='none')
#~ plt.scatter(x_t, y_t ,color ='c', edgecolors='none')
plt.show()
This source diff could not be displayed because it is too large. You can view the blob instead.
(lp0
(lp1
F1.3883679267337088
aF-1.3456031945776192
aF0.3800606994110079
aF-0.23940111318695914
aF0.09446131433393672
aF-0.25010033939826926
aF0.933388443967473
aF0.09628885190480152
aF0.2258939233130223
aF0.5972699880885892
aF-0.22924053491463303
aa(lp2
F1.7569348779882739
aF2.4999994423028578
aF0.3353622010435627
aF0.8273853369617395
aF0.1669956465857352
aF0.0022711036855769012
aF-0.5362283098552054
aF-0.30987404473155183
aF0.2838722903718278
aF-0.06058782552706543
aF0.28069000029251556
aa(lp3
F-0.3582234425994416
aF0.9999991032564858
aF0.37263911610602435
aF-0.1389824321853026
aF0.3116206858036602
aF0.3467615256367802
aF-0.8736892331150516
aF0.7563880918138912
aF0.7147913313638693
aF-0.023406005981432276
aF0.027061270862123732
aa(lp4
F0.29448831162037803
aF-1.3506572730310293
aF0.5226644834020887
aF-0.02780009604122432
aF0.6696626286429574
aF0.34880814843185187
aF-0.6550664043031411
aF-0.21812991110916247
aF0.7956506361546325
aF0.6892092354434634
aF0.7236519523611354
aa(lp5
F0.09336403284402675
aF-0.5451392530350248
aF1.8838116642568115
aF-0.653550741987522
aF-0.19411656532878824
aF0.6975505265236928
aF0.22048457918646516
aF0.3894244990264355
aF0.8479569175430033
aF0.00614240296933255
aF-0.10334450426638941
aa(lp6
F1.1032047182372777
aF-1.3459519791367784
aF1.124834360704175
aF0.35462515903159725
aF-0.0007570255503086359
aF-0.07499911661825624
aF-0.9319954699464004
aF-0.5571754809652705
aF0.3635894399307439
aF-0.5484276042483158
aF-0.35784751564099804
aa(lp7
F1.0786804465260489
aF-1.521229395515876
aF0.4479750843179203
aF0.9829671291520543
aF-0.09742526891284671
aF0.018826701020160905
aF0.15469161358174993
aF0.4290645770752852
aF0.8442968265690296
aF0.34807001959031636
aF0.2116438003055937
aa(lp8
F-0.36669795367944386
aF-0.9999999384573348
aF0.46428579785397533
aF0.03679183695965415
aF-0.4753128262545869
aF0.6404574956587745
aF0.6021115130816733
aF0.6653642864694047
aF0.6096701301584306
aF0.05312445498546703
aF-0.12378440455614514
aa(lp9
F1.2535496244424724
aF-1.3360167998093593
aF1.9999998446835354
aF-0.5065947245654163
aF-0.6979740840774187
aF-0.4454322495126895
aF0.24038318180114782
aF0.42341303970772914
aF0.9285445927985264
aF0.5551956689891042
aF0.6402711507963025
aa(lp10
F0.4657282582172765
aF-1.2825144923476108
aF0.3354012523798595
aF-0.6717446941519805
aF0.16721780095935518
aF0.5297010683082405
aF-0.4901163649115264
aF-0.19753405789571266
aF0.6818570817242299
aF-0.4389047106281159
aF-0.26541499276197844
aa(lp11
F0.1490791268469345
aF0.9999994067308957
aF0.5986142290470846
aF-0.6118200411977466
aF-0.013892918871346766
aF0.2701252311585237
aF-0.7433139198787926
aF-0.38103160442794193
aF0.447221436299031
aF0.3972098804456683
aF0.34946582862369435
aa(lp12
F-0.46702129402268
aF0.013809315783447428
aF0.36501059236213884
aF-0.9284397206592203
aF0.10379953196584332
aF-0.2811230398378621
aF0.2195340036035891
aF0.3747327888501486
aF0.5149107414982749
aF0.3597320884958888
aF0.04005286298746935
aa(lp13
F-2.836434665447795
aF0.8471558027827126
aF0.0
aF0.5822161019027822
aF-0.2996157245633785
aF0.07995597169293904
aF0.7515729311717717
aF0.4584200893454544
aF0.5059227627620436
aF-0.03833135144107103
aF-0.24395839178899043
aa(lp14
F0.07116534288633657
aF-0.027451304927376546
aF0.621434141703625
aF0.6937145692915118
aF0.022826961109316084
aF-0.22807492222125889
aF-0.6828036731396316
aF0.09329646749253695
aF-0.021355170652321634
aF-0.718771439708852
aF-0.1667409174129948
aa(lp15
F1.8640682598622016
aF2.4999993567746306
aF0.3867932241010556
aF-0.05300381148733282
aF-0.12634925788609244
aF-0.3082419173119624
aF-0.9413890701571239
aF0.1646240452984301
aF0.23684408760813694
aF0.6174344700060899
aF-0.17211494418238937
aa(lp16
F-0.02144319687972488
aF-0.26714076741503756
aF0.6505086314513084
aF0.5289908857442797
aF0.5856694414332689