diff --git a/.gitignore b/.gitignore
old mode 100644
new mode 100755
diff --git a/.gitmodules b/.gitmodules
old mode 100644
new mode 100755
diff --git a/LICENSE b/LICENSE
old mode 100644
new mode 100755
diff --git a/data/blender/car.blend b/data/blender/car.blend
old mode 100644
new mode 100755
diff --git a/data/blender/car.dae b/data/blender/car.dae
old mode 100644
new mode 100755
diff --git a/data/blender/darpa.blend b/data/blender/darpa.blend
old mode 100644
new mode 100755
diff --git a/data/blender/darpareduced.blend b/data/blender/darpareduced.blend
old mode 100644
new mode 100755
diff --git a/data/blender/groundcrouch.blend b/data/blender/groundcrouch.blend
old mode 100644
new mode 100755
diff --git a/data/blender/hyq.blend b/data/blender/hyq.blend
old mode 100644
new mode 100755
diff --git a/data/blender/hyq.blend1 b/data/blender/hyq.blend1
old mode 100644
new mode 100755
diff --git a/data/blender/obstacle.blend b/data/blender/obstacle.blend
old mode 100644
new mode 100755
diff --git a/data/blender/stair.blend b/data/blender/stair.blend
old mode 100644
new mode 100755
diff --git a/data/blender/stair_bauzil.blend b/data/blender/stair_bauzil.blend
old mode 100644
new mode 100755
diff --git a/data/blender/truck.blend b/data/blender/truck.blend
old mode 100644
new mode 100755
diff --git a/data/blender/truck.blend1 b/data/blender/truck.blend1
old mode 100644
new mode 100755
diff --git a/data/hyq_description/meshes/leg/lowerlegreduced.dae b/data/hyq_description/meshes/leg/lowerlegreduced.dae
old mode 100644
new mode 100755
diff --git a/data/hyq_description/srdf/hyq.srdf b/data/hyq_description/srdf/hyq.srdf
old mode 100644
new mode 100755
diff --git a/data/hyq_description/urdf/hyq.urdf b/data/hyq_description/urdf/hyq.urdf
old mode 100644
new mode 100755
diff --git a/data/meshes/car.dae b/data/meshes/car.dae
old mode 100644
new mode 100755
diff --git a/data/meshes/chair.stl b/data/meshes/chair.stl
old mode 100644
new mode 100755
diff --git a/data/meshes/climb.stl b/data/meshes/climb.stl
old mode 100644
new mode 100755
diff --git a/data/meshes/darpa.stl b/data/meshes/darpa.stl
old mode 100644
new mode 100755
diff --git a/data/meshes/darpareduced.stl b/data/meshes/darpareduced.stl
old mode 100644
new mode 100755
diff --git a/data/meshes/ground_table.stl b/data/meshes/ground_table.stl
old mode 100644
new mode 100755
diff --git a/data/meshes/ground_tablec.stl b/data/meshes/ground_tablec.stl
old mode 100644
new mode 100755
diff --git a/data/meshes/groundcrouch.stl b/data/meshes/groundcrouch.stl
old mode 100644
new mode 100755
diff --git a/data/meshes/hrp2_larm_rom.stl b/data/meshes/hrp2_larm_rom.stl
old mode 100644
new mode 100755
diff --git a/data/meshes/hrp2_lleg_rom.stl b/data/meshes/hrp2_lleg_rom.stl
old mode 100644
new mode 100755
diff --git a/data/meshes/hrp2_rarm_rom.stl b/data/meshes/hrp2_rarm_rom.stl
old mode 100644
new mode 100755
diff --git a/data/meshes/hrp2_rleg_rom.stl b/data/meshes/hrp2_rleg_rom.stl
old mode 100644
new mode 100755
diff --git a/data/meshes/hrp2_rom.stl b/data/meshes/hrp2_rom.stl
old mode 100644
new mode 100755
diff --git a/data/meshes/hrp2_trunk.stl b/data/meshes/hrp2_trunk.stl
old mode 100644
new mode 100755
diff --git a/data/meshes/hrp2_trunk_body.stl b/data/meshes/hrp2_trunk_body.stl
old mode 100644
new mode 100755
diff --git a/data/meshes/hrp2_trunk_body_view.dae b/data/meshes/hrp2_trunk_body_view.dae
old mode 100644
new mode 100755
diff --git a/data/meshes/hrp2_trunk_torso.dae b/data/meshes/hrp2_trunk_torso.dae
old mode 100644
new mode 100755
diff --git a/data/meshes/hrp2_trunk_torso.stl b/data/meshes/hrp2_trunk_torso.stl
old mode 100644
new mode 100755
diff --git a/data/meshes/hrp2_trunk_torso_view.dae b/data/meshes/hrp2_trunk_torso_view.dae
old mode 100644
new mode 100755
diff --git a/data/meshes/hyq/hyq_all.stl b/data/meshes/hyq/hyq_all.stl
old mode 100644
new mode 100755
diff --git a/data/meshes/hyq/hyq_lfleg_rom.stl b/data/meshes/hyq/hyq_lfleg_rom.stl
old mode 100644
new mode 100755
diff --git a/data/meshes/hyq/hyq_lhleg_rom.stl b/data/meshes/hyq/hyq_lhleg_rom.stl
old mode 100644
new mode 100755
diff --git a/data/meshes/hyq/hyq_rfleg_rom.stl b/data/meshes/hyq/hyq_rfleg_rom.stl
old mode 100644
new mode 100755
diff --git a/data/meshes/hyq/hyq_rhleg_rom.stl b/data/meshes/hyq/hyq_rhleg_rom.stl
old mode 100644
new mode 100755
diff --git a/data/meshes/hyq/hyq_rom.stl b/data/meshes/hyq/hyq_rom.stl
old mode 100644
new mode 100755
diff --git a/data/meshes/hyq/hyq_trunk.stl b/data/meshes/hyq/hyq_trunk.stl
old mode 100644
new mode 100755
diff --git a/data/meshes/hyq/hyq_trunk_large.stl b/data/meshes/hyq/hyq_trunk_large.stl
old mode 100644
new mode 100755
diff --git a/data/meshes/hyq/lfleg.obj b/data/meshes/hyq/lfleg.obj
old mode 100644
new mode 100755
diff --git a/data/meshes/hyq/lhleg.obj b/data/meshes/hyq/lhleg.obj
old mode 100644
new mode 100755
diff --git a/data/meshes/hyq/rfleg.obj b/data/meshes/hyq/rfleg.obj
old mode 100644
new mode 100755
diff --git a/data/meshes/hyq/rhleg.obj b/data/meshes/hyq/rhleg.obj
old mode 100644
new mode 100755
diff --git a/data/meshes/stair_bauzil.stl b/data/meshes/stair_bauzil.stl
old mode 100644
new mode 100755
diff --git a/data/meshes/stair_bauzil_reduced.stl b/data/meshes/stair_bauzil_reduced.stl
old mode 100644
new mode 100755
diff --git a/data/meshes/stepladder.mtl b/data/meshes/stepladder.mtl
old mode 100644
new mode 100755
diff --git a/data/meshes/stepladder.stl b/data/meshes/stepladder.stl
old mode 100644
new mode 100755
diff --git a/data/meshes/truck.stl b/data/meshes/truck.stl
old mode 100644
new mode 100755
diff --git a/data/meshes/truck_vision.stl b/data/meshes/truck_vision.stl
old mode 100644
new mode 100755
diff --git a/data/roms/centers.txt b/data/roms/centers.txt
old mode 100644
new mode 100755
diff --git a/data/roms/centers.txt~ b/data/roms/centers.txt~
old mode 100644
new mode 100755
diff --git a/data/roms/comlArm.erom b/data/roms/comlArm.erom
old mode 100644
new mode 100755
diff --git a/data/roms/comlArm.obj b/data/roms/comlArm.obj
old mode 100644
new mode 100755
diff --git a/data/roms/comlArmSimplified.mtl b/data/roms/comlArmSimplified.mtl
old mode 100644
new mode 100755
diff --git a/data/roms/comlArmSimplified.obj b/data/roms/comlArmSimplified.obj
old mode 100644
new mode 100755
diff --git a/data/roms/comlLeg.erom b/data/roms/comlLeg.erom
old mode 100644
new mode 100755
diff --git a/data/roms/comlLeg.obj b/data/roms/comlLeg.obj
old mode 100644
new mode 100755
diff --git a/data/roms/comlLegSimplified.mtl b/data/roms/comlLegSimplified.mtl
old mode 100644
new mode 100755
diff --git a/data/roms/comlLegSimplified.obj b/data/roms/comlLegSimplified.obj
old mode 100644
new mode 100755
diff --git a/data/roms/comrArm.erom b/data/roms/comrArm.erom
old mode 100644
new mode 100755
diff --git a/data/roms/comrArm.obj b/data/roms/comrArm.obj
old mode 100644
new mode 100755
diff --git a/data/roms/comrArmSimplified.mtl b/data/roms/comrArmSimplified.mtl
old mode 100644
new mode 100755
diff --git a/data/roms/comrArmSimplified.obj b/data/roms/comrArmSimplified.obj
old mode 100644
new mode 100755
diff --git a/data/roms/comrLeg.erom b/data/roms/comrLeg.erom
old mode 100644
new mode 100755
diff --git a/data/roms/comrLeg.obj b/data/roms/comrLeg.obj
old mode 100644
new mode 100755
diff --git a/data/roms/comrLegSimplified.mtl b/data/roms/comrLegSimplified.mtl
old mode 100644
new mode 100755
diff --git a/data/roms/comrLegSimplified.obj b/data/roms/comrLegSimplified.obj
old mode 100644
new mode 100755
diff --git a/data/roms/hyq/lfleg.erom b/data/roms/hyq/lfleg.erom
old mode 100644
new mode 100755
diff --git a/data/roms/hyq/lhleg.erom b/data/roms/hyq/lhleg.erom
old mode 100644
new mode 100755
diff --git a/data/roms/hyq/rfleg.erom b/data/roms/hyq/rfleg.erom
old mode 100644
new mode 100755
diff --git a/data/roms/hyq/rhleg.erom b/data/roms/hyq/rhleg.erom
old mode 100644
new mode 100755
diff --git a/data/roms/larm.erom b/data/roms/larm.erom
old mode 100644
new mode 100755
diff --git a/data/roms/larm.mtl b/data/roms/larm.mtl
old mode 100644
new mode 100755
diff --git a/data/roms/larm.obj b/data/roms/larm.obj
old mode 100644
new mode 100755
diff --git a/data/roms/larm_simplified.mtl b/data/roms/larm_simplified.mtl
old mode 100644
new mode 100755
diff --git a/data/roms/larm_simplified.obj b/data/roms/larm_simplified.obj
old mode 100644
new mode 100755
diff --git a/data/roms/lleg.erom b/data/roms/lleg.erom
old mode 100644
new mode 100755
diff --git a/data/roms/lleg.mtl b/data/roms/lleg.mtl
old mode 100644
new mode 100755
diff --git a/data/roms/lleg.obj b/data/roms/lleg.obj
old mode 100644
new mode 100755
diff --git a/data/roms/lleg_simplified.mtl b/data/roms/lleg_simplified.mtl
old mode 100644
new mode 100755
diff --git a/data/roms/lleg_simplified.obj b/data/roms/lleg_simplified.obj
old mode 100644
new mode 100755
diff --git a/data/roms/phantom/lf.erom b/data/roms/phantom/lf.erom
old mode 100644
new mode 100755
diff --git a/data/roms/phantom/lm.erom b/data/roms/phantom/lm.erom
old mode 100644
new mode 100755
diff --git a/data/roms/phantom/lr.erom b/data/roms/phantom/lr.erom
old mode 100644
new mode 100755
diff --git a/data/roms/phantom/rf.erom b/data/roms/phantom/rf.erom
old mode 100644
new mode 100755
diff --git a/data/roms/phantom/rm.erom b/data/roms/phantom/rm.erom
old mode 100644
new mode 100755
diff --git a/data/roms/phantom/rr.erom b/data/roms/phantom/rr.erom
old mode 100644
new mode 100755
diff --git a/data/roms/rarm.erom b/data/roms/rarm.erom
old mode 100644
new mode 100755
diff --git a/data/roms/rarm.mtl b/data/roms/rarm.mtl
old mode 100644
new mode 100755
diff --git a/data/roms/rarm.obj b/data/roms/rarm.obj
old mode 100644
new mode 100755
diff --git a/data/roms/rarm_simplified.mtl b/data/roms/rarm_simplified.mtl
old mode 100644
new mode 100755
diff --git a/data/roms/rarm_simplified.obj b/data/roms/rarm_simplified.obj
old mode 100644
new mode 100755
diff --git a/data/roms/rleg.erom b/data/roms/rleg.erom
old mode 100644
new mode 100755
diff --git a/data/roms/rleg.mtl b/data/roms/rleg.mtl
old mode 100644
new mode 100755
diff --git a/data/roms/rleg.obj b/data/roms/rleg.obj
old mode 100644
new mode 100755
diff --git a/data/roms/rleg_simplified.mtl b/data/roms/rleg_simplified.mtl
old mode 100644
new mode 100755
diff --git a/data/roms/rleg_simplified.obj b/data/roms/rleg_simplified.obj
old mode 100644
new mode 100755
diff --git a/data/srdf/car.srdf b/data/srdf/car.srdf
old mode 100644
new mode 100755
diff --git a/data/srdf/climb.srdf b/data/srdf/climb.srdf
old mode 100644
new mode 100755
diff --git a/data/srdf/darpa.srdf b/data/srdf/darpa.srdf
old mode 100644
new mode 100755
diff --git a/data/srdf/ground.srdf b/data/srdf/ground.srdf
old mode 100644
new mode 100755
diff --git a/data/srdf/groundcrouch.srdf b/data/srdf/groundcrouch.srdf
old mode 100644
new mode 100755
diff --git a/data/srdf/hrp2_larm_rom.srdf b/data/srdf/hrp2_larm_rom.srdf
old mode 100644
new mode 100755
diff --git a/data/srdf/hrp2_lleg_rom.srdf b/data/srdf/hrp2_lleg_rom.srdf
old mode 100644
new mode 100755
diff --git a/data/srdf/hrp2_rarm_rom.srdf b/data/srdf/hrp2_rarm_rom.srdf
old mode 100644
new mode 100755
diff --git a/data/srdf/hrp2_rleg_rom.srdf b/data/srdf/hrp2_rleg_rom.srdf
old mode 100644
new mode 100755
diff --git a/data/srdf/hrp2_rom.srdf b/data/srdf/hrp2_rom.srdf
old mode 100644
new mode 100755
diff --git a/data/srdf/hrp2_trunk.srdf b/data/srdf/hrp2_trunk.srdf
old mode 100644
new mode 100755
diff --git a/data/srdf/hrp2_trunk_flexible.srdf b/data/srdf/hrp2_trunk_flexible.srdf
old mode 100644
new mode 100755
diff --git a/data/srdf/hyq/hrp2_trunk.srdf b/data/srdf/hyq/hrp2_trunk.srdf
old mode 100644
new mode 100755
diff --git a/data/srdf/hyq/hyq_lfleg_rom.srdf b/data/srdf/hyq/hyq_lfleg_rom.srdf
old mode 100644
new mode 100755
diff --git a/data/srdf/hyq/hyq_lhleg_rom.srdf b/data/srdf/hyq/hyq_lhleg_rom.srdf
old mode 100644
new mode 100755
diff --git a/data/srdf/hyq/hyq_rfleg_rom.srdf b/data/srdf/hyq/hyq_rfleg_rom.srdf
old mode 100644
new mode 100755
diff --git a/data/srdf/hyq/hyq_rhleg_rom.srdf b/data/srdf/hyq/hyq_rhleg_rom.srdf
old mode 100644
new mode 100755
diff --git a/data/srdf/hyq/hyq_rom.srdf b/data/srdf/hyq/hyq_rom.srdf
old mode 100644
new mode 100755
diff --git a/data/srdf/hyq/hyq_trunk.srdf b/data/srdf/hyq/hyq_trunk.srdf
old mode 100644
new mode 100755
diff --git a/data/srdf/hyq/hyq_trunk_large.srdf b/data/srdf/hyq/hyq_trunk_large.srdf
old mode 100644
new mode 100755
diff --git a/data/srdf/scene_wall.srdf b/data/srdf/scene_wall.srdf
old mode 100644
new mode 100755
diff --git a/data/srdf/stair_bauzil.srdf b/data/srdf/stair_bauzil.srdf
old mode 100644
new mode 100755
diff --git a/data/srdf/stepladder.srdf b/data/srdf/stepladder.srdf
old mode 100644
new mode 100755
diff --git a/data/srdf/truck.srdf b/data/srdf/truck.srdf
old mode 100644
new mode 100755
diff --git a/data/urdf/car.urdf b/data/urdf/car.urdf
old mode 100644
new mode 100755
diff --git a/data/urdf/climb.urdf b/data/urdf/climb.urdf
old mode 100644
new mode 100755
diff --git a/data/urdf/darpa.urdf b/data/urdf/darpa.urdf
old mode 100644
new mode 100755
diff --git a/data/urdf/ground.urdf b/data/urdf/ground.urdf
old mode 100644
new mode 100755
diff --git a/data/urdf/groundcrouch.urdf b/data/urdf/groundcrouch.urdf
old mode 100644
new mode 100755
diff --git a/data/urdf/hrp2_larm_rom.urdf b/data/urdf/hrp2_larm_rom.urdf
old mode 100644
new mode 100755
diff --git a/data/urdf/hrp2_lleg_rom.urdf b/data/urdf/hrp2_lleg_rom.urdf
old mode 100644
new mode 100755
diff --git a/data/urdf/hrp2_rarm_rom.urdf b/data/urdf/hrp2_rarm_rom.urdf
old mode 100644
new mode 100755
diff --git a/data/urdf/hrp2_rleg_rom.urdf b/data/urdf/hrp2_rleg_rom.urdf
old mode 100644
new mode 100755
diff --git a/data/urdf/hrp2_rom.urdf b/data/urdf/hrp2_rom.urdf
old mode 100644
new mode 100755
diff --git a/data/urdf/hrp2_trunk.urdf b/data/urdf/hrp2_trunk.urdf
old mode 100644
new mode 100755
diff --git a/data/urdf/hrp2_trunk08.urdf b/data/urdf/hrp2_trunk08.urdf
old mode 100644
new mode 100755
diff --git a/data/urdf/hrp2_trunk_flexible.urdf b/data/urdf/hrp2_trunk_flexible.urdf
old mode 100644
new mode 100755
diff --git a/data/urdf/hyq/hyq_lfleg_rom.urdf b/data/urdf/hyq/hyq_lfleg_rom.urdf
old mode 100644
new mode 100755
diff --git a/data/urdf/hyq/hyq_lhleg_rom.urdf b/data/urdf/hyq/hyq_lhleg_rom.urdf
old mode 100644
new mode 100755
diff --git a/data/urdf/hyq/hyq_rfleg_rom.urdf b/data/urdf/hyq/hyq_rfleg_rom.urdf
old mode 100644
new mode 100755
diff --git a/data/urdf/hyq/hyq_rhleg_rom.urdf b/data/urdf/hyq/hyq_rhleg_rom.urdf
old mode 100644
new mode 100755
diff --git a/data/urdf/hyq/hyq_rom.urdf b/data/urdf/hyq/hyq_rom.urdf
old mode 100644
new mode 100755
diff --git a/data/urdf/hyq/hyq_trunk.urdf b/data/urdf/hyq/hyq_trunk.urdf
old mode 100644
new mode 100755
diff --git a/data/urdf/hyq/hyq_trunk_large.urdf b/data/urdf/hyq/hyq_trunk_large.urdf
old mode 100644
new mode 100755
diff --git a/data/urdf/scene_wall.urdf b/data/urdf/scene_wall.urdf
old mode 100644
new mode 100755
diff --git a/data/urdf/stair_bauzil.urdf b/data/urdf/stair_bauzil.urdf
old mode 100644
new mode 100755
diff --git a/data/urdf/stepladder.urdf b/data/urdf/stepladder.urdf
old mode 100644
new mode 100755
diff --git a/data/urdf/truck.urdf b/data/urdf/truck.urdf
old mode 100644
new mode 100755
diff --git a/doc/Doxyfile.extra.in b/doc/Doxyfile.extra.in
old mode 100644
new mode 100755
diff --git a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
index e933f8d54a968a9baa5ded53cedfb6a0b0d00e81..34164d2b842649abd50f98b3497351930f5bb71a 100755
--- a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
+++ b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
@@ -178,6 +178,13 @@ module hpp
/// \param path path computed.
floatSeqSeq interpolate(in double timestep, in double path) raises (Error);
+ /// Provided a path has already been computed, interpolates it and generates the statically stable
+ /// constact configurations along it. setStartState and setEndState must have been called prior
+ /// to this function. If these conditions are not met an error is raised.
+ /// \param timestep normalized step for generation along the path (ie the path has a length of 1).
+ /// \param path path computed.
+ floatSeqSeq interpolateConfigs(in floatSeqSeq configs) raises (Error);
+
/// Saves the last computed states by the function interpolate in a filename.
/// Raises an error if interpolate has not been called, or the file could not be opened.
/// \param filename name of the file used to save the contacts.
diff --git a/out/stair_bauzil/2.ogv b/out/stair_bauzil/2.ogv
old mode 100644
new mode 100755
diff --git a/out/stair_bauzil/3.ogv b/out/stair_bauzil/3.ogv
old mode 100644
new mode 100755
diff --git a/out/stair_bauzil/4.ogv b/out/stair_bauzil/4.ogv
old mode 100644
new mode 100755
diff --git a/out/stair_bauzil/fourth.csv b/out/stair_bauzil/fourth.csv
old mode 100644
new mode 100755
diff --git a/out/stair_bauzil/fourthchoice b/out/stair_bauzil/fourthchoice
old mode 100644
new mode 100755
diff --git a/out/stair_bauzil/secondchoice b/out/stair_bauzil/secondchoice
old mode 100644
new mode 100755
diff --git a/out/stair_bauzil/secondchoice.csv b/out/stair_bauzil/secondchoice.csv
old mode 100644
new mode 100755
diff --git a/out/stair_bauzil/thirdchoice b/out/stair_bauzil/thirdchoice
old mode 100644
new mode 100755
diff --git a/out/stair_bauzil/thirdchoice.csv b/out/stair_bauzil/thirdchoice.csv
old mode 100644
new mode 100755
diff --git a/profile/profile.py b/profile/profile.py
old mode 100644
new mode 100755
diff --git a/script/scenarios/car_hrp2_interp.py b/script/scenarios/car_hrp2_interp.py
old mode 100644
new mode 100755
diff --git a/script/scenarios/car_hrp2_path.py b/script/scenarios/car_hrp2_path.py
old mode 100644
new mode 100755
diff --git a/script/scenarios/darpa_hrp2_interp.py b/script/scenarios/darpa_hrp2_interp.py
old mode 100644
new mode 100755
diff --git a/script/scenarios/darpa_hrp2_path.py b/script/scenarios/darpa_hrp2_path.py
old mode 100644
new mode 100755
diff --git a/script/scenarios/darpa_hyq_interp.py b/script/scenarios/darpa_hyq_interp.py
old mode 100644
new mode 100755
index 741f62ea38d467de2d0ce1606c9eaabe65093b38..e8c5562323c75efcfde3408cc98a99391f32ee98
--- a/script/scenarios/darpa_hyq_interp.py
+++ b/script/scenarios/darpa_hyq_interp.py
@@ -33,7 +33,7 @@ rfoot = 'rf_foot_joint'
rLegOffset = [0.,0,0.]
rLegNormal = [0,1,0]
rLegx = 0.02; rLegy = 0.02
-fullBody.addLimb(rLegId,rLeg,rfoot,rLegOffset,rLegNormal, rLegx, rLegy, nbSamples, "forward", 0.1)
+fullBody.addLimb(rLegId,rLeg,rfoot,rLegOffset,rLegNormal, rLegx, rLegy, nbSamples, "manipulability", 0.1)
#~ fullBody.addLimb(rLegId,rLeg,rfoot,rLegOffset,rLegNormal, rLegx, rLegy, nbSamples, "manipulability", 0.05)
#~ fullBody.addLimb(rLegId,rLeg,rfoot,rLegOffset,rLegNormal, rLegx, rLegy, nbSamples, "random", 0.05)
@@ -43,7 +43,7 @@ lfoot = 'lh_foot_joint'
lLegOffset = [0,0,0]
lLegNormal = [0,1,0]
lLegx = 0.02; lLegy = 0.02
-fullBody.addLimb(lLegId,lLeg,lfoot,lLegOffset,rLegNormal, lLegx, lLegy, nbSamples, "forward", 0.05)
+fullBody.addLimb(lLegId,lLeg,lfoot,lLegOffset,rLegNormal, lLegx, lLegy, nbSamples, "manipulability", 0.05)
#~ fullBody.addLimb(lLegId,lLeg,lfoot,lLegOffset,rLegNormal, lLegx, lLegy, nbSamples, "manipulability", 0.05)
#~ fullBody.addLimb(lLegId,lLeg,lfoot,lLegOffset,rLegNormal, lLegx, lLegy, nbSamples, "random", 0.05)
@@ -53,7 +53,7 @@ rHand = 'rh_foot_joint'
rArmOffset = [0.,0,-0.]
rArmNormal = [0,1,0]
rArmx = 0.02; rArmy = 0.02
-fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, nbSamples, "forward", 0.05)
+fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, nbSamples, "manipulability", 0.05)
#~ fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, nbSamples, "manipulability", 0.05)
#~ fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, nbSamples, "random", 0.05)
diff --git a/script/scenarios/darpa_hyq_path.py b/script/scenarios/darpa_hyq_path.py
old mode 100644
new mode 100755
index 5898046914a564da195ce7b8d6307af3aa4c5997..b18f33930514ae8290a38510c7ee73b05c72d5d4
--- a/script/scenarios/darpa_hyq_path.py
+++ b/script/scenarios/darpa_hyq_path.py
@@ -52,7 +52,7 @@ f.close()
from hpp.gepetto import PathPlayer
pp = PathPlayer (rbprmBuilder.client.basic, r)
-#~ rbprmBuilder.exportPath (r, ps.client.problem, 1, 0.1, "darpa_hyq_path.txt")
+rbprmBuilder.exportPath (r, ps.client.problem, 1, 0.1, "darpa_hyq_robust_20_path.txt")
#~ pp.fromFile("/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
#~
#~ pp (2)
diff --git a/script/scenarios/ground_crouch_hyq_interp.py b/script/scenarios/ground_crouch_hyq_interp.py
old mode 100644
new mode 100755
index 4383eac2349f22482cb01c1f94d7db18d35fe49b..76774043dd8ae9a746b4e643038f9da1910b020f
--- a/script/scenarios/ground_crouch_hyq_interp.py
+++ b/script/scenarios/ground_crouch_hyq_interp.py
@@ -79,7 +79,7 @@ r(q_init)
configs = fullBody.interpolate(0.1)
r.loadObstacleModel ('hpp-rbprm-corba', "groundcrouch", "contact")
-fullBody.exportAll(r, configs, 'obstacle_hyq_robust_1');
+fullBody.exportAll(r, configs, 'obstacle_hyq_robust_10');
i = 0;
r (configs[i]); i=i+1; i-1
diff --git a/script/scenarios/ground_crouch_hyq_path.py b/script/scenarios/ground_crouch_hyq_path.py
old mode 100644
new mode 100755
index 8ddbf8d5efba361965ad4b71a82ff02a010a9a3e..29b3ed1518d4ece915b753677a64904456a2356e
--- a/script/scenarios/ground_crouch_hyq_path.py
+++ b/script/scenarios/ground_crouch_hyq_path.py
@@ -46,7 +46,7 @@ t = ps.solve ()
f = open('log.txt', 'a')
f.write("path computation " + str(t) + "\n")
f.close()
-rbprmBuilder.exportPath (r, ps.client.problem, 1, 0.1, "obstacle_hyq_path.txt")
+rbprmBuilder.exportPath (r, ps.client.problem, 1, 0.1, "obstacle_hyq_robust_10_path.txt")
from hpp.gepetto import PathPlayer
diff --git a/script/scenarios/hrp2_walkgen.py b/script/scenarios/hrp2_walkgen.py
old mode 100644
new mode 100755
diff --git a/script/scenarios/josephload.py b/script/scenarios/josephload.py
old mode 100644
new mode 100755
diff --git a/script/scenarios/josephpath.py b/script/scenarios/josephpath.py
old mode 100644
new mode 100755
diff --git a/script/scenarios/stair_bauzil_hrp2_interp.py b/script/scenarios/stair_bauzil_hrp2_interp.py
old mode 100644
new mode 100755
diff --git a/script/scenarios/stair_bauzil_hrp2_path.py b/script/scenarios/stair_bauzil_hrp2_path.py
old mode 100644
new mode 100755
diff --git a/script/scenarios/stair_bauzil_hyq_interp.py b/script/scenarios/stair_bauzil_hyq_interp.py
old mode 100644
new mode 100755
diff --git a/script/scenarios/stair_bauzil_hyq_path.py b/script/scenarios/stair_bauzil_hyq_path.py
old mode 100644
new mode 100755
diff --git a/script/scenarios/standing_hrp2_interp.py b/script/scenarios/standing_hrp2_interp.py
old mode 100644
new mode 100755
diff --git a/script/scenarios/standing_hrp2_path.py b/script/scenarios/standing_hrp2_path.py
old mode 100644
new mode 100755
diff --git a/script/scenarios/truck_hrp2_interp.py b/script/scenarios/truck_hrp2_interp.py
old mode 100644
new mode 100755
diff --git a/script/scenarios/truck_hrp2_path.py b/script/scenarios/truck_hrp2_path.py
old mode 100644
new mode 100755
diff --git a/script/tools/admissibleRootPositionsFromEffector.py b/script/tools/admissibleRootPositionsFromEffector.py
old mode 100644
new mode 100755
diff --git a/script/tools/check_motion.py b/script/tools/check_motion.py
new file mode 100755
index 0000000000000000000000000000000000000000..f4ad4cc0e9fb2cebbfd489c612eb462f3617975c
--- /dev/null
+++ b/script/tools/check_motion.py
@@ -0,0 +1,36 @@
+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.gepetto import Viewer
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+
+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)
+
+ps = ProblemSolver( fullBody )
+r = Viewer (ps)
+
+justin = "/home/stonneau/dev/justin/hrp2-motions/standup/"
+
+def check(csv):
+ qs=[]
+ file = open(justin+csv,"r+");
+ # first retrieve frame range
+ for line in file.readlines():
+ objs = line.rstrip("\n").split(" ");
+ objs.pop(0)
+ q = fullBody.getCurrentConfig();
+ for i in range (0, len(q)-1):
+ q[i+7] = float(objs[i])
+ qs.append(q)
+ return qs
+
+configs = check("standup_2015_11_16_16_23_35.pos")
diff --git a/script/tools/generateROMs.py b/script/tools/generateROMs.py
old mode 100644
new mode 100755
diff --git a/script/tools/generateROMsHyq.py b/script/tools/generateROMsHyq.py
old mode 100644
new mode 100755
diff --git a/script/tools/generateROMsPhantom.py b/script/tools/generateROMsPhantom.py
old mode 100644
new mode 100755
diff --git a/script/tools/loadobj.py b/script/tools/loadobj.py
old mode 100644
new mode 100755
diff --git a/script/tools/quaternion.py b/script/tools/quaternion.py
old mode 100644
new mode 100755
diff --git a/src/hpp/corbaserver/rbprm/rbprmfullbody.py b/src/hpp/corbaserver/rbprm/rbprmfullbody.py
index 30fa95c00f0f9ec692b570e45cca22af071d83fb..c3d4cc00cbe89162bf2ef9a3ec32618ca1775c09 100755
--- a/src/hpp/corbaserver/rbprm/rbprmfullbody.py
+++ b/src/hpp/corbaserver/rbprm/rbprmfullbody.py
@@ -176,6 +176,14 @@ class FullBody (object):
def interpolate(self, stepsize, pathId = 1):
return self.client.rbprm.rbprm.interpolate(stepsize, pathId)
+ ## Given start and goal states
+ # generate a contact sequence over a list of configurations
+ #
+ # \param stepSize discretization step
+ # \param pathId Id of the path to compute from
+ def interpolateConfigs(self, configs):
+ return self.client.rbprm.rbprm.interpolateConfigs(configs)
+
## Create octree nodes representation for a given limb
#
# \param stepSize discretization step
diff --git a/src/rbprmbuilder.impl.cc b/src/rbprmbuilder.impl.cc
index a44e660a6209716bcc4d8a29ebc98629bd7ced10..ac245ec83df928f87d920f946353eaf5b12b686a 100755
--- a/src/rbprmbuilder.impl.cc
+++ b/src/rbprmbuilder.impl.cc
@@ -210,6 +210,18 @@ namespace hpp {
return config;
}
+ std::vector<model::Configuration_t> doubleDofArrayToConfig (const model::DevicePtr_t& robot,
+ const hpp::floatSeqSeq& doubleDofArray)
+ {
+ std::size_t configsDim = (std::size_t)doubleDofArray.length();
+ std::vector<model::Configuration_t> res;
+ for (std::size_t iConfig = 0; iConfig < configsDim; iConfig++)
+ {
+ res.push_back(dofArrayToConfig(robot, doubleDofArray[iConfig]));
+ }
+ return res;
+ }
+
std::vector<std::string> stringConversion(const hpp::Names_t& dofArray)
{
std::vector<std::string> res;
@@ -416,6 +428,49 @@ namespace hpp {
}
}
+ floatSeqSeq* RbprmBuilder::interpolateConfigs(const hpp::floatSeqSeq& configs) throw (hpp::Error)
+ {
+ try
+ {
+ if(startState_.configuration_.rows() == 0)
+ {
+ throw std::runtime_error ("Start state not initialized, can not interpolate ");
+ }
+ if(endState_.configuration_.rows() == 0)
+ {
+ throw std::runtime_error ("End state not initialized, can not interpolate ");
+ }
+ hpp::rbprm::RbPrmInterpolationPtr_t interpolator = rbprm::RbPrmInterpolation::create(fullBody_,startState_,endState_);
+ std::vector<model::Configuration_t> configurations = doubleDofArrayToConfig(fullBody_->device_, configs);
+ lastStatesComputed_ = interpolator->Interpolate(problemSolver_->collisionObstacles(),configurations);
+ hpp::floatSeqSeq *res;
+ res = new hpp::floatSeqSeq ();
+
+ res->length (lastStatesComputed_.size ());
+ std::size_t i=0;
+ std::size_t id = 0;
+ for(std::vector<State>::const_iterator cit = lastStatesComputed_.begin(); cit != lastStatesComputed_.end(); ++cit, ++id)
+ {
+ std::cout << "ID " << id;
+ cit->print();
+ const core::Configuration_t config = cit->configuration_;
+ _CORBA_ULong size = (_CORBA_ULong) config.size ();
+ double* dofArray = hpp::floatSeq::allocbuf(size);
+ hpp::floatSeq floats (size, size, dofArray, true);
+ //convert the config in dofseq
+ for (model::size_type j=0 ; j < config.size() ; ++j) {
+ dofArray[j] = config [j];
+ }
+ (*res) [i] = floats;
+ ++i;
+ }
+ return res;
+ }
+ catch(std::runtime_error& e)
+ {
+ throw Error(e.what());
+ }
+ }
floatSeqSeq* RbprmBuilder::interpolate(double timestep, double path) throw (hpp::Error)
{
@@ -436,7 +491,7 @@ namespace hpp {
throw std::runtime_error ("No path computed, cannot interpolate ");
}
- hpp::rbprm::RbPrmInterpolationPtr_t interpolator = rbprm::RbPrmInterpolation::create(problemSolver_->paths()[pathId],fullBody_,startState_,endState_);
+ hpp::rbprm::RbPrmInterpolationPtr_t interpolator = rbprm::RbPrmInterpolation::create(fullBody_,startState_,endState_,problemSolver_->paths()[pathId]);
lastStatesComputed_ = interpolator->Interpolate(problemSolver_->collisionObstacles(),timestep);
hpp::floatSeqSeq *res;
diff --git a/src/rbprmbuilder.impl.hh b/src/rbprmbuilder.impl.hh
index 0e596f230bd0d9613eee87a9891574fc68c69b64..c48993661217bb145118f6f8e42eb2500cc56e62 100755
--- a/src/rbprmbuilder.impl.hh
+++ b/src/rbprmbuilder.impl.hh
@@ -115,6 +115,7 @@ namespace hpp {
virtual void setStartState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) throw (hpp::Error);
virtual void setEndState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) throw (hpp::Error);
virtual hpp::floatSeqSeq* interpolate(double timestep, double path) throw (hpp::Error);
+ virtual hpp::floatSeqSeq* interpolateConfigs(const hpp::floatSeqSeq& configs) throw (hpp::Error);
virtual void saveComputedStates(const char* filepath) throw (hpp::Error);
virtual hpp::floatSeqSeq* GetOctreeBoxes(const char* limbName, const hpp::floatSeq& configuration) throw (hpp::Error);
virtual hpp::floatSeq* getOctreeTransform(const char* limbName, const hpp::floatSeq& configuration) throw (hpp::Error);