def derivadaX(xn, X0, U0, indice, i): glob = GlobalVariables() h = glob.getH() xnX = TrajetoriaVariandoX(X0, U0, i, indice) dFx = (xnX - xn) / h return dFx
def derivadaU(xn, X0, U0, indice, i): glob = GlobalVariables() h = glob.getH() xnU = TrajetoriaVariandoU(X0, U0, i, indice) dFu = (xnU - xn) / h return dFu
def primeiraDerivadaGradiente(X0, U0, ind): #----------------------------------------------------------- #variáveis globais #---------------------------------------------------------- glob = GlobalVariables() h = glob.getH() #global h #----------------------------------------------------------- #cálculo do primeiro ponto para 2h #---------------------------------------------------------- y = U0[ind, 0] + 2 * h Y = U0 #copiando o vetor de controle original Y[ind, 0] = y #substituindo o valor com a variação na posição desejada [pa, pb, pc, M, ponto] = trajetoria(Y, X0) #cálculo dos pontos da trajetória #pa - ponto do pé A #pb - ponto do pé B #pc - ponto do centro de massa a = funcaoObjetivo(pa, pb, pc) #calculo da função objetivo #----------------------------------------------------------- #cálculo do segundo ponto para h #---------------------------------------------------------- y = U0[ind, 0] + h Y = U0 Y[ind, 0] = y [pa, pb, pc, M, ponto] = trajetoria(Y, X0) b = funcaoObjetivo(pa, pb, pc) #----------------------------------------------------------- #cálculo do terceiro ponto para -h #---------------------------------------------------------- y = U0[ind, 0] - h Y = U0 Y[ind, 0] = y [pa, pb, pc, M, ponto] = trajetoria( Y, X0 ) #tive de acrescentar as variáveis M e ponto, pois a função trajetória #retorna todos esses valores, mas só me interessam pa, pb e pc, porém, não sei como pegar, no python, #apenas uma parte do retorno da função c = funcaoObjetivo(pa, pb, pc) #----------------------------------------------------------- #cálculo do quarto ponto para -2h #---------------------------------------------------------- y = U0[ind, 0] - 2 * h Y = U0 Y[ind, 0] = y [pa, pb, pc, M, ponto] = trajetoria(Y, X0) d = funcaoObjetivo(pa, pb, pc) #----------------------------------------------------------- #cálculo da derivada #---------------------------------------------------------- #print((-a + 8*b -8*c + d)/(12*h) + h**4) d1 = (-a + 8 * b - 8 * c + d) / (12 * h) + h**4 #print(h) return d1
def trajetoria(U, X): #----------------------------------------------------------- #variáveis globais #----------------------------------------------------------- glob = GlobalVariables() m = glob.getM() #massa L = glob.getL() #tamanho da perna g = glob.getG() #gravidade h = glob.getH() #passo para o calculo das derivadas hEdo = glob.getHEDO() #passo para calculo - EDO #----------------------------------------------------------- #condições iniciais para MS (referentes ao CM) #----------------------------------------------------------- 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 = [theta phi 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 rungekuttaLIMP inicial N = 10000 #número máximo de iterações #primeiro metodo sh = h #tamanho do passo para o método rungekuttaLIMP atualizando durante a execução do método ind = 0 #contador #traj = ind + 1 #tamanho do vetor com os pontos da trajetória #----------------------------------------------------------- #vetores auxiliares para guardar a trajetória #----------------------------------------------------------- px = [y0[0, 0]] py = [y0[2, 0]] pz = [y0[4, 0]] #y = np.zeros((6,1)) #----------------------------------------------------------- #inicio do método 1 MS para TD #----------------------------------------------------------- for i in range(N): #for de 0 até N*h, com passo 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 = rungeKuttaLIPM(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 t >= 0.105: break #----------------------------------------------------------- #colocando os valores nos vetores auxiliares #----------------------------------------------------------- else: 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: #não preciso desse if, porque iniciei o contador no 0 #ind = ind -1 #o Juan havia iniciado em 1 #end ponto = ind #----------------------------------------------------------- #Posição do centro de massa no momento de Touchdown (TD) #----------------------------------------------------------- pc = np.array([[px[ind]], [py[ind]], [pz[ind]]]).reshape( (3, 1)) #centro de massa #----------------------------------------------------------- #posição do pé de balanço quando toca o chão #----------------------------------------------------------- pfb = pc + L * np.array([[np.sin(theta) * np.cos(phi)], [np.sin(theta) * np.sin(phi)], [-np.cos(theta)]]) #----------------------------------------------------------- #tempo em que acontece a codição de touchdown #----------------------------------------------------------- TD = t #tempo de TD #----------------------------------------------------------- #parametros 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 #traj2 = ind2 + 1 sh = h #tamanho do passo para o método rungekuttaLIMP 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 range(N): #----------------------------------------------------------- #vetor de parâmetros #----------------------------------------------------------- var = np.array([[t], [h], [0]]) #----------------------------------------------------------- #método numérico para solucionar as equações diferenciais #passo a passo #----------------------------------------------------------- y = rungeKuttaLIPM(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 v.all(): if t >= 0.135: break #----------------------------------------------------------- #atualizando os vetores auxiliares da trajetória #----------------------------------------------------------- else: px2.append(y0[0, 0]) py2.append(y0[2, 0]) pz2.append(y0[4, 0]) #----------------------------------------------------------- #atualizando o contador #----------------------------------------------------------- ind2 = ind2 + 1 #----------------------------------------------------------- #atualizando o contador - tratando o valor #----------------------------------------------------------- #if ind2 > 1 # ind2 = ind2 -1; #----------------------------------------------------------- #trajetória do centro de massa CoM M = [x y z] #----------------------------------------------------------- # concatenando as listas, para preencher o vetor M pxTot = px + px2 pyTot = py + py2 pzTot = pz + pz2 pxTot = np.asarray(pxTot) pxTot = pxTot.reshape(-1, 1) pyTot = np.asarray(pyTot) pyTot = pyTot.reshape(-1, 1) pzTot = np.asarray(pzTot) pzTot = pzTot.reshape(-1, 1) M = np.concatenate((pxTot, pyTot, pzTot), axis=1) M = M.reshape(-1, 3) #----------------------------------------------------------- #atualizando a posição do centro de massa #----------------------------------------------------------- pc = np.array([[px2[ind2]], [py2[ind2]], [pz2[ind2]]]).reshape( (3, 1)) #centro de massa #----------------------------------------------------------- #atualizando a posição dos pés e do centro de massa #para o retorno da função #----------------------------------------------------------- pa = np.array([[0], [0], [0]]) pb = np.array([[pfb[0, 0]], [pfb[1, 0]], [0]]) pc = np.array([[pc[0, 0]], [pc[1, 0]], [pc[2, 0]]]) #print(np.shape(pc[1,0])) return pa, pb, pc, M, ponto
def caminhada2(U0, X0, tam, vecGanho1, vecGanho2): #----------------------------------------------------------- #Obter todas as trajeotrias do CoM #----------------------------------------------------------- import numpy as np [PA, PB, PC, trajCoM1, indContadoPe] = trajetoria(U0, X0) #trajetória para o CoM #trajetoria 2 trajCoM = trajCoM1 #print(np.size(trajCoM1)) ind = np.size(trajCoM, 0) #pegar a última posição do vetor de pontos #a partir daqui vai dentro do loop #até aqui vai dentro do loop #----------------------------------------------------------- #Trajetória dos pés #----------------------------------------------------------- passoComprimento = PB[0, 0] #tamanho do passo passoLargura = PB[1, 0] #Largura do passo passoAltura = 0.2 #altura de cada passo #trajetoria pé B inicial tamTrajPeB1 = indContadoPe #trajPB1 = trajetoriaPes(np.array([[passoComprimento],[passoLargura],[0]]),passoComprimento,passoAltura,1,tamTrajPeB1) trajPB1 = trajetoriaPesInicio( np.array([[passoComprimento], [passoLargura], [0]]), passoComprimento, passoAltura, tamTrajPeB1) passoComprimento2 = PB[0, 0] #tamanho do passo passoLargura2 = 0 #Largura do passo passoAltura2 = 0.2 #altura de cada passo #trajetoria pé A tamTrajPa = (np.size(trajCoM1, 0) + indContadoPe) / 2 trajPA = trajetoriaPes( np.array([[passoComprimento2 + passoComprimento2], [passoLargura2], [0]]), passoComprimento2, passoAltura2, 0, tamTrajPa) #trajPA = np.asarray(trajPA) #trajtoria pé B trajPB = trajPA #k = np.size(trajPB,0) #for i in range(k): trajPB[:, 0] = trajPB[:, 0] + passoComprimento trajPB[:, 1] = passoLargura #-------------------------------------------------------------------------------------- #Modelo do robô #primeiros testes #implementação começa aquiiii #-------------------------------------------------------------------------------------- #parâmetros necessarios glob = GlobalVariables() h = glob.getH() #hpi = glob.getHpi() #L1 = glob.getL1() #L2 = glob.getL2() #L3 = glob.getL3() #L4 = glob.getL4() #L5 = glob.getL5() #height = glob.getHeight() #MDH = glob.getMDH() #global hpi, L1, L2, L3, L4, L5, height, MDH #hpi = np.pi/2 #N = 6 #Numero de Juntas #Comprimento das Juntas #parametros hubo #L1 = 0.085 #L2 = 0.0 #L2 = 0.182;#modificando L2 para que L seja 1 depois rodar a simulação de novo #L3 = 0.3 #L4 = 0.3 #L5 = 0.0663 #height = L2+L3+L4+L5 #altura inicial total do robô pé para o centro de massa #Parametros de D.H. Perna- tabela do artigo thetaR = glob.getOr() thetaL = glob.getOl() theta = np.concatenate((thetaR, thetaL), axis=1) # parametros variaveis tempo = 0 un = U0 #primeira parte da caminhdada #print('primeiro passinho') passos = 1 [ha, ha2, theta, tempo1] = fase1(trajCoM1, indContadoPe, trajPB1, theta, vecGanho1) if passos >= tam: return tempo = tempo + tempo1 i = 0 while 1: glob = GlobalVariables() dt = glob.getHEDO() n = ind dx = (trajCoM[n - 1, 0] - trajCoM[n - 2, 0]) / dt dy = (trajCoM[n - 1, 1] - trajCoM[n - 2, 1]) / dt #dz = (trajCoM3_1[n-1,2] - trajCoM3_1[n-2,2])/dt dz = 0 #print(X0[0:3,:]) x = X0[0:3, :] + np.array([[0.43605039], [0.05947525], [0.0]]) #print(x) v = np.array([[dx], [dy], [dz]]) #print(v) xn = np.concatenate((x, v), axis=0) un = LRQ3D(U0, X0, passos, tempo, xn, i) #aqui deve ser rodado em paralelo #trajCoM2 = np.zeros((ind,3)) trajCoM2_1 = np.zeros((ind, 3)) trajCoM2_2 = np.zeros((ind, 3)) #calculo da trajetória do CoM na fase2: offsetx = trajCoM[ind - 1, 0] #cálcular o offset em x offsety = trajCoM[ind - 1, 1] #calcular o offset em y trajCoM2_1[:, 0] = -trajCoM[ range(ind - 1, -1, -1), 0] + 2 * offsetx #calcular a trajetória simétrica para x trajCoM2_1[:, 1] = -trajCoM[ range(ind - 1, -1, -1), 1] + 2 * offsety #calcular a trajetória simétrica para y trajCoM2_1[:, 2] = trajCoM[range(ind - 1, -1, -1), 2] #em z não muda #trajCoM2_2 = np.zeros((ind,3)) #o tamanho da trajetória será sempre o mesmo??????????????? #Aqui será feito o espelhamento da trajetória offsetx = trajCoM2_1[ind - 1, 0] #cálcular o offset em x offsety = trajCoM2_1[ind - 1, 1] #calcular o offset em y trajCoM2_2[:, 0] = -trajCoM2_1[ range(ind - 1, -1, -1), 0] + 2 * offsetx #calcular a trajetória simétrica para x trajCoM2_2[:, 1] = trajCoM2_1[range(ind - 1, -1, -1), 1] #calcular a trajetória simétrica para y trajCoM2_2[:, 2] = trajCoM2_1[range(ind - 1, -1, -1), 2] #em z não muda trajCoM2 = np.concatenate((trajCoM2_1, trajCoM2_2), axis=0) passoTrajCoM = trajCoM2[np.size(trajCoM2, 0) - 1, 0] - trajCoM2[0, 0] trajCoM = trajCoM2 #for j in range(np.size(trajCoM,0)): trajCoM[:, 0] = trajCoM[:, 0] + i * 2 * passoTrajCoM trajP = trajPA #k = np.size(trajP,0) #for j in range(k): trajP[:, 0] = trajP[:, 0] + i * 2 * passoComprimento passos = passos + 1 [ha, ha2, theta, tempo2] = fase2(ha, ha2, trajCoM, np.size(trajPA, 0), trajP, theta, tempo, vecGanho2) if passos >= tam: #tam é a quantidade de passos da trajetória desejada return #aqui começa a fase3######################################################### tempo = tempo + tempo2 #É necessário recalcular a trajetória do CoM com o novo vetor u #ind = np.size(trajCoM2_1,0) #pegar a última posição do vetor de pontos trajCoM3_1 = np.zeros((ind, 3)) #isso está correto??????????????????????????? offsetx = trajCoM2_1[(ind - 1), 0] #cálcular o offset em x offsety = trajCoM2_1[(ind - 1), 1] #calcular o offset em y #clr trajCoM3 trajCoM3_1[:, 0] = -trajCoM2_1[range( (ind - 1), -1, -1 ), 0] + 2 * offsetx #calcular a trajetória simétrica para x trajCoM3_1[:, 1] = -trajCoM2_1[range( (ind - 1), -1, -1 ), 1] + 2 * offsety #calcular a trajetória simétrica para y trajCoM3_1[:, 2] = trajCoM2_1[range((ind - 1), -1, -1), 2] #em z não muda #calculando as velocidades #glob = GlobalVariables() #dt = glob.getHEDO() #n = np.size(trajCoM3_1,0) #dx = (trajCoM3_1[n-1,0] - trajCoM3_1[n-2,0])/dt #dy = (trajCoM3_1[n-1,1] - trajCoM3_1[n-2,1])/dt #dz = (trajCoM3_1[n-1,2] - trajCoM3_1[n-2,2])/dt #dz = 0 #v = np.array([dx,dy,dz]) #print(v) #print(U0) #print(X0) #print(trajCoM[n-1,:].reshape((3,1)) - X0[0:3,0]) #xn = np.concatenate((trajCoM[n-1,:],v),0).reshape((6,1)) #dx = xn - X0 #print(dx) #un = LRQ3D(U0,X0,passos,tempo,xn,i) u0 = np.array([un[0, 0], un[1, 0], U0[2, 0], un[2, 0], U0[4, 0]]).reshape((5, 1)) [PA, PB, PC, trajCoM3_2, indContadoPe] = trajetoria(u0, xn) X0 = xn U0 = u0 #print(U0) ind = np.size(trajCoM3_1, 0) + np.size(trajCoM3_2, 0) trajCoM3 = np.zeros((ind, 3)) trajCoM3 = np.concatenate((trajCoM3_1, trajCoM3_2), axis=0) trajCoM = trajCoM3 #for j in range(np.size(trajCoM,0)): trajCoM[:, 0] = trajCoM[:, 0] + i * 2 * passoTrajCoM trajP = trajPB #k = np.size(trajP,0) #for j in range(k): trajP[:, 0] = trajP[:, 0] + i * 2 * passoComprimento #A cada 2 passos ou quando a velocidade muda, é necessário chamar o #LQR do modelo 3D #quem serão xn e pc?????????????????????????????????????????????????? #CoM = trajCoM ind = np.size(trajCoM, 0) passos = passos + 1 [ha, ha2, theta, tempo3] = fase3(ha, ha2, trajCoM, np.size(trajPB, 0), trajP, theta, tempo, vecGanho1) if passos >= tam: return tempo = tempo + tempo3 i = i + 1
def TrajetoriaVariandoX(X0, U0, i, indice): #funçõa para calcular a trajetória variando X0 e U0, para calcular os jacobianos glob = GlobalVariables() h = glob.getH() X0[indice, 0] = X0[indice, 0] + h #trajetórias na fase 1 [PAx, PBx, PCx, trajCoMX1, indContadoPex] = trajetoria(U0, X0) #trajetórias na fase 2: indX = np.size(trajCoMX1, 0) trajCoMX2_1 = np.zeros((indX, 3)) trajCoMX2_2 = np.zeros((indX, 3)) #primeira metade################################################################ offsetxX = trajCoMX1[indX - 1, 0] #cálcular o offset em x offsetyX = trajCoMX1[indX - 1, 1] #calcular o offset em y trajCoMX2_1[:, 0] = -trajCoMX1[ range(indX - 1, -1, -1), 0] + 2 * offsetxX #calcular a trajetória simétrica para x trajCoMX2_1[:, 1] = -trajCoMX1[ range(indX - 1, -1, -1), 1] + 2 * offsetyX #calcular a trajetória simétrica para y trajCoMX2_1[:, 2] = trajCoMX1[range(indX - 1, -1, -1), 2] #em z não muda #segunda metade#################################################################3 offsetxX = trajCoMX2_1[indX - 1, 0] #cálcular o offset em x offsetyX = trajCoMX2_1[indX - 1, 1] #calcular o offset em y trajCoMX2_2[:, 0] = -trajCoMX2_1[ range(indX - 1, -1, -1), 0] + 2 * offsetxX #calcular a trajetória simétrica para x trajCoMX2_2[:, 1] = trajCoMX2_1[range(indX - 1, -1, -1), 1] #calcular a trajetória simétrica para y trajCoMX2_2[:, 2] = trajCoMX2_1[range(indX - 1, -1, -1), 2] #em z não muda #concatenando as duas: trajCoM2X = np.concatenate((trajCoMX2_1, trajCoMX2_2), axis=0) passoTrajCoMX = trajCoM2X[np.size(trajCoM2X, 0) - 1, 0] - trajCoM2X[0, 0] trajCoM2X[:, 0] = trajCoM2X[:, 0] + i * 2 * passoTrajCoMX #Trajetoria na fase3: trajCoMX3_1 = np.zeros((indX, 3)) #isso está correto??????????????????????????? offsetxX = trajCoMX2_1[(indX - 1), 0] #cálcular o offset em x offsetyX = trajCoMX2_1[(indX - 1), 1] #calcular o offset em y #clr trajCoM3 trajCoMX3_1[:, 0] = -trajCoMX2_1[range( (indX - 1 ), -1, -1), 0] + 2 * offsetxX #calcular a trajetória simétrica para x trajCoMX3_1[:, 1] = -trajCoMX2_1[range( (indX - 1 ), -1, -1), 1] + 2 * offsetyX #calcular a trajetória simétrica para y trajCoMX3_1[:, 2] = trajCoMX2_1[range((indX - 1), -1, -1), 2] #em z não muda trajCoMX3_1[:, 0] = trajCoMX3_1[:, 0] + i * 2 * passoTrajCoMX #Calculando as derivadas dt = glob.getHEDO() #n = np.size(trajCoMX3_1,0) dx = (trajCoMX3_1[indX - 1, 0] - trajCoMX3_1[indX - 2, 0]) / dt dy = (trajCoMX3_1[indX - 1, 1] - trajCoMX3_1[indX - 2, 1]) / dt #dz = (trajCoMX3_1[indX-1,2] - trajCoMX3_1[indX-2,2])/dt vx = np.array([dx, dy]) xnX = np.concatenate((trajCoMX3_1[indX - 1, :], vx), 0).reshape((5, 1)) return xnX
def TrajetoriaVariandoU(X0, U0, i, indice): #função para calcular a trajetória variando U0, para calcular os jacobianos glob = GlobalVariables() h = glob.getH() dt = glob.getHEDO() U0[indice, 0] = U0[indice, 0] + h #trajetoria na fase 1 [PAu, PBu, PCu, trajCoMU1, indContadoPeu] = trajetoria(U0, X0) #trajetoria na fase 2 indU = np.size(trajCoMU1, 0) trajCoMU2_1 = np.zeros((indU, 3)) trajCoMU2_2 = np.zeros((indU, 3)) #primeira metade ####################################### offsetxU = trajCoMU1[indU - 1, 0] #cálcular o offset em x offsetyU = trajCoMU1[indU - 1, 1] #calcular o offset em y trajCoMU2_1[:, 0] = -trajCoMU1[ range(indU - 1, -1, -1), 0] + 2 * offsetxU #calcular a trajetória simétrica para x trajCoMU2_1[:, 1] = -trajCoMU1[ range(indU - 1, -1, -1), 1] + 2 * offsetyU #calcular a trajetória simétrica para y trajCoMU2_1[:, 2] = trajCoMU1[range(indU - 1, -1, -1), 2] #em z não muda #segunda metade ############################################### offsetxU = trajCoMU2_1[indU - 1, 0] #cálcular o offset em x offsetyU = trajCoMU2_1[indU - 1, 1] #calcular o offset em y trajCoMU2_2[:, 0] = -trajCoMU2_1[ range(indU - 1, -1, -1), 0] + 2 * offsetxU #calcular a trajetória simétrica para x trajCoMU2_2[:, 1] = trajCoMU2_1[range(indU - 1, -1, -1), 1] #calcular a trajetória simétrica para y trajCoMU2_2[:, 2] = trajCoMU2_1[range(indU - 1, -1, -1), 2] #em z não muda #concatenando as duas: trajCoM2U = np.concatenate((trajCoMU2_1, trajCoMU2_2), axis=0) passoTrajCoMU = trajCoM2U[np.size(trajCoM2U, 0) - 1, 0] - trajCoM2U[0, 0] trajCoM2U[:, 0] = trajCoM2U[:, 0] + i * 2 * passoTrajCoMU #trajetoria na fase3 trajCoMU3_1 = np.zeros((indU, 3)) offsetxU = trajCoMU2_1[(indU - 1), 0] #cálcular o offset em x offsetyU = trajCoMU2_1[(indU - 1), 1] #calcular o offset em y #clr trajCoM3 trajCoMU3_1[:, 0] = -trajCoMU2_1[range( (indU - 1 ), -1, -1), 0] + 2 * offsetxU #calcular a trajetória simétrica para x trajCoMU3_1[:, 1] = -trajCoMU2_1[range( (indU - 1 ), -1, -1), 1] + 2 * offsetyU #calcular a trajetória simétrica para y trajCoMU3_1[:, 2] = trajCoMU2_1[range((indU - 1), -1, -1), 2] #em z não muda trajCoMU3_1[:, 0] = trajCoMU3_1[:, 0] + i * 2 * passoTrajCoMU #CALCULANDO as derivadas dx = (trajCoMU3_1[indU - 1, 0] - trajCoMU3_1[indU - 2, 0]) / dt dy = (trajCoMU3_1[indU - 1, 1] - trajCoMU3_1[indU - 2, 1]) / dt #dz = (trajCoMU3_1[indU-1,2] - trajCoMU3_1[indU-2,2])/dt vu = np.array([dx, dy]) xnU = np.concatenate((trajCoMU3_1[indU - 1, :], vu), 0).reshape((5, 1)) return xnU
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
#massa do corpo m = glob.getM() #tamanho da perna L = glob.getL() #gravidade g = glob.getG() #posição do pé de suporte em MS pfa = glob.getPfa() #----------------------------------------------------------- #Numero maximo de iterações para o metodo do gradiente #----------------------------------------------------------- #global maxNGrad, ganhoAlpha, gamma, h, hEdo maxNGrad = glob.getMaxNGrad() #número máximo de iterações método ganhoAlpha = glob.getGanhoAlpha() #ganho do fator de ganho para cada passo gamma = glob.getGamma() #ganho para os método gradiente(momento) h = glob.getH() #passo para o calculo das derivadas #calculado (comparado com a função do matlab) hEdo = glob.getHEDO() #passo para o calculo das derivadas #----------------------------------------------------------- #condição inicial para MS #----------------------------------------------------------- #hubo 2 + velocidade maxima 0.4 m/s # xod = 0.00 # yod = 0.05 # zod = 0.6 # dxod = 0.4 # dyod = 0.00 # dzod = 0.00 xod = 0.00 #x inicial (d - desejado)
def DerivadaJacobiano(theta, hOrg, hP, tipo, CoM, ind): #----------------------------------------------------------- #variaveis globais #---------------------------------------------------------- glob = GlobalVariables() h = glob.getH() #MDH = glob.getMDH() #global h, MDH thetar = np.zeros((6, 1)) thetal = np.zeros((6, 1)) #for j in range(6): thetar[:, 0] = theta[:, 0] thetal[:, 0] = theta[:, 1] #----------------------------------------------------------- #cálculo do primeiro ponto para 2h #---------------------------------------------------------- if tipo == 1: if CoM == 1: thetar[ind - 1, 0] = thetar[ind - 1, 0] + 2 * h else: thetal[ind - 1, 0] = thetal[ind - 1, 0] + 2 * h else: if CoM == 1: thetal[ind - 1, 0] = thetal[ind - 1, 0] + 2 * h else: thetar[ind - 1, 0] = thetar[ind - 1, 0] + 2 * h #calculando f(x+2h) theta1 = np.concatenate((thetar, thetal), axis=1) a = kinematicRobo(theta1, hOrg, hP, tipo, CoM) #----------------------------------------------------------- #cálculo do segundo ponto para h #---------------------------------------------------------- if tipo == 1: if CoM == 1: thetar[ind - 1, 0] = thetar[ind - 1, 0] + h else: thetal[ind - 1, 0] = thetal[ind - 1, 0] + h else: if CoM == 1: thetal[ind - 1, 0] = thetal[ind - 1, 0] + h else: thetar[ind - 1, 0] = thetar[ind - 1, 0] + h #calculando f(x+h) theta2 = np.concatenate((thetar, thetal), axis=1) b = kinematicRobo(theta2, hOrg, hP, tipo, CoM) #----------------------------------------------------------- #cálculo do terceiro ponto para -h #---------------------------------------------------------- if tipo == 1: if CoM == 1: thetar[ind - 1, 0] = thetar[ind - 1, 0] - h else: thetal[ind - 1, 0] = thetal[ind - 1, 0] - h else: if CoM == 1: thetal[ind - 1, 0] = thetal[ind - 1, 0] - h else: thetar[ind - 1, 0] = thetar[ind - 1, 0] - h #calculando f(x-h) theta3 = np.concatenate((thetar, thetal), axis=1) c = kinematicRobo(theta3, hOrg, hP, tipo, CoM) #----------------------------------------------------------- #cálculo do quarto ponto para -2h #---------------------------------------------------------- if tipo == 1: if CoM == 1: thetar[ind - 1, 0] = thetar[ind - 1, 0] - 2 * h else: thetal[ind - 1, 0] = thetal[ind - 1, 0] - 2 * h else: if CoM == 1: thetal[ind - 1, 0] = thetal[ind - 1, 0] - 2 * h else: thetar[ind - 1, 0] = thetar[ind - 1, 0] - 2 * h #calculando f(x-2h) theta2 theta4 = np.concatenate((thetar, thetal), axis=1) d = kinematicRobo(theta4, hOrg, hP, tipo, CoM) #----------------------------------------------------------- #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
#addpath('dif_Kinematic'); #----------------------------------------------------------- #variáveis globais #----------------------------------------------------------- import numpy as np from caminhada import caminhada from globalVariables import GlobalVariables from caminhada2 import caminhada2 import time begin = time.time() glob = GlobalVariables() m = glob.getM() L = glob.getL() g = glob.getG() h = glob.getH() hEdo = glob.getHEDO() #global m, L, g, h, hEdo U0 = np.zeros((5, 1)) #----------------------------------------------------------- #condição inicial para MS #----------------------------------------------------------- # xod = 0.0001 #x inicial # yod = 0.05 #y inicial # zod = 0.6 #z inicial # dxod = 0.7 #velocidade desejada no MS # dyod = 0.00 #condição de balanço # dzod = 0.00 #velocidade em z (igual a zero condição necessaria)