contact generators for posture generator for arbitrary num contacts now functionnal

script/tools/LA_com_reduced.mtl

+# 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

script/tools/LL_com._reduced.mtl

+# 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

script/tools/RA_com_reduced.mtl

+# 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

script/tools/RL_com._reduced.mtl

+# 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

script/tools/calibrate-w/3_equilibrium_feasibility.py

+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]))

script/tools/calibrate-w/cbalanced_feasible_2DGrid_stair_bauzil.pkl

+(dp0
+.
\ No newline at end of file

script/tools/calibrate-w/cgeom_feasible_2DGrid_stair_bauzil.pkl

+(dp0
+.
\ No newline at end of file

script/tools/calibrate-w/computecol_free.py

+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()