diff --git a/script/dynamic/darpa_hyq_pathKino.py b/script/dynamic/darpa_hyq_pathKino.py
index 4defee69c752754096fbbc15bed5665d6e363b09..3a5bbbb873d247f64f025337dca294d3025101e5 100644
--- a/script/dynamic/darpa_hyq_pathKino.py
+++ b/script/dynamic/darpa_hyq_pathKino.py
@@ -41,16 +41,16 @@ r = Viewer (ps)
 
 from hpp.corbaserver.affordance.affordance import AffordanceTool
 afftool = AffordanceTool ()
-afftool.loadObstacleModel (packageName, "ground", "planning", r)
+afftool.loadObstacleModel (packageName, "darpa", "planning", 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 [0:3] = [-5, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
+q_init [0:3] = [-2, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
 q_goal = q_init [::]
-q_goal [0:3] = [4, 0, 0.63]
+q_goal [0:3] = [3, 0, 0.63]
 #q_goal[0:3]=[3,-4,0.4]#position easy
 q_goal[7:10]=[0,0,0]#velocity
 r (q_goal)
@@ -70,8 +70,8 @@ ps.selectPathPlanner("DynamicPlanner")
 
 #solve the problem :
 r(q_init)
-ps.client.problem.prepareSolveStepByStep()
-ps.client.problem.executeOneStep()
+#ps.client.problem.prepareSolveStepByStep()
+#ps.client.problem.executeOneStep()
 
 ps.solve ()
 
@@ -82,13 +82,18 @@ pp = PathPlayer (rbprmBuilder.client.basic, r)
 pp.dt=0.03
 #r.client.gui.removeFromGroup("rm0",r.sceneName)
 pp.displayVelocityPath(0)
+r.client.gui.setVisibility("path_0_root","ALWAYS_ON_TOP")
 #display path
-#pp (0)
+pp.speed=0.3
+pp (0)
+
 #display path with post-optimisation
-#r.client.gui.removeFromGroup("path_0_root",r.sceneName)
-#pp.displayVelocityPath(1)
-#pp (1)
+"""
+r.client.gui.removeFromGroup("path_0_root",r.sceneName)
+pp.displayVelocityPath(1)
+pp (1)
 
+"""
 q_far = q_init[::]
 q_far[2] = -3
 r(q_far)
@@ -100,3 +105,23 @@ for i in range(1,10):
     #time.sleep(2)
 """
 
+
+
+"""
+
+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)
+
+
+
+
+
+
+"""
+
diff --git a/script/dynamic/darpa_hyq_interpKino.py b/script/dynamic/flatGround_hyq_interpKino.py
similarity index 89%
rename from script/dynamic/darpa_hyq_interpKino.py
rename to script/dynamic/flatGround_hyq_interpKino.py
index bd3b1ed1a052eb946b865584392cab134c45fa2c..06a9b83d9b328e1632b658a34eda3d9b08abd3f1 100644
--- a/script/dynamic/darpa_hyq_interpKino.py
+++ b/script/dynamic/flatGround_hyq_interpKino.py
@@ -5,7 +5,7 @@ from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
 from hpp.gepetto import Viewer
 
 #calling script darpa_hyq_path to compute root path
-import darpa_hyq_pathKino as tp
+import flatGround_hyq_pathKino as tp
 
 from os import environ
 ins_dir = environ['DEVEL_DIR']
@@ -54,14 +54,19 @@ addLimbDb(larmId, "static")
 q_0 = fullBody.getCurrentConfig(); 
 q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.ps.configAtParam(0,0.01)[0:7] # use this to get the correct orientation
 q_goal = fullBody.getCurrentConfig(); q_goal[0:7] = tp.ps.configAtParam(0,tp.ps.pathLength(1))[0:7]
+dir_init = tp.ps.configAtParam(0,0.01)[7:10]
+acc_init = tp.ps.configAtParam(0,0.01)[10:13]
+dir_goal = tp.ps.configAtParam(0,tp.ps.pathLength(1))[7:10]
+acc_goal = tp.ps.configAtParam(0,tp.ps.pathLength(1))[10:13]
+
 
 # Randomly generating a contact configuration at q_init
 fullBody.setCurrentConfig (q_init)
-q_init = fullBody.generateContacts(q_init, [0,0,1])
+q_init = fullBody.generateContacts(q_init,dir_init,acc_init)
 
 # Randomly generating a contact configuration at q_end
 fullBody.setCurrentConfig (q_goal)
-q_goal = fullBody.generateContacts(q_goal, [0,0,1])
+q_goal = fullBody.generateContacts(q_goal, dir_goal,acc_goal)
 
 # specifying the full body configurations as start and goal state of the problem
 fullBody.setStartState(q_init,[])
@@ -71,7 +76,7 @@ fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
 r(q_init)
 # computing the contact sequence
 # configs = fullBody.interpolate(0.12, 10, 10, True) #Was this (Pierre)
-configs = fullBody.interpolate(0.5,pathId=0)
+configs = fullBody.interpolate(0.05,pathId=0)
 #~ configs = fullBody.interpolate(0.11, 7, 10, True)
 #~ configs = fullBody.interpolate(0.1, 1, 5, True)
 
diff --git a/script/dynamic/flatGround_hyq_pathKino.py b/script/dynamic/flatGround_hyq_pathKino.py
new file mode 100644
index 0000000000000000000000000000000000000000..5cc61175107362e6031025d257ef5230445b71b2
--- /dev/null
+++ b/script/dynamic/flatGround_hyq_pathKino.py
@@ -0,0 +1,127 @@
+# 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
+
+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 = ""
+vMax = 2;
+aMax = 1;
+# Creating an instance of the helper class, and loading the robot
+rbprmBuilder = Builder ()
+rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+rbprmBuilder.setJointBounds ("base_joint_xyz", [-5,5, -1.5, 1.5, 0.5, 0.8])
+# The following lines set constraint on the valid configurations:
+# a configuration is valid only if all limbs can create a contact ...
+rbprmBuilder.setFilter(['hyq_rhleg_rom', 'hyq_lfleg_rom', 'hyq_rfleg_rom','hyq_lhleg_rom'])
+rbprmBuilder.setAffordanceFilter('hyq_rhleg_rom', ['Support'])
+rbprmBuilder.setAffordanceFilter('hyq_rfleg_rom', ['Support',])
+rbprmBuilder.setAffordanceFilter('hyq_lhleg_rom', ['Support'])
+rbprmBuilder.setAffordanceFilter('hyq_lfleg_rom', ['Support',])
+# We also bound the rotations of the torso.
+rbprmBuilder.boundSO3([-0.4,0.4,-3,3,-3,3])
+rbprmBuilder.client.basic.robot.setDimensionExtraConfigSpace(2*3)
+rbprmBuilder.client.basic.robot.setExtraConfigSpaceBounds([-vMax,vMax,-vMax,vMax,0,0,0,0,0,0,0,0])
+
+# 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",aMax)
+ps.client.problem.setParameter("vMax",vMax)
+r = Viewer (ps)
+
+from hpp.corbaserver.affordance.affordance import AffordanceTool
+afftool = AffordanceTool ()
+afftool.loadObstacleModel (packageName, "ground", "planning", 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 [0:3] = [-2, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
+q_goal = q_init [::]
+
+q_goal[0:3]=[2,0,0.63]#position easy
+#q_goal [0:3] = [4, 0, 0.71]
+#q_goal[7:10]=[2,0,0.9]#velocity
+q_goal[7:10]=[1,0,0]#velocity
+r (q_goal)
+#~ q_goal [0:3] = [-1.5, 0, 0.63]; r (q_goal)
+
+# Choosing a path optimizer
+ps.addPathOptimizer ("RandomShortcutOriented")
+ps.setInitialConfig (q_init)
+ps.addGoalConfig (q_goal)
+# Choosing RBPRM shooter and path validation methods.
+ps.client.problem.selectConFigurationShooter("RbprmShooter")
+ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
+# Choosing kinodynamic methods : 
+ps.selectSteeringMethod("RBPRMKinodynamic")
+ps.selectDistance("KinodynamicDistance")
+ps.selectPathPlanner("DynamicPlanner")
+
+#solve the problem :
+r(q_init)
+#ps.client.problem.prepareSolveStepByStep()
+#ps.client.problem.executeOneStep()
+
+ps.solve ()
+
+
+# Playing the computed path
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (rbprmBuilder.client.basic, r)
+pp.dt=0.03
+#r.client.gui.removeFromGroup("rm0",r.sceneName)
+pp.displayVelocityPath(0)
+#display path
+#pp (0)
+
+#display path with post-optimisation
+"""
+r.client.gui.removeFromGroup("path_0_root",r.sceneName)
+pp.displayVelocityPath(1)
+pp (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)
+"""
+
+
+
+"""
+
+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)
+
+
+
+
+
+
+"""
+