diff --git a/profile/profile.py b/profile/profile.py
new file mode 100644
index 0000000000000000000000000000000000000000..df5678797d7a3309f86d4e28191b47cca6c9eb4c
--- /dev/null
+++ b/profile/profile.py
@@ -0,0 +1,104 @@
+#!/usr/bin/env python
+
+import subprocess as sp
+import os
+
+scenarios = ['stair_bauzil_hrp2']#,'standing_hrp2','darpa_hrp2']
+n_trials = 3
+
+
+python_script_extension = "_interp.py"
+analysis = {}
+lower = -100000
+higher = 100000000
+
+#~
+#~ *** PROFILING RESULTS [ms] (min - avg - max - lastTime - nSamples) ***
+#~ complete generation 5110.09 5110.09 5110.09 5110.09 1
+#~ test balance 0.09 0.22 0.60 0.54 1199
+#~ contact: 20
+#~ no contact: 8
+#~ planner succeeded: 1
+#~ unstable contact: 2
+#~ path computation 0.81
+#~
+#~ *** PROFILING RESULTS [ms] (min - avg - max - lastTime - nSamples) ***
+#~ complete generation 7162.32 7162.32 7162.32 7162.32 1
+#~ test balance 0.09 0.20 0.63 0.50 1431
+#~ contact: 21
+#~ no contact: 11
+#~ planner failed: 1
+#~ unstable contact: 3
+
+def avg(l):
+ return reduce(lambda x, y: x + y, l) / len(l)
+
+
+def parseData(scenario):
+ filename = scenario+"_log.txt";
+ #~ os.rename("log.txt", filename)
+ analysis[scenario] = {}
+ data = analysis[scenario]
+ data['no contact'] = 0
+ data['contact'] = 0
+ data['unstable contact'] = 0
+ data['balance_min'] = higher
+ data['balance_max'] = lower
+ data['balance'] = []
+ data['minnumbalance'] = higher
+ data['maxnumbalance'] = lower
+ data['numbalance'] = []
+ data['mingen_time'] = higher
+ data['maxgen_time'] = lower
+ data['gen_time'] = []
+ data['failed'] = 0
+ data['success'] = 0
+ data['path_time'] = []
+ file = open(filename,"r+");
+ for line in file.readlines():
+ if not (line.find('complete generation') == -1):
+ time = float(line.split()[3])
+ data['mingen_time'] = min(data['mingen_time'],float(time))
+ data['maxgen_time'] = max(data['maxgen_time'],float(time))
+ data['gen_time'].append(time);
+ elif not (line.find('test balance') == -1):
+ print line.split()
+ _1, _2, _min, _avg, _max, _last, _numiter = line.split()
+ data['balance_min'] = min(data['balance_min'],float(_min))
+ data['balance_max'] = max(data['balance_max'],float(_max))
+ data['minnumbalance'] = min(data['minnumbalance'],float(_numiter))
+ data['maxnumbalance'] = max(data['maxnumbalance'],float(_numiter))
+ data['numbalance'].append(float(_numiter))
+ for i in range(0,1): #int(_numiter)):
+ data['balance'].append(float(_avg))
+ elif not (line.find('no contact:') == -1):
+ data['no contact'] = data['no contact'] + float(line.rstrip("\n").split()[2]);
+ elif not (line.find('unstable contact:') == -1):
+ data['unstable contact'] = data['unstable contact'] + float(line.rstrip("\n").split()[2]);
+ elif not (line.find('contact:') == -1):
+ data['contact'] = data['contact'] + float(line.rstrip("\n").split()[1]);
+ elif not (line.find('planner failed:') == -1):
+ data['failed'] = data['failed'] + 1;
+ elif not (line.find('planner succeeded') == -1):
+ data['success'] = data['success'] + 1;
+ elif not (line.find('path computation') == -1):
+ data['path_time'].append(float(line.rstrip("\n").split()[2]));
+
+def analyzeData():
+ for scenario in scenarios:
+ data = analysis[scenario]
+ data['balance'] = avg(data['balance'])
+ data['numbalance'] = avg(data['numbalance'])
+ data['path_time'] = avg(data['path_time'])
+ data['gen_time'] = avg(data['gen_time'])
+ print(analysis)
+
+for scenario in scenarios:
+ for i in range(0,n_trials):
+ var = scenario+python_script_extension;
+ #~ print(var);
+ #~ sp.check_call(["./profile.sh", str(var)]);
+ # move log
+ parseData(scenario)
+ analyzeData()
+ #~ os.rename("logpath.txt", scenario+"_log_path.txt")
diff --git a/profile/profile.sh b/profile/profile.sh
new file mode 100755
index 0000000000000000000000000000000000000000..15b70e3d9e40258b1b06fd649903e4749d6a3106
--- /dev/null
+++ b/profile/profile.sh
@@ -0,0 +1,8 @@
+#!/bin/bash
+
+gepetto-viewer-server &
+hpp-rbprm-server &
+ipython ../script/scenarios/$1
+
+pkill -f 'gepetto-viewer-server'
+pkill -f 'hpp-rbprm-server'
diff --git a/script/scenarios/darpa_hrp2_path.py b/script/scenarios/darpa_hrp2_path.py
index 14bb947102ce81f7eabb349edab1dfb677a83788..3d3d5c38cfd60f7153ba012ec7db71dd74f8f3f1 100644
--- a/script/scenarios/darpa_hrp2_path.py
+++ b/script/scenarios/darpa_hrp2_path.py
@@ -41,7 +41,10 @@ ps.addGoalConfig (q_goal)
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
r.loadObstacleModel (packageName, "darpa", "planning")
-ps.solve ()
+t = ps.solve ()
+f = open('log.txt', 'a')
+f.write("path computation " + str(t) + "\n")
+f.close()
from hpp.gepetto import PathPlayer
@@ -49,8 +52,8 @@ 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 (0)
-pp (1)
+#~ pp (1)
#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
r (q_init)
diff --git a/script/scenarios/darpa_hyq_path.py b/script/scenarios/darpa_hyq_path.py
index 60d932d412b1fc9a9ee3ff0074e283b9c0296312..5898046914a564da195ce7b8d6307af3aa4c5997 100644
--- a/script/scenarios/darpa_hyq_path.py
+++ b/script/scenarios/darpa_hyq_path.py
@@ -43,7 +43,10 @@ ps.addGoalConfig (q_goal)
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
r.loadObstacleModel (packageName, "darpa", "planning")
-ps.solve ()
+t = ps.solve ()
+f = open('log.txt', 'a')
+f.write("path computation " + str(t) + "\n")
+f.close()
from hpp.gepetto import PathPlayer
@@ -55,6 +58,6 @@ pp = PathPlayer (rbprmBuilder.client.basic, r)
#~ pp (2)
#~ pp (0)
-pp (1)
+#~ pp (1)
#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
r (q_init)
diff --git a/script/scenarios/ground_crouch_hyq_path.py b/script/scenarios/ground_crouch_hyq_path.py
index 1a4eeacdfaeeaa9a8ee03d543fdb39497beb897a..8ddbf8d5efba361965ad4b71a82ff02a010a9a3e 100644
--- a/script/scenarios/ground_crouch_hyq_path.py
+++ b/script/scenarios/ground_crouch_hyq_path.py
@@ -42,7 +42,10 @@ ps.addGoalConfig (q_goal)
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation",0.01)
r.loadObstacleModel (packageName, "groundcrouch", "planning")
-ps.solve ()
+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")
diff --git a/script/scenarios/stair_bauzil_hrp2_path.py b/script/scenarios/stair_bauzil_hrp2_path.py
index 080c7a69bd2966e450e07ce69db971a53c9655d9..829c65abcd614e19fc8669b8135921509f8aac08 100644
--- a/script/scenarios/stair_bauzil_hrp2_path.py
+++ b/script/scenarios/stair_bauzil_hrp2_path.py
@@ -47,8 +47,13 @@ ps.addGoalConfig (q_goal)
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
r.loadObstacleModel (packageName, "stair_bauzil", "planning")
-ps.solve ()
+#~ ps.solve ()
+t = ps.solve ()
+f = open('log.txt', 'a')
+f.write("path computation " + str(t) + "\n")
+f.close()
+print ("solving time " + str(t));
from hpp.gepetto import PathPlayer
pp = PathPlayer (rbprmBuilder.client.basic, r)
diff --git a/script/scenarios/stair_bauzil_hyq_path.py b/script/scenarios/stair_bauzil_hyq_path.py
index e98ae9bd2cf26fb700ab35e6d348bb1992f07040..e929fe0dc4b0368ae27e03c5c60259287dd94876 100644
--- a/script/scenarios/stair_bauzil_hyq_path.py
+++ b/script/scenarios/stair_bauzil_hyq_path.py
@@ -41,7 +41,9 @@ ps.addGoalConfig (q_goal)
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
r.loadObstacleModel (packageName, "stair_bauzil", "planning")
-ps.solve ()
+t = ps.solve ()
+
+print ("solving time " + t);
from hpp.gepetto import PathPlayer
diff --git a/script/scenarios/standing_hrp2_path.py b/script/scenarios/standing_hrp2_path.py
index 7fe43b5faae1b014641d20092dfb363745e20d91..e19b56f466ff2733a12c847eb1e16c4858290eb1 100644
--- a/script/scenarios/standing_hrp2_path.py
+++ b/script/scenarios/standing_hrp2_path.py
@@ -53,7 +53,10 @@ ps.addGoalConfig (q_goal)
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation",0.01)
r.loadObstacleModel (packageName, "scene_wall", "planning")
-ps.solve ()
+t = ps.solve ()
+f = open('log.txt', 'a')
+f.write("path computation " + str(t) + "\n")
+f.close()
from hpp.gepetto import PathPlayer
diff --git a/script/scenarios/truck_hrp2_path.py b/script/scenarios/truck_hrp2_path.py
index a2b0bb6cd00608b46d1e2f06c1a6d885f9537eb2..5830b7542af499da4580e8cdb1ffb9f3f59e6171 100644
--- a/script/scenarios/truck_hrp2_path.py
+++ b/script/scenarios/truck_hrp2_path.py
@@ -56,8 +56,10 @@ ps.addGoalConfig (q_goal)
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
r.loadObstacleModel (packageName, "truck", "planning")
-ps.solve ()
-
+t = ps.solve ()
+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)
diff --git a/src/rbprmbuilder.impl.cc b/src/rbprmbuilder.impl.cc
index 3d956fc23c33c4c581cdfdb7f7871f4b775ce55b..a44e660a6209716bcc4d8a29ebc98629bd7ced10 100755
--- a/src/rbprmbuilder.impl.cc
+++ b/src/rbprmbuilder.impl.cc
@@ -477,14 +477,15 @@ namespace hpp {
void RbprmBuilder::saveComputedStates(const char* outfilename) throw (hpp::Error)
{
std::stringstream ss;
- ss << lastStatesComputed_.size()-1 << "\n";
- std::vector<rbprm::State>::const_iterator cit = lastStatesComputed_.begin()+1;
+ ss << lastStatesComputed_.size()-2 << "\n";
+ std::vector<rbprm::State>::iterator cit = lastStatesComputed_.begin()+1;
int i = 0;
ss << i++ << " ";
cit->print(ss);
- for(std::vector<rbprm::State>::const_iterator cit2 = lastStatesComputed_.begin()+2;
- cit2 != lastStatesComputed_.end(); ++cit2, ++cit, ++i)
+ for(std::vector<rbprm::State>::iterator cit2 = lastStatesComputed_.begin()+2;
+ cit2 != lastStatesComputed_.end()-1; ++cit2, ++cit, ++i)
{
+ cit2->robustness = stability::IsStable(this->fullBody_, *cit2);
ss << i<< " ";
cit2->print(ss,*cit);
}