updateikfiles.py

"""updates the cached ikfast files in the plugins/ikfastsolvers directory
"""

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

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
    RaveInitialize()
    env=Environment()
    try:
        with env:
            robot = env.ReadRobotXMLFile(robotfilename,{'skipgeometry':'1'})
            env.AddRobot(robot)
            if manipname is not 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([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 += """
#include "ikbase.h"
namespace IKFAST_NAMESPACE {
#ifdef RAVE_REGISTER_BOOST
#include BOOST_TYPEOF_INCREMENT_REGISTRATION_GROUP()
BOOST_TYPEOF_REGISTER_TYPE(IKSolution)
#endif
IkSolverBasePtr CreateIkSolver(EnvironmentBasePtr penv, const std::vector<dReal>& vfreeinc) {
    std::vector<int> vfree(getNumFreeParameters());
    for(size_t i = 0; i < vfree.size(); ++i) {
        vfree[i] = getFreeParameters()[i];
    }
    return IkSolverBasePtr(new IkFastSolver<IKReal,IKSolution>(ik,vfree,vfreeinc,getNumJoints(),static_cast<IkParameterizationType>(getIKType()), boost::shared_ptr<void>(), getKinematicsHash(), penv));
}
} // end namespace
"""
                print 'writing %s'%destfilename
                open(destfilename,'w').write(code)
    finally:
        print "destroying environment"
        env.Destroy()


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:
        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:
        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]