def SGD(U0,X0):
#-----------------------------------------------------------
#variáveis globais
#-----------------------------------------------------------
glob = GlobalVariables()
ganhoAlpha = glob.getGanhoAlpha()
#global ganhoAlpha
#-----------------------------------------------------------
#cálculo do gradiente da função
#-----------------------------------------------------------
G = gradienteFuncao(X0,U0)
g1 = G[0,0]
g2 = G[1,0]
g3 = G[2,0]
g4 = G[3,0]
#-----------------------------------------------------------
#atualizando o valor do vetor de controle - U SGD
#-----------------------------------------------------------
u1 = U0[0,0] - ganhoAlpha*g1
u2 = U0[1,0] - ganhoAlpha*g2
u3 = U0[2,0] - ganhoAlpha*g3
u4 = U0[3,0] - ganhoAlpha*g4
u5 = U0[4,0]
#-----------------------------------------------------------
#Valor atualizado
#-----------------------------------------------------------
U = np.array([[u1],[u2],[u3],[u4],[u5]])
return U
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 adagrad(U0, X0, G0):
#-----------------------------------------------------------
#variáveis globais
#-----------------------------------------------------------
glob = GlobalVariables()
ganhoAlpha = glob.getGanhoAlpha()
#X0 = np.array((6,1))
#global ganhoAlpha
h = 10**(-8)
#-----------------------------------------------------------
#valor do ganho anterior
#-----------------------------------------------------------
G1 = G0[0, 0]
G2 = G0[1, 0]
G3 = G0[2, 0]
G4 = G0[3, 0]
#-----------------------------------------------------------
#cálculo do gradiente da função
#-----------------------------------------------------------
G = gradienteFuncao(X0, U0)
g1 = G[0, 0]
g2 = G[1, 0]
g3 = G[2, 0]
g4 = G[3, 0]
#-----------------------------------------------------------
#cálculo do novo ganho
#-----------------------------------------------------------
G1 = G1 + g1 * g1
G2 = G2 + g2 * g2
G3 = G3 + g3 * g3
G4 = G4 + g4 * g4
G0 = np.array([[G1], [G2], [G3], [G4]])
#-----------------------------------------------------------
#atualizando o valor do vetor de controle - U
#-----------------------------------------------------------
u1 = U0[0, 0] - (ganhoAlpha / (mt.sqrt(G1 + h))) * g1
u2 = U0[1, 0] - (ganhoAlpha / (mt.sqrt(G2 + h))) * g2
u3 = U0[2, 0] - (ganhoAlpha / (mt.sqrt(G3 + h))) * g3
u4 = U0[3, 0] - (ganhoAlpha / (mt.sqrt(G4 + h))) * g4
u5 = U0[4, 0]
#-----------------------------------------------------------
#Valor atualizado
#-----------------------------------------------------------
U = np.array([[u1], [u2], [u3], [u4], [u5]])
return U, G0
def NAG(U0, X0, vt):
#-----------------------------------------------------------
#variáveis globais
#-----------------------------------------------------------
glob = GlobalVariables()
ganhoAlpha = glob.getGanhoAlpha()
gamma = glob.getGamma()
#global ganhoAlpha, gamma
#-----------------------------------------------------------
#cálculando o valor com o ganho anterior
#-----------------------------------------------------------
y1 = U0[0, 0] - gamma * vt[0, 0]
y2 = U0[1, 0] - gamma * vt[1, 0]
y3 = U0[2, 0] - gamma * vt[2, 0]
y4 = U0[3, 0] - gamma * vt[3, 0]
y5 = U0[4, 0]
Y0 = np.array([[y1], [y2], [y3], [y4], [y5]])
#-----------------------------------------------------------
#cálculo do gradiente da função
#-----------------------------------------------------------
G = gradienteFuncao(X0, Y0)
g1 = G[0, 0]
g2 = G[1, 0]
g3 = G[2, 0]
g4 = G[3, 0]
#-----------------------------------------------------------
#cálculando o valor com o ganho atual
#-----------------------------------------------------------
vt[0, 0] = gamma * vt[0, 0] + ganhoAlpha * g1
vt[1, 0] = gamma * vt[1, 0] + ganhoAlpha * g2
vt[2, 0] = gamma * vt[2, 0] + ganhoAlpha * g3
vt[3, 0] = gamma * vt[3, 0] + ganhoAlpha * g4
u1 = U0[0, 0] - vt[0, 0]
u2 = U0[1, 0] - vt[1, 0]
u3 = U0[2, 0] - vt[2, 0]
u4 = U0[3, 0] - vt[3, 0]
u5 = U0[4, 0]
#-----------------------------------------------------------
#Valor atualizado
#-----------------------------------------------------------
U = np.array([[u1], [u2], [u3], [u4], [u5]])
return U, vt
def setLimites(U0):
#-----------------------------------------------------------
#variáveis globais
#----------------------------------------------------------
glob = GlobalVariables()
thetaM = glob.getThetaM()
phiM = glob.getPhiM()
KM = glob.getKM()
BSSM = glob.getBSSM()
#global thetaM, phiM, KM, BSSM
#-----------------------------------------------------------
#Separar as variaveis de controle
#----------------------------------------------------------
u1 = U0[0, 0]
u2 = U0[1, 0]
u3 = U0[2, 0]
u4 = U0[3, 0]
u5 = U0[4, 0]
#-----------------------------------------------------------
#tratar os dados da variável de controle - theta
#-----------------------------------------------------------
if u1 < 0:
u1 = 0
if u1 > thetaM:
u1 = thetaM
#-----------------------------------------------------------
#tratar os dados da variável de controle - phi
#-----------------------------------------------------------
if u2 < 0:
u2 = 0
if u2 > phiM:
u2 = phiM
#-----------------------------------------------------------
#tratar os dados da variável de controle - K
#-----------------------------------------------------------
if u3 < 0:
u3 = 0
if u3 > KM:
u3 = KM
#-----------------------------------------------------------
#tratar os dados da variável de controle - BSS
#-----------------------------------------------------------
if u4 < 0:
u4 = 0
if u4 > BSSM:
u4 = BSSM
#-----------------------------------------------------------
#retorno da função
#vetor corrigido
#-----------------------------------------------------------
U = np.array([[u1], [u2], [u3], [u4], [u5]])
return U
def SGDMomento(U0, X0, vt):
#-----------------------------------------------------------
#variáveis globais
#-----------------------------------------------------------
glob = GlobalVariables()
ganhoAlpha = glob.getGanhoAlpha()
gamma = glob.getGamma()
#global ganhoAlpha, gamma
#-----------------------------------------------------------
#cálculo do gradiente da função
#-----------------------------------------------------------
G = gradienteFuncao(X0, U0)
g1 = G[0, 0]
g2 = G[1, 0]
g3 = G[2, 0]
g4 = G[3, 0]
#-----------------------------------------------------------
#cálculo do momento da função
#-----------------------------------------------------------
vt = np.zeros((4, 1))
vt[0, 0] = gamma * vt[0, 0] + ganhoAlpha * g1
vt[1, 0] = gamma * vt[1, 0] + ganhoAlpha * g2
vt[2, 0] = gamma * vt[2, 0] + ganhoAlpha * g3
vt[3, 0] = gamma * vt[3, 0] + ganhoAlpha * g4
#-----------------------------------------------------------
#atualizando o valor do vetor de controle - U
#-----------------------------------------------------------
u1 = U0[0, 0] - vt[0, 0]
u2 = U0[1, 0] - vt[1, 0]
u3 = U0[2, 0] - vt[2, 0]
u4 = U0[3, 0] - vt[3, 0]
u5 = U0[4, 0]
#-----------------------------------------------------------
#Valor atualizado
#-----------------------------------------------------------
U = np.array([[u1], [u2], [u3], [u4], [u5]])
return U, vt
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 aqui #-------------------------------------------------------------------------------------- #parâmetros necessarios glob = GlobalVariables() #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 #primeira parte da caminhdada phase = 1 passos = 1 trajCoM = trajCoM1 trajP = trajPB1 T = np.size(trajCoM, 0) #o tamanho de trajCoM = ind [ha, ha2, hP, Mhd, Mhd2, Mdhd, Mdhd2, tempo1] = calculoMhd(trajCoM, theta, trajP, phase) [ha, ha2, theta, Mtheta,
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 calculoMhd(trajCoM, theta, trajP, phase):
glob = GlobalVariables()
hEdo = glob.getHEDO()
L1 = glob.getL1()
height = glob.getHeight()
#MDH = glob.getMDH()
hpi = glob.getHpi()
ind = np.size(trajP, 0)
hOrg = np.array([[1], [0], [0], [0], [0], [0], [0], [0]]) #posição da base
dt = hEdo
#cacular posição atual do´pé
n = np.array([0, 1, 0]) # n é o vetor diretor do quaternio
thetab = hpi #parametro da função de caminhada que é igual a pi/2
realRb = np.array([np.cos(thetab / 2)])
rb = np.concatenate((realRb, np.sin(thetab / 2) * n), axis=0).reshape(
(4, 1))
pb = np.array([[0], [0], [-L1], [-height]])
#base B para a base O6 (B é a perna em movimento)
hB_O6 = transformacao(pb, rb)
hP = dualQuatMult(hOrg, hB_O6)
#dt é o tempo da solução da equação Edo e, ao mesmo tempo, o passo
T = np.size(trajCoM, 0) #o tamanho de trajCoM = ind
tempo = (
T - 1
) * dt #o tempo é o produto da quantidade de iterações necessárias para calcular a trajetória do CoM
#matrizes e vetores auxiliares
Mhd = np.zeros((8, T))
Mha = np.zeros((8, T))
Mdhd = np.zeros((8, T))
Mtheta = np.zeros((6, T))
Mdhd = np.zeros((8, T))
Mhd2 = np.zeros((8, T))
Mdhd2 = np.zeros((8, T))
# Mhd2 = np.zeros((8,T))
# Mha2 = np.zeros((8,T))
# Mdhd2= np.zeros((8,T))
# Mtheta2 = np.zeros((6,T))
# angle = np.zeros(T)
# angled = np.zeros(T)
# angle2 = np.zeros(T)
# angled2 = np.zeros(T)
# Pos = np.zeros((3,T))
# Posd = np.zeros((3,T))
# Pos2 = np.zeros((3,T))
# Posd2 = np.zeros((3,T))
#calculo de Mhd - matriz de hd
r = np.array([1, 0, 0, 0]).reshape((4, 1))
p = np.array([0, 0, -L1, -height]).reshape(
(4, 1)) #height = L2 + L3 + L4 + L5
hB1 = transformacao(p, r) #transformação base robô
for i in range(0, T, 1):
p = np.array([0, trajCoM[i, 0], trajCoM[i, 1], trajCoM[i, 2]]).reshape(
(4, 1))
r = np.array([1, 0, 0, 0]).reshape((4, 1))
hd = transformacao(p, r)
hd = dualQuatMult(hB1, hd)
Mhd[:, i] = hd[:, 0]
#transformação da base até o centro de massa
#se i<ind, o robô ainda não atingiu a posição de td, então a transformação é calculada em relação ao pé
#quando o robô chega na fase de TD, a transformação é calculada em relação ao CoM
if i < ind:
p = np.array([0, trajP[i, 0], trajP[i, 1], trajP[i, 2]]).reshape(
(4, 1))
n = np.array([0, 1, 0])
#angulo = mt.pi/2.0 #graus ou radianos????????????????????????????????????????????????????????????/
realRb = np.array([np.cos(thetab / 2)])
rb = np.concatenate((realRb, np.sin(thetab / 2) * n),
axis=0).reshape((4, 1))
hd = transformacao(p, rb) #posição desejada
hd = dualQuatMult(hB1, hd) #transformação da base até o pé
Mhd2[:, i] = hd[:, 0]
else:
Mhd2[:, i] = Mhd2[:, ind - 1]
#Mdhd[:,0] = (Mhd[:,0] - Mhd[:,0])*(1/dt) #não deveria ser i-1, no segundo Mhd???????????????????????????????????
#Mdhd2[:,0] = (Mhd2[:,0] - Mhd2[:,0])*(1/dt)
for i in range(1, T, 1):
Mdhd[:, i] = (Mhd[:, i] - Mhd[:, i - 1]) * (
1 / dt
) #por que ele fazer isso????????????????????????????????????????????????????
Mdhd2[:, i] = (Mhd2[:, i] - Mhd2[:, i - 1]) * (
1 / dt) #derivada de hd, que é a posição desejada
if phase == 1:
ha = kinematicRobo(theta, hOrg, hP, 1,
1) #cinemática do pé esquerdo até o CoM
ha2 = kinematicRobo(theta, hOrg, hP, 1,
0) #cinemática de um pé até o outro
else:
ha = ha2 = np.zeros((8, 1))
return ha, ha2, hP, Mhd, Mhd2, Mdhd, Mdhd2, tempo
def kinematicRobo(theta, hOrg, hP, tipo, CoM):
#--------------------------------------
#Método para calcular o a cinemática direta
#do robô utilizando quatérnio dual
#height é a altura do robô
#L1 é o tamanho do link da bacia
#L2 é a altura da bacia até o primeiro motor
#--------------------------------------
#variaveis globais-----------------------------------------------------------------------
glob = GlobalVariables()
#global hpi, L1, L2, height, MDH #se eu chamar essas variáveis em outro lugar do código, ele vai pegar esses valores?
hpi = glob.getHpi()
L1 = glob.getL1()
L2 = glob.getL2()
#height = glob.getHeight()
MDH = glob.getMDH()
#------------------------------------------------------------------------------------------------
# l = np.size(theta[:,0],0)
# r = np.size(theta[:,1],0)
# thetar = np.zeros((l,1))
# thetal = np.zeros((r,1))
thetar = theta[:, 0].reshape((6, 1))
thetal = theta[:, 1].reshape((6, 1))
# for i in range(r):
# thetar[i,0] = theta[i,0]
# for i in range(l):
# thetal[i,0] = theta[i,1]
# a3 = 6
# a4 = a3
# a5 = 2
#MDH = np.array([[0, 0, 0, np.pi/2], # do fixo pro 0 (sobre o motthetar1)
# [-np.pi/2, 0, 0, -np.pi/2], # motthetar1 do frame 0 pro 1(sobre o motthetar2)
# [0, 0, a3, 0], # motthetar2 do frame 1 pro 2(sobre o motthetar3)
# [0, 0, a4, 0], # motthetar3 do frame 2 pro 3(sobre o motthetar4)
# [0, 0, 0, np.pi/2], # motthetar4 do frame 3 pro 4(sobre o motthetar5)
# [0, 0, a5, 0]]) # motthetar5 do frame 4 pro 5(sobre o motthetar6)
#transformações da origem para a origem
#da configuração inicial das 2 pernas
hCoM_O0_rightLeg = dualQuatDH(
hpi, -L2, -L1, 0, 0
) #transformação do sist de coordenadas do centro de massa para a origem 0 da perna direita
hCoM_O0_leftLeg = dualQuatDH(
hpi, -L2, L1, 0, 0
) #transformação do sist de coordenadas do centro de massa para a origem 0 da perna esquerda
hB_O6a = dualQuatMult(
hOrg, hP
) #transformação para auxiliar na localização do pé em contato com o chão
if tipo == 1: #tipo = 1 significa que a perna direita está apoiada
hO6_O0 = KinematicModel(
MDH, thetar, 1, 0
) #transformação do sistema de coordenadas do link O6 par ao link O0 (do início da perna para o pé)
else:
hO6_O0 = KinematicModel(MDH, thetal, 1, 0)
hB_O0 = dualQuatMult(
hB_O6a, hO6_O0
) #representa a base ou sistema global (hOrg + hp), ou seja, do sistema base para o sistema O0
#transformação da base global para a base O0, depois de O0 para o CoM
if tipo == 1:
hB_CoM = dualQuatMult(hB_O0, dualQuatConj(hCoM_O0_rightLeg))
else:
hB_CoM = dualQuatMult(hB_O0, dualQuatConj(hCoM_O0_leftLeg))
hr = hB_CoM #a função retornará a orientação do CoM (em relação à base global)
if CoM == 0:
#transformação da base O0 para O6
if tipo == 1:
hB_O0 = dualQuatMult(
hB_CoM, hCoM_O0_leftLeg
) #multiplica hB_CoM por hCoM_O0_leftLeg, para eliminar hCoM_O0_leftLeg conjugado
hO0_O6 = KinematicModel(MDH, thetal, 6, 1)
hr = dualQuatMult(
hB_O0,
hO0_O6) #posição do pé suspenso (nesse caso, o esquerdo)
else:
hB_O0 = dualQuatMult(hB_CoM, hCoM_O0_rightLeg)
hO0_O6 = KinematicModel(MDH, thetar, 6, 1)
hr = dualQuatMult(hB_O0, hO0_O6)
return hr
# from matplotlib import pyplot as mp # import numpy as np # def gaussian(x, mu, sig): # return np.exp(-np.power(x - mu, 2.) / (2 * np.power(sig, 2.))) # x_values = np.linspace(75, 200,120) # for mu, sig in [(124, 11.97)]: # mp.plot(x_values, gaussian(x_values, mu, sig)) # mp.show() from globalVariables import GlobalVariables from kinematicRobo import kinematicRobo from jacobianoPes import jacobianoPes import numpy as np glob = GlobalVariables() theta = glob.getTheta() hOrg = np.array([1, 0, 0, 0, 0, 0, 0, 0]).reshape((8,1)) hP = hOrg xe2 = kinematicRobo(theta,hOrg,hP,1,0) Ja2 = jacobianoPes(theta,ha,xe2,0)
def controles(theta, ha, ha2, hP, Mhd2, Mdhd2, Mhd, Mdhd, vecGanho, T, phase,
publishers):
if phase == 2:
hP = ha2
hOrg = np.array([[1], [0], [0], [0], [0], [0], [0],
[0]]) #posição da base
ha2 = kinematicRobo(theta, hOrg, hP, 0, 0) #posição da perna direita
else:
if phase == 3:
hP = ha2
glob = GlobalVariables()
dt = glob.getHEDO()
MDH = glob.getMDH()
angleMsg = Float64()
mhd = np.zeros((8, 1))
mdhd = np.zeros((8, 1))
mhd2 = np.zeros((8, 1))
mdhd2 = np.zeros((8, 1))
angle = np.zeros(T)
angled = np.zeros(T)
angle2 = np.zeros(T)
angled2 = np.zeros(T)
Pos = np.zeros((3, T))
Posd = np.zeros((3, T))
Pos2 = np.zeros((3, T))
Posd2 = np.zeros((3, T))
Mha2 = np.zeros((8, T))
Mha = np.zeros((8, T))
Mtheta2 = np.zeros((6, T))
Mtheta = np.zeros((6, T))
#LQR
#calculo dos parâmetros
ganhoS = vecGanho[0, 0]
ganhoQ = vecGanho[1, 0]
ganhoR = vecGanho[2, 0]
#controlador proporcional
ganhoK2 = vecGanho[3, 0]
K2 = ganhoK2 * np.eye(8)
S = ganhoS * np.eye(8)
Q = ganhoQ * np.eye(8)
R = ganhoR * np.eye(8)
Rinv = np.linalg.inv(R)
# print('R::',Rinv)
# return
ab = np.array([1, -1, -1, -1, 1, -1, -1, -1])
C8 = np.diag(ab)
hOrg = np.array([[1], [0], [0], [0], [0], [0], [0], [0]]) #posição da base
#iniciar condições finais esperadas para P e E
Pf = S
Ef = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]).reshape((8, 1))
P = Pf
MP2 = np.zeros((8, 8, T))
for j in range(8):
for k in range(8):
MP2[j, k, T - 1] = P[j, k]
E = Ef
ME2 = np.zeros((8, T))
for j in range(8):
ME2[j, T - 1] = E[j, 0]
#calcular matrizes de ganho
for i in range(T - 2, -1, -1):
for j in range(8):
mhd[j, 0] = Mhd[j, i + 1]
mdhd[j, 0] = Mdhd[j, i + 1]
aux = dualQuatMult(dualQuatConj(mhd), mdhd)
A = dualHamiltonOp(aux, 0)
c = -aux
# prod2 = np.dot(P,Rinv)
P = P - (-P @ A - A.T @ P + P @ Rinv @ P - Q) * dt
for j in range(8):
for k in range(8):
MP2[j, k, i] = P[j, k]
E = E - ((-1) * (A.T) @ E + P @ Rinv @ E - P @ c) * dt
for j in range(8):
ME2[j, i] = E[j, 0]
for i in range(0, T, 1):
#tic = tm.time()
#Controlador LQR para o CoM
#calculo de A e c
for j in range(8):
mhd[j, 0] = Mhd[j, i]
mdhd[j, 0] = Mdhd[j, i]
aux = dualQuatMult(dualQuatConj(mhd),
mdhd) #calculo de hd conjugado * hd derivada
A = dualHamiltonOp(aux, 0)
c = -aux
#inicio do controlador
#hB_O6a = dualQuatMult(hOrg,hP)
xe = KinematicModel(MDH, theta, 6, 0)
if (phase == 1 | phase == 3):
Ja = jacobiano2(theta, hOrg, hP, xe,
0) #jacobiano para a perna direita
else:
Ja = jacobiano2(theta, hOrg, hP, xe, 1)
# Ja = jacobianoCinematica(theta,hOrg,hP,1,1)
#calculo de P e E
#calculo de N
Hd = dualHamiltonOp(mhd, 0)
# prod3 = np.dot(Hd,C8)
N = Hd @ C8 @ Ja
#pseudo inversa de N
Np = np.linalg.pinv(N)
#calculo do erro
e = np.array([1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]).reshape(
(8, 1)) - dualQuatMult(dualQuatConj(ha), mhd)
#calculo de P e E
E[:, 0] = ME2[:, i]
P[:, :] = MP2[:, :, i].reshape((8, 8))
#Pxe= np.dot(P,e)
#do = Np@Rinv@(P@e + E)
do = Np @ Rinv @ (P @ e + E) #equação final para theta ponto
#calculo do theta deseja
od = (do * dt) / 2
#print('od', od)
if (phase == 1 or phase == 3):
theta[:, 0] = theta[:, 0] + od[:, 0]
else:
theta[:, 1] = theta[:, 1] + od[:, 0]
#o movimento dos motores é limitado entre pi/2 e -pi/2, então, se theta estiver
#fora do intervalo, esse for faz theta = limite do intervalo
# for j in range(0,6,1):
# if abs(theta[j,0]) > hpi:
# theta[j,0] = np.sign(theta[j,0])*hpi
if (phase == 1 or phase == 3):
ha = kinematicRobo(theta, hOrg, hP, 1,
1) #posição do CoM com perna direita apoiada
else:
ha = kinematicRobo(theta, hOrg, hP, 0,
1) #posição do CoM com perna esquerda apoiada
#plotar os dados
for j in range(8):
Mha[j, i] = ha[j, 0]
#posição
PosAux = getPositionDualQuat(ha)
PosdAux = getPositionDualQuat(mhd)
for j in range(3):
Pos[j, i] = PosAux[j, 0] #retorna a posição x,y e z de ha
Posd[j, i] = PosdAux[j,
0] #retorna a posição x,y e z de todos os hd
#orientação
ra = getRotationDualQuat(
ha) ##extrai o vetor do dual quat que representa a rotação
rd = getRotationDualQuat(mhd)
co = mt.acos(ra[0, 0])
angle[i] = co
co = mt.acos(rd[0, 0])
angled[i] = co
for j in range(6):
Mtheta[j, i] = theta[j, 0]
#controlador 2 para os pés (proporcional com feed forward)
#calculo de A e c
#é necessário??????????????????????????????????????????????????????????????????
#aux2 = dualQuatMult(dualQuatConj(Mhd2(:,i)),Mdhd2(:,i));
#A2 = dualHamiltonOp(aux2,0);
#c = -aux2;
#inicio do controlador
#hB_O6a = dualQuatMult(hOrg,hP)
xe2 = kinematicRobo(theta, hOrg, hP, 1, 0)
Ja2 = jacobianoPes(theta, ha, xe2, 1)
#Ja2 = jacobianoCinematica(theta,hOrg,hP,1,0)
#calculo de P e E
#calculo de N
mhd2 = np.zeros((8, 1))
mdhd2 = np.zeros((8, 1))
for j in range(8):
mhd2[j, 0] = Mhd2[j, i]
mdhd2[j, 0] = Mdhd2[j, i]
Hd2 = dualHamiltonOp(mhd2, 0)
N2 = Hd2 @ C8 @ Ja2
#pseudo inversa de N
Np2 = np.linalg.pinv(N2)
#calculo do erro
e2 = np.array([1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]).reshape(
(8, 1)) - dualQuatMult(dualQuatConj(ha2), mhd2)
vec2 = dualQuatMult(dualQuatConj(ha2), mdhd2)
#
# K2xe2 = np.dot(K2,e2)
do2 = Np2 @ (K2 @ e2 - vec2)
od2 = (do2 * dt) / 2
if (phase == 1 or phase == 3):
theta[:, 1] = theta[:, 1] + od2[:, 0]
else:
theta[:, 0] = theta[:, 0] + od2[:, 0]
# for j in range (0,6,1):
# if abs(theta[j,1]) > hpi:
# theta[j,1] = np.sign(theta[j,1])*hpi
print(theta)
# publicando os ângulos nas juntas
angleMsg.data = theta[0, 0] #escrevendo theta[0] em angle
publishers[0].publish(angleMsg) #publicando angle
angleMsg.data = theta[1, 0]
publishers[1].publish(angleMsg)
angleMsg.data = theta[2, 0]
publishers[2].publish(angleMsg)
angleMsg.data = theta[3, 0]
publishers[3].publish(angleMsg)
angleMsg.data = theta[4, 0]
publishers[4].publish(angleMsg)
angleMsg.data = theta[5, 0]
publishers[5].publish(angleMsg)
angleMsg.data = theta[0, 1]
publishers[6].publish(angleMsg)
angleMsg.data = theta[1, 1]
publishers[7].publish(angleMsg)
angleMsg.data = theta[2, 1]
publishers[8].publish(angleMsg)
angleMsg.data = theta[3, 1]
publishers[9].publish(angleMsg)
angleMsg.data = theta[4, 1]
publishers[10].publish(angleMsg)
angleMsg.data = theta[5, 1]
publishers[11].publish(angleMsg)
rospy.sleep(1)
if (phase == 1 or phase == 3):
ha2 = kinematicRobo(theta, hOrg, hP, 1, 0)
else:
ha2 = kinematicRobo(theta, hOrg, hP, 0,
0) #posição da perna direita
#plotar os dados
Mha2[:, i] = ha2[:, 0]
#posição
Pos2Aux = getPositionDualQuat(ha2)
Posd2Aux = getPositionDualQuat(mhd2)
#for j in range(3):
Pos2[:, i] = Pos2Aux[:, 0]
Posd2[:, i] = Posd2Aux[:, 0]
#orientação
ra = getRotationDualQuat(ha2)
rd = getRotationDualQuat(mhd2)
co = mt.acos(ra[0, 0])
angle2[i] = co
co = mt.acos(rd[0, 0])
angled2[i] = co
for j in range(6):
Mtheta2[j, i] = theta[j, 1]
#mostrar no console o andamento do metódo
#toc = tm.time() - t #elapsed
#msg = print('#d de #d | tempo (s): #f',i,T,toc);
#disp(msg);
t1 = 0
#plotGraficosControle(t1,dt,T,Pos,Posd,angle,angled,Mha,Mhd,Mtheta,Pos2,Posd2,angle2,angled2,Mha2,Mhd2,Mtheta2,'b','r')
return ha, ha2, theta, Mtheta, Mtheta2
def jacobianoPes(theta,ha,xe,leg):
#-----------------------------------------------------------
#c�lculo das derivadas para cada vari�vel de controle
#----------------------------------------------------------
glob = GlobalVariables()
MDH = glob.getMDH()
hpi = glob.getHpi()
L1 = glob.getL1()
L2 = glob.getL2()
thetal = theta[:,1].reshape((6,1))
thetar = theta[:,0].reshape((6,1))
z = np.zeros((8,1))
#transforma��es da origem para a origem
#da configura��o inicial das 2 pernas
hCoM_O0_rightLeg = dualQuatDH(hpi,-L2,-L1,0,0) #transformação do sist de coordenadas do centro de massa para a origem 0 da perna direita
hCoM_O0_leftLeg = dualQuatDH(hpi,-L2, L1,0,0) #transformação do sist de coord. do CoM para a origem 0 da perna esquerda
if leg == 1: #left leg
hB_O6a = dualQuatMult(ha,hCoM_O0_leftLeg)
h = hB_O6a
#h = [1 0 0 0 0 0 0 0]'
z[0,0] = 0
z[1,0] = h[1,0]*h[3,0] + h[0,0]*h[2,0]
z[2,0] = h[2,0]*h[3,0] - h[0,0]*h[1,0]
z[3,0] = (h[3,0]**2 - h[2,0]**2 - h[1,0]**2 + h[0,0]**2)/2
z[4,0] = 0
z[5,0] = h[1,0]*h[7,0] + h[5,0]*h[3,0] + h[0,0]*h[6,0] + h[4,0]*h[2,0]
z[6,0] = h[2,0]*h[7,0] + h[6,0]*h[3,0] - h[0,0]*h[5,0] - h[4,0]*h[1,0]
z[7,0] = h[3,0]*h[7,0] - h[2,0]*h[6,0] - h[1,0]*h[5,0] + h[0,0]*h[4,0]
j0 = dualQuatMult(z,xe)
##################################j1##################################
h = dualQuatMult(hB_O6a,KinematicModel(MDH,thetal,1,1))
#h = [1 0 0 0 0 0 0 0]'
z[0,0] = 0
z[1,0] = h[1,0]*h[3,0] + h[0,0]*h[2,0]
z[2,0] = h[2,0]*h[3,0] - h[0,0]*h[1,0]
z[3,0] = (h[3,0]**2 - h[2,0]**2 - h[1,0]**2 + h[0,0]**2)/2
z[4,0] = 0
z[5,0] = h[1,0]*h[7,0] + h[5,0]*h[3,0] + h[0,0]*h[6,0] + h[4,0]*h[2,0]
z[6,0] = h[2,0]*h[7,0] + h[6,0]*h[3,0] - h[0,0]*h[5,0] - h[4,0]*h[1,0]
z[7,0] = h[3,0]*h[7,0] - h[2,0]*h[6,0] - h[1,0]*h[5,0] + h[0,0]*h[4,0]
j1 = dualQuatMult(z,xe)
##################################j2##################################
h = dualQuatMult(hB_O6a,KinematicModel(MDH,thetal,2,1))
z[0,0] = 0
z[1,0] = h[1,0]*h[3,0] + h[0,0]*h[2,0]
z[2,0] = h[2,0]*h[3,0] - h[0,0]*h[1,0]
z[3,0] = (h[3,0]**2 - h[2,0]**2 - h[1,0]**2 + h[0,0]**2)/2
z[4,0] = 0
z[5,0] = h[1,0]*h[7,0] + h[5,0]*h[3,0] + h[0,0]*h[6,0] + h[4,0]*h[2,0]
z[6,0] = h[2,0]*h[7,0] + h[6,0]*h[3,0] - h[0,0]*h[5,0] - h[4,0]*h[1,0]
z[7,0] = h[3,0]*h[7,0] - h[2,0]*h[6,0] - h[1,0]*h[5,0] + h[0,0]*h[4,0]
j2 = dualQuatMult(z,xe)
##################################j3##################################
h = dualQuatMult(hB_O6a,KinematicModel(MDH,thetal,3,1))
z[0,0] = 0
z[1,0] = h[1,0]*h[3,0] + h[0,0]*h[2,0]
z[2,0] = h[2,0]*h[3,0] - h[0,0]*h[1,0]
z[3,0] = (h[3,0]**2 - h[2,0]**2 - h[1,0]**2 + h[0,0]**2)/2
z[4,0] = 0
z[5,0] = h[1,0]*h[7,0] + h[5,0]*h[3,0] + h[0,0]*h[6,0] + h[4,0]*h[2,0]
z[6,0] = h[2,0]*h[7,0] + h[6,0]*h[3,0] - h[0,0]*h[5,0] - h[4,0]*h[1,0]
z[7,0] = h[3,0]*h[7,0] - h[2,0]*h[6,0] - h[1,0]*h[5,0] + h[0,0]*h[4,0]
j3 = dualQuatMult(z,xe)
##################################j4##################################
h = dualQuatMult(hB_O6a,KinematicModel(MDH,thetal,4,1))
z[0,0] = 0
z[1,0] = h[1,0]*h[3,0] + h[0,0]*h[2,0]
z[2,0] = h[2,0]*h[3,0] - h[0,0]*h[1,0]
z[3,0] = (h[3,0]**2 - h[2,0]**2 - h[1,0]**2 + h[0,0]**2)/2
z[4,0] = 0
z[5,0] = h[1,0]*h[7,0] + h[5,0]*h[3,0] + h[0,0]*h[6,0] + h[4,0]*h[2,0]
z[6,0] = h[2,0]*h[7,0] + h[6,0]*h[3,0] - h[0,0]*h[5,0] - h[4,0]*h[1,0]
z[7,0] = h[3,0]*h[7,0] - h[2,0]*h[6,0] - h[1,0]*h[5,0] + h[0,0]*h[4,0]
j4 = dualQuatMult(z,xe)
##################################j5##################################
h = dualQuatMult(hB_O6a,KinematicModel(MDH,thetal,5,1))
z[0,0] = 0
z[1,0] = h[1,0]*h[3,0] + h[0,0]*h[2,0]
z[2,0] = h[2,0]*h[3,0] - h[0,0]*h[1,0]
z[3,0] = (h[3,0]**2 - h[2,0]**2 - h[1,0]**2 + h[0,0]**2)/2
z[4,0] = 0
z[5,0] = h[1,0]*h[7,0] + h[5,0]*h[3,0] + h[0,0]*h[6,0] + h[4,0]*h[2,0]
z[6,0] = h[2,0]*h[7,0] + h[6,0]*h[3,0] - h[0,0]*h[5,0] - h[4,0]*h[1,0]
z[7,0] = h[3,0]*h[7,0] - h[2,0]*h[6,0] - h[1,0]*h[5,0] + h[0,0]*h[4,0]
j5 = dualQuatMult(z,xe)
else:
hB_O6a = dualQuatMult(ha,hCoM_O0_rightLeg)
h = hB_O6a
#h = [1 0 0 0 0 0 0 0]'
z[0,0] = 0
z[1,0] = h[1,0]*h[3,0] + h[0,0]*h[2,0]
z[2,0] = h[2,0]*h[3,0] - h[0,0]*h[1,0]
z[3,0] = (h[3,0]**2 - h[2,0]**2 - h[1,0]**2 + h[0,0]**2)/2
z[4,0] = 0
z[5,0] = h[1,0]*h[7,0] + h[5,0]*h[3,0] + h[0,0]*h[6,0] + h[4,0]*h[2,0]
z[6,0] = h[2,0]*h[7,0] + h[6,0]*h[3,0] - h[0,0]*h[5,0] - h[4,0]*h[1,0]
z[7,0] = h[3,0]*h[7,0] - h[2,0]*h[6,0] - h[1,0]*h[5,0] + h[0,0]*h[4,0]
j0 = dualQuatMult(z,xe)
##################################j1##################################
h = dualQuatMult(hB_O6a,KinematicModel(MDH,thetar,1,1))
#h = [1 0 0 0 0 0 0 0]'
z[0,0] = 0
z[1,0] = h[1,0]*h[3,0] + h[0,0]*h[2,0]
z[2,0] = h[2,0]*h[3,0] - h[0,0]*h[1,0]
z[3,0] = (h[3,0]**2 - h[2,0]**2 - h[1,0]**2 + h[0,0]**2)/2
z[4,0] = 0
z[5,0] = h[1,0]*h[7,0] + h[5,0]*h[3,0] + h[0,0]*h[6,0] + h[4,0]*h[2,0]
z[6,0] = h[2,0]*h[7,0] + h[6,0]*h[3,0] - h[0,0]*h[5,0] - h[4,0]*h[1,0]
z[7,0] = h[3,0]*h[7,0] - h[2,0]*h[6,0] - h[1,0]*h[5,0] + h[0,0]*h[4,0]
j1 = dualQuatMult(z,xe)
##################################j2##################################
h = dualQuatMult(hB_O6a,KinematicModel(MDH,thetar,2,1))
z[0,0] = 0
z[1,0] = h[1,0]*h[3,0] + h[0,0]*h[2,0]
z[2,0] = h[2,0]*h[3,0] - h[0,0]*h[1,0]
z[3,0] = (h[3,0]**2 - h[2,0]**2 - h[1,0]**2 + h[0,0]**2)/2
z[4,0] = 0
z[5,0] = h[1,0]*h[7,0] + h[5,0]*h[3,0] + h[0,0]*h[6,0] + h[4,0]*h[2,0]
z[6,0] = h[2,0]*h[7,0] + h[6,0]*h[3,0] - h[0,0]*h[5,0] - h[4,0]*h[1,0]
z[7,0] = h[3,0]*h[7,0] - h[2,0]*h[6,0] - h[1,0]*h[5,0] + h[0,0]*h[4,0]
j2 = dualQuatMult(z,xe)
##################################j3##################################
h = dualQuatMult(hB_O6a,KinematicModel(MDH,thetar,3,1))
z[0,0] = 0
z[1,0] = h[1,0]*h[3,0] + h[0,0]*h[2,0]
z[2,0] = h[2,0]*h[3,0] - h[0,0]*h[1,0]
z[3,0] = (h[3,0]**2 - h[2,0]**2 - h[1,0]**2 + h[0,0]**2)/2
z[4,0] = 0
z[5,0] = h[1,0]*h[7,0] + h[5,0]*h[3,0] + h[0,0]*h[6,0] + h[4,0]*h[2,0]
z[6,0] = h[2,0]*h[7,0] + h[6,0]*h[3,0] - h[0,0]*h[5,0] - h[4,0]*h[1,0]
z[7,0] = h[3,0]*h[7,0] - h[2,0]*h[6,0] - h[1,0]*h[5,0] + h[0,0]*h[4,0]
j3 = dualQuatMult(z,xe)
##################################j4##################################
h = dualQuatMult(hB_O6a,KinematicModel(MDH,thetar,4,1))
z[0,0] = 0
z[1,0] = h[1,0]*h[3,0] + h[0,0]*h[2,0]
z[2,0] = h[2,0]*h[3,0] - h[0,0]*h[1,0]
z[3,0] = (h[3,0]**2 - h[2,0]**2 - h[1,0]**2 + h[0,0]**2)/2
z[4,0] = 0
z[5,0] = h[1,0]*h[7,0] + h[5,0]*h[3,0] + h[0,0]*h[6,0] + h[4,0]*h[2,0]
z[6,0] = h[2,0]*h[7,0] + h[6,0]*h[3,0] - h[0,0]*h[5,0] - h[4,0]*h[1,0]
z[7,0] = h[3,0]*h[7,0] - h[2,0]*h[6,0] - h[1,0]*h[5,0] + h[0,0]*h[4,0]
j4 = dualQuatMult(z,xe)
##################################j5##################################
h = dualQuatMult(hB_O6a,KinematicModel(MDH,thetar,5,1))
z[0,0] = 0
z[1,0] = h[1,0]*h[3,0] + h[0,0]*h[2,0]
z[2,0] = h[2,0]*h[3,0] - h[0,0]*h[1,0]
z[3,0] = (h[3,0]**2 - h[2,0]**2 - h[1,0]**2 + h[0,0]**2)/2
z[4,0] = 0
z[5,0] = h[1,0]*h[7,0] + h[5,0]*h[3,0] + h[0,0]*h[6,0] + h[4,0]*h[2,0]
z[6,0] = h[2,0]*h[7,0] + h[6,0]*h[3,0] - h[0,0]*h[5,0] - h[4,0]*h[1,0]
z[7,0] = h[3,0]*h[7,0] - h[2,0]*h[6,0] - h[1,0]*h[5,0] + h[0,0]*h[4,0]
j5 = dualQuatMult(z,xe)
jac = np.concatenate((j0, j1, j2, j3, j4, j5),axis = 1)
return jac
def fase3(ha, ha2, trajCoM, ind, trajPB, theta, t1, vecGanho):
# #global hpi, L1, L2, L3, L4, L5, height, MDH, hEdo
# glob = GlobalVariables()
# hEdo = glob.getHEDO()
# L1 = glob.getL1()
# #L2 = glob.getL2()
# #L3 = glob.getL3()
# #L4 = glob.getL4()
# #L5 = glob.getL5()
# height = glob.getHeight()
# MDH = glob.getMDH()
# hpi = glob.getHpi()
# dt = hEdo #dt é o tempo da solução da equação Edo
# hOrg = np.array([1, 0, 0, 0, 0, 0, 0, 0]).reshape((8,1)) #posição da base
# T = np.size(trajCoM,0)
# #T = 500;
# #t = 1:1:T;
# tempo = (T-1)*dt
# #tempo = (T)*dt
# #matrizes auxiliares
# Mhd = np.zeros((8,T))
# Mha = np.zeros((8,T))
# Mdhd = np.zeros((8,T))
# Mtheta = np.zeros((6,T))
# mhd = np.zeros((8,1))
# mdhd = np.zeros((8,1))
# mhd2 = np.zeros((8,1))
# mdhd2 = np.zeros((8,1))
# mhdPlus = np.zeros((8,1))
# mdhdPlus = np.zeros((8,1))
# Mhd2 = np.zeros((8,T))
# Mha2 = np.zeros((8,T))
# Mdhd2= np.zeros((8,T))
# Mtheta2 = np.zeros((6,T))
# angle = np.zeros(T)
# angled = np.zeros(T)
# angle2 = np.zeros(T)
# angled2 = np.zeros(T)
# Pos = np.zeros((3,T))
# Posd = np.zeros((3,T))
# Pos2 = np.zeros((3,T))
# Posd2 = np.zeros((3,T))
# #calculo de Mhd - matriz de hd
# r = np.array([1, 0, 0, 0]).reshape(4,1)
# #p = [0 0 0 0]';
# p = np.array([0, 0, -L1, -height]).reshape((4,1))
# hB1 = transformacao(p,r) #transformação base robô
# for i in range(0,T,1):
# p = np.array([0, trajCoM[i,0], trajCoM[i,1], trajCoM[i,2]]).reshape((4,1))
# r = np.array([1, 0, 0, 0]).reshape((4,1))
# hd = transformacao(p,r)
# hd = dualQuatMult(hB1,hd)
# mhd = hd
# #for j in range(8):
# Mhd[:,i] = mhd[:,0]
# if i <ind:
# p = np.array([0, trajPB[i,0], trajPB[i,1], trajPB[i,2]]).reshape((4,1))
# n = np.array([0, 1, 0])
# angulo = np.pi/2
# realR = np.array([mt.cos(angulo/2)])
# imagR = mt.sin(angulo/2)*n
# rb = np.concatenate((realR,imagR), axis = 0).reshape((4,1))
# hd = transformacao(p,rb)
# hd = dualQuatMult(hB1,hd)
# mhd2 = hd
# #for j in range(8):
# Mhd2[:,i] = mhd2[:,0]
# else:
# #for j in range(8):
# Mhd2[:,i] = Mhd2[:,ind-1]
# hP = ha2
# #Mdhd[:,0] = (Mhd[:,0] - Mhd[:,0])*(1/dt)
# #Mdhd2[:,0] = (Mhd2[:,0] - Mhd2[:,0])*(1/dt)
# for i in range(1,T,1):
# #for j in range(8):
# Mdhd[:,i] = (Mhd[:,i] - Mhd[:,i-1])*(1/dt)
# #Mdhd[:,i] = mdhd[:,0]
# Mdhd2[:,0] = (Mhd2[:,i]- Mhd2[:,i-1])*(1/dt)
# #Mdhd2[:,i] = mdhd2[:,0]
# ##################################
# #inicio do codigo
# #LQR
# ganhoS = vecGanho[0,0]
# ganhoQ = vecGanho[1,0]
# ganhoR = vecGanho[2,0]
# #controlador proporcional
# ganhoK2 = vecGanho[3,0]
# K2 = ganhoK2*np.eye(8)
# #ganho P-FF
# S = ganhoS*np.eye(8)
# Q = ganhoQ*np.eye(8)
# R = ganhoR*np.eye(8)
# Rinv = np.linalg.inv(R)
# C8 = np.diag([1, -1, -1, -1, 1, -1, -1, -1])
# #iniciar condições finais esperadas para P e E
# Pf = S
# Ef = np.array([0, 0, 0, 0, 0, 0, 0, 0]).reshape((8,1))
# P = Pf
# MP2 = np.zeros((8,T))
# MP2 = np.zeros((8,8,T))
# for j in range(8):
# for k in range(8):
# MP2[j,k,T-1] = P[j,k]
# E = Ef
# ME2 = np.zeros((8,T))
# #for j in range(8):
# ME2[:,T-1] = E[:,0]
# for i in range(T-2,0,-1):
# #for j in range(8):
# mhdPlus[:,0] = Mhd[:,i+1]
# mdhdPlus[:,0] = Mdhd[:,i+1]
# mhd[:,0] = Mhd[:,i]
# mdhd[:,0] = Mdhd[:,i]
# aux = dualQuatMult(dualQuatConj(Mhd[:,i+1].reshape((8,1))),Mdhd[:,i+1].reshape((8,1)))
# A = dualHamiltonOp(aux,0)
# c = -aux
# #prod2 = np.dot(P,Rinv)
# P = P -(-P@A -A.T@P + P@Rinv@P - Q)*dt
# #MP2[:,i] = P[:]
# for j in range(8):
# for k in range(8):
# MP2[j,k,i] = P[j,k]
# E = E - ((-1)*(A.T)@E + P@Rinv@E - P@c)*dt
# #for j in range(8):
# ME2[:,i] = E[:,0]
# for i in range(0, T, 1):
# #tic
# #Controlador LQR para O CoM
# #calculo de A e c
# #for j in range(8):
# mhd[:,0] = Mhd[:,i]
# mdhd[:,0] = Mdhd[:,i]
# mhd2[:,0] = Mhd2[:,i]
# mdhd2[:,0] = Mdhd2[:,i]
# aux = dualQuatMult(dualQuatConj(Mhd[:,i].reshape((8,1))),Mdhd[:,i].reshape((8,1)))
# A = dualHamiltonOp(aux,0)
# c = -1*np.transpose(aux)
# #inicio do controlador
# #Ja = jacobianoCinematica(theta,hOrg,hP,1,1)
# xe = KinematicModel(MDH,theta,6,0)
# Ja = jacobiano2(theta,hOrg,hP,xe,0)
# #calculo de P e E
# #calculo de N
# Hd = dualHamiltonOp(mhd,0)
# #prod3 = np.dot(Hd,C8)
# N = Hd@C8@Ja
# #pseudo inversa de N
# Np = np.linalg.pinv(N)
# #calculo do erro
# e = np.array([1, 0, 0, 0, 0, 0, 0, 0]).reshape((8,1)) - dualQuatMult(dualQuatConj(ha),Mhd[:,i].reshape((8,1)))
# #calculo de P e E
# #for j in range(8):
# E[:,0] = ME2[:,i]
# #P = np.reshape(MP2[:,i],np.shape(A))
# # Pxe= np.dot(P,e)
# # NpxRinv= np.dot(Np,Rinv)
# do = Np@Rinv@(P@e + E)
# #calculo do o deseja
# od = (do*dt)/2
# for j in range(6):
# theta[j,0] = theta[j,0] + od[j,0]
# for j in range(0,6,1):
# if abs(theta[j,0]) > hpi:
# theta[j,0] = np.sign(theta[j,0])*hpi
# ha = kinematicRobo(theta,hOrg,hP,1,1)
# #plotar os dados
# for j in range(8):
# Mha[j,i] = ha[j,0]
# #posição
# pos = getPositionDualQuat(ha)
# posd = getPositionDualQuat(Mhd[:,i].reshape((8,1)))
# for j in range(3):
# Pos[j,i] = pos[j,0]
# Posd[j,i] = posd[j,0]
# #orientação
# ra = getRotationDualQuat(ha)
# rd = getRotationDualQuat(mhd)
# if ra[0,0] > 1:
# ra[0,0] = 1
# co = mt.acos(ra[0,0])
# angle[i] = co
# co = mt.acos(rd[0,0])
# angled[i] = co
# for j in range(6):
# Mtheta[j,i] = theta[j,0]
# #controlador 2
# #calculo de A e c
# aux2 = dualQuatMult(dualQuatConj(mhd2),mdhd2)
# #A2 = dualHamiltonOp(aux2,0)
# c = -np.transpose(aux2)
# #inicio do controlador
# #Ja2 = jacobianoCinematica(theta,hOrg,hP,1,0)
# xe2 = kinematicRobo(theta,hOrg,hP,1,0)
# Ja2 = jacobianoPes(theta,ha,xe2,1)
# #calculo de P e E
# #calculo de N
# Hd2 = dualHamiltonOp(mhd2,0)
# #prod1= np.dot(Hd2,C8)
# N2 = Hd2@C8@Ja2
# #pseudo inversa de N
# Np2 = np.linalg.pinv(N2)
# #calculo do erro
# e2 = np.array([1, 0, 0, 0, 0, 0, 0, 0]).reshape((8,1)) - dualQuatMult(dualQuatConj(ha2),mhd2)
# vec2 = dualQuatMult(dualQuatConj(ha2),mdhd2)
# #
# #produto= np.dot(K2,e2-vec2)
# do2 = Np2@(K2@e2-vec2)
# od2 = (do2*dt)/2
# theta[:,1] = theta[:,1] + od2[:,0]
# ha2 = kinematicRobo(theta,hOrg,hP,1,0)
# #plotar os dados
# #for j in range(8):
# Mha2[:,i] = ha2[:,0]
# #posição
# pos2 = getPositionDualQuat(ha2)
# posd2 = getPositionDualQuat(mhd2)
# for j in range(3):
# Pos2[j,i] = pos2[j,0]
# Posd2[j,i] = posd2[j,0]
# #orientação
# ra = getRotationDualQuat(ha2)
# rd = getRotationDualQuat(mhd2)
# co = mt.acos(ra[0,0])
# angle2[i] = co
# co = mt.acos(rd[0,0])
# angled2[i] = co
# #for j in range(6):
# Mtheta2[:,i] = theta[:,1]
# #mostrar no console o andamento do metódo
# #msg = sprintf('#d de #d | tempo: #f',i,T,toc)
# #disp(msg)
# #hold on
# plotGraficosControle(t1,dt,T,Pos,Posd,angle,angled,Mha,Mhd,Mtheta,Pos2,Posd2,angle2,angled2,Mha2,Mhd2,Mtheta2,'b','r')
# return ha,ha2,theta,tempo, Mtheta, Mtheta2
#global hpi, L1, L2, L3, L4, L5, height, MDH, hEdo
glob = GlobalVariables()
hEdo = glob.getHEDO()
L1 = glob.getL1()
#L2 = glob.getL2()
#L3 = glob.getL3()
#L4 = glob.getL4()
#L5 = glob.getL5()
height = glob.getHeight()
MDH = glob.getMDH()
hpi = glob.getHpi()
dt = hEdo #dt é o tempo da solução da equação Edo
hOrg = np.array([1, 0, 0, 0, 0, 0, 0, 0]).reshape((8, 1)) #posição da base
T = np.size(trajCoM, 0)
#T = 500;
#t = 1:1:T;
tempo = (T - 1) * dt
#tempo = (T)*dt
#matrizes auxiliares
Mhd = np.zeros((8, T))
Mha = np.zeros((8, T))
Mdhd = np.zeros((8, T))
Mtheta = np.zeros((6, T))
mhd = np.zeros((8, 1))
mdhd = np.zeros((8, 1))
mhd2 = np.zeros((8, 1))
mdhd2 = np.zeros((8, 1))
mhdPlus = np.zeros((8, 1))
mdhdPlus = np.zeros((8, 1))
Mhd2 = np.zeros((8, T))
Mha2 = np.zeros((8, T))
Mdhd2 = np.zeros((8, T))
Mtheta2 = np.zeros((6, T))
angle = np.zeros(T)
angled = np.zeros(T)
angle2 = np.zeros(T)
angled2 = np.zeros(T)
Pos = np.zeros((3, T))
Posd = np.zeros((3, T))
Pos2 = np.zeros((3, T))
Posd2 = np.zeros((3, T))
#calculo de Mhd - matriz de hd
r = np.array([1, 0, 0, 0]).reshape(4, 1)
#p = [0 0 0 0]';
p = np.array([0, 0, -L1, -height]).reshape((4, 1))
hB1 = transformacao(p, r) #transformação base robô
for i in range(0, T, 1):
p = np.array([0, trajCoM[i, 0], trajCoM[i, 1], trajCoM[i, 2]]).reshape(
(4, 1))
r = np.array([1, 0, 0, 0]).reshape((4, 1))
hd = transformacao(p, r)
hd = dualQuatMult(hB1, hd)
mhd = hd
#for j in range(8):
Mhd[:, i] = mhd[:, 0]
if i < ind:
p = np.array([0, trajPB[i, 0], trajPB[i, 1],
trajPB[i, 2]]).reshape((4, 1))
n = np.array([0, 1, 0])
angulo = np.pi / 2
realR = np.array([mt.cos(angulo / 2)])
imagR = mt.sin(angulo / 2) * n
rb = np.concatenate((realR, imagR), axis=0).reshape((4, 1))
hd = transformacao(p, rb)
hd = dualQuatMult(hB1, hd)
mhd2 = hd
#for j in range(8):
Mhd2[:, i] = mhd2[:, 0]
else:
#for j in range(8):
Mhd2[:, i] = Mhd2[:, ind - 1]
hP = ha2
#Mdhd[:,0] = (Mhd[:,0] - Mhd[:,0])*(1/dt)
#Mdhd2[:,0] = (Mhd2[:,0] - Mhd2[:,0])*(1/dt)
for i in range(1, T, 1):
#for j in range(8):
Mdhd[:, i] = (Mhd[:, i] - Mhd[:, i - 1]) * (1 / dt)
#Mdhd[:,i] = mdhd[:,0]
Mdhd2[:, i] = (Mhd2[:, i] - Mhd2[:, i - 1]) * (1 / dt)
#Mdhd2[:,i] = mdhd2[:,0]
##################################
#inicio do codigo
#LQR
ganhoS = vecGanho[0, 0]
ganhoQ = vecGanho[1, 0]
ganhoR = vecGanho[2, 0]
#controlador proporcional
ganhoK2 = vecGanho[3, 0]
K2 = ganhoK2 * np.eye(8)
#ganho P-FF
S = ganhoS * np.eye(8)
Q = ganhoQ * np.eye(8)
R = ganhoR * np.eye(8)
Rinv = np.linalg.inv(R)
C8 = np.diag([1, -1, -1, -1, 1, -1, -1, -1])
#iniciar condições finais esperadas para P e E
Pf = S
Ef = np.array([0, 0, 0, 0, 0, 0, 0, 0]).reshape((8, 1))
P = Pf
MP2 = np.zeros((8, T))
MP2 = np.zeros((8, 8, T))
for j in range(8):
for k in range(8):
MP2[j, k, T - 1] = P[j, k]
E = Ef
ME2 = np.zeros((8, T))
#for j in range(8):
ME2[:, T - 1] = E[:, 0]
for i in range(T - 2, -1, -1):
#for j in range(8):
mhdPlus[:, 0] = Mhd[:, i + 1]
mdhdPlus[:, 0] = Mdhd[:, i + 1]
mhd[:, 0] = Mhd[:, i]
mdhd[:, 0] = Mdhd[:, i]
aux = dualQuatMult(dualQuatConj(Mhd[:, i + 1].reshape((8, 1))),
Mdhd[:, i + 1].reshape((8, 1)))
A = dualHamiltonOp(aux, 0)
c = -aux
#prod2 = np.dot(P,Rinv)
P = P - (-P @ A - A.T @ P + P @ Rinv @ P - Q) * dt
#MP2[:,i] = P[:]
for j in range(8):
for k in range(8):
MP2[j, k, i] = P[j, k]
E = E - ((-1) * (A.T) @ E + P @ Rinv @ E - P @ c) * dt
#for j in range(8):
ME2[:, i] = E[:, 0]
for i in range(0, T, 1):
#tic
#Controlador LQR para O CoM
#calculo de A e c
#for j in range(8):
mhd[:, 0] = Mhd[:, i]
mdhd[:, 0] = Mdhd[:, i]
mhd2[:, 0] = Mhd2[:, i]
mdhd2[:, 0] = Mdhd2[:, i]
aux = dualQuatMult(dualQuatConj(Mhd[:, i].reshape((8, 1))),
Mdhd[:, i].reshape((8, 1)))
A = dualHamiltonOp(aux, 0)
c = -1 * np.transpose(aux)
#inicio do controlador
#Ja = jacobianoCinematica(theta,hOrg,hP,1,1)
xe = KinematicModel(MDH, theta, 6, 0)
Ja = jacobiano2(theta, hOrg, hP, xe, 0)
#calculo de P e E
#calculo de N
Hd = dualHamiltonOp(mhd, 0)
#prod3 = np.dot(Hd,C8)
N = Hd @ C8 @ Ja
#pseudo inversa de N
Np = np.linalg.pinv(N)
#calculo do erro
e = np.array([1, 0, 0, 0, 0, 0, 0, 0]).reshape(
(8, 1)) - dualQuatMult(dualQuatConj(ha), Mhd[:, i].reshape((8, 1)))
#calculo de P e E
#for j in range(8):
E[:, 0] = ME2[:, i]
P[:, :] = MP2[:, :, i].reshape((8, 8))
#P = np.reshape(MP2[:,i],np.shape(A))
# Pxe= np.dot(P,e)
# NpxRinv= np.dot(Np,Rinv)
do = Np @ Rinv @ (P @ e + E)
#calculo do o deseja
od = (do * dt) / 2
for j in range(6):
theta[j, 0] = theta[j, 0] + od[j, 0]
#for j in range(0,6,1):
# if abs(theta[j,0]) > hpi:
# theta[j,0] = np.sign(theta[j,0])*hpi
ha = kinematicRobo(theta, hOrg, hP, 1, 1)
#plotar os dados
for j in range(8):
Mha[j, i] = ha[j, 0]
#posição
pos = getPositionDualQuat(ha)
posd = getPositionDualQuat(Mhd[:, i].reshape((8, 1)))
for j in range(3):
Pos[j, i] = pos[j, 0]
Posd[j, i] = posd[j, 0]
#orientação
ra = getRotationDualQuat(ha)
rd = getRotationDualQuat(mhd)
co = mt.acos(ra[0, 0])
angle[i] = co
co = mt.acos(rd[0, 0])
angled[i] = co
for j in range(6):
Mtheta[j, i] = theta[j, 0]
#controlador 2
#calculo de A e c
aux2 = dualQuatMult(dualQuatConj(mhd2), mdhd2)
#A2 = dualHamiltonOp(aux2,0)
c = -np.transpose(aux2)
#inicio do controlador
#Ja2 = jacobianoCinematica(theta,hOrg,hP,1,0)
xe2 = kinematicRobo(theta, hOrg, hP, 1, 0)
Ja2 = jacobianoPes(theta, ha, xe2, 1)
#calculo de P e E
#calculo de N
Hd2 = dualHamiltonOp(mhd2, 0)
#prod1= np.dot(Hd2,C8)
N2 = Hd2 @ C8 @ Ja2
#pseudo inversa de N
Np2 = np.linalg.pinv(N2)
#calculo do erro
e2 = np.array([1, 0, 0, 0, 0, 0, 0, 0]).reshape(
(8, 1)) - dualQuatMult(dualQuatConj(ha2), Mhd2[:, i].reshape(
(8, 1)))
vec2 = dualQuatMult(dualQuatConj(ha2), Mhd2[:, i].reshape((8, 1)))
#
#produto= np.dot(K2,e2-vec2)
do2 = Np2 @ (K2 @ e2 - vec2)
od2 = (do2 * dt) / 2
theta[:, 1] = theta[:, 1] + od2[:, 0]
ha2 = kinematicRobo(theta, hOrg, hP, 1, 0)
#plotar os dados
#for j in range(8):
Mha2[:, i] = ha2[:, 0]
#posição
pos2 = getPositionDualQuat(ha2)
posd2 = getPositionDualQuat(Mhd2[:, i].reshape((8, 1)))
for j in range(3):
Pos2[j, i] = pos2[j, 0]
Posd2[j, i] = posd2[j, 0]
#orientação
ra = getRotationDualQuat(ha2)
rd = getRotationDualQuat(Mhd2[:, i].reshape((8, 1)))
co = mt.acos(ra[0, 0])
angle2[i] = co
co = mt.acos(rd[0, 0])
angled2[i] = co
#for j in range(6):
Mtheta2[:, i] = theta[:, 1]
#mostrar no console o andamento do metódo
#msg = sprintf('#d de #d | tempo: #f',i,T,toc)
#disp(msg)
#hold on
plotGraficosControle(t1, dt, T, Pos, Posd, angle, angled, Mha, Mhd, Mtheta,
Pos2, Posd2, angle2, angled2, Mha2, Mhd2, Mtheta2,
'b', 'r')
return ha, ha2, theta, tempo, Mtheta, Mtheta2
#iniciar, limpar memória, fechar todas as janelas #----------------------------------------------------------- #clc #close all #clear #----------------------------------------------------------- #adicionar as libs necessárias #----------------------------------------------------------- from otimizacao import otimizacao import numpy as np from globalVariables import GlobalVariables #----------------------------------------------------------- #variáveis globais #----------------------------------------------------------- #global m, L, g, lstep, pfa, thetaM, phiM, KM, expK, BSSM glob = GlobalVariables() #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)
def otimizacao(U, X, tipo, metodo):
if tipo == 1:
#-----------------------------------------------------------
#variáveis globais
#-----------------------------------------------------------
glob = GlobalVariables()
maxNGrad = glob.getMaxNGrad()
#ganhoAlpha = glob.getGanhoAlpha
#gamma = glob.getGamma
#global maxNGrad, ganhoAlpha, gamma
#-----------------------------------------------------------
#iniciar variáveis de controle inicial
#-----------------------------------------------------------
u1 = U[0, 0]
u2 = U[1, 0]
u3 = U[2, 0]
u4 = U[3, 0]
u5 = U[4, 0]
#-----------------------------------------------------------
#inicio do método
#----------------------------------------------------------
fo = 1 #condição de parada
#-----------------------------------------------------------
#melhores valores
#----------------------------------------------------------
fm = fo
UM = U
#-----------------------------------------------------------
#vetores axiliares para os métodos de otimização
#----------------------------------------------------------
vt = np.zeros((4, 1)) #usado como auxiliar NAG
Grad = np.zeros((4, 1)) #usado como auxiliar adagrad
[pa, pb, pc, M, ponto] = trajetoria(U, X)
fo = funcaoObjetivo(pa, pb, pc)
if fo < 1 * 10**(-10):
print("Valores já otimizados")
return
for j in range(1, maxNGrad, 1):
#-----------------------------------------------------------
# gradiente descendente estocástico SGD
#----------------------------------------------------------
if metodo == 0:
U = SGD(U, X)
#-----------------------------------------------------------
#SGD com momento
#----------------------------------------------------------
if metodo == 1:
[U, vt] = SGDMomento(U, X, vt)
#-----------------------------------------------------------
#Nesterov accelerated gradient
#----------------------------------------------------------
if metodo == 2:
[U, vt] = NAG(U, X, vt)
#-----------------------------------------------------------
#Adagrad
#----------------------------------------------------------
if metodo == 3:
[U, Grad] = adagrad(U, X, Grad)
#-----------------------------------------------------------
#Setar os limites inferiores e superiores em U
#----------------------------------------------------------
U0 = np.zeros((5, 1))
U0 = setLimites(U)
u1 = U0[0, 0]
u2 = U0[1, 0]
u3 = U0[2, 0]
u4 = U0[3, 0]
u5 = U0[4, 0]
#-----------------------------------------------------------
#atualizar o vetor U (variáveis de controle)
#----------------------------------------------------------
U = np.array([[u1], [u2], [u3], [u4], [u5]])
#-----------------------------------------------------------
#Cálculo do valor da função objetivo
#a função de otimização é usada para calcular os valores de U,
#que serão inseridos na função de calcular a trajetória
#----------------------------------------------------------
[pa, pb, pc, M, ponto] = trajetoria(U, X)
fo = funcaoObjetivo(pa, pb, pc)
#-----------------------------------------------------------
#verificar melhor resultado
#fm = 1, inicialmente
#cada vez que fo < fm, fm armazena o valor de fo
#fo sempre é comparado com seu valor anterior, desde que
#esteja convergindo
#valores de fo maiores que o anterior, serão ignorados
#----------------------------------------------------------
if fo < fm:
fm = fo
UM = U
#-----------------------------------------------------------
#verificar condição de parada
#a condição de parada ocorre quando a projeção do CoM no plano xy
#praticamente coincide com o ponto médio das duas pernas, no mesmo plano
#isso deve acontecer na fase LH, da caminhada
#----------------------------------------------------------
if fo < 1 * 10**(-10):
break
#-----------------------------------------------------------
#imprimir resultado no console
#----------------------------------------------------------
imprimirConsole(j, [U, fo])
#-----------------------------------------------------------
#imprimir resultado final no console
#----------------------------------------------------------
print('************************************************************')
print('Melhor Solução: ')
imprimirConsole(0, [UM, fm])
print('************************************************************')
#-----------------------------------------------------------
#mostrar a trajetória
#----------------------------------------------------------
plotarTrajetoria(UM, X)
else:
#-----------------------------------------------------------
#mostrar a trajetoria
#----------------------------------------------------------
plotarTrajetoria(U, X)
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
def fase1(trajCoM1, ind, trajPB1, theta, vecGanho):
begin = time.time()
#global hpi, L1, L2, L3, L4, L5, height, MDH, hEdo
glob = GlobalVariables()
hEdo = glob.getHEDO()
L1 = glob.getL1()
#L2 = glob.getL2()
#L3 = glob.getL3()
#L4 = glob.getL4()
#L5 = glob.getL5()
height = glob.getHeight()
MDH = glob.getMDH()
hpi = 0.7 * mt.pi
hOrg = np.array([[1], [0], [0], [0], [0], [0], [0], [0]]) #posição da base
dt = hEdo
#cacular posição atual do´pé
n = np.array([0, 1, 0]) # n é o vetor diretor do quaternio
thetab = hpi #parametro da função de caminhada que é igual a pi/2
realRb = np.array([np.cos(thetab / 2)])
rb = np.concatenate((realRb, np.sin(thetab / 2) * n), axis=0).reshape(
(4, 1))
pb = np.array([[0], [0], [-L1], [-height]])
#base B para a base O6 (B é a perna em movimento)
hB_O6 = transformacao(pb, rb)
hP = dualQuatMult(hOrg, hB_O6)
#dt é o tempo da solução da equação Edo e, ao mesmo tempo, o passo
T = np.size(trajCoM1, 0) #o tamanho de trajCoM = ind
#T = 100;
#tempo = (T-1)*dt #o tempo é o produto da quantidade de iterações necessárias para calcular a trajetória do CoM
tempo = (T - 1) * dt
#pelo tamanho do intervalo de tempo(passo)
#t = 1:1:T;
#matrizes e vetores auxiliares
Mhd = np.zeros((8, T))
Mha = np.zeros((8, T))
Mdhd = np.zeros((8, T))
Mtheta = np.zeros((6, T))
mhd = np.zeros((8, 1))
mdhd = np.zeros((8, 1))
mhd2 = np.zeros((8, 1))
mdhd2 = np.zeros((8, 1))
Mhd2 = np.zeros((8, T))
Mha2 = np.zeros((8, T))
Mdhd2 = np.zeros((8, T))
Mtheta2 = np.zeros((6, T))
angle = np.zeros(T)
angled = np.zeros(T)
angle2 = np.zeros(T)
angled2 = np.zeros(T)
Pos = np.zeros((3, T))
Posd = np.zeros((3, T))
Pos2 = np.zeros((3, T))
Posd2 = np.zeros((3, T))
#calculo de Mhd - matriz de hd
r = np.array([1, 0, 0, 0]).reshape((4, 1))
p = np.array([0, 0, -L1, -height]).reshape(
(4, 1)) #height = L2 + L3 + L4 + L5
hB1 = transformacao(p, r) #transformação base robô
for i in range(0, T, 1):
p = np.array([0, trajCoM1[i, 0], trajCoM1[i, 1],
trajCoM1[i, 2]]).reshape((4, 1))
r = np.array([1, 0, 0, 0]).reshape((4, 1))
hd = transformacao(p, r)
hd = dualQuatMult(hB1, hd)
for j in range(8):
Mhd[j, i] = hd[j, 0]
#transformação da base até o centro de massa
#se i<ind, o robô ainda não atingiu a posição de td, então a transformação é calculada em relação ao pé
#quando o robô chega na fase de TD, a transformação é calculada em relação ao CoM
if i < ind:
p = np.array([0, trajPB1[i, 0], trajPB1[i, 1],
trajPB1[i, 2]]).reshape((4, 1))
n = np.array([0, 1, 0])
#angulo = mt.pi/2.0 #graus ou radianos????????????????????????????????????????????????????????????/
realRb = np.array([np.cos(thetab / 2)])
rb = np.concatenate((realRb, np.sin(thetab / 2) * n),
axis=0).reshape((4, 1))
hd = transformacao(p, rb) #posição desejada
hd = dualQuatMult(hB1, hd) #transformação da base até o pé
for j in range(8):
Mhd2[j, i] = hd[j, 0]
else:
Mhd2[:, i] = Mhd2[:, ind - 1]
ha = kinematicRobo(theta, hOrg, hP, 1,
1) #cinemática do pé esquerdo até o CoM
ha2 = kinematicRobo(theta, hOrg, hP, 1,
0) #cinemática de um pé até o outro
#Mdhd[:,0] = (Mhd[:,0] - Mhd[:,0])*(1/dt) #não deveria ser i-1, no segundo Mhd???????????????????????????????????
#Mdhd2[:,0] = (Mhd2[:,0] - Mhd2[:,0])*(1/dt)
for i in range(1, T, 1):
Mdhd[:, i] = (Mhd[:, i] - Mhd[:, i - 1]) * (
1 / dt
) #por que ele fazer isso????????????????????????????????????????????????????
Mdhd2[:, i] = (Mhd2[:, i] - Mhd2[:, i - 1]) * (
1 / dt) #derivada de hd, que é a posição desejada
##################################
#inicio do codigo
#LQR
#calculo dos parâmetros
ganhoS = vecGanho[0, 0]
ganhoQ = vecGanho[1, 0]
ganhoR = vecGanho[2, 0]
#controlador proporcional
ganhoK2 = vecGanho[3, 0]
K2 = ganhoK2 * np.eye(8)
Ki = 10 * np.eye(8)
Kd = 10000 * np.eye(8)
S = ganhoS * np.eye(8)
Q = ganhoQ * np.eye(8)
R = ganhoR * np.eye(8)
Rinv = np.linalg.inv(R)
# print('R::',Rinv)
# return
ab = np.array([1, -1, -1, -1, 1, -1, -1, -1])
C8 = np.diag(ab)
#iniciar condições finais esperadas para P e E
Pf = S
Ef = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]).reshape((8, 1))
P = Pf
MP2 = np.zeros((8, 8, T))
for j in range(8):
for k in range(8):
MP2[j, k, T - 1] = P[j, k]
E = Ef
ME2 = np.zeros((8, T))
for j in range(8):
ME2[j, T - 1] = E[j, 0]
#calcular matrizes de ganho
for i in range(T - 2, -1, -1):
for j in range(8):
mhd[j, 0] = Mhd[j, i + 1]
mdhd[j, 0] = Mdhd[j, i + 1]
aux = dualQuatMult(dualQuatConj(mhd), mdhd)
A = dualHamiltonOp(aux, 0)
c = -aux
# prod2 = np.dot(P,Rinv)
P = P - (-P @ A - A.T @ P + P @ Rinv @ P - Q) * dt
for j in range(8):
for k in range(8):
MP2[j, k, i] = P[j, k]
E = E - ((-1) * (A.T) @ E + P @ Rinv @ E - P @ c) * dt
for j in range(8):
ME2[j, i] = E[j, 0]
integral = 0
e_previous = 0
for i in range(0, T, 1):
#tic = tm.time()
#Controlador LQR para o CoM
#calculo de A e c
for j in range(8):
mhd[j, 0] = Mhd[j, i]
mdhd[j, 0] = Mdhd[j, i]
aux = dualQuatMult(dualQuatConj(mhd),
mdhd) #calculo de hd conjugado * hd derivada
A = dualHamiltonOp(aux, 0)
c = -aux
#inicio do controlador
#hB_O6a = dualQuatMult(hOrg,hP)
xe = KinematicModel(MDH, theta, 6, 0)
Ja = jacobiano2(theta, hOrg, hP, xe,
0) #jacobiano para a perna direita
# Ja = jacobianoCinematica(theta,hOrg,hP,1,1)
#calculo de P e E
#calculo de N
Hd = dualHamiltonOp(mhd, 0)
# prod3 = np.dot(Hd,C8)
N = Hd @ C8 @ Ja
#pseudo inversa de N
Np = np.linalg.pinv(N)
#Np = (1/np.linalg.norm(Np))*Np
#calculo do erro
e = np.array([1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]).reshape(
(8, 1)) - dualQuatMult(dualQuatConj(ha), mhd)
#calculo de P e E
E[:, 0] = ME2[:, i]
P[:, :] = MP2[:, :, i].reshape((8, 8))
#Pxe= np.dot(P,e)
#do = Np@Rinv@(P@e + E)
do = Np @ Rinv @ (P @ e + E) #equação final para theta ponto
#calculo do theta deseja
od = (do * dt) / 2
theta[:, 0] = theta[:, 0] + od[:, 0]
# print('theta::',theta)
# return
#o movimento dos motores é limitado entre pi/2 e -pi/2, então, se theta estiver
#fora do intervalo, esse for faz theta = limite do intervalo
for j in range(0, 6, 1):
if abs(theta[j, 0]) > hpi:
theta[j, 0] = np.sign(theta[j, 0]) * hpi
ha = kinematicRobo(
theta, hOrg, hP, 1,
1) #não deveria ser hd?????????????????????????????????????????
#plotar os dados
for j in range(8):
Mha[j, i] = ha[j, 0]
#posição
PosAux = getPositionDualQuat(ha)
PosdAux = getPositionDualQuat(mhd)
for j in range(3):
Pos[j, i] = PosAux[j, 0] #retorna a posição x,y e z de ha
Posd[j, i] = PosdAux[j,
0] #retorna a posição x,y e z de todos os hd
#orientação
ra = getRotationDualQuat(
ha) ##extrai o vetor do dual quat que representa a rotação
rd = getRotationDualQuat(mhd)
co = mt.acos(ra[0, 0])
angle[i] = co
co = mt.acos(rd[0, 0])
angled[i] = co
for j in range(6):
Mtheta[j, i] = theta[j, 0]
#controlador 2 para os pés (proporcional com feed forward)
#calculo de A e c
#é necessário??????????????????????????????????????????????????????????????????
#aux2 = dualQuatMult(dualQuatConj(Mhd2(:,i)),Mdhd2(:,i));
#A2 = dualHamiltonOp(aux2,0);
#c = -aux2;
#inicio do controlador
#hB_O6a = dualQuatMult(hOrg,hP)
xe2 = kinematicRobo(theta, hOrg, hP, 1, 0)
Ja2 = jacobianoPes(theta, ha, xe2, 1)
#Ja2 = jacobianoCinematica(theta,hOrg,hP,1,0)
#calculo de P e E
#calculo de N
mhd2 = np.zeros((8, 1))
mdhd2 = np.zeros((8, 1))
for j in range(8):
mhd2[j, 0] = Mhd2[j, i]
mdhd2[j, 0] = Mdhd2[j, i]
Hd2 = dualHamiltonOp(mhd2, 0)
N2 = Hd2 @ C8 @ Ja2
#pseudo inversa de N
Np2 = np.linalg.pinv(N2)
# if (i<20):
# print(Np2)
# else:
# return
#calculo do erro
e2 = np.array([1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]).reshape(
(8, 1)) - dualQuatMult(dualQuatConj(ha2), mhd2)
vec2 = dualQuatMult(dualQuatConj(ha2), mdhd2)
#
# K2xe2 = np.dot(K2,e2)
# do2 = np.dot(Np2,(K2xe2-vec2))
# integral = integral + e2*dt
# do2 = Np2@(K2@e2 + Kd@(e2 - e_previous) + Ki@integral - vec2)
do2 = Np2 @ (K2 @ e2 - vec2)
od2 = (do2 * dt) / 2
for j in range(6):
theta[j, 1] = theta[j, 1] + od2[j, 0]
# e_previous = e2
for j in range(0, 6, 1):
if abs(theta[j, 1]) > hpi:
theta[j, 1] = np.sign(theta[j, 1]) * hpi
ha2 = kinematicRobo(theta, hOrg, hP, 1, 0)
#plotar os dados
Mha2[:, i] = ha2[:, 0]
#posição
Pos2Aux = getPositionDualQuat(ha2)
Posd2Aux = getPositionDualQuat(mhd2)
#for j in range(3):
Pos2[:, i] = Pos2Aux[:, 0]
Posd2[:, i] = Posd2Aux[:, 0]
#orientação
ra = getRotationDualQuat(ha2)
rd = getRotationDualQuat(mhd2)
co = mt.acos(ra[0, 0])
angle2[i] = co
co = mt.acos(rd[0, 0])
angled2[i] = co
for j in range(6):
Mtheta2[j, i] = theta[j, 1]
#mostrar no console o andamento do metódo
#toc = tm.time() - t #elapsed
#msg = print('#d de #d | tempo (s): #f',i,T,toc);
#disp(msg);
t1 = 0
end = time.time()
Mtheta = Mtheta * 180 / mt.pi
df = pd.DataFrame(Mtheta.T, columns=['0', '1', '2', '3', '4', '5'])
df.to_csv('thetaRight.csv')
Mtheta2 = Mtheta2 * 180 / mt.pi
df = pd.DataFrame(Mtheta2.T, columns=['0', '1', '2', '3', '4', '5'])
df.to_csv('thetaLeft.csv')
print('execution time:', end - begin)
plotGraficosControle(t1, dt, T, Pos, Posd, angle, angled, Mha, Mhd, Mtheta,
Pos2, Posd2, angle2, angled2, Mha2, Mhd2, Mtheta2,
'b', 'r')
return ha, ha2, theta, tempo, Mtheta, Mtheta2
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 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()
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
#addpath('quaternion_library');
#addpath('dual_quaternion_library');
#addpath('kinematic_dualquartenion_library');
#addpath('transformation');
#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
def jacobiano2(theta,hOrg,hP,xe,leg):
#-----------------------------------------------------------
#c�lculo das derivadas para cada vari�vel de controle
#----------------------------------------------------------
z = np.zeros((8,1))
thetar = theta[:,0].reshape((6,1))
thetal = theta[:,1].reshape((6,1))
glob = GlobalVariables()
L1 = glob.getL1()
L2 = glob.getL2()
hpi = glob.getHpi()
MDH = glob.getMDH()
#hCoM_O0_rightLeg = dualQuatDH(hpi,-L2,-L1,0,0) #transformação do sist de coordenadas do centro de massa para a origem 0 da perna direita
#hCoM_O0_leftLeg = dualQuatDH(hpi,-L2, L1,0,0) #transformação do sist de coordenadas do centro de massa para a origem 0 da perna esquerda
#perna = 1 (perna direita)
#perna = 0 (perna esquerda)
#hB = dualQuatMult(hOrg,hP)
#hB_O6a = dualQuatMult(hOrg,hP)
#hB = [1 0 0 0 0 0 0 0]'
##################################j1##################################
#h = dualQuatMult(hB,KinematicModel(MDH,thetar,6,0))
if leg == 0: #right leg
h = dualQuatMult(hP,KinematicModel(MDH,thetar,6,0)) #da base global até a junta 6
#h = [1 0 0 0 0 0 0 0]'
#z[0,0] = 0
z[1,0] = h[1,0]*h[3,0] + h[0,0]*h[2,0]
z[2,0] = h[2,0]*h[3,0] - h[0,0]*h[1,0]
z[3,0] = (h[3,0]**2 - h[2,0]**2 - h[1,0]**2 + h[0,0]**2)/2
#z[4,0] = 0
z[5,0] = h[1,0]*h[7,0] + h[5,0]*h[3,0] + h[0,0]*h[6,0] + h[4,0]*h[2,0]
z[6,0] = h[2,0]*h[7,0] + h[6,0]*h[3,0] - h[0,0]*h[5,0] - h[4,0]*h[1,0]
z[7,0] = h[3,0]*h[7,0] - h[2,0]*h[6,0] - h[1,0]*h[5,0] + h[0,0]*h[4,0]
j0 = dualQuatMult(z,xe)
##################################j1##################################
h = dualQuatMult(hP,KinematicModel(MDH,thetar,5,0))
#z[0,0] = 0
z[1,0] = h[1,0]*h[3,0] + h[0,0]*h[2,0]
z[2,0] = h[2,0]*h[3,0] - h[0,0]*h[1,0]
z[3,0] = (h[3,0]**2 - h[2,0]**2 - h[1,0]**2 + h[0,0]**2)/2
#z[4,0] = 0
z[5,0] = h[1,0]*h[7,0] + h[5,0]*h[3,0] + h[0,0]*h[6,0] + h[4,0]*h[2,0]
z[6,0] = h[2,0]*h[7,0] + h[6,0]*h[3,0] - h[0,0]*h[5,0] - h[4,0]*h[1,0]
z[7,0] = h[3,0]*h[7,0] - h[2,0]*h[6,0] - h[1,0]*h[5,0] + h[0,0]*h[4,0]
j1 = dualQuatMult(z,xe)
##################################j2##################################
h = dualQuatMult(hP,KinematicModel(MDH,thetar,4,0))
#z[0,0] = 0
z[1,0] = h[1,0]*h[3,0] + h[0,0]*h[2,0]
z[2,0] = h[2,0]*h[3,0] - h[0,0]*h[1,0]
z[3,0] = (h[3,0]**2 - h[2,0]**2 - h[1,0]**2 + h[0,0]**2)/2
#z[4,0] = 0
z[5,0] = h[1,0]*h[7,0] + h[5,0]*h[3,0] + h[0,0]*h[6,0] + h[4,0]*h[2,0]
z[6,0] = h[2,0]*h[7,0] + h[6,0]*h[3,0] - h[0,0]*h[5,0] - h[4,0]*h[1,0]
z[7,0] = h[3,0]*h[7,0] - h[2,0]*h[6,0] - h[1,0]*h[5,0] + h[0,0]*h[4,0]
j2 = dualQuatMult(z,xe)
##################################j3##################################
h = dualQuatMult(hP,KinematicModel(MDH,thetar,3,0))
#z[0,0] = 0
z[1,0] = h[1,0]*h[3,0] + h[0,0]*h[2,0]
z[2,0] = h[2,0]*h[3,0] - h[0,0]*h[1,0]
z[3,0] = (h[3,0]**2 - h[2,0]**2 - h[1,0]**2 + h[0,0]**2)/2
#z[4,0] = 0
z[5,0] = h[1,0]*h[7,0] + h[5,0]*h[3,0] + h[0,0]*h[6,0] + h[4,0]*h[2,0]
z[6,0] = h[2,0]*h[7,0] + h[6,0]*h[3,0] - h[0,0]*h[5,0] - h[4,0]*h[1,0]
z[7,0] = h[3,0]*h[7,0] - h[2,0]*h[6,0] - h[1,0]*h[5,0] + h[0,0]*h[4,0]
j3 = dualQuatMult(z,xe)
##################################j4##################################
h = dualQuatMult(hP,KinematicModel(MDH,thetar,2,0))
#z[0,0] = 0
z[1,0] = h[1,0]*h[3,0] + h[0,0]*h[2,0]
z[2,0] = h[2,0]*h[3,0] - h[0,0]*h[1,0]
z[3,0] = (h[3,0]**2 - h[2,0]**2 - h[1,0]**2 + h[0,0]**2)/2
#z[4,0] = 0
z[5,0] = h[1,0]*h[7,0] + h[5,0]*h[3,0] + h[0,0]*h[6,0] + h[4,0]*h[2,0]
z[6,0] = h[2,0]*h[7,0] + h[6,0]*h[3,0] - h[0,0]*h[5,0] - h[4,0]*h[1,0]
z[7,0] = h[3,0]*h[7,0] - h[2,0]*h[6,0] - h[1,0]*h[5,0] + h[0,0]*h[4,0]
j4 = dualQuatMult(z,xe)
##################################j5##################################
h = dualQuatMult(hP,KinematicModel(MDH,thetar,1,0))
#z[0,0] = 0
z[1,0] = h[1,0]*h[3,0] + h[0,0]*h[2,0]
z[2,0] = h[2,0]*h[3,0] - h[0,0]*h[1,0]
z[3,0] = (h[3,0]**2 - h[2,0]**2 - h[1,0]**2 + h[0,0]**2)/2
#z[4,0] = 0
z[5,0] = h[1,0]*h[7,0] + h[5,0]*h[3,0] + h[0,0]*h[6,0] + h[4,0]*h[2,0]
z[6,0] = h[2,0]*h[7,0] + h[6,0]*h[3,0] - h[0,0]*h[5,0] - h[4,0]*h[1,0]
z[7,0] = h[3,0]*h[7,0] - h[2,0]*h[6,0] - h[1,0]*h[5,0] + h[0,0]*h[4,0]
j5 = dualQuatMult(z,xe)
else:
h = dualQuatMult(hP,KinematicModel(MDH,thetal,6,0)) #da base global até a junta 6
#h = [1 0 0 0 0 0 0 0]'
#z[0,0] = 0
z[1,0] = h[1,0]*h[3,0] + h[0,0]*h[2,0]
z[2,0] = h[2,0]*h[3,0] - h[0,0]*h[1,0]
z[3,0] = (h[3,0]**2 - h[2,0]**2 - h[1,0]**2 + h[0,0]**2)/2
#z[4,0] = 0
z[5,0] = h[1,0]*h[7,0] + h[5,0]*h[3,0] + h[0,0]*h[6,0] + h[4,0]*h[2,0]
z[6,0] = h[2,0]*h[7,0] + h[6,0]*h[3,0] - h[0,0]*h[5,0] - h[4,0]*h[1,0]
z[7,0] = h[3,0]*h[7,0] - h[2,0]*h[6,0] - h[1,0]*h[5,0] + h[0,0]*h[4,0]
j0 = dualQuatMult(z,xe)
##################################j1##################################
h = dualQuatMult(hP,KinematicModel(MDH,thetal,5,0))
#z[0,0] = 0
z[1,0] = h[1,0]*h[3,0] + h[0,0]*h[2,0]
z[2,0] = h[2,0]*h[3,0] - h[0,0]*h[1,0]
z[3,0] = (h[3,0]**2 - h[2,0]**2 - h[1,0]**2 + h[0,0]**2)/2
#z[4,0] = 0
z[5,0] = h[1,0]*h[7,0] + h[5,0]*h[3,0] + h[0,0]*h[6,0] + h[4,0]*h[2,0]
z[6,0] = h[2,0]*h[7,0] + h[6,0]*h[3,0] - h[0,0]*h[5,0] - h[4,0]*h[1,0]
z[7,0] = h[3,0]*h[7,0] - h[2,0]*h[6,0] - h[1,0]*h[5,0] + h[0,0]*h[4,0]
j1 = dualQuatMult(z,xe)
##################################j2##################################
h = dualQuatMult(hP,KinematicModel(MDH,thetal,4,0))
#z[0,0] = 0
z[1,0] = h[1,0]*h[3,0] + h[0,0]*h[2,0]
z[2,0] = h[2,0]*h[3,0] - h[0,0]*h[1,0]
z[3,0] = (h[3,0]**2 - h[2,0]**2 - h[1,0]**2 + h[0,0]**2)/2
#z[4,0] = 0
z[5,0] = h[1,0]*h[7,0] + h[5,0]*h[3,0] + h[0,0]*h[6,0] + h[4,0]*h[2,0]
z[6,0] = h[2,0]*h[7,0] + h[6,0]*h[3,0] - h[0,0]*h[5,0] - h[4,0]*h[1,0]
z[7,0] = h[3,0]*h[7,0] - h[2,0]*h[6,0] - h[1,0]*h[5,0] + h[0,0]*h[4,0]
j2 = dualQuatMult(z,xe)
##################################j3##################################
h = dualQuatMult(hP,KinematicModel(MDH,thetal,3,0))
#z[0,0] = 0
z[1,0] = h[1,0]*h[3,0] + h[0,0]*h[2,0]
z[2,0] = h[2,0]*h[3,0] - h[0,0]*h[1,0]
z[3,0] = (h[3,0]**2 - h[2,0]**2 - h[1,0]**2 + h[0,0]**2)/2
#z[4,0] = 0
z[5,0] = h[1,0]*h[7,0] + h[5,0]*h[3,0] + h[0,0]*h[6,0] + h[4,0]*h[2,0]
z[6,0] = h[2,0]*h[7,0] + h[6,0]*h[3,0] - h[0,0]*h[5,0] - h[4,0]*h[1,0]
z[7,0] = h[3,0]*h[7,0] - h[2,0]*h[6,0] - h[1,0]*h[5,0] + h[0,0]*h[4,0]
j3 = dualQuatMult(z,xe)
##################################j4##################################
h = dualQuatMult(hP,KinematicModel(MDH,thetal,2,0))
#z[0,0] = 0
z[1,0] = h[1,0]*h[3,0] + h[0,0]*h[2,0]
z[2,0] = h[2,0]*h[3,0] - h[0,0]*h[1,0]
z[3,0] = (h[3,0]**2 - h[2,0]**2 - h[1,0]**2 + h[0,0]**2)/2
#z[4,0] = 0
z[5,0] = h[1,0]*h[7,0] + h[5,0]*h[3,0] + h[0,0]*h[6,0] + h[4,0]*h[2,0]
z[6,0] = h[2,0]*h[7,0] + h[6,0]*h[3,0] - h[0,0]*h[5,0] - h[4,0]*h[1,0]
z[7,0] = h[3,0]*h[7,0] - h[2,0]*h[6,0] - h[1,0]*h[5,0] + h[0,0]*h[4,0]
j4 = dualQuatMult(z,xe)
##################################j5##################################
h = dualQuatMult(hP,KinematicModel(MDH,thetal,1,0))
#z[0,0] = 0
z[1,0] = h[1,0]*h[3,0] + h[0,0]*h[2,0]
z[2,0] = h[2,0]*h[3,0] - h[0,0]*h[1,0]
z[3,0] = (h[3,0]**2 - h[2,0]**2 - h[1,0]**2 + h[0,0]**2)/2
#z[4,0] = 0
z[5,0] = h[1,0]*h[7,0] + h[5,0]*h[3,0] + h[0,0]*h[6,0] + h[4,0]*h[2,0]
z[6,0] = h[2,0]*h[7,0] + h[6,0]*h[3,0] - h[0,0]*h[5,0] - h[4,0]*h[1,0]
z[7,0] = h[3,0]*h[7,0] - h[2,0]*h[6,0] - h[1,0]*h[5,0] + h[0,0]*h[4,0]
j5 = dualQuatMult(z,xe)
jac = np.concatenate((-j5, -j4, -j3, -j2, -j1, -j0),axis=1)
return jac
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
if not filename.endswith(".csv"):
filename = filename + ".csv"
np.savetxt(pathToSave +filename ,np.array( dataToExport),fmt='%s', delimiter=',',newline='\r\n')
print "Data exported: %s"%(pathToSave +filename)
if __name__ == '__main__':
print "Simulator PID: %s on %s"%(str( os.getpid()) , socket.gethostname())
if len(sys.argv) == 6 or len(sys.argv) == 5 or len(sys.argv) == 4:
configFile = None
for item in sys.argv[1:]:
if item.endswith(".py") or item.endswith(".txt"):
configFile = item
globalVar = GlobalVariables(item)
break
elif item == '--motion':
print item
with open('tmp_ConfigData.txt', 'w') as myFile:
myFile.write("staticDelivery = False \n")
# myFile.write("arrayAmpl = [0.0, 0, 0.0] \n")
configFile = 'tmp_ConfigData.txt'
globalVar = GlobalVariables(configFile)
os.system("%s tmp_ConfigData.txt"%rmCmd)
break
if configFile is None:
globalVar = GlobalVariables(None)
pathToTest = globalVar.mainPath + globalVar.nameSimulation
if pathToTest[-1] != '/':
def LRQ3D(U0, X0, passo, t, xn, i):
#deltaU = np.zeros((5,1))
glob = GlobalVariables()
#L = glob.getL
#theta = un[1,0]
#phi = un[0,0]
#X0 = X0[0:5,:]
#U0 = U0[0:3,:]
A = np.diag([1, -1, 1, 1, -1])
B = np.diag([-1, 1, 1])
#Xn = (A^n)*X0
Xn = np.dot(np.linalg.matrix_power(A, passo), X0[0:5, :])
Un = np.dot(np.linalg.matrix_power(B, passo), U0[0:3, :])
#pfb = pc + L*np.array([[np.sin(theta)*np.cos(phi)],[np.sin(theta)*np.sin(phi)],[-np.cos(theta)]])
#pfb = np.array([[2.5], [6.80], [5.8]])
#f = np.dot(A,Xn)
[Jx, Ju] = jacobianos(A, xn, X0, U0, i)
xn = xn[0:5, :]
#Jx = A*(df/dx) avaliado em (X0,U0)
#Ju = A*(df/du) avaliado em (X0,U0)
#Aqui começa o LRQ:
#definindo os ganhos:
Q = 0.01 * np.eye(5)
R = 0.2 * np.eye(3)
#S = 0.08*np.eye(5)
#resolvendo a equação de Ricatti para encontrar P:
#P = Jx.T*(P-P*Ju*np.inv(B.T*P*Ju+R)*Ju.T*P)*Jx+Q
#P = Q #setando Pf=S
#MP = np.zeros((5,5,N))
#for j in range(5):
# for k in range(5):
# MP2[j,k,T-1] = P[j,k]
#def Pfun(P):
# JuTransP = np.dot(Ju.T,P) #Ju.T*P
# JutransPJu = np.dot(JuTransP,Ju) + R #Ju.T*P*Ju+R
# PJu = np.dot(P,Ju) #P*Ju
# Inv = np.linalg.inv(JutransPJu) #(Ju.T*P*Ju+R)^-1
# prod = np.dot(PJu,Inv) #P*Ju*np.inv(B.T*P*Ju+R)
# prod1 = np.dot(prod,JuTransP) #P*Ju*np.inv(B.T*P*Ju+R)*Ju.T*P
# Sub = P-prod1 #P-P*Ju*np.inv(B.T*P*Ju+R
# prod3 = np.dot(Jx.T, Sub) #Jx.T*(P-P*Ju*np.inv(B.T*P*Ju+R)
#P = np.dot(prod3,JuTransP) + Q
# return P - np.dot(prod3,JuTransP) - Q #retorna a função que
#será usada para encontrar P com o método de raízes, do python
#from scipy.optimize import fsolve
#P = fsolve(Pfun,1)
#calculando o ganho K, do controlador
from scipy import linalg as lg
P = lg.solve_discrete_are(Jx, Ju, Q, R)
K = -np.linalg.inv((Ju.T) @ P @ Ju + R) @ (Ju.T) @ P @ Jx
#n realmente é passo ????????????????????????????????????????????????????/
#calculando o vetor de parâmetros controlado
un = Un + np.linalg.matrix_power(
B, passo) @ K @ (np.linalg.matrix_power(A, passo) @ (xn - Xn))
return un
def caminhada(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
CoM = trajCoM1
print(np.size(trajCoM1))
ind = np.size(CoM, 0) #pegar a última posição do vetor de pontos
trajCoM2 = np.zeros((ind, 3))
trajCoM3 = np.zeros((ind, 3))
offsetx = CoM[ind - 1, 0] #cálcular o offset em x
offsety = CoM[ind - 1, 1] #calcular o offset em y
trajCoM2[:,
0] = -CoM[range(ind - 1, -1, -1),
0] + 2 * offsetx #calcular a trajetória simétrica para x
trajCoM2[:,
1] = -CoM[range(ind - 1, -1, -1),
1] + 2 * offsety #calcular a trajetória simétrica para y
trajCoM2[:, 2] = CoM[range(ind - 1, -1, -1), 2] #em z não muda
offsetx = trajCoM2[ind - 1, 0] #cálcular o offset em x
offsety = trajCoM2[ind - 1, 1] #calcular o offset em y
trajCoM3[:, 0] = -trajCoM2[
range(ind - 1, -1, -1),
0] + 2 * offsetx #calcular a trajetória simétrica para x
trajCoM3[:, 1] = trajCoM2[range(ind - 1, -1, -1),
1] #calcular a trajetória simétrica para y
trajCoM3[:, 2] = trajCoM2[range(ind - 1, -1, -1), 2] #em z não muda
trajCoM2 = np.concatenate((trajCoM2, trajCoM3), axis=0)
passoTrajCoM = trajCoM2[np.size(trajCoM2, 0) - 1, 0] - trajCoM2[0, 0]
#trajetoria3
ind = np.size(trajCoM2, 0) #pegar a última posição do vetor de pontos
trajCoM3 = np.zeros((ind, 3))
offsetx = trajCoM2[ind - 1, 0] #cálcular o offset em x
offsety = trajCoM2[ind - 1, 1] #calcular o offset em y
#clr trajCoM3
trajCoM3[:, 0] = -trajCoM2[
range(ind - 1, -1, -1),
0] + 2 * offsetx #calcular a trajetória simétrica para x
trajCoM3[:, 1] = -trajCoM2[
range(ind - 1, -1, -1),
1] + 2 * offsety #calcular a trajetória simétrica para y
trajCoM3[:, 2] = trajCoM2[range(ind - 1, -1, -1), 2] #em z não muda
#-----------------------------------------------------------
#Trajetória dos pés
#-----------------------------------------------------------
passoComprimento = PB[0, 0] #tamanho do passo
passoLargura = PB[1, 0] #Largura do passo
passoAltura = 0.02 #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.02 #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()
#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
#primeira parte da caminhdada
passos = 1
[ha, ha2, theta, tempo1, Mtheta,
Mtheta2] = fase1(trajCoM1, indContadoPe, trajPB1, theta, vecGanho1)
if passos >= tam:
return
tempo = tempo + tempo1
i = 0
while 1:
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, Mtheta,
Mtheta2] = 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
tempo = tempo + tempo2
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
#un = LRQ3D(U0,X0,passos,tempo,xn,un,PC)
#quem serão xn e pc??????????????????????????????????????????????????
#[PA,PB,PC,trajCoM,indContadoPe] = trajetoria(un,xn)
passos = passos + 1
[ha, ha2, theta, tempo3, Mtheta,
Mtheta2] = fase3(ha, ha2, trajCoM, np.size(trajPB, 0), trajP, theta,
tempo, vecGanho1)
if passos >= tam:
return
tempo = tempo + tempo3
i = i + 1
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