performance measure script almost done

profile/profile.py

+#!/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")

profile/profile.sh

+#!/bin/bash         
+
+gepetto-viewer-server & 
+hpp-rbprm-server &
+ipython ../script/scenarios/$1
+
+pkill -f  'gepetto-viewer-server'
+pkill -f  'hpp-rbprm-server'

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)

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)

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")
 
 

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)

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

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

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)

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);
         }