コード例 #1
0
ファイル: TrajMaker.py プロジェクト: osigaud/ArmModelPython
    def __init__(self, rs, sizeOfTarget, saveTraj, thetaFile):
        '''
    	Initializes the parameters used to run the functions below
    
    	Inputs:		
    			-arm, armModel, class object
                        -rs, readSetup, class object
    			-sizeOfTarget, size of the target, float
    			-Ukf, unscented kalman filter, class object
    			-saveTraj, Boolean: true = Data are saved, false = data are not saved
    	'''
        self.arm = Arm()
        self.arm.setDT(rs.dt)

        self.controller = initRBFNController(rs,thetaFile)
        #load the controller, i.e. the vector of parameters theta
        theta = self.controller.loadTheta(thetaFile+".theta")
        #put theta to a one dimension numpy array, ie row vector form
        #theta = matrixToVector(theta)
 
        self.rs = rs
        self.sizeOfTarget = sizeOfTarget
        #6 is the dimension of the state for the filter, 4 is the dimension of the observation for the filter, 25 is the delay used
        self.stateEstimator = StateEstimator(rs.inputDim,rs.outputDim, rs.delayUKF, self.arm)
        self.saveTraj = saveTraj
コード例 #2
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("---------------------------------------------------------")
コード例 #3
0
ファイル: FileReading.py プロジェクト: osigaud/ArmModelPython
def loadStateCommandPairsByStartCoords(foldername):
    '''
    Get all the data from a set of trajectories, sorted by the starting xy coordinates
    
    Output : dictionary of data whose keys are y then x coordinates
    '''
    arm = Arm()
    dataOut = {}
    #    j = 0
    for el in os.listdir(foldername):
        #        j = j+1
        #        if j>4500 or rd.random()<0.5:
        data = np.loadtxt(foldername + el)
        coordHand = arm.mgdEndEffector(np.array([[data[0, 10]], [data[0,
                                                                      11]]]))
        x, y = str(coordHand[0][0]), str(coordHand[1][0])
        if not y in dataOut.keys():
            dataOut[y] = {}
        if not x in dataOut[y].keys():
            dataOut[y][x] = []
        traj = []
        for i in range(data.shape[0]):
            currentState = (data[i][8], data[i][9], data[i][10], data[i][11],
                            data[i][12], data[i][13])
            noisyActiv = ([
                data[i][14], data[i][15], data[i][16], data[i][17],
                data[i][18], data[i][19]
            ])
            pair = (currentState, noisyActiv)
            traj.append(pair)
        dataOut[y][x].append(traj)
    return dataOut
コード例 #4
0
ファイル: FileReading.py プロジェクト: osigaud/ArmModelPython
def loadStateCommandPairsByStartCoords(foldername):
    """
    Get all the data from a set of trajectories, sorted by the starting xy coordinates
    
    Output : dictionary of data whose keys are y then x coordinates
    """
    arm = Arm()
    dataOut = {}
    #    j = 0
    for el in os.listdir(foldername):
        #        j = j+1
        #        if j>4500 or rd.random()<0.5:
        data = np.loadtxt(foldername + el)
        coordHand = arm.mgdEndEffector(np.array([[data[0, 10]], [data[0, 11]]]))
        x, y = str(coordHand[0][0]), str(coordHand[1][0])
        if not y in dataOut.keys():
            dataOut[y] = {}
        if not x in dataOut[y].keys():
            dataOut[y][x] = []
        traj = []
        for i in range(data.shape[0]):
            currentState = (data[i][8], data[i][9], data[i][10], data[i][11], data[i][12], data[i][13])
            noisyActiv = [data[i][14], data[i][15], data[i][16], data[i][17], data[i][18], data[i][19]]
            pair = (currentState, noisyActiv)
            traj.append(pair)
        dataOut[y][x].append(traj)
    return dataOut
コード例 #5
0
ファイル: Arm38.py プロジェクト: osigaud/ArmModelPython
 def __init__(self):
     Arm.__init__(self, ArmParameters(3,8), MusclesParameters(3,8), np.array([0.,0.]))
     self.k1=self.armP.m[2]*(self.armP.l[0]**2)
     self.k2=self.armP.m[1]*self.armP.l[0]*self.armP.s[1]
     self.k3=self.armP.m[1]*(self.armP.l[0]**2)
     self.k4=self.armP.I[0]+self.armP.m[0]*(self.armP.s[0]**2)
     self.k5=self.armP.m[2]*self.armP.l[0]*self.armP.l[1]
     self.k6=self.armP.m[1]*self.armP.s[1]*self.armP.l[0]
     self.k7=self.armP.m[2]*(self.armP.l[1]**2)
     self.k8=self.armP.I[1]+self.armP.m[1]*(self.armP.s[1]**2)
     self.k9=self.armP.m[2]*self.armP.l[0]*self.armP.s[2]
     self.k10=self.armP.m[2]*self.armP.l[1]*self.armP.s[2]
     self.k11=self.armP.I[2]+self.armP.m[2]*(self.armP.s[2]**2)
コード例 #6
0
ファイル: FileReading.py プロジェクト: osigaud/ArmModelPython
def getInitPos(foldername):
    """
    Get all the initial positions from a set of trajectories, in xy coordinates
    
    Output : dictionary of initial position of all trajectories
    """
    arm = Arm()
    xy = {}
    for el in os.listdir(foldername):
        data = np.loadtxt(foldername + el)
        coordHand = arm.mgdEndEffector(np.array([[data[0, 10]], [data[0, 11]]]))
        # if coordHand[1]<0.58:
        xy[el] = (coordHand[0], coordHand[1])
    return xy
コード例 #7
0
ファイル: Arm38.py プロジェクト: osigaud/ArmModelPython
 def __init__(self):
     Arm.__init__(self, ArmParameters(3, 8), MusclesParameters(3, 8),
                  np.array([0., 0.]))
     self.k1 = self.armP.m[2] * (self.armP.l[0]**2)
     self.k2 = self.armP.m[1] * self.armP.l[0] * self.armP.s[1]
     self.k3 = self.armP.m[1] * (self.armP.l[0]**2)
     self.k4 = self.armP.I[0] + self.armP.m[0] * (self.armP.s[0]**2)
     self.k5 = self.armP.m[2] * self.armP.l[0] * self.armP.l[1]
     self.k6 = self.armP.m[1] * self.armP.s[1] * self.armP.l[0]
     self.k7 = self.armP.m[2] * (self.armP.l[1]**2)
     self.k8 = self.armP.I[1] + self.armP.m[1] * (self.armP.s[1]**2)
     self.k9 = self.armP.m[2] * self.armP.l[0] * self.armP.s[2]
     self.k10 = self.armP.m[2] * self.armP.l[1] * self.armP.s[2]
     self.k11 = self.armP.I[2] + self.armP.m[2] * (self.armP.s[2]**2)
コード例 #8
0
ファイル: FileReading.py プロジェクト: osigaud/ArmModelPython
def getXYElbowData(foldername):
    """
    Put all the states of trajectories generated by the Brent controller into a dictionary
    
    Outputs:    -state: dictionary keys = filenames, values = array of data
    """
    arm = Arm()
    xy = {}
    for el in os.listdir(foldername):
        xy[el] = []
        data = np.loadtxt(foldername + el)
        for i in range(data.shape[0]):
            coordElbow, coordHand = arm.mgdFull(np.array([[data[i][10]], [data[i][11]]]))
            xy[el].append((coordElbow[0], coordElbow[1]))
    return xy
コード例 #9
0
ファイル: FileReading.py プロジェクト: osigaud/ArmModelPython
def getInitPos(foldername):
    '''
    Get all the initial positions from a set of trajectories, in xy coordinates
    
    Output : dictionary of initial position of all trajectories
    '''
    arm = Arm()
    xy = {}
    for el in os.listdir(foldername):
        data = np.loadtxt(foldername + el)
        coordHand = arm.mgdEndEffector(np.array([[data[0, 10]], [data[0,
                                                                      11]]]))
        #if coordHand[1]<0.58:
        xy[el] = (coordHand[0], coordHand[1])
    return xy
コード例 #10
0
ファイル: FileReading.py プロジェクト: osigaud/ArmModelPython
def getEstimatedXYHandData(foldername):
    """
    Put all the states of trajectories generated by the Brent controller into a dictionary
    
    Outputs:    -state: dictionary keys = filenames, values = array of data
    """
    arm = Arm()
    xyEstim = {}
    for el in os.listdir(foldername):
        xyEstim[el] = []
        data = np.loadtxt(foldername + el)
        for i in range(data.shape[0]):
            coordHand = arm.mgdEndEffector(np.array([[data[i][6]], [data[i][7]]]))
            xyEstim[el].append((coordHand[0], coordHand[1]))
    return xyEstim
コード例 #11
0
ファイル: FileReading.py プロジェクト: osigaud/ArmModelPython
def getXYEstimError(foldername):
    """
    Returns the error estimations in the trajectories from the given foldername
    
    Outputs:    -errors: dictionary keys = filenames, values = array of data
    """
    arm = Arm()
    errors = {}
    for el in os.listdir(foldername):
        errors[el] = []
        data = np.loadtxt(foldername + el)
        for i in range(data.shape[0]):
            statePos = (data[i][10], data[i][11])
            estimStatePos = (data[i][6], data[i][7])
            errors[el].append(arm.estimErrorReduced(statePos, estimStatePos))
    return errors
コード例 #12
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)
コード例 #13
0
ファイル: FileReading.py プロジェクト: osigaud/ArmModelPython
def getEstimatedXYHandData(foldername):
    '''
    Put all the states of trajectories generated by the Brent controller into a dictionary
    
    Outputs:    -state: dictionary keys = filenames, values = array of data
    '''
    arm = Arm()
    xyEstim = {}
    for el in os.listdir(foldername):
        xyEstim[el] = []
        data = np.loadtxt(foldername + el)
        for i in range(data.shape[0]):
            coordHand = arm.mgdEndEffector(
                np.array([[data[i][6]], [data[i][7]]]))
            xyEstim[el].append((coordHand[0], coordHand[1]))
    return xyEstim
コード例 #14
0
ファイル: FileReading.py プロジェクト: osigaud/ArmModelPython
def getXYEstimError(foldername):
    '''
    Returns the error estimations in the trajectories from the given foldername
    
    Outputs:    -errors: dictionary keys = filenames, values = array of data
    '''
    arm = Arm()
    errors = {}
    for el in os.listdir(foldername):
        errors[el] = []
        data = np.loadtxt(foldername + el)
        for i in range(data.shape[0]):
            statePos = (data[i][10], data[i][11])
            estimStatePos = (data[i][6], data[i][7])
            errors[el].append(arm.estimErrorReduced(statePos, estimStatePos))
    return errors
コード例 #15
0
ファイル: FileReading.py プロジェクト: osigaud/ArmModelPython
def getXYElbowData(foldername):
    '''
    Put all the states of trajectories generated by the Brent controller into a dictionary
    
    Outputs:    -state: dictionary keys = filenames, values = array of data
    '''
    arm = Arm()
    xy = {}
    for el in os.listdir(foldername):
        xy[el] = []
        data = np.loadtxt(foldername + el)
        for i in range(data.shape[0]):
            coordElbow, coordHand = arm.mgdFull(
                np.array([[data[i][10]], [data[i][11]]]))
            xy[el].append((coordElbow[0], coordElbow[1]))
    return xy
コード例 #16
0
ファイル: FileReading.py プロジェクト: osigaud/ArmModelPython
def getXYEstimErrorOfSpeed(foldername):
    """
    Returns the error estimations in the trajectories as a function of speed from the given foldername
    
    Outputs:    -state: dictionary keys = filenames, values = array of data
    """
    arm = Arm()
    errors = {}
    for el in os.listdir(foldername):
        errors[el] = []
        data = np.loadtxt(foldername + el)
        for i in range(data.shape[0]):
            speed = arm.cartesianSpeed((data[i][8], data[i][9], data[i][10], data[i][11]))
            statePos = (data[i][10], data[i][11])
            estimStatePos = (data[i][6], data[i][7])
            error = arm.estimErrorReduced(statePos, estimStatePos)
            errors[el].append((speed, error))
    return errors
コード例 #17
0
ファイル: FileReading.py プロジェクト: osigaud/ArmModelPython
def getXYEstimError(foldername):
    '''
    Returns the error estimations in the trajectories from the given foldername
    
    Outputs:    -errors: dictionary keys = filenames, values = array of data
    '''
    arm = Arm()
    errors = {}
    for el in os.listdir(foldername):
        errors[el] = []
        data = np.loadtxt(foldername + el)
        for i in range(data.shape[0]):
            currentState = (data[i][12], data[i][13], data[i][14], data[i][15], data[i][16], data[i][17])
            estimState = (data[i][6], data[i][7], data[i][8], data[i][9], data[i][10], data[i][11])
            statePos = (currentState[3], currentState[4], currentState[5])
            estimStatePos = (estimState[3], estimState[4], estimState[5])
            errors[el].append(arm.estimErrorReduced(statePos,estimStatePos))
    return errors
コード例 #18
0
ファイル: FileReading.py プロジェクト: osigaud/ArmModelPython
def getXYEstimErrorOfSpeed(foldername):
    '''
    Returns the error estimations in the trajectories as a function of speed from the given foldername
    
    Outputs:    -state: dictionary keys = filenames, values = array of data
    '''
    arm = Arm()
    errors = {}
    for el in os.listdir(foldername):
        errors[el] = []
        data = np.loadtxt(foldername + el)
        for i in range(data.shape[0]):
            speed = arm.cartesianSpeed(
                (data[i][8], data[i][9], data[i][10], data[i][11]))
            statePos = (data[i][10], data[i][11])
            estimStatePos = (data[i][6], data[i][7])
            error = arm.estimErrorReduced(statePos, estimStatePos)
            errors[el].append((speed, error))
    return errors
コード例 #19
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)
コード例 #20
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("---------------------------------------------------------")
コード例 #21
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)
コード例 #22
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([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([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(rs.XTarget, rs.YTarget, c = 'g', marker=u'*', s = 200)
    plt.plot(xb, yb, c = 'r')
    plt.plot([-0.3,0.3], [rs.YTarget, rs.YTarget], c = 'g')
    plt.savefig("ImageBank/setup.png", bbox_inches='tight')
    plt.show(block = True)
コード例 #23
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)
コード例 #24
0
ファイル: TrajMaker.py プロジェクト: osigaud/ArmModelPython
class TrajMaker:
    
    def __init__(self, rs, sizeOfTarget, saveTraj, thetaFile):
        '''
    	Initializes the parameters used to run the functions below
    
    	Inputs:		
    			-arm, armModel, class object
                        -rs, readSetup, class object
    			-sizeOfTarget, size of the target, float
    			-Ukf, unscented kalman filter, class object
    			-saveTraj, Boolean: true = Data are saved, false = data are not saved
    	'''
        self.arm = Arm()
        self.arm.setDT(rs.dt)

        self.controller = initRBFNController(rs,thetaFile)
        #load the controller, i.e. the vector of parameters theta
        theta = self.controller.loadTheta(thetaFile+".theta")
        #put theta to a one dimension numpy array, ie row vector form
        #theta = matrixToVector(theta)
 
        self.rs = rs
        self.sizeOfTarget = sizeOfTarget
        #6 is the dimension of the state for the filter, 4 is the dimension of the observation for the filter, 25 is the delay used
        self.stateEstimator = StateEstimator(rs.inputDim,rs.outputDim, rs.delayUKF, self.arm)
        self.saveTraj = saveTraj
        #Initializes variables used to save trajectory
 
    def setTheta(self, theta):
        self.controller.setTheta(theta)

    def computeManipulabilityCost(self):
        '''
        Computes the manipulability cost on one step of the trajectory
		
        Input:	-cost: cost at time t, float
				
        Output:		-cost: cost at time t+1, float
        '''
        dotq, q = getDotQAndQFromStateVector(self.arm.getState())
        manip = self.arm.directionalManipulability(q,self.cartTarget)
        return 1-manip

    def computeStateTransitionCost(self, U):
        '''
		Computes the cost on one step of the trajectory
		
		Input:	-cost: cost at time t, float
				-U: muscular activation vector, numpy array (6,1)
				-t: time, float
				
		Output:		-cost: cost at time t+1, float
		'''
        #compute the square of the norm of the muscular activation vector
        norme = np.linalg.norm(U)
        mvtCost = norme*norme
        #compute the cost following the law of the model
        #return np.exp(-t/self.rs.gammaCF)*(-self.rs.upsCF*mvtCost)
        return -self.rs.upsCF*mvtCost
    
    def computePerpendCost(self):  
        dotq, q = getDotQAndQFromStateVector(self.arm.getState())
        J = self.arm.jacobian(q)
        xi = np.dot(J,dotq)
        xi = xi/np.linalg.norm(xi)
        return 500-1000*xi[0]*xi[0]

    def computeFinalReward(self, t, coordHand):
        cost = self.computePerpendCost()
        '''
		Computes the cost on final step if the target is reached
		
		Input:		-cost: cost at the end of the trajectory, float
					-t: time, float
					
		Output:		-cost: final cost if the target is reached
		'''
        #check if the target is reached and give the reward if yes
        if coordHand[1] >= self.rs.YTarget:
            #print "main X:", coordHand[0]
            if coordHand[0] >= -self.sizeOfTarget/2 and coordHand[0] <= self.sizeOfTarget/2:
                cost += np.exp(-t/self.rs.gammaCF)*self.rs.rhoCF
            else:
                cost += -500-500000*(coordHand[0]*coordHand[0])
        else:
            cost += -4000
        return cost

        
    def runTrajectory(self, x, y, foldername):
        '''
    	Generates trajectory from the initial position (x, y)
    
    	Inputs:		-x: abscissa of the initial position, float
    			-y: ordinate of the initial position, float
    
    	Output:		-cost: the cost of the trajectory, float
    	'''
        #computes the articular position q1, q2 from the initial coordinates (x, y)
        q1, q2 = self.arm.mgi(x, y)
        #creates the state vector [dotq1, dotq2, q1, q2]
        q = createVector(q1,q2)
        state = np.array([0., 0., q1, q2])
        #print("start state --------------: ",state)

        #computes the coordinates of the hand and the elbow from the position vector
        coordElbow, coordHand = self.arm.mgdFull(q)
        #assert(coordHand[0]==x and coordHand[1]==y), "Erreur de MGD" does not work because of rounding effects

        #initializes parameters for the trajectory
        i, t, cost = 0, 0, 0
        self.stateEstimator.initStore(state)
        self.arm.setState(state)
        estimState = state
        dataStore = []
        qtarget1, qtarget2 = self.arm.mgi(self.rs.XTarget, self.rs.YTarget)
        vectarget = [0.0, 0.0, qtarget1, qtarget2]

        #loop to generate next position until the target is reached 
        while coordHand[1] < self.rs.YTarget and i < self.rs.numMaxIter:
            stepStore = []
            #computation of the next muscular activation vector using the controller theta
            #print ("state :",self.arm.getState())

            U = self.controller.computeOutput(estimState)

            if det:
                Unoisy = muscleFilter(U)
            else:
                Unoisy = getNoisyCommand(U,self.arm.musclesP.knoiseU)
                Unoisy = muscleFilter(Unoisy)
            #computation of the arm state
            realNextState = self.arm.computeNextState(Unoisy, self.arm.getState())
 
            #computation of the approximated state
            tmpState = self.arm.getState()

            if det:
                estimNextState = realNextState
            else:
                U = muscleFilter(U)
                estimNextState = self.stateEstimator.getEstimState(tmpState,U)
            
            #print estimNextState

            self.arm.setState(realNextState)

            #computation of the cost
            cost += self.computeStateTransitionCost(Unoisy)

            '''
            print "U =", U
            print "Unoisy =", Unoisy
            print "estimstate =", estimState
            #print "theta =", self.controller.theta
            if math.isnan(cost):
                print "NAN : U =", U
                print "NAN : Unoisy =", Unoisy
                print "NAN : estimstate =", estimState
                #print "NAN : theta =", self.controller.theta
                sys.exit()
            '''

            #get dotq and q from the state vector
            dotq, q = getDotQAndQFromStateVector(tmpState)
            coordElbow, coordHand = self.arm.mgdFull(q)
            #print ("dotq :",dotq)
            #computation of the coordinates to check if the target is reach or not
            #code to save data of the trajectory

            #Note : these structures might be much improved
            if self.saveTraj == True: 
                stepStore.append(vectarget)
                stepStore.append(estimState)
                stepStore.append(tmpState)
                stepStore.append(Unoisy)
                stepStore.append(np.array(U))
                stepStore.append(estimNextState)
                stepStore.append(realNextState)
                stepStore.append([coordElbow[0], coordElbow[1]])
                stepStore.append([coordHand[0], coordHand[1]])
                #print ("before",stepStore)
                tmpstore = np.array(stepStore).flatten()
                row = [item for sub in tmpstore for item in sub]
                #print ("store",row)
                dataStore.append(row)

            estimState = estimNextState
            i += 1
            t += self.rs.dt

        cost += self.computeFinalReward(t,coordHand)

        if self.saveTraj == True:
            filename = findFilename(foldername+"Log/","traj",".log")
            np.savetxt(filename,dataStore)
            '''
            if coordHand[0] >= -self.sizeOfTarget/2 and coordHand[0] <= self.sizeOfTarget/2 and coordHand[1] >= self.rs.YTarget and i<230:
                foldername = pathDataFolder + "TrajRepository/"
                name = findFilename(foldername,"Traj",".traj")
                np.savetxt(name,dataStore)
            '''

        lastX = -1000 #used to ignore dispersion when the target line is not crossed
        if coordHand[1] >= self.rs.YTarget:
            lastX = coordHand[0]
        #print "end of trajectory"
        return cost, t, lastX