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 calculoMhd(trajCoM, theta, trajP, phase):
glob = GlobalVariables()
hEdo = glob.getHEDO()
L1 = glob.getL1()
height = glob.getHeight()
#MDH = glob.getMDH()
hpi = glob.getHpi()
ind = np.size(trajP, 0)
hOrg = np.array([[1], [0], [0], [0], [0], [0], [0], [0]]) #posição da base
dt = hEdo
#cacular posição atual do´pé
n = np.array([0, 1, 0]) # n é o vetor diretor do quaternio
thetab = hpi #parametro da função de caminhada que é igual a pi/2
realRb = np.array([np.cos(thetab / 2)])
rb = np.concatenate((realRb, np.sin(thetab / 2) * n), axis=0).reshape(
(4, 1))
pb = np.array([[0], [0], [-L1], [-height]])
#base B para a base O6 (B é a perna em movimento)
hB_O6 = transformacao(pb, rb)
hP = dualQuatMult(hOrg, hB_O6)
#dt é o tempo da solução da equação Edo e, ao mesmo tempo, o passo
T = np.size(trajCoM, 0) #o tamanho de trajCoM = ind
tempo = (
T - 1
) * dt #o tempo é o produto da quantidade de iterações necessárias para calcular a trajetória do CoM
#matrizes e vetores auxiliares
Mhd = np.zeros((8, T))
Mha = np.zeros((8, T))
Mdhd = np.zeros((8, T))
Mtheta = np.zeros((6, T))
Mdhd = np.zeros((8, T))
Mhd2 = np.zeros((8, T))
Mdhd2 = np.zeros((8, T))
# Mhd2 = np.zeros((8,T))
# Mha2 = np.zeros((8,T))
# Mdhd2= np.zeros((8,T))
# Mtheta2 = np.zeros((6,T))
# angle = np.zeros(T)
# angled = np.zeros(T)
# angle2 = np.zeros(T)
# angled2 = np.zeros(T)
# Pos = np.zeros((3,T))
# Posd = np.zeros((3,T))
# Pos2 = np.zeros((3,T))
# Posd2 = np.zeros((3,T))
#calculo de Mhd - matriz de hd
r = np.array([1, 0, 0, 0]).reshape((4, 1))
p = np.array([0, 0, -L1, -height]).reshape(
(4, 1)) #height = L2 + L3 + L4 + L5
hB1 = transformacao(p, r) #transformação base robô
for i in range(0, T, 1):
p = np.array([0, trajCoM[i, 0], trajCoM[i, 1], trajCoM[i, 2]]).reshape(
(4, 1))
r = np.array([1, 0, 0, 0]).reshape((4, 1))
hd = transformacao(p, r)
hd = dualQuatMult(hB1, hd)
Mhd[:, i] = hd[:, 0]
#transformação da base até o centro de massa
#se i<ind, o robô ainda não atingiu a posição de td, então a transformação é calculada em relação ao pé
#quando o robô chega na fase de TD, a transformação é calculada em relação ao CoM
if i < ind:
p = np.array([0, trajP[i, 0], trajP[i, 1], trajP[i, 2]]).reshape(
(4, 1))
n = np.array([0, 1, 0])
#angulo = mt.pi/2.0 #graus ou radianos????????????????????????????????????????????????????????????/
realRb = np.array([np.cos(thetab / 2)])
rb = np.concatenate((realRb, np.sin(thetab / 2) * n),
axis=0).reshape((4, 1))
hd = transformacao(p, rb) #posição desejada
hd = dualQuatMult(hB1, hd) #transformação da base até o pé
Mhd2[:, i] = hd[:, 0]
else:
Mhd2[:, i] = Mhd2[:, ind - 1]
#Mdhd[:,0] = (Mhd[:,0] - Mhd[:,0])*(1/dt) #não deveria ser i-1, no segundo Mhd???????????????????????????????????
#Mdhd2[:,0] = (Mhd2[:,0] - Mhd2[:,0])*(1/dt)
for i in range(1, T, 1):
Mdhd[:, i] = (Mhd[:, i] - Mhd[:, i - 1]) * (
1 / dt
) #por que ele fazer isso????????????????????????????????????????????????????
Mdhd2[:, i] = (Mhd2[:, i] - Mhd2[:, i - 1]) * (
1 / dt) #derivada de hd, que é a posição desejada
if phase == 1:
ha = kinematicRobo(theta, hOrg, hP, 1,
1) #cinemática do pé esquerdo até o CoM
ha2 = kinematicRobo(theta, hOrg, hP, 1,
0) #cinemática de um pé até o outro
else:
ha = ha2 = np.zeros((8, 1))
return ha, ha2, hP, Mhd, Mhd2, Mdhd, Mdhd2, tempo
def 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