diff --git a/data/meshes/hrp2_trunk_body.stl b/data/meshes/hrp2_trunk_body.stl
old mode 100755
new mode 100644
index dbfe476c959e2d49c5bd188a0b71bf8a7d7ca3e0..fad9f4f69717e571867f608e7cd2befccf7628e5
Binary files a/data/meshes/hrp2_trunk_body.stl and b/data/meshes/hrp2_trunk_body.stl differ
diff --git a/data/urdf/hrp2_trunk_flexible.urdf b/data/urdf/hrp2_trunk_flexible.urdf
index 6dbbf2f2e0d552d39f9edbc693e4daf25210e354..f6ccb657e21cd4224980d17a3a5912dbd51fc7a4 100755
--- a/data/urdf/hrp2_trunk_flexible.urdf
+++ b/data/urdf/hrp2_trunk_flexible.urdf
@@ -1,3 +1,4 @@
+
<robot name="hrp2_trunk_flexible">
<link name="base_link"/>
<joint name="WAIST" type="fixed">
diff --git a/script/dynamic/stair_bauzil_hrp2_interp.py b/script/dynamic/stair_bauzil_hrp2_interp.py
index 32a1b0743883d1eca43fb485bef982acf00b1581..78d2c360ce345a83482a7538b9f4f567ac97b2cc 100644
--- a/script/dynamic/stair_bauzil_hrp2_interp.py
+++ b/script/dynamic/stair_bauzil_hrp2_interp.py
@@ -28,15 +28,15 @@ 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]
+rLegOffset = [0,0,-0.105]
+rLegNormal = [0,0,1]
rLegx = 0.09; rLegy = 0.05
fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 20000, "manipulability", 0.1)
lLegId = '1lLeg'
lLeg = 'LLEG_JOINT0'
-lLegOffset = [0,-0.105,0]
-lLegNormal = [0,1,0]
+lLegOffset = [0,0,-0.105]
+lLegNormal = [0,0,1]
lLegx = 0.09; lLegy = 0.05
fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 20000, "manipulability", 0.1)
@@ -131,7 +131,7 @@ fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId])
-configs = fullBody.interpolate(0.08,pathId=0,robustnessTreshold = 1, filterStates = True)
+configs = fullBody.interpolate(0.1,pathId=1,robustnessTreshold = 3, filterStates = True)
print "number of configs :", len(configs)
diff --git a/script/dynamic/stair_bauzil_hrp2_path.py b/script/dynamic/stair_bauzil_hrp2_path.py
index bf24bd0930e9ceb8075ee1be193ac8a99617f68d..d68f1fd64623e90a007b893fa15d94986ec3722e 100644
--- a/script/dynamic/stair_bauzil_hrp2_path.py
+++ b/script/dynamic/stair_bauzil_hrp2_path.py
@@ -31,7 +31,7 @@ srdfSuffix = ""
rbprmBuilder = Builder ()
rbprmBuilder.loadModel(urdfName, urdfNameRoms, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
-rbprmBuilder.setJointBounds ("base_joint_xyz", [0,1.55, -0.8, -0.5, 0.50, 1.3])
+rbprmBuilder.setJointBounds ("base_joint_xyz", [0,1.55, -0.7, -0.5, 0.50, 1.3])
#rbprmBuilder.setJointBounds ("base_joint_xyz", [0,2, -1, 1, 0, 2.2])
rbprmBuilder.setJointBounds('CHEST_JOINT0',[0,0])
rbprmBuilder.setJointBounds('CHEST_JOINT1',[0,0.45])
@@ -47,7 +47,7 @@ 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])
+rbprmBuilder.client.basic.robot.setExtraConfigSpaceBounds([-vMax,vMax,-0.2,0.2,-vMax,vMax,0,0,0,0,0,0])
indexECS = rbprmBuilder.getConfigSize() - rbprmBuilder.client.basic.robot.getDimensionExtraConfigSpace()
#~ from hpp.corbaserver.rbprm. import ProblemSolver
@@ -67,6 +67,8 @@ q_init = rbprmBuilder.getCurrentConfig ();
q_init [0:3] = [0, -0.65, 0.55];
q_init[8] = 0.4
+#q_init [0:3] = [0, -0.65, 0.58];
+#q_init[8] = 0.43
rbprmBuilder.setCurrentConfig (q_init); r (q_init)
q_goal = q_init [::]
@@ -97,22 +99,23 @@ ps.selectPathPlanner("DynamicPlanner")
#ps.client.problem.finishSolveStepByStep()
-r.solveAndDisplay("rm",1,0.01)
+#r.solveAndDisplay("rm",1,0.01)
ps.solve()
+# was 5.5
+ps.client.problem.extractPath(0,5.7,ps.client.problem.pathLength(0))
+
# Playing the computed path
from hpp.gepetto import PathPlayer
pp = PathPlayer (rbprmBuilder.client.basic, r)
pp.dt=1/30.
-r.client.gui.removeFromGroup("rm",r.sceneName)
-pp.displayVelocityPath(0)
-pp.speed=1
-pp(0)
-
-
+#r.client.gui.removeFromGroup("rm",r.sceneName)
+pp.displayVelocityPath(1)
+pp.speed=0.25
+#pp(0)
diff --git a/script/dynamic/wall_hrp2_interp.py b/script/dynamic/wall_hrp2_interp.py
index 5a5ad047275026fffc87401152c972c9290a82bb..67552de4a9fdf41bcf90eb1565cf1b44a07e53ca 100644
--- a/script/dynamic/wall_hrp2_interp.py
+++ b/script/dynamic/wall_hrp2_interp.py
@@ -28,15 +28,15 @@ 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]
+rLegOffset = [0,0,-0.105]
+rLegNormal = [0,0,1]
rLegx = 0.09; rLegy = 0.05
fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 20000, "manipulability", 0.1)
lLegId = '1lLeg'
lLeg = 'LLEG_JOINT0'
-lLegOffset = [0,-0.105,0]
-lLegNormal = [0,1,0]
+lLegOffset = [0,0,-0.105]
+lLegNormal = [0,0,1]
lLegx = 0.09; lLegy = 0.05
fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 20000, "manipulability", 0.1)
diff --git a/script/tools/displayCOntact.py b/script/tools/displayCOntact.py
index 4716022e6be58f4d439eda29310bde42241376ff..a73c95ffb5c54ce7b030bedc78b60e63221a83f0 100644
--- a/script/tools/displayCOntact.py
+++ b/script/tools/displayCOntact.py
@@ -25,9 +25,9 @@ r.client.gui.refresh()
""""""""""""""""""""""""""""""""""""""""""""
# stairs :
-q = [[0.440782 , -0.629846 , 6.94538e-310],[0.440054 , -1.19294 , 0.325858],[0.447571 , -1.195 , 0.407322],]
+q= [[0.445947 , -0.487313 , 0.15],[0.413544 , -1.19627 , 0.82163],[0.436797 , -0.761877 , 0.15],]
-q2 = [[0.320782 , -0.559846 , 2.62221e-09],[0.320782 , -0.699846 , -2.62221e-09],[0.560782 , -0.559846 , 2.62221e-09],[0.560782 , -0.699846 , -2.62221e-09],[0.344786 , -1.13056 , 0.277322],[0.344786 , -1.2553 , 0.277311],[0.535321 , -1.13057 , 0.374404],[0.535321 , -1.25531 , 0.374393],[0.352304 , -1.13263 , 0.358786],[0.352304 , -1.25737 , 0.358776],[0.542839 , -1.13263 , 0.455868],[0.542839 , -1.25737 , 0.455858],]
+q2 = [[0.325947 , -0.417313 , 0.15],[0.325947 , -0.557313 , 0.15],[0.565947 , -0.417313 , 0.15],[0.565947 , -0.557313 , 0.15],[0.318277 , -1.1339 , 0.773095],[0.318277 , -1.25864 , 0.773084],[0.508811 , -1.13391 , 0.870177],[0.508811 , -1.25865 , 0.870166],[0.316797 , -0.691877 , 0.15],[0.316797 , -0.831877 , 0.15],[0.556797 , -0.691877 , 0.15],[0.556797 , -0.831877 , 0.15],]
""""""""""""""""""""""""""""""""""""""""""""