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
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
# Blender MTL File: 'None'
# Material Count: 1
newmtl None
Ns 0
Ka 0.000000 0.000000 0.000000
Kd 0.8 0.8 0.8
Ks 0.8 0.8 0.8
d 1
illum 2
This diff is collapsed.
# Blender MTL File: 'None'
# Material Count: 1
newmtl None
Ns 0
Ka 0.000000 0.000000 0.000000
Kd 0.8 0.8 0.8
Ks 0.8 0.8 0.8
d 1
illum 2
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
# Blender MTL File: 'None'
# Material Count: 1
newmtl None
Ns 0
Ka 0.000000 0.000000 0.000000
Kd 0.8 0.8 0.8
Ks 0.8 0.8 0.8
d 1
illum 2
This diff is collapsed.
# Blender MTL File: 'None'
# Material Count: 1
newmtl None
Ns 0
Ka 0.000000 0.000000 0.000000
Kd 0.8 0.8 0.8
Ks 0.8 0.8 0.8
d 1
illum 2
This diff is collapsed.
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)
scale = sys.argv[len(sys.argv)-2]
scene = sys.argv[len(sys.argv)-1]
#~ configFile = sys.argv[len(sys.argv)-1]
import pickle
sFile = "false_negative_configs_"+scene+'_'+scale+'.pkl'
pkl_file = open(sFile, 'rb')
falseNeg = pickle.load(pkl_file)
pkl_file.close()
ps = ProblemSolver( fullBody )
r = Viewer (ps)
r.loadObstacleModel ('hpp-rbprm-corba', scene, "planning")
q_init = [
0.1, -0.82, 0.648702, 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)
confsize = len(falseNeg[0])
falseFalseNegative = 0
trueFalseNegative = 0
nbnegative = 0
totalconfigs = 0
q_init = fullBody.getCurrentConfig();
for q in falseNeg:
q_init[0:confsize] = q[0:confsize]
totalconfigs = totalconfigs + 1
#~ print "avant " + str(fullBody.isConfigValid(q_init)) + str(q_init)
#~ q_init = fullBody.makeCollisionFree(q_init)
#~ print "apres " + str(fullBody.isConfigValid(q_init)) + str(q_init)
#~ raise ValueError ("tg")
#~ fullBody.canGenerateLimbContact(limb, q_init)
if (fullBody.canGenerateBalancedContact(q_init, [0,0,1])):
trueFalseNegative = trueFalseNegative + 1
else:
falseFalseNegative = falseFalseNegative +1
#~
f = open('log.txt', 'a')
f.write("size 1 \n \t true false negative " + str(float(trueFalseNegative)/float(len(falseNeg))) + "\n")
f.write("size 1 \n \t false false negative " + str(float(falseFalseNegative)/float(len(falseNeg))) + "\n")
f.close()
#~ for limb in positive.keys():
#~ print 'positive ' + limb + ': ' + str(len(positive[limb]))
#~
#~ for limb in negative.keys():
#~ print 'negative ' + limb + ': ' + str(len(negative[limb]))
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 = 0
y_start = 0
x_max = 2
y_max = 1.64
iter_step = 0.01
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 = []
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.82
q[1] = 0
q[2] = y
if (tested.isReachable(q)):
x_t.append(x)
y_t.append(y)
ys[y] = True
res[x] = ys
x_t.append(0)
y_t.append(0) #for scale
#~ x_t.append(0.25)
#~ y_t.append(0)
#~
#~ x_t.append(0.3)
#~ y_t.append(0.15)
#~ x_t.append(0.6)
#~ y_t.append(0.15)
#~
#~ x_t.append(0.6)
#~ y_t.append(0.30)
#~ x_t.append(0.9)
#~ y_t.append(0.30)
#~
#~ x_t.append(0.9)
#~ y_t.append(0.45)
#~ x_t.append(1.2)
#~ y_t.append(0.45)
#~
#~ x_t.append(1.2)
#~ y_t.append(0.6)
#~ x_t.append(1.80)
#~ y_t.append(0.6)
#~
#~ x_t.append(0.4)
#~ y_t.append(0.81)
#~ x_t.append(2)
#~ y_t.append(1.6)
#~ 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.87)
#~ y_t.append(0.2) #for scale
#~ x_t.append(-0.67)
#~ y_t.append(0.2) #for scale
#~
#~ x_t.append(-0.23)
#~ y_t.append(0.4) #for scale
#~ x_t.append(-0.03)
#~ y_t.append(0.4) #for scale
#~
#~ x_t.append(0.77)
#~ y_t.append(0.6) #for scale
#~ x_t.append(0.97)
#~ y_t.append(0.6) #for scale
#~
#~ x_t.append(2.2)
#~ y_t.append(1) #for scale
#~ x_t.append(2.4)
#~ y_t.append(1) #for scale
#~
#~ x_t.append(0.62)
#~ y_t.append(2.35) #for scale
#~ x_t.append(1.12)
#~ y_t.append(2.35) #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()
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