fixed rectangle contact computation

idl/hpp/corbaserver/rbprm/rbprmbuilder.idl

@@ -126,6 +126,13 @@ module hpp
     /// and the selected sample
     floatSeq getSamplePosition(in string sampleName, in unsigned short sampleId) raises (Error);
 
+    /// Get the end effector position for a given configuration, assuming z normal
+    /// \param limbName name of the limb from which to retrieve a sample
+    /// \param dofArray configuration of the robot
+    /// \return world position of the limb end effector given the current robot configuration.
+    /// array of size 4, where each entry is the position of a corner of the contact surface
+    floatSeqSeq getEffectorPosition(in string limbName, in floatSeq dofArray) raises (Error);
+
     /// Get the end effector position of a given limb configuration
     /// \param limbName name of the limb from which to retrieve a sample
     /// \return number of samples generated for the limb

script/scenarios/car_hrp2_path.py

@@ -16,8 +16,11 @@ rbprmBuilder.setJointBounds ("base_joint_xyz", [-1,2, -1.5, 1, 0.5, 0.9])
 rbprmBuilder.setFilter(['hrp2_lleg_rom','hrp2_rleg_rom','hrp2_larm_rom'])
 #~ rbprmBuilder.setNormalFilter('hrp2_larm_rom', [0,0,1], 0.4)
 #~ rbprmBuilder.setNormalFilter('hrp2_rarm_rom', [0,0,1], 0.4)
-rbprmBuilder.setNormalFilter('hrp2_lleg_rom', [0,0,1], 0.6)
-rbprmBuilder.setNormalFilter('hrp2_rleg_rom', [0,0,1], 0.6)
+rbprmBuilder.setAffordanceFilter('3Rarm', ['Support','Lean'])
+rbprmBuilder.setAffordanceFilter('4Larm', ['Support','Lean'])
+rbprmBuilder.setAffordanceFilter('0rLeg', ['Support'])
+rbprmBuilder.setAffordanceFilter('1lLeg', ['Support'])
+
 rbprmBuilder.boundSO3([-1.5,1.5,0,3,-0.0,0.0])
 
 #~ from hpp.corbaserver.rbprm. import ProblemSolver
@@ -42,9 +45,14 @@ ps.addPathOptimizer("RandomShortcut")
 ps.setInitialConfig (q_init)
 ps.addGoalConfig (q_goal)
 
-ps.client.problem.selectConFigurationShooter("RbprmShooter")
-ps.client.problem.selectPathValidation("RbprmPathValidation",0.01)
-r.loadObstacleModel (packageName, "car", "planning")
+from hpp.corbaserver.affordance.affordance import AffordanceTool
+afftool = AffordanceTool ()
+afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
+afftool.loadObstacleModel (packageName, "car", "planning", r)
+#~ afftool.analyseAll()
+afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
+afftool.visualiseAffordances('Lean', r, [0, 0, 0.9])
+
 t = ps.solve ()
 print (t)
 if isinstance(t, list):
@@ -64,6 +72,9 @@ pp = PathPlayer (rbprmBuilder.client.basic, r)
 
 rob = rbprmBuilder.client.basic.robot
 r(q_init)
+q_loin = q_init[::]
+q_loin[0:3] = [100,100,100]
+r (q_loin)
 
 
 #~ configs = []

script/scenarios/gen_hrp2_statically_balanced_positions_2d.py

+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.gepetto import Viewer
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+
+from hpp.corbaserver.rbprm.tools.cwc_trajectory import *
+
+from hpp import Error as hpperr
+
+
+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", [-0.135,2, -1, 1, 0, 2.2])
+
+
+ps = ProblemSolver( fullBody )
+r = Viewer (ps)
+
+#~ AFTER loading obstacles
+rLegId = '0rLeg'
+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.1)
+
+lLegId = '1lLeg'
+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.1)
+
+rarmId = '3Rarm'
+rarm = 'RARM_JOINT0'
+rHand = 'RARM_JOINT5'
+rArmOffset = [0,0,-0.1]
+rArmNormal = [0,0,1]
+rArmx = 0.024; rArmy = 0.024
+#disabling collision for hook
+fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 10000, "manipulability", 0.05, "_6_DOF", True)
+
+#~ limbsCOMConstraints = { rLegId : {'file': "hrp2/RL_com.ineq", 'effector' : 'RLEG_JOINT5'},  
+						#~ lLegId : {'file': "hrp2/LL_com.ineq", 'effector' : 'LLEG_JOINT5'}, 
+						#~ rarmId : {'file': "hrp2/RA_com.ineq", 'effector' : rHand} }
+
+#~ AFTER loading obstacles
+larmId = '4Larm'
+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, "manipulability", 0.05, "_6_DOF", True)
+
+ #~ 
+
+#~ fullBody.client.basic.robot.setJointConfig('LARM_JOINT0',[1])
+#~ fullBody.client.basic.robot.setJointConfig('RARM_JOINT0',[-1])
+
+def draw_cp(cid, limb, config):
+	global r
+	posetc = fullBody.getEffectorPosition(limb, config)
+	print "pos ", posetc[0]
+	scene = "qds"+limb+str(cid)
+	r.client.gui.createScene(scene)
+	for i in range(4):
+		pos = posetc[2*i]
+		r.client.gui.addBox(scene+"/b"+str(i),0.01,0.01,0.01, [1,0,0,1])
+		r.client.gui.applyConfiguration(scene+"/b"+str(i),pos+[1,0,0,0])
+		r.client.gui.refresh()	
+	r.client.gui.addSceneToWindow(scene,0)
+
+
+def fill_contact_points(limbs, config, config_pinocchio):
+	res = {}
+	res["q"] = config_pinocchio[:]
+	res["contact_points"] = {}
+	res["P"] = []
+	res["N"] = []
+	for limb in limbs:
+		posetc = fullBody.getEffectorPosition(limb, config)
+		res["contact_points"][limb] = {}
+		res["contact_points"][limb]["P"] = [p for i, p in enumerate (posetc) if (i%2 == 0)]
+		res["P"] += [p for i, p in enumerate (posetc) if (i%2 == 0)]
+		res["contact_points"][limb]["N"] = [n for i, n in enumerate (posetc) if (i%2 == 1)]
+		res["N"] += [n for i, n in enumerate (posetc) if (i%2 == 1)]
+	return res
+
+q_0 = fullBody.getCurrentConfig(); 
+#~ fullBody.getSampleConfig()
+qs = []; qs_gepetto = []; states = []
+limbs = [lLegId,rLegId] 
+for _ in range(1000):
+	q = fullBody.generateGroundContact(limbs)
+	q_gep = q[:]
+	quat_end = q[4:7]
+	q[6] = q[3]
+	q[3:6] = quat_end
+	qs.append(q)
+	qs_gepetto.append(q_gep)
+	states.append(fill_contact_points(limbs,q_gep,q))
+	
+from pickle import dump
+f1=open("configs_feet_on_ground_static_eq", 'w+')
+dump(states, f1)
+f1.close()

script/scenarios/gen_hyq_statically_balanced_positions_2d.py

+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.gepetto import Viewer
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+
+from hpp.corbaserver.rbprm.tools.cwc_trajectory import *
+
+from hpp import Error as hpperr
+
+
+packageName = "hyq_description"
+meshPackageName = "hyq_description"
+rootJointType = "freeflyer"
+
+#  Information to retrieve urdf and srdf files.
+urdfName = "hyq"
+urdfSuffix = ""
+srdfSuffix = ""
+
+fullBody = FullBody ()
+
+fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+fullBody.setJointBounds ("base_joint_xyz", [-0.135,2, -1, 1, 0, 2.2])
+
+
+#  This time we load the full body model of HyQ
+fullBody = FullBody () 
+fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+fullBody.setJointBounds ("base_joint_xyz", [-2,5, -1, 1, 0.3, 4])
+
+#  Setting a number of sample configurations used
+nbSamples = 20000
+
+ps = ProblemSolver( fullBody )
+r = Viewer (ps)
+
+rootName = 'base_joint_xyz'
+
+#  Creating limbs
+# cType is "_3_DOF": positional constraint, but no rotation (contacts are punctual)
+cType = "_3_DOF"
+# string identifying the limb
+rLegId = 'rfleg'
+# First joint of the limb, as in urdf file
+rLeg = 'rf_haa_joint'
+# Last joint of the limb, as in urdf file
+rfoot = 'rf_foot_joint'
+# Specifying the distance between last joint and contact surface
+offset = [0.,-0.021,0.]
+# Specifying the contact surface direction when the limb is in rest pose
+normal = [0,1,0]
+# Specifying the rectangular contact surface length
+legx = 0.02; legy = 0.02
+# remaining parameters are the chosen heuristic (here, manipulability), and the resolution of the octree (here, 10 cm).
+fullBody.addLimb(rLegId,rLeg,rfoot,offset,normal, legx, legy, nbSamples, "manipulability", 0.1, cType)
+
+lLegId = 'lhleg'
+lLeg = 'lh_haa_joint'
+lfoot = 'lh_foot_joint'
+fullBody.addLimb(lLegId,lLeg,lfoot,offset,normal, legx, legy, nbSamples, "manipulability", 0.05, cType)
+
+rarmId = 'rhleg'
+rarm = 'rh_haa_joint'
+rHand = 'rh_foot_joint'
+fullBody.addLimb(rarmId,rarm,rHand,offset,normal, legx, legy, nbSamples, "manipulability", 0.05, cType)
+
+larmId = 'lfleg'
+larm = 'lf_haa_joint'
+lHand = 'lf_foot_joint'
+fullBody.addLimb(larmId,larm,lHand,offset,normal, legx, legy, nbSamples, "forward", 0.05, cType)
+
+q_0 = fullBody.getCurrentConfig(); 
+
+
+r(q_0)
+def draw_cp(cid, limb, config):
+	global r
+	posetc = fullBody.getEffectorPosition(limb, config)
+	print "pos ", posetc[0]
+	scene = "qds"+limb+str(cid)
+	r.client.gui.createScene(scene)
+	for i in range(4):
+		pos = posetc[2*i]
+		r.client.gui.addBox(scene+"/b"+str(i),0.01,0.01,0.01, [1,0,0,1])
+		r.client.gui.applyConfiguration(scene+"/b"+str(i),pos+[1,0,0,0])
+		r.client.gui.refresh()	
+	r.client.gui.addSceneToWindow(scene,0)
+
+def fill_contact_points(limbs, config, config_pinocchio):
+	res = {}
+	res["q"] = config_pinocchio[:]
+	res["contact_points"] = {}
+	res["P"] = []
+	res["N"] = []
+	for limb in limbs:
+		posetc = fullBody.getEffectorPosition(limb, config)
+		res["contact_points"][limb] = {}
+		res["contact_points"][limb]["P"] = [p for i, p in enumerate (posetc) if (i%2 == 0)]
+		res["P"] += [p for i, p in enumerate (posetc) if (i%2 == 0)]
+		res["contact_points"][limb]["N"] = [n for i, n in enumerate (posetc) if (i%2 == 1)]
+		res["N"] += [n for i, n in enumerate (posetc) if (i%2 == 1)]
+	return res
+
+q_0 = fullBody.getCurrentConfig(); 
+#~ fullBody.getSampleConfig()
+qs = []; qs_gepetto = []; states = []
+limbs = [lLegId,rLegId] 
+for i in range(2):
+	q = fullBody.generateGroundContact(limbs)
+	q_gep = q[:]
+	quat_end = q[4:7]
+	q[6] = q[3]
+	q[3:6] = quat_end
+	qs.append(q)
+	qs_gepetto.append(q_gep)
+	states.append(fill_contact_points(limbs,q_gep,q))
+	
+from pickle import dump
+f1=open("configs_feet_on_ground_static_eq", 'w+')
+dump(qs, f1)
+f1.close()

script/scenarios/ground_crouch_hyq_interp.py

@@ -17,6 +17,7 @@ srdfSuffix = ""
 fullBody = FullBody ()
  
 fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+fullBody.setJointBounds ("base_joint_xyz", [-6,5, -4, 4, 0.6, 2])
 
 from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
 
@@ -76,16 +77,100 @@ fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
 
 r(q_init)
 
-configs = fullBody.interpolate(0.1)
+configs = fullBody.interpolate(0.1,1,5)
 
 r.loadObstacleModel ('hpp-rbprm-corba', "groundcrouch", "contact")
 #~ fullBody.exportAll(r, configs, 'obstacle_hyq_robust_10');
 i = 0;
 r (configs[i]); i=i+1; i-1
 
-#~ q0 = configs[2]
-#~ q0 = fullBody.generateContacts(q0, [0,0,1])
-#~ r(q0)
-c = fullBody.getContactSamplesIds("rfleg",q_init, [0,0,1])
-#~ r(fullBody.getSample("rfleg",int(c[i]))); i = i+1
+
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (fullBody.client.basic, r)
+
+
+from hpp.corbaserver.rbprm.tools.cwc_trajectory import *
+
+from hpp import Error as hpperr
+
+def displayComPath(pathId,color=[0.,0.75,0.15,0.9]) :
+	pathPos=[]
+	length = pp.end*pp.client.problem.pathLength (pathId)
+	t = pp.start*pp.client.problem.pathLength (pathId)
+	while t < length :
+		q = pp.client.problem.configAtParam (pathId, t)
+		pp.publisher.robot.setCurrentConfig(q)
+		q = pp.publisher.robot.getCenterOfMass()
+		pathPos = pathPos + [q[:3]]
+		t += pp.dt
+	nameCurve = "path_"+str(pathId)+"_com"
+	pp.publisher.client.gui.addCurve(nameCurve,pathPos,color)
+	pp.publisher.client.gui.addToGroup(nameCurve,pp.publisher.sceneName)
+	pp.publisher.client.gui.refresh()
+
+
+def displayIn(pp, pathId,length):
+	t = 0
+	while t < length :
+		start = time.time()
+		q = pp.client.problem.configAtParam (pathId, t)
+		pp.publisher.robotConfig = q
+		pp.publisher.publishRobots ()
+		t += (pp.dt * pp.speed)
+		elapsed = time.time() - start
+		if elapsed < pp.dt :
+		  time.sleep(pp.dt-elapsed)
+
+res = []
+import sys
+numerror = 0
+def act(i, optim):
+	try:
+		pid = solve_com_RRT(fullBody, configs, i, True, 0.5, 0.2, False, optim, False, True)
+		displayComPath(pid, [0.9,0.,0.15,0.9])
+		#~ pp(pid)
+		global res
+		res = res + [pid]
+		global numerror
+	except hpperr as e:
+		print "failed at id " + str(i) , e.strerror
+		numerror+=1
+	except ValueError as e:
+		print "failed at id " + str(i) , e
+		numerror+=1
+	except IndexError as e:
+		print "failed at id " + str(i) , e
+		numerror+=1
+	except Exception as e:
+		print e
+		numerror+=1
+	except:
+		print "smthg wrong"
+		return
+	
+	
+	
+
+def displayInSave(pp, pathId, configs):
+	length = pp.end*pp.client.problem.pathLength (pathId)
+	t = pp.start*pp.client.problem.pathLength (pathId)
+	while t < length :
+		q = pp.client.problem.configAtParam (pathId, t)
+		configs.append(q)
+		t += (pp.dt * pp.speed)
+
+pp.setSpeed(2)
+
+respath = []
+for p in res:
+	print p
+	displayInSave(pp,p, respath)
+	
+#~ for p in res:
+	#~ displayIn(pp,p,1.5)
+import time
+#~ fullBody.exportAll(r, respath, 'groun_crouch_hyq_full_bridge_05');
+#~ fullBody.exportAll(r, respath, 'groun_crouch_hyq_full_hole_05');
+
+
 

script/scenarios/ground_crouch_hyq_path.py

@@ -14,10 +14,10 @@ rbprmBuilder = Builder ()
 rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
 rbprmBuilder.setJointBounds ("base_joint_xyz", [-6,5, -4, 4, 0.6, 2])
 rbprmBuilder.setFilter(['hyq_rhleg_rom', 'hyq_lfleg_rom', 'hyq_rfleg_rom','hyq_lhleg_rom'])
-rbprmBuilder.setNormalFilter('hyq_lhleg_rom', [0,0,1], 0.9)
-rbprmBuilder.setNormalFilter('hyq_rfleg_rom', [0,0,1], 0.9)
-rbprmBuilder.setNormalFilter('hyq_lfleg_rom', [0,0,1], 0.9)
-rbprmBuilder.setNormalFilter('hyq_rhleg_rom', [0,0,1], 0.9)
+rbprmBuilder.setAffordanceFilter('hyq_rhleg_rom', ['Support'])
+rbprmBuilder.setAffordanceFilter('hyq_rfleg_rom', ['Support',])
+rbprmBuilder.setAffordanceFilter('hyq_lhleg_rom', ['Support'])
+rbprmBuilder.setAffordanceFilter('hyq_lfleg_rom', ['Support',])
 rbprmBuilder.boundSO3([-0.1,0.1,-1,1,-1,1])
 
 #~ from hpp.corbaserver.rbprm. import ProblemSolver
@@ -37,9 +37,16 @@ ps.addPathOptimizer("RandomShortcut")
 ps.setInitialConfig (q_init)
 ps.addGoalConfig (q_goal)
 
+from hpp.corbaserver.affordance.affordance import AffordanceTool
+afftool = AffordanceTool ()
+afftool.loadObstacleModel (packageName, "groundcrouch", "planning", r)
+afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
+
+# Choosing RBPRM shooter and path validation methods.
+# Note that the standard RRT algorithm is used.
 ps.client.problem.selectConFigurationShooter("RbprmShooter")
-ps.client.problem.selectPathValidation("RbprmPathValidation",0.01)
-r.loadObstacleModel (packageName, "groundcrouch", "planning")
+ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
+#~ r.loadObstacleModel (packageName, "groundcrouch", "planning")
 #~ ps.solve ()
 t = ps.solve ()
 
@@ -58,3 +65,7 @@ pp = PathPlayer (rbprmBuilder.client.basic, r)
 #~ pp (0)
 #~ pp (1)
 r (q_init)
+
+q_far = q_init [::]
+q_far [0:3] = [-2, -3, 0.63]; 
+r(q_far)

script/tools/admissibleComPositionsFromEffectorHyQ.py

+#Importing helper class for RBPRM
+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.gepetto import Viewer
+import quaternion as quat
+
+packageName = "hyq_description"
+meshPackageName = "hyq_description"
+rootJointType = "freeflyer"
+
+#  Information to retrieve urdf and srdf files.
+urdfName = "hyq"
+urdfSuffix = ""
+srdfSuffix = ""
+
+#  This time we load the full body model of HyQ
+fullBody = FullBody () 
+fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+fullBody.setJointBounds ("base_joint_xyz", [-20,20, -20, 20, -20, 20])
+
+#  Setting a number of sample configurations used
+nbSamples = 1
+
+rootName = 'base_joint_xyz'
+
+#  Creating limbs
+# cType is "_3_DOF": positional constraint, but no rotation (contacts are punctual)
+cType = "_3_DOF"
+# string identifying the limb
+rLegId = 'rfleg'
+# First joint of the limb, as in urdf file
+rLeg = 'rf_haa_joint'
+# Last joint of the limb, as in urdf file
+rfoot = 'rf_foot_joint'
+# Specifying the distance between last joint and contact surface
+offset = [0.,-0.021,0.]
+# Specifying the contact surface direction when the limb is in rest pose
+normal = [0,1,0]
+# Specifying the rectangular contact surface length
+legx = 0.02; legy = 0.02
+# remaining parameters are the chosen heuristic (here, manipulability), and the resolution of the octree (here, 10 cm).
+fullBody.addLimb(rLegId,rLeg,rfoot,offset,normal, legx, legy, nbSamples, "manipulability", 0.1, cType)
+
+lLegId = 'lhleg'
+lLeg = 'lh_haa_joint'
+lfoot = 'lh_foot_joint'
+fullBody.addLimb(lLegId,lLeg,lfoot,offset,normal, legx, legy, nbSamples, "manipulability", 0.05, cType)
+
+rarmId = 'rhleg'
+rarm = 'rh_haa_joint'
+rHand = 'rh_foot_joint'
+fullBody.addLimb(rarmId,rarm,rHand,offset,normal, legx, legy, nbSamples, "manipulability", 0.05, cType)
+
+larmId = 'lfleg'
+larm = 'lf_haa_joint'
+lHand = 'lf_foot_joint'
+fullBody.addLimb(larmId,larm,lHand,offset,normal, legx, legy, nbSamples, "forward", 0.05, cType)
+
+#make sure this is 0
+q_0 = fullBody.getCurrentConfig ()
+zeroConf = [0,0,0, 1, 0, 0, 0]
+q_0[0:7] = zeroConf
+fullBody.setCurrentConfig (q_0)
+
+effectors = [rfoot, lfoot, lHand, rHand]
+limbIds = [rLegId, lLegId, larmId, rarmId ]
+
+import numpy as np
+#~ effectorName = rfoot
+#~ limbId = rLegId
+#~ q = fullBody.getSample(limbId, 1)
+#~ fullBody.setCurrentConfig(q) #setConfiguration matching sample
+#~ qEffector = fullBody.getJointPosition(effectorName)
+#~ q0 = quat.Quaternion(qEffector[3:7])
+#~ rot = q0.toRotationMatrix() #compute rotation matrix world -> local
+#~ p = qEffector[0:3] #(0,0,0) coordinate expressed in effector fram
+#~ rm=np.zeros((4,4))
+#~ for i in range(0,3):
+	#~ for j in range(0,3):
+		#~ rm[i,j] = rot[i,j]
+#~ for i in range(0,3):
+	#~ rm[i,3] = qEffector[i]
+#~ rm[3,3] = 1
+#~ invrm = np.linalg.inv(rm)
+#~ p = invrm.dot([0,0,0,1])
+
+points = [[],[],[],[]]
+
+def printComPosition(nbConfigs):
+	num_invalid = 0
+	for i in range(0,nbConfigs):
+		q = fullBody.shootRandomConfig()
+		q[0:7] = zeroConf
+		fullBody.setCurrentConfig(q) #setConfiguration matching sample
+		com = fullBody.getCenterOfMass()
+		for x in range(0,3):
+			q[x] = -com[x]
+		fullBody.setCurrentConfig(q)
+		#~ print ("final com" + str(com))
+		#~ print ("final com" + str(fullBody.getCenterOfMass()))
+		if(fullBody.isConfigValid(q)[0]):
+			for j in range(0,len(effectors)):
+				effectorName = effectors[j]
+				limbId = limbIds[j]
+				qEffector = fullBody.getJointPosition(effectorName)
+				q0 = quat.Quaternion(qEffector[3:7])
+				rot = q0.toRotationMatrix() #compute rotation matrix world -> local
+				p = qEffector[0:3] #(0,0,0) coordinate expressed in effector fram
+				rm=np.zeros((4,4))
+				for k in range(0,3):
+					for l in range(0,3):
+						rm[k,l] = rot[k,l]
+				for m in range(0,3):
+					rm[m,3] = qEffector[m]
+				rm[3,3] = 1
+				invrm = np.linalg.inv(rm)
+				p = invrm.dot([0,0,0,1])
+				points[j].append(p)
+				#~ print (points[j])
+		else:
+			num_invalid +=1
+	for j in range(0,len(limbIds)):
+		f1=open('./'+str(limbIds[j])+'_com.erom', 'w+')
+		for p in points[j]:
+			f1.write(str(p[0]) + "," + str(p[1]) + "," + str(p[2]) + "\n")