diff --git a/script/dynamic/bauzilStairs_hrp2_interStatic.py b/script/dynamic/bauzilStairs_hrp2_interStatic.py
new file mode 100644
index 0000000000000000000000000000000000000000..77096cf7c60c96c9707aaea366ee86ec30bfde0e
--- /dev/null
+++ b/script/dynamic/bauzilStairs_hrp2_interStatic.py
@@ -0,0 +1,172 @@
+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.gepetto import Viewer
+from tools import *
+import bauzilStairs_hrp2_pathKino as tp
+import time
+import omniORB.any
+
+
+packageName = "hrp2_14_description"
+meshPackageName = "hrp2_14_description"
+rootJointType = "freeflyer"
+##
+#  Information to retrieve urdf and srdf files.
+urdfName = "hrp2_14"
+urdfSuffix = "_reduced"
+srdfSuffix = ""
+pId = tp.ps.numberPaths() -1
+fullBody = FullBody ()
+
+fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+fullBody.setJointBounds ("base_joint_xyz",  [-5.5,5.5, -2.5, 2.5, 0.3, 0.65])
+fullBody.client.basic.robot.setDimensionExtraConfigSpace(tp.extraDof)
+
+ps = tp.ProblemSolver( fullBody )
+ps.client.problem.setParameter("aMax",omniORB.any.to_any(tp.aMax))
+ps.client.problem.setParameter("vMax",omniORB.any.to_any(tp.vMax))
+
+r = tp.Viewer (ps,viewerClient=tp.r.client,displayArrows = True, displayCoM = True)
+
+q_init =[0, 0, 0.648702, 1.0, 0.0 , 0.0, 0.0,0.0, 0.0, 0.0, 0.0,0.261799388,  0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17,0.261799388, -0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17,0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0,0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0,0,0,0,0,0,0]; r (q_init)
+q_ref = q_init[::]
+fullBody.setCurrentConfig (q_init)
+fullBody.setReferenceConfig (q_ref)
+
+
+
+#~ AFTER loading obstacles
+rLegId = 'hrp2_rleg_rom'
+lLegId = 'hrp2_lleg_rom'
+tStart = time.time()
+
+
+rLeg = 'RLEG_JOINT0'
+rLegOffset = [0,0,-0.105]
+rLegLimbOffset=[0,0,-0.06]#0.035
+rLegNormal = [0,0,1]
+rLegx = 0.09; rLegy = 0.05
+#fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 50000, "forward", 0.1,"_6_DOF")
+fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 50000, "dynamicWalk", 0.05,"_6_DOF",limbOffset=rLegLimbOffset)
+fullBody.runLimbSampleAnalysis(rLegId, "ReferenceConfiguration", True)
+#fullBody.saveLimbDatabase(rLegId, "./db/hrp2_rleg_db.db")
+
+lLeg = 'LLEG_JOINT0'
+lLegOffset = [0,0,-0.105]
+lLegLimbOffset=[0,0,0.06]
+lLegNormal = [0,0,1]
+lLegx = 0.09; lLegy = 0.05
+#fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 50000, "forward", 0.1,"_6_DOF")
+fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 50000, "dynamicWalk", 0.05,"_6_DOF",limbOffset=lLegLimbOffset)
+fullBody.runLimbSampleAnalysis(lLegId, "ReferenceConfiguration", True)
+#fullBody.saveLimbDatabase(lLegId, "./db/hrp2_lleg_db.db")
+tGenerate =  time.time() - tStart
+print "generate databases in : "+str(tGenerate)+" s"
+
+
+"""
+fullBody.addLimbDatabase("./db/hrp2_rleg_db.db",rLegId,"forward")
+fullBody.addLimbDatabase("./db/hrp2_lleg_db.db",lLegId,"forward")
+tLoad =  time.time() - tStart
+print "Load databases in : "+str(tLoad)+" s"
+"""
+
+
+q_0 = fullBody.getCurrentConfig(); 
+#~ fullBody.createOctreeBoxes(r.client.gui, 1, rarmId, q_0,)
+
+
+
+
+configSize = fullBody.getConfigSize() -fullBody.client.basic.robot.getDimensionExtraConfigSpace()
+
+q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.ps.configAtParam(pId,0)[0:7] # use this to get the correct orientation
+q_goal = fullBody.getCurrentConfig(); 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)[tp.indexECS+3:tp.indexECS+6]
+dir_goal = tp.ps.configAtParam(pId,tp.ps.pathLength(pId))[tp.indexECS:tp.indexECS+3]
+acc_goal = [0,0,0]
+
+robTreshold = 1
+# 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]
+
+
+# FIXME : test
+q_init[2] = q_init[2]+0.1
+q_goal[2] = q_goal[2]+0.1
+
+
+# Randomly generating a contact configuration at q_init
+fullBody.setStaticStability(True)
+fullBody.setCurrentConfig (q_init)
+r(q_init)
+q_init = fullBody.generateContacts(q_init,dir_init,acc_init,robTreshold)
+r(q_init)
+
+# Randomly generating a contact configuration at q_end
+fullBody.setCurrentConfig (q_goal)
+q_goal = fullBody.generateContacts(q_goal, dir_goal,acc_goal,robTreshold)
+r(q_goal)
+
+# specifying the full body configurations as start and goal state of the problem
+r.addLandmark('hrp2_14/BODY',0.3)
+r(q_init)
+
+
+fullBody.setStartState(q_init,[rLegId,lLegId])
+fullBody.setEndState(q_goal,[rLegId,lLegId])
+fullBody.setStaticStability(True) # only set it after the init/goal configuration are computed
+
+
+
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (fullBody.client.basic, r)
+
+import fullBodyPlayerHrp2
+
+tStart = time.time()
+configs = fullBody.interpolate(0.01,pathId=pId,robustnessTreshold = robTreshold, filterStates = True)
+tInterpolate = time.time()-tStart
+print "number of configs : ", len(configs)
+print "generated in "+str(tInterpolate)+" s"
+r(configs[len(configs)-2])
+
+
+player = fullBodyPlayerHrp2.Player(fullBody,pp,tp,configs,draw=False,use_window=1,optim_effector=True,use_velocity=False,pathId = pId)
+
+# remove the last config (= user defined q_goal, not consitent with the previous state)
+
+#r(configs[0])
+#player.displayContactPlan(1.)
+
+#player.interpolate(2,len(configs)-1)
+
+"""
+r(configs[5])
+dir = configs[5][37:40]
+fullBody.client.rbprm.rbprm.evaluateConfig(configs[5],dir)
+"""
+
+from planning.config import *
+from generate_contact_sequence import *
+cs = generateContactSequence(fullBody,configs,r)
+filename = OUTPUT_DIR + "/" + OUTPUT_SEQUENCE_FILE
+cs.saveAsXML(filename, "ContactSequence")
+print "save contact sequence : ",filename
+
+
+r(q_init)
+pos=fullBody.getJointPosition('RLEG_JOINT0')
+addSphere(r,r.color.blue,pos)
+
+
+
+id = r.client.gui.getWindowID("window_hpp_")
+r.client.gui.attachCameraToNode( 'hrp2_14/BODY_0',id)
+
+
+
diff --git a/script/dynamic/bauzilStairs_hrp2_pathKino.py b/script/dynamic/bauzilStairs_hrp2_pathKino.py
new file mode 100644
index 0000000000000000000000000000000000000000..077419bd61c28920be76a69f51085ad78a609540
--- /dev/null
+++ b/script/dynamic/bauzilStairs_hrp2_pathKino.py
@@ -0,0 +1,251 @@
+## Importing helper class for setting up a reachability planning problem
+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+
+# Importing Gepetto viewer helper class
+from hpp.gepetto import Viewer
+import time
+import math
+import omniORB.any
+from planning.config import *
+
+from hpp.corbaserver import Client
+from hpp.corbaserver.robot import Robot as Parent
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+
+class Robot (Parent):
+	rootJointType = 'freeflyer'
+	packageName = 'hpp-rbprm-corba'
+	meshPackageName = 'hpp-rbprm-corba'
+	# URDF file describing the trunk of the robot HyQ
+	urdfName = 'hrp2_trunk_flexible'
+	urdfSuffix = ""
+	srdfSuffix = ""
+	def __init__ (self, robotName, load = True):
+		Parent.__init__ (self, robotName, self.rootJointType, load)
+		self.tf_root = "base_footprint"
+		self.client.basic = Client ()
+		self.load = load
+		
+
+
+rootJointType = 'freeflyer'
+packageName = 'hpp-rbprm-corba'
+meshPackageName = 'hpp-rbprm-corba'
+urdfName = 'hrp2_trunk_arms_flexible'
+urdfNameRoms =  ['hrp2_larm_rom','hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom']
+urdfSuffix = ""
+srdfSuffix = ""
+
+
+# Creating an instance of the helper class, and loading the robot
+rbprmBuilder = Builder ()
+rbprmBuilder.loadModel(urdfName, urdfNameRoms, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+
+
+
+# The following lines set constraint on the valid configurations:
+# a configuration is valid only if all limbs can create a contact ...
+rbprmBuilder.setFilter(['hrp2_lleg_rom','hrp2_rleg_rom'])
+#rbprmBuilder.setAffordanceFilter('hrp2_rarm_rom', ['Lean'])
+#rbprmBuilder.setAffordanceFilter('hrp2_larm_rom', ['Lean'])
+rbprmBuilder.setAffordanceFilter('hrp2_lleg_rom', ['Support',])
+rbprmBuilder.setAffordanceFilter('hrp2_rleg_rom', ['Support'])
+vMax = 0.3;
+aMax = 0.5;
+extraDof = 6
+
+rbprmBuilder.setJointBounds ("base_joint_xyz", [,, ,, 0.55, 0.6])
+rbprmBuilder.setJointBounds('CHEST_JOINT0',[-0.05,0.05])
+rbprmBuilder.setJointBounds('CHEST_JOINT1',[-0.05,0.05])
+# We also bound the rotations of the torso. (z, y, x)
+rbprmBuilder.boundSO3([-math.pi,math.pi,-0.1,0.1,-0.1,0.1])
+rbprmBuilder.client.basic.robot.setDimensionExtraConfigSpace(extraDof)
+rbprmBuilder.client.basic.robot.setExtraConfigSpaceBounds([0,0,0,0,0,0,0,0,0,0,0,0])
+indexECS = rbprmBuilder.getConfigSize() - rbprmBuilder.client.basic.robot.getDimensionExtraConfigSpace()
+
+
+# Creating an instance of HPP problem solver and the viewer
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+ps = ProblemSolver( rbprmBuilder )
+ps.client.problem.setParameter("aMax",omniORB.any.to_any(aMax))
+ps.client.problem.setParameter("vMax",omniORB.any.to_any(vMax))
+ps.client.problem.setParameter("orientedPath",omniORB.any.to_any(0.))
+ps.client.problem.setParameter("friction",omniORB.any.to_any(MU))
+ps.client.problem.setParameter("sizeFootX",omniORB.any.to_any(0.24))
+ps.client.problem.setParameter("sizeFootY",omniORB.any.to_any(0.14))
+
+
+r = Viewer (ps,displayArrows = True)
+
+
+from hpp.corbaserver.affordance.affordance import AffordanceTool
+afftool = AffordanceTool ()
+afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
+afftool.loadObstacleModel (packageName, ENV_NAME, ENV_PREFIX, r)
+#r.loadObstacleModel (packageName, "ground", "planning")
+afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
+r.addLandmark(r.sceneName,1)
+
+# Setting initial and goal configurations
+q_init = rbprmBuilder.getCurrentConfig ();
+q_init[3:7] = [1,0,0,0]
+q_init [0:3] = [0, 0, 0.58]; r (q_init)
+
+
+rbprmBuilder.setCurrentConfig (q_init)
+q_goal = q_init [::]
+
+q_goal[3:7] = [1,0,0,0]
+q_goal [0:3] = [5, 1, 0.58]; r(q_goal)
+
+r (q_goal)
+
+
+# Choosing a path optimizer
+ps.setInitialConfig (q_init)
+ps.addGoalConfig (q_goal)
+# Choosing RBPRM shooter and path validation methods.
+ps.client.problem.selectConFigurationShooter("RbprmShooter")
+ps.client.problem.selectPathValidation("RbprmDynamicPathValidation",0.05)
+# Choosing kinodynamic methods : 
+ps.selectSteeringMethod("RBPRMKinodynamic")
+ps.selectDistance("KinodynamicDistance")
+ps.addPathOptimizer("RandomShortcutDynamic")
+ps.addPathOptimizer("OrientedPathOptimizer")
+ps.selectPathPlanner("DynamicPlanner")
+
+#solve the problem :
+r(q_init)
+
+#ps.client.problem.prepareSolveStepByStep()
+
+q_far = q_init[::]
+q_far[2] = -3
+r(q_far)
+
+
+
+
+
+
+
+
+"""
+camera = [0.6293167471885681,
+ -9.560577392578125,
+ 10.504343032836914,
+ 0.9323806762695312,
+ 0.36073973774909973,
+ 0.008668755181133747,
+ 0.02139890193939209]
+r.client.gui.setCameraTransform(0,camera)
+"""
+
+
+t = ps.solve ()
+
+#r.displayRoadmap('rm',radiusSphere=0.01)
+#r.displayPathMap("pm",0)
+
+#tf=r.solveAndDisplay("rm",1,0.01)
+#t = [0,0,tf,0]
+#r.client.gui.removeFromGroup("rm_group",r.sceneName)
+
+
+
+
+
+# Playing the computed path
+pi = ps.numberPaths()-1
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (rbprmBuilder.client.basic, r)
+pp.dt=0.03
+pp.displayVelocityPath(pi)
+r.client.gui.setVisibility("path_"+str(pi)+"_root","ALWAYS_ON_TOP")
+#display path
+pp.speed=1
+#pp (0)
+
+
+import parse_bench
+
+parse_bench.parseBenchmark(t)
+
+
+###########################
+#display path with post-optimisation
+
+"""
+print("Press Enter to display the optimization ...")
+raw_input()
+i=0
+
+r.client.gui.removeFromGroup("path_0_root",r.sceneName)
+pp.displayVelocityPath(1)
+
+for i in range(1,5):
+  time.sleep(3)
+  ps.optimizePath(i)
+  r.client.gui.removeFromGroup("path_"+str(i)+"_root",r.sceneName)
+  pp.displayVelocityPath(i+1)
+"""
+
+###########################
+
+
+"""
+q_far = q_init[::]
+q_far[2] = -3
+r(q_far)
+"""
+
+
+"""
+for i in range(1,10):
+    rbprmBuilder.client.basic.problem.optimizePath(i)
+    r.client.gui.removeFromGroup("path_"+str(i)+"_root",r.sceneName)
+    pp.displayVelocityPath(i+1)
+    #time.sleep(2)
+"""
+
+"""
+i=0
+
+ps.clearRoadmap()
+ps.solve()
+r.client.gui.removeFromGroup("path_"+str(i)+"_root",r.sceneName)
+i = i+1
+pp.displayVelocityPath(i)
+
+pp(i)
+
+
+"""
+
+"""
+r.client.gui.addCurve("c1",qlist,r.color.red)
+r.client.gui.setVisibility("c1","ALWAYS_ON_TOP")
+r.client.gui.addToGroup("c1",r.sceneName)
+
+
+r.client.gui.addCurve("c2",qlist2,r.color.blue)
+r.client.gui.setVisibility("c2","ALWAYS_ON_TOP")
+r.client.gui.addToGroup("c2",r.sceneName)
+
+
+
+"""
+
+
+"""
+nodes = ["hyq_trunk_large/base_link","Vec_Acceleration","Vec_Velocity"]
+r.client.gui.setCaptureTransform("yaml/hyq_slalom_path.yaml",nodes)
+r.client.gui.captureTransformOnRefresh(True)
+pp(1)
+r.client.gui.captureTransformOnRefresh(False)
+
+r.client.gui.writeNodeFile('path_1_root','meshs/slalom_path.obj')
+
+"""
+
+
diff --git a/script/dynamic/walkBauzil_hrp2_interStatic.py b/script/dynamic/walkBauzil_hrp2_interStatic.py
new file mode 100644
index 0000000000000000000000000000000000000000..7a7506fe32ff5208c53e070e71fa4741fd2f6c85
--- /dev/null
+++ b/script/dynamic/walkBauzil_hrp2_interStatic.py
@@ -0,0 +1,173 @@
+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.gepetto import Viewer
+from tools import *
+import walkBauzil_hrp2_pathKino as tp
+import time
+import omniORB.any
+
+
+packageName = "hrp2_14_description"
+meshPackageName = "hrp2_14_description"
+rootJointType = "freeflyer"
+##
+#  Information to retrieve urdf and srdf files.
+urdfName = "hrp2_14"
+urdfSuffix = "_reduced"
+srdfSuffix = ""
+pId = tp.ps.numberPaths() -1
+fullBody = FullBody ()
+
+fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+fullBody.setJointBounds ("base_joint_xyz",  [-3,4.5,-2 ,2.5, 0.55, 0.6])
+fullBody.client.basic.robot.setDimensionExtraConfigSpace(tp.extraDof)
+
+ps = tp.ProblemSolver( fullBody )
+ps.client.problem.setParameter("aMax",omniORB.any.to_any(tp.aMax))
+ps.client.problem.setParameter("vMax",omniORB.any.to_any(tp.vMax))
+
+r = tp.Viewer (ps,viewerClient=tp.r.client,displayArrows = True, displayCoM = True)
+
+q_init =[0, 0, 0.648702, 1.0, 0.0 , 0.0, 0.0,0.0, 0.0, 0.0, 0.0,0.261799388,  0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17,0.261799388, -0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17,0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0,0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0,0,0,0,0,0,0]; r (q_init)
+q_ref = q_init[::]
+fullBody.setCurrentConfig (q_init)
+fullBody.setReferenceConfig (q_ref)
+
+
+
+#~ AFTER loading obstacles
+rLegId = 'hrp2_rleg_rom'
+lLegId = 'hrp2_lleg_rom'
+tStart = time.time()
+
+
+rLeg = 'RLEG_JOINT0'
+rLegOffset = [0,0,-0.105]
+rLegLimbOffset=[0,0,-0.06]#0.035
+rLegNormal = [0,0,1]
+rLegx = 0.09; rLegy = 0.05
+#fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 50000, "forward", 0.1,"_6_DOF")
+fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 50000, "dynamicWalk", 0.05,"_6_DOF",limbOffset=rLegLimbOffset)
+fullBody.runLimbSampleAnalysis(rLegId, "ReferenceConfiguration", True)
+#fullBody.saveLimbDatabase(rLegId, "./db/hrp2_rleg_db.db")
+
+lLeg = 'LLEG_JOINT0'
+lLegOffset = [0,0,-0.105]
+lLegLimbOffset=[0,0,0.06]
+lLegNormal = [0,0,1]
+lLegx = 0.09; lLegy = 0.05
+#fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 50000, "forward", 0.1,"_6_DOF")
+fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 50000, "dynamicWalk", 0.05,"_6_DOF",limbOffset=lLegLimbOffset)
+fullBody.runLimbSampleAnalysis(lLegId, "ReferenceConfiguration", True)
+#fullBody.saveLimbDatabase(lLegId, "./db/hrp2_lleg_db.db")
+tGenerate =  time.time() - tStart
+print "generate databases in : "+str(tGenerate)+" s"
+
+
+"""
+fullBody.addLimbDatabase("./db/hrp2_rleg_db.db",rLegId,"forward")
+fullBody.addLimbDatabase("./db/hrp2_lleg_db.db",lLegId,"forward")
+tLoad =  time.time() - tStart
+print "Load databases in : "+str(tLoad)+" s"
+"""
+
+
+q_0 = fullBody.getCurrentConfig(); 
+#~ fullBody.createOctreeBoxes(r.client.gui, 1, rarmId, q_0,)
+
+
+
+eps=0.0001
+configSize = fullBody.getConfigSize() -fullBody.client.basic.robot.getDimensionExtraConfigSpace()
+
+q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.ps.configAtParam(pId,eps)[0:7] # use this to get the correct orientation
+q_goal = fullBody.getCurrentConfig(); q_goal[0:7] = tp.ps.configAtParam(pId,tp.ps.pathLength(pId)-0.0001)[0:7]
+dir_init = tp.ps.configAtParam(pId,eps)[tp.indexECS:tp.indexECS+3]
+acc_init = tp.ps.configAtParam(pId,0)[tp.indexECS+3:tp.indexECS+6]
+dir_goal = tp.ps.configAtParam(pId,tp.ps.pathLength(pId)-eps)[tp.indexECS:tp.indexECS+3]
+acc_goal = [0,0,0]
+
+robTreshold = 1
+# 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]
+
+
+# FIXME : test
+q_init[2] = q_init[2]+0.1
+q_goal[2] = q_goal[2]+0.1
+
+
+# Randomly generating a contact configuration at q_init
+fullBody.setStaticStability(True)
+fullBody.setCurrentConfig (q_init)
+r(q_init)
+q_init = fullBody.generateContacts(q_init,dir_init,acc_init,robTreshold)
+r(q_init)
+
+# Randomly generating a contact configuration at q_end
+fullBody.setCurrentConfig (q_goal)
+q_goal = fullBody.generateContacts(q_goal, dir_goal,acc_goal,robTreshold)
+r(q_goal)
+
+# specifying the full body configurations as start and goal state of the problem
+r.addLandmark('hrp2_14/BODY',0.3)
+r(q_init)
+
+
+fullBody.setStartState(q_init,[rLegId,lLegId])
+fullBody.setEndState(q_goal,[rLegId,lLegId])
+fullBody.setStaticStability(True) # only set it after the init/goal configuration are computed
+
+
+
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (fullBody.client.basic, r)
+
+import fullBodyPlayerHrp2
+
+tStart = time.time()
+configs = fullBody.interpolate(0.01,pathId=pId,robustnessTreshold = robTreshold, filterStates = True)
+tInterpolate = time.time()-tStart
+print "number of configs : ", len(configs)
+print "generated in "+str(tInterpolate)+" s"
+r(configs[len(configs)-2])
+
+
+player = fullBodyPlayerHrp2.Player(fullBody,pp,tp,configs,draw=False,use_window=1,optim_effector=True,use_velocity=False,pathId = pId)
+
+# remove the last config (= user defined q_goal, not consitent with the previous state)
+
+#r(configs[0])
+#player.displayContactPlan(1.)
+
+#player.interpolate(2,len(configs)-1)
+
+"""
+r(configs[5])
+dir = configs[5][37:40]
+fullBody.client.rbprm.rbprm.evaluateConfig(configs[5],dir)
+"""
+
+from planning.config import *
+from generate_contact_sequence import *
+cs = generateContactSequence(fullBody,configs,r)
+filename = OUTPUT_DIR + "/" + OUTPUT_SEQUENCE_FILE
+cs.saveAsXML(filename, "ContactSequence")
+print "save contact sequence : ",filename
+
+
+"""
+r(q_init)
+pos=fullBody.getJointPosition('RLEG_JOINT0')
+addSphere(r,r.color.blue,pos)
+"""
+
+
+wid = r.client.gui.getWindowID("window_hpp_")
+#r.client.gui.attachCameraToNode( 'hrp2_14/BODY_0',wid)
+
+
+
diff --git a/script/dynamic/walkBauzil_hrp2_pathKino.py b/script/dynamic/walkBauzil_hrp2_pathKino.py
new file mode 100644
index 0000000000000000000000000000000000000000..d98cddfd387545c0706811568845c430d6fa1fa1
--- /dev/null
+++ b/script/dynamic/walkBauzil_hrp2_pathKino.py
@@ -0,0 +1,251 @@
+## Importing helper class for setting up a reachability planning problem
+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+
+# Importing Gepetto viewer helper class
+from hpp.gepetto import Viewer
+import time
+import math
+import omniORB.any
+from planning.config import *
+
+from hpp.corbaserver import Client
+from hpp.corbaserver.robot import Robot as Parent
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+
+class Robot (Parent):
+	rootJointType = 'freeflyer'
+	packageName = 'hpp-rbprm-corba'
+	meshPackageName = 'hpp-rbprm-corba'
+	# URDF file describing the trunk of the robot HyQ
+	urdfName = 'hrp2_trunk_flexible'
+	urdfSuffix = ""
+	srdfSuffix = ""
+	def __init__ (self, robotName, load = True):
+		Parent.__init__ (self, robotName, self.rootJointType, load)
+		self.tf_root = "base_footprint"
+		self.client.basic = Client ()
+		self.load = load
+		
+
+
+rootJointType = 'freeflyer'
+packageName = 'hpp-rbprm-corba'
+meshPackageName = 'hpp-rbprm-corba'
+urdfName = 'hrp2_trunk_arms_flexible'
+urdfNameRoms =  ['hrp2_larm_rom','hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom']
+urdfSuffix = ""
+srdfSuffix = ""
+
+
+# Creating an instance of the helper class, and loading the robot
+rbprmBuilder = Builder ()
+rbprmBuilder.loadModel(urdfName, urdfNameRoms, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+
+
+
+# The following lines set constraint on the valid configurations:
+# a configuration is valid only if all limbs can create a contact ...
+rbprmBuilder.setFilter(['hrp2_lleg_rom','hrp2_rleg_rom'])
+#rbprmBuilder.setAffordanceFilter('hrp2_rarm_rom', ['Lean'])
+#rbprmBuilder.setAffordanceFilter('hrp2_larm_rom', ['Lean'])
+rbprmBuilder.setAffordanceFilter('hrp2_lleg_rom', ['Support',])
+rbprmBuilder.setAffordanceFilter('hrp2_rleg_rom', ['Support'])
+vMax = 0.3;
+aMax = 0.5;
+extraDof = 6
+
+rbprmBuilder.setJointBounds ("base_joint_xyz", [-3,4.5,-2 ,2.5, 0.55, 0.6])
+rbprmBuilder.setJointBounds('CHEST_JOINT0',[-0.05,0.05])
+rbprmBuilder.setJointBounds('CHEST_JOINT1',[-0.05,0.05])
+# We also bound the rotations of the torso. (z, y, x)
+rbprmBuilder.boundSO3([-math.pi,math.pi,-0.1,0.1,-0.1,0.1])
+rbprmBuilder.client.basic.robot.setDimensionExtraConfigSpace(extraDof)
+rbprmBuilder.client.basic.robot.setExtraConfigSpaceBounds([0,0,0,0,0,0,0,0,0,0,0,0])
+indexECS = rbprmBuilder.getConfigSize() - rbprmBuilder.client.basic.robot.getDimensionExtraConfigSpace()
+
+
+# Creating an instance of HPP problem solver and the viewer
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+ps = ProblemSolver( rbprmBuilder )
+ps.client.problem.setParameter("aMax",omniORB.any.to_any(aMax))
+ps.client.problem.setParameter("vMax",omniORB.any.to_any(vMax))
+ps.client.problem.setParameter("orientedPath",omniORB.any.to_any(0.))
+ps.client.problem.setParameter("friction",omniORB.any.to_any(MU))
+ps.client.problem.setParameter("sizeFootX",omniORB.any.to_any(0.24))
+ps.client.problem.setParameter("sizeFootY",omniORB.any.to_any(0.14))
+
+
+r = Viewer (ps,displayArrows = True)
+
+
+from hpp.corbaserver.affordance.affordance import AffordanceTool
+afftool = AffordanceTool ()
+afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
+afftool.loadObstacleModel (packageName, ENV_NAME, ENV_PREFIX, r)
+#r.loadObstacleModel (packageName, "ground", "planning")
+#afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
+r.addLandmark(r.sceneName,1)
+
+# Setting initial and goal configurations
+q_init = rbprmBuilder.getCurrentConfig ();
+q_init[3:7] = [0.7071,0,0,0.7071]
+q_init [0:3] = [-0.9, 0.2, 0.58]; r (q_init)
+
+
+rbprmBuilder.setCurrentConfig (q_init)
+q_goal = q_init [::]
+
+q_goal[3:7] = [0.7071,0,0,-0.7071]
+q_goal [0:3] = [3.5, -0.5, 0.58]; r(q_goal)
+
+r (q_goal)
+
+
+# Choosing a path optimizer
+ps.setInitialConfig (q_init)
+ps.addGoalConfig (q_goal)
+# Choosing RBPRM shooter and path validation methods.
+ps.client.problem.selectConFigurationShooter("RbprmShooter")
+ps.client.problem.selectPathValidation("RbprmDynamicPathValidation",0.05)
+# Choosing kinodynamic methods : 
+ps.selectSteeringMethod("RBPRMKinodynamic")
+ps.selectDistance("KinodynamicDistance")
+ps.addPathOptimizer("RandomShortcutDynamic")
+ps.addPathOptimizer("OrientedPathOptimizer")
+ps.selectPathPlanner("DynamicPlanner")
+
+#solve the problem :
+r(q_init)
+
+#ps.client.problem.prepareSolveStepByStep()
+
+q_far = q_init[::]
+q_far[2] = -3
+r(q_far)
+
+
+
+
+
+
+
+
+"""
+camera = [0.6293167471885681,
+ -9.560577392578125,
+ 10.504343032836914,
+ 0.9323806762695312,
+ 0.36073973774909973,
+ 0.008668755181133747,
+ 0.02139890193939209]
+r.client.gui.setCameraTransform(0,camera)
+"""
+
+
+t = ps.solve ()
+
+#r.displayRoadmap('rm',radiusSphere=0.01)
+#r.displayPathMap("pm",0)
+
+#tf=r.solveAndDisplay("rm",1,0.01)
+#t = [0,0,tf,0]
+#r.client.gui.removeFromGroup("rm_group",r.sceneName)
+
+
+
+
+
+# Playing the computed path
+pi = ps.numberPaths()-1
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (rbprmBuilder.client.basic, r)
+pp.dt=0.03
+pp.displayVelocityPath(pi)
+r.client.gui.setVisibility("path_"+str(pi)+"_root","ALWAYS_ON_TOP")
+#display path
+pp.speed=1
+#pp (0)
+
+
+import parse_bench
+
+parse_bench.parseBenchmark(t)
+
+
+###########################
+#display path with post-optimisation
+
+"""
+print("Press Enter to display the optimization ...")
+raw_input()
+i=0
+
+r.client.gui.removeFromGroup("path_0_root",r.sceneName)
+pp.displayVelocityPath(1)
+
+for i in range(1,5):
+  time.sleep(3)
+  ps.optimizePath(i)
+  r.client.gui.removeFromGroup("path_"+str(i)+"_root",r.sceneName)
+  pp.displayVelocityPath(i+1)
+"""
+
+###########################
+
+
+"""
+q_far = q_init[::]
+q_far[2] = -3
+r(q_far)
+"""
+
+
+"""
+for i in range(1,10):
+    rbprmBuilder.client.basic.problem.optimizePath(i)
+    r.client.gui.removeFromGroup("path_"+str(i)+"_root",r.sceneName)
+    pp.displayVelocityPath(i+1)
+    #time.sleep(2)
+"""
+
+"""
+i=0
+
+ps.clearRoadmap()
+ps.solve()
+r.client.gui.removeFromGroup("path_"+str(i)+"_root",r.sceneName)
+i = i+1
+pp.displayVelocityPath(i)
+
+pp(i)
+
+
+"""
+
+"""
+r.client.gui.addCurve("c1",qlist,r.color.red)
+r.client.gui.setVisibility("c1","ALWAYS_ON_TOP")
+r.client.gui.addToGroup("c1",r.sceneName)
+
+
+r.client.gui.addCurve("c2",qlist2,r.color.blue)
+r.client.gui.setVisibility("c2","ALWAYS_ON_TOP")
+r.client.gui.addToGroup("c2",r.sceneName)
+
+
+
+"""
+
+
+"""
+nodes = ["hyq_trunk_large/base_link","Vec_Acceleration","Vec_Velocity"]
+r.client.gui.setCaptureTransform("yaml/hyq_slalom_path.yaml",nodes)
+r.client.gui.captureTransformOnRefresh(True)
+pp(1)
+r.client.gui.captureTransformOnRefresh(False)
+
+r.client.gui.writeNodeFile('path_1_root','meshs/slalom_path.obj')
+
+"""
+
+