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 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 DerivadaJacobiano(theta, hOrg, hP, tipo, CoM, ind):
#-----------------------------------------------------------
#variaveis globais
#----------------------------------------------------------
glob = GlobalVariables()
h = glob.getH()
#MDH = glob.getMDH()
#global h, MDH
thetar = np.zeros((6, 1))
thetal = np.zeros((6, 1))
#for j in range(6):
thetar[:, 0] = theta[:, 0]
thetal[:, 0] = theta[:, 1]
#-----------------------------------------------------------
#cálculo do primeiro ponto para 2h
#----------------------------------------------------------
if tipo == 1:
if CoM == 1:
thetar[ind - 1, 0] = thetar[ind - 1, 0] + 2 * h
else:
thetal[ind - 1, 0] = thetal[ind - 1, 0] + 2 * h
else:
if CoM == 1:
thetal[ind - 1, 0] = thetal[ind - 1, 0] + 2 * h
else:
thetar[ind - 1, 0] = thetar[ind - 1, 0] + 2 * h
#calculando f(x+2h)
theta1 = np.concatenate((thetar, thetal), axis=1)
a = kinematicRobo(theta1, hOrg, hP, tipo, CoM)
#-----------------------------------------------------------
#cálculo do segundo ponto para h
#----------------------------------------------------------
if tipo == 1:
if CoM == 1:
thetar[ind - 1, 0] = thetar[ind - 1, 0] + h
else:
thetal[ind - 1, 0] = thetal[ind - 1, 0] + h
else:
if CoM == 1:
thetal[ind - 1, 0] = thetal[ind - 1, 0] + h
else:
thetar[ind - 1, 0] = thetar[ind - 1, 0] + h
#calculando f(x+h)
theta2 = np.concatenate((thetar, thetal), axis=1)
b = kinematicRobo(theta2, hOrg, hP, tipo, CoM)
#-----------------------------------------------------------
#cálculo do terceiro ponto para -h
#----------------------------------------------------------
if tipo == 1:
if CoM == 1:
thetar[ind - 1, 0] = thetar[ind - 1, 0] - h
else:
thetal[ind - 1, 0] = thetal[ind - 1, 0] - h
else:
if CoM == 1:
thetal[ind - 1, 0] = thetal[ind - 1, 0] - h
else:
thetar[ind - 1, 0] = thetar[ind - 1, 0] - h
#calculando f(x-h)
theta3 = np.concatenate((thetar, thetal), axis=1)
c = kinematicRobo(theta3, hOrg, hP, tipo, CoM)
#-----------------------------------------------------------
#cálculo do quarto ponto para -2h
#----------------------------------------------------------
if tipo == 1:
if CoM == 1:
thetar[ind - 1, 0] = thetar[ind - 1, 0] - 2 * h
else:
thetal[ind - 1, 0] = thetal[ind - 1, 0] - 2 * h
else:
if CoM == 1:
thetal[ind - 1, 0] = thetal[ind - 1, 0] - 2 * h
else:
thetar[ind - 1, 0] = thetar[ind - 1, 0] - 2 * h
#calculando f(x-2h) theta2
theta4 = np.concatenate((thetar, thetal), axis=1)
d = kinematicRobo(theta4, hOrg, hP, tipo, CoM)
#-----------------------------------------------------------
#cálculo da derivada (elemento do jacobiano)
#----------------------------------------------------------
q1 = (-a + 8 * b - 8 * c + d) * ((1 / 12) * h)
r = getRotationDualQuat(q1)
#normq = np.linalg.norm(r)
# if normq != 0:
# q1 = q1/(normq)
return q1
def 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 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