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)