profiling now includes trajectory optimization

.gitignore

@@ -30,5 +30,6 @@
 *.app
 
 build/
+profile/logs
 
 CMakeLists.txt.user

profile/profile.py

@@ -2,20 +2,27 @@
 
 import subprocess as sp
 import os
+import shutil
 import datetime
 
 #~ scenarios = ['standing_hrp2']
-scenarios = ['car_hrp2']
+scenarios = ['ground_crouch_hyq_hole']
 #~ scenarios = ['stair_bauzil_hrp2']
-n_trials = 100
+n_trials = 1
 
-stats = ['balance','collision','ik','cwc_optim','com_traj']
+stats = ['balance','collision','ik']
+stats_optim = ['time_cwc','com_traj']
+allstats = stats + stats_optim
 
 python_script_extension = "_interp.py"
 analysis = {}
 lower = -100000
 higher = 100000000
 
+log_dir = "./logs"
+if not os.path.exists(log_dir):
+	os.makedirs(log_dir)
+
 def avg(l):
 	if(len(l) == 0):
 		return -1
@@ -53,7 +60,8 @@ def parseLine(sep, name, line, data):
 	
 def parseData(scenario):
 	filename = scenario+"_log.txt";
-	os.rename("log.txt", filename)
+	os.rename("log.txt", filename)	
+	shutil.move("./"+filename, log_dir+"/"+filename)
 	analysis[scenario] = {}
 	data = analysis[scenario]
 	data['no contact'] = 0
@@ -74,9 +82,9 @@ def parseData(scenario):
 	data[     "optim_num_success"] = 0
 	data[      "optim_num_trials"] = 0
 	data[            "num_errors"] = 0
-	for stat in stats:
+	for stat in allstats:
 		initdata(stat, data)	
-	file = open(filename,"r+");
+	file = open(log_dir+"/"+filename,"r+");
 	for line in file.readlines():
 		if not (line.find('complete generation') == -1):
 			time = float(line.split()[3])
@@ -106,25 +114,25 @@ def parseData(scenario):
 			data['max_path_time'] = max(data['max_path_time'],time)
 		#now, analyzing optimization
 		elif not (line.find('optim_error_optim_fail') == -1):
-			val = float(line.rstrip("\n").split()[2])
+			val = float(line.rstrip("\n").split()[1])
 			data['optim_error_optim_fail']+= val;
 		elif not (line.find('optim_error_com_proj') == -1):
-			val = float(line.rstrip("\n").split()[2])
+			val = float(line.rstrip("\n").split()[1])
 			data['optim_error_com_proj']+= val;
 		elif not (line.find('optim_error_unknown') == -1):
-			val = float(line.rstrip("\n").split()[2])
+			val = float(line.rstrip("\n").split()[1])
 			data['optim_error_unknown']+= val;
 		elif not (line.find('optim_num_success') == -1):
-			val = float(line.rstrip("\n").split()[2])
+			val = float(line.rstrip("\n").split()[1])
 			data['optim_num_success']+= val;
 		elif not (line.find('optim_num_trials') == -1):
-			val = float(line.rstrip("\n").split()[2])
+			val = float(line.rstrip("\n").split()[1])
 			data['optim_num_trials']+= val;
 		elif not (line.find('num_errors') == -1):
-			val = float(line.rstrip("\n").split()[2])
+			val = float(line.rstrip("\n").split()[1])
 			data['num_errors']+= val;
-		elif not (line.find('cwc_optim') == -1):
-			parseLine(1, 'cwc_optim', line, data)
+		elif not (line.find('time_cwc') == -1):
+			parseLine(1, 'time_cwc', line, data)
 		elif not (line.find('com_traj') == -1):
 			parseLine(1, 'com_traj', line, data)
 
@@ -141,9 +149,21 @@ def printOneStat(f, name, data, g_time, tTime):
 	f.write ("\t total time (min / avg / max / % of total / % of total wtht path planning):\n \t " + str(d['total_'+n+'_min']) +  "\t" + str(d['total_'+n]) + "\t" + str(d['total_'+n+'_max']) + "\t" +  str(float(d['total_'+n]) / tTime * 100) + "%" + "\t" +  str(float(d['total_'+n]) / g_time * 100) + "%\n")
 	f.write ("\t number of tests (min / avg / max):\n \t " + str(d['minnum'+n]) +  "\t" + str(d['num'+n]) + "\t" + str(d['maxnum'+n]) + "\n")
 		
+def printOneStatOptim(f, name, data):
+	n = name
+	d = data
+	print(n, len(d[n]))
+	d[n] = avg(d[n])
+	d['num'+n] = avg(d['num'+n])
+	d['total_'+n] = avg(d['total_'+n])
+	f.write (n +" tests: \n")
+	f.write ("\t single operation time (min / avg / max):\n \t " + str(d[n+'_min']) +  "\t" + str(d[n]) + "\t" + str(d[n+'_max']) + "\t \n")
+	f.write ("\t total time (min / avg / max ):\n \t " + str(d['total_'+n+'_min']) +  "\t" + str(d['total_'+n]) + "\t" + str(d['total_'+n+'_max']) + "\t"  + "%\n")
+	f.write ("\t number of tests (min / avg / max):\n \t " + str(d['minnum'+n]) +  "\t" + str(d['num'+n]) + "\t" + str(d['maxnum'+n]) + "\n")
+	
 		
 def analyzeData():
-	f = open("log_"+str(datetime.datetime.now())+".txt","w+")
+	f = open(log_dir+"/log_"+str(datetime.datetime.now())+".txt","w+")
 		
 	for scenario in scenarios:
 		d = analysis[scenario]
@@ -173,12 +193,16 @@ def analyzeData():
 		f.write ("% of unstable contact generation (no balanced candidates found): " + str(uc / tc * 100)  + "%\n")
 		f.write ("\n")
 		
-		f.write ("***********************%\n")
-		f.write (" optimization related data %\n")
-		f.write ("% succes rate of optimization: " + str(data["optim_num_success"] / data["optim_num_trials"] * 100)  + "%\n")
-		f.write ("% succes rate of optimization disregarding collision: " + str(data["optim_num_success"] / (data["optim_num_trials"] - data["optim_error_com_proj"]) * 100)  + "%\n")
-		f.write ("% errors due to problem infeasibility (% of errors, % over all trials): " + str(data["optim_error_optim_fail"] / data["num_errors"] * 100) + " " str(data["optim_error_optim_fail"] / data["optim_num_trials"] * 100)  + "%\n")
-		f.write ("% errors due to unknown reasons (% of errors, % over all trials): " + str(data["optim_error_unknown"] / data["num_errors"] * 100) + " " str(data["optim_error_unknown"] / data["optim_num_trials"] * 100)  + "%\n")
+		if(d["optim_num_trials"] > 0):
+			f.write ("***********************%\n")
+			f.write (" optimization related data %\n")
+			f.write ("% succes rate of optimization: " + str(d["optim_num_success"] / d["optim_num_trials"] * 100)  + "%\n")
+			f.write ("% succes rate of optimization disregarding collision: " + str(d["optim_num_success"] / (d["optim_num_trials"] - d["optim_error_com_proj"]) * 100)  + "%\n")
+			if(d["num_errors"] > 0):
+				f.write ("% errors due to problem infeasibility (% of errors, % over all trials): " + str(d["optim_error_optim_fail"] / d["num_errors"] * 100) + " " + str(d["optim_error_optim_fail"] / d["optim_num_trials"] * 100)  + "%\n")
+				f.write ("% errors due to unknown reasons (% of errors, % over all trials): " + str(d["optim_error_unknown"] / d["num_errors"] * 100) + " " + str(d["optim_error_unknown"] / d["optim_num_trials"] * 100)  + "%\n")
+			for stat in stats_optim:
+				printOneStatOptim(f, stat, d)
 		f.write ("\n \n \n")
 	f.close()
 	

profile/scenarios/ground_crouch_hyq_bridge.py

@@ -90,7 +90,7 @@ from hpp.gepetto import PathPlayer
 pp = PathPlayer (fullBody.client.basic, r)
 
 
-from hpp.corbaserver.rbprm.tools.cwc_trajectory_helper import step, clean,stats, saveAllData, play_traj
+from hpp.corbaserver.rbprm.tools.cwc_trajectory_helper import step, clean,stats, saveAllData, play_traj, profile
 
 	
 limbsCOMConstraints = { rLegId : {'file': "hyq/"+rLegId+"_com.ineq", 'effector' : rfoot},  

profile/scenarios/ground_crouch_hyq_hole_interp.py

+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+from hpp.gepetto import Viewer
+
+import ground_crouch_hyq_path as tp
+
+packageName = "hyq_description"
+meshPackageName = "hyq_description"
+rootJointType = "freeflyer"
+##
+#  Information to retrieve urdf and srdf files.
+urdfName = "hyq"
+urdfSuffix = ""
+srdfSuffix = ""
+
+fullBody = FullBody ()
+ 
+fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+fullBody.setJointBounds ("base_joint_xyz", [-6,5, -4, 4, 0.6, 2])
+
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+
+nbSamples = 20000
+
+ps = tp.ProblemSolver( fullBody )
+r = tp.Viewer (ps)
+
+rootName = 'base_joint_xyz'
+
+#  Creating limbs
+# cType is "_3_DOF": positional constraint, but no rotation (contacts are punctual)
+cType = "_3_DOF"
+# string identifying the limb
+rLegId = 'rfleg'
+# First joint of the limb, as in urdf file
+rLeg = 'rf_haa_joint'
+# Last joint of the limb, as in urdf file
+rfoot = 'rf_foot_joint'
+# Specifying the distance between last joint and contact surface
+offset = [0.,-0.021,0.]
+# Specifying the contact surface direction when the limb is in rest pose
+normal = [0,1,0]
+# Specifying the rectangular contact surface length
+legx = 0.02; legy = 0.02
+# remaining parameters are the chosen heuristic (here, manipulability), and the resolution of the octree (here, 10 cm).
+fullBody.addLimb(rLegId,rLeg,rfoot,offset,normal, legx, legy, nbSamples, "forward", 0.1, cType)
+
+lLegId = 'lhleg'
+lLeg = 'lh_haa_joint'
+lfoot = 'lh_foot_joint'
+fullBody.addLimb(lLegId,lLeg,lfoot,offset,normal, legx, legy, nbSamples, "backward", 0.05, cType)
+
+rarmId = 'rhleg'
+rarm = 'rh_haa_joint'
+rHand = 'rh_foot_joint'
+fullBody.addLimb(rarmId,rarm,rHand,offset,normal, legx, legy, nbSamples, "backward", 0.05, cType)
+
+larmId = 'lfleg'
+larm = 'lf_haa_joint'
+lHand = 'lf_foot_joint'
+fullBody.addLimb(larmId,larm,lHand,offset,normal, legx, legy, nbSamples, "forward", 0.05, cType)
+
+q_0 = fullBody.getCurrentConfig(); 
+q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.q_init[0:7]
+q_goal = fullBody.getCurrentConfig(); q_goal[0:7] = tp.q_goal[0:7]
+
+fullBody.setCurrentConfig (q_init)
+q_init = fullBody.generateContacts(q_init, [0,0,1])
+q_0 = fullBody.getCurrentConfig(); 
+
+fullBody.setCurrentConfig (q_goal)
+q_goal = fullBody.generateContacts(q_goal, [0,0,1])
+
+fullBody.setStartState(q_init,[])
+fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
+
+r(q_init)
+
+configs = fullBody.interpolate(0.08,1,3) #hole 
+#~ configs = fullBody.interpolate(0.08,1,5) # bridge
+
+#~ r.loadObstacleModel ('hpp-rbprm-corba', "groundcrouch", "contact")
+#~ fullBody.exportAll(r, configs, 'obstacle_hyq_robust_10');
+i = 0;
+r (configs[i]); i=i+1; i-1
+
+
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (fullBody.client.basic, r)
+
+
+from hpp.corbaserver.rbprm.tools.cwc_trajectory_helper import step, clean,stats, saveAllData, play_traj, profile
+
+	
+limbsCOMConstraints = { rLegId : {'file': "hyq/"+rLegId+"_com.ineq", 'effector' : rfoot},  
+						lLegId : {'file': "hyq/"+lLegId+"_com.ineq", 'effector' : lfoot},  
+						rarmId : {'file': "hyq/"+rarmId+"_com.ineq", 'effector' : rHand},  
+						larmId : {'file': "hyq/"+larmId+"_com.ineq", 'effector' : lHand} }
+
+
+
+def act(i, numOptim = 0):
+	return step(fullBody, configs, i, numOptim, pp, limbsCOMConstraints, 0.5, optim_effectors = False, time_scale = 20., useCOMConstraints = False)
+
+def play(frame_rate = 1./24.):
+	play_traj(fullBody,pp,frame_rate)
+	
+def saveAll(name):
+	saveAllData(fullBody, r, name)
+#~ fullBody.exportAll(r, trajec, 'obstacle_hyq_t_var_04f_andrea');
+#~ saveToPinocchio('obstacle_hyq_t_var_04f_andrea')
+
+profile(fullBody, configs, 4, 10, limbsCOMConstraints)	
+fullBody.dumpProfile()
+    
+
+
+

profile/scenarios/ground_crouch_hyq_path.py

+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.gepetto import Viewer
+
+rootJointType = 'freeflyer'
+packageName = 'hpp-rbprm-corba'
+meshPackageName = 'hpp-rbprm-corba'
+urdfName = 'hyq_trunk'
+urdfNameRom = ['hyq_lhleg_rom','hyq_lfleg_rom','hyq_rfleg_rom','hyq_rhleg_rom']
+urdfSuffix = ""
+srdfSuffix = ""
+
+
+
+rbprmBuilder = Builder ()
+
+rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+rbprmBuilder.setJointBounds ("base_joint_xyz", [-6,5, -4, 4, 0.6, 2])
+rbprmBuilder.setFilter(['hyq_rhleg_rom', 'hyq_lfleg_rom', 'hyq_rfleg_rom','hyq_lhleg_rom'])
+rbprmBuilder.setAffordanceFilter('hyq_rhleg_rom', ['Support'])
+rbprmBuilder.setAffordanceFilter('hyq_rfleg_rom', ['Support',])
+rbprmBuilder.setAffordanceFilter('hyq_lhleg_rom', ['Support'])
+rbprmBuilder.setAffordanceFilter('hyq_lfleg_rom', ['Support',])
+rbprmBuilder.boundSO3([-0.1,0.1,-1,1,-1,1])
+
+#~ from hpp.corbaserver.rbprm. import ProblemSolver
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+
+ps = ProblemSolver( rbprmBuilder )
+
+r = Viewer (ps)
+
+
+q_init = rbprmBuilder.getCurrentConfig ();
+q_init [0:3] = [-5, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
+#~ q_init [0:3] = [-2,  0.47, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
+q_goal = q_init [::]
+q_goal [0:3] = [5, 0, 0.63]; r (q_goal)
+#~ q_goal [0:3] = [-2, 0, 0.63]; r (q_goal)
+
+ps.addPathOptimizer("RandomShortcut")
+ps.setInitialConfig (q_init)
+ps.addGoalConfig (q_goal)
+
+from hpp.corbaserver.affordance.affordance import AffordanceTool
+afftool = AffordanceTool ()
+afftool.loadObstacleModel (packageName, "groundcrouch", "planning", r)
+afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
+
+# Choosing RBPRM shooter and path validation methods.
+# Note that the standard RRT algorithm is used.
+ps.client.problem.selectConFigurationShooter("RbprmShooter")
+ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
+#~ r.loadObstacleModel (packageName, "groundcrouch", "planning")
+#~ ps.solve ()
+t = ps.solve ()
+
+
+if isinstance(t, list):
+	t = t[0]* 3600000 + t[1] * 60000 + t[2] * 1000 + t[3]
+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_robust_10_path.txt")
+
+
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (rbprmBuilder.client.basic, r)
+
+#~ pp (0)
+#~ pp (1)
+r (q_init)
+
+q_far = q_init [::]
+q_far [0:3] = [-2, -3, 0.63]; 
+r(q_far)

src/CMakeLists.txt

@@ -113,6 +113,7 @@ INSTALL(
         ${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/tools/cwc_trajectory_helper.py
         ${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/tools/path_to_trajectory.py
         ${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/tools/obj_to_constraints.py
+        ${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/tools/time_out.py
   DESTINATION ${PYTHON_SITELIB}/hpp/corbaserver/rbprm/tools
   )
   

src/hpp/corbaserver/rbprm/tools/cwc_trajectory.py

@@ -74,7 +74,8 @@ def gen_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, dt=
 		t_end_phases[2] = t_end_phases[1] + fly_time
 		t_end_phases[3] = t_end_phases[2] + support_time
 		t_end_phases = [float((int)(math.ceil(el*10.))) / 10. for el in t_end_phases]
-		print "end_phases", t_end_phases
+		if (not profile):
+			print "end_phases", t_end_phases
 	cones = None
 	if(computeCones):
 		cones = [fullBody.getContactCone(state_id, mu)[0]]
@@ -82,7 +83,8 @@ def gen_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, dt=
 			cones.append(fullBody.getContactIntermediateCone(state_id, mu)[0])
 		if(len(p) > len(cones)):
 			cones.append(fullBody.getContactCone(state_id+1, mu)[0])
-	print "num cones ", len(cones)
+	if (not profile):
+			print "num cones ", len(cones)
 	
 	COMConstraints = None
 	if(not (limbsCOMConstraints == None)):
@@ -177,7 +179,7 @@ def __optim__threading_ok(fullBody, states, state_id, computeCones = False, mu =
 	return comPosPerPhase, res[2] #res[2] is timeelapsed
 
 def solve_com_RRT(fullBody, states, state_id, computeCones = False, mu = 1, dt =0.1, phase_dt = [0.4, 1], reduce_ineq = True, num_optims = 0, draw = False, verbose = False, limbsCOMConstraints = None, profile = False):
-	comPosPerPhase, timeElapsed = __optim__threading_ok(fullBody, states, state_id, computeCones, mu, dt, phase_dt, reduce_ineq, num_optims, draw, verbose, limbsCOMConstraints)
+	comPosPerPhase, timeElapsed = __optim__threading_ok(fullBody, states, state_id, computeCones, mu, dt, phase_dt, reduce_ineq, num_optims, draw, verbose, limbsCOMConstraints, profile)
 	print "done. generating state trajectory ",state_id	
 	print "comePhaseLength", len(comPosPerPhase[0]),  len(comPosPerPhase[1]),  len(comPosPerPhase[2])
 	paths_ids = [int(el) for el in fullBody.comRRTFromPos(state_id,comPosPerPhase[0],comPosPerPhase[1],comPosPerPhase[2],num_optims)]
@@ -185,7 +187,7 @@ def solve_com_RRT(fullBody, states, state_id, computeCones = False, mu = 1, dt =
 	return paths_ids[-1], paths_ids[:-1], timeElapsed
 	
 def solve_effector_RRT(fullBody, states, state_id, computeCones = False, mu = 1, dt =0.1, phase_dt = [0.4, 1], reduce_ineq = True, num_optims = 0, draw = False, verbose = False, limbsCOMConstraints = None, profile = False):
-	comPosPerPhase, timeElapsed = __optim__threading_ok(fullBody, states, state_id, computeCones, mu, dt, phase_dt, reduce_ineq, num_optims, draw, verbose, limbsCOMConstraints)
+	comPosPerPhase, timeElapsed = __optim__threading_ok(fullBody, states, state_id, computeCones, mu, dt, phase_dt, reduce_ineq, num_optims, draw, verbose, limbsCOMConstraints, profile)
 	print "done. generating state trajectory ",state_id	
 	print "comePhaseLength", len(comPosPerPhase[0]),  len(comPosPerPhase[1]),  len(comPosPerPhase[2])
 	paths_ids = [int(el) for el in fullBody.effectorRRT(state_id,comPosPerPhase[0],comPosPerPhase[1],comPosPerPhase[2],num_optims)]

src/hpp/corbaserver/rbprm/tools/cwc_trajectory_helper.py

@@ -217,7 +217,7 @@ def step_profile(fullBody, configs, i, optim, limbsCOMConstraints,  friction = 0
 			dt = 0.05
 		else:
 			dt = 0.1
-		if(distance > 0.0001):			
+		if(distance > 0.0001):				
 			stat_data["num_trials"] += 1
 			if(useCOMConstraints):
 				comC = limbsCOMConstraints
@@ -334,14 +334,14 @@ def write_stats(filename):
 	return sd
 
 def profile(fullBody, configs, i_start, i_end, limbsCOMConstraints,  friction = 0.5, optim_effectors = False, time_scale = 20., useCOMConstraints = False, filename ="log.txt"):	
-	global stat_data	
-	if(i_end < len(configs)-1):
+	global stat_data		
+	if(i_end > len(configs)-1):
+		print "ERROR: i_end < len_configs ", i_end, len(configs)-1
 		return # no point in trying optim, path was not fully computed
-	for i in range(i_start, i_end):
-		step(fullBody, configs, i, 0, limbsCOMConstraints,  friction, optim_effectors, time_scale, useCOMConstraints)
+	for i in range(i_start, i_end):		
+		step_profile(fullBody, configs, i, 0, limbsCOMConstraints,  friction, optim_effectors, time_scale, useCOMConstraints)
 	stat_data["time_cwc"]["avg"] = stat_data["time_cwc"]["totaltime"] / float(stat_data["time_cwc"]["numiter"])
 	write_stats(filename)
-}
 
 def saveAllData(fullBody, r, name):
 	fullBody.exportAll(r, trajec, name)

src/hpp/corbaserver/rbprm/tools/time_out.py

+import signal
+
+# Register an handler for the timeout
+def handler_factory(name, t):
+	def handler(signum, frame):
+		raise Exception("end of time allowed for method " + name + ": " + str(t))
+	return handler
+
+def run_time_out(method, t, *args):
+	hd = handler_factory(method.__name__, t)
+	signal.signal(signal.SIGALRM, hd)
+	signal.alarm(t)
+	method(*args)
+	signal.alarm(0)
+	
+
+
+if __name__ == '__main__':
+	def loop(dt1, dt2):
+		import time
+		for i in range(dt1 + dt2):
+			print "sec"
+			time.sleep(1)
+        
+	try:
+		run_time_out(loop, 3, 3, 2)
+	except Exception, exc: 
+		print exc
+		print "exception caught, ok"
+		
+	try:
+		run_time_out(loop, 6, 3, 2)
+	except Exception, exc: 
+		print exc
+		print "exception should NOT becaught"