From ed054fb454decbef23c3c8fba96498a4b919c2da Mon Sep 17 00:00:00 2001
From: Pierre Fernbach <pierre.fernbach@laas.fr>
Date: Sat, 16 Jun 2018 16:54:37 +0200
Subject: [PATCH] [script] add/update lot of scenario scripts

---
 script/dynamic/darpa_hrp2_interStatic.py      |   4 +-
 .../darpa_hrp2_interStatic_testTransition.py  | 269 ++++++++++++++++++
 script/dynamic/darpa_hrp2_path.py             |   6 +-
 script/dynamic/darpa_line_hrp2_interStatic.py |  59 ++--
 script/dynamic/darpa_line_hrp2_path.py        |  22 +-
 script/dynamic/downSlope_hrp2_loadPath.py     | 171 +++++++++++
 .../dynamic/downSlope_hrp2_pathKino_oneTry.py | 231 +++++++++++++++
 script/dynamic/export_blender.py              |  44 ++-
 ...Ground_hrp2_interpSTATIC_testTransition.py |  82 ++++--
 script/dynamic/flatGround_hrp2_pathKino.py    |  13 +-
 script/dynamic/fullBodyPlayerHyQ.py           | 107 +++++++
 script/dynamic/hrp2_wall_path_visual.py       | 129 +++++++++
 script/dynamic/hyq_jumpLow_path.py            |  99 +++++++
 .../plateform_hrp2_interp_testTransition.py   |  14 +-
 ...eform_hrp2_interp_testTransition_muscod.py | 267 +++++++++++++++++
 script/dynamic/qs.py                          |  55 ++++
 script/dynamic/sarpa_hrp2_interStatic.py      | 198 +++++++++++++
 .../sideWall_hyq_interpKino_fullBody.py       | 190 +++++++++++++
 .../dynamic/sideWall_hyq_interpKino_muscod.py | 166 +++++++++++
 .../dynamic/slalom_bauzil_hrp2_interStatic.py | 193 +++++++++++++
 script/dynamic/slalom_bauzil_hrp2_pathKino.py | 158 ++++++++++
 script/dynamic/slalom_hyq_interpKino05.py     | 140 +++++++++
 script/dynamic/slalom_hyq_pathKino05.py       | 180 ++++++++++++
 script/dynamic/stair_bauzil_hrp2_interp.py    | 108 +++----
 script/dynamic/stair_bauzil_hrp2_path.py      |   8 +-
 script/dynamic/stairs10_hrp2_interStatic.py   |  24 +-
 .../stairs10_hrp2_interStatic_noCroc.py       | 192 +++++++++++++
 script/dynamic/stairs10_hrp2_pathKino.py      |   5 +-
 script/dynamic/test_flatGround.py             | 130 +++++++++
 script/dynamic/test_kinematics_muscod.py      |  59 ++++
 30 files changed, 3168 insertions(+), 155 deletions(-)
 create mode 100644 script/dynamic/darpa_hrp2_interStatic_testTransition.py
 create mode 100644 script/dynamic/downSlope_hrp2_loadPath.py
 create mode 100644 script/dynamic/downSlope_hrp2_pathKino_oneTry.py
 create mode 100644 script/dynamic/fullBodyPlayerHyQ.py
 create mode 100644 script/dynamic/hrp2_wall_path_visual.py
 create mode 100644 script/dynamic/hyq_jumpLow_path.py
 create mode 100644 script/dynamic/plateform_hrp2_interp_testTransition_muscod.py
 create mode 100644 script/dynamic/qs.py
 create mode 100644 script/dynamic/sarpa_hrp2_interStatic.py
 create mode 100644 script/dynamic/sideWall_hyq_interpKino_fullBody.py
 create mode 100644 script/dynamic/sideWall_hyq_interpKino_muscod.py
 create mode 100644 script/dynamic/slalom_bauzil_hrp2_interStatic.py
 create mode 100644 script/dynamic/slalom_bauzil_hrp2_pathKino.py
 create mode 100644 script/dynamic/slalom_hyq_interpKino05.py
 create mode 100644 script/dynamic/slalom_hyq_pathKino05.py
 create mode 100644 script/dynamic/stairs10_hrp2_interStatic_noCroc.py
 create mode 100644 script/dynamic/test_flatGround.py
 create mode 100644 script/dynamic/test_kinematics_muscod.py

diff --git a/script/dynamic/darpa_hrp2_interStatic.py b/script/dynamic/darpa_hrp2_interStatic.py
index 8024355..abf0630 100644
--- a/script/dynamic/darpa_hrp2_interStatic.py
+++ b/script/dynamic/darpa_hrp2_interStatic.py
@@ -47,7 +47,7 @@ rLegLimbOffset=[0,0,-0.035]#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, 100000, "fixedStep06", 0.01,"_6_DOF",limbOffset=rLegLimbOffset,kinematicConstraintsMin=0.3)
+fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 100000, "fixedStep06", 0.01,"_6_DOF",limbOffset=rLegLimbOffset,kinematicConstraintsMin=0.5)
 fullBody.runLimbSampleAnalysis(rLegId, "ReferenceConfiguration", True)
 #fullBody.saveLimbDatabase(rLegId, "./db/hrp2_rleg_db.db")
 
@@ -57,7 +57,7 @@ lLegLimbOffset=[0,0,0.035]
 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, 100000, "fixedStep06", 0.01,"_6_DOF",limbOffset=lLegLimbOffset,kinematicConstraintsMin=0.3)
+fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 100000, "fixedStep06", 0.01,"_6_DOF",limbOffset=lLegLimbOffset,kinematicConstraintsMin=0.5)
 fullBody.runLimbSampleAnalysis(lLegId, "ReferenceConfiguration", True)
 #fullBody.saveLimbDatabase(lLegId, "./db/hrp2_lleg_db.db")
 
diff --git a/script/dynamic/darpa_hrp2_interStatic_testTransition.py b/script/dynamic/darpa_hrp2_interStatic_testTransition.py
new file mode 100644
index 0000000..63e219f
--- /dev/null
+++ b/script/dynamic/darpa_hrp2_interStatic_testTransition.py
@@ -0,0 +1,269 @@
+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.gepetto import Viewer
+from tools import *
+import darpa_hrp2_path as tp
+import time
+import omniORB.any
+import numpy as np
+from numpy import linalg as LA
+from hpp.corbaserver.rbprm.rbprmstate import State,StateHelper
+
+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",  [-1.2,1.5,-0.05 ,0.05, 0.55, 0.85])
+fullBody.client.basic.robot.setDimensionExtraConfigSpace(tp.extraDof)
+fullBody.client.basic.robot.setExtraConfigSpaceBounds([-0,0,-0,0,-0,0,0,0,0,0,0,0])
+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.035]#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, 100000, "fixedStep08", 0.01,"_6_DOF",limbOffset=rLegLimbOffset,kinematicConstraintsPath = "package://hpp-rbprm-corba/com_inequalities/empty_com_constraints.obj")
+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.035]
+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, 100000, "fixedStep08", 0.01,"_6_DOF",limbOffset=lLegLimbOffset,kinematicConstraintsPath = "package://hpp-rbprm-corba/com_inequalities/empty_com_constraints.obj")
+fullBody.runLimbSampleAnalysis(lLegId, "ReferenceConfiguration", True)
+#fullBody.saveLimbDatabase(lLegId, "./db/hrp2_lleg_db.db")
+
+## Add arms (not used for contact) : 
+
+
+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]
+
+
+
+
+
+# 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, testReachability=True, quasiStatic=True)
+tInterpolate = time.time()-tStart
+print "number of configs : ", len(configs)
+print "generated in "+str(tInterpolate)+" s"
+r(configs[len(configs)-2])
+
+
+
+
+f = open("/local/fernbac/bench_iros18/kin_constraint_tog/without_kin_constraints.log","a")
+
+global num_transition
+global valid_transition
+global length_traj
+global valid_length
+global dt
+num_transition = 0.
+valid_transition = 0.
+dt = 0.001
+valid_length = 0.
+length_traj = 0.
+
+
+def check_traj(s,c0,c1,reverse=False):
+  global length_traj
+  global valid_length
+  global dt  
+  dist =  LA.norm(c1-c0)
+  if dist < dt :
+    return True
+  length_traj += dist
+  current_dist = 0.
+  dtu = dt/dist  
+  successProj_total = True
+  if not reverse :
+    u = 0.
+    while u < 1.:
+      c = c0*(1.-u) + c1*u
+      successProj = s.projectToCOM(c.transpose().tolist(),20) 
+      current_dist += dt
+      if successProj :
+        valid_length += dt
+      else :
+        print "projection failed."
+        successProj_total = False
+      u += dtu
+      
+    successProj = s.projectToCOM(c1.transpose().tolist(),20) 
+    if successProj :
+      valid_length += dist - current_dist
+    else :
+      print "projection failed."
+      successProj_total = False  
+  else :
+    u = 1.
+    while u > 0.:
+      c = c0*(1.-u) + c1*u
+      successProj = s.projectToCOM(c.transpose().tolist(),20) 
+      current_dist += dt
+      if successProj :
+        valid_length += dt
+      else :
+        print "projection failed."
+        successProj_total = False
+      u -= dtu
+      
+    successProj = s.projectToCOM(c0.transpose().tolist(),20) 
+    if successProj :
+      valid_length += dist - current_dist
+    else :
+      print "projection failed."
+      successProj_total = False 
+  
+  return successProj_total
+
+def check_kin_feasibility(s0Id,s1Id):
+  res = fullBody.isReachableFromState(s0Id,s1Id,True)
+  if not res[0]:
+    print "Error : isReachable returned False !"
+    raise Exception('Error : isReachable returned False !') 
+  r(configs[s0Id])
+  c0 = np.array(fullBody.getCenterOfMass())
+  c1 = np.array(res[1])
+  c2 = np.array(res[2])
+  r(configs[s1Id])
+  c3 = np.array(fullBody.getCenterOfMass())
+  s0 = State(fullBody,sId = s0Id)
+  s1 = State(fullBody,sId = s1Id)  
+  s0_orig = State(s0.fullBody,q=s0.q(),limbsIncontact=s0.getLimbsInContact())
+  s1_orig = State(s1.fullBody,q=s1.q(),limbsIncontact=s1.getLimbsInContact())  
+  moving_limb = s0.contactsVariations(s1)
+  smid,successMid = StateHelper.removeContact(s0_orig,moving_limb[0])
+  if not successMid :
+    return False
+  
+  successFeasible = True
+  print "check first part"
+  successFeasible = successFeasible and check_traj(s0_orig,c0,c1)
+  print "check mid part"
+  successFeasible = successFeasible and check_traj(smid,c1,c2)
+  print "check last part"
+  successFeasible = successFeasible and check_traj(s1_orig,c2,c3,True)
+  return successFeasible
+  
+
+for i in range(len(configs)-2):
+  print "check traj between state "+str(i)+" and "+str(i+1)
+  success = check_kin_feasibility(i,i+1)
+  num_transition +=1.
+  if success :
+    valid_transition +=1.
+  
+
+
+f.write("num_transition           "+str(num_transition)+"\n")
+f.write("valid_transition         "+str(valid_transition)+"\n")
+if num_transition > 0:
+  f.write("valid_transition_percent "+str(float(valid_transition/num_transition)*100.)+" %\n")
+f.write("traj_length              "+str(length_traj)+"\n")
+f.write("valid_length             "+str(valid_length)+"\n")
+if length_traj > 0 :
+  f.write("valid_traj_percent       "+str(float(valid_length/length_traj)*100.)+" %\n")
+
+f.close()
+
+
+
+#wid = r.client.gui.getWindowID("window_hpp_")
+#r.client.gui.attachCameraToNode( 'hrp2_14/BODY_0',wid)
+
+
+
+
+
diff --git a/script/dynamic/darpa_hrp2_path.py b/script/dynamic/darpa_hrp2_path.py
index 0783bc2..832dc0e 100644
--- a/script/dynamic/darpa_hrp2_path.py
+++ b/script/dynamic/darpa_hrp2_path.py
@@ -8,7 +8,7 @@ import math
 import omniORB.any
 
 
-
+from configs.darpa import * 
 from hpp.corbaserver import Client
 from hpp.corbaserver.robot import Robot as Parent
 from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
@@ -28,7 +28,7 @@ class Robot (Parent):
 		self.load = load
 		
 
-MU=0.5
+
 rootJointType = 'freeflyer'
 packageName = 'hpp-rbprm-corba'
 meshPackageName = 'hpp-rbprm-corba'
@@ -91,7 +91,7 @@ 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.8, 0, 0.53]; r (q_init)
+q_init [0:3] = [-0.95, 0, 0.53]; r (q_init)
 #q_init [0:3] = [-0.5, 0, 0.75]; r (q_init)
 
 rbprmBuilder.setCurrentConfig (q_init)
diff --git a/script/dynamic/darpa_line_hrp2_interStatic.py b/script/dynamic/darpa_line_hrp2_interStatic.py
index 9f8b428..f1c820e 100644
--- a/script/dynamic/darpa_line_hrp2_interStatic.py
+++ b/script/dynamic/darpa_line_hrp2_interStatic.py
@@ -13,15 +13,16 @@ rootJointType = "freeflyer"
 ##
 #  Information to retrieve urdf and srdf files.
 urdfName = "hrp2_14"
-urdfSuffix = "_reduced"
+urdfSuffix = "_reduced_safe"
+#srdfSuffix = "_disable_leg_autocol"
 srdfSuffix = ""
 pId = tp.ps.numberPaths() -1
 fullBody = FullBody ()
 
 fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
-fullBody.setJointBounds ("base_joint_xyz",  [-1.2,1.5,-0.05 ,0.05, 0.55, 0.85])
+fullBody.setJointBounds ("base_joint_xyz",  [-1.2,1.5,-0.2 ,0.2, 0.55, 1.])
 fullBody.client.basic.robot.setDimensionExtraConfigSpace(tp.extraDof)
-
+fullBody.client.basic.robot.setExtraConfigSpaceBounds([0,0,0,0,0,0,0,0,0,0,0,0])
 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))
@@ -33,6 +34,15 @@ q_ref = q_init[::]
 fullBody.setCurrentConfig (q_init)
 fullBody.setReferenceConfig (q_ref)
 
+view = [0.4,
+ -0.,
+ 5.5,
+ 0.7071067811865475,
+ 0.,
+ 0.,
+ -0.7071067811865475]
+r.client.gui.setCameraTransform(0,view)
+
 
 
 #~ AFTER loading obstacles
@@ -42,22 +52,23 @@ tStart = time.time()
 
 
 rLeg = 'RLEG_JOINT0'
-rLegOffset = [0,0,-0.105]
+rLegOffset = [0,0,-0.0955]
 rLegLimbOffset=[0,0,-0.035]#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, 100000, "dynamicWalk", 0.01,"_6_DOF",limbOffset=rLegLimbOffset)
+#fixedStep08
+fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 100000, "fixedStep1", 0.01,"_6_DOF",limbOffset=rLegLimbOffset,kinematicConstraintsPath = "package://hpp-rbprm-corba/com_inequalities/fullSize/RLEG_JOINT0_com_constraints.obj",kinematicConstraintsMin=0.)
 fullBody.runLimbSampleAnalysis(rLegId, "ReferenceConfiguration", True)
 #fullBody.saveLimbDatabase(rLegId, "./db/hrp2_rleg_db.db")
 
 lLeg = 'LLEG_JOINT0'
-lLegOffset = [0,0,-0.105]
+lLegOffset = [0,0,-0.0955]
 lLegLimbOffset=[0,0,0.035]
 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, 100000, "dynamicWalk", 0.01,"_6_DOF",limbOffset=lLegLimbOffset)
+fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 100000, "fixedStep1", 0.01,"_6_DOF",limbOffset=lLegLimbOffset,kinematicConstraintsPath = "package://hpp-rbprm-corba/com_inequalities/fullSize/LLEG_JOINT0_com_constraints.obj",kinematicConstraintsMin=0.)
 fullBody.runLimbSampleAnalysis(lLegId, "ReferenceConfiguration", True)
 #fullBody.saveLimbDatabase(lLegId, "./db/hrp2_lleg_db.db")
 
@@ -91,7 +102,7 @@ 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
+robTreshold = 0
 # copy extraconfig for start and init configurations
 q_init[configSize:configSize+3] = dir_init[::]
 q_init[configSize+3:configSize+6] = acc_init[::]
@@ -99,10 +110,13 @@ q_goal[configSize:configSize+3] = dir_goal[::]
 q_goal[configSize+3:configSize+6] = [0,0,0]
 
 
-q_init[2] = q_init[2]+0.02
-q_goal[2] = q_goal[2]+0.02
+#q_init[2] = q_init[2] + 0.02
+#q_goal[2] = q_goal[2] + 0.02
 
+q_init[2] = 0.86
+q_goal[2] = 0.86
 
+"""
 # Randomly generating a contact configuration at q_init
 fullBody.setStaticStability(True)
 fullBody.setCurrentConfig (q_init)
@@ -114,13 +128,14 @@ r(q_init)
 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.addLandmark('hrp2_14/BODY',0.3)
 r(q_init)
 
 
-fullBody.setStartState(q_init,[rLegId,lLegId])
+fullBody.setStartState(q_init,[lLegId,rLegId])
 fullBody.setEndState(q_goal,[rLegId,lLegId])
 fullBody.setStaticStability(True) # only set it after the init/goal configuration are computed
 
@@ -132,26 +147,26 @@ pp = PathPlayer (fullBody.client.basic, r)
 import fullBodyPlayerHrp2
 
 tStart = time.time()
-configs = fullBody.interpolate(0.1,pathId=pId,robustnessTreshold = robTreshold, filterStates = True)
-tInterpolate = time.time()-tStart
+configs = fullBody.interpolate(0.01,pathId=pId,robustnessTreshold = robTreshold, filterStates = True,testReachability=False,quasiStatic=False)
+tInterpolateConfigs = time.time()-tStart
 print "number of configs : ", len(configs)
-print "generated in "+str(tInterpolate)+" s"
-r(configs[len(configs)-2])
+print "generated in "+str(tInterpolateConfigs)+" s"
+r(configs[-1])
 
 
-player = fullBodyPlayerHrp2.Player(fullBody,pp,tp,configs,draw=False,use_window=1,optim_effector=True,use_velocity=False,pathId = pId)
+#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()
 
-beginState=2
-endState=4
+beginState=0
+endState=len(configs)-1
 
-configs=configs[beginState:endState+1]
+#configs=configs[beginState:endState+1]
 
-from planning.config import *
+from configs.darpa import *
 from generate_contact_sequence import *
 cs = generateContactSequence(fullBody,configs,beginState,endState,r)
 filename = OUTPUT_DIR + "/" + OUTPUT_SEQUENCE_FILE
@@ -180,7 +195,7 @@ addVector(r,fullBody,r.color.red,vlb)
 """
 
 
-wid = r.client.gui.getWindowID("window_hpp_")
+#wid = r.client.gui.getWindowID("window_hpp_")
 #r.client.gui.attachCameraToNode( 'hrp2_14/BODY_0',wid)
 
 
diff --git a/script/dynamic/darpa_line_hrp2_path.py b/script/dynamic/darpa_line_hrp2_path.py
index 0c670b9..211b311 100644
--- a/script/dynamic/darpa_line_hrp2_path.py
+++ b/script/dynamic/darpa_line_hrp2_path.py
@@ -6,7 +6,7 @@ from hpp.gepetto import Viewer
 import time
 import math
 import omniORB.any
-from planning.config import *
+from configs.darpa import *
 
 from hpp.corbaserver import Client
 from hpp.corbaserver.robot import Robot as Parent
@@ -50,11 +50,11 @@ rbprmBuilder.setFilter(['hrp2_lleg_rom','hrp2_rleg_rom'])
 #rbprmBuilder.setAffordanceFilter('hrp2_larm_rom', ['Lean'])
 #rbprmBuilder.setAffordanceFilter('hrp2_lleg_rom', ['Support'])
 #rbprmBuilder.setAffordanceFilter('hrp2_rleg_rom', ['Support'])
-vMax = 0.2;
-aMax = 0.1;
+vMax = 0.1;
+aMax = 0.05;
 extraDof = 6
 
-rbprmBuilder.setJointBounds ("base_joint_xyz", [-1.2,1.5,-0.1 ,0.1, 0.55, 0.85])
+rbprmBuilder.setJointBounds ("base_joint_xyz", [-1.2,1.5,-0.1 ,0.1, 0.55, 1.])
 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)
@@ -76,7 +76,7 @@ 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)
+r = Viewer (ps,displayArrows = False,displayCoM=True)
 
 
 from hpp.corbaserver.affordance.affordance import AffordanceTool
@@ -85,19 +85,21 @@ afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.005])
 afftool.loadObstacleModel (packageName, ENV_NAME, ENV_PREFIX, r,reduceSizes=[0.14,0])
 #r.loadObstacleModel (packageName, "ground", "planning")
 afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
-r.addLandmark(r.sceneName,1)
+#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.37, 0, 0.74]; r (q_init)
-#q_init [0:3] = [-0.5, 0, 0.75]; r (q_init)
+#q_init [0:3] =[-0.37, 0, 0.74]; r (q_init)
+q_init [0:3] =[-0.71, -0.03, 0.76]; r (q_init)
+
 
 rbprmBuilder.setCurrentConfig (q_init)
 q_goal = q_init [::]
 
-q_goal [0:3] =[0.85, 0, 0.74]; r(q_goal)
-#q_goal [0:3] = [1, 0, 0.75]; r(q_goal)
+q_goal [0:3] =[1.12, -0.03, 0.76]; r(q_goal)
+#q_goal [0:3] =[0.85, 0, 0.74]; r(q_goal)
+
 r (q_goal)
 
 
diff --git a/script/dynamic/downSlope_hrp2_loadPath.py b/script/dynamic/downSlope_hrp2_loadPath.py
new file mode 100644
index 0000000..04d0a3e
--- /dev/null
+++ b/script/dynamic/downSlope_hrp2_loadPath.py
@@ -0,0 +1,171 @@
+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.gepetto import Viewer
+from hpp.corbaserver import Client
+from hpp.corbaserver.robot import Robot as Parent
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+import omniORB.any
+
+
+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_flexible'
+urdfNameRom =  ['hrp2_larm_rom','hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom']
+urdfSuffix = ""
+srdfSuffix = ""
+vMax = 4;
+aMax = 6;
+extraDof = 6
+
+# 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", [-1.25,2, -0.5, 5.5, 0.6, 1.8])
+rbprmBuilder.setJointBounds ("base_joint_xyz", [-2,4, 0, 2, 0.2, 1.8])
+rbprmBuilder.setJointBounds('CHEST_JOINT0',[0,0])
+rbprmBuilder.setJointBounds('CHEST_JOINT1',[-0.35,0.1])
+rbprmBuilder.setJointBounds('HEAD_JOINT0',[0,0])
+rbprmBuilder.setJointBounds('HEAD_JOINT1',[0,0])
+
+# 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_lleg_rom', ['Support',])
+rbprmBuilder.setAffordanceFilter('hrp2_rleg_rom', ['Support'])
+# We also bound the rotations of the torso. (z, y, x)
+rbprmBuilder.boundSO3([-0.1,0.1,-0.65,0.65,-0.2,0.2])
+rbprmBuilder.client.basic.robot.setDimensionExtraConfigSpace(extraDof)
+rbprmBuilder.client.basic.robot.setExtraConfigSpaceBounds([-4,4,-1,1,-2,2,0,0,0,0,0,0])
+indexECS = rbprmBuilder.getConfigSize() - rbprmBuilder.client.basic.robot.getDimensionExtraConfigSpace()
+
+# Creating an instance of HPP problem solver and the viewer
+
+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("sizeFootX",omniORB.any.to_any(0.24))
+ps.client.problem.setParameter("sizeFootY",omniORB.any.to_any(0.14))
+r = Viewer (ps)
+
+from hpp.corbaserver.affordance.affordance import AffordanceTool
+afftool = AffordanceTool ()
+afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
+afftool.loadObstacleModel (packageName, "downSlope", "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[3:7] = [1,0,0,0]
+q_init[8] = -0.2
+q_init [0:3] = [-1.6, 1, 1.75]; r (q_init)
+
+#q_init[3:7] = [0.7071,0,0,0.7071]
+#q_init [0:3] = [1, 1, 0.65]
+
+rbprmBuilder.setCurrentConfig (q_init)
+q_goal = q_init [::]
+
+
+q_goal[3:7] = [1,0,0,0]
+q_goal[8] = 0
+q_goal [0:3] = [3, 1, 0.55]; r (q_goal)
+
+r (q_goal)
+#~ q_goal [0:3] = [-1.5, 0, 0.63]; 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("RbprmPathValidation",0.05)
+# Choosing kinodynamic methods : 
+ps.selectSteeringMethod("RBPRMKinodynamic")
+ps.selectDistance("KinodynamicDistance")
+ps.selectPathPlanner("DynamicPlanner")
+
+#solve the problem :
+r(q_init)
+
+
+#r.solveAndDisplay("rm",1,0.01)
+
+
+#ps.solve()
+
+#ps.client.problem.prepareSolveStepByStep()
+
+#ps.client.problem.finishSolveStepByStep()
+
+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)
+"""
+
+"""
+r.client.gui.removeFromGroup("rm",r.sceneName)
+r.client.gui.removeFromGroup("rmstart",r.sceneName)
+r.client.gui.removeFromGroup("rmgoal",r.sceneName)
+for i in range(0,ps.numberNodes()):
+  r.client.gui.removeFromGroup("vecRM"+str(i),r.sceneName)
+
+"""
+
+# for seed 1486657707
+#ps.client.problem.extractPath(0,0,2.15) 
+
+# Playing the computed path
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (rbprmBuilder.client.basic, r)
+pp.dt=0.03
+pp.displayVelocityPath(1)
+r.client.gui.setVisibility("path_1_root","ALWAYS_ON_TOP")
+#display path
+pp.speed=0.3
+#pp (0)
+
+#display path with post-optimisation
+
+
+
+
+"""
+q_far = q_init[::]
+q_far[2] = -3
+r(q_far)
+"""
+
+
+
+
diff --git a/script/dynamic/downSlope_hrp2_pathKino_oneTry.py b/script/dynamic/downSlope_hrp2_pathKino_oneTry.py
new file mode 100644
index 0000000..7a82334
--- /dev/null
+++ b/script/dynamic/downSlope_hrp2_pathKino_oneTry.py
@@ -0,0 +1,231 @@
+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.gepetto import Viewer
+from hpp.corbaserver import Client
+from hpp.corbaserver.robot import Robot as Parent
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+import omniORB.any
+
+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_flexible'
+urdfNameRom =  ['hrp2_larm_rom','hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom']
+urdfSuffix = ""
+srdfSuffix = ""
+vMax = 4.5;
+aMax = 6.;
+extraDof = 6
+
+# 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", [-1.25,2, -0.5, 5.5, 0.6, 1.8])
+rbprmBuilder.setJointBounds ("base_joint_xyz", [-1.7,2.75, 0.95, 1.05, 0.1, 1.8])
+rbprmBuilder.setJointBounds('CHEST_JOINT0',[0,0])
+rbprmBuilder.setJointBounds('CHEST_JOINT1',[-0.35,0.1])
+rbprmBuilder.setJointBounds('HEAD_JOINT0',[0,0])
+rbprmBuilder.setJointBounds('HEAD_JOINT1',[0,0])
+
+# 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_lleg_rom', ['Support',])
+rbprmBuilder.setAffordanceFilter('hrp2_rleg_rom', ['Support'])
+# We also bound the rotations of the torso. (z, y, x)
+rbprmBuilder.boundSO3([-0.001,0.001,-0.001,0.001,-0.001,0.001])
+rbprmBuilder.client.basic.robot.setDimensionExtraConfigSpace(extraDof)
+rbprmBuilder.client.basic.robot.setExtraConfigSpaceBounds([-4.5,4.5,0,0,-2,2,0,0,0,0,0,0])
+indexECS = rbprmBuilder.getConfigSize() - rbprmBuilder.client.basic.robot.getDimensionExtraConfigSpace()
+
+# Creating an instance of HPP problem solver and the viewer
+
+ps = ProblemSolver( rbprmBuilder )
+ps.client.problem.setParameter("aMax",omniORB.any.to_any(aMax))
+ps.client.problem.setParameter("aMaxZ",omniORB.any.to_any(10.))
+ps.client.problem.setParameter("vMax",omniORB.any.to_any(vMax))
+ps.client.problem.setParameter("tryJump",omniORB.any.to_any(1.))
+ps.client.problem.setParameter("sizeFootX",omniORB.any.to_any(0.24))
+ps.client.problem.setParameter("sizeFootY",omniORB.any.to_any(0.14))
+ps.client.problem.setTimeOutPathPlanning(3600)
+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, "downSlope", "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[3:7] = [1,0,0,0]
+q_init[8] = -0.2
+q_init [0:3] = [-1.6, 1, 1.75]; r (q_init)
+
+#q_init[3:7] = [0.7071,0,0,0.7071]
+#q_init [0:3] = [1, 1, 0.65]
+
+rbprmBuilder.setCurrentConfig (q_init)
+q_goal = q_init [::]
+
+
+q_goal[3:7] = [1,0,0,0]
+q_goal[8] = 0
+q_goal [0:3] = [2.5, 1, 0.5]; r (q_goal)
+
+r (q_goal)
+#~ q_goal [0:3] = [-1.5, 0, 0.63]; 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)
+ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
+# Choosing kinodynamic methods : 
+ps.selectSteeringMethod("RBPRMKinodynamic")
+ps.selectDistance("KinodynamicDistance")
+ps.selectPathPlanner("DynamicPlanner")
+#ps.addPathOptimizer("RandomShortcutDynamic")
+
+#solve the problem :
+r(q_init)
+
+
+#r.solveAndDisplay("rm",1,0.01)
+
+#t = ps.solve ()
+
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (rbprmBuilder.client.basic, r)
+pp.dt=0.03
+pp.displayVelocityPath(0)
+r.client.gui.setVisibility("path_0_root","ALWAYS_ON_TOP")
+
+
+import parse_bench
+
+parse_bench.parseBenchmark(t)
+print "path lenght = ",str(ps.client.problem.pathLength(ps.numberPaths()-1))
+
+
+
+
+"""
+if isinstance(t, list):
+	t = t[0]* 3600000 + t[1] * 60000 + t[2] * 1000 + t[3]
+f = open('log.txt', 'a')
+f.write("path computation " + str(t) + "\n")
+f.close()
+"""
+
+"""
+for i in range(0,9):
+  t = ps.solve()
+  if isinstance(t, list):
+    ts = t[0]* 3600. + t[1] * 60. + t[2] + t[3]/1000.	
+  f= open("/local/dev_hpp/logs/benchHrp2_slope_LP.txt","a")
+  f.write("t = "+str(ts) + "\n")
+  f.write("path_length = "+str(ps.client.problem.pathLength(i)) + "\n")
+  f.close()
+  print "problem "+str(i)+" solved \n"
+  ps.clearRoadmap()
+"""
+#ps.client.problem.prepareSolveStepByStep()
+
+#ps.client.problem.finishSolveStepByStep()
+
+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)
+"""
+
+"""
+r.client.gui.removeFromGroup("rm",r.sceneName)
+r.client.gui.removeFromGroup("rmstart",r.sceneName)
+r.client.gui.removeFromGroup("rmgoal",r.sceneName)
+for i in range(0,ps.numberNodes()):
+  r.client.gui.removeFromGroup("vecRM"+str(i),r.sceneName)
+
+"""
+
+
+"""
+# for seed 1486657707
+ps.client.problem.extractPath(0,0,2.15) 
+
+# Playing the computed path
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (rbprmBuilder.client.basic, r)
+pp.dt=0.03
+pp.displayVelocityPath(1)
+r.client.gui.setVisibility("path_1_root","ALWAYS_ON_TOP")
+#display path
+pp.speed=0.3
+#pp (0)
+"""
+#display path with post-optimisation
+
+
+
+
+"""
+q_far = q_init[::]
+q_far[2] = -3
+r(q_far)
+"""
+
+
+
+
+ # Manually add waypoints to roadmap:
+"""
+ps.client.problem.prepareSolveStepByStep()
+pbCl = rbprmBuilder.client.basic.problem
+q1= [0.6, 1, 0.5, 1, 0, 0, 0, 0.0, 0, 0.0, 0.0, 3, 0.0, -1.5, 0.0, 0.0, 0.0]
+
+pbCl.addConfigToRoadmap (q1)
+
+
+pbCl.directPath(q1,q_goal,True)
+
+pbCl.directPath(q_init,q1,False)
+r.client.gui.removeFromGroup("path_"+str(ps.numberPaths()-2)+"_root",r.sceneName)
+pp.displayVelocityPath(ps.numberPaths()-1)
+
+pbCl.addEdgeToRoadmap (q_init, q1, 1, False)
+pbCl.addEdgeToRoadmap (q1, q_goal, 0, False)
+
+"""
+
diff --git a/script/dynamic/export_blender.py b/script/dynamic/export_blender.py
index 9a95f45..15ecebb 100644
--- a/script/dynamic/export_blender.py
+++ b/script/dynamic/export_blender.py
@@ -35,7 +35,7 @@ r.client.gui.captureTransformOnRefresh(False)
 
 nodes = ["hrp2_14",'s']
 nodes = ["hrp2_14"]
-r.client.gui.setCaptureTransform("/local/dev_hpp/screenBlender/iros2018/yaml/darpa_contactSequence.yaml",nodes)
+r.client.gui.setCaptureTransform("/local/dev_hpp/screenBlender/tro/yaml/darpa/success_cs.yaml",nodes)
 r(q_init)
 r.client.gui.captureTransform()
 r.client.gui.captureTransformOnRefresh(True)
@@ -44,11 +44,11 @@ r(q_goal)
 r.client.gui.captureTransformOnRefresh(False)
 
 
-nodes = ['world/pinocchio']
-gui.setCaptureTransform("/local/dev_hpp/screenBlender/locomote/yaml/stairs10_NO_effectorRRT.yaml",nodes)
+nodes = ['world/pinocchio/visuals']
+r.client.gui.setCaptureTransform("/local/dev_hpp/screenBlender/tro/yaml/darpa/motion2.yaml",nodes)
 #gui.captureTransform()
-gui.captureTransformOnRefresh(True)
-gui.captureTransformOnRefresh(False)
+r.client.gui.captureTransformOnRefresh(True)
+r.client.gui.captureTransformOnRefresh(False)
 
 
 
@@ -75,6 +75,40 @@ r.client.gui.captureTransformOnRefresh(False)
 r.client.gui.writeNodeFile("path_0_root","/local/dev_hpp/screenBlender/iros2017/meshs/path_detour_geom_CTC.obj")
 
 
+"""
+l = end_effector_bezier_list['hrp2_lleg_rom'][3]
+
+for i in range(len(l)):
+  displayBezierCurve(r,l[i],offset = dict_offset['hrp2_lleg_rom'].translation.transpose().tolist()[0])
+  
+dir = "/local/dev_hpp/screenBlender/tro/curve/"
+r.client.gui.writeNodeFile('path_73_LLEG_JOINT5',dir+"rrt.obj")
+r.client.gui.writeNodeFile('bezier_curve_6',dir+"b0.obj")
+r.client.gui.writeNodeFile('bezier_curve_7',dir+"b02.obj")
+r.client.gui.writeNodeFile('bezier_curve_8',dir+"b04.obj")
+r.client.gui.writeNodeFile('bezier_curve_9',dir+"b06.obj")
+r.client.gui.writeNodeFile('bezier_curve_10',dir+"b08.obj")
+r.client.gui.writeNodeFile('bezier_curve_11',dir+"b09.obj")
+r.client.gui.writeNodeFile('bezier_curve_12',dir+"b1.obj")
+"""
+
+for i in range(6,10):
+  r.client.gui.writeNodeFile("bezier_curve_"+str(i),dir+"b"+str(i)+".obj")
+
+dir = "/local/dev_hpp/screenBlender/tro/curve/bar_xwp/"
+for i in range(29):
+  r.client.gui.writeNodeFile("path_"+str(i)+"_root",dir+"c_"+str(i)+".obj")
+
+for i in range(23):
+  r.client.gui.writeNodeFile("s"+str(i),dir+"s_"+str(i)+".stl")
+
+
+dir = "/local/dev_hpp/screenBlender/tro/meshs/stones/darpa/fail2/"
+for i in range(29):
+  r.client.gui.writeNodeFile('stone_'+str(i),dir+'stone_'+str(i)+".stl")
+
+
+
 
 
 
diff --git a/script/dynamic/flatGround_hrp2_interpSTATIC_testTransition.py b/script/dynamic/flatGround_hrp2_interpSTATIC_testTransition.py
index b6dc5cf..e2b8c11 100644
--- a/script/dynamic/flatGround_hrp2_interpSTATIC_testTransition.py
+++ b/script/dynamic/flatGround_hrp2_interpSTATIC_testTransition.py
@@ -7,7 +7,7 @@ from hpp.corbaserver.rbprm.rbprmstate import State,StateHelper
 from display_tools import *
 import flatGround_hrp2_pathKino as tp
 import time
-
+import numpy as np
 tPlanning = tp.tPlanning
 
 
@@ -50,7 +50,7 @@ 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, "fixedStep1", 0.01,"_6_DOF",limbOffset=rLegLimbOffset,kinematicConstraintsMin=0.3)
+fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 50000, "forward", 0.01,"_6_DOF",limbOffset=rLegLimbOffset,kinematicConstraintsPath = "package://hpp-rbprm-corba/com_inequalities/fullSize/RLEG_JOINT0_com_constraints.obj",kinematicConstraintsMin=0.3)
 fullBody.runLimbSampleAnalysis(rLegId, "ReferenceConfiguration", True)
 #fullBody.saveLimbDatabase(rLegId, "./db/hrp2_rleg_db.db")
 
@@ -60,13 +60,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, "fixedStep1", 0.01,"_6_DOF",limbOffset=lLegLimbOffset,kinematicConstraintsMin=0.3)
+fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 50000, "forward", 0.01,"_6_DOF",kinematicConstraintsPath = "package://hpp-rbprm-corba/com_inequalities/fullSize/LLEG_JOINT0_com_constraints.obj",limbOffset=lLegLimbOffset,kinematicConstraintsMin=0.3)
 fullBody.runLimbSampleAnalysis(lLegId, "ReferenceConfiguration", True)
 #fullBody.saveLimbDatabase(lLegId, "./db/hrp2_lleg_db.db")
 fullBody.setReferenceConfig (q_ref)
 ## Add arms (not used for contact) : 
 
 
+"""
 rarmId = 'hrp2_rarm_rom'
 rarm = 'RARM_JOINT0'
 rHand = 'RARM_JOINT5'
@@ -77,6 +78,23 @@ larm = 'LARM_JOINT0'
 lHand = 'LARM_JOINT5'
 fullBody.addNonContactingLimb(larmId,larm,lHand, 10000)
 fullBody.runLimbSampleAnalysis(larmId, "ReferenceConfiguration", True)
+"""
+
+"""
+rarmId = 'hrp2_rarm_rom'
+rarm = 'RARM_JOINT0'
+rHand = 'RARM_JOINT5'
+fullBody.addLimb(rarmId,rarm,'',lLegOffset,rLegNormal, lLegx, lLegy, 100000, "fixedStep1", 0.01,"_6_DOF",limbOffset=lLegLimbOffset,kinematicConstraintsMin=0.3)
+
+fullBody.runLimbSampleAnalysis(rarmId, "ReferenceConfiguration", True)
+larmId = 'hrp2_larm_rom'
+larm = 'LARM_JOINT0'
+lHand = 'LARM_JOINT5'
+fullBody.addLimb(larmId,larm,'',lLegOffset,rLegNormal, lLegx, lLegy, 100000, "fixedStep1", 0.01,"_6_DOF",limbOffset=lLegLimbOffset,kinematicConstraintsMin=0.3)
+
+fullBody.runLimbSampleAnalysis(larmId, "ReferenceConfiguration", True)
+"""
+
 
 
 tGenerate =  time.time() - tStart
@@ -153,34 +171,34 @@ q = fullBody.generateContacts(q,dir_init,acc_init,robTreshold)
 mid_sid = fullBody.addState(q,[lLegId,rLegId])
 """
 
+
 from hpp.gepetto import PathPlayer
 pp = PathPlayer (fullBody.client.basic, r)
 
 import fullBodyPlayerHrp2
 
 tStart = time.time()
-configsFull = fullBody.interpolate(0.001,pathId=pId,robustnessTreshold = 2, filterStates = False)
+configsFull = fullBody.interpolate(0.01,pathId=pId,robustnessTreshold = 0, filterStates = True,testReachability=False,quasiStatic=False)
 tInterpolateConfigs = time.time() - tStart
 print "number of configs :", len(configsFull)
+r(configsFull[-1])
 
-
-
+"""
 import check_qp
-
-
-planValid,curves_initGuess,timings_initGuess = check_qp.check_contact_plan(ps,r,pp,fullBody,0,len(configsFull)-1)
-
+planValid,curves_initGuess,timings_initGuess = check_qp.check_contact_plan(ps,r,pp,fullBody,0,len(configsFull))
 print "Contact plan valid : "+str(planValid)
+"""
 
 
 
-from planning.config import *
+from configs.straight_walk_dynamic_planning_config import *
 from generate_contact_sequence import *
 
 beginState = 0
-endState = len(configsFull)-2
+endState = len(configsFull)-1
 configs=configsFull[beginState:endState+1]
-cs = generateContactSequence(fullBody,configs,beginState, endState,r,curves_initGuess =curves_initGuess , timings_initGuess =timings_initGuess)
+#cs = generateContactSequence(fullBody,configs,beginState, endState,r,curves_initGuess =curves_initGuess , timings_initGuess =timings_initGuess)
+cs = generateContactSequence(fullBody,configs,beginState, endState,r)
 #player.displayContactPlan()
 
 
@@ -243,11 +261,20 @@ displayOneStepConstraints(r)
 
 """ # tests front line
 s0 = State(fullBody,q=q_init,limbsIncontact = [rLegId,lLegId])
-s02,success = StateHelper.addNewContact(s0,rLegId,[0.2,0-0.1,0.01],[0,0,1])
+smid = State(fullBody,q=q_init,limbsIncontact = [lLegId])
+sright = State(fullBody,q=q_init,limbsIncontact = [rLegId])
+s02,success = StateHelper.addNewContact(s0,rLegId,[0.2,0-0.1,0.0],[0,0,1])
 assert(success)
 fullBody.isReachableFromState(s0.sId,s02.sId)
+pIds = fullBody.isDynamicallyReachableFromState(s0.sId,s02.sId,True)
+pIds = fullBody.isDynamicallyReachableFromState(s0.sId,s02.sId,numPointsPerPhases=0)
+
+
+
 s04,success = StateHelper.addNewContact(s0,rLegId,[0.4,0-0.1,0.01],[0,0,1])
 assert(success)
+s05,success = StateHelper.addNewContact(s0,rLegId,[0.5,0-0.1,0.01],[0,0,1])
+assert(success)
 fullBody.isReachableFromState(s0.sId,s04.sId)
 s06,success = StateHelper.addNewContact(s0,rLegId,[0.6,0-0.1,0.01],[0,0,1])
 assert(success)
@@ -265,6 +292,29 @@ fullBody.isReachableFromState(s0.sId,s08.sId)
 s085,success = StateHelper.addNewContact(s0,rLegId,[0.85,0-0.1,0.01],[0,0,1])
 assert(success)
 fullBody.isReachableFromState(s0.sId,s09.sId)
+
+for i in range(1000):
+  pIds = fullBody.isDynamicallyReachableFromState(s0.sId,s04.sId,numPointsPerPhases = 15)
+
+for [s0,s1] in states:
+  fullBody.isDynamicallyReachableFromState(s0.sId,s1.sId,numPointsPerPhases = 0)
+
+
+
+for i in range(10000):
+  q = fullBody.shootRandomConfig()
+  s0 = State(fullBody,q=q,limbsIncontact = [lLegId])
+  fullBody.isReachableFromState(s0.sId,s0.sId)
+
+
+
+from parse_bench import *
+parseBenchmark([])
+
+from check_qp import check_traj_valid 
+check_traj_valid(ps,fullBody,s0,s02,pIds)
+
+
 """
 
 
@@ -306,7 +356,7 @@ fullBody.isReachableFromState(s0.sId,s09.sId)
 
 
 # test right / diag
-
+"""
 s0 = State(fullBody,q=q_init,limbsIncontact = [rLegId,lLegId])
 s02,success = StateHelper.addNewContact(s0,rLegId,[0.2,0-0.1,0.01],[0,0,1])
 assert(success)
@@ -319,6 +369,6 @@ assert(success)
 fullBody.isReachableFromState(s0.sId,s06.sId)
 s07,success = StateHelper.addNewContact(s0,rLegId,[0.7,0-0.1,0.01],[0,0,1])
 assert(success)
-
+"""
 
 
diff --git a/script/dynamic/flatGround_hrp2_pathKino.py b/script/dynamic/flatGround_hrp2_pathKino.py
index 269278f..31b6980 100644
--- a/script/dynamic/flatGround_hrp2_pathKino.py
+++ b/script/dynamic/flatGround_hrp2_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.straight_walk_config import *
 import time
 
 class Robot (Parent):
@@ -173,20 +173,21 @@ for i in range(0,ps.numberNodes()):
 """
 
 
-"""
+
 # for seed 1486657707
-ps.client.problem.extractPath(0,0,2.15) 
+#ps.client.problem.extractPath(0,0,2.15) 
 
 # Playing the computed path
 from hpp.gepetto import PathPlayer
 pp = PathPlayer (rbprmBuilder.client.basic, r)
 pp.dt=0.03
-pp.displayVelocityPath(1)
-r.client.gui.setVisibility("path_1_root","ALWAYS_ON_TOP")
+#pp.displayVelocityPath(0)
+pp.displayPath(0)
+r.client.gui.setVisibility("path_0_root","ALWAYS_ON_TOP")
 #display path
 pp.speed=0.3
 #pp (0)
-"""
+
 #display path with post-optimisation
 
 
diff --git a/script/dynamic/fullBodyPlayerHyQ.py b/script/dynamic/fullBodyPlayerHyQ.py
new file mode 100644
index 0000000..35a74c2
--- /dev/null
+++ b/script/dynamic/fullBodyPlayerHyQ.py
@@ -0,0 +1,107 @@
+
+from hpp.corbaserver.rbprm.tools.cwc_trajectory_helper import step, clean,stats, saveAllData, play_traj
+import time
+
+class Player (object):
+    def __init__ (self, fullBody,pathPlayer,tp,configs=[],draw=False,optim_effector=False,use_velocity=False,pathId = 0):
+        self.viewer = pathPlayer.publisher
+        self.tp = tp
+        self.pp = pathPlayer
+        self.fullBody=fullBody
+        self.configs=configs
+        self.rLegId = 'rfleg'
+        self.rfoot = 'rf_foot_joint'
+        self.lLegId = 'lhleg'
+        self.lLeg = 'lh_haa_joint'
+        self.lfoot = 'lh_foot_joint'
+        self.rarmId = 'rhleg'
+        self.rHand = 'rh_foot_joint'
+        self.larmId = 'lfleg'
+        self.lHand = 'lf_foot_joint'
+        self.draw=draw
+        self.pathId = pathId
+        self.use_velocity = use_velocity
+        self.optim_effector = optim_effector
+        self.limbsCOMConstraints = { self.rLegId : {'file': "hyq/"+self.rLegId+"_com.ineq", 'effector' : self.rfoot},  
+						    self.lLegId : {'file': "hyq/"+self.lLegId+"_com.ineq", 'effector' : self.lfoot},  
+						    self.rarmId : {'file': "hyq/"+self.rarmId+"_com.ineq", 'effector' : self.rHand},  
+						    self.larmId : {'file': "hyq/"+self.larmId+"_com.ineq", 'effector' : self.lHand} }
+
+
+    def act(self,i, numOptim = 0, use_window = 0, friction = 0.5, optim_effectors = True, time_scale = 1,  verbose = True, draw = False, trackedEffectors = []):
+	    return step(self.fullBody, self.configs, i, numOptim, self.pp, self.limbsCOMConstraints, friction, optim_effectors = optim_effectors, time_scale = time_scale, useCOMConstraints = False, use_window = use_window,
+	    verbose = verbose, draw = draw, trackedEffectors = trackedEffectors,use_velocity=self.use_velocity, pathId = self.pathId)
+
+    def initConfig(self):
+        self.viewer.client.gui.setVisibility("hyq", "ON")
+        self.tp.r.client.gui.setVisibility("toto", "OFF")
+        self.tp.r.client.gui.setVisibility("hyq_trunk", "OFF")
+        if len(self.configs)>1:
+            self.viewer(self.configs[0])
+        else:
+            q_init = self.fullBody.getCurrentConfig()
+            q_init[0:7] = self.tp.ps.configAtParam(0,0.01)[0:7] # use this to get the correct orientation
+            dir_init = self.tp.ps.configAtParam(0,0.01)[7:10]
+            acc_init = self.tp.ps.configAtParam(0,0.01)[10:13]
+            # Randomly generating a contact configuration at q_init
+            self.fullBody.setCurrentConfig (q_init)
+            q_init = self.fullBody.generateContacts(q_init,dir_init,acc_init)
+            self.viewer(q_init)
+
+	
+    def endConfig(self):
+        self.viewer.client.gui.setVisibility("hyq", "ON")
+        self.tp.r.client.gui.setVisibility("toto", "OFF")
+        self.tp.r.client.gui.setVisibility("hyq_trunk", "OFF")
+        if len(self.configs)>1:
+            self.viewer(self.configs[len(self.configs)-1])
+        else:
+            q_goal = self.fullBody.getCurrentConfig()
+            q_goal[0:7] = self.tp.ps.configAtParam(0,self.tp.ps.pathLength(0))[0:7] # use this to get the correct orientation
+            dir_goal = self.tp.ps.configAtParam(0,0.01)[7:10]
+            acc_goal = self.tp.ps.configAtParam(0,0.01)[10:13]
+            # Randomly generating a contact configuration at q_init
+            self.fullBody.setCurrentConfig (q_goal)
+            q_goal = self.fullBody.generateContacts(q_goal,dir_goal,acc_goal)
+            self.viewer(q_goal)
+
+    def rootPath(self):
+	    self.viewer.client.gui.setVisibility("hyq", "OFF")
+	    self.tp.r.client.gui.setVisibility("toto", "OFF")
+	    self.viewer.client.gui.setVisibility("hyq", "OFF")
+	    self.viewer.client.gui.setVisibility("hyq_trunk", "ON")
+	    self.tp.pp(0)
+	    self.viewer.client.gui.setVisibility("hyq_trunk", "OFF")
+	    self.viewer.client.gui.setVisibility("hyq", "ON")
+	    self.tp.cl.problem.selectProblem("default")
+	
+    def genPlan(self):
+	    self.viewer.client.gui.setVisibility("hyq", "ON")
+	    self.tp.r.client.gui.setVisibility("toto", "OFF")
+	    self.tp.r.client.gui.setVisibility("hyq_trunk", "OFF")
+	    start = time.clock() 
+	    self.configs = self.fullBody.interpolate(0.12, 10, 10, True)
+	    end = time.clock() 
+	    print "Contact plan generated in " + str(end-start) + "seconds"
+	
+    def displayContactPlan(self,pause = 0.5):
+	    self.viewer.client.gui.setVisibility("hyq", "ON")
+	    self.tp.r.client.gui.setVisibility("toto", "OFF")
+	    self.tp.r.client.gui.setVisibility("hyq_trunk", "OFF")
+	    for i in range(0,len(self.configs)-1):
+		    self.viewer(self.configs[i]);
+		    time.sleep(pause)		
+		
+    def interpolate(self,begin=1,end=0):
+        if end==0:
+            end = len(self.configs) - 1
+        self.viewer.client.gui.setVisibility("hyq", "ON")
+        self.tp.r.client.gui.setVisibility("toto", "OFF")
+        self.tp.r.client.gui.setVisibility("hyq_trunk", "OFF")
+        for i in range(begin,end):
+            self.act(i,1,optim_effectors=self.optim_effector,draw=self.draw)
+		
+    def play(self,frame_rate = 1./60.):
+	    play_traj(self.fullBody,self.pp,frame_rate)
+	
+
diff --git a/script/dynamic/hrp2_wall_path_visual.py b/script/dynamic/hrp2_wall_path_visual.py
new file mode 100644
index 0000000..6201580
--- /dev/null
+++ b/script/dynamic/hrp2_wall_path_visual.py
@@ -0,0 +1,129 @@
+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.gepetto import Viewer
+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_flexible'
+urdfNameRoms =  ['hrp2_larm_rom_visual','hrp2_rarm_rom_visual','hrp2_lleg_rom_visual','hrp2_rleg_rom_visual']
+urdfSuffix = ""
+srdfSuffix = ""
+
+rbprmBuilder = Builder ()
+
+rbprmBuilder.loadModel(urdfName, urdfNameRoms, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+rbprmBuilder.setJointBounds ("base_joint_xyz", [0,3, -2, 0, 0.3, 1])
+rbprmBuilder.setFilter(['hrp2_lleg_rom_visual','hrp2_rleg_rom_visual'])
+rbprmBuilder.setAffordanceFilter('hrp2_rarm_rom_visual', ['Lean'])
+rbprmBuilder.setAffordanceFilter('hrp2_lleg_rom_visual', ['Support',])
+rbprmBuilder.setAffordanceFilter('hrp2_rleg_rom_visual', ['Support'])
+rbprmBuilder.boundSO3([-0.,0,-1,1,-1,1])
+vMax = 1;
+aMax = 10;
+extraDof = 6
+rbprmBuilder.client.basic.robot.setDimensionExtraConfigSpace(extraDof)
+rbprmBuilder.client.basic.robot.setExtraConfigSpaceBounds([-vMax,vMax,-vMax,vMax,0,0,0,0,0,0,0,0])
+indexECS = rbprmBuilder.getConfigSize() - rbprmBuilder.client.basic.robot.getDimensionExtraConfigSpace()
+
+#~ from hpp.corbaserver.rbprm. import ProblemSolver
+
+
+ps = ProblemSolver( rbprmBuilder )
+ps.client.problem.setParameter("aMax",aMax)
+ps.client.problem.setParameter("vMax",vMax)
+ps.client.problem.setParameter("sizeFootX",0.24)
+ps.client.problem.setParameter("sizeFootY",0.14)
+r = Viewer (ps)
+
+
+from hpp.corbaserver.affordance.affordance import AffordanceTool
+afftool = AffordanceTool ()
+afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
+afftool.setAffordanceConfig('Lean', [0.5, 0.03, 0.00005])
+afftool.loadObstacleModel (packageName, "roomWall", "planning", r)
+r.addLandmark(r.sceneName,1)
+afftool.visualiseAffordances('Support', r,r.color.brown)
+afftool.visualiseAffordances('Lean', r, r.color.blue)
+
+
+q_init = rbprmBuilder.getCurrentConfig ();
+#q_init[0:3] = [2.1, -1, 0.58];
+q_init[0:3] = [2, -1, 0.58];
+#q_init[0:3] = [1.85, -1, 0.58];
+q_init[3:7] = [0.7071,0,0,0.7071]
+#q_init[indexECS:indexECS+3] = [2,0,0]
+q_init[indexECS:indexECS+3] = [1,0,0]
+rbprmBuilder.setCurrentConfig (q_init); r (q_init)
+
+
+q_goal = q_init [::]
+q_goal [0:3] = [1.2, -1, 0.58]; 
+q_goal[indexECS:indexECS+3] = [-1,0,0]
+
+
+#r(q_goal)
+
+
+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")
+
+#t = ps.solve ()
+
+
+ps.client.problem.prepareSolveStepByStep()
+
+
+ps.client.problem.finishSolveStepByStep()
+
+
+
+
+
+# Playing the computed path
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (rbprmBuilder.client.basic, r)
+pp.dt=1./30.
+#r.client.gui.removeFromGroup("rm0",r.sceneName)
+pp.displayVelocityPath(0)
+pp.speed=0.5
+#pp(0)
+
+
+
+
+
+q_far = q_init[::]
+q_far[2] = -3
+r(q_far)
+
+#~ pp(0)
+
+
+
+
diff --git a/script/dynamic/hyq_jumpLow_path.py b/script/dynamic/hyq_jumpLow_path.py
new file mode 100644
index 0000000..deb8999
--- /dev/null
+++ b/script/dynamic/hyq_jumpLow_path.py
@@ -0,0 +1,99 @@
+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+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 = ""
+vMax = 4;
+aMax = 4.9;
+extraDof = 6
+rbprmBuilder = Builder ()
+rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+rbprmBuilder.setJointBounds ("base_joint_xyz", [-6,6, -3, 3, 0, 2.5])
+rbprmBuilder.boundSO3([-0.1,0.1,-1,1,-1,1])
+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',])
+rbprmBuilder.client.basic.robot.setDimensionExtraConfigSpace(extraDof)
+rbprmBuilder.client.basic.robot.setExtraConfigSpaceBounds([-vMax,vMax,-vMax,vMax,0,0,0,0,0,0,0,0])
+
+
+#~ from hpp.corbaserver.rbprm. import ProblemSolver
+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_jump_low", "planning", r)
+afftool.visualiseAffordances('Support', r, r.color.brown)
+r.addLandmark(r.sceneName,1)
+q_init = rbprmBuilder.getCurrentConfig ();
+#q_init[(len(q_init)-3):]=[0,0,1] # set normal for init / goal config
+#q_init [0:3] = [-3, 0, 0.79]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
+q_init[0:3] = [-0.8,0,0.77];rbprmBuilder.setCurrentConfig (q_init); r (q_init)
+
+
+q_goal = q_init [::]
+#q_goal [0:3] = [-2, 0, 0.9]; r (q_goal) # premiere passerelle
+q_goal [0:3] = [0.7, 0, 0.28]; r (q_goal) # pont
+
+
+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.01)
+# Choosing kinodynamic methods : 
+ps.selectSteeringMethod("RBPRMKinodynamic")
+ps.selectDistance("KinodynamicDistance")
+ps.selectPathPlanner("DynamicPlanner")
+
+
+
+r(q_init)
+
+
+ps.client.problem.prepareSolveStepByStep()
+
+ps.client.problem.finishSolveStepByStep()
+
+#i = 0
+#r.displayRoadmap("rm"+str(i),0.02)
+#ps.client.problem.executeOneStep() ;i = i+1; r.displayRoadmap("rm"+str(i),0.02) ; r.client.gui.removeFromGroup("rm"+str(i-1),r.sceneName) ;
+#t = 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)
+
+
+
+#r.client.gui.removeFromGroup("rm",r.sceneName)
+r.client.gui.removeFromGroup("rmPath",r.sceneName)
+r.client.gui.removeFromGroup("path_1_root",r.sceneName)
+#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
+
+
+math.sqrt((np.linalg.norm(u)*np.linalg.norm(u)) * (np.linalg.norm(v)*np.linalg.norm(v))
+
+from hpp import quaternion as Quaternion
+q = Quaternion.Quaternion([1,0,0],[2,3,-1])
+
diff --git a/script/dynamic/plateform_hrp2_interp_testTransition.py b/script/dynamic/plateform_hrp2_interp_testTransition.py
index 38ae188..7fba5f1 100644
--- a/script/dynamic/plateform_hrp2_interp_testTransition.py
+++ b/script/dynamic/plateform_hrp2_interp_testTransition.py
@@ -46,7 +46,7 @@ tStart = time.time()
 
 
 rLeg = 'RLEG_JOINT0'
-rLegOffset = [0,0,-0.105]
+rLegOffset = [0,0,-0.0955]
 rLegLimbOffset=[0,0,-0.035]#0.035
 rLegNormal = [0,0,1]
 rLegx = 0.09; rLegy = 0.05
@@ -56,7 +56,7 @@ fullBody.runLimbSampleAnalysis(rLegId, "ReferenceConfiguration", True)
 #fullBody.saveLimbDatabase(rLegId, "./db/hrp2_rleg_db.db")
 
 lLeg = 'LLEG_JOINT0'
-lLegOffset = [0,0,-0.105]
+lLegOffset = [0,0,-0.0955]
 lLegLimbOffset=[0,0,0.035]
 lLegNormal = [0,0,1]
 lLegx = 0.09; lLegy = 0.05
@@ -96,8 +96,8 @@ q_goal[configSize+3:configSize+6] = [0,0,0]
 
 
 # FIXME : test
-q_init[2] = q_ref[2]+0.01
-q_goal[2] = q_ref[2]+0.01
+q_init[2] = q_ref[2]+0.011
+q_goal[2] = q_ref[2]+0.011
 
 fullBody.setStaticStability(True)
 # Randomly generating a contact configuration at q_init
@@ -156,11 +156,11 @@ si = State(fullBody,q=q_init,limbsIncontact=[lLegId,rLegId])
 
 
 n = [0.0, -0.42261828000211843, 0.9063077785212101]
-p = [0.775, 0.23, -0.02]
+p = [0.775, 0.22, -0.019]
 moveSphere('s',r,p)
-smid,success = StateHelper.addNewContact(si,lLegId,p,n)
+smid,success = StateHelper.addNewContact(si,lLegId,p,n,100)
 assert(success)
-smid2,success = StateHelper.addNewContact(sf,lLegId,p,n)
+smid2,success = StateHelper.addNewContact(sf,lLegId,p,n,100)
 assert(success)
 r(smid.q())
 
diff --git a/script/dynamic/plateform_hrp2_interp_testTransition_muscod.py b/script/dynamic/plateform_hrp2_interp_testTransition_muscod.py
new file mode 100644
index 0000000..f52f37b
--- /dev/null
+++ b/script/dynamic/plateform_hrp2_interp_testTransition_muscod.py
@@ -0,0 +1,267 @@
+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.gepetto import Viewer
+import time
+from constraint_to_dae import *
+from hpp.corbaserver.rbprm.rbprmstate import State,StateHelper
+from display_tools import *
+import plateform_hrp2_path as tp
+import time
+
+tPlanning = tp.tPlanning
+
+
+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", [-1,2, -0.5, 0.5, 0.5, 0.8])
+fullBody.client.basic.robot.setDimensionExtraConfigSpace(tp.extraDof)
+fullBody.client.basic.robot.setExtraConfigSpaceBounds([-1,1,-1,1,-0.5,0.5,0,0,0,0,0,0])
+ps = tp.ProblemSolver( fullBody )
+ps.client.problem.setParameter("aMax",tp.aMax)
+ps.client.problem.setParameter("vMax",tp.vMax)
+
+r = tp.Viewer (ps,viewerClient=tp.r.client)
+
+
+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)
+qfar=q_ref[::]
+qfar[2] = -5
+
+#~ 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.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, "fixedStep1", 0.01,"_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.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, "fixedStep1", 0.01,"_6_DOF",limbOffset=lLegLimbOffset)
+fullBody.runLimbSampleAnalysis(lLegId, "ReferenceConfiguration", True)
+#fullBody.saveLimbDatabase(lLegId, "./db/hrp2_lleg_db.db")
+fullBody.setReferenceConfig (q_ref)
+## Add arms (not used for contact) : 
+
+
+tGenerate =  time.time() - tStart
+print "generate databases in : "+str(tGenerate)+" 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.01)[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.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]
+
+
+# FIXME : test
+q_init[2] = q_ref[2]+0.01
+q_goal[2] = q_ref[2]+0.01
+
+fullBody.setStaticStability(True)
+# Randomly generating a contact configuration at q_init
+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,[lLegId,rLegId])
+fullBody.setEndState(q_goal,[lLegId,rLegId])
+
+
+#p = fullBody.computeContactPointsAtState(init_sid)
+#p = fullBody.computeContactPointsAtState(int_sid)
+
+
+"""
+q = q_init[::]
+q[0] += 0.3
+q = fullBody.generateContacts(q,dir_init,acc_init,robTreshold)
+mid_sid = fullBody.addState(q,[lLegId,rLegId])
+"""
+
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (fullBody.client.basic, r)
+
+import fullBodyPlayerHrp2
+
+"""
+tStart = time.time()
+configsFull = fullBody.interpolate(0.001,pathId=pId,robustnessTreshold = 1, filterStates = False)
+tInterpolateConfigs = time.time() - tStart
+print "number of configs :", len(configsFull)
+"""
+
+q_init[0] += 0.05
+createSphere('s',r)
+
+n = [0,0,1]
+p = [0,0.1,0]
+
+
+sf = State(fullBody,q=q_goal,limbsIncontact=[lLegId,rLegId])
+si = State(fullBody,q=q_init,limbsIncontact=[lLegId,rLegId])
+
+
+n = [0.0, -0.42261828000211843, 0.9063077785212101]
+p = [0.775, 0.23, -0.02]
+moveSphere('s',r,p)
+smid,success = StateHelper.addNewContact(si,lLegId,p,n)
+assert(success)
+smid2,success = StateHelper.addNewContact(sf,lLegId,p,n)
+assert(success)
+r(smid.q())
+
+sf2 = State(fullBody,q=q_goal,limbsIncontact=[lLegId,rLegId])
+"""
+com = fullBody.getCenterOfMass()
+com[1] = 0
+"""
+import disp_bezier
+pp.dt = 0.0001
+pids = []
+curves = []
+timings = []
+pid = fullBody.isDynamicallyReachableFromState(si.sId,smid.sId,True)
+#assert (len(pid)>0)
+disp_bezier.showPath(r,pp,pid)
+curves += [fullBody.getPathAsBezier(int(pid[0]))]
+timings += [[ps.pathLength(int(pid[1])), ps.pathLength(int(pid[2])), ps.pathLength(int(pid[3]))]]
+pids += pid
+pid = fullBody.isDynamicallyReachableFromState(smid.sId,smid2.sId,True)
+#assert (len(pid)>0)
+disp_bezier.showPath(r,pp,pid)
+curves += [fullBody.getPathAsBezier(int(pid[0]))]
+timings += [[ps.pathLength(int(pid[1])), ps.pathLength(int(pid[2])), ps.pathLength(int(pid[3]))]]
+pids += pid
+pid = fullBody.isDynamicallyReachableFromState(smid2.sId,sf2.sId,True)
+#assert (len(pid)>0)
+curves += [fullBody.getPathAsBezier(int(pid[0]))]
+timings += [[ps.pathLength(int(pid[1])), ps.pathLength(int(pid[2])), ps.pathLength(int(pid[3]))]]
+pids += pid
+disp_bezier.showPath(r,pp,pid)
+
+
+"""
+
+n = [0,0,1]
+p = [1.2,0.1,0]
+moveSphere('s',r,p)
+
+sE,success = StateHelper.addNewContact(sE,lLegId,p,n)
+assert(success)
+p = [1.15,-0.1,0] 
+sfe, success = StateHelper.addNewContact(sE,rLegId,p,n)
+assert(success)
+
+
+
+n = [0.0, -0.42261828000211843, 0.9063077785212101]
+p = [0.775, 0.23, -0.02]
+moveSphere('s',r,p)
+
+sE,success = StateHelper.addNewContact(sE,lLegId,p,n)
+assert(success)
+
+
+pids = []
+pids += [fullBody.isDynamicallyReachableFromState(si.sId,sE.sId)]
+pids += [fullBody.isDynamicallyReachableFromState(sE.sId,sfe.sId)]
+for pid in pids :
+  if pid > 0:
+    print "success"
+    pp.displayPath(pid,color=r.color.blue)
+    r.client.gui.setVisibility('path_'+str(pid)+'_root','ALWAYS_ON_TOP')
+  else:
+    print "fail."
+
+"""
+
+configs = []
+configs += [si.q()]
+configs += [smid.q()]
+configs += [smid2.q()]
+configs += [sf2.q()]
+
+
+
+from planning.config import *
+from generate_contact_sequence import *
+
+
+
+
+beginState = si.sId
+endState = sf2.sId
+#cs = generateContactSequence(fullBody,configs,beginState, endState,r,curves,timings)
+cs = generateContactSequence(fullBody,configs,beginState, endState)
+#cs = generateContactSequence(fullBody,configs,smid.sId, smid2.sId,r)
+
+
+filename = OUTPUT_DIR + "/" + OUTPUT_SEQUENCE_FILE
+cs.saveAsXML(filename, "ContactSequence")
+print "save contact sequence : ",filename
+
+
+
+import planning.generate_muscod_problem as mp
+mp.generate_muscod_problem(filename,True)
+
+
+
+
+
+
+
+
diff --git a/script/dynamic/qs.py b/script/dynamic/qs.py
new file mode 100644
index 0000000..f5d8966
--- /dev/null
+++ b/script/dynamic/qs.py
@@ -0,0 +1,55 @@
+qs=[0,
+ 0,
+ 0.6,
+ 1.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ -0.3,
+ 0.0,
+ 0.3,
+ 0.0,
+ -0.7,
+ 0.6,
+ -0.8,
+ -1.4,
+ 0.0,
+ 0.0,
+ 0.17,
+ -0.3,
+ -0.2,
+ 0.4,
+ -0.8,
+ 0,
+ 0.2,
+ 0.17,
+ 0.007707112056539298,
+ -0.019015318209438203,
+ -0.9045605499154598,
+ 0.8236469062028707,
+ 0.08085496983862162,
+ 0.01900945896910387,
+ 0.031512053989645315,
+ 0.0367111889160564,
+ 0.2331176225446984,
+ 0.6425168164883693,
+ -0.8756104726750402,
+ -0.03670764999208465,
+ 0.3,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0]
+
+
+r(qs)
+
+
+nodes = ["hrp2_14"]
+r.client.gui.setCaptureTransform("/local/dev_hpp/screenBlender/qs.yaml",nodes)
+r.client.gui.captureTransform()
+
+
+r.client.gui.captureTransformOnRefresh(False)
+
diff --git a/script/dynamic/sarpa_hrp2_interStatic.py b/script/dynamic/sarpa_hrp2_interStatic.py
new file mode 100644
index 0000000..f8f17ce
--- /dev/null
+++ b/script/dynamic/sarpa_hrp2_interStatic.py
@@ -0,0 +1,198 @@
+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.035]#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.01,"_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.035]
+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.01,"_6_DOF",limbOffset=lLegLimbOffset)
+fullBody.runLimbSampleAnalysis(lLegId, "ReferenceConfiguration", True)
+#fullBody.saveLimbDatabase(lLegId, "./db/hrp2_lleg_db.db")
+
+## Add arms (not used for contact) : 
+
+
+rarmId = 'hrp2_rarm_rom'
+rarm = 'RARM_JOINT0'
+rHand = 'RARM_JOINT5'
+fullBody.addNonContactingLimb(rarmId,rarm,rHand, 50000)
+fullBody.runLimbSampleAnalysis(rarmId, "ReferenceConfiguration", True)
+larmId = 'hrp2_larm_rom'
+larm = 'LARM_JOINT0'
+lHand = 'LARM_JOINT5'
+fullBody.addNonContactingLimb(larmId,larm,lHand, 50000)
+fullBody.runLimbSampleAnalysis(larmId, "ReferenceConfiguration", True)
+
+
+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 = False)
+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()
+
+#player.interpolate(2,len(configs)-1)
+
+
+from planning.config import *
+from generate_contact_sequence import *
+cs = generateContactSequence(fullBody,configs[:5],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)
+
+
+dir = fullBody.getCurrentConfig()[37:40]
+fullBody.client.rbprm.rbprm.evaluateConfig(fullBody.getCurrentConfig(),dir)
+
+vd[0:3] = fullBody.getCurrentConfig()[0:3]
+addVector(r,fullBody,r.color.black,vd)
+
+vl[0:3] = fullBody.getCurrentConfig()[0:3]
+addVector(r,fullBody,r.color.blue,vl)
+
+vlb[0:3] = fullBody.getCurrentConfig()[0:3]
+addVector(r,fullBody,r.color.red,vlb)
+
+"""
+
+
+wid = r.client.gui.getWindowID("window_hpp_")
+#r.client.gui.attachCameraToNode( 'hrp2_14/BODY_0',wid)
+
+
+
diff --git a/script/dynamic/sideWall_hyq_interpKino_fullBody.py b/script/dynamic/sideWall_hyq_interpKino_fullBody.py
new file mode 100644
index 0000000..6b7574e
--- /dev/null
+++ b/script/dynamic/sideWall_hyq_interpKino_fullBody.py
@@ -0,0 +1,190 @@
+#Importing helper class for RBPRM
+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+from hpp.corbaserver.rbprm.rbprmstate import State,StateHelper
+
+from display_tools import *
+from constraint_to_dae import *
+
+from hpp.gepetto import Viewer
+
+
+#calling script darpa_hyq_path to compute root path
+import sideWall_hyq_pathKino as tp
+
+
+from os import environ
+ins_dir = environ['DEVEL_DIR']
+db_dir = ins_dir+"/install/share/hyq-rbprm/database/hyq_"
+
+
+
+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.client.basic.robot.setDimensionExtraConfigSpace(tp.extraDof)
+fullBody.setJointBounds("base_joint_xyz", [0.8,5.6, -0.5, 0.5, 0.4, 1.2])
+#  Setting a number of sample configurations used
+dynamic=True
+
+ps = tp.ProblemSolver(fullBody)
+r = tp.Viewer (ps,viewerClient=tp.r.client,displayArrows = True, displayCoM = True)
+
+#  Setting a number of sample configurations used
+nbSamples = 20000
+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
+rfLegId = 'rfleg'
+# First joint of the limb, as in urdf file
+rfLeg = 'rf_haa_joint'
+# Last joint of the limb, as in urdf file
+rfFoot = '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(rfLegId,rfLeg,rfFoot,offset,normal, legx, legy, nbSamples, "manipulability", 0.05, cType)
+fullBody.runLimbSampleAnalysis(rfLegId, "jointLimitsDistance", True)
+
+lhLegId = 'lhleg'
+lhLeg = 'lh_haa_joint'
+lhFoot = 'lh_foot_joint'
+fullBody.addLimb(lhLegId,lhLeg,lhFoot,offset,normal, legx, legy, nbSamples, "manipulability", 0.05, cType)
+fullBody.runLimbSampleAnalysis(lhLegId, "jointLimitsDistance", True)
+
+lfLegId = 'lfleg'
+lfLeg = 'lf_haa_joint'
+lfFoot = 'lf_foot_joint'
+fullBody.addLimb(lfLegId,lfLeg,lfFoot,offset,normal, legx, legy, nbSamples, "manipulability", 0.05, cType)
+fullBody.runLimbSampleAnalysis(lfLegId, "jointLimitsDistance", True)
+
+rhLegId = 'rhleg'
+rhLeg = 'rh_haa_joint'
+rhFoot = 'rh_foot_joint'
+fullBody.addLimb(rhLegId,rhLeg,rhFoot,offset,normal, legx, legy, nbSamples, "manipulability", 0.05, cType)
+fullBody.runLimbSampleAnalysis(rhLegId, "jointLimitsDistance", True)
+
+
+
+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(0))[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(0))[7:10]
+acc_goal = tp.ps.configAtParam(0,tp.ps.pathLength(0))[10:13]
+configSize = fullBody.getConfigSize() -fullBody.client.basic.robot.getDimensionExtraConfigSpace()
+
+fullBody.setStaticStability(True)
+# Randomly generating a contact configuration at q_init
+fullBody.setCurrentConfig (q_init) ; r(q_init)
+s_init = StateHelper.generateStateInContact(fullBody,q_init,dir_init,acc_init)
+q_init = s_init.q()
+r(q_init)
+
+# Randomly generating a contact configuration at q_end
+fullBody.setCurrentConfig (q_goal)
+s_goal = StateHelper.generateStateInContact(fullBody,q_goal, dir_goal,acc_goal)
+q_goal = s_goal.q()
+
+# 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] = acc_goal[::]
+# specifying the full body configurations as start and goal state of the problem
+fullBody.setStartStateId(s_init.sId)
+fullBody.setEndStateId(s_goal.sId)
+
+q_far = q_init[::]
+q_far[2] = -5
+
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (fullBody.client.basic, r)
+pp.dt = 0.0001
+
+r(q_init)
+# computing the contact sequence
+
+#~ configs = fullBody.interpolate(0.08,pathId=1,robustnessTreshold = 2, filterStates = True)
+configs = fullBody.interpolate(0.001,pathId=0,robustnessTreshold = 1, filterStates = True)
+r(configs[-1])
+
+
+
+
+print "number of configs =", len(configs)
+r(configs[-1])
+
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (fullBody.client.basic, r)
+
+from fullBodyPlayer import Player
+player = Player(fullBody,pp,tp,configs,draw=True,optim_effector=False,use_velocity=False,pathId = 0)
+
+#player.displayContactPlan()
+
+player.interpolate(0,len(configs)-1)
+
+#### whole body :
+from hpp.corbaserver.rbprm.tools.path_to_trajectory import *
+from disp_bezier import *
+
+beginState = 0 
+endState = len(configs)-2
+time_per_paths = []
+total_paths_ids = []
+merged_paths_ids = [] 
+
+for id_state in range(beginState,endState):
+
+  pid = fullBody.isDynamicallyReachableFromState(id_state,id_state+1,True)
+  assert(len(pid) > 0 and "isDynamicallyReachable failed for state "+str(id_state))
+  showPath(r,pp,pid)
+
+  pId1 = int(pid[0]) ; pId2 = int(pid[1]) ; pId3=int(pid[2])
+  paths_ids = fullBody.effectorRRTFromPosBetweenState(id_state,id_state+1,pId1,pId2,pId3,1)
+
+  time_per_paths += [ps.pathLength(pId1)]
+  time_per_paths += [ps.pathLength(pId2)]
+  time_per_paths += [ps.pathLength(pId3)]
+  total_paths_ids += paths_ids[:-1]
+  merged_paths_ids += [paths_ids[-1]]
+
+
+
+end_effector_joints_names=["lf_foot_joint","lh_foot_joint","rf_foot_joint","rh_foot_joint"]
+colors=[r.color.red,r.color.lightRed,r.color.brow,r.color.lightBrown]
+#fullBody.client.basic.robot.setDimensionExtraConfigSpace(tp.extraDof+1) # needed because the intermediate path have one more degree of freedom
+displayFeetTrajectory(pp,end_effector_joints_names,colors,merged_paths_ids)
+#fullBody.client.basic.robot.setDimensionExtraConfigSpace(tp.extraDof)
+
+
+trajectory = gen_trajectory_to_play(fullBody,pp,total_paths_ids,time_per_paths)
+play_trajectory(fullBody,pp, trajectory)
+
+
+
+
+
+
+
+
+
+
diff --git a/script/dynamic/sideWall_hyq_interpKino_muscod.py b/script/dynamic/sideWall_hyq_interpKino_muscod.py
new file mode 100644
index 0000000..709d9b8
--- /dev/null
+++ b/script/dynamic/sideWall_hyq_interpKino_muscod.py
@@ -0,0 +1,166 @@
+#Importing helper class for RBPRM
+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+from hpp.corbaserver.rbprm.rbprmstate import State,StateHelper
+
+from display_tools import *
+from constraint_to_dae import *
+
+from hpp.gepetto import Viewer
+
+
+#calling script darpa_hyq_path to compute root path
+import sideWall_hyq_pathKino as tp
+
+
+from os import environ
+ins_dir = environ['DEVEL_DIR']
+db_dir = ins_dir+"/install/share/hyq-rbprm/database/hyq_"
+
+
+
+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.client.basic.robot.setDimensionExtraConfigSpace(tp.extraDof)
+fullBody.setJointBounds("base_joint_xyz", [0.8,5.6, -0.5, 0.5, 0.4, 1.2])
+#  Setting a number of sample configurations used
+dynamic=True
+
+ps = tp.ProblemSolver(fullBody)
+r = tp.Viewer (ps,viewerClient=tp.r.client,displayArrows = True, displayCoM = True)
+
+#  Setting a number of sample configurations used
+nbSamples = 20000
+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
+rfLegId = 'rfleg'
+# First joint of the limb, as in urdf file
+rfLeg = 'rf_haa_joint'
+# Last joint of the limb, as in urdf file
+rfFoot = '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(rfLegId,rfLeg,rfFoot,offset,normal, legx, legy, nbSamples, "manipulability", 0.05, cType)
+fullBody.runLimbSampleAnalysis(rfLegId, "jointLimitsDistance", True)
+
+lhLegId = 'lhleg'
+lhLeg = 'lh_haa_joint'
+lhFoot = 'lh_foot_joint'
+fullBody.addLimb(lhLegId,lhLeg,lhFoot,offset,normal, legx, legy, nbSamples, "manipulability", 0.05, cType)
+fullBody.runLimbSampleAnalysis(lhLegId, "jointLimitsDistance", True)
+
+lfLegId = 'lfleg'
+lfLeg = 'lf_haa_joint'
+lfFoot = 'lf_foot_joint'
+fullBody.addLimb(lfLegId,lfLeg,lfFoot,offset,normal, legx, legy, nbSamples, "manipulability", 0.05, cType)
+fullBody.runLimbSampleAnalysis(lfLegId, "jointLimitsDistance", True)
+
+rhLegId = 'rhleg'
+rhLeg = 'rh_haa_joint'
+rhFoot = 'rh_foot_joint'
+fullBody.addLimb(rhLegId,rhLeg,rhFoot,offset,normal, legx, legy, nbSamples, "manipulability", 0.05, cType)
+fullBody.runLimbSampleAnalysis(rhLegId, "jointLimitsDistance", True)
+
+
+
+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(0))[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(0))[7:10]
+acc_goal = tp.ps.configAtParam(0,tp.ps.pathLength(0))[10:13]
+configSize = fullBody.getConfigSize() -fullBody.client.basic.robot.getDimensionExtraConfigSpace()
+
+fullBody.setStaticStability(True)
+# Randomly generating a contact configuration at q_init
+fullBody.setCurrentConfig (q_init) ; r(q_init)
+s_init = StateHelper.generateStateInContact(fullBody,q_init,dir_init,acc_init)
+q_init = s_init.q()
+r(q_init)
+
+# Randomly generating a contact configuration at q_end
+fullBody.setCurrentConfig (q_goal)
+s_goal = StateHelper.generateStateInContact(fullBody,q_goal, dir_goal,acc_goal)
+q_goal = s_goal.q()
+
+# 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] = acc_goal[::]
+# specifying the full body configurations as start and goal state of the problem
+fullBody.setStartStateId(s_init.sId)
+fullBody.setEndStateId(s_goal.sId)
+
+q_far = q_init[::]
+q_far[2] = -5
+
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (fullBody.client.basic, r)
+pp.dt = 0.0001
+
+r(q_init)
+# computing the contact sequence
+
+#~ configs = fullBody.interpolate(0.08,pathId=1,robustnessTreshold = 2, filterStates = True)
+configs = fullBody.interpolate(0.001,pathId=0,robustnessTreshold = 1, filterStates = True)
+r(configs[-1])
+
+
+
+
+print "number of configs =", len(configs)
+r(configs[-1])
+
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (fullBody.client.basic, r)
+
+from fullBodyPlayer import Player
+player = Player(fullBody,pp,tp,configs,draw=True,optim_effector=False,use_velocity=dynamic,pathId = 0)
+
+
+
+from planning_hyq.config import *
+from generate_contact_sequence_hyq import *
+
+beginState = 0
+endState = len(configs)-1
+
+cs = generateContactSequence(fullBody,configs,beginState, endState,r)
+
+
+filename = OUTPUT_DIR + "/" + OUTPUT_SEQUENCE_FILE
+cs.saveAsXML(filename, "ContactSequence")
+print "save contact sequence : ",filename
+
+
+
+#player.displayContactPlan()
+
+#player.interpolate(0,len(configs)-1)
+
+
+
+
+
+
diff --git a/script/dynamic/slalom_bauzil_hrp2_interStatic.py b/script/dynamic/slalom_bauzil_hrp2_interStatic.py
new file mode 100644
index 0000000..6ee14e0
--- /dev/null
+++ b/script/dynamic/slalom_bauzil_hrp2_interStatic.py
@@ -0,0 +1,193 @@
+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.gepetto import Viewer
+#from tools import *
+import slalom_bauzil_hrp2_pathKino as tp
+import time
+import omniORB.any
+from constraint_to_dae import *
+from display_tools import *
+from configs.stairs10_bauzil_stairs import *
+from disp_bezier import *
+from hpp.corbaserver.rbprm.rbprmstate import State,StateHelper
+
+packageName = "hrp2_14_description"
+meshPackageName = "hrp2_14_description"
+rootJointType = "freeflyer"
+##
+#  Information to retrieve urdf and srdf files.
+urdfName = "hrp2_14"
+urdfSuffix = "_reduced_safe"
+srdfSuffix = ""
+pId = tp.ps.numberPaths() -1
+fullBody = FullBody ()
+tPlanning = 0.
+
+fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+fullBody.setJointBounds ("base_joint_xyz",  [-1,2.5,0.5 ,3, 0.5, 0.7])
+fullBody.setJointBounds ("LLEG_JOINT3",  [0.35,2.61799])
+fullBody.setJointBounds ("RLEG_JOINT3", [0.35,2.61799])
+
+
+fullBody.client.basic.robot.setDimensionExtraConfigSpace(tp.extraDof)
+fullBody.client.basic.robot.setExtraConfigSpaceBounds([0,0,0,0,0,0,0,0,0,0,0,0])
+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 = False, 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_init)
+
+
+#~ AFTER loading obstacles
+tStart = time.time()
+
+
+
+rLegOffset = [0,0,-0.0955]
+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, "fixedStep06", 0.01,"_6_DOF",kinematicConstraintsPath = "package://hpp-rbprm-corba/com_inequalities/fullsize/RLEG_JOINT0_com_constraints.obj",limbOffset=rLegLimbOffset,kinematicConstraintsMin=0.6)
+fullBody.runLimbSampleAnalysis(rLegId, "ReferenceConfiguration", True)
+#fullBody.saveLimbDatabase(rLegId, "./db/hrp2_rleg_db.db")
+
+
+lLegOffset = [0,0,-0.0955]
+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, "fixedStep06", 0.01,"_6_DOF",kinematicConstraintsPath = "package://hpp-rbprm-corba/com_inequalities/fullsize/LLEG_JOINT0_com_constraints.obj",limbOffset=lLegLimbOffset,kinematicConstraintsMin=0.6)
+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[0:7] = tp.ps.configAtParam(pId,eps)[0:7] # use this to get the correct orientation
+q_goal = q_ref[::]
+q_goal[0:7] = tp.ps.configAtParam(pId,tp.ps.pathLength(pId)-0.0001)[0:7]
+dir_init = [0,0,0]
+acc_init = [0,0,0]
+dir_goal = tp.ps.configAtParam(pId,eps)[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]
+
+
+# FIXME : test
+q_init[2] = q_ref[2] + 0.0005
+q_goal[2] = q_ref[2] + 0.0005
+
+# Randomly generating a contact configuration at q_init
+fullBody.setStaticStability(True)
+fullBody.setCurrentConfig (q_init)
+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,[lLegId,rLegId])
+fullBody.setEndState(q_goal,[lLegId,rLegId])
+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()
+configsFull = fullBody.interpolate(0.01,pathId=pId,robustnessTreshold = robTreshold, filterStates = True,testReachability=True,quasiStatic=False)
+
+
+
+tInterpolateConfigs = time.time()-tStart
+print "number of configs : ", len(configsFull)
+print "generated in "+str(tInterpolateConfigs)+" s"
+r(configsFull[len(configsFull)-1])
+
+
+
+from generate_contact_sequence import *
+
+beginState = 0
+endState = len(configsFull)-1
+configs=configsFull[beginState:endState+1]
+cs = generateContactSequence(fullBody,configs,beginState, endState,r)
+
+
+
+
+import check_qp
+
+
+#planValid,curves_initGuess,timings_initGuess = check_qp.check_contact_plan(ps,r,pp,fullBody,0,len(configsFull)-1)
+
+
+#cs = generateContactSequence(fullBody,configs,beginState, endState,curves_initGuess =curves_initGuess , timings_initGuess =timings_initGuess)
+
+
+#displayContactSequence(r,configsFull)
+
+
+filename = OUTPUT_DIR + "/" + OUTPUT_SEQUENCE_FILE
+cs.saveAsXML(filename, "ContactSequence")
+print "save contact sequence : ",filename
+
+
+"""
+
+s0 = State(fullBody,q=q_init,limbsIncontact = [rLegId,lLegId])
+s0 = State(fullBody,q=configsFull[10],limbsIncontact = [rLegId,lLegId])
+s1,success = StateHelper.removeContact(s0,lLegId)
+r(s1.q())
+fullBody.isReachableFromState(s1.sId,s1.sId)
+displayOneStepConstraints(r)
+
+
+"""
+
+
+
diff --git a/script/dynamic/slalom_bauzil_hrp2_pathKino.py b/script/dynamic/slalom_bauzil_hrp2_pathKino.py
new file mode 100644
index 0000000..81eaf3c
--- /dev/null
+++ b/script/dynamic/slalom_bauzil_hrp2_pathKino.py
@@ -0,0 +1,158 @@
+## 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 configs.slalom_bauzil 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'
+	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 =  [rLegId,lLegId,rArmId,lArmId]
+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_lleg_rom', ['Support',])
+rbprmBuilder.setAffordanceFilter('hrp2_rleg_rom', ['Support'])
+vMax = 0.2;
+aMax = 0.1;
+extraDof = 6
+
+rbprmBuilder.setJointBounds ("base_joint_xyz", [-1,2.5,0.5 ,3, 0.6, 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.0,0.0,-0.0,0.0])
+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(1.))
+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)
+
+
+from hpp.corbaserver.affordance.affordance import AffordanceTool
+afftool = AffordanceTool ()
+afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.2])
+afftool.loadObstacleModel (packageName, ENV_NAME, ENV_PREFIX, r,reduceSizes=[0.2,0,0])
+#r.loadObstacleModel (packageName, "ground", "planning")
+#afftool.visualiseAffordances('Support', r,r.color.lightYellow)
+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.9, 1, 0.6]; r (q_init)
+q_init [0:3] = [0.5, 2.5, 0.6]; r (q_init)
+q_init[-6:-3] = [0.05,0,0]
+#q_init [0:3] = [0.05, 0.86, 0.59]; r (q_init)
+
+rbprmBuilder.setCurrentConfig (q_init)
+q_goal = q_init [::]
+
+q_goal [0:3] = [2, 2.5, 0.6]; r(q_goal)
+#q_goal [0:3] = [1.83, 0.86, 1.13]; 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("RbprmPathValidation",0.01)
+# Choosing kinodynamic methods : 
+ps.selectSteeringMethod("RBPRMKinodynamic")
+ps.selectDistance("KinodynamicDistance")
+ps.selectPathPlanner("DynamicPlanner")
+ps.addPathOptimizer("RandomShortcutDynamic")
+ps.addPathOptimizer("OrientedPathOptimizer")
+
+#solve the problem :
+r(q_init)
+
+
+#r.solveAndDisplay("rm",1,radiusSphere=0.01)
+
+
+
+t = ps.solve()
+
+
+
+
+
+
+
+
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (rbprmBuilder.client.basic, r)
+pp.dt=0.03
+pp.displayVelocityPath(2)
+r.client.gui.setVisibility("path_2_root","ALWAYS_ON_TOP")
+
+
+
+
+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)
+"""
+
+
diff --git a/script/dynamic/slalom_hyq_interpKino05.py b/script/dynamic/slalom_hyq_interpKino05.py
new file mode 100644
index 0000000..3d9931f
--- /dev/null
+++ b/script/dynamic/slalom_hyq_interpKino05.py
@@ -0,0 +1,140 @@
+#Importing helper class for RBPRM
+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+from hpp.gepetto import Viewer
+
+#calling script darpa_hyq_path to compute root path
+import slalom_hyq_pathKino05 as tp
+
+from os import environ
+ins_dir = environ['DEVEL_DIR']
+db_dir = ins_dir+"/install/share/hyq-rbprm/database/hyq_"
+
+pathId = tp.ps.numberPaths()-1
+
+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.client.basic.robot.setDimensionExtraConfigSpace(tp.extraDof)
+fullBody.setJointBounds ("base_joint_xyz", [-6,6, -2.5, 2.5, 0.0, 1.])
+
+#  Setting a number of sample configurations used
+nbSamples = 20000
+dynamic=True
+
+ps = tp.ProblemSolver(fullBody)
+ps.client.problem.setParameter("aMax",tp.aMax)
+ps.client.problem.setParameter("vMax",tp.vMax)
+r = tp.Viewer (ps,viewerClient=tp.r.client)
+
+rootName = 'base_joint_xyz'
+
+
+def addLimbDb(limbId, heuristicName, loadValues = True, disableEffectorCollision = False):
+	fullBody.addLimbDatabase(str(db_dir+limbId+'.db'), limbId, heuristicName,loadValues, disableEffectorCollision)
+
+rLegId = 'rfleg'
+lLegId = 'lhleg'
+rarmId = 'rhleg'
+larmId = 'lfleg'
+
+addLimbDb(rLegId, "manipulability")
+addLimbDb(lLegId, "manipulability")
+addLimbDb(rarmId, "manipulability")
+addLimbDb(larmId, "manipulability")
+
+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(pathId,tp.ps.pathLength(pathId))[0:7]
+dir_init = tp.ps.configAtParam(pathId,0.01)[7:10]
+acc_init = tp.ps.configAtParam(pathId,0.01)[10:13]
+dir_goal = tp.ps.configAtParam(pathId,tp.ps.pathLength(pathId))[7:10]
+acc_goal = tp.ps.configAtParam(pathId,tp.ps.pathLength(pathId))[10:13]
+configSize = fullBody.getConfigSize() -fullBody.client.basic.robot.getDimensionExtraConfigSpace()
+
+# 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] = acc_goal[::]
+
+
+fullBody.setStaticStability(False)
+# Randomly generating a contact configuration at q_init
+fullBody.setCurrentConfig (q_init)
+q_init = fullBody.generateContacts(q_init,dir_init,acc_init,2)
+
+# Randomly generating a contact configuration at q_end
+fullBody.setCurrentConfig (q_goal)
+q_goal = fullBody.generateContacts(q_goal, dir_goal,acc_goal,2)
+
+
+# specifying the full body configurations as start and goal state of the problem
+fullBody.setStartState(q_init,[larmId,rLegId,rarmId,lLegId])
+fullBody.setEndState(q_goal,[larmId,rLegId,rarmId,lLegId])
+
+
+r(q_init)
+# computing the contact sequence
+
+configs = fullBody.interpolate(0.08,pathId=pathId,robustnessTreshold = 0, filterStates = True)
+
+
+print "number of configs =", len(configs)
+r(configs[-1])
+
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (fullBody.client.basic, r)
+
+import fullBodyPlayer
+player = fullBodyPlayer.Player(fullBody,pp,tp,configs,draw=True,optim_effector=False,use_velocity=dynamic,pathId = pathId)
+
+#player.displayContactPlan()
+
+r(configs[5])
+player.interpolate(5,99)
+
+#player.play()
+
+
+
+"""
+
+camera = [0.5681925415992737,
+ -6.707448482513428,
+ 2.5206544399261475,
+ 0.8217507600784302,
+ 0.5693002343177795,
+ 0.020600343123078346,
+ 0.01408931240439415]
+r.client.gui.setCameraTransform(0,camera)
+
+"""
+
+
+
+
+"""
+import hpp.corbaserver.rbprm.tools.cwc_trajectory
+import hpp.corbaserver.rbprm.tools.path_to_trajectory
+import hpp.corbaserver.rbprm.tools.cwc_trajectory_helper
+
+reload(hpp.corbaserver.rbprm.tools.cwc_trajectory)
+reload(hpp.corbaserver.rbprm.tools.path_to_trajectory)
+reload(hpp.corbaserver.rbprm.tools.cwc_trajectory_helper)
+reload(fullBodyPlayer)
+
+
+"""
+
+
diff --git a/script/dynamic/slalom_hyq_pathKino05.py b/script/dynamic/slalom_hyq_pathKino05.py
new file mode 100644
index 0000000..8e93c04
--- /dev/null
+++ b/script/dynamic/slalom_hyq_pathKino05.py
@@ -0,0 +1,180 @@
+## 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 = 0.5;
+aMax = 5;
+extraDof = 6
+# 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.3,5.1, -1.75, 1.75, 0.6, 0.65])
+# 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. (z, y, x)
+#rbprmBuilder.boundSO3([-3,3,-0.1,0.1,-0.1,0.1])
+rbprmBuilder.client.basic.robot.setDimensionExtraConfigSpace(extraDof)
+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.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
+afftool.loadObstacleModel (packageName, "slalom", "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[3:7] = [1,0,0,0]
+q_init [0:3] = [-5, 1.2, 0.63]; 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.63]; 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("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()
+
+q_far = q_init[::]
+q_far[2] = -3
+r(q_far)
+
+r.solveAndDisplay("rm",1,0.01)
+
+
+#ps.solve ()
+
+
+
+"""
+camera = [0.6293167471885681,
+ -9.560577392578125,
+ 10.504343032836914,
+ 0.9323806762695312,
+ 0.36073973774909973,
+ 0.008668755181133747,
+ 0.02139890193939209]
+r.client.gui.setCameraTransform(0,camera)
+"""
+
+
+r.client.gui.removeFromGroup("rm",r.sceneName)
+r.client.gui.removeFromGroup("rmstart",r.sceneName)
+r.client.gui.removeFromGroup("rmgoal",r.sceneName)
+for i in range(0,ps.numberNodes()):
+  r.client.gui.removeFromGroup("vecRM"+str(i),r.sceneName)
+
+
+
+
+
+# Playing the computed path
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (rbprmBuilder.client.basic, r)
+pp.dt=0.03
+pp.displayVelocityPath(0)
+r.client.gui.setVisibility("path_0_root","ALWAYS_ON_TOP")
+#display path
+pp.speed=1
+#pp (0)
+
+#display path with post-optimisation
+
+
+ps.client.problem.extractPath(0,3.1,24.24)
+r.client.gui.removeFromGroup("path_0_root",r.sceneName)
+pp.displayVelocityPath(1)
+r.client.gui.setVisibility("path_1_root","ALWAYS_ON_TOP")
+#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)
+"""
+
+"""
+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)
+
+
+
+"""
+
+
+
diff --git a/script/dynamic/stair_bauzil_hrp2_interp.py b/script/dynamic/stair_bauzil_hrp2_interp.py
index 796c39e..6b6cb7b 100644
--- a/script/dynamic/stair_bauzil_hrp2_interp.py
+++ b/script/dynamic/stair_bauzil_hrp2_interp.py
@@ -13,7 +13,7 @@ rootJointType = "freeflyer"
 ##
 #  Information to retrieve urdf and srdf files.
 urdfName = "hrp2_14"
-urdfSuffix = "_reduced"
+urdfSuffix = "_reduced_safe"
 srdfSuffix = ""
 
 fullBody = FullBody ()
@@ -29,69 +29,52 @@ ps.client.problem.setParameter("aMax",omniORB.any.to_any(tp.aMax))
 ps.client.problem.setParameter("vMax",omniORB.any.to_any(tp.vMax))
 ps.client.problem.setParameter("friction",tp.mu)
 
-r = tp.Viewer (ps,viewerClient=tp.r.client)
+r = tp.Viewer (ps,viewerClient=tp.r.client, displayCoM = True)
+tStart = time.time()
+#~ AFTER loading obstacles
+
 
 #~ AFTER loading obstacles
 rLegId = 'hrp2_rleg_rom'
+lLegId = 'hrp2_lleg_rom'
+tStart = time.time()
+
+
 rLeg = 'RLEG_JOINT0'
-rLegOffset = [0,0,-0.105]
+rLegOffset = [0,0,-0.0955]
+rLegLimbOffset=[0,0,-0.035]#0.035
 rLegNormal = [0,0,1]
 rLegx = 0.09; rLegy = 0.05
-fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 50000, "manipulability", 0.01,"_6_DOF")
 
-lLegId = 'hrp2_lleg_rom'
+fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 100000, "fixedStep1", 0.01,"_6_DOF",limbOffset=rLegLimbOffset,kinematicConstraintsPath = "package://hpp-rbprm-corba/com_inequalities/fullSize/RLEG_JOINT0_com_constraints.obj",kinematicConstraintsMin=0.)
+fullBody.runLimbSampleAnalysis(rLegId, "ReferenceConfiguration", True)
+#fullBody.saveLimbDatabase(rLegId, "./db/hrp2_rleg_db.db")
+
 lLeg = 'LLEG_JOINT0'
-lLegOffset = [0,0,-0.105]
+lLegOffset = [0,0,-0.0955]
+lLegLimbOffset=[0,0,0.035]
 lLegNormal = [0,0,1]
 lLegx = 0.09; lLegy = 0.05
-fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 50000, "manipulability", 0.01,"_6_DOF")
+fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 100000, "fixedStep1", 0.01,"_6_DOF",limbOffset=lLegLimbOffset,kinematicConstraintsPath = "package://hpp-rbprm-corba/com_inequalities/fullSize/LLEG_JOINT0_com_constraints.obj",kinematicConstraintsMin=0.)
+fullBody.runLimbSampleAnalysis(lLegId, "ReferenceConfiguration", True)
+#fullBody.saveLimbDatabase(lLegId, "./db/hrp2_lleg_db.db")
+
 
 rarmId = 'hrp2_rarm_rom'
 rarm = 'RARM_JOINT0'
 rHand = 'RARM_JOINT5'
-rArmOffset = [0,0,-0.1]
-rArmNormal = [0,0,1]
+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, 100000, "manipulability", 0.01, "_6_DOF", True)
 
-"""
 
-#~ 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, 0.05)
-
-rKneeId = '0RKnee'
-rLeg = 'RLEG_JOINT0'
-rKnee = 'RLEG_JOINT3'
-rLegOffset = [0.105,0.055,0.017]
-rLegNormal = [-1,0,0]
-rLegx = 0.05; rLegy = 0.05
-#~ fullBody.addLimb(rKneeId, rLeg,rKnee,rLegOffset,rLegNormal, rLegx, rLegy, 10000, 0.01)
-#~ 
-lKneeId = '1LKnee'
-lLeg = 'LLEG_JOINT0'
-lKnee = 'LLEG_JOINT3'
-lLegOffset = [0.105,0.055,0.017]
-lLegNormal = [-1,0,0]
-lLegx = 0.05; lLegy = 0.05
-#~ fullBody.addLimb(lKneeId,lLeg,lKnee,lLegOffset,lLegNormal, lLegx, lLegy, 10000, 0.01)
- #~ 
-
-#~ fullBody.runLimbSampleAnalysis(rLegId, "jointLimitsDistance", True)
-#~ fullBody.runLimbSampleAnalysis(lLegId, "jointLimitsDistance", True)
 
-#~ fullBody.client.basic.robot.setJointConfig('LARM_JOINT0',[1])
-#~ fullBody.client.basic.robot.setJointConfig('RARM_JOINT0',[-1])
+tGenerate =  time.time() - tStart
+print "generate databases in : "+str(tGenerate)+" s"
 
 
-"""
-
 
 q_0 = fullBody.getCurrentConfig(); 
 #~ fullBody.createOctreeBoxes(r.client.gui, 1, rarmId, q_0,)
@@ -104,8 +87,8 @@ fullBody.setReferenceConfig (q_ref)
 
 configSize = fullBody.getConfigSize() -fullBody.client.basic.robot.getDimensionExtraConfigSpace()
 
-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(0))[0:7]
+q_init = fullBody.getCurrentConfig(); q_init[0:3] = tp.ps.configAtParam(0,0.01)[0:3] # use this to get the correct orientation
+q_goal = fullBody.getCurrentConfig(); q_goal[0:3] = tp.ps.configAtParam(0,tp.ps.pathLength(0))[0:3]
 dir_init = tp.ps.configAtParam(0,0.01)[tp.indexECS:tp.indexECS+3]
 acc_init = tp.ps.configAtParam(0,0.01)[tp.indexECS+3:tp.indexECS+6]
 dir_goal = tp.ps.configAtParam(0,tp.ps.pathLength(0))[tp.indexECS:tp.indexECS+3]
@@ -118,18 +101,22 @@ fullBody.runLimbSampleAnalysis(lLegId, "ReferenceConfiguration", True)
 
 
 # FIXME : test
-q_init[2] = q_init[2]+0.02
-q_goal[2] = q_goal[2]+0.02
+#q_init[2] = q_init[2]+0.0
+#q_goal[2] = q_goal[2]+0.02
+q_init[2] = q_ref[2] + 0.01
+q_goal[2] = 1.25
+
 
-q_init[0:3]=[0.28994563306701016,-0.82,0.6191688248477717]
 
 
 fullBody.setStaticStability(True)
+"""
 # Randomly generating a contact configuration at q_init
 fullBody.setCurrentConfig (q_init)
 r(q_init)
 q_init = fullBody.generateContacts(q_init,dir_init,acc_init)
 r(q_init)
+"""
 
 # Randomly generating a contact configuration at q_end
 fullBody.setCurrentConfig (q_goal)
@@ -146,11 +133,12 @@ r(q_init)
 
 
 fullBody.setStartState(q_init,[rLegId,lLegId])
-fullBody.setEndState(q_goal,[rLegId,lLegId])
-
+fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId])
 
 
-configs = fullBody.interpolate(0.03,pathId=0,robustnessTreshold = 2, filterStates = True)
+tStart = time.time()
+configs = fullBody.interpolate(0.01,pathId=0,robustnessTreshold = 0, filterStates = True,testReachability=False,quasiStatic=False)
+tInterpolateConfigs = time.time()-tStart
 print "number of configs :", len(configs)
 r(configs[-1])
 
@@ -166,26 +154,16 @@ player = Player(fullBody,pp,tp,configs,draw=False,optim_effector=False,use_veloc
 player.displayContactPlan()
 
 
+beginState=0
+endState=len(configs)-1
 
 
-print "####################################"
-print "#            SOLVING P2 :          #"
-print "#               DONE               #"
-print "####################################"
-print "# Writing contact sequence file :  #"
-print "####################################"
-
-from planning.configs.stairs_config import *
+from configs.stairs_config import *
 from generate_contact_sequence import *
-cs = generateContactSequence(fullBody,configs[:5],r)
+cs = generateContactSequence(fullBody,configs,beginState,endState,r)
 filename = OUTPUT_DIR + "/" + OUTPUT_SEQUENCE_FILE
-cs.saveAsXML(filename, CONTACT_SEQUENCE_XML_TAG)
+cs.saveAsXML(filename, "ContactSequence")
 print "save contact sequence : ",filename
-print "####################################"
-print "# Writing contact sequence file :  #"
-print "#               DONE               #"
-print "####################################"
-
 
 
 
diff --git a/script/dynamic/stair_bauzil_hrp2_path.py b/script/dynamic/stair_bauzil_hrp2_path.py
index 172e5cd..60b9eca 100644
--- a/script/dynamic/stair_bauzil_hrp2_path.py
+++ b/script/dynamic/stair_bauzil_hrp2_path.py
@@ -3,7 +3,7 @@ from hpp.gepetto import Viewer
 from hpp.corbaserver import Client
 from hpp.corbaserver.robot import Robot as Parent
 from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
-from planning.configs.stairs_config import *
+from configs.stairs_config import *
 import omniORB.any
 
 class Robot (Parent):
@@ -72,7 +72,7 @@ from hpp.corbaserver.affordance.affordance import AffordanceTool
 afftool = AffordanceTool ()
 afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
 afftool.loadObstacleModel (packageName, "stair_bauzil", "planning", r,reduceSizes=[0.1,0])
-#afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
+afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
 
 
 q_init = rbprmBuilder.getCurrentConfig ();
@@ -84,7 +84,7 @@ q_init[8] = 0
 rbprmBuilder.setCurrentConfig (q_init); r (q_init)
 
 q_goal = q_init [::]
-q_goal [3:7] =  [ 0.98877108,  0.        ,  0.14943813,  0.        ]
+#q_goal [3:7] =  [ 0.98877108,  0.        ,  0.14943813,  0.        ]
 q_goal[8] = 0
 q_goal [0:3] = [1.40, -0.82, 1.18]; r (q_goal)
 #~ q_goal [0:3] = [1.2, -0.65, 1.1]; r (q_goal)
@@ -119,7 +119,7 @@ ps.client.problem.finishSolveStepByStep()
 from hpp.gepetto import PathPlayer
 pp = PathPlayer (rbprmBuilder.client.basic, r)
 #r.client.gui.removeFromGroup("rm",r.sceneName)
-#pp.displayVelocityPath(0)
+pp.displayVelocityPath(0)
 pp.speed=0.3
 #pp(0)
 
diff --git a/script/dynamic/stairs10_hrp2_interStatic.py b/script/dynamic/stairs10_hrp2_interStatic.py
index 95e1e46..ad6f2e1 100644
--- a/script/dynamic/stairs10_hrp2_interStatic.py
+++ b/script/dynamic/stairs10_hrp2_interStatic.py
@@ -7,7 +7,7 @@ import time
 import omniORB.any
 from constraint_to_dae import *
 from display_tools import *
-from planning.configs.stairs10_bauzil_stairs import *
+from configs.stairs10_bauzil_stairs import *
 from disp_bezier import *
 from hpp.corbaserver.rbprm.rbprmstate import State,StateHelper
 
@@ -17,16 +17,17 @@ rootJointType = "freeflyer"
 ##
 #  Information to retrieve urdf and srdf files.
 urdfName = "hrp2_14"
-urdfSuffix = "_reduced_safe20"
-srdfSuffix = ""
+#urdfSuffix = "_reduced_safe20"
+urdfSuffix = "_reduced"
+srdfSuffix = "_disable_leg_autocol"
 pId = tp.ps.numberPaths() -1
 fullBody = FullBody ()
 tPlanning = 0.
 
 fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
 fullBody.setJointBounds ("base_joint_xyz",  [0,2,0.6 ,1.1, 0.35, 1.5])
-fullBody.setJointBounds ("LLEG_JOINT3",  [0.4,2.61799])
-fullBody.setJointBounds ("RLEG_JOINT3", [0.4,2.61799])
+fullBody.setJointBounds ("LLEG_JOINT3",  [0.3,2.61799])
+fullBody.setJointBounds ("RLEG_JOINT3", [0.3,2.61799])
 
 
 fullBody.client.basic.robot.setDimensionExtraConfigSpace(tp.extraDof)
@@ -66,7 +67,7 @@ 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, "fixedStep06", 0.01,"_6_DOF",limbOffset=rLegLimbOffset,kinematicConstraintsMin=0.5)
+fullBody.addLimb(rLegId,rleg,'',rLegOffset,rLegNormal, rLegx, rLegy, 50000, "fixedStep06", 0.01,"_6_DOF",limbOffset=rLegLimbOffset,kinematicConstraintsPath = "package://hpp-rbprm-corba/com_inequalities/fullSize/RLEG_JOINT0_com_constraints.obj",kinematicConstraintsMin=0.2)
 fullBody.runLimbSampleAnalysis(rLegId, "ReferenceConfiguration", True)
 #fullBody.saveLimbDatabase(rLegId, "./db/hrp2_rleg_db.db")
 
@@ -76,7 +77,7 @@ 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, "fixedStep06", 0.01,"_6_DOF",limbOffset=lLegLimbOffset,kinematicConstraintsMin=0.5)
+fullBody.addLimb(lLegId,lleg,'',lLegOffset,rLegNormal, lLegx, lLegy, 50000, "fixedStep06", 0.01,"_6_DOF",limbOffset=lLegLimbOffset,kinematicConstraintsPath = "package://hpp-rbprm-corba/com_inequalities/fullSize/LLEG_JOINT0_com_constraints.obj",kinematicConstraintsMin=0.2)
 fullBody.runLimbSampleAnalysis(lLegId, "ReferenceConfiguration", True)
 #fullBody.saveLimbDatabase(lLegId, "./db/hrp2_lleg_db.db")
 
@@ -120,7 +121,7 @@ q_goal[configSize+3:configSize+6] = [0,0,0]
 
 # FIXME : test
 q_init[2] = q_ref[2] + 0.0005
-q_goal[2] = q_ref[2] +0.6005
+q_goal[2] = q_ref[2] +0.3005
 
 # Randomly generating a contact configuration at q_init
 fullBody.setStaticStability(True)
@@ -150,11 +151,9 @@ pp = PathPlayer (fullBody.client.basic, r)
 import fullBodyPlayerHrp2
 
 tStart = time.time()
-configsFull = fullBody.interpolate(0.01,pathId=pId,robustnessTreshold = robTreshold, filterStates = True,testReachability=True,quasiStatic=True)
-
-
-
+configsFull = fullBody.interpolate(0.01,pathId=pId,robustnessTreshold = robTreshold, filterStates = True,testReachability=True,quasiStatic=False)
 tInterpolateConfigs = time.time()-tStart
+
 print "number of configs : ", len(configsFull)
 print "generated in "+str(tInterpolateConfigs)+" s"
 r(configsFull[len(configsFull)-1])
@@ -177,7 +176,6 @@ filename = OUTPUT_DIR + "/" + OUTPUT_SEQUENCE_FILE
 cs.saveAsXML(filename, "ContactSequence")
 print "save contact sequence : ",filename
 
-
 """
 
 s0 = State(fullBody,q=q_init,limbsIncontact = [rLegId,lLegId])
diff --git a/script/dynamic/stairs10_hrp2_interStatic_noCroc.py b/script/dynamic/stairs10_hrp2_interStatic_noCroc.py
new file mode 100644
index 0000000..2cec766
--- /dev/null
+++ b/script/dynamic/stairs10_hrp2_interStatic_noCroc.py
@@ -0,0 +1,192 @@
+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.gepetto import Viewer
+from tools import *
+import stairs10_hrp2_pathKino as tp
+import time
+import omniORB.any
+from constraint_to_dae import *
+from display_tools import *
+from configs.stairs10_bauzil_stairs import *
+from disp_bezier import *
+from hpp.corbaserver.rbprm.rbprmstate import State,StateHelper
+
+packageName = "hrp2_14_description"
+meshPackageName = "hrp2_14_description"
+rootJointType = "freeflyer"
+##
+#  Information to retrieve urdf and srdf files.
+urdfName = "hrp2_14"
+#urdfSuffix = "_reduced_safe20"
+urdfSuffix = "_reduced"
+srdfSuffix = ""
+pId = tp.ps.numberPaths() -1
+fullBody = FullBody ()
+tPlanning = 0.
+
+fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+fullBody.setJointBounds ("base_joint_xyz",  [0,2,0.6 ,1.1, 0.35, 1.5])
+fullBody.setJointBounds ("LLEG_JOINT3",  [0.3,2.61799])
+fullBody.setJointBounds ("RLEG_JOINT3", [0.3,2.61799])
+
+
+fullBody.client.basic.robot.setDimensionExtraConfigSpace(tp.extraDof)
+fullBody.client.basic.robot.setExtraConfigSpaceBounds([0,0,0,0,0,0,0,0,0,0,0,0])
+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 = False, displayCoM = True)
+
+view = [0.8,
+ 0.75,
+ 4.85,
+ -0.7071,
+ 0,
+ 0,
+ 0.7071]
+
+r.client.gui.setCameraTransform(0,view)
+
+
+
+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_init)
+
+
+#~ AFTER loading obstacles
+tStart = time.time()
+
+
+
+rLegOffset = [0,0,-0.0955]
+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, 50000, "forward", 0.01,"_6_DOF",limbOffset=rLegLimbOffset,kinematicConstraintsPath = "package://hpp-rbprm-corba/com_inequalities/fullsize/RLEG_JOINT0_com_constraints.obj",kinematicConstraintsMin=0.3)
+fullBody.runLimbSampleAnalysis(rLegId, "ReferenceConfiguration", True)
+#fullBody.saveLimbDatabase(rLegId, "./db/hrp2_rleg_db.db")
+
+
+lLegOffset = [0,0,-0.0955]
+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, 50000, "forward", 0.01,"_6_DOF",limbOffset=lLegLimbOffset,kinematicConstraintsPath = "package://hpp-rbprm-corba/com_inequalities/fullsize/LLEG_JOINT0_com_constraints.obj",kinematicConstraintsMin=0.3)
+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[0:7] = tp.ps.configAtParam(pId,eps)[0:7] # use this to get the correct orientation
+q_goal = q_ref[::]
+q_goal[0:7] = tp.ps.configAtParam(pId,tp.ps.pathLength(pId)-0.0001)[0:7]
+dir_init = [0,0,0]
+acc_init = [0,0,0]
+dir_goal = tp.ps.configAtParam(pId,eps)[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]
+
+
+# FIXME : test
+q_init[2] = q_ref[2] + 0.0005
+q_goal[2] = q_ref[2] +0.3005
+
+# Randomly generating a contact configuration at q_init
+fullBody.setStaticStability(True)
+fullBody.setCurrentConfig (q_init)
+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,[lLegId,rLegId])
+fullBody.setEndState(q_goal,[lLegId,rLegId])
+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()
+configsFull = fullBody.interpolate(0.01,pathId=pId,robustnessTreshold = robTreshold, filterStates = True,testReachability=False,quasiStatic=False)
+tInterpolateConfigs = time.time()-tStart
+
+print "number of configs : ", len(configsFull)
+print "generated in "+str(tInterpolateConfigs)+" s"
+r(configsFull[len(configsFull)-1])
+
+
+
+
+
+from generate_contact_sequence import *
+
+beginState = 0
+endState = len(configsFull)-1
+configs=configsFull[beginState:endState+1]
+cs = generateContactSequence(fullBody,configs,beginState, endState,r)
+
+#displayContactSequence(r,configsFull)
+
+
+filename = OUTPUT_DIR + "/" + OUTPUT_SEQUENCE_FILE
+cs.saveAsXML(filename, "ContactSequence")
+print "save contact sequence : ",filename
+
+"""
+
+s0 = State(fullBody,q=q_init,limbsIncontact = [rLegId,lLegId])
+s0 = State(fullBody,q=configsFull[10],limbsIncontact = [rLegId,lLegId])
+s1,success = StateHelper.removeContact(s0,lLegId)
+r(s1.q())
+fullBody.isReachableFromState(s1.sId,s1.sId)
+displayOneStepConstraints(r)
+
+
+"""
+
+
+
diff --git a/script/dynamic/stairs10_hrp2_pathKino.py b/script/dynamic/stairs10_hrp2_pathKino.py
index 3d8df5c..4f5844d 100644
--- a/script/dynamic/stairs10_hrp2_pathKino.py
+++ b/script/dynamic/stairs10_hrp2_pathKino.py
@@ -6,7 +6,7 @@ from hpp.gepetto import Viewer
 import time
 import math
 import omniORB.any
-from planning.configs.stairs10_bauzil_stairs import *
+from configs.stairs10_bauzil_stairs import *
 
 from hpp.corbaserver import Client
 from hpp.corbaserver.robot import Robot as Parent
@@ -124,12 +124,13 @@ q_far[2] = -3
 r(q_far)
 
 
+ps.client.problem.extractPath(0,0,5.6)
 
 
 from hpp.gepetto import PathPlayer
 pp = PathPlayer (rbprmBuilder.client.basic, r)
 pp.dt=0.03
-pp.displayVelocityPath(0)
+pp.displayVelocityPath(1)
 r.client.gui.setVisibility("path_0_root","ALWAYS_ON_TOP")
 
 
diff --git a/script/dynamic/test_flatGround.py b/script/dynamic/test_flatGround.py
new file mode 100644
index 0000000..0d3203f
--- /dev/null
+++ b/script/dynamic/test_flatGround.py
@@ -0,0 +1,130 @@
+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.gepetto import Viewer
+import time
+from constraint_to_dae import *
+from hpp.corbaserver.rbprm.rbprmstate import State,StateHelper
+from display_tools import *
+import flatGround_hrp2_pathKino as tp
+import time
+
+tPlanning = tp.tPlanning
+
+
+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, -1.5, 1.5, 0.5, 0.8])
+fullBody.client.basic.robot.setDimensionExtraConfigSpace(tp.extraDof)
+fullBody.client.basic.robot.setExtraConfigSpaceBounds([-0,0,-0,0,-0,0,0,0,0,0,0,0])
+ps = tp.ProblemSolver( fullBody )
+ps.client.problem.setParameter("aMax",tp.aMax)
+ps.client.problem.setParameter("vMax",tp.vMax)
+r = tp.Viewer (ps,viewerClient=tp.r.client,displayArrows = True, displayCoM = True)
+
+
+q_init =[0.1, -0.82, 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)
+qfar=q_ref[::]
+qfar[2] = -5
+
+#~ AFTER loading obstacles
+rLegId = 'hrp2_rleg_rom'
+lLegId = 'hrp2_lleg_rom'
+tStart = time.time()
+fullBody.setReferenceConfig (q_ref)
+
+rLeg = 'RLEG_JOINT0'
+rLegOffset = [0,0,-0.105]
+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, "fixedStep1", 0.01,"_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.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, "fixedStep1", 0.01,"_6_DOF",limbOffset=lLegLimbOffset)
+fullBody.runLimbSampleAnalysis(lLegId, "ReferenceConfiguration", True)
+#fullBody.saveLimbDatabase(lLegId, "./db/hrp2_lleg_db.db")
+fullBody.setReferenceConfig (q_ref)
+## Add arms (not used for contact) : 
+
+
+rarmId = 'hrp2_rarm_rom'
+rarm = 'RARM_JOINT0'
+rHand = 'RARM_JOINT5'
+fullBody.addNonContactingLimb(rarmId,rarm,rHand, 10000)
+fullBody.runLimbSampleAnalysis(rarmId, "ReferenceConfiguration", True)
+larmId = 'hrp2_larm_rom'
+larm = 'LARM_JOINT0'
+lHand = 'LARM_JOINT5'
+fullBody.addNonContactingLimb(larmId,larm,lHand, 10000)
+fullBody.runLimbSampleAnalysis(larmId, "ReferenceConfiguration", True)
+
+
+tGenerate =  time.time() - tStart
+print "generate databases in : "+str(tGenerate)+" 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.01)[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.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]
+
+
+# FIXME : test
+q_init[2] = q_ref[2]+0.01
+q_goal[2] = q_ref[2]+0.01
+
+fullBody.setStaticStability(True)
+# Randomly generating a contact configuration at q_init
+fullBody.setCurrentConfig (q_init)
+r(q_init)
+#q_init = fullBody.generateContacts(q_init,dir_init,acc_init,robTreshold)
+r(q_init)
+
+
+
+
+
+fullBody.isDynamicallyReachableFromState(s.sId,s2.sId,timings=[0.4,0.6,0.4])
+
+
+
+
+
+
diff --git a/script/dynamic/test_kinematics_muscod.py b/script/dynamic/test_kinematics_muscod.py
new file mode 100644
index 0000000..17d24aa
--- /dev/null
+++ b/script/dynamic/test_kinematics_muscod.py
@@ -0,0 +1,59 @@
+# use this script after a contact sequence AND a trajectory from muscod has been loaded
+
+#from hpp.corbaserver.rbprm.tools.obj_to_constraints import *
+from hpp.corbaserver.rbprm.tools.com_constraints import *
+
+
+
+
+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},
+                        larmId : {'file': "hrp2/LA_com.ineq", 'effector' : lHand} }
+
+#DEFINE state and config: 
+
+state1=1
+state2=2
+
+config1=fullBody.getConfigAtState(state1)
+config2=fullBody.getConfigAtState(state2)
+
+
+#res = get_com_constraint(fullBody, state, config, limbsCOMConstraints, interm = False, exceptList = [])
+res1 = get_com_constraint(fullBody, state1, config1, limbsCOMConstraints, interm = False)
+res1Bis = get_com_constraint(fullBody, state1, config1, limbsCOMConstraints, interm = True)
+res2Bis = get_com_constraint(fullBody, state2, config2, limbsCOMConstraints, interm = True)
+res2 = get_com_constraint(fullBody, state2, config2, limbsCOMConstraints, interm = False)
+
+
+
+# DEFINE com :
+
+com1 = c1[0].tolist()[0]
+com1Bis = c2[0].tolist()[0]
+com2Bis = c3[0].tolist()[0]   
+com2 = c3[-1].tolist()[0]
+
+
+
+value = np.max(res1[0].dot(com1) - res1[1])
+print "value 1 = "+str(value)
+if value > 0. :
+    print "Infeasible."
+
+
+value = np.max(res1Bis[0].dot(com1Bis) - res1Bis[1])
+print "value 1 bis = "+str(value)
+if value > 0. :
+    print "Infeasible."
+
+value = np.max(res2Bis[0].dot(com2Bis) - res2Bis[1])
+print "value 2 bis = "+str(value)
+if value > 0. :
+    print "Infeasible."
+    
+value = np.max(res2[0].dot(com2) - res2[1])
+print "value 2 = "+str(value)
+if value > 0. :
+    print "Infeasible."            
\ No newline at end of file
-- 
GitLab