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
示例#2
0
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
示例#3
0
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
示例#4
0
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
示例#5
0
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
示例#6
0
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