def DerivadaFX(X0, ind, t, model, pfb, An): #----------------------------------------------------------- #variaveis globais #---------------------------------------------------------- glob = GlobalVariables() h = glob.getH() m = glob.getM() L = glob.getL() g = glob.getG() k = glob.getK() Bss = glob.getBss() expK = glob.getExpK() #MDH = glob.getMDH() #global h, MDH #----------------------------------------------------------- #X = [x,y,z,dx,dy] #U = [phi, theta, k] (model1) #U = [phi,theta,Bss] (model2) var = np.array([[t], [h], [model]]) if model == 1: params = np.array([[m], [L], [g], [k], [Bss], [expK]]) else: params = np.array([[m], [L], [g], [k], [Bss], [t], [pfb[0, 0]], [pfb[1, 0]], [pfb[2, 0]], [expK]]) #----------------------------------------------------------- #cálculo do primeiro ponto para 2h #---------------------------------------------------------- X0[ind, 0] = x[ind, 0] + 2 * h #calculando f(x+2h) #theta1 = np.concatenate((thetar, thetal),axis=1) #xn vem na forma x = [x,y,z,dx,dy,dz], mas para ser utilizado na função #rungeKutta42, é preciso estar na forma x = [x,dx,y,dy,z,dz] yn = baguncaX(x) xn = rungeKutta42(var, yn, params) #voltar xn para a forma xn = [x,y,z,dx,dy,dz] e eliminar o último elemento xn = arrumaX(xn) xn = xn[0:5, :] a = np.dot(An, xn) #a = a[:-1] #----------------------------------------------------------- #cálculo do segundo ponto para h #---------------------------------------------------------- x[ind, 0] = x[ind, 0] + h #calculando f(x+h) #theta1 = np.concatenate((thetar, thetal),axis=1) yn = baguncaX(x) xn = rungeKutta42(var, yn, params) #voltar xn para a forma xn = [x,y,z,dx,dy,dz] e eliminar o último elemento xn = arrumaX(xn) xn = xn[0:5, :] b = np.dot(An, xn) #b = b[:-1] #----------------------------------------------------------- #cálculo do terceiro ponto para -h #---------------------------------------------------------- x[ind, 0] = x[ind, 0] - h #calculando f(x-h) #theta1 = np.concatenate((thetar, thetal),axis=1) yn = baguncaX(x) xn = rungeKutta42(var, yn, params) #voltar xn para a forma xn = [x,y,z,dx,dy,dz] e eliminar o último elemento xn = arrumaX(xn) xn = xn[0:5, :] c = np.dot(An, xn) #c = c[:-1] #----------------------------------------------------------- #cálculo do quarto ponto para -2h #---------------------------------------------------------- x[ind, 0] = x[ind, 0] - 2 * h #calculando f(x-2h) #theta1 = np.concatenate((thetar, thetal),axis=1) yn = baguncaX(x) xn = rungeKutta42(var, yn, params) #voltar xn para a forma xn = [x,y,z,dx,dy,dz] e eliminar o último elemento xn = arrumaX(xn) xn = xn[0:5, :] d = np.dot(An, xn) #d = a[:-1] #----------------------------------------------------------- #cálculo da derivada (elemento do jacobiano) #---------------------------------------------------------- q1 = (-a + 8 * b - 8 * c + d) * ((1 / 12) * h) #r = getRotationDualQuat(q1) #normq = np.linalg.norm(r) #if normq != 0: #q1 = q1/(normq) return q1
def DerivadaFU(u,x,ind,t,model,pfb,An): #----------------------------------------------------------- #variaveis globais #---------------------------------------------------------- glob = GlobalVariables() h = glob.getH() m = glob.getM() L = glob.getL() g = glob.getG() k = glob.getK() Bss = glob.getBss() expK = glob.getExpK() #------------------------------------------------------------ #X = [x,y,z,dx,dy] #U = [phi, theta, k] (model1) #U = [phi,theta,Bss] (model2) var = np.array([[t],[h],[model]]) if model == 1: #----------------------------------------------------------- #cálculo do primeiro ponto para 2h #---------------------------------------------------------- u[ind,0] = u[ind,0] + 2*h #calculando f(x+2h) #xn vem na forma x = [x,y,z,dx,dy,dz], mas para ser utilizado na função #rungeKutta42, é preciso estar na forma x = [x,dx,y,dy,z,dz] params = np.array([[m],[L],[g],[k],[u[2]],[expK]]) yn = baguncaX(x) xn = rungeKutta42(var,yn,params) #voltar xn para a forma xn = [x,y,z,dx,dy,dz] e eliminar o último elemento xn = arrumaX(xn) xn = xn[0:5,:] a = np.dot(An,xn) #a = a[:-1] #----------------------------------------------------------- #cálculo do primeiro ponto para h #---------------------------------------------------------- u[ind,0] = u[ind,0] + h #calculando f(x+h) params = np.array([[m],[L],[g],[k],[u[2]],[expK]]) yn = baguncaX(x) xn = rungeKutta42(var,yn,params) #voltar xn para a forma xn = [x,y,z,dx,dy,dz] e eliminar o último elemento xn = arrumaX(xn) xn = xn[0:5,:] b = np.dot(An,xn) #a = a[:-1] #----------------------------------------------------------- #cálculo do primeiro ponto para -h #---------------------------------------------------------- u[ind,0] = u[ind,0] - h #calculando f(x-h) params = np.array([[m],[L],[g],[k],[u[2]],[expK]]) yn = baguncaX(x) xn = rungeKutta42(var,yn,params) #voltar xn para a forma xn = [x,y,z,dx,dy,dz] e eliminar o último elemento xn = arrumaX(xn) xn = xn[0:5,:] c = np.dot(An,xn) #a = a[:-1] #----------------------------------------------------------- #cálculo do primeiro ponto para -2h #---------------------------------------------------------- u[ind,0] = u[ind,0] - 2*h #calculando f(x-2h) params = np.array([[m],[L],[g],[k],[u[2]],[expK]]) yn = baguncaX(x) xn = rungeKutta42(var,yn,params) #voltar xn para a forma xn = [x,y,z,dx,dy,dz] e eliminar o último elemento xn = arrumaX(xn) xn = xn[0:5,:] d = np.dot(An,xn) #a = a[:-1] else: #----------------------------------------------------------- #cálculo do primeiro ponto para 2h #---------------------------------------------------------- u[ind,0] = u[ind,0] + 2*h #calculando f(x+2h) params = np.array([[m],[L],[g],[u[2]],[Bss],[t],[pfb[0,0]],[pfb[1,0]],[pfb[2,0]],[expK]]) yn = baguncaX(x) xn = rungeKutta42(var,yn,params) #voltar xn para a forma xn = [x,y,z,dx,dy,dz] e eliminar o último elemento xn = arrumaX(xn) xn = xn[0:5,:] a = np.dot(An,xn) #a = a[:-1] #----------------------------------------------------------- #cálculo do primeiro ponto para h #---------------------------------------------------------- u[ind,0] = u[ind,0] + h #calculando f(x+h) params = np.array([[m],[L],[g],[u[2]],[Bss],[t],[pfb[0,0]],[pfb[1,0]],[pfb[2,0]],[expK]]) yn = baguncaX(x) xn = rungeKutta42(var,yn,params) #voltar xn para a forma xn = [x,y,z,dx,dy,dz] e eliminar o último elemento xn = arrumaX(xn) xn = xn[0:5,:] b = np.dot(An,xn) #a = a[:-1] #----------------------------------------------------------- #cálculo do primeiro ponto para -h #---------------------------------------------------------- u[ind,0] = u[ind,0] - h #calculando f(x-h) params = np.array([[m],[L],[g],[u[2]],[Bss],[t],[pfb[0,0]],[pfb[1,0]],[pfb[2,0]],[expK]]) yn = baguncaX(x) xn = rungeKutta42(var,yn,params) #voltar xn para a forma xn = [x,y,z,dx,dy,dz] e eliminar o último elemento xn = arrumaX(xn) xn = xn[0:5,:] c = np.dot(An,xn) #a = a[:-1] #----------------------------------------------------------- #cálculo do primeiro ponto para -2h #---------------------------------------------------------- u[ind,0] = u[ind,0] - 2*h #calculando f(x-2h) params = np.array([[m],[L],[g],[k],[u[2]],[expK]]) yn = baguncaX(x) xn = rungeKutta42(var,yn,params) #voltar xn para a forma xn = [x,y,z,dx,dy,dz] e eliminar o último elemento xn = arrumaX(xn) xn = xn[0:5,:] d = np.dot(An,xn) #a = a[:-1] q1 = (-a + 8*b -8*c + d)* ((1/12) * h) return q1
dyod = 0.00 #condição de balanço dzod = 0.00 #velocidade em z (igual a zero condição necessaria) #----------------------------------------------------------- #Tamanho do passo (não sendo usado ainda estudar como incorporar #esse dado) #----------------------------------------------------------- lstep = 0.5 + 0.1 * (dxod - 1) #----------------------------------------------------------- #valores máximos das variáveis de controle #reduzir o espaço de busca #----------------------------------------------------------- thetaM = glob.getThetaM() phiM = glob.getPhiM() KM = glob.getKM() expK = glob.getExpK() #ordem de grandeza da constante massa-mola BSSM = glob.getBSSM() #params = [thetaM;phiM;KM;expK;BSSM]; #----------------------------------------------------------- #variavel de controle inicial #modificar os valores obtidos aqui #Geralmente na literatura são usados x para variável de estado #e u para a variável de controle sendo usados essas letras #em maiusculo para representação #----------------------------------------------------------- #phi = 0.5000000000 #theta = 0.3801423352 #k = 19611.4821640244 #Bss = 0.0275951012
def plotarTrajetoria(U, X): #----------------------------------------------------------- #variáveis globais #----------------------------------------------------------- #global m L g pfa expK hEdo glob = GlobalVariables() m = glob.getM() L = glob.getL() g = glob.getG() pfa = glob.getPfa() hEdo = glob.getHEDO() expK = glob.getExpK() #----------------------------------------------------------- #condições iniciais para MS #----------------------------------------------------------- xod = X[0, 0] yod = X[1, 0] zod = X[2, 0] dxod = X[3, 0] dyod = X[4, 0] dzod = X[5, 0] #----------------------------------------------------------- #valores para a otimização - valores de u #----------------------------------------------------------- #u = [phi theta k Bss] theta = U[0, 0] phi = U[1, 0] k = U[2, 0] Bss = U[3, 0] expK = U[4, 0] #----------------------------------------------------------- #vetor com as condições iniciais MS #----------------------------------------------------------- y0 = np.array([[xod], [dxod], [yod], [dyod], [zod], [dzod]]) #----------------------------------------------------------- #vetor com os parâmetros constantes #----------------------------------------------------------- params = np.array([[m], [L], [g], [k], [Bss], [expK]]) #----------------------------------------------------------- #Parâmetros para os métodos #----------------------------------------------------------- t = 0 #inicio do tempo t = 0 h = hEdo #passo do método rungeKutta42 inicial N = 10000 #número máximo de iterações y = np.zeros((6, 1)) #primeiro método sh = h #tamanho do passo para o método rungeKutta42 atualizando durante a execução do método ind = 0 #contador #----------------------------------------------------------- #vetores auxiliares para guardar a trajetória #----------------------------------------------------------- px = [y0[0, 0]] py = [y0[2, 0]] pz = [y0[4, 0]] #----------------------------------------------------------- #inicio do método primeiro MS para TD #----------------------------------------------------------- for x in np.arange(0, N * h, h): #----------------------------------------------------------- #vetor de parâmetros #----------------------------------------------------------- var = np.array([[t], [h], [1]]) #----------------------------------------------------------- #método numérico para solucionar as equações diferenciais #passo a passo #----------------------------------------------------------- y = rungeKutta42(var, y0, params) #----------------------------------------------------------- #atualizando a condição inicial #----------------------------------------------------------- y0 = y #----------------------------------------------------------- #atualizando o instante t #----------------------------------------------------------- t = t + sh #----------------------------------------------------------- #verificando a condição de parada posição Z < que Z de touchdown #Z de touchdown = L*cos(theta) #----------------------------------------------------------- if y0[4, 0] < L * mt.cos(theta): break #----------------------------------------------------------- #colocando os valores nos vetores auxiliares #----------------------------------------------------------- px.append(y0[0, 0]) py.append(y0[2, 0]) pz.append(y0[4, 0]) #----------------------------------------------------------- #atualizando o contador #----------------------------------------------------------- ind = ind + 1 #----------------------------------------------------------- #atualizando o contador - tratando o valor #----------------------------------------------------------- #if ind > 1: #ind = ind -1 #----------------------------------------------------------- #Posição do centro de massa no momento de Touchdown (TD) #----------------------------------------------------------- pc = np.array([[px[ind]], [py[ind]], [pz[ind]]]) #centro de massa px = np.asarray(px) py = np.asarray(py) pz = np.asarray(pz) #----------------------------------------------------------- #posição do pé de balaço quando toca o chão #----------------------------------------------------------- pfb = pc + L * np.array([ mt.sin(theta) * mt.cos(phi), mt.sin(theta) * mt.sin(phi), -mt.cos(theta) ]) #----------------------------------------------------------- #tempo em que acontece a codição de touchdown #----------------------------------------------------------- TD = t #tempo de TD #----------------------------------------------------------- #parâmetros constante para o segundo método #----------------------------------------------------------- params = np.array([[m], [L], [g], [k], [Bss], [t], [pfb[0, 0]], [pfb[1, 0]], [pfb[2, 0]], [expK]]) #----------------------------------------------------------- #iniciando o segundo contador #----------------------------------------------------------- ind2 = 0 sh = h #tamanho do passo para o método rungeKutta42 atualizando durante a execução do método #----------------------------------------------------------- #vetores auxiliares para guardar a trajetória #----------------------------------------------------------- px2 = [y0[0, 0]] py2 = [y0[2, 0]] pz2 = [y0[4, 0]] #----------------------------------------------------------- #inicio do método 2 - TD para LH #----------------------------------------------------------- for x in np.arange(0, N * h, h): #----------------------------------------------------------- #vetor de parâmetros #----------------------------------------------------------- var = np.array([[t], [h], [0]]) #----------------------------------------------------------- #método numérico para solucionar as equações diferenciais #passo a passo #----------------------------------------------------------- y = rungeKutta42(var, y0, params) #----------------------------------------------------------- #atualizando nova condição inicial #----------------------------------------------------------- y0 = y #----------------------------------------------------------- #atualizando o instante t #----------------------------------------------------------- t = t + sh #----------------------------------------------------------- #verificando a condição de parada posição dZ > 0 #----------------------------------------------------------- if y0[5, 0] > 0: break #----------------------------------------------------------- #atualizando o contador #----------------------------------------------------------- ind2 = ind2 + 1 #----------------------------------------------------------- #atualizando os vetores auxiliares da trajetória #----------------------------------------------------------- px2.append(y0[0, 0]) py2.append(y0[2, 0]) pz2.append(y0[4, 0]) #----------------------------------------------------------- #atualizando o contador #----------------------------------------------------------- #if ind2 > 1: #ind2 = ind2 -1 #----------------------------------------------------------- #atualizando a posição do centro de massa #----------------------------------------------------------- pc = np.array([px2[ind2], py2[ind2], pz2[ind2]]) #centro de massa px2 = np.asarray(px2) py2 = np.asarray(py2) pz2 = np.asarray(pz2) #----------------------------------------------------------- # trajetoria # a trajetoria é simetrica ao ponto de middle suport # desta forma fazemos um espelho da função #deslocado ela para a origem #----------------------------------------------------------- #plot3(px(1,1:ind),py(1,1:ind),pz(1,1:ind),'b') #plot3(px2(1,1:ind2),py2(1,1:ind2),pz2(1,1:ind2),'r') plot3 = plt.figure(px[0:ind], py[0:ind], pz[0:ind], 'b') plot3 = plt.figure(px2[0:ind2], py2[0:ind2], pz2[0:ind2], 'r') #----------------------------------------------------------- #espelho da função #----------------------------------------------------------- offsetx = px2[ind2] offsety = py2[ind2] plot3 = plt.figure(-px2 + 2 * offsetx, -py2 + 2 * offsety, pz2, 'r') offsetx = -px2[ind2] + 2 * offsetx offsety = -py2[ind2] + 2 * offsety plot3 = plt.figure(-px[range(ind, -1, -1)] + 2 * offsetx, -py[range(ind, -1, -1)] + 2 * offsety, pz[range(ind, -1, -1)], 'b') #----------------------------------------------------------- #projeção #----------------------------------------------------------- plot3 = plt.figure(px[0:ind], py[0, 0:ind], 0 * pz[0:ind], 'g') plot3 = plt.figure(px2[0, 0:ind2], py2[0, 0:ind2], 0 * pz2[0:ind2], 'm') #----------------------------------------------------------- #espelho da função #----------------------------------------------------------- offsetx = px2[0, ind2] offsety = py2[0, ind2] plot3 = plt.figure(-px2([range(ind2, -1, -1)]) + 2 * offsetx, -py2[range(ind2, -1, -1)] + 2 * offsety, 0 * pz2[range(ind2, -1, -1)], 'm') offsetx = -px2[0, ind2] + 2 * offsetx offsety = -py2[0, ind2] + 2 * offsety plot3 = plt.figure(-px[range(ind, -1, -1)] + 2 * offsetx, -py[range(ind, -1, -1)] + 2 * offsety, 0 * pz[range(ind, -1, -1)], 'g') #----------------------------------------------------------- #plotar posição dos pés #----------------------------------------------------------- plot3 = plt.figure([pfa[0, 0], pfb[0, 0]], [pfa[1, 0], pfb[1, 0]], [0, 0], '--k') #----------------------------------------------------------- #projeção centro de massa e projeção centro de massa #----------------------------------------------------------- plot3 = plt.figure(pc[0, 0], pc[1, 0], pc[2, 0], 'o') plot3 = plt.figure([pc[0, 0], pc[0, 0]], [pc[1, 0], pc[1, 0]], [pc[2, 0], 0], ':') plot3 = plt.figure(pc[0, 0], pc[1, 0], 0, 'x') #----------------------------------------------------------- #configuração do grafico #----------------------------------------------------------- plot3 = plt.figure(figsize=(60, 35)) ax = plot3.add_subplot(111, projection='3d') plt.grid(True) ax.set_xlim([-1, 1]) ax.set_ylim([-0.5, 0.5]) ax.set_zlim([-1, -0.2]) ax.set_xlabel('X') ax.set_ylabel('Y') ax.set_zlabel('Z') ax.show()