change to use the multiprocessing module

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] <arg>"
+    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]
+