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 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 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 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
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
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