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)
