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
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
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("---------------------------------------------------------")
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)
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
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
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
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
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
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)
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)