Commit ad475f18 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

profiling now includes trajectory optimization

parent 21ebbfb7
......@@ -30,5 +30,6 @@
*.app
build/
profile/logs
CMakeLists.txt.user
......@@ -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()
......
......@@ -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},
......
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()
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)
......@@ -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
)
......
......@@ -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)]
......
......@@ -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)
......
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"
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment