def plotManipulability(): 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.directionalManipulability(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/manipulability.png", bbox_inches='tight') plt.show(block = True)
def plotManipulability(): 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.directionalManipulability(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/manipulability.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