def trajetoria(U, X):
#-----------------------------------------------------------
#variáveis globais
#-----------------------------------------------------------
glob = GlobalVariables()
m = glob.getM() #massa
L = glob.getL() #tamanho da perna
g = glob.getG() #gravidade
h = glob.getH() #passo para o calculo das derivadas
hEdo = glob.getHEDO() #passo para calculo - EDO
#-----------------------------------------------------------
#condições iniciais para MS (referentes ao CM)
#-----------------------------------------------------------
xod = X[0, 0]
yod = X[1, 0]
zod = X[2, 0]
dxod = X[3, 0]
dyod = X[4, 0]
dzod = X[5, 0]
#-----------------------------------------------------------
#valores para a otimização valores de u
#-----------------------------------------------------------
#u = [theta phi k Bss]
theta = U[0, 0]
phi = U[1, 0]
k = U[2, 0]
Bss = U[3, 0]
expK = U[4, 0]
#-----------------------------------------------------------
#vetor com as condições iniciais MS
#-----------------------------------------------------------
y0 = np.array([[xod], [dxod], [yod], [dyod], [zod], [dzod]])
#-----------------------------------------------------------
#vetor com os parâmetros constantes
#-----------------------------------------------------------
params = np.array([[m], [L], [g], [k], [Bss], [expK]])
#-----------------------------------------------------------
#Parâmetros para os métodos
#-----------------------------------------------------------
t = 0 #inicio do tempo t = 0
h = hEdo #passo do método rungekuttaLIMP inicial
N = 10000 #número máximo de iterações
#primeiro metodo
sh = h #tamanho do passo para o método rungekuttaLIMP atualizando durante a execução do método
ind = 0 #contador
#traj = ind + 1 #tamanho do vetor com os pontos da trajetória
#-----------------------------------------------------------
#vetores auxiliares para guardar a trajetória
#-----------------------------------------------------------
px = [y0[0, 0]]
py = [y0[2, 0]]
pz = [y0[4, 0]]
#y = np.zeros((6,1))
#-----------------------------------------------------------
#inicio do método 1 MS para TD
#-----------------------------------------------------------
for i in range(N): #for de 0 até N*h, com passo h
#-----------------------------------------------------------
#vetor de parâmetros
#-----------------------------------------------------------
var = np.array([[t], [h], [1]])
#-----------------------------------------------------------
#método numérico para solucionar as equações diferenciais
#passo a passo
#-----------------------------------------------------------
y = rungeKuttaLIPM(var, y0, params)
#-----------------------------------------------------------
#atualizando a condição inicial
#-----------------------------------------------------------
y0 = y
#-----------------------------------------------------------
#atualizando o instante t
#-----------------------------------------------------------
t = t + sh
#-----------------------------------------------------------
#verificando a condição de parada posição Z < que Z de touchdown
#Z de touchdown = L*cos(theta)
#-----------------------------------------------------------
if t >= 0.105:
break
#-----------------------------------------------------------
#colocando os valores nos vetores auxiliares
#-----------------------------------------------------------
else:
px.append(y0[0, 0])
py.append(y0[2, 0])
pz.append(y0[4, 0])
#-----------------------------------------------------------
#atualizando o contador
#-----------------------------------------------------------
ind = ind + 1
#-----------------------------------------------------------
#atualizando o contador - tratando o valor
#-----------------------------------------------------------
#if ind > 1: #não preciso desse if, porque iniciei o contador no 0
#ind = ind -1 #o Juan havia iniciado em 1
#end
ponto = ind
#-----------------------------------------------------------
#Posição do centro de massa no momento de Touchdown (TD)
#-----------------------------------------------------------
pc = np.array([[px[ind]], [py[ind]], [pz[ind]]]).reshape(
(3, 1)) #centro de massa
#-----------------------------------------------------------
#posição do pé de balanço quando toca o chão
#-----------------------------------------------------------
pfb = pc + L * np.array([[np.sin(theta) * np.cos(phi)],
[np.sin(theta) * np.sin(phi)], [-np.cos(theta)]])
#-----------------------------------------------------------
#tempo em que acontece a codição de touchdown
#-----------------------------------------------------------
TD = t #tempo de TD
#-----------------------------------------------------------
#parametros constante para o segundo método
#-----------------------------------------------------------
params = np.array([[m], [L], [g], [k], [Bss], [t], [pfb[0, 0]],
[pfb[1, 0]], [pfb[2, 0]], [expK]])
#-----------------------------------------------------------
#iniciando o segundo contador
#-----------------------------------------------------------
ind2 = 0
#traj2 = ind2 + 1
sh = h #tamanho do passo para o método rungekuttaLIMP atualizando durante a execução do método
#-----------------------------------------------------------
#vetores auxiliares para guardar a trajetória
#-----------------------------------------------------------
px2 = [y0[0, 0]]
py2 = [y0[2, 0]]
pz2 = [y0[4, 0]]
#-----------------------------------------------------------
#inicio do método 2 TD para LH
#-----------------------------------------------------------
for x in range(N):
#-----------------------------------------------------------
#vetor de parâmetros
#-----------------------------------------------------------
var = np.array([[t], [h], [0]])
#-----------------------------------------------------------
#método numérico para solucionar as equações diferenciais
#passo a passo
#-----------------------------------------------------------
y = rungeKuttaLIPM(var, y0, params)
#-----------------------------------------------------------
#atualizando nova condição inicial
#-----------------------------------------------------------
y0 = y
#-----------------------------------------------------------
#atualizando o instante t
#-----------------------------------------------------------
t = t + sh
#-----------------------------------------------------------
#verificando a condição de parada posição dZ > 0
#-----------------------------------------------------------
#if v.all():
if t >= 0.135:
break
#-----------------------------------------------------------
#atualizando os vetores auxiliares da trajetória
#-----------------------------------------------------------
else:
px2.append(y0[0, 0])
py2.append(y0[2, 0])
pz2.append(y0[4, 0])
#-----------------------------------------------------------
#atualizando o contador
#-----------------------------------------------------------
ind2 = ind2 + 1
#-----------------------------------------------------------
#atualizando o contador - tratando o valor
#-----------------------------------------------------------
#if ind2 > 1
# ind2 = ind2 -1;
#-----------------------------------------------------------
#trajetória do centro de massa CoM M = [x y z]
#-----------------------------------------------------------
# concatenando as listas, para preencher o vetor M
pxTot = px + px2
pyTot = py + py2
pzTot = pz + pz2
pxTot = np.asarray(pxTot)
pxTot = pxTot.reshape(-1, 1)
pyTot = np.asarray(pyTot)
pyTot = pyTot.reshape(-1, 1)
pzTot = np.asarray(pzTot)
pzTot = pzTot.reshape(-1, 1)
M = np.concatenate((pxTot, pyTot, pzTot), axis=1)
M = M.reshape(-1, 3)
#-----------------------------------------------------------
#atualizando a posição do centro de massa
#-----------------------------------------------------------
pc = np.array([[px2[ind2]], [py2[ind2]], [pz2[ind2]]]).reshape(
(3, 1)) #centro de massa
#-----------------------------------------------------------
#atualizando a posição dos pés e do centro de massa
#para o retorno da função
#-----------------------------------------------------------
pa = np.array([[0], [0], [0]])
pb = np.array([[pfb[0, 0]], [pfb[1, 0]], [0]])
pc = np.array([[pc[0, 0]], [pc[1, 0]], [pc[2, 0]]])
#print(np.shape(pc[1,0]))
return pa, pb, pc, M, ponto
def DerivadaFX(X0, ind, t, model, pfb, An):
#-----------------------------------------------------------
#variaveis globais
#----------------------------------------------------------
glob = GlobalVariables()
h = glob.getH()
m = glob.getM()
L = glob.getL()
g = glob.getG()
k = glob.getK()
Bss = glob.getBss()
expK = glob.getExpK()
#MDH = glob.getMDH()
#global h, MDH
#-----------------------------------------------------------
#X = [x,y,z,dx,dy]
#U = [phi, theta, k] (model1)
#U = [phi,theta,Bss] (model2)
var = np.array([[t], [h], [model]])
if model == 1:
params = np.array([[m], [L], [g], [k], [Bss], [expK]])
else:
params = np.array([[m], [L], [g], [k], [Bss], [t], [pfb[0, 0]],
[pfb[1, 0]], [pfb[2, 0]], [expK]])
#-----------------------------------------------------------
#cálculo do primeiro ponto para 2h
#----------------------------------------------------------
X0[ind, 0] = x[ind, 0] + 2 * h
#calculando f(x+2h)
#theta1 = np.concatenate((thetar, thetal),axis=1)
#xn vem na forma x = [x,y,z,dx,dy,dz], mas para ser utilizado na função
#rungeKutta42, é preciso estar na forma x = [x,dx,y,dy,z,dz]
yn = baguncaX(x)
xn = rungeKutta42(var, yn, params)
#voltar xn para a forma xn = [x,y,z,dx,dy,dz] e eliminar o último elemento
xn = arrumaX(xn)
xn = xn[0:5, :]
a = np.dot(An, xn)
#a = a[:-1]
#-----------------------------------------------------------
#cálculo do segundo ponto para h
#----------------------------------------------------------
x[ind, 0] = x[ind, 0] + h
#calculando f(x+h)
#theta1 = np.concatenate((thetar, thetal),axis=1)
yn = baguncaX(x)
xn = rungeKutta42(var, yn, params)
#voltar xn para a forma xn = [x,y,z,dx,dy,dz] e eliminar o último elemento
xn = arrumaX(xn)
xn = xn[0:5, :]
b = np.dot(An, xn)
#b = b[:-1]
#-----------------------------------------------------------
#cálculo do terceiro ponto para -h
#----------------------------------------------------------
x[ind, 0] = x[ind, 0] - h
#calculando f(x-h)
#theta1 = np.concatenate((thetar, thetal),axis=1)
yn = baguncaX(x)
xn = rungeKutta42(var, yn, params)
#voltar xn para a forma xn = [x,y,z,dx,dy,dz] e eliminar o último elemento
xn = arrumaX(xn)
xn = xn[0:5, :]
c = np.dot(An, xn)
#c = c[:-1]
#-----------------------------------------------------------
#cálculo do quarto ponto para -2h
#----------------------------------------------------------
x[ind, 0] = x[ind, 0] - 2 * h
#calculando f(x-2h)
#theta1 = np.concatenate((thetar, thetal),axis=1)
yn = baguncaX(x)
xn = rungeKutta42(var, yn, params)
#voltar xn para a forma xn = [x,y,z,dx,dy,dz] e eliminar o último elemento
xn = arrumaX(xn)
xn = xn[0:5, :]
d = np.dot(An, xn)
#d = a[:-1]
#-----------------------------------------------------------
#cálculo da derivada (elemento do jacobiano)
#----------------------------------------------------------
q1 = (-a + 8 * b - 8 * c + d) * ((1 / 12) * h)
#r = getRotationDualQuat(q1)
#normq = np.linalg.norm(r)
#if normq != 0:
#q1 = q1/(normq)
return q1
def DerivadaFU(u,x,ind,t,model,pfb,An):
#-----------------------------------------------------------
#variaveis globais
#----------------------------------------------------------
glob = GlobalVariables()
h = glob.getH()
m = glob.getM()
L = glob.getL()
g = glob.getG()
k = glob.getK()
Bss = glob.getBss()
expK = glob.getExpK()
#------------------------------------------------------------
#X = [x,y,z,dx,dy]
#U = [phi, theta, k] (model1)
#U = [phi,theta,Bss] (model2)
var = np.array([[t],[h],[model]])
if model == 1:
#-----------------------------------------------------------
#cálculo do primeiro ponto para 2h
#----------------------------------------------------------
u[ind,0] = u[ind,0] + 2*h
#calculando f(x+2h)
#xn vem na forma x = [x,y,z,dx,dy,dz], mas para ser utilizado na função
#rungeKutta42, é preciso estar na forma x = [x,dx,y,dy,z,dz]
params = np.array([[m],[L],[g],[k],[u[2]],[expK]])
yn = baguncaX(x)
xn = rungeKutta42(var,yn,params)
#voltar xn para a forma xn = [x,y,z,dx,dy,dz] e eliminar o último elemento
xn = arrumaX(xn)
xn = xn[0:5,:]
a = np.dot(An,xn)
#a = a[:-1]
#-----------------------------------------------------------
#cálculo do primeiro ponto para h
#----------------------------------------------------------
u[ind,0] = u[ind,0] + h
#calculando f(x+h)
params = np.array([[m],[L],[g],[k],[u[2]],[expK]])
yn = baguncaX(x)
xn = rungeKutta42(var,yn,params)
#voltar xn para a forma xn = [x,y,z,dx,dy,dz] e eliminar o último elemento
xn = arrumaX(xn)
xn = xn[0:5,:]
b = np.dot(An,xn)
#a = a[:-1]
#-----------------------------------------------------------
#cálculo do primeiro ponto para -h
#----------------------------------------------------------
u[ind,0] = u[ind,0] - h
#calculando f(x-h)
params = np.array([[m],[L],[g],[k],[u[2]],[expK]])
yn = baguncaX(x)
xn = rungeKutta42(var,yn,params)
#voltar xn para a forma xn = [x,y,z,dx,dy,dz] e eliminar o último elemento
xn = arrumaX(xn)
xn = xn[0:5,:]
c = np.dot(An,xn)
#a = a[:-1]
#-----------------------------------------------------------
#cálculo do primeiro ponto para -2h
#----------------------------------------------------------
u[ind,0] = u[ind,0] - 2*h
#calculando f(x-2h)
params = np.array([[m],[L],[g],[k],[u[2]],[expK]])
yn = baguncaX(x)
xn = rungeKutta42(var,yn,params)
#voltar xn para a forma xn = [x,y,z,dx,dy,dz] e eliminar o último elemento
xn = arrumaX(xn)
xn = xn[0:5,:]
d = np.dot(An,xn)
#a = a[:-1]
else:
#-----------------------------------------------------------
#cálculo do primeiro ponto para 2h
#----------------------------------------------------------
u[ind,0] = u[ind,0] + 2*h
#calculando f(x+2h)
params = np.array([[m],[L],[g],[u[2]],[Bss],[t],[pfb[0,0]],[pfb[1,0]],[pfb[2,0]],[expK]])
yn = baguncaX(x)
xn = rungeKutta42(var,yn,params)
#voltar xn para a forma xn = [x,y,z,dx,dy,dz] e eliminar o último elemento
xn = arrumaX(xn)
xn = xn[0:5,:]
a = np.dot(An,xn)
#a = a[:-1]
#-----------------------------------------------------------
#cálculo do primeiro ponto para h
#----------------------------------------------------------
u[ind,0] = u[ind,0] + h
#calculando f(x+h)
params = np.array([[m],[L],[g],[u[2]],[Bss],[t],[pfb[0,0]],[pfb[1,0]],[pfb[2,0]],[expK]])
yn = baguncaX(x)
xn = rungeKutta42(var,yn,params)
#voltar xn para a forma xn = [x,y,z,dx,dy,dz] e eliminar o último elemento
xn = arrumaX(xn)
xn = xn[0:5,:]
b = np.dot(An,xn)
#a = a[:-1]
#-----------------------------------------------------------
#cálculo do primeiro ponto para -h
#----------------------------------------------------------
u[ind,0] = u[ind,0] - h
#calculando f(x-h)
params = np.array([[m],[L],[g],[u[2]],[Bss],[t],[pfb[0,0]],[pfb[1,0]],[pfb[2,0]],[expK]])
yn = baguncaX(x)
xn = rungeKutta42(var,yn,params)
#voltar xn para a forma xn = [x,y,z,dx,dy,dz] e eliminar o último elemento
xn = arrumaX(xn)
xn = xn[0:5,:]
c = np.dot(An,xn)
#a = a[:-1]
#-----------------------------------------------------------
#cálculo do primeiro ponto para -2h
#----------------------------------------------------------
u[ind,0] = u[ind,0] - 2*h
#calculando f(x-2h)
params = np.array([[m],[L],[g],[k],[u[2]],[expK]])
yn = baguncaX(x)
xn = rungeKutta42(var,yn,params)
#voltar xn para a forma xn = [x,y,z,dx,dy,dz] e eliminar o último elemento
xn = arrumaX(xn)
xn = xn[0:5,:]
d = np.dot(An,xn)
#a = a[:-1]
q1 = (-a + 8*b -8*c + d)* ((1/12) * h)
return q1
#adicionar as libs necessárias #----------------------------------------------------------- from otimizacao import otimizacao import numpy as np from globalVariables import GlobalVariables #----------------------------------------------------------- #variáveis globais #----------------------------------------------------------- #global m, L, g, lstep, pfa, thetaM, phiM, KM, expK, BSSM glob = GlobalVariables() #massa do corpo m = glob.getM() #tamanho da perna L = glob.getL() #gravidade g = glob.getG() #posição do pé de suporte em MS pfa = glob.getPfa() #----------------------------------------------------------- #Numero maximo de iterações para o metodo do gradiente #----------------------------------------------------------- #global maxNGrad, ganhoAlpha, gamma, h, hEdo maxNGrad = glob.getMaxNGrad() #número máximo de iterações método ganhoAlpha = glob.getGanhoAlpha() #ganho do fator de ganho para cada passo gamma = glob.getGamma() #ganho para os método gradiente(momento) h = glob.getH() #passo para o calculo das derivadas #calculado (comparado com a função do matlab) hEdo = glob.getHEDO() #passo para o calculo das derivadas #----------------------------------------------------------- #condição inicial para MS #-----------------------------------------------------------
def plotarTrajetoria(U, X):
#-----------------------------------------------------------
#variáveis globais
#-----------------------------------------------------------
#global m L g pfa expK hEdo
glob = GlobalVariables()
m = glob.getM()
L = glob.getL()
g = glob.getG()
pfa = glob.getPfa()
hEdo = glob.getHEDO()
expK = glob.getExpK()
#-----------------------------------------------------------
#condições iniciais para MS
#-----------------------------------------------------------
xod = X[0, 0]
yod = X[1, 0]
zod = X[2, 0]
dxod = X[3, 0]
dyod = X[4, 0]
dzod = X[5, 0]
#-----------------------------------------------------------
#valores para a otimização - valores de u
#-----------------------------------------------------------
#u = [phi theta k Bss]
theta = U[0, 0]
phi = U[1, 0]
k = U[2, 0]
Bss = U[3, 0]
expK = U[4, 0]
#-----------------------------------------------------------
#vetor com as condições iniciais MS
#-----------------------------------------------------------
y0 = np.array([[xod], [dxod], [yod], [dyod], [zod], [dzod]])
#-----------------------------------------------------------
#vetor com os parâmetros constantes
#-----------------------------------------------------------
params = np.array([[m], [L], [g], [k], [Bss], [expK]])
#-----------------------------------------------------------
#Parâmetros para os métodos
#-----------------------------------------------------------
t = 0 #inicio do tempo t = 0
h = hEdo #passo do método rungeKutta42 inicial
N = 10000 #número máximo de iterações
y = np.zeros((6, 1))
#primeiro método
sh = h #tamanho do passo para o método rungeKutta42 atualizando durante a execução do método
ind = 0 #contador
#-----------------------------------------------------------
#vetores auxiliares para guardar a trajetória
#-----------------------------------------------------------
px = [y0[0, 0]]
py = [y0[2, 0]]
pz = [y0[4, 0]]
#-----------------------------------------------------------
#inicio do método primeiro MS para TD
#-----------------------------------------------------------
for x in np.arange(0, N * h, h):
#-----------------------------------------------------------
#vetor de parâmetros
#-----------------------------------------------------------
var = np.array([[t], [h], [1]])
#-----------------------------------------------------------
#método numérico para solucionar as equações diferenciais
#passo a passo
#-----------------------------------------------------------
y = rungeKutta42(var, y0, params)
#-----------------------------------------------------------
#atualizando a condição inicial
#-----------------------------------------------------------
y0 = y
#-----------------------------------------------------------
#atualizando o instante t
#-----------------------------------------------------------
t = t + sh
#-----------------------------------------------------------
#verificando a condição de parada posição Z < que Z de touchdown
#Z de touchdown = L*cos(theta)
#-----------------------------------------------------------
if y0[4, 0] < L * mt.cos(theta):
break
#-----------------------------------------------------------
#colocando os valores nos vetores auxiliares
#-----------------------------------------------------------
px.append(y0[0, 0])
py.append(y0[2, 0])
pz.append(y0[4, 0])
#-----------------------------------------------------------
#atualizando o contador
#-----------------------------------------------------------
ind = ind + 1
#-----------------------------------------------------------
#atualizando o contador - tratando o valor
#-----------------------------------------------------------
#if ind > 1:
#ind = ind -1
#-----------------------------------------------------------
#Posição do centro de massa no momento de Touchdown (TD)
#-----------------------------------------------------------
pc = np.array([[px[ind]], [py[ind]], [pz[ind]]]) #centro de massa
px = np.asarray(px)
py = np.asarray(py)
pz = np.asarray(pz)
#-----------------------------------------------------------
#posição do pé de balaço quando toca o chão
#-----------------------------------------------------------
pfb = pc + L * np.array([
mt.sin(theta) * mt.cos(phi),
mt.sin(theta) * mt.sin(phi), -mt.cos(theta)
])
#-----------------------------------------------------------
#tempo em que acontece a codição de touchdown
#-----------------------------------------------------------
TD = t #tempo de TD
#-----------------------------------------------------------
#parâmetros constante para o segundo método
#-----------------------------------------------------------
params = np.array([[m], [L], [g], [k], [Bss], [t], [pfb[0, 0]],
[pfb[1, 0]], [pfb[2, 0]], [expK]])
#-----------------------------------------------------------
#iniciando o segundo contador
#-----------------------------------------------------------
ind2 = 0
sh = h #tamanho do passo para o método rungeKutta42 atualizando durante a execução do método
#-----------------------------------------------------------
#vetores auxiliares para guardar a trajetória
#-----------------------------------------------------------
px2 = [y0[0, 0]]
py2 = [y0[2, 0]]
pz2 = [y0[4, 0]]
#-----------------------------------------------------------
#inicio do método 2 - TD para LH
#-----------------------------------------------------------
for x in np.arange(0, N * h, h):
#-----------------------------------------------------------
#vetor de parâmetros
#-----------------------------------------------------------
var = np.array([[t], [h], [0]])
#-----------------------------------------------------------
#método numérico para solucionar as equações diferenciais
#passo a passo
#-----------------------------------------------------------
y = rungeKutta42(var, y0, params)
#-----------------------------------------------------------
#atualizando nova condição inicial
#-----------------------------------------------------------
y0 = y
#-----------------------------------------------------------
#atualizando o instante t
#-----------------------------------------------------------
t = t + sh
#-----------------------------------------------------------
#verificando a condição de parada posição dZ > 0
#-----------------------------------------------------------
if y0[5, 0] > 0:
break
#-----------------------------------------------------------
#atualizando o contador
#-----------------------------------------------------------
ind2 = ind2 + 1
#-----------------------------------------------------------
#atualizando os vetores auxiliares da trajetória
#-----------------------------------------------------------
px2.append(y0[0, 0])
py2.append(y0[2, 0])
pz2.append(y0[4, 0])
#-----------------------------------------------------------
#atualizando o contador
#-----------------------------------------------------------
#if ind2 > 1:
#ind2 = ind2 -1
#-----------------------------------------------------------
#atualizando a posição do centro de massa
#-----------------------------------------------------------
pc = np.array([px2[ind2], py2[ind2], pz2[ind2]]) #centro de massa
px2 = np.asarray(px2)
py2 = np.asarray(py2)
pz2 = np.asarray(pz2)
#----------------------------------------------------------- # trajetoria
# a trajetoria é simetrica ao ponto de middle suport
# desta forma fazemos um espelho da função
#deslocado ela para a origem
#-----------------------------------------------------------
#plot3(px(1,1:ind),py(1,1:ind),pz(1,1:ind),'b')
#plot3(px2(1,1:ind2),py2(1,1:ind2),pz2(1,1:ind2),'r')
plot3 = plt.figure(px[0:ind], py[0:ind], pz[0:ind], 'b')
plot3 = plt.figure(px2[0:ind2], py2[0:ind2], pz2[0:ind2], 'r')
#-----------------------------------------------------------
#espelho da função
#-----------------------------------------------------------
offsetx = px2[ind2]
offsety = py2[ind2]
plot3 = plt.figure(-px2 + 2 * offsetx, -py2 + 2 * offsety, pz2, 'r')
offsetx = -px2[ind2] + 2 * offsetx
offsety = -py2[ind2] + 2 * offsety
plot3 = plt.figure(-px[range(ind, -1, -1)] + 2 * offsetx,
-py[range(ind, -1, -1)] + 2 * offsety,
pz[range(ind, -1, -1)], 'b')
#-----------------------------------------------------------
#projeção
#-----------------------------------------------------------
plot3 = plt.figure(px[0:ind], py[0, 0:ind], 0 * pz[0:ind], 'g')
plot3 = plt.figure(px2[0, 0:ind2], py2[0, 0:ind2], 0 * pz2[0:ind2], 'm')
#-----------------------------------------------------------
#espelho da função
#-----------------------------------------------------------
offsetx = px2[0, ind2]
offsety = py2[0, ind2]
plot3 = plt.figure(-px2([range(ind2, -1, -1)]) + 2 * offsetx,
-py2[range(ind2, -1, -1)] + 2 * offsety,
0 * pz2[range(ind2, -1, -1)], 'm')
offsetx = -px2[0, ind2] + 2 * offsetx
offsety = -py2[0, ind2] + 2 * offsety
plot3 = plt.figure(-px[range(ind, -1, -1)] + 2 * offsetx,
-py[range(ind, -1, -1)] + 2 * offsety,
0 * pz[range(ind, -1, -1)], 'g')
#-----------------------------------------------------------
#plotar posição dos pés
#-----------------------------------------------------------
plot3 = plt.figure([pfa[0, 0], pfb[0, 0]], [pfa[1, 0], pfb[1, 0]], [0, 0],
'--k')
#-----------------------------------------------------------
#projeção centro de massa e projeção centro de massa
#-----------------------------------------------------------
plot3 = plt.figure(pc[0, 0], pc[1, 0], pc[2, 0], 'o')
plot3 = plt.figure([pc[0, 0], pc[0, 0]], [pc[1, 0], pc[1, 0]],
[pc[2, 0], 0], ':')
plot3 = plt.figure(pc[0, 0], pc[1, 0], 0, 'x')
#-----------------------------------------------------------
#configuração do grafico
#-----------------------------------------------------------
plot3 = plt.figure(figsize=(60, 35))
ax = plot3.add_subplot(111, projection='3d')
plt.grid(True)
ax.set_xlim([-1, 1])
ax.set_ylim([-0.5, 0.5])
ax.set_zlim([-1, -0.2])
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
ax.show()