diff --git a/script/scenarios/demos/darpa_hyq.py b/script/scenarios/demos/darpa_hyq.py
index 2b4b3122bac97208a99b8ab7b2d39b1ba2d9bc3b..1f103a9eaa194ae84f7906d7cdd58a4498f01ce2 100644
--- a/script/scenarios/demos/darpa_hyq.py
+++ b/script/scenarios/demos/darpa_hyq.py
@@ -1,9 +1,7 @@
 #Importing helper class for RBPRM
-from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.corbaserver.rbprm.hyq import Robot
 from hpp.corbaserver.problem_solver import ProblemSolver
 from hpp.gepetto import Viewer
-#reference pose for hyq
-from hyq_ref_pose import hyq_ref
 
 #calling script darpa_hyq_path to compute root path
 import darpa_hyq_path as tp
@@ -16,18 +14,9 @@ db_dir = ins_dir+"/install/share/hyq-rbprm/database/hyq_"
 from hpp.corbaserver import Client
 
 
-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 = Robot () 
 fullBody.setJointBounds ("root_joint", [-2,5, -1, 1, 0.3, 4])
 
 #  Setting a number of sample configurations used
@@ -37,40 +26,21 @@ ps = tp.ProblemSolver(fullBody)
 r = tp.Viewer (ps, viewerClient=tp.r.client)
 
 rootName = 'base_joint_xyz'
-
 cType = "_3_DOF"
-rLegId = 'rfleg'
-rLeg = 'rf_haa_joint'
-rfoot = 'rf_foot_joint'
-offset = [0.,-0.021,0.]
-normal = [0,1,0]
-legx = 0.02; legy = 0.02
 
 def addLimbDb(limbId, heuristicName, loadValues = True, disableEffectorCollision = False):
 	fullBody.addLimbDatabase(str(db_dir+limbId+'.db'), limbId, heuristicName,loadValues, disableEffectorCollision)
 
-fullBody.addLimb(rLegId,rLeg,rfoot,offset,normal, legx, legy, nbSamples, "random", 0.1, cType)
-
-lLegId = 'lhleg'
-lLeg = 'lh_haa_joint'
-lfoot = 'lh_foot_joint'
-fullBody.addLimb(lLegId,lLeg,lfoot,offset,normal, legx, legy, nbSamples, "random", 0.05, cType)
-#~ 
-rarmId = 'rhleg'
-rarm = 'rh_haa_joint'
-rHand = 'rh_foot_joint'
-fullBody.addLimb(rarmId,rarm,rHand,offset,normal, legx, legy, nbSamples, "random", 0.05, cType)
-
-larmId = 'lfleg'
-larm = 'lf_haa_joint'
-lHand = 'lf_foot_joint'
-fullBody.addLimb(larmId,larm,lHand,offset,normal, legx, legy, nbSamples, "random", 0.05, cType)
+fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "random", 0.1, cType)
+fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "random", 0.05, cType)
+fullBody.addLimb(fullBody.rArmId,fullBody.rarm,fullBody.rHand,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "random", 0.05, cType)
+fullBody.addLimb(fullBody.lArmId,fullBody.larm,fullBody.lHand,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "random", 0.05, cType)
 
 #~ fullBody.runLimbSampleAnalysis(rLegId, "jointLimitsDistance", True)
 #~ fullBody.runLimbSampleAnalysis(lLegId, "jointLimitsDistance", True)
 #~ fullBody.runLimbSampleAnalysis(rarmId, "jointLimitsDistance", True)
 #~ fullBody.runLimbSampleAnalysis(larmId, "jointLimitsDistance", True)
-
+"""
 q_init = [-2.0,
  0.0,
  0.6838277139631803,
@@ -90,14 +60,15 @@ q_init = [-2.0,
  0.03995660287873871,
  -0.9577096766517215,
  0.9384602821326071]
- 
-q_goal = hyq_ref[:]; q_goal[0:7] = tp.q_goal[0:7]; q_goal[2]=hyq_ref[2]+0.02
+"""
+q_init=fullBody.referenceConfig[::]; q_init[0:7] = tp.q_init[0:7]; q_init[2]=fullBody.referenceConfig[2]+0.02
+q_goal = fullBody.referenceConfig[::]; q_goal[0:7] = tp.q_goal[0:7]; q_goal[2]=fullBody.referenceConfig[2]+0.02
 
 
 
 # specifying the full body configurations as start and goal state of the problem
-fullBody.setStartState(q_init,[rLegId,lLegId,rarmId,larmId])
-fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
+fullBody.setStartState(q_init,[fullBody.rLegId,fullBody.lLegId,fullBody.rArmId,fullBody.lArmId])
+fullBody.setEndState(q_goal,[fullBody.rLegId,fullBody.lLegId,fullBody.rArmId,fullBody.lArmId])
 
 
 r(q_init)
diff --git a/script/scenarios/demos/darpa_hyq_path.py b/script/scenarios/demos/darpa_hyq_path.py
index fe641ded72ef81623bba7625cc4ac3f2b2585439..984461592a7ff6f6b078e56e85e6b1e2c94b9fab 100644
--- a/script/scenarios/demos/darpa_hyq_path.py
+++ b/script/scenarios/demos/darpa_hyq_path.py
@@ -1,22 +1,12 @@
 # Importing helper class for setting up a reachability planning problem
-from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.hyq_abstract import Robot
 
 # Importing Gepetto viewer helper class
 from hpp.gepetto import Viewer
 
-rootJointType = 'freeflyer'
-packageName = 'hpp-rbprm-corba'
-meshPackageName = 'hpp-rbprm-corba'
-# URDF file describing the trunk of the robot HyQ
-urdfName = 'hyq_trunk_large'
-# URDF files describing the reachable workspace of each limb of HyQ
-urdfNameRom = ['hyq_lhleg_rom','hyq_lfleg_rom','hyq_rfleg_rom','hyq_rhleg_rom']
-urdfSuffix = ""
-srdfSuffix = ""
 
 # Creating an instance of the helper class, and loading the robot
-rbprmBuilder = Builder ()
-rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+rbprmBuilder = Robot ()
 rbprmBuilder.setJointBounds ("root_joint", [-2,5, -1, 1, 0.3, 4])
 # The following lines set constraint on the valid configurations:
 # a configuration is valid only if all limbs can create a contact ...
@@ -48,7 +38,7 @@ ps.addGoalConfig (q_goal)
 from hpp.corbaserver.affordance.affordance import AffordanceTool
 afftool = AffordanceTool ()
 #~ afftool.loadObstacleModel (packageName, "darpa", "planning", r, reduceSizes=[0.05,0.,0.])
-afftool.loadObstacleModel (packageName, "darpa", "planning", r)
+afftool.loadObstacleModel ("hpp-rbprm-corba", "darpa", "planning", r)
 afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
 
 # Choosing RBPRM shooter and path validation methods.
@@ -79,22 +69,6 @@ for i in range(1,10):
 	
 
 from hpp.corbaserver import Client
-from hpp.corbaserver.robot import Robot as Parent
-
-class Robot (Parent):
-	rootJointType = 'freeflyer'
-	packageName = 'hpp-rbprm-corba'
-	meshPackageName = 'hpp-rbprm-corba'
-	# URDF file describing the trunk of the robot HyQ
-	urdfName = 'hyq_trunk_large'
-	urdfSuffix = ""
-	srdfSuffix = ""
-	def __init__ (self, robotName, load = True):
-		Parent.__init__ (self, robotName, self.rootJointType, load)
-		self.tf_root = "base_footprint"
-		self.client = Client ()
-		self.load = load
-		
  #DEMO code to play root path and final contact plan
 cl = Client()
 cl.problem.selectProblem("rbprm_path")
diff --git a/script/scenarios/demos/hyq_ref_pose.py b/script/scenarios/demos/hyq_ref_pose.py
deleted file mode 100644
index ad4a9380c3e853500dcacb0e104195e550458701..0000000000000000000000000000000000000000
--- a/script/scenarios/demos/hyq_ref_pose.py
+++ /dev/null
@@ -1,19 +0,0 @@
-hyq_ref = [-1.652528468457493,
- 0.06758953014152885,
- 0.6638277139631803,
- 0.9995752585582991,
- -0.016391515572502728,
- -0.011295242081121508,
- 0.02128469407050025,
- 0.17905666752078864,
- 0.9253512562075908,
- -0.8776870832724601,
- 0.11147422537786231,
- -0.15843632504615043,
- 1.150049183494211,
- -0.1704998924604114,
- 0.6859376445755911,
- -1.1831277202117043,
- 0.06262698472369518,
- -0.42708925470675,
- 1.2855999319965081]
diff --git a/script/scenarios/demos/talos_flatGround.py b/script/scenarios/demos/talos_flatGround.py
index 7b553ab98700d0730ca9f46469d66af2cf12352c..e8d68ba4d00ddded20637a35e190e35dcd4955c2 100644
--- a/script/scenarios/demos/talos_flatGround.py
+++ b/script/scenarios/demos/talos_flatGround.py
@@ -1,4 +1,4 @@
-from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.corbaserver.rbprm.talos import Robot
 from hpp.gepetto import Viewer
 from display_tools import *
 import time
@@ -8,37 +8,10 @@ import talos_flatGround_path as tp
 print "Done."
 import time
 
-##
-#  Information to retrieve urdf and srdf files.
-packageName = "talos_data"
-meshPackageName = "talos_data"
-rootJointType = "freeflyer"    
-urdfName = "talos"
-urdfSuffix = "_reduced"
-srdfSuffix = ""
-
-
-rLegId = 'talos_rleg_rom'
-rleg = 'leg_right_1_joint'
-rfoot = 'leg_right_6_joint'
-
-lLegId = 'talos_lleg_rom'
-lleg = 'leg_left_1_joint'
-lfoot = 'leg_left_6_joint'
-
-rArmId = 'talos_rarm_rom'
-rarm = 'arm_right_1_joint'
-rhand = 'arm_right_7_joint'
-
-lArmId = 'talos_larm_rom'
-larm = 'arm_left_1_joint'
-lhand = 'arm_left_7_joint'
-
-
 pId = tp.ps.numberPaths() -1
-fullBody = FullBody ()
+fullBody = Robot ()
+
 
-fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
 fullBody.setJointBounds ("root_joint",  [-5,5, -1.5, 1.5, 0.95, 1.05])
 fullBody.client.robot.setDimensionExtraConfigSpace(tp.extraDof)
 fullBody.client.robot.setExtraConfigSpaceBounds([-tp.vMax,tp.vMax,-tp.vMax,tp.vMax,0,0,-tp.aMax,tp.aMax,-tp.aMax,tp.aMax,0,0])
@@ -48,15 +21,7 @@ ps.setParameter("Kinodynamic/accelerationBound",tp.aMax)
 v = tp.Viewer (ps,viewerClient=tp.v.client, displayCoM = True)
 
 
-q_ref = [
-        0.0, 0.0,  1.0232773,  0.0 ,  0.0, 0.0, 1.,                   #Free flyer
-        0.0,  0.0, -0.411354,  0.859395, -0.448041, -0.001708,          #Left Leg
-        0.0,  0.0, -0.411354,  0.859395, -0.448041, -0.001708,          #Right Leg
-        0.0 ,  0.006761,                                                #Chest
-        0.25847 ,  0.173046, -0.0002, -0.525366, 0.0, -0.0,  0.1,-0.005,  #Left Arm
-        -0.25847 , -0.173046, 0.0002  , -0.525366, 0.0,  0.0,  0.1,-0.005,#Right Arm
-        0.,  0.  ,                                                       #Head
-        0,0,0,0,0,0]; v (q_ref)
+q_ref = fullBody.referenceConfig[::]+[0]*6
 
 q_init = q_ref[::]
 
@@ -69,20 +34,12 @@ tStart = time.time()
 # generate databases : 
 
 nbSamples = 10000
-rLegOffset = [0.,  -0.00018, -0.107]
-rLegOffset[2] += 0.006
-rLegNormal = [0,0,1]
-rLegx = 0.1; rLegy = 0.06
-fullBody.addLimb(rLegId,rleg,rfoot,rLegOffset,rLegNormal, rLegx, rLegy, nbSamples, "fixedStep08", 0.01)
-fullBody.runLimbSampleAnalysis(rLegId, "ReferenceConfiguration", True)
+fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.rLegOffset,fullBody.rLegNormal, fullBody.rLegx, fullBody.rLegy, nbSamples, "fixedStep06", 0.01)
+fullBody.runLimbSampleAnalysis(fullBody.rLegId, "ReferenceConfiguration", True)
 #fullBody.saveLimbDatabase(rLegId, "./db/talos_rLeg_walk.db")
 
-lLegOffset = [0.,  -0.00018, -0.107]
-lLegOffset[2] += 0.006
-lLegNormal = [0,0,1]
-lLegx = 0.1; lLegy = 0.06
-fullBody.addLimb(lLegId,lleg,lfoot,lLegOffset,rLegNormal, lLegx, lLegy, nbSamples, "fixedStep08", 0.01)
-fullBody.runLimbSampleAnalysis(lLegId, "ReferenceConfiguration", True)
+fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.lLegOffset,fullBody.rLegNormal, fullBody.lLegx, fullBody.lLegy, nbSamples, "fixedStep06", 0.01)
+fullBody.runLimbSampleAnalysis(fullBody.lLegId, "ReferenceConfiguration", True)
 #fullBody.saveLimbDatabase(rLegId, "./db/talos_lLeg_walk.db")
 
 
@@ -125,8 +82,8 @@ v(q_goal)
 v.addLandmark('talos/base_link',0.3)
 v(q_init)
 
-fullBody.setStartState(q_init,[rLegId,lLegId])
-fullBody.setEndState(q_goal,[rLegId,lLegId])
+fullBody.setStartState(q_init,[fullBody.rLegId,fullBody.lLegId])
+fullBody.setEndState(q_goal,[fullBody.rLegId,fullBody.lLegId])
 
 
 from hpp.gepetto import PathPlayer
@@ -134,7 +91,7 @@ pp = PathPlayer ( v)
 
 print "Generate contact plan ..."
 tStart = time.time()
-configs = fullBody.interpolate(0.01,pathId=pId,robustnessTreshold = 2, filterStates = False)
+configs = fullBody.interpolate(0.01,pathId=pId,robustnessTreshold = 2, filterStates = True,testReachability=False)
 tInterpolateConfigs = time.time() - tStart
 print "Done."
 print "number of configs :", len(configs)
diff --git a/script/scenarios/demos/talos_flatGround_path.py b/script/scenarios/demos/talos_flatGround_path.py
index a52c405f152be6a8fc5a6a1d3460f2544880d2f6..31b1e9bbad5602b1142ca4cf7b417ab2e3f1cf1d 100644
--- a/script/scenarios/demos/talos_flatGround_path.py
+++ b/script/scenarios/demos/talos_flatGround_path.py
@@ -1,4 +1,4 @@
-from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.talos_abstract import Robot
 from hpp.gepetto import Viewer
 from hpp.corbaserver import Client
 from hpp.corbaserver import ProblemSolver
@@ -6,21 +6,13 @@ import time
 
 
 
-
-rootJointType = 'freeflyer'
-packageName = 'talos-rbprm'
-meshPackageName = 'talos-rbprm'
-urdfName = 'talos_trunk'
-urdfNameRom =  ['talos_larm_rom','talos_rarm_rom','talos_lleg_rom','talos_rleg_rom']
-urdfSuffix = ""
-srdfSuffix = ""
 vMax = 0.3
 aMax = 0.1
 extraDof = 6
 mu=0.5
 # Creating an instance of the helper class, and loading the robot
-rbprmBuilder = Builder ()
-rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+rbprmBuilder = Robot ()
+
 rbprmBuilder.setJointBounds ("root_joint", [-5,5, -1.5, 1.5, 0.95, 1.05])
 
 
@@ -45,14 +37,6 @@ ps.setParameter("DynamicPlanner/sizeFootY",0.12)
 ps.setParameter("DynamicPlanner/friction",0.5)
 ps.setParameter("ConfigurationShooter/sampleExtraDOF",False)
 
-p_lLeg = [-0.008846952891378526, 0.0848172440888579,-1.019272022956703]
-p_lLeg[0]=0. # assure symetry of dynamic constraints on flat ground
-p_rLeg = [-0.008846952891378526,-0.0848172440888579,-1.019272022956703]
-p_rLeg[0] = 0.
-p_lArm = [0.13028765672452458, 0.44360498616312666,-0.2881211563246389]
-p_rArm = [0.13028765672452458,- 0.44360498616312666,-0.2881211563246389]
-rbprmBuilder.setReferenceEndEffector('talos_lleg_rom',p_lLeg)
-rbprmBuilder.setReferenceEndEffector('talos_rleg_rom',p_rLeg)
 
 from hpp.gepetto import ViewerFactory
 vf = ViewerFactory (ps)
@@ -62,7 +46,7 @@ afftool = AffordanceTool ()
 afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
 afftool.loadObstacleModel ("hpp-rbprm-corba", "ground", "planning", vf)
 v = vf.createViewer(displayArrows = True)
-afftool.visualiseAffordances('Support', v, v.color.lightBrown)
+#afftool.visualiseAffordances('Support', v, v.color.lightBrown)
 
 # Setting initial configuration
 q_init = rbprmBuilder.getCurrentConfig ();
diff --git a/script/scenarios/demos/talos_navBauzil.py b/script/scenarios/demos/talos_navBauzil.py
index e39533e1aac6eeab59136e2d3b7f6230d01e0cdd..f514503f3403071ad9cbb73b10a2b9cf61632335 100644
--- a/script/scenarios/demos/talos_navBauzil.py
+++ b/script/scenarios/demos/talos_navBauzil.py
@@ -1,4 +1,4 @@
-from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.corbaserver.rbprm.talos import Robot
 from hpp.gepetto import Viewer
 from display_tools import *
 import time
@@ -8,37 +8,11 @@ import talos_navBauzil_path as tp
 print "Done."
 import time
 
-##
-#  Information to retrieve urdf and srdf files.
-packageName = "talos_data"
-meshPackageName = "talos_data"
-rootJointType = "freeflyer"    
-urdfName = "talos"
-urdfSuffix = "_reduced"
-srdfSuffix = ""
-
-
-rLegId = 'talos_rleg_rom'
-rleg = 'leg_right_1_joint'
-rfoot = 'leg_right_6_joint'
-
-lLegId = 'talos_lleg_rom'
-lleg = 'leg_left_1_joint'
-lfoot = 'leg_left_6_joint'
-
-rArmId = 'talos_rarm_rom'
-rarm = 'arm_right_1_joint'
-rhand = 'arm_right_7_joint'
-
-lArmId = 'talos_larm_rom'
-larm = 'arm_left_1_joint'
-lhand = 'arm_left_7_joint'
 
 
 pId = tp.ps.numberPaths() -1
-fullBody = FullBody ()
+fullBody = Robot ()
 
-fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
 root_bounds = tp.root_bounds[::]
 root_bounds[0] -= 0.2
 root_bounds[1] += 0.2
@@ -55,18 +29,9 @@ ps.setParameter("Kinodynamic/accelerationBound",tp.aMax)
 v = tp.Viewer (ps,viewerClient=tp.v.client, displayCoM = True)
 
 
-q_ref = [
-        0.0, 0.0,  1.0232773,  0.0 ,  0.0, 0.0, 1.,                   #Free flyer
-        0.0,  0.0, -0.411354,  0.859395, -0.448041, -0.001708,          #Left Leg
-        0.0,  0.0, -0.411354,  0.859395, -0.448041, -0.001708,          #Right Leg
-        0.0 ,  0.006761,                                                #Chest
-        0.25847 ,  0.173046, -0.0002, -0.525366, 0.0, -0.0,  0.1,-0.005,  #Left Arm
-        -0.25847 , -0.173046, 0.0002  , -0.525366, 0.0,  0.0,  0.1,-0.005,#Right Arm
-        0.,  0.  ,                                                       #Head
-        0,0,0,0,0,0]; v (q_ref)
 
+q_ref = fullBody.referenceConfig[::]+[0,0,0,0,0,0]
 q_init = q_ref[::]
-
 fullBody.setReferenceConfig(q_ref)
 fullBody.setCurrentConfig (q_init)
 
@@ -76,20 +41,12 @@ tStart = time.time()
 # generate databases : 
 
 nbSamples = 10000
-rLegOffset = [0.,  -0.00018, -0.107]
-rLegOffset[2] += 0.006
-rLegNormal = [0,0,1]
-rLegx = 0.1; rLegy = 0.06
-fullBody.addLimb(rLegId,rleg,rfoot,rLegOffset,rLegNormal, rLegx, rLegy, nbSamples, "fixedStep08", 0.01)
-fullBody.runLimbSampleAnalysis(rLegId, "ReferenceConfiguration", True)
+fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.rLegOffset,fullBody.rLegNormal, fullBody.rLegx, fullBody.rLegy, nbSamples, "fixedStep08", 0.01)
+fullBody.runLimbSampleAnalysis(fullBody.rLegId, "ReferenceConfiguration", True)
 #fullBody.saveLimbDatabase(rLegId, "./db/talos_rLeg_walk.db")
 
-lLegOffset = [0.,  -0.00018, -0.107]
-lLegOffset[2] += 0.006
-lLegNormal = [0,0,1]
-lLegx = 0.1; lLegy = 0.06
-fullBody.addLimb(lLegId,lleg,lfoot,lLegOffset,rLegNormal, lLegx, lLegy, nbSamples, "fixedStep08", 0.01)
-fullBody.runLimbSampleAnalysis(lLegId, "ReferenceConfiguration", True)
+fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.lLegOffset,fullBody.rLegNormal, fullBody.lLegx, fullBody.lLegy, nbSamples, "fixedStep08", 0.01)
+fullBody.runLimbSampleAnalysis(fullBody.lLegId, "ReferenceConfiguration", True)
 #fullBody.saveLimbDatabase(rLegId, "./db/talos_lLeg_walk.db")
 
 
@@ -132,8 +89,8 @@ v(q_goal)
 v.addLandmark('talos/base_link',0.3)
 v(q_init)
 
-fullBody.setStartState(q_init,[lLegId,rLegId])
-fullBody.setEndState(q_goal,[lLegId,rLegId])
+fullBody.setStartState(q_init,[fullBody.lLegId,fullBody.rLegId])
+fullBody.setEndState(q_goal,[fullBody.lLegId,fullBody.rLegId])
 
 
 from hpp.gepetto import PathPlayer
@@ -141,7 +98,7 @@ pp = PathPlayer ( v)
 
 print "Generate contact plan ..."
 tStart = time.time()
-configs = fullBody.interpolate(0.01,pathId=pId,robustnessTreshold = 2, filterStates = False)
+configs = fullBody.interpolate(0.01,pathId=pId,robustnessTreshold = 2, filterStates = True,testReachability=False)
 tInterpolateConfigs = time.time() - tStart
 print "Done."
 print "number of configs :", len(configs)
diff --git a/script/scenarios/demos/talos_navBauzil_hard.py b/script/scenarios/demos/talos_navBauzil_hard.py
new file mode 100644
index 0000000000000000000000000000000000000000..e9b3ef4300baa78d9ea349a2798e5fd6c7485464
--- /dev/null
+++ b/script/scenarios/demos/talos_navBauzil_hard.py
@@ -0,0 +1,109 @@
+from hpp.corbaserver.rbprm.talos import Robot
+from hpp.gepetto import Viewer
+from display_tools import *
+import time
+from hpp.corbaserver.rbprm.rbprmstate import State,StateHelper
+print "Plan guide trajectory ..."
+import talos_navBauzil_hard_path as tp
+print "Done."
+import time
+
+##
+#  Information to retrieve urdf and srdf files.
+
+
+pId = tp.ps.numberPaths() -1
+fullBody = Robot ()
+
+root_bounds = tp.root_bounds[::]
+root_bounds[0] -= 0.2
+root_bounds[1] += 0.2
+root_bounds[2] -= 0.2
+root_bounds[3] += 0.2
+root_bounds[-1] = 1.2
+root_bounds[-2] = 0.8
+fullBody.setJointBounds ("root_joint",  root_bounds)
+fullBody.client.robot.setDimensionExtraConfigSpace(tp.extraDof)
+fullBody.client.robot.setExtraConfigSpaceBounds([-tp.vMax,tp.vMax,-tp.vMax,tp.vMax,0,0,-tp.aMax,tp.aMax,-tp.aMax,tp.aMax,0,0])
+ps = tp.ProblemSolver( fullBody )
+ps.setParameter("Kinodynamic/velocityBound",tp.vMax)
+ps.setParameter("Kinodynamic/accelerationBound",tp.aMax)
+v = tp.Viewer (ps,viewerClient=tp.v.client, displayCoM = True)
+
+q_ref = fullBody.referenceConfig[::] + [0]*6
+q_init = q_ref[::]
+
+fullBody.setReferenceConfig(q_ref)
+fullBody.setCurrentConfig (q_init)
+
+
+print "Generate limb DB ..."
+tStart = time.time()
+# generate databases : 
+nbSamples = 10000
+fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.rLegOffset,fullBody.rLegNormal, fullBody.rLegx, fullBody.rLegy, nbSamples, "fixedStep08", 0.01)
+fullBody.runLimbSampleAnalysis(fullBody.rLegId, "ReferenceConfiguration", True)
+#fullBody.saveLimbDatabase(rLegId, "./db/talos_rLeg_walk.db")
+
+fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.lLegOffset,fullBody.rLegNormal, fullBody.lLegx, fullBody.lLegy, nbSamples, "fixedStep08", 0.01)
+fullBody.runLimbSampleAnalysis(fullBody.lLegId, "ReferenceConfiguration", True)
+#fullBody.saveLimbDatabase(rLegId, "./db/talos_lLeg_walk.db")
+
+
+
+tGenerate =  time.time() - tStart
+print "Done."
+print "Databases generated in : "+str(tGenerate)+" s"
+
+
+q_0 = fullBody.getCurrentConfig(); 
+configSize = fullBody.getConfigSize() -fullBody.client.robot.getDimensionExtraConfigSpace()
+
+q_init[0:7] = tp.ps.configAtParam(pId,0.01)[0:7] # use this to get the correct orientation
+q_goal = q_init[::]; q_goal[0:7] = tp.ps.configAtParam(pId,tp.ps.pathLength(pId))[0:7]
+dir_init = tp.ps.configAtParam(pId,0.01)[tp.indexECS:tp.indexECS+3]
+acc_init = tp.ps.configAtParam(pId,0.01)[tp.indexECS+3:tp.indexECS+6]
+dir_goal = tp.ps.configAtParam(pId,tp.ps.pathLength(pId)-0.01)[tp.indexECS:tp.indexECS+3]
+acc_goal = [0,0,0]
+
+robTreshold = 3
+# copy extraconfig for start and init configurations
+q_init[configSize:configSize+3] = dir_init[::]
+q_init[configSize+3:configSize+6] = acc_init[::]
+q_goal[configSize:configSize+3] = dir_goal[::]
+q_goal[configSize+3:configSize+6] = [0,0,0]
+
+
+q_init[2] = q_ref[2]
+q_goal[2] = q_ref[2]
+
+
+fullBody.setStaticStability(True)
+fullBody.setCurrentConfig (q_init)
+v(q_init)
+
+fullBody.setCurrentConfig (q_goal)
+v(q_goal)
+
+# specifying the full body configurations as start and goal state of the problem
+v.addLandmark('talos/base_link',0.3)
+v(q_init)
+
+fullBody.setStartState(q_init,[fullBody.lLegId,fullBody.rLegId])
+fullBody.setEndState(q_goal,[fullBody.lLegId,fullBody.rLegId])
+
+
+from hpp.gepetto import PathPlayer
+pp = PathPlayer ( v)
+
+print "Generate contact plan ..."
+tStart = time.time()
+configs = fullBody.interpolate(0.01,pathId=pId,robustnessTreshold = 2, filterStates = False)
+tInterpolateConfigs = time.time() - tStart
+print "Done."
+print "number of configs :", len(configs)
+raw_input("Press Enter to display the contact sequence ...")
+displayContactSequence(v,configs)
+
+
+
diff --git a/script/scenarios/demos/talos_navBauzil_hard_path.py b/script/scenarios/demos/talos_navBauzil_hard_path.py
index 06923c81dee8a1e97d440f7f7d71337c2be4939f..0303842295a89da1d34ccdaf681fa8e471639da0 100644
--- a/script/scenarios/demos/talos_navBauzil_hard_path.py
+++ b/script/scenarios/demos/talos_navBauzil_hard_path.py
@@ -1,30 +1,20 @@
-from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.talos_abstract import Robot
 from hpp.gepetto import Viewer
 from hpp.corbaserver import Client
 from hpp.corbaserver import ProblemSolver
 import time
 
 
-
-
-rootJointType = 'freeflyer'
-packageName = 'talos-rbprm'
-meshPackageName = 'talos-rbprm'
-urdfName = 'talos_trunk'
-urdfNameRom =  ['talos_larm_rom','talos_rarm_rom','talos_lleg_rom','talos_rleg_rom']
-urdfSuffix = ""
-srdfSuffix = ""
 vMax = 0.3
 aMax = 0.1
 extraDof = 6
 mu=0.5
 # Creating an instance of the helper class, and loading the robot
-rbprmBuilder = Builder ()
-rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+rbprmBuilder = Robot ()
 root_bounds = [-2.3,4.6,-1.5,3.3, 0.98, 0.98]
 rbprmBuilder.setJointBounds ("root_joint", root_bounds)
-rbprmBuilder.setJointBounds ('torso_1_joint', [0,0])
-rbprmBuilder.setJointBounds ('torso_2_joint', [0,0])
+#rbprmBuilder.setJointBounds ('torso_1_joint', [0,0])
+#rbprmBuilder.setJointBounds ('torso_2_joint', [0,0])
 
 # The following lines set constraint on the valid configurations:
 # a configuration is valid only if all limbs can create a contact ...
@@ -47,17 +37,8 @@ ps.setParameter("DynamicPlanner/sizeFootX",0.2)
 ps.setParameter("DynamicPlanner/sizeFootY",0.12)
 ps.setParameter("DynamicPlanner/friction",mu)
 ps.setParameter("ConfigurationShooter/sampleExtraDOF",False)
-ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops",100)
-
+ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops",50)
 
-p_lLeg = [-0.008846952891378526, 0.0848172440888579,-1.019272022956703]
-p_lLeg[0]=0. # assure symetry of dynamic constraints on flat ground
-p_rLeg = [-0.008846952891378526,-0.0848172440888579,-1.019272022956703]
-p_rLeg[0] = 0.
-p_lArm = [0.13028765672452458, 0.44360498616312666,-0.2881211563246389]
-p_rArm = [0.13028765672452458,- 0.44360498616312666,-0.2881211563246389]
-rbprmBuilder.setReferenceEndEffector('talos_lleg_rom',p_lLeg)
-rbprmBuilder.setReferenceEndEffector('talos_rleg_rom',p_rLeg)
 
 
 from hpp.gepetto import ViewerFactory
diff --git a/script/scenarios/demos/talos_navBauzil_path.py b/script/scenarios/demos/talos_navBauzil_path.py
index 2a03a15fa66ed7cc371664d24d60af802a954946..7bd4c90e504af84267a914c04e2cecac42ae3bce 100644
--- a/script/scenarios/demos/talos_navBauzil_path.py
+++ b/script/scenarios/demos/talos_navBauzil_path.py
@@ -1,4 +1,4 @@
-from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.talos_abstract import Robot
 from hpp.gepetto import Viewer
 from hpp.corbaserver import Client
 from hpp.corbaserver import ProblemSolver
@@ -6,25 +6,16 @@ import time
 
 
 
-
-rootJointType = 'freeflyer'
-packageName = 'talos-rbprm'
-meshPackageName = 'talos-rbprm'
-urdfName = 'talos_trunk'
-urdfNameRom =  ['talos_larm_rom','talos_rarm_rom','talos_lleg_rom','talos_rleg_rom']
-urdfSuffix = ""
-srdfSuffix = ""
 vMax = 0.3
 aMax = 0.1
 extraDof = 6
 mu=0.5
 # Creating an instance of the helper class, and loading the robot
-rbprmBuilder = Builder ()
-rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+rbprmBuilder = Robot()
 root_bounds = [-1.5,3,0.,3.3, 0.98, 0.98]
 rbprmBuilder.setJointBounds ("root_joint", root_bounds)
-rbprmBuilder.setJointBounds ('torso_1_joint', [0,0])
-rbprmBuilder.setJointBounds ('torso_2_joint', [0,0])
+#rbprmBuilder.setJointBounds ('torso_1_joint', [0,0])
+#rbprmBuilder.setJointBounds ('torso_2_joint', [0,0])
 
 # The following lines set constraint on the valid configurations:
 # a configuration is valid only if all limbs can create a contact ...
@@ -49,14 +40,6 @@ ps.setParameter("DynamicPlanner/friction",mu)
 ps.setParameter("ConfigurationShooter/sampleExtraDOF",False)
 ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops",100)
 
-p_lLeg = [-0.008846952891378526, 0.0848172440888579,-1.019272022956703]
-p_lLeg[0]=0. # assure symetry of dynamic constraints on flat ground
-p_rLeg = [-0.008846952891378526,-0.0848172440888579,-1.019272022956703]
-p_rLeg[0] = 0.
-p_lArm = [0.13028765672452458, 0.44360498616312666,-0.2881211563246389]
-p_rArm = [0.13028765672452458,- 0.44360498616312666,-0.2881211563246389]
-rbprmBuilder.setReferenceEndEffector('talos_lleg_rom',p_lLeg)
-rbprmBuilder.setReferenceEndEffector('talos_rleg_rom',p_rLeg)
 
 from hpp.gepetto import ViewerFactory
 vf = ViewerFactory (ps)