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 trajetoria(U, X):
#-----------------------------------------------------------
#variáveis globais
#-----------------------------------------------------------
glob = GlobalVariables()
m = glob.getM() #massa
L = glob.getL() #tamanho da perna
g = glob.getG() #gravidade
h = glob.getH() #passo para o calculo das derivadas
hEdo = glob.getHEDO() #passo para calculo - EDO
#-----------------------------------------------------------
#condições iniciais para MS (referentes ao CM)
#-----------------------------------------------------------
xod = X[0, 0]
yod = X[1, 0]
zod = X[2, 0]
dxod = X[3, 0]
dyod = X[4, 0]
dzod = X[5, 0]
#-----------------------------------------------------------
#valores para a otimização valores de u
#-----------------------------------------------------------
#u = [theta phi k Bss]
theta = U[0, 0]
phi = U[1, 0]
k = U[2, 0]
Bss = U[3, 0]
expK = U[4, 0]
#-----------------------------------------------------------
#vetor com as condições iniciais MS
#-----------------------------------------------------------
y0 = np.array([[xod], [dxod], [yod], [dyod], [zod], [dzod]])
#-----------------------------------------------------------
#vetor com os parâmetros constantes
#-----------------------------------------------------------
params = np.array([[m], [L], [g], [k], [Bss], [expK]])
#-----------------------------------------------------------
#Parâmetros para os métodos
#-----------------------------------------------------------
t = 0 #inicio do tempo t = 0
h = hEdo #passo do método rungekuttaLIMP inicial
N = 10000 #número máximo de iterações
#primeiro metodo
sh = h #tamanho do passo para o método rungekuttaLIMP atualizando durante a execução do método
ind = 0 #contador
#traj = ind + 1 #tamanho do vetor com os pontos da trajetória
#-----------------------------------------------------------
#vetores auxiliares para guardar a trajetória
#-----------------------------------------------------------
px = [y0[0, 0]]
py = [y0[2, 0]]
pz = [y0[4, 0]]
#y = np.zeros((6,1))
#-----------------------------------------------------------
#inicio do método 1 MS para TD
#-----------------------------------------------------------
for i in range(N): #for de 0 até N*h, com passo h
#-----------------------------------------------------------
#vetor de parâmetros
#-----------------------------------------------------------
var = np.array([[t], [h], [1]])
#-----------------------------------------------------------
#método numérico para solucionar as equações diferenciais
#passo a passo
#-----------------------------------------------------------
y = rungeKuttaLIPM(var, y0, params)
#-----------------------------------------------------------
#atualizando a condição inicial
#-----------------------------------------------------------
y0 = y
#-----------------------------------------------------------
#atualizando o instante t
#-----------------------------------------------------------
t = t + sh
#-----------------------------------------------------------
#verificando a condição de parada posição Z < que Z de touchdown
#Z de touchdown = L*cos(theta)
#-----------------------------------------------------------
if t >= 0.105:
break
#-----------------------------------------------------------
#colocando os valores nos vetores auxiliares
#-----------------------------------------------------------
else:
px.append(y0[0, 0])
py.append(y0[2, 0])
pz.append(y0[4, 0])
#-----------------------------------------------------------
#atualizando o contador
#-----------------------------------------------------------
ind = ind + 1
#-----------------------------------------------------------
#atualizando o contador - tratando o valor
#-----------------------------------------------------------
#if ind > 1: #não preciso desse if, porque iniciei o contador no 0
#ind = ind -1 #o Juan havia iniciado em 1
#end
ponto = ind
#-----------------------------------------------------------
#Posição do centro de massa no momento de Touchdown (TD)
#-----------------------------------------------------------
pc = np.array([[px[ind]], [py[ind]], [pz[ind]]]).reshape(
(3, 1)) #centro de massa
#-----------------------------------------------------------
#posição do pé de balanço quando toca o chão
#-----------------------------------------------------------
pfb = pc + L * np.array([[np.sin(theta) * np.cos(phi)],
[np.sin(theta) * np.sin(phi)], [-np.cos(theta)]])
#-----------------------------------------------------------
#tempo em que acontece a codição de touchdown
#-----------------------------------------------------------
TD = t #tempo de TD
#-----------------------------------------------------------
#parametros constante para o segundo método
#-----------------------------------------------------------
params = np.array([[m], [L], [g], [k], [Bss], [t], [pfb[0, 0]],
[pfb[1, 0]], [pfb[2, 0]], [expK]])
#-----------------------------------------------------------
#iniciando o segundo contador
#-----------------------------------------------------------
ind2 = 0
#traj2 = ind2 + 1
sh = h #tamanho do passo para o método rungekuttaLIMP atualizando durante a execução do método
#-----------------------------------------------------------
#vetores auxiliares para guardar a trajetória
#-----------------------------------------------------------
px2 = [y0[0, 0]]
py2 = [y0[2, 0]]
pz2 = [y0[4, 0]]
#-----------------------------------------------------------
#inicio do método 2 TD para LH
#-----------------------------------------------------------
for x in range(N):
#-----------------------------------------------------------
#vetor de parâmetros
#-----------------------------------------------------------
var = np.array([[t], [h], [0]])
#-----------------------------------------------------------
#método numérico para solucionar as equações diferenciais
#passo a passo
#-----------------------------------------------------------
y = rungeKuttaLIPM(var, y0, params)
#-----------------------------------------------------------
#atualizando nova condição inicial
#-----------------------------------------------------------
y0 = y
#-----------------------------------------------------------
#atualizando o instante t
#-----------------------------------------------------------
t = t + sh
#-----------------------------------------------------------
#verificando a condição de parada posição dZ > 0
#-----------------------------------------------------------
#if v.all():
if t >= 0.135:
break
#-----------------------------------------------------------
#atualizando os vetores auxiliares da trajetória
#-----------------------------------------------------------
else:
px2.append(y0[0, 0])
py2.append(y0[2, 0])
pz2.append(y0[4, 0])
#-----------------------------------------------------------
#atualizando o contador
#-----------------------------------------------------------
ind2 = ind2 + 1
#-----------------------------------------------------------
#atualizando o contador - tratando o valor
#-----------------------------------------------------------
#if ind2 > 1
# ind2 = ind2 -1;
#-----------------------------------------------------------
#trajetória do centro de massa CoM M = [x y z]
#-----------------------------------------------------------
# concatenando as listas, para preencher o vetor M
pxTot = px + px2
pyTot = py + py2
pzTot = pz + pz2
pxTot = np.asarray(pxTot)
pxTot = pxTot.reshape(-1, 1)
pyTot = np.asarray(pyTot)
pyTot = pyTot.reshape(-1, 1)
pzTot = np.asarray(pzTot)
pzTot = pzTot.reshape(-1, 1)
M = np.concatenate((pxTot, pyTot, pzTot), axis=1)
M = M.reshape(-1, 3)
#-----------------------------------------------------------
#atualizando a posição do centro de massa
#-----------------------------------------------------------
pc = np.array([[px2[ind2]], [py2[ind2]], [pz2[ind2]]]).reshape(
(3, 1)) #centro de massa
#-----------------------------------------------------------
#atualizando a posição dos pés e do centro de massa
#para o retorno da função
#-----------------------------------------------------------
pa = np.array([[0], [0], [0]])
pb = np.array([[pfb[0, 0]], [pfb[1, 0]], [0]])
pc = np.array([[pc[0, 0]], [pc[1, 0]], [pc[2, 0]]])
#print(np.shape(pc[1,0]))
return pa, pb, pc, M, ponto
def 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 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 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 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
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
#tamanho da perna L = glob.getL() #gravidade g = glob.getG() #posição do pé de suporte em MS pfa = glob.getPfa() #----------------------------------------------------------- #Numero maximo de iterações para o metodo do gradiente #----------------------------------------------------------- #global maxNGrad, ganhoAlpha, gamma, h, hEdo maxNGrad = glob.getMaxNGrad() #número máximo de iterações método ganhoAlpha = glob.getGanhoAlpha() #ganho do fator de ganho para cada passo gamma = glob.getGamma() #ganho para os método gradiente(momento) h = glob.getH() #passo para o calculo das derivadas #calculado (comparado com a função do matlab) hEdo = glob.getHEDO() #passo para o calculo das derivadas #----------------------------------------------------------- #condição inicial para MS #----------------------------------------------------------- #hubo 2 + velocidade maxima 0.4 m/s # xod = 0.00 # yod = 0.05 # zod = 0.6 # dxod = 0.4 # dyod = 0.00 # dzod = 0.00 xod = 0.00 #x inicial (d - desejado) yod = 0.035 #y inicial zod = 0.22 #0.244 #z inicial (muito pequeno apens alguns centimetros)
#----------------------------------------------------------- #variáveis globais #----------------------------------------------------------- import numpy as np from caminhada import caminhada from globalVariables import GlobalVariables from caminhada2 import caminhada2 import time begin = time.time() glob = GlobalVariables() m = glob.getM() L = glob.getL() g = glob.getG() h = glob.getH() hEdo = glob.getHEDO() #global m, L, g, h, hEdo U0 = np.zeros((5, 1)) #----------------------------------------------------------- #condição inicial para MS #----------------------------------------------------------- # xod = 0.0001 #x inicial # yod = 0.05 #y inicial # zod = 0.6 #z inicial # dxod = 0.7 #velocidade desejada no MS # dyod = 0.00 #condição de balanço # dzod = 0.00 #velocidade em z (igual a zero condição necessaria) #Darwin
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 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 fase2(ha, ha2, trajCoM, ind, trajPA, theta, t1, vecGanho):
print('aqui começa a fase2')
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()
#global hpi, L1, L2, L3, L4, L5, height, MDH, hEdo
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 = 1:1:T;
#tempo = (T-1)*dt
tempo = (T - 1) * dt
#matrizes auxiliares
Mhd = np.zeros((8, T))
Mha = np.zeros((8, T))
Mdhd = np.zeros((8, T))
Mtheta = np.zeros((6, T))
Mhd2 = np.zeros((8, T))
Mha2 = np.zeros((8, T))
Mdhd2 = np.zeros((8, T))
Mtheta2 = np.zeros((6, T))
Pos = np.zeros((3, T))
Posd = np.zeros((3, T))
Pos2 = np.zeros((3, T))
Posd2 = np.zeros((3, T))
angle = np.zeros(T)
angled = np.zeros(T)
angle2 = np.zeros(T)
angled2 = np.zeros(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) #posição desejada do CoM
mhd = dualQuatMult(hB1, hd)
#for j in range(8):
Mhd[:, i] = mhd[:, 0]
if i < ind:
p = np.array([0, trajPA[i, 0], trajPA[i, 1],
trajPA[i, 2]]).reshape((4, 1))
n = np.array([0, 1, 0])
angulo = mt.pi / 2 #é isso mesmo?????????????????????????????????????????????????????????????
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) #posição desejada do pé
mhd2 = dualQuatMult(hB1, hd)
#for j in range(8):
Mhd2[:, i] = mhd2[:, 0] #transformação da base até o pé
else:
#for j in range(8):
#Mhd2[j,i] = Mhd2[j,ind-1]
Mhd2[:, i] = Mhd2[:, ind - 1]
hP = ha2
ha2 = kinematicRobo(theta, hOrg, hP, 0, 0) #posição da perna direita
#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)
Mdhd2[:, i] = (Mhd2[:, i] - Mhd2[:, i - 1]) * (1 / dt)
##################################
#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, 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]
Pos = np.zeros((3, T))
Posd = np.zeros((3, T))
angle2 = np.zeros(T)
angled2 = np.zeros(T)
#mhdPlus = np.zeros((8,1))
#mdhdPlus = np.zeros((8,1))
mhd = np.zeros((8, 1))
mdhd = np.zeros((8, 1))
for i in range(T - 2, -1, -1):
#for j in range(8):
#mhdPlus[:,0] = Mhd[:,i+1]
#mdhdPlus[:,0] = Mdhd[:,i+1]
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
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]
aux = dualQuatMult(dualQuatConj(Mhd[:, i].reshape((8, 1))),
Mdhd[:, i].reshape((8, 1)))
A = dualHamiltonOp(aux, 0)
c = -aux
#inicio do controlador
#Ja = jacobianoCinematica(theta,hOrg,hP,0,1)
xe = KinematicModel(MDH, theta, 6, 0)
Ja = jacobiano2(theta, hOrg, hP, xe, 1)
#calculo de P e E
#calculo de N
Hd = dualHamiltonOp(Mhd[:, i].reshape((8, 1)), 0)
# prod3 = np.dot(Hd,C8)
N = Hd @ C8 @ Ja
#pseudo inversa de N
Np = np.linalg.pinv(N)
#######################################################paramos aqui
#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))
P[:, :] = MP2[:, :, i].reshape((8, 8))
do = Np @ Rinv @ (P @ e + E)
#calculo do o deseja
od = dt * do / 2
#for j in range(6):
theta[:, 1] = theta[:, 1] + od[:, 0]
# for j in range(1,6,1):
# if abs(theta[j,1]) > hpi:
# theta[j,1] = np.sign(theta[j,1])*hpi
ha = kinematicRobo(theta, hOrg, hP, 0,
1) #posição do CoM com perna esquerda apoiada
#controlador 2
#calculo de A e c
# mdhd2 = np.zeros((8,1))
# mhd2 = np.zeros((8,1))
#for j in range(8):
# mdhd2[:,0] = Mdhd2[:,i]
# mhd2[:,0] = Mhd2[:,i]
aux2 = dualQuatMult(dualQuatConj(Mhd2[:, i].reshape((8, 1))),
Mdhd2[:, i].reshape((8, 1)))
#A2 = dualHamiltonOp(aux2,0)
c = -aux2
#inicio do controlador
#Ja2 = jacobianoCinematica(theta,hOrg,hP,0,0)
xe2 = kinematicRobo(theta, hOrg, hP, 1, 0)
Ja2 = jacobianoPes(theta, ha, xe2, 0)
#calculo de P e E
#calculo de N
Hd2 = dualHamiltonOp(Mhd2[:, i].reshape((8, 1)), 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), Mdhd2[:, i].reshape((8, 1)))
#do2 = np.zeros(20,20)
do2 = Np2 @ (K2 @ e2 - vec2)
#od2 = np.zeros(100)
od2 = (do2 * dt) / 2
#for j in range(6):
theta[:, 0] = theta[:, 0] + od2[:, 0]
ha2 = kinematicRobo(theta, hOrg, hP, 0, 0) #posição da perna direita
#plotar os dados
#for j in range(8):
Mha[:, i] = ha[:, 0]
#posição
pos = getPositionDualQuat(ha)
posd = getPositionDualQuat(Mhd[:, i].reshape((8, 1)))
#for j in range(3):
Pos[:, i] = pos[:, 0]
Posd[:, i] = posd[:, 0]
#orientação
ra = getRotationDualQuat(ha)
rd = getRotationDualQuat(Mhd[:, i].reshape((8, 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[:, i] = theta[:, 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[:, i] = pos2[:, 0]
Posd2[:, i] = posd2[:, 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
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,
'r', 'b')
return ha, ha2, theta, tempo, Mtheta, Mtheta2