diff --git a/CMakeLists.txt b/CMakeLists.txt
index a894528e77523ab558a75e13a17ed0ee16217b47..ae409074be1203b79eb7b8a4aaf37529840210f4 100755
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -120,6 +120,7 @@ install(FILES
data/meshes/truck.stl
data/meshes/truck_vision.stl
data/meshes/stair_bauzil.stl
+ data/meshes/stair_bauzil_reduced.stl
data/meshes/climb.stl
data/meshes/stepladder.stl
data/meshes/chair.stl
diff --git a/data/meshes/stair_bauzil.stl b/data/meshes/stair_bauzil.stl
index 13c37c2ace14fbdab4c404cb6f5c5ea270b7bf55..7b608963d78806e796ce789a4b234f0ef90bc774 100644
Binary files a/data/meshes/stair_bauzil.stl and b/data/meshes/stair_bauzil.stl differ
diff --git a/data/meshes/stair_bauzil_reduced.stl b/data/meshes/stair_bauzil_reduced.stl
new file mode 100644
index 0000000000000000000000000000000000000000..3c1e07ce7b5872ee3602baa813ef5cf2dc1afaec
Binary files /dev/null and b/data/meshes/stair_bauzil_reduced.stl differ
diff --git a/data/stair_bauzil.blend b/data/stair_bauzil.blend
index 992e4e21ce52332e8b3c031f5f521a506a8335b8..afa523a04285e659f3148abac75fbc908771f4d6 100644
Binary files a/data/stair_bauzil.blend and b/data/stair_bauzil.blend differ
diff --git a/data/stair_bauzilreduced.blend b/data/stair_bauzilreduced.blend
new file mode 100644
index 0000000000000000000000000000000000000000..ebddb79a25631b3f6f8235f722fee128f46188c9
Binary files /dev/null and b/data/stair_bauzilreduced.blend differ
diff --git a/data/urdf/stair_bauzil.urdf b/data/urdf/stair_bauzil.urdf
index 5a49b44d233a4bdc5a39eb51ad62dc1066e63e72..3635b5d8b0a4dbf9523b2512789dff1289709987 100644
--- a/data/urdf/stair_bauzil.urdf
+++ b/data/urdf/stair_bauzil.urdf
@@ -12,7 +12,7 @@
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
- <mesh filename="package://hpp-rbprm-corba/meshes/stair_bauzil.stl"/>
+ <mesh filename="package://hpp-rbprm-corba/meshes/stair_bauzil_reduced.stl"/>
</geometry>
</collision>
</link>
diff --git a/script/loadhrp2stair_bauzil.py b/script/loadhrp2stair_bauzil.py
index a26b2de2ebce40b2d70f5d10cd78a53322622a7d..9c6cb81e894eecab656e651751796a8c994cf88e 100644
--- a/script/loadhrp2stair_bauzil.py
+++ b/script/loadhrp2stair_bauzil.py
@@ -18,7 +18,7 @@ srdfSuffix = ""
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
-fullBody.setJointBounds ("base_joint_xyz", [0,2, -1, 1, 0, 2.2])
+fullBody.setJointBounds ("base_joint_xyz", [-0.135,2, -1, 1, 0, 2.2])
ps = tp.ProblemSolver( fullBody )
@@ -43,8 +43,8 @@ fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 10000, 0.01
rarmId = '3Rarm'
rarm = 'RARM_JOINT0'
rHand = 'RARM_JOINT5'
-rArmOffset = [-0.05,-0.050,-0.050]
-rArmNormal = [1,0,0]
+rArmOffset = [0,0,-0.1]
+rArmNormal = [0,0,1]
rArmx = 0.024; rArmy = 0.024
fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 30000, 0.05)
@@ -85,12 +85,19 @@ q_goal = fullBody.getCurrentConfig(); q_goal[0:7] = tp.q_goal[0:7]
fullBody.setCurrentConfig (q_init)
q_0 = fullBody.getCurrentConfig();
-q_init = fullBody.generateContacts(q_init, [0,0,1]); r (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.174532925, # LARM 11-17
+ 0.261799388, -0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.174532925, # 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)
+#~ r(q_goal)
q_goal = fullBody.generateContacts(q_goal, [0,0,1])
-r(q_goal)
+#~ r(q_goal)
fullBody.setStartState(q_init,[rLegId,lLegId]) #,rarmId,larmId])
fullBody.setEndState(q_goal,[rLegId,lLegId])#,rarmId,larmId])
@@ -98,11 +105,12 @@ fullBody.setEndState(q_goal,[rLegId,lLegId])#,rarmId,larmId])
#~ configs = fullBody.interpolate(0.1)
configs = fullBody.interpolate(0.1)
#~ configs = fullBody.interpolate(0.15)
-i = 1;
+i = 0;
r (configs[i]); i=i+1; i-1
#~ q_init = fullBody.generateContacts(q_init, [0,0,-1]); r (q_init)
-f1 = open("secondchoice","w+")
+#~ f1 = open("secondchoice","w+")
+f1 = open("new","w+")
f1.write(str(configs))
f1.close()
diff --git a/script/stair_path_bauzil_hrp2.py b/script/stair_path_bauzil_hrp2.py
index 9d85ddefba0576dfb0a4a21d1f3a74dfd5f69898..2799215d7b499005e7762c2ace57055467e549de 100644
--- a/script/stair_path_bauzil_hrp2.py
+++ b/script/stair_path_bauzil_hrp2.py
@@ -23,12 +23,15 @@ r = Viewer (ps)
q_init = rbprmBuilder.getCurrentConfig ();
-q_init [0:3] = [0, -0.7, 0.6]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
+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_init [3:7] = [ 0.98877108, 0. , 0.14943813, 0. ]
q_goal = q_init [::]
-q_goal [0:3] = [1.49, -0.6, 1.25]; r (q_goal)
+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")
diff --git a/script/stair_path_hrp2.py b/script/stair_path_hrp2.py
index 45634be4c19f4fdf6f3abc0f95037cd12ea03378..5f21989fe49c6e1d8087870dd8c900c81da44c88 100644
--- a/script/stair_path_hrp2.py
+++ b/script/stair_path_hrp2.py
@@ -4,15 +4,16 @@ from hpp.gepetto import Viewer
rootJointType = 'freeflyer'
packageName = 'hpp-rbprm-corba'
meshPackageName = 'hpp-rbprm-corba'
-urdfName = 'hrp2_trunk'
-urdfNameRom = 'hrp2_rom'
+urdfName = 'hrp2_trunk_flexible'
+urdfNameRoms = ['hrp2_larm_rom','hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom']
urdfSuffix = ""
srdfSuffix = ""
rbprmBuilder = Builder ()
-rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+rbprmBuilder.loadModel(urdfName, urdfNameRoms, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds ("base_joint_xyz", [-1,1, -1, 1, 0, 2.2])
+rbprmBuilder.setFilter(['hrp2_larm_rom','hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom'])
#~ from hpp.corbaserver.rbprm. import ProblemSolver
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
diff --git a/src/rbprmbuilder.impl.cc b/src/rbprmbuilder.impl.cc
index 97ef3c2a01e9f49c152232c7e0e8b9761269ab82..0e950c18eebd8cf60275893394b63e5229d95fa4 100755
--- a/src/rbprmbuilder.impl.cc
+++ b/src/rbprmbuilder.impl.cc
@@ -21,6 +21,7 @@
#include "hpp/rbprm/rbprm-device.hh"
#include "hpp/rbprm/rbprm-validation.hh"
#include "hpp/rbprm/rbprm-path-interpolation.hh"
+#include "hpp/rbprm/stability/stability.hh"
#include "hpp/model/urdf/util.hh"
#include <fstream>
@@ -342,6 +343,7 @@ namespace hpp {
core::Configuration_t old = fullBody->device_->currentConfiguration();
model::Configuration_t config = dofArrayToConfig (fullBody->device_, configuration);
fullBody->device_->currentConfiguration(config);
+ fullBody->device_->computeForwardKinematics();
std::vector<std::string> names = stringConversion(contactLimbs);
for(std::vector<std::string>::const_iterator cit = names.begin(); cit != names.end();++cit)
{
@@ -362,6 +364,7 @@ namespace hpp {
}
state.nbContacts = state.contactNormals_.size() ;
state.configuration_ = config;
+ state.stable = stability::IsStable(fullBody,state);
fullBody->device_->currentConfiguration(old);
}