From f21e54cd29d539527f6eab7c3b547961d1aa9228 Mon Sep 17 00:00:00 2001 From: m-ishida Date: Fri, 6 Jan 2012 10:27:46 +0000 Subject: [PATCH] change to use the multiprocessing module --- sandbox/updateikfiles.py | 201 +++++++++++++++++++++++++++++++++------ 1 file changed, 174 insertions(+), 27 deletions(-) diff --git a/sandbox/updateikfiles.py b/sandbox/updateikfiles.py index f88a5492e3..bf481dff3c 100644 --- a/sandbox/updateikfiles.py +++ b/sandbox/updateikfiles.py @@ -1,13 +1,21 @@ """updates the cached ikfast files in the plugins/ikfastsolvers directory """ -from openravepy import * + +from numpy import * +from itertools import * import time,platform,os,sys +import multiprocessing +from optparse import OptionParser import logging +import pickle +from openravepy import * from openravepy import ikfast -def updateik(robotfilename,manipname,iktype,destfilename,freeindices=None): +databases.inversekinematics.log.setLevel(logging.ERROR) + +def updateik(robotfilename,manipname,iktype,destfilename=None,freeindices=None,results=None, do_test=True, testnum='5000', delta='0.01'): print robotfilename, manipname, iktype, destfilename - robotid = os.path.split(destfilename)[1][:-4] + RaveInitialize() env=Environment() try: with env: @@ -17,18 +25,24 @@ def updateik(robotfilename,manipname,iktype,destfilename,freeindices=None): manip = robot.SetActiveManipulator(manipname) ikmodel = databases.inversekinematics.InverseKinematicsModel(robot,iktype=iktype,freeindices=freeindices) ikmodel.manip.SetIKSolver(None) + ikmodel.ikfast.log.setLevel(logging.ERROR) if not ikmodel.load(): ikmodel.autogenerate() - ikmodel.setrobot() - successrate, wrongrate = ikmodel.testik('100' if ikmodel.manip.GetIkSolver().GetNumFreeParameters() <= 1 else '10') # sanity check - assert(wrongrate==0) - print 'success: ',successrate - code = """#define IKFAST_NAMESPACE %s + ikmodel.setrobot([float(delta)]*len(freeindices)) + if do_test: + successrate, wrongrate = ikmodel.testik(testnum) + if results is not None: + results[0].value = successrate + results[1].value = wrongrate + results[2].value = mean(ikmodel.perftiming(100000)) + if destfilename is not None: + robotid = os.path.split(destfilename)[1][:-4] + code = """#define IKFAST_NAMESPACE %s #include "plugindefs.h" """%robotid - code += open(ikmodel.getsourcefilename(True),'r').read() - code += """ + code += open(ikmodel.getsourcefilename(True),'r').read() + code += """ #include "ikbase.h" namespace IKFAST_NAMESPACE { #ifdef RAVE_REGISTER_BOOST @@ -44,26 +58,159 @@ def updateik(robotfilename,manipname,iktype,destfilename,freeindices=None): } } // end namespace """ - open(destfilename,'w').write(code) + print 'writing %s'%destfilename + open(destfilename,'w').write(code) finally: + print "destroying environment" env.Destroy() -if __name__ == "__main__": + +def get_freeindies_combinations(robot_file, manip_name): + print robot_file, manip_name + if manip_name == None: + return [None] RaveInitialize() + env=Environment() + env.Load(robot_file) + robot=env.GetRobots()[0] + manip=robot.SetActiveManipulator(manip_name) + joints = manip.GetArmIndices() + if len(joints) <= 6: + freeindies_combination=[None] + else: + freeindies_combination = list(combinations(joints, len(joints)-6)) + RaveDestroy() + return freeindies_combination + + +if __name__ == "__main__": + parser = OptionParser() + usage = "usage: %prog [options] " + parser = OptionParser(usage) + parser.add_option("-n", "--numthreads", dest="numthreads", default=multiprocessing.cpu_count(), help='the number of using core') + parser.add_option("-l", "--timelimit", dest="time_limit", default='600', help='time to stop test ik.') + parser.add_option("-d", "--destdir", dest="destdir", default=None, + help='destination directory to save ik file results ') + parser.add_option("-r", "--robot", dest="robot", default=None, help='Robot file path') + parser.add_option("-m", "--manip", dest="manip", default=None, help='Manipulator') + parser.add_option("-t", "--type", dest="type", default=IkParameterization.Type.Transform6D, help='Ik type') + parser.add_option("", "--testnum", dest="testnum", default='5000', help='the number of ik test sets') + parser.add_option("-e", "--delta", dest="delta", default='0.01', help='the step of free indies angle') + + (options, args) = parser.parse_args() + + numthreads=options.numthreads + time_limit=int(options.time_limit) #seconds + robot_manip_type_fouts = None + if (not options.robot is None and options.manip is None) or (options.robot is None and not options.manip is None): + print 'set robot and manip name' + sys.exit (0) + elif not options.robot is None and not options.manip is None: + fout = os.path.splitext(os.path.basename(options.robot))[0]+'.cpp' + robot_manip_type_fouts = [[options.robot, options.manip, options.type, fout]] + + # default files + if robot_manip_type_fouts is None: + robot_manip_type_fouts=[['robots/puma.robot.xml', None, IkParameterization.Type.Transform6D, 'ik_puma.cpp'], + ['robots/barrettwam.robot.xml', None, IkParameterization.Type.Transform6D, 'ik_barrettwam.cpp'], + ['robots/pa10schunk.robot.xml','arm',IkParameterization.Type.Transform6D, 'ik_pa10.cpp'], + ['robots/pr2-beta-static.zae','head',IkParameterization.Type.Lookat3D, 'ik_pr2_head.cpp'], + ['robots/pr2-beta-static.zae','head_torso',IkParameterization.Type.Lookat3D,'ik_pr2_head_torso.cpp'], + ['robots/pr2-beta-static.zae','leftarm',IkParameterization.Type.Transform6D,'ik_pr2_leftarm.cpp'], + ['robots/pr2-beta-static.zae','rightarm',IkParameterization.Type.Transform6D,'ik_pr2_rightarm.cpp'], + ['robots/pr2-beta-static.zae','leftarm_torso',IkParameterization.Type.Transform6D,'ik_pr2_leftarm_torso.cpp'], + ['robots/pr2-beta-static.zae','rightarm_torso',IkParameterization.Type.Transform6D,'ik_pr2_rightarm_torso.cpp'], + ['robots/schunk-lwa3.zae',None,IkParameterization.Type.Transform6D,'ik_schunk_lwa3.cpp'], + ['robots/neuronics-katana.zae','arm',IkParameterization.Type.TranslationDirection5D,'ik_katana5d.cpp'], + ['robots/neuronics-katana.zae','armgrasp',IkParameterization.Type.Translation3D,'ik_katana5d_trans.cpp']] + + + # create all jobs/args + args = [] + robotmanip_offsets = [] + offset = 0 + for robotfilename,manipname,iktype,destfilename in robot_manip_type_fouts: + freeindices_combs = get_freeindies_combinations(robotfilename,manipname) + robotmanip_offsets.append([offset,len(freeindices_combs)]) + offset += len(freeindices_combs) + for freeindices in freeindices_combs: + a={'robotfilename':robotfilename, 'manipname':manipname, 'iktype':iktype, 'freeindices':freeindices, 'testnum':options.testnum, 'delta':options.delta} + if destfilename is not None and options.destdir is not None: + a['destfilename'] = os.path.join(options.destdir, destfilename) + args.append(a) + + finalresults=[None]*len(args) + timer=0 + processes = [] try: - destdir = '../plugins/ikfastsolvers' - updateik('robots/puma.robot.xml',None,IkParameterization.Type.Transform6D,os.path.join(destdir,'ik_puma.cpp')) - updateik('robots/barrettwam.robot.xml',None,IkParameterization.Type.Transform6D,os.path.join(destdir,'ik_barrettwam.cpp')) - updateik('robots/pa10schunk.robot.xml','arm',IkParameterization.Type.Transform6D,os.path.join(destdir,'ik_pa10.cpp')) - updateik('robots/pr2-beta-static.zae','head',IkParameterization.Type.Lookat3D,os.path.join(destdir,'ik_pr2_head.cpp')) - updateik('robots/pr2-beta-static.zae','head_torso',IkParameterization.Type.Lookat3D,os.path.join(destdir,'ik_pr2_head_torso.cpp')) - updateik('robots/pr2-beta-static.zae','leftarm',IkParameterization.Type.Transform6D,os.path.join(destdir,'ik_pr2_leftarm.cpp')) - updateik('robots/pr2-beta-static.zae','rightarm',IkParameterization.Type.Transform6D,os.path.join(destdir,'ik_pr2_rightarm.cpp')) - updateik('robots/pr2-beta-static.zae','leftarm_torso',IkParameterization.Type.Transform6D,os.path.join(destdir,'ik_pr2_leftarm_torso.cpp')) - updateik('robots/pr2-beta-static.zae','rightarm_torso',IkParameterization.Type.Transform6D,destdir+'/ik_pr2_rightarm_torso.cpp') - updateik('robots/schunk-lwa3.zae',None,IkParameterization.Type.Transform6D,os.path.join(destdir,'ik_schunk_lwa3.cpp')) - updateik('robots/neuronics-katana.zae','arm',IkParameterization.Type.TranslationDirection5D,os.path.join(destdir,'ik_katana5d.cpp')) - updateik('robots/neuronics-katana.zae','armgrasp',IkParameterization.Type.Translation3D,os.path.join(destdir,'ik_katana5d_trans.cpp'),[3,4]) - print 'finished updating all files' + starttime=time.time() + for i in range (len(args)): + results = [multiprocessing.Value('f'),multiprocessing.Value('f'),multiprocessing.Value('f')] + results[0].value = 0 + results[1].value = 0 + results[2].value = 0 + kwargs = dict(args[i]) + kwargs['results'] = results + p = multiprocessing.Process(target=updateik, kwargs=kwargs) + print 'start process ('+str(i)+'/'+str(args[i])+')' + p.start() + p.endtime = (time.time()-starttime)+time_limit + p.index = i + p.results = results + p.kwargs = kwargs + processes.append(p) + + # wait until thread finishes, or it times out + waituntilend = len(args)==i+1 + while len(processes) >= numthreads or (waituntilend and len(processes)>0): + terminateprocesses = [] + for p in processes: + if not p.is_alive(): + finalresults[p.index] = [p.results[0].value,p.results[1].value,p.results[2].value] + print 'finished %s with %s rate'%(p.kwargs,finalresults[p.index]) + processes.remove(p) + elif p.endtime < (time.time()-starttime): + terminateprocesses.append(p) + + for p in terminateprocesses: + p.terminate() + p.join() + processes.remove(p) + + if len(processes) >= numthreads or (waituntilend and len(processes)>0): + time.sleep(1) + + #if None or 0.0, failed. + print finalresults + finally: - RaveDestroy() + for p in processes: + p.terminate() + p.join() + + saveresults = [[args[i], finalresults[i]] for i in range(len(finalresults)) if finalresults[i] is not None] + pickle.dump(saveresults,open(os.path.join(options.destdir, 'results.pp'),'w')) + print 'results: %s',saveresults + + # select max success rate one in all free indies combinations. + findices=[] + for offset,numjobs in robotmanip_offsets: + sorted_results=[[finalresults[offset+j],offset+j] for j in range(numjobs) if finalresults[offset+j] is not None] + sorted_results.sort(key=lambda k: k[0][2]) + sorted_results.sort(key=lambda k: k[0][0],reverse=True) + findex = None + for [successrate,wrongrate,perftime],offset in sorted_results: + if wrongrate <= 0.0 or wrongrate == None: + findex = offset + break + if findex is None: + raise ValueError('ik has failures') + findices.append(findex) + + for i in findices: + try: + updateik(do_test=False,**args[i]) + except Exception,e: + print e + print 'error occured in writing file %s.'%args[i] +