diff --git a/script/scenarios/sandbox/log.txt b/script/scenarios/sandbox/log.txt
new file mode 100644
index 0000000000000000000000000000000000000000..55cabad85a37088ca178f791817a9bdd3e6f3ddb
--- /dev/null
+++ b/script/scenarios/sandbox/log.txt
@@ -0,0 +1,7 @@
+path computation 24
+path computation 2
+path computation 3
+path computation 2
+path computation 2
+path computation 2
+path computation 2
diff --git a/script/scenarios/sandbox/run.sh b/script/scenarios/sandbox/run.sh
new file mode 100755
index 0000000000000000000000000000000000000000..c249455bcb37f8e2ae1c60795925cc6660ab54f3
--- /dev/null
+++ b/script/scenarios/sandbox/run.sh
@@ -0,0 +1,8 @@
+#!/bin/bash
+
+gepetto-viewer-server &
+hpp-rbprm-server &
+ipython -i --no-confirm-exit ./$1
+
+pkill -f 'gepetto-viewer-server'
+pkill -f 'hpp-rbprm-server'
diff --git a/script/scenarios/sandbox/rund.sh b/script/scenarios/sandbox/rund.sh
new file mode 100755
index 0000000000000000000000000000000000000000..eab7645220a9bb27f8fee067d65204a90e897868
--- /dev/null
+++ b/script/scenarios/sandbox/rund.sh
@@ -0,0 +1,6 @@
+#!/bin/bash
+
+gepetto-viewer-server &
+ipython -i --no-confirm-exit ./$1
+
+pkill -f 'gepetto-viewer-server'
diff --git a/script/scenarios/sandbox/stair_bauzil_hrp2_interp.py b/script/scenarios/sandbox/stair_bauzil_hrp2_interp.py
new file mode 100755
index 0000000000000000000000000000000000000000..d94ab793bc454aa85274f22d28962f5defb22a2a
--- /dev/null
+++ b/script/scenarios/sandbox/stair_bauzil_hrp2_interp.py
@@ -0,0 +1,229 @@
+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.gepetto import Viewer
+
+import stair_bauzil_hrp2_path as tp
+import time
+
+
+
+packageName = "hrp2_14_description"
+meshPackageName = "hrp2_14_description"
+rootJointType = "freeflyer"
+##
+# Information to retrieve urdf and srdf files.
+urdfName = "hrp2_14"
+urdfSuffix = "_reduced"
+srdfSuffix = ""
+
+fullBody = FullBody ()
+
+fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+fullBody.setJointBounds ("base_joint_xyz", [-0.135,2, -1, 1, 0, 2.2])
+
+
+ps = tp.ProblemSolver( fullBody )
+r = tp.Viewer (ps, viewerClient=tp.r.client)
+
+#~ AFTER loading obstacles
+rLegId = '0rLeg'
+rLeg = 'RLEG_JOINT0'
+rLegOffset = [0,-0.105,0,]
+rLegNormal = [0,1,0]
+rLegx = 0.09; rLegy = 0.05
+fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 10000, "manipulability", 0.1)
+
+lLegId = '1lLeg'
+lLeg = 'LLEG_JOINT0'
+lLegOffset = [0,-0.105,0]
+lLegNormal = [0,1,0]
+lLegx = 0.09; lLegy = 0.05
+fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 10000, "manipulability", 0.1)
+
+rarmId = '3Rarm'
+rarm = 'RARM_JOINT0'
+rHand = 'RARM_JOINT5'
+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, 10000, "manipulability", 0.05, "_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, "manipulability", 0.05, "_6_DOF", True)
+
+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])
+
+q_0 = fullBody.getCurrentConfig();
+#~ fullBody.createOctreeBoxes(r.client.gui, 1, rarmId, q_0,)
+q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.q_init[0:7]
+q_goal = fullBody.getCurrentConfig(); q_goal[0:7] = tp.q_goal[0:7]
+
+
+fullBody.setCurrentConfig (q_init)
+q_init = [
+ 0.1, -0.82, 0.648702, 1.0, 0.0 , 0.0, 0.0, # Free flyer 0-6
+ 0.0, 0.0, 0.0, 0.0, # CHEST HEAD 7-10
+ 0.261799388, 0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17, # LARM 11-17
+ 0.261799388, -0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17, # RARM 18-24
+ 0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0, # LLEG 25-30
+ 0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0, # RLEG 31-36
+ ]; r (q_init)
+
+fullBody.setCurrentConfig (q_goal)
+#~ r(q_goal)
+q_goal = fullBody.generateContacts(q_goal, [0,0,1])
+#~ r(q_goal)
+
+fullBody.setStartState(q_init,[rLegId,lLegId]) #,rarmId,larmId])
+fullBody.setEndState(q_goal,[rLegId,lLegId])#,rarmId,larmId])
+#~
+#~ configs = fullBody.interpolate(0.1)
+configs = fullBody.interpolate(0.1)
+#~ configs = fullBody.interpolate(0.15)
+i = 0;
+fullBody.draw(configs[i],r); i=i+1; i-1
+
+r.loadObstacleModel ('hpp-rbprm-corba', "stair_bauzil", "contact")
+#~ fullBody.exportAll(r, configs, 'stair_bauzil_hrp2_robust_2');
+#~ fullBody.client.basic.robot.setJointConfig('LLEG_JOINT0',[-1])
+#~ q_0 = fullBody.getCurrentConfig();
+#~ fullBody.draw(q_0,r);
+#~ print(fullBody.client.rbprm.rbprm.getOctreeTransform(rarmId, q_0))
+#~
+#~
+#~ fullBody.client.basic.robot.setJointConfig('LLEG_JOINT0',[1])
+#~ q_0 = fullBody.getCurrentConfig();
+#~ fullBody.draw(q_0,r);
+#~ print(fullBody.client.rbprm.rbprm.getOctreeTransform(rarmId, q_0))
+#~ q_init = fullBody.generateContacts(q_init, [0,0,-1]); r (q_init)
+
+#~ f1 = open("secondchoice","w+")
+#~ f1 = open("hrp2_stair_not_robust_configs","w+")
+#~ f1.write(str(configs))
+#~ f1.close()
+
+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} }
+
+#~ fullBody.limbRRTFromRootPath(0,len(configs)-1,0,2)
+from hpp.corbaserver.rbprm.tools.cwc_trajectory_helper import step, clean,stats, saveAllData, play_traj
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (fullBody.client.basic, r)
+
+def act(i, numOptim = 0, use_window = 0, friction = 0.5, optim_effectors = True, verbose = False, draw = False):
+ return step(fullBody, configs, i, numOptim, pp, limbsCOMConstraints, 0.4, optim_effectors = optim_effectors, time_scale = 20., useCOMConstraints = True, use_window = use_window,
+ verbose = verbose, draw = draw)
+
+def play(frame_rate = 1./24.):
+ play_traj(fullBody,pp,frame_rate)
+
+def saveAll(name):
+ saveAllData(fullBody, r, name)
+
+
+def initConfig():
+ r.client.gui.setVisibility("hrp2_14", "ON")
+ tp.cl.problem.selectProblem("default")
+ tp.r.client.gui.setVisibility("toto", "OFF")
+ tp.r.client.gui.setVisibility("hrp2_trunk_flexible", "OFF")
+ r(q_init)
+
+def endConfig():
+ r.client.gui.setVisibility("hrp2_14", "ON")
+ tp.cl.problem.selectProblem("default")
+ tp.r.client.gui.setVisibility("toto", "OFF")
+ tp.r.client.gui.setVisibility("hrp2_trunk_flexible", "OFF")
+ r(q_goal)
+
+
+def rootPath():
+ tp.cl.problem.selectProblem("rbprm_path")
+ r.client.gui.setVisibility("hrp2_14", "OFF")
+ tp.r.client.gui.setVisibility("toto", "OFF")
+ r.client.gui.setVisibility("hyq", "OFF")
+ r.client.gui.setVisibility("hrp2_trunk_flexible", "ON")
+ tp.pp(0)
+ r.client.gui.setVisibility("hrp2_trunk_flexible", "OFF")
+ r.client.gui.setVisibility("hyq", "ON")
+ tp.cl.problem.selectProblem("default")
+
+def genPlan():
+ r.client.gui.setVisibility("hrp2_14", "ON")
+ tp.cl.problem.selectProblem("default")
+ tp.r.client.gui.setVisibility("toto", "OFF")
+ tp.r.client.gui.setVisibility("hrp2_trunk_flexible", "OFF")
+ global configs
+ start = time.clock()
+ configs = configs = fullBody.interpolate(0.1, True)
+ end = time.clock()
+ print "Contact plan generated in " + str(end-start) + "seconds"
+
+def contactPlan():
+ tp.cl.problem.selectProblem("default")
+ r.client.gui.setVisibility("hrp2_14", "ON")
+ tp.r.client.gui.setVisibility("toto", "OFF")
+ tp.r.client.gui.setVisibility("hrp2_trunk_flexible", "OFF")
+ for i in range(0,len(configs)-1):
+ r(configs[i]);
+ time.sleep(0.5)
+
+def d():
+ genPlan()
+
+def e():
+ contactPlan()
+
+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} }
+
+#~ fullBody.limbRRTFromRootPath(0,len(configs)-1,0,2)
+from hpp.corbaserver.rbprm.tools.cwc_trajectory_helper import step, clean,stats, saveAllData, play_traj
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (fullBody.client.basic, r)
+
+def act(i, numOptim = 0, use_window = 0, friction = 0.5, optim_effectors = False, verbose = False, draw = False, trackedEffectors = []):
+ return step(fullBody, configs, i, numOptim, pp, limbsCOMConstraints, 0.4, optim_effectors = optim_effectors, time_scale = 20., useCOMConstraints = False, use_window = use_window,
+ verbose = verbose, draw = draw, trackedEffectors = trackedEffectors)
+
+def play(frame_rate = 1./24.):
+ play_traj(fullBody,pp,frame_rate)
+
+def saveAll(name):
+ saveAllData(fullBody, r, name)
+print "Root path generated in " + str(tp.t) + " ms."
+
+
+genPlan()
diff --git a/script/scenarios/sandbox/stair_bauzil_hrp2_path.py b/script/scenarios/sandbox/stair_bauzil_hrp2_path.py
new file mode 100755
index 0000000000000000000000000000000000000000..17ed92b1eb9334d3f4268058c48ef93f857b1929
--- /dev/null
+++ b/script/scenarios/sandbox/stair_bauzil_hrp2_path.py
@@ -0,0 +1,108 @@
+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
+
+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','hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom']
+urdfSuffix = ""
+srdfSuffix = ""
+
+rbprmBuilder = Builder ()
+
+rbprmBuilder.loadModel(urdfName, urdfNameRoms, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+rbprmBuilder.setJointBounds ("base_joint_xyz", [0,2, -1, 1, 0, 2.2])
+#~ rbprmBuilder.setFilter(['hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom'])
+rbprmBuilder.setAffordanceFilter('3Rarm', ['Support'])
+rbprmBuilder.setAffordanceFilter('0rLeg', ['Support',])
+rbprmBuilder.setAffordanceFilter('1lLeg', ['Support'])
+#~ rbprmBuilder.setNormalFilter('hrp2_rarm_rom', [0,0,1], 0.5)
+#~ rbprmBuilder.setNormalFilter('hrp2_lleg_rom', [0,0,1], 0.9)
+#~ rbprmBuilder.setNormalFilter('hrp2_rleg_rom', [0,0,1], 0.9)
+#~ rbprmBuilder.setNormalFilter('hyq_rhleg_rom', [0,0,1], 0.9)
+rbprmBuilder.boundSO3([-0.,0,-1,1,-1,1])
+
+#~ from hpp.corbaserver.rbprm. import ProblemSolver
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+
+ps = ProblemSolver( rbprmBuilder )
+
+r = Viewer (ps)
+
+
+q_init = rbprmBuilder.getCurrentConfig ();
+q_init [0:3] = [0, -0.82, 0.648702]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
+q_init [0:3] = [0.1, -0.82, 0.648702]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
+#~ q_init [0:3] = [0, -0.63, 0.6]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
+#~ q_init [3:7] = [ 0.98877108, 0. , 0.14943813, 0. ]
+
+q_goal = q_init [::]
+q_goal [3:7] = [ 0.98877108, 0. , 0.14943813, 0. ]
+q_goal [0:3] = [1.49, -0.65, 1.25]; r (q_goal)
+#~ q_goal [0:3] = [1.2, -0.65, 1.1]; r (q_goal)
+
+#~ ps.addPathOptimizer("GradientBased")
+ps.addPathOptimizer("RandomShortcut")
+ps.setInitialConfig (q_init)
+ps.addGoalConfig (q_goal)
+
+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)
+#~ afftool.analyseAll()
+#~ afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
+#~ afftool.visualiseAffordances('Lean', r, [0, 0, 0.9])
+
+ps.client.problem.selectConFigurationShooter("RbprmShooter")
+ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
+#~ ps.solve ()
+t = ps.solve ()
+
+print t;
+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()
+
+
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (rbprmBuilder.client.basic, r)
+#~ pp.fromFile("/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
+#~
+#~ pp (2)
+#~ pp (0)
+
+#~ pp (1)
+#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
+#~ rbprmBuilder.exportPath (r, ps.client.problem, 1, 0.01, "stair_bauzil_hrp2_path.txt")
+
+cl = Client()
+cl.problem.selectProblem("rbprm_path")
+rbprmBuilder2 = Robot ("toto")
+ps2 = ProblemSolver( rbprmBuilder2 )
+cl.problem.selectProblem("default")
+cl.problem.movePathToProblem(1,"rbprm_path",rbprmBuilder.getAllJointNames())
+r2 = Viewer (ps2, viewerClient=r.client)
+r.client.gui.setVisibility("toto", "OFF")
+r.client.gui.setVisibility("hrp2_trunk_flexible", "OFF")
+#~ r2(q_far)