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