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