def saveOneTraj(self, x, y):
#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
self.coordHand = self.arm.mgdEndEffector(q)
#assert(coordHand[0]==x and coordHand[1]==y), "Erreur de MGD" does not work because of rounding effects
#initializes parameters for the trajectory
self.i, self.t, self.cost = 0, 0, 0
self.stateEstimator.initStore(state)
self.arm.setState(state)
self.estimState = state
self.dataStore=[]
self.vectarget=[0.0, 0.0, self.rs.XTarget, self.rs.YTarget]
totalCost=0
while(not self.isFinished()):
action=self.actor.action(self.estimState)
_,cost = self.actAndStore(action)
totalCost+= cost[0]
totalCost += self.trajCost.computeFinalReward(self.arm,self.t,self.coordHand,self.sizeOfTarget)
filename = findDataFilename(self.saveName+"Log/","traj"+str(x)+"-"+str(y),".log")
np.savetxt(filename,self.dataStore)
#used to ignore dispersion when the target line is not crossed
if self.coordHand[1] >= self.rs.YTarget:
self.lastCoord.append(self.coordHand[0])
return totalCost, self.t
def plotEstimator(setupFile, target_size, thetaName,x, y):
arm = ArmType["Arm26"]()
setupFile.det = True
tm=TrajMaker(setupFile, target_size, False, thetaName, "Inv")
state1,_,_,_=tm.runTrajectoryForPlot(x, y)
setupFile.det = False
state2,_,_,_=tm.runTrajectoryForPlot(x, y)
tm=TrajMaker(setupFile, target_size, False, thetaName, "No")
state3,_,_,_=tm.runTrajectoryForPlot(x, y)
plt.figure(1, figsize=(16,9))
hand=[]
for state in state1 :
hand.append(arm.mgdEndEffector(state[2:]))
hand=np.array(hand)
plt.plot(hand[:,0],hand[:,1], color="blue")
hand=[]
for state in state2 :
hand.append(arm.mgdEndEffector(state[2:]))
hand=np.array(hand)
plt.plot(hand[:,0],hand[:,1], color="red")
hand=[]
for state in state3 :
hand.append(arm.mgdEndEffector(state[2:]))
hand=np.array(hand)
plt.plot(hand[:,0],hand[:,1], color="green")
plt.xlabel("X (m)")
plt.ylabel("Y (m)")
imageFolder = setupFile.OPTIpath + "/ImageBank/"
name=findDataFilename(imageFolder, "Estimation", ".svg")
plt.savefig(name, bbox_inches='tight')
plt.show(block = True)
def saveOneTraj(self, x, y):
#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
self.coordHand = self.arm.mgdEndEffector(q)
#assert(coordHand[0]==x and coordHand[1]==y), "Erreur de MGD" does not work because of rounding effects
#initializes parameters for the trajectory
self.i, self.t, self.cost = 0, 0, 0
self.stateEstimator.initStore(state)
self.arm.setState(state)
self.estimState = state
self.dataStore = []
self.vectarget = [0.0, 0.0, self.rs.XTarget, self.rs.YTarget]
totalCost = 0
while (not self.isFinished()):
action = self.actor.action(self.estimState)
_, cost = self.actAndStore(action)
totalCost += cost[0]
totalCost += self.trajCost.computeFinalReward(self.arm, self.t,
self.coordHand,
self.sizeOfTarget)
filename = findDataFilename(self.saveName + "Log/",
"traj" + str(x) + "-" + str(y), ".log")
np.savetxt(filename, self.dataStore)
#used to ignore dispersion when the target line is not crossed
if self.coordHand[1] >= self.rs.YTarget:
self.lastCoord.append(self.coordHand[0])
return totalCost, self.t
def runTrajectories(self,theta, fonction):
'''
Generates all the trajectories of the experimental setup and return the mean cost. This function is used by cmaes to optimize the controller.
Input: -theta: vector of parameters, one dimension normalized numpy array
Ouput: -meanAll: the mean of the cost of all trajectories generated, float
'''
#c = Chrono()
self.initTheta(theta)
#print "theta avant appel :", theta
#compute all the trajectories x times each, x = numberOfRepeat
meanCost, meanTime = fonction(self.numberOfRepeat)
#cma.plot()
#opt = cma.CMAOptions()
#print "CMAES options :", opt
#c.stop()
#print("Indiv #: ", self.call, "\n Cost: ", meanCost)
if (self.call==0):
self.localBestCost = meanCost
self.localWorstCost = meanCost
self.localBestTime = meanTime
self.localWorstTime = meanTime
self.periodMeanCost = 0.0
self.periodMeanTime = 0.0
else:
if meanCost>self.localBestCost:
self.localBestCost = meanCost
elif meanCost<self.localWorstCost:
self.localWorstCost = meanCost
if meanTime>self.localBestTime:
self.localBestTime = meanTime
elif meanTime<self.localWorstTime:
self.localWorstTime = meanTime
if meanCost>self.bestCost:
self.bestCost = meanCost
if meanCost>0:
extension = ".save" + str(meanCost)
filename = findDataFilename(self.foldername+"Theta/", "theta", extension)
np.savetxt(filename, self.theta)
filename2 = self.foldername + "Best.theta"
np.savetxt(filename2, self.theta)
self.periodMeanCost += meanCost
self.periodMeanTime += meanTime
self.call += 1
self.call = self.call%self.period
if (self.call==0):
self.periodMeanCost = self.periodMeanCost/self.period
self.periodMeanTime = self.periodMeanTime/self.period
self.CMAESCostStore.append((self.localWorstCost,self.periodMeanCost,self.localBestCost))
self.CMAESTimeStore.append((self.localWorstTime,self.periodMeanTime,self.localBestTime))
costfoldername = self.foldername+"Cost/"
checkIfFolderExists(costfoldername)
cost = open(costfoldername+"cmaesCost.log","a")
time = open(costfoldername+"cmaesTime.log","a")
cost.write(str(self.localWorstCost)+" "+str(self.periodMeanCost)+" "+str(self.localBestCost)+"\n")
time.write(str(self.localWorstTime)+" "+str(self.periodMeanTime)+" "+str(self.localBestTime)+"\n")
cost.close()
time.close()
#np.savetxt(costfoldername+"cmaesCost.log",self.CMAESCostStore) #Note: inefficient, should rather add to the file
#np.savetxt(costfoldername+"cmaesTime.log",self.CMAESTimeStore) #Note: inefficient, should rather add to the file
return 10.0*(self.rs.rhoCF-meanCost)/self.rs.rhoCF
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
'''
self.costU12=0
#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.maxSteps:
stepStore = []
#computation of the next muscular activation vector using the controller theta
#print ("state :",self.arm.getState())
U = self.controller.computeOutput(estimState)
if self.rs.det:
Unoisy = muscleFilter(U)
else:
#Unoisy = getNoisyCommand(U,self.arm.getMusclesParameters().getKnoiseU())
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 self.rs.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.trajCost.computeStateTransitionCost(Unoisy)
self.costU12+=self.trajCost.computeStateTransitionCostU12(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 = self.arm.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.trajCost.computeFinalReward(self.arm,t,coordHand,self.sizeOfTarget)
if self.saveTraj == True:
filename = findDataFilename(foldername+"Log/","traj"+str(x)+"-"+str(y)+".",".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
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
'''
self.costU12 = 0
#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.maxSteps:
stepStore = []
#computation of the next muscular activation vector using the controller theta
#print ("state :",self.arm.getState())
U = self.controller.computeOutput(estimState)
if self.rs.det:
Unoisy = muscleFilter(U)
else:
#Unoisy = getNoisyCommand(U,self.arm.getMusclesParameters().getKnoiseU())
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 self.rs.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.trajCost.computeStateTransitionCost(Unoisy)
self.costU12 += self.trajCost.computeStateTransitionCostU12(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 = self.arm.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.trajCost.computeFinalReward(self.arm, t, coordHand,
self.sizeOfTarget)
if self.saveTraj == True:
filename = findDataFilename(foldername + "Log/",
"traj" + str(x) + "-" + str(y) + ".",
".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
