Пример #1
0
def plotPerfSizeDist(foldername):
    rs = ReadSetupFile()
    dicoCost = {}

    for i in range(len(rs.sizeOfTarget)):
        name = rs.CMAESpath + str(
            rs.sizeOfTarget[i]) + "/" + foldername + "/Cost/"

        costs = getCostData(name)

        for k, v in costs.items():
            for j in range(len(v)):
                distance = round(rs.getDistanceToTarget(v[j][0], v[j][1]), 2)
                if not distance in dicoCost.keys():
                    dicoCost[distance] = {}
                if not rs.sizeOfTarget[i] in dicoCost[distance].keys():
                    dicoCost[distance][rs.sizeOfTarget[i]] = []
                dicoCost[distance][rs.sizeOfTarget[i]].append(v[j][2])

    plotTab = []
    fig = plt.figure(1, figsize=(16, 9))
    plt.ylabel("performance")
    plt.xlabel("Target size (mm)")
    for key in sorted(dicoCost.keys()):
        plotTab.append(
            plt.plot([i for i in sorted(dicoCost[key].keys())], [
                np.mean(dicoCost[key][i]) for i in sorted(dicoCost[key].keys())
            ],
                     label=str("Distance: " + str(key))))
    plt.legend(loc=0)
    plt.savefig("ImageBank/perfdist" + foldername + ".png",
                bbox_inches='tight')
    plt.show(block=True)
Пример #2
0
def plotPerfSizeDist(foldername):
    rs = ReadSetupFile()
    dicoCost = {}
 
    for i in range(len(rs.sizeOfTarget)):
        name =  rs.CMAESpath + str(rs.sizeOfTarget[i]) + "/" + foldername + "/Cost/"

        costs = getCostData(name)

        for k, v in costs.items():
            for j in range(len(v)):
                distance = round(rs.getDistanceToTarget(v[j][0],v[j][1]),2)
                if not distance in dicoCost.keys():
                    dicoCost[distance] = {}
                if not rs.sizeOfTarget[i] in dicoCost[distance].keys():
                    dicoCost[distance][rs.sizeOfTarget[i]] = []
                dicoCost[distance][rs.sizeOfTarget[i]].append(v[j][2])

    plotTab = []
    fig = plt.figure(1, figsize=(16,9))
    plt.ylabel("performance")
    plt.xlabel("Target size (mm)")
    for key in sorted(dicoCost.keys()):
        plotTab.append(plt.plot([i for i in sorted(dicoCost[key].keys())], [np.mean(dicoCost[key][i]) for i in sorted(dicoCost[key].keys())], label = str("Distance: " + str(key))))
    plt.legend(loc = 0)
    plt.savefig("ImageBank/perfdist"+foldername+".png", bbox_inches='tight')
    plt.show(block = True)
Пример #3
0
def plotXYEstimErrorOfSpeed(what, foldername="None", targetSize="All"):
    rs = ReadSetupFile()
    plt.figure(1, figsize=(16, 9))

    if what == "CMAES" and targetSize == "All":
        for i in range(len(rs.sizeOfTarget)):
            ax = plt.subplot2grid((2, 2), (i / 2, i % 2))
            name = rs.CMAESpath + str(
                rs.sizeOfTarget[i]) + "/" + foldername + "/Log/"
            plotEstimErrorOfSpeed(name, ax)

            #makeInitPlot(rs)
            ax.set_xlabel("Velocity (m/s)")
            ax.set_ylabel("Estimation error (m)")
            ax.set_title("Estimation error function of velocity for target " +
                         str(rs.sizeOfTarget[i]))

    else:
        if what == "CMAES":
            name = rs.CMAESpath + targetSize + "/" + foldername + "/Log/"
        elif what == "Brent":
            name = BrentTrajectoriesFolder
        else:
            name = rs.RBFNpath + foldername + "/Log/"

        plotEstimErrorOfSpeed(name, plt)
        #makeInitPlot(rs)

        plt.xlabel("Velocity (m/s)")
        plt.ylabel("Estimation error (m)")
        plt.title("Estimation error function of velocity for " + what)

    plt.savefig("ImageBank/" + what + '_estimError' + foldername + '.png',
                bbox_inches='tight')
    plt.show(block=True)
Пример #4
0
def plotVelocityProfile(what, foldername="None"):
    rs = ReadSetupFile()
    arm = Arm()
    plt.figure(1, figsize=(16, 9))

    if what == "CMAES":
        for i in range(4):
            ax = plt.subplot2grid((2, 2), (i / 2, i % 2))
            name = rs.CMAESpath + str(
                rs.sizeOfTarget[i]) + "/" + foldername + "/Log/"
            makeVelocityData(rs, arm, name, ax)
            ax.set_xlabel("time (s)")
            ax.set_ylabel("Instantaneous velocity (m/s)")
            ax.set_title(
                str("Velocity profiles for target " + str(rs.sizeOfTarget[i])))
    else:
        if what == "Brent":
            name = BrentTrajectoriesFolder
        else:
            name = rs.RBFNpath + foldername + "/Log/"

        makeVelocityData(rs, arm, name, plt)
        plt.xlabel("time (s)")
        plt.ylabel("Instantaneous velocity (m/s)")
        plt.title("Velocity profiles for " + what)

    plt.savefig("ImageBank/" + what + '_velocity_profiles' + foldername +
                '.png',
                bbox_inches='tight')
    plt.show(block=True)
Пример #5
0
def plotArticularPositions(what, foldername="None", targetSize="0.05"):
    rs = ReadSetupFile()

    if what == "CMAES":
        name = rs.CMAESpath + targetSize + "/" + foldername + "/Log/"
    elif what == "Brent":
        name = BrentTrajectoriesFolder
    else:
        name = rs.RBFNpath + foldername + "/Log/"

    state = getStateData(name)

    plt.figure(1, figsize=(16, 9))
    for k, v in state.items():
        if rd.random() < 0.06 or what != "Brent":
            Q1, Q2 = [], []
            for j in range(len(v)):
                Q1.append(v[j][2])
                Q2.append(v[j][3])
            plt.plot(Q1, Q2, c='b')
    plt.xlabel("Q1 (rad)")
    plt.ylabel("Q2 (rad)")
    plt.title("Articular positions for " + what)
    plt.savefig("ImageBank/" + what + '_articular' + foldername + '.png',
                bbox_inches='tight')
    plt.show(block=True)
Пример #6
0
def UnitTestArmModel():
    '''
    Tests the next state 
    '''
    rs = ReadSetupFile()

    arm = Arm()
    arm.setDT(rs.dt)

    state, estimState, command, noisycommand, nextEstimState, nextState = {}, {}, {}, {}, {}, {}
    for el in os.listdir(BrentTrajectoriesFolder):
            state[el], estimState[el], command[el], noisycommand[el], nextEstimState[el], nextState[el] = [], [], [], [], [], []
            data = np.loadtxt(BrentTrajectoriesFolder + el)
            for i in range(data.shape[0]):
                estimState[el].append(np.array([data[i][4], data[i][5], data[i][6], data[i][7]]))
                state[el].append(np.array([data[i][8], data[i][9], data[i][10], data[i][11]]))
                noisycommand[el].append(np.array([data[i][12], data[i][13], data[i][14], data[i][15], data[i][16], data[i][17]]))
                command[el].append(np.array([data[i][18], data[i][19], data[i][20], data[i][21], data[i][22], data[i][23]]))
                nextEstimState[el].append(np.array([data[i][24], data[i][25], data[i][26], data[i][27]]))
                nextState[el].append(np.array([data[i][28], data[i][29], data[i][30], data[i][31]]))

    for el in os.listdir(BrentTrajectoriesFolder):
            for i in range(len(state[el])):
                if rd.random()<0.06:
                    outNextStateNoisy = arm.computeNextState(noisycommand[el][i],state[el][i])
                    outNextState = arm.computeNextState(command[el][i],state[el][i])
                    
                    print("U      :", command[el][i]) 
                    print("UNoisy :", noisycommand[el][i])
                    print("---------------------------------------------------------")
                    print("Real :", nextState[el][i]) 
                    print("ArmN :", outNextStateNoisy)
                    print("Arm :", outNextState)
                    print("---------------------------------------------------------")
Пример #7
0
def plotXYPositions(what, foldername = "None", targetSize = "All", plotEstim=False):
    rs = ReadSetupFile()
    plt.figure(1, figsize=(16,9))

    if what == "CMAES" and targetSize == "All":
        for i in range(len(rs.sizeOfTarget)):
            ax = plt.subplot2grid((2,2), (i/2,i%2))
            name =  rs.CMAESpath + str(rs.sizeOfTarget[i]) + "/" + foldername + "/Log/"
            plotPos(name, ax, plotEstim)

            #makeInitPlot(rs)
            ax.set_xlabel("X (m)")
            ax.set_ylabel("Y (m)")
            ax.set_title("XY Positions for target " + str(rs.sizeOfTarget[i]))

    else:
        if what == "CMAES":
            name = rs.CMAESpath + targetSize + "/" + foldername + "/Log/"
        elif what == "Brent":
            name = BrentTrajectoriesFolder
        else:
            name = rs.NNpath + foldername + "/Log/"

        plotPos(name, plt, plotEstim)
        #makeInitPlot(rs)

        plt.xlabel("X (m)")
        plt.ylabel("Y (m)")
        plt.title("XY Positions for " + what)

    plt.savefig("ImageBank/"+what+'_trajectories'+foldername+'.png', bbox_inches='tight')
    plt.show(block = True)
Пример #8
0
def launchCMAESForSpecificTargetSize(sizeOfTarget, thetaFile, save):
    '''
    Run cmaes for a specific target size

    Input:	-sizeOfTarget, size of the target, float
    '''
    print("Starting the CMAES Optimization for target " + str(sizeOfTarget) +
          " !")
    rs = ReadSetupFile()
    foldername = rs.CMAESpath + str(sizeOfTarget) + "/"
    thetaname = foldername + thetaFile
    if save:
        copyRBFNtoCMAES(rs, thetaFile, sizeOfTarget)

    #Initializes all the class used to generate trajectory
    exp = Experiments(rs, sizeOfTarget, False, foldername, thetaname,
                      rs.popsizeCmaes, rs.period)
    theta = exp.tm.controller.theta
    thetaCMA = theta.flatten()

    #run the optimization (cmaes)
    resCma = cma.fmin(exp.runTrajectoriesCMAES,
                      thetaCMA,
                      rs.sigmaCmaes,
                      options={
                          'maxiter': rs.maxIterCmaes,
                          'popsize': rs.popsizeCmaes,
                          'CMA_diagonal': True,
                          'verb_log': 50,
                          'verb_disp': 1,
                          'termination_callback': term()
                      })
    print("End of optimization for target " + str(sizeOfTarget) + " !")
Пример #9
0
def UnitTestRBFNController():
    '''
    Tests the approximated command obtained from training states
    '''
    rs = ReadSetupFile()
    fa = initRBFNController(rs)
    fa.train_rbfn()
    fa.saveTheta("test")
    fa.loadTheta("test")

    state, command = {}, {}
    for el in os.listdir(BrentTrajectoriesFolder):
        state[el], command[el] = [], []
        data = np.loadtxt(BrentTrajectoriesFolder + el)
        for i in range(data.shape[0]):
            state[el].append(
                (data[i][8], data[i][9], data[i][10], data[i][11]))
            command[el].append((data[i][18], data[i][19], data[i][20],
                                data[i][21], data[i][22], data[i][23]))

    for el in os.listdir(BrentTrajectoriesFolder):
        for i in range(len(state[el])):
            if rd.random() < 0.06:
                outrbfn = fa.computeOutput(np.array(state[el][i]))
                print("Real  :", command[el][i])
                print("Learn :", outrbfn)
Пример #10
0
def generateRichDataFromCMAES(repeat, thetaFile, saveDir='Data'):
    rs = ReadSetupFile()
    for el in rs.sizeOfTarget:
        thetaName = rs.CMAESpath + str(el) + "/" + thetaFile
        saveName = rs.CMAESpath + str(el) + "/" + saveDir + "/"
        GenerateRichDataFromTheta(rs, el, saveName, thetaName, repeat, True)
    print("CMAES:End of generation")
Пример #11
0
def UnitTestNNController():
    '''
    Tests the approximated command obtained from training states
    '''
    rs = ReadSetupFile()
    fa = initNNController(rs)

    stateAll, commandAll = stateAndCommandDataFromTrajs(
        loadStateCommandPairsByStartCoords(pathDataFolder + "TrajRepository/"))
    #stateAll, commandAll = stateAndCommandDataFromTrajs(loadStateCommandPairsByStartCoords(BrentTrajectoriesFolder))
    #print ("len:", len(commandAll[0]))
    state = np.vstack(np.array(stateAll))
    command = np.vstack(np.array(commandAll))

    fa.getTrainingData(state, command)
    fa.train()
    fa.saveTheta("test")
    fa.loadTheta("test")

    for el in os.listdir(BrentTrajectoriesFolder):
        for i in range(len(state)):
            if rd.random() < 0.06:
                outNN = fa.computeOutput(np.array(state[i]))
                print("Real  :", command[i])
                print("Learn :", outNN)
Пример #12
0
def plotHitDispersion(foldername, sizeT):
    rs = ReadSetupFile()
    name = rs.CMAESpath + sizeT + "/" + foldername + "/finalX/"
    data = getLastXData(name)

    tabx, taby = [], []
    for el in data.values():
        for j in range(len(el)):
            tabx.append(el[j])
            taby.append(rs.YTarget)

    fig = plt.figure(1, figsize=(16, 9))
    plt.plot([-rs.sizeOfTarget[0] / 2, rs.sizeOfTarget[0] / 2],
             [rs.YTarget, rs.YTarget],
             c='r')
    plt.scatter([-rs.sizeOfTarget[0] / 2, rs.sizeOfTarget[0] / 2],
                [rs.YTarget, rs.YTarget],
                marker=u'|',
                s=100)
    plt.scatter(tabx, taby, c='b')
    plt.xlabel("X (m)")
    plt.ylabel("Y (m)")
    plt.savefig("ImageBank/hit" + str(sizeT) + foldername + ".png",
                bbox_inches='tight')
    plt.show(block=True)
Пример #13
0
def runNN(name):
    ''' 
    Takes the Brent trajectories as input, shuffles them, and then runs the NN regression algorithm
    '''
    rs = ReadSetupFile()
    #state, command = getStateAndCommandData(BrentTrajectoriesFolder)
    #stateAll, commandAll = dicToArray(state), dicToArray(command)
    #print ("old:", stateAll[0])

    stateAll, commandAll = stateAndCommandDataFromTrajs(
        loadStateCommandPairsByStartCoords(pathDataFolder + "TrajRepository/"))
    #stateAll, commandAll = stateAndCommandDataFromTrajs(loadStateCommandPairsByStartCoords(BrentTrajectoriesFolder))
    #print ("len:", len(commandAll[0]))
    stateAll = np.vstack(np.array(stateAll))
    commandAll = np.vstack(np.array(commandAll))
    #print ("len global:", len(commandAll))
    #print ("new:", commandAll[0])

    #this works because both shuffles generate the same order, due to the seed
    np.random.seed(0)
    np.random.shuffle(stateAll)
    np.random.seed(0)
    np.random.shuffle(commandAll)

    print("nombre d'echantillons: ", len(stateAll))
    fa = NeuralNet(rs.inputDim, rs.outputDim)
    fa.getTrainingData(stateAll, commandAll)
    fa.train()
    fa.saveTheta(rs.NNpath + name + ".theta")
Пример #14
0
def runRBFN(name,fromStruct):
    ''' 
    Takes the Brent trajectories as input, shuffles them, and then runs the RBFN regression algorithm
    '''
    rs = ReadSetupFile()
    #state, command = getStateAndCommandData(BrentTrajectoriesFolder)
    #stateAll, commandAll = dicToArray(state), dicToArray(command)
    #print ("old:", stateAll[0])

    stateAll, commandAll = stateAndCommandDataFromTrajs(loadStateCommandPairsByStartCoords(pathDataFolder + "TrajRepository/"))
    #stateAll, commandAll = stateAndCommandDataFromTrajs(loadStateCommandPairsByStartCoords(BrentTrajectoriesFolder))
    #print ("len:", len(commandAll[0]))
    stateAll = np.vstack(np.array(stateAll))
    commandAll = np.vstack(np.array(commandAll))
    #print ("len global:", len(commandAll))
    #print ("new:", commandAll[0])

    #this works because both shuffles generate the same order, due to the seed
    np.random.seed(0)
    np.random.shuffle(stateAll)
    np.random.seed(0)
    np.random.shuffle(commandAll)

    print("nombre d'echantillons: ", len(stateAll))
    fa = rbfn(rs.numfeats,rs.inputDim,rs.outputDim)
    fa.getTrainingData(stateAll, commandAll)
    if fromStruct == True:
        initFromExistingStruct(fa,rs.RBFNpath+name+".struct")
    else:
        initFromData(fa,rs.RBFNpath+name+".struct")
    fa.train_reg_rbfn(rs.lamb)
    fa.saveTheta(rs.RBFNpath + name+".theta")
Пример #15
0
def trajectoriesAnimation(what, foldername="None", targetSize="0.05"):
    rs = ReadSetupFile()
    if what == "CMAES":
        name = rs.CMAESpath + targetSize + "/" + foldername + "/Log/"
    elif what == "Brent":
        name = BrentTrajectoriesFolder
    else:
        name = rs.RBFNpath + foldername + "/Log/"

    ec = getXYElbowData(name)
    hc = getXYHandData(name)

    posIni = np.loadtxt(pathDataFolder + rs.experimentFilePosIni)

    xEl, yEl, xHa, yHa = [], [], [], []
    for key, val in ec.items():
        for el in val:
            xEl.append(el[0])
            yEl.append(el[1])
        for elhc in hc[key]:
            xHa.append(elhc[0])
            yHa.append(elhc[1])

    fig = plt.figure()
    upperArm, = plt.plot([], [])
    foreArm, = plt.plot([], [])
    plt.xlim(-0.7, 0.7)
    plt.ylim(-0.7, 0.7)
    plt.plot([-0.7, 0.7], [rs.YTarget, rs.YTarget])
    plt.scatter([-rs.sizeOfTarget[3] / 2, rs.sizeOfTarget[3] / 2],
                [rs.YTarget, rs.YTarget],
                c='g',
                marker='o',
                s=50)
    plt.scatter([el[0] for el in posIni], [el[1] for el in posIni], c='b')

    def init():
        upperArm.set_data([0], [0])
        foreArm.set_data([xEl[0]], [yEl[0]])
        return upperArm, foreArm

    def animate(i):
        xe = (0, xEl[i])
        ye = (0, yEl[i])
        xh = (xEl[i], xHa[i])
        yh = (yEl[i], yHa[i])
        upperArm.set_data(xe, ye)
        foreArm.set_data(xh, yh)
        return upperArm, foreArm

    ani = animation.FuncAnimation(fig,
                                  animate,
                                  init_func=init,
                                  frames=len(xEl),
                                  blit=True,
                                  interval=20,
                                  repeat=True)
    plt.show(block=True)
Пример #16
0
def plotInitPos(filename):
    '''
    Plots the initial position of trajectories present in the Brent directory
    '''
    plt.figure()
    rs = ReadSetupFile()
    makeInitPlot(rs, filename)

    plt.show(block=True)
Пример #17
0
def setPosCircu540():
    rs=ReadSetupFile()
    filename2 = pathDataFolder + "PosCircu540"
    data2 = []
    for j in range(20):
        for i in range(27):
            point = [rs.XTarget+ (0.1+j*0.02)*np.cos(-i*np.pi/40-np.pi/5), rs.YTarget+ (0.1+j*0.02)*np.sin(-i*np.pi/40-np.pi/5)]
            data2.append(point)
    np.savetxt(filename2,data2)
Пример #18
0
def setPosSquare():
    rs = ReadSetupFile()
    filename3 = pathDataFolder + "PosSquare"
    data3 = []
    for j in range(20):
        for i in range(20):
            point = [rs.XTarget - 0.3 + i * 0.03, rs.YTarget - 0.4 + j * 0.01]
            data3.append(point)
    np.savetxt(filename3, data3)
Пример #19
0
def setPosCircu28():
    rs=ReadSetupFile()
    filename = pathDataFolder + "PosCircu28"
    data2 = []
    for j in range(4):
        for i in range(7):
            point = [rs.XTarget+ (0.1+j*0.1)*np.cos(-i*np.pi/12-np.pi/4), rs.YTarget+ (0.1+j*0.1)*np.sin(-i*np.pi/12-np.pi/4)]
            data2.append(point)
    np.savetxt(filename,data2)
Пример #20
0
def launchCMAESForAllTargetSizesMulti():
    '''
    Launch in parallel (on differents processor) the cmaes optimization for each target size
    '''
    #initializes setup variables
    rs = ReadSetupFile()
    #initializes a pool of worker, ie multiprocessing
    p = Pool()
    #run cmaes on each targets size on separate processor
    p.map(launchCMAESForSpecificTargetSize, rs.sizeOfTarget, "theta")
Пример #21
0
def plotMuscularActivations(what, foldername="None", targetSize="0.05"):
    '''
    plots the muscular activations from a folder
    
    input:    -foldername: the folder where the data lies
              -what: get from Brent, rbfn or from cmaes controllers

    '''
    rs = ReadSetupFile()
    if what == "CMAES":
        name = rs.CMAESpath + targetSize + "/" + foldername + "/Log/"
    elif what == "Brent":
        name = BrentTrajectoriesFolder
    else:
        name = rs.RBFNpath + foldername + "/Log/"

    U = getNoiselessCommandData(name)

    for key, el1 in U.items():
        t = []
        u1, u2, u3, u4, u5, u6 = [], [], [], [], [], []
        if rd.random() < 0.01 or what != "Brent":
            for i in range(len(el1)):
                t.append(i)
                u1.append(el1[i][0])
                u2.append(el1[i][1])
                u3.append(el1[i][2])
                u4.append(el1[i][3])
                u5.append(el1[i][4])
                u6.append(el1[i][5])

            plt.figure()
            plt.plot(t, u1, label="U1")
            plt.plot(t, u2, label="U2")
            plt.plot(t, u3, label="U3")
            plt.plot(t, u4, label="U4")
            plt.plot(t, u5, label="U5")
            plt.plot(t, u6, label="U6")
            plt.legend(loc=0)
            plt.xlabel("time")
            plt.ylabel("U")
            plt.title("Muscular Activations for " + what)
            plt.savefig("ImageBank/" + what + "_muscu" + key + foldername +
                        ".png",
                        bbox_inches='tight')

            print key
            val = raw_input('1 to see data, anything otherwise: ')
            val = int(val)
            if val == 1:
                print el1
            #plt.clf()

    plt.show(block=True)
Пример #22
0
def plotScattergram(what, foldername):
    rs = ReadSetupFile()
    data = {}

    if what == "CMAES":
        for i in range(len(rs.sizeOfTarget)):
            name = rs.CMAESpath + str(
                rs.sizeOfTarget[i]) + "/" + foldername + "/finalX/"
            tmp = getLastXData(name)
            tabx = []
            for el in tmp.values():
                for j in range(len(el)):
                    tabx.append(el[j])

                    data[rs.sizeOfTarget[i]] = tabx

        plt.figure(1, figsize=(16, 9))

        for i in range(len(rs.sizeOfTarget)):
            ax = plt.subplot2grid((2, 2), (i / 2, i % 2))
            ax.hist(data[rs.sizeOfTarget[i]], 20)
            ax.plot([-rs.sizeOfTarget[i] / 2, -rs.sizeOfTarget[i] / 2],
                    [0, 500],
                    c='r',
                    linewidth=3)
            ax.plot([rs.sizeOfTarget[i] / 2, rs.sizeOfTarget[i] / 2], [0, 500],
                    c='r',
                    linewidth=3)
            ax.set_title(
                str("Hit Dispersion for Target " + str(rs.sizeOfTarget[i])))

    elif what == "RBFN":
        name = rs.RBFNpath + foldername + "/finalX/"
        tmp = getLastXData(name)
        tabx = []
        for el in tmp.values():
            for j in range(len(el)):
                tabx.append(el[j])
        plt.hist(tabx, 20)
        for i in range(len(rs.sizeOfTarget)):
            plt.plot([-rs.sizeOfTarget[i] / 2, -rs.sizeOfTarget[i]] / 2,
                     [0, 20],
                     c='r',
                     linewidth=3)
            plt.plot([rs.sizeOfTarget[i] / 2, rs.sizeOfTarget[i]] / 2, [0, 20],
                     c='r',
                     linewidth=3)
        plt.xlabel("X (m)")
        plt.ylabel("Y (m)")
        plt.title("Hit Dispersion for RBFN")

    plt.savefig("ImageBank/" + what + "_hitdisp" + foldername + ".png",
                bbox_inches='tight')
    plt.show(block=True)
Пример #23
0
def plotTrajsInRepo():
    rs = ReadSetupFile()
    plt.figure(1, figsize=(16, 9))
    plotPos(pathDataFolder + "TrajRepository/", plt, False)
    plt.xlabel("X (m)")
    plt.ylabel("Y (m)")
    plt.title("XY Positions")

    plt.savefig("ImageBank/TrajRepo.png", bbox_inches='tight')
    #plt.savefig("ImageBank/"+what+'_trajectories.png')
    plt.show(block=True)
Пример #24
0
def plotFittsLaw(foldername, rbfn=False):
    rs = ReadSetupFile()

    timeDistWidth = []
    for i in range(len(rs.sizeOfTarget)):
        name = rs.CMAESpath + str(
            rs.sizeOfTarget[i]) + "/" + foldername + "/TrajTime/"

        trajTimes = getTrajTimeData(name)

        for k, v in trajTimes.items():
            for j in range(len(v)):
                distance = rs.getDistanceToTarget(v[j][0], v[j][1])
                trajtime = v[j][2]
                size = rs.sizeOfTarget[i]
                timeDistWidth.append((distance, size, trajtime))

    MT, DI = [], []
    for el in timeDistWidth:
        MT.append(el[2])
        DI.append(np.log2(el[0] / el[1]))
    slope, intercept, r_value, p_value, std_err = stats.linregress(DI, MT)
    yLR = slope * np.asarray(DI) + intercept
    plt.figure()

    for el in timeDistWidth:
        if el[0] <= 0.15:
            plt.scatter(np.log2(el[0] / el[1]), el[2], c='blue')
        elif el[0] <= 0.28:
            plt.scatter(np.log2(el[0] / el[1]), el[2], c='green')
        else:
            plt.scatter(np.log2(el[0] / el[1]), el[2], c='red')

    plt.plot(DI, yLR)
    plt.title("a = " + str(slope) + " b = " + str(intercept) + " r^2 = " +
              str(r_value**2))
    plt.xlabel("log(D/W)/log(2)")
    plt.ylabel("Movement time (s)")
    plt.savefig("ImageBank/fitts" + foldername + ".png", bbox_inches='tight')
    plt.show(block=True)
Пример #25
0
def plotFittsLaw(foldername, rbfn = False):
    rs = ReadSetupFile()

    timeDistWidth = []
    for i in range(len(rs.sizeOfTarget)):
        name =  rs.CMAESpath + str(rs.sizeOfTarget[i]) + "/" + foldername + "/TrajTime/"

        trajTimes = getTrajTimeData(name)

        for k, v in trajTimes.items():
            for j in range(len(v)):
                distance = rs.getDistanceToTarget(v[j][0],v[j][1])
                trajtime = v[j][2]
                size = rs.sizeOfTarget[i]
                timeDistWidth.append((distance, size, trajtime))
            
    MT, DI = [], []
    for el in timeDistWidth:
        MT.append(el[2])
        DI.append(np.log2(el[0]/el[1]))
    slope, intercept, r_value, p_value, std_err = stats.linregress(DI,MT)
    yLR = slope * np.asarray(DI) + intercept
    plt.figure()

    for el in timeDistWidth:
            if el[0]<=0.15:
                plt.scatter(np.log2(el[0]/el[1]), el[2], c ='blue')
            elif el[0]<=0.28:
                plt.scatter(np.log2(el[0]/el[1]), el[2], c ='green')
            else:
                plt.scatter(np.log2(el[0]/el[1]), el[2], c ='red')
        
    plt.plot(DI, yLR)
    plt.title("a = " + str(slope) + " b = " + str(intercept) + " r^2 = " + str(r_value**2))
    plt.xlabel("log(D/W)/log(2)")
    plt.ylabel("Movement time (s)")
    plt.savefig("ImageBank/fitts"+foldername+".png", bbox_inches='tight')
    plt.show(block = True)
Пример #26
0
def setPosCircu15():
    rs=ReadSetupFile()
    filename = pathDataFolder + "PosCircu15"
    data = []
    for i in range(3):
        point = [rs.XTarget+ 0.0975*np.cos((-i-1)*np.pi/4), rs.YTarget+ 0.0975*np.sin((-i-1)*np.pi/4)]
        data.append(point)
    for i in range(5):
        point = [rs.XTarget+ 0.243*np.cos(-i*np.pi/8-np.pi/4), rs.YTarget+ 0.243*np.sin(-i*np.pi/8-np.pi/4)]
        data.append(point)
    for i in range(7):
        point = [rs.XTarget+ 0.39*np.cos(-i*np.pi/12-np.pi/4), rs.YTarget+ 0.39*np.sin(-i*np.pi/12-np.pi/4)]
        data.append(point)
    np.savetxt(filename,data)
Пример #27
0
def plotManipulability2():
    rs = ReadSetupFile()
    fig = plt.figure(1, figsize=(16, 9))
    arm = Arm()
    q1 = np.linspace(-0.6, 2.6, 100, True)
    q2 = np.linspace(-0.2, 3, 100, True)
    target = [rs.XTarget, rs.YTarget]

    pos = []
    for i in range(len(q1)):
        for j in range(len(q2)):
            config = np.array([q1[i], q2[j]])
            coordHa = arm.mgdEndEffector(config)
            pos.append(coordHa)

    x, y, cost = [], [], []
    for el in pos:
        x.append(el[0])
        y.append(el[1])
        config = arm.mgi(el[0], el[1])
        manip = arm.manipulability(config, target)
        cost.append(manip)

    xi = np.linspace(-0.7, 0.8, 100)
    yi = np.linspace(-0.5, 0.8, 100)
    zi = griddata(x, y, cost, xi, yi)

    #t1 = plt.scatter(x, y, c=cost, marker=u'o', s=5, cmap=cm.get_cmap('RdYlBu'))
    #CS = plt.contourf(xi, xi, zi, 15, cmap=cm.get_cmap('RdYlBu'))

    t1 = plt.scatter(x, y, c=cost, s=5, cmap=cm.get_cmap('RdYlBu'))
    CS = plt.contourf(xi, yi, zi, 15, cmap=cm.get_cmap('RdYlBu'))
    fig.colorbar(t1, shrink=0.5, aspect=5)
    plt.scatter(rs.XTarget, rs.YTarget, c="g", marker=u'*', s=200)
    #plt.plot([-0.3,0.3], [rs.YTarget, rs.YTarget], c = 'g')
    plt.xlabel("X (m)")
    plt.ylabel("Y (m)")
    plt.title(str("Manipulability map"))
    plt.savefig("ImageBank/manipulability2.png", bbox_inches='tight')
    plt.show(block=True)
Пример #28
0
def plotExperimentSetup():
    rs = ReadSetupFile()
    fig = plt.figure(1, figsize=(16, 9))
    arm = Arm()
    q1 = np.linspace(-0.6, 2.6, 100, True)
    q2 = np.linspace(-0.2, 3, 100, True)
    posIni = np.loadtxt(pathDataFolder + rs.experimentFilePosIni)
    xi, yi = [], []
    xb, yb = [0], [0]
    t = 0
    for el in posIni:
        if el[1] == np.min(posIni, axis=0)[1] and t == 0:
            t += 1
            a, b = arm.mgi(el[0], el[1])
            a1, b1 = arm.mgdFull(np.array([[a], [b]]))
            xb.append(a1[0])
            xb.append(b1[0])
            yb.append(a1[1])
            yb.append(b1[1])
        xi.append(el[0])
        yi.append(el[1])
    pos = []
    for i in range(len(q1)):
        for j in range(len(q2)):
            coordHa = arm.mgdEndEffector(np.array([[q1[i]], [q2[j]]]))
            pos.append(coordHa)
    x, y = [], []
    for el in pos:
        x.append(el[0])
        y.append(el[1])

    plt.scatter(x, y)
    plt.scatter(xi, yi, c='r')
    plt.scatter(0, 0.6175, c="r", marker=u'*', s=200)
    plt.plot(xb, yb, c='r')
    plt.plot([-0.3, 0.3], [0.6175, 0.6175], c='g')
    plt.savefig("ImageBank/setup.png", bbox_inches='tight')
    plt.show(block=True)
Пример #29
0
def plotCMAESCostProgress():
    rs = ReadSetupFile()
    fig = plt.figure(1, figsize=(16, 9))

    for i in range(len(rs.sizeOfTarget)):
        ax = plt.subplot2grid((2, 2), (i / 2, i % 2))
        name = rs.CMAESpath + str(rs.sizeOfTarget[i]) + "/Cost/cmaesCost.log"
        data = np.loadtxt(name)

        x, w, m, b = [], [], [], []
        for j in range(len(data)):
            x.append(j)
            w.append(data[j][0])
            m.append(data[j][1])
            b.append(data[j][2])
        ax.plot(x, w, c='b')
        ax.plot(x, m, c='g')
        ax.plot(x, b, c='r')

        ax.set_title(str("Cost Target " + str(rs.sizeOfTarget[i])))

    plt.savefig("ImageBank/costProgress.png")
    plt.show(block=True)
Пример #30
0
def saveRandomNN():
    rs = ReadSetupFile()
    fa = NeuralNet(rs.inputDim, rs.outputDim)
    fa.saveTheta(rs.NNpath + "Random.theta")
Пример #31
0
def generateRichDataFromRBFN(repeat, thetaFile, saveDir):
    rs = ReadSetupFile()
    thetaName = rs.RBFNpath + thetaFile
    saveName = rs.RBFNpath + saveDir + "/"
    GenerateRichDataFromTheta(rs, 0.05, saveName, thetaName, repeat, False)
    print("RBFN:End of generation")
Пример #32
0
def launchCMAESForAllTargetSizes(thetaname, save):
    rs = ReadSetupFile()
    for el in rs.sizeOfTarget:
        launchCMAESForSpecificTargetSize(el, thetaname, save)