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 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 __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)
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 __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)
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 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 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 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 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 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 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 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
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 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 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)
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
