[tool] add/update tools script

CMakeLists.txt

@@ -139,6 +139,8 @@ install(FILES
 	data/urdf/flat_step.urdf
 	data/urdf/flat_hole.urdf
 	data/urdf/cross_gap.urdf
+  data/urdf/floor_bauzil.urdf
+  data/urdf/floor_bauzil_obstacles.urdf
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/urdf
   )
 install(FILES
@@ -215,6 +217,8 @@ install(FILES
 	data/srdf/flat_step.srdf
 	data/srdf/flat_hole.srdf
 	data/srdf/cross_gap.srdf
+  data/srdf/floor_bauzil.srdf
+  data/srdf/floor_bauzil_obstacles.srdf
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/srdf
   )
 install(FILES
@@ -295,6 +299,8 @@ install(FILES
   data/meshes/flat_hole.stl
   data/meshes/cross_gap.stl
   data/meshes/cross_gap2.stl
+  data/meshes/floor_bauzil.stl
+  data/meshes/floor_bauzil_obstacles.stl
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/meshes
   )
 

idl/hpp/corbaserver/rbprm/rbprmbuilder.idl

@@ -771,7 +771,7 @@ module hpp
 
     boolean areKinematicsConstraintsVerifiedForState(in unsigned short stateFrom,in floatSeq point)raises (Error);
 
-    boolean isReachableFromState(in unsigned short stateFrom, in unsigned short stateTo)raises (Error);
+    floatSeq isReachableFromState(in unsigned short stateFrom, in unsigned short stateTo)raises (Error);
 
     floatSeq isDynamicallyReachableFromState(in unsigned short stateFrom, in unsigned short stateTo,in boolean addPathPerPhase, in floatSeq timings,in short numPointsPerPhase)raises (Error);
 

script/dynamic/talos/flatGround_pyrene_interp.py

@@ -7,7 +7,7 @@ from hpp.corbaserver.rbprm.rbprmstate import State,StateHelper
 #from display_tools import *
 import talos.flatGround_pyrene_pathKino as tp
 import time
-from planning.robot_config.talos import *
+from robot_config.talos import *
 
 tPlanning = tp.tPlanning
 
@@ -156,7 +156,7 @@ print "number of configs :", len(configsFull)
 
 
 
-from planning.configs.talos_flatGround import *
+from configs.talos_flatGround import *
 from generate_contact_sequence import *
 
 beginState = 0

script/dynamic/talos/flatGround_pyrene_pathKino.py

@@ -4,7 +4,7 @@ from hpp.corbaserver import Client
 from hpp.corbaserver.robot import Robot as Parent
 from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
 import omniORB.any
-from planning.configs.straight_walk_config import *
+from configs.talos_flatGround import *
 import time
 
 class Robot (Parent):

script/tools/bench_contactGeneration.py

+from stair_bauzil_hrp2_interp import *
+import generate_muscod_problem as mp
+from pathlib2 import Path
+import muscodSSH as ssh
+tryWarmStart = True
+
+print "run bench with feasibility criterion"
+
+com = fullBody.getCenterOfMass()
+if com[0] > 1.25 :
+    success = True
+else :
+    success = False
+
+numConf = len(configs)
+if success :
+    # muscod without warm start :
+    if Path(CONTACT_SEQUENCE_WHOLEBODY_FILE).is_file():
+        os.remove(CONTACT_SEQUENCE_WHOLEBODY_FILE)
+    filename_xml = OUTPUT_DIR + "/" + OUTPUT_SEQUENCE_FILE
+    mp.generate_muscod_problem(filename_xml,True)
+    successMuscod,ssh_ok = ssh.call_muscod()
+    time.sleep(5.) # wait for sync of the ~/home (worst case, usually 0.1 is enough ... )
+    muscodConverged = successMuscod and Path(CONTACT_SEQUENCE_WHOLEBODY_FILE).is_file()
+  
+    if tryWarmStart :
+        if not success : 
+            # generate warm start from planning : 
+            if Path(CONTACT_SEQUENCE_WHOLEBODY_FILE).is_file():
+                os.remove(CONTACT_SEQUENCE_WHOLEBODY_FILE)
+            filename_xml = OUTPUT_DIR + "/" + OUTPUT_SEQUENCE_FILE 
+            cs = generateContactSequenceWithInitGuess(ps,fullBody,configs,beginState,endState)
+            cs.saveAsXML(filename, "ContactSequence")
+            mp.generate_muscod_problem(filename_xml,True)
+            successMuscod,ssh_ok = ssh.call_muscod()
+            time.sleep(5.) # wait for sync of the ~/home (worst case, usually 0.1 is enough ... )
+            muscodWarmStartConverged = successMuscod and Path(CONTACT_SEQUENCE_WHOLEBODY_FILE).is_file()
+        else : 
+            muscodWarmStartConverged= True
+    else :
+        muscodWarmStartConverged = False
+else : 
+    muscodConverged = False
+    muscodWarmStartConverged = False
+    tInterpolateConfigs = 0.
+    numConf = 0.
+
+name = "/local/fernbac/bench_iros18/bench_contactGeneration/stairsMC_croc.log"
+f = open(name,"a")
+f.write("new\n")
+f.write("success "+str(success)+"\n")
+f.write("muscodNoWarmStart "+str(muscodConverged)+"\n")
+f.write("muscodWarmStart "+str(muscodWarmStartConverged)+"\n")
+f.write("time "+str(tInterpolateConfigs)+"\n")
+f.write("configs "+str(numConf)+"\n")
+f.close()
+

script/tools/bench_contactGeneration_noCroc.py

+from stair_bauzil_hrp2_interp import *
+import generate_muscod_problem as mp
+from pathlib2 import Path
+import muscodSSH as ssh
+tryWarmStart = False
+
+print "run bench without feasibility criterion"
+
+com = fullBody.getCenterOfMass()
+if com[0] > 1.3:
+    success = True
+else :
+    success = False
+
+#success = False
+numConf = len(configs)
+
+if success :
+    # muscod without warm start :
+    if Path(CONTACT_SEQUENCE_WHOLEBODY_FILE).is_file():
+        os.remove(CONTACT_SEQUENCE_WHOLEBODY_FILE)
+    filename_xml = OUTPUT_DIR + "/" + OUTPUT_SEQUENCE_FILE
+    mp.generate_muscod_problem(filename_xml,True)
+    successMuscod,ssh_ok = ssh.call_muscod()
+    time.sleep(5.) # wait for sync of the ~/home (worst case, usually 0.1 is enough ... )
+    muscodConverged = successMuscod and Path(CONTACT_SEQUENCE_WHOLEBODY_FILE).is_file()
+    
+    crocConverged = True
+    for id_state in range(beginState,endState):
+        pid = fullBody.isDynamicallyReachableFromState(id_state,id_state+1,numPointsPerPhases=0)
+        if len(pid) == 0:
+            print "Croc did not converge for state "+str(id_state)
+            crocConverged = False
+        
+else : 
+    crocConverged = False    
+    muscodConverged = False
+    tInterpolateConfigs = 0.
+    numConf = 0.
+    
+
+name = "/local/fernbac/bench_iros18/bench_contactGeneration/stairsMC_noCroc.log"
+f = open(name,"a")
+f.write("new\n")
+f.write("success "+str(success)+"\n")
+f.write("muscodNoWarmStart "+str(muscodConverged)+"\n")
+f.write("crocConverged "+str(crocConverged)+"\n")
+f.write("time "+str(tInterpolateConfigs)+"\n")
+f.write("configs "+str(numConf)+"\n")
+f.close()
+

script/tools/check_qp.py

 from hpp.corbaserver.rbprm.rbprmstate import State,StateHelper
 from disp_bezier import * 
-max_acc = 1
-pointsPerPhase=4
+import numpy as np
+max_acc = 5.
+pointsPerPhase=5
 ROUND = 1000.
 
 global curves_initGuess
@@ -10,6 +11,14 @@ global timings_initGuess
 curves_initGuess = []
 timings_initGuess = []
 
+    
+def stdVecToMatrix(std_vector):
+    vec_l = []
+    for vec in std_vector:
+        vec_l.append(vec)
+
+    res = np.hstack(tuple(vec_l))
+    return res
 
 
 def compute_time_array(t_qp,t_c):
@@ -45,57 +54,70 @@ def check_acceleration_bounds(ddc_qp,t_discretized):
     print "acceleration between : "+str(min)+" ; "+str(max)
     return True;
 
-def check_projection(s,c,dc,ddc,t):
+def check_projection(s,c,t):
+    return True
     success = s.projectToCOM(c(t).transpose().tolist()[0],500)
     if not success : 
         print "Unable to project to com at time : "+str(t)
         return False  
-    # check dynamic stability
+    else:
+        return True
+
+def check_stability(s,c,dc,ddc,t):
     q = s.q()
     q[-6:-3] =  dc(t).transpose().tolist()[0]    
     q[-3:]   = ddc(t).transpose().tolist()[0]
-    success = s.fullBody.isConfigBalanced(q,s.getLimbsInContact())
+    success = s.fullBody.isConfigBalanced(q,s.getLimbsInContact(),CoM = c(t).transpose().tolist()[0])
     if not success : 
         print "UNSTABLE  at time : "+str(t)
-        return False      
-    return True
+        #print "q = ",q
+        return False          
+    else : 
+        return True
 
-def check_projection_path(s0,s1,c,dc,ddc,t_qp,t_per_phase):
+def check_projection_path(s0,s1,c,dc,ddc,t_per_phase):
+    kin_valid = True
+    stab_valid = True
     moving_limb = s0.contactsVariations(s1)
+    s0_orig = State(s0.fullBody,q=s0.q(),limbsIncontact=s0.getLimbsInContact())
+    s1_orig = State(s1.fullBody,q=s1.q(),limbsIncontact=s1.getLimbsInContact())
+    
     if len(moving_limb)>1:
         print "Too many contact variation between adjacent states"
-        return False
+        return False,False
     if len(moving_limb)==0:
         print "No contact between adjacent states"
-        return True
-    print "test for phase 0 :"
+        return True,True
+    #print "test for phase 0 :"
     for t in t_per_phase[0]:
-        if not check_projection(s0,c,dc,ddc,t):
-            return False
+        if kin_valid and not check_projection(s0,c,t):
+            kin_valid = False
+        if stab_valid and not check_stability(s0_orig,c,dc,ddc,t):
+            stab_valid = False
     smid,success = StateHelper.removeContact(s0,moving_limb[0])
-    if not success:
+    smid_orig,success_orig = StateHelper.removeContact(s0_orig,moving_limb[0])
+    
+    if not (success and success_orig):
         print "Error in creation of intermediate state"
-        return False    
-    print "test for phase 1 :"
+        return False,False   
+    #print "test for phase 1 :"
     for t in t_per_phase[1]:
-        #if t == t_per_phase[0][-1]:
-         #   t -= EPS  
-        if not check_projection(smid,c,dc,ddc,t):
-            return False
-    print "test for phase 2"
+        if kin_valid and not check_projection(smid,c,t):
+            kin_valid =  False
+        if stab_valid and not check_stability(smid_orig,c,dc,ddc,t):
+            stab_valid = False            
+    #print "test for phase 2"
     # go in reverse order for p2, it's easier for the projection : 
     for i in range(1,len(t_per_phase[2])+1):
-        t = t_per_phase[1][-i]
-        if not check_projection(s1,c,dc,ddc,t):
-            return False
- 
-    t = t_per_phase[1][-1]
-    if not check_projection(s1,c,dc,ddc,t):
-        return False
-  
-    return True
-    
-        
+        t = t_per_phase[2][-i]
+        if kin_valid and not check_projection(s1,c,t):
+            kin_valid = False
+        if stab_valid and not check_stability(s1_orig,c,dc,ddc,t):
+            stab_valid = False             
+
+    return kin_valid,stab_valid
+
+
 
 def check_one_transition(ps,fullBody,s0,s1,r=None,pp=None):
     global curves_initGuess
@@ -116,14 +138,96 @@ def check_one_transition(ps,fullBody,s0,s1,r=None,pp=None):
     print "### test acceleration bounds ###"
     valid = valid and check_acceleration_bounds(ddc_qp,t_discretized)
     print "### test kinematic projection ###"
-    valid = valid and check_projection_path(s0,s1,c_qp,dc_qp,ddc_qp,t_qp,t_per_phase)
+    valid = valid and check_projection_path(s0,s1,c_qp,dc_qp,ddc_qp,t_per_phase)
     
     curves_initGuess.append(c_qp)
     timings_initGuess.append(t_qp)
     return valid
-  
+ 
+
+ 
+def genTimeArray(Ts,dt):
+    t = 0.
+    res = []
+    current_t = 0.
+    for phase_id in range(len(Ts)):
+        t_phase = []
+        while t <= (current_t + Ts[phase_id]):
+            t_phase += [t]
+            t += dt
+        res += [t_phase]
+        current_t += Ts[phase_id]
+    return res
+    
+def check_traj_valid(ps,fullBody,s0_,s1_,pIds,dt=0.001):
+    # create copy of original states : 
+    s0 = State(fullBody,q=s0_.q(),limbsIncontact=s0_.getLimbsInContact())
+    s1 = State(fullBody,q=s1_.q(),limbsIncontact=s1_.getLimbsInContact())
+    fullBody.setStaticStability(False)
+    
+    Ts = [ps.pathLength(int(pIds[1])), ps.pathLength(int(pIds[2])), ps.pathLength(int(pIds[3]))]  
+    t_array = genTimeArray(Ts,dt)
+    #print "Time array : ",t_array
+    
+    c_qp = fullBody.getPathAsBezier(int(pIds[0]))
+    dc_qp = c_qp.compute_derivate(1)
+    ddc_qp = dc_qp.compute_derivate(1)
+    return check_projection_path(s0,s1,c_qp,dc_qp,ddc_qp,t_array)    
+    
+    
+def check_muscod_traj(fullBody,cs,s0_,s1_):
+    if cs.size() != 3 :
+        print "Contact sequence must be of size 3 (one step)"
+        return False,False
+    kin_valid = True
+    stab_valid = True
+    stab_valid_discretized = True
+    # create copy of original states : 
+    s0 = State(fullBody,q=s0_.q(),limbsIncontact=s0_.getLimbsInContact())
+    s1 = State(fullBody,q=s1_.q(),limbsIncontact=s1_.getLimbsInContact()) 
+    moving_limb = s0.contactsVariations(s1)
+    smid,success = StateHelper.removeContact(s0,moving_limb[0])
+    if not success:
+        print "Error in creation of intermediate state"
+        return False,False   
+    phases = []
+    c_array = []
+    dc_array = []
+    ddc_array = []
+    t_array = [] 
+    states = [s0,smid,s1]
+    states_copy = [State(fullBody,q=s0_.q(),limbsIncontact=s0_.getLimbsInContact()), State(fullBody,q=smid.q(),limbsIncontact=smid.getLimbsInContact()), State(fullBody,q=s1_.q(),limbsIncontact=s1_.getLimbsInContact()) ]
+    for k in range(3) : 
+        phases += [cs.contact_phases[k]]
+        state_traj = stdVecToMatrix(phases[k].state_trajectory).transpose()
+        control_traj = stdVecToMatrix(phases[k].control_trajectory).transpose() 
+        c_array = state_traj[:,:3]
+        dc_array = state_traj[:,3:6]
+        ddc_array = control_traj[:,:3]
+        t_array = stdVecToMatrix(phases[k].time_trajectory).transpose()    
+    
+        for i in range(len(c_array)):
+            #if kin_valid and not states[k].projectToCOM(c_array[i].tolist()[0],500) :
+            #    kin_valid = False
+            q = states_copy[k].q()
+            q[-6:-3] = dc_array[i].tolist()[0]    
+            q[-3:]   = ddc_array[i].tolist()[0]
+            if stab_valid_discretized and not states_copy[k].fullBody.isConfigBalanced(q,states_copy[k].getLimbsInContact(),CoM = c_array[i].tolist()[0]) :
+                stab_valid_discretized = False  
+        for i in range(int(len(c_array)/100)):
+            q = states_copy[k].q()
+            q[-6:-3] = dc_array[i*100].tolist()[0]    
+            q[-3:]   = ddc_array[i*100].tolist()[0]
+            if stab_valid and not states_copy[k].fullBody.isConfigBalanced(q,states_copy[k].getLimbsInContact(),CoM = c_array[i*100].tolist()[0]) :
+                stab_valid = False              
         
+    
+    return kin_valid,stab_valid, stab_valid_discretized
    
+    
+    
+    
+    
  
 def check_contact_plan(ps,r,pp,fullBody,idBegin,idEnd):
     fullBody.client.basic.robot.setExtraConfigSpaceBounds([-0,0,-0,0,-0,0,0,0,0,0,0,0])
@@ -166,4 +270,12 @@ valid = True
 t_qp = [ps.pathLength(int(pid[1])), ps.pathLength(int(pid[2])), ps.pathLength(int(pid[3]))]  
 t_discretized,t_per_phase = compute_time_array(t_qp)
 
-"""
\ No newline at end of file
+"""
+
+"""
+
+cs = ContactSequenceHumanoid(0)
+cs.loadFromXML('/home/pfernbac/Documents/muscod/uc-dual/PROBLEM/random/res/contact_sequence_trajectory.xml',CONTACT_SEQUENCE_XML_TAG)
+
+
+"""

script/tools/cut_contact_sequence.py

@@ -5,11 +5,11 @@ from pinocchio.utils import *
 
 import locomote
 from locomote import WrenchCone,SOC6,ControlType,IntegratorType,ContactPatch, ContactPhaseHumanoid, ContactSequenceHumanoid
-import planning.generate_muscod_problem as mp
+import generate_muscod_problem as mp
 import muscodSSH as ssh
-from planning.config import *
+from config import *
 
-statesPerStep=4 # number of double support configs from the planning per call to muscod
+statesPerStep=5 # number of double support configs from the planning per call to muscod
 stepSize=statesPerStep*2 - 1 # contact_sequence contain double support AND simple support states
 
 def solveMuscodProblem(configsFull,cs):

script/tools/displayContactFeet_justin.py

+
+name = 's2'
+gui.addSphere(name,0.01,[1,0,0,1])
+gui.setVisibility(name,"ALWAYS_ON_TOP")
+gui.addToGroup(name,"world")
+gui.applyConfiguration(name,p)
+gui.refresh()
+
+
+gui=r.client.gui
+name = 's2'
+gui.addSphere(name,0.01,[1,0,0,1])
+gui.setVisibility(name,"ALWAYS_ON_TOP")
+gui.addToGroup(name,r.sceneName)
+gui.applyConfiguration(name,p)
+gui.refresh()

script/tools/gen_hrp2_statically_balanced_positions_2d_state.py

@@ -4,7 +4,6 @@ from hpp.gepetto import Viewer
 from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
 import numpy as np
 #from hpp.corbaserver.rbprm.tools.cwc_trajectory import *
-
 from hpp import Error as hpperr
 from numpy import array, matrix
 from hpp.corbaserver.rbprm.rbprmstate import State,StateHelper
@@ -21,7 +20,7 @@ srdfSuffix = ""
 fullBody = FullBody ()
 
 fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
-fullBody.setJointBounds ("base_joint_xyz", [-0.3,0.3, -0.3, 0.3, 0.6, 0.7])
+fullBody.setJointBounds ("base_joint_xyz", [-0.7,0.7, -0.7, 0.7, 0.6, 0.7])
 fullBody.setJointBounds ("CHEST_JOINT0", [0.,0.])
 fullBody.setJointBounds ("CHEST_JOINT1", [0.,0.])
 fullBody.setJointBounds ("HEAD_JOINT0", [0.,0.])
@@ -61,7 +60,8 @@ rLegLimbOffset=[0,0,-0.035]#0.035
 rLegNormal = [0,0,1]
 rLegx = 0.09; rLegy = 0.05
 #fullBody.addLimbDatabase("./db/hrp2_rleg_db.db",rLegId,"forward")
-fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 100000, "manipulability", 0.01,"_6_DOF",limbOffset=rLegLimbOffset)
+fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 100000, "manipulability", 0.01,"_6_DOF",kinematicConstraintsPath = "package://hpp-rbprm-corba/com_inequalities/fullSize/RLEG_JOINT0_com_constraints.obj",limbOffset=rLegLimbOffset,kinematicConstraintsMin=0.2)
+#fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 100000, "manipulability", 0.01,"_6_DOF",kinematicConstraintsPath = "package://hpp-rbprm-corba/com_inequalities/empty_com_constraints.obj",limbOffset=rLegLimbOffset,kinematicConstraintsMin=0.2)
 fullBody.runLimbSampleAnalysis(rLegId, "ReferenceConfiguration", True)
 #fullBody.saveLimbDatabase(rLegId, "./db/hrp2_rleg_db.db")
 
@@ -71,11 +71,14 @@ lLegLimbOffset=[0,0,0.035]
 lLegNormal = [0,0,1]
 lLegx = 0.09; lLegy = 0.05
 #fullBody.addLimbDatabase("./db/hrp2_lleg_db.db",lLegId,"forward")
-fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 100000, "manipulability", 0.01,"_6_DOF",limbOffset=lLegLimbOffset)
+fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 100000, "manipulability", 0.01,"_6_DOF",kinematicConstraintsPath = "package://hpp-rbprm-corba/com_inequalities/fullSize/LLEG_JOINT0_com_constraints.obj",limbOffset=lLegLimbOffset,kinematicConstraintsMin=0.2)
 fullBody.runLimbSampleAnalysis(lLegId, "ReferenceConfiguration", True)
 #fullBody.saveLimbDatabase(lLegId, "./db/hrp2_lleg_db.db")
 
-
+rleg = 'RLEG_JOINT0'
+rfoot = "RLEG_JOINT5"
+lleg = 'LLEG_JOINT0'
+lfoot = "LLEG_JOINT5"
 
 limbsCOMConstraints = { rLegId : {'file': "hrp2/RL_com.ineq", 'effector' : 'RLEG_JOINT5'},  
 						lLegId : {'file': "hrp2/LL_com.ineq", 'effector' : 'LLEG_JOINT5'}, 
@@ -223,21 +226,35 @@ def _feet_far_enough(fullBody,q):
 	for i in range(3):
 		rd += (c[i]-rf[i])*(c[i]-rf[i])
 		ld += (c[i]-lf[i])*(c[i]-lf[i])
-	if rd < 0.2 or ld < 0.2:
+	if rd < 0.35 or ld < 0.35:
 		return False
 	else:
 		return True
 	
+def projectMidFeet(fullBody,q,limbs):
+	fullBody.setCurrentConfig(q)
+	s = State(fullBody,q=q,limbsIncontact=limbs)
+	com = np.array(fullBody.getJointPosition(rfoot)[0:3])
+	com += np.array(fullBody.getJointPosition(lfoot)[0:3])
+	com /= 2.			
+	com[2] = fullBody.getCenterOfMass()[2]
+	successProj = s.projectToCOM(com.tolist())
+	if successProj and fullBody.isConfigValid(s.q())[0]  and fullBody.isConfigBalanced(s.q(), limbs, 5) :
+    		return s
+  	else :
+    		return None
 
-def _genbalance(limbs):
+def _genbalance(fullBody,limbs):
 	for i in range(10000):
 		q = fullBody.client.basic.robot.shootRandomConfig()
 		q[3:7] = [1,0,0,0]
 		#q = _boundSO3(q, len(limbs))
-		if fullBody.isConfigValid(q)[0] and fullBody.isConfigBalanced(q, limbs, 5) and __loosely_z_aligned(limbs[0], q) and __loosely_z_aligned(limbs[1], q) and _feet_far_enough(fullBody,q):
-		#~ if fullBody.isConfigValid(q)[0] and  __loosely_z_aligned(limbs[0], q) and __loosely_z_aligned(limbs[1], q):
-			return q
+		fullBody.setCurrentConfig(q)
+		#r(q)
+		if __loosely_z_aligned(limbs[0], q) and __loosely_z_aligned(limbs[1], q) and _feet_far_enough(fullBody,q) and fullBody.isConfigValid(q)[0]  and fullBody.isConfigBalanced(q, limbs, 5):
+		  return q
 	print "can't generate equilibrium config"
+	return []
 
 all_qs = []
 def genStates(fullbody,limbs, num_samples = 1000, coplanar = True):
@@ -249,10 +266,11 @@ def genStates(fullbody,limbs, num_samples = 1000, coplanar = True):
 		if(coplanar):
 			q = fullBody.generateGroundContact(limbs)
 		else:
-			q = _genbalance(limbs)
-		qs.append(q)
-		s = State(fullbody,q=q,limbsIncontact=limbs)
-		states.append(s)
+			q = _genbalance(fullBody,limbs)
+		if len(q)>1 :
+			qs.append(q)
+			s = State(fullbody,q=q,limbsIncontact=limbs)
+			states.append(s)
 	return states
 
 def genStateWithOneStep(fullbody,limbs, num_samples = 100, coplanar = True):
@@ -262,35 +280,50 @@ def genStateWithOneStep(fullbody,limbs, num_samples = 100, coplanar = True):
 	states = []
 	assert(len(limbs) == 2 and "only implemented for 2 limbs in contact for now")
 	it = 0
-	while len(states) < num_samples and it < 5000:
+	while len(states) < num_samples and it < 10000:
 		i = len(states)
 		if(coplanar):
 			q0 = fullBody.generateGroundContact(limbs)
 		else:
-			q0 = _genbalance(limbs)
-		s0 = State(fullbody,q=q0,limbsIncontact=limbs)
-		success = False
-		iter = 0
-		# try to make a step
-		while not success and iter < 100 :
-			if(coplanar):
-				q2 = fullBody.generateGroundContact(limbs)
-			else:
-				q2 = _genbalance(limbs)
-			s2 = State(fullbody,q=q2,limbsIncontact=limbs)
-			moving_limb = limbs[i%2]
-			[p,n]= s2.getCenterOfContactForLimb(moving_limb) # get position of the moving limb in the next state
-			s1,success = StateHelper.addNewContact(s0,moving_limb,p,n) # try to move the limb for s0 to it's position in s2
-			if success:
-				success = fullBody.isConfigValid(s1.q())[0] and fullBody.isConfigBalanced(s1.q(), limbs, 5) and __loosely_z_aligned(limbs[0], s1.q()) and __loosely_z_aligned(limbs[1], s1.q())
-			iter += 1
-		if success:
-			# recreate the states to assure the continuity of the index in fullBody : 
-			state0 = State(fullBody,q=s0.q(),limbsIncontact=limbs)
-			states += [[s1,state0]]
-			print "Step "+str(i)+" done."
-		else :
-			print "cannot make the step."
+			q0 = _genbalance(fullBody,limbs)
+		if len(q0)> 1 :